【基于搜索的机器人路径规划算法】

图搜索基础

图：有节点和边的表示方式，产生一个搜索树。
广度优先搜索（BFS）：用的是队列，所有的元素先进先出，元素被压入队列，如果从队列弹出一个元素，从队列的头开始弹出。

在地图中建立搜索树，先搜索根节点的每一个子节点，每一个子节点作为根节点再产生的子节点压入队列，搜索树一层一层地完成，在地图上表现的是由一个起点向周围每一个方向扩散。

/

深度优先搜索（DFS）： 用的是堆栈，所有元素后进先出，把所有元素压入堆栈，弹出元素时从最上面弹出。

在地图中建立搜索树，先搜索根节点的一个子节点，第一个子节点作为根节点再产生的子节点压入堆栈，从压入的子节点中最后压入的一个节点取出来再进行搜索，压入堆栈中，以此类推，在地图上表现的是由一个起点向一个方向扩散。

Dijkstra算法和A*算法

Dijkstra算法 在广度优先搜索的基础上，增加一个cost代价因素g(n);
在广度优先搜索的搜索树建立过程中，考虑了每一个搜索点的代价，即搜索点到起始点的欧拉距离，体现了在选择搜索点后，机器人在路程中估计需要消耗的能量。在每一个根节点压入队列的过程中，需要对每一个字节点的代价进行计算，代价小的先压入队列。Dijkstra算法是在广度优先搜索的基础上进行的优化。

Dijkstra算法是完备的，当地图中存在路径解时，Dijkstra算法一定能够找出一条最优路径。
在Dijkstra算法中，没有确定的终点信息，只能扩散盲目搜索。

A*算法 启发式函数帮助下的搜索算法，和Dijkstra算法的框架完全一样，在Dijkstra算法的基础上，加上一个启发式函数h(n)， f(n) = g(n) + h(n)；
g(n) 表示当前节点到起点的估计代价，用欧拉距离或者曼哈顿距离计算；
h(n) 表示当前节点到终点的估计代价，也用欧拉距离或者曼哈顿距离计算；
A*算法是在Dijkstra算法的基础上进行优化，在代价估计函数中加入一个启发式函数，启发树节点向终点靠。在每一个根节点压入队列的过程中，需要对每一个字节点的 f(n) 代价进行计算，代价小的先压入队列，进行循环搜索。

在 h(n) 代价估计的计算中，需要小于真实距离，否则找到的路径可能不是最优。

补充 ： A* ➡ Weighted A* ➡ Anytime A* ➡ARA* ➡D*
Weighted A* 在 A* 的基础上加了一个启发式的权重因数 ➡ f(n) = g(n) + k*h(n) , k > 1；
用最优性换取算法速度。

open list ： 存放被其他节点扩展，还没有作为父节点扩展子节点的节点，即上图绿色部分；
close list ：存放已经扩展过的节点，即上图蓝色部分。

JPS(Jump Point Search)算法

JPS(Jump Point Search)算法：一种进阶的图搜索算法，系统性地实现 Tie Breaker 的算法。核心思想是找到图搜索中的对称性，打破对称性。在相同的起点和终点的地图中，A*可能会搜索很多对称的点，在工程计算中没有必要，JPS算法会智能地忽略掉图搜索中可能对称的点，并且唯一地选择一条路径。
JPS算法在扩展每一步的时候，根据当前节点父节点过来的方向以及周围是否有障碍物、障碍物位置等信息，来判断哪些节点是需要扩展的。

Look Ahead Rule规则
左图中无障碍物的情况下，第一个搜索图 x 节点从它的父节点 4 号节点搜索过来，x 周围的地方没有障碍物，这时候 x 节点需要扩展的节点只有 5 号节点，是 x 节点的自然邻居。2 号节点为什么不需要扩展？因为 2 号节点可以由 x 父节点（4号节点）直接到达，路程代价为 sqrt(2) ，若经过 x 到达 2 号节点，需要的路程代价为 2 ，所以没有必要从 x 节点扩展 2 号节点。其他的也是相同的原理，注意在路径代价相同的情况下，取的是到达节点的最后一段代价最小的路径，所以 3 号节点不从 x 节点扩展，而是需要从 2 号节点扩展。第二个搜索图 x 节点从它的父节点 6 号节点搜索过来，这时 x 的自然邻居有 2、3、5号节点，1、4、7、8 节点可以由 x 的父节点不经过 x 节点的最短路径到达。

右图中有障碍物的情况下，第一个搜索图 x节点的上方有一个障碍物（2号节点），因为在没有障碍物的情况下，3 号节点需要从 2 号节点扩展过来，当 2 号节点为障碍物时，原本的路径被障碍物切断，这时 3 号节点需要作为 x 节点的强迫邻居被扩展。第二个搜索图 x节点的左侧有一个障碍物（4号节点），相同的原理，1 号节点会成为 x 节点的强迫邻居被扩展。

下面是由 Look Ahead Rule 规则扩展出的 Jumping Rules 跳跃规则：

在第一个图片中，x 节点在迭代扩展的时候不需要向上下扩展，因为上下两排可以由 x 父节点以一个更优的路径到达。x 节点就可以直接跳跃到 y 节点，而 y 节点上方有一个障碍物，这时产生了一个强迫邻居 z 节点，就需要向 z 节点扩展了。在第二个图片中，x 节点由父节点对角线扩展，并产生了一个强迫邻居 w ，x 节点在直线扩展的时候都会碰到障碍物（地图边界也视为障碍物），右边障碍物的下方路径由 w 节点最优扩展，所以 x 节点跳到 w 节点， w 节点放入 open list。 x 节点在直线扩展时都会碰到障碍物，所以进行对角线扩展，每次扩展一个对角单位然后进行直线跳跃扩展，若碰到障碍物继续进行对角扩展，直到扩展到 y 节点，向右扩展 z 节点，这时 z 节点下方有障碍物，会产生一个强迫邻居，将 z 节点的父节点 y 节点放入 open list，这样就完成了 x 节点向 y 节点的跳跃扩展。（跳跃点只能往一个方向跳跃）

JPS(Jump Point Search)算法就可以实现在地图上空旷的地方进行跳跃搜索。