📚 课程目录第2章数学基础

2. 数学基础

在机器人三维语义地图构建中，所有传感器观测、位姿估计、地图点坐标最终都要落到统一的数学框架里。若把语义地图比作一座建筑，那么三维空间表示、坐标系变换、旋转矩阵、四元数、李群与李代数就是它的钢筋骨架。本节不追求纯数学推导的完备性，而是围绕“工程中如何选、如何算、如何避免踩坑”来梳理这条主线。

一、核心原理：从坐标到变换

三维空间中的点通常用三维向量 p = [x, y, z]^T 表示，它描述的是某个坐标系下的位置。机器人系统中同时存在多个坐标系：世界坐标系 W、机器人本体坐标系 B、相机坐标系 C、激光雷达坐标系 L，以及语义地图中每个物体自身的局部坐标系。一个点在不同坐标系下的坐标不同，但描述的是同一个物理点，连接它们的桥梁就是刚体变换。

刚体变换由旋转和平移组成，齐次形式为：

T = [ R  t ]
    [ 0  1 ]

p_W = T_WB * p_B
其中 R 为 3x3 旋转矩阵，t 为 3x1 平移向量。

旋转矩阵 R 满足正交性 R^T R = I 且 det(R) = 1，它属于特殊正交群 SO(3)。旋转矩阵的优点是可直接与向量相乘、可级联，但存在冗余：9 个参数只有 3 个自由度，且数值迭代后容易失去正交性，需要重新正交化。

二、旋转的三种常用表示与选择

工程中常见的旋转表示有欧拉角、旋转矩阵和四元数。欧拉角直观，但存在万向节死锁，且插值不连续，适合人机交互显示，不适合内部状态传播。旋转矩阵适合坐标变换计算，但存储冗余、优化时需额外约束。四元数用 4 个参数表示旋转，单位四元数满足 ||q|| = 1，无奇异性，插值平滑，是 SLAM 与语义地图后端优化的主流选择。

四元数 q = [w, x, y, z] 与旋转矩阵的转换关系如下：

R = [ 1-2(y^2+z^2)   2(xy-wz)     2(xz+wy)  ]
    [ 2(xy+wz)     1-2(x^2+z^2)   2(yz-wx)  ]
    [ 2(xz-wy)       2(yz+wx)   1-2(x^2+y^2)]

单位四元数乘法可级联旋转：
q_total = q_1 * q_2

在语义地图中，物体位姿常用 position + quaternion 存储，例如 [x, y, z, qw, qx, qy, qz]，这是 ROS TF、PCL、Open3D 等库的通用约定。

三、李群与李代数：为什么优化离不开它

旋转矩阵构成李群 SO(3)，刚体变换构成李群 SE(3)。李群是光滑流形，不能直接像普通向量一样做加减法。位姿优化时，我们希望对旋转求导并迭代更新，但 R + ΔR 不再保证是旋转矩阵。李代数正是解决这个问题的工具：SO(3) 的李代数 so(3) 是三维向量空间，元素为旋转向量 φ = θ * n，其中 θ 为旋转角，n 为单位轴。

指数映射 exp: so(3) -> SO(3) 把旋转向量变成旋转矩阵，对数映射则相反。在优化中，我们通常在当前估计 R 的切空间上做小扰动：

R_new = R * exp(δφ^)
其中 δφ^ 为 δφ 的反对称矩阵。

SE(3) 上的扰动类似：
T_new = T * exp(δξ^)
δξ = [δρ, δφ] 为 6 维扰动向量。

这样，优化变量就变成了无约束的 6 维向量，高斯牛顿、LM 等算法可以直接使用。语义地图中的位姿图优化、BA、物体位姿精修，几乎都建立在这一套“李群上定义误差、李代数上求导更新”的范式上。

四、工程案例：语义地图中的位姿链

假设机器人携带相机在室内建图，检测到一张“椅子”并赋予语义标签。要把它写入地图，需要完成如下坐标链：

相机检测得到椅子在相机坐标系下的位姿 T_Cobj；
通过外参 T_BC 转到本体坐标系：T_Bobj = T_BC * T_Cobj；
通过里程计位姿 T_WB 转到世界坐标系：T_Wobj = T_WB * T_Bobj；
将 T_Wobj 分解为位置与四元数，写入语义地图数据库。

如果后端做位姿图优化，则把 T_WB 作为待优化变量，在 SE(3) 上定义重投影误差或语义观测误差，用李代数扰动迭代更新。这样，语义地图中的物体位置会随着全局位姿优化而自动修正，而不是各自独立漂浮。

五、常见误区与复盘

误区	后果	正确做法
用欧拉角做位姿插值	万向节死锁、轨迹跳变	内部用四元数或李代数，仅显示时转欧拉角
直接对旋转矩阵做加法更新	R 失去正交性，地图扭曲	在切空间扰动：R * exp(δφ^)
四元数未归一化	旋转矩阵尺度错误	每次迭代后执行 q = q / \|\|q\|\|
混淆坐标系变换方向	物体位置镜像或偏移	明确 T_AB 表示“把 B 系点变到 A 系”
忽略外参标定误差	语义标签与点云错位	将外参纳入优化或定期在线标定

在语义地图工程中，最常见的隐蔽错误是“旋转矩阵未正交化”和“四元数符号翻转”。前者会让地图在长时间运行后缓慢扭曲，后者会导致插值路径绕远路。建议在每次后端优化后检查 ||R^T R - I|| 和四元数模长，并在四元数插值时选择最短路径。

六、小结与工程建议

三维空间表示是语义地图的“度量底座”，坐标系变换是“连接器”，旋转矩阵、四元数、李群与李代数则是“运算引擎”。工程上建议：内部状态统一用四元数或 SE(3) 表示，优化时用李代数扰动，显示和交互时再转欧拉角；所有坐标变换明确方向并保留变换链；对旋转矩阵和四元数做数值健康检查。掌握这套数学工具，后续的点云配准、位姿图优化、语义融合才能稳定落地。