slam::优化

asdiiiqqq

已于 2023-06-09 15:25:45 修改

阅读量173

点赞数

文章标签：算法机器学习笔记

于 2023-06-09 15:23:06 首次发布

本文链接：https://blog.csdn.net/asdiiiqqq/article/details/130984344

版权

滤波只对当前最佳状态进行估计，优化是根据过去一段时间的所有的观测求出过去一段时间的最大后验估计

核函数可以去除outlier

imu预积分就是把很多数据搞到一个一个观测上

1.状态估计问题与最大似然估计的引出

在SLAM中，我们可以通过滤波来单步进行优化状态估计的结果(马尔科夫思想)，但需要知道的是，即使我们的传感器精度再高，运动方程与观测方程也只是近似成立。所以，与其假设数据符合目标方程，不如直接讨论如何在有噪声的数据中进行准确的状态估计。首先想起我们最基本的状态方程和观测方程。

令状态变量为 $X={{x_{1},x_{2},x_{3},...}}$ ,观测变量为 $Z=\left \{ z_{1},z_{2},z_{3},... \right \}$ 。

那么状态估计问题等同于求解条件概率P(X|Z),即在当前观测状态Z下的状态X的分布。由贝叶斯概率可以知道： $P(X|Z)=\frac{P(Z|X)P(X)}{P(Z)}$ 。直接求取后验分布是困难的，但是求一个状态最优估计，使得该状态下的后验概率最大(Maximize A Posterior,MAP),是可以的：

$X_{MAP}^{*}=argmaxP(X|Z)=argmaxP(Z|X)P(X)$

注意：贝叶斯概率的分母部分与待估计的状态X无关，因此可以忽略。

而求解最大后验概率相当于最大化似然和先验的乘积，假设我们不知道机器人的初始位置在什么地方，即没有先验，那么对目标的求解就变成了求X的最大似然估计（Maximize likelihood Estimation，MLE）

$X_{MLE}^{*}=argmaxP(Z|X)$

直观一点讲，似然是指“现在的位姿下，可能会产生怎样的观测数据”。由于我们能够获得观测数据，所以最大似然估计可以理解为：“在什么样的状态下，最可能产生现在观测到的数据”。

2.线性估计最小二乘法的引出

最小二乘法的目标是：求解误差的最小平方和。根据Cost Function的对应，分为了线性与非线性。这个取决于对应的残差（Residual）是线性还是非线性的。

先从模型角度入手，我们假设某次观测为：

$z_{k,j}=h\left \{ h(y_{j},x_{k}) \right \}+v_{k,j}$

而系统噪声都符合高斯分布，假设噪声项 $v_{k}\sim N(0,Q_{k,j})$ ,所以观测数据的条件概率为：

$P(z_{j,k}|x_{k},y_{i})=N(h(y_{j},x_{k}),Q_{k,j})$

这样的一个概率分布依旧符合高斯分布，按照高数和概率的知识，对于该式的最大似然估计，可以使用最小化负对数来求取它的似然。考虑任意高维高斯分布 $x\sim N(u,\Sigma )$ ,它的概率密度展开式为： $P(x)=\frac{1}{\sqrt{(2\Pi )^Ndet(\Sigma )}}exp(-\frac{1}{2}(x-u)^{T}\Sigma ^{-1}(x-u))$

取负对数，则上式变为：

$-ln(P(x))=\frac{1}{2}ln((2\Pi )^{N}det(\Sigma ))+\frac{1}{2}(x-u)^{T}\Sigma ^{-1}(x-u)$

因为对数函数式单调递增的，所以对原函数最大化相当于求取了负对数最小化，而最小化该式时，第一项与x无关，相当于只最小化右侧的二次型项就可以，就得到对状态的最大似然估计。带入SLAM观测模型，相当于：

$(x_{k},y_{i})^{*}=argmaxN(h(y_{j},x_{k}),Q_{k,j})$

$=argmin((z_{k,j}-h(x_{k},y_{j}))^{T}Q_{k,j}^{-1}(z_{k,j}-h(x_{k},y_{i})))$

上述式子中的二次型称为马哈拉诺比斯距离(Mahalanobis distance),也称为马氏距离。可以认为是由 $Q^{_{k,j}^{-1}}$ 加权后的欧氏距离，而 $Q^{_{k,j}^{-1}}$ 称为信息矩阵，即高斯分布的协方差矩阵的逆。

