陳騁，陳秀田，朱明亮

（1.中煤科工集團(tuán)沈陽研究院有限公司，遼寧 撫順 113122；2.華晉焦煤有限責(zé)任公司,山西 呂梁 033000；3.煤礦安全技術(shù)國家重點(diǎn)實(shí)驗(yàn)室，遼寧 撫順 113122）

0 引言

我國煤炭資源豐富，是世界上最大的煤炭生產(chǎn)和消費(fèi)國。由于作業(yè)設(shè)備和工藝相對(duì)落后，煤礦井下事故發(fā)生率較高，煤礦安全形勢(shì)嚴(yán)峻[1]?！度珖踩a(chǎn)專項(xiàng)整治三年行動(dòng)計(jì)劃》中明確指出，要全面推進(jìn)煤礦智能化建設(shè)[2]，實(shí)現(xiàn)煤礦高危險(xiǎn)現(xiàn)場(chǎng)少人化、無人化。這給煤礦機(jī)器人發(fā)展帶來新的機(jī)遇和挑戰(zhàn)[3]。煤礦井下作業(yè)環(huán)境惡劣且存在高危氣體，消防滅火機(jī)器人不僅能夠幫助救援人員率先探明災(zāi)情、預(yù)測(cè)火勢(shì)、熟悉環(huán)境，還能代替救援者進(jìn)入火災(zāi)現(xiàn)場(chǎng)援救受困人員，更能發(fā)揮滅火功效，高效完成搶險(xiǎn)救災(zāi)工作，是煤礦井下防滅火治理及災(zāi)后救援的重要裝備。

目前煤礦機(jī)器人研究在底盤設(shè)計(jì)及防爆處理等方面取得進(jìn)展，機(jī)器人底盤環(huán)境適應(yīng)性及越障能力顯著提高，但針對(duì)煤礦井下復(fù)雜地形的機(jī)器人定位、路徑規(guī)劃[4]、火災(zāi)險(xiǎn)情識(shí)別及精準(zhǔn)滅火等方面的研究較少，相關(guān)技術(shù)仍需重點(diǎn)攻克。為減少煤炭生產(chǎn)環(huán)節(jié)消防安全隱患，提升煤礦防滅火及救援時(shí)效性，本文設(shè)計(jì)了煤礦消防滅火機(jī)器人系統(tǒng)，結(jié)合超寬帶（Ultra Wide Band，UWB）和激光雷達(dá)技術(shù)，采用自適應(yīng)蒙特卡洛定位（Adaptive Monte Carlo Localization，AMCL）算法實(shí)現(xiàn)機(jī)器人定位，采用優(yōu)化A*算法實(shí)現(xiàn)機(jī)器人路徑規(guī)劃，采用模板匹配方法識(shí)別險(xiǎn)情目標(biāo)，并通過云臺(tái)控制將消防彈投射至險(xiǎn)情目標(biāo)，為煤礦精準(zhǔn)滅火提供技術(shù)支持。

1 系統(tǒng)總體設(shè)計(jì)

煤礦消防滅火機(jī)器人系統(tǒng)包括機(jī)器人硬件子系統(tǒng)、定位與路徑規(guī)劃子系統(tǒng)、目標(biāo)識(shí)別與投擲子系統(tǒng)3 個(gè)部分，如圖1 所示。機(jī)器人硬件子系統(tǒng)為機(jī)器人基礎(chǔ)運(yùn)動(dòng)和信息采集提供保障。定位與路徑規(guī)劃子系統(tǒng)讀取UWB 定位模塊數(shù)據(jù)、激光雷達(dá)數(shù)據(jù)和里程計(jì)信息，實(shí)現(xiàn)機(jī)器人初始化定位、實(shí)時(shí)避障運(yùn)動(dòng)和路徑規(guī)劃等功能。目標(biāo)識(shí)別與投擲子系統(tǒng)通過調(diào)用攝像頭的視頻流信息，采用模板匹配方法進(jìn)行特定目標(biāo)識(shí)別，并控制二軸云臺(tái)運(yùn)動(dòng)和消防彈投擲。

圖1 煤礦消防滅火機(jī)器人系統(tǒng)結(jié)構(gòu)Fig.1 Structure of coal mine fire-fighting robot system

2 機(jī)器人硬件子系統(tǒng)

