基于LabVIEW 和单线激光雷达的障碍物识别算法研究

袁 春，曾 凡，李 昊，胡 萌，徐 哲

（1.重庆理工大学 汽车零部件先进制造技术教育部重点实验室，重庆 400054；2.重庆理工大学 机械检测技术与装备教育部工程研究中心，重庆 400054）

激光雷达由于可以获得极高的角度、距离分辨率和良好的抗干扰能力，被大量应用于车辆智能驾驶中。多线激光雷达能够获得障碍物丰富的点云信息，但价格昂贵，给智能车辆普及带来较大的挑战。单线激光雷达结构简单，并且由于价格低廉，在自动驾驶领域开始有所涉及。对于雷达感知算法的整体架构，杨飞等［1］采用Ubuntu下的ROS机器人实时操作系统实现动态障碍物的检测和追踪。汪佩［2］采用嵌入式软硬件系统，基于OpenCv、OpenCL及QT环境进行数据处理。于金霞等［3］采用Windows下的Visual C++编写的障碍物识别算法。

LabVIEW 是近年来逐步应用的一款图形化交互式编程软件［4］，通过虚拟仪器模块，将软件和各种硬件工具连接起来。依靠LabVIEW 在数据采集、实时通信、图形信息显示方面的强大功能，以及在智能驾驶领域的逐步应用，将LabVIEW 运用于激光雷达环境感知中的障碍物检测。有关障碍物的单线激光雷达检测算法，湖南大学庄秀华依据雷达的点云数据扫描特点，采用中值滤波算法处理噪声数据，使用最近邻算法归类障碍物的点云，最近匹配法和卡尔曼滤波进行跟踪［5］。刘伟［6］选取车辆几何特征作为目标识别的依据，选用支持向量机进行分类［7］，通过交叉验证和网格搜索寻找最优参数以提高车辆的识别率。杨成提出一种近邻域和层次聚类相结合的算法进行聚类，得到各个聚类的质心［8］。通过置信区间的关联性辨识同一个障碍物的速度和航向。

本文提出一种基于LabVIEW 的单线激光雷达障碍物识别算法。一方面，依靠LabVIEW强大的数据通信和图形显示功能，识别算法具有良好的实时性和可显示性。另一方面，针对DBSCAN［9］算法聚类的缺点，提出一种融合高斯模型，即对雷达扫描范围分区域自主计算聚类算法的阈值参数的方法，提高对障碍物点云数据的检测能力和识别算法的执行效率。算法完整流程参考图1。

以北京佳光科技有限公司生产的L1NO单线离轴激光雷达作为主要传感器，水平扫描角度为100°，最小测距误差为2 cm，激光的扫描频率为5～20 Hz，测距范围为0.3～30m，采用以太网通讯。

图1 障碍物识别算法流程框图

1 障碍物检测

1.1 雷达数据解析

将雷达报文信息进行解析，在LabVIEW 底层中读取原雷达数据，雷达原始数据组成见表1。

表1 雷达UDP数据包组成

LabVIEW 解析过程见图2。基于单线激光雷达的水平视场角和测距范围，以及路面的实际情况，设定感兴趣区域为雷达前方30 m的测量范围。感兴趣区域［10］的设定，一方面需考虑雷达的扫描范围，另一方面是为了减小数据处理规模，提高算法的执行效率，去掉无关的点云数据。但感兴趣区域的设定也不能过小，以防止路面前方车辆信息的丢失，影响车辆的行驶安全。

图2 数据解析程序框图

1.2 雷达点云的影响因素

1.2.1 雷达的反射率

激光束扫描到不同的障碍物表面，反射率［11-12］差别较大，表2是经过反射实验采集的反射率数据。经过试验，L1NO雷达对于白色车辆的反射率最好，对于黑色车辆，可能会出现车辆轮廓点云失真而导致障碍物信息丢失。

表2 不同物体的反射率

1.2.2 距离因素

雷达离地高度设定为0.7 m，真实环境中，随着距离的增加，光路的衰减将极大地影响物体的反射率，从而影响测量的准确性。通过雷达实测，在0～30 m的障碍物，随着障碍物逐渐远离雷达，得到的雷达点云点数越来越少。对于30 m以外的障碍物，由于受到激光测距范围的影响，点云数量稀少，不能表征出一个障碍物的外部轮廓，因此设定探测区域为雷达正前方30 m。

1.3 障碍物的聚类

