多ROS移动小车同步定位构图与自主避障方法研究与实现

摘" 要：单个ROS移动小车已无法完成多类型场景的快速同步定位与构图，为此需要多个ROS机器人的协同定位、构图与自主避障。选择了麦克纳姆轮的全向移动机器人作为单个ROS小车，开展了Gmapping、Hector、Karto和Cartographer四类同步定位与建图方法研究和效果对比分析，完成了多个ROS小车的同步定位与构图实验，实验结果表明基于Gmapping方法精度最高，选择该方法实现定位与构图，在获得环境地图基础上设计了基于领航—跟随方法的多个ROS小车协同避障，并分别进行了有障碍物和无障碍物的自主避障实验，结果表明了自主避障策略的有效性。

关键词：同步定位与构图；ROS小车；SLAM；Gmapping；自主避障

中图分类号：TP242" 文献标识码：A" 文章编号：2096-4706（2024）10-0183-06

Research and Implementation of Synchronous Localization and Mapping and Autonomous Obstacle Avoidance Method for Multiple ROS Mobile Vehicles

HE Rui1， LI Ji2， HAN Mingwei2， HU Bin2， SA Yina2， WANG Mengyu2

（1.School of Information Science and Technology， North China University of Technology， Beijing" 100144， China;

2.Brunel London School North China University of Technology， Beijing" 100144， China;）

Abstract： A single ROS mobile vehicle is no longer able to complete the rapid and synchronous positioning and mapping of multi-type scenes. For this reason， it is necessary to carry out the collaborative localization and mapping and autonomous obstacle avoidance of multiple ROS robots. An omnidirectional mobile robot with McNamee wheel is selected as a single ROS vehicle， and four types of synchronous localization and mapping methods and effect research and comparisons of Gmapping， Hector， Karto and Cartographer are given， and the synchronous localization and mapping experiments of multiple ROS vehicles are completed. The experiment results show that the accuracy of the Gmapping-based method is the highest. This paper selects the method to realize the localization and mapping， designs the multiple ROS vehicles for cooperative obstacle avoidance based on the pilot-follower method on the basis of the obtained environment maps. The autonomous obstacle avoidance experiments with and without obstacles are carried out respectively， and the results verify the effectiveness of the autonomous obstacle avoidance strategy.

Keywords： synchronous localization and mapping; ROS vehicle; SLAM; Gmapping; autonomous obstacle avoidance

0" 引" 言

近年来，伴随机器人技术的发展和复杂任务的迫切需求，某些复杂任务无法仅仅依靠单机器人完成，目前的发展趋势由单机器人自主完成简单任务向多机器人高效协同方向发展[1]。多机器人系统已经引起了大量学者密切关注，陈卫东等人使用四台Pionneer 2机器人搭建了多机器人分布式系统，实现了编队控制与物料搬运[2]；潘无为使用12台水下滑翔机对南海地区进行协同观察[3]；京东物流也建立了全球首个无人仓库，配合机器人协同控制大大提高了货物自动分拣的效率，降低了人工成本[4]。但是大多数研究还处于试验阶段，距离多机器人编队的普及还面临着许多挑战。

同步定位与构图（Simultaneous Localization and Mapping， SLAM）是指移动机器人在无环境先验信息的条件下机器人在运动过程中能够通过传感器检测周围环境信息来确定自身位姿，同时构建周围环境的增量式地图[5]。SLAM常用的传感器为激光雷达，使用基于图优化的2D平面激光SLAM算法实现ROS小车建图。

目前典型的建图算法有Gmapping、Hector、Karto和Cartographer。Gmapping是基于粒子滤波算法的一种建图方法，特点是将定位与建图的过程分离，先通过粒子滤波算法定位，再利用里程计信息得到先验位姿并不断矫正里程计误差形成地图[6]。该算法对激光雷达频率要求较低，但是严重依赖里程计且没有回环检测，小范围建图精度较高。Hector使用高斯牛顿方法解决scan-matching问题，无须里程计，只根据激光雷达即可构建地图[7]。该算法估计了激光点在地图的表示和占据网格的概率，加入惯性测量系统并利用卡尔曼滤波算法进行滤波以避免在建图中出现局部最小而非全局最优。但该算法要求激光雷达有更高的频率且同样没有回环检测，需要将机器人小车控制在较低速度时建图效果才会理想。Karto算法是基于图优化方法进行建图，采用系数点调整法[8]，通过扫匹配和闭环检测，先求得位姿后建立地图。随着地图扩大所需内存增加，相比与其他方法在大环境下制图更让有优势，但会影响时效性。Cartographer算法也是基于图优化框架，支持多传感器融合建图，通过处理激光雷达、IMU、里程计等传感器的信息数据进行地图构建。该算法的主要理论是通过闭环检测来消除构图过程中产生的积累误差[9]，所以使用低成本的激光雷达也能构建出效果不错的地图。但是在提高地图准确性的同时需要占用更多内存，计算量较大。

