转向避撞工况下装载激光雷达车辆的障碍物跟踪

赵治国，王 鹏，陈晓蓉，梁凯冲

（同济大学汽车学院，上海201804）

前言

随着人工智能和5G技术的不断发展，自动驾驶将是车辆未来发展的主要趋势［1-2］。自动驾驶车辆集环境感知、决策规划和运动控制等多功能于一体［3］，其中环境感知是实现自动驾驶的关键功能，道路边界和障碍物等信息的感知为自动驾驶车辆提供可行驶区域，并帮助自动驾驶车辆躲避道路行人和车辆［4］。激光雷达凭借测距精度高、受环境影响小、实时性好等优点，在环境感知领域中受到广泛应用［5］。

自动驾驶车辆感知到前方道路的障碍物信息后，需进行转向避撞。由于车载激光雷达与车辆之间为刚性连接，其坐标系将随车辆转向而旋转，导致相邻时刻检测的同一障碍物位置相距过大，增加了障碍物信息关联、运动状态分析和后续避撞路径规划的难度。因此，实时获取车辆行驶的航向角，将有利于对障碍物的跟踪。

障碍物的检测和跟踪是车辆感知的重要环节，为提高检测和跟踪的效果，国内外做了大量研究。Maclachlan等［6］采用距离特征对动态车辆相关数据进行关联，并通过卡尔曼滤波对车辆运动状态进行估计。邹斌等［7］采用可变阈值的密度聚类方法对障碍物点云聚类，提高了障碍物检测的准确性，并通过多假设跟踪模型和卡尔曼滤波实现了对动态障碍物的跟踪。Leonard等［8］对检测的障碍物用最近邻方法对前后帧的数据关联，然后对多帧速度进行滤波，实现障碍物的跟踪。王涛等［9］利用加权多特征数据关联算法集合卡尔曼滤波器实现了对动态障碍物的跟踪。上述方法未考虑车辆转向对障碍物跟踪的影响，当雷达坐标系随车辆转向而旋转时，前后帧同一障碍物的坐标发生大幅变化，易造成漏跟和误跟。

为实时获取车辆行驶的航向角，从而对雷达坐标系变换，需要精确提取道路边界点云，在这方面已有不少研究。刘梓等［10］采用了基于线性鉴别分析分类思想的算法，有效检测出道路边界，该算法兼具可靠性和实时性，但没有考虑到有障碍物的情况，抗干扰能力不足。朱学葵等［11］采用随机抽样一致性算法对道路边界点云进行直线拟合，但该算法在有多条边界线时存在阈值设定困难、迭代次数较多的缺点。Sun等［12］为准确提取不规则或被障碍物部分遮挡的道路边界，利用车辆的预测轨迹搜索道路边界候选点，有效提取出道路边界，但该方法在车辆转向避撞工况下并不适用。Li等［13］采用主、次窗口方法搜索道路边界点，提高了边界拟合精度，但搜索时沿着车辆行驶方向，在车辆转向避撞时容易失效。

综上所述，现有的道路边界提取和障碍物跟踪方法较少考虑到车辆转向避撞工况，针对这一问题，本文中基于激光雷达坐标系变换，提出了一种车辆转向避撞工况下道路边界提取和目标障碍物跟踪的方法。首先对原始点云进行地面点云分割和道路边缘检测等预处理，并采用改进的K-means聚类方法提取道路边界；而后根据道路边界利用麻雀搜索算法获取车辆航向角，并对雷达坐标系进行变换；最后利用关联算法和粒子滤波器，实现转向避撞工况下目标障碍物的跟踪。

1 道路边界提取

道路边界提取不仅是获取车辆航向角的基础，且可以缩小目标障碍物的搜索范围，提高障碍物检测和跟踪的准确率［14-15］。

1.1 点云预处理

本文中使用的三维激光雷达为速腾聚创的RSLiDAR-32，该雷达水平角分辨率为0.2°，竖直方向角度范围是-25°~+15°，共有32线激光束，工作频率为10 Hz。每帧数据约有5万个点，为提高处理效率，需对原始点云进行预处理。

点云的预处理主要包括地面点云分割和道路边界检测。地面点云约占总数量的30%，其中包含了道路边界点。因此在地面点云中检测道路边界，既可提高检测速度，又可避免诸如车辆、行人、树木等障碍物的干扰。有研究采用平面拟合法，较好分割出地面点云［16］，并在地面点云中，根据边界点的高度差值和距离差值，实现道路边界的检测［17］。

