張會清， 許瀟民

(北京工業(yè)大學 信息學部，北京 100124)

0 引 言

導航是引導人或物，從指定路線的一點運動到另一點的方法[1]。由于衛(wèi)星信號到達地面時信號較弱、不能穿透建筑物[2]，室內(nèi)環(huán)境需要采取新的技術(shù)方法實現(xiàn)導航定位。按照是否需要載體設(shè)備與相關(guān)室內(nèi)環(huán)境中的設(shè)備配合，目前，室內(nèi)定位導航技術(shù)可以分為自主式導航和非自主式導航[3]。慣性導航技術(shù)是一種自主式導航技術(shù)，相對其他室內(nèi)定位技術(shù)具有不需要布設(shè)接收裝置、不受環(huán)境變化影響等優(yōu)點。1996年,Levi R W和Judd T提出了行人航跡推算(pedestrian dead reckoning,PDR)的概念[4]。很多研究機構(gòu)將微機電系統(tǒng) (micro-electro-mechanical system，MEMS)慣性傳感器佩戴于人體的一個或多個不同部位，以準確采集行人運動時的慣性數(shù)據(jù)。由于走動時人體運動情況較復雜，慣性導航算法過程中存在累積誤差，使得行人長距離導航時精確性逐漸降低，限制了其應用。

本文提出了一種基于運動姿態(tài)識別的行人慣性導航算法，將慣性傳感器佩戴于行人腳部，采用零速修正方法和姿態(tài)識別方法將行人運動軌跡以單步長劃分，分步解算單步位移，統(tǒng)計誤差較小的前期步長數(shù)據(jù)并建立步頻—步長模型，根據(jù)行人步頻和方向的變化實時校正單步長結(jié)果，通過行人的運動姿態(tài)變化判斷匹配相應的步長估計算法，得到精確的行人運動軌跡。最終通過實驗驗證了改進的行人慣性導航算法的精確性。

1 數(shù)據(jù)處理與步頻檢測

1.1 誤差分析和數(shù)據(jù)處理

MEMS慣性傳感器靜止時輸出的數(shù)據(jù)噪聲包括靜態(tài)漂移噪聲和動態(tài)漂移噪聲[5]。系統(tǒng)對MEMS慣性傳感器進行初始校準，將模塊保持靜止狀態(tài)，保存一段時間內(nèi)的測量數(shù)據(jù)，取其均值作為零偏數(shù)據(jù)。使用離散傅里葉變換的快速算法對校準后的數(shù)據(jù)進行頻域分析，獲得信號的頻率結(jié)構(gòu)以及各諧波和相位信息。頻譜分析表明：行走時的步頻為1～3 Hz，故保留低頻分量，頻率高于5 Hz的數(shù)據(jù)判定為干擾，使用低通數(shù)字濾波器對糾正零偏后的傳感器數(shù)據(jù)進行濾波處理，經(jīng)過校準后的靜止輸出數(shù)據(jù)趨近于真實的零值，濾除了大部分的靜態(tài)漂移噪聲。

1.2 零速修正算法設(shè)計

零速修正是一種操作簡便且高效的誤差補償技術(shù)，可以有效地為慣性導航系統(tǒng)降低累積誤差。行走時、腳部接觸地面的過程中，慣性傳感器相對于地面有一段短暫的零速時刻，可以認為此時行人處于瞬時靜止狀態(tài)。本文采用三條件判斷法，融合了加速度方差檢測、加速度幅值檢測和角速度幅值檢測3種檢測方法。公式如下

(1)

(2)

(3)

(4)

(5)

(6)

將單個條件結(jié)果進行“與”(&)運算，求出腳著地，即“零速度”的時刻

c=c1&c2&c3

(7)

式中c為邏輯“與”運算的結(jié)果。當c取值為1時，說明此刻是“零速度”時刻；當取值為0時，為行人運動的“非零”速度時刻。

零速修正結(jié)果如圖1，可以看出，通過適當?shù)拈撝翟O(shè)置，零速修正可以較好地判斷出行人的零速時刻。但零速修正算法存在一些缺陷，當行人行走速度較慢時，可能會出現(xiàn)區(qū)域性的連續(xù)零速度點，影響結(jié)果的準確性。

圖1 零速修正結(jié)果

1.3 姿態(tài)識別檢測

