最小二乘与伪逆

机器人拿到的数据几乎从来不是“刚刚好”的。激光雷达扫过一面墙，返回两百个带噪声的点，而一条直线只有两个参数（斜率和截距）。轮子转了几千圈的里程日志，只为估计一个数：轮半径。标定陀螺仪的零偏和比例、相机内参、手眼关系，都要拿几十组测量去定几个未知数。

这时方程比未知数多得多，而且每个方程都带着噪声，没有任何一个解能同时满足所有方程。我们能做的，是找一个“最不坏”的折中。这就是最小二乘 (least squares)。

反方向的情况同样常见：一台 7 自由度机械臂只需要满足 6 个末端约束，多出来的那个自由度让关节速度有无穷多种选择。我们得有一个规则来挑，比如“关节动得最少”。再加上第三种麻烦：靠近奇异位形时，精确的逆会把微小的误差放大成巨大的关节速度（1.12 节），需要给逆加上“阻尼”。

先看最小的例子。平面上有三个点 \((-1,\,1)\)、\((0,\,0)\)、\((1,\,2)\)，想找一条直线 \(y = mt + c\) 穿过它们。每个点给一个方程：

前两行说 \(c = 0\)、\(m = -1\)，第三行却要求 \(m + c = 2\)。矛盾：这三个点不共线，任何直线都会“错过”其中某些点。在 1.6 节的语言里，\(\mathbf b\) 不在 \(A\) 的列空间里。

既然不可能全部满足，就换一个问题：让每个点的偏差，也就是残差 (residual) \(r_i = y_i - (mt_i + c)\)，合起来尽量小。怎么把几个残差合成一个数？取平方和。为什么是平方？

• 大偏差被重罚。偏差翻倍，代价翻四倍，所以一个大错比很多小错更要紧。
• 平方是光滑的。最小值可以用求导或者几何直接解出，不需要迭代。
• 它就是长度的平方。残差向量的平方和是 \(\|\mathbf r\|^2\)，最小化它就是找最近的点，而最近点由垂线给出 (1.9 节)。
• 它对应高斯噪声下的最大似然（4.6 节会看到）。

下图里 8 个点是带噪声的测量（比如激光雷达打在一面斜墙上）。两个琥珀色手柄决定你的直线 \(y = mt + c\)。每个点到直线的竖直线段是它的残差，以残差为边长画出的红色方块，面积就是残差的平方。所有方块面积之和就是 SSE (sum of squared errors)。你的任务只有一个：让红色的总面积尽量小。点也可以拖动。

玩一会儿你会发现：最优线并不是让每个方块都变小，它是让总面积最小，有的点会为大局让步。最优线一定穿过重心 \((\bar t,\bar y)\)；而穿过重心的线残差之和都是零，所以零和只是必要条件。把 8 个点写成 8 行方程，就是 \(A\mathbf x\approx\mathbf b\)，其中 \(A\) 的每一行是 \([\,t_i\;\;1\,]\)，\(\mathbf x = (m, c)^\top\)，\(\mathbf b = (y_1,\dots,y_8)^\top\)。

FIG 1 的坐标是 \((t, y)\)：每个数据点是平面上的一个点（“模型空间”）。现在换一个空间看同一个问题：把所有测量值排成一个向量 \(\mathbf b\)，每个测量占一根坐标轴。这样整组数据就是一个点。为了能画出来，只取三个点，固定在 \(t = -1, 0, 1\) 上，于是 \(\mathbf b = (y_1, y_2, y_3)\) 是三维空间里的一个点。

\(A\) 的两列是 \(\mathbf a_1 = (-1, 0, 1)\)（\(t\) 的取值）和 \(\mathbf a_2 = (1, 1, 1)\)（常数 1）。\(A\mathbf x = m\,\mathbf a_1 + c\,\mathbf a_2\) 是它们的线性组合，所以“能被某条直线精确解释的数据”正好是这两列张成的一张平面（列空间）。真实数据 \(\mathbf b\) 一般不在平面上，最近的点 \(A\hat{\mathbf x}\) 是它的垂足。

拖动右图里的三个点（或者说，在左图里移动 \(\mathbf b\)），再用滑块试别的 \((m, c)\)。

