路径规划算法

1、Dijkstra算法

从物体所在的初始点开始，访问图中的结点。迭代检查 待检查节点集中的节点，该节点从初始节点向外扩展，直到达到目标节点，该算法能够保证找到一条从初始点到目标点的最短路径。

2.最佳优先搜索（BFS）算法

按照类似的流程进行，不同的是它能够评估任意节点到目标节点的 代价 （距离大小）。它选择的不是离初始点最近的节点，而是离目标点最近的节点 。它用了一个启发式函数快速的导向目标节点。例如，如果目标位于出发点的南方，BFS将趋向于导向南方的路径。

如下所示；越黄的值代表越高的启发式值（移动到目标点的代价越高），越暗淡的节点代表越低的启发式值（移动到目标的代价越低）。

3 A*算法

启发式方法，如BFS和常规方法如Dijsktra算法结合在一起的算法，有点不同的是，类似BFS的启发式方法经常给出一个近似解而不是保证最佳解，A是路径搜索中最受欢迎的选择，它相当灵活，并且能用于多种多样的情理之中，A * 潜在的搜索一个很大的区域，A 算法能用于搜素最短路径，A* 能用启发式函数引导它自己，在简单的情况中，它和BFS一样快。

在凹型障碍物的例子中，A*找到一条和Dijkstra算法一样好的路径：

A * 算法把Dijkstra算法（靠近初始点的结点）和BFS算法（靠近目标点的结点）的信息块结合起来。A * 的标准术语中：g(n) 表示从初始结点到任意结点n的代价，h(n)表示从结点n到目标点的启发式评估代价。在上图中，yellow(h)表示远离目标的结点而teal(g)表示远离初始点的结点，当从初始点向目标点移动时，A* 权衡这两者。每次进行主循环时，它检查f(n)最小的结点 n，其中f(n) = g(n) + h(n)。

启发式算法

启发式函数h(n)告诉A*从任意结点n到目标结点的最小 代价 评估值。选择一个好的启发式函数是重要的。

启发式函数可以控制A*的行为；

1 一种极端的条件下，如果h(n)=0;那么只有g（n）起作用，此时A* 转换为Dijkstra算法，

2 如果h(n)经常都比从n移动到目标的实际距离小（或者相等），则A* 能保证找到一条最优路径，h（n）越小，A*的扩展点越多，运行也就越慢。

3.如果h(n)精确地等于从n移动到目标的代价，则A*将会仅仅寻找最佳路径而不扩展别的任何结点，这会运行得非常快

理想情况下，我们想最快的得到最短路径。如果我们的目标太低，我们仍然会得到最短路径，不过速度变慢了；如果我们的目标太高，那我们就放弃了最短路径。但A运行的更快。

A*计算f(n) = g(n) + h(n)。为了对这两个值进行相加，这两个值必须使用相同的衡量单位。

精确的启发式函数

如果你的启发式函数精确的等于实际最佳路径，此时 A* 扩展的结点将非常的少。A* 算法内部发生的事情是：在每一结点，他都计算f(n) = g(n) + h(n)；当h(n)精确地和g(n)匹配时，f(n)的值在沿着该路径时将不会改变，不在正确路径（right path）上的所有结点的f值均大于正确路径上的f值，如果已经有较低f值的结点，A* 将不考虑f值较高的结点。

（1）预计算的精确启发式函数

构造精确启发函数的一种方法是预先计算任意一对结点之间最短路径的长度。

（2）线性精确启发式算法

4， 网格地图中的启发式算法

在网格地图中，有一些众所周知的启发函数。

4.1 曼哈顿距离

