用Python+Matplotlib实现机器人MDH与SDH参数建模全流程解析

刚接触机器人运动学时，MDH（修正DH）和SDH（标准DH）参数总让人困惑不已——它们看起来如此相似，却又在细节上存在关键差异。本文将通过Python代码实现一个平面3R机械臂的完整建模过程，用动态可视化帮你彻底理解这两种参数体系的本质区别。

1. 理解DH参数的基本原理

DH参数是描述机器人连杆之间几何关系的经典方法，由Denavit和Hartenberg于1955年提出。它用四个参数（a, α, d, θ）就能完整描述相邻连杆坐标系之间的变换关系。

核心参数定义：

• a （连杆长度）：沿X轴测量的两相邻Z轴距离
• α （连杆扭转）：绕X轴旋转的角度
• d （连杆偏移）：沿Z轴测量的两相邻X轴距离
• θ （关节角度）：绕Z轴旋转的角度

# DH参数示例结构
dh_params = [
    {'a': 0.5, 'alpha': 0, 'd': 0, 'theta': np.pi/4},  # 关节1
    {'a': 0.3, 'alpha': 0, 'd': 0, 'theta': np.pi/6},  # 关节2
    {'a': 0.2, 'alpha': 0, 'd': 0, 'theta': np.pi/3}   # 关节3
]

注意：MDH和SDH的主要区别在于坐标系附着位置和参数定义顺序，这直接影响变换矩阵的计算方式。

2. 标准DH(SDH)参数实现

SDH将坐标系{i}固定在连杆i的远端（靠近关节i+1的一端），其参数定义顺序为：

1. 绕Z_{i-1}轴旋转θ_i
2. 沿Z_{i-1}轴平移d_i
3. 沿X_i轴平移a_i
4. 绕X_i轴旋转α_i

def sdh_transform_matrix(a, alpha, d, theta):
    """计算标准DH变换矩阵"""
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = 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]
    ])

SDH坐标系建立步骤：

1. 确定各关节旋转轴Z_i
2. 找出相邻Z轴的公垂线，确定X_i方向
3. 按右手定则确定Y_i
4. 原点设在X_i与Z_i交点

3. 修正DH(MDH)参数实现

MDH将坐标系{i}固定在连杆i的近端（靠近关节i的一端），其变换顺序为：

1. 沿Z_i轴平移d_i
2. 绕Z_i轴旋转θ_i
3. 沿X_i轴平移a_i
4. 绕X_i轴旋转α_i

def mdh_transform_matrix(a, alpha, d, theta):
    """计算修正DH变换矩阵"""
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    
    return np.array([
        [ct, -st, 0, a],
        [st*ca, ct*ca, -sa, -d*sa],
        [st*sa, ct*sa, ca, d*ca],
        [0, 0, 0, 1]
    ])

MDH与SDH的关键差异对比：

4. 平面3R机器人完整建模示例

我们以一个三连杆平面机械臂为例，分别用SDH和MDH方法建模：

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

# 定义连杆长度
L1, L2, L3 = 1.0, 0.8, 0.6

# 初始化图形
fig = plt.figure(figsize=(12, 6))
ax1 = fig.add_subplot(121, aspect='equal')
ax2 = fig.add_subplot(122, aspect='equal')
ax1.set_title('Standard DH (SDH)')
ax2.set_title('Modified DH (MDH)')

def update(frame):
    # 清除之前绘制的内容
    ax1.clear()
    ax2.clear()
    
    # 设置相同的坐标范围
    for ax in [ax1, ax2]:
        ax.set_xlim(-2, 2)
        ax.set_ylim(-2, 2)
        ax.grid(True)
    
    # 计算当前关节角度
    theta1 = np.radians(frame % 360)
    theta2 = np.radians((frame * 1.5) % 360)
    theta3 = np.radians((frame * 2) % 360)
    
    # SDH计算
    T0_sdh = np.eye(4)
    T1_sdh = T0_sdh @ sdh_transform_matrix(L1, 0, 0, theta1)
    T2_sdh = T1_sdh @ sdh_transform_matrix(L2, 0, 0, theta2)
    T3_sdh = T2_sdh @ sdh_transform_matrix(L3, 0, 0, theta3)
    
    # MDH计算
    T0_mdh = np.eye(4)
    T1_mdh = T0_mdh @ mdh_transform_matrix(L1, 0, 0, theta1)
    T2_mdh = T1_mdh @ mdh_transform_matrix(L2, 0, 0, theta2)
    T3_mdh = T2_mdh @ mdh_transform_matrix(L3, 0, 0, theta3)
    
    # 绘制SDH结果
    plot_robot(ax1, [T0_sdh, T1_sdh, T2_sdh, T3_sdh])
    
    # 绘制MDH结果
    plot_robot(ax2, [T0_mdh, T1_mdh, T2_mdh, T3_mdh])

