基于车载激光雷达的隧道内障碍物检测

曾 祥，蒋国涛，鲍纪宇，刘邦繁，肖志鸿

（中车株洲电力机车研究所有限公司，湖南 株洲 412001）

0 引言

激光雷达因具有高测距精度、高分辨率、受光照影响小的优点而被广泛应用于车辆环境感知领域。Premebida Cristiano，Ludwig Oswaldo等[1]对激光点云进行特征提取和分类，实现了对城市场景中行人的识别。Zhang Wende[2]使用激光雷达对道路和道路边缘进行了检测。Muresan Mircea Paul，Nedevschi Sergiu等[3]针对高线数激光雷达存在的价格昂贵的不足，提出了一种基于4线激光雷达的实时目标检测算法。Granström Karl，Renter Stephan 等[4]提出了一种随机优化方法，以直接处理多目标跟踪中的数据关联问题。Sahba Ramin，Sahba Amin等[5]使用PointPillars网络对汽车、行人进行了识别。Szarvas M.，Sakai U.等[6]采用卷积神经网络实现了激光点云中对行人的高效检测。黄如林、梁华为等[7]提出了基于多特征融合的动态障碍物检测跟踪方法和基于时空特征向量的动态障碍物识别方法，提高了激光雷达对动态障碍物的识别和跟踪准确率。宗长富、文龙等[8]提出了一种新颖的点云去畸变方法和一种基于距离分区的欧氏聚类算法，在一定程度上解决了欧氏聚类算法在点云分布不均及存在畸变时的障碍物检测成功率低的问题。胡云卿、冯江华等[9]利用激光雷达、毫米波雷达和360度环视系统构建了智轨电车的环境感知系统。然而，目前基于激光雷达的障碍物检测多在开放环境中进行，针对隧道内障碍物检测的研究尚不多见。近年来，随着山区公路交通建设和城市轨道交通建设的快速发展，隧道里程增长迅速，应用激光雷达对隧道内的障碍物进行检测，有助于提高车辆在隧道内运行的安全性。

本文对基于车载激光雷达的隧道内的障碍物检测问题进行了研究。在滤除隧道壁点云和地面点云的基础上，对点云进行聚类以识别障碍物；采用改进的距离准则进行数据关联，结合卡尔曼滤波实现障碍物的运动状态更新，并设计了生命状态转移机制以实现障碍物的航迹管理。

1 基础算法原理

本文对点云的处理主要涉及随机采样一致性（random sample consensus，RANSAC）方法[10]和欧几里得聚类方法。

1.1 RANSAC模型拟合

基于待拟合的模型（如直线），可由N个空间点Pi(xi,yi,zi)(其中i=1, 2, …,N)构成的点集P建立超定方程组Pv=q，其中矩阵P和向量q由点集P确定，v为待估计的参数向量。通常，v的估计通过最小二乘法进行。当P中存在异常数据时，若直接采用最小二乘法求解v，可能会产生较大偏差。

RANSAC算法在求解v时充分考虑了异常数据的影响。设P中局内点的占比为ε，重复地从P中随机抽取m个点构成子集。若某子集中不含有异常点，则该子集为良性子集。记至少取得一个良性子集的概率为p*，其与相应的最小迭代次数Nmin满足如下关系：

设定任意点是否归属于拟合模型的阈值Tε，并记集合为每次迭代结束后点集P中归属于拟合模型的点构成的子集。记M为集合的元素个数，K为待估计参数的个数，即向量v的维数。基于RANSAC的模型拟合步骤如下：

（2）从P中随机选择K个不共线的点Pi(xi,yi,zi)，其中i=1, 2, …,K。

（3）产生估计假设。由选择的K个点坐标计算参数向量v。为避免混淆，记此时的参数向量为v0。

（4）假设验证和优选。计算由v0引起的偏差向量e=Pv0-q。引入计数m并初始化，使m=0。依次遍历e中的所有元素，若第j个元素e(j)满足|e(j)|≤Tε，则计数m增1。比较m和M的大小。若m>M，则代表当前估计的参数向量v0较上次的结果更优，此时，先清空，再将所有满足|e(j)|≤Tε的点Pj加入集合中；反之，若m≤M，则放弃当前估计。

（5）重复步骤（2）～（4），直到迭代次数达到Nmin。

1.2 欧几里得聚类

