基于MIMU和磁力計的姿態更新算法研究*

米 剛,田增山,金 悅,李 澤,周 牧

(重慶郵電大學移動通信技術重慶市重點實驗室,重慶 400065)

?

基于MIMU和磁力計的姿態更新算法研究*

米 剛,田增山*,金 悅,李 澤,周 牧

(重慶郵電大學移動通信技術重慶市重點實驗室,重慶 400065)

針對傳統人體姿態解算算法中存在MEMS陀螺誤差發散快的問題,提出一種基于微慣性測量單元(MIMU)及磁力計信息融合的姿態解算算法。該算法利用互補濾波結合PI調節控制完成陀螺零偏校正,然后在加速度計和磁強計的輔助校正下,通過EKF(Expand Kalman Filter)濾波器更新四元數法實現陀螺姿態解算。本算法采用MPU9150傳感器模塊完成測試實驗,實驗中對比分析了單獨擴展卡爾曼濾波算法與本算法的濾波效果。實驗結果表明,本算法能夠有效地抑制陀螺的發散,實現穩定地輸出高精度姿態數據。

傳感器;姿態解算;陀螺校正;互補濾波;PI調節;EKF

人體姿態的準確解算是實現行人航跡推算系統的基礎,其在行人定位系統的研究中起著很重要的作用。隨著MEMS(Micro-Electro-Mechanical Systems)及計算機技術的快速發展,由陀螺、加速度計組成的微慣性測量單元MIMU(Miniature Inertial Measuringunit),輔以微磁力計,組成了航向姿態參考系統AHRS(Attitude and Heading Reference System),其可實現載體的全姿態參數測量。在AHRS中,加速計同時對重力加速度和運動加速度敏感,并且磁傳感器易受鐵磁性物質干擾,所以利用加速度計和磁傳感器的測量值解算姿態角在載體運動情況下易產生較大誤差。同時MEMS陀螺也存在零偏問題,短時間內陀螺偏移也會引起較大的姿態誤差。

常用的解決上述問題的方法是采用互補濾波和卡爾曼濾波器。互補濾波在定姿算法中用于濾除加速度計存在的線加速度,以及磁力計受到干擾等因素引起的測量信息中的高頻噪聲,同時還可以消除陀螺零偏及其他誤差造成的積分低頻噪聲漂移,使兩種來源的姿態信息互補,得到準確的姿態輸出[1-2]?？柭鼮V波則利用來自衛星定位系統、加速度計和磁力計等傳感器的數據來補償陀螺偏差引起的姿態誤差。Rehbinder H等人利用陀螺儀和加速度傳感器信號進行融合[3],從而修正陀螺儀的偏差,該方法僅能減少水平面傾角的漂移,無法修正偏航角誤差且易受運動加速度等的干擾。Elder M Hemerly[4]等人利用GPS修正加速度信息,從而利用補償后的加速度結合陀螺儀和磁力計完成姿態的更新過程,但是其對GPS的性能和可用性提出了要求。Brigante C M N等人設計了基于MEMS的小型化可穿戴運動捕捉系統[5],提出采用擴展卡爾曼濾波器對陀螺儀、加速度傳感器、磁阻傳感器3種姿態數據進行融合,獲得了精度較高的人體姿態。但受限于MEMS器件的精度問題,需要卡爾曼濾波器在融合過程中對誤差信號進行精確建模并建立穩定的非線性觀測方程。Madgwick S[6-7]采用梯度下降算法對加速度計和磁強計測得的數據進行處理,并將其通過互補濾波器與陀螺儀測得的結果融合,修正陀螺偏移,但梯度下降算法本身計算比較復雜,同時互補濾波適用的精度有限。

針對上述眾多問題,本文設計了一種基于微慣性測量單元(MIMU)及磁力計信息融合的姿態解算算法。利用小型集成IMU采集人體運動信息,求取磁力計和加速度計的觀測值,并將其作為PI調節器[6]的誤差輸入,同時結合互補濾波算法修正陀螺儀角速度輸出,將修正后的陀螺儀角速度數據與加速度計、磁力計數據通過EKF(Extend Kalman Filter)濾波器處理,實現姿態精確解算。

1 算法總體結構

