路径规划之A*算法

路径规划是非常常见的一类问题，例如移动机器人从A点移动到B点，游戏中的人物从A点移动到B点，以及自动驾驶中，汽车从A点到B点。这类问题中，都有两个关键问题需要解决：

一是找到最短路径
二是避开障碍物

解决这类问题，不得不提的一个经典的算法就是A*算法。

我们尽量以浅显易懂的语言讲解清楚A*算法的原理及实现过程。

首先，A*算法是什么？

A*算法是一种基于采样搜索的粗略路径规划算法，由stanford研究院的Peter Hart，Nils Nilsson以及Bertram Raphael发表于1968年。

A*算法的提出是想要解决移动机器人路径规划问题，也就是要在地图上找到一条从起点到终点的最短路径。

其次，如何搜索？

那么A*算法是如何去找到一条既短又无障的路径的呢?

图1

简化搜索区域

这张图是不是很难很快的给出答案。那么可以先将问题简化一下：先将图网格化，如图2所示

图2

可以这么理解，网格化就是将连续的问题离散化，离散的数据更便于计算机处理，同时也便于理解。

如图2所示，我们将要搜寻的区域划分成了正方形的格子。

这是搜索路径的第一步：简化搜索区域。

将搜索区域简化为2维数组。数组的每一项代表一个格子，它的状态就是可走和不可走。通过计算出从S到D需要走过哪些格子，就找到了路径。

如图2所示，蓝色格子是障碍物，灰色格子是可通过的区域，绿色格子是起点，标记为S，红色各种是终点，标记为D。

我们要做的就是找到一条从起点（S）到终点（D）的最优路径。

约束条件

为了顺利地解决问题，我们还需要设定一些约束条件：
为了顺利地解决问题，我们还需要设定一些约束条件：

1、从一个格子可以朝周围8个方向移动，其中朝上、下、左、右移动的成本为1，朝左上、右上、左下、右下移动的成本为1.4，也就是图片的近似值；

2、不能朝障碍物所在格子移动；

3、如果右边和上边两个格子都是障碍物，那么不能朝右上方的格子移动。

开始搜索

设定好约束条件后，就可以开始搜索路径了。

列表Open List将放入待检查的节点，openlist中的格子是路径可能会经过的，也有可能不经过。

列表closed list将加入已检查节点，也就是不需要再关注的节点。

首先将起点S加入open list；然后找出S周围所有可移动的格子，这些格子也可以被称为邻居；接下来算出从S移动到该格子的代价（cost），我们把这个代价记为G；将S设为父节点。每一个邻居格子中都有一个箭头，指向的是它的父亲节点，在这一步中，他们的父亲节点都是起点S。

这样，我们就完成了对S的检查。

图3

将S从openlist中移除，并将其加入cloed list。

我们可以用图4表示，橙色边框代表待检查节点，黑色边框表示已检查节点。

图4

现在可以看到openlist中一下有了8个待检查节点，先检查哪一个呢？

先给出结论：选择具有最小F值的那个格子

路径排序

计算出组成路径的方格的关键是下面这个等式：F=G+H

G代表从起点S移动到这个节点的代价，沿着到达该节点而生成的路径。

H是从指定的节点移动到终点D的估算成本。因为在这个时候我们还不知道到终点的真正距离，所以H只是对剩余距离的估算值，在这里我们采用曼哈顿方法对其进行估算。

曼哈顿距离：计算从当前节点横向或纵向移动到达目标所经过的方格数，忽略对角移动。也就是说只通过朝上、下、左、右四个方向的移动，抵达终点D的最短距离。

例如，在平面上，坐标为（x1，y1）的i点与坐标为（x2，y2）的j点的曼哈顿距离为d（i，j）=|x1-x2|+|y1-y2|。

要注意的是，这里用曼哈顿方法计算H时要忽略路径中的障碍物。

最后，把G和H相加，就可以得到F。我们在每个方格上都标注了G,H,F值。

图5

