激光雷达点云运动畸变补偿

转自：https://zhuanlan.zhihu.com/p/109379384

一、概述

在基于机械雷达的定位系统中，点云畸变补偿是必须要做的事情，因为按照机械雷达的原理，有运动就有畸变。

畸变产生的原因是一帧点云中的点不是在同一时刻采集的，在采集过程中，雷达随着载体在运动，但是雷达点测量的是物体和雷达之间的距离，所以不同激光点的坐标系就不一样了。

解决这一问题，就需要把采集过程中雷达的运动计算出来，并在对应的激光点上补偿这个运动量。

大家接触比较多的运动畸变补偿代码应该是loam里面的那段，这里稍有些不同，首先，雷达的点云输出一般是按照列排列，而此处经过第三方程序对kitti数据进行二次加工后，它变成了行排列，这种排列方式的改变会导致补偿方法有所区别。另一点不同是，loam使用了高频率的imu数据进行补偿，从原理上讲，这样确实更精确，尤其是高动态环境下，但是稍显复杂，对于车来讲，运动并不剧烈，没有高频运动，所以我们在点云的一个采集周期（100ms）内可以认为它是匀速运动，这样实现起来就简单多了。

二、点云畸变原理

我们更详细地解释一下这个事。

首先要说一帧点云的产生过程。对于kitti数据集来讲，使用的是velodyne HDL 64E，就是这个像全家桶的东西。

这个雷达在纵向上排列着64个激光发射器和接收器，也就是说，它一次发射和接收会得到一列64个点。这64个发射器沿着不同的水平角发射，在HDL 64E中，最大的水平角是2°，最小的水平角是-24.8°，中间的水平角均匀变化，相邻的两个发射器的水平角相差大概0.4°。在采集过程中，这一列设备绕着竖直方向旋转，在100ms内旋转一周，在旋转过程中，不断地发射和接收，就会得到n列激光点，这些激光点共同构成了一帧点云。HDL 64E中，这个n是4500，也就是水平方向上分辨率是0.08°。

由于竖直方向分辨率是0.4°，水平方向上分辨率是0.08°，即竖直方向间隔要比水平方向大很多，所以我们看到的点云都是一圈一圈的。

由于雷达计算激光点坐标时，都是以接收到激光束时刻的雷达自身坐标系为基础的，所以载体运动过程中，每一列激光点的基准坐标系都是不一样的，但是他们在同一帧点云里，我们希望能统一在同一个坐标系下，所以我们需要知道每次采集时的坐标系相对于初始时刻坐标系的转换关系。

到这里，我们就找到了问题的核心，只要计算出来每个激光束接收时刻，雷达相对于初始时刻的相对运动就可以了，它就是我们需要的坐标系转换关系，然后这个激光点坐标乘以这个转换关系，就转换成了在初始坐标系下的激光点了。

三、畸变补偿方法

我们再把这个问题分解一下，补偿可分为计算和转换两步，如下

1. 计算相对坐标

在匀速模型假设前提下，坐标 = 运动×时间，所以又可以分解为两步：

1）获取载体运动信息

运动信息在原始数据里已经存在，包括角速度、速度，分别用来计算相对角度和相对位移。而且数据是我们已经做好时间同步的。

2）获取该激光点相对于起始时刻的时间差

由于是顺序扫描，我们可以很容易通过atan2(y, x)来计算出该激光点相对于第一个激光点旋转过的角度 ，我们知道雷达内部旋转360°用了100ms，那么旋转 角度用了多长时间，就了然了。

2. 转换激光点

其实就是坐标系×向量，坐标系是转换矩阵，向量是转换之前的激光点坐标，这个转换可以先旋转再平移，也可以用4X4矩阵一次性计算，都行，不是核心问题。