樊 垚，費(fèi)瑋瑋，楊洪康，劉海旻

(中國船舶工業(yè)系統(tǒng)工程研究院，北京 100094)

0 引 言

由于被動(dòng)探測(cè)器僅能獲得目標(biāo)的方位信息而不能檢測(cè)目標(biāo)的距離，利用單平臺(tái)單基陣測(cè)量定位時(shí)，在本艦不進(jìn)行機(jī)動(dòng)的情況下，僅使用該探測(cè)平臺(tái)的方位角測(cè)量值，目標(biāo)運(yùn)動(dòng)狀態(tài)不可觀測(cè)[1-3]。被動(dòng)聲吶跟蹤水中目標(biāo)是典型的目標(biāo)運(yùn)動(dòng)分析（TMA）問題，探測(cè)平臺(tái)機(jī)動(dòng)是目標(biāo)運(yùn)動(dòng)可觀測(cè)的必要條件，通過非線性濾波方法以及探測(cè)平臺(tái)機(jī)動(dòng)提高純方位TMA 問題中目標(biāo)狀態(tài)可觀測(cè)性。目前常用的目標(biāo)跟蹤定位方法有直角坐標(biāo)系下的擴(kuò)展卡爾曼濾波、偽線性濾波及修正極坐標(biāo)下的擴(kuò)展卡爾曼濾波等[4-5]。隨著軍事技術(shù)的不斷發(fā)展，敵我雙方的對(duì)抗性顯著提高，在雙方平臺(tái)對(duì)抗博弈的條件下，被跟蹤目標(biāo)會(huì)通過采取必要的機(jī)動(dòng)提高生存能力，因此，機(jī)動(dòng)目標(biāo)運(yùn)動(dòng)要素估計(jì)的研究成為軍事上的迫切需求。

本文在修正極坐標(biāo)系下研究了單平臺(tái)被動(dòng)目標(biāo)跟蹤定位問題，提出一種距離參數(shù)化濾波器設(shè)計(jì)方法，降低濾波器初始參數(shù)對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)估計(jì)的不利影響，提高估計(jì)算法收斂速度和精度。給出基于目標(biāo)運(yùn)動(dòng)狀態(tài)濾波器的目標(biāo)機(jī)動(dòng)檢測(cè)算法，實(shí)現(xiàn)對(duì)水中機(jī)動(dòng)逃逸目標(biāo)運(yùn)動(dòng)要素迅速、準(zhǔn)確的估計(jì)，解決水中機(jī)動(dòng)目標(biāo)檢測(cè)與跟蹤問題。

1 修正極坐標(biāo)系下目標(biāo)運(yùn)動(dòng)分析

1.1 修正極坐標(biāo)系

在不同的坐標(biāo)系中目標(biāo)運(yùn)動(dòng)估計(jì)濾波系統(tǒng)方程有不同的特點(diǎn)，本文采用修正極坐標(biāo)將系統(tǒng)狀態(tài)變量分成了可觀測(cè)和不可觀測(cè)兩部分。目標(biāo)與探測(cè)平臺(tái)的幾何關(guān)系如圖1 所示。

圖1 目標(biāo)與探測(cè)平臺(tái)的幾何關(guān)系Fig. 1 Relative geometric graph of target and detective platform

直角坐標(biāo)系的狀態(tài)向量定義為：

其中， vx， vy， rx， ry分別為目標(biāo)在x，y 方向的相對(duì)速度和相對(duì)距離。

修正極坐標(biāo)系下的系統(tǒng)狀態(tài)向量定義為：

令目標(biāo)和探測(cè)平臺(tái)在x，y 方向上的加速度分別為atx， aty， aox， aoy， 相對(duì)加速度為 ax， ay，則

令

得

令

可得到修正極坐標(biāo)系下的狀態(tài)方程為：

由于在修正極坐標(biāo)系中，目標(biāo)方位角 β(t)是狀態(tài)向量的一個(gè)分量，因此，在修正極坐標(biāo)系中，純方位TMA 問題的測(cè)量方程為：

