一种二维激光雷达的运动补偿方法、装置及系统与流程

1.本发明涉及二维激光雷达运动补偿领域，特别涉及一种二维激光雷达的运动补偿方法、装置及系统。


背景技术：

2.二维激光雷达(laser radar)，是以发射激光束探测目标的位置、速度等特征量的雷达系统。其工作原理是向目标发射探测信号(激光束),然后将接收到的从目标反射回来的信号(目标回波)与发射信号进行比较,作适当处理后,就可获得目标的有关信息,如目标距离、方位、高度、速度、姿态、甚至形状等参数,从而对飞机、导弹等目标进行探测、跟踪和识别。它由激光发射机、光学接收机、转台和信息处理系统等组成，激光器将电脉冲变成光脉冲发射出去，光接收机再把从目标反射回来的光脉冲还原成电脉冲。
3.二维激光雷达对机器人在未知环境中的位姿估计具有重要的作用。但是高扫描频率和多线二维激光雷达价格昂贵，大大限制了二维激光雷达的发展和应用。单线的、低扫描频率的二维激光雷达虽然成本较低，但由于激光扫描频率低导致测量时存在畸变。
4.低成本的二维激光雷达以低速旋转，并以多个范围组成的子帧的形式产生观测结果。这些范围结合起来形成一个完整的扫描，可以提供给扫描匹配器。而在合并子帧以生成完整扫描时，必须考虑装置自身的运动。没有任何运动补偿方法的激光扫描会影响运动估计和映射的精度。


技术实现要素：

