馬正華，盧成俊，戎海龍，賀小捧

(常州大學 信息科學與工程學院， 江蘇 常州 213000)

?

基于Kalman濾波算法的陀螺儀動態(tài)漂移補償研究

馬正華，盧成俊，戎海龍，賀小捧

(常州大學 信息科學與工程學院， 江蘇 常州 213000)

應用MEMS陀螺儀測量人體手臂運動姿態(tài)時，針對陀螺儀受線加速度干擾導致測量姿態(tài)發(fā)散的問題，提出基于Kalman濾波算法的姿態(tài)誤差補償方法；該方法首先將陀螺儀采集到的角速度通過方向余弦算法解算得到姿態(tài)角，并將陀螺儀動態(tài)漂移造成的姿態(tài)角誤差視為時變信號，通過建立姿態(tài)角漂移誤差的狀態(tài)方程及觀測方程，應用卡爾曼濾波算法，實現(xiàn)對姿態(tài)角漂移誤差的估計，最終達到對陀螺儀動態(tài)漂移誤差的補償；實驗與仿真結果表明，應用該算法能夠有效的抑制線加速度干擾導致的陀螺儀測量的姿態(tài)發(fā)散，適用于陀螺儀對人體手臂運動姿態(tài)的測量。

姿態(tài)測量；方向余弦算法；卡爾曼濾波算法；姿態(tài)補償

0 引言

獲取真實準確的人體運動姿態(tài)信息是對人體開展生物力學方面的研究和對人體的各種運動信息進行檢測、跟蹤以及統(tǒng)計分析的基礎。MEMS慣導器件由于體積小、成本低、易于數(shù)字化和動態(tài)測量特性良好等優(yōu)點成為人體姿態(tài)測量的理想選擇[1-3]。對陀螺儀輸出進行積分便能得到姿態(tài)信息[4]，而由于本身特性的限制， MEMS陀螺儀的輸出噪聲以及隨機漂移導致解算得到的姿態(tài)產生漂移誤差[5-6]，并隨時間推移而累積增大，因此陀螺儀通常與其他姿態(tài)傳感器組合使用獲得穩(wěn)定可靠的姿態(tài)信息。目前傳統(tǒng)的方法是采用融合算法，以加速度傳感器、地磁傳感器輸出姿態(tài)角來修正陀螺儀的輸出。

Sabatini AM提出了一種VSD-EKF(variable-state imension extend kalman filter)算法,該算法假設目標物體在緩慢移動,即假設載體沒有線加速度干擾,用 MARG 傳感器估計人體三維姿態(tài)信息,但該算法解算精度只排除了環(huán)境磁場影響和陀螺儀隨機誤差影響,沒有考慮到人體姿態(tài)變化帶來的加速度變化導致加速度計干擾的情況[7]。文獻[8]采用的測量融合方法將加速度傳感器和陀螺儀傳感器數(shù)據(jù)進行加權平均，可應用于動態(tài)環(huán)境，但測量精度不高。Rehbinder H等人[9]基于陀螺儀和加速計兩種類型的傳感器，提出了一個姿態(tài)解算算法，但由于缺少磁強計信息，而使得航向角的累積誤差不能得到消除。文獻[10]通過建立加速度和陀螺儀的誤差數(shù)據(jù)模型，采用卡爾曼濾波方法預測飛行器的飛行姿態(tài)，能夠較好抑制陀螺儀漂移造成的姿態(tài)發(fā)散，但不適用于運動狀態(tài)變化較快的場景。文獻[11]和文獻[12]采用了融合了加速度計和地磁計的擴展卡爾曼(EKF)算法，但在載體存在線加速度時，加速度傳感器給出的是運動加速度和重力加速度的疊加值，此時計算出的載體姿態(tài)角就有較大的誤差；同時在室內使用地磁計時特別容易受周圍環(huán)境的干擾而產生隨機誤差，影響磁強計的輸出[13]。

