机器人学实战:从零推导SDH与MDH变换矩阵的Python实现

刚接触机器人运动学时,DH参数法总是让人又爱又恨——概念清晰但矩阵推导繁琐,理论易懂可代码实现时总出错。今天我们就用Python的SymPy和NumPy库,以三自由度平面机械臂为例,手把手带您推导两种DH参数法的变换矩阵,并解释为何工业机器人领域更偏爱MDH方法。

1. 准备工作:理解坐标系差异

在开始写代码前,先明确两种方法的本质区别。SDH(标准DH)和MDH(改进DH)最核心的不同在于坐标系附着规则:

  • SDH坐标系:固定在连杆的远端(即远离基座的一端)
  • MDH坐标系:固定在连杆的近端(即靠近基座的一端)

这种差异导致参数定义和变换顺序完全不同。来看一个直观对比:

# 坐标系定义示例(伪代码)
class SDH_Frame:
    z_axis = previous_joint_axis
    x_axis = along_link_to_next_joint
    
class MDH_Frame:
    z_axis = current_joint_axis  
    x_axis = along_link_from_previous_joint

提示:在串联机器人中,MDH通常能减少参数耦合,这就是UR、KUKA等厂商采用它的原因。

2. 建立三自由度机械臂模型

我们以经典的平面RRR机械臂为例,其DH参数如下:

2.1 MDH参数表

关节iαᵢ₋₁aᵢ₋₁dᵢθᵢ
1000θ₁
20L₁0θ₂
30L₂0θ₃

2.2 SDH参数表

关节iθᵢdᵢaᵢαᵢ
1θ₁0L₁0
2θ₂0L₂0
3θ₃0L₃0

用Python定义这些参数:

import sympy as sp
import numpy as np

# 定义符号变量
theta1, theta2, theta3 = sp.symbols('theta1 theta2 theta3')
L1, L2, L3 = sp.symbols('L1 L2 L3')

# MDH参数
mdh_params = [
    {'alpha': 0, 'a': 0, 'd': 0, 'theta': theta1},
    {'alpha': 0, 'a': L1, 'd': 0, 'theta': theta2},
    {'alpha': 0, 'a': L2, 'd': 0, 'theta': theta3}
]

# SDH参数
sdh_params = [
    {'theta': theta1, 'd': 0, 'a': L1, 'alpha': 0},
    {'theta': theta2, 'd': 0, 'a': L2, 'alpha': 0},
    {'theta': theta3, 'd': 0, 'a': L3, 'alpha': 0}
]

3. 推导MDH变换矩阵

MDH的变换顺序为:RotX(α) → TransX(a) → RotZ(θ) → TransZ(d)

3.1 单关节变换实现

