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

Abstract

本发明属于多传感器目标跟踪与数据融合技术领域。具体涉及在复杂数据环境中的针对多雷达直线航迹线的目标状态估计方法。该方法针对传统多雷达目标状态估计与航迹融合方法在复杂数据环境中的存在的问题，利用目标处于匀速直线运动状态的特点，分别对当前来自不同雷达的有限观测点的X、Y分量进行相对于的测量时间的垂直距离双加权估计，然后根据得到的分量直线航迹线模型参数确定最终的目标状态(位置、速度、航向)估计结果。该方法不但易于工程实现，通过与传统卡尔曼滤波算法在目标位置、航向和速度估值上进行对比实验表明，本发明取得了较好的估值效果，为在本系统中开展多雷达目标跟踪与数据融合工作提供了一种稳定、有效且易于工程实现的滤波方法。

Description

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

技术领域

本发明属于多传感器目标跟踪与数据融合技术领域。具体涉及在复杂数据环境中，一种针对多雷达直线航迹线的目标状态估计方法。

背景技术

多传感器目标跟踪是指利用多传感器(雷达、声纳、红外等)所获得的对运动目标(飞机、坦克、舰艇等)的时序离散观测数据，对目标的运动状态(位置、速度、航向等)进行估计和跟踪的方法。多雷达直线航迹线目标状态估计就是利用多部雷达对同一目标沿直线飞行时测量数据对目标位置、速度和航向进行实时估计。卡尔曼滤波是目标跟踪与数据融合应用中一种经典的最优线性无偏估计方法，当目标状态空间和测量空间均为线性高斯系统时，可以在干扰噪声背景中对目标运动状态进行最小方差估计和跟踪，因而得到了广泛的实际应用并成为目标跟踪领域的理论基础。

在多雷达组网探测与目标跟踪系统(以下简称“系统”)中，雷达跨代使用(测量精度差异较大)、部署分散且地域跨度大(产生坐标转换误差)，微波、短波、光缆、电缆等多种数据传输手段并存(产生传输误差)，探测环境易受电磁、气象、季节等因素影响(过程噪声协方差Q和量测噪声协方差R难以固定)，因此，在实际应用中，单雷达测量数据采样间隔较大，测量误差、坐标转换误差以及数据标定与传输误差同时存在；多雷达测量数据精度各不相同，在时间和空间上存在难以彻底消除的相对系统误差。

以上复杂的数据环境对采用传统卡尔曼滤波方法进行多雷达目标跟踪与数据融合造成了很大的困扰。

发明内容

(一)要解决的技术问题

本发明所要解决的技术问题是：如何提供一种针对多雷达直线航迹线的目标状态估计方法。

(二)技术方案

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

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

步骤1：通过统计方法计算多雷达目标跟踪与数据融合系统中各单雷达测量在统一直角坐标系中X、Y方向上的样本标准偏差σ_k'x和σ_k'y，k'表示雷达序号；

步骤2：将每部雷达针对某个处于匀速直线飞行期的目标的ds_k个观测点数据(ρ_k'j,θ_k'j,h_k'j,t_k'j)转换为统一直角坐标(X_k'j,Y_k'j,t_k'j)；其中，(ρ_k'j,θ_k'j,h_k'j,t_k'j)表示t_k'j时刻雷达k'测得的目标距离ρ_k'j、方位θ_k'j和高度h_k'j，k'＝1,2,…m'，j＝1,2,…ds_k'；m'为有效雷达数量；

步骤3：对以上n'个多雷达观测点{(X_k'j,Y_k'j,t_k'j,σ_k'x,σ_k'y)，k'＝1,2,…m'，j＝1,2,…ds_k'，

}，按照t_k'j升序进行排列，得到多雷达观测点序列{(X_I,Y_I,t_I,σ_xI,σ_yI)，I＝1,2,…n'}；

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

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

步骤6：则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中采用误差传递法替代统计方法后得到的目标真实位置、卡尔曼估值位置和本发明估值位置三者比较显示图。

具体实施方式

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

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

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

步骤1：通过统计方法计算多雷达目标跟踪与数据融合系统中各单雷达测量在统一直角坐标系中X、Y方向上的样本标准偏差σ_k'x和σ_k'y，k'表示雷达序号；

步骤2：将每部雷达针对某个处于匀速直线飞行期的目标的ds_k'个观测点数据(ρ_k'j,θ_k'j,h_k'j,t_k'j)转换为统一直角坐标(X_k'j,Y_k'j,t_k'j)；其中，(ρ_k'j,θ_k'j,h_k'j,t_k'j)表示t_k'j时刻雷达k'测得的目标距离ρ_k'j、方位θ_k'j和高度h_k'j，k'＝1,2,…m'，j＝1,2,…ds_k'；m'为同时段内对空中处于匀速直线飞行状态的同一目标实施有效探测的雷达数量；

