视觉SLAM理论入门——（5）非线性优化

最新推荐文章于 2025-06-06 01:16:47 发布

原创

最新推荐文章于 2025-06-06 01:16:47 发布 · 1.1k 阅读

9 ·

CC 4.0 BY-SA版权

文章标签：

#slam

本文探讨了在视觉SLAM中如何通过最大后验与最大似然方法，结合滤波器和非线性优化技术，处理受噪声影响的相机数据，以实现精确状态估计。重点介绍了最小二乘法在最大似然估计中的应用，以及高斯牛顿法和列文伯格—马夸尔特法在求解非线性最小二乘问题中的策略。

即使我们有着高精度的相机，我们得到的数据通常是受各种未知噪声影响的，运动方程和观测方程也只能近似的成立，因此需要研究如何在有噪声的数据中进行准确的状态估计。

1、状态估计问题

1.1、最大后验与最大似然

经典 SLAM 模型由一个运动方程和一个观测方程构成

其中 $x_k$ 表示相机位姿， $u_k$ 表示传感器输入， $z_{k,j}$ 表示观测数据， $y_j$ 表示路标， $w_k$ 和 $v_{k,j}$ 表示噪声，一般认为噪声服从零均值高斯分布 $w_k$ ~ $N(0,R_k)$ ， $v_{k,j}$ ~ $N(0,Q_{k,j})$

运动方程在视觉 SLAM 中没有特殊性，一般讨论观测方程。对于观测方程，希望通过带噪声的数据 $z_{k,j}$ 和 $u_k$ ，推断位姿 $x_k$ 和地图 $y_j$ （以及它们的概率分布），这构成了一个状态估计问题。主要的解决思路是使用滤波器、非线性优化

滤波器关心当前时刻的状态估计，而对之前的状态则不多考虑；而非线性优化使用所有时刻采集到的数据进行状态估计，一般认为优于滤波器

在非线性优化中，将待估计变量放入一个状态变量 $x$ 中

$x = [x_1,x_2,...,x_N,y_1,y_2,...y_M]^T$

对机器人进行状态估计，也就是求已知输入 $u$ 和观测 $z$ 的条件下，状态 $x$ 的条件概率分布 $P(x|z,u)$

特别地，当我们没有测量运动的传感器，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 $P(x|z)$ 的条件概率分布。如果忽略图像在时间上的联系，把它们看作一堆彼此没有关系的图片，该问题也称为SfM，即如何从许多图像中重建三维空间结构，SLAM 可以看作是图像具有时间先后顺序的，需要实时求解一个 SfM 问题