两阶段搜索的A*全局路径规划算法

赵卫东 蒋 超

(安徽工业大学电气与信息工程学院 安徽 马鞍山 243032)

0 引 言

随着自动化和智能化技术的发展，移动机器人在生产和生活中的应用越来越广泛。移动机器人的路径规划问题是移动机器人研究的核心问题之一。所谓移动机器人路径规划[1]，就是当移动机器人处于已知或未知的环境中，其根据自身传感器反馈的信息来感知环境，从而避开障碍物自行规划出一条最优的安全运行路线。移动机器人路径规划研究最早始于20世纪60年代[2]。时至今日，移动机器人路径规划可分为传统路径规划方法和现代路径规划方法。传统的路径规划方法包括Dijkstra算法[3]、A*算法[4]、D*算法[5]等，现代路径规划算法包括蚁群算法[6]、遗传算法[7]、神经网络算法[8]、模糊路径算法[9]等。相较于蚁群算法等智能算法，传统算法环境建模简单，易于实现。

在众多传统算法中，A*算法是一种基于栅格地图的路径规划算法，相比于主要应用在环境未知的动态场景的D*算法，对于环境已知的静态场景A*算法能更加有效地求得最优路径[10]。同时，相比于Dijkstra算法，A*算法采用了启发式的搜索方式，大幅度降低了搜索节点的数量，从而极大地提高了搜索效率。但是，A*算法存在以下问题：其规划出的全局路径中易出现转折点，而考虑到机器人的运动学限制，路径中存在过多的转折点对于机器人的运动控制是不利的。

本文重点研究了A*算法生成的全局路径平滑处理以及提高该算法实时性的有效手段，采用两阶段搜索算法，通过选择合适的估价函数，指导搜索朝着最有希望的方向前进，以期获得最优路径。

1 传统的A*算法

A*算法是比较常用的移动机器人路径规划算法，它是一种启发式搜索方法。A*算法中的评价函数定义为：

f(n)=s(n)+h(n)

(1)

式中：n表示机器人在路径中的当前位置节点；s(n)代表机器人从起始节点到当前节点的实际花费代价和；h(n)是启发函数，表示机器人从当前节点到目标节点的距离估计代价。通过估价函数来引导其搜索方向，传统A*算法使用曼哈顿距离或者欧氏距离来定义启发函数。曼哈顿距离表示两个点在标准坐标系上的绝对轴距总和，距离计算方向仅分为横向和纵向，计算简单但对实际距离存在明显的高估。欧氏距离表示两个点在坐标系中的直线最短距离，是对实际距离很好的描述，所以采用欧氏距离来构建启发函数性能较好。即：

(2)

式中：nx、ny分别为当前节点n在直角坐标系中的横坐标和纵坐标；g是目标节点；gx、gy分别为目标节点g在直角坐标系中的横坐标和纵坐标。

A*算法采用八邻域搜索，如图1所示。通过中心节点向附近8个邻域扩散，然后通过每个邻域的评价函数值的大小来选择路径方向，运动方向的角度也被限定为这8个方向。因此，规划出来的路径会有很多的转折点，而对于移动机器人自身机械结构所决定的运动限制来说，这些转折点明显不符合运动学原理，所以A*算法在实际工程应用中效果并不理想。

图1 八邻域搜索

2 改进的A*算法

A*算法折点过多，机器人运动轨迹不平滑的原因有两种：1)采用八邻域的搜索方式，其子节点与父节点之间的实际移动代价也是按照八邻域方向计算的，这就造成了采用欧氏距离描述的两点之间实际距离所估计的代价低于实际移动代价而导致算法扩展多余节点。2)评价函数只考虑到起点到当前节点和当前节点到终点的代价，并没有考虑到机器人自身的运动学约束的代价。因此，本文首先改进A*算法的评价函数，实现一级搜索，然后对一级搜索的结果进行二级平滑处理，最终通过两级搜索的方式实现对路径的平滑纠正的目的。

2.1 优化的启发函数

根据A*算法的八邻域搜索方向：横向、纵向、对角线方向，把A*算法启发函数hE(n)采用的欧氏距离分解成这三个方向的组合，这样能够更准确地表示栅格地图上任意两点的实际移动代价，并使算法尽量处于仅寻得最佳路径而不扩展其他扩展点的理想情况。优化A*的启发函数为：

