基于自適應(yīng)平方根UKF的微機(jī)械傳感器融合航姿估計(jì)①

馬幫立

（鄖陽師范高等?？茖W(xué)校物理與電子工程系，湖北 十堰 442700）

0 引言

航姿估計(jì)廣泛應(yīng)用于導(dǎo)航制導(dǎo)、智能交通、光電穩(wěn)定平臺(tái)以及慣性穩(wěn)定平臺(tái)等系統(tǒng)中[1].但是，目前廣泛使用的車載航姿測(cè)量系統(tǒng)大多是基于高精度慣性器件和GPS組合的姿態(tài)航向參考系統(tǒng)(attitude and heading reference system，AHRS)，造價(jià)昂貴，嚴(yán)重制約了車載航姿測(cè)量系統(tǒng)的應(yīng)用和推廣.快速發(fā)展的微機(jī)電技術(shù)(micro-electromechanical systems，MEMS)為開發(fā)低成本航姿估計(jì)系統(tǒng)提供了很好的方向.目前，采用MIMU(MEMSinertialmeasurementunit，MIMU)和GPS設(shè)計(jì)組合導(dǎo)航系統(tǒng)已成為導(dǎo)航領(lǐng)域研究的熱點(diǎn)[2].

目前廣泛報(bào)道的航姿估計(jì)方法[3-5]依靠陀螺積分得到姿態(tài)角，增加輔助傳感器校正陀螺漂移，利用微機(jī)械慣性器件構(gòu)成低成本姿態(tài)估計(jì)，采用MEMS陀螺、MEMS加速度計(jì)以及磁傳感器估計(jì)姿態(tài).但采用加速度計(jì)測(cè)量重力分量估計(jì)水平姿態(tài)，容易受到載體機(jī)動(dòng)加速度的影響.文獻(xiàn)[3]中采用測(cè)量手臂姿態(tài)，沒有考慮機(jī)動(dòng)加速度干擾問題.文獻(xiàn)[4]中采用開關(guān)判斷即g判斷，加速度測(cè)量矢量的范數(shù)大于g時(shí)不融合，文獻(xiàn)[5]中研究了機(jī)器人的姿態(tài)估計(jì)問題，也采用了開關(guān)判斷，采用卡爾曼濾波器對(duì)陀螺零偏值進(jìn)行估計(jì)，克服載體的機(jī)動(dòng)加速度對(duì)加速度計(jì)姿態(tài)測(cè)量的影響.但3個(gè)姿態(tài)角的估計(jì)開關(guān)同時(shí)控制，不能區(qū)分線加速度和向心加速度，估計(jì)精度受到影響，特別是對(duì)于兩種機(jī)動(dòng)加速度交替連續(xù)出現(xiàn)的情況，系統(tǒng)無法工作；同時(shí)該開關(guān)算法對(duì)于車體振動(dòng)等外界干擾過于敏感，判斷誤差較大；采用的互補(bǔ)濾波器無法給出陀螺零偏估計(jì)，短時(shí)間無測(cè)量值時(shí)難以利用陀螺積分保持精度.

本文中采用姿態(tài)估計(jì)結(jié)構(gòu)即由陀螺積分得到姿態(tài)角，輔以磁強(qiáng)計(jì)和加速度計(jì)校正陀螺漂移[3-5]，并提出一種新的基于自適應(yīng)平方根UKF算法，估計(jì)和補(bǔ)償機(jī)動(dòng)加速度，有效克服了開關(guān)判斷即g判斷的缺陷.跑車實(shí)驗(yàn)結(jié)果表明，該算法有效地去除機(jī)動(dòng)加速度的干擾，提高車輛的航姿估計(jì)精度.

1 系統(tǒng)原理與姿態(tài)運(yùn)動(dòng)學(xué)描述

