【路径规划】A*

A*算法，A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。

从上章节的RRT算法的效果来看，A*算法相较于RRT的效果是要好许多的。如果说有个地图的缝隙很小，对于RRT算法来说，是比较难找到出口的，就像下面这个地图：

因为RRT是一种随机搜索，但A*的话，遇到这种地图，虽然也要迭代许多次，但是是可以最终找到出口的，而且最终搜索出来的路径就会好许多。

原理与实现

A*算法的原理，如果是比较基础的算法版本，通俗来讲的话，也是比较好理解的。

我们可以把地图都做一个数字化处理，比如数字图像是由一个个像素点组成，地图也可以像像素点一样，加入我们现在有一个起点，从起点开始搜索。

1.选择起点作为当前的扩展点，以起点为中心向上下左右四个方向扩展，如果说我们可以斜向运动的话，也可以向八个方向扩展，即上下左右再加上左上，左下，右上，右下方向：

上图中，蓝色即为扩展的区域，这里以向上方扩展为例子看一下代码，其他的七个方向以此类推：

需要判断一下扩展点是否是障碍物点，是否超过地图边界，并且使用一个数组mapIsTraverse标记点是否已经被遍历过，扩展点也不能被遍历过

2.可以看到在保存改点信息时，有三个代价值，即A*算法中非常关键的一个思想：

总代价sumCost = g_cost + h_cost 这里的 g_cost 表示从起始点到改点扩展的步数代价，现在是第一次扩展，因此就是1，h_cost是距离终点的距离代价，有多种方式，欧几里得距离或者曼哈顿距离，这两个我都尝试了一下，差别不是特别大，最后我使用了曼哈顿距离。即当前点与终点的横坐标的差的绝对值加上纵坐标的差的绝对值。

3.当保存完信息之后，就需要选择下一个扩展的点了，一般是选择总代价最小的点来扩展。这里我单独写了一个函数 checkTree 来从当前的树(树上存储的是已经遍历过的点的信息)中挑选出来下一个可扩展的点。

挑选的思路也比较简单直接，先从已经遍历过的树上找出可扩展的点，即有空间扩展的点，例如下面蓝色的点都是可扩展的点：

可扩展的点需要满足几个条件：

1）可以向外扩展，以A点来说，因为八个扩展的方向都已经遍历过了，因此A点就不满足这个条件

2）向外扩展的方向至少有一个没有阻碍或被遍历过

上图中，以C点为例，此时树Tree中一共有9个点，即A点与蓝色的8个点，C点是可以扩展的点，黄色点即为可扩展的区域，没有障碍，且没有被遍历过(不在树Tree上)

满足上述条件的在本次循环中即为蓝色的8个点

4.计算可扩展的点中代价最小的点，为C点，C点的 g_cost 为1(从起点走1步即可到达C点)，h_cost 为 6(距离终点B的距离)，之后将代价最小的点的编号传出来：

5.以传出的point点(此时为C点)为扩展点，开始向8个方向扩展，并且将满足条件的点保存到Tree中，开启下一次的循环。

6.我们可以清楚看到在下一个循环中，C点右边的那个点(E点)由于周围的点要么被遍历过，要么是障碍物，因此不满足可扩展的条件，因此不可扩展，再计算一下代价的话，代价最小的应该是上下的两个黄点(D点与F点)，这时候选哪个进行扩展都可以：

按照这个思路，并且修改完过程中一些细节的bug，最终做出来的效果是这样的：

Astar路径规划

可以看到效果是比RRT好许多的，不过还是有优化的空间，比如它找出来的路径还可以更优化一些，并且如果我们要做全角度扩展的话，这个要怎么做呢？还是需要继续深入研究，优化这个算法。