一种连通分量动态合并的多机3D激光SLAM方法及系统技术方案

本发明专利技术公开了一种连通分量动态合并的多机3D激光SLAM方法及系统，通过各个智能体获取感知数据执行单机SLAM，得到SLAM地图与激光特征；将数据传输给其它智能体；各个智能体根据自身与其它智能体SLAM地图的特征匹配优化与融合，实现多智能体的激光SLAM任务。本发明专利技术通过基于匹配相似度约束的动态连通图构造与合并算法，对多个智能体的子图进行动态逐步合并，解决了由于多机SLAM实际运行时因一个智能体与多个其它智能体都存在重合时出现结果混乱的情况；对激光点云特征引入质心特征分析和主成分分析，提高了回环检测效率与准确度；引入ros1

【技术实现步骤摘要】
一种连通分量动态合并的多机3D激光SLAM方法及系统


[0001]本专利技术属于多机器人3D激光SLAM领域，涉及一种连通分量动态合并的多机3D激光SLAM方法及系统。

技术介绍

[0002]随着激光雷达技术发展，同步定位与建图(SLAM)被广泛应用于无人驾驶和环境测量领域。在探索野外未知环境、矿洞或者战场以及救援等大规模且需要时间效率的场景中，使用单一智能体(如地面无人车辆)进行建图工作会增加存储压力并且难以满足时间效率，因此需要采用多个智能体协同建图解决以上问题。传统的中心式多机SLAM系统需要一个中心服务器，难以保障在复杂大场景中的鲁棒性。随着5G通信技术的发展以及智能体计算感知能力的提高，为分布式多智能体的3D激光雷达SLAM提供了可能。现有DiSCo
‑
SLAM算法通过两阶段因子图优化方法提供了稳定的定位结果，但是由于多机SLAM实际运行时无法保证匹配的先后顺序，从而导致当一个智能体与两个或多个其它智能体都存在重合时出现坐标转移混淆的情况，造成最终全局地图的结果紊乱。原有算法采用Scan Context特征作为回环检测，由于没有统一的观测坐标，当机器人视角变化时同一地点的SC特征将不一样，导致搜索效率降低。与此同时，传统的ROS1通信采取主从机通信机制，不能满足单个智能体独立运行的场景。

技术实现思路

