基于慣性傳感器行人室內(nèi)定位導(dǎo)航算法研究

劉昶

(合肥市測繪設(shè)計(jì)研究院，安徽 合肥 230000)

1 引 言

目前發(fā)展比較完善的全球定位系統(tǒng)(GPS)技術(shù)，在一些易受遮擋、GPS信號干擾、室內(nèi)場合等環(huán)境下無法提供有效的定位服務(wù)，而基于微機(jī)電系統(tǒng)(micro-electro-mechanic system，MEMS)的慣性傳感器的定位技術(shù)能很好解決這一問題，慣性系統(tǒng)是一種自主導(dǎo)航系統(tǒng)，它能夠提供連續(xù)性好、短期精度高和穩(wěn)定的導(dǎo)航信息(如位置、航向、姿態(tài)角數(shù)據(jù))[1]。隨著慣性傳感器不斷在人們生活中得到應(yīng)用和發(fā)展，特別在室內(nèi)的環(huán)境中，如通過加速度檢測行人的步長和步幅，結(jié)合慣性傳感器提供的航向信息，通過航位推算確定行人位置(pedestrian Dead Reckoning，PDR)[2～4]。Robertson等提出基于鞋底安裝慣性傳感器的貝葉斯定位算法框架，采用卡爾曼濾波估計(jì)行人位置[5]；John-OlofNilssion等提出了基于鞋綁式零速度修正(Zero Velocity Update，ZUPT)輔助的慣性導(dǎo)航系統(tǒng)，提供開源的代碼，但算法參數(shù)設(shè)置繁多，對不同的環(huán)境定位應(yīng)用帶來不便[6]；Skog I，H?ndel P等研究ZUPTs在輔助慣性導(dǎo)航系統(tǒng)中局限性和傳感器誤差建模影響分析[7]。

本文以航向姿態(tài)參考系統(tǒng)(AHRS)為基礎(chǔ)，IMU(Inertial Measurement Unit)的三軸陀螺儀和三軸加速度計(jì)追蹤載體運(yùn)動的旋轉(zhuǎn)和平移，提出對三軸動態(tài)方向估計(jì)濾波融合修正，優(yōu)化設(shè)計(jì)行人行走中零速度檢測算法，通過對行人靜止判斷，進(jìn)行加速度方向校準(zhǔn)和行人速度漂移誤差的剔除，最后對時刻速度積分獲取精確的位置信息，從而提高系統(tǒng)定位的精度。

2 行人步態(tài)跟蹤定位算法結(jié)構(gòu)

當(dāng)前MEMS IMU的應(yīng)用精度相對較低，進(jìn)行獨(dú)立的慣性導(dǎo)航解算時，其解算的位置積累誤差隨著時間成三次方發(fā)散，從而失去慣性傳感器導(dǎo)航定位的現(xiàn)實(shí)意義，因此采用零速修正(ZUPT)可有效解決慣性傳感器的積累誤差的三次方發(fā)散問題。行人行走過程如圖1所示： 從運(yùn)動可以看出， 行人行走的明顯規(guī)律為“運(yùn)動—靜止—運(yùn)動—靜止……”的過程，腳底A與地面接觸時會有短暫的靜止時間，從時刻T=0.48 s到T=0.72 s的行人短暫△T時間段內(nèi)為靜止?fàn)顟B(tài)，此時行人速度VA=0，針對△T靜止時間段內(nèi)進(jìn)行零速度修正，其修正效果如圖2(a)，其中慣性導(dǎo)航系統(tǒng)(INS)是未經(jīng)零速度修正的速度誤差呈三次方發(fā)散。INS+ZUPT為經(jīng)過零速度修正后誤差發(fā)散情況，由此可得，經(jīng)過零速度修正后的速度獲取的位置信息，如圖2(b)所示，位置誤差與行人步數(shù)成一定的線性相關(guān)。

圖1 行走動態(tài)階段

圖2 零速修正結(jié)果

本文將MEMS慣性傳感組件安裝于人體足部，結(jié)合人體足部行走特征，通過互補(bǔ)濾波融合的方向估計(jì)修正算法，及足部零速檢測判斷，構(gòu)成基于方向和零速修正的慣性輔助行人跟蹤定位系統(tǒng)。該系統(tǒng)主要為三部分組成：

①航向姿態(tài)更新算法。方向估計(jì)更新在AHRS系統(tǒng)上，采用方向四元數(shù)(Quaternion)算法解算，通過MEMS慣性傳感器的三軸加速度計(jì)和三軸陀螺儀的互補(bǔ)濾波融合對載體的三軸姿態(tài)進(jìn)行精確方向估計(jì)。與歐拉角算法相比，避免采用三角函數(shù)運(yùn)算，既保證更新速率，計(jì)算時也不會出現(xiàn)奇異性

