趙文金 朱子恒 吳云雁

（山西潞安環(huán)保能源開發(fā)股份有限公司常村煤礦，山西 長治 046100）

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

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

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

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

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

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

1 總體方案

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

圖1 自主導航功能結構圖

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

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

2 硬件方案

2.1 硬件分析

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

1）控制器類

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

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

以半導體的STM32F103ZET6 芯片為單片機核心，并搭載外圍電路，主要用于實現(xiàn)車輪電機的運動控制，包括轉角和速度的調節(jié)。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 算建圖主要依賴于環(huán)境數(shù)據(jù)和里程計數(shù)據(jù)，根據(jù)井下環(huán)境情況，采用360°激光雷達實時獲取環(huán)境數(shù)據(jù)。這里采用單線激光雷達RPLIDAR A1M8。該激光雷達采用三角測距原理，測距范圍為0.15～12 m，誤差為±1%，掃描角度范圍是0～360°，角度分辨率＜0.5°。

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

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

3）底盤類

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

2.2 硬件方案設計

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

圖2 硬件連接圖

各部分主要功能包括：

1）導航算法功能

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

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

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

2）運動控制功能

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

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

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

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

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

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

3 軟件方案

3.1 軟件分析

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

3.2 軟件方案設計

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

圖3 軟件結構示意圖

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

1）導航功能

主要在Jetson Nano 上實現(xiàn)。Jetson Nano 安裝Ubuntu 系統(tǒng)，并安裝ROS 系統(tǒng)，Jetson Nano 上所有的功能包都是在ROS 環(huán)境下開發(fā)。

主要的功能包括：

① 通信功能包

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

② 導航功能包

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

2）運動控制功能

主要在STM32 單片機上實現(xiàn)。STM32 單片機主要采用C 語言進行開發(fā)編程。

主要功能包括：編碼器數(shù)據(jù)解算、慣導數(shù)據(jù)解算、電機速度閉環(huán)控制、數(shù)據(jù)的串口發(fā)送編碼、數(shù)據(jù)串口接收處理。

4 實驗室測試

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

1）建圖測試

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

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

圖4 建圖節(jié)點與話題功能圖

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

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

圖5 實驗室二維柵格圖

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

2）導航測試

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

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

圖6 機器人路徑圖

5 結論

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