步骤3：对以上n'个多雷达观测点{(X_k'j,Y_k'j,t_k'j,σ_k'x,σ_k'y)，k'＝1,2,…m'，j＝1,2,…ds_k'，

}，按照t_kj升序进行排列，得到多雷达观测点序列{(X_I,Y_I,t_I,σ_xI,σ_yI)，I＝1,2,…n'}；

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

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

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

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

步骤1.1：选取雷达k'在典型航路(目标作匀速直线运动)上的一组观测点{(ρ_k'i',θ_k'i',h_k'i',t_k'i')，i'＝1,2,…ds_k'}，转换为统一直角坐标{(X_i',Y_i',t_i')，i'＝1,2,…ds_k'}；坐标转换公式如下：

X_i'＝X_k'i'cosδ_xz-Y_k'i'sinδ_xz+X_k'x

Y_i'＝X_k'i'sinδ_xz+Y_k'i'cosδ_xz+Y_k'x

其中：ds_k'表示观测点数，一般ds_k'取15～30为宜；(X_k'x,Y_k'x)为雷达k'在中心统一直角坐标系中的坐标；δ_xz为雷达k'与直角坐标系中心点的经度差，单位为弧度；

步骤1.2：将X轴运动分量{(t_i',X_i'),i'＝1,2,…ds_k}简记为：{(x_i,y_i)，i＝1,2,…n}，采用内加权迭代直线参数估计模型估计在X方向上的目标运动状态方程y-k_x×x-d_x＝0，所述步骤1.2包括如下步骤：

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

对于(1)式应有f(k,d)对于k和d的偏导数等于零同时成立，求解步骤如下：

①计算a₁,a₂,b₁,b₂,c₀。

②计算a,b,c。

a＝c₀-a₁b₁

c＝a₁b₁-c₀

③解方程，计算所有解。

d₁＝b₁-a₁k₁

d₂＝b₁-a₁k₂

④按照距离最小原则确定方程的合理解。

(k₁,d₁)和(k₂,d₂)都是(1)式的实根，且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₁)。

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

n为观测点数。

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

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

步骤1.2.4：求|l_i|的倒数。

步骤1.2.5：求各点的内部权值v_i。

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

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

对于(2)式应有f(k,d)对于k和d的偏导数等于零同时成立，求解步骤如下：

①计算a'₀,a'₁,a'₂,b'₁,b'₂,c'₀。

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

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

c'＝c'₀+a'₁b'₁

③解方程，计算所有解。

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

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

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

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

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

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

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

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

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

若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)；否则重复步骤1.2.3至步骤1.2.9。

步骤1.3：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_x×x+d_x的平均距离vd。

步骤1.4：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_x×x+d_x的距离的标准偏差σ_x，即为雷达k'测量在X坐标上的样本标准偏差σ_k'x。

步骤1.5：将Y轴运动分量{(t_i',Y_i'),i'＝1,2,…ds_k'}简记为：{(x_i,y_i)，i＝1,2,…n}，采用内加权迭代直线参数估计模型估计在Y方向上的目标运动状态方程y-k_y×x-d_y＝0，过程与步骤1.2相同，得到(k_y,d_y)。

步骤1.6：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_y×x+d_y的平均距离vd。

步骤1.7：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_y×x+d_y的距离的标准偏差σ_y，即为雷达k'测量在Y坐标上的样本标准偏差σ_k'y。

其中，所述步骤2包括如下坐标转换公式：

X_k'j＝x_k'jcosδ_k'-y_k'jsinδ_k'+X_k'x，

Y_k'j＝x_k'jsinδ_k'+y_k'jcosδ_k'+Y_k'x。

其中：k'＝1,2,…m'，j＝1,2,…ds_k'。m'表示参与本次融合计算的有效雷达数量，ds_k'表示雷达k'的观测点数。(X_k'x,Y_k'x)为雷达k'在中心统一直角坐标系中的坐标。δ_k'为雷达k'与直角坐标系中心点的经度差，单位为弧度。θ_k'j取值范围[0,360)，单位为度，其中雷达正北方向为0度，正东方向为90度。t_k'j单位为秒。

其中，如果雷达k'是三坐标雷达，一般认为h_k'j保持不变；如果是两坐标雷达，则认为h_k'j＝0。

其中，所述步骤3中：

第一，关于n'的取值。n'为当前参与融合的多雷达观测点总数，n'≤N，N表示参与融合的多部雷达最近时间内有限个观测点数之和，N一般取6～18为宜。

