CN104850615A - 一种基于g2o的SLAM后端优化算法方法 - Google Patents

Abstract

本发明公开了一种基于g2o的SLAM后端优化算法方法，机器人位姿节点信息作为输入数据，通过后端优化算法关注闭环约束，为每一条边添加权重因子，并通过推导出的权重因子与信息矩阵之间的数学关系来取得权重因子的值，经过后端优化算法之后，纠正了机器人位姿节点的信息；g2o自带的优化策略，基于g2o平台，通过最小二乘法进一步进行优化，构建出更符合真实路径情况的位姿节点路径。本发明中采用DCS1算法，用于解决鲁棒性后端优化问题，不仅减少了复杂度，降低了运行时间，而且提高了收敛速度。对未知环境下拓扑地图的纠正和优化等具有重要的意义。

Description

一种基于g2o的SLAM后端优化算法方法

技术领域

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

背景技术

自从在1980年被提出以来，SLAM问题已经成为机器人学科的一个重要研究方向，是机器人在未知环境实现真正自主的基础。SLAM本质上是一个状态估计问题，可以根据估计技术分为滤波方法和平滑方法。常见的滤波方法有扩展卡尔曼滤波EKF(extendedKalman 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

在步骤1中，若节点为3D节点，机器人位姿节点信息包括节点的编号(id)、点的坐标(pose.x,pose.y,pose.z)、四元组(pose.quad1,pose.quad2,pose.quad3,pose.quad4)，其边的信息包括两节点的编号(idFrom,idTo)、两个节点坐标的差值(mean.xmean.y mean.z)、四元组的差值(mean.quad1 mean.quad2 mean.quad3 mean.quad4)和不确定性稀疏矩阵(inf.xx inf.xy inf.xz inf.xt inf.yy inf.yz inf.yt inf.zz inf.ztinf.tt)，其不确定性矩阵也是一个稀疏矩阵。

节点：VERTEX_SE3 id pose.x pose.y pose.z pose.quad1 pose.quad2 pose.quad3pose.quad4

边：EDGE_SE2 idFrom idTo mean.x mean.y mean.z mean.quad1 mean.quad2 mean.quad3mean.quad4

inf.xx inf.xy inf.xz inf.xt inf.yy inf.yz inf.yt inf.zz inf.zt inf.tt

在步骤1中，闭环约束是指机器人走到以前已经走过的路时认为两个不同编号的节点位于同一个位置，由此而产生的约束，由于传感器等设备的感知误差而导致这两个不同编号的节点的坐标、角度等数据不同，所以闭环约束用来进行位姿节点的重新纠正，同时前端的位置识别算法同样会产生错误的闭环约束，错误的闭环约束很有可能导致严重的灾难性的错误，使得路径远远偏离真实情况。

在步骤1中，添加权重因子是为了剔除错误闭环约束而采取的一种策略，引入开关变量s_ij∈[0,1]，这个开关变量使潜在的错误闭环约束失效。这里介绍的改进的动态协方差算法是从SC算法获得灵感，SC算法将基于图的SLAM表示为如下式：

$\begin{array}{c}{X}^{*},S^{*}=\underset{X,S}{\arg \min }\underset{i}{\mathrm{\Σ}}{\left|\right|d_i^{\mathrm{odo}}\left|\right|}_{{\mathrm{\Σ}}_i}^2+\underset{\mathrm{ij}}{\mathrm{\Σ}}{\left|\right|\mathrm{\Ψ}\left(s_{\mathrm{ij}}\right)(f(x_i,u_{\mathrm{ij}})-x_j)\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{ij}}}^2+\underset{\mathrm{ij}}{\mathrm{\Σ}}{\left|\right|1-s_{\mathrm{ij}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{ij}}}^2\\ =\underset{X,S}{\arg \min }\underset{i}{\mathrm{\Σ}}{\left|\right|d_i^{\mathrm{odo}}\left|\right|}_{{\mathrm{\Σ}}_i}^2+\underset{\mathrm{ij}}{\mathrm{\Σ}}{\left|\right|d_{\mathrm{ij}}^{\mathrm{slc}}\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{ij}}}^2+\underset{\mathrm{ij}}{\mathrm{\Σ}}{\left|\right|d_{\mathrm{ij}}^{\mathrm{sp}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{ij}}}^2\end{array}---\left(3\right)$

在步骤1中，开关变量s_ij服从正态分布，其最大后验概率表达式如下：

