院老虎，連冬杉，盧聯(lián)杰，劉 義

(1.沈陽航空航天大學(xué) 航空宇航學(xué)院，沈陽 110136;2.湖北三江航天萬峰科技發(fā)展有限公司 測(cè)控技術(shù)研究室,湖北 孝感 432000;3.北京機(jī)械工業(yè)自動(dòng)化研究所有限公司 工業(yè)云平臺(tái)事業(yè)部,北京 100120)

近年來，隨著無人機(jī)的技術(shù)更新和需求增長(zhǎng)，預(yù)示著無人機(jī)將得到快速發(fā)展。四旋翼飛行器是四漿非共軸多旋翼飛行器的簡(jiǎn)稱。作為非共軸多旋翼飛行器的代表，與固定翼飛行器相比，它具有結(jié)構(gòu)簡(jiǎn)單、體積小、隱蔽性強(qiáng)、載荷高、可垂直起降、定點(diǎn)懸停和低速飛行等優(yōu)勢(shì)[1,2]，在民用和軍事領(lǐng)域均體現(xiàn)出極高的研究和應(yīng)用價(jià)值。四旋翼飛行器是無人機(jī)飛行器領(lǐng)域中的一個(gè)熱點(diǎn)研究課題[3,4]。

四旋翼飛行器雖然結(jié)構(gòu)簡(jiǎn)單，但它是一個(gè)多變量、強(qiáng)耦合的非線性欠驅(qū)動(dòng)系統(tǒng)[5,6]，機(jī)體對(duì)外界和自身的干擾表現(xiàn)的十分敏感。因此，獲得精確地姿態(tài)角數(shù)據(jù)顯得極為重要。姿態(tài)角解算精確程度直接影響著四旋翼飛行器在飛行過程中的靈活性、穩(wěn)定性、抗干擾性，所以對(duì)姿態(tài)解算算法的研究具有重大意義。

目前，歐拉角、方向余弦矩陣和四元素算法在姿態(tài)解算中得到了較多的關(guān)注與應(yīng)用[7-9]。歐拉角矩陣表示最為直觀，但存在奇點(diǎn)，不能實(shí)現(xiàn)對(duì)全部姿態(tài)的表示。方向余弦計(jì)算不存在奇點(diǎn)，然而矩陣有9個(gè)元素，解算時(shí)實(shí)際需要計(jì)算9個(gè)聯(lián)合微分方程，運(yùn)算量很大，不適合實(shí)時(shí)運(yùn)算。四元素法是一種全局的姿態(tài)描述方法，沒有奇點(diǎn)，且運(yùn)算為一般代數(shù)運(yùn)算，速度快，易于操作。

本文針對(duì)四旋翼飛行器慣導(dǎo)系統(tǒng)中的姿態(tài)解算問題，分別利用四元素、方向余弦矩陣和歐拉角算法對(duì)姿態(tài)角進(jìn)行姿態(tài)解算。把加速度計(jì)與陀螺儀當(dāng)成觀測(cè)量，利用叉積來修正陀螺儀角速度后，實(shí)時(shí)更新姿態(tài)，并對(duì)3種姿態(tài)解算的俯仰角在靜態(tài)與動(dòng)態(tài)進(jìn)行研究對(duì)比和最優(yōu)化設(shè)計(jì)。

1 姿態(tài)計(jì)算兩點(diǎn)前提假設(shè)

假設(shè)一：由于慣性系與機(jī)體系均為直角坐標(biāo)系，各軸相互正交，把坐標(biāo)系理解成剛體，考慮姿態(tài)角的變化，可以只研究坐標(biāo)系角度的變換關(guān)系。把其中一個(gè)坐標(biāo)系作平移，使其原點(diǎn)與另一個(gè)坐標(biāo)系原點(diǎn)重合。因此，可以將兩坐標(biāo)系間的空間角位置關(guān)系理解成剛體的定點(diǎn)轉(zhuǎn)動(dòng)。不管是用歐拉角、方向余弦矩陣，還是用四元素表示姿態(tài)都是基于這一假設(shè)出發(fā)。