其中， n(t)為目標(biāo)方位角測(cè)量噪聲。

1.2 目標(biāo)運(yùn)動(dòng)要素解算

修正極坐標(biāo)系下目標(biāo)運(yùn)動(dòng)要素解算表達(dá)式如下：

其中： vx， vy為 目標(biāo)在x，y 方向的絕對(duì)速度； θ為目標(biāo)的航向角； v為目標(biāo)速度。

2 距離參數(shù)化EKF 濾波器設(shè)計(jì)

2.1 EKF 算法

采用擴(kuò)展Kalman 濾波（EKF）算法求解純方位TMA 問題，該算法采用遞推方式，計(jì)算量較小，能夠?qū)ο到y(tǒng)進(jìn)行實(shí)時(shí)定位，且估計(jì)精度較高，利于實(shí)際工程的應(yīng)用。純方位TMA 問題中系統(tǒng)狀態(tài)方程（3）和測(cè)量方程（4）為連續(xù)形式，適當(dāng)選取t=kT、to=(k-1)T，可將連續(xù)方程離散化，其中，T為采樣周期。采用式（8）所示標(biāo)準(zhǔn)EKF 濾波算法對(duì)探測(cè)平臺(tái)與目標(biāo)相對(duì)運(yùn)動(dòng)狀態(tài)進(jìn)行實(shí)時(shí)估計(jì)。

其中：y(0|0)為狀態(tài)向量初始給定值；P(0|0)為給定的y(0|0)誤差協(xié)方差陣；σ2為測(cè)量量的誤差方差。

2.2 距離參數(shù)化（RP）算法

在純方位TMA 問題中，探測(cè)平臺(tái)與目標(biāo)相對(duì)距離是不可觀測(cè)變量，當(dāng)目標(biāo)與探測(cè)平臺(tái)相對(duì)距離先驗(yàn)分布信息比較模糊時(shí)，直接使用EKF 濾波算法容易導(dǎo)致狀態(tài)協(xié)方差矩陣過早收斂，從而使得濾波發(fā)散。距離參數(shù)化（RP）方法將初始距離按照相對(duì)距離估計(jì)值的變異系數(shù)分成若干個(gè)獨(dú)立區(qū)間，每個(gè)獨(dú)立區(qū)間使用獨(dú)立的EKF 濾波器進(jìn)行狀態(tài)估計(jì)計(jì)算，初始時(shí)刻每個(gè)濾波器將賦予均勻的權(quán)值。當(dāng)系統(tǒng)獲得下一時(shí)刻觀測(cè)值時(shí)，系統(tǒng)的狀態(tài)向量和協(xié)方差矩陣要升級(jí)一次，與此同時(shí)，在RPEKF 中各個(gè)獨(dú)立區(qū)間的權(quán)重也需要重新計(jì)算并歸一化處理。假定目標(biāo)方位角的估計(jì)值和測(cè)量值服從高斯分布，利用貝葉斯理論，給出子區(qū)間權(quán)重遞推計(jì)算公式，如式（9）。如果子區(qū)間內(nèi)目標(biāo)方位角的估計(jì)值與測(cè)量值有較大的差別，那么對(duì)應(yīng)的子區(qū)間權(quán)重就會(huì)降低。反之，權(quán)重就會(huì)升高。

式中：Wnk為第n個(gè)區(qū)間在k時(shí)刻的權(quán)重；為第n個(gè)區(qū)間方位角預(yù)測(cè)值；θm為方位角測(cè)量值；σ2=+為第n 個(gè)區(qū)間的方位角方差值；為第n 個(gè)區(qū)間的方位角預(yù)測(cè)值誤差方差值；為方位角測(cè)量值誤差方差值。

3 目標(biāo)機(jī)動(dòng)檢測(cè)算法

3.1 基于RPEKF 的目標(biāo)機(jī)動(dòng)檢測(cè)算法

