2. SLAM基础
SLAM（Simultaneous Localization and Mapping）系统的本质，是在未知环境中同时估计机器人自身轨迹与周围环境地图。要构建这样一个估计系统，首先必须建立严谨的数学描述框架：传感器如何产生数据、数据如何在不同坐标系间转换、位姿如何参数化、观测方程如何建模、噪声如何量化。本章作为整个课程的地基，将系统梳理这些核心概念，并重点说明它们如何在因子图框架下被统一表达。
上图展示了本章内容的整体逻辑关系：传感器模型提供原始测量数据，坐标系变换将数据统一到参考框架，位姿表示提供状态量的数学参数化，观测模型建立测量与状态之间的函数关系，最终所有要素汇聚到因子图框架中，通过非线性优化求解。下面逐一展开。
2.1 传感器模型与测量原理
核心原理：SLAM系统依赖多种传感器获取环境信息。不同传感器的物理原理不同，其数学模型也截然不同。视觉相机通过透视投影将三维空间点映射到二维像素平面；LiDAR通过激光测距直接获取三维点云；IMU则测量角速度和加速度。每种传感器都需要建立精确的数学模型，才能将原始测量数据转化为可用于状态估计的信息。
以针孔相机模型为例，其投影过程可描述为：
Z * [u, v, 1]ᵀ = K * [R|t] * [X, Y, Z, 1]ᵀ其中 K 为内参矩阵（焦距 fx, fy 和主点 cx, cy），[R|t] 为外参（相机在世界坐标系中的位姿），(X, Y, Z) 为空间点在世界系下的坐标，(u, v) 为像素坐标。实际相机还存在镜头畸变，通常用径向畸变（k1, k2, k3）和切向畸变（p1, p2）来建模。
传感器标定流程：
- 内参标定：使用棋盘格或圆点标定板，采集多角度图像，通过张正友标定法求解内参和畸变系数。
- 外参标定：确定传感器坐标系与机体坐标系（或IMU坐标系）之间的固定变换 T_sensor_body。
- 时间同步：多传感器之间需要时间戳对齐，通常采用硬件同步或软件插值方法。
| 传感器类型 | 测量量 | 数学模型 | 典型噪声 |
|---|---|---|---|
| 单目相机 | 像素坐标 (u,v) | 透视投影 + 畸变 | 像素噪声 σ≈1-2px |
| 双目/深度相机 | 深度 Z | 三角测量 / ToF | 深度噪声随距离增大 |
| LiDAR | 三维点 (x,y,z) | 激光测距 + 扫描 | 距离噪声 σ≈2-5cm |
| IMU | 加速度 a、角速度 ω | 牛顿力学 + 陀螺仪 | 白噪声 + 随机游走 |
2.2 坐标系变换与刚体运动
核心原理：SLAM中涉及多个坐标系：世界坐标系（W，固定参考）、机体坐标系（B，随机器人运动）、相机坐标系（C）、IMU坐标系（I）等。坐标系之间的变换属于刚体变换，包含旋转和平移两部分。在三维空间中，刚体变换可用齐次变换矩阵 T ∈ SE(3) 表示：
T = [ R t ]
[ 0 1 ]其中 R ∈ SO(3) 为旋转矩阵，t ∈ R³ 为平移向量。齐次坐标使得变换可以写成矩阵乘法形式，便于链式复合：T_WC = T_WB · T_BC，表示从相机系到世界系的变换。
坐标系变换的操作流程：
- 明确参考系：每个测量数据必须标注其所在坐标系，例如点云在LiDAR系下，图像在相机系下。
- 统一到机体系：通过外参 T_sensor_body 将所有传感器数据变换到机体坐标系。
- 变换到世界系：利用当前位姿估计 T_WB 将机体系数据变换到世界系。
- 注意变换方向：T_AB 表示从B系到A系的变换，复合时注意下标顺序。
2.3 位姿表示：旋转矩阵、四元数与李群李代数
核心原理：位姿表示是SLAM中最核心的数学问题之一。三维旋转有多种表示方式，各有优劣：
| 表示方式 | 维度 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 R | 9 | 直观、易于复合 | 冗余、需满足正交约束 |
| 欧拉角 | 3 | 直观、无冗余 | 万向锁、插值困难 |
| 四元数 q | 4 | 无奇异、紧凑、易插值 | 双覆盖（q 和 -q 等价） |
| 李代数 so(3) | 3 | 无约束、适合优化 | 需指数/对数映射 |
李群与李代数的关键作用：在非线性优化中，旋转矩阵 R 受正交约束 RᵀR=I 且 det(R)=1，这使得直接对 R 求导非常困难。李群 SO(3) 的李代数 so(3) 是一个三维向量空间，通过指数映射 exp(φ^) 与 SO(3) 建立联系。这样，优化时可以在无约束的李代数空间中进行增量更新：
R_new = R · exp(δφ^)其中 δφ ∈ R³ 是微小旋转增量。这种参数化方式避免了约束优化问题，是GTSAM、Ceres等优化库的标准做法。
四元数与旋转矩阵的转换：
- 四元数 q = (w, x, y, z)，满足 w²+x²+y²+z²=1
- 旋转矩阵 R 可由四元数通过 Rodrigues 公式转换
- 四元数乘法对应旋转复合：q₁ ⊗ q₂ 表示先旋转 q₂ 再旋转 q₁
- 注意四元数的双覆盖性：q 和 -q 表示同一旋转，在优化中需保持符号一致性
2.4 观测模型与噪声建模
核心原理：观测模型描述“状态量 → 测量值”的函数关系。在视觉SLAM中，最常用的是重投影模型：给定路标点在世界系下的坐标 P_w 和相机位姿 T_WC，预测像素坐标：
z_pred = proj(T_WC · P_w)其中 proj 包含内参投影和畸变校正。实际测量值 z_obs 与预测值之间的差异即为重投影误差：
e = z_obs - z_pred噪声建模：SLAM通常假设测量噪声服从零均值高斯分布：
z_obs = h(x) + ε, ε ~ N(0, Σ)其中 Σ 为测量协方差矩阵。在因子图中，每个观测因子对应一个残差项，其权重由信息矩阵 Ω = Σ⁻¹ 决定。协方差矩阵的准确估计对优化结果影响显著：
- 视觉重投影：通常假设像素噪声各向同性，Σ = σ²I，σ 取 1-2 像素。
- LiDAR 配准：噪声与距离相关，需根据传感器手册或经验标定。
- IMU 预积分：噪声包含白噪声和随机游走两部分，需通过 Allan 方差分析确定。
2.5 案例拆解：单目相机观测模型在因子图中的实现
场景背景：假设我们有一个单目相机在室内环境中运动，需要估计相机轨迹和稀疏路标点。相机内参已标定：fx=500, fy=500, cx=320, cy=240，无畸变。我们使用GTSAM构建因子图。
关键参数与初始条件：
- 相机分辨率：640×480
- 初始位姿：T_WC₀ = I（原点）
- 路标点：已知3个特征点的世界坐标
- 测量噪声：σ = 1.5 像素
因子图构建步骤：
- 定义变量节点：位姿节点 Pose3（李代数参数化），路标点节点 Point3。
- 添加先验因子：对初始位姿添加先验，约束其接近原点。
- 添加观测因子：对每个特征点观测，创建 GenericProjectionFactor，传入相机内参和测量像素坐标。
- 设置噪声模型：使用 Diagonal::Sigmas 设置为 [1.5, 1.5]（像素坐标两个方向）。
- 优化求解：使用 Levenberg-Marquardt 优化器迭代求解。
结果对比：
| 优化项 | 初始估计 | 优化后 | 真实值 |
|---|---|---|---|
| 位姿 x (m) | 0.05 | 0.012 | 0.01 |
| 位姿 y (m) | -0.03 | -0.008 | -0.01 |
| 路标点1 (m) | (1.2, 0.8, 3.0) | (1.05, 0.72, 2.95) | (1.0, 0.7, 3.0) |
| 平均重投影误差 (px) | 8.3 | 1.2 | — |
复盘经验：优化后重投影误差从8.3像素降至1.2像素，接近理论噪声水平。但需要注意：单目视觉存在尺度不确定性，如果没有其他传感器（如IMU）或已知尺度信息，路标点的尺度无法唯一确定。在实际系统中，通常需要初始化或融合IMU来恢复尺度。
2.6 常见误区与总结
常见误区梳理：
- 混淆坐标系：未明确数据所在坐标系，导致变换错误。解决：在代码中为每个变量标注坐标系，使用类型安全的库（如GTSAM的Pose3自带坐标系语义）。
- 四元数未归一化：优化过程中四元数可能偏离单位范数，导致旋转表示无效。解决：在优化步骤间重新归一化，或使用李代数参数化。
- 噪声模型过于简化：假设所有测量噪声相同，忽略了传感器特性差异。解决：根据传感器类型和测量距离分别设置协方差。
- 忽略外参不确定性：外参标定误差会系统性影响所有观测。解决：在标定时充分采集数据，必要时将外参也作为优化变量。
- 过度信任初始值：非线性优化对初始值敏感，差的初始值可能收敛到局部极小值。解决：使用运动模型或IMU预积分提供较好的初始估计。