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

最新推荐文章于 2022-10-06 10:48:00 发布

floatinglong

最新推荐文章于 2022-10-06 10:48:00 发布

阅读量941

点赞数 3

分类专栏： SLAM 文章标签： slam

本文链接：https://blog.csdn.net/floatinglong/article/details/116202102

版权

SLAM 专栏收录该内容

30 篇文章 22 订阅

订阅专栏

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

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 问题

利用贝叶斯法则估计状态变量的条件分布 $P (x|z) = \frac{P (z|x)P (x)}{P (z)}\propto P (z|x)P (x)$ ，其中左侧是后验概率， $P (z|x)$ 是似然， $P (x)$ 是先验

直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下，后验概率最大化（Maximize a Posterior，MAP），则是可行的

$x^*_{MAP} = argmax P (x|z) = argmaxP(z|x)P(x)$ ， $P(z)$ 与待估计的状态 $x$ 无关，因而可以忽略

上式说明，求解最大后验概率，相当于最大化似然和先验的乘积；进一步，如果我们不知道机器人的位姿（没有先验），则可以求状态的最大似然估计（Maximize Likelihood Estimation, MLE）

$x^*_{MLE} = argmaxP(z|x)$

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

1.2、最小二乘法

利用最小二乘法可以求解最大似然估计。对于某一次观测

假设噪声服从零均值高斯分布 $v_k$ ∼ $N (0,Q_{k,j})$ ，因此观测数据的条件概率也服从高斯分布 $P(z_{k,j}|x_k,y_j)$ ~ $N (h(y_j,x_k),Q_{k,j})$

对于任意维高斯分布 $x$ ~ $N (\mu,\sum )$ ，概率密度函数为

取负对数后

由于对数函数单调递增，求原函数最大化，相当于求负对数的最小化。负对数中第一项与状态 $x$ 无关，可以忽略（同理第二项的常系数也可以忽略）。因此只需要计算第二项（是一个二次型）的最小化，就能得到状态的最大似然估计

将SLAM的观测方程代入

这个式子中的二次型称为马哈拉诺比斯（马氏）距离，是由 $Q^{-1}_{k,j}$ （信息矩阵，也是高斯分布协方差矩阵的逆）加权后的欧氏距离

考虑批量时刻的数据，假设各个时刻的输入之间相互独立、观测之间相互独立、输入与观测之间也相互独立，因此可以对联合分布进行因式分解

$P(z,u|x,y) = \prod_k P(u_k|x_{k-1},x_k) \prod_{k,j}P(z_k|x_k,y_j)$

这说明各个时刻的运动和观测可以独立处理。定义各时刻输入和观测与模型之间的误差

解出下面最小二乘问题的最优解即等价于状态的最大似然估计

其中，最大似然中各个似然之积经过负对数后，变为各项之和

从上式发现 SLAM 中的最小二乘问题具有一些特定的结构：
1、首先，整个问题的目标函数由许多个误差的（加权的）平方和组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。例如运动误差只与 $x_{k-1}$ , $x_k$ 有关，观测误差只与 $x_k$ , $y_j$ 有关。每个误差项是一个小规模的约
束，对它们进行线性近似，最后再把这个误差项的小雅可比矩阵块放到整体的雅可比矩阵中。由于这种做法，每个误差项对应的优化变量被称为参数块（Parameter Block）。
2、整体误差由很多小型误差项之和组成的问题，其增量方程的求解会具有一定的稀疏性，使得它们在大规模时亦可求解。
3、其次，如果使用李代数表示，则该问题是无约束的最小二乘问题。但如果用旋转矩阵（变换矩阵）描述位姿，则会引入旋转矩阵自身的约束（旋转矩阵必须是正交阵且行列式为 1）。额外的约束会使优化变得更困难。这体现了李代数的优势。
4、最后，这里使用了平方形式（二范数）度量误差，它是直观的，相当于欧氏空间中距离的平方。但它也存在着一些问题，并且不是唯一的度量方式。也可使用其他的范数构建优化问题

1.3、例：批量状态估计

考虑一个简单的离散时间系统

其中k=1,2,3，第一个式子表示运动方程，第二个式子表示观测方程， $w_k$ 和 $n_k$ 是满足零均值高斯分布的噪声，假设初始状态 $x_0$ 已知，求批量状态的最大似然估计

令状态变量 $x = [x_0,x_1,x_2,x_3]^T$ ，观测变量 $z = [z_1,z_2,z_3]^T$ ，输入变量 $u = [u_1,u_2,u_3]^T$ ，假设各时刻输入之间、观测之间、输入和观测之间互相独立，则最大似然估计