陀螺的動(dòng)態(tài)響應(yīng)較好，在短時(shí)間內(nèi)精度較高，但是存在慢變的零偏誤差，積分得到的角度存在隨時(shí)間積累的誤差，長期精度較低.通過加速度計(jì)測(cè)量重力分量可以得到載體的俯仰角和橫滾角，不存在隨時(shí)間的累積誤差，長期精度較好，但是加速度計(jì)對(duì)于車輛振動(dòng)等噪聲比較敏感，短時(shí)精度不高.因此，利用陀螺與加速度計(jì)的互補(bǔ)特性可以得到較好的水平姿態(tài)估計(jì).但是，當(dāng)載體存在機(jī)動(dòng)加速度時(shí)，如線加速度(起步、剎車等)和向心加速度(轉(zhuǎn)彎)，會(huì)帶來較大的姿態(tài)估計(jì)誤差，因此利用陀螺和加速度計(jì)融合得到姿態(tài)角需要去除機(jī)動(dòng)加速度的影響.

四元數(shù)在描述剛體旋轉(zhuǎn)方面有獨(dú)特的優(yōu)勢(shì)，不僅是參數(shù)最少的無奇異姿態(tài)表示方式，而且用四元數(shù)表示的時(shí)間傳遞方程是線性的，在計(jì)算過程中避免了歐拉角奇異問題以及復(fù)雜的三角函數(shù)非線性關(guān)系[6].姿態(tài)四元數(shù)是基于剛體旋轉(zhuǎn)繞矢量軸一次轉(zhuǎn)成的原理，由4個(gè)分量表示為

四元數(shù)與方向余弦矩陣Cbn(qˉ)有關(guān)系

其時(shí)間傳遞方程為

其中

其中ωt為真實(shí)角速度，b為陀螺零偏，nω與nb分別為零均值與白噪聲，（3）式中，由于微機(jī)械（MEMS）陀螺精度較低，地球自轉(zhuǎn)角速度以及載體運(yùn)動(dòng)牽連角速度可以忽略不計(jì)，因此認(rèn)為微機(jī)械陀螺測(cè)量數(shù)據(jù)中只包含有載體姿態(tài)變化角速度和陀螺零偏噪聲.

參考慣性導(dǎo)航中的比力方程[6]可得，加速度計(jì)量測(cè)值為為求角速度矢量的反對(duì)稱矩陣.

陀螺誤差的一般建模為慢變零偏以及測(cè)量白噪聲，慢變零偏認(rèn)為是白噪聲驅(qū)動(dòng)的隨機(jī)游走[7]

式中的θ為俯仰角，φ為橫滾角為載體坐標(biāo)系的加速度計(jì)測(cè)量的重力的3個(gè)分量，單位為g，nθ與nφ分別為加速度計(jì)的測(cè)量誤差.

2 UKF融合估計(jì)姿態(tài)

姿態(tài)估計(jì)是典型的非線性問題，可以采用的方法有擴(kuò)展卡爾曼濾波器算法(extended Kalman filter，EKF)、無味卡爾曼濾波器算法(unscented Kalman filter，UKF)以及粒子濾波器算法(particle filter，PF)等.其中EKF算法復(fù)雜度最低，但是存在較大的線性化誤差，對(duì)初值的要求較高，較大的初值誤差可能導(dǎo)致其發(fā)散，同時(shí)求解Jacobian較為復(fù)雜；PF算法估計(jì)精度高，但是由于采用隨機(jī)采樣傳遞狀態(tài)的概率密度函數(shù)，其計(jì)算量很大，對(duì)于多維系統(tǒng)，難以做到實(shí)時(shí)估計(jì)；而UKF算法在克服EKF算法缺點(diǎn)的同時(shí)計(jì)算量適中，利于實(shí)時(shí)估計(jì)[8].為了增強(qiáng)濾波器的穩(wěn)定性，采用平方根形式的UKF算法，保證協(xié)方差陣的正定性，其算法實(shí)現(xiàn)過程如下：

1)初始化，

式中，S0為初始方差陣的Cholesky分解.

