最小二乘与伪逆Least Squares & the Pseudo-inverse

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

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

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

The data a robot gets is almost never “just right”. A lidar sweeps a wall and returns two hundred noisy points, while a line has only two parameters (slope and intercept). An odometry log with thousands of wheel turns exists to estimate a single number, the wheel radius. Calibrating a gyro's bias and scale, camera intrinsics or a hand–eye transform means fixing a few unknowns from dozens of measurements.

Now there are far more equations than unknowns, each one corrupted by noise, and no solution satisfies all of them. The best we can do is find the “least bad” compromise. That is least squares.

The opposite case is just as common: a 7-DoF arm only has to meet 6 end-effector constraints, so the extra degree of freedom leaves infinitely many joint velocities to choose from. We need a rule to pick one, for example “move the joints as little as possible”. And a third nuisance: near a singularity the exact inverse turns tiny errors into huge joint speeds (Lesson 1.12), so we must add “damping” to it.

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

Start with the smallest example. Three points \((-1,\,1)\), \((0,\,0)\), \((1,\,2)\) lie in the plane, and we want a line \(y = mt + c\) through all of them. Each point gives one equation:

前两行说 \(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 节会看到）。

The first two rows say \(c = 0\) and \(m = -1\), yet the third demands \(m + c = 2\). Contradiction: the three points are not collinear, so every line misses some of them. In the language of Lesson 1.6, \(\mathbf b\) is not in the column space of \(A\).

Since we cannot satisfy everything, ask a different question: make the deviations, the residuals \(r_i = y_i - (mt_i + c)\), small all together. How do we turn several residuals into one number? Take the sum of squares. Why squares?

• Big misses are punished hard. Double the miss and the cost quadruples, so one large error matters more than many small ones.
• Squares are smooth. The minimum can be found by calculus or geometry directly, with no iteration.
• It is a squared length. The sum of squared residuals is \(\|\mathbf r\|^2\), so minimising it means finding the nearest point, and the nearest point comes from a perpendicular (Lesson 1.9).
• It is maximum likelihood under Gaussian noise (we will see this in Lesson 4.6).

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

The 8 points below are noisy measurements (say, a lidar hitting a slanted wall). The two amber handles set your line \(y = mt + c\). The vertical segment from each point to the line is its residual, and the red square built on it has area equal to the squared residual. The total red area is the SSE (sum of squared errors). Your job: make the red area as small as you can. The points can be dragged too.

玩一会儿你会发现：最优线并不是让每个方块都变小，它是让总面积最小，有的点会为大局让步。最优线一定穿过重心 \((\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\)。

Play for a while and you will notice that the optimal line does not shrink every square; it minimises the total area, and some points give way for the common good. The optimal line always passes through the centroid \((\bar t,\bar y)\), but so does every line whose residuals sum to zero, so a zero sum is only a necessary condition. Writing the 8 points as 8 equations gives \(A\mathbf x\approx\mathbf b\), where each row of \(A\) is \([\,t_i\;\;1\,]\), \(\mathbf x = (m, c)^\top\) and \(\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)\)。

The axes of FIG 1 are \((t, y)\): each data point is a point in the plane (“model space”). Now look at the same problem in another space: stack all the measurements into one vector \(\mathbf b\), one coordinate axis per measurement. The whole data set becomes a single point. To be able to draw it, take just three points fixed at \(t = -1, 0, 1\); then \(\mathbf b = (y_1, y_2, y_3)\) is a point in 3D.

The two columns of \(A\) are \(\mathbf a_1 = (-1, 0, 1)\) (the values of \(t\)) and \(\mathbf a_2 = (1, 1, 1)\) (the constant 1). \(A\mathbf x = m\,\mathbf a_1 + c\,\mathbf a_2\) is a linear combination of them, so the data “that some line explains exactly” form precisely the plane spanned by the two columns (the column space). Real data \(\mathbf b\) is usually off the plane, and the nearest point \(A\hat{\mathbf x}\) is its foot of the perpendicular.

Drag the three points in the right panel (equivalently, move \(\mathbf b\) on the left), and use the sliders to try other \((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)

Perpendicularity can be written as an equation. The residual \(\mathbf r = \mathbf b - A\hat{\mathbf x}\) is perpendicular to the plane, which means perpendicular to both columns: \(\mathbf a_1^\top\mathbf r = 0\) and \(\mathbf a_2^\top\mathbf r = 0\). Stack the two conditions and you get \(A^\top(\mathbf b - A\hat{\mathbf x}) = \mathbf 0\), the 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\)，再把琥珀色的点沿解线滑动，看看有没有比紫色点更短的解。

Now the reverse: just one equation for two unknowns, \(a_1x_1 + a_2x_2 = \beta\), i.e. \(A = [\,a_1\;\;a_2\,]\), \(b = \beta\). In the \(x_1x_2\) plane the solutions form a line. By Lesson 1.6 that line is a particular solution plus the null space: \(\mathbf x = \mathbf x_p + \mathbf n\) with \(\mathbf n\in\operatorname{null}(A)\).

Which one should we pick? A natural rule is the one closest to the origin, the one of minimum norm. If \(x_1, x_2\) are two joint velocities, this means “move the joints as little as possible in total”. Geometrically, the closest point is the foot of the perpendicular dropped from the origin onto the solution line, so it is perpendicular to the null-space direction. The null space is exactly the set of vectors perpendicular to every row (\(A\mathbf n = \mathbf 0\) says each row has zero dot product with \(\mathbf n\)), so the directions perpendicular to it are those spanned by the rows: the row space of \(A\) (here, the direction of the row vector \((a_1, a_2)\)). The shortest solution must lie in the row space. That point is \(A^{+}\mathbf b\).

Drag the tip of the cyan row-vector arrow \((a_1, a_2)\), change \(\beta\) with the slider, and slide the amber point along the solution line to see whether any solution beats the purple one.

最小二乘（方程太多）和最小范数（方程太少）看上去是两个问题，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\)：非零奇异值取倒数，零奇异值忽略。同一个公式，既处理方程太多，也处理方程太少，还包括两者混合的秩亏情形。