$x^*_{MLE} = argmaxP(x|u,z) = argmaxP(u,z|x)=\prod_{k=1}^3 P(u_k|x_{k-1},x_k) \prod_{k=1}^3 P(z_k|x_k)$ ，其中 $P(u_k|x_{k-1},x_k) = N(x_k-x_{k-1},R_k)$ ， $P(z_k|x_k) = N(x_k,R_k)$

构建误差变量

$e_{u,k} = x_k - x_{k-1} - u_k$

$e_{z,k} = z_k - x_{k}$

最小二乘的目标函数

$x^*_{MLE}=min(\sum_{k=1}^3e_{u,k}^TQ_k^{-1}e_{u,k} + \sum_{k=1}^3e_{z,k}^TR_k^{-1}e_{z,k})$

令 $y=[u,z]^T$ ，使 $y-Hx=e$ ~ $N(0,\sum )$ ，则

$\sum = diag(Q_1,Q_2,Q_3,R_1,R_2,R_3)$

目标函数化简为

$x^*_{MLE} = argmin (e^T\Sigma ^{-1}e)$

唯一解为

$x^*_{MLE} = (H^T\Sigma ^{-1}H)^{-1}H^T\Sigma ^{-1}y$

2、非线性最小二乘

考虑简单的最小二乘问题

$\min_x ||f(x)||^2_2$ ，其中 $x\epsilon R^n,f(x) \epsilon R^m$ ， $f(x)$ 是任意非线性函数

若 $f(x)$ 形式简单，可令目标函数导数为0，求解x的最优解。对于不方便直接求解的最小二乘问题，可以用迭代的方法求解：从一个初始值出发，不断更新当前优化变量，使目标函数下降，具体如下

1. 给定某个初始值 $x_0$
2. 对于第 $k$ 次迭代，寻找一个增量 $\Delta x_k$ ，使得 $||f (x_k + \Delta x_k )||_2^2$ 达到极小值
3. 若 $\Delta x_k$ 足够小，则停止
4. 否则令 $x_{k+1} = x_k + \Delta x_k$ ，返回 2

这让求解导函数为零的问题，变成了一个不断寻找梯度并下降的过程。直到某个时刻增量非常小，无法再使函数下降。此时算法收敛，目标达到了一个局部极小，完成寻找局部极小值的过程。这里又出现另一个问题：增量如何确定

2.1、一阶和二阶梯度法

求解增量最直观的方式是将目标函数在 x 附近进行泰勒展开

$||f(x+\Delta x)||^2_2 \approx ||f(x)||^2_2 + J(x)\Delta x + \frac{1}{2}\Delta x^TH\Delta x$

雅可比矩阵 $J$ 是 $||f(x)||^2_2$ 关于 $x$ 的导数（行向量），海塞矩阵 $H$ 是二阶导数

2.1.1、一阶梯度法

若选择保留一阶梯度，增量的方向为 $\Delta x^* = -J^T(x)$ ，它的意义是沿着反向梯度方向前进。除了方向，还需要在这个方向上取一个步长 $\lambda$ ，求得最快的下降方式。这种方法也被称为最速下降法

2.1.2、二阶梯度法

若选择保留二阶梯度，则增量方程为 $\Delta x^* = argmin||f(x)||^2_2 + J(x)\Delta x + \frac{1}{2}\Delta x^TH\Delta x$

等式右边对 $\Delta x$ 求导令其为0，增量的解为 $H\Delta x = -J^T$ ，这种方法也被称为牛顿法

一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量作最小化即可。由于泰勒展开之后函数变成了多项式，所以求解增量时只需解线性方程即可，避免了直接求导函数为零这样的非线性方程的困难

不过，这两种方法也存在它们自身的问题。最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的 $H$ 矩阵，这在问题规模较大时非常困难

2.2、高斯牛顿（G-N)法

高斯牛顿法的思想是将 $f(x)$ （梯度法将 $F(x)$ 展开）进行一阶的泰勒展开 $f(x+\Delta x) \approx f(x) + J(x)\Delta x$

为了寻找下降矢量 $\Delta x$ 使得 $||f(x+\Delta x)||^2_2$ 达到最小，需要解最小二乘问题 $\Delta x^* = arg \min_{\Delta x}||f(x)+J(x)\Delta x||^2_2$

展开后即为 $\Delta x^* = ||f(x)||^2_2+2f(x)^TJ(x)\Delta x+\Delta x^T J(x)^TJ(x) \Delta x$

