六轴机械臂正运动学：D-H 参数法建模与代码实现

$$ T_0^6 = T_0^1 \cdot T_1^2 \cdot T_2^3 \cdot T_3^4 \cdot T_4^5 \cdot T_5^6 $$

通过这个总变换矩阵，我们可以直接提取末端执行器的位置（前 3 行第 4 列）和姿态（前 3 行 3 列旋转矩阵），这就是正运动学的核心目标。

三、六轴机械臂 D-H 建模实战：以通用工业机械臂为例

理论讲完，我们用一个通用六轴工业机械臂（类似 UR5、PUMA560 构型）进行实战建模，步骤分为'建立坐标系→填写 D-H 参数表→计算总变换矩阵'三步。

1. 第一步：为机械臂建立 D-H 坐标系

建立坐标系是 D-H 建模的关键，需遵循严格的规则（避免参数混乱）：

1. z 轴：与关节 i 的旋转轴（或移动轴）重合，方向任意（建议统一向上，方便后续计算）；
2. x 轴：沿两个相邻 z 轴（zᵢ₋₁和 zᵢ）的公垂线方向，指向从 zᵢ₋₁到 zᵢ的方向；若 zᵢ₋₁与 zᵢ平行，x 轴垂直于 z 轴且指向机械臂前方；
3. y 轴：由右手定则确定（x×y=z），无需手动定义；
4. 原点：xᵢ轴与 zᵢ₋₁轴的交点；若 xᵢ轴与 zᵢ₋₁轴不相交，原点取 zᵢ₋₁轴上的任意点（建议取关节中心，简化参数）。

按照上述规则，为六轴机械臂的每个关节建立坐标系后，我们就能直观看到各坐标系的相对位置，为填写参数表做准备。

2. 第二步：填写 D-H 参数表

以下示例基于自定义机械臂模型，根据建立的坐标系，结合机械臂的物理结构尺寸，我们可以填写出完整的 D-H 参数表。

在实际研发中，我们常常需要根据自定义机械臂的结构建立参数表。例如，此自定义六轴机械臂的 D-H 参数表（含关节角零位偏移）如下：

注意事项： 注：a₂和 a₃为负值，是因为 x 轴方向与机械臂连杆延伸方向相反，不影响计算，只需严格按照坐标系定义填写即可。

3. 第三步：计算总变换矩阵与末端位姿

根据参数表，我们先将θᵢ转换为弧度，再依次计算每个连杆的变换矩阵（T₀¹、T₁²...T₅⁶），最后将 6 个矩阵相乘得到总变换矩阵 T₀⁶。

例如，当所有关节角为 0°时，代入参数计算得到的总变换矩阵中，前 3 行第 4 列即为末端执行器的位置（x,y,z），前 3 行 3 列即为末端姿态的旋转矩阵。通过这个结果，我们可以验证建模的正确性（比如零位时末端位置是否与机械臂物理结构一致）。

四、代码实现：用 Python 验证 D-H 建模结果

光有理论不够，我们用 Python 结合 NumPy 实现 D-H 建模与正运动学求解，同时加入 Matplotlib 可视化，直观看到机械臂姿态。

1. 依赖安装

首先安装必要的库（矩阵运算 +3D 可视化）：

pip install numpy matplotlib

2. 完整代码（含可视化）

import numpy as np
import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import matplotlib as mpl

# ====================== 解决 Matplotlib 中文显示问题 ======================
plt.rcParams["font.family"] = ["SimHei", "PingFang SC", "WenQuanYi Micro Hei", "DejaVu Sans"]
plt.rcParams['axes.unicode_minus'] = False
mpl.rcParams['font.sans-serif'] = plt.rcParams["font.family"]

# 角度转弧度
def deg2rad(deg):
    return deg * math.pi / 180

# 弧度转角度
def rad2deg(rad):
    return rad * 180 / math.pi

