基于一种改进A*算法的移动机器人路径规划研究

周 超，谷玉海，任 斌

（1.北京信息科技大学 现代测控教育部重点实验室，北京 100192；（2.北京航天长征飞行器研究所，北京 100076）

随着社会发展，机器人的应用更为广泛，移动机器人的路径规划问题［1］更是目前的一个研究热点。该问题可以描述为移动机器人按照某种性能指标在有障碍物的环境里从起点至终点间找到一条最短或最佳的安全无碰撞路径［2］。常用的路径规划算法有RRT算法［3］、蚁群算法［4-5］、人工势场算法［6］、SFLA［7］、A*算法［8］等。RRT算法搜索效率高，但得到的路径往往和最优路径相差甚远。蚁群算法的收敛速度相对较慢且容易陷入局部最优状态。人工势场算法虽然结构简单易于实现，但仍存在目标不可达、障碍物前产生震荡、规划路径不可达等问题。相较于其他路径规划算法，A*算法构建模型简单，搜索效率高，针对静态场景所得到的解接近最优解，对不同的场景具有很强的扩展性和适应性。A*算法同样存在很多的不足之处，如生成路径拐点较多、路径不够平滑、大范围搜索情况下计算量大等问题。针对这些问题，国内外研究人员从不同角度提出不同的改进方案。Duchoň等［9］在文中总结了Basic theta*、Phi*、Jump Point Search这3种对A*算法的改进方法。Basic theta*和Phi*解决了A*算法不能全方向搜索的问题，最终得到的路径长度更接近最优路径，但计算效率低；Jump Point Search能够提高搜索效率，但搜索方向仍受到限制，得到的路径相较于另外2种搜索方法更长。Guruji A K等［10］针对A*算法在动态环境下计算量过大的问题提出减少计算启发函数值次数的方法，仅在碰撞发生前计算A*算法启发函数的值，能够有效提高算法处理效率。辛煜等［11］提出一种将搜索邻域拓展至无限个的方法使得生成路径更短，拐点数更少，但搜索效率会降低。单伟等［12］、王红卫等［13］通过引入平滑模型对生成路径进行二次优化，有效减少了所得路径的转折次数和转折角度。王殿君［14］、陈若男等［15］针对移动机器人在室内的路径规划问题分别从提高移动机器人调整自身姿态能力和面向障碍物搜索以提升路径安全性对A*算法做出改进。针对大规划空间下机器人路径规划问题，任晓兵等［16］提出一种基于Voronoi图改进的A*算法，对初次规划的路径进行二次规划，有效减少了空间规划面积。赵晓等［17］在A*算法的基础上结合了跳点法，有效提高了大规模搜索下生成路径的速度。

总体分析，A*算法的改进方法有很多，但在移动机器人路径规划中仍存在以下问题：没有充分考虑移动机器人的体积而造成的路径安全性问题；复杂地形环境下算法生成路径合理性问题。针对上述问题，本文基于A*算法提出以下改进方法：

1）通过在存在障碍物的栅格中拓展障碍物矩阵的方法改善移动机器人易与障碍物边缘发生碰撞的问题。

2）引入坡度信息改进代价函数计算方式，考虑到地形坡度对移动机器人通过性的影响，解决算法生成路径在复杂地形中过于陡峭的问题。

1 环境模型

研究路径规划问题首先需要一个地图环境模型，常见的环境地图构建方法有可视图法［18］、栅格法［19］、拓扑图法。其中栅格法是将机器人工作环境分成一系列具有二值信息的网格单元，模型建立简单，易于维护，因此本文采用栅格法建立移动机器人工作环境的三维地形图。复杂环境下，移动机器人的路径规划问题应更侧重于如何得到一条可以安全通过的路径，考虑到机器人自身的体积问题，建立环境的栅格地图时选取比移动机器人自身体积稍大的栅格地图。同一环境下，构建相对更大的栅格地图会减少算法中搜索邻域节点的计算量，提高运行效率。但栅格过大会影响搜索效果，到达目标点的误差也会更大。因此，设计栅格地图时选取机器人投影面积和每一个栅格大小之比为2∶3的比例构建栅格地图。

