移动机器人路径规划算法介绍及A*算法详解
综述
随着人工智能的兴起，移动机器人逐渐走入了更多人的视线，ML和DL为移动机器人的导航及定位算法提供了很多的重要参考。对于移动机器人的研发最为基本的问题就是如何对机器人的运动进行控制，在目前众多的机器人路径规划算法中，针对静态、动态环境存在着很多不同的解决方案，本文将对现存的主流算法进行大致的讲解和优劣分析。同时，对于在静态环境中广泛使用的A*算法，进行详细的解读。
路径规划算法总结
下面是现在流行的主要路径规划算法：
- 广度优先算法（Breadth-First-Search, BFS）： 广度优先算法实际上已经能够找到最短路径，BFS通过一种从起点开始不断扩散的方式来遍历整个图。可以证明，只要从起点开始的扩散过程能够遍历到终点，那么起点和终点之间一定是连通的，因此他们之间至少存在一条路径，而由于BFS从中心开始呈放射状扩散的特点，它所找到的这一条路径就是最短路径。
- 启发式搜索：改变广度优先算法原来队列的FIFO模式，给不同的点加入优先级，可以知道，距离终点的曼哈顿距离越小的点，该点的优先级越高。然而这导致了先入为主地将最早遍历路径当成最短路径。
- Dijkstra算法：主要思想是从多条路径中选择最短的那一条：我们记录每个点从起点遍历到它所花费的当前最少长度，当我们通过另外一条路径再次遍历到这个点的时候，由于该点已经被遍历过了，我们此时不再直接跳过该点，而是比较一下目前的路径是否比该点最初遍历的路径花费更少，如果是这样，那就将该点纳入到新的路径当中去。
优点：肯定找到最短路径，算法简单。
缺点：需要遍历并记录所有节点。 - A*算法 ：我们需要算法有方向地进行扩散（启发式），另一方面我们需要得到尽可能最短的路径，因此A*就诞生了， 它结合了Dijkstra和启发式算法的优点，以从起点到该点的距离加上该点到终点的估计距离之和作为该点在Queue中的优先级。在下文会详细介绍此算法。
优点：比dijkstra快，因评价函数存在，算法会放弃掉图的边缘节点。
缺点：h函数的选择决定了算法速度。 - 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： 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值找到目标点到各个节点的最短距离 - RRT：通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与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列表上，减轻了储存负担，提高算法效率。
路径规划算法还有很多，其他的就不在此赘述了，大家可以自主搜索一些资料。
A*算法详解
1.原理解析
A*算法是启发式搜索，是一种尽可能基于现有信息的搜索策略，也就是说搜索过程中尽量利用目前已知的诸如迭代步数，以及从初始状态和当前状态到目标状态估计所需的费用等信息。
A*算法可以选择下一个被检查的节点时引入了已知的全局信息，对当前结点距离终点的距离作出估计，作为评价该节点处于最优路线上的可能性的量度，这样可以首先搜索可能性大的节点，从而提高了搜索过程的效率。
A* 算法的基本思想如下：引入当前节点j的估计函数f*,当前节点j的估计函数定义为：
f*(j)= g(j)+h*(j)
其中g(j)是从起点到当前节点j的实际费用的量度，h*(j)是从节点j到终点的最小费用的估计，可以依据实际情况，选择h*(j)的具体形式，h*(j)要满足一个要求：不能高于节点j到终点的实际最小费用。从起始节点点向目的节点搜索时，每次都搜索f*(j)最小的节点，直到发现目的节点。
2.曼哈顿距离
A*算法的核心是设计估价函数，设计估价函数h(j)有很多方法，下面介绍其中的一种——曼哈顿距离。
利用欧几里德距离计算估价函数的计算量很大，因此考虑将两点之间的曼哈顿距离作为估价函数。其中A点的经纬度为(Ax,Ay),B点的经纬度是(Bx,By),则A、B之间的Manhattan距离可以表示为:
M(A,B)=P*(|Ax-Bx|+|Ay-By|)
P = 2* π *R / 360
由于P是常数，可以简化为
f(j)=g(j)+(|Ax-Bx|+|Ay-By|)
此估价函数计算量小，虽然不是严格的方向优先，但基本能保证最短路径的搜索方向向目标点的方向进行。
3.算法步骤
A*算法在搜索中设置两个表:Open表和Close表。Open表保存了所有已生成而未被考察的节点，Close表中记录已被考察过的节点。算法中有一步是根据估计函数重排Open表中的节点，这样，循环的每一步只考虑Open表中状态最好（代价最小）的节点，如果被发现在Open表中存在同一个节点，就应比较两个节点代价的大小，如扩展得到的新节点代价大于已有的节点代价，可以放弃扩展得到的新节点，否则就以新节点代替原来的节点。如果在Close表中存在同一个节点号，则新新生成的路径的代价肯定大于原来路径的代价，可以淘汰新生成的节点。
具体算法步骤如下：
步1：生成空的Open表、Close表，将起点S放入Open表
步2：如果Open表为空，则失败退出。
步3：从Open表中，找出头节点U,作为当前节点,将它从OPEN表中移除。
步4：判断U是否为终点，如果是，则通过节点U的父指针，一直遍历到起点，找到到最优路径，算法结束。否则，转步5.
步5：扩展当前节点U,找到其扩展的后继节点集合V,遍历集合V中的节点，如果节点即不在Open表也不在Close表，计算该节点的估计值，将节点加入到OPEN表，设置父节点为U.如果节点在Open表、Close表，比较当前节点的代价g(v)和Open表、Close表中代价，如果当前节点的代价g(v)小，更新表中的代价和父节点指针。
步6：按估价值递增的顺序，对Open表中所有节点进行排序。
步7：转步2.
4.伪代码实现
根据以上思路，A*算法的伪代码如下：
A*()
{
Open = [起始节点];
Closed = [];
while (Open表非空)
{
从Open中取得一个节点X，并从OPEN表中删除。
if (X是目标节点)
{
求得路径PATH；
返回路径PATH；
}
for (每一个X的子节点Y)
{
if (Y不在OPEN表和CLOSE表中)
{
求Y的估价值；
并将Y插入OPEN表中；
}
//还没有排序
else if (Y在OPEN表中)
{
if (Y的估价值小于OPEN表的估价值)
更新OPEN表中的估价值；
}
else //Y在CLOSE表中
{
if (Y的估价值小于CLOSE表的估价值)
{
更新CLOSE表中的估价值；
从CLOSE表中移出节点，并放入OPEN表中；
}
}
将X节点插入CLOSE表中；
按照估价值将OPEN表中的节点排序；
}//end for
} //end while
} //end function
5. A*算法总结
A* 算法是一种静态路网中求解最短路径最有效的直接搜索方法。估价值与实际值越接近，估价函数取得就越好。注意是最有效的直接搜索算法。之后涌现了很多预处理算法（ALT，CH，HL等等），在线查询效率是A*算法的数千甚至上万倍。公式表示为： f(n)=g(n)+h(n),其中 f(n) 是从初始点经由节点n到目标点的估价函数，g(n) 是在状态空间中从初始节点到n节点的实际代价，h(n) 是从n到目标节点最佳路径的估计代价。保证找到最短路径（最优解的）条件，关键在于估价函数f(n)的选取：估价值h(n)<= n到目标节点的距离实际值，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。并且如果h(n)=d(n)，即距离估计h(n)等于最短距离，那么搜索将严格沿着最短路径进行，此时的搜索效率是最高的。如果估价值>实际值,搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。