智能預(yù)警消防車定位研究

趙建峰 莊乾輝 蔡依霖 李翔宇 趙 婧

南京工程學(xué)院自動化學(xué)院 江蘇南京 211167

近年來，隨著經(jīng)濟(jì)和科技的快速發(fā)展，各種大型石油化工企業(yè)密集，隧道、地鐵公共交通設(shè)施等不斷增多，油品燃?xì)?、毒氣泄漏爆炸事故時有發(fā)生。滅火機(jī)器人進(jìn)入易燃易爆、有毒、缺氧、濃煙等危險災(zāi)害事故現(xiàn)場進(jìn)行救援滅火，有效地解決了消防人員面臨的人身安全問題，并能對火災(zāi)情況進(jìn)行科學(xué)判斷，高效滅火。本文設(shè)計了一種基于STM32處理器并帶有多傳感器的智能消防小車，可實(shí)現(xiàn)火源的精準(zhǔn)定位及滅火。

1 定位系統(tǒng)

1.1 計算偏航角

計算姿態(tài)角，要用到歐拉角，如圖1所示。

圖1 歐拉角

歐拉角是用來確定定點(diǎn)轉(zhuǎn)動剛體位置的一組獨(dú)立角參量，由章動角β、旋進(jìn)角(即進(jìn)動角)α和自轉(zhuǎn)角γ組成。任何方向都可以通過3個基本旋轉(zhuǎn)角來實(shí)現(xiàn)，即圍繞坐標(biāo)系的軸旋轉(zhuǎn)。歐拉角可以通過這3個旋轉(zhuǎn)角來定義，它們也可以通過元素定義的幾何形狀和幾何定義來表明。3個角的基本旋轉(zhuǎn)可以是外在的，或者固有的。不同的需求或場合可以使用不同的旋轉(zhuǎn)軸組來定義歐拉角。因此，任何使用歐拉角分析的模型都應(yīng)該在定義之后進(jìn)行研究。在此項(xiàng)目里，歐拉角只針對姿態(tài)角中的偏航角進(jìn)行分析計算[1,2]。

根據(jù)捷聯(lián)慣性導(dǎo)航原理，預(yù)警車的姿態(tài)角就是載體坐標(biāo)系b相對于導(dǎo)航坐標(biāo)系n的方位關(guān)系。本文定義載體坐標(biāo)系OXbYbZb，該坐標(biāo)系與車體固聯(lián)，原點(diǎn)選為車體質(zhì)心Xb軸沿車體橫軸指向右，Yb軸沿車體縱軸指向前，Zb軸垂直指向車體上方，且構(gòu)成右手坐標(biāo)系。定義地理坐標(biāo)系OXnYnZn作為導(dǎo)航坐標(biāo)系，該坐標(biāo)系的原點(diǎn)在車體的質(zhì)心Xn，Yn和Zn軸分別從原點(diǎn)出發(fā)指向東、北和天向，簡稱導(dǎo)航坐標(biāo)系ENU。

設(shè)載體坐標(biāo)系b是由導(dǎo)航坐標(biāo)系n按照Z→Y→X順序，分別旋轉(zhuǎn)ψ→δ→θ角度得到。其中ψ為滾轉(zhuǎn)角；δ為偏航角；θ為俯仰角。本系統(tǒng)只需要用到偏航角δ，直接套用歐拉角微分方程來解算歐拉角姿態(tài)：

1.2 追蹤導(dǎo)向

智能預(yù)警消防車通過自身旋轉(zhuǎn)360°，并利用超聲波和火焰?zhèn)鞲衅鞯膾呙鑱矶惚苷系K并尋找火源。每當(dāng)消防車啟動，其身可視為一條線段，水泵視為箭頭，指向方向即為0°。每旋轉(zhuǎn)1°，停止車體并掃描前方，記錄周圍環(huán)境。

超聲波通過計算發(fā)出信號與接收到返回信號時間t來測距離D，其公式如下：

火焰?zhèn)鞲衅魍ㄟ^紅外探測火源，其輸出信號轉(zhuǎn)換值V(模數(shù)轉(zhuǎn)換)與離火源距離D成反比關(guān)系。受環(huán)境影響，其反比關(guān)系成非線性。此處忽略誤差，將其近似看成線性方程，公式如下：

通過傳感器獲得的障礙物距離和火源距離，結(jié)合姿態(tài)傳感器的偏航角，即可描繪以車為原點(diǎn)時360°范圍內(nèi)的所有物體。將火源位置一一標(biāo)出，從近到遠(yuǎn)依次進(jìn)行滅火。

