1.一种基于IMU的激光雷达三维点云实时运动补偿方法,其特征在于,步骤如下:步骤S1,将激光雷达固定安装于无人车上,对激光雷达与IMU进行时间轴同步与空间坐标系统一,同步实时采集激光雷达三维点云数据与IMU数据,进入步骤S2;
步骤S2,按照时间戳顺序对采集的激光雷达三维点云数据与IMU数据分别进行排序,得到排序后的IMU数据与激光雷达三维点云数据,进入步骤S3;
步骤S3,根据排序后的IMU数据时序对每帧激光雷达三维点云数据进行数据块划分,通过IMU采样频率Fimu和激光雷达采样频率Flidar之比来计算数据块划分的分块数N=Fimu/Flidar,且相邻数据块的时间间隔Δt为IMU采样周期,即Δt=1/Fimu,默认同一数据块时间间隔内激光雷达三轴角速度与线性加速度不变,进入步骤S4;
步骤S4,利用上述划分后的数据块,计算每个数据块中三轴平均角速度、三轴旋转角度,结合每个数据块中激光雷达三维点云数据点相对于第一个点的时间戳的时间偏移,计算划分出的数据块与每个数据块的三轴旋转角度,然后求得三轴旋转变换矩阵R,实现对每个激光雷达数据点进行三轴旋转运动补偿,进入步骤S5;
步骤S5,利用得到的三轴旋转变换矩阵R对每帧激光雷达三维点云数据中的每一个点进行三轴旋转补偿,并将补偿后的点云坐标系统一变换到帧尾坐标系,输出旋转补偿后的激光雷达三维点云数据,进入步骤S6;
步骤S6,将旋转运动补偿后的激光雷达三维点云数据分成两路输出,一路作为三轴平移运动补偿的源数据,另一路经过降采样、帧间运动量初步估计、扩展卡尔曼滤波完成三轴平移运动补偿;两路一起完成激光雷达三维点云数据的实时运动补偿。
2.根据权利要求1所述的一种基于IMU的激光雷达三维点云实时运动补偿方法,其特征在于,步骤S3根据IMU输出数据时序对每帧激光雷达点云数据进行数据块划分,具体如下:步骤S3.1,利用排序后的激光雷达三维点云数据和IMU数据,记录激光雷达的采样频率Flidar和IMU的采样频率Fimu,计算每帧激光雷达三维点云数据的分块数N,且相邻数据块的时间间隔等于IMU采样周期,默认每个数据块内IMU三轴角速度与线性加速度不变;
步骤S3.2,根据划分的数据块,计算输入的激光雷达三维点云数据中任一点P所属的数据块位置:
其中:Blockn为点P所属数据块,Ptimestamp为当前点的时间戳,Ptimeheader为每帧激光雷达三维点云中帧头时间戳,Δt为相邻数据块时间间隔。
3.根据权利要求1所述的一种基于IMU的激光雷达三维点云实时运动补偿方法,其特征在于,步骤S4中,计算每个数据块的三轴平均角速度,再根据三轴平均角速度计算三轴旋转角度,进而求得三轴旋转变换矩阵R,具体如下:步骤S4.1,根据划分后的数据块,计算每个数据块中的三轴平均角速度其中,x、y、z分别为每个激光雷达三维点云x轴坐标值、y轴坐标值、z轴坐标值,为每个激光雷达点云三维数据块中的三轴平均角速度,ωn(x,y,z)为IMU在tn时刻三轴旋转角速度,ωn+1(x,y,z)为IMU在tn+1时刻三轴旋转角速度,进入步骤S4.2;
步骤S4.2,根据三轴平均角速度计算三轴旋转角度θn:其中,θn为三轴旋转角度,x、y、z分别为每个三维点云x轴坐标值、y轴坐标值、z轴坐标值, 为每个数据块中的三轴平均角速度,Δt为相邻数据块时间间隔,进入步骤S4.3;
步骤S4.3,根据划分出的数据块,利用每个数据块的三轴旋转角度,对每个雷达三维点云数据进行三轴旋转运动补偿,得到三轴旋转变换矩阵R。
4.根据权利要求3所述的一种基于IMU的激光雷达三维点云实时运动补偿方法,其特征在于,步骤S4.3,根据划分出的数据块,利用每个数据块的三轴旋转角度,对每个激光雷达三维点云数据中的点进行三轴旋转运动补偿,得到三轴旋转变换矩阵R,具体如下:步骤S4.3.1,根据点P所属数据块的位置,计算点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和θsum;
θsum=θn+1+θn+2+…+θend其中:θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和,θn+1、θn+2、…θend分别为点P所属数据块到第n+1个数据块,n+2个数据块,...,最后一个数据块中三轴旋转角度,进入步骤S4.3.2;
步骤S4.3.2,计算点P相对于本数据块中最后一个点的三轴旋转角度其中: 为点P相对于本数据块中最后一个激光雷达三维点云数据的三轴旋转角度,ΔT为点P到其所在的第n个数据块尾部的时间之差, 为每个激光雷达三维点云数据的三轴平均角速度,Δt为相邻数据块的时间间隔,Ptimmestamp为当前点的时间戳,进入步骤S4.3.3;
步骤S4.3.3,利用得到的θsum和 计算点P相对与帧尾三轴旋转角度θP:其中:θP为点P到帧尾三轴旋转角度,θsum为点P所属数据块到帧尾之间所有数据块的三轴旋转角度之和, 为点P相对于本数据块中最后一个数据块的三轴旋转角度,进入步骤S4.3.4;
步骤S4.3.4,利用点P相对于帧尾三轴旋转角度,分别计算点P在x轴的旋转量Rroll、在y轴的旋转量Rpitch、在z轴的旋转量Ryaw:其中:Ryaw为点P在z轴的旋转量,Rpitch为点P在y轴的旋转量,Rroll为点云数据在x轴的旋转量,进入步骤S4.3.5;
步骤S4.3.5,计算点P相对于帧尾坐标系的旋转变换矩阵R:R=RyawRpitchRroll。