基于簡(jiǎn)化聯(lián)邦CKF的INS/GNSS組合導(dǎo)航算法*

盧 航 郝順義 沈 飛 李保軍

空軍工程大學(xué)航空航天工程學(xué)院， 西安710038

慣導(dǎo)系統(tǒng)利用慣性元件測(cè)得角速度和比力，通過(guò)積分進(jìn)而獲得導(dǎo)航參數(shù),但由于陀螺和加計(jì)存在漂移，導(dǎo)航誤差會(huì)隨時(shí)間積累，因此需要高精度的飛行器進(jìn)行導(dǎo)航[1]。GPS作為天基無(wú)線電導(dǎo)航系統(tǒng)的一種，具有定位和測(cè)速精度高，誤差不隨時(shí)間積累的特點(diǎn)，但當(dāng)可見(jiàn)星數(shù)量不足時(shí)會(huì)影響導(dǎo)航精度，且其使用受制于人，不可完全依賴[2-4]。BDS作為我國(guó)自主研發(fā)的一套導(dǎo)航系統(tǒng)，可以提供載體高精度的位置、速度信息。因此，將INS、GPS和BDS各導(dǎo)航系統(tǒng)進(jìn)行組合，可以在很大程度上提升組合導(dǎo)航性能。但是，組合后如何能夠更好地利用各導(dǎo)航系統(tǒng)信息，形成一個(gè)高精度導(dǎo)航系統(tǒng)，是當(dāng)前多傳感器組合導(dǎo)航的主要研究任務(wù)之一。

利用卡爾曼濾波技術(shù)對(duì)導(dǎo)航誤差參數(shù)做最優(yōu)估計(jì)，其估計(jì)精度嚴(yán)重依賴所建模型的準(zhǔn)確度。而在高精度多傳感器組合導(dǎo)航系統(tǒng)中，模型階數(shù)一般很高，且為非線性，所以在對(duì)導(dǎo)航信息進(jìn)行融合處理時(shí)，優(yōu)先考慮采用聯(lián)邦卡爾曼濾波器(Federated Kalman Filter，F(xiàn)KF)，而非集中卡爾曼濾波器。因?yàn)橄啾扔诼?lián)邦卡爾曼濾波器，集中卡爾曼濾波器雖然具有濾波精度高的優(yōu)點(diǎn)，但其模型階數(shù)高，計(jì)算量大，濾波器更易發(fā)散。此外，其容錯(cuò)性差，不利于故障診斷與隔離。聯(lián)邦卡爾曼濾波器作為一種分散化濾波器，設(shè)計(jì)靈活、容錯(cuò)性能好，且在高精度多傳感器組合導(dǎo)航中，針對(duì)高階系統(tǒng)模型可以降低計(jì)算量，已被越來(lái)越廣泛應(yīng)用[5-6]。

傳統(tǒng)的聯(lián)邦卡爾曼濾波器要求主濾波器和各子濾波器的模型必須為線性，但組合導(dǎo)航系統(tǒng)模型的本質(zhì)是非線性的，采用傳統(tǒng)的聯(lián)邦卡爾曼濾波需要對(duì)系統(tǒng)進(jìn)行線性化，否則會(huì)影響系統(tǒng)的濾波性能[7]。針對(duì)這一問(wèn)題，文獻(xiàn)[8-9]提出粒子聯(lián)邦濾波算法，用于多傳感器組合導(dǎo)航系統(tǒng)，并取得了良好的效果，文獻(xiàn)[10-11]提出了把UKF和聯(lián)邦濾波結(jié)合，用以解決組合導(dǎo)航系統(tǒng)濾波中的非線性問(wèn)題。

本文針對(duì)高精度多傳感器組合導(dǎo)航中子系統(tǒng)模型為部分非線性，提出將簡(jiǎn)化的求容積卡爾曼濾波(Reduced Cubature Kalman Filter，RCKF)[12]融入到聯(lián)邦濾波結(jié)構(gòu)當(dāng)中，并將濾波器應(yīng)用于建立的INS/GPS/BDS緊組合模型進(jìn)行仿真，仿真結(jié)果與聯(lián)邦UKF和聯(lián)邦CKF算法進(jìn)行了對(duì)比，驗(yàn)證了論文提出算法的優(yōu)勢(shì)，取得了較好效果。

1 INS/GNSS聯(lián)邦濾波器設(shè)計(jì)

1.1 子濾波器算法

考慮如下離散非線性系統(tǒng)：

