基于足部安裝MIMU的行人導航系統(tǒng)設計

,

(1.北京理工大學 自動化學院，北京100081; 2.北京自動化控制設備研究所，北京 100074)

0 引言

行人導航作為導航學科的一門新興分支，近年來在國內(nèi)外得到了廣泛的關(guān)注和研究，尤其是當前基于人員位置服務的市場化應用需求，更拓展了行人導航系統(tǒng)研究的廣度和熱度。行人導航系統(tǒng)可廣泛應用于消防搶險、智慧城市、單兵作戰(zhàn)等領(lǐng)域。

與汽車、飛機等載體不同，行人帶負載能力有限；此外，針對無衛(wèi)星信號環(huán)境中的人員定位及目標找尋，還需要行人導航系統(tǒng)具有較強的自主定位能力。隨著半導體設計方法及工藝的提升，目前市場上MEMS慣性傳感器的精度可以達到商業(yè)級或戰(zhàn)術(shù)級水平，因此由MEMS傳感器構(gòu)成的慣性測量單元能夠滿足行人在無衛(wèi)星或者衛(wèi)星失效環(huán)境下的自主定位需求[8，15]?；诖?，本文采用足部安裝慣性測量單元的方案設計行人導航系統(tǒng)，完成了傳感器選型并設計了行人導航鞋樣機，并在此基礎(chǔ)上開展了Kalman濾波架構(gòu)下的零速修正算法和航向修正算法設計，實現(xiàn)了對系統(tǒng)的誤差估計。

1 導航鞋樣機設計

根據(jù)行人導航系統(tǒng)的適用對象和實際應用環(huán)境，導航鞋樣機設計主要從鞋的選型、慣性測量單元的安裝等方面進行考慮。

1.1 傳感器選型

行人導航系統(tǒng)選用的傳感器應具有質(zhì)量小、尺寸小、功耗低、動態(tài)范圍大、便于穿戴等特性，這樣既不會影響行人的正常運動，又能實現(xiàn)長時間導航。當前關(guān)于行人導航系統(tǒng)研究的文獻中應用較為廣泛的慣性測量單元有荷蘭Xsens公司生產(chǎn)的MTi/MTx系列產(chǎn)品[1]、美國Analog Devices公司生產(chǎn)的ADIS系列產(chǎn)品[2]、美國MicroStrain公司生產(chǎn)的3DM-GX系列產(chǎn)品[3]及美國InterSense公司生產(chǎn)的InertiaCube系列產(chǎn)品[4-5]等[11]。這些慣性測量單元都在一個結(jié)構(gòu)中集成了可以測量3個正交軸向上載體加速度與角速率的加速度計和陀螺儀，同時還在結(jié)構(gòu)內(nèi)部集成了微處理器用于信號的處理及傳輸。這些慣性測量單元只通過一個復合接口就能夠?qū)崿F(xiàn)供電以及與其他外設通信的功能。除慣性傳感器外，有的慣性測量單元還集成了磁傳感器、氣壓計、溫度計等傳感器，以滿足不同場合的應用需求。綜合考慮，本文選用了Xsens公司生產(chǎn)的MTi系列大量程慣性測量單元，其主要性能指標如表1所示。

1.2 導航鞋選型

在導航鞋的選型方面，考慮到MTi慣性測量單元內(nèi)部集成了磁傳感器，需首先選擇不含鐵質(zhì)飾品的鞋子，以避免鐵磁性干擾對磁傳感器輸出造成的影響；其次，實際使用時需要將慣性測量單元與鞋體固連，所以制鞋的材料應以硬質(zhì)面料為主，例如皮革或仿皮質(zhì)材料。本文慣性測量單元安裝在鞋后跟處，所以在選鞋時還應以高幫鞋為主。

表1 MTi慣性測量單元傳感器性能指標

為了便于數(shù)據(jù)傳輸，在設計慣性測量單元的安裝方式時選用了數(shù)據(jù)線接口插槽豎直向上的方案。為了防止行走過程中慣性測量單元與地面接觸，慣性測量單元安裝在鞋上后，其底端與鞋底之間應至少有20mm的距離；同時結(jié)合傳感器尺寸，本文選擇導航鞋時鞋幫的高度應不小于85mm。綜合考慮以上因素，選用了一款由皮革面料制作的長筒軍靴用于導航鞋設計[13-14]。