当考虑到批量数据输入时，假设各个时刻输入和观测相互独立，联合分布进行因式分解，有：

$P(z,u|x,y)=\Pi _{k}P(v_{k}|x_{k-1})\Pi _{k,j}P\left ( z_{k,j}|x_{k},y_{j} \right )$

这样我们就可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

$e_{u,k}=x_{k}-f(x_{k-1},v_{k})$

$e_{z,j,k}=z_{k,j}-h(x_{k},y_{j})$

那么，最小化所有时刻的估计值与真实读数之间的马氏距离，等价于求解最大似然估计。而负对数允许我们把乘积变为求和：

$minJ(x,y)=\sum _{k}e_{u,k}^{T}R_{k}^{-1}e_{u,k}+\sum _{k}\sum _{j}e_{z,k,j}^{T}e_{z,k,j}$

这样便获得了模型上的最小二乘问题。直观上看，由于噪声的存在，我们把估计的轨迹与地图带入SLAM的运动，观测方程中，他们并不会完美成立，因此需要对状态估计值进行微调，使得误差下降，而如何使这个误差下降到最小，就是一个非线性优化过程考虑的问题。

从SLAM计算角度入手，通常上述概率的核心，是对应到了矩阵方程Ax=b的求解，A是一个矩阵，b是一个列向量，根据学过的线代知识，不难得出它的解是 $x=(A^{T}A)^{-1}A^Tb$

实质上该矩阵方程就是求 $\left \| Ax-b \right \|^{2}$ 的最小值，所以用该式子对x进行求导，便可以得到极小值。由于Ax-b是一个列向量，所以有： $\left \| Ax-b \right \|^{2}=(Ax-b)^{T}(Ax-b)$ .将右侧式子展开（转置符代入并结合）得：

$\left \| Ax-b \right \|^{2}=(x^{T}A^{T}-b^{T})(Ax-b)=x^{T}A^{T}Ax-x^{T}A^{T}b-b^{T}Ax+b^{T}b$

上式中，因为 $x^{T}A^{T}b$ 是一维度的标量，所以其转置就是其本身。（ $x^{T}$ 是一个1*n的行向量， $A^{T}$ 是一个n*n的矩阵），即 $x^{T}A^{T}b=b^{T}Ax$ .所以，上式右侧替换为 $x^{T}A^{T}Ax-2x^{T}A^{T}b-b^{T}b$ 。令 $S(x)=b^{T}b-2x^{T}A^{T}b+x^{T}A^{T}Ax$ （这么写是为了求导按项遍历时理解方便）。对x求导，令其为0，得： $x=(A^{T}A)^{-1}A^{T}b$

$\frac{\varphi Ax}{\varphi x}=A^{T}$

$\frac{\varphi Ax}{\varphi x^{T}}=A^{T}$

$\frac{\varphi xA}{\varphi x}=A$

$\frac{\varphi y}{\varphi x}=\frac{\varphi(x^{T}Ax) }{\varphi x}=(A^{T}+A)x$

特别的，当A为对称矩阵时，有 $\frac{\varphi(x^{T}Ax) }{\varphi x}=2Ax$ 。（此时 $x^{T}Ax$ 是一个标量，也就是一个数）

3.非线性最小二乘法

考虑一个简单的最小二乘问题：

$min_{x}F{x}= \frac{1}{2}\left \| f(x) \right \|_{2}^{2}$

其中，自变量 $x\in \\R^{n}$ ，f是一个任意标量的非线性函数f(x): $R^{n}n \to R$ .注意这里的系数0.5是无关紧要，并不会影响结论。对该问题进行讨论：由高中数学得知，求解一个函数的最小值，就是求取函数零点，求导后令其为0，判断各个解的结果大小就可以获得最小值。但假如f函数难以获取精确表达式或不易求解，便需要从导数的原理角度入手求解了。

首先是从关于目标函数的全局性质，我们不一定能够获得，但是我们从某一个初值进行一步步推进，其递增或递减特性在区间是固定的，由此我们不断优化当前的步长和迭代次数，使得目标函数值下降，便可以求得一个较好的结果，这便是第一个非线性优化的求解方法：梯度下降法：

步骤如下：

1.寻找一个初值x0；

