2. 运动学基础：刚体运动描述、旋转矩阵、四元数、齐次变换矩阵、正逆运动学基础

各位同学，欢迎来到第二章。说实话，运动学基础这块内容，很多搞算法的朋友觉得“太简单了，不就是几个矩阵嘛”。但我在项目里踩过的坑告诉我——运动学没搞透，后面轨迹规划全是空中楼阁。你想想看，机器人连自己胳膊在哪儿都不知道，还谈什么规划路径？

这一章，我会把刚体运动描述、旋转矩阵、四元数、齐次变换、正逆运动学这些核心概念串起来。不讲纯数学推导，重点讲工程直觉和避坑经验。

2.1 刚体运动描述：位置 + 姿态

刚体在三维空间里有几个自由度？六个。三个平移，三个旋转。说白了，你要描述一个刚体，就得告诉别人它在哪儿（位置），以及它脸朝哪边（姿态）。

位置好办，一个三维向量就搞定。但姿态就有点意思了——同一个姿态，你可以用旋转矩阵、四元数、欧拉角三种方式表达。每种方式都有自己的脾气。

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

旋转矩阵是一个 3×3 的正交矩阵，行列式为 +1。它把向量从一个坐标系映射到另一个坐标系。

举个例子，绕 Z 轴转 θ 角的旋转矩阵长这样：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

旋转矩阵的好处是直观，你一眼就能看出旋转轴和角度。但坏处也很明显——9 个元素，冗余参数多，而且必须满足正交约束。你想想看，在优化问题里用旋转矩阵做变量，每次迭代完还得把它投影回 SO(3) 流形上，多麻烦。

2.3 四元数：工程界的宠儿

四元数，说白了就是“带约束的复数在三维空间的推广”。一个四元数 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部，且满足 w² + x² + y² + z² = 1。

为什么四元数在机器人领域这么流行？三个原因：

• 无万向锁——这是欧拉角永远解决不了的痛
• 插值平滑——SLERP 插值可以生成平滑的旋转过渡
• 紧凑——4 个参数，比旋转矩阵少 5 个

四元数乘法表示旋转复合：

q_result = q2 * q1  // 先应用 q1，再应用 q2

注意顺序！四元数乘法不满足交换律。我刚开始用的时候经常搞反，结果机器人转得乱七八糟。

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

齐次变换矩阵 T 是一个 4×4 的矩阵：

T = [R   p]
    [0   1]

其中 R 是 3×3 旋转矩阵，p 是 3×1 平移向量。它把一个点从坐标系 {B} 变换到坐标系 {A}：

ᵃP = ᵃT_b * ᵇP

齐次变换的好处是链式法则——你可以把多个变换串起来：

ᵃT_c = ᵃT_b * ᵇT_c

这玩意儿在机器人运动学里太重要了。你想想看，从基座到末端执行器，中间经过 n 个关节，每个关节都有一个变换矩阵，乘起来就是正运动学。

表示方法	参数数量	优点	缺点
旋转矩阵	9	直观，运算方便	冗余，需正交约束
四元数	4	无万向锁，插值平滑	不够直观
欧拉角	3	最直观	万向锁，插值困难
齐次变换矩阵	16	统一表示旋转+平移	参数多，计算量大

2.5 正运动学：从关节空间到任务空间

正运动学，说白了就是给定关节角度，求末端位姿。对于串联机器人，就是连乘齐次变换矩阵。

以六轴工业机器人为例：

ᵇT_e = ᵇT_1 * ¹T_2 * ²T_3 * ³T_4 * ⁴T_5 * ⁵T_e

每个 ᵢT_{i+1} 由 DH 参数决定。DH 参数有四个：a、α、d、θ。其中 θ 是关节变量，其他三个是固定参数。

2.6 逆运动学：从任务空间到关节空间

逆运动学是正运动学的逆过程——给定末端位姿，求关节角度。这玩意儿比正运动学难多了，原因有三：

• 多解性——同一个末端位姿，可能有多个关节构型
• 奇异性——某些位姿下，雅可比矩阵不可逆
• 无解——给定的位姿可能超出工作空间

求解方法分两类：

1. 解析法——针对特定构型推导封闭解。比如 PUMA 560 有 8 组解。速度快，但通用性差。
2. 数值法——基于雅可比矩阵迭代求解。通用性强，但速度慢，且需要初值。

2.7 工程实践中的坐标系管理

最后聊点实际的。在轨迹规划工程里，你至少需要管理以下几个坐标系：

• 世界坐标系——全局参考
• 基座坐标系——机器人底座
• 工具坐标系——末端执行器
• 工件坐标系——被加工物体
• 相机坐标系——视觉传感器

我个人的习惯是：所有轨迹点统一在世界坐标系下规划，最后再通过齐次变换转到关节空间。这样如果工件位置变了，只需要更新工件坐标系到世界坐标系的变换，轨迹代码不用改。