假設(shè)二：四旋翼多處于懸停、勻速飛行狀態(tài)，且在四軸起飛、降落等加減速階段，其加速度相對(duì)重力加速度可以忽略不計(jì)。因此可以假設(shè)三軸加速度計(jì)僅僅測(cè)量重力加速度，其輸出為重力加速度在3個(gè)機(jī)體軸上的分量。在地球系上，重力加速度計(jì)的方向與大小是固定的?？梢詫⒅亓铀俣?地球坐標(biāo)系)投影到機(jī)體坐標(biāo)系，得到加速度計(jì)與水平面的角度關(guān)系。當(dāng)繞著重力軸旋轉(zhuǎn)三軸加速度計(jì)，其ADC值將不會(huì)改變，即加速度無法感知水平旋轉(zhuǎn)，只能觀測(cè)俯仰角與滾轉(zhuǎn)角，無法觀測(cè)偏航角。

2 四元素姿態(tài)解算

2.1 基于IMU的四元素姿態(tài)解算

基于IMU的四元素姿態(tài)解算如圖1所示。它利用機(jī)體坐標(biāo)系的加速度與上一時(shí)刻估算出的加速度估計(jì)值進(jìn)行叉積，得到方向誤差估計(jì)，通過低通濾波器去除加速度計(jì)的高頻信號(hào)，與機(jī)體角速度進(jìn)行互補(bǔ)后有效抑制了陀螺漂移。

圖1 基于IMU的四元素姿態(tài)解算流程圖

步驟一：四元素初始化

將[φ,θ,ψ]T=[0,0,0]T帶入[q0,q1,q2,q3]T=[1,0,0,0]T作為四元素的初始值。

步驟二：加速度計(jì)修正角速度

記反饋方向誤差量為ea，根據(jù)叉積的性質(zhì)：A×B=-B×A可得

(1)

通過PI控制器將ea與角速度融合來修正角速度

(2)

步驟三：四元素更新

根據(jù)一階龍格庫塔方程更新四元素的值(Δt為采樣周期)

(3)

步驟四：四元素規(guī)范化

(4)

步驟五：輸出姿態(tài)角

被輸出的三個(gè)姿態(tài)角如下

(5)

至此，整個(gè)IMU解算完成。

圖2展示了解算后的俯仰角與加速度計(jì)、陀螺儀的對(duì)應(yīng)關(guān)系。圖3為融合的滾轉(zhuǎn)角φ0與單獨(dú)用加速度計(jì)值解算的滾轉(zhuǎn)角φ1比較，靜態(tài)下φ0、φ1在小角度下是緊密貼合的，一旦角度增大，加速度計(jì)的精度就會(huì)變差，在2000采樣點(diǎn)附近甚至?xí)霈F(xiàn)類似脈沖等毛刺現(xiàn)象。而φ1在整個(gè)采樣過程中則表現(xiàn)地非常平滑。圖4與圖5是使用單獨(dú)的加計(jì)與四元素法解算出的俯仰角與滾轉(zhuǎn)角，動(dòng)態(tài)的加速度計(jì)受機(jī)體振動(dòng)的影響較為明顯。

圖2 解算俯仰角Accx與bωy(靜態(tài))

圖3 解算滾轉(zhuǎn)角與加計(jì)的滾轉(zhuǎn)角(靜態(tài))

圖4 解算俯仰角與加計(jì)的俯仰角(動(dòng)態(tài))

圖5 解算滾轉(zhuǎn)角與加計(jì)的滾轉(zhuǎn)角(動(dòng)態(tài))

2.2 基于MARG的四元素姿態(tài)解算

基于MARG的四元素姿態(tài)解算是在基于IMU的四元素姿態(tài)解算的基礎(chǔ)上，將加速度計(jì)、陀螺儀、磁力計(jì)數(shù)據(jù)相融合。流程圖如圖6所示。

圖6 基于MARG的四元素姿態(tài)解算

步驟一：確定磁力計(jì)參考矢量

將機(jī)體系的磁力計(jì)值轉(zhuǎn)換到慣性系下

(6)

獲得磁力計(jì)的參考矢量

(7)

步驟二：獲得磁力計(jì)方向誤差

將磁力計(jì)參考矢量通過加速度計(jì)校正的旋轉(zhuǎn)矩陣轉(zhuǎn)到機(jī)體系

(8)

得到磁力計(jì)方向誤差em

(9)

方向誤差ea、em通過PI控制器修正陀螺儀角速度，Kmp，Kmi用來控制修正陀螺儀的速度

emp=Kmpem

(10)

emi=emi+Kmiem

(11)

b′ω=b′ω+eap+eai+emp+emi

(12)

步驟三：更新四元素并輸出姿態(tài)角

