CN112083433A - 一种应用于两轮移动机器人的激光雷达除畸变方法 - Google Patents

Abstract

本发明公开了一种应用于两轮移动机器人的激光雷达除畸变方法，包括：获取一帧激光雷达中每一个激光点的原始位姿数据；获取第一时间段内里程计的第一位姿数据；获取第二时间段内IMU的加速度数据以及IMU的Z轴的角速度数据；利用IMU的Z轴的角速度数据对原始位姿数据进行第一次除畸变；利用IMU的加速度数据对里程计的第一位姿数据进行补偿得到补偿数据；利用补偿数据对第一次除畸变后的数据进行第二次除畸变，得到激光雷达最终的除畸变数据。本发明能够准确的反应激光雷达运动情况，并且能够实现激光雷达去除畸变和状态估计解耦。

Description

一种应用于两轮移动机器人的激光雷达除畸变方法

技术领域

本申请属于机器人建图和定位技术领域，具体涉及一种应用于两轮移动机器人的激光雷达除畸变方法。

背景技术

移动机器人作为一种新的运输工具，表现了巨大的优越性。不仅能够减轻劳动强度，提高生产效率，而且能够从生产模式方面把人们从危险、恶劣、繁重的工作环境中解放出来。从上个世纪80年代开始，许多国家便开始对移动机器人的研究。

激光雷达对机器人在未知环境中的建图和位姿估计具有重要的作用。但是高扫描频率和多线激光雷达价格昂贵，大大限制了室内服务移动机器人的发展和应用。而单线的、低扫描频率的激光雷达价格便宜、技术成熟、应用广泛。但应用于两轮移动机器人时，在扫描地形时会出现畸变。出现畸变的原因有：激光扫描频率，激光点的数据不是瞬时获取的；激光帧率低，机器人在运动过程中的位移不能忽略；在激光点测量时，两轮机器人晃动。

里程计有很高的更新频率(串口通信可以达到200HZ)，可以准确的反应机器人在一段时间内的运动情况，IMU可以准确的测量机器人的x轴方向加速度，可以准确的反应两轮移动机器人x轴方向的晃动。但是在利用里程计对激光雷达的数据进行除畸变时存在颠簸的情况，导致除畸变的准确率不高。

发明内容

本申请的目的在于提供一种应用于两轮移动机器人的激光雷达除畸变方法，能够准确的反应激光雷达运动情况，并且能够实现激光雷达去除畸变和状态估计解耦。

为实现上述目的，本申请所采取的技术方案为：

一种应用于两轮移动机器人的激光雷达除畸变方法，所述两轮移动机器人安装有激光雷达、里程计和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的安装位置之间的坐标转换矩阵。

作为优选，所述利用IMU的加速度数据对里程计的第一位姿数据进行补偿得到补偿数据，包括：

取数组Accarray1，通过线性插值处理数组Accarray1中的数据，并将处理后得到的数据存放在数组Accarray2中，数组Accarray2的长度为l_ol；

将队列Odomlist1中的第一位姿数据依次与数组Accarray2中对应的元素相叠加：

其中，(x_i，y_i，θ_i)为队列Odomlist1中第i+1个第一位姿数据，a_i为数组Accarray2中第i+1个加速度数据，0≤i≤(l_ol-1)，Δt_o为相邻两个第一位姿数据的时间间隔；

将叠加后得到的补偿数据存放在队列Odomlist2中，相邻两个补偿数据的时间间隔为Δt_o，队列Odomlist2的长度为l_ol，队列Odomlist2的队首存放t_a时刻的补偿数据，队列Odomlist2的队尾存放t_b时刻的补偿数据。

作为优选，所述利用补偿数据对第一次除畸变后的数据进行第二次除畸变，得到激光雷达最终的除畸变数据，包括：

令t_g＜t_s＜t_p，且t_s时刻不存在对应的补偿数据与第一次除畸变后的数据相等，t_g和t_p时刻存在对应的补偿数据与第一次除畸变后的数据相等，即

为第一次除畸变后得到的队列Lidarlist2中t_g时刻对应的数据，

为第一次除畸变后得到的队列Lidarlist2中t_p时刻对应的数据，

为队列Odomlist2中t_g时刻对应的补偿数据，

为队列Odomlist2中t_p时刻对应的补偿数据；

根据公式计算t_s时刻的插值点

如下：

同理计算得到t_e时刻的插值点

