[发明专利]一种应用于两轮移动机器人的激光雷达除畸变方法

权利要求书

1.一种应用于两轮移动机器人的激光雷达除畸变方法，所述两轮移动机器人安装有激光雷达、里程计和IMU，其特征在于，所述应用于两轮移动机器人的激光雷达除畸变方法，包括：

获取一帧激光雷达中每一个激光点的原始位姿数据，该帧激光雷达中起始激光点的时间为t_s、末尾激光点的时间为t_e；

获取第一时间段内里程计的第一位姿数据，所述第一时间段的起始时间为t_a、结束时间为t_b，并且t_a＜t_s＜t_e＜t_b；

获取第二时间段内IMU的加速度数据以及IMU的Z轴的角速度数据，所述第二时间段的起始时间为t_m、结束时间为t_n，并且t_m＜t_a＜t_b＜t_n；

利用IMU的Z轴的角速度数据对原始位姿数据进行第一次除畸变；

利用IMU的加速度数据对里程计的第一位姿数据进行补偿得到补偿数据；

利用补偿数据对第一次除畸变后的数据进行第二次除畸变，得到激光雷达最终的除畸变数据；

其中，所述原始位姿数据按照时间顺序存放在队列Lidarlist1中，t_s时刻获取的原始位姿数据存放在队列Lidarlist1的队首，记作t_e时刻获取的原始位姿数据存放在队列Lidarlist1的队尾，记作一帧激光雷达中每一个激光点的时间间隔为Δt_r，因此队列Lidarlist1的长度为l_rl；

其中，所述第一时间段内获取的里程计的第一位姿数据按照时间顺序存放在队列Odomlist1中，t_a时刻采集的第一位姿数据存放在队列Odomlist1的队首，记作t_b时刻采集的第一位姿数据存放在队列Odomlist1的队尾，记作相邻两个第一位姿数据的时间间隔为Δt_o，因此队列Odomlist1的长度为l_ol；

其中，所述第二时间段内获取的IMU的加速度数据按照时间顺序存放在数组Accarray1中，数组Accarray1的第一个数据存放t_m时刻采集的加速度数据，数组Accarray1的最后一个数据存放t_n时刻采集的加速度数据，相邻两个加速度数据的时间间隔为Δt_u，数组Accarray1的长度为l_al；

所述第二时间段内获取的IMU的Z轴的角速度数据按照时间顺序存放在数组Warray1中，数组Warray1的第一个数据存放t_m时刻采集的角速度数据，数组Warray1的最后一个数据存放t_n时刻采集的角速度数据，相邻两个角速度数据的时间间隔为Δt_u，数组Warray1的长度为l_al，并且存在l_ol＞l_al＞l_rl的关系；

其中，所述利用IMU的Z轴的角速度数据对原始位姿数据进行第一次除畸变，包括：

取数组Warray1，将数组Warray1中的每个角速度数据乘以时间间隔Δt_u，得到存放在数组Warray2中，表示数组Warray1中的第K+1个角速度数据，0≤k≤(l_al-1)；

利用数组Warray2更新队列Lidarlist1中的原始位姿数据：取队列Lidarlist1中第K+1个原始位姿数据Lidarlist1[K]中的方位角σ_k，更新为

将更新σ_k后的队列Lidarlist1中的每一个数据以x轴坐标，y轴坐标和方位角组成的列向量左乘以一个R矩阵，记为队列Lidarlist2，完成第一次除畸变，其中R矩阵为已知的激光雷达的安装位置与IMU的安装位置之间的坐标转换矩阵。