从零开始做自动驾驶定位(四): 前端里程计之初试

本文纯属转载，并认真学习一遍，感谢大佬分享！

本文章配套源代码地址：https://github.com/Little-Potato-1990/localization_in_auto_driving

测试数据：https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg 提取码: n9ys

本篇文章对应的代码Tag为 4.0

代码在后续可能会有调整，如和文章有出入，以实际代码为准

================================================================================

一、概述

上一篇我们介绍了怎样搭建一个ROS工程，并且根据我们的项目需求做一些改进。本篇所做的工作在上一篇的工程基础上进行，只是添加了前端里程计的算法进去。

前端里程计本质上其实就是一个点云匹配方法，通过匹配累计位姿而已，此处选用的是NDT匹配方法。本着先跑通再研究理论的原则，本篇先用它来实现功能，后面会专门介绍它的理论。

为了程序清晰，我们把前端里程计封装成一个类，输入和输出接口与原有工程保持一致，这样可以使程序更清晰。我们在include和src文件夹下各建一个名为front_end的文件夹，来存储这个类的头文件和源文件。执行功能的node文件在根目录下，名为front_end_node.cpp。

所以本篇文章的重点就是介绍这三个文件了。

二、里程计模块子功能介绍

设计一个类，核心就在于把它的功能切分成各个子功能，每个子功能对应一个模块去实现。

要设计里程计的类，就要了解它的工作流程，才方便知道怎样切分更合理。

我们不妨先小小梳理一下：

1）从接收到一帧点云开始，首先是要匹配，和地图匹配，如果它是第一帧数据，那么它就是地图，供下一帧匹配使用。

2）我们不能把每一帧匹配好的点云都加入地图，那样太大了，所以要提取关键帧，即每隔一段距离取一帧点云，用关键帧拼接成地图即可。

3）到这里，会想到一个问题，那就是地图会一直累加，那么我们一直用它匹配会导致很多不必要的计算量，所以应该还需要一个小地图，即把和当前帧一定距离范围内的关键帧找出来拼接即可，可以叫它滑窗

4）在匹配之前需要滤波，对点云稀疏化，不然匹配会非常慢。

5) 点云匹配还有一个特性，就是它对位姿的预测值比较敏感，所以在载体运动时，不能以它上一帧的位姿作为这一帧的预测值，可以使用IMU预测，也可以使用运动模型预测。

明白以上几步流程，后面模块的详细介绍就很容易理解了，我们开始吧

1. 点云匹配

我们直接使用*，用pcl库来做，后面的文章会介绍怎样自己实现ndt匹配方法。

首先定义一个ndt匹配方法的对象，记得初始化它

pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>::Ptr ndt_ptr_

然后设置ndt的匹配参数，暂时不用理解这些参数是啥，先混个脸熟，以后再介绍

ndt_ptr_->setResolution(1.0);
ndt_ptr_->setStepSize(0.1);
ndt_ptr_->setTransformationEpsilon(0.01);
ndt_ptr_->setMaximumIterations(30);

随后，输入点云。匹配自然是两坨点云，其中一坨就是小地图（滑窗），另一坨是当前帧

ndt_ptr_->setInputTarget(local_map_ptr_);
ndt_ptr_->setInputSource(filtered_cloud_ptr);

有了这些，就可以执行匹配，并获取位姿了

ndt_ptr_->setInputSource(filtered_cloud_ptr);
ndt_ptr_->align(*result_cloud_ptr_, predict_pose);
current_frame_.pose = ndt_ptr_->getFinalTransformation();

2. 关键帧

先定义一个关键帧结构体，其实就是在点云基础上加了个位姿矩阵

class Frame {
    public:  
      Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
      CloudData cloud_data;
};

目前设置的是每隔2米取一个关键帧，为了方便，直接采用曼哈顿距离。

if (fabs(last_key_frame_pose(0,3) - current_frame_.pose(0,3)) + 
    fabs(last_key_frame_pose(1,3) - current_frame_.pose(1,3)) +
    fabs(last_key_frame_pose(2,3) - current_frame_.pose(2,3)) > 2.0) {
    
    UpdateNewFrame(current_frame_);
    last_key_frame_pose = current_frame_.pose;
}

