應(yīng)用于多旋翼MAVs的姿態(tài)測量系統(tǒng)*

葉小嶺，杜 浩，王 偉，2

(1.南京信息工程大學(xué)信息與控制學(xué)院，南京 210044；2.南京信息工程大學(xué)江蘇省氣象傳感網(wǎng)技術(shù)工程中心，南京 210044)

旋翼(rotary wing)微型飛行器(Micro Aerial Vehicles，MAVS)［1］具有負載和功耗的限制，不能使用重量傳統(tǒng)傳感器。因此設(shè)計一種使用廉價輕量慣性傳感器設(shè)計的姿態(tài)測量系統(tǒng)，也可稱為慣性測量單元(簡稱IMU)［2］，用于輸出載體的姿態(tài)和航向等參數(shù)。進行姿態(tài)估算的方法很多:文獻［3］設(shè)計了基于方向旋轉(zhuǎn)矩陣的UKF濾波器。文獻［4］介紹基于廉價傳感器的姿態(tài)檢測系統(tǒng)設(shè)計，提出基于歐拉角和四元數(shù)的融合算法［5］。雖然考慮了動加速度和陀螺偏差，但是其數(shù)據(jù)更新頻率都較低，不適用于旋翼MAVs，而且該姿態(tài)檢測系統(tǒng)沒有搭載到實際MAVs中飛行實驗。文獻［6］設(shè)計基于低成本MEMS器件的航姿系統(tǒng)，采用二子樣等效旋轉(zhuǎn)矢量更新四元數(shù)，并應(yīng)用了降階的卡爾曼濾波器。文獻［7］研究了模型之間的轉(zhuǎn)換問題，對三種狀態(tài)空間模型進行了隨機漂移和角速率估計試驗。文獻［8］中提出了一種可以在載體運動情況下補償陀螺漂移的姿態(tài)確定方法。該方法利用三軸MEMS加速度計的輸出判斷載體是否處于加速運動狀態(tài)。如果載體處于運動狀態(tài)，則根據(jù)MEMS陀螺的測量，采用等效矢量法計算載體姿態(tài)；如處于靜止狀態(tài)，則利用加速度計和磁強計計算載體姿態(tài)誤差。而如果載體一直處于運動狀態(tài)，則陀螺儀的輸出隨著時間的推移，不斷發(fā)散。本設(shè)計與以上文獻方法不同，應(yīng)用微機械(MEMS)慣性傳感器，設(shè)計廉價輕量姿態(tài)測量系統(tǒng)，采用四元數(shù)法進行姿態(tài)解算，并應(yīng)用擴展卡爾曼濾波器。將機體在平移運動的情況下產(chǎn)生的動加速度做為噪音項來減少估算誤差。

1 基于擴展卡爾曼的姿態(tài)角推測算法

飛行器的姿態(tài)和航向是機體坐標系相對于參考坐標系的方位關(guān)系。載體相對于參考坐標系的姿態(tài)可以用不同的數(shù)學(xué)表達式來表示。常用的有三種姿態(tài)表達式:方向余弦、歐拉角和四元數(shù)。四元數(shù)由于可以避免歐拉角的奇異問題而應(yīng)用得更加廣泛，采用四元數(shù)來表示機體的姿態(tài)，四元數(shù)在文獻［2］中有詳細介紹。

本文用到參考坐標系和機體坐標系，參考坐標系是以地上的任意點為原點，磁場的北極方向為Xr軸，重力方向為Zr軸，XrZr平面的垂直方向取為Yr軸。而機體坐標系是以機體的重心為原點，機體的前方為Xb軸，機體的右方向為Yb軸，機體的下方取為Zb軸。另外，機體坐標系相對于參考坐標系的姿態(tài)定義為機體姿態(tài)。

參考坐標系上的重力矢量設(shè)為gr=［0 0g］T，地磁矢量為 mr=［mn0md］T。g為重力加速度，mn是地磁場在參考坐標系中的水平分量，md是地磁場在參考坐標系中的垂直分量。表示現(xiàn)在的姿態(tài)的四元數(shù)的推定值設(shè)為的話，機體坐標系上的重力矢量及地磁計矢量的推測值可表示為下式。

機體上搭載的加速度傳感器與地磁傳感器的輸出分別定義為 ameasure=［axayaz］，mmeasure=［mxmymz］，與之前講述的機體坐標系上的重力矢量和地磁矢量的推測值的偏差的矢量可表示為下式。

