一種基于球坐標(biāo)的角速率耦合跟蹤方法

梁 燊，徐國(guó)亮，張 遜

（中國(guó)船舶重工集團(tuán)公司江蘇自動(dòng)化研究所，江蘇 連云港 222006）

雷達(dá)、光電、陀螺等多種傳感器在對(duì)機(jī)動(dòng)目標(biāo)跟蹤的過(guò)程中，能夠獲取大量的量測(cè)信息，其中包括位置量測(cè)信息，也包括速度量測(cè)信息。如何對(duì)這些信息進(jìn)行有效地處理和整合，建立合理的目標(biāo)運(yùn)動(dòng)模型，就成為了機(jī)動(dòng)目標(biāo)跟蹤研究的重要課題。利用量測(cè)信息對(duì)目標(biāo)狀態(tài)進(jìn)行估計(jì)，既要做到盡可能地提高狀態(tài)估計(jì)的精度，又要盡量縮短狀態(tài)估計(jì)時(shí)間?，F(xiàn)有的目標(biāo)跟蹤模型多是針對(duì)目標(biāo)的位置信息進(jìn)行處理[1-2]，而目標(biāo)的速度量測(cè)信息則沒(méi)有得到充分的利用。Fitzgerald[3]在濾波過(guò)程中加入速度量測(cè)信息，分析了速度量測(cè)量對(duì)濾波的增益和跟蹤精度的影響，在同時(shí)具備速度量測(cè)信息和位置量測(cè)信息的條件下，對(duì)目標(biāo)位置的估計(jì)精度最高可以提高一個(gè)數(shù)量級(jí)。可見(jiàn)，研究含速度量測(cè)信息的目標(biāo)運(yùn)動(dòng)模型具有重要的意義。有學(xué)者[4,5]提出加入徑向速度信息的模型，跟蹤精度有了很大的提高，但模型中均不包括角速度量測(cè)信息。Zhu xinguo[6]提出了基于徑向距離R的三維、角度q,ε的二維解耦運(yùn)動(dòng)模型，能夠很好地處理目標(biāo)的機(jī)動(dòng)性。Pearson和Edwin.B.Stear[7]提出了基于高低誤差角和方位誤差角的三通道耦合模型，雖然調(diào)整了濾波增益以縮短反應(yīng)時(shí)間，但用時(shí)仍然較長(zhǎng)。本文充分利用徑向速度R˙、角速率ω量測(cè)值，提出一種新的基于徑向距離R，高低角ε、方位角q的三通道球坐標(biāo)系角速率耦合跟蹤模型，提高了狀態(tài)估計(jì)的精度，有效縮短了反應(yīng)時(shí)間。

1 坐標(biāo)系及問(wèn)題描述

瞄準(zhǔn)線坐標(biāo)系 O XaYaZa。原點(diǎn)O是跟蹤旋轉(zhuǎn)軸與俯仰軸的交點(diǎn)，OXa軸為跟蹤器俯仰軸，OYa為跟蹤器瞄準(zhǔn)線，OZa垂直于 O XaYa的方向，軸OXa、軸OYa和OZa組成右手直角坐標(biāo)系。如圖1，Δq、Δε為方向角和高低角跟蹤偏差。設(shè)ω為瞄準(zhǔn)線坐標(biāo)系旋轉(zhuǎn)角速度。

圖1 瞄準(zhǔn)線坐標(biāo)系與跟蹤誤差角

東北天坐標(biāo)系OXYZ。原點(diǎn)位于艦艇中心，OX平行于水平面指向正東，OY軸平行于水平面指向正北，OZ軸垂直平面OXY，鉛垂向上為正。

當(dāng)跟蹤偏差 Δ q ＝ Δε ＝ 0，則認(rèn)為瞄準(zhǔn)線坐標(biāo)系與視線坐標(biāo)系重合。東北天坐標(biāo)系與視線坐標(biāo)系的示意圖如圖2。

為了使問(wèn)題簡(jiǎn)化和研究方便，文中所有模型均在跟蹤偏差 Δ q ＝ Δε ＝ 0的假設(shè)下建立的。對(duì)目標(biāo)狀態(tài)的估計(jì)，要建立合適的目標(biāo)運(yùn)動(dòng)模型，并選用合理的濾波方法。目標(biāo)運(yùn)動(dòng)模型一般具有如下形式：

圖2 東北天坐標(biāo)系和視線坐標(biāo)系

其中X(k)為目標(biāo)在第k時(shí)刻的狀態(tài)向量，Φ(k)為狀態(tài)轉(zhuǎn)移矩陣，W(k)為系統(tǒng)的噪聲矩陣，

Z ( k)為k時(shí)刻的觀測(cè)向量，H(k)為觀測(cè)矩陣。V ( k)為 觀測(cè)噪 聲 ，且有 E[V(k )]＝0，E[V(k)V(k)T]＝ R(k)。

