机械臂运动学原理入门：正逆解、D-H 参数、轨迹规划基础

机械臂运动学原理入门：正逆解、D-H 参数、轨迹规划基础

运动学听起来吓人，其实就是回答两个问题：关节转了多少度，末端到了哪儿？反过来，末端想去哪儿，关节该转多少度？

一、运动学到底是什么

机械臂的**运动学（Kinematics）**研究的是"运动"本身，不涉及"力"。它只关心两件事：

1. 关节角度和末端位置之间的几何关系
2. 这个关系怎么算

和运动学对应的叫动力学（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）是正运动学的反向操作：输入是"我希望末端到这个位置"，输出是"各关节该转多少度"。

这个反算比正算难得多。原因有三：

1. 可能无解：目标位置超出机械臂工作空间，物理上够不着
2. 可能多解：同一个末端位置，可能对应多组关节角度组合（比如"肘朝上"和"肘朝下"都能到）
3. 非线性方程：涉及三角函数，没法简单反解

3.2 两种求解方法

• 解析法（Closed-form）：通过几何关系直接推导出角度公式，速度快、可实时，但只适用于结构简单的机械臂（且有特殊几何约束，比如相邻三轴交于一点）
• 数值法（Numerical）：用迭代优化（如牛顿-拉夫逊法、雅可比伪逆法）逼近解，通用性强但计算量大、可能不收敛

工业机械臂控制器一般用解析法（因为结构已知且优化过），通用机器人框架（如 ROS MoveIt）两者都支持。

四、D-H 参数模型：用 4 个参数描述一个关节

4.1 D-H 是什么

D-H 参数（Denavit-Hartenberg）是描述机械臂关节的经典方法，核心思想是：给每个关节指定一个坐标系，用 4 个参数描述相邻坐标系之间的关系。

这 4 个参数是：

一句话理解：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 最快但不控路径，LIN/CIRC 控路径但慢。抓取通常用 PTP 快速接近，最后一段用 LIN 精确对准。

7.2 关节空间 vs 笛卡尔空间

轨迹规划可以在两个空间进行：

• 关节空间规划：直接对各关节角度做插值（如梯形速度曲线），计算简单、不会出现奇异，但末端轨迹不可预测
• 笛卡尔空间规划：先在直角坐标系规划末端路径，再通过逆运动学实时算出各关节角度，末端轨迹可控但计算量大、可能遇到奇异点

实际系统通常混合使用：宏观移动用关节空间（快），精细操作用笛卡尔空间（准）。

7.3 速度规划：梯形速度曲线

最常用的速度曲线是梯形速度曲线：匀加速 → 匀速 → 匀减速。加速度恒定，速度曲线呈梯形。好处是计算简单、对电机冲击小。进阶的还有 S 型曲线，加速度也平滑变化，更柔和但计算更复杂。

八、小结：运动学的学习路径

给新手一个建议的学习顺序：

1. 先理解齐次变换矩阵（旋转 + 平移）
2. 手推二连杆平面机械臂的正逆解
3. 学 D-H 参数建模，给一个简单机械臂标 D-H 参数
4. 用 MATLAB 或 Python 实现正运动学计算
5. 接触逆运动学的数值解法（雅可比迭代）
6. 最后进入轨迹规划，实现 PTP/LIN 运动

运动学是机械臂控制的根基，这块搞透了，后面的视觉抓取、力控、大模型指令对接才有立足点。别跳过基础直接调包，不然出了问题根本不知道哪儿错了。