CN111337941A - 一种基于稀疏激光雷达数据的动态障碍物追踪方法 - Google Patents

Abstract

本发明公开了一种基于稀疏激光雷达数据的动态障碍物追踪方法及系统，属于动态障碍物追踪技术领域，包括以下步骤：S1：消除静态背景；S2：动态点云聚类；S3：凸包提取；S4：特征提取；S5：数据关联；S6：预测轨迹。本发明使用栅格地图过滤静态障碍物激光雷达点云，剩余少量静态点云通过dbscan算法，将极为稀疏的点云过滤，增强了最终的过滤效果；通过提取点云凸包的角度信息模糊分类点云为椭圆、矩形、直线，然后辅助点云拟合图形的尺寸获得正确的点云位置点，保证了数据关联的准确性；还综合了最近邻域和多目标假设算法的特性，改进了多目标关联算法，能够在保证准确性的前提下高效地完成数据关联工作。

Description

一种基于稀疏激光雷达数据的动态障碍物追踪方法

技术领域

本发明涉及动态障碍物追踪技术领域，具体涉及一种基于稀疏激光雷达数据的动态障碍物追踪方法及系统。

背景技术

基于视觉传感器的物体检测与追踪技术已经比较成熟了，但是视觉传感器通常不能提供距离信息，且必须在光照良好条件下使用，限制了其在机器人导航方面的使用。而激光雷达不受光照影响并且能提供距离数据，能完成机器人导航、追踪等任务。近年来，利用激光雷达对动态障碍物进行检测和轨迹预测是移动机器人领域研究的重点课题。

随着无人驾驶技术的发展，已经有大量动态障碍物的检测与追踪方法。对于车辆的检测和追踪，Konrad M等利用栅格地图对汽车进行检测和追踪。谌彤童提出了一种新的全局柱坐标直方图特征用于在城市环境进行车辆识别，基于似然场模型的动态车辆检测与跟踪算法用于追踪车辆，斯坦福大学的Anna Petrovskaya等提取被跟踪车辆的几何、动态特性，建立相应特征模型，利用贝叶斯滤波器对跟踪目标进行更新，但是被遮挡的车辆仍无法检测。杨飞等利用模板匹配法对障碍物的外轮廓形状特征进行匹配，但模型类别较少，障碍物匹配的适应性较差。邹斌等通过对单帧激光雷达点云数据进行聚类，提取障碍物外接矩形轮廓特征，采用多假设跟踪模型(MHT)算法对连续两帧的障碍物信息进行数据关联，利用卡尔曼滤波算法对动态障碍物进行连续地预测和跟踪。

无人驾驶领域除了要躲避车辆等大型障碍物，还要考虑行人的移动。近年来行人的检测与追踪技术也得到了普遍研究。传统的行人检测方法主要依赖于可见光摄像机等传感器获取的图像，采用机器视觉的相关方法进行行人检测工作。Premebida等提出了一种基于激光雷达的15维特征，用于在城市环境下进行行人检测。这些特征中的一部分特征，例如最小反射距离，聚类后的类内点个数等。Wan等提出了一种基于激光雷达和图像融合的行人检测方法，能够利用融合特征进行行人检测。韩骁枫等利用dbscan算法可以将非地面激光雷达点云进行聚类，并且提出了一种快速点特征直方图分布特征，用于训练支持向量机分类器进行行人的检测工作中。

在无人驾驶领域通常使用三维激光雷达检测和追踪，但是对于矿井的封闭环境，三维激光雷达的采集数据量大，这对于数据实时性要求高的全方位移动巡检机器人是一种比较大的负担。而二维激光雷达采集一个水平面的点云，数据量较小，处理则会更迅速。但数据量小导致点云稀疏，使用这些稀疏数据得到的处理结果准确性会变差。所以使用二维雷达作为传感器时，在数据处理过程中准确性是首先要考虑的问题。在矿井下，由于光照原因不能辅助以视觉传感器，而且矿井这种封闭环境三维雷达的数据量大，处理速度慢，但所以最终选择用二维雷达作为传感器辅助移动机器人完成检测和追踪任务。而二维激光雷达采集一个水平面的点云，数据量较小，数据量小则导致点云稀疏，使用这些稀疏数据得到的处理结果准确性会变差，为此，提出一种基于稀疏激光雷达数据的动态障碍物追踪方法及系统。

发明内容

本发明所要解决的技术问题在于：如何保证利用二维雷达的稀疏数据能获得正确追踪效果，并在数据处理过程中减少数据的误差，提供了一种基于稀疏激光雷达数据的动态障碍物追踪方法。

本发明是通过以下技术方案解决上述技术问题的，本发明包括以下步骤：

S1：消除静态背景

确定可行驶区域，删除可行驶区域的地面点云和超过指定高度的点云，剩余点云即为障碍物点云；

