燕 歡，郭 杭，Marcin URADZINSKI，熊 劍，Jacek RAPISKI，洪海斌

(1.南昌大學，南昌 330031；2.University of Warmia and Mazury in Olsztyn，Poland)

后向平滑容積濾波算法處理與分析組合導航數(shù)據

燕 歡1，郭 杭1，Marcin URADZINSKI2，熊 劍1，Jacek RAPISKI2，洪海斌1

(1.南昌大學，南昌 330031；2.University of Warmia and Mazury in Olsztyn，Poland)

通過研究采用經過改進的容積卡爾曼濾波算法，結合后向平滑濾波的思想聯(lián)合解算全球定位系統(tǒng)/慣性導航系統(tǒng)數(shù)據。兩次車載實驗在波蘭進行，分別是波蘭研發(fā)的慣性傳感器/單頻全球定位系統(tǒng)和國產雙頻雙模全球定位系統(tǒng)/慣導組合導航系統(tǒng)與差分全球定位系統(tǒng)數(shù)據比較的車載實驗。并將實驗得到的數(shù)據進行如下處理：其一是將單頻全球定位系統(tǒng)/慣導聯(lián)合解算后的結果，與差分全球定位系統(tǒng)數(shù)據進行對比；其二是用國產組合導航系統(tǒng)解算結果與差分數(shù)據結果進行對比。經處理后發(fā)現(xiàn)，低精度的單頻全球定位系統(tǒng)/慣導解算的結果，用后向平滑容積卡爾曼濾波算法處理后精度得以提高，達到了中等精度的國產組合導航系統(tǒng)同樣的精度。這個結論證明后向平滑濾波結合容積卡爾曼濾波是解算車載組合導航數(shù)據的有效方法。

后向平滑濾波；容積卡爾曼濾波；慣性導航系統(tǒng)；差分定位

1 引言

全球定位系統(tǒng)(global positioning system，GPS)與慣性測量單元(inertial measurement unit，IMU)組合導航[1]是指將GPS接收機接收到的數(shù)據和慣性導航傳感器得到的信息通過一定算法結合起來的組合導航方法，利用其性能上的互補特性，獲得比使用其中任意一種系統(tǒng)時更高的導航性能。由于GPS有全天候，全時間的特點，且定位精度比較高，所以用它對IMU進行補償，并且在依賴一定算法的條件下，可以提高導航精度。目前這類組合導航的方法已經廣泛運用于航空、航天、汽車、船舶、軍事等多個領域。對于近幾年組合導航取得的快速發(fā)展，高精度，高穩(wěn)定性的組合導航算法也越來越受到人們的關注。

容積卡爾曼濾波(cubature Kalman filter，CKF)是一種經過改進的新型的非線性濾波方法[2]。其利用數(shù)值積分原則對目標狀態(tài)的后驗概率進行近似。此算法相對于擴展卡爾曼濾波有較好的收斂效果，且實現(xiàn)較為簡單，因此成為聯(lián)合解算的重要算法。此次試驗處理中，在容積卡爾曼濾波的基礎上加入了平滑的后向遞歸運算[3]，有效減少了觀測誤差和觀測噪聲的影響，在提高了精度的基礎上加快了收斂速度。

2 容積卡爾曼濾波

首先考慮如下的非線性系統(tǒng)的狀態(tài)方程和觀測方程為

Xk+1=f(x)Xk+GWk

(1)

Zk=HkXk+Vk

(2)

式(1)及式(2)中：Xk為系統(tǒng)的狀態(tài)向量，Zk為系統(tǒng)的觀測向量，假定過程噪聲Wk和觀測噪聲Vk相互獨立，且分別服從Wk～N(0,Qk)；Vk～N(0,Rk)。

有積分

(3)

式(3)中，I(f)為待積分量，f(x)為系統(tǒng)矩陣，容積卡爾曼濾波首先計算加權函數(shù)為標準正態(tài)分布密度的積分的基本容積點和對應的權值，采用2n個等權值的容積點{ξj，ωj}，即有

(4)

容積點及其對應的權值分別為

(5)

式(5)中，m表示容積點總數(shù)，采用三階容積原則，容積點總數(shù)是狀態(tài)維數(shù)的2倍，m=2n；[1]j表示容積點集中地第j個元素。容積卡爾曼算法流程如下5.1和5.2所述。

