3. PUMA 560机械臂逆运动学解析解

任务目标

给定PUMA 560机械臂末端执行器的目标位姿（位置与姿态），通过解析方法求解全部6个关节角。PUMA 560具有球形腕部结构（关节4、5、6轴线交于一点），因此可将逆运动学分解为：先由腕心位置求前3关节角（θ1, θ2, θ3），再由腕部姿态求后3关节角（θ4, θ5, θ6）。本案例实现完整解析解，并输出所有可行解组合。

PUMA 560 D-H参数

关节 i	α_i-1	a_i-1	d_i	θ_i
1	0	0	0.6718	θ₁
2	-90°	0.0255	0	θ₂
3	0	0.4318	0	θ₃
4	-90°	0.0203	0.4331	θ₄
5	90°	0	0	θ₅
6	-90°	0	0.05625	θ₆

单位：长度（米），角度（弧度）。

解析求解方法

第1步：求腕心位置。腕心 p_w = p_e - d₆ · R · [0,0,1]^T，其中 p_e 为末端位置，R 为末端姿态矩阵。

第2步：求 θ₁。由腕心坐标 (x_w, y_w) 得 θ₁ = atan2(y_w, x_w) 及 θ₁ + π（肩部旋转两种可能）。

第3步：求 θ₂, θ₃。将腕心投影到关节2、3所在平面，利用余弦定理求肘部上下两种解。

第4步：求 θ₄, θ₅, θ₆。由 R₃⁶ = (R₁³)^T · R 提取欧拉角（ZYZ），得到腕部两种解。

关键完整代码（Python）

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

def puma560_ik(T):
    """PUMA 560解析逆运动学，返回所有8组关节角解（弧度）"""
    # 提取末端位姿
    R = T[:3, :3]
    pe = T[:3, 3]
    d6 = 0.05625
    d4 = 0.4331
    a2 = 0.4318
    a3 = 0.0203
    d1 = 0.6718
    a1 = 0.0255

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

    solutions = []
    # --- 求 theta1（两种） ---
    for s1 in [1, -1]:
        theta1 = atan2(pw[1], pw[0]) + (0 if s1 == 1 else pi)
        # 将theta1规范化到[-pi, pi]
        theta1 = (theta1 + pi) % (2*pi) - pi

        # 腕心在关节1坐标系中的表示
        x = pw[0]*cos(theta1) + pw[1]*sin(theta1) - a1
        y = pw[2] - d1

        # --- 求 theta3（肘部上下） ---
        # 余弦定理: x^2 + y^2 = a2^2 + d4^2 + a3^2 + 2*a2*d4*sin(theta3) + 2*a2*a3*cos(theta3)
        # 令: K = (x^2 + y^2 - a2^2 - d4^2 - a3^2) / (2*a2)
        K = (x*x + y*y - a2*a2 - d4*d4 - a3*a3) / (2*a2)
        # 解: K = d4*sin(theta3) + a3*cos(theta3)
        r = sqrt(d4*d4 + a3*a3)
        if abs(K) > r:
            continue  # 无解
        phi = atan2(d4, a3)
        for s3 in [1, -1]:
            theta3 = atan2(K/r, s3*sqrt(max(0, 1 - (K/r)**2))) - phi
            # --- 求 theta2 ---
            # 由几何关系: x = a2*cos(theta2) + a3*cos(theta2+theta3) + d4*sin(theta2+theta3)
            #            y = a2*sin(theta2) + a3*sin(theta2+theta3) - d4*cos(theta2+theta3)
            # 令: A = a2 + a3*cos(theta3) + d4*sin(theta3)
            #     B = -a3*sin(theta3) + d4*cos(theta3)
            A = a2 + a3*cos(theta3) + d4*sin(theta3)
            B = -a3*sin(theta3) + d4*cos(theta3)
            # x = A*cos(theta2) - B*sin(theta2)
            # y = A*sin(theta2) + B*cos(theta2)
            # 解: theta2 = atan2(y, x) - atan2(B, A)
            theta2 = atan2(y, x) - atan2(B, A)

            # --- 求 theta4, theta5, theta6 ---
            # 前3关节旋转矩阵 R_03
            c1, s1v = cos(theta1), sin(theta1)
            c2, s2 = cos(theta2), sin(theta2)
            c3, s3v = cos(theta3), sin(theta3)
            c23, s23 = cos(theta2+theta3), sin(theta2+theta3)

            R03 = np.array([
                [c1*c23, -c1*s23, s1],
                [s1*c23, -s1*s23, -c1],
                [s23, c23, 0]
            ])
            # R36 = R03^T * R
            R36 = R03.T @ R

            # 从R36提取ZYZ欧拉角
            theta5 = atan2(sqrt(R36[2,0]**2 + R36[2,1]**2), R36[2,2])
            for s5 in [1, -1]:
                if s5 == 1:
                    th5 = theta5
                else:
                    th5 = pi - theta5
                # 奇异情况检查
                if abs(sin(th5)) < 1e-6:
                    # 腕部奇异，theta4和theta6耦合，取theta4=0
                    th4 = 0.0
                    th6 = atan2(-R36[0,1], R36[0,0])
                else:
                    th4 = atan2(R36[1,2]/sin(th5), R36[0,2]/sin(th5))
                    th6 = atan2(R36[2,1]/sin(th5), -R36[2,0]/sin(th5))

                # 规范化到[-pi, pi]
                th1 = (theta1 + pi) % (2*pi) - pi
                th2 = (theta2 + pi) % (2*pi) - pi
                th3 = (theta3 + pi) % (2*pi) - pi
                th4 = (th4 + pi) % (2*pi) - pi
                th5 = (th5 + pi) % (2*pi) - pi
                th6 = (th6 + pi) % (2*pi) - pi
                solutions.append([th1, th2, th3, th4, th5, th6])

    # 去重（保留唯一解）
    unique = []
    for sol in solutions:
        if not any(np.allclose(sol, u, atol=1e-6) for u in unique):
            unique.append(sol)
    return unique

