动态场景下基于RGB-D相机的移动机器人定位算法研究

丛佩超　崔利营　万现全　刘俊杰　肖宜轩　李佳星

摘 要：针对移动机器人在室内动态场景下视觉导航过程中定位精度低的问题，提出一种新的动态场景下视觉定位算法。该算法基于ORB-SLAM2算法框架，首先，通过RGB-D相机获取图像数据，再利用Mask R-CNN神经网络与多视图几何技术进行数据处理，实现运动目标检测；其次，将运动物体区域的动态特征点滤除，保留其静态特征點，完成相机的位姿跟踪，从而降低动态物体对机器人定位的影响；最后，通过TUM数据集下的fr3_w_xyz序列完成试验验证。试验结果表明：相较ORB-SLAM2、DynaSLAM算法，本文算法在高动态场景下的运行轨迹更优，且绝对位姿的平均误差分别降低78.70%、17.13%，标准差分别降低76.87%、30.07%，验证了该算法在室内动态场景下可实现精准定位，且具有较好的鲁棒性与准确性，可为移动机器人在室内自主导航提供理论基础。

关键词：移动机器人；动态场景；Mask R-CNN；多视图几何；精准定位

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

0 引言

随着人工智能和物联网的发展，人们对移动机器人的功能要求越来越高，希望移动机器人能完成室内场景下的特定任务，为人们服务，比如扫地机器人、交互机器人等，具有十分明朗的发展前景[1]。

移动机器人的定位是机器人进行自主导航的前提，定位技术是机器人的核心技术之一，因此开展移动机器人定位技术的研究对于实现机器人的自主导航具有重要意义[2]。目前，室内广泛使用的定位方法按照传感器类型进行分类，主要可以分为基于激光雷达的定位方法与基于相机的定位方法[3]。由于激光雷达具有定位精度高、抗干扰的优点，所以基于激光雷达的定位技术被广泛应用于室内移动机器人定位中。目前，在移动机器人领域中，基于激光雷达的蒙特卡罗定位方法较为常用。该方法利用激光雷达进行数据实时观测，实现粒子滤波器的迭代更新，从而完成对机器人位置的估计[4]。叶强强等[5]在蒙特卡罗定位方法的基础上，提出了一种基于激光雷达的室内移动机器人定位导航系统，该系统可实现移动机器人在室内自主导航，且轨迹效果表现良好。季宇寒等[6]提出了一种基于激光雷达的巡检机器人导航系统，该系统利用自适应蒙特卡罗定位算法估计巡检机器人位置和姿态，使其定位的绝对轨迹误差精度不超过0.05 m。基于激光雷达的定位技术虽能获得较高精度的位置信息，但在运动过程中易产生点云畸变和误匹配，且多线激光雷达价格较高[7]。基于相机的定位方法不仅能够获得丰富的位置信息，而且其硬件成本低廉。目前，基于相机传感器的定位方法主要为视觉同时定位与建图（simultaneous localization and mapping，SLAM）技术，该技术也逐渐成为移动机器人领域的研究热点之一[8]。Engel等[9]基于直接法提出了一种LSD-SLAM系统，该系统是直接法中比较完整的视觉SLAM系统，不仅适用于相机周围场景的地图构建，而且能够准确地估计移动机器人的位姿。Wang等[10]提出了一种DSO算法，该算法在定位的准确性、系统的鲁棒性以及运行速度上均优于LSD-SLAM。Klein等[11]提出了一种基于关键帧的PTAM算法，该算法将跟踪和建图分为2个独立线程，并利用单目相机完成了机器人定位与地图构建。Mur-Artal等[12]基于特征点提出了一种ORB-SLAM2算法，该算法不仅支持单目相机，同时还支持双目相机与RGB-D相机，可在室内大规模场景下实现同时定位与建图，且运行效果良好。以上视觉SLAM技术均把工作环境限定为静态、刚体、光照变化不明显、没有人为干涉的场景[13]，但目前移动机器人在自主导航过程中的最大挑战来自环境中的动态物体，易影响移动机器人的定位精度，从而产生人机干涉，所以对移动机器人定位精度要求较高[14-15]。虽然视觉定位技术在静态场景应用较为成功，但在动态场景研究较少。若移动机器人利用上述视觉SLAM技术，不可避免地会受动态物体影响而产生定位干扰，则研究适用于移动机器人在动态场景下的定位方法具有重要的现实意义[16]。

