CN115840221B - 基于4d毫米波雷达实现目标特征提取与多目标跟踪的方法 - Google Patents

Abstract

本发明涉及一种基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其中，所述的方法包括以下步骤：（1）获取雷达输出的点云数据，并对点云数据进行预处理，剔除野值；（2）对经过预处理后得到的点进行DBSCAN聚类处理；（3）对获取到的每一聚类均进行目标特征提取处理；（4）对当前已有航迹进行预测，并将预测结果与聚类结果进行匹配关联；（5）根据关联聚类结果，执行相应的多目标跟踪运动状态处理；（6）基于目标运动状态，更新目标尺寸信息以及航向角信息。本发明还涉及一种相应的装置、处理器及其计算机可读存储介质。采用了本发明的该方法、装置、处理器及其计算机可读存储介质，能够有效的提高多目标跟踪中的航迹与观测的关联可靠性。

Description

基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法

技术领域

本发明涉及计算机视觉技术领域，尤其涉及目标特征提取与跟踪技术领域，具体是指一种基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法、装置、处理器及其计算机可读存储介质。

背景技术

毫米波雷达受雨、雪、雾等环境因素的影响小，具有全天候、全天时的工作特性。传统毫米波雷达对目标的检测通常为单个点或若干个强反射点，虽然能够测量到目标的速度信息，但相比于摄像头和激光雷达所能提供的信息仍较少。4D毫米波雷达相比传统雷达，增强了水平向和俯仰向的角度分辨率，能够获得被探测目标的更加丰富的信息。由于目标信息更丰富，表现之一是对单一目标检测到的反射点数更多，传统毫米波雷达的基于点迹的目标跟踪方法很难直接应用于4D毫米波雷达且不能充分利用4D毫米波雷达提供信息。其中，在本领域已公开的相关技术均存在不同的技术缺陷，具体如下：

申请号：CN202111612665.4发明专利申请提出了一种基于毫米波雷达的多目标跟踪与呼吸检测方法和装置。该发明提供的多目标跟踪方法中，直接将雷达点云关联至已有轨迹并对已有轨迹进行更新；对剩余未关联的雷达点云进行DBSCAN聚类然后仅取聚类中心作为新轨迹的起始。该发明没有充分利用毫米波雷达提供的对目标的速度观测信息，同时也没有对聚类后的点云进一步提取更丰富的目标特征。

申请号：CN202210473036.6发明专利申请提出了一种基于毫米波雷达的多目标跟踪方法。该发明提供的多目标跟踪方法中，通过计算雷达聚类目标与航迹预测值间的残差和协方差矩阵来计算高斯似然函数值，从而计算互联概率。事实上，雷达聚类目标与航迹预测值的状态参数并不是一一对应关系，计算似然函数需要建立目标观测模型或者对雷达聚类结果进行特征提取以匹配航迹状态参数，而该申请对这两方面均无说明。此外，该申请在航迹更新步骤利用多普勒的概率数据关联滤波器进行更新，但毫米波雷达测得的多普勒信息为目标的径向速度分量，直接利用此信息对目标的运动状态进行更新有较大误差。

申请号：CN201910661302.6发明专利申请提出了一种毫米波雷达多目标跟踪方法。该发明提供的多目标跟踪方法中，在对雷达点云数据聚类后进行了特征提取。该发明取同一类点的平均坐标作为目标坐标的估计；取同一类点与平均坐标的x轴、y轴最大差值的绝对值作为长轴和短轴，作为目标的尺寸估计；取同一类点的速度均值作为目标速度估计。该发明提供的雷达目标特征提取方法仅适用于沿x轴或y轴做直线运动的目标，当目标偏转时，其估计出的目标尺寸偏大，且缺少航向信息；当目标运动方向不相对雷达径向运动时，同一类点的速度均值仅能反映目标速度的径向分量，直接用此对目标的速度进行更新有较大误差。

综上，基于现有技术已公开的相关技术内容而言，其均存在技术功能上的局限性，无法同时满足精准跟踪与观测关联可靠性的综合评估。

发明内容

本发明的目的是克服了上述现有技术的缺点，提供了一种能够实现精准跟踪与可靠观测的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法、装置、处理器及其计算机可读存储介质。

为了实现上述目的，本发明的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法、装置、处理器及其计算机可读存储介质如下：

该基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其主要特点是，所述的方法包括以下步骤：

（1）获取雷达输出的点云数据，并对点云数据进行预处理，剔除野值；

（2）对经过预处理后得到的点进行DBSCAN聚类处理；

（3）对获取到的每一聚类均进行目标特征提取处理；

（4）对当前已有航迹进行预测，并将预测结果与聚类结果进行全局关联匹配；

（5）根据关联聚类结果，执行相应的多目标跟踪运动状态处理，获取横摆角速度；

（6）基于目标运动状态，更新目标尺寸信息以及航向角信息。

较佳地，所述的步骤（1）具体为：