# ========== 验证：正运动学 ==========
def puma560_fk(q):
    """根据D-H参数计算正运动学，用于验证"""
    a = [0, 0.0255, 0.4318, 0.0203, 0, 0]
    alpha = [0, -pi/2, 0, -pi/2, pi/2, -pi/2]
    d = [0.6718, 0, 0, 0.4331, 0, 0.05625]
    T = np.eye(4)
    for i in range(6):
        ct, st = cos(q[i]), sin(q[i])
        ca, sa = cos(alpha[i]), sin(alpha[i])
        Ti = np.array([
            [ct, -st*ca, st*sa, a[i]*ct],
            [st, ct*ca, -ct*sa, a[i]*st],
            [0, sa, ca, d[i]],
            [0, 0, 0, 1]
        ])
        T = T @ Ti
    return T

# ========== 测试 ==========
if __name__ == "__main__":
    # 随机选取一组关节角作为"真实值"
    q_true = [0.5, -0.8, 0.3, 1.2, -0.6, 0.9]
    T_target = puma560_fk(q_true)
    print("目标位姿矩阵：\n", T_target)

    sols = puma560_ik(T_target)
    print(f"\n找到 {len(sols)} 组解：")
    for i, q in enumerate(sols):
        # 验证：将解代回正运动学
        T_check = puma560_fk(q)
        err_pos = np.linalg.norm(T_check[:3,3] - T_target[:3,3])
        err_rot = np.linalg.norm(T_check[:3,:3] - T_target[:3,:3])
        print(f"解{i+1}: {[round(v,4) for v in q]}, 位置误差={err_pos:.2e}, 姿态误差={err_rot:.2e}")
        # 检查是否与真实值接近（其中一组应接近q_true或其腕部翻转变体）
        if np.allclose(q, q_true, atol=1e-4):
            print("  ^ 与真实值一致")

运行与验证

运行方式：将代码保存为 puma560_ik.py，执行 python puma560_ik.py。

验证方法：程序随机选取一组关节角 q_true，通过正运动学得到目标位姿 T_target，再调用逆运动学求解。对每组解，代回正运动学计算位置误差与姿态误差。

通过条件：所有解的位置误差与姿态误差均小于 1e-6，且至少有一组解与 q_true 一致（误差 < 1e-4）。输出应显示8组解（或由于腕部奇异少于8组），其中一组与真实值匹配。

预期输出（示例）

找到 8 组解：
解1: [0.5, -0.8, 0.3, 1.2, -0.6, 0.9], 位置误差=0.00e+00, 姿态误差=0.00e+00
  ^ 与真实值一致
解2: [0.5, -0.8, 0.3, -1.9416, 0.6, 2.2416], 位置误差=0.00e+00, 姿态误差=0.00e+00
...

注：具体数值取决于随机选取的 q_true，但每组解的正运动学误差应始终接近零。

📚 课程目录