对于具有明显邻域关系的元素的聚类问题，采用基于距离准则的近邻聚类方法通常能取得不错的效果。本文采用的是成熟的欧几里得聚类算法，同时采用维树（k-dimensional tree，kd树）以加快对邻域元素的搜索。算法流程如下：

（1）对输入数据P建立kd树，创建一个空的聚类列表C和待处理元素队列Q，设定元素的邻域半径δ。

（2）对P中每一个未处理的元素Pi，将Pi加入Q中。

（3）对Q中每一个点，在P中搜索其δ邻域内的一组近邻点；对任一近邻点，若其未被处理过，则将该点加入Q。重复该步骤，直到Q中所有点都被处理完毕，则将Q中的点加入C中，并清空Q。

（4）检查P中的所有点是否均已被处理且加入C中。若是，则算法结束；反之，则返回到步骤（2）。

2 障碍物检测算法

激光雷达在隧道内扫描时，点云的主要来源包括隧道壁、地面和可能存在的障碍物。图1示出隧道内的激光点云数据，可以看出，在接收点云中，隧道壁点云和地面点云的占比非常高。对于障碍物检测而言，海量的隧道壁点云和地面点云属于背景点云，容易造成虚警，且会显著降低障碍物检测的效率。基于此，针对隧道内障碍物的检测，对隧道壁点云和地面点云的滤除提出了很高要求。

图1 隧道内的激光点云数据Fig.1 Point cloud from LiDAR in the tunnel

此外，实践中，受激光雷达分辨率、点云稳定性和点云处理算法自身精度等的影响，障碍物位置和尺寸信息常会有所偏差，且障碍物的漏检或误检问题几乎不可避免。为实现障碍物信息的稳定输出，对障碍物的稳定跟踪是不可或缺的。综上要求，本文设计的基于车载激光的障碍物检测算法流程如下：（1）隧道壁点云分割；（2）地面点云分割；（3）目标聚类；（4）目标跟踪。

为简化计算，在隧道壁点云分割之前，本文首先采用VoxelGrid体素滤波器[11]对点云进行下采样处理，然后再将点云从激光雷达坐标系转换到车辆坐标系。车辆坐标系以向前方向为x轴的正方向，向上方向为z轴的正方向，且x轴、y轴、z轴构成右手坐标系。

目标聚类的基本任务是将障碍物点云划分为互斥的若干个子集，每个子集中的激光点云构成一个障碍物。本文采用欧几里得聚类法对障碍物进行聚类。其与隧道待拟合边界点云聚类的区别在于，障碍物聚类时，权重wx=1。聚类完成后，障碍物的中心坐标与尺寸信息可通过计算聚类后点云的轴向平行包围盒得到。相关算法均属于成熟方法，本文不赘述。

3 隧道壁点云分割

隧道壁包括隧道顶和隧道侧壁。显然，隧道顶点云的高程值z较大。由此，设定高程阈值为Tz，且感兴趣区域为满足z≤Tz的区域，即可滤除隧道顶点云的区域。

记滤除隧道顶后留下的点云为P0。为研究P0的分布，将P0向水平面上投影，得到如图2所示的投影点云P。可以看出，P表现出明显的带状分布，左右两侧的隧道侧壁点云各形成一条明显的边界点云带。采用曲线对边界点云带进行近似，则可将隧道侧壁的检测问题转换为边界曲线拟合问题。完成边界曲线拟合后，由边界曲线确定隧道侧壁点云，需要将边界曲线朝隧道内部进行等距离偏移，即需要求解边界曲线的等距曲线。

图2 点云的水平投影视图Fig.2 Horizontal projection view of the point cloud

基于以上思想，本文设计的隧道侧壁点云分割方案由5个过程组成：（1）点云栅格图产生；（2）边界点云初选；（3）边界点云复选；（4）边界曲线拟合；（5）边界点云滤除。

3.1 点云栅格图的产生

设置栅格的长度L和宽度W，将预处理后的点云分配到相应的栅格中。设点云的最小x,y坐标分别为xmin,ymin，那么对于坐标为(x,y,z)的点，其栅格坐标(i,j)满足i=[(x-xmin)/L]，j=[(y-ymin)/W]，其中“[]”表示向下取整算子。

依次处理点云中的所有点，生成点云栅格图Q。

3.2 边界点云的初选

激光雷达在隧道内扫描时，隧道侧壁能返回较多点云，相应的栅格中具有较多的激光点；隧道外部则理论上无点云返回，相应的栅格为空。由此，如图3所示，对于Q中的每一行栅格，从最左端的栅格开始遍历，首个非空栅格即为隧道侧壁栅格；从最右端的栅格开始遍历，情况亦然。为降低噪点的影响，设置阈值。当栅格内的激光点数不低于该阈值时，将栅格标记为隧道侧壁栅格。

