CN113160280B - 一种基于激光雷达的动态多目标跟踪方法 - Google Patents

Abstract

本发明公开了一种基于激光雷达的动态多目标跟踪方法，首先获取激光雷达目标检测结果，并显示检测结果，然后采用匈牙利算法进行目标关联，并同时通过目标预测位置和当前检测结果之间的交并比判断目标的生成与消失，当关联成功时，用检测框位置对目标位置进行更新，根据检测结果输出的属性标签对应的多模态卡尔曼滤波，估计目标的运动信息，关联失败时，删除未关联的目标状态并生成当前时刻新检测的目标。在一般环境中，相比于传统的多目标跟踪方法，本发明公开的多目标跟踪方法能够有效提高多目标跟踪的准确度。

Description

一种基于激光雷达的动态多目标跟踪方法

技术领域

本发明属于机器人自主导航技术领域，具体涉及一种基于激光雷达的动态多目标跟踪方法。

背景技术

随着应用需求的不断提升，移动机器人需要具备代替人类执行愈加复杂、高级的任务的能力。移动机器人在军事、交通、服务等领域中拥有越来越广泛的应用前景。移动机器人需要通过传感器感知环境和自身状态，才能实现在有障碍物的环境中面向目标的自主运动，从而完成复杂作业功能的机器人系统。在动态环境中，移动机器人在实现自身定位的同时，需要对周边复杂障碍物进行准确检测与预测以规划其安全行驶路线，因此，准确感知并跟踪动态障碍物对机器人自主运行有着不可或缺的作用。

常用的环境感知传感器主要有相机、激光雷达等。由于移动机器人所处环境的复杂性，激光雷达凭借视野范围广、光线依赖弱、测距精度高的优势，成为障碍物识别的主流传感器之一。当前主流的移动机器人的多目标相对定位与跟踪方法(Multiple ObjectTracking，MOT)大可分为三类：基于卡尔曼滤波匹配的后端追踪优化算法、基于多线程单目标追踪器的多目标追踪算法、基于深度学习的多目标追踪算法。

基于多线程单目标追踪器的多目标追踪算法代表性算法有KCF算法、LEDS算法、JDE算法和FAIRMOT算法，该类算法为每一个物体都分配了特定跟踪器，跟踪效果较好。但该算法对目标尺度变化要求较大，参数调试过程复杂，该算法计算量较大实时性较差。以DAN算法与MCSA算法为代表的基于深度学习的多目标跟踪算法，由于其网络架构设计难度较大，现阶段端到端的神经网络算法较少，还处于研究阶段，有待进一步落地应用。基于卡尔曼滤波器的多目标跟踪的步骤为：获得被检测到的障碍物标签，进行特征提取与运动预测，通过计算相似度进行帧间数据关联，根据关联结果更新当前时刻新检测的目标的运动状态。然而在环境中，同时存在运动特性相差较大的目标，基于单一运动模型的跟踪器无法较好地同时跟踪两类目标。

发明内容

针对现有技术的不足，本发明的目的在于提供一种基于激光雷达的动态多目标跟踪方法，以解决传统跟踪方法无法同时较好地对多类动态目标进行跟踪的问题；本发明提高了对动态多目标的跟踪准确率。

为达到上述目的，本发明的技术方案为：

一种基于激光雷达的动态多目标跟踪方法，包括步骤如下：

步骤一：获取原始激光雷达点云并进行聚类，然后对聚类后的结果进行目标识别检测，并显示检测结果；

步骤二：基于目标类别的不同，建立不同的状态方程表达式，即运动状态空间模型，并根据上一时刻的系统状态和所述运动状态空间模型，预测当前时刻的系统状态；

步骤三：对已跟踪目标和当前时刻新检测的目标进行关联：

步骤四：通过计算，判断已跟踪目标和当前时刻新检测的目标的关联结果，基于所述关联结果构建关联矩阵matrix_assign_valid；

步骤五：对于每一个已跟踪目标，若在所述关联矩阵matrix_assign_valid中成功关联到当前时刻新检测的目标m_j，则针对不同类别目标分别进行多模态卡尔曼滤波，然后输出已跟踪目标的运动信息；

若未关联到当前时刻新检测的目标m_j，则进行目标出现与消失的判断：对当前时刻新检测的目标，若没有被关联上，将用于生成当前时刻新检测的目标，对已跟踪目标如果没有被关联到一个当前时刻新检测的目标，将未关联目标从已跟踪目标集合中删除。

优选地，所述目标包括低速目标，所述低速目标的运动状态空间模型表示为