# 计算单个连杆的 D-H 变换矩阵（标准 D-H）
def dh_transform(theta, d, a, alpha):
    T = np.array([
        [math.cos(theta), -math.sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha), a * math.cos(theta)],
        [math.sin(theta), math.cos(theta) * math.cos(alpha), -math.cos(theta) * math.sin(alpha), a * math.sin(theta)],
        [0, math.sin(alpha), math.cos(alpha), d],
        [0, 0, 0, 1]
    ])
    return T

# 机械臂正运动学求解（含 offset）
def your_arm_forward_kinematics(joint_angles):
    """
    参数：
    joint_angles: 列表，6 个关节角（弧度）[q1, q2, q3, q4, q5, q6]
    返回：
    T_total: 末端总变换矩阵
    joint_positions: 各关节（0-6）的坐标列表
    end_rotation: 末端旋转矩阵
    """
    # 提取关节角（叠加 offset）
    q1, q2, q3, q4, q5, q6 = joint_angles
    offset = [math.pi / 2, -math.pi / 2, 0, -math.pi / 2, 0, 0]  # 关节角偏移
    theta1 = q1 + offset[0]
    theta2 = q2 + offset[1]
    theta3 = q3 + offset[2]
    theta4 = q4 + offset[3]
    theta5 = q5 + offset[4]
    theta6 = q6 + offset[5]

    # 机械臂 D-H 参数（根据你的参数表定义）
    d = [540, 0, 0, 500, 345, 175]  # 连杆偏移 d_i
    a = [0, -900, -900, 0, 0, 0]  # 连杆长度 a_i
    alpha = [math.pi / 2, 0, 0, math.pi / 2, -math.pi / 2, 0]  # 连杆扭转角α_i

    # 计算各连杆变换矩阵
    T1 = dh_transform(theta1, d[0], a[0], alpha[0])
    T2 = dh_transform(theta2, d[1], a[1], alpha[1])
    T3 = dh_transform(theta3, d[2], a[2], alpha[2])
    T4 = dh_transform(theta4, d[3], a[3], alpha[3])
    T5 = dh_transform(theta5, d[4], a[4], alpha[4])
    T6 = dh_transform(theta6, d[5], a[5], alpha[5])

    # 各关节相对于基坐标系的变换矩阵
    T0_0 = np.eye(4)  # 基坐标系
    T0_1 = T0_0 @ T1
    T0_2 = T0_1 @ T2
    T0_3 = T0_2 @ T3
    T0_4 = T0_3 @ T4
    T0_5 = T0_4 @ T5
    T0_6 = T0_5 @ T6

    # 末端坐标系
    # 提取各关节位置（x,y,z）
    joint_positions = [
        T0_0[:3, 3],
        T0_1[:3, 3],
        T0_2[:3, 3],
        T0_3[:3, 3],
        T0_4[:3, 3],
        T0_5[:3, 3],
        T0_6[:3, 3]
    ]
    return T0_6, joint_positions, T0_6[:3, :3]

