CN114170274A - 目标跟踪方法和装置、电子设备和存储介质 - Google Patents

Abstract

本发明涉及目标跟踪方法和装置、电子设备和存储介质。所述方法包括：从来自多种传感器的各传感器数据帧中获得障碍物信息；依据所述障碍物信息进行轨迹匹配；对经匹配的障碍物和轨迹进行轨迹跟踪与更新，其中，依据所述障碍物信息进行轨迹匹配包括：判断所述障碍物是否之前就存在；当判断出所述障碍物之前就存在时，判断是否在之前预定数量的传感器数据帧中都存在；在判断出在之前预定数量的传感器数据帧都存在的情况下，判断所述障碍物与该预定数量的传感器数据帧形成的轨迹之间的距离是否小于预定距离，当判断出小于预定距离时，确认障碍物与该预定数量的传感器数据帧形成的轨迹相匹配。

Description

目标跟踪方法和装置、电子设备和存储介质

技术领域

本发明涉及自动驾驶技术。

背景技术

随着人工智能的进步，自动驾驶行业发展迅速。感知、决策规划、控制构成了自动驾驶的主要环节。

感知主要是利用车载传感器实时精准地检测周围环境，并为自动驾驶决策提供依据，是智能汽车首先需要解决的问题，感知的精确性以及稳健性直接决定了自动驾驶汽车能否高效地、无事故地、符合预期效果地通过行驶路段。

现在的技术，无法满足全天候应用和复杂多样的障碍物目标的精确检测。另外，目前对于障碍物的匹配速度还比较慢，影响对于障碍物的跟踪。

发明内容

本发明鉴于现有技术的以上问题，用以解决现有技术中存在的一项或更多项的问题，至少提供一种有益的选择。

依据本发明的一个方面，提供了一种目标跟踪方法，包括：从来自多种传感器的各传感器数据帧中获得障碍物信息；依据所述障碍物信息进行轨迹匹配；对经匹配的障碍物和轨迹进行轨迹跟踪，其中，依据所述障碍物信息进行轨迹匹配包括：判断所述障碍物是否之前就存在；当判断出所述障碍物之前就存在时，判断是否在之前预定数量的传感器数据帧中都存在；在判断出在之前预定数量的传感器数据帧都存在的情况下，判断所述障碍物与该预定数量的传感器数据帧形成的轨迹之间的距离是否小于预定距离；以及当判断出小于预定距离时，确定此障碍物与该轨迹相匹配。

根据本发明的另一个方面，提供了一种目标跟踪装置，其特征在于，包括障碍物信息获取单元，对来自多种传感器的传感器数据帧进行空间同步和数据融合，从各传感器数据帧中获得障碍物信息；轨迹匹配单元，依据所述障碍物信息进行轨迹匹配；轨迹跟踪单元，对经匹配的障碍物和轨迹进行轨迹跟踪与更新，其中，所述轨迹匹配单元包括：存在判断单元，判断所述障碍物是否之前就存在；连续存在判断单元，当判断出所述障碍物之前就存在时，判断是否在之前预定数量的传感器数据帧中都存在；距离判断单元，在判断出在之前预定数量的传感器数据帧都存在的情况下，判断所述障碍物与之前预定数量的传感器数据帧形成的轨迹之间的距离是否小于预定距离；以及匹配确定单元，当距离判断单元判断出所述障碍物与之前预定数量的的轨迹之间的距离小于预定距离时，确定此障碍物与之前预定数据帧形成的轨迹相匹配。

根据本发明的另一方面，提供了一种电子设备，包括：处理器；用于存储所述处理器可执行指令的存储器；其中，所述处理器被配置为执行所述指令，以实现本发明所述的方法。

根据本发明的另一方面，提供了一种计算机可读存储介质，所述可读存储介质上存储设备控制程序，当所述设备控制程序被处理器执行时，实现本发明任一项所述的方法。

根据本发明的实施方式，在进行匈牙利匹配等匹配算法之前，进行基于现有轨迹的预匹配，减少了计算复杂度，使得能够提高匹配速度，减少事故的发生。

根据本发明的实施方式，将多种传感器分别获得数据进行串行合并，合成的数据流的数据更新频率均大于单个或单一种传感器数据更新频率，从而能够提高跟踪结果的精度。

