基于足部安裝MIMU的行人導(dǎo)航系統(tǒng)設(shè)計(jì)

,

(1.北京理工大學(xué) 自動(dòng)化學(xué)院，北京100081; 2.北京自動(dòng)化控制設(shè)備研究所，北京 100074)

0 引言

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

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

1 導(dǎo)航鞋樣機(jī)設(shè)計(jì)

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

1.1 傳感器選型

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

1.2 導(dǎo)航鞋選型

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

表1 MTi慣性測量單元傳感器性能指標(biāo)

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

1.3 傳感器安裝

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

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

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

圖2 導(dǎo)航鞋樣機(jī)Fig.2 Prototype of navi-shoe

2 零速修正算法

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

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

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

(1)

(2)

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

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

(3)

(4)

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

(5)

(6)

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

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

(7)

其中，ωy(k)表示k時(shí)刻橫向陀螺輸出的絕對值，Tω為角速率閾值，當(dāng)橫向陀螺輸出角速率絕對值低于閾值時(shí)，認(rèn)為運(yùn)動(dòng)腳靜止。

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

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

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

2.2 零速修正算法

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

(8)

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

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

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

(9)

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

(10)

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

(11)

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

(12)

3 航向角修正算法

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

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

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

n=ΔT/Ts

(13)

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

(14)

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

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

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

ΦSINS(ti)-Φmag(ti)

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

(15)

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

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

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

(16)

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

δφ=φSINS-φmag

(17)

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

(18)

4 實(shí)驗(yàn)測試及分析

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

圖4 上下坡測試實(shí)驗(yàn)Fig.4 Pedestrian positioning experiment in slope environment

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

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

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

5 結(jié)論

零速修正與航向修正是影響行人導(dǎo)航系統(tǒng)定位精度的2個(gè)關(guān)鍵因素。本文提出了一種基于多約束條件的零速檢測方法，在保證靈敏度的前提下提高系統(tǒng)的檢測正確率。此外，本文設(shè)計(jì)了一種基于陀螺輔助的抗干擾磁航向修正算法，解決因環(huán)境磁場干擾導(dǎo)致的航向角誤修正問題，實(shí)現(xiàn)了零速區(qū)間內(nèi)有效磁航向角的提取。采用三維速度零速修正方式，實(shí)現(xiàn)高度通道定位誤差的有效阻尼。導(dǎo)航試驗(yàn)表明，在上下坡環(huán)境中，本文設(shè)計(jì)的行人導(dǎo)航系統(tǒng)能夠較好地滿足行人在三維空間內(nèi)的導(dǎo)航定位需求，位置誤差為0.33%。在研究中發(fā)現(xiàn)，目前文獻(xiàn)中的各個(gè)算法對跑步條件下的零速檢測問題都沒有令人滿意的效果，是未來研究中亟待解決的重點(diǎ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ù)的鞋式個(gè)人導(dǎo)航系統(tǒng)[J]. 中國慣性技術(shù)學(xué)報(bào), 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] 田曉春，陳家斌，韓勇強(qiáng), 等. 多條件約束的行人導(dǎo)航零速區(qū)間檢測算法[J].中國慣性技術(shù)學(xué)報(bào), 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] 李若涵, 張金藝, 徐德政, 等. 運(yùn)動(dòng)分類步頻調(diào)節(jié)的微機(jī)電慣性測量單元室內(nèi)行人航跡推算[J]. 上海大學(xué)學(xué)報(bào): 自然科學(xué)版, 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.