自適應步長約束的雙腳MIMU行人導航系統誤差修正方法

時 偉，汪宇陽，舒靜青

（中南大學航空航天學院，長沙 410083）

行人導航系統（Pedestrian Navigation System，PNS），是指個體依靠自身攜帶的傳感器設備實現高精度導航定位，在陌生的環境實時地提供自身位置信息的技術[1]。室外場景下，導航定位需求大多通過現已基本成熟的全球定位系統（Global Positioning System,GPS）得到滿足，而對于衛星信號遮擋嚴重的室內環境，得益于近年來微電子機械系統技術（Micro Electro Mechanical Systems, MEMS）的發展，基于微慣性測量單元（Micro-Inertial Measurement Unit, MIMU）的行人導航成為主流的研究方向[2]。

MIMU具有對環境設施依賴程度低、自主性強、不易受外界干擾、安全隱蔽等突出優勢，面對復雜的室內環境，能夠連續穩定地為使用者提供姿態和位置信息，不需依賴外部設施，尤其適合環境障礙物不定的火災場景，是滿足消防人員火場定位需求的理想器件[3]。然而，基于MIMU的行人導航缺陷也很明顯：低成本的MEMS器件自身精度有限，在無有效補償措施的情況下存在嚴重漂移誤差，尤其在長距離行走過程中，由慣性解算算法得到的導航結果隨時間的積累發散問題尤為嚴重[4]。針對上述問題，國內外研究人員利用行人足部運動規律提出了多種思路對誤差進行補償修正：李佳璇等[5]將兩個MIMU分別設置在單足足尖和足跟，利用其間的固定距離構建等式約束；Jao C S等[6]則將兩個MIMU分別設置在雙足，利用攝像頭獲得足間相對位置測量輔助零速修正（Zero Velocity Update, ZUPT）展開修正；傅溫慧等[7]則利用雙足運動軌跡近似平行且存在最大垂直距離的特征對誤差累積進行限制。

本文將兩個MIMU分別固定在行人雙腳之上，提出一種基于零速檢測與步長約束的雙MIMU行人導航算法。首先檢測雙足的靜止狀態，通過零速修正算法在零速區間實現初步的誤差修正；在此基礎上充分利用行人步態特征和運動學特性，利用垂直方向加速度推算每一步步長，結合最大抬腳高度建立隨行人步態自適應變化的步長約束，通過不等式約束卡爾曼濾波進一步地校正軌跡漂移，提高導航精度；最后通過仿真實驗驗證了所提算法的有效性。

1 雙MIMU橢球約束誤差修正

1.1 不等式約束卡爾曼濾波

經典離散型卡爾曼濾波模型[8]為：

其中，式(1)為系統狀態方程，式(2)為量測方程，Xk表示系統狀態向量，Φk,k-1表示從(k-1)時刻到k時刻的狀態轉移矩陣，Γk-1為系統噪聲驅動陣；Wk和Vk分別為系統激勵噪聲序列和量測噪聲序列，且同時滿足以下條件：

離散型卡爾曼濾波的基本方程如下：

實際應用中，離散型卡爾曼濾波的系統狀態量之間常常滿足某些已知的不等關系，如若將這些先驗的不等關系轉化為約束方程，與卡爾曼濾波相結合構成不等式約束卡爾曼濾波進行狀態估計，便能夠實現對已知信息的充分利用，從而優化濾波性能，得到最優解[9]。將不等式約束方程表示為Dx≤d，矩陣D表示狀態量滿足的約束條件，采用投影法計算滿足約束方程的最優解，問題可以描述為：

式中，W為對稱正定的投影矩陣，一般取W=（Pk）-1，滿足問題描述的即為不等式約束卡爾曼濾波的最優估計，運用拉格朗日乘子求解得：

1.2 雙腳MIMU橢球約束

定義k時刻左、右腳MIMU慣性導航系統的狀態向量分別為狀態矢量包含三軸的位置、速度、姿態信息，即：定義k時刻雙腳MIMU聯合狀態向量：

將兩個MIMU分別固定在雙腳之上，觀察人體肢體結構易知，行人步行過程中雙腳之間距離的最大值會受到其邁步長度的限制，同時，雙腳在垂直方向上的相對高度也會因受抬腳高度限制而存在一個極限值。利用邁步長度及抬腳高度對于雙腳間距的約束構建不等式約束算法，能夠充分利用系統狀態量之間已知的關系，實現對雙MIMU系統導航誤差的修正[10]。

