基于自適應閾值的行人慣性導航零速檢測算法

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

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

0 引言

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

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

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

2) 經驗閾值法[7-8]。它根據加速度[7]和零速檢測閾值或運動頻率[8]與檢測閾值的關系調節閾值，實現閾值自適應的效果，但需要大量的數據分析。

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

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

1 自適應閾值的零速檢測

本文提出的基于自適應閾值的零速檢測算法的結構框圖如圖1所示。本節將分別對自適應閾值的提取過程、更新過程以及整體算法流程進行介紹。

圖1 基于自適應閾值的零速檢測算法框圖

1.1 步態過程中的零速檢測原理

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

圖2 正常步態過程

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

(1)

(2)

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

1.2 閾值提取

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

(3)

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

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

(4)

式中μ為設定的延拓窗口初值。

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

圖3 行走與跑步狀態的統計直方圖

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

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

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

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

1) 確定分界區間范圍。頻數空間中分界區間處在零速與運動區間之間，即Peak I與Peak II之間。因為拐點是曲線的凹凸分界點，利用Peak II的左拐點確定分界區間范圍。根據直方圖擬合函數F(x)的二階導數F(x)和三階導數F?(x)確定擬合函數的拐點G在坐標系中橫坐標值Gx為

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

(5)

2) 求取分界線坐標。利用分界區間的邊界點坐標值獲得頻數分布曲線擬合函數中的最小值minF(x)點在坐標系中橫坐標值minF(x)x為

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

(6)

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

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

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

(7)

圖4 統計直方圖的曲線擬合

1.3 閾值更新條件

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

(8)

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

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

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

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

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

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

(9)

式中：W′(xi)為W(xi)的一階導函數;W″(xi)為W(xi)的二階導函數。

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

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

(10)

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

1.4 自適應閾值算法

本文提出的自適應閾值算法，是根據峰值頻數和步態頻率的變化共同作用來判斷運動狀態是否發生變化，進而重新計算統計直方圖獲得新的閾值。具體算法流程為：

1) 首先設定初始滑動窗口長為600個點，延拓窗口長為20個點。利用檢測數據相鄰兩時刻差分值判斷開始運動的時刻，根據時刻值在窗口內的位置判斷運動前靜止時長。利用式(1)～(4)計算檢測數據并延拓峰值區間。

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

圖6 自適應閾值算法框圖

2 實驗驗證及結果

2.1 步數檢測實驗

實驗采用的傳感器為MPU9150模塊，該模塊封裝了3軸陀螺儀、3軸加速度計，利用STM32開發板對傳感器數據進行采集。為驗證零速檢測的有效性和實際效果，設計了閾值檢測實驗，實驗采用的傳感器固定在左腳足面上，采集了連續直線變速運動，第35 s前為行走狀態，35 s后為跑步狀態，主要對比驗證自適應閾值和固定閾值檢測效果，圖7為截取的部分對比結果。固定閾值法無法準確檢測到速度變化后的零速區間。而自適應閾值法對速度變化前、后零速區間都能成功檢測。

圖7 檢測結果對比圖

為驗證算法的可靠性分別采集多組行走、跑步及上、下樓梯數據，行走、跑步步態每組采集100步，臺階數據每組采集50步，對零速區間檢測的準確率如表1所示。

表1 零速檢測結果

續表

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

2.2 零速區間檢測實驗

每個零速區間起點和終點的準確性對零速補償以及導航軌跡推算至關重要，為驗證每個零速區間始末位置的準確性，采用文獻[1]的實驗數據并利用本文提出的方法得到的檢測結果計算導航軌跡，如圖8所示。該實驗數據采用的傳感器為3DM-GX2模塊，固定在鞋底上，實驗數據采集路徑為“8”字形閉環曲線軌跡，總行程為84 m，且采集起點和終點位置東向偏差和北向偏差在±1 cm內[1]。利用本文算法得到的檢測結果結合卡爾曼零速修正法獲得的導航軌跡如圖8所示。

圖8 運動軌跡圖

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

表2 實驗結果

3 結束語

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