1.8 运动控制：DH 参数与正逆运动学

本文档需要配套对应视频观看，请联系助手获取视频（微信：ffcv1024）

为保证复现效果，请在资料中心下载最新版本的 Episode1 上位机（ 0.9.3 以上）、RoboDK 驱动脚本（1.1 以上）、API Demo（1.2 以上）。

1. DH 参数

图片来自于网络，侵删。

现在我们已经熟悉姿态的概念，但上述姿态主要是描述两个坐标系之间关系，那机械臂又如何运动呢？我们还需要了解 DH 参数、正逆运动学。

问题	答案
在描述多个关节连杆的空间关系时，为什么单纯使用齐次变换矩阵难以直接管理各关节的相对运动？	因为每个关节都有自己的坐标系，若仅依赖齐次变换矩阵，不同关节间的旋转轴与位置偏移很难形成统一规范。引入 Denavit-Hartenberg (DH) 参数就能以标准化方式定义相邻连杆的“旋转 + 平移”，简化整体推导。
为什么要学习正运动学？它的核心目标是什么？	正运动学关心给定关节角度，如何计算末端执行器在基坐标系下的位置与姿态。这是最基础的功能：示教器或控制程序需要知道末端当前在哪个点、朝向如何，才能进一步做路径规划或避障。
已经知道末端在哪里，为何还需要逆运动学？	逆运动学关心给定末端在空间中的目标位姿（坐标 + 姿态），如何求解对应的关节角度。若没有逆运动学，我们就无法指定“让末端移动到某个空间点”并求出关节动作。
DH 参数和正逆运动学是什么关系？	DH 参数定义了各关节连杆之间的相对变换（齐次矩阵）。正运动学则基于这些连杆变换逐级相乘得到末端的位姿。逆运动学则逆向利用（或求解）同样的 DH 模型和矩阵来求解每个关节的角度。

1.1 机械臂的基本结构概念

1.1.1 关节（Joint）

在机器人或机械臂的多关节结构中，关节（Joint）是用来连接相邻连杆的机械机构，其作用是允许两段连杆之间产生相对运动。根据运动类型的不同，常见的关节可分为 旋转关节（Revolute Joint） 和 移动关节（Prismatic Joint），如下动画：

旋转关节	移动关节

1.1.1.1 旋转关节（Revolute Joint）

定义
旋转关节允许相邻连杆绕某一固定轴做角度变化，就像一根“铰链”或“轴承”一样，故又称“铰链关节”或“R 关节”（英文里常用 R 表示 Revolute）。
运动特性
- 关节的唯一运动自由度是“角度”，通常记作。
- 在二维示意图中，可以直观看到一个杆臂绕关节转动；在三维空间中，该关节的旋转轴可能是水平、垂直或某个倾斜方向。
- 一般会有限位（机械或软件），保证关节旋转范围（如 -180° ～ +180°）。
举例
- 六轴机械臂中，每个关节大多是旋转关节：像人类手臂的肩、肘、腕关节一样，可以绕各自的转轴旋转。
- 机器人手指在爪子上的开合关节。
- 一般六轴机械臂，如 Episode1，六个关节都是旋转关节：
- 甚至对于人型机器人，也基本都是旋转关节（Unitree H1）
优点 & 应用
- 适合绝大多数需要灵活“摆动”或“定位”的任务场景，如焊接机器人、喷涂机器人等。
- 结构相对成熟，伺服电机或减速机可直接驱动关节转动。

1.1.1.2 移动关节（Prismatic Joint）

