CN103308923A - 来自多个激光雷达的距离图像配准方法 - Google Patents

Abstract

本发明涉及来自多个激光雷达的距离图像配准方法，具体地，一种用于配准从来自由车辆上的多个激光雷达传感器探测到的物体的距离图像的系统和方法。该方法包括在传感器信号融合操作中将来自具有交叠视场的至少两个激光雷达传感器的数据帧对准，以追踪由传感器探测到的物体。该方法定义识别传感器的方位角和位置的用于激光雷达传感器中至少一个的变换值，并且提供来自由传感器探测到的物体的目标扫描点，其中对每个传感器的目标扫描点提供独立的目标点图。该方法采用当前变换值将来自所述至少一个传感器的目标点图投射到激光雷达传感器中的另外一个以交叠来自传感器的目标扫描点。

Description

来自多个激光雷达的距离图像配准方法

相关申请的交叉引用

本申请要求2012年3月15日提交的题为“Method forRegistration of Range Images From Multiple LiDARS”的美国临时专利申请序号61/611465的优先权日期的权益。

技术领域

本发明总体涉及一种用于对来自多个激光雷达传感器的距离图像(range image)进行配准(register)的系统和方法，并且更具体地涉及一种对来自车辆上的多个激光雷达传感器的距离图像进行配准的系统和方法，其中该方法包括对同时来自多个激光雷达传感器的两个数据帧进行配准。

背景技术

许多现代的车辆包括物体探测传感器，其用于实现碰撞警报或者碰撞避免以及其他主动的安全应用。物体探测传感器可使用任意多种的探测技术，诸如短距离雷达、具有图像处理的照相机、激光或激光雷达(LiDAR)、超声波等。物体探测传感器探测车辆以及处于主车辆的路径中的其他物体，并且应用软件使用物体探测信息以提供警告或者采取适当的动作。在许多车辆中，物体探测传感器直接被集成到车辆的前缓冲器或者其他的仪表板中。

为了使得应用软件最优地执行，物体探测传感器必须与车辆适当地对准。例如，如果传感器探测处于主车辆的路径中的物体，但由于传感器没有对准，传感器确定物体稍微处于主车辆的路径的左边，则这会对应用软件具有重大后果。即使在车辆上具有多个前视物体探测传感器，它们被适当地对准也是重要的，以便最小化或者消除不一致的传感器读数。

激光雷达传感器是一种传感器的类型，其有时候用在车辆上以探测车辆周围的物体并提供至这些物体的距离。激光雷达传感器是期望的，因为它们能够提供被跟踪的物体的驶向，而其他类型的传感器，诸如视觉系统以及雷达传感器，一般不能做到。对于一种类型的激光雷达传感器，来自物体的反射返回为作为点集群(cluster)距离图的部分的扫描点，其中为穿过传感器视场每1/2°而提供单独的扫描点。因此，如果探测到目标车辆在主车辆的前面，则可具有被返回的多个扫描点，其辨别目标车辆和主车辆之间的距离。

车辆可具有多个激光雷达传感器以提供绕车辆的360°视场。这些多个激光雷达传感器可包括旁视传感器、后视传感器和前视传感器。所述传感器中的每个独立于其他传感器在其视场中追踪物体。使用来自多个传感器的扫描点返回，产生距离图形以追踪靠近主车辆的物体。对于具有多个激光雷达传感器的车辆，多个点集群图被返回，并且对于交叠的传感器视场，传感器可以追踪相同的物体。有必要组合所述传感器的扫描点图，以使传感器所追踪的相同物体被处理为单个的目标。

被转让给本申请的受让人并且这里通过参考的方式而引入的于2010年11月9日提交的题为“Systems and Methods for TrackingObjects(用于追踪物体的系统和方法)”的美国专利申请序号12/942456，公开了一种使用激光雷达传感器用于监控目标物体的驶向和与主车辆的距离。该申请限于单个激光雷达传感器，并且没有讨论融合从多个激光雷达传感器的返回。

发明内容

根据本发明的教导，公开一种用于配准从来自由车辆上的多个激光雷达传感器探测到的物体的距离图像的系统和方法的系统和方法。该方法包括在传感器信号融合操作中将来自具有交叠视场的至少两个激光雷达传感器的数据帧对准，以追踪由传感器探测到的物体。该方法定义识别传感器的方位角和位置的用于激光雷达传感器中至少一个的变换值，并且提供来自由传感器探测到的物体的目标扫描点，其中对每个传感器的目标扫描点提供独立的目标点图。该方法采用当前变换值将来自所述至少一个传感器的目标点图投射到激光雷达传感器中的另外一个以交叠来自传感器的目标扫描点，并且采用当前变换值确定多个权重值，其中每个权重值将用于所述至少一个传感器的扫描点中之一的位置变化识别到所述传感器中的所述另外一个的扫描点的位置。该方法采用所述权重值计算新的变换值，将所述新的变换值与所述当前变换值进行比较以确定它们之间的差值，并且基于所述新的变换值与所述当前变换值之间的所述差值修正所述多个权重值，直到所述新的变换值匹配所述当前变换值，从而传感器被校准。

