在机器人运动学建模领域，D-H（Denavit-Hartenberg）参数法是核心技术。它以 4 个参数描述机械臂各连杆间的相对位姿关系，是实现正运动学求解、轨迹规划的基础。
一、D-H 法优势
六轴机械臂作为工业场景中最常用的机器人构型，其连杆与关节的空间关系复杂。D-H 法的核心优势在于'标准化'：
- 简化参数：用仅 4 个参数（关节角、连杆偏移、连杆长度、连杆扭转角）描述相邻连杆的位姿，替代复杂的三维坐标变换；
- 通用性强：适用于所有串联机械臂，无论是六轴、四轴还是协作机械臂，都能套用同一套建模逻辑；
- 计算高效：通过齐次变换矩阵的乘积，可快速求解末端执行器相对于基坐标系的位姿，为后续运动学分析奠定基础。
二、D-H 法核心参数与变换矩阵
在 D-H 法中，我们需要为机械臂的每个连杆和关节建立坐标系（通常称为'D-H 坐标系'），然后通过 4 个参数描述相邻坐标系间的变换关系，最终通过矩阵乘法得到整体变换。
1. 核心参数定义
D-H 法的 4 个参数是基于相邻两个关节坐标系（第 i-1 个关节坐标系和第 i 个关节坐标系）定义的，核心逻辑是'从坐标系 i-1 到坐标系 i 的变换需要 4 个步骤'，每个步骤对应一个参数：
| 参数符号 | 参数名称 | 核心定义 | 补充说明 |
|---|---|---|---|
| θᵢ（Theta） | 关节角 | 绕 zᵢ₋₁轴旋转，使 xᵢ₋₁轴与 xᵢ轴平行的角度 | 旋转关节为变量（随关节运动变化），移动关节为常量 |
| dᵢ（d） | 连杆偏移 | 沿 zᵢ₋₁轴移动，使 xᵢ₋₁轴与 xᵢ轴重合的距离 | 移动关节为变量，旋转关节为常量（机械臂结构固定值） |
| aᵢ（a） | 连杆长度 | 沿 xᵢ轴移动，使 zᵢ₋₁轴与 zᵢ轴重合的距离 | 由机械臂连杆物理结构决定，为固定值 |
| αᵢ（Alpha） | 连杆扭转角 | 绕 xᵢ轴旋转，使 zᵢ₋₁轴与 zᵢ轴平行的角度 | 由连杆的几何形状决定，为固定值 |
关键提醒：所有参数的定义都基于'相邻两个坐标系'，核心是'让两个坐标系逐步重合'——先旋转 z 轴，再平移 z 轴，接着平移 x 轴，最后旋转 x 轴，四步完成坐标变换。
2. 齐次变换矩阵
通过上述 4 个参数，我们可以构建出从第 i-1 个坐标系到第 i 个坐标系的齐次变换矩阵ⁱᵢ₋₁T，这个矩阵既包含了旋转变换，也包含了平移变换，公式如下：
矩阵的前 3 行 3 列是旋转矩阵，描述坐标系 i 相对于坐标系 i-1 的姿态；前 3 行第 4 列是平移向量，描述坐标系 i 原点相对于坐标系 i-1 的位置；最后一行是齐次坐标的标准形式（0,0,0,1）。
3. 末端位姿求解
对于六轴机械臂，我们需要依次建立基坐标系（0 号）、关节 16 的坐标系（16 号）、末端执行器坐标系（6 号，与关节 6 坐标系重合或固定偏移）。末端执行器相对于基坐标系的总变换矩阵⁶₀T，就是各相邻连杆变换矩阵的乘积：
通过这个总变换矩阵，我们可以直接提取末端执行器的位置（前 3 行第 4 列）和姿态（前 3 行 3 列旋转矩阵），这就是正运动学的核心目标。
三、六轴机械臂 D-H 建模实战
理论讲完，我们用一个通用六轴工业机械臂（类似 UR5、PUMA560 构型）进行实战建模，步骤分为'建立坐标系→填写 D-H 参数表→计算总变换矩阵'三步。
1. 建立 D-H 坐标系
建立坐标系是 D-H 建模的关键，需遵循严格的规则（避免参数混乱）：
- z 轴：与关节 i 的旋转轴（或移动轴）重合，方向任意（建议统一向上，方便后续计算）；
- x 轴：沿两个相邻 z 轴（zᵢ₋₁和 zᵢ）的公垂线方向，指向从 zᵢ₋₁到 zᵢ的方向；若 zᵢ₋₁与 zᵢ平行，x 轴垂直于 z 轴且指向机械臂前方；
- y 轴：由右手定则确定（x×y=z），无需手动定义；
- 原点：xᵢ轴与 zᵢ₋₁轴的交点；若 xᵢ轴与 zᵢ₋₁轴不相交，原点取 zᵢ₋₁轴上的任意点（建议取关节中心，简化参数）。
按照上述规则，为六轴机械臂的每个关节建立坐标系后，我们就能直观看到各坐标系的相对位置，为填写参数表做准备。
2. 填写 D-H 参数表
例如某通用六轴机械臂模型，根据建立的坐标系，结合机械臂的物理结构尺寸，我们可以填写出完整的 D-H 参数表。
在实际研发中，需根据自定义机械臂的结构建立参数表。例如，此自定义六轴机械臂的 D-H 参数表（含关节角零位偏移）如下：
| 连杆 i | 关节角 qᵢ（°） | 连杆偏移 dᵢ（mm） | 连杆长度 aᵢ（mm） | 连杆扭转角 αᵢ（°） | 关节角偏移 offset（°） |
|---|---|---|---|---|---|
| 1 | q₁（变量） | 540 | 0 | 90 | 90 |
| 2 | q₂（变量） | 0 | -900 | 0 | -90 |
| 3 | q₃（变量） | 0 | -900 | 0 | 0 |
| 4 | q₄（变量） | 500 | 0 | 90 | -90 |
| 5 | q₅（变量） | 345 | 0 | -90 | 0 |
| 6 | q₆（变量） | 175 | 0 | 0 | 0 |
注意：a₂和 a₃为负值，是因为 x 轴方向与机械臂连杆延伸方向相反，不影响计算，只需严格按照坐标系定义填写即可。
3. 计算总变换矩阵
根据参数表，我们先将θᵢ转换为弧度，再依次计算每个连杆的变换矩阵（¹₀T、²₁T...⁶₅T），最后将 6 个矩阵相乘得到总变换矩阵⁶₀T。
例如，当所有关节角为 0°时，代入参数计算得到的总变换矩阵中，前 3 行第 4 列即为末端执行器的位置（x,y,z），前 3 行 3 列即为末端姿态的旋转矩阵。通过这个结果，我们可以验证建模的正确性（比如零位时末端位置是否与机械臂物理结构一致）。
四、Python 代码实现
我们用 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 可视化窗口，蓝色线段为机械臂连杆，红色圆点为关节，红色星号为末端执行器；控制台会输出末端位置和旋转矩阵，方便验证建模结果。