S2：动态点云聚类

对步骤S1中的障碍物点云进行聚类，将属于不同动态障碍物的点云分为不同的簇；

S3：凸包提取

经过步骤S2的聚类后，提取聚类点云的凸包；

S4：特征提取

利用凸包的角度信息进行模糊分类，再对这些点云拟合获得障碍物位置信息；

S5：数据关联

根据不同帧中障碍物的位置信息关联属于同一障碍物的不同时刻的位置，并拟合为轨迹，将已经追踪到的轨迹与当前数据帧检测到的聚类点云关联，更新动态障碍物的运动轨迹；

S6：预测轨迹

利用轨迹函数预测动态障碍物的位置：根据轨迹函数增加最后一个轨迹点的x值的x_p，带入公式求得y_p坐标，(x_p,y_p)即为预测轨迹点，直线预测时带入的公式如下：

其中，(x_i,y_i)是拟合直线的轨迹点，a₀为拟合直线的斜率，a₁为拟合直线的截距,R²为轨迹点与拟合直线的距离差的和；

多项式曲线预测时带入的公式如下：

其中，(x_i,y_i)是拟合三次多项式的轨迹点，a₀、a₁、a₂、a₃为拟合三次多项式的系数,R²为轨迹点与拟合直线的距离差的和；

轨迹函数包括直线预测和多项式曲线预测函数，直线预测函数如下：

x_p＝x[n]+(len)/(t*n)

其中，X[n]是当前轨迹点，len为已有轨迹点的x坐标的增量绝对值的和，t为可调参数，n为已有轨迹点个数，在直线预测中根据已有轨迹的x变换程度的平均值估计当前时刻可能的x变换程度；

多项式曲线预测函数如下：

xp＝x[n]+len/(t*n)*k

其中，X[n]是当前轨迹点，t为可调参数，n为已有轨迹点个数，len为已有轨迹点的欧式距离的和，k为当前轨迹的斜率，在多项式曲线预测中，根据已有轨迹点之间的x增量的平均值估计当前时刻的x增量。

更进一步的，在所述步骤S1中，点云只包含坐标信息，坐标形式为二维坐标(x,y)。

更进一步的，所述步骤的S1中的具体处理过程包括以下步骤：

S11：确定采集平面

根据主要障碍物的高度设置采集平面的高度为0.8米～1.0米；

S12：生成模板

构建栅格地图或者转换已经存在矿井地图为栅格地图，栅格地图分为障碍层、膨胀层、可行驶层；

S13：预处理

加载步骤S12中已经建好的栅格地图，设置膨胀层厚度为0.05米，接收激光雷达的原始数据，然后处理点云，删除点云中的非法值；

S14：过滤点云

用点坐标和栅格地图做比较判断是否处于可行驶层，如果在可行驶层则保留否则删除此点。

更进一步的，在所述步骤S2中，聚类过程中将点云分为核心点和边界点，其中核心点为聚类后的骨架，聚类过程中涉及两个核心参数，分别为Eps、MinPts，Eps是点云之间的邻域的边界公式，如下所示：

Ne＝{x_i∈D|distance(x_i,x_j)＜Eps}

其中，x_i代表点云中的点，distance(x_i,y_i)为点云之间的欧式距离,Eps为聚类算法归为同一类点云的距离阈值，D为范围领域；

当两点之间的距离小于Eps，则判断该两点在同一邻域内；MinPts是判断核心点的参数，如果点的邻域内距离小于Eps的点个数大于MinPts则判断其为核心点。

更进一步的，聚类过程包括以下步骤：

S21：统计每个点邻域内点的个数，存入pts属性中，判断每个点pts属性判断是否大于MinPts，如果大于则是核心点，并存入核心点数组中；

S22：遍历核心点数组将在当前核心点邻域范围内的核心点串联为一类，

S23：遍历所有边界点，将核心点邻域内的边界点并入核心点类中。

更进一步的，在所述步骤S4中，矿井环境下的移动物体是行人和矿车，行人分类为椭圆点云，矿车从不同的角度观察可以分类为矩形点云和直线点云，根据上述物体的特点利用凸包的角度信息模糊分类为椭圆形、矩形、直线。

更进一步的，在所述步骤S4中，点云的夹角信息是通过凸包获取的，获得点云的一组凸包点后，选择其中距离激光雷达的最近点为角的顶点，统计以最近点为顶点的角度信息，以此作为点云的角度特征，即夹角信息。

更进一步的，点云角度特征的具体处理过程包括以下步骤：

S41：将所获得的凸包数组中距离激光雷达最近的点作为顶点；

S42：以凸包数组中左下角的点作为起始点顺时针开始作为角一边的端点，逆时针开始以凸包点作为另一边的端点计算角度，同时统计角度最大值并求和；

