一種優(yōu)化的零速檢測行人導(dǎo)航算法

孟祥賓，潘獻飛，胡小平

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

一種優(yōu)化的零速檢測行人導(dǎo)航算法

孟祥賓，潘獻飛，胡小平

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

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

行人導(dǎo)航；MIMU；零速檢測；檢測算法

0　引言

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

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

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

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

1　行人導(dǎo)航算法總體框架

1.1算法編排

圖1　導(dǎo)航編排框圖Fig.1 The blocks of navigation mechanism

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

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

系統(tǒng)的狀態(tài)量取：

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

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

選取北東地（N，E，D）坐標系為導(dǎo)航系，系統(tǒng)方程為：

式中，F(xiàn)為系統(tǒng)轉(zhuǎn)移矩陣，W為系統(tǒng)噪聲。F可通過對式（2）線性化獲得。觀測方程有：

利用建立的系統(tǒng)模型進行卡爾曼濾波，對誤差狀態(tài)量進行估計，實現(xiàn)零速修正。

可見，零速檢測和修正是實現(xiàn)精確行人導(dǎo)航的基礎(chǔ)，本文將重點研究幾種常用算法的優(yōu)缺點同時設(shè)計改進算法。

1.3常用零速檢測算法

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

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

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

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

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

2　優(yōu)化的零速檢測算法

2.1優(yōu)化零速檢測算法基本思路

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

優(yōu)化零速檢測算法可以自適應(yīng)地實現(xiàn)對加速行走（大于5km/h）、角速率值突變和轉(zhuǎn)彎等一些步態(tài)的零速檢測。算法流程圖如圖3所示。

2.2優(yōu)化算法執(zhí)行步驟及理論框架

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

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

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

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

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

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

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

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

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

3　實驗驗證

3.1硬件介紹

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

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

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

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

3.2零速檢測算法驗證

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

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

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

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

3.3樓內(nèi)實測零速修正導(dǎo)航軌跡結(jié)果

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

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

圖7　兩種算法導(dǎo)航定位結(jié)果Fig.7 Positioning trajectories of two methods

4　總結(jié)

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

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

［2］ Groves P D.GNSS與慣性及多傳感器組合導(dǎo)航系統(tǒng)原理（第二版）［M］.北京：國防工業(yè)出版社，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慣性技術(shù)的鞋式個人導(dǎo)航系統(tǒng)［J］.中國慣性技術(shù)學報，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

孟祥賓，男，碩士，研究方向為單兵導(dǎo)航算法。