Least squares (too many equations) and minimum norm (too few) look like two problems, and the SVD solves both at once. Substitute \(A = U\Sigma V^\top\) into \(A\mathbf x = \mathbf b\) and change coordinates: \(\mathbf y = V^\top\mathbf x\), \(\mathbf c = U^\top\mathbf b\). Because \(U\) and \(V\) are orthogonal (they keep lengths), the problem falls apart into independent scalar equations:

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

• Directions with \(\sigma_i > 0\): the only sensible choice is \(y_i = c_i/\sigma_i\), which is “divide each direction by \(\sigma_i\)”.
• Directions with \(\sigma_i = 0\) (\(i > r\)): the equation \(0\cdot y_i = c_i\) says two things. \(c_i\) cannot be satisfied, an unavoidable residual, the part of \(\mathbf b\) perpendicular to the column space (least squares: leave it alone). And \(y_i\) does not affect the residual at all, a null-space direction, so set it to 0 to keep the norm smallest (minimum norm).

Together that is \(\mathbf x^{+} = V\Sigma^{+}U^\top\mathbf b\): invert the nonzero singular values and ignore the zero ones. One formula covers too many equations, too few, and the rank-deficient mixture of both.

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

If the columns of \(A\) are linearly independent (full column rank, null space \(\{\mathbf 0\}\)), \(A^\top A\) is invertible and the solution is unique:

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

If the rows of \(A\) are linearly independent (full row rank, \(m < n\)), \(A\mathbf x = \mathbf b\) is always solvable, with infinitely many solutions. The one of smallest norm is