1.3 傳感器安裝

完成導航鞋的選型后，需考慮如何將慣性測量單元安裝在鞋體上。在慣性測量單元安裝方面，一些學者采用鞋帶或者其他材料捆綁的方式來固定慣性測量單元，這樣的安裝方式雖然便于拆卸，但是行人行走過程中慣性測量單元容易晃動，會引入干擾加速度和角速度，從而影響傳感器對步態(tài)運動參數(shù)的準確測量；還有學者通過在鞋底開槽的方式將慣性測量單元固定在鞋中，這種方法雖然能夠有效地測量行人的步態(tài)參數(shù)，但是傳感器的安裝、拆卸過程相對繁瑣。為了實現(xiàn)慣性測量單元安裝簡單、拆卸方便，且能夠穩(wěn)固地與導航鞋固連的目標，本文設計了一塊慣性測量單元安裝板，其外形圖以及與鞋體固聯(lián)后的效果如圖1所示。

圖1 傳感器安裝板示意圖Fig.1 Pinboard of inertial sensors

將MTi慣性測量單元用銅螺絲固定在安裝板上即完成了導航鞋樣機的設計，效果如圖2所示。本文設計的導航鞋樣機具有接觸穩(wěn)固、安裝簡單、拆卸方便等優(yōu)點。

圖2 導航鞋樣機Fig.2 Prototype of navi-shoe

2 零速修正算法

2.1 基于多條件約束的零速檢測算法

零速檢測是零速修正算法的先決條件和關(guān)鍵環(huán)節(jié)。本文設計了一種綜合加速度矢量和閾值、滑動窗口方差、加表峰值檢測+時間閾值、角速度閾值等多種指標的多條件約束零速檢測法，可以較好地兼顧零速檢測靈敏性與準確性[9]。幾種零速檢測指標的計算方法如下。

1)加速度矢量和閾值。該指標通過計算3個正交軸向上加速度計輸出的矢量和，將其與設定的零速閾值區(qū)間相比較，判斷是否處于零速狀態(tài)。

(1)

(2)

其中，Accnorm(k)表示k時刻加速度矢量和，ai(k)表示k時刻i軸向上加速度計輸出，i=x,y,z。TA_min、TA_max分別是閾值區(qū)間的左邊界和右邊界，當加速度模值在閾值區(qū)間內(nèi)時，判定為零速狀態(tài)。

2)滑動窗口方差。設置一個滑動窗口，動態(tài)計算加速度計信號的方差值，判斷加速度計信號離散度并以此作為零速狀態(tài)的判斷依據(jù)之一。

(3)

(4)

3)加表峰值檢測+時間閾值。足部觸地瞬間會產(chǎn)生加速度計輸出峰值，通過檢測這一峰值可以輔助確定觸地后的時間區(qū)間Δt為零速區(qū)間。

(5)

(6)

其中，aPeak(k)表示k時刻檢測到加速最大峰值點，ay(k)表示豎直軸向加速度，Tay表示加速度閾值， (k+Δt1,k+Δt2)表示判斷到加速度峰值后設定的行人處于靜止的時間區(qū)間。

4)角速度閾值。足部處于零速區(qū)間時橫向陀螺測量的足部擺動應處于比較小的狀態(tài)，可以利用橫向陀螺輸出作為零速檢測的約束條件。

(7)

其中，ωy(k)表示k時刻橫向陀螺輸出的絕對值，Tω為角速率閾值，當橫向陀螺輸出角速率絕對值低于閾值時，認為運動腳靜止。

單一的判據(jù)容易出現(xiàn)錯判和誤判，本文通過合理選擇閾值，將式(1)～式(4)中k時刻的判斷結(jié)果進行與運算，即

ZUPT(k)=C1(k)&C2(k)&C3(k)&C4(k)

在保證零速判斷靈敏度的前提下提高檢測的正確性。通過試驗驗證了不同步速條件下上述零速檢測算法的檢測效果，結(jié)果如圖3和表2所示[10]。

