基于A*与DWA 算法的混合路径规划算法研究

李森杰,郑洪瀛,杨 超,武 畅,王红波

(吉林大学 电子科学与工程学院,长春130012)

0 引 言

随着机器人已广泛应用于生产生活的各个方面,如智能物料运输[1],无人驾驶汽车[2]、服务机器人等,特别是近年来的新冠肺炎导致医护人员紧缺,传统以人工进行医疗资源配送方式已无法满足当今社会发展需要,而且病毒还可以通过人与人的接触进行传播,因此医疗资源的配送成为目前人们十分关注的问题。而解决这一问题的关键技术之一就是针对移动机器人的路径规划[3]。

路径规划[4]是指在周围环境存在障碍物的情况下,为控制对象规划一条从起点到终点的安全的最优路径[5-6]。路径规划算法目前应用较多的有遗传算法、蚁群算法和A*算法等[7-8]。其中A*算法的核心思想是利用启发函数对算法遍历的方向进行引导,避免盲目搜索,提高算法的收敛效率,但该算法不适用于存在动态障碍物的环境。动态窗口算法(DWA:Dynamic Window Approach)可以预估一定范围内动态障碍物在一段时间内的运动轨迹,规划路径以避开动态障碍物,但该算法对全局路径规划有维数爆炸增长特性[9]。

迄今为止,国内外学者在无人驾驶以及移动机器人领域提出了许多关于路径规划的智能算法,每种算法都在路径搜索上表现出不同的优势,但没有任何一种算法适用于任何场景,因此笔者将在利用Gmapping算法进行建图,AMCL(Adaptive Monte Carlo Localization)算法对机器人在地图中定位后,结合A*算法和DWA算法实现对路径的静态和动态规划以达到寻路和躲避动态障碍物的目的,并分析研究它们在各种不同地形下的路径规划时间和平均寻路速度。

1 建图与定位

1.1 Gmapping算法

其中dx,dy和dθ分别为对应位姿信息的增量,rand n为高斯随机数;esrr和estr分别为平移误差对平移误差和旋转误差的影响;esrt和estt分别为旋转误差对平移误差和旋转误差的影响;esxy为xy方向上的相互影响,取esxy=0.3 esrr。

Gmapping的结构如图1所示。

图1 Gmapping框架图Fig.1 Gmapping frame diagram

1.2 AMCL算法

AMCL算法即自适应蒙特卡洛定位算法,用于实时估计机器人在已知地图条件下的姿态信息[13]。当前时刻的姿态信息由后验信度决定,即

2 路径规划

2.1 A*算法

A*算法核心思想是利用启发函数对算法遍历的方向进行引导[14]。利用评估函数对网络的每个节点进行评估[15],其评估函数如下

其中F(n)为从起始节点到目标节点的评估代价,G(n)为特定起始节点到特定目标节点的评估代价,H(n)为当前节点到目标节点预测评估代价。对H(n),笔者采用曼哈顿预估算法

其中D为曼哈顿距离,ab为栅格方块面积,xn和yn分别为当前方块的水平和垂直位置,xgoal和ygoal分别为目标方块的水平和垂直位置。

A*算法工作原理如图2所示。

图2 A*算法流程图Fig.2 A*algorithm flow chart

A*算法实现需要两个列表即OpenList和CloseList用于存储路径上节点。OpenList用于存储待检查的节点,其初始化时只包含一个起始节点,而CloseList用于存储已检查过的节点,CloseList中的节点不会再被第2次检查,其初始值为空。A*算法从起始节点s开始进行路径规划,规划中产生的节点存储于上面的两个表中。路径上的每个节点n都有一个指针指向其父节点,同时每个节点n都有与它相关联的估价函数F(n)的值。

A*算法的每次循环,都会从OpenList中选择F(n)值最小的节点n,如果n是目标节点,则循环结束,找到了最佳路径。如果节点n不是目标节点,则将其从OpenList里取出,存到CloseList中。同时,检查所有与他相邻的节点,除在CloseList中的节点外,如果节点不在OpenList中,则将他们存到OpenList中,并将刚存入到CloseList的节点作为这些新存入节点的父节点。通过这种方式进行节点的扩展,最终OpenList上将有一个目标节点,算法终止,然后从终点开始,通过指针向父节点移动回到起始点,就是A*算法所规划的最佳路径。

2.2 DWA算法