1.3 PID調(diào)節(jié)

由于機(jī)械結(jié)構(gòu)和地形環(huán)境等因素，車體在自轉(zhuǎn)時會與設(shè)定角度產(chǎn)生偏差。前往火源時會與設(shè)定路線產(chǎn)生偏差，不能停止在火焰前方最佳距離處，因此需要采用閉環(huán)自動控制技術(shù)。

目前，閉環(huán)自動控制技術(shù)主要是基于反饋的概念以減少不確定性。反饋理論的要素由3個部分組成：測量、比較和執(zhí)行。測量被控變量的實(shí)際值，并與期望值相比較，用這個偏差來糾正系統(tǒng)的響應(yīng)，執(zhí)行調(diào)節(jié)控制。由于單級PID穩(wěn)定性不高[3]，本項(xiàng)目決定采用串級PID方式準(zhǔn)確控制車的自轉(zhuǎn)角度和直線行駛。車前端能夠在給定角度值的小范圍內(nèi)波動(±0.4°以內(nèi))時，車能近似直線行駛。

PID[4]基本算法：

比例項(xiàng)Kpe(t)：調(diào)整穩(wěn)態(tài)誤差，值越大則穩(wěn)態(tài)誤差越小，但系統(tǒng)穩(wěn)定性會降低。

本文中定義系統(tǒng)偏差：

外環(huán)中，u(t)是角度期望值，y(t)為當(dāng)前角度值。通過第一次外環(huán)PID先積分后微分的運(yùn)算，得出角速度環(huán)期望值u(t)'和角速度值y(t)'。然后再經(jīng)過第二次內(nèi)環(huán)PID先積分后微分的運(yùn)算得到需要的輸出值。

2 系統(tǒng)仿真

為了驗(yàn)證理論分析的真實(shí)性和準(zhǔn)確性，利用MatLab[5]數(shù)字仿真軟件，對算法效果進(jìn)行精準(zhǔn)度和響應(yīng)速率的仿真。

對兩級PID進(jìn)行仿真，預(yù)設(shè)外環(huán)常量：

內(nèi)環(huán)常量：

經(jīng)過MatLab多次仿真實(shí)驗(yàn)，得出最優(yōu)外環(huán)常量：

內(nèi)環(huán)常量：

當(dāng)設(shè)置期望角度為1°時，算法響應(yīng)速率如圖2所示。

圖2 模擬響應(yīng)

預(yù)估采樣時間為0.15 s，但如圖2測試結(jié)果顯示，在開始時會有0.3°的超調(diào)，但0.2 s后會達(dá)到平穩(wěn)，因此0.2 s是最好的采樣時間。這樣可以在系統(tǒng)開始響應(yīng)之前，獲得更多時間來采樣。當(dāng)設(shè)置期望角度為1°時，消防車實(shí)際行駛角度偏移變化曲線如圖3所示。

圖3 消防車角度偏移變化曲線

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

此實(shí)驗(yàn)?zāi)康脑谟跍y試預(yù)警車追蹤火源、停車滅火時車和與火源的距離。受車身長度和滅火裝置的推動力影響，最佳滅火位置為距火源點(diǎn)15 cm處。

通過5組實(shí)驗(yàn)對比，第一次關(guān)閉預(yù)警車的PID運(yùn)算功能，觀察其停止位置距離火源距離與設(shè)定值(15cm)之間的偏差ΔD。第二次實(shí)驗(yàn)開啟預(yù)警車PID運(yùn)算功能，與第一次實(shí)驗(yàn)進(jìn)行對比，觀察算法精準(zhǔn)度。5組實(shí)驗(yàn)中，第一次無PID算法時最大偏差值為-5 cm，最小偏差值為±2 cm。而有PID算法時，平均誤差只有±1 cm，大大提高了定位精準(zhǔn)(見表1)。

表1 算法優(yōu)化結(jié)果

4 結(jié)語

生活中存在各種安全隱患，層出不窮的安全事故不僅給人們帶來了財產(chǎn)上的損失，更造成了心靈上的傷害。近年來，國家大力研發(fā)新型智能防火機(jī)器人來降低損失，造福人類。本文介紹的多功能智能預(yù)警消防車正符合以上需求，具有結(jié)構(gòu)簡單、成本低廉、開放性好、功能性強(qiáng)、效率高等優(yōu)點(diǎn)，具有重要的研發(fā)價值。