本发明还提供如下方案：

1.一种用于融合来自具有交叠视场的至少两个激光雷达传感器的传感器信号以追踪由所述传感器探测到的物体的方法，所述方法包括：

定义用于所述激光雷达传感器中至少一个的变换值，其识别所述传感器的方位角和位置；

提供来自由所述传感器探测到的物体的目标扫描点，其中对每个传感器的所述目标扫描点提供独立的目标点图；

采用当前变换值将来自所述至少一个传感器的目标点图投射到所述激光雷达传感器中的另外一个，以交叠来自所述传感器的目标扫描点；

采用当前变换值确定多个权重值，其中每个权重值将用于所述至少一个传感器的扫描点中之一的位置变化识别到所述传感器中的所述另外一个的扫描点的位置；

采用所述权重值计算新的变换值；

将所述新的变换值与所述当前变换值进行比较以确定它们之间的差值；以及

基于所述新的变换值与所述当前变换值之间的所述差值修正所述多个权重值，直到所述新的变换值匹配所述当前变换值。

2.根据方案1所述的方法，其特征在于，所述激光雷达传感器位于车辆上。

3.根据方案1所述的方法，其特征在于，还包括为所述传感器中的所述至少一个传感器选择被初始地用作所述当前变换值的初始变换值。

4.根据方案3所述的方法，其特征在于，选择初始变换值包括从由制造商设定值、零以及估计的值构成的组中选择初始变换值：。

5.根据方案1所述的方法，其特征在于，确定多个权重值包括使用方程：

其中a_jk是用于具体扫描点的权重值，s_j是来自所述激光雷达传感器中的所述至少一个激光雷达传感器的扫描点，m_k是来自所述激光雷达传感器中的所述另外一个的扫描点，T是变换值，以及K是核函数，被定义为：

其中σ是方差。

6.根据方案1所述的方法，其特征在于，还包括确定当前变换值是否大于预定阈值，其中如果所述当前变换值大于预定阈值，那么所述变换值基于旋转矩阵和平移矢量而被定义。

7.根据方案6所述的方法，其特征在于，定义变换值包括采用粒子群优化方法，其包括：产生穿过预定数量的适格变换值的多个粒子；将归一的权重关联到每个粒子；为每个粒子设定最佳变换值；并且提供名义变换值，其具有从所述激光雷达传感器中的所述至少一个激光雷达传感器到所述激光雷达传感器中的所述另外一个的最佳匹配百分比。

8.根据方案1所述的方法，其特征在于，还包括确定从一个取样时间到下一个取样时间的变换值的变化是否低于预定阈值，并且如果是，则减小计算的复杂性以便确定当前变换值。

9.根据方案8所述的方法，其特征在于，确定当前变换值是否低于预定阈值包括将当前变换值建模为所述扫描点之间的变换。

10.根据方案1所述的方法，其特征在于，定义变换值包括使用期望最大化算法。

11.根据方案1所述的方法，其特征在于，提供目标扫描点包括提供目标扫描点为双向图。

12.根据方案1所述的方法，其特征在于，还包括提供其他类型的传感器，操作以探测物体并提供提示信号以开始传感器融合操作。

13.根据方案12所述的方法，其特征在于，所述其他类型的传感器包括雷达传感器和视觉系统。

14.一种用于融合来自具有交叠视场的至少两个激光雷达传感器的传感器信号以追踪由所述传感器探测到的物体的方法，所述激光雷达传感器位于车辆上，所述方法包括：

定义用于所述激光雷达传感器中至少一个的当前变换值，其识别所述传感器在一个取样时间的方位角和位置；

提供由来自所述传感器探测到的物体的目标扫描点，其中对于每个传感器的所述目标扫描点提供独立的目标点图；

采用所述当前变换值将来自所述至少一个传感器的目标点图投射到所述激光雷达传感器中的另外一个，以交叠来自所述传感器的目标扫描点；

在下一个取样时间计算新的变换值；

将所述新的变换值与所述当前变换值进行比较以确定它们之间的差值；以及

基于所述差值更新所述当前变换值。

15.根据方案14所述的方法，其特征在于，还包括确定所述当前变换值是否大于预定阈值，其中如果所述当前变换值大于所述预定阈值，那么所述变换值基于旋转矩阵和平移矢量来定义。

16.根据方案15所述的方法，其特征在于，定义变换值包括采用粒子群优化方法，其包括：产生穿过预定数量的适格变换值的多个粒子；将归一的权重关联到每个粒子；为每个粒子设定最佳变换值；并且提供名义变换值，其具有从所述激光雷达传感器中的所述至少一个激光雷达传感器到所述激光雷达传感器中的所述另外一个的最佳匹配百分比。

17.根据方案14所述的方法，其特征在于，还包括确定从一个取样时间到下一个取样时间的变换值的变化是否低于预定阈值，并且如果是，则减小计算的复杂性以便确定当前变换值。