def mdh_transform(alpha, a, d, theta):
    """计算MDH单关节变换矩阵"""
    # 绕X轴旋转α
    Rx = sp.Matrix([
        [1, 0, 0, 0],
        [0, sp.cos(alpha), -sp.sin(alpha), 0],
        [0, sp.sin(alpha), sp.cos(alpha), 0],
        [0, 0, 0, 1]
    ])
    
    # 沿X轴平移a
    Tx = sp.Matrix([
        [1, 0, 0, a],
        [0, 1, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])
    
    # 绕Z轴旋转θ
    Rz = sp.Matrix([
        [sp.cos(theta), -sp.sin(theta), 0, 0],
        [sp.sin(theta), sp.cos(theta), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])
    
    # 沿Z轴平移d
    Tz = sp.Matrix([
        [1, 0, 0, 0],
        [0, 1, 0, 0],
        [0, 0, 1, d],
        [0, 0, 0, 1]
    ])
    
    return Rx * Tx * Rz * Tz

3.2 验证三关节变换

计算从基座到末端执行器的总变换:

# 计算各关节MDH变换
T_mdh = []
for params in mdh_params:
    T_mdh.append(mdh_transform(**params))

# 总变换矩阵
T_total_mdh = T_mdh[0] * T_mdh[1] * T_mdh[2]

# 打印简化后的结果
print("MDH总变换矩阵:")
sp.simplify(T_total_mdh)

输出结果将显示一个4×4的符号矩阵,包含θ₁、θ₂、θ₃和L₁、L₂的组合表达式。

4. 推导SDH变换矩阵

SDH的变换顺序相反:RotZ(θ) → TransZ(d) → TransX(a) → RotX(α)

4.1 单关节变换实现

def sdh_transform(theta, d, a, alpha):
    """计算SDH单关节变换矩阵"""
    # 绕Z轴旋转θ
    Rz = sp.Matrix([
        [sp.cos(theta), -sp.sin(theta), 0, 0],
        [sp.sin(theta), sp.cos(theta), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])
    
    # 沿Z轴平移d
    Tz = sp.Matrix([
        [1, 0, 0, 0],
        [0, 1, 0, 0],
        [0, 0, 1, d],
        [0, 0, 0, 1]
    ])
    
    # 沿X轴平移a
    Tx = sp.Matrix([
        [1, 0, 0, a],
        [0, 1, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])
    
    # 绕X轴旋转α
    Rx = sp.Matrix([
        [1, 0, 0, 0],
        [0, sp.cos(alpha), -sp.sin(alpha), 0],
        [0, sp.sin(alpha), sp.cos(alpha), 0],
        [0, 0, 0, 1]
    ])
    
    return Rz * Tz * Tx * Rx

4.2 验证三关节变换

# 计算各关节SDH变换
T_sdh = []
for params in sdh_params:
    T_sdh.append(sdh_transform(**params))

# 总变换矩阵
T_total_sdh = T_sdh[0] * T_sdh[1] * T_sdh[2]

# 打印简化后的结果
print("SDH总变换矩阵:")
sp.simplify(T_total_sdh)

5. 两种方法的数值验证

为了验证我们的推导正确,用具体数值进行测试:

# 设置具体参数值
test_values = {
    L1: 1.0, L2: 0.8, L3: 0.6,
    theta1: np.pi/4, theta2: np.pi/3, theta3: np.pi/6
}

# 数值化MDH矩阵
T_mdh_num = T_total_mdh.subs(test_values)
np_mdh = np.array(T_mdh_num).astype(np.float64)

# 数值化SDH矩阵
T_sdh_num = T_total_sdh.subs(test_values)
np_sdh = np.array(T_sdh_num).astype(np.float64)

print("MDH末端位置:", np_mdh[:3, 3])
print("SDH末端位置:", np_sdh[:3, 3])

注意:虽然两种方法的中间坐标系不同,但末端执行器的最终位姿应该一致——这是验证实现正确性的关键。

6. 实际应用中的选择建议

在真实项目中该如何选择?考虑以下因素:

  • MDH优势:

    • 坐标系与关节自然对应
    • 参数物理意义更直观
    • 工业机器人常用(如UR机器人)
  • SDH适用场景:

    • 学术文献和历史代码中常见
    • 某些特定构型机械臂简化计算
# 实际项目中的推荐做法
def create_dh_parameters(robot_type):
    if robot_type in ['UR', 'KUKA', 'ABB']:
        return mdh_params  # 工业机器人用MDH
    elif robot_type == 'academic':
        return sdh_params  # 学术研究可能用SDH
    else:
        raise ValueError("Unknown robot type")

7. 常见错误排查

在实现过程中容易遇到的坑:

  1. 变换顺序错误:

    • MDH必须是X旋转→X平移→Z旋转→Z平移
    • SDH必须是Z旋转→Z平移→X平移→X旋转
  2. 参数混淆:

    # 错误示例:把MDH的a参数当成SDH的a使用
    wrong_transform = sdh_transform(theta1, 0, L1, 0)  # 这个L1在SDH中实际对应a₁
    
  3. 符号计算陷阱:

    • SymPy中角度默认是弧度
    • 记得调用simplify()或trigsimp()化简矩阵
  4. 数值验证失败:

    • 检查末端位置是否相同
    • 确认旋转矩阵行列式为1(有效的旋转矩阵)
# 检查旋转矩阵有效性的方法
def is_valid_rotation(R):
    det = np.linalg.det(R[:3, :3])
    return np.isclose(det, 1.0) and np.allclose(R @ R.T, np.eye(3))

8. 性能优化技巧

当需要高频计算时,可以:

  1. 预编译符号表达式:

    from sympy.utilities.lambdify import lambdify
    
    # 将符号表达式转为数值函数
    mdh_func = lambdify((theta1, theta2, theta3, L1, L2), T_total_mdh, 'numpy')
    sdh_func = lambdify((theta1, theta2, theta3, L1, L2, L3), T_total_sdh, 'numpy')
    
  2. 使用NumPy向量化计算:

    # 批量计算多个位形
    thetas = np.random.rand(100, 3) * 2*np.pi  # 100个随机位形
    results = np.array([mdh_func(*t, 1.0, 0.8, 0.6) for t in thetas])
    
  3. 缓存常用变换:

    from functools import lru_cache
    
    @lru_cache(maxsize=100)
    def cached_mdh(theta1, theta2, theta3):
        return mdh_func(theta1, theta2, theta3, 1.0, 0.8)
    

在机械臂控制项目中,我习惯先用SymPy推导验证公式,再转为NumPy函数集成到实时控制系统中。当遇到奇怪的位姿问题时,回头检查DH参数定义总能发现大多数错误——特别是新手容易混淆a和d参数的定义方向。

Logo

更多推荐