改進(jìn)的強(qiáng)跟蹤UKF在組合導(dǎo)航中的應(yīng)用研究

符 強(qiáng)，趙鴻悅，孫希延，3，紀(jì)元法，3*

(1. 桂林電子科技大學(xué)廣西精密導(dǎo)航技術(shù)與應(yīng)用重點(diǎn)實(shí)驗(yàn)室，廣西 桂林 541004；2. 衛(wèi)星導(dǎo)航定位與位置服務(wù)國(guó)家地方聯(lián)合工程研究中心，廣西 桂林 541004；3. 衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，河北 石家莊 050000)

1 引言

在組合導(dǎo)航緊耦合模型中，通常采用偽距和偽距率作為量測(cè)值，而這類量測(cè)值具有非線性特點(diǎn)[1]。UKF算法是目前處理非線性系統(tǒng)的一種有效算法[2]。然而標(biāo)準(zhǔn)UKF也有其局限性，如對(duì)系統(tǒng)模型精確性要求較高，在系統(tǒng)狀態(tài)突變或模型不準(zhǔn)確情況下，UKF濾波精度會(huì)大大降低甚至發(fā)散[3][4]。為了克服UKF的局限性，有學(xué)者將強(qiáng)跟蹤濾波和UKF濾波相結(jié)合，提高了系統(tǒng)在復(fù)雜環(huán)境下的穩(wěn)定性，但每個(gè)濾波周期都要經(jīng)過(guò)三次UT變換，計(jì)算量復(fù)雜，嚴(yán)重影響實(shí)時(shí)性[5]；文獻(xiàn)[6]對(duì)UKF濾波做出簡(jiǎn)化，并在大失準(zhǔn)角條件下實(shí)現(xiàn)了靜基座下SINS初始對(duì)準(zhǔn)。文獻(xiàn)[7]采用假設(shè)檢驗(yàn)的方法對(duì)系統(tǒng)狀態(tài)做出檢測(cè)，再利用漸消因子做出自適應(yīng)調(diào)整。文獻(xiàn)[8]對(duì)強(qiáng)跟蹤中次優(yōu)漸消因子的求解方法做出改進(jìn)，提高了強(qiáng)跟蹤UKF的實(shí)用性。

針對(duì)強(qiáng)跟蹤UKF在緊耦合模型中的局限性，特別是模型不匹配[9]、濾波性能欠佳及計(jì)算過(guò)程中迭代不穩(wěn)定的問(wèn)題，本文提出一種改進(jìn)的強(qiáng)跟蹤UKF算法，能夠提高UKF對(duì)緊耦合模型的適應(yīng)能力，與傳統(tǒng)的強(qiáng)跟蹤UKF相比，該算法改進(jìn)UKF與強(qiáng)跟蹤的結(jié)合方式，增強(qiáng)濾波的迭代穩(wěn)定性，改善了漸消因子的求解方式，減少了冗余計(jì)算，提高了濾波性能。本文分別運(yùn)用標(biāo)準(zhǔn)UKF、強(qiáng)跟蹤UKF和改進(jìn)的強(qiáng)跟蹤UKF在相同條件下對(duì)系統(tǒng)進(jìn)行濾波，比較不同算法的定位誤差。

2 組合導(dǎo)航系統(tǒng)模型

本文選取東北天坐標(biāo)系作為導(dǎo)航坐標(biāo)系，狀態(tài)方程中采用17個(gè)狀態(tài)參數(shù)，具體的狀態(tài)量為

(1)

X(t)=F(t)X(t)+W(t)

(2)

其中，F(xiàn)(t)表示狀態(tài)轉(zhuǎn)移矩陣，X(t)表示系統(tǒng)狀態(tài)誤差量，W(t)為系統(tǒng)過(guò)程噪聲。

將INS計(jì)算的偽距和偽距率和GPS的偽距和偽距率之差作為量測(cè)值Z，量測(cè)方程可以表示為

Z(t)=H(t)X(t)+V(t)

(3)

式中，H(t)表示偽距、偽距率的非線性觀測(cè)方程，V(t)表示白噪聲。

3 標(biāo)準(zhǔn)UKF算法

由前面可知，組合導(dǎo)航離散系統(tǒng)可表示為：

(4)

