紀志農，苗宏勝，李海軍，孫 偉

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

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

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

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

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

1 基于ZUPT的行人導航系統

1.1 行人導航系統設計

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

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

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

1.2 零速檢測算法

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

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

對于任意一只腳來說，在一個步態(tài)周期內，有一半左右的時間是靜止的，此時足部的速度為0，即零速狀態(tài)，這段時間即為零速區(qū)間，零速區(qū)間內的任一時刻為零速時刻[1]。目前常用的零速判據包括加速度閾值檢測[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為窗口寬度，σfth、σωth分別為單軸加速度、角速率的標準差閾值。

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

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

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

1.3 濾波器設計

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

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

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

量測量：

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

量測矩陣：

標準KF方程如下[3]：

其中，Tf為KF濾波周期，Tn為導航周期[3]。

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

2 基于SVM的自適應零速檢測算法

2.1 慣性數據預處理

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

（1）對當前時刻點所在滑動窗口內的數據點進行遍歷；

（2）將所有數據從小到大排列；

（3）求取序列中點的中位數；

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

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

2.2 步態(tài)的表征

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

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

2.3 基于SVM的零速檢測判據

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

（1）構造樣本數據集

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

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

其中y表示σωy、σωz等參數。

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

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

其中

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

（3）構造決策函數

根據式(12)(17)構造決策函數：

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

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

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

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

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

3 試驗驗證

3.1 試驗內容

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

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

3.2 零速檢測結果

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

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

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

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

3.3 行人導航結果

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

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

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

采用不同零速檢測算法后計算得定位誤差的統計結果見表3。

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

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

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

4 結 論

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

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