一種適用于慣性-地磁組合的自適應(yīng)卡爾曼算法

戎海龍，彭翠云

1.常州大學(xué) 城市軌道交通學(xué)院，江蘇 常州 213164

2.常州大學(xué) 信息科學(xué)與工程學(xué)院，江蘇 常州 213164

1 引言

慣性-地磁組合一般由三軸MEMS（微機電系統(tǒng)）陀螺儀、三軸MEMS加速度計和三軸MEMS地磁傳感器組成，然后通過多傳感器信息融合技術(shù)獲得運動體姿態(tài)，在人體運動分析等領(lǐng)域應(yīng)用很廣泛。這種組合可以看做是兩種姿態(tài)測量方法的重新組合。眾所周知，單獨利用三軸陀螺儀也可以實現(xiàn)姿態(tài)測量（基于多加速度計組合的所謂的無陀螺捷聯(lián)慣導(dǎo)系統(tǒng)也可以歸為此類），這種姿態(tài)測量方法的短時測量精度很高，相應(yīng)的姿態(tài)解算算法包括方向余弦算法、歐拉角法、四元數(shù)微分算法、等效矢量算法等，然而這些算法全部建立在數(shù)值積分的基礎(chǔ)上，因而如果陀螺儀為MEMS陀螺儀，則其自身所具有的幅值較大的隨機噪聲及從根本上無法完全消除的轉(zhuǎn)動不可交換性誤差很容易使解算結(jié)果快速發(fā)散，因而這種方法并不適合長時間的姿態(tài)測量。利用三軸加速度計和三軸地磁傳感器也可以獲得運動體姿態(tài)，這種方法的靜態(tài)精度很高，相應(yīng)的姿態(tài)解算算法包括TRIAD算法、QUEST算法、FORM算法、ESOQ、ESOQ2算法、q-算法、Euler-q算法等，然而加速度計不可能只測量運動體重力加速度，況且人體運動有一個不同于機械運動（包括飛行器）的特點，即運動的線加速度復(fù)雜多變，無規(guī)律可循，采用建模的方法抑制線加速度對算法本身的影響并不現(xiàn)實，因而即使加速度計精度再高，上述算法的動態(tài)精度仍然很差。慣性-地磁組合的目的是將上述兩種方法重新進(jìn)行組合，進(jìn)而獲得更好的靜動態(tài)精度，采用的姿態(tài)解算算法包括狀態(tài)方程非線性[1]或觀測方程非線性[2-5]的擴展卡爾曼算法、互補濾波算法[6-8]、梯度下降算法[9]，以及經(jīng)作者[10]實驗研究發(fā)現(xiàn)也適用的REQUEST 算法[11]、Optimal-REQUEST 算法[12]、自適應(yīng)Optimal-REQUEST算法[13]、filter-REQUEST算法[14]等，這些算法均為迭代算法，基本思想類似。狀態(tài)方程非線性的卡爾曼算法的觀測矩陣為單位陣，并假設(shè)載體角速度為一階馬爾科夫過程，因此對加速度計和地磁傳感器組合的依賴較大，這使得該算法的靜態(tài)結(jié)果較高且不發(fā)散，然而動態(tài)精度欠佳；觀測方程非線性的擴展卡爾曼算法由于狀態(tài)噪聲取的過?。ò碝EMS陀螺儀實際噪聲間接得出）導(dǎo)致卡爾曼增益很小，進(jìn)而使得其對陀螺儀的依賴較大，因而其靜態(tài)結(jié)果存在緩慢發(fā)散現(xiàn)象，但是其動態(tài)精度較高；互補濾波算法和梯度下降算法可以分別通過調(diào)整其截止頻率和相關(guān)的權(quán)值實現(xiàn)對陀螺儀或者加速度計與地磁傳感器組合的依賴程度，但調(diào)整的依據(jù)沒有提及，如果調(diào)整不合適，則亦或動態(tài)精度較低，亦或靜態(tài)精度較差；REQUEST算法家族隨著其記憶因子的逐步衰減，對加速度計和地磁傳感器組合的依賴程度越來越小，因此一段時間之后其動態(tài)精度比觀測方程非線性的擴展卡爾曼算法精度更高，然而由于過分依賴陀螺儀，使得其發(fā)散現(xiàn)象比觀測方程非線性的擴展卡爾曼算法更嚴(yán)重。如果能夠根據(jù)運動體運動情況對觀測噪聲或者過程噪聲進(jìn)行實時估計，也就是采用自適應(yīng)卡爾曼算法[15]顯然是比較合適的，然而自適應(yīng)卡爾曼算法適用的前提是觀測噪聲為方差可變的白噪聲，因此該算法目前只在組合導(dǎo)航系統(tǒng)中GPS觀測噪聲實時估計上獲得應(yīng)用。已有學(xué)者采用下式作為觀測噪聲調(diào)整依據(jù)：其中ax(k)、ay(k)、az(k)分別為由k時刻加速度計測得的三軸向加速度分量（包括線加速度和重力加速度分量），而后或者采用門技術(shù)[4]調(diào)整觀測噪聲方差或者直接將上式作為觀測噪聲并對其方差進(jìn)行實時估計[3，5]，這些方法盡管簡單有效，但是有四個問題：（1）很明顯，上式并不能看做噪聲，它甚至根本不是一個平穩(wěn)過程；（2）加速度計存在輸出零偏和隨機游走，這會對上式造成誤差；（3）用上式表示運動體線加速度大小并不準(zhǔn)確，εo=0時，上式在三維坐標(biāo)系下可表示為一個球，球表面上的任何點均滿足該式，這意味著滿足該式并不代表運動體處于靜止?fàn)顟B(tài)；（4）運動體線加速度是無界的，如果過大，則有可能導(dǎo)致算法過于依賴陀螺儀而發(fā)散。本文仍然將觀測方程非線性的擴展卡爾曼算法作為姿態(tài)解算算法，然而將給出一種新的觀測噪聲，這種觀測噪聲的方差在運動體運動時會增大，進(jìn)而加大對陀螺儀的依賴，這將構(gòu)成一種真正意義上的自適應(yīng)擴展卡爾曼算法，實驗表明這種算法是比較有效的。

