一種優化的零速檢測行人導航算法

孟祥賓，潘獻飛，胡小平

（國防科學技術大學機電工程與自動化學院，長沙410073）

一種優化的零速檢測行人導航算法

孟祥賓，潘獻飛，胡小平

（國防科學技術大學機電工程與自動化學院，長沙410073）

針對軍事上對單兵定位和民用領域為行人提供位置服務的需求，提出了足部固定MIMU的行人導航算法。利用人體行走過程中腳部與地面相接觸的靜止時間段，采用零速更新（Zero Velocity Update，ZVU）對INS繼續對準或標定，而零速檢測又是實現零速修正的基礎。研究對比了在足部固定MIMU進行零速檢測的常用方法，針對常用算法閾值自適應性差，無法檢測步速變化轉彎等一些異常情況對行人零速檢測產生誤判等問題，提出優化的零速判斷算法，使得加速度計信號和陀螺儀信號綜合應用到零速檢測。最后，利用改進算法設計行人室內平地正常行走（大約5km/h）包含步速變化和轉彎過程的實驗。實驗結果表明，優化的零速檢測算法相比之前的常用算法，能更準確地檢測零速并進行修正，對導航軌跡和導航誤差有更好的修正效果。

行人導航；MIMU；零速檢測；檢測算法

0　引言

行人導航（Pedestrian Navigation，PN）是指用戶通過負載一定的傳感器設備完成導航和實時定位的功能，同時可以選擇攜帶無線通信設備將導航信息傳輸給監控人員［1］，是未來導航領域最具挑戰性的應用之一。新興的行人導航技術能夠為復雜環境下實施搶險救援、樓宇消防、反恐作戰的人員提供準確的導航信息，又可以為普通市民在大型超市等室內場所提供準確的位置服務，避免人員走失等事故。

微慣性測量單元（Micro Inertial Measurement Unit，MIMU）具有體積小、價格低、功耗低及易攜帶等優點，是自主式行人導航的理想器件。實際應用中，MIMU可以負載在人體的腰部、足部、手腕等部位［2］。結合人體運動特點，MIMU負載在人體足部，利用人正常行走過程中，足部接觸地面的零速作為觀測量來修正系統誤差，即零速修正的方法是一種比較常見的行人導航方法。我們可以很好地利用零速時間段來對行人的位置進行修正。因此，準確地檢測出零速時刻成為零速修正的基礎。

因為MIMU能夠為用戶提供加速度和角速度信息，很多學者都借助這兩種信息的統計特征，利用信號的周期性來實現步態檢測。Jimenez在步態識別中利用了角速度信息，取得了較好的檢測效果［3］。西北工業大學的秦永元老師采用對加速度信息求取滑動方差，用固定閾值來對方差值依次比較，進而分辨出行人步態的擺動和著地靜止階段［4］，但常用算法多是采用加速度計輸出信號，實驗多限于簡單的直線，步速穩定的情況。給出零速判斷大致結果。還有學者利用其他傳感裝置輔助檢測，比如壓電傳感器裝置［5］、肌電圖等。精度雖然比利用傳統的MIMU提供信息要高，但要付出更大的代價和成本。

本文僅利用足部固定MIMU導航系統，分析零速檢測的三種方法：加速度移動方差檢測、加速度幅值檢測和角速率能量檢測方法。最后提出一種可以綜合三種算法，將加速度計和陀螺儀輸出信號一起用于零速檢測的算法。解決步速變化、轉彎、足部異常運動造成角速率變大引起的零速誤判問題，最大化地利用慣性傳感器的輸出信號，結果表明，新算法能夠更精確地實現零速檢測，能夠進一步地提高行人導航算法的精度和實用性。

1　行人導航算法總體框架

1.1算法編排

圖1　導航編排框圖Fig.1 The blocks of navigation mechanism

1.2擴展卡爾曼濾波器設計

擴展卡爾曼濾波器是實現零速修正的方式，在此考慮建立以系統誤差為狀態的卡爾曼濾波模型［6］。

系統的狀態量?。?/p>

式中，δφ、δv、δr分別為慣導器件的姿態誤差角向量、導航系下速度誤差向量和位置誤差向量。Δb、Δf、ηb、ηf分別為加速度計的零偏誤差和刻度因子誤差以及陀螺的零偏誤差和刻度因子誤差。狀態模型有：

