姿態解算的抗干擾算法*

王國交　蔚承英　李玲霞

(1.中冶賽迪工程技術股份有限公司　重慶　400013)(2.重慶郵電大學　重慶　400065)

?

姿態解算的抗干擾算法*

王國交1蔚承英2李玲霞2

(1.中冶賽迪工程技術股份有限公司重慶400013)(2.重慶郵電大學重慶400065)

摘要針對當前基于MARG多傳感器的行人導航算法中磁力計很容易受到外界磁場干擾且MEMS陀螺精度低及其長時間漂移大等問題,從復雜的使用環境模型出發,提出了在磁干擾環境下姿態解算的多傳感器融合算法,仿真分析結果表明該算法有效抑制了外界磁干擾對姿態解算的影響,對姿態解算融合算法進行了優化。

關鍵詞多傳感器; 復雜環境; 融合算法; 抗干擾

Class NumberTP301.6

1引言

微機械(MEMS)陀螺具有尺寸微小、質量輕、功耗低、成本低廉等優點,被廣泛應用在定位導航、人體姿態控制等領域[1～3]。MEMS陀螺是常用的姿態慣性傳感器。但是,由于MEMS陀螺精度較低,且陀螺固有的長時間漂移會產生很大的姿態解算偏差,因此需要和其他姿態傳感器組合才能獲得穩定可靠的姿態信息[4];

近年來,為了解決上述的問題,在利用MARG傳感器進行姿態解算方面取得了一些成果。文獻[5]采用基于MARG傳感器的擴展卡爾曼濾波算法(EKF)設計了基于陀螺四元數的擴展卡爾曼濾波器。文獻[6]利用四元數卡爾曼濾波器來進行人體姿態的實時定位跟蹤,其中四元數(QUEST)算法取代高斯-牛頓(Gauss-Newton)算法。文獻[7]和文獻[8]為了進一步MARG傳感器姿態解算的精度,采用了基于四元素的擴展卡爾曼濾波器。在此基礎上,文獻[9]通過建立傳感器模型,引入慣性傳感器偏差補償方法和構造自適應測量噪聲協方差矩陣來提高姿態解算的精度。

雖然上述算法都能夠順利解算出載體的姿態角度,但是它們都沒有考慮到實際測量中所受到的外界復雜磁干擾。在現代鋼筋混凝土結構的建筑中,磁性物質會引起局部的地磁異常,關鍵是這種地磁異常會隨著位置的變化而變化。當存在外界磁場干擾時,受干擾的磁強計數據變異,會使姿態解算產生較大計算誤差。為此本文采用了基于四元數的擴展卡爾曼濾波算法,通過對磁強計輸出的數據分析,提出抗磁干擾方法,有效地抑制了外界干擾磁場對姿態解算的不利影響。同時,對復雜環境下的姿態角度進行優化,最后通過實驗驗證了該方法的正確性和可行性。

2基于MARG傳感器的姿態測量算法

2.1陀螺四元素法姿態測量分析

載體坐標系b通常是將慣性測量單元中的正交三軸陀螺儀、正交三軸磁力計和正交三軸加速度計的測量數據定義到一個坐標系上,正交的三個軸分別對應載體的縱軸、橫軸和豎軸,并取載體的重心為坐標系的原點。絕對坐標系通常又被稱為導航坐標系n。載體坐標系和導航坐標系之間可以通過四元數法和歐拉角法實行轉換,由于四元數法避免了奇異問題的出現應用更加廣泛[10]。

(1)

式(1)是載體坐標系和導航坐標系之間的轉換關系式;其中旋轉四元數和旋轉矩陣分別為

q=q0+q1i+q2j+q3k

(2)

(3)

由三軸陀螺儀的測得的x、y、z軸的角速度和初始姿態,此時姿態四元數微分方程如下:

(4)

其中t0為載體運動的初始時間;q0為初始對準的姿態四元數;?為四元數乘法符號;其中ω為姿態角速度矢量,式(5)為其矩陣形式:

(5)

其中ω表示載體坐標系相對于導航坐標系角速度在載體坐標系上分量的四元數。

一般假設角速率在采樣時間間隔T內是一個固定值,其中旋轉四元數更新時間間隔也為T。那么與式(4)對應的姿態四元數離散時間模型為:

qk+1=exp(ωT)qk, (k=0,1…)

(6)

(7)

其中γ為橫滾角;θ為俯仰角;ψ為航向角。

2.2擴展Kalman濾波器設計

本文采用EKF來將傳感器數據進行融合,設計的系統如下:

(8)

qk+1=Aqk+wk

(9)

其中ωk是均值為零的高斯白噪聲,協方差矩陣Qk如下

(10)

將加速度計和磁力計的測量數據作為觀測量,則其觀測方程為

(11)

(12)

由于狀態矢量與測量矢量之間是非線性的關系,需要對式(11)進行線性化,得到觀測矩陣C,表示如下:

(13)

(14)

其中ka1、ka2為設定的權重因子。據此可以實時調整測量值的權重,當載體產生較大的線性加速度時,協方差也會隨之增大。

