基于自適應(yīng)閾值的行人慣性導(dǎo)航零速檢測算法

岑世欣，高振斌，于 明，曾 成

(河北工業(yè)大學(xué) 電子信息工程學(xué)院,天津 300401)

0 引言

隨著微機(jī)電系統(tǒng)(MEMS)的興起，許多低成本的MEMS慣性傳感器出現(xiàn)，這極大地促進(jìn)了慣性導(dǎo)航系統(tǒng)(INS)在民用領(lǐng)域的發(fā)展。但MEMS慣性傳感器存在嚴(yán)重的漂移誤差，在低成本的MEMS慣性傳感器上的誤差表現(xiàn)尤為明顯。這些誤差會(huì)隨著時(shí)間的變化不斷積累，導(dǎo)致系統(tǒng)長時(shí)間精度較差，這是慣性導(dǎo)航技術(shù)面臨的最大難題。利用零速修正技術(shù)抑制累積誤差可以提高導(dǎo)航系統(tǒng)的精確性，其中零速修正技術(shù)包括零速區(qū)間檢測和零速誤差補(bǔ)償，零速區(qū)間檢測的精確性直接影響著零速修正的效果，是提高INS定位精度的重要一環(huán)[1]。

傳統(tǒng)零速檢測算法一般采用加速度和角速度平方和[1-3]或加速度滑動(dòng)窗口方差[4]作為檢測數(shù)據(jù),然后利用固定閾值[1]檢測零速區(qū)間。固定閾值法在固定運(yùn)動(dòng)步態(tài)條件下能實(shí)現(xiàn)零速區(qū)間的判斷，但對于不同行人或不同運(yùn)動(dòng)狀態(tài)適應(yīng)性較差。除固定閾值法外，目前典型的零速檢測方法主要有：

1) 多步態(tài)檢測法[5-6]。它通過判斷步態(tài)模式調(diào)節(jié)閾值檢測零速區(qū)間，但對于非正常的步態(tài)模式檢測效果較差。

2) 經(jīng)驗(yàn)閾值法[7-8]。它根據(jù)加速度[7]和零速檢測閾值或運(yùn)動(dòng)頻率[8]與檢測閾值的關(guān)系調(diào)節(jié)閾值，實(shí)現(xiàn)閾值自適應(yīng)的效果，但需要大量的數(shù)據(jù)分析。

3) 機(jī)器學(xué)習(xí)方法[9]。它利用隱馬爾可夫模型(HMM)對數(shù)據(jù)進(jìn)行判斷，但同樣需要大量數(shù)據(jù)訓(xùn)練模型參數(shù)，且復(fù)雜度較高[1]。

針對上述問題，本文提出了一種基于自適應(yīng)閾值的零速檢測算法，通過安裝在足部的MEMS慣性傳感器采集數(shù)據(jù)，以合加速度A(k)、合角速度W(k)作為檢測樣本數(shù)據(jù)，利用滑動(dòng)極值濾波器對數(shù)據(jù)濾波，將濾波后的檢測數(shù)據(jù)映射到頻數(shù)空間，分析其統(tǒng)計(jì)特征提取閾值，通過插值波峰檢測法獲得步態(tài)頻率信息及峰值頻數(shù)，以判斷運(yùn)動(dòng)狀態(tài)變化，繼而更新濾波窗口長度得到新閾值來實(shí)現(xiàn)不同個(gè)體以及不同運(yùn)動(dòng)狀態(tài)下的零速區(qū)間檢測閾值的自適應(yīng)性。

1 自適應(yīng)閾值的零速檢測

本文提出的基于自適應(yīng)閾值的零速檢測算法的結(jié)構(gòu)框圖如圖1所示。本節(jié)將分別對自適應(yīng)閾值的提取過程、更新過程以及整體算法流程進(jìn)行介紹。

圖1 基于自適應(yīng)閾值的零速檢測算法框圖

1.1 步態(tài)過程中的零速檢測原理

人在步行時(shí)的步態(tài)過程如圖2所示。根據(jù)零速檢測原理，當(dāng)運(yùn)動(dòng)到第三階段時(shí)，整個(gè)足部與地面有短時(shí)間的接觸，稱為零速靜止階段[7]。在零速靜止階段時(shí)，足部速度為0。因此，此刻的合角速度值及去除重力分量的合加速度值在零速靜止階段都為0。當(dāng)檢測到零速區(qū)間，將速度誤差重置，這種算法被稱為零速更新算法[10]，實(shí)現(xiàn)性能依賴于零速區(qū)間的檢測精度。

