CN110658531A - 一种用于港口自动驾驶车辆的动态目标追踪方法 - Google Patents

Abstract

本发明公开了一种用于港口自动驾驶车辆的动态目标追踪方法，首先通过安装在卡车头部的激光雷达和GPS定位信息对连续帧点云进行融合合并，得到相对稠密的点云感知数据；然后对融合后的点云数据进行体素化形成栅格3D图，降低点云的随机性；接着利用体素3D信息的高度数据对地面进行消除，同时，利用前一帧的融合数据做差，消除大部分静态障碍物，仅保留动态目标信息；然后，通过基于密度的聚类算对剩余点云进行聚类，得到本帧的移动目标信息；最后，利用相邻帧的移动目标进行启发式追踪算法计算当前帧的移动目标中心、大小、速度、运动方向、生命周期和历史轨迹，并将本帧的追踪障碍物列表输出。该方法数据处理量较小、感知区域广、成本较低。

Description

一种用于港口自动驾驶车辆的动态目标追踪方法

技术领域

本发明涉及一种用于港口自动驾驶车辆的动态目标追踪方法，属于人工智能技术领域。

背景技术

随着车载传感器和汽车自动化技术的发展，特定场景下的自动驾驶任务得以实现。通过单目或多目相机进行光学成像的感知、基于多普勒效应的毫米波雷达感知和基于主动激光的多线激光雷达是目前主流的无人驾驶感知方案。但相机受环境光和成像视野等问题，虽然可以获得高清道路语义信息，但数据处理量较大，且基于单目或双目的测距精度随距离增加不断衰减。毫米波雷达虽然在高速状态下能够利用多普勒效应进行动态目标检测和追踪，但在低速状态的误检率较高；同时，毫米波传感器容易受到周边非障碍物(如金属井盖、路标)的干扰，产生进一步干扰。而超声雷达产生的超声波在空气衰减严重，有效检测精度和距离十分有限。

在港口场景下，有大量的集装箱卡车且一般行驶速度较低，另外，场景中还有大量金属箱体和其他金属障碍物。以激光雷达为主的传感器感知方案既能覆盖较为广泛的感知区域，又可以保障全面的感知覆盖率，能够有效弥补相机视野不足和毫米波误检的情况。

早期的研究中，基于光学相机的目标检测和追踪算法相对主流，但受限于光学相机，无法提供障碍物的有效距离；同时，基于双目测距的方法仅在室内场景的近距离范围内相对有效，不能满足中长距离的目标追踪需求。另外，早期的激光雷达目标检测算法多以开放道路场景的小型车辆为主，通过顶部的单个全向激光雷达感知周边环境。而集装箱卡车由于体积较大，高度超过3米，难以通过在车辆顶部安装单个雷达实现车身及周边，特别是近距离地面的全面覆盖。同时，高线束的激光雷达成本更高，通常是单个16线雷达的数十倍。而基于深度学习的目标检测算法也严重依赖密集激光束所获取的稠密点云的障碍物特征，激光线数较少的点云特征较难获得稳定目标；同时，高功耗的GPU设备无法在车载环境进行部署，使得深度学习方案在目前来说实用性不高。

发明内容

本发明的目的是提供一种数据量较小、感知区域广、成本较低的用于港口自动驾驶车辆的动态目标追踪方法。

为此，本发明的技术方案如下：

一种用于港口自动驾驶车辆的动态目标追踪方法，包括以下步骤：

S1，在车辆行驶过程中，通过安装在车头两侧的激光雷达获取环境点云数据，通过车载惯性导航***得到车体的经纬度坐标，将激光雷达的坐标系标定到以惯性导航***的位置为原点的车体坐标系上，将获取的环境点云坐标从车体坐标系转换至大地坐标系；将大地坐标系下当前帧的环境点云与前一帧的环境点云进行叠加，作为当前帧的环境点云；

S2，设置一个直通滤波器，对步骤1得到当前帧的环境点云的长、宽、高进行过滤，保留固定距离范围内的环境点云；再对过滤后的环境点云进行体素化处理，得到3D体素栅格；之后设定地面高度阈值，遍历所有体素栅格，删除高度在所述地面高度阈值以下的体素栅格；

