MEMS慣性鞋式行人導(dǎo)航系統(tǒng)*

宋麗君，路明慧，趙思晗，周 彬，王金虎，鄭普亮，劉峰秀

(1.西安建筑科技大學(xué) 信息與控制工程學(xué)院·西安·710055；2.中國科學(xué)院深地資源裝備技術(shù)工程實(shí)驗(yàn)室 中國科學(xué)院地質(zhì)與地球物理研究所·北京·100029；3.上海航天控制技術(shù)研究所·上海·201109；4.浙江工業(yè)大學(xué) 機(jī)械工程學(xué)院·杭州·310023)

0 引 言

行人導(dǎo)航系統(tǒng)(Pedestrian Navigation System，PNS)是一種實(shí)時(shí)獲取行人航向、位置、速度等信息的導(dǎo)航系統(tǒng)，在國民體系各個(gè)行業(yè)都有廣泛應(yīng)用的前景[1]。目前的行人導(dǎo)航系統(tǒng)是基于GPS定位設(shè)備進(jìn)行設(shè)計(jì)，容易受到外界環(huán)境的干擾，僅適合在室外開闊環(huán)境下工作[2]。而基于微電子機(jī)械系統(tǒng)(Micro-Electro-Mechanical System，MEMS)慣性器件的行人導(dǎo)航系統(tǒng)具有自主性強(qiáng)、更新率高、導(dǎo)航信息完整、便于在人體上安裝與攜帶等特點(diǎn)，是對衛(wèi)星定位系統(tǒng)的有效補(bǔ)充，且隨著MEMS傳感器精度的逐步提高，基于MEMS器件的行人導(dǎo)航系統(tǒng)的精度也將得到提升[3]。

文獻(xiàn)[4]提出一種基于磁、角速率和重力傳感器的運(yùn)動(dòng)感知和步態(tài)校正行人航位推算(Pedestrian Dead Reckoning，PDR)方法，但這種方法僅可得到水平面內(nèi)二維導(dǎo)航結(jié)果，無法提供海拔位置變化，適用范圍有局限性。文獻(xiàn)[5]根據(jù)步行者的步態(tài)特征，以陀螺輸出角速度矢量和、加計(jì)輸出比力矢量及滑動(dòng)方差為檢測量，其僅將速度信息作為偽量測量設(shè)計(jì)了Kalman濾波器，雖然可提供三維導(dǎo)航結(jié)果，但其未考慮方位誤差的存在對導(dǎo)航精度的影響。文獻(xiàn)[6]提出了一種基于條件隨機(jī)場(Conditional Random Field，CRF)的PDR與偽室內(nèi)規(guī)劃的融合方法，以解決PDR累積誤差和初始位置不確定的問題，但這種方法需提前構(gòu)建先驗(yàn)數(shù)據(jù)庫，增加了導(dǎo)航的成本和難度。

本文設(shè)計(jì)了一種基于MEMS慣性測量組件的行人導(dǎo)航算法，將MEMS慣性測量組件安裝于人體足部，在足部著地期間通過零速檢測以及零速修正算法估計(jì)并修正MEMS慣性導(dǎo)航系統(tǒng)及MEMS慣性器件誤差。此外，增加磁強(qiáng)計(jì)方位信息，以實(shí)現(xiàn)方位誤差估計(jì)。

1 行人導(dǎo)航系統(tǒng)方案設(shè)計(jì)

1.1 慣性行人導(dǎo)航系統(tǒng)算法結(jié)構(gòu)

基于MEMS器件的PNS導(dǎo)航算法方案采用捷聯(lián)慣導(dǎo)系統(tǒng)(Strapdown Inertial Navigation System，SINS)算法，可通過六自由度信息解算得出完整導(dǎo)航信息[7]，但由于MEMS器件精度較低，若不能有效修正位置誤差，其將以時(shí)間的三次方趨勢發(fā)散，最終失去導(dǎo)航功能[8]。本文采用SINS算法并將PNS附著固定在鞋子上，結(jié)合行走者腳部運(yùn)動(dòng)特征，設(shè)計(jì)了一種零速修正(Zero Velocity Update，ZUPT)算法，以進(jìn)行實(shí)時(shí)導(dǎo)航修正，提升導(dǎo)航精度。ZUPT算法結(jié)構(gòu)如圖1所示。

圖1 PNS導(dǎo)航系統(tǒng)修正算法Fig.1 Navigation algorithm structure of PNS

慣導(dǎo)行人系統(tǒng)算法的研究核心在于步態(tài)檢測和零速修正算法設(shè)計(jì)，國內(nèi)外學(xué)者設(shè)計(jì)了不同的步態(tài)檢測方法和零速修正方法來提高系統(tǒng)的長航時(shí)導(dǎo)航精度[9]。

