顾及数字地图中车道走向的车辆跟踪增强算法

庄瀚洋，王晓亮，王春香，杨 明

1. 上海交通大学密西根学院，上海 200240； 2. 广西科技大学广西汽车零部件与整车技术重点实验室，广西 柳州 545006； 3. 上海交通大学自动化系，上海 200240； 4. 系统控制与信息处理教育部重点实验室，上海 200240； 5. 上海工业智能管控工程技术研究中心，上海 200240

智能驾驶系统的环境感知功能对于车辆的运行安全起着至关重要的作用，目标检测与跟踪是环境感知的核心，目标检测任务能够对交通参与者进行识别，而目标跟踪任务则是在时间尺度上分辨是否为同一目标并且进行持续的状态估计。基于三维激光雷达的目标检测与跟踪算法是无人驾驶系统的一个重要组成部分[1]，对于路径规划和决策控制等高级任务具有重要意义。

现阶段基于激光雷达的目标检测方法较为成熟[2]：基于先验规则的检测手段，通过地面分割、聚类、生成包围盒等流程完成目标检测，但稳健性较差[3-6]；近年来基于神经网络的激光雷达目标检测算法趋于成熟，且具备更高的检测精度与更好的适应性，如PointRCNN[7]、Part-A2Net[8]、PVRCNN[9]等。而基于检测的目标跟踪方法通过多帧的数据关联与状态估计生成目标轨迹[10]，数据关联旨在确定多帧数据之间多个目标的对应关系，是目标跟踪的基础[11-13]；状态估计指的是对同一目标多帧测量结果进行滤波，并对目标的状态量进行精确估计[14-16]。

跟踪道路上行驶的车辆是智能车辆的首要任务，车辆跟踪算法在距离较近时能够保持较高的精度。但在高速公路或城市快速路中，车辆速度较快、制动距离较长，要求车辆跟踪功能对中远距离的车辆依旧够保持准确、稳定的跟踪效果，对跟踪算法提出了严格的要求。然而在车辆距离较远时激光雷达点云较为稀疏，如图1所示，检测结果存在较大误差从而导致跟踪算法精度下降，并且跟踪的车辆轨迹出现明显抖动，最终影响决策系统的意图判断，产生安全隐患。因此，如何提升远距离下车辆跟踪算法的精度与稳定性，是本文拟解决的问题。

对远距离的车辆检测与跟踪需求发生在道路状况良好的结构化道路场景中，如高速公路或高架路等，在这种情况下绝大多数车辆的行驶轨迹是沿着车道线方向的，因此基于该先验假设，通过车道线的几何结构对车辆的行驶朝向做出约束从而提高在车辆朝向估计上的精度是行之有效的技术路线，相关的研究也从仿真与理论方面证明了其效果[17-18]。然而，在实际场景的应用过程中，由于传感器测量误差、标定误差、远处点云稀疏等因素会导致远处车道线的朝向估计准确性下降，因此如何有效、准确地获取车道信息是本文研究课题的核心问题。

图1 不同距离上激光雷达的点云数量Fig.1 Point number of LiDAR point cloud at different distances

在现阶段的研究中，车道线建模已经是较为成熟的技术手段，如二值图像[19]、多项式[20-21]、样条曲线[22-23]等，但简单的直线模型无法处理复杂弯道，而复杂的模型又容易过拟合，均无法有效应对任务要求。智能驾驶车辆的决策与规划所依赖的数字地图基本是必不可少的部分[24]，无论是OpenStreetMap(OSM)这类拓扑地图，或是OpenDRIVE这类高精地图，都能够提供基本的道路几何结构信息。