定义
移动关节允许相邻连杆沿某个方向做直线滑动或伸缩，因此也叫“滑动关节”或“P 关节”（英文用 P 表示 Prismatic）。
运动特性
- 关节的唯一运动自由度是“长度”或“直线位移 ”。
- 常见结构：一个气缸、液压缸、丝杠，或线性滑轨等，使连杆能在该轴向上移动。
- 一般也会有限位，如 0～100 mm、0～200 mm 等。
举例
- 直线模组：在一条直线导轨上移动的单元，被广泛用于自动化搬运、3D 打印机等。
- 具有伸缩臂的机械臂，一段能沿中心轴抽拉、改变整体长度。
优点 & 应用
- 对需要“平移或伸缩”动作的场合特别直观（例如需要在水平面上快移、或抓取时做线性插入、拔出）。
- 某些工业场景结合旋转关节与移动关节，可实现更大工作空间，或者简化特定方向的运动控制。

1.1.2 连杆（Link）

人手	协作机械臂	人型机器人

连杆（Link）在机器人多关节结构中指的是 连接关节与关节之间的刚性构件，它将一个关节传递的运动或力延伸到下一个关节或末端执行器。

机械臂的骨架
- 在多关节机器人中，关节和连杆相互交替：关节 → 连杆 → 关节 → 连杆……形成机械臂的基本骨架。连杆不可主动运动，但通过关节的活动获得整体运动能力。
尺寸与形状
- 连杆具有固定的长度和几何形状，通常由金属或复合材料制成，确保在高负载、快速运动时具备足够的刚度和强度。
- 对于机器人运动学建模，常关注连杆的参考长度（在 DH 参数中用等符号描述）。
坐标系设置
- 为了分析运动学，每个连杆会在其起始关节或末端关节处定义一个坐标系，并根据不同坐标系间的相对位置与姿态，用齐次变换矩阵或 DH 参数进行描述。
动力学与负载（扩展概念）
- 虽然连杆本身只是传递力和运动的构件，但在动力学分析中，需要知道连杆的质量、转动惯量等，以计算运动过程中的力矩、加速度或能耗。
- 在一般的入门学习中，我们多关注连杆长度、位置关系等几何属性。

1.2 DH 参数的由来与目的

有朋友会问：“我只想知道末端在哪里、要怎么到那里。能不能别让我量一大堆麻烦的数据？为什么非要学 DH？”

目标：把整条机械臂拆成“关节‑关节”小段：

核心任务：描述每个关节相对前一个关节的位姿。
把这些“小段”对应的齐次矩阵连乘 ⇒ 得到整机 正运动学，为轨迹规划、逆运动学打基础。

我们已有武器：齐次变换矩阵

它一次性打包旋转与平移。但难点是：现实中很难直接量出和的全部 12 个数字。

思路转变：量“几何量”替代“量矩阵”

对相邻关节，总能找到四个易测、可重复的几何量
1. ：两轴公共垂线长度（沿）
2. ：两轴绕的夹角
3. ：当前关节沿自身的平移量
4. ：当前关节绕自身的旋转角
这四个量 ⇢ 可直接生成齐次矩阵，形成 Denavit–Hartenberg (DH) 参数法。

DH 的核心操作

放坐标系（遵循 DH 规则）
- 对准关节轴
- 公共垂线方向设为
读取四个参数
- 其它杆件形状、厚度、孔位一律忽略——对位姿链无影响。

为何偏选 DH

简洁：4 个量 → 1 个矩阵
复用：换机器人只需重测这 4 个量
通用：教材、工业软件、科研论文广泛采用；“改进 DH 法”依旧同一套路

结论：学 DH 的目的，是用最少、最可靠的几何量，快速搭建整条“位姿骨架”。正 / 逆运动学、动力学分析都建立在这条骨架之上。

1.3 定义 DH 参数（经典/标准法）

关于 DH 参数的说明：

网络上关于 DH 参数的教程众多，定义方式也不尽相同，常见差异主要在于：

所采用的 坐标系放置规则不同（即经典 DH 与改进 DH 混用）
关节编号与坐标系编号不统一，导致理解混乱

由于目前并没有一个被所有人完全统一接受的“权威标准”，本课程采用林沛群老师的 DH 定义方式。

若你在其他资料中看到不同版本的 DH 表达，请不必困惑或争论。只要我们合理放置坐标系，按照统一规则使用四个参数正确构建出齐次变换矩阵，都是可以的。

