同时定位、建图与移动目标跟踪
同时定位与建图（SLAM）和多目标跟踪（MOT）是自动驾驶或机器人系统中的两个基本问题。
- SLAM的目标：同时构建周围静态环境的模型，并在所构建的地图中精确估计车辆的位姿。
- MOT则专注于：感知并估计周围移动目标的状态。
目前，SLAM和MOT的解决方案通常是分开应用的，并依赖于某些假设。
- 大多数SLAM方法依赖于静态环境假设。尽管已有许多成熟的SLAM解决方案被提出用于单车应用，但在包含大量移动目标的动态环境中，这些方法仍面临显著挑战，可能会导致性能下降甚至失败。
- 另一方面，多目标跟踪通常假设车辆位姿是已知的准确先验信息，而很少关注其估计。这种依赖性使得传统的MOT方法对车辆位姿估计的精度极为敏感。然而，在未知动态环境中，很难保证存在可靠的静态结构，这通常会导致不可靠的位姿估计。
因此，由于上述两个挑战之间的内在相互依赖性，将SLAM和多目标跟踪结合起来是一种可行的解决方案。SLAMMOT，即同时定位、建图与移动目标跟踪，通过将SLAM与MOT结合，能够在动态环境中同时估计自车和移动目标的状态。SLAMMOT不仅能在复杂动态环境中无法满足假设条件时，保证SLAM或MOT的性能，还能够使二者互相受益。具体而言，对于SLAM，考虑移动目标的影响可以将它们从简单的动态干扰转变为增强位姿估计的辅助信息。而对于MOT，SLAM产生的准确车辆位姿估计能够显著提升目标状态的估计。
近年来，SLAMMOT技术取得了显著进展，例如基于视觉的SLAMMOT方法和基于LiDAR的方法。尽管基于视觉的方法在机器人和智能车辆领域中发挥着重要作用，但LiDAR的优势——例如适应各种天气条件、距离测量的高精度、高测距准确性以及较宽的水平视野——吸引了更多研究者的关注。然而，现有的基于LiDAR的SLAMMOT方法仍面临一些挑战。大多数现有的SLAMMOT方法直接采用某种关联策略，在目标预测结果与检测之间进行数据关联。然而，目标预测并不总是准确的，特别是对于连续帧中因临时遮挡或目标距离较远而导致漏检的目标，其预测误差可能会由于缺乏状态更新而累积。同时，一些低质量的检测结果也可能影响数据关联的结果。不准确的预测和低质量的检测可能会导致匹配轨迹的状态不稳定，甚至最终导致跟踪失败，从而同时降低SLAMMOT的性能。这种跟踪问题在严重情况下可能威胁驾驶安全。因此，在SLAMMOT的跟踪过程中，必须在数据关联中考虑预测和检测的置信度。
Conf SLAMMOT[1]是紧耦合基于LiDAR的SLAMMOT解决方案，该方法在多目标跟踪中执行基于置信引导的数据关联，能够在动态环境中同时估计自车与移动目标的状态，并解决由临时遮挡或目标距离过远引起的连续漏检问题。图1展示了所提出的Conf SLAMMOT在KITTI Tracking数据集上的结果。Conf SLAMMOT包含一个SLAM模块、一个目标检测模块、一个基于置信引导数据关联的多目标跟踪模块（依赖于因子图），以及用于耦合和跟踪的联合图优化后端。需要注意的是，SLAM和目标检测模块是可替代的，可以采用各自领域中任何具有代表性的解决方案。基于因子图的跟踪方法利用预测和检测的置信度进行隐式数据关联，不仅可以避免某些基于滤波方法中由于错误初始分配导致的性能下降，还能处理如连续漏检等问题。由SLAM模块和目标跟踪模块分别提供的局部因子图被集成到Conf SLAMMOT的图优化后端中，自车和目标的状态被联合优化。所提出的方法在多种场景下的SLAM和多目标跟踪任务中均取得了优异的结果。
主要亮点：
- 提出了一种基于LiDAR的SLAMMOT方法，该方法将SLAM与基于置信引导数据关联的多目标跟踪紧密结合，能够同时估计自车和移动目标的状态，特别是在漏检场景下实现可靠的跟踪。
- 设计了一个预测器，该预测器采用恒转率和速度模型来估计目标的未来状态，并利用预测置信模型和检测置信分数通过灵活调整数据关联的隐式搜索范围来引导数据关联。
- 基于因子图的跟踪方法利用预测和检测的置信度进行数据关联，避免了某些基于滤波方法中的错误初始分配问题，并有效恢复了由于目标遮挡或距离较远引起的连续漏检跟踪问题。
具体方法
框架概述
所提出的Conf SLAMMOT解决方案在基于图优化的LiDAR SLAM基础上进行了扩展。Conf SLAMMOT的架构如图2所示，主要包括一个可替换的LiDAR里程计模块、一个可替换的移动目标检测模块，以及一个联合图优化后端，这是一个在动态环境中紧耦合LiDAR SLAM和多目标跟踪的方法，用于同时估计自车和移动目标的状态。3D点云信息分别作为LiDAR里程计模块和移动目标检测网络的输入。所使用的LiDAR SLAM方法（本方案中采用LeGO-LOAM模块）由LiDAR里程计模块和系统整体的局部图优化后端组成。
在目标检测模块中，采用先进的基于深度学习的方法PV-RCNN。对于系统的目标跟踪部分，受到FG-3DMOT的启发，采用最大混合模型（max-mixture model）将目标预测状态与检测结果进行隐式关联，并结合预测置信模型实现基于置信引导的数据关联。为展示我们在数据关联方面的改进，我们在框架中将这种隐式数据关联作为一个模块展示出来。需要注意的是，这种隐式数据关联是作为因子图优化的一部分被管理的，并与整体系统联合优化。最终，联合因子图优化后端同时输出自车和移动目标的状态估计结果。
LiDAR 里程计模块
在所提出的Conf SLAMMOT解决方案中，SLAM模块可以采用任何具有代表性的LiDAR SLAM框架。由于系统中包含多个子模块，因此在计算资源有限的条件下，需要考虑每个模块的轻量化特性。此外，IMU并不是必要的传感器。因此，本方案中LiDAR里程计模块采用了LeGO-LOAM，其在保持较高定位和建图精度的同时，相较于LOAM消耗更少的计算资源。
LeGO-LOAM接收来自3D LiDAR的输入，并输出6自由度（6-DoF）的自车位姿估计和3D点云地图。该框架分为五个子模块：分割、特征提取、LiDAR里程计、LiDAR建图和轨迹整合。LeGO-LOAM首先将原始点云投影为距离图像，并在特征提取之前区分地面点和分割点。然后，它匹配相同类别的特征点，并在后端集成iSAM2优化器。最终，通过该模块获得的自车位姿估计将提供给多目标跟踪模块和联合图优化后端使用。
目标检测模块
在Conf SLAMMOT解决方案中，目标检测模块可以采用任何具有代表性的检测方法。本文中使用了PV-RCNN，这是一种能够以较低内存消耗实现精确3D目标检测的先进框架。PV-RCNN结合了3D体素卷积神经网络（Voxel CNN）和稀疏卷积与基于PointNet的网络的优点，从而学习到更具区分性的点云特征。
该方法采用3D体素稀疏卷积神经网络作为主干网络，并利用体素集抽象模块将3D场景转换为关键点，然后通过关键点到网格的区域兴趣（RoI）特征抽象模块进行精炼，用于生成检测提议的置信度预测和位置精炼。该框架的两步策略有效地结合了基于体素和基于点的特征学习，在无需额外传感器输入的情况下，展示了在点云数据的3D目标检测中的高效性和准确性。PV-RCNN创新性的检测方法以及在KITTI基准测试中的表现，使其成为自动驾驶目标检测领域的一种流行解决方案。
目标状态预测
对于目标状态预测，考虑到预测精度和效率，大多数当前的3D目标检测算法通常采用运动模型来预测目标的未来状态。常用的模型包括恒速模型（Constant Velocity, CV）、恒加速模型（Constant Acceleration, CA）和恒转率与速度模型（Constant Turn Rate and Velocity, CTRV）。
- 恒速模型（CV） 假设目标以恒定速度运动，忽略加速度的影响。该模型在目标运动平稳、没有急转弯或加速变化的情况下表现良好。
- 恒加速模型（CA） 假设目标以恒定加速度运动，适用于加速度相对稳定的场景。
- 恒转率与速度模型（CTRV） 假设目标在转弯时保持恒定转率与速度，适用于涉及曲线运动的情况。
此外，一些基于学习的跟踪方法探索通过神经网络从历史轨迹数据中学习目标位置的变换来预测状态。然而，与上述运动模型相比，这些方法通常慢数倍，并在实际应用中面临挑战。因此，本文在综合预测精度和效率的同时，针对漏检问题（如转弯场景中目标更易丢失），采用CTRV模型预测目标的未来状态。
在获得目标状态预测和检测结果后，大多数传统方法直接将二者进行显式或隐式关联，以解决分配问题。然而，目标的预测状态并不总是准确，例如在连续漏检的情况下，预测误差可能会显著累积，从而扩大恢复跟踪时的目标可能检测范围。同时，检测结果的质量也需要被考虑。因此，我们在当前系统中引入了预测置信模型，使数据关联对每个预测的质量更为敏感。
预测置信度的更新公式为：
基于置信引导的隐式数据关联
检测结果和预测结果的管理策略
- 未匹配的检测结果：置信度较低的检测直接丢弃，较高置信度的检测初始化为新目标轨迹。
- 未匹配的预测结果：分为两种情况：
- 目标自然从视野中消失。
- 目标被其他物体暂时遮挡或距离过远导致漏检。
联合因子图优化
所提出的联合因子图优化后端将SLAM和多目标跟踪耦合在一起，从而实现对自车状态和移动目标状态的联合优化估计。该后端在LiDAR SLAM模块的因子图优化基础上进行了扩展，并与基于因子图的跟踪方法集成。
异步状态估计
联合优化目标
优化结果
通过联合因子图优化，系统能够同时估计自车和移动目标的状态，同时显著提升跟踪的稳定性和精度，尤其是在漏检场景中表现出色。联合优化框架的灵活性和鲁棒性，使其能够高效应对动态环境中的复杂挑战。
实验效果
总结一下
Conf SLAMMOT系统将LiDAR SLAM与基于置信引导的隐式数据关联的多目标跟踪紧密耦合。该方法不仅能够在动态环境中同时优化自车与移动目标的状态估计，还能通过置信引导的数据关联有效恢复连续漏检的目标跟踪，从而实现更稳定的后端和更高的估计性能。与其他基准方法相比，各种实验表明该方法在自车位姿估计和目标状态估计方面取得了竞争性的精度和性能。此外，实验结果还表明，Conf SLAMMOT在由于遮挡或目标距离较远导致漏检的场景中表现优越，这在实际应用中具有重要意义。
未来工作方向包括引入语义信息以改进对不同类别目标或环境的感知能力，以及将多种运动模型集成到多目标跟踪中以应对目标的模糊运动状态和不规则运动模式，从而进一步拓展研究。
参考
[1] LiDAR SLAMMOT based on Confidence-guided Data Association
发表回复