CN117036408A - 一种动态环境下联合多目标跟踪的物体slam方法 - Google Patents

Abstract

本发明公开了一种动态环境下联合多目标跟踪的物体SLAM方法，完成在动态环境中对动态与静态对象的定位与建图任务。首先，为精确获得物体的运动区域，通过短时密集连接(STDC)网络进行语义分割得到运动物体掩膜，依据运动物体掩膜对特征点进行区分得到静态特征点与动态特征点。同时，将图像输入到单目3D目标检测(SMOKE)网络进行三维目标检测，并将检测结果输入到数据关联模块进行多目标跟踪。利用动静态特征点与数据关联后的目标位姿共同估计相机与动静态物体位姿。最后，依据动静态特征点、相机与动静态物体位姿进行跟踪优化，提升定位与建图精度。本方法与传统SLAM方法相比，有效提升了SLAM系统的鲁棒性，建立的地图语义明确、可解释性好。

Description

一种动态环境下联合多目标跟踪的物体SLAM方法

(一)技术领域

本发明属于计算机视觉领域，特别涉及同时定位与建图技术，尤其是一种动态环境下联合多目标跟踪的物体SLAM方法。

(二)背景技术

随着人工智能和机器人技术的快速发展，移动机器人在现实世界中的应用日益广泛，从自动驾驶汽车到工业自动化，从增强现实到智能巡检。然而，这些机器人在动态环境中的导航和定位问题仍然是一个挑战。传统的SLAM(Simultaneous Localization andMapping)技术在解决这一问题方面迈出了重要的一步，但随着技术的进步和应用的多样性，新的问题和需求也逐渐浮现出来。

其中之一是如何在存在动态物体的环境中实现精准的定位、地图构建和目标跟踪。传统的SLAM方法在静态环境中表现出色，但一旦涉及到动态物体(如行人、车辆等)，问题变得复杂起来。这些动态物体的运动可能不受机器人控制，因此，机器人需要能够实时地检测、跟踪和适应这些变化，以确保其定位和地图的准确性。这要求将3D目标跟踪与动态SLAM技术结合起来，不仅仅是传统SLAM与目标跟踪的简单组合，更是要使得两种技术相互促进、协同发展。其核心目标是使移动机器人能够在存在动态物体的环境中，同时实现自身的定位、环境地图的构建，并实时跟踪和预测动态物体的运动。

一些现有的工作致力于解决该问题，论文《CubeSLAM:Monocular 3D ObjectSLAM》中使用点线检测确定VP点，结合VP点与二维检测框确定物体的8个立体角点，进而确定三维锚框。但该方法在环境噪音较大的复杂场景中效果较差且无法检测非立方体外形的物体，如行人、自行车与路灯等。专利《野外环境下多运动目标检测与轨迹预测方法》使用雷达点云与RGB图像作为输入，通过基于点云的目标检测算法确定三维锚框，该方法需要激光雷达作为输入设备，在激光雷达无法作用的环境如雷雨环境中，该方法无法取得很好的效果。专利《一种面向地库环境自主定位的物体级语义SLAM方法》使用基于深度学习的3D目标检测算法检测物体，但其仅能够建立静态语义地图，无法建立与维护动态地图。

针对现有技术在充斥大量噪音的动态环境中难以实现对动态和静态物体进行鲁棒检测的问题，本发明提供一种在动态环境下联合多目标跟踪的物体SLAM方法，该方法能够有效的提高SLAM系统在动态环境下的精度与鲁棒性，并实时建立与维护动态环境地图。

(三)发明内容

本发明的目的是提供一种在动态环境下联合多目标跟踪的物体SLAM方法。该方法以单目或双目立体图作为输入，结合3D目标检测与3D目标跟踪技术构建动态3D对象与静态3D对象，并与动静态特征点、相机一起进行图优化得到优化后的位姿信息。

为实现上述目的，本发明采用如下技术方案：

S1、获取输入图像序列，包括RGB图像；

S2、对输入帧的RGB图像进行ORB特征点的提取；

S3、将输入帧的RGB图像输入短时密集连接(STDC)网络进行语义分割，得到含有运动物体语义信息的运动物体掩膜；

S4、根据STDC网络获得的物体掩膜，将落在运动物体掩膜上的特征点判定为动态特征点，否则，判定为静态特征点；

S5、将输入帧t的RGB图像输入单目3D目标检测(SMOKE)网络进行三维目标检测，得到该帧中所有目标的目标检测数据其中n_t为检测到的物体的数量，每一个检测结果/>为一个8维向量(x,y,z,l,w,h,θ,s)，其中x,y,z为物体中心的三维坐标，l,w,h为物体的长宽高，θ为朝向角，s为置信度；

