一種基于改進(jìn)卡爾曼濾波的GPS/BDS/SINS深組合定位算法

陳柯勛，邱 偉

(1.太原理工大學(xué) 信息與計(jì)算機(jī)學(xué)院，太原 030600；2.北京強(qiáng)度環(huán)境研究所，北京 100076)

隨著全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System，GNSS)各項(xiàng)技術(shù)的不斷發(fā)展，GNSS定位導(dǎo)航的精度和可靠性不斷得到提升，但是在電磁波干擾嚴(yán)重、衛(wèi)星信號(hào)受干擾嚴(yán)重的復(fù)雜地貌環(huán)境中，由于多普勒頻偏、多徑效應(yīng)等問(wèn)題，現(xiàn)有GNSS接收機(jī)的定位精度仍然差強(qiáng)人意，接收機(jī)的各項(xiàng)性能有待進(jìn)一步提升[1]。為了解決該問(wèn)題，許多研究單位和學(xué)者提出了不同的解決思路。例如，改進(jìn)天線設(shè)計(jì)，改進(jìn)接收機(jī)基帶信號(hào)處理算法，GNSS組合定位，GNSS與其他系統(tǒng)組合定位等。其中，多模多頻GNSS組合定位和GNSS與其他系統(tǒng)組合定位備受人們關(guān)注。因?yàn)槠淇梢猿浞纸Y(jié)合不同定位系統(tǒng)的特點(diǎn)，充分利用現(xiàn)有的衛(wèi)星信號(hào)資源和其他定位手段，直接解決了單系統(tǒng)可見(jiàn)衛(wèi)星數(shù)目少、衛(wèi)星信號(hào)幾何結(jié)構(gòu)差等問(wèn)題，大幅度提高接收機(jī)的定位精度和可靠性[2-3]。但現(xiàn)有的組合定位方法通常是通過(guò)大量的迭代運(yùn)算(犧牲計(jì)算效率)的方式來(lái)提高組合定位精度[4]。

為了進(jìn)一步解決該問(wèn)題，筆者提出了一種GPS/BDS/SINS深組合定位算法，該算法引入慣導(dǎo)測(cè)量值，借助漸消卡爾曼濾波修正定位初值，在提高定位精度的同時(shí)不需過(guò)多增加計(jì)算量。

1 算法總體設(shè)計(jì)

本文所設(shè)計(jì)的GPS/BDS/SINS深組合定位算法原理如圖1所示。首先，通過(guò)捷聯(lián)慣導(dǎo)系統(tǒng)得到接收機(jī)位置信息，將該位置信息與已知的衛(wèi)星位置信息進(jìn)行運(yùn)算，得到慣導(dǎo)系統(tǒng)的偽距和偽距率；其次，設(shè)計(jì)了一種基于模糊控制的漸消卡爾曼濾波器，并基于該濾波器設(shè)計(jì)了GPS/BDS/SINS深組合定位算法，該算法中漸消卡爾曼濾波器的輸入為慣導(dǎo)系統(tǒng)、GPS和BDS系統(tǒng)的偽距和偽距率，通過(guò)對(duì)量測(cè)噪聲和系統(tǒng)噪聲進(jìn)行在線估計(jì)，該濾波器可以輸出GPS和BDS系統(tǒng)偽距和偽距率的修正值，進(jìn)而提高GPS和BDS偽距測(cè)量值的精度，形成完備的GPS和BDS偽距測(cè)量值閉環(huán)修正系統(tǒng)；最后，再通過(guò)已經(jīng)被廣泛應(yīng)用的加權(quán)最小二乘法[5]組合解算高精度的GPS和BDS偽距測(cè)量值，得到高精度的位置信息。

圖1 GPS/BDS/SINS深組合定位算法原理框圖Fig.1 Principle block diagram of GPS/BDS/INS integrated positioning algorithm

2 漸消自適應(yīng)卡爾曼濾波

傳統(tǒng)卡爾曼濾波計(jì)算過(guò)程可能由于模型不準(zhǔn)確而出現(xiàn)發(fā)散[6]，因此本文通過(guò)引入漸消記憶因子來(lái)調(diào)整新舊測(cè)量向量對(duì)濾波估計(jì)值的校正效果[7]。

假設(shè)，系統(tǒng)的狀態(tài)和量測(cè)模型可表示為：

Xk=Ak,k-1Xk-1+Bk,k-1uk+Gk,k-1wk-1,

(1)

Zk=HkXk+vk.

(2)

(3)

(4)

(5)

根據(jù)傳統(tǒng)Kalman濾波方法的思想，可得到漸消記憶濾波的方程如(6)-(9)所示：

(6)

(7)

(8)

(9)

(10)

(11)

當(dāng)λ=1時(shí)，判斷條件可表示為：

(12)

(13)

當(dāng)濾波過(guò)程穩(wěn)定時(shí)，可用前N個(gè)時(shí)刻新息的協(xié)方差均值作為當(dāng)前時(shí)刻新息的協(xié)方差，即

(14)

因此，漸消自適應(yīng)Kalman濾波的方程為：

(15)

(16)