综上所述，针对移动机器人在运动过程中，利用传统视觉SLAM技术不能实时检测和处理室内动态场景中的运动物体，从而导致定位精度不足的问题，本文提出了一种动态场景下基于RGB-D相机的移动机器人定位算法。主要工作由4部分组成：1）动态场景下的视觉SLAM问题描述；2）本文算法框架介绍；3）室内动态场景下的运动目标检测；4）TUM数据集仿真试验。

1 动态场景下的视觉SLAM问题描述

为实现移动机器人的高精度定位，需有丰富的图像信息作为保证，所以本文采用RGB-D相机传感器，机器人通过RGB-D相机获取环境信息。

RGB-D相机的运动状态示意图如图1所示，机器人在pose1，pose2，…，poseN处观测环境中的路标点y1，y2，…，yn。一般情况下，视觉SLAM问题可以用RGB-D相机的运动模型与观测模型来描述。

在静态场景下，通过迭代方法可有效地解决最小二乘问题；再根据相机的运动方程、观测方程估计相机位姿；最后，借助坐标系转换关系确定机器人的位置与姿态，以实现移动机器人的精准定位[13]。但在动态场景下，由于移动路标与观测值不匹配，数据关联错误，从而导致机器人定位精度低或无法自主定位[14]。为了解决移动机器人在动态环境下定位的问题，本文算法需检测运动物体，并将其区域内的动态特征点滤除。

2 动态场景下的视觉SLAM算法

2.1 本文算法框架

ORB-SLAM2是基于特征点法的实时SLAM开源算法，其三大线程较好地完成了定位与建图任务，并且可以保证其全局一致性[12]。但其在复杂动态场景下易受室内动态物体干扰，造成移动机器人定位精度不足，无法满足自主导航需求[17]。为了解决上述问题，实现移动机器人室内精准定位，本文基于ORB-SLAM2算法架构提出一种新的动态场景下视觉定位算法。首先，通过RGB-D相机获取图像数据传输到跟踪线程，利用Mask R-CNN算法检测RGB图像序列中的运动目标；其次，将Mask R-CNN处理后的图像数据与RGB-D相机获取的深度图像数据传入多视图几何模块，从而检测出残余的运动目标，实现运动目标检测；最后将运动物体区域的动态特征点滤除，保留周围静态特征点用于跟踪定位，从而提高移动机器人在动态场景下的定位精度。其算法框架如图2所示。

2.2 动态场景下运动目标检测

动态物体不但无法提供定位信息，反而会降低定位的成功率，所以需要将相对于移动机器人正在运动的物体进行检测并滤掉，如行人、推车等[2]。但是，Mask R-CNN神经网络可以较好地解决动态物体检测问题，该算法是一种目标实例分割算法，既可以实现像素级的语义分割，还能检测动态物体，比如动物、运动的汽车、自行车以及可能移动的物体[16]。本文利用Mask R-CNN获取像素级语义分割图像，检测运动目标，剔除动态特征点，其操作过程如下：1）下载基于COCO数据集训练成功的权重，并利用Pycharm编辑器构建TensorFlow版本的Mask R-CNN模型。本文的模型将人、椅子、桌子进行像素级的语义分割，并把检测到的人定义为运动目标，桌子、椅子定义为静态目标。2）将Mask R-CNN模型应用于本文算法，并启动运行，从而检测动态场景下的运动目标。

但是，对于不是先验的动态物体，无法被Mask R-CNN算法检测，例如餐厅客人正在移动椅子，虽然利用Mask R-CNN可以检测人，但正在运动的椅子未能检测。但是，多视图几何技术却能很好地解决上述运动目标的检测问题[18]。因此，本文引入多视图几何技术，使其与Mask R-CNN算法相结合，实现动态场景下的目标检测。首先，获取Mask R-CNN处理后的RGB图像帧，将该图像数据与深度图像数据传输到多视图几何模块，进一步检测正在运动的目标，其静态物体与动态物体检测如图3所示。