本文基于上述思路，将数字地图中的道路信息进行提取与建模，利用道路的朝向约束来增强车辆跟踪。本文在KITTI数据集上展开[25]，在检测的基线方法上选择PVRCNN来进行，因为该方法是目前KITTI排行榜上效果最好的三维目标检测算法之一，基于该方法进行研究不仅能提供较为稳定的检测结果，而且也能体现道路的朝向约束对车辆跟踪方法的提升。由于KITTI数据集本身不提供与之配套的高精度地图，本文通过OpenStreetMap这一开源的地图数据平台进行车道线的先验信息提取，所提出的方法仅使用道路的朝向信息作为约束为后续的目标跟踪提供增强，而不需要考虑车道线在空间中的位置、车道线的个数、宽度等信息。这种方法具有良好的地图数据类型兼容性，而且在使用更准确的高精地图时会达到更好的效果。

本文的系统架构如图2所示，首先通过车辆定位信息首先获取前方道路的OSM地图数据，然后对激光雷达点云数据中的车道线信息进行提取，并结合车道线与OSM地图进行道路建模以获取道路的朝向信息作为车辆的朝向约束；此外，PVRCNN方法用于对激光雷达点云数据进行车辆的检测，检测的车辆位姿结果与朝向约束共同用于车辆跟踪。

图2 本文的系统架构Fig.2 System architecture of the proposed method

1 基于数字地图融合的道路建模

1.1 基于反射率的车道线特征提取

用于KITTI数据集制作的激光雷达为Velodyne的64线激光雷达，所得到的点云数据包含三维坐标以及反射率等4个维度的信息。其中，反射率信息能够在一定程度上反映物体的表面特性，道路上的沥青等黑色物体的反射率低，而车道线标记等白色物体的反射率高，因此本文采用激光强度信息来提取车道线[26]。

车道线的主要组成是虚线，照射到车道线上的点云数量较少，而场景中的车辆又会对车道线产生遮挡，使车道线上的点云变得更加稀疏。因此，从单帧点云中获取车道线信息较为有限，如图3(a)所示，对后续的车道线拟合造成了较大困难，针对这一问题，本文采用多帧点云叠加的方式提高密度。根据KITTI数据集提供的传感器标定参数和车辆定位信息，首先，根据标定参数将道路点云从激光雷达坐标系转到车辆坐标系；然后，根据定位信息将其转换到世界坐标系；最后，对世界坐标系下的点云进行叠加。多帧点云叠加的结果如图3(b)所示。由图3(b)可以看出，叠加后的点云相比于单帧点云更加稠密，车道线比之前也更加清晰。

激光雷达点云的反射率强度值，不仅与物体表面的反射率有关，也与物体距离、激光入射角有很大的关系[27]，因此会导致不同线束的激光照射到同一车道线上产生不同的反射率强度值。为了保证同一车道线上点云强度值的一致性，本文引入一种无监督的点云强度修正方法[28]，为每一个激光束计算了一个响应函数，处理其中的非线性关系。将叠加后的点云投影到鸟瞰图上的二维栅格中，其中每个栅格存储了该处点云的强度值和激光束编号。在二维栅格中寻找所有包含激光束为i并且强度值为I的点的栅格，然后计算这些栅格中除了属于第i个激光束以外的所有点强度的平均值，并将其作为真实的反射率强度值。对于每一个激光束返回的每一个强度值均进行上述计算得到真实强度值，同时将这种对应关系存储在一个二维查找表中方便后续查找。本文所使用的三维激光雷达为64线，而强度返回值的取值范围为0～0.99、分辨率为0.01，因此二维查找表的大小为64×100。基于这个查找表，能够将每个点根据其所属的激光束和强度值进行查表，使用表中存储的强度值对原始的强度值进行修正。点云强度标定的结果如图3(c)所示，标定后的车道线与地面之间的反射率强度对比更强、噪声更少，有助于车道线特征的提取。

车道线与地面之间的反射率强度值在经过强度标定后呈现较为明显的二分类趋势，因此只需要确定一个强度阈值就足以进行分割。对于强度阈值的确定，本文采用大津法[29]对三维点云投影到地面的二维图像进行处理，将每个点的反射率强度值转化为像素值，求解出强度阈值。最终依据该阈值对点云进行二值化分割，得到反射率强度较高的点云，并将其视作车道线。分割结果如图3(d)所示，道路上的车道线均被正确检测出来。由于不同场景下道路和车道线的材质不同，道路磨损情况也不同，因此需要对数据中的每一个场景单独进行一次强度标定和阈值计算。