S3，将步骤2得到的当前帧的体素栅格与上一帧的体素栅格做差运算，消除静止状态的障碍物的体素栅格，得到动态障碍物的体素栅格；

S4，按高度信息对体素栅格进行放缩，之后对放缩后的体素栅格进行聚类，得到当前帧的聚类结果列表，再按照启发条件剔除不符合要求的障碍物信息；

S5，根据步骤4得到的当前帧的障碍物列表，结合上一帧的障碍物列表信息进行目标匹配和追踪，计算出当前帧的移动目标中心、大小、速度、运动方向、生命周期和历史轨迹，并将本帧的追踪障碍物列表输出；

S6，重复步骤1-5，直到自动驾驶结束。

优选的是，上述的步骤S1中，所述激光雷达为16线激光雷达，激光雷达的数据获取频率为10Hz，以GPS时钟作为时钟基准。所述大地坐标系采用WGS-84下的大地坐标系。

优选的是，在步骤S2中，进行过滤的方法为设定保留的点云范围为前后各50米、左右各50米以及高度-1.9～2.4米，然后，将保留的点云投射到边长为0.2米的体素栅格内，以KDTree数据结构存储。

优选的是，在步骤S4中，对体素栅格进行放缩的具体方法为对z轴的数据进行尺度放缩，将高度信息压缩到原先的0.01倍，并保持体素栅格的长宽不变。进行聚类的方法为DBSCAN聚类法。

优选的是，上述聚类参数的最小搜索半径为体素栅格的边长的1-1.5倍。更优选的是，聚类参数的最小搜索半径为体素栅格的边长1.3倍

在步骤S4中，所述启发条件为：1)面积小于最小面积阈值的障碍物；2)长度大于最大车长阈值的障碍物。

在上述的步骤S5中，进行目标匹配和追踪的步骤如下：

(1)遍历上一帧的每个障碍物Pi，设Pi的位置中心坐标为(xi,yi)，遍历当前帧的每个障碍物的位置中心，找到与Pi(xi,yi)的几何距离最小的障碍物Oj，设Oj的位置中心坐标为(xj，yj)，设所述最小几何距离为min_dis；

(2)设定极限运动距离阈值，所述极限运动距离阈值为极限速度乘以相邻帧的时间差δt，若min_dis大于极限运动距离阈值，则表示Pi追踪丢失；若min_dis小于极限运动距离阈值,则根据公式(1)计算得到Pi的速度，更新Pi的追踪生命周期和累计路程：

Vx、Vy:分别为Pi的横向速度和纵向速度，单位：m/s；

xi、yi：分别为Pi的横轴和纵轴坐标；

xj、yj：分别为Oj的横轴和纵轴坐标；

δt:相邻帧的时间差，单位：秒；；

(3)对于连续两帧都追踪丢失的障碍物，将其删除；对于丢失一帧的追踪障碍物，按公式(2)进行中心位置估算，并更新累计路程，但不更新生命周期：

(4)遍历当前帧的每一个障碍物Oj，遍历上一帧的每个障碍物的位置中心，找到与Oj(xj，yj)的几何距离最小的障碍物Pi，若min_dis大于极限运动距离阈值，则表示Oj为新追踪的障碍物，将其加入到本帧的障碍物序列中；

(5)输出本帧的追踪障碍物的列表。

本发明综合考虑现有激光雷达传感器的特性和港口场景下的驾驶任务，借助两个装载在集装箱卡车头部两侧的16线激光雷达实现了对车辆周边50米半径内的有效感知覆盖，实现了低速集装箱卡车在港口道路场景下的移动障碍物自动识别和追踪。本方法能感知的港口场景移动障碍物主要包括卡车、汽车和龙门吊。

与现有技术相比，本发明具有以下有益效果：

(1)针对低速港口场景下无人驾驶集装箱卡车的动态目标检测，仅需要借助两颗16线激光雷达，成本较低；

(2)基于多种组合的启发条件进行动态目标检测，仅需通过CPU完成，且配置方便；

(3)基于双16线激光雷达的点云感知数据叠加和体素化，能够有效降低点云的不稳定特性，并且数据量较小、感知区域广，远低于图像处理的运算量。