構(gòu)成擴展卡爾曼濾波器首先需要建立系統(tǒng)的過程模型。下面推導(dǎo)狀態(tài)方程式。四元數(shù)微分方程如下:

式(4)右邊的ωb可以從角速度陀螺來得到，但角速度陀螺與加速度傳感器這些慣性傳感器因為各種因素而存在誤差。為了實現(xiàn)更高精度的推測，期望能夠把這些誤差也推測出來，所以把角速度陀螺的誤差作為狀態(tài)量導(dǎo)入到系統(tǒng)中。把從角速度陀螺獲得的角速度定義為ωmeasure=［ωxωyωz］T，角速度陀螺的誤差為 Δωb=［σωxσωyσωz］T，這些量和ωb的關(guān)系可以表達為下式

把式(5)帶入式(4)得

一般情況下，導(dǎo)致角速度陀螺誤差的原因可以考慮為偏移誤差ωbias、未對準誤差及噪音等，本文設(shè)定Δωb?ωbias，只對偏移誤差進行推測，根據(jù)參考文獻［9］，從實際經(jīng)驗可以知道角速度陀螺的偏移誤差具有以下的動力學(xué)結(jié)構(gòu)。

其中 ω=［ωxωyωz］T為白色噪聲，βx，βy，βz為參數(shù)，這是通過實測長時間靜止放置的角速度陀螺的數(shù)據(jù)得到的。通過之前的推導(dǎo)，可以得到狀態(tài)量x=［()T，(ωbias)T］T的狀態(tài)方程為:

其中的各個行列式為

ω=［ωxωyωz］T為白色噪聲。

式(8)經(jīng)過離散化后可得離散時間的狀態(tài)方程表示如式(12)

其次是考慮到包括加到機體上的動加速度的加速度誤差，把動加速度作為姿態(tài)傳感器系統(tǒng)的噪音項。以下為觀測方程式。

Δa=［δaxδayδaz］T及地磁計誤差 Δm=［δmxδmyδmz］T，加速度傳感器及地磁計傳感器的輸出可以寫為下式:

其中

經(jīng)過推導(dǎo)，得到式(12)與式(15)的過程模型。但這兩個公式都是非線性方程式，不能直接運用線性卡爾曼濾波器算法。這種情況下，本文使用了擴展卡爾曼濾波器。

設(shè)計擴展卡爾曼濾波器，把tk時刻的被估計狀態(tài)xk的濾波估計值設(shè)為，預(yù)測估計值為時，行列式Fk及Hk可以定義為下式:

使用這些行列，擴展卡爾曼濾波器可以通過下面這些公式來得到［10］。

其中，Kk為卡爾曼增益，Pk為估計誤差方差矩陣，Pk，k－1為一步預(yù)測誤差方差矩陣，Qk是系統(tǒng)過程噪聲的非負定方差矩陣，Rk為觀測噪聲的正定方差矩陣，Hk為觀測矩陣。Gk，k－1為噪聲輸入矩陣。式(18)到式(22)的算法由使用得到的觀測值來對推測值進行濾波的部分及對下一步的推測值進行預(yù)測兩個部分構(gòu)成，式(18)到式(20)為前一部分，式(21)及(22)為后一部分。前面的部分也可稱為觀測更新，后面的部分可稱為時間更新。使用上面給出的這幾個公式按順序進行計算，能夠推測出最接近于tk時刻的xk的真實值的濾波估計值。

2 姿態(tài)測量系統(tǒng)設(shè)計

姿態(tài)測量系統(tǒng)硬件部分主要包括三個單軸陀螺儀、三軸加速度計、地磁計、12位模數(shù)轉(zhuǎn)換器、微控制器等。本文利用這些微機械(MEMS)傳感器組合研制了一種應(yīng)用于微小型飛行器的姿態(tài)測量系統(tǒng)，該系統(tǒng)的硬件組成框圖如圖1所示。

圖1 姿態(tài)測量系統(tǒng)硬件組成