第二，对于多雷达观测点序列{(X_I,Y_I,t_I,σ_xI,σ_yI)，i＝1,2,…n'}，要保证所有t_I>t_I-1成立。如果出现t_I＝t_I-1的情况，则只保留测量方差

较小的雷达观测点，相应的当前参与融合的多雷达观测点总数变为n'-1。

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

步骤4.1：对X轴运动分量{(t_I,X_I,σ_xI),I＝1,2,…n'}采用不加权直线参数估计模型粗略估计在X方向上的目标运动状态方程x-k₁t-d₁＝0，包括如下步骤：

步骤4.1.1：用所有点{(t_I,X_I,σ_xI),I＝1,2,…n'}(简记为：{(x_i,y_i,σ_i)，i＝1,2,…n})到某条直线的距离l_i的平方和最小作为条件构造直线，计算在此条件下的这条直线的最佳参数(k,d)，其中，k为该直线的斜率，d为该直线在x轴上的截距；

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

对以上方程组的求解步骤如下：

①计算a₁,a₂,b₁,b₂,c₀。

②计算a,b,c。

a＝c₀-a₁b₁

c＝a₁b₁-c₀

③解方程，计算所有解。

d₁＝b₁-a₁k₁

d₂＝b₁-a₁k₂

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

(k₁,d₁)和(k₂,d₂)都是方程(3)的实根，且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₁)。

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

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

n为观测点数。

步骤4.2.2：计算各点(x_i,y_i,σ_i)的外部权值w_i。

步骤4.2.3：求各点(x_i,y_i)到直线y-k^(m)x-d^(m)＝0的外部加权距离l_i。

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

步骤4.2.4：求|l_i|的倒数。

步骤4.2.5：求各点的内部权值v_i。

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

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

对于(4)式应有f(k,d)对于k和d的偏导数等于零同时成立，求解步骤如下：

①计算a'₀,a'₁,a'₂,b'₁,b'₂,c'₀。

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

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

c'＝c'₀+a'₁b'₁

③解方程，计算所有解。

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

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

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

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

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

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

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

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

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

若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)；否则重复步骤4.2.3至步骤4.2.9。

步骤4.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。

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

步骤5.1：对Y轴运动分量{(t_I,Y_I,σ_yI),I＝1,2,…n'}采用不加权直线参数估计模型粗略估计在Y方向上的目标运动状态方程y-k₁t-d₁＝0，具体过程与步骤类同4.1。

步骤5.2：采用双加权直线参数估计模型迭代估计目标在Y方向上的运动状态方程y-k_yt-d_y＝0，具体过程与步骤类同4.2。

步骤5.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：通过统计方法计算系统中各单雷达测量在统一直角坐标系中X、Y方向上的样本标准偏差σ_k'x和σ_k'y，k'表示雷达序号。

步骤1.1：选取雷达k'在典型航路(目标作匀速直线运动)上的一组观测点{(ρ_k'i',θ_k'i',h_k'i',t_k'i')，i'＝1,2,…ds_k'}，转换为统一直角坐标{(X_i',Y_i',t_i')，i'＝1,2,…ds_k'}。坐标转换公式如下：

X_i'＝X_k'i'cosδ_xz-Y_k'i'sinδ_xz+X_k'x

Y_i'＝X_k'i'sinδ_xz+Y_k'i'cosδ_xz+Y_k'x

相关数据见表1.1～表1.4，三部雷达均为两坐标雷达。

表1.1：雷达基本参数

表1.2：雷达1观测数据

表1.3：雷达2观测数据

表1.4：雷达3观测数据

步骤1.2：对每部雷达，将其X轴运动分量{(t_i',X_i'),i'＝1,2,…30}简记为：{(x_i,y_i)，i＝1,2,…30}，采用“内加权迭代直线参数估计模型”估计该雷达在X方向上测得的目标运动状态方程y-k_x×x-d_x＝0，包括如下步骤：

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

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

表1.5：参数计算结果1

参数	雷达1	雷达2	雷达3
				a<sub>1</sub>	145.000000	148.433333	151.600000
a<sub>2</sub>	28516.666667	29542.766667	30490.600000
				b<sub>1</sub>	1551.487562	1552.963802	1554.232807
b<sub>2</sub>	2407776.698879	2412350.803622	2416270.767187
				c<sub>0</sub>	227193.442138	232727.910253	237797.469408

②计算a,b,c。

a＝c₀-a₁b₁

c＝a₁b₁-c₀

表1.6：参数计算结果2

参数	雷达1	雷达2	雷达3
				a	2227.745708	2216.316565	2175.775838
b	6828.621544	6856.079159	6876.891749
				c	-2227.745708	-2216.316565	-2175.775838

③解方程，计算所有解。

d₁＝b₁-a₁k₁

d₂＝b₁-a₁k₂

表1.7：方程所有解

参数	雷达1	雷达2	雷达3
				k<sub>1</sub>	0.297385	0.295110	0.289815
k<sub>2</sub>	-3.362646	-3.388567	-3.450477
				d<sub>1</sub>	1508.366753	1509.159637	1510.296851
d<sub>2</sub>	2039.071183	2055.940045	2077.325104

④按照距离最小原则确定方程的合理解。

(k₁,d₁)和(k₂,d₂)都是(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₁)。

表1.8：距离最小原则确定方程的合理解

参数	雷达1	雷达2	雷达3
				l<sub>1</sub>	0.006390	0.544899	0.488216
l<sub>2</sub>	151.274020	150.430595	153.815191
				k<sub>1</sub>	0.297385	0.295110	0.289815
d<sub>1</sub>	1508.366753	1509.159637	1510.296851

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

表1.9：距离之和

参数	雷达1	雷达2	雷达3
				f<sup>(1)</sup>	17.836936	9.935514	18.545335

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

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

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

步骤1.2.5：求各点的内部权值v_i。计算结果见表1.10。

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

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

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

①计算a'₀,a'₁,a'₂,b'₁,b'₂,c'₀(n＝30)。计算结果见表1.11。

②计算a',b',c'。计算结果见表1.11。

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

参数	雷达1	雷达2	雷达3
				a’<sub>0</sub>	0.323462	0.264647	0.047912
a’<sub>1</sub>	19.479072	208.509544	114.848703
				a’<sub>2</sub>	5219.853507	45229.467347	21979.363904
b’<sub>1</sub>	1514.162023	1570.698063	1543.543537
				b’<sub>2</sub>	2293114.217465	2467245.036155	2383266.432128
c’<sub>0</sub>	-30933.111271	-328022.832754	-179823.675443
				a’	1438.639953	517.296025	2549.702457
b’	4412.834363	1600.607910	8049.357185
				c’	-1438.639953	-517.296025	-2549.702457

③解方程，计算所有解。

表1.12:X方向直线参数解第m次计算结果

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

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

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

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

表1.13：距离最小原则确定方程的合理解

参数	雷达1	雷达2	雷达3
				l<sub>1</sub>	0.000813	0.528557	0.421091
l<sub>2</sub>	20.320979	213.069709	115.541072
				k<sub>1</sub>	0.297214	0.295052	0.290101
d<sub>1</sub>	1508.372571	1509.176916	1510.225855

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

式中m表示迭代次数，n＝30，表示数据点数。

表1.14：内加权距离之和

参数	雷达1	雷达2	雷达3
				f<sup>(m)</sup>	0.095104	0.052524	0.397449

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

若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)；否则重复步骤1.2.3至步骤1.2.9。