并在t_s时刻到t_e时刻之间计算得到另外3个任意时刻的插值点；

利用线性插值在这5个插值点中的两相邻插值点之间***

个数据，将插值后生成的所有数据存入队列Lidarlist3中作为激光雷达最终的除畸变数据，完成二次除畸变。

本申请提供的应用于两轮移动机器人的激光雷达除畸变方法，利用IMU的Z轴的角速度数据对原始位姿数据进行第一次除畸变。由于IMU对航向角比较敏感，因此在除畸变过程中可以采用IMU进行第一次除畸变，实现数据的初步修正，有利于后期得到更好的修正结果；利用IMU的加速度数据对里程计进行补偿，以降低里程计受机器人前后晃动而产生位置漂移颠簸带来的畸变；最后利用补偿后准确度较高的补偿数据对第一次除畸变后的数据进行二次修正，可有效完成激光雷达的除畸变，以便于能够准确的反应激光雷达运动情况。

附图说明

图1为本申请的应用于两轮移动机器人的激光雷达除畸变方法的流程图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

其中一个实施例中，提供一种应用于两轮移动机器人的激光雷达除畸变方法，用于对激光雷达获取的数据进行除畸变修正，以便于两轮移动机器人(简称机器人)的建图和定位。该两轮移动机器人至少安装有激光雷达、里程计和IMU，并且激光雷达、里程计和IMU安装的数量至少为一个。本实施例重点在于对激光雷达数据除畸变，对激光雷达、里程计和IMU的数量不做严格限制。

本实施例以激光雷达、里程计和IMU均为一个为例进行说明，若有多个激光雷达、里程计和IMU，则可以将一个激光雷达、里程计和IMU作为一套数据硬件进行除畸变，或者利用多个里程计的数据均值或者多个IMU的数据均值对激光雷达数据进行除畸变。

如图1所示，本实施例中的应用于两轮移动机器人的激光雷达除畸变方法，包括以下步骤：

步骤S1、数据获取。

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

本实施例将获取原始位姿数据按照时间顺序存放在队列Lidarlist1中，t_s时刻获取的原始位姿数据存放在队列Lidarlist1的队首，记作

t_e时刻获取的原始位姿数据存放在队列Lidarlist1的队尾，记作

一帧激光雷达中每一个激光点的时间间隔为Δt_r，因此队列Lidarlist1的长度为l_rl。激光发射器在激光雷达内部进行匀速的旋转，每旋转一个小角度即发射一次激光，轮巡一定的角度后，就生成了一帧完整的数据。

容易理解的是，由于位姿数据包括x轴坐标，y轴坐标以及方位角，因此本实施例采用队列的形式存储位姿数据，但并不限定队列存储形式为唯一的存储形式，例如还可以将位姿数据拆分存储在3个独立的数组中。

(2)获取第一时间段内里程计的第一位姿数据，所述第一时间段的起始时间为t_a、结束时间为t_b，并且t_a＜t_s＜t_e＜t_b，即获取里程计的数据的时间范围要涵盖住一帧激光的时间，以保证后续数据处理的有效性。

获取的第一位姿数据具有多种储存形式，可以是队列、数组或者索引指针等。本实施例为了降低存储压力并且保证数据读写速度，提供一种优选的存储形式如下：

所述第一时间段内获取的里程计的第一位姿数据按照时间顺序存放在队列Odomlist1中，t_a时刻采集的第一位姿数据存放在队列Odomlist1的队首，记作

t_b时刻采集的第一位姿数据存放在队列Odomlist1的队尾，记作

相邻两个第一位姿数据的时间间隔为Δt_o，因此队列Odomlist1的长度为l_ol。

(3)获取第二时间段内IMU(Inertial measurement unit，惯性测量单元，是测量物体三轴姿态角(或角速率)以及加速度的装置)的加速度数据以及IMU的Z轴的角速度数据，所述第二时间段的起始时间为t_m、结束时间为t_n，并且t_m＜t_a＜t_b＜t_n，即获取IMU的数据的时间范围要涵盖住里程计获取数据的时间范围。

由于IMU可以同时获取加速度数据和角速度数据，因此本实施例中同时获取同一时刻的加速度数据和角速度数据，并在获取后进行数据拆分并储存。

其中对于加速度数据的存储本实施例提供的一种优选方式为：所述第二时间段内获取的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。

同理加速度数据和角速度数据的存储形式不仅限于上述说明的形式，以上仅为一种优选的存放方法。

