融合改进A*和TEB算法的移动机器人路径规划

郭志军,尹亚昆,李亦轩,于秀涛,张 鹏

(1.河南科技大学 车辆与交通工程学院,河南 洛阳 471003;2.河南凯瑞车辆检测认证中心有限公司,河南 焦作 454000;3.黄河交通学院 汽车工程学院,河南 焦作 454000)

0 引言

移动机器人路径规划是实现移动机器人自主导航的重要环节[1]。日益复杂的应用场景使得移动机器人路径规划功能遇到了越来越多的挑战[2-3]。主要包括如何满足各种静、动态复杂场景要求,如何兼顾局部和全局路径规划要求,如何提升路径规划的实时性等[4-5]。这些问题受到重视。

为了解决移动机器人在复杂环境中路径规划问题,国内外学者对此进行了大量研究,主要提出了全局和局部两大类规划算法。全局路径规划算法主要有传统的Dijsra[6]、A*[7-9]、D*[10]、快速搜索树(rapid-exploration random tree,RRT)[11,12]、蚁群算法[13]、粒子群算法[14]、遗传算法[15]等。局部路径规划算法主要有动态窗口法(dynamic window approach,DWA)[16-18]、人工势场法[19,20]和定时弹性带(time elastic band,TEB)算法[21]等。其中,全局路径规划算法中的Dijstra和A*算法较适用于栅格地图条件下的路径规划。A*算法由于具有启发式搜索特点,搜索效率较Dijstra算法有一定优势,但存在搜索路线转折点较多或路线不够平顺的问题。

针对该问题,研究了传统A*算法的改进方法[22-24]。鉴于TEB算法在局部路径规划具有时间最优的性质,能对局部动态障碍物有效避障。尝试融合改进A*和TEB两种算法。新的算法不仅改善了移动机器人全局规划的性能,并且能够对环境中的动态障碍物进行规避。通过仿真和实物试验研究了所提融合算法的规律和优势。

1 移动机器人运动学模型

图1为研究的四轮差速驱动移动机器人简化模型[25-26]。在低速状态下,该模型假定移动机器人悬架和轮子都是刚性的,轮子做纯滚动,忽略Z轴平移。对该移动机器人分别建立全局坐标系(XGOYG)和局部坐标系(XOY)。移动机器人局部坐标系依据全局坐标系进行确定。附着于移动机器人自身的局部坐标系随着移动机器人的运动而运动。局部坐标系以移动机器人底盘质心为坐标系原点,以移动机器人前进方向为X轴,沿着X轴逆时针水平旋转90°建立Y轴。以移动机器人的前进方向(即X轴)和全局坐标系X之间的夹角θ为移动机器人的方向角。该夹角为[-180°, 180°],顺时针方向为负,逆时针方向为正。这样,该移动机器人下一时刻状态Xt+1在1个输入控制量Ut的驱动下,可由当前时刻状态Xt推导得出,如式(1)所示。

图1 移动机器人简化模型

Xt+1=F(Xt,Ut)+Vt,

(1)

其中：Ut为t时刻系统的输入;Vt为系统的随机噪声和模型本身的不确定性,采用正态分布,均值为0,方差根据定位精度确定。

2 改进A*算法

为解决传统A*算法规划的路线转折点较多的问题,对传统A*算法进行改进。改进之处在于由原来的8个搜索方向改为符合运动学约束的6个搜索方向,并对路径进行平滑处理,如图2所示。

(a) 传统A*算法搜索策略 (b) 改进A*算法搜索策略图2 改进A*算法和传统A*算法搜索策略对比

改进算法流程图如图3所示,具体步骤如下：

图3 改进算法流程图

(Ⅰ)划分栅格地图。

(Ⅱ)维护2个列表(open_list 和close_list)。open_list中存放待搜索的节点,close_list中存放搜索过的节点。

(Ⅲ)改进搜索策略。对于部分差速移动机器人来说,移动机器人不能直接进行左右方向的平移运动,故将传统A*向周围8个邻近节点搜索方式改进成考虑运动学约束的6个搜索方式,即左前、直行、右前、左后、右后和倒退方向。

(Ⅳ)按照改进A*进行启发式搜索,其中每个节点的评价函数可以表示为

f(n)=g(n)+h(n),

(2)

其中：f(n)为规划点n的评价函数;g(n)为定点n到起始位置的代价值;h(n)为定点n到目标位置的启发式代价值。对于每一个规划点n,改进A*算法都要评价其代价函数f(n),选取最小的f(n)对应的路径点作为可行的路径点。

(Ⅴ)判断每个节点的曲率是否发生突变。若突变,将当前节点去除;若不突变,则保留。

(Ⅵ)将去除节点的前后2个节点进行3次样条插值。

(Ⅶ)规划结束,输出全局路径。

与用直线或者圆弧去拟合机器人路径相比,3次样条插值法在路径平滑上具有很大优势。将相邻2个节点之间的变化用1个3次多项式来描述,就可解决规划路线转折点较多的问题,使得规划的轨迹更加平滑。3次样条插值方式如图4所示。

