多傳感器行人航位推算算法研究*

陳新東 蔡 鵬 高大遠(yuǎn) 王 超 朱海榮

（海軍潛艇學(xué)院 青島 266000）

1 引言

全球定位系統(tǒng)（GPS）能提供三維位置和速度信息，已經(jīng)廣泛應(yīng)用于人們的日常生活中，但GPS信號容易受衛(wèi)星信號的衰減以及干擾信息的影響，在室內(nèi)、地下建筑以及城市街道等環(huán)境下信號弱，信息可用性差，GPS不能進(jìn)行準(zhǔn)確的定位［1］。目前有很多學(xué)者對室內(nèi)定位技術(shù)進(jìn)行了研究，如WiFi定位技術(shù)、超寬帶技術(shù)、超聲波技術(shù)、視覺定位、紅外技術(shù)、FRID定位、藍(lán)牙技術(shù)等定位傳感技術(shù)［2～7］。然而多傳感器的室內(nèi)定位技術(shù)是比較熱門的一種定位手段。當(dāng)前，國內(nèi)外有不少學(xué)者對多傳感器定位技術(shù)進(jìn)行了研究。Foxlin等開發(fā)了”NavSheo”的定位系統(tǒng)［8］；鄧林坤等設(shè)計了低成本多傳感器的室內(nèi)行人航位推算（PDR）系統(tǒng)，并且在ipad上進(jìn)行了實(shí)驗(yàn)［9］；Ruiz和 Madrid 等利用慣性導(dǎo)航系統(tǒng)與RFID定位系統(tǒng)進(jìn)行結(jié)合實(shí)現(xiàn)了室內(nèi)行人的定位［10］。

本文提出了一種基于足綁式的多傳感器PDR算法，并采用磁角速率更新（MARU）很好地克服了磁干擾的影響，利用慣性測量單元（IMU）對行人的線運(yùn)動以及角運(yùn)動等信息進(jìn)行采集，同時利用三軸磁力計來計算得到對應(yīng)的磁感應(yīng)電勢信息，采用EKF算法對各個傳感器的誤差進(jìn)行補(bǔ)償與修正，最終推算得到行人的位置。

2 初始對準(zhǔn)

靜止?fàn)顟B(tài)時，利用加速度計與陀螺儀的輸出進(jìn)行初始對準(zhǔn)。加速度計測量重力加速度，可據(jù)此來計算俯仰角和橫滾角。陀螺儀測量地球自轉(zhuǎn)角速度，可計算得到航向角。但是對于低成本的MEMS IMU來說，MEMS IMU陀螺儀的漂移非常大，用陀螺儀的輸出來計算航向角，其誤差非常大。因此本文采用靜止條件下加速度計和磁力計獲得初始IMU的姿態(tài)角，導(dǎo)航坐標(biāo)系（n系）為北西天坐標(biāo)系，載體坐標(biāo)系（b系）固連在傳感器上。一般情況下，由加速度計和磁力計的輸出可得：

3 系統(tǒng)模型

3.1 狀態(tài)模型

行人導(dǎo)航系統(tǒng)狀態(tài)方程與狀態(tài)向量呈非線性關(guān)系，可以進(jìn)行線性化處理［11-13］。系統(tǒng)誤差狀態(tài)向量可以表示為

式中：δΨ 、Δω 、δr、δr?、Δa分別代表k時刻的姿態(tài)誤差信息，陀螺零偏信息，位置偏差信息，速度偏差信息，加計零偏信息，總共15維信息當(dāng)做系統(tǒng)的誤差狀態(tài)向量。

經(jīng)過線性化處理得狀態(tài)方程：

式中：Xk-1代表k-1時刻的誤差狀態(tài)向量后驗(yàn)估計，Xk代表k時刻誤差狀態(tài)向量先驗(yàn)估計，Wk-1代表k-1時刻的過程噪聲，它的協(xié)方差矩陣是Qk=E(WkWkT)，Φk/k-1則代表狀態(tài)轉(zhuǎn)移矩陣。其表達(dá)式如下：

其中：

ab(k)=Acc(k)-Δa為EKF修正后的比力，為k時刻加計的量測值，是k時刻b系經(jīng)過坐標(biāo)變換到n系方向余弦矩陣的先驗(yàn)估計。

3.2 量測模型