2)計(jì)算采樣點(diǎn)，采用對(duì)稱采樣的采樣點(diǎn)為2n+1個(gè)，其中γi,i=0,2,…,2n為Sigma點(diǎn)序列[9]，

4)更新采樣點(diǎn)，利用一步預(yù)測(cè)值更新采樣點(diǎn)，

其中，

其中chol{·}為Cholesky分解，qr{·}為QR分解，cholupdate{·}為平方根矩陣的更新[9].

由于陀螺零偏誤差較大，因此狀態(tài)變量含有需要估計(jì)的姿態(tài)角及陀螺零偏誤差，即狀態(tài)變量為由(2)～(3)式得到狀態(tài)方程為

由(5)式，結(jié)合歐拉角與四元數(shù)的關(guān)系[6]，得到不考慮機(jī)動(dòng)加速度時(shí)的測(cè)量方程為

式中的ψm由磁強(qiáng)計(jì)得到，通過水平姿態(tài)角的一步預(yù)測(cè)值進(jìn)行磁強(qiáng)計(jì)測(cè)量的水平姿態(tài)補(bǔ)償，得到水平面內(nèi)的磁強(qiáng)計(jì)測(cè)量分量，然后通過反正切計(jì)算得出航向角[10]，式中的nψ為測(cè)量誤差.將狀態(tài)方程(12)～(13)式帶入平方根UKF算法中即可實(shí)現(xiàn)航姿融合估計(jì)，同時(shí)可以實(shí)現(xiàn)微機(jī)械陀螺（包含航向陀螺，俯仰陀螺以及橫滾陀螺）的零偏估計(jì)和校正.

3 自適應(yīng)UKF算法

(13)式中的俯仰角和橫滾角由加速度計(jì)測(cè)量得到，未考慮機(jī)動(dòng)加速度對(duì)姿態(tài)角測(cè)量的干擾.由(4)式可知，在有機(jī)動(dòng)加速度干擾的情況下，實(shí)際的測(cè)量值為

式中的ax、ay分別為車輛機(jī)動(dòng)引起的向心加速度和前向加速度，這些機(jī)動(dòng)加速度值均會(huì)體現(xiàn)在濾波過程中的測(cè)量更新階段即殘差中.定義測(cè)量矢量為機(jī)動(dòng)加速度矢量為ak=[0,ay,k,ax,k]T，則殘差表示為rk=yk-y^k|k-1，由于其中含有機(jī)動(dòng)加速度干擾，故殘差的協(xié)方差為

式中的Syy由(11)式得到，Rk為測(cè)量噪聲，Ca,k為機(jī)動(dòng)加速度矢量，是ak的協(xié)方差矩陣，即

如果能夠估計(jì)協(xié)方差矩陣Ca,k，則可以分別計(jì)算車輛的向心加速度與前向加速度干擾.

新息序列的協(xié)方差可以通過如下公式近似得到

式中的N為估計(jì)滑動(dòng)窗，通過（16）式可得到Ca,k的估計(jì)值

結(jié)合(15)～(17)式可得到機(jī)動(dòng)加速度的估計(jì)值.但是，該方法必須保證(17)式給出的協(xié)方差矩陣的估計(jì)值是正定的，因此，必須對(duì)(17)式作必要的改動(dòng).由于Wk為對(duì)稱矩陣，對(duì)其做特征值分解，可得

式中的λi,k為Wk的特征值，相應(yīng)的正交矢量為ui,k.令ξi,k滿足

由于λi,k≥0，并且ξi,k≥0，則可以得到

則最終的最適應(yīng)算法為

上式中的判斷窗口M以及閾值κ均是為了確保能夠計(jì)及由于加速度計(jì)自身的測(cè)量噪聲對(duì)機(jī)動(dòng)加速度的估計(jì)產(chǎn)生的影響，將估計(jì)的加速度值在加速度計(jì)的測(cè)量中進(jìn)行補(bǔ)償即可實(shí)現(xiàn)自適應(yīng)UKF融合估計(jì)姿態(tài).

4 實(shí)驗(yàn)結(jié)果與分析