其中，p_x、p_y分别表示低速目标在x、y方向的位置分量，v_x、v_y分别表示低速目标在x、y方向的速度分量；

所述低速目标的运动模型的状态转移函数表示为

其中，Δ_t表示上一时刻到当前时刻的时间间隔，

为上一时刻t的运动状态，

为当前时刻t+Δt的运动状态。

优选地，所述目标还包括高速目标，所述高速目标的运动状态空间模型表示为

其中，x、y分别表示高速目标在x、y方向的位置分量，v为前向运动速度，

为偏航角，即追踪的高速目标在当前目标坐标系下与x轴的夹角，逆时针方向为正，取值范围是[0,2π)，ω是偏航角速度；

所述高速目标的运动模型的状态转移函数表示为

其中，

为上一时刻t的运动状态，

为当前时刻t+Δt的运动状态。

优选地，所述步骤三中，使用匈牙利算法对已跟踪目标和当前时刻新检测的目标进行关联。

优选地，记所述已跟踪目标为n个，所述当前时刻新检测的目标为m个，构建一个n×m维的得分矩阵score_matrix，所述得分矩阵score_matrix内各元素满足如下条件：

其中，max_dist为设定的最大距离阈值，表示目标预测位置和实际量测位置之间的最大偏差；dist(i,j)表示已跟踪目标预测位置和当前时刻新检测的目标位置之间的距离，dist(i,j)＝||trac_i.pose-m_j.pose||；min(dist(i,j),max_dist)表示取两者的最小值；

trac_i.pose表示直到上一时刻目标的位置，即已跟踪目标预测位置，m_j.pose表示当前时刻目标的位置，即当前时刻新检测的目标位置。

优选地，采用匈牙利算法求解得分矩阵score_matrix，找到已跟踪目标和当前时刻新检测的目标之间的最优分配关系，记最终的求解结果为关联矩阵matrix_assign_k×2，

即将每个trac_i最优地关联到一个m_j，找到k对对应关系，且必然有k≤min(n,m)。

优选地，所述步骤四中是否关联成功的方法为：当前时刻目标的预测位置和当前时刻目标检测框之间的交并比IOU，若IOU小于指定IOU阈值，则关联失败，否则关联成功。

优选地，所述步骤五中，针对不同类别的目标，采用不同的卡尔曼滤波，针对低速目标采用线性卡尔曼滤波器，针对高速目标采用无损卡尔曼滤波器。

本发明公开了以下技术效果：本发明通过对不同运动特性的物体分别建立不同的运动模型——针对低速目标建立匀速运动模型并采用线性卡尔曼滤波器跟踪估计，针对高速目标建立匀速匀角速度运动模型并采用无损卡尔曼滤波器跟踪估计，在环境中能够有效地对多类动态目标同时进行跟踪，提高了对动态多目标的跟踪准确率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1所示，本发明提供一种基于激光雷达的动态多目标跟踪方法，包括步骤如下：

步骤一：基于欧式距离分割对原始激光雷达点云进行聚类，利用环形卷积神经网络PolarNet对聚类结果进行识别检测，以3D检测框显示检测结果；

1.1记3D检测结果为：

M＝{m_j},j＝1,2,...,m

其中m为检测物体的数量，m_j表示每一个检测出的目标，包含如下信息：

其中，label：类别标签；

shape：形状边界盒，主要包括圆柱、长方体、多边形，并且低速目标使用圆柱进行描述，高速目标使用长方体，未知障碍物使用多边形；

pose：边界盒中心在全局坐标系下的位置和姿态(将检测结果通过叠加当前时刻激光雷达自身的位姿转换至全局系得到)。

1.2记直到上一时刻已跟踪目标集合为

TRACKERS＝{trac_i},i＝1,2,...,n

其中，n为已跟踪物体的数量，每一个trac_i表示一个跟踪的目标，包含如下信息：

其中，label：类别标签；shape：形状边界盒，主要包括圆柱、长方体、多边形，并且低速目标使用圆柱进行描述，高速目标使用长方体，未知障碍物使用多边形；pose：边界盒中心在全局坐标系下的位置和姿态，且该位姿为上一时刻位姿通过步骤二中的状态转移函数得到的当前时刻的预测位姿

object_id：唯一标识该物体的索引号；velocity：边界盒中心在全局坐标系下的速度。

步骤二：基于目标类别的不同，建立不同的状态方程表达式，即运动状态空间模型，并根据上一时刻的系统状态和所述运动状态空间模型，预测当前时刻的系统状态

