CN109856624B - 一种针对单雷达直线航迹线的目标状态估计方法 - Google Patents

Abstract

本发明属于传感器目标跟踪与数据融合技术领域，具体涉及一种在复杂数据环境中，且系统噪声和观测噪声均未知的情况下，针对单雷达直线航迹线的目标状态估计方法。该方法其充分利用目标处于匀速直线运动状态这一特点，分别对当前有限测量点的X、Y分量进行相对于的测量时间的垂直距离加权估计，然后根据得到的分量直线航迹线模型参数确定最终的目标状态估计(位置、速度、航向)结果。对比试验表明，该方法不但易于工程实现，而且比传统卡尔曼滤波方法具有更优的目标状态估计效果。

Description

一种针对单雷达直线航迹线的目标状态估计方法

技术领域

本发明属于传感器目标跟踪与数据融合技术领域，具体涉及一种在复杂数据环境中，且系统噪声和观测噪声均未知的情况下，针对单雷达直线航迹线的目标状态估计方法。

背景技术

目标跟踪是指利用传感器(雷达、声纳、红外等)所获得的对运动目标(飞机、坦克、舰艇等)的时序离散观测数据，对目标的运动状态(位置、速度、航向、加速度等)进行估计和跟踪的方法。卡尔曼滤波是目标跟踪与数据融合应用中一种经典的最优线性无偏估计方法，当目标状态空间和测量空间均为线性高斯系统时，可以在干扰噪声背景中对目标运动状态进行最小方差估计和跟踪，而且它是一种递推估计方法，根据每一时刻的观测数据进行滤波更新，大大减少了计算量和存储量，容易满足实时计算要求，因而得到了广泛的实际应用并成为目标跟踪领域的理论基础。

在多雷达组网探测与目标跟踪系统(以下简称“系统”)中，虽然传感器类型相对单一，但是雷达跨代使用、部署分散且地域跨度大，微波、短波、光缆、电缆等多种数据传输手段并存，探测环境易受电磁、气象、季节等因素影响，因此，传统卡尔曼滤波方法在本系统应用中存在以下难点：

1、传统卡尔曼滤波应用条件比较苛刻，与实际系统中复杂的应用数据环境之间存在矛盾。实际系统中测量数据采样间隔较大、测量误差、坐标转换误差以及数据标定与传输误差同时存在。而卡尔曼滤波要求系统模型精确以及系统误差模型和观测误差模型已知，这在实际应用中是很难满足的，或者在系统工作过程中，任一误差分量模型发生变化，都将导致传统卡尔曼滤波方法发散或精度降低。

2、过程噪声协方差Q和量测噪声协方差R在参数值选取上存在困难，且不能兼顾稳态性和瞬时性。传统卡尔曼滤波算法中需要预先估计过程噪声协方差Q和量测噪声协方差R，且在整个滤波过程中保持不变，所以当这两个估计噪声协方差矩阵参数不准确时，在滤波过程中将产生累计误差，导致滤波发散。同时，当Q取值较小，R取值较大时，滤波后形成的目标状态估值稳态性较好，但瞬时性变差，表现在目标位置估计上就是时间配准点(同一时刻目标真实位置点与估计位置点)间距较大，有时可能落后一个测量周期。反之，当Q取值较大，R取值较小时，滤波后形成的目标状态估值瞬时性较好，但稳态性较差，表现为：目标位置估计上更加靠近测量点位置，但是目标航向和速度估值变化较大，这样滤波也就失去了意义。

3、系统测量中不可避免的野值出现，对传统卡尔曼滤波器的工作产生严重影响。由于雷达故障、人为因素或外部观测条件发生改变，就有可能使观测数据发生突变，即出现野值(也称为飞点)。野值可能导致滤波发散或目标状态估计在短时间内失效，表现为估值航迹线出现较大锯齿、目标航向和速度估计突变。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何提供一种在复杂数据环境中，且系统噪声和观测噪声均未知的情况下，针对单雷达直线航迹线的目标状态估计方法。

(二)技术方案

为解决上述技术问题，本发明提供一种针对单雷达直线航迹线的目标状态估计方法，所述估计方法应用于单雷达数据跟踪与多雷达数据融合系统的前期数据预处理过程中；

所述修正方法包括如下步骤：

步骤1：将某雷达针对某个处于匀速直线飞行期的目标的n个观测点数据(ρ_i,θ_i,h_i,t_i)转换为统一直角坐标(X_i,Y_j,t_i)；其中，(ρ_i,θ_i,h_i,t_i)表示t_i时刻该雷达测得的目标距离ρ_i、方位θ_i和高度h_i，i＝1,2,…n；

步骤2：对目标X轴运动分量{(t_i,X_i),i＝1,2,…n}使用加权直线参数估计模型进行迭代估计，得到t_n时刻目标在X方向上的位置P_xn、速度V_xn和方向k_xn；

步骤3：对目标Y轴运动分量{(t_i,Y_j),i＝1,2,…n}使用加权直线参数估计模型进行迭代估计，得到t_n时刻目标在Y方向上的位置P_yn、速度V_yn和方向k_yn；

步骤4：则t_n时刻目标在统一直角坐标系中的位置为(P_xn,P_yn)，此时，目标速度V_n和航向K_n分别为：

(三)有益效果

针对上述现有技术中的三个问题，本发明提供一种基于复杂数据环境，且无须获取系统噪声和观测噪声等先验知识的情况下，针对单雷达直线航迹线观测数据的目标状态估计方法，通过与传统卡尔曼滤波算法在目标位置、航向和速度估值上进行对比实验表明，本发明取得了更好的估值效果，为在本系统中开展目标跟踪与数据融合工作提供了一种稳定、有效且易于工程实现的滤波方法。

与现有技术相比较，本发明提出的适应多雷达组网探测与目标跟踪系统复杂数据环境，针对单雷达直线航迹线的目标状态估计方法，充分利用目标处于匀速直线运动状态这一特点，分别对当前有限测量点的X、Y分量进行相对于的测量时间的垂直距离加权估计，然后根据得到的分量直线航迹线模型参数确定最终的目标状态估计(位置、速度、航向)结果。对比试验表明，该方法不但易于工程实现，而且比传统卡尔曼滤波方法具有更优的目标状态估计效果。