def plot_robot(ax, transforms):
    """绘制机器人状态"""
    # 提取各坐标系原点
    origins = [t[:2, 3] for t in transforms]
    
    # 绘制连杆
    for i in range(len(origins)-1):
        ax.plot([origins[i][0], origins[i+1][0]], 
                [origins[i][1], origins[i+1][1]], 'b-', linewidth=3)
    
    # 绘制关节
    for origin in origins:
        ax.plot(origin[0], origin[1], 'ro', markersize=8)
    
    # 绘制末端执行器
    ax.plot(origins[-1][0], origins[-1][1], 'go', markersize=10)

# 创建动画
ani = FuncAnimation(fig, update, frames=360, interval=50)
plt.tight_layout()
plt.show()

实际调试中发现的问题：

1. 当关节角度为0时，MDH和SDH的坐标系朝向不同
2. 连杆长度参数在两种方法中的含义有细微差别
3. 对于树状结构机器人，MDH通常更易建模

5. 进阶应用与性能优化

当处理更复杂的机器人结构时，可以考虑以下优化策略：

# 使用numba加速矩阵运算
from numba import jit

@jit(nopython=True)
def fast_sdh_matrix(a, alpha, d, theta):
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = 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]
    ])

# 批量计算变换矩阵
def batch_compute(dh_params, method='sdh'):
    transforms = [np.eye(4)]
    for params in dh_params:
        if method == 'sdh':
            T = fast_sdh_matrix(**params)
        else:
            T = mdh_transform_matrix(**params)
        transforms.append(transforms[-1] @ T)
    return transforms

可视化技巧提升：

• 使用不同颜色区分连杆和关节
• 添加坐标系箭头显示方向
• 实时显示末端轨迹
• 添加滑块交互控制关节角度

# 增强版可视化设置
def enhanced_plot(ax, transforms, trail=None):
    ax.clear()
    ax.set_xlim(-3, 3)
    ax.set_ylim(-3, 3)
    
    # 绘制轨迹
    if trail:
        ax.plot(trail[:,0], trail[:,1], 'g:', alpha=0.5)
    
    # 绘制连杆和关节
    origins = [t[:2, 3] for t in transforms]
    for i in range(len(origins)-1):
        ax.plot([origins[i][0], origins[i+1][0]], 
                [origins[i][1], origins[i+1][1]], 
                color=plt.cm.viridis(i/3), linewidth=4)
        ax.plot(origins[i][0], origins[i][1], 'o', 
                markersize=12, color=plt.cm.plasma(i/3))
    
    # 绘制坐标系
    for i, T in enumerate(transforms):
        draw_frame(ax, T, label=f'Frame {i}')
    
    # 显示末端位置
    ax.text(0.05, 0.95, f'End Effector: ({origins[-1][0]:.2f}, {origins[-1][1]:.2f})',
            transform=ax.transAxes, bbox=dict(facecolor='white', alpha=0.8))

def draw_frame(ax, T, scale=0.2, label=None):
    """绘制坐标系"""
    origin = T[:2, 3]
    x_axis = origin + T[:2, 0] * scale
    y_axis = origin + T[:2, 1] * scale
    
    ax.arrow(origin[0], origin[1], 
             x_axis[0]-origin[0], x_axis[1]-origin[1], 
             head_width=0.05, color='r')
    ax.arrow(origin[0], origin[1], 
             y_axis[0]-origin[0], y_axis[1]-origin[1], 
             head_width=0.05, color='g')
    
    if label:
        ax.text(origin[0]+0.05, origin[1]+0.05, label, 
                color='blue', fontsize=10)