基于激光雷达的连续车辆速度检测方法与流程

本发明涉及到激光雷达测速技术，以及车辆速度检测技术领域。

背景技术：

据世界卫生组织who2010年统计数据显示，每年全球由于道路交通事故死亡的人数约为125万人，有2000至5000万人受伤。道路交通安全问题日益严重，而准确提取道路信息对改善道路交通安全作用日期明显。

车辆作为道路信息的重要组成部分，准确的提取车辆特征，是解决道路交通安全问题的前提。近年来，随着激光雷达技术的发展，为车辆检测提供了新的手段。通过激光雷达3d点云数据，能够获得道路交通三维场景，对交通参与对象进行准确的检测与分类。

当前车辆的检测方法主要有视频检测、环形线圈检测、微波雷达检测、红外检测、磁传感器检测等。其中，视频检测方式在商业上应用广泛，视频检测方式的检测范围大，能够检测各种交通事件等。但是，由于视频检测对光线非常敏感，对于阴影和可见光的突然改变，会导致视频检测误差增大，而且视频检测方式缺乏距离信息。相比之下，激光雷达作为一种主动型传感器，返回的信息受环境干扰小、精度高，易得到物体的深度信息，不受可见光的干扰。这些优点使得激光雷达的应用变得越来越广泛，为解决交通工程相关问题提供了有效手段。随着激光雷达制造技术的发展，其成本正在逐渐减低，为大规模的应用提供了可能。

当前，已经有许多关于道路交通分析的工作开展，其中主要的问题在于道路上运动目标的检测和轨迹追踪。目前，道路轨迹追踪采用不同的传感方式，有不同的检测方法。其中，目前主要采用的有基于视觉的道路车辆检测方法和基于距离的道路车辆检测方法，其中，基于视觉的道路车辆检测方法容易受到可见光的干扰和阴影的干扰，车辆不同的形状、大小和颜色，以及车辆在多车道上不同的运动方向，使得车辆姿态估计变得非常具有挑战性；基于距离的道路车辆检测和轨迹追踪，根据选用的传感器不同，如激光雷达、微波雷达等。

对于激光雷达动态检测场景，采用slammot算法进行车辆追踪，最早的工作可以追溯到2007年。对于连续交通数据采集工作，明尼苏达大学its研究组开发了一个基于激光雷达网络的交通信息采集系统，用来采集交通路口道路信息。使用激光雷达检测进行车辆检测和轨迹追踪，采用2d激光雷达，在一个水平面进行车辆检测和追踪。由于2d激光雷达检测方式限制，在检测角度的水平面以上或者以下的物体信息都无法获得，得不到物体的高度信息，因此不能对车辆以及非车辆的物体进行分类。

目前采用3d激光雷达进行车辆检测，主要集中在机器人领域，研究自动驾驶车辆周边的车辆行为问题。这些研究工作，都是基于64线激光雷达velodynehdl-64elidar，是在一个动态的环境下进行车辆检测，研究的只是自动驾驶车辆周边较小范围内的车辆行为。由于64线激光雷达成本较高，不能大规模进行工程应用。

技术实现要素：

本发明解决了现有采用激光雷达实现车辆速度检测的技术中存在的采用2d激光雷达检测存在不能对车辆以及非车辆的物体进行分类的问题，以及采用成本高64线的3d激光雷成本过高的问题。

本发明所述的基于激光雷达的连续车辆速度检测方法采用16线激光雷达连续采集行车道的路面场景数据，然后根据采集的k帧数据获得当前帧数据中车辆的平均速度，具体过程为：

1)、根据相邻两帧数据计算获得相邻帧数据所有车辆之间的距离矩阵：

其中，第k帧数据中有mk辆车，sij表示第k帧数据中的第i辆车的类中心与第k-1帧数据中的第j辆车的类中心之间的距离，i∈(0,mk]，j∈(0,mk-1]，

其中，表示第k帧数据中第i辆车的类中心的横坐标，表示第k-1帧数据中第j辆车的类中心的横坐标，表示第k帧数据中第i辆车的类中心的纵坐标，表示第k-1帧数据中第j辆车的类中心的纵坐标；