附图说明

图1是本发明的港口动态目标追踪方法的流程图；

图2是本发明的实施例中激光雷达获取的原始点云数据；

图3是本发明的实施例中点云叠加前后的对比图；

图4是本发明的实施例中得到的3D体素格栅；

图5是本发明的实施例中地面过滤前后的体素栅格对比图；

图6是本发明的实施例中帧间差运算过滤前后的体素栅格对比图；

图7是本发明的实施例中DBSCAN聚类效果图；

图8是本发明实施例中的一组追踪结果序列。

具体实施方式

本发明的用于港口自动驾驶车辆的动态目标追踪方法主要包括以下步骤：

首先，对安装在车头两侧的两颗激光雷达进行标定，将激光雷达的感知结果映射到惯导的中心点上，获取激光雷达输出的环境点云数据。在获取点云数据数据前，设定激光雷达传感器的数据获取频率，并以GPS时钟作为时钟基准。接着，在实时行驶过程中，将在车体坐标系下得到的激光雷达点云数据结合和惯导得到的车体的经纬度坐标，将坐标转换到大地坐标系下，将相邻两帧的大地坐标系点云数据进行叠加。之后对叠加后的点云进行过滤和体素化处理，以得到3D体素栅格信息，降低点云的随机性；再设定地面高度阈值，遍历所有体素栅格，删除高度在所述地面高度阈值以下的体素栅格。通过对相邻的体素栅格做差运算滤除当前帧的静态障碍物，保留动态障碍物的栅格。然后，按高度信息对体素栅格进行放缩，对放缩后的体素栅格进行基于密度的DBSCAN方法的聚类，得到当前帧的聚类结果列表。按照启发式条件，剔除不符合条件的聚类结果。最后，结合上一帧的聚类结果列表的信息进行目标追踪，更新当前帧的跟踪结果，得到当前帧的移动目标中心、大小、速度、运动方向和历史轨迹。

下面结合附图和具体实施例对本发明的方法进行详细说明。

实施例一

参见图1，发明的港口动态目标追踪方法包括以下步骤：

步骤1，获取环境点云数据，转换点云坐标并进行点云叠加：

在本实施例中，在车头两侧约1.7m高的位置处各安装一16线激光雷达，通过这两个16线激光雷达获取周围环境的原始点云数据,所获取的原始点云数据如图2所示。设激光雷达传感器的数据获取频率为10Hz，以GPS时钟作为时钟基准。通过车载惯性导航***(简称惯导)得到车体的经纬度坐标。由于激光雷达得到的点云数据处于车体坐标系下，而惯导得到的则是经纬度，随着车辆运动，相邻的两帧雷达点云数据存在相对运动误差，因此需要先通过坐标转换将点云投影到统一的坐标系下再进行叠加。为此，将激光雷达坐标系标定到以惯导位置为原点的车体坐标系上，再根据惯导得到的经纬度坐标进行坐标转换，得到大地坐标系下的激光雷达点云，本实施例采用WGS-84下的大地坐标系。由于16线激光雷达的采样密度较低，特征不明显，因此将大地坐标系下当前帧的环境点云与前一帧的环境点云进行叠加，得到相对稠密的点云感知数据，使得障碍物特征更加稳定，将叠加后的环境点云作为当前帧的点云。点云叠加前后的对比图如图3所示，左图为点云叠加前，右图为点云叠加后，由图中可以看出，叠加后的点云密集了很多，在一定程度上强化了特征。

步骤2，对点云进行直通滤波、体素化和地面过滤：

设置一个直通滤波器，按照距离对步骤1得到的当前帧的点云的长、宽、高进行过滤，保留固定距离范围内的点云。本实施例中，设定保留的点云范围为：前后各50米，左右各50米，高度-1.9到2.4米(以激光雷达为坐标原点)。然后，将过滤后的点云投射到边长为0.2米(即体素化的分辨率250*250*18)的体素栅格内，以KDTree数据结构存储，所得到的体素栅格如图4所示。由于点云的波动性较大，可通过降采样稳定点云的特征，以提高聚类速度。然后进行地面过滤：设定一个地面高度阈值，例如1.6m，遍历所有的体素栅格，删除高度在地面高度阈值以下的体素栅格。地面过滤前后的体素栅格对比图参见图5，图中左边为消除前体素栅格，右边为消除后体素栅格。

