摘要:最小二乘不仅是一个求解公式,更是一套将观测、噪声、先验和约束组织成优化问题的方法。本文从普通线性最小二乘出发,讨论超定、欠定与秩亏,加权与广义最小二乘,非线性最小二乘中的高斯—牛顿与 LM,鲁棒估计、正则化、等式约束、整体最小二乘与递归最小二乘,最后以 SLAM 点到平面位姿优化串联残差建模、雅可比推导和可观测性分析。各部分提供 C++17/Eigen 实现,文末附完整工程源码及确定性数值测试结果。

关键词:最小二乘;Eigen;高斯—牛顿;Levenberg–Marquardt;Huber;白化;点云配准;SLAM。

1. 先建立分类框架:问题类型不等于求解算法

学习最小二乘时,很容易把“线性最小二乘、加权最小二乘、SVD、高斯—牛顿、LM”并列记忆。但这些概念回答的不是同一个问题。

建模层面关心残差如何定义、各观测是否同等可信、有没有异常值和先验;解的性质关心方程是否独立、参数能否唯一确定;求解层面才讨论 QR、SVD、高斯—牛顿和 LM。

分类维度常见情况需要回答的问题
残差对参数的关系线性、非线性能否直接组织成固定的矩阵方程?
噪声模型普通、加权、广义观测精度是否不同?误差是否相关?
异常值处理平方损失、鲁棒损失大残差是否应该持续受到平方惩罚?
额外信息无先验、正则化、带约束参数是否有先验,或必须满足条件?
输入数据误差普通、整体最小二乘只有观测值有误差,还是输入坐标也有误差?
数据到达方式批量、递归数据一次给全,还是逐条到来?
解的性质超定、欠定、满秩、秩亏独立约束是否足够,解是否唯一?
数值求解QR、SVD、Cholesky、GN、LM如何求解线性子问题,如何组织非线性迭代?

因此,一个 SLAM 问题完全可以同时是“加权、鲁棒、带先验、定义在流形上的非线性最小二乘”。Ceres 的接口也将残差块、损失函数、参数块和流形分别建模,而不是把它们视作互斥选项。

2. 统一符号与基本目标

考虑观测模型:

y_i=f(x_i;\boldsymbol\theta)+\epsilon_i.

其中,x_i 是已知输入,y_i是观测值,\boldsymbol\theta\in\mathbb R^n 是待估计参数。全文统一使用“预测减观测”的残差约定:

r_i(\boldsymbol\theta)=f(x_i;\boldsymbol\theta)-y_i

将 m 个标量残差堆叠为向量 \mathbf r\in\mathbb R^m,普通最小二乘为:

\boxed{\boldsymbol\theta^*=\arg\min_{\boldsymbol\theta}F(\boldsymbol\theta),\qquad F(\boldsymbol\theta)=\frac12\|\mathbf r(\boldsymbol\theta)\|_2^2.}

系数 1/2 只是让求导更简洁,不改变最优解。主要符号如下。

符号含义
A\in\mathbb R^{m\times n}线性模型的设计矩阵
\mathbf b\in\mathbb R^m线性模型中的观测向量
J\in\mathbb R^{m\times n}残差对参数或局部增量的雅可比
\Sigma观测噪声协方差
W=\Sigma^{-1}正定协方差对应的信息矩阵
\sigma_i第 i 个标量观测的标准差,不是方差
\Delta\boldsymbol\theta当前迭代的参数增量

若误差独立、零均值、服从固定方差的高斯分布,负对数似然中与参数相关的部分为:

-\log p(\mathbf y\mid\boldsymbol\theta)=\frac12\sum_i\frac{r_i(\boldsymbol\theta)^2}{\sigma_i^2}+\text{const}.

因此,固定高斯噪声模型下的最大似然估计可以写成最小二乘;方差不同时得到加权形式。这里“固定”很重要:若协方差也随待估计参数变化,通常不能忽略似然中的对数行列式等项。[5]

2.1 代码组织约定

正文中的函数共用以下头文件、类型别名和检查函数;调用示例写在 main() 中。非线性迭代和位姿优化部分先展示核心步骤,完整的迭代控制、输入检查和数值测试在文末源码附录中。