圖1所示為左腳靜止、右腳邁步狀態下的行人雙腳約束模型示意圖，將邁步長度記為γ，抬腳高度的最大值記為H，將左右腳MIMU在水平面上距離差值記為Δd。則在步行過程中任意時刻，由于邁步長度的制約，Δd必定滿足不等式Δdγ≤ ，即在xoy平面內雙腳相對位置被約束在一個以γ為半徑的圓中；類似地，行人雙腳在垂直方向上的高度差受到最大抬腳高度的制約，即雙腳在xoz、yoz平面被約束在一個以γ為長半軸、以h為短半軸的橢圓內。其中，h是一個與抬腳高度H相關的極限值。因此，邁步時雙腳位置約束在空間內構成一個半橢球形（z＞0）。

圖1 行人步長約束模型示意圖Fig.1 Schematic diagram of step-length constraint model

設k時刻左右腳MIMU的位置坐標分別為：對前文所述步長約束進行正交分解，校正水平面xoy內雙腳位置：如圖1所示，以左腳落地、右腳擺動的狀態為例，以落地腳所在位置O點為中心，將擺動腳所在位置向xoy平面投影，將投影點與O點的連線同x軸正方向間夾角記為α，則有：

將擺動腳所在位置與O點的連線同水平面所成線面角記為β，則通過β可以在垂直方向上實現隨步態變化而改變的高度約束，提高誤差修正的有效性，β表達式為：

聯合狀態向量xk滿足不等式約束模型：

式中，

假設k時刻雙腳聯合狀態在橢球模型所約束的范圍之外，即：則可以通過投影法[11]將聯合狀態約束到子空間：求解不等式卡爾曼濾波最優解，得：

2 自適應步長橢球約束誤差修正

2.1 總體方案設計

本研究中將兩個MIMU分別固定在雙腳之上，基于人體運動學特征，利用邁步長度和抬腳高度對雙腳間距的限制構建不等式約束模型，輔助卡爾曼濾波算法，從而在對雙腳MIMU測量單元分別零速修正的基礎上，進一步改善導航誤差修正效果。

在實際步行運動過程中，邁步長度往往會受步行速度、步伐頻率、行人身體狀態等因素影響發生改變，而不是一個固定值。一般來說，變化規律是步行速度越快、步伐頻率越高，步長也就相應地越大[12]?？紤]到這一特點，本研究根據雙腳MIMU零速檢測的結果分離步態周期，在每個周期內利用非線性模型推算步長，使雙腳不等式約束模型中的步長實時地更新，從而實現約束模型的自適應變化?？傮w方案設計如圖2所示。

圖2 總體方案設計框圖Fig.2 Overall design block diagram

2.2 零速檢測

步行運動看似復雜，但實際上存在一定的規律性。步行過程中，行人的雙腳步態會發生周期性變化，可以歸納為腳跟落地、全腳落地、腳跟離地、全腳離地四個階段的循環反復。

如圖3所示，將腳部一次落地到下一次落地之間所經歷的過程稱為一個步態周期[13]。在一個步態周期內，將單腳在空中擺動的階段稱為擺動區間、單腳接觸地面處于靜止狀態的階段稱為零速區間，則步行運動即為擺動區間與零速區間交替出現的過程。足綁式行人導航系統通常利用零速區間的特性，采用ZUPT算法校正慣性傳感器的累積誤差[14,15]。

圖3 行人步態周期示意圖Fig.3 Schematic diagram of pedestrian gait cycle

零速區間內腳部應處于靜止狀態，速度為零，加速度則應等于重力加速度。零速修正算法的基本思想就是利用這一特性，將零速區間解算的速度誤差作為觀測值，送入卡爾曼濾波器進行估計，對導航誤差周期性重置。ZUPT的修正效果很大程度上決定于對步行過程中零速區間檢測的準確度。Skog等[16]在基于足部MIMU的項目Openshoe中將常用零速修正方法分為基于加速度幅值（MAG）、基于加速度方差（MV）、基于角速度能量（ARE）和廣義似然比檢測（GLRT）四種。考慮到單一數據來源檢測方式在可靠性上的局限，本文采用廣義似然比檢測方法，綜合加速度計和陀螺儀的輸出信息進行檢測。

定義MIMU輸出為xk=[akωk]T，ak表示k時刻加速度計測得的比力信息，ωk表示k時刻陀螺儀測得的角速度值。GLRT零速檢測方法可以理解為一個二元的假設檢驗，假設H0：靜止，即處于零速區間；假設H1：運動，即處于擺動區間。根據Neyman-Pearson理論，虛警概率也即錯誤判決概率為PF=P{H0|H1}，檢測概率也即正確判決概率為PD=P{H0|H0}，GLRT表達式：

