运动学基础：刚体运动描述

做轨迹规划，说白了就是让机器人知道「怎么动」。但动之前，你得先学会「描述运动」。这就像学开车前得先看懂仪表盘——旋转矩阵、欧拉角、四元数这些，就是机器人世界的仪表盘。

我个人习惯把刚体运动拆成两件事：位置和姿态。位置好办，三维坐标 (x, y, z) 就搞定了。姿态才是真正让人头疼的地方。嗯，咱们一个一个来啃。

旋转矩阵：最直观，但也最啰嗦

旋转矩阵是个 3×3 的矩阵，每一列代表旋转后的坐标轴在原坐标系下的投影。你想想看，一个物体绕 X 轴转 30°，它的 Y 轴和 Z 轴都会跟着变——旋转矩阵就是记录这个「怎么变」的。

我在项目中遇到过一个问题：用旋转矩阵做连续旋转，矩阵会越乘越复杂，而且很容易出现数值误差累积。说白了，旋转矩阵有 9 个元素，但真正的自由度只有 3 个——冗余信息太多了。

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

欧拉角：直观，但有坑

欧拉角用三个角度来描述姿态，比如 (roll, pitch, yaw) 或者 (α, β, γ)。每个角度对应绕一个坐标轴的旋转。这玩意儿人类看着舒服，但机器人用着容易出问题。

最经典的坑是什么？万向锁。我曾经调试一个六轴机械臂，用欧拉角做插值，结果在 pitch 接近 90° 时，roll 和 yaw 突然「锁死」了——两个轴变得无法独立控制。当时我盯着示教器看了十分钟才反应过来：万向锁来了。

四元数：计算友好，理解不友好

四元数是个四维向量 q = (w, x, y, z)，其中 w 是实部，(x, y, z) 是虚部。它没有万向锁，插值平滑，计算效率高——说白了，这是机器人内部最常用的姿态表示法。

我第一次接触四元数时，觉得这东西反人类。后来我找到个记忆方法：四元数本质上是一个「旋转轴 + 旋转角度」的打包形式。轴是单位向量 (x, y, z)，角度 θ 藏在 w = cos(θ/2) 里。

// 四元数乘法（表示两个旋转的复合）
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)

// 单位四元数（表示无旋转）
q_identity = (1, 0, 0, 0)

齐次变换矩阵：把位置和姿态打包

齐次变换矩阵是个 4×4 的矩阵，左上角 3×3 是旋转矩阵，右上角 3×1 是位置向量，最后一行是 [0, 0, 0, 1]。它把「旋转 + 平移」合并成一个矩阵乘法，方便做坐标变换的链式计算。

我习惯用齐次变换矩阵做「坐标系链」——比如从基座到末端，中间经过好几个关节，每个关节一个变换矩阵，乘起来就是末端位姿。这玩意儿在机器人运动学里是基本功。

// 齐次变换矩阵结构
T = [
  [R_11, R_12, R_13, p_x],
  [R_21, R_22, R_23, p_y],
  [R_31, R_32, R_33, p_z],
  [0,    0,    0,    1  ]
]

四种表示法的对比

知识体系总览

下面这张图把四种表示法的关系串起来了。你想想看，从「刚体运动」这个根节点出发，分叉出「位置」和「姿态」——姿态又分出三条路，最后齐次变换矩阵把它们合在一起。

实际工程中的选择策略

说了这么多，到底该用哪个？我总结了一套自己的选择逻辑：

1. 跟人打交道 → 用欧拉角。示教器、调试界面、日志输出，人类只看角度。
2. 内部计算 → 用四元数。插值、滤波、融合，四元数最稳。
3. 坐标变换链 → 用齐次变换矩阵。从基座到末端，一路乘过去。
4. 需要求逆或微分 → 用旋转矩阵。雅可比矩阵、动力学计算，旋转矩阵的导数好求。

好了，运动学基础就聊到这儿。记住一句话：没有最好的表示法，只有最合适的场景。你想想看，你现在的项目里用的是哪种？有没有踩过万向锁的坑？