步骤3，静态障碍剔除：

通过帧间信息可以消除静止状态的障碍物的体素栅格，将步骤2得到的当前帧的体素栅格与上一帧的体素栅格做差运算，差运算后的体素栅格即为动态的障碍物的体素栅格，如图6所示，其中，左图为差运算前的体素化栅格，右图为差运算后的体素化栅格。

步骤4，障碍物聚类和过滤：

对步骤3得到的剩余体素栅格进行聚类。首先，对z轴的数据进行尺度放缩(即对体素栅格的高度信息进行放缩)，将高度信息压缩到原先的0.01倍，并保持体素栅格的长宽不变。这样既保留了高度的信息，也使得所有的体素在一个近似的平面内。然后使用基于密度的DBSCAN(Density-Based Spatial Clustering of Applications with Noise)聚类法对体素栅格进行聚类，聚类参数的最小搜索半径与体素化的体素栅格边长参数相关，一般设置为体素栅格的边长的1-1.5倍，本实施例中设置为体素栅格的边长1.3倍。聚类得到当前帧的障碍物列表，障碍物列表信息应包括每一个目标在平面投影的算术中心即障碍物中心的x,y坐标、障碍物类别的序号和点云数据帧的序号，图7展示了DBSCAN的聚类结果。之后，按照下述的启发条件剔除不符合要求的障碍物信息：

1)面积小于最小面积阈值的障碍物；

2)长度大于最大车长阈值的障碍物。

上述启发条件中的阈值参数可根据实际情况设定。

步骤5，启发式障碍物追踪：

根据步骤4得到的当前帧的障碍物列表，结合上一帧的障碍物列表信息进行目标匹配和追踪。遍历上一帧的每个障碍物Pi，设Pi的位置中心为(xi,yi)，遍历当前帧的每个障碍物的位置中心，找到与Pi(xi,yi)的几何距离最小的障碍物Oj，设Oj的位置中心为(xj，yj)，设所述最小几何距离为min_dis。

设定一个极限运动距离阈值，所述极限运动距离阈值为极限速度乘以相邻帧的时间差δt，若min_dis大于极限运动距离阈值，则表示Pi追踪丢失；若min_dis小于极限运动距离阈值,则根据公式(1)计算得到Pi的速度。更新Pi的追踪生命周期(每次累加1)和累计路程(累加几何距离)：

其中：

Vx、Vy:分别为Pi的横向速度和纵向速度，单位：m/s；

xi、yi：分别为Pi的横轴和纵轴坐标；

xj、yj：分别为Oj的横轴和纵轴坐标；

δt:相邻帧的时间差，单位：s。

对于连续两帧都追踪丢失的障碍物，将其删除。对于丢失一帧的追踪障碍物，按公式(2)进行中心位置估算，并更新累计路程，但不更新生命周期：

遍历当前帧的每一个障碍物Oj，遍历上一帧的每个障碍物的位置中心，找到与Oj(xj，yj)的几何距离最小的障碍物Pi，若min_dis大于极限运动距离阈值，则表示Oj为新追踪的障碍物，将其加入到本帧的障碍物序列中，最终计算出当前帧的移动目标中心、大小、速度、运动方向、生命周期和历史轨迹，并将本帧的追踪障碍物列表输出。图8展示了此方法的一组追踪结果序列。

步骤6，重复步骤1-5，直到自动驾驶结束。

Claims (10)

1.一种用于港口自动驾驶车辆的动态目标追踪方法，包括以下步骤：

S1，在车辆行驶过程中，通过安装在车头两侧的激光雷达获取环境点云数据，通过车载惯性导航***得到车体的经纬度坐标，将激光雷达的坐标系标定到以惯性导航***的位置为原点的车体坐标系上，将获取的环境点云坐标从车体坐标系转换至大地坐标系；将大地坐标系下当前帧的环境点云与前一帧的环境点云进行叠加，作为当前帧的环境点云；