加速度是MAV姿態(tài)測量以及分析MAV飛行性能的重要狀態(tài)量，選擇的ADXL335能以±3 gn的滿量程范圍測量加速度。通過調(diào)節(jié)輸出引腳上的電容來選擇合適的帶寬。采用的加速度計的X軸和Y軸的帶寬范圍為0.5 Hz至1 600 Hz，Z軸的帶寬范圍為0.5 Hz～550 Hz。為了減少噪音，將加速度傳感器帶寬設(shè)定為50 HZ，即使用0.1 μF濾波電容［11］。文中所用的陀螺是ADI公司的一款功能完備、成本低廉的角速率傳感器(陀螺儀)，用于測量角速率，該陀螺儀測量范圍達到±300°/s，抗干擾能力強，具有溫度校準功能，漂移誤差小，能夠滿足多旋翼飛行器的飛行控制性能需求。利用外接電容和片上電阻構(gòu)成一個低通濾波器用于限制ADXRS610速率響應(yīng)帶寬［12］，帶寬設(shè)為 361 Hz。模數(shù)轉(zhuǎn)換器使用了12位分辨率的MCP3204，帶SPI串行接口，當(dāng)供電電壓為5 V時；最大采樣速率能夠達到100 ksps。為了增強微型MAVS的抗風(fēng)能力，需要提高ADC的采樣率，設(shè)計的姿態(tài)測量系統(tǒng)將以100 Hz的頻率輸出數(shù)據(jù)。信號以模擬和數(shù)字(RS－232)兩種格式輸出。模擬信號包括三軸加速度計電壓，三軸磁強計電壓，加速度計電壓。數(shù)字量輸出包括三軸角速率；滾轉(zhuǎn)、俯仰、偏航角。留有GPS輸入接口，ADC輸入引腳留有擴展接口，方便根據(jù)實際需要增加相應(yīng)的傳感器。設(shè)計的姿態(tài)測量系統(tǒng)由兩塊雙層電路板構(gòu)成，上層包括三個單軸角速率陀螺、12位模數(shù)轉(zhuǎn)換器、微處理器、PWM信號輸出接口；下層包括DC/DC電路、一個三軸加速度，一個三軸地磁計、一個ADC轉(zhuǎn)換器，ADC留有擴展接口方便根據(jù)飛行需要添加相關(guān)傳感器。

3 實驗及實驗結(jié)果

為了證明上邊介紹的姿態(tài)估計算法及硬件電路的可行性，將數(shù)據(jù)采集及姿態(tài)估計算法程序，下載到設(shè)計的姿態(tài)測量電路板中進行驗證。下面結(jié)合實際研制的姿態(tài)測量系統(tǒng)對實驗過程和結(jié)果進行講解分析。

實驗共分為三個部分。實驗一為將自行研制的姿態(tài)測量系統(tǒng)和商用高精度傳感器同時裝載到飛行器上進行比較，來驗證測量系統(tǒng)的性能。實驗二為不考慮動加速度的傳統(tǒng)姿態(tài)角推算法和本文提出的將動加速度考慮為噪音項的算法的比較，來驗證該算法的有效性。實驗三為使用本文的姿態(tài)測量系統(tǒng)來實現(xiàn)飛行器的姿態(tài)控制，目的在于驗證該系統(tǒng)的實用性。

本研制姿態(tài)測量系統(tǒng)與商用高精度傳感器比較結(jié)果如圖2所示。商用高精度傳感器采用Crossbow公司的MNAV100CA，MNAV100CA是一種已校準的數(shù)字傳感器與伺服控制系統(tǒng)，圖中實線x是使用本文研制的傳感器得到的估算姿態(tài)角，虛線y為MNAV100CA傳感器輸出姿態(tài)角。實驗數(shù)據(jù)是將兩個傳感器同時搭載在多旋翼MAV上，在飛行中實時記錄得到。從圖中橫軸時間軸看，0～15 s處于加速飛行狀態(tài)，20 s～60 s基本處于盤旋狀態(tài)，二者對比可以看出，在各種運動狀態(tài)下，上文介紹的系統(tǒng)基本能得到商用姿態(tài)測量系統(tǒng)相同的測量結(jié)果。

圖2 與高精度傳感器比較結(jié)果

將傳感器水平放置在桌面上，在保持傾角不變的基礎(chǔ)上做水平加速度運動時輸出的姿態(tài)角，結(jié)果如圖3所示。圖中橫坐標為時間軸，縱坐標為姿態(tài)角輸出軸，初始水平線為傳感器水平靜止時狀態(tài)，載體在做線加速度運動時，理論值應(yīng)該為一條水平直線。實線x為沒有考慮動加速度的情況下，機體在做線性加速度運動時，姿態(tài)角在－15°～+15°范圍內(nèi)變化。虛線y為使用上文研究的算法后的推算角，在－5°～+5°范圍內(nèi)波動，從對比的結(jié)果可以看出，考慮了動加速度的算法能夠削減由動加速度引起的估算誤差，能夠更加精確的推算出飛行器的姿態(tài)角。

圖3 線加速度運動姿態(tài)角輸出結(jié)果

