CN104035071B - 融合雷达/摄像机物体数据和LiDAR扫描点的方法和装置 - Google Patents

Abstract

一种用于融合来自于车辆上的多个LiDAR传感器的输出的系统和方法，包括响应于雷达传感器和／或观察系统检测到物体而插入融合处理过程。所述方法包括提供先前采样时间时由LiDAR传感器检测到的物体的物体档案，其中所述物体档案标示了检测到的物体的位置、方向和速度。所述方法将物体档案中的物体模型从先前采样时间投影以提供预测物体模型。所述方法还包括接收当前采样时间时在传感器的视野中检测到的物体的多个扫描返回并从扫面返回中构造点云。然后该方法将点云中的扫描点分割为预测扫描群，其中，每个群标示由传感器检测到的物体。

Description

融合雷达/摄像机物体数据和LiDAR扫描点的方法和装置

相关申请的交叉引用

本申请要求2012年3月15日提交的美国临时专利申请的优先权日期的权益，其申请号为61／611,492，题目为“Methods and Apparatus of Fusing Radar／Camera ObjectData and LiDAR Scan Points”(融合雷达/摄像机物体数据和LiDAR扫描点的方法和装置)。

技术领域

本申请总体上涉及融合车辆中的多个传感器的输出的系统和方法，并且特别地，涉及融合车辆中多个LiDAR传感器的扫描点的系统和方法，其中响应于车辆中的雷达传感器或观察系统检测到—物体，插入融合LiDAR传感器扫描点的处理过程。

背景技术

许多新式车辆带有物体检测传感器，其被用于允许碰撞警告或防止碰撞和其他主动安全应用。所述物体检测传感器可以使用多种检测技术中的任一种，例如短程雷达、带有图像处理的摄像机、激光或LiDAR(激光雷达)、超声等。所述物体检测传感器检测主车辆路径中的车辆及其他物体，并且应用软件使用该物体检测信息以提供警告或采取适当的操作。在许多车辆中，所述物体检测传感器被直接集成在车辆的前端或其他饰板上。

为了使得应用软件最优地执行，所述物体检测传感器必须与车辆适当地对齐。例如，如果传感器检测到一实际上位于主车辆路径内的物体，但由于传感器未对准，传感器判断该物体稍微偏向主车辆路径的左侧，这可能对应用软件有显著影响。即使车辆上有多个前视物体检测传感器，将其适当地对齐也是非常重要的，以将不一致的传感器读数最小化或去除。

LiDAR传感器是一种类型的传感器，其有时用于车辆上以检测车辆周围的物体并提供距这些物体的距离。LiDAR传感器可以提供被追踪物体的航向，因此其是理想的，而其他类型的传感器，例如观察系统和雷达传感器，通常不 能够这样做。对于一种类型的LiDAR传感器，物体反射被作为点群距离图的一部分的扫描点被返回，其中在传感器的视野中每隔1/2°便提供一单独的扫描点。因此，如果在主车辆前方检测到一目标车辆，可存在返回的多个扫描点以确定目标车辆距离主车辆的距离。

车辆可以带有多个LiDAR传感器以提供车辆周围360°的视野。所述多个LiDAR传感器可以包括侧视传感器、后视传感器和前视传感器。每个传感器都独立于其他传感器在各自的视野里追踪物体。使用来自多个传感器的扫描点返回，生成距离图以追踪接近主车辆的物体。对于带有多个LiDAR传感器的车辆，将返回多个点群图，并且对于重叠的传感器视野，该传感器可追踪同一物体。有必要对传感器的扫描点图进行组合，使得被传感器追踪的同一物体作为单个物体处理。

2010年11月9日提交的申请号为12／942,456，标题为“Systems and Methods forTracking Objects”(追踪物体的系统和方法)的美国专利申请被转让给本申请的受让者，在.比通过参考被引入，其介绍了使用LiDAR传感器从主车辆监控目标物体的距离和航向的技术。该应用限于单个的LiDAR传感器，且没有介绍对多个LiDAR传感器的返回进行融合。

发明内容

依据本发明的教导，介绍了一种方法和系统，用于对车辆上多个LiDAR传感器的输出进行融合，其包括响应于雷达传感器和／或观察系统检测到一物体，插入融合处理过程。该方法包括提供LiDAR传感器在先前采样时间时检测到的物体的物体档案，其中该物体档案标示了所检测物体的位置、方向和速度。该方法将物体档案中的物体模型从先前采样时间投影(project，映射或预测)以提供预测物体模型。该方法还包括对当前采样时间时在传感器视野中检测到的物体的多个扫描返回进行接收并从扫描返回中构造点云。然后该方法将点云中的扫描点分割为预测扫描群，其中每个群标示了传感器检测的物体。所述方法将预测群与预测物体模型进行匹配。所述方法读取来自车辆上的雷达传感器或观察系统检测到的目标数据并将该目标数据配准在分配给LiDAR传感器扫描点的坐标系上。所述方法使用预测物体模型将目标数据与物体档案中的物体模型进行匹配。所述方法使用匹配后的预测扫描群和匹配后的目标数据合并在先前采样时间时被识别为独立扫描群而现在被识别为单个扫描群的预测物体 模型，并拆分在先前采样时间里被识别为单个扫描群而现在在当前扫描时间内被识别为独立扫描群的预测物体模型。所述方法还使用匹配后的预测扫描群和匹配后的目标数据创建新的物体模型并删除将要失效的物体模型。所述方法基于合并的物体模型、拆分的物体模型、新的物体模型和删除的物体模型提供物体模型更新，并使用更新后的物体模型更新当前采样时间的物体档案。

方案1.一种用于对来自于车辆上的多个LiDAR传感器的输出进行融合的方法，所述方法包括：

提供先前采样时间时由LiDAR传感器检测到的物体的物体档案，所述物体档案具有标示由传感器检测到的物体的位置、方向和速度的物体模型；

追踪由传感器检测到的物体的物体模型；

对被追踪的物体档案中的物体模型从先前采样时间投影以提供预测物体模型；

接收当前采样时间时在传感器的视野中检测到的物体的多个扫描返回；

从扫描返回中构造扫描点云；

将扫描点云中的扫描点分割为预测扫描群，其中每个扫描群最初标示了传感器检测到的物体；

将预测扫描群与预测物体模型进行匹配；

读取来自雷达传感器或观察系统检测到的物体的物体数据；

将物体数据配准在分配给LiDAR传感器扫描点的坐标系上；

使用预测物体模型对物体数据与物体档案中的物体模型进行匹配；

使用匹配后的预测扫描群和匹配后的物体数据合并在先前采样时间时被识别为独立扫描群而现在在当前采样时间时被识别为单个扫描群的预测物体模型；

使用匹配后的预测扫描群和匹配后的物体数据拆分在先前采样时间时被识别为单个扫描群而现在在当前采样时间时被识别为独立扫描群的预测物体模型；

使用匹配后的预测扫描群和匹配后的物体数据为当前采样时间时检测到而在先前采样时间时不存在的物体创建新的物体模型；

使用匹配后的预测扫描群和匹配后的物体数据删除在预测扫描群中不再存在的物体模型；

基于合并的物体模型、拆分的物体模型、新的物体模型和删除的物体模型，提供物体模型更新；以及

使用物体模型更新对当前采样时间的物体档案进行更新。

方案2.根据方案1的方法，其中，分割点云中的扫描点包括分离点云中的扫描点群使得群标示出正被追踪的独立物体。

方案3.根据方案1的方法，其中，对预测扫描群和预测物体模型进行匹配包括生成使得扫描群与预测物体模型匹配的二部图。

方案4.根据方案3的方法，其中，生成二部图包括在图的顶点定位扫描点。

方案5.根据方案3的方法，其中，合并预测物体模型和拆分预测物体模型包括将所述二部图转换为导出的二部图，其显示出正被合并或拆分的物体模型。

方案6.根据方案5的方法，其中，将二部图转换为导出的二部图包括为导出的二部图计算权重和边的基数(cardinality)，其中，每个权重标示了从至少一个传感器的一个扫描点至另一个传感器的扫描点定位的位置变化，且边定义了物体模型点和分割扫描点之间的匹配。

方案7.根据方案6的方法，其中，将二部图转换为导出的二部图包括将所述边的基数与一阈值进行比较，并且如果基数大于阈值则突出显示该边，且如果边没有被突出显示则移除该边。

方案8.根据方案7的方法，其中，删除在预测扫描群中不再存在的物体模型包括删除与被移除的边关联的物体模型。

方案9.根据方案1的方法，其中，提供物体模型更新包括在贝叶斯网络中针对先前时间帧至当前时间帧标示出物体数据、转换参数、物体模型和扫描图群。

方案10.一种用于对来自于车辆上多个LiDAR传感器的输出进行融合的方法，所述方法包括：

提供先前采样时间时由LiDAR传感器检测到的物体的物体档案，所述物体档案具有标示由传感器检测到的物体的位置、方向和速度的物体模型；

追踪由传感器检测到的物体的物体模型；

对被追踪的物体档案中的物体模型从先前采样时间投影以提供预测物体模型；

接收当前采样时间时在传感器的视野中检测到的物体的多个扫描返回；