具体实现过程如下：1）利用三角测量方法计算上一帧的像素点q投影到当前帧像素点q0的深度[V]′，如式（5）所示。2）计算视差角α与重投影误差ΔV，如式（6）、式（7）所示，并分别与其对应的设定阈值β、γ进行比较，若α>β且ΔV >γ，则将像素点q0标记为动态像素点；若不满足α>β、ΔV >γ的条件，则视其为静态像素点，从而完成运动目标的最终检测。3）通过ORB特征点的提取，将其运动目标区域内的动态特征点滤除，保留周围的静态特征点，并采用随机采样一致性算法进行图像特征匹配[19-20]。4）完成本文算法的跟踪与定位，降低动态物体对移动机器人定位的影响。

其中：为[q0]的反对称矩阵；α为向量[qQ]与向量[q0Q0]投影之间的视差角；[?]为向量积运算符号；ΔV为重投影误差；V0为当前帧像素点[q]0的深度。

3 TUM数据集仿真试验

3.1 试验条件

试验选用自带标准轨迹与比较工具EVO的TUM数据集，该数据集包括高动态和低动态2类场景，被广泛用于室内动态环境下的视觉SLAM算法定位准确性和鲁棒性测试。由于移动机器人在室内运动中会遇到较多人，所以本文选择高动态场景fr3_w_xyz序列，该数据集序列中包含彩色图、深度图及相机运动过程中的真实轨迹groundtruth；并且在fr3_w_xyz序列中，相机分别沿着x、y、z轴运动，拍摄2个人围绕桌子大幅度运动。本文试验平台采用Intel core i5-9600KF 3.7 GHz处理器、8 G运行内存、NVIDIA GeForce GTX 1650显卡的PC，其操作系统为Ubuntu18.04。

3.2 仿真结果与分析

为验证本文算法能否检测运动目标，并将其动态特征点滤除，本文在TUM数据集fr3_w_xyz序列上进行试验，分别运行ORB-SLAM2与本文算法，通过终端窗口查看其运行效果，如图4所示。对比图4（a）与图4（b）可知，ORB-SLAM2未能检测到运动中的人，且不能剔除其区域内的动态特征点；本文算法可以很好地检测到运动物体，并将其区域内的动态特征点滤除掉，从而降低动态物体对定位的影响。

为进一步验证本文算法的有效性与鲁棒性，将本文算法、ORB-SLAM2与目前较流行的DynaSLAM[21]这3种算法分别在fr3_w_xyz序列上运行，生成3种算法对应的KeyFrame.txt文件，并利用测评工具EVO的轨迹生成功能，使其与数据集真实值文件groundtruth.txt进行三维轨迹对比（如图5所示）与二维轨迹对比（如图6所示）。观察图5、图6可知，相比ORB-SLAM2，本文算法的估计值更加接近虚线真实值，并与DynaSLAM的估计值相接近，甚至在某段时间表现更优。

与此同时，分别运行3种算法进行15次初始化时间测试，并计算其平均值，结果如图7所示。试验结果表明：相较于ORB-SLAM，本文算法的初始化时间均值相差7.36 s；相比DynaSLAM，时间略短，具有较好的实时性。

本文利用EVO的绝对位姿误差（absolute pose error，APE）功能和相对位姿误差（relative pose error，RPE）功能评估各个算法的定位精度。其中，APE是估计值与真实值之间的差值，可用于评估算法定位精度和位姿的全局一致性；RPE包含相对平移误差和相对旋转误差，反映了两帧间估计位姿变换矩阵与真实位姿变化矩阵之间的差值[14]。

经过计算机处理，最后获得3种算法的绝对位姿误差，如表1所示，表中均采用最大误差（max）、平均误差（mean）、误差中值（median）、最小误差（min）、均方根誤差（RMSE）、方差（sse）与标准差（std）作为算法的评价指标，误差值越小，则算法的估计值越接近真实位姿值，算法鲁棒性越好。

观察表1可知，本文算法的绝对位姿误差评价指标相较于其他2种算法均有所降低。其中，相较ORB-SLAM2的绝对位姿平均误差降低78.70%，均方根误差降低78.30%，标准差降低76.87%；与DynaSLAM相比，本文算法的绝对位姿平均误差降低17.13%，均方根误差降低20.85%，标准差降低30.07%。由此可知，相比其他2种算法，本文算法在高动态场景下的表现更好。

