基于优化RTAB-Map的室内巡检机器人视觉导航方法

周加超 葛动元 丛佩超 吕昆峰

摘 要：巡檢机器人对室内场景进行自主导航监测时，采用视觉同时定位与地图构建（simultaneous localization and mapping，SLAM）方法构建的三维深度地图存在实时性不高、定位精度下降的问题。对此，提出了一种基于RGB-D相机和优化RTAB-Map（real time appearance based mapping）算法的巡检机器人视觉导航方法。首先，通过重新配置RTAB-Map点云更新频率，实现算法优化，构建稠密的点云地图后；采用启发式A*算法、动态窗口法（dynamic window approach，DWA）分别制定全局与局部巡检路径，通过自适应蒙特卡罗定位（adaptive Monte Carlo localization，AMCL）方法更新机器人的实时位姿信息，再将搭建好的实体巡检机器人在软件、硬件平台上完成视觉导航测试实验。结果表明：优化后的RTAB-Map算法运行时的内存占比稍有增加，但获得与真实环境一致性更高的三维深度地图，在一定程度上提高视觉导航的准确性与实用性。

关键词：巡检机器人；自主导航；RGB-D相机；视觉SLAM；RTAB-Map算法

中图分类号：TH165   DOI：10.16375/j.cnki.cn45-1395/t.2023.01.012

0 引言

室内巡检是为了及时发现并消除设备在运行时可能存在的安全隐患。随着市场需求和科技的快速发展，智能巡检机器人替代人工巡检已成为发展趋势[1-2]。然而，智能巡检机器人能否高效地完成任务，很大程度上取决于可靠有效的室内自主导航技术。

目前，轮式里程计和惯性测量单元常用于机器人的室内定位技术，但长时间使用有严重的累积误差，容易造成机器人航迹推算偏差过大[3-4]。近年来，Fairchild等[5]提出的同时定位与地图构建（simultaneous localization and mapping，SLAM）方法，成为巡检机器人室内自主导航的技术基础。叶强强等[6]采用Gmapping算法[7]，根据采集到的激光雷达和里程计信息在创建二维栅格地图时进行准确自定位。李振拯等[8]结合激光雷达和惯性测量单元融合数据，消除长期建图过程中的定位漂移累积问题。然而，激光雷达造价昂贵，不宜应用于对经济性要求较高的机器人开发领域。因此，诸多学者对基于消费级相机的视觉SLAM展开深入研究。冯明驰等[9]提出一种多焦距动态立体视觉SLAM，虽然定位精度提高了6.97%，但是在KITTI数据集中的速度仅为10 FPS，其实时性欠佳。陈艳等[10]针对田间图像拟合出的导航线精度不高的问题，采用最大类间方差等图像处理方法得到田间的特征点并进行修正，以此拟合成新的导航线，优化后的算法精度提升显著，但平均耗时反而增加4 ms，约为5 FPS，时间性能有待提高。马鑫等[11]针对视觉SLAM在弱纹理场景下的定位精度和鲁棒性较差的问题，通过降低线特征匹配过程运算量来提高基于RGB-D相机的视觉SLAM算法的实时性，选取TUM RGB-D数据集[12]序列测试算法的每帧图像追踪时间，所提算法平均每帧图像的追踪时间为67 ms，时间效率较高。赵伟卓等[13]基于RTAB-Map（real time appearance based mapping）算法作为机器人的SLAM方法，通过融合RGB-D相机和激光雷达里程计感知到的数据，构建出信息更为丰富的环境地图，但同时也会增加了数据处理的复杂度，进而影响算法的实时性。因此，基于视觉SLAM的机器人导航技术对实时性和定位精度两方面有较高的要求。

综上所述，使用视觉SLAM技术建立大尺度的环境地图并完成定位功能时，由于相机采集的图像数据量过于庞大，易出现算法定位精度和实时性变差的情况。为提升算法定位精度，本文提出一种基于RGB-D相机和优化RTAB-Map算法的视觉导航方法，对算法中的初始参数进行重新配置，以构建更为精细化的环境地图。在采用上述方法建图的基础上，通过A*算法和动态窗口法（dynamic window approach，DWA）来规划巡检路线，采用自适应蒙特卡罗定位（adaptive Monte Carlo localization，AMCL）方法来完成自主巡检任务。

1 巡检机器人系统整体设计

1.1 硬件系统设计

巡检机器人硬件由感知层、计算层、执行层和监控层4部分组成，如图1所示。

