【文献阅读】Sensor Fusion Algorithm Design in Detecting Vehicles Using Laser Scanner and Stereo Vision

利用激光扫描仪和立体视觉检测车辆的传感器融合算法设计

摘要
众所周知，激光扫描仪在探测障碍物的距离和速度方面比立体视觉具有更好的精度，而立体视觉比激光扫描仪更能分辨物体。
通过传感器融合方法，可以最大限度地发挥每个传感器的这些优点，从而准确地检测出前方的障碍物。
本文针对激光扫描仪和立体视觉传感器之间的目标匹配问题，提出了一种高层次的传感器融合方法。
时间同步、对象年龄和重新排序算法是为对象的鲁棒跟踪而设计的。
提出了一种确定激光扫描仪加工延时的延时更新算法。利用卡尔曼滤波对每1毫秒的扩展激光扫描仪数据进行预测，并与每66毫秒的立体视觉数据进行匹配，建立了一个描述传感器之间目标匹配相似性的代价函数，并选择最佳匹配候选函数作为最小代价函数。在各种机动情况的现场试验中，对所提出的匹配算法进行了实验验证。
关键词：传感器融合，激光扫描仪，立体视觉，时间延迟，对象匹配，卡尔曼滤波器。
Index Terms—Sensor fusion, laser scanner, stereo vision, time delay, object matching, Kalman filter.

一、引言

随着对先进驾驶辅助系统（ADAS）的研究不断深入，各种传感器在识别车辆周围环境中的应用起到了关键作用。在各种传感器中，视觉传感器最受欢迎的原因不仅是它们能够识别物体类别，而且可以以低成本大量生产，而且还因为立体视觉可以计算物体的距离[1]。然而，距离计算的精度不如其他传感器的精度[1]，[2]。雷达能精确测量物体的距离和相对速度，在昼夜恶劣天气下性能稳定。但是，它无法测量物体的大小，因此无法区分障碍物的形状[1]。
激光扫描器可以比其他传感器探测到视野更宽的物体，由于其扫描能力，它可以测量物体的尺寸[3]。它可以精确地测量相对距离和速度，但其成本可能很高，这取决于用于ADAS的规格。
由于各传感器的优缺点，传感器融合结合上述传感器已得到积极发展，以提高周围障碍物的检测可靠性。特别是，视觉和雷达的传感器融合已成功地应用于各种ADAS装置，如ACC（自适应巡航控制）、AEB（自动紧急制动）等[2]、[4]–[12]。
例如，车辆阴影、尾灯和车道通过彩色单目检测，前方障碍物通过雷达传感器融合识别[5]。
研究了利用灰度单目和雷达进行传感器融合，通过缩小视场范围来减少计算时间和减少误报[6]–[8]。（类似于On Road那篇文章）
在传感器融合中采用随机抽样共识[9]、神经网络多模型滤波[10]和交叉校验法[11]实现高精度、高可靠性。

通过立体视觉和雷达的传感器融合识别目标的类别和位置，但由于立体视觉的距离精度不高，与目标的距离高度依赖于雷达数据[2]，[12]。
近年来，由于低端激光扫描仪价格适中、规格适中，立体相机和激光扫描仪的传感器融合引起了人们的关注。激光扫描仪可以提供雷达所能提供的精确距离和速度数据，它们可以识别物体的大小，这使得激光扫描仪在ADAS应用中具有吸引力。Perrollaz等人[13]对激光扫描仪和立体摄像机进行了数据融合研究，考虑传感器坐标和视觉像素尺寸。为了实现激光扫描仪和立体视觉相机的数据融合，对三维激光扫描仪的运动和旋转进行了研究[14]。Evangelidis等人[15]提出了使用高分辨率场景建模的测距立体融合。还进行了原始数据融合和目标数据融合的结合[16]。然而，这些研究需要大量的计算复杂性，因为它们处理原始传感器数据，如激光扫描仪的云点和视觉摄像机的像素。此外，通过通信交换原始数据会导致网络中的繁重工作负荷。

