路径规划算法总结

广度优先算法（Breadth-First-Search, BFS）

广度优先算法实际上已经能够找到最短路径，BFS通过一种从起点开始不断扩散的方式来遍历整个图。可以证明，只要从起点开始的扩散过程能够遍历到终点，那么起点和终点之间一定是连通的，因此他们之间至少存在一条路径，而由于BFS从中心开始呈放射状扩散的特点，它所找到的这一条路径就是最短路径;

启发式搜索

改变广度优先算法原来队列的FIFO模式，给不同的点加入优先级，可以知道，距离终点的曼哈顿距离越小的点，该点的优先级越高

存在问题 然而这导致了先入为主地将最早遍历路径当成最短路径

Dijkstra算法

主要思想是从多条路径中选择最短的那一条：我们记录每个点从起点遍历到它所花费的当前最少长度，当我们通过另外一条路径再次遍历到这个点的时候，由于该点已经被遍历过了，我们此时不再直接跳过该点，而是比较一下目前的路径是否比该点最初遍历的路径花费更少，如果是这样，那就将该点纳入到新的路径当中去（

A*算法

我们需要算法有方向地进行扩散（启发式），另一方面我们需要得到尽可能最短的路径，因此A*就诞生了， 它结合了Dijkstra和启发式算法的优点，以从起点到该点的距离加上该点到终点的估计距离之和作为该点在Queue中的优先级

LPA_star

是A_star的增量版本，它可以适应图形中的变化而无需重新计算整个图形，方法是在当前搜索期间更新前一次搜索的g值（从开始起的距离），以便在必要时进行更正。与A_star一样，LPA*使用启发式算法，该启发性来源于从给定节点到目标路径代价的更低边界。如果保证是非负的（零可以接受）并且从不大于到目标的最低路径的代价，则允许该启发式。

 

启发式搜索和增量式搜索的区别在于，启发式搜索是利用启发函数来对搜索进行指导，从而实现高效的搜索，启发式搜索是一种“智能”搜索，典型的算法例如A_star算法、遗传算法等。增量搜索是对以前的搜索结果信息进行再利用来实现高效搜索，大大减少搜索范围和时间，典型的例如LPA_star、D_star Lite算法等。

D*路径搜索算法

“D_star算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化的场景。

 

同A_star算法类似，D-star通过一个维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D*不是由起始点开始搜索，而是以目标点为起始，通过将目标点置于Openlist中来开始搜索，直到机器人当前位置节点由队列中出队为止（当然如果中间某节点状态有动态改变，需要重新寻路，所以才是一个动态寻路算法）

D_star Lite算法是Koenig S和Likhachev M基于LPA_star 算法基础上提出的路径规划算法。

D_star Lite

D_star Lite 算法之于 LPA_star 算法犹如 D_star 算法之于 A_star 算法。与 LPA_star 采用的正向搜索算法不同，D_star Lite 采用反向搜索方式，效果与D_star 算法相当。无论是前文提到LPA_star 算法还是A_star 算法都不能满足移动机器人在未知环境中的路径规划需求，因为其在未知地图中需要不断的尝试，与边走边找到最优路径背道而驰。此时反向搜索算法能够很好的处理这种情况，D_star 算法虽然可以实现未知环境的路径规划，但效率较低，基于 LPA_star 的D_star Lite可以很好的应对环境未知的情况，其算法核心在于假设了未知区域都是自由空间，以此为基础，增量式地实现路径规划，通过最小化rhs值找到目标点到各个节点的最短距离。

RRT

（RRT / rapidly exploring random tree）的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似，该方法是概率完备且不最优的。

 

PRM

PRM是一种基于图搜索的方法，它将连续空间转换成离散空间，再利用A*等搜索算法在路线图上寻找路径，以提高搜索效率。这种方法能用相对少的随机采样点来找到一个解，对多数问题而言，相对少的样本足以覆盖大部分可行的空间，并且找到路径的概率为1（随着采样数增加，P（找到一条路径）指数的趋向于1）。显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所以PRM是概率完备且不最优的。

 

 

算法比较

A*、LPA*以及D* lite都可以用于静态环境下移动机器人的路径规划，此时三者计算效率都相差不大，都利用了启发式搜索来提高效率，LPA*和D* Lite的增量式搜索在这时没有任何帮助，但对于动态环境的路径规划，A*算法却有心无力，但是对于动态环境下进行二次搜索，LPA*和D* Lite效率明显高于A*。

LPA*以及D* lite原理大体类似，都是基于这样一个思想：发生变化后的环境与最初的地图信息相差不大，可以利用增量式搜索利用先前存储信息来提高二次、三次及以后的搜索效率。

A*算法的代价函数为f=g+h，其各个网格点的优先权也是用f来衡量。

而LPA*和D* Lite都引入rhs变量并作为代价函数，key作为优先权的比较基准，而且key有两 个元素[k1;k2]，打破A*算法出现“平级”的局面；由于D*Lite算法Start点一直在移动，故引入km来提高计算效率。

LPA*和D* Lite引入局部一致性的概念来描述网格点的状态以代替A*的Closedlist、Openlist，即所有Openlist的点都局部不一致，所有局部不一致的点都在Openlist列表上，减轻了储存负担，提高算法效率。