非线性优化同时定位与地图创建问题

季晨　宋燕燕　秦军　敖甜甜

摘  要：由于SLAM（Simultaneous Localization and Mapping）算法能够在陌生环境中进行高精准度的实时定位以及对当前环境进行重建地图的特点，SLAM技术逐渐成为当前研究热点。本文主要研究基于图优化的同时定位与地图创建，即SLAM创建中非线性图优化的算法。在基于图优化的SALM问题中，最主要的就是解决非线性最小二乘问题。本文对非线性最小二乘问题的算法和常见的非线性优化方案进行阐述与分析，分析最速下降法、高斯—牛顿法、列文伯格—马夸尔特法的原理和步骤，总结比较三种方法的特征和缺点，在SLAM框架中选择最适合的优化算法。

关键词：图优化;非线性最小二乘;SLAM

中图分类号：TP391.41     文献标识码：A

Research on Simultaneous Localization and Mapping based on

Nonlinear Optimization Algorithm

JI Chen， SONG Yanyan， QIN Jun， AO Tiantian

（Communication University of China， Nanjing， Nanjing 211172， China）

592619059@qq.com; sophiesong1231@163.com; 1059182465@qq.com; 986952863@qq.com

Abstract： This paper studies nonlinear graph optimization algorithm in Simultaneous Localization and Mapping （SLAM）. In the SLAM problem， the major issue is to solve the nonlinear least squares problem. In this paper， algorithm of nonlinear least squares problem and common nonlinear optimization schemes are analyzed， such as the principles and steps of the steepest descent method， Gauss Newton method and Levenberg-Marquardt method. Characteristics and shortcomings of the three methods are compared with each other， and the most suitable optimization algorithm in the SLAM framework is selected.

Keywords： graph optimization; nonlinear least squares; SLAM

1   引言（Introduction）

由于SLAM（Simultaneous Localization and Mapping）算法能够在陌生环境中进行高精准度的同时定位以及对当前环境进行重建地图的特点，SLAM技术逐渐成为当前研究热点。对当下研究热点SLAM技术中的图优化算法进行研究[1-3]， SLAM模型由运动模型和观测模型组成，最大似然估计模型作为SLAM模型的“虚拟观测模型”，可以近似于最小二乘问题[4]。由于噪声的存在，运动方程和观测方程会对测量结果有偏差，需要在有噪声的数据中进行准确的状态估计，从噪声数据中恢复出有用信息，优化处理噪声数据，深入到图优化。图优化技术最重要的就是解决最小二乘问题。鉴于此，本文通过最速下降法、高斯—牛顿法（Gauss-Newton）和列文伯格—马夸尔特方法（Levenberg-Marquardt，LM）的推导原理，可知迭代法是解决最小二乘问题的核心，运用不同的迭代思路，加快求解的速度、保证收敛性是图优化的方向[5，6]。

2   SLAM算法原理（Principle of SLAM algorithm）

SLAM模型由两个部分组成：运动模型和观测模型。由于实际操作中往往存在观测噪声，我们通过观测信息所得到的数据通常不具备一致性，并且利用运动方程和观测方程进行的计算对测量结果存在差异。

2.1   运动模型和观测模型

运动模型用于描述相机的运动状态，影响SLAM模型中相机位置姿态的准确性。通常用运动方程计算得出。运动模型方程可简化为式（1）：

（1）

式中，含义为t时刻时，ut为相机的位移，xt和xt-1指这一时刻和上一时刻相机的位姿，wt是噪声[5]。

观测模型用于描述相机对待测环境的感知状态，影响SLAM模型中重建地图的精准度。由于观测模型受诸多方面的影响，其中观测传感器的类型和传感器噪声导致随机误差较多，该误差一般符合正态分布。对于普通传感器，通用的观测模型方程为式（2）：

（2）

式中，zt，j表示t时刻观测传感器返回的数据，yj为环境地图中观测到的第j个路标的位姿，xt为t时刻相机的位姿，vt，j为传感器噪声误差，函数h表示实际情况与传感器数据的关系，由传感器类型与原理决定[5]。

2.2   最大似然估計模型

对于相机位置姿态之间的相关性可以视为一种“虚拟观测”[4]。我们可以根据这一种观测得到的一系列数据对相机的状态（包括位置姿态的连续数据）作最大似然估计（Maximize Likelihood Estimation，MLE）：是一种使用概率模型求估计量的方法，最终目的是找到以较高概率产生观测数据的系统发生树。我们可以利用这一原理，首先找到相机在当前位置姿态下会产生的观测数据，利用所知的观测数据和最大似然估计可以得到“相机在什么样的条件下，最有可能产生当前得到的观测数据”。

