机器人 / 系统课时

检验奇异位形附近的求解稳定性

检验奇异位形附近的求解稳定性

将双关节机械臂摆到几乎伸直的位置,用同一个可达目标检验雅可比伪逆与阻尼最小二乘。实验会记录未限速的关节速度峰值、达到位置误差阈值所需步数和最终末端误差,帮助区分数值放大、限速影响与阻尼过强。

学习目标

  • 用最小奇异值识别接近奇异的关节配置。
  • 在相同目标、控制周期和限速下比较两种逆解。
  • 根据速度峰值、收敛步数及末端误差定位不稳定原因。

构造近奇异测试配置

选用平面二旋转关节机械臂,两根连杆各长 1 m。第一关节角从世界 X 轴逆时针计量,第二关节角相对第一连杆逆时针计量。起始角度为 q1=0∘q_1=0^\circ、q2=0.5∘q_2=0.5^\circ,机械臂接近完全伸直;目标 TCP 位置设为 (1.99,0.10)(1.99,0.10) m,距基座约 1.993 m,处于半径 2 m 的几何可达范围内。

近伸直机械臂的弱速度方向及两种求解器比较

验收条件固定为:控制周期 0.02 s、误差反馈增益 4 s−14\ \mathrm{s}^{-1}、每个关节速度上限 0.3 rad/s0.3\ \mathrm{rad/s}、最大 150 次更新,TCP 位置误差小于 1 mm 才算达标。两种方法均使用相同的逐关节限速,保证差异来自求解器而非执行条件。

伪逆与阻尼为什么给出不同速度

设 JJ 是将关节角速度映射到末端 XY 线速度的雅可比,v=4(ptarget−p)v=4(p_{\mathrm{target}}-p) 是期望末端速度。两种更新使用:

q˙pinv=J+v\dot q_{\mathrm{pinv}}=J^+v
q˙dls=JT(JJT+λ2I)−1v\dot q_{\mathrm{dls}}=J^T(JJ^T+\lambda^2I)^{-1}v

近伸直时,雅可比的两列几乎平行,末端沿连杆方向的瞬时运动能力很弱。本例初始最小奇异值约为 0.003903;伪逆会对这一弱方向产生很大的关节速度指令。阻尼最小二乘取 λ=0.05\lambda=0.05,抑制放大,但也会牺牲弱方向上的瞬时跟踪精度。这里比较的是二维位置,不含末端姿态误差。

运行同条件数值实验

使用 Python 3.10+ 和 NumPy 1.24+;缺少依赖时可先执行 python -m pip install numpy。保存下面代码为 singularity_test.py 并运行 python singularity_test.py。代码每步重新计算正运动学和雅可比,先记录原始速度指令,再施加相同的 0.3 rad/s 限速。

python
import numpy as np

def forward(q):
    a, b = q
    return np.array([np.cos(a) + np.cos(a + b),
                     np.sin(a) + np.sin(a + b)])

def jacobian(q):
    a, b = q
    return np.array([
        [-np.sin(a) - np.sin(a + b), -np.sin(a + b)],
        [ np.cos(a) + np.cos(a + b),  np.cos(a + b)]
    ])

def run(method, initial, target):
    q = initial.copy()
    first = peak = 0.0
    for k in range(151):
        error = target - forward(q)
        distance = np.linalg.norm(error)
        if distance < 0.001 or k == 150:
            return k, distance, first, peak
        J = jacobian(q)
        v = 4.0 * error  # 位置误差反馈增益,单位 1/s
        if method == "pinv":
            qdot = np.linalg.pinv(J) @ v
        else:
            # 正则化逆解,避免小奇异值引起的速度放大
            qdot = J.T @ np.linalg.solve(J @ J.T + 0.05**2 * np.eye(2), v)
        raw = np.max(np.abs(qdot))
        if k == 0:
            first = raw
        peak = max(peak, raw)
        # 两种方法使用相同的关节限速与周期
        q += np.clip(qdot, -0.3, 0.3) * 0.02

initial = np.deg2rad([0.0, 0.5])
target = np.array([1.99, 0.10])
sigma = np.linalg.svd(jacobian(initial), compute_uv=False)
print(f"初始误差: {np.linalg.norm(target - forward(initial)):.6f} m")
print(f"最小奇异值: {sigma[-1]:.6f}")
for method in ("pinv", "dls"):
    k, error, first, peak = run(method, initial, target)
    status = "达标" if error < 0.001 else "未达标"
    print(f"{method}: {status}, 迭代={k}, 末端误差={error*1000:.3f} mm, "
          f"首步峰值={first:.3f} rad/s, 全程峰值={peak:.3f} rad/s")

预期输出:

text
初始误差: 0.091815 m
最小奇异值: 0.003903
pinv: 达标, 迭代=83, 末端误差=0.956 mm, 首步峰值=8.767 rad/s, 全程峰值=8.767 rad/s
dls: 达标, 迭代=61, 末端误差=0.954 mm, 首步峰值=0.126 rad/s, 全程峰值=0.219 rad/s

依据读数定位不稳定

两种方法都满足 1 mm 阈值,但伪逆首步要求的最大关节速度为 8.767 rad/s,远超 0.3 rad/s 的执行上限,因此发生限速并用 83 步收敛。阻尼方法全程原始速度峰值只有 0.219 rad/s,在本组参数下无需触发限速,61 步即可达标。原始速度指令不是实际执行速度;只看最终误差会漏掉这类风险。

可将代码里的阻尼系数由 0.05 改为 0.20 再运行。在同样的 150 步预算内,阻尼法末端误差约为 6.59 mm,未达到 1 mm 阈值。这说明阻尼过大会导致响应过慢,并非数值越大越安全。也可以把初始 q2q_2 改为 0.1∘0.1^\circ 和 5∘5^\circ,观察最小奇异值与速度峰值怎样变化。

若从 q2=0∘q_2=0^\circ 完全伸直位置出发,目标误差又恰好只有径向分量,一阶雅可比在该方向没有响应,伪逆与阻尼法都可能停滞。此时应改用稍微弯曲的初值或引入脱离奇异的策略,而不是无限增大反馈增益。真实机器人还需分别校验关节位置、加速度、碰撞限制;本实验的逐关节限速不等于完整的运动安全约束。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

检验奇异位形附近的求解稳定性

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通