行人導(dǎo)航系統(tǒng)中的MEMS誤差在線修正技術(shù)

李清華，于文昭，謝陽光, 黃志威，李新年

(1.哈爾濱工業(yè)大學(xué)空間控制與慣性技術(shù)研究中心，哈爾濱 150001;2.飛行器控制一體化技術(shù)國防科技重點(diǎn)實(shí)驗(yàn)室，航空工業(yè)自控所，西安 710065)

0 引言

行人導(dǎo)航系統(tǒng)(Pedestrian Navigation System，PNS)，又名單兵導(dǎo)航系統(tǒng)，是導(dǎo)航中的一個(gè)重要組成部分，近年來受到越來越多的關(guān)注。由于其不受衛(wèi)星信號(hào)的約束，故能被應(yīng)用于商場(chǎng)等室內(nèi)場(chǎng)景。慣性行人導(dǎo)航系統(tǒng)通常采用微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System, MEMS)，具有成本低、體積小、攜帶方便[1]等優(yōu)點(diǎn)。但目前市面上的MEMS精度相對(duì)較低，長(zhǎng)時(shí)間的工作會(huì)導(dǎo)致誤差積累，從而影響定位精度[2]。傳統(tǒng)的標(biāo)定方法大多在實(shí)驗(yàn)室中進(jìn)行，需要將器件從設(shè)備或載體上拆下來，用實(shí)驗(yàn)室的轉(zhuǎn)臺(tái)等設(shè)備進(jìn)行標(biāo)定。這種方法的過程復(fù)雜，且耗時(shí)較長(zhǎng)[3]。另外，經(jīng)過一段時(shí)間，MEMS的標(biāo)定參數(shù)也會(huì)發(fā)生變化，影響使用性能。

為了減少隨著時(shí)間推移累計(jì)誤差的影響，提高行人導(dǎo)航系統(tǒng)的精度，F(xiàn)oxlin[4]提出了在靜止區(qū)間使用零速更新，輔助卡爾曼濾波的方法對(duì)誤差進(jìn)行修正,但精度有待提升。Fischer C[5]提出了利用無跡卡爾曼濾波器(Unscented Kalman Filter，UKF)進(jìn)行信息融合。時(shí)偉等提出了用2個(gè)微慣性測(cè)量單元(Miniature Inertial Measurement Unit，MIMU)分別固聯(lián)在單兵的雙腳上，利用基于假設(shè)檢驗(yàn)和極大似然估計(jì)的零速檢測(cè)器進(jìn)行零速檢測(cè)及修正[6]，但工作量大，操作復(fù)雜。Borestein J等提出了啟發(fā)式漂移消除算法(Heuristic Drift Elimination，HDE)[7]，將方向分成4個(gè)或8個(gè)主方向，對(duì)角速度信息進(jìn)行修正，從而修正航向。A. R. Jiménez 等在此基礎(chǔ)上進(jìn)一步做了改進(jìn)，提出了改進(jìn)型啟發(fā)式漂移消除算法[8](Improved Heuristic Drift Elimination，iHDE),將姿態(tài)角和主方向信息加入擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)，降低航向誤差。在器件在線標(biāo)定方面，王海亮等[9]提出了一種彈載捷聯(lián)慣導(dǎo)簡(jiǎn)易在線標(biāo)定技術(shù)，利用車體做常規(guī)運(yùn)動(dòng)下的橫滾角和俯仰角對(duì)慣導(dǎo)誤差的激勵(lì)作用實(shí)現(xiàn)在線標(biāo)定，但是是面向彈體等可以控制固定軌跡來進(jìn)行實(shí)驗(yàn)標(biāo)定的對(duì)象，不適用于行人導(dǎo)航。由于MEMS的零偏和刻度系數(shù)受環(huán)境影響較大，而安裝誤差受環(huán)境影響小、穩(wěn)定性好，何昆鵬等[10]提出了一種野外標(biāo)定MEMS器件參數(shù)的方法，對(duì)微組合導(dǎo)航系統(tǒng)的3個(gè)軸分別進(jìn)行位置擺放，通過方程標(biāo)定零偏和標(biāo)度因數(shù)，實(shí)現(xiàn)組合導(dǎo)航系統(tǒng)的野外在線標(biāo)定，但因行人導(dǎo)航系統(tǒng)中，器件始終固定在腳上，因此也不適用于行人導(dǎo)航系統(tǒng)。

