基于多导弹协同目标探测的空间配准算法

    谢宇婷 周军 卢晓东

    

    

    

    摘要： 在多导弹协同目标跟踪技术中， 通过将多传感器的观测信息进行融合产生目标的全局航迹。 理想情况下， 各传感器的偏差认为是零， 然而实际中由于偏差的存在导致融合效果变差， 需要通过空间配准对误差进行估计和补偿。 本文主要针对两类误差： 传感器量测（系统）误差和姿态（定向）误差， 提出一种改进的基于地心地固坐标系的卡尔曼滤波（ECEF-KF）方法。 首先建立了系统误差和姿态误差模型； 其次将弹上传感器的量测转换至公共坐标系（ECEF system）， 隔绝了导弹自身的运动； 然后构造关于状态的线性伪量测并通过卡尔曼滤波（KF）对各偏差进行估计。 仿真结果表明， 该算法可准确估计各偏差量的大小， 并通过误差补偿极大提高目标跟踪的精度。

    关键词： 目标跟踪； 空间配准； 传感器； 误差； 卡尔曼滤波； 协同制导

    中图分类号： TJ765文献标识码： A文章编号： 1673-5048（2018）02-0009-07

    0引言

    空间配准是多传感器量测信息融合的前提[1]， 指的是通过对公共目标的量测来估计和补偿传感器误差。 这一领域中的主流算法是基于合作目标的空间配准。

    依据系统偏差模型的维数区分， 存在基于球（极）投影的二维系统偏差估计算法和基于地心地固坐标 （EarthCentered EarthFixed Coordinate， ECEF） 的三维系统偏差估计算法[2]。 许多经典的二维算法已经在各类文献中提出， 例如， 实时质量控制（RTQC）法、 最小二乘 （LS） 法、 广义最小二乘 （GLS） 法、 精确极大似然估计（EML）法[3]。 与三维算法相比， 二维算法的计算量小但精度低。 传统的基于ECEF三维算法虽然能够弥补上述方法的不足， 但只考虑传感器的量测误差而忽略了其自身的定向偏差和量测噪声。 与此同时， 一些三维雷达在线配准算法已经被提出。 文献[4]提出一种基于卡尔曼滤波的方法， 用于估计和校准多传感器的姿态误差， 但忽略了量测偏差的影响。 文献[5]同时考虑了传感器量测偏差和姿态偏差， 但只适用于姿态角很小且传感器彼此相距很近的情况。 文献[6]提出一种基于网格搜索偏差估计器的卡尔曼滤波方法。 文献[7]展示了一种基于扩展卡尔曼平滑器的期望最大化（EM-EKS） 方法。 文献[8]展示了一种基于状态值以及无迹卡尔曼滤波（UKF）和标准卡尔曼滤波（KF）构成的联合滤波器的空间偏差的改进算法。 上述方法大都是针对静基座传感器， 因此， 有待于提出一种考虑高速移动导弹上姿态和位置不断变化的传感器的空间配准算法。

    在多导弹协同目标探测的背景下， 应用了一种改进的基于地心地固坐标系的卡尔曼滤波 （ECEF-KF） 算法。 该算法将对公共目标的量测

    收稿日期： 2018-01-24

    基金项目： 中央高校基本科研业务费专项资金项目（3102016ZB021）

    作者簡介： 谢宇婷（1994-）， 女， 陕西西安人， 硕士研究生， 研究方向为飞行器导航与探测。

    引用格式： 谢宇婷， 周军， 卢晓东 . 基于多导弹协同目标探测的空间配准算法[ 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坐标系下， 隔绝了弹体的运动， 并且利用两弹上传感器的量测差值来同时估计系统（量测）偏差和姿态角误差。

    算法基于以下假设：

    （1） 系统偏差认为是小量且是常值。

    （2） 姿态偏差是由陀螺误差引起的随时间缓慢变化的小量。

    （3） 忽略各个误差的耦合。

    （4） 不考虑传感器的位置误差。 这可以理解为由于弹上其他辅助测量装置（例如GPS）、 传感器的位置是精确已知的。

    1坐标系的定义和转换

    1.1各个坐标系的定义

    在多传感器空间配准中， 每个传感器都有其自身参考框架或坐标系。 其中， 传感器的量测在本体系中表述， 传感器的姿态角在平台地理坐标系中表述， 传感器和目标的位置在ECEF坐标系中表述。 这些坐标系的定义如图1所示。

    图1坐标系

    Fig.1Coordinate system

    1.1.1ECEF坐标系 OXeYeZe

    正如WGS-84坐标系[9]中所定义的， 原点位于地球质心， Ze轴指向协议地球极（Conventional Terrestrial Pole， CTP）的方向， Xe轴指向零子午面与CTP赤道的交点， Ye轴与Xe， Ze垂直， 构成右手坐标系。

    1.1.2平台地理坐标系 OXdYdZd

    原点位于平台（导弹）质心在地球表面的投影点， Xd， Yd和Zd分别指向该点的正东、 正北和天向。

    1.1.3本体坐标系 OXbYbZb

    原点位于平台（导弹）质心。 Xb轴与弹体纵轴重合， 指向头部为正， Yb轴与弹体竖轴重合， 指向天为正， Zb由右手坐标系规则确定。

    1.2各个参考系间的转换关系

    航空兵器2018年第2期谢宇婷， 等： 基于多导弹协同目标探测的空间配准算法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）

    式中： λ和φ表示经度和纬度。 如果传感器的经度、 纬度和高度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表示， 这一矩阵也叫做导弹的姿态矩阵：

    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数学模型建立

    状态向量由两弹上传感器的误差项组成：

    X=ξkδkT （5）

    其中： k表示第k枚导弹， k=1， 2； ξk=ΔrkΔqγkΔqλkT表示第k枚导弹的量测固定偏差， 包含斜距、 视线高低角和视线偏角误差； δk = ΔψkΔθkΔγkT表示第k枚导弹的姿态偏差， 包含天向、 北向和东向姿态误差角。 最终， 所有误差项构成了12维状态向量， 状态方程可以表示为

    X·=FX+W （6）

    其中： 过程噪声W是一个零均值高斯白噪声向量。

    量测向量Z可以表示为状态量的线性函数：

    Z=HX+V（7）

    其中： 量测噪声V被近似认为是零均值高斯白噪声向量。

    下面， 首先对各项误差模型进行分析， 然后再对状态方程和量测方程的具体形式提出讨论。

    2.1量测误差和姿态误差模型

    传感器的量测误差被认为是在理想测量的基础上增加的一个可加性常值：

    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）

    根据假设， 量测偏差是小量， 式（10）可以展开为关于误差项的一阶线性形式：

    Xb=xb

    yb

    zb=X～b+Jξ （11）

    其中： 系数矩阵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）

    根据假设， 量测误差是固定量， 这里有

    ξ·=0（13）

    文献[10]给出了标准姿态误差模型， 在不考虑速度和位置误差影响以及误差角相互耦合的情况下给出其简单形式：

    Δψ·=εu

    Δθ·=εn

    Δγ·=εe （14）

    其中： εu， εn， εe表示陀螺天向、 北向和东向测量误差， 这些误差被认为是标准差为σu， σn， σe的零均值高斯白噪声。

    根据假设， 姿态误差角均为小量， 则含有误差的姿态矩阵可以表示为

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

    =0-ΔψΔθ

    Δψ0-Δγ

    -ΔθΔγ0 （16）

    2.2量测和状态方程

    目标在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展开成为姿态误差δ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）， 最终获得量测量和状态量的线性关系：

    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）和姿态误差模型（14）， 可以很容易地得到状态方程：

    X·=FX+GW （34）

    其中：

    F=012×12； G=06×6

    I6×612×6；

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

    将式（32）和（34）离散化， 得到离散化后的量测方程和状态方程：

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

    Zk=HkXk+Vk （35）

    其中： Wk和Vk是不相关的高斯白噪声矩阵， 方差为Qk和Rk。

    卡尔曼滤波通过式（35）可以在线估计出姿态偏差和传感器量测偏差。

    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仿真验证

    基于两枚导弹同时探测同一目标的场景， 算法的有效性在这一部分得到验证。 仿真中， 地面目标做变速曲线运动， 初始位置（经度108.5°， 纬度34.01°， 高度0 m）。 弹1和弹2的初始位置分别是（经度108°， 纬度34°， 高度300 m）和（经度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°）。 两弹的初始姿态都是0°， 陀螺误差的标准差是1e-4 （°）/s。 采样时间是0.1 s。

    图3展示了用经纬高坐标表示的导弹和目标绝对运动轨迹。 状态量的估计值如图4和图5所示。 由于卡尔曼滤波的量测方程是关于状态量的一阶线性方程， 因此估计值迅速收敛（约1～2 s）。 对比同一弹上的量测误差估计， 方位角偏差与真值最接近， 估计误差远小于斜距偏差（这一现象从表1也可以看出）。 这是因为由斜距偏差Δr引起的观测项Z远小于角度偏差Δqγ， Δqλ对其的影响， 因此后者的估计精度比前者高[5]。 另外， 由于陀螺误差的累积， 姿态偏差的估计随时间缓慢变化， 如果将仿真时间增长， 这一现象会更明显。 所有姿态误差角的估计量中， 东向姿态偏差角收敛最快， 这可能是由于导弹自旋导致的。 因为当陀螺图3弹目绝对运动轨迹

    Fig.3Absolute trajectory of missiles and target

    绕弹体纵轴Xb旋转时， 将静态误差调制成了周期信号， 因此东向陀螺误差被抵消[11]。 对比两弹的同一类型估计量（例如图4（a）和图5（a））， 可以发现变化趋势几乎相反。 这一现象可以通过式（24）， （29）和（30）解释， 由于两弹间距离远小于导弹和目标的距离， 有矩阵H1≈-H2， H3≈-H4， 所以同类状态的估计误差几乎是相反的。

    图4弹1传感器和姿态偏差估计

    Fig.4Sensor and attitude biases of missile 1

    图5弹2传感器和姿态偏差估计

    Fig.5Sensor and attitude biases of missile 2

    表1列出状态估计的均方根误差 （Root Mean Square Error， RMSE）， 从中可以看出方位误差角的RMSE比其他量测误差项的小， 北向姿态误差角的RMSE比其他姿态误差角的小。 所有状态的估计都以较高的精度接近真值， 证明算法在空间配准方面有较好的效果。 为了证明该算法对目标跟踪精度的提高， 各个量测偏差和姿态偏差的估计值被用于修正式（8）和（15）中的r～， q～λ， q～γ， C～db項。 图6对比了修正后和修正前目标在平台地理坐标系中的预测轨迹。

    表1估计量的RMSE

    Table 1RMSE of estimationRMSE斜距

    偏差/

    m高低角

    偏差/

    （°）方位角

    偏差/

    （°）姿态误差角/（°）天向北向东向弹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中， 包围在最外层的曲线是未修正的目标轨迹， 由于此时导弹做包含姿态误差角的自旋运动， 因此该曲线是旋转的。 这条轨迹与目标真实轨迹的平均偏差是492 m。 包裹在内层的曲线是修正后的目标轨迹， 平均偏差是21 m。 这表明算法不仅能精确估计出各个偏差量， 而且能通过误差补偿提高目标跟踪的精度。

    4结论

    本文基于多弹协同目标探测的场景， 针对传感器的固定量测偏差和时变姿态偏差， 提出一种新的ECEF-KF空间配准算法。 仿真结果显示该算法不仅能获得偏差量的精确估计， 而且通过实时校准， 可极大提高目标跟踪的精度。

    未来工作中将考虑一种扩维的空间配准算法， 适用于由多弹构成的动态传感器网络。 并且将重新设计一种变结构滤波器以适用于网络中节点数可变的情况。

    参考文献：

    [1] 李教. 多平台多传感器多源信息融合系统时空配准及性能评估研究[D]. 西安： 西北工业大学， 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] 宋文彬. 传感器数据空间配准算法研究进展[J]. 传感器与微系统， 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]. 现代雷达， 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] 罗建军， 马卫华， 袁建平， 等. 组合导航原理与应用[M]. 西安： 西北工业大学出版社， 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] 孫伟. 旋转调制型捷联惯性导航系统[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