路径规划（二）、A*算法

路径规划（二）、A算法（转）
一、关于A算法基本原理的相关博客或地址
1.这是一篇讲述如何使用matlab实现A算法的文章，其程序为动态演示A搜索过程以及路径规划结果。
原文章链接
2.这是一篇关于A算法的详细讲解，是一片译文，但是翻译的很棒。写的非常详细，对A算法的原理、流程和实现为程序的时候需要额外考虑的东西。
A*译文随笔笔记https://www.cnblogs.com/chxer/p/4542068.html
3.这是一篇讲述A寻路算法的文章，它从广度优先搜索（Breadth-First-Search,BFS）、Dijkstra算法以及贪心算法（最佳优先搜索（Best-First-Search，BFS））的基本原理（其中还有动态演示具体搜索过程的GIF）讲到了A*，一定不要将广度优先搜索和最佳优先搜索搞混，并讨论了启发函数 H 对搜索结果与过程的影响。以下为其链接：
A寻路算法入门详解https://www.cnblogs.com/wangnfhy/p/4956711.html
4.这是一篇A*的外文介绍，非常之详细。有可控制的动画演示过程
优秀外文A介绍https://www.redblobgames.com/pathfinding/a-star/introduction.html
二、A算法的变形
在明白A算法的基本原理之后，我们会知道，启发函数对A的搜索速度有着很大的影响，即启发函数对于两点之间的距离描述越精确，搜索速度越快，路径也会越短。因此，一个精确的启发函数模型对于A算法而言是很有必要的。但是，由于精确描述两点之间距离的启发函数的构建实在太为困难，因此，衍生了多种A算法的变形，来处理或改善一些经典A算法存在的缺点。






在经过一段时间的学习之后，对Astart算法进行一个总结：
避障路径规划问题具有以下特点：
（1）复杂性。在复杂环境（例如迷宫）或者是动态时变环境下，避障路径规划问题非常复杂，且计算量会非常大。
（2）随机性。复杂环境中存在很多随机性和不确定性。这一点对于在动态时变环境下的路径规划中尤为突出。
（3）约束条件繁多。在运动空间中的路径规划，我们要考虑它的约束条件，即明确的告诉机器人哪些地方能去，哪些地方不能去。这些约束条件大概可以归为几何约束条件和物理约束条件。其中，几何约束条件指的是运动对象（例如机器人）的外观形状、尺寸大小；而物理约束则是指它在该环境下的速度、加速度、转弯角度等。
（4）多个目标。运动对象在运动过程中，根据不同的要求会规划不同属性的路径，这样就会导致在进行路径规划的时候可能会存在多中目标，例如路径长度最小、时间最短、安全性能最好以及能源消耗最小等。但是，这些目标之间通常会存在冲突，因此，我们需要根据实际情况去着重考虑其中的一个因素，忽略或弱化其他的一些因素。
求解避障路径规划涉及到的问题会包括：位姿空间、障碍物的环境表示、规划方法（理论）和路径搜索策略（设计）等。
A算法
1.A算法的由来
1.1 广度优先搜索算法、Dijkstra算法与贪心最佳优先算法（GBFS）
其实，在任一地图上的搜索算法的本质就是我们对搜索过程的跟踪。反映在栅格地图上则为从起点开始向外扩展的过程。当然，搜索仅仅是指从起点出发找到了目标点，但是搜索过程循环并不会规划路径，它只会告诉我们如何搜索地图上的内容。而路径规划其实是对搜索过程的跟踪与记录。
1.1.1 广度优先搜索算法
广度优先搜索算法其实就是从起点出发向外扩展，逐个访问当前点的所有邻居，与此同时，将邻居的父节点设为当前点，即完成了搜索过程的跟踪与记录。当搜索结束时，再通过对目标点的回溯输出规划的完整路径。其规划完成的示意图如下所示：

广度优先搜索的路径回溯
广度优先搜索算法是最简单的寻路算法之一（还有另外一种叫做深度优先算法），它不仅仅适用于上图所示的栅格地图，还适用于任何类型的图形结构，如拓扑地图。一般来说，我们在进行路径规划的时候，只需要找到一条符合要求的路径即可，并不需要找到起点到终点的所有路径，这就意味着我们并不需要搜索完整个地图，在搜索到目标点的时候，可以提前退出搜索。
1.1.2 Dijkstra算法介绍
1）.Dijksra原理
在上面的描述中，我们发现搜索的每一步付出的代价几乎是一样多的，但是在一些表达更加丰富的地图上，相邻栅格的移动代价往往是不同的。比如游戏地图中，平原和沙漠的代价为1，森林的代价为3，山地的代价是5等，通过设定不同的移动代价，可以让我们规划出的路径更加的符合实际的要求。如下图所示的路径规划示意图：

