3. 坐标系与位姿表示：世界坐标系、机器人坐标系、齐次变换矩阵、旋转矩阵与四元数

各位同学，咱们今天聊点硬核的。坐标系和位姿表示，说白了就是回答三个问题：我在哪？我要去哪？我怎么去？

我刚开始做扫地机器人那会儿，觉得这玩意儿不就是个数学坐标嘛，有啥好讲的？结果第一次做路径规划，机器人直接撞墙了。嗯，后来才发现，坐标系没搞对，位姿算错了。今天咱们就把这块彻底掰扯清楚。

3.1 世界坐标系与机器人坐标系

先说说这两个坐标系。世界坐标系，你可以理解成房间的固定地图。比如你家客厅，以墙角为原点，东为X轴，北为Y轴，天花板方向为Z轴。这个坐标系是固定不变的，所有地图上的特征点、障碍物，都用这个坐标系来描述。

机器人坐标系呢？它是跟着机器人走的。原点在机器人中心，X轴朝前，Y轴朝左，Z轴朝上。你想想看，机器人身上的传感器，比如激光雷达、里程计，它们测量的数据都是相对于机器人自身的。

我在项目中遇到过一个问题：激光雷达扫描到前方0.5米有障碍物，但机器人实际位置已经移动了。如果不做坐标系转换，直接拿这个数据去更新地图，地图就全乱了。所以，搞清楚数据在哪个坐标系下，是第一步。

3.2 齐次变换矩阵

好，现在问题来了：怎么把一个坐标系下的点，转换到另一个坐标系下？

最简单的做法是：先旋转，再平移。但如果你分开做，公式写起来很麻烦。齐次变换矩阵就是来解决这个问题的。

它长这样：

| R   t |
| 0   1 |

其中R是3x3的旋转矩阵，t是3x1的平移向量。最后一行是[0 0 0 1]。这个矩阵可以把一个点的坐标从机器人坐标系转换到世界坐标系。

举个例子：

// 假设机器人坐标系下有一个点 (0.5, 0.3, 0)
// 机器人当前在世界坐标系下的位置是 (1.0, 2.0, 0)
// 机器人朝向是 30度

// 齐次变换矩阵 T 可以这样构建：
// 先构建旋转矩阵 R (绕Z轴旋转30度)
// 再构建平移向量 t = [1.0, 2.0, 0]

// 世界坐标系下的点 = T * 机器人坐标系下的点

我个人习惯用齐次变换矩阵，因为它统一了旋转和平移，而且可以连续变换。比如机器人先移动再旋转，只需要把多个变换矩阵乘起来就行。

3.3 旋转矩阵

旋转矩阵，说白了就是描述怎么转。3D空间中的旋转，可以用3x3的矩阵表示。

绕X轴旋转：

| 1    0       0   |
| 0  cosθ  -sinθ   |
| 0  sinθ   cosθ   |

绕Y轴旋转：

| cosθ   0   sinθ   |
| 0      1    0     |
| -sinθ  0   cosθ   |

绕Z轴旋转：

| cosθ  -sinθ   0   |
| sinθ   cosθ   0   |
| 0      0      1   |

我记得有一次做多传感器融合，IMU给的姿态是欧拉角，激光雷达给的姿态是旋转矩阵。两者对不上，折腾了半天。后来发现，欧拉角有万向锁问题，而旋转矩阵没有。从那以后，我内部计算一律用旋转矩阵，只在输入输出时用欧拉角。

3.4 四元数

四元数，听起来很玄乎，其实它就是旋转的另一种表示方式。一个四元数 q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。

为什么要有四元数？因为旋转矩阵有9个参数，但实际自由度只有3个，存在冗余。而且旋转矩阵的乘法计算量大。四元数只有4个参数，计算快，而且没有万向锁问题。

四元数转旋转矩阵的公式：

R = | 1-2y²-2z²   2xy-2zw     2xz+2yw   |
    | 2xy+2zw     1-2x²-2z²   2yz-2xw   |
    | 2xz-2yw     2yz+2xw     1-2x²-2y²  |

看着复杂？别怕。实际开发中，我们直接用库函数。比如Eigen库：

Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0);  // 单位四元数
Eigen::Matrix3d R = q.toRotationMatrix();

我个人更偏爱四元数。为什么？因为插值平滑。做路径规划时，机器人需要从姿态A平滑过渡到姿态B。用四元数的球面线性插值，轨迹非常平滑。用欧拉角插值？嗯，你会看到机器人像抽风一样乱转。

最后说一句：这些概念不是背出来的，是用出来的。你写几行代码，让机器人转几圈，自然就理解了。下次咱们讲传感器模型与观测方程，到时候你会更深刻地体会到坐标系的重要性。