python路线规划_利用Python实现A*算法路径规划

一、A*算法介绍

A*算法实际上是一种启发式算法，也是路径规划中应用最为普遍的算法之一。A*算法并不是只用于路径规划，同时，路径规划中也不只有A*一种启发式方法。A*算法相比其他路径规划算法，如遗传算法、蚁群算法等，其算法过程较为简单、易于理解，运行速度快。而且，应用A*的路径规划结果也还不错。因此，总体来说，A*算法应该是性价比较高的一种路径规划算法。

A*算法的基本思想是，对于当前的搜索点CNode，首先找到节点CNode的所有邻居节点，如四方向移动就有4个邻居节点，而八方向移动就有8个邻居节点；然后计算所有邻居节点的目标函数值f(x)，路径规划中的目标函数即为从起点出发，经过当前节点到达终点的距离大小；最后，从所有邻居节点中选择f(x)值最小的节点，并重复上述过程，直到搜索到达终点。

下面，我们用伪代码将A*算法大体过程描述如下：step1：设置起点S，终点E，地图大小mapsize，障碍物集合Blocklist

step2：将起点S添加到开放列表Openlist中

step3：计算S的目标函数f(x)

step4：查找Openlist中f(x)最小的节点，记为Nmin

step5：将Nmin从Openlist中删除，并添加Nmin到封闭列表Closelist中

step6：查找Nmin的所有邻居节点，得到集合Nlist

step7：对于Nlist中的每个元素N，进行如下循环:

step8： if N 属于 Closelist 或者 N 属于 Blocklist:

step9： 跳过该节点

step10： if N不属于Openlist：

step11： 添加N到Openlist

step12： 设置节点N的父节点为Nmin

step13： 计算节点N的目标函数f(x)

step14： else if N属于Openlist：

step15： 节点N以前计算过f(x)，记原有的f(x)为f(x)_old

step16： 计算节点N以Nmin为父节点的新的目标函数f(x)_new

step17： if f(x)_new < f(x)_old:

step18： 设置节点N的父节点为Nmin

step19： 并且更新节点N的目标函数f(x) = f(x)_new

step20： 转入step7，循环遍历列表Nlist中的所有邻居节点

step21：转入step4，直到Openlist为空或者Nmin等于终点E

这里着重说明一下A*算法中目标函数f(x)的计算。f(x)上文已经提到过，从起点出发，经过当前节点到达终点的距离大小。其具体计算包括两部分：g(x)和h(x)，也就是说，f(x) = g(x) + h(x)。g(x) 是指从起点到当前节点CNode的实际最小距离，而h(x)是指从CNode到目标节点的估计最小值。

关于距离的计算，主要有两种方法：一种是欧式距离，即物体移动时可以朝八个方向移动，其计算公式为：

另一种是曼哈顿距离，即物体移动时只能朝四个方向移动，不能斜向移动，其距离计算公式为：

我们采用欧式距离进行计算。

上述伪代码中，Openlist是开放列表，表示待搜索的节点；Closelist是封闭列表，表示已经过搜索的节点；Blocklist为障碍物节点，障碍物是不可通行的。

下面，我们根据实例对A*算法的具体搜索过程进行演示说明。

初始化地图如下，S为起点，E为终点，黑色填充单元格表示障碍物。以每个单元格的左上角坐标表示该单元格的坐标，那么S为(0, 1), E为(5, 6)。

①首先，将起点S加入Openlist中，计算S点的目标函数值f(S)，因为起点S没有父节点，所以g(S)=0；根据欧式距离计算公式可得：

②因为Openlist中只有一个节点S，Nmin = S，将S从Openlist中删除