基于多導彈協(xié)同目標探測的空間配準算法

謝宇婷 周軍 盧曉東

摘要： 在多導彈協(xié)同目標跟蹤技術中， 通過將多傳感器的觀測信息進行融合產(chǎn)生目標的全局航跡。 理想情況下， 各傳感器的偏差認為是零， 然而實際中由于偏差的存在導致融合效果變差， 需要通過空間配準對誤差進行估計和補償。 本文主要針對兩類誤差： 傳感器量測（系統(tǒng)）誤差和姿態(tài)（定向）誤差， 提出一種改進的基于地心地固坐標系的卡爾曼濾波（ECEF-KF）方法。 首先建立了系統(tǒng)誤差和姿態(tài)誤差模型； 其次將彈上傳感器的量測轉換至公共坐標系（ECEF system）， 隔絕了導彈自身的運動； 然后構造關于狀態(tài)的線性偽量測并通過卡爾曼濾波（KF）對各偏差進行估計。 仿真結果表明， 該算法可準確估計各偏差量的大小， 并通過誤差補償極大提高目標跟蹤的精度。

關鍵詞： 目標跟蹤； 空間配準； 傳感器； 誤差； 卡爾曼濾波； 協(xié)同制導

中圖分類號： TJ765文獻標識碼： A文章編號： 1673-5048（2018）02-0009-07

0引言

空間配準是多傳感器量測信息融合的前提[1]， 指的是通過對公共目標的量測來估計和補償傳感器誤差。 這一領域中的主流算法是基于合作目標的空間配準。

依據(jù)系統(tǒng)偏差模型的維數(shù)區(qū)分， 存在基于球（極）投影的二維系統(tǒng)偏差估計算法和基于地心地固坐標 （EarthCentered EarthFixed Coordinate， ECEF） 的三維系統(tǒng)偏差估計算法[2]。 許多經(jīng)典的二維算法已經(jīng)在各類文獻中提出， 例如， 實時質量控制（RTQC）法、 最小二乘 （LS） 法、 廣義最小二乘 （GLS） 法、 精確極大似然估計（EML）法[3]。 與三維算法相比， 二維算法的計算量小但精度低。 傳統(tǒng)的基于ECEF三維算法雖然能夠彌補上述方法的不足， 但只考慮傳感器的量測誤差而忽略了其自身的定向偏差和量測噪聲。 與此同時， 一些三維雷達在線配準算法已經(jīng)被提出。 文獻[4]提出一種基于卡爾曼濾波的方法， 用于估計和校準多傳感器的姿態(tài)誤差， 但忽略了量測偏差的影響。 文獻[5]同時考慮了傳感器量測偏差和姿態(tài)偏差， 但只適用于姿態(tài)角很小且傳感器彼此相距很近的情況。 文獻[6]提出一種基于網(wǎng)格搜索偏差估計器的卡爾曼濾波方法。 文獻[7]展示了一種基于擴展卡爾曼平滑器的期望最大化（EM-EKS） 方法。 文獻[8]展示了一種基于狀態(tài)值以及無跡卡爾曼濾波（UKF）和標準卡爾曼濾波（KF）構成的聯(lián)合濾波器的空間偏差的改進算法。 上述方法大都是針對靜基座傳感器， 因此， 有待于提出一種考慮高速移動導彈上姿態(tài)和位置不斷變化的傳感器的空間配準算法。

在多導彈協(xié)同目標探測的背景下， 應用了一種改進的基于地心地固坐標系的卡爾曼濾波 （ECEF-KF） 算法。 該算法將對公共目標的量測

收稿日期： 2018-01-24

基金項目： 中央高?；究蒲袠I(yè)務費專項資金項目（3102016ZB021）

作者簡介： 謝宇婷（1994-）， 女， 陜西西安人， 碩士研究生， 研究方向為飛行器導航與探測。

引用格式： 謝宇婷， 周軍， 盧曉東 . 基于多導彈協(xié)同目標探測的空間配準算法[ J]. 航空兵器， 2018（ 2）： 9-15.

Xie Yuting， Zhou Jun， Lu Xiaodong. Spatial Registration Algorithm Based on MultiMissiles Cooperative Target Detection[J]. Aero Weaponry， 2018（ 2）： 9-15.（ in Chinese）轉換到ECEF坐標系下， 隔絕了彈體的運動， 并且利用兩彈上傳感器的量測差值來同時估計系統(tǒng)（量測）偏差和姿態(tài)角誤差。

