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)?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矩陣的乘積。$