李榮和，關(guān)凱元，繆顧鑫，李振康

（黑龍江科技大學(xué) 電氣與控制工程學(xué)院，黑龍江 哈爾濱 150028）

0 引言

我國(guó)是一個(gè)產(chǎn)煤和耗煤的大國(guó)。煤炭產(chǎn)業(yè)是我國(guó)經(jīng)濟(jì)發(fā)展的重要產(chǎn)業(yè)之一。煤礦井下的安全和設(shè)備完好性，是制約我國(guó)煤炭行業(yè)發(fā)展的主要問(wèn)題。煤礦井下的工作環(huán)境惡劣危險(xiǎn)，有許多巷道，可能突發(fā)危險(xiǎn)情況,而且在大多數(shù)救援過(guò)程中，礦井還可能發(fā)生瓦斯爆炸、坍塌、礦井下迷失等危險(xiǎn)問(wèn)題，會(huì)延誤救援人員的救援工作[1]。

為此，本文研究一款智能井下巡檢機(jī)器人。總體作為一個(gè)礦井探測(cè)救援系統(tǒng)，可以幫助人員深入礦井，能夠完成具有一定危險(xiǎn)性的救援探索任務(wù)，救出被困的礦井工人，減少救援時(shí)間，從而降低傷亡人數(shù)。

日常巡檢時(shí)，智能巡檢機(jī)器人可有效拍攝出巡檢環(huán)境內(nèi)各種設(shè)備的高清圖像，并利用網(wǎng)絡(luò)將采集到的圖像信息實(shí)時(shí)傳輸?shù)胶笈_(tái)主控制系統(tǒng)，方便工作人員能夠通過(guò)接收到的視頻、圖像對(duì)巡檢設(shè)備的運(yùn)行狀態(tài)做出有效判斷并制定操作計(jì)劃。當(dāng)井下發(fā)生異常警報(bào)時(shí)，可以通過(guò)機(jī)器人發(fā)送的信息發(fā)現(xiàn)井下的異常情況，救援人員可根據(jù)機(jī)器人傳出的視頻和環(huán)境建圖來(lái)排查井下情況。

1 機(jī)器人基本結(jié)構(gòu)

基于分層控制的設(shè)計(jì)理念，本文對(duì)巡檢救助機(jī)器人系統(tǒng)各個(gè)功能模塊進(jìn)行分層設(shè)計(jì)，分為監(jiān)控層、通信層以及控制層。其中，控制層又分為上位機(jī)控制系統(tǒng)和下位機(jī)控制系統(tǒng)。各層次功能明確，如圖1 所示。

圖1 機(jī)器人各層各級(jí)分布圖

1.1 監(jiān)控層

監(jiān)控層可以通過(guò)計(jì)算機(jī)遠(yuǎn)程操作或采用電腦連接機(jī)器人后部的顯示屏進(jìn)行代碼修改或控制，接受工作人員發(fā)出的指令進(jìn)行工作，在正常狀態(tài)下可以進(jìn)行區(qū)域自主導(dǎo)航，檢查井下設(shè)備的工作狀態(tài)以及巡檢地區(qū)是否發(fā)生異常。監(jiān)控層可以載入對(duì)人或設(shè)備的訓(xùn)練模型，然后用卷積層替換整個(gè)連接層，并對(duì)其進(jìn)行端到端的訓(xùn)練（一種深度神經(jīng)網(wǎng)絡(luò)訓(xùn)練），以便進(jìn)行目標(biāo)檢測(cè)。YOLO 培訓(xùn)分類器使用224×224 張照片，然后使用448×448 張圖片進(jìn)行目標(biāo)檢測(cè)；通過(guò)巡檢設(shè)備（激光雷達(dá)和深度相機(jī)）利用即時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）完成建圖后，地圖信息會(huì)被保存為文件。

工作人員可以在可視化工具內(nèi)加載掃描的點(diǎn)云地圖文件，查看地圖具體信息，以實(shí)現(xiàn)機(jī)器人的自主巡航、定點(diǎn)導(dǎo)航的功能；監(jiān)控層還能夠人為地對(duì)地圖上的區(qū)域進(jìn)行劃分，并且選擇劃分的區(qū)域設(shè)計(jì)為機(jī)器人所代表的無(wú)障礙點(diǎn)，將記錄的好的點(diǎn)在可視化工具中設(shè)置為坐標(biāo)，并對(duì)點(diǎn)進(jìn)行賦值命名從而實(shí)現(xiàn)定點(diǎn)導(dǎo)航。

1.2 通信層

考慮到礦井的特殊性，通信層在實(shí)際應(yīng)用中分為三部分。