S6、对S5所得到的目标检测数据创建对象实例，结合卡尔曼滤波预测之前帧目标在当前帧的位置，使用匈牙利匹配算法进行数据关联以实现目标跟踪，具体步骤如下：

S61、将目标轨迹状态建模为10维向量T＝(x,y,z,θ,l,w,h,v_x,v_y,v_z)，其中v_x,v_y,v_z分别代表目标在3D空间的速度；

S62、在输入帧t中，对前一帧所有目标轨迹状态进行前向预测：

其中，x_est,y_est,z_est为当前帧目标中心的预测坐标，m_t-1为前一帧中的轨迹数，则目标轨迹状态在当前帧被相应的预测为：

其中，为当前帧目标轨迹状态的预测；

S63、计算每一个预测轨迹与检测结果/>间的3D交并比(IoU)，并以此来构建维数为m_t-1×n_t的相似度矩阵；

S64、判断每个3D交并比(IoU)是否小于阈值IoU_min，若小于，则匹配失败，否则匹配成功，得到数据关联的输出：

其中，T_match与D_match为匹配成功的轨迹与检测，w_t为成功匹配的轨迹与检测的数量，T_unmatch与D_unmatch为匹配失败的轨迹与检测；

S65、在D_match中根据每个轨迹的相应检测更新T_match中每个轨迹的状态，得到最终关联轨迹其中每个轨迹的更新状态为T_t ^k＝(x′,y′,z′,θ′,l′,w′,h′,s′,v′_x,v′_y,v′_z)，此处k∈{1,2,...,w_t}，T_t ^k为/>与/>的加权平均值，权重由/>与的状态不确定性确定；

S66、对D_unmatch在连续帧中匹配失败的次数进行计数，若大于F_min帧就创建新的轨迹对象

S67、对T_unmatch在连续帧中匹配失败的次数进行计数，若大于Age_max帧，则取消对其的跟踪；

S7、使用S6得到的对象状态数据，结合S4中得到的动态与静态特征点构建图优化，包括以下步骤：

S71、通过对S6得到的对象状态数据与S4得到的动态与静态特征点计算三维欧氏距离得到动态对象与静态对象的初始位姿，并将动态对象与静态对象的初始位姿设为静态对象节点与动态对象节点；

S72、将S4中得到的动态与静态特征点设为动态点节点与静态点节点；

S73、根据相机在t-1时刻与t时刻的位姿变换T和相机在t-1时刻的位姿确定相机在t时刻的初始位姿，并将其设为相机节点；

S74、根据动态对象在t-1时刻的位姿与t-1时刻到t时刻的时间戳确定动态对象在t时刻的速度，并将其设为速度节点；

S75、以静态点节点与相机节点为顶点构造静态投影边e_cp，以相机节点与动态物体节点为顶点构造动态物体测量边e_co-3D，以静态物体节点与静态点节点为顶点构造静态物体测量边e_op，以相机节点、动态物体节点与动态点节点为顶点构造三元动态重投影边e_dp，以前一帧动态物体节点、当前帧动态物体节点与速度节点构造三元速度估计边e_mo；

S76、对上述构造的图优化问题进行求解，获得当前帧相机、动静态特征点与动静态物体的优化后位姿。

本发明具有如下的有益效果：

(1)本发明通过轻量级单目3D目标检测(SMOKE)网络进行三维目标检测，同时使用短时密集连接(STDC)网络进行语义分割得到运动物体掩膜，解决了在动态环境中检测与跟踪目标噪音过大的问题，有效保证了系统的精度与鲁棒性，同时也使系统能够实时运行；

(2)本发明通过构建运动物体的运动学模型，并将其纳入图优化问题中，实现对动态环境中运动物体运动状态的有效估计。同时，还对环境中的动态和静态物体进行语义建图，为随后的路径规划阶段提供了更为丰富的场景信息，包括目标位姿、目标速度以及语义属性等关键内容；

(3)相较于传统的动态SLAM仅仅利用环境中的静态特征点来对相机进行定位，本发明还充分利用了动态特征点以及动态物体和静态物体的位姿等关键信息进行定位与建图。

(四)附图说明

图1为SLAM系统的总体流程图；

图2为数据关联流程图；

图3为本发明方法3D目标检测与跟踪效果图；

图4为本发明方法对动静对象的定位与建图效果图。

(五)具体实施方式

为了使本发明的目的、技术方案以及优点更加清楚明白，以下结合附图及试验实例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。本发明系统的总体流程图如图1所示。

S1、获取输入图像序列，包括RGB图像；

