行人慣性導(dǎo)航零速檢測(cè)算法*

費(fèi)程羽， 蘇　中， 李　擎

(北京信息科技大學(xué) 高動(dòng)態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京 100101)

?

行人慣性導(dǎo)航零速檢測(cè)算法*

費(fèi)程羽， 蘇中， 李擎

(北京信息科技大學(xué) 高動(dòng)態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京 100101)

摘要:在行人慣性導(dǎo)航中，零速檢測(cè)是實(shí)現(xiàn)速度誤差清零和導(dǎo)航誤差估計(jì)的前提，有著重要的作用。針對(duì)行人運(yùn)動(dòng)過(guò)程中零速區(qū)間時(shí)間間隔短難以檢測(cè)的問(wèn)題，提出了一種基于人體腳部運(yùn)動(dòng)特征的零速檢測(cè)算法，將步行運(yùn)動(dòng)抽象成了一個(gè)包含4個(gè)隱含狀態(tài)與15個(gè)觀測(cè)量的隱馬爾可夫模型，并闡述了模型構(gòu)建機(jī)理。利用Baum—Welch算法訓(xùn)練和優(yōu)化模型參數(shù)，提高了檢測(cè)準(zhǔn)確率。實(shí)驗(yàn)結(jié)果表明：所提出的方法零速檢測(cè)效果較好，且采用該方法的行人慣性導(dǎo)航系統(tǒng)，其定位誤差約為行進(jìn)距離的0.73 %，定位精度較高。

關(guān)鍵詞：行人慣性導(dǎo)航； 零速檢測(cè)； 隱馬爾可夫模型； Baum—Welch算法

0引言

現(xiàn)有的行人慣性導(dǎo)航中絕大多數(shù)采用慣性導(dǎo)航計(jì)算位置，并設(shè)置零速修正算法消除速度積累誤差[1～4]。所謂零速修正算法是指在行人運(yùn)動(dòng)過(guò)程中的零速區(qū)間內(nèi)利用卡爾曼濾波器估計(jì)導(dǎo)航誤差，抑制位置與速度誤差的發(fā)散的方法。由于行人步行運(yùn)動(dòng)過(guò)程中腳部接觸地面的時(shí)間即零速區(qū)間時(shí)間很短(通常為0.1～0.3 s)，因此，如何準(zhǔn)確可靠地檢測(cè)零速區(qū)間成為了一個(gè)重要問(wèn)題。

將步行運(yùn)動(dòng)按照腳的狀態(tài)進(jìn)行劃分，如果每一步中腳的不同狀態(tài)均能夠被有效檢測(cè)，那么,就可以得到準(zhǔn)確的零速區(qū)間，依據(jù)這種思想,本文提出了一種基于隱馬爾可夫模型的零速檢測(cè)算法，將行人的步行運(yùn)動(dòng)過(guò)程抽象為一個(gè)隱馬爾可夫模型，并利用Baum—Welch算法訓(xùn)練和優(yōu)化模型參數(shù)，從而實(shí)現(xiàn)的運(yùn)動(dòng)過(guò)程中的零速檢測(cè)。

1行人的步行運(yùn)動(dòng)特征

對(duì)于行人的步行運(yùn)動(dòng)而言，在運(yùn)動(dòng)中的任意時(shí)刻至少有一只腳處于與地面接觸狀態(tài)，所以，按照步行過(guò)程中腳的運(yùn)動(dòng)狀態(tài)，可以將一個(gè)步行周期大體分為兩個(gè)階段：靜止階段與擺動(dòng)階段。擺動(dòng)階段又可以分為三部分，即前腳尖離地、空中擺動(dòng)、后腳跟著地三個(gè)過(guò)程，因此,將一個(gè)步行周期分為前腳尖離地、空中擺動(dòng)、后腳跟著地與全腳掌著地4個(gè)過(guò)程。為了獲取行人運(yùn)動(dòng)的慣性信息，探索行人的步行運(yùn)動(dòng)特性，將慣性測(cè)量單元(IMU)安裝于行人的腳部，記錄行走運(yùn)動(dòng)過(guò)程的加速度與角速度信息，設(shè)置采樣頻率為200 Hz，以保證記錄的數(shù)據(jù)能夠真實(shí)地反映全部步行運(yùn)動(dòng)過(guò)程。IMU的安裝位置與軸向如圖1所示。

圖1　慣性測(cè)量裝置Fig 1　Inertial measurement unit

