DH（Denavit–Hartenberg）矩阵

DH 矩阵（Denavit-Hartenberg 矩阵）是 1955 年由 Denavit 和 Hartenberg 提出的一种机器人运动学建模方法，用于描述机器人连杆和关节之间的关系。该方法通过在机器人每个连杆上建立坐标系，并用 4×4 的齐次变换矩阵（DH 矩阵）描述相邻连杆的空间关系，从而推导出末端执行器相对于基坐标系的位姿，建立机器人的运动学方程 。

DH 方法包括传统 DH（Classic DH）和改进 DH（Modified DH），主要区别在于坐标系建立位置和参数变换顺序 。每个连杆用 4 个参数（如关节角 $\theta$、连杆长度 $a$、连杆偏移 $d$、连杆扭角 $\alpha$）描述，转动关节的关节变量为 $\theta$，移动关节的关节变量为 $d$ 。通过依次相乘各关节的 DH 矩阵，可得到机器人末端在基坐标系中的位姿，适用于机器人正运动学解算和 3D 模型运算 。

视频链接
在本人复刻Dummy机械臂的时候，重温DH矩阵，并做如下记录：

DH 方法也被用于五轴机床等复杂系统的运动学建模和几何误差补偿 。下面给出一份系统性、逐条拆解的 DH（Denavit–Hartenberg）矩阵教程。

---

1. 为什么要用 DH？

• 机器人/机床通常由 $n$ 个连杆 $+n$ 个关节 串联而成。

• 我们需要把 关节变量 $q$（转动关节的 $\theta$ 或移动关节的 $d$）映射到 末端位姿 $T(q)$，用于正运动学、仿真、控制、标定。

• DH 提供一种最少参数、无歧义的建系规范，使所有运动学方程都可以用 4×4 齐次变换矩阵连乘得到：

  $T_{0n}(q)=T_{01}(q_1)T_{12}(q_2)\cdots T_{(n-1)n}(q_n)$

---

2. 两种约定：Classic vs. Modified

对比项	Classic DH (1955)	Modified DH (MDH, 1986)
坐标系位置	连杆 远端（关节 $i+1$ 处）	连杆 近端（关节 $i$ 处）
变换顺序	先沿 $z_i$ 旋转/平移，再沿 $x_i$ 平移/旋转( 依次$z_i$ 旋转->$z_i$ 平移，再沿 $x_i$ 平移->$x_i$ 旋转)	先沿 $x_{i-1}$ 平移/旋转，再沿 $z_i$ 旋转/平移( 依次$x_{i-1}$ 旋转->$x_{i-1}$ 平移，再沿 $z_i$ 旋转->$z_i$ 平移)
工业软件	$$\begin{gathered} RoboticsToolbox(MATLAB)、 \\ KUKAKRC、ABBRAPID \end{gathered}$$	$$\begin{gathered} ROSMoveIt、OpenRAVE、 \\ URDF、SolidWorks \end{gathered}$$

参考 https://www.bilibili.com/video/BV1Ue4y1R7QJ/?spm_id_from=333.337.search-card.all.click&vd_source=6c355cf343a2ceefccfcf5bb64aee668

同一套参数，两种约定算出的矩阵不同，千万别混用。

---

3. 四个参数几何意义（以 MDH 为例）

在 Modified DH（MDH） 里，四个参数必须严格对应“前一连杆”和“当前关节”：

参数	下标	几何意义	变量
连杆扭角 $\alpha$	${\alpha }_{i-1}$	绕 $x_{i-1}$ 轴，从 $z_{i-1}$ 旋转到 $z_i$ 的角度	常数
连杆长度 $a$	$a_{i-1}$	沿 $x_{i-1}$ 轴，从 $z_{i-1}$ 移动到 $z_i$ 的距离	常数
关节角 $\theta$	${\theta }_i$	绕 $z_i$ 轴，从 $x_{i-1}$ 旋转到 $x_i$ 的角度	转动关节变量
连杆偏距 $d$	$d_i$	沿 $z_i$ 轴，从 $x_{i-1}$ 移动到 $x_i$ 的距离	移动关节变量

---

4. 变换矩阵公式（MDH）

变换顺序：

1. 先绕 $x_{i-1}$ 轴旋转 ${\alpha }_{i-1}$ 再平移 $a_{i-1}$
2. 再绕 $z_i$ 轴旋转 ${\theta }_i$ 再平移 $d_i$

4×4 齐次矩阵 $T_{i-1,i}$ 应该是：

$T_{i-1,i}(q_i)=\mathrm{Rot}(x,{\alpha }_{i-1})\mathrm{Trans}(x,a_{i-1})\mathrm{Rot}(z,{\theta }_i)\mathrm{Trans}(z,d_i)$

