葉小嶺，杜 浩，王 偉，2

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

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

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

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

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

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

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

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

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

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

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

其中 ω=［ωxωyωz］T為白色噪聲，βx，βy，βz為參數(shù)，這是通過實測長時間靜止放置的角速度陀螺的數(shù)據(jù)得到的。通過之前的推導，可以得到狀態(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)過推導，得到式(12)與式(15)的過程模型。但這兩個公式都是非線性方程式，不能直接運用線性卡爾曼濾波器算法。這種情況下，本文使用了擴展卡爾曼濾波器。

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

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

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

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

姿態(tài)測量系統(tǒng)硬件部分主要包括三個單軸陀螺儀、三軸加速度計、地磁計、12位模數(shù)轉換器、微控制器等。本文利用這些微機械(MEMS)傳感器組合研制了一種應用于微小型飛行器的姿態(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。為了減少噪音，將加速度傳感器帶寬設定為50 HZ，即使用0.1 μF濾波電容［11］。文中所用的陀螺是ADI公司的一款功能完備、成本低廉的角速率傳感器(陀螺儀)，用于測量角速率，該陀螺儀測量范圍達到±300°/s，抗干擾能力強，具有溫度校準功能，漂移誤差小，能夠滿足多旋翼飛行器的飛行控制性能需求。利用外接電容和片上電阻構成一個低通濾波器用于限制ADXRS610速率響應帶寬［12］，帶寬設為 361 Hz。模數(shù)轉換器使用了12位分辨率的MCP3204，帶SPI串行接口，當供電電壓為5 V時；最大采樣速率能夠達到100 ksps。為了增強微型MAVS的抗風能力，需要提高ADC的采樣率，設計的姿態(tài)測量系統(tǒng)將以100 Hz的頻率輸出數(shù)據(jù)。信號以模擬和數(shù)字(RS－232)兩種格式輸出。模擬信號包括三軸加速度計電壓，三軸磁強計電壓，加速度計電壓。數(shù)字量輸出包括三軸角速率；滾轉、俯仰、偏航角。留有GPS輸入接口，ADC輸入引腳留有擴展接口，方便根據(jù)實際需要增加相應的傳感器。設計的姿態(tài)測量系統(tǒng)由兩塊雙層電路板構成，上層包括三個單軸角速率陀螺、12位模數(shù)轉換器、微處理器、PWM信號輸出接口；下層包括DC/DC電路、一個三軸加速度，一個三軸地磁計、一個ADC轉換器，ADC留有擴展接口方便根據(jù)飛行需要添加相關傳感器。

3 實驗及實驗結果

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

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

本研制姿態(tài)測量系統(tǒng)與商用高精度傳感器比較結果如圖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)相同的測量結果。

圖2 與高精度傳感器比較結果

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

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

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

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

4 結束語

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

通過對噪音項加權矩陣的調(diào)節(jié)，可以降低動加速度導致的姿態(tài)推測誤差。但是在實際設計過程中，如果過多的考慮動加速度噪音的影響，將導致姿態(tài)推算收斂變慢，在飛行中無法正確的推測出飛行器的姿態(tài)。因此，通過大量的仿真來獲得合適的加權函數(shù)是本姿態(tài)傳感系統(tǒng)的一個關鍵。另外，本系統(tǒng)算法沒有涉及到飛行器的運動原理，如果能在估算法中結合飛行器的姿態(tài)數(shù)學模型的話，將會更加精確的推測出動加速度，實現(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］.中國慣性技術學報，2005，13(2):27－30，34.

［3］趙鶴，王喆垚.基于UKF的MEMS傳感器姿態(tài)測量系統(tǒng)［J］.傳感技術學報，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)設計［J］.傳感技術學報，2010，23(11):1662－1666.

［7］袁贛南，梁海波，何昆鵬，等.MEMS陀螺隨機漂移的狀態(tài)空間模型分析及應用［J］.傳感技術學報，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濾波理論及其在導航系統(tǒng)中的應用［M］.第二版.北京:科學出版社，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