針對零速修正中出現(xiàn)連續(xù)零速度點影響判斷精度的情況，本文引入了腳部姿態(tài)識別的方法。行走時，腳面繞水平方向與行進方向垂直的軸向(俯仰角)具有周期性的動作，可以檢測行人腳部姿態(tài)角變化判斷行人的運動狀態(tài)。

(8)

(9)

公式適用于在靜止狀態(tài)計算姿態(tài)角，而運動時的姿態(tài)角需要通過陀螺儀數(shù)據(jù)解算得出。一個動坐標系相對參考坐標系的方位可以完全由動坐標系依次繞3個不同的軸轉(zhuǎn)動的3個角度來確定。如將載體坐標系作為動坐標系，將導航坐標系作為參考坐標系，則姿態(tài)角即為一組歐拉角，按一定的轉(zhuǎn)動順序可以得到導航坐標系到載體坐標系的轉(zhuǎn)換公式[6]

(10)

式中 左側(cè)為本次更新終的歐拉角，對應橫滾角γ、俯仰角θ和偏轉(zhuǎn)角ψ；右側(cè)為上個周期測算出的角度，以及三軸陀螺儀在周期t內(nèi)測得的角速度。因此，求解該微分方程即可解算出當前的歐拉角

(11)

式中φ為需要解算的歐拉角；k為當前數(shù)據(jù)點；t為慣性傳感器的采樣周期，取為0.01 s；et為陀螺儀的零偏值。計算出陀螺儀三個軸的角度變化，以加速度計算出的初始姿態(tài)角為基準，即可得出佩戴在行人腳部的設(shè)備姿態(tài)角數(shù)據(jù)。

從圖2的行人俯仰角數(shù)據(jù)可以看出：行走時俯仰角具有周期性的變化，其極大值和極小值分別為在踏向地面時和離開地面時的姿態(tài)角，由此可以判斷行人的步頻間隔。優(yōu)點是可以根據(jù)行人運動姿態(tài)判斷步數(shù)，判定結(jié)果更加直觀、準確，缺點是姿態(tài)識別判斷條件有限，可能出現(xiàn)多極值的情況，影響判斷精度。

圖2 姿態(tài)識別檢測結(jié)果

本文采取零速修正和姿態(tài)識別結(jié)合算法，首先使用零速修正技術(shù)檢測行人的靜止狀態(tài)，若某靜止時刻超過正常步頻范圍，則在此時刻進行姿態(tài)識別檢測，區(qū)分連續(xù)零速度點，實驗中3種方法的檢測正確率如表1所示，可以看出，零速修正和姿態(tài)識別的結(jié)合算法提高了計步精度。

表1 3種算法準確率對比

2 改進的行人慣性導航算法設(shè)計

獲得了精確的步頻后，可以通過慣性導航算法計算出單步的位移。傳統(tǒng)的捷聯(lián)慣性導航算法原理是通過對加速度進行二次積分，得到速度和位移數(shù)據(jù)。

2.1 單步長慣性導航算法

(12)

再由，可得到物體的位移變化量，最后得出運動物體在n系下的位置為

(13)

通過以上慣性導航算法可以計算出每步步頻對應的單步長，如圖3所示。為利于統(tǒng)計單步長的對應時間以及后續(xù)計算，圖中“步頻”的單位為秒(s)。

圖3 慣性導航算法計算步幅

經(jīng)過統(tǒng)計結(jié)果可以看出，基本捷聯(lián)慣性導航算法計算出的步長與真實值的偏移在30 %以內(nèi)，誤差會隨著時間累積，導致位移結(jié)果出現(xiàn)較大偏差，影響導航精度。

2.2 步長估計算法

統(tǒng)計數(shù)據(jù)表明，人的步頻和步幅之間具有對應關(guān)系，行人步頻一定時，其步幅基本固定，而且行人在直線行走時的步幅和步頻相對穩(wěn)定。同時在慣性導航算法從行人靜止狀態(tài)開始計算的一段時間內(nèi)，其加速度積分導致的累積誤差較小，慣性導航算法結(jié)果準確性較高，故本系統(tǒng)使用行人前期運動的單步長數(shù)據(jù)建立行人的步頻—步長模型。實驗進行了多組不同步速下的實驗并建模，統(tǒng)計得出的步頻—步長模型如表2所示。

表2 步頻—步長模型示例