(17)

(18)

(19)

3 GPS/BDS/SINS深組合定位算法

GPS/BDS/SINS深組合定位算法，其原理是將三個(gè)系統(tǒng)的觀測(cè)誤差通過(guò)自適應(yīng)Kalman濾波器聯(lián)合估計(jì)，通過(guò)迭代降低定位誤差估計(jì)值，從而實(shí)現(xiàn)高精度定位。針對(duì)誤差估計(jì)過(guò)程，需要明確系統(tǒng)誤差方程和系統(tǒng)觀測(cè)方程[8-9]。

3.1 系統(tǒng)誤差方程

3.1.1平臺(tái)誤差角方程

在捷聯(lián)慣性導(dǎo)航系統(tǒng)中，平臺(tái)相對(duì)于慣性空間的轉(zhuǎn)動(dòng)角速度表示為：

(20)

(21)

平臺(tái)誤差角方程的矩陣表達(dá)式如下：

(22)

其中，φE,φN,φU代表ENU坐標(biāo)系平臺(tái)誤差角，ωie為地球自轉(zhuǎn)角速度。

3.1.2速度誤差方程和位置誤差方程

速度誤差方程矩陣表達(dá)式如下：

(23)

(24)

式中：vE,vN,vU代表ENU坐標(biāo)系速度；?vE,?vN為東北EN方向的速度誤差；?L,?λ分別為維度誤差和經(jīng)度誤差；fE,fN,fU代表ENU坐標(biāo)系的加速度計(jì)感受到的比特力。陀螺儀和加速度計(jì)在ENU方向上的漂移忽略不計(jì)[10]。

忽略天向速度誤差和高度誤差，GPS/BDS慣導(dǎo)組合導(dǎo)航的狀態(tài)變量可以設(shè)置為：

(25)

系統(tǒng)的噪聲向量為：

W=[ωεE,ωεN,ωεU,ωaE,ωaN,0,0,0,
0,0,0,0,0]T.

(26)

式中：ωεE,ωεN,ωεU代表陀螺沿著東北天方向的零線漂移；ωaE,ωaN為加速度計(jì)沿東北方向的零線漂移，實(shí)際測(cè)量漂移可以假設(shè)服從高斯分布。

3.2 系統(tǒng)觀測(cè)方程

捷聯(lián)慣性導(dǎo)航位置信息觀測(cè)方程表示為：

(27)

在組合定位過(guò)程中，對(duì)GPS和BDS的定位結(jié)果取加權(quán)平均[11]，可以得到GPS/BDS組合定位觀測(cè)方程為：

(28)

式中：Lt,λt,ht為目標(biāo)真實(shí)位置；NNG,NEG,NUG為GPS接收機(jī)沿ENU三軸向方向的位置誤差；NNB,NEB,NUB為BDS接收機(jī)沿ENU三軸向方向的位置誤差。則位置矢量的觀測(cè)方程為：

(29)

由狀態(tài)方程和觀測(cè)方程，可以使用前文介紹的自適應(yīng)卡爾曼濾波算法進(jìn)行求解。

4 實(shí)驗(yàn)仿真分析

為了驗(yàn)證本文所述算法的有效性，基于本算法進(jìn)行定位解算，其結(jié)果通過(guò)散點(diǎn)圖的形式呈現(xiàn)，如圖2所示。從圖中可以看出，深組合定位算法在X、Y、Z三軸向均方根誤差(RMS)值分別為1.187，1.916，1.208 m；三維位置誤差為3.312 m.從仿真圖形和解算誤差結(jié)果來(lái)看，不管是單軸向還是三維位置分析，深組合定位算法定位誤差較低。

進(jìn)一步驗(yàn)證本文所述算法的有效性，通過(guò)實(shí)驗(yàn)仿真，將深組合定位算法、GPS/BDS組合定位算法和GPS單系統(tǒng)定位算法下的定位結(jié)果進(jìn)行對(duì)比分析，如圖3所示。從圖中可以看出，本文所述深組合定位算法的定位精度要遠(yuǎn)遠(yuǎn)高于GPS單系統(tǒng)定位算法，略高于GPS/BDS雙系統(tǒng)組合定位算法，進(jìn)一步說(shuō)明了本文所述算法的有效性。

圖2 深組合定位算法定位結(jié)果散點(diǎn)圖Fig.2 Scatter plot of positioning results of deep combined positioning algorithm

5 結(jié)語(yǔ)

本文在GPS/BDS偽距組合定位的基礎(chǔ)上引入慣性器件測(cè)量值，借助漸消卡爾曼濾波器設(shè)計(jì)了GPS/BDS/SINS深組合定位算法及算法性能驗(yàn)證實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，本文所述的深組合定位算法能夠進(jìn)一步提高GNSS組合定位算法的定位精度，實(shí)現(xiàn)高精度定位，可以為多模高精度GNSS接收機(jī)的設(shè)計(jì)與實(shí)現(xiàn)提供參考，具有一定的理論價(jià)值和實(shí)際意義。

圖3 三種定位解算算法定位結(jié)果Fig.3 Positioning results of three algorithm