基于SLAM 導航的煤礦井下機器人設計

趙文金 朱子恒 吳云雁

（山西潞安環保能源開發股份有限公司常村煤礦，山西 長治 046100）

隨著煤炭行業對無人化、智能化需求日益增加，采用機器人參與到井下探測、開采、運輸、救援等工作已經成為主要發展趨勢。對于井下機器人來說，自主導航能力成為判定機器人智能化的關鍵因素。國家煤礦安全監察局在2019 年發布的《煤礦機器人重點研發目錄》中指出，對于掘進類、采煤類、運輸類、安控類、救援類五大類煤礦機器人，自主導航功能是首要解決的問題[1-2]。

自主導航功能包括了定位、建圖、路徑規劃、避障等方面，由于井下環境復雜多變，巷道狹長、轉角多，且存在復雜信號干擾，因此GPS 導航無法應用于井下環境。對于井下弱GPS 信號的場景，有學者提出利用UWB 室內定位技術實現建圖和導航。UWB（超寬帶，Ultra Wide Band）是一種無線載波通信技術，利用納秒級的非正弦波窄脈沖傳輸數據，發射信號功率譜密度低，對信號衰落不敏感，因此其定位精度高，非常適合室內定位。然而，實現UWB 定位需要在若干位置安裝相應的基站，目前基站價格昂貴，對于井下環境需要投入成本較高，且UWB 定位方式不能滿足所有功能情景，例如完成救援等。許多學者提出采用視覺方式實現環境識別和定位，但由于井下光線較暗、粉塵濃度高，因此無法獲取清晰的圖像。為解決室內機器人自主導航的問題，SLAM 算法逐漸進入人們視野[3-4]。SLAM（Simultaneous Localization and Mapping）由Randall C.Smith 和Peter Cheeseman 在1986 年 首次提出，其實現自主導航的方式是以當前位置為起始，在機器人移動過程中根據攜帶傳感器數據（包括激光雷達、里程計等）和地圖路標進行自身定位，同時構建增量式地圖，完成對未知環境的建圖和導航[5-7]。1999 年的機器人國際研討會（International Foundation of Robotics Research，ISRR） 是SLAM發展史上的一個重要節點，這是第一次針對SLAM問題的討論，主要是討論了基于卡爾曼濾波的方法與基于概率學方法下SLAM 的收斂問題。

到目前為止，SLAM 根據搭載傳感器的不同主要分為激光SLAM 與視覺SLAM。激光SLAM 發展時間長，數據處理相對簡單，是目前比較成熟、比較穩定、比較主流的定位導航方法；視覺SLAM還處于發展階段，且對光的依賴程度高，但其適應的場景多，有很大的發展空間。根據優化方法的不同，SLAM 還可分為基于粒子濾波的方法和基于圖優化方法的SLAM。

國內在SLAM 上的研究也取得了較大進展。河海大學的王秉洲等[8]為了解決Fast-SLAM 算法粒子退化的問題，應用自適應漸消的無跡卡爾曼濾波，一方面減小了擴展卡爾曼濾波線性化時的誤差，另一方面減小了粒子的退化；云南師范大學的羅景文等[9]在箱粒子濾波算法中引入螢火蟲算法的尋優機制進行位姿估計，減小了構建地圖所需的粒子數量，提高了Fast-SLAM 的構圖和定位精度；上海大學的任明宇等[10]將激光雷達采集的點云數據與相機采集的三維點云數據通過貝葉斯方法對地圖進行融合與更新，從而構建出更精確的地圖。

雖然SLAM 定位范圍相對GPS 要小很多，但擺脫了對衛星的依賴，非常適合于井下弱GPS 信號的場景，同時，只需要在機器人上搭載雷達等傳感器，不需要類似UWB 技術一樣在井下安裝大量基站。因此，對于井下機器人的自主導航設計來說，SLAM 算法是最有效和最經濟的手段。

該文將設計基于SLAM 導航的井下機器人，搭載Jetson Nano 主機運算導航算法，實現建圖、定位、路徑規劃等，同時開發單片機以實現對車輪電機的控制，Jetson Nano 主機實時向單片機發布車輪控制指令，單片機實時反饋里程計數據等。將井下機器人在實驗室進行測試，利用ROS 系統提供的rviz 三維可視化工具完成井下機器人位置及數據流監控，并對算法進行修正與參數校準，完善井下機器人的自主導航功能。

1 總體方案

自主導航功能主要分成兩個部分：建圖和導航。如圖1 所示。

圖1 自主導航功能結構圖