或者，传感器融合尝试匹配来自两个传感器的目标数据，而不是原始数据。高层次数据融合需要两个预处理系统：一是设计两个采样周期不同的传感器之间的同步过程；二是对每个传感器进行延时补偿，以调整目标数据的定时。
例如，时间延迟被认为是伯努利随机变量，在传感器融合中被建模为马尔可夫链[17]，[18]。在数据传输中假设随机时间延迟，并使用卡尔曼滤波器对其计算误差进行补偿[19]。利用两个传感器之间的通信间接计算了惯性和视觉传感器的时间延迟[20]。上述研究试图间接确定时间延迟，而不是直接计算。它们的准确性可能会受到物体数量和周围环境的复杂性的影响。

本研究以激光扫描器与立体相机之高阶感测器融合为研究对象，进行激光扫描器得到的对象与立体视觉检测到的目标的匹配。特别是，在传感器融合设计中考虑了两个传感器的时间和（specific）方面。
考虑到每个传感器的目标数据处理所带来的时间延迟，并开发了其直接计算算法。由于激光扫描器和立体摄像机通过CAN（区域控制网络）连接，因此利用时间同步来确定异步通信网络的时间延迟。
对象年龄和重新排序算法（Object age and reordering algorithms）是为对象的鲁棒跟踪而设计的。由于激光扫描仪和立体照相机的采样周期不同，因此可以在采样数据之间预测每1毫秒一次的扩展目标数据的位置估计方法。此扩展位置数据用于每隔66毫秒与相机数据匹配的对象。从目标匹配的角度出发，将两个传感器目标之间的相似性表述为一个包含坐标误差、速度误差和宽度误差的成本函数。通过最小化成本函数得到最佳的匹配候选，然后应用大小和速度等约束条件，所提出的算法的系统结构如图1所示。将所提出的匹配算法和传感器应用于试验车辆，并在市区、城市和公路等不同的驾驶场景下对其性能进行了验证。

图1。提出的系统结构匹配算法。

二。传感器配置分析

首先分析各传感器的配置，对传感器数据进行处理，实现对障碍物的精确跟踪。
数据处理部分设计了 目标数据重排序、目标年龄和延时更新等算法。目标数据如位置、速度和大小是通过传感器内部处理生成的输出数据。

A.重新排序算法（Re-Ordering Algorithm）
为了根据传感器获取的信息跟踪同一物体的位置，需要识别并不断更新来自同一物体的位置和速度数据。
然而，本研究中使用的激光扫描器[21]在每次取样时以扫描顺序提供目标数据。
在下一步中，前一个对象可能会消失在视野之外，新对象可能出现在视野内，这使得仅从传感器数据中跟踪相同的对象非常困难。图2显示了通过CAN提供的激光扫描仪数据的示例。在下一步（n+1），对象4和对象5被重组，对象1从FOV中消失，而新对象6和7突然出现在FOV中。因此，连续步骤的扫描器数据可能不匹配，无法直接实现对相同对象的跟踪。

图2。激光扫描仪数据示例。

为了解决这一问题，设计了一种考虑目标位置和速度的激光扫描数据重排序算法。需要注意的是，在一个采样周期（80毫秒）内，物体的位置会有一点变化，其下一个位置应该接近上一个位置。因此，本研究所建议的重新排序算法比较了每个采样周期后物体之间的距离。

图3。提出的重排序算法的概念。

图3说明了所提出的重新排序算法的概念。
使用An、Bn、和Cn表示前一步检测到的车辆对象，而B’n+1和C’n+1为当前步骤(n +1)检测到的对象(车辆)。
假设车辆An在当前阶段已经从FOV中消失。

对于对象的连续性检查如下：
首先，将以前检测到的对象，即被定义为中心对象（pivot object），以此为基础，构建一个新检测到的对象列表，以查找最近的对象。
结果，B’n+1被定义为了候选对象。中心对象和候选对象之间的距离定义为最小距离候选者（MinDistCand），对应于图三中的LAB’ 。
在该程序中，根据纵向和横向速度确定可能位置的界限，如图4所示。边缘参数αn和βn通常设置为大于1。第二，对于候选目标，以B’n+1为基础，比较与上一步检测到的物体之间的距离。其中，最近的一个被定义为"MinDist"，对应于图三中的LB’B 。类似的界限如图4所示，适用于本程序。
如果“mindistcand”和“mindist”在给定的阈值内相同，那么中心对象、An和候选对象B’n+1，被认为是同一个物体。

