路径规划算法

广度优先搜索

广度优先搜索是相对于当前(节)点，完全遍历其相邻(节)点后再进行相邻(节)点的相邻(节)点的遍历，即如涟漪一样由近及远晕散开去，与之相对的是深度优化搜索，即相对于当前(节)点，先访问其中一个相邻(节)点，然后再访问此相邻(节)点的一个相邻(节)点，如此迭代直到最后的(节)点没有相邻节点后，再回到最前没没有访问的某点的相邻(节)点直到所有(节)点或目标出现。即如射击一样，不断地朝四面八方开火。然而广度优先搜索只是漫无目的地向外扩散，随缘偶遇目标。效率不高

Dijkstra算法

为提高搜索效率，Dijkstra算法在广度优先算法的基础上其利用贪心思想寻找最短路径(或者是代价(成本)最小路径，下面不加以区分)。它从起始点出发，每次都将当前节点后继节点加入到一个优先队列(如某一节已经在队列中，就要比较其到起始点的路径长度，保留较小值)中，然后弹出距离起点最近的节点，然后把这个点所有后继节点加入到优先队列中，如此迭代直到终点弹出。相比于广度优先搜索其效率优化不少。

Best first search

也是一种贪心算法与Dijkstra算法类似，只不过它考虑的是不是当前节点到起始节点的距离，而是当前节点到目标节点的距离。因为我们并不知道当前节点到目标的实际情况，所以是一种预估距离，或称启发(heuristic)距离。比如直接计算当前节点与目标节点的欧式距离，汉明距离等。

A* 算法

A*算法则是在以上方法的基础上的综合。A* 算法通过下函数来确定优先级:

\[f(n) = g(n) + h(n) \]

其中\(f(n)\)表示当前节点n的优先级， \(g(n)\)表示的是节点n与起点的距离(成本)， 而\(h(n)\)是当前节点与目标点的启发距离。\(h(n)\)的选择对算法本身会有影响，如果\(h(n)\)的值选的过小，那么相对于Dijkstra算法优化不多，如果\(h(n)\)选择过大，大于(从当前节点到目标节点的)最优距离时，那么很可能得到的路径不是最优的，但速度会比较快，因为有很多不必要的路径被过滤掉了。

上面g(n)更详细地，其为其父节点(上一节点)集合中，父节点成本与此父节点与当前节点成本的和的最小值:

\[g(n) = \min(g(n,p),c(n,p))\quad p\in P_n \]

其中，p表示当前节点n的某一父节点， \(P_n\)表示当前节点n的父节点为集合。

D* 算法

上面介绍的基本都属于静态规划算法，而D*算法是一种动态避障算法，即在到达目标节点之前，路径上的情况可能会发生变化。相对于A*算法，D*算法反向检索算法，即其先从目标节点开始，直到起始节点。

D*算法首先先从目标点到起始点使用Dijkstra算法计算最优路径，然后机器人从起始点向目标点行进，其间如果路径情况出现变化，比如出现捷径或障碍时，D*算法会在之前路径的基础上，重新计算路径。因为之前是从目标点开始的，那么现在重新规划时，从目标点到当前障碍的这段路径是没有变化的，因此不必重新计算，效率会提升不少。 D*算法的思路逐渐清晰： 当行进时，发现路径情况发生变化，D*就在当前变化节点重新规划路径，即从当前节点开始考虑，经过计算后，预计路径“越过”障碍后，再经计算“碰触”到之前计算的最优路径，那么后面不用计算了，可以直接利用。

因为每一个节点的成本可由下面公式计算:

\[h(X) = \min(h(Y) +c(X,Y)) \ \ Y \in P_{X} \]

其中X表示当前节点， Y表示其父节点(上一个节点，即在D*算法中，从目标节点到起始节点过程中，上一个节点当到当前节点，这个上一节点即为父节点)，\(P_X\)表示X的父节点集合。

LPA* 算法

LPA*算法即Lifelong Planning A*算法，是一种增量启发式搜索版本的A*算法，这种路径规划算法适用于随着时间改变导致有限栅格地图上的边缘代价c(s1,s2)改变的问题，也就是随着时间改变障碍物增多或减少，网格点发生增删等，在许多场合下比再利用A*重新搜索更高效。启发式搜索和增量式搜索的区别在于，启发式搜索是利用启发函数来对搜索进行指导，从而实现高效的搜索，启发式搜索是一种“智能”搜索，典型的算法例如A*算法、遗传算法等。增量搜索是对以前的搜索结果信息进行再利用来实现高效搜索，大大减少搜索范围和时间，典型的例如LPA、D Lite算法等。

在LPA*算法中提出了名为一致性的概念。一致性考虑的是g(s)与rhs(s)的一致性问题。其中g(s)是g*(s)的估计，而g*(s)表示的是起始点到当前节点s的最短距离。

\[g^*(s) = \min_{s'\in pred(s)}(g^*(s')+c(s',s)) \]

其中\(pred(s)\)表示节点s的你父节点集合。而rhs(s)则被定义为:

\[rhs(s) = \min_{s'\in pred(s)}(g(s')+c(s',s)) \]

正常情况下， g(s)与rhs(s)是相等的，那么此时s为局部一致的(locally consistent);

但当边缘成本c(s',s)发生变化时，那么g(s) 与rhs(hs)就不再相等。当g(s) >rhs(s)时，称为局部过一致(locally overconsistent)， 此时表示c(s',s)成本变小，可能会出现比之前路径更优的路径；当g(s)<rhs(s)时，称为局部欠一致，当s点欠一致时，表示c(s',s)变大， 此时表明之前的最优路径可能不再最优，甚至不可通过，需要重新计算出最优路径。

D* Lite

D* Lite 与LPA* 算法思路基本类似。只不也如D*相对于A*算法那样，D* Lite算法从目标节点开始向起始节点搜索。