从DH参数到机器人运动仿真：Python实战UR机械臂运动学

在工业自动化领域，UR(Universal Robots)协作机器人以其灵活性和易用性著称。但你是否好奇过，这些机械臂是如何精确计算出每个关节应该转动多少角度，才能让末端执行器到达指定位置？本文将带你深入理解DH(Denavit-Hartenberg)参数背后的数学原理，并用Python构建完整的运动学验证系统。

1. 理解DH参数：机器人运动的数学语言

DH参数是描述机器人关节和连杆之间几何关系的标准化方法。每个UR机器人的技术手册都会提供这样一组看似神秘的四个参数：

# UR5e的DH参数示例
dh_params = [
    {'theta': 0, 'a': 0, 'd': 0.1625, 'alpha': pi/2},    # 关节1
    {'theta': 0, 'a': -0.425, 'd': 0, 'alpha': 0},       # 关节2
    {'theta': 0, 'a': -0.392, 'd': 0, 'alpha': 0},       # 关节3
    {'theta': 0, 'a': 0, 'd': 0.1333, 'alpha': pi/2},    # 关节4
    {'theta': 0, 'a': 0, 'd': 0.0997, 'alpha': -pi/2},   # 关节5
    {'theta': 0, 'a': 0, 'd': 0.0996, 'alpha': 0}        # 关节6
]

这四个参数分别代表：

• θ(Theta) : 关节角度，这是我们的控制变量
• a : 连杆长度，沿x轴的距离
• d : 连杆偏移，沿z轴的距离
• α(Alpha) : 连杆扭转角，绕x轴的旋转

注意：UR机器人的DH参数采用"改进的DH参数法"(Modified DH Parameters)，与标准DH参数在坐标系定义上有区别。

2. 构建正运动学模型：从关节角度到末端位姿

正运动学解决的问题是：给定各关节角度，计算机械臂末端执行器的位置和姿态。我们需要通过DH参数构建每个关节的变换矩阵，然后连乘得到最终变换。

import numpy as np
from math import cos, sin, pi

