全新完整SLAM
LiDAR SLAM与LiDAR里程计的区别在于，LiDAR SLAM的目标是在移动机器人上实时构建一致的环境地图并估计当前姿态，而LO系统专注于实时定位，并在不考虑地图优化的情况下累积地图，这容易导致漂移。受ORB-SLAM3概念的启发，LiDAR SLAM优于LO的地方在于其能够充分利用以下四种数据关联来匹配和优化先前的测量：
短期数据关联：将当前扫描与地图关联，并尽可能高效地估计当前的自运动，以便用于后续的规划和控制过程。大多数LO/LIO系统都采用这种短期数据关联并将当前LiDAR扫描累积到它们的地图中，但不考虑进一步优化状态或地图，导致累积漂移。 中期数据关联：关联最近的多个扫描到地图中并相应地优化地图。这通常通过捆绑调整（BA）技术在一段最近的时间内同时优化状态和局部地图，从而减轻累积漂移并增强系统的鲁棒性。 长期数据关联：关联所有先前的LiDAR扫描以实现全局地图一致性。长期数据关联能够检测先前访问过的地点，并通过位姿图优化（PGO）重置漂移。此外，还可以使用全局捆绑调整（BA）来进一步提高精度。 多地图数据关联：关联多个地图会话。该方法类似于长期数据关联，能够将不同的会话地图合并成一个，这些会话可能是在不同的时间收集数据或由于LiDAR退化导致新会话的初始化而产生的。
Voxel-SLAM系统[1]充分利用了上述四种数据关联，该系统由五个关键模块组成：初始化、里程计、局部建图、回环检测和全局建图。Voxel-SLAM基于BALM2高效的激光雷达捆绑调整、HBA的全局建图方法、BTC的地点识别工作以及VoxelMap的地图表示。除了开源系统本身，Voxel-SLAM的主要贡献如下：
所有任务中使用统一的地图格式：初始化、里程计、局部建图、回环检测和全局建图中都使用了相同的地图格式。Voxel-SLAM采用了高效且通用的自适应体素地图，它为不同任务提供了足够的特征，并且能够很好地适应各种场景。 鲁棒且快速的初始化：Voxel-SLAM只需要短时间的数据（在我们的实现中为1秒）即可完成初始化（在系统发散时进行重新初始化），且能在静态和动态初始状态下进行初始化。该初始化模块为后续模块提供了精确的状态和一致的地图。 高效的局部建图：Voxel-SLAM利用高效的激光雷达-惯性捆绑调整，在最近的LiDAR扫描滑动窗口内优化局部体素地图和状态，从而增强系统的精度和鲁棒性。局部建图（窗口大小为10）能够以10Hz的速度与里程计实时运行，即使在资源有限的机器人车载计算机上也能实现。 多会话的回环检测：Voxel-SLAM具备在当前和先前的会话中检测回环的能力，并能全局优化所有相关的扫描位姿。 高效且精确的单会话或多会话全局建图：Voxel-SLAM采用分层捆绑调整（BA）方法，实现了扫描位姿的高效全局优化和地图一致性。 充分利用四种数据关联：Voxel-SLAM在里程计和局部建图中分别利用了短期和中期数据关联，在回环检测和全局建图中利用了长期和多地图数据关联，从而实现了实时操作和全局地图的一致性。
系统流程
如图1所示，Voxel-SLAM由五个模块组成，分布在三个并行线程中运行。首先，系统基于短时间段（我们目前的实现中为1秒）的LiDAR扫描和IMU数据进行初始化过程（如果在启动或重启后尚未完成）。初始化通过专门的LiDAR-惯性BA优化来估计所有扫描的状态、初始局部地图以及世界坐标系中的重力向量。在获得初始化状态、局部地图和重力向量后，里程计模块紧密融合LiDAR和IMU的测量数据，以实时估计当前状态，并检测因持续的LiDAR退化导致的潜在系统发散问题。
随后，局部建图模块将当前扫描整合到滑动窗口中，并采用LiDAR-惯性BA优化，精确同时优化滑动窗口内的所有状态和局部地图。滑动窗口中的最旧扫描将被边缘化，以累积关键帧，这些关键帧随后由回环检测模块提取回环描述符，并检测当前和之前会话中的回环重现。一旦成功检测到回环，涉及的当前或先前会话中的位姿将用于构建位姿图进行优化。位姿图优化（PGO）完成后，全局建图模块执行关键帧的BA优化，并实时将关键帧合并到子地图中。在收到会话结束信号时，全局建图模块会对所有子地图进行全局BA，并通过自上而下的优化来获得所有扫描的准确位姿
激光惯性BA优化
LiDAR-惯性捆绑调整（BA）优化用于初始化和局部建图模块，以同时估计多个扫描的状态。相比基于滤波器的LIO方法，LiDAR-惯性BA由于利用了较长时间段的中期数据关联，其估计更加准确和鲁棒。然而，BA方法的缺点是计算量较大，导致大多数现有的LiDAR（-惯性）里程计或SLAM系统难以实时运行。
为了应对这个问题，BALM2引入了一种高效的LiDAR捆绑调整方法，主要通过以下三种方式提升效率：（1）解析几何特征（即平面和线）的求解，消除了BA优化中的特征参数，显著降低了优化维度；（2）引入点簇数据结构，将所有点聚合为紧凑的表示，避免在BA中枚举每一个原始点；（3）推导了BA优化的解析二阶导数，从而开发了一种高效的二阶求解器。这三种技术使LiDAR BA优化能够高效地应用于实时系统。
我们采用了BALM2的方法，并将其与IMU预积分相结合，构建了LiDAR-惯性BA优化，以同时估计多个状态以及必要时的重力向量（见图2中的因子图表示）。代价函数为：
代价函数：
状态向量为：
初始化
许多基于滤波器的LIO系统假设传感器最初是静止的。在非静态初始状态下（例如在序列中途重新启动），这种静态假设会传播错误状态，导致LiDAR扫描的畸变校正错误，以及在扫描配准中的特征关联错误。这会显著影响初期状态估计的准确性，并导致不一致（甚至错误）的初始地图，从而进一步降低后续里程计的精度。为了解决这一问题，我们提出了一个鲁棒且高效的初始化阶段，基于之前的LiDAR-惯性BA优化，在存在非静态初始状态的情况下，提供初始状态、重力向量和一致的初始局部地图的鲁棒估计。此外，该模块可以确认初始化是否成功，并重新将重力向量对齐到世界坐标系的z轴。
初始化过程在前N次LiDAR扫描及相应的IMU测量上进行。具体来说，它首先实时运行中的LIO方法，粗略估计前N次扫描的状态和重力向量。估计的状态和重力向量作为后续粗到细体素化和BA优化的初始值。优化后的状态和重力向量用于确定初始化是否成功。如果成功，则初始化完成；否则，系统将收集下一个N次LiDAR扫描，并重复上述初始化过程直到成功。
粗到细体素化和BA优化
在获得一个粗略的初始状态（例如由LIO估计）后，粗到细体素化和BA优化旨在LiDAR-惯性BA优化，细化状态及其对应的地图。不准确的初始状态可能会在注册的点云地图中引入较大的差异，导致体素化过程中出现平面关联的误判。错误的平面关联反过来会导致BA优化的错误。为了解决这一问题，我们提出了一种粗到细的体素化和BA优化方法，在自适应体素化过程中确定平面特征的标准逐步收紧。最初，该标准较为宽松，即使在初始状态误差较大的情况下也能召回平面特征。基于召回的平面特征，进行LiDAR-惯性BA优化。然后，使用优化后的状态来补偿每次扫描中的运动畸变，并构建下一轮具有更严格平面关联标准的自适应体素地图。体素化和BA优化会进行多轮，直到收敛。在每一轮中，平面标准都会比前一轮收紧。当从上一轮到当前轮的代价值减少量低于某个特定值时，认为该粗到细过程已收敛。
初始化成功标准
我们使用三个标准来确定初始化是否成功：（1）粗到细过程应在一定轮次内收敛；（2）优化后的重力向量的大小应接近9.8 m/s²；（3）从优化状态构建的初始体素地图应包含三个线性独立方向上的平面约束。这个退化评估在附录B中有详细描述。如果在最终的BA优化中发生退化，这意味着初始化中涉及的状态没有通过足够的平面特征得到良好的约束，可能会有较大的估计误差。
初始化完成
成功初始化后，我们获得了准确的重力向量和来自前N次扫描的无畸变点云状态。然后，通过旋转矩阵将重力向量对齐到世界参考系的z轴，状态也通过该矩阵进行旋转。此后，重力向量将在后续模块中固定。由优化状态构建的自适应体素地图将作为后续里程计模块的初始局部地图。每次启动（或重新启动）时，初始化仅进行一次，这意味着后续的LiDAR扫描和IMU测量将直接传递给里程计。
里程计
在Voxel-SLAM中，里程计利用扫描与地图配准中的短期数据关联。里程计的目的是以最小的时间延迟估计当前状态，这是其他任务（如规划和控制）所必需的。估计的状态还为后续的局部建图模块提供了一个相对准确的初始值。
状态预测与更新
为了在局部自适应体素地图上进行状态估计，我们使用扫描与地图配准方法，以及扩展卡尔曼滤波器（EKF）来紧密耦合LiDAR和惯性测量。具体来说，在同步LiDAR扫描和IMU测量后，里程计通过IMU传播预测状态，同时补偿LiDAR扫描的运动畸变。未畸变的扫描点以规定的分辨率进行空间下采样，并通过已知的LiDAR-IMU外部参数从LiDAR体坐标系转换到IMU体坐标系。扫描中的每个点通过中的方法计算协方差矩阵，并投影到世界坐标系中，以最大概率匹配体素地图中的平面。点到平面的距离被用作系统测量，通过误差状态迭代卡尔曼滤波器更新IMU预测的状态。估计的状态和LiDAR扫描将发布给其他模块以进行进一步处理。
系统发散检测
当机器人遇到缺乏足够几何特征的退化环境时，状态估计变得病态，可能导致SLAM系统发散和失败，这需要积极监测和处理。在Voxel-SLAM中，在每次扫描配准后检测系统可能的发散。里程计中的状态估计完成后，收集用于扫描与地图配准的平面，并通过附录B中的方法评估退化情况。单次扫描中的偶然退化不被视为发散，因为IMU的短期预测相对准确。连续扫描中的持续退化将被视为发散。在这种情况下，当前的数据会话将被保存，整个系统将从一个远离当前会话的新起点重新启动，以防止下一个会话中的点与当前会话中的点重叠。重新启动还会触发新的初始化，以获得下一个会话的初始状态和局部地图。
局部建图
局部建图利用中期数据关联，优化滑动窗口内的局部体素地图和所有最近的状态。与里程计相比，局部建图更为精确和鲁棒，因为其LiDAR-惯性BA使用了更长时间段的LiDAR和IMU测量进行状态估计。
关键帧加载与体素化
当局部建图模块接收到IMU测量数据和当前的LiDAR扫描（以及从里程计估计的状态）时，扫描中的点将按照其在世界坐标系中的位置分布到相应的体素中。同时，IMU测量数据根据进行预积分以获得IMU预积分因子。由于局部地图仅在距离当前LiDAR位置范围内保持，因此在当前或先前会话中重新访问的地方，体素地图无法确保长期数据关联。为了建立长期关联，我们动态加载在时间上较远（例如 ≥ 1分钟以避免最近的关键帧）但空间上较近（例如 ≤ 10米）的关键帧 。
滑动窗口BA优化
在获得点簇关联和IMU预积分后，滑动窗口BA优化通过第IV节描述的LiDAR-惯性BA优化，精确优化滑动窗口内的所有状态。滑动窗口BA优化与第IV节的不同之处在于，它结合了每个体素中的“固定”点作为地图约束，确保了世界坐标系中估计的精度。
地图边缘化
BA优化完成后，局部体素地图中的点簇和平面参数（平面中心和法向量）会根据优化后的状态进行更新。更新后的平面将在里程计模块的下一个扫描到地图的配准中使用。滑动窗口中的最旧扫描将被边缘化，即从滑动窗口中移除，其点云被合并到“固定”点簇中，作为地图约束。
回环检测
回环检测在我们的框架中起到了两个重要作用。首先，通过利用长期数据关联，它有效地减轻了漂移问题，通过检测先前访问过的地点并通过位姿图优化（PGO）来纠正累积误差。其次，回环检测将当前会话与同一世界框架中的先前会话匹配，实现多地图数据关联。这个过程能够整合和对齐不同测图会话中收集的数据，有助于创建统一的环境地图。
关键帧生成与回环检测
当局部建图中滑动窗口的扫描被边缘化后，回环检测模块会将这些扫描推送到位姿图中。N次连续边缘化的扫描会累积为一帧，如果当前帧与最新关键帧之间的移动距离和旋转角度超过了预定阈值（如0.5米和5度），则该帧将被选为关键帧。关键帧用于提取BTC描述符，并与当前和先前会话中的候选回环关键帧匹配，具体方法可参见。关键帧同时被发送到全局建图线程以进行进一步处理。
在识别出回环候选帧后，必须验证其真实性以避免误检。除了BTC内置的几何验证外，我们还采用了两项额外标准以增强回环检测的可靠性：（1）当前关键帧与检测到的候选帧的点对平面对齐应在三个线性独立的方向上包含平面约束，标准参见附录B；（2）漂移距离与行进距离的比率应足够小（例如，≤1%）。这一标准适用于候选回环关键帧位于当前会话中的情况（见图4(a)）或位于之前会话中的情况（见图4(b)）。
位姿图优化与地图重建
在成功检测到回环后，如果检测到的回环关键帧是第一次出现在先前会话中，则将当前会话与先前会话对齐，所有当前会话中的位姿将被对齐到先前会话的世界坐标系中，并将两个会话的位姿图拼接在一起。否则，如果检测到的回环关键帧在先前会话中是第二次（或更多次）出现，或者位于当前会话中，这两种情况都会导致位姿图中出现回环，从而触发位姿图优化（PGO）。
全局建图
回环检测生成的点云地图，尤其是在多次会话的SLAM过程中，可能会出现不一致性。这是因为位姿图优化（PGO）仅考虑了位姿约束，而未直接捕捉地图一致性的约束。为了解决这个问题，我们引入了全局建图模块，进一步优化回环检测生成的位姿和地图，采用了一种分层全局捆绑调整（BA）方法来减少过多的优化维度。
基于图1中的数据金字塔，全局建图线程包括三个步骤。首先，在接收到回环检测线程中的关键帧后，全局建图线程实时对关键帧窗口执行捆绑调整（BA）。该窗口大小为10，并以步长5滑动。在每个滑动窗口的BA中，固定第一个关键帧的位姿为PGO结果，其他九个关键帧的位姿则在优化过程中同时优化。完成后，将该窗口中的10个关键帧合并为子地图。
其次，一旦当前会话结束（例如数据收集结束或系统重启），系统会对所有子地图进行全局BA。考虑到PGO返回的扫描位姿可能无法确保全局地图的一致性，因此我们采用了第V-A节的粗到细体素化和BA优化方法，用于全局子地图BA，此时不再考虑IMU预积分因子和重力优化。关键帧到子地图的BA优化，再从子地图到全局地图的BA优化，被称为自下而上的BA优化，它显著减少了相对于直接在关键帧或扫描上进行全局BA的优化维度。
最后，采用了带有PGO的自顶向下优化，以提高全局一致性。
实验效果
总结一下
Voxel-SLAM是一个完整、精确且多功能的激光雷达-惯性SLAM系统，充分利用了短期、中期、长期和多地图数据关联，支持单次和多次会话。Voxel-SLAM通过自适应体素地图在所有模块中提供统一的地图表示，使其适用于各种场景。实验结果表明，我们的系统在高动态的初始状态下能够快速启动，并在多个公开数据集上的精度超越了其他最先进的系统。特别是在复杂的多次会话中，Voxel-SLAM能够检测回环并通过高效的全局优化确保全局地图的一致性。
参考
[1] Voxel-SLAM: A Complete, Accurate, and Versatile LiDAR-Inertial SLAM System
自动驾驶之星和生成式AI与具身智能知识星球，新人优惠券来袭，结识一群志同道合的小伙伴一起成长。
自动驾驶之星社区将在10月26号在上海举办大模型线下交流活动。欢迎大家报名参加，我们邀请行业专家和大家一起来分享大模型在具身智能和自动驾驶场景中的应用！
自动驾驶之星社区大模型线下活动报名开始啦！（10月26号 上海）我们回来啦！
下一个风口会不会是生成式AI 与具身智能的时代，我们特意创建了生成式AI与具身智能交流社区，关于大模型，机器人的相关业界动态，学术方向，技术解读等等都会在社区与大家交流，欢迎感兴趣的同学加入我们(备注具身智能)！
自动驾驶之星知识星球主打自动驾驶量产全技术栈学习，并包括: 学习板块，求职面试，有问必答，论文速递，行业动态五大板块！星球内部包括端到端大模型，VLM大模型，BEV 障碍物/车道线/Occ 等的学习资料！
生成式AI与具身智能知识星球，我们相信生成式AI 与具身智能会碰撞出出乎我们意料的内容，本知识形象并包括: 学习板块，求职面试，有问必答，论文速递，行业动态五大板块！星球内部包括生成式AI大模型，具身智能，业界资料整理等的学习资料！
👇点个“赞”和“在看”吧