k＝2,3lk，获得多个相邻帧数据所有车辆之间的距离矩阵；

2)、根据上述距离矩阵获得的车辆关联矩阵：

根据获得的所有相邻两帧数据的车辆的距离矩阵获得对应的相邻两帧数据的车辆关联数据，汇总所有相邻两帧数据的车辆关联数据获得车辆关联矩阵为：

其中，akj表示第k-1帧数据中与第k帧数据中第j辆关联成功的车辆类号，j表示k帧数据中含有车辆最多的数据帧中的车辆数，j∈(0,j]；当第k帧数据中的车辆数mk小于j时，则有：均为0；

3)、遍历第k帧数据中所有车辆，获得已检测到的车辆集合p＝{vid＝1,vid＝2,l,vid＝count}，具体过程为：

判断第i辆车对应的aki是否为0，

若aki为0，则判断第i辆车的属性id是否为0，若为0，针对所述第i辆车新建车辆id，id＝count+1，同时更新count＝count+1，并创建vid＝count对象，并将该对象添加入已检测到的车辆集合p＝{vid＝1,vid＝2,l,vid＝count}中；否则，所述第i辆车的属性id不变；

若aki不为0，则将第i辆车与已检测到的车辆集合中的车辆相关联，如果关联成功，则该辆车的id与关联成功的车辆的id相对应，并将其加入关联成功的车辆集合p′中，如果关联失败，则针对所述第i辆车新建车辆id，id＝count+1，同时更新count＝count+1，并创建对象vid＝count，并将该对象添加入已检测到的车辆集合p＝{vid＝1,vid＝2,l,vid＝count}中；

遍历第k帧数据中所有车辆之后，获得已检测的车辆集合p＝{vid＝1,vid＝2,l,vid＝count}，以及关联成功的车辆集合p′＝{v′id＝1,v′id＝2,l,v′id＝count′}，count′≤count；

4)、针对上述关联成功的车辆集合中每辆车计算其车辆的平均速度

其中，kindex′,s表示车辆id为index′的车辆首次出现的数据帧的序号，kindex′,e表示车辆id为index′的车辆最后一次所出现的数据帧的序号，表示车辆id为index′的车辆的特征点在第kindex′,e帧数据中x轴坐标，表示车辆id为index′的车辆的特征点在第kindex′,s帧数据中x轴坐标，表示车辆id为index′的车辆的特征点在第kindex′,e帧数据中y轴坐标，表示车辆id为index′的车辆的特征点在第kindex′,s帧数据中y轴坐标，tindex′,s为第kindex′,s帧数据帧的时间戳，tindex′,e为第kindex′,e帧数据帧的时间戳。

本发明采用16线激光雷达实现了连续车辆速度的检测，进而为实现车辆轨迹追踪提供了有效的手段。

本发明所采用的16线激光雷达成本相对较低，并且具有丰富的3d点云信息。

现有采用相邻帧数据测量车辆速度时，当相邻两帧数据中的车辆数量不相通时，会影响速度测量，例如：在当前帧数据中有m台车辆，而前一帧数据中有n台车辆，则根据m与n的大小关系，有三种情形：

情形1：在当前帧车辆数m小于前一帧车辆数n时，则说明此时有一辆离开检测区域或者由于车辆遮挡等原因而消失。那么此时只有小于等于当前帧车辆数的车辆能够关联成功，前一帧剩下未被当前帧关联的车辆只能通过后续的帧进行关联。

情形2：在当前帧车辆数m等于前一帧车辆数n时，此时两帧车辆最可能都相互关联成功。也可能存在消失车辆的数量与进入车辆数量相等情况，此时只有相邻帧都出现的车辆才会关联成功。

情形3：在当前帧车辆数m大于前一帧车辆数n时，则说明此时有一辆车新进入或者之前消失的车辆出现。那么此时当前帧有车辆会出现关联失败的情况，

基于以上分析，由于车辆遮挡以及检测距离较远，导致车辆会随机的在某一帧数据中丢失的情况，会导致采用相邻帧数据获得车辆关联失败，进而无法实现速度检测。

