紀志農(nóng)，苗宏勝，李海軍，孫 偉

（北京自動化控制設(shè)備研究所，北京 100074）

對于傳統(tǒng)的行人導(dǎo)航方法，在有衛(wèi)星信號時可依靠衛(wèi)星提供定位導(dǎo)航和授時服務(wù)。但在室內(nèi)、隧道等衛(wèi)星信號無法到達或者被拒止的場景下，迫切需要行人導(dǎo)航系統(tǒng)具有不依賴衛(wèi)星的自主定位導(dǎo)航能力[1]?；谖T性測量單元（Miniature Inertial Measurement Unit, MIMU）的足綁式行人自主導(dǎo)航系統(tǒng)具有低成本、低功耗、體積小等優(yōu)勢，是目前的研究熱點。

然而其核心部件微機電（Micro Electro Mechanical System, MEMS）慣性器件存在精度低、噪聲大等問題，因此目前普遍采用零速修正（Zero Velocity Update,ZUPT）算法抑制航向、位置誤差發(fā)散，以提高導(dǎo)航精度。由于ZUPT僅在零速區(qū)間對載體作濾波修正，因此零速檢測的準確率直接決定了導(dǎo)航精度[2,4]。

為提高零速檢測的準確率對復(fù)雜步態(tài)的適應(yīng)性，戴洪德等提出一種基于偽標準差和N-P準則的行人導(dǎo)航零速檢測算法[5]，該方法可在短距離、正常行走時實現(xiàn)較高的零速檢測精度并提高適應(yīng)性，但在大范圍、長時間、高動態(tài)條件下適應(yīng)性變差。賈錚洋等設(shè)計了混合運動模式下的雙重閾值零速區(qū)間檢測算法[6]，可在上、下樓梯時進行準確的零速檢測，然而由于受閾值限制仍無法適應(yīng)跑步狀態(tài)。陳澤等設(shè)計了一種區(qū)間搜索零速檢測器[7]，通過與J. Nillsson等設(shè)計的基于貝葉斯網(wǎng)絡(luò)的零速檢測器Adaptive-SHOE[8]進行對比，發(fā)現(xiàn)在跑步狀態(tài)下Adaptive-SHOE失效[7]，區(qū)間搜索零速檢測器精度更高。然而由于該方法以Adaptive-SHOE為基準，無法得到快速運動時的零速檢測準確率，且在大動態(tài)運動時易出現(xiàn)局部失效的風險。綜上，提高零速檢測的準確率和適應(yīng)性仍然存在較大的困難。

為解決該問題，本文提出了一種基于SVM自適應(yīng)零速檢測的行人自主導(dǎo)航算法。首先采用中值濾波（Median Filtering, MF）算法對MIMU采集的原始慣性數(shù)據(jù)進行降噪處理，由濾波窗口內(nèi)三軸加速度矢量和的峰值判斷當前步態(tài)，利用支持向量機（Support Vector Machine, SVM）對不同步態(tài)下零速點和非零速點的角速率、加速度的矢量和、標準差進行分類，進而提高零速檢測對復(fù)雜步態(tài)的適應(yīng)性，并通過ZUPT算法實現(xiàn)行人自主導(dǎo)航。

1 基于ZUPT的行人導(dǎo)航系統(tǒng)

1.1 行人導(dǎo)航系統(tǒng)設(shè)計

基于ZUPT的行人自主導(dǎo)航系統(tǒng)流程圖如圖1所示。首先由足綁式MIMU采集人體運動時足部的角速率和加速度，經(jīng)中值濾波預(yù)處理后進行導(dǎo)航解算。同時檢測當前是否為零速狀態(tài)，若判斷當前為零速狀態(tài)，則通過卡爾曼濾波（Kalman Filtering, KF）對當前足部的姿態(tài)、速度、位置誤差進行估計，并進行修正，便可得到行人當前的準確位置[2,3]；若判斷當前為非零速狀態(tài)，則不作修正，以慣性導(dǎo)航解算結(jié)果作為導(dǎo)航數(shù)據(jù)。

圖1 基于ZUPT的行人自主導(dǎo)航流程Fig.1 The process of pedestrian autonomous navigation based on ZUPT