機(jī)器人底盤搭載基于STM32F427 芯片的主控板，采用4 個(gè)由M3508 減速電動(dòng)機(jī)驅(qū)動(dòng)的麥克納姆輪作為動(dòng)力組件，進(jìn)行前行、橫移、斜移、旋轉(zhuǎn)及其組合方式的多種運(yùn)動(dòng)，實(shí)現(xiàn)機(jī)器人全向移動(dòng)。

激光雷達(dá)是一種通過發(fā)射激光束來探測(cè)目標(biāo)對(duì)象位置及速度等特征量的傳感器[5-6]，本文選用RPLIDAR A3M1 型激光雷達(dá)。

機(jī)載計(jì)算設(shè)備選用Intel NUC8i7BEH 型工業(yè)計(jì)算機(jī)，運(yùn)行機(jī)器人操作系統(tǒng)（Robot Operating System，ROS），實(shí)現(xiàn)圖像處理和機(jī)器人運(yùn)動(dòng)控制等相關(guān)功能。

UWB 是一種無線載波通信技術(shù)，具有功耗低、抗干擾能力和穿透能力強(qiáng)、定位準(zhǔn)確度較高等優(yōu)點(diǎn)[7-8]。本文選用DJI-RM-V1 UWB 系統(tǒng)，其最高水平定位精度為5.5 cm，最高垂直定位精度為6.5 cm，位置更新頻率為50 Hz。

3 定位與路徑規(guī)劃子系統(tǒng)

3.1 實(shí)時(shí)定位

3.1.1 定位原理

機(jī)器人開機(jī)后，首先通過UWB 定位模塊確定機(jī)器人當(dāng)前位置，在獲取到的位置附近生成一系列正態(tài)分布的坐標(biāo)點(diǎn)，再對(duì)這些坐標(biāo)點(diǎn)進(jìn)行篩選。針對(duì)每個(gè)坐標(biāo)點(diǎn)，調(diào)用激光雷達(dá)掃描周邊環(huán)境，得到二維點(diǎn)云數(shù)據(jù)；采用迭代最近點(diǎn)（Iterative Closest Point,ICP）算法對(duì)二維點(diǎn)云數(shù)據(jù)和預(yù)先存儲(chǔ)的地圖數(shù)據(jù)進(jìn)行比較，根據(jù)誤差最小原則確定機(jī)器人在該坐標(biāo)點(diǎn)的姿態(tài)角及其置信度。對(duì)所有坐標(biāo)點(diǎn)姿態(tài)角的置信度進(jìn)行比較，將置信度最高的坐標(biāo)點(diǎn)作為機(jī)器人的真實(shí)坐標(biāo)，并將機(jī)器人在該坐標(biāo)點(diǎn)的姿態(tài)角作為初始化時(shí)的真實(shí)姿態(tài)信息。

采用AMCL 算法[9]實(shí)現(xiàn)機(jī)器人實(shí)時(shí)定位。AMCL 算法是適用于二維平面內(nèi)機(jī)器人運(yùn)動(dòng)位姿估計(jì)的算法，其流程如圖2 所示。

圖2 AMCL 算法流程Fig.2 Flow of the AMCL algorithm

AMCL 算法實(shí)現(xiàn)步驟：

（1）隨機(jī)生成一批粒子并進(jìn)行地圖載入工作，根據(jù)慣性測(cè)量裝置和里程計(jì)數(shù)據(jù)更新粒子的位姿信息。

（2）通過UWB 數(shù)據(jù)判斷機(jī)器人位置，通過激光雷達(dá)數(shù)據(jù)判斷機(jī)器人與周邊障礙物的距離（根據(jù)灰度值判斷是否為障礙物），對(duì)初始生成的粒子進(jìn)行置信度更新。距離機(jī)器人越遠(yuǎn)的粒子，其置信度越低；與激光雷達(dá)所測(cè)得的周邊障礙物姿態(tài)信息越不匹配的粒子，其置信度越低。

（3）根據(jù)粒子置信度高低賦予權(quán)值，置信度越高，權(quán)值越大。舍棄置信度低于閾值的粒子。

（4）重采樣階段，根據(jù)粒子權(quán)值對(duì)粒子進(jìn)行復(fù)制，復(fù)制份數(shù)與權(quán)值大小成正比。采用KL 散度（Kullback-Leibler Divergense，KLD）算法確定粒子數(shù)量。KLD 算法是一種自適應(yīng)的粒子數(shù)量確定算法，可使粒子數(shù)量估計(jì)值與后驗(yàn)值之間的偏差不超過設(shè)定界限，即新生成粒子的數(shù)量隨迭代次數(shù)增加而逐漸減少。粒子之間的距離不超過規(guī)定距離0.05 m時(shí)，認(rèn)為定位成功，輸出機(jī)器人當(dāng)前位置。

