1. 建立六轴机械臂DH参数模型

任务目标

根据给定的六轴机械臂DH参数表，建立标准DH（SDH）正向运动学模型，输入各关节角度后输出末端位姿矩阵，并通过一组已知关节角与末端坐标的对应关系验证模型正确性。

DH参数表

实现代码（Python + NumPy）

import numpy as np

def dh_transform(a, alpha, d, theta):
    """标准DH法单关节齐次变换矩阵"""
    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]
    ])

def forward_kinematics(joint_angles_deg):
    """输入六关节角度(度)，返回末端齐次变换矩阵"""
    # DH参数: [a, alpha, d, theta_offset]
    dh_params = [
        [0,   0,        150, 0],
        [50,  -np.pi/2, 0,   0],
        [300, 0,        0,   0],
        [80,  -np.pi/2, 280, 0],
        [0,   np.pi/2,  0,   0],
        [0,   -np.pi/2, 70,  0]
    ]
    T = np.eye(4)
    for i, (a, alpha, d, offset) in enumerate(dh_params):
        theta = np.deg2rad(joint_angles_deg[i]) + offset
        T = T @ dh_transform(a, alpha, d, theta)
    return T

# 验证用例：全部关节角为0°
angles = [0, 0, 0, 0, 0, 0]
T = forward_kinematics(angles)

# 提取位置与姿态
position = T[:3, 3]
print("末端位置 (mm):", position)
print("末端姿态矩阵:")
print(np.round(T[:3, :3], 6))

运行方式

1. 安装依赖：pip install numpy
2. 将代码保存为 dh_model.py，执行 python dh_model.py

验证方法与判定标准

当所有关节角为0°时，根据DH参数逐关节推导：

• 关节1~3使末端到达位置 (350, 0, 150)：a₂=300 与 a₃=80 沿X方向延伸，d₁=150 抬升高度
• 关节4~6的旋转不改变末端位置（d₄=280 沿Z₄方向，但该方向与末端位置计算无关）

预期输出：

末端位置 (mm): [350. 0. 150.]
末端姿态矩阵:
[[1. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]]

通过条件：程序输出的位置与姿态矩阵和上述预期一致（数值误差在1e-6以内）。若一致，说明DH参数建模与正解计算正确。

常见问题

• 角度单位：代码内部使用弧度计算，对外接口接收角度值并自动转换。
• DH约定：本案例采用标准DH（SDH），若使用改进DH（MDH）需调整变换矩阵形式。
• 符号约定：α的正负方向遵循右手定则，表中负值表示绕X轴反向旋转。