多机器人协同避障是指在编队控制算法下多个机器人在未知环境下进行交互，并保持特定的几何队形到达目标区域，同时能够躲避运动至目标位置的过程中出现的障碍物[10]。典型的机器人编队控制方法有领航跟随法、人工势场法、虚构结构法等。本文以塔克创新出品的小车平台为研究对象，通过增加通信模块建立多机器人间通信，使用SLAM技术和PID算法探究领航跟随模式下多机器人小车室内整体避障效果。

1" 移动小车运动学模型

麦克纳姆轮的所有轮子均为主动轮，可以实现全方位移动。在ROS系统中坐标系遵循右手定则，即以机器人底部的几何中心建立机器人局部坐标系，对于小车来说X轴正方向为小车的正前方，Y轴正方向为小车的左方，Z轴正方向为小车逆时针旋转方向。由于目标物在移动的机器人坐标系和真实坐标系中的坐标不相等，所以需要进行坐标变换。文献[11]从四轮驱动的全向移动机器人平台的运动矢量出发，将机器人小车视为刚体在刚体平面运动，推导出了机器人小车各机械轮的运动学逆解速度模型为：

（1）

其中， 表示某个轮子的速度，、 表示小车沿X轴、Y轴运动速度，ω表示yaw轴自转的角速度，a、b表示小车底盘质心与车轮周之间的半径。

2" SLAM方法

2.1" SLAM理论框架

机器人小车的自主导航是指给小车下达目标位置指令，小车在避开障碍物的前提下进行运动到目标点位的最佳路径轨迹选择。实现自主导航的核心是机器人的定位和路径规划。在该功能的框架中有两个功能包：move_base功能包和amcl功能包，如图1所示。

move_base功能包提供导航的主要运行与接口交互，实现了最优路径规划，主要由两大规划器组成。一是全局路径规划（global_planner），根据给定的目标位置和全局地图进行总体路径的规划。二是本地实时规划（local_planner），在实际情况中机器人小车针对地图信息和周围随时可能出现的障碍物使用TEB（Timed Elastic Band）算法探索躲避，通过“是否会撞击障碍物”“所需要时间”等评价标准选取最优路径。为了保证导航点准确性，小车还需要进行精准的定位，由自适应蒙特卡洛定位方法实现，根据粒子滤波的不断迭代收敛推算出小车的精准定位。

2.2" 多机器人编队

如图2所示，本文使用领航跟随法进行多小车的协同控制与整体避障，通过领航者计算障碍物间距判断车队是否能按照期望队形通过，若小于可行间距则进行整体避障[12]。

在编队过程中，领航者基于地图中的环境信息，通过轨迹优化等方法可以优化出路径最短的路径，同时根据领航者当前的位置和预期位置计算得出位置误差反馈控制调整领航者的位置，同时将目标位置信息发送给跟随者，跟随者根据接收到的目标位置信息进行移动。实验中增加了通信模块，能够将领航者的位姿信息以及运动轨迹通过话题节点发布给跟随者，跟随者通过订阅接收信息并根据相互之间的距离形成编队，借助多机器人之间的通信实现编队控制。在一个编队系统中，只能有一个最高级别的领航者。该方法的优点是仅需领航者的位置和姿态信息，领航者和跟随者之间的相对位置差关系，即可实现协同编队。协同编队的结构和运动轨迹由领航者控制，不受跟随者的限制。同时该方法未考虑跟随者避障区域范围的信息未传递给领航者，容易造成领航者成功避障，某一个或多个跟随者避障失败的情况。

3" 实验验证

3.1" 单机器人室内建图比较

SLAM技术是机器人小车路径规划的前提，而单机器人小车构建未知环境的地图是一切实验的基础，是实现多ROS机器人小车高效快速实现协同编队的基础。在获得位置信息后，移动小车可依据自身位置与目标位置的位置差优化最优路径，实现协同导航与避障。在多小车编队中往往是以领航者所规划出的最优路径作为整个队形的整体路径。Gmapping、Hector、Karto和Cartographer四种SLAM激光雷达算法实际建图中存在较大的性能差异，为了评估实际建图的效果和有效性，开展单机器人建图性能分析与对比。