實(shí)驗(yàn)設(shè)備主要包括星網(wǎng)宇達(dá)公司的微機(jī)械慣性測(cè)量單元XW-IMU5220，參考系統(tǒng)為XW-ADU7612.其中IMU5220中包含有3個(gè)MEMS陀螺（零偏≤0.08°/s，零偏重復(fù)性≤0.05°/s，測(cè)量噪聲RMS≤0.05°/s）和3個(gè)MEMS加速度計(jì)，并且做了相應(yīng)的溫度補(bǔ)償，能夠穩(wěn)定地輸出100 Hz的測(cè)量值.參考系統(tǒng)XW-ADU 7612能以100 Hz的數(shù)據(jù)更新速率，給出被測(cè)對(duì)象精確的姿態(tài)角(航向角、俯仰角和橫滾角)，其航向角精度為0.1°，俯仰角和橫滾角的精度靜態(tài)時(shí)均達(dá)0.05°，動(dòng)態(tài)時(shí)均可達(dá)0.1°.根據(jù)所用器件的靜態(tài)測(cè)量數(shù)據(jù)給定如下濾波器參數(shù)，預(yù)測(cè)時(shí)間0.01 s，更新時(shí)間0.1 s，過程噪聲和測(cè)量噪聲均為零均值白噪聲，過程噪聲方差分別為測(cè)量噪聲方差分別為

在校園進(jìn)行了繞圈實(shí)驗(yàn)，采集時(shí)間為267 s，由于實(shí)際更新頻率為100Hz，數(shù)據(jù)量較大，作圖時(shí)每1 s顯示一個(gè)數(shù)據(jù).

圖1表示各傳感器姿態(tài)角單獨(dú)估計(jì)結(jié)果與對(duì)比.可以看出，以陀螺積分得到的姿態(tài)角明顯存在累積誤差；由于3軸耦合，航向陀螺在轉(zhuǎn)彎時(shí)數(shù)值較大，因此陀螺積分得到的俯仰角和橫滾角在轉(zhuǎn)彎階段，其累積誤差淹沒在航向耦合帶來的誤差中；采用加速度計(jì)計(jì)算的姿態(tài)角不存在累積誤差，但是噪聲較大，并且在有機(jī)動(dòng)加速度時(shí)存在較大的偏差.按照坐標(biāo)系定義，線加速度對(duì)俯仰角產(chǎn)生影響，向心加速度對(duì)橫滾角產(chǎn)生影響，在車起步和停車過程中以及轉(zhuǎn)彎過程中，采用加速度計(jì)單獨(dú)估計(jì)姿態(tài)角均有較大偏差.

圖1 姿態(tài)角單獨(dú)估計(jì)結(jié)果與對(duì)比

圖2和圖3分別是俯仰角和橫滾角的UKF算法估計(jì)結(jié)果與對(duì)比.可以看出，采用UKF算法可以實(shí)現(xiàn)陀螺數(shù)據(jù)與加速度數(shù)據(jù)的融合，去除了采用加速度計(jì)單獨(dú)估計(jì)時(shí)的誤差，說明陀螺的短時(shí)精度與加速度計(jì)的長時(shí)精度相結(jié)合能夠得到較為精確的姿態(tài)估計(jì)；采用自適應(yīng)UKF估計(jì)機(jī)動(dòng)加速度并且進(jìn)行補(bǔ)償之后，誤差明顯變小，基本上與參考姿態(tài)曲線重合，說明本文中提出的自適應(yīng)UKF算法能夠有效地去除加速度計(jì)中的機(jī)動(dòng)加速度干擾.圖4為自適應(yīng)UKF算法估計(jì)得出的機(jī)動(dòng)加速度，自適應(yīng)參數(shù)選擇為N=10，M=10，閾值選擇為κ=0.1.

圖2 俯仰角UKF算法估計(jì)結(jié)果與對(duì)比

圖3 橫滾角UKF算法估計(jì)結(jié)果與對(duì)比