垂直这件事可以写成方程。残差 \(\mathbf r = \mathbf b - A\hat{\mathbf x}\) 垂直于平面，就是同时垂直于两列：\(\mathbf a_1^\top\mathbf r = 0\)，\(\mathbf a_2^\top\mathbf r = 0\)。把两个条件叠起来就是 \(A^\top(\mathbf b - A\hat{\mathbf x}) = \mathbf 0\)，即正规方程 (normal equations)

现在反过来：只有一个方程，却有两个未知数，\(a_1x_1 + a_2x_2 = \beta\)，也就是 \(A = [\,a_1\;\;a_2\,]\)，\(b = \beta\)。在 \(x_1x_2\) 平面上，所有解排成一条直线。按 1.6 节，这条线是一个特解加上零空间：\(\mathbf x = \mathbf x_p + \mathbf n\)，\(\mathbf n\in\operatorname{null}(A)\)。

该选哪一个？一个自然的规则是选离原点最近的，也就是范数最小的。如果 \(x_1, x_2\) 是两个关节的速度，这就是“让关节总共动得最少”。几何上，离原点最近的点是原点向解线作垂线的垂足，所以它垂直于零空间方向。零空间正是与每一行都垂直的向量（\(A\mathbf n = \mathbf 0\) 就是说每一行与 \(\mathbf n\) 的点积为 0），所以垂直于零空间的方向就是各行张成的方向，叫作 \(A\) 的行空间（这里就是行向量 \((a_1, a_2)\) 的方向）。最短解一定落在行空间里。这个点就是 \(A^{+}\mathbf b\)。

拖动青色的行向量 \((a_1, a_2)\) 的箭头尖端，用滑块改 \(\beta\)，再把琥珀色的点沿解线滑动，看看有没有比紫色点更短的解。

最小二乘（方程太多）和最小范数（方程太少）看上去是两个问题，SVD 能一起解决。把 \(A = U\Sigma V^\top\) 代进 \(A\mathbf x = \mathbf b\)，并换坐标：\(\mathbf y = V^\top\mathbf x\)，\(\mathbf c = U^\top\mathbf b\)。因为 \(U\)、\(V\) 是正交矩阵（不改变长度），问题就拆成了互不相干的标量方程：

\(\sigma_i\,y_i = c_i,\qquad i = 1,\dots,r\)，\(\qquad 0\cdot y_i = c_i,\qquad i > r\)。

• \(\sigma_i > 0\) 的方向：唯一合理的选择是 \(y_i = c_i/\sigma_i\)，这就是“把每个方向除以 \(\sigma_i\)”。
• \(\sigma_i = 0\)（\(i > r\)）的方向，方程 \(0\cdot y_i = c_i\) 有两件事：\(c_i\) 无法被满足，这是不可避免的残差，也是 \(\mathbf b\) 垂直于列空间的那一部分（最小二乘：不管它）。同时 \(y_i\) 取什么都不影响残差，这是零空间方向，取 0 让范数最短（最小范数）。

合起来就是 \(\mathbf x^{+} = V\Sigma^{+}U^\top\mathbf b\)：非零奇异值取倒数，零奇异值忽略。同一个公式，既处理方程太多，也处理方程太少，还包括两者混合的秩亏情形。

如果 \(A\) 的列线性无关（列满秩，零空间只有 \(\mathbf 0\)），\(A^\top A\) 可逆，解唯一：

如果 \(A\) 的行线性无关（行满秩，\(m < n\)），\(A\mathbf x = \mathbf b\) 总有解，且有无穷多个。其中范数最小的是

\(\mathbf x^{+} = A^{+}\mathbf b\) 是所有使 \(\|A\mathbf x - \mathbf b\|\) 最小的 \(\mathbf x\) 里范数最小的那个（推导见第 5 节的拆分）。几个常用事实：

• \(AA^{+}\) 是到列空间的投影，\(A^{+}A\) 是到行空间的投影。列满秩时 \(AA^{+} = P\)。
• 四个 Penrose 条件：\(AA^{+}A = A\)，\(A^{+}AA^{+} = A^{+}\)，\((AA^{+})^\top = AA^{+}\)，\((A^{+}A)^\top = A^{+}A\)。满足它们的 \(A^{+}\) 是唯一的。
• 列满秩时，\((A^\top A)^{-1}A^\top = V\Sigma^{-2}V^\top\cdot V\Sigma U^\top = V\Sigma^{-1}U^\top\)，与 SVD 公式一致。行满秩时同理。