SLAM 是自主導航的核心算法，機器人頂部搭載激光雷達實時掃描平面360°，獲取周圍環境數據，依賴這些數據構建增量式地圖。通常建圖采用的傳感器主要包括激光雷達和深度相機，考慮井下環境較為復雜和多變，粉塵較多，光線較暗，因此采用激光雷達作為檢測環境數據的主要傳感器。導航的實現依賴路徑規劃算法，該算法接收實時更新的地圖數據，完成基于地圖的定位，同時接收來自單片機上里程計數據完成基于里程計的定位，將這兩個定位信息結合完成局部路徑規劃和全局路徑規劃。全局路徑規劃用于指導機器人走向目標點的路徑，局部路徑規劃用于掃描當前路徑上的障礙物，實現避障。經過路徑規劃后，產生速度控制指令，Jetson Nano 主機將更新的速度控制指令發送給單片機，單片機根據電機控制算法實現車輪電機的速度與轉角控制，最終完成機器人導航功能。

因此，該文針對運輸類機器人給出設計方案，為了實現建圖及導航功能，需要提供實時掃描的地圖數據、里程計數據和目標點信息數據，SLAM 算法根據這些數據計算出速度控制指令，并發送給控制器。

2 硬件方案

2.1 硬件分析

機器人的硬件主要考慮三個方面：控制器類、傳感器類、底盤類。

1）控制器類

控制器主要完成兩部分功能：導航算法實現和運動控制。

采 用Jetson Nano 作 為 主 機，Jetson Nano 是GPU 運算平臺，以Jetson Nano 為系統的總控制器，主要用于SLAM 算法及路徑規劃算法的計算。具體型號采用Jetson Nano B01，該款主機是高性能嵌入式計算平臺，采用了NVIDIA Maxwell GPU 和四核ARM Cortex-A57 CPU 的組合，擁有強大的計算能力和低功耗特性，具有128 個CUDA 核心，支持硬件加速和視頻編解碼和圖形處理，主頻高達1.43 GHz，支持多線程并發處理。由于需要與單片機進行通信，需要充足的接口，Jetson Nano B01 包含多個USB 接口、HDMI 接口、以太網接口、WiFi、藍牙等通信接口，以及GPIO、SPI、I2C 等嵌入式接口，方便與單片機等多種外設進行通信。

以半導體的STM32F103ZET6 芯片為單片機核心，并搭載外圍電路，主要用于實現車輪電機的運動控制，包括轉角和速度的調節。STM32F103ZET6是半導體公司生產的32 位具有低功耗、高性能的處理器，具有ARM Corex-M3RISC 內核，工作頻率可達72 MHz，高速嵌入式存儲器，包括閃存512 KB，SRAM64 KB，提供了3 個12 位ADC 通道、4 個通用16 位定時器，還有多種標準通信接口，包括2 個I2C、3 個SPI、1 個SDIO、5 個USART、1個USB 和1 個CAN 通道。

2）傳感器類

SLAM 算建圖主要依賴于環境數據和里程計數據，根據井下環境情況，采用360°激光雷達實時獲取環境數據。這里采用單線激光雷達RPLIDAR A1M8。該激光雷達采用三角測距原理，測距范圍為0.15～12 m，誤差為±1%，掃描角度范圍是0～360°，角度分辨率＜0.5°。

里程計數據主要包括航向角、角速度、加速度等，依賴于慣導采集，并利用單片機計算得到。采用MPU6050 作為IMU 傳感器，將獲取的加速度、角速度等數據解算得到歐拉角數據，為導航提供里程計數據。MPU6050 整合了3 軸陀螺儀和3 軸角速度傳感器，解決了陀螺儀與加速度傳感器軸間差的問題，同時自帶數字運動處理器，使用InvenSense 公司提供的運動處理庫實現姿態解算，實現硬件加速。

速度和方向數據主要依賴編碼器傳輸給單片機。這里采用高精度巨磁阻編碼器，該編碼器集成了光電編碼器的高精度特點，相較于霍爾編碼器提高了數倍，在低速時表現出優異的穩定性。

3）底盤類

底盤采用阿克曼類型底盤，其控制方式主要采用舵機實現前輪轉向功能、利用雙電機完成后輪差速驅動功能。選用額定扭矩為13.5 kg·cm、額定電流為2.3 A、額定轉速為230 r/min、減速比為27:1型后輪驅動直流無刷電動機。電動機與其配套的底盤電機輪組模塊帶有滑動軸承，電動機不直接驅動車輪，而是負責傳遞動力。

2.2 硬件方案設計

根據硬件方案，設計如圖2 所示的硬件連接方式。

圖2 硬件連接圖

各部分主要功能包括：

1）導航算法功能

Jetson Nano 通過USB 接口連接激光雷達，獲取激光雷達掃描的地圖數據，用于建圖功能中地圖的建立與導航功能中獲取實時地圖用于定位；

Jetson Nano 通 過USART 與STM32 連 接，獲取STM32 讀取到的里程計數據實現定位功能，在路徑規劃時將計算得到的智能車速度控制指令傳給STM32，實時控制機器人的移動，實現導航與避障功能；