从扫描返回中构造扫描点云；

将扫描点云中的扫描点分割为预测扫描群，其中，每个扫描群最初标示了传感器检测到的物体；

将预测扫描群与预测物体模型进行匹配；

读取来自雷达传感器或观察系统检测到的物体的物体数据；

将物体数据配准在分配给LiDAR传感器扫描点的坐标系上；

使用预测物体模型对物体数据与物体档案中的物体模型进行匹配；

使用匹配后的预测扫描群和匹配后的物体数据合并在先前采样时间时被识别为独立扫描群而现在在当前采样时间时被识别为单个扫描群的预测物体模型；以及

使用匹配后的预测扫描群和匹配后的物体数据拆分在先前采样时间时被识别为单个扫描群而现在在当前采样时间时被识别为独立扫描群的预测物体模型。

方案11.根据方案10的方法，其中，分割点云中的扫描点包括分离点云中的扫描点群使得群标示出正被追踪的独立物体。

方案12.根据方案10的方法，其中，对预测扫描群和预测物体模型进行匹配包括生成使得扫描群与预测物体模型匹配的二部图。

方案13.根据方案12的方法，其中，生成二部图包括在图的顶点定位扫描点。

方案14.根据方案12的方法，其中，合并预测物体模型和拆分预测物体模型包括将所述二部图转换为导出的二部图，其显示出正被合并或拆分的物体模型。

方案15.根据方案14的方法，其中，将二部图转换为导出的二部图包括为导出的二部图计算权重和边的基数，其中，每个权重标示了从至少一个传感器的一个扫描点至另一个传感器的扫描点定位的位置变化，且边定义了物体模型点和分割扫描点之间的匹配。

方案16.根据方案15的方法，其中，将二部图转换为导出的二部图包括将所述边的基数与一阈值进行比较，并且如果基数大于阈值则突出显示该边，且如果边没有被突出显示则移除该边。

方案17.根据方案16的方法，其中，删除在预测扫描群中不再存在的物体模型包括删除与被移除的边关联的物体模型。

方案18.根据方案10的方法，其中，提供物体模型更新包括在贝叶斯网络中针对先前时间帧至当前时间帧标示出物体数据、转换参数、物体模型和扫描图群。

方案19.一种用于对来自于车辆上的多个LiDAR传感器的输出进行融合的系统，所述系统包括：

用于提供先前采样时间时由LiDAR传感器检测到的物体的物体档案的设备，所述物体档案具有标示由传感器检测到的物体的位置、方向和速度的物体模型；

用于追踪由传感器检测到的物体的物体模型的设备；

用于对被追踪的物体档案中的物体模型从先前采样时间投影以提供预测物体模型的设备；

用于接收当前采样时间时在传感器的视野中检测到的物体的多个扫描返回的设备；

用于从扫描返回中构造扫描点云的设备；

用于将扫描点云中的扫描点分割为预测扫描群的设备，其中每个扫描群最初标示了传感器检测到的物体；

用于将预测扫描群与预测物体模型进行匹配的设备；

用于读取来自雷达传感器或观察系统检测到的物体的物体数据的设备；

用于将物体数据配准在分配给LiDAR传感器扫描点的坐标系上的设备；

用于使用预测物体模型对物体数据与物体档案中的物体模型进行匹配的设备；

用于使用匹配后的预测扫描群和匹配后的物体数据合并在先前采样时间时被识别为独立扫描群而现在在当前采样时间时被识别为单个扫描群的预测物体模型的设备；和

用于使用匹配后的预测扫描群和匹配后的物体数据拆分在先前采样时间时被识别为单个扫描群而现在在当前采样时间时被识别为独立扫描群的预测物体模型的设备。

方案20.根据方案19的系统，其中，用于分割点云中的扫描点的设备分离点云中的扫描点群使得群标示出正被追踪的独立物体。

通过下面的描述和所附的权利要求，并结合相应的附图，本发明的其他特征将显而易见。

附图说明

图1是跟在目标车辆后面的主车辆的示意图，并显示了主车辆上四个LiDAR传感器的视野；

图2是用于对车辆上多个LiDAR传感器的扫描点进行融合的融合系统的总体框图；

图3(A)和(B)示出了LiDAR传感器的扫描点；

图4是流程图，显示了用于配准多个LiDAR传感器的扫描点的方法；

图5为物体后方的轮廓概率密度函数；

图6是迭代界限最优化图；

图7是框图，显示了在新的LiDAR扫描点图可得时，估计物体运动和更新物体模型的两个交错处理过程。

图8是正在被追踪算法追踪的物体的动态贝叶斯网络；

图9是流程图，显示了多物体追踪算法在单个时间步骤中的处理过程；

图10是二部图，显示了在图9的图中将扫描群与预测物体模型进行匹配的步骤中的预测物体模型点和分割扫描图点；

图11是由图10所示的二部图生成的导出的二部图；

图12是来自LiDAR传感器、观察系统和雷达传感器的扫描点的示意图；

图13是跟在目标车辆后面的主车辆的示意图，并显示了主车辆上LiDAR传感器、雷达传感器和观察系统的视野；

图14是追踪算法的概要流程图，使得LiDAR传感器使用来自雷达传感器或观察系统的插入；

图15是流程图，示出了多物体追踪算法的处理过程，其使用来自雷达传感器或观察系统的插入；

图16是二部图，其示出了为所有的LiDAR扫描点、雷达传感器返回和观察系统图像，对目标数据和物体模型进行匹配；

图17是从图16中所示的二部图中生成的导出的二部图；

图18是二部图，示出了通过提供使用观察系统进行的匹配对两个投影的物体模型进行合并；

图19是导出的二部图，显示了图18中物体模型的匹配；

图20是二部图，示出了通过使用观察系统进行的匹配对投影物体模型进行拆分；

图21是导出的二部图，示出了图20中物体模型的匹配；

图22是二部图，示出了通过使用雷达传感器进行匹配的投影物体模型；

图23是导出的二部图，示出了图22中模型的匹配；和

图24是追踪算法和使用来自雷达传感器或观察系统的插入信息进行模型更新的动态贝叶斯网络。

具体实施方式

下面对本发明实施例的介绍是关于一种用于对来自于车辆上的多个LiDAR传感器的输出进行融合的系统和方法，其中通过雷达传感器或观察系统检测到的物体插入融合处理过程，本质上仅是示例性的，绝不旨在限制本发明及其应用或使用。例如，本发明将被描述为对车辆上LiDAR传感器的输出进行融合。然而，本领域技术人员应当理解，本发明的融合处理过程可以具有除了车辆应用之外的应用。

图1示出了车辆系统10的概况，其包括跟在正被追踪的目标车辆14后面的所示主车辆12。所述主车辆12包括4个LiDAR传感器，即，带有视野18的前视传感器16、带有视野22的后视传感器20、带有视野26的左视传感器24和带有视野30的右视传感器28。如图所示，所述传感器16、24和28被安装在车辆12的前方，并具有重叠的视野。如果一物体，例如目标车辆14，位于传感器16、20、24和30中特定一个的视野内，则该传感器返回标示该物体的多个扫描点。目标车辆14上的点32代表了来自传感器16、24和28中每一个的从目标车辆14返回的扫描点。所述点32使用坐标转换技术转换为主车辆12上的车辆坐标系统(x，y)，然后在车辆坐标系统中使用点32执行物体检测。如下所述，所述点32可以被用于限定目标车辆14的尾部造型。

各传感器16、20、24和28将会为传感器检测到的每个单独物体提供一扫描点云。本发明提出了一种融合算法，其将来自每个传感器16、20、24和28的输出进行组合，使得当传感器16、20、24和28追踪相同的物体时，该物体作为单个目标处理，其中所述算法输出每个被追踪物体的位置、方向和速度。尽管该说明包括四个LiDAR传感器，所提出的融合算法可以应用于具有重叠视野的任意数量和位置的多个LiDAR传感器。

在该图中，目标车辆14的位置通过锚点34来表示，即为，扫描图的中心。下列值用于表示在时间步骤t时目标车辆14的物体模型M。特别地，物体模型M限定相对纵向速度V_x，相对横向速度V_y，横向位移y和目标车辆航向ξ或目标对地速度向量的方向。值M是一系列由参数平均值m_j和方差σ²所表示的高斯组分。所述平均值以多个超参数v_j，η_j，k_j和标记(例如，被访问和到期)为特征。

图2是融合系统36的原理框图，该系统用于对来自多个LiDAR传感器(例如传感器16、20、24和28)的扫描点云返回进行融合。方块38代表左 LiDAR传感器24的扫描点云返回，方块40代表右视LiDAR传感器28的扫描点云返回，方块42代表前视LiDAR传感器16的扫描点云返回，且方块44代表后视LiDAR传感器20返回的扫描点云。在方块46，来自LiDAR传感器16、20、24和28的距离图被配准并且构造出一360°距离图(点云)。一旦配准了来自传感器16、20、24和28的点云并在车辆坐标内形成360°点云，则在方块48算法对来自多个目标的点云进行融合，这将在下面详细介绍。在车辆坐标系内对目标融合之后，在方块50，所述算法输出目标的位置、方向和速度。