2.2 零速修正算法

本文設計的行人導航系統(tǒng)本質(zhì)為組合系統(tǒng)，故可通過Kalman濾波技術(shù)進行數(shù)據(jù)融合，估計行人導航系統(tǒng)的誤差并利用誤差的估計值對導航參數(shù)進行校正。離散化的Kalman方程可表示為：

(8)

實驗類型速度/(km/h)步數(shù)零速區(qū)間實際值計算值誤差/%慢速行走3781447272034514874740常速行走4481346767045813467670快速行走6011165858057911859590

其中，δxk為k時刻的系統(tǒng)狀態(tài)，Φk為狀態(tài)轉(zhuǎn)移矩陣，wk為過程噪聲；δzk為k時刻誤差觀測量，H為測量矩陣，vk為測量噪聲。

本文行人導航系統(tǒng)的誤差方程可表示為：

(9)

結(jié)合式(9)中導航系統(tǒng)誤差方程及慣性傳感器誤差方程設計Kalman濾波器，系統(tǒng)狀態(tài)向量取

(10)

對于足部安裝自包含傳感器的行人導航系統(tǒng)來說，零速修正是數(shù)據(jù)融合算法的基礎(chǔ)，本文Kalman濾波器狀態(tài)更新的觸發(fā)條件就是檢測到步態(tài)中的零速區(qū)間。行人步態(tài)中的零速區(qū)間可認為是虛擬的零速速度傳感器，其測量值能夠作為步態(tài)靜止相位中的速度觀測值。采用Kalman濾波進行誤差狀態(tài)估計時，可根據(jù)式(11)將行人導航系統(tǒng)中慣性測量單元解算的步態(tài)速度值與步態(tài)靜止相位中輸出的虛擬零速作差，并將計算的差值作為速度誤差觀測量輸入到Kalman濾波器。通過利用速度誤差與姿態(tài)誤差、位置誤差以及傳感器誤差之間的耦合關(guān)系可以估計出行人導航系統(tǒng)的狀態(tài)誤差參數(shù)[6]。

(11)

將計算得到的速度誤差δV輸入到Kalman濾波器進行誤差估計。零速修正過程中測量矩陣H的表達式為

(12)

3 航向角修正算法

對于基于足部安裝慣性傳感器的行人導航系統(tǒng)來說，零速修正是一種十分有效的誤差修正方法。它利用行人步態(tài)中周期性存在零速區(qū)間的特點對系統(tǒng)誤差進行修正，能有效抑制橫滾俯仰誤差，但由于航向誤差不可觀測，ZUPT無法估計航向誤差漂移[7]，而在行人導航系統(tǒng)中航向誤差會導致計算的行人航跡位置信息存在較大的誤差[12]。

針對行人導航系統(tǒng)中航向角誤差修正問題，本文利用陀螺儀計算航向角具有短時精度高、磁傳感器在不受外界干擾時計算的航向角精度高的特點，設計了零速區(qū)間內(nèi)有效磁航向角判定函數(shù)，實現(xiàn)了對航向角的準確提取。同時，采用平滑濾波技術(shù)對零速區(qū)間內(nèi)所有的有效航向角求最優(yōu)值。這種最優(yōu)航向角計算方法在一定程度上抑制了外界干擾導致的航向誤差，優(yōu)化了局部航向角的計算值，為濾波器提供較為精確的外部航向觀測量。

設行人步態(tài)中零速區(qū)間的時間長度為ΔT，微慣性測量單元數(shù)據(jù)輸出周期為Ts，則零速區(qū)間內(nèi)包含的零速點數(shù)目為

n=ΔT/Ts

(13)

當檢測到行人零速區(qū)間后，根據(jù)式(14)中航向角計算公式并結(jié)合零速區(qū)間內(nèi)磁傳感器的輸出便能計算出每一個零速時刻點對應的航向角

(14)

其中，Φmag(ti)為零速區(qū)間內(nèi)ti時刻由磁傳感器計算的航向角，Bx(ti)和By(ti)分別為磁傳感器輸出的水平磁分量，D為當?shù)卮牌恰?/p>

