基于SLAM的井下巡檢機器人的研究

劉璐 高振航 張燦 胡飄

摘? 要：文章設計了基于多傳感器融合SLAM的井下巡檢機器人。作為SLAM的感知設備，相機、激光雷達和慣性傳感器的性能和可靠性在不同的運行速度和環境下相差較大，針對這一特點，結合激光和視覺SLAM子系統以及子系統的誤差項對機器人的位姿進行非線性優化，提高了機器人在光線昏暗、地形復雜的井下環境中運行的定位準確性和穩定性。

關鍵詞：SLAM；巡檢機器人；數據融合

中圖分類號：TP242? 文獻標識碼：A? 文章編號：2096-4706（2023）05-0060-05

Research on Underground Inspection Robot Based on SLAM

LIU Lu， GAO Zhenhang， ZHANG Can， HU Piao

（China University of Mining and Technology-Beijing， Beijing? 100083， China）

Abstract： This paper designs an underground inspection robot based on multi-sensor fusion SLAM. As the sensing equipment of SLAM， the performance and reliability of cameras， laser radars and inertial sensors vary greatly under different operating speeds and environments. Aiming at this feature， combining the laser and vision SLAM subsystems and error items of subsystems， the robot's position and attitude are optimized nonlinearly to improve the positioning accuracy and stability of the robot's operation in the dark and complex underground environment.

Keywords： SLAM; inspection robot; data fusion

0? 引? 言

現代化礦井的生產設備日益向大型、連續、高速和自動化的方向發展，基于天然因素和人為因素的各種故障影響著生產效率和人員安全，給企業和工人造成損失。因此需要對礦井生產設備和系統進行定期、定點的檢查，對照標準查驗設備的異常現象和隱患及時提出維護方案[1]。煤炭生產過程常采用需要工人直接或間接參與的人工巡檢方式進行故障排除，由于井下通信距離長、通信信號易受干擾、照明不足等環境原因導致井下人工作業存在安全隱患。近年來煤炭行業正進一步實現生產過程智能化、無人化和通信設備網絡化，并在關鍵工作區和相對危險的工作崗位采用機器人代替工人進行設備和儀器的操作和檢查以提高煤炭井下作業的工作效率和安全性[2]。設計一款在易發生特大事故的重要場所可通過遠程操控或自主移動的方式進入危險區域進行巡檢和測繪工作的井下巡檢機器人，對提高煤炭生產的效率和保障煤炭工人的安全有重要意義。

1? 總體設計

井下巡檢機器人由感知模塊、主控制器模塊、運動控制模塊和通信模塊組成。數據采集模塊將攝像頭采集的圖像信息、激光雷達返回的距離數據、IMU陀螺儀采集的加速度信息發送到主控制器模塊，主控制器模塊將采集到的數據進行數據融合和計算得到機器人的位姿信息，構建地圖并進行路徑規劃，主控制器模塊通過通信模塊向運動控制模塊發送指令和接收運動信息并將機器人的位姿信息和地圖信息反饋到上位機，運動控制模塊通過控制電機轉速控制機器人的移動并返回運動狀態信息。井下巡檢機器人的總體設計如圖1所示。

2? 硬件設計

硬件部分包括主控制器模塊、運動控制模塊、感知模塊、電源模塊、速度檢測模塊和通信模塊。主控制器模塊根據感知模塊采集到的環境信息和速度檢測模塊返回的運動狀態信息通過基于多傳感器融合的SLAM算法進行定位、建圖和路徑規劃并通過通信模塊向運動控制模塊發送指令控制機器人的運動。

2.1? 主控制器模塊

主控制器采用NVIDIA的Jetson Nano開發套件，總體尺寸為69 mm×45 mm，采用4核Quad-core ARM A57微處理器的CPU，采用NVIDIA研發的128-core Maxwell作為GPU，使用4 GB 64-bit LPDDR4 25.6 GB/s的內存。板載2路CSI攝像頭接口、4個USB 3.0接口和一個Micro USB接口，提供外接GPIO口、兩個I2C接口、兩個UART接口和兩個SPI接口。Jetson Nano是業內先進的AI計算平臺，支持多種深度學習框架，如TensorFlow、PyTorch、Caffe、Keras、MXNet等，可實現圖像分類、目標檢測、語義分割和智能分析等功能，可用于構建自主機器人和復雜人工智能系統。同時具有低功耗和并行處理快速的優勢，可以并行處理多個傳感器并在每個傳感器上運行多個現代神經網絡。Jetson Nano搭載Ubuntu 18.04系統，支持Melodic版本的ROS系統，可實現井下巡檢機器人的同步定位與建圖功能和自主導航功能。