純方位TMA 問題針對(duì)靜止目標(biāo)或勻速直線運(yùn)動(dòng)目標(biāo)開展研究，當(dāng)目標(biāo)出現(xiàn)機(jī)動(dòng)時(shí)，目標(biāo)運(yùn)動(dòng)要素估計(jì)算法無法實(shí)現(xiàn)對(duì)目標(biāo)的無偏估計(jì)，系統(tǒng)狀態(tài)無法收斂至真實(shí)狀態(tài)值。為克服此缺陷，設(shè)計(jì)機(jī)動(dòng)檢測(cè)環(huán)節(jié)，在檢測(cè)到目標(biāo)機(jī)動(dòng)時(shí)重新初始化目標(biāo)運(yùn)動(dòng)狀態(tài)濾波器。

由于RPEKF 是幾個(gè)獨(dú)立EKF 共同工作的濾波系統(tǒng)，所以其目標(biāo)機(jī)動(dòng)檢測(cè)算法應(yīng)在各EKF 目標(biāo)機(jī)動(dòng)檢測(cè)算法的基礎(chǔ)上進(jìn)行修正，算法中使用系統(tǒng)狀態(tài)綜合值和系統(tǒng)協(xié)方差矩陣綜合值實(shí)現(xiàn)目標(biāo)機(jī)動(dòng)檢測(cè)，表達(dá)式如下：

其中：

式中： Wik為k 時(shí)刻，第i 個(gè)獨(dú)立濾波器的權(quán)重值；yi(k|k-1)為k 時(shí)刻，第i 個(gè)獨(dú)立濾波器的狀態(tài)估計(jì)值；Pi(k|k-1)為k 時(shí)刻，第i 個(gè)獨(dú)立濾波器的狀態(tài)誤差協(xié)方差矩陣；若 Uk大于給定閾值 μk則認(rèn)為目標(biāo)機(jī)動(dòng)，將狀態(tài)向量 yi(k|k) 和狀態(tài)誤差協(xié)方差矩陣 Pi(k|k)進(jìn)行重置，并且暫停機(jī)動(dòng)檢測(cè)環(huán)節(jié) S 個(gè)周期， S為滯后步長。

4 仿真驗(yàn)證

4.1 基于RPEKF 的目標(biāo)運(yùn)動(dòng)要素估計(jì)仿真分析

4.1.1 仿真初始條件

探測(cè)平臺(tái)與目標(biāo)相對(duì)距離34000m，目標(biāo)舷角30°。

本文中探測(cè)平臺(tái)采用“Z”字形機(jī)動(dòng)方式，機(jī)動(dòng)過程由勻速直線運(yùn)動(dòng)與勻速圓周運(yùn)動(dòng)交替進(jìn)行。其中，探測(cè)平臺(tái)移動(dòng)速度為5m/s，轉(zhuǎn)彎角速率為1°s。該運(yùn)動(dòng)方式由3 個(gè)參數(shù)決定，分別是一周期內(nèi)勻速直線運(yùn)動(dòng)的時(shí)間Tline為400s，一周期內(nèi)勻速圓周運(yùn)動(dòng)的時(shí)間Tcircle為90s，機(jī)動(dòng)運(yùn)動(dòng)的瞄準(zhǔn)線夾角Φaim為β0。這里，β0為初始時(shí)刻目標(biāo)方位角。

探測(cè)平臺(tái)測(cè)量量為疊加了白噪聲的目標(biāo)方位角，噪聲均值為0，標(biāo)準(zhǔn)差為1°。

4.1.2 濾波器初始參數(shù)

探測(cè)平臺(tái)采用被動(dòng)聲吶測(cè)量目標(biāo)方位角，由于對(duì)目標(biāo)初始位置缺乏先驗(yàn)信息，這里給出目標(biāo)初始相對(duì)距離分布區(qū)間為[10000m，35000m]，采用RPEKF 濾波器對(duì)目標(biāo)運(yùn)動(dòng)要素進(jìn)行估計(jì)。

