[发明专利]一种基于连续运动模型的激光SLAM方法及系统

说明书

本发明公开了一种基于连续运动模型的激光SLAM方法及系统，该方法包括：获取激光点云数据并提取特征点；基于捆绑调整对特征点进行特征关联，得到地标特征；基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差；构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度；基于滑动窗口选取关键帧和对地标特征的观测残差进行计算优化，并结合地标特征的位姿与速度构建地图。该系统包括：特征点提取模块、地标关联模块、残差构建模块、联合优化模块和建图模块。通过使用本发明，能够降低建图过程中的累积误差且减少计算量。本发明作为一种基于连续运动模型的激光SLAM方法及系统，可广泛应用于同步定位与建图领域。

技术领域

本发明涉及同步定位与建图领域，尤其涉及一种基于连续运动模型的激光SLAM方法及系统。

背景技术

SLAM是现在自动驾驶和智能机器人领域的一个基础性模块。SLAM不仅能够实时提供关于智能体自身位置的信息，还能够实时绘制周围环境的三维地图，对智能体的导航，控制以及决策都非常重要；通常，SLAM技术可以基于视觉或者LiDAR来实现；近年来，同时使用两种传感器的方案也开始发展了起来，由于依赖于稳定的光照，并且对光照变化，环境纹理等非常敏感，在夜晚甚至无法工作，基于视觉的方案被严重限制了可以应用的范围；相反，LiDAR是一种主动设备，可以一定程度上降低对环境的要求，并且LiDAR可以直接获取精确三维信息，因而得到广泛的应用。

基于LiDAR的SLAM算法的核心是里程计，而激光SLAM在特征稀疏的场景中，容易丢失跟踪目标，导致点云匹配出现失误，从而产生误差，这些误差会逐渐累积，导致最终地图出现重叠的问题，并且，激光SLAM在动态环境中，动态物体也容易对点云匹配产生影响，无法估计正确的位姿影响了算法的鲁棒性和精度。

发明内容

为了解决上述技术问题，本发明的目的是提供一种基于连续运动模型的激光SLAM方法及系统，能够降低建图过程中的累积误差且减少计算量。

本发明所采用的第一技术方案是：一种基于连续运动模型的激光SLAM方法，包括以下步骤：

获取激光点云数据并进行特征提取，得到特征点；

基于捆绑调整对特征点进行特征关联，得到地标特征；

基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差；

构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度；

基于滑动窗口选取关键帧和对地标特征的观测残差进行计算优化，并结合地标特征的位姿与速度构建地图。

进一步，所述获取激光点云数据并进行特征提取，得到特征点这一步骤，具体包括：

获取激光点云数据并基于球面投影将每一帧激光点云数据投影到球形平面上，得到投影图；

基于广度优先搜索算法对投影图进行特征提取，得到平面特征点和直线特征点；

整合平面特征点和直线特征点，得到特征点。

进一步，所述基于捆绑调整对特征点进行特征关联，得到地标特征这一步骤，具体包括：

获取现有地标特征，并在当前帧中选出与现有地标特征对应的特征点；

跟踪当前帧中选出的特征点，加入现有地标特征；

搜索当前帧中未被跟踪的特征点的邻居特征点，记为邻域点集；

对邻域点集的协方差矩阵进行特征值分解，得到特征值；

根据特征值的大小将未被跟踪的特征点划分为新的平面地标特征和新的直线地标特征；

整合现有地标特征与新的平面地标特征和新的直线地标特征，得到地标特征。