圖4中的線加速度與向心加速度估計(jì)結(jié)果分別與圖3中未采用UKF自適應(yīng)算法時(shí)的俯仰角和橫滾角估計(jì)誤差相對(duì)應(yīng).圖5為姿態(tài)角自適應(yīng)UKF算法的估計(jì)誤差.可以看出，航向角誤差絕對(duì)值在0.4°以內(nèi)，俯仰角和橫滾角誤差絕對(duì)值均小于0.6°，滿足一般的車輛姿態(tài)估計(jì)精度要求.由于機(jī)動(dòng)加速度對(duì)航向角的估計(jì)無影響，航向角估計(jì)依靠磁強(qiáng)計(jì)，圖5中給出了其估計(jì)誤差.

圖4 機(jī)動(dòng)加速度自適應(yīng)UKF算法估計(jì)結(jié)果

圖5 姿態(tài)角自適應(yīng)UKF算法估計(jì)誤差

5 結(jié)論

本文中提出一種適用于車載環(huán)境的低成本航姿測(cè)量系統(tǒng)，采用3軸陀螺積分得到姿態(tài)角，同時(shí)利用加速度計(jì)校正水平陀螺零偏，利用磁強(qiáng)計(jì)保證航向角精度.針對(duì)加速度計(jì)測(cè)量姿態(tài)角容易受到非重力加速度的影響，本文中提出了一種自適應(yīng)UKF算法，可實(shí)時(shí)估計(jì)并補(bǔ)償機(jī)動(dòng)加速度對(duì)姿態(tài)角估計(jì)的影響.實(shí)際跑車實(shí)驗(yàn)結(jié)果表明，提出的自適應(yīng)UKF算法能夠得到較高精度的姿態(tài)角估計(jì).此外，該低成本姿態(tài)測(cè)量系統(tǒng)還可用于車載穩(wěn)定平臺(tái)、指向跟蹤系統(tǒng)以及小型無人機(jī)的姿態(tài)控制系統(tǒng)中.

[1]Beljour H，Coryell L，Tat F.Army SATCOM on the move technology initiatives[C]//IEEE Military Communications Conference.Boston，MA，2009:1-7.

[2]祝燕華，蔡體菁，楊卓鵬.MEMS-IMU/GPS組合導(dǎo)航系統(tǒng)的實(shí)現(xiàn)[J].中國慣性技術(shù)學(xué)報(bào)，2009，17(5)：552-556.

[3]沈曉衛(wèi)，姚敏立，趙鵬.基于互補(bǔ)濾波的動(dòng)中通天線姿態(tài)估計(jì)[J].中國慣性技術(shù)學(xué)報(bào)，2011，19(2)：194-197.

[4]Yun Xiaoping，Bachmann Eric R.Design，implementation，and experimental results ofa quaternion-based Kalman filter for human bodymotion tracking[J].IEEE Transactionson Robotics，2006，22(6):1216-1227.

[5]Henrik Rehbinder，Hu Xiaoming.Drift-free attitude estimation for accelerated rigid bodies[J].Automatica，2004，40:653-659.

[6]秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2006.

[7]Crassidis John L，Landis Markley F，Yang Cheng.Survey of nonlinear attitude estimation0methods[J].Journal of Guidance，Control，and Dynamics，2007，30(1):12-28.

[8]Julier SJ，Uhlmann JK，Durrant-whyte H F.A newmethod for nonlinear transformation of means and covariances in filters and estimators[J].IEEE Transactionson Automatic Control，2000，45(3):477-482.

[9]伍宗偉，姚敏立，馬紅光，等.移動(dòng)衛(wèi)星通信低成本多傳感器融合姿態(tài)估計(jì)方法[J].西安交通大學(xué)學(xué)報(bào)，2012，46(12):55-61.

[10]Crassidis John L，Landis Markley F，Yang Cheng.Applicaiton of magnetic sensors for low cost compass system[C].IEEE Plans，Position Location and Navigation Symposium，2000:12-28.