基于MEMS慣性傳感器的行人導航軌跡復現研究*

趙 慧， 王 斌， 阮 巍

(重慶郵電大學 光電工程學院，重慶 400065)

0 引 言

慣性導航系統(inertial navigation system,INS)可滿足無線電信號傳播困難的地方對精確導航的需求。行人慣性導航是一種通過微機電系統(micro-electro-mechanical system,MEMS)慣性傳感器采集行人運動信息，并運用計算機進行導航解算，及時更新行人的行走方向、位置和速率等信息，從而為行人提供一種可視化導航信息的方法。最近幾年，MEMS慣性導航高速發展，體積和功耗不斷減小的同時成本也大幅降低，使得MEMS慣性器件成為行人導航的理想選擇[1]。

目前,行人慣性導航應用主要有2種方法。一種是采用行人航位推算(pedestrian dead reckoning，PDR)方法，即通過加速度輸出值來檢測行走的步數、近似步長，磁場或陀螺儀判斷航向，推算出行人的位置[2]。文獻[3]采用傳統慣導與零速檢測結合，由于進行濾波優化，最終誤差為8.2 %。文獻[4]采用步長、步數、航向的方法，最大誤差低于總行走距離的3.0 %，但定位精度很大程度上取決于方向角精度以及行人的步行習慣。文獻[5]采用了互補濾波將數據融合得到軌跡，行走過程中方向會轉變360°，造成較大誤差，誤差控制在6.3 %。文獻[6]采用了粒子濾波算法融合室內地圖，室內誤差為3.0 %，但航向角不可觀測，軌跡容易發生穿墻現象，導致軌跡漂移。文獻[7]給出了較好的結果，但采用經驗性的方法由于不能識別不同行人的步態變化，故當采用與以往不同的方式進行移動時算法失效。

另一種方法為傳統的捷聯式慣性導航，即將慣性測量單元(intertial measurement unit,IMU)固定在鞋上，通過處理IMU輸出的加速率、角速率值，預測行人的姿態信息和位置[9],此方法不會因為人的改變而失效。因此,本文采用捷聯式方法進行實驗。將慣性傳感器固定于腳面，得到濾波后的加速率和角速率值，實現方位角的修正，從而提高行人導航的定位精度，行走一圈平均誤差控制在3 %、行走多圈平均誤差在5 %以內。

1 方案設計

為選取合適的慣性測量單元，需考慮多種因素，如重量、體積、采樣率、成本及功耗等。本文選取的慣性傳感器模塊為一集成三軸加速度計與三軸陀螺儀的MPU6050和一三軸數字羅盤HMC5883L。

硬件系統以開源電子原型平臺ARDUINO UNO為核心，軟件環境為MATLAB。方案設計流程如圖1所示，通過三軸加速度計與三軸陀螺儀采集數據，一方面利用捷聯式慣性計算，另一方面進行零速檢測，滿足零速修正(zero velocity update,ZUPT)條件時則觸發卡爾曼濾波，與捷聯式慣性計算融合，最終得到行人導航位置軌跡。

圖1 方案設計流程

2 算法實現

為實現完整的行人慣性導航：1)對坐標進行轉換，將載體坐標系轉換到導航坐標系中，在導航坐標系得到姿態角；2)對經過濾波的加速率和角速率進行零速檢測，通過改進的卡爾曼濾波處理，得到行人的慣性導航。

2.1 姿態角初步估計

行人行走是一種低速運動，因此忽略地球自轉所帶來的影響，只在空間內定義2種坐標系：導航坐標系(n系)和載體坐標系(b系)。

n系為當地水平坐標系，x軸指向北，y軸指向西，z軸與x,y軸滿足右手定則指向天，即為北西天坐標系，行人行走的方向和速率均在系內推算。b系三軸與慣性測量單元(inertial measurement unit,IMU)固定，并隨著載體的運動而變化，定義x軸為前向，y軸為左向，z軸與x,y軸成右手定則，原點O在IMU的重心，陀螺儀和加速度計的初始值均基于載體坐標系[9]。

(1)

圖2 載體—導航坐標轉換

2.2 零速檢測

零速檢測指當行人行走腳落地過程或者行人靜止狀態下，束有INS的腳在落地期間，理論上,其角速率和加速率均為零。通過檢測落腳瞬間，可以人為消除陀螺儀和加速度計輸出值的累積誤差。

為了精確判斷零速度時刻，提高定位精度，本文采用三條件判斷法，即三軸總加速度檢測、三軸總加速度方差檢測和三軸總角速度檢測。以“1”表示零速度狀態，即腳完全落地的狀態。

2.2.1 三軸總加速度檢測

當腳落地靜止時，基本只受重力作用，此時三軸總加速度的幅值處于2個閾值之間

(2)

其中

(3)

式中 |ak|為三軸總加速度;thamin,thamax為加速度區間閾值。

2.2.2 三軸總加速度方差檢測

行人步行過程中，加速度劇烈變化，行人靜止時刻，加速度變化平緩，此時三軸總加速度方差處于一個閾值如下

(4)

(5)

則

(6)

式中thσmax為加速度方差的閾值。

2.2.3 三軸總角速度檢測