本文选取25×25的平面栅格地图作为平面环境地图模型，认为每一个栅格是一个节点，将每个栅格划分为4×4的小栅格。

由此方法划分完成，得到一个100×100的二维平面地图模型，在此基础上在Matlab中生成1个100×100的高度信息矩阵，用mesh函数生成一个三维地图模型，如图1所示。

图1 三维地图模型

2 A*算法

A*算法是一种启发式搜索算法，结合了Dijkstra和BFS 2种算法各自的优势，能够在保证搜索效率的前提下得到最优路径［20］。其基本思想是通过一个代价函数［21］计算从起始节点开始到其每个邻域节点的代价值来确定搜索方向。用G（n）表示从起始节点到当前节点n的实际代价，H（n）表示从当前节点n到目标点的代价估计值，可得代价函数的计算方式为：A*算法的基本计算流程为：

a）初始化一个OPEN表和一个CLOSE表用于存储节点信息。

b）将起点放入OPEN表中，设其F（n）值为0。

c）判断OPEN表中存在节点则选取F（n）值最小节点N。

d）节点N是终点，从终点开始追溯每一个节点的父节点直到起点，取出所有父节点即为路径。

e）N不是终点，将节点N从OPEN表中取出加入CLOSE表，遍历N的邻域节点，若邻域节点M不在CLOSE表中且不在OPEN表中，计算M的F（n）值，并将节点M加入OPEN表。

图2是A*算法寻路过程的一个示例。在上述计算过程中，若节点的一侧存在障碍物，即使其斜向为非障碍物节点，其寻路方向也不可设定为斜向，这是为了避免移动机器人与障碍物发生碰撞的问题。

图2 A*算法寻路过程示意图

在图2的寻路过程中是将机器人看作为一个质点去进行计算的，而实际上机器人自身是有一定体积大小的，在上图计算过程中，认定如节点的正右侧存在障碍物，则当前节点的右上和右下节点即为不可到达点，通过这样的方式避免移动机器人与障碍物发生碰撞，但实际上这种方向并不可取，一是没有考虑到栅格大小和机器人大小的关系，二是如果遇到如图2中Nj的障碍物情况，是否将该点认为是一个障碍物点。针对这一问题，本文提出一种节点拓展障碍物矩阵的搜索方法。同时考虑在复杂地形环境下，地形坡度对路径规划的影响，对代价函数计算方式进行改进。

3 优化方法

3.1 节点拓展障碍物矩阵

将移动机器人在前进过程中与周边障碍发生碰撞的问题进行简化，将机器人自身体积在平面坐标系的投影视作一个正方形，因此，仅需要考虑正方形的4个角与周边环境是否发生碰撞来进行判断。为解决传统路径规划算法产生的路径会导致移动机器人和障碍物发生碰撞的问题，提出一种拓展栅格地图节点障碍物矩阵的方法，即将栅格地图中的每个节点划分为n阶的障碍物节点，根据一个节点内障碍物的分布情况划分为完全障碍物节点、不完全障碍物节点和无障碍节点。

在每次邻域搜索时，如果当前节点为不完全障碍节点，需要根据节点中障碍物分布的情况判断目标邻域节点是否满足可通行条件。

本文设定机器人的投影和每一个栅格大小比例为2∶3，为了便于计算移动机器人是否与障碍物边缘存在碰撞可能性，选用4阶矩阵来表示每个栅格的障碍物信息并提出如下规则对节点进行分类：

a）若矩阵A中不存在非0点则认为是无障碍节点。

b）若矩阵A中主对角线或次对角线上存在2个及以上非0点则认为该节点为完全障碍物节点。

c）若矩阵A中第2、3列或第2、3行存在2个及以上非0点则认为该节点为完全障碍物节点。

