📚 课程目录第3章 SLAM数学基础

3. SLAM数学基础

激光SLAM（Simultaneous Localization and Mapping）的核心任务，是在未知环境中同时估计机器人自身的位姿（位置与姿态）并构建环境地图。这一过程本质上是一个状态估计问题，而状态估计的每一步都建立在严谨的数学语言之上。本章将系统梳理支撑Cartographer等主流建图算法的数学基石，涵盖坐标系定义、刚体变换、旋转矩阵、四元数、李群与李代数以及概率论基础。理解这些概念，是读懂前端扫描匹配、后端图优化以及回环检测代码的前提。

3.1 坐标系定义与刚体变换

核心原理

在SLAM系统中，所有传感器数据（激光点云、IMU角速度与加速度）都必须在统一的坐标系下进行融合。通常定义三个关键坐标系：世界坐标系（World，记为W）固定于环境中的某个参考点，是地图构建的全局基准；机器人坐标系（Robot，记为R）固连于机器人本体，通常以底盘几何中心为原点，x轴指向正前方；激光雷达坐标系（Lidar，记为L）固连于激光传感器，其原点位于激光发射中心。由于激光雷达安装位置与机器人中心存在偏移，必须通过外参标定获得L到R的变换矩阵，才能将点云转换到机器人坐标系下进行后续处理。

刚体变换描述了一个物体在三维空间中只发生旋转和平移、不发生形变的运动。在数学上，三维刚体变换由旋转矩阵 R（3×3正交矩阵，行列式为+1）和平移向量 t（3×1）组成。为了将旋转和平移统一表达，常使用齐次坐标形式：

T = [ R  t ]
    [ 0  1 ]    ∈ SE(3)

其中SE(3)表示三维特殊欧氏群。齐次变换矩阵使得连续变换可以表示为矩阵乘法，例如将激光点从L系转换到W系：p_W = T_WL · p_L，其中T_WL = T_WR · T_RL。

判断方法与操作流程

在实际工程中，判断一个变换矩阵是否正确，可以从以下三个维度验证：

正交性检查：验证 R^TR = I 且 det(R) = 1，若行列式为-1则包含镜像变换，属于非法旋转。
尺度保持：变换前后两点距离不变，即 ||p₁ - p₂|| = ||R·p₁ + t - (R·p₂ + t)||。
坐标轴方向：右手定则验证旋转后的坐标轴方向，避免因标定错误导致点云翻转。

操作流程上，首先使用标定板或工厂提供的参数确定L到R的外参，然后编写坐标变换模块，最后通过可视化工具（如RViz）将点云投影到机器人坐标系下，检查地面点是否水平、墙面是否垂直。

案例拆解：激光雷达安装偏移补偿

某移动机器人平台搭载16线激光雷达，安装位置相对于机器人中心在x方向偏移0.15m，y方向偏移0.05m，z方向偏移0.20m，且绕z轴旋转了2°。若直接使用原始点云进行建图，会导致地图出现重影和边缘模糊。正确的处理流程为：

构建外参矩阵 T_RL，其中 R 为绕z轴旋转2°的旋转矩阵，t = [0.15, 0.05, 0.20]^T。
对每一帧点云执行变换：p_R = R·p_L + t。
将变换后的点云输入Cartographer前端，与子图进行匹配。

结果对比：补偿前地图点云厚度约0.12m，补偿后降至0.03m，建图精度显著提升。复盘经验：外参标定误差是建图质量的头号杀手，建议每次拆装传感器后重新标定。

常见误区：初学者常混淆“变换矩阵”与“坐标转换”的方向。T_WL 表示将L系下的坐标转换到W系，而非将W系坐标转换到L系。方向搞反会导致机器人位置完全错误。

3.2 旋转矩阵与四元数

核心原理

旋转矩阵是描述姿态最直观的方式，但存在9个参数、6个约束，且插值困难。四元数是一种超复数形式，用一个实部和三个虚部表示旋转，具有无奇异性、插值平滑的优点。单位四元数 q = (q₀, q₁, q₂, q₃) 满足 q₀² + q₁² + q₂² + q₃² = 1，其中 q₀ 为实部。四元数与旋转矩阵的转换关系为：