图4.在一个步骤中车辆轨迹的约束。

上述提到的阈值 由相对速度、采样周期和测量误差决定。当传感器连续检测到同一物体时，前一时刻与当前时刻的距离数据应一致。因此，选择“MinDistCand”和“MinDist”的阈值需要考虑距离的变化。假设传感器和物体之间的相对速度在采样期间（例如80 ms）是恒定的，则阈值可以表示为由于相对速度和采样期间而产生的距离差。

式中，rin和vin是在n时刻测量的第i个物体的距离和相对速度。τ和ε传感器分别是采样周期和传感误差。

在图3中，LAB’的长度和LB’B，那么An和B’n+1被判定为不同的对象。否则，将Bn和Cn作为主对象重复相同的过程，以连续找到相同的对象。

当Bn和B’(n+1)两个对象被认为是同一个目标时，更新B’(n+1)对象的位置、速度、图像等数据，并将其顺序重新排列为与前一步相同的数据序列。
与此同时，新对象数据首先被放置到空位置，如图5所示。

B.对象年龄法
将目标年龄法分别应用于激光扫描仪和双目相机数据的跟踪结果。
由于道路和制动引起的意外的俯仰和滚动运动会导致激光扫描仪和视觉传感器中的目标数据错误跟踪。对象年龄统计标识同一对象的时间步数（time steps），并且每次连续检测同一对象时，时间步数都会增加。通过这样做，在FOV内的许多时间步骤中存在的对象可以具有较大的年龄编号，而在FOV内短暂错误出现的对象将具有较小的年龄编号。为了提高目标跟踪性能，只考虑阈值年龄段以上的目标数据作为跟踪候选对象，对目标进行重新排序处理。图6说明了年龄阈值如何分别用于激光扫描仪和立体视觉传感器的实验结果。

对象年龄方法的结果。

C.延时更新算法
为了使激光扫描仪和双目相机的目标数据同步，需要考虑多个因素，如异步车载CAN(控制区域网络)网络、传感器的数据处理时间等。图7为CAN网络与传感器和车辆数据的配置。在RCP (Rapid Control Prototyping)设备中实现了传感器融合算法，并利用CANcaseXL进行数据记录。

配置带传感器的CAN网络。

一般来说，多传感器的传感器融合需要同步和位置预测[22]。激光扫描仪和双目相机在处理目标数据时有不同的规格。本研究所使用的激光扫描器[21]和立体相机的采样周期分别为80毫秒和66毫秒。对于激光扫描器，基于原始数据的处理时间会造成较大的时间延迟，这反过来会根据相对速度引起显著的位置误差。为了防止这个错误，时间戳和位置数据被存储在一起并在数据处理单元中使用。该步骤对于立体摄像机的传感器融合也很重要，因为两个传感器的检测到的目标信息应在相同的时间基础上进行比较。本研究分别采用时间同步法和延迟更新法解决了传感器中的时间漂移和延迟问题。

1）时间同步：由于传感器中使用的晶振频率在某种程度上不相同，其定时器也不相同。这种不一致性导致传感器之间的时间差，这是导致定时器漂移的主要原因。由于错误结束，计时器漂移随时间而增加。为了实现各传感器之间的时间同步，需要对各传感器的定时偏移进行一定的计算和补偿。实际上，可以通过在主设备和从设备之间交换CAN消息来设置时间标准。例如，主设备（例如RCP）获取每个传感器的漂移率信息，并计算计时器漂移估计。当估计值可能超过允许误差范围时，RCP装置通过CAN向从属装置（如传感器）发送时间同步信息，进行时间同步。

图8。RCP装置和传感器之间的同步。

图8描述了基于测量时间和时间漂移误差的同步概念。最大时间漂移误差取决于时间漂移率和时间同步周期。时间漂移率是由晶体振荡器的精度引起的定时器误差率。最大时间漂移误差需要受允许时间限制的限制。同步间隔越短，时间误差越小，但会给通信带来负担。本研究中的时间同步间隔根据扫描仪测试选择为2秒。