Jetson Nano 作為主機，將虛擬機的Ubuntu 作為從機，通過局域網與虛擬機的Ubuntu 系統建立通信，通過虛擬機的ROS 系統提供的節點與話題工具實現對各節點與話題的監控，通過虛擬機ROS系統提供的rviz 三維可視化工具實現對智能車在二維柵格地圖中的位置的監控，并可以通過rviz 發布目標點話題。

2）運動控制功能

STM32 通過調節控制舵機PWM 信號的占空比，實現對后驅電動機的控制，舵機帶動前輪轉動實現轉向功能；

STM32 連接驅動器，驅動器連接后驅電動機，STM32 通過調節驅動器控制電路PWM 信號的占空比，實現對驅動器驅動電路驅動電機電流大小的控制，進而實現STM32 對電機轉速的控制；

STM32 通過調節驅動器控制電路輸入邏輯電平的高低，實現對后驅電動機電流方向及有無的控制，進而實現STM32 對電機轉向的控制；

STM32 通過計數編碼器輸入的A、B 相脈沖數并獲取A、B 相相位差，通過計算得到實時速度數據與速度方向；

STM32 通 過I2C 接 口 與IMU 通 信，實 現 對IMU 的配置與讀取IMU 內各寄存器值，通過一定的計算得到機器人實時航向角、角速度、加速度等數據；

STM32 通過USART 接口與Jetson Nano 通信，實現STM32 與Jetson Nano 之間的數據交換，用來向Jetson Nano 傳輸各傳感器數據、機器人狀態數據并接收Jetson Nano 傳入的控制指令。

3 軟件方案

3.1 軟件分析

軟件功能主要包括SLAM 算法、路徑規劃算法、里程計數據結算、運動控制算法、主機與單片機之間的通信等功能。

3.2 軟件方案設計

根據上述主要軟件功能介紹，設計如圖3 所示的軟件方案。

圖3 軟件結構示意圖

軟件功能上主要分成兩個部分：

1）導航功能

主要在Jetson Nano 上實現。Jetson Nano 安裝Ubuntu 系統，并安裝ROS 系統，Jetson Nano 上所有的功能包都是在ROS 環境下開發。

主要的功能包括：

① 通信功能包

主要完成STM32 與Jetson Nano 的串口通信，自定義編碼協議，將需要傳遞的數據轉換為16 進制發送/解讀。在對波特率、通信端口等通信基本參數、里程計、IMU 坐標系和話題名進行配置后，啟動通信文件，通過訂閱速度話題、里程計話題、tf 坐標變化話題等，讀取導航所需數據，通過SLAM 算法得到的控制指令再發送給單片機。

② 導航功能包

基于gampping 算法完成地圖的構建；提取地圖數據、里程計數據等，基于move_base 功能包實現路徑規劃；設定目標位置，通過auto_run 功能包實現自動巡航功能，計算電機速度控制指令。

2）運動控制功能

主要在STM32 單片機上實現。STM32 單片機主要采用C 語言進行開發編程。

主要功能包括：編碼器數據解算、慣導數據解算、電機速度閉環控制、數據的串口發送編碼、數據串口接收處理。

4 實驗室測試

對井下機器人進行實驗室測試，主要包括建圖測試、導航測試。

1）建圖測試

建圖是導航的基礎，地圖是否正確影響導航效果。因此首先對建圖功能進行測試。

利用ROS 的rqt 工具可以得到建圖時的建圖節點與話題功能圖。建圖節點與話題功能圖如圖4。

圖4 建圖節點與話題功能圖

圖中完成了雷達到底盤的靜態tf 坐標變換，雷達掃描節點發布激光雷達實時掃描的地圖數據，SLAM 算法接收實時地圖掃描的數據和做底盤與激光雷達之間位置轉換的tf 坐標變換數據，完成地圖的構建。

對實驗室進行實地測試，圖5 為實驗室的二維柵格圖。

圖5 實驗室二維柵格圖

圖5 表示根據雷達數據所創建的地圖，其中黑色邊界表示實驗室圍墻，其內部黑色區域表示障礙物，包括靜態和動態障礙物，白色區域為可通行區域。從建圖結果可以看出，建得的地圖與實際實驗室的地圖吻合，建圖功能可以正常運行。

2）導航測試

在實驗室中設定幾個坐標點，測試機器人是否能夠正確地運行到指定位置，并且能夠繞開障礙物。

圖6 中虛線為全局路徑規劃路線，點線所包圍的網格線為局部路徑規劃路線。路徑規劃運行的結果滿足預期，可以正常進行目標點的導航及巡航功能。

圖6 機器人路徑圖

5 結論

該文設計了井下機器人，通過激光雷達對井下環境進行掃描，搭載Jetson Nano 主機接收地圖數據并通過SLAM 導航算法實現建圖、定位、導航、避障等功能，發送指令給單片機，單片機根據控制算法實現車輪轉角與速度的控制。通過實驗室的測試，驗證了設計的機器人導航等功能獲得較好效果，在未來的應用中，可完成井下探測、開采、運輸、救援等工作。