一種強跟蹤UKF及其在GPS/SINS深組合導航中的應用

葉 晨，崔雙喜

0 引 言

基于GPS/SINS的深組合導航系統與松/緊組合導航不同，信息融合程度更深。其中導航濾波器的狀態(tài)更新可由接收機通道采樣值提供，濾波輸出值校正碼產生器與載波的控制值，使得載波相位跟蹤帶寬更寬，抗干擾性能更強。然而在動態(tài)高、干擾過強等惡劣環(huán)境下，接收機相關器輸出值仍具有較強的非線性[1]。

針對這種非線性問題，一般有兩種方法[2～3]：a）將非線性函數進行線性化處理，只保留低階項信息，經典方法是擴展卡爾曼濾波（Extended Kalman Filter，EKF）；b）使用采樣的方法近似非線性分布，該方法可以較好地避免EKF存在的部分問題，如無損卡爾曼濾波（Unscented Kalman Filter, UKF）算法[4～5]。然而，狀態(tài)突變、模型不準確均可能引起系統的跟蹤能力變差、魯棒性能降低，針對此難題，根據新息向量的正交性原理，有學者提出了強跟蹤濾波器的思想，并運用在EKF的改進中，可有效處理模型不確定性問題，提高系統的動態(tài)跟蹤性能[6]。

系統狀態(tài)異常時，標準UKF無法自適應調節(jié)，將導致組合導航濾波結果精度降低。本文立足于基本UKF算法框架，并結合強跟蹤濾波的基本理論，可使狀態(tài)預測估計協方差矩陣在線自適應修改調整，建立一種多重漸消因子的強跟蹤UKF跟蹤器，并將此算法運用到發(fā)射慣性系下 GPS/SINS深組合導航仿真研究中。結果表明，當狀態(tài)系統、量測系統遭到干擾時，該算法相對于標準UKF估計精度更高、適應性能更強。

1 UKF算法

UKF和標準KF都屬于線性最小方差估計濾波算法。但與標準KF不同的是，UKF算法根據被估計量和量測量的協方差陣來確定最佳增益陣，協方差陣又根據復現的 1倍σ樣本點來計算，這些樣本點則根據系統方程和量測方程來確定且并未對系統方程和量測方程提出任何附加條件。因此算法既適用于線性對象，也適用于非線性對象[7]。

本文考慮如下離散時間非線性系統：

式中 Xk， Zk分別為k時刻系統的狀態(tài)向量、觀測向量，Xk∈Rn,Zk∈Rm； Fk為線性狀態(tài)轉移矩陣； h(·)為系統非線性量測函數； wk， vk為零均值高斯白噪聲序列，向量互不相關，統計特性滿足如下條件：

式中kQ為非負定矩陣；kR為正定矩陣；kjδ為Kronecker-δ函數。

標準UKF算法的具體流程[4]如下：

a）初始化。

假定系統的初始狀態(tài)為0X高斯分布的隨機向量狀態(tài)初始化條件：

式中 P為協方差。

計算采樣點：對于 n ≥1，在僅考慮輸入變量的均值X?和協方差P的情況下，將X?和P由Sigma點近似，由 Sigma點對稱采樣策略的采樣函數，可得到如下2 n +1個采樣點：

式中 ξ為狀態(tài)量參數；()i為矩陣Pk?1均方根的第i列；nγλ=+，參數λ的確定為

式中 參數α是很小的正數，可取 1 0?4≤α≤1；κ=3? n ；確定采樣點的權值：

式中 β取值與X的分布有關，對于正態(tài)分布，2β=為最優(yōu)值。

b）預測方程。

c）更新方程。

2 強跟蹤UKF算法

2.1 強跟蹤濾波器

強跟蹤濾波器[6～8]（Strong Tracking Filter，STF）與普通的濾波器相比，狀態(tài)突變時的跟蹤能力更強，模型不確定性時魯棒性能更高。其主要思想是：實時調節(jié)修正濾波K矩陣，強制殘差向量彼此保持正交，這樣可使濾波器依然能夠跟蹤在系統模型不確定時的系統狀態(tài)變化，從而改善了UKF魯棒性差和濾波發(fā)散的問題。漸消因子λk根據系統可實時變化，并影響預測協方差矩陣Pk/k?1的構造，從而實時修正增益Kk。記導航濾波器輸出的殘差序列 εk= Z ?k? Z?k/k?1，則強跟蹤器應滿足如下條件：

式中表征濾波器可以實現最優(yōu)估計的性能要求；保證殘差序列向量在不同時刻保持正交，同時具有類似白噪聲的性質，將殘差序列的不相關性作為衡量濾波性能是否優(yōu)良的標志。

2.2 漸消因子的引入

經過理論基礎的支持，本系統采用多重次優(yōu)漸消因子，分別對不同的數據進行漸消，進一步提高濾波器的跟蹤能力。對一步預測狀態(tài)誤差協方差陣式（7）作如下調整[9～11]：

式中

稱為漸消矩陣。

為n個漸消因子。由系統的先驗知識可大致確定：

式中 α為可提前確定的常數，αi≥1；ck為待定因子，則多重漸消次優(yōu)因子的一步算法如下：

式中 β為弱化因子；1β≥，引入弱化因子可以使狀態(tài)估計更加平滑，它的取值可以通過經驗來確定；kV為實際輸出殘差序列的協方差陣，可有下式估算：

式中 ρ為遺忘因子，01ρ＜≤，一般取0.95。

3 深組合導航模型建立

GPS/SINS深組合導航系統的重要特征是GPS與SINS信息融合程度更深，涉及到慣導輔助GPS接收機內部跟蹤環(huán)路等深層次信息交互。

