四旋翼飛行器雙環(huán)姿態(tài)穩(wěn)定算法研究

宋建國(guó)，吳鑫磊，謝敏波

(北京工業(yè)大學(xué) 數(shù)字社區(qū)教育部工程研究中心和城市軌道交通北京實(shí)驗(yàn)室，北京 100124)

0 引 言

四旋翼飛行器現(xiàn)在廣泛應(yīng)用于生活的各方各面，包括工業(yè)、農(nóng)業(yè)、商業(yè)、軍事偵察等應(yīng)用領(lǐng)域，也是其成為了現(xiàn)在研究的熱點(diǎn)話題。其中四旋翼飛行器的姿態(tài)控制算法以及姿態(tài)融合解算算法是四旋翼飛行器的研究的重中之重，姿態(tài)控制算法將直接影響飛行器的姿態(tài)調(diào)整的快速與魯棒性，是飛行器實(shí)時(shí)平穩(wěn)飛行的核心。

而姿態(tài)融合結(jié)算算法則是飛行器姿態(tài)控制的前提，將直接影響飛行控制的可靠性、抗干擾性能以及飛行控制實(shí)現(xiàn)的難易程度[1-3]。目前市場(chǎng)上對(duì)于飛行器控制主要有兩個(gè)方向，其一是基于光纖陀螺儀作為硬件基礎(chǔ)的飛行控制器，其精度較高但是價(jià)格極其昂貴難以得到推廣[4-5]。而基于MEMS傳感器的飛行器由于價(jià)格相對(duì)優(yōu)勢(shì)較大得以廣泛推廣，本文則是以后者為基礎(chǔ)進(jìn)行一系列的研究。

基于航向姿態(tài)參考系統(tǒng)(Attitude and Heading Reference System，AHRS)所提供的基于自身X、Y、Z坐標(biāo)軸下的橫滾角、航向角、俯仰角等一系列姿態(tài)信息對(duì)飛行器自身當(dāng)前姿態(tài)進(jìn)行精準(zhǔn)的融合分析，并通過(guò)濾波處理將姿態(tài)數(shù)據(jù)整合到其自身的雙閉環(huán)控制系統(tǒng)中作為姿態(tài)控制輸入初始值，經(jīng)過(guò)雙閉環(huán)控制算法對(duì)飛行器姿態(tài)做出快速準(zhǔn)確運(yùn)算，從而實(shí)現(xiàn)了穩(wěn)定輸出誤差相對(duì)較小的姿態(tài)數(shù)據(jù)作為初始值以供雙閉環(huán)控制算法應(yīng)用，進(jìn)而達(dá)到四旋翼飛行器穩(wěn)定高效控制的要求。

1 四旋翼飛行器模型分析

1.1 物理機(jī)體模型

圖1為四旋翼飛行器的物理機(jī)體模型，從結(jié)構(gòu)上分析其機(jī)體模型分為十字模式和X模式，兩種結(jié)構(gòu)差別不大，但是一般常常采用X模型，方便航拍拓展。

圖1 物理機(jī)體模型

俯仰角(pitch)：機(jī)體坐標(biāo)系X軸與水平面的夾角，圍繞X軸旋轉(zhuǎn)。當(dāng)X軸的正半軸 位于過(guò)坐標(biāo)原點(diǎn)的水平面之上(抬頭)時(shí)，俯仰角為正，否則為負(fù)[6]。

偏航角(yaw)：機(jī)體坐標(biāo)系X軸在水平面上投影與地面坐標(biāo)系Xg軸之間的夾角，圍繞Y軸旋轉(zhuǎn)。機(jī)頭右偏航為正，反之為負(fù)。

滾轉(zhuǎn)角(roll)：機(jī)體坐標(biāo)系Z軸與通過(guò)機(jī)體Z軸的鉛垂面間的夾角，圍繞Z軸旋轉(zhuǎn)。 機(jī)體向右滾為正，反之為負(fù)。