S43：通过对动态障碍物的点云角度特征提取后的角度统计，得到角度分类标准，根据角度分类标准确定角度类型；

S44：根据角度类型判断点云数据的来源，根据不同来源的不同行驶特征使用不同的拟合公式对不同类型的图形点云拟合，进而计算出其集合特征。

更进一步的，利用椭圆拟合公式拟合椭圆获得椭圆的圆心、长轴、短轴、偏角，椭圆拟合公式如下：

f＝∑x_i ²+Ax_iy_i+By_i ²+Cx_i+Dy_i+E

其中，(x_i，y_i)为拟合椭圆的点，A、B、C、D、E为拟合椭圆的非标准方程的系数；

利用直线拟合公式拟合直线获得长度、起始点坐标、终止点坐标、中心点坐标，直线拟合公式如下：

其中，(x_i,y_i)是拟合直线的轨迹点，a为拟合直线的斜率，b为拟合直线的截距,R²为轨迹点与拟合直线的距离差的和；

利用现有矩形拟合函数获得矩形的中心点、长、宽、四个角点坐标；接下来根据行人和矿车的不同确定代表位置的点，在确定位置点时，用车前端和靠近雷达一侧的边组成的直角点代表车辆的位置。代表行人的椭圆，并以椭圆的中心点作为行人的位置点。

本发明还提供了一种基于稀疏激光雷达数据的动态障碍物追踪系统，包括：

消除模块，用于确定可行驶区域，删除可行驶区域的地面点云和超过一定高度的点云；

聚类模块，用于对障碍物点云进行聚类，将属于不同动态障碍物的点云分为不同的簇；

凸包提取模块，用于在聚类后提取聚类点云的凸包；

特征提取模块，用于利用凸包的角度信息进行模糊分类，再对这些点云拟合获得障碍物位置信息；

数据关联模块，用于根据不同帧中障碍物的位置信息关联属于同一障碍物的不同时刻的位置，并拟合为轨迹，将已经追踪到的轨迹与当前数据帧检测到的聚类点云关联，更新动态障碍物的运动轨迹；

轨迹预测模块，用于利用轨迹函数预测动态障碍物的位置；

控制模块，用于控制其余模块执行指令；

所述消除模块、聚类模块、凸包提取模块、特征提取模块、数据关联模块、轨迹预测模块均与控制模块电连接。

本发明相比现有技术具有以下优点：该基于稀疏激光雷达数据的动态障碍物追踪方法及系统，使用栅格地图过滤静态障碍物激光雷达点云，剩余少量静态点云通过dbscan算法，将极为稀疏的点云过滤，增强了最终的过滤效果；通过提取点云凸包的角度信息模糊分类点云为椭圆、矩形、直线，然后辅助点云拟合图形的尺寸获得正确的点云位置点，保证了数据关联的准确性；综合了最近邻域和多目标假设算法的特性，改进了多目标关联算法，能够在保证准确性的前提下高效地完成数据关联工作。

附图说明

图1是本发明实施例中基于稀疏激光雷达数据的动态障碍物追踪方法的总体流程图；

图2(a)是本发明实施例中的栅格地图；图2(b)是本发明实施例中的原始点云图；图2(c)是本发明实施例中经过实际数据采集和背景消除后的效果图；

图3是本发明实施例中点云经过聚类后的效果图；

图4(a)是本发明实施例中模糊分类为椭圆的点云图；图4(b)是本发明实施例中模糊分类为直线的点云图；图4(c)是本发明实施例中模糊分类为矩形的点云图；

图5是本发明实施例中的凸包提取效果图；

图6(a)是本发明实施例中车头和靠近雷达一侧的点云组成的矩形边界图；图6(b)是本发明实施例中车靠近雷达一侧点云组成的直线边界图；图6(c)是本发明实施例中车尾和靠近雷达一侧的点云组成的矩形边界图；

图7是本发明实施例中推算车前端直角点的过程示意图；

图8是本发明实施例中轨迹更新匹配流程示意图；

图9(a)是本发明实施例中直线拟合的轨迹图；图9(b)是本发明实施例中多项式拟合轨迹图。

具体实施方式

下面对本发明的实施例作详细说明，本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

本实施例提供一种技术方案：一种基于稀疏激光雷达数据的动态障碍物追踪方法，如图1所示，包括以下步骤：

第一步：消除静态背景

激光雷达采集的数据包括静态障碍物和动态障碍物，对于动态障碍物的追踪，静态障碍物点云是噪声，这一部分点云是要去除的。基于三维雷达进行背景消除的一般过程为：首先确定可行驶区域，然后删除可行驶区域的地面点云和超过一定高度的点云，剩余点云即为障碍物点云。这个过程能删除地面点云、过高的环境点云但是仍有部分静态点云保留。二维雷达的数据量少，为了达到理想的效果要尽量减少噪声。

