苗宏勝，李海軍，孫 偉，蔣 榮

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

為滿足行人對獲取實時高精度位置、速度的需求，目前的行人導(dǎo)航系統(tǒng)普遍依賴衛(wèi)星提供導(dǎo)航、授時等服務(wù)。但是衛(wèi)星信號易受到干擾，且在室內(nèi)、隧道、叢林等環(huán)境下不能穩(wěn)定覆蓋，迫切需要行人導(dǎo)航系統(tǒng)具有不依賴衛(wèi)星的自主定位導(dǎo)航能力[1]。

慣性導(dǎo)航系統(tǒng)不受時間、電磁、地域等因素的限制，具有較高的自主性。光學捷聯(lián)慣導(dǎo)系統(tǒng)可實現(xiàn)高精度自主導(dǎo)航，然而其體積、重量較大，無法滿足可穿戴需求；基于微慣性測量單元（Miniature Inertial Measurement Unit,MIMU）的微慣性導(dǎo)航系統(tǒng)體積小、重量輕，是目前慣性導(dǎo)航技術(shù)領(lǐng)域研究熱點[2]。

行人自主導(dǎo)航系統(tǒng)目前普遍采用零速修正（Zero Velocity Update,ZUPT）算法修正速度、姿態(tài)誤差，以提高導(dǎo)航精度。然而ZUPT 僅在零速區(qū)間對載體作濾波修正，因此零速檢測的準確率直接決定了導(dǎo)航精度[3]。然而由于MIMU 存在精度低、噪聲大的問題，因此常規(guī)零速檢測算法存在大量的錯判現(xiàn)象，影響導(dǎo)航精度。為解決該問題，文獻[4]提出了一種基于支持向量機（Supporting Vector Machine,SVM）的自適應(yīng)零速檢測算法，可有效提高零速檢測的準確性和對復(fù)雜運動狀態(tài)的適應(yīng)性。

由于行人運動幅度大、狀態(tài)復(fù)雜，因此要求MIMU具有較大量程；為提高行人航向精度，又要求MIMU具有較高精度。目前MEMS 慣性器件的技術(shù)水平難以同時滿足上述需求。為保證量程滿足要求，目前MEMS 慣性器件普遍精度較低，導(dǎo)致系統(tǒng)定位精度較低，且航向誤差隨時間發(fā)散。然而ZUPT 算法中航向誤差不可觀測，不能被修正[2]。為抑制系統(tǒng)航向誤差，國內(nèi)外學者作了大量的研究。Chi Xu 等對視覺輔助的航向修正方法進行了研究，通過點線匹配的方式提升了載體的航向精度，然而當光線較弱，特征點不足時該方法將不再適用[5]。N.Kronenwett 等采用多傳感器（包括RGB-D 相機、視覺慣性SLAM 等）融合的方式，實現(xiàn)了對系統(tǒng)航向和位置進行修正，然而同樣受到各傳感器的限制[6]。陳昌浩等基于物聯(lián)網(wǎng)及深度神經(jīng)網(wǎng)絡(luò)學習的方法實現(xiàn)了基于低精度MIMU 設(shè)備的航向誤差抑制，然而該算法較為復(fù)雜，不易于工程實現(xiàn)[7]。鄧志紅等采用互補濾波器及啟發(fā)式濾波器的方法實現(xiàn)了對行人正常行走時航向誤差的修正，當劇烈運動時航向誤差抑制效果將變差[8]。綜上，抑制行人導(dǎo)航系統(tǒng)的航向誤差仍然存在較大的困難。