S2、对输入帧的RGB图像进行ORB特征点的提取；

S3、将输入帧的RGB图像输入短时密集连接(STDC)网络进行语义分割，得到含有运动物体语义信息的运动物体掩膜；

S4、根据STDC网络获得的物体掩膜，将落在运动物体掩膜上的特征点判定为动态特征点，否则，判定为静态特征点；

S5、将输入帧t的RGB图像输入单目3D目标检测(SMOKE)网络进行三维目标检测，得到该帧中所有目标的目标检测数据其中n_t为检测到的物体的数量，每一个检测结果/>为一个8维向量(x,y,z,l,w,h,θ,s)，其中x,y,z为物体中心的三维坐标，l,w,h为物体的长宽高，θ为朝向角，s为置信度；

S6、对S5所得到的目标检测数据创建对象实例，结合卡尔曼滤波预测之前帧目标在当前帧的位置，使用匈牙利匹配算法进行数据关联以实现目标跟踪，数据关联流程图如图2所示，具体步骤如下：

S61、将目标轨迹状态建模为10维向量T＝(x,y,z,θ,l,w,h,v_x,v_y,v_z)，其中v_x,v_y,v_z分别代表目标在3D空间的速度；

S62、在输入帧t中，对前一帧所有目标轨迹状态进行前向预测：

其中，x_est,y_est,z_est为当前帧目标中心的预测坐标，m_t-1为前一帧中的轨迹数，则目标轨迹状态在当前帧被相应的预测为：

其中，为当前帧目标轨迹状态的预测；

S63、计算每一个预测轨迹与检测结果/>间的3D交并比(IoU)，并以此来构建维数为m_t-1×n_t的相似度矩阵；

S64、判断每个3D交并比(IoU)是否小于阈值IoU_min，若小于，则匹配失败，否则匹配成功，得到数据关联的输出：

其中，T_match与D_match为匹配成功的轨迹与检测，w_t为成功匹配的轨迹与检测的数量，T_unmatch与D_unmatch为匹配失败的轨迹与检测；

S65、在D_match中根据每个轨迹的相应检测更新T_match中每个轨迹的状态，得到最终关联轨迹其中每个轨迹的更新状态为T_t ^k＝(x′,y′,z′,θ′,l′,w′,h′,s′,v′_x,v′_y,v′_z)，此处k∈{1,2,...,w_t}，T_t ^k为/>与/>的加权平均值，权重由/>与的状态不确定性确定；

S66、对D_unmatch在连续帧中匹配失败的次数进行计数，若大于F_min帧就创建新的轨迹对象

S67、对T_unmatch在连续帧中匹配失败的次数进行计数，若大于Age_max帧，则取消对其的跟踪，检测与跟踪的效果图如图3所示，其中，矩形框框住的为检测对象，其上方的数字表示其作为被跟踪对象的编号；

S7、使用S6得到的对象状态数据，结合S4中得到的动态与静态特征点构建图优化，包括以下步骤：

S71、通过对S6得到的对象状态数据与S4得到的动态与静态特征点计算三维欧氏距离得到动态对象与静态对象的初始位姿，并将动态对象与静态对象的初始位姿设为静态对象节点与动态对象节点；

S72、将S4中得到的动态与静态特征点设为动态点节点与静态点节点；

S73、根据相机在t-1时刻与t时刻的位姿变换T和相机在t-1时刻的位姿确定相机在t时刻的初始位姿，并将其设为相机节点；

S74、根据动态对象在t-1时刻的位姿与t-1时刻到t时刻的时间戳确定动态对象在t时刻的速度，并将其设为速度节点；

S75、以静态点节点与相机节点为顶点构造静态投影边e_cp，以相机节点与动态物体节点为顶点构造动态物体测量边e_co-3D，以静态物体节点与静态点节点为顶点构造静态物体测量边e_op，以相机节点、动态物体节点与动态点节点为顶点构造三元动态重投影边e_dp，以前一帧动态物体节点、当前帧动态物体节点与速度节点构造三元速度估计边e_mo；

S76、对上述构造的图优化问题进行求解，获得当前帧相机、动静态特征点与动静态物体的优化后位姿，优化后的效果如图4所示，其中，立方体表示环境中的动静态对象，上方的数字表示其编号，其后的轨迹表示其自被检测到后在环境中的运动轨迹，而图中的交叉对角线矩形组成的轨迹则表示相机的运动轨迹。

如图3、图4所示，本发明在KITTI数据集中，能够提供运动物体的运动轨迹以及建立实时更新的动态地图，验证了3D目标检测方法的准确性、3D多目标跟踪效果的稳定性以及所建立的动态地图的有效性。在实验设备(CPU为Intel Core i7-12700H，显卡为NvidiaGeForce RTX3060)上能够实时运行。该系统对于复杂感知环境中的动态对象具有重要意义。

