[发明专利]一种针对地面移动机器人的Lidar-IMU融合的slam方法

权利要求书

1.一种针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，包括如下步骤：

步骤1：数据预处理；包括点云去畸变和特征提取；

步骤2：关键帧选择与子地图构建；引入视觉slam中的关键帧概念，使用两个关键帧之间的激光雷达信息来构造局部子地图；

步骤3：关键帧的因子图优化；通过因子图优化将关键帧与世界坐标系相关联，构建一个因子图来联合优化激光雷达关键帧和IMU的预积分；

步骤4：位姿图优化；通过位姿图优化得到一个全局一致的环境地图。

2.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，所述步骤1包括点云去畸变和特征提取；

所述点云去畸变：在因子优化过程中，IMU的预积分提供预积分因子，而优化过程IMU的偏差也会作为优化参数参与优化，利用得到的IMU偏差纠正的IMU数据来估计任意时刻载体的位姿，将激光点变换到统一坐标系下，进行点云畸变校正；

所述特征提取：从去畸变的点云中提取特征点云，提取边点和表面点作为匹配的特征点，对于任何点p，从与p同一行扫描中找到10个连续点，用S表示，均分的位于p的两侧，则p的粗糙度可以通过以下公式计算：

式中|S|是S中点的个数，取10，r_i表示第i个点的深度；

选择粗糙度值较小的点作为平面点，粗糙度值较大的点作为边缘点，则从任一激光雷达扫描得到的特征点表示为表示平面点，表示边缘点。

3.根据权利要求1所述的针对地面移动机器人的Lidar-IMU融合的slam方法，其特征在于，所述步骤2包括关键帧的选择和子地图的构建；

所述关键帧的选择：引入了视觉slam中的关键帧概念，使用两个关键帧之间的激光雷达信息来构造局部子地图，假设地面移动机器人是一个刚体，相邻激光雷达扫描时刻的机器人本体坐标系的xy平面的变化可以反映地平面的变化，当新的激光雷达扫描到来时，将当前帧地面法线矢量e₃转换到前一帧坐标系中e′_3i，则相邻激光雷达扫描的地平面之间的角度为：

式中是e₃的转置矩阵；

如果θ大于设定的阈值θ_th，则认为地平面的斜率发生了变化，机器人不再与之前的扫描位于同一地平面上，此扫描将被选为关键帧；

所述子地图的构建：如果新的Lidar扫描为关键帧，则创建一个新的子地图，并将该关键帧作为子地图的参考帧，此时机器人的本体坐标系为子地图的坐标系，用M_i表示，然后，构建一个包括激光雷达里程计约束和地面约束的最大后验估计，优化得到后续的普通雷达帧相对于子地图的位姿变换；

当新的激光雷达普通帧F_j到来时，在预处理后获得其特征点将其与所在子地图M_i相匹配，此时，本地子地图具有如下激光雷达帧：

M_i＝{L_i,F_i,0,F_i,1…F_i,j-1}

式中L_i表示子地图M_i的参考关键帧，F_i,k,k＝1,2,…j-1表示子地图中已有的普通帧，对于扫描匹配方法，使用与loam相同的方法，首先将此普通帧的特征点转换为其所在子地图中得到初始变换是通过使用来自校正后的IMU的预测获得的，对于中的每个特征点，在子地图中找到相应的边缘点和平面点，然后，最小化特征点到其相应边和平面的距离，以获得最最优的变换：

式中

式中分别是当前帧的边缘特征点和面特征点，是转换到局部坐标系后得到的点，是局部地图中与对应的匹配边缘点，是局部地图中与对应的匹配面点；

子地图中的所有激光雷达帧都位于同一地平面上，因此，使用在SE(3)位姿-SE(2)约束的方法向每个激光雷达普通帧添加平面约束，理想情况下，在子地图中，机器人在平面中移动，将普通帧和子地图之间的变换参数化为SE(2)位姿，其李代数表示为v＝[v₁ v₂ φ]，其中φ是旋转分量，(v₁,v₂)并表示平移分量；在实际环境中，由于机器人运动中的崎岖地形和机器人自身的晃动，机器人的姿态在SE(2)以外的维度上存在运动干扰，因此，不是直接在SE(2)上参数化机器人姿势，而是通过李代数将姿势从SE(2)恢复到SE(3)：

ξ＝[v₁ v₂ 0 0 0 φ]^T

同时，用高斯模型对其他三个维度的运动扰动的误差进行建模，表示其他三维空间中允许的运动扰动振幅，这是由地形条件和机器人结构决定的；

因此，地平面约束的残差定义为：

其中，是平面约束的测量值，可以通过将当前姿态估计值投影到SE(2)空间，然后再恢复到SE(3)来获得：

平面约束的雅可比矩阵和协方差矩阵为：

式中分别是z方向，roll方向和pitch方向的机器人状态的方差。