2.1对于低速目标的运动状态空间模型表示为：

其中，p_x、p_y分别表示低速目标在x、y方向的位置分量，v_x、v_y分别表示低速目标在x、y方向的速度分量；

所述低速目标的运动模型的状态转移函数表示为

其中，Δt表示上一时刻到当前时刻的时间间隔，

为上一时刻t的运动状态，

为当前时刻t+Δt的运动状态。

即，使用状态转移函数

将低速目标的状态从上一时刻t的

预测至当前时刻t+Δt的

2.2对于高速目标的运动状态空间模型表示为

其中，x、y分别表示高速目标在x、y方向的位置分量，v为前向运动速度，

为偏航角，即追踪的高速目标在当前目标坐标系下与x轴的夹角，逆时针方向为正，取值范围是[0,2π)，ω是偏航角速度。

将所述高速目标的运动模型的状态转移函数表示为

其中，

为上一时刻t的运动状态，

为当前时刻t+Δt的运动状态。

即，使用

状态转移函数将高速目标的状态从上一时刻t的

预测至当前时刻t+Δt的

步骤三：采用匈牙利算法对已跟踪目标和当前时刻新检测的目标进行关联：

3.1根据n个已跟踪目标和m个当前时刻新检测的目标构建一个n×m维的得分矩阵score_matrix，该矩阵内各元素满足如下条件：

其中，max_dist为设定的最大距离阈值，表示目标预测位置和实际量测位置之间的最大偏差；dist(i,j)表示已跟踪目标预测位置和当前时刻新检测的目标位置之间的距离，dist(i,j)＝||trac_i.pose-m_j.pose||；min(dist(i,j),max_dist)表示取两者的最小值，trac_i表示直到上一时刻目标，m_j表示当前时刻目标。

3.2采用Kuhn–Munkres算法(匈牙利算法)求解得分矩阵score_matrix，找到已跟踪目标trackers和当前时刻新检测的目标measurements之间的最优分配关系，记最终的求解结果为关联矩阵matrix_assign_k×2，

即将每个trac_i最优地关联到一个m_j，一种找到k对对应关系，且必然有k≤min(n,m)。

步骤四：计算目标在当前时刻中的预测位置和当前时刻目标检测框之间的交并比IOU，若IOU小于指定IOU阈值，则指派结果无效即关联失败，否则关联成功：

4.1将全局空间体素化，即将空间划分为若干小立方体体素，对所述关联矩阵matrix_assign的每一行计算交并比：

其中，IOU_ij表示trac_i和m_j之间的交并比，S_{trac_i}为trac_i的边界盒占据的空间，S_{m_j}为m_j的边界盒占据的空间，n_{trac_i}为S_{trac_i}占据的体素数量，n_{m_j}为S_{m_j}占据的体素数量，n_{shared_ij}为同时被S_{trac_i}与S_{m_j}占据的体素数量。

4.2若最终计算得到的IOU_ij满足IOU_ij≥IOU_thread，则指派关联结果有效，保留该关联结果；若最终计算得到的IOU_ij满足IOU_ij＜IOU_thread，则指派关联结果无效，在关联矩阵matrix_assign中删除该关联结果。设最终评估后指派了k_invalid对关联无效，则最终保留的关联结果有k_valid＝k-k_invalid，记最终得到的关联矩阵为matrix_assign_valid，维度为k_valid×2。

步骤五：基于步骤四中的关联矩阵，对于每一个已跟踪的目标trac_i，在步骤四中得到的关联矩阵matrix_assign_valid中搜索关联的量测m_j，若成功搜索到关联的量测目标m_j，则针对不同类别目标分别进行多模态卡尔曼滤波：

5.1针对低速目标，采用线性卡尔曼滤波器(KF)

a)根据低速目标的运动状态空间模型，构建如下状态一步预测方程：

x_p,k+1＝A_px_p,k+γ

其中，

为k到k+1时刻的系统一步转移矩阵，Δt为k到k+1时刻的时间间隔，

为k时刻的系统估计状态量，x_k+1为k+1时刻的系统预测状态量，

为k时刻的系统噪声，a_x、a_y分别为k时刻x、y方向的运动加速度。

b)根据k-1时刻最优估计值的误差P计算k时刻预测值的误差

其中，W＝G·G^T·a²为过程噪声的协方差矩阵，G＝[0.5Δt² 0.5Δt² Δt Δt]^T，a＝a_x＝a_y为k时刻x和y方向的运动加速度。

c)构建量测方程并计算滤波增益：

量测方程构建如下：

