机器人 / 系统课时
识别雅可比降秩与奇异位形
识别雅可比降秩与奇异位形
当机械臂接近完全伸直时,即使末端只要求很小的水平速度,求解器也可能给出极大的关节角速度。本篇用平面双旋转关节机械臂检查雅可比的秩、奇异值和条件数,再以具体速度指令验证奇异位形与近奇异位形的差别。
学习目标
- 根据雅可比行列式与矩阵秩识别真正的奇异位形。
- 用最小奇异值和条件数衡量接近奇异的程度。
- 运行数值示例,解释关节速度放大,并选择适当的限制措施。
从末端速度方向判断降秩
设两段连杆长度均为 ,关节角为 ,只控制末端在平面内的 位置。正运动学为 、。对关节角求导得到速度映射 。
这个 雅可比的行列式为:
当 时,行列式非零、秩为 2,两个独立关节速度可以组合出任意平面内的瞬时末端速度。当 或 时,两根连杆共线,雅可比降为秩 1:末端只能产生沿机械臂垂直方向的瞬时线速度,无法以有限关节速度立即产生沿连杆方向的速度。这不表示机械臂永远不能折回,而是当前位置的一阶速度映射失去一个方向。

用奇异值识别近奇异
仅检查秩会漏掉危险的近奇异情况。对 做奇异值分解,得到从大到小排列的 与 :它们对应单位关节速度变化经雅可比映射后,末端速度椭圆的长、短半轴。短半轴接近零时,某个末端速度方向需要很大的关节速度才能实现。
对于非奇异的方阵,二范数条件数是 ;最小奇异值为零时,条件数为无穷大。在本例中, 的条件数约 2.6, 仍是秩 2,但条件数已经约 286.5; 才是实际降秩。
工程报警可先试用条件数大于 100,或最小奇异值低于 0.02 的阈值,再结合连杆尺寸、速度上限和标定噪声调整。这不是通用安全阈值。数值秩还依赖计算容差;对于同时混合线速度与角速度的六维雅可比,需先处理量纲与尺度,不能直接照搬本例的条件数阈值。
运行三种位形的速度实验
使用 Python 3.10+ 和 NumPy 1.26+,将下列代码保存为 check_singularity.py,运行 python check_singularity.py。目标末端速度固定为沿 方向 ,输出关节角速度的单位为 rad/s。示例用伪逆求最小范数的最小二乘解,并同时回算实际末端速度,以免把不可达目标误判为成功。
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})")预期输出:
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)其中 时,目标仅为 0.1 m/s,却要求第二关节约 -11.459 rad/s;若关节额定速度上限为 2 rad/s,就不能按原指令跟踪。 时伪逆输出零速度,并非已达到目标:回算的 也是零,速度残差为 0.1 m/s。
控制器怎样处理危险位形
示例最后一行使用阻尼最小二乘:,取 后,关节速度降到约 0.136 和 -0.273 rad/s,但实际水平速度也只剩约 0.002 m/s。阻尼抑制速度放大,不保证原末端速度目标仍能实现。
实际控制循环建议同时检查最小奇异值、条件数、 与末端速度残差。以本例作为演练:条件数超过 100 时提前减速;预测关节速度超过 2 rad/s 时拒绝直接执行;要求精确水平运动时,先规划到非共线位形再继续。阈值须经机器人限速与实机测试确认,不能只靠伪逆计算成功来判定轨迹安全。