為提高行人導(dǎo)航系統(tǒng)的航向精度，本文提出一種基于自適應(yīng)零速檢測及雙MIMU 速度+角速率匹配的行人自主導(dǎo)航算法，在一套導(dǎo)航系統(tǒng)中同時包含兩種MIMU，一套大量程MIMU 用于保證數(shù)據(jù)完整性，一套高精度MIMU 用于提高系統(tǒng)航向精度。當高精度MIMU 數(shù)據(jù)有效時，通過卡爾曼濾波器（Kalman Filtering,KF）對大量程MIMU 航向陀螺漂移作修正，進而實現(xiàn)對航向誤差的抑制；采用基于SVM 的自適應(yīng)零速檢測算法確保零速檢測的準確性，當判斷足部速度為0 時，采用ZUPT 算法對系統(tǒng)速度、姿態(tài)誤差等進行修正，實現(xiàn)基于適應(yīng)零速檢測及雙MIMU 速度+角速率匹配的高精度行人自主導(dǎo)航。

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

1.1 行人導(dǎo)航系統(tǒng)原理

人在行走的過程中，足部的運動存在明顯的周期性變化，即步態(tài)周期。每個步態(tài)周期可分為4 個階段：離地、懸空擺動、落地、持續(xù)靜止[1]，如圖1 所示。其中在足部持續(xù)靜止的階段，足部速度恒定為0，該階段即為零速區(qū)間，可通過零速檢測算法利用足綁式MIMU 采集到的加速度、角速率的幅值、標準差等參數(shù)對當前是否為零速狀態(tài)進行檢測[9]。

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

若判斷當前為零速狀態(tài)時，通過KF 對當前足部的姿態(tài)、速度誤差等進行估計并修正；若判斷當前為非零速狀態(tài)，則不作修正，以僅通過慣性導(dǎo)航解算獲取系統(tǒng)的導(dǎo)航參數(shù)[9,10]?；赯UPT 的行人導(dǎo)航算法流程圖如圖2 所示。

圖2 基于ZUPT 的行人導(dǎo)航算法流程[4]Fig.2 The process of pedestrian navigation algorithm based on ZUPT[4]

1.2 濾波器設(shè)計

卡爾曼濾波（KF）是一種通過線性化系統(tǒng)的觀測量，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法[10]。線性化后行人自主導(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)為量測噪聲向量。在ZUPT 算法中，普遍取狀態(tài)變量：

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

量測量：

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

量測矩陣：

標準KF 方程如下[10]：

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

由KF 估計出各狀態(tài)變量的誤差后，對各狀態(tài)變量進行修正。由于ZUPT 算法中系統(tǒng)航向誤差及航向陀螺漂移不可觀測，故系統(tǒng)航向誤差不能被修正，導(dǎo)致系統(tǒng)航向誤差隨時間發(fā)散。

1.3 航向誤差的影響

在零速區(qū)間內(nèi)，系統(tǒng)的航向誤差近似滿足：

式中前兩項數(shù)值較小，即航向誤差的主要來源為航向陀螺（天向陀螺）漂移εu。通過在航向陀螺上人為加入誤差并進行仿真，得到航向誤差對系統(tǒng)的影響如圖3 所示。即航向誤差會使軌跡方向發(fā)生偏移，進而導(dǎo)致系統(tǒng)定位精度降低。

圖3 仿真得航向誤差對定位的影響Fig.3 The influence of heading error on positioning by s

2 自適應(yīng)零速檢測算法

目前常用的零速判據(jù)包括加速度閾值檢測、角速率閾值檢測、加速度標準差檢測、角速率標準差檢測等[4]。

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

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

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

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

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

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

上述各判據(jù)的閾值fth、ωth、σfth、σωth等常采用經(jīng)多次試驗確定的常數(shù)，對于固定人員的固定運動狀態(tài)（如正常行走）可以實現(xiàn)較高的準確率。然而，當運動狀態(tài)復(fù)雜時，會出現(xiàn)大量誤判和漏判的現(xiàn)象[4]。

為提高零速檢測準確率，本文采用基于SVM 的自適應(yīng)零速檢測算法，由濾波窗口內(nèi)三軸合加速度的峰值fp表征當前運動狀態(tài)，以fp與三軸角速率、加速度的矢量和、標準差等變量構(gòu)成樣本集：

其中，y表示f、ω、σf、σω等參數(shù)，zi為已有樣本點的狀態(tài)，zi=-1 或1，表示當前的足部運動狀態(tài)，zi=1表示該點為零速點，zi=-1 表示該點為非零速點。