由于人體姿態(tài)運動的特殊性譬如人體手臂運動，運動時常常伴隨著短時間內線加速度有較大幅度變化等情況。在這種情況下，由于加速度計輸出的信息既包含重力加速度信息又包含運動加速度信息，導致采用融合算法受加速計輸出信息不準確的影響其解算得到的姿態(tài)信息其與實際姿態(tài)信息存在很大誤差[14]。因此本文針對以上融合算法在線加速度干擾情況下手臂姿態(tài)解算誤差較大的問題，采取單獨使用陀螺儀的輸出信號解算姿態(tài)，避免引入加速度計和地磁計受干擾的數(shù)據(jù)。而在利用陀螺儀對運動載體姿態(tài)進行測量時，載體線加速度能夠使陀螺儀產生漂移[15]，進而產生姿態(tài)誤差，即載體姿態(tài)漂移程度與陀螺儀所測載體的運動復雜度有關[16]。因此，本文將姿態(tài)漂移誤差視為時變信號，認為該姿態(tài)漂移誤差大小與上一時刻的的姿態(tài)漂移誤差大小及當前時刻姿態(tài)角的變化量相關，據(jù)此建立陀螺儀姿態(tài)漂移誤差狀態(tài)方程；選取陀螺儀輸出姿態(tài)角的低通濾波作為陀螺儀姿態(tài)漂移誤差觀測方程，在此基礎上，應用卡爾曼濾波算法，對姿態(tài)漂移誤差進行估計，從而達到補償姿態(tài)誤差，提高慣性器件測量手臂運動姿態(tài)精度的目的。

1 關于四元數(shù)與方向余弦矩陣的姿態(tài)角分析

通常將慣性測量單元固定安裝在運動載體上，由運載體的機體軸確定的坐標系一般稱之為載體坐標系b，與之相對應的慣導系統(tǒng)所采用的坐標系稱為導航坐標系n。確定載體的姿態(tài)和航向實際上就是確定載體坐標系相對于導航坐標系的方位關系。而載體坐標系相對于導航坐標系的轉換可以通過四元數(shù)法或者歐拉角法實現(xiàn)，由于四元數(shù)法可以避免歐拉角法的奇異問題，因而被廣泛采用。經3次旋轉后的導航坐標系和機體坐標系之間的坐標關系可由一個矩陣來表示，即方向余弦矩陣：

(1)

利用四元數(shù)可確定出導航坐標系至載體坐標系的方向余弦矩陣:

(2)

(3)

(4)

由 式(1)(2)便可求得姿態(tài)角：

(5)

2 卡爾曼濾波器的設計

本文將陀螺儀動態(tài)漂移造成的姿態(tài)誤差視為時變信號，并建立相應的狀態(tài)方程和觀測方程，通過卡爾曼濾波得到較為準確的姿態(tài)誤差估計值，從而補償姿態(tài)角的誤差。圖1所示為卡爾曼濾波過程設計框圖。

圖1 卡爾曼濾波過程設計框圖

2.1 陀螺儀姿態(tài)漂移誤差的狀態(tài)方程

利用陀螺儀測量手臂運動姿態(tài)，陀螺儀傳感器信號經過AD轉換器變成計算機可處理的數(shù)字信號。在使用這些數(shù)字信號進行手臂姿態(tài)解算前還應補償陀螺儀的靜態(tài)隨機漂移，以減少陀螺儀靜態(tài)隨機漂移對姿態(tài)解算造成的影響。經過處理后陀螺儀角速度通過方向余弦算法便可得到三軸姿態(tài)角。由于線加速度的干擾造成陀螺儀輸出的漂移，解算得到的姿態(tài)角與實際姿態(tài)存在一個姿態(tài)漂移誤差，即在k時刻，實際姿態(tài)角與解算得到的姿態(tài)角存在如下關系：

(6)

式中,θ(k)′為k時刻實際姿態(tài)角，θ(k)為k時刻陀螺儀輸出的角速度通過方向余弦算法解算得到的姿態(tài)角，error(k)為k時刻的姿態(tài)漂移誤差。

在利用陀螺儀對運動載體進行測量時，載體線加速度使陀螺儀產生漂移造成的姿態(tài)誤差，與陀螺儀所測載體的運動復雜度有關[16]。本文將該運動復雜度描述為k時刻姿態(tài)角相對于k-1時刻姿態(tài)角的變化量，并認為由陀螺儀漂移帶來的姿態(tài)誤差是累積的，據(jù)此建立姿態(tài)漂移誤差模型方程：