附图说明

图1为本发明技术方案中目标状态估计方法的流程图；

图2为本发明实施例2中目标真实位置与测量位置在统一直角坐标系中的显示图；

图3为本发明实施例2中目标真实位置、卡尔曼估值位置和本发明估值位置三者比较显示图；

图4为本发明实施例2中目标测量位置、卡尔曼估值位置和本发明估值位置三者比较显示图；

图5为本发明实施例2中使用卡尔曼方法和使用本发明方法得到的所有时刻的时间配准点间距显示图；

图6为本发明实施例2中使用卡尔曼方法和使用本发明方法得到的所有时刻的平均时间配准点间距显示图；

图7为本发明实施例2中目标真实速度、卡尔曼估计速度和本发明估计速度三者比较显示图；

图8为本发明实施例2中使用卡尔曼方法和使用本发明方法得到的各点速度差显示图；

图9为本发明实施例2中使用卡尔曼方法和使用本发明方法得到的平均速度差随测量点数变化情况显示图；

图10为本发明实施例2中目标真实航向、卡尔曼估计航向和本发明估计航向三者比较显示图；

图11为本发明实施例2中使用卡尔曼方法和使用本发明方法得到的各点航向差显示图；

图12为本发明实施例2中使用卡尔曼方法和使用本发明方法得到的平均航向差随测量点数变化情况显示图；

图13为本发明实施例2中含有野值时目标真实位置与测量位置在统一直角坐标系中的显示图；

图14为本发明实施例2中含有野值时目标真实位置、卡尔曼估值位置和本发明估值位置三者比较显示图；

图15为本发明实施例2中含有野值时目标测量位置、卡尔曼估值位置和本发明估值位置三者比较显示图；

图16为本发明实施例2中含有野值时使用卡尔曼方法和使用本发明方法得到的所有时刻的时间配准点间距显示图；

图17为本发明实施例2中含有野值时使用卡尔曼方法和使用本发明方法得到的所有时刻的平均时间配准点间距显示图；

图18为本发明实施例2中含有野值时目标真实速度、卡尔曼估计速度和本发明估计速度三者比较显示图；

图19为本发明实施例2中含有野值时使用卡尔曼方法和使用本发明方法得到的各点速度差显示图；

图20为本发明实施例2中含有野值时使用卡尔曼方法和使用本发明方法得到的平均速度差随测量点数变化情况显示图；

图21为本发明实施例2中含有野值时目标真实航向、卡尔曼估计航向和本发明估计航向三者比较显示图；

图22为本发明实施例2中含有野值时使用卡尔曼方法和使用本发明方法得到的各点航向差显示图；

图23为本发明实施例2中含有野值时使用卡尔曼方法和使用本发明方法得到的平均航向差随测量点数变化情况显示图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

为解决现有技术的问题，本发明提供一种针对单雷达直线航迹线的目标状态估计方法，如图1所示，所述估计方法应用于单雷达数据跟踪与多雷达数据融合系统的前期数据预处理过程中；

所述修正方法包括如下步骤：

步骤1：将某雷达针对某个处于匀速直线飞行期的目标的n个观测点数据(ρ_i,θ_i,h_i,t_i)转换为统一直角坐标(X_i,Y_j,t_i)；其中，(ρ_i,θ_i,h_i,t_i)表示t_i时刻该雷达测得的目标距离ρ_i、方位θ_i和高度h_i，i＝1,2,…n；

步骤2：对目标X轴运动分量{(t_i,X_i),i＝1,2,…n}使用加权直线参数估计模型进行迭代估计，得到t_n时刻目标在X方向上的位置P_xn、速度V_xn和方向k_xn；

步骤3：对目标Y轴运动分量{(t_i,Y_j),i＝1,2,…n}使用加权直线参数估计模型进行迭代估计，得到t_n时刻目标在Y方向上的位置P_yn、速度V_yn和方向k_yn；

步骤4：则t_n时刻目标在统一直角坐标系中的位置为(P_xn,P_yn)，此时，目标速度V_n和航向K_n分别为：

其中，所述步骤1包括如下步骤：

步骤1.1：选择某雷达观测数据集{(ρ_i,θ_i,h_i,t_i),i＝1,2,…n}，是针对某个处于匀速直线飞行期的目标的时序观测数据；如果是三坐标雷达观测数据，一般认为h_i保持不变；如果是两坐标雷达观测数据，则认为h_i＝0。θ_i取值范围[0,360)，单位为度，其中雷达正北方向为0度，正东方向为90度。

步骤1.2：将雷达观测数据(ρ_i,θ_i,h_i,t_i)，i＝1,2,…n转换为以本站为中心的二维直角坐标(X_zi,Y_zi)：

步骤1.3：将(X_zi,Y_zi)，i＝1,2,…n转换为中心统一直角坐标(X_i,Y_i)：

X_i＝X_zicosδ_xz-Y_zisinδ_xz+X_zx

Y_i＝X_zisinδ_xz+Y_zicosδ_xz+Y_zx

其中：(X_zx,Y_zx)为雷达站址在中心统一直角坐标系中的坐标；δ_xz为雷达站址与直角坐标系中心点的经度差，单位为弧度。

其中，所述步骤1.1中，t_i<t_i+1，且n不大于N，N表示参与目标状态估计的最近时间内有限个观测数据点的个数，一般N取4～15为宜。

其中，所述步骤2包括如下步骤：

步骤2.1：对目标X轴运动分量{(t_i,X_i),i＝1,2,…n}采用不加权直线参数估计模型粗略估计在X方向上的目标运动状态方程x-k₁t-d₁＝0；

不加权直线参数估计模型的运算方法包括如下步骤：

步骤2.1.1：用目标X轴运动分量{(t_i,X_i),i＝1,2,…n}(简记为：{(x_i,y_i)，i＝1,2,…n})的所有点到某条直线的距离l_i的平方和最小作为条件构造直线，计算在此条件下的这条直线的最佳参数(k,d)；其中，k为该直线的斜率，d为该直线在x轴上的截距；

