4. 阻尼最小二乘法逆运动学

任务目标

本案例实现阻尼最小二乘法（Damped Least Squares, DLS）求解平面2连杆机械臂的逆运动学。核心目标是：在关节速度层使用 Jᵀ(J·Jᵀ + λ²I)⁻¹ 代替伪逆 J⁺，使机械臂经过奇异位形（两臂完全伸直）附近时关节速度仍然有界，并对比不同阻尼系数 λ 对末端轨迹精度与关节速度平滑度的影响。

实现原理与关键代码

对于末端速度 ẋ 与关节速度 q̇，DLS 解为：

q̇ = Jᵀ(J·Jᵀ + λ²I)⁻¹·ẋ

当机械臂接近奇异位形时，J·Jᵀ 的最小特征值趋近于零，直接求伪逆会导致 q̇ 趋于无穷大。加入阻尼项 λ²I 后，矩阵条件数被限制，从而保证数值稳定。λ 越大，关节速度越平滑，但末端跟踪误差也越大。

import numpy as np
import matplotlib.pyplot as plt

def forward_kinematics(q, l1=1.0, l2=1.0):
    """平面2连杆正运动学，返回末端位置 (x, y)"""
    x = l1*np.cos(q[0]) + l2*np.cos(q[0]+q[1])
    y = l1*np.sin(q[0]) + l2*np.sin(q[0]+q[1])
    return np.array([x, y])

def jacobian(q, l1=1.0, l2=1.0):
    """解析雅可比矩阵 2x2"""
    return np.array([
        [-l1*np.sin(q[0]) - l2*np.sin(q[0]+q[1]), -l2*np.sin(q[0]+q[1])],
        [ l1*np.cos(q[0]) + l2*np.cos(q[0]+q[1]),  l2*np.cos(q[0]+q[1])]
    ])

def dls_ik(q_current, x_target, lam, step=0.1, max_iter=50, tol=1e-3):
    """
    阻尼最小二乘逆运动学迭代求解
    返回: 最终关节角, 是否收敛, 迭代次数
    """
    q = q_current.copy()
    for i in range(max_iter):
        x_cur = forward_kinematics(q)
        err = x_target - x_cur
        if np.linalg.norm(err) < tol:
            return q, True, i
        J = jacobian(q)
        # DLS核心：J^T (J J^T + lam^2 I)^{-1}
        A = J @ J.T + lam**2 * np.eye(2)
        dq = J.T @ np.linalg.solve(A, err)
        q = q + step * dq
    return q, False, max_iter

# ========== 测试：直线轨迹经过奇异点 ==========
# 目标轨迹：从 (1.8, 0.2) 直线运动到 (2.0, 0.0)（奇异点）再返回
# 奇异位形：q = [0, 0] 时末端在 (2.0, 0.0)
l1 = l2 = 1.0
q_init = np.array([0.2, 0.5])   # 起始关节角
targets = []
for t in np.linspace(0, 1, 30):
    x = 1.8 + 0.2*t
    y = 0.2 - 0.2*t
    targets.append([x, y])
for t in np.linspace(0, 1, 30):
    x = 2.0 - 0.2*t
    y = 0.0 + 0.2*t
    targets.append([x, y])
targets = np.array(targets)

# 对比不同阻尼系数
for lam in [0.0, 0.01, 0.1, 0.5]:
    q_traj = []
    q = q_init.copy()
    success_count = 0
    for target in targets:
        q, conv, _ = dls_ik(q, target, lam)
        if conv:
            success_count += 1
        q_traj.append(q.copy())
    q_traj = np.array(q_traj)
    
    # 计算末端实际轨迹与误差
    x_traj = np.array([forward_kinematics(qi) for qi in q_traj])
    pos_err = np.linalg.norm(x_traj - targets, axis=1)
    # 关节速度（差分近似）
    q_vel = np.diff(q_traj, axis=0) / 0.01
    
    print(f"λ={lam:.2f} | 收敛率: {success_count}/{len(targets)} | "
          f"平均位置误差: {pos_err.mean():.4f} m | "
          f"最大关节速度: {np.abs(q_vel).max():.3f} rad/s")