在特别介绍对来自多个LiDAR传感器的扫描点进行组合的融合处理过程之前，将先介绍在方块46执行的扫描点配准算法，以在与所述物体相应的物体模型M和当前扫描图S可得时估计物体的运动。

在许多车辆中，所述物体检测传感器被直接集成在车辆的前饰板上。这种安装类型简单、有效并且美观，但有一个缺点，即没有切实可行的方法以对传感器的对准进行物理调节。因此，如果由于饰板的损坏或老化以及气候导致的变形使得传感器与车辆的实际航向没有对准，则按照惯例没有方法对该失准进行校正，除非更换包含传感器的整个饰板组件。

如下面将要介绍的，在方块46执行的坐标系配准对来自传感器20、24和28的距离扫描点进行匹配以适应传感器20、24和28的位置和方向上的可能漂移。当车辆12为新的时，传感器20、24和28是初始校准的。如前所述，多种因素导致这些方向随着时间而改变，因此，需要执行一种处理过程以重新校准传感器的方向，使得在视野26和30的重叠部分内检测到的物体更加准确。本发明提出了一种期望最大化(EM)匹配算法以寻找多个LiDAR传感器之间的变换式T，其定义了传感器的方位角和x，y位置。例如，该算法将对从左视LiDAR传感器24到右视LiDAR传感器28的变换式T进行匹配，并且当在后续计算中变换式T匹配时，传感器24和28将对齐。

所述EM算法开始于选择初始变换式值T₀(其可以为0)、先前估计值、制造商提供的传感器24和28之间的方向等等。然后该算法使用变换式T将左传感器距离图投影到右视LiDAR传感器28的坐标系中。图3表示了扫描点图120，其中圆圈124代表左LiDAR传感器24的扫描点返回，且椭圆126代表了右视LiDAR传感器28的扫描点返回。图3(A)显示了所有的扫描点返回，且图3(B)显示了在圆圈122中为了少数的扫描点返回而放大的区域。图3(B) 显示了如何使用箭头128将左视LiDAR传感器扫描点返回124映射至右视LiDAR传感器扫描点返回126。通过使用当前可得的投影图箭头128的变换式T，左视LiDAR传感器扫描点返回124被相对于右视LiDAR传感器扫描点返回126进行移动以试图使得二者重合。

当前使用的变换式T对于当前的左视LiDAR传感器24至右视LiDAR传感器28的方向可能是不准确的，因此变换式T需要对于传感器24和28的当前位置进行更新。算法使用当前变换式T以更新左传感器扫描点s_j和右传感器扫描点m_k之间的权重a_jk，如下：

其中内核函数K为：

$K\left(x\right)=\frac{1}{\mathrm{\σ}\sqrt{2\mathrm{\π}}}{e}^{-\frac{x^2}{{2\mathrm{\σ}}^2}}.---\left(2\right)$

然后可以使用修正后的权重a_jk来确定新的变换式T′：

接着将新的变换式T′与先前计算的变换式T进行比较，并基于它们之间的差，使用新的变换式T′重新计算权重a_jk直到新计算的变换式T′与先前计算出的变换式T匹配，其中传感器24和28已对齐。

在某些情况下，用于对准扫描数据点的变换式T将充分大，它可能有助于增强公式(3)中T′的解。对于该示例，变换式T被定义为x′=T·x，其中x′=Rx+t，并且其中R是旋转矩阵，和t是变换向量。在该分析中，假定：

S=[S₁，S₂，…，S_L]^T， (4)

M=[m₁，m₂，…，m_N]^T， (5)

A=[a_jk]， (6)

1=[1，1，…，1]^T， (7)

${\mathrm{\μ}}_S=\frac{1}{L}{S}^T{A}^{T}1,---\left(8\right)$

${\mathrm{\μ}}_m=\frac{1}{N}{M}^{T}A1,---\left(9\right)$

$\hat{S}=S-1{\mathrm{\μ}}_S^T,---\left(10\right)$

$\hat{M}=M-1{\mathrm{\μ}}_m^T.---\left(11\right)$

然后公式(3)中新变换式T′的解可以为：

T′=[R′，t′]， (12)

其中：

R′=UCV， (13)

t′=μ_s-R′μ_m， (14)

并且其中U和V被定义为下式奇异值分解的因子：

$\mathrm{USV}=\mathrm{svd}\left({\hat{S}}^T{A}^T\hat{M}\right),---\left(15\right)$

并且C为：

$C=\left[\begin{array}{cc}1& 0\\ 0& \det \left({\mathrm{UV}}^T\right)\end{array}\right].---\left(16\right)$

以上介绍的用于确定变换式T的EM算法可能只是局部最优的并且对初始变换式值敏感。可以使用粒子群优化算法(PSO)寻找初始变换式T₀来对该算法进行增强。在该优化算法中，假设E为从左LiDAR传感器24至右LiDAR传感器28的合格变换式T的集合。所述算法根据合格变换式E的均匀分布随机地产生N个粒子{t_i／t，∈E}，且每个粒子t_i与归一化的权重w_i=1／N有关。对于每个粒子t_i，所述EM算法被设置为最佳的变换式T_i，其中t_i为初始值。假定权重w_i为变换式T_i的两个扫描之间的匹配百分比。然后该算法输出带有最佳匹配百分比的变换式T_k，作为从左LiDAR传感器24至右LiDAR传感器28的变换式的标称值，其中w_k=max(w_i)。

在大多数的情况中，从一个采样时间至下一个采样时间变换式T的变化通常是很小的，因此，不需要使用上述的用于计算对于变换式T的大变化的新变换式T′的复杂计算处理。特别地，如果从上述的PSO算法中估计出了标称变换式T_n，则可以使用下列小计算量的EM匹配算法对变换式T的新估计值进行递归精炼。首先，将变换式T_n应用于右LiDAR传感器28的每个扫描点：

修正的变换式ΔT被确定为：

并被定义为：

x′=x-εy+t_x， (19)

y′=εx+y+t_y， (20)

其中变换式ΔT被模型化为(t_x，t_y，ε)并且其中先前的变换式ΔT₀为(t_x0，t_y0，ε₀)。

然后公式(3)被替换为：

并且其解为：

$t_x^{\′}=\frac{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}(x_j^{\′}+\mathrm{\ϵ}{y}_k-x_k)+\mathrm{\λ}{t}_{x0}}{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}+\mathrm{\λ}},---\left(22\right)$

其中：

s_j=(x′_j，y′_j)， (23)

m_k=(x_k，y_k)， (24)

T′=t′_x，t′_y，ε′， (25)

$t_y^{\′}=\frac{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}(y_j^{\′}+\mathrm{\ϵ}{x}_k-y_k)+\mathrm{\λ}{t}_{y0}}{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}+\mathrm{\λ}},---\left(26\right)$

${\mathrm{\ϵ}}^{\′}=\frac{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}[(y_j^{\′}-t_y^{\′}-y_k)x_k-(x_j^{\′}-t_x^{\′}-x_k)y_k]+\mathrm{\λ}{\mathrm{\ϵ}}_0}{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}(x_k^2+y_k^2)+\mathrm{\λ}},---\left(27\right)$

并且其中λ为权重因子，其可以根据先前估计被使用的程度来调节。

图4为一流程图130，其显示了对齐LiDAR传感器的操作，例如，如上所述的LiDAR传感器24和28。在方块132，所述算法选择如制造商设置的初始变换式T_n。在方块134，所述算法采集来自LiDAR传感器24和28的扫描距离图数据，并在判决菱形136处确定来自LiDAR传感器24和28视野中的物体的扫描点的数量是否充分以足以执行计算。如果扫描点不够，则该算法返回方块134以采集更多的距离图数据。如果在判决菱形136处扫描点足够多，则在方块138该算法使用EM匹配算法以寻找修正后的变换式ΔT。然后在判决菱形140所述算法确定修正后的变换式ΔT是否大于预定阈值，并且如果是，则在方块142使用PSO算法采用该大的变换式估计值以寻找新的标称变换式T_n。如果在判决菱形140处匹配百分比不大于阈值，贝所述算法返回至方块134以采集接下来的扫描点。

返回至融合算法，下面的介绍首先介绍了一种提出的扫描点配准算法，其在给定相应于目标车辆14的物体模型M和当前扫描图S时，估计目标车辆14的运动。上述关于EM算法的介绍提供了空间匹配，所述EM算法用于确 定对齐所述LiDAR传感器之间的坐标系的变换式T，特别是，不同LiDAR传感器在同一时刻的两个坐标系之间的匹配。所述关于扫描点配准的介绍还使用一点集配准算法以寻找变换式T，其对当前扫描图S和从过去的扫描图中得出的物体模型M之间的两个坐标系在时间上匹配。

首先定义概率物体模型M，且然后提供提出的迭代算法以寻找一固定变换式，使得概率值为最大，给定后续帧的扫描图。为了表征物体的几何形状，定义采样空间R²中的轮廓概率分布函数(PDF)。图5显示了目标车辆14后部的示例PDF的瞬态图(快照)。不是使用某些参数，PDF直接表示为一系列粒子(圆点)，标记为M=(m₁，m₂，…，m_nM)^T。

表示PDF的该非参数方法可以记为：