经计算，雷达1：m＝2、执行步骤1.2.7时，|l₁|+|l₂|＝983.357377，大于上一次迭代时的Lmin＝20.321792，因此输出解(k⁽²⁾,d⁽²⁾)，并简记为(k_x,d_x)＝(0.297214，1508.372571)。雷达2：m＝2、执行步骤1.2.7时，|l₁|+|l₂|＝980.443637，大于上一次迭代时的Lmin＝213.598266，因此输出解(k⁽²⁾,d⁽²⁾)，并简记为(k_x,d_x)＝(0.295052，1509.176916)。雷达3：m＝2、执行步骤1.2.7时，|l₁|+|l₂|＝1564.190714，大于上一次迭代时的Lmin＝115.962163，因此输出解(k⁽²⁾,d⁽²⁾)，并简记为(k_x,d_x)＝(0.290101，1510.225855)。于是得到三部雷达关于X轴测量的直线方程的最优解。

表1.15：Y轴直线方程的最优解

参数	雷达1	雷达2	雷达3
				k<sub>x</sub>	0.297214	0.295052	0.290101
d<sub>x</sub>	1508.372571	1509.176916	1510.225855

步骤1.3：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_x×x+d_x的平均距离vd。计算结果见表1.16。

步骤1.4：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_x×x+d_x的距离的标准偏差σ_x，即为雷达k'对X坐标测量的样本标准偏差σ_k'x。计算结果见表1.16。

表1.16：均值、方差和标准差计算结果

