自主移动机器人（AMR）混合导航算法改进

孙明，陈日辉，陈燕，陈小冰

（1.澳门科技大学，澳门 999078；2.珠海科技学院，广东 珠海 519041）

0 引 言

随着信息技术的发展，机器人的智能性和自主性不断增强，新一代的自主移动机器人（Autonomous Mobile Robot，AMR）逐步在工农业生产和日常生活中发挥更加重要的作用。特别是近年来，根据新冠疫情防控的需要，大规模的消毒机器人，配送机器人的使用，大大提高了工作效率，降低了人际病毒的风险。其中，路径规划和导航技术的使用，是自主移动机器人（AMR）实现各项功能，完成各种任务的关键性因素。

路径规划是指机器人通过一定的算法规划出一条从当前节点到任务节点的路径，属于运筹学和机器人学的基本问题之一。根据机器人对当前地图环境的掌握程度，路径规划分为两种类型，分别是通过激光、机器视觉等传感器掌握局部地图信息的局部路径规划，掌握全局地图信息的全局路径规划。国内外学者分别对两种路径规划方法进行了研究，探索出适用于局部路径规划的滚动窗口法（陈明建，林伟等，2017）、人工势场法（石为人等，2010）、模糊逻辑控制法（孙扬智，2016）等，以及适用于全局路径规划的A*算法（章淑君与曹建成，2005）、蚁群算法（Dorigo，Maniezzo& Colorni，1996）、遗传算法以及粒子群算法（王雷与李明，2017）等。

目前，机器人路径规划中的主流算法是A*算法以及在其基础上进行的各种改进算法。A*算法在经典路径规划的Dijkstra算法的基础上发展起来的一种启发式搜索算法，具有运算时间短，收敛速度快，求解路径的时间更短的优点，从而达到快速寻路的要求（章淑君与曹建成，2005）。作为一种全局路径规划算法，A*算法在动态环境中出现障碍物的情况下并不适用，这时候需要引入人工势场法等算法帮助机器人实时规划出平滑安全的新路径。

本文围绕自主移动机器人（AMR）在复杂动态环境中的运行问题，分别从地图构建、定位、路径规划三部分，综合考虑全局路径规划和局部路径规划问题，提出基于双向A*算法进行路径规划和基于激光SLAM加人工势场法进行局部路径规划的混合导航算法，以提高机器人自主运行的稳定性和实时性。

1 地图构建与机器人定位

自主移动机器人（AMR）可以利用自身装备的激光传感器、摄像头等设备，通过SLAM算法，自主构建运行区域的环境地图模型并进行自主定位。

1.1 环境建模

目前，机器人进行环境建模应用最广泛的是google推出的基于图优化的SLAM算法Cartographer，该算法使用姿态优化进行优化，具有闭环检测，有良好的系统实时性。在计算机资源有限的情况下，能获得更高精准度的2D地图。Cartographer算法核心思想为利用全局优化和局部优化对机器人位姿=(ε，ε，ε)进行优化，其中ε和ε为机器人在和方向位移的位移量，ε为机器人绕平面旋转的角度。激光雷达初始扫描帧记为原点（0，0），激光雷达扫描数据描述为={h}，∈1，2，3…，，扫描数据帧映射到子图的位姿变换记作T，公式如下：

子图由多个扫描帧构成，使用概率网格表示。每个网格定义两种状态hit和miss，每次扫描被插入概率网格中，将hit临近的所有网格点加入hit集合，除已在hit集合中的点外，将hit与扫描点连接射线上的所有接触点加入miss集合。没有加入任何集合的点设置，已观测到的点概率按照式（3）进行更新。

使用Ceres求解器，在一个扫描被插入子图前，和当前的子图进行局部优化。此操作是为了在子图中找到使插入扫描中所有的扫描点概率和最大的扫描位姿，由此便可转变为一个非线性最小二乘问题。