针对障碍物形状轮廓的提取，通常采用聚类算法进行处理。与其他聚类算法相比，DBSCAN算法对不同点云数量的簇以及不同形状的簇，通过对形成一个完整障碍物点云簇的最少点的数量和点间距参数的设置，快速判定该样本点所对应的外部实体的种类。该算法构成主要包括：

1）核心对象：假定存在某一个数据点为核心对象，则要求在以该点为球心、长度Eps为半径的球面范围内，存有最少Min pts个空间点。定义该点为（Eps，Min pts）的核心对象，存在以Eps邻域为半径的完整点云簇。

2）密度可达：存在一组数据点集｛Q1，Q2，…，Qn｝，对Qi＜D（0＜i＜n），Qi+1是从Qi出发关于（Eps，Min pts）直接密度可达，直接密度可达则表示对象链中的任一个元素Qi密度可达。对于不同大小的实体障碍物，雷达扫描得到的点数也不一样，并且由于雷达点云的扫描特点，随着距离的变化，同一个障碍物表面的反射面积也会变化，障碍物点云数量也随之变化。阈值参数的算法如下：

式（1）表示阈值Eps为点云数据data和最小点数Min pts的函数，式（2）是求取点云data的数量，式（3）是求取距离阈值Eps的大小。参数m表示点簇的数量，n值为2，计算过程引入伽玛（gamma）函数，k为调节系数。常见雷达扫描车辆外形轮廓的点云形状分为“I”型、“L”型和其他型，如图3所示。

参考图4的高斯分布［13-14］曲线，可以把“I”型看作一个类似的高斯分布，“L”型看作是2个高斯分布的组合，高斯模型采用式（5）表示：r式（4）中模型p表示由多个正态函数g（x|μi，σi）叠加组成；π表示模型权值的向量；σ表示正态分布的方差值矩阵；μ表示统计模型中的期望值向量；i表示数据点所代表的统计正态分布模型的个数。

图3 雷达扫描点云形状

图4 高斯分布曲线

雷达扫描障碍物的动态调节聚类［15］算法的重要参数点数阈值Min pts表达式见式（6），图5为不同距离下的扫描示意图。

图5 不同扫描距离示意图

式中：d为X方向上二维高斯分布模型点的坐标值（d1、d2、d3表示运动的障碍物不同时刻X方向不同的高斯坐标值），d取值为0＜d≤30，s为Y方向上二维高斯分布点的坐标值（即高斯分布的期望值），s取值为0＜s≤30，θ为相邻扫描线的夹角，δ为激光雷达扫描宽×长为2 m×4 m车辆外部轮廓的点数经验值，取值35，γ为调节因子。

2 障碍物跟踪

障碍物的跟踪首先需要建立不同时刻障碍物信息的数据关联，以找到同一个障碍物在不同时间域的点云数据。数据关联主要依靠前后帧的障碍物点云信息，即障碍物的外部轮廓长宽信息、速度信息、位置信息，通过时间上的连续性，判定是否属于同一个物体。对于激光雷达传感器来说，由于雷达扫描受到外部光线干扰、对深色物体扫描失真、路边花丛树叶反射等观测噪声，以及由于车身抖动造成雷达自身振动带来的过程噪声，单帧的检测结果无法判别出运动对象的准确信息，必须依靠连续多帧的点云数据。同一个运动对象，只有被多次检测并且能与路面中真实的车辆和行人匹配［16］，才能确定其运动状态。

本文中的数据关联算法，采用最近邻算法的思想，依据距离最近的K个邻居所归属的类来判别新数据集的类别属性。在目标扫描跟踪范围内，若检测到只有一个观测点云数据点，则使用该观测数据对目标进行更新。但是如果在扫描跟踪范围内存在多个观测数据，计算观测点与目标预测点的距离，并比较距离的大小，通过距离最小的观测点来更新目标状态。根据这种近邻算法思想，对于可能属于同一个障碍物的点云，聚类成一个块后，经过一个扫描周期，相邻2个时刻的障碍物外部矩形框轮廓，必然存在重叠区域，为了简化这种描述，用矩形框的极坐标的夹角的重叠区域来表示，如图6所示。

图6 前后时刻障碍物的运动关联示意图

2.1 运动参数