图4 3次样条插值

图5为改进A*算法和传统A*算法仿真结果对比。由图5可知：改进A*算法转折点明显减少,规划的路径更为平滑。参数如表1所示。改进A*算法的转折次数相对减少了67%,安全距离增加了88%,路径长度减少了18%,搜索时间缩短了21%。改进A*算法在全局路径规划的效果大大改善,增加了全局规划的最优性。但是仍然无法对环境中的动态障碍物进行避障。为此,在Move_base框架下,结合局部路径规划算法实现动态避障的功能。

(a) 传统A*算法

表1 路径规划结果对比

3 局部路径规划

TEB局部路径规划由一系列的位姿组成,用符号Q表示,如式(3)所示。每个点的位姿由Xi=(xi,yi,θi)表达,描述了移动机器人每个位置点的坐标xi,yi,以及朝向角θi。

Q={Xi}i=0,…,n,n∈N,

(3)

其中：xi为位姿点的横坐标,m;yi为位姿点的纵坐标,m;θi为位姿点的朝向角,rad。

TEB算法将2个位姿点之间设置时间间隔ΔTi,表示从一个构型到另外一个构型所需的时间。并将所有相邻位姿点时间间隔的集合用τ表示：

τ={ΔTi}i=0,…,n-1,

(4)

其中：ΔTi为相邻位姿点时间间隔,s。

最终,TEB被定义为2个序列的元组,如式(5)所示。

TEB=(Q,τ)。

(5)

TEB在一些情况下会对移动机器人本身进行限制,具体有如下限制：

(Ⅰ)速度约束fvel。限制移动机器人最大线速度和最大角速度。速度限制以损失的形式实现,用fvel表示。速度Vxi、Vyi和角速度根据向后差分可得。

(6)

(7)

(8)

fvel=f(vxi,vyi,ωi),

(9)

其中：Vxi为位姿点的横向速度,m/s;Vyi为位姿点的纵向速度,m/s;ωi为位姿点的角速度,rad/s。

(Ⅱ)最快路径约束fk。

fk=(∑ΔTi)2。

(10)

(Ⅲ)通过点约束fpath和障碍物约束fobs。TEB算法在规划的时候既要跟随全局规划路径点,又要避免碰撞障碍物。全局路径规划点对TEB规划点有吸引的作用。约束以惩罚函数的形式实现。

fpath=f(dmin,rpmax);

(11)

fobs=f(-dmin,romin),

(12)

其中：dmin为TEB局部规划点和障碍物之间的最小距离,m;rpmax为TEB局部规划点和全局规划点的最大距离,m;romin为TEB局部规划点与全局路径点之间的最小距离,m。

TEB约束的求解方式采用图优化求解,求解模型如图6所示。将每一个状态和时间间隔作为顶点,各种约束作为边建立超图进行多目标优化,使用g2o框架进行求解。

图6 TEB求解模型

4 融合算法实现

融合算法基于Move_base框架实现,如图7所示。所有节点利用机器人操作系统(robot operating system,ROS)节点进行通信。里程计信息、传感器数据和地图信息分别以Odom msgs、Sensor msgs和Map msgs的话题传入Move_base导航框架。改进A*全局规划器将全局参考路径以Refer routing msgs话题的形式传递给局部路径规划器。全局规划和局部规划部分均使用插件的方式进行实现。局部路径规划将全局路径规划纳入评价函数来达到跟踪全局路径的效果。评价函数参数包含轨迹与障碍物之间的距离、轨迹曲率与全局路径的跟随程度,具体可以用式(13)表示。

图7 算法融合框架

(13)

其中：di为第n个节点与障碍物之间的距离,m;ci为第n个节点的轨迹曲率,m-1;ui为轨迹与全局路径的跟随程度,1代表完全跟随,0代表完全不跟随;Ki、Li、Ri分别为3个评价函数影响因子的权重。从多条路径中选出一条评价函数评分最高的路径作为局部避障轨迹。

Tra=max{tra1,tra2, … ,tran},

(14)

其中：tra1,tra2,…,tran为局部路径规划规划出来的多条轨迹对应的评分。Tra作为得最高评分对应的轨迹。

5 仿真验证

为检验本文所提算法的性能,在CPU Inter(R) Core(TM) i5-9300,主频2.5 GHz,4核8线程计算机上对所提算法进行仿真验证。改进A*算法规划最大转向角为80°,移动分辨率为1 cm,地图尺寸为50 cm×50 cm。为了验证融合算法的有效性,设置多组仿真对比试验,分别搭建多组仿真环境。简单环境搭建如图8a所示。静态障碍物环境搭建如图8c所示。动态障碍物环境搭建如图8e所示。简单环境仿真结果如图8b所示。静态障碍物环境搭建如图8d所示。动态障碍物环境搭建如图8f所示。

