车载激光雷达的应用算法

杜海林，张杭铖，王齐超

（长安大学 汽车学院，陕西 西安 710064）

在国家交通强国战略和新基建政策推动下，智能网联汽车产业已进入加速发展的新阶段。在未来万物互联的体系中，只有实现“车-路-云”三方数据互联互通，形成车路感知数据融合与决策控制才有可能实现最高级别的全场景自动驾驶。而激光雷达作为目前技术成熟度和检测效果较为均衡的传感器之一，是智能网联汽车实现车路协同感知不可或缺的传感器。

激光雷达系统是以收发激光束侦察目标物体的位移、速率等特征量的雷达系统[1]。

目前，车载激光雷达上的应用算法主要包括目标跟踪和识别、即时定位与地图构建（Simultaneous Localization And Mapping, SLAM）和点云分割[2]。

1 目标跟踪与识别算法

激光雷达的目标跟踪是点云数据处理的最关键的一部分。目标跟踪的主要目的是获取障碍物的距离、速度以及运动轨迹等信息。通过目标跟踪算法对障碍物运动状态做出估计和预测，我们便可以实时地评估障碍物和无人驾驶车辆的安全等级，作出相应的决策。因此，目标跟踪算法的优劣直接影响到无人驾驶车辆的安全等级。

目标跟踪算法需要处理的信息有很多，除了对目标位置，速度的估计之外，有时候目标的运动状态也需要考虑。所以为了评估激光雷达目标跟踪算法的性能，算法的速度、准确度以及跟踪稳定性是重要的指标。

目标跟踪可以分为以下几种任务：单目标追踪、多目标追踪、姿态追踪、在线追踪、离线追踪。虽然目标跟踪的前景非常广阔，但是也存在一些问题限制了其发展。形态变化，追踪过程中目标的姿态发生变化时，会导致其特征以及外观模型变化，容易使得目标追踪失败[3]。尺度变化，当目标尺度缩小时，容易包含多的背景色；而当目标尺度增大时，又容易导致跟踪框不能包括整个物体。激光雷达点云随距离增加变得稀疏，环境感知算法对障碍物检测性能降低，远处目标不能有效检测[4]。遮挡与消失，在跟踪目标时，如果遇到障碍物或者目标突然消失，容易造成跟踪失败。图像模糊，目标的快速移动，外界环境如：光照、雨雾等因素会造成分辨率降低，从而使得跟踪失败。

随着无人驾驶技术的日益发展，目标跟踪算法理论也日益成熟，在目标跟踪算法中主要的有加权最小二乘滤波法、CSK滤波跟踪法、DCS滤波法以及卡尔曼滤波法。其中使用最为广泛的就是卡尔曼滤波法。然而在车辆行驶过程中由于激光雷达的特性，在每一帧中属于同一个障碍物的点云数量会产生差异，这就导致了聚类出的同一障碍物的大小发生变化，使得卡尔曼滤波对目标进行跟踪预测时会产生较大的误差。而且误差可能会随着时间的推进逐步增大，导致目标跟踪丢失[5]。李帅印等[6]结合了卡尔曼滤波算法和改进匈牙利算法，提出了一种基于多层激光雷达的目标检测与跟踪算法，以适应无人驾驶中目标增多或减少的情况。王海等[7]提出了一种基于激光雷达的3D实时车辆跟踪方法，使用全局最近邻（Global Nearest Neighbor, GNN）算法作为关联算法，并通过双波门设计，提升了算法精度与速度。ALIREZA A等[8]在激光雷达数据的背景下，提供了一个基于体素和平面的三维感知体系，可以进行在大城市环境中的地面模型和障碍物测量。由于对周围环境感知的需求提升，可以看出多目标跟踪算法将会是未来的主流方向之一，同时应用GNN也能帮助我们解决多目标跟踪中存在的一些问题。也可以考虑摄像头与激光雷达融合的跟踪算法。虽然多算法与多传感器的耦合能提高整个系统的精度，但同时伴随着的也有整个系统复杂性和成本的提高，这也是以后研究需要考虑的问题。

2 即时定位与地图构建

SLAM 即机器人在移动的过程中根据自身的位置进行自我定位，从而实现机器人的自我导航和定位[9]。建图这一步骤也可利用激光雷达扫描周围环境完成，目前激光雷达主要分为单轨制与多线这两个主要方面，根据这两大类激光雷达（Light Detection and Ranging, LIDAR）提供了不同的计算方式。首先，单轨制激光雷达主要使用在平面体育运动场景中，而多线激光雷达则使用在三维空间的体育运动场景。当面对复杂多变的体育运动自然环境时，多传感器融合的SLAM则成为了当前发展趋势。视野特点会带来精确的体育里程数计和数据信息量巨大的地图信息内容，而激光雷达则为视野特点带来更精确的体育深度信号。因此，SLAM 计算的鲁棒特征和实时化能力有待进一步提高。在提升SLAM算法鲁棒性技术领域方面，还必须充分考虑里程计的标准、激光雷达的外参与时戳标准、激光雷达运动畸变的消除等数据处理步骤，但同样也受制于退化环境、全局位置、动态环境位置等技术问题。