d）除上述情况以外的节点认为是不完全障碍节点。

首先讨论当前节点自身的障碍物分布情况，已知每个节点都存在8个邻域节点，用坐标的方式来表示，将当前点视为［0，0］，设定当前点0°的方向为［1，0］，则其8个邻域的表示方法如表1所示，每个邻域搜索方向都有不同的判断条件，判断当前节点在其邻域搜索方向上是否满足通行条件的顺序如图3所示。

图3 障碍物矩阵顺序示意图

为方便表示，将节点自身的障碍物矩阵按照Nobs所示为每一项编号，在进行邻域搜索时，对应搜索目标邻域需满足除表1中对应忽略项外其余项均为0，可认定当前节点在该方向上可以进行搜索。

表1 节点自身障碍物矩阵忽略项

在满足节点自身能够搜索该邻域的条件时，判断目标邻域节点是否满足可通行条件。以目标邻域节点为右上节点时为例，N0为当前节点的轨迹起始点，在目标邻域节点不存在障碍物的情况下存在如图4所示5种情况，此时除考虑N0和当前目标节点Ni的障碍物分布，同时也需要考虑其邻域的障碍物分布情况是否满足可通行条件，即如图4中3条实线构成的区域其障碍物矩阵项的值均为0。

图4 目标邻域节点为无障碍节点时不同起始点的轨迹

当目标邻域节点边缘存在障碍物时，需要判断目标邻域节点的障碍物分布情况，由图4分析，当目标邻域节点为右上节点时，考虑机器人在栅格地图中移动的轨迹，将满足可通行条件的障碍物情况分为2种：

a）目标邻域节点障碍物分布为左上侧时：即Nobs中1，2，5区域的任意区域存在障碍，其余区域均不存在障碍时，可将轨迹中心点进行偏移以获取可通行的路径。

b）目标邻域节点障碍物分布为右下侧时：即Nobs中12，15，16区域的任意区域存在障碍，其余区域均不存在障碍时，可将轨迹中心点进行偏移以获取可通行的路径。

如图5为节点左上侧存在障碍区域，目标邻域节点Ni的障碍物矩阵中1，2，5为不可通行区域，而其余区域满足可通行的条件，进而再判断相邻邻域节点是否具有可通行的条件，即N0右侧和上方的节点的障碍物矩阵中被虚线划过的区域均不存在障碍物，称其满足“借道条件”，则将目标邻域节点的“中心点”偏移，避开边角存在的障碍物，从而取得一条从起始点到终点代价较低的安全路径。图6为节点右下侧存在障碍区域的情况。

3.2 代价函数的优化

计算三维坐标系下的距离信息，对启发值的计算通常采用欧式距离计算方法：

采用该方法计算时，坡度信息没有完全的体现在代价函数的计算值中，因此虽然得到了一条可行路径，但路径过于“坎坷”，在复杂地形环境下，移动机器人的路径规划问题更需要考虑坡度因素所带来的影响，上坡和下坡都会一定程度影响机器人的运动，因为需要选取一条更为平缓的路径，为实现这一目标，对代价函数的计算方式进行修改。

图5 目标邻域节点障碍分布为“左上侧”

图6 目标邻域节点障碍分布为“右下侧”

根据参考文献［22］中提出的在山地行走时速度和坡度的关系：

式中：Slope为坡度信息，计算公式为

式中：H为当前节点中最高的高度信息值；L为水平宽度。

对代价函数中G（n）和H（n）的计算方式进行改进，仍选取欧式距离的计算方法

式中：ρ为2个节点间的平面欧式距离，对于存在坡度的路面计算其等效平面距离，根据式（3）中坡度和速度的关系，取其等效平面距离为

可得G（n）的计算公式：

相应的H（n）也需要考虑和终点间的估计代价值，仍选取欧式距离作为代价函数的计算方式，在已知当前节点和目标点之间的平面欧式距离的基础上，假设从当前节点到目标节点的坡度信息均为Slope（n），参照式（6）得到其等效平面距离：