②零速度檢測算法。零速度檢測算法通過MEMS慣性傳感器的三軸加速度值檢測行人行走過程中足部與地面接觸時間段內(nèi)的零速度，本次實(shí)驗(yàn)設(shè)計(jì)廣義似然比檢測算法(GLRT)，通過設(shè)置零速檢測閾值判斷是否處于零速度狀態(tài)，為零速更新提供必要條件。對于正常行走步態(tài)可實(shí)現(xiàn)很好的檢測，但對于跳動、抖動的步態(tài)難以檢測，目前本文只研究正常行走步態(tài)。

③修正算法。修正算法主要是削弱圖2(a)和圖2(b)中的速度積累誤差和位置積累誤差的發(fā)散問題，通過對方向修正及零速、非零速時漂移誤差的剔除更新，獲取用于導(dǎo)航計(jì)算最終人體速度。

3 濾波融合方向估計(jì)算法

AHRS系統(tǒng)，是一種主要采用慣性傳感器數(shù)據(jù)測量空間姿態(tài)的傳感系統(tǒng)，一般情況，AHRS系統(tǒng)能夠確定載體的三軸姿態(tài)[10]。

方向是基于AHRS系統(tǒng)進(jìn)行估計(jì)，采用慣性傳感器的三軸加速度計(jì)和三軸陀螺儀，分別量測載體坐標(biāo)系下三軸上的加速度和角速率，利用陀螺儀補(bǔ)償角解算出量測誤差的四元數(shù)的微分形式，從而估計(jì)出地理坐標(biāo)下的方向四元數(shù)。

設(shè)三軸陀螺儀在載體坐標(biāo)系下的角速度為wx，wy，wz，則角速度向量Ww：

Ww=[0wxwywz]

(1)

(2)

(3)

(4)

(5)

則設(shè)誤差函數(shù)f：

(6)

通過迭代計(jì)算來估計(jì)方向四元數(shù)，即計(jì)算公式為：

(7)

(8)

4 零速度檢測算法

零速檢測的目的是檢測行走過程中速度為零的時段，即對圖1中△T的時間段內(nèi)VA=0的檢測，零速度檢測是觸發(fā)零速修正的前提，也是實(shí)現(xiàn)速度誤差清零關(guān)鍵步驟。崔瀟等文獻(xiàn)采用一種多條件閾值復(fù)合檢測算法(比力模值、比力方差、角速度模值)，只有三條件同時滿足，對零速檢測效果表現(xiàn)出更有效性[8]；Skog I，H?ndel P，Nilsson J O等通過與常用的加速度方差檢測、加速度幅度值檢測、角速度能量檢測方法相比較，它們都存在一定缺陷，而廣義似然比檢測方法適用性更有效[9]。

(1)加速度值和角速度值向量表示。

(9)

(2)檢測算法公式。

(10)

式(10)中g(shù)為重力加速度值。

(3)設(shè)置檢測閾值γ：

(11)

當(dāng)式(11)滿足條件，認(rèn)為速度為零(足部靜止?fàn)顟B(tài))，設(shè)置為1，非零速度設(shè)置為0(足部離開地面)。

對本文樓道采集的數(shù)據(jù)進(jìn)行零速檢測，截取部分時段數(shù)據(jù)如圖3所示，Stationary為零速檢測折線圖，數(shù)值為1表示為行人靜止?fàn)顟B(tài)，由Stationary折線圖可得，零速檢測對行走過程中靜止時刻能有效檢測。X、Y、Z分別為三軸加速度，X、Y軸加速度周期性的出現(xiàn)加速度為零(aA=0)，即對應(yīng)行人速度為零的時刻(vA=0)。

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

5 基于方向和零速修正輔助定位算法

本章節(jié)修正算法包括對方向估計(jì)后三軸方向的修正以及零速檢測后的速度修正，并對非零速狀態(tài)下的速度漂移改正，來約束行人在運(yùn)動過程中的方向漂移以及速度漂移，從而提高行人行走狀態(tài)下的位置更新精度。

5.1 互補(bǔ)濾波融合方向修正算法

(12)

(13)

(14)

(15)

則式(14)中可簡化為：

(16)

將式(2)、式(15)和式(16)帶入式(12)，可簡化為：

(17)

(18)

(19)

5.2 零速及速度漂移修正輔助定位

將MEMS慣性傳感器固定在行人足部，按照文中5.1節(jié)的互補(bǔ)濾波融合算法的方向修正和第4節(jié)的零速檢測的零速更新修正實(shí)現(xiàn)行人行走狀態(tài)下導(dǎo)航定位。