對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)的估計(jì)，要在目標(biāo)運(yùn)動(dòng)模型的基礎(chǔ)上進(jìn)行濾波，一般多采用Kalman[8]濾波方法。

實(shí)現(xiàn)目標(biāo)跟蹤和預(yù)測(cè)的關(guān)鍵在于如何從帶有噪聲的觀測(cè)量中提出有用的目標(biāo)狀態(tài)信息，多數(shù)目標(biāo)跟蹤算法是基于目標(biāo)運(yùn)動(dòng)建模技術(shù)的。目標(biāo)運(yùn)動(dòng)建模是機(jī)動(dòng)目標(biāo)跟蹤中的核心問(wèn)題之一。一般所建立的機(jī)動(dòng)目標(biāo)模型既要符合機(jī)動(dòng)實(shí)際，又要便于數(shù)學(xué)處理。同樣對(duì)于既具有徑向速度R˙，又具有角速度ω量測(cè)值的目標(biāo)運(yùn)動(dòng)問(wèn)題，如何充分利用這些量測(cè)信息，建立合適的目標(biāo)運(yùn)動(dòng)模型，就成為問(wèn)題研究的關(guān)鍵。

2 傳統(tǒng)方法

2.1 不含角速度量測(cè)的目標(biāo)運(yùn)動(dòng)模型

假設(shè)徑向距離R在采樣時(shí)間間隔T內(nèi)，滿足勻加速運(yùn)動(dòng)條件，角度q、ε在采樣時(shí)間間隔T內(nèi)滿足勻速運(yùn)動(dòng)條件?？芍嚯x通道R的離散運(yùn)動(dòng)模型：

由于方位角q和高低角ε所建立模型相同，這里僅以q為例，給出方程：

系統(tǒng)噪聲協(xié)方差矩陣為

這里 ωza,ωxa分別為方位角q的角速率在OZa方向上的分量、高低角ε的角速率在OXa方向上的分量。為系統(tǒng)的噪聲方差。

2.2 含角速度量測(cè)的目標(biāo)運(yùn)動(dòng)模型

這里距離通道R所采用的模型仍然是勻加速模型，它的離散時(shí)間模型與不含角速率量測(cè)的目標(biāo)運(yùn)動(dòng)模型相同。即狀態(tài)向量和狀態(tài)轉(zhuǎn)移矩陣為式（3），系統(tǒng)噪聲矩陣為式（4），觀測(cè)矩陣為式（5）。

對(duì)于角度通道q、ε，假設(shè)角度的變換在采樣時(shí)間間隔T內(nèi)滿足勻加速運(yùn)動(dòng)模型。這里僅以q為例，給出它的離散時(shí)間模型：

觀測(cè)矩陣：

這里ωza為方位角q的角速率在OZa方向上的分量。aza分別為q的角加速度在OZa方向上的分量。為系統(tǒng)的噪聲方差。

3 球坐標(biāo)角速率耦合跟蹤方法

在瞄準(zhǔn)線坐標(biāo)系下，目標(biāo)的坐標(biāo)為：

若跟蹤偏差 Δ q ＝ Δε ＝ 0，瞄準(zhǔn)線坐標(biāo)系與視線坐標(biāo)系重合，則可得：

等式（15）可以寫(xiě)為如下形式：

對(duì)于R，有運(yùn)動(dòng)學(xué)方程：

對(duì)式（19）離散化，可以得到：

QR為XR的系統(tǒng)噪聲矩陣，HR為觀測(cè)矩陣。τ為OY方向上的機(jī)動(dòng)時(shí)間常數(shù)。

對(duì)于角度q，ε，同樣假設(shè)在采樣時(shí)間間隔T內(nèi)，它們的變化滿足勻加速條件，則它們運(yùn)動(dòng)模型同樣可以寫(xiě)為式（9）、（10）、（11）。

對(duì)于含徑向速度和角速度的耦合模型，濾波過(guò)程首先從角度濾波開(kāi)始，將得出的最優(yōu)估計(jì)的ω?za和ω?xa代入距離通道R的方程中，通過(guò)距離通道的自身濾波來(lái)求得R的最優(yōu)估計(jì)值，這樣就完成了一次濾波過(guò)程。濾波過(guò)程示意圖如下：

圖3 濾波過(guò)程

4 仿真計(jì)算

圖4 目標(biāo)運(yùn)動(dòng)示意圖

航路一：假設(shè)目標(biāo)距觀測(cè)點(diǎn)的初始距離為6000m，目標(biāo)距地高度為 50m，航路捷徑為 300m，采樣周期T＝0.02s 。目標(biāo)運(yùn)動(dòng)時(shí)間為20s。

觀測(cè)誤差的標(biāo)準(zhǔn)差 σR＝10m ，σq＝σε＝1mrad，徑向速度的標(biāo)準(zhǔn)差＝5m /s ，角速度的標(biāo)準(zhǔn)差