$p(x;M)=\frac{1}{n_M}{\mathrm{\Σ}}_{j=1}^{n_M}{K}_{\mathrm{\σ}}(x-m_j),---\left(28\right)$

其中x∈R²，为高斯内核函数，并且σ>0为一平滑参数，称为带宽或帕曾(Parzen)窗的尺寸。

假定y为变换式T_y的参数。运算符被定义为带有参数y的变换操作符T_y(·)以及相应的逆变换1(·)。不失一般性，所述物体模型M可以被认为在原点34对中。在后续帧中，位于y的物体可以通过PDF表征为：

$p(x;y,M)=\frac{1}{n_M}{\mathrm{\Σ}}_{j=1}^{n_M}{K}_{\mathrm{\σ}}(T_y^{-1}\left(x\right)-m_j).---\left(29\right)$

假定S=(s₁，s₂，…，s_ns)^T为包含一系列扫描点s_k的当前扫描图。概率函数可以表示为：

$L(S;y,M)={\mathrm{\Π}}_{K=1}^{n_s}\mathrm{\ρ}(T_y^{-1}\left(s_k\right);M),---\left(30\right)$

其中假设扫描点s_k为物体模型M在位置y的独立且分布相同的样本。

此处的目的是要寻找相对于变换式参数y的L(S；y，M)最大值，其相当于寻找J(y；M)≡-LogL(S；y，M)最小值，如下：

y^*argmin_yJ(y；M)， (31)

其中：

$J(y;M)={\mathrm{\Σ}}_{k=1}^{n_s}-\log \left(\frac{1}{n_m}{\mathrm{\Σ}}_{j=1}^{n_M}{K}_{\mathrm{\σ}}(T_y^{-1}\left(s_k\right)-m_j)\right).---\left(32\right)$

该局部最小值指示出了在接下来的帧中出现物体，其具有类似在原点34

定义的p(x；M)的表示。

引入一辅助的矩阵A≥{a_kj}，其中a_kj≥0，j=1，…，n_M且k=1，…，n_s，其中：对有所有的k

${\mathrm{\Σ}}_{j=1}^{n_m}{a}_{\mathrm{kj}}=1,---\left(33\right)$

因此，公式(32)变为：

$J(y;M)={\mathrm{\Σ}}_{k=1}^{n_s}-\log \left({\mathrm{\Σ}}_{j=1}^{n_M}{a}_{\mathrm{kj}}\frac{K_{\mathrm{\σ}}(T_y^{-1}\left(s_k\right)-m_j)}{a_{\mathrm{kj}}}\right),---\left(34\right)$

$\≤-{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}\log \frac{K_{\mathrm{\σ}}(T_y^{-1}\left(s_k\right)-m_j)}{a_{\mathrm{kj}}},---\left(35\right)$

≡J(A，y；M)。 (36)

在公式(32)中，归一化因子被并入一忽略的常数项。对公式(34)应用詹恩逊不等式(jensen′sinequality)得到公式(36)。

假设负对数函数为凸的，则可以通过对公式(36)求导和拉格朗日乘子(Lagrangemultiplier)约束得出公式(36)的最佳上限J^*(y;M)，见公式(33)，相对于a_kj，并设置为0。最优辅助变量可以表示为：

$a_{\mathrm{kj}}=\frac{K_{\mathrm{\σ}}(T_y^{-1}\left(s_k\right)-m_j)}{{\mathrm{\Σ}}_{j^{\′}=1}^{n_M}{K}_{\mathrm{\σ}}(T_y^{-1}\left(s_k\right)-m_{j^{\′}})},---\left(37\right)$

j=1，…，n_M和k=1，…，n_s。

因此，公式(31)可以通过“边界最优化”来迭代计算出，其在相对于辅助矩阵A，变换式T的参数y，和内核函数σ的带宽的最优J^*(y₁；M)之间交替。

图6为图表，水平轴y显示了迭代的边界最优化关系，其中线52是一复杂曲线J(y；M)，且虚线为在y₀，y₁和y₂的连续上限，即分别地，图线54为J^*(y₂；M)，图线56为J^*(y₁；M)，且图线58为J^*(y₁；M)。

下方所示算法1的迭代过程被重复直至收敛，即，两个迭代所估计的y的差值小于预定的小值。经验结果显示2-3轮迭代就足以使所提出的算法收敛。

算法1：扫描点配准

输入：物体模型M，当前扫描图S，和初始变换参数y₀。

1)假设σ=σ₀，其中σ₀为正数。

2)计算A：给定变换参数y_n的先验估计值，使用公式(37)对a_kj进行更新。

3)最小化J^*(y；M)：计算y^*以最小化J^*(y；M)，即：

$y^{*}={\arg \min }_y-{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}\log K_{\mathrm{\σ}}(T_y^{-1}\left(s_k\right)-m_j).---\left(38\right)$

4)计算σ：相对于σ的不同公式(36)，将其导数设置为0，得出：

${\mathrm{\σ}}^2=\frac{1}{n_s}{\mathrm{\Σ}}_{k,j}{a}_{kj}{\left|\right|T_{y^{*}}^{-1}\left(s_k\right)-m_j\left|\right|}^2.---\left(39\right)$

5)假设y_n+1=y^*。如果||y_n+1-y_n||>ε，则至步骤2。否则，输出y^*作为变换式的估计参数。

下面的介绍描述了点集配准的特殊情况，其可以用于带有LiDAR扫描器的机器人技术中。可以写成固定变换式x′=T_y(x)和x′=Rx+t，其中参数向量y包括一旋转矩阵R和转换向量t。公式(38)可以被简化为：

y^*argmin_y∑_k，ja_kj||s_k-Rm_j-t||²， (40)

使得det(R)=1且R^TR=I。

分别求公式(40)对t和R的偏导数，可以求解出公式(40)。为了表示该解，定义下列量：

${\mathrm{\μ}}_s=\frac{1}{n_s}{S}^T{A}^{T}1,---\left(41\right)$

${\mathrm{\μ}}_M=\frac{1}{n_M}{M}^{T}A1,---\left(42\right)$

$\hat{S}=S-1{\mathrm{\μ}}_S^T,---\left(43\right)$

$\hat{M}=M-1{\mathrm{\μ}}_m^T,---\left(44\right)$

其中1=[1，1，…，1]^T。

公式(40)的解为：

R=UCV^T， (45)

t=μ_s-Rμ_m， (46)

其中U和V被定义为奇异值分解的因子，即且C=diag(1，det(UV^T))。

下面的介绍提出了一种基于贝叶斯原理的算法，其递归地估计物体模型M的运动和更新。假设S₀，…，S_t和S_t+1为在时间步骤0，…，t和t+1分别从动态物体采样得到的扫描图。

图7显示了追踪可以被当作如下处理：i)估计物体运动的问题，和ii)当接收到新的帧的扫描图时，更新物体模型M的问题。特别地，图7显示了一处理过程60，用于在方块64估计物体位置，在方块62使用物体位置更新物体模型，并当新的扫描图可得时估计物体位置。

下面的介绍将要包括对二部图和贝叶斯图的参照。这两种类型的图包括代表不同事物的节点。贝叶斯图中的节点代表要被估计的变量，例如变换式T和物体模型M，其为随机的，且只可以通过PDF来表示。这些节点被设置在跨越不同时间帧的序列中，且每个序列只对一个物体建模。相反地，二部图中的节点为扫描图的分割点。每个扫描图可以在同一时间帧中包括多个物体。因此，其目标为寻找与物体相对应的各片段。沿着时间轴建立片段(s₁，s_s，…，s_n)之间的联系，可以获得多个序列，每个对应于一个物体。这样，二部图可以用于使用贝叶斯方法来追踪各单独的物体。

图8描述了动态贝叶斯网络70，其表示了所提出的追踪算法的两个时间步骤。在网络70中，节点72代表变换参数y_t和y_t+1，即目标的物体位置和姿态，节点74代表物体模型M_t和M_t+1，且节点76代表扫描图S_t和S_t+1。在图8中，y_t和y_t+1分别为在时间步骤t和t+1的估计变换参数，且M_t和M_t+1分别为在时间步骤t和t+1的物体模型。为了适应贝叶斯网络70，p(y)被模型化为以为中心参数化的狄拉克增量分布，且物体模型M为高斯组份的联合PDF，其分别由均值{m_j}和固定方差σ²参数化。每个均值m_j通过带有超参数的高斯PDF来表示，即，均值和方差

下面详细介绍了参数和超参数{v_j，η_j|j=1，…，n_M}的更新规则。分别使用贝叶斯和链式法则，给定的至时间t+1的扫描图集合，后验PDF可以写为：

p(y，M|S^(0：t+1))∝p(S|y，M)p(y，M|S^(0：t))=p(S|y，M)p(y|S^(0：t))p(M|S^0:t)， (47)

其中y和M为y_t+1的简写，S^(0：t+1)表示一直到时间步骤t+1的扫描图，S为时间点t+1的扫描图，p(S|y，M)为公式(30)中L(S；y，M)的等同表示，以及假定给定S^(0：t)时的条件性独立得出最后的等式。

在公式(47)中，p(y|S^(0：t))为在时间步骤t-1参数y的先验PDF，已知先验扫描图S^{(0 ：t)}，其可以被计算为：