第一部分是地面控制中心（數(shù)據(jù)管理中心）。通過(guò)設(shè)置在地面的數(shù)據(jù)管理主機(jī)，能夠?qū)ρ矙z機(jī)器人進(jìn)行遠(yuǎn)程操控，遠(yuǎn)程監(jiān)控煤礦井下的運(yùn)行設(shè)備狀態(tài)、溫度以及傳輸，實(shí)現(xiàn)機(jī)器人的大數(shù)據(jù)管理和數(shù)據(jù)、圖像、視頻的傳輸與處理，機(jī)器人運(yùn)動(dòng)軌跡遠(yuǎn)程控制等功能。

第二部分是數(shù)據(jù)傳輸。數(shù)據(jù)傳輸部分主要依托工業(yè)級(jí)的網(wǎng)絡(luò)平臺(tái)如以太網(wǎng)或總線設(shè)置。通過(guò)RJ-45 通信端口（一種常用的布線系統(tǒng)中信息插座連接器）就近接入交換機(jī)、以太網(wǎng)或總線站等。在機(jī)器人的調(diào)試過(guò)程中，由于不具備真實(shí)的礦井情況和以太網(wǎng)等工業(yè)級(jí)的網(wǎng)絡(luò)平臺(tái)，在測(cè)試中可用兩個(gè)通信模塊代替。

第三部分是數(shù)據(jù)終端，用來(lái)實(shí)現(xiàn)信號(hào)覆蓋和通信，本部分主要用于與巡檢機(jī)器人和電腦終端的聯(lián)系。

1.3 控制層

巡檢救助機(jī)器人的控制層分為上位機(jī)控制系統(tǒng)和下位機(jī)控制系統(tǒng)兩大部分。兩個(gè)部分相互獨(dú)立運(yùn)行并實(shí)現(xiàn)各自的基本功能，通過(guò)數(shù)據(jù)總線的方式進(jìn)行上下位機(jī)數(shù)據(jù)的交互；Jetson nano 主控作為上位機(jī)，類似于人體的大腦結(jié)構(gòu)，需要對(duì)采集到的數(shù)據(jù)量和下位機(jī)運(yùn)行的數(shù)據(jù)進(jìn)行數(shù)據(jù)處理，對(duì)處理過(guò)后的數(shù)據(jù)進(jìn)行實(shí)時(shí)的定位和建圖，并對(duì)機(jī)器人的運(yùn)動(dòng)做出決策[2]；最后將控制命令發(fā)送給下位機(jī)，完成控制運(yùn)動(dòng)平臺(tái)的動(dòng)作。

STM32 作為下位機(jī)控制器起到兩個(gè)至關(guān)重要的作用，即對(duì)機(jī)器人運(yùn)動(dòng)的控制和收集傳感器獲得的原始數(shù)據(jù)，將原始數(shù)據(jù)進(jìn)行簡(jiǎn)單的處理后發(fā)送給上位機(jī)，根據(jù)上位機(jī)對(duì)數(shù)據(jù)處理后發(fā)送的指令，從而控制機(jī)器人的運(yùn)動(dòng)。激光雷達(dá)和深度相機(jī)安裝在機(jī)器人的前端和上端，根據(jù)機(jī)器人的移動(dòng)得到距離信息和位移記錄。

2 系統(tǒng)軟硬件設(shè)計(jì)

2.1 系統(tǒng)硬件設(shè)計(jì)

井下救助機(jī)器人搭載了RPLIDAR A3 激光測(cè)距傳感器、深度相機(jī)、Jetson nano 主控等設(shè)備。操作系統(tǒng)采用基于Linux 系統(tǒng)的機(jī)器人軟件控制平臺(tái)（Robot Operating System，ROS）。ROS 系統(tǒng)可以提供一系列程序庫(kù)和工具以幫助軟件開發(fā)者創(chuàng)建機(jī)器人應(yīng)用軟件，有硬件抽象、設(shè)備驅(qū)動(dòng)、函數(shù)庫(kù)、可視化工具等諸多功能。本文采用ROS 系統(tǒng)的原因是ROS 的通信框架資源生態(tài)豐富易于操作使用，并且支持對(duì)機(jī)器人采用分層設(shè)計(jì)的控制方式[3]。

2.1.1 激光雷達(dá)與深度相機(jī)融合建圖

機(jī)器人要完成自主避障和導(dǎo)航，最主要的是解決SLAM 建圖的問(wèn)題。最基本的方法是，借助多源傳感器，使機(jī)器人擁有一定的感知周圍環(huán)境深度信息的能力。本文通過(guò)激光雷達(dá)和深度相機(jī)來(lái)實(shí)現(xiàn)這種能力，因此本文的硬件部分只對(duì)激光雷達(dá)和深度相機(jī)進(jìn)行介紹。