\(\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 公式一致。行满秩时同理。

\(\mathbf x^{+} = A^{+}\mathbf b\) is the smallest-norm vector among all \(\mathbf x\) that minimise \(\|A\mathbf x - \mathbf b\|\) (the derivation is the decoupling in Section 5). A few handy facts:

• \(AA^{+}\) projects onto the column space and \(A^{+}A\) onto the row space. With full column rank, \(AA^{+} = P\).
• The four Penrose conditions: \(AA^{+}A = A\), \(A^{+}AA^{+} = A^{+}\), \((AA^{+})^\top = AA^{+}\), \((A^{+}A)^\top = A^{+}A\). Exactly one matrix \(A^{+}\) satisfies them.
• With full column rank, \((A^\top A)^{-1}A^\top = V\Sigma^{-2}V^\top\cdot V\Sigma U^\top = V\Sigma^{-1}U^\top\), matching the SVD formula. Likewise for full row rank.

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

Small singular values are the source of trouble: \(1/\sigma_i\) amplifies noise. Damping penalises the size of the solution too, trading accuracy against size:

第一个等号的推导很简洁：这等价于一个普通的最小二乘问题，只是在方程后面多叠了 \(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\) 小时，要求逆的矩阵更小。

The first equality has a neat derivation: it is an ordinary least-squares problem with \(n\) extra equations \(\lambda\mathbf x\approx\mathbf 0\) stacked underneath: \(\begin{bmatrix}A\\ \lambda I\end{bmatrix}\mathbf x\approx\begin{bmatrix}\mathbf b\\ \mathbf 0\end{bmatrix}\). Write its normal equations and you immediately get \((A^\top A + \lambda^2 I)\mathbf x = A^\top\mathbf b\). The eigenvalues of \(A^\top A + \lambda^2 I\) are all at least \(\lambda^2\), so it is always invertible. Statisticians call this ridge regression; in inverse problems it is Tikhonov regularisation.

In SVD coordinates the gain of each direction changes from \(1/\sigma\) to \(\sigma/(\sigma^2+\lambda^2)\) (hover that term in the formula):

• \(\sigma\gg\lambda\): gain \(\approx 1/\sigma\), almost untouched.
• \(\sigma\ll\lambda\): gain \(\approx\sigma/\lambda^2\to 0\); weak directions are quietly given up instead of amplifying noise.
• The largest gain is \(1/(2\lambda)\) (at \(\sigma = \lambda\)), so \(\lambda\) directly caps the worst-case amplification.

The price is exactness: the goal along weak directions is only partly achieved and the residual grows. The larger \(\lambda\), the smaller the solution and the larger the residual; \(\lambda\to 0\) gives back the pseudo-inverse. Put \(A = J\), \(\mathbf x = \dot{\mathbf q}\), \(\mathbf b = \dot{\mathbf x}\) and you have DLS for inverse kinematics. An equivalent form is \(\dot{\mathbf q} = J^\top(JJ^\top + \lambda^2 I)^{-1}\dot{\mathbf x}\), where the matrix to invert is smaller when the task dimension \(m\) is below the number of joints \(n\).

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

So far every measurement was trusted equally. If measurement \(i\) has noise standard deviation \(s_i\), reliable measurements should speak louder: minimise \((A\mathbf x - \mathbf b)^\top W(A\mathbf x - \mathbf b)\) with \(W = \operatorname{diag}(1/s_i^2)\), which gives

让平面两连杆机械臂（和 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 秒里的最大需求。

Let the end effector of a planar two-link arm (the same one as in Lesson 1.12, \(\ell_1 = 1.2\), \(\ell_2 \approx 0.85\)) chase a target point. Each frame:

1. The desired end-effector velocity is \(\dot{\mathbf x} = K(\mathbf x_{\text{target}} - \mathbf x)\) (head for the target, with a speed cap).
2. Solve \(J(\mathbf q)\,\dot{\mathbf q} = \dot{\mathbf x}\) for the joint velocity \(\dot{\mathbf q}\), in one of three ways: the inverse \(J^{-1}\); the pseudo-inverse, which simply drops directions with \(\sigma_i<0.08\); or damped DLS with gain \(\sigma/(\sigma^2+\lambda^2)\).
3. Real motors have a speed limit, so each joint speed is clipped to \(\pm 6\) rad/s before the update \(\mathbf q \leftarrow \mathbf q + \dot{\mathbf q}\,\Delta t\).

Drag the purple target. Use the shortcut buttons to put it just outside the workspace edge, or into the “hole” near the base (both the outer and inner rings are singular, Lesson 1.12), and switch between the three solvers. The bars at the right show the joint speeds the solver demands (before clipping), the dashed line is the motor limit, and the strip below plots the largest demand over the last 4 seconds.

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

One subtlety: for a reachable target the plain inverse can still work near the edge, because the desired speed shrinks as the hand nears the target, so \(\dot{\mathbf x}/\sigma_2\) does not explode. Trouble starts when the target is out of reach, or when it sweeps quickly along the singular direction: then the demand is “a big step along the weak direction”, and the gain of that direction is exactly \(1/\sigma_2\). The damped solver copes with both, at the price of reacting more slowly along the singular direction (lag).