本发明采用基于多帧数据的车辆关联方法，进而克服了采用相邻帧数据存在的缺陷，能够完全避免由于丢帧对测量的影响，也完全克服了由于车辆相互遮挡对测量的影响。

在交通工程中，车辆的连续速度变化曲线具有重要意义，本发明从激光雷达3d点云数据出发，通过车辆检测与关联，得到了车辆连续速度变化规律，对进一步分析车辆行为具有重要意义。

附图说明

图1是具体实施方式一所述的velodynelidarvlp-16激光雷达检测到的双向四车道的场景。

图2是实施方式中所述的一台车辆的盒子模型(boxmodelofavehicle)。

图3是实施方式中所述的依据盒子模型依据雷达检测获得一辆车的真实数据模型(modelofarealvehicle)。

图4是实施方式中所述的车辆位于激光雷达不同位置所检测到的车辆形态(differentstatusofavehiclebyrelativeposittioncomparedtothelidar)。

图5至8分别是测试实验一所述的针对四个车道中单独车辆样本的速度曲线分析结果示意图。

图9是图5所示的第一车道的车辆在237帧原始图像与栅格滤波之后的结果。

图10是测试实验二中所述的根据激光雷达0°位置找到的车辆真实车尾点算法获得的车辆尾点p的位置示意图。

图11是测试实验二所述的位于第一车道的车辆速度修正后的结果示意图。

图12是测试实验二所述的位于第二车道的车辆速度修正后的结果示意图。

图13是测试实验四所述的，1-2500帧数据表示车辆刚进入和最后离开的检测距离的测试结果，该结果的评价指标2-3参见图14所示。

图15是测试实验四所述的，2500-5456帧数据表示车辆刚进入和最后离开检测距离的测试结果，该结果评价指标2-3参见图16所示。其中，图中的leftfirstdetection雷达左侧首次检测到车辆，rightfirstdetection表示雷达右侧首次检测到车辆。

具体实施方式

实施方式一：本实施方式所述的基于激光雷达的连续车辆速度检测方法采用16线激光雷达连续采集行车道的路面场景数据，然后根据采集的k帧数据获得当前帧数据中车辆的平均速度，具体过程为：

1)、根据相邻两帧数据计算获得相邻帧数据所有车辆之间的距离矩阵：

其中，第k帧数据中有mk辆车，sij表示第k帧数据中的第i辆车的类中心与第k-1帧数据中的第j辆车的类中心之间的距离，i∈(0,mk]，j∈(0,mk-1]，

其中，表示第k帧数据中第i辆车的类中心的横坐标，表示第k-1帧数据中第j辆车的类中心的横坐标，表示第k帧数据中第i辆车的类中心的纵坐标，表示第k-1帧数据中第j辆车的类中心的纵坐标；在相邻帧车辆关联中，一辆车在1帧沿x轴和沿y轴运动的距离都是一个闭合的区间，不可能发生突变；

k＝2,3lk，获得多个相邻帧数据所有车辆之间的距离矩阵；

2)、根据上述距离矩阵获得的车辆关联矩阵：

根据一辆车在相邻数据帧中沿x轴和沿y轴运动的距离都是一个闭合的区间、不可能发生突变的原理，对相邻数据帧中的车辆进行关联，获得相邻两帧数据的车辆关联数据，汇总所有相邻两帧数据中的车辆关联数据形成车辆关联矩阵为：

其中，akj表示第k-1帧数据中与第k帧数据中第j辆关联成功的车辆类号，j表示k帧数据中含有车辆最多的数据帧中的车辆数，j∈(0,j]；当第k帧数据中的车辆数mk小于j时，则有：均为0；

3)、遍历第k帧数据中所有车辆，获得已检测到的车辆集合p＝{vid＝1,vid＝2,l,vid＝count}，具体过程为：

判断第i辆车对应的aki是否为0，

若aki为0，则判断第i辆车的属性id是否为0，若为0，表示该车辆为第一次进入检测区域，针对所述第i辆车新建车辆id，id＝count+1，同时更新count＝count+1，并创建对象vid＝count，并将该对象添加入已检测到的车辆集合p＝{vid＝1,vid＝2,l,vid＝count}中；否则，所述第i辆车的属性id不变；

