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

权利要求书

1.一种基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于包括，

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

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

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

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

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

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

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

根据预积分结果构建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帧时刻的速度、Δt_k为IMU在激光雷达坐标系L第k帧到第k+1帧对应的时间变化量、g^L为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的观测误差方程：

E_B表示IMU的观测误差。

2.根据权利要求1所述的基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于，

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

3.根据权利要求1所述的基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于，

根据激光雷达特征点集构建激光雷达点云匹配的原始误差方程为：

式中E_L表示激光雷达点云匹配的原始误差，R表示旋转矩阵，A_i表示源点云、B_i表示目标点云，t表示平移矩阵。

4.根据权利要求3所述的基于IMU耦合的激光雷达迭代最近点改进算法，其特征在于，

所述联合误差方程为

式中E表示联合误差。