$\begin{array}{c}{S}^{*}=\underset{S}{\arg \max }P\left(S\right|\mathrm{\Γ})\\ =\arg \max \underset{\mathrm{ij}}{\mathrm{\Σ}}{\left|\right|1-s_{\mathrm{ij}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{ij}}}^2\\ =\arg \max \underset{\mathrm{ij}}{\mathrm{\Σ}}{\left|\right|d_{\mathrm{ij}}^{\mathrm{sp}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{ij}}}^2\end{array}$

其中，开关变量服从正态分布：s_ij～N(1,Ξ_ij)；

由开关变量引入一个开关函数Ψ(s_ij)，Ψ(s_ij)∈[0,1]函数是一个缩放函数，在开关变量s_ij和开关先验Ξ_ij已知的情况下，决定闭环约束的权重；

将开关函数Ψ(s_ij)看作是对与约束相关的信息矩阵的缩放函数，关于闭环约束部分表示为如下式：

$\begin{array}{c}{\left|\right|d_{\mathrm{ij}}^{\mathrm{slc}}\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{ij}}}^2={\left|\right|\mathrm{\Ψ}\left(s_{\mathrm{ij}}\right)(f(x_i,u_{\mathrm{ij}})-x_j)\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{ij}}}^2\\ =\mathrm{\Ψ}\left(s_{\mathrm{ij}}\right){(f(x_i,u_{\mathrm{ij}})-x_j)}^T{\mathrm{\Λ}}_{\mathrm{ij}}^{-1}\mathrm{\Ψ}\left(s_{\mathrm{ij}}\right)(f(x_i,u_{\mathrm{ij}})-x_j)\\ ={(f(x_i,u_{\mathrm{ij}})-x_j)}^T{\mathrm{\Λ}}_{\mathrm{ij}}^{-1}\mathrm{\Ψ}{\left(s_{\mathrm{ij}}\right)}^2(f(x_i,u_{\mathrm{ij}})-x_j)\\ ={\left|\right|(f(x_i,u_{\mathrm{ij}})-x_j)\left|\right|}_{\mathrm{\Ψ}{\left(s_{\mathrm{ij}}\right)}^{-2}{\mathrm{\Λ}}_{\mathrm{ij}}}^2\end{array}$

开关函数成为了信息矩阵的缩放函数；

权重因子与信息矩阵之间的数学关系是经过数学推导推算出来的，其推导过程分为以下几步：

第一步：分析开关变量如何影响误差函数的局部极小值，考虑两个节点m和n之间的边，将误差函数分为了两部分，一部分是包含除了边mn之外的所有边，另一部分仅仅包含边mn；分析任意一条边mn，误差函数分为两部分，SLAM问题表示为如下式(3)：

$\begin{array}{c}{X}^{*},S^{*}=\underset{X,S}{\arg \min }\underset{i}{\mathrm{\Σ}}{\left|\right|d_i^{\mathrm{odo}}\left|\right|}_{{\mathrm{\Σ}}_i}^2+\underset{\mathrm{ij}\≠\mathrm{mn}}{\mathrm{\Σ}}{\left|\right|d_{\mathrm{ij}}^{\mathrm{slc}}\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{ij}}}^2+\underset{\mathrm{ij}\≠\mathrm{mn}}{\mathrm{\Σ}}{\left|\right|d_{\mathrm{ij}}^{\mathrm{sp}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{ij}}}^2\\ +{\left|\right|s_{\mathrm{mn}}(f(x_m,u_{\mathrm{mn}})-x_n\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{mn}}}^2+{\left|\right|1-s_{\mathrm{mn}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{mn}}}^2\\ =\underset{X,S}{\arg \min }h(X_{\mathrm{ij}\≠\mathrm{mn}},S_{\mathrm{ij}\≠\mathrm{mn}})+s_{\mathrm{mn}}^2{\mathrm{\χ}}_{l_{\mathrm{mn}}}^2+{(1-s_{\mathrm{mn}})}^2\mathrm{\Φ}\\ =\underset{X,S}{\arg \min }h(X_{\mathrm{ij}\≠\mathrm{mn}},S_{\mathrm{ij}\≠\mathrm{mn}})+g(X_{\mathrm{ij}=\mathrm{mn}},S_{\mathrm{ij}=\mathrm{mn}})\end{array}---\left(3\right);$

第二步：对误差函数求偏导；当优化器收敛的时候，对属于{X,S}的所有变量求偏导都等于0，因此，关于s_mn的偏导必然也等于0，如下式：

第三步：由关于s_mn的偏导必然也等于0，推导出当目标函数取得最小值的时候s的值：

$s=\frac{\mathrm{\Φ}}{{\mathrm{\χ}}_l^2+\mathrm{\Φ}};$

第四步：开关变量的取值范围是0～1，所以，综合第三步求出的s的值与s的取值范围，从而得到s的值：