本文使用ASTRA深度相机，水平视场角为58.4°，垂直视场角为45.5°，深度范围为0.6 ～ 8.0 m，最高支持1 280 × 1 024像素。此相机安装在机器人顶部，效果如图2（a）所示。通过红外接收器收集从物体反射回来的红外数据计算得到相机与物体之间的距离，并经过USB2.0传输给主控单元。主控单元选用带有Intel Processor J1900 4核处理器的工控机。底盘选用Spark两轮差速机器人底盘结构件，控制芯片为STM32单片机。差速驱动轮装有编码器，用于实时测量车轮转速。机器人在室内巡检时的数据采集场景如图2（b）所示。

1.2 软件系统设计

巡检机器人软件由指令发布模块、数据处理模块和指令执行模块构成，如图3所示。其中，指令发布模块通过远程通信技术发布巡检任务程序，并接收反馈信息。数据处理模块是基于机器人开源操作系统（robot operating system，ROS）设计的巡检机器人自主导航软件框架，包含SLAM、路径规划、导航自定位等数据的处理。指令执行模块为移动平台控制程序，伺服驱动器通过PWM信号控制2个直流电机转动，并配合万向轮实现巡检机器人的直线行驶和左右差速转向运动。

2 RTAB-Map算法原理及优化

2.1 RTAB-Map算法原理

RTAB-Map是由Labbé 等[14]学者于2013年提出、2019年进行扩展升级[15]的基于内存管理的闭环检测技术，可修正里程计漂移。本文采用基于深度相机数据接口输入的视觉SLAM技术，能在处理器上实时定位和构建稠密地图，所建地图可用于机器人导航、避障等复杂任务。该技术将数据分为3个部分进行处理，分别是短时内存（short term memory，STM）、工作内存（work memory，WM）和长时内存（long term memory，LTM）。RTAB-Map系统框架图如图4所示，所需输入有坐标转换TF、里程计信息和深度相机图像，TF用于定义传感器相对于机器人底座的位置，相机输入为深度图像，且带有相应的校准信息，这些输入信息被同步并传送给Graph-SLAM算法。输出的有原始图像、3D点云图、八叉图以及2D占用栅格地图。

RTAB-Map的闭环检测技术具体过程如下。

1）定位点获取：RTAB-Map采用SURF获取视觉单词的特征，图像在[t]时刻的特征集合为图像签名[zt]，通过签名[zt]和时间[t]创建当前定位点[Lt]。

2）定位点权重更新：通过比较签名间相互匹配的单词对定位点个数与签名总单词个数大小来求取相似度[s]，作为当前定位点[Lt]和短时内存中最后定位点之间的相似程度，得到新的权重。

