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

摘要

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

主权项

一种基于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，构建出更符合真实路径情况的位姿节点路径。