5.有鉴于此，本发明提出了一种二维激光雷达的运动补偿方法、装置及系统，具体方案如下：
6.第一部分，本技术提出了一种二维激光雷达的运动补偿方法，适用于包括二维激光雷达、惯性测量单元和里程计的装置，方法包括：
7.使所述二维激光雷达在第一时段内以第一预设频率执行采样动作，得到子扫描数据，将该子扫描数据作为初始子扫描数据；
8.使所述里程计和所述惯性测量单元在第一时段内以第二预设频率同步执行采样动作，分别得到里程计数据和惯性数据；
9.根据同一采集时刻的里程计数据和惯性数据计算装置在该采集时刻的姿态数据，获取装置在第一时段内各个采集时刻的姿态数据；
10.基于所述第一预设频率与所述第二预设频率的关系，结合每个子扫描数据所对应的采集时刻，计算各个子扫描数据所对应的姿态数据，得到校正姿态数据；
11.各个姿态数据和各个校正姿态数据按时间顺序排序共同构成姿态数据集合，基于各个校正姿态数据的时间关系，计算所述姿态数据集合中各个姿态数据对应的子扫描数据，得到补偿子扫描数据；
12.融合初始子扫描数据和补偿子扫描数据，得到补偿后的子扫描数据。
13.在一个具体实施例中，所述校正姿态数据的获取过程包括：
14.基于第一预设频率，确定各个子扫描数据在第一时段中所处的采集时刻；
15.基于第二预设频率，确定各个姿态数据在第一时段中所处的采集时刻；
16.确定各个子扫描数据和各个姿态数据之间的时间顺序关系，以得到每个子扫描数据在各个姿态数据中所对应的插值位置；
17.基于插值位置相邻的n个姿态数据，插值得到各插值位置所对应的姿态数据，综合各插值位置的姿态数据得到校正姿态数据；其中，n为大于1的自然数。
18.在一个具体实施例中，所述第一预设频率小于所述第二预设频率。
19.在一个具体实施例中，基于插值位置处前后各两个姿态数据进行线性插值。
20.在一个具体实施例中，所述第一预设频率不超过10hz；
21.和/或，所述第二预设频率不小于50hz。
22.在一个具体实施例中，在同一采集时刻：
23.基于里程计数据中的线速度计算装置的坐标，以此来估计装置的位置信息；
24.基于惯性数据得到欧拉角中的偏航角，并将所述偏航角转换为二维旋转矩阵，以此来估计装置的航向信息；
25.综合位置信息和航向信息得到该采集时刻的姿态数据。
26.在一个具体实施例中，所述二维旋转矩阵的表达式为：
[0027][0028]
其中，ψ表示偏航角，r表示二维旋转矩阵；
[0029]
所述姿态数据的表达式为：
[0030][0031]
其中，t表示位置信息，表示在在tb采集时刻下的姿态数据。
[0032]
第二部分，本技术提出了一种带有二维激光雷达的装置，用于实现第一部分任一项所述的运动补偿方法；
[0033]
所述装置包括：
[0034]
二维激光雷达，用于在第一时段内以第一预设频率执行采样动作，得到子扫描数据，将该子扫描数据作为初始子扫描数据；
[0035]
里程计，用于在第一时段内以第二预设频率执行采样动作，得到里程计数据；
[0036]
惯性测量单元，用于在第一时段内以第二预设频率于所述里程计同步执行采样动作，得到惯性数据；
[0037]
主控电路，用于根据同一采集时刻的里程计数据和惯性数据计算装置在该采集时刻的姿态数据，获取装置在第一时段内各个采集时刻的姿态数据；
[0038]
基于所述第一预设频率与所述第二预设频率的关系，结合每个子扫描数据所对应的采集时刻，计算各个子扫描数据所对应的姿态数据，得到校正姿态数据；
[0039]
各个姿态数据和各个校正姿态数据按时间顺序排序共同构成姿态数据集合，基于各个校正姿态数据的时间关系，计算所述姿态数据集合中各个姿态数据对应的子扫描数
据，得到补偿子扫描数据；
[0040]
融合初始子扫描数据和补偿子扫描数据，得到补偿后的子扫描数据。
[0041]
第三部分，本技术提出了一种二维激光雷达的运动补偿系统，包括如下：
[0042]
第一采集模块，用于使二维激光雷达在第一时段内以第一预设频率执行采样动作，得到子扫描数据，将该子扫描数据作为初始子扫描数据；
[0043]
第二采集模块，用于使里程计和惯性测量单元在第一时段内以第二预设频率同步执行采样动作，分别得到里程计数据和惯性数据；
[0044]
姿态计算模块，用于根据同一采集时刻的里程计数据和惯性数据计算装置在该采集时刻的姿态数据，获取装置在第一时段内各个采集时刻的姿态数据；
[0045]
校正姿态计算模块，用于基于所述第一预设频率与所述第二预设频率的关系，结合每个子扫描数据所对应的采集时刻，计算各个子扫描数据所对应的姿态数据，得到校正姿态数据；
[0046]
补偿数据计算模块，用于将各个姿态数据和各个校正姿态数据按时间顺序排序以构成姿态数据集合，基于各个校正姿态数据的时间关系，计算所述姿态数据集合中各个姿态数据对应的子扫描数据，得到补偿子扫描数据；
[0047]
数据融合模块，用于融合初始子扫描数据和补偿子扫描数据，得到补偿后的子扫描数据。
[0048]
在一个具体实施例中，所述校正姿态计算模块具体包括：
[0049]
基于第一预设频率，确定各个子扫描数据在第一时段中所处的采集时刻；
[0050]
基于第二预设频率，确定各个姿态数据在第一时段中所处的采集时刻；
[0051]
确定各子扫描数据和各个姿态数据之间的时间顺序关系，以得到每个子扫描数据在各个姿态数据中所对应的插值位置；
[0052]
基于插值位置相邻的n个姿态数据，插值得到各插值位置所对应的姿态数据，综合各插值位置的姿态数据得到校正姿态数据；其中，n为大于1的自然数。
[0053]
有益效果：本发明提供了一种二维激光雷达的运动补偿方法、装置及系统，基于里程计和惯性测量单元实现低成本二维激光雷达的运动补偿，确保最终的子扫描数据是基于同一个位置扫描到的，最大程度降低装置偏移带来的运动失真干扰。基于高频的里程计数据和imu数据能得到精准的姿态数据，通过插值得到同样具备高精度的校正姿态数据。将姿态数据对应的子扫描数据融入到二维激光雷达所获取的子扫描数据中，不仅实现了对二维激光雷达的运动补偿，还能提升数据的精度，进一步扩展子扫描数据的数据量。
附图说明
[0054]
图1是本发明实施例1提出的运动补偿方法流程图；
[0055]
图2是本发明实施例2提出的装置的结构示意图；
[0056]
图3是本发明实施例3提出的运动补偿模块示意图。
[0057]
附图标记：1-二维激光雷达；2-里程计；3-惯性测量单元；4-主控电路；a1-第一采集模块；a2-第二采集模块；a3-姿态计算模块；a4-校正姿态计算模块；a5-补偿数据计算模块；a6-数据融合模块。
具体实施方式
[0058]
在下文中，将更全面地描述本发明公开的各种实施例。本发明公开可具有各种实施例，并且可在其中做出调整和改变。然而，应理解：不存在将本发明公开的各种实施例限于在此公开的特定实施例的意图，而是应将本发明公开理解为涵盖落入本发明公开的各种实施例的精神和范围内的所有调整、等同物和/或可选方案。
[0059]
需要说明的是，本技术中的位姿，包括位置和姿态。
[0060]
需要说明的是，本技术中的采集时刻，是指第一时段内执行采样动作的时刻。二维激光雷达存在采集时刻，里程计和imu存在相同的采集时刻。一般情况下，二维激光雷达的采集时刻，与里程计或imu的采集时刻不同，且里程计和imu的采集时刻明显多于二维激光雷达的采集时刻。
[0061]
实施例1
[0062]
本发明实施例1公开了一种二维激光雷达的运动补偿方法，将里程计数据和imu数据，融入到二维激光雷达所获取的子扫描数据中，不仅实现对二维激光雷达的运动补偿，还能进一步扩展子扫描数据。运动补偿方法的具体流程如说明书附图1所示。
[0063]
一种二维激光雷达的运动补偿方法，包括如下步骤：
[0064]
101、使二维激光雷达在第一时段内以第一预设频率执行采样动作，得到子扫描数据，将该子扫描数据作为初始子扫描数据；
[0065]
102、使里程计和惯性测量单元在第一时段内以第二预设频率同步执行采样动作，分别得到里程计数据和惯性数据；
[0066]
103、根据同一采集时刻的里程计数据和惯性数据计算装置在该采集时刻的姿态数据，获取装置在第一时段内各个采集时刻的姿态数据；
[0067]
104、基于第一预设频率与第二预设频率的关系，结合每个子扫描数据所对应的采集时刻，计算各个子扫描数据所对应的姿态数据，得到校正姿态数据；
[0068]
105、各个姿态数据和各个校正姿态数据按时间顺序排序共同构成姿态数据集合，基于各个校正姿态数据的时间关系，计算姿态数据集合中各个姿态数据对应的子扫描数据，得到补偿子扫描数据；
[0069]
106、融合初始子扫描数据和补偿子扫描数据，得到补偿后的子扫描数据。
[0070]
本实施例的运动补偿方法适用于包括二维激光雷达、惯性测量单元和里程计的装置，装置的结构如说明书附图2所示。二维激光雷达以子扫描的形式给出观察结果，其中每个子扫描由多个范围组成，并且可以通过合并所有子扫描来形成完整的扫描数据。里程计优选高频的轮式里程计，通过轮子转速来测量装置行程，以提升补偿精度。imu(inertial measurement unit)，即惯性测量单元，用于测量物体三轴姿态角(或角速率)以及加速度。
[0071]
在本实施例中，二维激光雷达是一种低转速、低扫描频率、低成本的激光雷达。一般情况下，二维激光雷达在采集时，要尽可能的保证原地旋转，而激光雷达被搭载于装置上，装置会不可避免的产生偏移，加上二维激光雷达的扫描频率低，使其极易受到运动失真的影响。
[0072]
本实施例的运动补偿方法基于高频车轮里程计和imu(惯性测量单元)的运动方程来估计二维激光雷达观察到的子扫描帧中每个点的校正位姿。考虑到装置的运动，一旦估计了某点的位姿，那么每个子扫描中的所有点都将转换为激光雷达观察到的第一个点的
帧，以消除装置偏移带来的影响并完成当前扫描。
[0073]
例如，二维激光雷达在a点，理想情况下是沿着a点原地旋转采集360度的数据。二维激光雷达首先采集到了b1点的数据，在接下来的采集过程中，装置逐渐发生偏移，使二维激光雷达偏离a点，采用本实施例的运动补偿方法，最终得到的数据是以a点为基准采集的数据。
[0074]
二维激光雷达、惯性测量单元和里程计都搭载于装置上，在同一时段内执行采样动作。其中，二维激光雷达在第一时段内以第一预设频率执行采样动作，得到子扫描数据，将该子扫描数据作为初始子扫描数据。第一预设频率是二维激光雷达的采集频率，低成本二维激光雷达的转速较低，第一预设频率不超过10hz。通常情况下，第一预设频率约为5-6赫兹，采样的数据不仅达不到精度要求，还极易受到运动失真的影响。
[0075]
而惯性测量单元和里程计则是以第二预设频率同步执行采样动作，确保每个采集时刻的惯性数据都有对应的里程计数据。里程计和惯性测量单元是相对于二维激光雷达的高频采样，第二预设频率不小于50hz。第二预设频率相对于第一预设频率来说高频。惯性测量单元测得惯性数据可用来计算装置的方向，里程计测得的里程计数据可用来确定各个采集时刻装置的位置。
[0076]
具体地，基于里程计数据中的线速度计算装置的坐标，以此来估计装置的位置信息。在本实施例中，装置的位置信息以坐标形式表示。通过对里程计数据的线速度可以估计位置差。
[0077]
基于惯性数据得到欧拉角中的偏航角，并将偏航角转换为二维旋转矩阵，以此来估计装置的航向信息。imu中三轴陀螺仪返回来的数据是三个轴上的角速度，经过积分之后就会得到3个角度(欧拉角)，这三个欧拉角分别围绕着载体坐标系旋转的。其中，欧拉角θ、ψ分别对应绕x、y、z轴旋转。由于需要估计当前子扫描数据的二维姿态，因此欧拉角中只涉及绕z轴旋转的偏航角ψ。将偏航角ψ转换为二维旋转矩阵，如下所示：
[0078][0079]
其中，ψ表示偏航角，r表示二维旋转矩阵；综合位置信息和航向信息得到该采集时刻的姿态数据。姿态数据的表达式为：
[0080][0081]
其中，t表示位置信息，表示在tb采集时刻下的姿态数据。
[0082]
在本实施例中，通过插值来估计每个子扫描数据对应的姿态数据。每组惯性数据和里程计数据也都涉及时间，得到的每个姿态数据也都具备时序性。每个子扫描数据都涉及时间，基于子扫描数据与姿态数据之间的时间关系，能够有效得到每个子扫描数据在姿态数据中所处的位置。基于各个姿态数据之间的关系，能够插值得到子扫描数据所对应的姿态，以得到校正姿态数据。
[0083]
优选地，校正姿态数据的获取过程包括：基于第一预设频率，确定各个子扫描数据在第一时段中所处的采集时刻；基于第二预设频率，确定各个姿态数据在第一时段中所处
的采集时刻；确定各子扫描数据和各个姿态数据之间的时间顺序关系，以得到每个子扫描数据在各个姿态数据中所对应的插值位置；基于插值位置相邻的n个姿态数据，插值得到各插值位置所对应的姿态数据，综合各插值位置的姿态数据得到校正姿态数据；其中，n为大于1的自然数。
[0084]
在实际应用中，主要通过线性插值的方法获取插值的数据。例如，计算插值位置前后各两个姿态数据的差，比照插值位置的时间点得到该插值位置的校正姿态数据。
[0085]
得到各个子扫描数据对应的校正姿态数据后，按时间顺序将校正姿态数据插入姿态数据中，得到姿态数据集合。姿态数据集合相当于基于高频采样得到子扫描数据的姿态，再求解出姿态数据集合所有数据的子扫描数据，得到高频的子扫描数据。
[0086]
具体地，由于姿态数据集合中包括校正姿态数据和姿态数据，校正姿态数据对应的子扫描数据是已知的，基于校正姿态数据和子扫描数据之间的关系、校正姿态数据与姿态数据之间的关系，即可求解各个姿态数据对应的子扫描数据。二维激光雷达测得的子扫描数据以及各个姿态数据的子扫描数据融合，即可得到完整的子扫描数据，且该子扫描数据是以激光雷达扫描第一个点时装置所处位置为基准。
[0087]
最终获取的子扫描数据不仅在样本数量上增加了，而且样本质量得到了提升，保证所有的子扫描数据都是基于同一个点测得的。
[0088]
示例性的，本实施例提供了一种运动反馈方法的框架流程。
[0089]
输入参数：
[0090]
1.二维激光雷达以6hz的采样频率测得的数据：s1、s2、s3
……
sn；
[0091]
2.里程计以50hz的采样频率测得的数据，经过计算得到各个采集时刻的装置位置信息：(x1，y1)—t1；(x2，y2)—t2
……
(xm，ym)—tm；
[0092]
3.惯性测量单元以50hz的采样频率测得的数据，经过计算得到各个采集时刻的装置航向信息：r1、r2、r3
……
rm；
[0093]
计算：
[0094]
1.根据t1和r1确定姿态数据p1，根据t2和r2确定姿态数据p2
……
根据tm和rm确定姿态数据pm；得到p1、p2、p3
……
pm；
[0095]
2.若s1对应的时刻位于p3和p4之间，则可以利用插值法在p3和p4之间插值，利用p2、p3、p4、p5得到s1对应的姿态p11；完成s1-sn所有的姿态计算，得到p11、p12
……
p1n；
[0096]
3.将p11、p12
……
p1n融合到p1、p2、p3
……
pm中，得到p1、p2、p3、p11、p4
……
p1n
……
pm；
[0097]
4.基于p1、p2、p3、p11、p4
……
p1n
……
pm确定p1、p2、p3
……
pm对应的子扫描数据，得到s1’、s2’、s3
’……
sm’；
[0098]
5.融合s1’、s2’、s3
’……
sm’和s1、s2、s3
……
sn，得到完整且准确的子扫描数据。
[0099]
本实施例提供了一种二维激光雷达的运动补偿方法，将里程计和imu采集的数据，融入到二维激光雷达所获取的子扫描数据中，不仅实现对二维激光雷达的运动补偿，还能进一步扩展子扫描数据。
[0100]
实施例2
[0101]
本实施例提供了一种带有二维激光雷达的装置，该装置可用于实现实施例1的一种二维激光雷达的运动补偿方法，装置结构如说明书附图2所示。具体方案如下：
[0102]
二维激光雷达1，用于在第一时段内以第一预设频率执行采样动作，得到子扫描数据，将该子扫描数据作为初始子扫描数据；
[0103]
里程计2，用于在第一时段内以第二预设频率执行采样动作，得到里程计2数据；
[0104]
惯性测量单元3，用于在第一时段内以第二预设频率于所述里程计2同步执行采样动作，得到惯性数据；
[0105]
主控电路4，用于根据同一采集时刻的里程计2数据和惯性数据计算装置在该采集时刻的姿态数据，获取装置在第一时段内各个采集时刻的姿态数据；基于所述第一预设频率与所述第二预设频率的关系，结合每个子扫描数据所对应的采集时刻，计算各个子扫描数据所对应的姿态数据，得到校正姿态数据；各个姿态数据和各个校正姿态数据按时间顺序排序共同构成姿态数据集合，基于各个校正姿态数据的时间关系，计算所述姿态数据集合中各个姿态数据对应的子扫描数据，得到补偿子扫描数据；融合初始子扫描数据和补偿子扫描数据，得到补偿后的子扫描数据。
[0106]
主控电路4可将获得的子扫描数据传递至扫描匹配器进行应用，用以根据子扫描数据创建地图和定位等。
[0107]
优选地，本实施例的装置可以是清洁装置，如扫地机器人等。
[0108]
实施例3
[0109]
本实施例提供了一种二维激光雷达1的运动补偿系统，基于里程计2和imu实现二维激光雷达1的运动补偿。运动补偿系统的结构框图如说明书附图3所示。具体方案如下：
[0110]
一种二维激光雷达1的运动补偿系统，包括如下：
[0111]
第一采集模块a1，用于使二维激光雷达1在第一时段内以第一预设频率执行采样动作，得到子扫描数据，将该子扫描数据作为初始子扫描数据；
[0112]
第二采集模块a2，用于使里程计2和惯性测量单元3在第一时段内以第二预设频率同步执行采样动作，分别得到里程计2数据和惯性数据；
[0113]
姿态计算模块a3，用于根据同一采集时刻的里程计2数据和惯性数据计算装置在该采集时刻的姿态数据，获取装置在第一时段内各个采集时刻的姿态数据；
[0114]
校正姿态计算模块a4，用于基于第一预设频率与第二预设频率的关系，结合每个子扫描数据所对应的采集时刻，计算各个子扫描数据所对应的姿态数据，得到校正姿态数据；
[0115]
补偿数据计算模块a5，用于将各个姿态数据和各个校正姿态数据按时间顺序排序以构成姿态数据集合，基于各个校正姿态数据的时间关系，计算姿态数据集合中各个姿态数据对应的子扫描数据，得到补偿子扫描数据；
[0116]
数据融合模块a6，用于融合初始子扫描数据和补偿子扫描数据，得到补偿后的子扫描数据。
[0117]
其中，校正姿态计算模块a4具体包括：基于第一预设频率，确定各个子扫描数据在第一时段中所处的采集时刻；基于第二预设频率，确定各个姿态数据在第一时段中所处的采集时刻；确定各子扫描数据和各个姿态数据之间的时间顺序关系，以得到每个子扫描数据在各个姿态数据中所对应的插值位置；基于插值位置相邻的n个姿态数据，插值得到各插值位置所对应的姿态数据，综合各插值位置的姿态数据得到校正姿态数据；其中，n为大于1的自然数。
[0118]
本发明提供了一种二维激光雷达的运动补偿方法、装置及系统，基于里程计和惯性测量单元实现低成本二维激光雷达的运动补偿，确保最终的子扫描数据是基于同一个位置扫描到的，最大程度降低装置偏移带来的运动失真干扰。基于高频的里程计数据和imu数据能得到精准的姿态数据，通过插值得到同样具备高精度的校正姿态数据。将姿态数据对应的子扫描数据融入到二维激光雷达所获取的子扫描数据中，不仅实现了对二维激光雷达的运动补偿，还能提升数据的精度，进一步扩展子扫描数据的数据量。
[0119]
本领域技术人员可以理解附图只是一个优选实施场景的示意图，附图中的模块或流程并不一定是实施本发明所必须的。本领域技术人员可以理解实施场景中的装置中的模块可以按照实施场景描述进行分布于实施场景的装置中，也可以进行相应变化位于不同于本实施场景的一个或多个装置中。