PythonAI算法
六轴机械臂正运动学建模与 Python 实现
在机器人运动学建模领域，D-H（Denavit-Hartenberg）参数法是核心技术。它以 4 个参数清晰描述机械臂各连杆间的相对位姿关系，是实现正运动学求解、轨迹规划的基础。将从理论原理出发，拆解六轴机械臂的 D-H 法建模流程，并结合代码实现让理论落地。 一、为什么选择 D-H 法？ 六轴机械臂作为工业场景中最常用的机器人构型，其连杆与关节的空间关系复杂。如果直接用三维坐标系叠加计算，不…
在机器人运动学建模领域，D-H（Denavit-Hartenberg）参数法是核心技术。它以 4 个参数清晰描述机械臂各连杆间的相对位姿关系，是实现正运动学求解、轨迹规划的基础。将从理论原理出发，拆解六轴机械臂的 D-H 法建模流程，并结合代码实现让理论落地。 一、为什么选择 D-H 法？ 六轴机械臂作为工业场景中最常用的机器人构型，其连杆与关节的空间关系复杂。如果直接用三维坐标系叠加计算，不…
在机器人运动学建模领域，D-H（Denavit-Hartenberg）参数法是核心技术。它以 4 个参数清晰描述机械臂各连杆间的相对位姿关系，是实现正运动学求解、轨迹规划的基础。本文将从理论原理出发，拆解六轴机械臂的 D-H 法建模流程，并结合代码实现让理论落地。
六轴机械臂作为工业场景中最常用的机器人构型，其连杆与关节的空间关系复杂。如果直接用三维坐标系叠加计算，不仅公式繁琐，还容易出现坐标混乱的问题。而 D-H 法的核心优势在于'标准化'：
掌握 D-H 法即掌握串联机械臂建模的核心。
在 D-H 法中，我们需要为机械臂的每个连杆和关节建立坐标系（通常称为'D-H 坐标系'），然后通过 4 个参数描述相邻坐标系间的变换关系，最终通过矩阵乘法得到整体变换。
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 轴，四步完成坐标变换。
通过上述 4 个参数，我们可以构建出从第 i-1 个坐标系到第 i 个坐标系的齐次变换矩阵ⁱᵢ₋₁T，这个矩阵既包含了旋转变换，也包含了平移变换，公式如下：
矩阵的前 3 行 3 列是旋转矩阵，描述坐标系 i 相对于坐标系 i-1 的姿态；前 3 行第 4 列是平移向量，描述坐标系 i 原点相对于坐标系 i-1 的位置；最后一行是齐次坐标的标准形式（0,0,0,1）。
对于六轴机械臂，我们需要依次建立基坐标系（0 号）、关节 16 的坐标系（16 号）、末端执行器坐标系（6 号，与关节 6 坐标系重合或固定偏移）。末端执行器相对于基坐标系的总变换矩阵⁶₀T，就是各相邻连杆变换矩阵的乘积：
通过这个总变换矩阵，我们可以直接提取末端执行器的位置（前 3 行第 4 列）和姿态（前 3 行 3 列旋转矩阵），这就是正运动学的核心目标。
理论讲完，我们用一个通用六轴工业机械臂（类似 UR5、PUMA560 构型）进行实战建模，步骤分为'建立坐标系→填写 D-H 参数表→计算总变换矩阵'三步。
建立坐标系是 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 轴方向与机械臂连杆延伸方向相反，不影响计算，只需严格按照坐标系定义填写即可。
根据参数表，我们先将θᵢ转换为弧度，再依次计算每个连杆的变换矩阵（¹₀T、²₁T...⁶₅T），最后将 6 个矩阵相乘得到总变换矩阵⁶₀T。
例如，当所有关节角为 0°时，代入参数计算得到的总变换矩阵中，前 3 行第 4 列即为末端执行器的位置（x,y,z），前 3 行 3 列即为末端姿态的旋转矩阵。通过这个结果，我们可以验证建模的正确性（比如零位时末端位置是否与机械臂物理结构一致）。
理论需结合实践，我们用 Python 结合 NumPy 实现 D-H 建模与正运动学求解，同时加入 Matplotlib 可视化，直观看到机械臂姿态。
首先安装必要的库（矩阵运算 +3D 可视化）：
pip install numpy matplotlib
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: 末端旋转矩阵
"""
q1, q2, q3, q4, q5, q6 = joint_angles
offset = [math.pi / , -math.pi / , , -math.pi / , , ]
theta1 = q1 + offset[]
theta2 = q2 + offset[]
theta3 = q3 + offset[]
theta4 = q4 + offset[]
theta5 = q5 + offset[]
theta6 = q6 + offset[]
d = [, , , , , ]
a = [, -, -, , , ]
alpha = [math.pi / , , , math.pi / , -math.pi / , ]
T1 = dh_transform(theta1, d[], a[], alpha[])
T2 = dh_transform(theta2, d[], a[], alpha[])
T3 = dh_transform(theta3, d[], a[], alpha[])
T4 = dh_transform(theta4, d[], a[], alpha[])
T5 = dh_transform(theta5, d[], a[], alpha[])
T6 = dh_transform(theta6, d[], a[], alpha[])
T0_0 = np.eye()
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
joint_positions = [
T0_0[:, ],
T0_1[:, ],
T0_2[:, ],
T0_3[:, ],
T0_4[:, ],
T0_5[:, ],
T0_6[:, ]
]
T0_6, joint_positions, T0_6[:, :]
():
T_total, joint_positions, end_rot = your_arm_forward_kinematics(joint_angles)
x = [pos[] pos joint_positions]
y = [pos[] pos joint_positions]
z = [pos[] pos joint_positions]
fig = plt.figure(figsize=(, ))
ax = fig.add_subplot(, projection=)
ax.plot(x, y, z, , linewidth=, label=)
ax.scatter(x, y, z, c=, s=, label=, marker=)
ax.scatter(x[-], y[-], z[-], c=, s=, label=, marker=)
ax.set_xlabel(, fontsize=)
ax.set_ylabel(, fontsize=)
ax.set_zlabel(, fontsize=)
ax.set_title(title, fontsize=, fontweight=)
ax.legend(loc=, fontsize=)
ax.set_xlim([-, ])
ax.set_ylim([-, ])
ax.set_zlim([, ])
( * )
()
()
()
(np.(end_rot, ))
():
sy = math.sqrt(rot[, ] ** + rot[, ] ** )
singular = sy <
singular:
roll = math.atan2(rot[, ], rot[, ])
pitch = math.atan2(-rot[, ], sy)
yaw = math.atan2(rot[, ], rot[, ])
:
roll = math.atan2(-rot[, ], rot[, ])
pitch = math.atan2(-rot[, ], sy)
yaw =
[rad2deg(roll), rad2deg(pitch), rad2deg(yaw)]
rpy = rot2euler(end_rot)
()
( * )
plt.show()
():
( * )
()
:
mode = ()
mode [, ]:
mode = (mode)
:
()
joint_angles_input = []
joint_names = [, , , , , ]
i, name (joint_names):
:
:
angle = (())
joint_angles_input.append(angle)
ValueError:
()
mode == :
joint_angles_rad = [deg2rad(angle) angle joint_angles_input]
()
()
:
joint_angles_rad = joint_angles_input
()
( * )
joint_angles_rad
__name__ == :
joint_angles = input_joint_angles()
visualize_your_arm(joint_angles, )
代码核心逻辑：用户输入 6 个关节角（角度制）→ 转换为弧度→ 计算各连杆 D-H 变换矩阵→ 得到总变换矩阵→ 提取关节位置并可视化。
运行后，将弹出 3D 可视化窗口，蓝色线段表示机械臂连杆，红色圆点表示关节，红色星号表示末端执行器；控制台将输出末端位置和旋转矩阵，方便验证建模结果。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
解析常见 curl 参数并生成 fetch、axios、PHP curl 或 Python requests 示例代码。 在线工具，curl 转代码在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online