Cartographer算法通过稀疏位姿调整方法来全局优化，保存扫描帧插入子图的位姿，当子图构建完毕，使用所有对应的扫描帧和子地图进行闭环检测，Cartographer框架如图1所示。

图1 Cartographer框架图

1.2 自主定位

机器人通过Gmapping算法成功构建环境地图后，需要准确判断自己的位置，才能更好的进行路径规划，此时一般采用二维移动过程中自适应蒙特卡洛定位（Adaptive Monte Carlo Localization，AMCL），AMCL采用粒子滤波器来跟踪已经知道的地图中机器人位姿，对于大范围的局部定位问题工作良好。该算法可在已知地图的条件下，实时估计机器人的位姿信息。

AMCL算法思路如下：

位姿信息由机器人时刻的状态x的后验信度决定，使用S表示一组有限的粒子集，则后验信度分布为：

AMCL算法在一段时间间隔内通过采集S和重采集S来绘制粒子，更新姿态并计算出后验置信分布bel（x）在当前状态下的离散估计：

式中为样本估计概率。

重采样的计算公式如下：

更新后的粒子集为：

使粒子均满足：

2 路径规划

自主移动机器人（AMR）在地图建模和自主定位完成后，根据收到的目标节点信息，既可利用A*算法进行路径规划。

2.1 A*算法

A*算法利用启发函数对遍历方向进行引导，使用评估函数计算每个格子的代价值()，是该算法中的核心，其启发函数公式如下：

式中()为当前搜索节点到起点的真实距离代价，()为当前搜索节点到终点预测距离代价。为了实现最优路径规划，常用距离代价包括曼哈顿距离、对角线距离和欧几里得距离，它们对应的距离如图2所示，其中线1表示曼哈顿距离，适合4象限（东-西-南-北）运动，路径规划速度相对较快；线2表示对角线距离，适合8象限（东-东南-南-西南-西-西北-北-东北）运动；线3表示欧几里得距离，适合任意角度的运动，路径规划的速度较慢。

哈曼顿距离不符合大部分机器人的实际运动场景，欧几里距离需要遍历大量节点，导致规划速度较慢。在范围较大且不规则地图环境下，对角线距离比较符合真实机器人的移动模型并且路径规划的时间较快，因此本文采用对角线距离（Diagonal Distance）作为对()的计算。

对角线距离（Diagonal Distance）的计算公式如下：

式中x和y为当前节点的水平位置和垂直位置，和为终点的水平位置和垂直位置。

图2 三种距离图示

A*算法在运行中需要两个列表来储存遍历的点，Open List用来存储被遍历的点，Close List用来存储被选中的点。每个点被遍历时会计算其()，Close List中的点被加入后不会再更新，每个点存储着一个指向其父节点的指针和该点的()。

初始化两个列表为空，然后将起点加入Open List。取出Open List中()最小的值加入Close List中，遍历取出点的八个象限的所有点，如果该点为障碍物或者已在Close List中，则跳过该点。如果该点在Open List列表中，则重新计算该点的父节点，否则将该点加入Open List中，继续遍历直到终点或者Open List为空。如果Open List为空，说明没有路径可以到达终点。如果遍历到终点，结束遍历，随着终点的父节点直到起点为该次寻路的最优路径，A-star算法流程如图3所示。

2.2 传统A-star算法的不足及改进

伴随着机器人运行区域的扩大，地图规模也越来越大，传统A-star算法随着搜索空间增大，扩展节点增大，搜索速度下降。同时规划出来的路径存在折角过大，路径不够平滑的问题。

本文通过改进，让A-star算法从起点和终点同时进行遍历，直到两边遍历过同一个栅格，将终点遍历的路径进行反向，获取到一条完整的起点到终点的路径，改进的A-star算法流程如图4所示。

图3 A-star算法流程图

图4 改进的A-star算法流程图

2.3 解决路径冲突的人工势场法

在实际运行中，沿着规划路径前进的机器人会遇到各种动态的障碍物，形成路径冲突，沿着既定路线前进的机器人需要重新规划新的路径，既进行局部路径规划。人工势场法结构简单，广泛应用于机器人躲避障碍的局部路径规划。