附图说明

结合附图可以更好地理解本发明。附图只是示意性的，不是对本发明的保护范围的限制。

图1是示出了依据本发明的一种实施方式的目标跟踪方法的示意性流程图。

图2是示出了依据本发明的一种实施方式的进行坐标系转换的示意图。

图3是示出了依据本发明的一种实施方式的进行数据融合的示意图。

图4是示出了依据本发明的一种实施方式判断障碍物是否与现存轨迹匹配的示意图。

图5是示出了依据本发明的一种实施方式的目标跟踪装置的示意性方框图。

图6示出了依据本发明的一种实施方式的轨迹匹配单元的示意性方框图。

具体实施方式

下面结合附图对本发明的具体实施方式进行说明。这些说明都是示例性的，旨在使本领域技术人员能够实现本发明的实施方式，不是对本发明的保护范围的限制。说明中也没有描述对于实际实施不可缺少，但是对于理解本发明无关的内容。

图1是示出了依据本发明的一种实施方式的目标跟踪方法的示意性流程图。如图1所示，依据本发明的一种实施方式，依据本发明的目标跟踪方法首先在步骤S100对来自多种传感器的传感器数据帧进行空间同步和数据融合，从各传感器数据帧中获得障碍物信息。雷达或摄像机按照各自的监控周期进行障碍物检测，例如进行拍照或扫描。在本说明书的上下文中，将雷达或摄像机按照监控周期每次发送来数据称为传感器数据帧。可以进行空间同步和数据融合来获得障碍物信息。这里所述的障碍物可以是车辆，所述的多种传感器可以包括毫米波雷达、摄像头、激光雷达以及超声波雷达等。根据一种实施方式，通过将多种传感器在各自的传感器坐标系下测得数据全部统一到自车坐标系，来完成空间同步。自车是指搭载这些传感器的车辆。自车完成本发明的目标跟踪。将各种传感器在各自的传感器坐标系下测得数据全部统一到自车坐标系需要进行坐标系的转换。

本发明涉及的传感器可以包括毫米波雷达、摄像头、激光雷达以及超声波雷达等。各种传感器具有不同的特性，例如:（1）毫米波雷达可以准确检测径向距离和速度，探测距离远，视场角宽，具有较强的穿透烟、雾、灰尘的能力，不受天气影响，但其方位角分辨率较低，量测杂波较多，不能对目标进行分类；（2）摄像头对物体检测方位角分辨率较高，可以对目标进行分类，虚报目标数目少，成本低，但对目标距离的测量精度随着距离增加迅速下降，容易受光照、雨雾天气影响而失效；（3）激光雷达能获得包含反射强度的目标点云的精确位置信息，多线的激光雷达甚至能对物体进行分类，但在雨雪雾天气下测量精度变差，有实验表明随着下雨量增加，激光雷达探测距离急剧下降。（4）超声波雷达体积小巧，测量稳定，但只能获得径向距离信息，由于超声波在空气中传播会快速衰减，超声波雷达探测距离较短，多用于近距离障碍物检测场景，如自动泊车。本发明的技术方案通过综合使用多种传感器，使得各传感器数据帧均匀分布，从而扬长避短。

图2示出了依据本发明的一种实施方式的进行坐标系转换的示意图。如图2所示，传感器坐标系转换到自车坐标系需要一个3×3旋转矩阵M和一个3×1平移向量t。

式中，

为自车坐标系下的坐标，

为传感器坐标系下的坐标，t为平移向量，根据传感器安装位置和自车后轴的距离对x、y、z三个方向进行加减计算，M为3×3旋转矩阵，将传感器坐标系绕三个坐标轴分别进行一定角度的旋转。空间转换即传感器坐标系下的坐标通过旋转和平移得到自车坐标系下的坐标。

图3示出了依据本发明的一种实施方式的进行数据融合的示意图。在图3中，三角形及五角星代表两传感器测量值更新时刻。如图3所示，通过根据先到先处理的原则按照时间顺序对多种传感器各别所获得的数据进行串行合并来完成所述数据融合。从图3中可以看出，经过串行合并后合成数据流的数据更新频率均大于单个传感器数据更新频率，这意味着中心航迹的状态更新频率也会加大，跟踪结果的精度也会变高。