xk=Φk,k-1xk-1+Γk-1wk-1
zk=h(xk)+vk

(1)

式中，Φk,k-1∈Rn×n為線性狀態(tài)轉(zhuǎn)移矩陣；Γk-1為系統(tǒng)噪聲驅(qū)動(dòng)陣；h(·)為非線性量測(cè)函數(shù)；系統(tǒng)狀態(tài)向量：xk∈Rn；量測(cè)向量：zk∈Rm；wk-1和vk為互不相關(guān)的高斯白噪聲，其方差分別為Qk-1和Rk。

1.1.1 標(biāo)準(zhǔn)容積卡爾曼濾波算法

考慮如下形式的多維高斯權(quán)重積分：

(2)

其中：f(x)為非線性函數(shù)，Rn為積分區(qū)域，一般情況下，上述積分無(wú)法獲得解析解，需用數(shù)值積分方法得到其近似值，可選擇一組具有權(quán)重值(wi,ξi)的點(diǎn)集來(lái)近似積分，即

(3)

CKF采用Spherical-Radial準(zhǔn)則選取2n(n為狀態(tài)維數(shù))個(gè)具有相同權(quán)值的容積點(diǎn)(wi,ξi)，計(jì)算方法見(jiàn)式(4)，計(jì)算出容積點(diǎn)集后，可以通過(guò)時(shí)間更新和量測(cè)更新得到CKF濾波算法[13]。

計(jì)算容積點(diǎn)和權(quán)值：

(4)

式中，[1]=[I,-I]；I為n維單位矩陣；[1]i表示取[1]第i列。

CKF計(jì)算步驟如下：

1)初始化

給定均值和協(xié)方差：

(5)

計(jì)算容積點(diǎn)ξi和相應(yīng)權(quán)值wi。

2)時(shí)間更新

濾波協(xié)方差矩陣分解：

Sk-1=chol(Pk-1)

(6)

其中，chol{·}表示矩陣的Cholesky分解。

計(jì)算求積分點(diǎn)：

(7)

傳播求積分點(diǎn)：

(8)

狀態(tài)預(yù)測(cè)：

(9)

狀態(tài)預(yù)測(cè)協(xié)方差

(10)

3)量測(cè)更新

矩陣分解：

Sk/k-1=chol(Pk/k-1)

(11)

計(jì)算求積分點(diǎn)：

(12)

求積點(diǎn)傳播：

Zj,k/k-1=h(Xj,k/k-1)

(13)

量測(cè)預(yù)測(cè)值：

(14)

預(yù)測(cè)誤差協(xié)方差：

(15)

計(jì)算互協(xié)方差矩陣：

(16)

計(jì)算濾波增益：

(17)

狀態(tài)更新：

(18)

狀態(tài)協(xié)方差矩陣更新：

(19)

1.1.2 簡(jiǎn)化求容積卡爾曼濾波算法

考慮到系統(tǒng)的狀態(tài)方程為線性，量測(cè)方程為非線性，可在時(shí)間更新環(huán)節(jié)用線性卡爾曼濾波對(duì)CKF進(jìn)行簡(jiǎn)化，得到RCKF算法。在該環(huán)節(jié)中，標(biāo)準(zhǔn)CKF算法先要計(jì)算容積點(diǎn)和權(quán)值，后對(duì)濾波協(xié)方差矩陣進(jìn)行Cholesky分解，再計(jì)算容積點(diǎn)、傳播容積分點(diǎn)，最后根據(jù)權(quán)值計(jì)算狀態(tài)預(yù)測(cè)值預(yù)測(cè)協(xié)方差陣。其中，式(4)用到的矩陣分解，其運(yùn)算時(shí)間與狀態(tài)維數(shù)緊密相關(guān)，大大增加了計(jì)算量。而相同過(guò)程，RCKF算法則避免了計(jì)算容積點(diǎn)以及協(xié)方差矩陣分解，直接用狀態(tài)轉(zhuǎn)移矩陣計(jì)算一步預(yù)測(cè)值。

通過(guò)上述說(shuō)明可以給出該RCKF算法的實(shí)現(xiàn)流程如下：

1)時(shí)間更新

狀態(tài)預(yù)測(cè)：

(20)

狀態(tài)預(yù)測(cè)協(xié)方差陣:

(21)

2)量測(cè)更新

計(jì)算式(11)～(19)，即可完成RCKF濾波算法。

1.2 聯(lián)邦濾波器結(jié)構(gòu)

聯(lián)邦濾波器結(jié)構(gòu)如圖所示：

