技术领域
[0001] 本
发明涉及目标跟踪
定位领域,具体涉及一种基于多观测者测速信息的分布式目标跟踪方法。
背景技术
[0002] 目标跟踪问题源于雷达站的出现,1937年二战前夕第一部跟踪雷达站SCR-28在英国的出现标志着目标跟踪问题的正式出现,之后相继发展了各种跟踪技术手段,包括雷达、激光、红外、声纳等目标跟踪系统,这些技术手段在日后的发展中又不断得到完善。早前,人们并不熟知目标跟踪技术,直到70年代,在目标跟踪领域成功应用了卡尔曼滤波理论之后,人们才普遍关注了机动目标跟踪技术,并产生极大兴趣。之后,机动目标跟踪技术发展迅速,现已成为国内外研究最热
门的领域之一。近几十年来,为适应科技和应用的需要,相继出现和发展了
扩展卡尔曼滤波、多模型
算法和粒子滤波等新技术。而随着时代的发展,相关
传感器技术也在不断进步。目前的目标跟踪传感器除了能获取目标的相对距离、相对
角度等信息外,还可以对目标的相对轴向速度(目标-观测者连线方向上的速度分量)进行实时测量,获取目标的相对轴向速度。
发明内容
[0003] 为进一步强化目标跟踪性能,本发明将相对轴向速度信息引入目标跟踪系统,可以极大的提升对于目标的跟踪能
力与跟踪
精度;同时,采用多观测者协同观测的模式对同一目标进行跟踪,利用多观测者的信息可以更加有效的提高目标跟踪的精度与
稳定性,以实现在存在多观测者测速信息情况下的分布式目标跟踪定位。
[0004] 本发明的目的是通过以下技术方案实现的:
[0005] 一种基于多观测者测速信息的分布式目标跟踪方法,包括以下步骤:
[0006] 步骤1:本地目标定位阶段:每个观测者利用自身接收到的目标相对角度和目标相对距离信息对目标的
位置信息进行估计,得到自身本地定位结果;
[0007] 步骤2:信息通信阶段:每个观测者将步骤1中的自身本地定位结果发送至融合中心,融合中心在综合所有观测者信息后,得到全局定位结果,并将该结果反馈至每一个观测者;
[0008] 步骤3:本地目标跟踪阶段:利用步骤2中反馈得到的全局定位结果与自身所观测到的相对轴向速度信息对目标再次进行目标跟踪计算,得到目标的自身本地跟踪结果;
[0009] 步骤4:信息通信阶段:每个观测者将步骤3中的自身本地跟踪结果发送至融合中心,融合中心在综合所有观测者信息后,得到全局跟踪结果,并将该结果反馈至每一个观测者;
[0010] 步骤5:结束当前步的目标跟踪计算,并将目标跟踪结果输出。
[0011] 进一步的,步骤1中得到所述自身本地定位结果的具体方法为:
[0012] 利用目标定位模型,将k时刻的观测者a所敏感到的ra(k),θa(k), 作为滤波输入,利用扩展kalman滤波算法(EKF)进行滤波估计,得到k时刻目标定位阶段的观测者a的本地目标定位结果 与其对应的定位误差协方差矩阵Pa(k)。以观测者a为例,其余观测者依此类推。
[0013] 进一步的,所述目标定位模型为:
[0014] x1(k+1)=Φ1x1(k)+w1(k)
[0015] z1(k)=H1(x1(k))+r1(k)
[0016] 对应的状态向量与状态转移矩阵为:
[0017]
[0018] 依序为X,Y,Z轴下的目标三轴位置与目标三轴速度;
[0019]
[0020] T为系统
采样间隔,状态误差w(k)为对应维数的零均值高斯白噪声;
[0021] 其对应的观测向量为: 即目标相对距离、目标相对方位角、目标相对
俯仰角;对应的
观测矩阵为:
[0022]
[0023] r1(k)为对应维数的量测噪声,为零均值高斯白噪声;为k时刻,测试者a自身三轴位置。
[0024] 进一步的,步骤2中所述信息通信阶段全部观测者均将自身的定位结果与对应的定位误差协方差矩阵发送至融合中心,在k时刻的计算中,融合中心可以接收到如下信息:Pa(k), Pb(k), Pc(k);
[0025] 基于上述信息,做出如下融合计算:
[0026]
[0027] P(k)=((Pa(k))-1+(Pb(k))-1+Pc(k)-1)-1
[0028] 其中, 为融合后的定位结果,P(k)为融合后的定位结果的定位误差的协方差矩阵;在此
基础上,融合中心将 与P(k)反馈至观测者a,b,c。
[0029] 进一步的,步骤3中所述的目标跟踪计算方法为:
[0030] 首先,利用步骤2中反馈得到的全局定位结果对目标相对角度进行估计;
[0031] 然后,利用目标跟踪模型,将k时刻的观测者a所敏感到的dva(k)作为滤波输入,利用上述目标跟踪模型进行EKF计算,得到k时刻目标跟踪阶段的观测者a的本地目标跟踪结果 与其对应的跟踪误差协方差矩阵
[0032] 进一步的,步骤3中所述利用步骤2中反馈得到的全局定位结果对目标相对角度进行估计的具体方法为:以观测者a为例,其余观测者依此类推;
[0033] θ1和 分别为计算得到的相对方位角与相对俯仰角,均由 计算得到,即:
[0034]
[0035]
[0036] 其中 与 分别为 中的第1个第2个和第3个元素。
[0037] 进一步的,步骤3中所述的目标跟踪模型为:
[0038] x2(k+1)=Φ2x2(k)+w2(k)
[0039] z2(k)=H2(x2(k))+r2(k)
[0040] 对应的状态向量与状态转移矩阵为:
[0041]
[0042] 依序为X,Y,Z轴下的目标三轴速度与目标三轴
加速度;
[0043]
[0044] 其对应的观测向量为:Z2(k)=[dv]T,即目标相对轴向速度;对应的观测矩阵为:
[0045]
[0046] 进一步的,步骤4中所述信息通信阶段全部观测者均将自身的跟踪结果与对应的跟踪误差协方差矩阵发送至融合中心,在k时刻的计算中,融合中心可以接收到如下信息:
[0047] 基于上述信息,做出如下融合计算:
[0048]
[0049]
[0050] 其中, 为融合后的跟踪结果,P2(k)为融合后的跟踪结果的定位误差的协方差矩阵,并由融合中心将其反馈至各观测者。
[0051] 本发明所述的目标跟踪算法的输入除了目标相对角度和目标相对距离信息之外,额外引入了目标的相对轴向速度;本发明所述的目标跟踪算法是一种多观测者存在情况下的分布式算法,目标跟踪的计算不仅在融合中心执行,同时在观测者本地执行;本发明所述的目标跟踪算法共包含两次跟踪计算与两次观测者与融合中心通信,其中第一次跟踪计算主要是对目标的位置进行估计,第一次通信的通信内容为第一次跟踪计算的结果;第二次跟踪计算主要是对目标的速度与加速度进行估计,第二次通信的通信内容为第二次跟踪计算的结果;算法的最终输出为第二次通信后的目标跟踪结果。
[0053] 1、本发明通过引入目标轴向速度观测量与引入多观测者的方式,与传统目标跟踪算法相比,显著提升了目标跟踪算法的跟踪精度与跟踪稳定性;
[0054] 2、本发明采用了分布式的算法架构,一方面降低了计算的复杂度,提升了运算效率;另一方面增强了目标跟踪系统的鲁棒性,有利于增强算法的稳定性;
[0055] 3、本发明采用双阶段计算的目标跟踪计算模式,有效的降低了计算的复杂度与待估计参数之间的耦合性,更加便于在工程实践中应用。
附图说明
[0056] 图1为本发明中所设计的分布式目标跟踪算法流程
框图;
[0057] 图2为本发明所述的分布式目标跟踪算法时序示意图;
[0058] 图3为本发明所述的分布式目标跟踪算法运行结果仿真示意图1;
[0059] 图4为本发明所述的分布式目标跟踪算法运行结果仿真示意图2;
[0060] 图5为本发明所述的分布式目标跟踪算法运行结果仿真示意图3;
[0061] 图6为本发明所述的分布式目标跟踪算法运行结果仿真示意图4。
[0062] 图7为本发明所述的分布式目标跟踪算法运行结果仿真示意图5;
[0063] 图8为本发明所述的分布式目标跟踪算法运行结果仿真示意图6。
具体实施方式
[0064] 下面结合附图并举
实施例,对本发明进行详细描述。
[0065] 本实施例提供了一种基于多观测者测速信息的分布式目标跟踪方法,所述方法的整体流程框图如图1所示,系统时序图如图2所示。
[0066] 为方便后续进行介绍,本实施例中做出如下定义:
[0067] 定义系统中共有3个观测者对目标进行观测,分别命名为a,b,c(需要说明的是,本发明并不仅限于3个观测者,本发明所述的方法亦可扩展至更多观测者存在的情况);在k时刻,其所获得的对应的观测信息依序为:
[0068] 其中,ra(k),θa(k), dva(k)依序分别为观测者a在k时刻所获得的目标相对距离、目标相对方位角、目标相对俯仰角以及目标相对轴向速度;
[0069] 其中,rb(k),θb(k), dvb(k)依序分别为观测者b在k时刻所获得的目标相对距离、目标相对方位角、目标相对俯仰角以及目标相对轴向速度;
[0070] 其中,ra(k),θa(k), dva(k)依序分别为观测者c在k时刻所获得的目标相对距离、目标相对方位角、目标相对俯仰角以及目标相对轴向速度。
[0071] 在k时刻,三个观测者自身的运动信息为:
[0072] 观测者a:自身三轴位置为 自身三轴速度为
[0073] 观测者b:自身三轴位置为 自身三轴速度为
[0074] 观测者c:自身三轴位置为 自身三轴速度为
[0075] 观测者自身的运动信息由自身所装备的
导航系统提供。
[0076] 步骤1:本地目标定位阶段:每个观测者利用自身接收到的目标相对角度和目标相对距离信息对目标的位置信息进行估计,得到自身本地定位结果。
[0077] 以观测者a的计算流程为例进行说明:
[0078] 首先构建如下的目标定位模型:
[0079] x1(k+1)=Φ1x1(k)+w1(k)
[0080] z1(k)=H1(x1(k))+r1(k)
[0081] 对应的状态向量与状态转移矩阵为:
[0082]
[0083] 依序为X,Y,Z轴下的目标三轴位置与目标三轴速度;
[0084]
[0085] T为系统采样间隔,状态误差w(k)为对应维数的零均值高斯白噪声。
[0086] 其对应的观测向量为: 即目标相对距离、目标相对方位角、目标相对俯仰角;对应的观测矩阵为:
[0087]
[0088] r1(k)为对应维数的量测噪声,为零均值高斯白噪声。
[0089] 利用上文所述的目标定位模型,将k时刻的观测者a所敏感到的ra(k),θa(k),作为滤波输入,利用扩展kalman滤波算法(EKF)进行滤波估计,得到k时刻目标定位阶段的观测者a的本地目标定位结果 与其对应的定位误差协方差矩阵Pa(k)。EKF算法为一种常用的非线性滤波算法,具体介绍如下:
[0090] 对于如下的非线性连离散系统:
[0091] Xk+1=f(Xk)+wk
[0092] Zk=h(Xk)+vk
[0093] 其中,Xk为系统状态,Zk为系统观测信息,随机变量wk和vk分别为系统的过程噪声和测量噪声,且为相互独立的零均值高斯白噪声。其中,过程噪声的方差为Q,量测噪声的方差为R。f为
状态方程中与系统状态变量Xk和时间k相关的非线性函数;h为观测方程中与状态变量Xk和时间k相关的非线性函数。
[0094] 对上述模型进行应用EKF算法:
[0095]
[0096] Pk,k-1=APk-1AT+Q
[0097] Kk=Pk,k-1HT[HPk,k-1HT+R]-1
[0098]
[0099] Pk=[I-KkH]Pk,k-1
[0100] 其中, 代表上一步滤波结果, 代表状态一步预测,Pk,k-1代表一步预测误差方差,Kk代表滤波增益,Pk代表当前步滤波误差方差,Pk-1代表k-1步的滤波误差方差;I代表对应维度的单位矩阵,其中A和H分别代表f和h在 处的雅可比矩阵,雅可比矩阵是数学中常用的一种函数的一阶偏导数以一定方式排列成的矩阵。
[0101] 对观测者b和c同样执行上述操作,可以得到观测者b在本步中的定位结果与其对应的定位误差协方差矩阵Pb(k)以及观测者c在本步中的定位结果 与其对应的定位误差协方差矩阵Pc(k)。
[0102] 步骤2:信息通信阶段:每个观测者将步骤1中的自身本地定位结果发送至融合中心,融合中心在综合所有观测者信息后,得到全局定位结果,并将该结果反馈至每一个观测者。
[0103] 在完成步骤1的计算后,全部观测者均将自身本地定位结果与对应的定位误差协方差矩阵发送至融合中心,在k时刻的计算中,融合中心可以接收到如下信息: Pa(k), Pb(k), Pc(k)。此处是以3个观测者为例进行说明,需要说明的是该方法可以扩展至更多观测者存在的情况。
[0104] 基于上述信息,做出如下融合计算:
[0105]
[0106] P(k)=((Pa(k))-1+(Pb(k))-1+Pc(k)-1)-1
[0107] 其中, 为融合后的定位结果,P(k)为融合后的定位结果的定位误差的协方差矩阵。在此基础上,融合中心将 与P(k)反馈至观测者a,b,c。
[0108] 同时,为了进一步提升长时间下的目标精度与稳定性,每一个观测者均将自身的局部结果替换为全局结果,即:
[0109] 观测者a:
[0110] Pa(k)=P(k)
[0111] 观测者b:
[0112] Pb(k)=P(k)
[0113] 观测者c:
[0114] Pc(k)=P(k)。
[0115] 步骤3:本地目标跟踪阶段:利用步骤2中反馈得到的全局定位结果与自身所观测到的相对轴向速度信息对目标再次进行目标跟踪计算,得到目标的本地跟踪结果。
[0116] 以观测者a的计算流程为例进行说明:
[0117] 首先利用步骤2中反馈得到的定位结果对目标相对角度进行估计,如下:
[0118] θ1和 分别为计算得到的相对方位角与相对俯仰角,均由 计算得到,即:
[0119]
[0120]
[0121] 其中 与 分别为 中的第1个第2个和第3个元素。
[0122] 基于上述计算结果,介绍目标跟踪模型如下:
[0123] x2(k+1)=Φ2x2(k)+w2(k)
[0124] z2(k)=H2(x2(k))+r2(k)
[0125] 对应的状态向量与状态转移矩阵为:
[0126]
[0127] 依序为X,Y,Z轴下的目标三轴速度与目标三轴加速度;
[0128]
[0129] 其对应的观测向量为:Z2(k)=[dv]T,即目标相对轴向速度;对应的观测矩阵为:
[0130]
[0131] 利用上述模型,将k时刻的观测者a所敏感到的dva(k)作为滤波输入,利用上述目标跟踪模型进行EKF计算,得到k时刻目标跟踪阶段的观测者a的本地目标跟踪结果与其对应的跟踪误差协方差矩阵
[0132] 对观测者b和c同样执行上述操作,可以得到观测者b在本步中的跟踪结果与其对应的跟踪误差协方差矩阵 以及观测者c在本步中的跟踪结果 与其对应的跟踪误差协方差矩阵
[0133] 步骤4:信息通信阶段:每个观测者将步骤3中的自身本地跟踪结果发送至融合中心,融合中心在综合所有观测者信息后,得到全局跟踪结果,并将该结果反馈至每一个观测者。
[0134] 在完成步骤3的计算后,全部观测者均将自身的跟踪结果与对应的跟踪误差协方差矩阵发送至融合中心,在k时刻的计算中,融合中心可以接收到如下信息:此处是以3个观测者为例进
行说明,需要说明的是该方法可以扩展至更多观测者存在的情况。
[0135] 基于上述信息,做出如下融合计算:
[0136]
[0137]
[0138] 其中, 为融合后的跟踪结果,P2(k)为融合后的跟踪结果的定位误差的协方差矩阵,并由融合中心将其反馈至各观测者。
[0139] 同时,为了进一步提升长时间下的目标精度与稳定性,每一个观测者均将自身的局部结果替换为全局结果,即:
[0140] 观测者a:
[0141]
[0142] 观测者b:
[0143]
[0144] 观测者c:
[0145]
[0146] 则最终得到k时刻的目标跟踪结果为:
[0147] 目标三轴位置:
[0148] 目标三轴速度:
[0149] 目标三轴加速度:
[0150] 其中, 依序代表 中的第1个,第2个和第3个元素; 依序代表
中的第1个,第2个,第3个,第4个,第5个和第6个元素。
[0151] 将步骤5得到的滤波结果作为最终的目标跟踪结果进行输出,最终输出结果如图3至图7所示,其中图3为三轴位置真实值与跟踪结果示意图,图4为三轴跟踪位置误差示意图,图5为三轴速度真实值与跟踪结果示意图,图6为三轴跟踪速度误差示意图,图7为三轴加速度真实值与跟踪结果示意图,图8为三轴加速度跟踪误差示意图。从上述仿真图形可以看出,利用本发明所设计的目标跟踪算法,不仅三轴向的
位置跟踪结果与速度跟踪结果均可以快速准确的收敛至真实值附近,而且三轴的加速度跟踪结果也可实现对真实加速度的准确长时跟踪,也就是说利用该算法可以有效实现对目标的准确定位跟踪。
[0152] 综上所述,以上仅为本发明的较佳实施例而已,并非用于限定本发明的保护范围。凡在本发明的精神和原则之内,所作的任何
修改、等同替换、改进等,均应包含在本发明的保护范围之内。