算法基于以下假設：

（1） 系統(tǒng)偏差認為是小量且是常值。

（2） 姿態(tài)偏差是由陀螺誤差引起的隨時間緩慢變化的小量。

（3） 忽略各個誤差的耦合。

（4） 不考慮傳感器的位置誤差。 這可以理解為由于彈上其他輔助測量裝置（例如GPS）、 傳感器的位置是精確已知的。

1坐標系的定義和轉換

1.1各個坐標系的定義

在多傳感器空間配準中， 每個傳感器都有其自身參考框架或坐標系。 其中， 傳感器的量測在本體系中表述， 傳感器的姿態(tài)角在平臺地理坐標系中表述， 傳感器和目標的位置在ECEF坐標系中表述。 這些坐標系的定義如圖1所示。

圖1坐標系

Fig.1Coordinate system

1.1.1ECEF坐標系 OXeYeZe

正如WGS-84坐標系[9]中所定義的， 原點位于地球質心， Ze軸指向協(xié)議地球極（Conventional Terrestrial Pole， CTP）的方向， Xe軸指向零子午面與CTP赤道的交點， Ye軸與Xe， Ze垂直， 構成右手坐標系。

1.1.2平臺地理坐標系 OXdYdZd

原點位于平臺（導彈）質心在地球表面的投影點， Xd， Yd和Zd分別指向該點的正東、 正北和天向。

1.1.3本體坐標系 OXbYbZb

原點位于平臺（導彈）質心。 Xb軸與彈體縱軸重合， 指向頭部為正， Yb軸與彈體豎軸重合， 指向天為正， Zb由右手坐標系規(guī)則確定。

1.2各個參考系間的轉換關系

航空兵器2018年第2期謝宇婷， 等： 基于多導彈協(xié)同目標探測的空間配準算法1.2.1ECEF坐標系和平臺地理坐標系

令向量re=xeyezeT，rd=xdydzdT分別表示ECEF和平臺地理系下的坐標。 后者與前者的轉換關系可以表示為

re=Cedrd（1）

這里Ced是3×3正交矩陣， 即

Ced=-sinλ-sinφcosλcosφcosλ

cosλ-sinφsinλcosφsinλ

0cosφsinφ （2）

式中： λ和φ表示經(jīng)度和緯度。 如果傳感器的經(jīng)度、 緯度和高度h已知， 則其ECEF坐標可以通過如下公式計算得到：

xe=（C+h）cosφcosλ

ye=（C+h）cosφsinλ

ze=（C（1-e2）+h）sinφ

C=Re/1-e2sin2φ（3）

其中： Re為赤道半徑； e表示地球偏心率。

1.2.2平臺地理坐標系和本體坐標系

令向量rb=xbybzbT表示本體系下坐標， 同樣地， 從本體系轉換至平臺地理系的轉換關系可以用矩陣Cdb表示， 這一矩陣也叫做導彈的姿態(tài)矩陣：

Cdb=cosθcosψsinθ-cosθsinψ

-sinθcosψcosγ+sinψsinγcosθcosγsinθsinψcosγ+cosψsinγ

sinθsinγcosψ+cosγsinψ-cosθsinγ-sinθsinψsinγ+cosγcosψ（4）

其中： ψ， θ和γ分別表示導彈的偏航、 俯仰和滾轉角。

2數(shù)學模型建立

狀態(tài)向量由兩彈上傳感器的誤差項組成：

X=ξkδkT （5）

其中： k表示第k枚導彈， k=1， 2； ξk=ΔrkΔqγkΔqλkT表示第k枚導彈的量測固定偏差， 包含斜距、 視線高低角和視線偏角誤差； δk = ΔψkΔθkΔγkT表示第k枚導彈的姿態(tài)偏差， 包含天向、 北向和東向姿態(tài)誤差角。 最終， 所有誤差項構成了12維狀態(tài)向量， 狀態(tài)方程可以表示為

X·=FX+W （6）

其中： 過程噪聲W是一個零均值高斯白噪聲向量。

量測向量Z可以表示為狀態(tài)量的線性函數(shù)：

