CN111915675B - 基于粒子漂移的粒子滤波点云定位方法及其装置和系统 - Google Patents

Abstract

本发明公开了一种基于粒子漂移的粒子滤波点云定位方法，基于粒子滤波定位算法，提升定位的精度，包括将初始粒子集根据当前时刻的里程计数据转换为预测粒子集，并根据漂移算法，获取漂移粒子集，并根据所述漂移粒子集中权重大的漂移粒子和坐标信息，输出定位结果。同时，还提供一种基于粒子漂移的粒子滤波定位系统和基于粒子漂移的粒子滤波定位装置；所述基于粒子漂移的粒子滤波定位系统和基于粒子漂移的粒子滤波定位装置应用所述基于粒子漂移的粒子滤波点云定位方法提升定位的精度。

Description

基于粒子漂移的粒子滤波点云定位方法及其装置和系统

技术领域

本发明涉及定位技术领域，特别是涉及一种基于粒子漂移的粒子滤波点云定位方法及其装置和系统。

背景技术

基于高精度点云地图的点云定位过程中，由于地图的非实时性与传感器噪声，实时采集的数据与地图之间存在不一致的问题。这一问题通常会给基于地图匹配的定位算法的精度带来影响，甚至造成局部最优或者定位无法收敛的情况。针对这一问题，现有算法通常从如道路两旁的墙壁、房屋、电线杆等固定物体上提取点、线、面等特征进行定位。理想情况下，上述算法能够在一定程度上确保实时采集数据与地图数据的一致性。但是，大量的特征提取给定位算法带来计算开销，场景的变化也可能让基于固定特征的定位算法失效。现有技术中，基于贝叶斯概率理论，常采用粒子滤波算法，充分将不一致性通过概率的形式表达，通过将定位问题转换为预测和预测更新两个步骤，从而对定位的不确定性有了更好的预测估计。然而，由于地图与实时采集的数据间存在不一致且粒子具有随机性，粒子权重的计算值并不能准确地反映其所在定位的正确性，基于粒子滤波算法的自动驾驶点云定位技术通常面临收敛程度不高，粒子退化等问题。

发明内容

本发明旨在至少解决现有技术中存在的技术问题之一。为此，第一方面，本发明提供了一种基于粒子漂移的粒子滤波点云定位方法，提升定位的精度；第二方面，本发明提供一种基于粒子漂移的粒子滤波定位系统，采用第一方面所述基于粒子漂移的粒子滤波点云定位方法以提升定位的精度。第三方面，基于粒子漂移的粒子滤波定位装置，采用第一方面所述的基于粒子漂移的粒子滤波点云定位方法以提升定位的精度。

第一方面，本发明提出一种基于粒子漂移的粒子滤波点云定位方法，包括如下步骤：

获取位置数据，所述位置数据包括前一时刻的位置粒子集、当前时刻的里程计数据。

将所述位置粒子集与所述里程计数据进行状态叠加，得到预测粒子集。

根据粒子漂移算法对所述预测粒子集中各预测粒子进行漂移，获得漂移粒子集。

将所述漂移粒子集中各粒子进行权重处理，获得当前时刻的位置信息。

其中，根据粒子漂移算法对所述预测粒子集中各预测粒子进行漂移，获得漂移粒子集包括如下步骤：

获取与所述预测粒子对应的第一观测点云集合，以及获取与所述第一观测点云集合中各点最近的点的第一地图点云集合。

设定漂移矩阵变量，根据漂移矩阵变量对所述第一观测点云集合进行多次漂移模拟，将漂移后的所述第一观测点云集合与所述第一地图点云集合相似度最高的一次漂移对应的漂移矩阵变量的值设为最优漂移矩阵。

根据最优漂移矩阵对所述预测粒子进行漂移，获得漂移粒子。

获取所述预测粒子的第一相似度以及所述漂移粒子的第二相似度。