1.2 MEMS器件安裝位置分析

在行人導(dǎo)航系統(tǒng)中，慣性傳感器被固連在行人足部，且有內(nèi)、外兩種安裝方式[10]，圖2為MEMS器件的兩種安裝方式[11-12]。從某種角度來說，作為載體的鞋子也是硬件系統(tǒng)的一部分。

(a) 內(nèi)置安裝

(b) 外置安裝圖2 慣性傳感器安裝方式Fig.2 Installation styles of inertial sensor

考慮到腳背部分的表面積大且骨骼突出部分少這一因素，采取將傳感器安裝在腳背部的方式，具體為圖2(b)中的位置2附近。

2 行人導(dǎo)航系統(tǒng)算法設(shè)計(jì)

2.1 行人步態(tài)特征分析

行人正常水平行走的步態(tài)是近似于周期性的一個(gè)過程，每一個(gè)完整的步態(tài)周期中均有四個(gè)關(guān)鍵的運(yùn)動(dòng)學(xué)事件[13]：腳跟著地(Heel-Strike，HS)、全腳著地(Foot-Flat，F(xiàn)F)、腳跟離地(Heel-Off，HO)、腳尖離地(Toe-Off，TO)。圖3為行人行走的一個(gè)周期性過程。

圖3 行走步態(tài)的關(guān)鍵事件Fig.3 Key events of walking gait

對行人行走過程中的單只腳進(jìn)行觀察，有明顯的“運(yùn)動(dòng)—停止—運(yùn)動(dòng)—……”交替變換的規(guī)律，安裝在鞋上的MEMS器件可感測到鞋子與地面的觸碰和靜止。成人的正常行走過程中，支撐相的典型持續(xù)時(shí)間約為單步運(yùn)動(dòng)持續(xù)時(shí)間的30%，為0.3～0.5s，這一段時(shí)間足夠用來進(jìn)行零速修正[14]。鞋式PNS算法充分利用該規(guī)律，在靜止時(shí)間段進(jìn)行零速修正，消除位置誤差隨時(shí)間的三次方發(fā)散。

2.2 零速檢測算法設(shè)計(jì)

MEMS器件量測數(shù)據(jù)的輸出是靜止檢測算法的基礎(chǔ)，該算法主要是基于對加速度或者角速度的統(tǒng)計(jì)特性的分析，來判斷某一階段是否為靜止?fàn)顟B(tài)[15]。本文同時(shí)使用加計(jì)輸出的比力信息和陀螺的角速度信息設(shè)計(jì)多條件零速檢測算法，這種算法只需要使用三軸陀螺儀以及三軸加速度計(jì)輸出數(shù)據(jù)便可判別出零速區(qū)間，不需要額外的輔助傳感器。該檢測算法一共利用了三個(gè)條件(記為T1，T2與T3)，以下是其詳盡的計(jì)算和判斷方法。

(1)

2)比力方差，腳步靜止期間比力方差需小于設(shè)定閾值cσ=1×10-5m/s2。

(2)

(3)

(4)

若同時(shí)滿足這三個(gè)判斷條件(T1，T2和T3)，則將該種情況記為0，即表示零速檢測結(jié)果為靜止，否則記為1。

多條件算法零速區(qū)間識別方法流程圖如圖4所示。

圖4 多條件約束的零速區(qū)間檢測方法流程圖Fig.4 Flowchart of zero-velocity interval detection method with multi-condition constraints

2.3 零速修正卡爾曼濾波器設(shè)計(jì)

設(shè)系統(tǒng)狀態(tài)空間表達(dá)式為

(5)

其中，F(xiàn)(t)代表狀態(tài)轉(zhuǎn)移矩陣，X(t)代表狀態(tài)矩陣，W(t)代表輸入矩陣，H(t)代表系統(tǒng)觀測矩陣，V(t)代表系統(tǒng)觀測噪聲。

將式(5)離散化并簡記為

(6)

卡爾曼濾波算法為[16]

(7)

(8)

(9)

(10)

(11)

式(7)～式(11)構(gòu)成離散卡爾曼濾波完整算法。

(12)

卡爾曼濾波器量測方程如下所示

Zk=HkXk+Vk

(13)

由于僅采用速度作為量測量難以對方位誤差做出有效估計(jì)，因此，采用速度誤差和磁強(qiáng)計(jì)輸出作為量測信息

Δψk=ψk-ψmagk

(14)

其中，ψk表示imu輸出信息，ψmagk表示磁航向角。

Zk=[Δψk,δv]=HkXk+Vk