标准的启发函数是曼哈顿距离，考虑代价函数，并且找到一个从一个位置移动到邻近的最小代价D，H(n) = D * (abs ( n.x – goal.x ) + abs ( n.y – goal.y ) ；

4.2 对角线距离

如果在你的地图中你允许对角运动那么你需要一个不同的启发函数；h(n) = D * max(abs(n.x - goal.x), abs(n.y - goal.y))

4.3 欧几里得距离

4.4 Breaking ties

导致低性能的一个原因来自于启发函数的ties。当某些路径具有相同的f值的时候，它们都会被搜索（explored），尽管我们只需要搜索其中的一条：

为了解决这个问题，我们可以为附加函数添加一个附加值，附加值对于结点必须是确定性的，，而且他必须让 f 体现区别，一种添加附加值的方式是稍微改变衡量单位，如果我们减少衡量单位（译者注：原文为scale it downwards），那么当我们朝着目标移动的时候 f 将逐渐增加。很不幸，这意味着A倾向于扩展到靠近初始点的结点，而不是靠近目标的结点。我们可以增加衡量单位，A就会倾向于扩展到靠近目标的结点。

4.5 区域搜索

如果你想搜索邻近目标的任意不确定结点，而不是某个特定的结点，你应该建立一个启发函数h’(x)，使得h’(x)为h1(x), h2(x), h3(x)。。。的最小值，而这些h1, h2, h3是邻近结点的启发函数。

如果不考虑具体实现代码，A* 算法是相当简单的，有两个集合OPEN和CLOSE集，其中OPEN 集保存待考察的结点，开始时，OPEN集只包含一个元素，初始结点。CLOSE集保存已考察过的结点，开始时，CLOSED集是空的。如果绘制成图，OPEN集就是被访问区域的边境，而CLOSED集合则是被访问区域的内部。每个结点同时保存其父结点的指针，因此我们可以知道它是如何被找到的。

5 RRT算法

快速搜索随机数（RRT）算法，是一种增量式采样的搜索算法，该方法在应用中不需要进行任何整定，具备良好的性能，它利用增量式方法构建搜索树，以逐渐提高分辨能力，而无需设置任何分辨率参数，在极限条件下，该搜索树将稠密的布满整个空间，此时搜索树由很多较短曲线和路径构成，以实现充满整个空间的目的

为了提高搜索效率采用双向随机搜索树(Bi~RRT)，从起始点和目标点并行生成两颗，直到两棵树相遇，算法收敛，由于这个算法收敛速度快，因此广泛用于路径规划中

5.1滚动在线RRT算法

基本的RRT算法，倾向于遍历整个自由空间获得可行路径，这使得不可能用于未知或动态环境中的机器人在线运动规划。滚动规划的思想可以将RRT算法进行改进，使其具备在线规划的能力。

滚动规划;机器人利用局部信息进行局部运动规划，并根据一定的评价标准得到局部目标，机器人到达局部目标后在进行新的局部规划如此反复进行直到达到全局目标。

滚动规划的基本原理：

① 环境信息预测：在滚动的每一步，机器人根据探测到的视野信息和已知的环境信息，建立环境模型，包括设置已知区域内结点类型信息。

②局部滚动规划：将上述环境信息模型看成一个优化的窗口，在此基础上，根据目标点的位置和特定的优化策略计算出下一步的最优子目标，然后根据子目标，选择局部优化算法，确定向子目标行进的局部路径，并实施当前策略，依照所规划的局部路径行进若干步，窗口相应向前滚动。

③反馈信息校正：根据局部最优路径，机器人行走一段时间后，机器人会探测到新的未知信息，此时可以根据机器人在行走过程中探测到的新的信息补充校正原来的环境模型，用于滚动后下一步的路径规划。

基于滚动窗口的路径规划算法依靠实时探测到的局部环境信息，以滚动方式进行在线规划。

基于滚动窗口的路径规划算法的具体步骤如下：

步骤0：对起点，终点，工作环境，机器人的视野半径，步长进行初始化

步骤1：如果终点到达，规划终止

步骤2：对当前滚动窗口内的环境信息进行刷新；

步骤3：产生局部子目标；

步骤4：根据子目标及已知环境信息，在当前滚动窗口内规划一条优化的局部可行路径；

步骤5：依规划的局部路径行进一步，步长小于视野半径；

步骤6：返回步骤1

6、 人工势场法

人工势场法是由K提出的用于机器人运动规划的虚拟力方法，其基本思想是将目标和障碍物对机器人运动的影响具体化成人造市场，目标处 势能低，障碍物处 势能高，这种势差产生了目标对机器人的引力，障碍物对机器的斥力，其合力控制机器人沿势场负梯度方向向目标点运动，人工势场法方便计算，得到的路径安全平滑，但是复杂的势场环境可能在目标点之外产生局部最优。

解决人工势场法局部最优值问题，主要有两个方向：一是构造合适的势函数以减少或避免局部最小值的出现，需要全局地图的信息，并且依赖于障碍物的形状，情况复杂难以应用。

​ 二是在机器人遇到局部极小值点后，结合其他的方法使机器人离开局部极小点。多利用搜索法，多势场法和沿墙行走法等方法使机器人离开局部极小点。

6.1 人工势场法算法改进

当机器人的运行环境中包含形状复杂或者距离很近的障碍物时，可能出现势场局部极小点，导致机器人在该处停止或在其周围振动。如下图所示，当环境中出现“陷阱”形障碍物或者与目标成特定位置关系的障碍物时，可能在人工势场中产生局部极小点（图中L点），当机器人运动到局部极小点附近时，势场的负梯度方向指向L点。机器人将在L点处停止或在其附近振动或作圆周运动。

为了使机器人从局部极小点中逃离，在人工势场法的基础上引入应激行为，即增加绕行行为。当机器人遇到局部极小点时，忽略目标引力势能的作用，沿着斥力势的等势面方向移动，直到机器人离开局部极小区域。

7.BUG算法

BUG算法是一种完全应激的机器人避障算法，在未遇到障碍物时，沿直线向目标运动；在遇到障碍物后，沿着障碍物边界绕行，并利用一定的判断准则离开障碍物继续直行，这种应激式的算法计算简便，不需要获知全局地图和障碍物形状，具备完备性。但是其生成的路径平滑性不够好，对机器人的各种微分约束适应性比较差。

7,1 BUG1算法

该算法的基本思想是在没有障碍物时，沿着直线向目标运动可以得到最短的路线。当传感器检测到障碍物时，机器人绕行障碍物直到能够继续沿直线项目标运动。BUG1算法实现了最基本的向目标直行和绕行障碍物的思想。

7.2 BUG2算法

BUG2算法也有两种运动：朝向目标的直行和沿边界绕行。与BUG1算法不同的是，BUG2算法中的直线是连接初始点和目标点的直线，在计算过程中保持不变。当机器人在点遇到障碍物时，机器人开始绕行障碍物，如果机器人在绕行过程中在距离目标更近的点再次遇到直线，那么就停止绕行，继续沿着直线，向目标直行。如此循环，直到机器人到达目标点，如果机器人在绕行过程中未遇到直线上与目标更近的点，那么得出结论，机器人不能到达目标。

7.3 TangentBUG算法

TangentBUG算法是对BUG2算法的提高。它利用机器人上距离传感器的读数对障碍物做出提前规避，可以获得更短更平滑的机器人路径。TangentBUG算法也有两种行为：直行（motion-to-go）和环绕障碍物（boundary-following）。

8 增量式启发算法

8.1 LPA*算法

是一种增量启发式搜索版本的A算法，这种路径规划算法适用于随着时间改变导致有限栅格地图上的边缘代价c(s1,s2)改变的问题，也就是随着时间改变障碍物增多或减少，网格点发生增删等，在许多场合下比再利用A重新搜索更高效。

8.2 D* Lite算法

D* Lite算法是以LPA为基础，结合A算法思想，提出一种增量启发式算法，适用于在未知环境中的导航以及路径规划，广泛用于目前各种移动机器人和自主车辆载具，例如“机遇号”和“勇气号”火星车测试的原型导航系统。