σ2m= km1(|

‖mk‖-‖m0‖|)

+km2var(‖mk-N‖:‖mk+N‖)

(15)

其中:km1和km2為設定的權重因子;var(||mk-N||:||mk+N||)為滑動窗口大小為2N的每一步的磁力計測量值模值的方差(N的大小為實際數據測得);m0為當地的標準磁場強度。同樣當外界的磁干擾較大時,磁力計數據變化異常,模值以及其方差都會變大,協方差故隨之增大。

3復雜環境下的抗磁干擾算法

3.1磁場補償

為了消除外界環境中存在的磁場干擾導致的磁羅差橢圓效應,在載體運動之前需對磁力計進行初始標定,稱之為磁場補償技術。在忽略載體磁場和測量誤差的前提下,當載體在地磁場變化較小的區域內運動時,則捷聯式磁力計的x、y軸上的測量值的軌跡是一個以原點為圓心,以磁場強度為半徑的圓[12～14]。然而在實際的測量過程中,由于各種測量誤差和外景磁場的干擾,圓心會產生偏移,形狀畸變,軌跡近似為一個橢圓[15]。如圖1所示。

圖1　捷聯磁傳感器測得的二維磁場軌跡

具體補償方法為:在運動前對載體進行“8字校準”,以確定磁力計X、Y、Z三軸數據的最大值和最小值,然后求取三軸的標定因子和零偏值[16～17]。計算公式如下:

(16)

(17)

式中:Xzero、Yzero、Zzero分別為磁力計三軸的零偏值;Kx、Ky、Kz分別為磁力計三軸的標定因子;補償后磁力計三軸的輸出為

(18)

3.2閾值法航向修正

磁場補償算法只能針對外界磁場干擾相對穩定的情況下的磁場干擾的影響,當外界干擾不穩定的時候,磁場補償算法便無法有效地修正磁力計數據。為此,本文采用了閾值法對航向角度進行修正。

本文中,先對磁力計三軸模值進行預處理,首先求取三軸磁力計模值的方差,再利用閾值法,判斷此點是否存在干擾。具體方程如下:

(19)

其中:var(‖mk-N‖:‖mk+N‖)為滑動窗口大小為2M的每一步的磁力計測量值模值的方差;m0為當地的標準磁場強度。

當上面的條件都滿足的時候,說明此時外界的磁場強度變化相對較小或者說此刻的地磁場變化不足以影響姿態測量的精度,EKF解算的姿態角度權重加大;而當上述條件不滿足時,EKF解算的姿態角可信度降低,此時陀螺的輸出航向值權重加大,進而在一定程度上降低了外界磁干擾對姿態角的影響。

在復雜環境下,比如商場、車庫等干擾不穩定的環境下,磁力計數據局部變化較大,干擾對航向角的影響不穩定。陀螺四元數解算的姿態角不受磁場干擾的影響,但是由于累計誤差造成姿態角的漂移,然而短時間內陀螺解算的姿態角是準確的[18]。據此,在干擾處本文利用陀螺四元數解算的每一步航向角的差值,提出了抗干擾算法求最優估計值,具體方程如下:

(20)

其中Δyquer(i)是基于陀螺四元數法解算出來的當前一步的航向角度;yEKF(i-1)是利用擴展卡爾曼融合算法解算出來的前一步的航向角度;yave(i)為最優估計值。

4實驗結果

本文采用華為智能手機Mate終端集成的MARG傳感器作為慣性測量單元,該慣性測量單元包含正交三軸加速度計傳感器(STLIS3DH)、正交三軸磁力計傳感器(akm8963)和正交三軸陀螺儀(ST L3G4200D)。該智能手機為安卓操作系統,從系統提供的應用程序編程接口(API)可直接獲取MARG傳感器的原始數據(采樣速率為50Hz)。

本實驗選點在重慶郵電大學第三教學樓的走廊,行人沿走廊行走一圈正好可以圍成一個閉合的長方形。通過圖2的磁力計三軸模值可以看出該環境下存在著很強的磁干擾。

圖2　磁力計三軸模制

在此干擾環境下,通過該抗干擾算法對采集傳感器數據進行融合優化,并和EKF融合算法姿態解算的航向角度進行了對比,如圖3所示。

圖3　航向角輸出對比

圖3中,細線為EKF解算的航向角度;虛線為在EKF的基礎上本文提出的抗干擾算法進行優化的航向角度輸出結果;粗線則為實際的航向參考輸出值;由上圖的結果可以看出,當周圍環境中存在較強的磁場干擾時,EKF解算出的航向角會產生較大的誤差,而在EKF基礎上本文提出的抗干擾算法有效地抑制了誤差的產生。

5結語

本文在基于MARG傳感器擴展卡爾曼融合算法的基礎上,采用磁場補償法來消除外界干擾磁場的影響,結合閾值法對干擾下的航向誤差進行補償和優化,有效提高了系統姿態測量的精度,并通過仿真實驗結果證明了該算法的有效性。該算法對磁力計在導航定位方面的應用和推廣具有一定的參考價值。

