李劉頌， 徐向波

(北京林業(yè)大學 工學院，北京 100083)

0 引 言

步行者導航廣泛應用于礦井定位、消防員定位等步行者的步行特征沒有規(guī)律?；趹T性導航系統(tǒng)的步行者導航主要有兩種解算算法：一種是行人航跡推算(pedestrian dead-reckoning，PDR)算法[1,2]，包括航向、步頻和步長的估計，這種算法主要適用于有規(guī)律的步行者導航的情況，但由于樣本的差異性和周圍環(huán)境的復雜度，有一定的局限性，由于傳感器自身精度較低，會產(chǎn)生較大的累積誤差，無法滿足要求[3，4]；另一種是基于零速檢測(zero velocity detection，ZVD)[5]的慣性導航系統(tǒng)推算算法，這種方法通過零速檢測將步行狀態(tài)分為靜止和運動兩個狀態(tài)，運動狀態(tài)用慣性導航進行步長估計，在靜止狀態(tài)進行零速檢測，根據(jù)零速的速度誤差、位置誤差進行修正，以提高導航精度。通過零速修正現(xiàn)在常用的幾種零速檢測算法對比[6,7]，本文提出一種基于滑動平均的極大似然估計算法，經(jīng)實驗驗證，該方法能夠有效檢測零速區(qū)間的范圍，證明了該方法的有效性。

1 步態(tài)分析

人體行走的典型步態(tài)，根據(jù)傳感器的運動特征，步態(tài)可分為兩個階段。分析步態(tài)特性便于對行人行進過程中的零速狀態(tài)進行檢測。 結(jié)合步態(tài)周期，本文將步態(tài)周期靜態(tài)相位中行人運動腳腳后跟著地、站立階段定義為行人步態(tài)中的零速區(qū)間，靜止階段大約持續(xù)時間為0.1～0.3 s[8]，加速度的理論輸出值是重力加速度，陀螺理論輸出值為零。

2 定位算法設(shè)計

本文提出的基于零速檢測的慣導導航算法主要包括零速區(qū)間的檢測算法、步長估計算法、零速修正算法、初始對準算法以及濾波算法。如圖1所示，通過加速度計以及陀螺儀的輸出值進行慣性導航系統(tǒng)解算得到速度、姿態(tài)以及位置，同時步態(tài)區(qū)間檢測算法判斷行人處于何種步態(tài)，進行零速更新，結(jié)合零速更新算法對速度、姿態(tài)、位置進行修正，提高行人定位系統(tǒng)的精度。

圖1 算法結(jié)構(gòu)

3 零速檢測算法

根據(jù)加速度計和陀螺儀的輸出，設(shè)計相應的閾值，可以判斷步行狀態(tài)。行人的行走步態(tài)可分為邁步階段和靜止階段，理論上行人處于靜止狀態(tài)，角速度值和水平加速度值應為零以及豎直方向的加速度值應等于重力常量。多數(shù)的零速檢測算法是基于對傳感器輸出的加速度和角速度的模值、方差以及幅值和已設(shè)定的閾值進行比較，提取出步態(tài)中的零速點[9]，但行人在行走過程中，足部與地面的接觸時間特別短，這類算法易誤判零速區(qū)間導致丟失零速點，導致行人的速度、位置誤差增加。本文算法是通過對行人處于站立相、擺動相進行概率分配，根據(jù)廣義似然函數(shù)法，確定出行人的站立相[10]。

設(shè)行人處于擺動相的狀態(tài)值為H0，處于站立相的狀態(tài)值為H1。假設(shè)H1為真值時，此時行人處于擺動狀態(tài)的概率表達式為:PFA=Pr{H1|H0}，以及假設(shè)H0為真值時，行人處于擺動狀態(tài)的概率表達式為PD=Pr{H1|H1}

(1)

式(1)為當前時刻是處于行走擺動狀態(tài)，其中，γ為檢測的閾值，L為對于每一個Zn的值的似然比。

采用極大似然估計法對θ進行估計，其中，θ為加速度和角速度值

(2)

(3)

(4)

(5)