S2，设置一个直通滤波器，对步骤1得到当前帧的环境点云的长、宽、高进行过滤，保留固定距离范围内的环境点云；再对过滤后的环境点云进行体素化处理，得到3D体素栅格；之后设定地面高度阈值，遍历所有体素栅格，删除高度在所述地面高度阈值以下的体素栅格；

S3，将步骤2得到的当前帧的体素栅格与上一帧的体素栅格做差运算，消除静止状态的障碍物的体素栅格，得到动态障碍物的体素栅格；

S4，按高度信息对体素栅格进行放缩，之后对放缩后的体素栅格进行聚类，得到当前帧的聚类结果列表，再按照启发条件剔除不符合要求的障碍物信息；

S5，根据步骤4得到的当前帧的障碍物列表，结合上一帧的障碍物列表信息进行目标匹配和追踪，计算出当前帧的移动目标中心、大小、速度、运动方向、生命周期和历史轨迹，并将本帧的追踪障碍物列表输出；

S6，重复步骤1-5，直到自动驾驶结束。

2.根据权利要求1所述的动态目标追踪方法，其特征在于：步骤S1中，所述激光雷达为16线激光雷达，激光雷达的数据获取频率为10Hz，以GPS时钟作为时钟基准。

3.根据权利要求1所述的动态目标追踪方法，其特征在于：步骤S1中，所述大地坐标系是WGS-84下的大地坐标系。

4.根据权利要求1所述的动态目标追踪方法，其特征在于：步骤S2中，进行过滤的方法为设定保留的点云范围为前后各50米、左右各50米以及高度-1.9～2.4米，然后，将保留的点云投射到边长为0.2米的体素栅格内，以KDTree数据结构存储。

5.根据权利要求1所述的动态目标追踪方法，其特征在于：步骤S4中，对体素栅格进行放缩的具体方法为对z轴的数据进行尺度放缩，将高度信息压缩到原先的0.01倍，并保持体素栅格的长宽不变。

6.根据权利要求1所述的动态目标追踪方法，其特征在于：步骤S4中，进行聚类的方法为DBSCAN聚类法。

7.根据权利要求6所述的动态目标追踪方法，其特征在于：聚类参数的最小搜索半径为体素栅格的边长的1-1.5倍。

8.根据权利要求7所述的动态目标追踪方法，其特征在于：聚类参数的最小搜索半径为体素栅格的边长1.3倍。

9.根据权利要求1所述的动态目标追踪方法，其特征在于，步骤S4中，所述启发条件为：1)面积小于最小面积阈值的障碍物；2)长度大于最大车长阈值的障碍物。

10.根据权利要求1所述的动态目标追踪方法，其特征在于，步骤S5中，进行目标匹配和追踪的步骤如下：

(1)遍历上一帧的每个障碍物Pi，设Pi的位置中心坐标为(xi,yi)，遍历当前帧的每个障碍物的位置中心，找到与Pi(xi,yi)的几何距离最小的障碍物Oj，设Oj的位置中心坐标为(xj，yj)，设所述最小几何距离为min_dis；

(2)设定极限运动距离阈值，所述极限运动距离阈值为极限速度乘以相邻帧的时间差δt，若min_dis大于极限运动距离阈值，则表示Pi追踪丢失；若min_dis小于极限运动距离阈值,则根据公式(1)计算得到Pi的速度，更新Pi的追踪生命周期和累计路程：

其中：

Vx、Vy:分别为Pi的横向速度和纵向速度，单位：米/秒；

xi、yi：分别为Pi的横轴和纵轴坐标；

xj、yj：分别为Oj的横轴和纵轴坐标；

δt:相邻帧的时间差，单位：秒；

(3)对于连续两帧都追踪丢失的障碍物，将其删除；对于丢失一帧的追踪障碍物，按公式(2)进行中心位置估算，并更新累计路程，但不更新生命周期：

(4)遍历当前帧的每一个障碍物Oj，遍历上一帧的每个障碍物的位置中心，找到与Oj(xj，yj)的几何距离最小的障碍物Pi，若min_dis大于极限运动距离阈值，则表示Oj为新追踪的障碍物，将其加入到本帧的障碍物序列中；

(5)输出本帧的追踪障碍物列表。

Images