获取雷达输出的点云数据，并对所述的点云数据进行删除速度异常的点、删除高度异常的点、过滤感兴趣区域以外的点的数据预处理，以剔除野值。

较佳地，所述的步骤（2）具体为：

根据点云信息中的各个点的坐标位置以及相对径向速度，进行DBSCAN聚类处理，具体为：

；

其中， ，表示参与判别的两个点的索引， x，，分别表示每个点的坐标位置，表示相对径向速度。

较佳地，所述的步骤（3）具体包括以下步骤：

（3.1）计算聚类平均径向速度、 x平均值 和 y平均值 ；

（3.2）统计聚类点在 z方向的最大值和最小值，用最大值减去最小值的差作为聚类高度特征；

（3.3）将同一聚类下的每个点的 x， y坐标分别减去 ，得到 ，；

（3.4）以1°为间隔，遍历0至90°范围，用每个角度对得到的所有 ，进行旋转，确定旋转后的 、 、 和 ，并通过以下公式计算损失函数：

；

并记录所述的损失函数最小的角度值 angle 及相应的 、 、 和；

（3.5）使用 和 中的较大者作为聚类长度特征，将较小者作为聚类宽度特征，并用 ，的平均值 和 ，的平均值 经旋转－angle 角度后，再加上 ， 后得到的值作为聚类中心坐标（ ）；

（3.6）如果，angle增加90度，且angle方向与平均径向速度的方向夹角超过90度，则angle再增加180度，取最终的angle值作为聚类的航向角特征；

（3.7）根据前述获取到的所述的聚类中心坐标、聚类长度特征、聚类宽度特征、聚类高度特征以及航向角特征，构造聚类矩形目标包围框，以供后续进行目标特征提取处理。

较佳地，所述的步骤（4）具体包括以下步骤：

（4.1）取运动状态为，并采用线性卡尔曼滤波方式进行已有航迹预测处理，具体为：

；

其中，为 x向速度分量，为 x向加速度分量，为 y向速度分量，为 y向加速度分量，为目标 x向位置预测值，为 x向速度分量预测值，为 x向加速度分量预测值，为目标y向位置预测值，为 y向速度分量预测值，为 y向加速度分量预测值， T为相邻两帧雷达点云间的时间间隔；

（4.2）利用已有航迹的长和宽计算对角线长度dist_cri_1：

；

利用聚类长度和宽度特征，计算对角线长度dist_cri_2：

；

并按0.6和0.4的权值加权计算关联距离的上限dist_cri：

；

计算已有航迹预测位置(,)与聚类中心的欧式距离：

；

并根据以下公式计算距离关联分数：

；

（4.3）根据已有航迹预测位置、聚类位置、尺寸以及航向角，计算所述的聚类矩形目标包围框各顶点的坐标和矩形框的面积，并根据以下公式计算尺寸关联分数：

；

（4.4）根据已有航迹的预测位置、预测速度和横摆角速度，通过以下公式计算待关联聚类每一点的预测径向速度：

；

其中， 表示预测径向速度，表示目标横摆角速度，表示聚类点云中每个点的径向速度方向；

对同一聚类内的各点预测径向速度取平均值得到平均预测径向速度，并与聚类实测的平均径向速度求差，得到差值 delta_vr，并通过以下公式计算速度关联分数：

；

其中，为预设阈值；

（4.5）将上述计算获取的距离关联分数、尺寸关联分数以及速度关联分数进行加权求和，以获得航迹与聚类的最终关联分数；

（4.6）根据每个聚类与每个已有航迹的最终关联分数，构造关联矩阵，包括：矩阵的行数等于已有航迹的个数，矩阵的列数为当前聚类的个数，矩阵第 i行第 j列的值为第 i个已有航迹与第 j个聚类的关联分数，基于关联矩阵进行全局关联匹配。

较佳地，所述的步骤（5）包括：

采用以下计算公式建立航迹观测模型：

；

其中，，为聚类中心，为聚类点云平均径向速度，为聚类点云的平均方位角观测的余弦值平均，为聚类点云的平均方位角观测的正弦值平均，、、、、、为航迹的运动状态。

较佳地，所述的步骤（5）具体包括以下步骤：

（5.1）判断对应的聚类与每个已有航迹是否关联成功，如果是，则进入步骤（5.2），否则，进入步骤（5.4）；

（5.2）对已关联的已有航迹，根据航迹预测运动状态和所述的航迹观测模型，计算航迹的预测观测值、和：

；

（5.3）将该聚类下相应的聚类特征值减去预测观测值，得到残差，并将该残差乘以滤波增益矩阵K，并加上航迹预测运动状态，得到已关联航迹运动状态的更新结果：

；

其中、、、、和为更新后的航迹运动状态；

（5.4）对关联失败的航迹进行消亡判定，如果其连续累计未关联次数小于消亡条件的预设阈值，则用航迹预测结果作为更新结果继续进行航迹外推，否则直接删除该航迹；

（5.5）对关联失败的聚类建立新航迹，新航迹的运动状态参数采用以下公式进行表示：