图3给出了实验环境场景，该实验环境位于某个楼梯间的过渡区，整体呈长方形，分为自主搭建的工作区和阳台，两片区域之间可连通并由分隔墙隔开。工作区中设立一张较大的桌子、椅子和黄色小板凳作为障碍物，阳台没有障碍物，可行区域较大，可验证小车在空旷和复杂环境中的建图与导航效果。

以Gmapping算法为例探究小车在未知环境中建图的过程。首先使用SSH完成远程PC端与小车的通信连接，确认网络配置成功后开始地图创建。输入指令打开Gmapping算法节点，启动rviz仿真环境，获得初始时刻激光雷达采集的点云数据，如图4所示。

通过开启键盘遥控节点，在实验场地内ROS机器人顺时针方向运动。建图结果如图5所示，该图表明激光雷达获得环境点云数据随时间积累，不断构成了实验场景的环境信息。观察整个建图过程可以发现当ROS机器人离黄色小板凳较远时并无法探测出其信息，所以在地图上并不能显示出小板凳的存在，但当ROS机器人行驶到如图5（c）所示的工作区位置时，距离小板凳的距离较近，可以扫描出它的存在。当小车绕环境一周回到初始位置，实现了回环。环境信息建图完成，与实际环境信息对比可以发现，建图结果与实际场景相符合，建图精度较高。

四种方法最终构建的地图效果如图6所示。其中，白色区域表示可通行区域，灰色区域表示未知区域，黑色区域表示障碍物或边界。当小车完成一次顺时针闭环运动后，由图6（a）和图6（c）可以看出使用Gmapping算法和Karto算法可以构建出质量较高、较为完整的实验环境地图，但相比之下Gmapping算法所用时间更短、所占的内存更少，可以在较小环境中拥有更高的建图效率。由图6（b）可以看出Hector算法虽然不许要里程计信息，但是由于没有回环检测以及对雷达频率的要求，即使将小车运动速度降低至0.2 m/s也会产生地图错位，导致建图质量较低。而由图6（d）可以看出Cartographer算法的计算量较大，在小车顺时针运动一圈后较其他算法而言地图中仍存在较多灰色未知区域，不能准确反馈障碍物信息，建图质量较差。

根据理论分析以及实验分析，得出四种建图方法的建图效果对比，如表1所示。

根据多方面比较，Gmapping算法计算量小，虽然没有回环检测但在小范围的室内建图能得到不错的效果，最终选择Gmapping算法进行实验环境的地图构建，并使用该算法建图的结果图6（a）作为后续多小车协同编队避障实验的基础。

3.2" 多机器人导航与避障实验

为了验证多机器人编队的有效性，将3辆机器人小车摆放至实际场景中，启动小车，使其保持三角形队结构，领航者位于视场范围前方，跟随者位于后方。当领航者检测到障碍物时，如果障碍物宽度远大于小车队形宽度，则可以直接编队通过，不需要避障；如果障碍物宽度小于等于小车队形宽度，则对移动小车的通信结构路径再优化，实现协同避障，在避障过程中，多机器人通信结构保持三角形。

首先需要在rviz可视化界面使用方框中的绿色箭头“2D Pose Estimate”进行小车初始位置的标定与修正，当领航小车的雷达信息与地图的边缘及障碍物完全重合时即为标定成功。图7为三辆小车进行标定后在实验环境地图中的真实位置。然后使用“2D Nav Goal”给小车发布导航信息，小车便可避开障碍物实现自动路径规划。

给领航者设置一个目标点于工作区与阳台连接处，小车以0.3 m/s的速度进行无障碍定点导航。实验结果如表2所示。ROS小车协同完成了导航任务。

图8呈现了整体避障过程，三辆小车保持三角形通信结构。首先在机器人视场内设置一个目标点，图8（a）中箭头所示方向为全局的路径方向，三辆机器人小车始终以排列成三角形运动，从图8（b）表明，当领航者发现障碍物时，同时未发现可行空间时，领航者对可行路径进行规划，即从左侧行驶避开障碍物，并继续向目标点前进，此时领航者将位置信息和规划的路径信息发送给跟随者，使得三辆小车能够保持原队形避开障碍物，到达目标点位，如图8（c）所示。

为验证算法有效性，开展了5次自主避障实验，结果如表3所示。

