基于定向搜索A*算法的平滑路径规划

李圣达，郑宇锋，吕娜，李诗瑶，祁宇峰

(1.大连交通大学 电气信息工程学院，辽宁 大连 116028；2.大连交通大学 机械工程学院，辽宁 大连 116028；3.大连交通大学 软件学院，辽宁 大连 116028；4.大连理工大学 化工学院，辽宁 大连 116023)

移动机器人在当今物流行业中具有举足轻重的地位.为了适应物流行业的高速发展，移动机器人路径规划问题对于时效性的需求日益增加，已逐步成为国内外机器人学者研究的热点问题.路径规划指在当前环境下，移动机器人能够依据先验地图信息及实时环境数据，自主规划出一条从起始点(A点)到目标点(B点)的最优或次优无碰撞路径[1].当前主流路径规划算法包括：蚁群算法、Dijkstra算法、A*算法、遗传算法及可视图法等.其中A*算法因原理通俗易懂、搜索方式直接及准确度高等特点，在静态全局路径规划算法中占据了重要地位.然而，传统A*算法在路径规划中，存在冗余路径节点过多、路径较长、算法效率低、移动机器人与障碍物之间无安全距离且所得路径非平滑曲线等问题.为此,任玉洁等[2]提出了一种拓展搜索领域的平滑A*算法，消除了路径中的部分冗余节点，但是其算法拓展了搜索领域，使得搜寻路径时间变长；乔云侠等[3]提出了一种背向障碍物的搜索方法，避免了移动机器人贴合障碍物的边缘前进的可能，但增加了路径长度、搜寻时间；陈靖辉等[4]提出了一种改进A*算法，消除了对称路径，缩短了路径长度、搜寻时间，但所得路径平滑效果不明显.

通过分析上述各方法的优缺点，本文对传统A*算法进行改进.改进后的A*算法增加了定向搜索策略、关键点提取策略以及“拉直”处理策略，不仅使路径规划效率得到提升，而且减少了路径转折次数并去除了路径冗余节点，此外引入贝塞尔曲线对所得路径节点进行路径平滑优化处理，使移动机器人更加顺畅的移动至目标点.

1 传统A*算法

传统A*算法是静态全局路径规划算法中的一种经典的启发式搜索算法[5-6].其中的代价函数可表示为：

f(x)=g(x)+h(x)

(1)

式中:g(x)表示从起始节点(STRAT)至指定节点的(x)实际代价；h(x)表示点(x)至目标点(GOAL)的估计代价；f(x)表示点(x)的优先搜索程度，其值越小代表该点(x)搜索优先级越高.

传统A*算法中，启发函数h(x)的计算方法直接决定了A*算法的结果.一般来说，传统A*算法中有三种启发函数计算方法：曼哈顿距离、对角线距离及欧氏距离.其中欧式距离是在N维空间中的两点之间的真实最短距离，精确度最高且不易产生误差.本文也是应用欧氏距离方法计算启发函数h(x)：

(2)

2 定向搜索的A*算法

传统的A*算法采取8邻域搜索方式[7]进行全局路径规划.该方法不但存在搜索邻域过多、耗时长及计算量大等不足，而且无法避免移动机器人贴合障碍物边缘前进的不良情况.为解决上述问题,本文提出定向搜索方式改进的A*算法.首先在传统的A*算法的基础上增加定向引导启发函数γs作为搜索最高优先级，随后加入避障约束条件，明确搜索方向；其次对所得初始路径节点进行关键节点提取操作并进行路径“拉直”处理；最后通过贝塞尔曲线对经过上述步骤处理后的路径节点进行平滑路径优化，得到一条最优平滑路径.

(1)定向搜索策略

通过当前节点与目标点坐标所组成的直线求出γs值，并选择合适的方位判定阈值(ε1，ε2)与其进行差值比较，确定搜索方向.其中定向导引启发函数公式为：

(3)

具体为以下四种情况：

1)γs大于ε1且小于ε2时，优先搜索方向为东北、西南方向；

2)γs大于等于-ε2且小于等于-ε1时，优先搜索东南、西北方向；

3)γs大于-ε1且小于-ε2时，优先搜索东、西方向；

4)γs大于ε2或者小于-ε2或不存在时，优先搜索南、北方向.

再依据起始点与目标点之间的横坐标或纵坐标的关系和障碍物约束条件进一步明确搜索方向，并确定下一待扩展节点，解决了传统A*算法盲目搜索和耗时长的问题.定向搜索策略示意图见图1.

(a) 障碍物在直线上