；

其中，为聚类航向角特征，为点云径向速度的平均方向角，、为聚类中心，为聚类点云平均径向速度；

（5.6）根据当前获取到的已关联航迹运动状态的更新结果、航迹外推结果以及新航迹运动状态计算结果，通过以下公式计算对应的横摆角速度：

；

其中，速度向量与加速度向量的叉乘结果 的正负取值作为横摆角速度的正负取值。

较佳地，所述的步骤（5.5）具体包括以下步骤：

（5.5.1）将聚类中心作为新航迹起始位置；

（5.5.2）将聚类航向角特征作为新航迹的速度方向，聚类平均径向速度反投影至航向角方向得到新航迹的速度大小；

（5.5.3）将新航迹加速度取零；

（5.5.4）取所述的聚类长度特征作为新航迹的长度，取所述的聚类宽度特征作为新航迹的宽度，取聚类航向角特征作为新航迹的航向角。

较佳地，所述的步骤（6）具体包括以下步骤：

（6.1）将航迹长度与聚类长度特征进行加权平均，得到航迹长度更新结果，将航迹宽度与聚类宽度特征进行加权平均，得到航迹宽度更新结果；

（6.2）基于航迹运动状态的速度矢量和加速度矢量，计算垂直于速度矢量方向的加速度分量，然后用垂直加速度分量的模除以速度矢量的模得到目标横摆角速度的大小，速度矢量与加速度矢量的叉积方向为横摆角速度的方向；

（6.3）根据航迹速度计算速度航向角，将聚类航向角特征、速度航向角和航迹航向角进行加权平均，得到航迹航向角更新结果。

该基于4D毫米波雷达实现目标特征提取与多目标跟踪的装置，其主要特点是，所述的装置包括：

处理器，被配置成执行计算机可执行指令；

存储器，存储一个或多个计算机可执行指令，所述计算机可执行指令被所述处理器执行时，实现上述所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

该基于4D毫米波雷达实现目标特征提取与多目标跟踪的处理器，其主要特点是，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

该计算机可读存储介质，其主要特点是，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现上述所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

采用了本发明的该基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法、装置、处理器及其计算机可读存储介质，其相较于传统的雷达多目标跟踪方法而言，从雷达聚类点云中获取了除位置以外的目标尺寸、航向角等信息，并充分结合这些信息提高多目标跟踪中的航迹与观测的关联可靠性；并且针对4D毫米波雷达，还提供了新的航迹观测模型，通过该模型可以充分利用同一目标的各个观测点协同进行更新；除此之外，本发明还引入了目标横摆角速度，以此能够实现对转弯机动目标的更稳定地跟踪。

附图说明

图1为本发明的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的流程图。

图2为本发明的对点云聚类进行特征提取并表示成矩形包围框的效果图。

图3为本发明在一具体实施例中对机动车进行目标跟踪的效果示意图。

具体实施方式

为了能够更清楚地描述本发明的技术内容，下面结合具体实施例来进行进一步的描述。

在详细说明根据本发明的实施例前，应该注意到的是，在下文中，术语“包括”、“包含”或任何其他变体旨在涵盖非排他性的包含，由此使得包括一系列要素的过程、方法、物品或者设备不仅包含这些要素，而且还包含没有明确列出的其他要素，或者为这种过程、方法、物品或者设备所固有的要素。

请参阅图1所示，该基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其中，所述的方法包括以下步骤：

（1）获取雷达输出的点云数据，并对点云数据进行预处理，剔除野值；

（2）对经过预处理后得到的点进行DBSCAN聚类处理；

（3）对获取到的每一聚类均进行目标特征提取处理；

（4）对当前已有航迹进行预测，并将预测结果与聚类结果进行全局关联匹配；

（5）根据关联聚类结果，执行相应的多目标跟踪运动状态处理，获取横摆角速度；

（6）基于目标运动状态，更新目标尺寸信息以及航向角信息。

作为本发明的优选实施方式，所述的步骤（1）具体为：

获取雷达输出的点云数据，并对所述的点云数据进行删除速度异常的点、删除高度异常的点、过滤感兴趣区域以外的点的数据预处理，以剔除野值。

作为本发明的优选实施方式，所述的步骤（2）具体为：

根据点云信息中的各个点的坐标位置以及相对径向速度，进行DBSCAN聚类处理，具体为：

；

其中， ，表示参与判别的两个点的索引，，，分别表示每个点的坐标位置，表示相对径向速度。

作为本发明的优选实施方式，所述的步骤（3）具体包括以下步骤：

（3.1）计算聚类平均径向速度、 x平均值 和 y平均值 ；

（3.2）统计聚类点在 z方向的最大值和最小值，用最大值减去最小值的差作为聚类高度特征；

（3.3）将同一聚类下的每个点的 x， y坐标分别减去 ，得到 ，；

