机器人笔记

第6讲 非线形优化

《SLAM十四讲》高翔

Posted by ZhouSh on January 10, 2023

第6讲　非线形优化

6.1 状态估计问题

经典SLAM模型由运动方程和状态方程构成： $ x_k=f(x_{k-1},u_k)+w_k \tag{1} $

$ z_{k,j}=h(y_j,x_k)+v_{k,j} \tag{2} $

在非线性优化中，把所有待估计的变量放在一个“状态变量”中：$x={x_1,\cdots,x_N,y_1,\cdots,y_M}$

为了估计状态变量的条件分布，利用贝叶斯法则，有：

$ P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x) \tag{3} $

贝叶斯法则的左侧 $P(x|z)$ 通常被称为后验概率，右侧 $P(z|x)$ 称为似然，$P(x)$ 称为先验。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化（Maximize a Posterior，MAP），则是可行的：

$ x^*_{MAP}=arg\ max\ P(x|z)=arg\ max\ P(z|x)P(x) \tag{4} $

贝叶斯法则的分母与代估计的状态x无关，因而可以忽略。贝叶斯法则告诉我们，求解最大后验概率相当于最大化似然和先验的乘积。当不知道机器人位姿大概在哪时，就没有了先验。那么，可以求解x的最大似然估计（Maximize Likelihood Eistimation，MLE）：

$ x^*_{MLE}=arg\ max\ P(z|x) \tag{5} $

直观讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”。最大似然估计可以理解成“在什么样的状态下，最可能产生现在观测到的数据”。

高维高斯分布的概率密度函数为：

$ P(x)=\frac 1{\sqrt{(2\pi)^Ndet(\Sigma)}}exp(-\frac12(x-\mu^T\Sigma^{-1}(x-\mu))) \tag{6} $

一般用最小化负对数来求一个高斯分布的最大似然，对其取负对数，得：

$ -ln(P(x))=\frac12ln((2\pi)^Ndet(\Sigma))+\frac12(x-\mu)^T\Sigma^{-1}(x-\mu) \tag{7} $

第一项与x无关，舍去，只要最小化右侧的二次型项$(x-\mu)^T\Sigma^{-1}(x-\mu)$，就得到对状态的最大似然估计。

代入SLAM的观测模型，得： \(\begin{eqnarray} x^*=arg\ min((z_{k,j}-h(x_k,y_j))^TQ^{-1}_{k,j}(z_{k,j}-h(x_k,y_j))) \tag{8}\end{eqnarray}\)

代入SLAM的观运动模型，得： \(\begin{eqnarray} x^*=arg\ min((x_k-f(x_{k-1},u_k))^TR^{-1}_k(x_k-f(x_{k-1},u_k))) \tag{9}\end{eqnarray}\)

令误差为

$ e_{v,k}=x_k-f(x_{k-1},u_k) \tag{10} $

$ e_{y,j,k}=z_{k,j}-h(x_k,y_j) \tag{11} $

那么整个SLAM的误差平方和为 \(\begin{eqnarray} J(x)=\sum_ke^T_{v,k}R^{-1}_ke_{v,k}+\sum_k\sum_je^T_{y,k,j}Q^{-1}_{k,j}e_{y,k,j} \tag{12}\end{eqnarray}\)

6.2 非线性最小二乘

对于不方便直接求解的最小二乘问题，可以用迭代的方式，步骤如下：

  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$，返回第二步

确定增量$\Delta x_k$常用的方式有2种：高斯牛顿法和列文伯格-马夸尔特法

6.2.1 一阶梯度法(最速下降法) 和 二阶梯度法(牛顿法)

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

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

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

我们可以选择保留泰勒展开的一阶或二阶项，对应的求解方法则为一阶梯度法或二阶梯度法。

若保留一阶梯度，则增量的解为（14）。他的直观意义为沿着反向梯度方向前进即可。通常还会计算该方向的步长$\lambda$求得最快的下降方式。这种方法被称为最速下降法。

$ \Delta x^*=-J^T(x) \tag{14} $

若保留二阶梯度，则增量方程为（15）。求右侧关于$\Delta x$的导数并令它为0，得增量的解（16）。该方法又称为牛顿法。

$ \Delta x^*=arg \ min ||f(x)||^2_2+J(x)\Delta x+\frac12\Delta x^TH\Delta x \tag{15} $