18.根据方案17所述的方法，其特征在于，确定当前变换值是否低于预定阈值包括将当前变换值建模为所述扫描点之间的变换。

19.根据方案14所述的方法，其特征在于，定义变换值包括使用期望最大化算法。

通过结合附图所做的以下描述以及所附权利要求，本发明的其他特征将变得显而易见。

附图说明

图1是跟随目标车辆的主车辆的图示，并且示出了在主车辆上的四个激光雷达传感器的视场；

图2是用于融合来自车辆上的多个激光雷达传感器的扫描点的融合系统的总体框图；

图3(A)和(B)示出了来自激光雷达传感器的扫描点；

图4是流程图，示出了用于配准来自多个激光雷达传感器的扫描点的方法；

图5是物体后部的轮廓概率密度函数；

图6是迭代边界优化图；

图7是方框图，示出了估计物体移动和当新的激光雷达扫描点图可用时更新物体模型的两个交织过程；

图8是用于由追踪算法追踪的物体的动态贝叶斯网络；

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

图10是双向图，示出了用于图9的图示中将扫描集群与预测的物体模型进行匹配的步骤的预测的物体模型点和分段扫描图点；

图11是从图10中示出的双向图产生的诱导双向图；

图12是来自激光雷达传感器、视觉系统以及雷达传感器的扫描点的图示；

图13是跟随目标车辆的主车辆的图示，并示出了位于主车辆上的激光雷达传感器、雷达传感器以及视觉系统的视场；

图14是用于采用来自雷达传感器或者视觉系统提示的激光雷达传感器的追踪算法的总体流程图。

图15是流程图，示出了采用来自雷达传感器或者视觉系统提示的多个物体追踪算法的过程；

图16是双向图，示出了对所有的激光雷达扫描点、雷达传感器返回和视觉系统图像而将目标数据与物体模型进行匹配；

图17是产生自图16中示出的双向图的诱导双向图；

图18是双向图，示出了通过使用视觉系统提供匹配来将两个投射的物体模型合并；

图19是图18的诱导双向图，示出了来自图18的物体模型的匹配；

图20是双向图，示出了通过使用视觉系统进行匹配而分离所投射的物体模型；

图21是示出匹配来自图20的物体模型的诱导双向图；

图22是示出通过使用雷达传感器进行匹配的所投射的物体模型的双向图；

图23是示出匹配来自图22的模型的诱导双向图；以及

图24是通过来自雷达传感器或者视觉系统的提示信息的追踪算法和模型更新的动态贝叶斯网络。

具体实施方式

以下对用于来自位于车辆上的多个激光雷达传感器的距离图像进行配准的系统和方法的本发明的实施例的讨论，实质上仅是示例性的，并且不意在对本发明或者其应用或者使用进行限制。例如，将对位于车辆上的激光雷达传感器的距离图像的配准进行描述本发明。然而，如本领域技术人员将理解的，本发明的配准过程将具有车辆应用以外的应用。

图1示出包括主车辆12的车辆系统10的总览，主车辆12被示出追随其正追踪的目标车辆14。主车辆12包括四个激光雷达传感器，即：具有视场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将为被传感器探测到的每个单独的物体提供扫描点云(point cloud)。本发明提出一种融合算法，其将每个传感器16、20、24和28的输出组合使得：在传感器16、20、24和28追踪相同的物体时，该物体被处理为单个目标，其中该算法输出每个被追踪物体的位置、方位以及速度。虽然该讨论包括四个激光雷达传感器，但所提出的融合算法将适用于任意数量和位置的具有交叠视场的多个激光雷达传感器。

目标车辆14的位置在该图中由定位点34即扫描图的中心来表示。以下值被用于表示在时间步t处目标车辆14的物体模型M。特别地，物体模型M定义相对的纵向速度Vx、相对的横向速度Vy、横向位移y、以及目标车辆的驶向ξ或者目标的对地速度矢量的方向。值M是由参数平均值m_j和方差σ²表示的高斯分量的列表。该平均值由多个超参数v_j，η_j，k_j以及标记(即，访问过的以及到期的)来表征。

图2是用于融合来自多个激光雷达传感器诸如传感器16、20、24和28的扫描点云返回的融合系统36的示意方框图。框38表示来自左激光雷达传感器24的扫描点云返回，框40表示来自右视激光雷达传感器28的扫描点云返回，框42表示来自前视激光雷达传感器16的扫描点云返回，以及框44表示来自后视激光雷达传感器20的扫描点云返回。来自激光雷达传感器16、20、24和28的距离图被配准并且在框16中构建360°距离图(点云)。一旦来自激光雷达传感器16、20、24和28的点云被配准并且在车辆坐标系中形成360°点云，那么在框48中，算法融合来自多个目标的点云，以下进行详细描述。在目标在车辆坐标系框架中被融合之后，在框50中，算法输出目标的位置、方位以及速度。