图3 栅格的遍历方式Fig.3 The way of traversing all grids

收集所有隧道侧壁栅格中的点云，用于下一步处理。

3.3 边界点云的复选

初选的边界点云中从属于左侧隧道壁和右侧隧道壁的点云未被分离，且可能存在不属于任何一侧隧道侧壁的虚假边界点云。这些不足可能会对后续边界曲线拟合造成干扰。

理论上，从属于同一侧隧道壁的边界点云，其中的激光点应具有较小的距离，即在空间上是相邻的，整个边界点云带是连续的、不具有明显间隔的窄长区域；而不同侧隧道壁的边界点云则具有较大的距离，即在空间上不相邻。基于此假设，对于源自左、右侧隧道壁初选的边界点云，可以采用欧几里得聚类方法予以分离。

然而，在实际中，受到障碍物遮挡、隧道侧壁栅格标记错误等因素的影响，从属于同一侧隧道壁的边界点云带可能出现局部断裂现象。此时，边界点云带是在x方向分段的、有少数明显间隔的窄长区域。为将分段的窄长区域聚类成一体，需要考虑到x方向间隔的影响。基于此，对初选的边界点云在x方向作压缩处理，即对其中的激光点Pi(xi,yi)执行缩放变换：

式中：wx——权重。

wx越小，聚类对x方向局部断裂的鲁棒性越强，但也增大了将多个独立的仅在x方向上不相邻的目标聚类为一体的风险。

初选的边界点云经聚类产生多个独立的子点云。其中，隧道侧壁点云在x方向较长，而虚假边界点云则较隧道侧壁点云明显偏短。基于此，分别计算聚类后产生的子点云在x方向的长度Δx=xmax-xmin，其中xmax和xmin分别为子点云的x坐标的最大值和最小值。Δx最大的两个子点云即为两组隧道侧壁边界点云。

3.4 边界曲线拟合

采用RANSAC算法拟合隧道边界曲线，拟合模型为抛物线。设抛物线的二次项、一次项和常数项系数分别为A1，B1和C1，即抛物线方程为y=y(x)=A1x2+B1x+C1。超定方程组Pv=q满足

显然，C1>0时拟合所得的为左侧隧道壁边界曲线，反之为右侧隧道壁曲线。为予以区分，分别记左侧、右侧隧道边界曲线为G1:y=y1(x)，G2:y=y2(x)，G1和G2的由二次项、一次项及常数项系数构成的参数向量分别为v1=[A11,B11,C11]T，v2=[A12,B12,C12]T。

3.5 边界点云滤除

隧道边界曲线G1，G2的邻域可由它们的等距曲线确定。记G1，G2处于隧道内部一侧的等距曲线分别为G*1，G*2，对于给定的激光点Pi(xi,yi,zi)，若满足Pi在G*1，G*2之间，则可认为Pi为非隧道边界点；反之则为隧道边界点。记G*1和G*2的曲线方程分别为y=y1*(x)，y=y2*(x)，则Pi的滤除条件可以表示为(yi-y1*)×(yi-y2*)≥0。由此，边界点云滤除问题转换为等距曲线G*1和G*2的估计问题。

显然，当隧道边界曲线G1和G2为抛物线时，等距曲线G*1和G*2的形式比较复杂；但实际应用中，仍然可以采用抛物线对G*1和G*2进行近似，实现对边界点云的滤除。以根据G1计算G*1为例，其计算过程如下：

（1）隧道边界采样

对y1(x)进行等间隔采样，得到由采样点P1i(iΔx,y1(iΔx))（i=0, 1, …,Nc-1，Nc为总采样数）构成的采样点集。同时，计算G1在P1i处的单位法向量ni(nix,niy)。

如果niy>0，将ni取反。

（2） 等距曲线采样

图4示出等距曲线采样点的生成示意，其中给定邻域半径为r0。对于给定的隧道边界点P1i(iΔx,y1(iΔx))，可计算等距曲线的采样点：

（3）等距曲线估计

经采样所得的P*1i(x*i,y*i)均不是局外点，因此可直接应用最小二乘法求解G*1。

类似地，可求解得出G*2，继而按照条件(yiy*1)×(yi-y*2)≥0滤除所有符合要求的点Pi(xi,yi,zi)，保留下来的点云则将用于地面分割。至此，隧道壁点云分割完成。