为了更加直观地查看本文算法的定位效果，将Our-SLAM、ORB-SLAM2与DynaSLAM算法的绝对位姿和相对位姿及其位姿误差分别进行比较，结果如图8—图11所示。

通过观察图8、图9的位姿图可知：Our-SLAM估计的位姿与真实位姿（虚线所示）偏差较小，可与DynaSLAM估计的位姿效果相媲美，而ORB-SLAM2估计的位姿与真实位姿偏差较大。由图10、图11的位姿误差图可知：相较另外2种算法，本文算法在高动态场景中的单帧误差波动较少，且绝对位姿误差与相对位姿误差范围较小。

综上所述，本文算法相较ORB-SLAM2，可以将室内动态场景下的动态特征点滤除掉，降低动态物体对定位的影响，从而使本文算法的估计位姿更接近真实位姿；并且相比ORB-SLAM2与DynaSLAM，本文算法的定位精度也有明显提升。通过结果对比，最终验证了本文算法能够较好地解决室内动态场景下的定位问题，且在室内动态场景下具有良好的定位精度与鲁棒性，可为室内移动机器人的自主定位提供参考。

4 结论

本文提出了一种在室内动态场景下基于RGB-D相机的定位算法，该算法基于ORB-SLAM2算法架构，在其跟踪线程中引用Mask R-CNN神经网络与多视图几何技术，实现了动态场景下的运动目标检测，剔除了运动目标的动态特征点，从而降低了动态物体对移动机器人定位的影响；最终利用静态物体的特征点，完成了相机位姿的跟踪。本文使用公开的TUM数据集的高动态场景fr3_w_xyz序列，對比了本文算法与ORB-SLAM2算法的运行效果，并对比了Our-SLAM、ORB-SLAM2和DynaSLAM 这3种算法的三维轨迹和二维轨迹及其位姿误差。试验结果表明：本文算法在室内高动态场景下可实现动态特征点滤除，并比另外2种主流算法的估计位姿更接近真实位姿，绝对位姿误差与相对位姿误差范围较小。其中，相较ORB-SLAM2，本文算法的绝对位姿平均误差降低了78.70%，均方根误差降低了78.30%，标准差降低了76.87%；与DynaSLAM相比，本文算法的绝对位姿平均误差降低了17.13%，均方根误差降低了20.85%，标准差降低了30.07%，从而验证了本文算法在室内动态场景下具有较好的位姿估计精度和鲁棒性，可为室内动态场景下的移动机器人自主定位提供参考。

目前该算法仅适用于室内动态场景下的移动机器人定位，在室外场景下运行效果较差，其定位精度较低。未来可考虑引用激光雷达、惯性测量单元等传感器，实现多传感器融合，使其优势互补，以适应复杂多样的情况，从而更好地完成机器人的自主导航。

参考文献

[1] 田野，陈宏巍，王法胜，等. 室内移动机器人的SLAM算法综述[J]. 计算机科学，2021，48（9）：223-234.

[2] 黄山，黄洪钟，曾奇，等. 一种室内高动态环境的机器人定位方法[J]. 电子科技大学学报，2021，50（3）：382-390.

[3] KUUTTI S，FALLAH S，KATSAROS K，et al. A survey of the state-of-the-art localization techniques and their potentials for autonomous vehicle applications[J]. IEEE Internet of Things Journal，2018，5（2）：829-846.

[4] DELLAERT F，FOX D，BURGARD W，et al. Monte Carlo localization for mobile robots[C]//Proceedings 1999 IEEE International Conference on Robotics and Automation，IEEE，1999，2：1322-1328.

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

[6] 季宇寒，李寒，张漫，等. 基于激光雷达的巡检机器人导航系统研究[J]. 农业机械学报，2018，49（2）：14-21.

[7] 李晨阳，彭程，张振乾，等. 融合里程计信息的农业机器人定位与地图构建方法[J]. 农业工程学报，2021，37（21）：16-23.

[8] JIN Q Y，LIU Y G，MAN Y C，et al. Visual SLAM with RGB-D cameras[C]//2019 Chinese Control Conference（CCC），IEEE，2019：4072-4077.

