本发明专利技术公开了一种基于连续运动模型的激光SLAM方法及系统，该方法包括：获取激光点云数据并提取特征点；基于捆绑调整对特征点进行特征关联，得到地标特征；基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差；构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度；基于滑动窗口选取关键帧和对地标特征的观测残差进行计算优化，并结合地标特征的位姿与速度构建地图。该系统包括：特征点提取模块、地标关联模块、残差构建模块、联合优化模块和建图模块。通过使用本发明专利技术，能够降低建图过程中的累积误差且减少计算量。本发明专利技术作为一种基于连续运动模型的激光SLAM方法及系统，可广泛应用于同步定位与建图领域。定位与建图领域。定位与建图领域。
【技术实现步骤摘要】
一种基于连续运动模型的激光SLAM方法及系统
[0001]本专利技术涉及同步定位与建图领域，尤其涉及一种基于连续运动模型的激光SLAM方法及系统。
技术介绍
[0002]SLAM是现在自动驾驶和智能机器人领域的一个基础性模块。SLAM不仅能够实时提供关于智能体自身位置的信息，还能够实时绘制周围环境的三维地图，对智能体的导航，控制以及决策都非常重要；通常，SLAM技术可以基于视觉或者LiDAR来实现；近年来，同时使用两种传感器的方案也开始发展了起来，由于依赖于稳定的光照，并且对光照变化，环境纹理等非常敏感，在夜晚甚至无法工作，基于视觉的方案被严重限制了可以应用的范围；相反，LiDAR是一种主动设备，可以一定程度上降低对环境的要求，并且LiDAR可以直接获取精确三维信息，因而得到广泛的应用。
[0003]基于LiDAR的SLAM算法的核心是里程计，而激光SLAM在特征稀疏的场景中，容易丢失跟踪目标，导致点云匹配出现失误，从而产生误差，这些误差会逐渐累积，导致最终地图出现重叠的问题，并且，激光SLAM在动态环境中，动态物体也容易对点云匹配产生影响，无法估计正确的位姿影响了算法的鲁棒性和精度。
技术实现思路
[0004]为了解决上述技术问题，本专利技术的目的是提供一种基于连续运动模型的激光SLAM方法及系统，能够降低建图过程中的累积误差且减少计算量。
[0005]本专利技术所采用的第一技术方案是：一种基于连续运动模型的激光SLAM方法，包括以下步骤：
[0006]获取激光点云数据并进行特征提取，得到特征点；
[0007]基于捆绑调整对特征点进行特征关联，得到地标特征；
[0008]基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差；
[0009]构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度；
[0010]基于滑动窗口选取关键帧和对地标特征的观测残差进行计算优化，并结合地标特征的位姿与速度构建地图。
[0011]进一步，所述获取激光点云数据并进行特征提取，得到特征点这一步骤，具体包括：
[0012]获取激光点云数据并基于球面投影将每一帧激光点云数据投影到球形平面上，得到投影图；
[0013]基于广度优先搜索算法对投影图进行特征提取，得到平面特征点和直线特征点；
[0014]整合平面特征点和直线特征点，得到特征点。
[0015]进一步，所述基于捆绑调整对特征点进行特征关联，得到地标特征这一步骤，具体
包括：
[0016]获取现有地标特征，并在当前帧中选出与现有地标特征对应的特征点；
[0017]跟踪当前帧中选出的特征点，加入现有地标特征；
[0018]搜索当前帧中未被跟踪的特征点的邻居特征点，记为邻域点集；
[0019]对邻域点集的协方差矩阵进行特征值分解，得到特征值；
[0020]根据特征值的大小将未被跟踪的特征点划分为新的平面地标特征和新的直线地标特征；
[0021]整合现有地标特征与新的平面地标特征和新的直线地标特征，得到地标特征。
[0022]进一步，所述基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差这一步骤，具体包括：
[0023]基于连续运动模型，推导出当前帧扫描中每个特征点从局部坐标系到全局坐标系的变换矩阵；
[0024]根据投影公式和变换矩阵计算，得到地标特征点的全局位置；
[0025]根据地标特征点的全局位置构建平面地标特征和直线地标特征的损失函数；
[0026]根据平面地标特征和直线地标特征的损失函数计算地标特征的观测残差。
[0027]进一步，所述构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度这一步骤，具体包括：
[0028]根据IMU预积分因子构建平滑残差，所述平滑残差包括连续残差和先验残差；
[0029]对滑动窗口内的地标特征的观测残差和平滑残差进行联合优化，得到地标特征的位姿与速度。
[0030]进一步，所述地标特征的观测残差和平滑残差联合优化的计算公式，具体如下：
[0031][0032]上式中，ρ是损失函数，χ是所有需要优化的历史帧状态向量的集合，是直线地标特征的观测残差，是平面地标特征的观测残差，M
L
是当前地图中的直线地标特征的集合，M
π
是当前地图中的平面地标特征的集合，r
s
(x
k
,x
k+1
)是连续残差，K是历史帧的总数量。
[0033]进一步，所述基于滑动窗口选取关键帧和对地标特征的观测残差进行计算优化，并结合地标特征的位姿与速度构建地图这一步骤，具体包括：
[0034]基于滑动窗口选取关键帧，并基于滑动窗口机制对关键帧的位姿进行优化，对其它帧的位姿固定且不参与优化，并以边缘化残差的形式保留在滑动窗口中；
[0035]获取去除速度的地标特征点的全局位置并计算，得到最终的地标特征的协方差矩阵；
[0036]将边缘化残差保存为地标特征的特定参数，并添加到最终的地标特征的协方差矩阵中；
[0037]基于最终的地标特征的协方差矩阵计算，去除平滑残差，得到计算优化后的地标特征的观测残差；
[0038]根据计算优化后的地标特征的观测残差和地标特征的位姿与速度构建地图。
[0039]进一步，所述关键帧的选取标准包括：
[0040]滑动窗口内的扫描次数小于预设值；
[0041]或成功跟踪的平面地标特征数与创建的新特征数之比小于2；
[0042]或成功跟踪的边缘地标特征数与创建的新特征数之比小于1.5；
[0043]或与最后一个关键帧的时间差大于指定的常规扫描次数。
[0044]进一步，所述最终的地标特征的协方差矩阵的计算公式，具体如下：
[0045][0046]上式中，A是协方差，N
f
是地标特征中特征点的总数量，是N
f
中属于第k帧特征点的数量，K是历史帧的总数量，是第k帧中的第j个特征点基于连续运动模型补偿后的位置。
[0047]本专利技术所采用的第二技术方案是：一种基于连续运动模型的激光SLAM系统，包括：
[0048]特征点提取模块，用于获取激光点云数据并进行特征提取，得到特征点；
[0049]地标关联模块，基于捆绑调整对特征点进行特征关联，得到地标特征；
[0050]残差构建模块，基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差；
[0051]联合优化模块，用于构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度；
[0052]建图模块，基于滑动窗口选取关键帧和对地标特征的观测残差进行计算优化，并结合地标特征的位姿与速度构建地图。
[0053]本专利技术方法及系统的有益效果是：本专利技术首先通过基于球本文档来自技高网...
【技术保护点】
【技术特征摘要】
1.一种基于连续运动模型的激光SLAM方法，其特征在于，包括以下步骤：获取激光点云数据并进行特征提取，得到特征点；基于捆绑调整对特征点进行特征关联，得到地标特征；基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差；构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度；基于滑动窗口选取关键帧和对地标特征的观测残差进行计算优化，并结合地标特征的位姿与速度构建地图。2.根据权利要求1所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述获取激光点云数据并进行特征提取，得到特征点这一步骤，具体包括：获取激光点云数据并基于球面投影将每一帧激光点云数据投影到球形平面上，得到投影图；基于广度优先搜索算法对投影图进行特征提取，得到平面特征点和直线特征点；整合平面特征点和直线特征点，得到特征点。3.根据权利要求2所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述基于捆绑调整对特征点进行特征关联，得到地标特征这一步骤，具体包括：获取现有地标特征，并在当前帧中选出与现有地标特征对应的特征点；跟踪当前帧中选出的特征点，加入现有地标特征；搜索当前帧中未被跟踪的特征点的邻居特征点，记为邻域点集；对邻域点集的协方差矩阵进行特征值分解，得到特征值；根据特征值的大小将未被跟踪的特征点划分为新的平面地标特征和新的直线地标特征；整合现有地标特征与新的平面地标特征和新的直线地标特征，得到地标特征。4.根据权利要求2所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差这一步骤，具体包括：基于连续运动模型，推导出当前帧扫描中每个特征点从局部坐标系到全局坐标系的变换矩阵；根据投影公式和变换矩阵计算，得到地标特征点的全局位置；根据地标特征点的全局位置构建平面地标特征和直线地标特征的损失函数；根据平面地标特征和直线地标特征的损失函数计算地标特征的观测残差。5.根据权利要求1所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度这一步骤，具体包括：根据IMU预积分因子构建平滑残差，所述平滑残差包括连续残差和先验残差；对滑动窗口内的地标特征的观测残差和平滑残差进行联合优化，得到地标特征的位姿与速度。6.根据权利要求5所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述地标特征的观测残差和平滑残差联合优化的计算公式，具体如下：
上式中，ρ...
【专利技术属性】
技术研发人员：谭晓军，介璐，张乐天，吴加学，任杰，
申请(专利权)人：南方海洋科学与工程广东省实验室珠海，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。