一种融合视觉回环检测的激光SLAM方法及系统

技术特征：
1.一种融合视觉回环检测的激光slam方法，其特征在于，包括：步骤s1：激光里程计：通过3d激光雷达采集激光点云数据，提取所述激光点云数据的点云线特征与面特征，并以所述3d激光雷达的位移量为判断条件获取激光点云关键帧，同时将所述激光点云关键帧与局部点云地图、将所述局部点云地图与全局点云地图按照所述点云线特征与面特征进行匹配，以获取当前定位信息；步骤s2：地图构建：将每一个所述激光点云关键帧中包含的点云注册到世界坐标系下，经点云降采样后得到更新后的全局点云地图；步骤s3：视觉回环检测：使用视觉相机采集视觉图像，并融合所述激光点云关键帧构建点云-图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；步骤s4：全局图优化：基于所述回环轨迹上的所有的所述点云-图像融合关键帧建立位姿图模型，将所述回环约束作为约束边添加到所述位姿图模型中，使用gtsam库对所述位姿图模型进行优化得到优化后的点云-图像融合关键帧，并根据所述优化后的点云-图像融合关键帧更新当前所述定位信息与所述全局点云地图。2.根据权利要求1所述的融合视觉回环检测的激光slam方法，其特征在于，所述步骤s1中以所述3d激光雷达的位移量为判断条件获取激光点云关键帧，具体包括：计算所述3d激光雷达在运行时的位移量，当所述位移量超过阈值时，则建立一帧新的激光点云关键帧，并在当前工作周期[t
k
,t
k 1
]的t
k 1
时刻发布所述激光点云关键帧。3.根据权利要求1所述的融合视觉回环检测的激光slam方法，其特征在于，所述步骤s3中使用视觉相机采集视觉图像，并融合所述激光点云关键帧构建点云-图像融合关键帧，具体包括：步骤s301：获取距离时刻t
k 1
最近的t
p
时刻的视觉图像作为视觉图像关键帧与t
k 1
时刻发布的所述激光点云关键帧进行关联，构成初始点云-图像融合关键帧，其中，t
p
在工作周期[t
k
,t
k 1
]之间；步骤s302：对[t
k
,t
k 1
]时间内所述初始点云-图像融合关键帧中所包含激光点云数据通过线性插值方法进行运动补偿，首先如公式(1)所示，计算所述初始点云-图像融合关键帧在t
i
时刻的点云x
i
在[t
k
,t
i
]之间的位姿变换t
k,i
,同理可计算[t
k
,t
p
]之间的位姿变换t
k,p
如公式(2)：公式(2)：其中，t
k,k 1
是所述激光点云数据在激光点云时间戳[t
k
,t
k 1
]之间的位姿变换，可由激光里程计直接获得；将所述初始点云-图像融合关键帧中的所有点云x
i
利用位姿变换t
k,i
转换到t
k
时刻坐标下，得到点云x
k
，再将点云x
k
利用位姿变换t
k,p
转换到视觉图像时间戳t
p
时刻下，得到与视觉图像的时间戳同步的x
p
，实现了点云与视觉图像的时间同步，得到运动补偿后的点云-图像融合关键帧。4.根据权利要求1所述的融合视觉回环检测的激光slam方法，其特征在于，所述步骤s3
中检测回环信息、获取对应的回环约束及回环轨迹，具体包括：步骤s311：选用orb特征对已有的视觉图像样本集提取特征点集合，使用kmeans算法对所述特征点集合进行聚类，将聚类结果作为单词并以树形数据结构存储，构建视觉词典；步骤s312：对当前所述视觉图像关键帧进行orb特征提取得到特征点，在所述视觉词典中查询所述特征点对应单词出现的词频，采用tf-idf方法计算每个所述单词的分数，得到当前所述视觉图像关键帧的词袋向量v
k
(v1,v2,
···
v
n
)；步骤s313：遍历所有历史点云-图像融合关键帧，将其中所述历史点云-图像融合关键帧中所对应的视觉图像作为历史视觉图像关键帧，并获取所有所述历史视觉图像关键帧的词袋向量v
h
(v1,v2,
···
v
n
)，并根据公式(3)～(4)计算v
k
(v1,v2,
···
v
n
)与v
h
(v1,v2,
···
v
n
)的曼哈顿距离s得到二者图像相似度η，选取相似度η最高且大于阈值t的历史帧作为回环关键帧：帧作为回环关键帧：其中，v
i
∈v
k
，v
j
∈v
h
，s
(k,h)
为v
k
和v
h
之间的曼哈顿距离用来表示词袋向量距离；s
(k,k-1)
)表示当前所述视觉图像关键帧与前一视觉图像关键帧之间的词袋向量距离；步骤s314：将检测到所述回环关键帧的点云-图像融合关键帧的点云特征点投影到所述视觉图像关键帧，得到所述视觉图像关键帧特征点二维像素坐标，采用三点估计的方法对所述二维像素坐标进行深度赋值，可得到所述当前视觉图像关键帧特征点的三维空间坐标；步骤s315：利用所述特征点的三维空间坐标，将所述特征点与所述回环关键帧进行特征点匹配，应用pnp算法构建重投影误差模型，通过l-m算法优化求解式(x)最小二乘问题，可得到回环约束。5.一种融合视觉回环检测的激光slam系统，其特征在于，包括下述模块：激光里程计模块，用于通过3d激光雷达采集激光点云数据，提取所述激光点云数据的点云线特征与面特征，并以所述3d激光雷达的位移量为判断条件获取激光点云关键帧，同时将所述激光点云关键帧与局部点云地图、将所述局部点云地图与全局点云地图按照所述点云线特征与面特征进行匹配，以获取当前定位信息；地图构建模块，用于将每一个所述激光点云关键帧中包含的点云注册到世界坐标系下，经点云降采样后得到更新后的全局点云地图；视觉回环检测模块，用于使用视觉相机采集视觉图像，并融合所述激光点云关键帧构建点云-图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；全局图优化模块，用于基于所述回环轨迹上的所有的所述点云-图像融合关键帧建立位姿图模型，将所述回环约束作为约束边添加到所述位姿图模型中，使用gtsam库对所述位姿图模型进行优化得到优化后的点云-图像融合关键帧，并根据所述优化后的点云-图像融合关键帧更新当前所述定位信息与所述全局点云地图。

技术总结
本发明涉及一种融合视觉回环检测的激光SLAM方法及系统，其方法包括：S1：通过3D激光雷达采集激光点云数据，并构建激光点云关键帧，利用激光点云关键帧获取当前定位信息；S2：将每一个激光点云关键帧中包含的点云注册到世界坐标系下后获取全局点云地图；S3：使用视觉相机采集视觉图像，并融合激光点云关键帧构建点云-图像融合关键帧以检测回环信息，当检测到回环信息时，获取对应的回环约束及回环轨迹；S4：基于回环轨迹上的所有的点云-图像融合关键帧建立位姿图模型，利用回环约束作为约束边对位姿图模型进行优化，得到优化后的点云-图像融合关键帧以更新当前定位信息与全局点云地图。本发明提供的方法及系统可提高激光SLAM系统定位与建图的精度。SLAM系统定位与建图的精度。SLAM系统定位与建图的精度。


技术研发人员：李庆玲 王凤栋 芦迪 胡一鸣
受保护的技术使用者：中国矿业大学（北京）
技术研发日：2022.08.04
技术公布日：2022/10/25