與基于IMU的四元素姿態(tài)解算方法相同。

至此，整個(gè)MARG解算完成。

如圖7所示，將四軸靜置，加入磁力計(jì)觀測(cè)后，有效地抑制了偏航角的漂移現(xiàn)象。同時(shí)，靜置的磁力計(jì)精確度極低，在叉積法融合磁力計(jì)后，明顯降低了偏航角的波動(dòng)幅度。圖8是四軸懸停時(shí)觀測(cè)的偏航角，明顯發(fā)現(xiàn)融合后的偏航角波動(dòng)范圍較小。

圖8 磁力計(jì)觀測(cè)下的偏航角(動(dòng)態(tài))

圖7 磁力計(jì)觀測(cè)下的偏航角(靜態(tài))

3 基于MEMS的DCM姿態(tài)解算

方向余弦矩陣(DCM)不存在奇點(diǎn),沒有近似值,且包含從慣性系到機(jī)體系以及機(jī)體系到慣性系的所有信息，具體的流程圖如圖9所示。

圖9 基于MEMS的DCM姿態(tài)解算流程圖

步驟一：DCM初始化

(13)

作為DCM的初始值。

步驟二：獲得加速度計(jì)與磁力計(jì)方向誤差獲取磁力計(jì)參考矢量

(14)

帶入方向余弦矩陣得

(15)

根據(jù)慣性系下的磁力計(jì)值和地磁場(chǎng)的參考矢量得到磁力計(jì)的參考矢量

(16)

(17)

(18)

獲取加速度計(jì)方向誤差ea。

(19)

再根據(jù)轉(zhuǎn)換公式獲取磁力計(jì)方向誤差em。方向誤差ea，em帶入式(11)，通過PI控制器修正陀螺角速度，Kmp，Kmi用來控制修正陀螺儀的速度。

步驟三：更新并正交化DCM

將校正后的陀螺儀帶入旋轉(zhuǎn)矩陣變換率與角速度的轉(zhuǎn)換公式中更新DCM，根據(jù)Paul提出的一種強(qiáng)制正交法[9]對(duì)其正交化。

步驟四：歐拉角轉(zhuǎn)換

利用旋轉(zhuǎn)矩陣元素反解出歐拉角

(20)

θ=arctan(-rxz)

(21)

(22)

至此，整個(gè)MEMS解算完成。

圖2與圖10比較，在靜態(tài)時(shí)，兩者都表現(xiàn)出非常優(yōu)秀的濾波效果，解算后的俯仰角比直接用加計(jì)讀取的俯仰有更好的光滑程度。由圖11可知，在靜態(tài)、無線性振動(dòng)情況下，單獨(dú)使用加速度計(jì)可以反映出滾轉(zhuǎn)角，在使用DCM的融合解算后，加計(jì)的毛刺得到有效抑制。

圖10 解算俯仰角與和(靜態(tài))

圖11 解算滾轉(zhuǎn)角與加計(jì)的滾轉(zhuǎn)角(靜態(tài))

4 基于IMU的歐拉角姿態(tài)解算

由于高速旋轉(zhuǎn)的陀螺對(duì)線性振動(dòng)不敏感，因此，可以通過陀螺儀去修正加速度計(jì)，其輸出值為

(23)

將其進(jìn)行單位化處理

(24)

(25)

(26)

算法的流程圖如圖12所示。

圖12 基于IMU的歐拉角解算流程圖

(27)

步驟二：更新Axz(n)，Ayz(n)

(28)

(29)

陀螺儀測(cè)量的是Axz角度變化率，利用龍格庫塔二階方程更新

bωy(n-1))dt

(30)

同理可得

(31)

步驟三：更新Rg

對(duì)Rg作一些恒等變換，首先Rg的模為1：

(32)

則Rg在b′x軸的分量Rgx

(33)

由于以下等式

(34)

tan(Ayz)=Rgy/Rgz

(35)

(36)

則有

(37)

同理可得

(38)

結(jié)合式(32)又可以得到Rgz

(39)

Rgz的方向可由cos(Axz)的正負(fù)確定。

(40)

步驟五：規(guī)范化

(41)

步驟六：求出俯仰角θ′與滾轉(zhuǎn)角φ′

(42)

根據(jù)經(jīng)驗(yàn)可知，w在5～20區(qū)間內(nèi)可以得到滿意的效果。

至此，整個(gè)IMU解算完成。