对于(1)式，应有f(k,d)分别对k和d求偏导数，并等于零，即有下式成立：

于是有：

化简得：

-kd²+(-a₁k²+2b₁k+a₁)d+c₀k²+(a₂-b₂)k-c₀＝0 (2)

其中：

化简得：

b₁-a₁k-d＝0 (3)

其中：

由(3)式解得：

d＝b₁-a₁k (4)

(4)式代入(2)式得：

整理后得：

记a＝c₀-a₁b₁，

c＝a₁b₁-c₀，则(5)式化简为：

ak²+bk+c＝0 (6)

解(6)式得：

(7)，(8)分别代入(4)式得：

d₁＝b₁-a₁k₁

d₂＝b₁-a₁k₂

步骤2.1.2：按照距离最小原则确定方程的合理解；

(k₁,d₁)和(k₂,d₂)都是方程(2)的实根，且k₁×k₂＝-1，即解得的两条直线相互垂直；按照点{(x_i,y_i)，i＝1,2,…n}到所求直线的距离的平方和最小原则，确定合理的直线参数值；该问题也可以简化为：计算点(x₁,y₁)分别到直线y＝k₁×x+d₁和直线y＝k₂×x+d₂的距离l₁,l₂：

若|l₁|<|l₂|，则取(k₁,d₁)，否则取(k₂,d₂)作为所求直线的合理参数，记为(k₁,d₁)；

步骤2.2：采用加权直线参数估计模型迭代估计目标在X方向上的运动状态方程x-k_xt-d_x＝0；

加权直线参数估计模型的运算方法包括如下步骤：

步骤2.2.1：计算各点(x_i,y_i)到直线y-k₁x-d₁＝0的距离|l_i|之和；

n为数据点数；

步骤2.2.2：求各点(x_i,y_i)到直线y-k₁x-d₁＝0的距离l_i；

式中m表示迭代次数，n表示数据点数；m初始值为1，即：k⁽¹⁾＝k₁，d⁽¹⁾＝d₁；

步骤2.2.3：求|l_i|的倒数；

步骤2.2.4：求各点的权值v_i；

步骤2.2.5：求解加权直线参数估计模型；

用所有数据点{(x_i,y_i)，i＝1,2,…n}到某条直线的加权距离(v_i×l_i)的平方和最小作为条件构造直线，计算在此条件下的这条直线的最佳参数(k,d)；

对于(9)式，应有f(k,d)分别对k和d求偏导数，并等于零，即有下式成立：

于是有：

记：

则(10)式化简为：

c₀'-b₂'k+b₁'kd+a₂'k-c₀'k²-a₁'k²d+a₁'d+b₁'kd-kd²＝0 (11)

由(12)式解得：

d＝b′₁-a′₁k (13)

将(13)式代入(11)式得：

(-c₀'-a₁'b₁')k²+(b′₁ ²+a₂'-a′₁ ²-b₂')k+(c₀'+a₁'b₁')＝0 (14)

记a'＝-c₀'-a₁'b₁'，b'＝b′₁ ²+a′₂-a′₁ ²-b'₂，c'＝c₀'+a₁'b₁'，则(14)式化简为：

a'k+b'k+c'＝0 (15)

解(15)式得：

将(16)，(17)分别代入(13)式得：

d₁＝b₁'-a₁'k₁

d₂＝b₁'-a₁'k₂

步骤2.2.6：按照距离最小原则确定方程的合理解；

按照点{(x_i,y_i)，i＝1,2,…n}到所求直线的距离的平方和最小原则，确定合理的直线参数值；该问题也可以简化为：计算点(x₁,y₁)分别到直线y＝k₁×x+d₁和直线y＝k₂×x+d₂的距离l₁,l₂：

m加1，若|l₁|+|l₂|>Lmin(Lmin初值为10⁶)，则输出(k^(m-1),d^(m-1))作为所求直线的合理参数，迭代过程退出；否则Lmin＝|l₁|+|l₂|；

若|l₁|<|l₂|，则取(k₁,d₁)，否则取(k₂,d₂)作为所求直线的合理参数，记为记为(k^(m),d^(m))，m表示迭代次数；

步骤2.2.7：计算所有数据点到新直线y-k^(m)x-d^(m)＝0的加权距离之和f^(m)(k^(m),d^(m))；

式中m表示迭代次数，n表示数据点数；

步骤2.2.8：判别是否为最佳解；

若f^(m)(k^(m),d^(m))≥f^(m-1)(k^(m-1),d^(m-1))或f^(m)(k^(m),d^(m))≤10^-6，则输出解(k^(m-1),d^(m-1))，并简记为(k_x,d_x)；否则重复步骤2.2.2至步骤2.2.8；式中m表示迭代次数；

步骤2.3：计算t_n时刻目标在X方向上的位置P_xn、速度V_xn和方向k_xn：

P_xn＝k_xt_n+d_x，V_xn＝k_x，k_xn＝k_x。

其中，所述步骤3包括如下步骤：

步骤3.1：对目标Y轴运动分量{(t_i,Y_i),i＝1,2,…n}采用步骤2.1的不加权直线参数估计模型粗略估计在Y方向上的目标运动状态方程y-k₁t-d₁＝0；

步骤3.2：采用步骤2.2的加权直线参数估计模型迭代估计目标在Y方向上的运动状态方程y-k_yt-d_y＝0；

步骤3.3：计算t_n时刻目标在Y方向上的位置P_yn、速度V_yn和方向k_yn：

P_yn＝k_yt_n+d_y，V_yn＝k_y，k_yn＝k_y。

实施例1

本实施例具体描述本发明所提出的一种针对单雷达直线航迹线的目标状态估计方法，所述估计方法应用于单雷达数据跟踪与多雷达数据融合系统的前期数据预处理过程中。

所述估计方法包括如下步骤：

步骤1：将某雷达针对某个处于匀速直线飞行期的目标的n个观测点数据(ρ_i,θ_i,h_i,t_i)(表示t_i时刻该雷达测得的目标距离ρ_i、方位θ_i和高度h_i，i＝1,2,…n。)转换为统一直角坐标(X_i,Y_j,t_i)。如果n＝1，则输出目标位置(X₁,Y₁,t₁)、目标速度和航向均为0，程序退出。

