00.9｜导航与 SLAM 前置：观测残差、最小二乘与不可观方向

从一个距离观测推导残差与高斯牛顿，再连接 SE(3) 位姿图、地图规范自由度、闭环与 map/odom，理解 SLAM 在估计什么以及它没有解决什么。

1. 导航、定位、建图和控制不是同一件事

定位估计“我在哪里、朝哪里”；建图估计环境的几何或其他属性；SLAM 同时估计轨迹与地图；规划选择希望经过的路径；控制驱动实际系统跟随参考。一个机器人可以定位很好却撞上未建模障碍，也可以有漂亮点云却没有稳定的速度反馈。

本节只建立读懂导航与 SLAM 数学所需的基础，不把所有算法名堆在一起。阅读顺序是：传感器真正测到什么 → 状态如何预测该测量 → 两者的差如何定义 → 怎样调整状态让解释更一致 → 哪些方向无论怎么优化都没有足够信息。

2. 一个测距值怎样变成位置约束？

设平面机器人位置 $p=[x,y]^T$ ，已知信标位置 $a=[a_x,a_y]^T$ ，单位 m。测距传感器输出标量 $z$ ，模型为

z=h(p)+n,\qquad h(p)=\|p-a\|=\sqrt{(x-a_x)^2+(y-a_y)^2}.

$n$ 是测量噪声。传感器输出只有一维，并不妨碍它约束二维位置；只是一个距离圈不能独立确定两个坐标。

本节优化统一用预测减测量残差：

r(p)=h(p)-z.

若预测距离为 $4.2$ m、实际测量为 $4.0$ m，残差是 $0.2$ m。KF 常用测量减预测的新息，两种符号都可以，但雅可比、更新右端项必须跟着统一。

对状态求导，记 $d=\|p-a\|>0$ ：

J=\frac{\partial r}{\partial p}=\begin{bmatrix}(x-a_x)/d&(y-a_y)/d\end{bmatrix}.

推导来自链式法则：平方根导数为 $1/(2\sqrt{\cdot})$ ，内部平方项导数为 $2(x-a_x)$ ，相乘后消掉二。几何上， $J$ 是从信标指向机器人方向的单位行向量；沿这个方向移动会一阶改变距离，沿切线方向小幅移动则一阶不变。

当 $p=a$ ，范数在原点不可微，这张雅可比不成立；代码应识别退化，而不是直接除零。真实传感器通常也有最小有效测距范围。

3. 为什么平方残差前面要乘信息矩阵？

假设每项测量的噪声独立且为零均值高斯，标量标准差为 $\sigma_i$ 。它的概率密度含指数项

p(z_i\mid p)\propto\exp\left(-\frac12\frac{r_i(p)^2}{\sigma_i^2}\right).

独立观测的联合概率是各项乘积；取负对数，把乘积变成加和，丢弃不依赖状态的常数，就得到

\min_p\ \mathcal E(p)=\frac12\sum_i\frac{r_i(p)^2}{\sigma_i^2}.

小标准差的观测对应更大权重。这不是“我觉得 GPS 更好所以给它十倍权重”，而是在表达噪声统计模型。方差估错、系统偏差未建模、观测相关时，这个概率解释就会偏离真实情况。

多维残差的协方差记为 $\Sigma_i$ ，信息矩阵为 $\Omega_i=\Sigma_i^{-1}$ ，则每项变成 $r_i^T\Omega_i r_i/2$ 。若 $\Sigma_i$ 正定，可找到白化矩阵 $W_i$ 使 $W_i^TW_i=\Omega_i$ ，于是等价于 $\|W_ir_i\|^2/2$ 。经过正确白化后，不同单位和精度的误差才能在统计意义上相比较。

若两组数据来自同一底层观测，不能因为接口不同就当作独立因子重复加权。轮速已经构成里程计约束，又把同一轮速积分的位置当独立绝对观测，会重复使用信息；这与 KF 中先验—观测相关的问题一致。

GTSAM 官方因子图教程将变量节点与概率约束分开：变量储存待估计的状态，因子只连接与该测量有关的状态。因子图不是另一种传感器，而是组织这些依赖关系的方法。

4. 从一阶展开亲手推出高斯—牛顿

当前猜测是 $p_0$ ，希望找到小修正 $\delta p$ 。对每项残差做一阶展开：