请把重点放在 建立清晰一致的坐标系、理解四个几何量的意义与作用，细节取舍不必过于纠结。

下面介绍的主要是旋转关节的 DH 参数。

1.3.1 放置连杆坐标系（）

图片来自林老师课程。

步骤	操作	目的与要点
① 设	与第个关节旋转轴（或移动方向）重合	保证关节变量仅影响
② 找公共垂线	在与之间找最短连线（公垂线，Common Normal），其方向即	若两轴相交，则令通过交点且
③ 得	右手系：	保证正交、右手

下面用一些动画加深理解：

图片来源于网络

动画	解释
	方向与第个关节旋转轴（或移动方向）重合（这里是基座关节）
	对于 Base 坐标系，轴的方向是任意的；对于后续的关节，轴方向就有限制了（见后文）
	的方向遵循右手准则
	加入另一个关节，的方向和旋转方向一致，的方向应使得时是右手法则方向的旋转，即四指指向旋转方向

1.3.2 四个 DH 参数

参数	测量方向	几何含义
连杆长度	沿	沿轴，和之间的距离
扭转角	绕	绕轴，到的扭转角，取值，逆时针为正
关节偏移	沿	沿轴，和之间的距离；对移动关节为关节变量
关节角	绕	绕轴，和之间的夹角，逆时针为正；对旋转关节为关节变量

对旋转关节：为变量，为常量
对移动关节：为变量，为常量（可置）

动画	解释
	DH 的四个参数从两个关节轴的公垂线 (Common normal)定义后产生。公垂线也是两个轴之间最短连线
	与公垂线共线，方向从到注意：如图，第二个关节坐标系原点是可以在关节外部的（DH 建模考虑的是连杆 Link，而不是真实的物理分布）
	关节偏移：沿轴，和之间的距离；
	关节角：绕轴，和之间的夹角；
	连杆长度（图中为）：沿轴，和之间的距离
	扭转角：绕轴，到的扭转角

助记点：

轴好确定，根据旋转或移动方向即可，那再只要确定轴即可方便得到 DH 参数，轴如何确定？

在为相邻关节放置坐标系时，有两种做法：

让 上一节坐标系的轴沿公共垂线（common normal）；
让 下一节坐标系的轴沿公共垂线。

方案	谁占公共垂线？	对分叉链影响
A	（上一节）	祖先坐标系的只能指向一个后继，若一关节分出多支链，就不能同时对齐多条公共垂线。
B	（下一节）	每个子链的都沿自己与祖先轴的公共垂线，祖先坐标系无需改变，即可挂多个分支。

我们一般选择第 2 种，这样对“分叉链”更高效，因为同一个祖先关节可以向多个子链各自指出一条公共垂线；而若采用第 1 种方案，祖先的轴只能指向其中 1 个子链。

末端法兰连杆 Frame{n}
- 为了简化，通常取取同方向
  - 旋转关节：即关节偏移，关节角为任意值（通常选择 0）
  - 偏移关节：即关节角度，关节偏移为任意值（通常选择 0）

1.3.3 例子

这里举一些常见机械臂例子

表中图示坐标系颜色说明（红蓝标反了）：

红色：Z

蓝色：X

绿：Y

R：Revolute（旋转关节）

P：Prismatic（滑动关节）

上述自由度组合是常见结构，部分类型在特定实现中可能会有所不同。

名称	自由度组合	应用方向
Planar Elbow（平面肘型）	R–R 或 R–R–R	教学、二维轨迹仿真
Elbow Robot（肘型机械臂）	R–R–R（3D）	工业搬运、焊接
Anthropomorphic（类人型）	R–R–R–R–R–R	服务、人机协作
Episode 1	R–R–R–R–R–R	教学
SCARA（顺应装配臂）	R–R–P	平面装配、电子制造
CNC Robot（数控机器人）	结构不固定（常为 P-P-P）	精密切削、自动加工