采用SVM 算法對不同步態(tài)下零速點和非零速點進行分類，求解零速點和非零速點間的最優(yōu)超平面。將當前采樣點代入所求得超平面方程，即可作根據(jù)結(jié)果符號判斷當前是否為零速狀態(tài)。即得到零速檢測判據(jù)（決策函數(shù)）：

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

3.1 雙MIMU 空間結(jié)構(gòu)設(shè)計

人在運動時，足部角速率可達上千度每秒。為保證系統(tǒng)數(shù)據(jù)的完整性，需要在系統(tǒng)中包含一套上述量程的三軸陀螺儀（大量程陀螺儀），然而該陀螺儀精度僅能達到10 °/h 量級。

為提高該套陀螺儀的精度，需要一組精度較高的陀螺儀為其提供角速率基準，從而對其進行修正。其精度較大量程陀螺儀提升一個數(shù)量級，相應(yīng)地，量程減小一個數(shù)量級。

采用3 只大量程陀螺儀及加速度計構(gòu)成一套大量程MIMU；另采用3 只高精度陀螺儀構(gòu)成一套高精度MIMU。

為保證系統(tǒng)的小型化和可穿戴性，需要對導(dǎo)航系統(tǒng)內(nèi)部空間充分利用。為避免姿態(tài)轉(zhuǎn)換引入額外的誤差，需保證同一軸向的兩個陀螺儀的敏感軸近似重合。綜合考慮多種因素，系統(tǒng)內(nèi)部雙MIMU 的空間結(jié)構(gòu)設(shè)計如圖4 所示。

圖4 雙MIMU 空間結(jié)構(gòu)設(shè)計Fig.4 The space structure design for dual MIMUs

3.2 算法設(shè)計

由大量程MIMU 采集人體足部運動過程中的角速率和加速度，并全程采用慣性導(dǎo)航算法解算行人當前的姿態(tài)、速度、位置。

當通過零速檢測算法判斷當前為零速狀態(tài)時，此時系統(tǒng)的角速率也應(yīng)為0，高精度MIMU 不會超出量程?？赏瑫r取速度與雙MIMU 間角速率之差作為量測量，即采用速度+角速率匹配方案，通過KF 對系統(tǒng)所有可觀測的狀態(tài)變量進行估計并修正，從而實現(xiàn)對系統(tǒng)導(dǎo)航精度的提升。

當通過零速檢測算法判斷當前為非零速狀態(tài)時，若此時高精度MIMU 的三軸輸出均未超量程，則取雙MIMU 間的角速率之差為觀測量，即采用角速率匹配的方案，通過KF 對三軸的陀螺漂移進行估計并修正，從而實現(xiàn)對航向誤差的抑制。

當上述條件均不滿足時，以慣性導(dǎo)航得到的位置、姿態(tài)等作為最終輸出。

基于雙MIMU 速度+角速率匹配的行人導(dǎo)航算法流程圖如圖5 所示。

圖5 基于雙MIMU 速度+角速率匹配的行人導(dǎo)航算法流程Fig.5 The process of pedestrian navigation algorithm based on dual-MIMU velocity &angular rate matching method

3.3 濾波器設(shè)計

定義高精度MIMU系統(tǒng)所構(gòu)建的載體坐標系為m系，大量程MIMU 系統(tǒng)構(gòu)建的載體坐標系為b 系。兩套系統(tǒng)均取地理坐標系為導(dǎo)航坐標系。高精度MIMU到大量程MIMU 系統(tǒng)間的姿態(tài)角誤差為：

圖6 雙MIMU 間的空間模型Fig.6 The spatial model of dual MIMUs

由于m 系到b 系間的姿態(tài)誤差較小，因此可對φx、φy、φz作小角度假設(shè)，因此有從m 系到b 系的姿態(tài)轉(zhuǎn)換矩陣：

足部角速率分別在m 系和b 系的投影ωm、ωb間滿足：

即：