四旋翼飛行器的某一時(shí)刻的姿態(tài)可以用歐拉角來(lái)表示[7]。其中當(dāng)機(jī)體懸停于空中時(shí)，4個(gè)旋翼所產(chǎn)生的升力與飛行器自身的重力相等，而四旋翼中的M1與M3電機(jī)順時(shí)針旋轉(zhuǎn)，M2與M4電機(jī)逆時(shí)針旋轉(zhuǎn)，各個(gè)旋翼的轉(zhuǎn)動(dòng)慣量相等，從而可以很好的抵消其自身的的自旋，從而使得飛行器更加方便控制[8-9]。

該飛行器采用9軸傳感器MPU9250來(lái)對(duì)四旋翼飛行器的角速度以及加速度進(jìn)行采集，并通過(guò)數(shù)據(jù)融合濾波對(duì)原始數(shù)據(jù)進(jìn)行處理，使得其作為飛行器控制輸入計(jì)算時(shí)更加準(zhǔn)確，以便于實(shí)現(xiàn)飛行器更加穩(wěn)定有效的響應(yīng)控制。

1.2 飛行器的雙閉環(huán)控制模型

四旋翼飛行器簡(jiǎn)化的雙閉環(huán)控制框圖如圖2所示，該結(jié)構(gòu)運(yùn)用了9軸傳感器MPU9250來(lái)獲取原始數(shù)據(jù)，經(jīng)過(guò)一系列的濾波校正與融合來(lái)解算飛行器的實(shí)時(shí)姿態(tài)，即飛行器的實(shí)時(shí)角度與角速度。

圖2 雙閉環(huán)控制框圖

之所以選用雙閉環(huán)PID對(duì)四旋翼飛行器的飛行姿態(tài)進(jìn)行控制，是因?yàn)閭鹘y(tǒng)的角度單環(huán)PID控制算法僅僅考慮了飛行器的飛行角度，這樣飛行器在飛行過(guò)程中的抗擾動(dòng)能力很差，而本文采用了雙閉環(huán)的PID控制算法，這在很大程度上增加了飛行的穩(wěn)定性，有效的提高了飛行器的控制品質(zhì)，所謂的雙閉環(huán)控制即使得角度環(huán)作為外環(huán)，角速度環(huán)作為內(nèi)環(huán)，這使得該控制方法可以控制更多的飛行變量，從而飛行器在外界惡劣環(huán)境下的適應(yīng)能力變得更強(qiáng)。雙閉環(huán)PID控制的程序流程圖如圖3所示。

圖3 程序流程圖

其中主控制器對(duì)PID各個(gè)控制參數(shù)進(jìn)行初始化，并對(duì)姿態(tài)傳感器進(jìn)行自檢與零漂校正，然后以一定周期對(duì)傳感器姿態(tài)原始數(shù)據(jù)進(jìn)行采集并給定初始油門，然后經(jīng)過(guò)濾波運(yùn)算將角度信息送給外環(huán)角度環(huán)輸入并與遙控端的給定值進(jìn)行做差處理，然后再將外環(huán)輸出值與角速度值進(jìn)行做差處理作為角速度內(nèi)環(huán)輸入，從而內(nèi)環(huán)輸出PWM波占空比控制量以方便控制各個(gè)電機(jī)的轉(zhuǎn)速進(jìn)而來(lái)控制飛行器的實(shí)時(shí)姿態(tài)。在其中還增加了無(wú)人機(jī)觸地判斷算法來(lái)及時(shí)判斷飛機(jī)是否準(zhǔn)確降落，以及擴(kuò)展卡爾曼濾波算法對(duì)數(shù)據(jù)姿態(tài)進(jìn)行融合解算更新。從而可以準(zhǔn)確進(jìn)行一次飛行器姿態(tài)的更新控制。

2 飛行器的濾波融合算法

2.1 通過(guò)加速計(jì)與磁力計(jì)進(jìn)行數(shù)據(jù)解析計(jì)算求出歐拉角

本文先通過(guò)三軸加速度計(jì)來(lái)計(jì)算出飛行器的橫滾角γ和俯仰角θ，先使得飛行器初始位置處于水平靜置狀態(tài)，并假設(shè)當(dāng)?shù)氐闹亓铀俚膅為1g，此時(shí)

四旋翼飛行器所固聯(lián)的載體坐標(biāo)系b系的三軸加速度ax,ay,az為重力加速的g在其載體坐標(biāo)系上的分量[10-13]。