#include <Eigen/Dense>
 #include <Eigen/Geometry>
 #include <algorithm>
 #include <cmath>
 #include <iomanip>
 #include <iostream>
 #include <stdexcept>
 #include <string>
 #include <vector>
 ​
 using Eigen::MatrixXd;
 using Eigen::VectorXd;
 using Eigen::Matrix3d;
 using Eigen::Vector3d;
 using Eigen::Vector2d;
 using Vector6d = Eigen::Matrix<double, 6, 1>;
 ​
 void require(bool ok, const std::string& message) {
     if (!ok) throw std::runtime_error(message);
 }
 void checkAB(const MatrixXd& A, const VectorXd& b) {
     require(A.rows() > 0 && A.cols() > 0 && A.rows() == b.size(),
             "Invalid A/b dimensions");
     require(A.allFinite() && b.allFinite(), "A/b contain NaN or Inf");
 }

代码中的 x 或 x_ 表示待估计参数向量,不要与数学表达式中的已知采样输入 x_i 混淆。

3. 普通线性最小二乘:从直线拟合开始

3.1 “线性”指的是对未知参数线性

对于直线模型:

y=ax+b

未知参数 a,b 以线性方式出现,因此属于线性最小二乘。

对于二次多项式:

y=ax^2+bx+c,

虽然图像是曲线,但 x_i 和 x_i^2 都是已知数,模型对 a,b,c 仍然是线性的。

判断标准是未知参数如何进入模型,而不是拟合曲线的外形。

3.2 矩阵形式与正规方程

以直线拟合为例:

A=\begin{bmatrix}x_1&1\\x_2&1\\\vdots&\vdots\\x_m&1\end{bmatrix},\quad \boldsymbol\theta=\begin{bmatrix}a\\b\end{bmatrix},\quad \mathbf b=\begin{bmatrix}y_1\\y_2\\\vdots\\y_m\end{bmatrix}.

最小化:

F(\boldsymbol\theta)=\frac12\|A\boldsymbol\theta-\mathbf b\|^2.

梯度与 Hessian 为:

\nabla F=A^\top(A\boldsymbol\theta-\mathbf b),\qquad \nabla^2F=A^\top A.

令梯度为零得到正规方程:

\boxed{A^\top A\boldsymbol\theta=A^\top\mathbf b.}

若 A 列满秩,则 A^\top A 正定,目标严格凸,解唯一;若列秩亏,最小二乘目标仍是凸的,但参数一般不能唯一确定。

3.3 C++:使用列主元 QR

 VectorXd solveLS(const MatrixXd& A, const VectorXd& b) {
     checkAB(A, b);
     Eigen::ColPivHouseholderQR<MatrixXd> qr(A);
     require(qr.rank() == A.cols(), "Rank-deficient A; use COD/SVD or add a justified prior");
     VectorXd x = qr.solve(b);
     require(x.allFinite(), "Non-finite LS solution");
     return x;
 }

调用示例:

 MatrixXd A(5, 2);
 VectorXd b(5);
 A << 0, 1,
      1, 1,
      2, 1,
      3, 1,
      4, 1;
 b << 1.1, 2.9, 5.2, 6.8, 9.1;
 ​
 VectorXd theta = solveLS(A, b);
 std::cout << theta.transpose() << '\n';
 // a = 1.99, b = 1.04

3.4 为什么不直接计算逆矩阵?

满列秩时,可以在纸上写:

\boldsymbol\theta=(A^\top A)^{-1}A^\top\mathbf b.

但这不意味着代码应该先构造逆矩阵。工程中应当使用分解来求解线性系统。对于二范数条件数,满列秩矩阵满足:

\kappa_2(A^\top A)=\kappa_2(A)^2.

因此,形成正规方程可能明显放大数值敏感性。Eigen 官方文档提供 QR、SVD 和正规方程三种路线,并明确提醒正规方程的条件数问题。

 // QR:一般的小型稠密最小二乘。
 VectorXd x_qr = A.colPivHouseholderQr().solve(b);
 ​
 // SVD:分析奇异值、秩和最小范数解。
 Eigen::JacobiSVD<MatrixXd> svd(
     A, Eigen::ComputeThinU | Eigen::ComputeThinV);
 VectorXd x_svd = svd.solve(b);
 ​
 // 正规方程:应先确认秩、条件数和问题规模适合。
 VectorXd x_normal =
     (A.transpose() * A).ldlt().solve(A.transpose() * b);