仿真均采用kalman濾波方法。進(jìn)行Monte Carlo仿真300次。為簡(jiǎn)單起見(jiàn)，僅給出了方位角通道濾波均方根和徑向距離的誤差均方根曲線圖如圖5、圖6。

圖5 R的誤差均方根

圖6 方位角q的誤差均方根

通過(guò)仿真圖形，可以得出：使用了角速率耦合跟蹤方法對(duì)徑向距離R進(jìn)行濾波，能夠較快的收斂，并且誤差均方根較小。在目標(biāo)與觀測(cè)點(diǎn)最接近點(diǎn)，仍然能夠保持較小的誤差。對(duì)于角度的濾波，我們可以看到使用了角速率量測(cè)值的耦合模型，目標(biāo)最接近觀測(cè)點(diǎn)時(shí)角度估計(jì)仍然保持較小的誤差均方值。

為了比較在不同航路下球坐標(biāo)角速率耦合跟蹤方法的效果，設(shè)計(jì)了另外一條航路：航路捷徑為100m，其他條件與航路一相同。同樣進(jìn)行Monte Carlo仿真300次，二者的徑向距離和角度誤差均方根對(duì)比如圖7、圖8：

圖7 航路一與航路二的徑向距離誤差均方根對(duì)比圖

通過(guò)仿真圖形，可以得出：在所選的兩條航路上，使用球坐標(biāo)角速率耦合跟蹤方法對(duì)徑向距離R進(jìn)行濾波，從濾波穩(wěn)定開(kāi)始至濾波結(jié)束，濾波的誤差均方根相差不多。在濾波結(jié)束點(diǎn)附近，捷徑越小的航路的角度濾波收斂性越好。

圖8 航路一與航路二的方位角誤差均方根對(duì)比圖

5 結(jié)束語(yǔ)

本文分析了球坐標(biāo)系下的跟蹤濾波過(guò)程，提出了基于球坐標(biāo)的角速率耦合跟蹤方法。在考慮了瞄準(zhǔn)線坐標(biāo)系繞慣性坐標(biāo)系旋轉(zhuǎn)所產(chǎn)生的Coriolis加速度的情況下，建立了徑向距離R的運(yùn)動(dòng)模型。本文將基于球坐標(biāo)的角速率耦合跟蹤方法與不含角速率量測(cè)和含角速率量測(cè)值的兩種運(yùn)動(dòng)模型做了對(duì)比。仿真結(jié)果顯示，含角速率量測(cè)的運(yùn)動(dòng)模型和角速率耦合跟蹤方法的跟蹤精度明顯優(yōu)于不含角速率量測(cè)信息的運(yùn)動(dòng)模型；而與含角速率量測(cè)的模型相比，應(yīng)用基于球坐標(biāo)的角速率耦合跟蹤方法，能夠使徑向距離較快地收斂，并且在整個(gè)濾波過(guò)程中保持較好的收斂性。

[1]X. Rong. Li, Vesselin . P. Jilkovt. A Survey of Maneuvering Target Tracking: Dynamic Models[J]. IEEE Transactions on aerospace and electronic systems.Oct,2003: 1333-1364.

[2]H.Thou, K. S. P. Kumar. A "Current" Statistical Model and Adaptive Algorithm for Estimating Maneuvering Targets[J].AIAA Journal of Guidance, Sept.-Oct. 1984,7(5):596-602.

[3]Robert. J, Fitzgerald. Simple Tracking Filters: Position and Velocity Measurements[J]. IEEE Transactions on aerospace and electronic systems. Sept.1982,AES-18,NO.5:531-537.

[4]張懷根,張林讓,吳順君.利用徑向速度觀測(cè)值提高目標(biāo)跟蹤性能[J]. 西安電子科技大學(xué)學(xué)報(bào)(自然科學(xué)版),2005, 32(5):667-670.

[5]S. Zollo, B. Ristic. on polar versus Cartesian coordinates for target tracking[C]. Fifth International Symposium on Signal Processing and its Applications, ISSPA,1999:499-502.

[6]Zhu xinguo, Hong Jie, Cui wei. Study on Radar Data processing Algorithm with Improved Kalman Filter[C].ICIEA 2009: 3826-3829.

[7]J. B. Pearson, E. B. Stear. Kalman Filter Applications In Airborne Radar Tracking[J]. IEEE Transactions on aerospace and electronic systems.May.1974,AES-10(3):319-329.

[8]R. E. Kalman. A new approach to linear filtering and prediction problems[J]. Trans. ASME, J.Basic. Engrg,1960,82(2):34-45.

[9]王洪蘭.陀螺理論及在工程測(cè)量中的應(yīng)用[M].北京：國(guó)防工業(yè)出版社，1985:47-57.