📚 课程目录第4章误差建模与参数辨识理论

4. 误差建模与参数辨识理论

好，咱们进入第四章。这一章是理论核心，也是很多工程师觉得「劝退」的地方。但我想说，搞懂误差建模和参数辨识，你才算真正入了机器人标定的门。我当年在产线上调试一台六轴机器人，重复定位精度死活提不上去，折腾了两周，最后发现就是参数辨识的模型没建对。嗯，从那以后，我再也不敢轻视这部分理论了。

4.1 运动学参数误差模型

先问一个问题：为什么我们明明用了高精度的编码器，机器人末端还是会偏？

说白了，你给控制器发的指令是「理论关节角」，但实际机器人身上有加工公差、装配间隙、减速器背隙、连杆变形……这些误差叠加起来，末端位置就跑了。我们需要一个数学模型，把这些误差和末端位姿偏差之间的关系描述清楚。

我个人习惯用 MDH（Modified Denavit-Hartenberg） 参数法来建模。每个关节有四个参数：

参数	符号	物理含义	常见误差来源
连杆长度	a_i	相邻关节轴线的公垂线距离	加工误差、装配偏差
连杆扭角	α_i	相邻关节轴线的空间夹角	铸造变形、热处理变形
关节距离	d_i	沿关节轴线的偏移量	垫片厚度、轴承游隙
关节转角	θ_i	绕关节轴线的旋转角度	编码器零位偏移、减速比误差

每个参数都有一个名义值和一个实际值。误差就是两者的差值：

Δa_i = a_i_real - a_i_nominal
Δα_i = α_i_real - α_i_nominal
Δd_i = d_i_real - d_i_nominal
Δθ_i = θ_i_real - θ_i_nominal

把这些误差代入正向运动学方程，做一阶泰勒展开，就得到了误差模型：

ΔP = J * Δq

其中：

ΔP：末端位姿误差向量（6×1，包含3个位置误差和3个姿态误差）
J：辨识雅可比矩阵（6×4n，n为关节数）
Δq：待辨识的运动学参数误差向量（4n×1）

关键点：这个模型是线性的！虽然原始运动学是非线性的，但我们在名义值附近做了一阶近似，把问题转化成了线性最小二乘问题。这是整个标定方法的数学基础。

4.2 最小二乘法原理

有了误差模型，下一步就是求解 Δq。你想想看，我们测量了 m 组末端位姿数据，每组都能列一个方程：

ΔP_i = J_i * Δq

把所有方程堆起来：

⎡ ΔP_1 ⎤   ⎡ J_1 ⎤
⎢ ΔP_2 ⎥   ⎢ J_2 ⎥
⎢  ... ⎥ = ⎢ ... ⎥ * Δq
⎣ ΔP_m ⎦   ⎣ J_m ⎦

即：Y = A * Δq

现在问题变成了：已知 Y 和 A，求 Δq 使得残差平方和最小。

标准最小二乘解是：

Δq = (A^T * A)^{-1} * A^T * Y

这个公式看起来简单，但我在项目中踩过坑。有一次标定结果发散，检查了半天，发现是 A^T*A 矩阵条件数太大，接近奇异。为什么会这样？因为某些测量位姿太接近，导致雅可比矩阵的列向量线性相关。

避坑指南：我曾经在产线上连续采集了50组数据，结果辨识出来的参数误差比不标定还大。后来发现，测量位姿全部集中在工作空间的一个小区域里。记住：测量位姿必须充分激励所有关节，覆盖整个工作空间。

解决病态问题的方法有两个：

增加测量数据量：m >> 4n，让方程超定
使用正则化：在 A^T*A 上加一个阻尼项

4.3 非线性优化方法（Levenberg-Marquardt）

最小二乘法虽然好用，但它假设误差模型是线性的。实际上，一阶泰勒展开的近似只在名义值附近有效。如果初始误差很大，或者机器人构型远离名义值，线性模型就不够用了。

这时候就需要 Levenberg-Marquardt（LM）算法。它结合了梯度下降法和高斯-牛顿法的优点，是机器人标定领域最常用的非线性优化方法。

LM算法的核心思想是：

Δq = -(J^T * J + λ * I)^{-1} * J^T * e

其中：

J：误差对参数的雅可比矩阵
e：当前参数下的末端位姿误差向量
λ：阻尼因子，控制算法在梯度下降和高斯-牛顿之间切换
I：单位矩阵

当 λ 很大时，算法退化为梯度下降法，收敛慢但稳定；当 λ 很小时，算法接近高斯-牛顿法，收敛快但可能发散。LM算法会在迭代过程中动态调整 λ。

实战技巧：我习惯的初始 λ 设为 0.01，然后每次迭代后检查残差是否下降。如果下降，λ 减小10倍；如果上升，λ 增大10倍。这个策略在大多数情况下都能稳定收敛。

下面是一个在STM32上实现的LM算法核心代码片段：

// LM算法单步迭代
float lm_step(float *params, float *measurements, int n_params, int n_meas) {
    // 1. 计算当前残差和雅可比矩阵
    float J[n_meas][n_params];
    float e[n_meas];
    compute_residual_and_jacobian(params, measurements, e, J);
    
    // 2. 计算 J^T * J 和 J^T * e
    float JTJ[n_params][n_params];
    float JTe[n_params];
    compute_JTJ_and_JTe(J, e, JTJ, JTe, n_meas, n_params);
    
    // 3. 添加阻尼项
    for (int i = 0; i < n_params; i++) {
        JTJ[i][i] += lambda;
    }
    
    // 4. 求解线性方程组 (JTJ + λI) * Δq = JTe
    float delta[n_params];
    solve_linear_system(JTJ, JTe, delta, n_params);
    
    // 5. 更新参数
    float new_params[n_params];
    for (int i = 0; i < n_params; i++) {
        new_params[i] = params[i] + delta[i];
    }
    
    // 6. 计算新残差，决定是否接受更新
    float new_error = compute_residual(new_params, measurements);
    if (new_error < current_error) {
        lambda *= 0.1;  // 成功，减小阻尼
        memcpy(params, new_params, sizeof(float) * n_params);
        return new_error;
    } else {
        lambda *= 10.0; // 失败，增大阻尼
        return current_error;
    }
}

注意：在STM32上跑LM算法，矩阵求逆是最大的性能瓶颈。我建议使用Cholesky分解或QR分解来求解线性方程组，而不是直接求逆。另外，float精度在迭代过程中可能不够，必要时可以切换到double。

4.4 本章知识体系

为了让你更直观地理解这三部分的关系，我画了一张流程图：

从这张图可以看出，整个辨识流程是一个闭环：先建立误差模型，然后用最小二乘法得到初始解，再用LM算法迭代优化，最后把标定后的参数写回控制器。如果精度不达标，就重新采集数据再来一轮。

我个人建议，在工程实践中不要一上来就上LM算法。先用最小二乘法算一遍，看看残差的大小和分布。如果残差已经很小，就没必要折腾非线性优化了。毕竟在STM32上跑矩阵运算，算力还是很宝贵的。