《视觉SLAM14讲》ch6学习记录

最新推荐文章于 2024-08-14 15:20:01 发布

只是什么都不会

最新推荐文章于 2024-08-14 15:20:01 发布

阅读量181

点赞数

文章标签：学习人工智能

本文链接：https://blog.csdn.net/weixin_44169087/article/details/132333982

版权

1. 状态估计问题

对于slam模型来说，它由一个运动方程和一个状态方程组成。运动方程与输入的具体形式有关，观测方程则由针孔模型给定，与相机内参，像素点的距离等因素有关。现在考虑数据受噪声影响，在运动和观测方程中，分别加入一个满足零均值高斯分布的噪声。在这些噪声的影响下，希望可以通过带噪声的数据推断位姿和地图（以及他们的概率分布），这构成了一个状态估计问题。

如何处理这种状态估计问题，在slam中，一般使用两种方法，滤波器和批量处理的方法。由于slam中的数据是随着时间逐渐到来的，前者在持有一个当前时刻的估计状态下，使用新的数据来更新它，这种方式称为增量/递进的方法，也叫做滤波器；后者是把一段时间的数据攒下来一并处理。

增量方法仅关心当前时刻的状态估计，批量方法则可以在更大范围内达到最优化，故其优于传统的滤波器，成为视觉slam的主流方法。

为了估计状态变量的条件分布，可以利用贝叶斯法则，该法则将后验概率分解为似然概率和先验概率的乘积。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化是可行的，等价于求最大化似然和先验的乘积。

似然指的是“在现在的位姿下，可能产生怎样的观测数据”，由于我们知道观测数据，所以最大似然估计就可以理解为：“在什么样的状态下，最可能产生现在观测到的数据”。

对于非线性优化问题的解释，这篇文章讲的很细致，下面我将使用其中一些内容做一些概括，感兴趣的读者可以移步原文阅读。

首先，上述说到可以将状态变量的估计等效于最大似然的估计，具体来说就是，将当前状态估计与观测的数据的误差最小化，那么也可以将SLAM优化问题，变成一个最小二乘问题。

2. 最小二乘问题

最小二乘法形式可以表示为如下：

等式左边即为目标函数，θ即为要优化的参数。对于矩阵形式，我们可以直接把简单的非线性最小二乘问题定义为：

如果f 是个数学形式上很简单的函数，问题可以直接用解析形式来求。令目标函数的导数为零，然后求解x 的最优值，但事实的情况通常是：由于我们使用李代数来表示机器人的旋转和位移，我们不能够顺利求解上式这样一个复杂的非线性方程。而对于不方便直接求解的最小二乘问题，我们可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。具体步骤可列写如下：

让求解导函数为零的问题，变成了一个不断寻找梯度并下降的过程。直到某个时刻增量非常小，无法再使函数下降。此时算法收敛，目标达到了一个极小。这种算法的问题是，如果初值和增量选取不合适，容易使求解陷入局部最优。

3. 梯度下降法和牛顿法

梯度下降法是求解无约束优化问题最简单和最古老的方法之一，虽然现在已经不具有实用性，但是许多有效算法都是以它为基础进行改进和修正而得到的。梯度下降法，其本质是一阶优化算法，有时候也称为最速下降法（Steepest Descent）。

首先，我们将目标函数在X附近进行泰勒展开：

J(x)是f(x)关于x的导数（雅可比矩阵），H(x)是二阶导数(海森矩阵)。我们可以选择保留泰勒公式的一阶导数和二阶导数，如果保留一阶导数，则增量的解就是：

直观意义非常简单，只要我们找到梯度然后沿着反向梯度方向前进即可。当然，我们还需要该方向上取一个步长 λ，求得最快的下降方式。这种方法被称为最速梯度下降法或是一阶梯度法。

梯度下降法的示意图如下：

该方法可以总结为以下几个点：

1、实际使用中使用梯度下降法找到的通常是局部极小值，而不是全局最小值。