(15)

其中，

零速修正卡爾曼濾波器可實(shí)現(xiàn)如下三種功能：速度誤差修正、水平姿態(tài)誤差修正以及運(yùn)動(dòng)過程中的累積位置誤差的修正。

3 硬件平臺搭建

3.1 硬件平臺搭建

根據(jù)功能需求，選用STM32F405作為MEMS行人導(dǎo)航系統(tǒng)核心處理器，九軸慣性測量單元選用MPU9250，氣壓傳感器選用MS5611。繪制的PCB版圖采用Cadence Allegro軟件四層板設(shè)計(jì)。電路板與IMU傳感器、MS5611模塊緊緊貼合在一起，形成了一個(gè)尺寸小且質(zhì)量小的板塊。硬件系統(tǒng)總體的框圖如圖5所示。

圖5 硬件總體結(jié)構(gòu)框圖Fig.5 Hardware overall structure block diagram

硬件結(jié)構(gòu)以STM32為核心并與IMU完成數(shù)據(jù)的采集，計(jì)算得出導(dǎo)航信息并傳輸至PC端。為在初始化階段得到準(zhǔn)確度更高的高度通道信息，又借助MS5611氣壓高度計(jì)對初始高度信息進(jìn)行采集。STM32與計(jì)算機(jī)的通信這里采用串口轉(zhuǎn)USB有線模式，這種方法主要在實(shí)驗(yàn)前期數(shù)據(jù)采樣的階段應(yīng)用，為實(shí)驗(yàn)驗(yàn)證階段做準(zhǔn)備。

3.2 PCB設(shè)計(jì)與系統(tǒng)硬件調(diào)試

根據(jù)上述各器件的原理圖，使用Cadence Allegro軟件繪制電路以及PCB布局。本文所繪制的PCB版圖如圖6所示。采用四層板設(shè)計(jì)，為信號層-電源層-地層-電源層。

(a) 信號層 (b) 電源層

(c) 地層 (d)電源層圖6 PCB版圖Fig.6 PCB layout

所搭建的硬件系統(tǒng)實(shí)物圖如圖7所示。

圖7 硬件系統(tǒng)實(shí)物圖Fig.7 Hardware system physical diagram

4 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)采用的MEMS慣性單元為本文第3章所設(shè)計(jì)搭建的行人導(dǎo)航硬件系統(tǒng)，其尺寸為3.7cm×3.7cm×0.15cm，其主要由三軸加計(jì)、三軸陀螺和三軸磁強(qiáng)計(jì)組成。本文實(shí)驗(yàn)過程中將MEMS IMU固定在步行者的一只鞋上，數(shù)據(jù)輸出頻率為100Hz，通過RS232-USB傳送到筆記本電腦，由VC++數(shù)據(jù)采集界面保存數(shù)據(jù)，然后將實(shí)驗(yàn)采集數(shù)據(jù)做離線處理。實(shí)驗(yàn)過程中將MEMS IMU固定在步行者的一只鞋上，完整行人導(dǎo)航系統(tǒng)如圖8所示。

圖8 完整行人導(dǎo)航系統(tǒng)Fig.8 Pedestrian navigation system

下面將對本文所設(shè)計(jì)的零速檢測算法檢測效果進(jìn)行實(shí)驗(yàn)驗(yàn)證，這里首先對各個(gè)檢測條件的有效性進(jìn)行對比。選取行人行走過程中的6步行走數(shù)據(jù)，然后依次選取2.2節(jié)所提及的三種綜合零速檢測算法中的單一檢測條件(即比力模值零速判斷方法、比力方差零速判斷方法、角速度模值零速判斷方法)進(jìn)行單條件零速檢測，檢測結(jié)果如圖9所示。

圖9 單一條件靜止檢測效果Fig.9 The effect of single condition stance phase detection

從圖9可以看出，僅使用單一零速檢測條件的方法在某一較短時(shí)間區(qū)間內(nèi)的縱坐標(biāo)值在0和1兩個(gè)數(shù)字極端反復(fù)跳變，這表明存在較為明顯的誤檢與漏檢情況。即將運(yùn)動(dòng)狀態(tài)判斷為靜止，靜止?fàn)顟B(tài)判斷為運(yùn)動(dòng)。需要特別說明，當(dāng)圖9某一時(shí)刻縱坐標(biāo)值為1時(shí)檢測為運(yùn)動(dòng)狀態(tài)，值為0時(shí)為靜止?fàn)顟B(tài)。

下面給出使用三條件進(jìn)行零速檢測的檢測結(jié)果，如圖10所示。

圖10 三條件復(fù)合檢測結(jié)果Fig.10 The effect of three-condition composite detection

