基于激光雷达的多机协同在线建图方法、系统及终端设备与流程

本发明涉及自动驾驶，具体是一种基于激光雷达的多机协同在线建图方法、系统及终端设备。

背景技术：

1、在自动驾驶、机器人领域中，基于激光雷达，融合惯性测量单元(imu)、轮速编码器等多传感器信息，可运用同时建图与定位(slam)技术实现移动载体运动过程的位姿估计和场景点云地图的构建，从而实现对实际场景环境的勘测和建模。然而，对于大场景和极端场景，使用单机建图的效率较低，难以实现对大范围场景的单次采集完成地图构建。

2、针对多机协同建图的技术研究难点在于各载体的相对位置关系未知，各载体在各自的局部坐标系下的建图结果需统一到同一坐标系，即需要估计各载体在的局部坐标系原点到全局坐标系的相对位置变换关系。此外，随着各载体的运动推移，通过里程计估算的位姿存在累计误差，在实现局部到全局位姿变换估计的同时需通过引入回环修正消除各载体位姿估计结果的累计误差，对各载体估计的里程计位姿进行修正，最终得到消除累计误差后全局一致的点云地图。为了实现多机协同在线建图，如何有效的减少在线传输的数据效率和稳定性实现高效实时的信息交互也是应用的关键。

3、目前公开的多机协同建图的专利中，cn114923477a提出了一种融合视觉与激光slam技术的多维度空地协同建图系统和方法，该方法通过地面站实现多载体的控制和信息处理实现建图，但未解决随着里程计推移的地图累计误差的问题。cn108362294a提出了一种应用于自动驾驶的多车协同建图方法使用gps进行多车局部地图的协同建图，没有考虑建图过程的累计误差问题且不适用于gps信号差的场景。

技术实现思路

1、针对上述现有技术中的不足，本发明提供一种基于激光雷达的多机协同在线建图方法、系统及终端设备，不依赖gps等传感器信息即可进行载体间的相对定位，适用于地下勘探场景、室内场景等gps信号弱的场景，且在线数据传输仅包括里程计位姿信息、描述符信息、构成回环约束的点云帧信息，数据量小，利于实时数据传输。

2、为实现上述目的，本发明提供一种基于激光雷达的多机协同在线建图方法，将所有载体中的一个定义为主载体，将其它载体定义为分载体，并将主载体的局部坐标系定义为全局坐标系，所述多机协同在线建图方法包括如下步骤：

3、以预设频率进行基于描述符的回环匹配，筛选得到与所述主载体关联的分载体，并将其定义为关联载体；

4、将所有关联载体在对应局部坐标系下的里程计位姿结果转换至全局坐标系下，并以预设频率进行全局回环检测；

5、基于全局回环检测得到的存在回环关系的两帧点云帧构建相对位姿约束关系，并基于所述相对位姿约束关系对主载体及所有关联载体点云帧的里程计位姿结果进行全局位姿优化；

6、基于全局位姿优化后的里程计位姿结果将主载体及所有关联载体的点云帧进行拼接，即得到场景点云地图。

7、在其中一个实施例，所述基于描述符的回环匹配，包括：

8、基于主载体所有历史点云帧的描述符构建描述符搜索库，并对进行回环匹配的任一分载体进行如下处理：

9、步骤101，将分载体的候选点云帧定义为点云帧#1，并获取点云帧#1的描述符，定义为描述符#1；

10、步骤102，在所述描述符搜索库中搜索与描述符#1相似的描述符：

11、若所述描述符搜索库中存在与描述符#1相似的描述符，将其定义为描述符#2，并进行步骤103；

12、否则，结束当前分载体的回环匹配；

13、步骤103，将描述符#2对应的点云帧定义为点云帧#2，并对点云帧#1与点云帧#2进行点云匹配：

14、若匹配成功，则判定当前分载体为关联载体，并将当前分载体所有历史点云帧的描述符加入所述描述符搜索库；

15、否则，结束当前分载体的回环匹配。

16、在其中一个实施例，所述全局回环检测包括：

17、将主载体与所有关联载体在上一次全局回环检测之前的所有历史点云帧加入点云帧搜索库，将主载体与所有关联载体在上一次全局回环检测之后采集的所有点云帧定义为新点云帧，并对每一新点云帧进行如下处理：

18、步骤201，在所述点云帧搜索库中搜索与当前新点云帧相似的历史点云帧：

19、若所述点云帧搜索库中存在与当前新点云帧相似的历史点云帧，则进行步骤202；

20、否则，结束当前新点云帧的全局回环检测；

21、步骤202，对当前新点云帧与对应历史点云帧进行点云匹配：

22、若匹配成功，则判定当前新点云帧与对应历史点云帧之间存在回环关系；

23、否则，结束当前新点云帧的全局回环检测。

24、在其中一个实施例，步骤201中，若所述点云帧搜索库中存在一历史点云帧与当前新点云帧之间的欧式距离小于欧式距离阈值，则判定该历史点云帧与当前新点云帧相似。

