基于優化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

（責任編輯：羅小芬）