1.一种激光雷达点云运动畸变的矫正方法,其特征在于包括,步骤一:采用RTK作为脉冲发生器,同步触发激光雷达和IMU;对采集的激光雷达第一帧点云,以第一帧点云为原点建立全局坐标系;
步骤二:采集非第一帧的激光雷达当前帧点云,以当前帧点云为原点建立局部坐标系;
同时将当前帧点云采集时刻对应的IMU坐标系转换到所述局部坐标系下;并同步获得IMU位姿;
步骤三:根据所述局部坐标系,采用插值方法获得当前帧点云中每个数据点对应的IMU位姿,并根据所述IMU位姿矫正激光雷达当前帧点云运动畸变;
步骤四:将所述局部坐标系转换到所述全局坐标系下,再利用RTK位姿校准IMU位姿;然后返回步骤二,直到完成最后一帧激光雷达点云运动畸变的矫正。
2.根据权利要求1所述的激光雷达点云运动畸变的矫正方法,其特征在于,步骤二中,对于采集的激光雷达点云,在局部坐标系下,第k帧第i点的坐标为其中xki、yki、zki分别为局部坐标系下第k帧第i点的X、Y、Z轴的坐标,其中k∈(1,2,3...m),i∈(1,2,3...n),m为点云总帧数,n为帧数据包含的数据点数;
将激光雷达点云表述为矩阵形式为:
3.根据权利要求2所述的激光雷达点云运动畸变的矫正方法,其特征在于,步骤二中,所述IMU位姿用矩阵形式表达为:其中 为激光雷达点云第k帧第i点的位姿, 为激光雷达点云第k帧第i点到相应局部坐标系的旋转矩阵, 为激光雷达点云第k帧第i点到相应局部坐标系的平移矩阵;
为激光雷达点云第k帧第i点绕局部坐标系Z轴旋转的角度,β为激光雷达点云第k帧第i点绕局部坐标系Y轴旋转的角度,α为激光雷达点云第k帧第i点绕局部坐标系X轴旋转的角度;
式中sx为激光雷达点云第k帧第i点沿局部坐标系X轴的位移,sy为激光雷达点云第k帧第i点沿局部坐标系Y轴的位移,sz为激光雷达点云第k帧第i点沿局部坐标系Z轴的位移;
当i=0时,
式中 为当前激光雷达点云第k帧的起始位姿, 为当前激光雷达点云第k帧数据到相应局部坐标系的旋转矩阵,为当前激光雷达点云第k帧数据到相应局部坐标系的平移矩阵。
4.根据权利要求3所述的激光雷达点云运动畸变的矫正方法,其特征在于,步骤三中根据所述IMU位姿矫正激光雷达点云运动畸变包括:将激光雷达点云第k帧的帧头时间戳记为td,帧中时间戳记为te,帧尾时间戳记为tf;第k帧帧头前后两帧IMU位姿分别为 和 第k帧帧中前后两帧IMU位姿分别为 和 第k帧帧尾前后两帧IMU位姿为 和k
对IMU位姿插值,获得第k帧帧头、帧中和帧尾的位姿依次为 Pe和 再进行二次曲线拟合,得到点云位姿曲线拟合方程:k 2
Pt=At+Bt+C,
k
式中Pt为第k帧点云中每个数据点对应的位姿,t为时间,A为二次项系数,B为一次项系数,C为常数;
根据点云位姿曲线拟合方程矫正激光雷达当前帧点云运动畸变,得到:式中 表示第k帧第i点的矫正后点云坐标。
5.根据权利要求4所述的激光雷达点云运动畸变的矫正方法,其特征在于,步骤四中利用RTK位姿校准IMU位姿包括:根据局部坐标系下RTK位姿和IMU位姿,判断IMU位姿的时间戳是否小于RTK位姿的时间戳;
若IMU位姿的时间戳小于RTK位姿的时间戳,将RTK位姿作为IMU位姿观测值;若IMU位姿的时间戳大于或等于RTK位姿的时间戳,寻找当前帧IMU位姿时间戳的前后两帧RTK位姿,利用RTK插值得到的位姿作为IMU位姿观测值;
对局部坐标系下IMU位姿和所述IMU位姿观测值进行卡尔曼融合,得到校准后的IMU位姿。