一种基于回环检测的SLAM后端轨迹优化方法与流程

本发明涉及全自动自助移动机器人领域，更具体地，涉及一种基于回环检测的slam后端轨迹优化方法。

背景技术：

同时定位与地图构建(simultaneouslocationandmapping，简称slam)是利用机器人自身携带的传感器获取所在环境的信息估计机器人位姿并构建环境地图，是实现机器人完全自主移动的关键技术；本质上是一个状态估计问题，可以根据估计技术分为滤波方法和平滑方法。

常见的滤波方法有扩展卡尔曼滤波ekf(extendedkalmanfilters)、稀疏扩展信息滤波ekfs、粒子滤波等；最开始基于图优化的slam被认为非常的耗时，随着一些开源的稀疏矩阵运算算法的出现，大大提高了基于图优化slam的计算效率，基于图的slam优化方法逐渐成为了研究的主流和热点；一般的slam后端轨迹优化算法都是将前端获取的数据全部获取后，根据构建的位姿图得到一个总的目标函数。

虽然近年来研究者发现slam问题具有稀疏性，但是随着机器人运动的轨迹越来越长，我们最终都将面对增量线性方程：

hδx＝g

其中h是误差对于优化变量(路标及位姿节点)的雅克比矩阵，δx是对整体自变量的增量，误差项eij对变量的雅克比矩阵为jij(x)，对于整体的h：

h矩阵的形式如下：

其中方块表示误差对于优化变量(路标及位姿节点)的雅克比矩阵，每个小的方块都是一个矩阵，随着机器人轨迹的增加，h矩阵维度会越来越大，对线性方程hδx＝g的求解也会变得越来越复杂，计算量非常庞大，非常消耗资源和降低优化的效率。

技术实现要素：

本发明为克服上述现有技术所述的计算量大、资源消耗大、优化效率低的缺陷，提供一种基于回环检测的slam后端轨迹优化方法。

所述方法包括以下步骤：

s1：获取传感器数据；

s2：对s1所获取的数据进行数据处理，以获取机器人初始位姿；

s3：通过s2所得的初始位姿，构建位姿图模型；

s4：对s3所构建的位姿图模型，进行闭环检测；分割出位姿图模型的闭环部分和非闭环部分

s5：对检测后的不同部分分别进行优化。

slam被认为是实现真正全自主移动机器人的关键，主要分为前端数据预处理和后端优化两个部分，前端对通过各种传感器获得的数据进行预处理，根据相邻图像的信息粗略的估计出相机的运动，给后端提供较好的初始值，后端优化是利用前端提供的数据关系构建图模型，运用最优化方法对初值进行优化，实现更高精度的建图和定位。

本算法利用回环检测和图模型中的边的信息矩阵对全局误差项进行合理分配，对其中误差较大的边利用核函数(huber核或者cauchy核、tukey核)进行约束，保证每条边的误差不会大的没边儿淹盖掉其他的边，使整个优化过程边的更加稳健，从而实现对机器人轨迹快速并且有效的优化，解决slam在线实时性不好的问题，同时也能满足精度的要求，为解决slam后端优化问题提供了一种快速有效地解决方法。

优选地，步骤s2所述的数据处理过程为：首先通过前端的特征匹配，再使用对极几何、pnp或者icp算法来初步估计相机的运动(r，t)，获取机器人初始位姿，r是旋转矩阵，t是平移向量。

优选地，步骤s3步骤s3构建的位姿图模型为以机器人运动轨迹中的位姿为节点，以连接相邻的节点的轨迹为边的模型。

优选地，所述步骤s5的优化方法包括以下步骤：

s5.1：计算闭环部分的总误差；

s5.2：在误差分配的过程中加入核函数，对闭环部分按照边的权重合理分配误差；

s5.3：对闭环部分进行优化；

s5.4：在闭环部分优化完成之后，再进行非闭环部分的位姿优化；

s5.5：计算非闭环部分的误差；

s5.7：在误差分配的过程中加入核函数，对非闭环部分按照边的权重合理分配误差；

s5.8：对非闭环部分进行优化；

s5.9：优化后进入下一次闭环。

优选地，步骤s5.1和s5.5所述的总误差的计算公式为：

其中，其中eij表示误差项，ωij表示误差所占权重的大小，t为机器人位姿矩阵，ξ为t的四元数表示，c为机器人位姿节点的集合，i∈c，j∈c，假设i和j之间有相对运动，则

优选地，步骤s5.1闭环部分误差的计算公式为：

其中，t1为闭环初始节点位姿，tn为闭环位姿节点，tn为闭环部分最后一个位姿节点。

优选地，步骤s5.5非闭环部分误差的计算公式为：

其中，tm为非闭环最后一个节点位姿，t′m为tm经过闭环部分优化后的节点位姿。

优选地，步骤s5.2和s5.7所述的核函数公式如下，该函数对每一条边分配的误差进行约束：

其中，e为误差，δ为阈值。

与现有技术相比，本发明技术方案的有益效果是：本方法在每次检测到一个闭环就开始优化，每出现一个闭环，就计算闭环部分的总误差，然后计算非闭环部分总误差，合理利用前端提供的数据中边的权重这个信息，同时添加核函数(huber核)，不仅可以对位姿误差进行快速有效的分配，提高计算效率，增强机器人后端优化过程的在线实时性，同时又能保证精度。

附图说明

图1为基于回环检测的slam后端轨迹优化方法流程图。

