SLAM文献之-Multi-input Multi-scale Surfel-Based Lidar-Inertial Continuous-Time Odometry and Mapping

《SLICT: Multi-input Multi-scale Surfel-Based Lidar-Inertial Continuous-Time Odometry and Mapping》是一种针对复杂环境与高频传感器数据设计的连续时间 LiDAR-IMU SLAM 系统。下面是该算法的核心原理、主要创新点和关键推导细节：

---

一、算法核心原理概述

SLICT 是一种连续时间（continuous-time）SLAM 框架，结合了多输入传感器（主要为 LiDAR + IMU）和多尺度 surfel 表示（Surfel：surface element，面元），其设计目标是实现：

• 在高速运动和大视场环境下的鲁棒里程计与建图；
• 高效建图：支持异步、多源数据输入；
• 高精度对齐：使用 surfel-based 表示解决点云间非同步和尺度不一致问题。

---

二、主要创新点

1. 多输入融合（Multi-input Fusion）

• 支持异步接入多个 LiDAR（如多个 Velodyne、Livox）及 IMU；
• 系统通过轨迹节点管理机制（continuous-time trajectory nodes）融合多个传感器数据，消除异步带来的误差。

2. 多尺度 surfel 表示（Multi-scale Surfel Representation）

• 点云数据在建图时以不同尺度的 surfel 表示，支持增量更新与高效的全局地图查询；
• surfel 模型包括局部法线、协方差、置信度等统计量，有利于提高匹配精度；
• 多尺度机制可自适应处理稠密或稀疏区域。

3. 连续时间轨迹建模（Continuous-time Trajectory Optimization）

• 系统采用B-spline轨迹表示器建模位姿随时间变化的连续函数；
• 支持通过时间插值从轨迹中获取精确位姿，用于点云去畸变与配准；
• 在优化过程中，将 surfel 误差项加入优化目标函数。

---

三、系统结构与关键模块

1. 输入数据处理

• 支持任意时间顺序的异步输入（IMU、LiDAR）。
• 点云时间戳通过轨迹插值得到相应位姿，用于去畸变（dewarping）与配准。

2. Surfel 构建与匹配

• 每个 surfel 包含：

  • 中心点位置；
  • 法线方向；
  • 局部协方差；
  • 时间戳与置信度。

• 匹配目标为最小化 surfel-to-surfel 的残差，包含如下形式的 cost：

  $$E_{\text{surf}}=\sum _i{w}_i\cdot \Vert n_i^{\top }(T(t_i)p_i-{\hat{p}}_i){\Vert }^2$$

  其中：

  • $T(t_i)$：时间 $t_i$ 处的位姿；
  • $p_i$：源点；
  • ${\hat{p}}_i,n_i$：目标 surfel 中心点与法线；
  • $w_i$：权重（通常来自匹配置信度或协方差矩阵）。

3. 连续时间轨迹优化

• 使用 cubic B-spline 建模每段轨迹段之间的连续姿态变化；

• 将 surfel 匹配误差 + IMU 预积分误差一同加入优化：

  $$\underset{\text{CtrlPts}}{\min }{E}_{\text{surf}}+E_{\text{imu}}$$

• 优化变量为 B-spline 控制点构成的轨迹表示，求解一般采用 Ceres Solver 或 GTSAM。

---

四、重点部分相关推导

1、SE(3) 空间中的 B-spline 位姿建模推导

SLICT 使用三次 B-spline（cubic B-spline）在李代数空间中插值连续时间轨迹。基本思想是：

1.1 轨迹表示

在时间 $t\in [t_k,t_{k+1}]$ 区间内，轨迹通过 4 个控制点 ${\mathbf{T}}_i,{\mathbf{T}}_{i+1},{\mathbf{T}}_{i+2},{\mathbf{T}}_{i+3}$ 来插值生成：

$$\mathbf{T}(t)=\exp (\Omega (t))\cdot {\mathbf{T}}_{i+1}$$

其中：

• $\Omega (t)\in \mathfrak{se}(3)$ 是李代数空间中以 ${\mathbf{T}}_{i+1}$ 为参考帧的相对运动；
• $\Omega (t)$ 通过 B-spline 基函数和控制点间的相对运动（李代数量）加权得到。

1.2 B-spline 插值构造（在李代数空间）

令：