在具体讨论用于组合来自多个激光雷达传感器的扫描点的融合过程之前，提供在框46中执行的扫描点配准算法的讨论以在对应于物体的物体模型M和当前扫描图S可获得时估计物体的移动。

在许多车辆中，物体探测传感器被直接集成到车辆的前部仪表板中。该类型的安装简单、有效且美观，但具有缺点：不能实际地去物理地调整传感器的对准。因此，如果由于仪表板的损坏或者老化以及与天气有关的变形引起传感器与车辆的实际驶向不对准，则除了替换包含传感器的整个仪表板组件之外，传统上没有办法校对该不对准。

如以下将讨论的，在框46中执行的帧配准匹配来自传感器20、24和28的距离扫描点，以适应传感器20、24和28的位置和方位的可能漂移。当车辆12是新的时，传感器20、24和28被初始地校准。如提到的，各种因素引起那些方位随着时间改变，因此需要实施方法来对传感器的方位进行再校准，使得在视场26和30的交叠部分中探测到的物体更精确。本发明提出期望最大化(EM)匹配算法来寻找限定传感器的方位角以及x和y位置的多个激光雷达传感器之间的变换T。例如，算法将匹配从左视激光雷达传感器24到右视激光雷达传感器28的变换T，然后当来自随后计算的变换T匹配时，传感器24和28将得到对准。

通过选择初始变换值T₀(其可以是零、之前估计的值、由制造商提供的在传感器24和28之间的方位等)，EM算法开始。然后该算法通过使用变换T将左传感器距离图投射到用于右视激光雷达传感器28的帧。图3是扫描点图120的表示，其中圆圈124表示来自左激光雷达传感器24的扫描点返回，而椭圆126表示来自右视激光雷达传感器28的扫描点返回。图3(A)示出了所有的扫描点返回，而图3(B)示出了对于几个扫描点返回的在圆圈122中的放大区域。图3(B)示出了左视激光雷达传感器扫描点返回124如何通过箭头128被映射到右视激光雷达传感器扫描点返回126。通过使用当前可获得的用于投射图箭头128的变换T，左视激光雷达传感器扫描点返回124相对于右视激光雷达传感器扫描点返回126移动以尝试使得它们交叠。

当前使用的变换T对于左视激光雷达传感器24到右视激光雷达传感器28的当前方位可能不准确，使得对于传感器24和28的当前位置来说变换T需要被更新。算法使用当前变换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是从左激光雷达传感器24到右激光雷达传感器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作为从左激光雷达传感器24到右激光雷达传感器28的变换的名义值，其中w_k＝max(w_i)。

在大多数情况下，变换T从一个采样时间到下一个的变化一般为小，并且因此，不需要对于如上所述的变换T的巨大变化而使用计算密集型处理来计算新的变换T’。特别地，如果名义变换T_n由如上的PSO算法估算，那么变换T’的新的估算可以使用如下的少的计算EM匹配算法而递归改进。首先，变换T_n被应用于右激光雷达传感器28的每个扫描点如：

S_j←T_nοS_j.    (17)

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

ΔT：x′＝ΔTοx，    (18)

并且定义为：

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^{\′}-t_y^{\′}-y_k)x_k-(x_j^{\′}-t_x^{\′}-x_k)]+\mathrm{\λ}{\mathrm{\ϵ}}_0}{{\mathrm{\Σ}}_{j,k}{a}_{\mathrm{jk}}(x_k^2+y_k^2)+\mathrm{\λ}},---\left(27\right)$

并且其中λ是对于之前的估算被使用多少来说可被调节的权重因子。

图4是流程图130，示出了对准激光雷达传感器例如如上所述的激光雷达传感器24和28的操作。在框132中，算法选择初始变换T_n诸如制造商设定。在框134中，算法收集来自激光雷达传感器24和28的扫描距离图数据，并且确定是否存在足够量的来自激光雷达传感器24和28的视场中的物体的扫描点以在判定菱形136处充分地执行计算。如果扫描点不足够，那么算法返回至框134以收集更多距离图数据。如果在判定菱形136处具有足够的扫描点，那么算法使用EM匹配算法以在框138处寻找修正的变换ΔT。然后该算法在判定菱形140处确定修正的变换ΔT是否大于预定阈值，并且如果是，那么在框142处，通过使用PSO算法而采用大变换估算以寻找新的名义变换T_n。如果在判定菱形140处匹配百分比不大于阈值，那么算法返回至框134以收集下一扫描点。

返回至融合算法，以下讨论首先呈现提出的扫描点配准算法，其在给定对应于目标车辆14的物体模型M和当前扫描图S时估算目标车辆14的移动。以上关于用于确定对准激光雷达传感器之间的帧的变换T的EM算法的讨论提供空间匹配，具体地在同一时间来自不同激光雷达传感器的两个帧之间的匹配。涉及扫描点配准的该讨论还使用点集合配准算法以寻找暂时匹配在当前扫描图S和源自过去的扫描图的物体模型M之间的两个帧的变换T。

