陳騁，陳秀田，朱明亮

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

3.1 實時定位

3.1.1 定位原理

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

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

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

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

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

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

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

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

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

3.1.2 定位測試

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

圖3 機器人實時定位測試結(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)先算法的特性，被廣泛應用于靜態(tài)場景導航中[12-13]。該算法在深度優(yōu)先策略的基礎(chǔ)上增加了一個啟發(fā)函數(shù)，使得搜索方向具有目標性。但是當選取的啟發(fā)因子不適合當前規(guī)劃任務(wù)時，會導致路徑規(guī)劃失敗。針對該問題，對A*算法進行優(yōu)化，加入尋找新目標點的容忍距離參數(shù)。加入該參數(shù)后，若路徑規(guī)劃失敗，則在目標位置附近尋找另一個可以到達的位置替換當前目標位置，重新規(guī)劃一條全局路徑[14]。

3.2.2 路徑規(guī)劃測試

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

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

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

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

4 目標識別與投擲子系統(tǒng)

4.1 目標識別

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

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

在分辨率為640×480 的較復雜場景下，在距離機器人3 m 范圍內(nèi)，著火點目標識別測試結(jié)果見表2?？煽闯鰴C器人對動態(tài)多著火點的虛警率在10%以內(nèi)，識別率達90%，滿足應用要求。

表2 著火點目標識別測試結(jié)果Table 2 Test results of ignition points detection

4.2 云臺控制

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

圖5 云臺串級控制流程Fig.5 Cascade control process of the pan-tilt-zoom（PTZ）

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

5 結(jié)論

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

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

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

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