全向移動(dòng)偵察消防機(jī)器人

（遼寧工業(yè)大學(xué) 電氣工程學(xué)院，遼寧 錦州 121001）

隨著社會經(jīng)濟(jì)的發(fā)展，各種建筑物結(jié)構(gòu)越來越復(fù)雜。若復(fù)雜結(jié)構(gòu)的建筑中發(fā)生火災(zāi)，在沒有對現(xiàn)場進(jìn)行充分的信息采集和分析的情況下，救援人員貿(mào)然進(jìn)入現(xiàn)場進(jìn)行救援，不僅會造成巨大的財(cái)產(chǎn)損失，也會給受災(zāi)人員和救援人員帶來巨大的生命危險(xiǎn)。消防機(jī)器人作為一種無生命載體，可以代替救援人員進(jìn)入危險(xiǎn)復(fù)雜的環(huán)境進(jìn)行信息獲取，也可以輔助救援人員進(jìn)行救援活動(dòng)，能夠極大地減少財(cái)產(chǎn)損失和人員傷亡。

我國開始研究消防機(jī)器人起步較晚，但對消防機(jī)器人的研發(fā)工作進(jìn)展非常迅速?，F(xiàn)在常見的消防機(jī)器人基本都使用履帶作為移動(dòng)平臺，履帶構(gòu)成的移動(dòng)平臺體積大、重量沉，無法進(jìn)入狹小通道前進(jìn)。所以設(shè)計(jì)了一種使用麥克納姆輪作為移動(dòng)平臺的消防機(jī)器人，能夠使用遙控器進(jìn)行手動(dòng)控制，也可以進(jìn)行自動(dòng)控制，完成火焰識別并與火焰保持一定的安全距離后發(fā)射消防彈丸滅火。

1 機(jī)械結(jié)構(gòu)

機(jī)器人機(jī)械結(jié)構(gòu)包括移動(dòng)平臺、發(fā)射裝置，具體機(jī)械結(jié)構(gòu)如圖1 所示。

圖1 機(jī)器人機(jī)械結(jié)構(gòu)

采用麥克納姆輪構(gòu)成機(jī)器人底盤，使機(jī)器人可以進(jìn)行全向移動(dòng)，能夠更好適應(yīng)狹小的工作環(huán)境[1]。移動(dòng)平臺與發(fā)射管之間使用舵機(jī)連接，舵機(jī)固定在機(jī)器人底盤上，可以控制發(fā)射管角度。彈艙用來存儲消防彈丸，最多能夠存儲12 個(gè)彈丸。

小型投擲型消防球具有抗壓抗高溫、遇明火才會破裂滅火的特性，所以在發(fā)射裝置中使用摩擦輪對消防彈進(jìn)行加速，使彈丸能夠以較高的初速度從發(fā)射管中射出，并在一定距離內(nèi)保持近似直線的運(yùn)動(dòng)軌跡。

2 控制系統(tǒng)設(shè)計(jì)

系統(tǒng)可采用2 種控制方式，一種為采用無線遙控的方式手動(dòng)控制機(jī)器人進(jìn)行全向移動(dòng)、瞄準(zhǔn)、發(fā)射彈丸等動(dòng)作；另一種為機(jī)器人自動(dòng)控制，進(jìn)行瞄準(zhǔn)、調(diào)整機(jī)器人與火焰之間距離、發(fā)射彈丸滅火的動(dòng)作。

在手動(dòng)控制下，使用遙控器發(fā)送控制信息到機(jī)器人主控模塊，機(jī)器人主控模塊根據(jù)接收的控制信息對機(jī)器人進(jìn)行控制，完成相關(guān)動(dòng)作；在自動(dòng)控制下，機(jī)器人通過攝像頭識別火焰位置，分析機(jī)器人與火焰距離，調(diào)整至安全距離后，發(fā)射彈丸對火焰進(jìn)行處理。機(jī)器人系統(tǒng)控制框圖如圖2 所示。

圖2 系統(tǒng)控制框圖

2.1 硬件結(jié)構(gòu)設(shè)計(jì)

2.1.1 主控板

主控板采用STM32F103 單片機(jī)作為主控板的核心，STM32 單片機(jī)基于ARM 公司“Cortex-M3”內(nèi)核的32 位微控制器，功耗低、體積小且功能強(qiáng)大，可對其擴(kuò)展外設(shè)，軟件調(diào)試多樣，編程靈活，硬件容易實(shí)現(xiàn)，且運(yùn)算功能強(qiáng)大[2-3]。

2.1.2 溫度采集

采用PT100 傳感器對機(jī)器人內(nèi)外溫度進(jìn)行檢測，外部的溫度傳感器用于實(shí)時(shí)監(jiān)測現(xiàn)場內(nèi)環(huán)境溫度，內(nèi)部的溫度傳感器用于監(jiān)控機(jī)器人內(nèi)部溫度，防止機(jī)器人由于內(nèi)部溫度過高導(dǎo)致停止工作。