首先定义概率性的物体模型M，然后提供建议的迭代算法以寻找严格(rigid)的变换使得似然性得到最大化，给定随后的帧的扫描图的情况下。为了表征物体的几何形状，在样本空间

中定义轮廓概率密度函数(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窗的尺寸。

假设y是变换T_y的参数。算子被定义为变换算子T_y(·)，其具有参数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_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被假定是在位置y处的物体模型M的独立且同分布样本。

这里的目标是寻找L(S；y，M)相对于变换参数Y的最大值，其等价于寻找J(y；M)≡-LogL(S；y，M)的最小值如：

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

其中：

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

该局部最小值表明在随后的具有与在原点34处限定的p(x；M)相似的表达的帧中的物体的存在。

引入附加矩阵A≡{a_kj}，其中，对于j＝1，...，n_M和k＝1，...，n，来说，a_kj≥0，其中：对于所有的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)中，归一化因子

被吸收到可以被忽略的常数项中。当将Jensen不等式应用到方程(34)时方程(36)遵循。

如果负对数函数是凸的，方程(36)的最佳上界

可以通过取方程(36)的表达式和约束的拉格朗日乘子(见方程式(33))相对于a_kj的微分并且设定为0而导出。最优辅助变量可以由如下表示：

      (37)，对于j＝1，...，n_M和k＝1，...，n_S。

因此，方程(31)可以通过“边界优化”而迭代求解，其在关于附加矩阵A、变换T的参数y以及核函数σ的带宽的优化

之间交替。

图6是具有y在水平轴上的图形，示出了迭代的边界优化关系，其中线52是复杂曲线J(y；M)，而虚曲线是在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{\σ}}(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}_{\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^*被用作对于变换的所估计的参数。

如下讨论描述可用在具有激光雷达扫描器的机器人的点集合配准的特殊情况。可以写出严格的变换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＝1。

取方程(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)$

其中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_O，...，S_t和S_t+1分别是取样自时间步0，...，t，和t+1处的动态物体的扫描图。

图7示出了追踪可以被处理为i)估计物体的移动的问题，以及ii)当收到新的帧扫描图时更新物体模型M的问题。尤其是，图7示出过程60，用于在框63中估计物体位置，使用物体位置以在框62中更新物体模型并且当获得新的扫描图时估计物体位置。

以下讨论将包括参照双向图和贝叶斯图。这两种类型的图形包括代表不同事情的节点。在贝叶斯图中的节点表示将被估计的变量，诸如变换T以及物体模型M，其是随机的，并且可仅由PDF表示。这些节点穿过不同时间帧而序列布置，并且仅每个序列建模物体。相反，在双向图中的节点是扫描图的分段。每个扫描图在相同时间帧可包含多个物体。因此，目的是寻找每个对应于物体的分段。通过建立分段(s₁，s₂，...，s_n)沿时间轴的关联，可以获得多个序列，其中每个对应于物体。以这种方式，可以使用双向图以将贝叶斯方法应用到追踪每个单独物体。

图8描述了表示提出的追踪算法的两个时间步的动态贝叶斯网络70。在网络70中，节点72表示变换参数y_t和y_t+1，即，物体位置和目标的姿态(attitude)，节点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)被建模为由其中心参数化的狄拉克δ分布(dirac delta distribution)，而物体模型M建模为分别由其均值{m_j}和固定的方差σ²参数化的高斯分量的联合PDF。每个均值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))是参数y在时间步t-1处的先验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(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所示，物体模型M在时间步t处的先验数据是：

现在考虑估计后验p(y，M|S^(0:t+1))在时间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}\left|\right|}_Q^2,---\left(55\right)$

其中

以及

因此，方程(38)可以替换为方程(55)以便集成之前时间步的先验信息，然后应用算法1以得出

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

注意到方程(56)是对于所有可能的y值的J(M)的最佳上边界。

如图6所示，可以使用边界优化而通过寻找上述的上边界函数的最小值来迭代地寻找最优物体模型M。设定方程(56)关于物体模型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/p_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)中的m_j ^*是模式，而方程(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)如果

大于阈值，则使用方程(60)和(61)计算超参数，设定粒子为已访问，并且将访问的量K_j增加1。如果K_j＞5，则第j粒子被标记为到期的。

4)对所有k计算Q_k＝Σ_ja_kj。如果

小于阈值，则增加新粒子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)的扫描图，并设定

是具有顶点集

的无向图。如果||p₁-p₂||小于预定的距离阈值，则边缘(p₁，p₂)∈E链接p₁和p₂。然后使用连接的分量标签来将扫描图分割成集群列表

将扫描点分割成集群包括在返回的点云中隔离扫描点的集群，以使集群识别被追踪的独立的物体。

一旦扫描点被分割成集群，算法然后在框88中将集群与预测的物体模型

进行匹配。具体地，算法在框90中使用物体文件78投射在之前时间步处的所追踪的物体模型M。为了预测投射的物体模型，假定

是在时间步t处的物体模型M的列表。对于每个物体模型

高斯分量的平均值表示为

如果