z_p,k＝Z_px_p,k+V_k

其中，z_k＝(v_x v_y)^T为k时刻量测，使用三维激光雷达传感器可通过插值获得低速目标的速度v_x,v_y，量测矩阵

V_k为k时刻的量测噪声。

k+1时刻卡尔曼增益K_k+1计算如下：

其中，

为量测噪声的协方差矩阵，

分别为v_x、v_y的量测噪声方差。

d)计算k+1时刻最优估计值

e)更新k+1时刻最优估计值

的误差P_k+1：

P_k+1＝(I-K_k+1Z_p)P

5.2针对高速目标，采用无损卡尔曼滤波器(UKF)

a)根据高速目标运动状态空间模型，构建如下状态一步预测方程：

x_v,k+1＝g(x_v,k)+noise

其中g()表示高速目标运动模型的状态转移函数，x_v,k为k时刻估计状态，x_v,k+1为k+1时刻预测状态，noise为系统噪声：

其中Δt表示k到k+1时刻的时间间隔，

其中，a_a、a_ω为直线加速度和转角加速度，

计算过程噪声的协方差矩阵：

W＝E[noise·noise^T]＝E[Gaa^TG^T]＝G·E[aa^T]·G^T

其中，

分别为a_a、a_ω的方差。

根据上述系统状态一步预测方程，进行系统状态的一步预测：

i.对系统状态进行扩展，将过程噪声a_a和a_ω加入状态量，得到增广状态向量

计算增广状态向量

的均值：

其中，μ为x_v,k的均值。

计算

的误差协方差矩阵：

其中，P为x_v,k的误差协方差矩阵，

是过程噪声的协方差矩阵，

分别为a_a、a_ω的方差。

ii.计算sigma点：

其中，

为增广状态

的维度，σ表示sigma点远离分布均值的程度，σ越大，sigma点就越远离分布的均值；σ越小，sigma点就越靠近分布的均值。

iii.由sigma点集，用高速目标运动模型的非线性函数g()预测状态：

其中，

包含7个状态量(其中考虑了两个噪声量)，

包含5个状态量。

a)计算预测sigma点集的均值和方差：

计算各个sigma点的权重：

计算预测sigma点集的均值x_v,k+1|k和方差P_k+1|k：

b)构建量测方程并计算滤波增益：

i.高速目标运动模型的量测方程构建如下：

其中，r为高速目标相对于机体系坐标原点的距离，θ为目标与机体系x轴的夹角，

为高速目标与本系统的相对距离变化率，即目标的实际速度在传感器与目标连线上的分量。

ii.对于上述非线性函数，进行无损变换

将5.2.a.iii中生成的sigma点集，通过观测函数，映射到新的sigma点集：

其中，

为第i个sigma点预测的状态量，

为映射的量测量。

计算新sigma点集的均值z_v,k+1|k和方差S_k+1|k：

计算新sigma点集状态与量测的互相关函数：

iii.计算卡尔曼增益：

c)更新状态：

x_v,k+1|k+1＝x_v,k+1|k+K_k+1|k(z_v,k+1-z_v,k+1|k)

其中，z_v,k+1是最新得到的测量，而z_v,k+1|k则是根据先验计算出来的在量测空间的测量。

d)更新状态协方差矩阵：

其中，P_k+1|k为预测sigma点集的协方差矩阵，P_k+1|k+1为最终估计状态的协方差矩阵。

若关联失败，则进行目标出现与消失的判断：对当前时刻新检测的目标，若没有被关联上，将用于生成当前时刻新检测的目标，对已跟踪目标没有关联上，将未关联目标从已跟踪目标集合中删除。

a)对于每一个已跟踪的目标trac_i，在步骤(4)中得到的关联矩阵matrix_assign_valid中搜索关联的量测m_j，若未搜索到关联的量测目标，进入death判定，认为目标离开视野，需要相应地销毁唯一标识，将未关联目标从已跟踪目标集合TRACKERS＝{trac_i},i＝1,2,...,n中删除。

b)对于每一个当前时刻新检测的目标m_j，在步骤(4)中得到的关联矩阵matrix_assign_valid中搜索关联的已跟踪目标，若未搜索到关联关系，进入birth判定，认为当前时刻新检测的目标进入视野，相应地创建唯一标识，创建新的已跟踪目标对象，使用速度设置为零的边界框信息初始化跟踪器，由于此时无法观测到速度，因此速度分量设置为0，速度分量的协方差用较大的值初始化，反映出这种不确定性：

