SLAM后端优化算法的研究

来源:用户上传      作者:

　　摘要：随着虚拟现实和自动驾驶的飞速发展，同时定位与地图创建（sLAM）是近几年研究的热点，也是各领域实现智能化的关键。本文针对SLAM后端优化的两种主要方法一基于滤波理论优化和基于非线性优化（图优化）展开综述。首先，介绍了基于线性优化的卡尔曼滤波器（KF）、粒子滤波器（PF），并详细分析了不同滤波器模型的具体实现、性能、优缺点;其次，阐述了基于图优化算法的整体框架、关键技术，着重介绍了在欧式空间以及流形空间优化算法;最后对SLAM后端优化算法的未来发展进行了展望。
　　关键词：同时定位与地图创建;滤波器模型;图优化;流形空间
　　0引言
　　智能主体能够在未知环境中自主地完成一些功能，例如自动驾驶、扫地机器人、灾后救援等，需要不断对周围环境进行探测并创建环境地图，对自身定位进行路径规划，避开障碍物准确到达目的。这些任务要求智能主体既要明白自身狀态（即位置），也需要了解外在环境（即地图）。研究者把这类问题称为同时定位与地图创建（Simultaneous Locationand Mapping.SLAM），定位是指在室内环境下，GPS或BD模块接收不到卫星信号，无法获取经纬度等位置相关信息时，智能主体则通过环境中特征（如路标或障碍物）来对自身的位姿进行估计：地图创建是指将未知环境中所有物体的相关位置等信息提取出来，即对环境进行描述，这两者密切相关，相互依赖。智能主体需要利用已经创建出来的地图对自身进行定位，又需要智能主体依据当前的位姿信息对环境地图进行更新。近几年来，SLAM成为智能主体研究的核心内容，SLAM主要有传感器数据读取、前端视觉里程计、后端优化、回环检测、建图五部分构成。本文主要研究后端优化部分，后端在接受不同时刻视觉里程计测量的相机位姿信息、回环检测的信息，对这些信息进行优化，得到智能主体全局一致的轨迹和地图。主要有两种实现方式：基于滤波理论优化和基于非线性优化（图优化）。
　　基于滤波理论优化的方法主要是利用贝叶斯原理，从开始时刻到结束递归地进行。依据上一时刻置信度和运动变换概率的积分（或求和）估计当前状态的置信度，然后利用当前时刻传感器的观测数据概率乘以当前时刻状态的置信度，得到后验概率。对于全局估计问题，由于存在很多不同的假设，每一种假设都会形成不同的后验模式，从而存在不同的滤波器算法。常见的有：卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）、信息滤波器（IF）和粒子滤波器（PF）。滤波算法具有时间约束和增量特性，通常也被称为在线SLAM（On-Line SLAM）。
　　与滤波理论的求解算法相反，基于非线性（图优化）不再依赖某一时刻的信息，而是通过智能主体所有的运动信息和观测数据来优化智能主体完整的运动轨迹和环境特征地图，也被称为完全SLAM（Full SLAM）。其核心思想是：把后端优化算法转换成图的一种形式，图中的顶点代表了不同时刻智能主体的位姿和环境特征，有约束关系的各个顶点用边来表示。建好图之后，利用图优化算法对智能主体的位姿进行求解，使得顶点更好地满足对应边上的约束条件，优化算法结束之后，对应的图即是智能主体的运动轨迹和环境地图。
　　本文将从以上两方面对SLAM后端优化算法展开研究。首先介绍线性优化SLAm.包括高斯滤波算法卡尔曼滤波和非参数滤波算法粒子滤波：其次阐述图优化SLAm.包括欧式空间随机梯度下降法和松弛算法，以及流形空间图优化算法。
　　1线性优化SLAM
　　1.1线性优化基本思想
　　SLAM过程可以由智能主体的运动方程和观测方程来进行描述。其中运动方程是依据某一时刻之前智能主体的位姿、周围环境空间特征、控制输入变量来预测该时刻的状态：观测方程是智能主体自身携带传感器的观测值。在SLAM的求解过程中，线性优化采用滤波器模型，该模型是基于贝叶斯概率的求解方式，进而将SLAM问题转成求解智能主体位姿和周围环境空间特征联合概率。假设在某一时刻k.智能主体的位姿为xk，周围环境空间特征为yk，控制输入变量为uk。
　　首先利用以前状态的后验概率密度p（xk-1，yk-1|xk-2，uk-1，zk-1）和运动变换转移概率P（xk|xk-1，uk）来预测k时刻的状态分布：
　　其中，η为归一化变量（乘积结果不再是一个概率，其总和可能不为1），Zk为传感器观察值。
　　依据上述的分析，可知线性优化模型使用概率思想求解后端优化问题，为了递归求解后验概率，需要初始化置信度，并且在求解的过程中依据有关测量、运动变换转移概率的不同假设，每一种假设都形成自己的后验模式，因此得出不同的滤波算法。常见的两种滤波算法有：卡尔曼滤波器和粒子滤波器。
　　1.2 卡尔曼滤波算法
　　卡尔曼滤波器是一种线性系统的最优无偏估计算法，使用高斯函数表示后验概率，在可能的状态空间中使用均值和方差（即高斯函数分布的均值和方差）来表达置信度。其中运动变换方程必须是带有随机高斯噪声参数的线性函数，观测方程也与带高斯噪声的自变量呈线性关系，初始置信度必须服从正态分布，依据以上的假定得到一个全局最优的智能主体和周围环境空间状态估计。在具体的SLAM应用中，KF算法主要由两部分组成：状态预测和更新。预测：依据智能主体上一时刻的位姿和环境特征、传感器数据预测当前时刻的位姿和环境特征的联合状态估计（置信度），其中位姿和环境特征的联合状态表示均值，方差由运动变换矩阵求解：更新：首先计算卡尔曼增益，利用预测置信度、卡尔曼增益更新置信度，即更新智能主体当前时刻的后验概率分布;以上两个步骤不断地递归执行。算法的具体执行步骤如下： 　　（1）假定运动变换概率p（xk|xk-1，uk）和观测概率p（zk|xk）用下式表示：式中，xk和xk-1为状态向量;uk为k时刻的控制变量;Ak运动变换矩阵;Bk为输入控制矩阵;εk是一个高斯随机变量，表示由状态转移引入的不确定性，均值为O;方差用只Rk表示;Ck为观测矩阵;向量δk为观测噪声，均值为0.方差为Qk。
　　（2）预测
　　卡尔曼滤波作为线性高斯系统，观测是状态的线性函数，并且下一时刻状态是历史状态的线性函数，使用最小均方误差的方式估计线性系统，实际上运动变化和测量很少是线性的。因此，研究者提出了改进算法扩展卡尔曼滤波（EKF），放宽了其中的一个假设条件，将KF方法中线性的状态转移和观测转换为非线性的，使用雅可比矩阵替换KF中线性系统矩阵，从而置信度不再是一个高斯分布，而是高斯的近似值，得到状态系统的次优估计，EKF对一些强非线性和非高斯分布的应用存在较大的近似误差。
　　1.3粒子滤波算法
　　粒子滤波（PF）是贝叶斯滤波的一种非参数实现。与线性高斯KF不同，其不依赖确定的后验函数，而通过有限数量的值来近似后验，每一个值大致与状态空间的某个区域有关，将Bayes中求解积分转换成有限个样本点求和的过程。PF的核心思想是利用一系列随机状态采样的加权和来近似积分，求解后验概率密度函数（置信度），当采样粒子数量不断增多达到一定阈值时，加权和就接近于状态变量的后验概率密度函数。在SLAM应用中，PF主要包括两个过程：采样、权值更新，算法的详細执行如下：
　　（1）采样：依据系统已知的、容易采样的重要性概率密度函数g（x0：k|z1：k，从中抽取N个独立的粒子样本集合。
　　（2）权值更新：对样本集合中每个采样粒子进行权值求解。
　　系统的重要性概率密度函数为：
　　由于进行了乘法运算，乘法的总和可能不再是1.因此需要对粒子的权值进行归一化，则有：
　　使用所有粒子以及对应的权值求和来近似位姿和环境特征点的联合后验函数，则有：
　　当采样粒子的数量足够大（Nm→∞）时，公式（17）可以很接近地描述在智能主体的位姿和环境特征在该状态下，最有可能产生当前的观测数据。
　　粒子滤波算法为了得到正确的后验概率估计值，样本空间中粒子权值方差越接近零越好，但是随着算法迭代次数的增多，粒子样本权值存在退化现象。在样本空间中只有少数粒子参与计算，甚至迭代次数达到某一阈值后，可能只有一个粒子样本的权值为非零，其它粒子样本的权值都无效，导致计算量浪费在对联合后验概率几乎不起作用的无效粒子的更新操作上，导致SLAM系统的性能降低。为了缓解上述涉及的粒子退化现象，一般采用以下两种方法：在进行粒子采样时选择恰当的重要性概率密度函数，在SLAM中通常选用运动变换概率作为采样的概率密度函数。使用该方法的缺点是：在进行采样时，没有考虑当前智能主体的观测值：在权值更新完成之后引入重采样技术，重采样有多种实现方式，其中最简单的是移除权值较小的粒子样本，对剩余的粒子样本进行归一化处理，所以粒子的权值都相等，为粒子样本空间数的倒数，重采样技术减少了粒子的多样性，减轻了样本权值退化的现象，但并不能从本质上得到解决，
　　2 基于图优化SLAM
　　2.1 基本思想
　　图优化利用图论意义上的图，将SLAM后端最优化问题转换图的一种表现形式，图是由若干个顶点（vertex）和连接这些顶点的边（edge）构成的。在SLAM应用中，智能主体在任意时刻的位姿构成了图优化的顶点，相邻时刻位姿之间的运动状态转换构成了图优化的边，顶点即需要优化的变量，在构建好图之后，需要调整智能主体的位姿尽可能地满足图中每条边对应的约束。因此，后端图优化过程可以分成以下两个步骤来实现一构造图和优化图。构造图：智能主体位姿表示顶点，位姿之间的关系表示边，智能主体自身携带的传感器信息构造出图，该步骤是传感器数据的堆积，也被称为前端（front-end）;优化图：即优化智能主体位姿，保证图中顶点最符合其对应边的约束，也被称为后端（back-end）。
　　2.2 基于欧式空间的图优化算法
　　2.2.1 基于随机梯度下降的图优化方法
　　Olson等将机器学习中最常用的随机梯度下降法（stochastic gradient descent.SGD）应用到SLAM的后端优化算法中，提出了基于SGD的图优化算法，在求解SLAM迭代的过程中，遍历位姿图中的每条边，依据边上限制的位姿变换关系，不断地寻找梯度并下降的过程，从而在该方向上寻找最优的参数。直到某一时刻增量非常小，函数无法再下降，此时SGD收敛，目标函数达到了极小，完成极小值的搜索。通过实验证明，基于SGD的图优化算法计算速度快，对SLAM系统的初始值具有较高的健壮性，并且不易陷入局部最优值，同时对SLAM系统给不同的初始值，零值或随机产生值，甚至与最优值相差很多，该算法都会产生很好的收敛效果。在Olson的基础上，Grisetti对SGD进行了进一步的改进和扩展，使用数据结构中的树结构来描述智能主体（2维空间或3维空间）的位姿变换，采用增量方式来求解智能主体最可能的状态，从而将整个优化问题的核心转换成求解增量方程。通过实验证明，该算法既能够有效地更新智能主体位姿和环境特征，也加快了收敛速度。除此之外，Grisetti将树结构和SGD结合提出了基于树型网格的优化法（tree-basednetwork optimizer.TORO），该算法用于6自由度的智能主体优化。 　　2.2.2 基于松弛的图优化方法
　　Howard等将松弛算法（relaxation）应用到SLAM后端优化算法中，提出了基于松弛的图优化算法。在后端算法的迭代过程中，对位姿图中的每个顶点都做如下操作：其中顶点中包含智能主体位姿和环境特征信息，利用选取顶点的邻接节点信息和运动变换方程重新计算顶点的最优解并更新。同时Duckett等假设在智能主体旋转角度已知的情况下，证明了relaxation的图优化算法必定收敛于系统最优解。通过实验证明，基于relaxation的图优化算法既可以解决静态环境特征点下不变的SLAM问题，还可以应用于增量式SLAM系统，当环境中有特征发生改变，例如增加一个路标点，该算法可以直接将新环境特征更新到原有地图中。但该算法存在一个缺陷，两个顶点之间的约束条件存在较大的误差时，relaxation可能需要多次迭代才能将两顶点对应边上的误差减少并分配到其它頂点对应的约束条件上，从而增加了算法复杂度，同时环形闭合也需要解决此问题。在relaxation算法基础上不断改进，Frese等结合多网格方法来替代SLAM系统中的偏微分方程，提出一种多层次松弛（MLR）优化算法，提高了在环形闭合情况下顶点的优化效率。
　　2.3 基于流形空间的图优化算法
　　基于SGD和relaxation的图优化算法是在欧式空间进行运算的，在欧式三维空间中，对于智能主体来说，旋转矩阵需要9个变量来描述3自由度的旋转，具有一定的冗余性：其次欧拉角和旋转向量是紧凑的但具有奇异性。因此需要寻找不带奇异性的三维向量描述方式，引入四元数，会产生额外的自由度，Grisetti等进一步提出了在流形空间进行SLAM后端图优化的思想，提出一种分层的图优化算法（hierarchical graph optimization.HGO），该算法在图优化算法创建位姿图的过程，依据智能主体观测值只在粗略描述这一层对地图进行更新，降低了算法复杂度。在图优化中，常见的有g20库，Kummerle扩展了g2o库，提出了基于流形空间的g2o通用库，提高了开发效率：在通用库基础上又扩展了状态空间，从而更好地完成智能主体自身定位和同步创建地图的任务。
　　3 结束语
　　SLAM是实现移动机器人、自动驾驶以及混合现实等研究领域的关键技术之一，同时也是智能移动主体感知周围环境的重要部分，近年来SLAM成为很多领域研究的重点与热点。SLAM后端优化算法出现了滤波算法和图优化算法，滤波算法依赖上一时刻智能主体的状态，对于高斯滤波算法不能解决非强高斯或者非强线性的情况，而非参数滤波中PF存在粒子退化无法保证粒子多样性问题：图优化算法则依据系统所有的状态信息进行优化，随着对图优化算法的深入研究，算法处理环境的规模能力逐渐提高，鲁棒性较好，在非线性、稀疏结构以及扩展性方面仍需深入研究，语义地图的创建具有较大的发展趋势。
转载注明来源:https://www.xzbu.com/8/view-15125573.htm