2.1 時間更新

1)計算容積點

(6)

(7)

式(7)中，Sk-1|k-1=chol(Pk-1|k-1)，chol()函數(shù)表示對矩陣進行Cholesky分解，應用于對狀態(tài)方程容積點的計算。

2)計算通過狀態(tài)方程傳播容積點

(8)

3)估計k時刻的狀態(tài)預測值為

(9)

4)估計k時刻的狀態(tài)誤差協(xié)方差預測值為

(10)

式(10)中 ，Qk-1陣為Wk陣的協(xié)方差矩陣。

2.2 量測更新

通過cholesky分解可得

(11)

式(11)中，Sk|k-1=chol(Pk|k-1)，chol()函數(shù)表示對矩陣進行Cholesky分解，應用于對量測方程容積點的計算。

(12)

式(12)中，ξi表示加權后容積點的總數(shù)。

可以通過觀測方程傳播容積點，即

(13)

估計k時刻觀測預測值

(14)

估計自相關協(xié)方差陣為

(15)

式(15)中，Rk陣為Vk陣的協(xié)方差矩陣

估計互相關協(xié)方差陣

(16)

估計卡爾曼增益

(17)

綜合式(6)～式(17)可得K時刻狀態(tài)估計

(18)

k時刻協(xié)方差陣估計值為

(19)

3 后向平滑卡爾曼濾波

由于后向平滑濾波[4]是根據之前的濾波進行的遞歸運算，且經過前向濾波可以消除大部分的誤差，所以將平滑后的值重新作為起始值就可以進一步地提高精度。所以結合后向平滑的思想與容積卡爾曼濾波的算法進行融合，算法如下：由k-1時刻的狀態(tài)經過一次 CKF濾波得到k時刻的狀態(tài)估計，再經過后向平滑過程更新k-1時刻的估計值，最后將更新后的k-1時刻的平滑值進行第二次CKF濾波得到k時刻的狀態(tài)估計。

狀態(tài)后向平滑表達式為

(20)

式(20)中，A(k-1)是后向平滑增益。協(xié)方差后向平滑表達式為

Pk-1|k=Pk-1|k-1+Ak-1[Pk-k-Pk|k-1]Ak-1

(21)

經過一次前向濾波和一次后向平滑，就可以解算出當前時刻狀態(tài)估計的實時量。

4 GPS/IMU聯(lián)合解算

參照文獻[2]GPS/IMU聯(lián)合解算過程，利用波蘭研發(fā)的單頻GPS定位接收機LEI輸出的載體的位置信息與ADISIMU聯(lián)合解算的位置之差作為觀測值[5]，以ADIS的速度誤差、位置誤差、姿態(tài)誤差、陀螺漂移以及加速度零偏等作為狀態(tài)量，通過后向平滑容積卡爾曼濾波估計出狀態(tài)誤差值，最后通過閉環(huán)反饋或者開環(huán)反饋對ADIS進行校正[6]，最終獲得載體的準確位置、速度和姿態(tài)信息。其原理圖如圖1所示。

圖1 GPS/IMU聯(lián)合解算框圖

對于狀態(tài)量

X=[φE,φN,φU,δVE,δVN,δVU,δL,δλ,δh,
ζx,ζy,ζz,▽x,▽y,▽z]

(22)

式(22)中，F(xiàn)(t)為狀態(tài)轉移矩陣，G(t)為誤差系數(shù)矩陣[7]，W(t)為白噪聲。離散化為

(23)

對于量測方程

(24)

(25)

(26)

式(23)～式(26)中，Q為功率譜密度矩陣，σa和σω分別表示加速度計和陀螺儀的標準偏差[8]；Δt為采樣間隔；Rk為觀測噪聲矩陣，σφ、σλ和σh分別表示經度、緯度和高度標準偏差。

5 實測數(shù)據處理與分析

