确定自定位初始化的初始自我姿态的方法和系统

摘要

提供了一种确定自定位初始化的初始自我姿态的方法和系统。所述方法包括由计算机硬件组件执行的以下步骤：在地图中设置多个粒子；将所述多个粒子分组成多个聚类，各个聚类包括所述多个粒子的相应子集；在粒子过滤期间，基于所述多个聚类注入粒子；以及基于所述粒子过滤确定初始自我姿态。

说明书

技术领域

本公开涉及确定(例如车辆的)自定位初始化的初始自我姿态(ego-pose)的方法和系统。

背景技术

自定位是许多自主驾驶应用中的最重要部分。有多种方法来解决自我定位问题，诸如使用全球导航卫星系统(GNSS)、航位推算法或者同步定位与地图构建(SLAM)法。

可以将自定位问题分类成局部自定位和全局自定位这两大组。

如果初始自我姿态是未知的，则可以执行全局自定位。假设已知的初始自我姿态会导致局部自定位。例如，如果GNSS信号不可用，则必须使用其它手段来确定初始姿态。

因此，需要提供即使当GNSS信号不可用时也能确定车辆的初始姿态的有效且可靠的方法。

发明内容

本公开提供了一种计算机实现方法、计算机系统、车辆以及非暂时性计算机可读介质。

在一个方面，本公开针对一种确定自定位初始化的初始自我姿态的计算机实现方法，该计算机实现方法包括由计算机硬件组件执行(换句话说：实现)的以下步骤：在地图中设置多个粒子；将多个粒子分组成多个聚类，各个聚类包括所述多个粒子的相应子集；在粒子过滤期间，基于所述多个聚类(系统地(systematically))注入粒子；以及基于所述粒子过滤来确定初始自我姿态。

换句话说，在过滤器发散(filter divergence)的情况下可以基于聚类跟踪(系统地)来注入粒子。

根据另一方面，该计算机实现方法还包括由所述计算机硬件组件执行的以下步骤：确定多个聚类中的各个聚类的粒子的相应子集的相应位置的均值和方差。

已经发现，对各个聚类的均值和方差进行跟踪可以提供有效的数据管理，同时保持较低的处理工作量。通过对均值和方差进行更新，可以有效地跟踪有关各个聚类的位置(或区域)的信息。对于不再存在聚类的粒子的情形以及需要粒子注入的情形，可能就更是如此。利用更新后的均值和方差，可以获得有关聚类将位于何处的信息，就好像粒子仍将存在一样，并由此可以将该信息用于粒子注入。

根据另一方面，基于所述多个聚类注入粒子的步骤包括：基于相应均值和相应方差来注入粒子。例如，针对各个聚类，即使不再存在所述聚类的粒子，也可以根据该聚类的更新后的均值和该聚类的更新后的方差来将粒子注入到所述聚类将位于的位置。

根据另一方面，所述计算机实现方法还包括由所述计算机硬件组件执行的以下步骤：基于运动模型来更新所述多个聚类中的各个聚类的相应均值。

通过使用例如要执行自定位的车辆的运动模型，可以将所述车辆的运动用于对所述聚类的位置进行更新，这可以通过对所述聚类的均值(换句话说：所述聚类的重心)进行更新来表示。

根据另一方面，该计算机实现方法还包括由所述计算机硬件组件执行的以下步骤：基于有关自我姿态不确定度(ego-pose uncertainty)的不确定度传播律(uncertaintypropagation law)来更新多个聚类中的各个聚类的相应方差。

通过更新多个聚类中的各个聚类的相应方差，该多个聚类中的各个聚类的方差可以随时间增加(这可以对应于所述聚类的增加的区域)，这可以表示位置的不确定性。

根据另一方面，多个粒子是基于所述地图上的随机分布和/或基于自我姿态的估计中的至少一项来设置的。

如果未获得自我姿态的信息，则可以在地图上提供随机分布。否则，可以使用更集中于自我姿态的估计的分布。

根据另一方面，执行所述粒子过滤的步骤包括：样本分配、预测、更新和重采样。通过对多个粒子进行预测，可以确定所述多个粒子中的各个粒子在下一个时间步中的位置。通过更新，在考虑所述地图和传感器观测结果的情况下对样本进行加权。通过重采样，可以由合适的样本集合(换句话说：多个粒子)来表示针对后续时间步的粒子的位置。

