机器人 / 系统课时

用数值差分验证雅可比矩阵

用数值差分验证雅可比矩阵

解析雅可比可能因关节轴方向、连杆长度或求导符号写错而给出错误速度。本课用平面双关节机械臂作可运行校验:每次只扰动一个关节,用正运动学的位姿变化估算雅可比的一列,再与解析矩阵逐项比对,定位出错位置。

学习目标

  • 用中心差分从正运动学估算每个关节对应的雅可比列。
  • 运行 Python 脚本,比较解析矩阵与数值矩阵并判定误差。
  • 根据异常列、单位和扰动步长排查常见错误。

确定待验证的位姿与单位

使用平面二旋转关节机械臂:连杆长 l1=1.0 ml_1=1.0\,\mathrm{m}、l2=0.7 ml_2=0.7\,\mathrm{m},关节角 q1=30∘q_1=30^\circ、q2=−45∘q_2=-45^\circ。关节角在计算中统一转为弧度,末端位姿记为 F(q)=[x,y,ϕ]TF(q)=[x,y,\phi]^T。

x=l1cos⁡q1+l2cos⁡(q1+q2),y=l1sin⁡q1+l2sin⁡(q1+q2)x=l_1\cos q_1+l_2\cos(q_1+q_2),\quad y=l_1\sin q_1+l_2\sin(q_1+q_2)

末端朝向 ϕ=q1+q2\phi=q_1+q_2。该位置应约为 (x,y)=(1.542173,0.318827) m(x,y)=(1.542173,0.318827)\,\mathrm{m},朝向为 −15∘-15^\circ。待验证的解析雅可比是 3×23\times2 矩阵:前两行对应位置对关节角的导数,第三行对应朝向对关节角的导数;其列单位分别为米每弧度和弧度每弧度。速度映射为 F˙=Jq˙\dot F=J\dot q。

单独扰动关节得到差分列

对第 jj 个关节,其他关节角保持不变,分别计算 F(q+hej)F(q+h e_j) 和 F(q−hej)F(q-h e_j)。取 h=10−6 radh=10^{-6}\,\mathrm{rad},中心差分为:

J:,jnum≈F(q+hej)−F(q−hej)2hJ_{:,j}^{\mathrm{num}}\approx\frac{F(q+h e_j)-F(q-h e_j)}{2h}

其中 eje_j 只在第 jj 个位置为 1。中心差分的截断误差通常为 O(h2)O(h^2),比同样步长的单边差分更小。朝向差值必须取最短角差,避免跨越 ±π\pm\pi 时出现错误的大跳变。

平面双关节机械臂两个关节微扰与雅可比列方向

运行解析与数值对照

下面只用 Python 标准库,无需安装第三方包。代码把末端朝向的两个采样值先相减,再归一化到 [−π,π)[-\pi,\pi),避免角度跳变;打印的每一列对应一个关节。

python
import math

L1, L2 = 1.0, 0.7
q = [math.radians(30), math.radians(-45)]
h = 1e-6  # 扰动使用弧度,而非角度

def wrap(angle):
    return (angle + math.pi) % (2 * math.pi) - math.pi

def fk(angles):
    a, b = angles
    t = a + b
    # 位姿顺序固定为 x(m)、y(m)、yaw(rad)
    return [L1 * math.cos(a) + L2 * math.cos(t),
            L1 * math.sin(a) + L2 * math.sin(t), t]

def analytic_jacobian(angles):
    a, b = angles
    t = a + b
    return [
        [-L1 * math.sin(a) - L2 * math.sin(t), -L2 * math.sin(t)],
        [ L1 * math.cos(a) + L2 * math.cos(t),  L2 * math.cos(t)],
        [1.0, 1.0],
    ]

def numeric_jacobian(angles, step):
    columns = []
    for j in range(2):
        plus, minus = angles.copy(), angles.copy()
        plus[j] += step
        minus[j] -= step  # 其余关节始终不变
        p, m = fk(plus), fk(minus)
        changes = [p[0] - m[0], p[1] - m[1],
                   wrap(p[2] - m[2])]  # 朝向用最短角差
        columns.append([value / (2 * step) for value in changes])
    return [[columns[j][i] for j in range(2)] for i in range(3)]

analytic = analytic_jacobian(q)
numeric = numeric_jacobian(q, h)
for name, matrix in [("解析", analytic), ("差分", numeric)]:
    print(name)
    for row in matrix:
        print("  " + " ".join(f"{value:+.6f}" for value in row))

max_error = max(abs(analytic[i][j] - numeric[i][j])
                for i in range(3) for j in range(2))
print(f"最大绝对误差: {max_error:.2e}")
print("判定:", "PASS" if max_error < 1e-6 else "FAIL")

预期输出(浮点数末位可能随运行环境略有差异):

text
解析
  -0.318827 +0.181173
  +1.542173 +0.676148
  +1.000000 +1.000000
差分
  -0.318827 +0.181173
  +1.542173 +0.676148
  +1.000000 +1.000000
最大绝对误差: 1.40e-10
判定: PASS

脚本采用最大绝对误差小于 10−610^{-6} 作为这个米制示例的通过阈值,并非所有机器人通用的公差。实际项目应分别为平移和角速度行设置容差,还要考虑不同关节类型的单位。

从误差分布定位导数问题

不要只看最终的 PASS/FAIL:逐列查看差值 Janalytic−JnumJ^{\mathrm{analytic}}-J^{\mathrm{num}},可更快判断错误来源。

观察到的现象 优先检查 修正与复验
第一、第二列的位置行都偏差明显 第一连杆长度、基座轴方向,以及共同角 q1+q2q_1+q_2 核对长度单位与三角函数符号后重算
只有第二列偏差明显 第二关节轴、第二连杆或把 q2q_2 当绝对角度 第二连杆使用 q1+q2q_1+q_2,再逐列比较
位置两行吻合,朝向行不吻合 角度/弧度混用、角差未归一化 将扰动统一为弧度,对角差做最短弧处理
误差随 hh 缩小反而增大 浮点舍入造成相减消去 试 10−410^{-4}、10−510^{-5}、10−610^{-6}、10−710^{-7} rad,对比误差趋势

再以 q=(0∘,0∘)q=(0^\circ,0^\circ) 和 q=(90∘,0∘)q=(90^\circ,0^\circ) 进行回归:前者末端位置应为 (1.7,0)(1.7,0) 米、位置雅可比的 xx 行应为零;后者末端位置应为 (0,1.7)(0,1.7) 米、位置雅可比的 yy 行应接近零。靠近关节限位时若无法双侧扰动,应改用合法侧的单边差分,并相应放宽误差预期。

当多组角度、多档扰动步长都能稳定复现小误差时,才适合将这套逐列差分检查加入运动学回归测试。

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

目录与阅读设置

阅读设置

17

记录一个想法

用数值差分验证雅可比矩阵

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

登录哪都通