基于多傳感器抗差融合的UKF彈道跟蹤算法

黃姣茹， 李靈芝， 高 嵩， 錢(qián)富才,2， 王 敏

(1.西安工業(yè)大學(xué)電子信息工程學(xué)院，西安，710021； 2.西安理工大學(xué)陜西省復(fù)雜系統(tǒng)控制與智能信息處理重點(diǎn)實(shí)驗(yàn)室，西安，710048； 3.西安衛(wèi)星測(cè)控中心宇航動(dòng)力學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安，710043)

導(dǎo)彈武器具有超強(qiáng)攻擊性和巨大威懾力等特點(diǎn)，是現(xiàn)代社會(huì)中維持戰(zhàn)略平衡的支撐[1-2]。隨著導(dǎo)彈試驗(yàn)技術(shù)的發(fā)展和需要，多傳感器測(cè)量系統(tǒng)在導(dǎo)彈跟蹤中備受關(guān)注，因此多傳感器數(shù)據(jù)融合濾波技術(shù)對(duì)提高彈道跟蹤精度具有極其重要的意義[3-4]。

目前常用的多傳感器測(cè)量融合方法主要有兩種結(jié)構(gòu)：集中式測(cè)量融合(centralized measurement fusion，CMF)和加權(quán)測(cè)量融合(weighted measurement fusion，WMF)[5-8]，加權(quán)測(cè)量融合方法以其高精度和低計(jì)算量的優(yōu)勢(shì)被廣泛使用。同時(shí)，由于導(dǎo)彈系統(tǒng)模型具有非線性特點(diǎn)，基于非線性濾波的加權(quán)觀測(cè)融合方法得到了廣泛的研究[9-13]，但是，這些算法往往直接對(duì)來(lái)自各個(gè)傳感器的測(cè)量數(shù)據(jù)按照不同的加權(quán)準(zhǔn)則計(jì)算融合測(cè)量值，并未考慮異常觀測(cè)值對(duì)融合算法精度的影響。

然而，在實(shí)際跟蹤測(cè)量中，由于量測(cè)設(shè)備的沖擊、振動(dòng)，傳感器的老化、故障，復(fù)雜環(huán)境的干擾，操作人員的失誤等，即使采用高精度測(cè)量設(shè)備和測(cè)量機(jī)制跟蹤目標(biāo)，都會(huì)導(dǎo)致測(cè)量數(shù)據(jù)出現(xiàn)異常值[14-15]。這些異常值帶來(lái)的數(shù)據(jù)質(zhì)量問(wèn)題，對(duì)彈道協(xié)同跟蹤量測(cè)系統(tǒng)的工作存在不利影響，直接影響彈道跟蹤的精度和穩(wěn)定性。

針對(duì)觀測(cè)值存在異常值的問(wèn)題，國(guó)內(nèi)外學(xué)者基于抗差理論進(jìn)行了大量的研究[16-18]，主要針對(duì)單傳感器中測(cè)量值存在污染的情況進(jìn)行討論，然而在實(shí)際應(yīng)用中，多傳感器測(cè)量機(jī)制更為普遍。

本文針對(duì)多傳感器跟蹤導(dǎo)彈過(guò)程中數(shù)據(jù)污染導(dǎo)致彈道解算精度下降的問(wèn)題，將抗差估計(jì)與加權(quán)觀測(cè)融合、平方根UKF及自適應(yīng)估計(jì)相結(jié)合，通過(guò)構(gòu)造加權(quán)觀測(cè)融合方程，抗差權(quán)函數(shù)以及抗差權(quán)因子，自適應(yīng)計(jì)算等價(jià)噪聲協(xié)方差融合矩陣來(lái)修正異常值，減弱了數(shù)據(jù)污染對(duì)彈道參數(shù)估計(jì)精度的影響，同時(shí)有效地避免了融合估計(jì)發(fā)散的問(wèn)題。

1 問(wèn)題描述

1.1 彈道狀態(tài)方程

設(shè)再入彈道的狀態(tài)方程為：

x(k+1)=f(x(k),k)+w(k)

(1)

式中：f(·)為已知非線性函數(shù)；x(k)為第k個(gè)時(shí)刻系統(tǒng)狀態(tài)；w(k)為狀態(tài)噪聲，為零均值方差陣為Q的白噪聲。

