数学基础（一）：空间描述与变换

各位同学好，我是你们的轨迹规划讲师。今天咱们聊聊数学基础——说白了，就是怎么在三维空间里描述一个物体在哪儿、朝哪儿看。

我记得刚入行那会儿，总觉得数学是纸上谈兵。直到第一次调机械臂，明明算好了角度，结果末端执行器直接怼到工件上去了。嗯，从那以后，我再也不敢小看这些矩阵和四元数了。

位置与姿态：描述一个刚体的“身份证”

想象一下，你手里拿着一个手机。它在空间中的状态，其实就两件事：位置（在哪儿）和姿态（朝哪儿）。

• 位置：用三维坐标 (x, y, z) 表示，相对于某个参考坐标系。
• 姿态：描述物体坐标轴相对于参考坐标系的指向。

位置好理解，姿态才是坑。我见过不少新手，把姿态和旋转混为一谈，结果程序跑起来，机器人转得跟陀螺似的。

旋转矩阵：最直观，但最“贵”

旋转矩阵 R 是一个 3×3 的正交矩阵，行列式为 +1。它的每一列，就是物体坐标系各轴在参考系中的投影。

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

旋转矩阵的好处是直观，组合旋转就是矩阵乘法。但坏处也很明显——9 个元素，只有 3 个自由度，冗余严重。而且连续旋转时，矩阵乘法计算量大，还容易累积数值误差。

齐次变换矩阵：把旋转和平移打包

实际工程中，我们经常需要同时处理旋转和平移。齐次变换矩阵 T 就是干这个的——它是一个 4×4 的矩阵，把旋转矩阵 R 和平移向量 t 塞到一起。

T = [ R   t ]
    [ 0ᵀ  1 ]

// 例如：先绕 Z 轴转 30°，再沿 X 轴移动 5 个单位
T = [
  [0.866, -0.5, 0, 5],
  [0.5,   0.866, 0, 0],
  [0,     0,    1, 0],
  [0,     0,    0, 1]
]

为什么用齐次？因为你可以把多次变换连乘成一个矩阵，一次算完。这在轨迹规划里太重要了——你想想看，机械臂从基座到末端，中间经过好几个关节，每个关节都有自己的变换。用齐次矩阵，一次乘法就能算出末端位姿。

欧拉角：直观但“有毒”

欧拉角用三个角度（比如滚转、俯仰、偏航）来描述姿态。听起来很简单，对吧？

但问题来了：

• 顺序依赖：XYZ、ZYX、ZYZ……不同顺序，结果完全不同。
• 万向锁：当中间角接近 ±90° 时，会丢失一个自由度。我在做无人机仿真时就遇到过——飞机垂直俯冲时，偏航和滚转突然分不清了，控制直接炸掉。

表示方法	优点	缺点
旋转矩阵	数值稳定，无奇异性	冗余参数，计算量大
欧拉角	直观，参数少	万向锁，顺序依赖
四元数	无奇异性，插值平滑	不够直观，需归一化

四元数：轨迹规划的“瑞士军刀”

四元数 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部，且满足 w² + x² + y² + z² = 1。

为什么轨迹规划偏爱四元数？

• 无万向锁：随便你怎么转，不会丢自由度。
• 平滑插值：球面线性插值（SLERP）可以生成平滑的旋转过渡。
• 计算高效：比旋转矩阵少一半的乘法。

// 四元数乘法（表示旋转组合）
q_result = q1 * q2

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

// SLERP 插值
q(t) = (q1 * sin((1-t)Ω) + q2 * sin(tΩ)) / sin(Ω)

知识体系总览

下面这张图，是我自己总结的空间描述与变换的知识结构。你可以把它当作本章的“地图”。

实际工程中的选择策略

说了这么多，到底该用哪个？我根据自己的项目经验，总结了一个简单的选择原则：

1. 做底层驱动或实时控制：用旋转矩阵。数值稳定性是第一位的。
2. 做轨迹规划或动画插值：用四元数。SLERP 平滑，无奇异性。
3. 做用户界面或调试显示：用欧拉角。人看得懂。
4. 做多坐标系变换链：用齐次变换矩阵。一次乘法搞定所有变换。