图2为闭环检测的位姿图模型。

图3为核函数图。

图4为机器人真实轨迹图。

图5为机器人初始估计轨迹图。

图6为初试估计轨迹与真实估计之间的误差值图。

图7为位姿的局部更新量图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

本实施例提供一种基于回环检测的slam后端轨迹优化方法。

所述方法包括以下步骤：

s1：获取传感器数据；

s2：对s1所获取的数据进行数据处理，以获取机器人初始位姿；

s3：通过s2所得的初始位姿，构建位姿图模型；

s4：对s3所构建的位姿图模型，进行闭环检测；分割出位姿图模型的闭环部分和非闭环部分

s5：对检测后的不同部分分别进行优化。

(1)该方法的总体实现步骤：

基于回环检测的slam后端轨迹优化方法，该方法的主要流程如下：

首先通过前端的特征匹配，再使用对极几何、pnp或者icp算法来初步估计相机的运动(r，t)，构建位姿图，如图2所示，其中机器人的位姿为图中的节点，边连接相邻的节点，即为机器人位姿间的约束。

再通过回环检测检测到回环，针对每一个闭环的最后一个节点，计算它与闭环初始节点之间的位姿变换将其作为整个闭环轨迹相对运动累计的总误差，然后按照每一条边的权重将误差进行快速分配，权重越大，说明边的可靠性越高，距离真实值越近，相应的误差分配占比值就会越小，在此过程中加入核函数(huber核或者cauchy核、tukey核)，当分配到的误差大于一定的阈值时利用核函数进行约束，使误差变得平滑，保证每条边的误差不会大的没边儿淹盖掉其他的边，使整个优化过程边的更加稳健，然后将此误差值使用利群上的运算法则，右乘初始位姿，更新每一步的位姿，在闭环内部对每一个位姿节点进行优化。

(2)信息矩阵：

假设闭环初始位姿是ξ1，检测到闭环结束时的位姿时ξn，闭环模型如图2所示

假如没有累计误差的情况下ξ1＝ξn，由于累计误差的存在ξ1和ξn之间存在一个相对运动δξ1n，表示从1到n这个过程的总的相对误差，按照利群的写法为：误差项用e1n表示，对于非闭环部分也是一样。图优化问题最终转化成了一个最小二乘问题，机器人运动过程中最优轨迹就是求解机器人的位姿使得下面的误差平方函数最小：

其中eij表示相关节点间的位姿误差，ωij表示误差所占权重的大小，，误差的权重矩阵(信息矩阵)等于协方差矩阵的逆，由于图优化的每一条边表示一个测量值，如相邻位姿关系通过特征匹配得到的位姿变换矩阵，协方差越小，表示测量越准，信息权重就越大。

(3)核函数(huber)

利用回环检测我们可以求出一次回环累计的总体误差eij，利用信息矩阵边的权重信息，权重越大，说明边的可靠性越高，距离真实值越近，相应的误差分配值就会越小，在整个过程中加入核函数(huber核或者cauchy核、tukey核)，这里选择huber核：

可以看到，当误差e大于某个阈值δ后，函数增长由二次形式变成了一次形式，相当于限制了梯度的最大值。同时，huber核函数又是光滑的，可以方便求导，核函数的曲线图如图3所示。

(4)如图4-7所示，该方法的具体实现过程如下：

机器人的位姿由旋转矩阵r和平移向量t组成，记为t，假设机器人的闭环初试节点位姿为t1，闭环节点位姿为tn，从t1运动到tn中间要经过与闭环边条数相同的位姿变换，即t1t12......t(n-1)n＝tn，机器人从t1运动到tn，就会检测到一个回环，假如没有累计误差那么t1和tn应该是相同的位姿矩阵，但是由于运动过程中会产生累积的误差，所以t1和tn之间也会有相对位姿变换，这个相对位姿变化就当做闭环部分累积的误差，即t1t1n＝tn，那么相对位姿变换就可以写成即闭环部分总误差。对于非闭环部分假设非闭环最后一个位姿节点为tm，经过闭环部分优化后，该节点位姿变为t′m，那么对于非闭环部分有tmtmm′＝t′m，相对位姿变换为即为非闭环部分的总误差。

分别将闭环部分和非闭环部分的总误差按照各部分边的条数和边的权重分配到每一条边上，得到边的局部更新量，如图7所示，b-＞1-＞2-＞3-＞a，分别表示边局部更新量，每次更新的尺度又与位姿图中边的权重对应，权重越大，说明边的可靠性越高，越接近真实值，那么相对应的误差分配量就会小，添加huber核函数，设置阈值δ，开始分配误差，经过这种分布式更新各节点位姿，使得每一步的误差值都变小，最终使得b收敛到a，则机器人闭环和非闭环的运动轨迹都收敛到真实值，再将两个部分的轨迹结合形成整个的机器人较为准确的轨迹。

在闭环部分优化完成以后，再对非闭环部分进行优化，由于闭环与非闭环有一个位姿节点是公共的，利用这个公共的位姿节点优化前后的位姿变化，将其相对位姿变换当做非闭环部分的累计总误差，然后按照上述闭环部分相同的优化方法对闭环部分进行优化。

每次检测到回环就进行优化，这样就能避免机器人运动的轨迹越长，需要优化的变量就越多导致计算量庞大，从而使得优化慢效率低的问题。

将闭环部分和非闭环部分进行分割，简化优化过程。

相同或相似的标号对应相同或相似的部件；

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。