步骤1.1：选择某雷达针对某个处于匀速直线飞行期的目标的n个时序观测数据{(ρ_i,θ_i,h_i,t_i),i＝1,2,…n}，其中：t_i<t_i+1，且n取值为7。因为是两坐标雷达观测数据，认为h_i＝0。相关数据见表1.1～表1.2。

表1.1：基本参数

表1.2：雷达观测数据

序号	时间t(秒)	距离ρ(km)	方位θ(度)	高度h(km)
					1	30	213.67	205.97	0
2	40	209.76	205.97	0
					3	51	205.81	205.68	0
4	61	201.68	205.1	0
					5	70	199.27	205.96	0
6	81	194.32	205.56	0
					7	90	191.82	204.8	0

步骤1.2：将雷达观测数据(ρ_i,θ_i,h_i,t_i)，i＝1,2,…29转换为以本站为中心的二维直角坐标(X_zi,Y_zi)：

计算结果见表1.3。

表1.3：以雷达为中心的二维直角坐标

序号	时间t(秒)	X<sub>z</sub>(km)	Y<sub>z</sub>(km)
				1	30	-93.56	-192.10
2	40	-91.85	-188.59
				3	51	-89.17	-185.49
4	61	-85.54	-182.64
				5	70	-87.24	-179.16
6	81	-83.84	-175.30
				7	90	-80.47	-174.13

步骤1.3：将(X_zi,Y_zi)，i＝1,2,…29转换为中心统一直角坐标(X_i,Y_i)：

X_i＝X_zicosδ_xz-Y_zisinδ_xz+X_zx

Y_i＝X_zisinδ_xz+Y_zicosδ_xz+Y_zx

其中：(X_zx,Y_zx)为雷达站址在中心统一直角坐标系中的坐标。δ_xz为雷达站址与直角坐标系中心点的经度差(单位为弧度)。计算结果见表1.4。

表1.4：测量点的中心统一直角坐标

序号	时间t(秒)	X(km)	Y(km)
				1	30	1503.10	2334.16
2	40	1504.88	2337.71
				3	51	1507.61	2340.85
4	61	1511.29	2343.76
				5	70	1509.65	2347.22
6	81	1513.12	2351.13
				7	90	1516.51	2352.37

步骤2：对目标X轴运动分量{(t_i,X_i),i＝1,2,…7}使用“加权直线参数估计模型”进行迭代估计，得到t_n时刻目标在X方向上的位置P_xn、速度V_xn和方向k_xn。

步骤2.1：对目标X轴运动分量{(t_i,X_i),i＝1,2,…7}采用“不加权直线参数估计模型”粗略估计在X方向上的目标运动状态方程x-k₁t-d₁＝0；所述不加权直线参数估计模型的运算过程包括如下步骤：

步骤2.1.1：用所有点{(t_i,X_i),i＝1,2,…7}(简记为：{(x_i,y_i)，i＝1,2,…7})到某条直线的距离l_i的平方和最小作为条件构造直线，计算在此条件下的这条直线的最佳参数(k,d)。求得直线参数。对(1)式的求解步骤如下。

表1.5：目标在X轴的运动分量

序号	t＝x	X＝y
			1	30	1503.10
2	40	1504.88
			3	51	1507.61
4	61	1511.29
			5	70	1509.65
6	81	1513.12
			7	90	1516.51

①计算a₁,a₂,b₁,b₂,c₀(n＝7)。

表1.6：参数计算结果1

a<sub>1</sub>	60.428571428571431
		a<sub>2</sub>	4054.7142857142858
b<sub>1</sub>	1509.4479365716902
		b<sub>2</sub>	2278451.8354599578
c<sub>0</sub>	91298.2384983306

②计算a,b,c。

a＝c₀-a₁b₁

c＝a₁b₁-c₀

表1.7：中间参数计算结果2

a	84.4560454984603
		b	384.33980139158666
c	-84.4560454984603

③解方程，计算所有解。

d₁＝b₁-a₁k₁

d₂＝b₁-a₁k₂

表1.8：X方向观测航迹线参数解粗略计算结果

k<sub>1</sub>	0.21004805826508055
		k<sub>2</sub>	-4.7608152546594873
d<sub>1</sub>	1496.755032479386
		d<sub>2</sub>	1797.1372012461136

步骤2.1.2：按照距离最小原则确定方程的合理解。

计算测量点(x₁,y₁)分别到直线y＝k₁×x+d₁和直线y＝k₂×x+d₂的距离l₁,l₂：

因|l₁|<|l₂|，则取(k₁,d₁)＝(0.210048，1496.755032)作为所求直线的合理参数，记为(k₁,d₁)。

步骤2.2：采用“加权直线参数估计模型”迭代估计目标在X方向上的运动状态方程x-k_xt-d_x＝0；所述加权直线参数估计模型的运算过程包括如下步骤：

步骤2.2.1：计算各点(x_i,y_i)到直线y-k₁x-d₁＝0的距离|l_i|之和(n＝7)。

步骤2.2.2：求各点(x_i,y_i)到直线y-k₁x-d₁＝0的距离l_i。

式中：m表示迭代次数，初始值为1，k⁽¹⁾＝k₁，d⁽¹⁾＝d₁。计算结果见表1.9。

步骤2.2.3：求|l_i|的倒数。计算结果见表1.9。

步骤2.2.4：求各点的权值v_i。计算结果见表1.9

表1.9:距离和权值计算结果

序号	l<sub>i</sub>	P<sub>i</sub>	v<sub>i</sub>
				1	0.040986	24.39842	0.619349
2	-0.27544	3.630562	0.092161
				3	0.134585	7.430221	0.188615
4	1.680321	0.595124	0.015107
				5	-1.77307	0.563995	0.014317
6	-0.63698	1.569905	0.039852
				7	0.829595	1.205408	0.030599

步骤2.2.5：求解加权直线参数估计模型。

