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

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

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

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

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

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

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

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

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

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

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

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

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

1.2 濾波器設(shè)計(jì)

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

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

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

量測(cè)量：

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

量測(cè)矩陣：

標(biāo)準(zhǔn)KF 方程如下[10]：

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

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

1.3 航向誤差的影響

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

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

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

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

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

（1）加速度、角速率閾值檢測(cè)

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

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

（2）加速度、角速率標(biāo)準(zhǔn)差檢測(cè)

若三軸加速度、角速率的標(biāo)準(zhǔn)差分別滿足：

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

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

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

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

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

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

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

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

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

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

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

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

3.2 算法設(shè)計(jì)

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

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

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

當(dāng)上述條件均不滿足時(shí)，以慣性導(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è)計(jì)

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

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

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

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

即：

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

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

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

取量測(cè)量：

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

在零速區(qū)間內(nèi)，取：

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

相應(yīng)地，還需對(duì)Pk、Qk和Rk陣的初始狀態(tài)P0、Q0和R0進(jìn)行相應(yīng)地?cái)U(kuò)充。

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

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

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

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

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

4 試驗(yàn)驗(yàn)證

4.1 試驗(yàn)內(nèi)容

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

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

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

4.2 試驗(yàn)結(jié)果

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

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

分別統(tǒng)計(jì)采用雙MIMU 速度+角速率匹配及僅采用單MIMU 速度匹配時(shí)不同條次的航向誤差與定位誤差，統(tǒng)計(jì)結(jié)果見(jiàn)表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，均達(dá)到了較高的水平；而采用速度匹配方案時(shí)系統(tǒng)的航向誤差最大為7.30 °，定位誤差最大為9.62 m。即相較于采用速度匹配，基于雙MIMU 速度+角速率匹配的行人導(dǎo)航系統(tǒng)的航向精度提升了52.9%，定位精度提升了48.9%。所設(shè)計(jì)的基于雙MIMU 速度+角速率匹配的行人導(dǎo)航系統(tǒng)可有效抑制系統(tǒng)的航向誤差，實(shí)現(xiàn)高精度的行人自主導(dǎo)航。

5 結(jié)論

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

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