考慮到陀螺漂移與時間延遲的影響，實際上有：

其中，εb為大量程MIMU 的三軸陀螺漂移；εm為高精度MIMU 的三軸陀螺漂移；Tδ為高精度MIMU 相對于大量程MIMU 的時間延遲。由于高精度MIMU 的精度較高，近似認為其相對于大量程MIMU的陀螺漂移可忽略不計，式(19)可簡化為：

在引入高精度MIMU 系統(tǒng)后，又引入了4 個未知參數(shù)，分別為從m 系到b 系的安裝誤差φx、φy、φz及雙MIMU 間的時間延遲Tδ。因此需將KF 的狀態(tài)變量擴充為：

取量測量：

在非零速區(qū)間內(nèi)，當三軸高精度陀螺測量值均在量程范圍內(nèi)時，取量測矩陣：

在零速區(qū)間內(nèi)，?。?/p>

其中，（ωm×)為由ωm構(gòu)成的反對稱陣。

相應(yīng)地，還需對Pk、Qk和Rk陣的初始狀態(tài)P0、Q0和R0進行相應(yīng)地擴充。

3.4 系統(tǒng)可觀性分析

利用奇異值分解理論分析系統(tǒng)可觀測度。分別采集導(dǎo)航系統(tǒng)靜止朝北（航向角為0 °）和靜止朝西（航向角為90 °）時雙MIMU 輸出的角速率、加速度，并通過導(dǎo)航解算得到北向、天向、東向速度，與雙MIMU間角速率之差共同作為觀測量，對系統(tǒng)進行可觀性分析。

由于在零速區(qū)間速度為0，速度誤差絕對可觀，因此令量測量δVn、δVu、δVe的可觀測度為1，得到引入角速率匹配前后系統(tǒng)狀態(tài)變量的可觀測度見表1。

由表1 得在引入角速率匹配后，系統(tǒng)y軸陀螺漂移的可觀測度明顯提高，由原來的5.45×10-4提高至0.98，由不可觀測量變?yōu)榭捎^測量。而在人的行走過程中，y軸陀螺又長期近似指天，為航向誤差的主要來源，因此可通過對系統(tǒng)三軸陀螺漂移進行補償，來實現(xiàn)對航向誤差的抑制。

表1 各狀態(tài)變量的可觀測度Tab.1 The degree of observability for state varibles

4 試驗驗證

4.1 試驗內(nèi)容

將一套基于雙MIMU 速度+角速率匹配的行人導(dǎo)航系統(tǒng)固定在足部，使系統(tǒng)的x軸與人體行進方向一致；y軸指天；z軸與人體行進方向垂直并指向行進方向的右側(cè)。確保系統(tǒng)樣機相對于足部固定，不會出現(xiàn)滑動、脫落等現(xiàn)象[4]。在系統(tǒng)開始工作前，保持足部靜止并對系統(tǒng)初始位置、航向等進行裝訂，系統(tǒng)進入導(dǎo)航狀態(tài)后開始行走。行走30 min 左右后回到初始位置，并通過機械手段保證與初始方位一致性誤差小于0.5 °。

重復(fù)進行多次試驗，每條次試驗中同時利用雙MIMU 進行導(dǎo)航解算可得到基于雙MIMU 速度+角速率匹配的導(dǎo)航數(shù)據(jù)，利用大量程MIMU 可得到僅采用速度匹配的導(dǎo)航數(shù)據(jù)。統(tǒng)計兩種方案終止時刻與起始時刻系統(tǒng)航向的差值的絕對值，即為系統(tǒng)的航向誤差，取所有條次中航向誤差的最大值作為當前系統(tǒng)的航向誤差。

采用全球定位系統(tǒng)（Global Positioning System,GPS）對起始點及各轉(zhuǎn)彎處的經(jīng)緯度進行多次測量，得到其準確位置，作為定位誤差計算的基準點。由于每次行走的路線不同，因此基準點依據(jù)實際行走的路線選取。統(tǒng)計各轉(zhuǎn)彎處由導(dǎo)航系統(tǒng)與GPS 得到的位置間的距離即為經(jīng)過該點時的位置誤差，取每條次位置誤差的最大值作為該條次的定位誤差，取所有條次中定位誤差的最大值作為當前系統(tǒng)的定位誤差。

