朱立新，孟 ?

（1.解放軍電子工程學(xué)院，安徽 合肥30037；2.解放軍72671部隊，山東 濟南250022）

于二維狀態(tài)，即n=2，有：

0 引言

GPS／INS組合導(dǎo)航系統(tǒng)克服了各自缺點，使組合后的導(dǎo)航精度高于兩個系統(tǒng)單獨工作的精度［1］。當(dāng)接收機進行大角度運動，或是在城市、峽谷等復(fù)雜環(huán)境中，衛(wèi)星信號容易受到遮擋或者干擾［2］，此時以偽距和偽距率為觀測量的GPS／INS緊組合能夠利用少于4顆衛(wèi)星提供的觀測值來使組合模式得以維持［3］。

針對GPS／INS 緊組合導(dǎo)航模型的非線性特點，采用狀態(tài)方程為線性，量測方程為非線性的線性／非線 性 混 合 模 型［4］。EKF（Extended Kalman filter，擴展卡爾曼濾波）和UKF（Unscented Kalman filter，無跡卡爾曼濾波）是典型的非線性濾波，EKF將非線性方程按泰勒級數(shù)展開，取一階截斷作為原方程的近似。這種方法雖然能夠在一定程度上解決非線性系統(tǒng)的狀態(tài)估計問題，但存在明顯缺點：首先，略去了高階項，帶來了截斷誤差，這種截斷誤差嚴(yán)重影響估計精度；其次，需要計算雅克比矩陣，在方程非線性程度比較高時該矩陣的求取十分復(fù)雜，甚至難以得到。UKF的計算量和EKF相當(dāng)，性能上優(yōu)于EKF，且無需計算雅可比陣，其應(yīng)用領(lǐng)域不斷擴展；但UKF 在進行迭代濾波過程中需要矩陣分解和求逆運算，UKF狀態(tài)估計協(xié)方差矩陣難以保持正定性。

本文針對以上問題，在GPS／INS 緊組合導(dǎo)航衛(wèi)星信號缺失情況下，提出了基于強跟蹤濾波理論和均方根策略的容積卡爾曼濾波算法。

1 GPS／INS緊組合模型

衛(wèi)星信號正常情況下，緊組合是組合程度較深的組合方式，其主要特點是GPS接收機和慣性導(dǎo)航系統(tǒng)相互輔助。用GPS接收機給出的衛(wèi)星星歷數(shù)據(jù)和慣性導(dǎo)航系統(tǒng)給出的位置和速度，解算對應(yīng)于慣性導(dǎo)航系統(tǒng)位置和速度的偽距ρI 和偽距率˙ρI。把ρI 和˙ρI 與GPS接收機測量的ρG 和˙ρG 相比較作為觀測量，通過組合卡爾曼濾波器估計慣性導(dǎo)航系統(tǒng)和GPS接收機的誤差量，然后對兩個系統(tǒng)進行校正。其原理框圖如圖1所示［5］。

圖1 緊組合工作框圖Fig.1 Work diagram of tightly-coupled

1.1 狀態(tài)方程

GPS誤差狀態(tài)變量為2維的XG（t）；，INS誤差XI（t）包括平臺誤差角（3維）、速度誤差（3維）、位置誤 差（3 維）、INS 元 件 誤 差（9 維）共 計18 維。GPS／INS組合導(dǎo)航系統(tǒng)的狀態(tài)方程可寫作：

即

其中：

分別表示姿態(tài)誤差、速度誤差、位置誤差、陀螺漂移誤差（常值偏移和隨機偏移）、加速度計誤差、GPS鐘差等效距離和等效距離變化率，F(xiàn) 是狀態(tài)轉(zhuǎn)移矩陣，G 為噪聲系數(shù)矩陣，W 為噪聲向量，具體參見參考文獻［6］。

1.2 量測方程

設(shè)載體在地球大地坐標(biāo)系中的慣導(dǎo)輸出位置可表示為經(jīng)度λI、緯度LI、高度hI，則載體真實位置可表示為λ=λI-Δλ，L=LI-ΔL，h=hI-Δh，將其轉(zhuǎn)換為地心地球固連（ECEF）坐標(biāo)系為：

則衛(wèi)星到載體真實距離ρj（j=1，2，3，4）可表示為：

GPS接收機測得到衛(wèi)星j 之間的偽距可表示為：

即：

將x、y、z帶入上式得：

同理，對于偽距率可得：

帶入ρ·j最終得到ρ·jG 為： 將x、y、z帶入上式即可得到偽距率的量測方程，以接收機輸出的偽距信號作為觀測量的量測方程（限于篇幅不再展開）表示為：

2 基于強跟蹤濾波理論和均方根策略的容積卡爾曼濾波算法