圖2 正常步態(tài)過程

A(k)、W(k)都能有效表現(xiàn)出步態(tài)過程中的零速靜止階段,合角速度包含更清晰的步態(tài)頻率信息[1]。A(k)、W(k)分別為

(1)

(2)

式中：ax(k)、ay(k)、az(k)為慣性傳感器的k時(shí)刻的三軸加速度值;wx(k)、wy(k)、wz(k)為慣性傳感器k時(shí)刻的三軸角速度值。

1.2 閾值提取

通過多次進(jìn)行運(yùn)動(dòng)檢測的實(shí)驗(yàn)表明，一個(gè)有效區(qū)分運(yùn)動(dòng)和靜止的指標(biāo)是在滑動(dòng)窗口內(nèi)數(shù)據(jù)的極值變化。窗口內(nèi)數(shù)據(jù)的峰值分布范圍能有效反映該時(shí)刻傳感器是否處于動(dòng)態(tài)狀態(tài)。統(tǒng)計(jì)特征可以提取數(shù)據(jù)分布屬性，從而有效反映行人的當(dāng)前狀態(tài)，即將樣本數(shù)據(jù)映射到有限個(gè)分組的頻數(shù)空間獲得統(tǒng)計(jì)特征，再利用其統(tǒng)計(jì)特征提取檢測閾值，并且頻數(shù)空間可用頻數(shù)分布直方圖直觀表示。但利用式(1)、(2)計(jì)算得到A(k)后直接映射到頻數(shù)空間會(huì)存在數(shù)據(jù)的峰值分布不明顯的問題，因此，對A(k)進(jìn)行預(yù)處理十分關(guān)鍵。利用滑動(dòng)極值濾波處理非峰值數(shù)據(jù)以提升峰值在滑動(dòng)窗口中的分布占比：

(3)

式中z(k)可分別取A(k)或W(k)。其中滑動(dòng)窗口內(nèi)最大值為

g[z(k)]=argmaxx-μsxsx+μ[z(x)]

(4)

式中μ為設(shè)定的延拓窗口初值。

圖3為行走步態(tài)及跑步步態(tài)下濾波后合加速度的頻數(shù)直方圖。在直角坐標(biāo)系中，x表示樣本數(shù)據(jù)的連續(xù)可取數(shù)值，按A(k)的最小值a和最大值b把數(shù)據(jù)分為m組，各數(shù)據(jù)組的邊界范圍按左閉右開區(qū)間，如[a,a+d)，[a+d,a+2d)，…,組距為d=(b-a)/m。頻數(shù)表示落在各組樣本數(shù)據(jù)的個(gè)數(shù)。

圖3 行走與跑步狀態(tài)的統(tǒng)計(jì)直方圖

頻數(shù)分布直方圖能清楚顯示各組頻數(shù)分布情況,又易于顯示各組之間頻數(shù)的差別，且可以獲取直觀、形象的數(shù)據(jù)分布情況。由于人體運(yùn)動(dòng)的數(shù)據(jù)中均混有零速區(qū)間和運(yùn)動(dòng)區(qū)間數(shù)據(jù)，造成了將樣本數(shù)據(jù)映射到頻數(shù)空間后服從雙峰分布，即包含2個(gè)獨(dú)立的偏態(tài)分布。

圖3中兩種不同步態(tài)的直方圖均包含雙峰分布,且分組數(shù)m=10。以行走狀態(tài)為例，其中左偏態(tài)分布峰值(記為Peak I)由直方圖橫坐標(biāo)分布區(qū)間段x=1的矩形組成；直方圖橫坐標(biāo)x=5處為拐點(diǎn)，右峰值(記為Peak II)由直方圖橫坐標(biāo)區(qū)間[5,10]的多個(gè)矩形組成。