采用電源模塊輸出的5 V電壓為主控制器Jetson Nano供電，通過串口通信模塊和運動模塊相連實現運動指令的下發和里程計信息的獲取，通過USB接口連接感知模塊的攝像頭和激光雷達實時獲取環境信息。

2.2? 運動控制模塊

運動控制模塊基于STM32F407VET6微控制器設計，STM32F407VET6是基于32位ARM Contex M4的微控制器，工作頻率可達168 MHz，采用RISC精簡指令集、LQFP表面貼裝封裝，具有14個定時器、2個UART接口、3個I2C接口、3個SPI通道和3路16位AD轉換器。

運動控制模塊的STM32F407VET6微處理器的主要功能為接收來自主控制器的運動指令，通過運動學公式根據運動指令計算左右車輪的轉速，并通過PID控制輸出到電機驅動模塊的PWM波控制左右車輪的轉速。同時結合光電編碼器返回的信息，計算出電機的實時轉速得到機器人的實時運動狀態信息，并將得到的信息和IMU返回的加速度信息上傳到主控制器。

2.3? 感知模塊

感知模塊由雙目攝像頭、2D激光雷達和IMU慣性測量裝置組成。采用微雪的IMX219雙目攝像頭作為視覺傳感器實時獲取環境中的圖像信息。IMX219雙目攝像頭的分辨率為800萬像素、視場角為83°、支持紅外夜視功能，采用雙CSI排線作為攝像頭接口。雙目攝像頭成本較低且易于獲取環境中的深度信息，同時IMX219的紅外夜視功能可以在昏暗環境下更好地獲取環境信息。激光雷達采用思嵐科技的RPLIDAR-A1型雷達，通過USB轉串口模塊與主控制器相連，RPLIDAR-A1型雷達的正常工作功率為3 W，測距范圍為12 m，工作頻率為5.5 Hz，測量頻率為8 000次/秒。利用高精度激光雷達可以準確測量機器人周邊環境的距離信息，有效提高煤炭井下機器人建圖的準確性。IMU慣性測量裝置采用內置輸出16位數字量的三軸加速度計、三軸陀螺儀和三軸磁力計的MPU9250，通過I2C通信向單片機發送機器人的運動和姿態信息。

目前基于視覺傳感器的SLAM框架已經較為成熟，但由于煤炭井下光照不足、空氣中有較多粉塵和水霧的特殊環境，基于視覺的SLAM方法會產生一定誤差，因此加入了可對周邊環境的障礙物進行測距的激光雷達和實時提供機器人加速度信息的IMU慣性測量裝置，用于數據融合，增加井下巡檢機器人定位與路徑規劃的精度和魯棒性。

2.4? 電機驅動模塊

電機驅動模塊選用東芝半導體公司生產的TB6612電機驅動元件，TB6612電機驅動模塊的工作溫度為-20 ℃～85 ℃，具有兩路最高頻率為100 kHz、最高電流為1.2 A直流電流的PWM波輸出，可同時驅動兩路直流電機實現正轉、反轉和急停功能。相比于常用的L298N電機驅動模塊散熱性能更好，進行電路設計時不需設計額外的散熱元件。電機選擇帶有高精度編碼器的12 V直流減速電機。直流減速電機通過在直流電機外加裝一套齒輪通過齒輪線速度的變化，實現在低轉速時為車輪提供較大力矩。高精度光電編碼器通過光電裝置實時測量車輪的實時轉速數據，通過運動學公式計算得到機器人的里程計信息。

2.5? 電源模塊

采用12 V鋰電池供電，其中直流減速電機由12 V電源電壓直接供電，電源電壓經過降壓得到5 V和3.3 V電壓為各個模塊供電。將12 V電源電壓通過RT8289模塊降壓得到5 V電壓，再將5 V電壓通過CP2102模塊進一步降壓得到3.3 V電壓。

2.6? 通信模塊