由于人在行走過程中，在非零速時刻，以足部速度為0作為觀測量對姿態(tài)、速度、位置誤差進行修正會使導(dǎo)航結(jié)果產(chǎn)生較大謬誤，因此只能在零速時刻對姿態(tài)、速度、位置誤差進行修正，而其余時刻則不做修正。因此，能否準確檢測出當前是否為零速狀態(tài)，直接影響到行人導(dǎo)航的精度。

1.2 零速檢測算法

人在行走的過程中，每個步態(tài)周期可分為4個階段：離地、懸空擺動、落地、持續(xù)靜止[9,10]，如圖2所示。

圖2 單個完整步態(tài)周期Fig.2 A complete gait cycle

對于任意一只腳來說，在一個步態(tài)周期內(nèi)，有一半左右的時間是靜止的，此時足部的速度為0，即零速狀態(tài)，這段時間即為零速區(qū)間，零速區(qū)間內(nèi)的任一時刻為零速時刻[1]。目前常用的零速判據(jù)包括加速度閾值檢測[11]、角速率閾值檢測[11]、加速度標準差檢測[2]、角速率標準差檢測[2]等。

（1）加速度、角速率閾值檢測

若三軸加速度、角速度的矢量和分別滿足：

則判斷當前為零速狀態(tài)。其中fx(k)、fy(k)、fz(k)分別為k時刻MIMU輸出的三軸加速度，ωx(k)、ωy(k)、ωz(k)分別為三軸角速率，分別為三軸加速度、角速率矢量和，g為重力加速度，fth、ωth分別為加速度、角速率的零速檢測閾值。

（2）加速度、角速率標準差檢測

若三軸加速度、角速率的標準差分別滿足：

則判斷當前為零速狀態(tài)。其中σf(k)、σω(k)分別為k時刻MIMU輸出的單軸加速度、角速率的標準差，fk、ωk分別為單軸加速度、角速率，、分別為窗口內(nèi)加速度、角速率的平均值，N為窗口寬度，σfth、σωth分別為單軸加速度、角速率的標準差閾值。

由于MIMU自身存在一定誤差，且不同人的行走習慣也不同，單一的零速判據(jù)很容易造成大量的誤判（非零速狀態(tài)被判為零速狀態(tài)）和漏判（零速狀態(tài)被判為非零速狀態(tài)），因此目前常采用多條件零速區(qū)間判別方法，即判斷當前MIMU輸出同時滿足多條判據(jù)才認為當前是零速狀態(tài)[12]。

上述各判據(jù)的閾值fth、ωth、σfth、σωth等常采用經(jīng)多次試驗確定的常數(shù)，即得到了固定閾值零速檢測法，對于固定人員的固定步態(tài)（如正常行走）可以實現(xiàn)較高的準確率。然而，當試驗人員、步態(tài)發(fā)生變化（如快跑、慢跑等）時，仍存在大量誤判和漏判的現(xiàn)象。

可將不同步態(tài)劃分為不同的區(qū)間，在不同區(qū)間內(nèi)對零速檢測閾值fth、ωth、σfth、σωth分別進行調(diào)整，即分別對正常行走、慢跑、快跑等步態(tài)設(shè)置不同的閾值，以實現(xiàn)零速檢測算法對不同步態(tài)的適應(yīng)性，即得到了自適應(yīng)閾值零速檢測法。然而在不同區(qū)間內(nèi)，該方法各閾值依然為常數(shù)，雖然在高動態(tài)運動時相較于固定閾值零速檢測法可獲得更高的零速檢測準確率，但仍會存在少量漏判、誤判的現(xiàn)象。

1.3 濾波器設(shè)計

卡爾曼濾波（KF）是一種通過線性化系統(tǒng)的觀測量，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法[3]。線性化后行人自主導(dǎo)航系統(tǒng)的狀態(tài)空間模型可表示為：

其中X(t)為狀態(tài)變量，F(xiàn)(t)為系統(tǒng)矩陣，Z(t)為量測量，H(t)為量測矩陣，w(t)為系統(tǒng)噪聲向量，v(t)為量測噪聲向量。取狀態(tài)變量：