本文將加速度計和磁力計計算的誤差向量作為PI調節的輸入,結合互補濾波算法校正陀螺儀的角速度輸出,然后通過EKF濾波器進行姿態角求解。姿態解算前,傳感器數據均經過中值濾波去噪處理。本文設計的行人姿態解算算法流程如圖1所示。

圖1 系統算法框圖

姿態更新系統總體分為三部分,第1部分利用起始姿態角求取初始四元數,以便給EKF的起始狀態賦初值;第2部分利用互補濾波器與PI控制對角速度進行校正,同時將校正后的角速度值送入EKF;第3部分EKF對四元數進行更新,將更新后的四元數求取最終的姿態角。

2 利用起始姿態角計算初始四元數

通常將慣性測量單元中陀螺儀、加速度計和磁強計的測量數據定義到載體坐標系b,取載體的重心為載體坐標系原點,3個軸分別與載體的縱軸、橫軸和豎軸相互重合,分別定義為右前上(XYZ)。為繼續后面的計算,載體坐標系要轉換到導航坐標系n(東北天ENU)。一般載體坐標系向導航坐標系的轉換可以通過四元數法或Euler角法實現,由于四元數法可以避免Euler角的奇異問題,應用更加廣泛,本文使用四元數法進行坐標系轉換[8]。

2.1 陀螺儀計算姿態角[8-9]

根據四元數理論,姿態角計算公式為

(1)

式中γ為翻滾角,θ為俯仰角,ψ為航向角。

2.2 利用加速度計、磁強計計算初始四元數

單獨的三軸加速度計只能確定橫滾角、俯仰角,與磁強計組合后可確定偏航角。當載體在導航坐標系中靜止,并且載體坐標系和導航坐標系重合時,加速度計和磁力計的測量輸出為:

an=[0 0g]mn=[0bnbu]

(2)

式中:g是重力加速度,bn是地磁場在ENU坐標系中的北向分量,bu是地磁場在ENU坐標系中的垂直分量。

初始靜止情況下的導航坐標系加速度為:

(3)

對上式作矢量展開,得到初始水平姿態角(翻滾角和俯仰角):

(4)

同樣可得初始靜止情況下的導航坐標系磁力計為:

(5)

磁航向角的初值估計可通過下式獲得

(6)

根據獲取的起始姿態角代入式(7)計算得到初始四元數為

(7)

3 互補濾波器和PI控制算法設計

由于陀螺長時間工作會引起漂移、再加上機械振動引起的加速度計誤差和傳感器周圍磁干擾的作用,基于MEMS慣性器件測量值解算出來的姿態會存在漂移,為抑制陀螺的漂移,通過互補濾波器和PI控制來調節誤差。

3.1 互補濾波和PI控制原理

由于陀螺短時精度高,長時間工作會引起漂移,而對于加速度計和磁力計,它們短時間內精度沒有陀螺儀高,但長時間內卻能保持穩定。同時從頻域上看陀螺顯示高通特性,加速度計與磁力計顯示低通特性,因此它們在頻域上可以相互補充,從而實現高精度的姿態測量?；诖吮疚膶⑼ㄟ^磁羅盤和四元數法分別計算得到的姿態角之間的差值作為修正值,用其校正陀螺儀的角速度輸出。互補濾波器的原理如下[11-12]:

假設經過校正后得到的姿態角度為θ,則在s域上有如下等式成立

(8)

式中A(s)=K/(S+K)為低通濾波器,B(s)=S/(S+K)為高通濾波器,且A(s)+B(s)=1,wm表示陀螺儀測量值,θref為磁力計結合旋轉矩陣解算出來的姿態角。

式(8)求拉普拉斯反變換,可得其在時域中的表達式為:

(9)

θref-θ即為通過磁羅盤算法和四元數法分別計算得到的姿態角度的誤差量,將其作為陀螺修正值,以校正陀螺儀的角速度輸出,θref-θ的值通過PI調節器來控制[13]。

PI調節器的模型為:

(10)

其中,e(t)為t時刻系統的誤差,u(t)為t時刻PI調節器的輸出。通過調節Kp和Ki兩個參數就可以獲得較好的誤差估計量。

3.2 向量誤差校正

對于陀螺誤差的校正,誤差量的計算是一個難點,誤差量的計算往往是基于梯度下降法[14]計算的,但是梯度下降法比較復雜并且計算量大,為降低復雜度,這里使用一種基于向量叉乘的算法來計算誤差量。