是在时间步t的变换的估值，则在时间步t＋1处的预测的变换为

因此，第n个物体的预测的物体模型

对于所有j来说是

通过

预测的物体点可以从时间步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)使得

和q′∈q时在

和之间存在边缘。

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

$c(p^{\text{'}},q^{\text{'}})=\frac{\left|E(p^{\text{'}},q^{\text{'}})\right|}{\left|q^{\text{'}}\right|},---\left(64\right)$

一旦诱导双向图B’被构建，并且边缘的权重和基数被计算，使用以下的算法3来选择强的边缘和被修剪的弱链接。

图10是双向图100，即B，处于在如上讨论的当前的时间步t的五个预测的物体模型和五个集群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处的扫描图中去除，而在时间t+1处S₅达到。在算法3的步骤3中，边缘

和

被高亮显示。在步骤4中，

与S₂匹配，边缘

被高亮显示。因此，在步骤5中，虚线

被修剪为弱链接。图11说明了

直接与S₁匹配，然而，两个其他情况需要集群合并和分离。

预测的物体模型

被分离成两个匹配的集群S₃和S₄。当前的集群S₃和S₄的扫描点，采用在之前的帧中被用于物体的现有知识而将被重新集群。常常，需要更强的差异性证据将存在的所追踪物体分离成两个集群。

两个预测的物体模型和

在当前的帧中与集群S₂相关联。集群S₂中的扫描点将被重新集群，因为为了将两个追踪的物体合并成一个物体需要在扫描点之间的更强近似性。

算法3：扫描关联算法

输入：集群

和预测的物体模型

输出：修剪的诱导双向图B’。

1)建立具有作为顶点的扫描点的双向图B。

2)建立具有作为顶点的集群和轮廓的诱导图B’，并且分别使用方程(63)和(64)计算边缘的权重和基数。

3)对于每个

寻找

以使基数大于阈值(即，c(p′_*，q′)＞C)以及

并且高亮显示边缘(p′_*，q′)。

4)对于没有被高亮边缘所覆盖的每个

寻找集

以使每个元素p″被一个且仅一个高亮边缘覆盖。寻找p″以使(p″，q″)＞c和

并且高亮显示边缘(p″，q″_*)。

5)修剪在B’中没有被高亮显示的全部边缘。

如上所提到的，在框94中创造新的物体并且删除消失的物体。尤其是，两种特殊的情况需要处理以创造新的物体以及移除物体文件中的存在的物体，其中扫描集群S₅不存在引入的边缘。将触发追踪初始化步骤，并且新的物体将被增加到物体文件中用于扫描集群S₅，而预测的物体模型

消失，因为没有从其引入的边缘，并且将从物体文件中移除。

一旦在框94中新的物体被创造并且消失的物体被删除，算法然后在框96中提供追踪和模型更新。对于每一个匹配的物体模型M_t和扫描集群S_t+1对，例如假定提供算法1的扫描配准过程，算法2用于追踪和物体模型更新。

在框98中，物体文件78然后被更新并为接下来的时间步存储为物体文件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和t-1之间的持续。

以下更新法则可以导出：

