机器人路径规划
基于动态窗口的规划¶
基于网格搜索的规划¶
Dijkstra算法¶
Dijkstra也叫迪杰斯特拉，是典型最短路径算法，计算一个起始节点到路径中其他所有节点的最短路径的算法和思想。在一些专业课程中如数据结构，图论，运筹学等都有介绍。 其思想是一种基础的求最短路径的算法，通过基础思想的变化可以解决很多复杂问题，如导航线路，动态规划等。 注意，Dijkstra算法只能用于路径权值不小于0的静态图
dijkstra的算法思想是从以上最短距离数组中每次选择一个最近的点，将其作为下一个点，然后重新计算从起始点经过该点到其他所有点的距离，更新最短距离数据。 已经选取过的点就是确定了最短路径的点，不再参与下一次计算。
如下图是一个多节点，多路径图。下面以该图为例子讲解dijkstra算法寻找最短路径的过程。
以A点为起始点，求A点到其他点B C D E F 5个点的最短路径，最后得出A到其他点的最短路径。 因为要求A到其他5个点的最短距离，所以构造一个数组记录A到B C D E F 5个点的路径距离。约定：
- 如果A能够直接达到节点，则使用路径长度即权值作为其距离
- 如果A节点不能直接达到节点则使用无穷大表示A到该点距离。
- 任何点到自身都为0
那么在最开始时，A点到图中所有点的距离数组如下：
| A | B | C | D | E | F |
|---|---|---|---|---|---|
| 0 | 10 | 无穷大 | 4 | 无穷大 | 无穷大 |
- 第一次选取
| A | B | C | D | E | F |
|---|---|---|---|---|---|
| 0 | 10 | 无穷大 | 4 | 无穷大 | 无穷大 |
第一步选取该最短路径数组中值最小的一个点。因为A点到本身不需要参与运算，所以从剩下的点中选择最短的一个是D。 第二步以A-D的距离为最近距离更新A点到所有点的距离。即相当于A点经过D点，计算A到其他点的距离。
将现在A到各个点的距离和之前的比较，到相同点取最小值。更新了B C E的距离，得到如下新的最短距离数组：
| A | B | C | D | E | F |
|---|---|---|---|---|---|
| 0 | 6 | 无穷大 | 4 | 无穷大 | 无穷大 |
同时现在A D两点已经计算过，不参与下面的计算 - 第二次选取 第二次选取的数组为第一次中更新过最短距离的数组
| A | B | C | D | E | F |
|---|---|---|---|---|---|
| 0 | 6 | 无穷大 | 4 | 无穷大 | 无穷大 |
第一步：因为A D 不参与选取，所有从剩下的点中选取最近距离是点B
第二步：以B为最新点，更新最短数组
对比现在的最短距离和上一个数组的距离，到相同节点取最小的，C点由19更新成14，E点走A-D-E为10，距离更短所以不更新，得到如下数组：
| A | B | C | D | E | F |
|---|---|---|---|---|---|
| 0 | 6 | 14 | 4 | 10 | 无穷大 |
此时B点加入最短路径范围中。
- 第三次选取 第一步：选取除了A B D节点之外的剩余节点中最短节点，为点E
第二步：以E点为最新节点，更新最短路径数组
因为在上一部中计算达到E点的距离时没有更新距离，A-D-E 为10 最短，所以更新E点到B C F点的距离时走的路径是A-D-E。注意这里的最短距离有对应的路径，选择最小值就是选择最短距离。 对比现在的最短距离和上一个数组的距离，到相同节点取最小的，更新C点走A-D-E-C 为11，比之前的A-D-B-C14距离更近，更新到F点距离，得到如下数组：
| A | B | C | D | E | F |
|---|---|---|---|---|---|
| 0 | 6 | 11 | 4 | 10 | 22 |
此时E点加入最短路径范围中。
- 第四次选取 第一步：选取除了A B D E节点之外的剩余节点中最短节点，为点C
第二步：以C点为最新节点，更新最短路径数组
对比现在的最短距离和上一个数组的距离，到相同节点取最小的，更新到F点距离，可以得到如下数组：
| A | B | C | D | E | F |
|---|---|---|---|---|---|
| 0 | 6 | 11 | 4 | 10 | 16 |
- 第五次选取
第一步：选取除了A B C D E节点之外的剩余节点中最短节点，也就是最后一个节点：F
第二步：以F点为最新节点，更新最短路径数组。由于F点是最后一个点，所以也不用更新数组，目前的数组就是所求数组 将F点加入最短路径范围中，此时所有的点都加入了最短路径范围，也就是说A点到所有点的距离都找到了。最总得出的距离值为：
A star算法¶
适用于静态图搜索
搜索区域¶
我们假设某人要从A点移动到B点，但是这两点之间被一堵墙隔开。绿色是A，红色是B，中间蓝色是墙
开始搜索¶
- 从起点A开始，并把它就加入到一个由方格组成的open list(开放列表) 中。这个open list有点像是一个购物单。当然现在open list里只有一项，它就是起点A， 后面会慢慢加入更多的项。Open list里的格子是路径可能会是沿途经过的，也有可能不经过。基本上open list是一个待检查的方格列表。
- 查看与起点A相邻的方格(忽略其中墙壁所占领的方格及其他非法地形占领的方格)，把其中可走的或可到达的方格也加入到open list中。把起点A设置为这些方格的父亲。 当我们在追踪路径时，这些父节点的内容是很重要的。
- 把A从open list中移除，加入到close list中，close list中的每个方格都是现在不需要再关注的。
如下图所示，深绿色的方格为起点，它的外框是青色色，表示该方格被加入到了close list 。与它相邻的黑色方格是需要被检查的，他们的外框是亮绿色。每个黑方格 都有一个灰色的指针指向他们的父节点，这里是起点A。
下一步，我们需要从open list中选一个与起点A相邻的方格，按下面描述的一样或多或少的重复前面的步骤。但是到底选择哪个方格好呢？具有最小F值的那个
路径排序¶
计算出组成路径的方格的关键是下面这个等式
F = G + H
G是从起点A移动到指定方格的移动代价，也就是从起点到该点的距离。
H是从指定的方格移动到终点B的估算成本。这个通常被称为试探法,为什么这么叫呢，因为这是个猜测。直到我们找到了路径我们才会知道真正的距离，因为途中有各种各样的东西 (比如墙壁，水等)。
我们的路径是这么产生的：反复遍历open list，选择F值最小的方格。这个过程稍后详细描述。我们还是先看看怎么去计算上面的等式。
如上所述，G是从起点Ａ移动到指定方格的移动代价。在本例中，横向和纵向的移动代价为10 ，对角线的移动代价为14 。之所以使用这些数据，是因为实际的对角移动距离是2的平方根， 或者是近似的1.414倍的横向或纵向移动代价。使用10和14就是为了简单起见。比例是对的，避免小数的计算。这并不是我们没有这个能力或是不喜欢数学。使用这些数字也可以使计算机更快。 稍后你便会发现，如果不使用这些技巧，寻路算法将很慢。
既然我们是沿着到达指定方格的路径来计算G值，那么计算出该方格的G值的方法就是找出其父亲的G值，然后按父亲是直线方向还是斜线方向加上10或14 。随着我们离开起点而得到更多的方格， 这个方法会变得更加明朗。
有很多方法可以估算H值。这里我们使用Manhattan方法，计算从当前方格横向或纵向移动到达目标所经过的方格数，忽略对角移动，然后把总数乘以10。之所以叫做Manhattan方法， 是因为这很像统计从一个地点到另一个地点所穿过的街区数，而你不能斜向穿过街区。重要的是，计算H是，要忽略路径中的障碍物。这是对剩余距离的估算值，而不是实际值，因此才称为试探法。
把G和H相加便得到F。我们第一步的结果如下图所示。每个方格都标上了F，G，H的值，就像起点右边的方格那样，左上角是F，左下角是G，右下角是H。
好，现在让我们看看其中的一些方格。在标有字母的方格，G=10 。这是因为水平方向从起点到那里只有一个方格的距离。与起点直接相邻的上方，下方，左方的方格的G值都是10， 对角线的方格G值都是14。
H值通过估算起点于终点(红色方格)的 anhattan距离得到，仅作横向和纵向移动，并且忽略沿途的墙壁。使用这种方式，起点右边的方格到终点有3个方格的距离，因此H=30。 这个方格上方的方格到终点有4个方格的距离(注意只计算横向和纵向距离)，因此H=40。对于其他的方格，你可以用同样的方法知道H值是如何得来的。
继续搜索¶
为了继续搜索，我们从open list中选择F值最小的节点，然后对所选择的方格作如下操作：
- 把它从open list里取出，放到close list中。
- 检查所有与它相邻的方格，忽略其中在close list中或是不可走的方格(比如墙，水，或是其他非法地形)，如果方格不在open lsit中，则把它们加入到open list中。把我们选定的方格设置为这些新加入的方格的父亲。
- 如果某个相邻的方格已经在open list中，则检查这条路径是否更优，也就是说经由当前方格(我们选中的方格) 到达那个方格是否具有更小的G值。如果没有，不做任何操作。 相反，如果G值更小，则把那个方格的父亲设为当前方格(我们选中的方格)，然后重新计算那个方格的F值和G值。
每个方格的F值，直接把G值和H值相加就可以了。
让我们看看它是怎么工作的。在我们最初的9个方格中，还有8个在open list中，起点被放入了close list中。在这些方格中，起点右边的格子的F值40最小， 因此我们选择这个方格作为下一个要处理的方格。它的外框用青线打亮。
首先，我们把它从open list移到close list中。然后我们检查与它相邻的方格。它右边的方格是墙壁，我们忽略。它左边的方格是起点，在close list中， 我们也忽略。其他4个相邻的方格均在open list中，我们需要检查经由这个方格到达那里的路径是否更好，使用G值来判定。让我们看看上面的方格。 它现在的G值为14 。如果我们经由当前方格到达那里，G值将会为20(其中10为到达当前方格的G值，此外还要加上从当前方格纵向移动到上面方格的G值10)。 显然20比14大，因此这不是最优的路径。如果你看图你就会明白。直接从起点沿对角线移动到那个方格比先横向移动再纵向移动要好。
当把4个已经在open list中的相邻方格都检查后，没有发现经由当前方格的更好路径，因此我们不做任何改变。现在我们已经检查了当前方格的所有相邻的方格，并也对他们作了处理， 是时候选择下一个待处理的方格了。
因此再次遍历我们的open list，现在它只有7个方格了，我们需要选择F值最小的那个。有趣的是，这次有两个方格的F值都54，选哪个呢？没什么关系。 从速度上考虑，选择最后加入open list的方格更快。这导致了在寻路过程中，当靠近目标时，优先使用新找到的方格的偏好。但是这并不重要。 (对相同数据的不同对待，导致两种版本的A* 找到等长的不同路径)。
我们选择起点右下方的方格，如下图所示。
这次，当我们检查相邻的方格时，我们发现它右边的方格是墙，忽略之。上面的也一样。
我们把墙下面的一格也忽略掉。为什么？因为如果不穿越墙角的话，你不能直接从当前方格移动到那个方格。你需要先往下走，然后再移动到那个方格，这样来绕过墙角。 (注意：穿越墙角的规则是可选的，依赖于你的节点是怎么放置的)
这样还剩下5个相邻的方格。当前方格下面的2个方格还没有加入open list，所以把它们加入，同时把当前方格设为他们的父亲。在剩下的3个方格中，有2个已经在 close list中(一个是起点，一个是当前方格上面的方格，外框被加亮的)，我们忽略它们。最后一个方格，也就是当前方格左边的方格，我们检查经由当前方格到达那里是否具有更小的G 值。没有。因此我们准备从open list中选择下一个待处理的方格。
不断重复这个过程，直到把终点也加入到了open list中，此时如下图所示。
注意，在起点下面2格的方格的父亲已经与前面不同了。之前它的G值是28并且指向它右上方的方格。现在它的G值为20，并且指向它正上方的方格。这在寻路过程中的某处发生， 使用新路径时G值经过检查并且变得更低，因此父节点被重新设置，G和F值被重新计算。尽管这一变化在本例中并不重要，但是在很多场合中，这种变化会导致寻路结果的巨大变化。
那么我们怎么样去确定实际路径呢？很简单，从终点开始，按着箭头向父节点移动，这样你就被带回到了起点，这就是你的路径。如下图所示。 从起点A移动到终点B就是简单从路径上的一个方格的中心移动到另一个方格的中心，直至目标。
总结¶
其实A*就相当于带剪枝的Dijkstra算法，因为F>=真实值，所以每次选择最小的就相当于把更大的潜在路线剪掉了，只不过这里的H选择的是曼哈顿距离， 所以不保证能找到最优的路线。现在你已经看完了整个的介绍，现在我们把所有步骤放在一起：
- 把起点加入open list。
- 重复如下过程：
- 遍历 open list ，查找F值最小的节点，把它作为当前要处理的节点。
- 把这个节点移到 close list 。
- 对当前方格的8个相邻方格的每一个方格：
- 如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。
- 如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。
- 如果它已经在open list中，检查这条路径(即经由当前方格到达它那里)是否更好，用G值作参考。更小的G值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的G和F值。如果你的open list是按F值排序的话，改变后你可能需要重新排序。
- 停止，当你
- 把终点加入到了 open list 中，此时路径已经找到了，或者
- 查找终点失败，并且 open list 是空的，此时没有路径。
- 保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。
D star算法¶
简介¶
“D算法”的名称源自Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。 它是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化*的场景。
由于是动态环境，算法主要是两个阶段，
- 第一阶段：是从目标点往起点进行探索，得到搜索区域节点距离终点最短路径的信息（父节点信息）
- 第二阶段：是机器人动态环境交互中，遇见障碍物时对路径做动态修改，机器人从起点开始往终点行走过程中遇见环境发生变化时的动作过程。
同A-Star算法类似，D-star通过一个维护一个优先队列（OpenList）来对场景中的路径节点进行搜索，所不同的是，D-Star不是由起始点开始搜索， 而是以目标点为起始，通过将目标点置于Open list中来开始搜索，直到机器人当前位置节点由队列中出队为止 （当然如果中间某节点状态有动态改变，需要重新寻路，所以才是一个动态寻路算法）。
相比A-star算法，D-star的主要特点就是由目标位置开始向起始位置进行路径搜索，当物体由起始位置向目标位置运行过程中，发现路径中存在新的障碍时，对于目标位置到新障碍之间的范围内的路径节点，新的障碍是不会影响到其到目标的路径的。新障碍只会影响的是物体所在位置到障碍之间范围的节点的路径。在这时通过 将新的障碍周围的节点加入到Openlist中进行处理然后向物体所在位置进行传播，能最小程度的减少计算开销。 D*路径搜索的过程和Dijkstra算法比较像，A-star算法中f(n)=g(n)+h(n)，h(n)在D-star中并没有体现，路径的搜索并没有A-star所具有的方向感，即朝着目标搜索的感觉，这种搜索更多的是一种由目标位置向四周发散搜索，直到把起始位置纳入搜索范围为止，更像是Dijkstra算法。 同时，由示例的算法效果来看，D_star算法能够在障碍物发生变化时，仍能找到一条路径，但不一定是一条最短的路径。
举例¶
起点为（2,1），目标终点为(7,6)，搜索中每个节点都有一个反向指针，指向让其h最小的父节点。当起点在open表被弹出时，从起点到终点的路径就出现了。h代表该节点到终点，也就是目标点G的代价。k是该节点所有过的h的最小值。b是父节点
假设我们检测到某点被阻碍了，如上图的（4,3）点，它变成了墙，它的h会被改为一个大值，并将该点和其邻接点重新加入open表中进行搜索，该点h>k变成了“raise”态， （h=k为lower态）通过搜索，如果它自己周围找不到让h低下来（变为lower）的途径，则它会将这一状态传播到周围。（3,2）点会受影响，它的h将改变，（4,4）（5,4）（5,3）均不会受影响。 具体原因可详细看论文。(3,2)点呢，它可以找到点（4,1），只需要把（3,2）的父节点从（4,3）改为（4,1），就能够降低其h的值，它的最终h,k将变为6.2+1.4=7.6 就变为lower态， 这一改变也将扩散到（2,1）（2,2）点。
最终，只需要把（3,2）的父节点从（4,3）改为（4,1）即可，因为后面到目标点的路径其实是之前计算过的，不必计算。这样就保证了对动态障碍物再规划的效率。最终的路径规划为：
D star lite算法¶
D_star Lite算法是Koenig S和Likhachev M基于LPA_star 算法基础上提出的路径规划算法。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值找到目标点到各个节点的最短距离。在移动机器人按照规划的路径进行前进时其所到的节点即设置为起始节点，因此路径变化或者key值需要更新时， 需要更新从目标点到新起点的启发值以及估计成本。由于移动机器人不断的靠近目标点，节点的启发值将不断减少，且减少至不会超过h（start Org,start New）。 由于每次都要减去相同的值，开启列表的顺序并不会改变，因此可以不进行这部分的计算，这样便避免了每次路径改变时的队列遍历过程。
若前行过程中发现障碍物则将障碍物所对应环境地图位置设置为障碍物空间，并再以之为起点利用“路径场”信息重新规划出一条路径来。此时不仅更新规划路径的节点数据， 也要更新智能体遍历过的节点。其关键点在于如何在未知的环境中根据传感器获取的极少周边地图信息来进行最有效的靠近目标点的任务。其实整个靠近的过程一直在扩大已知地图范围， 尽可能少的尝试次数来实现完成抵达目标点的任务。下图为 D_star Lite 搜索示意图，黑点是在按照反向搜索的路径执行时发现的障碍点，到遇到不能通行的障碍点后便更新地图信息，重新规划出一条新的路径继续前行。
D_star Lite结合了D_star算法动态规划的特性（由目标位置开始向起始位置进行路径搜索。当路径中存在新的障碍时，对于目标位置到新障碍之间的范围内的路径节点， 新的障碍是不会影响到其到目标的路径的）和LPA_star算法的利用增量式搜索特性。
D_star Lite算法能够很好的适用于未知环境做路经规划，由于其增量规划的思想，它可以做到较少重规划次数以及较少的重规划影响节点数。但是当状态空间比较大， 也就是环境地图比较大的时候，采用的 D_star Lite路径规划算法的反向搜索过程需要维护的栅格节点数急剧增加，增加了搜索的时间复杂度。除此之外，D_star Lite 路径规划不能应对环境复杂的情况，即局部环境的精细规划。对于大环境下的路径规划，D_star Lite算法的做法是将环境地图进行更细粒度的栅格化， 虽然在足够细粒化的环境地图中可以实现较优的路径解，但也会带来更多的规划序列导致执行次数以及重规划次数增多，进而路径规划执行花费时间也会变得更长。