根据所述第一相似度和所述第二相似度的比较结果，判断是否将所述预测粒子替换成所述漂移粒子，并形成所述漂移粒子集。

根据本发明的上述实施例，至少具有如下有益效果：由于基于粒子滤波的点云定位算法中，每个预测粒子表示一个可能的定位点。而第一地图点云集合为围绕该定位点真实值的周围的点的集合。因此可以基于漂移后的第一观测点云集合与第一地图点云集合相似度最高得到的最优漂移矩阵，以及第一观测点云集合和预测粒子具有相同的收敛特征，从而得到收敛度更高的漂移粒子，从而可以根据漂移粒子集进行真实位置判定，提升定位的精度。

根据本发明的一些实施例，所述第一相似度的获取包括如下步骤：

所述第一相似度的获取包括如下步骤：

获取所述第一观测点云集合中各点和所述第一地图点云集合中对应的点的第一平均距离；将所述第一平均距离取相反数，获得所述第一相似度。

所述第二相似度获取包括如下步骤：

获取所述漂移粒子对应的第二观测点云集合，以及与所述第二观测点云集合中各点最近的点的第二地图点云集合。

获取所述第二观测点云集合中各点和所述第二地图点云集合中对应的点的第二平均距离；将所述第二平均距离取相反数，获得所述第二相似度。

通过平均距离取相反数，使得求取的值的含义更接近相似度的含义，便于理解。

根据本发明的一些实施例，根据所述第一相似度和所述第二相似度的比较结果，判断是否将所述预测粒子替换成所述漂移粒子，从而形成所述漂移粒子集包括如下步骤：

当所述第一相似度小于所述第二相似度时，将所述预测粒子替换成所述漂移粒子。

当所述第一相似度大于等于所述第二相似度，且第一相似度与第二相似度的比值满足预设的概率值时，将所述预测粒子替换成所述漂移粒子。

通过引入概率值，使得漂移粒子集中存在非最优解的粒子，防止粒子滤波陷入局部最优，减轻粒子退化的问题。

根据本发明的一些实施例，所述概率值的从随机函数random()输出值获取。通过random()简化概率值的处理，提升计算效率。

根据本发明的一些实施例，将所述漂移粒子集中各粒子进行权重处理，获得当前时刻的位置信息包括如下步骤：

获取所述漂移粒子集中各粒子的权重，并根据重采样算法对所述漂移粒子集中的粒子进行重采样，获得收敛粒子集。

将所述收敛粒子集设置为下一时刻的位置粒子集。

根据所述收敛粒子集中各收敛粒子的在地图中的坐标位置，获取中心坐标位置以及所述中心坐标位置到所述收敛粒子的方差值，将所述中心坐标位置以及所述方差值作为定位结果。

通过重采样，使得收敛粒子集中的数据更能代表真实位置的粒子集，从而使得中心坐标位置所对应的定位信息更加准确。

根据本发明的一些实施例，所述权重为所述漂移粒子集中对应粒子的对数值。通过相似度取对数，使得权重值可以介于0-1之间，且可以使得各粒子之间的权重区别更大。

根据本发明的一些实施例，所述最优漂移矩阵包括旋转矩阵以及平移矩阵。通过将最优漂移矩阵拆解为旋转矩阵以及平移矩阵，使得运算的漂移粒子更接近于真实值。

第二方面，本发明实施例提出一种基于粒子漂移的粒子滤波定位系统，包括观测点检测装置，所述观测点检测装置用于记录所述粒子滤波定位系统的位置数据，所述位置数据包括行进变化数据、环境数据；定位计算装置，所述定位计算装置获取所述位置数据；所述定位计算装置执行第一方面提出的实施例中任一所述的方法，并输出定位估计结果。由于本发明实施例的定位计算装置有用于执行如本发明第一方面中任一项所述的基于粒子漂移的粒子滤波点云定位方法，因此具有本发明第一方面的所有有益效果。