步骤1.5：将Y轴运动分量{(t_i',Y_i'),i'＝1,2,…ds_k}简记为：{(x_i,y_i)，i＝1,2,…n}，采用“内加权迭代直线参数估计模型”估计在Y方向上的目标运动状态方程y-k_y×x-d_y＝0，过程与步骤1.2相同，得到(k_y,d_y)。计算结果见表1.17。

表1.17：Y轴直线方程的最优解

参数	雷达1	雷达2	雷达3
				k<sub>y</sub>	0.293111	0.290900	0.295446
d<sub>y</sub>	2289.648328	2289.710955	2289.651876

步骤1.6：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_y×x+d_y的平均距离vd。计算结果见表1.18。

步骤1.7：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_y×x+d_y的距离的标准偏差σ_y，即为雷达k对Y坐标测量的样本标准偏差σ_k'y。计算结果见表1.18。

表1.18：均值、方差和标准差计算结果

步骤2：将每部雷达针对某个处于匀速直线飞行期的目标的ds_k'个观测点数据(ρ_k'j,θ_k'j,h_k'j,t_k'j)(表示t_k'j时刻雷达k'测得的目标距离ρ_k'j、方位θ_k'j和高度h_k'j，k＝1,2,…m'，j＝1,2,…ds_k')转换为统一直角坐标(X_k'j,Y_k'j,t_k'j)。k'＝1,2,…m'，j＝1,2,…ds_k'；m'为有效雷达数量；

包括如下坐标转换公式：

X_k'j＝x_k'jcosδ_k'-y_k'jsinδ_k'+X_k'x，

Y_k'j＝x_k'jsinδ_k'+y_k'jcosδ_k'+Y_k'x。

其中：m'＝3，表示参与本次融合计算的有效雷达数量，ds_k'表示雷达k'的观测点数，此处分别取值为ds₁＝3，ds₂＝2，ds₃＝2。(X_k'x,Y_k'x)为雷达k'在中心统一直角坐标系中的坐标。δ_k'为雷达k'与直角坐标系中心点的经度差(单位为弧度)。因为是两坐标雷达，认为h_k'j＝0。θ_kj取值范围[0,360)，单位为度，其中雷达正北方向为0度，正东方向为90度。t_k'j单位为秒。雷达基本参数见表1.1。各雷达观测点数据以及经转换后的坐标见表1.19。

表1.19：各雷达观测点及坐标转换数据

步骤3：对以上7个多雷达观测点{(X_k'j,Y_k'j,t_k'j,σ_k'x,σ_k'y)，k'＝1,2,…3，j＝1,2,…ds_k'，

}，按照t_kj升序进行排列，得到多雷达观测点序列{(X_i,Y_i,t_i,σ_xi,σ_yi)，i＝1,2,…7}。排序结果见表1.20，其中标准偏差σ_x和σ_y来自步骤1的计算结果σ_k'x和σ_k'y。

表1.20：多雷达观测点坐标排序结果

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

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

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

②计算a,b,c。

a＝c₀-a₁b₁＝7.869102，

c＝a₁b₁-c₀＝-7.869102。

③解方程，计算所有解。

d₁＝b₁-a₁k₁＝1500.169325，

d₂＝b₁-a₁k₂＝1655.927711。

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

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

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

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

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

步骤4.2.2：计算各点(x_i,y_i,σ_i)的外部权值w_i。计算结果见表1.21。

步骤4.2.3：求各点(x_i,y_i)到直线y-k^(m)x-d^(m)＝0的外部加权距离l_i。

式中m表示迭代次数，数据点数为7。m初始值为1，即：

k⁽¹⁾＝k₁，d⁽¹⁾＝d₁。计算结果见表1.21。

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

步骤4.2.5：求各点的内部权值v_i。计算结果见表1.21。

表1.21:外部权值、距离和内部权值计算结果

序号	σ<sub>i</sub>	w<sub>i</sub>	l<sub>i</sub>	P<sub>i</sub>	v<sub>i</sub>
						1	0.454973	0.091367	-1340.346857	0.000746	0.133594
2	0.263616	0.272156	-1074.465485	0.000931	0.166652
						3	0.456411	0.090793	-1342.35603	0.000745	0.133394
4	0.454973	0.091367	-1342.124501	0.000745	0.133417
						5	0.263616	0.272156	-1075.692258	0.00093	0.166462
6	0.456411	0.090793	-1343.902508	0.000744	0.13324
						7	0.454973	0.091367	-1343.888311	0.000744	0.133242

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

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

①计算a'₀,a'₁,a'₂,b'₁,b'₂,c'₀。

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

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

c'＝c'₀+a'₁b'₁＝-7.566298。

③解方程，计算所有解。

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

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

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

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

因|l₁|+|l₂|＝10.120469<Lmin(初值为10⁶)，则Lmin＝10.120469。m值加1，若|l₁|＜|l₂|，则取(k₁,d₁)，记为(k^(m),d^(m))＝(0.204509，1499.913651)，m表示迭代次数。

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

式中m表示迭代次数，数据点数为7。

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

若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)；否则重复步骤4.2.3至步骤4.2.9。

经计算，m＝2、执行步骤4.2.7时，|l₁|+|l₂|＝792.040028，大于上一次迭代时的Lmin＝10.120469，因此输出解(k⁽²⁾,d⁽²⁾)，并简记为(k_x,d_x)＝(0.204509，1499.913651)。

步骤4.3：计算t_n'＝40秒时，目标在X方向上的位置P_xn'、速度V_xn'和方向k_xn'：

P_xn'＝k_xt_n'+d_x＝0.204509×40+1499.913651＝1508.093995，

V_xn'＝k_x＝0.204509，

k_xn'＝k_x＝0.204509。

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

步骤5.1：对Y轴运动分量{(t_i,Y_i,σ_yi),i＝1,2,…n}采用“不加权直线参数估计模型”粗略估计在Y方向上的目标运动状态方程y-k₁t-d₁＝0，具体过程与步骤类同4.1。经计算(k₁,d₁)＝(0.275546，2299.267965)。

步骤5.2：采用“双加权直线参数估计模型”迭代估计目标在Y方向上的运动状态方程y-k_yt-d_y＝0，具体过程与步骤类同4.2。经计算(k_y,d_y)＝(0.271974，2299.432554)。

步骤5.3：计算t_n'＝40秒时，目标在Y方向上的位置P_yn'、速度V_yn'和方向k_yn'：

P_yn'＝k_yt_n'+d_y＝0.271974×40+2299.432554＝2310.311511，

V_yn'＝k_y＝0.271974，

k_yn'＝k_y＝0.271974。

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

(P_xn',P_yn')＝(1508.093995，2310.311511)公里；

实施例2

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

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

步骤1：通过统计方法计算系统中各单雷达测量在统一直角坐标系中X、Y方向上的样本标准偏差σ_k'x和σ_k'y，k'表示雷达序号。该过程中使用的统计数据、方法步骤、计算结果与实施例1的步骤1完全相同，计算结果见表2.1。

表2.1：各雷达标准差计算结果

参数	雷达1	雷达2	雷达3
				σ<sub>k'x</sub>	0.454973	0.263616	0.456411
σ<sub>k'y</sub>	0.139602	0.445306	0.173762

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

按照表2.2中的模拟目标参数值，仿真三部雷达观测该匀速直线飞行状态下目标各20个位置点的真实值和测量值，结果见表2.3～2.5。

表2.2：基本参数

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

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

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

步骤3：将每部雷达观测点数据(ρ_k'j,θ_k'j,h_k'j,t_k'j)(k'＝1,2,3，j＝1,2,…20)转换为统一直角坐标(X_k'j,Y_k'j,t_k'j)。为了便于比较，同时将各时刻目标点真实位置由极坐标转换为中心统一直角坐标。

采用如下坐标转换公式：

X_k'j＝x_k'jcosδ_k'-y_k'jsinδ_k'+X_k'x，

Y_k'j＝x_k'jsinδ_k'+y_k'jcosδ_k'+Y_k'x。

坐标转换结果见表2.6～2.8。各时刻目标点真实位置和雷达测量位置在中心统一直角坐标系中的显示如图2所示。

表2.6：雷达1测量目标点的中心统一直角坐标

表2.7：雷达2测量目标点的中心统一直角坐标

表2.8：雷达3测量目标点的中心统一直角坐标

步骤4：对以上三部雷达总计60个观测点{(X_k'j,Y_k'j,t_k'j,σ_k'x,σ_k'y)，k'＝1,2,…3，j＝1,2,…ds_k'，

}，按照t_k'j升序进行排列，得到多雷达观测点序列{(X_i,Y_i,t_i,σ_xi,σ_yi)，i＝1,2,…60}。观测点排序结果以及n＝7时参与估值计算的点序号见表2.9，其中标准偏差σ_x和σ_y来自步骤1的计算结果σ_kx和σ_ky。

表2.9：多雷达观测点排序以及n＝7时参与融合的点序号

步骤5：针对表2.9数据，按照本发明步骤4，对目标X轴运动分量逐点使用“双加权直线参数估计模型”进行迭代估计。n取值为7，即每次取点最多7个，{(t_i,X_i,σ_xi),i＝1,2,…7}，得到t₁～t₆₀时刻目标在X方向上的位置P_xj、速度V_xj和方向k_xj，j＝1,2,…60。t₁时刻目标位置分量值为测量值，速度和航向分量估值为0。

针对表2.9数据，按照本发明步骤5，对目标Y轴运动分量逐点使用“双加权直线参数估计模型”进行迭代估计。n取值为7，即每次取点最多7个，{(t_i,Y_i,σ_yi),i＝1,2,…7}，得到t₁～t₆₀时刻目标在Y方向上的位置P_yj、速度V_yj和方向k_yj，j＝1,2,…60。t₁时刻目标位置分量值为测量值，速度和航向分量估值为0。

对目标X、Y轴运动分量{(P_xj，P_yj，V_xj，V_yj，k_xj，k_yj)，j＝1,2,…60}按照本发明步骤6方法计算每个时刻的目标速度V_j和航向K_j。于是得到t_j时刻按照本发明方法估计的目标位置(wP_xj,wP_yj)＝(P_xj,P_yj)，速度

和航向

计算结果见表2.10。

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

步骤6：对表2.9中的测量值{(t_i,X_i,Y_j),i＝1,2,…60}使用传统卡尔曼滤波方法逐点进行目标位置(kP_xi,kP_yi)、速度kV_i和航向kK_i估计。其中：Q、R参数取值见表2.2。t₁时刻目标位置估值取测量值，速度和航向估值为0。表2.11是应用卡尔曼滤波方法的逐点估计结果。

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

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

步骤7：时间配准点比较

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

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

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

图5和图6是本发明方法和卡尔曼方法时间配准点间距与平均时间配准点间距的对比图。经过统计，1分钟内有13个点的时间配准点间距wd_i≤kd_i，占1分钟内总点数的72.22％；全程有37个点的时间配准点间距wd_i≤kd_i，占全部点数的61.67％；所有时刻的平均时间配准点间距均有wvd_i≤kvd_i；1分钟内平均时间配准点间距wvd₁₈占kvd₁₈的79.23％。于是可得到以下结论：在本次试验中，以目标真实位置为基准，在测量初期、点数较少时就能获得较为准确的目标位置估值；使用本发明方法得到的目标位置点较卡尔曼方法有一定优势。

步骤6：速度估值比较

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

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

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

步骤7：航向估值比较

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

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

图10、图11和图12是本发明方法和卡尔曼方法分别在航向、航向差和平均航向差的对比图。经过统计，1分钟内有10个点的航向差wdk_i≤kdk_i，1分钟内总点数的55.56％；全程有36个点的航向差wdk_i≤kdk_i，占全部点数的60.00％；所有时刻的平均航向差均有wvdk_i≤kvdk_i；1分钟内平均航向差wvdk₁₈占kvdk₁₈的73.92％。图上可以看出，本发明得到的航向估值比较准确、稳定，与卡尔曼方法相比，有一定优势。

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

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

(1)在本发明步骤1中给出了通过统计方法计算系统中各单雷达观测点在直角坐标系中X、Y方向上的样本标准偏差σ_k'x和σ_k'y的方法，其中统计数据来自日常积累，而对于不同样本数据可能获得不同的标准差，需要按区域进行聚类、平均处理。经过大量统计和平均，最终可获得与雷达探测距离ρ和探测方位θ有关的标准差表。这一统计、积累过程比较复杂，可能会影响目标状态估计的准确性，因此，我们给出另外一种观测点在X、Y方向上标准偏差的估计方法，称为误差传递计算法。

在实际应用中，如果雷达经过校准且状态稳定，可以通过雷达出厂或校准后给定的距离、方位测量标准差，采用误差传递定律一般式，计算得到每个观测点的X、Y方向上标准偏差，此法我们称之为误差传递法。具体步骤如下：

雷达对目标的一个位置观测点(距离，方位)用极坐标(ρ,θ)表示，距离ρ的标准差为σ_ρ，方位θ的标准差为σ_θ，在忽略测量高度影响的情况下，间接测量x＝ρsinθ和y＝ρcosθ的标准差σ_x和σ_y按照误差传递定律一般式表示为：

对于系统中所有单雷达的观测点(ρ_k'j,θ_k'j,h_k'j,t_k'j)，在执行步骤2进行坐标转换后，按照上式计算该点的X、Y方向上标准偏差σ_x和σ_y。图13就是实施例2中，按照表2.2给定的雷达测距随机误差和测向随机误差，采用误差传递法替代统计方法计算标准偏差σ_x和σ_y后得到目标位置比较图，估值效果较为理想。

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

另外，当参与状态估值的雷达较多，n取值较小时(6～9个)，目标在短时间(20～30秒)内可以看成匀速直线运动，所以，此时本发明也可以用于机动性不大的非线性运动目标的状态估计与跟踪。

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

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

Claims (7)

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

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

步骤1：通过统计方法计算多雷达目标跟踪与数据融合系统中各单雷达测量在中心统一直角坐标系中X、Y方向上的样本标准偏差σ_k'x和σ_k'y，k'表示雷达序号；

步骤2：将每部雷达针对某个处于匀速直线飞行期的目标的ds_k'个观测点数据(ρ_k'j,θ_k'j,h_k'j,t_k'j)转换为统一直角坐标(X_k'j,Y_k'j,t_k'j)；其中，(ρ_k'j,θ_k'j,h_k'j,t_k'j)表示t_k'j时刻雷达k'测得的目标距离ρ_k'j、方位θ_k'j和高度h_k'j，k'＝1,2,…m'，j＝1,2,…ds_k'；m'为同时段内对空中处于匀速直线飞行状态的同一目标实施有效探测的雷达数量；

步骤3：对以上n'个多雷达观测点

按照t_k'j升序进行排列，得到多雷达观测点序列{(X_I,Y_I,t_I,σ_xI,σ_yI)，I＝1,2,…n'}；

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

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

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

