【泡泡传感器评测】禾赛Pandar40P激光雷达评测（三）SLAM、感知算法和卡尔斯鲁厄车队
以下文章来源于泡泡机器人SLAM ，作者泡泡传感器评测
SLAM的最新资讯，干货内容。
我们希望架起一座定位传感器的桥梁，在导航定位技术进化的道路上，起到一点点促进作用
作者：冯喆
编辑：周平
欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权
目录
FSG-欧洲汽车顶级工程师的摇篮
针对FSG场景的激光雷达测试方案和评估标准
卡尔斯鲁厄理工（KIT）ka-raceing车队的“核武器”
SLAM pipeline
感知算法工作原理
多传感器校准方案
关于卡尔斯鲁厄理工（KIT）ka-raceing车队
04
SLAM pipeline
SLAM 流程图示意
我们目前使用的SLAM pipeline如上图所示，这里涉及到多个传感器信息的融合操作（multi-sensors fusion）。雷达获取的点云数据先进行预处理(pre-processing)，经过剪裁(clipper)，动态畸变矫正(distortion correction)，地面滤波(ground removal)后，我们根据赛事特点对点云特征进行聚类(clustering)和进一步的滤波，直接获取只属于桩桶的点云的position information，并将这个proposal传给分类器(classifier)。
分类器中，我们可以通过处理聚类点云并返回原始点云提取更多intensity信息从而做出桩桶颜色判断，也可以使用摄像头获取的图像信息估计桩桶的颜色信息，这是一个冗余(redundancy)设计, 最后通过一个简单的管理器评估一个最优结果，向下一个节点递交数据。
与此同时，我们使用一个卡尔曼滤波(Kalman filter)融合了轮速计，IMU，光学地面测速计的信息来提供一个稳定（robust）的速度估计，获取的动态信息用来做刚才提到的“点云动态畸变矫正”，也输入进姿态估计模块(pose estimation)。
获取landmarks后就可以用它们累积建图(accumulation)，同时把它们与位姿估计（pose estimation）的结果进行数据联结（date association），执行特征追踪（feature tracking）：通过关联“特征点”，计算帧与帧之间的位移，做点云配准（registration），并把不断获取的landmark信息和先验地图（prior map）进行匹配，完成回环检测（loop closure），从而通过这个节点不断修正位姿以及更新地图。
算法实现上，我们在本篇中重点介绍下识别桩桶，提取其特征的Pipeline。
经过了前期多种测试评估，我们确定了雷达在赛车上的安装点，也收集了雷达在各种速度下的原始点云数据包，用于接下来做雷达数据特征提取算法的测试。
05
感知算法工作原理
1.运动畸变补偿 Motion-caused Distortion Correction
我们在测试中发现，做过处理的点云在高速驾驶过程中，会出现一定程度的偏移。以直线跑为例，如果前方出现的桩桶分布是很规律的矩形结构，那速度越快，感知后累积出来的点云就更像是一个倾斜的平行四边形。后来我们发现，因为雷达的最快频率只有20hz，而每一帧的点云数据都是以雷达自身作为坐标系原点的，如果在0.05s的时间内，坐标原点发生了快速移动，它对于一帧内目标点的感知就会发生畸变。于是我们根据这篇论文（https://www.ifr.ing.tu-bs.de/static/files/forschung/papers/download_pdf.php?id=919）的思想对点云进行了error补偿。
在经典激光SLAM开源方案 loam中也有处理点云运动畸变的模块，感兴趣的可以在开发过程中同时参考。
这里做补偿比较关键的一点是，对瞬时矢量和速度要有比较好的预估。虽然我们有Kisler的optical Ground Speed Sensor作为比较精确的动态信息来源，但是我们还是用卡尔曼滤波融合IMU和轮速传感器的数据做了一个速度估计的容错设计，以防止比赛中某个传感器出现故障。
2.地面分割Ground Removal
我们根据日本名古屋大学的一篇论文（https://www.researchgate.net/publication/329612826_A_Slope-robust_Cascaded_Ground_Segmentation_in_3D_Point_Cloud_for_Autonomous_Vehicles）进行复现，这篇论文实现的算法原理很简单，多线机械旋转雷达因为发射器列式分布的原因，一般而言都会在完全平坦的地面上会产生经典的多环图案，环与环的间距与“线束角度分布和雷达安装高度”直接相关。而这时如果在地面上出现坡度差异或者有障碍物，这种间距会产生变化，这时可以根据对地形坡度的判断，引入一个角度阈值，去补偿这个坡度对间距造成的影响。
论文中介绍了两层滤波，为了防止上述第一层效果不好，作者又设计了一种基于网格划分的滤波，他将场地以LiDAR为中心画成蜘蛛网格形状，并在每一个网格中做Ransac算法。我们也实现了这种滤波，但在使用过程中，发现第一层滤波效果已经很好，而第二层滤波容易出现网格欠分割滤波效果差或者过分割资源消耗过多的问题。最后在应用上，我们只使用了第一种。具体的原理和实现方法请参见上面论文链接。
Tips: 深度学习是点云处理算法的一个发展趋势，我们也在开发一种语义分割（semantic segmentation）的方法，尝试解决这个问题。目前在不断测试赛车，收集用于训练的数据，大家敬请期待，这里可以推荐一种不错的神经网络架构。（https://github.com/xuanyuzhou98/SqueezeSegV2）
下图是滤波前后效果对比图的gif：
滤波前后效果对比（第一人称视角）
滤波前后效果对比（俯视视角）
3.网格聚类Occupancy Grid Clustering
聚类算法可以在这里让几何距离比较近的点云被识别成同类物体，因为要应用到赛车的工作环境，所以时间和空间复杂度自然是越小越好。我们观察到比赛场地除了各种尺寸相仿的桩桶和周围墙体外没有其他物体，所以需要聚类的点云，高度差基本都在0.35m以内，于是我们实现了一种基于2D网格的Occupancy Grid Clustering，在网格中对高度差在一定阈值范围内的点云进行聚类。实时性和聚类效果都不错。
Tip：我们实现的Euclidean Clustering（来源于PCL）和DBSCAN Clustering在这里的表现并不是很好，推荐大家都尝试一下。
网格聚类示意
黑色为聚类物体中心点
4.邻域检测滤波Neighborhood Detection Filter
为了进一步滤去周围环境物体形成的聚类，只留下桩桶的聚类，我们基于桩桶自身几何特点和分布规律开发了一种滤波。
1. 我们首先去除了体积远超过桩桶尺寸的聚类（比如周围围墙），剩下的聚类中心点成为集合A；
2. 接着考虑到动态赛中每四个桩桶都以一种面积差异不大的四边形结构分布（根据具体的动态赛项目有差异），根据这个特点，我们先设置两个阈值a和b，去尽可能贴近这种四边形的面积，见下图。然后对每个聚类的中心点以a做半径做领域检测，筛选出拥有四个以上领域点的中心点集C；
面积检测方法示意
3. 接着我们再进行高度检测，筛选出符合离地高度明显大于桩桶的点集B；
4.再进行如图所示的面积检测：找到邻域中（以a为半径）离中心点最远的点及离它们之间连线最远的点，获得两个距离m,n，再进一步筛选出符合（（mxn > axb）&&（n>b））的中心点集S；
5. 最后我们把做完地面滤波的原始点和聚类的中心点一起输入系统，检测中心点在一米范围内原始点的点集中是否有两个以上的相邻点，符合这个要求的成为中心点集M，从而得到我们检测出的属于桩桶的中心点集（A∩ C∩ B∩ S∩ M）。
我们设置了两个长度和宽度的阈值a和b
滤波处理后的效果如图所示：
红色物体为最终筛选出的桩桶点云
（赛车最高速度为18m/s左右）
5.激光雷达桩桶分类LiDAR Cone Classification
将上述算法识别出来桩桶的点云与原始点云比对，还原出更多因为滤波算法损失的点云，并将沿Z轴点云的intensity作为输入训练SVM（Support Vector Machine）。最后得到的SVM模型可以分类识别10m内的两种类别桩桶，效果如下图。
黄、蓝两种桩桶在禾赛雷达点云中明显的反射率差异
桩桶反射回来的intensity信息沿z轴（高度轴）呈现出来的直方图分布
黄、蓝桩桶识别结果（Rviz中没有特意显示颜色，但算法已经做出区分）
6.激光雷达和摄像头融合 Fusion with Camera and IMU
为了给系统增加更多冗余，我们安装了三个总视野覆盖率达到200度的摄像头，两个传感器的融合方案如下所示：
点云空间：雷达信息经过处理后，只留下桩桶点云的中心点信息。
图像空间：将cone proposal generator生成的的中心点投影到图像空间后，给出较大的bounding box（尺寸会随着深度增加按比例缩小）。Calibration做好的话，可以大概率圈住图像空间桩桶的所有像素，然后通过计算bounding box中像素颜色信息的center of gravity，对bounding box中心位置进行矫正(利用场地中除了桩桶没有鲜艳颜色物体的特点)。然后将bouding box 中的图像裁剪出来，输入一个轻量化的、能够识别桩桶的神经网络模型，从而做到对桩桶的分类。这个简洁的方法可以延展感知模块对桩桶种类信息分辨的探测距离。而且理论上图像可以检测更多种类的桩桶（如图所示），尽管在我们比赛中能够检测两种颜色的桩桶就已经足够了。
组图：图像处理和识别桩桶
06
多传感器校准方案
我们的多传感器融合架构需要雷达，摄像头，IMU做很好的时序和空间对齐，我们根据Pandar40P的特点采取了对应的方法：
1. 摄像机和IMU空间校准
Pandar40P雷达对棋盘格的识别能力出奇的好（如下图所示），几乎每一行的黑白色块都可以表达出来。
Hesai Pandar40P点云可清楚地识别棋盘格
我们尝试了几种开源的使用Checkerboard的方法进行标定，都取得了不错的标定效果。这里可以推荐下Autoware，Butler，Baidu Apollo的开源方法，网上都可以找到资料。
今年二月在和欧洲老牌强队苏黎世理工AMZ（去年FSG第一）参加在汉堡举行的ARWO交流会的时候，他们的老队员和我们推荐了这篇paper（https://arxiv.org/abs/1705.09785）。其中介绍的标定方法因为使用了一种带类似二维码图案的立方体，所以对雷达和相机的识别都很友好，他们已经复现了这种方法，并取得了不错的标定效果。有意思的是，他们在接下来的比赛中也将使用Hesai的雷达。
相机和IMU的标定我们使用了Kalibr（https://github.com/ethz-asl/kalibr）。
LiDAR和IMU的标定可以推荐大家使用Apollo开源方案。标定IMU是因为点云运动畸变补偿融合了IMU的信息，我们需要知道LiDAR坐标到IMU坐标的TF信息。
2. 基于PTP网络进行雷达和相机的时间校准
要想达到激光雷达和相机有很好的融合质量，良好的时序对齐在这里很重要，而并不是精确的绝对时间。我们这里将GPS作为一个架构补充使用，如果我们可以获得足以提升localization精度的GPS，可以考虑将它作为时钟来源。
在这个架构中Car PC时钟作为PTP master使用，通过工业级的Internet Switch，Arduino Yun, Pandar40P和Car PC构成PTP网络。
其中，Arduino Yun通过自己的Linux系统向自带的MCU发送PPS指令从而让MCU对三个摄像头发射trigger信号。Arduino Yun在发送PPS信号的同时也向网络中发布一个带有时间戳信息的topic。
我们默认PTP网络中的节点时间误差精度达到微秒级，所以最后通过获取Arduino Yun端Linux到MCU 的delay，我们可以较为准确地把握网络中各个节点的时间戳.
这个架构允许我们不需要过多设置，直接通过网线可以接入任何支持PTP协议的设备。
当然PTP想要达到比较好的效果还是需要每个节点硬件拥有支持“硬件PTP”的NIC（Network Interface Controller）。
摄像头和雷达传感器网络结构示意
07
关于车队
卡尔斯鲁厄理工（KIT）ka-raceing车队，欢迎大佬前来赞助！
本文作者：冯喆
文章贡献者为卡尔斯鲁理工Formular Student车队
车队其他成员：Simon Schärfer, Nico Denner, Nick le Large, Luca Wahl, Josua Janus, Sherif Nekkah, David Borberly, Benjamin Schmidt, Lukas Harsh, Haocen Yu, Carlos Rosero, Stefan Hirsch, Yuchen Liu, Mario Boxheimer, Sebastian Herz, Felix Bening, Luca Bauer, Felix Rodach, Emir Smajic, Beatrice Morhard, Frederic Witkind Hirth, Hendrik Knispel, Jan Beyer, Johannes Mohr, Leif Kahler, Max Ehrfeld, Niklas Friedrich, Paul Roschka, Robert Timmermann, Simon Noll, Yannick Reuter, Yifan Cai, Meng Zhang, Anna Glöckler
往期文章：