用所有数据点{(x_i,y_i)，i＝1,2,…7}到某条直线的加权距离(v_i×l_i)的平方和最小作为条件构造直线，计算在此条件下的这条直线的最佳参数(k,d)。对(9)式的求解步骤如下。

①计算a₀,a₁,a₂,b₁,b₂,c₀(n＝7)。

表1.10：中间参数计算结果3

a<sub>0</sub>’	0.43062036934622938
		a<sub>1</sub>’	32.286170046601832
a<sub>2</sub>’	1094.2668062910109
		b<sub>1</sub>'	1503.5792870236251
b<sub>2</sub>'	2260752.9983791234
		c<sub>0</sub>	-48555.779283781565

②计算a',b',c'。

a'＝-c₀'-a₁'b₁'，b'＝b′₁ ²+a'₂-a′₁ ²-b'₂，c'＝c₀'+a₁'b₁'；

表1.11：中间参数计算结果4

a’	10.962744388460123
		b'	49.54401736240834
c’	-10.962744388460123

③解方程，计算所有解。

d₁＝b₁'-a₁'k₁

d₂＝b₁'-a₁'k₂

表1.12:X方向观测航迹线参数解第m次计算结果

k<sub>1</sub>	0.21138550191927813
		k<sub>2</sub>	-4.7306934057467718
d<sub>1</sub>	1496.7544587632729
		d<sub>2</sub>	1656.3152587599034

步骤2.2.6：按照距离最小原则确定方程的合理解。

计算测量点(x₁,y₁)分别到直线y＝k₁×x+d₁和直线y＝k₂×x+d₂的距离l₁,l₂：

m值加1，因|l₁|+|l₂|＝2.338488<Lmin＝10⁶，则Lmin＝|l₁|+|l₂|＝2.338488。

因|l₁|<|l₂|，则取(k₁,d₁)＝(0.211386，1496.754459)作为所求直线的合理参数，记为(k^(m),d^(m))。

步骤2.2.7：计算所有数据点到新直线y-k^(m)x-d^(m)＝0的加权距离之和f^(m)(k^(m),d^(m))，m表示迭代次数。

步骤2.2.8：判别是否为“最佳”解。

因f⁽¹⁾(k⁽¹⁾,d⁽¹⁾)＝5.370975，f⁽²⁾(k⁽²⁾,d⁽²⁾)＝0.146692,f⁽²⁾(k⁽²⁾,d⁽²⁾)<f⁽¹⁾(k⁽¹⁾,d⁽¹⁾)，则重复步骤2.2.2-步骤2.2.8。

经计算，m＝3、执行步骤2.2.6时，|l₁|+|l₂|＝775.635331，大于上一次迭代时的Lmin＝2.338488，因此输出解(k⁽²⁾,d⁽²⁾)，并简记为(k_x,d_x)＝(0.211386，1496.754459)。

步骤2.3：计算t_n时刻目标在X方向上的位置P_xn、速度V_xn和方向k_xn(n＝7)：

P_xn＝k_xt_n+d_x＝0.211386×90+1496.754459＝1515.779154，

V_xn＝k_x＝0.211386，

k_xn＝k_x＝0.211386。

步骤3：对目标Y轴运动分量{(t_i,Y_i),i＝1,2,…n}使用“加权直线参数估计模型”进行迭代估计，得到t_n时刻目标在Y方向上的位置P_yn、速度V_yn和方向k_yn(n＝7)。

表1.13：目标在Y轴的运动分量

序号	t＝x	Y＝y
			1	30	2334.16
2	40	2337.71
			3	51	2340.85
4	61	2343.76
			5	70	2347.22
6	81	2351.13
			7	90	2352.37

步骤3.1：对目标Y轴运动分量{(t_i,Y_i),i＝1,2,…n}采用“不加权直线参数估计模型”粗略估计在Y方向上的目标运动状态方程y-k₁t-d₁＝0，具体过程与步骤2.1类同。解得(k₁,d₁)＝(0.312850，2324.980758)作为所求直线的合理参数。

步骤3.2：采用“加权直线参数估计模型”迭代估计目标在Y方向上的运动状态方程y-k_yt-d_y＝0，具体过程与步骤2.2类同，解得(k_y,d_y)＝(0.314830，2324.824389)。

步骤3.3：计算t_n时刻目标在Y方向上的位置P_yn、速度V_yn和方向k_yn(n＝7)：

P_yn＝k_yt_n+d_y＝0.314830×90+2324.824389＝2353.159116，

V_yn＝k_y＝0.314830，

k_yn＝k_y＝0.314830。

步骤4：则t_n时刻目标在统一直角坐标系中的位置(P_xn,P_yn)、目标速度V_n和航向K_n分别为(n＝7)：

(P_xn,P_yn)＝(1515.779154，2353.159116)公里

实施例2

本实施例具体描述本发明所提出的一种针对单雷达直线航迹线的目标状态估计方法与传统卡尔曼滤波方法的对比试验过程，以及二者在执行效果上的比较。

所述对比试验过程包括如下步骤：

步骤1：产生模拟数据的真实值和测量值。

按照表2.1中的模拟目标参数值产生匀速直线飞行状态下目标的29个位置点的真实值和测量值，结果见表2.2。接下来将按照本发明所述方法对这29个测量点逐一进行位置、速度和航向估计。

表2.1：基本参数

表2.2：雷达观测数据的真实值和测量值

步骤2：将目标点数据{(ρ_i,θ_i,h_i,t_i)，i＝1,2,…29。}转换为统一直角坐标{(X_i,Y_i,t_i),i＝1,2,…29。}。

X_i＝X_zicosδ_xz-Y_zisinδ_xz+X_zx

Y_i＝X_zisinδ_xz+Y_zicosδ_xz+Y_zx

其中：(X_zx,Y_zx)为雷达站址在中心统一直角坐标系中的坐标。δ_xz为雷达站址与直角坐标系中心点的经度差(单位为弧度)。计算结果见表2.3。目标点真实位置和测量位置在中心统一直角坐标系中的显示如图2所示。

表2.3：目标点的中心统一直角坐标

序号	时刻t	真实值X	真实值Y	测量值X	测量值Y
						1	0	1496.5646	2326.2047	1497.0174	2326.1440