这不是“QR 永远比 Cholesky 好”的结论,而是速度、稳定性、稀疏性和规模之间的选择。

4. 超定、欠定与秩亏:方程很多,为什么仍然解不出来?

设 A\in\mathbb R^{m\times n},m 为方程数,n 为参数数。

情况条件对无约束线性最小二乘的影响
超定m>n可能不一致,也可能精确一致;是否唯一还要看列秩
方阵系统m=n满秩时唯一,秩亏时不能唯一确定全部参数
欠定m<n不可能列满秩,因此最小二乘参数不唯一
列秩亏\operatorname{rank}(A)<n存在非零参数方向不改变预测值

4.1 一个欠定例子

只有一个方程:

\theta_1+\theta_2=2.

(0,2)、(1,1) 和 (2,0) 都是精确解。

若规定在所有最小二乘解中选择欧氏范数最小者:

\boldsymbol\theta^\dagger=\arg\min_{\boldsymbol\theta\in\arg\min\|A\boldsymbol\theta-\mathbf b\|^2}\|\boldsymbol\theta\|_2,

得到:

\boxed{\boldsymbol\theta^\dagger=[1,1]^\top.}

Eigen 的完整正交分解 COD 支持直接求最小范数最小二乘解。

VectorXd solveMinimumNormLS(const MatrixXd& A, const VectorXd& b) {
     checkAB(A, b);
     Eigen::CompleteOrthogonalDecomposition<MatrixXd> cod(A);
     VectorXd x = cod.solve(b);
     require(x.allFinite(), "Non-finite minimum-norm solution");
     return x;
 }
 ​
 // main() 中:
 MatrixXd A(1, 2);
 VectorXd b(1);
 A << 1, 1;
 b << 2;
 std::cout << solveMinimumNormLS(A, b).transpose() << '\n';
 // 1 1

4.2 超定也可能秩亏

下面有三个方程、两个参数:

\begin{cases} \theta_1+\theta_2=2,\\ 2\theta_1+2\theta_2=4,\\ 3\theta_1+3\theta_2=6. \end{cases}

但三行只提供一个独立约束,矩阵秩仍为 1。

若 A\mathbf v=0,则对任意 \alpha:

A(\boldsymbol\theta+\alpha\mathbf v)=A\boldsymbol\theta

因此,数据无法区分沿 \mathbf v 方向变化的参数。SVD 或 COD 能选择一个解,却不能凭空补上观测信息。

在实际计算中,还要区分严格秩亏与近似秩亏:非常小但非零的奇异值意味着参数对噪声高度敏感。矩阵的数值秩阈值也不等于应用中的可观测性阈值;单位、尺度和噪声模型都必须考虑。

5. 加权与广义最小二乘:先把误差放到可比较的尺度上

5.1 加权最小二乘

若第 i条标量观测的标准差为 \sigma_i>0:

F(\boldsymbol\theta)=\frac12\sum_i\frac{r_i^2}{\sigma_i^2} =\frac12\mathbf r^\top W\mathbf r, \qquad W=\operatorname{diag}\left(\frac1{\sigma_1^2},\ldots,\frac1{\sigma_m^2}\right).

对线性残差:

A^\top WA\boldsymbol\theta=A^\top W\mathbf b.

更方便的实现是白化残差:

\tilde r_i=\frac{r_i}{\sigma_i}.

 VectorXd solveWeightedLS(const MatrixXd& A, const VectorXd& b,
                         const VectorXd& sigma) {
     checkAB(A, b);
     require(sigma.size() == b.size() && sigma.allFinite() &&
             (sigma.array() > 0.0).all(), "sigma must be finite and positive");
     MatrixXd Aw = A;
     VectorXd bw = b;
     for (Eigen::Index i = 0; i < A.rows(); ++i) {
         Aw.row(i) /= sigma(i);
         bw(i) /= sigma(i);
     }
     return solveLS(Aw, bw);
 }

