CN104035439A - 用多个激光雷达传感器利用扫描点追踪物体的贝叶斯网络 - Google Patents

Abstract

本发明涉及用多个激光雷达传感器利用扫描点追踪物体的贝叶斯网络。具体地，提供了一种用于融合来自车辆上的多个LiDAR传感器的输出的系统和方法。该方法包括提供先前采样时间由这些传感器检测到的物体的物体档案，其中物体档案识别检测到的物体的位置、取向和速度。该方法还包括接收当前采样时间来自传感器视野中检测到的物体的多个扫描返回，并从这些扫描返回构造点云。然后该方法将点云中的扫描点分割为预测群，其中每个群初始地识别由传感器检测到的物体。该方法将预测群与从先前采样时间期间正被追踪的物体生成的预测物体模型进行匹配。该方法创建新的物体模型、删除正在消失的物体模型，并基于当前采样时间的物体模型来更新物体档案。

Description

用多个激光雷达传感器利用扫描点追踪物体的贝叶斯网络

相关申请的交叉引用

本申请要求2012年3月15日提交的名称为“Bayesian Network to TrackObjects Usmg Scan Points Using Multiple LiDAR Sensors”且序列号为No.61/611471的美国临时专利申请的优先权日期的权益。

技术领域

本申请总体上涉及融合来自车辆上多个传感器的输出的系统和方法，并且更特别地，涉及使用贝叶斯网络融合来自车辆上多个激光雷达(LiDAR)传感器的扫描点的系统和方法。

背景技术

许多现代车辆都包括有物体检测传感器，其被用于使能够实现碰撞警告或防止碰撞以及其他主动安全应用。物体检测传感器可以使用多种检测技术，例如，短程雷达、带有图像处理的摄像机、激光或激光雷达、超声波，等等。物体检测传感器检测主车辆路径中的车辆及其他物体，并且应用软件使用物体检测信息来提供警告或采取适当的措施。在许多车辆中，物体检测传感器被直接集成在车辆的前端或其他仪表盘中。

为了使应用软件被最优地执行，物体检测传感器必需关于车辆被正确地调准。例如，如果传感器检测到确实位于主车辆路径内的物体，但由于传感器未调准，所以传感器判断该物体稍微偏向主车辆路径的左侧，这将导致应用软件出现严重后果。即使车辆上有多个前向的物体检测传感器，将它们全部正确地调准也是非常重要的，以便将不一致的传感器读数最小化或去除。

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

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

2010年11月9日提交的序列号为No.12/942456且标题为“System andMethods for Tracking Objects”的美国专利申请被转让给本申请的所有人，在此通过引用被并入，其公开了一种使用LiDAR传感器从主车辆监控目标物体的距离和方向的技术。该应用局限于使用单个的LiDAR传感器，并且也没有公开对来自多个LiDAR传感器的返回进行融合。

发明内容

依据本发明的教导，公开了一种方法和系统，用于使用贝叶斯网络融合来自车辆上多个LiDAR传感器的输出。该方法包括：提供由LiDAR传感器在先前采样时间检测到的物体的物体档案，其中所述物体档案识别了所检测物体的位置、取向和速度。该方法还包括：接收来自当前采样时间的在传感器视野中检测到的物体的多个扫描返回；以及，从这些扫描返回来构造点云。然后，该方法将点云中的扫描点划分为预测扫描群，其中每个群初始地识别由传感器检测的物体。该方法将这些预测群与在先前采样时间期间从被追踪的物体生成的预测物体模型进行匹配。该方法合并下述这样的预测物体模型，这些预测物体模型在先前的采样时间中被识别为一些单独的扫描群，而现在则被识别为单一扫描群；并且，该方法拆分下述这样的预测物体模型，这些预测物体模型在先前的采样时间中被识别为单一扫描群，而现在在当前采样时间中则被识别为一些单独的扫描群。该方法创建新的物体模型、删除正在消失的物体模型，以及基于当前采样时间的物体模型来更新物体档案。