$ H\Delta x=-J^T \tag{16} $

但是最速下降法过于贪心，容易走出锯齿路线，反而增加迭代次数。牛顿法则需要计算目标函数的H矩阵，在问题规模较大时非常困难。所以后面两种方法更为实用。

6.2.2 高斯牛顿法

将$f(x)$进行一阶泰勒展开（注意不是目标函数$f(x)^2$)：

$ f(x+\Delta x)\approx f(x)+J(x)\Delta x \tag{17} $

其中$J(x)$为$f(x)$关于$x$的导数，实际上是一个$m\times n$矩阵，也是一个雅可比矩阵

当前目标是寻找下降矢量$\Delta x$，使得$||f(x+\Delta x)||^2$最小。为了求$\Delta x$，我们需要解一个线形的最小二乘问题：

$ \Delta x^*=arg\ min_{\Delta x}\frac12||f(x)+J(x)\Delta x||^2 \tag{18} $

注意与之前不同，是对$\Delta x$求导，而不是对$x$求导。

展开平方项：

$ \frac12||f(x)+J(x)\Delta x||^2=\frac12(||f(x)||^2_2+2f(x)^TJ(x)\Delta x+\Delta x^TJ(x)^TJ(x)\Delta x) \tag{19} $

求上式关于$\Delta x$的导数，并令其为零（20），可得（21）。

$ 2J(x)^Tf(x)+2J(x)^TJ(x)\Delta x=0 \tag{20} $

$ J(x)^TJ(x)\Delta x=-J(x)^Tf(x) \tag{21} $

注意我们要求解的变量为$\Delta x$，因此这是一个线性方程组，我们称它为增量方程或高斯牛顿方程或正规方程。

我们把左边的系数定义为H，右边定义为g，那么上式变为：

$ H\Delta x=g \tag{22} $

这里把左侧记作H是有意义的。对比牛顿法可见，高斯牛顿法用$J^TJ$作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算H的过程。

求解增量方程是整个优化问题的核心所在。如果顺利解出该方程，那么高斯牛顿法的算法步骤可以写成：

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

6.2.3 列文伯格-马夸尔特法

由于高地牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给$\Delta x$添加一个信赖区域，不能让它太大而是的近似不准确。在信赖区域里边，我们认为近似有效。

可以根据我们的近似模型跟实际模型的差异来确定信赖区域的范围。如果差异小，就让范围尽可能大，因此考虑使用（23）来判断泰勒近似是否够好。

$ \rho=\frac{f(x)+\Delta x-f(x)}{J(x)\Delta x} \tag{23} $

$\rho$的分子是实际函数下降的值，分母是近似模型下降的值。如果$\rho$接近于1，则近似是好的；如果$\rho$太小，说明实际减小的值远少于近似减小的值，需要缩小近似范围；如果$\rho$太大，说明实际下降的比预计更大，可以放大近似范围。

于是，列文伯格-马夸尔特法的算法步骤为：

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

在（24）中，我们把增量限定于一个半径为$\mu$的球中，认为只在这个球内才是有效的。带上D后，这个球可以看成一个椭球。D取单位矩阵I时为球。

将（24）这个带不等式约束的优化问题，用拉格朗日乘子转化为无约束优化问题： $ min_{\Delta x_k}\frac12||f(x_k)+J(x_k)\Delta x_k||^2+\frac \mu 2||D\Delta x_k||^2 \tag{25} $

其中$\lambda$为拉格朗日乘子

类似高斯牛顿法中的做法，把它展开后，我们发现该问题的核心仍是计算增量的线性方程（26）。D取单位矩阵I，则相当于求解（27）

$ (H+\lambda D^TD)\Delta x=g \tag{26} $

$ (H+\lambda I)\Delta x=g \tag{27} $

当参数$\lambda$较小时，H占主要地位，说明二次近似模型在该范围内较好，此时列文伯格-马夸尔特法接近于高斯牛顿法。当$\lambda$较大时，$\lambda I$占主要地位，说明附近的二次近似不够好，此时接近于最速下降法。

参考

CSDN：SLAM最小二乘的引出

CATALOG
    ARCHIVE
    论文笔记 机器人笔记 读书笔记 值得一记
    RECOMMEND