主控制器模塊和運動控制模塊通過串口通信實現數據傳輸。運功控制模塊通過CP2102 USB轉串口模塊實現單片機的UART接口和Jetson Nano USB接口的連接，CP2102模塊使用5 V電壓供電，支持300 bit/s -1 Mbit/s波特率的數據傳輸。主控制器模塊采用intel 8265NGW雙頻網卡，通過Wi-Fi通信和上位機連接滿足傳輸視頻流的需求。

3? 軟件設計

軟件設計基于ROS開發平臺和Keil5開發平臺，采用C++和Python語言進行程序編寫，根據功能主要分為通信子程序、SLAM子程序、運動控制子程序和路徑規劃子程序。

用戶通過通信子程序遠程控制機器人的運動和查看周邊環境的信息。SLAM子程序利用激光雷達和視覺傳感器返回的數據，通過提取運動環境的特征同時結合里程計信息得到機器人的位置信息和周邊環境信息并生成地圖。運動控制子程序進行機器人的運功控制，根據IMU傳感器和光電編碼器的信息通過卡爾曼濾波進行數據融合，計算機器人的位姿信息，并通過SLAM子程序進行建圖。路徑規劃子程序根據地圖結合里程計信息和傳感器信息，進行路徑規劃并向運動控制模塊發送運動指令，從而在移動過程中通過傳感器信息進行移動路徑的優化和局部避障。

3.1? 通信子程序設計

單片機通過通信子程序接收主控模塊發送的前進、后退、變速、轉向等指令并控制機器人的運動。單片機和主控模塊通過波特率為115 200串口實現通信。單片機以20 Hz的頻率向主控模塊發送數據幀，單片機發送的數據幀如表1所示，每個數據幀由幀頭、使能標志位、XYZ三軸速度、IMU三軸加速度、三軸角速度、電源電壓、數據校驗位和幀尾組成。

單片機采用中斷接收的方式接收主控制器發送的數據。單片機接收的數據幀由幀頭、使能標志位、機器人XYZ三軸目標速度數據校驗位和幀尾組成。

3.2? 同步定位與建圖子程序設計

近年來以ORB-SLAM框架為代表的傳統視覺SLAM技術已經較為成熟，但煤炭井下光線暗、多粉塵的復雜環境限制了視覺傳感器在井下環境的應用。因此煤炭井下等不利于視覺傳感器的特殊環境多采用基于激光雷達的SLAM技術[3]，雖然激光SLAM的效果優于傳統的視覺SLAM，但低價格的激光雷達存在激光點不夠密集、噪聲大、閉環檢測效果不好的問題，而視覺SLAM由于圖像包含的豐富信息在閉環檢測方面存在天然的優勢。因此采用激光結合視覺的方式提高定位精度是井下移動機器人的研究熱點[4]。

井下巡檢機器人采用一種激光和視覺相結合的定位與建圖方法，通過融合激光點云數據和圖像特征點數據基于稀疏姿態調整對機器人位姿進行優化的同時，采用基于視覺的詞袋模型進行閉環檢測并生成基于3D點云的柵格地圖。加入視覺信息的激光視覺聯合優化的SLAM整體框架，如圖2所示。

常用的SLAM數學模型有基于濾波的SLAM算法和基于圖優化的SLAM算法，圖優化通過構造節點和構造邊表示一系列位姿變量的關系，通過非線性優化求出最優變量得到準確的位姿估計。

將視覺相鄰幀誤差函數公式和激光誤差函數公式改寫成李代數形式并合并，整體誤差函數為：

將機器人位姿作為唯一變量并進行優化，視覺觀測量和激光點云數據作為位姿之間的約束，誤差函數為：

采用L-M法求解非線性優化問題。

3.3? 運動控制子程序設計

單片機接收指令后按照由運動控制子程序根據運動學公式進行計算得到左右輪的轉速，并根據光電編碼器返回的左右輪實時轉速信息通過輸出PWM脈沖控制電機實現對車輪轉速的PID控制從而實現控制機器人按照指定速度運動，車輪轉速的控制流程圖如圖3所示。

車輪轉速的PID控制算法采用增量式離散PID公式，根據單位時間內光電編碼器獲取的脈沖數得到車輪的速度并與目標值進行比較得到偏差，通過控制偏差的比例、積分和微分使偏差趨向于零。

增量式離散PID控制的計算公式為：

Pwm+=Kp[e（k）-e（k-1）]+Kie（k）+Kd[e（k）-2e（k-1）+（k-2）]