圖1 聯(lián)邦濾波器結(jié)構(gòu)

本文建立的SINS/GNSS緊組合導(dǎo)航聯(lián)邦濾波系統(tǒng)，將慣導(dǎo)系統(tǒng)設(shè)為公共參考系統(tǒng)，其輸出信息和GPS、BDS信息構(gòu)成子系統(tǒng)量測(cè)量。針對(duì)子系統(tǒng)模型為狀態(tài)線性、量測(cè)非線性的問(wèn)題，提出將RCKF的計(jì)算過(guò)程融入到聯(lián)邦濾波結(jié)構(gòu)框架中，構(gòu)成一種新的簡(jiǎn)化聯(lián)邦CKF濾波算法。相比于標(biāo)準(zhǔn)CKF算法，在時(shí)間更新過(guò)程中，RCKF算法狀態(tài)轉(zhuǎn)移矩陣計(jì)算一步預(yù)測(cè)和預(yù)測(cè)協(xié)方差矩陣，避免了采用求容積點(diǎn)近似計(jì)算的復(fù)雜過(guò)程及協(xié)方差矩陣分解，大大減少了計(jì)算量。

聯(lián)邦濾波采用方差上屆技術(shù)、信息分配方法使得各子濾波器估計(jì)狀態(tài)可以按不相關(guān)的方式處理，具體算法流程如下：

2)將主、子濾波器的過(guò)程噪聲協(xié)方差陣按式(23)設(shè)置為組合系統(tǒng)過(guò)程噪聲協(xié)方差陣的γi倍；

3)各子濾波器處理自己的量測(cè)信息，獲得相應(yīng)估計(jì)量；

4)在得到各子濾波器的局部估計(jì)和主濾波器的估計(jì)后按式(24)進(jìn)行最優(yōu)融合，得到主濾波器的濾波值和方差信息。

(22)

(23)

(24)

2 INS/GNSS組合聯(lián)邦濾波模型

2.1 系統(tǒng)狀態(tài)方程線性模型

組合導(dǎo)航誤差狀態(tài)方程如下[1]：

(25)

式中X(t)是22維誤差狀態(tài)向量，W(t)為系統(tǒng)噪聲，G(t)為噪聲分配陣，X(t)表示為：

X(t)=[φEφNφUδvEδvNδvUδLδλδhεbx
εbyεbzεrxεryεrzxyzδtGδtrGδtBδtrB]T

(26)

其中φE，φN和φU分別表示東、北、天向的失準(zhǔn)角誤差；δvE，δvN和δvU表示3個(gè)方向上的速度誤差；δL，δλ和δh為位置誤差；εbx,εby,εbz,εrx,εry,εrz,x,y和z分別表示陀螺常值漂移、相關(guān)漂移和加計(jì)零偏；δtG，δtB，δtrG和δtrB分別為GPS、BDS的時(shí)鐘誤差等效的距離誤差，與時(shí)鐘頻率誤差等效的距離誤差。

2.2 INS/GNSS量測(cè)方程非線性模型

2.2.1 偽距量測(cè)方程

GPS和BDS使用同一量測(cè)模型，S衛(wèi)星(S表示GPS或BDS)接收機(jī)測(cè)定的相對(duì)于第i(i=1,2,3,4)顆衛(wèi)星的偽距值為：

(27)

式中：(x,y,z)表示載體在地球坐標(biāo)系(ECEF)下的位置真值，(xsi,ysi,zsi)為第i顆衛(wèi)星的位置，δts為與時(shí)鐘誤差等效的距離誤差，vρi為接收機(jī)的偽距量測(cè)誤差。

慣導(dǎo)系統(tǒng)以東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，其所得的測(cè)量值是以緯度LI、經(jīng)度λI和高度hI表示的，那么載體的真實(shí)位置為：L=LI-ΔL,λ=λI-Δλ,h=hI-Δh，將其轉(zhuǎn)化到ECEF坐標(biāo)系，轉(zhuǎn)換關(guān)系如下：

(28)

式中，RN為卯酉圈主曲率半徑；e為橢圓扁心率。同理，慣導(dǎo)解算位置在ECEF系坐標(biāo)如下：

(29)

聯(lián)合式(28)、(29)帶入到式(27)，可得偽距量測(cè)方程為：

(30)

2.2.2 偽距率量測(cè)方程

S衛(wèi)星接收機(jī)測(cè)定的相對(duì)于第i(i=1,2,3,4)顆衛(wèi)星的偽距率為：

(31)

(32)

(33)