行人步行過程中，角速度劇烈變化，靜止時刻，角速度幾乎為零，此刻三軸總角速度幅值處于一個閾值范圍內

(7)

(8)

式中thωmax為角速度閾值。

上述3個條件通過邏輯“與”(&)運算，即可得到最終落地點，即零速度的時刻

C=C1&C2&C3

(9)

3個判斷條件中，設定結果為“1”時表示速度為零，即為腳全部落地時刻。圖3為零速檢測結果，前3行依次為總加速度檢測結果C1、總加速度方差檢測C2、總角速度檢測C3，第四行為前三行進行邏輯“與”運算得到的結果C。

圖3 零速檢測結果

2.3 卡爾曼濾波優化算法

為了預測零速度時刻的誤差，對于卡爾曼濾波，設置其在k時刻的15個狀態向量狀態?xk=[?γk?vk?φk?ak?wk]。其中,?γk,?vk,?φk分別為位置、速度和姿態向量，而?ak,?wk分別為加速度和陀螺儀的漂移錯誤值[10]。

2.3.1 時間更新

通過加速度和角速度采樣值進行導航解算，更新當前狀態向量狀態?xk以及相應的四元數q，并根據結果計算卡爾曼濾波中的狀態轉移矩陣Fk、系統噪聲增益矩陣Gk以及協方差矩陣P。

狀態轉移矩陣為

(10)

式中St為加速度的斜對稱矩陣，矩陣元素依據轉換成導航坐標系的加速度值創建；B1，B2則分別為加速度和陀螺儀的偏差相關常數。

(11)

(12)

2.3.2 測量更新

計算當前卡爾曼增益，并將速度信息作為系統速度誤差的測量值，再根據狀態估計更新方程對行人的速度、位置進行重新估計，同時計算系統的姿態矩陣，最終得到新的速度、位置和姿態角信息。

3 實驗驗證

為了驗證設計的行人導航系統的可行性，在室外繞圓形路線，在室內繞矩形路線進行行走實驗。

3.1 圓形路線測試

實際路線為繞直徑為13.00 m的圓形，繞實際路徑進行順時針行走一圈得到結果如圖4所示，測量的誤差為2.8 %，實驗行走的落腳點落在實際路線的外圍，原因為左腳位于人體中心左側，與實際情況相符，最終導航結果滿足導航要求。

圖4 圓形軌跡落腳點軌跡

3.2 方形路線測試

實際矩形路線為6.20 m×6.70 m，如圖6中點劃線所示。圖5為僅用加速率和角速率直接累積得到的導航位置，誤差結果很大。圖6為經過修正得到的位置軌跡，誤差主要產生在轉彎處，測得誤差為2.8 %，滿足導航要求。圖7為繞矩形方塊行走3圈的軌跡，計算誤差為4.2 %，產生誤差的原因主要包括：磁力計的偏差、步行運動的抖動以及轉彎時產生的偏差。

圖5 矩形形狀直接解算軌跡

圖6 矩形形狀單圈導航軌跡

圖7 矩形形狀3圈導航軌跡

3.3 對比結果

為了驗證算法的準確性，將本文測量結果與其他文獻結果進行比較，如表1所示。

表1 不同方法得到的軌跡對比 %

4 結 論

采用低精度的MEMS慣性器件，通過分析行人行走的步態特征，利用卡爾曼濾波算法和零速檢測，實現了行人行走的軌跡復現。實驗結果表明:一次行走路徑圓形與矩形時測量誤差均小于3 %，較好地滿足了行人導航的需求。軌跡復現的過程中有一定延時，原因為行人按實際路徑行走時傳感器采集數據上傳至MATLAB軟件，MATLAB軟件運算產生了延遲，如何減少延遲時間成為下一步的工作目標。

[1] Hazas M,Scott J,Krumm J.Location-aware computing comes of age[J].Computer,2004,37(2):95-97.

[2] Jimenez A R.Indoor pedestrian navigation using an INS and EKF framework for yaw drift reduction and a foot-mounted IMU[C]∥Positioning Navigation and Communication,2010.

[3] 萬駿煒,曾慶化,陳磊江,等.行人慣性導航系統平臺設計與實現[J].計算機應用與軟件,2015,1(2):45-47.

[4] 李金鳳,王慶輝,劉曉梅,等.基于MEMS慣性傳感器的行人航位推算系統[J].傳感器與微系統,2014,33(12):85-87.

[5] 肖永健,肖 力,孫志剛，等.基于步行者航位推算的井下人員輔助定位[J].太赫茲科學與電子信息學報,2013，6(4):583-587.

[6] 崔 瀟,秦永元,周 琪,等.鞋式個人導航系統算法和試驗研究[J].測控技術,2013,32(3):138-142.

[7] Renaudin V,Yalak O,Tomé P,et al.Indoor navigation of emergency agents[J].European Journal of Navigation,2007,5(3):52-58.

[8] Caruso J.Application of magneto resistive sensors in navigation systems[J].Sensors and Actuators,1997,1220(1):15-21.

[9] 秦永元.慣性導航[M].2版，北京:科學出版社,2006:5-8.

[10] 楊 輝.基于MEMS傳感器的高精度行人導航算法研究[D].廈門：廈門大學,2014.