其中，Vδn、Vδu、Vδe分別為系統(tǒng)北向、天向、東向的速度誤差；Lδ、hδ、δλ分別為系統(tǒng)的緯度、高度、經(jīng)度的誤差；φn、φu、φe分別為系統(tǒng)導(dǎo)航坐標系內(nèi)北、天、東三個方向的失準角；?x、?y、?z分別為系統(tǒng)載體坐標系內(nèi)x、y、z三個方向的加速度計零偏；εx、εy、εz分別為載體坐標系內(nèi)x、y、z三個方向的陀螺漂移。

量測量：

其中Vn、Vu、Ve分別為系統(tǒng)北向、天向和東向慣性導(dǎo)航的速度；Vnbase、Vubase、Vebase分別為系統(tǒng)中可作為基準的北向、天向和東向的速度，在零速時刻有：

量測矩陣：

標準KF方程如下[3]：

其中，Tf為KF濾波周期，Tn為導(dǎo)航周期[3]。

由KF估計出各狀態(tài)變量的誤差后，對各狀態(tài)變量進行修正。由于ZUPT算法的固有缺陷，KF無法對航向角進行準確估計，且對三軸加速度計零偏、三軸陀螺漂移的估計周期較長，因此僅對系統(tǒng)的北向、天向、東向速度，俯仰角，滾動角進行修正。且由于僅在零速區(qū)間存在= 0，因此僅在零速區(qū)間對行人的姿態(tài)及速度進行修正，其余時刻則不作修正。

2 基于SVM的自適應(yīng)零速檢測算法

2.1 慣性數(shù)據(jù)預(yù)處理

由于人在行走時，足部運動具有幅度大、沖擊大等特點，因此足綁式MIMU采集的原始慣性數(shù)據(jù)會存在大量的脈沖干擾和隨機噪聲，會降低零速檢測的準確度。本文采用中值濾波算法對MIMU原始數(shù)據(jù)進行預(yù)處理，其具體流程如下[11,13]：

（1）對當前時刻點所在滑動窗口內(nèi)的數(shù)據(jù)點進行遍歷；

（2）將所有數(shù)據(jù)從小到大排列；

（3）求取序列中點的中位數(shù)；

（4）用求取的中位數(shù)替代該時刻原始的數(shù)值。

經(jīng)中值濾波后，MIMU原始數(shù)據(jù)中的高頻噪聲信號得以濾除，且保留了零速點原有的數(shù)據(jù)特征，可有效減少脈沖干擾和隨機噪聲對零速檢測的干擾[11,13]。下文所提到的角速率、加速度均為中值濾波后的數(shù)據(jù)。

2.2 步態(tài)的表征

統(tǒng)計正常行走、慢跑、快跑時MIMU輸出加速度峰值的變化范圍、零速區(qū)間內(nèi)加速度和角速率的均值，結(jié)果見表1。由表1得隨著運動速度的加快，零速和非零速狀態(tài)下加速度、角速度的矢量和均增大，此時固定閾值判據(jù)顯然已不再適用。而其中加速度的變化較為明顯：隨著運動速度的加快，足部需要以更快的速度運動，而步態(tài)周期又會進一步縮短，因此在步態(tài)周期的非零速區(qū)間內(nèi)足部加速度的峰值會增大。因此，可以采用任意時刻所在滑窗內(nèi)加速度矢量和的峰值來對當前的運動狀態(tài)進行表征。

表1 不同步態(tài)足綁式MIMU輸出數(shù)據(jù)Tab.1 The output of foot-mounted MIMU for different gaits

2.3 基于SVM的零速檢測判據(jù)

SVM是一種用于解決二分類問題的機器學(xué)習方法，其基本原理為通過機器學(xué)習，在空間中找到一個最優(yōu)超平面，使不同類型的點間的幾何間隔最大[14,15]?；赟VM的自適應(yīng)零速檢測算法的具體實現(xiàn)過程如下：

（1）構(gòu)造樣本數(shù)據(jù)集

將一套MIMU設(shè)備綁在一只腳上，采集多組不同人員包含多種不同步態(tài)的數(shù)據(jù)，并將其匯總，挑選出其中特征明顯的零速點和非零速點。目前基于SVM的零速檢測與行人導(dǎo)航普遍將樣本數(shù)據(jù)集構(gòu)造為如下形式[14]：