使用本文設(shè)計姿態(tài)測量系統(tǒng)，實現(xiàn)姿態(tài)控制結(jié)果如圖4所示。圖4中，橫坐標為時間軸，縱坐標為滾轉(zhuǎn)角和俯仰角，實線為參考輸入，虛線跟蹤結(jié)果。從圖4可以看出，使用本設(shè)計的姿態(tài)測量系統(tǒng)能夠達到很好的姿態(tài)控制效果。

圖4 姿態(tài)控制實驗結(jié)果

4 結(jié)束語

應(yīng)用MEMS慣性傳感器，設(shè)計應(yīng)用于多旋翼MAVs的輕量廉價姿態(tài)測量系統(tǒng)，采用四元數(shù)法進行姿態(tài)矩陣計算，并應(yīng)用了擴展卡爾曼濾波器。飛行器在飛行過程中，加速度計除了檢測到重力加速度在機體坐標系上坐標變換的成分外，還能檢測到由飛行器的運動而產(chǎn)生的動加速度，采用的算法將動加速度作為姿態(tài)傳感器系統(tǒng)的噪音項，然后利用卡爾曼濾波器來構(gòu)成姿態(tài)推測算法來降低誤差。通過實際飛行中對比商用高精度傳感器，證明所提的姿態(tài)測量系統(tǒng)能夠很好的反映機體姿態(tài)，能夠應(yīng)用于多旋翼MAVs的飛行控制。

通過對噪音項加權(quán)矩陣的調(diào)節(jié)，可以降低動加速度導(dǎo)致的姿態(tài)推測誤差。但是在實際設(shè)計過程中，如果過多的考慮動加速度噪音的影響，將導(dǎo)致姿態(tài)推算收斂變慢，在飛行中無法正確的推測出飛行器的姿態(tài)。因此，通過大量的仿真來獲得合適的加權(quán)函數(shù)是本姿態(tài)傳感系統(tǒng)的一個關(guān)鍵。另外，本系統(tǒng)算法沒有涉及到飛行器的運動原理，如果能在估算法中結(jié)合飛行器的姿態(tài)數(shù)學(xué)模型的話，將會更加精確的推測出動加速度，實現(xiàn)更高精度的姿態(tài)角估算與姿態(tài)控制。

［1］Kenzo Nonami，F(xiàn)arid Kendoul，Satoshi Suzuki，Wei Wang，et al.Autonomous Flying Robots:Unmanned Aerial Vehicles and Micro Aerial Vehicles［M］.Berlin:Springer，2010.

［2］黃旭，王常虹，伊國興，等.利用磁強計及微機械加速度計和陀螺的姿態(tài)估計擴展卡爾曼濾波器［J］.中國慣性技術(shù)學(xué)報，2005，13(2):27－30，34.

［3］趙鶴，王喆垚.基于UKF的MEMS傳感器姿態(tài)測量系統(tǒng)［J］.傳感技術(shù)學(xué)報，2011，24(5):642－646.

［4］Gebre－Egziabher D，Hayward R C，Powell J D.Design of Multi－SensorAttitude Determination Systems［J］.Aerospace ＆Electronic Systems Society，2004，40(2):627－649.

［5］ZHAI Yu－tao，WEI Qiang，WANG Xiao－h(huán)ao，et al.Micro Attitude Determination System Based on MEMS Inertial Sensors［J］.Nanotechnology and Precision Engineering，2009，7(4):375－379

［6］杜繼永，黃國榮，張鳳鳴，等.基于低成本MEMS器件的捷聯(lián)航姿系統(tǒng)設(shè)計［J］.傳感技術(shù)學(xué)報，2010，23(11):1662－1666.

［7］袁贛南，梁海波，何昆鵬，等.MEMS陀螺隨機漂移的狀態(tài)空間模型分析及應(yīng)用［J］.傳感技術(shù)學(xué)報，2011，24(6):853－858.

［8］高宗余，李德勝.多MEMS傳感器姿態(tài)測量系統(tǒng)的研究［J］.電光與控制，2010，17(3):68－70，97.

［9］Robert M Rogers.Applied Mathematics in Integrated Navigation System［M］.Second Edition.AIAA，2003.

［10］付夢印，鄧志宏，閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用［M］.第二版.北京:科學(xué)出版社，2010.

［11］Analog Devices.Inc.ADXL335 DataSheet［EB/OL］.http://www.analog.com/en/index.html

［12］Analog Devices.Inc.ADXRS610 DataSheet［EB/OL］.http://www.analog.com/en/index.html