每当激光扫描器接收到来自RCP设备的时间同步信息时，时间重置为零以计数相对计时器。即使CAN是异步网络，时间同步消息也被设置为在CAN总线中具有最高优先级。由于设备之间的同步消息DCAN,SYNC导致的CAN延迟平均约为185微秒，对于传感器融合而言可以忽略不计。

2）全延时测量：在激光扫描器的数据处理中，由于各种原因产生延时：扫描周围障碍物，获取并存储扫描数据，将原始数据转换为目标数据，将目标数据传输到主设备等。在节中，激光扫描仪的总延时分解如图9所示。

图9。激光扫描仪中的延时元件

在时间TTxSync,RCP时，基于RCP计时器，RCP向激光扫描仪发送同步消息，扫描仪计时器在收到消息后立即重置为0。
假设激光扫描器在Tsnap,sensor开始扫描，经过数据处理和通信后，RCP在TRxData,RCP时刻接收到目标数据。
因此，Tsnap,sensor和TRxData,RCP之间的差异代表了从RCP的角度来看的时间延迟，应该对准确的时间数据进行补偿。
上述关系可表示为以下方程：

或者

3)延时更新的实验结果：
实验结果的总时间推迟了一个测试车辆配备一个RCP设备(如μ-AutoBox)和一个激光扫描仪(例如ibeo LUX)。
激光扫描仪接收到同步信息后，将扫描到的数据与定时器信息一起处理并传输到RCP。
总延时DData是通过比较RCP上的数据接收时间、TRxData,RCP和数据采集时间 和 激光扫描仪上的数据采集时间、Tsnap,sensor来确定的。

图10。延时测量的实验结果。

图10显示，在扫描对象的数量几乎不变的前33秒期间，总时间延迟约为55毫秒。但是，由于附加的物体在33秒后出现在视野内，时间延迟也会增加。由于在该扫描仪中被扫描到的物体的最大数量设置为40，因此基于该实验，该激光扫描仪和CAN连接的总时间延迟可以假定为60毫秒。同样的程序也适用于立体视觉摄像机，结果表明，立体摄像机的总延时几乎为200毫秒。处理时间比采样时间长的原因是由于并行计算。图像处理的复杂性要求较高的计算能力，并行计算使处理器能够同时处理多个图像。

**

三、目标位置估计及延时补偿算法

**
激光扫描仪[21]和立体照相机的采样周期分别为80毫秒和66毫秒。采样频率的不一致性使得传感器融合非常困难，因为无法同时比较传感器检测到的障碍物。一个很小的时间差，如几十毫秒，可能导致车辆产生一米的位置误差。另外，每个传感器在数据处理和通信中都有不同的延时，使得传感器的融合更加困难。
本文提出了一种激光扫描仪数据的位置估计方法，在80毫秒的采样周期内，可以估计出每1毫秒的扩展目标数据。这样，每66毫秒的双目相机数据可以与精确定时的扩展扫描仪数据进行比较。图11说明了所提出的估算方案的概念。

图11。不同采样周期的两个传感器的时间一致性。

a.激光扫描仪的数据处理
激光扫描仪的估计算法由两部分组成：每1毫秒的轨迹预测和时间延迟引起的时间偏移。假设在一个采样周期内加速度是恒定的，则可以用运动模型来表示物体的运动轨迹。

其中τ和a分别是采样周期和加速度。基于上述关系，每1毫秒第i个目标的轨迹可以分别描述为x和y方向的离散状态空间形式。

其中，
dt 预测时间步长（本研究中为1毫秒）
xi,yi 第i个物体在x和y方向的真实位置
vix,viy 第i个物体在x和y方向的真实速度
aix,aiy 第i个物体在x和y方向的加速度

测量方程用激光扫描仪传输的数据和总延时来表示。

其中，

上标代表激光扫描仪上的物体。

xiscanner和 yiscanner分别是第i个物体在x和y方向上的扫描位置。

vix,scanner 和 viy,scanner 分别是第i个物体在x和y方向上的扫描速度。

Wk和Vk分别是干扰噪声和测量噪声，它们被建模为高斯噪声。

在上述方程中，DData是激光扫描仪的总延时，每次扫描数据传输到RCP时，其值都会更新。由于扫描数据每80msec接收一次，因此应用卡尔曼滤波器[23]估计第i个目标在每1毫秒的位置和速度。
在（4）的基础上，首先对预测阶段的状态向量和误差协方差矩阵进行了预测。

