坐标系与位姿表示：世界坐标系、机器人坐标系、齐次变换矩阵、欧拉角与四元数

做轨迹跟踪这些年，我踩过最大的坑，就是坐标系搞混了。

你想想看，传感器给的数据是相机坐标系下的，底盘控制要的是机器人坐标系下的，路径规划又是在世界坐标系里算的。这三个坐标系但凡有一个转换错了，机器人就会像喝醉了一样乱跑。我刚开始做AGV的时候，就因为把机器人坐标系下的速度直接发给电机，结果车子原地转圈，差点撞墙。

所以这一章，咱们把坐标系和位姿表示彻底聊透。

2.1 三个核心坐标系

轨迹跟踪里，你只需要记住三个坐标系：世界坐标系、机器人坐标系、传感器坐标系。其他的都是它们的衍生。

坐标系	符号	原点	用途
世界坐标系	{W}	固定在地面某点	全局路径规划、地图构建
机器人坐标系	{R}	机器人中心	运动控制、速度指令
传感器坐标系	{S}	传感器安装位置	激光雷达、相机数据

世界坐标系是固定的，一般选在机器人启动位置或者地图的某个角落。机器人坐标系跟着机器人走，x轴朝前，y轴朝左，z轴朝上——这是ROS里的标准约定。传感器坐标系嘛，看你怎么装，装在车头还是车尾，角度偏了多少，都得标定。

2.2 位姿的数学表示

位姿就是位置+姿态。位置好说，三个坐标(x, y, z)。姿态就麻烦了，怎么表示旋转？

常用的有四种：旋转矩阵、欧拉角、轴角、四元数。咱们一个一个说。

2.2.1 旋转矩阵

旋转矩阵是个3x3的正交矩阵，行列式为1。它能把一个向量从一个坐标系转到另一个坐标系。

// 绕Z轴旋转θ角的旋转矩阵
R_z(θ) = [
  [cosθ, -sinθ, 0],
  [sinθ,  cosθ, 0],
  [0,      0,    1]
]

旋转矩阵的好处是直观，坏处是参数太多——9个数，但自由度只有3。而且连续旋转时，矩阵乘法容易累积误差。我在项目里很少直接用旋转矩阵做运算，太占CPU了。

2.2.2 欧拉角

欧拉角用三个角度表示旋转：滚转(Roll)、俯仰(Pitch)、偏航(Yaw)。

名称	轴	范围	说明
Roll (φ)	X轴	-π ~ π	绕前进方向旋转，像飞机侧倾
Pitch (θ)	Y轴	-π/2 ~ π/2	绕横向轴旋转，像飞机抬头低头
Yaw (ψ)	Z轴	-π ~ π	绕垂直轴旋转，像原地转向

2.2.3 四元数

四元数是个超复数，形式是 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。它没有万向锁，插值平滑，计算效率高。

说白了，四元数就是用四个数表示三维旋转。单位四元数满足 w² + x² + y² + z² = 1。

// 绕单位向量 (nx, ny, nz) 旋转 θ 角的四元数
q = [cos(θ/2), nx*sin(θ/2), ny*sin(θ/2), nz*sin(θ/2)]

// 四元数乘法（表示连续旋转）
q_result = q2 * q1  // 先转q1，再转q2

// 用四元数旋转向量 v
v_rotated = q * v * q_conjugate

2.3 齐次变换矩阵

齐次变换矩阵是个4x4的矩阵，把旋转和平移打包在一起。形式如下：

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

// R: 3x3 旋转矩阵
// t: 3x1 平移向量
// 最后一行是 [0, 0, 0, 1]

为什么用齐次？因为可以用一个矩阵同时表示旋转和平移，而且可以连续乘。比如从传感器坐标系到机器人坐标系，再到世界坐标系：

T_W_R = T_W_R * T_R_S  // 注意乘法顺序

这里有个坑：左乘和右乘不一样。左乘是相对于固定坐标系变换，右乘是相对于当前坐标系变换。我刚开始做的时候搞反了，结果机器人路径全乱套了。

2.4 坐标系变换实战

咱们看一个实际例子。假设激光雷达装在机器人前方0.5米处，偏右0.1米，俯仰角5°。激光雷达检测到一个障碍物，在雷达坐标系下的坐标是 (2.0, 0.3, 0.0)。问：这个障碍物在世界坐标系下的位置？

步骤很简单：

1. 先算传感器到机器人的变换 T_R_S
2. 再算机器人到世界的变换 T_W_R（由定位模块给出）
3. 最后 T_W_S = T_W_R * T_R_S
4. 障碍物在世界坐标系下的坐标 = T_W_S * p_S

// 伪代码示例
// 传感器到机器人：平移(0.5, 0.1, 0.0)，绕Y轴旋转5°
T_R_S = translation(0.5, 0.1, 0.0) * rotation_y(5°)

// 机器人到世界：由定位模块给出
T_W_R = localization.get_pose()

// 合成变换
T_W_S = T_W_R * T_R_S

// 障碍物在雷达坐标系下
p_S = [2.0, 0.3, 0.0, 1.0]  // 齐次坐标

// 转换到世界坐标系
p_W = T_W_S * p_S

2.5 知识体系总览

下面这张图把本章的核心逻辑串起来了。你对照着看，应该能一目了然。

嗯，这张图把咱们这章的核心逻辑都串起来了。从三个坐标系出发，到四种位姿表示方法，再到齐次变换矩阵，最后落到实际应用。你写代码的时候，就照着这个链路走，不会乱。

2.6 本章小结

• 三个坐标系：世界、机器人、传感器。所有数据必须统一坐标系。
• 位姿表示：旋转矩阵（冗余）、欧拉角（有万向锁）、四元数（推荐）。
• 齐次变换矩阵：4x4矩阵，旋转+平移打包，支持连续变换。
• 实战要点：注意乘法顺序（左乘vs右乘），验证变换方向，用已知点调试。