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

劉昶

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

1 引 言

目前發(fā)展比較完善的全球定位系統(tǒng)(GPS)技術(shù)，在一些易受遮擋、GPS信號(hào)干擾、室內(nèi)場(chǎng)合等環(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)境中，如通過加速度檢測(cè)行人的步長(zhǎng)和步幅，結(jié)合慣性傳感器提供的航向信息，通過航位推算確定行人位置(pedestrian Dead Reckoning，PDR)[2～4]。Robertson等提出基于鞋底安裝慣性傳感器的貝葉斯定位算法框架，采用卡爾曼濾波估計(jì)行人位置[5]；John-OlofNilssion等提出了基于鞋綁式零速度修正(Zero Velocity Update，ZUPT)輔助的慣性導(dǎo)航系統(tǒng)，提供開源的代碼，但算法參數(shù)設(shè)置繁多，對(duì)不同的環(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)動(dòng)的旋轉(zhuǎn)和平移，提出對(duì)三軸動(dòng)態(tài)方向估計(jì)濾波融合修正，優(yōu)化設(shè)計(jì)行人行走中零速度檢測(cè)算法，通過對(duì)行人靜止判斷，進(jìn)行加速度方向校準(zhǔn)和行人速度漂移誤差的剔除，最后對(duì)時(shí)刻速度積分獲取精確的位置信息，從而提高系統(tǒng)定位的精度。

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

當(dāng)前MEMS IMU的應(yīng)用精度相對(duì)較低，進(jìn)行獨(dú)立的慣性導(dǎo)航解算時(shí)，其解算的位置積累誤差隨著時(shí)間成三次方發(fā)散，從而失去慣性傳感器導(dǎo)航定位的現(xiàn)實(shí)意義，因此采用零速修正(ZUPT)可有效解決慣性傳感器的積累誤差的三次方發(fā)散問題。行人行走過程如圖1所示： 從運(yùn)動(dòng)可以看出， 行人行走的明顯規(guī)律為“運(yùn)動(dòng)—靜止—運(yùn)動(dòng)—靜止……”的過程，腳底A與地面接觸時(shí)會(huì)有短暫的靜止時(shí)間，從時(shí)刻T=0.48 s到T=0.72 s的行人短暫△T時(shí)間段內(nèi)為靜止?fàn)顟B(tài)，此時(shí)行人速度VA=0，針對(duì)△T靜止時(shí)間段內(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 行走動(dòng)態(tài)階段

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

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

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

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

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

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

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

方向是基于AHRS系統(tǒng)進(jìn)行估計(jì)，采用慣性傳感器的三軸加速度計(jì)和三軸陀螺儀，分別量測(cè)載體坐標(biāo)系下三軸上的加速度和角速率，利用陀螺儀補(bǔ)償角解算出量測(cè)誤差的四元數(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 零速度檢測(cè)算法

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

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

(9)

(2)檢測(cè)算法公式。

(10)

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

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

(11)

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

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

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

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

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

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

(12)

(13)

(14)

(15)

則式(14)中可簡(jiǎn)化為：

(16)

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

(17)

(18)

(19)

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

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

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

(20)

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

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

vt=vt-1+at△t

(21)

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

v=v-vD

(22)

③計(jì)算行人位置。對(duì)最終求得的速度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型號(hào)，固定于足部，如圖6所示。實(shí)驗(yàn)為閉合路徑，共四次拐彎，起點(diǎn)為西北處，沿樓道連續(xù)行走兩圈，數(shù)據(jù)量共166.04×2 m。

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

利用MATLAB軟件編寫上述算法，實(shí)現(xiàn)INS(未修正)、INS+ZUPT(修正，包括方向和速度修正)與實(shí)際樓道路徑對(duì)比實(shí)驗(yàn)的方法。圖7為實(shí)驗(yàn)行走路徑對(duì)比結(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)色路徑為行走一圈路徑，隨時(shí)間推移已嚴(yán)重偏離實(shí)際路徑，且位置誤差積累不斷增大，主要原因是未經(jīng)方向誤差補(bǔ)償修正和速度修正導(dǎo)致。則紅色路徑是采用互補(bǔ)濾波融合方向修正計(jì)算，能有效補(bǔ)償陀螺儀的漂移誤差和濾除加速度計(jì)的高頻錯(cuò)誤，從而獲取精確的方向，并結(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ì)零速檢測(cè)算法(GLRT)，并觸發(fā)ZUPT更新，完成跟蹤定位。在對(duì)缺少GPS信號(hào)的室內(nèi)和室外環(huán)境均可有效的較長(zhǎng)時(shí)間進(jìn)行跟蹤定位，具有重要的意義。