本发明还包括下列技术方案：

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

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

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

将来自所述先前扫描时间的这些物体档案中的被追踪的物体模型进行投影，以提供预测物体模型；

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

从这些扫描返回来构造扫描点云；

将所述点云中的扫描点分割为预测扫描群，其中每个扫描群初始地识别由这些传感器检测到的物体；

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

将在所述先前采样时间内被识别为分散的扫描群而现在在所述当前采样时间内被识别为单一扫描群的预测物体模型进行融合；

将在所述先前采样时间内被识别为单一的扫描群而现在在所述当前采样时间内被识别为分散的扫描群的预测物体模型进行拆分；

为所述当前采样时间内检测到而在所述先前采样时间内未出现的物体创建新的物体模型；

删除在所述预测扫描群中不再存在的物体模型；

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

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

方案2.根据方案1所述的方法，其中，分割所述点云中的扫描点包括：分离所述点云中的扫描点的群，使得这些群识别正被追踪的单独物体。

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

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

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

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

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

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

方案9.根据方案1所述的方法，其中，提供物体模型更新包括在从先前时间帧至当前时间帧的贝叶斯网络中识别目标数据、变换参数、所述物体模型和所述扫描图群。

方案10.根据方案1所述的方法，其中，更新物体档案包括修正所述传感器的变换式，其识别所述传感器的取向角和位置。

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

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

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

对来自所述先前扫描时间的这些物体档案中的被追踪的物体模型进行投影，以提供预测物体模型；

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

从这些扫描返回来构造扫描点云；

将所述扫描点云中的扫描点分割为预测扫描群，其中每个扫描群初始地识别由这些传感器检测到的物体；

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

将在所述先前采样时间内被识别为分散的扫描群而现在在所述当前采样时间内被识别为单一扫描群的预测物体模型进行融合；和

将在所述先前采样时间内被识别为单一的扫描群而现在在所述当前采样时间内被识别为分散的扫描群的预测物体模型进行拆分。

方案12.根据方案11所述的方法，其中，分割所述点云中的扫描点包括：分离所述点云中的扫描点的群，使得这些群识别正被追踪的单独的物体。

方案13.根据方案11所述的方法，其中，将所述预测扫描群与所述预测物体模型进行匹配包括：生成使这些扫描群与这些预测物体模型匹配的二部图。

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

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

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

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

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

用于提供先前采样时间时由这些LiDAR传感器检测到的物体的物体档案的装置，所述物体档案具有识别由这些传感器检测到的这些物体的位置、取向和速度的物体模型；

用于追踪由这些传感器检测到的这些物体的物体模型的装置；

用于对来自所述先前扫描时间的这些物体档案中的被追踪物体模型进行投影以提供预测物体模型的装置；

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

用于从这些扫描返回构造扫描点云的装置；

用于将所述扫描点云中的扫描点分割为预测扫描群的装置，其中每个扫描群初始地识别由这些传感器检测到的物体；

用于将所述预测扫描群与所述预测物体模型进行匹配的装置；

用于将在所述先前采样时间内被识别为分散的扫描群而现在在所述当前采样时间内被识别为单一扫描群的预测物体模型进行融合的装置；以及

用于将在所述先前采样时间内被识别为单一的扫描群而现在在所述当前采样时间内被识别为分散的扫描群的预测物体模型进行拆分的装置；

用于为所述当前采样时间内检测到而在所述先前采样时间内未出现的物体创建新的物体模型的装置；

用于删除在这些预测扫描群中不再存在的物体模型的装置；

用于基于融合的物体模型、拆分的物体模型、新的物体模型和删除的物体模型来提供物体模型更新的装置；和

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

方案19.根据方案18所述的系统，其中，用于将所述预测扫描群与所述预测物体模型进行匹配的装置生成二部图，所述二部图将所述扫描群与所述预测物体模型进行匹配。

方案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_xo,t_yo，ε_o)。

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

并且其解为：