通過(guò)激光雷達(dá)可實(shí)現(xiàn)對(duì)周圍環(huán)境的360 度全方位掃描測(cè)距檢測(cè)，從而獲得周圍環(huán)境的輪廓圖，使機(jī)器人可以在更加曠闊的環(huán)境中進(jìn)行地圖構(gòu)建和自主定位。

深度相機(jī)用于視覺(jué)的圖像采集，可提高機(jī)器人建圖的質(zhì)量，能夠采集場(chǎng)景的三維信息并配合激光雷達(dá)的點(diǎn)云識(shí)別實(shí)現(xiàn)對(duì)環(huán)境的感知。采用深度相機(jī)配合雷達(dá)的方法，能夠加快對(duì)環(huán)境的構(gòu)建和視覺(jué)的調(diào)試效果。效果如圖2 所示。

圖2 激光雷達(dá)與深度相機(jī)融合建圖

2.1.2 機(jī)械臂

巡檢救助機(jī)器人的機(jī)械臂采用的是四自由度機(jī)械臂并配置了柔性二指爪，使用數(shù)字信號(hào)來(lái)進(jìn)行控制?？刂菩盘?hào)進(jìn)入下位機(jī)控制器，可獲得直流偏置電壓，將直流偏置電壓與電位器的電壓比較，獲得電壓差之后輸出。電壓差的正負(fù)輸出到舵機(jī)，決定舵機(jī)的正反轉(zhuǎn)。并且，利用機(jī)械臂動(dòng)力學(xué)模型并進(jìn)行了線性化處理，以降低算法復(fù)雜度、保證實(shí)時(shí)性。

2.2 系統(tǒng)的軟件設(shè)計(jì)

2.2.1 濾波算法

要完成機(jī)器人的自主導(dǎo)航和避障，首先要解決SLAM 建圖的問(wèn)題。本文前部分已經(jīng)簡(jiǎn)述了硬件原理部分。SLAM 是一個(gè)龐大的理論體系，這里只對(duì)其中應(yīng)用到的一種重要的濾波算法進(jìn)行簡(jiǎn)單的敘述，便于讀者理解。

機(jī)器人采用了一種RBPF 粒子濾波算法，使機(jī)器人按照先進(jìn)行定位之后再進(jìn)行建圖的順序工作。這樣做的目的是將定位和建圖過(guò)程分離。在實(shí)時(shí)接收激光雷達(dá)的建圖數(shù)據(jù)后，與最初始的模型圖進(jìn)行匹配。其中一個(gè)雷達(dá)在匹配到位姿之后，會(huì)將當(dāng)前幀的激光插入圖中，當(dāng)圖中的位姿沒(méi)有變化或變化不大時(shí)就會(huì)將其除去。后端部分采用界定法，對(duì)地圖搜索后進(jìn)行加速，這樣會(huì)構(gòu)成一個(gè)回環(huán)檢測(cè)，可以更好地檢測(cè)出深度相機(jī)是否重復(fù)經(jīng)過(guò)一個(gè)地方，來(lái)消除匹配誤差[4]。

2.2.2 自主路徑規(guī)劃的實(shí)現(xiàn)

路徑規(guī)劃是機(jī)器人導(dǎo)航最基本的環(huán)節(jié)。在有障礙物的工作環(huán)境中，機(jī)器人需要找到一條從起點(diǎn)到終點(diǎn)的適當(dāng)?shù)倪\(yùn)動(dòng)路徑，使機(jī)器人在運(yùn)動(dòng)過(guò)程中能安全、無(wú)接觸地繞過(guò)所有障礙物。根據(jù)對(duì)環(huán)境信息的掌握程度不同，機(jī)器人路徑規(guī)劃可分為全局路徑規(guī)劃和局部路徑規(guī)劃兩種。

全局路徑規(guī)劃是通過(guò)計(jì)算出機(jī)器人到目標(biāo)位姿的路線，來(lái)利用全局代價(jià)地圖進(jìn)行全局的路徑規(guī)劃[5]。本設(shè)計(jì)采用A*算法最短路徑的算法來(lái)實(shí)現(xiàn)，通過(guò)引入啟發(fā)函數(shù)，使整體性能相比于Dijkstra（迪杰斯特拉算法）有了較大改善。

