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
标系L第k帧到第k+1帧对应的时间变化量、g 为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的观测误差。
5.根据权利要求4所述的基于IMU耦合的激光雷达迭代最近点改进算法,其特征在于,根据激光雷达特征点集构建激光雷达点云匹配的原始误差方程为:式中EL表示激光雷达点云匹配的原始误差,R表示旋转矩阵,Ai表示源点云、Bi表示目标点云,t表示平移矩阵。
6.根据权利要求5所述的基于IMU耦合的激光雷达迭代最近点改进算法,其特征在于,所述联合误差方程为
式中E表示联合误差。
7.根据权利要求6所述的基于IMU耦合的激光雷达迭代最近点改进算法,其特征在于,对联合误差方程进行ICP算法迭代计算包括:将相邻两帧激光雷达原始点云记为源点云Ai=(xi yi zi)和目标点云Bj=(xj yj zj),其中i∈(1,2,3,...,n),j∈(1,2,3,...,m),式中(x y z)分表为点云在激光雷达坐标系L下的坐标值,n表示源点云数量,m表示目标点云数量;
根据两点之间欧式距离最小原则,确定相邻两帧激光雷达原始点云对应的角点点集和平面点点集;再以所述转换初始矩阵作为初值,对联合误差方程进行ICP算法迭代计算,获得最优变换矩阵,所述最优变换矩阵使联合误差方程值最小。
8.根据权利要求7所述的基于IMU耦合的激光雷达迭代最近点改进算法,其特征在于,对联合误差方程进行ICP算法迭代计算还包括:对目标点云采用所述最优变换矩阵进行变换,得到新目标点云;
计算新目标点云与源点云的平均距离d:式中B'i表示新目标点云;
当所述平均距离d小于设定距离阈值λ或迭代次数达到设定迭代次数N时,停止迭代计算,获得的最优变换矩阵作为最终最优变换矩阵。