Z=HX+V（7）

其中： 量測噪聲V被近似認為是零均值高斯白噪聲向量。

下面， 首先對各項誤差模型進行分析， 然后再對狀態(tài)方程和量測方程的具體形式提出討論。

2.1量測誤差和姿態(tài)誤差模型

傳感器的量測誤差被認為是在理想測量的基礎上增加的一個可加性常值：

r～=r+Δr

q～γ=qγ+Δqγ

q～λ=qλ+Δqλ （8）

其中： 上標～表示含有偏差的實際測量。 這里暫時忽略量測噪聲的影響， 在后面構建量測方程時會考慮進來。

對目標的量測如圖2所示。

目標在本體系下的坐標可以表示為

xb=rcosqγcosqλ

yb=rsinqγ

zb=-rcosqγsinqλ （9）

將式（8）代入式（9）：

圖2對目標的量測

Fig.2Measurement of target

xb=r～-Δrcosq～γ-Δqγcosq～λ-Δqλ

yb=r～-Δrsinq～γ-Δqγ

zb=-r～-Δrcosq～γ-Δqγsinq～λ-Δqλ （10）

根據(jù)假設， 量測偏差是小量， 式（10）可以展開為關于誤差項的一階線性形式：

Xb=xb

yb

zb=X～b+Jξ （11）

其中： 系數(shù)矩陣J是式（10）關于誤差項ξ的雅克比矩陣， 表示為

J=-cosΔqγcosΔqλrsinΔqγcosΔqλrcosΔqγsinΔqλ

-sinΔqγ-rcosΔqγ0

cosΔqγsinΔqλ-rsinΔqγsinΔqλrcosΔqγcosΔqλ（12）

根據(jù)假設， 量測誤差是固定量， 這里有

ξ·=0（13）

文獻[10]給出了標準姿態(tài)誤差模型， 在不考慮速度和位置誤差影響以及誤差角相互耦合的情況下給出其簡單形式：

Δψ·=εu

Δθ·=εn

Δγ·=εe （14）

其中： εu， εn， εe表示陀螺天向、 北向和東向測量誤差， 這些誤差被認為是標準差為σu， σn， σe的零均值高斯白噪聲。

根據(jù)假設， 姿態(tài)誤差角均為小量， 則含有誤差的姿態(tài)矩陣可以表示為

C～db=（I-）Cdb（15）

=0-ΔψΔθ

Δψ0-Δγ

-ΔθΔγ0 （16）

2.2量測和狀態(tài)方程

目標在ECEF坐標系下的坐標可以分別由目標在兩導彈的本體坐標系下的坐標轉換而來：

Xet=Ced2Cd2b2Xb2+Xe2 （17）

Xet=Ced1Cd1b1Xb1+Xe1 （18）

其中： Xe1， Xe2分別表示兩彈的ECEF坐標。

由式（17）～（18）可得

Xb1=Cd1b1TCed1TCed2Cd2b2Xb2+ΔXe2-1 （19）

由式（15）可知

Cd1b1=（I-1）TC～d1b1

Cd2b2=（I-2）TC～d2b2（20）

將式（20）代入式（19）， 忽略1和2的乘積：

Xb1=C～b1d1（Cd1eCed2-1Cd1eCed2-

Cd1eCed2T2）C～d2b2Xb2+

C～b1d1（I-1）Cd1eΔXe2-1（21）

式（21）兩邊同乘Ced1C～d1b1， 得

Ced1C～d1b1Xb1= （Ced2-Ced11Ced1Ced2-

Ced2T2）C～b2d2Xb2 +

Ced1（I-1）Cd1eΔXe2-1 （22）

將Xb1和Xb2展成式（11）所示形式， 因為Δ和Δξ均為小量， 所以忽略他們的乘積項， 式（22）變成

Ced1C～d1b1X～b1-Ced2C～d2b2X～b2+ΔXe1-2=

Ced2C～d2b2J2ξ2-Ced1C～d1b1J1ξ1-（Ced11Cd1e+

Ced2T2Cd2e）Ced2C～d2b2X～b2 （23）

式中， 等號左邊的項即是構造的偽量測， 記為Z。 令H1=-Ced1C～d1b1J1， H2=Ced2C～d2b2J2， 并將等號右邊最后一項記為P， 式（23）變成

