劉璐 高振航 張燦 胡飄

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

關鍵詞：SLAM；巡檢機器人；數(shù)據融合

中圖分類號：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? 引? 言

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

1? 總體設計

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

2? 硬件設計

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

2.1? 主控制器模塊

主控制器采用NVIDIA的Jetson Nano開發(fā)套件，總體尺寸為69 mm×45 mm，采用4核Quad-core ARM A57微處理器的CPU，采用NVIDIA研發(fā)的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是業(yè)內先進的AI計算平臺，支持多種深度學習框架，如TensorFlow、PyTorch、Caffe、Keras、MXNet等，可實現(xiàn)圖像分類、目標檢測、語義分割和智能分析等功能，可用于構建自主機器人和復雜人工智能系統(tǒng)。同時具有低功耗和并行處理快速的優(yōu)勢，可以并行處理多個傳感器并在每個傳感器上運行多個現(xiàn)代神經網絡。Jetson Nano搭載Ubuntu 18.04系統(tǒng)，支持Melodic版本的ROS系統(tǒng)，可實現(xiàn)井下巡檢機器人的同步定位與建圖功能和自主導航功能。

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

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波控制左右車輪的轉速。同時結合光電編碼器返回的信息，計算出電機的實時轉速得到機器人的實時運動狀態(tài)信息，并將得到的信息和IMU返回的加速度信息上傳到主控制器。

2.3? 感知模塊

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

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

2.4? 電機驅動模塊

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

2.5? 電源模塊

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

2.6? 通信模塊

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

3? 軟件設計

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

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

3.1? 通信子程序設計

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

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

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

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

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

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

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

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

采用L-M法求解非線性優(yōu)化問題。

3.3? 運動控制子程序設計

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

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

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

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

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

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

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

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

3.4? 路徑規(guī)劃子程序設計

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

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

4? 實驗結果

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

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

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

5? 結? 論

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

參考文獻：

[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] 張保龍，周觀民，黃海燕，等.礦用巡檢機器人及其控制管理系統(tǒng)的設計研究 [J].煤炭技術，2021，40（9）：165-167.

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

收稿日期：2022-10-11