其中，F(xiàn)k表示k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，Xk表示k時(shí)刻的狀態(tài)向量，Hk表示觀測(cè)方程，Zk表示k時(shí)刻的量測(cè)向量，Wk和Vk表示高斯白噪聲且互不相關(guān)。

標(biāo)準(zhǔn)UKF對(duì)系統(tǒng)方程的濾波步驟為：

1)初始化

2)利用均值、方差得到Sigma采樣點(diǎn)集及其對(duì)應(yīng)的權(quán)值。

3)將點(diǎn)集做一步預(yù)測(cè)

4)將Sigma點(diǎn)集的預(yù)測(cè)值加權(quán)求和，得到系統(tǒng)狀態(tài)量的一步預(yù)測(cè)和協(xié)方差。

5)根據(jù)一步預(yù)測(cè)值，再次使用UT變換，產(chǎn)生新的Sigma點(diǎn)集。

6)將新產(chǎn)生的Sigma點(diǎn)集帶入觀測(cè)方程，得到預(yù)測(cè)的觀測(cè)值。

7)將步驟6)的Sigma點(diǎn)集的觀測(cè)預(yù)測(cè)值加權(quán)求和得到系統(tǒng)預(yù)測(cè)的均值及協(xié)方差。

8)計(jì)算卡爾曼增益

9)更新系統(tǒng)的狀態(tài)向量和協(xié)方差。

4 改進(jìn)的強(qiáng)跟蹤UKF算法

傳統(tǒng)結(jié)構(gòu)下的強(qiáng)跟蹤UKF在每個(gè)濾波周期中都需要進(jìn)行三次UT變換[11]，每次變換都會(huì)有大量計(jì)算，這會(huì)嚴(yán)重影響濾波性能。本文通過(guò)重構(gòu)漸消矩陣的求解方式和改進(jìn)強(qiáng)跟蹤UKF的濾波結(jié)構(gòu)，增強(qiáng)算法與模型的匹配度，提高濾波迭代的穩(wěn)定性和濾波精度。

4.1 漸消矩陣的求解

組合導(dǎo)航系統(tǒng)除了受自身的非線性因素影響外，主要是受到量測(cè)噪聲的影響，標(biāo)準(zhǔn)UKF對(duì)系統(tǒng)模型的嚴(yán)格要求，在大量的量測(cè)噪聲影響下很難精確定位，強(qiáng)跟蹤是一種有效的解決辦法。強(qiáng)跟蹤濾波應(yīng)滿足如下條件[12]

(5)

在UKF中估計(jì)誤差與預(yù)測(cè)誤差為

(6)

(7)

(8)

根據(jù)殘差序列的定義可知

εk=Zk-k/k-1

(9)

根據(jù)量測(cè)方程可知

Zk=H(Xk)

(10)

(11)

則由式(10)、式(11)得殘差序列表示為

(12)

那么k+j時(shí)刻的殘差矩陣可以表示為

(13)

根據(jù)強(qiáng)跟蹤的正交性原理可知

(14)

將式(12)、式(13)帶入可知[7]

(P-KkVk)

(15)

要保證正交性，即式(16)成立即可

P-KkVk=0

(16)

根據(jù)卡爾曼增益公式，式(17)可變形為

Kk(P-Vk)=0

(17)

由于增益矩陣為非零矩陣，式可以簡(jiǎn)化為

P-Vk=0

(18)

將協(xié)方差展開可得

(19)

定義Nk，Mk為

Nk=Vk-Rk

(20)

(21)

將式(19)用Nk，Mk表示為λkMk=Nk，對(duì)兩邊求跡，得到漸消因子τk

(22)

再根據(jù)式(23)、式(24)將求得的漸消因子組成漸消矩陣λk=diag(λ1，1，λ2，2，…λn，n)。

(23)

λi，i=τkκi

(24)

本文根據(jù)強(qiáng)跟蹤的正交性原則，重新構(gòu)筑漸消因子的求解方式，再組成漸消矩陣對(duì)每個(gè)狀態(tài)量做針對(duì)性調(diào)整，利用漸消矩陣直接作用于預(yù)測(cè)協(xié)方差陣，避免了傳統(tǒng)算法先求出標(biāo)準(zhǔn)狀態(tài)協(xié)方差、再通過(guò)UT變換重新計(jì)算預(yù)測(cè)協(xié)方差和互協(xié)方差，減少了UT變換次數(shù)，有效降低了計(jì)算量。