在實(shí)際應(yīng)用中，可能出現(xiàn)機(jī)器人運(yùn)動(dòng)過程中定位丟失的情況。為解決該問題，采用ROS 中的SnapMapICP 包進(jìn)行機(jī)器人重定位[10-11]。

3.1.2 定位測(cè)試

首先獲取UWB 信號(hào)，確定機(jī)器人初始位置信息，使用激光雷達(dá)實(shí)時(shí)掃描得到點(diǎn)云數(shù)據(jù)，并與該位置地圖數(shù)據(jù)進(jìn)行匹配。初始化階段，機(jī)器人姿態(tài)角的預(yù)測(cè)精度達(dá)3～5°，耗時(shí)1～2 s。在運(yùn)動(dòng)過程中，通過UWB 模塊判斷機(jī)器人大致位置，根據(jù)激光雷達(dá)數(shù)據(jù)進(jìn)行精確定位修正，測(cè)試結(jié)果如圖3 所示。圖3（b）中十字坐標(biāo)位置為機(jī)器人中心位置，與障礙物（黃色）之間的距離以二維云圖方式體現(xiàn)，實(shí)測(cè)定位精度達(dá)5～10 cm。

圖3 機(jī)器人實(shí)時(shí)定位測(cè)試結(jié)果Fig.3 Test results of robot real-time positioning

3.2 路徑規(guī)劃

3.2.1 A*算法原理

A*算法作為一種求解靜態(tài)網(wǎng)絡(luò)最短路徑最有效的直接搜索算法，綜合了深度優(yōu)先算法和廣度優(yōu)先算法的特性，被廣泛應(yīng)用于靜態(tài)場(chǎng)景導(dǎo)航中[12-13]。該算法在深度優(yōu)先策略的基礎(chǔ)上增加了一個(gè)啟發(fā)函數(shù)，使得搜索方向具有目標(biāo)性。但是當(dāng)選取的啟發(fā)因子不適合當(dāng)前規(guī)劃任務(wù)時(shí)，會(huì)導(dǎo)致路徑規(guī)劃失敗。針對(duì)該問題，對(duì)A*算法進(jìn)行優(yōu)化，加入尋找新目標(biāo)點(diǎn)的容忍距離參數(shù)。加入該參數(shù)后，若路徑規(guī)劃失敗，則在目標(biāo)位置附近尋找另一個(gè)可以到達(dá)的位置替換當(dāng)前目標(biāo)位置，重新規(guī)劃一條全局路徑[14]。

3.2.2 路徑規(guī)劃測(cè)試

機(jī)器人路徑規(guī)劃測(cè)試結(jié)果如圖4 所示，圖4（a）中右上角紅色為機(jī)器人位置，綠色曲線為通過優(yōu)化A*算法規(guī)劃的路徑，綠色曲線終點(diǎn)為目標(biāo)滅火點(diǎn)。

圖4 機(jī)器人路徑規(guī)劃測(cè)試結(jié)果Fig.4 Test results of robot path planning

在試驗(yàn)場(chǎng)地內(nèi)設(shè)置不同著火點(diǎn)進(jìn)行測(cè)試，實(shí)測(cè)數(shù)據(jù)見表1?？煽闯鰞?yōu)化A*算法用時(shí)短，規(guī)劃的路徑合理。

表1 路徑規(guī)劃指標(biāo)Table 1 Path planning indexes

4 目標(biāo)識(shí)別與投擲子系統(tǒng)

4.1 目標(biāo)識(shí)別

通過先驗(yàn)知識(shí)生成著火點(diǎn)特征圖像庫，將特征圖像作為特征匹配環(huán)節(jié)的模板圖像。讀取攝像頭采集的視頻流，并對(duì)模板圖像和視頻幀進(jìn)行歸一化處理。采用滑動(dòng)窗口方式對(duì)模板圖像（分辨率為128×128）和視頻幀（分辨率為1 920×1 080）進(jìn)行比較，計(jì)算相關(guān)系數(shù)R。相關(guān)系數(shù)R反映模板圖像與待檢驗(yàn)圖像的相關(guān)程度，當(dāng)模板圖像與視頻幀中某區(qū)域完全一致時(shí)，R=1。設(shè)置相關(guān)系數(shù)閾值，將相關(guān)系數(shù)大于閾值且相關(guān)系數(shù)值最大的區(qū)域確定為著火點(diǎn)目標(biāo)。

