動態場景下基于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]?；谙鄼C的定位方法不僅能夠獲得豐富的位置信息，而且其硬件成本低廉。目前，基于相機傳感器的定位方法主要為視覺同時定位與建圖（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