傅溫慧 伍萍輝 曾成 花中秋

摘要 基于MIMU的行人導(dǎo)航系統(tǒng)受慣性器件固有屬性的影響，導(dǎo)航誤差會隨著時間累積而發(fā)散，為提高行人慣性導(dǎo)航系統(tǒng)的精度，提出了一種基于雙MIMU的行人導(dǎo)航算法。將2套MIMU分別固聯(lián)在行人的左右腳上，先對單足進行零速檢測和零速修正，再根據(jù)行人雙足的運動特征，設(shè)計了一種行人雙足軌跡間距離約束的算法。利用行人正常行走時左腳軌跡和右腳軌跡間距不超過某個最大值，構(gòu)造不等式約束的卡爾曼濾波器，對零速修正后的行人位置進行二次校正。實驗結(jié)果表明，采用行人雙足軌跡間最大距離約束算法，位置誤差比約束前平均降低了39.14%。

關(guān) 鍵 詞 慣性導(dǎo)航;MIMU;零速修正;不等式約束;卡爾曼濾波

中圖分類號 TN966? ? ?文獻標(biāo)志碼 A

Abstract The MIMU-based pedestrian navigation system is affected by the inherent properties of the inertial device， and the navigation error will accumulate over time. To improve the accuracy of the pedestrian inertial navigation system， a pedestrian navigation algorithm based on dual MIMU is proposed. The two MIMUs are fixed on the left and right feet of the pedestrian respectively. Firstly， the zero-velocity detection and zero-velocity updates are performed on the single foot. Then， according to the motion characteristics of the pedestrian's feet， an algorithm for the distance constraint between the pedestrian and the foot is designed. When the pedestrian is walking normally， the distance between the left foot track and the right foot track does not exceed a certain maximum value， and the Kalman filter with inequality constraint is constructed to perform secondary correction on the pedestrian position after zero-velocity updates. The experimental results show that the position error is reduced by 39.14% on average before the constraint by using the maximum distance constraint algorithm between pedestrian bipeds.

Key words inertial navigation;? MIMU;? zero-velocity update;? inequality constraints;? Kalman filter

0 引言

隨著科技的發(fā)展，全球定位系統(tǒng)（Global Positioning System，GPS）在信號良好的室外環(huán)境可以達到很高的定位精度，被廣泛應(yīng)用于眾多領(lǐng)域。但是對于內(nèi)部結(jié)構(gòu)復(fù)雜的室內(nèi)環(huán)境，GPS不能達到令人滿意的定位效果。所以需要一種可以提供室內(nèi)精確位置的定位技術(shù)來彌補這一不足。目前，國內(nèi)外已經(jīng)研究出多種室內(nèi)定位技術(shù)，其中基于微機電系統(tǒng)（Micro-Electro-Mechanical System，MEMS）的慣性導(dǎo)航便是其中的一種。

基于MEMS的微慣性測量單元（Micro Inertial Measurement Unit，MIMU）具有隱蔽性強、抗干擾性強、體積小、耗能低、環(huán)境適應(yīng)性強等優(yōu)點[1-3]，可以用作行人室內(nèi)導(dǎo)航的核心器件。但是基于MEMS慣性傳感器的導(dǎo)航方案存在誤差隨時間累積而發(fā)散[4-7]的缺點，定位精度會隨誤差的累積迅速降低。因此需要利用MIMU得到的慣性信息和解算的姿態(tài)信息等對誤差進行補償。零速修正技術(shù)[8]是一種簡單有效的誤差補償技術(shù)，利用慣性器件的零速作為觀測量來修正系統(tǒng)誤差。在行人慣性導(dǎo)航中MIMU多采用足綁式，將腳接觸地面時作為零速來對載體的其他信息進行修正。由于天向陀螺的可觀測性較低[3]，零速修正技術(shù)無法補償行人在導(dǎo)航過程中的航向漂移[9-10]，容易造成航向發(fā)散。

針對這些問題國內(nèi)外學(xué)者進行了多種方案的嘗試，Prateek等[8]提出了一種雙MIMU行人慣性導(dǎo)航方案，將2套MIMU分別綁在左右腳上，利用行人步行中兩腳之間的距離對解算結(jié)果進行約束，起到了很好的效果?？▋?nèi)基梅隆大學(xué)對雙足方案進行了深入研究，實驗達到了很高的定位精度。哈爾濱工程大學(xué)的李佳璇等[1]提出了一種單足雙MIMU的行人導(dǎo)航方案，將2套MIMU固定在同一只腳上，兩套MIMU之間的距離是不變的，在解算中利用MIMU之間的固定距離對結(jié)果進行約束，起到了提高精度的作用。為提高行人導(dǎo)航的位置精度，抑制行人長時間運動中的航向漂移，本文采用雙MIMU的行人導(dǎo)航方案，提出了一種將行人雙足軌跡間距離作為約束條件，利用不等式卡爾曼濾波校正位置誤差的算法，并進行了約束前后的實驗對比，驗證了該算法的有效性。

1 行人雙足運動軌跡分析

