机械臂正解（MDH）推导过程及python示例

一、Modified DH 参数简介（Khalil-Breteler 版本）

Modified DH 参数（MDH）与标准 Denavit-Hartenberg 参数（SDH）不同，它的连杆坐标系是按照如下规则建立的：

1. $z_{i-1}$ 轴：沿第 $i-1$ 个关节轴方向。
2. $x_i$ 轴：沿从 $z_{i-1}$ 到 $z_i$ 的公垂线方向。
3. 原点：位于 $x_i$ 与 $z_{i-1}$ 的交点。
4. 参数定义如下：

参数	含义
${\theta }_i$	绕 $z_{i-1}$ 轴旋转，从 $x_{i-1}$ 到 $x_i$ 的角度
$d_i$	沿 $z_{i-1}$ 轴从 $x_{i-1}$ 到 $x_i$ 的平移
$a_i$	沿 $x_i$ 轴从 $z_{i-1}$ 到 $z_i$ 的平移
${\alpha }_i$	绕 $x_i$ 轴旋转，从 $z_{i-1}$ 到 $z_i$ 的角度

---

二、MDH 变换的分解

MDH 的变换矩阵可以分解为如下四个基本变换步骤（按顺序）：

1. 绕 $z_{i-1}$ 轴旋转 ${\theta }_i$
   $$\text{Rot}(z_{i-1},{\theta }_i)$$

2. 沿 $z_{i-1}$ 轴平移 $d_i$
   $$\text{Trans}(z_{i-1},d_i)$$

3. 绕 $x_i$ 轴旋转 ${\alpha }_i$
   $$\text{Rot}(x_i,{\alpha }_i)$$

4. 沿 $x_i$ 轴平移 $a_i$
   $$\text{Trans}(x_i,a_i)$$

注意：这里的顺序是 Khalil-Breteler 版本的 MDH 定义。

---

三、逐个写出变换矩阵

1. 绕 $z_{i-1}$ 轴旋转 ${\theta }_i$

$$\text{Rot}(z,\theta )=\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& 0\\ \sin \theta & \cos \theta & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

2. 沿 $z_{i-1}$ 轴平移 $d_i$

$$\text{Trans}(z,d)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d\\ 0& 0& 0& 1\end{array}\right]$$

3. 绕 $x_i$ 轴旋转 ${\alpha }_i$

$$\text{Rot}(x,\alpha )=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \alpha & -\sin \alpha & 0\\ 0& \sin \alpha & \cos \alpha & 0\\ 0& 0& 0& 1\end{array}\right]$$

4. 沿 $x_i$ 轴平移 $a_i$

$$\text{Trans}(x,a)=\left[\begin{array}{cccc}1& 0& 0& a\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

---

四、矩阵乘法合成变换矩阵

将以上四个变换按顺序相乘：

$$T_i^{i-1}=\text{Trans}(x,a)\cdot \text{Rot}(x,\alpha )\cdot \text{Trans}(z,d)\cdot \text{Rot}(z,\theta )$$

逐个相乘：

步骤 1：先乘 Rot(z) 和 Trans(z)

$$T_{12}=\text{Trans}(z,d)\cdot \text{Rot}(z,\theta )=\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& 0\\ \sin \theta & \cos \theta & 0& 0\\ 0& 0& 1& d\\ 0& 0& 0& 1\end{array}\right]$$

步骤 2：乘 Rot(x, α)

$$T_{123}=\text{Rot}(x,\alpha )\cdot T_{12}=\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& 0\\ \sin \theta \cos \alpha & \cos \theta \cos \alpha & -\sin \alpha & -d\sin \alpha \\ \sin \theta \sin \alpha & \cos \theta \sin \alpha & \cos \alpha & d\cos \alpha \\ 0& 0& 0& 1\end{array}\right]$$

步骤 3：乘 Trans(x, a)

$$T_i^{i-1}=\text{Trans}(x,a)\cdot T_{123}=\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& a\\ \sin \theta \cos \alpha & \cos \theta \cos \alpha & -\sin \alpha & -d\sin \alpha \\ \sin \theta \sin \alpha & \cos \theta \sin \alpha & \cos \alpha & d\cos \alpha \\ 0& 0& 0& 1\end{array}\right]$$