若aki不为0，则将第i辆车与已检测到的车辆集合中的车辆相关联，如果关联成功，则说明该车辆之前出现在检测区域中，将该辆车的id与关联成功的车辆的id相对应，并将其加入关联成功的车辆集合p′中，如果关联失败，则说明该车是首次进入检测区域，针对所述第i辆车新建车辆id，id＝count+1，同时更新count＝count+1，并创建对象vid＝count，并将该对象添加入已检测到的车辆集合p＝{vid＝1,vid＝2,l,vid＝count}中；

遍历第k帧数据中所有车辆之后，获得已检测的车辆集合p＝{vid＝1,vid＝2,l,vid＝count}，以及关联成功的车辆集合p′＝{v′id＝1,v′id＝2,l,v′id＝count′}，count′≤count；

4)、针对上述关联成功的车辆集合中每辆车计算其当前时刻的平均速度：

其中，kindex′,s表示车辆id为index′的车辆首次出现的数据帧的序号，kindex′,e表示车辆id为index′的车辆最后一次所出现的数据帧的序号，表示车辆id为index′的车辆的特征点在第kindex′,e帧数据中x轴坐标，表示车辆id为index′的车辆的特征点在第kindex′,s帧数据中x轴坐标，表示车辆id为index′的车辆的特征点在第kindex′,e帧数据中y轴坐标，表示车辆id为index′的车辆的特征点在第kindex′,s帧数据中y轴坐标，tindex′,s为第kindex′,s帧数据帧的时间戳，tindex′,e为第kindex′,e帧数据帧的时间戳。在激光雷达检测车辆背景下，由于同一辆车在不同数据帧聚类的类号不同，所以必须要对车辆进行关联，这样才能得到同一辆车在不同数据帧的速度。为了无干扰的进行交通车辆检测，激光雷达被安放在路边。由于激光的直线传播，远车道的车辆存在1帧或者多帧被近车道车辆遮挡的情况。

在这样的检测方式下，车辆关联需要解决以下问题：

1)由于车辆完全遮挡，导致原车道上的车辆完全消失，造成相邻两帧关联失败；

2)由于车辆部分遮挡，导致原车道所检测到的车辆形态不规则，使得车辆几何特征不统一；

3)小型车行驶在较远车道，激光雷达检测到有效点较少，造成相应车辆外观形状跟真实车辆外观差距较大。

针对上述现实问题的存在，本实施方式采取基于相邻帧关联为基础的多帧车辆关联方法，完全解决了上述三个问题。

上述步骤2)中所述的获得相邻两帧数据的车辆关联数据的方法为：

根据步骤1)获得的距离矩阵，依次选取每行的最小距离sia，该最小距离为第k-1帧数据中的第a辆车与第k帧数据中的第i辆车之间的距离，然后进一步判断下述两个条件：

条件1：sia＜＝tmax_xy_move，其中tmax_xy_move是一辆车在相邻帧之间能够行驶的最大距离，该距离决定于该路段限定的最大速度；

条件2：yo(i)-yo(a)＜＝tmax_y_move，tmax_y_move表示一辆车在相邻帧之间的纵轴最大偏移量，该偏移量表示车辆与行驶方向的偏移量，实际上代表了该车的车道信息，选择tmax_y_move参数的优点：可以防止其他车道的车辆匹配到当前车道，也可以防止变道的车辆关联失败；

如果同时满足上述两个条件，认为关联成功。

上述步骤3)中，将第i辆车与已检测到的车辆集合中的车辆相关联的方法为：

判断条件1：被关联的车辆是否位于待关联车辆所在车道或者其附近，即，当满足条件

时，则认为被关联车辆在待关联车辆所在车道或者附近，其中，yo,ki表示待关联的车辆的类中心纵坐标，所述待关联车辆为第k帧数据中的第i辆车；为被关联的车辆在其最后一次所出现的数据帧中的类中心纵坐标，所述被关联车辆为已检测到的车辆集合中车辆id为index的车辆，tmax_y_move表示示一辆车在相邻帧之间的纵轴最大偏移量；