組成Peak I的直方圖中矩形逆映射到數(shù)據(jù)組邊界范圍是[a,a+d)，分布于該組邊界范圍的樣本數(shù)據(jù)對應(yīng)運(yùn)動(dòng)過程中的零速區(qū)間；組成Peak II的直方圖中矩形逆映射到數(shù)據(jù)組邊界范圍是[b-5d,b)，分布于該組邊界范圍的樣本數(shù)據(jù)對應(yīng)運(yùn)動(dòng)過程中的運(yùn)動(dòng)區(qū)間。在上述兩區(qū)間中存在一數(shù)據(jù)組，邊界范圍是[a+d,b-5d)，對應(yīng)直方圖中被映射為直方圖x∈[1,5]的矩形組成的區(qū)域，且位于樣本數(shù)據(jù)的零速區(qū)間與運(yùn)動(dòng)區(qū)間之間，是兩區(qū)間的分界區(qū)間。

在分界區(qū)間內(nèi)，直方圖存在一頻數(shù)最小數(shù)據(jù)組，根據(jù)直方圖定義可知,分布在該組所映射在樣本數(shù)據(jù)空間邊界范圍內(nèi)的樣本數(shù)據(jù)最少。所以根據(jù)頻數(shù)最小數(shù)據(jù)組逆映射于樣本數(shù)據(jù)空間的位置，即零速檢測最優(yōu)閾值所在的數(shù)據(jù)組，能夠區(qū)分零速和運(yùn)動(dòng)兩種狀態(tài)。但是由于直方圖將樣本數(shù)據(jù)映射到頻數(shù)空間的分組有限，每組組距較大，利用頻數(shù)最小數(shù)據(jù)組不能獲得精確的閾值。因此，通過曲線擬合法近似地刻畫頻數(shù)空間中離散點(diǎn)之間的函數(shù)關(guān)系，用擬合曲線函數(shù)在分界區(qū)間的最小值，即圖4中垂直于直方圖橫軸的分界線的橫坐標(biāo)值，逆映射到樣本數(shù)據(jù)空間中可得到精確的零速檢測閾值。具體選取閾值過程為:

1) 確定分界區(qū)間范圍。頻數(shù)空間中分界區(qū)間處在零速與運(yùn)動(dòng)區(qū)間之間，即Peak I與Peak II之間。因?yàn)楣拯c(diǎn)是曲線的凹凸分界點(diǎn)，利用Peak II的左拐點(diǎn)確定分界區(qū)間范圍。根據(jù)直方圖擬合函數(shù)F(x)的二階導(dǎo)數(shù)F(x)和三階導(dǎo)數(shù)F?(x)確定擬合函數(shù)的拐點(diǎn)G在坐標(biāo)系中橫坐標(biāo)值Gx為

Gx={xi|F(xi)=0 &F?(xi)<0}

(5)

2) 求取分界線坐標(biāo)。利用分界區(qū)間的邊界點(diǎn)坐標(biāo)值獲得頻數(shù)分布曲線擬合函數(shù)中的最小值minF(x)點(diǎn)在坐標(biāo)系中橫坐標(biāo)值minF(x)x為

minF(x)x={xi|F′(xi)=0 &xi

(6)

式中F(x)為F(x)的一階導(dǎo)函數(shù)，小于Gx的條件是為了防止與擬合函數(shù)極大值后面的極小值在判斷時(shí)混淆。根據(jù)最小值點(diǎn)的橫坐標(biāo)值可得分界線為x=minF(x)x。

3) 求取閾值。零速檢測閾值可根據(jù)分界線逆映射到樣本數(shù)據(jù)空間得到：

ε=minF(x)x×(b-a)/m

(7)

圖4 統(tǒng)計(jì)直方圖的曲線擬合

1.3 閾值更新條件

當(dāng)運(yùn)動(dòng)速度或運(yùn)動(dòng)模式變化，但閾值未改變的情況下會(huì)出現(xiàn)檢測不到、誤檢測等問題[6]，需要更新閾值以適應(yīng)變化的檢測數(shù)據(jù)。運(yùn)動(dòng)狀態(tài)變化根據(jù)頻數(shù)空間Peak II的偏態(tài)分布與運(yùn)動(dòng)頻率兩方面判斷。通過觀測多組實(shí)驗(yàn)發(fā)現(xiàn)，相對于慢速度的運(yùn)動(dòng)，隨著運(yùn)動(dòng)速度的增大，零速區(qū)間減小，代表運(yùn)動(dòng)區(qū)間的樣本數(shù)據(jù)組邊界范圍內(nèi)數(shù)據(jù)出現(xiàn)的頻數(shù)增加，因此頻數(shù)空間中組成Peak II的總頻數(shù)會(huì)隨之變化。由于組成Peak II在直方圖中矩形個(gè)數(shù)，即直方圖分組數(shù)不變，頻數(shù)增加只引起矩形高度變化。運(yùn)動(dòng)狀態(tài)的變化可以由峰值平均頻數(shù)的變化判斷。峰值平均頻數(shù)Vave為