2 基于觀測噪聲實時估計的自適應(yīng)EKF算法

卡爾曼算法的k+1時刻的驗后估計為：

Fk+1為k+1時刻的觀測陣（或者線性化后的觀測陣）；Rk+1為觀測噪聲；為k+1時刻的先驗估計的誤差協(xié)方差陣：

Qk為過程噪聲；Pk為k時刻的驗后估計的誤差協(xié)方差陣。

REQUEST算法的迭代方程為：

其中bi和ri為k時刻由三軸加速度計（三軸地磁傳感器）得到的載體坐標(biāo)系表示的重力矢量（地磁場矢量）觀測值以及該矢量在參考坐標(biāo)系下的表示值；ρk為記憶因子，取值在0至1之間，用于逐步舍棄最初的歷史測量信息，避免逐步累積的測量誤差嚴(yán)重污染K矩陣。k+1時刻的姿態(tài)四元數(shù)qk+1為與矩陣Kk+1/k+1的最大特征值相對應(yīng)的歸一化的特征向量。

由式（1）和式（4）知，k+1時刻的姿態(tài)估計結(jié)果由兩部分組成，第一項為姿態(tài)預(yù)測，由三軸陀螺儀得到，第二項為姿態(tài)修正（姿態(tài)修正增益分別為Kk+1和1 mk+1），由三軸加速度計和三軸地磁傳感器得到。由式（1）可知，卡爾曼增益陣Kk+1中的元素值越小，則姿態(tài)修正量越小，估計結(jié)果越依賴于陀螺儀，反之則越依賴于加速度計和地磁傳感器組合。狀態(tài)方程非線性的卡爾曼算法由于其觀測陣為單位陣，其卡爾曼增益陣中的元素數(shù)量級恒定維持在101左右，這就表明狀態(tài)方程非線性的卡爾曼算法側(cè)重于依賴加速度計和地磁傳感器組合。觀測方程非線性的卡爾曼算法的過程噪聲協(xié)方差陣中的元素與采樣時間的平方成正比，而通常慣性-地磁組合的采樣時間在幾十毫秒，因此，由式（2）和（3）不難發(fā)現(xiàn)卡爾曼增益被嚴(yán)重衰減，這表明觀測方程非線性的卡爾曼算法更傾向于依賴陀螺儀信息。由式（4）可知，隨著時間的增長，1 mk+1逐漸衰減至0，mkmk+1逐漸接近1，這就表明REQUEST算法對陀螺儀的依賴程度更甚于觀測方程為非線性的卡爾曼算法。以上分析表明狀態(tài)方程非線性的卡爾曼算法的靜態(tài)精度比觀測方程為非線性的卡爾曼算法和REQUEST算法要高，而動態(tài)精度要差。