兩者矢量之間的叉乘公式如下:

(11)

(12)

理論情況下,Δψ和Δθ,γ值應恒為0,但由于測量、數值截斷、坐標對準、非正交化等誤差的存在,此式不總為零。將上述兩種向量進行叉乘,向量積的長度為兩向量夾角的正弦,并不完全與兩組向量的誤差角度成正比但在誤差較小時,誤差角度與其正弦近似相等,因此

(13)

(14)

空間誤差角度θ投影到3個軸上形成誤差矢量,所以叉乘的結果可以校正陀螺所產生的漂移,獲得的總誤差矢量為:

Δ=Δθ,γ+Δψ

(15)

根據式(9)、式(10)、式(15)可得:

(16)

4 基于EKF的四元數更新算法

4.1 狀態量選取及狀態方程的建立

取四元數的4個參數作為狀態變量,根據捷聯慣導系統的四元數理論[9],狀態方程為:

X(k)=Φ(k,k-1)X(k-1)+Γ(k-1)W(k-1)

(17)

其中:

(18)

(19)

其中W(k)近似為白噪聲序列,Γ(k)為噪聲驅動陣。

4.2 觀測量的選取及觀測方程的建立

選取觀測量為:

Z=[axayazmxmymz]T

(20)

其中ax、ay、az為加速度計在載體坐標系中測量出的歸一化后的數據,mx、my、mz為三軸磁力計在載體坐標系中測量出的歸一化后的數據。

基于EKF的觀測方程為:

Z(k)=H(k)X(k)+V(k)

(21)

其中:

(22)

5 實驗驗證及結果分析

本文采用MPU9150九軸慣性傳感器測量單元進行測試,MPU9150由MPU6050傳感器(其中包括三軸加速度計和三軸陀螺儀)與HMC5883L三軸磁力計集成,數據通過I2C總線進行讀取。利用本文設計的濾波算法對傳感器輸出的原始數據進行處理來獲得載體準確的姿態信息。在實際使用中,傳感器因為存在一定的固定誤差,需要對其進行提前校正。MPU9150模塊如圖2所示。

圖2 MPU9150模塊

5.1 靜止狀態下測試

本文靜止狀態下的測試地點為室內過道中,行人手持傳感器模塊靜止2min,然后通過本文設計的姿態解算算法在MATLAB上對MPU9150傳感器數據進行處理得到其靜止下的姿態圖,如圖3～圖5所示。

5.2 運動狀態下測試

本文運動狀態下的測試地點選在室內,行人手持傳感器模塊行走一個方形,時間為2min,通過本文設計的姿態解算算法對MPU9150傳感器數據進行處理得到其行走狀態下的姿態圖,如圖6～圖8所示。

圖3 靜止時有PI控制和無PI控制下的航向角

圖4 靜止時有PI控制和無PI控制下的航向角

圖5 靜止時有PI控制和無PI控制下的翻滾角

圖6 運動時有PI控制和無PI控制下的航向角

圖7 運動時有PI控制和無PI控制下的俯仰角

圖8 運動時有PI控制和無PI控制下的翻滾角

傳感器按50Hz/s更新數據,分別在靜止和運動狀態下進行了2min左右的多次測試,其數據統計特性如表1所示(分析時剔除了俯仰角和橫滾角在轉彎處的較大偏差值)。

表1 單獨EKF與EKF+PI姿態數據統計對比表

結論:通過圖3～圖5可知以及表1可知,在靜止狀態下,加入PI控制后可以有效抑制載體3個姿態角的漂移,并能很好的保持穩定;通過圖6～圖8以及表1可知,在運動狀態下,PI控制對航向角起到很好的調整作用,但是在轉彎處,PI控制對俯仰角和橫滾角的調整則會短暫失效。

6 總結

本文針對傳統人體姿態解算算法中存在的MEMS陀螺誤差發散快問題,提出了一種基于MIMU及磁力計信息融合的姿態解算算法。該算法利用互補濾波,同時結合PI調節控制完成陀螺零偏校正,然后在加速度計和磁力計的輔助校正下,通過EKF濾波器實現陀螺姿態解算。通過仿真結果表明,本算法能夠實現穩定地輸出高精度姿態數據,在對航向角有較高要求的行人導航等應用中具有實際應用價值。