（3.4）以1°为间隔，遍历0至90°范围，用每个角度对得到的所有 ，进行旋转，确定旋转后的 、 、 和 ，并通过以下公式计算损失函数：

；

并记录所述的损失函数最小的角度值angle及相应的、、和；

（3.5）使用 和 中的较大者作为聚类长度特征，将较小者作为聚类宽度特征，并用 ，的平均值 和 ，的平均值 经旋转－angle 角度后，再加上 ， 后得到的值作为聚类中心坐标（，）；

（3.6）如果，angle增加90度，且angle方向与平均径向速度的方向夹角超过90度，则angle再增加180度，取最终的angle值作为聚类的航向角特征；

（3.7）根据前述获取到的所述的聚类中心坐标、聚类长度特征、聚类宽度特征、聚类高度特征以及航向角特征，构造聚类矩形目标包围框，以供后续进行目标特征提取处理。

作为本发明的优选实施方式，所述的步骤（4）具体包括以下步骤：

（4.1）取运动状态为，并采用线性卡尔曼滤波方式进行已有航迹预测处理，具体为：

；

其中，为 x向速度分量，为 x向加速度分量，为 y向速度分量，为 y向加速度分量，为目标 x向位置预测值，为 x向速度分量预测值，为 x向加速度分量预测值，为目标y向位置预测值，为 y向速度分量预测值，为 y向加速度分量预测值， T为相邻两帧雷达点云间的时间间隔；

（4.2）利用已有航迹的长和宽计算对角线长度dist_cri_1：

；

利用聚类长度和宽度特征，计算对角线长度dist_cri_2：

；

并按0.6和0.4的权值加权计算关联距离的上限dist_cri：

；

计算已有航迹预测位置(,)与聚类中心的欧式距离：

；

并根据以下公式计算距离关联分数：

；

（4.3）根据已有航迹预测位置、聚类位置、尺寸以及航向角，计算所述的聚类矩形目标包围框各顶点的坐标和矩形框的面积，并根据以下公式计算尺寸关联分数：

；

（4.4）根据已有航迹的预测位置、预测速度和横摆角速度，通过以下公式计算待关联聚类每一点的预测径向速度：

；

其中， 表示预测径向速度，表示目标横摆角速度，表示聚类点云中每个点的径向速度方向；

对同一聚类内的各点预测径向速度取平均值得到平均预测径向速度，并与聚类实测的平均径向速度求差，得到差值 delta_vr，并通过以下公式计算速度关联分数：

；

其中， 为预设阈值；

（4.5）将上述计算获取的距离关联分数、尺寸关联分数以及速度关联分数进行加权求和，以获得航迹与聚类的最终关联分数；

（4.6）根据每个聚类与每个已有航迹的最终关联分数，构造关联矩阵，包括：矩阵的行数等于已有航迹的个数，矩阵的列数为当前聚类的个数，矩阵第 i行第 j列的值为第 i个已有航迹与第 j个聚类的关联分数，基于关联矩阵进行全局关联匹配。

作为本发明的优选实施方式，所述的步骤（5）包括：

采用以下计算公式建立航迹观测模型：

；

其中，，为聚类中心，为聚类点云平均径向速度，为聚类点云的平均方位角观测的余弦值平均，为聚类点云的平均方位角观测的正弦值平均，、、、y、、为航迹的运动状态。

作为本发明的优选实施方式，所述的步骤（5）具体包括以下步骤：

（5.1）判断对应的聚类与每个已有航迹是否关联成功，如果是，则进入步骤（5.2），否则，进入步骤（5.4）；

（5.2）对已关联的已有航迹，根据航迹预测运动状态和所述的航迹观测模型，计算航迹的预测观测值、和：

；

（5.3）将该聚类下相应的聚类特征值减去预测观测值，得到残差，并将该残差乘以滤波增益矩阵K，并加上航迹预测运动状态，得到已关联航迹运动状态的更新结果：

；

其中、、、、和为更新后的航迹运动状态；

（5.4）对关联失败的航迹进行消亡判定，如果其连续累计未关联次数小于消亡条件的预设阈值，则用航迹预测结果作为更新结果继续进行航迹外推，否则直接删除该航迹；

（5.5）对关联失败的聚类建立新航迹，新航迹的运动状态参数采用以下公式进行表示：

；

其中，为聚类航向角特征，为点云径向速度的平均方向角，、为聚类中心，为聚类点云平均径向速度；

（5.6）根据当前获取到的已关联航迹运动状态的更新结果、航迹外推结果以及新航迹运动状态计算结果，通过以下公式计算对应的横摆角速度：

；

其中，速度向量与加速度向量的叉乘结果 的正负取值作为横摆角速度的正负取值。

作为本发明的优选实施方式，所述的步骤（5.5）具体包括以下步骤：

（5.5.1）将聚类中心作为新航迹起始位置；

（5.5.2）将聚类航向角特征作为新航迹的速度方向，聚类平均径向速度反投影至航向角方向得到新航迹的速度大小；

（5.5.3）将新航迹加速度取零；