4 地面点云分割

与隧道壁点云类似，大量的地面点云存在于激光雷达接收到的点云中，需要被滤除。本文采用基础的基于栅格平面拟合的方法进行隧道内的地面点云分割[12-14]。

沿用隧道侧壁点云分割时的点云栅格图。对每一个栅格，计算其中激光点的个数n、点云高程的中值zmed和最小值zmin，并定义该栅格的高程差Δz：

障碍物具有一定的高度，因此其相应栅格应具有较大的高程差，而地面相应栅格高程差较小，据此实现对地面点云的初步筛选。对于起伏较少、仅需考虑坡度因素的地面，如隧道地面，可采用空间平面进行近似。

采用RANSAC算法拟合地面，拟合模型为平面z=A2x+B2y+D2，其中A2,B2,D2为系数，则：

地面点云分割的基本流程如图5所示，具体如下：

图5 地面点云分割的流程Fig.5 Flow chart of ground segmentation

（1）点云栅格化，计算栅格高程差Δz。

（2）地面栅格点云收集。初始化候选地面点云集合P，使。设置阈值Ts，满足Δz≤Ts的栅格被视作地面栅格，将该栅格内的点云加入集合P中。

（3）地面拟合。采用RANSAC方法拟合P中的数据，拟合模型为平面∑。

（4）地面点云移除。设定阈值Td，对P中的点Pi，计算Pi到平面∑的距离di。若Pi≤Td，则Pi属于地面点，予以移除。

5 目标跟踪

考虑到噪声及其他干扰以及障碍物检测算法自身的局限性，检测得到的多个障碍物位置信息不可避免地存在偏差。为此，有必要引入目标跟踪算法以降低检测偏差。针对单目标跟踪，本文采用成熟的卡尔曼滤波算法；针对多目标的跟踪，先采用成熟的全局最近邻（global nearest neighbour, GNN）[15]数据关联算法将其分解为多个单目标跟踪问题，然后再应用卡尔曼滤波进行处理。常规的GNN算法只采用了障碍物的位置信息作为距离准则，在特定场景下的关联效果不佳。为此，本文对距离准则进行改进，以提高GNN数据关联的鲁棒性。

此外，相比真实情形，聚类结果仍可能存在障碍物的漏检或误检。基于此，本文提出一种基于生命状态转移的航迹管理策略，对聚类给出的结果进行辨识，以快速地实现错误检测的去除和遗漏检测的补充。

5.1 改进距离准则的GNN数据关联

设卡尔曼滤波预测得到的目标A位置为(xi,yi)，测量目标B位置为(xj,yj)。将二者间常规的距离定义为，其中，vij=(xj-xi,yj-yi)，为A和B的位置残差，S为卡尔曼滤波中的残差协方差。d1的定义只考虑了目标的位置信息而忽略了目标的尺寸信息，可能产生不准确的关联结果。为此，本文引入视觉领域的交并比（intersection over union，IOU）概念，并记d2=2-IOU且d2∈[1, 2]。在d1和d2的基础上，本文将A和B间的距离d3定义为

式(9)中，指数λ反映了d2的重要性。λ越大，d2的重要性越高。

图6示出采用仿真数据对di(i=1, 2, 3)的关联效果进行比较的结果。图6（a）中，目标R保持静止，位置坐标相同的目标P和Q以相同速度沿直线l向x轴正方向运动。显然，因P和R的尺寸差异较Q和R的小，在同一时刻，P和R的关联程度应不低于Q和R的，即距离应满足关系d(P,R)≤d(Q,R)；同时，对于P或Q，在其逐渐接近R到逐渐远离R的过程中，距离应具有先递减后递增的变化规律。

图6（b）～图6（d）给出了P和Q从接近R到远离R的d1,d2和d3的变化规律，计算时取S为单位矩阵I，λ=1。由图6（b）可以看出，由于仅利用了位置信息而忽略了尺寸信息，因此d1(P,R)、d1(Q,R)始终相等，即仅依靠d1无法对P，Q和R的关联程度进行区分；同时，由图6（c）可以看出，由于Q在区间K1～K2内沿x方向始终被R完全包含，二者的交集为Q，并集为R，因此d2(Q,R)在该区间内始终保持不变，即此时仅依靠d2无法区分Q与R在“接近—远离”过程中的关联程度；类似地可对P进行分析。相比之下，图6（d）中的d3很好地揭示了“P与R的关联程度更高”和“P或Q与R的关联程度先递增后递减”的事实。

