MEMS慣性鞋式行人導航系統*

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

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

0 引 言

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

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

本文設計了一種基于MEMS慣性測量組件的行人導航算法，將MEMS慣性測量組件安裝于人體足部，在足部著地期間通過零速檢測以及零速修正算法估計并修正MEMS慣性導航系統及MEMS慣性器件誤差。此外，增加磁強計方位信息，以實現方位誤差估計。

1 行人導航系統方案設計

1.1 慣性行人導航系統算法結構

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

圖1 PNS導航系統修正算法Fig.1 Navigation algorithm structure of PNS

慣導行人系統算法的研究核心在于步態檢測和零速修正算法設計，國內外學者設計了不同的步態檢測方法和零速修正方法來提高系統的長航時導航精度[9]。

1.2 MEMS器件安裝位置分析

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

(a) 內置安裝

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

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

2 行人導航系統算法設計

2.1 行人步態特征分析

行人正常水平行走的步態是近似于周期性的一個過程，每一個完整的步態周期中均有四個關鍵的運動學事件[13]：腳跟著地(Heel-Strike，HS)、全腳著地(Foot-Flat，FF)、腳跟離地(Heel-Off，HO)、腳尖離地(Toe-Off，TO)。圖3為行人行走的一個周期性過程。

圖3 行走步態的關鍵事件Fig.3 Key events of walking gait

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

2.2 零速檢測算法設計

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

(1)

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

(2)

(3)

(4)

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

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

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

2.3 零速修正卡爾曼濾波器設計

設系統狀態空間表達式為

(5)

其中，F(t)代表狀態轉移矩陣，X(t)代表狀態矩陣，W(t)代表輸入矩陣，H(t)代表系統觀測矩陣，V(t)代表系統觀測噪聲。

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

(6)

卡爾曼濾波算法為[16]

(7)

(8)

(9)

(10)

(11)

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

(12)

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

Zk=HkXk+Vk

(13)

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

Δψk=ψk-ψmagk

(14)

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

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

(15)

其中，

零速修正卡爾曼濾波器可實現如下三種功能：速度誤差修正、水平姿態誤差修正以及運動過程中的累積位置誤差的修正。

3 硬件平臺搭建

3.1 硬件平臺搭建

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

圖5 硬件總體結構框圖Fig.5 Hardware overall structure block diagram

硬件結構以STM32為核心并與IMU完成數據的采集，計算得出導航信息并傳輸至PC端。為在初始化階段得到準確度更高的高度通道信息，又借助MS5611氣壓高度計對初始高度信息進行采集。STM32與計算機的通信這里采用串口轉USB有線模式，這種方法主要在實驗前期數據采樣的階段應用，為實驗驗證階段做準備。

3.2 PCB設計與系統硬件調試

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

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

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

所搭建的硬件系統實物圖如圖7所示。

圖7 硬件系統實物圖Fig.7 Hardware system physical diagram

4 實驗驗證

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

圖8 完整行人導航系統Fig.8 Pedestrian navigation system

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

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

從圖9可以看出，僅使用單一零速檢測條件的方法在某一較短時間區間內的縱坐標值在0和1兩個數字極端反復跳變，這表明存在較為明顯的誤檢與漏檢情況。即將運動狀態判斷為靜止，靜止狀態判斷為運動。需要特別說明，當圖9某一時刻縱坐標值為1時檢測為運動狀態，值為0時為靜止狀態。

下面給出使用三條件進行零速檢測的檢測結果，如圖10所示。

圖10 三條件復合檢測結果Fig.10 The effect of three-condition composite detection

從圖10可以明顯看出，縱坐標值在0和1兩個數字極端的跳變狀態基本符合所選取的6步運動數據的實際情況，這表明同時使用比力與角速度信息的多條件檢測方法提高了檢測結果的準確度。同樣地，圖10某一時刻縱坐標值為1時檢測為運動狀態，值為0時為靜止狀態。

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

(a) 直線行走姿態解算結果

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

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

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

5 結 論

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