為了得到高精度的彈道模型，需根據(jù)導(dǎo)彈的機(jī)動(dòng)性能和噪聲統(tǒng)計(jì)特性，結(jié)合目標(biāo)運(yùn)動(dòng)學(xué)信息，利用相應(yīng)的隨機(jī)函數(shù)來(lái)逼近彈道運(yùn)動(dòng)。本文采用勻加速模型作為目標(biāo)的狀態(tài)方程。

1.2 彈道觀測(cè)方程

本文中采用光電經(jīng)緯儀與雷達(dá)協(xié)同工作的測(cè)量機(jī)制跟蹤導(dǎo)彈，觀測(cè)設(shè)備為多個(gè)光電經(jīng)緯儀與多個(gè)雷達(dá)組成的測(cè)量機(jī)制，觀測(cè)量為測(cè)距和角度，以單個(gè)光電經(jīng)緯儀與單個(gè)雷達(dá)協(xié)同工作為例介紹測(cè)量原理，測(cè)量坐標(biāo)系光電經(jīng)緯儀與雷達(dá)的測(cè)量關(guān)系如下[19]：

(2)

式中：(x1,y1,z1)為光電經(jīng)緯儀的坐標(biāo)；(x2,y2,z2)為雷達(dá)的坐標(biāo)；(x,y,z)為目標(biāo)的坐標(biāo)；R為雷達(dá)測(cè)距值；E為方位角和A為高低角，經(jīng)過(guò)對(duì)式(2)進(jìn)行數(shù)值解算及分析，得到觀測(cè)量如下：

(3)

可得第j個(gè)測(cè)量體系的觀測(cè)方程為：

z(j)(k)=h(j)(x(k),k)+vj(k),j=1,2,…,L

(4)

式中：v(j)(k)為第j個(gè)傳感器的觀測(cè)噪聲，為零均值方差陣為R的白噪聲。h(j)為j個(gè)測(cè)量體系的觀測(cè)量，可表示為：

h(j)(x(k),k)=

(5)

假設(shè)h(j)(x(k),k)滿足如下形式：

h(j)(x(k),k)=H(j)h(x(k),k)

(6)

式中：H(j)(k)為線性化系數(shù)矩陣，則系統(tǒng)(1)和(4)的加權(quán)測(cè)量近似融合方程可表示如下：

z(I)(k)=H(I)h(x(k),k)+v(I)(k)

(7)

這里，對(duì)式(5)中h(j)(x(k),k)進(jìn)行μ階泰勒級(jí)數(shù)展開(kāi)得到：

(8)

(9)

(10)

H(I)=M-1H(0)

(11)

M和H(I)分別為H(0)的列滿秩矩陣和行滿秩矩陣。近似函數(shù)為：

(12)

本文要解決的問(wèn)題是，對(duì)系統(tǒng)式(1)和式(4)，考慮測(cè)量數(shù)據(jù)質(zhì)量問(wèn)題對(duì)跟蹤精度的影響，研究一種抗差加權(quán)觀測(cè)融合平方根無(wú)跡卡爾曼濾波算法(weighted measurement robust fusion square root unscented Kalman filter，WMRF-SRUKF)提高彈道跟蹤的精度和穩(wěn)定性。

2 抗差加權(quán)觀測(cè)融合平方根UKF算法

在多臺(tái)傳感器跟蹤導(dǎo)彈的過(guò)程中，由于導(dǎo)彈飛行的復(fù)雜性和測(cè)量設(shè)備自身的問(wèn)題，會(huì)出現(xiàn)觀測(cè)值存在異常值的問(wèn)題，如果不對(duì)這部分異常值及時(shí)地進(jìn)行修正，會(huì)影響彈道跟蹤精度。本節(jié)利用抗差估計(jì)理論自適應(yīng)地實(shí)現(xiàn)觀測(cè)異常值的檢驗(yàn)和分離以及修正，提高估計(jì)精度。

2.1 抗差估計(jì)理論

當(dāng)彈道觀測(cè)數(shù)據(jù)中存在異常值時(shí)，根據(jù)極大似然估計(jì)原理，通過(guò)對(duì)整個(gè)時(shí)間段的殘差取合適的極值函數(shù)，抑制異常值的影響。極值函數(shù)如下：