2.对于第k次求解(迭代)。确定一个增量 $\Delta x$ ，使得 $\left \| f(x_{k}+\Delta x_{k}) \right \|_{2}^{2}$ 达到极小值。

3.若 $\Delta x$ 足够小，则停止。

4.否则，令x=x+ $\Delta x$ ，继续迭代运算。

这让对函数直接求导取0的方法变成了用使结果下降的增量 $\Delta x$ 不断逼近的方法。由于可以对f函数进行线性化，计算上也会变得相对容易一些。当函数下降到增量足够小的时候，就认为此时的结果收敛，目标达到了极小值。

在这个求解过程中，问题在于如何确定每次迭代的增量值，这是一个局部问题，我们只需要关心f在迭代处的局部性质就可以，这类思想在最优化，机器学习中广泛应用。

考虑在第k次迭代，假设在 $x_{k}$ 处，想要寻求增量 $\Delta x$ ，最直观的方法是将函数在 $x_{k}$ 处进行泰勒展开：

$F(x+\Delta x)\approx F(x_{k})+J(x_{k})^{T}\Delta x_{k}+\frac{1}{2}\Delta x_{k}^{T}H(x_{k})\Delta x$

其中j(xk)是F(x)关于x的一阶导数，也称为梯度，其形式为雅可比(jacobin)矩阵，H(xk)是二阶导数，其形式为海塞（Hessian）矩阵，他们都在xk处取值，具体计算可以参考微积分。我们选择保留泰勒展开的一阶或二阶项，对应的求解方法称为一阶梯度法或二阶梯度法。如果保留一阶梯度或二阶梯度法。如果保留一阶梯度，那么取增量的反向梯度，则可以让原函数结果下降，即：

$\Delta x^{*}=-J(x_{k})$

这种方法被称为最速下降法，它的集合含义也很好理解，我们沿着反向梯度前进，在一阶近似的情况下，目标函数结果必定下降（趋于极值点）

当保留二阶梯度信息时，增量方程扩展为：

$\Delta x^{*}=argmin(F(x)+J(x^{T}\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x))$

而上式的右侧包含 $\Delta x$ 的零次F(x),一次(xt $\Delta x$ )和二次项(0.5V $\Delta x$ TH $\Delta x$ ),对其求导令其等于0，则有：

$J+H\Delta x=0\Rightarrow H\Delta x=-J$

求解这个线性方程，就得到了增量，这种方法称为牛顿法。

另：该方程与前述求解Ax=b结构相同，求解方法可以自行带入。

由此，我们可以获悉：非线性最小二乘的优化思想，是一种按照导数的定义对目标函数局部性质进行探索，采用步长迭代寻求极小值点，直到收敛的方法。其中在泰勒展开一阶截断求取雅克比矩阵的方法称为一阶梯度法，又称为最速下降法，展开式二阶截断的计算雅可比与海塞矩阵关系的方法称为二阶梯度法，又称为牛顿法。

高斯-牛顿法

高斯牛顿法是优化方法里最具代表的一种，也是比较简单的方法。它的核心思想是把f(x)进行一阶泰勒展开。需要注意：这里展开的不是目标函数F(x)而是f(x).

$f(x+\Delta x)\approx f(x)+J(x)^{T}\Delta x$

这里的 $J(x)^{T}$ 是f(x)关于x的导数，为一个nx1的列向量。根据前面的概念，当前的目标是寻求最小的 $\Delta x$ ，使得 $\left \| f(x+\Delta x) \right \|^{2}$ 达到最小。为了求 $\Delta x$ ，构建一个线性的最小二乘问题：

$\Delta x^{*}=argmin_{\Delta x}\left \| f(x)+(J(x)^{T}\Delta x) \right \|^{2}$

根据极值条件，对上述目标函数对 $\Delta x$ 求导，并令其导数为0。为此，先展开目标函数的平方项：