本文在分析了上述問題的基礎(chǔ)上，通過研究行人導(dǎo)航和微慣性器件的特點(diǎn)，提出了一種基于MEMS器件模型的在線修正算法。該方法基于微慣性器件的誤差模型，對(duì)陀螺儀和加速度計(jì)的零偏進(jìn)行修正。初始時(shí)，利用誤差模型進(jìn)行初步修正，從而達(dá)到修正初始姿態(tài)角的目的；在行走過程中，搜索判定行走過程中的修正區(qū)間，進(jìn)行在線修正，并對(duì)修正之后的數(shù)據(jù)進(jìn)行主航向角反饋修正，綜合提高了行人導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間定位性能。

1 基于微慣性的導(dǎo)航系統(tǒng)誤差模型

1.1 微慣性器件誤差模型

考慮陀螺儀和加速度計(jì)的刻度因數(shù)、安裝誤差和零偏誤差，誤差模型如下

(1)

(2)

其中，Bx、By、Bz為陀螺儀三軸向的零偏誤差，Dx、Dy、Dz為加速度計(jì)三軸向的零偏誤差；δkgx、δkgy、δkgz為陀螺儀三軸向的刻度因數(shù)誤差，δkax、δkay、δkaz為加速度計(jì)三軸向的刻度因數(shù)誤差；δLij(i=x,y,z;j=x,y,z)和δL′ij(i=x,y,z;j=x,y,z)分別為陀螺儀及加速度計(jì)三軸之間的安裝誤差變化；ωx、ωy、ωz、fx、fy、fz分別為陀螺儀和加速度計(jì)的理想輸出。

1.2 誤差模型的簡(jiǎn)易處理

由于陀螺儀和加速度計(jì)集成在一個(gè)芯片上，安裝誤差的長(zhǎng)期穩(wěn)定性較好，因此可將其安裝誤差看作常值，使用事先標(biāo)定好的參數(shù)加入捷聯(lián)慣導(dǎo)系統(tǒng)中，進(jìn)而達(dá)到簡(jiǎn)化誤差模型的目的，簡(jiǎn)化后的模型如下[11]

(3)

(4)

2 基于誤差模型的MEMS器件在線修正算法

為了實(shí)現(xiàn)陀螺儀和加速度計(jì)的在線修正，首先需要判斷出行人步行的零速區(qū)間，即零速檢測(cè)。零速檢測(cè)[12]指當(dāng)行人行走腳落地過程或者行人靜止?fàn)顟B(tài)下，綁有慣性測(cè)量單元(Inertial Measurement Unit，IMU)的腳在落地期間，理論上，其角速率和加速度均為零。通過檢測(cè)落腳瞬間，可以人為消除陀螺儀和加速度計(jì)輸出值的累積誤差。在此基礎(chǔ)上，選擇可修正區(qū)間，利用在線修正算法修正陀螺儀和加速度計(jì)的參數(shù)，進(jìn)而優(yōu)化導(dǎo)航解算的位置信息。

2.1 在線修正區(qū)間的判定

為了判定在線修正區(qū)間，在四條件法零速檢測(cè)的基礎(chǔ)上，進(jìn)行長(zhǎng)度檢測(cè)與識(shí)別。取數(shù)據(jù)窗口的長(zhǎng)度為L(zhǎng)，半窗口的長(zhǎng)度為s，L=2s+1。

加速度閾值條件為

(5)

|ak|即為判斷窗口內(nèi)的加速度的均值，若處于給定的閾值區(qū)間，則判斷此時(shí)腳部處于靜止?fàn)顟B(tài)。用C1來表示人行走的狀態(tài)，C1為1時(shí)表示零速狀態(tài)，C1為0時(shí)表示非零速狀態(tài)，即

(6)

角速度閾值條件為

(7)