1.2 车道线与地图的融合建模

车辆所在场景周围的数字地图是依据车辆的导航信息从OpenStreetMap(https:∥www.openstreetmap.org/)中得到的。图4展示了KITTI数据集中某一场景对应的OSM数据，其由3个基本单元组成：节点(nodes)、路(ways)和关系(relations)。其中，节点包含了地理坐标点的GPS坐标和可用标签列表；路由2～2000个节点构成，主要用于表示道路、河流、铁路等；关系用于描述两个或者多个基本单元的关系，以形成更加复杂的结构。

OSM中每个道路所包含的节点数量由道路的复杂度决定：通常直道由2、3个点组成，而弯道则可能由数十个点组成。通过激光雷达点云数据提取的车道线特征点和OSM数据都相对于世界坐标系，因此只需要将OSM数据中除了道路以外的其他几何信息删除，然后投影到车道线特征点上即可，结果如图5所示，其中黄色实线为OSM中的道路，白色点云为车道线特征点，三原色坐标轴中的红色直线指向车辆前进方向。图5中显示了自车前后25 m范围内的车道线特征点，其中自车前方的车道线特征点非常稀疏、有效距离只有10 m左右，但是OSM道路不存在视距限制，因此将其作为车道模型，既与道路形状高度一致又不会产生过拟合的问题。

由图5可以看出，OSM的道路形状与真实的道路形状有一定的一致性，但也存在着误差，为了消除误差，需要对OSM道路进行旋转平移变换。由于车辆目标一般是在二维平面上运动，因此本文的旋转平移忽略了高度方向上的变换。与无人驾驶中使用的高精地图不同，OSM中不包含实际场景中的车道级信息，而只包含了道路的几何形状。本文仅使用道路的朝向信息来增强目标跟踪效果，不考虑车道线在空间中的位置关系，因此平移变换也被省略，而只需进行旋转变换，最终使OSM道路与特征提取的朝向一致即可。

(1)

(2)

式中，oi,j为第i段中第j簇所代表的车道线的主方向；ci为第i段中聚类出的簇的个数。车道线建模的结果如图6所示，其中不同颜色的点属于不同的段。与图5相比，经过旋转后的OSM道路与真实道路基本一致。

2 融合道路模型的车辆跟踪

车辆的跟踪方法基于卡尔曼滤波框架[15]，包含目标状态的预测与更新两部分，但由于空间中存在多个目标车辆，每次进行目标状态更新时需要将预测结果与当前帧的检测结果进行对应，因此在预测与更新过程中引入数据关联模块进行目标的对应。在完成基于检测结果的目标状态更新后，会将目标的朝向状态依据车道约束再次进行更新，完成当前帧的车辆跟踪。整个跟踪任务的流程如图7所示，分为4个主要部分，其中数字编号对应的是本节中的小节编号。

图3 车道线点云结果Fig.3 Lane marker detection result from point cloud

图4 KITTI数据集中某一场景对应的OSM数据Fig.4 The OSM data of a scene in KITTI dataset

图5 OSM道路投影到车道线特征点的结果Fig.5 The projection of OSM lane data on the lane marker features

图6 与地图融合后的道路建模结果Fig.6 The road modeling result by fusing the lane marker detection and digital map

图7 车辆跟踪算法Fig.7 The multi-vehicle tracking algorithm

2.1 目标状态预测

在三维目标检测任务中，目标表示为(x,y,z,l,w,h,θ)，其中(x,y,z)为目标在三维空间中的位置，(l,w,h)为目标的长宽高，θ为目标的朝向。在城市道路环境中，目标一般沿着地平面运动，z方向的运动可以忽略。同时在目标跟踪过程中，目标的长宽高变化不明显、无须对其进行滤波。因此，由检测任务得到的目标状态量即为(x,y,θ)。