(8)

式中:h(x)為x的頻數(shù)值，x可取[1,m]中任意整數(shù);[Gx]表示一個(gè)小于或等于Gx的最大整數(shù)。

檢測數(shù)據(jù)中的周期性波峰能夠體現(xiàn)運(yùn)動(dòng)的頻率信息，從而體現(xiàn)運(yùn)動(dòng)狀態(tài)的變化。合角速度的周期性波峰分布較明顯[1]，根據(jù)合角速度的波峰信息可獲得步態(tài)頻率信息。但運(yùn)動(dòng)過程中抖動(dòng)或傳感器噪聲等原因在主峰旁會(huì)出現(xiàn)偽波峰，如圖5所示。

圖5 角速度極大值擬合曲線示意圖

由于偽波峰的存在，通過比較數(shù)據(jù)的值不能有效地提取峰值頻率。因此，使用一、二階差分信息來識別波峰。具體算法流程為:

1) 計(jì)算合角速度，并利用一、二階差分運(yùn)算得到所有極大值點(diǎn)，其中包含主波峰點(diǎn)和偽波峰點(diǎn)。所有極大值點(diǎn)橫坐標(biāo)的集合P′x為

P′x={xi|W′(xi)=0 &W″(xi)<0}

(9)

式中：W′(xi)為W(xi)的一階導(dǎo)函數(shù);W″(xi)為W(xi)的二階導(dǎo)函數(shù)。

2) 對第一步得到的所有極值點(diǎn)利用3次樣條插值擬合出合角速度極大值曲線(見圖5)。

3) 利用第一步中式(9)求極值法對極大值曲線求極值可得到主波峰點(diǎn)集合Px。根據(jù)主波峰點(diǎn)之間差值的倒數(shù)可得到人的步態(tài)頻率為

(10)

式中：Px(i)為主波峰點(diǎn)集合中第i個(gè)點(diǎn);fs為傳感器采樣頻率。

1.4 自適應(yīng)閾值算法

本文提出的自適應(yīng)閾值算法，是根據(jù)峰值頻數(shù)和步態(tài)頻率的變化共同作用來判斷運(yùn)動(dòng)狀態(tài)是否發(fā)生變化，進(jìn)而重新計(jì)算統(tǒng)計(jì)直方圖獲得新的閾值。具體算法流程為：

1) 首先設(shè)定初始滑動(dòng)窗口長為600個(gè)點(diǎn)，延拓窗口長為20個(gè)點(diǎn)。利用檢測數(shù)據(jù)相鄰兩時(shí)刻差分值判斷開始運(yùn)動(dòng)的時(shí)刻，根據(jù)時(shí)刻值在窗口內(nèi)的位置判斷運(yùn)動(dòng)前靜止時(shí)長。利用式(1)～(4)計(jì)算檢測數(shù)據(jù)并延拓峰值區(qū)間。

2) 對第一步得到數(shù)據(jù)計(jì)算統(tǒng)計(jì)直方圖，根據(jù)直方圖進(jìn)行曲線擬合，計(jì)算拐點(diǎn)的橫坐標(biāo)Gx是否小于m/2,判斷頻數(shù)空間是否符合雙峰分布。利用式(9)、(10)中插值波峰檢測法計(jì)算其峰值初始滑動(dòng)窗口內(nèi)的f及Vave。根據(jù)閾值提取算法獲得初始零速檢測閾值。

圖6 自適應(yīng)閾值算法框圖

2 實(shí)驗(yàn)驗(yàn)證及結(jié)果

2.1 步數(shù)檢測實(shí)驗(yàn)