利用定向导引函数值确定搜索方向后，若出现图1(a)所示情况：当障碍物在目标节点与当前节点之间的连线上时，因需要避开障碍物，则终止该方向搜索；若出现图1(b)所示情况：当障碍物在连线的某一侧时，则可以继续搜索；若出现图1(c)所示情况：当障碍物在连线的两侧且位于正方向时，可以继续搜索.待在初步确定的方向搜索完成后，转而搜索东、南、西、北四个方向，以其中代价函数值最小的节点作为下一待扩展节点.

本文中定向搜索的A*算法不再对当前节点的周围8邻域进行搜索，而是依据定向导引启发函数γs进行定向搜索，有效缩短搜索时间，提高路径规划效率.

(2)关键节点提取策略

栅格法规划出的路径包含许多路径节点，其中有大量路径节点为路径冗余节点，只有少数节点为生成路径所需的必要节点，如：起始点、目标点及路径转折点.针对此问题，本文采用了关键节点提取策略.具体策略如下：

若生成的路径节点集合为L={Qi|i=0,1,2,…，n}，其中Qi表示路径上的第i个路径节点.依据三点共线原理判断Q0、Q1、Q2三个节点是否共线，若三个节点处于同一条直线上，则说明Q1节点为可舍节点，执行去除Q1节点并更新路径L的操作.随后继续判断Q0、Q1、Q2三个节点是否存在可舍节点，若不存在，则继续遍历Q2、Q3、Q4三个节点，直至遍历所有的路径节点为止.遍历完成后，得到仅含有起始节点、路径转折点以及目标节点的集合L.

(3)“拉直”处理策略

利用定向搜索策略所得路径，虽然减少了搜索邻域、提高了路径规划效率，但可能存在锯齿效应，产生“Z”形路径，且该现象不利于移动机器人的运动.因此将最初得到的路径进行“拉直”处理，“拉直”处理策略示意图见图2.具体流程如下:

(a) 起始点与目标点之间无障碍情况示意图

经过关键节点提取策略优化后的路径节点集合为S={Qi|i=0,1,2,…,n}，确定起始点Q0与目标节点Qn所在直线方程，遍历该线段所经过的节点坐标，判断两者之间是否存在障碍物.若无障碍物，则删除两者之间的其他路径点，并将Q0和Qn两点保留至新的路径，同时退出程序，见图2 (a)；否则连接Q0，Qn-1，若两者之间的连线满足无障碍物条件，则保留Q0，Qn-1，Qn三个路径节点作为最终路径节点集合；否则连接Q0，Qn-2，以此类推，直至找到与起始点之间的连线满足条件的Qi节点.随后将Qi节点作为新的起始点Q0，重复上述步骤，直至路径中不存在多余路径节点，见图2 (b).采取关键点提取策略及“拉直”处理策略后与无处理策略的对比效果,见图3.

(a)无处理策略 (b)本文处理策略

本文通过定向搜索策略、关键节点提取策略及“拉直”处理策略，形成优化后的A*算法.改进后的A*算法流程图见图4，具体流程如下：

1)初始化节点信息.

2)若指定节点是目标节点，跳转至7)，否则继续执行.

3) 计算出起始节点或指定节点与目标节点之间的定向导引函数值，确定优先搜索方向.

4)依据障碍物的分布情况，选择待拓展节点.将待指定节点和东、南、西、北四个方向添加到Open_list，同时将障碍物节点添加到Close_list.

图4 改进后的A*算法流程图

5)找出最小代价函数f(x)对应的节点作为指定节点，并将该指定节点标记为Close_list中的

元素，记录其父节点并继续执行2).

6)路径关键节点提取及“拉直”处理.

7)输出路径.

3 贝塞尔曲线路径优化

本文提出的定向搜索的A*算法虽然减少了搜索路径所耗时长，避免了移动机器人无限接近障碍物的问题，但其生成的部分路径仍为折线并且存在尖峰拐点，并不利于移动机器人的转弯运动.而贝塞尔曲线[8-10]具有端点性质以及凸包性，非常适用于路径平滑工作，也使其成为在全局路径优化领域[11-12]中最重要的方法.因此本文中引用贝塞尔曲线对改进后的A*算法所生成的路径进行平滑处理.

贝塞尔曲线的定义取决于控制点的个数，当有n+1个控制点时，就可以确定n次多项式的贝塞尔曲线参数方程：

(4)

式中:Bi,n(t)为贝塞尔曲线基函数；t为归一化时间变量；n为n次贝塞尔曲线优化.

