机器人 / 系统课时
用数值差分验证雅可比矩阵
用数值差分验证雅可比矩阵
解析雅可比可能因关节轴方向、连杆长度或求导符号写错而给出错误速度。本课用平面双关节机械臂作可运行校验:每次只扰动一个关节,用正运动学的位姿变化估算雅可比的一列,再与解析矩阵逐项比对,定位出错位置。
学习目标
- 用中心差分从正运动学估算每个关节对应的雅可比列。
- 运行 Python 脚本,比较解析矩阵与数值矩阵并判定误差。
- 根据异常列、单位和扰动步长排查常见错误。
确定待验证的位姿与单位
使用平面二旋转关节机械臂:连杆长 、,关节角 、。关节角在计算中统一转为弧度,末端位姿记为 。
末端朝向 。该位置应约为 ,朝向为 。待验证的解析雅可比是 矩阵:前两行对应位置对关节角的导数,第三行对应朝向对关节角的导数;其列单位分别为米每弧度和弧度每弧度。速度映射为 。
单独扰动关节得到差分列
对第 个关节,其他关节角保持不变,分别计算 和 。取 ,中心差分为:
其中 只在第 个位置为 1。中心差分的截断误差通常为 ,比同样步长的单边差分更小。朝向差值必须取最短角差,避免跨越 时出现错误的大跳变。

运行解析与数值对照
下面只用 Python 标准库,无需安装第三方包。代码把末端朝向的两个采样值先相减,再归一化到 ,避免角度跳变;打印的每一列对应一个关节。
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脚本采用最大绝对误差小于 作为这个米制示例的通过阈值,并非所有机器人通用的公差。实际项目应分别为平移和角速度行设置容差,还要考虑不同关节类型的单位。
从误差分布定位导数问题
不要只看最终的 PASS/FAIL:逐列查看差值 ,可更快判断错误来源。
| 观察到的现象 | 优先检查 | 修正与复验 |
|---|---|---|
| 第一、第二列的位置行都偏差明显 | 第一连杆长度、基座轴方向,以及共同角 | 核对长度单位与三角函数符号后重算 |
| 只有第二列偏差明显 | 第二关节轴、第二连杆或把 当绝对角度 | 第二连杆使用 ,再逐列比较 |
| 位置两行吻合,朝向行不吻合 | 角度/弧度混用、角差未归一化 | 将扰动统一为弧度,对角差做最短弧处理 |
| 误差随 缩小反而增大 | 浮点舍入造成相减消去 | 试 、、、 rad,对比误差趋势 |
再以 和 进行回归:前者末端位置应为 米、位置雅可比的 行应为零;后者末端位置应为 米、位置雅可比的 行应接近零。靠近关节限位时若无法双侧扰动,应改用合法侧的单边差分,并相应放宽误差预期。
当多组角度、多档扰动步长都能稳定复现小误差时,才适合将这套逐列差分检查加入运动学回归测试。
按自己的节奏,确认真正理解。