(13)

(14)

(15)

(16)

(17)

根據(jù)多傳感器的觀測(cè)融合值與該時(shí)刻融合預(yù)測(cè)值，定義測(cè)量融合殘差向量：

(18)

則抗差權(quán)的具體表達(dá)式為：

(19)

(20)

(21)

為了計(jì)算方便，引入誤差因子di，其中φ為抗差權(quán)函數(shù)，w為抗差權(quán)因子。當(dāng)觀測(cè)數(shù)據(jù)存在污染時(shí)，通過(guò)計(jì)算抗差權(quán)重因子wi，自適應(yīng)分布狀態(tài)融合預(yù)測(cè)值與當(dāng)前觀測(cè)融合信息的權(quán)重，從而提高了彈道參數(shù)估計(jì)的精度。當(dāng)觀測(cè)數(shù)據(jù)正常時(shí)，wi為1，算法為標(biāo)準(zhǔn)的加權(quán)觀測(cè)融合算法。

2.2 WMRF-SRUKF算法

Step 1：初始化

在初始時(shí)刻傳感器的狀態(tài)向量以及協(xié)方差矩陣的平方根初值分別為：

(22)

(23)

式(23)中的函數(shù)chol(·)表示對(duì)矩陣進(jìn)行Cholesky分解。

Step 2：時(shí)間更新

基于測(cè)量數(shù)據(jù)z(j)(0)～z(j)(k)(j=1,2,…,L)計(jì)算的Sigma點(diǎn)為：

(24)

對(duì)應(yīng)的權(quán)值：

(25)

式中：下標(biāo)m為均值;c為協(xié)方差;上標(biāo)i為第幾個(gè)采樣點(diǎn)。參數(shù)λ=a2(n+κ)-n，κ=0或κ=3-n，β=2。

計(jì)算狀態(tài)融合預(yù)測(cè)向量：

i=0,1,…,2n

(26)

(27)

由式(8)和式(9)計(jì)算變換矩陣，測(cè)量方程預(yù)測(cè)的采樣點(diǎn)計(jì)算如下：

(28)

預(yù)測(cè)測(cè)量值計(jì)算：

(29)

預(yù)測(cè)融合誤差協(xié)方差矩陣的平方根為：

(30)

式中：qr(·)表示矩陣的QR分解。

(31)

式中：chol update(·)表示矩陣Cholesky分解的修正。

Step 3：觀測(cè)值異常值判斷以及抗差處理

計(jì)算k+1的測(cè)量融合值：

(32)

(33)

當(dāng)誤差因子di滿足：

(34)

Step 4：測(cè)量更新

新息協(xié)方差矩陣平方根為：

(35)

(36)

互協(xié)方差矩陣為：

(37)

增益矩陣為：

(38)

z(I)(k)由式(7)得到，則k+1時(shí)刻的狀態(tài)估計(jì)為：

(39)

狀態(tài)估計(jì)誤差協(xié)方差矩陣的平方根矩陣：

UI(k+1)=KI(k+1)SyI(k+1|k)

(40)

(41)

綜上所述，通過(guò)泰勒級(jí)數(shù)展開(kāi)方法實(shí)時(shí)計(jì)算觀測(cè)融合矩陣，對(duì)來(lái)自多個(gè)傳感器的信息進(jìn)行融合，采用抗差估計(jì)方法構(gòu)造抗差權(quán)重因子，自適應(yīng)調(diào)節(jié)測(cè)量信息的權(quán)重，并基于平方根UKF算法進(jìn)行濾波求解等價(jià)協(xié)方差矩陣，對(duì)融合過(guò)程中可能出現(xiàn)的異常值進(jìn)行分離與修正，實(shí)時(shí)地調(diào)節(jié)濾波增益，從而實(shí)現(xiàn)了平方根UKF加權(quán)觀測(cè)融合算法的抗差性能。

3 仿真分析

3.1 參數(shù)設(shè)置

本文采用光電經(jīng)緯儀與雷達(dá)協(xié)同工作跟蹤導(dǎo)彈，考慮3個(gè)測(cè)站構(gòu)成的傳感器網(wǎng)絡(luò)，采用100次蒙特卡羅仿真，對(duì)導(dǎo)彈進(jìn)行加權(quán)融合式實(shí)時(shí)軌道的確定，以光電經(jīng)緯儀的位置為原點(diǎn)創(chuàng)建相對(duì)三維坐標(biāo)系。

