[发明专利]激光雷达和惯性测量单元融合的定位与建图方法及系统

权利要求书

1.激光雷达和惯性测量单元融合的定位与建图方法，其特征在于，包括如下步骤：

步骤一、对激光雷达和惯性测量单元IMU进行联合标定；

步骤二、激光雷达获取原始点云数据，以激光帧的形式送入激光里程计；

惯性测量单元IMU获取激光雷达位姿送入所述激光里程计；

步骤三、所述激光里程计包括高频激光里程计和低频激光里程计；其中高频激光里程计的处理频率为10Hz，低频激光里程计的处理频率为3Hz；

所述高频激光里程计，对所述原始点云数据进行运动畸变去除以及特征点提取，得到特征点云，针对前后两次激光帧对应的特征点云进行配准，得到激光雷达的帧间变换矩阵，即为激光雷达的位姿差值；

所述低频激光里程计，选取关键帧，多个关键帧对应的特征点云投影到世界坐标系下并拼接形成局部点云地图，针对当前关键帧与所述局部点云地图进行匹配，得到激光雷达的激光里程计位姿并输出；

步骤四、构建因子图模型G＝(X,I,E_I,E_L)；其中因子图模型的变量节点为X＝{X₁,X₂,X₃,...,X_K}，X₁,X₂,X₃,...,X_K为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子图模型中的因子节点I＝{I₁,I₂,I₃,...,I_K}，I₁,I₂,I₃,...,I_K为第1～K时刻惯性测量单元IMU观测的激光雷达姿态；一元边E_I连接变量节点和因子节点；二元边E_L为激光里程计得到的3D激光雷达的位姿差值；对所述因子图模型进行优化求解得到优化后的变量节点，即得到优化后的第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿并输出，同时拼接输出3D点云地图。

2.如权利要求1所述的方法，其特征在于，所述对激光雷达和惯性测量单元IMU进行联合标定，具体为：

利用手眼标定方程对激光雷达和惯性测量单元IMU进行联合标定。

3.如权利要求1所述的方法，其特征在于，所述高频激光里程计中，对所述原始点云数据进行运动畸变去除，具体为：

激光雷达生成的原始点云数据中，第k帧点云中每一个激光点i对应的激光雷达位姿分布在第k-1帧和第k帧点云对应的世界坐标系下激光雷达位姿之间，并认为第k帧点云最后一个接收到的激光点i_end对应的激光雷达位姿即为

第k-1帧和第k帧点云之间的激光雷达变换矩阵平移分量为匀速运动，则第i个激光点对应的激光雷达位姿相对于的平移分量由第k-2帧和第k-1帧点云之间的激光雷达变换矩阵平移分量进行时间线性插值给出，而每一个激光点相对于的旋转分量则依靠所述惯性测量单元IMU的测量差值给出；由此求得激光点i相对于点i_end对应的激光雷达运动关系矩阵将第k帧点云每一个激光点i根据变换到i_end对应的激光雷达位姿下，得到除去运动畸变的第k帧点云。

4.如权利要求3所述的方法，其特征在于，所述特征点提取高频激光里程计中，特征点提取具体采用如下方法：

特征点包括边缘点和平面点；

首先将去除运动畸变的点云按照激光束的水平角均分为四部分，对于每一部分点云，求解其中每条线束中每个点的曲率，则每条线束中曲率最大的点作为边缘点，提取到边缘点集每条线束中曲率最小的点作为平面点，提取到平面点集每一部分点云最多提供2个边缘点和4个平面点，由此得到特征点云。