然后，在步骤S200中，依据障碍物信息进行轨迹匹配。依据障碍物信息进行轨迹匹配可以分步完成。即判断障碍物是否与现存轨迹匹配；以及障碍物与现存轨迹不匹配时，利用匈牙利算法进行轨迹匹配。

图4是示出了依据本发明的一种实施方式判断障碍物是否与现存轨迹匹配的示意图。如图4所示，根据一种实施方式，如下地判断障碍物是否与现存轨迹匹配。

首先，判断障碍物是否之前就存在。根据一种实施方式，使用匈牙利算法等匹配算法将现有障碍物的列表与在传感器数据帧的当前帧中发现的障碍物进行匹配，从而确定障碍物是否之前就存在。匈牙利算法是一种寻找最大匹配的算法，即在两个集合之间促成最多的匹配对。匈牙利算法本身是本领域的公知技术，因此在此不予赘述。根据本发明的一种实施方式，可以先设置一个障碍物辨识（ID）池，在障碍物进行轨迹初始化时，会向ID池中申请一个ID，在删除轨迹时会释放这个ID，从而确保一个障碍物对应一个唯一的ID。自车在行驶过程中，因为不断发现需要追踪的障碍物，因而其会持续保存一个现有障碍物的列表。当雷达或摄像机按照监控周期发送来数据时（即传感器数据帧），该数据中会有障碍物，提取这些障碍物信息也会形成列表。将这两个列表进行匹配对比，能够确定在帧（传感器数据帧）中发现的障碍物是否之前就存在。

然后，当判断出该障碍物之前存在时，判断是否在之前预定数量的帧中都存在。根据一种实施方式，该预定数量的帧所对应的时间少于车辆紧急刹车最短滑行距离的一半，从而能够使得即便之前的障碍物出现突然变道、紧急刹车等意外情况，也能有足够的时间进行紧急处理。

例如，当前车速为36公里/小时（10米/秒），此时车辆紧急刹车最短滑行距离假定为10米，软件运行周期为50ms（帧间隔），则该预定数量的帧数为10帧。在这种情况下，10帧对应的物理时间是0.5秒，其滑行距离为5米，为车辆紧急刹车最短滑行距离10米的一半。上面的示例为了方便本领域技术人员的理解而做出，简化了计算。

此外，这样的帧数应该多于8帧。实验表明，多于8帧时，能够更好地进行匹配。在图4所示的实施例中，该预定数量的帧为16帧。

可以将该障碍物的ID与之前保存的与该预定数量（16）帧中的障碍物对应的ID列表进行比对，来确定该障碍物是否在之前的16帧都存在。

接着，在判断出在之前预定数量的帧中都存在的情况下，进行匹配确定。根据一种实施方式，通过判断该障碍物与预定数量的帧形成的轨迹之间的距离是否小于预定距离来进行匹配确定。根据一种实施方式，该预定距离应在0.9m到1.1m之间（图中示例为1m）。设置这样的预定距离，可以最大可能地匹配已有轨迹，减少误匹配。该预定距离为欧式距离。

根据一种实施方式，可以如下地计算欧式距离：当前帧中的障碍物横纵向位置坐标为（x_sensor，y_sensor），该预定数量的帧形成的障碍物轨迹也有对应的横纵向位置坐标（x_track，y_track），预定距离计算为：

根据一种实施方式，障碍物轨迹对应的横纵向位置坐标（x_track，y_track）是障碍物在最近的一帧中的横纵向位置坐标。根据另一种实施方式，障碍物轨迹对应的横纵向位置坐标（x_track，y_track）为根据障碍物在最近的一帧中的横纵向位置坐标等状态信息，利用位置推算算法推算出该障碍物在当前帧的时刻应处的位置的位置坐标。该位置推算算法例如为扩展卡尔曼滤波算法等。

当判断出当前帧中该障碍物与预定数量的帧形成的轨迹之间的距离小于预定距离时，确定此障碍物与预定数量的帧形成的轨迹相匹配。判断障碍物与已经维护的障碍物轨迹是否为同一个障碍物的过程为匹配，满足距离判断条件，则为匹配上。