2.1.3 圖像采集

采用USBFHD08H型攝像頭進(jìn)行視頻圖像的采集。使用USB2.0 接口與樹莓派進(jìn)行連接，支持1 920×1 080 60 幀、1 280×720 120 幀、640×260 260幀分辨率及幀率。攝像頭采集現(xiàn)場視頻圖像，通過無線傳輸模塊發(fā)送到遙控器進(jìn)行顯示，對采集范圍內(nèi)的火焰進(jìn)行識別并在圖像中進(jìn)行標(biāo)記，自動(dòng)模式中，發(fā)送火焰在視頻采集范圍內(nèi)的坐標(biāo)到主控模塊，控制機(jī)器人原地旋轉(zhuǎn)，使火焰坐標(biāo)在視頻采集范圍中心位置，完成自動(dòng)瞄準(zhǔn)。

2.1.4 姿態(tài)獲取

采用MPU6050 傳感器獲取機(jī)器人底盤角度，MPU6050 是一種6 軸姿態(tài)傳感器，集成了3 軸陀螺儀和3 軸加速度計(jì)。通過傳感器輸出的四元數(shù)計(jì)算機(jī)器人此時(shí)相對于地面的俯仰角、橫滾角、偏航角。得到機(jī)器人底盤的姿態(tài)角，可以確定機(jī)器人當(dāng)前狀態(tài)，也可以在自動(dòng)控制中控制發(fā)射管與水平面之間的角度保持不變。

2.1.5 發(fā)射控制結(jié)構(gòu)

彈丸發(fā)射部分使用摩擦輪給彈丸提供動(dòng)力從發(fā)射管射出。底座上的舵機(jī)控制發(fā)射管角度，發(fā)射管中的直流無刷電機(jī)帶動(dòng)摩擦輪轉(zhuǎn)動(dòng)，使用光電對管檢測在彈艙出口處與發(fā)射管入口處是否存在彈丸，從而控制是否上彈。

2.2 控制算法

2.2.1 底盤電機(jī)速度控制

機(jī)器人的移動(dòng)速度與遙控器搖桿模擬量相關(guān)，模擬量越大則電機(jī)速度越大，為了使機(jī)器人更快更穩(wěn)定地達(dá)到遙控器需求的目標(biāo)速度，采用增量PID對電機(jī)速度進(jìn)行控制。增量式離散PID 公式：

式中：e(k)為本次偏差值；e(k-)1為上一次的偏差值；pwm為增量輸出。

讀取編碼器的值，積分后得到速度值，之后將得到的速度值與目標(biāo)速度值代入PID 函數(shù)中計(jì)算后得到需要的速度對應(yīng)的PWM 值，最后將PWM 值存入寄存器進(jìn)行電機(jī)速度控制。

2.2.2 機(jī)器人姿態(tài)獲取與發(fā)射管角度控制

為防止機(jī)器人在不平整地面行進(jìn)時(shí)攝像頭跟隨機(jī)器人動(dòng)作造成的視角變化，對機(jī)器人底盤進(jìn)行姿態(tài)采集，通過對底盤俯仰角變化值進(jìn)行計(jì)算，使發(fā)射管保持受遙控器調(diào)整后與水平面的角度。

在機(jī)器人的姿態(tài)獲取中將原始角速度數(shù)據(jù)轉(zhuǎn)換為四元組數(shù)據(jù)，完成歐拉角計(jì)算。

DMP 輸出四元數(shù)為Q30 格式的數(shù)據(jù)，比浮點(diǎn)數(shù)放大了230倍。在使用時(shí)轉(zhuǎn)換為浮點(diǎn)數(shù)[4]。數(shù)據(jù)轉(zhuǎn)換公式為：

代入歐拉角公式，得俯仰角為：

翻滾角為：

偏航角為：

經(jīng)過計(jì)算得到歐拉角。

在得到機(jī)器人底盤俯仰角后，根據(jù)前一次與后一次的角度變化，計(jì)算出控制舵機(jī)角度的PWM，將得到的PWM 賦值給舵機(jī)控制函數(shù)中完成對發(fā)射管角度的控制。

2.2.3 自動(dòng)控制

使用攝像頭對火焰進(jìn)行識別，判斷火焰在視頻采集范圍內(nèi)的坐標(biāo)，主控模塊根據(jù)火焰坐標(biāo)控制機(jī)器人進(jìn)行原地旋轉(zhuǎn)并調(diào)整發(fā)射管角度來完成對火焰的瞄準(zhǔn)，并對機(jī)器人與火焰之間的距離進(jìn)行獲取。

如圖3 所示，點(diǎn)P 為待測物體上的一點(diǎn)，OR、OT為2 個(gè)攝像頭的光心，點(diǎn)P'和P"為P 點(diǎn)在感光器上的成像點(diǎn)，f為攝像頭焦距，B為2 攝像頭中心距，Z為所求深度[5]。

圖3 雙目測距原理圖