由于贝塞尔曲线的导数由控制点所决定，因此贝塞尔曲线的一阶导数可以由式(5)、式(6)表示：

(5)

贝塞尔曲线上任意一点的曲率公式为：

图5 贝塞尔曲线优化流程图

(6)

式中，x′(t)、y′(t)、x″(t)和y″(t)分别为贝塞尔曲线的坐标x和y方向上的分量对x和y的一阶导数和二阶导数.贝塞尔曲线优化的具体流程见图5.

4 基于ROS平台的仿真实验

ROS机器人操作系统是针对机器人编程及应用的一种开源操作系统.ROS能够同时支持多种编程语言来编写所用的功能包，比如最常见的C++、PYTHON、JAVA等.与此同时，还能支持GAZEBO仿真平台和计算图可视化工具、数据绘图工具、图像渲染工具及RVIZ三维可视化工具等，为广大ROS使用者提供方便.

4.1 定向搜索的A*算法仿真实验

在GAZEBO仿真平台下，采用RBPF粒子滤波算法生成20×20的栅格地图，单位为m，其中生成的先验地图的分辨率为0.05 m/栅格；同时为保证粒子滤波算法中的粒子多样性及地图的精确度，将其粒子数设置为30.在已经生成的先验地图的基础上，分别选取其中无障碍物区间和有障碍物区间进行了路径规划的仿真实验.其中仿真实验结果见图6，点划线为传统A*算法所生成路径；直线为改进后A*算法所生成的路径.

图6 A*算法改进前后仿真实验结果对比

为进一步验证实验的准确性，在无障碍物区间选取 (31,38)，(99,32) 作为X向无障碍物区间实验的起始点与目标点；在无障碍物区间选取(64,86)，(92,154)作为Y向无障碍物区间实验的起始点与目标点；在有障碍物区间选取(98,70)，(123,159)作为一般复杂障碍物区间实验的起始点与目标点，选取(98,70)，(123,183)作为较复杂障碍物区间实验的起始点与目标点.每组进行10次重复仿真实验，并记录10次实验中的传统A*算法、定向搜索的A*算法路径规划过程中的拓展节点集合N、路径节点集合L、路径转折点P、搜寻路径时间T1及计算各路径所走长度S，并将各项数据的平均值记录到表1中.

表1 不同算法实验数据结果对比

从表1、图6中的数据和仿真结果可以清晰地得出以下结论：a)在无障碍物区间内：虽然改进后的A*算法路径搜索时间较慢，但减少了移动机器人转弯次数并且得到较短的路径；b)在障碍物区间内：较传统A*算法，改进后A*算法在搜索过程中产生的拓展节点数平均减少了26.90%，路径转折点数平均减少了50%，路径节点数平均减少了96.76%，为改进A*算法的高效完成提供了有力的保障.同时也因路径转折点数的减少以及更短的路径长度，更有利于移动机器人运动.与传统A*算法相比，应用改进后的A*算法所生成的路径不再有与障碍物无限接近的位置，能够保证一定的安全距离，满足机器人基本避障需求；改进后的A*算法寻路效率与传统A*算法寻路效率优势会随着待搜索路径的长度以及复杂程度突显出来，由较复杂障碍物区间实验数据可知，改进后A*算法寻路时间比传统A*算法减少了50.32%，大大提升了算法搜索效率.

4.2 贝塞尔曲线平滑路径仿真实验

将利用定向搜索的A*算法所得到的路径，进行贝塞尔曲线平滑处理，平滑路径结果如图7所示，灰色线代表改进后A*算法生成的路径，没有进行平滑优化；黑色线代表贝塞尔曲线平滑后的路径.结果显而易见：定向搜索的A*算法在没有加入贝塞尔曲线优化之前，所得路径存在尖峰拐点，不利于移动机器人转向运动；而加入贝塞尔曲线之后，路径变得平滑有利于移动机器人快速通过路径拐角.

图7 贝塞尔曲线平滑路径

5 结论

针对传统A*算法在规划路径时存在盲目搜索、算法效率低、规划路径转折点尖锐及路径与障碍物之间安全距离不足等问题，本文提出了基于定向搜索A*算法的平滑路径规划.改进后的A*算法虽然在无障碍物区域进行路径规划时耗时略长，但在有障碍物区域的路径规划效率的优势会随着路径长度的增加而显现出来，同时通过增加关键点提取策略以及“拉直”处理策略去除了路径冗余点并减少了路径转折次数；此外引入了贝塞尔曲线，明显使路径中的尖峰拐点部分变得平滑，能够更好地满足移动机器人路径规划的实际需求.