从而可以由已知量来求出开关变量的值，这样就减少了一组开关变量，大大减少计算量、优化性能。

步骤2：g2o自带的优化策略，基于g2o平台，通过最小二乘法进一步进行优化，g2o平台中的优化策略包含有梯度下降法、牛顿法、高斯牛顿法、LM，降低了错误闭环的影响，构建出更符合真实路径情况的位姿节点路径。

作为本发明进一步的方案：在步骤2中，g2o平台是一个图优化通用求解器，其中集成了最小二乘法的求解方法，也集成了SC方法，SC方法利用了开关变量和开关函数，所以计算量相对复杂；后端优化问题从数学的角度上来分析都是最小二乘法的问题，所以，经过步骤1的算法处理，最后在g2o中求得最后的位姿序列。2o中选择高斯牛顿法进行优化，最终计算出使得误差函数最小的位姿序列解，构建出位姿节点路径。

与现有技术相比，本发明的有益效果是：

1)相对于DCS算法，提出的改进算法缩小了参数的取值范围，使得优化效果更加明显。

2)针对不同的数据集以及不同情况，包括错误闭环约束数目、添加错误闭环策略不同、二维/三维数据集、模拟/真实数据集、初始估计不同的同一数据集，都有相当好的效果。

3)与现有的SC、RRR、MM、DCS算法比较，综合性能上占有优势。比SC算法收敛速度快很多；DCS1算法的优化结果不依赖于闭环数目的多少一直都保持了很小的误差，而RRR算法随着闭环数目的不同有波动且误差很大；MM算法对于大部分的数据集该算法丢弃了所有的闭环没有对数据集进行任何的优化；对于某些特定的数据集提出的算法较DCS算法表现良好。

附图说明

图1为基于图的SLAM的整个过程图；

图2为连接节点x_i和节点x_j的边的位姿图；

图3为g2o的框架图；

图4为添加开关变量之后的位姿图；

图5为DCS1算法对Sequence00数据集的优化结果图；

图6为DCS1算法对有错误闭环的City10000数据集的优化结果图；

图7为DCS1算法对有错误闭环的Sphere2500数据集的优化结果图；

图8为DCS1和SC算法对数据集优化的曲线图；

图9为DCS1算法和DCS算法对ringCity数据集的优化结果图；

图10是模拟数据集ring的结果图。

具体实施方式

下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种基于g2o的SLAM后端优化算法方法，包括以下内容：

1)提出了一种改进的动态协方差缩放(Dynamic Covariance Scaling，DCS)算法。DCS1算法对SC算法进行了改进，提出了一个封闭解用于解决鲁棒性后端优化问题。该算法没有像SC算法那样引入新的开关变量，而是直接找到最优的开关变量的值，DCS1算法并不需要重新定义节点和边，实际上，DCS1算法是实现了一个鲁棒性的代价函数，它可以用于任何基于图的SLAM中。不像SC算法，该算法没有改变问题规模的大小。SC和DCS1算法没有对闭环约束给出确定的yes或no的决定，即没有明确的说明哪个闭环约束是错误的，所有的闭环约束都参与了后端优化过程，只是一些闭环约束对优化的影响大，一些的影响小。与此不同的是，RRR和MM算法明确的指出了哪个闭环约束是错误的，对于错误的闭环约束直接丢弃，并不参与优化过程，对优化结果不起任何的作用。

1.1SC算法

SC算法的主要思想是引入开关变量，这个开关变量可以使潜在的错误闭环约束失效。这里介绍的改进的动态协方差算法是从SC算法获得灵感，SC算法将基于图的SLAM表示为如下式(1)：

$X^{*},S^{*}=\underset{X,S}{\arg \min }\underset{i}{\mathrm{\Σ}}{\left|\right|f(x_i,x_{i+1})-z_{i,i+1}\left|\right|}_{{\mathrm{\Σ}}_i}^2+\underset{\mathrm{ij}}{\mathrm{\Σ}}{\left|\right|\mathrm{\Ψ}\left(s_{\mathrm{ij}}\right)(f(x_i,x_j)-z_{\mathrm{ij}})\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{ij}}}^2+\underset{\mathrm{ij}}{\mathrm{\Σ}}{\left|\right|1-s_{\mathrm{ij}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{ij}}}^2---\left(1\right)$

式将SLAM问题解释为三个不同的误差的和，即增量式约束，闭环约束和开关先验约束。这里，Ψ(s_ij)∈[0,1]是一个缩放函数，在开关变量s_ij和开关先验Ξ_ij已知的情况下，决定闭环约束的权重。对函数Ψ(s_ij)还有另一种解释，可以将它看作是对与约束相关的信息矩阵的缩放函数，因为，关于闭环约束部分可以表示为如下式(2)：

