[发明专利]基于IMU耦合的激光雷达迭代最近点改进算法

说明书

一种基于IMU耦合的激光雷达迭代最近点改进算法，属于激光雷达建图与定位领域。本发明针对现有迭代最近点算法存在运行速度低、对初始值敏感容易限于局部最优解的问题。包括：使激光雷达和IMU钟源同步；对激光雷达原始点云预处理后进行特征点集提取，得到激光雷达特征点集；根据相邻两帧激光雷达原始点云对应的IMU原始位姿，得到预积分结果和相邻两帧激光雷达原始点云的转换初始矩阵；构建激光雷达点云匹配的原始误差方程、构建IMU的观测误差方程，再构建IMU和激光雷达的联合误差方程；对联合误差方程进行ICP算法迭代计算，得到最优变换矩阵。本发明提高了ICP算法的运行速度和匹配精度。

技术领域

本发明涉及基于IMU耦合的激光雷达迭代最近点改进算法，属于激光雷达建图与定位领域。

背景技术

激光雷达能帮助无人车在未知环境中建立周围环境地图，并同时自我运动估计，是无人车进行精确导航的重要方式之一。

激光雷达建图与定位可以分为前端扫描匹配、后端优化、闭环检测和地图构建四个模块。其中前端扫描匹配为核心模块，它通过点云匹配估计相邻两帧点云间的位姿变化矩阵，根据位姿变换关系依次将相邻两帧点云拼接成全局地图，即实现建图；然后根据位姿变换矩阵推算出相邻两帧之间的状态信息，即实现定位。

前端扫描匹配模块中，实现建图与定位最常用的是由Besl和McKay开发的迭代最近点(ICP)算法，该算法为一种基于对应点的配准算法，以欧式距离最小的两个点作为对应点，通过最小化两个点集中对应点的距离平方和得到两帧点云数据间的变换矩阵。这种算法操作简便，匹配精度高，但运行速度低、对初始值敏感容易限于局部最优解。

发明内容

针对现有迭代最近点算法存在运行速度低、对初始值敏感容易限于局部最优解的问题，本发明提供一种基于IMU耦合的激光雷达迭代最近点改进算法。

本发明的一种基于IMU耦合的激光雷达迭代最近点改进算法，包括，

使激光雷达和IMU钟源同步；

对激光雷达原始点云预处理后进行特征点集提取，得到激光雷达特征点集；根据相邻两帧激光雷达原始点云对应的IMU原始位姿，得到预积分结果和相邻两帧激光雷达原始点云的转换初始矩阵；

根据激光雷达特征点集构建激光雷达点云匹配的原始误差方程；根据预积分结果构建IMU的观测误差方程；再基于原始误差方程和观测误差方程构建IMU和激光雷达的联合误差方程；

将所述转换初始矩阵作为初值，基于相邻两帧激光雷达原始点云对应的激光雷达特征点集，对联合误差方程进行ICP算法迭代计算，得到最优变换矩阵。

根据本发明的基于IMU耦合的激光雷达迭代最近点改进算法，

所述激光雷达和IMU通过时间同步器实现钟源同步。

根据本发明的基于IMU耦合的激光雷达迭代最近点改进算法，

对激光雷达原始点云预处理后进行特征点集提取，得到激光雷达特征点集的过程包括：

采集激光雷达原始点云，使用滤波器移除离群点；将保留的所有原始点云依次连接成线，计算每个原始点云与前后相邻的两个原始点云连线所成夹角的余弦绝对值；若所述余弦绝对值小于预设最小阈值，则当前原始点云为角点；若所述余弦绝对值大于或等于预设最小阈值并小于预设最大阈值，则当前原始点云为非特征点；若所述余弦绝对值大于预设最大阈值，则当前原始点云为平面点；

激光雷达特征点集包括所有角点构成的角点点集和所有平面点构成的平面点点集。

根据本发明的基于IMU耦合的激光雷达迭代最近点改进算法，根据预积分结果构建IMU的观测误差方程的具体过程包括：

建立激光雷达坐标系L和IMU坐标系B，将IMU坐标系B转换到激光雷达坐标系L；