行人導航系統中的MEMS誤差在線修正技術

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

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

0 引言

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

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

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

1 基于微慣性的導航系統誤差模型

1.1 微慣性器件誤差模型

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

(1)

(2)

其中，Bx、By、Bz為陀螺儀三軸向的零偏誤差，Dx、Dy、Dz為加速度計三軸向的零偏誤差；δkgx、δkgy、δkgz為陀螺儀三軸向的刻度因數誤差，δkax、δkay、δkaz為加速度計三軸向的刻度因數誤差；δ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 誤差模型的簡易處理

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

(3)

(4)

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

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

2.1 在線修正區間的判定

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

加速度閾值條件為

(5)

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

(6)

角速度閾值條件為

(7)

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

(8)

加速度方差條件為

(9)

(10)

(11)

角速度方差條件為

(12)

(13)

(14)

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

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

(15)

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

(16)

圖1 導航系-載體系坐標變換示意圖Fig.1 Coordinate transformation between navigation and body frame

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

(17)

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

(18)

(19)

(20)

(21)

初步修正之后，根據2.1節提到的方法進行可修正區間的判別，選擇出符合條件的幾個可修正區間。取可修正區間內窗口長度為S,根據

(22)

(23)

可將后續加速度和角速度的值進行進一步優化。優化后的加速度值通過式(23)即可重新計算該區間內俯仰角和滾轉角。

3 主航向反饋修正算法

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

3.1 航向估計

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

(24)

(25)

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

(26)

設定一定的角度閾值thturn，判斷是否處于轉彎狀態

(27)

引入權重系數μ，對行人行走的狀態進行進一步的描述，即

(28)

3.2 主航向反饋濾波修正

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

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

(29)

φk|k-1=

(30)

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

Zk=[δψ,δvk]

(31)

對應地

(32)

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

(33)

R′k-head=μ2Rk-head

(34)

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

4 實驗驗證與結果分析

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

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

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

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

圖4 操場環形實驗軌跡Fig.4 Circular walking trajectory on the playground

表1 實驗驗證結果

Tab.1 Experiments results

實驗類別起點位置(x/m, y/m)實際終點位置(x/m, y/m)解算出的終點位置(x/m, y/m)誤差/%室內-在線修正前(0, 0)(0, 40)(4.25, 38.75)11.08室內-在線修正后(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 結論

本文提出了一種基于誤差模型的MEMS器件參數在線修正技術，對陀螺儀和加速度計的零偏進行在線修正。根據行人行走的特點，檢測并區分行走過程中的可修正區間，使用逆向解算算法進行數據的解算和修正補償，并提出了主航向角反饋修正算法，以提高行人導航系統長時間定位性能。從實驗驗證的結果可以看出，直線實驗的誤差降低了9.07%，環形實驗的誤差降低了13.14%，效果很好。該方法提高了行人導航系統的定位精度，其思路和方法對于行人自主導航系統的優化具有很好的應用價值。