4.2 試驗結(jié)果

采用基于SVM 的自適應(yīng)零速檢測算法對零速區(qū)間進行檢測，采用雙MIMU 速度+角速率匹配與僅采用單MIMU 速度匹配時得到的部分導(dǎo)航軌跡如圖7 所示。由于在實際行走時，大部分時間段內(nèi)大致沿著正東、正南、正西或正北方向，因此運動軌跡應(yīng)大致與坐標軸平行。圖中基于雙MIMU 的行人導(dǎo)航系統(tǒng)的軌跡更趨近于實際情況，且各轉(zhuǎn)彎處與基準點間的間距較小，說明基于雙MIMU 的行人導(dǎo)航系統(tǒng)具有更高的航向及定位精度。

圖7 采用不同算法時的部分導(dǎo)航軌跡Fig.7 Fragments of the navigation trajectories when using different algorithms

分別統(tǒng)計采用雙MIMU 速度+角速率匹配及僅采用單MIMU 速度匹配時不同條次的航向誤差與定位誤差，統(tǒng)計結(jié)果見表2。

表2 系統(tǒng)的航向與定位誤差Tab.2 The heading and positioning errors of system

在其他條件完全相同的條件下，在30 min 內(nèi)，采用雙MIMU 速度+角速率匹配的行人導(dǎo)航系統(tǒng)的航向誤差不大于3.44 °，定位誤差最大為4.92 m，均達到了較高的水平；而采用速度匹配方案時系統(tǒng)的航向誤差最大為7.30 °，定位誤差最大為9.62 m。即相較于采用速度匹配，基于雙MIMU 速度+角速率匹配的行人導(dǎo)航系統(tǒng)的航向精度提升了52.9%，定位精度提升了48.9%。所設(shè)計的基于雙MIMU 速度+角速率匹配的行人導(dǎo)航系統(tǒng)可有效抑制系統(tǒng)的航向誤差，實現(xiàn)高精度的行人自主導(dǎo)航。

5 結(jié)論

本文對基于雙MIMU 速度+角速率匹配的行人自主導(dǎo)航系統(tǒng)的結(jié)構(gòu)及算法進行了設(shè)計。采用SVM 算法對不同運動狀態(tài)下的零速點和非零速點分別進行分類，可提高零速檢測算法的準確率和對不同運動狀態(tài)的適應(yīng)性。在一套行人導(dǎo)航系統(tǒng)中同時包含2 套三軸MIMU，一套具有較大量程但精度較低，用于采集人體運動時足部的角速率和加速度，用以保證數(shù)據(jù)的完整性；一套具有較高精度但量程較小，用于為大量程MIMU 提供角速率基準。若判斷當前為非零速狀態(tài)，且三軸高精度陀螺儀均未超量程時，采用角速率匹配方案提升大量程MIMU 三軸陀螺漂移的可觀測度，通過KF 對其進行估計并修正；若判斷當前為零速狀態(tài)，采用速度+角速率匹配方案，通過KF 對所有的可觀測變量進行修正，從而實現(xiàn)對系統(tǒng)航向誤差的抑制。

經(jīng)試驗驗證，基于雙MIMU 速度+角速率匹配的行人自主導(dǎo)航系統(tǒng)的航向精度達到了3.44 °/30 min，定位精度達到了4.92 m/30 min，較僅采用單MIMU 速度匹配時精度有了很大的提升，航向精度提升了52.9%，定位精度提升了48.9%，且導(dǎo)航軌跡更接近實際情況。所設(shè)計基于雙MIMU 速度+角速率匹配的行人自主導(dǎo)航系統(tǒng)可有效抑制航向誤差隨時間發(fā)散，進而提升系統(tǒng)定位精度，具有一定的實際應(yīng)用價值。