葉 晨，崔雙喜

0 引 言

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

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

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

1 UKF算法

UKF和標(biāo)準(zhǔn)KF都屬于線性最小方差估計(jì)濾波算法。但與標(biāo)準(zhǔn)KF不同的是，UKF算法根據(jù)被估計(jì)量和量測(cè)量的協(xié)方差陣來(lái)確定最佳增益陣，協(xié)方差陣又根據(jù)復(fù)現(xiàn)的 1倍σ樣本點(diǎn)來(lái)計(jì)算，這些樣本點(diǎn)則根據(jù)系統(tǒng)方程和量測(cè)方程來(lái)確定且并未對(duì)系統(tǒng)方程和量測(cè)方程提出任何附加條件。因此算法既適用于線性對(duì)象，也適用于非線性對(duì)象[7]。

本文考慮如下離散時(shí)間非線性系統(tǒng)：

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

式中kQ為非負(fù)定矩陣；kR為正定矩陣；kjδ為Kronecker-δ函數(shù)。

標(biāo)準(zhǔn)UKF算法的具體流程[4]如下：

a）初始化。

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

式中 P為協(xié)方差。

計(jì)算采樣點(diǎn)：對(duì)于 n ≥1，在僅考慮輸入變量的均值X?和協(xié)方差P的情況下，將X?和P由Sigma點(diǎn)近似，由 Sigma點(diǎn)對(duì)稱采樣策略的采樣函數(shù)，可得到如下2 n +1個(gè)采樣點(diǎn)：

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

式中 參數(shù)α是很小的正數(shù)，可取 1 0?4≤α≤1；κ=3? n ；確定采樣點(diǎn)的權(quán)值：

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

b）預(yù)測(cè)方程。

c）更新方程。

2 強(qiáng)跟蹤UKF算法

2.1 強(qiáng)跟蹤濾波器

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

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

2.2 漸消因子的引入

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

式中

稱為漸消矩陣。

為n個(gè)漸消因子。由系統(tǒng)的先驗(yàn)知識(shí)可大致確定：

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

式中 β為弱化因子；1β≥，引入弱化因子可以使?fàn)顟B(tài)估計(jì)更加平滑，它的取值可以通過(guò)經(jīng)驗(yàn)來(lái)確定；kV為實(shí)際輸出殘差序列的協(xié)方差陣，可有下式估算：

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

3 深組合導(dǎo)航模型建立

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

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

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

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

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

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

系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣 F ( t )、噪聲驅(qū)動(dòng)矩陣 G ( t)和噪聲向量 W ( t)的計(jì)算公式見參考文獻(xiàn)[14]。

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

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

式中，ruT為時(shí)鐘漂移的相關(guān)時(shí)間；uw為GPS時(shí)鐘誤差白噪聲，ruw為 GPS時(shí)鐘頻率誤差白噪聲。

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

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

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

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

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

故，偽距率差非線性觀測(cè)方程為

同樣可推導(dǎo)出偽距率差非線性觀測(cè)方程為

式中 ρ˙為此刻真實(shí)的偽距率；rulδ為由GPS時(shí)鐘引起的距離誤差；vρ˙為測(cè)量噪聲。具體推導(dǎo)過(guò)程見文獻(xiàn)[14]。應(yīng)注意，由于采用非線性UKF濾波方法，系統(tǒng)觀測(cè)方程無(wú)需線性化。

4 仿真校驗(yàn)

將提出的強(qiáng)跟蹤UKF算法應(yīng)用于GPS/SINS深組合導(dǎo)航進(jìn)行仿真校驗(yàn)，并且與標(biāo)準(zhǔn)UKF算法進(jìn)行比較。

4.1 仿真條件

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

4.2 仿真結(jié)果與分析

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

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

圖2 標(biāo)準(zhǔn)UKF定位誤差曲線Fig.2 Positioning Error of Standard UKF

圖3 強(qiáng)跟蹤UKF定位誤差曲線Fig.3 Positioning Error of Strong Tracking UKF

5 結(jié) 論

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