Cartographer中对激光雷达运动畸变的处理方法分析

任务动机：梳理cartographer处理激光雷达运动畸变的原理，并针对特殊数据特性的雷达数据做相应适配，进而提升建图效果。

任务描述：查阅cartographer源码中激光雷达运动畸变的处理流程，对比分析实际使用中特定激光雷达数据处理时可能产生的问题。针对于不同的实际问题作相应修改后，进行对比建图实验验证。

本文使用bag包下载：https://download.csdn.net/download/zhzwang/12821562

1. 激光雷达运动畸变的原因

        一帧激光点云由多个激光点组成，而这些激光点不是同一时刻产生的，所以在雷达运动过程中，一帧内的各个激光点基准不同，就会产生运动畸变。例如：当机器人旋转速度为1.57rad/s，此时雷达若频率为10hz，那么一帧激光点内的首尾两点基准会相差1.57 * 0.1 =0.157rad，即约为9°，在点云帧间匹配时该误差不能忽视。

2. Cartographer中对雷达运动畸变的处理

        Cartographer中对于激光雷达的运动补偿大致为以下流程，具体细节可从代码（pose_extrapolator）中查看。

1. 对于一帧激光数据中的每一点都赋予对应的时间戳；
2. 假设匀速运动模型，通过前面帧间匹配的位移除以时间间隔求出速度；
3. 根据每一个激光点对应的时间戳进行运动补偿。

3. Cartographer处理运动畸变中可能出现的问题

        Cartographer中处理雷达点云运动畸变时已经默认拿到的雷达数据已经足够准确，使用雷达驱动中发出的time_increament直接对单帧内的没一点赋值时间戳。而现实中激光雷达由于硬件设备问题，会出现两种异常情况：

3.1 问题一

        雷达点云打包发出的顺序与真实电机旋转扫描时的数据正好相反，对于cartographer计算过程中每个点对应时间戳恰好相反。即处理运动畸变时第一个激光点被假设成了最后一个激光点，结果适得其反。

3.2 问题二

        激光雷达的频率不稳定，例如官方标定为10HZ，但是实际运行中数据间隔可能徘徊与0.08-0.12s之间，只能保证平均间隔为0.1s，但在cartographer中收到的time_increament多数情况下是固定的，致使处理运动畸变时，单帧激光数据中的每个点无法对应其准确的时间戳，例如下面几个雷达数据（rostopic记录）连续七帧时间戳的对比。

	雷达一	雷达二	雷达三
扫描频率	15hz	13hz	28hz
理论帧间间隔（s）	0.066667	0.076923	0.03571
时间戳1（stamp.nsecs）	138875719	217470838	574852851
时间戳2（stamp.nsecs）	205275613	297503257	611164101
时间戳3（stamp.nsecs）	271705863	369562690	646372642
时间戳4（stamp.nsecs）	338044979	449569886	681654101
时间戳5（stamp.nsecs）	404755814	529576631	717150226
时间戳6（stamp.nsecs）	471542889	601488252	752839434
时间戳7（stamp.nsecs）	538550579	681515123	788386892

        可以看出，雷达一数据间隔为0.0664、0.06643、0.06634、0.06671、0.06679、0.06701，雷达二时间帧间间隔为0.08003s、0.07206s、0.08s、0.07192s、0.08003s，雷达三数据间隔为0.03631s、0.03521s、0.0352s、0.0355s、0.03569、0.03555s，相对于雷达一和雷达三，雷达二数据波动更为明显。对于这种波动，从数据源上影响cartographer建图效果。

4. 问题解决方法与实验对比

4.1 问题一解决方法

        可以从硬件驱动层面上来处理，通过雷达厂家上位机软件或者驱动程序使雷达输出反序。 如果不便于进行上述处理，对于问题1从算法层面修改，可以从rostopic读到的原始数据调整。cartographer中的对于每一个激光点point都带有一个时间戳，其计算方式为每一帧从0开始，计算方式为i * msg.time_increment，其中i为一帧激光点中的点索引，msg.time_increment为相邻两激光点间时间增量。详细代码可在cartographer_ros_extended-master/cartographer_ros/cartographer_ros/msg_conversion.cc中对于laserscan数据处理中可查到。从rostopic中读到的msg.time_increment多为固定值，不能适应于频率波动情况。可以修改msg_conversion.cc中的代码如下：

    angle += msg.angle_increment;
}
/********************** Adjust timestamp for point reverse ********************/
std::vector<float>time_tempx;
std::vector<float>time_tempy;
for (auto& point :point_cloud.points) {
    time_tempx.push_back(point.position.x());
    time_tempy.push_back(point.position.y());
}
int i_time = 0;
for (auto& point :point_cloud.points) {
    point.position.x() =  time_tempx[point_cloud.points.size()-1-i_time];
    point.position.y() =  time_tempy[point_cloud.points.size()-1-i_time];
    i_time++;
}
/********************** end *********************/ 
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);

        对应修改如上代码，可解决数据反序问题。 如下两图为未调整反序的建图效果（后端优化关闭）

 

        点云反序调整后建图效果（其余参数一样）

 

 

        第一幅对比图为雷达原地旋转情形，可以看出为调序的激光旋转一圈后地图出现明显重影，调整点云反序后原地旋转一圈激光与地图基本能够对齐。 第二幅地图为使用雷达建立的一个办公室场景（无回环），可以看出在调整点云反序后建图效果有明显提升。

4.2 问题二解决方法

        由于雷达在实际运行中频率不稳定导致建图受到影响，主要原因是多数雷达驱动发过来的topic中的time_increament是一个固定的值，而真实值却实时变化。解决方法为使用连续两帧之间的时间戳差值求出的实时time_increament代替原topic收到的固定值。修改msg_conversion.cc代码如下。

PointCloudWithIntensities point_cloud;
float angle = msg.angle_min;
/************Adjust timestamp for point frequency *********/
static double timestamp_current = 0 ;
static double timestamp_last = 0 ;
double time_increment_vary;
timestamp_last = timestamp_current;
timestamp_current = msg.header.stamp.toSec();
if(timestamp_last != 0) {
    time_increment_vary = (timestamp_current - timestamp_last)/(6.2831852/msg.angle_increment);
}
else time_increment_vary = msg.time_increment;
/************ end *********/
for (size_t i = 0; i < msg.ranges.size(); ++i) {
    const auto& echoes = msg.ranges[i];

        使用求解出的time_increment_vary代替源码中的msg.time_increment 如下

const cartographer::sensor::TimedRangefinderPoint point {
    rotation * (first_echo * Eigen::Vector3f::UnitX()), i * time_increment_vary
};//i * msg.time_increment
point_cloud.points.push_back(point);
if (msg.intensities.size() > 0)

        如下图为使用某雷达未调整实时频率建图效果

        调整实时频率建图效果

        可以看出长方形边有细微差别，调整实时频率后的建图边缘较为光滑，无明显重影。对于不同的雷达建图可能表现不同，某些雷达自身输出的频率较为稳定，波动不明显，则无需实时校准帧间时间间隔。对于帧间频率波动较大的激光雷达，建图效果会有提升。

5. 总结

        由于雷达自身硬件与数据输出问题导致建图受到影响，尤其是时间戳间隔的问题，往往会疏于考虑。博主也是在调试时偶然发现，这里进行梳理与总结，抛砖引玉，针对于问题二也需要更多款型的雷达进行试验，文中如有不妥之处，还望指正。如果之前没注意到这一点，调参好久也没有得到理想的地图，那么，快去查看自己使用的雷达的频率吧！