若設(shè)未知數(shù)θ在H0,H1中極大似然估計值分別為0,1，則得到

(6)

由式(6)進行相應的計算可以得到

(7)

式(7)兩邊取對數(shù)可得

(8)

(9)

式中ZVD為檢測結(jié)果，1為零速，0為邁步狀態(tài)。

4 零速修正算法

在零速檢測區(qū)間進行零速修正[11]，采用卡爾曼濾波算法。在檢測為零速區(qū)間時，將速度置為0，利用零速修正姿態(tài)誤差、速度誤差和位置誤差，設(shè)計卡爾曼濾波器。

狀態(tài)變量為X=[PVφaω]，其中,P=[PxPyPz]為位置誤差，V=[VxVyVz]為速度誤差，φ=[φxφyφz]為姿態(tài)誤差，a和ω分別為加速度計和陀螺儀的零偏誤差。

觀測變量選擇零速區(qū)間解算出的速度誤差V，所以，觀測矩陣為H=[O3×3I3×3O3×3O3×3O3×3],在零速區(qū)間進行時間更新和信息更新，達到零速修正的效果。

5 實驗驗證

5.1 實驗平臺

選用慣性元器件(MPU6050)和磁強計(HMC5983)組合的方式，傳感器的性能參數(shù)如表1所示。

表1 MEMS傳感器性能參數(shù)

實驗場地選在北京林業(yè)大學圖書館前廣場，經(jīng)測量廣場每塊瓷磚的長度為0.6 m，將傳感器固定在步行者腳上，采集數(shù)據(jù)如圖2所示。

圖2 實驗數(shù)據(jù)采集

5.2 零速區(qū)間數(shù)檢測

通過對加速度和角速度的數(shù)據(jù)值，選定合適的閾值γ′，本實驗中γ′為108，如圖3所示判斷靜態(tài)區(qū)間，結(jié)果如圖4所示。分別計算行人在不同行走速度下的零速區(qū)間的個數(shù)，并進行誤差分析，由表2可以看出，該零速檢測算法具有較高的精確性。

圖3 T值

圖4 零速區(qū)間判斷

試驗類型零速區(qū)間實際值計算值誤差/%慢速行走2550255000常速行走2550255000快速行走2550255000

5.3 步長估計

實驗采集數(shù)據(jù)，步行者在圖書館門口沿一塊瓷磚東向行走一瓷磚的距離，圖5中x軸、y軸、z軸分別代表地理坐標系的東北天方向，從圖5可以得到表3中的數(shù)據(jù)，可以看出對于步長的估計推算相對誤差為2.7 %。單步誤差比較大，主要原因是實驗人員不能準確跨出單步距離。

圖5 單步三軸位移

測試項x/my/mz/m理論值0.5930.0920實測值0.6110.0780.018誤差0.0180.0140.018

5.4 行人軌跡推算

實驗采集數(shù)據(jù)，步行者在圖書館門口沿瓷磚西向行走12塊瓷磚，大概12 m的距離，通過實驗得到圖6虛線為計算軌跡，實線為實際軌跡，方塊標記為起始位置，由圖看出：兩個軌跡基本重合。經(jīng)計算得到表4，步行12 m的相對誤差為1.6 %。圖7為基于零速檢測和基于慣性導航推算，結(jié)果表明，零速檢測能夠有效解決慣性導航長時間誤差漂移、步行者軌跡推算精度低的缺點，且能夠檢測腳步的運動特征。

圖6 三維軌跡信息

測試項x/my/mz/m理論值-12.0000實測值-11.790.28-0.29總誤差0.210.280.29

圖7 基于零速檢測和純慣導軌跡推算的三軸位置

6 結(jié)束語

本文采用的基于滑動平均的極大似然估計算法，能夠準確檢測步行者零速區(qū)間，并進行零速修正，實驗結(jié)果表明：基于滑動平均的零速檢測算法能夠準確檢測步數(shù)，將累計的相對誤差減小到1.6 %，避免了慣性導航長時間漂移累積，最終定位效果比較穩(wěn)定，滿足了礦井、消防員等的定位要求。