[发明专利]一种用于移动机器人的激光雷达点云运动畸变校正方法

权利要求书

1.一种用于移动机器人的激光雷达点云运动畸变校正方法，其特征在于，所述方法包括以下步骤：

S1.利用二维激光雷达采集一帧激光点云数据，与此同时，采集轮速计和惯性测量单元的数据并将两者数据使用扩展卡尔曼滤波进行融合，得到机器人坐标系下的融合位姿序列；

S2.已知机器人坐标系与二维激光雷达坐标系的转换关系，将机器人融合位姿序列转换为二维激光雷达位姿序列，该位姿序列描述了单帧激光点云数据采集过程中二维激光雷达的运动过程；

S3.根据收到数据的时间戳和当前激光帧整体扫描时间，估计第一个激光点云时间戳，在此基础之上，根据激光点云的时间间隔，估计每一个激光点云的时间戳，针对每一个激光点云时间戳，通过与二维激光雷达位姿序列进行时间戳比对，找到其前后相邻的两个二维激光雷达位姿；

S4.利用单一激光点云前后相邻的两个二维激光雷达位姿和三者的时间戳，通过线性插值和球面线性插值分别求出对应的激光点云位姿的位移和旋转四元数，得到激光点云位姿序列，并记录最后一个激光点云的位姿和时间戳；

S5.以最后一个激光点云所在的坐标系为基准坐标系，利用激光点云位姿序列，将所有激光点云坐标转换至该坐标系；

S6.重新封装此一帧校正后的激光点云数据，其中该帧激光点云数据的时间戳为最后一个激光点云时间戳；

所述步骤S3中，当前帧激光雷达点云数有N个，激光雷达点云的时间戳按时间顺序的集合为：

K＝{k_i}_i＝0…N-1

其中两个激光雷达点云的扫描间隔时间为Δk，而k₀和k_N-1分别为起始点云时间戳和结束点云时间戳；为了获取每个激光点云位于激光雷达坐标系的位姿T_wi，首先通过设置时间戳条件k_lk_ik_r，寻找激光点云时间戳k_i前后相邻的两个二维激光雷达位姿T_wl和T_wr，其中w表示世界坐标系，k_l和k_r是距离k_i最近的时间戳；最后根据比例分别对两个二维激光雷达位姿T_wl和T_wr的位移和旋转进行线性插值和球面线性插值，得到激光雷达点云位姿T_wi；

所述步骤S4中，根据比例分别对两个二维激光雷达位姿T_wl和T_wr的位移和旋转进行线性插值和球面线性插值，得到激光雷达点云位姿T_wi；

在移动机器人运动过程中，由于每个激光点云以二维激光雷达坐标系为基准坐标系，而该坐标系跟随移动机器人运动，使得每个激光点云都有独立的基准坐标系，为了校正当前帧的激光雷达点云数据，将所有激光点云的基准坐标系统一到最后一个激光点云的基准坐标系，所述变换位姿为：

激光雷达点云校正之前的坐标为p_iw，校正之后的坐标为：

p_(N-1)w＝T_(N-1)i·p_iw。