其中，“~”和“^”的上标分别表示预测值和估计值。预测阶段每1毫秒工作一次，当激光扫描仪数据未被接收到时，预测结果将取代估计数据。

相反，如果更新了激光扫描仪数据，则在校正阶段使用预测值和测量值估计物体的准确位置。卡尔曼滤波器增益Kk计算为[23]

估计了状态向量和误差协方差矩阵。

b.立体视觉数据处理
立体视觉在图像感知时间和传输到RCP的时间之间有200毫秒的延时。由于从双目相机接收的对象数据表示在接收时间之前200毫秒处的对象位置和速度，因此应通过预测将其向前移动到当前时间。立体视觉中的第j个物体的运动轨迹也可以用运动模型来描述，并假设在采样期间加速度是恒定的，将其表示为状态空间模型。

其中，
dt 立体视觉采样周期（66毫秒）
xj,yj 第j个物体在x和y方向的真实位置
vjx,viy 第j个物体在x和y方向的真实速度
ajx,ajy 第j个物体在x和y方向的加速度

利用第j个目标的传输数据和立体视觉的总时延来表示测量方程。

其中，

上标j表示立体视觉中的第j个对象。
DData是立体视觉传感器的延时（200毫秒）。
WL和VL分别是干扰噪声和测量噪声，并被建模为高斯噪声。
在（13）和（14）的基础上，设计了卡尔曼滤波器来预测当前第j个目标的位置和速度。

首先，预测状态向量和误差协方差。

其中，“~”和“^”的上标分别表示预测值和估计值。kalman增益kl，i计算如下[23]。

最后，更新状态向量和误差协方差矩阵。

C.扩展对象数据的实验验证
为了验证激光扫描仪数据中的扩展目标轨迹和延迟时间补偿，在VT-HILS[24]实验台上进行了实验。如图12所示，VT-HILS由底盘测功机平台和轨道上的电缆操作物体组成。试验车辆的驱动轮位于两个滚柱的顶部，然后用固定带将车辆固定到位。激光扫描仪安装在试验车前保险杠上，扫描数据通过can通信传输到rcp装置。具有车辆形状的物体由电缆拉动，以沿着试验车辆前方的轨道跟随预设速度和距离。在VT-HILS设备中，障碍物的真实距离和速度是相对于试验车辆提供的。

图12。VT-HILS设备的实际视图[24]。

图13。估计算法的实验结果。 from corrected method从修正的方法

将与所提出算法的估计距离与图13中的真值进行比较。与预期的一样，由于时间延迟，从激光扫描仪收集的原始数据落后于真实值。此外，原始数据曲线显示由于80毫秒采样周期的逐步形状，并显示最大1.5米误差。与此相反，由于每1毫秒进行一次路径估计，使得与该方法的校正距离平滑，并且通过补偿更新后的时间延迟接近真实值。

**

四、目标匹配算法

**
基于激光扫描仪和双目相机的目标数据，设计了一种传感器融合算法来匹配每个传感器的相同目标。事实上，有的时候激光雷达会将花圃等物体误判成被测车辆，立体视觉会错误地计算出物体的距离。每一种传感器到的局限性都会导致对障碍物的错误匹配，在传感器融合中应该将这种可能性降到最低。

a.目标匹配中极坐标的选择

图14。扫描前面物体的几何结构。

坐标的选择与传感器的特性密切相关。虽然每个传感器的目标数据都是以笛卡尔坐标提供的，但在目标匹配中最好使用极坐标。如图14所示，激光扫描仪和立体照相机都通过光识别物体。在光线性假设的基础上，车辆D由于遮挡隐藏在车辆C的后面，两个传感器都无法检测到。

（毫无关联的转折，极坐标的好处是下面一段说明的）因此，极坐标可以为每一个视线中的物体指定唯一的角度，它比笛卡尔坐标在识别相同物体时提供更好的区分。

图15。扫描对象和视觉对象图像在极坐标系下。

