基于 Livox 激光雷达的自运动畸变算法开源

一、什么是激光雷达自运动畸变

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

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

二、自运动畸变产生的本质以及校正

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

激光雷达点云自运动畸变的形态，与其扫描方式是相关的。

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

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

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

图2 点云坐标系发生变化

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

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

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

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

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

我们以  表示  在雷达坐标系1 中的坐标，坐标系 i 到 j 的转换表示为。则一帧点云中每个点到第一个点坐标系的转化分别为, , ……，用下式可轻松将对应点坐标转到第一个点所在坐标系：

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

在实际应用中，一般首先设法测量激光雷达的运动信息，如一帧点云首尾（100ms间隔）的雷达位姿变化 T。然后根据某点到初始点或末尾点之间的时间差 Δt，通过短时匀速假设进行线性插值得到该点的。

位姿变化 T 可通过惯性导航系统（INS）或激光雷达里程计（LiDAR Odometry, 如 LIO-Livox ）提供的位姿信息获得。若使用惯性测量单元（IMU，可提供角速度以及加速度信息）计算位姿变化，则需要额外提供雷达或者自车的初始速度信息。

那 Δt 的值怎么获得呢? 

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

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

下图 4 展示了去畸变后的点云。

三、自运动畸变校正工具使用及说明

上述去畸变的过程代码已上传 Github, 有兴趣的读者欢迎点击下方阅读原文查看。

https://github.com/Livox-SDK/livox_cloud_undistortionhttps://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 进行去畸变。

使用：

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

输出：校正后的点云 /livox_unidistort

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