基于智能手機MARG 傳感器的行人導航算法

田增山，張 媛

(重慶郵電大學移動通信重點實驗室，重慶 400065)

0 引言

目前比較完善的導航技術(shù)如全球定位系統(tǒng)(global positioning system，GPS)，由于微波易被建筑物、金屬遮蓋物等吸收，在室內(nèi)場合則無法使用，基于多微機電(micro－electro－mechanic system，MEMS)慣性傳感器的導航系統(tǒng)便能很好地解決這一問題。慣性系統(tǒng)是一種自主導航系統(tǒng)，能提供位置、航向和姿態(tài)角數(shù)據(jù)，所產(chǎn)生的導航信息連續(xù)性好而且數(shù)據(jù)更新率高、短期精度和穩(wěn)定性好［1］。近年來，隨著基于MEMS技術(shù)的磁力計，陀螺儀和加速度計(magnetic，angular rate and gravity，MARG)傳感器在智能手機上的廣泛應(yīng)用，行人慣性導航應(yīng)用成為國內(nèi)外研究的熱點。特別是在無法接收到GPS信號的環(huán)境下，對于手持式移動設(shè)備中的行人導航需求在過去的幾年里已大大增加。

在行人慣性導航系統(tǒng)中，最常用的方法就是行人航跡推算［2］(pedestrian dead reckoning，PDR)算法，該算法避免了對加速度觀測值積分，而基于行人步行的運動生理學特性，利用行走時加速度波形的周期性特征和統(tǒng)計值與行走速度相關(guān)的特點，直接估計行走步長，航向則主要是將陀螺儀通過積分得到航向變化值或通過磁羅盤直接得到［3］。該算法的技術(shù)難點在于如何在復雜且沒有任何外界參考的環(huán)境下獲得一個很好的航向。

利用陀螺角速率對姿態(tài)角度更新的算法主要有歐拉角法和四元數(shù)法。歐拉角算法通過求解歐拉角微分方程直接計算航向角、俯仰角和橫滾角。由于歐拉角固有的缺陷，總是存在一對奇異點，當俯仰角接近90°時，方程會出現(xiàn)退化現(xiàn)象，使得航向角和橫滾角無法唯一確定。四元數(shù)法避免了歐拉角的奇異問題，只需求解4個未知量的線性微分方程組，算法簡單，易于操作，在實際應(yīng)用中非常廣泛。由于陀螺儀測量角度存在累計誤差，使得無法使用陀螺儀對角度進行長時間測量，必須與加速度計和磁力計組合使用才能獲得穩(wěn)定可靠的姿態(tài)角，這種數(shù)據(jù)融合的方法主要有互補濾波、卡爾曼濾波和梯度下降算法等。文獻［4］采用了基于四元數(shù)的梯度下降算法，將測得的絕對角度與角速度進行數(shù)據(jù)融合，實現(xiàn)了三自由度的姿態(tài)測量，該算法簡單且易于實現(xiàn)，但是測量精度不高。文獻［5］采用了基于四元數(shù)的擴展卡爾曼濾波(extended Kalman filter，EKF)算法，但系統(tǒng)狀態(tài)量較多，計算量大，不利于實時處理，不易在手機平臺上實現(xiàn)。另外，還存在的主要問題是，當載體產(chǎn)生大的線性加速度，或者磁力計受到周圍環(huán)境(如鐵制品)等產(chǎn)生的磁場干擾時，會導致加速度計和磁力計輸出的測量值解算的姿態(tài)角誤差增大，從而降低了系統(tǒng)測量精度。

為了能在智能手機上實現(xiàn)行人慣性導航應(yīng)用，本文提出一種易于實現(xiàn)且精度較高的行人導航算法，針對姿態(tài)測量精度問題，設(shè)計了一種EKF，利用自適應(yīng)的方法來調(diào)整測量噪聲的協(xié)方差矩陣，從而提高系統(tǒng)精度。

1 PDR算法

本文采用的PDR算法是一種基于已知用戶的初始位置的相對定位方法。智能手機實驗平臺通常包含多個MEMS傳感器，其中，最為主要的是三軸加速度計、三軸磁力計和三軸陀螺儀。本文主要利用加速度計數(shù)據(jù)，探測用戶行走時的步態(tài)和總步數(shù)，并作為位置更新點;同時，利用經(jīng)驗模型，估計行人的步長，并作為點到點的位移大小。此外，將MARG傳感器數(shù)據(jù)通過EKF進行融合，以獲得航向信息，從而最終推算行人的位置信息，系統(tǒng)的總體架構(gòu)如圖1所示。

圖1 行人導航系統(tǒng)架構(gòu)Fig.1 Pedestrian navigation system architecture

2 步態(tài)檢測和步長估計

根據(jù)行人步行的運動生理學特性，行走時加速度計三軸模值輸出波形成周期性變化，因此，可利用該周期性特征來檢測步態(tài)。三軸加速度模值為