(3)

对于目标车辆的运动，状态向量的微分表达式为

(4)

假设离散的时间步k和连续时间值tk相关，离散的时间步k+1与连续时间值tk+1相关，tk和tk+1之间的时间差为Δt，则状态转移方程为

(5)

对式(5)中的积分进行求解，可得

(6)

(7)

(8)

由于CTRV模型的状态转移方程是非线性的，因此需要利用泰勒展开对非线性模型进行近似，计算雅克比矩阵J，这里将角标k移除掉以减少公式长度

(9)

最终得到扩展卡尔曼滤波的预测方程则是

Xk+1|k=g(Xk)+E

(10)

Pk+1|k=JPkJT+Q

(11)

式中，J为上述状态转移方程的雅可比矩阵；P为状态协方差矩阵，其初始值一般由人为指定；Q为过程噪声协方差矩阵。Q的计算方式为

(12)

2.2 目标数据关联

由于场景中一般存在多个目标，需要通过数据关联将预测结果与检测结果进行匹配。本文选择全局最近邻进行数据关联，采用数据关联矩阵描述目标关联问题，采用匈牙利算法寻找最优匹配。数据关联矩阵是一种通过二维矩阵来描述两帧目标关联问题的方法，其存储的是目标之间的相似度[33]。使用当前测量构建维度为m的检测列表，上一帧预测的目标构建维度为n为预测列表，则数据关联矩阵

(13)

式中，C为数据关联矩阵；元素cij表示第m个检测目标与第n个预测目标的相似度，该相似度通过两个目标包围盒的三维交并比(intersection of union，IoU)来计算。由于目标在时间序列上的唯一性，对于第i个检测目标，最多只有一个预测目标与之对应，同样对于第j个预测目标，最多只有一个检测目标与之对应，因此数据关联问题也可以被看作线性分配问题。优化目标就是找出数据关联矩阵中的一组对应关系使相似度的和最大，其数学形式为

(14)

式中，wij为置换矩阵中的元素，wij=1表示第i个检测目标和第j个预测目标是同一个目标，wij=0表示第i个检测目标和第j个预测目标不是同一个目标。对于上述线性分配问题，采用匈牙利算法[34]进行求解便可获取相邻帧间的数据关联矩阵，得到检测结果间的最大相似度对应关系。依据这一关联矩阵，能够将每一辆车在的预测结果与正确的检测结果相匹配，为后续状态更新过程提供数据一致性保障。

2.3 目标状态更新

基于数据关联的结果，提取同一车辆的预测结果与检测结果，并进行目标状态的更新，此时的更新方程为

Kk+1=Pk+1|kHT(HPk+1|kHT+R)-1

(15)

Xk+1=Xk+1|k+Kk+1(zk+1-HXk+1|k)

(16)

Pk+1=(I-Kk+1H)Pk+1|k

(17)

式中，z为通过PVRCNN方法进行目标检测得到的测量向量；H为测量矩阵；R为测量噪声协方差矩阵，由传感器的用户手册得到；K为卡尔曼增益；I为单位矩阵。式(15)、式(16)、式(17)构成了本文中目标跟踪算法中的卡尔曼滤波更新流程。

在目标距离较远时，由于检测结果的不稳定导致目标状态更新时存在较大误差，而且这种误差会始终存在，从而导致目标跟踪的轨迹产生抖动。因此2.4节中在扩展卡尔曼滤波的基础上，利用道路约束作为伪测量对车辆的朝向进行二次更新，以提升跟踪效果。

2.4 目标朝向状态更新

本文假设道路上行驶的车辆基本都是沿着车道线行驶，车辆的真实朝向与车道线的朝向相同。如图8所示，θo为车道线朝向，θd是目标检测的朝向，朝向误差δθ=θo-θd。以目标底边的中心点为坐标原点，车道线朝向为x轴，车道线垂直方向为y轴，建立目标坐标系，则虚拟目标真实框(图中虚线框)与目标检测框(图中实线框)位置上的误差为