根據(jù)距離分布區(qū)間大小決定了EKF 濾波器數(shù)量，本文選擇濾波器個(gè)數(shù)為4。假定每個(gè)獨(dú)立距離分布區(qū)間中距離誤差服從均勻分布，對(duì)應(yīng)于4 組EKF 濾波器的初始距離可以分為4 個(gè)獨(dú)立區(qū)間[10000m，14000m]，[14000m，19600m]，[19600m，27400m]，[27400m，35 000 m]，每個(gè)獨(dú)立區(qū)間都對(duì)應(yīng)一個(gè)獨(dú)立的EKF 濾波器，每個(gè)獨(dú)立區(qū)間分配的權(quán)重為0.25。

4 組獨(dú)立的EKF 濾波器初值如下：

式中：β(0)為初始時(shí)刻目標(biāo)方位角測(cè)量值； Rn為第n 個(gè)區(qū)間的中心值；σθ˙n為第n 個(gè)區(qū)間狀態(tài)變量 θ˙的標(biāo)準(zhǔn)差，取為1 5/Rn；σR˙/Rn為第n 個(gè)區(qū)間狀態(tài)變量 R˙/R的標(biāo)準(zhǔn)差，取為15/Rn；σθ為目標(biāo)方位角測(cè)量誤差標(biāo)準(zhǔn)差，取為1°；σ1/Rn為第n 個(gè)區(qū)間狀態(tài)變量1 /R的標(biāo)準(zhǔn)差，取為。

4.1.3 仿真分析

定義目標(biāo)運(yùn)動(dòng)要素估計(jì)誤差百分比如式(15) 所示，估計(jì)誤差比例反映了目標(biāo)運(yùn)動(dòng)要素估計(jì)值的相對(duì)誤差水平。

基于前述仿真條件，采用如式（3）所示EKF 濾波器狀態(tài)方程，如式（4）所示測(cè)量方程，如式（8）所示濾波方程，如式（9）所示各EKF 濾波器權(quán)重升級(jí)方程，進(jìn)行數(shù)字仿真分析。圖2 為探測(cè)平臺(tái)與目標(biāo)相對(duì)運(yùn)動(dòng)軌跡。圖3～圖5 為目標(biāo)距離、速度、航向估計(jì)誤差百分比。

圖2 本艦-目標(biāo)相對(duì)運(yùn)動(dòng)軌跡Fig. 2 Trajectory of target and detective platform

圖3 目標(biāo)距離估計(jì)誤差百分比Fig. 3 Evaluated error percentage of target distance

圖4 目標(biāo)速度估計(jì)誤差百分比Fig. 4 Evaluated error percentage of target velocity

圖6～圖9 為RPEKF 濾波器中4 組獨(dú)立的EKF 濾波器權(quán)值變化曲線。

由圖可知，目標(biāo)距離，目標(biāo)速度和目標(biāo)航向均能收斂于真值。由于初始相對(duì)距離為34 000 m 在第4 區(qū)間內(nèi)，第1、2、3 區(qū)間權(quán)重值變?yōu)?，第4 區(qū)間權(quán)重值為1，表明本文中提出的距離參數(shù)化方法是有效的。

4.2 機(jī)動(dòng)目標(biāo)運(yùn)動(dòng)要素估計(jì)仿真分析

4.2.1 仿真初始條件

在實(shí)際水上目標(biāo)機(jī)動(dòng)場(chǎng)景中，最常見的運(yùn)動(dòng)形式是分段勻速直線運(yùn)動(dòng)，目標(biāo)機(jī)動(dòng)方式描述如下：

圖6 第1 區(qū)間權(quán)值變化曲線Fig. 6 Weight coefficient curve of the first section

圖7 第2 區(qū)間權(quán)值變化曲線Fig. 7 Weight coefficient curve of the second section

圖8 第3 區(qū)間權(quán)值變化曲線Fig. 8 Weight coefficient curve of the third section

圖9 第4 區(qū)間權(quán)值變化曲線Fig. 9 Weight coefficient curve of the fourth section

其中：cmnew為目標(biāo)機(jī)動(dòng)后的航向；βTm為本艇相對(duì)目標(biāo)的方位；qTm為本艇相對(duì)目標(biāo)的舷角，目標(biāo)機(jī)動(dòng)時(shí)刻設(shè)為1 500 s。

