盧 航 郝順義 沈 飛 李保軍

空軍工程大學航空航天工程學院， 西安710038

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

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

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

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

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

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ū)動陣；h(·)為非線性量測函數(shù)；系統(tǒng)狀態(tài)向量：xk∈Rn；量測向量：zk∈Rm；wk-1和vk為互不相關的高斯白噪聲，其方差分別為Qk-1和Rk。

1.1.1 標準容積卡爾曼濾波算法

考慮如下形式的多維高斯權重積分：

(2)

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

(3)

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

計算容積點和權值：

(4)

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

CKF計算步驟如下：

1)初始化

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

(5)

計算容積點ξi和相應權值wi。

2)時間更新

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

Sk-1=chol(Pk-1)

(6)

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

計算求積分點：

(7)

傳播求積分點：

(8)

狀態(tài)預測：

(9)

狀態(tài)預測協(xié)方差

(10)

3)量測更新

矩陣分解：

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

(11)

計算求積分點：

(12)

求積點傳播：

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

(13)

量測預測值：

(14)

預測誤差協(xié)方差：

(15)

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

(16)

計算濾波增益：

(17)

狀態(tài)更新：

(18)

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

(19)

1.1.2 簡化求容積卡爾曼濾波算法

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

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

1)時間更新

狀態(tài)預測：

(20)

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

(21)

2)量測更新

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

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

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

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

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

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

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

3)各子濾波器處理自己的量測信息，獲得相應估計量；

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

(22)

(23)

(24)

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

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

組合導航誤差狀態(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分別表示東、北、天向的失準角誤差；δvE，δvN和δvU表示3個方向上的速度誤差；δL，δλ和δh為位置誤差；εbx,εby,εbz,εrx,εry,εrz,x,y和z分別表示陀螺常值漂移、相關漂移和加計零偏；δtG，δtB，δtrG和δtrB分別為GPS、BDS的時鐘誤差等效的距離誤差，與時鐘頻率誤差等效的距離誤差。

2.2 INS/GNSS量測方程非線性模型

2.2.1 偽距量測方程

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

(27)

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

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

(28)

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

(29)

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

(30)

2.2.2 偽距率量測方程

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

(31)

(32)

(33)

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

(34)

ECEF坐標系下慣導輸出速度如下：

(35)

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

(36)

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

(37)

3 仿真實驗及分析

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

表1 定位誤差比較

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

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

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

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

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

4 結(jié)束語

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

參 考 文 獻

[1] 劉建業(yè),曾慶化,趙偉,等. 導航系統(tǒng)理論與應用[M].西安:西北工業(yè)大學出版社,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組合導航聯(lián)邦濾波算法[J].中國慣性技術學報, 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è),熊智,等. 應用聯(lián)邦自適應UKF的衛(wèi)星多傳感器數(shù)據(jù)融合[J].應用科學學報, 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組合導航濾波算法[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.)