(18)

式中，l为目标包围盒的长度；δx和δy分别为两个方向上的位置误差，同时假设目标真实框与目标检测框的旋转中心为目标坐标系的原点。

图8 目标位置误差计算Fig.8 Object position error calculation

在计算得到目标朝向误差和位置误差后，根据误差的大小对测量噪声协方差矩阵Ro进行动态调整

(19)

(20)

式中，R11等4个参数为测量噪声协方差矩阵Ro左上角的两维方阵。图9是两个不同朝向误差目标对应的位置噪声协方差示意图，可以看出对于不同朝向误差的目标，本文所提出的方法可以对其位置噪声进行实时估计，更加符合真实情况。

图9 不同朝向误差目标位置的噪声协方差Fig.9 Noise covariance of object position with different orientation errors

由于本文方法假设目标朝向与车道线的方向相同，因此将依据道路建模所得的道路方向作为测量结果对目标的朝向θ进行更新

(21)

Xo,k+1=Xk+1+Ko,k+1(zo,k+1-HoXk+1)

(22)

Po,k+1=(I-Ko,k+1Ho)Pk+1

(23)

式中，zo=θo是由车道线方向得到的伪测量；Ho=[0 0 1 0 0]为测量矩阵；Ro为车道线方向约束的测量噪声方差，表征了车道线方向与目标真实朝向的误差分布。为了确定测量噪声方差Ro的取值，本文对训练集中目标真实朝向与车道线方向的误差进行了统计，并利用高斯分布对误差统计结果进行拟合，最终得到的高斯分布的方差就是测量噪声方差Ro。

本文所述的目标跟踪算法的扩展卡尔曼滤波流程如下。

(1) 通过式(10)和式(11)根据上一帧的状态进行预测。

(2) 通过式(15)、式(16)、式(17)根据目标检测结果对目标的状态进行更新。

(3) 通过式(21)、式(22)、式(23)根据车道线约束的伪测量结果对目标的朝向进行更新。

(4) 迭代上述步骤(1)—步骤(3)进行持续跟踪。

3 试验结果与分析

3.1 试验条件与评价指标

本文所选用的试验数据来源于KITTI数据集，是自动驾驶领域的最为权威的数据集之一，其中的道路场景具有较好的代表性。由于本文所提出的方法是通过车道线提供的朝向约束提升远距离车辆跟踪效果，而在一般路口处没有车道线覆盖且此时的检测任务主要集中在近处，因此试验基于KITTI数据集中典型的城郊道路和高速道路展开。全部数据共约800帧：其中0018是一个弯道场景，0020(前500帧)是一个直道场景。

在试验分析上，本文主要采用两个主要的评价指标进行分析，首先是KITTI数据集所采用的多目标跟踪准确度(multiple object tracking accuracy,MOTA)指标，主要用于车辆跟踪算法准确性的评估，其定义如下

(24)

式中，TP(true positive)表示检测为正样本并且真值也是正样本的个数；FN(false negative)表示检测为负样本但是真值是正样本的个数；FP(false positive)表示检测为正样本但是真值是负样本的个数；IDS(ID switch)表示目标ID切换的次数，对于一个良好的目标跟踪系统，需要对目标进行持续跟踪，同一目标在不同帧之间的ID应该保持一致，因此IDS越多指标越低。

另一个评价指标采用平均位移误差(average translation error，ATE)[35]，主要用于车辆跟踪轨迹误差的评估，其定义为

(25)

3.2 定量试验与分析

表1为两个车段场景的MOTA对比，两个场景的MOTA都得到了提升，特别是车辆比较多的0020场景，车辆之间相互遮挡严重，也会导致点云数较少，在基于车道朝向先验信息对目标跟踪进行增强后，跟踪的效果提升效果明显。

表1 两个路段场景的MOTA对比

ATE评价结果见表2，加入车道朝向先验后，两个场景的ATE指标均有所降低，在0018场景中ATE降低了0.013 8 m，而在0020场景中ATE降低0.028 5 m。