2.1 Gmapping

Gmapping是一种通过粒子滤波的2D激光雷达 SLAM，可以用来形成二维栅格图，它融合里程计数据，但没有回环检测。它的优点是在小环境中，运算容量小，速度也比较快。但不足之处是每种粒子都包含着一张版图，根本无法处理大环境（存储和运算量很大）；一旦运营里程错误或者标定参数错误，在较长回廊的环境中会将版图重建歪。使用粒子滤波架构的激光 SLAM，融合了里程计和激光数据信息，每种粒子都可以装载一张版图[10]，建立的长场景版图所需要的运算时间相对较少，而且准确性也较好。正因为这是一种基于粒子滤波的新技术，所以存在的问题还有粒子耗尽的问题。GRISETTI G等[11]在论文中提供了一个对有选择性进行重采样操作的办法，这有效地克服了粒子耗尽的问题。

2.2 Hector_slam

通过优化算法实现帧内匹配的激光SLAM算法，由于不要求里程计数据，所以代码量较小。Hector slam是完全基于扫描匹配的，扫描匹配就是使用当前帧与已有地图数据构建误差函数，并用高斯牛顿法得到最优解和偏差量。Hector slam可以通过迭代优化的算法求得匹配的最佳距离，为了防止陷入局部极值错误，还可以使用更高解析度的地图匹配。但因为完全依赖于扫描匹配，所以需要雷达设备的位置准确度很高、雷达测速效率高、视野范围宽，或者扫描效率很高（或移动速度慢）。在噪音大、特征点少的情况下就比较容易失效。文中[12]提出的方法加入了惯性测量单元（Inertial Measurement Unit, IMU），使用扩展卡尔曼滤波（Extended Kalman Filter, EKF）估计整体的六自由度（Degree Of Freedom, DOF）位姿，并通过横摆角和俯仰角将激光扫描图像数据投影到XY平面位置，因而支持激光雷达数据有一些程度的偏斜，例如：手持或机器人运动在并非很水平的地板上。KOHLBRECHER S 等[12]则通过使用对地图梯度的快速近似和高分辨率网格，从而达到了在各种挑战性场合中的准确位置和地图测绘水平。

2.3 Karto

图优化 SLAM，包括回环，一般使用对矩阵的稀疏化加以解决，在大规模条件下建图时具有优越性。KARTO是使用了scan-matching，回环检测和图优化的SLAM方法，并通过稀疏姿态调整（Sparse Pose Adjustment, SPA）实现优化。KONOLIGE K等[10]提供了一个建立并解决线性子问题的最有效方式，即SPA优化法。该方法与其他直接方法相比较，有更优越的收敛速度和精度，这一方法在真实地图和大规模数据集上展示了其有效性。

2.4 Cartographer

Cartographer是谷歌开源的激光 SLAM 技术框架，其优点是首先引入了 submap，即 scan to submap matching，将自动更新到的每帧数据都和最近的submap对应，并放在最佳地址上。一旦不再有自动更新的当前scan或者自动更新到最近的当前 submap时，就封存了这个 submap，然后再去建立最新的submap。第二点是加入了回环测试与优化。使用最近 submap和当前 scan开始回环测试，一旦当前 scan和最近开始建立的 submap在长度上已有一定接近，则开始回环测试。在监测到的回环过后使用 ceres加以调节，并改变submap间的相对位置。为完成回环分析，通常使用分支定界法。WOLFGANG H等[13]提出并实验验证了一个二维SLAM系统，该系统将子图像扫描匹配、闭合回路监测与图形优化相结合。使用基于网格的局部SLAM方法创建单独的子地图轨迹。

2.5 LOAM

要在一个算法当中实现同时定位和建图，这个难度是比较大的。LOAM 是一种面向多线激光雷达的改进SLAM技术。由于激光雷达获得的点是在不同时刻，再加上雷达一直处在运动的状态中，这会使得点云图发生失真。因此，在 LOAM上将同时定位与建图可以区分为两种算法，第一种算法追求更高频段但保真度相对较低，因此，可用来预测激光雷达的速度。而另一个算法则减少频率，从而增加了精度，用于点云的精细配准[14]。LOAM的提出在一定程度上解决了点云的运动估计和建图，当然LOAM也有需要改进的地方，比如没有开发闭合回路修复运动估计中的漂移量，这一问题在后面的LeGO-LOAM得到了解决。