$\begin{array}{c}{\left|\right|\mathrm{\Ψ}\left(s_{\mathrm{ij}}\right)(f\left(x_i{x}_j\right)-z_{\mathrm{ij}})\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{ij}}}^2=\mathrm{\Ψ}\left(s_{\mathrm{ij}}\right){(f(x_i,x_j)-z_{\mathrm{ij}})}^T{\mathrm{\Λ}}_{\mathrm{ij}}^{-1}\mathrm{\Ψ}\left(s_{\mathrm{ij}}\right)(f(x_i,x_j)-z_{\mathrm{ij}})\\ ={(f(x_i,x_j)-z_{\mathrm{ij}})}^T{\mathrm{\Λ}}_{\mathrm{ij}}^{-1}\mathrm{\Ψ}{\left(s_{\mathrm{ij}}\right)}^2(f(x_i,x_j)-z_{\mathrm{ij}})\\ ={\left|\right|f(x_i,x_j)-z_{\mathrm{ij}}\left|\right|}_{\mathrm{\Ψ}{\left(s_{\mathrm{ij}}\right)}^{-2}{\mathrm{\Λ}}_{\mathrm{ij}}}^2\end{array}---\left(2\right)$

在Sunderhauf和Protzel论文中建议设置函数Ψ(s_ij)＝s_ij∈[0,1]，因为经过测试发现这可以获得更好的结果，下面我们将直接用s_ij替换Ψ(s_ij)。Sunderhauf和Protzel提出了将机器人位姿X和开关变量S联合优化的思想，对于每一个闭环约束都需要额外添加一个变量s_ij，这个额外增加的开关变量增加了每次迭代的计算量，也就增加了问题的复杂度并且减慢了收敛速度。对于这些缺点，我们通过对目标函数进行分析，可以以解析解的形式得到开关变量的取值，这样就不需要开关函数，也就没有必要引入开关变量，简化了计算的复杂度。

1.2DCS1算法推导过程

通过对式(1)中误差函数进行分析，尤其分析了开关变量如何影响误差函数的局部极小值。不失一般性，考虑两个节点k和l之间的边，将误差函数分为了两部分，一部分是包含除了边kl之外的所有边，另一部分仅仅包含边kl。SLAM问题可以表示为如下式(3)：

其中函数g(·)表示除了边kl外的所有边的误差，包括里程计约束。

当优化器收敛的时候，对属于{X,S}的所有变量求偏导都等于0。因此关于s_kl的偏导必然也等于0。式(3)中的b对s_kl求偏导可得式(4)：

$\▿b=\left[\begin{array}{c}\·\\ \·\\ \·\\ \frac{\∂b}{\∂s}\\ \·\\ \·\\ \·\end{array}\right]=\left[\begin{array}{c}\·\\ \·\\ \·\\ 2s{\mathrm{\χ}}_l^2-2(1-s)\mathrm{\Φ}\\ \·\\ \·\\ \·\end{array}\right]=\left[\begin{array}{c}\·\\ \·\\ \·\\ 0\\ \·\\ \·\\ \·\end{array}\right]---\left(4\right)$

对式(4)推导可得式(5)：

$2s{\mathrm{\χ}}_l^2-2(1-s)\mathrm{\Φ}=0$

$s({\mathrm{\χ}}_l^2+\mathrm{\Φ})=\mathrm{\Φ}$

$s=\frac{\mathrm{\Φ}}{{\mathrm{\χ}}_l^2+\mathrm{\Φ}}---\left(5\right)$

将s带入函数h(·)可得式(6)：

$\hat{h}=\frac{{\mathrm{\Φ}}^2{\mathrm{\χ}}_l^2}{{({\mathrm{\χ}}_l^2+\mathrm{\Φ})}^2}+\mathrm{\Φ}-\frac{2{\mathrm{\Φ}}^2}{{\mathrm{\χ}}_l^2+\mathrm{\Φ}}+\frac{{\mathrm{\Φ}}^3}{{({\mathrm{\χ}}_l^2+\mathrm{\Φ})}^2}---\left(6\right)$

函数表示函数h(·)在导数等于0时的值。找到这个函数的最大值也就等于获得了由优化器计算可得的的所有可能值的上限。对函数求导数如下式(7)：

$\frac{d\hat{h}}{d{\mathrm{\χ}}_l}=\frac{2{\mathrm{\χ}}_l{\mathrm{\Φ}}^2}{{(\mathrm{\Φ}+{\mathrm{\χ}}_l^2)}^2}---\left(7\right)$

