03 - DH展开式与正运动学四步算法

1. 齐次变换矩阵模型

DH 变换本质上由以下 4 个基础平移与旋转齐次矩阵按固定顺序连乘复合而成：

1. 绕 Z 轴旋转 $\mathrm{Rot}(z, \theta)$

$${\begin{bmatrix} \cos\theta & -\sin\theta & 0 & 0 \\ \sin\theta & \cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}}$$

2. 沿 Z 轴平移 $\mathrm{Trans}(z, d)$

$${\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d \\ 0 & 0 & 0 & 1 \end{bmatrix}}$$

3. 沿 X 轴平移 $\mathrm{Trans}(x, a)$

$${\begin{bmatrix} 1 & 0 & 0 & a \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}}$$

4. 绕 X 轴旋转 $\mathrm{Rot}(x, \alpha)$

$${\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha & 0 \\ 0 & \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}}$$

标准 DH 变换顺序：绕当前 $Z$ 轴旋转 $\theta_i$ $\rightarrow$ 沿当前 $Z$ 轴平移 $d_i$ $\rightarrow$ 沿新 $X$ 轴平移 $a_i$ $\rightarrow$ 绕新 $X$ 轴旋转 $\alpha_i$

$$^{i-1}\mathbf{T}_i = \mathrm{Rot}(z, \theta_i) \cdot \mathrm{Trans}(z, d_i) \cdot \mathrm{Trans}(x, a_i) \cdot \mathrm{Rot}(x, \alpha_i)$$

$$^{i-1}\mathbf{T}_i = \begin{bmatrix} \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{bmatrix}$$

import numpy as np

# 1. 计算单步标准 SDH 齐次变换矩阵
def compute_T_SDH(theta, d, a, alpha):
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = 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]
    ])

# 2. 正运动学流水线：链式矩阵乘法求解 ^0T_n
def forward_kinematics_sdh(dh_params):
    T_total = np.eye(4)
    for theta, d, a, alpha in dh_params:
        T_total = T_total @ compute_T_SDH(theta, d, a, alpha)
    return T_total

改进 MDH 变换顺序 (Craig 法)：绕前一 $X$ 轴旋转 $\alpha_{i-1}$ $\rightarrow$ 沿前一 $X$ 轴平移 $a_{i-1}$ $\rightarrow$ 绕当前 $Z$ 轴旋转 $\theta_i$ $\rightarrow$ 沿当前 $Z$ 轴平移 $d_i$

$$^{i-1}\mathbf{T}_i = \mathrm{Rot}(x, \alpha_{i-1}) \cdot \mathrm{Trans}(x, a_{i-1}) \cdot \mathrm{Rot}(z, \theta_i) \cdot \mathrm{Trans}(z, d_i)$$

$$^{i-1}\mathbf{T}_i = \begin{bmatrix} \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{bmatrix}$$

import numpy as np

# 1. 计算单步改进 MDH 齐次变换矩阵
def compute_T_MDH(alpha_prev, a_prev, theta, d):
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha_prev), np.sin(alpha_prev)
    return np.array([
        [ct, -st, 0, a_prev ],
        [st * ca, ct * ca, -sa, -d * sa ],
        [st * sa, ct * sa, ca, d * ca ],
        [0, 0, 0, 1 ]
    ])

# 2. 正运动学流水线：链式矩阵乘法求解 ^0T_n
def forward_kinematics_mdh(mdh_params):
    T_total = np.eye(4)
    for alpha_prev, a_prev, theta, d in mdh_params:
        T_total = T_total @ compute_T_MDH(alpha_prev, a_prev, theta, d)
    return T_total

2. 正运动学标准化四步求解法

建立坐标系，列写 DH 参数表

根据关节旋转/移动轴定义各连杆局部坐标系，测量并列出参数 $(\theta_i, d_i, a_i, \alpha_i)$。

代入展开式，计算单步变换矩阵 $^{i-1}\mathbf{T}_i$

将每一行连杆参数代入对应的单步解析矩阵公式中，计算出各关节间的级联变换。

应用链式法则，连乘求出基座到末端总矩阵 $^0\mathbf{T}_n$

$$^0\mathbf{T}_n = \,^0\mathbf{T}_1 \cdot \,^1\mathbf{T}_2 \cdots \,^{n-1}\mathbf{T}_n$$ 利用“右下对消左上”的矩阵链乘关系，求得整体正运动学传递函数。

投影末端目标点位姿 (乘以 $\begin{bmatrix} ^n\mathbf{p} \\ 1 \end{bmatrix}$)

若末端工具上存在一点 $^n\mathbf{p} = \begin{bmatrix} x & y & z \end{bmatrix}^\top$（齐次坐标为 $\begin{bmatrix} ^n\mathbf{p} \\ 1 \end{bmatrix}$），其在基座坐标系 $\{0\}$ 下的位置 $^0\mathbf{p}$ 通过总变换矩阵投影求得：

$$\begin{bmatrix} ^0\mathbf{p} \\ 1 \end{bmatrix} = \,^0\mathbf{T}_n \begin{bmatrix} ^n\mathbf{p} \\ 1 \end{bmatrix} = \begin{bmatrix} \mathbf{R}_{3\times 3} & \mathbf{P}_{3\times 1} \\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} ^n\mathbf{p} \\ 1 \end{bmatrix}$$

💡 特例说明：当求末端坐标系原点自身在基系中的位置时，取 $^n\mathbf{p} = \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^\top$，此时 $^0\mathbf{T}_n$ 的第 4 列恰好就是末端原点的基坐标 $\mathbf{P}$。

📐 正运动学通用计算标准管线