考虑到硬件成本以及数据量的大小，取里程计数据输出频率大于IMU数据输出频率，IMU数据输出频率大于激光雷达数据输出频率，并且一次除畸变中获取的数据长度满足l_ol＞l_al＞l_rl的关系。

步骤S2、利用IMU的Z轴的角速度数据对原始位姿数据进行第一次除畸变。由于IMU对航向角比较敏感，因此在除畸变过程中可以采用IMU进行第一次除畸变，实现数据的初步修正，有利于后期得到更好的修正结果。

常用的除畸变操作可以是利用线性插值进行，但本实施例为了保证第一次除畸变的力度，避免出现过除或除畸变效果不明显等问题，提供一种优选的除畸变方法如下：

取数组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的安装位置之间的坐标转换矩阵。并且R是3*3的正矩阵。

在第一次除畸变中，以R矩阵对更新方位角σ_k后的队列Lidarlist1进行转换，从而进一步提升除畸变的有效性，避免由于部件安装位置导致除畸变效果不佳。

步骤S3、利用IMU的加速度数据对里程计的第一位姿数据进行补偿得到补偿数据。由于里程计会受机器人前后晃动而产生位置漂移颠簸，为了降低这种漂移颠簸带来的畸变，本实施例加入IMU，IMU会时时测出激光雷达随着车体摆动的加速度，因此利用IMU的加速度数据对里程计进行补偿。

数据补偿的方式可以是比例补偿、简单叠加等方式。本实施例结合IMU和里程计的特性，提供一种优选的补偿方式为：

取数组Accarray1，通过线性插值处理数组Accarray1中的数据，并将处理后得到的数据存放在数组Accarray2中，数组Accarray2的长度为l_ol。

将队列Odomlist1中的第一位姿数据依次与数组Accarray2中对应的元素相叠加：

其中，(x_i，y_i，θ_i)为队列Odomlist1中第i+1个第一位姿数据，a_i为数组Accarray2中第i+1个加速度数据，0≤i≤(l_ol-1)，Δt_o为相邻两个第一位姿数据的时间间隔。

将叠加后得到的补偿数据存放在队列Odomlist2中，相邻两个补偿数据的时间间隔为Δt_o，队列Odomlist2的长度为l_ol，队列Odomlist2的队首存放t_a时刻的补偿数据，队列Odomlist2的队尾存放t_b时刻的补偿数据。

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

经过第一次除畸变后激光雷达的数据已部分修正，此时利用补偿后准确度较高的补偿数据进行二次修正，可有效完成激光雷达的除畸变，以便于能够准确的反应激光雷达运动情况。

本实施例中二次除畸变采用线性插值实现，具体过程如下：

令t_g＜t_s＜t_p，且t_s时刻不存在对应的补偿数据与第一次除畸变后的数据相等，t_g和t_p时刻存在对应的补偿数据与第一次除畸变后的数据相等，即

为第一次除畸变后得到的队列Lidarlist2中t_g时刻对应的数据，

为第一次除畸变后得到的队列Lidarlist2中t_p时刻对应的数据，

为队列Odomlist2中t_g时刻对应的补偿数据，

为队列Odomlist2中t_p时刻对应的补偿数据。在数据查找过程中，t_g和t_p时刻在队列Odomlist2中分别强制找一个数据与队列Lidarlist2中相等。

根据公式计算t_s时刻的插值点

如下：

同理计算得到t_e时刻的插值点

并在t_s时刻到t_e时刻之间计算得到另外3个任意时刻的插值点。插值点的计算以

为例，其他插值点的计算过程相同，本实施例中不再一一赘述。

利用线性插值在这5个插值点中的两相邻插值点之间***

个数据，将插值后生成的所有数据存入队列Lidarlist3中作为激光雷达最终的除畸变数据，完成二次除畸变。

在二次除畸变时计算5个时刻的插值点是因为本实施例使用的激光雷达为思岚a2，该雷达的扫描频率为10赫兹，即扫描一周的时间是0.05秒。通过直线性插值处理，激光雷达的频率是20赫兹，也就是再***5个点，相当于100赫兹，这样可以假设误差为0。

本实施例中采用的线性插值处理方法为现有的线性插值法，即在相邻两点之前***一个均值，时间均值处理和数据均值处理，针对线性插值处理方法本实施例中未提到的部分细节可参考现有技术中实现，本实施例中不做进一步限制。