vE=vIE-δvE,vN=vIN-δvN,
vU=vIU-δvU

(34)

ECEF坐標(biāo)系下慣導(dǎo)輸出速度如下：

(35)

綜上各式，可得到偽距率量測(cè)方程為：

(36)

根據(jù)式(30)和(36)，可以得到衛(wèi)星的非線性量測(cè)方程為：

(37)

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

對(duì)比圖2～3及表1中數(shù)據(jù)發(fā)現(xiàn)，聯(lián)邦UKF(k=0)和聯(lián)邦CKF對(duì)位置誤差的估計(jì)精度相當(dāng)，其原因在于兩種確定性采樣非線性濾波方法所捕獲的均值和方差估計(jì)值都能達(dá)到泰勒展開(kāi)式的三階精度[14]。不過(guò)CKF使用2n個(gè)采樣點(diǎn)，而UKF需2n+1個(gè)采樣點(diǎn)，因此聯(lián)邦CKF計(jì)算量小一些。此外，UKF需合理地調(diào)節(jié)各參數(shù)因子，才能選擇出有效的sigma點(diǎn)，而CKF只需由式(4)便能得到容積點(diǎn)，其算法設(shè)計(jì)也更為簡(jiǎn)單。綜上，針對(duì)本文所建模型，相比于聯(lián)邦UKF，聯(lián)邦CKF在保證濾波精度相當(dāng)?shù)那闆r下，在算法實(shí)現(xiàn)復(fù)雜度方面更具有優(yōu)勢(shì)。

表1 定位誤差比較

圖2 聯(lián)邦UKF定位誤差

圖3 聯(lián)邦CKF定位誤差

圖4 簡(jiǎn)化聯(lián)邦CKF定位誤差

對(duì)比圖3～4以及表1中數(shù)據(jù)發(fā)現(xiàn)，簡(jiǎn)化聯(lián)邦CKF算法與聯(lián)邦CKF算法位置誤差估計(jì)精度相當(dāng)。但在子濾波器時(shí)間更新過(guò)程中，聯(lián)邦CKF算法先要計(jì)算容積點(diǎn)，后對(duì)濾波協(xié)方差矩陣進(jìn)行Cholesky分解，再計(jì)算求積點(diǎn)、傳播求積分點(diǎn)，最后根據(jù)權(quán)值計(jì)算狀態(tài)預(yù)測(cè)值、以及預(yù)測(cè)協(xié)方差陣，其中，求取平方根矩陣用到的矩陣分解，其運(yùn)算時(shí)間與狀態(tài)維數(shù)緊密相關(guān)，大大增加了計(jì)算量。而同樣過(guò)程，簡(jiǎn)化聯(lián)邦CKF算法則避免了計(jì)算求積點(diǎn)以及協(xié)方差矩陣分解，直接用狀態(tài)轉(zhuǎn)移矩陣計(jì)算一步預(yù)測(cè)值，減小了運(yùn)算成本。

將2種算法用在仿真環(huán)境為：Lenovo M4360 i3-3.4GHz CPU, 2G內(nèi)存,Matlab R2009a 32位版本的計(jì)算機(jī)上進(jìn)行仿真；得一次聯(lián)邦CKF的濾波時(shí)間為0.004312s；一次簡(jiǎn)化聯(lián)邦CKF的濾波時(shí)間為0.002339s。通過(guò)比較可知，簡(jiǎn)化聯(lián)邦CKF算法的時(shí)間消耗減少了近46%。

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

由于濾波器對(duì)導(dǎo)航參數(shù)的估計(jì)精度嚴(yán)重依賴于建模準(zhǔn)確度，所以在高精度組合導(dǎo)航中，系統(tǒng)模型階數(shù)一般很高，且為非線性。但考慮到系統(tǒng)實(shí)時(shí)性，模型中狀態(tài)方程階數(shù)遠(yuǎn)高于量測(cè)方程，將狀態(tài)方程線性化，建立了狀態(tài)方程為線性、量測(cè)方程為非線性的系統(tǒng)模型，同時(shí)選用計(jì)算簡(jiǎn)單、設(shè)計(jì)靈活、容錯(cuò)性能好的聯(lián)邦濾波器做狀態(tài)估計(jì)。本文針對(duì)所建模型，提出了一種新的簡(jiǎn)化聯(lián)邦CKF濾波算法，該算法將CKF的計(jì)算過(guò)程融入到聯(lián)邦濾波結(jié)構(gòu)框架中，針對(duì)子系統(tǒng)模型存在狀態(tài)方程線性的問(wèn)題，對(duì)算法的時(shí)間更新過(guò)程進(jìn)行了簡(jiǎn)化，簡(jiǎn)化算法避免了計(jì)算求積點(diǎn)及協(xié)方差矩陣分解，直接用狀態(tài)轉(zhuǎn)移矩陣計(jì)算一步預(yù)測(cè)值，減小了運(yùn)算成本。最后，通過(guò)仿真實(shí)驗(yàn)對(duì)本算法進(jìn)行了驗(yàn)證，將結(jié)果與聯(lián)邦UKF算法、聯(lián)邦CKF算法進(jìn)行對(duì)比。仿真結(jié)果表明，相比于聯(lián)邦UKF和CKF算法，所提算法在保證濾波精度的情況下，有效減小了計(jì)算量，改善了組合導(dǎo)航系統(tǒng)的性能。