|ωk|即為判斷窗口內(nèi)的角速度的均值，若處于給定的閾值區(qū)間，則判斷此時(shí)腳部處于靜止?fàn)顟B(tài)。用C2來表示人行走的狀態(tài)，C2為1時(shí)表示零速狀態(tài)，C2為0時(shí)表示非零速狀態(tài)，即

(8)

加速度方差條件為

(9)

(10)

(11)

角速度方差條件為

(12)

(13)

(14)

當(dāng)上述4個(gè)條件都滿足時(shí)，即C=C1&C2&C3&C4=1時(shí)，表示為零速時(shí)刻。

取thcor為判斷零速區(qū)間長(zhǎng)度閾值，Lk-zero為第k個(gè)零速區(qū)間的長(zhǎng)度，在零速區(qū)間檢測(cè)之后，利用公式進(jìn)行判定，判斷關(guān)系式如下

(15)

2.2 基于模型的在線修正算法

(16)

圖1 導(dǎo)航系-載體系坐標(biāo)變換示意圖Fig.1 Coordinate transformation between navigation and body frame

根據(jù)角度轉(zhuǎn)換關(guān)系，可得n系下的比力(0,0,fg)′與b系下的比力(fx,fy,fz)′之間的轉(zhuǎn)換關(guān)系為

(17)

根據(jù)式(16)和式(17)，可以計(jì)算求得初始的俯仰角和滾轉(zhuǎn)角分別為

(18)

(19)

(20)

(21)

初步修正之后，根據(jù)2.1節(jié)提到的方法進(jìn)行可修正區(qū)間的判別，選擇出符合條件的幾個(gè)可修正區(qū)間。取可修正區(qū)間內(nèi)窗口長(zhǎng)度為S,根據(jù)

(22)

(23)

可將后續(xù)加速度和角速度的值進(jìn)行進(jìn)一步優(yōu)化。優(yōu)化后的加速度值通過式(23)即可重新計(jì)算該區(qū)間內(nèi)俯仰角和滾轉(zhuǎn)角。

3 主航向反饋修正算法

在足綁式行人導(dǎo)航系統(tǒng)中，俯仰角和橫滾角可以通過加速度計(jì)輸出計(jì)算得到，但是航向角的誤差存在不可觀性，所以會(huì)導(dǎo)致航向角的漂移和誤差累積[13]。為了實(shí)現(xiàn)修正航向角的目的，提出了主航向角誤差反饋修正算法，將行走過程中的航向角變化進(jìn)行在線計(jì)算，判斷直行與轉(zhuǎn)彎兩種狀態(tài)。將航向角誤差作為觀測(cè)量反饋到EKF中。

3.1 航向估計(jì)

對(duì)于足綁式行人導(dǎo)航系統(tǒng)，由于IMU器件綁在腳面上，且腳面運(yùn)動(dòng)比較復(fù)雜，所以其航向角的變化很快。規(guī)定行人行走時(shí)的狀態(tài)為直線和轉(zhuǎn)彎兩種，那么在行走兩步之間的航向角變化為

(24)

(25)

由此可知，在第k個(gè)直線行走的路徑中，經(jīng)過一段時(shí)間，在行走第j步時(shí)，對(duì)應(yīng)的主航向角變化值為

(26)

設(shè)定一定的角度閾值thturn，判斷是否處于轉(zhuǎn)彎狀態(tài)

(27)

引入權(quán)重系數(shù)μ，對(duì)行人行走的狀態(tài)進(jìn)行進(jìn)一步的描述，即

(28)

3.2 主航向反饋濾波修正

為了實(shí)現(xiàn)預(yù)測(cè)行走過程中零速時(shí)刻的誤差并進(jìn)行修正，使用EKF，狀態(tài)方程和量測(cè)方程如式(29)所示。

Xk=φk|k-1Xk-1+Wk-1
Zk=HδXk|k+Vk

(29)

φk|k-1=

(30)

在量測(cè)更新部分，取主航向角誤差量δψ和速度變化向量δvk為觀測(cè)量，即

Zk=[δψ,δvk]

(31)

對(duì)應(yīng)地

(32)

設(shè)置量測(cè)噪聲Rk中對(duì)應(yīng)主航向角誤差的部分為R′k-head，對(duì)應(yīng)速度變化的部分為Rk-v，即

