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

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

0 引言

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

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

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

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

1.1 微慣性器件誤差模型

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

(1)

(2)

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

1.2 誤差模型的簡易處理

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

(3)

(4)

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

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

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

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

加速度閾值條件為

(5)

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

(6)

角速度閾值條件為

(7)

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

(8)

加速度方差條件為

(9)

(10)

(11)

角速度方差條件為

(12)

(13)

(14)

當上述4個條件都滿足時，即C=C1&C2&C3&C4=1時，表示為零速時刻。

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

(15)

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

(16)

圖1 導航系-載體系坐標變換示意圖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)，可以計算求得初始的俯仰角和滾轉(zhuǎn)角分別為

(18)

(19)

(20)

(21)

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

(22)

(23)

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

3 主航向反饋修正算法

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

3.1 航向估計

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

(24)

(25)

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

(26)

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

(27)

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

(28)

3.2 主航向反饋濾波修正

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

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

(29)

φk|k-1=

(30)

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

Zk=[δψ,δvk]

(31)

對應地

(32)

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

(33)

R′k-head=μ2Rk-head

(34)

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

4 實驗驗證與結(jié)果分析

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

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

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

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

圖4 操場環(huán)形實驗軌跡Fig.4 Circular walking trajectory on the playground

表1 實驗驗證結(jié)果

Tab.1 Experiments results

實驗類別起點位置(x/m, y/m)實際終點位置(x/m, y/m)解算出的終點位置(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ù)，對陀螺儀和加速度計的零偏進行在線修正。根據(jù)行人行走的特點，檢測并區(qū)分行走過程中的可修正區(qū)間，使用逆向解算算法進行數(shù)據(jù)的解算和修正補償，并提出了主航向角反饋修正算法，以提高行人導航系統(tǒng)長時間定位性能。從實驗驗證的結(jié)果可以看出，直線實驗的誤差降低了9.07%，環(huán)形實驗的誤差降低了13.14%，效果很好。該方法提高了行人導航系統(tǒng)的定位精度，其思路和方法對于行人自主導航系統(tǒng)的優(yōu)化具有很好的應用價值。