一种基于g2o的SLAM后端优化算法方法
【技术领域】
[0001] 本发明涉及移动机器人的同步定位与构建地图（SLAM)领域，具体是一种基于g2o 的SLAM后端优化算法方法。
【背景技术】
[0002] 自从在1980年被提出以来，SLAM问题已经成为机器人学科的一个重要研宄方 向，是机器人在未知环境实现真正自主的基础。SLAM本质上是一个状态估计问题，可以根 据估计技术分为滤波方法和平滑方法。常见的滤波方法有扩展卡尔曼滤波EKF(extended Kalman filters)、稀疏扩展信息滤波EKFs、粒子滤波等。基于图优化的SLAM早在1997年 就被引入，它使用位姿图对SLAM问题进行建模。近些年，已经有许多基于图优化的算法被 提出，著名的有Olson, TreeMap，TORO, iSAM，还有一些基于图优化的开源框架g2o和iSAM2〇 近年来关于SLAM问题结构的新观点以及稀疏线性代数领域（Csparse，CH0LM0D等c库）的 发展，可以用高效的方法来解决基于图的优化问题。因此，基于图的SLAM才成为研宄的热 点。
[0003] 一般基于图优化的SLAM算法都包含两个步骤：图的构建和图的优化。图的构建称 为前端，图的优化称为后端。前端对机器人获得的原始传感器数据进行处理并进行数据融 合来完成位姿图的构建。位姿图完成之后，由后端对图进行优化。在位姿图中，节点表示机 器人的位姿，节点之间的边表示位姿之间的空间约束关系。节点之间的边代表由观测值得 到的位姿之间的约束条件，有两种不同的约束条件：一种是通过里程计得到相邻节点之间 的约束，称为序列约束或里程计约束；另一种是通过传感器观测值得到的非相邻节点之间 的约束，一般要借助于前端的位置识别算法产生，称为闭环约束。目前SLAM后端优化部分 都是用最小二乘法来实现的，没有对异常值的处理。而实际情况是前端并没有很好的解决 数据融合的问题，而这些问题将对机器人位姿的估计及地图构建产生破坏性的影响，对于 这些问题，后端基本没有进行处理，这可能使得后端的优化结果大大地偏离实际值。
[0004] 动态协方差缩放（Dynamic Covariance Scaling，DCS)算法是对SC算法的改进， 由于SC算法在实现过程中为每一个闭环约束对应的边都引入了相应的开关变量，增加了 最小二乘法优化过程中的变量个数，从而增加了计算的复杂度，通过对优化的目标函数进 行分析，DCS算法得到了开关变量的解析解形式。虽然DCS算法已经可以达到很好的优化 结果，但是对于某些数据集如ringCity来说DCS算法并不能得到好的优化结果。所以在SC 算法的基础上对DCS算法进行了改进。
【发明内容】
[0005] 本发明的目的在于提供一种基于g2o的SLAM后端优化算法方法，，解决了同步定 位与构建地图的后端优化问题，减小错误闭环约束的影响或者说剔除错误的闭环约束，纠 正机器人的位姿序列，使得前端能够构建出的更加准确的地图。
[0006] 为实现上述目的，本发明提供如下技术方案：
[0007] -种基于g2o的SLAM后端优化算法方法，包括以下几个步骤：
[0008] 步骤1:机器人位姿节点信息作为输入数据，通过后端优化关注闭环约束，为每一 条边添加权重因子，并通过推导出的权重因子与信息矩阵之间的数学关系来取得权重因子 的值，从而纠正了机器人位姿节点的信息；
[0009] 在步骤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. 1:t)，而且因为不确定矩阵 是一个稀疏矩阵，所以仅包含一个斜三角矩阵即非零值。
[0010] 节 点：VERTEX-SE2 id pose, x pose, y pose, theta
[0011] 边：EDGE-SE2 idFrom idTo mean, x mean, y mean, theta
[0012] inf. xx inf. xy inf. xt inf. yy inf. yt inf. tt
[0013] 在步骤1中，若节点为3D节点，机器人位姿节点信息包括节点的编号（id)、点 的坐标（pose, x，pose, y，pose, z)、四兀组（pose, quadl，pose. quad2, pose. quad3, pose. quad4)，其边的信息包括两节点的编号（idFrom，idTo)、两个节点坐标的差值（mean, x mean, y mean, z)、四元组的差值（mean, quadl mean. quad2 mean. quad3 mean. quad4)和不 石角定性稀疏矩阵（inf. xx inf. xy inf. xz inf. xt inf. yy inf. yz inf. yt inf. zz inf. zt inf. tt)，其不确定性矩阵也是一个稀疏矩阵。
[0014]节点：VERTEX_SE3 id pose, x pose, y pose, z pose, quadl pose. quad2 pose. quad3 pose. quad4
[0015] 边：EDGE_SE2 idFrom idTo mean, x mean, y mean, z mean, quadl mean. quad2 mean. quad3 mean. quad4
[0016] inf. xx inf. xy inf. xz inf. xt inf. yy inf. yz inf. yt inf. zz inf. zt inf. tt
[0017] 在步骤1中，闭环约束是指机器人走到以前已经走过的路时认为两个不同编号的 节点位于同一个位置，由此而产生的约束，由于传感器等设备的感知误差而导致这两个不 同编号的节点的坐标、角度等数据不同，所以闭环约束用来进行位姿节点的重新纠正，同时 前端的位置识别算法同样会产生错误的闭环约束，错误的闭环约束很有可能导致严重的灾 难性的错误，使得路径远远偏离真实情况。
[0018] 在步骤1中，添加权重因子是为了剔除错误闭环约束而采取的一种策略，引入开 关变量[0，1]，这个开关变量使潜在的错误闭环约束失效。这里介绍的改进的动态协 方差算法是从SC算法获得灵感，SC算法将基于图的SLAM表;^为如下式：
【主权项】
1. 一种基于g2o的SLAM后端优化算法方法，其特征在于，包括W下步骤： 步骤1 ;机器人位姿节点信息作为输入数据，通过后端优化算法关注闭环约束，为每一 条边添加权重因子，并通过推导出的权重因子与信息矩阵之间的数学关系来取得权重因子 的值，经过后端优化算法之后，纠正了机器人位姿节点的信息； 机器人位姿节点信息作为输入数据，若节点为2D节点，其位姿节点信息包括节点的编 号、点的坐标（x，y)、角度（thera)，其边的信息包括两节点的编号、两点的差值坐标（mean. X，mean,y)、差值角度（mean,theta)和不确定矩阵，而且因为不确定矩阵是一个稀疏矩阵， 所W仅包含一个斜=角矩阵即非零值； 若节点为3D节点，机器人位姿节点信息包括节点的编号、点的坐标（X，y，Z)、四元组， 其边的信息包括两节点的编号、两个节点坐标的差值、四元组的差值和不确定性矩阵，其不 确定性矩阵也是一个稀疏矩阵； 权重因子与信息矩阵之间的数学关系是经过严格的数学推导推算出来的，其推导过程 分为W下几步： 第一步；分析开关变量如何影响误差函数的局部极小值，考虑两个节点m和n之间的 边，将误差函数分为了两部分，一部分是包含除了边mn之外的所有边，另一部分仅仅包含 边皿；分析任意一条边皿，误差函数分为两部分，SLAM问题表示为如下式做：
第二步；对误差函数求偏导；当优化器收敛的时候，对属于化到的所有变量求偏导都 等于0,因此，关于的偏导也等于0,如下式；
第S步；由关于Sm。的偏导也等于0,推导出当目标函数取得最小值的时候S的值：
第四步：开关变量的取值范围是0~1，所W，综合第S步求出的S的值与S的取值范 围，从而得到S的值：
步骤2 ;g2o自带的优化策略，基于g2o平台，通过最小二乘法进一步进行优化，g2o平 台中的优化策略包含有梯度下降法、牛顿法、高斯牛顿法、LM，构建出更符合真实路径情况 的位姿节点路径。
2. 根据权利要求1所述的基于g2o的SLAM后端优化算法方法，其特征在于，在步骤2
【专利摘要】本发明公开了一种基于g2o的SLAM后端优化算法方法，机器人位姿节点信息作为输入数据，通过后端优化算法关注闭环约束，为每一条边添加权重因子，并通过推导出的权重因子与信息矩阵之间的数学关系来取得权重因子的值，经过后端优化算法之后，纠正了机器人位姿节点的信息；g2o自带的优化策略，基于g2o平台，通过最小二乘法进一步进行优化，构建出更符合真实路径情况的位姿节点路径。本发明中采用DCS1算法，用于解决鲁棒性后端优化问题，不仅减少了复杂度，降低了运行时间，而且提高了收敛速度。对未知环境下拓扑地图的纠正和优化等具有重要的意义。
【IPC分类】G06F17-30, G01C21-20, G01C21-00, G06K9-00
【公开号】CN104850615
【申请号】CN201510246230
【发明人】张亮, 沈沛意, 朱光明, 宋娟, 刘静
【申请人】西安电子科技大学
【公开日】2015年8月19日
【申请日】2015年5月14日