本实施例采取的具体处理过程如下：

1)确定采集平面，二维雷达只能采集一个水平面的信息，确定水平面时，必须考虑主要障碍物的高度，查阅相关资料可知通用矿井移动设备高度为0.8米～2.4米，行人腿长一般0.8米～1.1米，因此设置采集平面在0.8米～1.0米。

2)生成模板，利用gmapping的建图功能包构建栅格地图或者转换已经存在矿井地图为栅格地图，栅格地图分为障碍层、膨胀层、可行驶层。

3)预处理，加载已经建好的栅格地图，当动态障碍物距离静态障碍物大于0.1米时，能准确的过滤静态点云；当动态障碍物距离静态障碍物0～0.1米，由于膨胀层的原因会过滤部分动态点云，因此通过缩小膨胀层的尺寸减小这里出现的误删除，本实施例中设置膨胀层厚度为0.05米，接收激光雷达的原始数据。

因为激光雷达采集的数据坐标系称为“laser”与静态地图坐标系称为“map”是不同的，所以之后转换激光雷达数据到地图坐标系。然后处理点云数据，删除点云中非法值例如inf和无效值例如(0，0)。

4)过滤点云，用点坐标和栅格地图做比较判断是否处于可行驶层，如果在可行驶层则保留否则删除此点；因为激光雷达数据的不稳定，障碍物的点云会有小尺寸的浮动所以对于处于膨胀层的点云也认为是静态障碍物。

已经建好的栅格地图如图2(a)所示，原始点云如图2(b)所示，经过实际数据采集和背景消除后，效果图如图2(c)所示，通过背景消除，过滤了大部分静态点云。即使有小部分静态点云还留存，经过处理后留存的静态点云分布过于稀疏，通过接下来的聚类算法可以完全过滤。消除了静态点云的干扰，进一步减少数据量，接下来的算法处理会更迅速；并且没有静态点云干扰，障碍物的追踪会更准确。

第二步：动态点云聚类

动态点云包含多个动态障碍物的数据，并且点云只有坐标信息。为了实现动态障碍物的追踪，必须将属于同一障碍物的点云分为一类。这样，可以通过提取一类点云的综合特征作为动态障碍物的特征。比较常用的两种聚类算法DBSCAN与K-means。k-means需要指定聚类簇数k，并且初始聚类中心对聚类影响很大。k-means把任何点都归到了某一个类，对异常点比较敏感。DBSCAN能剔除噪声，需要指定邻域距离阈值eps和样本个数阈值MinPts，可以自动确定簇个数。因为动态障碍物的数量是不确定的并且属于同一障碍物的点云与属于不同障碍物的点云之间有明显差异，所以本实施例选用DBSCAN算法实现聚类。

DBSCAN是一种基于密度的聚类算法，这类密度聚类算法一般假定类别通过样本分布的紧密程度决定。同一类别的样本，点云之间的距离较近，即在该类别任意样本周围不远处一定有同类别的样本存在，聚类算法将点云分为核心点和边界点，其中核心点为聚类后的骨架。该算法有两个核心参数Eps、MinPts。Eps是点云之间的邻域的边界公式，如下所示：

Ne＝{x_i∈D|distance(x_i,x_j)＜Eps} (1)

两点之间的距离小于Eps，则认为在同一邻域内。另外，MinPts是判断核心点的参数，x_i代表点云中的点，distance(x_i,y_i)为点云之间的欧式距离,Eps为聚类算法归为同一类点云的距离阈值，D为范围领域。数学中领域定义为，∈邻域：给定对象半径∈内的区域称为该对象的∈邻域。

聚类工作过程如下：首先，统计每个点邻域内点的个数，存入pts属性中，然后，统计每个点pts属性判断是否大于MinPts，如果大于则是核心点，并存入核心点数组中；其次，深度遍历核心点数组将在当前核心点邻域范围内的核心点串联为一类；最后，遍历所有边界点，将核心点邻域内的边界点并入核心点类。经过聚类后的效果图如图3所示，视场内有三个障碍物，两个行人障碍物和一个类车障碍物如图3所示。

结果显示，经过DNSCAN算法聚类后能将属于不同动态障碍物的点云分为不同的簇。并且对于没有完全过滤的静态障碍物点云有过滤作用，这类点云数量少并且分布不集中不会被分类。聚类之后删除没有分类的点云将这部分点云过滤。

第三步：凸包提取