2.如权利要求1所述的针对多雷达直线航迹线的目标状态估计方法，其特征在于，所述步骤1包括如下步骤：

步骤1.1：选取雷达k'在典型航路上的一组观测点{(ρ_k'i',θ_k'i',h_k'i',t_k'i')，i'＝1,2,…ds_k'}，转换为统一直角坐标{(X_i',Y_i',t_i')，i'＝1,2,…ds_k'}；坐标转换公式如下：

X_i'＝X_k'i'cosδ_xz-Y_k'i'sinδ_xz+X_k'x

Y_i'＝X_k'i'sinδ_xz+Y_k'i'cosδ_xz+Y_k'x

其中：ds_k'表示观测点数，ds_k'取值15～30；(X_k'x,Y_k'x)为雷达k'在中心统一直角坐标系中的坐标；δ_xz为雷达k'与直角坐标系中心点的经度差，单位为弧度；

步骤1.2：将X轴运动分量{(t_i',X_i'),i'＝1,2,…ds_k}简记为：{(x_i,y_i)，i＝1,2,…n}，采用内加权迭代直线参数估计模型估计在X方向上的目标运动状态方程y-k_x×x-d_x＝0；所述步骤1.2包括如下步骤：

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

对于(1)式应有f(k,d)对于k和d的偏导数等于零同时成立，求解步骤如下：

①计算a₁,a₂,b₁,b₂,c₀；

②计算a,b,c；

a＝c₀-a₁b₁

c＝a₁b₁-c₀

③解方程，计算所有解；

d₁＝b₁-a₁k₁

d₂＝b₁-a₁k₂

④按照距离最小原则确定方程的合理解；

(k₁,d₁)和(k₂,d₂)都是(1)式的实根，且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₁)；

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

