田增山，張 媛

(重慶郵電大學(xué)移動(dòng)通信重點(diǎn)實(shí)驗(yàn)室，重慶 400065)

0 引言

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

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

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

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

1 PDR算法

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

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

2 步態(tài)檢測(cè)和步長(zhǎng)估計(jì)

根據(jù)行人步行的運(yùn)動(dòng)生理學(xué)特性，行走時(shí)加速度計(jì)三軸模值輸出波形成周期性變化，因此，可利用該周期性特征來(lái)檢測(cè)步態(tài)。三軸加速度模值為

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

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

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

本文根據(jù)三軸加速度模值的行走特征，估算行人的步長(zhǎng)。步長(zhǎng)估計(jì)采用經(jīng)驗(yàn)?zāi)Ｐ?，其公式?/p>

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

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

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

3 航向估算

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

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

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

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

3.2 擴(kuò)展卡爾曼濾波器設(shè)計(jì)

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

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

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

系統(tǒng)中的觀測(cè)方程是非線性的，通過(guò)(14)式求得雅可比矩陣F來(lái)進(jìn)行線性化處理。

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

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

4 實(shí)驗(yàn)驗(yàn)證及結(jié)果分析

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

4.1 無(wú)磁干擾環(huán)境下測(cè)試

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

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

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

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

圖4 仿真軌跡復(fù)原圖Fig.4 Walking simulation trajectory of pedestrians

圖5 磁力計(jì)模值Fig.5 Modulus values ofmagnetometer

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

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

5 結(jié)束語(yǔ)

圖6 航向估計(jì)對(duì)比圖Fig.6 Comparison of heading angles

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

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

［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)外無(wú)縫定位算法研究［D］.合肥:中國(guó)科技大學(xué)，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］孫作雷，茅旭初，田蔚風(fēng)，等.基于動(dòng)作識(shí)別和步幅估計(jì)的步行者航位推算［J］.上海交通大學(xué)學(xué)報(bào)，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］.重慶郵電大學(xué)學(xué)報(bào):自然科學(xué)版，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.

(編輯:劉 勇)