表2 两个路段场景的ATE对比

不同距离下的ATE指标见表3。对于0018场景，ATE指标的降低基本随着距离的增加而增大，当目标距离为50～60 m时ATE指标降低最多为0.025 3 m，由于该场景60 m以外的车辆未标注，因此用N/A表示。对于0020场景，其ATE指标的降低幅度大于0018场景，并在60～70 m时达到最大为0.081 6 m，验证了本文方法对于远距离点云稀疏车辆的优势。

表3 不同距离下两个场景的ATE对比

对目标跟踪方法的运行时间进行统计，结果见表4。虽然加入车道特征进行增强后，方法的运行时间有所增加，但是还是满足实时性要求。同时，本节提取的车道结果还可以用于车道保持等其他模块，具有较高的实用价值。

表4 两个路段场景的运行时间对比

由上述结果可知，本文提出的车辆跟踪增强方法不仅可以提高多目标跟踪的准确度，同时可以减少目标跟踪轨迹与真实轨迹的误差，特别对于远距离点云稀疏的车辆，其误差降低较为明显。虽然在运行时间方面，本文相比之前增加较多，但是可以满足实时性要求。

3.3 定性试验与分析

为了对增强前后车辆跟踪轨迹进行直观分析，对其进行了可视化。图10显示了目标直行场景下的对比结果。其中红色曲线为道路朝向先验增强前的目标跟踪轨迹，绿色曲线为增强后的目标跟踪轨迹。以776号目标为例，增强前目标跟踪轨迹抖动较为严重，而增强后的目标跟踪轨迹较为平滑。同时774号目标原本的跟踪轨迹在中间位置存在突变，但是基于本文方法进行增强后，目标跟踪轨迹更加平滑。

图10 直行车辆在增强前(红色)与增强后(绿色)的跟踪轨迹对比Fig.10 Comparison of tracking trajectory before (red) and after (green) enhancement for straight driving object

在真实的道路环境中，目标不会只沿着车道线直线行驶，而是存在换道、超车等情况。因此，本文对目标换道情况下的目标跟踪轨迹进行了对比，结果如图11所示。其中，102号目标从最右边车道换道至中间车道，而在目标跟踪增强前后的车辆轨迹基本一致。因此，本文方法在利用车道朝向先验对目标跟踪进行增强的同时，不会影响目标换道等复杂运动。

图11 变道车辆在增强前(红色)与增强后(绿色)的跟踪轨迹对比Fig.11 Comparison of tracking trajectory before (red) and after (green) enhancement for lane-changing object

4 总 结

本文针对基于激光雷达的车辆跟踪方法在远距离处精度下降以及轨迹抖动等问题展开研究，提出了一种基于数字地图中车道朝向先验的车辆跟踪增强方法。首先，对多帧叠加后的点云进行无监督强度标定，通过大津法计算强度阈值并提取车道线特征点，并从车辆周围的OSM地图数据中提取车道信息，进行道路建模获取道路的朝向约束；然后，通过PVRCNN方法对点云中车辆进行目标检测，获取车辆的位姿数据；最后，建立扩展卡尔曼滤波器进行数据关联和目标跟踪，利用目标检测结果进行车辆状态更新之后又进行了基于道路朝向约束的车辆朝向状态更新，从而提升目标跟踪的精度、增强预测结果的稳定性。

基于KITTI数据集的试验结果表明，本文所提出的方法在多目标跟踪(MOTA)指标上提升了至少0.33%，而平均位移误差(ATE)指标则优化了0.014 m，同时，在最远距离(约60 m外)的跟踪误差降低了0.08 m以上。本文所提出的方法充分利用了智能驾驶车辆所依赖的数字地图，将车道线检测的结果与地图中的道路先验信息进行融合，将地图与车辆感知功能进行了有机结合。未来工作将会集中于利用道路边界等其他高精地图中的道路信息，进一步提升跟踪系统的精度和稳定性，并将该方法推广至更为复杂的道路环境当中。