改進(jìn)的平方根UKF在再入滑翔目標(biāo)跟蹤中的應(yīng)用

葉澤浩，畢紅葵，譚賢四，曲智國，張裕祿，程 楊

(空軍預(yù)警學(xué)院，武漢 430019)

0 引 言

臨近空間通常指海拔在20～100 km的高空[1-3]；臨近空間高超聲速飛行器指飛行于臨近空間，飛行馬赫數(shù)大于5的飛行器[4-5]。其具有飛行速度快飛行距離遠(yuǎn)，機(jī)動能力強(qiáng)，軌跡形式復(fù)雜多變等特點[6]。這對于該類目標(biāo)的跟蹤提出了嚴(yán)峻的挑戰(zhàn)。

由于臨近空間飛行器的運(yùn)動狀態(tài)相互之間、狀態(tài)與觀測量之間存在嚴(yán)重的非線性關(guān)系，因此需要用非線性濾波技術(shù)來得到其狀態(tài)變量的最優(yōu)估計[7]。目前，普遍應(yīng)用的非線性濾波方法有：擴(kuò)展卡爾曼濾波(EKF)以及無跡卡爾曼濾波(UKF)。文獻(xiàn)[8]針對助推—滑翔飛行器提出了一種基于空氣動力模型的擴(kuò)展卡爾曼跟蹤算法，該算法具有較好的跟蹤精度，且可以有效估計助推—滑翔彈道的機(jī)動參數(shù)。文獻(xiàn)[9]針對高超聲速滑翔目標(biāo)提出了一種基于氣動力模型的交互多模型(IMM) EKF跟蹤算法，當(dāng)目標(biāo)發(fā)生機(jī)動時，該算法具有更強(qiáng)的適應(yīng)性和魯棒性。但是EKF存在自身的理論缺陷：對于強(qiáng)非線性系統(tǒng)，EKF存在濾波性能極不穩(wěn)定，甚至發(fā)散的問題；其在線性化處理時需要計算雅克比(Jacobian)矩陣，計算過程繁瑣復(fù)雜且容易出錯[10-12]。UKF則不需要計算Jacobian矩陣，在相當(dāng)運(yùn)算量下具有估計精度和魯棒性更高等優(yōu)點[13]。但是UKF需要對協(xié)方差矩陣進(jìn)行Cholesky分解，這要求協(xié)方差矩陣必須為對稱正定矩陣。文獻(xiàn)[14]則提出了一種平方根UKF濾波方法，利用協(xié)方差的平方根代替協(xié)方差參加遞推運(yùn)算，從而解決了由于協(xié)方差矩陣負(fù)定而不能求其平方根的問題，同時還提高了濾波精度。文獻(xiàn)[15]將平方根UKF用于航天器的測角導(dǎo)航系統(tǒng)中，相比于傳統(tǒng)的EKF和UKF，具有計算速度快、穩(wěn)定性好、導(dǎo)航精度高等優(yōu)勢。

本文根據(jù)臨近空間高超聲速目標(biāo)的特點，采用了氣動力模型，并進(jìn)行了變換；同時將平方根UKF濾波算法應(yīng)用到此類目標(biāo)跟蹤中，并對其從權(quán)系數(shù)和sigma點的選取、平方根矩陣的分解方法以及在協(xié)方差矩陣中引入穩(wěn)定因子三方面進(jìn)行了改進(jìn)。該算法能在保證較高濾波精度和穩(wěn)定性的同時大大減少了計算量加快了計算速度。

1 再入滑翔目標(biāo)系統(tǒng)模型

設(shè)非線性系統(tǒng)的狀態(tài)方程和量測方程：

(1)

式中：x為n×1維狀態(tài)變量，f(x)為系統(tǒng)的非線性狀態(tài)方程，z為m×1維觀測向量，h(x)為非線性的測量方程；wk為過程噪聲，Vk為測量噪聲，且wk及Vk均為均值為零協(xié)方差矩陣分別為Qk和Rk的高斯白噪聲。

1.1 系統(tǒng)狀態(tài)方程

對此類目標(biāo)，通過動力學(xué)來建模，可以較好的匹配其運(yùn)動特性，達(dá)到精確穩(wěn)定跟蹤的目的。

在雷達(dá)站ENU(East north-up)坐標(biāo)系下，傳統(tǒng)的空氣動力學(xué)模型可以表示為[16]：