實(shí)驗(yàn)采用的傳感器為MPU9150模塊，該模塊封裝了3軸陀螺儀、3軸加速度計(jì)，利用STM32開發(fā)板對傳感器數(shù)據(jù)進(jìn)行采集。為驗(yàn)證零速檢測的有效性和實(shí)際效果，設(shè)計(jì)了閾值檢測實(shí)驗(yàn)，實(shí)驗(yàn)采用的傳感器固定在左腳足面上，采集了連續(xù)直線變速運(yùn)動(dòng)，第35 s前為行走狀態(tài)，35 s后為跑步狀態(tài)，主要對比驗(yàn)證自適應(yīng)閾值和固定閾值檢測效果，圖7為截取的部分對比結(jié)果。固定閾值法無法準(zhǔn)確檢測到速度變化后的零速區(qū)間。而自適應(yīng)閾值法對速度變化前、后零速區(qū)間都能成功檢測。

圖7 檢測結(jié)果對比圖

為驗(yàn)證算法的可靠性分別采集多組行走、跑步及上、下樓梯數(shù)據(jù)，行走、跑步步態(tài)每組采集100步，臺階數(shù)據(jù)每組采集50步，對零速區(qū)間檢測的準(zhǔn)確率如表1所示。

表1 零速檢測結(jié)果

續(xù)表

第一組/步第二組/步第三組/步真實(shí)步數(shù)/步固定閾值行走10099102100跑步989498100臺階45474550

2.2 零速區(qū)間檢測實(shí)驗(yàn)

每個(gè)零速區(qū)間起點(diǎn)和終點(diǎn)的準(zhǔn)確性對零速補(bǔ)償以及導(dǎo)航軌跡推算至關(guān)重要，為驗(yàn)證每個(gè)零速區(qū)間始末位置的準(zhǔn)確性，采用文獻(xiàn)[1]的實(shí)驗(yàn)數(shù)據(jù)并利用本文提出的方法得到的檢測結(jié)果計(jì)算導(dǎo)航軌跡，如圖8所示。該實(shí)驗(yàn)數(shù)據(jù)采用的傳感器為3DM-GX2模塊，固定在鞋底上，實(shí)驗(yàn)數(shù)據(jù)采集路徑為“8”字形閉環(huán)曲線軌跡，總行程為84 m，且采集起點(diǎn)和終點(diǎn)位置東向偏差和北向偏差在±1 cm內(nèi)[1]。利用本文算法得到的檢測結(jié)果結(jié)合卡爾曼零速修正法獲得的導(dǎo)航軌跡如圖8所示。

圖8 運(yùn)動(dòng)軌跡圖

將本文算法與固定閾值算法及多步態(tài)零速檢測算法[5]利用圖8的數(shù)據(jù)進(jìn)行對比。將3種算法的檢測結(jié)果結(jié)合零速修正算法生成導(dǎo)航軌跡，用終點(diǎn)平面誤差[8]對比3種算法的檢測效果。檢測結(jié)果如表2所示。其中，終點(diǎn)平面誤差為終點(diǎn)與起點(diǎn)位置的歐式距離，誤差率為終點(diǎn)平面誤差與總行程的比值。

表2 實(shí)驗(yàn)結(jié)果

3 結(jié)束語

本文提出了基于自適應(yīng)閾值的MEMS行人慣性導(dǎo)航零速檢測算法，首先對檢測數(shù)據(jù)進(jìn)行滑動(dòng)極值濾波，利用統(tǒng)計(jì)分布特征提取最優(yōu)閾值，通過插值波峰檢測法獲得步態(tài)頻率，根據(jù)頻率變化及數(shù)據(jù)頻數(shù)分布特性更新閾值,以達(dá)到自適應(yīng)閾值的目的。經(jīng)多組實(shí)驗(yàn)驗(yàn)證在行走、跑步及臺階的運(yùn)動(dòng)模式下檢測的準(zhǔn)確率達(dá)到98%。相對于固定閾值法檢測零速區(qū)間的精確度有較大提高，將檢測結(jié)果用于導(dǎo)航位置解算的誤差率小于1%。與其他自適應(yīng)閾值算法相比，本文算法減少了運(yùn)算量，且不需要輔助傳感器，避免了機(jī)器學(xué)習(xí)類算法的高復(fù)雜度，具有精度高，成本低，實(shí)時(shí)性好等特點(diǎn)。對提升導(dǎo)航精度有很好的實(shí)際應(yīng)用價(jià)值。