根据本发明的一些实施例，所述观测点获取装置包括至少如下之一：GPS；IMU；车轮编码器；激光雷达传感器。通过不同的观测点装置可以获取更加详细的行进数据，以提升定位计算装置数据的准确性。

第三方面，本发明实施例提出一种基于粒子漂移的粒子滤波定位装置，包括存储器、处理器及存储在所述存储器上并可在处理器上运行的计算机程序，其特征在于。所述处理器执行所述程序时实现如第一方面提出的实施例中任一所述的基于粒子漂移的粒子滤波点云定位方法。由于本发明实施例的定位计算装置有用于执行如本发明第一方面中任一项所述的基于粒子漂移的粒子滤波点云定位方法，因此具有本发明第一方面的所有有益效果。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为本发明实施例的基于粒子漂移的粒子滤波点云定位方法的主要步骤图；

图2为本发明实施例的基于粒子漂移的粒子滤波点云定位方法中粒子漂移算法的主要步骤图；

图3为本发明实施例的基于粒子漂移的粒子滤波点云定位方法中当前位置信息获取的主要步骤图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。

下面结合附图中图1至图3对本发明的实施例做进行进一步阐述：

第一方面，本发明提出一种基于粒子漂移的粒子滤波点云定位方法，包括如下步骤：

S100、获取位置数据，位置数据包括前一时刻的位置粒子集、当前时刻的里程计数据。

应理解的是，里程计数据能够为算法提供行进变化的数据，包括行驶的里程，航向角的变化，偏移。里程计数据可以从GPS(Global Positioning System)、IMU(Inertialmeasurement unit)或者车轮编码器中获得。

应理解的是，位置粒子集表示设备定位信息的状态值。状态值包括定位数据x，y，z，yaw，pitch，roll，分别表示设备当前在地图中的纵向、横向、高度坐标值，以及设备自身的航向角、俯仰角以及翻滚角。在一些实施例中，设备表示自动驾驶的车辆，在另一些实施例中，可以用于自动行驶的其他装置，如机器人。

S200、将位置粒子集与里程计数据进行状态叠加，得到预测粒子集。

应理解的是，状态叠加结果可以根据滤波算法中状态转移模型获取得到预测粒子集。

S300、根据粒子漂移算法对预测粒子集中各预测粒子进行漂移，获得漂移粒子集。

S400、将漂移粒子集中各粒子进行权重处理，获得当前时刻的位置信息。

应理解的是，漂移粒子越接近与真实值，则权重越大，此时根据权重获取的定位结果更加准确。

应理解的是，漂移粒子集中各粒子本身携带了坐标信息，因此可以进行坐标处理。

其中，S300、根据粒子漂移算法对预测粒子集中各预测粒子进行漂移，获得漂移粒子集包括如下步骤：

应理解的是，S300为针对每个预测粒子均执行步骤S310、S320、S330、S340、S350、S351、S352。

S310、获取与预测粒子对应的第一观测点云集合，以及获取与第一观测点云集合中各点最近的点的第一地图点云集合。

应理解的是，激光雷达传感器等传感器获取的点云数据后，经过计算转换成地图中对应的点，地图中对应的点的集合即为第一观测点云集合。

应理解的是，每一个观测点云集合对应一个预测粒子。