最后，在步骤S300，对匹配上的轨迹进行更新。对于在步骤S200匹配上的障碍物，利用卡尔曼滤波对其轨迹进行更新，对于未匹配上轨迹的障碍物在轨迹列表中创建新的轨迹。

根据一种实施方式，如下地采用卡尔曼滤波来对轨迹进行更新。

步骤A：根据当前时刻位置预测下一时刻位置:

=Ax_k-1+Bu_k

式中，

为基于k-1时刻对k时刻状态的估计值，X_k-1为k-1时刻的状态。状态是指对于障碍物跟踪一系列需要维护的信号集合，可以包括但不限于障碍物的纵向位置、横向位置、纵向速度、横向速度、纵向加速度和横向加速度等。u_k为k时刻的控制器向量，可以为零。A为状态转移矩阵，B为作用在控制器向量上的控制矩阵。这两个矩阵是预先设定的，都跟算法所采用的运动学模型（如恒定速度模型、恒定加速度模型、恒定转率和速度模型、恒定转率和加速度模型等）相关。

步骤B：根据当前协方差矩阵预测下一时刻协方差矩阵

=AP _k-1A^T+Q

式中，

为基于k-1时刻对k时刻状态估计值的协方差矩阵，P _k-1为k-1时刻状态的协方差矩阵，Q为过程噪声的协方差矩阵。

步骤C：根据预测出的下一时刻的协方差矩阵计算卡尔曼增益:

K_k=

H^T(H

H^T+R)^-1

式中，K _k 为k时刻的卡尔曼增益，H为将真实状态空间映射到观测空间的观测矩阵，R为观测噪声的协方差矩阵。传感器实际测量的信号量和上述所要维护的状态量不是完全一一对应的。H矩阵即是描述两者之间转换关系的矩阵，随传感器种类和所要维护的状态量不同而不同，可以在确定传感器种类、确定传感器输出、确定所要维护的状态量后，通过简单运动学或动力学关系确定。R矩阵描述的是传感器在其所输出的各信号量上的协方差，对应的现实意义是传感器各信号量的不确定性，根据上游传感器给定的其所输出的各信号量的测量不确定度获取，也可通过搭载真值***来测试传感器的误差分布来代替。

步骤D：根据卡尔曼增益，对预测出的下一时刻位置和协方差矩阵进行更新。

更新估计量:

为k时刻的状态测量值；

更新协方差矩阵:

I为单位矩阵。

完成了估计量更新和协方差矩阵更新即完成了轨迹更新。

根据一种实施方式，更新还可以包括存储当前帧中的障碍物的位置坐标作为轨迹位置坐标、增加当前轨迹的连续帧数等。

根据一种实施方式，对于超过预定帧数都没有障碍物来更新的轨迹从轨迹列表中移除。根据一种实施方式，该预定帧数为5帧到7帧，并且应小于前述的用以确定障碍物连续存在的预定数量的帧数。这样可以较好地避免错误移除障碍物。

图5是示出了依据本发明的一种实施方式的目标跟踪装置的示意性方框图。

如图5所示，依据本发明的一种实施方式的目标跟踪装置包括障碍物信息获取单元100，从各传感器数据帧中获得障碍物信息，这例如可以通过对来自多种传感器的传感器数据帧进行空间同步和数据融合来进行；轨迹匹配单元200，依据所述障碍物信息进行轨迹匹配；以及轨迹跟踪单元300，对经匹配的障碍物和轨迹进行轨迹跟踪与更新。

图6示出了依据本发明的一种实施方式的轨迹匹配单元的示意性方框图。如图6所示，依据一种实施方式，该轨迹匹配单元200包括：存在判断单元201，判断所述障碍物是否之前就存在；连续存在判断单元202，当判断出所述障碍物之前就存在时，判断是否在之前预定数量的传感器数据帧中都存在；距离判断单元203，在判断出在之前预定数量的传感器数据帧都存在的情况下，判断所述障碍物与预定数量的帧形成的轨迹之间的距离是否小于预定距离；以及匹配确定单元204，当判断出所述障碍物与预定数量的帧形成的轨迹之间的距离小于预定距离时，确认此障碍物与该预定数量的帧形成的轨迹相匹配。

根据一种实施方式，存在判断单元201通过使用匈牙利算法将现有障碍物的列表与在传感器数据帧中发现的障碍物进行匹配，来确定障碍物是否之前就存在。