点云聚类之后，将每个类作为一个检测到的移动物体，接下来要将属于不同帧的移动物体数据作关联从而确定移动物体的轨迹。在此之前，要有每个类的特征信息。点集Q的凸包(convex hull)是指一个最小凸多边形，满足Q中的点或者在多边形边上或者在其内。图5中由线段表示的多边形就是点集Q＝{p0,p1,...p12}的凸包。聚类点云的轮廓形状是分类的主要特征，使用graham扫描算法获得聚类点云的凸包。算法基于叉积的性质，叉积的长度|a×b|可以解释成这两个叉乘向量a，b共起点时，所构成平行四边形的面积。

二维向量的叉积公式如下：

如果p1Xp2小于零，则p1至p2是逆时针方向，如果p1Xp2大于零，则p1至p2是顺时针方向。

经过graham算法提取的凸包，以多边形的形式包络同一组的所有点云。凸包反映了动态障碍物的外轮廓信息，接下来提取动态障碍物的几何特征。

第四步：特征提取

实现动态障碍物的追踪最关键的是将障碍物不同时刻的位置关联为一组。本实施例根据对二维点云的观察提出了一种关联数据的方式。关联数据的主要依据是动态障碍物的几何特征。对数据的处理分为两步：首先，根据提取的凸包的角度信息对障碍物模糊分类为椭圆形、矩形、直线；然后，对不同种类的点云拟合，提取感兴趣位置点、几何特征。

矿井环境下主要的移动物体是行人和矿车，行人近似于椭圆，而矿车从不同的角度观察可以分类为矩形点云和直线点云。任意点云可以归类为这三种几何形状。根据这一事实设计算法将聚类后的点云分为‘e’(椭圆)、‘r’(矩形)、‘l’(直线)三种类型。

雷达获得的数据只能是移动物体的一个方向的点云如图4所示，一个方向的点云只反映障碍物的部分几何特征，其最突出的几何特征是夹角信息。对于行人，各个方向的点云都近似于圆弧，而车辆可以归纳为矩形。本实施例根据夹角的信息对点云模糊分类如图4所示。将类似4(a)的点云分为椭圆，类似4(b)的点云分为直线，将类似4(c)的点云分为矩形。

点云的夹角信息是通过凸包获取的，如图5所示。

经过graham算法计算后，获得点云的一组凸包点。根据对实际车辆、行人的雷达数据观察发现。距离激光雷达最近的点通常是角的顶点。统计以最近点为顶点的角度信息，以此作为点云的角度特征。具体处理过程如下：

1)将所获得凸包数组中距离激光雷达最近的点作为顶点。

2)以凸包数组中左下角的点作为起始点顺时针开始作为角一边的端点，逆时针开始以凸包点作为另一边的端点利用公式(3)计算角度，同时统计角度最大值并求和，在遍历所有凸包点过程中，当端点距离顶点0.05米时认为该角度无效。总结大量角度运算结果，角度对聚类点云分类范围，如表1所示：

表1角度分类标准表

3)、通过对行人和类车障碍物的雷达点云角度特征提取后的角度统计，获得表1所示的分类标准。首先以角度平均值作为依据判断，如果处于0.7～3.0的区间内再以最大角度为依据，经过实验证明该方法正确率在90％左右。

4)类型确定以后，可以确定点云数据是来源车还是行人，根据车和行人的不同行驶特征对点云进一步处理。

使用不同的拟合公式对不同类型的图形点云拟合，从而推算出其集合特征。利用公式(3)拟合椭圆获得椭圆的圆心、长轴、短轴、偏角。利用公式(4)拟合直线获得长度、起始点坐标、终止点坐标、中心点坐标。利用opencv提供的矩形拟合函数获得矩形的中心点、长、宽、四个角点坐标，接下来根据行人和车辆的不同确定代表位置的点。

f＝∑((x_i-x_c)²+(y_i+y_c)²-R²)² (3)

对车辆的处理：如图6所示，假设一辆矿车驶来，对应的激光雷达数据会经历三个状态的变化，第一种情况如图6(a)所示，车头和靠近雷达一侧的点云组成的矩形边界，第二种情况如图6(b)所示，车靠近雷达一侧点云组成的直线边界，第三种情况如图6(c)所示，车尾和靠近雷达一侧的点云组成的矩形边界。

本实施例选用车前端和靠近雷达一侧的边组成的直角点代表车辆的位置，因为在这个过程中车最能反应车轮廓信息的点是车前端的直角点a和车后端的直角点b。这两个点分别反映了车的最前端和最后端，并共同反映了车的侧边。然后，采集到车辆的直线状态后可以获得车长度len，根据这个长度可以根据b点位置推算得到a点的位置，即三种情况下都可以获得前端直角点。推算车前端直角点的过程如图7所示。包括以下步骤：

1)确定车辆的行驶方向，根据车当前位置的矩形中心点位置和上一时刻矩形中心点位置，做向量差。得到车辆行驶方向的矢量A。如果此时为直线状态则分别用直线的起始点和终止点做差作为矢量A；