人工势场法（Artificial Potential Field Method）由Khatib在1994年提出，它通过将机器人在场景中的移动抽象到人工引力场之中，通过将机器人与目标产生的“斥力”和障碍物与机器人产生的“引力”之间产生的“合力”控制机器人的运动，图5为人工势场法的示意图。通过建立对应的势能场函数解决避障的问题。该方法能够充分考虑机器人与每个障碍物之间产生的斥力，在A*算法找到最短路径的同时避免碰撞。

目标位置与移动机器人之间引力场为：

机器人所受引力方向为引力势场负梯度方向：

图5 人工势场法示意图

设以障碍物为中心的斥力场影响半径为ρ，机器人与障碍物之间的距离为，当机器人离开斥力场影响范围，所受到的斥力为0，则斥力场大小为：

机器人所受斥力方向为斥力势场负梯度方向：

机器人所受合力大小为：

3 混合算法的设计及实现

在复杂动态动态环境路径规划中，采取双向A*加人工势场法的混合算法的机器人更具有优势，如图6所示，混合算法的实现流程是：

（1）机器人采取双向A*算法进行路径规划

（2）机器人在向目标节点运行过程中使用激光雷达等传感器对行进路径进行搜索，当发现障碍物存在时，启动人工势场算法进行局部路径规划。

（3）完成局部规划后，判断子目标点是否为终点，若是则完成路径规划，否则更新局部目标点。

图6 混合算法流程图

3.1 仿真环境搭建

本文仿真模拟采用的计算机配置如表1所示。

表1 仿真环境配置

本文利用Gazebo系统仿真软件，选用Kobuki底盘作为仿真实验主体，该底盘使用二轮差分驱动式，具有可扩展性强、易于搭建和配置的优点，在ROS系统上具有良好的适配性，Kobuki底盘参数如表2所示。传感器使用思岚A2激光雷达进行机器人周围环境搜索和建模，RPLIDAR A2参数如表3所示。可以确保机器人快速度移动时的地图构建质量。

表2 Kobuki底盘参数

高（mm） 124.8重量（kg） 2.35最大平移速度（cm·s-1） 70最大旋转速度（°/s） 180里程计52 ticks/enc rev 2 578.33 ticks/wheel rev 11.7 ticks/mm陀螺仪 1轴（110度/秒）

表3 RPLIDAR A2参数

为便于研究，将移动机器人模型进行改造：

（1）对kobuki底盘模型进行简化，删掉无用传感器，只留下左右轮、底盘、前后滚轮等必要的部件。

（2）在底盘上搭建支架，加入激光雷达传感器。

（3）为机器人配置各个部件的碰撞属性。

（4）设置关节类型和发布TF树。

（5）修改Gazebo仿真中机器人的传感器的参数和话题。

建好的机器人拓扑关系如图7所示。

在Rviz中查看机器人模型的各项参数并进行调试。图8、图9为TF变换关系正视图与仰视图，使用joint_state_publisher_gui节点检查动力轮的旋转方向是否正确，观察机器人模型各个link的TF变换关系，是否都与base_footprint有映射。

图7 机器人拓扑关系图

图8 TF变换关系正视图

图9 TF变换关系仰视图

完成机器人模型的构建后，使用Gazebo建立仿真环境，模拟机器人在真实的公共场合中运作。Gazebo可以使用Building Editor快速的绘制外围墙壁，同时里面拥有大量的立方体、垃圾桶、桌子等模型，可以极大还原的还原真实世界。环境构建的步骤如下：

（1）确定环境的外围墙壁的形状、位置和尺寸；

（2）确定环境中的障碍物的类型、尺寸和数量，绘制障碍物模型；

（3）确定各个类型的障碍物的位置，根据实际需要摆放在合适的位置；

（4）使用Building Editor绘制围墙，并将障碍物模型加入到环境中；