如图5所示，与起点直接相邻的上方、下方、左方、右方的方格的G值都是1，对角线方格的G值为1.4.

H值是通过估算该节点与终点的曼哈顿距离得到，仅作横向和纵向移动，并且忽略沿途的障碍物。采用这种方式，我们可以看到起点S左边的方格到终点D有5个方格的距离，因此H=5。这个方格上方的方格到终点D有6个方格的距离，所以H=6。

用同样的方法可以计算出其他的方格的H值。

继续搜索

为了继续搜索，我们从open list中选择F值最小的节点，这里对应的应该是起点S右边F值为4的格子。将所选节点从open list中取出，加入close list中，然后对它执行前面的检查。

这次检查时要注意以下几点：

1、如果邻居节点已经在closed list中，或者是不可走的方格，直接忽略；

2、如果邻居节点不在open list中，计算该邻居的G、H、F，并将当前选定的节点设置为该邻居的父节点。另外记得将该邻居加入open list中。

3、如果邻居节点已经在open list中，也就是说，这个邻居已有父节点，计算从起点经由当前所选节点到达该邻居的G值，检查G值是否更小。如果没有，那么不做任何操作。

如果新的G值更小，那么将这个邻居的父节点重设为当前选定的节点，并更新该邻居的G值和F值。

当将相邻的4个邻居都检查之后，没有发现经由当前方格的更优的路径，因此我们不做任何改变。

现在我们已经检查了当前方格的所有相邻的方格，也对他们进行了处理，接下来该选择下一个待检查的方格了。

再次遍历open list，现在只有7个方格了，依然选择F值最小的那个。

这次我们会发现有两个方格的F值都是54，选哪个都可以。通常从速度上考虑，选择最后加入open list的方格更快。

我们选择起点右下方的方格。

将不在open list中的邻居加入open list，同时将当前选定的方格设为他们的父亲节点；当前选定方格剩下的3个邻居中，有2个已在close list中，可以忽略。

对已在openlist中的邻居方格进行检查，即检查从起点经过当前方格到达那里是否具有更小的G值。没有，那么不做任何的操作。

不断重复这个过程，直到将终点D也加入到了open list中,并且是其中F值最小的节点。按照之前的步骤，取出F值最小的节点，发现它的H值为0，这意味着这个节点就是终点D。到此搜索也就可以告一段落了。

这里要注意的是，在起点下面2格的方格（也就是标记了星标的方案）已与前面不同了。之前它的G值是2.8，并且指向它右上方的方格。现在它的G值为2，指向它正上方的方格。

这是在寻路过程中的某处发生的，经过检查发现使用新路径时G值变得更低，因此父亲节点被重置,G值和F值被重新计算。

确定最短路径

最后，我们怎么去确定最短路径呢？

从终点开始，按着箭头依次向父亲节点移动，直到回到起点S，这个路径就是最佳路径。

要注意的是，最佳路径可能有多条；例如在这个案例中，下图也是一条F=5.6的路径，这取决于当openlist中存在多个F值最小的节点时，先选取哪一个进行搜索。

图7

A*算法总结

1.将地图网格化
2.创建openlist列表与close list列表
3.将起点加入openlist
4.遍历openlist，查找F值最小的节点，将它作为当前要检查的节点。
5.将这个节点移到close list

6.对与当前节点方格相邻的8个方格进行进行以下检查：

如果它不在open list中，将它加入open list，并且将当前方格设置为它的父亲节点，记录这个方格的G、H和F值；

如果它已经在openlist中，检查经由当前方格到达它是否是更优路径，用G值作参考，更小的G值表示这是更优的路径。如果新G值比原G值小，将它的父亲节点重新设置为当前方格节点，并重新计算它的G值和F值。

7.重复4-6步，直到遇到以下情况，即可停止搜索。

将终点加入到了open list中，此时路径已经找到了；

查找终点失败，并且openlist是空的，此时意味着没有路径。

***8.保存路径。***从终点开始，依次向父亲节点移动直到起点，这就是搜索到的最优路径。