3. 基于雅可比矩阵的数值逆运动学（牛顿-拉夫森法）

任务目标

实现基于雅可比矩阵的牛顿-拉夫森迭代法，求解三连杆平面机械臂的逆运动学问题。给定末端目标位姿，通过迭代更新关节角，使正运动学结果逼近目标，并验证不同初始猜测下的收敛行为。

核心原理

牛顿-拉夫森法通过线性化正运动学方程迭代求解：

θ_{k+1} = θ_k + J⁻¹(θ_k) · Δx

其中 Δx = x_target - f(θ_k) 为末端位置误差，J 为雅可比矩阵。当机械臂自由度等于任务空间维度时，J 为方阵，可直接求逆。

完整实现（Python + NumPy）

import numpy as np

# 三连杆平面机械臂参数（杆长）
L = [1.0, 0.8, 0.6]

def forward_kinematics(theta):
    """正运动学：关节角 → 末端位置 (x, y)"""
    x = 0.0
    y = 0.0
    angle_sum = 0.0
    for i in range(3):
        angle_sum += theta[i]
        x += L[i] * np.cos(angle_sum)
        y += L[i] * np.sin(angle_sum)
    return np.array([x, y])

def jacobian(theta):
    """解析雅可比矩阵 2×3（位置对关节角求导）"""
    J = np.zeros((2, 3))
    angle_sum = 0.0
    for i in range(3):
        angle_sum += theta[i]
        for j in range(i, 3):
            J[0, j] -= L[i] * np.sin(angle_sum)
            J[1, j] += L[i] * np.cos(angle_sum)
    return J

def newton_raphson_ik(target, theta_init, tol=1e-6, max_iter=50):
    """
    牛顿-拉夫森逆运动学求解
    返回: (解, 是否收敛, 迭代次数, 最终误差)
    """
    theta = theta_init.copy()
    for i in range(max_iter):
        pos = forward_kinematics(theta)
        error = target - pos
        if np.linalg.norm(error) < tol:
            return theta, True, i, np.linalg.norm(error)
        J = jacobian(theta)
        # 2×3 矩阵，使用伪逆处理（此处因3自由度2任务空间，用最小范数解）
        J_pinv = np.linalg.pinv(J)
        delta_theta = J_pinv @ error
        theta = theta + delta_theta
    return theta, False, max_iter, np.linalg.norm(error)

# ========== 测试与验证 ==========
if __name__ == "__main__":
    # 生成一个可达目标点（由已知关节角正解得到）
    theta_true = np.array([0.3, 0.8, -0.5])
    target = forward_kinematics(theta_true)
    print(f"目标位置: {target}")

    # 测试1：从接近真值的初始值开始
    theta_init1 = np.array([0.2, 0.7, -0.4])
    sol1, conv1, it1, err1 = newton_raphson_ik(target, theta_init1)
    print(f"\n[测试1] 初始值接近真值")
    print(f"  收敛: {conv1}, 迭代次数: {it1}, 最终误差: {err1:.2e}")
    print(f"  解: {sol1}")

    # 测试2：从较远的初始值开始
    theta_init2 = np.array([0.0, 0.0, 0.0])
    sol2, conv2, it2, err2 = newton_raphson_ik(target, theta_init2)
    print(f"\n[测试2] 初始值为零位姿")
    print(f"  收敛: {conv2}, 迭代次数: {it2}, 最终误差: {err2:.2e}")
    print(f"  解: {sol2}")

    # 验证：正运动学回代
    pos_check = forward_kinematics(sol1)
    print(f"\n验证: 正运动学回代结果 = {pos_check}")
    print(f"与目标误差: {np.linalg.norm(pos_check - target):.2e}")

运行方式

pip install numpy
python newton_ik.py

验证方法与判定标准

• 收敛判定：程序输出 conv=True 且最终误差 < 1e-6，即视为迭代成功。
• 回代验证：将求解出的关节角代入正运动学，末端位置与目标点误差应小于 1e-6。
• 收敛性观察：测试1（初始值接近真值）应快速收敛（通常5次以内）；测试2（零初始位姿）可能收敛较慢或需要更多迭代，但最终仍应收敛——若发散，说明初始猜测超出收敛域，可调整初始值或改用阻尼最小二乘法。

关键说明

• 本案例使用3自由度机械臂追踪2维平面位置，雅可比为2×3矩阵，故采用伪逆 pinv 求解最小范数关节增量，而非直接求逆。
• 牛顿-拉夫森法对初始值敏感：初始猜测越接近真实解，收敛越快；远离解时可能振荡或发散。实际工程中可结合解析解或随机多起点策略提供初始值。
• 若需追踪末端姿态（含方向），需将雅可比扩展为3×3（位置+姿态），本案例聚焦位置追踪以简化演示。

常见问题

• 迭代发散：当初始关节角远离真实解时，雅可比矩阵可能接近奇异，导致增量过大。可引入步长缩放因子 α（θ += α·Δθ，α∈(0,1]）或使用阻尼最小二乘法（Levenberg-Marquardt）增强鲁棒性。
• 多解性：逆运动学通常存在多组解，牛顿法只能收敛到初始值附近的解。若需特定构型（如肘部朝上/朝下），需通过初始值选择或关节限位约束实现。
• 雅可比奇异：当机械臂完全伸展或折叠时，雅可比矩阵秩亏，伪逆解可能产生过大关节速度。检测条件数并切换阻尼策略可缓解。