判断条件2：被关联的车辆是否在待关联车辆出现的数据帧之前较近的数据帧中出现，当满足条件

时，则认为被关联的车辆在待关联车辆出现的数据帧之前较近的数据帧中出现了，其中，kki为待关车辆所在数据帧的序号，表示被关联车辆最后一次所出现在的数据帧的序号，tmax_frame_lose表示允许的最大丢失帧数；

判断条件3：被关联的车辆是否在待关联车辆出现位置以前出现，首先获得车辆运动方向：

其中，v_direction表示车辆运动方向，当其取值为+1时表示车辆运动方向为沿横轴(x轴)正方向，当其取值为-1时表示车辆运动方向为沿横轴(x轴)负方向，tv_change_lane表示车辆运动方向开始改变的车道距离，yo,ki表示待关联车辆的类中心纵坐标，则待关联车辆与被关联车辆位置关系应当满足条件：

其中，xo,ki表示待关联车辆的类中心的横坐标，被关联车辆在最后一次出现的数据帧中其类中心的横坐标；

同时满足上述三个条件，则初步判定车辆关联成功；

然后，针对上述初步判定关联成功的每个被关联车辆与待关联车辆，推算二者在当前帧所处的位置的距离差，然后选定所述距离差中的最小值所对应的被关联车辆与待关联车辆关联成功。

上述推算获得推算二者在当前帧所处的位置的距离差的过程为：

根据被关联车辆的平均速度vindex′估计该车辆在当前帧的位置为：

sindex′＝vindex′(tki-tindex′,e)，

k为当前帧的数据帧序号，tki为当前帧的时间戳，tindex′,e为被关联车辆最后一次出现的数据帧的时间戳；

待关联车辆与被关联车辆之间的距离为：

其中，xfore,ki表示当前帧数据中待关联车辆的组合特征点的x轴坐标，yfore,ki表示当前帧数据中待关联车辆的组合特征点的y轴坐标；

获得被关联车辆与待关联车辆在当前帧所处的位置的距离差|sindex′,ki-sindex′|。

上述选定所述距离差中的最小值所对应的被关联车辆与待关联车辆关联成功的过程为：

取待关联的车辆与所有被关联车辆的距离之差中的最小值：

δsmin_index′,ki＝argmin|sindex′,ki-sindex′|

min_index′为所有被关联车辆中与待关联车辆的距离之差最小的车辆id，当所述最小值满足条件δsmin_index′,ki≤tmin_δs时，则判定关联成功，将待关联车辆的id赋值为min_index′，tmin_δs表示距离估算允许的误差值。

本实施方式中计算车辆距离时，是将激光雷达获得的数据中的车辆相关数据采用车辆盒子模型的方式表达，参见图2所示，该车辆盒子模型中的a、b、c、d分别表示车辆的顶点，o点是车辆的中心点，是车辆的一个特征点，又称为车辆的类中心。图中l1、l2、l3、l4分别表示对应两点之间的距离，v表示车辆行进的速度，(xcenter，ycenter)表示车辆中心点o点的坐标，(xmin，ymin)表示车辆顶点a的坐标，(xmax，ymin)表示车辆顶点b的坐标。e点和f点为车辆盒子的两个特征点，是激光雷达所检测到的车辆上的真实的点，带有时间戳信息，e点和f点通过分别选择距离a、b最近的n个点取其x轴坐标和y轴坐标的平均值得到，其时间戳信息是取距离最近的n个点的时间戳的平均值。图3是激光雷达采集的真实车辆的车辆盒子模型。

车辆盒子模型还有组合特征点fore，其是根据车辆与激光雷达的相对位置对车辆的e点和f点进行组合获得。由于车辆与激光雷达的相对位置不同，造成检测到车辆的形态不同。参见图4所示：

s1表示车辆在激光雷达左边，车辆只有a、b和c点能够被检测到；

s2表示车辆正对激光雷达激光雷达，只有a和b点被检测到；

s3表示车辆在激光雷达右边，车辆只有a、b和d点被检测到。而随着车辆的移动，检测到的ab段不能代表真实的车长。