$t_x^{\text{'}}=\frac{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}(x_j^{\text{'}}+{\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^{\text{'}}=\frac{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}(y_j^{\text{'}}-{\mathrm{\ϵx}}_k-y_k)+{\mathrm{\λt}}_{y0}}{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}+\mathrm{\λ}},---\left(26\right)$

${\mathrm{\ϵ}}^{\text{'}}=\frac{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}[(y_j^{\text{'}}-t_y^{\text{'}}-y_k)x_k-(x_j^{\text{'}}-t_x^{\text{'}}{-x}_k)y_k]+{\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的扫描距离图数据，并确定来自LiDAR传感器24和28视野中的物体的扫描点的充足数量是否足以执行菱形判决块136处的计算。如果扫描点不够，则该算法返回方块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，并且然后提供建议的迭代算法以寻找刚性变换式，使得似然值为最大，假定给出了后续帧的扫描图。为了表征物体的几何形状，定义样本空间中的轮廓概率密度函数(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)$

其中为高斯核函数，并且σ＞0为一平滑参数，其被称为例如带宽，或帕曾窗口(Parzen window)的尺寸。

令y为变换式T_y的参数。运算符被定义为带有参数y的变换运算符T_y(·)以及对应的逆变换不失一般性，物体模型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，…，s_ns)^T为包含一列扫描点s_k的当前扫描图。似然函数可以表示为：

$L(S;y,M)={\mathrm{\Π}}_{k=1}^{n_s}p(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)$

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

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

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

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

因此，公式(31)可以通过“边界最优化”来迭代求解，其在相对于辅助矩阵A的最优变换式T的参数y，和核函数σ的带宽之间交替。

图6为一水平轴上具有y的图形，其显示了迭代的边界最优化关系，其中线52是复杂曲线且虚线为在y₀、y₁和y₂处的连续上限，也就是说分别地，图线54为图线56为且图线58为

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

算法1：扫描点配准

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

1)令σ＝σ₀，其中σ₀为一个正数。

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

3)最小化计算y^*以最小化即：

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

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

${\mathrm{\σ}}^2=\frac{1}{n_s}{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{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^*＝arg min_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的运动以及对物体模型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_i和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)为物体运动的后续发展模型(plant model)的条件概率：

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

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

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

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

其中：

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

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

现在考虑对时间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{\σ}}(T_y^{-1}\left(s_k\right)-m_j)+{\left|\right|y-\tilde{y}||}_Q^2,---\left(55\right)$

其中且因此，公式(38)可以被公式(55)代替以对先前时间步骤中的先前信息进行积分，然后使用算法1以导出

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

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

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