# ========== 可视化对比 ==========
fig, axes = plt.subplots(1, 3, figsize=(14, 4))

# 左图：末端轨迹
for lam, color in zip([0.0, 0.01, 0.1, 0.5], ['r', 'g', 'b', 'm']):
    q_traj = []
    q = q_init.copy()
    for target in targets:
        q, _, _ = dls_ik(q, target, lam)
        q_traj.append(q.copy())
    x_traj = np.array([forward_kinematics(qi) for qi in q_traj])
    axes[0].plot(x_traj[:, 0], x_traj[:, 1], color=color, label=f'λ={lam}')
axes[0].plot(targets[:, 0], targets[:, 1], 'k--', label='期望轨迹')
axes[0].plot(2.0, 0.0, 'ro', markersize=8)  # 奇异点
axes[0].set_xlabel('x (m)'); axes[0].set_ylabel('y (m)')
axes[0].legend(); axes[0].set_title('末端轨迹对比'); axes[0].axis('equal')

# 中图：位置误差
for lam, color in zip([0.0, 0.01, 0.1, 0.5], ['r', 'g', 'b', 'm']):
    q_traj = []
    q = q_init.copy()
    for target in targets:
        q, _, _ = dls_ik(q, target, lam)
        q_traj.append(q.copy())
    x_traj = np.array([forward_kinematics(qi) for qi in q_traj])
    err = np.linalg.norm(x_traj - targets, axis=1)
    axes[1].plot(err, color=color, label=f'λ={lam}')
axes[1].set_xlabel('时间步'); axes[1].set_ylabel('位置误差 (m)')
axes[1].legend(); axes[1].set_title('跟踪误差')

# 右图：关节1速度
for lam, color in zip([0.0, 0.01, 0.1, 0.5], ['r', 'g', 'b', 'm']):
    q_traj = []
    q = q_init.copy()
    for target in targets:
        q, _, _ = dls_ik(q, target, lam)
        q_traj.append(q.copy())
    q_traj = np.array(q_traj)
    q_vel = np.diff(q_traj[:, 0]) / 0.01
    axes[2].plot(q_vel, color=color, label=f'λ={lam}')
axes[2].set_xlabel('时间步'); axes[2].set_ylabel('关节1速度 (rad/s)')
axes[2].legend(); axes[2].set_title('关节速度对比')

plt.tight_layout()
plt.savefig('dls_ik_comparison.png', dpi=150)
plt.show()

运行与验证

环境依赖：Python 3.8+，numpy，matplotlib。

运行方式：将上述代码保存为 dls_ik.py，在终端执行 python dls_ik.py。

验证方法：

1. 观察终端输出的收敛率与误差数据：λ=0 时（纯伪逆）在奇异点附近迭代可能不收敛或关节速度极大；λ>0 时收敛率应接近 100%。
2. 查看生成的 dls_ik_comparison.png：左图中 λ=0 的轨迹在奇异点附近出现明显偏离，λ 越大轨迹越平滑但偏离期望轨迹越远；中图显示 λ 越大稳态误差越大；右图显示 λ=0 时关节速度在奇异点附近出现尖峰，λ 增大后尖峰被抑制。
3. 判定标准：λ=0.1 时最大关节速度应显著小于 λ=0 的情况（通常降低 5 倍以上），且平均位置误差保持在 0.01m 量级以内，即认为 DLS 有效解决了奇异点数值稳定性问题。

关键参数说明

• λ（阻尼系数）：λ 过小（如 0.01）接近伪逆，奇异点附近仍可能速度过大；λ 过大（如 0.5）轨迹平滑但精度下降。实际应用中 λ 可取 0.05~0.2 作为折中。
• step（步长）：控制每次迭代的关节角更新幅度，0.1 为经验值，过大易振荡，过小收敛慢。
• 奇异点位置：本案例中 q=[0,0] 对应末端 (2.0, 0.0)，即两臂完全伸直，此时雅可比矩阵秩亏。