def dh_matrix(theta, a, d, 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 forward_kinematics(dh_params, joint_angles):
    """计算正运动学"""
    T = np.eye(4)
    for i, params in enumerate(dh_params):
        theta = joint_angles[i] + params['theta']
        a, d, alpha = params['a'], params['d'], params['alpha']
        T_i = dh_matrix(theta, a, d, alpha)
        T = np.dot(T, T_i)
    return T

让我们测试一个UR5e的典型构型：

# UR5e的六个关节角度(弧度)
joints = [0, -pi/2, pi/2, 0, pi/2, 0]

# 计算末端位姿
end_effector_pose = forward_kinematics(dh_params, joints)
print("末端执行器位置:", end_effector_pose[:3,3])
print("旋转矩阵:\n", end_effector_pose[:3,:3])

3. 可视化机器人构型：让数学变得可见

理解变换矩阵可能比较抽象，我们可以用Matplotlib进行3D可视化：

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def plot_robot(dh_params, joint_angles):
    fig = plt.figure(figsize=(10,8))
    ax = fig.add_subplot(111, projection='3d')
    
    # 计算各关节位置
    positions = [np.array([0,0,0])]
    T = np.eye(4)
    for i, params in enumerate(dh_params):
        theta = joint_angles[i] + params['theta']
        a, d, alpha = params['a'], params['d'], params['alpha']
        T = np.dot(T, dh_matrix(theta, a, d, alpha))
        positions.append(T[:3,3])
    
    # 绘制连杆
    pos_array = np.array(positions)
    ax.plot(pos_array[:,0], pos_array[:,1], pos_array[:,2], 'o-', linewidth=2)
    
    # 设置坐标轴
    ax.set_xlabel('X轴')
    ax.set_ylabel('Y轴')
    ax.set_zlabel('Z轴')
    ax.set_title('UR机械臂构型可视化')
    plt.show()

# 可视化一个示例构型
plot_robot(dh_params, [0, -pi/3, pi/3, 0, pi/2, 0])

这段代码会生成一个3D图形，清晰展示机器人在给定关节角度下的实际构型，帮助我们直观理解DH参数的含义。

4. 逆运动学初探：从末端位姿反求关节角度

逆运动学是正运动学的逆问题：给定末端执行器的目标位姿，求解各关节角度。这是一个更复杂的问题，通常有解析解和数值解两种方法。这里我们介绍数值解法中的雅可比矩阵方法。

def jacobian(dh_params, joint_angles):
    """计算雅可比矩阵"""
    J = np.zeros((6,6))
    T = np.eye(4)
    positions = [np.array([0,0,0])]
    transforms = [T]
    
    # 计算各关节位置和z轴方向
    for i, params in enumerate(dh_params):
        theta = joint_angles[i] + params['theta']
        a, d, alpha = params['a'], params['d'], params['alpha']
        T = np.dot(T, dh_matrix(theta, a, d, alpha))
        transforms.append(T)
        positions.append(T[:3,3])
    
    # 末端执行器位置
    pe = positions[-1]
    
    # 计算雅可比矩阵各列
    for i in range(6):
        z_i = transforms[i][:3,2]
        p_i = positions[i]
        J[:3,i] = np.cross(z_i, pe - p_i)
        J[3:,i] = z_i
    
    return J

def inverse_kinematics(dh_params, target_pose, initial_angles=None, max_iter=100, tol=1e-6):
    """数值逆运动学求解"""
    if initial_angles is None:
        initial_angles = np.zeros(6)
    
    angles = initial_angles.copy()
    for _ in range(max_iter):
        # 计算当前位姿
        T = forward_kinematics(dh_params, angles)
        # 计算误差
        error_pos = target_pose[:3,3] - T[:3,3]
        error_rot = 0.5 * (np.cross(T[:3,0], target_pose[:3,0]) + 
                          np.cross(T[:3,1], target_pose[:3,1]) + 
                          np.cross(T[:3,2], target_pose[:3,2]))
        error = np.concatenate([error_pos, error_rot])
        
        # 检查是否收敛
        if np.linalg.norm(error) < tol:
            break
            
        # 计算雅可比矩阵并更新关节角度
        J = jacobian(dh_params, angles)
        delta_angles = np.linalg.pinv(J) @ error
        angles += delta_angles
    
    return angles

使用示例：

# 定义目标位姿
target_pose = np.array([
    [0, 0, 1, 0.3],
    [0, 1, 0, 0.1],
    [-1, 0, 0, 0.4],
    [0, 0, 0, 1]
])

# 求解逆运动学
joint_angles = inverse_kinematics(dh_params, target_pose)
print("求解得到的关节角度(弧度):", joint_angles)

# 验证结果
achieved_pose = forward_kinematics(dh_params, joint_angles)
print("实际达到的位姿:\n", achieved_pose)

5. 实际应用中的注意事项

在真实项目中应用这些算法时，有几个关键点需要考虑：

1. 奇异构型 ：当雅可比矩阵失去满秩时，机器人处于奇异构型，此时逆运动学可能无解或有无穷多解。常见的奇异构型包括：

   • 腕部奇异：关节4和关节6对齐
   • 肘部奇异：关节2、3、5共线
   • 肩部奇异：关节1、2、4共面

2. 关节限位 ：UR机器人每个关节都有运动范围限制，例如：

   # UR5e关节限位(弧度)
   joint_limits = [
       [-2*pi, 2*pi],   # 关节1
       [-pi, 0],        # 关节2
       [-pi, pi],       # 关节3
       [-2*pi, 2*pi],   # 关节4
       [-2*pi, 2*pi],   # 关节5
       [-2*pi, 2*pi]    # 关节6
   ]
   

3. 多解选择 ：6轴机械臂通常有8种不同的逆解，需要根据实际情况选择最合适的解，考虑因素包括：

   • 离当前位姿最近
   • 避免碰撞
   • 能量最优

4. 实时性要求 ：工业应用中通常需要毫秒级的计算速度，可能需要：

   • 预计算查找表
   • 使用更高效的数值方法
   • 考虑硬件加速

在完成这些基础工作后，可以进一步扩展功能，如添加碰撞检测、轨迹规划等模块，构建完整的机器人控制系统。