CABINET / Machines in Motion / 具身智能研究
状态估计与SLAM
具身智能系统在物理世界中运行，必须持续回答两个基本问题：我在哪里（localization），周围环境的几何结构是什么（mapping）。状态估计与SLAM（Simultaneous Localization and Mapping）层提供对这两个问题的联合求解。本文从概率理论基础出发，逐层展开视觉、惯性、激光雷达等传感器模态下的主流算法架构，覆盖前端里程计、后端图优化、地图表示、多传感器融合，并附主要工业公司格局。
1. 概率状态估计基础
1.1 贝叶斯滤波框架（Bayesian Filtering）
状态估计的核心任务：给定传感器观测序列 $z_{1:t}$ 和控制输入序列 $u_{1:t}$，推断系统当前状态 $x_t$ 的后验概率分布。
贝叶斯滤波的递推公式由两步组成：
预测步骤（Prediction）：
$$ p(x_t | z_{1:t-1}, u_{1:t}) = \int p(x_t | x_{t-1}, u_t) \cdot p(x_{t-1} | z_{1:t-1}, u_{1:t-1}) , dx_{t-1} $$
其中 $p(x_t | x_{t-1}, u_t)$ 为运动模型（motion model），描述状态转移的概率。
更新步骤（Update）：
$$ p(x_t | z_{1:t}, u_{1:t}) = \frac{p(z_t | x_t) \cdot p(x_t | z_{1:t-1}, u_{1:t})}{p(z_t | z_{1:t-1}, u_{1:t})} $$
其中 $p(z_t | x_t)$ 为观测模型（observation model），分母为归一化常数。
这一框架是所有概率状态估计方法的理论基础，不同算法的区别在于如何参数化分布、如何近似积分。
1.2 卡尔曼滤波（Kalman Filter）
当系统满足以下条件时，贝叶斯滤波存在解析解：
- 线性运动模型：$x_t = F_t x_{t-1} + B_t u_t + w_t$，其中 $w_t \sim \mathcal{N}(0, Q_t)$
- 线性观测模型：$z_t = H_t x_t + v_t$，其中 $v_t \sim \mathcal{N}(0, R_t)$
- 初始状态服从高斯分布：$x_0 \sim \mathcal{N}(\hat{x}_0, P_0)$
在这些条件下，后验分布始终保持高斯形式 $p(x_t | z_{1:t}) = \mathcal{N}(\hat{x}_t, P_t)$。
完整推导：
预测步骤：
$$ \hat{x}t^{-} = F_t \hat{x}{t-1} + B_t u_t $$
$$ P_t^{-} = F_t P_{t-1} F_t^T + Q_t $$
更新步骤：
新息（Innovation）： $$ y_t = z_t - H_t \hat{x}_t^{-} $$
新息协方差： $$ S_t = H_t P_t^{-} H_t^T + R_t $$
卡尔曼增益（Kalman Gain）： $$ K_t = P_t^{-} H_t^T S_t^{-1} $$
状态更新： $$ \hat{x}_t = \hat{x}_t^{-} + K_t y_t $$
协方差更新： $$ P_t = (I - K_t H_t) P_t^{-} $$
卡尔曼增益的物理含义：$K_t$ 决定了预测值与观测值之间的权重分配。当观测噪声 $R_t$ 趋近于零时，$K_t \to H_t^{-1}$，系统完全信任观测；当预测协方差 $P_t^{-}$ 趋近于零时，$K_t \to 0$，系统完全信任预测。
数值示例：一维匀速运动目标跟踪
状态: x = [position, velocity]^T
F = [[1, dt], [0, 1]], dt = 0.1s
H = 1, 0 (只观测位置)
Q = [[0.01, 0], [0, 0.01]]
R = 0 (观测噪声方差)
初始: x_hat = [0, 1]^T, P = [[1, 0], [0, 1]]
Step 1 预测:
x_hat^- = [[1, 0.1], [0, 1]] * [0, 1]^T = [0.1, 1]^T
P^- = F*P*F^T + Q = [[1.02, 0.11], [0.11, 1.02]]
Step 1 更新 (假设 z_1 = 0.5):
y = 0.5 - [1,0]*[0.1, 1]^T = 0.4
S = [1,0]*P^-*[1,0]^T + 1.0 = 1.02 + 1.0 = 2.02
K = P^-*[1,0]^T / S = [1.02/2.02, 0.11/2.02]^T = [0.505, 0.054]^T
x_hat = [0.1, 1]^T + [0.505, 0.054]^T * 0.4 = [0.302, 1.022]^T
P = (I - K*H)*P^- = [[0.505, 0.054], [0.054, 0.965]]1.3 扩展卡尔曼滤波（Extended Kalman Filter, EKF）
实际系统几乎都是非线性的。EKF 通过一阶泰勒展开将非线性系统局部线性化。
非线性系统模型： $$ x_t = f(x_{t-1}, u_t) + w_t $$ $$ z_t = h(x_t) + v_t $$
线性化过程：在当前估计值处计算雅可比矩阵（Jacobian）：
$$ F_t = \frac{\partial f}{\partial x}\bigg|{\hat{x}{t-1}, u_t} $$
$$ H_t = \frac{\partial h}{\partial x}\bigg|_{\hat{x}_t^{-}} $$
预测步骤变为： $$ \hat{x}t^{-} = f(\hat{x}{t-1}, u_t) $$ $$ P_t^{-} = F_t P_{t-1} F_t^T + Q_t $$
更新步骤中的 $H_t$ 使用线性化后的雅可比矩阵，其余公式与标准 KF 相同。
EKF 的局限性：
- 一阶线性化在强非线性区域产生较大误差
- 雅可比矩阵的解析计算对复杂系统可能困难
- 无法表示多模态分布
1.4 无迹卡尔曼滤波（Unscented Kalman Filter, UKF）
UKF 的核心思想：用确定性采样点（sigma points）来近似概率分布通过非线性变换后的统计特性，精度可达三阶（对高斯输入）。
对于 $n$ 维状态，生成 $2n+1$ 个 sigma 点：
$$ \chi_0 = \hat{x} $$ $$ \chi_i = \hat{x} + \left(\sqrt{(n+\lambda)P}\right)i, \quad i = 1, \ldots, n $$ $$ \chi{i+n} = \hat{x} - \left(\sqrt{(n+\lambda)P}\right)_i, \quad i = 1, \ldots, n $$
其中 $\lambda = \alpha^2(n + \kappa) - n$ 为缩放参数，$\left(\sqrt{(n+\lambda)P}\right)_i$ 表示矩阵平方根的第 $i$ 列。
权重： $$ W_0^{(m)} = \frac{\lambda}{n+\lambda}, \quad W_0^{(c)} = \frac{\lambda}{n+\lambda} + (1 - \alpha^2 + \beta) $$ $$ W_i^{(m)} = W_i^{(c)} = \frac{1}{2(n+\lambda)}, \quad i = 1, \ldots, 2n $$
典型参数选择：$\alpha = 10^{-3}$，$\kappa = 0$，$\beta = 2$（对高斯分布最优）。
UKF 预测步骤：
- 将 sigma 点通过非线性运动模型传播：$\chi_i^{-} = f(\chi_i, u_t)$
- 计算预测均值：$\hat{x}t^{-} = \sum{i=0}^{2n} W_i^{(m)} \chi_i^{-}$
- 计算预测协方差：$P_t^{-} = \sum_{i=0}^{2n} W_i^{(c)} (\chi_i^{-} - \hat{x}_t^{-})(\chi_i^{-} - \hat{x}_t^{-})^T + Q_t$
1.5 粒子滤波（Particle Filter）
粒子滤波使用一组加权样本（粒子）来表示任意形状的概率分布，不受高斯假设和线性化限制。
算法流程：
输入: 粒子集 {x_{t-1}^{(i)}, w_{t-1}^{(i)}}_{i=1}^N, 控制 u_t, 观测 z_t
1. 对每个粒子 i = 1, ..., N:
a. 从提议分布采样: x_t^{(i)} ~ p(x_t | x_{t-1}^{(i)}, u_t)
b. 计算权重: w_t^{(i)} = p(z_t | x_t^{(i)})
2. 归一化权重: w_t^{(i)} = w_t^{(i)} / sum(w_t^{(j)})
3. 重采样（Resampling）:
根据权重 {w_t^{(i)}} 重新采样 N 个粒子
常用方法: 系统重采样（Systematic Resampling）
4. 输出: 加权粒子集 {x_t^{(i)}, 1/N}_{i=1}^N蒙特卡洛定位（Monte Carlo Localization, MCL）：
MCL 是粒子滤波在已知地图定位问题中的直接应用。粒子表示机器人可能的位姿，观测模型通常使用激光雷达的 beam model 或 likelihood field model。
粒子退化问题：经过多次迭代后，大部分权重集中在少数粒子上。有效粒子数（Effective Sample Size）用于监控退化：
$$ N_{eff} = \frac{1}{\sum_{i=1}^N (w_t^{(i)})^2} $$
当 $N_{eff}$ 低于阈值（通常 $N/2$）时触发重采样。
1.6 各滤波方法对比
| 方法 | 分布假设 | 计算复杂度 | 适用非线性程度 | 典型应用 |
|---|---|---|---|---|
| KF | 线性高斯 | $O(n^3)$ | 仅线性系统 | 基线对比 |
| EKF | 近似高斯 | $O(n^3)$ | 弱非线性 | IMU积分, VIO |
| UKF | 近似高斯 | $O(n^3)$ | 中等非线性 | 姿态估计 |
| PF | 任意分布 | $O(N \cdot n)$ | 强非线性/多模态 | 全局定位, MCL |
2. 视觉里程计（Visual Odometry, VO）
视觉里程计通过连续图像帧之间的几何关系估计相机的增量运动。根据信息利用方式分为特征法（feature-based）和直接法（direct method）。
2.1 基于特征的方法（Feature-based VO）
2.1.1 ORB 特征
ORB（Oriented FAST and Rotated BRIEF）是当前实时SLAM中使用最广泛的特征描述子。
检测阶段：使用 FAST（Features from Accelerated Segment Test）角点检测器。对像素 $p$，检查其周围 Bresenham 圆（半径3，16个像素）上是否存在连续 $N$（通常 $N=9$）个像素亮度全部大于 $I_p + t$ 或全部小于 $I_p - t$。
方向计算：使用灰度质心法（Intensity Centroid）。在特征点邻域内计算图像矩：
$$ m_{pq} = \sum_{x,y} x^p y^q I(x, y) $$
质心为 $C = (m_{10}/m_{00}, m_{01}/m_{00})$，方向角 $\theta = \text{atan2}(m_{01}, m_{10})$。
描述子：rBRIEF（旋转 BRIEF），对 256 对点进行二值比较，根据特征点方向旋转采样模式，输出 256-bit 二进制向量。匹配时使用 Hamming 距离，可通过 CPU 的 POPCNT 指令高效计算。
2.1.2 特征匹配
匹配策略：
- 暴力匹配（Brute Force）：计算描述子间所有两两距离
- 快速近似最近邻（FLANN）：使用 KD-Tree 或随机化树
- 比率测试（Lowe’s Ratio Test）：最近邻距离与次近邻距离之比小于阈值（通常 0.7）
2.1.3 对极几何（Epipolar Geometry）
两个相机观测同一3D点 $P$，记其在两帧图像中的归一化坐标为 $p_1$ 和 $p_2$，相机间的相对运动为旋转 $R$ 和平移 $t$。
本质矩阵（Essential Matrix）：
$$ p_2^T E , p_1 = 0 $$
其中 $E = [t]\times R$，$[t]\times$ 为平移向量的反对称矩阵。
$E$ 有5个自由度（3旋转 + 2平移方向，尺度不可观）。
基础矩阵（Fundamental Matrix）：
当使用像素坐标 $\tilde{p}_1, \tilde{p}_2$ 时：
$$ \tilde{p}_2^T F , \tilde{p}_1 = 0 $$
其中 $F = K_2^{-T} E K_1^{-1}$，$K_1, K_2$ 为相机内参矩阵。$F$ 有7个自由度。
八点法求解 $F$：将对极约束展开为线性方程组 $Af = 0$，其中 $f$ 为 $F$ 的9个元素。至少需要8对匹配点，使用 SVD 分解求解最小二乘问题，并施加 $\text{rank}(F)=2$ 约束。
2.1.4 PnP（Perspective-n-Point）
当已知3D地图点时，PnP 直接求解相机在世界坐标系中的位姿。
给定 $n$ 个3D-2D对应 $(P_i, p_i)$，求解 $[R|t]$ 使得：
$$ s_i \begin{bmatrix} u_i \ v_i \ 1 \end{bmatrix} = K [R|t] \begin{bmatrix} X_i \ Y_i \ Z_i \ 1 \end{bmatrix} $$
常用方法：
- P3P：使用3个点的几何约束，最多产生4个解，需第4个点消歧
- EPnP（Efficient PnP）：$O(n)$ 复杂度，将3D点表示为4个虚拟控制点的加权和
- DLS（Direct Least Squares）：直接最小化重投影误差
2.1.5 RANSAC（Random Sample Consensus）
特征匹配中不可避免存在误匹配（outliers）。RANSAC 通过随机采样和一致性验证进行鲁棒估计。
算法: RANSAC
输入: 匹配点对集合, 模型(如本质矩阵), 内点阈值 epsilon
参数: 最大迭代次数 k
1. 重复 k 次:
a. 随机选择最小点集 (E矩阵需5点, F矩阵需8点, H矩阵需4点)
b. 拟合模型
c. 计算所有点到模型的误差
d. 统计内点数 (误差 < epsilon 的点)
e. 如果内点数 > 当前最优, 更新最优模型
2. 用所有内点重新拟合模型 (最小二乘)
迭代次数估计:
k = log(1-p) / log(1-w^n)
其中: p = 成功概率(通常0.99)
w = 内点比例
n = 最小样本量
例: w=0.5, n=5, p=0.99 => k = 1452.2 直接法（Direct Methods）
直接法不提取特征，而是直接最小化像素灰度误差（photometric error）。
2.2.1 光度误差模型
对于参考帧中的像素 $\mathbf{u}$，其深度为 $d$，当前帧相对参考帧的位姿为 $T \in SE(3)$。光度误差定义为：
$$ e(\mathbf{u}) = I_2(\pi(T \cdot \pi^{-1}(\mathbf{u}, d))) - I_1(\mathbf{u}) $$
其中 $\pi$ 为相机投影函数，$\pi^{-1}$ 为反投影。
总体优化目标：
$$ T^* = \arg\min_T \sum_{\mathbf{u} \in \Omega} \rho\left( \frac{e(\mathbf{u})^2}{\sigma^2} \right) $$
其中 $\rho$ 为鲁棒核函数（如 Huber 核），$\Omega$ 为选取的像素集合。
2.2.2 LSD-SLAM（Large-Scale Direct Monocular SLAM）
LSD-SLAM 是半稠密直接法的代表性工作。其特点：
- 仅在图像梯度显著的区域（边缘）估计深度，形成半稠密深度图
- 深度值用逆深度（inverse depth）参数化，假设服从高斯分布
- 通过多帧三角化逐步收敛深度估计
- 位姿估计使用 $\mathfrak{se}(3)$ 上的加权光度误差最小化
2.2.3 DSO（Direct Sparse Odometry）
DSO 在直接法中引入了严格的光度标定模型：
$$ I’(\mathbf{u}) = \frac{t_j \cdot e^{a_j}}{t_i \cdot e^{a_i}} \cdot \left( I(\mathbf{u}) - b_i \right) + b_j $$
其中 $t$ 为曝光时间，$a, b$ 为仿射亮度参数。DSO 联合优化几何参数（位姿、逆深度）与光度参数。
DSO 使用稀疏点集（约2000个高梯度点），在滑动窗口内进行联合优化，计算效率优于稠密方法。
2.3 ORB-SLAM3 系统架构
ORB-SLAM3 是当前最完整的视觉SLAM系统，支持单目、双目、RGB-D相机以及视觉惯性模式。
+---------------------------------------------------------------+
| ORB-SLAM3 架构 |
+---------------------------------------------------------------+
| |
| +-------------------+ +-------------------+ +-----------+ |
| | Tracking Thread | | Local Mapping | | Loop | |
| | | | Thread | | Closing | |
| | - ORB提取 | | | | Thread | |
| | - 初始位姿估计 | | - 关键帧插入 | | | |
| | (恒速模型/ | | - 局部BA | | - DBoW2 | |
| | 参考关键帧/ | | - 地图点剔除 | | 候选检测| |
| | 重定位) | | - 新地图点创建 | | - Sim(3) | |
| | - 局部地图跟踪 | | - 局部地图维护 | | 验证 | |
| | - 关键帧判定 | | | | - 位姿图 | |
| | | | | | 优化 | |
| +--------+----------+ +--------+----------+ +-----+-----+ |
| | | | |
| v v v |
| +-----------------------------------------------------------+|
| | Map Atlas ||
| | - 多地图管理 (Active Map + Non-active Maps) ||
| | - 地图合并 (Map Merging) ||
| | - DBoW2 数据库 ||
| +-----------------------------------------------------------+|
+---------------------------------------------------------------+三个并行线程的功能：
Tracking 线程（实时运行，帧率等于相机帧率）：
- 对每帧提取 ORB 特征（金字塔8层，每层1000个特征）
- 通过三种策略之一估计初始位姿：恒速运动模型、与参考关键帧匹配、重定位
- 将当前帧投影到局部地图中，搜索更多匹配，优化位姿
- 根据规则判定是否插入新关键帧
Local Mapping 线程：
- 处理新关键帧，计算 BoW 向量
- 通过三角化创建新地图点
- 剔除质量差的地图点（观测次数少、跟踪率低）
- 执行局部 Bundle Adjustment（优化当前关键帧及其共视关键帧的位姿和地图点）
- 剔除冗余关键帧
Loop Closing 线程：
- 使用 DBoW2 检测回环候选
- 计算 Sim(3) 变换（单目模式下包含尺度校正）
- 执行位姿图优化（Pose Graph Optimization）传播校正
- 可选的全局 Bundle Adjustment
Map Atlas：ORB-SLAM3 引入多地图管理机制。当跟踪丢失时，系统创建新地图继续运行；当检测到当前地图与已有地图存在重叠时，执行地图合并（Map Merging），将两张地图的关键帧和地图点统一到同一坐标系下。
3. 视觉惯性里程计（Visual-Inertial Odometry, VIO）
3.1 IMU 预积分理论（IMU Preintegration）
IMU 输出加速度计读数 $\tilde{a}_t$ 和陀螺仪读数 $\tilde{\omega}_t$：
$$ \tilde{a}t = R_t^T(a_t - g) + b_a + n_a $$ $$ \tilde{\omega}t = \omega_t + b\omega + n\omega $$
其中 $g$ 为重力向量，$b_a, b_\omega$ 为缓变偏置（bias），$n_a, n_\omega$ 为白噪声。
预积分的动机：IMU 频率通常为 200-1000Hz，如果每次偏置估计更新后都从头重新积分，计算开销极大。预积分将两关键帧之间的 IMU 测量压缩为与起始状态无关的增量。
定义两关键帧 $i, j$ 之间的预积分量：
$$ \Delta R_{ij} = \prod_{k=i}^{j-1} \text{Exp}((\tilde{\omega}k - b\omega) \Delta t) $$
$$ \Delta v_{ij} = \sum_{k=i}^{j-1} \Delta R_{ik} (\tilde{a}_k - b_a) \Delta t $$
$$ \Delta p_{ij} = \sum_{k=i}^{j-1} \left[ \Delta v_{ik} \Delta t + \frac{1}{2} \Delta R_{ik} (\tilde{a}_k - b_a) \Delta t^2 \right] $$
这些预积分量仅依赖 IMU 测量和当前偏置估计。当偏置更新量 $\delta b$ 较小时，可通过一阶近似修正：
$$ \Delta R_{ij}(b_\omega + \delta b_\omega) \approx \Delta R_{ij}(b_\omega) \cdot \text{Exp}\left(\frac{\partial \Delta R_{ij}}{\partial b_\omega} \delta b_\omega\right) $$
3.2 紧耦合与松耦合（Tight Coupling vs Loose Coupling）
| 特性 | 松耦合 | 紧耦合 |
|---|---|---|
| 融合层级 | 结果层 | 测量层 |
| 架构 | VO输出位姿 + IMU位姿，通过EKF/图优化融合 | 视觉特征观测与IMU预积分在同一优化问题中联合求解 |
| 信息利用率 | 低（丢失了视觉不确定性的方向信息） | 高（完整利用所有约束） |
| 精度 | 较低 | 较高 |
| 实现复杂度 | 低 | 高 |
| 代表系统 | 早期 MSCKF 变体 | VINS-Mono, ORB-SLAM3 VI模式 |
3.3 VINS-Mono 架构
VINS-Mono（Visual-Inertial Navigation System）是紧耦合滑动窗口优化VIO的代表系统。
系统流程：
IMU数据 ──────────────────────────────────┐
v
图像 ─> 特征提取 ─> KLT光流跟踪 ─> 预处理 ─> 滑动窗口优化 ─> 位姿输出
^
│
初始化(SfM+IMU对齐)
回环检测(DBoW2)
全局位姿图优化状态向量（滑动窗口内包含 $n+1$ 个关键帧）：
$$ \mathcal{X} = [x_0, x_1, \ldots, x_n, \lambda_0, \lambda_1, \ldots, \lambda_m] $$
其中每帧状态 $x_k = [p_k, v_k, R_k, b_{a,k}, b_{\omega,k}]$，$\lambda_l$ 为特征点逆深度。
优化目标函数：
$$ \min_{\mathcal{X}} \left{ |r_p|^2 + \sum_{k \in \mathcal{B}} |r_{\mathcal{B}}(\hat{z}{b_k b{k+1}}, \mathcal{X})|{P{b_k b_{k+1}}}^2 + \sum_{(l,j) \in \mathcal{C}} |r_{\mathcal{C}}(\hat{z}l^{c_j}, \mathcal{X})|{P_l^{c_j}}^2 \right} $$
三项分别为：边缘化先验（marginalization prior）、IMU预积分残差、视觉重投影残差。
3.4 MSCKF（Multi-State Constraint Kalman Filter）
MSCKF 是基于滤波的VIO方法，通过维护滑动窗口内多帧相机状态来构建约束。
核心思想：当一个特征点被多帧观测到并即将离开视野时，不将其加入状态向量（避免状态维度增长），而是利用该点在多帧中的观测构建对相机状态的约束（nullspace projection消除特征点坐标）。
状态向量仅包含 IMU 状态和滑动窗口内的相机位姿：
$$ x = [x_{IMU}, x_{C_1}, x_{C_2}, \ldots, x_{C_N}] $$
计算优势：状态维度固定（与特征点数无关），更新步骤为 $O(N^2)$（$N$ 为窗口大小），适合计算资源受限的平台。
3.5 OpenVINS
OpenVINS 是 MSCKF 的现代开源实现，提供模块化架构：
- 支持多种特征跟踪器（KLT, 描述子匹配）
- 支持多相机配置
- 在线标定（相机-IMU外参、时间偏移、内参）
- FEJ（First Estimates Jacobian）保证一致性
- 提供仿真器用于算法验证
3.6 时间同步（Time Synchronization）
相机和 IMU 的时间戳之间通常存在固定偏移 $t_d$：
$$ t_{cam} = t_{imu} + t_d $$
VINS-Mono 将 $t_d$ 作为状态变量联合估计。对于特征观测，时间偏移的影响通过一阶近似建模：
$$ \hat{z}(t + t_d) \approx \hat{z}(t) + \frac{\partial \hat{z}}{\partial t} t_d $$
其中 $\frac{\partial \hat{z}}{\partial t}$ 可通过特征点的像素速度近似。
3.7 退化运动（Degenerate Motions）
VIO 系统在特定运动模式下会出现可观测性退化：
| 退化运动 | 不可观测状态 | 原因 |
|---|---|---|
| 匀速直线运动 | 加速度计偏置、重力方向 | 无法区分偏置与重力投影 |
| 纯旋转 | 视觉尺度、平移 | 三角化失败 |
| 静止 | 陀螺仪偏置（部分） | 缺乏激励 |
| 单轴旋转 | 该轴方向陀螺仪偏置 | 偏置与角速度耦合 |
工程实践中，通过检测运动激励不足并降低相关状态的更新权重来缓解退化问题。
4. LiDAR SLAM
4.1 LOAM 框架（Lidar Odometry and Mapping）
LOAM 是激光雷达 SLAM 的奠基性工作，其双频率架构设计被后续大量系统继承。
特征提取：根据局部曲率将点分类：
- 边缘点（Edge Points）：曲率最大的点，对应环境中的边缘
- 平面点（Planar Points）：曲率最小的点，对应环境中的平面
曲率计算： $$ c_i = \frac{1}{|S_i|} \left| \sum_{j \in S_i} (p_j - p_i) \right| $$
其中 $S_i$ 为点 $p_i$ 的邻域点集。
双频率架构：
10Hz LiDAR扫描
│
v
┌──────────────────┐ ┌───────────────────────┐
│ Lidar Odometry │ │ Lidar Mapping │
│ (高频, 10Hz) │────>│ (低频, 1Hz) │
│ │ │ │
│ 扫描内运动补偿 │ │ 点到地图的精配准 │
│ 帧间特征匹配 │ │ 全局地图维护 │
│ 粗略位姿估计 │ │ 精确位姿估计 │
└──────────────────┘ └───────────────────────┘点到边缘距离（Edge point residual）：
点 $p$ 到由 $a, b$ 两点确定的直线的距离： $$ d_e = \frac{|(p-a) \times (p-b)|}{|a-b|} $$
点到平面距离（Planar point residual）：
点 $p$ 到由 $a, b, c$ 三点确定的平面的距离： $$ d_p = \frac{|(p-a) \cdot ((b-a) \times (c-a))|}{|(b-a) \times (c-a)|} $$
4.2 LIO-SAM（LiDAR Inertial Odometry via Smoothing and Mapping）
LIO-SAM 将 LOAM 的思想融入因子图（factor graph）框架，实现激光雷达-惯性紧耦合。
因子图结构：
因子类型:
[I] IMU预积分因子
[L] LiDAR里程计因子
[G] GPS因子 (可选)
[C] 回环因子
x0 ─[I]─ x1 ─[I]─ x2 ─[I]─ x3 ─[I]─ x4
| | | | |
[L] [L] [L] [L] [L]
| | |
[G] [G] [C]───────┐
| |
└────────┘
(回环: x4 与 x1)关键设计：
- IMU 预积分提供帧间初始位姿估计和运动畸变校正
- 关键帧选择基于位移和角度阈值
- 局部地图使用基于关键帧的滑动窗口，而非全局点云
- 使用 GTSAM 库的增量平滑（iSAM2）进行在线优化
4.3 FAST-LIO2（Fast LiDAR-Inertial Odometry）
FAST-LIO2 通过 iKD-Tree（incremental KD-Tree）实现了极高的计算效率。
核心创新：
-
iKD-Tree：增量式 KD-Tree 数据结构，支持高效的点插入、删除和最近邻查询。避免每次扫描后重建 KD-Tree 的开销。
-
迭代卡尔曼滤波（IEKF）：将 LiDAR 点到地图的配准问题嵌入 EKF 的更新步骤中。状态包含位姿和 IMU 偏置。
-
直接配准：不提取特征，对每个原始点直接搜索最近邻平面，计算点到平面残差。
状态方程：
$$ x_{k+1} = x_k \boxplus (\Delta t \cdot f(x_k, u_k, w_k)) $$
其中 $\boxplus$ 为流形上的加法运算（对 $SO(3)$ 使用指数映射）。
性能：在中等规模环境中，FAST-LIO2 可在嵌入式平台（如 ARM Cortex-A72）上实时运行，单帧处理时间约 10-20ms。
4.4 KISS-ICP（Keep It Small and Simple ICP）
KISS-ICP 追求极简设计，仅包含四个核心组件：
- 自适应阈值：基于当前运动速度动态调整对应点搜索半径
- 体素降采样：对输入点云进行体素网格滤波
- 点到点 ICP：迭代最近点配准
- 局部地图管理：固定大小的体素化滑动窗口地图
无需 IMU、无需特征提取、无需回环检测，仅依赖逐帧 ICP 配准。在多个公开数据集上展现了与复杂系统相当的精度。
4.5 ICP 变体对比
点到点 ICP（Point-to-Point）：
$$ E = \sum_i | p_i - T q_i |^2 $$
使用 SVD 求解闭式解。收敛慢，对初值敏感。
点到平面 ICP（Point-to-Plane）：
$$ E = \sum_i \left( n_i^T (p_i - T q_i) \right)^2 $$
$n_i$ 为目标点处的法向量。收敛速度显著快于点到点。
GICP（Generalized ICP）：
将每个对应点对建模为两个局部平面分布的配准：
$$ E = \sum_i d_i^T (C_i^B + T C_i^A T^T)^{-1} d_i $$
其中 $C_i^A, C_i^B$ 为对应点邻域的协方差矩阵，$d_i = p_i - Tq_i$。GICP 统一了点到点和点到平面 ICP 作为特殊情况。
4.6 主要 LiDAR SLAM 系统对比
| 系统 | 传感器 | 配准方法 | 后端 | 回环 | 实时性能 | 开源 |
|---|---|---|---|---|---|---|
| LOAM | LiDAR | 特征(边缘+平面) | 无 | 无 | 10Hz (高频) + 1Hz (低频) | 是 |
| LeGO-LOAM | LiDAR | 地面分割+特征 | 位姿图 | ICP验证 | 实时 | 是 |
| LIO-SAM | LiDAR+IMU(+GPS) | 特征 | 因子图(iSAM2) | 距离+ICP | 实时 | 是 |
| FAST-LIO2 | LiDAR+IMU | 直接(点到平面) | IEKF | 无 | >100Hz | 是 |
| KISS-ICP | LiDAR | 点到点ICP | 无 | 无 | 实时 | 是 |
| CT-ICP | LiDAR | 连续时间ICP | 无 | 无 | 实时 | 是 |
| Faster-LIO | LiDAR+IMU | 直接(iVox) | IEKF | 无 | >FAST-LIO2 | 是 |
| LiDAR-SLAM (Cartographer) | LiDAR(+IMU) | 子图匹配 | 位姿图 | 分支定界 | 实时 | 是 |
5. 图优化后端（Graph-based Backend）
5.1 因子图（Factor Graph）
因子图是一种二部图（bipartite graph），包含变量节点（variable nodes）和因子节点（factor nodes）。
定义：因子图表示联合概率分布的因子分解：
$$ p(\mathcal{X}) \propto \prod_i f_i(\mathcal{X}_i) $$
其中 $\mathcal{X}$ 为所有变量，$f_i$ 为第 $i$ 个因子，$\mathcal{X}_i \subseteq \mathcal{X}$ 为该因子涉及的变量子集。
SLAM 中的典型因子：
因子类型 连接变量 残差定义
─────────────────────────────────────────────────────────────
先验因子 x_0 r = x_0 - x_prior
里程计因子 x_i, x_{i+1} r = x_{i+1} ⊖ (x_i ⊕ Δx_meas)
GPS因子 x_i r = pos(x_i) - p_GPS
IMU预积分因子 x_i, x_{i+1} r = 预积分残差(见3.1节)
视觉因子 x_i, l_j r = π(T_i, l_j) - z_ij
回环因子 x_i, x_j r = x_j ⊖ (x_i ⊕ Δx_loop)5.2 位姿图优化（Pose Graph Optimization）
位姿图是因子图的简化形式，仅保留位姿节点，路标点已被边缘化。
优化问题：
$$ \mathcal{X}^* = \arg\min_{\mathcal{X}} \sum_{(i,j) \in \mathcal{E}} | e_{ij}(\mathcal{X}) |{\Omega{ij}}^2 $$
其中 $e_{ij} = \log(T_{ij}^{-1} T_i^{-1} T_j)^\vee$ 为相对位姿误差在李代数上的表示，$\Omega_{ij}$ 为信息矩阵。
使用高斯-牛顿法（Gauss-Newton）求解：
- 在当前估计处线性化：$e_{ij}(\mathcal{X} + \delta) \approx e_{ij} + J_{ij} \delta$
- 构建法方程：$H \delta^* = -b$
- $H = \sum J_{ij}^T \Omega_{ij} J_{ij}$（信息矩阵/Hessian）
- $b = \sum J_{ij}^T \Omega_{ij} e_{ij}$
- 求解 $\delta^$ 并更新：$\mathcal{X} \leftarrow \mathcal{X} \boxplus \delta^$
- 重复直到收敛
5.3 回环检测（Loop Closure Detection）
5.3.1 DBoW2（Bags of Binary Words）
DBoW2 使用视觉词袋模型进行基于外观的位置识别。
离线阶段：对大量 ORB 描述子进行层次 K-means 聚类，构建视觉词典树（vocabulary tree）。
在线阶段：
- 将当前帧的描述子量化为词袋向量 $v$
- 使用 TF-IDF（Term Frequency-Inverse Document Frequency）加权
- 计算与数据库中所有关键帧的 L1 距离
- 时间一致性验证（temporal consistency）：连续多帧检测到同一候选才接受
5.3.2 NetVLAD
NetVLAD 是基于深度学习的全局描述子，将整张图像编码为固定维度向量。
网络结构：CNN backbone（VGG-16）+ VLAD pooling layer。VLAD 层学习 $K$ 个聚类中心 $c_k$，对特征进行软分配：
$$ V(j,k) = \sum_i \frac{e^{w_k^T x_i + b_k}}{\sum_{k’} e^{w_{k’}^T x_i + b_{k’}}} (x_i(j) - c_k(j)) $$
输出经过 L2 归一化和 PCA 降维后得到紧凑描述子（通常 4096 维降至 256 维）。
使用三元组损失（triplet loss）训练：使同一位置图像对描述子接近，不同位置图像对描述子远离。
5.4 优化库
5.4.1 g2o（General Graph Optimization）
g2o 的核心抽象：
// g2o 使用示例: 2D 位姿图优化
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 3>> BlockSolverType;
typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
auto solver = new g2o::OptimizationAlgorithmLevenberg(
std::make_unique<BlockSolverType>(
std::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
// 添加位姿顶点
for (int i = 0; i < num_poses; i++) {
auto v = new g2o::VertexSE2();
v->setId(i);
v->setEstimate(initial_poses[i]);
optimizer.addVertex(v);
}
// 添加里程计边
for (auto& constraint : odometry_constraints) {
auto e = new g2o::EdgeSE2();
e->setVertex(0, optimizer.vertex(constraint.from));
e->setVertex(1, optimizer.vertex(constraint.to));
e->setMeasurement(constraint.measurement);
e->setInformation(constraint.info_matrix);
optimizer.addEdge(e);
}
optimizer.initializeOptimization();
optimizer.optimize(20); // 20次迭代5.4.2 GTSAM（Georgia Tech Smoothing and Mapping）
GTSAM 基于 Bayes Tree 数据结构实现增量式优化（iSAM2），适合在线 SLAM。
// GTSAM 使用示例
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
gtsam::ISAM2Params params;
params.relinearizeThreshold = 0.1;
params.relinearizeSkip = 1;
gtsam::ISAM2 isam(params);
gtsam::NonlinearFactorGraph graph;
gtsam::Values initial_values;
// 添加 IMU 预积分因子
graph.add(gtsam::CombinedImuFactor(
X(i), V(i), X(i+1), V(i+1), B(i), B(i+1),
preintegrated_imu));
// 添加视觉因子
graph.add(gtsam::GenericProjectionFactor<Pose3, Point3, Cal3_S2>(
measured_pixel, noise_model, X(i), L(j), calibration));
// 增量更新
isam.update(graph, initial_values);
gtsam::Values result = isam.calculateEstimate();5.4.3 Ceres Solver
Ceres 是 Google 开发的通用非线性最小二乘求解器，在 SLAM 中广泛使用。
// Ceres 使用示例: 重投影误差
struct ReprojectionError {
double observed_x, observed_y;
template <typename T>
bool operator()(const T* const camera, // 6-DOF pose
const T* const point, // 3D point
T* residuals) const {
// 旋转和平移 (angle-axis)
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
// 投影
T predicted_x = p[0] / p[2];
T predicted_y = p[1] / p[2];
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
return true;
}
};
ceres::Problem problem;
for (auto& obs : observations) {
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<ReprojectionError, 2, 6, 3>(
new ReprojectionError(obs.x, obs.y)),
new ceres::HuberLoss(1.0),
cameras[obs.camera_idx],
points[obs.point_idx]);
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_SCHUR;
options.num_threads = 8;
ceres::Solve(options, &problem, &summary);5.5 边缘化（Marginalization）与舒尔补（Schur Complement）
在滑动窗口优化中，当旧关键帧被移出窗口时，不能简单丢弃，需要通过边缘化保留其对剩余变量的约束信息。
将变量分为要保留的 $x_r$ 和要边缘化的 $x_m$，法方程为：
$$ \begin{bmatrix} H_{mm} & H_{mr} \ H_{rm} & H_{rr} \end{bmatrix} \begin{bmatrix} \delta x_m \ \delta x_r \end{bmatrix} = \begin{bmatrix} b_m \ b_r \end{bmatrix} $$
通过 Schur 补消除 $\delta x_m$：
$$ (H_{rr} - H_{rm} H_{mm}^{-1} H_{mr}) \delta x_r = b_r - H_{rm} H_{mm}^{-1} b_m $$
$$ H_{prior} = H_{rr} - H_{rm} H_{mm}^{-1} H_{mr} $$
$$ b_{prior} = b_r - H_{rm} H_{mm}^{-1} b_m $$
$H_{prior}$ 和 $b_{prior}$ 构成了边缘化先验因子，作为下一次优化的约束项加入。
注意事项：
- 边缘化操作使稀疏矩阵变稠密（fill-in 效应）
- 边缘化的线性化点不能改变（First Estimates Jacobian, FEJ），否则会引入不一致性
- 在 VINS-Mono 中，边缘化策略为：如果次新帧是关键帧则边缘化最旧帧；否则丢弃次新帧的视觉观测但保留 IMU 约束
5.6 滑动窗口优化（Sliding Window Optimization）
滑动窗口优化在有界的计算资源下实现近似最优估计。
窗口管理策略：
时间轴: ─────────────────────────────────────────>
关键帧: KF0 KF1 KF2 KF3 KF4 KF5 KF6 KF7
└─────── 窗口 ──────┘
边缘化 当前帧
新关键帧到来:
情况A (KF8质量高): 边缘化KF2, 窗口变为 [KF3..KF8]
情况B (KF8质量低): 丢弃KF7的部分信息, 保持窗口大小窗口内执行完整的 Bundle Adjustment，包含所有因子（视觉、IMU、先验）。典型窗口大小：VINS-Mono 使用 10 个关键帧。
6. 地图表示（Map Representation）
6.1 稀疏点云地图（Sparse Point Cloud）
ORB-SLAM 等系统维护的地图由离散3D路标点组成。每个地图点存储：
- 3D坐标
- 代表性描述子（从所有观测帧中选择与其他描述子中位距离最小的）
- 观测方向范围
- 有效距离范围（对应特征金字塔层级）
用途：仅用于定位，不适合避障和路径规划。 存储效率：极高，典型场景仅需数万至数十万个点。
6.2 占据栅格地图（Occupancy Grid, 2D）
将环境划分为均匀网格，每个单元存储占据概率 $p(m_i | z_{1:t})$。
使用对数几率（log-odds）表示避免数值问题：
$$ l(m_i | z_{1:t}) = l(m_i | z_{1:t-1}) + l(m_i | z_t) - l_0 $$
其中 $l = \log \frac{p}{1-p}$，$l_0 = \log \frac{p_0}{1-p_0}$ 为先验。
栅格地图示例 (分辨率 0.05m):
0 1 2 3 4 5 6 7 8 9
0 . . . . . . . . . .
1 . . . # # # . . . .
2 . . . # . # . . . .
3 . . . # . # . . . .
4 . . . # # # . . . .
5 . . . . . . . . . .
6 . . . . . . # # . .
7 . . . . . . # # . .
8 . . . . . . . . . .
9 . . . . . . . . . .
图例: . = 空闲(free), # = 占据(occupied)
每个单元存储 log-odds 值适用场景：2D 导航（移动机器人地面规划）。 局限：仅表示单层平面，无法表示三维结构。
6.3 OctoMap（3D Octree Map）
OctoMap 使用八叉树（octree）高效表示三维占据信息。
数据结构：每个节点代表一个立方体空间，可递归细分为8个子节点。通过概率更新判定节点状态（空闲/占据/未知），状态一致的子节点可被剪枝合并到父节点。
优势：
- 多分辨率：根据需要选择不同精度
- 压缩存储：大面积同质区域自动合并
- 概率更新：支持传感器噪声建模
典型参数：分辨率 0.05-0.2m，内存占用约 50-200MB/大场景。
6.4 TSDF（Truncated Signed Distance Function）
TSDF 在每个体素中存储到最近表面的截断有符号距离值。
$$ D(v) = \begin{cases} \min(1, d(v) / \tau) & \text{if } d(v) > 0 \text{ (表面前方)} \ \max(-1, d(v) / \tau) & \text{if } d(v) \leq 0 \text{ (表面后方)} \end{cases} $$
其中 $\tau$ 为截断距离。表面位于 $D(v) = 0$ 的等值面上。
融合更新：对每次新观测，使用加权平均融合：
$$ D_{new}(v) = \frac{W_{old}(v) \cdot D_{old}(v) + w(v) \cdot d_{obs}(v)}{W_{old}(v) + w(v)} $$
$$ W_{new}(v) = \min(W_{old}(v) + w(v), W_{max}) $$
表面提取：使用 Marching Cubes 算法从 TSDF 中提取等值面网格。
应用：KinectFusion、Voxblox、NVIDIA nvblox 等实时三维重建系统。
6.5 语义地图（Semantic Maps）
在几何地图基础上附加语义标签。实现方式：
- 点级语义：对每个3D点赋予类别标签（来自语义分割网络，如 DeepLab, SegFormer）
- 物体级语义：检测物体实例，估计6DoF位姿和三维边界框
- 场景图（Scene Graph）：节点为物体/区域，边为空间关系（on, in, near, support 等）
语义地图使机器人能够理解”桌子上的杯子”而非仅仅”坐标 (1.2, 0.8, 0.7) 处的点云簇”。
6.6 神经隐式地图（Neural Implicit Maps）
使用神经网络隐式表示三维场景，通过网络参数编码几何和外观信息。
6.6.1 iMAP
iMAP 是首个实时神经隐式 SLAM 系统。使用单个 MLP 表示整个场景的 TSDF：
$$ f_\theta: \mathbb{R}^3 \to (\text{SDF value}, \text{RGB color}) $$
同时优化网络参数 $\theta$ 和关键帧位姿。使用分层采样策略在已有关键帧和当前帧之间平衡训练。
局限：单 MLP 容量有限，大场景下出现”遗忘”（catastrophic forgetting）。
6.6.2 NICE-SLAM
NICE-SLAM 引入层次化特征网格（hierarchical feature grid）解决 iMAP 的容量问题：
- 粗分辨率网格：编码全局几何
- 中分辨率网格：编码局部细节
- 细分辨率网格：编码精细表面
每层使用小型 MLP 解码器将特征转换为 SDF 值。使用深度监督和光度监督联合训练。
6.6.3 NeRF-SLAM
将 Neural Radiance Fields 与视觉里程计结合。前端使用 DROID-SLAM 提供位姿估计和深度图，后端使用 Instant-NGP（哈希编码）实现实时 NeRF 地图构建。
神经隐式地图的优劣势：
| 优势 | 劣势 |
|---|---|
| 连续表示，无分辨率限制 | GPU 计算需求高 |
| 自然支持补全（fill holes） | 训练/推理速度制约实时性 |
| 可微分，支持端到端学习 | 难以增量更新（全局优化） |
| 紧凑存储（网络参数） | 缺乏动态场景处理能力 |
6.7 地图表示方法总结
| 方法 | 维度 | 存储效率 | 导航适用 | 重建质量 | 语义扩展 |
|---|---|---|---|---|---|
| 稀疏点云 | 3D | 极高 | 差 | 差 | 困难 |
| 占据栅格 | 2D | 中 | 好 | 不适用 | 容易 |
| OctoMap | 3D | 高 | 好 | 中 | 中等 |
| TSDF | 3D | 低 | 中 | 好 | 中等 |
| 神经隐式 | 3D | 高 | 差（当前） | 极好 | 自然支持 |
| 场景图 | 拓扑 | 极高 | 中 | 不适用 | 核心 |
7. 多传感器融合 SLAM（Multi-sensor Fusion SLAM）
7.1 多模态融合的动机
单一传感器在特定条件下存在固有局限：
| 传感器 | 失效场景 | 原因 |
|---|---|---|
| 相机 | 暗光/过曝 | 光照依赖 |
| 相机 | 无纹理区域 | 特征/光度信息不足 |
| LiDAR | 退化几何（长走廊） | 缺乏沿走廊方向的约束 |
| LiDAR | 雨/雪/雾 | 激光散射 |
| IMU | 长时间静止后 | 偏置漂移无法校正 |
多模态融合的目标：利用不同传感器的互补特性，在各种环境条件下保持鲁棒性和精度。
7.2 R3LIVE（Robust Real-time RGB-colored point cloud LiDAR-Inertial-Visual）
R3LIVE 实现了 LiDAR-惯性-视觉三模态紧耦合。
系统架构：
┌─────────────────────────────────────────────────────────┐
│ R3LIVE │
├─────────────────────────┬───────────────────────────────┤
│ LIO 子系统 │ VIO 子系统 │
│ │ │
│ LiDAR点 + IMU │ 图像 + IMU │
│ │ │ │ │
│ v │ v │
│ FAST-LIO2 │ 光度误差最小化 │
│ (IEKF, iKD-Tree) │ (直接法, 帧到地图) │
│ │ │ │ │
│ v │ v │
│ 位姿 + 几何地图 │ 位姿精化 + RGB着色 │
│ │ │
├─────────────────────────┴───────────────────────────────┤
│ 共享状态 + RGB点云地图 │
└─────────────────────────────────────────────────────────┘设计要点：
- LIO 子系统提供高频位姿估计和几何点云
- VIO 子系统利用图像信息精化位姿并为点云着色
- 两个子系统共享 IMU 状态，但异步运行
- 最终输出带有真实颜色的稠密3D点云地图
7.3 LVI-SAM（LiDAR-Visual-Inertial SLAM via Smoothing and Mapping）
LVI-SAM 将 LIO-SAM 和 VINS-Mono 集成到统一的因子图框架中。
因子图结构：
变量节点: x_0, x_1, ..., x_n (位姿+速度+偏置)
因子:
1. IMU预积分因子: 连接相邻帧
2. LiDAR里程计因子: LiDAR帧间配准提供的位姿增量
3. 视觉里程计因子: 视觉特征跟踪提供的位姿增量
4. GPS因子(可选): 全局位置约束
5. 回环因子: 视觉DBoW2 + LiDAR ICP联合验证
x0 ──[IMU]── x1 ──[IMU]── x2 ──[IMU]── x3
| | | |
[LiDAR] [LiDAR] [LiDAR] [LiDAR]
| | | |
[Visual] [Visual] [Visual] [Visual]
| |
[GPS] [Loop]互补机制：
- LiDAR 提供精确的近距离几何约束和尺度信息
- 视觉在 LiDAR 退化场景（如长走廊）中提供额外约束
- 视觉回环检测能力强于纯 LiDAR（外观信息更丰富）
- IMU 提供高频运动估计并桥接传感器之间的时间间隔
7.4 融合架构分类
┌─────────────────┐
│ 融合层级分类 │
└────────┬────────┘
│
┌────────────┼────────────┐
│ │ │
┌─────v─────┐ ┌───v───┐ ┌─────v─────┐
│ 数据层融合 │ │特征层 │ │ 决策层融合 │
│ │ │融合 │ │ │
│ 原始数据 │ │中间 │ │ 各传感器 │
│ 直接融合 │ │表示 │ │ 独立处理 │
│ (点云着色)│ │融合 │ │ 结果融合 │
└───────────┘ └───────┘ └───────────┘
精度: 高 中高 中
耦合度: 紧 中 松
复杂度: 高 中 低
容错性: 低 中 高7.5 标定（Calibration）
多传感器系统的精度高度依赖外参标定质量。
需要标定的参数：
| 标定对 | 参数 | 方法 |
|---|---|---|
| Camera-IMU | $T_{CI} \in SE(3)$, $t_d$ | Kalibr (Allan Variance + 靶标) |
| LiDAR-Camera | $T_{LC} \in SE(3)$ | 棋盘格对齐 / 互信息最大化 |
| LiDAR-IMU | $T_{LI} \in SE(3)$ | LI-Init (运动对齐) |
| Multi-Camera | $T_{C_i C_j}$ | 多视角靶标观测 |
在线标定：部分系统（OpenVINS, VINS-Mono）支持在运行时联合估计外参，将外参作为状态变量加入优化。这对初始标定不精确或外参随时间漂移的场景尤为重要。
8. 公司格局
8.1 行业主要公司
| 公司 | 国家/地区 | 核心技术 | 主要产品/场景 | 技术路线 |
|---|---|---|---|---|
| Clearpath Robotics | 加拿大 | 自主导航平台 | 工业移动机器人 (Husky, Jackal, Dingo) | LiDAR SLAM + 多传感器融合 |
| NavVis | 德国 | 室内测绘 | VLX 移动扫描仪, IVION 数字孪生平台 | LiDAR-Visual SLAM + 全景相机 |
| Leica Geosystems | 瑞士 | 精密测量 | RTC360 扫描仪, BLK 系列 | 高精度 LiDAR + IMU |
| Google (Cartographer) | 美国 | 开源 SLAM | Cartographer 库, Google Maps 室内 | 子图匹配 + 位姿图 + 分支定界回环 |
| SLAMTEC | 中国 | 低成本 SLAM | RPLIDAR 系列, Slamware 导航模块 | 2D LiDAR SLAM (粒子滤波/图优化) |
| Kudan | 英国/日本 | SLAM 引擎 | KudanSLAM SDK (嵌入式) | 视觉 + LiDAR 融合, 专利算法 |
| HERE Technologies | 荷兰/美国 | 高精地图 | HD Live Map, 自动驾驶地图服务 | 众包 SLAM + 地图压缩 |
| Matterport | 美国 | 3D扫描 | Pro3 相机, 数字孪生平台 | RGB-D SLAM + 深度学习补全 |
| Zillow (已并入) | 美国 | 室内3D | 房产3D导览 | NeRF + 传统SLAM混合 |
| Foxglove | 美国 | 机器人可视化 | Foxglove Studio | SLAM数据可视化与调试工具 |
8.2 开源生态
| 项目 | 维护方 | 传感器 | 特点 |
|---|---|---|---|
| ORB-SLAM3 | Universidad de Zaragoza | Mono/Stereo/RGB-D + IMU | 最完整的视觉SLAM |
| VINS-Mono/Fusion | HKUST | Mono/Stereo + IMU | VIO 标杆 |
| LIO-SAM | MIT | LiDAR + IMU + GPS | 因子图 LiDAR-Inertial |
| FAST-LIO2 | HKU | LiDAR + IMU | 极致效率 |
| OpenVINS | University of Delaware | Multi-cam + IMU | 模块化 MSCKF |
| Cartographer | LiDAR + IMU | 工业级2D/3D SLAM | |
| RTAB-Map | IntRoLab | Multi-sensor | 通用图优化框架 |
| Kimera | MIT | Stereo + IMU | 语义SLAM + 3D场景图 |
| DROID-SLAM | Princeton | Mono/Stereo | 深度学习端到端VO |
| Gaussian Splatting SLAM | 多个研究组 | RGB-D/Mono | 3DGS地图表示 |
8.3 技术趋势
- 基于学习的前端：深度网络取代手工特征（SuperPoint, SuperGlue, LightGlue），端到端里程计（DROID-SLAM, DPVO）
- 神经地图表示：3D Gaussian Splatting 正在取代 NeRF 成为实时神经渲染的首选，同步 SLAM 系统（SplaTAM, Gaussian-SLAM）快速发展
- 基础模型集成：大型视觉模型（DINOv2, SAM）提供的语义特征增强回环检测和位置识别鲁棒性
- 边缘部署：模型量化、算子融合、专用硬件（SLAM加速器 ASIC）推动嵌入式实时运行
- 动态环境：从静态世界假设走向动态物体检测与跟踪的联合估计（DynaSLAM, DOT）
参考文献
- Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
- Hartley, R., & Zisserman, A. (2003). Multiple View Geometry in Computer Vision. Cambridge University Press.
- Cadena, C., et al. (2016). Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age. IEEE Transactions on Robotics, 32(6), 1309-1332.
- Mur-Artal, R., Montiel, J. M. M., & Tardós, J. D. (2015). ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Transactions on Robotics, 31(5), 1147-1163.
- Campos, C., et al. (2021). ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM. IEEE Transactions on Robotics, 37(6), 1874-1890.
- Qin, T., Li, P., & Shen, S. (2018). VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator. IEEE Transactions on Robotics, 34(4), 1004-1020.
- Forster, C., et al. (2017). On-Manifold Preintegration for Real-Time Visual-Inertial Odometry. IEEE Transactions on Robotics, 33(1), 1-21.
- Zhang, J., & Singh, S. (2014). LOAM: Lidar Odometry and Mapping in Real-time. Robotics: Science and Systems (RSS).
- Shan, T., et al. (2020). LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping. IEEE/RSJ IROS.
- Xu, W., et al. (2022). FAST-LIO2: Fast Direct LiDAR-Inertial Odometry. IEEE Transactions on Robotics, 38(4), 2053-2073.
- Vizzo, I., et al. (2023). KISS-ICP: In Defense of Point-to-Point ICP. IEEE Robotics and Automation Letters, 8(2), 1029-1036.
- Engel, J., Koltun, V., & Cremers, D. (2018). Direct Sparse Odometry. IEEE Transactions on Pattern Analysis and Machine Intelligence, 40(3), 611-625.
- Sucar, E., et al. (2021). iMAP: Implicit Mapping and Positioning in Real-Time. ICCV.
- Zhu, Z., et al. (2022). NICE-SLAM: Neural Implicit Scalable Encoding for SLAM. CVPR.
- Lin, J., & Zhang, F. (2022). R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package. ICRA.
- Shan, T., et al. (2021). LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping. ICRA.
- Dellaert, F., & Kaess, M. (2017). Factor Graphs for Robot Perception. Foundations and Trends in Robotics, 6(1-2), 1-139.
- Mourikis, A. I., & Roumeliotis, S. I. (2007). A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. ICRA.
- Arandjelović, R., et al. (2016). NetVLAD: CNN architecture for weakly supervised place recognition. CVPR.
- Kerbl, B., et al. (2023). 3D Gaussian Splatting for Real-Time Radiance Field Rendering. ACM TOG (SIGGRAPH).