其中n為樣本容量；zi=-1或1，表示當前的足部運動狀態(tài)，zi= 1表示該點為零速點，zi=-1表示該點為非零速點。這樣雖然可以對零速點和非零速點進行分類，然而由于未考慮不同步態(tài)的情況，導(dǎo)致在行人快速運動時分界面失效，且樣本類型單一，從而使零速檢測準確率變差。本文將各點的分別與σfx、σfy、σfz、σωx、σωy、σωz構(gòu)成二維數(shù)組作為SVM的樣本訓(xùn)練集，可實現(xiàn)對不同步態(tài)下零速點和非零速點的上述參數(shù)進行分類，提高零速檢測對不同步態(tài)的適應(yīng)性。構(gòu)造的多組訓(xùn)練樣本數(shù)據(jù)集形式如下：

其中y表示σωy、σωz等參數(shù)。

（2）求解最優(yōu)超平面

由于行人行走的樣本數(shù)據(jù)并不是線性可分的，因此需要先將樣本點通過映射關(guān)系轉(zhuǎn)換到高維空間使其線性可分。此時所求解最優(yōu)超平面的模型為：

其中

就可以將兩種狀態(tài)很好分開。而在復(fù)雜步態(tài)下，線性的分界面已不能將兩種狀態(tài)分開，因此需考慮更換核函數(shù)。由于高斯核對于各種類型的樣本都有較好的性能，且所需參數(shù)較少，因此在缺乏先驗信息的情況下選擇采用高斯核函數(shù)來對式(13)進行求解。取

（3）構(gòu)造決策函數(shù)

根據(jù)式(12)(17)構(gòu)造決策函數(shù)：

對于第j個樣本點，若為正，則認為該點為零速點，否則認為該點為非零速點，由此實現(xiàn)對所有零速點和非零速點的分類。

采用SVM算法對所采集的所有的樣本集中的樣本點進行分類。不同樣本集的分類結(jié)果如圖3所示。圖3中各曲線即為基于SVM得到的零速點和非零速點的邊界在圖示平面內(nèi)的投影。經(jīng)過樣本集的多次擴充后，發(fā)現(xiàn)該最優(yōu)超平面的形狀呈現(xiàn)出大致確定的趨勢，此時的決策函數(shù)即可作為成熟的零速檢測判據(jù)。

圖3 不同樣本集的SVM分類結(jié)果Fig.3 The result classified by SVM for different sample sets

然而由于單一判據(jù)錯判率較高，導(dǎo)致一些非零速點出現(xiàn)在邊界零速區(qū)域，而部分零速點出現(xiàn)在非零速區(qū)域，這些點分別對應(yīng)著誤判和漏判。由第1節(jié)可得，若發(fā)生誤判，會使KF的量測量出現(xiàn)錯誤，導(dǎo)致導(dǎo)航結(jié)果向錯誤的方向修正，引起導(dǎo)航誤差，并影響后續(xù)導(dǎo)航解算；而發(fā)生漏判，則會以慣性導(dǎo)航數(shù)據(jù)作為當前零速點的導(dǎo)航參數(shù)，而在后續(xù)的零速點被修正回來，不會影響后續(xù)導(dǎo)航。因此需優(yōu)先考慮去除誤判點。

通過統(tǒng)計發(fā)現(xiàn)大部分的誤判點僅能使個別參數(shù)y滿足零速條件，即僅會在個別圖中出現(xiàn)在非零速區(qū)域，因此可通過對其他判據(jù)進行判斷來淘汰這些誤判點。只有所有參數(shù)y同時滿足零速條件，才認為當前為零速狀態(tài)。又通過后續(xù)的試驗和仿真，發(fā)現(xiàn)所有錯判點中誤判點的概率要遠大于漏判點。該條件所導(dǎo)致的漏判可忽略不計。

3 試驗驗證

3.1 試驗內(nèi)容