图15显示了极坐标与笛卡尔坐标相比的优点。车辆s和对象f是由激光扫描器一次性扫描的结果。假设两个结果都被正确扫描，目标F表示路边被扫描的花坛。车辆v是立体摄像机同时检测到的目标，但包含距离误差。如图所示，来自摄像机的车辆V和来自扫描仪的对象F可以在笛卡尔坐标中匹配，因为视觉数据的车辆V比扫描数据的车辆S更接近对象F。相反，在极坐标系中，由于车辆的角坐标，车辆V与车辆S而不是对象F相匹配。如图所示，V车和S车之间的角差小于V车和F车之间的角差。尽管如此，应始终比较两个潜在物体的径向距离，使其处于一定的误差范围内。

图16。图像数据和扫描物体上的代表点（*）。

检测到的物体需要描述为极坐标中的一个点。如图16所示，激光扫描器提供具有纵向长度和横向宽度的盒状物体，立体视觉摄像头提供具有横向宽度和垂直高度的物体盒。为了一致地比较物体坐标，将代表点定义为最近或底部宽度的中心，并在水平面上获得其极坐标（r−θ）。

b.匹配算法设计

图17。对象匹配算法流程图。

为了使激光扫描仪扫描到的物体与立体相机拍摄到的物体图像相匹配，需要考虑物体的坐标和形状等因素。将激光扫描仪和立体摄像机检测到的物体的相似性表示为包含上述因素的成本函数。所提出的对象匹配算法的流程图如图17所示。
如(19)所述，在立体摄像机中检测到一个物体（如j* 物体）后，研究同一时刻激光扫描仪每个数据的相似性，并通过最小化i=1···imax 的成本函数来确定匹配的候选物体（如i* 物体）。

其中，
ri , θi 被扫描对象i的极坐标
vi 被扫描物体i的速度
wi 被扫描物i的宽度
imax 激光扫描仪的最大对象数。

下标i和j分别表示激光扫描仪的第i个对象和立体相机的第j个对象。Wθ和Wr表示极坐标系中物体的角度和距离匹配的权重。因为立体相机通常在距离上比在角度上包含更多的误差，所以在角度坐标上放置的权重比在径向距离上放置的权重大。Wvel和Wwidth分别是对象速度和宽度匹配的权重。即使来自激光扫描仪的物体宽度受视场和扫描方向的影响，也会比较两个传感器的物体宽度。速度上的权重选择得很小，因为来自立体相机的速度信息可能包含较大的错误。为了使（21）有意义，对权重进行规范化，使右侧的每个项具有相似的大小。

一旦使用（21）在激光扫描器数据中选择了候选对象（对象i*），则检查对象i*与同一时刻的每个立体相机数据的相似性，并且通过最小化j＝1···jmax的代价函数再次确定相反的候选对象（例如对象j+）。

其中jmax是立体摄像机检测到的最大对象数。如果由（22）确定的相对候选对象（object j+）与（21）中使用的对象j相同，则假定相机图像的object j和激光扫描仪的object i*是最佳匹配候选对象。此外，为了克服成本函数方法的局限性，采用了其他的约束条件来防止检测对象之间的不匹配。例如，当候选对象之间的欧几里得距离超过某个阈值时，它们将被排除在外。

欧几里得距离检查：如果激光扫描仪和立体视觉相机检测到相同的物体，则检测到的位置数据之间的差异被定义为“欧几里得距离”，并且应小于给定条件下的传感器误差之和。

其中，
D(i∗, j∗) 目标i和j之间的欧几里得距离
εscanner 激光扫描仪的距离误差
εvision 立体视觉的距离误差

车辆尺寸检查：
道路车辆的宽度一般小于3.5 m。当视觉图像中的候选对象被识别为车辆时，激光扫描仪所对应的对象需要宽度上的尺寸约束。
若j是车辆的尺寸，||Wi||小于等于测量的常规尺寸，这里Wi*是被扫描到的对象的宽度。

C.匹配年龄管理
与Section II-B节中的跟踪年龄方法一样，目标年龄概念也适用于鲁棒匹配性能。每当从图17的匹配算法中选择匹配的候选者时，匹配年龄增加到相应的候选者。通过激光扫描仪和立体摄像机的传感器融合，最终选择一定阈值年龄以上的匹配对象作为匹配对象。图18显示了匹配年龄过程的实验结果，其中一对匹配的候选者由于年龄较小而在40～45秒期间被丢弃。