[0003]本专利技术的目的在于解决现有技术中多机SLAM实际运行时无法保证匹配的先后顺序，从而导致当一个智能体与两个或多个其它智能体都存在重合时出现坐标转移混淆的情况，造成最终全局地图的结果紊乱的问题，提供一种连通分量动态合并的多机3D激光SLAM方法及系统。
[0004]为达到上述目的，本专利技术采用以下技术方案予以实现：
[0005]本专利技术提出的一种连通分量动态合并的多机3D激光SLAM方法，包括以下步骤：
[0006]根据多个智能体的激光里程计数据以及点云地图进行回环检测，获取任意两个智能体之间的相对位姿转换矩阵；
[0007]对相对位姿转换矩阵进行处理，得到各个智能体的因子图；
[0008]采用动态连通图构造与合并算法对各个智能体的因子图进行处理，获取多智能体整体激光SLAM地图。
[0009]优选地，根据惯性测量单元获取智能体的激光里程计数据；
[0010]根据激光雷达数据获取智能体的点云地图。
[0011]优选地，采用改进的Scan Context特征方法对多个智能体的激光里程计数据以及点云地图进行回环检测；
[0012]改进的Scan Context特征方法具体步骤如下：
[0013]1)在前端获得当前时刻激光雷达点云数据，将雷达数据沿Z轴方向投影，得到激光
雷达的鸟瞰图视角的二维点云；
[0014]2)将所述二维点云坐标原点平移至质心；
[0015]3)对所述二维点云进行主成分分析，获得分别表示三个主轴方向的三个特征向量；其中，为了确保Z轴垂直且三个轴相互垂直，通过取Z轴和X轴的叉积人为指定Y轴，指定在X主轴上点云数量较多的一侧为正方向；
[0016]4)将激光雷达的鸟瞰图视角的点云按照极坐标从选取的X主轴正方向开始等角度与等距离划分为N
×
M个扇形栅格，取每个栅格中Z轴高度最高的点，得到当前帧激光雷达点云改进的Scan Context特征，将特征矩阵逐点存到一个N
×
M维的kd树进行维护；
[0017]5)当接收到新的改进的Scan Context特征时，在kd树中采用最邻近算法检索以当前特征相邻近的若干个特征，再采用检索出的特征与当前特征计算余弦相似度；
[0018]6)使用基于改进的Scan Context特征方法检测到潜在回环后，使用潜在回环时刻的点云与当前时刻点云进行ICP匹配配准，并得到转移矩阵T
*
和相似度S
*
，若相似度S
*
满足给定的接受阈值τ，认为回环检测成功，将历史帧信息、当前帧信息、转移矩阵T
*
和相似度S
*
发送给其他智能体。
[0019]优选地，余弦相似度的计算方法如下：
[0020][0021]其中，I
q
为当前特征矩阵，I
c
为候选特征矩阵，为I
q
中第j行向量，||
·
||表示取向量的模，N
s
表示向量个数，d(I
q
,I
c
)表示两个向量的余弦相似度；通过余弦相似度得到与当前特征相似度最高的历史特征的索引c
*
及其余弦相似度为d
*
；若d
*
<τ时，则第c
*
个历史位姿与当前位姿存在潜在回环关系，τ为给定的接受阈值，根据接受阈值τ得到各个智能体经过回环区域的数据。
[0022]优选地，基于ROS2
‑
foxy的Fast
‑
DDS通信中间件，通过ros1_bridge搭建基于ros话题通信的通信架构，实现多个智能体的通信。
[0023]优选地，采用成对一致测量集最大化检测方法对相对位姿转换矩阵进行处理；
[0024]进行一致性检测满足环路闭合条件：
[0025][0026]其中，是智能体β在时间戳l和j之间的智能体内的位姿变换，是智能体β在时间戳j和智能体α在时间戳i的智能体间的位姿变换，是智能体α在时间戳i和k之间的智能体内的位姿变换，是智能体β在时间戳l和智能体α在时间戳k的智能体间的位姿反变换，∈是一个给定的阈值；
[0027]将所有观测重合位置作为全局因子图的节点，对于经过处理的回环点，将各个回环点作为全局因子图的变量节点，回环检测得到的两个回环点之间的相对位姿变换作为对应两个变量节点间的二元因子，最终得到一个包含各个智能体之间全局变换信息的因子图。
[0028]优选地，基于匹配相似度约束的动态连通图构造与合并算法对各个智能体的因子
图进行处理的方法如下：
[0029]1)根据智能体数量初始化各个节点不相连的图结构，各个智能体以各自的初始坐标作为坐标原点；
[0030]2)当接收到回环点的位姿变换结果，将各个回环点所在智能体对应的图节点相连，得到连通分量；
[0031]3)使用深度优先算法对图结构进行连通图检测，对于各个连通分量以最先匹配上的智能体作为全局坐标原点，将连通分量上其余智能体坐标根据转移矩阵转移到全局坐标原点上，作为后续因子图优化的近似初始值。
[0032]本专利技术提出的一种连通分量动态合并的多机3D激光SLAM系统，其特征在于，包括：
[0033]位姿转换矩阵获取模块，所述位姿转换矩阵获取模块用于根据多个智能体的激光里程计数据以及点云地图进行回环检测，获取任意两个智能体之间的相对位姿转换矩阵；
[0034]因子图获取模块，所述因子图获取模本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种连通分量动态合并的多机3D激光SLAM方法，其特征在于，包括以下步骤：根据多个智能体的激光里程计数据以及点云地图进行回环检测，获取任意两个智能体之间的相对位姿转换矩阵；对相对位姿转换矩阵进行处理，得到各个智能体的因子图；采用动态连通图构造与合并算法对各个智能体的因子图进行处理，获取多智能体整体激光SLAM地图。2.根据权利要求1所述的连通分量动态合并的多机3D激光SLAM方法，其特征在于，根据惯性测量单元获取智能体的激光里程计数据；根据激光雷达数据获取智能体的点云地图。3.根据权利要求1所述的连通分量动态合并的多机3D激光SLAM方法，其特征在于，采用改进的Scan Context特征方法对多个智能体的激光里程计数据以及点云地图进行回环检测；改进的Scan Context特征方法具体步骤如下：1)在前端获得当前时刻激光雷达点云数据，将雷达数据沿Z轴方向投影，得到激光雷达的鸟瞰图视角的二维点云；2)将所述二维点云坐标原点平移至质心；3)对所述二维点云进行主成分分析，获得分别表示三个主轴方向的三个特征向量；其中，为了确保Z轴垂直且三个轴相互垂直，通过取Z轴和X轴的叉积人为指定Y轴，指定在X主轴上点云数量较多的一侧为正方向；4)将激光雷达的鸟瞰图视角的点云按照极坐标从选取的X主轴正方向开始等角度与等距离划分为N
×
M个扇形栅格，取每个栅格中Z轴高度最高的点，得到当前帧激光雷达点云改进的Scan Context特征，将特征矩阵逐点存到一个N
×
M维的kd树进行维护；5)当接收到新的改进的Scan Context特征时，在kd树中采用最邻近算法检索以当前特征相邻近的若干个特征，再采用检索出的特征与当前特征计算余弦相似度；6)使用基于改进的Scan Context特征方法检测到潜在回环后，使用潜在回环时刻的点云与当前时刻点云进行ICP匹配配准，并得到转移矩阵T
*
和相似度S
*
，若相似度S
*
满足给定的接受阈值τ，认为回环检测成功，将历史帧信息、当前帧信息、转移矩阵T
*
和相似度S
*
发送给其他智能体。4.根据权利要求3所述的连通分量动态合并的多机3D激光SLAM方法，其特征在于，余弦相似度的计算方法如下：其中，I
q
为当前特征矩阵，I
c
为候选特征矩阵，为I
q
中第j行向量，||
·
||表示取向量的模，N
s
表示向量个数，d(I
q
,I
c
)表示两个向量的余弦相似度；通过余弦相似度得到与当前特征相似度最高的历史特征的索引c
*
及其余弦相似度为d
*
；若d

【专利技术属性】
技术研发人员：魏平，刘克勤，杨腾，洪灿斌，程翔，辛景民，郑南宁，
申请(专利权)人：北京大学，
类型：发明
国别省市：