CN109856622A - 一种约束条件下的单雷达直线航迹线目标状态估计方法 - Google Patents
一种约束条件下的单雷达直线航迹线目标状态估计方法 Download PDFInfo
- Publication number
- CN109856622A CN109856622A CN201910004684.5A CN201910004684A CN109856622A CN 109856622 A CN109856622 A CN 109856622A CN 201910004684 A CN201910004684 A CN 201910004684A CN 109856622 A CN109856622 A CN 109856622A
- Authority
- CN
- China
- Prior art keywords
- target
- formula
- point
- distance
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明属于传感器目标跟踪与数据融合技术领域,具体涉及一种约束条件下的单雷达直线航迹线目标状态估计方法。该方法针对目标由匀速圆周运动转变为匀速直线运动后单雷达直线航迹线,将目标在匀速圆周运动模型中的先验信息作为当前匀速直线运动模型的约束条件,在统一直角坐标系中对当前有限个测量点进行垂直距离加权估计得到与时间无关的目标运动方程和航向估值,通过目标时间‑路程加权匀速估计模型得到线速度和分量速度估值,进而最终确定目标状态位置估计结果。对比试验表明,该方法不但易于工程实现,而且比传统卡尔曼滤波方法具有更优的目标状态估计效果和收敛速度。
Description
技术领域
本发明属于传感器目标跟踪与数据融合技术领域,具体涉及在复杂数据环境中,且系统噪声和观测噪声均未知的情况下,针对目标由匀速圆周运动转变为匀速直线运动的一种约束条件下的单雷达直线航迹线目标状态估计方法。
背景技术
目标跟踪是指利用传感器所获得的对运动目标的时序离散观测数据,对目标的运动状态(位置、航向、速度等)进行估计和跟踪的方法。卡尔曼滤波方法是现阶段雷达中最常用的、占据主导地位的目标跟踪算法。它能够针对不同运动模型,通过累积测量对目标运动状态进行最小方差估计和跟踪,具有较好的跟踪精度,但通常收敛速度较慢,从开始滤波器建立到稳定跟踪一般所用时间为将近10个采样周期。而在通常情况下,当目标处于匀速直线飞行状态时,采用适合线性运动模型的标准卡尔曼滤波方法(KF);当目标沿圆弧航迹线匀速飞行时,采用适合非线性运动模型的扩展卡尔曼滤波方法(EKF)。在每次针对不同运动模型的卡尔曼滤波器建立初期(一分钟以内),都是目标运动状态的不稳定跟踪期,而对于飞机等高速、高机动目标,滤波收敛快慢直接影响到目标跟踪的稳定度和对目标的识别速度。
另外,在多雷达组网探测与目标跟踪系统(以下简称“系统”)中还存在实际系统数据环境复杂(测量数据采样间隔较大、测量误差、坐标转换误差以及数据标定与传输误差同时存在)与卡尔曼滤波方法应用条件比较苛刻的矛盾;卡尔曼滤波方法应用中过程噪声协方差Q和量测噪声协方差R在参数值选取上存在困难,不能兼顾稳态性和瞬时性的问题;系统测量中不可避免的野值出现,导致卡尔曼滤波发散或目标状态估计在短时间内失效,表现为估值航迹线出现较大锯齿、目标航向或速度估计出现突变等。
发明内容
(一)要解决的技术问题
本发明要解决的技术问题是:如何提供一种约束条件下的单雷达直线航迹线目标状态估计方法。
(二)技术方案
为解决上述技术问题,本发明提供一种约束条件下的单雷达直线航迹线目标状态估计方法,所述估计方法应用于单雷达数据跟踪与多雷达数据融合系统的前期数据预处理过程中;所述估计方法包括如下步骤:
步骤1:将某雷达针对某个由匀速圆周运动转变为匀速直线运动后直线段上目标的n个观测点数据(ρi,θi,hi,ti)转换为统一直角坐标(xi,yi,ti);其中,(ρi,θi,hi,ti)表示ti时刻该雷达测得的目标距离ρi、方位θi和高度hi,i=1,2,…n;
步骤2:对n个观测点数据{(xi,yi),i=1,2,…n}使用约束条件下的不加权直线参数估计模型粗略估计目标运动状态方程y-k1x-d1=0;
步骤3:将方程y-k1x-d1=0作为初始状态,对n个观测点数据{(xi,yi),i=1,2,…n}使用约束条件下的加权直线参数估计模型进行迭代估计,得到最佳目标运动状态方程y-kx-d=0,并使用取点定向法计算出tn时刻的目标航向Kn;
步骤4:使用时间-路程加权匀速估计模型迭代估计目标最佳线速度方程S-Vt-S0=0,得到tn时刻的目标速度Vn并计算出目标在X、Y方向上的分量速度vxn和vyn:
步骤5:计算n个观测点数据{(xi,yi,ti),i=1,2,…n}到直线y-kx-d=0的垂足点的X轴坐标均值和时间均值并令则tn时刻目标在统一直角坐标系中的位置估值(Pxn,Pyn)为:
由此得到目标由匀速圆周运动转变为匀速直线运动后,在tn时刻的位置(Pxn,Pyn)、速度Vn和航向Kn。
(三)有益效果
与现有技术相比较,本发明提供一种适应多雷达组网探测与目标跟踪系统复杂数据环境,针对目标由匀速圆周运动转变为匀速直线运动后单雷达直线航迹线的目标状态估计方法。该方法将目标在匀速圆周运动模型中的先验信息作为当前匀速直线运动模型的约束条件,在统一直角坐标系中对当前有限个测量点进行垂直距离加权估计得到与时间无关的目标航向估值,通过目标时间-路程加权匀速估计得到线速度和分量速度估值,进而最终确定目标状态位置估计结果。对比试验表明,该方法不但易于工程实现,而且比传统卡尔曼滤波方法具有更优的目标状态估计效果和收敛速度。
附图说明
图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中含有野值时使用卡尔曼方法和使用本发明方法得到的平均航向差随测量点数变化情况显示图;
图24为本发明技术方案中用到的取点定向法实现流程图。
具体实施方式
为使本发明的目的、内容和优点更加清楚,下面结合附图和实施例,对本发明的具体实施方式作进一步详细描述。
针对以上现有技术的问题,本发明通过对目标由匀速圆周运动转变为匀速直线运动初期时运动规律的研究,将目标在匀速圆周运动模型中的先验信息作为当前匀速直线运动模型的约束条件,最终目的就是基于系统复杂数据环境,在无须获取系统噪声和观测噪声等先验知识的情况下,有效提高约束条件下的单雷达直线航迹线目标状态估计精度和收敛速度。通过与传统卡尔曼滤波算法在目标位置、航向和速度估值上进行对比实验表明,本发明取得了更好的估值效果和跟踪收敛速度,为在本系统中开展目标跟踪与数据融合工作提供了一种稳定、有效且易于工程实现的滤波方法。
具体而言,为解决现有技术的问题,本发明提供一种约束条件下的单雷达直线航迹线目标状态估计方法,如图1所示,所述估计方法应用于单雷达数据跟踪与多雷达数据融合系统的前期数据预处理过程中;所述估计方法包括如下步骤:
步骤1:将某雷达针对某个由匀速圆周运动转变为匀速直线运动后直线段上目标的n个观测点数据(ρi,θi,hi,ti)转换为统一直角坐标(xi,yi,ti);其中,(ρi,θi,hi,ti)表示ti时刻该雷达测得的目标距离ρi、方位θi和高度hi,i=1,2,…n;此处要保证n个观测点数据均处于目标匀速直线运动期间;
步骤2:对n个观测点数据{(xi,yi),i=1,2,…n}使用约束条件下的不加权直线参数估计模型粗略估计目标运动状态方程y-k1x-d1=0;
步骤3:将方程y-k1x-d1=0作为初始状态,对n个观测点数据{(xi,yi),i=1,2,…n}使用约束条件下的加权直线参数估计模型进行迭代估计,得到最佳目标运动状态方程y-kx-d=0,并使用取点定向法计算出tn时刻的目标航向Kn;
步骤4:使用时间-路程加权匀速估计模型迭代估计目标最佳线速度方程S-Vt-S0=0,得到tn时刻的目标速度Vn并计算出目标在X、Y方向上的分量速度vxn和vyn:
步骤5:计算n个观测点数据{(xi,yi,ti),i=1,2,…n}到直线y-kx-d=0的垂足点的X轴坐标均值和时间均值并令则tn时刻目标在统一直角坐标系中的位置估值(Pxn,Pyn)为:
由此得到目标由匀速圆周运动转变为匀速直线运动后,在tn时刻的位置(Pxn,Pyn)、速度Vn和航向Kn。
其中,所述步骤1包括如下步骤:
步骤1.1:选择某雷达观测数据集{(ρi,θi,hi,ti),i=1,2,…n},是针对某个由匀速圆周运动转变为匀速直线运动后直线段上目标的时序观测数据;如果是三坐标雷达观测数据,一般认为hi保持不变;如果是两坐标雷达观测数据,则认为hi=0。θi取值范围[0,360),单位为度,其中雷达正北方向为0度,正东方向为90度。
步骤1.2:将雷达观测数据(ρi,θi,hi,ti),i=1,2,…n转换为以本站为中心的二维直角坐标(Xzi,Yzi):
步骤1.3:将(Xzi,Yzi),i=1,2,…n转换为中心统一直角坐标(Xi,Yi):
Xi=Xzicosδxz-Yzisinδxz+Xzx
Yi=Xzisinδxz+Yzicosδxz+Yzx
其中:(Xzx,Yzx)为雷达站址在中心统一直角坐标系中的坐标;δxz为雷达站址与直角坐标系中心点的经度差,单位为弧度。
其中,所述步骤1.1中,ti<ti+1,且n不大于N,N表示参与目标状态估计的最近时间内有限个观测数据点的个数,N取4~15。
其中,所述步骤2包括如下步骤:
步骤2.1:用所有观测点{(xi,yi),i=1,2,…n}到待求直线的距离li的平方和最小作为估计模型,用待求直线航迹线必须与已知圆相切作为约束条件,计算在此条件下的这条直线的最佳参数(k,d);其中,k为直线的斜率,d为该直线在x轴上的截距;
其中的已知圆就是目标在进入匀速直线运动前所处的圆周运动轨迹,为已知变量,其圆心为半径为r;
所述约束条件下的不加权直线参数估计模型如下:
由(2)式解得:
(3)式代入(1)式得:
对于(4)式,应有f(k)对于k的导数等于零,即:
记
则(5)式化简为:
为了消去“根号”,将(6)改写成:
(7)式两边平方后得:
(a3k2+(a2-b2)k-a3)2=(a1r+b1rk)2(1+k2) (8)
(8)式两边展开并整理后得:
记:
则(9)式可化简为:
k4+bk3+ck2+fk+e=0 (10)
在此采用求实系数代数方程全部根的牛顿-下山法(《C常用算法程序集》-徐士良编著,清华大学出版社,1996)对该式进行求解,得到k1、k2、k3和k4四个根;将k1、k2、k3、k4带入(3)式,对应得到d1、d2、d3、d4;
步骤2.2:按照距离最小原则确定模型的合理解;
(k1,d1)、(k2,d2)、(k3,d3)和(k4,d4)都是约束条件下的不加权直线参数估计模型的实根;按照点{(xi,yi),i=1,2,…n}到所求直线的距离的平方和最小原则,确定合理的直线参数值;该问题也可以简化为:计算点(xn,yn)分别到直线y=k1×x+d1、y=k2×x+d2、y=k3×x+d3和y=k4×x+d4的距离l1,l2,l3,l4:
取|l1|、|l2|、|l3|和|l4|中最小的|li|所对应的(ki,di),作为所求直线的合理参数,记为(k1,d1)。
其中,所述步骤3包括如下步骤:
步骤3.1:计算各点(xi,yi)到直线y-k1x-d1=0的距离|li|之和;
n为数据点数;
步骤3.2:求各点(xi,yi)到直线y-k1x-d1=0的距离li;
式中m表示迭代次数,n表示数据点数;m初始值为1,即:k(1)=k1,d(1)=d1;
步骤3.3:求|li|的倒数;
步骤3.4:求各点的权值wi;
步骤3.5:用所有观测点{(xi,yi),i=1,2,…n}到待求直线的加权距离wili的平方和最小作为估计模型,用待求直线航迹线必须与已知圆相切作为约束条件,计算在此条件下的这条直线的最佳参数(k,d);
其中的已知圆就是目标在进入匀速直线运动前所处的圆周运动轨迹,为已知变量,其圆心为半径为r;
所述约束条件下的加权直线参数估计模型如下:
由(12)式得:
(13)式代入(11)得:
对于(14)式,应有f(k)对于k的导数等于零,即:
记: 则(15)式化简为:
为了消去“根号”,将(16)改写成:
(17)式两边平方后得:
(a'3k2+(a'2-b'2)k-a'3)2=(a'1r+b'1rk)2(1+k2) (18)
(18)式两边展开并整理后得:
记:
则(19)式可化简为:
k4+b'k3+c'k2+f'k+e'=0 (20)
在此采用求实系数代数方程全部根的牛顿-下山法(《C常用算法程序集》-徐士良编著,清华大学出版社,1996)对该式进行求解,得到k'1、k'2、k'3和k'4四个根;将k'1、k'2、k'3、k'4带入(13)式,对应得到d'1、d'2、d'3、d'4;
步骤3.6:按照距离最小原则确定方程的合理解;
(k'1,d'1)、(k'2,d'2)、(k'3,d'3)和(k'4,d'4)都是模型的实根;我们按照点{(xi,yi),i=1,2,…n}到所求直线的距离的平方和最小原则,确定合理的直线参数值;该问题也可以简化为:计算点(xn,yn)分别到直线y=k'1×x+d'1、y=k'2×x+d'2、y=k'3×x+d'3和y=k'4×x+d'4的距离l'1,l'2,l'3,l'4:
m加1,并取|l'1|、|l'2|、|l'3|和|l'4|中最小的|l'i|所对应的(k'i,d'i),作为所求直线的合理参数,记为(k(m),d(m)),m表示迭代次数;
步骤3.7:计算所有数据点到新直线y-k(m)x-d(m)=0的加权距离之和f(m)(k(m),d(m));
式中m表示迭代次数,n表示数据点数;
步骤3.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,d);否则重复步骤3.2-步骤3.8;式中m表示迭代次数;
步骤3.9:使用取点定向法计算出tn时刻的目标航向Kn。
其中,所述步骤3.9中取点定向法的实现过程为:设(k,d)是估计得到的雷达观测航迹线参数,(x1,y1)和(xn,yn)是雷达对目标的首末测量点,经过滤波后,这两个点的坐标变为(x1,y′1)和(xn,y′n),其中:y′1=k×x1+d,y′n=k×xn+d;令:Δx=xn-1-x1,Δy=y′n-1-y′1,π为圆周率,接下来依次进行如下判断和计算;
①如果Δy等于0,转②,否则转③;
②如果Δx大于0,航向K取值为0度,否则,航向K取值为180度,程序结束;
③如果Δx等于0,转④,否则转⑤;
④如果Δy大于等于0,航向K取值为90度,否则,航向K取值为270度,取点定向结束;
⑤如果Δy大于0,航向K取值为度,否则,航向K取值为度,取点定向结束。
取点定向法计算机实现流程如图24所示。航向Kn正北方向为0度,正东方向为90度。
其中,所述步骤4包括如下步骤:
步骤4.1:选取n个测量点{(xi,yi,Ti),i=1,2,…n}作为时间-路程加权匀速估计模型的计算点;
(1)当直线航迹段的实际测量点数m′小于步骤1中确定的目标观测点数n时,需要使用目标处于圆弧运动期的n-m′个测量点,此时这n个点分别对应的时刻按照递增顺序表示为:tm'-n+1,tm'-n+2,…,t-1,t0,t1,t2,…,tm',其中:tm'-n+1,…,t-1,t0为圆弧航迹段上的测量点时刻,t1,t2,…,tm'为直线航迹段上的测量点时刻;
(2)当直线航迹段的实际测量点数m′不小于步骤1中确定的目标观测点数n时,这n个点均为直线航迹段上的测量点,分别对应的时刻按照递增顺序表示为tm'-n+1,tm'-n+2,…,tm';
令:
T1=tm'-n+1,
T2=tm'-n+2,
……
Tn=tm';
并取Ti时刻对应的测量点坐标(xi,yi),构成{(xi,yi,Ti),i=1,2,…n}作为时间-路程加权匀速估计模型的计算点;
步骤4.2:计算Ti时刻目标在航迹线上的垂点(cxi,cyi),i=1,2,…n;
(1)当(xi,yi,Ti)为直线航迹段(y-kx-d=0)上的测量点时;
令:A=k,B=-1,C=d,则:Ti时刻(xi,yi)在直线y-kx-d=0上的垂点(cxi,cyi)为:
(2)当(xi,yi,Ti)为圆心为半径为r的圆弧航迹段上的测量点时;
圆弧航迹线标准方程为:
式中圆心和半径r已知;
过观测点(xi,yi)与圆心的直线方程为:
解(22)式得:
将(23)式代入(21)式得:
解(24)式得:
代入(23)式得:
取(x1,y1)和(x2,y2)中距离测量点(xi,yi)最近的点作为合理解,记为(cxi,cyi);
步骤4.3:计算从T1时刻起,到Ti时刻,目标经过的路程Si,i=1,2,…n;
步骤4.3.1:求Ti时刻与上一计算点Ti-1时刻间的路程ΔSi,i=1时,ΔSi=0;
(1)当(xi,yi,Ti)为直线航迹段上的测量点时;
(2)当(xi,yi,Ti)为圆心为半径为r的圆弧航迹段上的测量点时;
求两相邻计算点之间的弦长:
求圆心到弦的距离:
求圆心角:
求圆心角θi的对应的弧长即为ΔSi:
ΔSi=rθi
步骤4.3.2:计算从T1时刻起,到Ti时刻,目标经过的路程Si;
步骤4.4:使用时间-路程加权匀速估计模型迭代估计目标最佳线速度方程S-Vt-S0=0;
该步骤4.4包括:
步骤4.4.1:对n个计算点的数据{(Ti,Si),i=1,2,…n}使用时间-路程不加权匀速估计模型粗略估计目标线速度方程S-V01t-S01=0;
时间-路程不加权匀速估计模型为:
式中Si是通过步骤4.3.2计算出来的对应Ti时刻目标经过的路程,S0为待求的调节路程,v为待求的匀速度;
接下来对模型(25)进行求解。
f(S0,v)分别对S0和v求偏导数并令其等于零,以ti表示Ti,则有:
(26)、(27)式化简得:
由(28)式解得:
(30)式代入(29)式得:
解(31)式得:
(32)式代入(30)式得:
由以上推导可知,不加权匀速度估计模型求解过程如下;
令:则有:
并将(v,S0)记为(V01,S01);
步骤4.4.2:计算各点(Ti,Si)到S-V01t-S01=0的距离|li|之和;
n为数据点数,V(1)=V01,
步骤4.4.3:求各点(Ti,Si)到S-V01t-S01=0的距离li;
式中m表示迭代次数,n表示数据点数;m初始值为1,即:V(1)=V01,
步骤4.4.4:求|li|的倒数;
步骤4.4.5:求各点的权值wi;
步骤4.4.6:对n个计算点{(Ti,Si),i=1,2,…n}使用时间-路程加权匀速估计模型估计目标线速度方程S-Vt-S0=0;
时间-路程加权匀速估计模型为:
式中Si是通过步骤4.3.2计算出来的对应Ti时刻目标经过的路程,wi是通过步骤4.4.5计算出来的权值,S0为待求的调节路程,v为待求的匀速度;
接下来对模型(34)进行求解。
f(S0,v)分别对S0和v求偏导数并令其等于零,则有:
(35)、(36)式化简得:
由(37)式解得:
(39)式代入(38)式得:
解(40)式得:
令:
则:
将(v,S0)记为m表示迭代次数;
步骤4.4.7:m加1,计算所有数据点{(Ti,Si),i=1,2,…n}到新直线的加权距离之和
式中m表示迭代次数,n表示数据点数;
步骤4.4.8:判别是否为“最佳”解;
若或则输出解并简记为(V,S0);否则重复步骤4.4.3至步骤4.4.8;
步骤4.5:计算tn时刻目标在X、Y方向上的分量速度vxn和vyn;
式中n表示数据点数,tn表示末点时刻;Vn表示末点速度,即步骤4.4.8求得的V;k表示直线航迹线的斜率,在步骤3.8中已求出;
其中,所述步骤5包括如下步骤:
步骤5.1:求步骤4.2得到的垂点{(Ti,cxi,cyi),i=1,2,…n}的X轴坐标均值和时间均值
步骤5.2:求垂点的Y轴均值
其中:(k,d)表示直线航迹线的斜率和截距,在步骤3.8中已求出;
步骤5.3:求tn时刻目标在统一直角坐标系中的位置坐标(Pxn,Pyn);
至此得到了目标由匀速圆周运动转变为匀速直线运动后,在tn时刻的位置(Pxn,Pyn)(步骤5.3)、速度Vn(步骤4.4.8)和航向Kn(步骤3.9)估值。
实施例1
本实施例具体描述本发明所提出的一种约束条件下的单雷达直线航迹线目标状态估计方法,所述估计方法应用于单雷达数据跟踪与多雷达数据融合系统的前期数据预处理过程中。
所述估计方法包括如下步骤:
步骤1:将某雷达针对某个由匀速圆周运动转变为匀速直线运动后直线段上目标的n个观测点数据(ρi,θi,hi,ti)(表示ti时刻该雷达测得的目标距离ρi、方位θi和高度hi,i=1,2,…n。)转换为统一直角坐标(xi,yi,ti)。此处要保证n个观测点数据均处于目标匀速直线运动期间。
步骤1.1:选择某雷达观测数据集{(ρi,θi,hi,ti),i=1,2,…n},是针对某个由匀速圆周运动转变为匀速直线运动后直线段上目标的时序观测数据,要求ti<ti+1。本例中n取值为7,因为是两坐标雷达观测数据,认为hi=0。相关数据见表1.1~表1.2。
表1.1:基本参数
表1.2:雷达观测数据
步骤1.2:将雷达观测数据(ρi,θi,hi,ti),i=1,2,…7转换为以本站为中心的二维直角坐标(Xzi,Yzi):
计算结果见表1.3。
表1.3:以雷达为中心的二维直角坐标
序号 | 时间t(秒) | X<sub>z</sub>(km) | Y<sub>z</sub>(km) |
1 | 849 | -30.78 | -356.85 |
2 | 860 | -28.99 | -355.85 |
3 | 871 | -23.94 | -354.57 |
4 | 881 | -19.82 | -353.49 |
5 | 890 | -17.2 | -352.16 |
6 | 899 | -12.58 | -351.21 |
7 | 911 | -10.22 | -349.66 |
步骤1.3:将(Xzi,Yzi),i=1,2,…7转换为中心统一直角坐标(Xi,Yi):
Xi=Xzicosδxz-Yzisinδxz+Xzx
Yi=Xzisinδxz+Yzicosδxz+Yzx
其中:(Xzx,Yzx)为雷达站址在中心统一直角坐标系中的坐标。δxz为雷达站址与直角坐标系中心点的经度差(单位为弧度)。计算结果见表1.4。
表1.4:测量点的中心统一直角坐标
序号 | 时间t(秒) | x(km) | y(km) |
1 | 849 | 1469.22 | 2193.15 |
2 | 860 | 1471.01 | 2194.15 |
3 | 871 | 1476.06 | 2195.43 |
4 | 881 | 1480.18 | 2196.51 |
5 | 890 | 1482.80 | 2197.84 |
6 | 899 | 1487.42 | 2198.79 |
7 | 911 | 1489.78 | 2200.34 |
步骤2:对n个观测点数据{(xi,yi),i=1,2,…n}使用“约束条件下的不加权直线参数估计模型”粗略估计目标运动状态方程y-k1x-d1=0。包括如下步骤:
步骤2.1:用所有观测点{(xi,yi),i=1,2,…n}到待求直线的距离平方和最小作为估计模型,用待求直线航迹线必须与已知圆相切作为约束条件,计算在此条件下的这条直线的最佳参数(k,d)。其中,k为直线的斜率,d为该直线在x轴上的截距;其中已知圆圆心半径r如表1.1所示。对(1)、(2)式的求解步骤如下。
①计算a1,a2,a3,b1,b2(n=7)。
②计算b,c,f,e。
③采用求实系数代数方程全部根的牛顿-下山法解方程k4+bk3+ck2+fk+e=0,得到所有解k1、k2、k3和k4。将k1、k2、k3、k4带入(3)式,对应得到d1、d2、d3、d4。计算结果见表1.5。
表1.5:方程的所有解
步骤2.2:按照距离最小原则确定模型的合理解。
计算测量点(x7,y7)=(1489.78,2200.34)分别到直线y=k1×x+d1、y=k2×x+d2、y=k3×x+d3和y=k4×x+d4的距离l1,l2,l3,l4:
取|l1|、|l2|、|l3|和|l4|中最小的|l4|所对应的(k4,d4)=(0.345903,1684.723792),作为所求直线的合理参数,记为(k1,d1)。
步骤3:将方程y-k1x-d1=0作为初始状态,对7个观测点数据{(xi,yi),i=1,2,…7}使用“约束条件下的加权直线参数估计模型”进行迭代估计,得到最佳目标运动状态方程y-kx-d=0,并使用“取点定向法”计算出t7时刻的目标航向K7。包括如下步骤:
步骤3.1:计算各点(xi,yi)到直线y-k1x-d1=0的距离|li|之和。
步骤3.2:求各点(xi,yi)到直线y-k1x-d1=0的距离li。
式中m表示迭代次数,n表示数据点数。m初始值为1,即:k(1)=k1,d(1)=d1。计算结果见表1.6。
步骤3.3:求|li|的倒数。计算结果见表1.6。
步骤3.4:求各点的权值wi。计算结果见表1.6。
表1.6:距离和权值计算结果
序号 | l<sub>i</sub> | P<sub>i</sub> | w<sub>i</sub> |
1 | 0.207934 | 4.809229 | 0.158118 |
2 | 0.567144 | 1.763220 | 0.057971 |
3 | 0.125351 | 7.977578 | 0.262288 |
4 | -0.206875 | 4.833839 | 0.158928 |
5 | 0.200250 | 4.993769 | 0.164186 |
6 | -0.414357 | 2.413377 | 0.079347 |
7 | 0.275911 | 3.624357 | 0.119162 |
步骤3.5:用所有观测点{(xi,yi),i=1,2,…n}到待求直线的加权距离的平方和最小作为估计模型,用待求直线航迹线必须与已知圆相切作为约束条件,计算在此条件下的这条直线的最佳参数(k,d)。其中已知圆圆心半径r如表1.1所示。对(11)、(12)式的求解步骤如下。
①计算a'1,a'2,a'3,b'1,b'2(n=7)。
②计算b',c',f',e'。
③采用求实系数代数方程全部根的牛顿-下山法解方程k4+b'k3+c'k2+f'k+e'=0,得到所有解k'1、k'2、k'3和k'4。将k'1、k'2、k'3、k'4带入(3)式,对应得到d'1、d'2、d'3、d'4。计算结果见表1.7。
表1.7:方程的所有解
序号 | k'<sub>i</sub> | d'<sub>i</sub> |
1 | 0.61831927677849863 | 1282.5138982275448 |
2 | 0.61831927677849863 | 1282.5138982275448 |
3 | 0.53180308969301637 | 1410.5992309474163 |
4 | 0.34911833477857185 | 1679.9972604501922 |
步骤3.6:按照距离最小原则确定模型的合理解。
计算测量点(x7,y7)=(1489.78,2200.34)分别到直线y=k'1×x+d'1、y=k'2×x+d'2、y=k'3×x+d'3和y=k'4×x+d'4的距离l'1,l'2,l'3,l'4:
m值加1,并取|l'1|、|l'2|、|l'3|和|l'4|中最小的|l'4|所对应的(k'4,d'4)=(0.349118,1679.997260),作为所求直线的合理参数,记为(k(m),d(m))。
步骤3.7:计算所有数据点到新直线y-k(m)x-d(m)=0的加权距离之和f(m)(k(m),d(m)),m表示迭代次数。
(当前m=2)
步骤3.8:判别是否为“最佳”解。
因f(1)(k(1),d(1))=1.997822,f(2)(k(2),d(2))=0.221203,f(2)(k(2),d(2))<f(1)(k(1),d(1)),则重复步骤3.2至步骤3.8。经计算,m=27、执行步骤3.8时,f(27)(k(27),d(27))=0.000000617345<10-6,因此输出解(k(26),d(26))=(0.361058,1662.438159),并简记为(k,d)。
步骤3.9:使用“取点定向法”计算出t7时刻的目标航向K7。
取首末点坐标(x1,y1)和(x7,y7),并计算y′1和y′7得:
x1=1469.22,y1=2193.15,y′1=k×x1+d=2192.912703;
x7=1489.78,y7=2200.34,y′7=k×x7+d=2200.335525。
按照图24所示流程计算得K7=70.147454度。
步骤4:使用“时间-路程加权匀速估计模型”迭代估计目标最佳线速度方程S-Vt-S0=0,得到tn时刻的目标速度Vn并计算出目标在X、Y方向上的分量速度vxn和vyn。包括如下步骤:
步骤4.1:选取7个测量点{(xi,yi,Ti),i=1,2,…7}作为“时间-路程加权匀速估计模型”的计算点。
因当前直线航迹段的实际测量点数为7,分别对应的时刻按照递增顺序表示为t1,t2,…,t7,令Ti=ti,i=1,2,…7,并取Ti时刻对应的测量点坐标(xi,yi),构成{(xi,yi,Ti),i=1,2,…7}作为“时间-路程加权匀速估计模型”的计算点,见表1.8。
步骤4.2:计算Ti时刻目标在航迹线上的垂点(cxi,cyi),i=1,2,…n。
因{(xi,yi,Ti),i=1,2,…7}为直线航迹段y-kx-d=0上的测量点,令:A=k=0.361058,B=-1,C=d=1662.438159,则:Ti时刻(xi,yi)在直线y-kx-d=0上的垂点(cxi,cyi)为:
计算结果见表1.8。
表1.8:速度估计模型的计算点、垂点和路程
步骤4.3:计算从T1时刻起,到Ti时刻,目标经过的路程Si,i=1,2,…7。
步骤4.3.1:求Ti时刻与上一计算点Ti-1时刻间的路程ΔSi(i=1时,ΔSi=0)。
因{(xi,yi,Ti),i=1,2,…7}为直线航迹段上的测量点,计算结果见表1.8。
步骤4.3.2:计算从T1时刻起,到Ti时刻,目标经过的路程计算结果见表1.8。
步骤4.4:使用“时间-路程加权匀速估计模型”迭代估计目标最佳线速度方程S-Vt-S0=0。
步骤4.4.1:对7个计算点的数据{(Ti,Si),i=1,2,…7}使用“时间-路程不加权匀速估计模型”粗略估计目标线速度方程S-V01t-S01=0。对模型(25)的求解步骤如下。
①计算
②计算v,S0,并记为(V01,S01)。
(单位:km/秒),
(单位:km)。
步骤4.4.2:计算各点(Ti,Si)到S-V01t-S01=0的距离|li|之和。
其中:V(1)=V01=0.375466。,
步骤4.4.3:求各点(Ti,Si)到S-V01t-S01=0的距离li。
式中m表示迭代次数,n表示数据点数。m初始值为1,即:V(1)=V01,计算结果见表1.9。
步骤4.4.4:求|li|的倒数。计算结果见表1.9。
步骤4.4.5:求各点的权值wi。计算结果见表1.9。
表1.9:权值计算过程表
序号 | l<sub>i</sub> | P<sub>i</sub> | w<sub>i</sub> |
1 | 0.857912 | 1.165621 | 0.062245 |
2 | -1.251272 | 0.799187 | 0.042677 |
3 | -0.198914 | 5.027293 | 0.268461 |
4 | 0.292688 | 3.416602 | 0.182449 |
5 | -0.170360 | 5.869917 | 0.313458 |
6 | 1.114817 | 0.897008 | 0.047901 |
7 | -0.644872 | 1.550696 | 0.082808 |
步骤4.4.6:对7个计算点{(Ti,Si),i=1,2,…7}使用“时间-路程加权匀速估计模型”估计目标线速度方程S-Vt-S0=0。对模型(34)的求解步骤如下。
①计算w0,wSt,wS,wt1,wt2(n=7)。
②计算v,S0,并记为m表示迭代次数。
步骤4.4.7:m值加1,计算所有数据点{(Ti,Si),i=1,2,…7}到新直线的加权距离之和m表示迭代次数。
(当前m为2)
步骤4.4.8:判别是否为“最佳”解。
因 则重复步骤4.4.3至步骤4.4.8。经计算,m=3时,则输出解并简记为(V,S0)。
步骤4.5:计算t7时刻目标在X、Y方向上的分量速度vx7和vy7。
vx7=V7cosk=0.370678×cos(0.361058)=0.346778,
vy7=V7sink==0.370678×sin(0.361058)=0.130947。
式中V7表示末点速度,即步骤4.4.8求得的V;k表示直线航迹线的斜率,在步骤3.8中已求出。
步骤5:计算7个观测点数据{(xi,yi,ti),i=1,2,…7}到直线y-kx-d=0的垂足点的X轴坐标均值和时间均值并令最终求出t7时刻目标在统一直角坐标系中的位置估值(Px7,Py7)。包括如下步骤:
步骤5.1:求表1.8中垂点{(Ti,cxi,cyi),i=1,2,…7}的X轴坐标均值和时间均值
步骤5.2:求垂点的Y轴均值
其中:(k,d)表示直线航迹线的斜率和截距,在步骤3.8中已求出。
步骤5.3:求t7=911秒时,目标在统一直角坐标系中的位置坐标(Px7,Py7)。
至此我们就得到了目标由匀速圆周运动转变为匀速直线运动后,在t7=911秒时:
位置(Px7,Py7)=(1490.189003,2200.660633)km,
速度V7=0.370678公里/秒=1334.439229公里/小时,
航向K7=70.147454度。(正北方向为0度,正东方向为90度)
实施例2
本实施例具体描述本发明所提出的一种约束条件下的单雷达直线航迹线目标状态估计方法与传统卡尔曼滤波方法的对比试验过程,以及二者在执行效果上的比较。
所述对比试验过程包括如下步骤:
步骤1:产生模拟数据的真实值和测量值。
按照表2.1中的模拟目标参数值产生匀速圆周运动后转入匀速直线飞行状态的目标的60个位置点(前30个处于匀速圆弧航迹段,后30个处于匀速直线航迹段)的真实值和测量值,结果见表2.2。
表2.1:基本参数
表2.2:雷达观测数据的真实值和测量值
步骤2:将目标点数据{(ρi,θi,hi,ti),i=1,2,…60。}转换为统一直角坐标{(Xi,Yj,ti),i=1,2,…60。}。
Xi=Xzicosδxz-Yzisinδxz+Xzx
Yi=Xzisinδxz+Yzicosδxz+Yzx
其中:所有测量点的高度hi均简化为0,(Xzx,Yzx)为雷达站址在中心统一直角坐标系中的坐标。δxz为雷达站址与直角坐标系中心点的经度差(单位为弧度)。坐标计算结果见表2.3。目标点真实位置和测量位置在中心统一直角坐标系中的显示如图2所示。
表2.3:目标点的中心统一直角坐标
步骤3:对表2.3中的直线航迹段测量值{(ti,Xi,Yj),i=31,32,…60}使用本发明所述估计方法逐点进行目标位置(wPxi,wPyi)、速度wVi和航向wKi估计。其中:参数n取值为7。表2.4是n=7时每次参与估值计算的点序号表。表2.5是应用本发明的估计结果。
表2.4:n=7时每次参与估值计算的点
表2.5:应用本发明(n=7)逐点进行目标状态估计
步骤4:对表2.3中的直线航迹段测量值{(ti,Xi,Yj),i=31,32,…60}使用传统卡尔曼滤波方法逐点进行目标位置(kPxi,kPyi)、速度kVi和航向kKi估计。其中:Q、R参数取值见表2.1。表2.6是应用卡尔曼滤波方法的逐点估计结果。
表2.6:应用卡尔曼方法逐点进行目标状态估计
目标点真实位置、测量点位置、本发明估计位置和卡尔曼滤波估计位置在中心统一直角坐标系中的显示如图3、图4所示。图上可见,两种滤波方法得到的航迹线几乎重合,均优于测量航迹线,接下来我们对时间配准点、速度和航向等指标进一步统计、比较。
步骤5:时间配准点比较
为了进一步比较位置估值效果,我们引入时间配准点概念,即:相同时刻目标真实位置点与估计位置点称为一对时间配准点。ti时刻一对时间配准点之间的距离称为时间配准点间距di,d1~dm的平均值定义为tm时刻的平均时间配准点间距vdm。根据表2.3~2.6的计算结果,可以统计使用本发明方法和使用卡尔曼方法得到时间配准点间距与平均时间配准点间距值,见表2.7和表2.8,其中(rPx,rPy)表示目标真实位置,wvd6表示使用本发明方法得到的滤波器开始工作后1分钟以内估值点的平均时间配准点间距,wvd30表示使用本发明方法得到的所有估值点的平均时间配准点间距;kvd6表示使用卡尔曼方法得到的滤波器开始工作后1分钟以内估值点的平均时间配准点间距,kvd30表示使用卡尔曼方法得到的所有估值点的平均时间配准点间距。
表2.7:本发明时间配准点间距wd与平均间距wvd
表2.8:卡尔曼时间配准点间距kd与平均间距kvd
图5和图6是本发明方法和卡尔曼方法时间配准点间距与平均时间配准点间距的对比图。经过统计,全程有21个点的时间配准点间距wdi≤kdi,占全部点数的70.00%;航迹起始1分钟内,总共得到6个测量点,其中有5个点的时间配准点间距wdi≤kdi,占比83.33%;所有时刻的平均时间配准点间距均有wvdi≤kvdi;全程平均时间配准点间距wvd30占kvd30的68.32%;航迹起始1分钟内平均时间配准点间距wvd6占kvd6的24.46%。本发明方法在测量初期、点数较少时能获得更加准确的目标位置估值,优势非常明显。于是可得到以下结论:在本次试验中,以目标真实位置为基准,使用本发明方法得到的目标位置点优于卡尔曼方法位置点。
步骤6:速度估值比较
我们定义ti时刻目标估计速度与真实速度差的绝对值为速度差dvi,dv1~dvm的平均值定义为tm时刻的平均速度差vdvm。根据表2.3~2.6的计算结果,可以统计使用本发明方法和使用卡尔曼方法得到的所有时间点上的速度差和平均速度差值,见表2.9,其中rV表示目标真实速度;wVi、wdvi、wvdvi表示使用本发明方法得到的ti时刻目标速度估值、速度差和平均速度差;kVi、kdvi、kvdvi表示使用卡尔曼方法得到的ti时刻目标速度估值、速度差和平均速度差。
表2.9:速度、速度差和平均速度差
图7、图8和图9是本发明方法和卡尔曼方法分别在速度、速度差和平均速度差的对比图。经过统计,全程有27个估值点的速度差wdvi≤kdvi,占全部点数的90.00%;航迹起始1分钟内得到的总共6个估值点的速度差均有wdvi≤kdvi,占比100.00%;所有时刻的平均速度差均有wvdvi≤kvdvi;全程平均速度差wvdv30只占kvdv30的25.54%;航迹起始1分钟内平均速度差wvdv6只占kvdv6的6.36%。图上可以看出,本发明得到的速度估值更加准确、稳定,而且本发明方法在测量初期、点数较少时就能获得更加准确的速度估值,优势非常明显。
步骤7:航向估值比较
我们定义ti时刻目标估计航向与真实航向差的绝对值为航向差dki,dk1~dkm的平均值定义为tm时刻的平均航向差vdkm。根据表2.3~2.6的计算结果,可以统计使用本发明方法和使用卡尔曼方法得到的所有时间点上的航向差和平均航向差值,见表2.10,其中rK表示目标真实航向;wKi、wdki、wvdki表示使用本发明方法得到的ti时刻目标航向估值、航向差和平均航向差;kKi、kdki、kvdki表示使用卡尔曼方法得到的ti时刻目标航向估值、航向差和平均航向差。
表2.10:航向、航向差和平均航向差
图10、图11和图12是本发明方法和卡尔曼方法分别在航向、航向差和平均航向差的对比图。经过统计,全程有30个点的航向差wdki≤kdki,占全部点数的100.00%;航迹起始1分钟内得到的总共6个估值点的航向差均有wdki≤kdki,占比100.00%;所有时刻的平均航向差均有wvdki≤kvdki;全程平均航向差wvdk30只占kvdk30的40.60%;航迹起始1分钟内平均航向差wvdk6占kvdk6的49.40%。图上可以看出,本发明得到的航向估值更加准确、稳定,而且本发明方法在测量初期、点数较少时就能获得更加准确的航向估值,优势非常明显。
步骤8:出现野值后的估值效果比较
为了模拟野值出现,我们在表2.3的第36个(t=899秒)测量点上人为加入较大误差。图13~23分别显示了出现野值后,本发明方法和卡尔曼方法在航迹线、时间配准点、速度和航向等指标上的比较效果,表2.11是测量中包含野值与不包含野值时各项指标的统计结果。
表2.11:包含/不包含野值时各项指标统计
从以上图、表可以看出,卡尔曼滤波方法在野值出现后,位置、速度、和航向估值出现较大突变。相比之下,本发明方法对野值抑制效果较为明显,无论是在野值点处,还是其后几个点上,估值效果都优于卡尔曼方法。
本发明基于复杂数据环境,在无须获取系统噪声和观测噪声等先验知识的情况下,通过对目标由匀速圆周运动转变为匀速直线运动初期时运动规律的研究,将目标在匀速圆周运动模型中的先验信息作为当前匀速直线运动模型的约束条件,在统一直角坐标系中对当前有限个测量点进行垂直距离加权估计得到与时间无关的目标运动方程和航向估值,通过目标时间-路程加权匀速估计模型得到线速度和分量速度估值,进而最终确定目标状态位置估计结果。通过与传统卡尔曼滤波方法在时间配准点间距、速度差和航向差等指标的对比试验表明,本发明不但易于工程实现,而且比传统卡尔曼滤波方法具有更优的起始和全程目标状态估计效果,以及野值抑制能力。该方法有效解决了传统卡尔曼滤波方法应用条件比较苛刻、过程噪声协方差和量测噪声协方差难以确定和系统测量中出现野值,可能导致滤波发散或目标状态估计突变等问题,极大地改善了单雷达数据质量,为在本系统中开展目标跟踪与数据融合工作提供了一种稳定、有效且易于工程实现的滤波方法。本发明估计方法科学、方案实施步骤合理,目标估计效果理想,对于提高了多雷达目标状态估计的一致性和准确性具有重要意义。本发明所提供的方法的时间复杂度和空间复杂度都很低,可操作性和实用性很强。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明技术原理的前提下,还可以做出若干改进和变形,这些改进和变形也应视为本发明的保护范围。例如但不限于以下几点:
(1)为了节省计算时间,在步骤1中,可以存储前n-1个测量点的统一直角坐标,每次只对第n个测量点进行极坐标到统一直角坐标转换。
(2)关于n的取值问题,我们认为一般取4~15为宜。但是,在相同数据环境中,在保证所取的n个测量点均处于目标匀速直线飞行状态的前提下,n越大,目标状态估值精度就越高。
(3)在步骤4.3.1中,求Ti时刻与上一计算点Ti-1时刻间的路程ΔSi时,当i=1时,如果ΔSi设置为与上一估值点(如:圆弧航迹段末点)的距离,效果就更为理想。
(4)在步骤2.1和步骤3.5中,也可以采用费拉里法等其它方法对一元四次方程(10)、(20)进行求解。
Claims (8)
1.一种约束条件下的单雷达直线航迹线目标状态估计方法,其特征在于,所述估计方法应用于单雷达数据跟踪与多雷达数据融合系统的前期数据预处理过程中;所述估计方法包括如下步骤:
步骤1:将某雷达针对某个由匀速圆周运动转变为匀速直线运动后直线段上目标的n个观测点数据(ρi,θi,hi,ti)转换为统一直角坐标(xi,yi,ti);其中,(ρi,θi,hi,ti)表示ti时刻该雷达测得的目标距离ρi、方位θi和高度hi,i=1,2,…n;
步骤2:对n个观测点数据{(xi,yi),i=1,2,…n}使用约束条件下的不加权直线参数估计模型粗略估计目标运动状态方程y-k1x-d1=0;
步骤3:将方程y-k1x-d1=0作为初始状态,对n个观测点数据{(xi,yi),i=1,2,…n}使用约束条件下的加权直线参数估计模型进行迭代估计,得到最佳目标运动状态方程y-kx-d=0,并使用取点定向法计算出tn时刻的目标航向Kn;
步骤4:使用时间-路程加权匀速估计模型迭代估计目标最佳线速度方程S-Vt-S0=0,得到tn时刻的目标速度Vn并计算出目标在X、Y方向上的分量速度vxn和vyn:
步骤5:计算n个观测点数据{(xi,yi,ti),i=1,2,…n}到直线y-kx-d=0的垂足点的X轴坐标均值和时间均值并令则tn时刻目标在统一直角坐标系中的位置估值(Pxn,Pyn)为:
由此得到目标由匀速圆周运动转变为匀速直线运动后,在tn时刻的位置(Pxn,Pyn)、速度Vn和航向Kn。
2.如权利要求1所述的约束条件下的单雷达直线航迹线目标状态估计方法,其特征在于,所述步骤1包括如下步骤:
步骤1.1:选择某雷达观测数据集{(ρi,θi,hi,ti),i=1,2,…n},是针对某个由匀速圆周运动转变为匀速直线运动后直线段上目标的时序观测数据;
步骤1.2:将雷达观测数据(ρi,θi,hi,ti),i=1,2,…n转换为以本站为中心的二维直角坐标(Xzi,Yzi):
步骤1.3:将(Xzi,Yzi),i=1,2,…n转换为中心统一直角坐标(Xi,Yi):
Xi=Xzicosδxz-Yzisinδxz+Xzx
Yi=Xzisinδxz+Yzicosδxz+Yzx
其中:(Xzx,Yzx)为雷达站址在中心统一直角坐标系中的坐标;δxz为雷达站址与直角坐标系中心点的经度差,单位为弧度。
3.如权利要求2所述的约束条件下的单雷达直线航迹线目标状态估计方法,所述步骤1.1中,ti<ti+1,且n不大于N,N表示参与目标状态估计的最近时间内有限个观测数据点的个数,N取4~15。
4.如权利要求2所述的约束条件下的单雷达直线航迹线目标状态估计方法,其特征在于,所述步骤2包括如下步骤:
步骤2.1:用所有观测点{(xi,yi),i=1,2,…n}到待求直线的距离li的平方和最小作为估计模型,用待求直线航迹线必须与已知圆相切作为约束条件,计算在此条件下的这条直线的最佳参数(k,d);其中,k为直线的斜率,d为该直线在x轴上的截距;
其中的已知圆就是目标在进入匀速直线运动前所处的圆周运动轨迹,为已知变量,其圆心为半径为r;
所述约束条件下的不加权直线参数估计模型如下:
由(2)式解得:
(3)式代入(1)式得:
对于(4)式,应有f(k)对于k的导数等于零,即:
记
则(5)式化简为:
为了消去“根号”,将(6)改写成:
(7)式两边平方后得:
(a3k2+(a2-b2)k-a3)2=(a1r+b1rk)2(1+k2) (8)
(8)式两边展开并整理后得:
记:
则(9)式可化简为:
k4+bk3+ck2+fk+e=0 (10)
在此采用求实系数代数方程全部根的牛顿-下山法对该式进行求解,得到k1、k2、k3和k4四个根;将k1、k2、k3、k4带入(3)式,对应得到d1、d2、d3、d4;
步骤2.2:按照距离最小原则确定模型的合理解;
(k1,d1)、(k2,d2)、(k3,d3)和(k4,d4)都是约束条件下的不加权直线参数估计模型的实根;按照点{(xi,yi),i=1,2,…n}到所求直线的距离的平方和最小原则,确定合理的直线参数值;该问题也可以简化为:计算点(xn,yn)分别到直线y=k1×x+d1、y=k2×x+d2、y=k3×x+d3和y=k4×x+d4的距离l1,l2,l3,l4:
取|l1|、|l2|、|l3|和|l4|中最小的|li|所对应的(ki,di),作为所求直线的合理参数,记为(k1,d1)。
5.如权利要求4所述的约束条件下的单雷达直线航迹线目标状态估计方法,其特征在于,所述步骤3包括如下步骤:
步骤3.1:计算各点(xi,yi)到直线y-k1x-d1=0的距离|li|之和;
n为数据点数;
步骤3.2:求各点(xi,yi)到直线y-k1x-d1=0的距离li;
式中m表示迭代次数,n表示数据点数;m初始值为1,即:
k(1)=k1,d(1)=d1;
步骤3.3:求|li|的倒数;
步骤3.4:求各点的权值wi;
步骤3.5:用所有观测点{(xi,yi),i=1,2,…n}到待求直线的加权距离wili的平方和最小作为估计模型,用待求直线航迹线必须与已知圆相切作为约束条件,计算在此条件下的这条直线的最佳参数(k,d);
其中的已知圆就是目标在进入匀速直线运动前所处的圆周运动轨迹,为已知变量,其圆心为半径为r;
所述约束条件下的加权直线参数估计模型如下:
由(12)式得:
(13)式代入(11)得:
对于(14)式,应有f(k)对于k的导数等于零,即:
记: 则(15)式化简为:
为了消去“根号”,将(16)改写成:
(17)式两边平方后得:
(a'3k2+(a'2-b'2)k-a'3)2=(a'1r+b'1rk)2(1+k2) (18)
(18)式两边展开并整理后得:
记:
则(19)式可化简为:
k4+b'k3+c'k2+f'k+e'=0 (20)
在此采用求实系数代数方程全部根的牛顿-下山法对该式进行求解,得到k'1、k'2、k'3和k'4四个根;将k'1、k'2、k'3、k'4带入(13)式,对应得到d'1、d'2、d'3、d'4;
步骤3.6:按照距离最小原则确定方程的合理解;
(k'1,d'1)、(k'2,d'2)、(k'3,d'3)和(k'4,d'4)都是模型的实根;我们按照点{(xi,yi),i=1,2,…n}到所求直线的距离的平方和最小原则,确定合理的直线参数值;该问题也可以简化为:计算点(xn,yn)分别到直线y=k'1×x+d'1、y=k'2×x+d'2、y=k'3×x+d'3和y=k'4×x+d'4的距离l'1,l'2,l'3,l'4:
m加1,并取|l'1|、|l'2|、|l'3|和|l'4|中最小的|l'i|所对应的(k'i,d'i),作为所求直线的合理参数,记为(k(m),d(m)),m表示迭代次数;
步骤3.7:计算所有数据点到新直线y-k(m)x-d(m)=0的加权距离之和f(m)(k(m),d(m));
式中m表示迭代次数,n表示数据点数;
步骤3.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,d);否则重复步骤3.2至步骤3.8。式中m表示迭代次数;
步骤3.9:使用取点定向法计算出tn时刻的目标航向Kn。
6.如权利要求5所述的约束条件下的单雷达直线航迹线目标状态估计方法,所述步骤3.9中取点定向法的实现过程为:设(k,d)是估计得到的雷达观测航迹线参数,(x1,y1)和(xn,yn)是雷达对目标的首末测量点,经过滤波后,这两个点的坐标变为(x1,y1′)和(xn,yn′),其中:y1′=k×x1+d,yn′=k×xn+d;令:Δx=xn-1-x1,Δy=y′n-1-y1′,π为圆周率,接下来依次进行如下判断和计算;
①如果Δy等于0,转②,否则转③;
②如果Δx大于0,航向K取值为0度,否则,航向K取值为180度,程序结束;
③如果Δx等于0,转④,否则转⑤;
④如果Δy大于等于0,航向K取值为90度,否则,航向K取值为270度,取点定向结束;
⑤如果Δy大于0,航向K取值为度,否则,航向K取值为度,取点定向结束。
7.如权利要求5所述的约束条件下的单雷达直线航迹线目标状态估计方法,其特征在于,所述步骤4包括如下步骤:
步骤4.1:选取n个测量点{(xi,yi,Ti),i=1,2,…n}作为时间-路程加权匀速估计模型的计算点;
(1)当直线航迹段的实际测量点数m′小于步骤1中确定的目标观测点数n时,需要使用目标处于圆弧运动期的n-m′个测量点,此时这n个点分别对应的时刻按照递增顺序表示为:tm'-n+1,tm'-n+2,…,t-1,t0,t1,t2,…,tm',其中:tm'-n+1,…,t-1,t0为圆弧航迹段上的测量点时刻,t1,t2,…,tm'为直线航迹段上的测量点时刻。
(2)当直线航迹段的实际测量点数m′不小于步骤1中确定的目标观测点数n时,这n个点均为直线航迹段上的测量点,分别对应的时刻按照递增顺序表示为tm'-n+1,tm'-n+2,…,tm'。
令:
T1=tm'-n+1,
T2=tm'-n+2,
……
Tn=tm';
并取Ti时刻对应的测量点坐标(xi,yi),构成{(xi,yi,Ti),i=1,2,…n}作为时间-路程加权匀速估计模型的计算点;
步骤4.2:计算Ti时刻目标在航迹线上的垂点(cxi,cyi),i=1,2,…n;
(1)当(xi,yi,Ti)为直线航迹段(y-kx-d=0)上的测量点时;
令:A=k,B=-1,C=d,则:Ti时刻(xi,yi)在直线y-kx-d=0上的垂点(cxi,cyi)为:
(2)当(xi,yi,Ti)为圆心为半径为r的圆弧航迹段上的测量点时;
圆弧航迹线标准方程为:
式中圆心和半径r已知;
过观测点(xi,yi)与圆心的直线方程为:
解(22)式得:
将(23)式代入(21)式得:
解(24)式得:
代入(23)式得:
取(x1,y1)和(x2,y2)中距离测量点(xi,yi)最近的点作为合理解,记为(cxi,cyi);
步骤4.3:计算从T1时刻起,到Ti时刻,目标经过的路程Si,i=1,2,…n;
步骤4.3.1:求Ti时刻与上一计算点Ti-1时刻间的路程ΔSi,i=1时,ΔSi=0;
(1)当(xi,yi,Ti)为直线航迹段上的测量点时;
(2)当(xi,yi,Ti)为圆心为半径为r的圆弧航迹段上的测量点时;
求两相邻计算点之间的弦长:
求圆心到弦的距离:
求圆心角:
求圆心角θi的对应的弧长即为ΔSi:
ΔSi=rθi
步骤4.3.2:计算从T1时刻起,到Ti时刻,目标经过的路程Si;
步骤4.4:使用时间-路程加权匀速估计模型迭代估计目标最佳线速度方程S-Vt-S0=0;
该步骤4.4包括如下步骤:
步骤4.4.1:对n个计算点的数据{(Ti,Si),i=1,2,…n}使用时间-路程不加权匀速估计模型粗略估计目标线速度方程S-V01t-S01=0;
时间-路程不加权匀速估计模型为:
式中Si是通过步骤4.3.2计算出来的对应Ti时刻目标经过的路程,S0为待求的调节路程,v为待求的匀速度;
接下来对模型(25)进行求解;
f(S0,v)分别对S0和v求偏导数并令其等于零,以ti表示Ti,则有:
(26)、(27)式化简得:
由(28)式解得:
(30)式代入(29)式得:
解(31)式得:
(32)式代入(30)式得:
由以上推导可知,不加权匀速度估计模型求解过程如下:
令:则有:
并将(v,S0)记为(V01,S01)。
步骤4.4.2:计算各点(Ti,Si)到S-V01t-S01=0的距离|li|之和;
其中,n为数据点数,V(1)=V01,
步骤4.4.3:求各点(Ti,Si)到S-V01t-S01=0的距离li;
式中m表示迭代次数,n表示数据点数;m初始值为1,即:V(1)=V01,
步骤4.4.4:求|li|的倒数;
步骤4.4.5:求各点的权值wi;
步骤4.4.6:对n个计算点{(Ti,Si),i=1,2,…n}使用时间-路程加权匀速估计模型估计目标线速度方程S-Vt-S0=0;
时间-路程加权匀速估计模型为:
式中Si是通过步骤4.3.2计算出来的对应Ti时刻目标经过的路程,wi是通过步骤4.4.5计算出来的权值,S0为待求的调节路程,v为待求的匀速度。接下来对模型(34)进行求解。
f(S0,v)分别对S0和v求偏导数并令其等于零,则有:
(35)、(36)式化简得:
由(37)式解得:
(39)式代入(38)式得:
解(40)式得:
令:
则:
将(v,S0)记为m表示迭代次数。
步骤4.4.7:m加1,计算所有数据点{(Ti,Si),i=1,2,…n}到新直线的加权距离之和
式中m表示迭代次数,n表示数据点数。
步骤4.4.8:判别是否为“最佳”解;
若或则输出解并简记为(V,S0);否则重复步骤4.4.3至步骤4.4.8;
步骤4.5:计算tn时刻目标在X、Y方向上的分量速度vxn和vyn;
式中n表示数据点数,tn表示末点时刻;Vn表示末点速度,即步骤4.4.8求得的V;k表示直线航迹线的斜率,在步骤3.8中已求出。
8.如权利要求7所述的约束条件下的单雷达直线航迹线目标状态估计方法,其特征在于,所述步骤5包括如下步骤:
步骤5.1:求步骤4.2得到的垂点{(Ti,cxi,cyi),i=1,2,…n}的X轴坐标均值和时间均值
步骤5.2:求垂点的Y轴均值
其中:(k,d)表示直线航迹线的斜率和截距,在步骤3.8中已求出;
步骤5.3:求tn时刻目标在统一直角坐标系中的位置坐标(Pxn,Pyn);
至此得到了目标由匀速圆周运动转变为匀速直线运动后,在tn时刻的位置(Pxn,Pyn)、速度Vn和航向Kn估值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910004684.5A CN109856622B (zh) | 2019-01-03 | 2019-01-03 | 一种约束条件下的单雷达直线航迹线目标状态估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910004684.5A CN109856622B (zh) | 2019-01-03 | 2019-01-03 | 一种约束条件下的单雷达直线航迹线目标状态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109856622A true CN109856622A (zh) | 2019-06-07 |
CN109856622B CN109856622B (zh) | 2021-04-20 |
Family
ID=66893845
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910004684.5A Active CN109856622B (zh) | 2019-01-03 | 2019-01-03 | 一种约束条件下的单雷达直线航迹线目标状态估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109856622B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111693962A (zh) * | 2020-06-18 | 2020-09-22 | 中国人民解放军空军研究院战略预警研究所 | 一种基于交叉检验的目标运动模型估计方法 |
CN111693984A (zh) * | 2020-05-29 | 2020-09-22 | 中国计量大学 | 一种改进的ekf-ukf动目标跟踪方法 |
CN112836354A (zh) * | 2021-01-12 | 2021-05-25 | 中南大学 | 一种目标跟踪定位方法、系统、装置及可读存储介质 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5270718A (en) * | 1992-08-21 | 1993-12-14 | Technology Service Corporation | Method and apparatus for tracking targets from direct and multipath reflected radar signals |
US20090091490A1 (en) * | 2007-10-09 | 2009-04-09 | National Taiwan University | Method and system for radar tracking of moving target from moving station |
CN103235291A (zh) * | 2013-04-24 | 2013-08-07 | 四川九洲空管科技有限责任公司 | 一种航迹滤波的方法 |
CN104020451A (zh) * | 2014-06-03 | 2014-09-03 | 西安电子科技大学 | 基于聚类的外辐射源雷达目标航迹处理方法 |
CN104076354A (zh) * | 2014-07-08 | 2014-10-01 | 西安电子科技大学 | 一种基于关联速度的雷达目标航迹的检测方法 |
CN105353368A (zh) * | 2015-11-09 | 2016-02-24 | 中国船舶重工集团公司第七二四研究所 | 一种基于策略判决的自适应变结构雷达对海目标跟踪方法 |
CN106054149A (zh) * | 2016-07-22 | 2016-10-26 | 中国船舶重工集团公司第七二四研究所 | 一种雷达机动目标三维航迹模拟方法 |
-
2019
- 2019-01-03 CN CN201910004684.5A patent/CN109856622B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5270718A (en) * | 1992-08-21 | 1993-12-14 | Technology Service Corporation | Method and apparatus for tracking targets from direct and multipath reflected radar signals |
US20090091490A1 (en) * | 2007-10-09 | 2009-04-09 | National Taiwan University | Method and system for radar tracking of moving target from moving station |
CN103235291A (zh) * | 2013-04-24 | 2013-08-07 | 四川九洲空管科技有限责任公司 | 一种航迹滤波的方法 |
CN104020451A (zh) * | 2014-06-03 | 2014-09-03 | 西安电子科技大学 | 基于聚类的外辐射源雷达目标航迹处理方法 |
CN104076354A (zh) * | 2014-07-08 | 2014-10-01 | 西安电子科技大学 | 一种基于关联速度的雷达目标航迹的检测方法 |
CN105353368A (zh) * | 2015-11-09 | 2016-02-24 | 中国船舶重工集团公司第七二四研究所 | 一种基于策略判决的自适应变结构雷达对海目标跟踪方法 |
CN106054149A (zh) * | 2016-07-22 | 2016-10-26 | 中国船舶重工集团公司第七二四研究所 | 一种雷达机动目标三维航迹模拟方法 |
Non-Patent Citations (1)
Title |
---|
李宏博, 沈一鹰: "一种基于航迹光滑滤波的目标跟踪方法", 《现代雷达》 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111693984A (zh) * | 2020-05-29 | 2020-09-22 | 中国计量大学 | 一种改进的ekf-ukf动目标跟踪方法 |
CN111693962A (zh) * | 2020-06-18 | 2020-09-22 | 中国人民解放军空军研究院战略预警研究所 | 一种基于交叉检验的目标运动模型估计方法 |
CN111693962B (zh) * | 2020-06-18 | 2023-03-14 | 中国人民解放军空军研究院战略预警研究所 | 一种基于交叉检验的目标运动模型估计方法 |
CN112836354A (zh) * | 2021-01-12 | 2021-05-25 | 中南大学 | 一种目标跟踪定位方法、系统、装置及可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN109856622B (zh) | 2021-04-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109459040B (zh) | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 | |
CN109856622A (zh) | 一种约束条件下的单雷达直线航迹线目标状态估计方法 | |
CN109459033A (zh) | 一种多重渐消因子的机器人无迹快速同步定位与建图方法 | |
CN103017771B (zh) | 一种静止传感器平台的多目标联合分配与跟踪方法 | |
CN108802721B (zh) | 一种任意直线约束下目标跟踪方法 | |
CN108536171A (zh) | 一种多约束下多无人机协同跟踪的路径规划方法 | |
CN109186601A (zh) | 一种基于自适应无迹卡尔曼滤波的激光slam算法 | |
CN110779519B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
CN110208792A (zh) | 同时估计目标状态和轨迹参数的任意直线约束跟踪方法 | |
CN107728138A (zh) | 一种基于当前统计模型的机动目标跟踪方法 | |
CN110646783B (zh) | 一种水下航行器的水下信标定位方法 | |
CN110794409A (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
CN109856616B (zh) | 一种雷达定位相对系统误差修正方法 | |
CN110749891A (zh) | 一种可估计未知有效声速的自适应水下单信标定位方法 | |
CN109856623A (zh) | 一种针对多雷达直线航迹线的目标状态估计方法 | |
CN103487800A (zh) | 基于残差反馈的多模型高速高机动目标跟踪方法 | |
CN103499809B (zh) | 一种纯方位双机协同目标跟踪定位路径规划方法 | |
CN107621632A (zh) | 用于nshv跟踪滤波的自适应滤波方法及系统 | |
CN113030940B (zh) | 一种转弯机动下的多星凸型扩展目标跟踪方法 | |
CN112986977A (zh) | 一种克服雷达扩展卡尔曼航迹滤波发散的方法 | |
CN116224320B (zh) | 一种极坐标系下处理多普勒量测的雷达目标跟踪方法 | |
CN116047495B (zh) | 一种用于三坐标雷达的状态变换融合滤波跟踪方法 | |
CN109856619B (zh) | 一种雷达测向相对系统误差修正方法 | |
Mallick et al. | Comparison of measures of nonlinearity for bearing-only and GMTI filtering | |
CN109856624B (zh) | 一种针对单雷达直线航迹线的目标状态估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |