An object detection and tracking algorithm based on LiDAR and camera information fusion

1.   引言

智能车辆^[1]指的是集成多种技术，包括环境感知、路径规划、决策控制等技术，搭载先进的车载传感器、控制器、执行器等装置，可以实现车与人、车、路、云端等的信息交流与共享，以达到安全、高效、节能的行驶，并最终可以实现自动驾驶的新型汽车^[2]。环境感知技术^[3]指的是智能车辆依靠自身携带的传感器，包括车载视觉传感器、激光雷达、毫米波雷达、GPS全球定位系统、INS惯性导航系统以及超声波雷达等，探测车辆周围的环境信息，并与存储在云端的环境数据进行匹配，描述车辆周围环境状况的技术^[4]。只有环境感知技术准确、实时、可靠，才能保证车辆规划出正确的通行路径，进而安全地实现自动行驶，因此环境感知技术是智能车辆实现安全驾驶的基础要求和前提条件^[5]。

为确保环境感知的快速性和准确性，需要利用智能车辆车载传感器对可通行区域的目标进行检测和跟踪^[6-7]。目标检测^[8]指在环境背景中提取出待检测的目标，如车辆、行人、建筑物等；目标跟踪^[9]指在连续的状态下对某一目标进行连续的检测，提取出目标的运动状态和运动轨迹，常用于轨迹预测和运动状态分析等。常见的目标检测和跟踪方法主要基于视觉传感器，在初始图像数据中利用时域或频域特征将目标从背景中分割出来，并针对该特征在后续图像或视频中提取追踪目标^[10]。常见的目标跟踪算法有基于卡尔曼滤波的目标跟踪算法^[11]，该算法基于对随机过程的估计，主要应用于线性高斯系统；还有基于粒子滤波的目标跟踪算法^[12]，该算法基于蒙特卡洛算法进行非线性跟踪；Dou等人^[13]在粒子滤波的框架下结合了相互作用的多个模型，包括CBWH、CLTP和HOG；侯志强等人^[14]在粒子滤波的基础上采用颜色、空间和纹理信息对目标进行跟踪。但以上算法主要依靠视觉传感器对目标进行跟踪，容易受到光照变化、遮挡、背景干扰的影响^[15]，在实际追踪过程中容易出现目标丢失的现象，而且常见的跟踪算法在初始状态下需要人为指定跟踪目标，在实际应用中带来不便。

本文提出一种基于激光雷达和视觉传感器信息融合的目标检测和跟踪算法。在初始状态下，利用激光雷达的点云数据对地面进行检测，根据点云数据的反射率对可通行区域进行提取，并在可通行区域上利用DBSCAN聚类算法对目标物进行检测；根据已有的激光雷达和视觉传感器的标定信息，将激光雷达检测到的障碍物投影到图片上，根据目标的颜色直方图信息确定跟踪目标；在粒子滤波算法的基础上，利用激光雷达点云数据对目标位置进行修正，从而实现对目标的跟踪。

2.   基于激光雷达和相机的目标检测算法

Figure  1.  Full-Size Img PowerPoint

Flow chart of target detection

基于激光雷达和相机的目标检测算法主要包含五部分：1)激光雷达和相机的联合标定，2)激光雷达的点云数据预处理，3)地面拟合，4)可通行区域提取和5)基于DBSCAN算法的点云数据聚类。本文采用激光雷达和相机信息融合的方式进行可通行区域提取，在图像数据中加入点云数据的反射率信息，降低了光照、遮挡对可通行区域提取的影响。在基于DBSCAN算法的点云数据聚类中，本文在原有的距离维度上增加激光雷达的反射率信息，提高聚类精度。算法流程如图 1所示。

2.1   激光雷达和相机的联合标定

1) 从激光雷达坐标系转换为相机坐标系，可以用旋转矩阵R和平移矩阵T表示。其中，R是大小为3×3的矩阵，表示空间坐标旋转；T是大小为3×1的矩阵，表示空间坐标平移。

