【技术分享002】从贝叶斯到因子图优化：理解现代SLAM后端的完整数学链路

😀

一个早该搞懂却一直懵懵懂懂的东西，这次来解决他

1. SLAM到底在估计什么？

SLAM的目标：

根据传感器观测，同时估计机器人的运动轨迹和环境地图。

假设二维机器人位置：

其中：

：位置x

：位置y

：yaw角

机器人运动过程中产生：

IMU数据

视觉里程计

激光匹配结果

GPS信息

这些数据组成：

我们希望找到：

使机器人轨迹最合理。

2. 为什么是概率问题？

在已有传感器数据之后，哪个状态(位置)最可能？

数学表示：。读作：“在知道观测Z以后，状态X出现的概率”。这就是后验概率。

3. 贝叶斯公式如何进入SLAM？

贝叶斯公式：

因为对于所有候选状态都是一个常数，所以优化时可以忽略。我们可以得到

这个公式非常重要。

它说明：机器人状态可信度来自两个部分：

3.1 传感器是否支持这个状态

也就是： → Likelihood（似然）

例如：机器人认为自己在(10,5)

如果Lidar看到的墙壁位置符合这个状态：概率高。

如果完全不符合：概率低。

3.2 这个状态本身是否合理

也就是： → Prior（先验）

例如：IMU告诉我们机器人不可能一秒移动100米，所以这种状态先验概率很低。

4. 最大后验估计（MAP）

我们希望找到概率最大的状态：

代入贝叶斯：

这就是最大后验估计MAP。

5. 为什么概率突然变成优化？

优化通常喜欢求最小值,所以我们对概率取负log。

原因：log不会改变大小关系：

如果：

那么：

所以最大化：等价于最大化：

利用：

得到：

乘以负号：

于是概率最大问题，变成了优化问题。

6. 为什么最后变成最小二乘？

关键假设：

传感器噪声服从高斯分布。

例如：

Lidar测量残差：

假设：

其概率密度：

取负log：

展开：

其中：与状态X无关，是常数。优化时可以忽略：

因此最大概率等价于：

这就是加权最小二乘（Weighted Least Squares）

矩阵形式：

其中：

：第i个factor残差

：噪声协方差

：信息矩阵（Information Matrix）

6.1 的处理：测量Factor

多个传感器：

Lidar

Camera

都会产生类似残差,可以根据上述推导得到加权最小二乘

6.2 的处理：Prior Factor

除了传感器观测，我们还有：

它表示对机器人状态本身的先验约束。

例如SLAM中通常需要固定初始位置：

我们可以认为：

同样根据高斯分布：

定义先验残差：

则：

这就是Prior Factor（先验因子）

6.3 统一

因此，最终的MAP问题：

统一转换为：

其中k包括：

IMU Factor

Lidar Factor

Camera Factor

GPS Factor

Prior Factor

Loop Closure Factor

这就是因子图优化最终求解的形式。

7. Factor是什么？

现在回到因子图。每个传感器产生一个概率约束。

例如：

IMU Factor

IMU测量机器人从移动到，测量

理论关系：

残差：

Lidar Factor

Lidar提供：

残差：

GPS Factor

GPS提供绝对位置

残差

因此：

Factor不是简单的误差，而是一个包含测量模型、残差计算和噪声模型的概率约束。

8. 什么是因子图？

因子图包含两类节点：

Variable Node

未知状态：X0、X1、X2

例如：

机器人pose。

Factor Node

测量约束：

9. 为什么不同传感器可以融合？

因为所有传感器最终都转换成残差。

例如

优化

所以IMU、Lidar、Camera、GPS虽然物理形式不同，但是最终都变成：

对机器人状态的概率约束。

10. 如何调节传感器置信度？

工程中调整的是factor的noise model

例如：

Lidar：m

IMU：m

权重：

因此Lidar权重大,优化更加相信Lidar。

但是!不是永远Lidar > IMU。

例如：

Lidar：

XY通常准确

长走廊yaw可能退化

IMU：

yaw角速度稳定

长时间平移漂移

所以实际系统会针对：分别设置协方差。

11. Gauss-Newton如何求解？

最终问题：

由于SLAM通常非线性需要迭代优化。

当前估计X → 寻找

对残差线性化：

得到：

求解：

更新：

不断迭代直到收敛。

义父，请我喝杯蜜雪冰城吧。