[发明专利]一种基于g2o的SLAM后端优化算法方法

说明书

技术领域

本发明涉及移动机器人的同步定位与构建地图(SLAM)领域，具体是一种基于g2o的SLAM后端优化算法方法。

背景技术

自从在1980年被提出以来，SLAM问题已经成为机器人学科的一个重要研究方向，是机器人在未知环境实现真正自主的基础。SLAM本质上是一个状态估计问题，可以根据估计技术分为滤波方法和平滑方法。常见的滤波方法有扩展卡尔曼滤波EKF(extended Kalman filters)、稀疏扩展信息滤波EKFs、粒子滤波等。基于图优化的SLAM早在1997年就被引入，它使用位姿图对SLAM问题进行建模。近些年，已经有许多基于图优化的算法被提出，著名的有Olson,TreeMap,TORO,iSAM，还有一些基于图优化的开源框架g2o和iSAM2。近年来关于SLAM问题结构的新观点以及稀疏线性代数领域(Csparse,CHOLMOD等c库)的发展，可以用高效的方法来解决基于图的优化问题。因此，基于图的SLAM才成为研究的热点。

一般基于图优化的SLAM算法都包含两个步骤：图的构建和图的优化。图的构建称为前端，图的优化称为后端。前端对机器人获得的原始传感器数据进行处理并进行数据融合来完成位姿图的构建。位姿图完成之后，由后端对图进行优化。在位姿图中，节点表示机器人的位姿，节点之间的边表示位姿之间的空间约束关系。节点之间的边代表由观测值得到的位姿之间的约束条件，有两种不同的约束条件：一种是通过里程计得到相邻节点之间的约束，称为序列约束或里程计约束；另一种是通过传感器观测值得到的非相邻节点之间的约束，一般要借助于前端的位置识别算法产生，称为闭环约束。目前SLAM后端优化部分都是用最小二乘法来实现的，没有对异常值的处理。而实际情况是前端并没有很好的解决数据融合的问题，而这些问题将对机器人位姿的估计及地图构建产生破坏性的影响，对于这些问题，后端基本没有进行处理，这可能使得后端的优化结果大大地偏离实际值。

动态协方差缩放(Dynamic Covariance Scaling，DCS)算法是对SC算法的改进，由于SC算法在实现过程中为每一个闭环约束对应的边都引入了相应的开关变量，增加了最小二乘法优化过程中的变量个数，从而增加了计算的复杂度，通过对优化的目标函数进行分析，DCS算法得到了开关变量的解析解形式。虽然DCS算法已经可以达到很好的优化结果，但是对于某些数据集如ringCity来说DCS算法并不能得到好的优化结果。所以在SC算法的基础上对DCS算法进行了改进。

发明内容

本发明的目的在于提供一种基于g2o的SLAM后端优化算法方法,，解决了同步定位与构建地图的后端优化问题，减小错误闭环约束的影响或者说剔除错误的闭环约束，纠正机器人的位姿序列，使得前端能够构建出的更加准确的地图。

为实现上述目的，本发明提供如下技术方案：

一种基于g2o的SLAM后端优化算法方法，包括以下几个步骤：

步骤1：机器人位姿节点信息作为输入数据，通过后端优化关注闭环约束，为每一条边添加权重因子，并通过推导出的权重因子与信息矩阵之间的数学关系来取得权重因子的值，从而纠正了机器人位姿节点的信息；

在步骤1中，机器人位姿节点信息作为输入数据，若节点为2D节点，其位姿节点信息包括节点的编号(id)、点的坐标(pose.x,pose.y)、角度(pose.thera)，其边的信息包括两节点的编号(idFrom,idTo)、两点的差值坐标(mean.x,mean.y)、差值角度(mean.theta)和不确定稀疏矩阵(inf.xx inf.xy inf.xt inf.yy inf.yt inf.tt)，而且因为不确定矩阵是一个稀疏矩阵，所以仅包含一个斜三角矩阵即非零值。

节点：VERTEX_SE2 id pose.x pose.y pose.theta

边：EDGE_SE2 idFrom idTo mean.x mean.y mean.theta

inf.xx inf.xy inf.xt inf.yy inf.yt inf.tt