建模与正逆运动学

建议配合对应视频学习，操作前先阅读本章注意事项。

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

一、DH参数

图片来自于网络，侵删。

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

1.1 机械臂的基本结构概念

1.1.1 关节（Joint）

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

旋转关节	移动关节

1.1.1.1 旋转关节（Revolute Joint）

1. 定义
   旋转关节允许相邻连杆绕某一固定轴做角度变化，就像一根“铰链”或“轴承”一样，故又称“铰链关节”或“R 关节”（英文里常用 R 表示 Revolute）。
2. 运动特性

• 关节的唯一运动自由度是“角度”，通常记作 $\theta$。
• 在二维示意图中，可以直观看到一个杆臂绕关节转动；在三维空间中，该关节的旋转轴可能是水平、垂直或某个倾斜方向。

• 一般会有限位（机械或软件），保证关节旋转范围（如 -180° ～ +180°）。
  3. 举例

• 六轴机械臂中，每个关节大多是旋转关节：像人类手臂的肩、肘、腕关节一样，可以绕各自的转轴旋转。

• 机器人手指在爪子上的开合关节。

• 一般六轴机械臂，六个关节都是旋转关节：

  
  - 甚至对于人型机器人，也基本都是旋转关节（Unitree H1）

  
  4. 优点 & 应用

• 适合绝大多数需要灵活“摆动”或“定位”的任务场景，如焊接机器人、喷涂机器人等。

• 结构相对成熟，伺服电机或减速机可直接驱动关节转动。

1.1.1.2 移动关节（Prismatic Joint）

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

1.1.2 连杆（Link）

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

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

1. 机械臂的骨架

• 在多关节机器人中，关节和连杆相互交替：关节→连杆→关节→连杆……形成机械臂的基本骨架。连杆不可主动运动，但通过关节的活动获得整体运动能力。
  2. 尺寸与形状

• 连杆具有固定的长度和几何形状，通常由金属或复合材料制成，确保在高负载、快速运动时具备足够的刚度和强度。

• 对于机器人运动学建模，常关注连杆的参考长度（在 DH参数 中用 $a$ 等符号描述）。
  3. 坐标系设置

• 为了分析运动学，每个连杆会在其起始关节或末端关节处定义一个坐标系，并根据不同坐标系间的相对位置与姿态，用齐次变换矩阵或 DH 参数进行描述。

  
  4. 动力学与负载（扩展概念）

• 虽然连杆本身只是传递力和运动的构件，但在动力学分析中，需要知道连杆的质量、转动惯量等，以计算运动过程中的力矩、加速度或能耗。

• 在一般的入门学习中，我们多关注连杆长度、位置关系等几何属性。

  

1.2 DH 参数的由来与目的

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

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

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

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

$$T=\left[\begin{array}{ccc}R& P000& 1\end{array}\right]=\left[\begin{array}{ccccccccccccc}{r}_{11}& r_{12}& r_{13}& x{r}_{21}& r_{22}& r_{23}& y{r}_{31}& r_{32}& r_{33}& z0& 0& 0& 1\end{array}\right]$$

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

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

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

DH 的核心操作

1. 放坐标系（遵循 DH 规则）
   - $Z_i$ 对准关节轴
   - 公共垂线方向设为 $X_i$
   - $Y_i=Z_i\times X_i$
2. 读取四个参数 $(a,\alpha ,d,\theta )$
   - 其它杆件形状、厚度、孔位一律忽略——对位姿链无影响。

为何偏选 DH

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

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

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

关于 DH 参数的说明：

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

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

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

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

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

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

1.3.1. 放置连杆坐标系（$i$）

图片来自林老师课程。

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

图片来源于网络

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

1.3.2 四个 DH 参数 $(a_i,{\alpha }_i,d_i,{\theta }_i)$

• 对旋转关节：${\theta }_i$ 为变量，$d_i$ 为常量
• 对移动关节：$d_i$ 为变量，${\theta }_i$ 为常量（可置 $0$）

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

助记点：

• $Z$轴好确定，根据旋转或移动方向即可，那再只要确定$X$轴即可方便得到DH参数，$X$轴如何确定？
• 在为相邻关节放置坐标系时，有两种做法：

1. 让 上一节坐标系的 $X_{i-1}$轴沿公共垂线（common normal）；
2. 让 下一节坐标系的 $X_i$ 轴沿公共垂线。

我们一般选择第 2 种，这样对“分叉链”更高效，因为同一个祖先关节可以向多个子链各自指出一条公共垂线；而若采用第 1 种方案，祖先的 $X_{i-1}$轴只能指向其中 1 个子链。
* 末端法兰连杆 Frame{n}

• 为了简化，通常取$X_n$取$X_{n-1}$同方向
  • 旋转关节：即关节偏移$d_n=0$，关节角${\theta }_n$为任意值（通常选择0）
  • 偏移关节：即关节角度${\theta }_n=0$，关节偏移$d_n$为任意值（通常选择0）

1.3.3 例子

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

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

• 红色：Z
• 蓝色：X
• 绿：Y

R：Revolute（旋转关节）

P：Prismatic（滑动关节）

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

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

你可能观察到：