点云预处理结果如图1所示。图中红色轴为X轴，绿色轴为Y轴，蓝色轴为Z轴。红色标记点为检测的道路边界候选点。可以看出，上述方法能够从原始点云中分割出地面点云，并能有效检测出道路的边界点。

1.2 边界候选点聚类

根据几何结构特征提取的直行道路边界点在理想情况下应近似分布于一条直线，然而由于实际路面存在凸起和凹陷、部分激光扫描线被车辆和行人遮挡等问题，提取的边界候选点存在干扰和不连续部分，如图1（c）所示。为解决上述问题，本文中提出了一种改进的K⁃means聚类方法，实现道路边界点的聚类。

K⁃means聚类具有算法简单、收敛速度快等优点［18］。它有两个重要参数：聚类数目K，聚类中心cj。K⁃means算法将所有数据点分配至相似度最高的聚类中心，计算各类数据的均值作为新的聚类中心，不断迭代直至聚类中心不变，从而实现数据点的聚类，其目标是使准则F最小：

式中：nj表示类j中数据点的总数；xi表示类j中的数据点。

K⁃means通常基于欧氏距离作相似度指标，即两点之间距离越近，其相似度越高，其公式为

式中：（xo，yo）为聚类中心坐标；（xi，yi）为数据点坐标。

采用式（2）对边界候选点进行聚类分析，仅能将相距较近的边界点聚类，对分布于同一直线的边界点的聚类效果不好。为解决这一问题，本文中使用点到直线的距离dl代替do，如式（4）所示，聚类中心则用各类中数据点拟合的直线（kl，bl）代替数据点坐标的平均值（xo，yo）。由于边界点的分布与坐标系Y轴近乎平行，为避免拟合直线斜率kl过大，直线方程采用式（3）表示：

式中kl、bl分别为拟合直线的斜率和截距。

聚类中心采用最小二乘法拟合出匹配各类数据点分布的直线方程，使数据点到拟合直线的误差平方和最小。

K⁃means算法对初始聚类中心非常敏感，易陷入局部最优，导致聚类稳定性不高。针对这一问题，初始聚类中心的确定步骤如下：

（1）基于式（5）对边界候选点拟合，得到初始直线方程（k，b）；

（2）直线斜率k不变，代入边界候选点坐标（xi，yi），求出最小截距bmin和最大截距bmax；

（3）根据聚类数目等分bmin和bmax，得到初始聚类中心（ki，bi）。

由图1（c）可知，上节中提取的边界候选点中大部分点为道路边界点，存在少部分的路面和路沿上的干扰点，根据这一特点，对式（4）的距离公式引入修正系数，避免因初始聚类中心（ki，bi）选取不当，导致聚类陷入局部最优，而未能正确提取道路边界点。修正公式为

式中：ni为聚类簇内的点数量；N为边界候选点总数量。

聚类数目K初始设定为3，选择聚类后包含点数量最多的类作为道路边界，类中心（kl，bl）则为拟合的道路边界线。当道路边界提取效果较差时，将增加聚类数目K重新聚类。由于雷达采集频率为10 Hz，在0.1 s的时间间隔内车辆航向角变化较小，道路边界线的斜率k变化不会过大。因此选取边界线相比前一帧的斜率变化Δk和边界点到拟合边界线的平均距离，即聚类的紧密性（compactness）CP指标作为评价标准。当拟合边界线的Δk和CP值，任一超过设定的阈值，将增加聚类数目K对边界候选点重新聚类。左右道路边界的提取流程如图2所示。

图2 边界提取流程图

道路边界候选点的聚类效果如图3所示。其中图3（a）为未修正的结果，图中红色点是对左、右边界候选点分别聚类提取的结果，由于初始聚类中心的选取不当，部分边界点（黑色点）被划分至其他类中，造成左边界提取错误；图3（b）为修正后的结果，黑色点为聚类后的左边界点，避免了局部最优，较好地将道路边界点聚类为一类。

图3 边界点聚类结果

2 车辆航向角获取

道路边界点在XY平面内的投影与Y轴之间的夹角，即为当前车辆的航向角。为提高航向角求解的精确性和稳定性，采用麻雀搜索算法（sparrow search algorithm，SSA）进行求解。SSA是一种新型群体智能优化算法，种群通过不断更新当前搜索位置，以达到全局最优解，相比于粒子群等其他智能优化算法具有搜索精度高、收敛速度快、稳定性好、鲁棒性强等特点［19］。

2.1 麻雀搜索算法原理