动态窗口法的核心思想是,在控制空间中对移动机器人每一时刻的速度空间即线速度vt和角速度wt进行多组采样,同时模拟在每个采样速度下,机器人在时间t内可能的运动轨迹以及可能会遇到的障碍,对所有可能的运动轨迹,采用目标函数选取最优路径,再将最优路径所对应下的速度(vt,wt),作为该路径下的速度指令,控制机器人在该路径下的运动状态。速度采样需满足以下约束条件[16]

2.3 混合路径算法

混合路径规划算法实现流程为:先加载全局栅格地图后,基于A*算法全局路径规划获取子目标点序列,然后确定局部子目标点判断子目标点是否被障碍物阻挡,若没被障碍物阻挡就基于DWA算法进行局部路径规划,否则更新全局路径。DWA局部路径规划后,判断子目标点是否为全局目标点,如是则完成路径规划,否则更新局部子目标点重复操作,如图3所示。

图3 混合算法流程图Fig.3 Hybrid algorithm flow chart

3 仿真环境搭建

由于目前仿真软件功能逐渐完善,已经能极大地还原真实情况,为研究人员的开发与研究节省了大量时间。因此笔者后续所有的实验都将基于仿真环境完成。所使用的环境配置如表1所示。

表1 仿真环境配置Tab.1 Simulation environment configuration

3.1 移动机器人模型建立

笔者选择Turtlebot2作为仿真实验主体机器人,选择Kinect作为地图图像获取传感器。Turtlebot2是一款二轮差分驱动式移动机器人,具有体积小、重量轻、易于搭建和配置的优点,与ROS系统具有较好的适配性。Kinect是一款双目RGB-D摄像头,优点是可以获取信息丰富的三维点云数据,不仅包含环境的RGB信息,还包含环境的深度信息,也可以进行降维转换成二维激光数据。Turtlebot2和Kinect的具体参数如表2,表3所示。

表2 Kobuki底盘参数Tab.2 Kobuki chassis parameters

表3 Kinect传感器参数Tab.3 Kinect sensor parameters

根据上述参数,使用URDF和Xacro构建移动机器人模型步骤如下:

1)简化机器人模型,只保留左右轮、前后承重轮、传感器、支撑板等关键部件;

2)根据实际数据确定各部件的形状,并为各个部件设置物理和碰撞属性;

3)确定各部件连接的关节类型和坐标变换关系;

4)使用URDF和Xacro编写机器人模型描述文件。

按照上述步骤建立的机器人模型的拓扑关系如图4所示。

图4 移动机器人模型拓扑图Fig.4 Topology diagram of AGV model

在Rviz中可对建成的机器人模型的各项参数进行可视化观察和调试,并使用joint_state_publisher节点检查模型轮子的旋转方向是否正确,机器人模型的TF(Tranform)变换关系如图5,图6所示。

图5 TF变换关系正视图Fig.5 Front view of TF transform

图6 TF变换关系俯视图Fig.6 Vertical view of TF transform

3.2 环境模型建立

搭建完成机器人模型后,需要使用Gazebo对机器人运行的环境进行搭建。Gazebo为用户提供了丰富的模型库,如立方体、圆柱体、桌子和垃圾箱等诸多障碍物模型,极大地提高了用户搭建环境的速度。而且Gazebo与ROS系统具有良好的通信接口,可以方便地在ROS上发布或订阅话题信息。使用Gazebo搭建环境模型步骤如下:

1)确定外围墙壁的形状和尺寸,绘制外围墙壁;

2)确定内部障碍物类型、形状和尺寸,绘制内部障碍物;

3)确定内部障碍物的布局,根据需要将内部障碍物布置在合适位置;

4)使用Gazebo的Building Editor功能绘制墙壁,并在主界面中创建和布置障碍物和机器人模型;

5)保存环境模型为.world文件,编写launch文件以方便后续启动。

经过考察和研究,笔者选取了4种典型的环境地形进行研究,分别是U型、S型、L型和狭长通道地形,建成的环境模型如图7~图10所示,图中标注了模型的尺寸参数。

图7 U型地形环境模型Fig.7 U-shaped Terrain environment model

图10 狭长通道地形环境模型Fig.10 Narrow passage terrain environment model

3.3 环境地图建立

环境模型搭建完成后,需要使用SLAM技术对环境模型进行建图。笔者采用最为常用和成熟的gmapping算法获取栅格地图,并使用Rviz获取和保存生成的栅格地图。具体步骤如下:

1)确定gmapping算法的配置参数,编写launch启动文件;

图8 S型地形环境模型Fig.8 S-shaped terrain environment model

图9 L型地形环境模型Fig.9 L-shaped terrain environment model

2)启动环境模型的launch文件;

3)启动gmapping算法的launch文件;