2)确定车辆行驶状态，用当前拟合矩形中心点与采集的直角点做差，获得矢量B；

3)计算矢量A和矢量B的夹角angle，如果大于90度则认为矩形中心点在直角点前是第三种状态。如果小于90则认为矩形中点在直角点后是第一种状态；

4)第三种状态下，利用公式(5)、(6)计算得直角点a。

Xa＝Xb+len*cos(arctan(angle)) (5)

Xa＝Xb+len*sin(arctan(angle)) (6)

对行人的处理：本实施例以椭圆的圆心代表行人的位置。因为行人的几何特征近似于椭圆，而且单个行人的占地面积在1平方米以内，代表行人的椭圆尺寸较小，椭圆的圆心和轮廓距离较近，所以以椭圆的圆心代表行人障碍物位置是可行的。

经过特征提取获得了动态障碍物类型信息和误差允许范围内的位置信息，接下来根据获得的可靠信息进行数据关联。

第五步：数据关联

经过上述运算后，将原始点云数据转化为特征图形如表2所示。数据关联是将已经追踪到的轨迹与当前数据帧检测到的聚类点云关联，从而更新动态障碍物的运动轨迹。本发明结合最近邻域(NN)与多目标假设跟踪(MHT)算法。提出一种以权重距离为关联依据的多目标关联方式。

表2点云特征信息表

常用的数据关联算法有最近邻领域算法、多目标假设跟踪算法。综合这两种算法的特点以权重距离为关联依据的多目标关联方式称为权重距离算法。权重距离算法根据点云多方面的量化特性并根据这些特性在数据关联中的重要程度赋予不同的权重。这样既综合考虑了障碍物点云的特征又简化了概率计算时间。

障碍物跟踪点之间的距离；障碍物的宽度和长度；障碍物矩形边界与x坐标轴的夹角；障碍物矩形边界与x坐标轴的夹角。对于同一障碍物，点云拟合的几何尺寸在不同方向不同距离下是会变化的，也就是说尺寸因素会增大误差。同理，对于二维雷达稀疏点云角度信息也不够稳定。最终点云权重以点云的模糊分类类型和修正后的位置作为关联依据。

权重公式如下：Weight＝coef[0]*W[0]+coef[1]-W[1] (7)

权重公式(7)中的Coef[]数组是各个权重的系数，W[0]是类型权重，如果同一类型值＝1，不同类型值＝-1；W[2]是距离权重，轨迹预测的点与当前帧点云的距离，因为距离越近，关联程度越高，权重越高。所以距离权重与最终的权重是负相关的。在本发明实施例中用(系数-距离)作为权重距离。

参考多目标关联算法的关联策略，分情况讨论轨迹更新匹配流程如图8所示。

匹配过程主要考虑以下三种情况：

第一种情况，新出现的障碍物，如果障碍物与所有轨迹的关联权重都小于成功关联的界限值wei_limit，则认为是新出现的障碍物。新建一条轨迹。因为只有一个位置点无法预测轨迹，定义初始半径pre_radius画圆如图8所示，下一时刻以点云位置与圆轨迹的距离作为距离依据。

第二种情况。障碍物与轨迹匹配，如果障碍物与现有轨迹中存在大于成功关联界限值wei_limit并且是最大的关联值，用障碍物提供的信息更新轨迹的信息。因为存在模糊分类错误的情况。轨迹中可能包含不同的点云类型。更新时统计轨迹中不同类型点云的数量，轨迹类型为数量最多的类型。更新轨迹类型数量、轨迹尺寸、轨迹位置。依据卡尔曼滤波的思想轨迹位置、尺寸有当前障碍物信息和预测轨迹点信息权重相加得到的。

第三种情况，障碍物消失，有一些轨迹匹配不到障碍物，删除这些轨迹。

第六步：轨迹预测

特征图形聚类完成后，将特征图形的中心坐标保存作为轨迹点。为了更准确描述和预测运动轨迹首选使用最小二乘法多项式曲线拟合每一条轨迹，公式如下：

A*X＝Y (8)

带入一条轨迹点集合，求使公式(8)值最小的A参数向量。使用上述公式(9)多项式曲线最小二乘法求轨迹必须要足够数量(大于等于4)的轨迹点，当采集的轨迹点数量少于4个则使用公式(10)的最小二乘法拟合直线的方法作为预测轨迹。

轨迹的预测本实施分为直线预测和多项式曲线预测，基本流程是首先根据推导的拟合函数，适当增加最后一个轨迹点的x值的x_p，带入公式求得y_p坐标，(x_p,y_p)即为预测轨迹点。

x_p＝x[n]+(len)/(t*n) (11)

xp＝x[n]+len/(t*n)*k (12)