标定是多传感器信息融合的准备条件，不同传感器有着独立的坐标系和不同的采集频率，必须把不同坐标系的数据转换到同一坐标系并进行时间配准，才能实现融合。对于激光雷达和相机的联合标定，其目的就是得到激光雷达和相机数据之间的转换关系，即找到同一时刻激光点云数据和图像中对应的像素点。

通过最终的转换将激光点云数据投影到图像上，通过张正友标定法^[16]可以得到坐标转换关系，实现激光雷达和相机的空间联合标定。由于激光雷达的探测视场较大，有些点云数据无法投影到图像上，本文对这部分点云数据进行直接剔除操作。

3) 从图像坐标系到像素坐标系的转换，此时不存在旋转变换，但是坐标原点位置不同，单位长度不同，主要涉及伸缩变换和平移变换。

Figure  2.  Full-Size Img PowerPoint

Flow chart of coordinate transformation

下面介绍激光雷达和相机的时间配准问题，由于相机的采集频率要高于激光雷达，同时相机图像数据和激光雷达点云数据有同步的时间戳，因此本文采用时间最近邻匹配的方法找到与每一帧激光雷达数据时间间隔最小的图像数据进行处理，实现激光雷达和相机的时间配准。

激光雷达坐标系可以描述物体与激光雷达的相对位置，表示为[X_L, Y_L, Z_L]，其中原点为激光雷达几何中心，X_L轴水平向前，Y_L轴水平向左，Z_L轴竖直向上，符合右手坐标系规则。相机系统中存在三个坐标系：像素坐标系、图像坐标系和相机坐标系，像素坐标系表示为[u, v]，其中原点为图像左上角，u轴水平向右，v轴竖直向下；图像坐标系指在图像像素坐标系下建立以物理单位(如毫米)表示的坐标系，使像素尺度具有物理意义，表示为[x, y]，其中原点为相机主点，即相机光轴与图像平面的交点，一般位于图像平面中心，x轴与u轴平行，y轴与v轴平行；相机坐标系可以描述物体与相机的相对位置，表示为[X_C, Y_C, Z_C]，其中原点为相机光心O点，X_C轴与x轴平行，Y_C轴与y轴平行，Z_C轴与摄像机光轴平行，与图像平面垂直。总体的变换思路如图 2所示。

2) 从相机坐标系到图像坐标系的转换，是从三维坐标系转换为二维坐标系的过程，属于透视投影关系，满足三角形的相似定理。其中f为相机焦距。

坐标变换过程一般可以分成三个步骤：

综上所述，激光雷达和相机的坐标转换关系可以表示为

2.2   激光雷达的点云数据预处理

根据激光雷达的性质，一般情况下激光雷达的扫描数据都存储成点云^[18](point cloud)的形式，即每个扫描数据包以若干扫描点的形式存储，每个扫描点包含该点的三维坐标、颜色信息或反射率信息等。由于环境噪声的干扰以及设备精准度的缺陷，原始的点云数据中存在误识别点和杂散点，这些点统称噪声点^[19]。针对原始点云数据中存在的噪声点，本文采用基于半径的滤波方法进行剔除，设置点云数据周围规定半径内的最小点云数，如果给定目标点周围点云数小于设置数字，则假定该点为噪声点。利用该方法可以初步剔除原始点云数据中的噪声点。

激光雷达^[17](Laser detection and ranging)常用于目标物探测和跟踪。环境中不同目标物距雷达距离、反射率、散射形状不同，导致其反射的激光脉冲参数不同，通过接收并测量反射回波信息，可以得到目标物的位置和反射率等信息。

2.3   基于点云数据的地面拟合

设定平面阈值$\Delta d$，若$\left| {{d_i} - d} \right| < \Delta d$，则判定该点属于地面。

对于任意点云数据x_i、y_i、z_i，根据拟合参数a、b、c计算该点的平面值d_i：

d为坐标原点到该平面的距离，利用最小二乘法可以计算出参数a、b、c、d的值。

基于点云数据的地面拟合的方法主要采用最小二乘法^[20]，在车辆初始状态下，选取车辆正前方区域的地面数据进行三维拟合，空间三维平面的拟合方程可以表示为

其中：a、b、c分别为x、y、z平面的单位法向量，即满足关系式：

2.4   基于点云反射率和图像融合的可通行区域提取

经过可通行区域提取，可以得到地面数据和路面边界数据，根据以上结果在原始点云数据中剔除地面数据，并分割出可通行区域的数据，得到的结果就是目标物点云数据的集合。

点云数据的反射率是激光雷达测量中的一项重要指标^[21]，根据不同材料对激光的反射效果不同以及路面颜色的差异，可以基于地面点云数据的反射率和图像融合进行可通行区域提取。

本文采用的可通行区域提取方法主要包含以下几步：首先利用栅格结构将地面点云数据进行网格化处理，将栅格内点云个数和点云反射率平均值记录下来，形成一幅二维的平面反射率分布图；由于有些栅格内点云数目特别稀少，导致该栅格反射率不太稳定，因此利用周围的四个栅格对点云数目过少的栅格进行插值处理，得到新的反射率分布图；然后利用反射率信息对路面区域进行提取，得到可通行区域信息；之后利用激光雷达和相机的标定信息将可通行区域投影到图片中，利用图像处理的方法，如灰度化、二值化(阈值分割)、霍夫变换以及膨胀腐蚀等方法对图像中的可通行区域进行提取；最后根据两种方式提取到的可通行区域进行加权叠加，得到最终的可通行区域结果。主要流程如图 3所示。

Figure  3.  Full-Size Img PowerPoint

Flow chart of passable area extraction

2.5   基于DBSCAN算法的点云数据聚类

3) 以步骤2)的方式不断扩张，直到该点群内所有点都处理完毕，从而得到一个包含核心点和边界点的最大化的点群。

通过DBSCAN算法对目标物点云数据进行聚类，可以将各个目标物分成不同类，提取出目标物的位置和大小。根据激光雷达和相机的标定信息，将目标物的点云数据投影到图片上，通过投影结果就可以确定目标物在图片中的位置。在可通行区域内可能存在一个或多个目标物，在之后的目标跟踪算法中将分别对各个目标进行跟踪。

DBSCAN(Density-based spatial clustering of application with noise)算法是一种典型的基于密度的聚类算法^[22]。该算法的核心思想是，对于构成一个簇的每个点，该点邻域的数据密度必须超过某个阈值。因此，该算法有两个重要的参数，规定半径的邻域Eps以及构成簇的点邻域内最少包含的数据点数目MinPts。本文在规定半径的邻域Eps的定义中加入反射率的信息，采用欧氏距离计算两点间的距离，提高聚类精度。

4) 之后再寻找下一个点群，重复步骤1)~3)，依次类推处理完毕所有数据点，则聚类完成，输出的每个数据点都标记上相应的点群编号或噪声点。

DBSCAN聚类算法流程如下：

1) 首先根据预先确定聚类参数Eps和MinPts对某个未处理的点进行判断，若其Eps邻域内数据点数量大于MinPts，则标记为核心点，并标记上对应的点群编号，同时将其Eps邻域内所有点标记为同一点群编号；若其半径Eps内数据点数量小于MinPts，但是该点落在某核心点的Eps邻域内，则称之为边界点，一个边界点可能同时落入一个或多个核心点的Eps邻域；若其Eps邻域内数据点数量小于MinPts，则将该点标记为噪声点，处理下一个点；

2) 然后对该点邻域内点处理，各自判断是否为核心点，并将其中属于核心点的数据点的Eps邻域内点加入对应的点群中并进行核心点判断；

3.   基于激光雷达和相机的目标跟踪算法

基于激光雷达和相机的目标跟踪算法主要基于粒子滤波算法的框架，由第2中检测到的目标确定初始跟踪目标，利用颜色信息在图像中跟踪目标，在粒子重采样后，用当前帧的点云目标检测结果对目标状态进行修正，得到更加精确的目标状态。具体流程图如图 4所示。

Figure  4.  Full-Size Img PowerPoint

Flow chart of object tracking

3.1   基于粒子滤波算法的目标跟踪

3.1.1   特征提取

其中：B_C是计算直方图的像素区间，$\xi$是区间的变量，I(x, y)是坐标(x, y)位置的像素值。本文采用Bhattacharyya系数^[23]作为相似性表征。

颜色特征是图像处理领域常用的一种特征，该特征对于图像尺寸、方向和视角依赖性很低，具有较高的鲁棒性，常常用于目标检测和目标跟踪领域。颜色直方图是颜色特征的一种常见表达形式，计算简单，使用便捷。对于图像I，颜色直方图的计算式为

3.1.2   粒子滤波

2) 粒子预测：根据状态转移方程，每个粒子预测得到一个新的粒子，在k时刻粒子集合为${\rm{\{ }}X_k^i{\rm{\} }}_{i = 1}^N$。

其中：A为状态转移矩阵，本文取单位矩阵；W为噪声，本文取均值为0的高斯噪声。

5) 状态估计：根据当前样本粒子集合的状态和权重估计目标状态：

4) 选择阶段(重采样)：根据粒子权重对样本进行重新采样，复制权重高的粒子，淘汰权重低的粒子，使新的粒子集合满足之前的权重分布，即权重越大，粒子保留的越多。重采样后，重新设置权重$w_k^i = 1/N$。

通过以上步骤，基本可以实现基于粒子滤波的目标跟踪。由于目标的状态转移特性很难预测，因此本文采用常用的一阶线性运动方程作为系统的状态转移方程：

3) 权值更新：根据当前时刻的测量值${Z_k}$，为每个粒子重新分配权重：

粒子滤波(Particle filter，PF)是一种基于蒙特卡洛原理的贝叶斯滤波算法^[24]。它利用粒子群的概率密度解决非线性的滤波问题，它的核心思想是从后验概率中随机抽取粒子表达系统的概率分布，通过抽取粒子的平均值估计系统状态，因此被广泛应用于视觉追踪、目标导航定位等领域。

粒子滤波的主要流程如下：

1) 初始化：根据先验概率分布$p({X_{\rm{0}}})$，对原始样本进行采样，投放N个粒子，建立粒子集合${\rm{\{ }}X_0^i{\rm{\} }}_{i = 1}^N$，每个粒子的权重设为$w_0^i = 1/N$。

并对粒子的权重进行归一化：

3.2   基于激光点云的目标状态修正

由于基于图像的目标跟踪算法容易受到光照、遮挡和畸变的影响，在目标跟踪过程中常常会出现目标漏检、错检的情况。点云数据是通过激光雷达发射和接收激光信息而获得的，因此点云数据不容易受光照和遮挡的影响，可以为图像数据进行信息补充。基于激光点云的目标状态修正主要包括以下几步：点云数据和图像数据时间同步，基于激光点云的可通行区域的目标物提取，目标物点云投影到图片上以及与粒子滤波追踪到的目标物进行匹配。

根据时间同步的结果，在相机图像数据对应的激光点云数据中进行目标物提取，主要方法在第2节有详细介绍。之后根据激光雷达和相机的标定信息，将基于激光点云提取到的目标物投影到图片上，找到与粒子滤波的跟踪结果X(k)匹配的目标物，得到对应的追踪目标x(k)。由于粒子滤波的目标追踪结果是粒子集的加权叠加，符合均匀分布，激光点云的目标提取结果是激光点的加权叠加，也符合均匀分布，因此目标真实状态可以认为是均匀分布的，假设当前时刻粒子滤波中的粒子数为N₁，目标处激光点数目为N₂，则当前时刻目标的真实位置：

由于激光雷达和相机的采样频率不同，导致激光雷达采集到的点云数据与相机采集到的图像数据无法直接匹配，需要对点云数据和图像数据进行时间同步。本文采用的时间同步方法为基于最近邻的时间戳匹配法，在激光雷达和相机采集数据时，记录下开始采集的时间，通过最近邻匹配的方式，找到采集时间最相近的数据。一般情况下，激光雷达的采集频率较低，相机的采集频率较高。最近邻匹配方法舍弃多采集的相机数据，用留下的相机图像数据与激光点云数据进行匹配。该方法可以认为每组数据的记录时间是相同的，即激光雷达和相机观测到的是同一时刻的场景。