在實際應用中會存在磁干擾的情況，從而導致磁傳感器計算的航向角存在誤差，因此在實際應用中需考慮磁干擾的問題。為了消除環(huán)境中突然出現(xiàn)的磁干擾，本文設計了干擾磁航向角識別算法用于識別外界干擾導致的錯誤航向角。在行人導航系統(tǒng)中，利用陀螺輸出計算的航向角具有短時精度高、不受外界干擾的特點，但隨著時間的增長航向角誤差不斷累積；而磁傳感器計算的航向角在不受外界干擾時精度較高。由于行人步態(tài)周期較短，短時間內(nèi)陀螺計算的航向角可以作為參考信息，因此對于判定零速區(qū)間內(nèi)ti時刻磁傳感器計算的航向角是否受外界干擾可通過捷聯(lián)解算的航向角進行輔助判斷。

定義ti時刻捷聯(lián)解算的航向角為ΦSINS(ti)，進行磁干擾判斷后得到的磁航向角有效輸出為ΦMAG(ti),則有效磁航向角的判定條件為

ΦSINS(ti)-Φmag(ti)

T1≤ΦSINS(ti)-Φmag(ti)

(15)

ΦSINS(ti)-Φmag(ti)≥T2

式中，k1、k2分別表示捷聯(lián)解算航向角和磁計算航向角的加權(quán)系數(shù)；T1、T2表示捷聯(lián)解算的航向角與磁傳感器計算的航向角差值的判斷閾值。

將零速區(qū)間內(nèi)計算的所有有效航向角進行平滑濾波，得到零速區(qū)間內(nèi)優(yōu)化的航向角，計算公式如下

(16)

其中，φmag(ti)表示經(jīng)過平滑濾波后ti時刻的航向角優(yōu)化值，可作為外部觀測量進行航向角誤差計算。航向角誤差表達式如下

δφ=φSINS-φmag

(17)

將計算的航向角誤差代入Kalman濾波器估計行人導航系統(tǒng)航向誤差，并對捷聯(lián)慣導系統(tǒng)中陀螺儀計算的航向參數(shù)進行修正，消除陀螺漂移引起的累積誤差。航向角修正過程中測量矩陣H的表達式為

(18)

4 實驗測試及分析

考慮到行人在坡度較緩的環(huán)境中運動時也需要提供高度參考信息的情況，本文開展了行人上下坡測試實驗，并采用文中設計的導航算法計算了行人在緩坡環(huán)境中的導航參數(shù)。如圖4所示，實驗環(huán)境為一坡型閉合區(qū)域，行人運動路線總長度為51.7m，坡頂與坡底之間的高程差為0.6m，行人在該實驗區(qū)域內(nèi)行走一圈，解算得到的路徑曲線及高度變化曲線分別如圖5、圖6所示。

圖4 上下坡測試實驗Fig.4 Pedestrian positioning experiment in slope environment

圖5 上下坡航跡曲線Fig.5 Calculated trajectory when going up/down the slope

從圖5中可以看出，導航解算的行人航跡曲線與實驗環(huán)境中規(guī)劃的行走路線一致，行人先從水平路段起始點出發(fā)，行走一段距離后進入上坡路段，然后經(jīng)過坡頂平臺后開始下坡，最后回到起始點。行人運動軌跡終點處位置誤差為0.17m，占行人行走總路程的0.33%。從終點處位置解算精度來看，在沒有高度約束的條件下，本文設計的行人導航算法也能精確地計算行人在緩坡路段行走過程中的位置信息。

圖6所示為行人上下坡行走時高度變化曲線。從圖6中可以看出，解算得到的高度變化值與行人實際運動過程中的高度變化趨勢一致，但由于沒有對導航解算高度誤差進行約束，隨著行走距離的增加，高度誤差不斷累積，在終點處達到最大。綜合分析圖5、圖6中的導航參數(shù)曲線可以看出，在包含坡路的環(huán)境中，隨時間累積的高度誤差是影響行人導航定位精度的主要因素。而對于緩坡路段，由于行人行走過程中高度變化較小，所以在不增加高度約束的條件下也能比較準確地計算出高度信息供行人參考。

5 結(jié)論