用P={Pi}表示相机的位置姿态集合，用T={}表示相机位置姿态之间的相对变化集合，则在设立一定的观测信息T条件下，对P作最大似然估计可表示为

（3）

=//独立性假设             （4）

=//马尔可夫性           （5）

=//符号替换             （6）

其中，E表示相机位置姿态改变时，相邻位置姿态相对变化的概率分布的集合。式（4）使用了条件独立假设，即假定在所设置的一定的相机姿态信息的条件下左右的观测之间都是相互独立。式（5）应用了马尔可夫性，PR（m）组成了Tm的马尔可夫邻域[4]。由于每个约束Tij（即每个相邻位置姿态相对变化的概率分布）只与两个位置姿态节点Pi和Pj相关，简化后，可以得到式（6）。

3  非线性最小二乘问题（Nonlinear least squares problem）

SLAM中的图优化问题我们可以看成是非线性最小二乘问题。最小二乘问题求解非线性优化的主要思想为：不通过求解问题全局的最优解，而是通过求解局部的最优解，再利用不断迭代去接近全局找到最优解作为求解结果。

若假定相机的位置姿态满足Pi和Pj的条件，此时Tij服从均值为-Pi（符号表示位姿复合的逆操作）、方差为Σij的高斯分布，即Tij～N（Pj-Pi，Σij），再对P作最大似然估计，可以得到等价于非线性最小二乘问题：

（7）

（8）

由此可以看出，要想解决非线性最小二乘问题，首先要将非线性最小二乘问题转换成线性最小二乘问题，再通过不断迭代求出最优解[4]。

假设对F（x）=这个函数求解，过程如下。

设置初始迭代点：xk，k=1;

第一步 线性化：把fi（x）在xk处用泰勒展开，高阶项忽略不计，函数可以写成为φi（x），此时φi（x）就是fi（x）在x=xk处的线性相关函数，可以近似代表fi（x）。

第二步 求导：我们已经把函数F（x）里面的所有非线性函数项都转化成了线性项，就可以进一步转换成线性最小二乘问题，设定ψ（x）=，再用ψ（x）近似F（x）。

此时我们就可以对ψ（x）求导，即令ψ'（x）=0，求解x，得到下一次的最优点x=xk+1，再用所求的新的最优点xk+1为基础，把F（x）在xk+1处泰勒展开，循环执行以上两个步骤进行迭代。

迭代停止条件：迭代停止的条件有很多种，例如当xk+1-x<阈值满足时，就可认为xk+1使F（x）接近极小值，迭代停止。其中阈值决定了x估计值的精确度以及最优化过程的迭代次数。

4   非线性优化方案（Nonlinear optimization scheme）

非线性优化最常见的方案是有最速下降法、高斯—牛顿法、列文伯格—马夸尔特方法等。在做最优化计算时，需要提供变量的初始值。由于目标函数复杂，对问题提供不同的初始值都容易陷入局部最小值。

4.1   最速下降法

最速下降法：利用选取最速下降梯度作为迭代的方向，局部收敛速度可以达到最快，但是整体收敛速度不一定是最快的，而且极容易陷入局部极小值。此方法的优点在于收敛速度比较稳定。

最速下降法步骤为：

（1）给定初始点x0。

（2）若足够小，停止迭代，否则计算。

（3）由线性搜索计算步长。

（4）令，k：=k+1，继续（2）。

迭代公式：

（为迭代步长，为梯度方向）

因为最速下降法是从局部出发找最优解，其下降速率就只能在局部中体现，从整体收敛速度来看反而是比较慢的。

4.2   高斯—牛顿法

高斯—牛顿法：将目标函数在初始点进行泰勒一阶展开，本质上则是步长为1的梯度下降法，优点在于计算雅各比矩阵代替海森矩阵，减少计算量。但是高斯—牛顿法依赖于展开点的选取。

以如下非线性最小二乘问题作为例子：

（9）

首先将进行一阶的泰勒展开：

（10）

带到式（9）中：

（11）

我们对上式进行求导，可得：

（12）

令H=JTJ，B=-JTf，代入式（12）得到：

（13）

求解式（13）得调整增量，所以可得高斯—牛顿法的步骤为：

（1）给定初始值x0。

（2）对第k次迭代，计算出雅克比矩阵J，矩阵H、B;根据式（13）计算增量。

（3）如果足够小，就停止迭代，否则。

（4）循环（2）（3）两步骤，直到最大循环次数，或者满足迭代停止条件。

从算法中可以看出，增量方程求解占据主要位置。必須满足近似H矩阵是正定且可逆的。当初始值和所求解相差较大时，高斯—牛顿法就无法确保是收敛的了。当出现近似奇异的时候，高斯—牛顿法也不能收敛。即使排除这一情况，在求步长时，若它过大，会使得局部近似存在较大偏差，也无法保证它的收敛性[6]。