2	8	1498.2505	2328.5832	1498.0045	2328.7740
						3	20	1500.6231	2331.9306	1500.0969	2332.3021
4	31	1502.9457	2335.2076	1503.1397	2334.8118
						5	40	1504.9833	2338.0823	1504.9885	2337.9738
6	50	1507.0022	2340.9307	1507.2381	2340.7606
						7	59	1508.7674	2343.4212	1509.8456	2343.1663
8	71	1511.2977	2346.9911	1510.4878	2347.2460
						9	81	1513.3549	2349.8935	1513.1317	2350.4127
10	91	1515.4561	2352.8580	1516.0866	2352.6654
						11	100	1517.3663	2355.5531	1516.7906	2355.9697
12	108	1519.1012	2358.0007	1519.9987	2357.4387
						13	119	1521.4287	2361.2845	1521.9205	2361.1152
14	128	1523.3614	2364.0113	1523.5174	2364.1355
						15	139	1525.6674	2367.2648	1526.7557	2366.6431
16	152	1528.3623	2371.0669	1528.7004	2370.8347
						17	159	1529.7063	2372.9631	1528.8759	2373.2831
18	172	1532.4737	2376.8675	1532.6452	2376.8696
						19	179	1533.9215	2378.9101	1533.9568	2378.7162
20	189	1536.0717	2381.9438	1536.4034	2381.7430
						21	199	1538.1876	2384.9291	1538.5191	2384.7892
22	209	1540.2849	2387.8881	1540.2800	2387.6327
						23	219	1542.3793	2390.8430	1541.5705	2391.3966
24	229	1544.4864	2393.8158	1544.0729	2393.7185
						25	242	1547.1235	2397.5364	1547.2335	2397.4669
26	250	1548.8015	2399.9039	1549.2211	2399.6019
						27	261	1551.0409	2403.0633	1550.2379	2403.2995
28	270	1552.8541	2405.6215	1552.3759	2405.7809
						29	281	1555.2600	2409.0159	1555.1291	2409.1223

步骤3：对表2.3中的测量值{(t_i,X_i,Y_i),i＝1,2,…29}使用本发明所述估计方法逐点进行目标位置(wP_xi,wP_yi)、速度wV_i和航向wK_i估计。其中：参数n取值为7。表2.4是n＝7时每次参与估值计算的点序号表。表2.5是应用本发明的估计结果。

表2.4：n＝7时每次参与估值计算的点

表2.5：应用本发明(n＝7)逐点进行目标状态估计

步骤4：对表2.3中的测量值{(t_i,X_i,Y_i),i＝1,2,…29}使用传统卡尔曼滤波方法逐点进行目标位置(kP_xi,kP_yi)、速度kV_i和航向kK_i估计。其中：Q、R参数取值见表2.1。表2.6是应用卡尔曼滤波方法的逐点估计结果。

表2.6：应用卡尔曼方法逐点进行目标状态估计

目标点真实位置、测量点位置、本发明估计位置和卡尔曼滤波估计位置在中心统一直角坐标系中的显示如图3、图4所示。图上可见，两种滤波方法得到的航迹线几乎重合，均优于测量航迹线，接下来我们对时间配准点、速度和航向等指标进一步统计、比较。

步骤5：时间配准点比较

为了进一步比较位置估值效果，我们引入时间配准点概念，即：相同时刻目标真实位置点与估计位置点称为一对时间配准点。t_i时刻一对时间配准点之间的距离称为时间配准点间距d_i，d₁～d_m的平均值定义为t_m时刻的平均时间配准点间距vd_m。根据表2.3～2.6的计算结果，可以统计使用本发明方法和使用卡尔曼方法得到时间配准点间距与平均时间配准点间距值，见表2.7和表2.8，其中(rP_x，rP_y)表示目标真实位置，wvd₂₉表示使用本发明方法得到的所有估值点的平均时间配准点间距，kvd₂₉表示使用卡尔曼方法得到的所有估值点的平均时间配准点间距。

表2.7：本发明时间配准点间距wd与平均间距wvd

表2.8：卡尔曼时间配准点间距kd与平均间距kvd

图5和图6是本发明方法和卡尔曼方法时间配准点间距与平均时间配准点间距的对比图。经过统计，有23个点的时间配准点间距wd_i≤kd_i，占全部点数的79.31％；所有时刻的平均时间配准点间距均有wvd_i≤kvd_i；全程平均时间配准点间距wvd₂₉只占kvd₂₉的30.15％。本发明方法在测量初期、点数较少时就能获得更加准确的目标位置估值，优势非常明显。于是可得到以下结论：在本次试验中，以目标真实位置为基准，使用本发明方法得到的目标位置点优于卡尔曼方法位置点。

步骤6：速度估值比较

我们定义t_i时刻目标估计速度与真实速度差的绝对值为速度差dv_i，dv₁～dv_m的平均值定义为t_m时刻的平均速度差vdv_m。根据表2.3～2.6的计算结果，可以统计使用本发明方法和使用卡尔曼方法得到的所有时间点上的速度差和平均速度差值，见表2.9，其中rV表示目标真实速度；wV_i、wdv_i、wvdv_i表示使用本发明方法得到的t_i时刻目标速度估值、速度差和平均速度差；kV_i、kdv_i、kvdv_i表示使用卡尔曼方法得到的t_i时刻目标速度估值、速度差和平均速度差。

表2.9：速度、速度差和平均速度差

图7、图8和图9是本发明方法和卡尔曼方法分别在速度、速度差和平均速度差的对比图。经过统计，有28个点的速度差wdv_i≤kdv_i，占全部点数的96.55％；所有时刻的平均速度差均有wvdv_i≤kvdv_i；全程平均速度差wvdv₂₉只占kvdv₂₉的8.01％。图上可以看出，本发明得到的速度估值更加准确、稳定，而且本发明方法在测量初期、点数较少时就能获得更加准确的速度估值，优势非常明显。

步骤7：航向估值比较

