2. 运动学基础：刚体运动描述、齐次变换矩阵、正运动学与逆运动学简介
各位同学，欢迎来到第二章。说实话，运动学是整个轨迹规划的“地基”。你想想看，如果连机器人怎么动、每个关节转了多少度、末端在哪儿都说不清楚，那后面的路径平滑、速度规划根本无从谈起。
我个人习惯把运动学比作“机器人的语言”。你得先学会这门语言，才能跟机器人“对话”，告诉它怎么走。这一章我们不讲太深的数学推导，重点是把几个核心概念讲透：刚体怎么描述、坐标怎么变换、正解和逆解到底在干什么。
本章核心脉络：刚体在空间中的位置和姿态 → 用齐次变换矩阵统一描述 → 从关节空间到笛卡尔空间（正运动学）→ 从笛卡尔空间回到关节空间（逆运动学）。
2.1 刚体运动描述：位置与姿态
先问个问题：怎么描述一个物体在空间中的状态？
很简单，两个东西：位置和姿态。合起来叫位姿。
- 位置：用三维坐标 (x, y, z) 表示。比如机械臂末端在空间中的某个点。
- 姿态：描述物体“朝向”哪里。比如一个扳手是正着拿还是斜着拿。
我记得刚入行时，有个同事把位置和姿态混为一谈，结果调试时机器人末端位置对了，但姿态反了，直接撞到工件上。嗯，从那以后我再也不敢小看“姿态”这两个字。
描述姿态最常用的工具是旋转矩阵。它是一个 3×3 的正交矩阵，列向量代表刚体坐标系各轴在参考坐标系中的投影。
小提示：旋转矩阵有个重要性质——它的逆等于它的转置。这意味着求反变换时，直接转置就行，省去大量计算。我在写代码时经常利用这个性质来加速。
2.2 齐次变换矩阵：把旋转和平移统一起来
有了位置和姿态，怎么把它们写在一起？
答案就是齐次变换矩阵。它是一个 4×4 的矩阵，长这样：
T = [ R p ]
[ 0 1 ]
其中：
R —— 3×3 旋转矩阵
p —— 3×1 平移向量
0 —— 1×3 零向量
1 —— 标量 1为什么用 4×4？说白了，就是为了把旋转和平移塞进一个矩阵里，方便做连续变换。你想想看，如果不用齐次形式，每次变换都要先旋转再平移，写起来又长又容易出错。
齐次变换矩阵的乘法对应坐标系的连续变换。比如从基坐标系到工具坐标系，中间可能经过好几个关节，每个关节对应一个变换矩阵，把它们乘起来就得到最终结果。
注意：矩阵乘法不满足交换律！先旋转再平移，和先平移再旋转，结果完全不同。我在项目中见过有人搞反顺序，结果机器人末端飞到了完全错误的位置。写代码时一定要确认变换顺序。
2.3 正运动学：从关节到末端
正运动学，说白了就是：给定每个关节的角度，求末端执行器的位姿。
对于串联机械臂，正运动学有标准解法——Denavit-Hartenberg 参数法（简称 DH 参数）。
DH 参数用四个值描述相邻两个关节坐标系的关系：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | a | 沿 x 轴方向，从 zi-1 到 zi 的距离 |
| 连杆扭角 | α | 绕 x 轴，从 zi-1 到 zi 的转角 |
| 关节距离 | d | 沿 zi-1 轴，从 xi-1 到 xi 的距离 |
| 关节转角 | θ | 绕 zi-1 轴，从 xi-1 到 xi 的转角 |
有了 DH 参数，每个关节的变换矩阵就能写出来。把所有关节的变换矩阵连乘，就得到正运动学方程。
举个例子，一个六轴工业机器人，正运动学就是六个变换矩阵相乘：
T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56结果 T_06 就是末端相对于基座的位姿。这个过程是唯一确定的——给定一组关节角，末端位姿只有一个。
正运动学的特点：计算简单、结果唯一、实时性好。在轨迹规划中，我们经常用它来验证逆运动学的结果，或者做前馈计算。
2.4 逆运动学：从末端到关节
逆运动学正好反过来：给定末端位姿，求各关节的角度。
这比正运动学难多了。为什么？
- 多解性：同一个末端位姿，可能对应多组关节角。比如“肘部朝上”和“肘部朝下”都能到达同一个点。
- 存在性：有些位姿根本到不了（超出工作空间）。
- 奇异性：在某些位姿下，关节速度会变得无穷大，机器人会“卡住”。
我记得有一次调试焊接机器人，末端位置明明在工作空间内，但逆解一直报错。查了半天，发现是姿态要求太苛刻，导致关节角超出限位。后来我加了一个“最优解选择”逻辑，才把问题解决。
逆运动学的解法主要有两类：
- 解析法：通过几何或代数推导出封闭解。速度快，但只适用于特定构型（比如后三个关节轴线交于一点的机器人）。
- 数值法：用迭代优化求解（比如牛顿法、雅可比伪逆法）。通用性强，但计算慢，且可能不收敛。
我的建议：如果机器人构型支持解析法，优先用解析法。实在不行再用数值法。在轨迹规划中，逆解通常要实时计算，数值法的性能瓶颈可能会拖后腿。
2.5 正逆运动学在轨迹规划中的角色
讲到这里，你可能想问：这些跟轨迹规划有什么关系？
关系大了。轨迹规划的核心任务，就是生成一条从起点到终点的路径，同时满足速度、加速度、平滑性等约束。而这条路径，既可以在关节空间规划，也可以在笛卡尔空间规划。
- 关节空间规划：直接对每个关节的角度、速度、加速度做规划。优点是简单、无奇异性问题。缺点是无法直接控制末端轨迹的形状。
- 笛卡尔空间规划：对末端位姿做规划，再通过逆运动学映射到关节空间。优点是轨迹形状可控（比如走直线）。缺点是需要频繁调用逆解，计算量大，且可能遇到奇异点。
实际项目中，我一般这样选：
| 场景 | 推荐空间 | 原因 |
|---|---|---|
| 点对点搬运 | 关节空间 | 中间路径无所谓，关节空间更高效 |
| 焊接、涂胶 | 笛卡尔空间 | 末端必须走直线或圆弧 |
| 避障场景 | 笛卡尔空间 | 需要精确控制末端位置避开障碍物 |
避坑指南：我曾经在笛卡尔空间规划时，没做奇异性检测，结果机器人经过奇异点附近时关节速度瞬间飙升，差点把电机烧了。从那以后，我每次做笛卡尔空间规划都会加一个“奇异点预警”模块，一旦接近奇异区域就自动切换到关节空间插值。
2.6 本章小结
这一章我们打下了运动学的基础。记住几个关键点：
- 刚体位姿 = 位置 + 姿态，用齐次变换矩阵统一描述。
- 正运动学：关节角 → 末端位姿，唯一确定，计算简单。
- 逆运动学：末端位姿 → 关节角，存在多解、奇异等问题，需要谨慎处理。
- 轨迹规划可以在关节空间或笛卡尔空间进行，各有优劣，根据场景选择。
这些概念后面会反复用到。尤其是齐次变换矩阵和逆运动学，几乎是每一章都会出现的“老朋友”。建议你多动手推导几个例子，比如两连杆机械臂的正逆解，把数学公式和物理意义对应起来。