4.2 ISTUKF具體算法

由于INS/GNSS緊耦合系統(tǒng)的狀態(tài)方程具有線性特征[13]， 而UT變換得到的Sigma點(diǎn)集是圍繞狀態(tài)量的均值對(duì)稱分布，并且對(duì)稱點(diǎn)之間的權(quán)值相同。因此在狀態(tài)更新中采用UT變換，實(shí)際上是將Sigma點(diǎn)集進(jìn)行線性映射，此時(shí)求得的映射后的Sigma點(diǎn)集與根據(jù)一步預(yù)測(cè)值的均值和方差獲取的Sigma點(diǎn)集是等價(jià)的，因此UT變換并未起到作用，反而帶來(lái)了額外的計(jì)算量。因此對(duì)強(qiáng)跟蹤UKF濾波算法進(jìn)行結(jié)構(gòu)分解，采用KF與UKF混合濾波的方式與強(qiáng)跟蹤濾波相結(jié)合，減少?gòu)?qiáng)跟蹤在狀態(tài)方程中的UT變換的次數(shù)。以下為具體流程：

步驟1：已知k-1時(shí)刻的狀態(tài)值k-1與狀態(tài)協(xié)方差Pk-1。

步驟2：時(shí)間更新，對(duì)系統(tǒng)狀態(tài)量的一步預(yù)測(cè)及標(biāo)準(zhǔn)協(xié)方差矩陣

k/k-1=Fk/k-1k-1

(25)

(26)

步驟3：計(jì)算Sigma點(diǎn)及其權(quán)值

(27)

(28)

(29)

步驟5：由步驟4得到的Sigma點(diǎn)集進(jìn)行加權(quán)求和得到系統(tǒng)預(yù)測(cè)值的均值和標(biāo)準(zhǔn)協(xié)方差。

(30)

(31)

(32)

步驟6：利用漸消矩陣修正協(xié)方差值

Pk/k-1=λkP′k/k-1

(33)

P=λkP′

(34)

P=λkP′

(35)

步驟7：計(jì)算卡爾曼增益矩陣、更新狀態(tài)估計(jì)值和協(xié)方差矩陣，完成一個(gè)整周濾波。

(36)

(37)

(38)

綜上所述，與傳統(tǒng)的強(qiáng)跟蹤UKF算法相比，改進(jìn)的強(qiáng)跟蹤UKF根據(jù)狀態(tài)方程的線性特征，將第一次UT變換省略。第二次UT變換相同，得到一步預(yù)測(cè)值的Sigma點(diǎn)集。根據(jù)正交性原理，改進(jìn)漸消矩陣的求解方式和作用位置，令漸消矩陣直接作用于預(yù)測(cè)協(xié)方差，保證了增益矩陣受到漸消矩陣的影響，從而減少了第三次UT變換，使濾波周期中只有一次UT變換。

5 仿真與分析

在相同仿真條件下，分別驗(yàn)證改進(jìn)的強(qiáng)跟蹤UKF濾波在不同環(huán)境條件下的效果，將算法與標(biāo)準(zhǔn)UKF及強(qiáng)跟蹤UKF對(duì)飛行器整個(gè)飛行軌跡進(jìn)行濾波估計(jì)。

實(shí)驗(yàn)1：理想環(huán)境下模擬載體飛行軌跡，載體的起始位置設(shè)置為北緯-21.786°，東經(jīng)為170.463°，高度為105 m，載體軌跡包含加速、減速、轉(zhuǎn)彎、下降和勻速等飛行狀態(tài)，飛行速度最高200 m/s，飛行軌跡如圖1所示。仿真共進(jìn)行180 s，慣導(dǎo)系統(tǒng)的采樣周期0.01 s。加速度一階馬爾科夫過(guò)程相關(guān)時(shí)間為300s。衛(wèi)星接收機(jī)的濾波和輸出周期均為1 s，速度誤差為0.1 m/s，水平方向誤差為5 m，天向誤差為5 m。分別利用3種濾波算法進(jìn)行濾波，為了能夠直觀對(duì)比不同濾波的性能，本文將標(biāo)準(zhǔn)UKF、強(qiáng)跟蹤UKF和強(qiáng)跟蹤混合濾波3種濾波的速度誤差、位置誤差和姿態(tài)誤差進(jìn)行比較。圖2為3種濾波的位置誤差。