將一套行人導(dǎo)航系統(tǒng)固定在一只腳上，使MIMU的x軸與人體行進方向一致，y軸指天，z軸與人體行進方向垂直并指向行進方向的右側(cè)。在系統(tǒng)開始工作前，將MIMU緊靠在基準面上，使系統(tǒng)工作后保持某一固定初始航向約5 min，然后開始行走。在行走時隨機加入多種特殊步態(tài)，如快跑、慢跑等，并保證行走過程中部分路段被重復(fù)行走至少2次，以便對比2次經(jīng)該路段時的位置。行走30 min左右后回到出發(fā)點，重新將MIMU靠在基準面上使系統(tǒng)位置、航向與初始時刻相同。分別由3位試驗人員重復(fù)上述過程各5次，用于后續(xù)統(tǒng)計。

分別采用基于SVM的自適應(yīng)零速檢測法（SVM自適應(yīng)檢測法）、固定閾值零速檢測法（固定閾值法）、自適應(yīng)閾值零速檢測法（自適應(yīng)閾值法）對足綁式MIMU采集的數(shù)據(jù)進行零速檢測，并進行導(dǎo)航解算。其中三種零速檢測方法的判據(jù)均在試驗前確定，新采集的數(shù)據(jù)可在試驗結(jié)束后作為樣本點對樣本集進行擴充。

3.2 零速檢測結(jié)果

為防止由其他零速檢測算法的結(jié)果作為基準而引起誤差的累加，提升零速檢測準確率的可信度，本文采用對MIMU的輸出進行人工判別來得到零速檢測結(jié)果的基準。由于多次試驗得到的數(shù)據(jù)量太過龐大，因此本文截取多組不同步態(tài)下零速特征明顯的行人行走的片段作為樣本來進行人工判別。以樣本的各幀數(shù)據(jù)的人工判別結(jié)果構(gòu)造序列(i= 1,2,...)作為基準，其中i為幀號，=0或1。若=1則該幀為零速狀態(tài)，= 0表示該幀為非零速狀態(tài)。再分別采用SVM自適應(yīng)檢測法、固定閾值法和自適應(yīng)閾值法對采集到的慣性數(shù)據(jù)進行零速檢測，分別得到零速檢測的結(jié)果序列Li,1、Li,2和Li,3。將Li,1、Li,2、Li,3分別與作對比，計算：

若 ΔLi,j= 0，則該幀零速檢測結(jié)果正確；若ΔLi,j= 1，則該幀發(fā)生了誤判；若 ΔLi,j=-1，則該幀發(fā)生了漏判。按照不同步態(tài)對ΔLi,j及分段，分別統(tǒng)計3種方法不同步態(tài)下零速檢測算法的準確率、誤檢率和漏檢率。取零速檢測準確率的最小值作為該方法的準確率，統(tǒng)計結(jié)果見表2。

由表2得，在正常行走時，三種零速檢測算法的準確率均能達到99%以上，且均維持在較高水平。然而隨著行走速度的加快，采用固定閾值法和自適應(yīng)閾值法的零速檢測準確率顯著下滑，分別降至51.4%和91.5%；而SVM自適應(yīng)檢測法的準確率雖有下降卻仍能達到97.2%，維持在95%以上，說明SVM自適應(yīng)檢測法較固定閾值法及常用自適應(yīng)檢測法具有更高的準確率，對不同步態(tài)的適應(yīng)能力更強。

表2 不同零速檢測算法的準確率Tab.2 The accuracy of zero-speed detection for different zero-speed detection algorithms

3.3 行人導(dǎo)航結(jié)果

采用全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System, GNSS）對起始點及轉(zhuǎn)彎處的經(jīng)緯度進行多次測量，得到其準確位置，作為定位誤差計算的基準點。分別采用3種零速檢測算法對實際采集的MIMU數(shù)據(jù)進行仿真，求仿真得到的導(dǎo)航軌跡各轉(zhuǎn)彎處的位置與基準點位置間的相對距離，即為各轉(zhuǎn)彎處的定位誤差。取單條軌跡定位誤差的最大值作為該條次的定位誤差。仿真得部分軌跡及基準點位置如圖4所示。

圖4 采用不同零速檢測算法時的部分導(dǎo)航軌跡Fig.4 Fragments of the navigation trajectories when using different zero-velocity detection algorithms