為了獲得更好的靜動態(tài)性能，一個自然的想法是觀測噪聲的方差與運動體線加速度的大小成正比，進(jìn)而通過觀測噪聲方差間接調(diào)整姿態(tài)修正增益，使得在運動體運動時加大對陀螺儀的依賴。由于狀態(tài)方程非線性的卡爾曼算法觀測矩陣為單位陣，由式（2）不難看出該算法的增益陣幾乎不受過程噪聲和測量噪聲的影響，所以通過干預(yù)過程噪聲或者測量噪聲的辦法來干預(yù)姿態(tài)修正增益幾乎沒有效果。而由式（4）可以看出，姿態(tài)修正增益1 mk+1隨時間逐漸衰減，對其進(jìn)行調(diào)整的結(jié)果只能是減緩其衰減的速度，因此調(diào)整REQUEST算法的姿態(tài)修正增益是沒有意義的?；谏鲜鲈颍疚牟扇崟r調(diào)整觀測方程非線性的卡爾曼算法的觀測噪聲的方法來達(dá)到此目的。

假設(shè)x=vg×vm，其中“×”表示兩矢量的向量積。對x求導(dǎo)得到：

取測量方程

其中C為單位陣。

將式（9）與式（10）組合在一起，并考慮過程噪聲及觀測噪聲后形成如下方程組：

之所以考慮過程噪聲及觀測噪聲，是由于觀測矢量vg、vm和ω本身存在測量噪聲，而且[ ]ω×、u(t)只能采用這些觀測矢量計算。將式（11）離散化，得到：

其中，k表示具體的采樣時刻。

ξ(t)和η(t)可以由式（15）得到：

由式（12）及式（15）可以發(fā)現(xiàn)：（1）過程噪聲和觀測噪聲并不為白噪聲；（2）兩種噪聲之間是相關(guān)的；（3）兩種噪聲的方差陣及兩者之間的協(xié)方差陣在不斷變化。為簡化計算起見，可以否定上述三條結(jié)論，針對式（15）的大量仿真計算也說明了這一點?？梢远x

其中Q、R為常量，可以利用式（15）估算，然后根據(jù)卡爾曼濾波效果再做調(diào)整?？柭鼮V波公式限于篇幅略去。

待x(k)估計出后，可利用式（17）計算估計殘差：

其中，ρ＞0殘差調(diào)整因子。值得指出的是，Allan方差分析表明，加速度計等慣性器件的輸出零偏和隨機游走屬于低頻成分，卡爾曼濾波器屬于低通濾波器，其本身又是無偏估計，所以加速度計的輸出零偏和隨機游走會直接通過卡爾曼濾波器而疊加到其輸出中，這就使得利用

圖2 靜態(tài)測試條件下MTi姿態(tài)輸出結(jié)果

式（17）計算出的估計殘差中不會包含任何有關(guān)加速度計的輸出零偏和隨機游走的成分。上述結(jié)論表明加速度計的輸出零偏和隨機游走并不影響本文算法。

估計殘差的方差可利用滑窗估計的方法（式（18））實時計算

其中，n為截取的滑窗長度。

觀測方程非線性的卡爾曼算法的觀測噪聲的協(xié)方差可按照下式實時進(jìn)行調(diào)整：

3 實驗結(jié)果

3.1 實驗設(shè)置

如圖1所示。實驗時將新近購得的兩種慣性-地磁組合MTi和ADIS16480通過扎帶固定于滑臺上，滑臺可以用手驅(qū)動在滑軌上做往復(fù)直線運動。MTi的基本噪聲參數(shù)為陀螺儀0.34°/s、加速度計0.008 m/s2、地磁傳感器0.1 μT，ADIS16480的基本噪聲參數(shù)則分別為0.16 °/s、0.014 7 m/s2及0.045 μT。這兩種慣性地磁組合的采樣率分別為256 Hz和307.5 Hz。本文算法以ADIS16480輸出的原始傳感器數(shù)據(jù)解算姿態(tài)。姿態(tài)用歐拉角表示，并且為了清晰展示，姿態(tài)解算結(jié)果已經(jīng)除去了其均值量。Q=0.01，R=0.1，ρ =100，n=30[3]。