通過(guò)實(shí)驗(yàn)發(fā)現(xiàn)，行人在步行運(yùn)動(dòng)過(guò)程中，其三軸加速度與三軸角速度均存在一定的周期性變化規(guī)律，總體來(lái)說(shuō)，x軸角速度(如圖2)的周期性特征更為明顯，且輸出曲線變化更為平緩，易于特征提取，因此,選取x軸角速度為后續(xù)的隱馬爾科夫零速檢測(cè)模型的觀測(cè)量。

圖2　x軸角速度Fig 2　x-axis angular velocity

2零速檢測(cè)算法

對(duì)于行人步行運(yùn)動(dòng)，其存在兩個(gè)隨機(jī)過(guò)程:一個(gè)是慣性加速度與角速度的測(cè)量，這是一個(gè)可見(jiàn)的過(guò)程；另一個(gè)是步行運(yùn)動(dòng)中4種狀態(tài)的傳遞過(guò)程，這是不可見(jiàn)的，但每個(gè)狀態(tài)通過(guò)可能的x軸角速度輸出記號(hào)，有了可能的概率分布。

2.1隱馬爾可夫模型

隱馬爾可夫概率模型具有模型參數(shù)。充分定義模型，需要以下元素[5]：

1)隱含狀態(tài)數(shù)N；

2)每個(gè)隱含狀態(tài)下可觀測(cè)向量數(shù)M；

3)隱含狀態(tài)轉(zhuǎn)移概率矩陣A={aij}

aij=P{qt+1=j|qt=i},1≤i,j≤N,

(1)

式中qt為t時(shí)刻的狀態(tài)，aij為狀態(tài)i到狀態(tài)j的轉(zhuǎn)移概率，并且應(yīng)滿足正態(tài)隨機(jī)約束

aij≥0,1≤i,j≤N,

(2)

(3)

4)輸出概率分布B={bj(Oi)}，bj(Oj)為與狀態(tài)j相關(guān)的有限高斯分布

(4)

5)初始狀態(tài)概率分布π={πj}，表示在初始時(shí)刻t=1時(shí)，觀察向量的概率分布

π=P(q1=i),1≤i≤N,

(5)

圖3　模型狀態(tài)與觀測(cè)向量的定義Fig 3　Definition of model state and observation vector

在行人的步行運(yùn)動(dòng)過(guò)程中，理想的狀態(tài)傳遞過(guò)程是A→B→C→D，然而，由于行人的運(yùn)動(dòng)存在一定的隨機(jī)性，加上外部因素的干擾，狀態(tài)傳遞往往不是在理想的情況下進(jìn)行的，因此,需要調(diào)整初始狀態(tài)轉(zhuǎn)移概率矩陣的矩陣參數(shù)，經(jīng)過(guò)實(shí)驗(yàn)分析得到調(diào)整后的初始狀態(tài)轉(zhuǎn)移概率矩陣A[6]

(6)

觀測(cè)概率為每個(gè)觀測(cè)值中出現(xiàn)隱含狀態(tài)(狀態(tài)A,狀態(tài)B,狀態(tài)C和狀態(tài)D)的概率，為了獲取初始的觀測(cè)概率，需利用統(tǒng)計(jì)學(xué)方法處理觀測(cè)樣本。由于原始的角速度數(shù)據(jù)不能直接作為觀測(cè)樣本，因此，需要對(duì)其進(jìn)行一系列的處理，處理流程如圖4所示。

圖4　觀測(cè)向量提取算法流程圖Fig 4　Flow chart of observation vector extraction algorithm

如圖4所示，由于原始的角速度數(shù)據(jù)存在尖峰(如圖4(a)圓圈內(nèi))，因此,首先采用平滑濾波處理數(shù)據(jù)得到濾波后的數(shù)據(jù)(如圖4(b))，然后對(duì)濾波后的數(shù)據(jù)進(jìn)行峰值提取得到僅保留峰值的數(shù)據(jù)(如圖4(c))，但由于同一個(gè)狀態(tài)內(nèi)存正在多個(gè)峰值(如圖4(c)圓圈內(nèi))不利于狀態(tài)的統(tǒng)計(jì)，所以,將同一狀態(tài)內(nèi)多余的峰值進(jìn)行濾除僅保留絕對(duì)值最大的峰值(如圖4(d))。最后將處理后的角速度數(shù)據(jù)按照不同的觀測(cè)值進(jìn)行區(qū)域劃分(如圖5)，統(tǒng)計(jì)每個(gè)狀態(tài)落在不同觀測(cè)值區(qū)域內(nèi)的概率。統(tǒng)計(jì)結(jié)果如表1所示。