當(dāng)足部處于靜止或者速度為0時，此時相對于地面，速度接近于0，ZUPT算法把速度誤差Δvk,k-1=vk,k-1-[0,0,0]T作為EKF的觀測量。

在室內(nèi)這種環(huán)境下，磁的干擾不容易估計，很容易會受到室內(nèi)建筑等設(shè)施的干擾從而引入更多的誤差，為了避免外界磁干擾的影響，引入MARU來進(jìn)行航向修正，即行人足部處于零速或靜止時，磁感應(yīng)強(qiáng)度恒定不變。而MEMS轉(zhuǎn)動時，三軸磁力計分量會發(fā)生旋轉(zhuǎn)，且旋轉(zhuǎn)速率與陀螺儀角速率ω(k)變化有關(guān)，所以將磁感應(yīng)強(qiáng)度變化作為量測矢量，對ω(k)進(jìn)行誤差估計補(bǔ)償來抑制航向角的發(fā)散。

當(dāng)MEMS轉(zhuǎn)動時，磁增量為

式中：Bb(k)和Bb(k-1)分別表示k時刻和k-1時刻的在載體坐標(biāo)系下磁感應(yīng)強(qiáng)度。

4 步態(tài)檢測

行人行走時，走的步態(tài)是周期性變化的；本實(shí)驗(yàn)對行人行走的數(shù)據(jù)進(jìn)行采集，每個傳感器的三個軸都具有周期性變化的特征。本文利用三個傳感器，分別是加速度計、陀螺儀以及磁力計，通過三者結(jié)合對行人行走進(jìn)行檢測測定。

其中γα加速度幅值閾值，γω為角速度幅值閾值，Δt為采樣時間間隔。

通過上述判定條件，行人步態(tài)檢測如圖1所示。

圖1 步態(tài)檢測

文中僅僅截取了一小段步態(tài)檢測數(shù)據(jù)用來進(jìn)行描述與分析，圖中X軸代表傳感器采樣樣本數(shù)，Y軸代表數(shù)值大小，無固定含義。從圖中可以看出，行人行走步態(tài)呈現(xiàn)出周期性變化規(guī)律，步態(tài)靜止時，行人靜止，步態(tài)移動時，行人則處于移動狀態(tài)。

5 實(shí)驗(yàn)數(shù)據(jù)結(jié)果分析

實(shí)驗(yàn)采用的IMU是XSENS公司的MTI產(chǎn)品，實(shí)驗(yàn)在室內(nèi)完成，本次實(shí)驗(yàn)將IMU綁在實(shí)驗(yàn)人員的足部，依據(jù)已經(jīng)設(shè)定好的路線行走，總共做了兩次實(shí)驗(yàn)，分別沿著不同路線行走，采樣頻率設(shè)置為100HZ，圖2和圖3分別為Matlab軟件下仿真的實(shí)驗(yàn)結(jié)果對比圖。

圖2 總路程28.8m測試對比結(jié)果

圖3 總路程110.7m測試對比結(jié)果

為了更加清晰地看出文中算法的效果，對兩種方法產(chǎn)生的定位誤差進(jìn)行相應(yīng)的統(tǒng)計，如表1所示。定位誤差采用實(shí)驗(yàn)路線終點(diǎn)值減去路線真實(shí)終點(diǎn)值，得到的絕對差值占總路程的比值來進(jìn)行表示。表1記錄了兩種不同方法的對比，其中初始對準(zhǔn)時間為120s。從表中定位誤差結(jié)果對比可得：在EKF基礎(chǔ)上的ZUPT+MARU融合的方法，對位置誤差具有改善作用，定位精度可以得到一定的提高。

表1 100HZ兩種算法IMU數(shù)據(jù)的計算結(jié)果比較

6 結(jié)語

文中提出了一種多傳感器行人航位推算方法，該方法可以減少加計零偏，同時修正行人步態(tài)誤差；對陀螺儀角速率進(jìn)行誤差估計補(bǔ)償，抑制了外界磁干擾，增強(qiáng)了對行人航向跟蹤性能；通過引入ZUPT+MARU融合構(gòu)建量測模型，有效地修正行人行走時航向的偏差，實(shí)驗(yàn)結(jié)果表明：在短時間內(nèi)，該系統(tǒng)能夠滿足行人在室內(nèi)的定位需求，可以實(shí)現(xiàn)短距離的導(dǎo)航，位置誤差能夠控制在0.56%范圍內(nèi)，定位效果良好。