2.6 LeGO-LOAM

LeGO-LOAM相比如LOAM，其主要的提升在于轻量化和地面优化[15]。LeGO-LOAM 主要包括四个方面，分别是地面分割、特征点提取、雷达里程计和雷达建图。地面分割主要是将非地面点和地面点进行区分，处理大量的散乱点，使得整个点云图变得干净。特征点提取中使用了和LOAM中基本相同的方法，区分边缘点和平面点。雷达里程计模块，对六自由度模型进行了两次列文伯格-马夸特法（Levenberg Marquardt, LM）算法优化求解。在雷达建图中，要得到相应的全局地图特征点云，将使用基于传感器视域和基于图像优化的方式。在地图优化的方法中，通常采用回环检查的方式，并加入了 gtsam优化，以得出最后的全局结果地图[15]。

2.7 LOAM_livox

这是大疆在2019年发表的基于小FOV Lidar的LOAM算法。该算法继承了标准LOAM算法的基本结构和技术，但有几个关键的新贡献[16]：1）特征点筛选方法的主要进步。省略了视角边界上有明显畸变的地方；去除反射强度偏大或过小的点；去除辐射角度与所在平面偏差或过小的点；2）迭代姿势优化；3）算法可以并行运行。该算法虽然在建图和定位方面实现了高水平的精度，但顺序扫描匹配本质上是漂移的。因此，未来的相关优化可能会集中在环路闭合和滑动窗口。LIN J R等[17]提出了一种环路闭合方法用于校正激光雷达里程计和建图中的长期漂移。提出的方法计算了关键帧的二维直方图，并利用对二维直方图的归一化互相关作为当前关键帧与贴纸中关键帧间的相似性度。经过测试，这一环路闭合可以显著降低长期漂移误差。

2.8 Lio-mapping

使用IMU预积分和LOAM融合进行雷达里程测量和建图的方法[18]。它包括里程计的状态优化和带旋转约束的细化。此种方法不同于仅使用激光雷达，也优于松耦合方法。紧耦合激光雷达IMU融合方法显示了具有快速更新率的鲁棒姿态估计结果。

3 点云分割算法

点云分割法是指通过空间，几何和纹理等特性对点云进行合理分割，从而使一个合理划分内的点云都具有相同的特性，点云的合理划分通常是多种应用的前提[19]，例如：物体识别和分类，地图构建等。而地面分割往往是一些三维点云处理算法的预先步骤，地面点的数量往往是比较多的，因此在自动驾驶车辆中，正确的区分地面点和非地面点就显得尤为重要[19]。地面分割的算法主要分为六类[20]。

3.1 基于高程的方法

在点云的统计中，道路点云相对于其他地方的点云存在着几个明显的特点：1）道路点属于低地方点，且高度远小于附近的任何地物，如建筑、植物等。使用该特性提供的点云滤波，可区别出路面点和其余非地面点；2）道路较周边的任何地物更平缓，并统计各点的方法向量，通过设置适当的坡度阈值在地面点中抽取出候选道路点；3）在几何形状上道路呈长条带状分布且彼此连通。在这种方式中，三维点云通常被投射在二维网格上，以减小网格中各个单元所包含的级别变量特征值，而其中特征值通常是根据网格内的测量数值统计而得到的，如：最大平均宽度、最大平均高度和宽度方差，以及最大平均相对高度和高度方差[21-22]。所以，分割通常是利用比较特征值和设定的阈值来实现的。基于高程的分割方法存在的一个不足之处在于，它不能很好地区分桥梁和带有坡度的地面，容易将这两种识别为非地面点。基于高程的方法其步骤如图1所示。

图1 基于高程的分割算法

3.2 基于地面建模的方法

三维点云的处理在许多时候都是作为一种整体或沿纵向的部分加以处理，但在文中[22]提供了一个对地面建模的分割方式。这一方式并不适用前述的处理方式，而只是通过将点云按角度分割，然后再独立地将二维线拟合在这些部分。而区分地面点与非地面点，主要是通过比较段内从各节点到线路的记录。当然这一方法也难免会遇到地面分割的最大难题，那就是带坡度的地面。论文中[22]给出的方案，使用了高斯过程（Gaussian Process, GP）等概率方式来得到更有效的拟合，从而处理带有坡度的地面。不过，正因为以这种方式单独处理每个部分，所以并无法保持分割结果沿角度走向的一致连续性。王肖等[23]针对于智能车辆环境感知实时性要求，研究一种基于3D全景式激光雷达的点云快速分割方法。采用带地面状态变化判断的分块直线拟合算法来去除背景地面数据，生成前景点云图像并利用机器视觉算法进行聚类分割。

3.3 基于马尔科夫的方法