根据一种实施方式，所述预定数量大于8，所述预定距离是0.9 m 到1.1m。

根据一种实施方式，障碍物信息获取单元100通过将多种传感器以传感器自身为坐标系测得数据全部统一到车辆坐标系，来完成空间同步，通过根据先到先处理的原则按照时间顺序对所述多种传感器各别所获得的数据进行串行合并来完成所述数据融合。使用多种传感器的数据，能够提高匹配速度。

根据一种实施方式，对经匹配的所述障碍物和所述轨迹进行轨迹跟踪时，对于某一轨迹，如超过预定帧数都没有得到更新，则轨迹跟踪单元300不再跟踪该轨迹。

根据一种实施方式，该预定帧数为5帧到7帧。

根据一种实施方式，轨迹跟踪单元300如下地采用卡尔曼滤波来进行轨迹更新：

步骤A：根据当前时刻位置预测下一时刻位置：

=Ax_k-1+Bu_k

式中，

为基于k-1时刻对k时刻状态的估计值，X_k-1为k-1时刻的状态；u_k为k时刻的控制器向量；A为状态转移矩阵；B为作用在控制器向量上的控制矩阵；

步骤B：根据当前协方差矩阵预测下一时刻协方差矩阵

=AP _k-1A^T+Q

式中，

为基于k-1时刻对k时刻状态估计值的协方差矩阵，

P _k-1为k-1时刻状态的协方差矩阵，Q为过程噪声的协方差矩阵；

步骤C：根据预测出的下一时刻的协方差矩阵计算卡尔曼增益:

K_k=

H^T(H

H^T+R)^-1

式中，K _k 为k时刻的卡尔曼增益，H为将真实状态空间映射到观测空间的观测矩阵，R为观测噪声的协方差矩阵；

步骤D：根据卡尔曼增益，对预测出的下一时刻位置和协方差矩阵进行更新：

更新估计量:

更新协方差矩阵:

，

其中，z_k为k时刻的状态测量值；I为单位矩阵，

完成了估计量更新和协方差矩阵更新即完成了轨迹更新。

本领域技术人员容易理解，可以利用以上对方法的说明和描述来理解和实现本发明的装置的对应部分。根据上下文，有时候更新和追踪可以表示相同的含义。

本领域技术人员容易理解，本发明的方法还可以包括与本发明的装置所完成的功能对应的其他步骤。以上的这些步骤也可以进行精简。

本发明对单元和步骤的标号仅仅是为了说明的方便，除非上下文中有相反的说明，否则不代表其执行的顺序。

本领域的技术人员应该理解，上述的各单元可以由软件或专门的硬件来实现，例如现场可编程门阵列、单片机、或微芯片等，或者也可以通过软件结合硬件的方式来实现。

本发明还提供了一种电子设备，包括：处理器；用于存储所述处理器可执行指令的存储器；其中，所述处理器被配置为执行所述指令，以实现本发明的方法。

本发明还涉及一种计算机软件，该计算机软件在被计算装置（例如单片机、电脑、CPU等）执行时，可以实现本发明的方法。

本发明还涉及一种计算机软件存储装置，例如硬盘、软盘、闪存等，该计算机软件存储装置存储有上述的计算机软件。

本发明对方法或步骤的说明可以用于理解对单元或装置的说明，对单元或装置的说明也可以用于理解本发明的方法或步骤。

以上说明仅仅是示意性的，不是对本发明的保护范围的限制，在本发明的权利要求范围内的任何改变、替换均在本发明的保护范围内。

Claims (10)

1.一种目标跟踪方法，其特征在于，包括以下步骤：

从来自多种传感器的各传感器数据帧中获得障碍物信息；

依据所述障碍物信息进行轨迹匹配；

对经匹配的障碍物和轨迹进行轨迹跟踪与更新，

其中，依据所述障碍物信息进行轨迹匹配包括：

判断所述障碍物是否之前就存在；

当判断出所述障碍物之前就存在时，判断是否在之前预定数量的传感器数据帧中都存在；

