4. UR5机械臂逆运动学解析解

任务目标

本案例实现UR5六自由度机械臂的解析逆运动学求解。给定末端执行器的目标位姿（位置与姿态），利用UR5的特殊几何结构（后三轴交于一点、相邻关节轴垂直等）推导并实现闭式解，输出全部8组可能的关节角组合。

UR5几何结构与DH参数

UR5采用标准DH参数（单位：mm）：

UR5后三关节（4、5、6）的轴线交于一点（腕心），因此可将逆运动学分解为：先由腕心位置求θ₁、θ₂、θ₃，再由姿态求θ₄、θ₅、θ₆。

解析求解算法

设目标位姿为齐次变换矩阵 T = [R | p]，其中 p 为末端位置，R 为3×3旋转矩阵。腕心位置 p_w = p - d₆ · R · [0,0,1]^T。

1. 求θ₁：由腕心在基坐标系中的x、y坐标，得 θ₁ = atan2(p_w,y, p_w,x) ± acos(d₄ / √(p_w,x² + p_w,y²)) + π/2，存在两个解。
2. 求θ₂、θ₃：将腕心坐标变换到关节1坐标系后，在x-z平面内按二连杆臂求解，利用余弦定理得 θ₃ 的两个解，对应肘部上/下；θ₂ 由几何关系唯一确定。
3. 求θ₄、θ₅、θ₆：已知前三角后，计算 R₃₆ = R₀₃^T · R，从该旋转矩阵中提取欧拉角（Z-Y-Z顺序）：θ₅ = atan2(±√(r₁₃²+r₂₃²), r₃₃)，θ₄ = atan2(r₂₃/sinθ₅, r₁₃/sinθ₅)，θ₆ = atan2(r₃₂/sinθ₅, -r₃₁/sinθ₅)。

完整实现代码

import numpy as np
from itertools import product

def dh_transform(a, alpha, d, theta):
    """标准DH参数转齐次变换矩阵"""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), 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 forward_kinematics(q):
    """UR5正运动学，q为6关节角(rad)"""
    # DH参数: [a, alpha, d]
    dh = [
        [0,      -np.pi/2, 89.159],
        [-425,    0,        0],
        [-392.25, 0,        0],
        [0,      -np.pi/2, 109.15],
        [0,       np.pi/2, 94.65],
        [0,      -np.pi/2, 82.3]
    ]
    T = np.eye(4)
    for i in range(6):
        T = T @ dh_transform(dh[i][0], dh[i][1], dh[i][2], q[i])
    return T

def inverse_kinematics(T_target):
    """
    UR5解析逆运动学，返回所有8组解(每组6个关节角)
    T_target: 4x4齐次变换矩阵
    """
    p = T_target[:3, 3]
    R = T_target[:3, :3]
    d6 = 82.3
    d4 = 109.15
    a2, a3 = -425, -392.25

    # 腕心位置
    pw = p - d6 * (R @ np.array([0, 0, 1]))

    solutions = []
    # θ1 两个解
    r = np.sqrt(pw[0]**2 + pw[1]**2)
    if r < 1e-6:
        theta1_candidates = [0.0]
    else:
        phi = np.arctan2(pw[1], pw[0])
        delta = np.arccos(np.clip(d4 / r, -1, 1))
        theta1_candidates = [phi + delta + np.pi/2, phi - delta + np.pi/2]

    for theta1 in theta1_candidates:
        # 变换到关节1坐标系
        T1_inv = np.linalg.inv(dh_transform(0, -np.pi/2, 89.159, theta1))
        pw1 = T1_inv @ np.append(pw, 1)
        x, z = pw1[0], pw1[2]

        # 平面二连杆求θ2、θ3
        D = (x**2 + z**2 - a2**2 - a3**2) / (2 * a2 * a3)
        D = np.clip(D, -1, 1)
        theta3_candidates = [np.arccos(D), -np.arccos(D)]

        for theta3 in theta3_candidates:
            # θ2 由几何关系确定
            k1 = a2 + a3 * np.cos(theta3)
            k2 = a3 * np.sin(theta3)
            theta2 = np.arctan2(z, x) - np.arctan2(k2, k1)

            # 前三角旋转矩阵 R03
            T01 = dh_transform(0, -np.pi/2, 89.159, theta1)
            T12 = dh_transform(-425, 0, 0, theta2)
            T23 = dh_transform(-392.25, 0, 0, theta3)
            R03 = (T01 @ T12 @ T23)[:3, :3]

            # 求R36 = R03^T * R
            R36 = R03.T @ R

            # 从R36提取Z-Y-Z欧拉角
            r13, r23, r33 = R36[0,2], R36[1,2], R36[2,2]
            r31, r32 = R36[2,0], R36[2,1]

            theta5_candidates = [np.arctan2(np.sqrt(r13**2 + r23**2), r33),
                                 np.arctan2(-np.sqrt(r13**2 + r23**2), r33)]

            for theta5 in theta5_candidates:
                if abs(np.sin(theta5)) < 1e-6:
                    # 奇异情况：θ5≈0，θ4与θ6无法分离，取θ4=0
                    theta4 = 0.0
                    theta6 = np.arctan2(R36[1,0], R36[0,0])
                else:
                    theta4 = np.arctan2(r23/np.sin(theta5), r13/np.sin(theta5))
                    theta6 = np.arctan2(r32/np.sin(theta5), -r31/np.sin(theta5))

                solutions.append([theta1, theta2, theta3, theta4, theta5, theta6])

    # 去重（数值容差）
    unique = []
    for sol in solutions:
        if not any(np.allclose(sol, u, atol=1e-6) for u in unique):
            unique.append(sol)
    return unique

# ===== 验证示例 =====
if __name__ == "__main__":
    # 给定一组关节角，先正运动学得到目标位姿
    q_test = [0.5, -1.2, 1.0, -0.8, 1.5, 0.3]
    T_target = forward_kinematics(q_test)

    # 逆运动学求解
    sols = inverse_kinematics(T_target)
    print(f"找到 {len(sols)} 组解")

    # 验证每组解的正运动学误差
    for i, q_ik in enumerate(sols):
        T_check = forward_kinematics(q_ik)
        pos_err = np.linalg.norm(T_check[:3,3] - T_target[:3,3])
        rot_err = np.linalg.norm(T_check[:3,:3] - T_target[:3,:3])
        print(f"解{i+1}: {np.round(q_ik, 4)}  位置误差={pos_err:.2e}  姿态误差={rot_err:.2e}")

运行与验证

运行方式：将代码保存为 ur5_ik.py，在Python环境中执行 python ur5_ik.py。需要安装NumPy（pip install numpy）。

验证方法：程序先以一组随机关节角通过正运动学生成目标位姿，再调用逆运动学求解。对每组解重新执行正运动学，计算与原始目标位姿的位置误差和姿态误差（Frobenius范数）。

通过标准：输出8组解（或由于奇异位姿少于8组），且每组解的位置误差与姿态误差均在1e-6量级以下，说明解析逆运动学实现正确。

关键说明

• UR5后三轴交于腕心，使位置与姿态解耦求解，这是解析法可行的核心前提。
• θ₁的±、θ₃的±、θ₅的±组合共产生8组解，对应不同的臂型与腕型。
• 当θ₅≈0时出现腕部奇异，此时θ₄与θ₆退化为一组自由度，代码中取θ₄=0处理。
• 实际应用中需结合关节限位从多组解中选取最优解，本案例仅实现全部解析解的求解与验证。