基于高斯牛顿法和列文-马夸尔特的SLAM非线性优化问题

本文探讨了基于高斯牛顿法和列文-马夸尔特法的SLAM非线性优化问题，介绍了最小二乘问题的基本概念及其数学模型，详细阐述了高斯牛顿法和列文-马夸尔特法的算法步骤及其区别，并提供了使用Ceres库进行曲线拟合的示例代码，展示了如何利用该库进行非线性优化计算。

type
status
date
slug
summary
tags
category
icon
password
摘要：本文对SLAM常见的位姿优化问题进行分析，使用高斯牛顿法和列文-马夸尔特方法进行求解与拟合，最后，使用ceres库实现该优化算法
关键词：SLAM、非线性优化、最小二乘、ceres

概论

对于经典SLAM模型，其位姿可以由变化矩阵来描述，然后用李代数进行优化.观测方程由相机成像模型给出.其中,内参是随相机固定的，而外参则是相机的位姿。然而，由于噪声的存在，运动方程和观测方程的等式必定不是完全一致的。我们得到的数据通常是受各种未知噪声影响的。因此，与其假设数据必须符合方程，不如讨论如何在有噪声的数据中进行准确的状态估计。

理论推导

状态估计问题

经典slam模型可表示为：
注意这里的  为相机的位姿，可以用李群 或李代数 表示。假设在 对路标 进行一次观测，对应到图像的位置为 ，则观测方程为：
式中为像素点的距离。式中 和 皆为齐次坐标.
不妨假设噪声项 和 满足零均值高斯分布：
先把所有的待估计变量放在一个状态变量中：
对机器人状态的估计，转变为求已知输入数据 和观测数据 和条件下，计算状态 的条件概率：
当没有运动测量的传感器，即不存在 时，上述问题转变为Structure from Motion(SfM)问题。此时，利用贝叶斯法则有：
上式中， 为似然， 为先验。此时可用MAP(Maximize a Posterior, MAP)求解：
进一步地，假设机器人无法获得自己的位置，即 没有，则可求解  的最大似然估计(Maximize Likelihood Estimation, MLE):
上式可理解为”在什么样的状态下，最可能产生现在观测到的数据”.

非线性最小二乘-最优化问题求解

最优化是寻找使得目标函数有最大或最小值的的参数向量。根据求导数的方法，可分为2大类。
  1. 若f具有解析函数形式，知道x后求导数速度快。
  1. 使用数值差分来求导数。根据使用模型不同，分为非约束最优化、约束最优化、最小二乘最优化等方法。

总体意义上的最小二乘问题

对一次观测：
并且有假设 ，条件概率为：
为求解该问题，回忆一个任意的高维高斯分布 ，其概率密度函数为：
两边取负对数后变为：
应用上述公式，第一项与x⃗ 无关可移除，代入slam观测模型结果为：
定义数据与误差之间误差为：
则误差平方项之和为：
此式即为总体意义下的最小二乘问题(Least Square Problem).

高斯牛顿法

高斯牛顿法是一种求解非线性最小二乘的简单算法，该算法的基本思想是将函数非线性F(x)进行一阶泰勒展开（此处我们展开的只是函数F(x)而非代价函数）：
此处J(x)为函数的雅克比矩阵，即函数F的一阶导。
根据前面所述我们的目的其实就是求解合适的变量值使得函数的值 达到最小。
为了实现这个目的我们可以依照前面的框架构建一个最小二乘问题，即：
现在我们将通过x求解函数F(x)的最小值问题转换为了通过求解函数最小值的问题，同时目标函数也转换为 。
现在我们将目标函数展开有：
为了求解其极值我们需要对式4.5求导并令其等于零。
由公式4.7我们将能够得到一个关于的线性方程组，我们称这个方程为高斯牛顿方程。如果我们将看做为将看做为，函数也可以写成为：
我们会发现公式4.8与上一篇博客中所讲的最小二乘的正规方程解一模一样，因此公式4.7也被成为正规方程。
同时更进一步，因为高斯牛顿法其实是第二章中下降法的一种，为了明白两者间的关系，我们可以讲增量拆分为下降方向和下降步长，然后我们从公式4.7能够发现，该公式的下降方向满足下降法的约束，即沿着函数梯度的负方向，而步长则等于为1。
明白公式4.7的相关意义后，我们将其进行简化：
此处的H矩阵其实是Hessian矩阵的近似，通过雅克比矩阵去避免掉真是的Hessian矩阵的计算。
基于以上内容我们能够得到高斯牛顿算法的执行步骤，参考[2]：
  1. 给定初始值 \(x_0\)；
  1. 对于某次迭代 \(k\)，求解雅克比矩阵 \(J(x_k)\) 和误差 \(F(x_k)\)；
  1. 求解增量方程 4.9；
  1. 如果 4.9 的解 \(\Delta x_k\) 足够小则停止；否则令 \(x_{k+1} = x_k + \Delta x_k\)，返回第二步。