p(y|S^(0：t))=∫p(y|y_t)p(y_t|S^(0：t))dy_t。 (48)

在公式(48)中，p(y_t|S^(0：t))表示了在时间t时变换参数的后验PDF， 且p(y|y_t)为物体运动的后续发展模型的条件概率：

y=f(y_t)+w， (49)

其中w为带有协方差矩阵Q的零均值高斯随机变量(即，p(w)=N)w|0，Q))。如果假设p(y_t|S^(0：t))为以为中心的狄拉克增量分布，则在时间步骤t=1时的先验PDF可以写作：

$p\left(y\right|S^{0:t})=N\left(y\right|\tilde{y},Q),---\left(50\right)$

其中为物体的后续发展模型的预测值。

表达式p(M_t-1|S^(0：t))可以被模型化为公式(30)中L(S；y，M)的共轭分布族以得到可追踪的后验PDF。带有已知方差σ²的高斯密度的结果可以表示为：

$p\left(M_{t-1}\right|S^{(0:t)})={\mathrm{\Π}}_{j}N\left(m_j\right|v_j,\frac{{\mathrm{\σ}}^2}{{\mathrm{\η}}_j}),---\left(51\right)$

其中：

$N\left(m_j\right|v_j\frac{{\mathrm{\σ}}^2}{{\mathrm{\η}}_j})=c\left({\mathrm{\η}}_j\right){\exp }^{-\frac{{\mathrm{\η}}_j}{{2\mathrm{\σ}}^2}{\left|\right|m_j{v}_j\left|\right|}^2},---\left(52\right)$

并且其中(η_j，v_j)为M_t-1的第j个组分的先验PDF的超参数，并且c(η_j)为归一化因子。

如图8中所示，在时间步骤t处物体模型M的先验数据为：

$p\left(M\right|S^{(0:t)})=p\left(M\right|y_t,M_{t-1})={\mathrm{\Π}}_{j}N\left(m_j\right|T_{\stackrel{\‾}{y}t}\left(v_j\right),\frac{{\mathrm{\σ}}^2}{{\mathrm{\η}}_j}).---\left(53\right)$

现在考虑估计时间t+1时的后验p(y，M|S^(0：t+1))的问题。因为y和M都为条件性独立的，即：

p(y，M|S^(0：t+1))=p(y|S^(0：t+1))p(m|S^(0：t+1))， (54)

可以使用接下来的两个步骤估计后验相关y和M。

假设p(y|S^(0：t+1))为以为中心的狄拉克增量PDF，其可以通过公式(47)相对于y最大化来估计。将公式(50)代入公式(47)，对公式(47)应用负对数并且忽略与y不相关的项，得到：

${\stackrel{\‾}{y}}_{t+1}=\arg {\min }_y-{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}\log K_{\mathrm{\σ}}\left(T_y^{-1}\left(s_k\right){-m}_j\right)+{\left|\right|y-\tilde{y}\left|\right|}_Q^2,---\left(55\right)$

其中，且。因此，公式(38)可以被公式(55)代替以集成先前时间步骤中的先验信息，然后使用算法1以导出

忽略与物体模型M不相关的项，公式(47)的负对数可以写为：

$J\left(M\right)=-\log p\left(S\right|y,M)-\log p\left(M\right|S^{(0;t)})\≤J^{*}({\stackrel{\‾}{y}}_{t+1},M)-{\mathrm{\Σ}}_{j}N\left(m_j\right|T_{{\stackrel{\‾}{y}}_t}\left(v_j\right),\frac{{\mathrm{\σ}}^2}{{\mathrm{\η}}_j}).---\left(56\right)$

注意对于y的所有可能值，公式(56)为J(M)的最佳上边界。

如在图6中，可以使用边界最优化通过寻找上述上边界函数的最小值来迭代地寻找最优物体模型M。将公式(56)相对于物体模型M的导数设置为0，则可以得到物体模型M的MAP估计：

$m_j^{*}=\frac{{\mathrm{\ρ}}_j{T}_{{\stackrel{\‾}{y}}_{t+1}}^{-1}\left({\stackrel{\‾}{s}}_j\right)+{\mathrm{\η}}_j{T}_{{\stackrel{\‾}{y}}_t}\left(v_j\right)}{{\mathrm{\ρ}}_j+{\mathrm{\η}}_j},---\left(57\right)$

其中：

ρ_j=∑_ka_kj， (58)

${\stackrel{\‾}{s}}_j={\mathrm{\Σ}}_k{a}_{\mathrm{kj}}{s}_k|{\mathrm{\ρ}}_j.---\left(59\right)$

对于时间t+1的S之后，物体模型M的后验分布的新超参数(η′_j，v′_j)的更新规则可以写为：

η′_j=η_j+ρ_j， (60)

$v_j^{\′}=\frac{1}{{\mathrm{\η}}_j+{\mathrm{\ρ}}_j}\left({\mathrm{\η}}_j{T}_{{\stackrel{\‾}{y}}_t}\left(v_j\right)\right)+{\mathrm{\ρ}}_j{T}_{{\stackrel{\‾}{y}}_{t+1}}^{-1}\left({\stackrel{\‾}{s}}_j\right).---\left(61\right)$

因此，在时间t目标的后验PDF为：

$p\left(M\right|S^{(0:t+1)})={\mathrm{\Π}}_{j}N\left(m_j\right|v_j^{\′},\frac{{\mathrm{\σ}}^2}{{\mathrm{\η}}_j^{\′}}).---\left(62\right)$

注意公式(57)中的为模式，且公式(61)中的v_j为物体模型M的第j个组分(粒子)的均值。由于高斯假定，它们是相同的。

该递归追踪和模型更新方法被概括在下面的算法2中。步骤1准备当前时间帧的物体模型M，而步骤2估计物体模型M的运动。步骤3基于当前的扫描图更新物体模型M。步骤4向物体模型M中添加新的粒子，而步骤5从物体模型M中去除异常值。

算法2：追踪和物体模型更新

输入：当前扫描图S_t+1，物体模型M_t-1={v_j，_j}，和变换式。

输出：估计出的和更新的M_t。

1)对于所有的j，计算公式(51)中p(M_t|S^(0：t))的超参数。将所有的粒子设为未访问。

2)假设对于所有的j，。将公式(38)替换为公式(55)且然后运行算法1以获得和A_t+1。

3)如果ρ_j=Σ_ka_kj大于阈值，使用公式(60)和(61)计算超参数，将粒子设置为已访问，并将访问计数K_j加1。如果K_j>5，则将第j个粒子标记为成熟。

4)对所有的k计算Q_k＝Σ_ja_kj。如果Q_k小于阈值，则增加一新的粒子s_k，其带有下列值K_k＝1，v_k＝s_k，η_k＝1，并被标记为已访问。

5)去除未被标记为已访问且未成熟的粒子。

图9为一流程图80，其显示了在每个时间步骤t所执行的提出的融合算法。方块78代表了在每个时间步骤生成的物体档案，并提供被检测和追踪的物体的位置、速度和航向以及被追踪的每个物体的物体模型M。当在方块82，来自传感器16、20、24和28的距离数据的新的帧到达主车辆12，所述算法首先在方块84用上述方法构造360°点云。一旦构造了点云，然后在方块86该算法将云中的扫描点分割为可标示特定物体的群。为了执行分割操作，假定R表示当前帧(t+1)的扫描图，且假定G=(R，E)为带有顶点集合R的不定向图。如果||p₁-p₂||小于预定的距离阈值，则边(p₁，p₂)∈E连接p₁和p₂。然后该连接的组分标记被用于将扫描图R分割为一系列群。将扫描点分割为群包括在返回的点云中分离扫描点群使得群代表正被追踪的单独物体。

一旦扫描点被分割为群，然后在方块88该算法将群与预测物体模型 进行匹配。特别地，在方块90所述算法使用物体档案78对先前时间步骤的追踪物体模型M投影。为了预测该投影的物体模型，假定为在时间步骤t处的一系列物体模型M。对于每个物体模型，高斯组分的均值表示为。如果 为在时间步骤t处变换式的估计值，则在时间步骤t+1处预测的变换式为 因此，对于所有的j，第n个物体的预测物体模型为通过所述预测的物体点从时间步骤t被指示出。为

使用该投影的追踪物体模型，在方块88所述分割的群与预测物体模型进行匹配。在顶点集L至R之间构造二部图B=(L，R，E_B)，E_B为边集。如果且仅如果，||p-q||<D，其中D为距离阈值，则在点p∈L和q∈R之间存在边。边(p，q)的权重被定义为w(p，q)=||p-q||。q∈R的邻域被定义为N(q)≡{p|(p，)∈E_B}。

可以从二部图B中定义导出的二部图B′=(L′，R′E_B′)，其中边集E_B代表在L′中的物体和分割群R′之间的可能的关联。如果且仅如果，在B中存在边(p，q)使得p′∈p且q′∈q，则在p′∈L′和q′∈R′之间存在边。

设E(p′，q′)为B中边集的子集，即E(p′，q′)≥{(p，q)|(p，q)∈E_B∩p∈p′∩q′∈_q}。权重(p′，q′)和边(p′，q′)的基数被分别定义为：