Z=H1ξ1+H2ξ2+P （24）

將P展開成為姿態(tài)誤差δk的線性形式， 表示為

P=-（A1AT+BT2BT）M （25）

其中：

A=Ced1=a1a2a3

B=Ced2=b1b2b3

M=BC～d2b2X～b2 （26）

式中， ai和bi分別是A和B的列向量， 這里不加推導地給出如下公式：

-A1ATM=

-A-a2a10

a30-a1

0-a3a2TM

M

MΔψ1

Δθ1

Δγ1（27）

同理

-BT2BTM =B2BTM=

B-b2 b1 0

b3 0 -b1

0 -b3 b2TM

M

MΔψ2

Δθ2

Δγ2 （28）

在式（27）和（28）中， 令

H3=-A-a2a10

a30-a1

0-a3a2TM

M

M （29）

H4=B-b2b10

b30-b1

0-b3b2TM

M

M（30）

將式（27）～（30）代入式（25）， 最終將P展開為

P=H3Δψ1

Δθ1

Δγ1+H4Δψ2

Δθ2

Δγ2=H3δ1+H4δ2 （31）

將式（31）代入式（24）， 最終獲得量測量和狀態(tài)量的線性關系：

Z=H1H2H3H4X+V（32）

其中： X=ξ1ξ2δ1δ2T是一個12維誤差向量。

考慮到量測噪聲的存在， 在式（32）中加入一個隨機向量V， 可以近似表示為

V=H1 H2vr1 vqγ1 vqλ1 vr2 vqγ2 vqλ2T（33）

其中： vrk， vqγk和vqλk分別為第k枚導彈上傳感器關于斜距、 高低角和方位角的量測噪聲。 這些量測噪聲被認為是已知標準差為σrk， σqγk和σqλk的白噪聲， 且彼此互不相干。

基于文中已給出的量測誤差模型（13）和姿態(tài)誤差模型（14）， 可以很容易地得到狀態(tài)方程：

X·=FX+GW （34）

其中：

F=012×12； G=06×6

I6×612×6；

W=εu1εn1εe1εu2εn2εe2T。

將式（32）和（34）離散化， 得到離散化后的量測方程和狀態(tài)方程：

Xk=Φk/k-1Xk-1+Wk-1

Zk=HkXk+Vk （35）

其中： Wk和Vk是不相關的高斯白噪聲矩陣， 方差為Qk和Rk。

卡爾曼濾波通過式（35）可以在線估計出姿態(tài)偏差和傳感器量測偏差。

X^k/k-1=Φk/k-1X^k-1

Pk/k-1=Φk/k-1Pk-1ΦTk/k-1+Qk-1

Kk=Pk/k-1HTk（HkPk/k-1HTk+Rk）-1

X^k=X^k/k-1+Kk（Zk-HkX^k/k-1）

Pk=（I-KkHk）Pk/k-1 （36）

3仿真驗證

基于兩枚導彈同時探測同一目標的場景， 算法的有效性在這一部分得到驗證。 仿真中， 地面目標做變速曲線運動， 初始位置（經(jīng)度108.5°， 緯度34.01°， 高度0 m）。 彈1和彈2的初始位置分別是（經(jīng)度108°， 緯度34°， 高度300 m）和（經(jīng)度108°， 緯度34.02°， 高度300 m）。 兩彈同時沿東北方向做勻速直線運動， 速度分別為（北向5 m/s， 東向150 m/s）和（北向-7 m/s， 東向100 m/s）。 假設兩彈在飛行過程中繞各自的Xb軸快速轉動。 彈1的傳感器固定偏差是（200 m， 0.5°， 0.1°）， 彈2的傳感器固定偏差是（300 m， 0.3°， 0.1°）。 量測噪聲的標準差是（10 m， 0.1°， 0.1°）。 兩彈的初始姿態(tài)都是0°， 陀螺誤差的標準差是1e-4 （°）/s。 采樣時間是0.1 s。

