slam非线性优化的思路

1万 · 2022-9-23 09:31

#每日话题# #有奖活动# #申请原创# 问题的引出：

经典SLAM模型的运动方程可以有变换矩阵描述，并由李代数进行优化；观测方程由相机模型给出，根据针孔成像原理，内参由相机标定后得到，外参是相机的位姿。

如果一切都是这种理想状态，那就简单多了

然而，不出意外，意外发生了。

由于噪声的存在，使得运动方程和观测方程并不是完全准确的，所以需要在有噪声的情况下进行准确的状态估计，于是引出了非线性优化的问题。

1万 · 2022-9-23 10:05

概率论与数理统计中的估计问题：状态的估计，是对位姿和路标的估计：

插入知识：参数估计

理论依据：

点估计：
设总体X的分布函数的形式已知，但它的一个或多个参数未知，借助于总体X的一个样本来估计未知参数的值的问题称为参数的点估计问题。
矩估计法和极大似然估计法是两种最常用的构造估计量的方法

矩估计：

1万 · 2022-9-23 10:07

极大似然估计：

1万 · 2022-9-23 10:29

本帖最后由 keer_zu 于 2022-9-23 11:02 编辑

根据贝叶斯公式：

其中P(x|z)称为后验（从结果到原因），P(z|x)称为似然，P(x)成为先验（可以通过经验和以往数据得到）

求概率分布转换为求状态的最优估计：

如若不知道先验概率P(x)，则可以求得最大似然概率MLE(Maximum Likely Estimation)。也可以说，最大似然概率求的是在那种状态估计下，最能够产生当前的观测值。

1万 · 2022-9-23 10:35

由观测方程，对于某一次观测，有：

假设噪声v是正态分布，那么Zk,j=h(yj,xk）+vk,j也是正态分布（可以尝试证明）

这里μ为什么是N(h(yj,xk))，需要自己证明。

1万 · 2022-9-23 12:44

本帖最后由 keer_zu 于 2022-9-23 12:48 编辑

考虑一个任意高斯分布 x ~ N(μ，∑）：

将上面的过程带入到观测模型的满足高斯分布的条件概率P（zk,j|xk,yj) = N ( h(yj,xk),Qk,j)得到。求此概率的最大，即为最小化：

注：这里的Qk,j为噪声vk,j分布的协方差，它的逆称为信息矩阵。

由上式可以看出，求上式的最小即为最小化噪声项（zk,j-h(xk,yj即为噪声项)的平方。

1万 · 2022-9-23 12:47

所以对于所有的运动和任意的观测定义数据与估计值的误差：

并定义该误差的平方和：

即上式的最优解等价于状态的最大似然估计！！！
而上式的最优解是一个最小二乘问题。

注1：上式的目标函数J（x)由许多个误差的平方和组成。
注2：如果使用李代数，则为无约束的最小二乘，使用R、T则因为有约束而很复杂。所以使用李代数。
注3：误差是使用2范数（平方形式）来度量。

1万 · 2022-9-23 12:50

非线性最小二乘：

对于简单的线性函数最小二乘很简单就是求导并令其为0，不介绍，但对于复杂的非线性的，使用迭代的方式。

这让求解导函数为零的问题，变成了一个不断寻找梯度并下降的过程。直到某个时刻增量非常小，无法再使函数下降。此时算法收敛，目标达到了一个极小，我们完成了寻找极小值的过程。在这个过程中，我们只要找到迭代点的梯度方向即可，而无需寻找全局导函数为零的情况。

这里就有两个问题：

1）初始值怎么给？
2）增量怎么确定？

先来解决第二个问题，增量的问题：
求解增量的最直观的方法是将目标函数在x附近进行泰勒展开：

这里 J 是∥f(x)∥2 关于 x 的导数（雅可比矩阵），而 H 则是二阶导数（海塞（Hessian）矩阵）。我们可以选择保留泰勒展开的一阶或二阶项，对应的求解方法则为一阶梯度或二阶梯度法

注：雅可比矩阵是函数的一阶偏导以一定形式排列成的矩阵。