(3)

优化的启发函数相对于使用曼哈顿距离的启发函数不会高估到目标点的移动代价，也不会像使用欧氏距离的启发函数那样低估移动代价。

2.2 角度约束函数

A*算法作为一种经典的路径规划方法，通过结合机器人的几何特征对地图上的障碍物进行轮廓膨胀，把机器人简化为一个质点，从而达到简化计算的目的，所以A*忽略了机器人实体自身的运动特性，导致算法规划出的路径机器人执行难度较大，甚至无法执行。另外，在实际应用中，机器人的路径实施过程依赖于其传感器定位，路径存在过多的转向需求(折点)也会令定位传感器的观测数据产生累计误差，使机器人在地图中自定位逐渐不准确，甚至位置丢失，而引入角度约束可以有效地抑制这些问题。

综合机器人自身的运动约束，机器人行进方式一般为区域性向前，不会产生大幅度的转向、频繁且长时间的后退行为[11]，在路径规划的第一阶段搜索过程中引入角度约束，使转向行为具有一定的代价来抑制路径转折点的产生，即评价函数变为：

f(n)=s(n)+h(n)+a(n)

(4)

新的评价函数在原有的基础上增加了一个新的约束，该约束为机器人相对上一运动方向的转角约束a(n)，其约束函数为：

(5)

式中：k为转角约束系数，通过新的评价函数对当前节点不同转角的子节点的评价可以确定k的取值范围，在此范围内，k越大，角度约束的效果就越明显；θn为当前时刻机器人的朝向角；θl为上一时刻机器人朝向角；l为单格步长。当机器人上一时刻转角与当前时刻相同时，新的约束函数即为0，当机器人转角越大，则约束越大。图2和图3分别给出了A*算法和本文改进算法的评价函数搜索方式。

图2 传统算法评价函数

图3 改进算法的评价函数

A*的评价函数在八邻域搜索时并没有考虑到机器人偏航角问题，步长与角度没有关联，容易出现偏航角折点。改进后的A*算法添加了角度约束，转偏航角度越大，则总步长越长，抑制了折点的产生，相比于传统算法对路径的平滑性方面的考虑上有较为明显的提升。

2.3 二次搜索

通过上述改进后，规划路径中仍然存在较多冗余点，为了获得更优路径，对上述路径进行二次搜索，剔除不必要的节点，在保证减少折点个数的同时又缩短了总路径的长度。具体操作步骤如下：

步骤一提取上一步骤规划的全局路径节点集P(N)，P(N)包含整个路径的节点。

步骤二从起点节点P(1)向后遍历P(N)中的所有节点，筛选出所有角度约束函数a(n)≠0的节点定义为折点，提取这些折点组成的节点集T(M)。

步骤三遍历折点节点集T(i)，i=1,2,…，m，找到当前节点T(i)相邻的前后节点T(i-1)和节点T(i+1)，连接这两个节点，判断两点连线是否穿过障碍物；若没有，则剔除节点T(i)。

步骤四遍历所有节点集T(m)，剔除所有冗余折点，得到最终节点集P(L)，P(L)作为最终的全局路径节点集。

采用15×15的栅格地图对上述算法进行测试与仿真，仿真效果如图4-图6所示，其中：图4是优化启发函数的A*算法的仿真结果；图5是添加了角度约束的第一阶段搜索结果；图6是经过二次搜索后的最终结果。图中三角形标志代表路径起点，叉号标志代表路径终点。由图中可知，传优化启发函数的A*算法出现5个折点，15个扩展点，总转折角度为675°。由图5可知，对优化启发函数的A*算法加了角度约束的A*算法只出现了2个折点，也是15个扩展点，总转折角度为270°，相对于传统A*，在总扩展点不变的前提下转折角度明显降低。由图6可知，采用Two-Stage(二级搜索)A*算法只出现了1个折点，扩展节点减少到6个，路径累计转折角度为121.45°。

图4 优化启发函数的A*算法规划仿真

图5 添加角度约束的A*算法规划仿真

图6 两阶段A*算法规划仿真