道路现场划分，在高速公路环境条件认识中是个十分关键的工作环节。关于路面场景分割方式对有所不同的场合下或许会形成误划分的提问，文章[24]使用基于粒子群优化算法（Particle Swarm Optimization, PSO）的改良马尔科夫随机场方式对路面情景实现了切割，先利用 Gabor对路面情景做出预处理，接着再利用改良马尔科夫随机场方式中的势函数统计图像能量，最后一步再利用PSO方式对图像处理的能力实现了优化，从而得出路面场景分割结论。

3.4 基于相邻点关系的方法

点云分割，形象地可以解释为找出与自己不相同的点。所以一种很自然的思路就是邻近搜索的分割方式。通过对比某一个点与其相邻点的属性，从而进行点云的划分。在相邻点关系的划分方式中，如何确定邻近点是一项非常重要的工作。在一些扫描的点云中，邻近点是确认的，可是对于有些杂乱无章或者经过某些滤波后的点，怎么样确认其临近点就显得尤为重要。这种确认可以通过“树”来完成，目前主流的树有 KDtree和Octree。确认了临近点之后，下一工作就是计算目标点与临近点的关系，这种关系可以使用欧几里得算法和区域生长算法。欧几里得计算通常采用了临近一个点之间的间距为判别准则，而区域生长计算则使用了法线、曲率、颜色等信息，来综合地判别点云是否应该被聚成一类。基于邻近点的分类方法，其运算速度快，能更好地适用于实时处理。此类方法的不足之处在于其不能很好地处理低矮点，会有很大的分割误差。另外本方法的阈值需要人为进行反复调试，故一般用于点云的粗分割。

3.5 基于深度学习的方法

CHARLES等提出了一个新的直接消耗节点云的神经网络，这一新技术较好地尊重了输入节点的置换不变性。网名被命名为PointNet，并为从大数据分类、零件分离和场景语义计算中的应用创造了统一的体系结构。PointNet方法非常有效，并且达到了令人印象深刻的结果，不过采用深度训练的方式还是有着使用了大量的标记方法进行训练的缺陷。而且，测试这种方法也相当麻烦。

3.6 基于平面拟合的方法

平面拟合法是指使用原始的几何形体模式（平面、圆柱体、圆柱体）作为先验知识进行空间划分，把带有相似表达式的点云数据都归于同一个区域中，这也被称之为基于模型的空间分割计算。随机抽样一致性方法（RANSAC）[25]，它能够在一个具有“局外点”特征的观测数据中，通过采用迭代方法预测数学模型的参数。假设大地是平直的，因此，可以用平面来描述大地。平面模型的基本数学公式是ax+by+cz+d=0[26]，可以通过不停地变化四个函数，找到在这个模型中一定阶段内可以拟合更多的点的函数。在找到了这组函数之后，这个可以被模型拟合的点就是在平面的最后一个点。平面被拟合后，接下来就是通过激光雷达的距离来分出地面点与非地面点。在自主汽车所遇到的实际情景中，由于地球表面并不永远都是平坦的，所以点云并非直接地将一个平面拟合到三维点云，而是首先沿汽车行进方向划定范围。接着从距离汽车最近的部门出发，在各个部门中，通过四分位距（Inter Quartile Range,IQR）的方法筛选出大部分离群点，之后再用剩余点估计带有RANSAC的平面。当通过检测高程与方向差来确定连续平面的稳定性以后，可以利用把截面中各个节点与结果平面间的长度与给定阈值加以对比来实现分割[22]。平面拟合的分割方式简单，以几何方法为基本，计算简单处理快，对噪声和奇异点都不敏感性。限制是指无法处理大量或复杂情景下的数据。

4 结论

高效的应用算法，对汽车激光雷达充分实现探测性能至关重要，所以汽车业界中更多的人都相信智能驾驶车辆，最终将变成一种“软件产品”。

为了有效准确了解周边环境，追踪和辨识障碍物、实现智能驾驶车辆定位系统和行车路线规划，保障其安全、有效地运行，车载激光雷达的信息处理将符合实时、稳定、精确的特点。

目前车内激光雷达应用算法都存在不同程度的局限：第一，算法安全性与实时性互相抑制，两者无法共同实现；其次，算法多是面向某一特殊场合而开发，因此，无法提高可移植性和稳定性。

场景的复杂性与多样化导致算法的发展异彩纷呈，并出现了全方位、多角度的多元融合趋势。

各种计算方式的合理选择应用将满足各种情景下的智慧行车问题，同时点云分割算法是实现目标追踪与识别的重要基石，通过目标追踪与识别将完成对车辆及周边障碍物运动状况与几何特性的综合评估，而SLAM则将完成对车辆的精确定位和可行驶路线规划。