基于IMU耦合的激光雷达迭代最近点改进算法制造技术

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

【技术实现步骤摘要】
基于IMU耦合的激光雷达迭代最近点改进算法
本专利技术涉及基于IMU耦合的激光雷达迭代最近点改进算法，属于激光雷达建图与定位领域。
技术介绍
激光雷达能帮助无人车在未知环境中建立周围环境地图，并同时自我运动估计，是无人车进行精确导航的重要方式之一。激光雷达建图与定位可以分为前端扫描匹配、后端优化、闭环检测和地图构建四个模块。其中前端扫描匹配为核心模块，它通过点云匹配估计相邻两帧点云间的位姿变化矩阵，根据位姿变换关系依次将相邻两帧点云拼接成全局地图，即实现建图；然后根据位姿变换矩阵推算出相邻两帧之间的状态信息，即实现定位。前端扫描匹配模块中，实现建图与定位最常用的是由Besl和McKay开发的迭代最近点(ICP)算法，该算法为一种基于对应点的配准算法，以欧式距离最小的两个点作为对应点，通过最小化两个点集中对应点的距离平方和得到两帧点云数据间的变换矩阵。这种算法操作简便，匹配精度高，但运行速度低、对初始值敏感容易限于局部最优解。
技术实现思路
针对现有迭代最近点算法存在运行速度低、对初始值敏感容易限于局部最优解的问题，本专利技术提供一种基于IMU耦合的激光雷达迭代最近点改进算法。本专利技术的一种基于IMU耦合的激光雷达迭代最近点改进算法，包括，使激光雷达和IMU钟源同步；对激光雷达原始点云预处理后进行特征点集提取，得到激光雷达特征点集；根据相邻两帧激光雷达原始点云对应的IMU原始位姿，得到预积分结果和相邻两帧激光雷达原始点云的转换初始矩阵；根据激光雷达特征点集构建激光雷达点云匹配的原始误差方程；根据预积分结果构建IMU的观测误差方程；再基于原始误差方程和观测误差方程构建IMU和激光雷达的联合误差方程；将所述转换初始矩阵作为初值，基于相邻两帧激光雷达原始点云对应的激光雷达特征点集，对联合误差方程进行ICP算法迭代计算，得到最优变换矩阵。根据本专利技术的基于IMU耦合的激光雷达迭代最近点改进算法，所述激光雷达和IMU通过时间同步器实现钟源同步。根据本专利技术的基于IMU耦合的激光雷达迭代最近点改进算法，对激光雷达原始点云预处理后进行特征点集提取，得到激光雷达特征点集的过程包括：采集激光雷达原始点云，使用滤波器移除离群点；将保留的所有原始点云依次连接成线，计算每个原始点云与前后相邻的两个原始点云连线所成夹角的余弦绝对值；若所述余弦绝对值小于预设最小阈值，则当前原始点云为角点；若所述余弦绝对值大于或等于预设最小阈值并小于预设最大阈值，则当前原始点云为非特征点；若所述余弦绝对值大于预设最大阈值，则当前原始点云为平面点；激光雷达特征点集包括所有角点构成的角点点集和所有平面点构成的平面点点集。根据本专利技术的基于IMU耦合的激光雷达迭代最近点改进算法，根据预积分结果构建IMU的观测误差方程的具体过程包括：建立激光雷达坐标系L和IMU坐标系B，将IMU坐标系B转换到激光雷达坐标系L；设定IMU原始位姿信息为：其中为激光雷达第k帧原始点云对应的激光雷达坐标系L下IMU状态，包括IMU位置IMU速度IMU四元数IMU速度漂移和IMU加速度漂移对激光雷达第k帧原始点云和第k+1帧原始点云之间对应的IMU状态数据进行预积分，获得相对于激光雷达第k帧原始点云、第k+1帧原始点云对应的IMU原始位姿在激光雷达坐标系L下含有噪声的状态信息：其中为IMU在激光雷达坐标系L第k+1帧时刻的位置、为IMU在激光雷达坐标系L第k帧时刻的位置、为IMU在激光雷达坐标系L第k帧时刻的速度、Δtk为IMU在激光雷达坐标系L第k帧到第k+1帧对应的时间变化量、gL为IMU在激光雷达坐标系L下的重力加速度、为IMU在激光雷达坐标系L第k帧到第k+1帧的位移；则IMU在激光雷达坐标系L第k+1帧时刻的速度为：式中为IMU在激光雷达坐标系L第k帧到第k+1帧的速度；进而得到其中为第k+1帧IMU四元数，为IMU在激光雷达坐标系L下第k帧到第k+1帧下的旋转；构建IMU的位移、速度、角度的状态误差方程：其中表示IMU在激光雷达坐标系L下第k帧到k+1帧的位移误差、表示IMU在激光雷达坐标系L下第k帧到k+1帧的速度误差、表示IMU在激光雷达坐标系L下第k帧到k+1帧的旋转误差；构建IMU的观测误差方程：EB表示IMU的观测误差。根据本专利技术的基于IMU耦合的激光雷达迭代最近点改进算法，根据激光雷达特征点集构建激光雷达点云匹配的原始误差方程为：式中EL表示激光雷达点云匹配的原始误差，R表示旋转矩阵，Ai表示源点云、Bi表示目标点云，t表示平移矩阵。根据本专利技术的基于IMU耦合的激光雷达迭代最近点改进算法，所述联合误差方程为式中E表示联合误差。根据本专利技术的基于IMU耦合的激光雷达迭代最近点改进算法，对联合误差方程进行ICP算法迭代计算包括：将相邻两帧激光雷达原始点云记为源点云Ai＝(xiyizi)和目标点云Bj＝(xjyjzj)，其中i∈(1,2,3,...,n)，j∈(1,2,3,...,m)，式中(xyz)分表为点云在激光雷达坐标系L下的坐标值，n表示源点云数量，m表示目标点云数量；根据两点之间欧式距离最小原则，确定相邻两帧激光雷达原始点云对应的角点点集和平面点点集；再以所述转换初始矩阵作为初值，对联合误差方程进行ICP算法迭代计算，获得最优变换矩阵，所述最优变换矩阵使联合误差方程值最小。根据本专利技术的基于IMU耦合的激光雷达迭代最近点改进算法，对联合误差方程进行ICP算法迭代计算还包括：对目标点云采用所述最优变换矩阵进行变换，得到新目标点云；计算新目标点云与源点云的平均距离d：式中Bi'表示新目标点云；当所述平均距离d小于设定距离阈值λ或迭代次数达到设定迭代次数N时，停止迭代计算，获得的最优变换矩阵作为最终最优变换矩阵。本专利技术的有益效果：为了提高ICP算法的运行速度并同时保证匹配精度，本专利技术首先提取激光雷达特征点集，降低了匹配计算量和错误匹配的对应点；其次利用惯性测量单元IMU原始位姿求解ICP算法的转换初始矩阵，同时为了降低IMU漂移带来的误差，构建IMU和激光雷达的联合误差方程，最后迭代计算出最优变换矩阵。本专利技术提高了ICP算法的运行速度和匹配精度。附图说明图1是本专利技术所述基于IMU耦合的激光雷达迭代最近点改进算法的具体实施流程图；图2是获得激光雷达特征点集的流程图；图3是构建IMU的观测误差方程的流程图；图4是对联合误差方程进行ICP算法迭代计算的流程图。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施本文档来自技高网...