在粒子滤波算法中，对跟踪结果影响最大的两个阶段是初始化阶段和选择阶段(也称重要性重采样阶段)。本文采用基于激光雷达和相机的目标检测算法确定跟踪目标，可以提高初始化阶段跟踪目标的准确性；为了提高选择阶段的准确性，本文采用基于激光点云的目标状态修正算法对跟踪结果进行修正。

3.3   本文算法流程

2) 根据3.1.1中颜色直方图的计算方式提取跟踪目标的颜色特征信息；

① 根据状态转移方程式(12)和上一时刻粒子状态${\rm{\{ }}X_{k - 1}^i{\rm{\} }}_{i = 1}^{{N_1}}$，预测当前时刻粒子状态${\rm{\{ }}X_k^i{\rm{\} }}_{i = 1}^{{N_1}}$；

1) 在初始状态下，根据第2节的流程在激光雷达点云数据中提取出可通行区域内的目标物，将目标物投影到相机图像上，确定跟踪目标的状态X(0)，包括目标的位置和大小；

综上所述，本文基于激光雷达和相机的目标检测及跟踪算法的整体流程如下：

② 根据Bhattacharyya系数，计算当前时刻粒子与上一时刻粒子的相似度；

⑤ 在当前帧，利用激光点云数据提取可通行区域的目标物，并与粒子滤波追踪到的目标物进行匹配，得到对应的追踪目标x(k)，假设当前时刻追踪目标处激光点数目为N₂，当前激光点状态为${\rm{\{ }}x_k^i{\rm{\} }}_{i = 1}^{{N_2}}$，则$x(k) = \frac{1}{{{N_2}}}\sum\limits_{i = 1}^{{N_2}} {x_k^i} ；$

3) 根据先验概率分布在目标位置均匀投放N₁个初始粒子${\rm{\{ }}X_0^i{\rm{\} }}_{i = 1}^{{N_1}}$，每个粒子的权重设为$w_0^i = 1/{N_1}$；

⑦ 输出当前帧的追踪结果$\hat X(k)$；

5) 结束。

③ 根据粒子相似度为粒子分配新的权重，并对粒子权重进行归一化；

⑥ 根据粒子滤波的目标追踪结果X(k)和激光点云的目标提取结果x(k)，计算当前时刻目标的真实状态$\hat X(k)$^[24]；

④ 根据粒子权重对样本进行重新采样，采用简单随机重采样的方法对粒子进行重新采样，并对重采样后的粒子重新分配权重$w_k^i = 1/{N_1}$，根据当前粒子集计算出当前时刻的状态估计$X(k) = \sum\nolimits_{i = 1}^{{N_1}} {w_k^iX_k^i}$；

4) 对k=1, 2, 3, …, F_Frame：

4.   实验结果及分析

实验采用4 GB内存的计算机作为实验平台，并在MATLAB 2017b上进行编程实现。本文采用粒子滤波算法(particle filter, PF)^[26]、无损卡尔曼滤波算法(unscented Kalman filter, UKF)^[27]、DCO-X算法^[28]作为对比算法，验证算法的有效性。

为了验证本文提出算法的有效性，采用KITTI数据集的目标追踪集合对算法性能进行测试。KITTI数据集^[25]是由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办，是目前国际上最大的自动驾驶场景下的计算机视觉算法评测数据集，该数据集提供了激光雷达和相机之间的坐标转换关系以及数据同步的时间戳，方便实验人员进行数据融合。同时，该数据集提供了人工标注的目标检测及目标跟踪的真实值，为实验人员验证算法有效性提供了依据。

4.1   实验参数设置

对于MinPts参数，选取原则是MinPts应不小于聚类数据的空间维度数加一，对于二维数据一般选取MinPts=4；对于Eps参数选取，可以通过距离法进行判断：对于数据集D中的任一点p，计算其到最近的第MinPts个点的距离，记为d_distm(p)，遍历所有数据点得到集合D_Distm，对集合中的数据升序排序，并绘制曲线，得到D_Distm曲线，对于密度分布较为均匀的数据，其D_Distm曲线一般如图 5所示，其中横坐标代表排序后数据点编号，纵坐标代表对应的d_distm值。A点一般是距离(n, 0)点最近的点(n为一帧数据中数据点总数)。记(n, 0)点为S点，将到S点的距离作为判断依据，选取距离最小的点作为A点，将A点对应的d_distm值作为Eps参数，可以得到较好的聚类效果。

