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

权利要求书

1.一种基于连续运动模型的激光SLAM方法，其特征在于，包括以下步骤：

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

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

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

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

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

2.根据权利要求1所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述获取激光点云数据并进行特征提取，得到特征点这一步骤，具体包括：

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

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

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

3.根据权利要求2所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述基于捆绑调整对特征点进行特征关联，得到地标特征这一步骤，具体包括：

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

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

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

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

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

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

4.根据权利要求2所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述基于连续运动模型对地标特征进行观测与计算，得到地标特征的观测残差这一步骤，具体包括：

基于连续运动模型，推导出当前帧扫描中每个特征点从局部坐标系到全局坐标系的变换矩阵；

根据投影公式和变换矩阵计算，得到地标特征点的全局位置；

根据地标特征点的全局位置构建平面地标特征和直线地标特征的损失函数；

根据平面地标特征和直线地标特征的损失函数计算地标特征的观测残差。

5.根据权利要求1所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述构建平滑残差并与地标特征的观测残差进行联合优化，得到地标特征的位姿与速度这一步骤，具体包括：

根据IMU预积分因子构建平滑残差，所述平滑残差包括连续残差和先验残差；

对滑动窗口内的地标特征的观测残差和平滑残差进行联合优化，得到地标特征的位姿与速度。

6.根据权利要求5所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述地标特征的观测残差和平滑残差联合优化的计算公式，具体如下：

上式中，ρ是损失函数，χ是所有需要优化的历史帧状态向量的集合，是直线地标特征的观测残差，是平面地标特征的观测残差，M_L是当前地图中的直线地标特征的集合，M_π是当前地图中的平面地标特征的集合，r_s(x_k,x_k+1)是连续残差，K是历史帧的总数量。

7.根据权利要求3所述一种基于连续运动模型的激光SLAM方法，其特征在于，所述基于滑动窗口选取关键帧和对地标特征的观测残差进行计算优化，并结合地标特征的位姿与速度构建地图这一步骤，具体包括：

基于滑动窗口选取关键帧，并基于滑动窗口机制对关键帧的位姿进行优化，对其它帧的位姿固定且不参与优化，并以边缘化残差的形式保留在滑动窗口中；

获取去除速度的地标特征点的全局位置并计算，得到最终的地标特征的协方差矩阵；

将边缘化残差保存为地标特征的特定参数，并添加到最终的地标特征的协方差矩阵中；

基于最终的地标特征的协方差矩阵计算，去除平滑残差，得到计算优化后的地标特征的观测残差；

根据计算优化后的地标特征的观测残差和地标特征的位姿与速度构建地图。