从上式(7)可得，当χ_l＝0时，导数等于0。因此，当χ_l趋于正负无穷大和等于0时，函数如下式(8)和(9)所示：

$\underset{{\mathrm{\χ}}_l\→\±\∞}{\lim }\hat{h}=0+\mathrm{\Φ}-0+0=\mathrm{\Φ}---\left(8\right)$

${\mathrm{\χ}}_l=0\⇒\hat{h}=0+\mathrm{\Φ}-2\mathrm{\Φ}+\mathrm{\Φ}=0---\left(9\right)$

从式(8)和(9)可得h(·)≤Φ。不失一般性，对于所有开关变量的值，每个约束都满足h(·)≤Φ。对于所有的边可得如下式(10)：

${(1-s)}^2\mathrm{\Φ}+s^2{\mathrm{\χ}}_l^2\≤\mathrm{\Φ}$

$\mathrm{\Φ}+s^2\mathrm{\Φ}-2{\mathrm{s\Φ}+s}^2{\mathrm{\χ}}_l^2\≤\mathrm{\Φ}$

$s^2(\mathrm{\Φ}+{\mathrm{\χ}}_l^2)+s(-2\mathrm{\Φ})+(\mathrm{\Φ}-\mathrm{\Φ})\≤0$

$s(s(\mathrm{\Φ}+{\mathrm{\χ}}_l^2)-2\mathrm{\Φ})\≤0---\left(10\right)$

由上式(10)解得：

$0\≤s\≤\frac{2\mathrm{\Φ}}{\mathrm{\Φ}+{\mathrm{\χ}}_l^2}---\left(11\right)$

理论上，s可取式(11)这个区间的所有值，但是实际情况下s是小于1的，所以我们取s等于1和使得取的最小值的s中的最小值，即满足：

$s=\min (1,\frac{2\mathrm{\Φ}}{\mathrm{\Φ}+{\mathrm{\χ}}_l^2})---\left(12\right)$

由于优化的目标是求目标函数的最小值，而通过上面的推导可得当目标函数取得最小值的时候s的值为：

$s=\frac{\mathrm{\Φ}}{\mathrm{\Φ}+{\mathrm{\χ}}_l^2}---\left(13\right)$

总而言之，我们得到了一个解析解用来计算每个闭环约束对应的s的值。这个s的值取决于每个闭环约束的初始误差χ_l。通过s²缩放与每个闭环约束边相关的信息矩阵。这个算法是通过分析误差函数的性质，推导出来的用于计算权重因子的解析解。算法不仅减少了复杂度，降低了运行时间，而且提高了收敛速度。

2)算法的稀疏性能，从对错误闭环约束的判断上，鲁棒性后端优化算法可分为两类：一类方法是二元决策，MM算法和RRR算法，另一类方法提供属于(0,1)区间的连续的值，SC和DCS1算法。虽然理论上，对于闭环约束的验证应该是一个二元决策，但是有时候做出非二元决策的意义更深远，尤其在增量式SLAM中，因为高效的求解器充分利用了问题的稀疏性。虽然MM算法做出的是二元决策，但是对于丢弃的闭环约束，它是用一个很小的信息矩阵来表示的，实际上这使得在信息矩阵的相应块非零。在增量式设置上，例如在iSAM2框架的贝叶斯树中，这会导致求解器要求的变量数量比实际上大。SC算法由于引入了开关变量而他们是优化中需要求解的状态向量的一部分，从而增加了额外的开销。

在每个算法中将信息矩阵使用COLAMD分解为上三角矩阵R。通常情况下，矩阵R的填充区域(即非零区域)越大，表示问题越不稀疏，问题也就越难解决。基于上面的讨论，SC算法由于开关变量的存在填充区域最大，也就表示问题的复杂度最大。DCS1算法通过使用解析解形式代替开关变量解决了这个问题，在信息矩阵中使用接近零但非零的块表示丢弃的闭环约束。MM算法同样如此，而RRR算法做出了二元决策，移除了与不正确闭环相应的信息，所以在矩阵R中填充区域最小。

实施例1

本发明实施例中，基于g2o平台集成了提出的DCS1算法方法，g2o是一个通用图优化的开源框架，这个框架主要解决基于图优化的非线性最小二乘法问题，是在位姿图已知的情况下进行的后端优化。在实施过程中，SLAM问题的后端优化问题在g2o平台中运用提出的DCS1算法得以解决，输出优化后的位姿序列。