圖3展示了用經(jīng)緯高坐標表示的導彈和目標絕對運動軌跡。 狀態(tài)量的估計值如圖4和圖5所示。 由于卡爾曼濾波的量測方程是關于狀態(tài)量的一階線性方程， 因此估計值迅速收斂（約1～2 s）。 對比同一彈上的量測誤差估計， 方位角偏差與真值最接近， 估計誤差遠小于斜距偏差（這一現(xiàn)象從表1也可以看出）。 這是因為由斜距偏差Δr引起的觀測項Z遠小于角度偏差Δqγ， Δqλ對其的影響， 因此后者的估計精度比前者高[5]。 另外， 由于陀螺誤差的累積， 姿態(tài)偏差的估計隨時間緩慢變化， 如果將仿真時間增長， 這一現(xiàn)象會更明顯。 所有姿態(tài)誤差角的估計量中， 東向姿態(tài)偏差角收斂最快， 這可能是由于導彈自旋導致的。 因為當陀螺圖3彈目絕對運動軌跡

Fig.3Absolute trajectory of missiles and target

繞彈體縱軸Xb旋轉時， 將靜態(tài)誤差調制成了周期信號， 因此東向陀螺誤差被抵消[11]。 對比兩彈的同一類型估計量（例如圖4（a）和圖5（a））， 可以發(fā)現(xiàn)變化趨勢幾乎相反。 這一現(xiàn)象可以通過式（24）， （29）和（30）解釋， 由于兩彈間距離遠小于導彈和目標的距離， 有矩陣H1≈-H2， H3≈-H4， 所以同類狀態(tài)的估計誤差幾乎是相反的。

圖4彈1傳感器和姿態(tài)偏差估計

Fig.4Sensor and attitude biases of missile 1

圖5彈2傳感器和姿態(tài)偏差估計

Fig.5Sensor and attitude biases of missile 2

表1列出狀態(tài)估計的均方根誤差 （Root Mean Square Error， RMSE）， 從中可以看出方位誤差角的RMSE比其他量測誤差項的小， 北向姿態(tài)誤差角的RMSE比其他姿態(tài)誤差角的小。 所有狀態(tài)的估計都以較高的精度接近真值， 證明算法在空間配準方面有較好的效果。 為了證明該算法對目標跟蹤精度的提高， 各個量測偏差和姿態(tài)偏差的估計值被用于修正式（8）和（15）中的r～， q～λ， q～γ， C～db項。 圖6對比了修正后和修正前目標在平臺地理坐標系中的預測軌跡。

表1估計量的RMSE

Table 1RMSE of estimationRMSE斜距

偏差/

m高低角

偏差/

（°）方位角

偏差/

（°）姿態(tài)誤差角/（°）天向北向東向彈11.229 00.011 00.002 76.23e-41.71e-40.002 7彈20.599 20.010 10.008 96.24e-41.72e-40.002 7圖6彈目相對運動軌跡

Fig.6Relative trajectory of missiles and target

圖6中， 包圍在最外層的曲線是未修正的目標軌跡， 由于此時導彈做包含姿態(tài)誤差角的自旋運動， 因此該曲線是旋轉的。 這條軌跡與目標真實軌跡的平均偏差是492 m。 包裹在內層的曲線是修正后的目標軌跡， 平均偏差是21 m。 這表明算法不僅能精確估計出各個偏差量， 而且能通過誤差補償提高目標跟蹤的精度。

4結論

本文基于多彈協(xié)同目標探測的場景， 針對傳感器的固定量測偏差和時變姿態(tài)偏差， 提出一種新的ECEF-KF空間配準算法。 仿真結果顯示該算法不僅能獲得偏差量的精確估計， 而且通過實時校準， 可極大提高目標跟蹤的精度。

未來工作中將考慮一種擴維的空間配準算法， 適用于由多彈構成的動態(tài)傳感器網(wǎng)絡。 并且將重新設計一種變結構濾波器以適用于網(wǎng)絡中節(jié)點數(shù)可變的情況。

參考文獻：

[1] 李教. 多平臺多傳感器多源信息融合系統(tǒng)時空配準及性能評估研究[D]. 西安： 西北工業(yè)大學， 2003： 17-29.

Li Jiao. Temporal and Spatial Alignment and Performance Assessment for MultiPlatform MultiSensor and MultiSource Information Fusion System [D]. Xian： Northwestern Polytechnical University， 2003： 17-29. （in Chinese）

[2] 宋文彬. 傳感器數(shù)據(jù)空間配準算法研究進展[J]. 傳感器與微系統(tǒng)， 2012， 31（8）： 5-8.