數(shù)據處理與分析是基于上述的兩次車載實驗進行的，實驗設備分別為ADIS慣性導航儀和LEIGPS接收機組合系統(tǒng)以及國產組合導航系統(tǒng)XW-ADU5660(IMU/GPS組合導航裝置)[9]。其中，ADIS慣性導航儀為低精度的慣性導航儀器，其初始對準時間為30s(粗對準時間為10s，準確的對準時間是20s)。陀螺零偏穩(wěn)定性為0.015°/s。加速度零偏是1.7m/s2，隨機噪聲是0.135。LEIGPS接收機為單頻接收機，其定位誤差為2.5m，頻率為2Hz。兩種傳感器均為低精度的慣性導航儀器。XW-ADU5660是中等精度的姿態(tài)方位組合導航系統(tǒng)，可以提供載體在靜止和運動過程中的方位、姿態(tài)、位置和時間等信息，由雙天線、GPS、格洛納斯衛(wèi)星導航系統(tǒng)[10](globalnavigationsatellitesystem，GLONASS)雙頻衛(wèi)星信號接收機、IMU和導航計算機組成。其衛(wèi)星定位精度較高，單點位置精度(GPS有效)：水平位置誤差2.4m，速度精度為0.2m/s。方位角誤差小于0.12°，姿態(tài)角誤差小于0.1°。陀螺零偏為10°/h，加速度計零偏小于0.1mm/s2。

實驗在波蘭UniversityofWarmiaandMazuryinOlsztyn的Kortowo校區(qū)內進行，兩次車載實驗分別利用ADIS慣導/單頻LEIGPS組合結果和XW-ADU5660(IMU/雙頻雙模GPS)的輸出結果與差分GPS結果進行對比，實驗過程詳見圖4：

圖2 車內XW-ADU5660安置圖

圖3 車頂天線安置

圖4 ADIS慣導和LEi GPS接收機安置

圖5 車載實驗軌跡

從谷歌地圖截取整個實驗區(qū)域的影像圖片，并繪制整個車載實驗過程的行跡路線圖，如圖5。

5.1 單頻全球定位系統(tǒng)/慣導聯(lián)合解算的對比分析

實驗結束后，將LEI GPS數(shù)據和ADIS慣性導航儀器進行IMU/GPS聯(lián)合解算[8]，其算法即為本文建議的后向平滑容積卡爾曼濾波，將處理后的位置、速度和差分GPS位置、速度進行對比。當?shù)氐牟罘諫PS位置作為此次實驗的標準值。結果如下：

圖6 ADIS/GPS處理后的軌跡與差分GPS比較

圖7 處理后的速度

圖8 處理后的位置與差分GPS位置誤差

表1 經處理后的位置與差分GPS位置誤差統(tǒng)計/m

北東天誤差最大最小平均標準差N方向8.170.011.752.37位置誤差E方向10.160.011.552.23U方向8.881.903.712.05

5.2 雙頻雙模導航系統(tǒng)的對比分析

為了與中等精度的車載組合導航實驗結果比較，進行了雙頻雙模XW-ADU5660與差分GPS的位置比較實驗。由于XW-ADU5660測得的數(shù)據GPS時間起始點與差分數(shù)據不一樣，且XW-ADU5660數(shù)據的采樣頻率較高與差分GPS頻率不一樣，所以剔除了XW-ADU5660一部分數(shù)據，使二者從相同的時間點開始比較，時間周期也處理成同步的。比較結果如圖9所示。

圖9 差分GPS與XW-ADU5660的軌跡比較

圖10 差分GPS與XW-ADU5660的位置誤差比較

表2 XW-ADU5660與差分GPS結果相比較后的誤差統(tǒng)計/m

通過將兩次實驗得出的誤差進行對比(參見表1和表2)：經后向CKF卡爾曼濾波處理的ADIS/LEI GPS數(shù)據與差分GPS的位置比較，其標準誤差，N方向為2.37 m，E方向為2.23 m，U方向為2.05 m；雙頻雙模XW-ADU5660與差分GPS位置相比較，它們的標準誤差，N方向為2.89 m，E方向為1.61 m，U方向為1.95 m。由此可見：后向平滑的CKF算法對誤差經行了兩次濾波，很大程度上彌補了低精度組合導航設備的不足，使得其精度接達到了中等精度的組合導航儀器XW-ADU5660類似的精度。這說明用后向平滑CKF濾波算法處理低精度的IMU/GPS組合系統(tǒng)(ADIS和LEI)數(shù)據，解算結果其位置的精度有了提高。這個結論證明后向平滑濾波結合容積卡爾曼濾波是解算車載組合導航數(shù)據的有效方法。