$m_j^{*}=\frac{{\mathrm{\ρ}}_j{T}_{{\stackrel{\‾}{t}}_{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^{\text{'}}=\frac{1}{{\mathrm{\η}}_j+{\mathrm{\ρ}}_j}({\mathrm{\η}}_j{T}_{{\stackrel{\‾}{y}}_t}\left(v_j\right)+{\mathrm{\ρ}}_j{T}_{{\stackrel{\‾}{y}}_{t+1}}^{-1}\left({\stackrel{\‾}{S}}_j\right)).---\left(61\right)$

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

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

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

算法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＝∑_j a_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处该算法将云中的扫描点分割为可以识别特定物体的群。为了执行分割操作，令表示当前帧(t+1)的扫描图，且令G＝(R，E)为带有顶点集合的不定向图。如果||p₁-p₂||小于预定的距离阈值，则边(p₁，p₂)∈E联接p₁和p₂。然后该连接的分量标记被用于将扫描图分割为一列群将扫描点分割为群包括：在返回的点云中分离扫描点群，使得这些群识别正被追踪的单独物体。

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

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

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

令E(p’，q’)为B中边集的子集，也就是说，E(p’，q’)≡{(p，q)|(p，q)∈E_B∩p∈p’∩q’∈q}。那么权重(p’，q’)以及边(p’，q’)的基数被分别定义为：

$c(p^{\text{'}},q^{\text{'}})=\frac{|E(p^{\text{'}},q^{\text{'}})}{\left|q^{\text{'}}\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处的正在消失2正被从时间t+1处的扫描图中移除，且S₅在时间t+1处到达。在算法3的步骤3，边和被突出显示。在步骤4，与S₂匹配，边被突出显示。因此，在步骤5，虚线被作为脆弱连接而切断。图11显示了与S₁直接匹配，然而，其他两种情况需要进行群的融合和拆分。

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

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

算法3：扫描关联算法

输入：群和预测物体模型

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

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

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

3)对于每个寻技使得基数大于阈值(即C(p'_*，q’)＞C)且w(p'，q')，并突出显示边(p'_*，q’)。

4)对于每个没有被突出显示的边覆盖的寻找集合使得每个元素q"被且仅被一个突出显示的边覆盖。寻找p″使得(p″，q″)＞C且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_v)为变换式。

令m_j＝(x_j，y_j)和S_k＝(x_k’，y_k’)。将公式(65)和(66)代入公式(55)，分别对t_x，t_y和∈求偏导数并设为0。设表示公式(49)预测出的y。如果假定为固定模型，给出且δt为时间步骤t和t-1之间的持续时间。

可以导出下列更新规则：

$t_s=\frac{{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}(x_k^{\text{'}}+\∈y_j-x_j)+{\mathrm{\λ}}_x{\tilde{t}}_x}{{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}+{\mathrm{\λ}}_x},---\left(67\right)$

$t_y=\frac{{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}(y_k^{\text{'}}+\∈x_j-y_j)+{\mathrm{\λ}}_y{\tilde{t}}_y}{{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}+{\mathrm{\λ}}_y},---\left(68\right)$

$\∈=\frac{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{kj}}[(y_k^{\text{'}}-t_y-y_j)x_j-(x_k^{\text{'}}-t_x-x_j)y_j]+{\mathrm{\λ}}_{\∈}\widetilde{\∈}}{{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}(x_j^2+y_j^2)+{\mathrm{\λ}}_{\∈}},---\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{\ξ}={\tan }^{-1}\left(\frac{v_{\mathrm{gy}}}{v_{\mathrm{gx}}}\right).---\left(72\right)$

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

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

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

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

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

本发明提出了一种增强的融合算法，以解决上述有关LiDAR传感器的限制。特别地，本发明使用来自车辆上的雷达传感器和视频系统中的一者或两者的输出，以便作为提示信号来操作，使得用于LiDAR传感器输出的融合系统识别目标。通常，雷达传感器具有较远的范围，但视野较窄。而视频系统通常视野很短，但提供了与检测目标的接触x如上，所介绍的增强的融合系统被设计成估计每个检测物体的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″_i，w″_i)带有下述各项的参数，所述各项包括：纵向位移(x″)，横向位移(y″)，径向速度(v″)和横向速度(w″)。带有每个扫描点s_k的LiDAR扫描图包括纵向偏置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^{\text{'}}=T_{y_o}\left(o_i\right).---\left(73\right)$

该配准参数y_o可以通过被追踪的LiDAR物体和来自雷达传感器170或视频系统174的目标之间的匹配对来被自动地估计。2011年8月7日公告的名为“Method and Apparatus for On-vehicle Calibration and Orientation ofObiect-Tracking System”的美国专利No.7991550(其被转让给本申请的受让人，并且在此通过引用被并入)，公开了一种适用于该目的的技术。

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

上述程序被用于构造图17中所示的被删切的二部图240。所述图240显示了位于节点246处的物体运动242和位于节点248处的物体模型244，其中为用于创建新物体的候选，且预测物体模型被标记为正在消失且可以被从物体档案中移除。

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

图19为导出的二部图252，其显示了将来自节点254处的视频目标O₁的节点256处的物体模型和融合成一个单独的物体模型

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

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

图22为二部图270，其显示了一种情况，其中用于两个物体模型和的点集与三个雷达目标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，其中假定LiDAR扫描图以及来自雷达传感器170或视频系统174的目标数据一直到时间步骤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))

＝p(S|y，M)p(o|y)p(y，|S^(0∶t)，o^(0∶t))p(M|S^(0∶t)，o^(0∶t))，(74)

其中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可以写为：

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

现在考虑模型M的后验，其中：

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

现在考虑估计时间步骤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无关的项，可以得到下式：

$\begin{array}{c}{\stackrel{\‾}{y}}_{t+1}=\arg {\min }_y-{\mathrm{\Σ}}_{\mathrm{kj}}{a}_{\mathrm{kj}}\log K_{\mathrm{\σ}}(T_y^{-1}\left(s_k\right)-m_j)+\\ o-\mathrm{hyE}2+y-\mathrm{yQ}2,\end{array}---\left(82\right)$

其中且

因此，算法1中的公式(38)可以替换成公式(55)，以便对先前时间步骤的先验信息和来自雷达传感器170或视频系统174的提示信息进行积分，然后使用算法1导出

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

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

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

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

Claims (10)

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

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

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

将来自所述先前扫描时间的这些物体档案中的被追踪的物体模型进行投影，以提供预测物体模型；

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

从这些扫描返回来构造扫描点云；

将所述点云中的扫描点分割为预测扫描群，其中每个扫描群初始地识别由这些传感器检测到的物体；

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

将在所述先前采样时间内被识别为分散的扫描群而现在在所述当前采样时间内被识别为单一扫描群的预测物体模型进行融合；

将在所述先前采样时间内被识别为单一的扫描群而现在在所述当前采样时间内被识别为分散的扫描群的预测物体模型进行拆分；

为所述当前采样时间内检测到而在所述先前采样时间内未出现的物体创建新的物体模型；

删除在所述预测扫描群中不再存在的物体模型；

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

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

2.根据权利要求1所述的方法，其中，分割所述点云中的扫描点包括：分离所述点云中的扫描点的群，使得这些群识别正被追踪的单独物体。

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

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

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

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

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

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

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

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

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

对来自所述先前扫描时间的这些物体档案中的被追踪的物体模型进行投影，以提供预测物体模型；

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

从这些扫描返回来构造扫描点云；

将所述扫描点云中的扫描点分割为预测扫描群，其中每个扫描群初始地识别由这些传感器检测到的物体；

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

将在所述先前采样时间内被识别为分散的扫描群而现在在所述当前采样时间内被识别为单一扫描群的预测物体模型进行融合；和

将在所述先前采样时间内被识别为单一的扫描群而现在在所述当前采样时间内被识别为分散的扫描群的预测物体模型进行拆分。

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

用于提供先前采样时间时由这些LiDAR传感器检测到的物体的物体档案的装置，所述物体档案具有识别由这些传感器检测到的这些物体的位置、取向和速度的物体模型；

用于追踪由这些传感器检测到的这些物体的物体模型的装置；

用于对来自所述先前扫描时间的这些物体档案中的被追踪物体模型进行投影以提供预测物体模型的装置；

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

用于从这些扫描返回构造扫描点云的装置；

用于将所述扫描点云中的扫描点分割为预测扫描群的装置，其中每个扫描群初始地识别由这些传感器检测到的物体；

用于将所述预测扫描群与所述预测物体模型进行匹配的装置；

用于将在所述先前采样时间内被识别为分散的扫描群而现在在所述当前采样时间内被识别为单一扫描群的预测物体模型进行融合的装置；以及

用于将在所述先前采样时间内被识别为单一的扫描群而现在在所述当前采样时间内被识别为分散的扫描群的预测物体模型进行拆分的装置；

用于为所述当前采样时间内检测到而在所述先前采样时间内未出现的物体创建新的物体模型的装置；

用于删除在这些预测扫描群中不再存在的物体模型的装置；

用于基于融合的物体模型、拆分的物体模型、新的物体模型和删除的物体模型来提供物体模型更新的装置；和

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