[发明专利]一种基于回环检测的SLAM后端轨迹优化方法

说明书

一种基于回环检测的SLAM后端轨迹优化方法，包括以下步骤：S1：获取传感器数据；S2：对S1所获取的数据进行数据处理，以获取机器人初始位姿；S3：通过S2所得的初始位姿，构建位姿图模型；S4：对S3所构建的位姿图模型，进行闭环检测；分割出位姿图模型的闭环部分和非闭环部分；S5：对检测后的不同部分分别进行优化；本方法在每次检测到一个闭环就开始优化，每出现一个闭环，就计算闭环部分的总误差，然后计算非闭环部分总误差，合理利用前端提供的数据中边的权重这个信息，同时添加核函数(Huber核)，不仅可以对位姿误差进行快速有效的分配，提高计算效率，增强机器人后端优化过程的在线实时性，同时又能保证精度。

技术领域

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

背景技术

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

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

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

HΔx＝g

其中H是误差对于优化变量(路标及位姿节点)的雅克比矩阵，ΔX是对整体自变量的增量，误差项e_ij对变量的雅克比矩阵为J_ij(x)，对于整体的H：

H矩阵的形式如下：

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

发明内容

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

所述方法包括以下步骤：

S1：获取传感器数据；

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

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

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

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

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