[发明专利]一种基于最小回环检测的协同建图方法

说明书

本发明属于机器人感知算法领域，涉及一种基于最小回环检测的协同建图方法。首先，单个机器人基于扫描匹配和图优化生成局部地图；其次，多个机器人之间通过网络交换数据；然后，利用坐标系统一算法进行坐标变换，最后，在统一后的坐标系寻找最小回环并进行优化。本发明提供的一种基于最小回环检测的协同建图方法，在多机器人参与建图的情况下，充分利用机器人轨迹之间的交集，寻找最小回环构建约束，保证后端优化处在小范围、多频次的情况，从而达到减少累计误差的传播和降低后端优化的耗时的效果，最终建立出更加符合真实场景的环境地图。

技术领域

本发明属于机器人感知算法领域，尤其涉及多机器人协同建图的方法，更具体地，涉及一种基于最小回环检测的协同建图方法。

背景技术

SLAM(simultaneous localization and mapping)即时定位与地图构建，是自主移动机器人和汽车自动驾驶的关键技术。点云匹配是将两个或两个以上坐标系中的大容量三维空间数据点的集合转换到统一坐标系中的数学计算过程，实际上就是要找出两个坐标系之间的变换关系。这种关系可以用一个旋转矩阵R和平移向量T来描述，点云匹配就是要求解出(R，T)。目前经常使用的点云匹配算法包括：最近点迭代算法(ICP)、RANSAC等。图优化是SLAM领域常用的后端优化的方式，其主要的实现方式是通过对整个系统构建约束方程，并通过特定的数学方法求解该约束方程，达到全局误差最小的结果。

专利CN108362294A，公开日为20180803，公开了一种应用于自动驾驶的多车协同建图方法，利用了激光传感器获得周围环境的点云信息，经过算法处理，产生出整个环境的全局地图。但是该专利没有考虑回环优化的情况。当存在多个节点的情况下，某节点与其他节点的转换关系都是通过自身的匹配算法计算得到，长时间长距离的距离运行会导致误差的积累。

发明内容

本发明为克服上述现有技术中的至少一个缺陷，提供一种基于最小回环检测的协同建图方法，引入图优化方法应用到最小回环中去，避免了大范围场景下的一次优化计算量过大，耗时过长。

为解决上述技术问题，本发明采用的技术方案是：一种基于最小回环检测的协同建图方法，首先，单个机器人基于扫描匹配和图优化生成局部地图；其次，多个机器人之间通过网络交换数据；然后，利用坐标系统一算法进行坐标变换，即坐标系的统一与数据融合；最后，在统一后的坐标系寻找最小回环并进行优化。

进一步的，将SLAM建图的全过程分为前端和后端两个部分，前端通过连续时刻的激光数据匹配和单车行进轨迹中检测到的回环来确定关键帧；后端通过通用图优化的方法来校正所有机器人的位姿信息，然后根据校正前后的位姿偏移量，对相应于该位姿的点云数据作出同样的偏移变化，得到最终的地图。

在其中一个实施例中，所述的单个机器人基于扫描匹配和图优化生成局部地图，采用16线的激光雷达，在距离较近的场景下能够获得丰富的点云，利用最近点迭代算法ICP或正态分布变换算法NDT针对连续时间的点云数据进行帧间匹配，根据设定的阈值抽取出关键帧，构成图的顶点；这些顶点包含的点云数据，同时也是当前节点生成的局部地图。

在其中一个实施例中，多个机器人之间通过网络交换数据中，所述的数据除了包括当前节点的点云数据外，还包括了采集这帧点云时的车辆ID、时间戳、IMU数据、GPS数据、激光里程计数据；这些数据将会一并打包分享给处于和当前节点距离阈值内的其他节点。

在其中一个实施例中，所述的利用坐标系统一算法进行坐标变换，当其他节点收到数据后，首先根据车辆ID确认是否已知转换关系，确认的话则直接合并到自己的局部地图中；否则进行转换关系的计算，成功的话接下来的步骤和已知转换关系一样，失败的话则暂存下来，等待收到更多数据后一并计算。

在其中一个实施例中，所述的在统一后的坐标系寻找最小回环并进行优化，包括：

在初始情况下，整个生成森林以车辆数为单位，构成连通分支，每一辆车的初始关键帧构成了当前森林的当前连通分支的第一个节点；