（5）调整好合适的观察视角后保存环境为.world文件，方便后续调用。

通过研究和观察，本文将对选取几种典型的公共场所环境地形进行研究，建完的地图模型如图10～图13所示。

图10 O形地图模型

图11 S形地图模型

图12 U形地图模型

图13 L形地图模型

环境模型建立好后，由机器人自行探索环境进行SLAM建图。本文使用Cartographer算法进行地图建立，利用Rviz可视化建立的地图模型数据。建图步骤如下：

（1）启动Launch打开启动Gazebo并加载地图和机器人模型到环境中；

（2）运行Cartographer算法构建地图的Launch文件；

（3）运行Rviz，订阅/submap_list和/scan话题；

（4）运行move_base功能包，接收控制机器人的话题；

（5）使用2D Nav Goal功能发布目标点，使机器人运动完成地图的构建。

待地图构建完成，使用命令“rosservice call /finish_trajectory 0”停止接收数据，构建好的地形图如图14～图17所示。

图14 O形地图构建

图15 S形地图构建

图16 U形地图构建

图17 L形地图构建

（1）运行rosservice call /write_state "{filename：'路径/文件名.pbstream'}"序列化保存当前的状态。

（2）运行cartographer_ros包里的cartographer_pbstream_to_ros_map将pbstream转换为pgm和yaml以便后续使用。

3.2 算法验证与分析

基于完成的准备工作，使用改进的A-star算法和人工势场法的混合算法，验证不同环境地形下机器人自主运作的避障能力。具体步骤如下：

（1）编写路径规划算法的C++文件，使用插件注册到ROS中来，查看可用插件，如图18所示。

图18 查看ROS路径规划可用插件

（2）将move_base的全局规划替换成A-star算法，本地规划替换成人工势场法。

（3）在地图模型中加入障碍物，模拟现实环境中的障碍。

（4）使用上述建立好的地图模型，加入地图启动的配置中。

（5）编写Launch文件一键启动环境，包括仿真环境、机器人模型、Rviz等的启动。

（6）启动move_base和amcl功能包。

（7）编写Python文件，使反复机器人到达选定终点并回到选定起点，输出路径规划时间和路径长度等信息（图19～图22）。

图19 O形地图路径规划

图20 S形地图路径规划

图21 U形地图路径规划

图22 L形地图路径规划

本文将A-star算法和人工势场法混合算法与传统的A-star算法在不同地形中进行比较，以研究该混合算法在不同场景下的普适性，并验证改进的A-star算法相对于A-star算法是否有更好的性能。本文进行两组实验，将所获得的实验数据汇总成表格如表4，表5所示。

表4 A-star算法实验数据

表5 改进A-star算法和人工势场法混合算法实验数据

由路径规划结果和实验数据可以得出：（1）移动机器人能避开障碍得到平滑路径，行进过程中再出现障碍依旧可以避开，即使遇到无法避免的障碍也能及时停下避免碰撞，算法具有有效性，在完成任务前提下保障安全；（2）改进的A-star算法规划的路径相对于传统的A-star算法较长；（3）改进的A-star算法相较于传统的 A-star算法具有更平滑的路线，使平均速度增快；（4）改进的A-star算法在U型地形中规划路径需要的时间更多。

4 结 论

本文通过系统仿真，搭建了地图模型、机器人模型、环境地图，验证了改进的A*算法和人工势场法混合算法的路径规划的可行性和有效性。通过选取几种常见的环境地形，并将路径规划时间、平均速度、路径长度、重新规划次数等实验数据进行了分析对比，最后得出结论，验证了改进的A-star算法相对于传统的A-star算法规划的路径更加平滑，能够适应不同的环境地形，具有良好的鲁棒性。改进的A-star算法规划的平均规划时间和平均运行时间分别减少了26.67%和23.4%，除此之外，平滑度提升明显。可以为智能机器人使用改进A*算法和人工势场法相结合的混合导航算法进行路径规划提供一种可靠的技术方案。