机器人 / 系统课时
检验奇异位形附近的求解稳定性
检验奇异位形附近的求解稳定性
将双关节机械臂摆到几乎伸直的位置,用同一个可达目标检验雅可比伪逆与阻尼最小二乘。实验会记录未限速的关节速度峰值、达到位置误差阈值所需步数和最终末端误差,帮助区分数值放大、限速影响与阻尼过强。
学习目标
- 用最小奇异值识别接近奇异的关节配置。
- 在相同目标、控制周期和限速下比较两种逆解。
- 根据速度峰值、收敛步数及末端误差定位不稳定原因。
构造近奇异测试配置
选用平面二旋转关节机械臂,两根连杆各长 1 m。第一关节角从世界 X 轴逆时针计量,第二关节角相对第一连杆逆时针计量。起始角度为 、,机械臂接近完全伸直;目标 TCP 位置设为 m,距基座约 1.993 m,处于半径 2 m 的几何可达范围内。

验收条件固定为:控制周期 0.02 s、误差反馈增益 、每个关节速度上限 、最大 150 次更新,TCP 位置误差小于 1 mm 才算达标。两种方法均使用相同的逐关节限速,保证差异来自求解器而非执行条件。
伪逆与阻尼为什么给出不同速度
设 是将关节角速度映射到末端 XY 线速度的雅可比, 是期望末端速度。两种更新使用:
近伸直时,雅可比的两列几乎平行,末端沿连杆方向的瞬时运动能力很弱。本例初始最小奇异值约为 0.003903;伪逆会对这一弱方向产生很大的关节速度指令。阻尼最小二乘取 ,抑制放大,但也会牺牲弱方向上的瞬时跟踪精度。这里比较的是二维位置,不含末端姿态误差。
运行同条件数值实验
使用 Python 3.10+ 和 NumPy 1.24+;缺少依赖时可先执行 python -m pip install numpy。保存下面代码为 singularity_test.py 并运行 python singularity_test.py。代码每步重新计算正运动学和雅可比,先记录原始速度指令,再施加相同的 0.3 rad/s 限速。
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")预期输出:
初始误差: 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 阈值。这说明阻尼过大会导致响应过慢,并非数值越大越安全。也可以把初始 改为 和 ,观察最小奇异值与速度峰值怎样变化。
若从 完全伸直位置出发,目标误差又恰好只有径向分量,一阶雅可比在该方向没有响应,伪逆与阻尼法都可能停滞。此时应改用稍微弯曲的初值或引入脱离奇异的策略,而不是无限增大反馈增益。真实机器人还需分别校验关节位置、加速度、碰撞限制;本实验的逐关节限速不等于完整的运动安全约束。