CKF是由加拿大學(xué)者Ienkaran Arasaratnam提出的基于確定性采樣的非線性濾波［7］，利用數(shù)值積分方法對目標(biāo)狀態(tài)后驗概率進行近似，通過確定一系列積分點和權(quán)值來直接計算后驗概率密度，在計算量和精度之間獲得了有效折衷。學(xué)者研究表明，對于維數(shù)高于3維的非線性系統(tǒng)，CKF 的濾波精度高于UKF，建議采用CKF最為濾波方法［8］。如前文所述，采用的組合導(dǎo)航模型狀態(tài)維數(shù)遠(yuǎn)高于3維，所以本文選用CKF。

CKF應(yīng)用于GPS／INS組合導(dǎo)航中表現(xiàn)出兩個問題：一是當(dāng)前時刻量測值對于一步預(yù)測值的修正作用受到以前時刻觀測值過強的抑制，致使濾波值不能較好地跟蹤真值變化［9］；二是由于計算機“截斷效應(yīng)”引入的代數(shù)運算誤差，導(dǎo)致誤差協(xié)方差陣的正定性和對稱性喪失，從而CKF濾波過程中的矩陣分解、求逆會進一步惡化該現(xiàn)象，使濾波不穩(wěn)定。

針對上述兩個問題，分別引入強跟蹤濾波理論和均方根策略對CKF進行改進，將改進的算法應(yīng)用于GPS／INS緊組合導(dǎo)航中的衛(wèi)星信號缺失情況下，驗證算法的有效性。

2.1 強跟蹤濾波器原理

假設(shè)狀態(tài)空間的系統(tǒng)模型和觀測模型為：

其中，xk為k時刻系統(tǒng)的狀態(tài)向量，zk為觀測值，wk和vk+1分別為系統(tǒng)噪聲和量測噪聲，且wk～N（0，Qk），vk～N（0，Rk），f為狀態(tài)轉(zhuǎn)移函數(shù)，h 為量測函數(shù)。

針對引言中提出的CKF 在組合導(dǎo)航應(yīng)用中的第一個問題，消除模型誤差所致濾波發(fā)散的一種思路是采用漸消記憶濾波，即在濾波過程中人為地相對突出新數(shù)據(jù)的作用，貶低舊數(shù)據(jù)的作用?；谠摾碚摶A(chǔ)，學(xué)者提出了一種帶次優(yōu)漸消因子的擴展卡爾曼濾波，成為強跟蹤濾波器（strong tracking filter，STF）［10-11］。

STF在狀態(tài)預(yù)測協(xié)方差陣Pk+1|k中引入漸消因子λk+1，實時調(diào)整增益矩陣Kk+1，使得同時滿足以下兩個條件：

如果模型與實際系統(tǒng)完全匹配時，上面兩式完全成立。若模型不確定則會造成濾波器的狀態(tài)估計值偏離系統(tǒng)的狀態(tài)，此時，在線調(diào)整增益矩陣，強迫式（15）成立，可以強迫強跟蹤濾波器保持對系統(tǒng)狀態(tài)的跟蹤。

強跟蹤濾波器的基本方程為：

式（16）中，λk+1為次優(yōu)漸消因子，其計算方法通常采用次優(yōu)算法，目的是為了提高算法的實時性。設(shè)ek+1=zk+1-^zk+1|k為輸出殘差序列，則：

式（17）中，tr（·）為對矩陣求跡，Vk+1為輸出殘差序列的協(xié)方差矩陣，實際計算中Vk+1是未知的，由下式進行估計：

式（18）中，0＜ρ≤1為遺忘因子，通常取ρ=0.95。

若已知狀態(tài)預(yù)測協(xié)方差矩陣Pk+1|k，估計值的自相關(guān)協(xié)方差矩陣Pzz，k+1|k和估計值的互相關(guān)協(xié)方差矩陣Pxz，k+1|k，則上文中Nk+1和Mk+1可以等效表示為：

2.2 均方根濾波策略

針對引言中提出的CKF 在組合導(dǎo)航應(yīng)用中的第二個問題，均方根策略是弱化矩陣病態(tài)影響的有效措施，在均方根容積卡爾曼濾波更新步驟中，參與迭代的是預(yù)測和后驗誤差協(xié)方差矩陣的均方根因子，而非協(xié)方差矩陣。而且利用最小二乘法計算增益，避免了矩陣求逆運算；利用三角分解或QR 分解獲取協(xié)方差矩陣的三角分解因子，而非均方根因子，這都增強了濾波的穩(wěn)定性。

一般的QR分解算法表示為S=Tria（A），其中S為下三角矩陣，矩陣A 和S 的關(guān)系為：假如R 是AT的QR 分解所得上三角矩陣，則S=RT。