你可能观察到：

Anthropomorphic 和 Episode 1 的存在负数，那它的正负如何定义？
- 因为：绕轴，到的扭转角，取值，逆时针为正
- 则：绕轴，到的扭转角方向为顺时针，所以为负
的方向是沿着关节轴，也就是有两个方向的选择，根据惯例，方向选择倾向于：
- 对 旋转关节：的方向应使得时是 右手法则方向的旋转，即四指指向旋转方向
- 对 移动关节：的方向应与的平移方向一致。
不过就算你选的方向不按这个原则也没事，只要你的坐标系定义是“自洽”的（剩余几个参数也跟着调整），齐次变换结果就会正确，但一旦 Z 轴方向变了而其余参数未作相应修改，齐次变换矩阵就会出错。如 Episode 1 的例子：
- 如朝外：的扭转角分别为
- 如朝内：的扭转角分别为

朝外	朝内

可以看到，这个会影响最终的 DH 参数表，但并不影响你建模计算，只要第四个关节的正角度旋转方向定义也对应的变化了，只要你代码对应变化即可。

课程中我使用的是朝外。

2. 正逆运动学

问题	正运动学 (FK)	逆运动学 (IK)
要解决什么？	关节角 → 末端位姿	末端目标位姿 → 关节角
典型场景	示教、仿真、雅可比计算	抓取、焊接、规划、避障
为什么重要	① 确定当前姿态 ② 可视化轨迹	① 让末端到目标 ② 生成平滑轨迹

本文只讨论 全旋转关节 (nR) 机械臂，例如 6R 工业机器人。

2.1 正运动学 —— “给角度，算末端”

这里用的是标准 DH。

建立 DH 表：
对纯旋转关节：为常量，为变量。
单节变换矩阵 的推导

标准 DH 把 {i‑1} → {i} 的相对位姿拆成四个 可直接写出 的基本变换（利用我们学过的欧拉角和齐次变换）：

顺序	基本操作	矩阵符号	形式
①	绕旋转
②	沿平移
③	沿平移
④	绕旋转

记号：

同理。

再连乘四步

逐行乘积（略去中间乘法步骤）得到

这就是 标准 DH 单节变换矩阵，后续连乘全部即可得到末端对基座的总变换。

连乘得到总变换

若有工具偏置

其中为法兰 → 工具 (TCP) 的固定刚性变换。

提取结果

位置向量 → 总矩阵第 1–3 行第 4 列
姿态矩阵 → 左上

代码示例

下载代码正运动学示例，并与 robodk 仿真结果对比

工作空间

机器人末端执行器（TCP）在关节限位内 能够到达的所有空间点 的集合。
若还考虑姿态，则称 可达位姿空间 (Task Space)。

2.2 逆运动学 —— “给目标，算角度”

2.2.1 常用 IK 求解方法

方法	适用特点	核心要点
几何 / 解析解	轴线排布规则性强（如 6R 球腕）	拆腕点 → 代数 / 三角方程，结果封闭形式，速度快
代数（符号）解	复杂三维链，轴不共面时也可	用 Gröbner、Resultant 消元，代码生成如 IKFast
数值迭代	冗余 or 解析困难	误差函数 + Jacobian 伪逆 / LM → Newton 迭代
优化 / 约束解	同时满足多任务	以任务误差为成本，约束关节限位、碰撞，QP / SQP
学习‑基 / 数据驱动	非结构化机械臂、大量采样	神经网络、GPR 把 IK 当回归；需离线训练

实际工程：能用解析就用解析（最快、无发散）→ 解析不可得再退到数值 / 优化。

2.2.2 通用解析解计算步骤（6R 机械臂为例）

可参考论文，大致步骤如下：

准备
- DH 参数 / 机器人几何
- 目标位姿
分离位置与姿态
- 若为球形腕，先求腕中心
求前 3 轴
- 解析
- 得到基座 → 关节 3 的旋转
  $示意$