由实验结果可以看出，跟随者1处于外侧，可以和领航者顺利完成避障，到达目标点。但处于内侧的跟随者2由于PID控制拥有的滞后性，在跟随领航者轨迹运动时不能及时避开障碍物，导致两次实验撞击到了挡板，未能完成避障。且在整体避障中领航者定位精度不一致，拥有一定的跟踪误差，会导致小车编队变形。

4" 结" 论

本文以基于麦克纳姆轮的全向移动ROS智能机器人小车为实验平台，通过实验验证了自主避障方法的有效性。首先对比了SLAM技术中四种激光雷达建图算法的效果，结果表明Gmapping算法更适用于小范围室内建图，并以建图结果作为后续实验的基础。其次为小车之间增加了通信模块，实现了多机器人小车间信息的实时传输，并采用领航跟随模式对小车进行协同控制编队，实现了对多机器人小车整体避障效果的实验探究。本文在避障实验时考虑的障碍物情况较为简单，未来将针对跟随者的避障问题对领航跟随方法进行改进，根据实际场景进行队形的自由切换，或选择不同路径到达目标点，以灵活运用于各种复杂的环境之中，拓展其工作范围并提升其工作效率。

参考文献：

[1] 任立敏.未知环境下多移动机器人编队控制及协作搬运策略研究 [D].哈尔滨：哈尔滨工业大学，2013.

[2] 陈卫东，席裕庚，顾冬雷，等.一个面向复杂任务的多机器人分布式协调系统 [J].控制理论与应用，2002（4）：505-510.

[3] 潘无为.分布式多水下机器人编队控制方法研究 [D].哈尔滨：哈尔滨工程大学，2018.

[4] 靳士超.基于麦克纳姆轮的全向智能移动机器人导航系统研究 [D].苏州：苏州大学，2018.

[5] GOBHINATH S，ANANDAPOORANI K，ANITHA K，et al. Simultaneous Localization and Mapping [SLAM] of Robotic Operating System for Mobile Robots [C]//2021 7th International Conference on Advanced Computing and Communication Systems （ICACCS）.Coimbatore：IEEE，2021：577-580.

[6] GRISETTI G，STACHNISS C，BURGARD W. Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters [J]. IEEE Transactions on Robotics，2007，23（1）：34-46.

[7] KOHLBRECHER S，STRYK O V，MEYER J，et al. A Flexible and Scalable SLAM System With Full 3D Motion Estimation [C]//2011 IEEE International Symposium on Safety，Security，and Rescue Robotics.Kyoto：IEEE，2011：155-160.

[8] NT S K，GAWANDE M，VERMA H，et al. Mobile Robot Terrain Mapping for Path Planning using Karto Slam and Gmapping Technique [C]//2022 IEEE Global Conference on Computing， Power and Communication Technologies （GlobConPT）.New Delhi：IEEE，2022：1-4.

[9] HESS W，KOHLER D，RAPP H，et al. Real-Time Loop Closure in 2D LIDAR SLAM [C]//2016 IEEE International Conference on Robotics and Automation （ICRA）.Stockholm：IEEE，2016：1271-1278.

[10] OH K K，PARK M C，AHN H S. A Survey of Multi-Agent Formation Control [J].Automatica，2015，53：424-440.

[11] MAULANA E，MUSLIM M A，HENDRAYAWAN V. Inverse Kinematic Implementation of Four-Wheels Mecanum Drive Mobile Robot using Stepper Motors [C]//2015 International Seminar on Intelligent Technology and Its Applications （ISITIA）.Surabaya：IEEE，2015：51-56.

[12] SHAO J Y，XIE G M，YU J Z，et al. Leader-Following Formation Control of Multiple Mobile Robots [C]//Proceedings of the 2005 IEEE International Symposium on， Mediterrean Conference on Control and Automation Intelligent Control.Limassol：IEEE，2005：808-813.

作者简介：何瑞（2002—），男，汉族，北京人，本科在读，研究方向：SLAM；李继（2002—），男，汉族，重庆人，本科在读，研究方向：大数据与SLAM；韩铭伟（2003—），女，汉族，山东德州人，本科在读，研究方向：大数据与SLAM；胡斌（2003—），男，汉族，福建莆田人，本科在读，研究方向：大数据；萨逸娜（2002—），女，蒙古族，北京人，本科在读，研究方向：大数据；王孟瑜（2003—），女，汉族，北京人，本科在读，研究方向：大数据。