【技术保护点】
1.一种基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于包括，/n使激光雷达和IMU钟源同步；/n对激光雷达原始点云预处理后进行特征点集提取，得到激光雷达特征点集；根据相邻两帧激光雷达原始点云对应的IMU原始位姿，得到预积分结果和相邻两帧激光雷达原始点云的转换初始矩阵；/n根据激光雷达特征点集构建激光雷达点云匹配的原始误差方程；根据预积分结果构建IMU的观测误差方程；再基于原始误差方程和观测误差方程构建IMU和激光雷达的联合误差方程；/n将所述转换初始矩阵作为初值，基于相邻两帧激光雷达原始点云对应的激光雷达特征点集，对联合误差方程进行ICP算法迭代计算，得到最优变换矩阵。/n

【技术特征摘要】
1.一种基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于包括，
使激光雷达和IMU钟源同步；
对激光雷达原始点云预处理后进行特征点集提取，得到激光雷达特征点集；根据相邻两帧激光雷达原始点云对应的IMU原始位姿，得到预积分结果和相邻两帧激光雷达原始点云的转换初始矩阵；
根据激光雷达特征点集构建激光雷达点云匹配的原始误差方程；根据预积分结果构建IMU的观测误差方程；再基于原始误差方程和观测误差方程构建IMU和激光雷达的联合误差方程；
将所述转换初始矩阵作为初值，基于相邻两帧激光雷达原始点云对应的激光雷达特征点集，对联合误差方程进行ICP算法迭代计算，得到最优变换矩阵。


2.根据权利要求1所述的基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于，
所述激光雷达和IMU通过时间同步器实现钟源同步。


3.根据权利要求1或2所述的基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于，
对激光雷达原始点云预处理后进行特征点集提取，得到激光雷达特征点集的过程包括：
采集激光雷达原始点云，使用滤波器移除离群点；将保留的所有原始点云依次连接成线，计算每个原始点云与前后相邻的两个原始点云连线所成夹角的余弦绝对值；若所述余弦绝对值小于预设最小阈值，则当前原始点云为角点；若所述余弦绝对值大于或等于预设最小阈值并小于预设最大阈值，则当前原始点云为非特征点；若所述余弦绝对值大于预设最大阈值，则当前原始点云为平面点；
激光雷达特征点集包括所有角点构成的角点点集和所有平面点构成的平面点点集。


4.根据权利要求3所述的基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于，根据预积分结果构建IMU的观测误差方程的具体过程包括：
建立激光雷达坐标系L和IMU坐标系B，将IMU坐标系B转换到激光雷达坐标系L；
设定IMU原始位姿信息为：



其中为激光雷达第k帧原始点云对应的激光雷达坐标系L下IMU状态，包括IMU位置IMU速度IMU四元数IMU速度漂移和IMU加速度漂移
对激光雷达第k帧原始点云和第k+1帧原始点云之间对应的IMU状态数据进行预积分，获得相对于激光雷达第k帧原始点云、第k+1帧原始点云对应的IMU原始位姿在激光雷达坐标系L下含有噪声的状态信息：



其中为IMU在激光雷达坐标系L第k+1帧时刻的位置、为IMU在激光雷达坐标系L第k帧时刻的位置、为IMU在激光雷达坐标系L第k帧时刻的速度、Δtk为IMU在激光雷达坐标系L第k帧到第k+1帧对应的时间变化量、gL为IMU在激光雷达坐标系L下...

【专利技术属性】
技术研发人员：刘飞，周志全，邹钰杰，屈婧婧，柴文静，杨起鸣，严景琳，
申请(专利权)人：桂林电子科技大学，
类型：发明
国别省市：广西;45