設(shè)P'到P"的距離為dis，得：

根據(jù)相似三角形原理：

得：

為防止機(jī)器人與火焰距離過近導(dǎo)致器件受損或快速升溫，在程序中設(shè)定機(jī)器人與火焰之間的安全距離為1 m。自動(dòng)控制下，若機(jī)器人判斷自身距離火焰在1 m 內(nèi)，首先瞄準(zhǔn)火焰，之后向后移動(dòng)至距離火焰1 m 處，之后對火焰發(fā)射消防彈丸。

3 調(diào)試結(jié)果

3.1 火焰識別與位置控制

機(jī)器人使用攝像頭對火焰進(jìn)行識別，利用雙目攝像頭測距原理對機(jī)器人與火源間的距離進(jìn)行檢測。

如圖4、圖5 為攝像頭對火焰識別圖。與原圖進(jìn)行對比可以看出，攝像頭在識別火焰時(shí)，只有橘紅色部分為白，背景及其他部分為黑色。

圖4 火焰識別圖

圖5 火焰原始圖像

在機(jī)器人前放置手電等光源進(jìn)行測試，機(jī)器人沒有對光源進(jìn)行瞄準(zhǔn)動(dòng)作，故機(jī)器人只對火焰進(jìn)行瞄準(zhǔn)動(dòng)作而不對手電筒等光源發(fā)生反映。

雙目測距在經(jīng)過標(biāo)定、校正、雙目匹配后，才能進(jìn)行深度計(jì)算。

在調(diào)試中，首先使用紅色色塊作為被測物體，進(jìn)行水平前移、后移的距離檢測并記錄。結(jié)果如表1 所示。

表1 機(jī)器人與目標(biāo)距離記錄 m

之后固定目標(biāo)物體，分別將機(jī)器人放置在不同位置，讓機(jī)器人自動(dòng)調(diào)整與目標(biāo)之間距離，記錄機(jī)器人移動(dòng)后位置[6]。結(jié)果如表2 所示。

表2 機(jī)器人位置記錄 m

經(jīng)反復(fù)測試證明，機(jī)器人可以完成對火焰的識別、瞄準(zhǔn)，并可以控制機(jī)器人保持在目標(biāo)1 m 左右距離。

3.2 發(fā)射管角度與彈丸發(fā)射調(diào)試

機(jī)器人在行進(jìn)過程中，發(fā)射管保持遙控器設(shè)定的發(fā)射管與水平面的角度。

首先將發(fā)射管與水平面角度設(shè)定為30°，之后改變機(jī)器人底盤及發(fā)射管與水平面之間的角度，并進(jìn)行記錄[6]。結(jié)果如表3 所示。

表3 發(fā)射管角度記錄 (°)

從表3 中可以看出，隨著機(jī)器人底盤角度的改變，發(fā)射管與水平面的角度基本保持一致，可以在機(jī)器人運(yùn)行過程中保證視角的穩(wěn)定。

在彈丸發(fā)射調(diào)試中，機(jī)器人與目標(biāo)之間的距離為1 m，發(fā)射管距離地面約為25 cm，彈丸重量約0.2 kg。

機(jī)器人使用自動(dòng)控制模式進(jìn)行瞄準(zhǔn)并發(fā)射彈丸，記錄彈丸發(fā)射距離并將落點(diǎn)與目標(biāo)之間的距離進(jìn)行記錄。結(jié)果如表4 所示。

表4 彈丸發(fā)射距離記錄 m

從表4 中可以看出，機(jī)器人可以在自動(dòng)控制下，自動(dòng)瞄準(zhǔn)目標(biāo)并較精準(zhǔn)地發(fā)射彈丸擊中目標(biāo)。

4 結(jié)論

基于STM32 單片機(jī)設(shè)計(jì)一種偵察消防機(jī)器人。機(jī)器人采用麥克納姆輪構(gòu)成移動(dòng)平臺，在發(fā)射裝置中利用摩擦輪加速消防彈從發(fā)射管射出。機(jī)器人能夠通過攝像頭對火焰進(jìn)行識別，并利用雙目測距原理測量火焰與機(jī)器人之間的距離，完成自動(dòng)控制機(jī)器人與火焰之間距離并自動(dòng)瞄準(zhǔn)、發(fā)射消防彈的功能。對火焰識別、距離檢測、機(jī)器人自動(dòng)調(diào)節(jié)與目標(biāo)之間的距離、保持發(fā)射管角度以及彈丸發(fā)射功能進(jìn)行調(diào)試，并證明各種功能的實(shí)用性。

這種消防機(jī)器人改變了目前一般消防機(jī)器人的移動(dòng)方式，使消防機(jī)器人體型更小、移動(dòng)更靈活，也改變了偵察型消防機(jī)器人只能進(jìn)行信息獲取而不能對火情進(jìn)行抑制的情況。增加了機(jī)器人的自動(dòng)控制功能，降低了機(jī)器人的操作難度。