[发明专利]一种紧耦合激光雷达和惯性里程计的建图方法及系统

权利要求书

1.一种紧耦合激光雷达和惯性里程计的建图方法，其特征在于，包括：

获取IMU数据和激光点云数据，对所述IMU数据进行预处理操作，得到当前激光帧姿态变化，对所述激光点云数据进行预处理操作，得到当前关键帧对应局部地图；

对所述当前激光帧姿态变化和所述当前关键帧对应局部地图进行降采样处理，建立位姿优化方程进行扫描匹配优化，根据匹配的结果对之前获取的所有点云数据进行矫正；

最后进行回环检测线程，将角点和平面点特征作为当前帧特征集，将关键帧特征点作为匹配帧特征集，对所述当前帧特征集和所述匹配帧特征集进行匹配并计算匹配得分，根据匹配结果计算所述当前帧和匹配帧之间的欧式距离，对位姿进行矫正后保存当前帧和匹配帧之间的关系，实现闭环检测，构建出可视化的全局地图。

2.根据权利要求1所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，对所述IMU数据进行预处理的过程包括：首先修剪IMU数据队列保证IMU时间戳和激光雷达时间戳同步，从而将IMU数据与扫描的时间戳进行对齐，对激光雷达初始帧到结束帧时间段内的所述IMU数据进行姿态预积分，获得IMU姿态的变化，找到距离激光起始时刻最近的IMU数据，并将其欧拉角作为激光起始时刻的初始值。

3.根据权利要求2所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，对所述IMU数据进行姿态预积分的过程包括：通过检测所述IMU数据队列在当前帧激光里程计时刻之前是否存在数据来判断系统是否进行初始化，若未进行初始化，则对所述IMU预积分器进行重置，用前一帧的状态、偏置，施加IMU预计分量，得到当前时刻预估的数据，并对变量节点赋予初值并执行优化，通过优化后的状态对IMU预积分器进行初始化，更新IMU的偏置，用最新的偏置重新计算当前激光里程计时刻之后的IMU预积分以计算每一时刻的位姿，将结果存放在所述IMU预积分器中，得到当前激光帧姿态变化。

4.根据权利要求3所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，对所述激光点云数据进行预处理操作的过程包括：对激光雷达点云数据进行畸变矫正，通过对点云数据的每一帧进行线性插值，得到当前时刻对应的IMU位姿，再根据IMU预积分得到所述当前帧姿态变化，实现点云偏斜矫正；然后计算曲率，根据曲率提取角点和平面点进行点云特征提取，得到当前关键帧对应局部地图。

5.根据权利要求4所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，通过评估局部区域上的点曲率提取所述角点和平面特征，在获取所述当前激光帧姿态变化和所述关键帧对应局部地图后进行降采样剔除噪声数据，建立特征点集合并进行角点、平面点匹配优化，通过获取激光里程计数据，得到激光里程计位姿的时间戳以及位姿变换矩阵，通过滑动窗口创建点云地图，利用点-线-面之间的几何关系，建立位姿求解方程，然后通过标签匹配和LM优化方法进行点云匹配，基于传感器视域和图优化方法进行位姿优化。

6.根据权利要求4所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，建立所述位姿求解方程的过程包括：通过提取边缘特征和平面特征，建立关键帧特征集合，利用位姿估计构建局部地图，针对六自由度的LM优化方法，优化计算点和线特征的对应点约束，得到相应的位姿信息。

7.根据权利要求1所述的紧耦合激光雷达和惯性里程计的建图方法，其特征在于，所述进行回环检测线程的过程包括：将角点和平面点特征作为当前帧的特征集，将前后相邻的5个关键帧特征点作为匹配帧特征集，将空间离散为立方体，采用正态分布变换进行两个特征集的匹配并计算匹配得分，根据匹配结果计算所述当前帧和匹配帧帧之间的欧氏距离，对位姿进行矫正后保存当前帧和匹配帧之间的关系，实现闭环检测，构建出可视化的全局地图。