(7)

其中:error(k)為k時刻的姿態(tài)漂移誤差，error(k-1)為k-1時刻姿態(tài)誤差，δ為某一常數(shù)。按照以下方式選取系統(tǒng)狀態(tài)矩陣X=errork，A=I，B=δ，控制量u(k)=θ(k)-θ(k-1)，便可建立姿態(tài)漂移誤差的狀態(tài)方程：

(8)

2.2 陀螺儀姿態(tài)漂移誤差的觀測方程

陀螺儀傳感器輸出的是角速度，當靜置時，陀螺儀輸出的角速度便是陀螺儀的靜態(tài)漂移。而當陀螺儀測量運動載體時，陀螺儀輸出的是載體的角速度，由線加速度導致的陀螺儀動態(tài)漂移無法直接測量得到，因此可將角速度經積分變?yōu)樽藨B(tài)角，通過姿態(tài)角誤差來表征載體運動時陀螺儀的漂移情況。本文采用陀螺儀在k時刻及k時刻前幾個時刻解算得到的姿態(tài)角之間的低通濾波，作為陀螺儀漂移造成的姿態(tài)漂移誤差的觀測量，即姿態(tài)漂移誤差觀測量為：

(9)

其中:k為采樣時間，a、b、c為某一常數(shù)，滿足a+b+c=1，θ(k)、θ(k-1)、θ(k-2)、θ(k-3)分別為陀螺儀在k、k-1、k-2、k-3時刻解算得到的姿態(tài)角。

2.3 Kalman濾波算法的實現(xiàn)

基于以上姿態(tài)漂移誤差的狀態(tài)方程和觀測方程應用Kalman濾波算法，以對姿態(tài)誤差進行估計，從而達到補償姿態(tài)，提高慣性器件測量手臂運動姿態(tài)精度的目的。

卡爾曼濾波過程分為以下幾步實現(xiàn)：

1)首先由陀螺儀輸出的k時刻、k-1時刻的角速度通過方向余弦算法得到對應時刻的姿態(tài)角θ(k)、θ(k-1)，則由(7)式可得姿態(tài)漂移誤差的狀態(tài)方程：

error(k/k-1)=error(k-1/k-1)+δ·(θ(k)-

(10)

可估計得到k時刻利用上一狀態(tài)預測的結果，其中error(k-1/k-1)是k-1時刻的姿態(tài)漂移誤差最優(yōu)的結果，設定0時刻的error(0)為0，δ的值為某一常數(shù)。

2)更新對應于error(k/k-1)的協(xié)方差：

(11)

其中:A=I，P(k-1/k-1)是error(k-1/k-1)對應的協(xié)方差，sigQ是系統(tǒng)過程的協(xié)方差。

3)計算卡爾曼增益：

(12)

其中:H=I，sigR為觀測方程系統(tǒng)過程協(xié)方差。

4)更新k時刻姿態(tài)誤差的最優(yōu)估計：

error(k/k)=error(k/k-1)+Kg(k)(Z(k)-

(13)

其中:Z(k)是姿態(tài)誤差的觀測值。

5)更新k時刻error(k/k)的協(xié)方差：

(14)

這樣，就基本完成k時刻Kalman濾波算法的所有計算，k時刻姿態(tài)漂移誤差的最優(yōu)估計error(k/k)的大小即為姿態(tài)角需補償?shù)拇笮　．斚到y(tǒng)進行到k+1時刻，回到第1步進行計算，式(12)和式(14)保證濾波算法的持續(xù)性與遞歸性。

3 實驗與仿真研究