展开后：
$T_{i-1,i}=\left[\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i& 0& a_{i-1}\\ \sin {\theta }_i\cos {\alpha }_{i-1}& \cos {\theta }_i\cos {\alpha }_{i-1}& -\sin {\alpha }_{i-1}& -d_i\sin {\alpha }_{i-1}\\ \sin {\theta }_i\sin {\alpha }_{i-1}& \cos {\theta }_i\sin {\alpha }_{i-1}& \cos {\alpha }_{i-1}& d_i\cos {\alpha }_{i-1}\\ 0& 0& 0& 1\end{array}\right]$

• $\alpha$ 和 $a$ 的下标是 $i-1$（因为它们属于“前一连杆”的几何属性）
• $\theta$ 和 $d$ 的下标是 $i$（关节变量属于“当前关节”）

---

5. 建立坐标系（MDH 流程）

步骤	做什么	对哪个标号	关键要点
0	编号	关节 $i=1\dots n$	基座系为 $0$，末端系为 $n$
1	画 $z_i$	对 $i=0\dots n$	$z_i$ 与关节 $i$ 的轴线重合，方向自定
2	找 $x_{i-1}$	对 $i=1\dots n$	$x_{i-1}$ 是 $z_{i-1}$ 与 $z_i$ 的公垂线，方向从 $z_{i-1}$ 指向 $z_i$
3	定 $O_{i-1}$	对 $i=1\dots n$	$O_{i-1}$ 位于 $z_{i-1}$ 与 $x_{i-1}$ 的交点
4	画 $y_{i-1}$	对 $i=1\dots n$	$y_{i-1}=z_{i-1}\times x_{i-1}$，右手定则
5	重复 $1-4$	直到 $i=n$	末端系 $n$ 的原点放在工具中心
6	量 $4$ 个参数	对 $i=1\dots n$	${\alpha }_{i-1}、a_{i-1}、{\theta }_i、d_i$，按定义测量

---

6. 完整示例：2 自由度平面 RR 机械臂

• 关节 1、2 平行于 $z$ 轴，连杆长度 $L1$、$L2$。
• 所有 ${\alpha }_i=0，d_i=0，a_1=L1，a_2=L2$。

$i$	${\theta }_i$	$d_i$	$a_i$	${\alpha }_i$
$1$	${\theta }_1$	$0$	$L1$	$0$
$2$	${\theta }_2$	$0$	$L2$	$0$

计算末端位姿：
$T_{02}=T_{01}({\theta }_1)T_{12}({\theta }_2)=\left[\begin{array}{cccc}{c}_{12}& -s_{12}& 0& L_1{c}_1+L_2{c}_{12}\\ s_{12}& c_{12}& 0& L_1{s}_1+L_2{s}_{12}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

其中 $c_1=cos{\theta }_1,s_1=sin{\theta }_1,c_{12}=cos({\theta }_1+{\theta }_2)\dots$

---

7. 常见陷阱

• 坐标系画错：Classic/Modified 的 $x_i$、$z_i$ 定义不同，容易混。
• 移动关节变量：在 MDH 中，移动关节变量是 $d_i$，不是 ${\theta }_i$。
• 参数符号：有的教材把 $a_i$ 写成 $r_i$，把 ${\alpha }_i$ 写成 ${\phi }_i$。
• 零位对齐：机器人出厂零位 $\ne DH$ 零位，需加 $offset$。
• URDF ↔ DH 转换：ROS 的 URDF 用 MDH，但标签的 $rpy/xyz$ 顺序是 $xyz$ 而非 $x\alpha z\theta$，需要二次解析。

---

8. Python 代码（以 MDH 为例）

import numpy as npdef dh_matrix(theta, d, a, alpha):"""返回 4×4 DH 变换矩阵（MDH 约定）"""ct, st, ca, sa = np.cos(theta), np.sin(theta), np.cos(alpha), np.sin(alpha)return np.array([[ct, -st*ca,  st*sa, a*ct],[st,  ct*ca, -ct*sa, a*st],[0,   sa,     ca,    d   ],[0,   0,      0,     1   ]])# 2R 机械臂参数
L1, L2 = 1.0, 0.5
q1, q2 = np.deg2rad(30), np.deg2rad(45)T01 = dh_matrix(q1, 0, L1, 0)
T12 = dh_matrix(q2, 0, L2, 0)
T02 = T01 @ T12print("End-effector position:", T02[:3, 3])

---

9. 进阶话题

• 标定：用激光跟踪仪或视觉测量实际 $DH$ 参数误差，建立误差模型 $\Delta T$。
• 微分运动学：对 $DH$ 矩阵求偏导得到雅可比 $J(q)$。
• 树形/闭环结构：标准 $DH$ 仅适用于串联链，需引入虚拟关节或 $URDF$ 的 <joint type="floating">。

---

一句话总结：
$DH矩阵=机械杆件的“身份证”+坐标系“说明书”，只要遵循同一套约定，就能把任何串联机构转换成一串4\times 4矩阵的乘积。$