公式(11)是直线预测x_p的计算公式，X[n]是当前轨迹点，len为已有轨迹点的x坐标的增量绝对值的和，t为可调参数，n为已有轨迹点个数。本实施例根据已有轨迹的x变换程度的平均值估计当前时刻可能的x变换程度。公式(12)是多项式曲线预测xp的计算公式，X[n]、n、t和直线预测中的含义对应相同，len为已有轨迹点的欧式距离的和，k为当前轨迹的斜率。曲线预测以已有轨迹的平局欧式距离估计当前时刻可能的x变换程度，其变换方向为切线方向。轨迹预测效果图如图9所示，其中，图9(a)为直线拟合的轨迹图，图9(b)为多项式拟合轨迹图。

为验证本发明算法的可行性，首先使用gmaping建图算法，构建了实验室的栅格地图，然后算法截获原本直接传输到导航模块的激光雷达数据，并进行数据处理生成障碍物轨迹预测点云。通过ros系统框架将生成的点云传输给导航模块。再接收算法产生的轨迹点云辅助下进行导航，导航结果：对比未经算法处理的情况，本算法产生的轨迹点云能有效的辅助机器人的避障导航功能。

综上所述，上述两组实施例的基于稀疏激光雷达数据的动态障碍物追踪方法，使用栅格地图过滤静态障碍物激光雷达点云，剩余少量静态点云通过dbscan算法，将极为稀疏的点云过滤，增强了最终的过滤效果；通过提取点云凸包的角度信息模糊分类点云为椭圆、矩形、直线，然后辅助点云拟合图形的尺寸获得正确的点云位置点，保证了数据关联的准确性；综合了最近邻域和多目标假设算法的特性，改进了多目标关联算法，能够在保证准确性的前提下高效地完成数据关联工作，值得被推广使用。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于，包括以下步骤：

S1：消除静态背景

确定可行驶区域，删除可行驶区域的地面点云和超过指定高度的点云，剩余点云即为障碍物点云；

S2：动态点云聚类

对步骤S1中的障碍物点云进行聚类，将属于不同动态障碍物的点云分为不同的簇；

S3：凸包提取

经过步骤S2的聚类后，提取聚类点云的凸包；

S4：特征提取

利用凸包的角度信息进行模糊分类，再对这些点云拟合获得障碍物位置信息；

S5：数据关联

根据不同帧中障碍物的位置信息关联属于同一障碍物的不同时刻的位置，并拟合为轨迹，将已经追踪到的轨迹与当前数据帧检测到的聚类点云关联，更新动态障碍物的运动轨迹；

S6：预测轨迹

利用轨迹函数预测动态障碍物的位置：使用最小二乘法拟合轨迹点得到轨迹函数，根据轨迹函数在最后一个轨迹点的横坐标基础增加适当增量x得到x_p，带入公式求得y_p坐标，(x_p,y_p)即为预测轨迹点，直线预测时带入的最小二乘法公式如下：

其中，(x_i,y_i)是拟合直线的轨迹点，a₁是拟合直线的斜率，a₀是拟合直线的截距,R²为轨迹点与拟合直线的距离差的和，多项式曲线预测时带入的公式如下：

其中，(x_i,y_i)是拟合三次多项式的轨迹点，a₀、a₁、a₂、a₃为拟合三次多项式的系数,R²为轨迹点与拟合直线的距离差的和；

轨迹函数包括直线预测和多项式曲线预测函数，直线预测函数如下：

x_p＝x[n]+(len)/(t*n)

其中，X[n]是当前轨迹点，len为已有轨迹点的x坐标的增量绝对值的和，t为可调参数，n为已有轨迹点个数，在直线预测中根据已有轨迹点之间的x增量的平均值估计当前时刻的x增量；其多项式曲线预测函数如下：

xp＝x[n]+len/(t*n)*k

其中，X[n]是当前轨迹点，t为可调参数，n为已有轨迹点个数，len为已有轨迹点的欧式距离的和，k为当前轨迹的斜率，在多项式曲线预测中根据已有轨迹点之间的x增量的平均值估计当前时刻的x增量。

2.根据权利要求1所述的一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于：点云只包含坐标信息，坐标形式为二维坐标(x,y)。

3.根据权利要求1所述的一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于，所述步骤S1中的具体处理过程包括以下步骤：

S11：确定采集平面

根据主要障碍物的高度设置采集平面的高度为0.8米～1.0米；

S12：生成模板

构建栅格地图或者转换已经存在矿井地图为栅格地图，栅格地图分为障碍层、膨胀层、可行驶层；

S13：预处理

加载步骤S12中已经建好的栅格地图，设置膨胀层厚度为0.05米，接收激光雷达的原始数据，然后处理点云，删除点云中的非法值；

S14：过滤点云