圖5　觀測(cè)值區(qū)域劃分Fig 5　Observation value region division

序號(hào)觀測(cè)概率/%狀態(tài)A狀態(tài)B狀態(tài)C狀態(tài)D10017.9020073.8503001.651.654001.651.655003.30060001.65760.031.2200825.200009001.6501001.6501.65114.557.65042.67127.581.65043.08131.321.6507.65141.3223.880015062.3000

2.2模型參數(shù)優(yōu)化

狀態(tài)轉(zhuǎn)移概率與觀測(cè)概率初始化完成后，利用Baum—Welch算法優(yōu)化模型參數(shù)。Baum—Welch算法是一種廣義的期望最大化算法，算法流程如下[5]

(7)

(8)

(9)

(10)

(11)

γt(j,m)=

(12)

優(yōu)化后的狀態(tài)轉(zhuǎn)移概率矩陣A1為

(13)

優(yōu)化后的狀態(tài)轉(zhuǎn)移流程如圖6所示，優(yōu)化后的觀測(cè)概率如表2所示。

圖6　狀態(tài)轉(zhuǎn)移流程圖Fig 6　Flow chart of state transition

3行人慣性導(dǎo)航系統(tǒng)框架

行人慣性導(dǎo)航系統(tǒng)中廣泛采用的系統(tǒng)架構(gòu)是由Eric Foxlin[1]提出的，即利用安裝于腳背上的慣性傳感器所輸出的數(shù)據(jù)進(jìn)行捷聯(lián)解算與零速檢測(cè)，并利用零速檢測(cè)結(jié)果觸發(fā)卡爾曼濾波器來(lái)估計(jì)系統(tǒng)的狀態(tài)誤差，然后反饋校正捷聯(lián)解算誤差，實(shí)現(xiàn)行人的定位導(dǎo)航。

本文中利用所提出的基于隱馬爾可夫模型的零速檢測(cè)方法替換原框架中的零速檢測(cè)部分，所構(gòu)成的新行人導(dǎo)航系統(tǒng)框架如圖7所示。

表2　優(yōu)化后的觀測(cè)概率

圖7　行人慣性導(dǎo)航系統(tǒng)框架Fig 7　Framework of pedestrian inertial navigation system

4實(shí)驗(yàn)設(shè)置與結(jié)果

為了驗(yàn)證本文所提出的隱馬爾可夫零速檢測(cè)算法的性能，用三條件零速檢測(cè)算法(C3)[2]的檢測(cè)結(jié)果作為參考量，實(shí)驗(yàn)中設(shè)定的三條件算法參數(shù)分別為：加速度模值在閾值thamin=9.5 m/s2和閾值thamax=10.8 m/s2之間；加速度方差小于閾值thσa=1.3 m/s2；角速度模值小于閾值thω=50°/s。

利用兩種零速檢測(cè)算法分別對(duì)采集到的行人步行運(yùn)動(dòng)數(shù)據(jù)進(jìn)行零速檢測(cè)，檢測(cè)效果的對(duì)比圖如圖8所示。

圖8　檢測(cè)效果對(duì)比Fig 8　Test effect contrast

通過(guò)實(shí)驗(yàn)可以看出：當(dāng)行人進(jìn)行步行運(yùn)動(dòng)時(shí)，本文提出的算法與三條件法的零速檢測(cè)效果基本一致，均能夠有效地檢測(cè)出行人運(yùn)動(dòng)過(guò)程中的零速區(qū)間，但三條件法在計(jì)算過(guò)程中需同時(shí)用到三個(gè)軸加速度與角速度數(shù)據(jù)，而本文所提出的算法僅需要x軸的角速度數(shù)據(jù)即可，減小了計(jì)算量與計(jì)算時(shí)間。為了進(jìn)一步驗(yàn)證零速檢測(cè)算法在行人導(dǎo)航系統(tǒng)中的應(yīng)用效果，進(jìn)行了閉合曲線行走實(shí)驗(yàn)，行進(jìn)軌跡顯示如圖9所示。

圖9　行進(jìn)軌跡顯示Fig 9　March trajectory display