---

五、最终 MDH 矩阵表达式

这就是你函数中返回的变换矩阵：

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

---

六、Python 实现

import numpy as np
from typing import List, Tuple, Optional
from dataclasses import dataclass

def mdh_matrix_khalil(alpha: float, a: float, d: float, theta: float) -> np.ndarray:
    """
    计算 Khalil-Breteler 版本的 Modified DH 变换矩阵。
    参数:
        alpha: 扭转角 (弧度)
        a: 连杆长度
        d: 偏移量
        theta: 关节角 (弧度)
    返回:
        T: 4x4 齐次变换矩阵
    """
    ca, sa = np.cos(alpha), np.sin(alpha)
    ct, st = np.cos(theta), np.sin(theta)

    return np.array([
        [ct,       -st,        0,         a        ],
        [st * ca,  ct * ca,  -sa,       -sa * d    ],
        [st * sa,  ct * sa,   ca,        ca * d    ],
        [0,         0,        0,         1         ]
    ], dtype=np.float64)


def forward_kinematics_mdh_khalil(
    dh_params: List[Tuple[float, float, float, float]],
    joint_angles: List[float]
) -> List[np.ndarray]:
    """
    使用 Khalil 版本的 Modified DH 参数计算正向运动学。
    返回所有中间关节的变换矩阵。
    
    参数:
        dh_params: 每个连杆的 MDH 参数列表 [(alpha, a, d, theta_offset), ...]
        joint_angles: 各关节角度列表 [theta1, theta2, ...]
    返回:
        transforms: 每个关节相对于基坐标系的变换矩阵列表
    """
    if len(dh_params) != len(joint_angles):
        raise ValueError("DH 参数与关节角度数量不一致")

    T = np.eye(4)
    transforms = [T.copy()]  # 初始为基坐标系 {0}

    for i, (alpha, a, d, theta_offset) in enumerate(dh_params):
        theta = theta_offset + joint_angles[i]  # 添加关节角度
        T_i = mdh_matrix_khalil(alpha, a, d, theta)
        T = T @ T_i  # 累乘变换矩阵
        transforms.append(T)

    return transforms


# ======================
# 示例部分
# ======================
if __name__ == "__main__":
    np.set_printoptions(precision=4, suppress=True)

    # # 示例 DH 参数（3自由度机械臂，MDH格式）
    # dh_params = [
    #     (0, 0, 0, 0),     # 连杆1
    #     (0, 1, 0, 0),     # 连杆2
    #     (0, 1, 0, 0),      # 连杆3
    #     (0, 1, 0, 0)      # 连杆3
    # ]

    # # 关节角度输入（弧度值）
    # joint_angles = [np.pi/2, np.pi/2, np.pi/2,-np.pi/2]
        # 示例 DH 参数（3自由度机械臂，MDH格式）
    dh_params = [
        (0, 0, 265, 0),     # 连杆1
        (-np.pi/2, 0, 0, 0),     # 连杆2
        (0, 444, 0, 0)      # 连杆3
        # (-np.pi/2, 110, 470, 0)      # 连杆3
    ]

    # 关节角度输入（弧度值）
    joint_angles = [0, -np.pi/2, 0]
    

    # 计算所有关节的变换矩阵
    transforms = forward_kinematics_mdh_khalil(dh_params, joint_angles)

    # 提取末端执行器（最后一个关节）的变换矩阵
    T_end_effector = transforms[-1]

    # 提取位置和姿态
    position = T_end_effector[:3, 3]
    x, y, z = position
    rotation_matrix = T_end_effector[:3, :3]

    print("末端执行器在基坐标系 x0y0z0 下的位姿：")
    print(f"\n位置 (x, y, z)： x={x:.4f}, y={y:.4f}, z={z:.4f}")
    print("\n姿态（旋转矩阵 R）：")
    print(rotation_matrix)

注意：dh_params 为MDH的参数，joint_angles为各个关节的角度(弧度)。