2、具体找到的是哪个极小值和初始点位置有很大关系。

3、如果函数是凸函数，那么梯度下降法可以保证收敛到全局最优解（最小值）

4、梯度下降法收敛速度通常比较慢

5、梯度下降法中搜索步长 λ 很关键，步长太长会导致找不到极值点甚至震荡发散，步长太小收敛非常慢。

另一方面，如果保留二阶梯度信息，那么增量方程为：

对Δx求导数并令它等于0，就得到了增量解。这种方法称为牛顿法或二阶梯度法，它的迭代公式可以表示为：

这两种方法只要把函数在迭代点附近进行泰勒展开，并针对更新量作最小化即可。由于泰勒展开之后函数变成了多项式，所以求解增量时只需解线性方程即可，避免了直接求导函数为零这样的非线性方程的困难。这两种方法也存在它们自身的问题。最速梯度下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的 H 矩阵，这在问题规模较大时非常困难，我们通常倾向于避免 H 的计算。

4. 高斯牛顿法

牛顿法中包含了Hessian 矩阵的计算。而在高维度下计算Hessian 矩阵需要消耗很大的计算量，很多时候甚至无法计算。而高斯牛顿（Gauss-Newton）法是对牛顿法的一种改进，它用雅克比矩阵的乘积近似代替牛顿法中的二阶Hessian 矩阵，从而省略了求二阶Hessian 矩阵的计算。
Gauss Newton 是最优化算法里面最简单的方法之一。它的思想是将 f(x) 进行一阶的泰勒展开，但不是对目标函数直接展开，而是做如下展开：

这里 J(x) 为 f(x) 关于 x 的导数，实际上是一个 m × n 的矩阵，也是一个雅可比矩阵。根据前面的框架，当前的目标是为了寻找下降矢量 ∆x，使得 ∥f (x + ∆x)∥2 达到最小。为了求 ∆x，我们构建一个线性的最小二乘问题：

根据极值条件，将上述目标函数对 ∆x 求导，并令导数为零。由于这里考虑的是 ∆x 的导数（而不是 x），我们最后将得到一个线性的方程。为此，先展开目标函数的平方项：

求上式关于 ∆x 的导数，并令其为零：

我们要求解的变量是 ∆x，这是一个线性方程组，我们称它为增量方程或高斯牛顿方程 (Gauss Newton equations) 或者正规方程 (Normal equations)。我们把左边的系数定义为 H，右边定义为 g，那么上式变为：

其中我们把J^TJ作为牛顿法中Hessian矩阵的近似，从而用求一阶雅克比矩阵代替了直接求二阶Hessian矩阵的过程。

5. 列文博格——马夸尔特（LM）方法

由于高斯牛顿方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给 ∆x 添加一个信赖区域（Trust Region），不能让它太大而使得近似不准确。在信赖区域里边，我们认为近似是有效的；出了这个区域，近似可能会出问题。一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定这个范围：如果差异小，我们就让范围尽可能大；如果差异大，我们就缩小这个近似范围。因此，考虑使用：

来判断泰勒近似是否够好。ρ 的分子是实际函数下降的值，分母是近似模型下降的值。如果 ρ 接近于 1，则近似是好的。如果 ρ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 ρ 比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。LM算法表示如下：

LM 方法更接近于高斯牛顿法，且在一定程度上修正了高斯牛顿法的缺点，因此它比高斯牛顿法更加鲁棒，不过这是以牺牲一定的收敛速度为代价的——它的收敛速度比高斯牛顿法慢。

总而言之，非线性优化问题的框架，分为 Line Search 和 Trust Region 两类。Line Search 先固定搜索方向，然后在该方向寻找步长，以最速下降法和高斯牛顿法为代表。而 Trust Region 则先固定搜索区域，再考虑找该区域内的最优点。此类方法以 LM 为代表。实际问题中，我们通常选择高斯牛顿或 LM 之一作为梯度下降策略。