$t_x=\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{'}}+{\mathrm{\ϵx}}_j-y_j)+{\mathrm{\λ}}_y{\tilde{t}}_y}{{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}+{\mathrm{\λ}}_y},---\left(68\right)$

$\mathrm{\ϵ}=\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{\λ}}_{\mathrm{\ϵ}}\widetilde{\mathrm{\ϵ}}}{{\mathrm{\Σ}}_{k,j}{a}_{\mathrm{kj}}(x_j^2+y_j^2)+{\mathrm{\λ}}_{\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)$

如上所述的用于探测并追踪多个物体的该方法和设备可以提供360°视场方案用于由主车辆12检测物体。然而，需要解决用于激光雷达传感器的接下来的问题。

第一个问题由遮挡或者激光测量失效所导致的部分观测，例如，低的反射或者镜面反射。例如，黑色车辆的探测范围要明显小于白色车辆的探测范围。

接下来的问题是由于导致低的特征显著性的低分辨率引起的稀疏测量，以及因此不足以探测远场物体。

另外一个问题是有限的垂直视场，比如4°，这会导致缺失检测非平整路面上的物体。

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

本发明提出一种增强的融合算法以解决对于激光雷达传感器来说的如上述的限制。具体地，本发明使用来自位于车辆上的雷达传感器或者视觉系统中的一个或者两者的输出以作为提示信号来操作以使用于激光雷达传感器输出的融合系统辨别目标。雷达传感器一般具有远范围，但是窄的视场。视觉系统一般具有短视场，但是提供与被探测目标的接触。如上，被讨论的增强的融合系统被设计为估算每个所探测物体的参数x、y、v_x、v_y和ξ。

图12是如由主车辆上的激光雷达传感器、雷达传感器和视觉系统探测到的在道路152中显示的目标的图像150。具体地，目标处于足够远的距离之外，或者被遮挡，这样激光雷达扫描点非常少，如点156所表示。为了提高目标的探测，视觉系统图像154从视觉系统输出定义相同的目标，而雷达数据点158表示由雷达传感器探测到相同目标。

图13是用于追随并跟踪目标车辆164的主车辆162的与以上讨论的系统10相似的车辆系统160的图示。如图1中所示的那样，使用用于位置、速度、驶向角等的相同的变量。在该示例中，主车辆162包括具有视场168的激光雷达传感器166、具有视场172的雷达传感器170以及具有视场176的视觉系统174。从激光雷达传感器166的返回被示出为扫描点178，从雷达传感器170的返回被指定为三角形180，而从视觉系统174的返回表示为框182。如本领域所公知的，视觉系统和雷达传感器从目标给出四个输出，即，与目标的距离、与目标的距离的变化即距离变化率、目标的驶向角以及目标的方位角的变化即方位角变化率。

图14是提出的融合系统190的总览的方框图。在每个时间步，从雷达传感器170和/或视觉系统174所捕获的目标

被从框192发送至提示环境块194。如上讨论的，在框196处激光雷达扫描集群图提供至激光雷达追踪算法框198。追踪算法接收来自框194的提示环境目标数据，并且目标数据与存储的物体模型匹配，其中值n₀是探测到的目标的数量。每个目标o_i＝(x″_i，y″_i，v″_i，w″_i)具有纵向位移(x″)、横向位移(y″)、径向速度(v″)、以及横向速度(w″)的参数。具有每个扫描点s_k的激光雷达扫描图

包括纵向偏移x″_i和横向偏移y″_i，其中N是扫描点的数量。追踪算法监视来自提示环境框194和激光雷达扫描图的输入，并为每个探测的目标产生输出参数x、y、v_x、v_y和ξ。应用块200使用物体的估算参数，并实施车辆的主动安全应用，诸如自适应巡航控制，碰撞减轻制动，等。

图15是与如上讨论的流程图80相似的流程图204，其中相同的操作以相同参考号标识。流程图204包括框206，其读取来自雷达传感器170或者视觉系统174的目标数据。接下来，算法在框208中将目标数据与激光雷达坐标系框进行配准。为了完成此，假设y₀是严格的变换T的参数(平移和旋转)，变换T将来自雷达传感器170或者视觉系统174的目标映射到激光雷达坐标系中，使得激光雷达系中的映射的目标为： $o_i^{\′}=T_{y_o}\left(o_i\right),---\left(73\right)$

配准参数y₀可以通过在追踪的激光雷达物体和来自雷达传感器170或者视觉系统174的目标之间的匹配对来自动地估算。被转让给本申请的受让人并且通过参考并入这里的、于2011年8月7日授予的题为“Method and Apparatus for On-vehicle Calibration andOrientation of Object-Tracking Systems”的美国专利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，其中

o₃是用于创造新的物体的候选，而预测的物体模型被标记为将消失的，其可从物体文件中去除。

使用来自框90的投射的所追踪的物体模型

来自框88的具有投射的物体模型

的匹配的集群以及来自框210的具有物体模型的匹配的目标数据，算法在框212中提供物体模型M的合并和分离。考虑到两种情况，即，视觉目标和雷达目标。图18示出了对于两种预测的物体模型

和

被匹配到视觉目标O₁的情况的双向图250。来自视觉系统74的该提示信息提供这两个物体模型

和

需要合并为新的物体模型

的证据。

图19是诱导双向图252，示出了将在节点256处的物体模型

和

从节点254处的视觉目标O₁合并到单个物体目标

图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的程序，两种特殊情况需要被处理以产生新的物体以及从物体文件中删除即将消失的物体。对于每一个来自激光雷达段的不匹配的扫描集群S和每一个来自雷达传感器170或者视觉系统174的不匹配的目标，使用追踪初始程序以向物体文件中增加新的物体。对于物体文件中的物体，如果没有与来自激光雷达传感器166的分段集群S或者来自雷达传感器170或视觉系统174的目标的匹配，那么该物体被标记为消失。如果该目标在几个连续的时间步被标记为消失，则其将从物体文件中移除。

接下来，算法在框216中提供追踪和模型更新。为了完成这，假定o₀，...，o_t和o_t+1是在动态物体的激光雷达坐标系中分别由雷达传感器170或者视觉系统174在时间步0，...，t和t+1映射的测量值。通过

从时间步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，给定激光雷达扫描图和直到时间步t+1的来自雷达传感器170或者视觉系统174的目标数据，为：

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)时y在时间步t+1处的先验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))是中心处在

的狄拉克δ分布，其可以通过方程(74)关于y的最大化来估算。

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

o＝h(y)+v，    (80)

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

将方程(76)和(81)插入到方程(74)中，对方程74应用负对数，并且略去与y不相关的项，得到如下：

