李劉頌， 徐向波

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

0 引 言

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

1 步態(tài)分析

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

2 定位算法設(shè)計(jì)

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

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

3 零速檢測(cè)算法

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

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

(1)

式(1)為當(dāng)前時(shí)刻是處于行走擺動(dòng)狀態(tài)，其中，γ為檢測(cè)的閾值，L為對(duì)于每一個(gè)Zn的值的似然比。

采用極大似然估計(jì)法對(duì)θ進(jìn)行估計(jì)，其中，θ為加速度和角速度值

(2)

(3)

(4)

(5)

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

(6)

由式(6)進(jìn)行相應(yīng)的計(jì)算可以得到

(7)

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

(8)

(9)

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

4 零速修正算法

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

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

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

5 實(shí)驗(yàn)驗(yàn)證

5.1 實(shí)驗(yàn)平臺(tái)

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

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

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

圖2 實(shí)驗(yàn)數(shù)據(jù)采集

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

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

圖3 T值

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

試驗(yàn)類型零速區(qū)間實(shí)際值計(jì)算值誤差/%慢速行走2550255000常速行走2550255000快速行走2550255000

5.3 步長(zhǎng)估計(jì)

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

圖5 單步三軸位移

測(cè)試項(xiàng)x/my/mz/m理論值0.5930.0920實(shí)測(cè)值0.6110.0780.018誤差0.0180.0140.018

5.4 行人軌跡推算

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

圖6 三維軌跡信息

測(cè)試項(xiàng)x/my/mz/m理論值-12.0000實(shí)測(cè)值-11.790.28-0.29總誤差0.210.280.29

圖7 基于零速檢測(cè)和純慣導(dǎo)軌跡推算的三軸位置

6 結(jié)束語(yǔ)

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