• ${\xi }_1=\log ({\mathbf{T}}_{i+1}^{-1}\cdot {\mathbf{T}}_{i+2})$
• ${\xi }_2=\log ({\mathbf{T}}_{i+2}^{-1}\cdot {\mathbf{T}}_{i+3})$
• ${\xi }_0=\log ({\mathbf{T}}_i^{-1}\cdot {\mathbf{T}}_{i+1})$

定义插值系数（B-spline 基函数） ${\beta }_j(u)\in [0,1]$，其中 $u=\frac{t-t_i}{t_{i+3}-t_i}$，是归一化时间。常见三次 B-spline basis 为：

$$\begin{array}{rl}{\beta }_0(u)& =\frac{(1-u{)}^3}{6}\\ {\beta }_1(u)& =\frac{3u^3-6u^2+4}{6}\\ {\beta }_2(u)& =\frac{-3u^3+3u^2+3u+1}{6}\\ {\beta }_3(u)& =\frac{u^3}{6}\end{array}$$

最终轨迹可以表示为：

$$\mathbf{T}(t)=\exp ({\beta }_3(u){\xi }_2)\cdot \exp ({\beta }_2(u){\xi }_1)\cdot \exp ({\beta }_1(u){\xi }_0)\cdot {\mathbf{T}}_i$$

这样就构造出了 连续的 SE(3) 轨迹函数，可以对任意时间点的 LiDAR 点进行插值定位。

---

2、Surfel 残差项 Jacobian 推导

考虑目标是最小化 surfel 残差（在局部帧中）：

2.1 残差项形式

对于一个点 ${\mathbf{p}}_i\in {\mathbb{R}}^3$，在时间 $t_i$ 对应的轨迹位置为：

$${\mathbf{p}}_i^{\text{world}}=\mathbf{T}(t_i)\cdot {\mathbf{p}}_i$$

将其投影到 surfel 平面（由 ${\mathbf{n}}_j,{\mathbf{q}}_j$ 表示）上，构造点到面的误差：

$$r_i={\mathbf{n}}_j^{\top }\cdot \left(\mathbf{T}(t_i){\mathbf{p}}_i-{\mathbf{q}}_j\right)$$

目标是最小化：

$$E=\sum _i{w}_i\cdot r_i^2$$

2.2 残差对 B-spline 控制点的导数（雅可比）

为进行优化，需要对每个控制点 ${\mathbf{T}}_k$ 的李代数扰动求导。关键是：

• $\mathbf{T}(t_i)$ 是由多个控制点的组合形成的；
• 使用链式法则求导，将误差对位姿的导数，乘以上游导数。

先定义：

$$\frac{\partial r_i}{\partial \mathbf{T}(t_i)}={\mathbf{n}}_j^{\top }\cdot \left[\frac{\partial \mathbf{T}(t_i)\cdot {\mathbf{p}}_i}{\partial \mathbf{T}(t_i)}\right]$$

注意：

• 在 SE(3) 空间内，扰动使用李代数：

  $$\mathbf{T}\cdot \exp (\delta \xi )\approx \mathbf{T}\cdot (I+[\delta \xi {]}^{\wedge })$$

于是导数可展开为：

$$\frac{\partial r_i}{\partial \delta \xi }={\mathbf{n}}_j^{\top }\cdot \left[\mathbf{T}(t_i)\cdot [{\mathbf{p}}_i{]}_{\times }\right]$$

接下来，对每个控制点 ${\mathbf{T}}_k$ 的导数（链式求导）：

$$\frac{\partial r_i}{\partial \delta {\xi }_k}=\frac{\partial r_i}{\partial \mathbf{T}(t_i)}\cdot \frac{\partial \mathbf{T}(t_i)}{\partial \delta {\xi }_k}$$

其中，后者是 B-spline 插值函数对控制点的雅可比，通常通过自动微分或数值导数实现。

---

3. IMU预积分误差

• 采用 Forster 预积分模型，引入 bias 校正项；
• 残差项基于速度、位姿、bias 三者间的协同关系。

---

五、总结

模块	贡献
多输入融合	灵活应对多个异步 LiDAR/IMU 数据输入
多尺度 surfel	增强稀疏区域匹配精度，提升地图分辨率适应性
连续时间轨迹优化	消除非同步畸变，提升帧间注册精度
高效优化模型	将 surfel 匹配误差与 IMU预积分融合于一体