求后 3 轴
- 由
  
  可得
- 从解析
组合与筛选
- 关节限位、奇异、碰撞
- 多解时按代价函数择优

2.2.3 常见逆运动学求解器 IK Solvers

因逆运动学求解复杂，异常情况多，实际求解的时候，我们大多是调库，这里列出常见的求解库：

名称	解法类型	是否开源	语言/平台	多解支持	支持冗余臂	是否带约束	特点 / 备注
IKFast	解析解（Closed-form）	是	C++ / 可绑定 Python	是（完整枚举）	否	否	超快（毫秒级），支持打包成 Python 库，工业用最多
MoveIt! IK Plugins	数值 / 插件式	是	ROS / C++	取决于插件	是	可配置	插件可用 IKFast、KDL、TRAC-IK 等
TRAC-IK	数值 + 优化	是	C++ / ROS	否（单解，初值相关）	是	是	替代 KDL，更快更稳
KDL (Kinematics and Dynamics Library)	数值牛顿迭代	是	C++ / Python（PyKDL）	否	是	弱	ROS 默认求解器之一，稳定但慢
Pinocchio	数值优化（几何 + 代数）	是	C++ / Python	否	是	是	快速，适合优化和控制
Robotics Toolbox for Python (RTB)	数值法（LM, QP, GN, NR）	是	Python	否（可手动多初值）	是	QP 支持	教学/研究利器，灵活但默认单解
Modern Robotics Library	解析 + 数值	是	Python / MATLAB / C++	部分支持	否	弱	Kevin Lynch 教材配套，支持 IK_NR 等方法
Drake	数值优化 + 符号解混合	是	C++ / Python	依赖配置	是	强约束	高级建模与控制系统，MIT 出品
OpenRAVE (非维护)	支持 IKFast	是	C++ / Python	是（IKFast）	否	弱	主要为 IKFast 提供环境
RoboDK	IKFast 封装（+ 数值补充）	否（商业）	GUI + Python API	是（内置多解）	取决于插件	有限支持	商用软件，适合工业与路径仿真，可导出 Python 脚本
Unity Articulation IK	数值 +Heuristic	否（商业/游戏引擎）	Unity / C#	否	是	弱	用于游戏/VR 仿真
PyBullet IK Solver	数值（Jacobian-based）	是	Python	否	是	轻约束	实时仿真用，精度不高，适合强化学习
V-REP / CoppeliaSim IK	数值 + 分支解	是（基础版）	Lua / Python / GUI	可部分配置	是	GUI 设置	图形环境好，教学仿真首选
ROS2 Kinematics Plugins	数值 / 插件式	是	C++ / ROS2	同上	是	支持	ROS2 环境下的默认 IK 系统

“支持冗余臂”：能不能处理自由度 > 任务要求的机器人。

比如：

末端只需要控制 + 姿态（6 个变量）

但机器人有 7 个关节（像 KUKA、Franka Panda）

这就叫冗余机器人，有无穷多个解。

支持冗余的求解器：

能找到一个合适的解（可能是最近的、最省力的）

有些还能“挑选”一个更优的解

“支持约束”：能不能考虑限制条件求解。

比如：

关节不能超过最大角度

不想靠近奇异点

要避障，或只允许某些方向

支持约束的求解器：

会在解的过程中自动避开这些“禁区”

适合真实机器人控制或避障任务

场景推荐：

需求场景	推荐求解器
工业机器人控制、实时响应	IKFast、RoboDK
任意结构机器人、教学/研究	Robotics Toolbox (RTB)、Modern Robotics
冗余机械臂、路径优化	TRAC-IK、Pinocchio、Drake
强约束任务（关节限制/避障）	IK_QP（RTB）、Drake
强仿真 / 虚拟仿真	PyBullet、CoppeliaSim、Unity Articulation

下载代码 RTB 逆运动学示例，并与 robodk 仿真结果对比

注意：在机械臂正前面求解较容易，背面容易失败