综上，为了防止由于选择的特征点不能够对应到车辆的同一位置，导致计算距离出现较大的偏差，使得车辆速度计算不准确，应根据车辆与激光雷达的位置，动态地选择特征点，也就是组合特征点fore。

测试实验一：针对四个车道的单独车辆样本速度曲线分析：

分别选取四个车道中单独一辆车连续通过的数据帧，参见图5-图8所示，分别是位于第一车道、第二车道、第三车道和第四车道上的单独车辆的速度曲线。针对每个车道的车辆，分别用特征点o点、e点、f点和组合特征点fore测量车辆速度。从整体上看，采用组合特征点fore检测车速获得的速度曲线，无论在车辆刚开始进入还是车辆离开检测区域，得到的速度曲线较采用其它三个特征单检测获得的速度曲线都更平稳。

测试实验二、第一车道和第二车道误差分析：

选择第一车道车辆在237帧的原始图像与栅格滤波之后的结果，如图9所示。#237处于激光雷达0°所在位置。在该位置是激光雷达一帧数据的起点和终点。这样导致检测到的车辆最小值点，实际上是#236在很短的时间内扫描到的车辆的点，并不是真实的车尾点。而真实的车尾点实际上是min的位中的最小值点。因此，此时计算出的速度是激光雷达在该位置的线速度，而不是车辆的运动速度。

根据激光雷达0°位置寻找车辆真实的车尾点，算法描述如下：

r1:判断是否处于激光雷达正对位置

r2:根据激光雷达数据水平角(azimuth)找到真实车尾点所在点集c

r3:在c点集找x_min_revise

r4:找到此点附近最近n＝5个点，计算x,y,t的平均，作为车辆真实车尾点x_min_revise_ave

r5:把该点投影到车的长边上，即(x_min_revise_ave,y_min)

r6:利用该点计算车速

根据激光雷达0°位置找到的车辆真实车尾点算法结果如图10所示，p点为真实车尾点。车辆经过0°正对位置之后紧接一帧速度出现极小值点，原因在于之前车辆在0°正对位置取到的车辆车尾点不真实，修正为真实的车尾点后，则会造成与其后一帧的时间差过小，会出现和上面误差出现的同样问题。此时的处理办法是将跳过激光雷达0°正对位置，利用该帧前后两帧计算车辆速度(这两帧的时间间隔约为0.1s左右)。

速度修正后的位于位于第一车道的车辆速度曲线图11所示，和位于第二车道的车辆速度曲线参见图12所示，速度修正后，原来异常帧的速度得到了修正，速度曲线变得平稳。

测试实验三：车辆连续速度分析及误差分析。

获得车辆连续速度的关键在于，车辆检测和车辆关联的准确性。在做算法分析的时候，1-2500帧在进行算法设计的时候，算法对其拟合的程度要高点；而后2500-5456作为验证集，用来测试算法工作性能。对车辆关联准确性分析时，选择以下评价指标来进行性能评估：

1)车辆数量vehicle_count。样本集的实际车辆经过的数量是一定的，若算法经过车辆关联之后的数量与实际存在差值，则说明车辆关联存在误差。

2)最大检测距离dr。车辆在该样本集进入和离开样本集的位置较为固定，若车辆关联成功，当车辆沿x轴正向运动，车辆从最左边进入检测区域，从最右边离开检测区域。反之，车辆沿x轴负向运动，则相反。依据此，可以作为检测车辆是否关联成功的标准之一。

3)车辆被检测到的帧数frames_num。车辆从最开始进入检测区域，到离开检测区域，所经过的帧数是与速度相关的一个量。当速度相差范围不大时，则车辆被检测到的帧数也是在某一个固定的区间。

在两个分开的数据集上进行测试，车辆数量结果准确率比较如表1所示。1-2500帧车辆刚开始进入和最后离开的检测距离的测试结果参见图13所示，其评价指标2-3的结果如图14所示。2500-5456帧车辆刚开始进入和最后离开检测距离如图15所示，评价指标2-3的结果如图16所示。

下表中：real表示车辆行驶的真实距离，detection表示采用本方法测量获得的距离，errorratio表示误差，accuracy表示精度。

表1