實(shí)驗(yàn)中的行走路徑為兩個(gè)約為13.5 m×6.8 m的矩形，共行走3圈，行走距離為243.6 m，由圖中軌跡顯示結(jié)果可以看出:三條軌跡基本重合，驗(yàn)證了算法的穩(wěn)定性。起落點(diǎn)的誤差距離為1.78 m，因此,其定位精度誤差為整個(gè)行走距離的0.73 %。

5結(jié)論

本文依據(jù)行人的步行運(yùn)動(dòng)特征將每一步按照腳的運(yùn)動(dòng)狀態(tài)劃分為4個(gè)隱馬爾可夫隱含狀態(tài)，以單軸的角速度數(shù)據(jù)為觀測(cè)量，同時(shí)利用Baum—Welch算法訓(xùn)練和優(yōu)化模型參數(shù)，從而實(shí)現(xiàn)了運(yùn)動(dòng)過(guò)程中的零速區(qū)間的有效檢測(cè)。實(shí)驗(yàn)結(jié)果表明:該方法能夠有效地檢測(cè)出行人步行運(yùn)動(dòng)過(guò)程中的零速區(qū)間，且采用該方法進(jìn)行零速檢測(cè)的行人慣性導(dǎo)航系統(tǒng)的穩(wěn)定性較好，其定位誤差在整個(gè)行走路程的1 %以內(nèi)。

參考文獻(xiàn):

[1]Foxlin E.Pedestrian tracking with shoe-mounted inertial sensor-s[J].Computer Graphics and Applications,IEEE,2005,25(6):38-46.

[2]崔瀟,秦永元,周琪,等.鞋式個(gè)人導(dǎo)航系統(tǒng)算法和試驗(yàn)研究[J].測(cè)控技術(shù),2013(3):138-142.

[3]李超,蘇中,朱嘉林.可穿戴式自主定位技術(shù)的零速觸發(fā)算法研究[J].傳感技術(shù)學(xué)報(bào),2014,27(5):627-632.

[4]Skog I,H?ndel P,Nilsson J O,et al.Zero-velocity detection—An algorithm evaluation[J].IEEE Transactions on Biomedical Engineering,2010,57(11):2657-2666.

[5]Cheshomi S,Rahati Q S,Akbarzadeh T M R.Hybrid of chaos optimization and Baum-Welch algorithms for HMM training in continuous speech recognition[C]∥2010 International Conference on Intelligent Control and Information Processing(ICICIP),IEEE,2010:83-87.

[6]ParkSK,SuhYS.Azerovelocitydetectionalgorithmusinginertialsensorsforpedestriannavigationsystems[J].Sensors,2010,10(10):9163-9178.

Zero velocity detection algorithm for pedestrian inertial navigation*

FEI Cheng-yu, SU Zhong, LI Qing

(Beijing Key Laboratory of High Dynamic Navigation Technology,University of Beijing Information Science & Technology,Beijing 100101,China)

Abstract:In pedestrian inertial navigation, zero velocity detection is premise to reset velocity errors and estimate navigation error.Aiming at problem that time interval of zero velocity is short and it is difficult to detect in pedestrian moving process,a zero velocity detection algorithm based on human foot movement characteristics is proposed,where walking motion is abstracted into a hidden Markov model with 4 hidden states and 15 observations, and construction mechanism of model is described.The model parameters are trained and optimized by Baum—Welch algorithm and detection accuracy is improved.Experimental results show that the proposed method has better effect on zero velocity detection, and the positioning error of pedestrian inertial navigation system using this method is about 0.73 % of the travel distance, and the positioning precision is high.

Key words:pedestrian inertial navigation; zero velocity detection; hidden Markov model; Baum—Welch algorithm

DOI:10.13873/J.1000—9787(2016)03—0147—04

收稿日期：2015—07—09

*基金項(xiàng)目：國(guó)家自然科學(xué)基金資助項(xiàng)目(61471046)；北京市科技計(jì)劃課題項(xiàng)目(Z131100005313009)；北京市教委創(chuàng)新能力提升計(jì)劃項(xiàng)目(PXM2015—014224—000029)

中圖分類號(hào)：TP 212.9

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1000—9787(2016)03—0147—04

作者簡(jiǎn)介:

費(fèi)程羽(1991-)，男，吉林省長(zhǎng)春人，碩士研究生，研究方向?yàn)閼T性器件和慣性導(dǎo)航系統(tǒng)。