用Python+Matplotlib动手验证：标准DH和改进DH建模的代码实现与可视化对比

PythonMatplotlib实战标准DH与改进DH建模的3D可视化对比机械臂运动学建模是机器人学中的核心课题而Denavit-HartenbergDH参数法则是其中最经典的建模方法之一。但很多初学者在接触标准DHsDH和改进DHmDH时常常被两者细微但关键的差异所困扰。本文将通过Python代码实现和3D可视化带您直观理解这两种方法的异同。1. 环境准备与基础概念在开始编码前我们需要准备好Python环境和必要的库。推荐使用Anaconda创建虚拟环境conda create -n robotics python3.8 conda activate robotics pip install numpy matplotlibDH参数法的核心思想是用四个参数θ, d, a, α来描述相邻连杆坐标系之间的变换关系。这两种方法的主要区别在于坐标系附着位置sDH坐标系附着在连杆的远端下一个关节mDH坐标系附着在连杆的近端当前关节参数顺序sDH(θ, d, a, α)mDH(α, a, θ, d)变换顺序sDH先绕Z轴旋转和平移再绕X轴旋转和平移mDH先绕X轴旋转和平移再绕Z轴旋转和平移2. 3自由度机械臂的DH参数设定我们以一个简单的3自由度平面机械臂为例其关节配置为旋转-旋转-旋转RRR。下面是两种方法的DH参数表关节sDH参数 (θ,d,a,α)mDH参数 (α,a,θ,d)1(θ₁, 0, L₁, 0)(0, L₁, θ₁, 0)2(θ₂, 0, L₂, 0)(0, L₂, θ₂, 0)3(θ₃, 0, L₃, 0)(0, L₃, θ₃, 0)虽然这个简单案例中参数看起来相似但它们的物理含义和变换顺序完全不同。让我们用代码来实现这两种方法的变换矩阵。3. 变换矩阵的Python实现首先定义通用的变换矩阵生成函数import numpy as np from math import cos, sin def sDH_transform_matrix(theta, d, a, alpha): 标准DH变换矩阵 return np.array([ [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta)], [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta)], [0, sin(alpha), cos(alpha), d], [0, 0, 0, 1] ]) def mDH_transform_matrix(alpha, a, theta, d): 改进DH变换矩阵 return np.array([ [cos(theta), -sin(theta), 0, a], [sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha)], [sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), d*cos(alpha)], [0, 0, 0, 1] ])接下来实现完整的正运动学计算def forward_kinematics_sDH(joint_angles, link_lengths): 标准DH正运动学计算 T np.eye(4) for i, (theta, L) in enumerate(zip(joint_angles, link_lengths)): T T sDH_transform_matrix(theta, 0, L, 0) return T def forward_kinematics_mDH(joint_angles, link_lengths): 改进DH正运动学计算 T np.eye(4) for i, (theta, L) in enumerate(zip(joint_angles, link_lengths)): T T mDH_transform_matrix(0, L, theta, 0) return T4. 3D可视化实现与对比分析使用Matplotlib进行3D可视化可以直观比较两种方法的差异import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D def plot_robot_3d(joint_angles, link_lengths): fig plt.figure(figsize(12, 6)) # 标准DH可视化 ax1 fig.add_subplot(121, projection3d) plot_dh_method(ax1, joint_angles, link_lengths, sDH) ax1.set_title(Standard DH Method) # 改进DH可视化 ax2 fig.add_subplot(122, projection3d) plot_dh_method(ax2, joint_angles, link_lengths, mDH) ax2.set_title(Modified DH Method) plt.tight_layout() plt.show() def plot_dh_method(ax, joint_angles, link_lengths, methodsDH): 绘制指定DH方法的机械臂 positions [np.array([0, 0, 0, 1])] T np.eye(4) for i, (theta, L) in enumerate(zip(joint_angles, link_lengths)): if method sDH: T T sDH_transform_matrix(theta, 0, L, 0) else: T T mDH_transform_matrix(0, L, theta, 0) positions.append(T np.array([0, 0, 0, 1])) # 绘制连杆 ax.plot([positions[i][0], positions[i1][0]], [positions[i][1], positions[i1][1]], [positions[i][2], positions[i1][2]], b-, linewidth3) # 绘制坐标系 draw_frame(ax, T) ax.set_xlim(-2, 2) ax.set_ylim(-2, 2) ax.set_zlim(0, 2) ax.set_xlabel(X) ax.set_ylabel(Y) ax.set_zlabel(Z) def draw_frame(ax, T, length0.2): 绘制坐标系 origin T[:3, 3] x_axis T[:3, 0] y_axis T[:3, 1] z_axis T[:3, 2] ax.quiver(*origin, *x_axis, colorr, lengthlength) ax.quiver(*origin, *y_axis, colorg, lengthlength) ax.quiver(*origin, *z_axis, colorb, lengthlength)运行可视化代码# 定义机械臂参数 joint_angles [np.pi/4, np.pi/6, -np.pi/8] # 三个关节的角度 link_lengths [1.0, 0.8, 0.5] # 三个连杆的长度 # 绘制对比图 plot_robot_3d(joint_angles, link_lengths)5. 实际应用中的选择建议虽然在这个简单案例中两种方法的结果看似相同但在更复杂的情况下会有显著差异闭环机构mDH更适合处理闭环机构因为它能避免坐标系重叠的问题树状结构sDH在处理树状结构的机械臂时更为直观计算效率两种方法在计算效率上差异不大但mDH的变换顺序有时能简化计算提示在实际项目中选择哪种DH方法通常取决于所使用的机器人库或框架的约定。例如ROS中的URDF通常采用mDH约定。对于想要进一步探索的读者可以尝试以下扩展实验修改关节角度观察两种方法在奇异点附近的表现差异添加旋转关节设置非零的α参数比较坐标系方向的变化实现逆运动学计算验证两种方法的一致性在机器人仿真和控制领域理解这两种DH方法的差异至关重要。通过这种代码实践的方式不仅能加深理论理解还能为后续的机器人算法开发打下坚实基础。