算法不同优化阶段的仿真结果如表1所示。改进的两阶段A*算法通过引入新的约束函数以及剔除冗余折点的方式，相比于优化启发函数的A*算法，在路径平滑性上有明显提升并显著减少了路径点个数，累计转折角度相比于传统算法减小了82%，路径点个数相比于传统算法降低了60%，路径的总长度相比于传统算法缩短了2.89%。

表1 算法不同优化阶段的仿真结果

3 实 验

为了验证改进A*算法在移动机器人路径规划中的实用性，将改进A*算法应用到如图7所示的自主搭建的机器人平台。该机器人平台配备三个全向轮，可以进行全方位的运动控制，其处理器采用Intel i3-4010U，采用德国SICK TIM561激光雷达安装于机器人底盘前部用以获取外界环境信息，其最远探测距离可达10 m。该实验平台基于ROS[12]操作系统，系统框架如图8所示。利用自适应蒙特卡洛方法进行机器人定位，最后通过move_base程序模块实现全局和局部路径规划，其输入为机器人激光雷达点云、编码器里程数据，输出为机器人各个驱动轮的实时控制速度信息。

图7 实验平台

图8 改进后的ROS系统框架

实验采用ROS系统中GMapping[13-14]分别在两种常见的现实场景中建立地图并进行全局路径规划。

测试场景1为狭长的走廊环境，地图大小为10 m×10 m，测试场景及其建立的地图如图9所示。测试场景2为空间复杂度较高的大厅环境，地图大小为12 m×8 m，测试场景及建立的地图如图10所示。建立出的地图栅格分辨率为0.05 m×0.05 m，实验设置机器人最大速度为0.5 m/s，最大转角速度为0.6 rad/s。

图9 实物测试场景1及建立的地图

图10 实物测试场景2及建立的地图

同一机器人在同一地图中取相同的起始点和目标点的情况下场景1测试效果图如图11所示，场景2测试效果图如图12所示。图中圆点表示机器人起点，白色区域中的曲线表示机器人的规划路径。可以看出，改进后的算法规划的路径上转折点更少，降低了不必要的路径弯曲程度，整体上缩短了机器人所需要实现的位移。同时改进后的算法减小了机器人在轨迹上偏航方向上的航向震荡，使机器人运动过程更加平稳。

(a)A*算法 (b)改进A*算法

(a)A*算法 (b)改进A*算法

实物测试结果如表2所示。相比传统A*算法，改进算法在扩展点个数上明显降低。场景1中算法扩展点总个数降低了266个，规划路径的总长度减少了1.89 m，算法规划过程耗时增加了0.028 s；场景2中算法扩展点总个数降低了181个，规划路径的总长度减少了0.92 m，算法规划过程耗时增加了0.48 s。虽然改进算法较优化启发函数的A*算法耗时增加，但可以生成最优路径，同时在很大程度上缩减了路径长度，方便了机器人的运动控制，使路径执行过程耗时明显减少。其在实验场景1下路径执行过程耗时减少了6.73 s；在实验场景2下路径执行过程耗时减少了2.46 s。在实验设定的两种常见的测试场景中，改进的A*算法明显优于传统A*算法。

表2 两种场景的实物测试结果

4 结 语

为了解决A*算法在机器人全局路径规划中产生过多折点的问题，本文提出了一种两阶段搜索A*算法，其采用添加角度约束的代价评价策略进行路径搜索，进而对搜索结果进行线形优化。理论和实验结果表明，本文算法的启发函数能够更准确地估计路径代价，让第一阶段搜索的效果接近最理想状态，减少了算法运算量，并加以角度约束抑制路径转折点的产生，不仅使路径考虑到了机器人运动约束，还尽量压缩了第二阶段冗余折点的计算量。再通过第二阶段搜索对路径线形地进一步优化，在保证生成满足机器人运动限制的最优路径的同时显著缩短规划的路径长度。该算法在较大面积的环境中优化效果更佳显著。将改进的A*算法应用于自主搭建的机器人平台上的实验表明，两阶段搜索的改进A*算法优化效果明显，且抑制了累计转角所引起的传感器累计误差，优化了机器人路径执行过程中的自定位，保证了路径执行的可靠性，满足实际应用要求。