25、在其中一个实施例，所述全局位姿优化包括：

26、在全局坐标系下，基于同一载体相邻时刻两个点云帧的相对位姿变化构建里程计的相邻位姿边约束残差，基于存在回环关系的两点云帧的相对位姿构建回环位姿边约束残差；

27、联合所有相邻位姿边约束残差与所有回环位姿边约束残差构建总残差，并以最小化总残差为目标函数、以主载体及所有关联载体点云帧的里程计位姿结果为控制参数进行优化，即得到主载体及所有关联载体全局位姿优化后点云帧的里程计位姿结果。

28、在其中一个实施例，所述目标函数为：

29、

30、其中，f(x)为目标函数，x为待求解的主载体及所有关联载体点云帧的里程计位姿结果，n、m为点云帧对应的序号，为相邻位姿边约束残差，为回环位姿边约束残差，α、β为权重系数，n为点云帧的数量。

31、在其中一个实施例，在构建所述目标函数的过程中，基于点云匹配解算的相似性自适应调整权重系数α、β，即每相邻时刻两个点云帧的相对位姿变化构成的相邻位姿边约束残差都具有一个权重系数αn，每一组存在回环关系的两点云帧构成的回环位姿边约束残差存在一权重系数βm，从而目标函数进一步为：

32、

33、其中，αn为第n组相邻时刻两个点云帧的相对位姿变化构成的相邻位姿边约束残差的权重系数，βm为第m组存在回环关系的两点云帧构成的回环位姿边约束残差的权重系数；

34、权重系数αn、βm为：

35、

36、

37、其中，为第n组相邻时刻两个点云帧的相似性得分，为第m组存在回环关系的两点云帧的相似性得分，c1、c2为比例系数。

38、在其中一个实施例，对于未被定义为关联载体的分载体，基于局部回环检测对分载体点云帧的里程计位姿结果进行局部位姿优化。

39、在其中一个实施例，所述局部位姿优化包括：

40、步骤301，获取分载体的当前点云帧，并在分载体的所有历史点云帧中基于欧式距离进行回环搜索：

41、若搜索到与当前点云帧欧式距离小于预设阈值的历史点云帧，则进行步骤302；

42、否则，结束分载体当前帧的局部位姿优化；

43、步骤302，对分载体的当前点云帧与对应历史点云帧进行点云匹配：

44、若匹配成功，则基于当前新点云帧与对应历史点云帧之间的相对位姿构建位姿约束，并基于所述位姿优化对分载体所有历史点云帧的里程计位姿结果进行优化，以消除累计误差，完成分载体当前帧的局部位姿优化；

45、否则，结束分载体当前帧的局部位姿优化。

46、为实现上述目的，本发明还提供一种基于激光雷达的多机协同在线建图系统，采用上述的方法的部分或全部步骤，所述多机协同在线建图系统包括：

47、回环匹配模块，用于以预设频率进行基于描述符的回环匹配，筛选得到与所述主载体关联的分载体，并将其定义为关联载体；

48、全局回环检测模块，用于将所有关联载体在对应局部坐标系下的里程计位姿转换至全局坐标系下，并以预设频率进行全局回环检测；

49、全局位姿优化模块，用于根据全局回环检测得到的存在回环关系的两帧点云帧构建相对位姿约束关系，并基于所述相对位姿约束关系对主载体及所有关联载体点云帧的里程计位姿结果进行全局位姿优化；

50、点云地图构建模块，用于根据全局位姿优化后点云帧的里程计位姿结果将主载体及所有关联载体的点云帧进行拼接，即得到场景点云地图。

51、为实现上述目的，本发明还提供一种终端设备，所述终端设备为计算机、无人车、无人机、无人驾驶设备或移动机器人，所述终端设备上设有：

52、存储器，用于存储程序；

53、处理器，用于执行所述存储器存储的所述程序，当所述程序被执行时，所述处理器用于执行如上述的方法的部分或全部步骤。

54、本发明具有如下有益技术效果：

55、1.本发明在关联载体匹配过程中采用了基于描述符的回环匹配，并基于里程计位姿结果进行全局回环检测，进而能够通过在线传输里程计位姿结果和描述符等小数据即可实现回环检测，从而实现未知位置载体间的坐标统一，数据量小，利于实时数据传输；

56、2.本发明根据存在回环关系的两帧点云帧构建相对位姿约束关系，并以该相对位姿约束关系对主载体及所有关联载体点云帧的里程计位姿结果进行全局位姿优化，从而有效地维护了全局一致的位姿图结果；

57、3.本发明通过在多移动载体间实现在线协同建图，可有效实现大规模场景的分布式在线建图，且不依赖gps等传感器信息进行载体间的相对定位因此适用于地下勘探场景、室内场景等gps信号弱的场景；

58、4.本发明在优选方案中用过在分载体内部进行局部回环检测以及局部位姿优化，从而有效地修正里程计的累计误差，提升载体间相对定位的精度。