4.3   列文伯格—马夸尔特方法

列文伯格—马夸尔特方法：是将最速下降法与高斯—牛顿法相结合的算法，通过设置阻尼参数调节减少步长。LM算法收敛速度快，能够避免算法不收敛，对初值依赖较小。

高斯—牛顿法在展开点附近利用泰勒展开才会有相对较好的近似结果，而LM算法则给添加了一个信赖区域（Trust Region）用于提高准确性。我们可以使用来衡量泰勒近似效果，作为调整信赖区域大小的依据：

上式中的分子表示函数实际的下降值，分母表示近似模型的下降值。若约等于1则近似效果好;若过小，则缩小信赖区域;若过大，则放大信赖区域。

列文伯格—马夸尔特的求解步骤如下：

（1）给定初始值，，v0=2。

（2）求解梯度，如果小于或等于阈值，则退出;否则继续。

（3）通过求解，如果足够小，就停止迭代;否则继续。

（4）令，计算。

（5）如果>0，则，，

vk+1=2;否则，，重复步骤（2）。

（6）判断收敛性，若不收敛，则返回（2）;若收敛，则停止。

与高斯—牛顿法相比较，LM法增加了一项。当较小时，H占主要部分，由此我们可以得知二次近似模型在该区域范围内是可以运用的;当较大时，将成为主要部分，此时就更接近于最速下降法，由此可知此区域附近的二次近似是不能运用的[6，7]。LM法是一种非常高效的迭代算法，该算法的许多变量决定了迭代的效率和結果的收敛性，而这些参数的理想值往往依目标函数不同而不同，LM法要做矩阵求逆，运算量过大，所以我们必须要用其他方法求解数据量过大的问题。列文伯格—马夸尔特法避免了一定程度的线性方程组的非奇异问题和病态问题，提供了更稳定的求解运算，提高了增量的准确性。

4.4   算法分析与总结

根据对三种算法的原理和推导，我们可以对其作出分析和总结，详见表1。

5   结论（Conclusion）

本篇是基于SLAM算法中非线性优化问题展开的核心算法学习。主要介绍了SLAM算法中的运动模型和观测模型以及最大似然估计模型，以及由此引出的非线性最小二乘问题。我们从解决最小二乘问题入手，进一步了解非线性优化的基本算法，通过对最速下降法、高斯—牛顿法及列文伯格—马夸尔特方法的学习，总结分析了三种方法的思路和不足之处。最速下降法由于步长难以确定且线性搜索法的计算量过大，在SLAM技术中并不能起到很好的效果。高斯—牛顿法对H矩阵要求为正定的，其他情况都会造成偏差甚至不收敛，需要根据情况适用。列文伯格—马夸尔特方法增加了一个信赖区域，虽然下降速度会有所降低，但是可以通过计算在最速下降法和高斯—牛顿法之间择优选择，是解决最小二乘问题最常用的方法。

参考文献（References）

[1] Liu L.， Wang Y.， Zhao L.， et al. Evaluation of Different SLAM Algorithms using Google Tangle Data[M]. Proceedings of the 2017 12th Ieee Conference on Industrial Electronics and Applications， 2017： 1954-1959.

[2] Mur-Artal Raul， Tardos Juan D. Visual-Inertial Monocular SLAM With Map Reuse[J].IEEE ROBOTICS AND AUTOMATION LETTERS， 2017， 2（2）： 769-803.

[3] Saeedi Sajad，Trentini Michael， Seto， et al. Multiple-Robot Simultaneous Localization and Mapping： A Review[J]. JOURNAL OF FIELD ROBOTICS， 2016， 33（1）： 3-46.

[4] 梁明杰，闵华清，罗荣华.基于图优化的同时定位与地图创建综述[J].机器人，2013，35（04）：500-512.

[5] 翁潇文.基于图优化的移动机器人SLAM算法研究[D].华南理工大学，2019.

[6] 张丽丽.最小二乘问题的算法与应用研究[D].华北电力大学，2017.

[7] 徐子锋，石超，王永锋，等.基于ORB+PROSAC误匹配剔除算法的视觉SLAM研究[J].软件工程，2019，22（5）：9-14.

作者简介：

季   晨（1999-），女，本科生.研究领域：数字媒体技术.

宋燕燕（1978-），女，硕士，副教授.研究领域：虚拟现实技术.

秦   军（1956-），女，本科，教授.研究领域：数字媒体技术.

敖甜甜（1998-），女，本科生.研究领域：数字媒体技术.