表2为简单环境下改进A*算法和传统A*算法仿真结果对比。由表2可以看出：简单环境中,改进A*算法在转折次数上减少了3次。同时,由于改进了搜索方向,减少多余节点的搜索,改进A*算法相比于传统A*算法降低了近63%的搜索时间。表3为静态障碍物环境下改进A*算法和传统A*算法仿真结果对比。由表3可以看出：静态障碍物环境中,由于环境复杂度增加,传统A*算法和改进A*算法搜索时间、转折次数、路径长度上都有增加,但改进A*算法相较于传统A*算法在搜索时间上仍有53%的提升,在转折次数上减少了4次。由于在安全距离上有所增加,在路径长度上会牺牲一定的代价。表4为动态障碍物环境下3种算法仿真结果对比。由表4可以看出：对于动态障碍物环境,单独的改进A*算法和传统A*算法均不能有效避开随机出现的动态障碍物。由于改进A*算法融合了TEB算法,此融合算法具备了避开障碍物的能力。同时保证了更大的安全距离,在安全距离上相比于传统A*算法提升了2.7倍,相比于改进A*算法提升了近1倍。由于要避开动态障碍物,故在转折次数和路径长度上有所牺牲。由图8d可以看出：传统A*算法和改进A*算法都能避开静态障碍物。由图8f可以看出：传统A*算法和改进A*算法均不能避开动态障碍物。改进A*算法融合TEB算法之后对动态障碍物能够有效避让。动态避障仿真结果如图8g和图8h所示。

(a) 简单环境仿真场景 (b) 简单环境仿真结果 (c) 静态障碍物仿真场景 (d) 静态障碍物仿真结果

表2 简单环境下改进A*算法和传统A*算法仿真结果对比

表3 静态障碍物环境下改进A*算法和传统A*算法仿真结果对比

表4 动态障碍物环境下3种算法仿真结果对比

6 实物试验

为了更一步验证融合算法在静态环境以及动态环境中的避障可行性。借助AUTOLABOR科研底盘进行试验,该底盘实物即试验移动机器人,如图9所示。底盘搭载单线激光雷达、16线激光雷达以及惯性导航系统。工控机CPU为AMD Ryzen3 2 200G,核心频率3.5 GHz,加速频率3.7 GHz,4核8线程。工控机安装Ubuntu18.04系统,在此之上配置了Melodic版本ROS。所有传感器节点信息由ROS进行连接,节点构建如图10所示。

图9 试验移动机器人

图10 ROS节点图

试验过程中,通过在环境中分别设置静态和动态障碍物,验证算法的有效性。首先将移动机器人放置在静态障碍物的环境中进行试验,来验证融合算法在静态障碍物环境中避障的有效性。然后将移动机器人移回到起点,将实验室中行走的人物作为动态障碍物继续进行试验,验证融合算法在动态障碍物环境中避障的有效性。

移动机器人构建环境地图后,在规划的路线上放置2个静态障碍物,如图11a所示。Rviz上位机显示规划结果如图11b所示,移动机器人合理规划出避开2个障碍物的路线,并按照规划路线进行行驶。移动机器人规划出的局部路径不仅对障碍物做出了避让,并在之后的规划以及行驶中跟随全局路径。移动机器人实际移动轨迹如图11c所示。

为了验证融合算法在动态环境中的有效性,将行走的人作为试验动态障碍物,如图12a所示。移动障碍物行驶到如图12b所示的A点时,移动机器人检测到移动障碍物,如图12c中A1点所示。移动机器人不再按照原来在静态障碍物环境中规划的路径进行行驶,而绕过障碍物向右行驶。改进A*算法融合TEB算法规划的路径,如图12d所示。试验结果显示移动机器人能够有效规划出避开动态障碍物的轨迹,验证了融合算法对动态障碍物避障的有效性。路径规划部分参数如表5所示。

由表5所示的规划参数可以看出：融合算法在静态环境和动态环境都能够有效避障。规划的路线不仅转折次数较少,而且较为平滑。充分验证了相比于传统A*算法,此融合算法在实际环境中不仅在全局规划更优,而且能对局部动态障碍物进行有效避障。

(a) 添加静态障碍物 (b) 规划结果 (c) 静态障碍物试验实际效果

(a) 添加动态障碍物 (b) 动态障碍物试验 (c) 上位机显示 (d) 规划结果

表5 路径规划试验结果

7 结论

(1)在全局规划上考虑了移动机器人的运动约束,并对传统A*算法进行改进,有效减少了转折点个数。使用3次样条插值可使规划的轨迹更加平顺。使得改进A*算法相比于传统A*算法在全局规划方面具有更优的性能。

(2)利用Move_base导航框架将改进A*算法和TEB算法进行充分融合,结合两者的优势,既保证了全局规划的最优性,又能保证局部动态避障的有效性。

(3)使用融合算法的移动机器人在静态以及动态环境中都能按照预期完成规划任务。此算法是建立在环境部分已知的情况下进行的,下一步将研究移动机器人在完全未知的环境中自主导航的算法,提升移动机器人的路径规划水平。