根据另一方面，多个粒子是基于潜在聚类中的粒子的数量来被分组成所述多个聚类的。例如，可以重复地(或迭代地)执行聚类，直到潜在聚类中的粒子的数量满足预定标准为止，例如，直到多个聚类中的各个聚类中的粒子的数量高于预定阈值为止(换句话说：直到各个聚类至少包括预定数量的粒子为止)。

根据另一方面，所述多个粒子是基于潜在聚类的数量来被分组成所述多个聚类的。例如，可以重复地(或迭代地)执行聚类，直到所述聚类的数量满足预定标准为止，例如，直到所述聚类的数量低于预定阈值为止。

根据另一方面，所述计算机实现方法还包括由所述计算机硬件组件执行的以下步骤：如果聚类处于关注区域之外，则排除(exhausting)该聚类。例如，如果聚类处于地图之外，则可以排除该聚类(换句话说：可以从考虑中删除或者去除该聚类)。如果聚类是部分地处于关注区域之外，则可以仅将该聚类的有效部分(换句话说：该聚类中仍处于关注区域内的部分)用于粒子注入。

根据另一方面，所述计算机实现方法还包括由所述计算机硬件组件执行的以下步骤：接收从车辆的传感器系统的至少一个发射器发射的并且在所述车辆附近朝向所述传感器系统反射的电磁辐射。例如，所述传感器系统可以包括雷达传感器和/或LiDAR传感器和/或红外传感器。

根据另一方面，所述粒子过滤的步骤是基于所接收到的电磁辐射以及基于所述地图来执行的。作为例示，通过根据对基于电磁辐射获得的信息(例如，距离和/或角度信息)与有关所述地图中表示的静态对象的信息的比较来执行加权(更新)处理，可以获得所述位置的估计。

根据另一方面，所述初始自我位置是基于熵来确定的，所述熵是基于以二值化网格基础的监测的。

在另一方面，本公开针对一种计算机系统，所述计算机系统包括多个计算机硬件组件，所述多个计算机硬件组件被配置成执行本文所描述的计算机实现方法的若干或所有步骤。所述计算机系统可以是车辆的一部分。

所述计算机系统可以包括多个计算机硬件组件(例如，处理单元、至少一个存储器单元以及至少一个非暂时性数据存储部)。应理解，可以提供其它计算机硬件组件并将其用于在所述计算机系统中执行所述计算机实现方法的步骤。所述非暂时性数据存储部和/或存储器单元可以包括计算机程序，所述计算机程序用于指令所述计算机例如使用所述处理单元和所述至少一个存储器单元来执行本文所描述的计算机实现方法的若干或所有步骤或方面。

在另一方面，本公开针对一种车辆，所述车辆配备有适于接收从传感器系统的至少一个发射器发射的电磁辐射和在所述车辆附近朝向所述传感器系统反射的电磁辐射的传感器系统；以及确定车辆的自定位初始化的初始自我姿态的计算机系统，例如，如上所述的计算机系统。

在另一方面，本公开针对一种非暂时性计算机可读介质，所述非暂时性计算机可读介质包括用于执行本文所描述的计算机实现方法的若干或所有步骤或方面的指令。所述计算机可读介质可以被配置为：诸如光盘或数字通用盘(DVD)之类的光学介质；诸如硬盘驱动器(HDD)之类的磁介质；固态驱动器(SSD)；诸如闪速存储器之类的只读存储器(ROM)等。而且，所述计算机可读介质可以被配置为能够经由诸如互联网连接之类的数据连接来访问的数据存储部。所述计算机可读介质例如可以是在线数据存储库或云存储。

本公开还针对一种计算机程序，所述计算机程序用于指令计算机执行本文所描述的计算机实现方法的若干或所有步骤或方面。

附图说明

本文结合以下附图，对本公开的示例性实施方式和功能进行描述，所述附图示意性地示出了：

图1是根据各种实施方式的对具有粒子的地图进行粒子聚类的例示图；

图2是作为根据各种实施方式的并行过滤的示例，在具有三个生成的聚类的停车场中，根据各种实施方式的利用粒子过滤进行自我姿态初始化的场景的例示图；

图3是根据各种实施方式的聚类跟踪的例示图；

图4是根据各种实施方式的粒子过滤器监测参数的例示图；