小奇异值是麻烦的来源：\(1/\sigma_i\) 把噪声放大。阻尼的想法是同时惩罚解的大小，让“准确”和“别太大”折中：

第一个等号的推导很简洁：这等价于一个普通的最小二乘问题，只是在方程后面多叠了 \(n\) 个方程 \(\lambda\mathbf x\approx\mathbf 0\)：\(\begin{bmatrix}A\\ \lambda I\end{bmatrix}\mathbf x\approx\begin{bmatrix}\mathbf b\\ \mathbf 0\end{bmatrix}\)。对它写正规方程，立刻得到 \((A^\top A + \lambda^2 I)\mathbf x = A^\top\mathbf b\)。\(A^\top A + \lambda^2 I\) 的特征值都不小于 \(\lambda^2\)，所以永远可逆。统计里这叫岭回归 (ridge)，反问题里叫 Tikhonov 正则化。

在 SVD 坐标里，每个方向的增益从 \(1/\sigma\) 换成了 \(\sigma/(\sigma^2+\lambda^2)\)（悬停公式里的这一项）：

• \(\sigma\gg\lambda\)：增益 \(\approx 1/\sigma\)，几乎不受影响。
• \(\sigma\ll\lambda\)：增益 \(\approx\sigma/\lambda^2\to 0\)，微弱的方向被安静地放弃，不再放大噪声。
• 增益最大值是 \(1/(2\lambda)\)（在 \(\sigma = \lambda\) 处），所以 \(\lambda\) 直接决定了最坏情况下的放大倍数。

代价是不再精确：沿弱方向的目标只完成了一部分，残差变大。\(\lambda\) 越大，解越小、残差越大；\(\lambda\to 0\) 回到伪逆。把 \(A = J\)、\(\mathbf x = \dot{\mathbf q}\)、\(\mathbf b = \dot{\mathbf x}\) 代入，就是逆运动学里的 DLS。还有一个等价的写法 \(\dot{\mathbf q} = J^\top(JJ^\top + \lambda^2 I)^{-1}\dot{\mathbf x}\)，当任务维数 \(m\) 比关节数 \(n\) 小时，要求逆的矩阵更小。

前面默认每个测量一样可靠。如果第 \(i\) 个测量的噪声标准差是 \(s_i\)，就该让可靠的测量说话更响：最小化 \((A\mathbf x - \mathbf b)^\top W(A\mathbf x - \mathbf b)\)，\(W = \operatorname{diag}(1/s_i^2)\)，解为

让平面两连杆机械臂（和 1.12 节的同一台，\(\ell_1 = 1.2\)，\(\ell_2 \approx 0.85\)）的末端去追一个目标点。每一帧：

1. 想要的末端速度 \(\dot{\mathbf x} = K(\mathbf x_{\text{target}} - \mathbf x)\)（朝目标走，限制最大速度）。
2. 解 \(J(\mathbf q)\,\dot{\mathbf q} = \dot{\mathbf x}\)，得到关节速度 \(\dot{\mathbf q}\)。解法有三种：逆 \(J^{-1}\)；伪逆，把 \(\sigma_i<0.08\) 的方向直接丢弃；阻尼 DLS，增益 \(\sigma/(\sigma^2+\lambda^2)\)。
3. 真实电机有速度上限，所以每个关节速度被截到 \(\pm 6\) rad/s，然后更新 \(\mathbf q \leftarrow \mathbf q + \dot{\mathbf q}\,\Delta t\)。

拖动紫色目标点。用快捷按钮把目标放到工作空间的边缘之外，或者放进靠近底座的“洞”里（外圈和内圈都是奇异位形，1.12 节），换着三种解法看。右侧的条形图是解法要求的关节速度（截断之前），虚线是电机上限；下方曲线是过去 4 秒里的最大需求。

注意一个细节：对可达的目标，逆在边缘附近也能工作，因为越靠近目标，想要的速度越小，\(\dot{\mathbf x}/\sigma_2\) 才没有爆炸。真正出问题的是目标在可达范围之外，或者目标本身在高速掠过奇异方向：此时需要的是“沿弱方向走一大步”，而弱方向的增益正是 \(1/\sigma_2\)。阻尼解法对两种情形都有效，代价是在奇异方向上反应变慢（滞后）。