一般基于图优化的SLAM算法都包含两个步骤：图的构建和图的优化。图的构建称为前端，图的优化称为后端。图1展示了基于图的SLAM的整个过程。前端对机器人获得的原始传感器数据进行处理并进行数据融合来完成位姿图的构建。位姿图完成之后，由后端对图进行优化。在位姿图中，节点表示机器人的位姿，节点之间的边表示位姿之间的空间约束关系。节点之间的边代表由观测值得到的位姿之间的约束条件，有两种不同的约束条件：一种是通过里程计得到相邻节点之间的约束，称为序列约束或里程计约束；另一种是通过传感器观测值得到的非相邻节点之间的约束，一般要借助于前端的位置识别算法产生，称为闭环约束。

求解SLAM后端优化问题主要分为以下几个步骤：

1)定义误差函数和目标函数

图2中节点x_i和x_j表示机器人不同时刻的两个位姿，z_ij和Ω_ij分别表示从节点x_i观测到节点x_j的均值和信息矩阵(协方差矩阵的逆)，协方差矩阵表示了观测到节点x_j的不确定性。e_ij表示观测值与节点间实际的状态转换之间的误差，如果x_i和x_j完美的匹配他们之间的约束关系，那么误差为0，误差函数e_ij(x_i,x_j)可表示为式(2-1)：

$e_{\mathrm{ij}}(x_i,x_j)=z_{\mathrm{ij}}-{\hat{z}}_{\mathrm{ij}}(x_i,x_j)---(2-1)$

其中z_ij表示实际的观测值，表示实际计算得到的两个位姿之间的状态转换值。

这样SLAM的优化问题就转化为求目标函数F(x)的最小值，即式(2-2)：

$x^{*}=\underset{x}{\arg \min }F\left(x\right)---(2-2)$

2)利用一阶泰勒展开式线性化误差函数和目标函数

如果机器人的初始位姿已知，将误差函数利用一阶泰勒式在处展开，J_ij表示e_ij在处的雅克比矩阵，误差项F_ij可表示为：

目标函数F(x)可表示为：

其中c＝∑c_ij，b＝∑b_ij，H＝∑H_ij。上式是关于Δx的二次函数。

3)建立线性化系统、解线性化系统以及更新状态向量

由于式(2-3)是关于Δx的二次函数，对Δx求微分之后等于0可得Δx的最小值Δx^*，求解方程如下式(2-4)：

HΔx＝-b    (2-4)

一般把b称作系数向量，H称作系数矩阵，也叫做信息矩阵。通过解上式的线性方程组可得最小值Δx^*。即新的状态向量估计值可以更新为：

4)迭代1,2,3直到满足某一结果标准。

常用的非线性最小平方算法有Guass-Newton，Levenberg-Marqudart等，均可以直接使用来求最后的最优解。由于系数向量和系数矩阵在形式上都是稀疏的，一些开源的稀疏矩阵运算算法结合非线性最小二乘算法可以用来大大提高计算效率。

本算法是基于g2o平台的，下面是g2o实现如下：

g2o是用C++来实现的，目的是尽可能实现通用性。为了实现通用性，g2o为节点和边提供了抽象的基类。这些基类里面定义了许多的虚函数便于派生类的使用，同时使用了大量的类模板来提高效率。g2o中也使用了Eigen线性代数库来达到高的计算性能。图3描述了g2o的整体框架。

g2o使用了很多高效的算法来达到高的性能，如利用了图的稀疏性，SLAM问题中图的特殊结构，还使用了解稀疏线性系统的高效算法。g2o不仅具有高的效率，还具有通用性和可扩展性，如图3所示，如果用户要定义特定的SLAM问题，只需要定义图中的误差函数、雅克比矩阵和增量操作符即可。包含节点x_i和节点x_j之间的边需要定义误差函数e_ij。通过重载基类的虚函数，开发者可以定义自己的雅克比矩阵J_ij。

提出的算法是由SC算法和DCS算法推导而来，SC算法的特点：

1)开关变量的引入

加入开关变量之后，SLAM的优化问题变为寻找使得误差函数取得最小值的机器人位姿序列X＝{x_i}和开关变量S＝{s_ij}问题。这样SLAM优化问题可以表示为下式：

这时位姿图的拓扑结构变为如图4所示，图中额外加入的开关变量s_2,i控制位姿节点x₂和x_i之间的闭环约束。每个开关变量通过开关函数控制权重的值。这个权重作用于闭环约束上，当权重为0时实际上相当于将这个闭环约束从SLAM问题中移除了。也就相当于从图中将这个相关的边移除了。

通过引入额外的隐变量到这个SLAM问题的优化中，使得关于SLAM问题的图的拓扑结构受制于优化，即随着优化的进行，图的拓扑结构也相应的发生了变化。

