一种IMU和激光雷达融合的三维点云运动去畸变方法

本发明涉及自动驾驶，特别是涉及一种imu(惯性测量单元)和激光雷达(lidar)融合的三维点云运动去畸变方法，通过融合imu提供的三轴加速度和三轴角度数据，与激光雷达采集的三维点云数据进行结合，从而有效地矫正由于传感器运动引起的点云畸变。

背景技术：

1、在自动驾驶和机器人导航等应用中，激光雷达(lidar)和惯性测量单元(imu)传感器是关键的感知设备。lidar传感器能够高精度地捕捉环境的三维点云数据，而imu传感器则提供关于设备运动状态的三轴加速度和角速度信息。然而，由于车辆或设备在运动过程中，lidar采集到的点云数据可能会受到传感器运动引起的畸变影响，导致点云数据的精度和一致性降低。

2、传统的方法中，点云数据处理通常是依赖于单一传感器的数据，难以应对复杂的动态环境变化。而现有的点云去畸变方法往往忽视了传感器融合的优势，没有充分利用imu提供的动态运动信息来对lidar点云进行实时校正。这样的处理方式在实际应用中，特别是针对于矿区自动驾驶项目和在厂区作业的叉车等工程类自动驾驶车辆，存在明显的局限性。

3、为了提高点云数据的精度和一致性，本专利提出了多种传感器融合的方法。通过将imu提供的三轴加速度和三轴角速度数据与lidar采集的三维点云数据进行融合，可以有效地矫正传感器运动引起的点云畸变。然而，目前的研究和应用中，对于如何高效地进行这种数据融合，以及如何在动态环境中实现实时的畸变补偿，仍然存在许多技术难题。

4、已有研究表明，车辆的运动状态对点云数据的准确性影响显著。例如，在路面坑洼较多的矿区作业场景或者工厂内的叉车作业场景，车辆的加速、减速和转弯等动态运动会导致点云数据发生显著畸变。传统的基于静态模型的点云处理方法无法适应这种动态变化，导致数据处理结果的可靠性下降。

5、因此，本专利提出一种基于imu和激光雷达融合的三维点云运动去畸变算法显得尤为重要。通过融合imu提供的动态运动信息，可以实现对lidar点云数据的实时校正，显著提高点云数据的精度和一致性。这对于自动驾驶领域在矿车或无人叉车等工程项目中的落地应用至关重要。

技术实现思路

1、本发明的目的是针对现有技术中点云数据易发生显著畸变的问题，而提供一种imu和激光雷达融合的三维点云运动去畸变方法。

2、为实现本发明的目的所采用的技术方案是：

3、一种imu和激光雷达融合的三维点云运动去畸变方法，包括以下步骤：

4、步骤1，从imu传感器获取三轴加速度数据(accx、accy、accz)和三轴角速率数据(gyrox、gyroy、gyroz)，从激光雷达传感器获取三维点云数据(x，y，z，intensity，ring，time)；

5、步骤2，将激光雷达传感器和imu传感器标定到同一坐标系下；

6、步骤3，采用队列同步的方法，将坐标系转换后的imu数据和lidar数据进行对齐，使其在相同时间戳下进行处理；

7、步骤4，imu数据和lidar数据对齐后，利用imu提供的三轴加速度数据和三轴角速率数据，对lidar原始点云进行运动畸变补偿，根据车辆的运动状态，对lidar原始点云数据进行实时矫正。

8、在上述技术方案中，所述三维点云运动去畸变方法还包括步骤5，将矫正、配准后的点云数据发布，并显示在车机交互平台上。

9、在上述技术方案中，所述步骤1中，通过ubuntu系统下的ros通讯机制，实现三轴加速度数据、三轴角速率数据以及三维点云数据的收发与融合

10、在上述技术方案中，所述步骤2中的标定方法包括以下步骤：

11、设imu坐标系中的点为pimu，lidar坐标系中的点为plidar，两者的关系为：plidar＝rpimu+t，其中：r是3×3的旋转矩阵，用于描述imu到lidar坐标系的旋转关系，t是3×1的平移向量，用于描述imu坐标系到lidar坐标系的平移关系，外参包括r和t。

12、在上述技术方案中，利用最小二乘法用于进一步优化外参r和t：

13、

14、pimu,i为i时刻的imu坐标系中的点，plidar,i为i时刻的lidar坐标系中的点。

15、在上述技术方案中，所述步骤3中，假设imu数据队列为qimu，lidar数据队列为qlidar，对于每一帧lidar数据plidar,i，其时间戳为tlidar,i，从imu队列qimu中找到与tlidar,i最近的imu数据timu,j，然后判断imu数据的时间戳timu,j是否在允许的时间范围内:

16、|tlidar,i-timu,j|≤δtmax

17、其中，δtmax是预设的最大容忍时间差；

18、如果imu数据的时间戳timu,j与lidar数据的时间戳tlidar,i不完全对齐，使用线性插值方法计算imu数据在tlidar,i时刻的估计值。

19、在上述技术方案中，线性插值公式如下:

20、

21、其中，pimu,interp是imu数据在tlidar,i时刻的估计值，pimu,j时间戳为timu,j的imu数据，tlidar,i为i时刻的lidar数据的时间戳，timu,j为j时刻的imu数据的时间戳，timu,j+1为紧随timu,j之后的时间戳，pimu,j+1是时间戳紧随timu,j之后的imu数据。

22、在上述技术方案中，所述步骤4中，假设车辆在时间间隔δt内的线速度和角速度保持不变，则点云数据点的运动表示为：

23、δx＝v·δt·cos(θ)

24、δy＝v·δt·sin(θ)

25、

26、其中，δx、δy、δz分别为点云数据点在x、y、z轴方向的位移补偿量，δt为时间间隔，θ为点云数据点的初始角度，通过点云数据点的初始位置计算得到，为车辆运行过程中的俯仰角，x、y、z为点云数据点的初始位置坐标，v为车辆的线速度，

27、在上述技术方案中，将点云数据点的初始位置和运动补偿结合起来，得到补偿后的新位置：点云数据点的位置补偿表示为:

28、

29、其中，x、y、z为点云数据点的初始坐标，x′、y′、z′为点云数据点的补偿后新坐标，δx、δy、δz分别为点云数据点在x、y、z轴方向的位移补偿量；

30、在上述技术方案中，根据imu提供的角速度数据，计算旋转矩阵r′:

31、

32、在上述技术方案中，点云数据点的角度变化如下式所示：

33、δθ＝ω·δt

34、其中，ω为车辆的角速度，

35、与现有技术相比，本发明的有益效果是：

36、1.相对于传统运动畸变补偿算法，本发明在三个维度上针对于矿区或者工厂等作业环境恶劣的工程车辆的工作场景进行点云运动畸变的剔除，对lidar点云数据的实时校正，显著提高点云数据的精度和一致性。

37、2.本发明算法流程清晰，结构简单，不需要较大的算力要求，易于实现。