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

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

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

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

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

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

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

1 引言

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

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

2 容積卡爾曼濾波

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

Xk+1=f(x)Xk+GWk

(1)

Zk=HkXk+Vk

(2)

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

有積分

(3)

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

(4)

容積點(diǎn)及其對(duì)應(yīng)的權(quán)值分別為

(5)

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

2.1 時(shí)間更新

1)計(jì)算容積點(diǎn)

(6)

(7)

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

2)計(jì)算通過(guò)狀態(tài)方程傳播容積點(diǎn)

(8)

3)估計(jì)k時(shí)刻的狀態(tài)預(yù)測(cè)值為

(9)

4)估計(jì)k時(shí)刻的狀態(tài)誤差協(xié)方差預(yù)測(cè)值為

(10)

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

2.2 量測(cè)更新

通過(guò)cholesky分解可得

(11)

式(11)中，Sk|k-1=chol(Pk|k-1)，chol()函數(shù)表示對(duì)矩陣進(jìn)行Cholesky分解，應(yīng)用于對(duì)量測(cè)方程容積點(diǎn)的計(jì)算。

(12)

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

可以通過(guò)觀測(cè)方程傳播容積點(diǎn)，即

(13)

估計(jì)k時(shí)刻觀測(cè)預(yù)測(cè)值

(14)

估計(jì)自相關(guān)協(xié)方差陣為

(15)

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

估計(jì)互相關(guān)協(xié)方差陣

(16)

估計(jì)卡爾曼增益

(17)

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

(18)

k時(shí)刻協(xié)方差陣估計(jì)值為

(19)

3 后向平滑卡爾曼濾波

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

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

(20)

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

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

(21)

經(jīng)過(guò)一次前向?yàn)V波和一次后向平滑，就可以解算出當(dāng)前時(shí)刻狀態(tài)估計(jì)的實(shí)時(shí)量。

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

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

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

對(duì)于狀態(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)轉(zhuǎn)移矩陣，G(t)為誤差系數(shù)矩陣[7]，W(t)為白噪聲。離散化為

(23)

對(duì)于量測(cè)方程

(24)

(25)

(26)

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

5 實(shí)測(cè)數(shù)據(jù)處理與分析

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

實(shí)驗(yàn)在波蘭UniversityofWarmiaandMazuryinOlsztyn的Kortowo校區(qū)內(nèi)進(jìn)行，兩次車載實(shí)驗(yàn)分別利用ADIS慣導(dǎo)/單頻LEIGPS組合結(jié)果和XW-ADU5660(IMU/雙頻雙模GPS)的輸出結(jié)果與差分GPS結(jié)果進(jìn)行對(duì)比，實(shí)驗(yàn)過(guò)程詳見圖4：

圖2 車內(nèi)XW-ADU5660安置圖

圖3 車頂天線安置

圖4 ADIS慣導(dǎo)和LEi GPS接收機(jī)安置

圖5 車載實(shí)驗(yàn)軌跡

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

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

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

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

圖7 處理后的速度

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

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

北東天誤差最大最小平均標(biāo)準(zhǔn)差N方向8.170.011.752.37位置誤差E方向10.160.011.552.23U方向8.881.903.712.05

5.2 雙頻雙模導(dǎo)航系統(tǒng)的對(duì)比分析

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

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

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

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

通過(guò)將兩次實(shí)驗(yàn)得出的誤差進(jìn)行對(duì)比(參見表1和表2)：經(jīng)后向CKF卡爾曼濾波處理的ADIS/LEI GPS數(shù)據(jù)與差分GPS的位置比較，其標(biāo)準(zhǔn)誤差，N方向?yàn)?.37 m，E方向?yàn)?.23 m，U方向?yàn)?.05 m；雙頻雙模XW-ADU5660與差分GPS位置相比較，它們的標(biāo)準(zhǔn)誤差，N方向?yàn)?.89 m，E方向?yàn)?.61 m，U方向?yàn)?.95 m。由此可見：后向平滑的CKF算法對(duì)誤差經(jīng)行了兩次濾波，很大程度上彌補(bǔ)了低精度組合導(dǎo)航設(shè)備的不足，使得其精度接達(dá)到了中等精度的組合導(dǎo)航儀器XW-ADU5660類似的精度。這說(shuō)明用后向平滑CKF濾波算法處理低精度的IMU/GPS組合系統(tǒng)(ADIS和LEI)數(shù)據(jù)，解算結(jié)果其位置的精度有了提高。這個(gè)結(jié)論證明后向平滑濾波結(jié)合容積卡爾曼濾波是解算車載組合導(dǎo)航數(shù)據(jù)的有效方法。

6 結(jié)束語(yǔ)

通過(guò)實(shí)驗(yàn)結(jié)果可以看出，經(jīng)后向CKF卡爾曼濾波處理的ADIS/LEI IMU/GPS數(shù)據(jù)與差分GPS位置的標(biāo)準(zhǔn)偏差，N方向?yàn)?.37 m，E方向?yàn)?.23 m，U方向?yàn)?.05 m；雙頻雙模XW-ADU5660與差分GPS結(jié)果相比較的位置標(biāo)準(zhǔn)誤差，N方向?yàn)?.89 m，E方向?yàn)?.61 m，U方向?yàn)?.95 m。后向平滑CKF卡爾曼濾波算法由于其兩次平滑濾波以及容積卡爾曼本身的優(yōu)越性，使得在處理低精度ADIS慣導(dǎo)和LEI GPS接收機(jī)的組合導(dǎo)航數(shù)據(jù)時(shí)，基本接近了中等精度的XW-ADU5660導(dǎo)航設(shè)備的精度，是一種有效的方法。

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

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

[3] 董旭榮，張守信，華仲春.GPS/INS組合導(dǎo)航定位及其應(yīng)用[M].長(zhǎng)沙：國(guó)防科技大學(xué)出版社，1998：12-13.

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

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

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

[7] 王睿.基于多傳感器融合的定位系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[D].北京：北京郵電大學(xué)，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]寧津生，姚宜斌，張小紅.全球?qū)Ш叫l(wèi)星系統(tǒng)發(fā)展綜述[J].導(dǎo)航定位學(xué)報(bào)，2013，1(1)：3-8.

收稿日期：2014-05-26

第一作者簡(jiǎn)介：楊安洪(1983)，男，四川彭山人，工程師，研究方向?yàn)楹教鞙y(cè)控。

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

國(guó)家自然科學(xué)基金(41164001、41374039)。

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

P228

A

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