损失中的权重是 1/\sigma_i^2,但残差与雅可比应乘 1/\sigma_i。 不能给残差再乘一次完整权重,否则平方后会使用错误的 1/\sigma_i^4。

在直线数据中,把最后一项改成 20,并事先声明它的标准差为 10,其余为 1:

 VectorXd noisy_b = b;
 noisy_b(4) = 20.0;
 VectorXd sigma = VectorXd::Ones(5);
 sigma(4) = 10.0;
 ​
 VectorXd ordinary = solveLS(A, noisy_b);
 VectorXd weighted = solveWeightedLS(A, noisy_b, sigma);

结果:

 普通最小二乘:a = 4.17000000,b = -1.14000000
 加权最小二乘:a = 1.99492611,b =  1.03507389

这里的改善来自正确的先验精度信息,不是加权最小二乘自动发现了异常点。

5.2 广义最小二乘:处理相关误差

当不同观测的误差相关,使用完整协方差:

F=\frac12(A\boldsymbol\theta-\mathbf b)^\top\Sigma^{-1}(A\boldsymbol\theta-\mathbf b).

若 \Sigma 对称正定,并有:

\Sigma=LL^\top,

则:

\mathbf r^\top\Sigma^{-1}\mathbf r =\|L^{-1}\mathbf r\|^2.

因此通过三角求解进行白化:

VectorXd solveGeneralizedLS(const MatrixXd& A, const VectorXd& b,
                           const MatrixXd& Sigma) {
    checkAB(A, b);
    require(Sigma.rows() == b.size() && Sigma.cols() == b.size() &&
            Sigma.allFinite() && Sigma.isApprox(Sigma.transpose(), 1e-12),
            "Invalid covariance dimensions/symmetry");
    Eigen::LLT<MatrixXd> llt(Sigma);
    require(llt.info() == Eigen::Success, "Covariance must be positive definite");
    MatrixXd Aw = llt.matrixL().solve(A);
    VectorXd bw = llt.matrixL().solve(b);
    return solveLS(Aw, bw);
}

这里必须使用 L^{-1}\mathbf r,不是随意改成 L^{-\top}\mathbf r。等价地,若直接从信息矩阵构造白化因子 S,应满足 S^\top S=W。

Ceres 不会替用户推断各观测的噪声单位;残差的协方差不是单位阵时,需要在建模时正确缩放。[5]

6. 非线性最小二乘:反复求解局部线性问题

6.1 指数模型与雅可比

考虑:

y_i=\exp(ax_i+b),\qquad r_i=\exp(ax_i+b)-y_i.

参数出现在指数函数内部,问题是非线性的。雅可比为:

J_i=\begin{bmatrix}x_i\exp(ax_i+b)&\exp(ax_i+b)\end{bmatrix}.

void evaluateExp(const VectorXd& xs, const VectorXd& ys, const Vector2d& theta,
                 VectorXd& r, MatrixXd& J) {
    r.resize(xs.size()); J.resize(xs.size(), 2);
    for (Eigen::Index i = 0; i < xs.size(); ++i) {
        const double prediction = std::exp(theta(0) * xs(i) + theta(1));
        r(i) = prediction - ys(i);
        J(i, 0) = xs(i) * prediction;
        J(i, 1) = prediction;
    }
}

一个容易忽略的问题是:对正观测取对数后,虽然 \log y_i\approx ax_i+b 可线性求解,但优化的是对数域残差,不再是原来的 \sum_i(\exp(ax_i+b)-y_i)^2。变量变换可能有助于初始化,却不能在不改变噪声假设和目标函数的情况下宣称两者等价。

6.2 高斯—牛顿推导

在当前估计处做一阶展开:

\mathbf r(\boldsymbol\theta+\Delta\boldsymbol\theta) \approx\mathbf r(\boldsymbol\theta)+J\Delta\boldsymbol\theta.

于是求解局部问题:

\min_{\Delta\boldsymbol\theta}\frac12\|\mathbf r+J\Delta\boldsymbol\theta\|^2,

得到:

\boxed{J^\top J\Delta\boldsymbol\theta=-J^\top\mathbf r.}

更新参数:

\boldsymbol\theta\leftarrow\boldsymbol\theta+\alpha\Delta\boldsymbol\theta.