6 結束語

通過實驗結果可以看出，經后向CKF卡爾曼濾波處理的ADIS/LEI IMU/GPS數(shù)據與差分GPS位置的標準偏差，N方向為2.37 m，E方向為2.23 m，U方向為2.05 m；雙頻雙模XW-ADU5660與差分GPS結果相比較的位置標準誤差，N方向為2.89 m，E方向為1.61 m，U方向為1.95 m。后向平滑CKF卡爾曼濾波算法由于其兩次平滑濾波以及容積卡爾曼本身的優(yōu)越性，使得在處理低精度ADIS慣導和LEI GPS接收機的組合導航數(shù)據時，基本接近了中等精度的XW-ADU5660導航設備的精度，是一種有效的方法。

[1] 秦永元，汪淑華.卡爾曼濾波與組合導航原理[M].西安：西北工業(yè)大學出版社，2010.

[2] 霍光，李冬梅.基于后向平滑容積卡爾曼濾波的單站無源定位算法[D].鄭州：解放軍信息工程大學，2013.

[3] 董旭榮，張守信，華仲春.GPS/INS組合導航定位及其應用[M].長沙：國防科技大學出版社，1998：12-13.

[4] 高為廣，楊元喜，崔先強，等.IMU/GPS組合導航系統(tǒng)自適應Kalman濾波算法[J].武漢大學學報：信息科學版，2006，31(5)：3-8.

[5] 劉勝，楊丹，蘇旭，等.隨機系統(tǒng)高權值便捷UKF濾波方法：中國，CN201310241427.6[P].2013-10-02.

[6] 常志海.導航定位產業(yè)的現(xiàn)狀和發(fā)展趨勢[EB/OL].[2014-05-12].http：//www.inav.cn/Article/Details/261.

[7] 王睿.基于多傳感器融合的定位系統(tǒng)設計與實現(xiàn)[D].北京：北京郵電大學，2012

[8] SAVAGE P G.Strapdown Inertial Navigation Integration Algorithm Design Part 2：Velocity and Position Algorithms[J].Journal of Guidance，Control and Dynamics，1998，21(2)：208-221.

[9] BRITTING K R.Inertial Navigation Systems Analysis[M].Hoboken，NJ：John Wiley & Sons Inc，1971.

[10]寧津生，姚宜斌，張小紅.全球導航衛(wèi)星系統(tǒng)發(fā)展綜述[J].導航定位學報，2013，1(1)：3-8.

收稿日期：2014-05-26

第一作者簡介：楊安洪(1983)，男，四川彭山人，工程師，研究方向為航天測控。

GPS/IMU Data Processing and Analysis with Back Smoothing and CKF Filtering Algorithm

YAN Huan1，GUO Hang1，Marcin URADZINSKI2，XIONG Jian1，Jacek RAPISKI2，HONG Hai-bin1

(1.Nanchang University，Nanchang 330031，China；2.University of Warmia and Mazury in Olsztyn，Poland)

Using the combined volume backword smoothing Kalman filter (CKF) algorithm for processing vehicle navigation data，the result accuracy is higher than that of using the Kalman filter forward only.In this paper，GPS/IMU data come from two vehicle experiments conducted in Poland，namely LEI and ADIS integrated system (single frequency GPS/IMU)，XW-ADU5660 (double frequency GPS，GLONASS/IMU) compared with the differential GPS car experiments.And experimental data obtained are processed as follows：one is the result of LEI GPS and ADIS IMU compared with the results of differential GPS positioning data；the other is XW-ADU5660 results compared with differential GPS.The results show that low accurate LEI and ADIS integrated system with the backword CKF smoothing filter algorithm reached the same level of medium accurate navigation instrument XW-ADU5660.

backward smoothing filter；volume Kalman filter；inertial navigation system；GPS differential positioning

2014-06-30

國家自然科學基金(41164001、41374039)。

燕歡(1991)，男，山西運城人，碩士生，主要從事慣性導航、組合導航的研究。

P228

A

2095-4999(2015)-01-0083-05