相比于粒子群算法，SSA通过将麻雀种群分为发现者、加入者和侦察者，并根据个体的不同身份制定不同的位置更新规则，提高了收敛速度［19］。

假设搜索过程中共有n只麻雀，第i只麻雀的位置如下：

式中d为搜索维度。

发现者位置更新方式如下：

式中：itermax为最大迭代次数；t为当前迭代次数；α为（0，1］间的随机数；Q为服从标准正态分布的随机数；L为大小1×d、元素均为1的矩阵；R2∈［0，1］和ST∈［0.5，1］分别表示预警值和安全值。R2＜ST时，种群安全，发现者可广泛搜索；否则种群发现捕食者，迅速向安全区移动。

加入者位置更新方式如下：

侦察者位置更新方式如下：

2.2 建立优化指标

采用SSA求解车辆航向角θ，因此种群搜索维度为1，由于激光雷达坐标系经过前一帧求得的航向角变换，Y轴近乎平行于道路延伸方向，所以初始化种群位置为0。随机选取左右边界各5个数据点，建立种群适应值函数为

式中：xi为道路边界点的横坐标；xˉ为道路边界点横坐标的平均值。

SSA通过个体位置所代表的含义，即车辆航向角，对提取的道路边界点进行坐标变换，根据式（12）求出该个体的适应值，以适应值最小作为优化目标，求解出当前车辆航向角的最优解，具体流程如下：

（1）初始化种群位置，即车辆航向角θ=0°；

（2）基于个体位置所代表的航向角对道路边界点坐标变换；

（3）根据式（9）~式（11）更新种群中发现者、加入者和侦察者的位置信息；

（4）根据式（12），更新种群适应值f；

（5）迭代次数达至最大或适应值f小于阈值，则输出航向角θ，否则转到步骤（2）。

根据SSA求解的车辆航向角θ，对地面点云坐标变换后的结果如图4所示。可以看出，激光雷达坐标系经过变换后，其Y轴与道路延伸方向基本平行，消除了车辆转向对障碍物跟踪的影响，为下节的障碍物跟踪奠定基础。

图4 地面点坐标变换

3 障碍物跟踪

行驶车辆关注的目标障碍物多为道路区域的车辆和行人，车与车、车与行人之间的距离一般较大，之前的研究中已通过DBSCAN聚类，较好地实现了障碍物的检测［20］。

为实现障碍物的跟踪，需将前后帧检测的障碍物信息关联，常用的方法是联合概率关联算法（joint probabilistic data association，JPDA）。JPDA算法选取总概率最大的事件匹配，其关键在于计算当前时刻障碍物（观测点）与前一时刻障碍物的预测位置（目标点）的联合概率矩阵，即

式中：i为观测点；j为目标点；ωij为观测点与目标点的关联概率。

关联概率的计算通常采用欧式距离，即观测点与目标点距离越近，则关联概率越大。然而欧氏距离的使用忽略了车辆在道路区域的行驶特性，在道路区域内车辆几乎为纵向行驶，很少出现横向行驶，且横向车速也相对较低。由于本文已基于车辆航向角对激光雷达坐标系变换，避免了前后帧检测的障碍物位置因坐标系旋转而大幅变化，因此可通过分别计算观测点与目标点的横、纵向距离，并增加横向距离的权重，来提高关联成功的概率，即

式中：ki为各项偏差的加权系数；Px和Py分别为观测点与目标点间的横向与纵向距离。

本文取k1=0.75，k2=0.25，增加横向距离的权重，当车辆和行人等障碍物在前后帧的横向距离越近，越有可能为同一障碍物。

由于实际中存在测量噪声和丢帧的现象，直接对前后帧检测的障碍物进行关联，容易发生关联错误。因此，采用粒子滤波器对目标进行估计和更新，提高下一时刻预测位置的准确性。

粒子滤波算法的基本思想是在先验概率分布中随机采样获得一组粒子，在跟踪过程中根据获得的量测值实时调整粒子的权重和位置，并利用粒子的状态信息近似表示系统的后验概率分布，由此估计非线性系统的状态。粒子滤波算法如表1所示。

表1 粒子滤波器算法

根据坐标变换后的各目标相对本车的距离和角度信息，利用上述粒子滤波算法可获得各目标的纵向位置、纵向车速、横向位置和横向车速等运动状态。由于已对激光雷达坐标系变换，对目标障碍物下一时刻的位置预测将更加准确。

4 实车试验