式中，N為采樣數，表示采樣滑動窗口長度，表示測得比力值的平均值；g為重力加速度，σa、σω分別表示加速度計和陀螺儀的測量噪聲標準差。當k時刻MIMU的輸出數據ak、ωk在滑動窗口內滿足式(19)時，判斷此時檢測到腳部處于零速區間。判別式如式(20)，λ為GLRT檢測閾值。

通過卡爾曼濾波對雙腳MIMU的慣性解算分別進行零速修正，觀測方程和量測方程分別為：

狀態向量取15維誤差向量：δX=[δpTδvTδφTδaTδωT]T，依次表示位置誤差、速度誤差、姿態誤差、加速度計誤差和陀螺漂移；Fk為狀態轉移矩陣：

Hk=[03x3I3x303x303x303x3]為量測矩陣；wk-1、vk分別為過程噪聲和量測噪聲矩陣。

2.3 動態步長估算

GLRT對零速區間的檢測總體準確，但實際測量中由于腳部的抖動等因素，仍有可能存在一些誤檢測點。根據行人步行規律，在正常行走狀態下，一個步態周期內零速區間占比約為30%～40%。設步態周期為T，0T為該周期內零速區間長度，則對于整個步行過程中任意一步，必滿足：

圖4 GLRT零速檢測結果示意圖Fig.4 Schematic diagram of GLRT zero velocity detection

研究結果表明行人在一個步態周期內的行走距離與加速度大小存在很大相關性。利用加速度測量值估算步長的方法主要分為查表、線性模型、非線性模型和神經網絡四類，綜合考慮估計準確度和計算復雜度，本文采用基于非線性模型的Weinberg步長估計算法[17]，利用一個步態周期內邁步長度與垂直方向加速度的相關性進行估算。Weinberg估算公式如下：

式中，k指第k個步態周期，表示第k個步態周期內垂直方向加速度的最大值，類似地，指垂直方向加速度的最小值，K為Weinberg模型的估算系數。

通常意義上，步長是指行人在邁出一步時左右腳之間的長度，也就是異側腳跟先后落地點位之間的距離，稱為單步（Stride），本文提出的步長約束方法所針對的步長就是基于單步而展開研究的。而通過式(26)估算出的Step(k)是指行人在第k個步態周期內行進的長度，也就是同側腳跟先后落地點位之間的距離，由兩個連續的單步組成，稱其為一個復步。分析單步與復步的關系，得到對應單步步長計算公式：

其中，d是雙腳之間的距離差值，實際測量過程中根據左右腳MIMU傳感器的安裝位置適當取值。根據2.2節分析，步長橢球約束模型為式(18)。在此基礎上，利用零速檢測識別步態，非線性模型估算實時步長，進而實現雙腳約束隨測量對象步長改變而動態變化。將第i步步長記為iγ，代入式(18)，替換固定的步長約束值γ，即有：

其中，t∈ (ti,s,ti,e),0＜i＜n。

3 實驗驗證

3.1 實驗條件設置

針對上述基于雙腳MIMU的自適應步長約束誤差修正方法設計驗證實驗。MIMU器件采用荷蘭Xsens公司生產的MTi-G-710系列微慣性測量單元，實時測量加速度和角速度，采樣頻率為400 Hz。

實驗在中南大學升華后樓樓內走廊進行，通過設置地面標識的方式確保實驗者能夠近似按照設定的步長行走，誤差在5 cm以內。將兩個MIMU分別固定在實驗者左右腳背，保證其在步行過程中不會松脫滑動?；谛腥苏Ｐ凶吡晳T和生理特征，抬腳高度極值取值H=0.15 m；步長系數K首先取隨意值，在實驗過程中通過比較模型估算步長與給定步長進行取值調整；GLRT零速檢測閾值取值λ=1.0×105。實驗者以原點為初始位置，做近似勻速的正常步行；在每次實驗起始與結束前，保持靜止狀態3 s，以便提取運動數據。

3.2 實驗結果與分析

實驗（1） 實驗者從初始位置以給定步長做近似勻速直線行走。首先以步長0.7 m行走約24.5 m，過程歷時約34.23 s；隨后，從初始位置再次出發，以步長0.5 m行走約25 m，過程歷時約43.75 s。兩次步長估算情況以及分別在無約束、γ= 0.7的固定步長約束、本文所提出的自適應步長約束處理后得到的左右腳軌跡如圖5，約束前后左右腳高度差如圖6所示。

