[发明专利]一种用于室内外场景的人为回环即时定位和建图方法及系统

说明书

一种用于室内外场景的人为回环即时定位和建图方法，其特征在于，包括以下步骤：步骤一，定义位姿关系SLAM初始因子图；步骤二，采集机器人定位数据，对第i帧定位数据输入校正因子，通过EM算法估算校正因子；步骤三，通过矩阵变换串联机器人前后变换的位姿，优化校正因子；步骤四，通过校正因子约束优化机器人位姿，消除累计误差，得到全局一致性地图。本申请消除累计误差，得到全局一致性地图。

技术领域

本发明设计一种用于室内外场景的人为回环即时定位和建图方法及系统，属于机器人应用技术领域。

背景技术

构建大规模且全局一致的地图一直是机器人导航建图的难题所在。全局一致地图的制定可协助机器人较为准确地按照指令进行操作，进而降低位姿估计误差。传统SLAM(Simultaneous Localization And Mapping，SLAM)技术仅仅采用视觉里程计来判断机器人在地图中的坐标，效果不理想，原因主要有两个：一是早期的视觉里程计由编码器实现，位姿估计漂移太快；二是视觉里程计原理是仅计算相邻图像间的位置关系，这样就容易导致上一个图像误差引入到当前图像的位置关系中。机器人经长时间运行后累积的偏差会越来越大，最终将导致无法建立大规模且全局一致的地图。

为了解决上述问题，回环检测技术使用外观信息检测机器人是否回到了先前位置，以期消除累积误差。难点在于一开始的微量误差不易被察觉，机器人绕环路一圈后因误差积累而无法闭合环路。特别是大范围场景，即使采用高精度激光雷达也难以避免。现有技术专利“利用激光SLAM在长走廊建图的方法、装置、系统、存储介质CN201910595880.4”、“一种基于距离度量学习的视觉SLAM回环检测方法CN201910575905.4”都是被动式的算法调整，建图过程中的误差无法实时控制，易导致构建的矢量地图达不到预期效果。

现有技术专利“基于单链序贯回溯Q学习的移动机器人路径规划算法CN102799179A”利用单链序回溯Q学习方法循环往复地从初始状态开始寻找到达目标状态的路径，并不断迭代和优化状态的Q值，直到收敛为止。该方法属于被动式地通过算法寻找最优参数，并没有能实时校准机器人在每一步迭代过程中产生的定位误差。

发明内容

本发明的目的在于：针对现有技术的不足，提供一种用于室内外场景的人为回环即时定位和建图方法及系统，能实时校准机器人在每一步迭代过程中产生的定位误差。

本发明具体技术方案为：

一种用于室内外场景的人为回环即时定位和建图方法，其特征在于，包括以下步骤：

步骤一，定义位姿关系SLAM初始因子图；

步骤二，采集机器人定位数据，对第i帧定位数据输入校正因子，通过EM算法估算校正因子；

步骤三，通过矩阵变换串联机器人前后变换的位姿，优化校正因子；

步骤四，通过校正因子约束优化机器人位姿，消除累计误差，得到全局一致性地图。

步骤一具体包括以下步骤：

定义机器人在地图中的初始位姿及其约束关系的初始因子图G₀＝{X,F}；其中X为机器人初始行驶轨迹(机器人初始行驶轨迹不包括后续回环过程中的行驶轨迹)上的位姿估计集合，F＝{R}，R表示测量值和观测值之间的相对位姿(相对位姿约束指的是观测值与测量值之间的相对位姿)。

步骤二具体包括以下步骤：

(201)，采集机器人定位数据，在机器人定位数据的第i帧输入用于修正地图(地图指的就是机器人当前所位于的矢量地图，包含了机器人当前位姿数据)的校正因子；