第4章 坐标系与运动学基础：关节坐标系与笛卡尔坐标系、正运动学与逆运动学、齐次变换矩阵
各位工程师朋友，欢迎来到第4章。说实话，坐标系和运动学是运动控制的「地基」。地基没打牢，后面路径规划、轨迹插补全得翻车。我见过不少项目，调试时发现机器人走位不对，查到最后——居然是坐标系定义错了。嗯，这种坑，踩一次就记住了。
这一章，我们就把关节坐标系、笛卡尔坐标系、正逆运动学、齐次变换矩阵这几个核心概念彻底讲透。别怕数学，我会用工程实例帮你理解。
4.1 关节坐标系 vs 笛卡尔坐标系
先问个问题：你让机器人「把手伸到桌子右上角」，用的是哪种坐标系？
答案是笛卡尔坐标系。但机器人内部怎么执行？它得把笛卡尔坐标换算成每个关节的角度——这就是关节坐标系。
4.1.1 关节坐标系
关节坐标系，说白了就是机器人各个关节的角度值。比如六轴机器人，关节坐标系就是 [J1, J2, J3, J4, J5, J6] 这六个角度。
- 优点：直接控制电机，没有奇异点问题。
- 缺点：不直观。你没法一眼看出末端在空间什么位置。
4.1.2 笛卡尔坐标系
笛卡尔坐标系就是我们熟悉的 X、Y、Z 轴，再加上姿态的 A、B、C（或 Roll、Pitch、Yaw）。
- 优点：直观，路径规划方便。
- 缺点：存在奇异点，某些位置无法用笛卡尔直线运动到达。
4.2 正运动学：从关节到笛卡尔
正运动学，就是已知每个关节的角度，求末端执行器在笛卡尔空间的位置和姿态。说白了：关节角度 → 末端位姿。
数学上，我们用齐次变换矩阵连乘来实现。以六轴机器人为例：
T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56其中每个 T_ij 是相邻关节间的变换矩阵。最终 T_06 就是末端相对于基座的位姿。
4.2.1 标准DH参数法
实际工程中，我们常用 Denavit-Hartenberg（DH）参数来建立运动学模型。四个参数：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | a | 相邻关节轴线的公垂线长度 |
| 连杆扭角 | α | 相邻关节轴线的夹角 |
| 关节距离 | d | 沿关节轴线的偏移 |
| 关节角度 | θ | 绕关节轴线的旋转角 |
有了这四个参数，每个关节的变换矩阵就能写出来。我习惯在代码里用一个函数封装：
def dh_transform(a, alpha, d, theta):
ct = cos(theta)
st = sin(theta)
ca = cos(alpha)
sa = sin(alpha)
return [
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
]4.3 逆运动学：从笛卡尔到关节
逆运动学正好反过来：已知末端位姿，求关节角度。这是运动控制中最头疼的部分，没有之一。
为什么头疼？因为：
- 多解性：同一个末端位姿，可能有多种关节角度组合。
- 无解：目标位姿超出工作空间。
- 奇异点：某些位姿下，关节速度会突变。
4.3.1 解析法与数值法
求解逆运动学，主要有两种方法：
| 方法 | 原理 | 适用场景 | 优缺点 |
|---|---|---|---|
| 解析法 | 通过几何关系推导出封闭解 | 结构简单的机器人（如6轴有腕部偏置） | 速度快，但推导复杂 |
| 数值法 | 迭代逼近（如牛顿-拉夫森法） | 通用机器人，冗余自由度 | 通用性强，但计算量大 |
4.3.2 多解选择策略
逆运动学有多个解，选哪个？我一般按这个优先级：
- 关节限位优先：先排除超出机械限位的解。
- 最小关节位移：选离当前关节角度最近的解，减少运动冲击。
- 避障需求：如果附近有障碍物，选能让机器人远离障碍物的解。
4.4 齐次变换矩阵
齐次变换矩阵，是连接关节坐标系和笛卡尔坐标系的数学工具。它是一个 4x4 矩阵：
T = [ R P ]
[ 0 1 ]其中：
- R：3x3 旋转矩阵，表示姿态。
- P：3x1 平移向量，表示位置。
- 最后一行：[0 0 0 1]，保持齐次形式。
4.4.1 旋转矩阵的三种基本形式
| 旋转轴 | 旋转矩阵（绕θ角） |
|---|---|
| 绕X轴 | [1, 0, 0; 0, cosθ, -sinθ; 0, sinθ, cosθ] |
| 绕Y轴 | [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ] |
| 绕Z轴 | [cosθ, -sinθ, 0; sinθ, cosθ, 0; 0, 0, 1] |
4.4.2 齐次变换的链式法则
这是运动学计算的核心。你想想看，机器人从基座到末端，中间经过 n 个关节，每个关节都有一个变换矩阵。那么：
T_0n = T_01 * T_12 * ... * T_(n-1)n这个连乘，就是正运动学的数学本质。我习惯在代码里用循环实现：
def forward_kinematics(dh_params):
# dh_params: list of [a, alpha, d, theta]
T = np.eye(4)
for a, alpha, d, theta in dh_params:
T_i = dh_transform(a, alpha, d, theta)
T = T @ T_i
return T4.5 知识体系总览
下面这张图，把本章的核心逻辑串起来了。建议你保存下来，以后做项目时对照着看。
4.6 工程实战要点
最后，分享几个我在项目中总结的实战要点：
- 坐标系标定：每次换工具或换工件，必须重新标定工具坐标系和工件坐标系。我见过有人偷懒不标，结果精度差了2mm。
- 奇异点规避：做笛卡尔路径规划时，一定要在逆运动学求解前做奇异点检测。检测方法很简单——看雅可比矩阵的行列式是否接近0。
- 多解缓存：如果逆运动学计算耗时，可以把最近几次的解缓存起来。下次优先从缓存里找，能省不少时间。
- 数值稳定性：齐次变换矩阵连乘时，注意数值误差累积。我习惯每乘10次矩阵，做一次正交化修正。
好了，这一章的内容就到这里。坐标系和运动学是运动控制的根基，理解透了，后面学路径规划、轨迹插补才会轻松。记住：正运动学是唯一的，逆运动学是多解的，齐次变换矩阵是桥梁。这三个点，贯穿整个运动控制领域。