图5是根据各种实施方式的在将过滤器发散识别成被跟踪的聚类之后的注入处理的例示图；以及

图6是例示了根据各种实施方式的对自定位初始化的初始自我姿态进行确定的方法的流程图。

具体实施方式

根据各种实施方式，可以使用地图来找到车辆的初始姿态，即，车辆开始移动的地方。例如，地图可以是OpenStreetMap和/或占据栅格地图(occupancy grid map)。该地图可以包括有关静态对象(诸如墙壁、柱子、枝条、房屋或护栏)的信息。可以在离散网格上提供由地图指示出的信息(因此该地图也可以被称为网格)。可以使用粒子过滤来找到车辆的初始姿态。可以将地图输入到粒子过滤器中，然后可以将过滤器初始化。初始化处理可以是整个区域中的样本(换句话说：粒子)的分布，在该区域中，初始自我姿态是未知的。虽然在理论上该区域可能是整个世界，但是通常可以获得一些有关初始自我姿态的粗略信息，诸如“车辆在停车场”或“车辆在城市中的这个区域”。

基于该初始的粗略信息，可以在该区域的地图内分布样本，并且可以执行粒子过滤。粒子过滤可以具有以下步骤：过滤器初始化(换句话说：样本分配)、预测、更新(换句话说：加权)以及重采样，如同下面更详细描述的那样。

在初始化之后，可以基于车辆移动信息(例如，偏航角速率和速度)以及车辆模型来对各个粒子的移动进行预测。基于更新后的样本姿态，可以基于传感器观测结果(例如，雷达、摄像头或LiDAR)与地图之间的比较来对各个样本进行加权。由于这种加权，一些样本相比于其它样本可能得到较高的权重。具有较高权重的粒子对其它样本的支配可能会导致被称为“退化(degeneracy)”的问题。为了避免这个问题，可以执行重采样，以将样本集中至样本权重较高的区域，这是因为车辆更可能位于这些区域。在经过一些采样时间之后，粒子会更集中在一个区域中，并且认为已找到初始自我姿态。所识别区域的大小可以由用户来定义，例如，用户可以为了初始化成功而定义5m

然而，一些因素可能导致过滤问题，甚或导致过滤器发散，这意味着过滤器收敛至错误的初始姿态。在过滤处理期间中可能会面临的一些常见问题是贫化(impoverishment)(这是指小区域中粒子的快速且高度集中)、退化(这是指许多样本的权重接近零的情形，使得样本权重之间会有很大的差异)或者过滤器发散(这是指过滤器的完全发散，使得初始化失败)。

粒子发散的来源可能是稀疏且有噪音的测量结果(例如在使用雷达的情况下，当观测结果是稀疏的时候)。

为了避免粒子过滤器发散，可以使用诸如基于不同传感器系统(例如基于雷达、LiDAR、摄像头或这些传感器的组合)的粒子注入的策略。如果识别出发散，则将新粒子注入整个初始化区域的过滤器中。然而，在整个初始化区域的过滤器中的这种粒子注入被认为是应被避免的过滤器重置。

根据各种实施方式，可以应用粒子聚类，这可以克服在有噪音和稀疏的测量结果或不准确的地图的情况下粒子过滤的发散问题。可以在整个区域上以预定分辨率提供二值化网格。可以在各个采样时间内对所有粒子执行二值化聚类。可以考虑样本数量超过预定阈值的聚类。所有的聚类可以表示直到聚类时间为止时根据测量结果的自我姿态的概率较高的地图区域。

图1示出了根据各种实施方式的对具有粒子的地图102进行粒子聚类以及针对具有若干障碍物112(例如，墙壁)的示例场景所生成的聚类106、108、110以便提供聚类的地图104的例示图100。

可以利用二值化网格(例如，其中在x方向上的分辨率为10cm，并且在y方向上的分辨率为10cm)来对地图102的粒子进行聚类。可以在聚类处理之后生成三个聚类106、108、110。