（5.5.4）取所述的聚类长度特征作为新航迹的长度，取所述的聚类宽度特征作为新航迹的宽度，取聚类航向角特征作为新航迹的航向角。

作为本发明的优选实施方式，所述的步骤（6）具体包括以下步骤：

（6.1）将航迹长度与聚类长度特征进行加权平均，得到航迹长度更新结果，将航迹宽度与聚类宽度特征进行加权平均，得到航迹宽度更新结果；

（6.2）基于航迹运动状态的速度矢量和加速度矢量，计算垂直于速度矢量方向的加速度分量，然后用垂直加速度分量的模除以速度矢量的模得到目标横摆角速度的大小，速度矢量与加速度矢量的叉积方向为横摆角速度的方向；

（6.3）根据航迹速度计算速度航向角，将聚类航向角特征、速度航向角和航迹航向角进行加权平均，得到航迹航向角更新结果。

在实际应用当中，本发明提供的该目标特征提取方法，主要包括以下步骤：1、计算聚类平均径向速度、平均 x值和平均 y值；2、确定聚类点在 z轴方向的极差，得到聚类高度特征；3、将同一类内的每个点的 x， y坐标减去 x， y均值，遍历0～90度范围，对每个角度，在 xoy平面内将减去均值后的点旋转相应角度值，并确定旋转后的最大 x值、最大 y值、最小 x值和最小 y值，计算如下损失函数：

；

其中  表示减去均值并旋转后的点的坐标，表示同一类的点数， 表示取四者中的最小。得到使上述损失函数最小的角度值 angle 及相应的 、 、和 。4、用 和 中的较大者作为聚类长度特征，较小者作为聚类的宽度特征。用 ，的平均值 和 ，的平均值 经旋转 —angle并加上点云 x， y均值后的值作为聚类中心坐标。5、如果，angle增加90度；如果angle方向与平均径向速度的方向（平均速度大于零，则从原点指向聚类中心，反之则从聚类中心指向原点）夹角超过90度，则angle再增加180度。取最终的angle值作为聚类的航向角特征。

在实际应用当中，本发明提供的该多目标跟踪方法，主要包括以下步骤：1、获取雷达输出的点云数据，对点云进行预处理，剔除野值；2、根据点云信息中的位置、相对径向速度、反射强度等进行聚类，并利用本发明提供的方法进行目标特征提取；3、对当前已有航迹进行预测，并将预测结果与聚类结果进行匹配关联；4、对关联上的航迹，进行更新，对未关联航迹进行外推或消亡；5、对未关联的聚类结果，生成新航迹；6、对当前正在跟踪的航迹，根据其关联的聚类结果更新目标尺寸信息，根据目标运动状态计算横摆角速度，根据聚类结果和目标运动状态更新航向角。

步骤3的航迹预测结果与聚类结果的匹配关联包括以下步骤：3.1、根据聚类尺寸和已有航迹的尺寸计算关联距离上限 dist_cri；计算航迹预测位置与聚类中心的欧式距离dist；用dist和dist_cri计算距离关联分数。3.2、根据聚类位置、尺寸、航向角与已有航迹的位置、尺寸、航向角计算尺寸关联分数。3.3、根据已有航迹的预测位置、预测速度和横摆角速度计算待关联聚类每一点的预测径向速度：

；

其中 表示预测径向速度，表示航迹预测速度，、表示航迹预测位置，表示目标横摆角速度，表示聚类点云中每个点的径向速度方向。对同一类内的各点预测径向速度取平均得到平均预测径向速度，并与聚类实测的平均径向速度求差得到 delta_vr，计算速度关联分数。3.4、对距离关联分数、尺寸关联分数和速度关联分数进行加权求和，作为航迹与聚类的最终关联分数；3.5、根据每个聚类与每个已有航迹的最终关联分数，构造关联矩阵，进行全局匹配。

步骤4中的关联航迹更新首先要求建立航迹观测模型。针对4D毫米波雷达能观测到目标点云和仅能观测目标相对雷达的径向速度的特性，本发明提供如下一种观测模型：

；

其中，为聚类中心，聚类点云平均径向速度，为聚类点云的平均方位角观测的余弦值平均，为聚类点云的平均方位角观测的正弦值平均。上式中航迹运动状态以（）为例，分别代表目标的x向位置，x向速度分量，x向加速度分量，y向位置，y向速度分量和y向加速度分量。实际航迹的运动状态表示不限于此种形式，但都能转化成此种形式从而利用上述观测模型。步骤4包括以下具体步骤：4.1、对已关联的已有航迹，根据航迹的预测运动状态和上述观测模型，计算航迹的预测观测值；4.2、将相应的聚类特征值减去预测观测值，得到残差。4.3、将残差乘以滤波增益，并加上航迹预测运动状态，得到已关联航迹运动状态的更新结果。4.4、对未关联的已有航迹，如果其连续累计未关联次数小于预设阈值，则用航迹预测结果作为更新结果继续进行外推，否则直接删除该航迹。