(2)

(3)

(4)

f(xk)=

(5)

wk=[wx,wy,wz,wvx,wvy,wvz,wd,wt,wc,wM]T，式中wx,wy,wz,wvx,wvy,wvz,wd,wt,wc,wM均為零均值高斯白噪聲。

式中μ為地球引力常數(shù)；ω為地球自轉(zhuǎn)速度，B為雷達(dá)站所在緯度，R0為地球半徑，r為目標(biāo)地心距。

1.2 系統(tǒng)觀測方程

雷達(dá)的觀測量建立在雷達(dá)地面球坐標(biāo)系內(nèi)，位置信息主要由觀測距離d，俯仰角β和方位角θ三個參數(shù)進(jìn)行描述。由上可得，系統(tǒng)的狀態(tài)方程式建立在雷達(dá)地面直角坐標(biāo)系內(nèi)。因此，需要對雷達(dá)的觀測量進(jìn)行轉(zhuǎn)換至直角坐標(biāo)系下的位置坐標(biāo)。用轉(zhuǎn)化后的間接觀測量來替代實際的觀測量不僅可以與狀態(tài)量實現(xiàn)統(tǒng)一還能大大簡化運(yùn)算。

目標(biāo)的位置矢量與觀測距離d，俯仰角β和方位角θ有如下關(guān)系：

圖1 轉(zhuǎn)換關(guān)系示意圖Fig.1 Schematic diagram of conversion relationship

則可得：

(6)

將位置x、y、z作為間接觀測量來代替實際觀測量r、θ和β，可得偽觀測方程為：

(7)

此時，還需要對觀測協(xié)方差矩陣進(jìn)行轉(zhuǎn)換，假設(shè)觀測距離標(biāo)準(zhǔn)差為σd，方位角和俯仰角標(biāo)準(zhǔn)差為σθ和σβ，則對應(yīng)的協(xié)方差矩陣為：

(8)

則根據(jù)協(xié)方差傳播理論可得轉(zhuǎn)換后的觀測協(xié)方差矩陣為：

(9)

(10)

2 平方根UKF濾波算法的改進(jìn)

2.1 標(biāo)準(zhǔn)的平方根UKF濾波算法

(11)

(12)

式中，“Fchol(·)”表示Cholesky分解。

2)sigma點的計算選?。?/p>

(13)

3)權(quán)系數(shù)的計算選取

(14)

式中，γ≥0為一個與狀態(tài)的先驗分布信息有關(guān)的參數(shù)，調(diào)節(jié)γ可以提高協(xié)方差矩陣的精度。對于高斯分布來說，γ=2是最優(yōu)的。

4)時間更新：

(15)

(16)

(17)

(18)

式中，“Fqr(·)”和“Fcholupdate(·)”分別表示QR分解和Cholesky分解一階更新。

5)量測更新

(19)

(20)

(21)

(22)

(23)

6)濾波結(jié)果更新

(24)

(25)

U=KkSz

(26)

Sk+1|k+1=Fcholupdate(Sk+1|k,U,-1)

(27)

2.2 算法的改進(jìn)

2.2.1球形無跡變換選取權(quán)系數(shù)和sigma點

標(biāo)準(zhǔn)的平方根UKF算法中的sigma點和權(quán)系數(shù)的計算選取，涉及參數(shù)種類多，各參數(shù)調(diào)節(jié)過程較為繁瑣；而且，對于n維的狀態(tài)向量無跡變換需要2n個sigma點，而球形無跡變換只需要(n+2)個sigma點就能達(dá)到一般無跡變換的精度和數(shù)值穩(wěn)定度，其對于權(quán)系數(shù)的選擇也比較容易確定。因此，采用球形無跡變換選取權(quán)系數(shù)和sigma點能在保持?jǐn)?shù)值精度和穩(wěn)定度的同時大大加快計算速度。

1)權(quán)系數(shù)的選擇：

W0∈[0,1)

(28)

(29)

則以式(28)和(29)對式(14)進(jìn)行替換。

2)計算sigma點：

(30)

(31)

(32)

則以式(30)、(31)和(32)對式(13)進(jìn)行替換。

2.2.2平方根矩陣的分解改進(jìn)

在標(biāo)準(zhǔn)的平方根UKF算法中對于求向前一步預(yù)測的估計協(xié)方差平方根Sk+1|k由式(17)與式(18)給出，但按式(18)實際的可表達(dá)為：