\alpha=1 是完整步长;为避免线性化不准确导致目标上升,可以通过回溯线搜索缩小 \alpha。完整工程使用 Armijo 条件,而不是不检查就接受完整步长。

// 当前 r、J 已由 evaluateExp() 计算。
Vector2d step = solveLS(J, VectorXd(-r));
// 之后进行回溯线搜索,再更新 theta。

6.3 高斯—牛顿与牛顿法的区别

对于 F=\frac12\sum_i r_i^2:

\nabla F=J^\top\mathbf r,
\boxed{\nabla^2F=J^\top J+\sum_i r_i\nabla^2r_i.}

牛顿法使用完整 Hessian;高斯—牛顿忽略第二项,以 J^\top J 近似 Hessian。当残差较小、且残差的二阶导没有异常增大时,或残差局部接近线性时,这一近似更合理。

6.4 LM:用阻尼控制局部更新

令:

H=J^\top J,\qquad\mathbf g=J^\top\mathbf r.

LM 的一种常见形式为:

\boxed{(H+\mu D)\Delta\boldsymbol\theta=-\mathbf g.}

其中 D 是正对角矩阵。本文代码采用 D_{ii}=\max(H_{ii},10^{-12}),这是示例中的数值保护,不是所有应用通用的物理阈值。代码局部变量 lambda 对应这里的阻尼 \mu。

Eigen::Matrix2d H = J.transpose() * J;
Vector2d g = J.transpose() * r;
Vector2d diagonal = H.diagonal().cwiseMax(1e-12);

Eigen::Matrix2d Hd = H;
Hd.diagonal() += lambda * diagonal;
Vector2d step = Hd.ldlt().solve(-g);

当阻尼较小时,它接近高斯—牛顿;当阻尼较大时:

\Delta\boldsymbol\theta\approx-\frac1\mu D^{-1}\mathbf g,

即尺度调整后的负梯度方向。

完整的 LM 不只是往 Hessian 上加一个常数。它还需要评估候选更新是否可信。以原始局部二次模型定义预测下降:

\Delta F_{\mathrm{pred}}=-\mathbf g^\top\Delta\boldsymbol\theta-\frac12\Delta\boldsymbol\theta^\top H\Delta\boldsymbol\theta,

以及下降比:

\eta=\frac{F(\boldsymbol\theta)-F(\boldsymbol\theta+\Delta\boldsymbol\theta)}{\Delta F_{\mathrm{pred}}}.

只有在预测下降有效时才计算此比值。若实际下降与预测一致,可以接受更新并减小阻尼;若实际下降很差甚至目标上升,应拒绝更新并增加阻尼。这是 LM 信赖域思想的核心。[4]

7. 鲁棒最小二乘:不知道哪些观测有问题怎么办?

加权最小二乘利用已知的噪声模型;鲁棒估计则尝试抑制与当前模型明显不一致的观测。两者可以组合使用。

7.1 Huber 损失

先标准化:

u_i=\frac{r_i}{\sigma_i}.

Huber 损失为:

h_\delta(u)=\begin{cases}\frac12u^2,&|u|\le\delta,\\\delta\left(|u|-\frac12\delta\right),&|u|>\delta.\end{cases}

小残差保持平方惩罚,大残差转为线性增长。

7.2 IRLS 推导与实现

令 \psi(u)=h_\delta'(u),冻结当前迭代的权重:

w_i=\frac{\psi(u_i)}{u_i}=\begin{cases}1,&|u_i|\le\delta,\\\delta/|u_i|,&|u_i|>\delta.\end{cases}

u_i=0 时按连续延拓取 w_i=1。每轮求解:

\min_{\boldsymbol\theta}\frac12\sum_iw_i\left(\frac{A_i\boldsymbol\theta-b_i}{\sigma_i}\right)^2.