4)启动turtlebot的keyboard_teleop的launch文件,实现对机器人模型的运动控制;

5)启动Rviz,订阅/map、/RobotModel、/LaserScan节点,对机器人的建图情况进行实时监控;

6)使用map_saver节点保存建成的环境地图,以备后续使用。

上述地形建成后的环境地图如图11~图14所示。

图11 U型地形环境地图Fig.11 U-shaped terrain environment ma p

图12 S型地形环境地图Fig.12 S-shaped terrain environment map

图13 L型地形环境地图Fig.13 L-shaped terrain environment map

图14 狭长通道地形环境地图Fig.14 Narrow passage terrain environment map

由于Kinect作为RGB-D传感器,精度不高,可以发现建成的地图都出现噪点,但环境模型的基本要素都已具备,并不影响使用。

4 算法验证与分析

基于上述准备,使用A*和DWA的混合算法,验证在不同地形条件下移动机器人在自主导航过程中的搜索能力。这将用到ROS中的两个核心功能包amcl和move_base。前者用于实现机器人的位姿估计,后者用于规划最优路径。具体的使用步骤如下。

1)确定各项配置参数,编写配置文件。在本地规划器配置文件中声明使用DWA算法;在全局规划器配置文件中声明使用A*算法。

2)编写launch启动文件。内容包括启动各项配置文件、地图服务器加载环境地图、move_base节点和amcl节点以及发布/odom与/map之间的静态坐标变换。

3)以环境模型文件作为输入,启动环境模型的launch启动文件。

4)以环境地图文件作为输入,启动自主导航的launch启动文件。

5)启动Rviz,订阅map、/RobotModel、/Path等节点。先使用2D Pose Estimate功能使机器人对自身位姿进行大致估计,再使用2D Nav Goal功能进行简单的导航,使机器人逐渐对自身位姿有准确的估计。

6)编写并运行Python脚本,使移动机器人在选定的起始点和目标点之间反复运动,并输出其路程长度和寻路时间等数据。

上述地形规划出的全局路径如图15~图18所示。

图15 U型地形路径规划Fig.15 U-shaped terrain path planning

图16 S型地形路径规划Fig.16 S-shaped terrain path planning

图17 L型地形路径规划Fig.17 L-shaped terrain path planning

图18 狭长通道地形路径规划Fig.18 Narrow passage terrain path planning

通过以上实验结果图可发现,移动机器人成功地在这4种地形下规划出了最优路径,但这并不一定是机器人最终实际的运行轨迹,由于配置了DWA算法作为局部路径规划器,在机器人运动的过程中,传感器仍然会不断采集数据,生成局部代价地图修正原先的路径以避免碰撞。

因此,笔者将A*和DWA混合算法与单独的A*算法在不同地形中进行比较,以研究该混合算法在不同场景下的普适性,并验证混合算法是否较单一算法有更好的性能。笔者进行两组实验,并多次测量求取平均值,最终将所获得的实验数据汇总成表格如表4,表5所示。

表4 A*算法实验数据Tab.4 Experimental data of A*algorithm

表5 A*算法和DWA混合算法实验数据Tab.5 Experimental data of A*algorithm and DWA hybrid algorithm

通过以上的路径规划结果和实验数据可以发现:1)移动机器人能避开障碍得到平滑路径,行进过程中再出现障碍依旧可以避开,即使遇到无法避免的障碍也能及时停下避免碰撞,算法具有有效性,在完成任务前提下保障安全;2)A*和DWA混合算法相较于单独的A*算法具有更短的路径长度,使平均速度增快;3)A*和DWA混合算法的机器人在L型地形下寻路情况最佳,平均寻路速度很快,在U型地形和S型地形下则速度一般。

5 结 语

笔者基于ROS仿真环境,搭建了机器人模型、传感器模型、环境模型、环境地图进行A*与DWA算法的混合路径规划研究。实验选取了U型、S型、L型、狭长通道地形4种常见的地形,统计分析了路径规划时间、寻路时间、路径长度、平均速度4个方面的数据。最终,基于实验数据可以发现,A*和DWA混合算法相较于单独的A*算法具有更加优良的性能,且4种地形在最初的路径规划时间上相差不大,但在寻路过程中,L型地形的寻路速度最快,U型地形和S型地形则速度较慢,原因是在尖锐拐角处A*与DWA的混合算法迭代次数多计算量大,未来可以通过优化DWA算法的评价函数改进这一问题。通过该实验可以为实际情况下移动机器人使用A*与DWA算法进行混合路径规划的选择提供一个可靠的思路和方向。