圖5 實驗(1)給定步長0.7 m時的結果Fig.5 The results of experiment (1) with a given step length of 0.7 m

圖6 實驗(1)給定步長0.5 m的結果Fig.6 The results of experiment (1) with a given step length of 0.5 m

分析步長推算數據可得，本文方法推算步長均值約為0.66 m和0.53 m，與給定步長0.7 m和0.5 m間誤差率在5.71%以下，能夠較準確地估算步長。對比分析圖5、圖6兩組圖像，無約束情況下，雙腳隨時間推進逐漸發散，xoy平面內距離差越來越大，有悖人體結構特征。固定步長實驗選取步長γ=0.7 m，在給定步長為0.7 m時，因其取值恰好與γ相契合，可以看出固定步長實驗與自適應步長實驗的表現效果大致相當，誤差的時間累積得到較好補償，圖線符合真實雙腳軌跡；而在給定步長為0.5 m時，固定步長約束γ仍為定值0.7 m，與實驗者步長不匹配，隨著時間的推進，雙腳間距離出現發散；而采用本文自適應步長約束方法后，在步長0.5 m和0.7m的情況下軌跡發散現象都能夠得到有效抑制，左右腳軌跡間距符合實際情況，導航定位準確度有效提高。分析圖5、圖6可知，通過加入本文所提出的約束，雙腳高度差隨時間發散的趨勢得到控制，高度差圖線呈現在零值附近波動的特點，符合實際行走特征和抬腳高度限制。

實驗（2） 實驗者從初始位置出發以變化的給定步長作近似勻速直線運動。首先以0.5 m步長行走14 m，而后轉變步長為0.7 m行走9.7 m，實驗全程歷時約44.32 s。步長估算、左右腳軌跡及約束前后雙腳高度差對比如圖7。

圖7 實驗(2)步長估計圖及雙腳軌跡圖Fig.7 Step-length estimation and feet trajectory in experiment (2)

觀察圖7(a)可以發現，本文所提方法能夠對邁步長度發生變化的情況作出反應，較準確地估算變化的步長，前后兩段的估算值與給定固定步長之間的誤差率均在5.8%以下，步長估算模型可靠。對比分析軌跡圖7(b)(c)(d)，對于行人步長發生改變的情況，相比固定步長橢球約束，本文所提出的自適應步長方法能夠良好地適應變化，有效改善雙腳軌跡的漂移現象，提高導航精度。分析約束前后雙腳高度差圖7(e)(f)，可以發現在無約束情況下，雙腳位置的高度差值隨著時間的推移越來越大，逐漸脫離正常抬腳高度的范圍；而采用本文方法修正后，雙腳高度差曲線顯著收斂，大體上呈現在零值附近波動的趨勢，符合實際行走的特點。

實驗（3） 實驗者從初始位置出發沿C型走廊近似勻速運動，實驗過程中不事先給定步長，令實驗者正常勻速行走。首先直線行走4.8 m，隨后逆時針旋轉90 °繼續直線行走5.6 m，接著再次逆時針旋轉90 °直線行走4.8 m，全程歷時約33.54 s。經測量初始位置與最終位置之間距離約為5.6 m，將始末位置距離差作為判斷定位效果的依據。左右腳軌跡如圖8，定位結果對比如表1所示。

圖8 實驗(3)雙腳軌跡圖Fig.8 Feet trajectory in experiment(3)

通過圖8可以看出，無約束情況下雙腳軌跡發散嚴重；在固定步長橢球約束算法修正后，軌跡發散有一定緩解，但行走到末位置時，左右腳在xoy平面內距離已超0.7 m，顯然不符合實際；采用本文所提自適應步長橢球約束方法修正后，雙腳軌跡漂移得到改善，更符合真實的雙腳間關系。分析表1可知，采用本文方法處理后，相比固定步長約束算法，行人始末位置距離誤差顯著減小，左右腳位置平均誤差下降了約46.43%，行人導航定位精度有效提高。

表1 實驗（3）定位結果Tab.1 Positioning result of experiment(3)

4 結 論

本文研究了基于自適應步長約束的雙腳MIMU行人導航系統的誤差修正方法，自適應地對雙腳MIMU的位置約束不等式進行動態調節。經過步行實驗驗證，所提方法在步長發生變化的情況下表現出良好的誤差修正效果，提高了GPS信號拒止環境下室內行人導航的精度。