式中，ωin為導航系相對慣性系的旋轉角速度，ωen為導航系相對地球的旋轉角速度，η為陀螺輸出誤差，Δ為加速度計輸出誤差。

選取北東地（N，E，D）坐標系為導航系，系統方程為：

式中，F為系統轉移矩陣，W為系統噪聲。F可通過對式（2）線性化獲得。觀測方程有：

利用建立的系統模型進行卡爾曼濾波，對誤差狀態量進行估計，實現零速修正。

可見，零速檢測和修正是實現精確行人導航的基礎，本文將重點研究幾種常用算法的優缺點同時設計改進算法。

1.3常用零速檢測算法

常用的零速檢測算法思路是：取定一個適當的滑動窗口，隨著時間的推移遍歷整個加速度數據，并對每一次窗口內的加速度序列求取方差，利用已經設定好的閾值對結果進行篩選。若大于閾值，則認為此刻腳步處于擺動，否則認為處于靜止零速狀態［3］。目前，大多數學者根據人體行走的步態特征，設置多個閾值用來檢測不同步速下的零速狀態，構建出適應多步態的零速檢測算法。

但是，常用的零速檢測算法存在一定的局限性。第一，加速度閾值單一，對不同行走狀態自適應性程度不高；第二，行人在正常行走時，可能出現的角速率值突變、步速加快等干擾，而加速度方差曲線規律性變化消失，不能很好地給出零速判斷結果。

圖2給出了一組傳統算法解算的零速檢測結果，在檢測結果的后半段，由于足部的運動狀態出現加速變化，使得用于求取加速度計平均方差用的零速檢測方法出現誤判，事先設定的加速度固定閾值也無法滿足這種步態變化對零速檢測帶來的影響。為了更好地進行零速檢測就需要改進閾值設置辦法，設置更多的限制約束條件來處理一些正常行走狀態可能出現的加速、轉彎等使零速檢測出現誤判情況的運動狀態。自適應閾值、陀螺儀輸出數據能夠體現這些變化帶來的影響，基于這樣一個事實，我們對常用零速檢測算法進行優化。

綜合考慮，我們將加速度方差、幅值、閾值設置以及角速度信號一起作為零速檢測的判斷標準，設計優化的零速檢測算法。

圖2　常用算法得出的一組零速檢測結果Fig.2 A result of stance phase detection of traditional algorithm

2　優化的零速檢測算法

2.1優化零速檢測算法基本思路

本文優化零速檢測算法的思路是：根據行人步態特征的先驗條件和傳感器輸出信號，在常用加速度移動方差檢測方法基礎上，引入加速度幅值檢測和角速率能量檢測兩種方法進行綜合判斷，增加了零速判斷的限制約束條件，由實際經驗以及相關文獻［7］可知，加速度移動方差檢測對于正常步速直線行走的零速判斷效果較好。加速度幅值檢測對于行人大步、步速稍快條件下零速檢測輔助效果較好。角速率能量檢測主要是針對陀螺信號處理，對于行走時可能出現的足部左右晃動異常引起的角速率值突變以及轉彎過程零速檢測效果明顯。這樣，將加速度計輸出信息和陀螺儀輸出信息綜合用于零速檢測，使我們能夠更準確地判斷出零速時刻，進行零速修正，降低由于微慣性測量單元精度低帶來的導航誤差漂移影響。

優化零速檢測算法可以自適應地實現對加速行走（大于5km/h）、角速率值突變和轉彎等一些步態的零速檢測。算法流程圖如圖3所示。

2.2優化算法執行步驟及理論框架

步驟1：根據原始加速度計、陀螺儀數據特征初步判斷行人在常速行走時出現加速行走、邁大步和角速率值突變等運動狀態，根據結果選擇最適合的零速判斷標準。

步驟2：優化算法采用自適應的算法設置閾值，與以往傳統固定閾值相比，算法閾值自適應變化，可以從前1～10步行走數據設置閾值。以行人第6步開始為例，加速度方差算法為：Var1，Var2，…，Var6分別為這6步的最小方差，令閾值γv=max（VarN-5，VarN-4，…，VarN），相比于單一閾值，改進算法的閾值設置可以根據步態特征自適應變化。加速度峰值、角速率能量檢測器閾值γm和γω設置參照此方法。

