3. 坐标系与空间变换：世界坐标系、机器人坐标系、传感器坐标系、齐次坐标与变换矩阵、欧拉角与四元数
各位同学，欢迎来到第三章。
说实话，坐标系和空间变换是机器人学里最基础、也最容易让人栽跟头的地方。我见过太多项目，最后定位飘了、建图歪了，追根溯源，都是坐标系没搞明白。这一章，咱们就把这些概念彻底理清楚。
3.1 为什么需要这么多坐标系？
你想想看，一个机器人身上有激光雷达、有IMU、有轮式里程计，每个传感器都有自己的“视角”。激光雷达看到的是前方2米有障碍物，IMU测到的是自身加速度，轮子告诉你它转了多少圈。这些数据如果不统一到一个“共同语言”下，根本没法用。
这个“共同语言”，就是坐标系。
核心概念：坐标系是描述物体位置和姿态的参考基准。在机器人系统中，我们至少需要三个层次的坐标系：世界坐标系、机器人坐标系、传感器坐标系。
3.2 三大坐标系详解
3.2.1 世界坐标系（World Frame）
世界坐标系是固定不变的，通常记为 {W}。它就像地图上的经纬度，是一个参考。机器人启动时，我们一般把第一帧的位置设为世界坐标系的原点。
我个人习惯用 W 表示世界坐标系，这样在代码里一眼就能认出来。比如 p_W 表示世界坐标系下的点。
3.2.2 机器人坐标系（Robot/Body Frame）
机器人坐标系固定在机器人本体上，通常记为 {B}。原点一般在机器人的几何中心或底盘中心。这个坐标系会随着机器人移动而移动。
举个例子：你让机器人“向前走1米”，这个“前”指的是机器人坐标系下的前，而不是世界坐标系下的前。如果机器人原地转了90度，它的“前”方向就变了。
3.2.3 传感器坐标系（Sensor Frame）
每个传感器都有自己的坐标系，记为 {S}。比如激光雷达的坐标系原点在旋转中心，相机的坐标系原点在光心。
这里有个坑：传感器坐标系和机器人坐标系通常不重合。你需要通过标定，知道传感器相对于机器人本体的位置和朝向。这个关系一旦标错，后面所有数据都是错的。
避坑指南：我曾经在一个项目里，激光雷达的安装角度标反了，结果建出来的地图全是镜像的。排查了整整两天才发现是坐标系变换矩阵写反了。所以，每次做坐标系变换前，先画个图确认一下方向。
3.3 齐次坐标与变换矩阵
好了，坐标系定义清楚了，接下来怎么把一个点从一个坐标系变换到另一个坐标系？
这里就要用到齐次坐标和变换矩阵了。
3.3.1 齐次坐标
简单说，齐次坐标就是在三维坐标 (x, y, z) 后面加一个维度 w，变成 (x, y, z, w)。通常 w=1 表示点，w=0 表示方向向量。
为什么要这么做？因为用齐次坐标，旋转和平移可以写在一个矩阵里，一次乘法搞定。
// 三维点 (x, y, z) 的齐次坐标表示
p = [x, y, z, 1]^T
// 三维方向向量 (dx, dy, dz) 的齐次坐标表示
v = [dx, dy, dz, 0]^T3.3.2 变换矩阵
变换矩阵是一个 4x4 的矩阵，左上角 3x3 是旋转矩阵 R，右上角 3x1 是平移向量 t，最后一行是 [0, 0, 0, 1]。
T = | R t |
| 0 1 |
// 例如：将传感器坐标系下的点 p_S 变换到机器人坐标系下
p_B = T_BS * p_S这里的 T_BS 表示“从传感器坐标系到机器人坐标系的变换”。我习惯把下标读作“从哪到哪”，这样不容易搞混。
个人技巧：写代码时，我习惯把变换矩阵的命名写成 T_B_S 这种格式，一眼就能看出是从 S 到 B 的变换。千万别写成 T_BS 这种模糊的命名，时间长了你自己都看不懂。
3.4 旋转的表达：欧拉角与四元数
旋转矩阵有9个参数，但自由度只有3个。实际工程中，我们更常用欧拉角和四元数来表达旋转。
3.4.1 欧拉角
欧拉角用三个角度来描述旋转：绕 X 轴旋转 roll（横滚角），绕 Y 轴旋转 pitch（俯仰角），绕 Z 轴旋转 yaw（偏航角）。
听起来很直观对吧？但有个大问题：万向锁（Gimbal Lock）。当 pitch 接近 ±90° 时，roll 和 yaw 会变得无法区分，丢失一个自由度。
避坑指南：我在做无人机姿态控制时，就遇到过万向锁。飞机垂直俯冲时，姿态解算突然炸了。从那以后，只要涉及三维空间的全姿态旋转，我坚决用四元数，不用欧拉角。
3.4.2 四元数
四元数用四个参数 (w, x, y, z) 表示旋转，没有万向锁问题，而且插值平滑。它的数学形式是：
q = w + x*i + y*j + z*k
其中 w^2 + x^2 + y^2 + z^2 = 1四元数虽然不直观，但计算稳定、效率高。在 SLAM 和机器人控制中，四元数是事实上的标准。
| 表达方式 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 直观，可直接用于变换 | 冗余，有正交约束 |
| 欧拉角 | 3 | 直观，参数少 | 万向锁，插值困难 |
| 四元数 | 4 | 无万向锁，插值平滑 | 不直观，需归一化 |
3.5 坐标系变换链
实际系统中，数据流经多个坐标系。比如激光雷达的数据，要先从传感器坐标系变换到机器人坐标系，再从机器人坐标系变换到世界坐标系。
这就形成了变换链：
p_W = T_WB * T_BS * p_S其中 T_WB 是机器人到世界的变换，T_BS 是传感器到机器人的变换。
我的习惯：每次写变换链，我都会在纸上画一遍坐标系树。根节点是世界坐标系，子节点是机器人坐标系，再下面是各个传感器坐标系。这样写代码时，变换关系一目了然，不容易出错。
3.6 实践中的坐标系管理
说了这么多理论，咱们看看实际工程中怎么管理坐标系。
- 统一命名规范：我建议团队内统一坐标系命名，比如
world、base_link、laser、camera。ROS 里的 tf 树就是很好的参考。 - 静态变换 vs 动态变换：传感器到机器人的变换通常是静态的（标定一次就不变了），机器人到世界的变换是动态的（随着机器人移动而变化）。
- 可视化验证：每次写完变换代码，我都会用可视化工具（比如 RViz）把坐标系画出来，看看各个坐标系之间的相对关系对不对。
总结一下：坐标系和空间变换是机器人学的基石。搞懂了世界、机器人、传感器三个坐标系的关系，掌握了齐次变换矩阵和四元数，你就能在 SLAM 和运动控制中自由穿梭了。别嫌基础，我见过太多高级算法最后栽在坐标系上。