$\frac{1}{2}\left \| f(x)+(J(x)^{T}\Delta x) \right \|^{2}=\frac{1}{2}(f(x)+(J(x)^{T}\Delta x)^{T}(f(x)+(J(x)^{T}\Delta x)) =\frac{1}{2}(\left \| f(x) \right \|_{2}^{2}+2f(x)J(x)^{T}\Delta x+\Delta x^{T}J(x)J(x)^{T}\Delta x)$

上式关于 $\Delta x$ 求导，并令其为0，有：

$J(x)f(x)+J(x)J^{T}(x)\Delta x=0$

可以得到如下方程组：

$J(x)J^{T}(x)\Delta x=-J(x)f(x)$

这个方程式关于变量 $\Delta x$ 的线性方程组，我们称它为增量方程，也称为高斯牛顿方程（Gauss-Newton equation）或者正规方程（Normal euqation）。将方程左侧的J(x)JT(x)系数定义为H(x),右侧-J(X)f(x)定义为g(x),那么上式变为：

$H\Delta x=g$

对比牛顿法可以得出：高斯牛顿法用JJT的形式最为了牛顿法中二阶（Hessian）矩阵的近似。从而省略了计算H的过程。而求解增量方程是整个优化问题的核心所在，如果我们能顺利接触该方程，那么高斯牛顿法的算法不走可以写成：

1.寻找一个初值x0；

2.对于第k次迭代，求出当前的雅可比矩阵J(xk)以及误差f(xk);

3.求解增量方程H $\Delta x$ =g;

4.若 $\Delta x$ 足够小则停止，否则，令x=x+ $\Delta x$ ，继续迭代运算；

为了求解增量方程，我们还需要求解H-1，这需要H矩阵可逆，但实际数据中计算的JJT却只有半正定性。也就是说，在使用高斯牛顿法的时候，可能会出现JJT为奇异矩阵的情况，此时增量的稳定性变差，导致无法收敛。直观的说，就是原函数在局部看起来并不像一个二次函数，导致其斜率(导数）不稳定或不存在，况且假设此时计算出了H矩阵，但得到的步长 $\Delta x$ 比较大导致结果不够准确，也会出现发散的可能。

虽然高斯牛顿法有上述的缺点，但其依然是非线性优化方面较为好理解与实现的方法，值得我们学习与改进并加入slam的应用中。

列文伯格-马夸尔特方法（Levenberg-Marquart，LM）

高斯牛顿法对f（x）进行二阶泰勒展开，只能在步长 $\Delta x$ 附近有较好的结果，如果给 $\Delta x$ 添加一个范围，应该可以弥补一部分高斯牛顿法的确定。那么LM方法就是给步长添加一个信赖区域（Trust Region），而这个信赖区域定义了在不同情况下二阶阶段的函数时近似有效的，这种方法时信赖区域方法（Trust Region Method）。在信赖区域里，可以认为方法的近似于结果都是有效的。而出了这个区域，则需要其他方法来弥补。

定义信赖区域的一种比较好的办法，就是用近似模型与实际函数之间的差异来确定：如果差异越小，则说明近似效果好，我们迭代就可扩大近似的范围，而当差异很大，则需要缩小近似范围。由此，引入一个指标 $\rho$ 来刻画近似好坏的程度。

$\rho =\frac{f(x+\Delta x)-f(x)}{J(x)^{T}\Delta x}$

$\rho$ 的分子是实际函数的下降值，而分母是近似模型的下降值。如果 $\rho$ 接近于1，则认为近似是好的；如果 $\rho$ 太小，说明实际减小的值远小于近似减小的值，则此时近似会比较差，需要缩小近似范围；反之，如果 $\rho$ 比较大，则说明实际下降的比预计的大，我们应该放大近似范围来加快收敛速度。

于是，我们构建这样一个改良的非线性优化框架，该框架回避高斯牛顿法有更好的效果，给出算法步骤：

1.寻找一个初值x0，以及一个初始的优化半径 $\mu$ ；

2.对于第k次迭代，在高斯牛顿法的基础上增加信赖区域，求解式子变为：

$min_{\Delta x}\frac{1}{2}\left \| f(x_{k}+J(x_{k})^{T}\Delta x_{k})\right \|^{2}\leq \mu$ ,其中 $\mu$ 是信赖区域的半径，D 是系数矩阵(几何理解是这个矩阵将空间中原本的一个半径确定的球体信赖区域转换成一个椭球体，可以根据需求来寻求空间中的有效部分。而在一般的使用中，这个系数矩阵都为单位矩阵I)

3.计算 $\rho$ 值。

4.若 $\rho$ >3/4,则设置 $\mu$ =2 $\mu$ ；(3/4是一个经验值，这个值可以在代码中进行调试来确保不同场景下的应用情况)

5.若 $\rho$ <1/4,则设置 $\mu$ =1/2 $\mu$ ;

6.若 $\rho$ 大于某阈值，则近似认为可行。令 $x_{k+1}=x_{k}+\Delta x_{k}$ ;

7.判断结果是否收敛，如果不收敛则返回第2步继续，否则结束。

附：如果将D矩阵为非负数对角阵———实际中通常用JJT的对角元素平方根，使得在梯度小的维度上约束范围更大一点（马夸尔特提出）。

无论如何，在列文伯格-马夸尔特优化中，我们都需要求解 $min_{\Delta x_k}\frac{1}{2}\left \| f(x_{k}+J(x_{k})^{T}\Delta x_{k}) \right \|^{2},s.t.\left \| D\Delta x_{k} \right \|^{2}\leq \mu$ 式子这样一个子问题来获得梯度。这个子问题是带不等式约束的优化问题，我们用拉格朗日乘子把约束项放到目标函数中，构建成拉格朗日函数：

$\zeta (\Delta x_{k},\lambda )=\frac{1}{2}\left \| f(x_{k}+J(x_{k})^{T}\Delta x_{k}) \right \|^{2}+\frac{\lambda }{2}(\left \| D\Delta x_{k} \right \|-\mu )$

这里 $\lambda$ 为拉格朗日乘子。类似于高斯牛顿中的做法，令该拉格朗日函数关于 $\Delta x$ 的导数为0，它的核心仍是计算增量的线性方程：

$\left ( H+\lambda D^{T}D \right )\Delta x_{k}=g$

相比高斯牛顿法，增量方程多了一项 $\lambda D^{T}D$ ,如果考虑它的简化形式，即D=I，那么相当于求解： $\left ( H+\lambda I \right )\Delta x_{k}=g$

理解：当参数 $\lambda$ 比较小的时候，H占据主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格-马夸尔特方法更接近高斯牛顿法。另一方面，当 $\lambda$ 比较大时， $\lambda$ I占据主要地位，列文伯格-马夸尔特方法更接近于高斯牛顿法。另一方面，当 $\lambda$ 比较大时， $\lambda$ I占据主要地位，列文伯格-马夸尔特方法更接近于一阶梯度下降法（最速下降法），这说明附近的二次近似不够好，需要快速寻求收敛域。这样的求解方式，可以在一定程度上避免线性方程组的系数矩阵与非奇异的病态问题，提供更稳定，更准确的增量 $\Delta x$ 。

实际SLAM应用中，还有许多方式求解增量，本篇讲解的时较为广泛使用的基本方法。通常选择高斯牛顿或者LM方法，当问题性质比较好，就用高斯牛顿法，如果问题出现的奇异点比较多或问题存在病态情况，用LM方法比较好。

Ceres的学习：

Ceres求解的最小二乘问题一般的形式如下（带边界的核函数最小二乘）

$min_{x}\frac{1}{2}\sum_{i}^{}\rho _{i}(\left \| f_{i}(x_{i1},...,x_{in}) \right \|^{2})s.t.(l_{j}\leq x_{j}\leq \mu _{j})$ 在这个问题上，x1，...,xn为优化变量，又称为参数块，fi称为代价函数（cost function），也称为残差块（Residual blocks），在slam中也可以理解为误差项。受限条件为优化变量的上下限。在最简单的情况下，可以取正负无穷，即认为不限制优化变量的界限。此时，目标函数由许多平方项经过一个核函数 $\rho (\cdot )$ 之后求和组成。类似的，可以取这个核函数为一个恒等函数，那么这个问题就变成了无约束的最小二乘问题。

内容来源于：https://zhuanlan.zhihu.com/p/421272861

asdiiiqqq

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
slam::优化

其中j(xk)是F(x)关于x的一阶导数，也称为梯度，其形式为雅可比(jacobin)矩阵，H(xk)是二阶导数，其形式为海塞（Hessian）矩阵，他们都在xk处取值，具体计算可以参考微积分。也就是说，在使用高斯牛顿法的时候，可能会出现JJT为奇异矩阵的情况，此时增量的稳定性变差，导致无法收敛。直观上看，由于噪声的存在，我们把估计的轨迹与地图带入SLAM的运动，观测方程中，他们并不会完美成立，因此需要对状态估计值进行微调，使得误差下降，而如何使这个误差下降到最小，就是一个非线性优化过程考虑的问题。
复制链接

扫一扫

slam::优化

“相关推荐”对你有帮助么？