对于粒子滤波算法，粒子数目的设置应当在不影响计算速度的情况下尽可能的增加，高斯方差按经验值选取，所以本文采取的粒子数目N=300，高斯方差σ=0.35。

Figure  5.  Full-Size Img PowerPoint

Typical D_Distm graph

本实验中需要设置的参数主要由有两部分，一部分是DBSCAN聚类算法中的规定半径的邻域Eps以及构成簇的点邻域内最少包含的数据点数目MinPts；另一部分是粒子滤波算法中的粒子数目和高斯方差。

4.2   目标检测结果

Figure  6.  Full-Size Img PowerPoint

Target detection results. (a), (b) 3D plot of point cloud; (c), (d) Target detection results in pictures

本实验中目标检测的作用有两方面，一是确定初始跟踪目标，二是对粒子滤波的结果进行修正。主要步骤包括可通行区域提取，点云数据聚类以及将目标结果投影到图片上。图 6(a)、6(b)展示的是点云数据的三维显示图，颜色代表该点的反射率，其中红框内是检测到的目标，黑线代表可通行区域边界线，图 6(c)、6(d)展示的是目标检测最终结果，其中红框内是检测到的目标，黄线代表可通行区域边界线。

4.3   目标跟踪结果

为了定量分析本实验的目标追踪结果，采用X方向追踪轨迹(X-trace)、Y方向追踪轨迹(Y-trace)、中心位置误差(center error)、区域重叠(region overlap)以及成功率(success rate)来评价跟踪算法。

表 1统计了使用KITTI数据集的目标追踪集合对车辆进行追踪的X方向平均误差(X average error，用e_xa表示)、Y方向平均误差(Y average error，用e_ya表示)、中心位置平均误差(average center error，用e_ac表示)、平均区域重叠(average region overlap，用o_ar表示)以及成功率(success rate，用R_s表示)。当本帧图像区域重叠超过50%时认为本帧追踪成功。从表中可以看出在使用KITTI数据集进行目标追踪时，本文提出的算法具有良好效果。

图 8显示了KITTI数据集2011年9月26日采集的数据的X方向追踪轨迹和Y方向追踪轨迹，该数据的图片尺寸是1242 pixels×375 pixels，从该图可以看出本文提出的算法的X方向追踪轨迹精度和Y方向追踪轨迹精度和稳定性是四种算法中最高的。

图 7显示的是本文算法的追踪效果图(performance plot)，蓝色框表示的是目标真实位置，红色框表示的是本文算法的目标追踪位置，紫色框表示的是DCO-X算法的追踪位置，绿色框表示的是UKF算法的追踪位置，青色框代表的是PF算法的追踪位置。从追踪效果图中可以看出，本文算法可以准确跟踪到追踪目标，追踪效果较好。

Figure  7.  Full-Size Img PowerPoint

Target tracking results

Figure  8.  Full-Size Img PowerPoint

Target tracking results. (a) X-trace; (b) Y-trace

5.   结论

针对智能车辆环境感知系统，本文提出了一种基于激光雷达和相机的信息融合的目标检测及追踪算法。首先，本文利用激光点云数据聚类方法对可通行区域的障碍物进行检测，并投影到图片上，确定跟踪目标。之后，本文在粒子滤波的算法基础上，利用颜色信息追踪目标，针对基于图像的目标追踪算法容易受光照、遮挡、背景干扰等因素的影响，采用激光雷达的目标检测结果对目标追踪结果进行修正，提高目标追踪效果。最后，本文使用KITTI数据集对算法进行测试和验证。实验表明，本算法在目标追踪的X方向误差、Y方向误差、中心误差、区域重叠、成功率等评价标准中具有较好的效果。