Song Wenbin. Research Progress of Spatial Registration Algorithms for Sensor Data[J]. Transducer and Microsystem Technologies， 2012， 31（8）： 5-8. （in Chinese）

[3] Liu Jianfeng， Zuo Yan， Xue Anke. An ASEKF Algorithm for 2D and 3D Radar Registration[C]∥ Proceedings of the 32nd Chinese Control Conference， 2013： 4758-4761.

[4] 劉煜， 楊哲， 韓崇昭. 傳感器定姿偏差的空間配準算法研究[J]. 現(xiàn)代雷達， 2009， 31（2）： 29-31， 35.

Liu Yu， Yang Zhe， Han Chongzhao. A Study on Space Registration Algorithm of Sensor Attitude Bias[J]. Modern Radar， 2009， 31（2）： 29-31， 35.（in Chinese）

[5] Helmick R E， Rice T R. Removal of Alignment Errors in an Integrated System of Two 3D Sensors[J]. IEEE Transactions on Aerospace and Electronic Systems， 1993， 29（4）： 1333-1343.

[6] Matsuzaki T， Kameda H， Uchida J， et al. A Study of Bias Error Estimation Method by KGBE[J]. 2010 IEEE International Conference on Control Applications， Yokohama， Japan， 2010： 452-457.

[7] Li Zhenhua， Henry Leung. Simultaneous Registration and Fusion of Radar and ESM by EMEKS[C]∥8th World Congress on Intelligent Control and Automation， Jinan， China， 2010： 1130-1134.

[8] Bo Yuming， Chen Zhimin， Yin Mingfeng， et al. Improved Different Dimensional Sensors Combined Space Registration Algorithm[J]. Mathematical Problems in Engineering， 2015（1）： 1-9.

[9] World Geodetic System[Z/OL].（2018-01-03）[2018-01-24]. https：∥en.wikipedia.org/wiki/World_Geodetic_System.

[10] 羅建軍， 馬衛(wèi)華， 袁建平， 等. 組合導航原理與應用[M]. 西安： 西北工業(yè)大學出版社， 2012： 67.

Luo Jianjun， Ma Weihua， Yuan Jianping， et al. Principle and Application of Integrated Navigation[M]. Xian： Northwestern Polytechnical University Press， 2012： 67.（in Chinese）

[11] 孫偉. 旋轉調制型捷聯(lián)慣性導航系統(tǒng)[M]. 北京： 測繪出版社， 2014.

Sun Wei. Rotary Modulation Strapdown Inertial Navigation System[M]. Beijing： Surveying and Mapping Press， 2014.（in Chinese）

Spatial Registration Algorithm Based on

MultiMissiles Cooperative Target Detection

Xie Yuting， Zhou Jun， Lu Xiaodong

（School of Astronautics， Northwestern Polytechnical University， Xian 710072， China）

Abstract： In the technology of multimissiles cooperative target tracking， an integrated target trajectory is generated by fusing the observations from sensors. In ideal environment， sensor biases are considered as zeros. However， due to the existence of biases， the effect of fusion deteriorates. The spatial registration is needed to estimate and compensate the errors. Focused on two types of biases： sensor measurement（systemic）biases and attitude（oriented）biases， the paper proposes an improved ECEFKF method. Firstly， the state models of systemic errors and attitude errors are developed. Secondly， the measurements of missiles are transformed into public（ECEF）system， isolating the motion and rotation of missiles. Thirdly， a Kalman filter based on linear pseudomeasured function is used to estimate the error state. Simulations demonstrate that the alignment errors can be exactly estimated and the accuracy of target tracking can be dramatically improved by error compensation.

Key words： target track； spatial registration； sensor； error； Kalman filter； cooperative guidance1Oppressive jamming will incapacitate its normal function for phased array radar。 for this problem， the basic of polarization mismatch will be used， and isolate the interference source at the receiver， improve the ability of antiinterference. In this paper， a joint beamforming technique for polarization and spatial domain is first proposed， which is derive， which is a problem of secondorder cone programs， to obtain the polarized beam with a null and polarization constraint in desired sidelobe region. Numerical examples are provided to demonstrate the usefulness and effectiveness of the proposed approaches.Polarization； interference rejection； phased array radar