如图7所示，障碍物沿着直线l运动，由于雷达扫描∠KOH水平视场角为100°，在进入雷达边界直线OK之前，雷达无法扫描。A点为雷达开始扫描点，受到雷达扫描距离30 m的限制（设定OB=30 m），因此AB段是有效扫描区域。随着距离越来越远，障碍物信息开始丢失，能部分显示车辆轮廓，点云数据不稳定，BC段是扫描失真区域（经过实测，OC约为45 m）。C点之外，障碍物离开雷达视线。图8是LabVIEW 实时采集的界面，考虑到车辆的外形轮廓，图8（a）表示障碍物刚进入雷达扫描范围，图8（b）表示车身完整轮廓显示，图8（c）表示车辆即将离开扫描范围。

图7 雷达坐标系下扫描示意图

图8 LabVIEW 实时采集的界面

常用运动模型分为匀速运动、匀加速运动、变速运动和统计模型，由于模型的复杂多变难以得到定性的实验结果，本文中试验场景包含静止的试验车扫描运动的目标车和运动的试验车扫描从左后方超车的目标车。

为了简化模型，在数据关联成功之后，提取当前时刻障碍物外接矩形框的几何点，取其坐标作为该障碍物的位置信息。连续两帧的同一个障碍物的位置除以扫描时间，可以直接求出速度。

式中：xt、yt为t时刻的X、Y方向的坐标值；xt+1、yt+1为（t+1）时刻的X、Y方向的坐标值；T为扫描周期。

2.2 轨迹跟踪

在实际情况下，很难得到运动对象的真实状态值。雷达在探测目标时，根据反射波能够得到目标的距离，但雷达探测时存在周围环境的不确定干扰，需要在包含噪声数据的观测信息数据集中，识别出车辆的状态参数。因为很难获取所需运动对象的真实状态值，只能根据观测信号估计或者预测这些状态变量，工程实践中，通常选取卡尔曼滤波器［17］作为降低噪声干扰的处理方法。

雷达空间中运动物体的运动模型可以表示为：

式中：X（k）为4阶的状态参数矩阵，包含位移的X方向和Y方向参量，速度的X方向和Y方向参量，F是状态矩阵，W（k）和Q分别表示过程噪声矩阵和过程噪声值。

系统的观测方程：

式中：Z（k+1）为雷达观测量；H 为观测矩阵；V（k）为观测误差矩阵。

目标信息的下一步预测：

一步预测协方差阵：

求滤波增益矩阵：

状态更新：

协方差更新：

通常，状态矩阵F表示为

观测矩阵H：

过程噪声Q：

观测噪声R：

协方差矩阵P：

式中：In为n阶单位矩阵；P为协方差矩阵，表示误差的偏差，与X（k）状态矩阵相对应，同为4阶矩阵；K为卡尔曼增益；X＾为卡尔曼估计量；Q为过程噪声方差；R为观测噪声方差；dt为雷达扫描周为100 ms。通过雷达采集的大量离线数据实测，Q和R取值参照式（17）和式（18）。本文中，对上述公式进行了修正，在式（9）中，原公式中的观测值Z（k）用雷达关联后的障碍物的信息data（k）代替，式（9）实际情况下不应用于滤波程序。式（13）中的Z（k+1）用data（k+1））替换，即表明雷达的观测数据，包含了雷达的观测噪声方差R，实时更新导入卡尔曼滤波程序中。经过多次迭代后，滤波器的增益矩阵和协方差矩阵不断更新，以适应障碍物连续多帧的航迹［18］。

3 试验验证

3.1 试验实车平台

图9为重庆理工大学机械工程中心搭建的第二代物流电动车平台。该物流小车主要用于解决社区和校园道路最后1 km的快递物件的配送问题。该实验车的传感器搭载方案包括L1NO单线激光雷达（图10）、1套RGPark 982 GNSS卫星导航系统（图11）、1台具备4核2.7 GHz中央处理器和8 G运行内存的工控机。表3为RGPark 982 GNSS卫星导航系统的定位精度。

图9 物流电动车平台

图10 L1NO单线激光雷达

图11 RGPark 982 GNSS卫星导航系统

表3 RGPark 982 GNSS卫星导航系统定位精度

3.2 试验设置

使用LabVIEW+DLL软件平台，动态链接库DLL是一个多程序可以共享的数据模块［19-20］，扩展名为dll，内部对代码进行封装。其与可执行文件（EXE）较为接近，区别在于DLL虽含有执行代码却不能单独执行，必须用Windows应用程序调用。实验道路选在靠近校园操场的一段路，如图12所示。

图12 实验现场

L1NO雷达扫描时间100 ms，算法处理时间150 ms。图13为雷达自带软件扫描原始场景和LabVIEW 软件下的扫描结果。

图13 雷达扫描障碍物

