机器人 / 系统课时

识别雅可比降秩与奇异位形

识别雅可比降秩与奇异位形

当机械臂接近完全伸直时,即使末端只要求很小的水平速度,求解器也可能给出极大的关节角速度。本篇用平面双旋转关节机械臂检查雅可比的秩、奇异值和条件数,再以具体速度指令验证奇异位形与近奇异位形的差别。

学习目标

  • 根据雅可比行列式与矩阵秩识别真正的奇异位形。
  • 用最小奇异值和条件数衡量接近奇异的程度。
  • 运行数值示例,解释关节速度放大,并选择适当的限制措施。

从末端速度方向判断降秩

设两段连杆长度均为 l1=l2=1 ml_1=l_2=1\,\text{m},关节角为 q1,q2q_1,q_2,只控制末端在平面内的 x,yx,y 位置。正运动学为 x=l1cos⁡q1+l2cos⁡(q1+q2)x=l_1\cos q_1+l_2\cos(q_1+q_2)、y=l1sin⁡q1+l2sin⁡(q1+q2)y=l_1\sin q_1+l_2\sin(q_1+q_2)。对关节角求导得到速度映射 v=J(q)q˙\boldsymbol v=J(q)\dot{\boldsymbol q}。

这个 2×22\times2 雅可比的行列式为:

det⁡J=l1l2sin⁡q2\det J=l_1l_2\sin q_2

当 q2=90∘q_2=90^\circ 时,行列式非零、秩为 2,两个独立关节速度可以组合出任意平面内的瞬时末端速度。当 q2=0∘q_2=0^\circ 或 180∘180^\circ 时,两根连杆共线,雅可比降为秩 1:末端只能产生沿机械臂垂直方向的瞬时线速度,无法以有限关节速度立即产生沿连杆方向的速度。这不表示机械臂永远不能折回,而是当前位置的一阶速度映射失去一个方向。

平面双关节弯曲与完全伸直时的瞬时速度可达方向

用奇异值识别近奇异

仅检查秩会漏掉危险的近奇异情况。对 JJ 做奇异值分解,得到从大到小排列的 σmax⁡\sigma_{\max} 与 σmin⁡\sigma_{\min}:它们对应单位关节速度变化经雅可比映射后,末端速度椭圆的长、短半轴。短半轴接近零时,某个末端速度方向需要很大的关节速度才能实现。

对于非奇异的方阵,二范数条件数是 κ2(J)=σmax⁡/σmin⁡\kappa_2(J)=\sigma_{\max}/\sigma_{\min};最小奇异值为零时,条件数为无穷大。在本例中,q2=90∘q_2=90^\circ 的条件数约 2.6,q2=1∘q_2=1^\circ 仍是秩 2,但条件数已经约 286.5;q2=0∘q_2=0^\circ 才是实际降秩。

工程报警可先试用条件数大于 100,或最小奇异值低于 0.02 的阈值,再结合连杆尺寸、速度上限和标定噪声调整。这不是通用安全阈值。数值秩还依赖计算容差;对于同时混合线速度与角速度的六维雅可比,需先处理量纲与尺度,不能直接照搬本例的条件数阈值。

运行三种位形的速度实验

使用 Python 3.10+ 和 NumPy 1.26+,将下列代码保存为 check_singularity.py,运行 python check_singularity.py。目标末端速度固定为沿 +X+X 方向 0.1 m/s0.1\,\text{m/s},输出关节角速度的单位为 rad/s。示例用伪逆求最小范数的最小二乘解,并同时回算实际末端速度,以免把不可达目标误判为成功。

python
import numpy as np

def jacobian(q1, q2, l1=1.0, l2=1.0):
    # 平面双转动关节到末端X、Y线速度的雅可比
    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)],
    ])

target = np.array([0.1, 0.0])  # 目标速度,m/s
for angle in (90, 1, 0):
    J = jacobian(0.0, np.deg2rad(angle))
    sigma = np.linalg.svd(J, compute_uv=False)
    cond = np.inf if sigma[-1] < 1e-12 else sigma[0] / sigma[-1]
    # 奇异时伪逆仍有解,但不一定能达到目标
    qdot = np.linalg.pinv(J, rcond=1e-12) @ target
    reached = J @ qdot
    print(
        f"q2={angle:>4}° rank={np.linalg.matrix_rank(J, tol=1e-9)} "
        f"sigma=({sigma[0]:.4f},{sigma[-1]:.4f}) cond={cond:.1f} "
        f"qdot=({qdot[0]:.3f},{qdot[1]:.3f}) "
        f"v=({reached[0]:.3f},{reached[1]:.3f})"
    )

J = jacobian(0.0, np.deg2rad(1))
lam = 0.05  # 阻尼限制近奇异位形的速度放大
qdot_dls = J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(2), target)
reached = J @ qdot_dls
print(f"DLS lam={lam:.2f} qdot=({qdot_dls[0]:.3f},{qdot_dls[1]:.3f}) v=({reached[0]:.3f},{reached[1]:.3f})")

预期输出:

text
q2=  90° rank=2 sigma=(1.6180,0.6180) cond=2.6 qdot=(0.000,-0.100) v=(0.100,0.000)
q2=   1° rank=2 sigma=(2.2360,0.0078) cond=286.5 qdot=(5.729,-11.459) v=(0.100,0.000)
q2=   0° rank=1 sigma=(2.2361,0.0000) cond=inf qdot=(0.000,0.000) v=(0.000,0.000)
DLS lam=0.05 qdot=(0.136,-0.273) v=(0.002,-0.001)

其中 q2=1∘q_2=1^\circ 时,目标仅为 0.1 m/s,却要求第二关节约 -11.459 rad/s;若关节额定速度上限为 2 rad/s,就不能按原指令跟踪。q2=0∘q_2=0^\circ 时伪逆输出零速度,并非已达到目标:回算的 vxv_x 也是零,速度残差为 0.1 m/s。

控制器怎样处理危险位形

示例最后一行使用阻尼最小二乘:q˙=JT(JJT+λ2I)−1v\dot{\boldsymbol q}=J^T(JJ^T+\lambda^2 I)^{-1}\boldsymbol v,取 λ=0.05\lambda=0.05 后,关节速度降到约 0.136 和 -0.273 rad/s,但实际水平速度也只剩约 0.002 m/s。阻尼抑制速度放大,不保证原末端速度目标仍能实现。

实际控制循环建议同时检查最小奇异值、条件数、max⁡∣q˙i∣\max|\dot q_i| 与末端速度残差。以本例作为演练:条件数超过 100 时提前减速;预测关节速度超过 2 rad/s 时拒绝直接执行;要求精确水平运动时,先规划到非共线位形再继续。阈值须经机器人限速与实机测试确认,不能只靠伪逆计算成功来判定轨迹安全。

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

目录与阅读设置

阅读设置

17

记录一个想法

识别雅可比降秩与奇异位形

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

登录哪都通