行人在正常行走時，左右腳交替前進，左腳軌跡與右腳軌跡近似平行，兩條軌跡之間的距離應(yīng)小于某個最大值。例如在行人導(dǎo)航解算中，右腳位置在水平面上的投影到左腳軌跡的垂直距離不會大于這個最大值。如圖1a）中虛線為雙足軌跡在水平面上的投影，黑點為落腳點的位置，為方便研究左右腳運動軌跡的關(guān)系，將軌跡近似為折線，如圖1a）中實線段所示。其中一步如圖1b）所示，AB為左腳軌跡，A點為前一個落腳點位置，B點為當(dāng)前落腳點位置;CD為估計的右腳軌跡，C點為右腳前一落腳點的位置，E點為當(dāng)前時刻右腳位置在水平面上的投影。E點到直線AB的距離為d，即雙足軌跡距離，在行人正常行走過程中雙足軌跡距離d不超過某個最大值λ。在解算時就可以利用這一約束關(guān)系對導(dǎo)航估計狀態(tài)進行二次修正，抑制系統(tǒng)的航向漂移，提高位置精度。

2 雙足軌跡間最大距離約束算法

卡爾曼濾波是慣性導(dǎo)航解算中常用的一種狀態(tài)估計方法[10]，利用前一時刻的狀態(tài)估計值和現(xiàn)在時刻的狀態(tài)觀測值來估計當(dāng)前時刻的狀態(tài)。簡單來說，卡爾曼濾波是一種最優(yōu)化自回歸數(shù)據(jù)處理算法，具有很高的運算效率。本文根據(jù)雙足軌跡間的距離關(guān)系，構(gòu)造約束方程，利用不等式約束的卡爾曼濾波[4]的方法來實現(xiàn)雙足軌跡間最大距離約束算法。

2.1 經(jīng)典離散型卡爾曼濾波

2.2 不等式約束卡爾曼濾波

在實際應(yīng)用中，狀態(tài)變量會受到現(xiàn)實環(huán)境、實驗方法等條件的限制，這些限制條件無法由系統(tǒng)模型表示，如果將其忽略就可能造成最優(yōu)解不滿足實際條件，可信度降低。若將這些限制條件用約束方程表示，例如某些限制條件和狀態(tài)變量之間的關(guān)系可以用不等式表示，再結(jié)合卡爾曼濾波使得最優(yōu)解滿足約束方程，這就是不等式約束卡爾曼濾波。

不等式約束卡爾曼濾波最優(yōu)解可以表示為

2.3 雙足軌跡間最大距離約束算法設(shè)計

在進行導(dǎo)航解算時，左右腳軌跡之間存在距離約束，可以將這種關(guān)系用不等式表示，再用不等式對卡爾曼濾波器進行約束，使最優(yōu)解滿足約束條件，通過這種方法得到的估計狀態(tài)更加符合實際。

在導(dǎo)航解算中卡爾曼濾波器的狀態(tài)量包括3維位置狀態(tài)向量s，3維速度狀態(tài)向量[v]，3維姿態(tài)角狀態(tài)向量[θ]。令[xji=[sjivjiθji]T]為慣性導(dǎo)航系統(tǒng)在k時刻9維狀態(tài)估計向量，其中j∈{L，R}，L表示左腳，R表示右腳。雙足之間的距離為

當(dāng)人在正常行走時[dik]總是不大于雙足軌跡間的最大距離，即存在[dik≤λ]?？蓪?dǎo)航估計狀態(tài)約束到合理的子空間，得到精度更高的狀態(tài)估計值，即滿足不等式約束[4]：

當(dāng)雙足軌跡間的距離超出最大值，即[dik>λ]時則使用不等式約束的卡爾曼濾波對導(dǎo)航狀態(tài)重新估計。定義超出雙足軌跡最大距離后運動腳的位置狀態(tài)觀測量為[pik]，根據(jù)雙足軌跡間的最大距離約束原理（圖1），將垂線上到垂足距離為[λ]的點作為位置觀測量;將導(dǎo)航解算位置作為一步預(yù)測量，重新估計導(dǎo)航狀態(tài)。

3 實驗驗證和結(jié)果分析

通過行人雙足足綁MIMU正常行走的實驗來驗證雙足軌跡間最大距離約束算法的可行性，根據(jù)實驗人員的步行特點，軌跡間最大距離取[λ] = 0. 3 m。為了更方便地判斷導(dǎo)航精度，本文采用的是封閉行走路線，通過最終位置與初始位置的距離差值來判斷導(dǎo)航定位的精度。

實驗人員以初始位置為原點做近似勻速運動，最終回到原點，初始航向角為0°，采樣頻率設(shè)為100 Hz。實驗開始后先在初始位置靜止2 min左右，通過靜止時的加速度信息得到初始橫滾角和俯仰角，通過角速度信息得到陀螺儀的零偏信息。然后開始正常步行，在行走過程中要確保MIMU與腳之間不出現(xiàn)滑動和脫落。最后回到初始位置，靜止1 min以方便提取運動過程的數(shù)據(jù)。

