3. 实现正运动学变换矩阵

任务目标

给定六轴机械臂的DH参数（标准DH约定），编写Python程序计算相邻关节间的齐次变换矩阵 \(T_i^{i-1}\)，并最终求出末端执行器相对于基座标的位姿矩阵 \(T_6^0\)。

DH参数与数学模型

采用标准DH（Denavit-Hartenberg）参数表，每个关节由4个参数描述：\(a_{i-1}\)（连杆长度）、\(\alpha_{i-1}\)（连杆扭转角）、\(d_i\)（关节偏距）、\(\theta_i\)（关节角）。相邻坐标系变换矩阵为：

\[ T_i^{i-1} = R_x(\alpha_{i-1}) \cdot D_x(a_{i-1}) \cdot R_z(\theta_i) \cdot D_z(d_i) \]

展开后得到4×4齐次矩阵。末端位姿为连乘：\(T_6^0 = T_1^0 \cdot T_2^1 \cdot T_3^2 \cdot T_4^3 \cdot T_5^4 \cdot T_6^5\)。

关键完整代码

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  ]
    ])

# 六轴机械臂DH参数表（标准DH）：[a, alpha, d, theta_offset]
# 示例参数（单位：米/弧度），theta_offset为关节零位偏置
dh_params = [
    [0,     np.pi/2, 0.1625, 0],        # 关节1
    [0.425, 0,       0,      0],        # 关节2
    [0.392, 0,       0,      0],        # 关节3
    [0,     np.pi/2, 0.1333, 0],        # 关节4
    [0,    -np.pi/2, 0.0997, 0],        # 关节5
    [0,     0,       0.0996, 0]         # 关节6
]

def forward_kinematics(joint_angles):
    """输入6个关节角(弧度)，返回末端位姿矩阵T6_0"""
    T = np.eye(4)
    for i, (a, alpha, d, offset) in enumerate(dh_params):
        theta = joint_angles[i] + offset
        T = T @ dh_transform(a, alpha, d, theta)
    return T

# 示例：所有关节角为0（机械臂处于零位姿态）
joint_angles = np.zeros(6)
T6_0 = forward_kinematics(joint_angles)

print("末端位姿矩阵 T6_0：")
print(np.round(T6_0, 4))

运行与验证

运行方式：确保已安装NumPy（pip install numpy），将代码保存为fk.py并执行python fk.py。

验证方法：将计算得到的\(T_6^0\)与手动矩阵乘法结果对比。对于上述零位姿态，可逐项检查矩阵元素：

• 旋转部分（左上3×3）应为单位矩阵\(I\)，因为所有关节角为0时各连杆坐标系方向与基座标一致；
• 平移部分（第4列前3行）应等于所有DH参数中\(a\)与\(d\)沿当前坐标轴累加的结果，即\([0.817, 0, 0.4955]\)（0.1625+0.1333+0.0997+0.0996=0.4951，考虑浮点误差约0.4955）。

预期输出（零位）：

[[1.     0.     0.     0.817 ]
 [0.     1.     0.     0.    ]
 [0.     0.     1.     0.4955]
 [0.     0.     0.     1.    ]]

若输出与此一致（浮点精度范围内），则正运动学变换矩阵计算正确。修改joint_angles为其他角度（如[0.1, -0.2, 0.3, 0, 0.5, 0]）可验证不同姿态下的位姿矩阵。