深組合導航系統中，首先采集 SINS輸出信息與GPS接收機輸出信息并進行處理，建立對SINS位置誤差、速度誤差及姿態(tài)誤差等狀態(tài)變量的系統狀態(tài)方程和量測方程，經過STUKF濾波器的自適應估計，輸出狀態(tài)誤差進行校正系統狀態(tài)，同時使用慣性信息輔助GPS接收機跟蹤環(huán)路，可實現兩個系統在觀測過程中的相互輔助。這種系統模式可降低載波環(huán)跟蹤環(huán)路噪聲誤差、抑制部分GPS跟蹤環(huán)路動態(tài)應力誤差，加強系統抗干擾性能和動態(tài)跟蹤能力，本系統中，采用基于偽距、偽距率的緊組合強跟蹤UKF濾波器，直接采用 GPS接收機的原始測量值（偽距、偽距率），不會引入GPS接收機導航解算帶來的誤差[12，13]。

3.1 深組合系統狀態(tài)方程

3.1.1 SINS的誤差狀態(tài)方程

SINS的誤差狀態(tài)方程為

式中 φx，φy，φz為姿態(tài)失準角；δVx，δVy，δVz為發(fā)射慣性系坐標系下三軸方向的速度誤差；δX，δY，δZ為發(fā)射慣性系三軸方向的位置誤差；εx，εy，εz和?x，?y，?z為彈體坐標系下陀螺儀常值漂移和加速度計常值偏置。

系統狀態(tài)轉移矩陣 F ( t )、噪聲驅動矩陣 G ( t)和噪聲向量 W ( t)的計算公式見參考文獻[14]。

3.1.2 GPS的誤差狀態(tài)方程

在深組合系統中，可認為距離率誤差rulΔ等效時鐘頻率誤差，距離誤差ulΔ等效時鐘誤差，作為GPS接收機的誤差狀態(tài)量，其狀態(tài)方程為

式中，ruT為時鐘漂移的相關時間；uw為GPS時鐘誤差白噪聲，ruw為 GPS時鐘頻率誤差白噪聲。

3.2 系統觀測方程

本系統中，發(fā)射慣性系彈道導彈GPS/SINS深組合導航系統分為偽距觀測方程和偽距率觀測方程兩個部分，其可表示如下：

式中 ρ為某一時刻導彈發(fā)射慣性系位置的真值為[X YZ ]T時，設該時刻導彈距離GPS衛(wèi)星理想的距離；ρi為SINS計算所得載體與第i顆衛(wèi)星間的偽距；ρg為GPS接收機得到位置與第i顆衛(wèi)星間的偽距為

式中ulδ為GPS的時鐘誤差引起的距離誤差；vρ為偽距量測噪聲。ρ與iρ可由下式計算得出：

式中為 SINS解算得到的導彈在發(fā)射慣性系下的位置；為第j顆衛(wèi)星轉換到發(fā)射慣性系下的位置坐標。設[δ X δY δZ ]T是SINS計算位置與真實位置的距離誤差，可知：

故，偽距率差非線性觀測方程為

同樣可推導出偽距率差非線性觀測方程為

式中 ρ˙為此刻真實的偽距率；rulδ為由GPS時鐘引起的距離誤差；vρ˙為測量噪聲。具體推導過程見文獻[14]。應注意，由于采用非線性UKF濾波方法，系統觀測方程無需線性化。

4 仿真校驗

將提出的強跟蹤UKF算法應用于GPS/SINS深組合導航進行仿真校驗，并且與標準UKF算法進行比較。

4.1 仿真條件

以仿真某彈道導彈軌跡為背景，設置仿真條件為：初始俯仰角為 90°，初始滾轉角和偏航角均為 0°；初始位置為：緯度32.08°，經度118.7°，高度為0 m；發(fā)射慣性系坐標系初始速度前向為394.891 7 m/s（地球自轉速度），天向和側向均為 0 m/s；發(fā)射方位角為 90°；陀螺零偏為10 (°)/h，白噪聲為1 (°)/h；加速度計零偏為1 mg，白噪聲為0.5 mg；收星數m=4。GPS采樣周期1 s，INS采樣周期0.005 s，濾波周期1 s，仿真時間360 s。在200～210 s時間段內，GPS偽距加入均值為0 m/s，標準差為100 m的白噪聲；GPS偽距率加入均值為0 m/s，標準差為1 m/s的白噪聲。

4.2 仿真結果與分析

圖1表示的是彈道導彈軌跡，圖2是標準UKF定位誤差曲線，圖 3是強跟蹤 UKF定位誤差曲線。由圖2與圖3相比較可知，在系統受到強力干擾的情況下，強跟蹤UKF算法的優(yōu)勢凸顯出來，相比標準UKF算法，誤差波動較小，組合導航定位精度有大幅提升，具有更優(yōu)越的抗干擾能力和魯棒性。

圖1 發(fā)射點慣性系下的彈道導彈軌跡Fig.1 Trajectory of Ballistic Missile

圖2 標準UKF定位誤差曲線Fig.2 Positioning Error of Standard UKF

圖3 強跟蹤UKF定位誤差曲線Fig.3 Positioning Error of Strong Tracking UKF

5 結 論

本文提出了一種強跟蹤UKF算法，該算法通過引入多重漸消因子，使得濾波器具有更多的系統先驗知識，增加了系統的魯棒性能，并將該算法應用于發(fā)射慣性系下的GPS/SINS深組合導航中。仿真結果表明，該算法能為組合導航提供較高的導航精度，證明了該算法的有效性。