基于多传感器抗差融合的UKF弹道跟踪算法

黄姣茹， 李灵芝， 高 嵩， 钱富才,2， 王 敏

(1.西安工业大学电子信息工程学院，西安，710021； 2.西安理工大学陕西省复杂系统控制与智能信息处理重点实验室，西安，710048； 3.西安卫星测控中心宇航动力学国家重点实验室，西安，710043)

导弹武器具有超强攻击性和巨大威慑力等特点，是现代社会中维持战略平衡的支撑[1-2]。随着导弹试验技术的发展和需要，多传感器测量系统在导弹跟踪中备受关注，因此多传感器数据融合滤波技术对提高弹道跟踪精度具有极其重要的意义[3-4]。

目前常用的多传感器测量融合方法主要有两种结构：集中式测量融合(centralized measurement fusion，CMF)和加权测量融合(weighted measurement fusion，WMF)[5-8]，加权测量融合方法以其高精度和低计算量的优势被广泛使用。同时，由于导弹系统模型具有非线性特点，基于非线性滤波的加权观测融合方法得到了广泛的研究[9-13]，但是，这些算法往往直接对来自各个传感器的测量数据按照不同的加权准则计算融合测量值，并未考虑异常观测值对融合算法精度的影响。

然而，在实际跟踪测量中，由于量测设备的冲击、振动，传感器的老化、故障，复杂环境的干扰，操作人员的失误等，即使采用高精度测量设备和测量机制跟踪目标，都会导致测量数据出现异常值[14-15]。这些异常值带来的数据质量问题，对弹道协同跟踪量测系统的工作存在不利影响，直接影响弹道跟踪的精度和稳定性。

针对观测值存在异常值的问题，国内外学者基于抗差理论进行了大量的研究[16-18]，主要针对单传感器中测量值存在污染的情况进行讨论，然而在实际应用中，多传感器测量机制更为普遍。

本文针对多传感器跟踪导弹过程中数据污染导致弹道解算精度下降的问题，将抗差估计与加权观测融合、平方根UKF及自适应估计相结合，通过构造加权观测融合方程，抗差权函数以及抗差权因子，自适应计算等价噪声协方差融合矩阵来修正异常值，减弱了数据污染对弹道参数估计精度的影响，同时有效地避免了融合估计发散的问题。

1 问题描述

1.1 弹道状态方程

设再入弹道的状态方程为：

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

(1)

式中：f(·)为已知非线性函数；x(k)为第k个时刻系统状态；w(k)为状态噪声，为零均值方差阵为Q的白噪声。

为了得到高精度的弹道模型，需根据导弹的机动性能和噪声统计特性，结合目标运动学信息，利用相应的随机函数来逼近弹道运动。本文采用匀加速模型作为目标的状态方程。

1.2 弹道观测方程

本文中采用光电经纬仪与雷达协同工作的测量机制跟踪导弹，观测设备为多个光电经纬仪与多个雷达组成的测量机制，观测量为测距和角度，以单个光电经纬仪与单个雷达协同工作为例介绍测量原理，测量坐标系光电经纬仪与雷达的测量关系如下[19]：

(2)

式中：(x1,y1,z1)为光电经纬仪的坐标；(x2,y2,z2)为雷达的坐标；(x,y,z)为目标的坐标；R为雷达测距值；E为方位角和A为高低角，经过对式(2)进行数值解算及分析，得到观测量如下：

(3)

可得第j个测量体系的观测方程为：

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

(4)

式中：v(j)(k)为第j个传感器的观测噪声，为零均值方差阵为R的白噪声。h(j)为j个测量体系的观测量，可表示为：

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

(5)

假设h(j)(x(k),k)满足如下形式：

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

(6)

式中：H(j)(k)为线性化系数矩阵，则系统(1)和(4)的加权测量近似融合方程可表示如下：

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

(7)

这里，对式(5)中h(j)(x(k),k)进行μ阶泰勒级数展开得到：

(8)

(9)

(10)

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

(11)

M和H(I)分别为H(0)的列满秩矩阵和行满秩矩阵。近似函数为：

(12)

本文要解决的问题是，对系统式(1)和式(4)，考虑测量数据质量问题对跟踪精度的影响，研究一种抗差加权观测融合平方根无迹卡尔曼滤波算法(weighted measurement robust fusion square root unscented Kalman filter，WMRF-SRUKF)提高弹道跟踪的精度和稳定性。

2 抗差加权观测融合平方根UKF算法

在多台传感器跟踪导弹的过程中，由于导弹飞行的复杂性和测量设备自身的问题，会出现观测值存在异常值的问题，如果不对这部分异常值及时地进行修正，会影响弹道跟踪精度。本节利用抗差估计理论自适应地实现观测异常值的检验和分离以及修正，提高估计精度。

2.1 抗差估计理论

当弹道观测数据中存在异常值时，根据极大似然估计原理，通过对整个时间段的残差取合适的极值函数，抑制异常值的影响。极值函数如下：

(13)

(14)

(15)

(16)

