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

权利要求书

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)：

X*,S*=argminX,SΣi||di0d0||Σi2+Σij≠mn||dijslc||Λij2+Σij≠mn||dijsp||Ξij2+||smn(f(xm,um)-xn||Λmn2+||1-smn||Ξmn2=argminX,Sh(Xij≠mn,Sij≠mn)+smn2χlmn2+(1-smn)2Φ=argminhX,S(Xij≠mn,Sij≠mn)+g(Xij=mn,Sij=mn)---(3);]]>

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

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

s=Φχle+Φ;]]>

第四步：开关变量的取值范围是0～1，所以，综合第三步求出的s的值与s的取值范围，从而得到s的值：s=min(1,ΦΦ+χl2);]]>

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

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