①計(jì)算平移加速度。通過5.1節(jié)的方向四元數(shù)估計(jì)，將加速度計(jì)量測的三軸加速度通過旋轉(zhuǎn)變換到地理坐標(biāo)系下，得到精確地理坐標(biāo)系下的三軸加速度a。即：

(20)

式中?表示將三軸加速度通過四元數(shù)乘積的旋轉(zhuǎn)變換計(jì)算。

②計(jì)算平移速度，將①中轉(zhuǎn)換得到的地理坐標(biāo)系中實(shí)際三軸加速度a，通過對a數(shù)值積分獲得速度v。

vt=vt-1+at△t

(21)

式(21)中，△t為采樣周期。在對通過積分求取的速度，并通過零速更新修正，當(dāng)判斷在某時刻速度為零時，速度設(shè)置為零，并觸發(fā)ZUPT更新機(jī)制，實(shí)現(xiàn)行人軌跡定位，同時本文考慮對非零時刻的速度，剔除非零速存在的整體漂移vD。

v=v-vD

(22)

③計(jì)算行人位置。對最終求得的速度v進(jìn)行積分獲取行人位置P。

Pt=Pt+vt△t

(23)

5.3 室內(nèi)導(dǎo)航定位算法流程

基于以上算法結(jié)構(gòu)，設(shè)計(jì)基于慣性傳感器的行人室內(nèi)導(dǎo)航定位算法系統(tǒng)的完整結(jié)構(gòu)圖，如圖4所示。

圖4 行人定位導(dǎo)航算法工作原理流程圖

6 實(shí)驗(yàn)結(jié)果及驗(yàn)證

為了驗(yàn)證設(shè)計(jì)的行人步態(tài)導(dǎo)航定位系統(tǒng)的有效性，本次實(shí)驗(yàn)區(qū)為連續(xù)的矩形樓道走廊，其實(shí)驗(yàn)區(qū)示意圖如圖5所示，慣性傳感器為X-IMU型號，固定于足部，如圖6所示。實(shí)驗(yàn)為閉合路徑，共四次拐彎，起點(diǎn)為西北處，沿樓道連續(xù)行走兩圈，數(shù)據(jù)量共166.04×2 m。

圖5 樓道示意圖 圖6 慣性傳感器與足部固定位置示意

利用MATLAB軟件編寫上述算法，實(shí)現(xiàn)INS(未修正)、INS+ZUPT(修正，包括方向和速度修正)與實(shí)際樓道路徑對比實(shí)驗(yàn)的方法。圖7為實(shí)驗(yàn)行走路徑對比結(jié)果。

圖7 兩圈矩形軌跡定位結(jié)果

表1為在矩形樓道中連續(xù)行走兩圈，回到最初起點(diǎn)的路徑統(tǒng)計(jì)結(jié)果，第一圈為各拐彎路徑的行走結(jié)果，然后統(tǒng)計(jì)第一圈和第二圈的統(tǒng)計(jì)結(jié)果。

表1 行走距離結(jié)果

在圖7中得：在實(shí)驗(yàn)中，藍(lán)色路徑為行走一圈路徑，隨時間推移已嚴(yán)重偏離實(shí)際路徑，且位置誤差積累不斷增大，主要原因是未經(jīng)方向誤差補(bǔ)償修正和速度修正導(dǎo)致。則紅色路徑是采用互補(bǔ)濾波融合方向修正計(jì)算，能有效補(bǔ)償陀螺儀的漂移誤差和濾除加速度計(jì)的高頻錯誤，從而獲取精確的方向，并結(jié)合零速修正，解決速度誤差積累，且與實(shí)際路徑相吻合，有效實(shí)現(xiàn)行人步態(tài)導(dǎo)航定位。在實(shí)驗(yàn)中，兩圈矩形路徑行走的最后閉合差約為 1.6 m，其定位精度均在1.1%以內(nèi)。

7 結(jié) 語

基于MEMS的慣性傳感器行人步態(tài)室內(nèi)導(dǎo)航定位算法的實(shí)現(xiàn)，主要采用基于AHRS系統(tǒng)的互補(bǔ)濾波融合方向估計(jì)算法和零速修正技術(shù)完成行人跟蹤定位的過程，本文重點(diǎn)論述了在AHRS系統(tǒng)上，融合MEMS慣性傳感器的加速度計(jì)和陀螺儀的數(shù)據(jù)提出一種濾波融合方向估計(jì)算法，有效補(bǔ)償陀螺儀漂移，并有效的計(jì)算出全局方向估計(jì)，結(jié)合足部慣性傳感器的加速度和角速度特性設(shè)計(jì)零速檢測算法(GLRT)，并觸發(fā)ZUPT更新，完成跟蹤定位。在對缺少GPS信號的室內(nèi)和室外環(huán)境均可有效的較長時間進(jìn)行跟蹤定位，具有重要的意義。