圖13展示了俯仰角與加速度計(jì)、俯仰角與陀螺儀的對(duì)應(yīng)關(guān)系。比較加速度計(jì)俯仰角的峰值，發(fā)現(xiàn)俯仰角的峰值較加速度計(jì)滯后，這是以上兩個(gè)算法都沒出現(xiàn)過的現(xiàn)象。在小范圍內(nèi)，解算出的角速度還能比較精確，一旦角度增大，此算法就會(huì)出現(xiàn)掉度現(xiàn)象。因此，這種算法只能適用于小范圍的角度變化。圖14可以看出此算法起到了濾波作用，但解算效果遠(yuǎn)遠(yuǎn)沒有先前兩種算法好。在大滾轉(zhuǎn)角下，會(huì)出現(xiàn)掉度現(xiàn)象。

圖13 解算俯仰角與Accx和bωy(靜態(tài))

圖14 解算滾轉(zhuǎn)角與Accx和bωy(靜態(tài))

5 姿態(tài)算法對(duì)比

四軸在靜止?fàn)顟B(tài)下，除重力加速度外無其他加速度，符合算法的假設(shè)。因此，可以比較由加速度計(jì)解算的俯仰角、滾轉(zhuǎn)角與用四元素和DCM解算出的俯仰角與滾轉(zhuǎn)角。圖15是靜態(tài)時(shí)，同一姿態(tài)解分別用加計(jì)、方向余弦矩陣、四元素、歐拉角法解算出的俯仰角；圖16是觀測(cè)其在大負(fù)的俯仰角下的解算情況；圖17是觀測(cè)其在大正的俯仰角下的解算情況。比較后可以發(fā)現(xiàn)4種方法解算出的俯仰角在小角度時(shí)近似相等的。單獨(dú)用加計(jì)解算出的角度不夠光滑但能反應(yīng)角度的大小變化。當(dāng)使用3種算法融合后，角度就會(huì)變得柔和光滑。圖16可以觀察到，在俯仰角的負(fù)極值點(diǎn)處，4種方法解算出的角度存在明顯的不同。使用DCM能與加計(jì)很好的貼合，使用四元素解較加計(jì)解大，而使用歐拉解就出現(xiàn)較大的角度偏差。類似，由圖17可以觀察到在正極值點(diǎn)附近歐拉角法出現(xiàn)明顯的偏離，并且極值點(diǎn)出現(xiàn)滯后現(xiàn)象。DCM解算的效果最好，與加計(jì)解最為接近。

圖15 加速度計(jì)、四元素、方向余弦矩陣、歐拉角解算出的俯仰角(靜態(tài))

圖16 加速度計(jì)、四元素、方向余弦矩陣、歐拉角法解算出大負(fù)俯仰角(靜態(tài))

在四軸增穩(wěn)飛行時(shí)，其飛行加速度小，可以忽略不計(jì)，滿足算法的假設(shè)二。由圖18可以觀測(cè)到在動(dòng)態(tài)過程中，線性振動(dòng)已經(jīng)使得加速度計(jì)失真，無法像靜態(tài)過程單獨(dú)使用加計(jì)去觀測(cè)俯仰角。DCM與四元素解算后如圖19所示，在小角度下，兩者解算出的角度趨于一致，在大的角度以及風(fēng)速干擾下出現(xiàn)一些偏差，但兩者能反應(yīng)姿態(tài)的變化。在姿態(tài)控制中可以通過PID去消除這種偏差。

圖19 DCM、四元素解算俯仰角(動(dòng)態(tài))

6 結(jié)論

本文針對(duì)四旋翼飛行器慣導(dǎo)系統(tǒng)中的姿態(tài)解算問題，首先提出了兩點(diǎn)基本假設(shè)，并分別利用四元素、方向余弦矩陣、歐拉角等姿態(tài)表示法設(shè)計(jì)了姿態(tài)解算算法。把加速度計(jì)與陀螺儀當(dāng)成觀測(cè)量，叉積去修正陀螺儀角速度，實(shí)時(shí)更新姿態(tài)。最后，對(duì)方向余弦矩陣、四元素法、歐拉法做了動(dòng)態(tài)與靜態(tài)的分析，舍棄了歐拉法。選擇在靜態(tài)與動(dòng)態(tài)表現(xiàn)較好的方向余弦矩陣、四元素法對(duì)四軸進(jìn)行姿態(tài)解算。