(17)

根据多传感器的观测融合值与该时刻融合预测值，定义测量融合残差向量：

(18)

则抗差权的具体表达式为：

(19)

(20)

(21)

为了计算方便，引入误差因子di，其中φ为抗差权函数，w为抗差权因子。当观测数据存在污染时，通过计算抗差权重因子wi，自适应分布状态融合预测值与当前观测融合信息的权重，从而提高了弹道参数估计的精度。当观测数据正常时，wi为1，算法为标准的加权观测融合算法。

2.2 WMRF-SRUKF算法

Step 1：初始化

在初始时刻传感器的状态向量以及协方差矩阵的平方根初值分别为：

(22)

(23)

式(23)中的函数chol(·)表示对矩阵进行Cholesky分解。

Step 2：时间更新

基于测量数据z(j)(0)～z(j)(k)(j=1,2,…,L)计算的Sigma点为：

(24)

对应的权值：

(25)

式中：下标m为均值;c为协方差;上标i为第几个采样点。参数λ=a2(n+κ)-n，κ=0或κ=3-n，β=2。

计算状态融合预测向量：

i=0,1,…,2n

(26)

(27)

由式(8)和式(9)计算变换矩阵，测量方程预测的采样点计算如下：

(28)

预测测量值计算：

(29)

预测融合误差协方差矩阵的平方根为：

(30)

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

(31)

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

Step 3：观测值异常值判断以及抗差处理

计算k+1的测量融合值：

(32)

(33)

当误差因子di满足：

(34)

Step 4：测量更新

新息协方差矩阵平方根为：

(35)

(36)

互协方差矩阵为：

(37)

增益矩阵为：

(38)

z(I)(k)由式(7)得到，则k+1时刻的状态估计为：

(39)

状态估计误差协方差矩阵的平方根矩阵：

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

(40)

(41)

综上所述，通过泰勒级数展开方法实时计算观测融合矩阵，对来自多个传感器的信息进行融合，采用抗差估计方法构造抗差权重因子，自适应调节测量信息的权重，并基于平方根UKF算法进行滤波求解等价协方差矩阵，对融合过程中可能出现的异常值进行分离与修正，实时地调节滤波增益，从而实现了平方根UKF加权观测融合算法的抗差性能。

3 仿真分析

3.1 参数设置

本文采用光电经纬仪与雷达协同工作跟踪导弹，考虑3个测站构成的传感器网络，采用100次蒙特卡罗仿真，对导弹进行加权融合式实时轨道的确定，以光电经纬仪的位置为原点创建相对三维坐标系。

雷达1相对站址：

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

雷达2相对站址：

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

雷达3相对站址：

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

将雷达带入式(4)得到各个测量体系具体的观测方程，根据式(8)利用二阶泰勒级数展开以及合并同类项去除零部分，计算测量方程的系数矩阵H(0)，根据式(12)计算近似函数为：

h(x(k),k)=

(42)

各个滤波器初始状态为：

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

3.2 仿真对比分析

本节采用本文提出的WMRF-SRUKF算法对式(1)和(4)构成的多传感器弹道动力学模型进行数据融合仿真，并与传统的WMF-UKF算法和WMF-SRUKF算法进行比较。得到弹道理想轨迹和各算法融合估计的弹道轨迹曲线见图1，位置、速度和加速的均方根误差(root mean squared error，RMSE)曲线如图2～4所示。

图1 真实状态及WMF-UKF、WMF-SRUKF及WMRF-SRUKF算法的轨迹估计曲线

图2 WMF-UKF、WMF-SRUKF及WMRF-SRUKF算法的位置RMSE曲线

图3 WMF-UKF、WMF-SRUKF及WMRF-SRUKF算法的速度RMSE曲线

由图1可以看出，采用WMF-UKF和WMF-SRUKF算法对设备进行融合得到的弹道轨迹直观上与理想轨迹存在偏差，而本文提出的WMRF-SRUKF算法估计的弹道融合轨迹相比于上述两种融合算法更接近于真实轨迹，具有精度高的优点。

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

综上所述采用WMRF-SRUKF算法可以对来自各个测量机制的数据进行有效融合，并且及时对测量异常值进行修正，最终得到的弹道参数更接近于真实值，弹道跟踪精度高。同时在融合过程中WMRF-SRUKF运用矩阵满秩分解将测量矩阵的维数由9维压缩至5维，该算法滤波过程中时间复杂度由O(x9)降至为O(x5)，计算负担明显减小。

4 结语

针对融合过程中观测值出现异常值和滤波器发散引起的弹道精度低的问题，本文在加权观测融合算法的基础上，提出了一种抗差融合平方根UKF弹道跟踪算法。该算法利用IGGI等价权函数，自适应估计以及平方根滤波思想，实时地对来自各个传感器的信息进行融合，并对测量数据出现的异常值进行实时剔除以及抗差修正，提高了多传感器信息融合估计的精度，实现了弹道的高精度跟踪问题，在弹道数据融合处理方面具有一定的应用价值。