可以根据粒子过滤器的参数(诸如有效样本大小和熵信息)来对该粒子过滤器进行监测。并行地，例如，可以通过对表示聚类的高斯参数进行更新来更新聚类。如果粒子集中在某个区域中(例如，5m

图2示出了作为根据各种实施方式的并行过滤的示例，在具有三个生成的聚类(表示为“1”、“2”以及“3”)的停车场中，根据各种实施方式的利用粒子过滤进行自我姿态初始化的场景的例示图200。在上面图2所示的示例中，在中下图中对样本进行聚类。

初始化区域(例如，在这种场景中为停车场)的对称形式，以及因此在地图的相对侧的观测结果的不确定性对粒子过滤提出了重大挑战。对称性导致聚类在重采样处理内得以幸存。随着第一聚类朝地图边界移动，该第一聚类的有效样本大小会随时间不断减少。由于第二聚类和第三聚类在有关停车场两个转角的观测结果上的相似性(affinity)，他们的样本在聚类内获得几乎相同的权重。根据从地图的右下角开始更密集的测量结果，针对第二聚类观测到有效的样本大小增加。

在另一实施方式中，可以将聚类用于粒子过滤器的聚类跟踪，以使用高斯参数(例如，均值和方差(或标准偏差))进行发散避免，如同下面将描述的那样。

为了在发散后恢复过滤器，可以使用基于不同传感器系统(例如，雷达、LiDAR、摄像头或这些传感器的组合)的注入方法。

根据各种实施方式，在过滤器发散的情况下，可以基于粒子的聚类来执行系统的粒子注入。

聚类的数量可以具有阈值，并且如果该阈值达到某个数，则按照以下方式对聚类进行跟踪。在过滤器发散的情况下，可以只将新的样本注入与聚类相关的区域(例如，具有基于均值周围的方差(例如，等于方差)的宽度(或直径)的区域)中，以使注入处理高效。

根据各种实施方式，可以执行基于其高斯分布参数(例如，均值和方差)的聚类跟踪。

对熵信息的变化率进行控制以捕获可能超过所定义的阈值的情况。显著的变化不仅会因过滤器发散而出现，而且会在预热(warm up)处理之后出现。通过观测

作为最大熵。在预热处理之后，熵信息减少，但是检测到M

注入的先决条件是已经执行了聚类，否则将不执行注入。在这种情况下，可以首先执行聚类，然后可以执行注入。

根据各种实施方式，可以假设各个粒子聚类都是高斯分布的。可以保留聚类的高斯分布参数(例如，均值和方差)。可以使用车辆模型和自我姿态协方差矩阵，在各个采样时间内预测高斯分布参数。因此，由于自我姿态的不确定，聚类的方差是随时间扩大的。可以根据有效样本大小和熵来监测粒子过滤器的功能。并行地，可以连续更新高斯聚类。当识别出过滤器发散时，仅将新的粒子系统地注入聚类中以恢复过滤器。以这种方式，粒子过滤器不会重置，并且可以利用高效的粒子注入来恢复过滤器。

由于在各个采样时间内可以仅更新两个参数/每聚类，因此，聚类的更新处理在计算上可以是高效的。

图3示出了根据各种实施方式的聚类跟踪的例示图300。左侧地图302例示了基于运动参数和车辆模型生成的粒子聚类及这些粒子聚类被跟踪的路线(由白色箭头表示)。可以利用误差传播原理基于自我姿态不确定度来更新聚类方差(在右边地图304中由白色圆圈表示)。在右侧地图304中示出了更新后的聚类均值(由白点表示)和方差(由白色圆圈表示)。

在发散的情况下，可以将这些更新后的聚类中的一个或更多个(或全部)聚类用于注入新的粒子。如果聚类完全处于有效区域之外，则不再将该聚类用于注入。如果该聚类部分处于有效区域之外，则仅向所述有效部分填充新注入的粒子。

图4示出了例示图400，其中，手动去除粒子，以便对根据各种实施方式的方法进行分析。出于测试的目的，在进行聚类之后的几个采样时间，手动消除位于正确的车辆自我姿态中的粒子。在这种场景中，针对潜在的粒子发散，生成并跟踪三个聚类。图4例示了监测过滤器参数，即，有效样本大小、熵信息及其变化率。

图4示出了根据各种实施方式的粒子过滤器监测参数的例示图400。例示了样本大小M 402、kM 404(其中，k为阈值)、有效样本大小406、熵ε408、k与最大熵的乘积410、最大熵412、阈值414和熵变化率416。如果所述参数在聚类处理之后超过了对应的阈值，则对被跟踪的聚类实行系统的注入。

在预热处理期间以及在对粒子进行聚类之前，不实行注入。如可以考虑的，针对显著的参数改变，有两个采样时间：一次在初始化时间时，以及一次在粒子消除之后。

在具有高概率自我姿态的区域中(即，在被跟踪的聚类中)识别出过滤器发散之后，执行系统的注入。在注入之后，参数可以处于阈值边界内(可能是在偏离之后)。在没有系统的注入的情况下并且通过重置过滤器，时间增加100％以在发散时间处实现相同的样本分配。

图5示出了根据各种实施方式的在将过滤器发散识别成被跟踪的聚类之后的注入处理的例示图500。例示了处理的三个层级，包括：聚类502、发散识别504以及基于聚类的高斯参数的系统粒子注入506。要注意的是，在识别阶段504中，出于演示目的，手动去除了包含真值的一个聚类中的所有粒子。在粒子注入阶段506中，即使不再存在聚类的粒子，也将粒子注入到假设聚类要位于的位置(基于聚类的更新后的均值和聚类的更新后的方差)。

图6示出了例示根据各种实施方式的确定自定位初始化的初始自我姿态的方法的流程图600。在602处，可以在地图中设置多个粒子。在604处，可以将多个粒子分组成多个聚类，各个聚类包括所述多个粒子的相应子集。在606处，例如可以通过对所述多个聚类中的各个聚类的相应高斯参数(比如均值和方差)进行更新来执行聚类跟踪。在608处，在粒子过滤期间，可以基于所述多个聚类来注入粒子。在610处，可以基于粒子过滤来确定初始自我姿态。

根据各种实施方式，可以确定所述多个聚类中的各个聚类的粒子的相应子集的相应位置的均值和方差。

根据各种实施方式，可以基于所述多个聚类注入粒子的步骤包括：基于相应均值和相应方差注入粒子。

根据各种实施方式，可以基于运动模型更新所述多个聚类中的各个聚类的相应均值。

根据各种实施方式，可以利用误差传播原理基于自我姿态不确定度传播律来更新所述多个聚类中的各个聚类的相应方差。

根据各种实施方式，所述多个粒子可以是基于地图上的随机分布和自我姿态的估计中的至少一项来设置的。

根据各种实施方式，执行粒子过滤的步骤可以包括：样本分配、预测、更新和重采样。

根据各种实施方式，所述粒子可以是基于潜在聚类中的粒子的数量和潜在聚类的数量中的至少一项来被分组成所述多个聚类的。

根据各种实施方式，如果聚类处于关注区域之外，则可以排除该聚类。根据各种实施方式，如果聚类的一部分处于关注区域之外，则可以忽略该聚类的该部分(换句话说：可以不被用于粒子注入)。

根据各种实施方式，可以接收从车辆的传感器系统的至少一个发射器发射的以及在该车辆附近朝向该传感器系统反射的电磁辐射。

根据各种实施方式，粒子过滤的步骤可以是基于所接收到的电磁辐射以及基于地图来执行的。

根据各种实施方式，可以基于针对聚类的数量的预定数量阈值和针对聚类的相应空间大小的预定大小阈值中的至少一项来确定初始自我位置。

根据各种实施方式，所述初始自我位置可以基于熵来确定，所述熵是基于以二值化网格为基础的监测的。

以上描述的步骤602、604、606、608以及其它步骤中的各个步骤可以由计算机硬件组件来执行。

100 粒子聚类的例示图

102 地图

104 聚类的地图

106 聚类

108 聚类

110 聚类

112 障碍物

200 根据各种实施方式的利用粒子过滤进行自我姿态初始化的场景的例示图

300 根据各种实施方式的聚类跟踪的例示图

302 聚类时间下的地图

304 在利用运动模型更新聚类之后的地图

400 根据各种实施方式的粒子过滤器监测参数的例示图

402 样本大小

404 k与样本大小的乘积

406 有效样本大小

408 熵

410 k与熵的乘积

412 最大熵

414 阈值

416 熵变化率

500 根据各种实施方式的在将过滤器发散识别成被跟踪的聚类之后的注入处理的例示图

502 聚类阶段

504 发散识别阶段

506 粒子注入阶段

600 例示了根据各种实施方式的确定自定位初始化的初始自我姿态的方法的流程图

602 在地图中设置多个粒子的步骤

604 将多个粒子分组成多个聚类的步骤

606 聚类跟踪的步骤

608 注入粒子的步骤

610 确定初始自我姿态的步骤