檢測(cè)目標(biāo)機(jī)動(dòng)后，本艦機(jī)動(dòng)形式如4.1 節(jié)所述，軌跡參數(shù)Tline為600 s，Tcircle為90 s，Φaim為βreset。其中，βreset為檢測(cè)到目標(biāo)機(jī)動(dòng)時(shí)刻目標(biāo)方位角。

其余仿真初始條件同4.1.3 節(jié)相關(guān)參數(shù)。

4.2.2 濾波器初始參數(shù)

當(dāng)檢測(cè)到目標(biāo)機(jī)動(dòng)后，濾波器重置，其初始狀態(tài)如下：

式中：βreset為濾波器重置時(shí)刻目標(biāo)方位角測(cè)量值；Rn_reset為濾波器重置后第n 個(gè)區(qū)間的中心值；σθ˙n_reset為濾波器重置后第n 個(gè)區(qū)間狀態(tài)變量 θ˙的標(biāo)準(zhǔn)差，取為1 5/Rn_reset；為第n 個(gè)區(qū)間狀態(tài)變量R˙/R的標(biāo)準(zhǔn)差，取為15/Rn_reset； σθ為目標(biāo)方位角測(cè)量量標(biāo)準(zhǔn)差，取為1°；σ1/Rn_reset為第n 個(gè)區(qū)間狀態(tài)變量1/R 的標(biāo)準(zhǔn)差，取為σRn_reset/。

其余濾波器初始參數(shù)同4.1.2 節(jié)相關(guān)參數(shù)。

4.2.3 仿真分析

基于前述仿真條件，采用如式（3）所示EKF 濾波器狀態(tài)方程，如式（4）所示測(cè)量方程，如（8）所示濾波方程，如式（9）所示各EKF 濾波器權(quán)重升級(jí)方程，如式（10）～式（12）所示目標(biāo)機(jī)動(dòng)檢測(cè)算法，進(jìn)行數(shù)字仿真分析。圖10 為探測(cè)平臺(tái)與目標(biāo)相對(duì)運(yùn)動(dòng)軌跡，圖11～圖13 為目標(biāo)距離、速度、航向估計(jì)誤差百分比。

圖10 本艦-目標(biāo)相對(duì)運(yùn)動(dòng)軌跡Fig. 10 Trajectory of target and detective platform

圖11 目標(biāo)距離估計(jì)誤差百分比Fig. 11 Evaluated error percentage of target distance

圖12 目標(biāo)速度估計(jì)誤差百分比Fig. 12 Evaluated error percentage of target velocity

圖13 目標(biāo)航向估計(jì)誤差百分比Fig. 13 Evaluated error percentage of target heading angle

可知，該條件下檢測(cè)到目標(biāo)機(jī)動(dòng)的時(shí)刻為1 721 s，目標(biāo)機(jī)動(dòng)檢測(cè)算法可行。濾波器重置后，目標(biāo)距離估計(jì)值于2 475 s 收斂于10%誤差范圍內(nèi)；目標(biāo)速度估計(jì)值于3 004 s 收斂于10%誤差范圍內(nèi)；目標(biāo)航向估計(jì)值于2 071 s 收斂于10°誤差范圍內(nèi)。

5 結(jié) 語

1）針對(duì)純方位TMA 問題，本文給出非線性RPEKF濾波方法，通過引入多組EKF 濾波器和濾波器權(quán)重調(diào)節(jié)，能夠有效降低濾波器性能對(duì)于初值偏差的敏感性，改善其目標(biāo)運(yùn)動(dòng)狀態(tài)量的收斂特性。

2）針對(duì)水面目標(biāo)常用的分段勻速直線機(jī)動(dòng)形式，本文提出的基于RPEKF 濾波器的目標(biāo)機(jī)動(dòng)檢測(cè)方法能夠有效檢測(cè)目標(biāo)機(jī)動(dòng)，并通過重置濾波器參數(shù)抑制目標(biāo)運(yùn)動(dòng)狀態(tài)濾波發(fā)散。