r_i(p_0+\delta p)\approx r_i(p_0)+J_i\delta p.

将所有已白化的残差堆成列向量 $r$ ，雅可比堆成矩阵 $J$ 。局部问题成为

\min_{\delta p}\frac12\|r+J\delta p\|^2.

展开平方得到 $\frac12r^Tr+r^TJ\delta p+\frac12\delta p^TJ^TJ\delta p$ 。对 $\delta p$ 求导并令零：

J^T(r+J\delta p)=0,\qquad H\delta p=-g,\quad H=J^TJ,\quad g=J^Tr.

负号来自本节定义的预测减测量残差。 $H$ 是高斯—牛顿近似 Hessian，并非任何非线性问题的完整二阶导数；被省略的是与残差乘二阶导数有关的项。在残差小、初值合理时通常有效。

解出后更新 $p\leftarrow p+\delta p$ ，重新计算残差和雅可比，再迭代。不要只在线性化点计算一次 $J$ ，却声称解决了整个强非线性问题。

一个完全可手算的二维例子

有两个已知信标 $a_1=[0,0]^T$ 、 $a_2=[2,0]^T$ m，观测均为 $\sqrt2$ m，候选真值为 $p=[1,1]^T$ m。在该点，两个雅可比为

J_1=\frac1{\sqrt2}[1,1],\qquad J_2=\frac1{\sqrt2}[-1,1].

若两项标准差相同并先忽略共同缩放， $J^TJ=I$ ，局部两个方向都能被约束。但 $[1,-1]^T$ 也给出相同距离，所以局部满秩不意味着全局只有一个解。需要额外信标、先验、运动信息或其他观测排除镜像解。

如果机器人和两个信标都恰好在同一直线上，雅可比行向量共线，局部对垂直方向不敏感。这是观测几何退化，不是多跑几次优化就会消失。

5. 状态包含旋转时，为什么不能直接给矩阵加九个数？

位姿 $T_i=T_{WB_i}\in SE(3)$ 表示机体 $B_i$ 到世界 $W$ 的变换。旋转矩阵必须满足正交和行列式约束。更新 $R\leftarrow R+\Delta R$ 通常会离开旋转群。

采用右局部扰动，令 $\delta\xi=[\delta\rho^T,\delta\phi^T]^T\in\mathbb R^6$ ，平移扰动在前、旋转扰动在后，定义

T_i\boxplus\delta\xi_i=T_i\operatorname{Exp}_{SE(3)}(\delta\xi_i).

这里 $\operatorname{Exp}_{SE(3)}$ 接受六维向量并在内部构造帽算子矩阵， $\operatorname{Log}_{SE(3)}$ 输出六维向量，与李群课的大写函数约定一致。 $\delta\rho$ 单位 m， $\delta\phi$ 单位 rad；局部扰动表达在当前机体系。优化是在六维切空间求增量，再用指数映射把增量送回合法位姿。指数中的平移分量与有限变换的实际平移还包含 $SO(3)$ Jacobian 关系，不能把任意大旋转下的 $\rho$ 直接当成全局位置增量，详见李群基础。

雅可比应对应这个具体更新方式：

J_i=\left.\frac{\partial r(T_i\operatorname{Exp}_{SE(3)}(\delta\xi_i))}{\partial\delta\xi_i}\right|_{\delta\xi_i=0}.

换成左扰动会改变雅可比。不能从一篇左扰动论文复制 $J$ ，再在右扰动代码里直接使用。

6. 里程计如何成为两个位姿之间的约束？

机器人在时刻 $i,j$ 的位姿分别为 $T_i,T_j$ 。它们预测的相对变换是

\hat Z_{ij}=T_i^{-1}T_j.

$T_j$ 先将 $B_j$ 坐标变到世界，再由 $T_i^{-1}$ 变到 $B_i$ 。因此 $\hat Z_{ij}$ 把 $B_j$ 坐标变到 $B_i$ ，这与观测 $Z_{ij}$ 的方向必须一致。

一种常用局部残差定义为

r_{ij}=\operatorname{Log}_{SE(3)}\left(Z_{ij}^{-1}T_i^{-1}T_j\right)\in\mathbb R^6.

若预测与观测完全相同，括号中为单位变换，对数为零；这就是它作为误差的理由。本节仍按平移三维、旋转三维排列。该残差所在局部坐标系以及观测噪声协方差的表达必须匹配；不同文献可能把误差乘在另一侧，不能只比较公式符号。