圖1 載體飛行軌跡

圖2 東向位置誤差對(duì)比

圖3 北向位置誤差對(duì)比

由圖2可知，三種濾波處理后的位置誤差比較穩(wěn)定，沒(méi)有隨時(shí)間進(jìn)行積累，其中ISTUKF與UKF的位置誤差較小，STUKF的誤差最大。這是因?yàn)镾TUKF中的強(qiáng)跟蹤由于需要保持殘差向量的正交性，漸消矩陣對(duì)協(xié)方差矩陣做出調(diào)整，削弱了歷史觀測(cè)值對(duì)當(dāng)前狀態(tài)的影響，由于強(qiáng)跟蹤濾波在載體處于模型確定狀態(tài)下歷史觀測(cè)信息利用較少，而文中ISTUKF比STUKF更貼近緊耦合模型。因此，STUKF濾波精度會(huì)低于標(biāo)準(zhǔn)UKF與ISTUKF。

實(shí)驗(yàn)2：模擬復(fù)雜環(huán)境下的載體飛行軌跡，與理想環(huán)境相比，復(fù)雜環(huán)境下系統(tǒng)模型精確度下降，設(shè)計(jì)模型不確定性為

(39)

由圖4、圖5可知，三種濾波處理后的位置誤差與理想條件下相比變大，但也趨于穩(wěn)定。其中UKF的東向、北向位置誤差相對(duì)于其它兩種算法較大。這是由于慣性器件受環(huán)境影響，產(chǎn)生大量的量測(cè)噪聲，標(biāo)準(zhǔn)UKF難以適用于此類環(huán)境。而強(qiáng)跟蹤濾波根據(jù)殘差矩陣變化求出漸消矩陣，并通過(guò)漸消矩陣加強(qiáng)當(dāng)前時(shí)刻觀測(cè)量的影響力，削弱舊觀測(cè)量及其噪聲的影響力，進(jìn)而調(diào)整增益矩陣，使得系統(tǒng)在復(fù)雜環(huán)境下穩(wěn)定性更強(qiáng)。因此STUKF和ISTUKF誤差較小，又ISTUKF比STUKF在濾波中有較少的近似計(jì)算，與模型更匹配，因此ISTUKF性能相對(duì)STUKF有小幅改善。

圖4 東向位置誤差對(duì)比

圖5 北向位置誤差對(duì)比

6 結(jié)論

傳統(tǒng)的強(qiáng)跟蹤UKF算法相較于標(biāo)準(zhǔn)UKF能夠增強(qiáng)系統(tǒng)在復(fù)雜環(huán)境下的穩(wěn)定性和魯棒性，但是該算法在組合導(dǎo)航緊耦合應(yīng)用中存在模型不匹配、冗余計(jì)算及濾波精度降低的問(wèn)題，針對(duì)這些問(wèn)題，本文改進(jìn)漸消因子的求解方式，調(diào)整漸消矩陣作用位置，改進(jìn)強(qiáng)跟蹤UKF濾波結(jié)構(gòu)。通過(guò)仿真對(duì)比標(biāo)準(zhǔn)UKF、強(qiáng)跟蹤UKF和ISTUKF的濾波效果，根據(jù)實(shí)驗(yàn)得到以下結(jié)論：

1)在理想環(huán)境下，強(qiáng)跟蹤UKF與其它兩種算法相比定位誤差較大。ISTUKF與標(biāo)準(zhǔn)UKF誤差接近。

2)在環(huán)境因素對(duì)系統(tǒng)影響較大的情況下，改進(jìn)的強(qiáng)跟蹤UKF濾波和強(qiáng)跟蹤UKF濾波定位誤差較標(biāo)準(zhǔn)UKF有所改善。

3)改進(jìn)的強(qiáng)跟蹤濾波使組合導(dǎo)航系統(tǒng)對(duì)不同環(huán)境的適應(yīng)性更強(qiáng)，濾波性能更完善。

綜上所述，本文提出的ISTUKF算法能夠改善傳統(tǒng)算法的不足。既能保證系統(tǒng)在不同環(huán)境下的適應(yīng)性，具有較高的實(shí)際應(yīng)用價(jià)值。