用点坐标和栅格地图做比较判断是否处于可行驶层，如果在可行驶层则保留否则删除此点。

4.根据权利要求1所述的一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于，在所述步骤S2中，聚类过程中将点云分为核心点和边界点，其中核心点为聚类后的骨架，聚类过程中涉及两个核心参数，分别为Eps、MinPts，Eps是点云之间的邻域的边界公式，如下所示：

Ne＝{x_i∈D|distance(x_i,x_j)＜Eps}

其中，x_i代表点云中的点，distance(x_i,y_i)为点云之间的欧式距离,Eps为聚类算法归为同一类点云的距离阈值，D为范围领域；

当两点之间的距离小于Eps，则认为两点属于同类点云；MinPts是判断核心点的参数，如果点的邻域内距离小于Eps的点个数大于MinPts则判断其为核心点。

5.根据权利要求4所述的一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于，聚类过程包括以下步骤：

S21：统计每个点邻域内点的个数，存入pts属性中，判断每个点pts属性判断是否大于MinPts，如果大于则是核心点，并存入核心点数组中；

S22：遍历核心点数组将在当前核心点邻域范围内的核心点串联为一类；

S23：遍历所有边界点，将核心点邻域内的边界点并入核心点类中。

6.根据权利要求1所述的一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于：在所述步骤S4中，矿井环境下的移动物体是行人和矿车，行人分类为椭圆点云，矿车从不同的角度观察可以分类为矩形点云和直线点云，根据上述移动物体的特点利用凸包的角度信息模糊分类为椭圆形、矩形、直线。

7.根据权利要求6所述的一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于：在所述步骤S4中，点云的夹角信息是通过凸包获取的，获得点云的一组凸包点后，选择其中距离激光雷达的最近点为角的顶点，统计以最近点为顶点的角度信息，以此作为点云的角度特征，即夹角信息。

8.根据权利要求7所述的一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于，点云角度特征的具体处理过程包括以下步骤：

S41：将所获得的凸包数组中距离激光雷达最近的点作为顶点；

S42：以凸包数组中左下角的点作为起始点顺时针开始作为角一边的端点，逆时针开始以凸包点作为另一边的端点计算角度，同时统计角度最大值并求和；

S43：通过对动态障碍物的点云角度特征提取后的角度统计，得到角度分类标准，根据角度分类标准确定角度类型；

S44：根据角度类型判断点云数据的来源，根据不同来源的不同行驶特征使用不同的拟合公式对不同类型的图形点云拟合，进而计算出其集合特征。

9.根据权利要求8所述的一种基于稀疏激光雷达数据的动态障碍物追踪方法，其特征在于，在所述步骤S44中，利用椭圆拟合公式拟合椭圆获得椭圆的圆心、长轴、短轴、偏角，椭圆拟合公式如下：

f＝∑x_i ²+Ax_iy_i+By_i ²+Cx_i+Dy_i+E

其中，(x_i，y_i)为拟合椭圆的点，A、B、C、D、E为拟合椭圆的非标准方程的系数；

利用直线拟合公式拟合直线获得长度、起始点坐标、终止点坐标、中心点坐标，直线拟合公式如下：

其中，(x_i,y_i)是拟合直线的轨迹点，a拟合直线的斜率，b拟合直线的截距,R²为轨迹点与拟合直线的距离差的和；

利用现有矩形拟合函数获得矩形的中心点、长、宽、四个角点坐标；接下来根据行人和矿车的不同确定代表位置的点，在确定位置点时，用车前端和靠近雷达一侧的边组成的直角点代表车辆的位置，代表行人的椭圆，并以椭圆的中心点作为行人的位置点。

10.一种基于稀疏激光雷达数据的动态障碍物追踪系统，其特征在于，利用如权利要求1～9任一所述的追踪方法进行动态障碍物的追踪工作，包括：

消除模块，用于确定可行驶区域，删除可行驶区域的地面点云和超过一定高度的点云；

聚类模块，用于对障碍物点云进行聚类，将属于不同动态障碍物的点云分为不同的簇；

凸包提取模块，用于在聚类后提取聚类点云的凸包；

特征提取模块，用于利用凸包的角度信息进行模糊分类，再对这些点云拟合获得障碍物位置信息；

数据关联模块，用于根据不同帧中障碍物的位置信息关联属于同一障碍物的不同时刻的位置，并拟合为轨迹，将已经追踪到的轨迹与当前数据帧检测到的聚类点云关联，更新动态障碍物的运动轨迹；

轨迹预测模块，用于利用轨迹函数预测动态障碍物的位置；

控制模块，用于控制其余模块执行指令；

所述消除模块、聚类模块、凸包提取模块、特征提取模块、数据关联模块、轨迹预测模块均与控制模块电连接。

Images