上图中，左图代表我们搜索的步数，右图代表我们搜索到的路径的距离（或长度），从图中我们可以看到，最终规划出的路径避开了移动成本较高的绿色区域。
为了完成上述的搜索方法，我们需要使用一些信息来判定路径的长短，以此来避开移动成本较高的区域。Dijkstra算法就可以解决上述问题。
Dijkstra算法，也称为统一成本搜索，允许我们优先考虑要探索的路径。它不是平等地探索所有可能的路径，而是倾向于降低成本路径。我们可以分配更低的成本来鼓励在道路上行驶，更高的成本来避免森林，更高的成本来组织靠近敌人，以及更多。当移动成本不同的时候，我们使用它而不是广度优先搜索。除此之外，对同一路径的规划，广度优先搜索规划路径所需要的时间往往要比Dijkstra算法的长。下图可以很明确表达出这两者的区别：

当地图中的移动代价不统一的时候
当移动成本相同或近似的时候，这两者规划出的路径相似。对于通常意义上的栅格地图来说，其表达的内容不会很丰富，即分为可通行区域和不可通行区域。因此，其移动成本就是相似的，此时，Dijkstra算法与广度优先搜索算法没什么太大的区别。如果对于拓扑地图来说，Dijkstra算法具有更高的使用价值。但是需要注意的一点就是，对于拓扑地图来说，Dijkstra算法的使用范围也是有限制的，它仅适用于有向无环的拓扑地图，这涉及到图论最短路的一些知识，这里不做详细说明。
2）.流程
对于栅格地图来说，每个区块之间的移动代价是相同的（在仅考虑前后左右四个方向移动的情况下），其算法的流程如下所示：
（1）设定起始点S，将起点S和移动代价C=0存入openlist中
（2）判断openlist是否为空，若为空，则算法结束，反之则继续下一步；
（3）对openlist中记录的点根据其移动代价C的大小进行排序；
（4）从openlist中取出C值最小的那个点N，判断点N是否为目标点，若是，则算法结束；否则算法继续；
（5）将点N加入closelist中，并且对N点周围的四个点依次进行遍历：
a.若该点为障碍物或者closelist中的点，忽略；
b.若N在openlist中，计算N对应的C值的大小C1，取出openlist中保存的N点对应的C值，比较C1与C的大小，若C1大于C不作处理；若C1小于C则C=C1，并将N设置为该点父节点；
c.若N不在openlist中，计算C的值的大小，将其加入openlist中，并将点N设为该点的父节点；
（6）重复步骤（3）-（5）直到算法结束。
1.1.3贪心最佳优先搜索算法（GBFS）
贪心最佳优先搜索算法的基本思想就是贪心策略，即每次都选择目前的情况来说最好的情况，这样，一直选择最优的点，最终规划出的路径即为最优路径，但是这仅仅是在起点与终点之间没有任何障碍物的情况下才能称为最优路径，而起点与终点之间一旦存在障碍物，则不能保证找到的路径为最优。通过广度优先搜索算法和贪心最佳优先搜索算法的比较，即下图所示，即可明白贪心最佳优先搜索策略的优缺点：

(a)起点与终点之间没有障碍物的时候

(b)起点与重点之间存在障碍物的时候
有上面的图片我们可以看到，由于贪心最佳优先搜索算法使用了启发式搜索，使其能够更快的向着目标点的方向搜索，搜索栅格少，因此其路径规划所需要的时间大大缩短，但是同时，不能保证所规划的路径一定是最优的。
2.A算法的原理
为了解决栅格地图中Dijkstra算法搜索速度慢、贪心最佳优先搜索算法不能保证路径最优的缺点，我们将二者有机结合，使新的方法能够同时拥有二者的优点，即保证路径最优的情况下，搜索速度尽可能的快。这种新方法就是A搜索算法。
A搜索算法的本质就是对Dijkstra算法和贪心最佳优先搜索的的改进，可以说是二者的综合体。Dijkstra算法利用当前搜索点与起始点的距离G的大小指导搜索从起点开始向外扩展，直到搜索到目标点停止搜索，这意味着Dijkstra算法可以找到所有位置的路径。贪心最佳优先搜索算法则是通过当前位置与目标点的距离H值的大小来引导搜索方向，使其在搜索过程中，始终沿着距离目标点最近的方向移动，即H值最小的那个栅格，这意味着它可以很快的到达目标点，但是，如果起点与终点之间存在障碍物，则会导致该方法规划的路径可能不是最优路径。A搜索算法则是将这两种方法的优点统一起来，它通过一个启发函数来完成路径搜索及规划，该启发函数为F=G+H，这样A可以更快的规划出到达目标点的最优路径。通过下图我们可以对这三者之间的差异进行一个比较：
通过下图我们可以看到，从规划的路径来看，最佳优先搜索算法规划出的路径长度明显要大于其余两种算法，而从搜索的栅格数目来说（即搜索速度）Dijkstra算法搜索的栅格数目最大，这意味着Dijkstra算法在规划路径时需要更多的时间。因此，我们可以看到，A的巨大优势，即搜索速度快且路径最优。