所以每一行实际乘上的因子是 \sqrt{w_i}/\sigma_i:

 VectorXd solveHuberIRLS(const MatrixXd& A, const VectorXd& b,
                         const VectorXd& sigma, double delta = 1.345) {
     require(std::isfinite(delta) && delta > 0.0, "Invalid Huber delta");
     VectorXd x = solveWeightedLS(A, b, sigma);
     for (int iter = 0; iter < 200; ++iter) {
         VectorXd u = (A * x - b).cwiseQuotient(sigma);
         MatrixXd Aw = A;
         VectorXd bw = b;
         for (Eigen::Index i = 0; i < A.rows(); ++i) {
             const double a = std::abs(u(i));
             const double w = (a <= delta) ? 1.0 : delta / a;
             const double scale = std::sqrt(w) / sigma(i);
             Aw.row(i) *= scale;
             bw(i) *= scale;
         }
         VectorXd next = solveLS(Aw, bw);
         if ((next - x).norm() <= 1e-10 * (1.0 + x.norm())) return next;
         x = next;
     }
     throw std::runtime_error("IRLS did not converge within 200 iterations");
 }

生成 x=-5,-4,\ldots,5,原本令 y=2x+1,再给 x=3 对应的观测增加 20。取 \sigma_i=1、Huber 阈值\delta=1:

普通最小二乘:a = 2.54545455,b = 2.81818182
Huber IRLS: a = 2.03296703,b = 1.10989011

IRLS 的内层求解把本轮权重视作常量,不是在内层继续对权重求导。

7.3 与 Ceres 损失函数接口的对应

Ceres 使用 \frac12\rho(\|\mathbf r\|^2) 的约定。对标量标准化残差:

\rho_\delta(s)=\begin{cases}s,&s\le\delta^2,\\2\delta\sqrt{s}-\delta^2,&s>\delta^2,\end{cases}

恰好有 \frac12\rho_\delta(u^2)=h_\delta(u)。向量残差块通常按整个块的平方范数施加损失,不一定等于逐分量独立使用 Huber。[6]

鲁棒核不能补回缺失的可观测性,也不能保证所有错误匹配都被识别。 对错误对应很多或初值很差的问题,仍需几何验证、对应筛选等机制。

8. 正则化与先验:最小范数解不等于加一个惩罚项

8.1 从先验信息到正则项

考虑:

F(\boldsymbol\theta)=\frac12\|A\boldsymbol\theta-\mathbf b\|^2+\frac\lambda2\|\boldsymbol\theta-\boldsymbol\theta_0\|^2,\qquad\lambda>0.

正规方程:

\boxed{(A^\top A+\lambda I)\boldsymbol\theta=A^\top\mathbf b+\lambda\boldsymbol\theta_0.}

用增广矩阵可避免额外显式构造正规方程:

\min_{\boldsymbol\theta}\left\|\begin{bmatrix}A\\\sqrt\lambda I\end{bmatrix}\boldsymbol\theta-\begin{bmatrix}\mathbf b\\\sqrt\lambda\boldsymbol\theta_0\end{bmatrix}\right\|^2.

VectorXd solveRidge(const MatrixXd& A, const VectorXd& b,
                    double lambda, const VectorXd& x0) {
    checkAB(A, b);
    require(std::isfinite(lambda) && lambda > 0.0 &&
            x0.size() == A.cols() && x0.allFinite(), "Invalid ridge prior");
    const Eigen::Index m = A.rows(), n = A.cols();
    MatrixXd Aa(m + n, n);
    VectorXd ba(m + n);
    Aa.topRows(m) = A;
    Aa.bottomRows(n) = std::sqrt(lambda) * MatrixXd::Identity(n, n);
    ba.head(m) = b;
    ba.tail(n) = std::sqrt(lambda) * x0;
    return solveLS(Aa, ba);
}

8.2 对同一个欠定问题比较两种答案

对于 \theta_1+\theta_2=2,最小范数最小二乘解是 [1,1]^\top。

加入零均值先验并取 \lambda=1:

\min(\theta_1+\theta_2-2)^2+\theta_1^2+\theta_2^2,

得到:

\boxed{\theta_1=\theta_2=\frac23.}

原因不是求解器不同,而是目标已经不同:最小范数是在原问题的最优解集合里选择;正则化则允许牺牲数据拟合,换取参数更接近先验。

8.3 真实先验、LM 阻尼和固定规范不能混为一谈

一般高斯先验应写作:

\frac12(\boldsymbol\theta-\boldsymbol\theta_0)^\top\Lambda_0(\boldsymbol\theta-\boldsymbol\theta_0),\qquad\Lambda_0=P_0^{-1}.