雷達(dá)1相對(duì)站址：

x01=800,y01=900,z01=700；

雷達(dá)2相對(duì)站址：

x02=1 500,y02=1 450,z02=1 100；

雷達(dá)3相對(duì)站址：

x03=1 600,y03=1 750,z03=1 200；

將雷達(dá)帶入式(4)得到各個(gè)測(cè)量體系具體的觀測(cè)方程，根據(jù)式(8)利用二階泰勒級(jí)數(shù)展開(kāi)以及合并同類項(xiàng)去除零部分，計(jì)算測(cè)量方程的系數(shù)矩陣H(0)，根據(jù)式(12)計(jì)算近似函數(shù)為：

h(x(k),k)=

(42)

各個(gè)濾波器初始狀態(tài)為：

xj(0)=[3 500,1 500,1 000,-1 100,-150,-50，10,10,10]T，j=1，2，3。

3.2 仿真對(duì)比分析

本節(jié)采用本文提出的WMRF-SRUKF算法對(duì)式(1)和(4)構(gòu)成的多傳感器彈道動(dòng)力學(xué)模型進(jìn)行數(shù)據(jù)融合仿真，并與傳統(tǒng)的WMF-UKF算法和WMF-SRUKF算法進(jìn)行比較。得到彈道理想軌跡和各算法融合估計(jì)的彈道軌跡曲線見(jiàn)圖1，位置、速度和加速的均方根誤差(root mean squared error，RMSE)曲線如圖2～4所示。

圖1 真實(shí)狀態(tài)及WMF-UKF、WMF-SRUKF及WMRF-SRUKF算法的軌跡估計(jì)曲線

圖2 WMF-UKF、WMF-SRUKF及WMRF-SRUKF算法的位置RMSE曲線

圖3 WMF-UKF、WMF-SRUKF及WMRF-SRUKF算法的速度RMSE曲線

由圖1可以看出，采用WMF-UKF和WMF-SRUKF算法對(duì)設(shè)備進(jìn)行融合得到的彈道軌跡直觀上與理想軌跡存在偏差，而本文提出的WMRF-SRUKF算法估計(jì)的彈道融合軌跡相比于上述兩種融合算法更接近于真實(shí)軌跡，具有精度高的優(yōu)點(diǎn)。

圖4 WMF-UKF、WMF-SRUKF及WMRF-SRUKF算法的加速度RMSE曲線

綜上所述采用WMRF-SRUKF算法可以對(duì)來(lái)自各個(gè)測(cè)量機(jī)制的數(shù)據(jù)進(jìn)行有效融合，并且及時(shí)對(duì)測(cè)量異常值進(jìn)行修正，最終得到的彈道參數(shù)更接近于真實(shí)值，彈道跟蹤精度高。同時(shí)在融合過(guò)程中WMRF-SRUKF運(yùn)用矩陣滿秩分解將測(cè)量矩陣的維數(shù)由9維壓縮至5維，該算法濾波過(guò)程中時(shí)間復(fù)雜度由O(x9)降至為O(x5)，計(jì)算負(fù)擔(dān)明顯減小。

4 結(jié)語(yǔ)

針對(duì)融合過(guò)程中觀測(cè)值出現(xiàn)異常值和濾波器發(fā)散引起的彈道精度低的問(wèn)題，本文在加權(quán)觀測(cè)融合算法的基礎(chǔ)上，提出了一種抗差融合平方根UKF彈道跟蹤算法。該算法利用IGGI等價(jià)權(quán)函數(shù)，自適應(yīng)估計(jì)以及平方根濾波思想，實(shí)時(shí)地對(duì)來(lái)自各個(gè)傳感器的信息進(jìn)行融合，并對(duì)測(cè)量數(shù)據(jù)出現(xiàn)的異常值進(jìn)行實(shí)時(shí)剔除以及抗差修正，提高了多傳感器信息融合估計(jì)的精度，實(shí)現(xiàn)了彈道的高精度跟蹤問(wèn)題，在彈道數(shù)據(jù)融合處理方面具有一定的應(yīng)用價(jià)值。