(1)

(2)

(3)

(4)

由式(1)，式(2)，式(3)可以推出橫滾角γ和俯仰角θ的計(jì)算公式：

(5)

(6)

(7)

最后經(jīng)過(guò)計(jì)算可以得到：

(8)

歐拉角與四元數(shù)之間可以進(jìn)行互相轉(zhuǎn)換，在進(jìn)行飛行器姿態(tài)解算中本文都運(yùn)用了四元數(shù)進(jìn)行算法運(yùn)算，因?yàn)樗脑獢?shù)在運(yùn)算期間更加方便快捷，這節(jié)省了大量的CPU資源以方便處理其他事件，最終再將融合出的四元數(shù)數(shù)據(jù)轉(zhuǎn)換為歐拉角來(lái)進(jìn)行串級(jí)PID控制。

2.2 擴(kuò)展卡爾曼濾波算法在飛行器的應(yīng)用

在進(jìn)行數(shù)據(jù)解算處理時(shí)需要對(duì)周圍磁場(chǎng)的干擾以及陀螺儀經(jīng)過(guò)長(zhǎng)時(shí)間的積分后所累加的積分誤差進(jìn)行處理與校正[14-15]。校正時(shí)通過(guò)加速度計(jì)的加速度數(shù)據(jù)以及磁力計(jì)的采集數(shù)據(jù)通過(guò)四維拓展卡爾曼濾波對(duì)數(shù)據(jù)進(jìn)行觀測(cè)更新，從而使得數(shù)據(jù)變得更加真實(shí)有效。

因?yàn)樵谶M(jìn)行飛行器姿態(tài)實(shí)時(shí)解算時(shí)運(yùn)用歐拉角會(huì)涉及到大量的三角函數(shù)運(yùn)算，為了保證更新速度以及保持良好的實(shí)時(shí)性，在進(jìn)行時(shí)間更新與觀測(cè)更新之前，先把歐拉角轉(zhuǎn)換為四元數(shù)。其轉(zhuǎn)換關(guān)系如:

(9)

由上面計(jì)算所得到的初始?xì)W拉角數(shù)據(jù)經(jīng)過(guò)式(9)轉(zhuǎn)換后就得到了初始的四元數(shù)，然后在運(yùn)用擴(kuò)展卡爾曼濾波對(duì)狀態(tài)進(jìn)行時(shí)間更新和觀測(cè)更新。

時(shí)間更新：

四旋翼飛行器控制系統(tǒng)的自身姿態(tài)的狀態(tài)方程如:

(10)

(11)

協(xié)方差矩陣Pk預(yù)測(cè)如：

Pk=APk-1A-T+W

(12)

式中,W為飛行器的過(guò)程激勵(lì)噪聲協(xié)方差。由以上各式我們就可以得到飛行器在飛行過(guò)程中隨著時(shí)間變化飛行器在各個(gè)時(shí)間段的飛行姿態(tài)，即飛行器的狀態(tài)變量Q。觀測(cè)更新：

飛行器的觀測(cè)更新是由加速度計(jì)與磁力計(jì)所采集到數(shù)據(jù)和本體坐標(biāo)系中的重力加速度與地磁場(chǎng)的參考矢量變換到導(dǎo)航坐標(biāo)系下的數(shù)據(jù)進(jìn)行比較，從而對(duì)飛行器的姿態(tài)進(jìn)行觀測(cè)更新[16]。而從飛行器自身坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)移矩陣如式(13)所示：

(13)

由此可知重力加速度與地磁場(chǎng)的參考矢量變換到導(dǎo)航坐標(biāo)系下的計(jì)算公式為

(14)

(15)

式中,Gb，Mb分別為飛行器的重力加速度參考矢量和磁場(chǎng)參考矢量。卡爾曼增益矩陣Kk=Pk-1HT(HPk-1HT+R)-1，其中R為三維觀測(cè)噪聲協(xié)方差矩陣，H為Gn和Mn對(duì)Q求偏導(dǎo)的雅可比矩陣，由此可以得出重力加速度和磁場(chǎng)的觀測(cè)更新公式為

(16)

(17)