列文伯格-马夸尔特法

LM算法大致与高斯牛顿算法理论相同，但是不同于高斯牛顿算法，LM算法是基于信赖区域理论(Trust Region Method)进行计算的。这是因为高斯牛顿法中的泰勒展开只有在展开点附近才会有比较好的效果，因此为了确保近似的准确性我们需要设定一个具有一定半径的区域作为信赖区域（和第三节中的信赖区域理论道理相同）。
假设我们有一个模型L，在当前迭代点x附近，该模型可以被近似为函数F形式的二阶泰勒展开式，则我们有：
此处我们用h替代变量，即当前迭代中需要在初始x上增加的量。
但是这个近似假设的成立是有一定程度限制的，我们可以设立一个正值变量△使得模型在一个以x为圆心为半径的圆中被视为可以精确近似。这个圆就是我们所说的信任区域。然后我们就可以基于以下公式求解变化值h：
该公式表示，在信任区域半径限制内，求解出最优的变化值h是的模型L的值最小。
但是在计算过程中，可信任区域半径值是会不停改变的，其改变的依据为模型的近似质量，模型的近似质量又可以通过增益比例（gain ration）进行判定。
首先我们定义增益比例的概念。增益比例为实际函数改变量和模型该变量之间的差异，数学表达式为：
当我们得到增益比例后我们就可以通过它去实时控制可信任区域半径以及迭代的步长：
如果增益比例过小（例如小于0.25），说明实际改变的量远远小于近似模型的值，此时近似情况并不理想，因此我们需要缩小近似范围。反之如果比例较大，说明实际下降的比预期的更大，近似情况比较好，我们可以适当放大一下近似范围。
以上就是信任区域法的简单介绍，采用信赖区域法我们就需要明确该区域该怎么确定。在LM算法中信赖区大小的确定也是运用增益比例来进行判定的(参考公式3.3)：
基于信赖区域我们能够重新构建一个更有效的优化框架。
  1. 给定初始值 \(x_0\)，以及优化半径 \(u\)；
  1. 对于某次迭代 \(k\)：
      2. 为信赖区域半径， 为单位矩阵；
  1. 计算增益比值 ；
  1. 如果 ，则 ；
  1. 如果 ，则 ；
  1. 如果 大于阈值，则近似可行，令 ；
  1. 判断是否收敛，不收敛返回 2，否则结束。
我们引入拉格朗日乘子将该有约束优化问题转化为无约束优化问题后我们能够得到：
因为，又有：
公式4.13就是LM算法的增量表达式。通过该式计算就可以避免掉高斯牛顿算法中出现的不收敛等问题。

实践: ceres

常用的基于C++的优化库有来自谷歌的ceres库以及基于图优化的g2o库。我们在这里主要介绍ceres库。

3.1. ceres简介

ceres是同g2o一样，运用在slam前端或后端，对相机位姿和路标点进行非线性寻优的算法，特别适用于解决后端Bundle Adjustment问题。相比较g2o优势在于其雅可比微分不需要自己推导（当然也可以自己制定，官方推荐使用自动微分），通过链式偏微分自行计算。同时在构造ceres solver时无需像g2o定义vertex 和 edge，ceres只需要构造好cost function，对解算器进行相关设置即可。

3.2. 代码环境

本工程使用ubuntu 18.04环境，并使用opencv4版本的环境。Ceres环境地址可从github中编译安装ceres-solver库。
首先通过github下载opencv4的库文件，将源码通过cmake编译安装至/usr/local/lib目录中，之后编译安装ceres库。
Ceres是一个cmake工程，先通过ubuntu自带的apt安装工具安装它的依赖项，之后进入其中使用cmake编译安装。安装完成后，即可在/usr/local/include/ceres目录下找到ceres的头文件。并在/usr/local/lib下找到名为libceres.a的库文件。有了这些文件，就可以使用ceres进行优化计算了。

3.3. 使用ceres拟合曲线

使用方法：在工程文件夹中，新建一个build文件夹并进入其中。之后使用cmake..通过CMakeLists.txt编译脚本构建makefile文件。然后使用make命令，使用gcc/clang编译器根据makefile文件中的指令编译生成可执行文件。最后直接运行该文件即可得到想要的效果。
Loading...