每条边只依赖两个位姿，所以雅可比在整张图中只有两个非零块。这是稀疏性的来源，不是人为把不重要数字删为零。回环检测增加“很久以前的位姿与现在的位姿”的约束，让误差能在整条轨迹中重新分配。错误回环同样会传播到整张图，必须做数据关联与几何一致性检查。Grisetti 等的图优化 SLAM 教程论文是这个建模思路的经典入口。

7. Gauge：为什么地图可以整体平移旋转，却不影响所有相对观测？

假设只有相对位姿约束，对所有位姿同时左乘同一个固定变换 $G$ ，令 $T_i'=GT_i$ 。那么

(T_i')^{-1}T_j'=(GT_i)^{-1}(GT_j)=T_i^{-1}G^{-1}GT_j=T_i^{-1}T_j.

所有相对预测都没变，残差和代价也没变。所以仅凭这些约束，不能决定地图的绝对原点与朝向；这称为规范自由度。纯三维位姿图通常有六维整体刚体自由度，二维有三维。

固定第一个位姿是选定坐标规范，不是凭空获得一个精确 GPS 测量。若所有位置都相对它描述，这样做方便且合理；但不能因此宣称得到地球坐标中的绝对精度。

单目视觉还可能缺少绝对尺度，因为将场景和相机平移一起按比例缩放，不改变理想透视投影。在有已知尺度基线、可辨识惯性激励、测距或其他度量信息时，尺度可以被约束；“加了 IMU 就任何运动下都立刻有准确尺度”不成立。

注意三个词的区别：

参数化冗余：例如单位四元数用四个数描述三自由度旋转。
规范自由度：整体换地图坐标系不改变观测。
退化运动或观测几何：例如沿缺乏特征的长走廊，某些运动方向难以从观测辨认。

它们都可能造成矩阵问题，但修复方式不同。换四元数不能修复缺少全局位置参照；加一个数值阻尼也不能创造真实观测信息。

8. 为什么先检查可观性，再谈求逆？

若存在非零方向 $d$ 使 $Jd=0$ ，则 $Hd=J^TJd=0$ ， $H$ 不可逆。这个方向的小变化不会改变当前线性化观测。加入 $\lambda I$ 可使数值求解可行，却只是人为惩罚该方向的增量；不要把正则化后的逆矩阵机械解释为真实后验精度。

在已正确固定规范、噪声模型合理且局部线性近似有效的条件下， $H^{-1}$ 可用作局部协方差近似。大型问题通常不显式求完整逆，而按需提取边缘协方差。协方差还取决于变量单位、残差白化以及线性化点。

优化返回“收敛”通常只说明步长或代价变化满足停止条件，不证明地图正确，更不证明数据关联正确。真实验收至少还看残差分布、几何退化、与独立参照的误差、不同初值和异常样本。

9. 离群观测：鲁棒核能做什么，不能做什么？

普通平方损失让大残差影响快速增加。可对已白化标量残差 $s$ 使用 Huber 损失：

\rho(s)=\begin{cases}\frac12s^2,&|s|\le c,\\c|s|-\frac12c^2,&|s|>c.\end{cases}

$c>0$ 是无量纲转折阈值。小残差保留二次模型，大残差只线性增长。迭代重加权中，对非零残差可用 $w=\rho'(s)/s$ ，得到 $w=1$ 或 $w=c/|s|$ 。对向量残差常按白化后的范数构造一组鲁棒权重，不能不说明方式就把逐元素与整体权重混用。

鲁棒核降低异常数据影响，但不负责找出正确对应关系，也不自动纠正系统时间偏差。若所有声学位置都存在同方向偏置，鲁棒核未必能区分“多数一致错误”与真值；应建立偏置、外参、时延模型或使用独立信息。

10. SLAM 闭环修正后，控制器为何不该把跳变当作真实瞬间移动？

ROS 的 REP-105 原始规范区分局部连续的 odom 与长期全局参考 map：前者可漂移，后者可因定位更新发生跳变。该划分是工程语义，不是保证任何实现都满足精度要求。

设连续局部位姿为 $T_{OB}$ ，全局校正为 $T_{MO}$ ，则

T_{MB}=T_{MO}T_{OB}.

回环发生时，主要改变全局对齐 $T_{MO}$ ，局部短时运动反馈仍可保持连续。全局目标若固定在地图系，应通过当前变换转换为控制使用的局部参考，并对速度、加速度等参考变化加约束。

不能把估计修正一概当作“坐标系整体平移”并同步平移所有目标：有时这是对真实位置误差的纠正，目标物理位置不该跟着漂移。必须明确目标绑定的坐标系、定位重置的语义与控制模式。这和水下 USBL 位置恢复时的控制问题一致，可继续读融合机制与位置冲突。

11. C++ 练习：二维距离定位与数值雅可比检查

下面用三个已知信标估计二维位置。它是定位而不是完整 SLAM，因为信标坐标已知。练习价值是把残差、雅可比、正规方程、更新符号连起来，再推广到同时估计信标或轨迹。

展开完整 C++17 参考程序与测试

#include <array>
#include <cassert>
#include <cmath>
#include <iostream>
using V=std::array<double,2>;
double range(V p,V a){return std::hypot(p[0]-a[0],p[1]-a[1]);}
V rangeJac(V p,V a){double d=range(p,a);assert(d>1e-9);
    return {(p[0]-a[0])/d,(p[1]-a[1])/d};}
int main(){
    std::array<V,3> anchors{V{0,0},V{3,0},V{0,4}};
    V truth{1,1.2},p{0.8,0.9};
    std::array<double,3> z{};
    for(int i=0;i<3;++i)z[i]=range(truth,anchors[i]);
    const double eps=1e-6;
    for(auto a:anchors){auto j=rangeJac(p,a);
        for(int c=0;c<2;++c){V pp=p,pm=p;pp[c]+=eps;pm[c]-=eps;
            assert(std::abs((range(pp,a)-range(pm,a))/(2*eps)-j[c])<1e-8);}}
    for(int iter=0;iter<15;++iter){
        double h00=0,h01=0,h11=0,g0=0,g1=0;
        for(int i=0;i<3;++i){auto j=rangeJac(p,anchors[i]);
            double r=range(p,anchors[i])-z[i]; // 预测减测量
            h00+=j[0]*j[0];h01+=j[0]*j[1];h11+=j[1]*j[1];
            g0+=j[0]*r;g1+=j[1]*r;
        }
        double det=h00*h11-h01*h01;assert(det>1e-10);
        V d{(-h11*g0+h01*g1)/det,(h01*g0-h00*g1)/det};
        p[0]+=d[0];p[1]+=d[1];
        if(std::hypot(d[0],d[1])<1e-12)break;
    }
    assert(range(p,truth)<1e-9);
    // 全局镜像歧义：只有x轴上的两个信标时，上下两点距离完全相同。
    assert(std::abs(range({1,1},{0,0})-range({1,-1},{0,0}))<1e-12);
    assert(std::abs(range({1,1},{2,0})-range({1,-1},{2,0}))<1e-12);
    std::cout<<"SLAM prerequisites PASS\n";
}

保存 slam_prerequisites.cpp，执行 g++ -std=c++17 -O2 slam_prerequisites.cpp -o slam_prerequisites 后运行。这里构造的是良好初值、无噪声单元测试；继续增加带噪观测、错误距离、共线退化、多个初值，才能评价实际求解器。大型问题使用 QR、稀疏 Cholesky 或成熟优化库，并检查相应前提，而不是推广这个手写二阶逆。

12. 最后把它与 KF 放在同一张思维图里

KF/ESKF 通常递推维护当前状态及不确定性；图优化可同时保留多个历史状态和约束，重新线性化、优化轨迹。在相同线性高斯假设和信息处理下，两者存在紧密联系，并不是“优化一定优于滤波”。

历史窗口更长可利用更多跨时刻关系，但增加计算和边缘化管理；滤波计算紧凑，仍需正确时间同步、观测模型和可观性处理。无论哪种，第一步都不是选一个算法名，而是明确状态、测量、坐标系、时间与噪声模型。

**自检：**为什么一个测距可约束二维位置却不能唯一确定它？为什么 $J^TJ$ 满秩仍可能有镜像解？为什么纯相对位姿图整体移动不会改变代价？为什么 map 跳变不代表机器人真的瞬移？

导航与 SLAM 前置：观测残差、最小二乘与不可观方向