以上所述具体实施方案，对本发明的发明目的、技术方案和有益效果进行了进一步说明，以上实施例仅用于说明本发明的技术方案，而非对本发明创造保护范围的限制，本领域的普通技术人员应当理解，凡在本发明的技术方案进行修改、等同替换，均包含在本发明的保护范围内。

Claims (1)

1.一种动态环境下联合多目标跟踪的物体SLAM方法，其特征在于，包括以下步骤：

S1、获取输入图像序列，包括RGB图像；

S2、对输入帧的RGB图像进行ORB特征点的提取；

S3、将输入帧的RGB图像输入短时密集连接(STDC)网络进行语义分割，得到含有运动物体语义信息的运动物体掩膜；

S4、根据STDC网络获得的物体掩膜，将落在运动物体掩膜上的特征点判定为动态特征点，否则，判定为静态特征点；

S5、将输入帧t的RGB图像输入单目3D目标检测(SMOKE)网络进行三维目标检测，得到该帧中所有目标的目标检测数据其中n_t为检测到的物体的数量，每一个检测结果/>为一个8维向量(x,y,z,l,w,h,θ,s)，其中x,y,z为物体中心的三维坐标，l,w,h为物体的长宽高，θ为朝向角，s为置信度；

S6、对S5所得到的目标检测数据创建对象实例，结合卡尔曼滤波预测之前帧目标在当前帧的位置，使用匈牙利匹配算法进行数据关联以实现目标跟踪，具体步骤如下：

S61、将目标轨迹状态建模为10维向量T＝(x,y,z,θ,l,w,h,v_x,v_y,v_z)，其中v_x,v_y,v_z分别代表目标在3D空间的速度；

S62、在输入帧t中，对前一帧所有目标轨迹状态进行前向预测：

其中，x_est,y_est,z_est为当前帧目标中心的预测坐标，m_t-1为前一帧中的轨迹数，则目标轨迹状态在当前帧被相应的预测为：

其中，为当前帧目标轨迹状态的预测；

S63、计算每一个预测轨迹与检测结果/>间的3D交并比(IoU)，并以此来构建维数为m_t-1×n_t的相似度矩阵；

S64、判断每个3D交并比(IoU)是否小于阈值IoU_min，若小于，则匹配失败，否则匹配成功，得到数据关联的输出：

其中，T_match与D_match为匹配成功的轨迹与检测，w_t为成功匹配的轨迹与检测的数量，T_unmatch与D_unmatch为匹配失败的轨迹与检测；

S65、在D_match中根据每个轨迹的相应检测更新T_match中每个轨迹的状态，得到最终关联轨迹其中每个轨迹的更新状态为T_t ^k＝(x′,y′,z′,θ′,l′,w′,h′,s′,v′_x,v′_y,v′_z)，此处k∈{1,2,...,w_t}，T_t ^k为/>与/>的加权平均值，权重由/>与/>的状态不确定性确定；

S66、对D_unmatch在连续帧中匹配失败的次数进行计数，若大于F_min帧就创建新的轨迹对象

S67、对T_unmatch在连续帧中匹配失败的次数进行计数，若大于Age_max帧，则取消对其的跟踪；

S7、使用S6得到的对象状态数据，结合S4中得到的动态与静态特征点构建图优化，包括以下步骤：

S71、通过对S6得到的对象状态数据与S4得到的动态与静态特征点计算三维欧氏距离得到动态对象与静态对象的初始位姿，并将动态对象与静态对象的初始位姿设为静态对象节点与动态对象节点；

S72、将S4中得到的动态与静态特征点设为动态点节点与静态点节点；

S73、根据相机在t-1时刻与t时刻的位姿变换T和相机在t-1时刻的位姿确定相机在t时刻的初始位姿，并将其设为相机节点；

S74、根据动态对象在t-1时刻的位姿与t-1时刻到t时刻的时间戳确定动态对象在t时刻的速度，并将其设为速度节点；

S75、以静态点节点与相机节点为顶点构造静态投影边e_cp，以相机节点与动态物体节点为顶点构造动态物体测量边e_co-3D，以静态物体节点与静态点节点为顶点构造静态物体测量边e_op，以相机节点、动态物体节点与动态点节点为顶点构造三元动态重投影边e_dp，以前一帧动态物体节点、当前帧动态物体节点与速度节点构造三元速度估计边e_mo；

S76、对上述构造的图优化问题进行求解，获得当前帧相机、动静态特征点与动静态物体的优化后位姿。