机械臂运动学原理入门：正逆解、D-H 参数、轨迹规划基础
运动学听起来吓人，其实就是回答两个问题：关节转了多少度，末端到了哪儿？反过来，末端想去哪儿，关节该转多少度？
一、运动学到底是什么
机械臂的**运动学（Kinematics）**研究的是"运动"本身，不涉及"力"。它只关心两件事：
- 关节角度和末端位置之间的几何关系
- 这个关系怎么算
和运动学对应的叫动力学（Dynamics），那才是研究"力、质量、加速度"的。新手先别碰动力学，把运动学搞明白就够对付 80% 的场景了。
运动学核心分两类：正运动学和逆运动学，下面分别拆解。
二、正运动学：已知关节角度，求末端位置
2.1 基本思路
正运动学（Forward Kinematics，简称 FK）的输入是各关节的角度，输出是末端执行器在空间中的位置和姿态。
比如一个六轴机械臂，告诉你 6 个关节各自转了多少度（θ₁ 到 θ₆），你能算出末端夹爪当前在三维空间哪个坐标、朝哪个方向。这叫"正向"——从关节推到末端。
2.2 数学本质：矩阵乘法
正运动学的计算核心是齐次变换矩阵的连乘。每个关节对应一个变换矩阵，把所有矩阵按顺序乘起来，就得到末端到基座的总变换关系。
齐次变换矩阵是一个 4×4 矩阵，把"旋转"和"平移"统一在一个矩阵里表示：
T = | R p | R: 3×3 旋转矩阵，描述姿态 | 0 1 | p: 3×1 平移向量，描述位置其中 R 是 3×3 的旋转矩阵，p 是 3×1 的位置向量。矩阵最后一行固定是 [0 0 0 1]，纯粹是为了凑成方阵好做矩阵乘法。
三、逆运动学：已知末端位置，求关节角度
3.1 基本思路
逆运动学（Inverse Kinematics，简称 IK）是正运动学的反向操作：输入是"我希望末端到这个位置"，输出是"各关节该转多少度"。
这个反算比正算难得多。原因有三：
- 可能无解：目标位置超出机械臂工作空间，物理上够不着
- 可能多解：同一个末端位置，可能对应多组关节角度组合（比如"肘朝上"和"肘朝下"都能到）
- 非线性方程：涉及三角函数，没法简单反解
3.2 两种求解方法
- 解析法（Closed-form）：通过几何关系直接推导出角度公式，速度快、可实时，但只适用于结构简单的机械臂（且有特殊几何约束，比如相邻三轴交于一点）
- 数值法（Numerical）：用迭代优化（如牛顿-拉夫逊法、雅可比伪逆法）逼近解，通用性强但计算量大、可能不收敛
工业机械臂控制器一般用解析法（因为结构已知且优化过），通用机器人框架（如 ROS MoveIt）两者都支持。
四、D-H 参数模型：用 4 个参数描述一个关节
4.1 D-H 是什么
D-H 参数（Denavit-Hartenberg）是描述机械臂关节的经典方法，核心思想是：给每个关节指定一个坐标系，用 4 个参数描述相邻坐标系之间的关系。
这 4 个参数是：
| 参数 | 含义 | 类型 |
|---|---|---|
| a | 连杆长度（沿 x 轴的偏移） | 结构参数，固定 |
| α | 连杆扭角（绕 x 轴的旋转） | 结构参数，固定 |
| d | 关节偏距（沿 z 轴的偏移） | 移动关节为变量，转动关节固定 |
| θ | 关节角（绕 z 轴的旋转） | 转动关节为变量，移动关节固定 |
一句话理解：a 和 α 描述连杆本身的形状，d 和 θ 描述关节怎么动。
4.2 每个关节对应一个变换矩阵
相邻两个关节坐标系之间的变换，就是把这 4 个参数对应的 4 个基本变换串起来：
T = Rot(z, θ) · Trans(z, d) · Trans(x, a) · Rot(x, α)展开后是一个固定的 4×4 矩阵形式，θ 或 d 作为变量代入即可。n 个关节就有 n 个这样的矩阵，全乘起来就是正运动学的解。
五、二连杆机械臂正解推导示例
最简单的入门案例：一个两连杆平面机械臂，两个转动关节，连杆长度分别为 l₁ 和 l₂。
5.1 建立坐标系
- 基座原点在关节 1 处
- 关节 1 角度 θ₁，从 x 轴逆时针量
- 关节 2 角度 θ₂，相对于连杆 1 的方向量
5.2 推导末端位置
末端坐标 (x, y) 直接用三角函数写出：
x = l₁·cos(θ₁) + l₂·cos(θ₁ + θ₂) y = l₁·sin(θ₁) + l₂·sin(θ₁ + θ₂)这就是正运动学解。输入 θ₁、θ₂，输出末端 (x, y)。
5.3 反推逆解
已知 (x, y)，求 θ₁ 和 θ₂。先用余弦定理算出 θ₂：
cos(θ₂) = (x² + y² - l₁² - l₂²) / (2·l₁·l₂)再算 θ₁：
θ₁ = atan2(y, x) - atan2(l₂·sin(θ₂), l₁ + l₂·cos(θ₂))注意 θ₂ 有正负两个解，对应"肘朝上"和"肘朝下"两种姿态——这就是逆运动学的多解性最直观的体现。
六、逆解的多解性问题
同一个末端位姿，机械臂可能有多组关节角度都能到达。六轴机械臂最多有8 组解。实际控制时要根据以下策略选一组：
- 最短路径准则：选离当前关节角度最近的解，减少运动量
- 避障准则：选不会撞到障碍物的解
- 奇异规避准则：避开雅可比矩阵奇异的位置
七、轨迹规划基础
知道"去哪儿"还不够，还得规划"怎么去"，这就是轨迹规划。
7.1 三种基本运动类型
| 类型 | 全称 | 说明 |
|---|---|---|
| PTP | Point-to-Point | 点到点运动，只关心起点终点，中间路径自由 |
| LIN | Linear | 直线插补，末端走直线轨迹 |
| CIRC | Circular | 圆弧插补，末端走圆弧轨迹 |
PTP 最快但不控路径，LIN/CIRC 控路径但慢。抓取通常用 PTP 快速接近，最后一段用 LIN 精确对准。
7.2 关节空间 vs 笛卡尔空间
轨迹规划可以在两个空间进行：
- 关节空间规划：直接对各关节角度做插值（如梯形速度曲线），计算简单、不会出现奇异，但末端轨迹不可预测
- 笛卡尔空间规划：先在直角坐标系规划末端路径，再通过逆运动学实时算出各关节角度，末端轨迹可控但计算量大、可能遇到奇异点
实际系统通常混合使用：宏观移动用关节空间（快），精细操作用笛卡尔空间（准）。
7.3 速度规划：梯形速度曲线
最常用的速度曲线是梯形速度曲线：匀加速 → 匀速 → 匀减速。加速度恒定，速度曲线呈梯形。好处是计算简单、对电机冲击小。进阶的还有 S 型曲线，加速度也平滑变化，更柔和但计算更复杂。
八、小结：运动学的学习路径
给新手一个建议的学习顺序：
- 先理解齐次变换矩阵（旋转 + 平移）
- 手推二连杆平面机械臂的正逆解
- 学 D-H 参数建模，给一个简单机械臂标 D-H 参数
- 用 MATLAB 或 Python 实现正运动学计算
- 接触逆运动学的数值解法（雅可比迭代）
- 最后进入轨迹规划，实现 PTP/LIN 运动
运动学是机械臂控制的根基，这块搞透了，后面的视觉抓取、力控、大模型指令对接才有立足点。别跳过基础直接调包，不然出了问题根本不知道哪儿错了。