從圖10可以明顯看出，縱坐標(biāo)值在0和1兩個(gè)數(shù)字極端的跳變狀態(tài)基本符合所選取的6步運(yùn)動(dòng)數(shù)據(jù)的實(shí)際情況，這表明同時(shí)使用比力與角速度信息的多條件檢測方法提高了檢測結(jié)果的準(zhǔn)確度。同樣地，圖10某一時(shí)刻縱坐標(biāo)值為1時(shí)檢測為運(yùn)動(dòng)狀態(tài)，值為0時(shí)為靜止?fàn)顟B(tài)。

基于第2節(jié)所設(shè)計(jì)的PNS算法，以第3節(jié)搭建的行人導(dǎo)航系統(tǒng)作為載體進(jìn)行直線行走實(shí)驗(yàn)。實(shí)驗(yàn)地點(diǎn)為西安建筑科技大學(xué)南山操場，將行人導(dǎo)航系統(tǒng)安裝至步行者腳背部位置，并通過RS232-USB與PC端連接進(jìn)行實(shí)驗(yàn)數(shù)據(jù)的保存與采集，行人沿正北方進(jìn)行行走實(shí)驗(yàn)，行走時(shí)間為5min。圖11是進(jìn)行修正導(dǎo)航并結(jié)合磁力計(jì)航向信息進(jìn)行修訂之后所得的解算結(jié)果，以及未進(jìn)行修正的慣導(dǎo)解算結(jié)果對比圖。

(a) 直線行走姿態(tài)解算結(jié)果

(b) 直線行走速度解算結(jié)果

(c) 直線行走位置解算結(jié)果圖11 行走實(shí)驗(yàn)修正算法解算結(jié)果及純慣導(dǎo)解算結(jié)果對比圖Fig.11 Comparison diagram between the results of walking experiment modified algorithm and pure inertial navigation

分析圖11可知，行人直線向正北方向行走時(shí)，由修正算法解算出的俯仰角和橫滾角符合行人運(yùn)動(dòng)規(guī)律，以磁力計(jì)作為觀測量的航向角也與實(shí)際情況相符，300s行走時(shí)間內(nèi)經(jīng)緯度變化與實(shí)際情況相符合，解算高度最大值約為0.2m，符合人體行走正常抬腳變化值。而在沒有引入修正算法的情況下，300s行走時(shí)間內(nèi)，純慣性導(dǎo)航解算速度結(jié)果嚴(yán)重發(fā)散，并且其經(jīng)度、緯度以及高度變化與實(shí)際情況相差較大，MEMS IMU已完全失去導(dǎo)航功能。因此可得出如下結(jié)論：本文所設(shè)計(jì)的導(dǎo)航算法可有效且正確地用于MEMS行人導(dǎo)航系統(tǒng)中，并使系統(tǒng)的導(dǎo)航精度得到較大提升。

5 結(jié) 論

以地圖匹配作為輔助的衛(wèi)星導(dǎo)航是當(dāng)前較為常用的導(dǎo)航系統(tǒng)，但在衛(wèi)星拒止環(huán)境中其難以完成導(dǎo)航任務(wù)，為一種非自主式的導(dǎo)航方法。因此，本文展開了基于MEMS慣性器件的鞋式行人導(dǎo)航系統(tǒng)的研究工作，該系統(tǒng)無需提前布置基礎(chǔ)設(shè)施，僅依靠行人自身固聯(lián)傳感器進(jìn)行自主導(dǎo)航。將MEMS IMU固定在步行者的鞋上，根據(jù)步行者腳步運(yùn)動(dòng)特征，采用SINS與PNS結(jié)合的零速修正算法，同時(shí)，增加磁力計(jì)方位信息進(jìn)行了MEMS IMU實(shí)驗(yàn)驗(yàn)證。零速檢測算法檢驗(yàn)效果實(shí)驗(yàn)表明本文所設(shè)計(jì)的復(fù)合條件檢測方法與比力模值零速判斷方法、比力方差零速判斷方法、角速度模值零速判斷方法等單一條件靜止檢測算法相比，提高了檢測結(jié)果的準(zhǔn)確度。進(jìn)行的半實(shí)物實(shí)驗(yàn)驗(yàn)證最終實(shí)驗(yàn)結(jié)果表明，本文所設(shè)計(jì)的鞋式行人導(dǎo)航算法行之有效，系統(tǒng)可以滿足行人導(dǎo)航需求。這對GPS缺失環(huán)境下的人體定位具有十分重要的意義，并為鞋式行人導(dǎo)航系統(tǒng)的工程化奠定了重要的理論與實(shí)踐基礎(chǔ)。