(1)式中:Acc_norm為加速度模值;ax，ay和az分別為加速度計三軸輸出的數(shù)據(jù)。

利用數(shù)字低通濾波器對加速度模值進行濾波，得到較好的單峰值曲線圖，如圖2所示?？梢詼蚀_地檢測到峰值點并計算出步數(shù)。由于行人將手機拿在手中時會發(fā)生一些小的抖動，這時加速度計模值輸出波形也會出現(xiàn)峰值，因此，需要設(shè)定相應(yīng)的閾值來消除因抖動而造成的計步誤差。步態(tài)檢測算法采用閾值法，設(shè)置的閾值條件為

(2)式中:ΔT為2個相鄰峰值之間的時間間隔;g為當?shù)刂亓铀俣?TTh和ATh分別是時間閾值和峰值閾值。

本文根據(jù)三軸加速度模值的行走特征，估算行人的步長。步長估計采用經(jīng)驗模型，其公式為

(3)式中:step_length為行人的步長;Amax和Amin分別為每一步加速度模值的最大值和最小值;C為校準系數(shù)，由參考軌跡距離的真實值與估計值的比率求得。

(4)式中，dreal和destimated分別為參考軌跡距離的真實值和估計值。該方法估計不同行人的步長，可以通過調(diào)整校準系數(shù)C得到較為準確的步長估計。由于該步長估計算法僅涉及一個參數(shù)，于是實現(xiàn)過程較為簡單。

圖2 加速度計模值輸出Fig.2 Modulus values output of accelerometer

3 航向估算

3.1 陀螺四元數(shù)法姿態(tài)更新

三軸陀螺儀可得到手機載體坐標系下x，y，z軸角速度，從已知初始姿態(tài)開始，通過對基于四元數(shù)剛體運動學微分方程進行積分，可以得到手機當前姿態(tài)角。該姿態(tài)四元數(shù)微分方程為

(6)式中:ωi(i=1，2，3)為相應(yīng)姿態(tài)角速度，即載體坐標系x，y，z軸上分量;Ω(ω)為一個4×4的反對稱矩陣。

為了便于計算，通常假設(shè)在采樣時間間隔Ts內(nèi)角速度為恒值，對微分方程組(6)進行求解，可得到姿態(tài)四元數(shù)離散時間計算公式:

3.2 擴展卡爾曼濾波器設(shè)計

盡管陀螺儀與組合磁力計/加速度計都可得到航向角，但這2種方法解算得到的航向角，會分別因為陀螺儀累積誤差和磁力計受外界干擾造成測量精度降低［7］。因此，將陀螺儀與組合磁力計/加速度計融合求解航向角，可以有效地提高航向角的解算精度。本文采用EKF將傳感器數(shù)據(jù)進行融合。

根據(jù)陀螺四元數(shù)法更新姿態(tài)的離散時間模型，將旋轉(zhuǎn)四元數(shù)作為狀態(tài)矢量，可以得到卡爾曼濾波器的狀態(tài)方程為

(13)式中:by和bz分別為磁場強度在水平方向和垂直方向上歸一化后的分量。

系統(tǒng)中的觀測方程是非線性的，通過(14)式求得雅可比矩陣F來進行線性化處理。

在靜止且無磁干擾狀態(tài)下，加速度計和磁力計的量測噪聲基本不變，但是當載體存在線性加速度時或者周圍環(huán)境中存在隨機磁源干擾時，會導致加速度計和磁力計實際的量測噪聲方差與理論值不符。針對這個問題，本文采用以下自適應(yīng)的方法構(gòu)造協(xié)方差和來修正測量噪聲協(xié)方差陣R的加權(quán)系數(shù):

(15)—(16)式中:‖ak‖和‖mk‖分別為加速度計和磁力計數(shù)據(jù)模值;ki1和ki2為設(shè)定的權(quán)重因子;var(ik－N/2:ik+N/2)為滑動窗口大小為N的測量值模值方差;i為a或m，N的大小由實驗得到。

4 實驗驗證及結(jié)果分析

本文采用華為智能手機集成的MARG傳感器作為慣性測量單元，慣性測量單元包含三軸加速度計傳感器(ST LIS3DH)、三軸磁力計傳感器(akm8963)和三軸陀螺儀(ST L3G4200D)。華為智能手機采用安卓操作系統(tǒng)，從系統(tǒng)提供的應(yīng)用程序接口(application program interface，API)可直接獲取MARG傳感器的原始數(shù)據(jù)(采樣速率為50 Hz)，利用本文設(shè)計的行人導航算法對這些原始數(shù)據(jù)進行處理，解算得到行人的行走軌跡。在實際使用中，由于固定偏差和比例誤差的存在，嚴重影響了數(shù)據(jù)融合的精度，因此，在實際應(yīng)用之前.必須對傳感器數(shù)據(jù)進行濾波、誤差校準等預處理［8］。

