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

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

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

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

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

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

1 姿態(tài)計算兩點前提假設(shè)

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

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

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

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

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

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

步驟一：四元素初始化

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

步驟二：加速度計修正角速度

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

(1)

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

(2)

步驟三：四元素更新

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

(3)

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

(4)

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

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

(5)

至此，整個IMU解算完成。

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

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

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

圖4 解算俯仰角與加計的俯仰角(動態(tài))

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

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

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

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

步驟一：確定磁力計參考矢量

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

(6)

獲得磁力計的參考矢量

(7)

步驟二：獲得磁力計方向誤差

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

(8)

得到磁力計方向誤差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)解算方法相同。

至此，整個MARG解算完成。

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

圖8 磁力計觀測下的偏航角(動態(tài))

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

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

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

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

步驟一：DCM初始化

(13)

作為DCM的初始值。

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

(14)

帶入方向余弦矩陣得

(15)

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

(16)

(17)

(18)

獲取加速度計方向誤差ea。

(19)

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

步驟三：更新并正交化DCM

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

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

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

(20)

θ=arctan(-rxz)

(21)

(22)

至此，整個MEMS解算完成。

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

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

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

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

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

(23)

將其進行單位化處理

(24)

(25)

(26)

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

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

(27)

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

(28)

(29)

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

bωy(n-1))dt

(30)

同理可得

(31)

步驟三：更新Rg

對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)的正負確定。

(40)

步驟五：規(guī)范化

(41)

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

(42)

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

至此，整個IMU解算完成。

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

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

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

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

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

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

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

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

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

6 結(jié)論

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