本发明涉及激光雷达检测技术领域，具体为一种三维点云数据的目标检测与跟踪方法及其装置。
背景技术：
近年来我国汽车工业一直保持着平稳的发展趋势，无人驾驶技术代表着整个产业的最新发展方向，此项技术的目的是全面提升汽车驾驶的安全性，舒适性及稳定性，满足更高的市场需求。
随着经济社会的不断发展，城市化步伐的不断加速，城市道路的交通状况显得越来越复杂，在无人驾驶领域，实时的行人、车辆检测有着重要的意义。如：可以通过检测道路两侧行人、车辆的行驶状态，从而合理安排控制决策系统进行规避与超车。总之，实时并能快速地进行目标检测与跟踪在无人驾驶技术中的应用非常重要，因此，本发明的研究就显得意义重大。
根据数据来源的不同，目标检测与跟踪主要分为基于图像的方法和基于雷达数据的方法。然而基于序列图像的目标检测与跟踪存在的突出问题就是图像处理的数据量大，算法的实时性差,而且难以获得目标的深度信息。
为了改善上述问题，近年来，激光雷达因具有数据量小、测距精度高、实时性能好等优点，在目标检测与跟踪领域已经得到了广泛应用。为此，我们提出一种三维点云数据的目标检测与跟踪方法。
技术实现要素：
本发明的目的在于提供一种三维点云数据的目标检测与跟踪方法及其装置，以解决上述背景技术中提出的问题。
为实现上述目的，本发明提供如下技术方案：
一种三维点云数据的目标检测与跟踪方法，包括如下步骤：
步骤1、从激光雷达设备中读取原始点云数据，对获取的原始点云数据进行地平面点的分割提取，得到地面点pg和非地面点png；
步骤2、对非地面点聚类处理，产生各自独立的目标点云簇；
步骤3、通过点云簇中计算，获取聚类生成的目标数据；
步骤4、通过对生成的目标数据进行判别，获得运动目标，以及目标的速度信息；
步骤5、基于匀速直线运动模型的运动估计，得到先验估计；
步骤6、对目标的先验估计和实际观测进行状态更新，获得更新后的状态为当前实时获得的目标数据，即为三维点云目标匹配。
优选的，地平面点的分割提取采用确定迭代次数的多平面拟合方法，步骤如下：
步骤1.1、对于平面的估计，采用线性模型：ax by cz d＝0，使用最小二乘法拟合平面；
步骤1.2、对初始的点云数据，选取h＝0的点作为初始的地面点pg；
步骤1.3、获取地平面的线性模型ground＝estimateplane(pg)；
步骤1.4、遍历输入点云p，对于其中的点pk，计算距离地平面ground的距离
步骤1.5、设立地平面的阈值距离thdist，判断hdist>thdist是否成立，若成立，将pk归入非地面点png；否则，将pk归入地面点pg；
步骤1.6、设立迭代次数niter，进行从步骤1.3到步骤1.5的循环计算，直至达到迭代次数niter；
由所述方法组成将输入点云p分类为地面点pg和非地面点png。
优选的，非地面点的聚类处理步骤如下：
步骤2.1、点云png中，从顶部扫描线开始以逆时针方式遍历，形成扫描线的runs并且每个扫描线都分配自己的新标签；
步骤2.2、对于每一个扫描线中的点，如果与后接点的间距d大于合并阈值thmerge，则后接点分配新的标签label，形成新的runs；
步骤2.3、然后，第一条扫描线的runs成为runsabove，用于将其标签label传播到后续扫描线中的runs，当新runs的点与上述扫描线中的最近邻点之间的距离d小于合并阈值thmerge时，runsabove的标签label被传播到新的runs，当同一runs中的点具有多个不同的可继承标签label的最近邻点时，其获得的标签label是其中数值最小的；
步骤2.4、当runs中的点找不到合适的最近含有标签label的邻点时，它会成为新的runsabove；
步骤2.5、执行第二次runsabove传递以最终更新点标签label。
优选的，获取聚类生成的目标数据为：
每一个标签label相同的点，即为获取到的目标点云簇，以激光雷达的自身位置作为原点，对每帧点云建立相同的三维直角坐标系，对于每一个点云簇中的点
优选的，运动目标的判别步骤如下：
步骤4.1、对于当前帧中经过聚类生成的目标数据，为每一个目标赋予唯一的id值；
步骤4.2、将前后两帧的目标数据进行比较，
步骤4.3、如果同一id的目标存在3帧以上的目标数据，则1可以确定该目标为运动目标，激光雷达扫描频率为10hz，所以相邻帧的点云数据时间间隔为0.1s，；利用目标数据的位置信息，获取目标的速度信息vx和vy。
优选的，基于匀速直线运动模型的运动估计步骤如下：
步骤5.1、建立状态向量xk＝[x,y,l,m,n,vx,vy]t，x和y表示运动目标中心点的x坐标和y坐标，vx和vy表示目标在x方向和y方向的速度，l，m，n表示运动目标外接长方体的长，宽，高，在实验场景中目标不会在z方向上发生运动，所以状态向量xk中剔除了z和vz，
步骤5.2、状态预测方程
优选的，三维点云目标匹配步骤如下：
步骤6.1、将前后两帧的目标数据进行比较，
步骤6.2、若符合匹配条件，将该目标的先验估计和实际观测，根据状态更新方程：
为实现上述目的，本发明还提供一种三维点云数据的目标检测与跟踪装置，可以快速、准确的实现目标的检测和分类。
一种三维点云数据的目标检测与跟踪装置，包括：
扫描单元，用于从激光雷达设备中读取原始点云数据，
滤波单元，用于对原始点云数据中的杂波信号滤除；
点云数据处理单元，按如下步骤对滤波后的原始点云数据进行处理：
步骤1、从激光雷达设备中读取原始点云数据，对获取的原始点云数据进行地平面点的分割提取，得到地面点pg和非地面点png；
步骤2、对非地面点聚类处理，产生各自独立的目标点云簇；
步骤3、通过点云簇中计算，获取聚类生成的目标数据；
目标匹配单元，按如下步骤对获取聚类生成的目标数据进行处理：
步骤4、通过对生成的目标数据进行判别，获得运动目标，以及目标的速度信息；
步骤5、基于匀速直线运动模型的运动估计，得到先验估计；
模式识别单元，对目标的先验估计和实际观测进行状态更新，获得更新后的状态为当前实时获得的目标数据，即为三维点云目标匹配。
优选的，滤波单元基于卡尔曼滤波器。
与现有技术相比，本发明的有益效果是：
1、本发明接受具有最低高度值的点云的点最可能属于地面的假设，该先验知识为拟合平面的初始地面点提供了依据；相对于典型的平面拟合技术，例如ransac中的随机选择初始地面点，有着更快的收敛效果；通过计算点在地面的投影高度是否在阈值距离内，以此更新地面点；进行多次迭代拟合地面，提高地面模型的适应性与准确度。
2、本发明利用扫描线来遍历非地面点，通过欧氏距离的阈值划不同分点云簇，逐步实现点之间，扫描线之间的聚类融合；利用扫描线生成的点向量作为点云簇的构成，相对于原有的kd-树索引，加速了在聚类过程中的搜索速度。
3、本发明通过点云序列前后帧中的目标位置信息，计算获取目标的运动速度，同时进行了运动目标的判定；将目标的位置、尺寸与速度作为卡尔曼滤波器的状态向量，建立匀速直线运动的运动模型；同时点云序列的帧间时间间隔微小，目标的运动状态具有连续性，运动估计的准确度可以得到保证。
4、本发明所采用的目标跟踪算法，结合了运动估计和基于目标信息的特征匹配，通过与当前帧的观测值进行比较，提升的目标跟踪的准确性。
附图说明
图1是本发明方法的流程图；
图2是本发明装置的原理框图。
图3是本发明目标检测与跟踪算法的实际效果图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1～3，本发明提供一种技术方案：
一种三维点云数据的目标检测与跟踪方法，是从激光雷达设备中读取原始点云数据，对获取的原始点云数据进行地平面的拟合与分割；利用扫描线遍历非地面点，通过欧式距离的阈值进行聚类；一是进行生成目标的尺寸大小估计，二是检索目标内的扫描线并计算其曲率值，进而通过这两种特征，与先验值进行比较，判断差值是否在阈值范围内，进行目标分类工作。具体的说，是按如下步骤进行：
步骤1、地平面点的分割提取：
提出了一种确定迭代次数的多平面拟合方法，用于快速提取地面点。
激光雷达扫描获取的原始点云数据，可能存在密度过大的问题，我们可以预先进行降采样处理，减少算法的运行时间。
步骤1.1、对于平面的估计，我们使用简单的线性模型：ax by cz＝0，使用最小二乘法拟合平面，此方法的实现函数可以在pcl库中找到；
步骤1.2、对初始的点云数据，选取h＝0的点作为初始的地面点pg，获取地平面的线性模型ground＝estimateplane(pg)；
步骤1.3、遍历输入点云p，对于其中的点pk，计算距离地平面ground的距离
步骤1.4、设立地平面的阈值距离thdist，判断hdist>thdist是否成立，若成立，将pk归入非地面点png；否则，将pk归入地面点pg；
步骤1.5、设立迭代次数niter，进行从步骤1.2到步骤1.4的循环计算，直至达到迭代次数niter；
由所述方法组成将输入点云p分类为地面点pg和非地面点png。
步骤2、非地面点的聚类处理：
不属于地面的剩余点png中，需要进行聚类处理，产生各自独立的目标点云簇。我们的目标是为每个点
通过扫描线来遍历非地面点png，根据扫描线生成runs。vector是c 标准模板库中的内容，中文偶尔译作“容器”，是一个多功能的，能够操作多种数据结构和算法的模板类和函数库。vector之所以被认为是一个容器，是因为它能够像容器一样存放各种类型的对象，简单地说，vector是一个能够存放任意类型的动态数组，能够增加和压缩数据。
步骤2.1、点云png中，从顶部扫描线开始以逆时针方式遍历，形成扫描线的runs并且每个扫描线都分配自己的新标签；
步骤2.2、对于每一个扫描线中的点，如果与后接点的间距d大于合并阈值thmerge，则后接点分配新的标签label，形成新的runs；
步骤2.3、然后，第一条扫描线的runs成为runsabove，用于将其标签label传播到后续扫描线中的runs。当新runs的点与上述扫描线中的最近邻点之间的距离d小于合并阈值thmerge时，runsabove的标签label被传播到新的runs。当同一runs中的点具有多个不同的可继承标签label的最近邻点时，其获得的标签label是其中数值最小的；
步骤2.4、当runs中的点找不到合适的最近含有标签label的邻点时，它会成为新的runsabove；
步骤2.5、执行第二次runsabove传递以最终更新点标签label。
步骤3、获取聚类生成的目标数据：
每一个标签label相同的点，即为获取到的目标点云簇。对于每一个点云簇中的点
通过距离极值，可以给聚类目标生成外界的长方体，近似获取目标的体积大小。设立阈值范围，剔除体积过大或者过小的目标(大面积的墙体或者点云噪声)，减少目标分类算法的运行时间。
步骤4、运动目标的判别：
通过对生成的目标数据进行判别，获得运动目标，以及目标的速度信息。
步骤4.1、对于当前帧中经过聚类生成的目标数据，为每一个目标赋予唯一的id值。
步骤4.2、将前后两帧的目标数据进行比较，
步骤4.3、如果同一id的目标存在3帧以上的目标数据，则1可以确定该目标为运动目标。激光雷达扫描频率为10hz，所以相邻帧的点云数据时间间隔为0.1s，；利用目标数据的位置信息，获取目标的速度信息vx和vy。
步骤5、基于匀速直线运动模型的运动估计：
步骤5.1、建立状态向量xk＝[x,y,l,m,n,vx,vy]t，x和y表示运动目标中心点的x坐标和y坐标，vx和vy表示目标在x方向和y方向的速度，l，m，n表示运动目标外接长方体的长，宽，高。在实验场景中目标不会在z方向上发生运动，所以状态向量xk中剔除了z和vz。
步骤5.2、状态预测方程
步骤6、三维点云目标匹配：
点云序列的相邻两帧之间的时间间隔较小，目标的运动状态不可能发生突变，运动具有连续性。可认为同一运动目标，由上一时刻的状态经过状态预测计算，得到的先验估计，与此时刻的实际观测相比较变化不大。
步骤6.1、将前后两帧的目标数据进行比较，
步骤6.2、若符合匹配条件，将该目标的先验估计和实际观测，根据状态更新方程：
本实施例中，一种三维点云数据的目标检测与跟踪装置，可以快速、准确的实现目标的检测和分类，包括：
扫描单元，用于从激光雷达设备中读取原始点云数据，
滤波单元，用于对原始点云数据中的杂波信号滤除；
点云数据处理单元，按如下步骤对滤波后的原始点云数据进行处理：
步骤1、从激光雷达设备中读取原始点云数据，对获取的原始点云数据进行地平面点的分割提取，得到地面点pg和非地面点png；
步骤2、对非地面点聚类处理，产生各自独立的目标点云簇；
步骤3、通过点云簇中计算，获取聚类生成的目标数据；
目标匹配单元，按如下步骤对获取聚类生成的目标数据进行处理：
步骤4、通过对生成的目标数据进行判别，获得运动目标，以及目标的速度信息；
步骤5、基于匀速直线运动模型的运动估计，得到先验估计；
模式识别单元，对目标的先验估计和实际观测进行状态更新，获得更新后的状态为当前实时获得的目标数据，即为三维点云目标匹配。
本发明的处理方法数据量小、处理速度快，并且目标分类准确率非常高。以期能快速且准确的检测目标并对目标进行跟踪，从而很好地帮助无人驾驶系统进行控制决策。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。
1.一种三维点云数据的目标检测与跟踪方法，其特征在于，包括如下步骤：
步骤1、从激光雷达设备中读取原始点云数据，对获取的原始点云数据进行地平面点的分割提取，得到地面点pg和非地面点png；
步骤2、对非地面点聚类处理，产生各自独立的目标点云簇；
步骤3、通过点云簇中计算，获取聚类生成的目标数据；
步骤4、通过对生成的目标数据进行判别，获得运动目标，以及目标的速度信息；
步骤5、基于匀速直线运动模型的运动估计，得到先验估计；
步骤6、对目标的先验估计和实际观测进行状态更新，获得更新后的状态为当前实时获得的目标数据，即为三维点云目标匹配。
2.根据权利要求1所述的一种三维点云数据的目标检测与跟踪方法，其特征在于，地平面点的分割提取采用确定迭代次数的多平面拟合方法，步骤如下：
步骤1.1、对于平面的估计，采用线性模型：ax by cz d＝0，使用最小二乘法拟合平面；
步骤1.2、对初始的点云数据，选取h＝0的点作为初始的地面点pg；
步骤1.3、获取地平面的线性模型ground＝estimateplane(pg)；
步骤1.4、遍历输入点云p，对于其中的点pk，计算距离地平面ground的距离
步骤1.5、设立地平面的阈值距离thdist，判断hdist>thdist是否成立，若成立，将pk归入非地面点png；否则，将pk归入地面点pg；
步骤1.6、设立迭代次数niter，进行从步骤1.3到步骤1.5的循环计算，直至达到迭代次数niter；
由所述方法组成将输入点云p分类为地面点pg和非地面点png。
3.根据权利要求2所述的一种三维点云数据的目标检测与跟踪方法，其特征在于，非地面点的聚类处理步骤如下：
步骤2.1、点云png中，从顶部扫描线开始以逆时针方式遍历，形成扫描线的runs并且每个扫描线都分配自己的新标签；
步骤2.2、对于每一个扫描线中的点，如果与后接点的间距d大于合并阈值thmerge，则后接点分配新的标签label，形成新的runs；
步骤2.3、然后，第一条扫描线的runs成为runsabove，用于将其标签label传播到后续扫描线中的runs，当新runs的点与上述扫描线中的最近邻点之间的距离d小于合并阈值thmerge时，runsabove的标签label被传播到新的runs，当同一runs中的点具有多个不同的可继承标签label的最近邻点时，其获得的标签label是其中数值最小的；
步骤2.4、当runs中的点找不到合适的最近含有标签label的邻点时，它会成为新的runsabove；
步骤2.5、执行第二次runsabove传递以最终更新点标签label。
4.根据权利要求3所述的一种三维点云数据的目标检测与跟踪方法，其特征在于，获取聚类生成的目标数据为：
每一个标签label相同的点，即为获取到的目标点云簇，以激光雷达的自身位置作为原点，对每帧点云建立相同的三维直角坐标系，对于每一个点云簇中的点
5.根据权利要求1所述的一种三维点云数据的目标检测与跟踪方法，其特征在于，运动目标的判别步骤如下：
步骤4.1、对于当前帧中经过聚类生成的目标数据，为每一个目标赋予唯一的id值；
步骤4.2、将前后两帧的目标数据进行比较，
步骤4.3、如果同一id的目标存在3帧以上的目标数据，则1可以确定该目标为运动目标，激光雷达扫描频率为10hz，所以相邻帧的点云数据时间间隔为0.1s，；利用目标数据的位置信息，获取目标的速度信息vx和vy。
6.根据权利要求1所述的一种三维点云数据的目标检测与跟踪方法，其特征在于，基于匀速直线运动模型的运动估计步骤如下：
步骤5.1、建立状态向量xk＝[x,y,l,m,n,vx,vy]t，x和y表示运动目标中心点的x坐标和y坐标，vx和vy表示目标在x方向和y方向的速度，l，m，n表示运动目标外接长方体的长，宽，高，在实验场景中目标不会在z方向上发生运动，所以状态向量xk中剔除了z和vz，
步骤5.2、状态预测方程
7.根据权利要求6所述的一种三维点云数据的目标检测与跟踪方法，其特征在于，三维点云目标匹配步骤如下：
步骤6.1、将前后两帧的目标数据进行比较，
步骤6.2、若符合匹配条件，将该目标的先验估计和实际观测，根据状态更新方程：
8.一种三维点云数据的目标检测与跟踪装置，其特征在于，包括：
扫描单元，用于从激光雷达设备中读取原始点云数据，
滤波单元，用于对原始点云数据中的杂波信号滤除；
点云数据处理单元，按如下步骤对滤波后的原始点云数据进行处理：
步骤1、从激光雷达设备中读取原始点云数据，对获取的原始点云数据进行地平面点的分割提取，得到地面点pg和非地面点png；
步骤2、对非地面点聚类处理，产生各自独立的目标点云簇；
步骤3、通过点云簇中计算，获取聚类生成的目标数据；
目标匹配单元，按如下步骤对获取聚类生成的目标数据进行处理：
步骤4、通过对生成的目标数据进行判别，获得运动目标，以及目标的速度信息；
步骤5、基于匀速直线运动模型的运动估计，得到先验估计；
模式识别单元，对目标的先验估计和实际观测进行状态更新，获得更新后的状态为当前实时获得的目标数据，即为三维点云目标匹配。
9.根据权利要求8所述的一种三维点云数据的目标检测与跟踪方法，其特征在于，滤波单元基于卡尔曼滤波器。
技术总结