继续我们的学习。本节课主要介绍了基于搜索的路径规划方法。首先推荐 这个网站，本课程相关的一些可视化动图都来自于此，而且这个网站也是干货十足，感谢大佬的无私分享！
首先是课程大纲：如上所示，四个部分，重点就是Dijkstra、A*和JSP这三种算法。
Graph Search Basis
Configuration Space
关于配置空间：C-Space 是机器人运动规划的理论基础，很多问题必须结合机器人的具体情况，才可能得到比较好的规划结果。参考某乎的回答。
一般来说，规划问题都是要进行碰撞检测的。
在配置空间中进行规划，将机器人表示为点，将障碍物膨胀。（对于简单的移动机器人适用）
在配置空间中做规划会简单有效得多。
Graph and Search Method
关于图：了解图论得一些基本理论即可。
栅格地图每一个格点都与周边有连接关系，所以天然就是图，基于图得搜素自然就是在图上进行搜索。
从s到g寻路：图搜索都是可以将其表示成一棵树得形式。例如上面pp的图和树就是表达的完全一样的意思。
这一张ppt是重点，它描述了一个完整的图搜索的流程：图搜索的一个整体框架就是要维护一个容器，这个容器装载了所有的将来有可能访问的节点；容器一开始是空的，第一个放入的就是初始节点；此后进入一个主循环：
- 首先从容器中弹出一个节点，称之为访问一个节点；
- 之后扩展它周围所有的邻居；
- 再把所有的邻居节点放入容器中；
直到循环终止结束。
循环终止的条件：当容器完全是空的时；
假如图是有回环的，为了陷入死循环，采用的方法是：维护一个新的容器，装载所有已经访问过的节点；
按照什么规则访问节点：涉及到后面具体的算法。
先介绍两种最常见的遍历方法：广度优先搜索(BFS)和深度优先搜索(DFS)。BFS维护的是一个队列，先进先出；DFS维护的是一个栈，后进先出。
DFS：弹出树状结构里最深层次的节点 。
有关DFS的具体介绍可以参考网上的一些博客，自己推一推。
这是博客开头推荐的网站的动图，建议大家去看一看。
BFS比DFS要容易理解一点。由于弹出的是最浅的元素，所以BFS是一层一层的进行搜索，逐行扫描。
依然建议去搜索一些有关详细解读BFS算法的博客。
两种算法的对比动画。下面我们所讲的算法都是基于BFS的。
Heuristic search(启发式算法)
启发式的算法：指引搜索的方向。启发函数是重点，上图中的欧氏距离和曼哈顿距离的启发函数所得到的都不是最短路径，紫色的才是。
动图链接
可以看出在没有障碍物时，贪心算法是最优的。
但是在有障碍物时，贪心算法就很容易陷入局部最优了。
另一个问题：我们可以在一个图里面，利用广度优先搜索成功的前提是每条边的权重是一样的。针对这样的问题，就是下面我们要介绍的两种非常重要的算法。
Dijkstra and A*
Algorithm Workflow
Dijkstra算法与BFS算法最大的不同在于弹出节点所依据的原则不同。Dijkstra算法每次弹出的节点是一个代价最小的函数值.代表从起点开始到n节点这条路上累计的cost总和最小的节点。
从优先级队列可以弹出cost最小的节点。下面就上图的伪代码做一个具体的解释说明：
首先是初始化容器并加进去起始节点；
起始节点的cost是0，其他节点设为无穷大；
进入主循环：
- 如果容器为空则算法结束，算法失败 ；
- 从优先级队列把具有最小g值的节点n弹出来；
- 把n标记为已经扩展过的节点并放入另一个容器；
- 如果n是目标节点，则算法成功，循环结束；
- 如果n不是目标节点，则需要继续搜索，对于n所有的邻居节点，如果已经扩展过则不需要再操作，如果没有被扩展过，则要做以下判断：1.如果g(m)为无穷大，说明这个节点是一个完全新的节点，所以需要将m此前无穷大的g值改为g(n)+Cnm；随后将其加入到优先级队列里。2.如果m的已经有g值了，说明m已经被之前的节点发现过了，也就是说已经被压进优先级队列中了，但是如果g值大于从n节点+Cnm，则需要对g值进行更新，因为从n节点过来的花费才是最小的。
- 主循环结束
反复循环迭代直至结束。
Dijkstra算法的优点：完备(如果有解就一定能找到)且最优
Dijkstra算法的缺点：找解时是往所有方向均匀搜索的，当每条边的权重都是1时，Dijkstra算法就相当于是广度优先算法；Dijkstra算法没有任何关于终点的信息。
贪心算法有一个关于到终点的guess，猜测一个节点到终点的远近。
A算法就是Dijkstra算法+贪心算法。与Dijkstra算法最大的不同就是在于
从A算法的伪代码可以看出，它与Dijkstra算法唯一的不同之处就在于把一个节点从容器中弹出的准则依据的是，而非仅有。
关于A的图示，其实这节课是强烈推荐博客一开始的网站的，可以说是最好的A讲解。
A*算法如果想要保有最优性，对于每个节点，h值必须要小于它到达最终目标的真实cost。也就是，估计出的距离要小于真实的距离。
上面介绍的几个算法的比较。
Engineering Considerations
关于A算法在工程领域内的具体应用：首先是将栅格地图变成一个graph，常用的就是四连接法和八连通法。有障碍物则打断它们之间的通行。
c++中建议的几种优先级队列实现方式如上图。
搜索过程中会扩展很多无用的节点，也就是“不紧”，或者说不是很tigth，其原因在于中有很多gap，由于只要是小于，我们都可以接受，所以才会扩展很多无用的节点。
对于非常结构化的地图，是直接可以求解出最优距离的。所以可以直接用搜对角的方式去作启发函数。
此类方法叫做：打破对称性。修改启发函数，将相同的h值的中的一个节点进行修改。
A就是要在h函数做文章！
Tie Breaker要根据具体的工程去做适配！
Jump Point Search(JPS)
Algorithm Workflow
JPS算法就是一个系统的去除“对称性”的算法。其核心思想就是：找到对称性并且将其打破。