$w(p^{\&prime;},q^{\&prime;})=\frac{{\mathrm{\&Sigma;}}_{q\&Element;q^{\&prime;}}{\min }_{p\&Element;N\left(q\right)\&cap;p\&Element;p^{\&prime;}}w(p,q)}{\left|q^{\&prime;}\right|}---\left(63\right)$

$c(p^{\&prime;},q^{\&prime;})=\frac{\left|E(p^{\&prime;},q^{\&prime;})\right|}{\left|q^{\&prime;}\right|}.---\left(64\right)$

一旦构造了导出的二部图B′，并计算出了边的权重和基数，则使用下面的算法3选出强边和切断的脆弱连接。

图10为二部图100，即图B，如上所述在当前时间步骤t有5个预测物体模型和五个群S的扫描点水平。在该二部图100中，点102代表预测物体模型点，且三角形104代表分割的扫描图点。点102和三角形104之间的连线106为图100的边。

然后在方块92所述算法对数据点群进行合并和拆分以组合相同的检测物体并拆分最初被检测为单个物体，但是已经形成多个物体的物体。为了对群进行合并和拆分，下面算法3提供了扫描关联。

图11示出了导出的二部图110，即为B′，其通过将点合并为群而从图B中导出。在所述导出的二部图110中，节点114代表物体模型点(M)，节点112代表分割扫描点(M)构成的群，且线116代表限定物体模型点(M)和分割扫描点(S)之间匹配的边。如下面将要描述的，时间t处的与时间t+1处的S₁匹配，和被合并至时间t+1处的S₂，时间t处的被拆分成时间t+1处的S₃和S₄，时间t处的正在衰减且从时间t+1处扫描图中移除，且S₅在时间t+1处到达。在算法3的步骤3，边和被突出显示。在步骤4，与匹配，边被突出显示。因此，在步骤5，虚线被作为脆弱连接而切断。图11显示了与直接匹配，然而，其他两种情况需要进行群的合并和拆分。

预测物体模型被拆分成两个匹配的群和当前群和的扫 描点将使用在先前帧中曾经存在物体的先前知识进行重新聚集。通常需要能够证明不同的有力证据来将现有追踪物体拆分成两个群。

在当前帧中，两个预测物体模型和与群S₂关联。群S₂中的扫描点将进行重新聚集因为在扫描点之间需要更强的邻近以将两个追踪物体合并为一个物体。

算法3：扫描关联算法

输入：群和预测物体模型

输出：被切断的导出的二部图B′。

1)构造将扫描点作为顶点的二部图B。

2)构造将群和轮廓线作为顶点的导出的二部图B′，并使用公式(63)和(64)分别计算边的权重和基数。

3)对于每个q′∈R′，寻找p*′∈L′使得基数大于阈值(即c(p*′，q′)>C)且p*′＝argmin_p′∈L′w(p′，q′)，并突出显示边(p*′，q′)。

4)对于每个没有被突出显示的边覆盖的p″∈L′，寻找集合使得每个成分q″被且仅被一个突出显示的边覆盖。寻找p″使得(p″，q″)>C且q*″＝argmin_q″∈R″w(p″，q″)，并突出显示边(p″，q*″)。

5)切断B′中所有未突出显示的边。

如上所述，在方块94创建新的物体并删除正在衰减的物体。特别地，需要解决两种特殊的情况以创建新的物体并移除在物体档案中的现有物体，其中不存在与扫描群S₅相关联的边。触发一追踪初始化步骤，并在物体档案中为扫描群S₅添加一新的物体，且由于没有边与预测物体模型关联，使得预测物体模型衰减并将从物体档案中移除。

一旦在方块94创建了新的物体并删除了衰减的物体，然后在方块96该算法进行追踪和模型更新。对于每个由例如算法1的扫描配准操作提供的物体模型M_t和扫描群S_t+1的匹配对，使用算法2进行追踪和物体模型更新。

然后物体档案78在方块98被更新并被存储用于下一时间步骤的物体档案78。每个物体的新的变换式y_t+1和更新后的物体模型M_t被存储回物体档案，且算法等待下一时间步骤中新的扫描图的到达。

如果两个连续扫描帧之间的旋转很小，则2D变换的参数可以近似为：

x′=x-εy+t_x， (65)

y′=εx+y+t_y， (66)

其中(x，y)和(x′，y′)分别为先前帧和当前帧中的两个点，ε为旋转角且(t_x，t_y)为变换式。

设m_j=(x_j，y_j)和s_k=(x_k′，y_k′)。将公式(65)和(66)代入公式(55)，分另对t_x，t_y，和ε求部分导数并设为0。设表示公式(49)预测出的y。如果假定为固定模型，给出且为时间步骤t和t-1之间的时间长度。

可以导出下列更新规则：

$t_x=\frac{{\mathrm{\&Sigma;}}_{k,j}{a}_{\mathrm{kj}}({x^{\&prime;}}_k+{\mathrm{\&epsiv;y}}_j-x_j)+{\mathrm{\&lambda;}}_x{\tilde{t}}_x}{{\mathrm{\&Sigma;}}_{k,j}{a}_{\mathrm{kj}}+{\mathrm{\&lambda;}}_x},---\left(67\right)$

$t_y=\frac{{\mathrm{\&Sigma;}}_{k,j}{a}_{\mathrm{kj}}({y^{\&prime;}}_k+{\mathrm{\&epsiv;x}}_j-y_j)+{\mathrm{\&lambda;}}_y{\tilde{t}}_y}{{\mathrm{\&Sigma;}}_{k,j}{a}_{\mathrm{kj}}+{\mathrm{\&lambda;}}_y},---\left(68\right)$

$\mathrm{\&epsiv;}=\frac{{\mathrm{\&Sigma;}}_{j,k}{a}_{\mathrm{kj}}[({y^{\&prime;}}_k-t_y-y_j)x_j-({x^{\&prime;}}_k-t_x-x_j)y_j]+{\mathrm{\&lambda;}}_{\mathrm{\&epsiv;}}\widetilde{\mathrm{\&epsiv;}}}{{\mathrm{\&Sigma;}}_{k,j}{a}_{\mathrm{kj}}(x_j^2+y_j^2)+{\mathrm{\&lambda;}}_{\mathrm{\&epsiv;}}},---\left(69\right)$

其中λ_x，λ_y和λ_ε为从公式(50)中的协方差矩阵Q中导出的权重。

将公式(38)替换成公式(67)-(69)，使用算法1迭代地计算t_x，t_y和作为的ε。

设v_H和ω_H分别表示HV地速和偏航率。目标车辆14的地速可以计算为：

v_gx=v_x+v_H-x_cω_H， (70)

v_gy=v_y+y_cω_H， (71)

且目标车辆14的航向为：

$\mathrm{\&xi;}={\tan }^{-1}\left(\frac{v_{\mathrm{gy}}}{v_{\mathrm{gx}}}\right).---\left(72\right)$

上述的用于检测和追踪多物体的方法和设备可以为主车辆12提供检测物体的360°视野方案。然而，还需要解决下列有关LiDAR传感器的问题。

第一个问题是局部观测，其由遮挡或激光测量故障导致，例如，低反射系数或镜面反射。例如，黑色车辆的检测距离大大小于白色车辆的检测距离。

接下来的问题是由低分辨率导致的稀疏测量，其导致了特征的显著度低，因此，对检测远场物体是不够的。

另一个问题是竖直视野受限，例如4°，其将导致漏检非平坦路面上的物体。

另一个问题是缺少可以将物体从混乱的背景中区分的环境信息。

本发明提出了增强的融合算法，以解决上述有关LiDAR传感器的缺陷。特别地，本发明使用来自车辆上的雷达传感器或观察系统的其中一个或两者的输出，作为插入信号以使得LiDAR传感器输出的融合系统识别目标。通常雷达传感器的距离较远，但视野较窄。而观察系统通常视野很短，但可以与检测目标接触。如上，所介绍的增强的融合系统被设计成估计每个检测物体的x，y，v_x，v_y和ξ。

图12为显示在道路152上的目标的图像150，该目标由主车辆上的LiDAR传感器、雷达传感器和观察系统检测到。特别地，该目标的距离足够远，或者被遮挡，使得LiDAR的扫描点很少，由点156表示。为了增强对目标的检测，观察系统图像154限定来自观察系统输出的相同目标和雷达数据点158代表了由雷达传感器检测到的相同目标。

图13为类似于上述系统10的车辆系统160的示意图，其用于尾随和追踪目标车辆164的主车辆162上。使用位置、速度、航向角等与图1中所示相同的变量。在该例子中，所述主车辆162包括具有视野168的LiDAR传感器166，具有视野172的雷达传感器170和具有视野176的观察系统174。LiDAR传感器166的返回显示为扫描点178，雷达传感器170的返回被标记为三角形180，和观察系统174的返回被表示为方块182。现有技术中己知，观察系统和雷达传感器给出来自于目标的四个输出，即为，至目标的距离，至目标的距离的变化(即，距离变化率)，目标航向角以及目标方位角的变化(即，方位角变化率)。