步驟3：行人大部分時間都是沿直線行走，首先采用加速度滑動方差方法進行判斷，利用MIMU采集到的原始加速度數據，構建一個滑動窗口，讓窗口隨著加速度信息的采集依次對數據進行遍歷，求取加速度移動方差：

當滿足式（5）時，初步判斷為零速靜止狀態，執行步驟2；不滿足式（5）則判斷此刻為非零速。

步驟4：將加速度幅值零速檢測方法應用于輔助加速度移動方差檢測，用于改進行走過程中大步或者步速稍快狀態下零速檢測的誤判。該算法基本思路是：檢測量要使得零速檢測算法能對多種步態比力矢量是否接近于g進行判斷，如果是則得出結論：IMU是靜止狀態，行人處于零速。如果加速度幅值滿足以下條件：

若滿足式（6）條件，執行步驟3進一步判斷，不滿足條件判定行人此刻非零速。

步驟5：行人正常行走時可能出現足部左右異常擺動，利用陀螺儀信號的能量值進一步檢測MIMU是否處于靜止狀態［8］，這樣就能改進在足部抖動、轉彎過程中的一些零速誤判，利用角速率能量來檢驗行人是否處于靜止狀態需滿足以下條件：

優化算法將加速度計和陀螺儀輸出信號同時應用于零速檢測，三種方法相互輔助，具有一定的互補性，提高行人導航零速檢測的精度。

圖3　優化零速檢測算法流程圖Fig.3 Flow chart of modified ZUPT mechanism

3　實驗驗證

3.1硬件介紹

實驗使用的MIMU為荷蘭Xsens公司的MTi集成傳感器。它內部包含有三軸加速度計和三軸陀螺以及三軸磁強計，可以通過RS232接口或USB接口直接與計算機相連。如圖4所示，該系統以其體積小、質量小、低成本和較好的可操作性使得其在工程中得到了廣泛應用。

圖4　實驗所用MIMUFig.4 The MIMU used in the experiment

MTi性能標稱參數：實際由靜態測試得到的陀螺三個軸零偏穩定性參數分別為30.7（°）/h，26.8（°）/h，40.5（°）/h。

器件用于實驗時，為減小腳部的抖動干擾以及行進時可能帶來的較大加速度沖擊對實驗數據的影響，在器件與腳面之間加入減震模塊，用于緩解干擾。

3.2零速檢測算法驗證

實驗測試路線為實驗樓的一層大廳，路徑為從起始點行走矩形框，行走過程中會有步速的不同變化和靜止時段以及轉彎過程，行人行走4圈，最后回到起始點；試驗過程中將試驗設備固定在試驗者左腳后鞋跟，從鞋后引出USB線連接電腦采集數據，安裝示意圖如圖5所示。首先，測試者在實驗場內進行15min的系統靜止預熱，完成系統的初始對準。MTi輸出頻率設為100Hz。

圖5　MIMU安裝示意圖Fig.5 The installation instruction of MIMU

圖6選取了一組行人沿設定軌跡、步態行走下兩種零速檢測算法處理后的對比結果。從圖6中可以看出常用加速度方差零速檢測在方差呈現規律變化時，零速判斷結果與方差曲線對應較好，如85s～87s的零速判斷。為驗證算法，實驗過程中我們設計了邁大步、步速加快和轉彎過程等特殊時刻，常用算法零速結果出現誤判，如83s～84s的大步行走使得加速度方差值突變，88s～90s的足部開始動作前異常抖動造成的不規律變化。在優化算法中，為了更好地區分對比，我們用“1”和“0”來代表零速決策的真假，“1”為零速時刻。對比步態不規律時刻零速判斷結果，與實際實驗零速結果對應可知改進的零速檢測算法能有效克服誤判，檢測精度更高。

圖6　零速檢測結果對比Fig.6 The comparison of two stance phase detection methods

3.3樓內實測零速修正導航軌跡結果

為了進一步驗證優化零速檢測算法對行人導航結果精度的提升，利用慣導解算方法得出行人運動軌跡作對比分析。