(33)

運(yùn)用cholupdate更新協(xié)方差矩陣有一定的要求，其需要等式(33)的右邊必須是正定的。

(34)

則進(jìn)一步可令：

Sk+1|k=rT

(35)

根據(jù)向前一步預(yù)測的估計協(xié)方差矩陣計算公式：

(36)

即：

(37)

則由式(35)、(36)、(37)可直接推得到Sk+1|k的更新方程如下：

(38)

(39)

同理對于式(21)和(22)的量測估計協(xié)方差的平方根矩陣更新方程可以改為如下：

(40)

(41)

另外，在標(biāo)準(zhǔn)的平方根UKF濾波算法中對于狀態(tài)誤差協(xié)方差矩陣的更新按式(26)、(27)可實際表示為：

(42)

這要求式(42)的右邊必須是正定的，但是在計算過程中，等式的右邊由于含有相減的項，對舍入誤差十分敏感，很容易因為舍入誤差的積累，使結(jié)果失去正定性。因此還需對狀態(tài)估計協(xié)方差的平方根矩陣作如下修改：

(43)

(44)

因此可以根據(jù)式(38)和式(39)的處理方法來得到改進(jìn)的更新狀態(tài)估計協(xié)方差的平方根矩陣：

(45)

(46)

2.2.3設(shè)計引入多重次穩(wěn)定因子

由于平方根UKF中，sigma點的選取與協(xié)方差密切相關(guān)，因此本文設(shè)計了在協(xié)方差更新中引入多重次穩(wěn)定因子Dk+1來直接調(diào)節(jié)協(xié)方差，其具體形式如下：

Pk+1|k+1=Dk+1(E-KkHk)Pk+1|k(E-KkHk)T·

(47)

其中，Dk+1=diag(d1,d2,…,dn)。則式(45)和(46)應(yīng)進(jìn)一步改為：

(48)

(49)

dn設(shè)為各穩(wěn)定因子。對于有測量輸入量的對應(yīng)因子取為1，即不需要調(diào)節(jié)該位置的協(xié)方差，假若取值大于1，更容易發(fā)散，使增益估計出現(xiàn)奇異值；而對于沒有測量輸入量的取為0.95～1之間，來減少對應(yīng)的協(xié)方差量，因為一開始這些參量的估計誤差不大，只是惡性循環(huán)使誤差呈現(xiàn)指數(shù)式增長，因此，對應(yīng)的穩(wěn)定因子的取值不需要過小，而且取值小了還會影響濾波精度。

3 仿真及分析

為了驗證改進(jìn)后的氣動力模型和算法的有效性，分別設(shè)計了臨近空間高超聲速再入飛行器跳躍滑翔軌跡和平衡滑翔軌跡，如圖2所示。

圖2 目標(biāo)再入軌跡仿真Fig.2 Target reentry trajectory simulation

將新氣動力(new aerodynamic force)的ISR-UKF算法(NISR-UKF)、原氣動力(traditional aerodynamic force)的ISR-UKF算法(TISR-UKF)、新氣動力的SR-UKF算法(NSR-UKF)以及原氣動力的SR-UKF算法(TSR-UKF)對上述軌跡進(jìn)行跟蹤濾波。設(shè)置仿真時長為500 s，跟蹤采樣間隔0.1 s，雷達(dá)測距精度100 m，測角精度0.0015 rad，進(jìn)行50次蒙特卡洛仿真，結(jié)果如下。

其中在仿真中，NSR-UKF和TSR-UKF兩種算法，需要設(shè)定合適的初始狀態(tài)值、初始過程噪聲和初始協(xié)方差才能正常運(yùn)行出結(jié)果，即未出現(xiàn)奇異值。因此這兩類算法的可靠性比較差。而同時為了保證結(jié)果有效性，將NISR-UKF算法與NSR-UKF算法(狀態(tài)變量都為十維)的初始值設(shè)置相同，將TISR-UKF算法與TSR-UKF算法(狀態(tài)變量都為九維)的初始值設(shè)置相同；并且，由于他們的狀態(tài)變量(無論是十維還是九維)，前六維都為位置量和速度量，因此，相對應(yīng)的初始值設(shè)置都相同。

3.1 平衡滑翔軌跡的跟蹤結(jié)果

圖3 目標(biāo)位置估計均方根誤差Fig.3 Target position estimation root mean square error

