CN114063059B - 一种航迹修正方法、设备及存储介质 - Google Patents

一种航迹修正方法、设备及存储介质 Download PDF

Info

Publication number
CN114063059B
CN114063059B CN202111356248.8A CN202111356248A CN114063059B CN 114063059 B CN114063059 B CN 114063059B CN 202111356248 A CN202111356248 A CN 202111356248A CN 114063059 B CN114063059 B CN 114063059B
Authority
CN
China
Prior art keywords
track
matrix
vehicle
determining
target vehicle
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.)
Active
Application number
CN202111356248.8A
Other languages
English (en)
Other versions
CN114063059A (zh
Inventor
许孝勇
顾超
陈俊德
仇世豪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Hurys Intelligent Technology Co Ltd
Original Assignee
Nanjing Hurys Intelligent Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nanjing Hurys Intelligent Technology Co Ltd filed Critical Nanjing Hurys Intelligent Technology Co Ltd
Priority to CN202111356248.8A priority Critical patent/CN114063059B/zh
Publication of CN114063059A publication Critical patent/CN114063059A/zh
Application granted granted Critical
Publication of CN114063059B publication Critical patent/CN114063059B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
    • G01S13/723—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar by using numerical data
    • G01S13/726—Multiple target tracking
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/40—Means for monitoring or calibrating

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本申请公开了一种航迹修正方法、设备及存储介质，该方法包括：确定目标车辆在第一时刻的第一航迹状态向量以及第一航迹状态协方差矩阵；基于状态转移矩阵确定第二时刻的第二航迹状态向量预测值以及第二航迹状态协方差矩阵；基于第二航迹状态向量预测值以及第二时刻探测区域内各车辆的量测，确定目标车辆的航迹与各车辆的量测之间的关联矩阵；基于关联矩阵确定航迹与各车辆的量测的相似度矩阵；然后基于相似度矩阵确定航迹状态修正量，利用航迹状态修正量对第二航迹状态向量预测值进行修正。利用目标车辆的航迹与各车辆的量测之间的相似度，对航迹预测值进行修正，使下一时刻对目标车辆的航迹预测更加准确，提高对目标车辆跟踪的准确性。

Description

一种航迹修正方法、设备及存储介质
技术领域
本申请涉及数据处理技术领域，尤其涉及一种航迹修正方法、设备及存储介质。
背景技术
随着社会经济的快速发展，私家车的数量越来越多，峰值时段交通流逐渐趋于饱和。为了减少峰值时段交通事故的发生，改善交通状况，雷达监测技术逐渐进入大众视野，利用雷达对目标车辆进行跟踪的技术手段可应用于交通业务。
跟踪是指雷达对目标的量测进行处理，以便实现对目标状态的估计。量测是指与目标状态有关的观测值，所以量测也可以称为测量或观测。航迹是由来自同一个目标的量测集合所估计的目标状态而形成的运动轨迹，即跟踪轨迹。
对于车辆比较密集的场景，雷达在某一时刻获取大量的量测，在进行量测与航迹关联时，可能会出现一个航迹关联多个量测，导致在进行航迹预测时出现错误，无法准确地跟踪车辆。
发明内容
本申请实施例提供了一种航迹修正方法、设备及存储介质，以便提高对车辆跟踪的准确性。
第一方面，本申请实施例提供了一种航迹修正方法，所述方法包括：
确定目标车辆在第一时刻的第一航迹状态向量以及第一航迹状态协方差矩阵；
基于状态转移矩阵以及所述第一航迹状态向量，确定所述目标车辆在第二时刻的第二航迹状态向量预测值；
基于所述状态转移矩阵以及所述第一航迹状态协方差矩阵，确定所述目标车辆在所述第二时刻的第二航迹状态协方差矩阵；
基于所述第二航迹状态向量预测值以及所述第二时刻探测区域内各车辆的量测，确定所述目标车辆的航迹与所述各车辆的量测之间的关联矩阵；
基于所述关联矩阵、所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定所述航迹与所述各车辆的量测之间的相似度矩阵；
基于所述相似度矩阵、所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定航迹状态修正量；
利用所述航迹状态修正量对所述第二航迹状态向量预测值进行修正。
在一种可能的实施方式中，所述基于所述第二航迹状态向量预测值以及所述第二时刻探测区域内各车辆的量测，确定所述目标车辆的航迹与所述各车辆的量测之间的关联矩阵，包括：
确定所述航迹的椭圆波门，判断所述各车辆的量测是否属于所述椭圆波门；
针对任一车辆的量测，当所述量测属于所述椭圆波门时，将所述航迹与所述量测的关联值记为第一参数；
当所述量测不属于所述椭圆波门时，将所述航迹与所述量测的关联值记为第二参数。
在一种可能的实施方式中，基于所述关联矩阵、所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定所述航迹与所述各车辆的量测之间的相似度矩阵，包括：
基于所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定所述航迹与所述各车辆的量测之间的马氏距离以及航迹预测不确定因子；
基于所述马氏距离、所述航迹预测不确定因子以及所述关联矩阵，确定所述航迹与所述各车辆的量测之间的相似度矩阵。
在一种可能的实施方式中，所述基于所述马氏距离、所述航迹预测不确定因子以及所述关联矩阵，确定所述航迹与所述各车辆的量测之间的相似度矩阵，包括：
基于所述马氏距离、所述航迹预测不确定因子以及所述关联矩阵，确定所述航迹与所述各车辆的量测之间的归属度矩阵；
基于所述归属度矩阵，确定所述航迹与所述各车辆的量测之间的相似度矩阵。
在一种可能的实施方式中，所述基于所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定所述航迹与所述各车辆的量测之间的马氏距离以及航迹预测不确定因子，包括：
基于所述第二航迹状态向量预测值以及所述各车辆的量测，确定所述航迹与所述各车辆的量测之间的状态残差矩阵；
基于所述第二航迹状态协方差矩阵，确定所述航迹与所述各车辆的量测之间的状态残差协方差矩阵；
基于所述状态残差矩阵以及所述状态残差协方差矩阵，确定所述航迹与所述各车辆的量测之间的马氏距离；
基于所述状态残差协方差矩阵，确定所述航迹预测不确定因子。
在一种可能的实施方式中，所述基于所述相似度矩阵、所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定航迹状态修正量，包括：
基于所述状态残差矩阵以及所述相似度矩阵，确定航迹状态修正因子；
基于所述第二航迹状态协方差矩阵，确定航迹状态修正增益；
基于所述航迹状态修正因子以及所述航迹状态修正增益，确定所述航迹状态修正量。
在一种可能的实施方式中，所述方法还包括：
利用所述航迹状态修正增益以及所述相似度矩阵对所述第二航迹状态协方差矩阵进行修正。
在一种可能的实施方式中，所述方法还包括：
当所述航迹的关联矩阵中存在所述第一参数时，在所述航迹的关联记录中添加关联标记；
当所述航迹的关联矩阵中均为所述第二参数时，在所述航迹的关联记录中添加无关联标记；
在预设时间内，当所述航迹的关联记录中所述关联标记的个数超过第一预设个数时，输出所述航迹，以便利用输出的所述航迹跟踪所述目标车辆；
在所述预设时间内，当所述航迹的关联记录中所述无关联标记的个数超过第二预设个数时，删除所述航迹。
第二方面，本申请实施例提供了一种航迹修正设备，所述设备包括：存储器以及处理器；
所述存储器，用于存储相关的程序代码；
所述处理器，用于调用所述程序代码，执行上述第一方面任意一种实施方式所述的航迹修正方法。
第三方面，本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质用于存储计算机程序，所述计算机程序用于执行上述第一方面任意一种实施方式所述的航迹修正方法。
在本申请实施例的上述实现方式中，首先确定目标车辆在第一时刻的第一航迹状态向量以及第一航迹状态协方差矩阵；基于状态转移矩阵以及第一航迹状态向量，确定目标车辆在第二时刻的第二航迹状态向量预测值；基于状态转移矩阵以及第一航迹状态协方差矩阵，确定目标车辆在第二时刻的第二航迹状态协方差矩阵；基于第二航迹状态向量预测值以及第二时刻探测区域内各车辆的量测，确定目标车辆的航迹与各车辆的量测之间的关联矩阵；基于关联矩阵、第二航迹状态向量预测值、各车辆的量测以及第二航迹状态协方差矩阵，确定航迹与各车辆的量测之间的相似度矩阵；然后基于相似度矩阵、第二航迹状态向量预测值、各车辆的量测以及第二航迹状态协方差矩阵，确定航迹状态修正量，利用航迹状态修正量对第二航迹状态向量预测值进行修正。本申请实施例提供的航迹修正方法，利用目标车辆的航迹与各车辆的量测之间的相似度，对航迹预测值进行修正，使下一时刻对目标车辆的航迹预测更加准确，提高对目标车辆跟踪的准确性。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见，下面描述中的附图仅仅是本申请中提供的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。
图1为本申请实施例中一种航迹修正方法的流程图；
图2为本申请实施例中一种航迹修正设备的示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整的描述，所描述的实施例仅为本申请示例性的实施方式，并非全部实现方式。本领域技术人员可以结合本申请的实施例，在不进行创造性劳动的情况下，获得其他的实施例，而这些实施例也在本申请的保护范围之内。
当雷达照射到物体表面时，所反射的电磁波会携带方位、距离等信息。由于扫描极为精细，则能够得到大量的回波，因而即可形成点云。
针对任一车辆，假设雷达探测到的该车辆的点云为5个点，通过聚类算法把这5个点聚类为一个簇，然后求这个簇的中心点的坐标，中心点坐标即为车辆的量测。
在雷达的探测区域内，会监测到多个车辆，将雷达探测到的一帧点云聚类后，会聚成多个簇，得到多个量测。现有的雷达跟踪算法中，对于车辆比较密集的场景，雷达在同一时刻可以获取大量的量测，将目标车辆的航迹与量测关联时，可能会出现一个航迹关联多个量测的情况，导致对航迹预测时出现错误，无法准确地跟踪车辆。
基于此，本申请实施例提供了一种航迹修正方法，以便提高对车辆跟踪的准确性。具体实现时，首先确定目标车辆在第一时刻的第一航迹状态向量和第一航迹状态协方差矩阵；基于状态转移矩阵以及第一航迹状态向量，确定目标车辆在第二时刻的第二航迹状态向量预测值；基于状态转移矩阵以及第一航迹状态协方差矩阵，确定目标车辆在第二时刻的第二航迹状态协方差矩阵；基于第二航迹状态向量预测值以及第二时刻探测区域内各车辆的量测，确定目标车辆的航迹与各车辆的量测之间的关联矩阵；基于关联矩阵、第二航迹状态向量预测值、各车辆的量测以及第二航迹状态协方差矩阵，确定航迹与各车辆的量测之间的相似度矩阵；然后基于相似度矩阵、第二航迹状态向量预测值、各车辆的量测以及第二航迹状态协方差矩阵，确定航迹状态修正量，利用航迹状态修正量对第二航迹状态向量预测值进行修正。本申请实施例提供的航迹修正方法，利用目标车辆的航迹与各车辆的量测之间的相似度，对目标车辆的航迹预测值进行修正，使下一时刻对目标车辆的航迹预测更加准确，提高对目标车辆跟踪的准确性。
参见图1，该图为本申请实施例提供的一种航迹修正方法的流程图。
该方法主要包括以下步骤：
S101：确定目标车辆在第一时刻的第一航迹状态向量以及第一航迹状态协方差矩阵。
首先确定目标车辆在第一时刻的第一航迹状态向量以及第一航迹状态协方差矩阵，其中，第一航迹状态向量用Xj(k-1|k-1)来表示，第一航迹状态向量Xj(k-1|k-1)＝(x0,vx0,y0,vy0)T，k-1时刻即表示第一时刻，j∈[1,t]，t为航迹的个数，x0和y0分别表示笛卡尔坐标系下x轴和y轴的坐标值，vx0和vy0分别表示x轴和y轴方向的速度。
由于在雷达的探测区域内，可以监测到的车辆不止一个，所以雷达跟踪的目标车辆也可能不止一个，为了方便描述，在本申请实施例中，Xj即代表任一目标车辆的航迹状态向量。
然后根据第一航迹状态向量Xj(k-1|k-1)计算得到第一航迹状态协方差矩阵Pj(k-1|k-1)。
在另一种可能的实现方式中，如果航迹与量测关联的过程中，发现量测不是现有航迹的雷达回波，比如，有新的目标车辆进入雷达探测区域，需要为目标车辆生成新的航迹，可以利用卡尔曼滤波中的航迹生成方法生成新的航迹。
S102：基于状态转移矩阵以及第一航迹状态向量，确定目标车辆在第二时刻的第二航迹状态向量预测值。
在本实施例中，可以用F表示状态转移矩阵，目标车辆在第二时刻的第二航迹状态向量预测值可以用Xj(k|k-1)来表示，k时刻即表示第二时刻，Xj(k|k-1)表示基于k-1时刻的航迹状态向量预测得到k时刻的第二航迹状态向量。
在一种可能的实现方式中，Xj(k|k-1)＝F*Xj(k-1|k-1)，其中，Xj(k|k-1)＝(x1,vx1,y1,vy1)T
S103：基于状态转移矩阵以及第一航迹状态协方差矩阵，确定目标车辆在第二时刻的第二航迹状态协方差矩阵。
目标车辆在第二时刻的第二航迹状态协方差矩阵可以用Pj(k|k-1)来表示。考虑到雷达对目标车辆的测量中可能会受到噪声污染，本实施例提供一种优选的实现方式，Pj(k|k-1)＝F*Pj(k-1|k-1)+Q，其中，Q表示过程噪声协方差矩阵。
S104：基于第二航迹状态向量预测值以及第二时刻探测区域内各车辆的量测，确定目标车辆的航迹与各车辆的量测之间的关联矩阵。
本申请实施例针对任一目标车辆的航迹进行修正，正常情况下，一个航迹关联一个量测，当雷达的探测区域内监测到多个车辆时，很有可能导致目标车辆的航迹关联到多个量测，所以需要通过确定各车辆的量测与目标车辆的航迹的关联程度，对目标车辆的航迹进行修正，使雷达更准确地跟踪目标车辆。
第二时刻探测区域内各车辆的量测用Zi(k)来表示，Zi(k)＝(x2,y2)T，其中，i∈[1,m]，m表示量测的个数，x2和y2分别表示量测在x轴和y轴的坐标值。根据Xj(k|k-1)和Zi(k)，计算目标车辆的航迹与各车辆的量测之间的关联矩阵。
交通场景下车辆的点云分布基本符合椭圆形，并且椭圆区域会随车辆转向而相应旋转，在一种可能的实现方式中，可以根据目标车辆的航迹，确定航迹的椭圆波门，根据量测是否属于该椭圆波门，确定航迹与量测的关联矩阵W。
具体地，可以以第二航迹状态向量预测值Xj(k|k-1)中的坐标值x1、y1作为椭圆波门的中心点，a表示预先设置的椭圆波门的短半轴长度，b表示预先设置的椭圆波门的长半轴长度。
针对任一量测Zi(k)＝(x2,y2)T以及目标车辆的第二航迹状态向量预测值Xj(k|k-1)＝(x1,vx1,y1,vy1)T，关联矩阵W中的元素可以用wij来表示，wij的计算公式为：
其中，i∈[1,m]，j∈[1,t]，θj表示航迹的航向角，θj＝cot(-vx1/vy1)。
当量测属于航迹的椭圆波门时，wij记为第一参数；当量测不属于航迹的椭圆波门时，wij记为第二参数。为了方便表示，可以将第一参数表示为1，将第二参数表示为0。计算完成航迹与各量测对应的wij之后，即得到航迹与各车辆的量测之间的关联矩阵W。
S105：基于关联矩阵、第二航迹状态向量预测值、各车辆的量测以及第二航迹状态协方差矩阵，确定航迹与各车辆的量测之间的相似度矩阵；
计算得到航迹与各量测的关联矩阵W之后，可以根据关联矩阵W、第二航迹状态向量预测值Xj(k|k-1)、各车辆的量测Zi(k)以及第二航迹状态协方差矩阵Pj(k|k-1)，确定航迹与各车辆的量测之间的相似度矩阵Tij(k)。
下面将结合具体实施例，介绍计算相似度矩阵Tij(k)的两种可能的方式。
一种可能的实现方式为，根据第二航迹状态向量预测值Xj(k|k-1)、各车辆的量测Zi(k)以及第二航迹状态协方差矩阵Pj(k|k-1)，计算得到航迹与各车辆的量测之间的马氏距离Lij(k)以及航迹预测不确定因子Mij(k)，马氏距离通常表示点与一个分布之间的距离，是一种计算两个未知样本集的相似度的方法，不确定因子代表了对航迹预测的不确定性程度。
基于马氏距离Lij(k)、航迹预测不确定因子Mij(k)以及关联矩阵W，计算得到航迹与各车辆的量测之间的归属度矩阵Gij(k)，将计算得到的归属度矩阵Gij(k)作为航迹与各车辆的量测之间的相似度矩阵Tij(k)。例如，归属度矩阵Gij(k)的一种计算方式为：Gij(k)＝wij/(Lij(k)+Mij(k))，其中，wij∈W。
在计算马氏距离Lij(k)以及航迹预测不确定因子Mij(k)时，一种可能的计算方式为，首先基于第二航迹状态向量预测值Xj(k|k-1)以及所述各车辆的量测Zi(k)，计算航迹与各车辆的量测之间的状态残差矩阵Vij(k)，并且满足Vij(k)＝Zi(k)-HXj(k|k-1)，其中，H为观测矩阵。
然后基于第二航迹状态协方差矩阵Pj(k|k-1)，计算航迹与各车辆的量测之间的状态残差协方差矩阵Cij(k)，满足Cij(k)＝HPj(k|k-1)H4+R，其中，H为观测矩阵，R为观测噪声协方差矩阵。
基于状态残差矩阵Vij(k)和状态残差协方差矩阵Cij(k)，计算航迹与各车辆的量测之间的马氏距离Lij(k)，满足Lij(k)＝Vij(k)TCij(k)-1Vij(k)。
基于状态残差协方差矩阵Cij(k)计算航迹预测不确定因子Mij(k)，满足Mij(k)＝ln(|Cij(k)|+1)，其中，|Cij(k)|表示Cij(k)的行列式，所以可以得到归属度矩阵Gij(k)的计算公式为：
Gij(k)＝wij/(Vij(k)TCij(k)-1Vij(k)+ln(|Cij(k)|+1))，并将得到的归属度矩阵Gij(k)作为航迹与各量测之间的相似度矩阵Tij(k)。
上述实施例中，将得到的归属度矩阵作为航迹与各量测之间的相似度，为了更准确地衡量航迹与各量测之间的相似度，本申请实施例还提供一种优选的实现方式，即另一种计算相似度矩阵Tij(k)的方式。
由上述实施例计算得到航迹与各量测之间的归属度矩阵Gij(k)之后，基于归属度矩阵Gij(k)计算相似概率矩阵Sij(k)，相似概率矩阵Sij(k)的计算公式为：Sij(k)＝Gij(k)/(∑iGij(k)+∑jGij(k)-Gij(k))，并将得到的相似概率矩阵Sij(k)作为航迹与各量测之间的相似度矩阵Tij(k)，即满足：
Tij(k)＝Gij(k)/(∑iGij(k)+∑jGij(k)-Gij(k))。
S106：基于相似度矩阵、第二航迹状态向量预测值、各车辆的量测以及第二航迹状态协方差矩阵，确定航迹状态修正量；
基于计算得到的相似度矩阵Tij(k)、第二航迹状态向量预测值Xj(k|k-1)、各车辆的量测Zi(k)以及第二航迹状态协方差矩阵Pj(k|k-1)，确定航迹状态修正量Yj(k)。
一种可能的实现方式为，根据上述计算得到的状态残差矩阵Vij(k)和相似度矩阵Tjj(k)，确定航迹状态修正因子Vj(k)，根据第二航迹状态协方差矩阵Pj(k|k-1)，确定航迹状态修正增益Kj(k)，然后根据航迹状态修正因子Vj(k)和航迹状态修正增益Kj(k)，确定航迹状态修正量Yj(k)，航迹状态修正量Yj(k)一种可能的计算方式为：Yj(k)＝Kj(k)Vj(k)。
进一步地，根据上述状态残差矩阵Vij(k)和相似度矩阵Tij(k)，确定航迹状态修正因子Vj(k)时，一种可能的计算方式为：Vj(k)＝∑iTij(k)Vij(k)。根据第二航迹状态协方差矩阵Pj(k|k-1)，确定航迹状态修正增益Kj(k)时，一种可能的计算方式为：Kj(k)＝Pj(k|k-1)HTI(k)-1，其中，I(k)为新息协方差矩阵，新息协方差矩阵I(k)＝HPj(k|k-1)+R，H为观测矩阵，R为观测噪声协方差矩阵。所以基于航迹状态修正因子Vj(k)和航迹状态修正增益Kj(k)，确定的航迹状态修正量Yj(k)满足：Yj(k)＝Pj(k|k-1)HTI(k)-1∑iTij(k)Vij(k)。
S107：利用航迹状态修正量对第二航迹状态向量预测值进行修正。
确定航迹状态修正量Yj(k)后，对第二航迹状态向量预测值Xj(k|k-1)进行修正，修正后的第二航迹状态向量预测值Xj(k)＝Xj(k|k-1)+Yj(k)。
本实施例提供的航迹修正方法，基于航迹与各量测之间的相似度，对预测的航迹状态进行修正，然后利用修正后的航迹状态，更准确地跟踪目标车辆。
由于在下一时刻，对航迹进行预测及修正的过程中，会利用上一时刻的航迹状态协方差，进一步地，本申请实施例还可以对第二航迹状态协方差矩阵Pj(k|k-1)进行修正。
一种可能的实现方式为，基于上述实施例计算得到的航迹状态修正增益Kj(k)以及相似度矩阵Tij(k)，对第二航迹状态协方差矩阵Pj(k|k-1)进行修正，修正后的第二航迹状态协方差矩阵Pj(k)的计算公式为：
Pj(k)＝T0j(k)Pj(k|k-1)+[1-T0j(k)][E-Kj(k)HPj(k|k-1)+Ψ(k)]，其中，T0j(k)＝1-∑iTij(k)，表示所有量测都不在航迹的波门内的相似度，Ψ(k)计算公式为：Ψ(k)＝Kj(k)[∑iTij(k)Vij(k)Vij(k)T-Vj(k)Vj(k)T]Kj(k)T，E为单位矩阵。
基于上述实施例得到航迹与各量测的关联矩阵，可以对目标车辆的航迹状态进行确认。目标车辆的航迹均包括一个关联记录，关联记录代表了航迹是否有量测关联。关联记录可以为表格的形式，也可以保存在数据库中，本申请实施例并不限定关联记录的具体形式。
在航迹的关联矩阵中，如果存在第一参数，则表明存在与航迹关联的量测，可以在航迹的关联记录中添加关联标记；如果关联矩阵中均为第二参数，则表明没有与航迹关联的量测，可以在航迹的关联记录中添加无关联标记。
雷达周期性地采集探测区域内的车辆信息，可以设置一段预设时间，根据预设时间内目标车辆的航迹所更新的关联记录，对航迹进行确认。当关联记录中关联标记的个数超过第一预设个数时，输出该航迹，以便利用输出的航迹跟踪目标车辆；当关联记录中无关联标记的个数超过第二预设个数时，删除该航迹。其中，第一预设个数和第二预设个数可以根据实际场景进行设置。
下面将结合一种具体的实施方式，介绍确认航迹的方法。
在本实施例中，设置目标车辆的航迹的关联记录为长度为20帧数据的关联记录表。当航迹的关联矩阵中存在第一参数时，表示航迹在该帧中有关联的量测，可以在航迹的关联记录表的尾部加入1；当航迹的关联矩阵中均为第二参数时，表示航迹在该帧中无关联的量测，可以在航迹的关联记录表的尾部加入-1。
另外，如果航迹的关联记录表已经记录了20帧数据，可以删除关联记录表第一帧的数据，并在尾部加入当前帧的数据
根据预设时间内航迹关联记录表的数据，进行航迹确认。一种可能的实现方式为，判断最后m帧数据中1的个数是否达到n，即关联标记的个数是否达到n，当关联标记的个数达到n时，可以输出该航迹；判断最后a帧数据中-1的个数是否达到b，即无关联标记的个数是否达到b，当无关联标记的个数达到b时，删除该航迹。在输出航迹后，利用输出的航迹状态进行车辆跟踪。
基于上述方法实施例，本申请实施例还提供了一种航迹修正设备，参见图2，该图为本申请实施例提供的一种航迹修正设备的示意图。
该设备200包括：存储器201以及处理器202；
存储器201，用于存储相关的程序代码；
处理器202，用于调用所述程序代码，执行上述方法实施例所述的航迹修正方法。
此外，本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质用于存储计算机程序，所述计算机程序用于执行上述方法实施例所述的航迹修正方法。
需要说明的是，本申请中使用的术语“第一”和“第二”是用于区别类似的对象，而不用于描述特定顺序或先后次序。应该理解这样使用的术语在适当情况下可以互换，这仅仅是描述本申请的实施例中对相同属性的对象在描述时所采用的区分方式。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
以上所述仅是本申请示例性的实施方式，并非对本申请做任何形式上的限制。对以上实施例所做的等同变化或修改，均属于本申请的保护范围。

Claims (10)

1.一种航迹修正方法，其特征在于，所述方法包括：
确定目标车辆在第一时刻的第一航迹状态向量以及第一航迹状态协方差矩阵；
基于状态转移矩阵以及所述第一航迹状态向量，确定所述目标车辆在第二时刻的第二航迹状态向量预测值；
基于所述状态转移矩阵以及所述第一航迹状态协方差矩阵，确定所述目标车辆在所述第二时刻的第二航迹状态协方差矩阵；
基于所述第二航迹状态向量预测值以及所述第二时刻探测区域内各车辆的量测，确定所述目标车辆的航迹与所述各车辆的量测之间的关联矩阵；
基于所述关联矩阵、所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的相似度矩阵；
基于所述相似度矩阵、所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定航迹状态修正量；
利用所述目标车辆的航迹状态修正量对所述第二航迹状态向量预测值进行修正。
2.根据权利要求1所述的方法，其特征在于，所述基于所述第二航迹状态向量预测值以及所述第二时刻探测区域内各车辆的量测，确定所述目标车辆的航迹与所述各车辆的量测之间的关联矩阵，包括：
确定所述目标车辆的航迹的椭圆波门，判断所述各车辆的量测是否属于所述椭圆波门；
针对任一车辆的量测，当所述量测属于所述椭圆波门时，将所述目标车辆的航迹与所述量测的关联值记为第一参数；
当所述量测不属于所述椭圆波门时，将所述目标车辆的航迹与所述量测的关联值记为第二参数。
3.根据权利要求1所述的方法，其特征在于，基于所述关联矩阵、所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的相似度矩阵，包括：
基于所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的马氏距离以及航迹预测不确定因子；
基于所述马氏距离、所述航迹预测不确定因子以及所述关联矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的相似度矩阵。
4.根据权利要求3所述的方法，其特征在于，所述基于所述马氏距离、所述航迹预测不确定因子以及所述关联矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的相似度矩阵，包括：
基于所述马氏距离、所述航迹预测不确定因子以及所述关联矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的归属度矩阵；
基于所述归属度矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的相似度矩阵。
5.根据权利要求3或4所述的方法，其特征在于，所述基于所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的马氏距离以及航迹预测不确定因子，包括：
基于所述第二航迹状态向量预测值以及所述各车辆的量测，确定所述目标车辆的航迹与所述各车辆的量测之间的状态残差矩阵；
基于所述第二航迹状态协方差矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的状态残差协方差矩阵；
基于所述状态残差矩阵以及所述状态残差协方差矩阵，确定所述目标车辆的航迹与所述各车辆的量测之间的马氏距离；
基于所述状态残差协方差矩阵，确定所述航迹预测不确定因子。
6.根据权利要求5所述的方法，其特征在于，所述基于所述相似度矩阵、所述第二航迹状态向量预测值、所述各车辆的量测以及所述第二航迹状态协方差矩阵，确定航迹状态修正量，包括：
基于所述状态残差矩阵以及所述相似度矩阵，确定航迹状态修正因子；
基于所述第二航迹状态协方差矩阵，确定航迹状态修正增益；
基于所述航迹状态修正因子以及所述航迹状态修正增益，确定所述航迹状态修正量。
7.根据权利要求6所述的方法，其特征在于，所述方法还包括：
利用所述航迹状态修正增益以及所述相似度矩阵对所述第二航迹状态协方差矩阵进行修正。
8.根据权利要求2所述的方法，其特征在于，所述方法还包括：
当所述目标车辆的航迹的关联矩阵中存在所述第一参数时，在所述目标车辆的航迹的关联记录中添加关联标记；
当所述目标车辆的航迹的关联矩阵中均为所述第二参数时，在所述目标车辆的航迹的关联记录中添加无关联标记；
在预设时间内，当所述目标车辆的航迹的关联记录中所述关联标记的个数超过第一预设个数时，输出所述目标车辆的航迹，以便利用输出的所述目标车辆的航迹跟踪所述目标车辆；
在所述预设时间内，当所述目标车辆的航迹的关联记录中所述无关联标记的个数超过第二预设个数时，删除所述目标车辆的航迹。
9.一种航迹修正设备，其特征在于，所述设备包括：存储器以及处理器；
所述存储器，用于存储相关的程序代码；
所述处理器，用于调用所述程序代码，执行权利要求1至8任一项所述的方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于存储计算机程序，所述计算机程序用于执行权利要求1至8任一项所述的方法。
CN202111356248.8A 2021-11-16 2021-11-16 一种航迹修正方法、设备及存储介质 Active CN114063059B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111356248.8A CN114063059B (zh) 2021-11-16 2021-11-16 一种航迹修正方法、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111356248.8A CN114063059B (zh) 2021-11-16 2021-11-16 一种航迹修正方法、设备及存储介质

Publications (2)

Publication Number Publication Date
CN114063059A CN114063059A (zh) 2022-02-18
CN114063059B true CN114063059B (zh) 2023-10-03

Family

ID=80272916

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111356248.8A Active CN114063059B (zh) 2021-11-16 2021-11-16 一种航迹修正方法、设备及存储介质

Country Status (1)

Country Link
CN (1) CN114063059B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303692A (zh) * 2018-01-30 2018-07-20 哈尔滨工业大学 一种解多普勒模糊的多目标跟踪方法
CN110542885A (zh) * 2019-08-13 2019-12-06 北京理工大学 一种复杂交通环境下的毫米波雷达目标跟踪方法
CN111007495A (zh) * 2019-12-10 2020-04-14 西安电子科技大学 一种基于双重融合最大熵模糊聚类jpda的目标航迹优化方法
WO2021036367A1 (zh) * 2019-08-30 2021-03-04 深圳大学 一种基于测量分配的目标跟踪方法及装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
IN2012DN00691A (zh) * 2008-07-08 2015-06-19 Thales Sa

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303692A (zh) * 2018-01-30 2018-07-20 哈尔滨工业大学 一种解多普勒模糊的多目标跟踪方法
CN110542885A (zh) * 2019-08-13 2019-12-06 北京理工大学 一种复杂交通环境下的毫米波雷达目标跟踪方法
WO2021036367A1 (zh) * 2019-08-30 2021-03-04 深圳大学 一种基于测量分配的目标跟踪方法及装置
CN111007495A (zh) * 2019-12-10 2020-04-14 西安电子科技大学 一种基于双重融合最大熵模糊聚类jpda的目标航迹优化方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于卡尔曼滤波和多种信息融合的在线多目标跟踪算法;陈志鸿;黄立勤;;信息通信(第03期);全文 *
模糊聚类的多雷达航迹关联算法;张良;陶海军;杨钒;王惊晓;;现代防御技术(第06期);全文 *

Also Published As

Publication number Publication date
CN114063059A (zh) 2022-02-18

Similar Documents

Publication Publication Date Title
CN111178385B (zh) 一种鲁棒在线多传感器融合的目标跟踪方法
CN109635816B (zh) 车道线生成方法、装置、设备以及存储介质
CN111292275B (zh) 基于复杂地面的点云数据滤除方法、装置、计算机设备
CN111768430B (zh) 一种基于多特征级联匹配的高速公路外场车辆跟踪方法
CN115063454B (zh) 多目标跟踪匹配方法、装置、终端及存储介质
CN109782240A (zh) 一种基于递推修正的多传感器系统误差配准方法和系统
Li et al. Spatio-temporal trajectory simplification for inferring travel paths
CN111770436A (zh) 一种基于CSI的室内Wi-Fi定位方法、装置及存储介质
CN111259332B (zh) 一种杂波环境下的模糊数据关联方法及多目标跟踪方法
CN114216434B (zh) 用于机动测控站的目标确认方法、系统、设备和存储介质
CN114137562B (zh) 一种基于改进全局最邻近的多目标跟踪方法
CN114063059B (zh) 一种航迹修正方法、设备及存储介质
CN112612034B (zh) 基于激光帧和概率地图扫描的位姿匹配方法
CN111474516B (zh) 一种基于众包样本曲面拟合的多层次室内定位方法及系统
CN111274529B (zh) 一种鲁棒的高斯逆威沙特phd多扩展目标跟踪算法
KR20090093152A (ko) 물체의 위치 추적 장치 및 방법
CN112767545A (zh) 一种点云地图构建方法、装置、设备及计算机存储介质
CN117392268A (zh) 一种基于自适应结合cpd和icp算法的激光扫描建图方法及系统
KR101045348B1 (ko) 직각 좌표계에서 도플러 속도를 이용한 표적 추적 방법
CN112965076A (zh) 一种用于机器人的多雷达定位系统及方法
CN116952277A (zh) 一种基于人工智能和行为特征的潜在位置估算方法及系统
JP3448640B2 (ja) 車両追跡方法及び車両追跡装置
CN113204891A (zh) 基于指数平滑预测的dp-tbd算法跟踪方法及设备
CN112880659B (zh) 一种基于信息概率的融合定位方法
CN112987010A (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