各参数单位和精度不同时,应使用合理的信息矩阵,而不是不加区分地惩罚所有分量。

LM 阻尼主要约束当前增量,随迭代调整;真实先验表达对参数本身的额外知识。SLAM 中固定参考位姿还可能只是选择全局坐标规范,不等同于解决所有场景退化。Ceres 文档将规范不确定性与其他秩亏来源专门区分。[5]

9. 约束最小二乘:参数必须满足条件

9.1 等式约束与 KKT 系统

考虑:

\min_{\boldsymbol\theta}\frac12\|A\boldsymbol\theta-\mathbf b\|^2,\qquad C\boldsymbol\theta=\mathbf d.

拉格朗日函数为:

\mathcal L=\frac12\|A\boldsymbol\theta-\mathbf b\|^2+\boldsymbol\nu^\top(C\boldsymbol\theta-\mathbf d).

对 \boldsymbol\theta,\boldsymbol\nu求导并令其为零:

\boxed{\begin{bmatrix}A^\top A&C^\top\\C&0\end{bmatrix}\begin{bmatrix}\boldsymbol\theta\\\boldsymbol\nu\end{bmatrix}=\begin{bmatrix}A^\top\mathbf b\\\mathbf d\end{bmatrix}.}

下面是用于小型问题的教学实现,要求 KKT 系统非奇异:

VectorXd solveEqualityLS(const MatrixXd& A, const VectorXd& b,
                         const MatrixXd& C, const VectorXd& d) {
    checkAB(A, b);
    require(C.rows() > 0 && C.cols() == A.cols() && C.rows() == d.size() &&
            C.allFinite() && d.allFinite(), "Invalid equality constraints");
    const Eigen::Index n = A.cols(), k = C.rows();
    MatrixXd K = MatrixXd::Zero(n + k, n + k);
    K.topLeftCorner(n, n) = A.transpose() * A;
    K.topRightCorner(n, k) = C.transpose();
    K.bottomLeftCorner(k, n) = C;
    VectorXd rhs(n + k);
    rhs.head(n) = A.transpose() * b;
    rhs.tail(k) = d;
    Eigen::FullPivLU<MatrixXd> lu(K);
    require(lu.rank() == n + k, "Singular KKT system; inspect constraints/nullspace");
    VectorXd solution = lu.solve(rhs);
    require(solution.allFinite() &&
            (K * solution - rhs).norm() <= 1e-9 * (1.0 + rhs.norm()),
            "KKT solve failed");
    return solution.head(n);
}

9.3 不等式与流形约束

线性最小二乘还可能带有 G\boldsymbol\theta\le\mathbf h 或参数上下界,通常应采用针对约束设计的求解方法;求无约束解后直接裁剪,一般不保证得到原约束问题的最优解。

旋转又是另一类约束:R^\top R=I,\det R=1,或四元数满足单位范数。使用与流形相容的局部更新,可以避免将旋转的表示维度误当作实际自由度。

10. 整体最小二乘与正交拟合:点云平面怎么估计?

10.1 普通直线拟合与几何距离的区别

普通的 y=ax+b 拟合最小化纵向残差 ax_i+b-y_i,将输入 x_i 视为已知。

而点云的各坐标都可能有误差。若坐标噪声独立、等方差且各向同性,最小化点到几何对象的正交距离更自然。这是整体最小二乘在几何拟合中的典型特例;任意异方差或各向异性噪声并不自动满足这一假设。

10.2 平面拟合推导

使用隐式平面:

\mathbf n^\top\mathbf p+d=0,\qquad\|\mathbf n\|=1.

最小化:

\min_{\mathbf n,d}\sum_i(\mathbf n^\top\mathbf p_i+d)^2.

对 d 求导得到:

d=-\mathbf n^\top\bar{\mathbf p},\qquad\bar{\mathbf p}=\frac1N\sum_i\mathbf p_i.

代回并定义:

C=\frac1N\sum_i(\mathbf p_i-\bar{\mathbf p})(\mathbf p_i-\bar{\mathbf p})^\top,

问题变成:

\min_{\|\mathbf n\|=1}\mathbf n^\top C\mathbf n.