對同一組數(shù)據(jù)分別采用零速修正技術(shù)和雙足軌跡間最大距離約束算法進行軌跡推算，以解算的最終位置到初始位置的距離作為判斷精度的依據(jù)。為消除實驗的偶然性，該測試在不同的時間段重復(fù)進行4次，多次實驗解算得到的結(jié)果應(yīng)無顯著差異。

實驗人員從初始位置開始圍繞大樓正常行走，行走完一圈后停在初始位置，總路程約為300 m。

在不進行雙足軌跡間最大距離約束時，行人雙足軌跡解算結(jié)果如圖2所示。從圖中可以看出，隨著行走距離的增加，左右腳軌跡出現(xiàn)偏移。圖3為該軌跡中偏離較為嚴重的兩部分，從圖中可以看出左右腳軌跡間的距離超過了3 m，雙足最終位置距離超5 m，顯然不符合行人正常的生理特征。

對同一組數(shù)據(jù)用雙足軌跡間最大距離約束算法解算，雙足軌跡如圖4所示。從圖中可以看出，雙足軌跡近似重合，無明顯偏離的部分。圖5軌跡與圖3軌跡為行人軌跡的同一部分，經(jīng)過雙足軌跡間最大距離約束后，雙足軌跡間的距離被限制在0.3 m之內(nèi)，兩條軌跡近似平行狀態(tài)，偏離情況得到明顯改善，符合行人正常生理特征。

為消除實驗偶然性，重復(fù)做4組實驗，約束前最終的位置誤差和約束后最終的位置誤差如表1所示。約束前4次實驗的平均位置誤差為2.475 5 m。約束后4次實驗的平均位置誤差為1.503 2 m。綜合以上實驗結(jié)果，經(jīng)過雙足軌跡間最大距離約束后雙足的平均位置誤差下降了39.28%，達到了提高行人導(dǎo)航精度的目的。

4 結(jié)論

本文對行人正常行走時雙足的運動特點進行了分析，在零速修正的基礎(chǔ)上加入了雙足軌跡間最大距離約束算法對導(dǎo)航位置估計進行二次修正。根據(jù)行人雙足軌跡特點，構(gòu)造約束方程，利用不等式約束卡爾曼濾波的方法對約束信息融合到慣性解算中，得到了比零速修正更高的位置精度。實驗結(jié)果表明，雙足軌跡間最大距離約束算法有效利用了行人雙足運動特征，進一步限制了慣性導(dǎo)航系統(tǒng)的誤差累積，起到了提高定位精度的效果。雙足軌跡間最大距離約束算法是利用行人本身的一些運動特點和生理特性，通過信息融合的方法提高導(dǎo)航精度，為行人慣性導(dǎo)航中其他約束校正方法提供了一種思路。

參考文獻：

[1]? ? 李佳璇，周廣濤，劉曉旭，等. 基于等式約束卡爾曼的雙MIMU行人導(dǎo)航方案[J]. 壓電與聲光，2015，37（2）：237-241.

[2]? ? 張釗，侯書軍. 雙軸式慣性激振器運轉(zhuǎn)噪音與振動特性關(guān)系研究[J]. 河北工業(yè)大學(xué)學(xué)報，2018，47（6）：50-55.

[3]? ? 時偉，王陽. 基于不等式約束卡爾曼濾波的雙MIMU導(dǎo)航位置校正方法[J]. 中國慣性技術(shù)學(xué)報，2017，25（1）：11-16.

[4]? ? 丁承君，趙澤羽. 基于多傳感器數(shù)據(jù)融合的火災(zāi)探測系統(tǒng)[J]. 河北工業(yè)大學(xué)學(xué)報，2018，47（5）：17-22.

[5]? ? DONOVAN G T. Position error correction for an autonomous underwater vehicle inertial navigation system （INS） using a particle filter[J]. IEEE Journal of Oceanic Engineering，2012，37（3）：431-445.

[6]? ? LIU W R，NING W，LIU G B，et al. Integration navigation method of dual INS[J]. Journal of Chinese Inertial Technology，2014，22（1）：1-4，13.

[7]? ? SKOG I，HANDEL P，NILSSON J O，et al. Zero-velocity detection：an algorithm evaluation[J]. IEEE Transactions on Biomedical Engineering，2010，57（11）：2657-2666.

[8]? ? PRATEEK G V，GIRISHA R，HARI K V S，et al. Data fusion of dual foot-mounted INS to reduce the systematic heading drift[C]//2013 4th International Conference on Intelligent Systems，Modelling and Simulation. Bangkok，Thailand. IEEE，2013：208-213.

[9]? ? ZHANG X X，ZHANG R，GUO M F，et al. Online three-axis magnetometer calibration for a pedestrian navigation system using a foot-mounted inertial navigation system[J]. Journal of Tsinghua University，2016，56（2）：211-217.

[10]? 周婧，陳廟紅，吳豪杰. 基于慣性導(dǎo)航的平面航跡推算的研究[J]. 計算機科學(xué)，2017，44（z1）：582-586.

[責(zé)任編輯? ? 田? ? 豐]