參 考 文 獻

[1] Fourati H, Manamanni N, Afilal L, et al. A nonlinear filtering approach for the attitude and dynamic body acceleration estimation based on inertial and magnetic sensors: Bio-logging application[J]. Sensors Journal, IEEE,2011,11(1):233-244.

[2] Roetenberg D, Luinge H J, Baten C T M, et al. Compensation of magnetic disturbances improves inertial and magnetic sensing of human body segment orientation[J]. Neural Systems and Rehabilitation Engineering, IEEE Transactions on,2005,13(3):395-405.

[3] W. Soehren, W. Hawkinson. A prototype personal navigation system[J]. IEEE Aerospace and Electronic Systems Magazine,2008,6(23):10-18.

[4] Evennou F, Marx F. Advanced integration of WiFi and inertial navigation systems for indoor mobile positioning[J]. Eurasip Journal on Applied Signal Processing,2006:164-164.

[5] Marins J L, Yun X, Bachmann E R, et al. An extended Kalman filter for quaternion-based orientation estimation using MARG sensors[C]//Intelligent Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ International Conference on. IEEE,2001,4:2003-2011.

[6] Yun X, Bachmann E R, McGhee R B. A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements[J]. Instrumentation and Measurement, IEEE Transactions on,2008,57(3):638-650.

[7] Strohrmann C, Harms H, Tr?ster G, et al. Out of the lab and into the woods: Kinematic analysis in running using wearable sensors[C]//Proceedings of the 13th international conference on ubiquitous computing. ACM,2011:119-122.

[8] Munguia R, Grau A. An attitude and heading reference system(AHRS) based in a dual filter[C]//Emerging Technologies & Factory Automation(ETFA), 2011 IEEE 16th Conference on. IEEE,2011:1-8.

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

LIU Xingchuan, ZHANG Sheng, LI Lizhe, et al. Quaternion-Based Algorithm for Orientation Estimation from MARG Sensors[J]. Journal of Tsinghua University(Science & Technology),2012,52(5):627-631.

[10] Li F, Zhao C, Ding G, et al. A reliable and accurate indoor localization method using phone inertial sensors[C]//Proceedings of the 2012 ACM Conference on Ubiquitous Computing. ACM,2012:421-430.

[11] Ali A, Siddharth S, Syed Z, et al. An Improved Personal Dead-Reckoning Algorithm for Dynamically Changing Smartphone User Modes[C]//Proceedings of the 25th International Technical Meeting of the Satellite Division of the Institute of Navigation(ION GNSS 2012), Nashville,2012:2432-2439.

[12] 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.

[13] Shin B, Lee J H, Lee H, et al. Indoor 3D pedestrian tracking algorithm based on PDR using smarthphone[C]//Control, Automation and Systems(ICCAS), 2012 12th International Conference on. IEEE,2012:1442-1445.

[14] Chon J, Cha H. Lifemap: A smartphone-based context provider for location-based services[J]. Pervasive Computing, IEEE,2011,10(2):58-67.

[15] Jahn J, Batzer U, Seitz J, et al. Comparison and evaluation of acceleration based step length estimators for handheld devices[C]//Indoor Positioning and Indoor Navigation(IPIN), 2010 International Conference on. IEEE,2010:1-6.

[16] Nilsson J O, Zachariah D, Skog I, et al. Cooperative localization by dual foot-mounted inertial sensors and inter-agent ranging[J]. arXiv preprint arXiv:1304.3663, 2013.

[17] Bird J, Arden D. Indoor navigation with foot-mounted strapdown inertial navigation and magnetic sensors [emerging opportunities for localization and tracking][J]. Wireless Communications, IEEE,2011,18(2):28-35.

Attitude Measurement Algorithm in Magnetic Interference Environment

WANG Guojiao1WEI Chengying2LI Lingxia2

(1. CISDI Engineering CO., LTD., Chongqing400013)(2. Chongqing University of Posts and Telecommunications, Chongqing400065)

AbstractAiming at the current pedestrian navigation algorithm based on MARG multi-sensor magnetometer is susceptible to interference of outside magnetic field and in the MEMS gyroscope problem such as low accuracy and gyro drift is larger, starting from the use of complex environment model, multi-sensor fusion algorithm of attitude estimation in the magnetic disturbance environment is put forward. Simulation results show that the proposed algorithm effectively restrains the influence of outside magnetic interference of attitude algorithm, and optimizes the attitude algorithm fusion algorithm.

Key Wordsmulti-sensor, complex environment, fusion algorithm, anti-magnetic interference

* 收稿日期:2015年11月7日,修回日期:2015年12月27日

作者簡介:王國交,男,高級工程師,研究方向:通信與信息系統。蔚承英,女,碩士,講師,研究方向:通信與信息系統。李玲霞,女,碩士,高級工程師,研究方向:通信與信息系統。

中圖分類號TP301.6

DOI:10.3969/j.issn.1672-9722.2016.05.007