n为观测点数；

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

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

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

步骤1.2.5：求各点的内部权值v_i；

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

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

对于(2)式应有f(k,d)对于k和d的偏导数等于零同时成立，求解步骤如下：

①计算a'₀,a'₁,a'₂,b'₁,b'₂,c'₀；

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

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

c'＝c'₀+a'₁b'₁

③解方程，计算所有解；

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

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

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

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

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

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

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

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

步骤1.2.9：判别是否为“最佳”解；

若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)；否则重复步骤1.2.3至步骤1.2.9；

步骤1.3：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_x×x+d_x的平均距离vd；

步骤1.4：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_x×x+d_x的距离的标准偏差σ_x，即为雷达k'在测量中在X坐标上的样本标准偏差σ_k'x；

步骤1.5：将Y轴运动分量{(t_i',Y_i'),i'＝1,2,…ds_k'}简记为：{(x_i,y_i)，i＝1,2,…n}，采用内加权迭代直线参数估计模型估计在Y方向上的目标运动状态方程y-k_y×x-d_y＝0，过程与步骤1.2相同，得到(k_y,d_y)；

步骤1.6：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_y×x+d_y的平均距离vd；

步骤1.7：计算样本点{(x_i,y_i)，i＝1,2,…n}到直线y＝k_y×x+d_y的距离的标准偏差σ_y，即为雷达k'在测量中在Y坐标上的样本标准偏差σ_k'y；