步骤5生成新航迹包括以下具体步骤：5.1、将聚类中心作为新航迹起始位置；5.2、将聚类航向角特征作为新航迹的速度方向，聚类平均径向速度反投影至航向角方向得到新航迹的速度大小；5.3、新航迹加速度取零；5.4、聚类长度特征和宽度特征分别作为新航迹的长和宽，航向角特征作为新航迹的起始航向角。

步骤6包括以下具体步骤：6.1、将航迹尺寸与聚类尺寸进行加权平均，得到航迹尺寸更新结果；6.2、基于航迹的运动状态的速度矢量、加速度矢量，计算垂直于速度矢量方向的加速度分量，然后用垂直加速度分量的模除以速度矢量的模得到目标横摆角速度的大小，速度矢量与加速度矢量的叉积方向为横摆角速度的方向；6.3、根据航迹速度计算速度航向角，将聚类航向角特征、速度航向角和航迹航向角进行加权平均，得到航迹航向角更新结果。

作为本发明的一具体实施例，本发明的该基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其具体包括以下步骤：

步骤1：获取雷达点云后，进行点云预处理，包括删除速度异常的点、删除高度异常的点、过滤感兴趣区域以外的点等。

步骤2：对预处理后的点进行聚类。本实施例中采用DBSCAN聚类，核心点的最小邻接点个数取3，邻接点的判别条件为：

；

其中，下标i，j表示参与判别的两个点的索引，x，y，z，vr表示每个点的坐标位置和相对径向速度。

步骤3：对每一聚类，进行特征提取。

3.1、计算聚类平均径向速度、x平均值 和y平均值 ；

3.2、确定聚类点在 z 轴方向的极差，得到聚类高度特征；

3.3、将同一类内的每个点的 x， y坐标分别减去 ， 得到 ，。以1度为间隔，遍历0-90度范围，用每个角度对所有 ，进行旋转，确定旋转后的 、 、 和 ，并计算损失函数：

；

记录使损失函数最小的角度值 angle 及相应的 、 、 和 。

3.4、用 和 中的较大者作为聚类长度特征，较小者作为聚类的宽度特征。用 ，的平均值 和 ，的平均值 经旋转 -angle并加上 ， 后的值作为聚类中心坐标。

3.5、计算聚类航向角特征。如果，angle增加90度；如果angle方向与平均径向速度的方向（平均速度大于零，则从原点指向聚类中心，反之则从聚类中心指向原点）夹角超过90度，则angle再增加180度。取最终的angle值作为聚类的航向角特征。

在本实施例中，根据聚类特征（中心坐标、长度、宽度、高度、航向角）构造表征目标的矩形包围框，效果如图2所示。

步骤4：对已有航迹进行运动状态预测。这里运动状态取（），则预测可采用线性卡尔曼滤波预测过程：

；

步骤5：将已有航迹预测结果与聚类进行关联。

5.1、利用已有航迹和聚类的长宽，计算对角线长度，按0.6和0.4的权值加权计算关联距离的上限dist_cri；计算已有航迹预测位置(,)与聚类中心的欧式距离，计算距离关联分数：

；

5.2、根据已有航迹和聚类的位置、尺寸、航向角计算矩形框各顶点的坐标和矩形框的面积，计算尺寸关联分数：

；

5.3、计算预测平均径向速度并减去聚类点云平均径向速度，得到差值，计算速度关联分数：

；

其中 为预设阈值，本实施例中取2.5m/s。

5.4、对距离关联分数、尺寸关联分数和速度关联分数加权求和，计算最终得分。本实施例中权值分别为0.4，0.3，0.3。

5.5、计算每一已有航迹与每一聚类的最终得分，构造得分矩阵，进行匹配。在本实施例中，用100减去得分矩阵的每个元素，将得分矩阵转换成成本矩阵。

5.6、成本矩阵的每一行减去该行的最小元素，随后将矩阵的每一列减去该列的最小元素。

5.7、用最少的行线和列线将新矩阵中的零全部穿起来，检查目前是否为最优分配。如果行线和列线没有将矩阵所有元素都穿起来，进入5.8，否则则进入5.9。

5.8、将行线和列线没有穿起来的元素中找到最小元素，将剩余元素减去最小元素，对应行线和列线的交叉点的元素加上最小元素，转回5.7。

5.9、找出每一行对应的0元素和列对应的0元素，根据0元素找到最优分配。

步骤6：对关联成功的航迹采用卡尔曼滤波进行运动状态的更新。

步骤7：对关联失败的航迹进行消亡判定。如果已有航迹的外推帧数已达到预设阈值，本实施例中取3帧，则删除航迹，否则将已有航迹的预测结果作为外推结果。

步骤8：基于关联失败的聚类建立新航迹。新航迹的运动状态参数为：

；

其中 为聚类航向角特征，为点云径向速度的平均方向角。新航迹的长宽取聚类的长度特征和宽度特征，新航迹的航向角取聚类的航向角特征。