距离准则确定后，其他步骤与常规的GNN算法相同，本文不再赘述。

图6 3种距离准则的性能比较Fig.6 Performance comparison among the three different distance metrics

5.2 基于生命状态转移的航迹管理

图7示出目标的生命状态转移关系。定义每个目标在生命周期内可能的生命状态为头、可见、隐藏、撤销。对于新发现的待确认（为障碍物或者误检）的目标，标记其生命状态为头；对于已确认为障碍物的目标，若在当前帧能检测到，则标记其生命状态为可见，反之则标记其生命状态为隐藏；对于已确认为误检的目标或隐藏过久的目标，标记其生命状态为撤销，并停止该目标的跟踪。上述4种生命状态可触发如（i）～（ix）所示的生命状态转移。

图7 可能的生命状态转移Fig.7 All possible life state transitions

设现有障碍物集合为S，待研究目标为O，对生命状态转移关系可进行如下描述。

5.2.1OS

采用滑窗法的m/n检测准则进行判断，即确认O∈S的条件如下：在宽为n的滑动窗口内，O至少出现m次。为此，从首次发现O开始建立滑动窗，初始时窗宽为1，随后每帧增1，直到达到n。与此相关的生命状态转移为（i），（ii）和（iii）：

（i） 当前窗宽未达到n，O保持头状态。

（ii）O经m/n检测通过，则O∈S，生命状态转换为可见。

（iii）O经m/n检测不通过，则生命状态转换为撤销，即删除O。

5.2.2O∈S

目标O一旦被确认为障碍物，对其进行撤销操作必须谨慎。为此，设定将O移除出S的条件为：O连续消失n*次。与此相关的生命状态转移为（iv）～（ix）：

（iv）O上次处于可见状态且当前仍可见，则维持可见状态。

（v）O上次处于可见状态但当前不可见，则连续消失次数为1。若n*=1，则生命状态转换为撤销，即直接从S中删除O。

（vi）O上次处于可见状态但当前不可见，即连续消失次数为1。若n*>1，则生命状态转换为隐藏。

（vii）O上次处于不可见状态但当前可见，转换为可见状态。

（viii）O上次处于不可见状态而当前仍不可见，且连续消失次数未达到n*，维持隐藏状态。

（ix）O连续消失次数达到n*，转换为撤销状态，从S中删除O。

6 实验验证

在某地铁隧道内采集激光点云数据，安排4名行人在激光雷达的视场范围内从远到近接近激光雷达，模拟现实中运动的障碍物。处理在此过程中的连续多帧数据，检查是否发生行人的漏检、误检以及行人处于静止状态中连续帧内的检测位置是否剧烈跳动，以验证本文障碍物检测算法的效果，实验结果如图8所示。

图8（a）示出采集的原始点云数据，可以看出，除障碍物点云外，还含有大量的隧道壁点云和地面点云。图8（b）示出隧道侧壁检测中，拟合所得的边界曲线和等距曲线。相应地，边界点云滤除的结果如图8（c）所示，可以看出，隧道壁点云在此图中已经被成功滤除。继续对点云进行地面点云分割，产生的地面点云和障碍物点云分别如图8（d）、图8（e）所示，可以看出，二者已经被成功分离。最后，对障碍物点云进行目标聚类和目标跟踪，输出的4个障碍物的包围盒如图8（f）所示；同时，图8（f）以文本形式给出了障碍物的ID和位置信息。这4个障碍物的包围盒即代表了运动的4名行人。由图8可以看出，采用本文方法成功地实现了对该帧点云中隧道内障碍物的检测。

图8 实验结果Fig.8 Experimental results

同时，对行人从远到近接近激光雷达这一过程中的多帧数据分析结果表明，本文方法均能成功输出相应的包围盒；在行人停留在同一位置的连续帧内，行人的位置跳动不超过0.15 m。实验结果表明，本文所提方法能够准确检测隧道内的障碍物。

7 结语

本文提出了一种基于激光雷达的隧道内障碍物检测算法，该算法首先对点云中的隧道边界和地面成分进行滤除，然后对障碍物进行聚类检测和跟踪。实测数据的处理结果验证了本文所提方法在隧道内障碍物检测方面的有效性。需要指出的是，本文方法是基于隧道不发生分岔的前提；对于隧道分岔的场景，本文方法并不适用，后续将对此进行研究。