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

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

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

1 定位系統(tǒng)

1.1 計(jì)算偏航角

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

圖1 歐拉角

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

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

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

1.2 追蹤導(dǎo)向

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

超聲波通過(guò)計(jì)算發(fā)出信號(hào)與接收到返回信號(hào)時(shí)間t來(lái)測(cè)距離D，其公式如下：

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

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

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

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

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

PID[4]基本算法：

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

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

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

2 系統(tǒng)仿真

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

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

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

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

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

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

圖2 模擬響應(yīng)

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

圖3 消防車(chē)角度偏移變化曲線

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

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

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

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

4 結(jié)語(yǔ)

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