数学基础（一）：空间描述与变换
做轨迹规划，说白了就是让机器人知道「自己在哪里，要去哪里，怎么过去」。而这一切的起点，就是空间描述与变换。我刚开始接触这个领域时，总觉得这些数学工具太抽象，直到第一次在项目里因为姿态表示搞混，导致机械臂差点撞到夹具……嗯，从那以后我再也不敢轻视这部分基础了。
1. 位置与姿态表示
一个刚体在三维空间中的状态，可以拆成两部分：位置和姿态。位置好理解，就是物体中心点的坐标 (x, y, z)。但姿态就麻烦一些——它描述的是物体「朝哪边看、怎么转」。
我个人习惯把姿态想象成：在物体上绑一个坐标系，看这个坐标系相对于世界坐标系是怎么旋转的。这个旋转关系，就是我们接下来要聊的核心。
2. 旋转矩阵
旋转矩阵是描述姿态最直接的方式。它是一个 3×3 的正交矩阵，行列式为 +1。每一列代表物体坐标系的一个轴在世界坐标系下的方向向量。
举个例子，绕 Z 轴旋转 θ 角的旋转矩阵长这样：
R_z(θ) = [
[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]
]我记得第一次手写旋转矩阵时，总是把 sin 的符号搞反。后来我养成一个习惯：用右手定则验证——拇指指向旋转轴正方向，四指弯曲方向就是正角度方向。
3. 欧拉角
旋转矩阵虽然严谨，但 9 个参数太冗余了。欧拉角用 3 个角度来描述旋转，更直观。常用的有 ZYX 欧拉角（也叫滚转-俯仰-偏航角）。
但欧拉角有个大坑——万向锁。当中间轴旋转到 ±90° 时，会丢失一个自由度。我在做无人机轨迹规划时就踩过这个坑：用欧拉角做插值，结果在俯仰 90° 附近姿态突然跳变，飞机差点翻过去。
| 表示方法 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 无奇异，易组合 | 冗余，不直观 |
| 欧拉角 | 3 | 直观，易理解 | 有万向锁，插值困难 |
| 四元数 | 4 | 无奇异，易插值 | 不够直观 |
4. 四元数
四元数可以看作是复数的推广，形式为 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。单位四元数可以用来表示旋转。
为什么用四元数？因为它没有万向锁，而且做球面线性插值（SLERP）非常平滑。我在做机械臂轨迹规划时，所有姿态插值都用四元数，效果很稳定。
// 四元数乘法（组合旋转）
q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2)5. 齐次变换矩阵
齐次变换矩阵把旋转和平移统一到一个 4×4 的矩阵里。形式如下：
T = [
[R, t],
[0, 1]
]其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。最后一行是 [0, 0, 0, 1]。
有了齐次变换矩阵，我们可以把多个变换串联起来：T_13 = T_12 * T_23。这在机器人运动学里太常用了——从基座到末端执行器，就是一连串的齐次变换矩阵相乘。
知识体系总览
下面这张图把本章的核心概念串起来了。你可以看到，位置和姿态是基础，然后衍生出三种姿态表示方法，最后统一到齐次变换矩阵。
总结与建议
这一章的内容是后续所有轨迹规划算法的数学基石。我个人建议你：
- 动手推导一遍旋转矩阵的公式，尤其是绕不同轴的旋转矩阵
- 写代码验证欧拉角转四元数、四元数转旋转矩阵的转换
- 理解齐次变换矩阵的物理意义，而不只是把它当成数学工具
这些基础打牢了，后面学运动学、动力学、轨迹插值就会顺畅很多。我在带新人时发现，凡是后面学得快的，都是这部分基础扎实的。