图14为所提出的融合系统190的概观的框图。在每个时间步骤，雷达传感器170和／或观察系统174捕获的目标从方块192发送至插入内容方块194。如上所述，在方块196将LiDAR扫描群图提供给LiDAR追踪算法方块198。所述追踪算法接收来自方块194的插入内容目标数据，并且该目标数据与存储的物体模型进行匹配，其中值n₀为检测到的目标的数量。每个目标o_i＝(x″_i，y″_i，v″_iw″_i)带有参数纵向位移(x″)，横向位移(y″)，径向速度(v″)和横向速度(w″)。带有每个扫描点s_k的LiDAR扫描图R=(s₁，s₂，…，s_N)^T包括一 纵向偏置x″_i和横向偏置y″_i，其中N为扫描点的数量。所述追踪算法监测插入内容方块194的输入和LiDAR扫描图，并生成每个检测物体的输出参数x，y，v_x，v_y和ξ。应用方块200使用所估计的物体参数，并执行车辆主动安全应用，例如自适应巡航控制、碰撞缓解制动等。

图15为类似于上述的流程图80的流程图204，其中相同的操作使用相同的附图标记标示。流程图204包括从雷达传感器170或观察系统174读取目标数据的方块206。接着在方块208，所述算法将目标数据配准至LiDAR坐标系。为了完成该步骤，设y_o为固定变换式T的参数(平移和旋转)，该变换将目标从雷达传感器170或观察系统174映射至LiDAR坐标系，使得LiDAR坐标系中的映射目标为：

$o_i^{\&prime;}=T_{y_o}\left(o_i\right).---\left(73\right)$

该配准参数y_o可以通过被追踪的LiDAR物体和来自雷达传感器170或观察系统174的目标之间的匹配对来自动估计。2011年8月7日公布的名为“Method and Apparatus forOn-vehicle Calibration and Orientation of Object-Tracking Systems(用于对车辆上的物体追踪系统进行校准和定位的方法和装置)”的美国专利7991550，被转让给本申请的受让人，在此通过参考被引入，介绍了—种适用于该目的的技术。

接着，在方块210所述算法将来自雷达传感器170或观察系统174的目标数据与物体模型M进行匹配。该处理过程显示为图16中的图像230，其包括主车辆232，其中点234为物体模型点，三角形236为雷达目标，以及菱形238为从观察系统174的图像数据中导出的虚拟扫描点。雷达目标Or被模型化为由三角形236表示的点。观察目标O_v被模型化为矩形，其可以表示为一系列在矩形边上采样得到的点，该矩形的边为从主车辆232入射的“虚拟”扫描线。值O_v={o_l|l=1…，6}显示为菱形238。定义出物体模型的集合和所有物体模型的扫描点。设R=(o_i)表示了所有的目标点，不论三角形或菱形，分别来自雷达传感器170或观察系统174。

上述过程被用于构造图17中的切断的二部图240。所述图240显示了位于节点246的物体运动242和位于节点248的物体模型244，其中 为创建新物体的候选，且预测物体模型被标记为正在衰减且可从物体文件中移除。

使用方块90得出的投影追踪物体模型方块88中与投影物体模型 匹配的群，和方块210中与物体模型匹配的目标数据，所述算法在方块212对物体模型M进行合并和拆分。考虑两种情况，即，观察目标和雷达目标。图18显示了如下情况的二部图，两个预测物体模型和与观察目标O₁匹配。该来自观察系统74的插入信息提供证据表明这两个物体模型和需要合并为新的物体模型

图19为导出的二部图252，其显示了将节点256处的物体模型和 从节点254处的观察目标O₁合并成单个的物体模型

图20为二部图260，其显示了另一种情况，其中物体模型与两个观察目标O₁和O₂匹配。来自观察系统174的插入信息提供证据表明物体模型需要被拆分成两个独立的物体模型和

图21为导出的二部图262，其显示了节点264处的物体模型(其分别被节点266和268所表示的两个观察目标O₁和O₂检测到)拆分成分别由节点270和272表示的物体模型和

图22为二部图270，其显示了如下情况，其中两个物体模型和的点集R与三个雷达目标o₁，o₂和o₃匹配。

图23为导出的二部图276，其显示了分配和特别地，图276显示了节点278处的雷达目标o₁与节点280处的物体模型匹配，并且节点282和284处的雷达目标o₂和o₃分别显示为合并成节点286处的单个物体模型

接下来，在方块214所述算法创建新的物体并删除正在衰减的物体。类似于上述流程图80的过程，需要处理两种特殊的情况以创建新物体并从物体档案中移除正在衰减的物体。对于未从LiDAR区段匹配的每个扫描群S和未从雷达传感器170或观察系统174匹配的每个目标，使用一追踪初始化过程以向物体档案中添加新物体。对于物体档案中的物体，如果来自于LiDAR传感器166的分割群S或来自雷达传感器170或观察系统174的目标不存在匹配，则该物体被标记为正在衰减。如果该物体在多个连续时间步骤中始终被标记为正在衰减，其将被从物体档案中移除。

接下来，在方块216所述算法提供追踪和模型更新。为了完成该步骤，设o₀，…，o_t和o_t+1为雷达传感器170或观察系统174在时间步骤0，…，t和t+1分别 检测到的动态物体在LiDAR坐标系中的映射测量值。o^0：t表示了从时间步骤0至时间步骤t的测量值。

图24显示了来自图8的修正的动态贝叶斯网络290，代表了带有插入信息的所提出的追踪算法的两个时间步骤。值y_t和y_t+1分别为在时间步骤t和t+1需要估计的变换式参数。模型M_t和M_t+1为在时间步骤t和t+1的物体模型。在网络290中，节点292为在时间步骤t和t+1的目标数据o_t和o_t+1，节点294为在时间步骤t和t+1的变换式参数y，节点296为在时间步骤t和t+1的物体模型M，以及节点298为在时间步骤t和t+1的扫描图。

与方程(47)中类似，贝叶斯和链式法则被分别用于导出后验PDF，给定来自于雷达传感器170或观察系统174LiDAR的至时间步骤t+1的扫描图和目标数据：

p(y，M|S^(0：t+1)，o^(0：t+1))∝p(S|y，M)p(o|y，M)p(y，M|S^(0：t)，o^(0:t))， (74)

=p(S|y，M)p(o|y)p(y，|S^(0：t)，o^(0：t))p(M|S^(0：t)，o^(0：t))

其中y和为M为简写的y_t+1和M_t，S^(0:t+1)代表了至时间步骤t+1的扫描图，S为在t+1处的扫描图，o＝o_t+1为时间步骤t+1处的测量目标的简写，以及假设给定S^(0：t)和o^(0：t)时的条件生独立得到最后的等式。

在公式(74)中，p(y，M|S^(0：t)，o^(0：t))为给定先前扫描图S^(0：t)和目标数据o^(0：t)时时间步骤t+1处y的先验PDF，其可以通过下式计算：