我们定义t_i时刻目标估计航向与真实航向差的绝对值为航向差dk_i，dk₁～dk_m的平均值定义为t_m时刻的平均航向差vdk_m。根据表2.3～2.6的计算结果，可以统计使用本发明方法和使用卡尔曼方法得到的所有时间点上的航向差和平均航向差值，见表2.10，其中rK表示目标真实航向；wK_i、wdk_i、wvdk_i表示使用本发明方法得到的t_i时刻目标航向估值、航向差和平均航向差；kK_i、kdk_i、kvdk_i表示使用卡尔曼方法得到的t_i时刻目标航向估值、航向差和平均航向差。

表2.10：航向、航向差和平均航向差

图10、图11和图12是本发明方法和卡尔曼方法分别在航向、航向差和平均航向差的对比图。经过统计，有19个点的航相差wdk_i≤kdk_i，占全部点数的65.52％；所有时刻的平均航向差均有wvdk_i≤kvdk_i；全程平均航向差wvdk₂₉只占kvdk₂₉的50.89％。图上可以看出，本发明方法在测量初期、点数较少时就能获得更加准确的航向估值，优势较为明显。

步骤8：出现野值后的估值效果比较

为了模拟野值出现，我们在表2.3的第22个测量点上人为加入较大误差。图13～23分别显示了出现野值后，本发明方法和卡尔曼方法在航迹线、时间配准点、速度和航向等指标上的比较效果，表2.11是测量中包含野值与不包含野值时各项指标的统计结果。

表2.11：包含/不包含野值时各项指标统计

从以上图表可以看出，加入野值后，卡尔曼滤波方法在野值点处，位置、速度和航向估值均出现较大突变；另外，野值还导致了卡尔曼方法滤波发散，影响了其后几个点的目标状态估值效果。相比之下，本发明方法对野值抑制效果较为明显，无论是在野值点处，还是其后几个点上，估值效果都优于卡尔曼方法。

本发明基于复杂数据环境，在无须获取系统噪声和观测噪声等先验知识的情况下，充分利用目标处于匀速直线运动状态这一特点，分别对当前n个有限测量点的X、Y分量进行相对于的测量时刻的垂直距离加权迭代估计，然后根据得到的分量直线航迹线模型参数确定最终的目标状态估计(位置、速度、航向)结果。通过与传统卡尔曼滤波方法在时间配准点间距、速度差和航相差等指标的对比试验表明，本发明不但易于工程实现，而且比传统卡尔曼滤波方法具有更优的目标状态估计效果和野值抑制能力。该方法有效解决了传统卡尔曼滤波方法应用条件比较苛刻、过程噪声协方差和量测噪声协方差难以确定和系统测量中出现野值，可能导致滤波发散或目标状态估计突变等问题，极大地改善了单雷达数据质量，为在本系统中开展目标跟踪与数据融合工作提供了一种稳定、有效且易于工程实现的滤波方法。本发明估计方法科学、方案实施步骤合理，目标估计效果理想，对于提高了多雷达目标状态估计的一致性和准确性具有重要意义。本发明所提供的方法的时间复杂度和空间复杂度都很低，可操作性和实用性很强。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。例如但不限于以下几点：

(1)为了节省计算时间，在步骤1中，可以存储前n－1个测量点的统一直角坐标，每次只对第n个测量点进行极坐标到统一直角坐标转换。

(2)关于n的取值问题，我们认为一般取4～15为宜。但是，在相同数据环境中，在保证目标处于匀速直线飞行状态的前提下，n越大，目标状态估值精度就越高。

(3)本发明中涉及到的目标航向也称为格式化或标准化航向，是指从正北(Y轴)算起，顺时针到目标行进方向的夹角，正北为0度，正东为90度；航向取值范围：大于等于0度，小于360度。通过步骤4的计算公式

得到的航向单位为弧度，且需要考虑k_yn→0时的情况。因此，在实际工程应用中，存在航向K_n计算时取极值和归一化到[0,360)度的问题，在此给出步骤4中更加详细的K_n(单位为弧度)计算公式：

Claims (1)

1.一种针对单雷达直线航迹线的目标状态估计方法，其特征在于，所述估计方法应用于单雷达数据跟踪与多雷达数据融合系统的前期数据预处理过程中；

所述估计方法包括如下步骤：

步骤1：将某雷达针对某个处于匀速直线飞行期的目标的n个观测点数据(ρ_i,θ_i,h_i,t_i)转换为统一直角坐标(X_i,Y_j,t_i)；其中，(ρ_i,θ_i,h_i,t_i)表示t_i时刻该雷达测得的目标距离ρ_i、方位θ_i和高度h_i，i＝1,2,…n；

步骤2：对目标X轴运动分量{(t_i,X_i),i＝1,2,…n}使用加权直线参数估计模型进行迭代估计，得到t_n时刻目标在X方向上的位置P_xn、速度V_xn和方向k_xn；

步骤3：对目标Y轴运动分量{(t_i,Y_j),i＝1,2,…n}使用加权直线参数估计模型进行迭代估计，得到t_n时刻目标在Y方向上的位置P_yn、速度V_yn和方向k_yn；

步骤4：则t_n时刻目标在统一直角坐标系中的位置为(P_xn,P_yn)，此时，目标速度V_n和航向K_n分别为：

所述步骤1包括如下步骤：

步骤1.1：选择某雷达观测数据集{(ρ_i,θ_i,h_i,t_i),i＝1,2,…n}，是针对某个处于匀速直线飞行期的目标的时序观测数据；

步骤1.2：将雷达观测数据(ρ_i,θ_i,h_i,t_i)，i＝1,2,…n转换为以本站为中心的二维直角坐标(X_zi,Y_zi)：

步骤1.3：将(X_zi,Y_zi)，i＝1,2,…n转换为中心统一直角坐标(X_i,Y_i)：

X_i＝X_zicosδ_xz-Y_zisinδ_xz+X_zx

Y_i＝X_zisinδ_xz+Y_zicosδ_xz+Y_zx

其中：(X_zx,Y_zx)为雷达站址在中心统一直角坐标系中的坐标；δ_xz为雷达站址与直角坐标系中心点的经度差，单位为弧度；