[9] ENGEL J，SCH?EPS T，CREMERS D. LSD-SLAM：large-scale direct monocular SLAM[C]//13th European Conference on Computer Vision，2014：834-849.

[10] WANG R，SCHWOERER M，CREMERS D. Stereo DSO：large-scale direct sparse visual odometry with stereo cameras[C]//Proceedings of the IEEE International Conference on Computer Vision，2017：3903-3911.

[11] KLEIN G，MURRAY D. Parallel tracking and mapping for small AR workspaces[C]//2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality，IEEE，2007：225-234.

[12] MUR-ARTAL R，TARD?S J D. ORB-SLAM2：an open-source SLAM system for monocular，stereo，and RGB-D cameras[J]. IEEE Transactions on Robotics，2017，33（5）：1255-1262.

[13] 高翔，张涛，刘毅，等. 视觉SLAM十四讲：从理论到实践[M]. 北京：电子工业出版社，2017.

[14] 张恒，徐长春，刘艳丽，等. 基于语义分割动态特征点剔除的SLAM算法[J]. 计算机应用研究，2022，39（5）：1472-1477.

[15] 夏田，张大为，代斌斌. 全向移动的送餐机器人控制系统研究与仿真[J]. 制造业自动化，2022，44（5）：17-20.

[16] 高兴波，史旭华，葛群峰，等. 面向动态物体场景的视觉SLAM综述[J]. 机器人，2021，43（6）：733-750.

[17] ZHANG L，ZHANG Y J. Improved feature point extraction method of ORB-SLAM2 dense map[J]. Assembly Automation，2022，42（4）：552-566.

[18] ZHANG Z Q，ZHANG J T，TANG Q R. Mask R-CNN based semantic RGB-D SLAM for dynamic scenes[C]//2019 IEEE/ASME International Conference on Advanced Intelligent Mechatronics，IEEE，2019：1151-1156.

[19] 譚光兴，张伦. 基于改进SIFT的图像特征匹配算法[J]. 广西科技大学学报，2022，33（2）：41-47.

[20] 苏宇，刘海燕，李国勇. 一种结合随机采样一致性与主成分分析的点云配准方法[J]. 广西科技大学学报，2022，33（4）：70-77.

[21] BESCOS B，F?CIL J M，CIVERA J，et al. DynaSLAM：tracking，mapping，and inpainting in dynamic scenes[J]. IEEE Robotics and Automation Letters，2018，3（4）：4076-4083.

Research on mobile robot positioning algorithm based on RGB-D camera in dynamic scene

CONG Peichao， CUI Liying， WAN Xianquan， LIU Junjie， XIAO Yixuan， LI Jiaxing

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

Abstract： To solve the problem of low positioning accuracy of mobile robot in the process of visual navigation in indoor dynamic scene， a new visual positioning algorithm under dynamic scene is proposed. The algorithm is based on the ORB-SLAM2 algorithm framework. Firstly， the image data is obtained by the RGB-D camera， and then the Mask R-CNN neural network and multi-view geometry technology are used for data processing to realize the moving target detection. Secondly， the dynamic feature points in the moving object area are filtered out and the static feature points are retained to complete the pose tracking of the camera， so as to reduce the effect of the dynamic object on the robot positioning. Finally， the fr3_w_xyz sequence under TUM data set is used to complete the test verification. The test results show that the trajectory of this our-SLAM algorithm is better than that of ORB-SLAM2 and DynaSLAM algorithms under high dynamic scenarios， and the mean error of absolute pose is reduced by 78.70% and 17.13%， respectively. The standard deviation decreases by 76.87% and 30.07%， respectively. It is verified that this algorithm can achieve accurate positioning in indoor dynamic scenes， and has good robustness and accuracy， which can provide a theoretical basis for mobile robots indoor autonomous navigation.

Key words： mobile robot; dynamic scenario; Mask R-CNN; multi-view geometry; precise positioning

（责任编辑：黎 娅）

收稿日期：2022-12-05

基金项目：中央引导地方科技发展专项资金项目（桂科ZY19183003）；广西重点研发计划项目（桂科AB20058001）资助

第一作者：丛佩超，博士，副教授，研究方向：移动机器人自主定位与导航，E-mail：cpclzx@163.com