2)开关变量先验的引入

假设开关变量的初始值为γ_ij，初始值的集合为Γ＝{γ_ij}。假定开关变量s_ij也服从正态分布，初始值作为这个分布的均值。

由于式(2-6)中可以通过将所有闭环约束(正确的和错误的)的权重因子都设置为0而求得最优解，但是这样就使得位姿图中正确的闭环约束也没有起到应有的作用，由于里程计的累计误差也会使得优化结果严重偏离实际值，显然这样的处理并不合理。所以想到将开关变量偏离初始值的程度作为一种惩罚加入到优化框架中，如此优化问题变为如下式(2-7)：

开关先验的协方差矩阵Ξ_ij可以由前端提供，来表示这个闭环约束是正确的可信度，从而驱使变量s_ij远离它的初始值γ_ij。

开关变量对优化的影响可以从两个方面进行解释：一种是开关变量作为缩放因子，控制闭环约束对优化的影响，另一种是通过开关变量的平方来缩放信息矩阵。当开关变量约等于0的时候，残差将等于0，对估计问题的贡献也几乎为零。从信息矩阵的角度来看，开关变量约等于0时，信息矩阵也接近于0，即信息矩阵几乎不提供信息，也就使得相应的闭环约束在估计过程中不被考虑。

DCS1算法对SC算法进行了改进，该算法没有像SC算法那样引入新的开关变量，而是直接找到最优的开关变量的值，并不需要重新定义节点和边。DCS1算法通过使用解析解形式代替开关变量解决了这个问题，在信息矩阵中使用接近零但非零的块表示丢弃的闭环约束。模拟数据集ring如图10所示。

图5所示为随着错误闭环和正确闭环数目的不同，得到的误差T-RPE基本保持不变，即DCS1算法对于真实数据集Sequence00不会随着闭环数目的不同而失效。经过我们的测试，对于所有的标准数据集，不管错误闭环数目达到多少，DCS1算法都能得到正确的优化结果。从测试结果可以得到以下结论，一：DCS1算法对不含错误闭环约束的数据集来说可以达到和不使用DCS1算法同样的效果；二：在不断增大错误闭环约束数目的情况下，DCS1算法仍然可以达到好的优化效果；三：不管是人工添加还是由前端生成的错误闭环，DCS1算法都能成功优化得到正确的结果。

图6所示为DCS1算法对有错误闭环的City10000数据集的优化结果。图7所示为DCS1算法对有错误闭环的Sphere2500数据集的优化结果。其中City10000数据集为二维数据集，Sphere2500数据集为三维数据集。图6(b)中实线是DCS1算法对图6(a)优化之后的结果，虚线的曲线表示实际的地图，从图中可以看出优化得到的地图与实际的地图基本吻合。图7(b)中实线是使用DCS1算法对图7(a)进行优化的结果，虚线是实际的地图，显然优化结果是正确的。由以上的测试结果可以看出DCS1算法对于含有错误闭环约束的二维和三维数据集都具有鲁棒性。

图8所示为DCS1算法和SC算法对数据集Manhattan(Olson)优化过程中的误差的变化情况，从图中可以看出DCS1算法比SC算法的收敛速度快。对所有的标准数据集和KITTI数据集进行测试，表1对DCS1算法和SC算法的优化结果进行了比较，这里的优化结果都是通过调整DCS1算法的w参数和SC算法的s参数得到的最优结果，从表中可以看出DCS1算法和SC算法的优化结果基本一样，但是DCS1算法所用的时间明显低于SC算法。

表1

图9所示为DCS1算法和DCS算法对ringCity数据集的优化结果，图中虚实线表示DCS算法的优化结果，实线表示DCS1算法的优化结果，虚线是实际的运动轨迹，显然DCS算法的优化结果严重偏离了实际的运动轨迹。下面通过ringCity数据集对两种算法的优化结果说明，表2列出了DCS算法和DCS1算法使用不同的参数w对ringCity数据集进行优化的结果比较。对表2进行分析可以看出通过调整参数w的值将产生不同的优化结果，可以找到使数据集得到最优结果的参数，这里DCS1和DCS算法都在w＝10的时候得到最优的优化结果，对比RPE和ATE的值可以看出DCS1算法比DCS算法的优化结果好。从以上的测试结果可以看出，DCS1算法对含有错误闭环的各种标准数据集都具有很好的鲁棒性，通过调整参数w的值，DCS1算法总能达到最优的结果，而且对于DCS算法失效的数据集，DCS1算法同样可以得到正确的优化结果。

表2