所述步骤1.1中，t_i<t_i+1，且n不大于N，N表示参与目标状态估计的最近时间内有限个观测数据点的个数，N取4～15；

所述步骤2包括如下步骤：

步骤2.1：对目标X轴运动分量{(t_i,X_i),i＝1,2,…n}采用不加权直线参数估计模型粗略估计在X方向上的目标运动状态方程x-k₁t-d₁＝0；

不加权直线参数估计模型的求解方法包括如下步骤：

步骤2.1.1：目标X轴运动分量{(t_i,X_i),i＝1,2,…n}，简记为：{(x_i,y_i)，i＝1,2,…n}，用目标X轴运动分量的所有点到某条直线的距离l_i的平方和最小作为条件构造直线，计算在此条件下的这条直线的最佳参数(k,d)；其中，k为该直线的斜率，d为该直线在x轴上的截距；

对于(1)式，应有f(k,d)分别对k和d求偏导数，并等于零，即有下式成立：

于是有：

化简得：

-kd²+(-a₁k²+2b₁k+a₁)d+c₀k²+(a₂-b₂)k-c₀＝0 (2)

其中：

化简得：

b₁-a₁k-d＝0 (3)

其中：

由(3)式解得：

d＝b₁-a₁k (4)

(4)式代入(2)式得：

整理后得：

记a＝c₀-a₁b₁，

c＝a₁b₁-c₀，则(5)式化简为：

ak²+bk+c＝0 (6)

解(6)式得：

(7)，(8)分别代入(4)式得：

d₁＝b₁-a₁k₁

d₂＝b₁-a₁k₂

步骤2.1.2：按照距离最小原则确定方程的合理解；

(k₁,d₁)和(k₂,d₂)都是方程(2)的实根，且k₁×k₂＝-1，即解得的两条直线相互垂直；按照点{(x_i,y_i)，i＝1,2,…n}到所求直线的距离的平方和最小原则，确定合理的直线参数值；该问题也可以简化为：计算点(x₁,y₁)分别到直线y＝k₁×x+d₁和直线y＝k₂×x+d₂的距离l₁,l₂：

若|l₁|<|l₂|，则取(k₁,d₁)，否则取(k₂,d₂)作为所求直线的合理参数，记为(k₁,d₁)；

步骤2.2：采用加权直线参数估计模型迭代估计目标在X方向上的运动状态方程x-k_xt-d_x＝0；

加权直线参数估计模型的求解方法包括如下步骤：

步骤2.2.1：计算各点(x_i,y_i)到直线y-k₁x-d₁＝0的距离|l_i|之和；

n为数据点数；

步骤2.2.2：求各点(x_i,y_i)到直线y-k₁x-d₁＝0的距离l_i；

式中m表示迭代次数，n表示数据点数；m初始值为1，即：k⁽¹⁾＝k₁，d⁽¹⁾＝d₁；

步骤2.2.3：求|l_i|的倒数；

步骤2.2.4：求各点的权值v_i；

步骤2.2.5：求解加权直线参数估计模型；

用所有数据点{(x_i,y_i)，i＝1,2,…n}到某条直线的加权距离(v_i×l_i)的平方和最小作为条件构造直线，计算在此条件下的这条直线的最佳参数(k,d)；

对于(9)式，应有f(k,d)分别对k和d求偏导数，并等于零，即有下式成立：

于是有：

记：

则(10)式化简为：

c₀'-b₂'k+b₁'kd+a₂'k-c₀'k²-a₁'k²d+a₁'d+b₁'kd-kd²＝0 (11)

由(12)式解得：

d＝b'₁-a'₁k (13)

将(13)式代入(11)式得：

记a'＝-c₀'-a₁'b₁'，

c'＝c'₀+a'₁b'₁，则(14)式化简为：

a'k+b'k+c'＝0 (15)

解(15)式得：

将(16)，(17)分别代入(13)式得：

d₁＝b₁'-a₁'k₁

d₂＝b₁'-a₁'k₂

步骤2.2.6：按照距离最小原则确定方程的合理解；

按照点{(x_i,y_i)，i＝1,2,…n}到所求直线的距离的平方和最小原则，确定合理的直线参数值；该问题也可以简化为：计算点(x₁,y₁)分别到直线y＝k₁×x+d₁和直线y＝k₂×x+d₂的距离l₁,l₂：

m加1，若|l₁|+|l₂|>Lmin(Lmin初值为10⁶)，则输出(k^(m-1),d^(m-1))作为所求直线的合理参数，迭代过程退出；否则Lmin＝|l₁|+|l₂|；

若|l₁|<|l₂|，则取(k₁,d₁)，否则取(k₂,d₂)作为所求直线的合理参数，记为记为(k^(m),d^(m))，m表示迭代次数；

步骤2.2.7：计算所有数据点到新直线y-k^(m)x-d^(m)＝0的加权距离之和f^(m)(k^(m),d^(m))；

式中m表示迭代次数，n表示数据点数；

步骤2.2.8：判别是否为最佳解；

若f^(m)(k^(m),d^(m))≥f^(m-1)(k^(m-1),d^(m-1))或f^(m)(k^(m),d^(m))≤10^-6，则输出解(k^(m-1),d^(m-1))，并简记为(k_x,d_x)；否则重复步骤2.2.2至步骤2.2.8；式中m表示迭代次数；

步骤2.3：计算t_n时刻目标在X方向上的位置P_xn、速度V_xn和方向k_xn：

P_xn＝k_xt_n+d_x，V_xn＝k_x，k_xn＝k_x；

所述步骤3包括如下步骤：

步骤3.1：对目标Y轴运动分量{(t_i,Y_i),i＝1,2,…n}采用步骤2.1的不加权直线参数估计模型粗略估计在Y方向上的目标运动状态方程y-k₁t-d₁＝0；

步骤3.2：采用步骤2.2的加权直线参数估计模型迭代估计目标在Y方向上的运动状态方程y-k_yt-d_y＝0；

步骤3.3：计算t_n时刻目标在Y方向上的位置P_yn、速度V_yn和方向k_yn：

P_yn＝k_yt_n+d_y，V_yn＝k_y，k_yn＝k_y。

Images