實驗表明:在行人以慣常的步長和步頻行走時，通過預先設(shè)置的參數(shù)可以保證測量的步長精度較高，但當行人轉(zhuǎn)向或者步頻突變時，實際步長與步頻-步長模型中的參考結(jié)果會有一些偏差。

基本捷聯(lián)慣性導航算法的優(yōu)勢是加速度計和陀螺儀可以適應行人的運動狀態(tài)突變，在行人步頻或航向變化時依然能輸出較為精確的結(jié)果，缺點是隨著行走距離的增加，基本慣性導航計算結(jié)果存在漂移和累積誤差；使用步頻—步長模型的優(yōu)勢是在行人直線行走和步頻變化較小時，其精確度很高，可以有效避免長距離行走之后的累積誤差，然而其在行人在轉(zhuǎn)彎或發(fā)生步頻變化時導致步長改變時偏差較大。

結(jié)合兩種算法的特點，本文采用的改進算法在行人以慣常運動姿態(tài)的行走時使用步頻—步長模型中的單步長數(shù)據(jù)；在行人運動姿態(tài)發(fā)生明顯變化時，使用基本慣性導航算法計算行人位移。根據(jù)行人的運動姿態(tài)變化實時匹配相應的位移算法。

2.3 航向角獲取與處理

智能手機電子羅盤的航向角誤差主要來自手機內(nèi)部電路運行時產(chǎn)生的磁場以及手機周圍環(huán)境磁場產(chǎn)生的磁干擾dt[7]。由于干擾磁場的值變化平緩，不會瞬間突變，因此采用滑動均值濾波算法對電子羅盤輸出數(shù)據(jù)進行校正?；瑒泳禐V波器公式為

(14)

式中k為當前數(shù)據(jù)點；xj為智能手機電子羅盤的輸出原始數(shù)據(jù)，j取當前數(shù)據(jù)點以及當前數(shù)據(jù)點最近的m組數(shù)據(jù)；yk為濾波輸出結(jié)果；m為一次進行濾波處理的數(shù)據(jù)個數(shù)，也稱滑動濾波器階數(shù)，本文根據(jù)行人的步頻特征和實驗數(shù)據(jù)，m取值100。

算法在行人步頻和方向平穩(wěn)時，使用單步中前后兩靜止時刻航向角的均值作為行人兩步之間位移的航向角數(shù)據(jù)，并去除行人腳在空中時的航向角，可以有效避免電子羅盤由于手機的俯仰角變化而產(chǎn)生的航向角漂移，輸出較為精確的方向數(shù)據(jù)。

3 實驗驗證

3.1 導航算法

系統(tǒng)使用了集成藍牙HC—06模塊和MPU6050慣性傳感器，集成了加速度計、角速度計和磁力計，根據(jù)行人的運動特點采用鞋綁式MPU6050慣性傳感器和手機相結(jié)合的數(shù)據(jù)采樣方式。實驗中模塊平穩(wěn)固定于行人腳面，行人手持手機正對行走方向。

基于步長的慣性導航技術(shù)主要包括4個關(guān)鍵技術(shù)：步頻檢測、步長估算、方向檢測和位置估算。定位算法流程如圖4所示，具體步驟如下：

1)數(shù)據(jù)采集和數(shù)據(jù)處理：采集佩戴在行人腳部的慣性傳感器數(shù)據(jù)以及行人手持智能手機中的電子羅盤數(shù)據(jù)并存儲。將原始數(shù)據(jù)進行零偏校準和濾波處理，得到相對準確的實驗數(shù)據(jù)；

2)步頻檢測：通過零速修正技術(shù)和腳部姿態(tài)識別技術(shù)確定行人行走時的零速時刻，并計算單步步頻；

3)步長估計：通過慣性導航算法解算出行人的單步步長，根據(jù)行人前期運動的慣性導航算法結(jié)果建立步長—步頻模型。根據(jù)行人運動姿態(tài)識別分別匹配相應的步長估計算法；

4)通過基于滑動均值濾波的手機電子羅盤獲取行人航向角數(shù)據(jù)，使用結(jié)合步頻檢測、步長估計的行人慣性導航算法計算得出行人的運動軌跡。

圖4 系統(tǒng)流程

3.2 實驗驗證

為了驗證本文算法在實際行人運動中的效果，分別進行了行人的直線行走實驗和矩形行走實驗。表3中的“基本慣性導航算法”指不區(qū)分單步長的基本捷聯(lián)慣性導航算法的位移解算結(jié)果；“改進算法”指加入了零速修正—姿態(tài)設(shè)別結(jié)合算法和基于步頻—步長模型的步長估計算法的改進行人慣性導航算法解算結(jié)果。

