I. 什么是激光雷达自运动畸变?

 

激光雷达通过发射激光束来测量周围环境物体的距离和方位，从而判断车辆与障碍物的相对位置。当其发射的激光束足够多时，这一个个的激光点将汇集成一片点云，勾勒出其所处的三维环境信息，这便是我们常说的点云数据。

 

对于多数激光雷达而言，尽管激光的发射与接收很快，但构成点云的每一个点仍非同一时刻生成的。一般我们会将100ms (对应典型值10Hz) 内累积的数据作为一帧点云输出。若在这100ms内，激光雷达本体或安装所在的机体发生绝对位置的变化，那么此帧点云中每一个点的坐标系就是不同的。直观上看，这一帧点云数据就会发生一定的“变形”，不能真实对应所探测到的环境信息，类似于拍照时手抖了，拍出来的照片就会糊。这便是激光雷达的自运动畸变。

 

II. 自运动畸变产生的本质及校正

 

我们来具体看一看自运动畸变是什么样的。

激光雷达点云自运动畸变的形态，与其扫描方式是相关的。比如传统360度机械式激光雷达每一帧，是以雷达为中心环绕扫描一周（100ms）得到的。当雷达本体或所在车体静止时，扫描起始点和终止点可以比较好地闭合（坐标原点始终保持不变）。而当雷达或自车运动时，自运动畸变就会发生，环绕一圈的数据就会发生扭曲，导致环绕不再闭合（不同点的坐标原点不同）。

Figure 1. 360度机械式激光雷达自运动畸变示意

 

下面我们再深入分析一下这一现象的本质。

简单来说，激光雷达点云自运动畸变的产生本质上是一帧中每一个点的坐标系不同。

如下图，左图p1~p3表示激光雷达依次扫描到的三个位置点，这三点在真实世界中共线。但由于激光雷达自身在一帧时间内存在“剧烈”运动，如中间图所示，雷达自身分别在三个不同的实际姿态下对三个点进行了扫描。因此在最后得到的点云中（最右图），三个点坐标实际处于不同的坐标系，看起来不再共线了。

 

Figure 2. 点云坐标系发生变化

 

图2给出一个实际应用的例子。

搭载Livox激光雷达的车辆因自身掉头发生了自运动畸变：远处的墙体和车辆都因为自车快速旋转产生了分层现象。

 

Figure 3. 由于车体运动，路边停放的车辆点云出现分层

 

       

那么，自运动畸变怎么校正呢？显然，只要我们把车开得足够的慢……

当然不是，需要我们将这一帧内所有点的坐标系都转换到同一个，如图1第一个点p1所在的雷达坐标系，这本质上就是对雷达的运动进行补偿。

我们以p¹_i表示p_i在雷达坐标系1中的坐标，坐标系i到j的位姿变化表示为T ^j_i。则一帧点云中每个点到第一个点坐标系的变化分别为T¹₂、T¹₃、T¹₄ …… ，用下式可轻松将对应点坐标转到第一个点所在坐标系：

     

 

 

原理上看起来非常简单（实际也非常简单）。只要知道每个点的T¹i 就行了，那到底怎么知道呢？

在实际应用中，一般首先设法测量激光雷达的运动信息，如一帧点云首尾（100ms间隔）的雷达位姿变化T。然后根据某点到初始点或末尾点之间的时间差Δt，通过短时匀速假设进行线性插值得到该点的T¹i。而位姿变化T可通过惯性导航系统（INS）或激光雷达里程计 (如 LIO ). 提供的位姿信息获得。如使用惯性测量单元（IMU，可以提供角速度以及加速度信息）计算位姿变化，需要额外提供雷达或者自车的初始速度信息。

 

那 Δt 的值怎么获得呢? Livox 激光雷达输出自带每个点的时间戳。在获取点云的时候就可从点云数据包Custom Msg中直接读取到。而其他雷达则可能需要根据各自雷达的SDK所提供信息或自行手动解算得到每个点的时间戳。

按上述公式将每一帧的各个点坐标转换到同一坐标系，就是去畸变的过程了。

下图展示了去畸变前后的点云对比。

 

Figure 4 点云校正后效果对比

 

III. 自运动畸变校正工具使用及说明

 

我们已将上述去畸变的过程代码上传至github, 有兴趣的读者欢迎点击下方链接查看。

https://github.com/Livox-SDK/livox_cloud_undistortion

 

代码说明：

依赖：

livox_ros_driver

PCL

ROS

 

编译：在工作空间下使用指令

catkin_make

 

运行：

source devel/setup.bash

roslaunch livox_dedistortion_pkg run.launch

 

接口说明：

在data_process.h中定义了ImuProcess类，该类的成员函数UndistortPcl为去畸变函数，该函数参数中 Sophus::SE3d Tbe为当前帧点云帧头和帧尾之间的位姿，如果可以直接提供该位姿，则可以调用该函数进行去畸变。如果只有IMU数据，则调用ImuProcess的成员函Process进行去畸变。

 

特别说明：
平移畸变的校正需要用户根据各自的平移信息来源（GPS位置坐标、速度等）手动计算对应时间差下的平移变化，作为代码内函数UndistortPcl输入。

 

使用：

输入：

        此工具基于ros开发，因此输入信息为两个topic，点云topic为/livox/lidar, customMsg格式，IMU信息topic为/livox/imu

输出：

输出为校正后的点云/livox_unidistort