应理解的是，当设定第一观测点云集合为P1，其中P1＝{p₁,p₂,...,p_N}；设定第一地图点云集合为P2，其中P2＝{p'₁,p'₂,...,p'_N}；则p'_N表示为p_N最近的点，需说明的是，最近的判断是根据p'_N与p_N在同一地图中点之间的距离最小进行判断。

S320、设定漂移矩阵变量，根据漂移矩阵变量对第一观测点云集合进行多次漂移模拟，将漂移后的第一观测点云集合与第一地图点云集合相似度最高的一次漂移对应的漂移矩阵变量的值设为最优漂移矩阵。

应理解的是，最优漂移矩阵获取基于以下事实：(1)基于粒子滤波的点云定位算法中，每个粒子表示一个可能的定位点。通过漂移后的第一观测点云集合与地图中该定位点周围的地图点云之间的相似度，可以得到该粒子的权重。相似度越大，权重越大。而粒子收敛的过程，就是粒子向真实值靠近的过程，粒子距离真实值越近，则其相似度就越大；(2)由于粒子的随机性与环境的复杂性，相似度高的粒子表示的定位并不一定真实，但真实值附近的粒子都会向真实值靠近。其中，该定位点周围的地图点云为第一地图点云集合。

应理解的是，漂移后的第一观测点云集合与第一地图点云集合相似度最高的判定条件为：第一地图点云集合通过最优漂移矩阵变量进行漂移后，第一观测点云集合中的各点与第一地图云集合中各点的距离之和最小。

S330、根据最优漂移矩阵对预测粒子进行漂移，获得漂移粒子。

应理解的是，预测粒子的收敛特性与第一观测点云集合的收敛特性一致。假设漂移后的第一观测点云集合为Q，与Q最近的集合为Q'，则相似度S表示Q的各点和Q'的各点之间远近程度，当S越大时，表示Q对应的漂移粒子为最接近真实值的粒子，因此，可以根据最优漂移矩阵获取的漂移粒子最接近真实值。

应理解的是，通过最优漂移矩阵可以获取到漂移粒子在地图中的三维坐标，在一些实施例中，漂移粒子的航向角、俯仰角以及翻滚角与对应预测粒子的航向角、俯仰角以及翻滚角一致。

S340、获取预测粒子的第一相似度以及漂移粒子的第二相似度。

应理解的是，漂移粒子的观测点云集合为第一云集合根据最优漂移矩阵计算得到。

S350、根据第一相似度和第二相似度的比较结果，判断是否将预测粒子替换成漂移粒子，并形成漂移粒子集。

应理解的是，由于对预测粒子进行漂移后，其所携带的观测点云集合与地图中的最近点的地图点云集合发生了变化。漂移粒子与真实值的相似度仍然可能出现变小的情况；因此进行相似度比较，以保证漂移粒子为相似度增加的粒子，即最接近真实值的粒子。

因此，由于基于粒子滤波的点云定位算法中，每个预测粒子表示一个可能的定位点。而第一地图点云集合为围绕该定位点真实值的周围的点的集合。因此可以基于漂移后的第一观测点云集合与第一地图点云集合相似度最高得到的最优漂移矩阵，以及第一观测点云集合和预测粒子具有相同的收敛特征，从而得到收敛度更高的漂移粒子，从而可以根据漂移粒子集进行真实位置判定，提升定位的精度。

在一些实施例中，第一相似度的获取包括如下步骤：

获取第一观测点云集合中各点和第一地图点云集合中对应的点的第一平均距离；将第一平均距离取相反数，获得第一相似度。

第二相似度获取包括如下步骤：

获取漂移粒子对应的第二观测点云集合，以及与第二观测点云集合中各点最近的点的第二地图点云集合。

获取第二观测点云集合中各点和第二地图点云集合中对应的点的距离的第二平均距离；将第二平均距离取相反数，获得第二相似度。

应理解的是，假设第二观测点云集合为Q，第二地图点云集合为Q'；则设定第一相似度为S1，第二相似度为S2时，此时S1、S2如下：

通过平均距离取相反数，使得求取的值的含义更接近相似度的含义，便于理解。

在一些实施例中，根据第一相似度和第二相似度的比较结果，判断是否将预测粒子替换成漂移粒子，从而形成漂移粒子集包括如下步骤：

S351、当第一相似度小于第二相似度时，将预测粒子替换成漂移粒子。

S352、当第一相似度大于等于第二相似度，且第一相似度与第二相似度的比值满足预设的概率值时，将预测粒子替换成漂移粒子。

应理解的是，当设定第一相似度为S1，第二相似度为S2，则当S2>S1，表示漂移算法后的漂移粒子更接近于真实值，将预测粒子替换成漂移粒子。

通过引入概率值，使得漂移粒子集中存在非最优解的粒子，防止粒子滤波陷入局部最优，减轻粒子退化的问题。

在一些实施例中，概率值的从随机函数random()输出值获取。通过random()简化概率值的处理，提升计算效率。

应理解的是random()每次仅可获取一次随机值，因此，仅需要进行一次随机概率的判断计算，从而提升了计算效率。且random()的参数范围可以根据实际应用情况设定。

在一些实施例中，S400、将所述漂移粒子集中各粒子进行权重处理，获得当前时刻的位置信息包括如下步骤：

S410、获取漂移粒子集中各粒子的权重，并根据重采样算法对漂移粒子集中的粒子进行重采样，获得收敛粒子集。

应理解的是，权重值越大表示越接近真实位置，当权重过小，表示粒子偏移真实位置过大，此时采取粒子进行定位已经无意义，因此重采样包括将权重小的粒子进行删除；并复制权重大的粒子，以保证收敛粒子集有足够的粒子进行评估。需说明的是，权重小的判定可以根据现有的重采样算法进行计算，此处不一一说明。

S420、将收敛粒子集设置为下一时刻的位置粒子集。

应理解的是，将收敛粒子集作为下一时刻的位置粒子集，可以简化下一轮操作的计算，使得每一次定位计算均基于上一次准确定位的基础上进行定位，提升定位的效率。

S430、根据收敛粒子集中各收敛粒子的在地图中的坐标位置，获取中心坐标位置以及中心坐标位置到收敛粒子的方差值，将中心坐标位置以及方差值作为定位结果。

应理解的是，收敛粒子中也包含了航向角、俯仰角以及翻滚角，因此输出定位结果时，还会输出各收敛粒子中航向角、俯仰角以及翻滚角的均值。

在一些实施例中，权重为漂移粒子集中对应粒子的对数值。通过相似度取对数，使得权重值可以介于0-1之间，且可以使得各粒子之间的权重区别更大。

应理解的是，由于相似度取的是距离之和的平均值的相反数，因此相似度为负值，此时将相似度取自然数对数，可以获取0-1之间的值。

在一些实施例中，漂移矩阵包括旋转矩阵以及平移矩阵。通过将最优漂移矩阵拆解为旋转矩阵以及平移矩阵，使得运算的漂移粒子更接近于真实值。

应理解的是，由于漂移矩阵用于对地图中的三维坐标进行漂移计算，因此旋转矩阵为3x3的矩阵。

应理解的是，当第一观测点云集合P1经最优漂移矩阵漂移后，第二观测点云集合和第一观测点云集合的质心重合，假设第一观测点云集合P1的质心为U1，第一地图点云集合为P2的质心为U2，当设定旋转矩阵变量为R，最小旋转矩阵为R*，平移矩阵为t，则R和R*均为3x3的矩阵,最优漂移矩阵为[R*,t]；则U1、U2分别表示如下：

此时，R、R*与U1、U2以及P1、P2的关系如下：

t＝U2-R^**U1

第二方面，本发明实施例提出一种基于粒子漂移的粒子滤波定位系统，包括观测点检测装置，观测点检测装置用于记录粒子滤波定位系统的位置数据，位置数据包括行进变化数据、环境数据；定位计算装置，定位计算装置获取位置数据；定位计算装置执行第一方面提出的实施例中任一的方法，并输出定位估计结果。由于本发明实施例的定位计算装置有用于执行如本发明第一方面中任一项的基于粒子漂移的粒子滤波点云定位方法，因此具有本发明第一方面的所有有益效果。

应理解的是，观测点检测装置的位置数据包括基于粒子漂移的粒子滤波点云定位方法中的位置数据，行进变化数据与基于粒子漂移的粒子滤波点云定位方法中当前时刻的里程计数据一致。

应理解的是，环境数据为激光雷达传感器获得的数据。

在一些实施例中，观测点获取装置包括至少如下之一：GPS；IMU；车轮编码器；激光雷达传感器。通过不同的观测点装置可以获取更加详细的行进数据，以提升定位计算装置数据的准确性。

应理解的是，GPS；IMU；车轮编码器；用于获取设备的里程计数据，包括行驶的里程，航向角的变化，偏移。

应理解的是，可以设置多个激光雷达传感器，以获取更广角度和更多数量的点云数据。

第三方面，本发明实施例提出一种基于粒子漂移的粒子滤波定位装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于。处理器执行程序时实现如第一方面提出的实施例中任一的基于粒子漂移的粒子滤波点云定位方法。由于本发明实施例的定位计算装置有用于执行如本发明第一方面中任一项的基于粒子漂移的粒子滤波点云定位方法，因此具有本发明第一方面的所有有益效果。

下面参考图1至图3以一个具体的实施例详细描述根据本发明实施例的基于粒子漂移的粒子滤波点云定位方法在自动驾驶中的应用。值得理解的是，下述描述仅是示例性说明，而不是对发明的具体限制。

具体的，如图1所示，基于粒子漂移的粒子滤波点云定位方法，包括如下步骤：

S100、获取位置数据，位置数据包括前一时刻的位置粒子集、当前时刻的里程计数据。

具体的，里程计数据能够为算法提供车辆行进变化的数据，包括车辆行驶的里程，车辆航向角的变化，车辆偏移。里程计数据可以从GPS(Global Positioning System)、IMU(Inertial measurement unit)或者车轮编码器中获得。

具体的，位置粒子集表示能够表示车辆定位信息的状态值。状态值包括定位数据x，y，z，yaw，pitch，roll，定位数据分别表示车辆当前在地图中的纵向、横向、高度坐标值，以及车辆自身的航向角、俯仰角以及翻滚角。

具体的，在车辆上还安装有多个激光雷达传感器，每一个激光雷达传感器可以获取车辆一个位置点的环境数据。

具体的，在车辆未启动时，获取车辆上多个点的状态值，组成位置粒子集。需说明的是，位置粒子集中的每一个点，均对应有一个环境数据。

S200、将位置粒子集与里程计数据进行状态叠加，得到预测粒子集。

具体的，状态叠加结果可以根据滤波算法中状态转移模型获取得到预测粒子集。

S300、根据粒子漂移算法对预测粒子集中各预测粒子进行漂移，获得漂移粒子集。

具体的，如图2所示，根据粒子漂移算法对预测粒子集中各预测粒子进行漂移，从而获得漂移粒子集包括如下步骤：

S310、获取与预测粒子对应的第一观测点云集合，以及获取与第一观测点云集合中各点最近的点的第一地图点云集合。

具体的，设定第一观测点云集合为P1，其中P1＝{p₁,p₂,...,p_N}；设定第一地图点云集合为P2，其中P2＝{p'₁,p'₂,...,p'_N}；则p'_N表示为p_N最近的点。

S320、设定漂移矩阵变量，根据漂移矩阵变量对第一观测点云集合进行多次漂移，将漂移后的第一观测点云集合与第一地图点云集合相似度最高的一次漂移对应的漂移矩阵变量的值设为最优漂移矩阵。

具体的，漂移矩阵包括旋转矩阵以及平移矩阵。

具体的，最优漂移矩阵的求解如下：

设定第一观测点云集合的第一质心为U1，第一地图点云集合的第二质心为U2，则U1、U2表示如下：

设定旋转矩阵变量为R，最小旋转矩阵为R*，平移矩阵为t，其中R为3x3的矩阵，则最优漂移矩阵为[R*,t]；此时，漂移矩阵与U1、U2以及P1、P2的关系如下：

t＝U2-R^**U1

S330、根据最优漂移矩阵对预测粒子进行漂移，获得漂移粒子。

具体的，假设与第一观测点云集合P1对应的预测粒子的在地图中的坐标为x_i，则漂移粒子在地图中的坐标x'_i＝R^**x_i+t。

S340、获取预测粒子的第一相似度以及漂移粒子的第二相似度。

具体的，第一相似度的获取包括如下步骤：

获取第一观测点云集合中各点和第一地图点云集合中对应的点的第一平均距离；将第一平均距离取相反数，获得第一相似度。

同理，第二相似度获取包括如下步骤：

获取漂移粒子对应的第二观测点云集合，以及与第二观测点云集合最近的点的第二地图点云集合。

具体的，第二观测点云集合中各点通过第一地图点云集合P2中各点进行最优漂移矩阵漂移获得。

获取第二观测点云集合中各点和第二地图点云集合中对应的点的距离的第二平均距离；将第二平均距离取相反数，获得第二相似度。

具体的，假设第二观测点云集合为Q，第二地图点云集合为Q'；则设定第一相似度为S1，第二相似度为S2时，此时S1、S2如下：

S350、根据第一相似度和第二相似度的比较结果，判断是否将预测粒子替换成漂移粒子，并形成漂移粒子集。

具体的，漂移粒子的航向角、俯仰角以及翻滚角为预测粒子的航向角、俯仰角以及翻滚角。

具体的，如图2所示，根据第一相似度和第二相似度的比较结果，判断是否将预测粒子替换成漂移粒子，并形成漂移粒子集包括如下步骤：

S351、当第一相似度小于第二相似度时，将预测粒子替换成漂移粒子。

S352、当第一相似度大于等于第二相似度，且第一相似度与第二相似度的比值满足预设的概率值时，将预测粒子替换成漂移粒子。

具体的，当S2>S1或者S2<＝S1且S1/S2>＝random(0,1)时，此时，将预测粒子替换成漂移粒子。

S400、将漂移粒子集中各粒子进行权重以及坐标处理，获得当前时刻的位置信息。

具体的，如图3所示，将漂移粒子集中各粒子进行权重以及坐标处理，获得当前时刻的位置信息包括如下步骤：

S410、获取漂移粒子集中各粒子的权重，并根据重采样算法对漂移粒子集中的粒子进行重采样，获得收敛粒子集。

具体的，根据重采样算法，将权重过小的漂移粒子删除，并将权重大的漂移粒子复制重新组成得到收敛粒子。需说明的是，权重小的判定可以根据现有的重采样算法进行计算，此处不一一说明。

具体的，权重为漂移粒子集中对应粒子的对数值。

S420、将收敛粒子集设置为下一时刻的位置粒子集。

S430、根据收敛粒子集中各收敛粒子的在地图中的坐标位置，获取中心坐标位置以及中心坐标位置到收敛粒子的方差值，将中心坐标位置以及方差值作为定位结果。

具体的，将收敛粒子中的航向角、俯仰角以及翻滚角取均值，并将该均值、中心坐标以及方差值作为定位结果。

通过上述步骤可以快速准确的获取车辆的定位信息，从而保证自动驾驶过程中，可以快速根据目的地调整行使路线，从而保证行使路线的正确性。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

上面结合附图对本发明实施例作了详细说明，但是本发明不限于上述实施例，在所述技术领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (10)

1.一种基于粒子漂移的粒子滤波点云定位方法，其特征在于，包括如下步骤：

获取位置数据，所述位置数据包括前一时刻的位置粒子集、当前时刻的里程计数据；

将所述位置粒子集与所述里程计数据进行状态叠加，得到预测粒子集；

根据粒子漂移算法对所述预测粒子集中各预测粒子进行漂移，获得漂移粒子集；

将所述漂移粒子集中各粒子进行权重处理，获得当前时刻的位置信息；

其中，根据粒子漂移算法对所述预测粒子集中各预测粒子进行漂移，获得漂移粒子集包括如下步骤：

获取与所述预测粒子对应的第一观测点云集合，以及获取与所述第一观测点云集合中各点最近的点的第一地图点云集合；

设定漂移矩阵变量，根据漂移矩阵变量对所述第一观测点云集合进行多次漂移模拟，将漂移后的所述第一观测点云集合与所述第一地图点云集合相似度最高的一次漂移对应的漂移矩阵变量的值设为最优漂移矩阵；

根据最优漂移矩阵对所述预测粒子进行漂移，获得漂移粒子；

获取所述预测粒子的第一相似度以及所述漂移粒子的第二相似度；

根据所述第一相似度和所述第二相似度的比较结果，判断是否将所述预测粒子替换成所述漂移粒子，并形成所述漂移粒子集。

2.根据权利要求1所述的基于粒子漂移的粒子滤波点云定位方法，其特征在于，

所述第一相似度的获取包括如下步骤：

获取所述第一观测点云集合中各点和所述第一地图点云集合中对应的点的第一平均距离；

将所述第一平均距离取相反数，获得所述第一相似度；

所述第二相似度获取包括如下步骤：

获取所述漂移粒子对应的第二观测点云集合，以及与所述第二观测点云集合中各点最近的点的第二地图点云集合；

获取所述第二观测点云集合中各点和所述第二地图点云集合中对应的点的第二平均距离；

将所述第二平均距离取相反数，获得所述第二相似度。

3.根据权利要求1所述的基于粒子漂移的粒子滤波点云定位方法，其特征在于，

根据所述第一相似度和所述第二相似度的比较结果，判断是否将所述预测粒子替换成所述漂移粒子，并形成所述漂移粒子集包括如下步骤：

当所述第一相似度小于所述第二相似度时，将所述预测粒子替换成所述漂移粒子；

当所述第一相似度大于等于所述第二相似度，且第一相似度与第二相似度的比值满足预设的概率值时，将所述预测粒子替换成所述漂移粒子。

4.根据权利要求3所述的基于粒子漂移的粒子滤波点云定位方法，其特征在于，

所述概率值的从随机函数random()输出值获取。

5.根据权利要求2所述的基于粒子漂移的粒子滤波点云定位方法，其特征在于，

将所述漂移粒子集中各粒子进行权重处理，获得当前时刻的位置信息包括如下步骤：

获取所述漂移粒子集中各粒子的权重，并根据重采样算法对所述漂移粒子集中的粒子进行重采样，获得收敛粒子集；

将所述收敛粒子集设置为下一时刻的位置粒子集；

根据所述收敛粒子集中各收敛粒子的在地图中的坐标位置，获取中心坐标位置以及所述中心坐标位置到所述收敛粒子的方差值，将所述中心坐标位置以及所述方差值作为定位结果。

6.根据权利要求5所述的基于粒子漂移的粒子滤波点云定位方法，其特征在于，

所述权重为所述漂移粒子集中对应粒子的对数值。

7.根据权利要求1所述的基于粒子漂移的粒子滤波点云定位方法，其特征在于，

所述最优漂移矩阵包括旋转矩阵以及平移矩阵。

8.一种基于粒子漂移的粒子滤波定位系统，其特征在于，包括：

观测点检测装置，所述观测点检测装置用于记录所述粒子滤波定位系统的位置数据，所述位置数据包括行进变化数据、环境数据；

定位计算装置，所述定位计算装置获取所述位置数据；所述定位计算装置执行权利要求1至7任一所述的方法，并输出定位估计结果。

9.根据权利要求8所述的基于粒子漂移的粒子滤波定位系统，其特征在于，

所述观测点检测装置包括至少如下之一：GPS；IMU；车轮编码器；激光雷达传感器。

10.一种基于粒子漂移的粒子滤波定位装置，其特征在于，包括存储器、处理器及存储在所述存储器上并可在处理器上运行的计算机程序，其特征在于：

所述处理器执行所述程序时实现如权利要求1至7任一所述的基于粒子漂移的粒子滤波点云定位方法。

Images