光鉴科技

IROS 2024 | 光鉴科技3D SLAM研究成果入选机器人顶会

2024年10月14—18日，智能机器人与系统国际会议（IROS 2024）在阿联酋阿布扎比举行。作为全球规模最大、最具影响力的机器人研究会议之一，IROS每年吸引来自世界各地的科研人员、学者以及行业专家，共同探讨机器人与智能系统领域的最新进展。

本次会议中，光鉴科技的研究成果 C3P-VoxelMap: Compact, Cumulative and Coalescible Probabilistic Voxel Mapping 凭借其创新性成功入选，并作为口头报告（Oral Pitch）展示。该研究提出了在嵌入式平台有限算力下实现 real-time 3D SLAM 的解决方案，为移动机器人自主导航提供了参考设计。

研究背景

3D SLAM 作为机器人自主导航的核心技术之一，如何在有限的计算和存储资源下实现高精度定位与建图，具有重要的理论和现实意义。

VoxelMap是3D SLAM中广泛使用的地图结构，通过将三维空间划分成相同结构的体素，并利用哈希表进行索引，具有维护简单，访存高效的优势。

然而，常规的VoxelMap在实际中存在局限，难以捕捉真实世界中的大尺度几何特征，例如墙壁、地板和天花板，使得同一个物理平面需要大量体素重复表示。另一方面，固定大小的体素面临 bias-variance trade-off 难题。以下图为例，小体素能更好地捕捉地图细节（bias小），但更容易受噪声影响，尤其是在点云稀疏的情况下（variance大）; 相反，大体素可以收集更多观测点来降低方差（variance小），但可能会过度平滑环境，导致表示偏差（bias大）。

除此之外，Probabilistic VoxelMap 需要存储每个体素的历史点云数据，并在新的观测点加入时重新遍历所有点，以更新体素平面的不确定性。这是由于计算平面的协方差矩阵时，需要计算法向量对逐个点求导的雅可比矩阵。其中，点p以及点p的协方差矩阵是和观测有关的项（以下简称观测项），q, F, 和U是和平面点集总体分布有关的项（以下简称分布项。q代表点集均值，F和 U是由 SVD 结果构成的矩阵，详细过程请参考论文）。当新的观测加入时，由于雅可比矩阵所包含的分布项会发生变化，需要将每个点再次代入公式，重新进行求导。

这种遍历消耗了大量内存和CPU资源。在一些资源受限的机器人平台上可能导致定位延迟增加，或者没有足够的内存来维护地图，进而影响下游的避障和路径规划等任务。

为了解决以上痛点，光鉴科技创新提出了C3P-VoxelMap，从计算效率、内存效率、算法精度三方面对 VoxelMap 进行了提升。

方法介绍

平面协方差的增量更新

为了避免重复求导，我们希望将观测项和分布项解耦，使得二者能够分别进行增量更新。

利用矩阵计算的分配律将 U 矩阵分离出来比较容易，难点是如何从B 矩阵分离出q和F。考虑到B矩阵整体不容易分解，我们根据其表达式，从单个元素入手进行考察。

由此可见，B 矩阵第m行n列的元素能够分解成四个形式相似的求和项，以其中一项为例

我们利用以下两条引理分离观测项

第一条性质帮助我们提取矩阵元素，第二条性质用来完成矩阵位置交换。

利用这两条性质，我们成功得到了只有观测项的累加形式X。其他各项的推导类似，请参考我们的论文和补充材料。当新的观测点加入时，我们只需将点和协方差映射成上面的累加形式即可完成增量更新。这种紧凑的表示方法避免了针对每个点重新求导，大幅减少了计算量和内存占用。

体素平面合并

为了进一步解决bias-variance trade-off难题，我们引入了一种体素合并策略，使其能够动态适应真实世界中的大平面特征。这一步的难点是如何保证合并效率，如果采用暴力搜索的方式，将会影响到算法的实时性。

为此，我们设计了一种基于局部敏感哈希（Locality Sensitive Hash）的合并方法。具有相似平面参数，同时物理距离较近的体素更有机会计算出相同的哈希值，进而被推入同一个 bucket 等待合并。当 bucket 元素达到一定数量时，合并操作被触发，点数最多的体素被选为参考体素，用来作为其它体素合并的比较基准。这样做的原因是，点数多的体素通常平面估计更准确。如果其它体素尝试合入后，点集分布的奇异值仍然满足平面条件，该体素将被允许合入，反之将被拒绝合入。

得益于平面协方差矩阵的紧凑形式，我们能够增量更新合并后的平面。单个体素和跨体素的不确定度更新具有统一的增量形式。因此，合并后的平面仍然具有完备的不确定度表达。通过合并，一个大平面特征只需一个或少量体素即可表示，进一步减少了内存占用。此外，在噪声水平相同的情况下，通过体素合并能够更加准确地估计平面，进而提高SLAM 精度(如下方示意图，理论证明请参考论文V-C小节)。