1万 · 2022-9-23 13:39

下面介绍2种常用的方法——高斯牛顿法和列文伯格—马夸尔特法

高斯牛顿法：
思想是将 f(x) 进行一阶的泰勒展开（请注意不是目标函数 f(x)2）

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

对上式进行展开：

注：这里的展开平方处理成转置乘原式，我没有看懂，数学上的问题缺失。

求上式关于 ∆x的倒数并令其等于0：

但是这种方法存在缺点：

1）在求解增量方程时，要求所用的近似H矩阵是可逆的（且正定的），但实际中JTJ只有半正定。也就是在求解过程中可能会出现JTJ为奇异矩阵或者病态的情况，这种情况下，增量的稳定性较差，导致算法不收敛。

2）就算H满足需求，如果求出来的步长太大，也会导致采用的局部近似不准确，甚至无法保证其收敛，甚至让目标函数变得更大。

所以SLAM中通常采用的是以下的方法 —— 列文伯格—马夸尔特（也叫阻尼牛顿法）

1万 · 2022-9-23 13:41

阻尼牛顿法：
因为高斯牛顿法采用的二阶近似泰特展开只能在展开点附近有较好的近似效果。所以给∆x添加一个信赖区间。非线性优化有一系列的这类方法，这类方法被称为信赖区域方法。

那么如何来确定这个信赖区域的范围，比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定。即如果差异小，则范围大；如差异大，则缩小范围。

怎么来判断泰勒近似是否足够好？使用下面的式子来判断：

p的分子是实际函数下降的值，分母是近似模型下降的值。
当p接近1，则近似是好的；
当p不接近1(太小），则认为近似太差，需要缩小近似范围；
当p较大时，则放大近似范围。

所以阻尼牛顿法的步骤如下：

在上图的式子6.24中，我们把增量限定于一个半径为 µ 的球中，认为只在这个球内才是有效的。带上 D 之后，这个球可以看成一个椭球。Levenberg 提出的优化方法中，把 D 取成单位阵 I，相当于直接把 ∆x 约束在一个球中。随后，Marqaurdt 提出将 D 取成非负数对角阵——实际中通常用 JTJ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。

不论如何，在 L-M 优化中，我们都需要解式（6.24）那样一个子问题来获得梯度。这个子问题是带不等式约束的优化问题，我们用 Lagrange 乘子将它转化为一个无约束优化问题：

这里 λ 为 Lagrange 乘子。类似于 Gauss-Newton 中的做法，把它展开后，我们发现该问题的核心仍是计算增量的线性方程：

注1：这里的展开类似于上文中高斯牛顿法的：

注2：H = J(x)TJ(x);
g = -J (x)Tf(x);
D为非负数对角阵。实际中通常使用JT的对角元素平方根。简化形式下：认为D=I；

当认为D为单位矩阵I时，则上式则相当于求解：

我们看到，当参数 λ 比较小时，H 占主要地位，这说明二次近似模型在该范围内是比较好的，L-M 方法更接近于 G-N 法。另一方面，当 λ 比较大时，λI 占据主要地位，L-M 更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。L-M 的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量 ∆x。

在实际中，还存在许多其它的方式来求解函数的增量，例如 Dog-Leg 等方法。我们在这里所介绍的，只是最常见而且最基本的方式，也是视觉 SLAM 中用的最多的方式。总而言之，非线性优化问题的框架，分为 Line Search 和 Trust Region 两类。Line Search 先固定搜索方向，然后在该方向寻找步长，以最速下降法和 Gauss-Newton 法为代表。而 Trust Region 则先固定搜索区域，再考虑找该区域内的最优点。此类方法以 L-M 为代表。实际问题中，我们通常选择 G-N 或 L-M 之一作为梯度下降策略。

1万 · 2022-10-11 14:24

slam总结

1万 · 2022-10-11 14:25

slam三大矩阵

slam非线性优化的思路

相关下载

相关帖子

突出贡献奖章

沉静之湖泊

七世轮回

技术奇才奖章