因此:

\boxed{\mathbf n=\operatorname{eigvec}_{\min}(C),\qquad d=-\mathbf n^\top\bar{\mathbf p}.}

PCL 的局部表面法向估计教程采用这一协方差矩阵特征分析思路。[7]

11. 递归最小二乘:观测逐条到达时如何更新?

11.1 从批量目标到递归更新

设线性观测为:

y_k=\boldsymbol\phi_k^\top\boldsymbol\theta+\epsilon_k,\qquad\epsilon_k\sim\mathcal N(0,\sigma_k^2).

无遗忘因子时,递归方法对应的批量目标为:

F_k=\frac12(\boldsymbol\theta-\boldsymbol\theta_0)^\top P_0^{-1}(\boldsymbol\theta-\boldsymbol\theta_0)+\frac12\sum_{i=1}^k\frac{(\boldsymbol\phi_i^\top\boldsymbol\theta-y_i)^2}{\sigma_i^2}.

其中 P_0是参数先验协方差。

引入遗忘因子 0<\beta\le1,先设置:

P_k^-=P_{k-1}/\beta.

再计算:

\mathbf K_k=\frac{P_k^-\boldsymbol\phi_k}{\sigma_k^2+\boldsymbol\phi_k^\top P_k^-\boldsymbol\phi_k},

\boxed{\boldsymbol\theta_k=\boldsymbol\theta_{k-1}+\mathbf K_k(y_k-\boldsymbol\phi_k^\top\boldsymbol\theta_{k-1}).}

用 Joseph 形式更新协方差:

P_k=(I-\mathbf K_k\boldsymbol\phi_k^\top)P_k^-(I-\mathbf K_k\boldsymbol\phi_k^\top)^\top+\sigma_k^2\mathbf K_k\mathbf K_k^\top.

\beta=1 保留全部历史权重;\beta<1则逐渐降低历史信息的影响,适用于跟踪缓慢变化的参数。MathWorks 的官方递归最小二乘资料也区分了这一遗忘机制。[8]

11.3 为什么 RLS 不一定等于无先验批量最小二乘?

对于直线拟合,取 \boldsymbol\phi_k=[x_k,1]^\top。设置:

\boldsymbol\theta_0=0,\qquad P_0=1000I,

对应先验信息 P_0^{-1}=0.001I。因此应与 \lambda=0.001 的零均值正则化批量解比较,而不是直接要求它与无先验 OLS 完全相同。

测试得到:

RLS:a = 1.99000895,b = 1.03977414

它与使用同一先验的批量最小二乘一致。有限的“大协方差”仍然是有限先验,不等于严格无先验。

14. 常见误区与排查顺序

常见说法或做法更准确的理解
多项式拟合是非线性最小二乘要看是否对待估计参数线性
超定问题一定没有精确解超定只描述行列数量,也可能完全一致
方程数多于参数数,解就一定唯一关键是独立约束,也就是列秩
使用逆矩阵公式就是最直接的实现应优先求解线性系统,避免不必要的显式求逆
加权时给残差乘 1/\sigma^2残差应乘 1/\sigma
Huber 权重能代替传感器协方差两者来源不同,通常先白化再进行鲁棒化
SVD 或 LM 解决了不可观测性它们能选择解或稳定计算,但不会补充观测信息
最小范数与有限正则化是同一问题前者选择原最优解,后者改变目标
RLS 与 OLS 应当无条件完全一致必须比较相同先验、权重和遗忘机制
点到平面雅可比可以直接照搬必须匹配坐标系、残差符号、增量顺序和扰动约定

15. 总结

最小二乘的共同出发点,是寻找能够解释观测的参数;不同变体则处理观测质量、异常值、参数先验、几何约束和数据组织方式的差异。

普通线性最小二乘通常直接求解;非线性最小二乘通过局部线性化反复求解。加权与白化负责尺度和噪声,鲁棒损失控制异常残差,先验与约束引入额外知识,而秩与零空间决定哪些方向真正被数据约束。

对于 SLAM、点云配准和标定,最重要的不只是记住正规方程,而是把下列关系建立起来:

求解器能把一个定义正确的问题算出来,但不能替代正确的建模,也不能创造原本不存在的信息。

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