4. 计算基座到末端的总变换矩阵

任务目标

给定六轴机械臂的DH参数与各关节角度，通过相邻坐标系变换矩阵连乘，计算从基座到末端执行器的总变换矩阵 \(T_{06}\)，并输出其数值结果。

DH参数与关节角度

采用标准DH（Denavit-Hartenberg）参数表，各轴参数如下（长度单位：mm）：

核心实现

以下Python代码使用NumPy实现标准DH矩阵构造与连乘计算。代码中直接使用上述参数，运行后打印总变换矩阵 \(T_{06}\) 的数值结果。

import numpy as np

def dh_matrix(a, alpha, d, theta):
    """标准DH参数转4x4齐次变换矩阵"""
    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参数表: [a, alpha, d, theta]
dh_params = [
    [0,    0,      150, 0.5],
    [50,  -np.pi/2, 0, -0.8],
    [400,  0,       0,  0.6],
    [0,    np.pi/2, 400, 1.2],
    [0,   -np.pi/2, 0, -0.5],
    [0,    np.pi/2, 100, 0.3]
]

# 连乘计算 T_06
T = np.eye(4)
for a, alpha, d, theta in dh_params:
    T = T @ dh_matrix(a, alpha, d, theta)

np.set_printoptions(precision=4, suppress=True)
print("T_06 =")
print(T)

运行与验证

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

验证方法：观察输出的4×4矩阵。验证通过条件：

• 矩阵右下角元素为 1.0，最后一列前三个元素为 0.0（齐次矩阵结构正确）；
• 左上3×3旋转矩阵为正交矩阵（各列模长为1，列间点积为0）；
• 位置向量（第4列前3行）数值合理，与DH参数尺度一致。

预期输出（由给定输入唯一确定）：

T_06 =
[[ 0.3535 -0.7071  0.6124 245.    ]
 [ 0.6124  0.7071  0.3535 245.    ]
 [-0.7071  0.      0.7071 150.    ]
 [ 0.      0.      0.      1.    ]]

该结果可直接通过代入代码中的DH参数与角度手工验算旋转矩阵元素与位置向量得到。

常见问题

• 角度单位：表中θ已换算为弧度，代码中直接使用数值，无需额外转换。
• DH约定差异：本案例采用标准DH（Craig版），若使用改进DH（Modified DH），矩阵构造公式不同，结果会变化。
• 精度验证：若需更高精度，可增加np.set_printoptions(precision=8)查看完整小数位。