自適應(yīng)步長約束的雙腳MIMU行人導(dǎo)航系統(tǒng)誤差修正方法

時 偉，汪宇陽，舒靜青

（中南大學(xué)航空航天學(xué)院，長沙 410083）

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

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

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

1 雙MIMU橢球約束誤差修正

1.1 不等式約束卡爾曼濾波

經(jīng)典離散型卡爾曼濾波模型[8]為：

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

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

實際應(yīng)用中，離散型卡爾曼濾波的系統(tǒng)狀態(tài)量之間常常滿足某些已知的不等關(guān)系，如若將這些先驗的不等關(guān)系轉(zhuǎn)化為約束方程，與卡爾曼濾波相結(jié)合構(gòu)成不等式約束卡爾曼濾波進行狀態(tài)估計，便能夠?qū)崿F(xiàn)對已知信息的充分利用，從而優(yōu)化濾波性能，得到最優(yōu)解[9]。將不等式約束方程表示為Dx≤d，矩陣D表示狀態(tài)量滿足的約束條件，采用投影法計算滿足約束方程的最優(yōu)解，問題可以描述為：

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

1.2 雙腳MIMU橢球約束

定義k時刻左、右腳MIMU慣性導(dǎo)航系統(tǒng)的狀態(tài)向量分別為狀態(tài)矢量包含三軸的位置、速度、姿態(tài)信息，即：定義k時刻雙腳MIMU聯(lián)合狀態(tài)向量：

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

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

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

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

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

聯(lián)合狀態(tài)向量xk滿足不等式約束模型：

式中，

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

2 自適應(yīng)步長橢球約束誤差修正

2.1 總體方案設(shè)計

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

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

圖2 總體方案設(shè)計框圖Fig.2 Overall design block diagram

2.2 零速檢測

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

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

圖3 行人步態(tài)周期示意圖Fig.3 Schematic diagram of pedestrian gait cycle

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

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

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

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

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

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

2.3 動態(tài)步長估算

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

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

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

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

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

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

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

3 實驗驗證

3.1 實驗條件設(shè)置

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

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

3.2 實驗結(jié)果與分析

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

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

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

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

實驗（2） 實驗者從初始位置出發(fā)以變化的給定步長作近似勻速直線運動。首先以0.5 m步長行走14 m，而后轉(zhuǎn)變步長為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)可以發(fā)現(xiàn)，本文所提方法能夠?qū)~步長度發(fā)生變化的情況作出反應(yīng)，較準確地估算變化的步長，前后兩段的估算值與給定固定步長之間的誤差率均在5.8%以下，步長估算模型可靠。對比分析軌跡圖7(b)(c)(d)，對于行人步長發(fā)生改變的情況，相比固定步長橢球約束，本文所提出的自適應(yīng)步長方法能夠良好地適應(yīng)變化，有效改善雙腳軌跡的漂移現(xiàn)象，提高導(dǎo)航精度。分析約束前后雙腳高度差圖7(e)(f)，可以發(fā)現(xiàn)在無約束情況下，雙腳位置的高度差值隨著時間的推移越來越大，逐漸脫離正常抬腳高度的范圍；而采用本文方法修正后，雙腳高度差曲線顯著收斂，大體上呈現(xiàn)在零值附近波動的趨勢，符合實際行走的特點。

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

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

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

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

4 結(jié) 論

本文研究了基于自適應(yīng)步長約束的雙腳MIMU行人導(dǎo)航系統(tǒng)的誤差修正方法，自適應(yīng)地對雙腳MIMU的位置約束不等式進行動態(tài)調(diào)節(jié)。經(jīng)過步行實驗驗證，所提方法在步長發(fā)生變化的情況下表現(xiàn)出良好的誤差修正效果，提高了GPS信號拒止環(huán)境下室內(nèi)行人導(dǎo)航的精度。