算法	参数(w)	T-RPE(m)	I-RPE(deg)	ATE(mean)(m)	ATE(std)(m)
						DCS1	1	0.0481	0.7247	2.7660	3.8252
DCS1	10	0.0460	0.7067	0.8315	0.4265
						DCS1	100	1.4093	3.4247	13.2240	11.1667
DCS	1	0.0466	0.7140	1.9679	2.5347
						DCS	10	0.0464	0.7425	3.6803	5.7081
DCS	100	0.7139	3.6273	16.5772	13.9462

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (2)

1.一种基于g2o的SLAM后端优化算法方法，其特征在于，包括以下步骤：

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

机器人位姿节点信息作为输入数据，若节点为2D节点，其位姿节点信息包括节点的编号、点的坐标(x,y)、角度(thera)，其边的信息包括两节点的编号、两点的差值坐标(mean.x,mean.y)、差值角度(mean.theta)和不确定矩阵，而且因为不确定矩阵是一个稀疏矩阵，所以仅包含一个斜三角矩阵即非零值；

若节点为3D节点，机器人位姿节点信息包括节点的编号、点的坐标(x,y,z)、四元组，其边的信息包括两节点的编号、两个节点坐标的差值、四元组的差值和不确定性矩阵，其不确定性矩阵也是一个稀疏矩阵；

权重因子与信息矩阵之间的数学关系是经过严格的数学推导推算出来的，其推导过程分为以下几步：

第一步：分析开关变量如何影响误差函数的局部极小值，考虑两个节点m和n之间的边，将误差函数分为了两部分，一部分是包含除了边mn之外的所有边，另一部分仅仅包含边mn；分析任意一条边mn，误差函数分为两部分，SLAM问题表示为如下式(3)：

$\begin{array}{c}{X}^{*},S^{*}=\underset{X,S}{\arg \min }\underset{i}{\mathrm{\Σ}}{\left|\right|d_i^{0d0}\left|\right|}_{{\mathrm{\Σ}}_i}^2+\underset{\mathrm{ij}\≠\mathrm{mn}}{\mathrm{\Σ}}{\left|\right|d_{\mathrm{ij}}^{\mathrm{slc}}\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{ij}}}^2+\underset{\mathrm{ij}\≠\mathrm{mn}}{\mathrm{\Σ}}{\left|\right|d_{\mathrm{ij}}^{\mathrm{sp}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{ij}}}^2\\ +{\left|\right|s_{\mathrm{mn}}(f(x_m,u_m)-x_n\left|\right|}_{{\mathrm{\Λ}}_{\mathrm{mn}}}^2+{\left|\right|1-s_{\mathrm{mn}}\left|\right|}_{{\mathrm{\Ξ}}_{\mathrm{mn}}}^2\\ =\underset{X,S}{\arg \min }h(X_{\mathrm{ij}\≠\mathrm{mn}},S_{\mathrm{ij}\≠\mathrm{mn}})+s_{\mathrm{mn}}^2{\mathrm{\χ}}_{l_{\mathrm{mn}}}^2+{(1-s_{\mathrm{mn}})}^2\mathrm{\Φ}\\ =\underset{X,S}{\arg \min h}(X_{\mathrm{ij}\≠\mathrm{mn}},S_{\mathrm{ij}\≠\mathrm{mn}})+g(X_{\mathrm{ij}=\mathrm{mn}},S_{\mathrm{ij}=\mathrm{mn}})\end{array}---\left(3\right);$

第二步：对误差函数求偏导；当优化器收敛的时候，对属于{X,S}的所有变量求偏导都等于0，因此，关于s_mn的偏导也等于0，如下式：

第三步：由关于s_mn的偏导也等于0，推导出当目标函数取得最小值的时候s的值：

$s=\frac{\mathrm{\Φ}}{{\mathrm{\χ}}_l^e+\mathrm{\Φ}};$

第四步：开关变量的取值范围是0～1，所以，综合第三步求出的s的值与s的取值范围，从而得到s的值： $s=\min (1,\frac{\mathrm{\Φ}}{\mathrm{\Φ}+{\mathrm{\χ}}_l^2});$

步骤2：g2o自带的优化策略，基于g2o平台，通过最小二乘法进一步进行优化，g2o平台中的优化策略包含有梯度下降法、牛顿法、高斯牛顿法、LM，构建出更符合真实路径情况的位姿节点路径。

2.根据权利要求1所述的基于g2o的SLAM后端优化算法方法，其特征在于，在步骤2中，g2o中选择高斯牛顿法进行优化，计算出使得误差函数最小的位姿序列解，构建出位姿节点路径。