图17。对象匹配算法流程图

图18。对象匹配年龄管理的结果。

**

五、实验对象数据

**

A.算法进程队列
提出的两部分传感器融合算法分别通过中断处理。

第一部分由以下串行任务组成：将CAN上来自传感器的信息转换成动态数据集。传感器数据以消息的形式传输，包括表头、数据和footer。数据采集过程，在接收到CAN消息时，将消息集转换为对象数据。数据转换完成后，RCP运行重排列算法，将原始对象数据与以前的数据进行比较，以跟踪相同的对象。此任务更新新传感器数据。然后，不管传感器数据的可用性如何，执行位置估计任务，以每1毫秒生成一次数据。利用卡尔曼滤波器对每个传感器的不同时间延迟和采样时间进行补偿，进行校正。最后一个任务是在获取立体视觉数据集后，根据校正后的数据进行目标匹配。

算法的第二部分与第一部分独立运行。每2秒执行一次时间同步算法，以补偿设备间的时间漂移误差，并计算RCP定时器的延时。另一个任务是当转换为对象数据完成时，确定RCP设备和传感器之间的处理时间延迟。传感器数据集包括传感器定时器中的延时信息，并基于RCP定时器分析时间信息。每个任务都是实时执行的，并通过快速控制原型装置（μ-autobox）进行验证。

B.实验验证
为了评估所提出的算法的性能，在一辆试验车的前面安装了立体视觉摄像机和激光扫描仪。在rcp器件（如μautobox）上实现了重排序、时延更新、位置估计、时延补偿和目标匹配算法。

图19。匹配前后检测到的对象的图例。

图19示出了从两个传感器检测到的对象的图例，其中支架（）和矩形（）分别表示立体视觉相机和激光扫描仪检测到的对象。一旦两个传感器检测到的对象匹配，括号和矩形就会改变颜色，并用虚线连接。图20展示了物体匹配的成功案例，即使立体摄像机检测到的物体在极坐标中包含几米的距离误差。这种匹配主要是由于检测对象的角坐标唯一性。

图20。对象匹配的成功案例。
在这里插入图片描述

并将该算法的性能与仅采用距离匹配而不进行延时补偿的算法进行了比较。验证是在城市驾驶情况下进行的，共有65个匹配案例。该算法对每一个匹配案例都进行了成功匹配，而仅使用距离匹配的算法在65个匹配案例中产生4个缺失案例和1个误判。比较结果见表二。
图21-23分别展示了城市和高速公路两种不同场景的匹配结果。特别是，在市区（downtown）驾驶过程中，所提出的算法成功地匹配了立体摄像机和激光扫描仪检测到的物体，即使存在许多干扰障碍物，如路障、花架、路边停放的车辆。

图21。商业区驾驶场景试验结果。

图22。城市驾驶场景试验结果。

图23、高速公路驾驶场景实验结果

六、结论
近年来，随着传感器和ecu数量的不断增加，基于主动安全的传感器融合技术在汽车工业中变得越来越重要。
在本研究中，激光扫描仪与立体相机视觉的传感器融合是为了探测障碍物而发展的，它考虑了两个传感器的时间和特定方面，例如采样时间和时间延迟。提出了传感器与快速控制原型（rcp）装置的同步方案，并进行了实验验证。分析了两种传感器在实际情况下的时间延迟，并在实验中得到了确定。由于激光扫描仪和立体摄像机的采样周期也不同（80 ms和66 ms），因此分别建立了包括时间延迟和采样时间的激光扫描仪和立体摄像机的车辆轨迹模型。特别地，基于激光扫描仪的模型，设计了卡尔曼滤波器来估计采样数据之间每隔1毫秒的扩展运动数据。通过这样做，立体视觉数据可以在每66毫秒的精确时间与扩展的扫描仪数据匹配。匹配算法的目的是比较立体摄像机的目标数据和激光扫描仪的扩展数据。通过最小化代价函数（包括目标间的坐标误差、速度误差和宽度误差）来确定匹配对象。此外，为了克服成本函数法的局限性，采用欧几里得距离检验和障碍物宽度检验方法。实验结果表明，该算法在市区、城市、公路等多种机动情况下的总精度可达99%左右。