自动驾驶定位系列教程七_ 点云畸变补偿

自动驾驶定位系列教程七: 点云畸变补偿

一、概述

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

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

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

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

附赠自动驾驶学习资料和量产经验：链接

本系列教程涉及完整代码V作者获取

二、点云畸变原理

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

首先要说一帧点云的产生过程。对于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°，即竖直方向间隔要比水平方向大很多，所以我们看到的点云都是一圈一圈的。像这样

假设我们把这个雷达放在一个圆柱形的建筑正中心，那么在静止状况下，如果我们只取64条线中的一条线，那么我们得到的就应该是一个圆环。就像下面这张丑图画的这样

那如果不是在静止情况下呢？比如雷达装在车上，车在原地逆时针旋转。

再上一张丑图

我们看到出现了两个正前方，即发射第一束激光时的正前方和发射最后一束激光时的正前方。在发射第一束时，就是它原来静止的位置，当发射最后一束时，相对于第一束时间已经过去了100ms，由于雷达自身在运动，此时的正前方往逆时针旋转了一定角度，所以出现了一个缺口。

如果是发生了平移，也可以画张图对应，为了便于理解，在平移时先不旋转。

原理类似，只不过这里是原点的变化，原点越往前，前方建筑物离自己就越近，得到的激光点距离就越小。

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

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

三、畸变补偿方法

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

1. 计算相对坐标

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

1）获取载体运动信息

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

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

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

2. 转换激光点

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

四、程序设计

通过上面的任务拆解，程序的设计思路应该就比较清晰了。需要注意的应该就只是程序结构问题。

由于点云补畸变是一个独立的功能，所以我们在models文件夹下新建一个文件夹scan_adjust，用于存放这个功能的类文件，专门用来补偿畸变。这个类需要运动信息，通过函数SetMotionInfo在每次补畸变之前传入。激光点坐标转换在AdjustCloud函数里，具体代码不贴了，内容和上一部分讲的基本一致。

我们在front_end_flow.cpp中调用这个类，其实就两行程序就搞定了

distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);
distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);

有两点需要注意：

第一点是，在kitti原始数据里，提供了每帧点云的起始采集时刻和终止采集时刻，kitti2bag这个功能包在把数据转成bag文件的过程中，利用起始时刻和终止时刻取了个平均值，即中间时刻，作为这一帧点云的采集时刻，上面的方法其实是把点全都转到起始时刻上去，所以我们在计算的每个激光点采集时刻上再减去50ms，这样就相当于把一帧点云的坐标系转到中间时刻对应的坐标系上去了。

第二点是，数据中提供的速度是IMU所处位置的速度，而我们要的是激光雷达所处位置的速度，由于这两者并不重合，即存在杆臂，所以在车旋转时他们的速度并不一致，需要按照这两者之间的相对坐标，把速度转到雷达对应的位置上去，这个功能我们放在了sensor_data的velocity_data.cpp，把它作为VelocityData类的成员函数，只要给他一个相对坐标，它就自动把类内部成员变量转换了，调用时就一行程序

current_velocity_data_.TransformCoordinate(lidar_to_imu_);

到这里，畸变补偿所增加的代码应该都提到了。

五、效果对比

按照上一篇文章提供的方法，在运行完程序之后，使用指令统计累计误差

evo_ape kitti ground_truth.txt laser_odom.txt -r full --plot --plot_mode xyz

把得到的结果和上一篇文章中没有做过畸变补偿的结果做对比。

从数据上看，还是有一些效果，另外，各位如果有兴趣，可以看一下建立的地图，畸变补偿除了提高定位精度以外，对地图质量，尤其是对消除路口转弯导致的地图重影有明显效果。

六、改进方向

从地图效果上看，有些路口仍然有重影，说明畸变还没有完全消除，回顾我们的这些做法，可以发现还有一些可以提高的地方，主要包括这样几个方面：

1. 时间精确度

我们刚才也提到，kitti原始数据是提供了每帧点云精确的起始采集时刻和终止采集时刻的，但是bag文件里就只剩下一个中间时刻了，导致我们只能把时间固定为100ms，但是如果我们从原始数据中做一些分析，发现并不是每帧都是100ms，而且很多帧会在此基础上相差十几个ms，这已经是相当大的比例了。

要解决这个问题，要从原始数据入手，那可能要自己写数据驱动了，至少要拿别人的改一改。

2. 点云数据排列方式

我们在概述中提到了这个问题，在这里详细解释一下。

做激光定位的多数都看过loam的程序，会发现在计算激光点采集时刻这一步上我们和它是有区别的，这就是这个点云排列方式导致的。

从激光雷达原理上讲，每次采集一列，边扫描边采集，所以数据应该是一列一列地存储，这样保证第一个点一定是最早时刻被采集的，最后一个点是最晚时刻被采集的，这样做有一个好处，那就是可以计算雷达真正扫描了多少度，因为它就是第一个点和最后一个点的角度差。

而在这个bag文件里，是按行存储的，也就是先索引第一根线上的点，再第二根，依次类推，直到最后一根，这样的缺点是，我们无法通过这个来计算雷达实际扫描角度了，因为第一个点不一定是时间最早的点，如果第一根线前半部分被遮挡，那么你就会得到错误的计算角度，所以这时候，像我们这样直接强制把扫描角度设置成360度是更稳妥的，缺点就是它不够精确。

这个问题的原因应该是数据的二次加工导致的，kitti先把数据存成bin文件，kitti2bag再转成bag文件，改变了原来的排列方式。一般雷达驱动中输出的数据都是按列排列的。

所以如果各位以后使用的数据是列排列的，那么时间计算上可以使用loam里的方法，会更准确。

3. 运动模型

匀速运动假设是否属于强假设，这个各位有兴趣可以自己试一试，比如把匀速运动模型改成匀加速，模型从一次变成了二次，看是否会更精确。