3. 滑窗

实现滑窗就是用一个deque容器把关键帧存储起来，关键帧超过一定数量，就把时间最靠前的关键帧给踢出去。

local_map_frames_.push_back(key_frame);
while (local_map_frames_.size() > 20) {
   local_map_frames_.pop_front();
}

4. 点云滤波

点云滤波是直接采用了pcl中的voxel_filter，它的基本原理就是把三维空间划分成等尺寸的立方体格子，在一个立方体格子内最多只留一个点，这样就起到稀疏作用。

由于小地图滤波和当前帧滤波采用的格子大小不一样，所以类内为这两个功能各定义了一个滤波器。

滤波格子大小决定了匹配的效率和精度，格子越小，点越多，精度越高，但是速度越慢，反之速度加快，精度下降。您可以通过自己调整参数，对比效果，来体会他们之间关系的实际情况。

5. 位姿预测

此处采用运动模型来做位姿预测，而没有用IMU，是为了方便在没有IMU时候仍然能够实现里程计功能。

具体实现方式也比较简单，假如当前帧是第k帧，那么用第k-2帧位姿和第k-1帧位姿就可以计算一个位姿变化量，车辆的运动是相对平缓的，所以在k-1帧位姿基础上累加这个位姿变化量，基本就是第k帧的预测值了。

step_pose = last_pose.inverse() * current_frame_.pose;
predict_pose = current_frame_.pose * step_pose;
last_pose = current_frame_.pose;

三、接口

一个类，除了子功能模块，基本也就剩下接口了。

此处和我们功能相关的接口有这样几个

1. 初始位姿输入

由于我们要把里程计轨迹和gnss轨迹做对比，所以把初始时刻gnss和imu给出的位姿作为里程计的初始位姿。

2. 地图输出

这个地图包括全局大地图和用来匹配的小地图，不过在里程计里，显示效果上前者包含后者。

3. 位姿输出

这个不必说了，里程计就是看这个的

四、实现效果

编译完成以后，启动程序

roslaunch lidar_localization front_end.launch

播放bag，等它运行完吧。运行过程中，rviz中会显示轨迹和滑窗小地图，大地图等bag播放完之后会显示。

这里需要注意的是，由于当前的滤波参数和匹配参数，算法运行还比较慢，而且越到后面越慢，完全不能够和bag播放速度达到同步，所以可以先降低bag播放速度，不然等bag播放完，程序没运行完，很多要释放的内存会得不到释放。

降低bag播放速度的指令如下，其中“-r 0.5”就是指0.5倍速率播放，可以根据自己电脑性能情况再调整这个值

rosbag play kitti_2011_10_03_drive_0027_synced.bag -r 0.5

运行完之后，里程计和gnss轨迹的对比如下，其中红色的是里程计，黄色的是gnss

点云图就是下面这个样子

五、总结与说明

到这一步，只是简单地实现了激光里程计的基本功能，它必然还有很多问题，我们会在后续改进它，比如

1. 速度加快

正常用ndt做匹配是能够实现在10HZ下实时的，不过要在点云稀疏上做一些特殊处理，而不是像现在这样用统一的立方格尺寸来稀疏，后续我们会把它提到实时的。

2. 进行内存管理

每个关键帧都存了点云，目前所有关键帧都放在内存里，必然要爆，即使不爆，会成为影响速度的原因之一。

其实除了小地图需要的关键帧放在内存里，其他的关键帧的点云可以先存储在硬盘里，等用到时候再读出来，这样就不会有内存大量积累情况，基本可以实现“硬盘有多大，地图就有多大”

3. 补运动畸变

对于机械雷达，补畸变是必须的，在当前程序运行过程中，您仔细观察会发现，在拐弯处匹配有晃动，而且匹配完之后生成的地图有重影，这就是畸变导致的。

4. 累计误差

对里程计来讲，这是不可避免的了，后续会加其他约束来消除累计误差。

5. 参数放在配置文件里

这样才方便调试嘛。

上一篇：从零开始做自动驾驶定位(三): 软件框架

下一篇：从零开始做自动驾驶定位(五): 前端里程计之代码优化