趙 慧， 王 斌， 阮 巍

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

0 引 言

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

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

另一種方法為傳統(tǒng)的捷聯(lián)式慣性導(dǎo)航，即將慣性測量單元(intertial measurement unit,IMU)固定在鞋上，通過處理IMU輸出的加速率、角速率值，預(yù)測行人的姿態(tài)信息和位置[9],此方法不會(huì)因?yàn)槿说母淖兌АＲ虼?本文采用捷聯(lián)式方法進(jìn)行實(shí)驗(yàn)。將慣性傳感器固定于腳面，得到濾波后的加速率和角速率值，實(shí)現(xiàn)方位角的修正，從而提高行人導(dǎo)航的定位精度，行走一圈平均誤差控制在3 %、行走多圈平均誤差在5 %以內(nèi)。

1 方案設(shè)計(jì)

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

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

圖1 方案設(shè)計(jì)流程

2 算法實(shí)現(xiàn)

為實(shí)現(xiàn)完整的行人慣性導(dǎo)航：1)對坐標(biāo)進(jìn)行轉(zhuǎn)換，將載體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中，在導(dǎo)航坐標(biāo)系得到姿態(tài)角；2)對經(jīng)過濾波的加速率和角速率進(jìn)行零速檢測，通過改進(jìn)的卡爾曼濾波處理，得到行人的慣性導(dǎo)航。

2.1 姿態(tài)角初步估計(jì)

行人行走是一種低速運(yùn)動(dòng)，因此忽略地球自轉(zhuǎn)所帶來的影響，只在空間內(nèi)定義2種坐標(biāo)系：導(dǎo)航坐標(biāo)系(n系)和載體坐標(biāo)系(b系)。

n系為當(dāng)?shù)厮阶鴺?biāo)系，x軸指向北，y軸指向西，z軸與x,y軸滿足右手定則指向天，即為北西天坐標(biāo)系，行人行走的方向和速率均在系內(nèi)推算。b系三軸與慣性測量單元(inertial measurement unit,IMU)固定，并隨著載體的運(yùn)動(dòng)而變化，定義x軸為前向，y軸為左向，z軸與x,y軸成右手定則，原點(diǎn)O在IMU的重心，陀螺儀和加速度計(jì)的初始值均基于載體坐標(biāo)系[9]。

(1)

圖2 載體—導(dǎo)航坐標(biāo)轉(zhuǎn)換

2.2 零速檢測

零速檢測指當(dāng)行人行走腳落地過程或者行人靜止?fàn)顟B(tài)下，束有INS的腳在落地期間，理論上,其角速率和加速率均為零。通過檢測落腳瞬間，可以人為消除陀螺儀和加速度計(jì)輸出值的累積誤差。

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

2.2.1 三軸總加速度檢測

當(dāng)腳落地靜止時(shí)，基本只受重力作用，此時(shí)三軸總加速度的幅值處于2個(gè)閾值之間

(2)

其中

(3)

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

2.2.2 三軸總加速度方差檢測

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

(4)

(5)

則

(6)

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

2.2.3 三軸總角速度檢測

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

(7)

(8)

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

上述3個(gè)條件通過邏輯“與”(&)運(yùn)算，即可得到最終落地點(diǎn)，即零速度的時(shí)刻

C=C1&C2&C3

(9)

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

圖3 零速檢測結(jié)果

2.3 卡爾曼濾波優(yōu)化算法

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

2.3.1 時(shí)間更新

通過加速度和角速度采樣值進(jìn)行導(dǎo)航解算，更新當(dāng)前狀態(tài)向量狀態(tài)?xk以及相應(yīng)的四元數(shù)q，并根據(jù)結(jié)果計(jì)算卡爾曼濾波中的狀態(tài)轉(zhuǎn)移矩陣Fk、系統(tǒng)噪聲增益矩陣Gk以及協(xié)方差矩陣P。

狀態(tài)轉(zhuǎn)移矩陣為

(10)

式中St為加速度的斜對稱矩陣，矩陣元素依據(jù)轉(zhuǎn)換成導(dǎo)航坐標(biāo)系的加速度值創(chuàng)建；B1，B2則分別為加速度和陀螺儀的偏差相關(guān)常數(shù)。

(11)

(12)

2.3.2 測量更新

計(jì)算當(dāng)前卡爾曼增益，并將速度信息作為系統(tǒng)速度誤差的測量值，再根據(jù)狀態(tài)估計(jì)更新方程對行人的速度、位置進(jìn)行重新估計(jì)，同時(shí)計(jì)算系統(tǒng)的姿態(tài)矩陣，最終得到新的速度、位置和姿態(tài)角信息。

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

為了驗(yàn)證設(shè)計(jì)的行人導(dǎo)航系統(tǒng)的可行性，在室外繞圓形路線，在室內(nèi)繞矩形路線進(jìn)行行走實(shí)驗(yàn)。

3.1 圓形路線測試

實(shí)際路線為繞直徑為13.00 m的圓形，繞實(shí)際路徑進(jìn)行順時(shí)針行走一圈得到結(jié)果如圖4所示，測量的誤差為2.8 %，實(shí)驗(yàn)行走的落腳點(diǎn)落在實(shí)際路線的外圍，原因?yàn)樽竽_位于人體中心左側(cè)，與實(shí)際情況相符，最終導(dǎo)航結(jié)果滿足導(dǎo)航要求。

圖4 圓形軌跡落腳點(diǎn)軌跡

3.2 方形路線測試

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

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

圖6 矩形形狀單圈導(dǎo)航軌跡

圖7 矩形形狀3圈導(dǎo)航軌跡

3.3 對比結(jié)果

為了驗(yàn)證算法的準(zhǔn)確性，將本文測量結(jié)果與其他文獻(xiàn)結(jié)果進(jìn)行比較，如表1所示。

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

4 結(jié) 論

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

[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] 萬駿煒,曾慶化,陳磊江,等.行人慣性導(dǎo)航系統(tǒng)平臺設(shè)計(jì)與實(shí)現(xiàn)[J].計(jì)算機(jī)應(yīng)用與軟件,2015,1(2):45-47.

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

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

[6] 崔 瀟,秦永元,周 琪,等.鞋式個(gè)人導(dǎo)航系統(tǒng)算法和試驗(yàn)研究[J].測控技術(shù),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] 秦永元.慣性導(dǎo)航[M].2版，北京:科學(xué)出版社,2006:5-8.

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