(a)起点与终点之间没有障碍物时，Dijkstra、GBFS、A算法的比较

(b)起点与终点之间有障碍物时，Dijkstra、GBFS、A算法的比较
3.A算法的流程（附图）

4.A算法的改进方向
（1）启发函数
目前所接触的搜索方法中，使用的启发式函数一般都是两点的距离函数，这些函数所计算出的距离通常是一个近似于实际距离的数值。对于启发函数来说，它对实际距离的描述越为精确，它的搜索速度越快，路径会越短。因此，我们需要根据实际情况去着重考虑启发函数的选取，满足实际应用要求的搜索速度和路径长度。所以我们可以通过H(n)来控制A*,如果我们的估算H(n)有着100%的准确度，那么我们可以非常快速的获得最短的路径，但是如果估算值远小于实际距离，那么我们会获得最短距离，但是它搜索速度减慢；但是如果估算值远大于实际距离，那么我们就放弃了最短路径，但是A的搜索速度会非常快。通常情况下，这些距离函数有以下两种：
设两点的坐标分别为A(x1,y1)和B(x2,y2)，则：
a.欧氏距离（任一移动方向）
Diatance=sqrt((x1-x2)^2+
(y1-y2)^2)
在起点和终点之间没有障碍物的时候，欧式距离所计算得到的距离是等于实际距离的，但是二点之间存在障碍物的时候，欧式距离会小于实际距离。但是由于欧氏距离比曼哈顿或对角线距离短，我们仍然会获得最短距离，但是A将需要更长的时间才能得到最终的规划结果。
b.曼哈顿距离（4个移动方向）
Distance=abs(x1-x2)+abs(y1-y2)
曼哈顿距离只有在起点与终点之间没有障碍物的时候才等于实际距离或略微大于实际距离，否则，曼哈顿距离小于实际距离。因此，当起点与终点之间不存在障碍物的时候，算法保证找到最短路径，而两者之间一旦出现障碍物，则规划路径的最短就无法保证了。
c.对角线距离（8个移动方向）
设D2为对角线移动成本，D为直线移动成本，则该启发函数的形式为：
dx=abs(x1-x2);
dy=abs(y1-y2);
Distance=D*(dx+dy)+(D2-2D)min(dx,dy) ;
或者
Distance=Dmax(dx,dy)+(D2-D)min(dx,dy);
由此我们可以看到，启发函数的选取对A算法得到的最终结果是有十分巨大的影响的。当H(n)=0的时候，A就退化成了Dijkstra算法；当H(n)远远大于G(n)的时候，A就会变成贪心最佳优先搜索算法；当H(n)始终保证不大于从当前点到终点的实际距离时，A始终可以找到最优路径，此时如果H(n)与实际距离的差距越大，那搜索速度就会越慢，性能就会变差；当H(n)等于当前点到终点的实际距离时，A就可以始终保持快速的搜索速度，且性能会保持在很高的水平上面。下图为对三种启发函数的比较，每幅图下面所标注的是路径搜索完成所需要的时间，但是，实际上Astar算法实现路径搜索要比这快的多，我这里做了延时处理，所以时间很长，但不影响我们对这三种启发函数作比较:

(a)曼哈顿式距离 2.64s 0.56s

(b)欧氏距离 3.76s 2.96s

©对角线距离 3.75s 2.59s
（2）openlist和closelist的存储形式
a.不定规模的数组或矩阵（在C中，二叉堆可能会更好）
b.规模固定的数组或矩阵（即大小与地图的大小相近）
（3）限制A的搜索区域（想法）
a.对地图做预处理，非连通区域设置为障碍物
对地图做预处理可以将地图表达的信息更加准确、简洁。考虑使用一个地图分块方法，，将地图分为几个小的区域（形状规则或不规则均可），在进行搜索之前，简单做一个判断：判断目标点处于哪个区域，而后将单独的不连通区域暂时设置为障碍物，这样，就达到了限制搜索区域的目的，提高搜索的速度和算法的性能。

(a)没有限制搜索区域，规划时间4.2s (b)限制搜索区域，规划时间2.9s

b.在搜索过程中，确定拐点并指明更好、更快的搜索方向（或区域）

（4）机器人的前进方向
在机器人在位姿空间中的行走路线的规划中，我们需要考虑的是机器人在实际中的行走方式，即它在某一时刻（或者瞬间）的前进方向有哪些选择，是前、后、左、右四个方向还是前、后、左、右、左前、右前、左后、右后八个方向，其具体的选择反映在路径规划中则为搜索过程中由中心点向外扩展遍历周围子节点，并且从中选取估价函数最小的节点的过程，一旦子节点确定之后，机器人在地图上下一步的路径规划也随之确定。
机器人前进方向的不同（即4个方向和八个方向），对路径规划的最终结果与路径规划中的搜索速度都会有一定的影响。*