圖1 實驗設(shè)置

3.2 靜態(tài)測試

圖3 靜態(tài)測試條件下ADIS16480姿態(tài)輸出結(jié)果

將兩種慣性-地磁組合在滑臺上靜止一段時間，以測試靜態(tài)精度。圖2和圖3分別為MTi和ADIS16480內(nèi)置的卡爾曼算法的姿態(tài)解算結(jié)果，圖4為本文姿態(tài)解算結(jié)果，圖5為REQUEST算法的姿態(tài)解算結(jié)果，其中REQUEST算法的記憶因子取為0.9。

3.3 動態(tài)測試

在手帶動下使滑臺在滑軌上做一段時間的往復(fù)直線運動。圖6～8為ADIS16480輸出的傳感器原始數(shù)據(jù)，圖9～12為姿態(tài)解算結(jié)果，圖13為估計殘差及其方差估計結(jié)果。

3.4 結(jié)果分析

對比圖2～5可以發(fā)現(xiàn)，在靜止情況下ADIS16480內(nèi)置卡爾曼算法和本文算法的姿態(tài)解算精度相當(dāng)且較高，均傾向于依賴加速度計和地磁傳感器信息，而MTi內(nèi)置的卡爾曼算法的姿態(tài)解算結(jié)果出現(xiàn)了發(fā)散現(xiàn)象，這說明MTi更傾向于依賴陀螺儀信息，REQUEST算法對陀螺儀信息依賴更嚴(yán)重，使得其姿態(tài)解算結(jié)果發(fā)散現(xiàn)象更嚴(yán)重，這也是為何REQUEST算法能夠應(yīng)用于廣泛使用光纖、激光或者靜電陀螺的航空航天領(lǐng)域而無法在普遍使用MEMS陀螺的民用領(lǐng)域獲得推廣的根本原因。

圖4 靜態(tài)測試條件下本文姿態(tài)解算結(jié)果

圖5 靜態(tài)測試條件下REQUEST算法姿態(tài)結(jié)算結(jié)果

圖6 動態(tài)測試條件下ADIS16480陀螺儀輸出結(jié)果

圖7 動態(tài)測試條件下ADIS16480加速度計輸出結(jié)果

圖8 動態(tài)測試條件下ADIS16480地磁傳感器輸出結(jié)果

圖9 動態(tài)測試條件下MTi姿態(tài)輸出結(jié)果

圖10 動態(tài)測試條件下ADIS16480姿態(tài)輸出結(jié)果

圖11 本文姿態(tài)解算結(jié)果

圖12 動態(tài)測試條件下REQUEST算法姿態(tài)解算結(jié)果

圖13 估計殘差及其方差

由圖6～8所示，如果在某段時間內(nèi)（圖中的1.85～23.9 s）使滑臺做直線往復(fù)運動而保持姿態(tài)不變，那么各種算法的姿態(tài)解算結(jié)果如圖9～12所示，很明顯，由于姿態(tài)保持不變，那么如果某種算法姿態(tài)解算結(jié)果出現(xiàn)變化，此變化量即為誤差，ADIS16480仍然傾向于依賴加速度計和地磁傳感器信息，這使得其姿態(tài)解算結(jié)果出現(xiàn)大幅擺動，俯仰角擺幅近似在±10°以上，而MTi傾向于依賴陀螺儀信息，所以其姿態(tài)解算結(jié)果的擺動較小，俯仰角擺幅近似在±1°左右，REQUEST算法給出的姿態(tài)解算結(jié)果也出現(xiàn)擺動，但是擺動逐步衰減，運動初始時刻俯仰角擺幅近似為±4°，而運動終止時近似在±0.75°左右，REQUEST算法對陀螺儀的依賴程度是一個逐步增大的過程，因此，如果在使滑臺運動之前使其處于靜止?fàn)顟B(tài)更長一段時間，那么其動態(tài)精度會更好一些。由圖13可以看到估計殘差在滑臺運動時會增大，本文利用這一特點實現(xiàn)的自適應(yīng)擴展卡爾曼算法的姿態(tài)解算結(jié)果如圖11所示，很明顯其俯仰角擺幅只有±0.2°左右，是上述所有算法中擺幅最小的，這說明在滑臺運動時，通過自適應(yīng)調(diào)整觀測噪聲，使算法在此時加大對陀螺儀的依賴，從而減少動態(tài)誤差，但同時也應(yīng)看到，由于對陀螺儀誤差加大，算法出現(xiàn)緩慢發(fā)散現(xiàn)象。

