葉小嶺，杜 浩，王 偉，2

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

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

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

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

本文用到參考坐標(biāo)系和機(jī)體坐標(biāo)系，參考坐標(biāo)系是以地上的任意點(diǎn)為原點(diǎn)，磁場(chǎng)的北極方向?yàn)閄r軸，重力方向?yàn)閆r軸，XrZr平面的垂直方向取為Yr軸。而機(jī)體坐標(biāo)系是以機(jī)體的重心為原點(diǎn)，機(jī)體的前方為Xb軸，機(jī)體的右方向?yàn)閅b軸，機(jī)體的下方取為Zb軸。另外，機(jī)體坐標(biāo)系相對(duì)于參考坐標(biāo)系的姿態(tài)定義為機(jī)體姿態(tài)。

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

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

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

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

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

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

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

其中的各個(gè)行列式為

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

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

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

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

其中

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)及實(shí)驗(yàn)結(jié)果

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

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

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

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

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

圖3 線加速度運(yùn)動(dòng)姿態(tài)角輸出結(jié)果

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

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

4 結(jié)束語(yǔ)

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

通過對(duì)噪音項(xiàng)加權(quán)矩陣的調(diào)節(jié)，可以降低動(dòng)加速度導(dǎo)致的姿態(tài)推測(cè)誤差。但是在實(shí)際設(shè)計(jì)過程中，如果過多的考慮動(dòng)加速度噪音的影響，將導(dǎo)致姿態(tài)推算收斂變慢，在飛行中無法正確的推測(cè)出飛行器的姿態(tài)。因此，通過大量的仿真來獲得合適的加權(quán)函數(shù)是本姿態(tài)傳感系統(tǒng)的一個(gè)關(guān)鍵。另外，本系統(tǒng)算法沒有涉及到飛行器的運(yùn)動(dòng)原理，如果能在估算法中結(jié)合飛行器的姿態(tài)數(shù)學(xué)模型的話，將會(huì)更加精確的推測(cè)出動(dòng)加速度，實(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］黃旭，王常虹，伊國(guó)興，等.利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2005，13(2):27－30，34.

［3］趙鶴，王喆垚.基于UKF的MEMS傳感器姿態(tài)測(cè)量系統(tǒng)［J］.傳感技術(shù)學(xué)報(bào)，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］杜繼永，黃國(guó)榮，張鳳鳴，等.基于低成本MEMS器件的捷聯(lián)航姿系統(tǒng)設(shè)計(jì)［J］.傳感技術(shù)學(xué)報(bào)，2010，23(11):1662－1666.

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

［8］高宗余，李德勝.多MEMS傳感器姿態(tài)測(cè)量系統(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］付夢(mèng)印，鄧志宏，閆莉萍.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