上式对 $\Delta x$ 求导令其为0，得到增量方程（高斯牛顿方程） $J(x)^TJ(x)\Delta x = -J(x)^Tf(x)$ ，即 $H\Delta x = g$

对比牛顿法可以发现，高斯牛顿法使用 $J^TJ$ 近似代替牛顿法的海塞矩阵。高斯牛顿法的算法步骤如下

1. 给定初始值 $x_0$
2. 对于第 $k$ 次迭代，求出当前的雅可比矩阵 $J(x_k )$ 和误差 $f(x_k )$
3. 求解增量方程： $H\Delta x_k = g$
4. 若 $\Delta x_k$ 足够小，则停止。否则，令 $x_{k+1} = x_k + \Delta x_k$ ，返回 2

求解增量方程要求 $H$ 是正定可逆的，而实际上 $J^TJ$ 只有半正定性。因此使用这个方法时，可能出现 $J^TJ$ 是奇异矩阵或病态的情况，这时增量的稳定性较差，算法不收敛

另外，即使假设H非奇异非病态，如果求出步长 $\Delta x$ 太大，会导致采用的局部近似不够准确，无法保证目标函数迭代收敛

2.3、列文伯格—马夸尔特（L-M）法

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，而列文伯格—马夸尔特法额外给 $\Delta x$ 添加一个信赖区域，不让它太大而使得近似不准确

确定这个信赖区域范围的方法是根据近似模型跟实际函数之间的差异来确定这个范围：如果差异小，让范围尽可能大；如果差异大，就缩小这个近似范围

因此使用 $\rho = \frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}$ 判断泰勒近似是否够好。ρ 的分子是实际函数下降的值，分母是近似模型下降的值。

如果 ρ 接近于 1，则近似是好的。如果 ρ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 ρ 比较大，则说明实际下降的比预计的更大，可以放大近似范围。

因此算法步骤归纳为

1. 给定初始值 $x_0$ ，以及初始优化半径 $\mu$
2. 对于第 $k$ 次迭代，求解 $\min_{\Delta x_k}\frac{1}{2}||f(x_k)+J(x_k)\Delta x||^2_2$ ， $s.t.||D\Delta x_k||^2 < \mu$
3. 计算 ρ
4. 若 $\rho > \frac{3}{4}$ ，则 $\mu = 2\mu$
5. 若 $\rho < \frac{1}{4}$ ，则 $\mu = 0.5\mu$
6. 如果 ρ 大于某阈值，认为近似可行。令 $x_{k+1} = x_k + \Delta x_k$
7. 判断算法是否收敛。如不收敛则返回 2，否则结束

其中，近似范围扩大的倍数和阈值都是经验值

在第二步中，增量被限定于一个椭球中，认为只在这个椭球内才是有效的。Levenberg提出将 $D$ 取为单位阵 $I$ ，将增量限制在球内；Marqaurdt 提出将 $D$ 取成非负数对角阵，实际中多取 $J^TJ$

在L-M优化中，需要解带不等式约束的问题，可以用拉格朗日乘子把约束项放入目标函数，构造拉格朗日函数

$L(\Delta x_k,\lambda) = \min_{\Delta x_k}\frac{1}{2}||f(x_k)+J(x_k)\Delta x||^2_2 + \frac{\lambda}{2}(||D\Delta x_k||^2 - \mu)$

等号右边对 $\Delta x$ 求导并令其为0，得到增量线性方程

如果考虑它的简化形式，即 $D = I$ ，相当于求解

当参数 λ 比较小时，H 占主要地位，这说明二次近似模型在该范围内是比较好的，L-M 方法更接近于 G-N 法。另一方面，当 λ 比较大时， $\lambda I$ 占据主要地位，L-M更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。因此L-M法可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题

floatinglong

关注

3
点赞
踩
8

收藏

觉得还不错? 一键收藏
0
评论
视觉SLAM理论入门——（5）非线性优化

即使我们有着高精度的相机，我们得到的数据通常是受各种未知噪声影响的，运动方程和观测方程也只能近似的成立，因此需要研究如何在有噪声的数据中进行准确的状态估计。1、状态估计问题1.1最大后验与最大似然经典 SLAM 模型由一个运动方程和一个观测方程构成其中表示相机位姿，表示传感器输入，表示观测数据，表示路标，和表示噪声，一般认为噪声服从零均值高斯分布~，~运动方程在视觉 SLAM 中没有特殊性，一般讨论观测方程。对于观测方程，希望通过带噪声的数据和，推断位姿和地图（以及它们的概率分布），
复制链接

扫一扫