3.如权利要求2所述的针对多雷达直线航迹线的目标状态估计方法，其特征在于，所述步骤2包括如下坐标转换公式：

X_k'j＝x_k'jcosδ_k'-y_k'jsinδ_k'+X_k'x，

Y_k'j＝x_k'jsinδ_k'+y_k'jcosδ_k'+Y_k'x；

其中：k'＝1,2,…m'，j＝1,2,…ds_k'；m'表示参与本次融合计算的有效雷达数量，ds_k'表示雷达k'的观测点数；(X_k'x,Y_k'x)为雷达k'在中心统一直角坐标系中的坐标；δ_k'为雷达k'与直角坐标系中心点的经度差，单位为弧度；θ_k'j取值范围[0,360)，单位为度，其中雷达正北方向为0度，正东方向为90度；t_k'j单位为秒。

4.如权利要求3所述的针对多雷达直线航迹线的目标状态估计方法，其特征在于，如果雷达k'是三坐标雷达，认为h_k'j保持不变；如果是两坐标雷达，则认为h_k'j＝0。

5.如权利要求3所述的针对多雷达直线航迹线的目标状态估计方法，其特征在于，所述步骤3中：

第一，关于n'的取值；n'为当前参与融合的多雷达观测点总数，n'≤N，N表示参与融合的多部雷达最近时间内有限个观测点数之和，N取6～18；

第二，对于多雷达观测点序列{(X_I,Y_I,t_I,σ_xI,σ_yI)，i＝1,2,…n'}，要保证所有t_I>t_I-1成立；如果出现t_I＝t_I-1的情况，则只保留测量方差

较小的雷达观测点，相应的当前参与融合的多雷达观测点总数变为n'-1。

6.如权利要求1所述的针对多雷达直线航迹线的目标状态估计方法，其特征在于，所述步骤4包括如下步骤：

步骤4.1：对X轴运动分量{(t_I,X_I,σ_xI),I＝1,2,…n'}采用不加权直线参数估计模型粗略估计在X方向上的目标运动状态方程x-k₁t-d₁＝0，包括如下步骤：

步骤4.1.1：用所有点{(t_I,X_I,σ_xI),I＝1,2,…n'}，简记为：{(x_i,y_i,σ_i)，i＝1,2,…n}，到某条直线的距离l_i的平方和最小作为条件构造直线，计算在此条件下的这条直线的最佳参数(k,d)，其中，k为该直线的斜率，d为该直线在x轴上的截距；不加权直线参数估计模型如下：

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

对以上方程组的求解步骤如下：

①计算a₁,a₂,b₁,b₂,c₀；

②计算a,b,c；

a＝c₀-a₁b₁

c＝a₁b₁-c₀

③解方程，计算所有解；

d₁＝b₁-a₁k₁

d₂＝b₁-a₁k₂

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

(k₁,d₁)和(k₂,d₂)都是方程的实根，且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₁)；

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

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

n为观测点数；

步骤4.2.2：计算各点(x_i,y_i,σ_i)的外部权值w_i；

步骤4.2.3：求各点(x_i,y_i)到直线y-k^(m)x-d^(m)＝0的外部加权距离l_i；

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

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

步骤4.2.5：求各点的内部权值v_i；

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

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

对于(4)式应有f(k,d)对于k和d的偏导数等于零同时成立，求解步骤如下：

①计算a'₀,a'₁,a'₂,b'₁,b'₂,c'₀；

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

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

c'＝c'₀+a'₁b'₁

③解方程，计算所有解；

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

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

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

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

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

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

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

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

步骤4.2.9：判别是否为“最佳”解；

若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)；否则重复步骤4.2.3至步骤4.2.9；

步骤4.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。

7.如权利要求6所述的针对多雷达直线航迹线的目标状态估计方法，其特征在于，所述步骤5包括如下步骤：

步骤5.1：对Y轴运动分量{(t_I,Y_I,σ_yI),I＝1,2,…n'}采用不加权直线参数估计模型粗略估计在Y方向上的目标运动状态方程y-k₁t-d₁＝0，具体过程类同步骤4.1；

步骤5.2：采用双加权直线参数估计模型迭代估计目标在Y方向上的运动状态方程y-k_yt-d_y＝0，具体过程类同步骤4.2；

步骤5.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