2. 运动学基础（上）：刚体运动描述、旋转矩阵、欧拉角、齐次变换矩阵

各位，咱们开始聊运动学。说实话，刚接触机器人控制的时候，我总觉得运动学就是一堆矩阵乘法，枯燥得很。直到第一次调试机械臂，发现末端位置死活对不上——嗯，那时候我才意识到，坐标系没搞明白，后面全是白搭。

这一章，咱们把刚体运动描述这件事彻底讲透。你想想看，人形机器人的每个关节、每根连杆，本质上都是一个刚体。你要控制它，首先得知道它“在哪儿”、“朝哪儿”。这就是运动学要解决的核心问题。

2.1 刚体运动：位置 + 姿态

一个刚体在三维空间里的状态，说白了就是两件事：位置和姿态。位置好办，三个坐标 (x, y, z) 就搞定了。但姿态呢？你得告诉机器人，它的“脑袋”是朝前还是朝左，是仰头还是低头。

我习惯把姿态理解成：刚体固连坐标系相对于世界坐标系的指向关系。比如，人形机器人的脚掌，它有一个自己的坐标系——脚尖方向是 X 轴，脚掌平面法线是 Z 轴。你要描述这个脚掌的姿态，就是描述这个坐标系相对于世界坐标系的旋转关系。

2.2 旋转矩阵：姿态的“身份证”

旋转矩阵 R 是一个 3×3 的正交矩阵，行列式为 +1。它属于特殊正交群 SO(3)。

说白了，旋转矩阵的每一列，就是刚体固连坐标系的基向量在世界坐标系下的投影。比如，R 的第一列是固连坐标系 X 轴在世界坐标系下的方向余弦。

我记得刚学的时候，总觉得旋转矩阵有 9 个元素，太冗余了。但后来发现，它的好处是没有奇异性，而且组合旋转就是矩阵乘法，非常直观。

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

// 绕 X 轴旋转 α 角
R_x(α) = [
  [1,    0,      0   ],
  [0,  cosα,  -sinα ],
  [0,  sinα,   cosα ]
]

// 绕 Y 轴旋转 β 角
R_y(β) = [
  [ cosβ,  0,  sinβ ],
  [ 0,     1,   0   ],
  [-sinβ,  0,  cosβ ]
]

2.3 欧拉角：更直观，但有代价

旋转矩阵虽然好用，但 9 个参数太冗余了。实际调试的时候，你更想直接说“绕 X 轴转 30 度，再绕 Y 轴转 45 度”。这就是欧拉角。

欧拉角用三个角度来描述姿态。最常见的两种是：

• ZYX 欧拉角（绕动轴）： 先绕 Z 轴转 yaw（偏航），再绕新的 Y 轴转 pitch（俯仰），最后绕新的 X 轴转 roll（滚转）。这是人形机器人中最常用的，因为 yaw/pitch/roll 物理意义明确。
• ZYZ 欧拉角： 常用于机械臂运动学分析，数学上更简洁。

欧拉角最大的问题是什么？万向锁（Gimbal Lock）。当 pitch = ±90° 时，yaw 和 roll 的旋转轴会重合，丢失一个自由度。我在做人形机器人腰部控制时就遇到过——当躯干前倾到 90 度时，偏航和滚转突然无法独立控制了。

2.4 齐次变换矩阵：把旋转和平移统一起来

位置用向量，姿态用矩阵。但实际中，你经常需要同时变换位置和姿态。比如，从肩关节坐标系转换到肘关节坐标系，既要旋转，又要平移。

齐次变换矩阵 T 就是干这个的。它是一个 4×4 矩阵：

T = [ R   p ]
    [ 0   1 ]

其中：
R ∈ SO(3) —— 旋转部分
p ∈ ℝ³   —— 平移部分
最后一行是 [0, 0, 0, 1]

齐次变换矩阵属于特殊欧氏群 SE(3)。它的好处是：一次矩阵乘法，同时完成旋转和平移。而且多个变换可以连乘：T_13 = T_12 * T_23。

我记得第一次写正运动学代码时，就是用齐次变换矩阵把每个关节的变换乘起来，最后得到末端位姿。代码非常简洁，而且不容易出错。

// 示例：从基座到末端的正运动学
// 假设有 3 个关节，每个关节的齐次变换矩阵为 T1, T2, T3
// 末端位姿 T_end = T1 * T2 * T3

Eigen::Matrix4d T1 = get_joint_transform(joint1_angle);
Eigen::Matrix4d T2 = get_joint_transform(joint2_angle);
Eigen::Matrix4d T3 = get_joint_transform(joint3_angle);

Eigen::Matrix4d T_end = T1 * T2 * T3;

// 提取位置和姿态
Eigen::Vector3d position = T_end.block<3,1>(0,3);
Eigen::Matrix3d rotation = T_end.block<3,3>(0,0);

2.5 知识体系总览

下面这张图，把本章的核心概念串起来了。你可以把它当作一个思维导图，每次写运动学代码前扫一眼，思路会清晰很多。

2.6 实战中的选择建议

说了这么多理论，最后给点实在的建议。你在写人形机器人控制代码时，到底该用哪种表示？

• 运动学计算（正解、逆解）： 用齐次变换矩阵。4×4 矩阵乘法，Eigen 库一行搞定，不容易出错。
• 姿态插值（比如步态规划）： 用四元数。SLERP 插值平滑，没有万向锁问题。
• 用户界面显示、手动示教： 用欧拉角（ZYX）。操作员说“绕 Z 轴转 10 度”比说“乘以一个旋转矩阵”直观得多。
• 状态估计（IMU 融合）： 用四元数或旋转矩阵。欧拉角在滤波里容易出问题。