圖5為行人自東向西行走35 m時，以坐標零點為起點，分別使用基本慣性導航算法和改進算法計算出行人運動軌跡。從表3的結(jié)果對比可以看出：基本慣性導航算法得到的行人位移信息在前期較為準確，其后期的位置計算有誤差擴大的趨勢，在35 m的直線行進中直線誤差超過4 m，垂直于行進方向的縱向誤差最大超過0.9 m。而改進算法在行人35 m的直線運動中，其直線誤差在1 m以內(nèi)，縱向誤差保持在0.5 m以內(nèi)，準確性更高。

圖5 基本慣導算法和改進算法直線行走軌跡

方向?qū)嶋H結(jié)果基本慣導解算改進算法東向/m35．0030．1734．35北向/m 0．100．920．36

圖6、圖7為行人繞建筑物進行矩形行走示意圖，行走軌跡的東西向距離為26.5 m，南北向距離為8.5 m，分別使用基本慣性導航算法和改進算法計算出行人的運動軌跡。從表4的結(jié)果對比可以看出：傳統(tǒng)的慣性導航算法得到的運動軌跡在前期較準確，但隨著時間的推移誤差逐漸累積和發(fā)散、導致定位失敗，最終終點在(-1.9,9.3)m，誤差很大。而改進算法充分考慮了行人運動的特點，根據(jù)行人步頻和航向的變化通過步頻—步長模型實時校準單步長，并通過滑動均值濾實時校準航向角，最終輸出的導航結(jié)果誤差為(0.19,-0.23)m，運動中東西向和南北向的位移誤差始終保持在2 m以內(nèi)，保持了較高的穩(wěn)定性和準確性。

圖6 基本慣性導航算法完整行走軌跡

圖7 改進算法完整行走軌跡

實際結(jié)果基本慣導解算改進算法起點/m(0，0)(0，0)(0，0)終點/m(0，0)(-1．9，9．3) (0．19，-0．23)

4 結(jié) 論

提出了一種基于運動姿態(tài)識別的行人慣性導航算法，使用MEMS慣性傳感器和智能手機采集行人運動時的慣性數(shù)據(jù)，通過零速修正和姿態(tài)識別獲得行人的腳部姿態(tài)數(shù)據(jù)，獲得了精確的零速時刻和單步步頻數(shù)據(jù)，根據(jù)行人前期運動的慣性導航計算結(jié)果建立步長—步頻模型，并使用基于滑動均值濾波的航向角算法校準行人的方向信息，根據(jù)行人當前的運動姿態(tài)情況分別使用相應的步長估計算法，保證了行人單步的位移保持較高的精度，最終得到精確的行人位移信息。由于慣性導航算法在室內(nèi)行人導航中無法對誤差進行校準，未來可以考慮引入GPS定位技術(shù)或無線定位技術(shù)，在使用慣性導航算法時每隔一段時間對行人的當前位置進行識別和校準，可以更好地校正慣性導航算法的累積誤差，建立應用范圍更廣的行人導航系統(tǒng)。

參考文獻:

[1] 李作虎.衛(wèi)星導航系統(tǒng)性能監(jiān)測及評估方法研究[D].鄭州：解放軍信息工程大學,2012.

[2] 汪少初.基于PDR的定位與跟蹤技術(shù)研究及系統(tǒng)設(shè)計[D].天津：天津大學,2014.

[3] Patrick E,Helen D.IMU:Inertial sensing of vertical CoM movement[J].Journal of Biomechanics,2009,42 (10):1578-1581.

[4] Levi R W,Judd T.Dead reckoning navigational system using accelerometer to measure foot impacts:US,US 5583776[P].1996—06—08.

[5] 孫 偉,初 婧,丁 偉,等.基于IMU旋轉(zhuǎn)的MEMS器件誤差調(diào)制技術(shù)研究[J].電子測量與儀器學報,2015(2):240-246.

[6] Jiménez A R,Seco F,Zampella F.PDR with a foot-mounted IMU and ramp detection[J].Sensors,2011,11(10):9393-9410.

[7] 代 剛.MEMS-IMU誤差分析補償與實驗研究[D].北京：清華大學,2011.