在此基础上再考虑目标点的坡度信息和当前节点坡度信息的差的等效平面距离，可得H（n）的计算方式：

4 仿真分析

无障碍情况下，在不考虑高度信息的情况下由传统A*算法得到的最短路径如图7虚线所示，这条路径虽然是理论上存在的最短路径，但没有考虑复杂地形对于移动机器人行动的影响，在此种地形环境中，路径规划问题应更多考虑到实际地形因素。如图7实线所示为改进了代价函数计算方式的A*算法，相较于前者，对地形条件的考虑更为充分，虽然路径更长，但生成的路径更为平缓，通过性更好。

图7 改进算法生成路径

在改进代价函数的基础上，进一步验证在考虑移动机器人本身体积的情况下在该路径下是否和障碍物发生碰撞的问题，在目标邻域节点为水平方向和对角线方向时进行讨论，图8为存在障碍物情况下的路径规划情况，虚线表示改进代价函数的A*算法的避开障碍物的路径，实线为拓展障碍物矩阵方法改进后生成的路径，由结果分析，如果不考虑移动机器人和障碍物体积而得到的规划路径是一条危险路径，且在斜方向为目标邻域节点的情况下，移动机器人会与障碍物发生碰撞，现实情况中移动机器人的路径规划问题通常会采用局部路径规划和全局路径规划相结合的方式进行实时避障，在遇到无法通过的障碍物的情况时会进行局部的二次规划，为减少重复规划的次数，在进行全局的路径规划时应得到一条当前静态环境下更为安全的路径。

图8 拓展障碍物矩阵方法生成路径

由图9能够看出，采用节点拓展障碍物矩阵的方法之后生成的路径和障碍物之间留有足够的安全距离，按照设定移动机器人本体大小和栅格地图比例为2∶3，路径为移动机器人中心点的轨迹，图中路径与障碍物的最短距离为1个网格长度，而机器人大小所占据面积的1/2长度小于1个网格距离，改进后的算法能够解决在目标邻域节点为对角线方向时，生成的路径会与障碍物的边界发生碰撞的问题。

图9 障碍物区域生成路径对比

图10为优化前后生成的路径节点的F（n）值变化曲线，可以看出优化前后F（n）变化趋势基本一致，坡度因素对F（n）的值影响较大，在后半部分的曲线基本重合（由于优化后的路线搜索节点相比于优化前多一个，所以图像后半部分后移一个单位），总体来看，优化前后F（n）的变化趋势基本一致，而优化后的算法不会对总的路径造成较大的影响，而是对存在障碍物的局部区域进行重新规划，提高生成路径安全性的同时保证其路径最优。

图10 生成路径的代价估计值曲线

为进一步验证算法在遇到栅格边缘存在障碍物的情况，在原有路径上添加不完全障碍物节点，如图11所示，实线部分为原路径即不存在该不完全障碍物节点时所生成的路径，虚线为添加不完全障碍物节点的情况下生成的路径，相较于不存在该障碍物的情况下向下偏移一个网格的距离，使得生成路径的安全性更高。

图11 存在不完全障碍物节点情况路径

5 结论

对传统A*算法在移动机器人路径规划应用中的问题进行改进，通过拓展栅格地图节点的障碍物矩阵的方法解决了因忽略移动机器人自身体积和栅格地图划分栅格大小造成的生成路径安全性问题，且生成的路径更为合理。同时针对在复杂地形条件下，设计了新的代价函数计算方法，引入坡度信息，将陡坡上移动的距离简化为平面等效距离计算每一个节点总的代价值，使得生成路径更趋于平缓。通过仿真结果验证本文提出的改进A*算法能够在复杂地形条件下得到一条平缓的路径且通过拓展节点障碍物矩阵的方法能够有效减少移动机器人与障碍物发生碰撞的概率。但最后生成的路径不够平滑，针对这一问题，下一步工作将从路径规划效率和使生成路径呈现出更平滑的曲线方面进行优化。