(33)

R′k-head=μ2Rk-head

(34)

計(jì)算當(dāng)前的卡爾曼增益，并把速度信息作為系統(tǒng)的量測(cè)值，通過狀態(tài)方程對(duì)信息進(jìn)行時(shí)間更新，再通過量測(cè)更新得到更新后修正的速度、位置和姿態(tài)角等信息。

4 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析

針對(duì)本文提出的在線修正與主航向反饋修正算法，分別在學(xué)校建筑物走廊和學(xué)校操場(chǎng)設(shè)計(jì)進(jìn)行了直線實(shí)驗(yàn)及環(huán)形實(shí)驗(yàn)。實(shí)驗(yàn)使用的微慣性測(cè)量單元為荷蘭Xsens公司的MTi-G-710。該元件內(nèi)部包括三軸加速度計(jì)、三軸陀螺儀和磁力計(jì)。加速度計(jì)量程為±50m/s2，零偏穩(wěn)定性40μg，零偏重復(fù)性0.03m/s2，陀螺儀量程為±450(°)/s，零偏穩(wěn)定性10(°)/h，零偏重復(fù)性0.2(°)/s，采樣頻率為100Hz，固定于腳面，如圖2所示。

直線實(shí)驗(yàn)部分在學(xué)校建筑物走廊進(jìn)行，路程為40m，開始時(shí)在原地靜止3s，在前進(jìn)到26m處左右時(shí)暫停5s，再走到終點(diǎn)。環(huán)形實(shí)驗(yàn)部分在學(xué)校操場(chǎng)進(jìn)行，路程為300m，開始時(shí)在原地靜止3s，在每前進(jìn)60m左右時(shí)暫停5s，直到走到終點(diǎn)。使用本文提到的在線修正技術(shù)及主航向反饋修正算法前后的軌跡分別如圖3和圖4所示，實(shí)驗(yàn)結(jié)果如表1所示。從中可以看出，直線行走實(shí)驗(yàn)誤差從11.08%降低至2.01%。環(huán)形行走實(shí)驗(yàn)的誤差從14.79%降低至1.65%，誤差有了很大程度降低。

圖2 MEMS器件及穿戴方式Fig.2 MEMS devices and wearing methods

圖3 直線行走軌跡Fig.3 Linear walking trajectory

圖4 操場(chǎng)環(huán)形實(shí)驗(yàn)軌跡Fig.4 Circular walking trajectory on the playground

表1 實(shí)驗(yàn)驗(yàn)證結(jié)果

Tab.1 Experiments results

實(shí)驗(yàn)類別起點(diǎn)位置(x/m, y/m)實(shí)際終點(diǎn)位置(x/m, y/m)解算出的終點(diǎn)位置(x/m, y/m)誤差/%室內(nèi)-在線修正前(0, 0)(0, 40)(4.25, 38.75)11.08室內(nèi)-在線修正后(0, 0)(0, 40)(0.77, 40.23)2.01室外-在線修正前(0, 0)(0, 0)(-43.97, -5.86)14.79室外-在線修正后(0, 0)(0, 0)(-0.92,-4.87)1.65

5 結(jié)論

本文提出了一種基于誤差模型的MEMS器件參數(shù)在線修正技術(shù)，對(duì)陀螺儀和加速度計(jì)的零偏進(jìn)行在線修正。根據(jù)行人行走的特點(diǎn)，檢測(cè)并區(qū)分行走過程中的可修正區(qū)間，使用逆向解算算法進(jìn)行數(shù)據(jù)的解算和修正補(bǔ)償，并提出了主航向角反饋修正算法，以提高行人導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間定位性能。從實(shí)驗(yàn)驗(yàn)證的結(jié)果可以看出，直線實(shí)驗(yàn)的誤差降低了9.07%，環(huán)形實(shí)驗(yàn)的誤差降低了13.14%，效果很好。該方法提高了行人導(dǎo)航系統(tǒng)的定位精度，其思路和方法對(duì)于行人自主導(dǎo)航系統(tǒng)的優(yōu)化具有很好的應(yīng)用價(jià)值。