在判断出在之前预定数量的传感器数据帧都存在的情况下，判断所述障碍物与该预定数量的传感器数据帧形成的轨迹之间的距离是否小于预定距离，当判断出小于预定距离时，确认障碍物与该预定数量的传感器数据帧形成的轨迹相匹配。

2.根据权利要求1所述的目标跟踪方法，其特征在于，通过使用匈牙利算法将现有障碍物的列表与在传感器数据帧中发现的障碍物进行匹配，来确定障碍物是否之前就存在。

3.根据权利要求1所述的目标跟踪方法，其特征在于，所述预定数量的传感器数据帧所对应的时间少于自车紧急刹车最短滑行距离的一半，所述预定数量大于8，所述预定距离在0.9m到1.1m之间。

4.根据权利要求1所述的目标跟踪方法，其特征在于，如下地通过对来自多种传感器的传感器数据帧进行空间同步和数据融合，以从各传感器数据帧中获得障碍物信息：

通过将多种传感器以传感器自身为坐标系测得的数据全部统一到车辆坐标系，来完成空间同步；

通过根据先到先处理的原则按照时间顺序对所述多种传感器各别所获得的数据进行串行合并来完成所述数据融合，所述多种传感器包括毫米波雷达、摄像头、激光雷达以及超声波雷达。

5.根据权利要求1所述的目标跟踪方法，其特征在于，如下地采用卡尔曼滤波来进行轨迹更新：

步骤A：根据当前时刻位置预测下一时刻位置：

=Ax_k-1+Bu_k

式中，

为基于k-1时刻对k时刻状态的估计值，X_k-1为k-1时刻的状态；u_k为k时刻的控制器向量；A为状态转移矩阵；B为作用在控制器向量上的控制矩阵；

步骤B：根据当前协方差矩阵预测下一时刻协方差矩阵

=AP _k-1A^T+Q

式中，

为基于k-1时刻对k时刻状态估计值的协方差矩阵，

P _k-1为k-1时刻状态的协方差矩阵，Q为过程噪声的协方差矩阵；

步骤C：根据预测出的下一时刻的协方差矩阵计算卡尔曼增益:

K_k=

H^T(H

H^T+R)^-1

式中，K _k 为k时刻的卡尔曼增益，H为将真实状态空间映射到观测空间的观测矩阵，R为观测噪声的协方差矩阵；

步骤D：根据卡尔曼增益，对预测出的下一时刻位置和协方差矩阵进行更新：

更新估计量:

更新协方差矩阵:

，

其中，

为k时刻的状态测量值；I为单位矩阵，

完成了估计量更新和协方差矩阵更新即完成了轨迹更新。

6.根据权利要求1所述的目标跟踪方法，其特征在于，对经匹配的所述障碍物和所述轨迹进行轨迹跟踪时，对于某一轨迹，如超过预定帧数都没有得到更新，则不在跟踪该轨迹。

7.根据权利要求6所述的目标跟踪方法，其特征在于，所述预定帧数为5帧到7帧。

8.一种目标跟踪装置，其特征在于，包括

障碍物信息获取单元，从来自多种传感器的各传感器数据帧中获得障碍物信息；

轨迹匹配单元，依据所述障碍物信息进行轨迹匹配；

轨迹跟踪单元，对经匹配的障碍物和轨迹进行轨迹跟踪与更新，

其中，所述轨迹匹配单元包括：

存在判断单元，判断所述障碍物是否之前就存在；

连续存在判断单元，当判断出所述障碍物之前就存在时，判断所述障碍物是否在之前预定数量的传感器数据帧中都存在；

距离判断单元，在判断出在之前预定数量的传感器数据帧都存在的情况下，判断所述障碍物与该预定数量的传感器数据帧形成的轨迹之间的距离是否小于预定距离；以及

匹配确定单元，当距离判断单元判断出小于预定距离时，匹配确定单元确认此障碍物与该预定数量的传感器数据帧形成的轨迹相匹配。

9.一种电子设备，其特征在于，包括：

处理器；

用于存储所述处理器可执行指令的存储器；

其中，所述处理器被配置为执行所述指令，以实现如权利要求1至7中任一项所述的方法。

10.一种计算机可读存储介质，其特征在于，所述可读存储介质上存储设备控制程序，当所述设备控制程序被处理器执行时，实现如权利要求1至7任一项所述的方法。

Images