步骤9：对当前航迹（包括已有航迹更新、外推结果和新航迹）计算横摆角速度。横摆角速度 的大小为：

；

速度向量与加速度向量的叉乘结果 的正负作为横摆角速度的正负。

步骤10：对当前航迹进行尺寸和航向角更新。将航迹尺寸与聚类尺寸进行加权平均，本实施例中权值分别为0.7和0.3，得到航迹尺寸更新结果：

；

根据航迹速度计算速度航向角，将聚类航向角、速度航向角和航迹航向角进行加权平均，本实施例中权值分别为0.4，0.3，0.3，得到航迹航向角更新结果：

；

该基于4D毫米波雷达实现目标特征提取与多目标跟踪的装置，其中，所述的装置包括：

处理器，被配置成执行计算机可执行指令；

存储器，存储一个或多个计算机可执行指令，所述计算机可执行指令被所述处理器执行时，实现上述所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

该基于4D毫米波雷达实现目标特征提取与多目标跟踪的处理器，其中，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

该计算机可读存储介质，其中，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行装置执行的软件或固件来实现。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成的，程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

上述提到的存储介质可以是只读存储器，磁盘或光盘等。

在本说明书的描述中，参考术语“一实施例”、“一些实施例”、“示例”、“具体示例”、或“实施例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

采用了本发明的该基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法、装置、处理器及其计算机可读存储介质，其相较于传统的雷达多目标跟踪方法而言，从雷达聚类点云中获取了除位置以外的目标尺寸、航向角等信息，并充分结合这些信息提高多目标跟踪中的航迹与观测的关联可靠性；并且针对4D毫米波雷达，还提供了新的航迹观测模型，通过该模型可以充分利用同一目标的各个观测点协同进行更新；除此之外，本发明还引入了目标横摆角速度，以此能够实现对转弯机动目标的更稳定地跟踪。

在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。

Claims (9)

1.一种基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其特征在于，所述的方法包括以下步骤：

（1）获取雷达输出的点云数据，并对点云数据进行预处理，剔除野值；

（2）对经过预处理后得到的点进行DBSCAN聚类处理；

（3）对获取到的每一聚类均进行目标特征提取处理；

（4）对当前已有航迹进行预测，并将预测结果与聚类结果进行全局关联匹配；

（5）根据关联聚类结果，执行相应的多目标跟踪运动状态处理，获取横摆角速度；

（6）基于目标运动状态，更新目标尺寸信息以及航向角信息；

所述的步骤（2）具体为：

根据点云信息中的各个点的坐标位置以及相对径向速度，进行DBSCAN聚类处理，具体为：

；

其中， i ，j 表示参与判别的两个点的索引，x ，y ，z 分别表示每个点的坐标位置，vr表示相对径向速度；

所述的步骤（3）具体包括以下步骤：

（3.1）计算聚类平均径向速度 、x平均值  和y平均值  ；

（3.2）统计聚类点在 z 方向的最大值和最小值，用最大值减去最小值的差作为聚类高度特征；

（3.3）将同一聚类下的每个点的x，y坐标分别减去， 得到  ，；

（3.4）以1°为间隔，遍历0至90°范围，用每个角度对得到的所有，进行旋转，确定旋转后的、 、和，并通过以下公式计算损失函数：

；

并记录所述的损失函数最小的角度值 angle 及相应的、、 和；

（3.5）使用  和  中的较大者作为聚类长度特征，将较小者作为聚类宽度特征，并用  ， 的平均值  和  ， 的平均值 经旋转－angle 角度后，再加上 ，  后得到的值作为聚类中心坐标（ ，）；

（3.6）如果 ，angle增加90度，且angle方向与平均径向速度的方向夹角超过90度，则angle再增加180度，取最终的angle值作为聚类的航向角特征；

（3.7）根据前述获取到的所述的聚类中心坐标、聚类长度特征、聚类宽度特征、聚类高度特征以及航向角特征，构造聚类矩形目标包围框，以供后续进行目标特征提取处理；

所述的步骤（4）具体包括以下步骤：

（4.1）取运动状态为，并采用线性卡尔曼滤波方式进行已有航迹预测处理，具体为：

；

其中，为x向速度分量，为x向加速度分量，为y向速度分量，为y向加速度分量，为目标x向位置预测值，为x向速度分量预测值，为x向加速度分量预测值，为目标y向位置预测值，为y向速度分量预测值，为y向加速度分量预测值，T为相邻两帧雷达点云间的时间间隔；

（4.2）利用已有航迹的长和宽计算对角线长度dist_cri_1：

；

利用聚类长度和宽度特征，计算对角线长度dist_cri_2：

；

并按0.6和0.4的权值加权计算关联距离的上限dist_cri：

；

计算已有航迹预测位置(,)与聚类中心的欧式距离：

；

并根据以下公式计算距离关联分数：

；