[s（zt， zc）=NpNzt，Nzt≥Nzc，NpNzc，Nzt

式中：[Np]表示匹配个数；[Nzt]与[Nzc]分别表示签名[zt]和[zc]的总单词个数。

3）贝叶斯过滤器更新：通过计算当前位置点[Lt]和位于WM内的位置点产生闭环的可能性大小来保留能形成闭环的假设。[St]为[t]时刻所有闭环假设的随机变量，[St=i]表示[Lt]与一个已经被访问过的定位点形成闭环。后验概率分布[p（St|Lt）]计算公式如下：

[p（St|Lt）=ρp（Lt|St）i=-1tnp（St|St-1=i）p（St-1=i|Lt-1） ]. （2）

式中：[ρ]是一个标准化系数；[Lt]为定位点序列；[tn]表示保存在WM内最新的定位点在时间上的索引。

观察函数[p]和似然函数[γ]来区分定位点[Lt]与[St]之间的相似度的差异程度。将[Lt]与[St=j]进行比较，得到对应的相似度[sj]，再与标准差[σ]求差。计算公式如下：

[p（Lt|St=j）=γ（St=j|Lt）=sj-σμ，sj≥u+σ，1，          sj

[Lt]是一个新的定位点[St]的可能性，即[St=-1]的概率，计算公式如下：

[p（Lt|St=-1）=γ（St=-1|Lt）=μσ+1.] （4）

式中，若[γ（St=-1|Lt）]数值越大，则[Lt]是一个新定位点的可能性就越大。

4）闭环假设选择：当[p（St=-1|Lt）]比设定的阈值小时，则转移模型[p（St|Lt）]中具有概率最大值的闭环假设[St=i]成立，新与旧的定位点相互间就建立起闭环链接，并更新定位点的权重[16]。

5）定位点取回：为充分利用权重较高的定位点，当利用一个闭环被检测后，若与其相邻的定位点形成闭环概率最高，将从LTM内取出放回WM，更新词典中的单词，用于将来的闭环检测。

6）定位点转移：为实现RTAB-Map满足实时性的要求，对定位匹配时间超过固定的时间最大值TL，表明这些定位点形成闭环检测可能性不大，会从WM中输送到LTM，不再参与闭环检测，减少非必要定位的时间损耗。此外，为达到实时在线处理图像的目的，本文参考机器人CPU配置对TL进行设置。

2.2 RTAB-Map算法优化

影响机器人硬件消耗的因素有：地图更新频率、是否使用仿真地图、仿真时间长短、特征点粒子数以及是否使用静态地图。由于机器人自身运动的速度较慢，因而需要对以上相关参数设置进行修改。通过运行Spark机器人的./onekey.sh文件，启动机器人基于RTAB-Map算法建图功能，可查看RTAB-Map算法的相关节点和关键参数设置。由于室内巡检机器人长期环境监测时，所建稠密地图的点云数据量过多导致实时性较差，可通过对一些初始参数的值进行优化，并通过数值优化前后占用内存和CPU消耗来对比分析，从而得出关键参数的较优值，并实现建图的质量与原来差别不大的目标，因此可以用于实际机器人的建图和导航。

3 路径规划和导航定位方法

3.1 路径规划方法

巡检机器人室内路径规划策略采用ROS提供的move_base功能包实现，主要功能包括全局和局部路径规划。为提高巡检效率、节约能源消耗，采用A*算法制定机器人到目标位置的最优巡检路线[17]。为避免机器人在自主导航过程中撞到動态障碍物，采用动态窗口法[18]制定巡检地图上的局部路径，控制机器人每个周期的速度，制定实时避障路线。

3.2 导航定位方法

在创建好室内巡检地图与路径的基础上，为判断实际轨迹与规划的路径是否一致，需通过导航定位算法推算机器人位姿。本文采用自适应蒙特卡罗定位方法，根据已有室内巡检地图，使用粒子滤波器实时估计机器人在自主巡检时的位置和姿态[19]。该方法在ROS中被集成AMCL功能包，通过订阅里程计消息、激光雷达数据话题、初始姿态以及地图话题等，发布机器人自主导航时的实时位姿变化。

4 结果与分析

4.1 优化前后建图效果分析

为分析RTAB-Map算法优化前后对建图的效果，将图1中的巡检机器人平台置于真实的室内场景中，通过监控端的电脑远程遥控机器人在室内移动，完成视觉SLAM实验。将RGB-D图像作为RTAB-Map的输入，保存构建好的三维点云地图，如图5所示。对比图5（a）和图5（b）可知，优化后的RTAB-Map算法相较于原算法，对墙体等特征点成像更清晰和稠密，边缘更精确，特征点的提取和匹配更准确，更能反映真实场景。因此，通过优化RTAB-Map算法可以提升建图效果，反映出机器人的定位较为精确。

4.2 优化前后CPU占用分析

为探究RTAB-Map算法优化前后的时效性变化情况，本文通过启动机器人Top后台任务管理器，实时查看机器人运行RTAB-Map算法时的硬件消耗情况，采用每隔5 s记录一次CPU占用数值的方法，最后计算出平均值，结果如图6所示。

从图6中数据结果可知，RTAB-Map算法优化前，当机器人处于运动状态时，地图更新频率较小，其CPU占用率越低，说明机器人的执行效率越高，有助于机器人降低自身的硬件算力需求，从而节省硬件成本。RTAB-Map算法优化后，内存占用率相比改进前有一定的增加，但是由图5可知，在提升算法点云更新频率的同时，建图效果有明显提升。说明优化后的建图算法在损失一定内存占比下能实现更高精度的建图。

5 结论

巡检机器人在导航时采用RTAB-Map算法构建稠密点云地图，为提升点云地图定位精度，本文提出一种通过优化点云更新频率的方案，以提高三维地图轮廓信息的稳定性和建图的时效性。通过优化前后建图效果和CPU占比情况的实验对比分析可知，相比于原始RTAB-Map算法，优化配置后的算法能够使深度地图的轮廓和空间点云的精度显著提升，特征点的提取和匹配更为准确，更能反映真实场景。本研究为开发一套可靠、低成本的室内巡检机器人视觉导航系统提供参考。

参考文献

[1] 周立辉，张永生，孙勇，等.智能变电站巡检机器人研制及应用[J].电力系统自动化，2011，35（19）：85-88，96.

[2] 张雄，刘浩，钟宙明，等.消毒防疫智能巡检机器人的设计[J].电子制作，2021（1）：23-24，42.

[3] 陈新伟，孟祥刚，阙雅静，等.基于IMU的移动机器人精确角度获取方法[J].闽江学院学报，2017，38（2）：20-27.

[4] 贝旭颖，平雪良，高文研.轮式移动机器人里程计系统误差校核[J].计算机应用研究，2018，35（9）：2696-2699，2703.

[5] FAIRCHILD C，HARMAN T L.ROS robotics by example：learning to control wheeled，limbed，and flying robots using ROS Kinetic Kame[M].Birmingham：Packt Publishing Ltd，2017.

[6] 叶强强，郑明魁，邱鑫.基于ROS的室内自主导航移动机器人系统实现[J].传感器与微系统，2022，41（2）：90-93.

[7] 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.

[8] 李振拯，丁恩杰，王戈琛.基于LiDAR-IMU松耦合的同时定位与建图方法[J].传感器与微系统，2022，41（4）：36-39，43.

[9] 冯明驰，刘景林，李成南，等.一种多焦距动态立体视觉SLAM[J].仪器仪表学报，2021，42（11）：200-209.

[10] 陈艳，李春贵，胡波.一种改进的田间导航特征点提取算法[J].广西科技大学学报，2018，29（3）：71-76.

[11] 马鑫，梁新武，蔡纪源.基于点线特征的快速视觉SLAM方法[J].浙江大学学报（工学版），2021，55（2）：402-409.

[12] STURM J，ENGELHARD N，ENDRES F，et al.A benchmark for the evaluation of RGB-D SLAM systems[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS），2012：573-580.

[13] 赵伟卓，杨旗，张子昂.一种优化传感器配置的机器人视觉导航系统[J].一重技术，2021（4）：67-70.

[14] LABB? M，MICHAUD F.Online global loop closure detection for large-scale multi-session graph-based SLAM[C]//2014 IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS 2014），2014：2661-2666.

[15] LABB? M，MICHAUD F.RTAB-Map as an open-source lidar and visual simultaneous localizationand mapping library for large‐scale and long‐term online operation[J].Journal of Field Robotics，2019，36（2）：416-446.

[16] 許曈，吴学娟，凌有铸，等.一种改进闭环检测算法的鼠类SLAM模型研究[J].控制工程，2019，26（3）：560-565.

[17] 陈家宝，文家燕，谢广明.基于改进A*算法的移动机器人路径规划[J].广西科技大学学报，2022，33（1）：78-84.

[18] FOX D，BURGARD W，THRUN S.The dynamic window approach to collision avoidance[J].IEEE Robotics & Automation Magazine，1997，4（1）：23-33.

[19] 杨元义，杨超.基于SLAM的机器人运动控制系统设计[J].粘接，2021，48（12）：107-111，145.

Autonomous navigation method of indoor inspection robot based on optimized RTAB-Map

ZHOU Jiachao， GE Dongyuan， CONG Peichao*， LYU Kunfeng

（School of Mechanical and Automotive Engineering， Guangxi University of Science and Technology， Liuzhou 545616， China）

Abstract：Aiming at the problem of low real-time performance and poor positioning accuracy when using visual simultaneous localization and mapping（SLAM）method to build three-dimensional depth map when patrol robots conduct autonomous navigation and monitoring of indoor scenes. In this paper， a visual navigation method for inspection robot based on RGB-D camera and optimized real time appearance based mapping（RTAB-Map）is proposed. Firstly， by reconfiguring the update frequency of RTAB-Map point cloud， the algorithm is optimized and a dense point cloud map is constructed. Then， the heuristic A* algorithm and the dynamic window approach（DWA）are used to formulate the global and local inspection paths respectively， and the real-time pose information of the robot is updated by the adaptive Monte Carlo localization（AMCL）method. Finally， the visual navigation test experiment is completed on the built hardware and software platform of the physical inspection robot. The results show that the optimized RTAB-Map algorithm slightly increases the memory ratio when it runs， but it can obtain a 3D depth map that is more consistent with the real environment， which improves the accuracy and practicality of visual navigation to a certain extent.

Key words：inspection robot; autonomous navigation; RGB-D camera;visual SLAM; RTAB-Map algorithm

（責任编辑：罗小芬）