为验证本文提出的车辆转向避撞工况下道路边界提取和目标障碍物的检测跟踪算法，开展了实车道路试验。试验车辆如图5所示，车顶部中间安装32线激光雷达以及GPS/IMU，其中GPS/IMU提供试验车的航向角，采集频率为100 Hz，以验证本文基于SSA获取航向角的准确性。如图6所示，试验场景选取了一条单向车道的校园道路，其中道路左边界为灌木丛，右边界为路沿。

图5 改装试验车

图6 试验道路场景

试验车在校园道路中先进行向左的转向行驶，而后减速停车，之后再加速向右转向，重新回到道路中心，模拟车辆的转向避撞场景。在试验的校园道路上共有4位试验人员，随机走动，模拟周围的障碍物。试验采集的激光雷达数据共有690帧，GPS/IMU数据共有7 000帧。

图7 为激光雷达数据处理结果。由图7（c）可知，基于改进的K⁃means方法有效去除了路沿上的干扰点；由图7（d）可以看出，坐标系经过变换后，其Y轴与道路边界平行。

图7 点云处理结果

图8 为改进的K⁃means聚类算法和SSA的求解结果。由图8（a）可知，由SSA求解的车辆航向角与GPS/IMU测得的基本一致，在前15 s车辆航向角为负，进行向左的转向行驶，在15~35 s车辆减速停车，航向角不变，在35 s后车辆航向角向正方向修正，回到道路中心。由图8（b）和图8（c）可知，由于道路左边界为灌木丛，右边界为路沿，左边界聚类后的边界斜率以及CP值相比于右边界明显更加稳定。本文设定的斜率变化阈值为0.02，CP阈值为0.5。结合图8（d）可以看出，增加聚类数目后能够确保所提取的左右边界线斜率绝对值在0.02以内，CP值在0.3以内，有效提高了边界提取的稳定性。由图8（d）可知，左边界提取更加稳定，聚类数目始终为3，右边界偶尔需要增加聚类数目，最多为6。

图8 改进的K⁃means和SSA结果

图9 为改进的K⁃means聚类算法和文献［11］中使用的Ransac算法结果对比。由图可知，在干扰点数量较多时，改进的K⁃means算法相比于Ransac算法求解的相邻帧边界线斜率的变化更小，稳定性更好。

图9 K-means与Ransac的对比结果

图10 为PSO和SSA不同时刻的对比结果。本文设定的适应度阈值为0.000 3，由图可知，SSA收敛速度更快，仅需迭代约200次，而PSO需迭代约900次，因此SSA的实时性和稳定性更好。

图10 SSA与PSO的对比结果

图11 为障碍物检测和跟踪算法的结果。目标1-4是试验人员，目标5是中途经过的骑自行车人员。由图11（a）和图11（b）可知，粒子滤波器计算的目标物相对速度更加平滑，有效去除了噪声，使预测点的位置更准确，具有良好的跟踪性能。图11（c）表明，提出的关联算法能够保证不同目标物的正确关联。

图11 粒子滤波器跟踪结果

图12 为激光雷达检测的目标物信息。在第440帧时出现目标5。在第484帧时由于目标1的遮挡，丢失目标5。在第487帧时目标5重新出现。在目标1和5距离相近且目标5连续丢失两帧时，提出的跟踪算法仍能实现目标1和5的正确跟踪。

图12 目标关联结果

在实时性上，以上激光雷达的点云处理程序在Intel i7处理器的计算机上运行。运行时间如图13所示，每帧从点云数据输入到各障碍物信息输出，所提算法的计算时间一般为50~75 ms，平均运行时间约为61 ms，最大耗时86 ms，小于激光雷达采样周期100 ms，能够满足实时要求。

图13 运行时间

5 结论

针对车辆转向避撞工况，提出了一种基于激光雷达的目标障碍物跟踪方法。采用改进的K⁃means聚类方法提高了道路边界拟合的准确性，并通过增加聚类数目保证了边界线的拟合效果，使相邻帧拟合的边界线斜率变化小于0.02，边界点至拟合边界线的平均距离小于0.3，相比于Ransac算法更加稳定；利用SSA获取的车辆航向角对激光雷达坐标系进行变换，避免了车辆转向对道路边界提取、障碍物关联和运动状态判断的干扰；再通过JPDA关联算法和粒子滤波器实现了有丢帧情况的障碍物跟踪。实车试验验证了提出的算法在转向避撞工况下具有良好的道路边界提取效果和目标障碍物的跟踪效果。

文中研究仅对结构化的直道进行了讨论，后续将进一步研究算法在非结构化道路和弯道的检测效果，提高算法的适用性。