導航解算行走軌跡如圖7所示。圖7（a）和圖7（b）分別表示常用加速度方差零速檢測算法軌跡和改進后零速檢測算法軌跡結果。對比兩組測試的解算軌跡可以看到，原算法處理后的軌跡尤其是每一圈矩形框之間進行切換時，矩形框的偏移很大，而轉彎過程、大步或快速行走的零速檢測誤判是造成導航軌跡偏差較大的原因之一；而優化后的零速檢測算法卻可以有效降低位置漂移率，使軌跡處理結果相對規則。從而表明改進后的零速檢測算法能對行走過程中步速加快、大步、轉彎等過程有較好的零速判斷能力，進而能夠有效地減少行人室內導航的漂移誤差，進一步提高在沒有衛星條件下行人導航的精度。

圖7　兩種算法導航定位結果Fig.7 Positioning trajectories of two methods

4　總結

本文研究了行人導航系統中的零速檢測算法，提出了一種優化的零速檢測算法，將加速度計信號和陀螺儀信號綜合應用于零速檢測，充分利用慣性傳感器的輸出信號特征，對行走過程中可能出現的邁大步、步速變化、轉彎過程的零速判別效果較好。并通過實驗驗證與常用的加速度方差零速檢測算法進行對比，結果表明優化的零速檢測導航算法具有更高的零速檢測精度和導航精度，進一步提高了行人導航算法的可靠性和實用價值。

［1］ 李辰祥.基于MEMS行人慣性導航的零速度修正技術研究［D］.廈門大學，2014. LIChen-xiang.Researchonzero-velocityupdate technology of pedestrian inertial navigation based on MEMS［D］.Xiamen University，2014.

［2］ Groves P D.GNSS與慣性及多傳感器組合導航系統原理（第二版）［M］.北京：國防工業出版社，2015. Groves P D.Principles of GNSS，inertial and multisensor intergrated navigation systems（2nd）［M］.Beijing：National Defense Industry Press，2015.

［3］ Jimenez A R，Seco F，Prieto J C，et al.A comparison of pedestrian dead-reckoning algorithms using a low-cost MEMS IMU［C］.Proceeding of the 6thInternational Symposium on Intelligent Signal Processing，Budapest，Hungary：IEEE Press，2009.

［4］ 張金亮，秦永元，梅春波.基于MEMS慣性技術的鞋式個人導航系統［J］.中國慣性技術學報，2011，19（3）：253-256. ZHANG Jin-liang，QIN Yong-yuan，MEI Chun-bo.Shoemounted personal navigation system based on MEMS inertial technology［J］.Journal of Chinese Inertial Technology，2011，19（3）：253-256.

［5］ Harle R，Taherian S，Pias M，et al.Towards real-time profiling of sprints using wearable pressure sensors［J］. Computer Communications，2012，35（6）：650-660.

［6］ Titterton D H，Weston J L.Strapdown inertial navigation technology［M］.Lexington，Massachusetts，USA，2007.

［7］ Skog I，Handel P，Nilsson J O，et al.Zero-velocity detection-an algorithm evaluation［J］.IEEE Transactions on Biomedical，Engineering，2010，57（11）：2657-2666.

［8］ Feliz R，Zalama E，Gómez J.Pedestrian tracking using inertial sensors［J］.Journal of Navigation，2007，60（3）：391-407.

A Novel Pedestrian Navigation Algorithm Based on ZVU

MENG Xiang-bin，PAN Xian-fei，HU Xiao-ping
（College of Mechatronic Engineering and Automation，National University of Defense Technology，Changsha 410073）

In order to meet the needs of the location of a solider in military fields and provide location service for pedestrian in civilian fields.We propose foot pedestrian navigation algorithm based on MIMU.We use zero-speed updates to the alignment of the INS or calibration at the stance period when the foot landing on the ground.The zero speed detection is the basis of zero velocity update.This paper compares the traditional approach in the foot fixed MIMU zero speed detection. As for threshold of conventional algorithms is not adaptive，and can not detect the impact of the problems caused by the change of velocity.We suggest an modified algorithm for zero speed judgment，making the accelerometer signal and the gyroscope signal in maximize application to zero speed detection.Finally，we carry out the experiment of indoor pedestrian walking in normal speed（about 5km/h）.The experimental results show that the new zero speed detection algorithm can detect the zero speed more accurately than the traditional algorithm and correct the navigation error and the navigation error is better than the traditional algorithm.

pedestrian navigation；MIMU；zero velocity update testing（ZUPT）；detection methods

U666.1

A

1674-5558（2016）01-01196

10.3969/j.issn.1674-5558.2016.04.005

2015-09-09

孟祥賓，男，碩士，研究方向為單兵導航算法。