式中Pwm為增量輸出的結果，e（k）為第k次車輪轉速與目標轉速的偏差，Kp為比例項參數，Ki為積分項參數，Kd為微分項參數。井下巡檢機器人的控制閉環系統采用PI控制，將增量離散PID公式簡化為：

Pwm+=Kp[e（k）-e（k-1）]+Kie（k）

采用串級PID進行機器人的位置控制，位置控制的流程圖如圖4所示，將外環位置環的輸出作為速度環的輸入，并由內環速度控制器的輸出控制作為執行器的電機轉動，從而實現在控制車輪轉速的同時使機器人到達目標位置。

差速履帶式機器人在經過某些復雜地形時會出現空轉或打滑的現象，IMU慣性傳感器在長時間使用時會出現零點漂移的情況，采用單一方法得到的里程計信息會存在較大的誤差[5]。為了得到更準確的里程計信息，單片機通過擴展卡爾曼濾波將IMU傳感器和編碼器的信息進行數據融合，并將得到的里程計信息發送到主控制器。

3.4? 路徑規劃子程序設計

井下巡檢機器人采用基于A*算法改進的D*算法進行路徑規劃和自主導航。D*算法是一種啟發式的路徑搜索算法，適合于井下動態未知環境和動態障礙物的場景。D*算法首先通過dijstra或A*算法從目標點向初始點進行反向搜索，在機器人向目標點移動的過程中遇到障礙物時對路線進行局部更改，算法流程如圖5所示。

設目標點為點G，Y為當前節點，X為Y的下一個節點，c（X， Y）為X到Y的路徑，open集合為待訪問的節點集合，close集合為已經訪問的節點和待訪問的節點，h函數為當前點到目標點的距離，k函數為當前點到G點的最小代價。當機器人到目標點的最短路徑上的節點發生變化時，D*算法通過傳播狀態和搜索臨接點的方法尋找地圖發生改變后的最優路徑。

4? 實驗結果

在實驗室環境下進行了樣機搭建和測試，樣機運行圖如圖6所示。將樣機和上位機連接在同個網絡下后進行ROS的主從機配置和通信。

進行系統初始化，給雷達連接的串口訪問權限后通過launch配置文件啟動建圖節點。如圖7所示。

控制樣機在環形實驗空間以0.2 m/s的速度運動兩圈，通過rviz可視化工具得到環境地圖，如圖8所示。

5? 結? 論

基于多傳感器融合的巡檢機器人在實驗室環境下相對于單一傳感器有更好的穩定性，且不受環境光的影響，缺點是由于并行運行多個系統導致運行幀率下降，以及激光雷達在收鏡面反射干擾產生誤差，今后的研究可從關聯視覺信息和激光信息的方向出發，在降低誤差的同時盡可能提高巡檢機器人的運動速度。

參考文獻：

[1] 黃騫，劉帥，任偉，等.基于雙通信模式的綜采工作面巡檢機器人研制 [J].煤炭科學技術，2021，49（S1）：167-169.

[2] 張愛民，翟德華，李棟慶.煤礦綜采工作面牽引式巡檢機器人的設計 [J].自動化應用，2021（7）：133-135.

[3] HOUBEN S，DROESCHEL D，BEHNKE S. Joint 3D Laser and Visual Fiducial Marker Based SLAM for a Micro Aerial Vehicle [C]//2016 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems（MFI）.Baden：IEEE，2016：609-614.

[4] Shi J Q，He B w，Zhang L w，et al. Vision-Based Real-Time 3D Mapping for UAV with Laser Sensor [C]//2016 IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS）.Daejeon：IEEE，2016：4524-4529.

[5] 張保龍，周觀民，黃海燕，等.礦用巡檢機器人及其控制管理系統的設計研究 [J].煤炭技術，2021，40（9）：165-167.

作者簡介：劉璐（1997—），女，漢族，山東濟寧人，碩士研究生在讀，研究方向：視覺SLAM；高振航（1998—），男，漢族，河南開封人，碩士研究生在讀，研究方向：嵌入式技術、自動控制；張燦（1998—），女，漢族，河北鹽山人，碩士研究生在讀，研究方向：機器人；胡飄（1998—），男，漢族，四川遂寧人，碩士研究生在讀，研究方向：嵌入式技術。

收稿日期：2022-10-11