📚 课程目录第4章最小二乘法在标定中的应用

4. 最小二乘法在标定中的应用：线性最小二乘、非线性最小二乘（Levenberg-Marquardt算法）

做运动学标定，说白了就是解方程。你给机器人发指令让它走到某个位置，它实际去了另一个位置。这中间的偏差，就是我们要用数学模型去拟合的参数。

怎么拟合？最常用的就是最小二乘法。我最早接触这玩意儿是在大学做实验，那时候觉得就是个公式，套进去算就完了。直到后来做机器人标定，才发现这东西的门道深着呢。

4.1 线性最小二乘：最朴素的解法

先说说最简单的场景。假设你的机器人关节角度和末端位置之间，是线性关系。比如一个简单的平面两连杆，DH参数里只有连杆长度是未知的。

这时候，你的测量方程可以写成：

y = A * x

其中 y 是末端位置测量值，A 是关节角度构成的矩阵，x 是待标定的参数（比如连杆长度）。

我们有 m 组测量数据，n 个未知参数。通常 m > n，方程超定，没有精确解。那就找误差平方和最小的解：

min ||A * x - y||²

解这个问题的标准公式是：

x = (Aᵀ * A)⁻¹ * Aᵀ * y

这就是线性最小二乘的闭式解。我在项目中用过很多次，比如标定一个简单的直角坐标机器人，三个轴互相垂直，误差模型就是线性的，直接套公式，几毫秒就算出来了。

我的习惯： 用线性最小二乘之前，先检查一下 AᵀA 的条件数。如果条件数太大（比如超过 1000），说明矩阵病态，算出来的参数可能很不稳定。这时候需要换激励轨迹，或者加正则化。

4.2 非线性最小二乘：现实世界的常态

但现实哪有那么美好？工业机器人的运动学模型，关节角度和末端位置之间是三角函数关系，非线性。你的误差函数长这样：

e(x) = f(x) - y

其中 f(x) 是正运动学模型，x 是待标定的 DH 参数，y 是测量值。这个 e(x) 对 x 不是线性的，没法直接求逆。

怎么办？迭代。从一个初始猜测开始，每一步找一个下降方向，让误差平方和越来越小。

最直观的方法是梯度下降。但梯度下降有个毛病——到了接近最优解的地方，收敛特别慢，像个老太太走路。我在标定一个六轴机器人时试过，跑了 5000 步还没收敛，气得我直接换算法。

另一个极端是高斯-牛顿法。它用二阶近似，收敛快，但要求雅可比矩阵满秩。如果矩阵奇异，一步就崩了。

  核心问题： 梯度下降太慢，高斯-牛顿太脆。我们需要一个既稳健又高效的算法。

4.3 Levenberg-Marquardt 算法：工程界的瑞士军刀

LM 算法就是来干这个的。它把梯度下降和高斯-牛顿揉在一起，根据当前情况自动切换。

核心思想很简单：每一步求解：

(JᵀJ + λI) * Δx = -Jᵀ * e

其中 J 是雅可比矩阵，e 是误差向量，λ 是阻尼因子。

λ 很大时：JᵀJ 可以忽略，方程变成 λ * Δx ≈ -Jᵀ * e，这就是梯度下降，步长小，稳健。
λ 很小时：方程退化成 JᵀJ * Δx = -Jᵀ * e，这就是高斯-牛顿，收敛快。

LM 算法的精髓在于 λ 的调整策略：

计算当前误差平方和 E_old
解方程得到 Δx，计算新误差平方和 E_new
如果 E_new < E_old，接受这一步，减小 λ（往高斯-牛顿靠拢）
如果 E_new ≥ E_old，拒绝这一步，增大 λ（往梯度下降靠拢），重新算

我曾经用 LM 算法标定一个六轴协作机器人，12 个 DH 参数，200 组测量数据。初始 λ 设成 0.01，迭代了 30 步就收敛了，误差从 15mm 降到了 0.3mm。效果相当满意。

避坑指南： 我曾经在标定一个重载机器人时，LM 算法怎么都不收敛。查了两天，发现是雅可比矩阵算错了——我用的 DH 模型是 Modified 版本，但雅可比推导用的是 Standard 版本。两个版本的坐标系定义不一样，差一个旋转顺序。所以，模型和雅可比必须严格对应，这是血泪教训。

4.4 知识结构总览

下面这张图把三种方法的关系和适用场景梳理清楚了：

4.5 代码实战：LM 算法核心实现

下面是我在 STM32 上跑过的 LM 算法精简版。注意，这是核心迭代部分，完整的矩阵运算库需要自己配。

// LM 算法一次迭代
// J: 雅可比矩阵 (m x n)
// e: 误差向量 (m x 1)
// x: 参数向量 (n x 1)，会被更新
// lambda: 阻尼因子，会被调整
// 返回：本次迭代后的误差平方和

float lm_step(float* J, float* e, float* x, 
              int m, int n, float* lambda) {
    float JTJ[n][n];  // JᵀJ
    float JTe[n];     // Jᵀe
    
    // 1. 计算 JᵀJ 和 Jᵀe
    mat_mul_transpose(J, J, JTJ, m, n, n);
    mat_mul_transpose_vec(J, e, JTe, m, n);
    
    // 2. 构造 (JᵀJ + λI)
    float H[n][n];
    memcpy(H, JTJ, sizeof(JTJ));
    for (int i = 0; i < n; i++) {
        H[i][i] += *lambda;
    }
    
    // 3. 解线性方程 H * dx = -JTe
    float dx[n];
    solve_linear_system(H, JTe, dx, n);
    for (int i = 0; i < n; i++) dx[i] = -dx[i];
    
    // 4. 尝试新参数
    float x_new[n];
    for (int i = 0; i < n; i++) {
        x_new[i] = x[i] + dx[i];
    }
    
    // 5. 计算新误差
    float e_new[m];
    compute_error(x_new, e_new, m);
    float E_new = 0;
    for (int i = 0; i < m; i++) {
        E_new += e_new[i] * e_new[i];
    }
    
    // 6. 判断是否接受
    float E_old = 0;
    for (int i = 0; i < m; i++) {
        E_old += e[i] * e[i];
    }
    
    if (E_new < E_old) {
        // 接受，减小 lambda
        memcpy(x, x_new, sizeof(float) * n);
        *lambda *= 0.5f;
        return E_new;
    } else {
        // 拒绝，增大 lambda
        *lambda *= 2.0f;
        return E_old;
    }
}

工程经验： 在 STM32 上跑 LM 算法，矩阵求逆是个大坑。我建议用 Cholesky 分解代替直接求逆，速度快一倍，数值稳定性也好。另外，lambda 的初始值我一般设成 0.01 到 0.1 之间，太大容易陷入局部最优，太小容易发散。

4.6 三种方法对比

方法	收敛速度	稳健性	实现复杂度	适用场景
线性最小二乘	一步到位	高（需矩阵非奇异）	低	线性模型，如直角坐标机器人
梯度下降	慢	高	低	参数少、精度要求不高的场景
LM 算法	快	高	中	工业机器人 DH 参数标定

你想想看，做标定的时候，数据采集花了大半天，结果算法不收敛或者收敛到错误值，那多憋屈。所以我的建议是：能用 LM 就别用梯度下降，能用闭式解就别用迭代。选对工具，事半功倍。