# 机械臂 3D 可视化函数
def visualize_your_arm(joint_angles, title="六轴机械臂正运动学可视化"):
    # 获取关节位置
    T_total, joint_positions, end_rot = your_arm_forward_kinematics(joint_angles)
    x = [pos[0] for pos in joint_positions]
    y = [pos[1] for pos in joint_positions]
    z = [pos[2] for pos in joint_positions]

    # 创建 3D 绘图
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection='3d')

    # 绘制连杆、关节、末端
    ax.plot(x, y, z, 'b-', linewidth=3, label='机械臂连杆')
    ax.scatter(x, y, z, c='r', s=100, label='关节', marker='o')
    ax.scatter(x[-1], y[-1], z[-1], c='red', s=300, label='末端执行器', marker='*')

    # 坐标轴与标题（中文）
    ax.set_xlabel('X 轴 (mm)', fontsize=12)
    ax.set_ylabel('Y 轴 (mm)', fontsize=12)
    ax.set_zlabel('Z 轴 (mm)', fontsize=12)
    ax.set_title(title, fontsize=14, fontweight='bold')
    ax.legend(loc='upper left', fontsize=10)

    # 调整坐标轴范围（适配机械臂尺寸）
    ax.set_xlim([-2000, 2000])
    ax.set_ylim([-2000, 2000])
    ax.set_zlim([0, 2000])

    # 打印末端位姿
    print("=" * 60)
    print("机械臂末端位姿信息：")
    print(f"末端位置 (x, y, z)：({x[-1]:.2f}, {y[-1]:.2f}, {z[-1]:.2f}) 毫米")
    print("末端旋转矩阵：")
    print(np.round(end_rot, 3))

    # 旋转矩阵转欧拉角（RPY）
    def rot2euler(rot):
        sy = math.sqrt(rot[0, 0] ** 2 + rot[1, 0] ** 2)
        singular = sy < 1e-6
        if not singular:
            roll = math.atan2(rot[2, 1], rot[2, 2])
            pitch = math.atan2(-rot[2, 0], sy)
            yaw = math.atan2(rot[1, 0], rot[0, 0])
        else:
            roll = math.atan2(-rot[1, 2], rot[1, 1])
            pitch = math.atan2(-rot[2, 0], sy)
            yaw = 0
        return [rad2deg(roll), rad2deg(pitch), rad2deg(yaw)]

    rpy = rot2euler(end_rot)
    print(f"末端姿态（横滚角 Roll, 俯仰角 Pitch, 偏航角 Yaw）：({rpy[0]:.2f}, {rpy[1]:.2f}, {rpy[2]:.2f}) 度")
    print("=" * 60)
    plt.show()

# 自定义输入关节角（支持角度/弧度输入）
def input_joint_angles():
    """让用户选择输入模式（角度/弧度），并输入 6 个关节角，返回弧度制的关节角列表"""
    print("=" * 60)
    print("欢迎输入六轴机械臂的关节角！")
    # 选择输入模式
    while True:
        mode = input("请选择输入模式（1=角度，2=弧度）：")
        if mode in ["1", "2"]:
            mode = int(mode)
            break
        else:
            print("输入错误！请输入 1 或 2。")

    # 输入 6 个关节角
    joint_angles_input = []
    joint_names = ["关节 1 (q1)", "关节 2 (q2)", "关节 3 (q3)", "关节 4 (q4)", "关节 5 (q5)", "关节 6 (q6)"]
    for i, name in enumerate(joint_names):
        while True:
            try:
                angle = float(input(f"请输入{name}的角度值："))
                joint_angles_input.append(angle)
                break
            except ValueError:
                print("输入错误！请输入数字（整数/小数均可）。")

    # 转换为弧度（如果是角度输入）
    if mode == 1:
        joint_angles_rad = [deg2rad(angle) for angle in joint_angles_input]
        print(f"\n你输入的角度（角度制）：{[round(angle, 2) for angle in joint_angles_input]}")
        print(f"转换为弧度制：{[round(rad, 4) for rad in joint_angles_rad]}")
    else:
        joint_angles_rad = joint_angles_input
        print(f"\n你输入的角度（弧度制）：{[round(rad, 4) for rad in joint_angles_rad]}")
    print("=" * 60)
    return joint_angles_rad

# 主函数
if __name__ == "__main__":
    # 自定义输入关节角
    joint_angles = input_joint_angles()
    # 可视化机械臂
    visualize_your_arm(joint_angles, "自定义关节角的六轴机械臂姿态")

3. 代码说明与运行效果

代码核心逻辑：用户输入 6 个关节角（角度制）→ 转换为弧度→ 计算各连杆 D-H 变换矩阵→ 得到总变换矩阵→ 提取关节位置并可视化。

运行后，会弹出 3D 可视化窗口，蓝色线段为机械臂连杆，红色圆点为关节，红色星号为末端执行器；控制台会输出末端位置和旋转矩阵，方便验证建模结果。