2.3 強跟蹤SRCKF

把強跟蹤濾波原理和均方根策略與CKF 算法相結(jié)合，構(gòu)造一種強跟蹤SRCKF（該算法命名具有一定局限性），算法具體步驟如下。

2.3.1 時間更新和計算漸消因子

1）假設(shè)k 時刻的后驗概率p（xk）= N，Pk|k）已知，通過因式分解誤差協(xié)方差：

2）構(gòu)造Cubature點ξi表示集合［1］的第i列，對

于二維狀態(tài)，即n=2，有：

3）計算傳播后的積分點

4）計算k+1時刻狀態(tài)預(yù)測值

5）計算k+1時刻狀態(tài)協(xié)方差預(yù)測值

其中Sk+1|k為協(xié)方差的平方根系數(shù)。

式中，SQ，k為Qk的平方根系數(shù)，加權(quán)的的中心矩陣為且

計算漸消因子λk+1：

6）計算傳播后的積分點

7）計算k+1時刻量測估計值

8）計算k+1 時刻估計值的自相關(guān)協(xié)方差矩陣Pzz，k+1|k

其中，Szz，k+1|k為協(xié)方差的平方根系數(shù)。

式中，SR，k+1為Rk+1的平方根系數(shù)，加權(quán)的的中心矩陣為Zk+1|k且

9）計算k+1 時刻估計值的互相關(guān)協(xié)方差矩陣Pxz，k+1|k

其中：

按照2.1 節(jié)中的算法，可以計算出漸消因子λk+1，計算引入漸消因子的狀態(tài)協(xié)方差預(yù)測值Pk+1|k

2.3.2 量測更新

1）對狀態(tài)協(xié)方差預(yù)測值Pk+1|k進行分解，得：

2）執(zhí)行2.3.1部分中的6）、8）、9）步驟，計算量測估計值以及估計值的自相關(guān)和互相關(guān)協(xié)方差矩陣。

3）計算濾波增益矩陣

4）狀態(tài)更新

5）計算相關(guān)協(xié)方差矩陣及其平方根系數(shù)

3 仿真驗證與結(jié)果分析

為了驗證改進算法的有效性，針對衛(wèi)星信號缺失情況下GPS／INS組合導(dǎo)航的導(dǎo)航精度進行仿真實驗。

設(shè)計仿真時長為500s的飛行過程，其中包括勻速、加速、減速、轉(zhuǎn)彎、爬升、俯沖、平飛等過程，具體飛行軌跡如圖2所示。

仿真參數(shù)設(shè)置如下：SINS 的陀螺隨機漂移εb為0.01（°）／h；陀螺儀一階馬爾科夫過程隨機噪聲εr為0.01（°）／h；加速度計漂移誤差Δ 為10-4g；GPS隨機偽距誤差為10 m，隨機偽距率測量誤差為0.05m／s；設(shè)模擬飛行器的采樣周期為0.01s，捷聯(lián)慣性導(dǎo)航的更新頻率為50 Hz，GPS接收機的更新頻率為1Hz，濾波器的數(shù)據(jù)融合頻率同樣為1 Hz。由設(shè)定的仿真軌跡和仿真參數(shù)可以看出，在200～400s有加速、轉(zhuǎn)彎、俯沖等大角度機動，在此情況下容易出現(xiàn)衛(wèi)星信號缺失情況，仿真實驗中設(shè)置在該時間段內(nèi)衛(wèi)星數(shù)目為3顆，驗證改進算法在正常情況下和缺星情況下的有效性。限于篇幅，圖3、圖4為正常情況下強跟蹤SRCKF 和強跟蹤UKF 濾波精度對比（僅以東向速度誤差和經(jīng)度誤差為例）。

圖2 飛行軌跡仿真圖Fig.2 The figure of flight path Simulation

圖3 東向速度誤差曲線圖Fig.3 The curve of eastern velocity error

圖4 經(jīng)度誤差曲線圖Fig.4 The curve of longitude error

由以上仿真結(jié)果可以看出，在組合導(dǎo)航中，針對算法應(yīng)用過程中存在的同一問題，對CKF 和UKF進行改進，改進的CKF濾波精度高于改進UKF，驗證了文獻［8］的結(jié)論。

圖5—圖10為缺星情況下誤差仿真結(jié)果，表1和表2為不同情況下導(dǎo)航信息的均方根誤差對比。

圖5 經(jīng)度誤差曲線圖Fig.5 The curve of longitude error

圖6 緯度誤差曲線圖Fig.6 The curve of latitude error

圖7 高度誤差曲線圖Fig.7 The curve of height error