參 考 文 獻(xiàn)

[1] 劉建業(yè),曾慶化,趙偉,等. 導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安:西北工業(yè)大學(xué)出版社,2010.

[2] Irokawa R, Ebinuma T. A Low-Cost Tightly Coupled GPS/INS for Small UAVs Augemented with Multiple GPS Antennas. 200935-44.

[3] Ali J, Mirza M. Performance for a low SINS/GPS [J].Nonlinear Dynamics, 2010, 61(3):491-502.

[4] Li Y, RIZOS C, J, et al. Sigma-point Kalman for Tightly Coupled GPS/INS Integration[J]. Journal of the Institute of Navigation, 2008, 55(3): 167-177.

[5] 張科,劉海鵬,李恒年,等. SINS/GPS/CNS組合導(dǎo)航聯(lián)邦濾波算法[J].中國(guó)慣性技術(shù)學(xué)報(bào), 2013, 21(2): 226-230.(Zhang Ke, Liu Haipeng, Li Hengnian, Qian Shan. SINS/GPS/CNS Integrated Navigation Federal Filtering Algorithm [J]. Journal of Chinese Inertial Technology, 2013, 21(2):226-230.)

[6] Carlson, N.A., Federated Filter for Distributed Navigation and Tracking Applications, Proc. ION 58th Annual Meeting, June 2002:340-353.

[7] A. Maier, S. Kiesel, G. F. Trommer. Performance Analysis of Federated Filter for SAR/TRN/GPS/INS Integration[J]. Gyroscopy and Navigation, 2011, 2(4):293-300.

[8] Aggarwal P, Syed Z, El-Sheimy N. Hybrid Extended Particle Filter(HEPF) for Integrated Cilvilian Navigation System.[C]//IEEE/ION Position, Location and Navigation Symposium, 2008:984-992.

[9] Lim Jaechan, Hong Daehyoung. Gaussian Particle Filtering Approach for Carrier Frequency Offset Estimation OFDM System[J]. IEEE Signal Processing Letters, 2013, 20(4):367-370.

[10] Hu H D, Huang X L. SINS/CNS/GPS Integrated Navigation Algorithm Based on UKF[J]. Journal of Systems Engineering and Electronics, 2010, 21(1):102-109.

[11] 李丹,劉建業(yè),熊智,等. 應(yīng)用聯(lián)邦自適應(yīng)UKF的衛(wèi)星多傳感器數(shù)據(jù)融合[J].應(yīng)用科學(xué)學(xué)報(bào), 2009, 27(4): 359-364.(Li Dan, Liu Jianye. Xiong Zhi, Yu Feng. Multi-sensor Data Fusion for Satellite Based on Federate Adaptive Unscented Kalman Filter [J]. Journal of Applied Sciences, 2009, 27(4):359-364.)

[12] Mohammed D, Abdelkrim M, Mokhtar K, Abdelziz O. Reduced Cubature Kalman Applied to Target Tracking [C]//The 2nd International Conference Control, Instrumentation and Automation (ICCIA), Shiraz: IEEE, 2011: 1097-1101.

[13] Arasaratnam I, Haykin S. Cubature Kalman Filter[J]. IEEE Trans on Automatic Control, 2009, 54(6): 1254-1269.

[14] 孫楓,唐李軍.基于Cubature Kalman Filter的INS/GPS組合導(dǎo)航濾波算法[J].控制與決策, 2012, 27(7): 1032-1036.(Sun Feng, Tang Lijun. INS/GPS Integrated Navigation Filter Algorithm Based on Cubature Kalman Filter [J]. Control and Decision, 2012, 27(7): 1032-1036.)