圖4 目標(biāo)速度估計均方根誤差Fig.4 Target speed estimation root mean square error

為了可以進(jìn)一步定量地分析四種算法的性能，對仿真的各狀態(tài)的RMSE估計結(jié)果做統(tǒng)計平均及算法的耗時情況進(jìn)行比較，結(jié)果如下：

由圖3和表1可以發(fā)現(xiàn)，在位置估計上四種方法均具有良好的估計精度、收斂性和穩(wěn)定性。那是因為氣動力模型與目標(biāo)的運(yùn)動特性匹配度比較高；而且平衡滑翔軌跡的軌跡相對較平緩，機(jī)動性較弱。但是還是存在較明顯的優(yōu)劣：算法NISR-UKF的估計精度最高，收斂性和穩(wěn)定性也好于其他三個；表現(xiàn)最差的是TSR-UKF算法；算法TISR-UKF與NSR-UKF相當(dāng)，但是前者還是略好于后者。由圖4和表1都可以發(fā)現(xiàn)，在速度估計上，存在明顯的優(yōu)劣勢，估計效果最好的是NISR-UKF算法，其次是TISR-UKF算法，之后是NSR-UKF算法，最差的是TSR-UKF算法。那是因為速度的估計精度與氣動參數(shù)估計精度直接相關(guān)，而TSR-UKF算法無任何改進(jìn)，其對于氣動參數(shù)的估計誤差較大。在算法的耗時上，NISR-UKF與TISR-UKF顯著好于NSR-UKF與TSR-UKF，那是因為前兩者采用球形無跡變換能顯著減少計算量，加快計算速度；而同時因為新氣動力的算法將狀態(tài)量維度比原氣動力方程又?jǐn)U展了一維，因此在耗時上，NISR-UKF大于TISR-UKF，NSR-UKF大于TSR-UKF。

表1 各算法的跟蹤性能比較Table 1 Comparison of tracking performance of each algorithm

3.2 跳躍滑翔軌跡的跟蹤結(jié)果

圖5 目標(biāo)位置估計均方根誤差Fig.5 Target position estimation root mean square error

圖6 目標(biāo)速度估計均方根誤差Fig.6 Target speed estimation root mean square error

為了可以進(jìn)一步定量地分析四種算法的性能，對仿真的各狀態(tài)的RMSE估計結(jié)果做統(tǒng)計平均及算法的耗時情況進(jìn)行比較，結(jié)果如下：

表2 各算法的跟蹤性能比較Table 2 Comparison of tracking performance of each algorithm

由圖5和圖6以及表2可以發(fā)現(xiàn)，在位置估計和速度估計上，均存在三處波動起伏(100～150 s、300～350 s、500 s左右)，那是因為該跳躍滑翔軌跡存在跳躍機(jī)動，軌跡變化比較劇烈，尤其在100～150 s、300～350 s、500 s左右位于軌跡跳躍的最低點。但是，NISR-UKF的估計效果明顯要好于其他三種，而且，總體走勢較為平緩，體現(xiàn)了良好的跟蹤性能。其次仍然是TISR-UKF算法、NSR-UKF算法，效果最差的仍然是TSR-UKF算法。在耗時上，與仿真1結(jié)論相同。

綜上所述，對于再入滑翔軌跡，本文的氣動力模型變換、引入的球形無跡變換、改進(jìn)的平方根分解方法以及引入的穩(wěn)定因子能不同程度地改善濾波性能，最主要是能避免奇異值問題的出現(xiàn)，算法的可靠性更強(qiáng)；而且引入的球形無跡變換還能顯著提高濾波的速度。NISR-UKF算法無論在計算速度、濾波精度、收斂速度、穩(wěn)定性以及可靠性上都具有良好的表現(xiàn)，仿真結(jié)果驗證了算法的有效性。

4 結(jié) 論

本文首先對傳統(tǒng)的氣動力模型進(jìn)行變換；其次引入平方根UKF濾波算法，并對其從權(quán)系數(shù)和sigma點的選取、平方根矩陣的分解方法以及在協(xié)方差矩陣中引入穩(wěn)定因子三方面進(jìn)行了改進(jìn)，提出了基于新氣動力模型的改進(jìn)的平方根UKF濾波算法。

仿真對比表明本文所提算法具有良好的濾波性能，而且能避免奇異值問題的出現(xiàn)，算法具有很好的可靠性。