為驗證以上基于Kalman濾波的陀螺儀動態(tài)漂移補償算法的有效性，實驗選用了Xsens公司提供的慣性測量單元MTi10系列傳感器模塊來采集實驗數(shù)據(jù)，MTI慣性測量單元包括三軸陀螺儀、三軸加速度計和三軸磁強計，本實驗僅選取其中三軸陀螺儀的輸出數(shù)據(jù)。同時，實驗還采用POLHEMUS公司提供的LongRanger電磁跟蹤系統(tǒng)，該裝置能對載體空間姿態(tài)進行精確測量，可作為慣性系統(tǒng)姿態(tài)輸出的對照基準。根據(jù)選用的測量單元，由官方手冊資料查詢得到陀螺儀的標準差為0.006rad/s，MTI慣性測量單元和LongRanger電磁跟蹤系統(tǒng)采樣頻率分別是256Hz和240Hz。實驗所做的手臂運動可以描述為：手握測量裝置在近似水平面內做圓周運動，同時手部也做一定的角度擺動，運動約10s左右后，停止動作，并將手臂恢復至初始姿態(tài)。

基于采集的數(shù)據(jù)，并根據(jù)MTI慣性測量單元與電磁跟蹤系統(tǒng)裝置的采樣頻率的不同對采集的數(shù)據(jù)作出必要的處理后，得到圖2所示陀螺儀采集的實測數(shù)據(jù)解算出的姿態(tài)變化曲線同電磁跟蹤系統(tǒng)實測數(shù)據(jù)解算出的實際姿態(tài)變化曲線的對比。可以看到，橫滾角的陀螺儀漂移最為明顯，10s后姿態(tài)解算誤差最大已經達到10°，這是因為，手部擺動運動帶來了線加速度，而線加速度的干擾導致了陀螺儀漂移，進而導致解算出的姿態(tài)開始發(fā)散；俯仰角和偏航角方向受線加速度干擾影響較小，由陀螺儀解算得到的姿態(tài)信息大體體現(xiàn)了手臂實際運動姿態(tài)，但由于陀螺儀的隨機漂移影響，其姿態(tài)累計誤差在逐漸增大，所得的姿態(tài)信息在緩慢發(fā)散。

圖2 陀螺儀解算得到的姿態(tài)與電磁跟蹤系統(tǒng)得到的
實際運動姿態(tài)變化曲線對比

圖3所示為經本文算法補償過的姿態(tài)曲線與陀螺儀采集的實測數(shù)據(jù)解算出的姿態(tài)曲線和電磁跟蹤系統(tǒng)解算得到的實際運動姿態(tài)曲線的對比。可以看到，經本文算法補償過后，橫滾角姿態(tài)發(fā)散情況得到抑制，其解算誤差縮小在3°左右，姿態(tài)測量精度相比使用本文算法前得到提高，體現(xiàn)了本文算法在補償由于陀螺儀動態(tài)漂移導致姿態(tài)誤差的有效性；同樣地，俯仰角和偏航角的姿態(tài)解算誤差相比原先縮小在1°左右，測量精度也得到一定提高。

圖3 加入本文算法補償后姿態(tài)變化曲線對比

4 結論

應用MEMS慣導器件測量人體手臂運動姿態(tài)時，針對陀螺儀受線加速度干擾導致姿態(tài)發(fā)散的問題，本文介紹了一種基于Kalman濾波算法的姿態(tài)誤差補償方法。該方法將姿態(tài)漂移誤差視為時變信號，據(jù)此建立陀螺儀姿態(tài)漂移誤差的狀態(tài)方程和觀測方程，在此基礎上，應用卡爾曼濾波算法，實現(xiàn)對姿態(tài)角漂移誤差的估計，最終達到對陀螺儀動態(tài)漂移誤差的補償。實驗與仿真結果表明，本文算法能夠有效的抑制由線加速度干擾導致的陀螺儀測量的姿態(tài)發(fā)散，適用于陀螺儀對人體手臂運動姿態(tài)的測量。

[1]RoetenbergD,LuingeHJ,BatenC,etal.Compensationofmagneticdisturbancesimprovesinertialandmagneticsensingofhumanbodysegmentorientation[J].NeuralSystemsandRehabilitationEngineering,IEEETransactionson, 2005, 13(3): 395-405.

[2]VeltinkPH,SlyckeP,HemssemsJ,etal.Threedimensionalinertialsensingoffootmovementsforautomatictuningofatwo-channelimplantabledrop-footstimulator[J].Medicalengineering&physics, 2003, 25(1): 21-28.

