基于SLAM 導(dǎo)航的煤礦井下機(jī)器人設(shè)計(jì)

趙文金 朱子恒 吳云雁

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

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

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

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

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

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

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

1 總體方案

自主導(dǎo)航功能主要分成兩個(gè)部分：建圖和導(dǎo)航。如圖1 所示。

圖1 自主導(dǎo)航功能結(jié)構(gòu)圖

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

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

2 硬件方案

2.1 硬件分析

機(jī)器人的硬件主要考慮三個(gè)方面：控制器類(lèi)、傳感器類(lèi)、底盤(pán)類(lèi)。

1）控制器類(lèi)

控制器主要完成兩部分功能：導(dǎo)航算法實(shí)現(xiàn)和運(yùn)動(dòng)控制。

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

以半導(dǎo)體的STM32F103ZET6 芯片為單片機(jī)核心，并搭載外圍電路，主要用于實(shí)現(xiàn)車(chē)輪電機(jī)的運(yùn)動(dòng)控制，包括轉(zhuǎn)角和速度的調(diào)節(jié)。STM32F103ZET6是半導(dǎo)體公司生產(chǎn)的32 位具有低功耗、高性能的處理器，具有ARM Corex-M3RISC 內(nèi)核，工作頻率可達(dá)72 MHz，高速嵌入式存儲(chǔ)器，包括閃存512 KB，SRAM64 KB，提供了3 個(gè)12 位ADC 通道、4 個(gè)通用16 位定時(shí)器，還有多種標(biāo)準(zhǔn)通信接口，包括2 個(gè)I2C、3 個(gè)SPI、1 個(gè)SDIO、5 個(gè)USART、1個(gè)USB 和1 個(gè)CAN 通道。

2）傳感器類(lèi)

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

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

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

3）底盤(pán)類(lèi)

底盤(pán)采用阿克曼類(lèi)型底盤(pán)，其控制方式主要采用舵機(jī)實(shí)現(xiàn)前輪轉(zhuǎn)向功能、利用雙電機(jī)完成后輪差速驅(qū)動(dòng)功能。選用額定扭矩為13.5 kg·cm、額定電流為2.3 A、額定轉(zhuǎn)速為230 r/min、減速比為27:1型后輪驅(qū)動(dòng)直流無(wú)刷電動(dòng)機(jī)。電動(dòng)機(jī)與其配套的底盤(pán)電機(jī)輪組模塊帶有滑動(dòng)軸承，電動(dòng)機(jī)不直接驅(qū)動(dòng)車(chē)輪，而是負(fù)責(zé)傳遞動(dòng)力。

2.2 硬件方案設(shè)計(jì)

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

圖2 硬件連接圖

各部分主要功能包括：

1）導(dǎo)航算法功能

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

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

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

2）運(yùn)動(dòng)控制功能

STM32 通過(guò)調(diào)節(jié)控制舵機(jī)PWM 信號(hào)的占空比，實(shí)現(xiàn)對(duì)后驅(qū)電動(dòng)機(jī)的控制，舵機(jī)帶動(dòng)前輪轉(zhuǎn)動(dòng)實(shí)現(xiàn)轉(zhuǎn)向功能；

STM32 連接驅(qū)動(dòng)器，驅(qū)動(dòng)器連接后驅(qū)電動(dòng)機(jī)，STM32 通過(guò)調(diào)節(jié)驅(qū)動(dòng)器控制電路PWM 信號(hào)的占空比，實(shí)現(xiàn)對(duì)驅(qū)動(dòng)器驅(qū)動(dòng)電路驅(qū)動(dòng)電機(jī)電流大小的控制，進(jìn)而實(shí)現(xiàn)STM32 對(duì)電機(jī)轉(zhuǎn)速的控制；

STM32 通過(guò)調(diào)節(jié)驅(qū)動(dòng)器控制電路輸入邏輯電平的高低，實(shí)現(xiàn)對(duì)后驅(qū)電動(dòng)機(jī)電流方向及有無(wú)的控制，進(jìn)而實(shí)現(xiàn)STM32 對(duì)電機(jī)轉(zhuǎn)向的控制；

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

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

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

3 軟件方案

3.1 軟件分析

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

3.2 軟件方案設(shè)計(jì)

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

圖3 軟件結(jié)構(gòu)示意圖

軟件功能上主要分成兩個(gè)部分：

1）導(dǎo)航功能

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

主要的功能包括：

① 通信功能包

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

② 導(dǎo)航功能包

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

2）運(yùn)動(dòng)控制功能

主要在STM32 單片機(jī)上實(shí)現(xiàn)。STM32 單片機(jī)主要采用C 語(yǔ)言進(jìn)行開(kāi)發(fā)編程。

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

4 實(shí)驗(yàn)室測(cè)試

對(duì)井下機(jī)器人進(jìn)行實(shí)驗(yàn)室測(cè)試，主要包括建圖測(cè)試、導(dǎo)航測(cè)試。

1）建圖測(cè)試

建圖是導(dǎo)航的基礎(chǔ)，地圖是否正確影響導(dǎo)航效果。因此首先對(duì)建圖功能進(jìn)行測(cè)試。

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

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

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

對(duì)實(shí)驗(yàn)室進(jìn)行實(shí)地測(cè)試，圖5 為實(shí)驗(yàn)室的二維柵格圖。

圖5 實(shí)驗(yàn)室二維柵格圖

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

2）導(dǎo)航測(cè)試

在實(shí)驗(yàn)室中設(shè)定幾個(gè)坐標(biāo)點(diǎn)，測(cè)試機(jī)器人是否能夠正確地運(yùn)行到指定位置，并且能夠繞開(kāi)障礙物。

圖6 中虛線為全局路徑規(guī)劃路線，點(diǎn)線所包圍的網(wǎng)格線為局部路徑規(guī)劃路線。路徑規(guī)劃運(yùn)行的結(jié)果滿足預(yù)期，可以正常進(jìn)行目標(biāo)點(diǎn)的導(dǎo)航及巡航功能。

圖6 機(jī)器人路徑圖

5 結(jié)論

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