4.1 無磁干擾環(huán)境下測試

本文選用的無磁干擾環(huán)境測試地點為標準足球場的外圍跑道，如圖3所示，總長度400 m，行人手持智能手機圍繞操場行走一圈，通過本文設(shè)計的行人導航算法對MARG傳感器數(shù)據(jù)進行處理得到其行走的軌跡圖，如圖4所示。誤差為1.8 m，滿足了誤差小于2%的需求。

4.2 有磁干擾環(huán)境下測試

本文選用的有磁干擾環(huán)境為地鐵站，長70 m，寬50 m的矩形場地，行人手持智能手機圍繞該場地行走一圈。通過磁力計三軸模值數(shù)據(jù)(見圖5)可以看出，該環(huán)境下存在著很強的磁干擾。

圖3 行走參考軌跡圖Fig.3Walking reference trajectory of pedestrians

圖4 仿真軌跡復原圖Fig.4 Walking simulation trajectory of pedestrians

圖5 磁力計模值Fig.5 Modulus values ofmagnetometer

圖6是航向估計對比圖，由于陀螺儀存在漂移，求得的航向角存在累計誤差，但在短時間內(nèi)精度較高;磁力計由于受到周圍環(huán)境中如鐵制品等產(chǎn)生的磁場干擾，解算得到的航向角出現(xiàn)上下波動的情況;本文提出的EKF算法減少了磁干擾的影響，從而提高了系統(tǒng)整體定位精度。

圖7為本文設(shè)計的行人導航算法得到的行人軌跡，通過EKF算法得到的軌跡圖更接近實際軌跡，誤差為3.5 m，也滿足了誤差小于2%的需求。

5 結(jié)束語

圖6 航向估計對比圖Fig.6 Comparison of heading angles

圖7 磁干擾下軌跡對比Fig.7 Comparison of trajectories undermagnetic interference

本文提出了一種基于智能手機MARG傳感器的行人導航算法，在智能手機平臺上進行測試驗證，實驗結(jié)果表明，在無磁或有磁干擾環(huán)境下，本文提出的行人導航算法均可保證準確、可靠、持續(xù)的位置信息。該算法除了在華為智能手機上實現(xiàn)，同樣適用于其他智能手機，為Wi－Fi/MEMS組合導航提供了重要的理論基礎(chǔ)。未來的工作將在以下2個方面進行展開:①最佳的航向估計仍然是研究的重點，它決定著系統(tǒng)的整體精度;② 通過對步長算法的優(yōu)化，算法將自動適用于不同的行人。

［1］SOEHREN W，HAWKINSON W.Prototype personal navigation system［J］.Aerospace and Electronic Systems Magazine，IEEE，2008，23(4):10－18.

［2］陳偉.基于 GPS和自包含傳感器的行人室內(nèi)外無縫定位算法研究［D］.合肥:中國科技大學，2010.

CHEN Wei.Research on GPS/Self－Contained Sensors Based Seam less Outdoor/Indoor Pedestrian Positioning Algorithm［D］.Hefei:University of Science and Technology of China，2010.

［3］孫作雷，茅旭初，田蔚風，等.基于動作識別和步幅估計的步行者航位推算［J］.上海交通大學學報，2008，42(12):2002－2005，2009.

SUN Zuolei，MAOXuchu，TIANWeifeng，etal.Pedestrian Dead Reckoning Based on Activity Recognition and Stride Assessement［J］.Journal of Shanghai jiaotong university，2008，42(12):2002－2005，2009.

［4］MADGWICK S O H，HARRISON A J L，VAIDYANATHAN R.Estimation of IMU and MARG orientation using a gradient descent algorithm［C］//IEEE International Conference on Rehabilitation Robotics，Zurich:IEEE Press，2011:1－7.

［5］SABATINIA M.Quaternion－based extended Kalman filter for determining orientation by inertial and magnetic sensing［J］.Biomedical Engineering，IEEE Transactions on，2006，53(7):1346－1356.

［6］羅磊，田增山，陳俊亞.EKF定位跟蹤算法研究［J］.重慶郵電大學學報:自然科學版，2009，21(1):50－52.

LUO lei，TIAN Zengshan，CHEN Junya.Algorithm of EKF positioning and tracking［J］.Journal of Chongqing University of Posts and Telecommunications:Natural Science Edition，2009，21(1):50－52.

［7］MUNGUIA R，GRAU A.An attitude and heading reference system(AHRS)based in a dual filter［C］//IEEE 16th Conference on Emerging Technologies＆Factory Automation.Toulouse:IEEE Press，2011:1－8.

［8］REN H，KAZANZIDESP.Investigation of attitude tracking using an integrated inertial and magnetic navigation system for hand－h(huán)eld surgical instruments［J］.Mechatronics，IEEE/ASME Transactions on，2012，17(2):210－217.

(編輯:劉 勇)