由圖4可得，采用SVM自適應(yīng)檢測法時轉(zhuǎn)彎處與基準點的重合度要高于自適應(yīng)閾值法，且明顯高于固定閾值法。圖中方框內(nèi)為跑步時的軌跡，實際軌跡為直線?？梢杂^察到采用固定閾值法時軌跡發(fā)生嚴重偏移，這是由于該段軌跡固定閾值法無法判斷出零速狀態(tài)，未進行零速修正造成的；相較于采用固定閾值法，采用自適應(yīng)閾值法時由于判別出部分零速區(qū)間，故軌跡有所修正，然而錯判現(xiàn)象仍然存在，軌跡仍然存在較大誤差；而采用SVM自適應(yīng)檢測法時，由于該段大部分零速狀態(tài)均能被判斷出來，因此軌跡可被多次修正，因此其偏移更小。

采用不同零速檢測算法后計算得定位誤差的統(tǒng)計結(jié)果見表3。

表3 基于不同零速檢測算法的定位誤差Tab.3 The positioning error based on different zero-speed detection algorithms

由表3得，在其他條件完全相同的情況下，在30 min內(nèi)，采用自適應(yīng)閾值法及固定閾值法時的最大定位誤差為12.37 m和32.02 m，而采用SVM自適應(yīng)檢測法時的最大定位誤差為8.43 m。采用SVM自適應(yīng)檢測法與采用自適應(yīng)閾值法時系統(tǒng)的定位誤差之比為60.3%到84.8%，與采用固定閾值法時系統(tǒng)的定位誤差之比為8.3%至32.6%，即采用SVM自適應(yīng)檢測法時系統(tǒng)的定位精度比采用自適應(yīng)閾值法時提高15.2%以上，比采用固定閾值法時提高67.4%以上。說明采用SVM自適應(yīng)檢測法時的導(dǎo)航精度要遠高于采用自適應(yīng)閾值法和固定閾值法的導(dǎo)航精度。

此外，相較于自適應(yīng)閾值法中判據(jù)需根據(jù)試驗反復(fù)調(diào)整，SVM自適應(yīng)檢測法更易于獲取穩(wěn)定的零速判據(jù)，具有更大的優(yōu)勢。

4 結(jié) 論

本文通過對經(jīng)中值濾波預(yù)處理的足綁式MIMU輸出數(shù)據(jù)進行分析，選擇以某一時刻所在滑窗內(nèi)加速度矢量和的峰值來劃分該時刻步態(tài)。采集多種不同步態(tài)下足部角速率、加速度的原始數(shù)據(jù)，以不同和其對應(yīng)的三軸加速度、角速率的矢量和、標準差等分別作為訓(xùn)練樣本集，采用SVM算法實現(xiàn)對零速點和非零速點的自適應(yīng)劃分。若某一時刻三軸加速度矢量和、角速率的矢量和、標準差等均落在零速區(qū)域，則認為該時刻為零速狀態(tài)。

經(jīng)試驗驗證，在正常行走時，基于SVM的自適應(yīng)零速檢測算法和固定閾值零速檢測算法的零速檢測準確率均能達到一個較高的水平；而跑步時采用SVM自適應(yīng)檢測法的準確率達到97.2%，高于采用自適應(yīng)閾值法時的91.5%，且遠高于采用固定閾值法時的51.4%，說明SVM自適應(yīng)檢測法對復(fù)雜步態(tài)具有更高的準確率和更強的適應(yīng)性；采用SVM自適應(yīng)檢測法時系統(tǒng)的定位誤差降至采用自適應(yīng)閾值法時的84.8%以下、采用固定閾值法時的32.6%以下，即系統(tǒng)導(dǎo)航精度較采用自適應(yīng)閾值法時至少提升了15.2%、較采用固定閾值法時至少提升了67.4%。說明采用SVM自適應(yīng)檢測法時可獲得更高的導(dǎo)航精度?；赟VM自適應(yīng)零速檢測的行人自主導(dǎo)航算法可提升系統(tǒng)對復(fù)雜步態(tài)的適應(yīng)性，進而提高系統(tǒng)導(dǎo)航精度，具有一定的實際應(yīng)用價值。