• Anthropomorphic 和当前机械臂的${\alpha }_5$存在负数，那它的正负如何定义？

• 因为${\alpha }_i$：绕$X_i$轴，$Z_{i-1}$ 到 $Z_i$ 的扭转角，取值 $(-\pi ,\pi ]$，逆时针为正
• 则${\alpha }_5$：绕$X_5$轴，$Z_4$ 到 $Z_5$ 的扭转角方向为顺时针，所以为负

• $Z_i$ 的方向是沿着关节轴$i$，也就是有两个方向的选择，根据惯例，$Z_i$方向选择倾向于：

• 对 旋转关节：$Z_i$ 的方向应使得 ${\theta }_i>0$ 时是 右手法则方向的旋转，即四指指向旋转方向

• 对 移动关节：$Z_i$ 的方向应与 $d_i>0$ 的平移方向一致。

不过就算你选的方向不按这个原则也没事，只要你的坐标系定义是“自洽”的（剩余几个参数也跟着调整），齐次变换结果就会正确， 但一旦 Z 轴方向变了而其余参数未作相应修改，齐次变换矩阵就会出错。如当前机械臂的例子：

• 如$Z_4$朝外：$q_4,q_5$的扭转角$\alpha$分别为$-90,+90$
• 如$Z_4$朝内：$q_4,q_5$的扭转角$\alpha$分别为$90,-90$

$Z_4$朝外	$Z_4$朝内

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

课程中我使用的是$Z_4$朝外。

二、正逆运动学

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

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

这里用的是标准 DH。

1. 建立 DH 表：$(a_i,{\alpha }_i,d_i,{\theta }_i),i=1\dots n$
   对纯旋转关节：$d_i$ 为常量，${\theta }_i$ 为变量。
2. 单节变换矩阵 ${}^{i-1}!T_i$ 的推导

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

记号：$c_{{\theta }_i}=\cos {\theta }_i,s_{{\theta }_i}=\sin {\theta }_i$

同理 $c_{{\alpha }_i},s_{{\alpha }_i}$。

再连乘四步

$${}^{i-1}!T_i=R_z({\theta }_i)T_z(d_i)T_x(a_i)R_x({\alpha }_i)$$

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

$${}^{i-1}!T_i=\left[\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i\cos {\alpha }_i& \sin {\theta }_i\sin {\alpha }_i& a_i\cos {\theta }_i\\ \sin {\theta }_i& \cos {\theta }_i\cos {\alpha }_i& -\cos {\theta }_i\sin {\alpha }_i& a_i\sin {\theta }_i\\ 0& \sin {\alpha }_i& \cos {\alpha }_i& d_i\\ 0& 0& 0& 1\end{array}\right]$$

这就是 标准 DH 单节变换矩阵，后续连乘全部 ${}^{i-1}!T_i$ 即可得到末端对基座的总变换。

1. 连乘得到总变换

$${}^0!T_n=\prod _{i=1}^n{}^{i-1}!T_i={}^0!T_1{}^1!T_2\dots {}^{n-1}!T_n$$

1. 若有工具偏置

$${}^0!T_{\text{TCP}}={}^0!T_n{}^n!T_{\text{tool}}$$

其中 ${}^n!T_{\text{tool}}$ 为法兰 → 工具 (TCP) 的固定刚性变换。

1. 提取结果

• 位置向量 $\mathbf{P}=[xyz{]}^T$ → 总矩阵第 1–3 行第 4 列
• 姿态矩阵 $R$ → 左上 $3\times 3$

1. 代码示例

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

1. 工作空间

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

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

2.2.1 常用 IK 求解方法

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

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

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

1. 准备

• DH 参数 / 机器人几何

• 目标位姿
  $$T_{\text{goal}}=\left[\begin{array}{ccc}R& P0& 1\end{array}\right]$$
  2. 分离位置与姿态

• 若为球形腕，先求腕中心
  $${\mathbf{p}}_w=P-d_{6}R\hat{z}$$
  3. 求前 3 轴

• 解析 ${\theta }_1,{\theta }_2,{\theta }_3$

• 得到基座 → 关节 3 的旋转
  $${}^0!R_3=R_z({\theta }_1)R_y({\theta }_2)R_y({\theta }_3)\text{(示意)}$$
  4. 求后 3 轴

• 由

  $$R={}^0!R_3{}^3!R_6$$

  可得
  $${}^3!R_6=({}^0!R_3{)}^{T}R$$
  - 从 ${}^3!R_6$ 解析 ${\theta }_4,{\theta }_5,{\theta }_6$
  5. 组合与筛选

• 关节限位、奇异、碰撞

• 多解时按代价函数择优

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

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

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

比如：

• 末端只需要控制 $x,y,z$ + 姿态（6 个变量）
• 但机器人有 7 个关节（像 KUKA、Franka Panda）

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

支持冗余的求解器：

• 能找到一个合适的解（可能是最近的、最省力的）
• 有些还能“挑选”一个更优的解

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

比如：

• 关节不能超过最大角度
• 不想靠近奇异点
• 要避障，或只允许某些方向

支持约束的求解器：

• 会在解的过程中自动避开这些“禁区”
• 适合真实机器人控制或避障任务

场景推荐：

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

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