数学基础（一）：坐标系与变换

各位同学，欢迎来到第二章。说实话，数学基础这块，很多人一听就头大。我当年刚入行时也一样，觉得搞机器人嘛，写代码就完了，学什么矩阵？直到第一次做多传感器融合，坐标系没对齐，数据全乱套了……嗯，从那以后我老老实实把这块啃完了。

这一章，咱们聊坐标系、旋转矩阵、平移向量、齐次坐标，还有欧拉角和四元数。别怕，我会用我踩过的坑帮你理解。

SLAM里，坐标系就是世界的“基准”。你想想看，机器人说“我前方1米有障碍”，这个“前方”是相对于谁的？是机器人自己，还是地图？

常见的坐标系有几种：

我个人习惯，在代码里用 world、robot、camera 做前缀命名变量。这样一看就懂，不会搞混。

  核心思想： 所有变换，本质就是从一个坐标系到另一个坐标系的映射。

旋转矩阵是个3x3的方阵，它告诉你一个向量在旋转后变成了什么。说白了，就是“把坐标系A下的点，转到坐标系B下”。

举个例子，绕Z轴旋转θ角：

R_z(θ) = [
  [cosθ, -sinθ, 0],
  [sinθ,  cosθ, 0],
  [0,      0,    1]
]

我在项目中遇到过一个问题：两个旋转矩阵相乘，顺序搞反了。结果机器人原地转圈，就是走不对。记住，矩阵乘法不满足交换律，R1 * R2 和 R2 * R1 完全不同。

小技巧： 写代码时，用 Eigen::Matrix3d 或 cv::Rodrigues 来生成旋转矩阵，别手算，容易错。

平移向量就是个3x1的向量，表示从原点出发的位移。比如 t = [1, 0, 0]^T 表示沿X轴移动1米。

旋转加平移，就构成了刚体变换：

p' = R * p + t

这里 p 是原坐标系下的点，p' 是新坐标系下的点。我曾经犯过一个低级错误：把平移向量加在了旋转之前。结果地图全歪了，排查了半天才发现。

注意： 平移向量是在旋转之后的坐标系下定义的。先旋转，再平移，顺序不能乱。

为什么要引入齐次坐标？因为上面的公式 p' = R * p + t 不是线性变换，没法用矩阵乘法一步搞定。齐次坐标就是在三维向量后面加个1，变成四维：

p_hom = [x, y, z, 1]^T

这样，变换矩阵就变成了4x4：

T = [
  [R, t],
  [0, 1]
]

于是 p'_hom = T * p_hom，一步到位。你想想看，多个变换连起来，就是矩阵连乘，多清爽。

  为什么好用？ 因为齐次坐标让变换可复合、可逆、可插值。SLAM后端优化里，全是4x4矩阵在跑。

欧拉角用三个角度表示旋转：偏航（Yaw）、俯仰（Pitch）、横滚（Roll）。听起来很直观，对吧？

但坑来了：万向锁。当俯仰角接近±90°时，偏航和横滚会耦合，丢失一个自由度。我记得有一次做无人机仿真，飞控突然乱转，查了半天发现是欧拉角插值导致的万向锁。

另外，欧拉角的定义顺序也不统一。有ZYX、ZYZ、XYZ……不同系统可能用不同约定。所以，我个人建议：只在人机交互时用欧拉角，内部计算一律用旋转矩阵或四元数。

四元数是个超复数，形式为 q = w + xi + yj + zk，其中 w 是实部，x, y, z 是虚部。它用4个参数表示旋转，没有万向锁，而且插值平滑。

单位四元数表示旋转：

q = [cos(θ/2), sin(θ/2) * u]

其中 u 是旋转轴的单位向量，θ 是旋转角。

我在做视觉惯性里程计（VIO）时，状态估计全用四元数。为什么？因为EKF里要对旋转做线性化，四元数的雅可比比欧拉角好算多了。

避坑指南： 我曾经把四元数的顺序搞反了。Eigen里是 [w, x, y, z]，但有些库是 [x, y, z, w]。一定要确认清楚。

下面这张图，是我自己梳理的本章知识结构。你可以把它当作学习地图：

嗯，这一章的内容就这些。数学基础是SLAM的骨架，别急着跳过。我当年就是在这里多花了点时间，后面写代码才顺风顺水。

  一句话总结： 坐标系是世界观，旋转矩阵是朝向，平移向量是位置，齐次坐标是统一框架，欧拉角是直观但危险，四元数是工程首选。

可可精品资料库