圖8 東向速度誤差曲線圖Fig.8 The curve of eastern velocity error

圖9 北向速度誤差曲線圖Fig.9 The curve of northern velocity error

圖10 天向速度誤差曲線圖Fig.10 The curve of vertical velocity error

表1 正常情況下位置、速度均方根誤差（RMSE）Tab.1 Position and speed RMSE under normal circumstances

表2 缺星情況下位置、速度均方根誤差（RMSE）Tab.2 Position and speed RMSE under signal invalidation circumstances

由圖5—圖10以及表1、表2的仿真結(jié)果可知，GPS／INS緊組合導(dǎo)航模式能夠在衛(wèi)星信號缺失情況下維持組合模式，提供一定精度范圍內(nèi)的導(dǎo)航信息。

基于強跟蹤濾波思想和均方根策略的改進CKF算法，通過提高當(dāng)前時刻觀測值的作用有效地減小了由于模型不確定性和在濾波過程中的計算問題所帶來的導(dǎo)航誤差。在衛(wèi)星信號正常情況下，和CKF算法相比，強跟蹤SRCKF 能夠提供更高精度的導(dǎo)航信息，導(dǎo)航精度能夠提高20%～30%；在衛(wèi)星信號缺失情況下，導(dǎo)航精度有所降低，改進算法能夠提供更加精確的導(dǎo)航信息，盡量減小由于衛(wèi)星信號缺失所帶來的影響，導(dǎo)航精度能夠提高30%～40%，改進的CKF 算法在衛(wèi)星信號缺失情況下，更能發(fā)揮其優(yōu)點。

組合導(dǎo)航對濾波算法的實時性有很高的要求，圖11對兩種算法的單次濾波迭代時間（該時間指在特定仿真環(huán)境下的時間，不能反映應(yīng)用中的運行時間，僅供相同條件下不同算法參考對照）進行對比，可知強跟蹤SRCKF 單次濾波迭代時間略高于CKF，這是由于算法在更新階段需要計算漸消因子λk+1，從而導(dǎo)致計算量有所增加，但仍然可以滿足實時性的要求。可見強跟蹤SRCKF是以計算量為代價來提高濾波精度。

圖11 濾波時間對比Fig.11 Filtering Time Contrast

4 結(jié)論

本文提出了基于強跟蹤濾波理論和均方根策略的容積卡爾曼濾波算法，該算法利用強跟蹤濾波突出了新數(shù)據(jù)的作用，提高了算法在模型不確定時的魯棒性；用均方根策略保證了協(xié)方差陣的正定性和對稱性。仿真實驗表明：與CKF 算法相比，強跟蹤SRCKF 能夠提供更高精度的導(dǎo)航信息，且在衛(wèi)星信號缺失情況下，更能發(fā)揮其優(yōu)點。雖然單次濾波迭代時間增加了40%左右，但是，就采用強跟蹤策略的非線性濾波算法而言，該算法的實時性仍然可以滿足要求。

［1］全偉.慣性／天文／衛(wèi)星組合導(dǎo)航技術(shù)［M］.北京：國防工業(yè)出版社，2011.

［2］Grewal M S，Weill L R，Andrews A P.Global Positioning Systems，Inertial Navigation and Integration［M］.New York：Wiley Interscience，2007.

［3］許剛，黃國榮，彭興釗，等.衛(wèi)星信號失效條件下SINS／GPS不同組合方式的性能比較［J］.計算機應(yīng)用研究，2012（10）：3888-3896.

［4］尹建君，張建秋，林青.Unscented卡爾曼濾波-卡爾曼濾波算法［J］.系 統(tǒng) 工 程 與 電 子 技 術(shù).2008，30（4）：617-620.

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

［6］王惠南.GPS 導(dǎo)航原理與應(yīng)用［M］.北京：科學(xué)出版社，2003.

［7］Ienkaran Arasaratnam，Simon Haykin.Cubature Kalman filters［J］.IEEE Trans on Automatic Control，2009，54（6）：1254-1269.

［8］孫楓，唐李軍.Cubature卡爾曼濾波與Unscented 卡爾曼濾波估計精度比較［J］.控制與決策，2013，28（2）：303-308.

［9］李莉.衰減記憶擴展卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用［J］.電子測量技術(shù)，2011，34（2）：36-38，65.

［10］周東華，席裕庚，張鐘俊.一種帶多重次優(yōu)漸消因子的擴展 卡 爾 曼 濾 波 器［J］.自 動 化 學(xué) 報，1991，16（7）：689-695.

［11］金天，王玉寶，叢麗，等.基于模糊邏輯的自適應(yīng)強跟蹤UKF定位濾波算法［J］.高技術(shù)通 訊，2012，22（4）：348-354.