R = [ 1-2(q2²+q3²)   2(q1q2 - q0q3)   2(q1q3 + q0q2) ]
    [ 2(q1q2 + q0q3)   1-2(q1²+q3²)   2(q2q3 - q0q1) ]
    [ 2(q1q3 - q0q2)   2(q2q3 + q0q1)   1-2(q1²+q2²) ]

四元数乘法对应旋转的复合，且比矩阵乘法计算量更小。在Cartographer中，位姿的旋转部分通常以四元数形式存储，便于后端图优化中的增量更新。

对比分析：旋转表示方法

表示方法	参数数量	优点	缺点	适用场景
旋转矩阵	9	直观、易于坐标变换	冗余、非紧凑	坐标变换计算
欧拉角	3	直观、易理解	万向锁、插值困难	人机交互显示
四元数	4	无奇异性、插值平滑	不够直观	姿态存储、插值
轴角	3	几何意义明确	加法不封闭	李代数求导

实战场景：四元数插值

在机器人平滑运动控制中，需要从当前姿态 q₁ 插值到目标姿态 q₂。使用线性插值（LERP）会导致角速度不均匀，而球面线性插值（SLERP）能保持恒速旋转。SLERP公式为：

q(t) = [sin((1-t)θ) / sinθ] · q₁ + [sin(tθ) / sinθ] · q₂

其中 cosθ = q₁·q₂（点积）。若点积为负，需先取反 q₂ 以保证最短路径插值。在Cartographer的位姿预测模块中，正是利用SLERP对IMU数据进行预积分，为前端扫描匹配提供初始估计。

经验提醒：四元数存在“双重覆盖”问题，即 q 和 -q 表示同一旋转。在优化过程中，若不加约束，可能导致优化变量在 q 和 -q 之间跳变，造成数值不稳定。通常约定 q₀ ≥ 0 来消除歧义。

3.3 李群与李代数

核心原理

旋转矩阵构成特殊正交群SO(3)，齐次变换矩阵构成特殊欧氏群SE(3)。这些群是流形，不是向量空间，因此不能直接进行加法运算。李代数是对应李群单位元处的切空间，SO(3)对应的李代数so(3)是三维向量 φ ∈ R³，其反对称矩阵形式为 [φ]_×。通过指数映射，可以将李代数映射回李群：

R = exp([φ]×) = I + [φ]× + ([φ]×)²/2! + ...

反之，对数映射将李群映射回李代数。这一映射关系使得我们可以将旋转的“增量”表示为普通向量，从而在优化问题中方便地求导。在Cartographer的后端图优化中，位姿节点被参数化为SE(3)元素，而优化变量则是李代数上的扰动 δξ ∈ se(3)。

操作流程：扰动模型求导

在SLAM中，经常需要计算误差函数关于位姿的雅可比矩阵。使用李代数左扰动模型，可以避免复杂的约束求导：

定义误差函数 e(T) = z - h(T)，其中 z 为观测值，h 为预测模型。
对 T 施加左扰动 δT = exp(δξ^)，则 e(T ⊕ δξ) ≈ e(T) + J·δξ。
雅可比矩阵 J = ∂e/∂δξ 可通过链式法则计算，其中关键步骤是 ∂(T·p)/∂δξ = [I, -(R·p + t)_×]。

这一方法避免了直接对旋转矩阵求导时遇到的约束优化问题，是G2O、Ceres等优化库的标准做法。

案例拆解：图优化中的位姿更新

假设有两个位姿节点 T₁ 和 T₂，它们之间的相对约束测量为 ΔT₁₂。误差项为 e = log(ΔT₁₂⁻¹ · T₁⁻¹ · T₂)。在优化迭代中，每次更新为：

T₁ ← T₁ · exp(δξ₁^)
T₂ ← T₂ · exp(δξ₂^)

