机器人 / 系统课时
用阻尼最小二乘缓解奇异放大
用阻尼最小二乘缓解奇异放大
机械臂接近伸直时,直接用雅可比伪逆追踪末端速度,可能得到远超电机允许值的关节角速度。本课用一台平面双关节机械臂推导阻尼最小二乘逆解,运行数值对比,并按关节速度上限与末端速度误差选择阻尼系数。
学习目标
- 识别小奇异值导致的关节速度放大及其几何原因。
- 推导阻尼最小二乘速度逆解,理解阻尼与跟踪精度的取舍。
- 用可运行程序比较阻尼系数,并按双重约束验收结果。
从近平直位形找出放大来源
考虑两段长度均为 的平面双旋转关节机械臂,取 、。若希望末端沿世界 方向以 运动,速度映射由下式给出:
这里 。当 时两杆伸直, 降秩,关节瞬时速度无法产生沿机械臂伸展方向的末端速度。在 时虽然仍可逆,但条件数约为 :为抵消竖直方向的运动并产生向右速度,两个关节必须反向快速转动。无阻尼计算得到 。

用正则项推导稳定的速度逆解
阻尼最小二乘(DLS)允许牺牲一部分瞬时末端速度精度,以抑制过大的关节角速度。对期望速度 ,最小化速度残差和关节速度惩罚之和:
令目标函数梯度为零,并整理成便于求解任务空间线性方程组的形式,可得:
从奇异值分解看,每个方向的逆映射增益由无阻尼时的 变为 。当 逼近零时,后者趋于零,不再无限放大小奇异值对应的速度分量;代价是 不再为零。式中的 应与雅可比奇异值使用一致尺度。
运行四组阻尼并测量误差
在 Python 3 环境安装 NumPy(python -m pip install numpy),运行以下完整脚本。使用 solve 解线性方程而不是显式计算矩阵逆; 时以伪逆作为基线。误差是当前姿态下的瞬时末端速度残差,不是走完整条轨迹后的定位误差。
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):
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增大阻尼后,最大单关节角速度从 降至 ,但瞬时末端速度残差从近零增至 。阻尼提高数值稳定性,不代表末端仍能保持原来的运动速度。
按实际限制选系数并更新控制
假设两个关节均要求 ,任务允许的瞬时速度残差不超过 。四组候选中, 和 超过关节速度上限, 超过残差上限; 同时满足两项,因而是本例的可用选择。
若控制周期为 ,在上述姿态和 下,两关节角速度约为 ,单周期的角度增量约为 。下一周期应按新的关节角重新计算雅可比,而不能沿用旧矩阵。