Dijkstra 算法的實(shí)質(zhì)是廣度優(yōu)先搜索，是一種發(fā)散式的搜索，所以空間復(fù)雜度和時(shí)間復(fù)雜度都比較高，計(jì)算量非常大。對(duì)路徑上的當(dāng)前點(diǎn)，A*算法不但記錄其到源點(diǎn)的代價(jià)，還計(jì)算當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的期望代價(jià)，是一種啟發(fā)式算法。相比之下，A*算法消耗時(shí)間較少，可以使機(jī)器人在導(dǎo)航過(guò)程中保持連續(xù)運(yùn)動(dòng)。其數(shù)字描述可表現(xiàn)為：

式中：f(n)為從初始節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)所消耗的代價(jià)值，g(n)為初始節(jié)點(diǎn)到任意節(jié)點(diǎn)n耗費(fèi)的真實(shí)的代價(jià)值，h(n)為機(jī)器人從節(jié)點(diǎn)n移動(dòng)到目標(biāo)節(jié)點(diǎn)所消耗的代價(jià)值的啟發(fā)函數(shù)[6]。

預(yù)估移動(dòng)代價(jià)h(n)采用曼哈頓距離表示為：

式中：xd和xn分別為目標(biāo)點(diǎn)和節(jié)點(diǎn)n的橫坐標(biāo)，yd和yn分別為目標(biāo)點(diǎn)和節(jié)點(diǎn)n的縱坐標(biāo)。

通過(guò)全局路徑規(guī)劃得到一條大致可行的路線，還要使機(jī)器人獲得目的地信息，此時(shí)要調(diào)用局部路徑規(guī)劃，利用路線信息及costmap 的信息來(lái)規(guī)劃出具體行動(dòng)策略。每當(dāng)move_base（機(jī)器人路徑規(guī)劃的中心樞紐）在處于規(guī)劃狀態(tài)，就調(diào)用DWA 算法在上位機(jī)主控中發(fā)出一條最佳的速度指令，發(fā)送給機(jī)器人下位機(jī)執(zhí)行并驅(qū)動(dòng)。局部規(guī)劃的路徑會(huì)靠近全局規(guī)劃的路徑，并結(jié)合實(shí)際情況，對(duì)局部路徑進(jìn)行調(diào)整。

3 實(shí) 驗(yàn)

巡檢過(guò)程中，機(jī)器人一般會(huì)遇到兩種情況。

一種是靜態(tài)障礙物（如圖3 上兩個(gè)圖、圖4 左圖的情況）。當(dāng)遇到靜態(tài)障礙物時(shí)，開始規(guī)劃出路徑。機(jī)器人合理避讓并有效的避開了第一個(gè)障礙物，路徑符合時(shí)間最優(yōu)，最終到達(dá)的目標(biāo)點(diǎn)的姿態(tài)也吻合發(fā)布的目標(biāo)點(diǎn)。

圖3 機(jī)器人定點(diǎn)規(guī)劃

另一種是動(dòng)態(tài)障礙物（如圖3 下兩個(gè)圖、圖4右圖的情況）。當(dāng)機(jī)器人所處環(huán)境對(duì)比之前建圖的圖像發(fā)生了變化或有障礙物突然出現(xiàn)在所規(guī)劃的路線前方時(shí)，通過(guò)深度相機(jī)識(shí)別障礙物的距離信息，及時(shí)地更新形成新的建圖模型圖層。機(jī)器人將根據(jù)新的模型圖層重新規(guī)劃局部路線來(lái)繞開障礙物并繼續(xù)向著目標(biāo)點(diǎn)運(yùn)動(dòng)，這樣就實(shí)現(xiàn)了自主避障的功能。

圖4 環(huán)境模擬

4 結(jié)語(yǔ)

本文通過(guò)對(duì)深度相機(jī)和激光雷達(dá)等硬件設(shè)備的搭建以及對(duì)算法的優(yōu)化，得到機(jī)器人實(shí)時(shí)定位與建圖；利用ROS 操作系統(tǒng)對(duì)機(jī)器人進(jìn)行編程控制最終進(jìn)行調(diào)試，使得機(jī)器人實(shí)現(xiàn)同時(shí)定位與建圖以及自主導(dǎo)航的功能。

實(shí)驗(yàn)結(jié)果表明，采用本思路設(shè)計(jì)的巡檢機(jī)器人可以規(guī)劃出一條最優(yōu)路徑，同時(shí)配合激光雷達(dá)和深度相機(jī)以及路徑規(guī)劃的算法，能夠完成對(duì)障礙物的躲避任務(wù)即自主避障功能，從而達(dá)到自主導(dǎo)航到達(dá)設(shè)置的目的位置完成井下自主巡航的工作任務(wù)。