陳騁,陳秀田,朱明亮
(1.中煤科工集團(tuán)沈陽研究院有限公司,遼寧 撫順 113122;2.華晉焦煤有限責(zé)任公司,山西 呂梁 033000;3.煤礦安全技術(shù)國家重點(diǎn)實(shí)驗(yàn)室,遼寧 撫順 113122)
我國煤炭資源豐富,是世界上最大的煤炭生產(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ù)支持。
煤礦消防滅火機(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
機(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.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.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
通過先驗(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
云臺(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)的精確處理。
(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ū)域。