其中 δξ₁ 和 δξ₂ 由线性方程 H·δξ = b 求解。通过李代数参数化，优化变量维度从12维（矩阵元素）降至6维（平移3+旋转3），大幅提升了求解效率。在Cartographer的稀疏位姿图优化中，正是利用这一特性处理数千个节点的联合优化。

  核心要点：李群与李代数的价值在于将旋转/变换的“乘法”转化为“加法”，使得高斯牛顿法等基于向量空间的优化算法可以直接应用。理解 exp 和 log 映射的数值实现（如Rodrigues公式）是编写高效优化代码的基础。

3.4 概率论基础与状态估计

核心原理

SLAM本质上是一个状态估计问题：给定一系列传感器观测 z_1:k 和控制输入 u_1:k，估计机器人位姿 x_1:k 和地图 m 的后验概率 p(x_1:k, m | z_1:k, u_1:k)。由于直接求解高维后验不可行，通常假设噪声服从高斯分布，将问题转化为非线性最小二乘：

x* = argmin Σ ||zᵢ - h(xᵢ)||²<sub>Σᵢ</sub>

其中 Σᵢ 为观测噪声协方差矩阵。这一形式与最大似然估计（MLE）等价。在Cartographer中，前端扫描匹配通过相关性搜索或ICP求解局部最优位姿，后端则通过图优化求解全局一致的位姿和地图。

关键概念与公式

概念	公式	在SLAM中的应用
高斯分布	N(x; μ, Σ) = (2π)^(-n/2) \|Σ\|^(-1/2) exp(-½(x-μ)ᵀΣ⁻¹(x-μ))	传感器噪声建模
贝叶斯法则	p(x\|z) = p(z\|x)p(x) / p(z)	递归状态更新
协方差传播	Σ_y = J·Σ_x·Jᵀ	位姿不确定性传递
马氏距离	d² = (x-μ)ᵀΣ⁻¹(x-μ)	回环检测中的相似度度量

实战场景：栅格地图的概率更新

Cartographer构建的栅格地图中，每个栅格存储的是被占据的概率。设栅格状态为 p(s=1) 表示被占据，p(s=0) 表示空闲。当激光束穿过某栅格时，更新其概率：

odds(s) = p(s) / (1 - p(s))
odds(s|z) = odds(s) · odds(z|s) / odds(z|¬s)

其中 z 为激光测量值。为了避免概率接近0或1导致数值饱和，通常使用对数几率（log-odds）表示：l(s) = log(odds(s))。每次观测后更新 l(s) ← l(s) + l_occ 或 l(s) ← l(s) + l_free。这一更新规则本质上是贝叶斯滤波的迭代形式，保证了地图的收敛性。

常见误区与复盘

误区1：忽略协方差矩阵。将观测噪声视为各向同性（即 Σ = σ²I），导致优化权重错误。激光雷达的测距噪声通常随距离增大而增大，应使用距离相关的协方差模型。
误区2：线性化误差。在强非线性场景下（如大角度旋转），一阶泰勒展开的误差会累积。此时应使用迭代最近点（ICP）或更精确的误差模型。
误区3：回环检测的误匹配。仅凭几何特征判断回环可能产生假阳性，需结合概率阈值（如马氏距离 < 3.0）和时序一致性验证。

风险提示：在低纹理或对称环境中（如长走廊），激光扫描匹配可能陷入局部极小值。此时应融合IMU先验信息，或增加回环检测的频率，避免建图漂移。

3.5 本章总结与工程要点

本章从坐标系定义出发，建立了刚体变换的数学框架，对比了旋转矩阵、四元数等姿态表示方法的优劣，并引入李群与李代数解决优化中的求导问题，最后以概率论为工具统一了状态估计的视角。在Cartographer的实际代码中，这些数学概念贯穿始终：transform 模块处理坐标变换，pose_graph 使用李代数进行优化，submap 通过概率更新维护栅格地图。掌握这些基础，是深入理解SLAM算法原理、排查建图异常（如地图漂移、重影）的关键能力。