${\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.---\left(82\right)$

其中 $m_j=T_{{\stackrel{\‾}{y}}_t}\left(v_j\right),$ 和 ${\left|\right|\mathrm{\α}\left|\right|}_{\mathrm{\Σ}}^2={\mathrm{\α}}^T{\mathrm{\Σ}}^{-1}\mathrm{\α}.$

因此，算法1中的方程(38)可以替换为方程(55)，以便集成先前时间步的先验信息和来自雷达传感器170或者视觉系统174的提示(cueing)信息，然后应用算法1以得到

当计算p(M/S^(0:t+1)，O^(0:t+1))时，注意到方程(78)和方程(53)时是等价的，因此应用与如上描述相同的超参数的更新法则。

物体文件78然后以如上讨论的方式在框98中被更新。

如本领域技术人员所很好地理解的，这里讨论的用于描述本发明的若干和不同的步骤和程序可指由计算机、处理器或者使用电现象操作和/或转变数据的其他电子计算设备执行的操作。那些计算机和电子设备可采用各种易失性和/或非易失性的存储介质，包括具有存储在其中的可执行程序(包括可以通过计算机或者处理器执行的各种代码或者可执行指令)的非瞬时性计算机可读介质，其中存储器和/或计算机可读介质可包括所有形式和类型的存储器以及其他的计算机可读介质。

前面公开的讨论和描述仅仅是本发明的示例性实施例。本领域技术人员从该讨论以及从附图和权利要求容易意识到可对本发明做出各种改变、修改和变化，而不偏离由随附权利要求限定的本发明的精神和范围。

Claims (10)

1.一种用于融合来自具有交叠视场的至少两个激光雷达传感器的传感器信号以追踪由所述传感器探测到的物体的方法，所述方法包括：

定义用于所述激光雷达传感器中至少一个的变换值，其识别所述传感器的方位角和位置；

提供来自由所述传感器探测到的物体的目标扫描点，其中对每个传感器的所述目标扫描点提供独立的目标点图；

采用当前变换值将来自所述至少一个传感器的目标点图投射到所述激光雷达传感器中的另外一个，以交叠来自所述传感器的目标扫描点；

采用当前变换值确定多个权重值，其中每个权重值将用于所述至少一个传感器的扫描点中之一的位置变化识别到所述传感器中的所述另外一个的扫描点的位置；

采用所述权重值计算新的变换值；

将所述新的变换值与所述当前变换值进行比较以确定它们之间的差值；以及

基于所述新的变换值与所述当前变换值之间的所述差值修正所述多个权重值，直到所述新的变换值匹配所述当前变换值。

2.根据权利要求1所述的方法，其特征在于，所述激光雷达传感器位于车辆上。

3.根据权利要求1所述的方法，其特征在于，还包括为所述传感器中的所述至少一个传感器选择被初始地用作所述当前变换值的初始变换值。

4.根据权利要求3所述的方法，其特征在于，选择初始变换值包括从由制造商设定值、零以及估计的值构成的组中选择初始变换值：。

5.根据权利要求1所述的方法，其特征在于，确定多个权重值包括使用方程：

其中a_jk是用于具体扫描点的权重值，S_j是来自所述激光雷达传感器中的所述至少一个激光雷达传感器的扫描点，m_k是来自所述激光雷达传感器中的所述另外一个的扫描点，T是变换值，以及K是核函数，被定义为：

其中σ是方差。

6.根据权利要求1所述的方法，其特征在于，还包括确定当前变换值是否大于预定阈值，其中如果所述当前变换值大于预定阈值，那么所述变换值基于旋转矩阵和平移矢量而被定义。

7.根据权利要求6所述的方法，其特征在于，定义变换值包括采用粒子群优化方法，其包括：产生穿过预定数量的适格变换值的多个粒子；将归一的权重关联到每个粒子；为每个粒子设定最佳变换值；并且提供名义变换值，其具有从所述激光雷达传感器中的所述至少一个激光雷达传感器到所述激光雷达传感器中的所述另外一个的最佳匹配百分比。

8.根据权利要求1所述的方法，其特征在于，还包括确定从一个取样时间到下一个取样时间的变换值的变化是否低于预定阈值，并且如果是，则减小计算的复杂性以便确定当前变换值。

9.根据权利要求8所述的方法，其特征在于，确定当前变换值是否低于预定阈值包括将当前变换值建模为所述扫描点之间的变换。

10.一种用于融合来自具有交叠视场的至少两个激光雷达传感器的传感器信号以追踪由所述传感器探测到的物体的方法，所述激光雷达传感器位于车辆上，所述方法包括：

定义用于所述激光雷达传感器中至少一个的当前变换值，其识别所述传感器在一个取样时间的方位角和位置；

提供由来自所述传感器探测到的物体的目标扫描点，其中对于每个传感器的所述目标扫描点提供独立的目标点图；

采用所述当前变换值将来自所述至少一个传感器的目标点图投射到所述激光雷达传感器中的另外一个，以交叠来自所述传感器的目标扫描点；

在下一个取样时间计算新的变换值；

将所述新的变换值与所述当前变换值进行比较以确定它们之间的差值；以及

基于所述差值更新所述当前变换值。

Images