当然容易理解的是，利用线性差值法进行二次畸变处理能够增加畸变后数据的柔和性，避免出现突变过大，并且数据的合理性也更强，在其他实施例中，也可以采用二次或是三角函数拟合，通过最小二乘等做到二次除畸变。

最终得到的除畸变数据为准确度较高的激光点的位姿数据，将每一个激光点的位姿数据转换成角度和距离数据，然后打包发布成激光束，即可用于机器人建图或定位等用途。

需要说明的是，本实施例为了便于描述，采用以时刻为基准对数据进行描述以及以个数为基准对数据进行描述两种方法，但由于所采集的数据均是以时间顺序进行存放的，因此每个时刻的数据与个数的数据均有相互对应。例如，t_a时刻采集的第一位姿数据存放在队列Odomlist1的队首，记作

存放在队首也理解为时队列Odomlist1中的第1个数据，即

其他同理理解。

应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (7)

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的加速度数据对里程计的第一位姿数据进行补偿得到补偿数据；

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

2.如权利要求1所述的应用于两轮移动机器人的激光雷达除畸变方法，其特征在于，所述原始位姿数据按照时间顺序存放在队列Lidarlist1中，t_s时刻获取的原始位姿数据存放在队列Lidarlist1的队首，记作

t_e时刻获取的原始位姿数据存放在队列Lidarlist1的队尾，记作

一帧激光雷达中每一个激光点的时间间隔为Δt_r，因此队列Lidarlist1的长度为l_rl。

3.如权利要求2所述的应用于两轮移动机器人的激光雷达除畸变方法，其特征在于，所述第一时间段内获取的里程计的第一位姿数据按照时间顺序存放在队列Odomlist1中，t_a时刻采集的第一位姿数据存放在队列Odomlist1的队首，记作

t_b时刻采集的第一位姿数据存放在队列Odomlist1的队尾，记作

相邻两个第一位姿数据的时间间隔为Δt_o，因此队列Odomlist1的长度为l_ol。

4.如权利要求3所述的应用于两轮移动机器人的激光雷达除畸变方法，其特征在于，所述第二时间段内获取的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的关系。

5.如权利要求4所述的应用于两轮移动机器人的激光雷达除畸变方法，其特征在于，所述利用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的安装位置之间的坐标转换矩阵。

6.如权利要求5所述的应用于两轮移动机器人的激光雷达除畸变方法，其特征在于，所述利用IMU的加速度数据对里程计的第一位姿数据进行补偿得到补偿数据，包括：

取数组Accarray1，通过线性插值处理数组Accarray1中的数据，并将处理后得到的数据存放在数组Accarray2中，数组Accarray2的长度为l_ol；

将队列Odomlist1中的第一位姿数据依次与数组Accarray2中对应的元素相叠加：

其中，(x_i，y_i，θ_i)为队列Odomlist1中第i+1个第一位姿数据，a_i为数组Accarray2中第i+1个加速度数据，0≤i≤(l_ol-1)，Δt_o为相邻两个第一位姿数据的时间间隔；

将叠加后得到的补偿数据存放在队列Odomlist2中，相邻两个补偿数据的时间间隔为Δt_o，队列Odomlist2的长度为l_ol，队列Odomlist2的队首存放t_a时刻的补偿数据，队列Odomlist2的队尾存放t_b时刻的补偿数据。

7.如权利要求6所述的应用于两轮移动机器人的激光雷达除畸变方法，其特征在于，所述利用补偿数据对第一次除畸变后的数据进行第二次除畸变，得到激光雷达最终的除畸变数据，包括：

令t_g＜t_s＜t_p，且t_s时刻不存在对应的补偿数据与第一次除畸变后的数据相等，t_g和t_p时刻存在对应的补偿数据与第一次除畸变后的数据相等，即

为第一次除畸变后得到的队列Lidarlist2中t_g时刻对应的数据，

为第一次除畸变后得到的队列Lidarlist2中t_p时刻对应的数据，

为队列Odomlist2中t_g时刻对应的补偿数据，

为队列Odomlist2中t_p时刻对应的补偿数据；

根据公式计算t_s时刻的插值点

如下：

同理计算得到t_e时刻的插值点

并在t_s时刻到t_e时刻之间计算得到另外3个任意时刻的插值点；

利用线性插值在这5个插值点中的两相邻插值点之间***

个数据，将插值后生成的所有数据存入队列Lidarlist3中作为激光雷达最终的除畸变数据，完成二次除畸变。

Images