实验场景1中，实验车辆静止，运动的目标车辆以12 km/h的平均时速左侧驶过，单线激光雷达扫描并采集到60帧的车辆信息。实验场景2，20 km/h平均时速的目标车辆对前方行驶的实验车辆超车后右转，实验车辆上的雷达感知系统采集85帧的目标车辆信息。受制于单线激光雷达100°的水平视场角和雷达的实时数据刷新率，对于宽×长为2 m×4 m的车辆的检测，目标车辆位于雷达前方5～30 m的识别效果最佳。

图14是场景2实验时算法捕捉雷达扫描目标相邻帧的LabVIEW 图形。

在场景2超车实验中，篮球场前侧的石墙被误判为车辆，如图12箭头所指（1）处。随着扫描帧数的增加，车辆的长宽外形尺寸上的稳定性以及速度的连续性，被数据关联和滤波算法判定为其他的障碍物，因而取消了对不属于同一个移动障碍物的跟踪。此算法能实时获取移动车辆的位置和速度，并且在LabVIEW 的VI程序前面板中动态显示。

图14 场景2实验LabVIEW 实时图形

3.3 实验结果分析

卡尔曼滤波算法依据时间维度进行滤波，程序迭代是进行目标状态的“预测—修正”。通过实验结果可知，刚开始前几帧滤波值跳动角度，滤波效果不理想。随着迭代次数的增加，卡尔曼滤波算法根据连续多帧的观测数据，不断调整自身的过程噪声误差Q、观测噪声R、误差协方差P。线性卡尔曼滤波能够有效减少位移和速度的跳变，使运行运动状态更加平稳。稳定跟踪后的滤波值受到观测状态的影响，但是总体上与实际状态相符。在雷达出现扫描失真时，通过对障碍物位置以及速度参数的滚动更新，提供最新的状态信息给决策模块，以适应复杂多变的道路环境。当连续5帧障碍物信息失真或者完全丢失，则终止该轨迹。图15、16表示场景1的位移和速度滤波结果。

图中散点表示雷达扫描的障碍物位移和速度信息，实线是滤波结果。通过分析，实验车辆以12 km/h的平均速度行驶，雷达扫描采集的观测数据波动较大，通过卡尔曼滤波，减小了环境噪声点的干扰，当前时刻所预测的位移和速度信息更接近下一时刻的观测信息。图17、18表示场景2位移和速度滤波结果。

图15 场景1位移滤波轨迹

图16 场景1速度滤波曲线

图17 场景2位移滤波轨迹

图18 场景2速度滤波曲线

场景2中，速度波动较大，这是因为雷达在运动的试验车上，受到自身支架轻微震动的影响，导致聚类点云位置波动。相比较场景1，场景2中的实时位移和速度波动较大，但通过卡尔曼算法对多个相邻时刻障碍物参数信息的滚动优化，观测值与预测值之间的误差变小，滤波之后的位移和速度减小了干扰信息，保证输出参数的平稳。通过在校园路段多次试验，本算法对于车辆前方30 m的障碍物的聚类的准确率为99%，跟踪的有效率为90%，少数未能准确匹配的几帧参数，主要原因是单线激光雷达在室外的工作环境下，外部强光光线的干扰以及雷达对车辆特殊颜色（黑色）的反射率不高。

4 结论

对校园环境下低速行驶的车辆的检测跟踪问题进行研究，提出了在单线激光雷达点云数据上检测移动障碍物算法。在Simulink中完成点云聚类，多帧点云信息的数据关联，对同一个运动目标观测出实时轨迹和速度。通过加载相关插件的编译环境后，编译成动态链接库DLL，放入LabVIEW底层框架下予以调用。考虑到激光雷达的观测噪声和过程噪声，对于出现障碍物信息丢失以及航迹跳动较大的情况，滤波程序能够给出修正后的估计值，提供给决策系统，保证航迹的稳定和行车的安全。实验表明，本算法能够以150 ms的刷新频率，对车辆前方30 m的运动的障碍物实时有效地识别和跟踪，满足低速行驶条件下物流电动车对道路环境感知的需求。

在此工作的基础上，下一步研究的重点是提高识别的正确率和多场景应用：

1）采用多个单线激光雷达，提高水平扫描视场角，降低漏检率；

2）考虑增加相机，通过相机这种视觉传感器采集的深度信息，并融合其他传感器采集的信息，强化对道路路面车辆信息的检测能力；

3）增加对行人的扫描识别，提高对车辆前方障碍物的感知能力。