4 結(jié)論

本文在觀測方程非線性卡爾曼算法的基礎(chǔ)上給出了一種新的自適應(yīng)擴展卡爾曼算法，該方法利用線性卡爾曼算法實時估計‖，并將估計殘差作為用于姿態(tài)解算的擴展卡爾曼算法的觀測噪聲，原因在于該估計殘差在運動體靜止和運動時方差并不相同，這樣就使得該算法可以兼具更好的靜動態(tài)性能。由于采用線性卡爾曼算法，本文提出的方法并不會大幅度提高擴展卡爾曼算法的運行時間。

未來將在以下兩個方面繼續(xù)開展研究：（1）事實上，估計殘差雖然為零均值且平穩(wěn)，但并不為白噪聲，這將使得本文提出的自適應(yīng)擴展卡爾曼算法并不是最優(yōu)的，因此需要進(jìn)一步做白化處理；（2）殘差調(diào)整因子ρ的取值是根據(jù)實驗調(diào)整的，并沒有根據(jù)某種準(zhǔn)則做最優(yōu)選取。

[1]Yun X P，Bachmann E R.Design，implementation，and experimental results of a quaternion-based Kalman filter forhuman body motion tracking[J].IEEE Transactions on Robotics，2006，22（6）：1216-1227.

[2]Sabatelli S，Galgani M，F(xiàn)anucci L，et al.A double stage Kalman filter for sensor fusion and orientation tracking in 9D IMU[C]//Proceedings of IEEE Symposium on Sensors Applications，Brescia，Italy，2012：1-5.

[3]Sessa S，Zecca M，Lin Z H，et al.A methodology for the performance evaluation of inertial measurement units[J].Journal of Intelligent&Robotic System，2013，71（2）：143-157.

[4]Sabatini A M.Kalman-filter-based orientation determina-tion using inertial/magnetic sensors：Observability analysis and performance evaluation[J].Sensors，2011，11（10）：9182-9206

[5]Ren H L，Kazanzides P.Investigation of attitude tracking using an integrated inertial and magnetic navigation system for hand-held surgical instruments[J].IEEE/ASME Transactions on Mechatronics，2012，17（2）：210-217.

[6]Gallagher A，Matsuoka Y，Ang W T.An efficient realtime human posture tracking algorithm using low-cost inertial and magnetic sensors[C]//Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems，Sendai，Japan，2004：2967-2972.

[7]Harms H，Amft O，Winkler R，et al.ETHOS：Miniature orientation sensor for wearable human motion analysis[C]//Proceedings of IEEE Conference on Sensors，Kona，USA，2010：1037-1042.

[8]Campolo D，Taffoni F，F(xiàn)ormica D.Inertial-magnetic sensors for assessing spatial cognition in infants[J].IEEE Transactions on Biomedical Engineering，2011，58（5）：1499-1503.

[9]Madgwick S O H，Harrison A J L，Vaidyanathan R.Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//Proceedings of IEEE International Conference on Rehabilitation Robotics，Zurich，Switzerland，2011：1-7.

[10]戎海龍，戴先中，劉信宇.烹飪過程中鍋具運動姿態(tài)測量方法[J].中國慣性技術(shù)學(xué)報，2009，17（4）：419-423.

[11]Bar-itzhack I Y.Request：A recursive QUEST algorithm for sequential attitude determination[J].Journal of Guidance，Control and Dynamics，1996，19（5）：1034-1038.

[12]Choukroun D，Bar-itzhack I Y，Oshman Y.Optimal-REQUEST algorithm for attitude determination[J].Journal of Guidance，Control and Dynamics，2004，27（3）：418-425.

[13]Choukroun D.Adaptive optimal-request algorithm for attitude determination[C]//Proceedings of AIAA Guidance，Navigation，and Control Conference，Hilton Head，USA，2007：4624-4647.

[14]Shuster M D.Filter QUEST or REQUEST[J].Journal of Guidance，Control and Dynamics，2009，32（2）：643-645.

[15]卞鴻巍，金志華，王俊璞，等.組合導(dǎo)航系統(tǒng)新息自適應(yīng)卡爾曼濾波算法[J].上海交通大學(xué)學(xué)報，2006，40（6）：1000-1003.