p(y|S^(0：t)，o^(0：t)＝∫p(y|y_t)p(y_t|S^(0：t)，o^(0：t))dy_t (75)

其中p(y_t|S^(0：t)，o^(0：t)表示在t处变换工参数的后验PDF，且p(y|y_t)表示了发展模型方程(49)。

如果假设p(y_t|S^(0：t)，o^(0：t))为以为中心的迪拉克增量分布，时间步骤t+1处的先验PDF可以写为：

$p\left(y\right|S^{(0:t)},o^{(0:t)})=N\left(y\right|\tilde{y},Q),---\left(76\right)$

其中是物体随后的发展模型的预测值

现在考虑物体模型M的后验概率，其中：

$p\left(M_{t-1}\right|S^{0:t},o^{(0:t)})={\mathrm{\&Pi;}}_{j}N\left(m_j\right|v_j,\frac{{\mathrm{\&sigma;}}^2}{{\mathrm{\&eta;}}_j}),---\left(77\right)$

并且其中在公式(51)中定义。如图24中所示，在时间步 骤t物体模型的先验网络可以写为：

$p\left(M\right|S^{(0:t)},o^{(0:t)})=p\left(M\right|y_t,M_{t-1})={\mathrm{\&Pi;}}_{j}N\left(m_j\right|T_{{\stackrel{\&OverBar;}{y}}_t}\left(v_j\right),\frac{{\mathrm{\&sigma;}}^2}{{\mathrm{\&eta;}}_j}),---\left(78\right)$

现在考虑估计时间步骤t+1的后验概率的问题，其可以被分解为：

p(y，M|S^(0：t+1)，o^(0：t+1))=p(y|S^(0：t+1)，o^(0：t+1))p(M|S^(0：t+1)，o^(0：t+1))， (79)

并且使用下面两个步骤独立地计算。

假设p(y|S^(0：t+1)，o^(0：t+1))为以为中心的狄拉克增量PDF，其可以通过将等式(74)相对于y最大化来估计。

目标数据p(o|y)的测量模型被模型化为：

O=h(y)+v， (80)

其中v为带有协方差矩阵E的零均值高斯随机变量。因此，p(o|y)为高斯PDF，即：

p(o|y)=N(o|h(y)，E)， (81)

将公式(76)和(81)代入公式(74)中，对公式(74)取负对数，并忽略与y无关的项，可以得到下式：

${\stackrel{\&OverBar;}{y}}_{t+1}=\arg {\min }_y-{\mathrm{\&Sigma;}}_{\mathrm{kj}}{a}_{\mathrm{kj}}\log K_{\mathrm{\&sigma;}}(T_y^{-1}\left(s_k\right)-m_j)+,---\left(82\right)$

o-hyE2+y-yQ2

其中 $m_j=T_{{\stackrel{\&OverBar;}{y}}_t}\left(v_j\right)$ 且 ${\left|\right|a\left|\right|}_{\mathrm{\&Sigma;}}^2=a^T{\mathrm{\&Sigma;}}^{-1}a$ 。

因此，算法1中的公式(38)可以替换成公式(55)以集成先前时间步骤的先验信息和来自雷达传感器170或观察系统174的插入信息，然后使用算法1导出

当计算p(M|S^(0：t+1)，o^(0：t+1))时，应当注意公式(78)和公式(53)是等同的，并且因此，使用了上述相同的超参数更新规则。

然后在方块98按照上述方法更新物体档案78。

本领域技术人员应当理解，在此用于描述本发明的多种不同的步骤和处理过程可以表示由计算机、处理器或其他使用电子现象来操作和／或传输数据的电子计算设备来执行的操作。这些计算机和电子设备可以使用多种易失性和／或非易失性存储器，包括非暂态计算机可读介质，其上存储有可执行程序，包括能够被计算机或处理器执行的多种代码或可执行指令，其中存储器和／或计算机可读介质可以包括所有形式和类型的存储器和其他计算机可读介质。

上述介绍仅仅公开和描述本发明的示例性实施例。本领域技术人员在上述介绍的基础上并结合相应的附图和权利要求，可以在不脱离如在所附权利要求中限定的本发明的精神和范围的情况下，对本发明做出多种改变，修正和变型。

Claims (20)

1.一种用于对来自于车辆上的多个LiDAR传感器的输出进行融合的方法，所述方法包括：

提供先前采样时间时由LiDAR传感器检测到的物体的物体档案，所述物体档案具有标示由传感器检测到的物体的位置、方向和速度的物体模型；

追踪由传感器检测到的物体的物体模型；

对被追踪的物体档案中的物体模型从先前采样时间投影以提供预测物体模型；

接收当前采样时间时在传感器的视野中检测到的物体的多个扫描返回；

从扫描返回中构造扫描点云；

将扫描点云中的扫描点分割为预测扫描群，其中每个扫描群最初标示了传感器检测到的物体；

将预测扫描群与预测物体模型进行匹配；

读取来自雷达传感器或观察系统检测到的物体的物体数据；

将物体数据配准在分配给LiDAR传感器扫描点的坐标系上；

使用预测物体模型对物体数据与物体档案中的物体模型进行匹配；

使用匹配后的预测扫描群和匹配后的物体数据合并在先前采样时间时被识别为多个独立扫描群而现在在当前采样时间时被识别为单个扫描群的预测物体模型；

使用匹配后的预测扫描群和匹配后的物体数据拆分在先前采样时间时被识别为单个扫描群而现在在当前采样时间时被识别为多个独立扫描群的预测物体模型；

使用匹配后的预测扫描群和匹配后的物体数据为当前采样时间时检测到而在先前采样时间时不存在的物体创建新的物体模型；

使用匹配后的预测扫描群和匹配后的物体数据删除在预测扫描群中不再存在的物体模型；

基于合并的物体模型、拆分的物体模型、新的物体模型和删除的物体模型，提供物体模型更新；以及

使用物体模型更新对当前采样时间的物体档案进行更新。

2.根据权利要求1的方法，其中，分割点云中的扫描点包括分离点云中的扫描点群使得群标示出正被追踪的独立物体。

3.根据权利要求1 的方法，其中，对预测扫描群和预测物体模型进行匹配包括生成使得扫描群与预测物体模型匹配的二部图。

4.根据权利要求3的方法，其中，生成二部图包括在图的顶点定位扫描点。

5.根据权利要求3的方法，其中，合并预测物体模型和拆分预测物体模型包括将所述二部图转换为导出的二部图，其显示出正被合并或拆分的物体模型。

6.根据权利要求5的方法，其中，将二部图转换为导出的二部图包括为导出的二部图计算权重和边的基数，其中，每个权重标示了从至少一个传感器的一个扫描点至另一个传感器的扫描点定位的位置变化，且边定义了物体模型点和分割扫描点之间的匹配。

7.根据权利要求6的方法，其中，将二部图转换为导出的二部图包括将所述边的基数与一阈值进行比较，并且如果基数大于阈值则突出显示该边，且如果边没有被突出显示则移除该边。

8.根据权利要求7的方法，其中，删除在预测扫描群中不再存在的物体模型包括删除与被移除的边关联的物体模型。

9.根据权利要求1的方法，其中，提供物体模型更新包括在贝叶斯网络中针对先前时间帧至当前时间帧标示出物体数据、转换参数、物体模型和扫描图群。

10.一种用于对来自于车辆上多个LiDAR传感器的输出进行融合的方法，所述方法包括：

提供先前采样时间时由LiDAR传感器检测到的物体的物体档案，所述物体档案具有标示由传感器检测到的物体的位置、方向和速度的物体模型；

追踪由传感器检测到的物体的物体模型；

对被追踪的物体档案中的物体模型从先前采样时间投影以提供预测物体模型；

接收当前采样时间时在传感器的视野中检测到的物体的多个扫描返回；

从扫描返回中构造扫描点云；

将扫描点云中的扫描点分割为预测扫描群，其中，每个扫描群最初标示了传感器检测到的物体；

将预测扫描群与预测物体模型进行匹配；

读取来自雷达传感器或观察系统检测到的物体的物体数据；

将物体数据配准在分配给LiDAR传感器扫描点的坐标系上；

使用预测物体模型对物体数据与物体档案中的物体模型进行匹配；

使用匹配后的预测扫描群和匹配后的物体数据合并在先前采样时间时被识别为多个独立扫描群而现在在当前采样时间时被识别为单个扫描群的预测物体模型；以及

使用匹配后的预测扫描群和匹配后的物体数据拆分在先前采样时间时被识别为单个扫描群而现在在当前采样时间时被识别为多个独立扫描群的预测物体模型。

11.根据权利要求10 的方法，其中，分割点云中的扫描点包括分离点云中的扫描点群使得群标示出正被追踪的独立物体。

12.根据权利要求10 的方法，其中，对预测扫描群和预测物体模型进行匹配包括生成使得扫描群与预测物体模型匹配的二部图。

13.根据权利要求12 的方法，其中，生成二部图包括在图的顶点定位扫描点。

14.根据权利要求12 的方法，其中，合并预测物体模型和拆分预测物体模型包括将所述二部图转换为导出的二部图，其显示出正被合并或拆分的物体模型。

15.根据权利要求14 的方法，其中，将二部图转换为导出的二部图包括为导出的二部图计算权重和边的基数，其中，每个权重标示了从至少一个传感器的一个扫描点至另一个传感器的扫描点定位的位置变化，且边定义了物体模型点和分割扫描点之间的匹配。

16.根据权利要求15 的方法，其中，将二部图转换为导出的二部图包括将所述边的基数与一阈值进行比较，并且如果基数大于阈值则突出显示该边，且如果边没有被突出显示则移除该边。

17.根据权利要求16 的方法，其中，删除在预测扫描群中不再存在的物体模型包括删除与被移除的边关联的物体模型。

18.根据权利要求10 的方法，其中，提供物体模型更新包括在贝叶斯网络中针对先前时间帧至当前时间帧标示出物体数据、转换参数、物体模型和扫描图群。

19.一种用于对来自于车辆上的多个LiDAR传感器的输出进行融合的系统，所述系统包括：

用于提供先前采样时间时由LiDAR传感器检测到的物体的物体档案的设备，所述物体档案具有标示由传感器检测到的物体的位置、方向和速度的物体模型；

用于追踪由传感器检测到的物体的物体模型的设备；

用于对被追踪的物体档案中的物体模型从先前采样时间投影以提供预测物体模型的设备；

用于接收当前采样时间时在传感器的视野中检测到的物体的多个扫描返回的设备；

用于从扫描返回中构造扫描点云的设备；

用于将扫描点云中的扫描点分割为预测扫描群的设备，其中每个扫描群最初标示了传感器检测到的物体；

用于将预测扫描群与预测物体模型进行匹配的设备；

用于读取来自雷达传感器或观察系统检测到的物体的物体数据的设备；

用于将物体数据配准在分配给LiDAR传感器扫描点的坐标系上的设备；

用于使用预测物体模型对物体数据与物体档案中的物体模型进行匹配的设备；

用于使用匹配后的预测扫描群和匹配后的物体数据合并在先前采样时间时被识别为多个独立扫描群而现在在当前采样时间时被识别为单个扫描群的预测物体模型的设备；和

用于使用匹配后的预测扫描群和匹配后的物体数据拆分在先前采样时间时被识别为单个扫描群而现在在当前采样时间时被识别为多个独立扫描群的预测物体模型的设备。

20.根据权利要求19的系统，其中，用于分割点云中的扫描点的设备分离点云中的扫描点群使得群标示出正被追踪的独立物体。