[3] 李金鳳, 王慶輝, 劉曉梅,等. 基于MEMS慣性器件的行人室內定位系統(tǒng)[J]. 計算機測量與控制, 2014, 22(11):3761-3763.

[4]MillerRB.Anewstrapdownattitudealgorithm[J].JournalofGuidance,Control,andDynamics, 1983, 6(4): 287-291.

[5] 王 帥, 魏 國. 卡爾曼濾波在四旋翼飛行器姿態(tài)測量中的應用 [J]. 兵工自動化, 2011, 30(1): 73-80.

[6] 秦永元，張洪鉞，汪淑華.卡爾曼濾波與組合導航原理[M].西安：西北工業(yè)大學出版社，1998.

[7]SabatiniAM.Variable-State-DimensionKalman-basedFilterfororientationdeterminationusinginertialandmagneticsensors[J].Sensors, 2012, 12(7): 8491-506.

[8] 李媛媛, 張立峰. 多傳感器自適應加權融合算法及其應用研究[J]. 自動化與儀器儀表, 2008(2):10-13.

[9]RehbinderH,HuX.Drift-freeattitudeestimationforacceleratedrigidbodies☆[A].RoboticsandAutomation, 2001.Proceedings2001ICRA.IEEEInternationalConferenceon[C].IEEE, 2001(4):4244-4249.

[10] 馮智勇, 曾 瀚, 張 力,等. 基于陀螺儀及加速度計信號融合的姿態(tài)角度測量[J]. 西南師范大學學報:自然科學版, 2011, 36(4):137-141.

[11]YunX,BachmannER.Design,implementation,andexperimentalresultsofaquaternion-basedKalmanfilterforhumanbodymotiontracking[J].Robotics,IEEETransactionson, 2006, 22(6): 1216-1227.

[12] 吳 杰, 閆建國. 基于修正的卡爾曼濾波的姿態(tài)估計算法研究[J]. 計算機仿真, 2012, 29(2):54-57.

[13]BirdJ,ArdenD.Indoornavigationwithfoot-mountedstrapdowninertialnavigationandmagneticsensors[EmergingOpportunitiesforLocalizationandTracking][J].IEEEWirelessCommunications, 2011, 18:28-35.

[14] 戎海龍, 戴先中, 劉信羽. 廚師手握鍋具運動信息獲取方法[J]. 儀器儀表學報, 2009 (11): 2291-2297.

[15]LiuC,ZhouZ,FuX.AttitudedeterminationforMAVsusingaKalmanfilter[J].TsinghuaScience&Technology, 2008, 13(5): 593-597.

[16]LambrechtS,JonkersI,PonsJL.Identificationanddecompositionoferrorin3Dmotioncaptureusinginertialandmagneticsensor.ConvergingClinicalandEngineeringResearchonNeurorehabilitationBiosystems&Biorobotics[Z]. 2013, 1: 721-725.

Research on Compensation of Dynamic Drift of Gyroscope Based on Kalman Filtering Algorithm

Ma Zhenghua，Lu Chengjun，Rong Hailong，He Xiaopeng

(College of Information Science and Engineering，Changzhou University，Changzhou 213000,China)

In order to solve the problem of attitude divergence caused by gyroscope which is interfered by the linear acceleration when use MEMS inertial navigation device to measure the attitude of the human arm movement, propose an attitude error compensation method based on Kalman filter algorithm. In this method, the attitude angle will be calculated first by angular velocity gyroscope output through direction cosine algorithm solution, through the establishment of the attitude angle drift error state equation and observation equation, attitude angle drift error will be estimated accurately and eliminated then. The experimental and simulation results show that , algorithm used in this paper can suppress the divergent attitude effectively and is suitable for gyroscope to measure the attitude of the human arm movement.

attitude measurement；direction cosine algorithm；Kalman filtering algorithm；attitude compensation

2016-03-19；

2016-04-25。

馬正華(1962-)，男，江蘇昆山人，教授，碩士研究生導師，主要從事嵌入式系統(tǒng)應用、計算機控制技術、電力電子技術等方向的研究。

1671-4598(2016)09-0191-04

10.16526/j.cnki.11-4762/tp.2016.09.054

TP3

A