獲取著火點(diǎn)中心位置，將云臺(tái)當(dāng)前位置與著火點(diǎn)中心位置的偏差值作為給定信息傳輸?shù)皆婆_(tái)控制模塊。

在分辨率為640×480 的較復(fù)雜場(chǎng)景下，在距離機(jī)器人3 m 范圍內(nèi)，著火點(diǎn)目標(biāo)識(shí)別測(cè)試結(jié)果見表2?？煽闯鰴C(jī)器人對(duì)動(dòng)態(tài)多著火點(diǎn)的虛警率在10%以內(nèi)，識(shí)別率達(dá)90%，滿足應(yīng)用要求。

表2 著火點(diǎn)目標(biāo)識(shí)別測(cè)試結(jié)果Table 2 Test results of ignition points detection

4.2 云臺(tái)控制

云臺(tái)控制的目的是準(zhǔn)確、快速地跟蹤著火點(diǎn)位置，使消防彈能夠投射至目標(biāo)區(qū)域。采用速度、位置雙閉環(huán)的串級(jí)控制器實(shí)現(xiàn)云臺(tái)俯仰軸和偏航軸角度控制。以偏航軸角度控制為例，串級(jí)控制流程如圖5所示。

圖5 云臺(tái)串級(jí)控制流程Fig.5 Cascade control process of the pan-tilt-zoom（PTZ）

通過速度、位置雙閉環(huán)PID 調(diào)節(jié)[15]，使得云臺(tái)響應(yīng)曲線較平滑且準(zhǔn)確地跟隨給定曲線。在實(shí)際測(cè)試中，即使目標(biāo)識(shí)別模塊給出的控制量波動(dòng)較大，云臺(tái)的實(shí)際響應(yīng)曲線也較平滑，且具有一定的超前預(yù)測(cè)效果[16]。通過整定串級(jí)控制器的PID 參數(shù)，可彌補(bǔ)目標(biāo)識(shí)別頻率略小于云臺(tái)控制頻率的不足，抵消零階保持效果[17]，實(shí)現(xiàn)對(duì)險(xiǎn)情目標(biāo)的精確處理。

5 結(jié)論

（1）結(jié)合UWB、激光雷達(dá)技術(shù)，采用ICP 算法實(shí)現(xiàn)機(jī)器人定位初始化；在實(shí)時(shí)定位階段，結(jié)合慣性測(cè)量裝置和里程計(jì)數(shù)據(jù)，采用AMCL 算法實(shí)現(xiàn)機(jī)器人定位，實(shí)測(cè)定位精度達(dá)5～10 cm。

（2）在A*算法中，當(dāng)選取的啟發(fā)因子不適合當(dāng)前規(guī)劃任務(wù)時(shí)，會(huì)導(dǎo)致路徑規(guī)劃失敗。針對(duì)該問題，對(duì)A*算法進(jìn)行優(yōu)化，加入尋找新目標(biāo)點(diǎn)的容忍距離參數(shù)。加入該參數(shù)后，若路徑規(guī)劃失敗，則在目標(biāo)位置附近尋找另一個(gè)可以到達(dá)的位置替換當(dāng)前目標(biāo)位置，重新規(guī)劃一條全局路徑。測(cè)試結(jié)果表明，采用優(yōu)化A*算法進(jìn)行路徑規(guī)劃用時(shí)短，規(guī)劃的路徑合理。

（3）通過先驗(yàn)知識(shí)生成著火點(diǎn)特征圖像庫，將特征圖像作為特征匹配環(huán)節(jié)的模板圖像，采用模板匹配方法識(shí)別險(xiǎn)情目標(biāo)。測(cè)試結(jié)果表明，機(jī)器人對(duì)動(dòng)態(tài)多著火點(diǎn)的虛警率在10%以內(nèi)，識(shí)別率達(dá)90%，滿足應(yīng)用要求。

（4）采用速度、位置雙閉環(huán)的串級(jí)控制器實(shí)現(xiàn)云臺(tái)俯仰軸和偏航軸角度控制，能夠準(zhǔn)確、快速地跟蹤著火點(diǎn)位置，使消防彈投射至目標(biāo)區(qū)域。