[1]Mahony R,Hamel T,Pflimlin J M.Nonlinear Complementary Filters on the Special Orthogonal Group[J].Automatic Control,IEEE Transactions on,2008,53(5):1203-1218

[2]Euston M,Coote P,Mahony R,et al.A Complementary Filter for Attitude Estimation of a Fixed-Wing Uav[C]//Intelligent Robots and Systems,2008.IROS 2008.IEEE/RSJ International Conference on.IEEE,2008:340-345

[3]Rehbinder H,Hu X.Drift-Free Attitude Estimation for Accelerated Rigid Bodies[J].Automatica,2004,40(4):653-659

[4]Elder M Hemerly,Benedito C O Maciel,Anderson de P Milhan,et al.Attitude and Heading Reference System with Acceleration Compensation[J].Aircraft Engineering and Aerospace Technology,2012,84(2):87-93

[5]Brigante C M N,Abbate N,Basile A,et al.Towards Miniaturization of a MEMS-Based Wearable Motion Capture System[J].Industrial Electronics,IEEE Transactions on,2011,58(8):3234-3241

[6]Madgwick S.An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays[J].Report x-io and University of Bristol(UK),2010

[7]Madgwick S O H,Harrison A J L,Vaidyanathan R.Estimation of IMU and MARG Orientation Using a Gradient Descent Algorithm[C]//Rehabilitation Robotics(ICORR),2011 IEEE International Conference on.IEEE,2011:1-7

[8]劉興川,張盛,李麗哲,等.基于四元數的MARG傳感器姿態測量算法[J].清華大學學報(自然科學版),2012(5):627-631

[9]賈瑞才.基于四元數EKF的低成本MEMS姿態估計算法[J].傳感技術學報,2014,27(1):90-95

[10]秦永元.慣性導航[M].北京:科學出版社,2006

[11]Mahony R,Hamel T,Pflimlin J M.Complementary Filter Design on the Special Orthogonal Group SO(3)[C]//Proc of the IEEE Conference on Decision and Control and European Control Conference,2005:1477-1484

[12]Robert Mahony,Tarek Hamel,Pascal Morin,et al.Nonlinear Complementary Filters on the Special Linear Group[J].International Journal of Control,2012,85(10):1557-1573.

[13]董恒,王輝,黃科元.永磁同步電動機驅動系統數字PI調節器參數設計[J].電氣傳動,2009(1):7-10

[14]朱文杰,王廣龍,高鳳岐,等.基于MIMU和磁強計的在線實時定姿方法[J].傳感技術學報,2013,26(4):536-540.

MIMU Update Algorithm Based on the Posture and Magnetometer*

MIGang,TIANZengshan*,JINYue,LIZe,ZHOUMu

(Chongqing Key Lab of Mobile Communications Technology,Chongqing University of Posts and Telecommunications,Chongqing 400065,China)

Since the conventional human attitude algorithms suffer from the fast divergence of MEMS gyro errors,this paper proposes a novel attitude algorithm based on the information fusion of micro inertial measurement unit(MIMU)and magnetometers.We first rely on the filter combined with PI regulation to conduct gyro zero slant correction,and then in the condition of accelerometer and magnetometer auxiliary correction,realize the gyro attitude algorithm by using the extended Kalman filter(EKF)to update quaternion.By selecting the MPU9150 sensor module,the extensive experiments are provided to compare the performance of the conventional single EKF and the proposed algorithm in filtering.The results demonstrate that the proposed algorithm can significantly restrain the divergence of MEMS gyro errors,and consequently output the stable and highly-accurate attitude data.

sensor;attitude algorithm;gyro calibration;complementary filter;PI regulation;EKF

米 剛(1987-),男,四川遂寧人,重慶郵電大學碩士研究生,主要研究方向為無線定位與慣性導行,migang126@163.com;

田增山(1968-),男,河南固始人,教授,博士,主要研究方向為移動通信、個人通信、GPS及蜂窩網定位系統及其應用技術研究,tianzs@cqupt.edu.cn。

項目來源:國家自然科學基金(61301126);重慶市基礎與前沿研究計劃項目(cstc2013jcyjA40032,cstc2013jcyjA40041);重慶市重點實驗室專項基金(CSTC)。

2014-09-04 修改日期:2014-11-05

TP391.9

A

1004-1699(2015)01-0043-06

C:7320E

10.3969/j.issn.1004-1699.2015.01.009