（4.3）根据已有航迹预测位置、聚类位置、尺寸以及航向角，计算所述的聚类矩形目标包围框各顶点的坐标和矩形框的面积，并根据以下公式计算尺寸关联分数：

；

（4.4）根据已有航迹的预测位置、预测速度和横摆角速度，通过以下公式计算待关联聚类每一点的预测径向速度：

；

其中，  表示预测径向速度， 表示目标横摆角速度， 表示聚类点云中每个点的径向速度方向；

对同一聚类内的各点预测径向速度取平均值得到平均预测径向速度，并与聚类实测的平均径向速度求差，得到差值 delta_vr，并通过以下公式计算速度关联分数：

；

其中，  为预设阈值；

（4.5）将上述计算获取的距离关联分数、尺寸关联分数以及速度关联分数进行加权求和，以获得航迹与聚类的最终关联分数；

（4.6）根据每个聚类与每个已有航迹的最终关联分数，构造关联矩阵，包括：矩阵的行数等于已有航迹的个数，矩阵的列数为当前聚类的个数，矩阵第i行第j列的值为第i个已有航迹与第j个聚类的关联分数，基于关联矩阵进行全局关联匹配。

2.根据权利要求1所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其特征在于，所述的步骤（1）具体为：

获取雷达输出的点云数据，并对所述的点云数据进行删除速度异常的点、删除高度异常的点、过滤感兴趣区域以外的点的数据预处理，以剔除野值。

3.根据权利要求1所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其特征在于，所述的步骤（5）包括：

采用以下计算公式建立航迹观测模型：

;

其中，， 为聚类中心， 为聚类点云平均径向速度， 为聚类点云的平均方位角观测的余弦值平均，为聚类点云的平均方位角观测的正弦值平均，、、、、、 为航迹的运动状态。

4.根据权利要求3所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其特征在于，所述的步骤（5）具体包括以下步骤：

（5.1）判断对应的聚类与每个已有航迹是否关联成功，如果是，则进入步骤（5.2），否则，进入步骤（5.4）；

（5.2）对已关联的已有航迹，根据航迹预测运动状态和所述的航迹观测模型，计算航迹的预测观测值 、和：

;

（5.3）将该聚类下相应的聚类特征值减去预测观测值，得到残差，并将该残差乘以滤波增益矩阵K，并加上航迹预测运动状态，得到已关联航迹运动状态的更新结果：

;

其中 、、、、 和为更新后的航迹运动状态；

（5.4）对关联失败的航迹进行消亡判定，如果其连续累计未关联次数小于消亡条件的预设阈值，则用航迹预测结果作为更新结果继续进行航迹外推，否则直接删除该航迹；

（5.5）对关联失败的聚类建立新航迹，新航迹的运动状态参数采用以下公式进行表示：

;

其中， 为聚类航向角特征，为点云径向速度的平均方向角，、为聚类中心， 为聚类点云平均径向速度；

（5.6）根据当前获取到的已关联航迹运动状态的更新结果、航迹外推结果以及新航迹运动状态计算结果，通过以下公式计算对应的横摆角速度：

;

其中，速度向量与加速度向量的叉乘结果  的正负取值作为横摆角速度的正负取值。

5.根据权利要求4所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其特征在于，所述的步骤（5.5）具体包括以下步骤：

（5.5.1）将聚类中心作为新航迹起始位置；

（5.5.2）将聚类航向角特征作为新航迹的速度方向，聚类平均径向速度反投影至航向角方向得到新航迹的速度大小；

（5.5.3）将新航迹加速度取零；

（5.5.4）取所述的聚类长度特征作为新航迹的长度，取所述的聚类宽度特征作为新航迹的宽度，取聚类航向角特征作为新航迹的航向角。

6.根据权利要求4所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法，其特征在于，所述的步骤（6）具体包括以下步骤：

（6.1）将航迹长度与聚类长度特征进行加权平均，得到航迹长度更新结果，将航迹宽度与聚类宽度特征进行加权平均，得到航迹宽度更新结果；

（6.2）基于航迹运动状态的速度矢量和加速度矢量，计算垂直于速度矢量方向的加速度分量，然后用垂直加速度分量的模除以速度矢量的模得到目标横摆角速度的大小，速度矢量与加速度矢量的叉积方向为横摆角速度的方向；

（6.3）根据航迹速度计算速度航向角，将聚类航向角特征、速度航向角和航迹航向角进行加权平均，得到航迹航向角更新结果。

7.一种基于4D毫米波雷达实现目标特征提取与多目标跟踪的装置，其特征在于，所述的装置包括：

处理器，被配置成执行计算机可执行指令；

存储器，存储一个或多个计算机可执行指令，所述计算机可执行指令被所述处理器执行时，实现权利要求1至6中任一项所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

8.一种基于4D毫米波雷达实现目标特征提取与多目标跟踪的处理器，其特征在于，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1至6中任一项所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

9.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现权利要求1至6中任一项所述的基于4D毫米波雷达实现目标特征提取与多目标跟踪的方法的各个步骤。

Images