机器人 / 系统课时

用阻尼最小二乘缓解奇异放大

用阻尼最小二乘缓解奇异放大

机械臂接近伸直时,直接用雅可比伪逆追踪末端速度,可能得到远超电机允许值的关节角速度。本课用一台平面双关节机械臂推导阻尼最小二乘逆解,运行数值对比,并按关节速度上限与末端速度误差选择阻尼系数。

学习目标

  • 识别小奇异值导致的关节速度放大及其几何原因。
  • 推导阻尼最小二乘速度逆解,理解阻尼与跟踪精度的取舍。
  • 用可运行程序比较阻尼系数,并按双重约束验收结果。

从近平直位形找出放大来源

考虑两段长度均为 1 m1\ \mathrm{m} 的平面双旋转关节机械臂,取 q1=0∘q_1=0^\circ、q2=5∘q_2=5^\circ。若希望末端沿世界 +x+x 方向以 0.10 m/s0.10\ \mathrm{m/s} 运动,速度映射由下式给出:

p˙=Jq˙,J=[−l1sin⁡q1−l2sin⁡(q1+q2)−l2sin⁡(q1+q2)l1cos⁡q1+l2cos⁡(q1+q2)l2cos⁡(q1+q2)]\dot{\boldsymbol p}=J\dot{\boldsymbol q},\quad J=\begin{bmatrix} -l_1\sin q_1-l_2\sin(q_1+q_2)&-l_2\sin(q_1+q_2)\\ l_1\cos q_1+l_2\cos(q_1+q_2)&l_2\cos(q_1+q_2) \end{bmatrix}

这里 det⁡J=l1l2sin⁡q2\det J=l_1l_2\sin q_2。当 q2=0∘q_2=0^\circ 时两杆伸直,JJ 降秩,关节瞬时速度无法产生沿机械臂伸展方向的末端速度。在 5∘5^\circ 时虽然仍可逆,但条件数约为 57.2657.26:为抵消竖直方向的运动并产生向右速度,两个关节必须反向快速转动。无阻尼计算得到 q˙≈(1.143,−2.290) rad/s\dot{\boldsymbol q}\approx(1.143,-2.290)\ \mathrm{rad/s}。

近平直双关节机械臂的弱速度方向与阻尼取舍

用正则项推导稳定的速度逆解

阻尼最小二乘(DLS)允许牺牲一部分瞬时末端速度精度,以抑制过大的关节角速度。对期望速度 vd\boldsymbol v_d,最小化速度残差和关节速度惩罚之和:

q˙λ=argmin⁡q˙(∥Jq˙−vd∥22+λ2∥q˙∥22)\dot{\boldsymbol q}_\lambda= \underset{\dot{\boldsymbol q}}{\operatorname{argmin}} \left(\|J\dot{\boldsymbol q}-\boldsymbol v_d\|_2^2+ \lambda^2\|\dot{\boldsymbol q}\|_2^2\right)

令目标函数梯度为零,并整理成便于求解任务空间线性方程组的形式,可得:

q˙λ=JT(JJT+λ2I)−1vd,λ>0\dot{\boldsymbol q}_\lambda= J^{\mathsf T}(JJ^{\mathsf T}+\lambda^2 I)^{-1}\boldsymbol v_d,\qquad \lambda>0

从奇异值分解看,每个方向的逆映射增益由无阻尼时的 1/σ1/\sigma 变为 σ/(σ2+λ2)\sigma/(\sigma^2+\lambda^2)。当 σ\sigma 逼近零时,后者趋于零,不再无限放大小奇异值对应的速度分量;代价是 ∥Jq˙λ−vd∥2\|J\dot{\boldsymbol q}_\lambda-\boldsymbol v_d\|_2 不再为零。式中的 λ\lambda 应与雅可比奇异值使用一致尺度。

运行四组阻尼并测量误差

在 Python 3 环境安装 NumPy(python -m pip install numpy),运行以下完整脚本。使用 solve 解线性方程而不是显式计算矩阵逆;λ=0\lambda=0 时以伪逆作为基线。误差是当前姿态下的瞬时末端速度残差,不是走完整条轨迹后的定位误差。

python
import numpy as np

def jacobian_2r(q1, q2, l1=1.0, l2=1.0):
    return np.array([
        [-l1*np.sin(q1)-l2*np.sin(q1+q2), -l2*np.sin(q1+q2)],
        [ l1*np.cos(q1)+l2*np.cos(q1+q2),  l2*np.cos(q1+q2)]
    ])

def solve_dls(jac, target, damping):
    if damping == 0.0:
        return np.linalg.pinv(jac) @ target
    # 解线性方程组,避免显式求逆带来的额外数值误差
    return jac.T @ np.linalg.solve(
        jac @ jac.T + damping**2 * np.eye(2), target
    )

def main():
    # 接近完全伸直的姿态,输入关节角统一使用弧度
    jac = jacobian_2r(0.0, np.deg2rad(5.0))
    target = np.array([0.10, 0.0])  # 末端期望速度,单位 m/s
    print(f"cond(J) = {np.linalg.cond(jac):.2f}")
    print("lambda  max|dq|  |dq|   error  vx_actual")
    for damping in (0.0, 0.02, 0.05, 0.10):
        dq = solve_dls(jac, target, damping)
        actual = jac @ dq
        error = np.linalg.norm(target - actual)
        print(f"{damping:>5.2f}   {np.max(np.abs(dq)):>6.3f}  "
              f"{np.linalg.norm(dq):>5.3f}  {error:>6.3f}  {actual[0]:>9.3f}")

if __name__ == "__main__":
    main()

预期输出(速度单位为 rad/s,残差和 vx_actual 的单位为 m/s):

text
cond(J) = 57.26
lambda  max|dq|  |dq|   error  vx_actual
 0.00    2.290  2.560   0.000      0.100
 0.02    1.814  2.027   0.021      0.079
 0.05    0.867  0.969   0.062      0.038
 0.10    0.303  0.338   0.087      0.013

增大阻尼后,最大单关节角速度从 2.2902.290 降至 0.303 rad/s0.303\ \mathrm{rad/s},但瞬时末端速度残差从近零增至 0.087 m/s0.087\ \mathrm{m/s}。阻尼提高数值稳定性,不代表末端仍能保持原来的运动速度。

按实际限制选系数并更新控制

假设两个关节均要求 ∣q˙i∣≤1.0 rad/s|\dot q_i|\le 1.0\ \mathrm{rad/s},任务允许的瞬时速度残差不超过 0.070 m/s0.070\ \mathrm{m/s}。四组候选中,λ=0\lambda=0 和 0.020.02 超过关节速度上限,λ=0.10\lambda=0.10 超过残差上限;λ=0.05\lambda=0.05 同时满足两项,因而是本例的可用选择。

若控制周期为 10 ms10\ \mathrm{ms},在上述姿态和 λ=0.05\lambda=0.05 下,两关节角速度约为 (0.431,−0.867) rad/s(0.431,-0.867)\ \mathrm{rad/s},单周期的角度增量约为 (0.00431,−0.00867) rad(0.00431,-0.00867)\ \mathrm{rad}。下一周期应按新的关节角重新计算雅可比,而不能沿用旧矩阵。

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

目录与阅读设置

阅读设置

17

记录一个想法

用阻尼最小二乘缓解奇异放大

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

登录哪都通