且满足trac_k.pose.v_x＝0，trac_k.pose.v_y＝0。

所述未搜索到关联关系指的是“一个当前时刻新检测的目标没有被关联到一个上一时刻已跟踪的目标”，那么就判定这个新检测的目标是新出现的目标，而不是上一时刻跟踪的某一个目标。

以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (7)

1.一种基于激光雷达的动态多目标跟踪方法，其特征在于，包括如下步骤：

步骤一：获取原始激光雷达点云并进行聚类，然后对聚类后的结果进行目标识别检测，并显示检测结果；

步骤二：基于目标类别的不同，建立不同的状态方程表达式，即运动状态空间模型，并根据上一时刻的系统状态和所述运动状态空间模型，预测当前时刻的系统状态；

步骤三：对已跟踪目标和当前时刻新检测的目标进行关联：

步骤四：通过计算，判断已跟踪目标和当前时刻新检测的目标的关联结果，基于所述关联结果构建关联矩阵matrix_assign_valid；

步骤五：对于每一个已跟踪目标，若在所述关联矩阵matrix_assign_valid中成功关联到当前时刻新检测的目标m_j，则针对不同类别目标分别进行多模态卡尔曼滤波，然后输出已跟踪目标的运动信息；

若未关联到当前时刻新检测的目标m_j，则进行目标出现与消失的判断：对当前时刻新检测的目标，若没有被关联上，将用于生成当前时刻新检测的目标，对已跟踪目标如果没有被关联到一个当前时刻新检测的目标，将未关联目标从已跟踪目标集合中删除；

所述目标包括低速目标，所述低速目标的运动状态空间模型表示为

其中，p_x、p_y分别表示低速目标在x、y方向的位置分量，v_x、v_y分别表示低速目标在x、y方向的速度分量；

所述低速目标的运动模型的状态转移函数表示为

其中，Δt表示上一时刻到当前时刻的时间间隔，

为上一时刻t的运动状态，

为当前时刻t+Δt的运动状态。

2.根据权利要求1所述的基于激光雷达的动态多目标跟踪方法，其特征在于，所述目标还包括高速目标，所述高速目标的运动状态空间模型表示为

其中，x、y分别表示高速目标在x、y方向的位置分量，v为前向运动速度，

为偏航角，即追踪的目标在当前目标坐标系下与x轴的夹角，逆时针方向为正，取值范围是[0,2π)，ω是偏航角速度；

所述高速目标的运动模型的状态转移函数表示为

其中，

为上一时刻t的运动状态，

为当前时刻t+Δt的运动状态。

3.根据权利要求1所述的基于激光雷达的动态多目标跟踪方法，其特征在于，所述步骤三中，使用匈牙利算法对已跟踪目标和当前时刻新检测的目标进行关联。

4.根据权利要求3所述的基于激光雷达的动态多目标跟踪方法，其特征在于，记所述已跟踪目标为n个，所述当前时刻新检测的目标为m个，构建一个n×m维的得分矩阵score_matrix，所述得分矩阵score_matrix内各元素满足如下条件：

其中，max_dist为设定的最大距离阈值，表示目标预测位置和实际量测位置之间的最大偏差；dist(i，j)表示已跟踪目标预测位置和当前时刻新检测的目标位置之间的距离，dist(i，j)＝||trac_i.pose-m_j.pose||；min(dist(i，j)，max_dist)表示取两者的最小值；

trac_i.pose表示直到上一时刻目标的位置，即已跟踪目标预测位置，m_j.pose表示当前时刻目标的位置，即当前时刻新检测的目标位置。

5.根据权利要求4所述的基于激光雷达的动态多目标跟踪方法，其特征在于，采用匈牙利算法求解得分矩阵score_matrix，找到已跟踪目标和当前时刻新检测的目标之间的最优分配关系，记最终的求解结果为关联矩阵matrix_assign_k×2，

即将每个trac_i最优地关联到一个m_j，找到k对对应关系，且必然有k≤min(n，m)。

6.基于权利要求1所述的基于激光雷达的动态多目标跟踪方法，其特征在于，所述步骤四中是否关联成功的方法为：当前时刻目标的预测位置和当前时刻目标检测框之间的交并比IOU，若IOU小于指定IOU阈值，则关联失败，否则关联成功。

7.基于权利要求1所述的基于激光雷达的动态多目标跟踪方法，其特征在于，

所述步骤五中，针对不同类别的目标，采用不同的卡尔曼滤波，针对低速目标采用线性卡尔曼滤波器，针对高速目标采用无损卡尔曼滤波器。

Images