📚 课程目录第4章逆运动学数值解（迭代法）

4. 逆运动学数值解（迭代法）

任务目标

本案例实现六轴机械臂的逆运动学数值求解：给定末端目标位姿，使用雅可比矩阵的阻尼最小二乘迭代法（DLS）求解关节角，处理奇异位形下的数值稳定性，并对比迭代解与正运动学重投影的精度。

核心实现

import numpy as np

# 标准DH参数: [a, alpha, d, theta_offset]
DH = np.array([
    [0,     np.pi/2, 0.1625, 0],
    [-0.425, 0,      0,      0],
    [-0.3922, 0,      0,      0],
    [0,     np.pi/2, 0.1333, 0],
    [0,    -np.pi/2, 0.0997, 0],
    [0,     0,       0.0996, 0]
])

def fk(theta):
    """正运动学: 返回末端齐次变换矩阵 (4x4)"""
    T = np.eye(4)
    for i in range(6):
        a, alpha, d, offset = DH[i]
        th = theta[i] + offset
        ct, st = np.cos(th), np.sin(th)
        ca, sa = np.cos(alpha), np.sin(alpha)
        A = np.array([
            [ct, -st*ca,  st*sa, a*ct],
            [st,  ct*ca, -ct*sa, a*st],
            [0,   sa,     ca,     d   ],
            [0,   0,      0,      1   ]
        ])
        T = T @ A
    return T

def jacobian(theta):
    """数值雅可比 (6x6): 对每个关节用中心差分"""
    J = np.zeros((6, 6))
    delta = 1e-6
    T0 = fk(theta)
    p0 = T0[:3, 3]
    R0 = T0[:3, :3]
    for i in range(6):
        th_p = theta.copy(); th_p[i] += delta
        th_m = theta.copy(); th_m[i] -= delta
        Tp = fk(th_p); Tm = fk(th_m)
        # 位置部分
        J[:3, i] = (Tp[:3, 3] - Tm[:3, 3]) / (2 * delta)
        # 姿态部分 (旋转向量差)
        Rp = Tp[:3, :3] @ R0.T
        Rm = Tm[:3, :3] @ R0.T
        wp = np.array([Rp[2,1]-Rp[1,2], Rp[0,2]-Rp[2,0], Rp[1,0]-Rp[0,1]]) / 2
        wm = np.array([Rm[2,1]-Rm[1,2], Rm[0,2]-Rm[2,0], Rm[1,0]-Rm[0,1]]) / 2
        J[3:, i] = (wp - wm) / (2 * delta)
    return J

def ik_dls(T_target, theta_init, lambda_=0.01, tol=1e-6, max_iter=100):
    """阻尼最小二乘逆运动学求解"""
    theta = theta_init.copy()
    for _ in range(max_iter):
        T_cur = fk(theta)
        # 位置误差
        e_pos = T_target[:3, 3] - T_cur[:3, 3]
        # 姿态误差 (旋转向量)
        R_err = T_target[:3, :3] @ T_cur[:3, :3].T
        e_rot = 0.5 * np.array([
            R_err[2,1] - R_err[1,2],
            R_err[0,2] - R_err[2,0],
            R_err[1,0] - R_err[0,1]
        ])
        e = np.concatenate([e_pos, e_rot])
        if np.linalg.norm(e) < tol:
            break
        J = jacobian(theta)
        # DLS: (J^T J + lambda^2 I)^{-1} J^T
        JtJ = J.T @ J
        damping = lambda_**2 * np.eye(6)
        dtheta = np.linalg.solve(JtJ + damping, J.T @ e)
        theta += dtheta
    return theta

# ========== 验证 ==========
if __name__ == "__main__":
    # 目标位姿: 由一组已知关节角生成
    q_truth = np.array([0.5, -0.8, 0.3, 1.2, -0.6, 0.4])
    T_target = fk(q_truth)

    # 多组初始值测试
    np.random.seed(42)
    print("=== 逆运动学数值解验证 ===")
    for trial in range(3):
        q_init = np.random.uniform(-1.5, 1.5, 6)
        q_sol = ik_dls(T_target, q_init)
        T_sol = fk(q_sol)
        # 精度指标
        pos_err = np.linalg.norm(T_sol[:3,3] - T_target[:3,3])
        R_err = T_sol[:3,:3] @ T_target[:3,:3].T
        rot_err = np.linalg.norm(0.5*np.array([
            R_err[2,1]-R_err[1,2], R_err[0,2]-R_err[2,0], R_err[1,0]-R_err[0,1]
        ]))
        print(f"试验{trial+1}: 初始角={np.round(q_init,2)}")
        print(f"  解算角={np.round(q_sol,4)}")
        print(f"  位置误差={pos_err:.2e} m, 姿态误差={rot_err:.2e} rad")
        print(f"  关节角误差={np.linalg.norm(q_sol-q_truth):.2e} rad")

运行与验证

环境依赖：仅需 numpy（pip install numpy）。

运行方式：直接执行脚本：python ik_dls.py。

验证方法：

程序使用随机初始值进行3次独立求解，每次均从同一目标位姿反解关节角。
通过条件：位置误差 < 1e-5 m 且姿态误差 < 1e-5 rad，表明迭代收敛到正确解。
关节角误差反映解的唯一性——由于数值迭代可能收敛到不同运动学位形，该值允许较大（>0.1 rad），但正运动学重投影误差必须满足上述精度。

奇异位形处理说明

当机械臂接近奇异位形时，雅可比矩阵接近奇异，J^T J 的条件数急剧增大。本实现采用阻尼最小二乘（DLS），通过在 J^T J 上叠加 λ²I 保证矩阵可逆，避免数值爆炸。阻尼系数 λ=0.01 在精度与稳定性间取得平衡——若增大λ则迭代更稳健但收敛精度下降，减小λ则反之。如需测试奇异位形，可将目标位姿设为机械臂完全伸展状态（如所有关节角为0），此时DLS仍能返回一个可行解而非发散。

常见问题：若迭代不收敛，首先检查目标位姿是否在机械臂工作空间内；其次适当增大阻尼系数λ至0.1可改善发散问题，但会牺牲精度；若初始值远离解，可尝试多组随机初始值并选取误差最小者。