零速修正與航向修正是影響行人導航系統(tǒng)定位精度的2個關(guān)鍵因素。本文提出了一種基于多約束條件的零速檢測方法，在保證靈敏度的前提下提高系統(tǒng)的檢測正確率。此外，本文設計了一種基于陀螺輔助的抗干擾磁航向修正算法，解決因環(huán)境磁場干擾導致的航向角誤修正問題，實現(xiàn)了零速區(qū)間內(nèi)有效磁航向角的提取。采用三維速度零速修正方式，實現(xiàn)高度通道定位誤差的有效阻尼。導航試驗表明，在上下坡環(huán)境中，本文設計的行人導航系統(tǒng)能夠較好地滿足行人在三維空間內(nèi)的導航定位需求，位置誤差為0.33%。在研究中發(fā)現(xiàn)，目前文獻中的各個算法對跑步條件下的零速檢測問題都沒有令人滿意的效果，是未來研究中亟待解決的重點問題。

[1] Liu W, Zhang Y, Yang X, et al. Pedestrian navigation using inertial sensors and altitude error correction[J]. Sensor Review, 2015, 35(1): 68-75.

[2] Lan K C, Shih W Y. Using smart-phones and floor plans for indoor location tracking-withdrawn[J]. IEEE Transactions on Human-Machine Systems, 2014, 44(2): 211-221.

[3] Tian X, Chen J, Han Y, et al. A novel zero velocity interval detection algorithm for self-contained pedestrian navigation system with inertial sensors[J]. Sensors 2016,16(10):1578.

[4] 張金亮, 秦永元, 梅春波. 基于MEMS慣性技術(shù)的鞋式個人導航系統(tǒng)[J]. 中國慣性技術(shù)學報, 2011, 19(3): 253-256.

[5] Yun X, Calusdian J, Bachmann E R, et al. Estimation of human foot motion during normal walking using inertial and magnetic sensor measurements[J]. IEEE Transactions on Instrumentation and Measurement, 2012, 61(7): 2059-2072.

[6] Bebek ?, Suster M A, Rajgopal S, et al. Personal navigation via high-resolution gait-corrected inertial measurement units[J]. IEEE Transactions on Instrumentation and Measurement, 2010, 59(11): 3018-3027.

[7] Woodman O. Pedestrian localisation for indoor environments[D]. St Catharine’s College, Digital Technology Group Computer Laboratory University of Cambridge, 2010:68-75.

[8] Abdulrahim K, Hide C, Moore T, et al. Rotating a MEMS inertial measurement unit for a foot-mounted pedestrian navigation[J]. Journal of Computer Science, 2014, 10(12): 2619-2627.

[9] 田曉春，陳家斌，韓勇強, 等. 多條件約束的行人導航零速區(qū)間檢測算法[J].中國慣性技術(shù)學報, 2016,24(1):1-5.

[10] Liu J, Chen R, Pei L, et al. A hybrid smartphone indoor positioning solution for mobile LBS[J]. Sensors, 2012, 12(12): 17208-17233.

[11] 李若涵, 張金藝, 徐德政, 等. 運動分類步頻調(diào)節(jié)的微機電慣性測量單元室內(nèi)行人航跡推算[J]. 上海大學學報: 自然科學版, 2014, 20(5): 612-623.

[12] Jirawimut R, Ptasinski P, Garaj V, et al. A method for dead reckoning parameter correction in pedestrian navigation system[J]. IEEE Transactions on Instrumentation and Measurement, 2003, 52(1): 209-215.

[13] Ladetto Q, Gabaglio V, Van Seeters J. Pedestrian navigation method and apparatus operative in a dead reckoning mode: U.S. Patent 6826477[P]. 2004-11-30.

[14] Stirling R, Collin J, Fyfe K, et al. An innovative shoe-mounted pedestrian navigation system[C]// Proceedings of European Navigation Conference GNSS. 2003: 110-5.

[15] Fang L, Antsaklis P J, Montestruque L A, et al. Design of a wireless assisted pedestrian dead reckoning system-the NavMote experience[J]. IEEE Transactions on Instrumentation and Measurement, 2005, 54(6): 2342-2358.