在式(16)和式(17)中Zk分別代表重力加速度觀測(cè)更新和磁場(chǎng)觀測(cè)更新時(shí)各傳感器所測(cè)量得到的三維矢量Zk=[axayaz]T和Zk=[MxMyMz]T。而在觀測(cè)更新下的協(xié)方差更新如：

Pk=(1-KkH)Pk-1

(18)

數(shù)據(jù)融合濾波程序流程圖如圖4所示。

圖4 數(shù)據(jù)融合濾波程序流程圖

3 仿真實(shí)驗(yàn)

在Matlab/Simulik仿真環(huán)境中進(jìn)行實(shí)驗(yàn)分析，在給定一個(gè)初始期望的橫滾角度后對(duì)比的橫滾角控制輸出曲線圖如圖5、圖6所示，同時(shí)對(duì)直接采集的原始數(shù)據(jù)與經(jīng)過(guò)卡爾曼濾波融合處理后的數(shù)據(jù)分別應(yīng)用到四旋翼飛行器的雙閉環(huán)控制系統(tǒng)中去，然后再對(duì)飛行器從靜態(tài)到運(yùn)動(dòng)狀態(tài)再回到初始狀態(tài)的姿態(tài)角進(jìn)行對(duì)比，其對(duì)比曲線圖如圖7、圖8、圖9所示。

圖5 傳統(tǒng)PID控制曲線

圖6 雙閉環(huán)PID控制曲線

在圖5與圖6的對(duì)比中我們可以明顯看出運(yùn)用雙閉環(huán)控制之后所得到的閉環(huán)控制系統(tǒng)的輸出曲線從一個(gè)給定值再到一個(gè)穩(wěn)定值的調(diào)節(jié)曲線更加平滑，控制效果更加穩(wěn)定，震蕩性更小，從圖中可以明顯看出雙閉環(huán)PID控制系統(tǒng)比單環(huán)控制系統(tǒng)的響應(yīng)速度快了將近4倍，相對(duì)比而言雙閉環(huán)控制系統(tǒng)在控制四旋翼飛行器的過(guò)程中所體現(xiàn)的優(yōu)勢(shì)更加明顯。

圖7 橫滾角對(duì)比曲線

圖8 俯仰角對(duì)比曲線

圖9 航向角對(duì)比曲線

在圖7、圖8、圖9中的橫滾角、俯仰角、航向角在純陀螺儀作用下隨著時(shí)間的增長(zhǎng)姿態(tài)角再次回到靜止?fàn)顟B(tài)下后會(huì)產(chǎn)生漂移誤差，然而在運(yùn)用了數(shù)據(jù)融合濾波之后姿態(tài)角的變化曲線如圖中AHRS線所示，姿態(tài)角的誤差得到了有效改善，使得飛行器在運(yùn)動(dòng)過(guò)程所產(chǎn)生的動(dòng)態(tài)誤差得以修正，從而使得飛行器的控制更加準(zhǔn)確高效。

4 結(jié) 論

四旋翼飛行器在雙閉環(huán)系統(tǒng)下能夠?qū)崟r(shí)有效的保證飛行器的正常飛行，并且相對(duì)于單一的角度環(huán)而言，該雙閉環(huán)控制系統(tǒng)的抗干擾能力以及魯棒性都有實(shí)質(zhì)性的提高。而在雙閉環(huán)系統(tǒng)的基礎(chǔ)上又增添了擴(kuò)展卡爾曼濾波，這種姿態(tài)調(diào)整復(fù)合控制算法使得控制系統(tǒng)最終輸出的控制量更加貼合真實(shí)值，準(zhǔn)確性和可靠性相對(duì)于單一的雙閉環(huán)控制又有了進(jìn)一步提高。本文將雙閉環(huán)控制系統(tǒng)與擴(kuò)展卡爾曼濾波算法進(jìn)行了互補(bǔ)融合運(yùn)用，最終將這種復(fù)合控制方法運(yùn)用到了飛行器的姿態(tài)調(diào)整控制中，最終使得飛行器在進(jìn)行前后運(yùn)動(dòng)、懸停運(yùn)動(dòng)以及偏航運(yùn)動(dòng)時(shí)比普通的飛行器控制方式更加的穩(wěn)定高效。