CN116067379B - 一种基于动态点云的长隧道场景下多传感器融合定位方法 - Google Patents

Abstract

本发明涉及无人驾驶高精度定位技术领域，提供一种基于动态点云的长隧道场景下多传感器融合定位方法，其包含：使用多源异构传感器采集长隧道场景下的多种特征数据，处理得到三维广义点云数据以及传感器数据；基于三维广义点云数据以及传感器数据生成三维广义点云地图，并对三维广义点云地图进行区块划分，选取与车辆当前状态信息最接近的局部地图；对三维广义点云数据以及局部地图进行扫描匹配，得到点云匹配结果，并将点云匹配结果以及传感器数据输入到无迹卡尔曼滤波中，得到车辆当前状态信息，作为车辆定位结果。本发明使用动态地图生成的方式，可以减少点云扫描匹配的时间消耗，提高在大规模场景下的定位效率。

Description

一种基于动态点云的长隧道场景下多传感器融合定位方法

技术领域

本发明涉及无人驾驶高精度定位技术领域，具体地说，涉及一种基于动态点云的长隧道场景下多传感器融合定位方法。

背景技术

随着时代的发展，无人驾驶技术开始进入到越来越多的领域中。自动驾驶车辆在运行时，需要通过传感器采集环境信息从而获取车辆当前的运动状态。环境感知传感器主要包括激光雷达，视觉相机，惯性测量单元（IMU），轮速计和超宽带（UWB）等。通过这些传感器信息，实现自动驾驶车辆在测试区域内安全平稳的运行。

由于自动驾驶技术的发展，传统领域也逐渐使用无人驾驶进行产业升级。其中，无人驾驶车辆的状态获取是其中最重要的一环。在开阔无遮挡的场景下，可以使用全球导航卫星系统（GNSS）进行定位，获取车辆当前状态信息。但是在弱纹理弱GNSS场景下，无法准确地获取车辆当前状态信息。

因此，本发明提供了一种基于动态点云的长隧道场景下多传感器融合定位方法。

发明内容

针对现有技术的不足，本发明提供了一种基于动态点云的长隧道场景下多传感器融合定位方法，所述方法包含：

使用多源异构传感器采集长隧道场景下的多种特征数据，处理得到三维广义点云数据以及传感器数据；

基于所述三维广义点云数据以及所述传感器数据生成三维广义点云地图，并对所述三维广义点云地图进行区块划分，选取与车辆当前状态信息最接近的局部地图；

对所述三维广义点云数据以及所述局部地图进行扫描匹配，得到点云匹配结果，并将所述点云匹配结果以及所述传感器数据输入到无迹卡尔曼滤波中，得到车辆当前状态信息，作为车辆定位结果。

根据本发明的一个实施例，所述多源异构传感器包含视觉相机、激光雷达、惯性测量单元、轮速计以及超宽带传感器。

根据本发明的一个实施例，通过以下步骤得到所述三维广义点云数据：

测验所述多源异构传感器中每个传感器的精度，确定有效探测距离；

对所述多源异构传感器中的多种传感器进行时间同步处理和标定处理，确保各传感器之间数据的统一；

对激光雷达数据以及视觉相机数据进行特征提取，得到所述三维广义点云数据，其中，每个三维广义点需要包含三维坐标，强度，环数，时间戳，色彩。

根据本发明的一个实施例，通过以下步骤得到所述传感器数据：

对惯性测量单元数据进行预积分处理，得到惯性测量因子；

对轮速计数据进行处理，得到轮速因子；

对超宽带传感器数据进行距离结算，得到超宽带因子。

根据本发明的一个实施例，惯性测量单元数据为

，在/>

时刻内，预积分处理包含以下表达式：

；

；

；

其中：

表示的是旋转，速度和平移量。

根据本发明的一个实施例，通过以下步骤生成所述三维广义点云地图：基于所述三维广义点云数据，提取长隧道场景下的结构特征和纹理特征信息，结合所述惯性测量因子、所述轮速因子以及所述超宽带因子，生成所述三维广义点云地图。

根据本发明的一个实施例，通过以下步骤得到所述局部地图：根据所述三维广义点云地图的坐标信息进行区块划分，统计每一个区块的点云中心点，搜寻与车辆当前状态信息最接近的区块及其周围若干区块，得到所述局部地图。

根据本发明的一个实施例，所述方法包含：

步骤a、将所述车辆定位结果作为下一次扫描匹配的初始值；

步骤b、基于所述车辆定位结果对所述局部地图进行更新，并重复进行扫描匹配、无迹卡尔曼滤波、步骤a、步骤b，得到车辆的定位输出，直到完成整个自动驾驶过程，从而实现长隧道场景下的多传感器融合定位。

根据本发明的另一个方面，还提供了一种存储介质，其包含用于执行如上任一项所述的方法步骤的一系列指令。

根据本发明的另一个方面，还提供了一种基于动态点云的长隧道场景下多传感器融合定位装置，执行如上任一项所述的方法，所述装置包含：

多源异构数据采集模块，其用于使用多源异构传感器采集长隧道场景下的多种特征数据，处理得到三维广义点云数据以及传感器数据；

动态三维地图生成模块，其用于基于所述三维广义点云数据以及所述传感器数据生成三维广义点云地图，并对所述三维广义点云地图进行区块划分，选取与车辆当前状态信息最接近的局部地图；

多传感器融合定位模块，其用于对所述三维广义点云数据以及所述局部地图进行扫描匹配，得到点云匹配结果，并将所述点云匹配结果以及所述传感器数据输入到无迹卡尔曼滤波中，得到车辆当前状态信息，作为车辆定位结果。

本发明提供的一种基于动态点云的长隧道场景下多传感器融合定位方法，与现有技术相比，具备以下有益效果：

1、本发明使用激光雷达，视觉相机，惯性测量单元（IMU），轮速计和超宽带（UWB）传感器首先构建好测试区域的高精度三维广义点云地图。并基于构建的高精度地图实现弱纹理弱GNSS长隧道场景下的自动驾驶高精定位，确保在该环境下自动驾驶的安全运行。

2、本发明使用动态地图生成的方式，可以减少点云扫描匹配的时间消耗，提高在大规模场景下的定位效率。

3、本发明提出了一种高精度的隧道场景下的定位方法，极大的提高了隧道矿井等场景下的作业安全。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例共同用于解释本发明，并不构成对本发明的限制。

图1显示了根据本发明的一个实施例的一种基于动态点云的长隧道场景下多传感器融合定位方法流程图。

图2显示了根据本发明的另一个实施例的一种基于动态点云的长隧道场景下多传感器融合定位方法流程图。

图3显示了根据本发明的一个实施例的长隧道场景下三维广义点云示意图。

图4显示了根据本发明的一个实施例的动态三维地图加载示意图。

图5显示了根据本发明的一个实施例的多传感器融合定位时各传感器信息关联示意图。

图6显示了根据本发明的一个实施例的一种基于动态点云的长隧道场景下多传感器融合定位装置结构框图。

附图中，相同的部件使用相同的附图标记。另外，附图并未按照实际的比例绘制。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，以下结合附图对本发明实施例作进一步地详细。

在弱纹理弱GNSS场景下，需要提出一种新的方法来实现定位，获取车辆状态信息。同时，当测试场景越来越大时，基于完整点云地图的定位方法的效率就会越低、为了解决这些问题，首先需要提出一种动态点云地图加载的方法，然后基于该方法实现长隧道场景下多传感器融合定位方法，从而确保在弱纹理弱GNSS的大规模场景下自动驾驶的安全运行。

综上，本发明的目的在于发明一种用于弱纹理弱GNSS长隧道场景下的基于动态点云地图的长隧道场景下多传感器融合定位方法，实现弱纹理弱GNSS大规模场景下的精确定位，为该场景下的自动驾驶提供保障，极大的提高了在弱纹理弱GNSS大规模场景下的自动驾驶运行的安全性。

图1显示了根据本发明的一个实施例的一种基于动态点云的长隧道场景下多传感器融合定位方法流程图。

如图1所示，在步骤S1中，使用多源异构传感器采集长隧道场景下的多种特征数据，处理得到三维广义点云数据以及传感器数据。

在一个实施例中，多源异构传感器包含视觉相机、激光雷达、惯性测量单元（IMU）、轮速计以及超宽带传感器（UWB），还可以包含毫米波雷达，红外相机。进一步地，车辆可以布置多颗激光雷达，激光雷达传感器为Velodyne 32线或64线，速腾32线或64线。视觉相机可以为单目相机、多目相机、深度相机或红外相机等。惯性测量单元（IMU）可以为6轴或9轴IMU。

在一个实施例中，多源异构传感器可以使用多种类型传感器或者多个同种类型的传感器来提高环境特征的采集量，获取更加全面的环境特征信息。

在一个实施例中，多源异构传感器中激光雷达和惯性测量单元是必需项，在一些极端场景下可以不使用视觉相机，轮速信息和超宽带传感器也可以实现精准的定位。

在一个实施例中，通过多源异构传感器采集数据使用的车辆可以和实时定位时的车辆不同，但需要配置相应的传感器来获取环境信息。

在一个实施例中，通过步骤S11-S13得到三维广义点云数据：

在步骤S11中，测验多源异构传感器中每个传感器的精度，确定有效探测距离。具体来说，需要测验视觉相机、激光雷达、惯性测量单元（IMU）、轮速计以及超宽带传感器（UWB）的精度，确定有效探测距离。

在步骤S12中，对多源异构传感器中的多种传感器进行时间同步处理和标定处理，确保各传感器之间数据的统一。具体来说，使用的时间同步方法可以为硬件级时间同步也可以为软件级时间同步，使用的标定方法需将所有传感器标定至同一坐标系下，一般为车体坐标系。

在步骤S13中，对激光雷达数据以及视觉相机数据进行特征提取，得到三维广义点云数据，其中，每个三维广义点需要包含三维坐标，强度，环数，时间戳，色彩。具体来说，基于标定的结果，对于采集到的传感器数据进行处理，获取三维广义点，具体信息如下所示：

；

其中，每个三维广义点X需要包含三维坐标（x，y，z）、强度intensity，环数ring，时间戳time，色彩（r，g，b）等信息。

在一个实施例中，通过以下步骤S14-S16得到传感器数据：

在步骤S14中，对惯性测量单元数据进行预积分处理，得到惯性测量因子。具体来说，惯性测量单元数据为

，在/>

时刻内，预积分处理包含以下表达式：

；

；

；

其中：

表示的是旋转，速度和平移量。

在步骤S15中，对轮速计数据进行处理，得到轮速因子。具体来说，对轮速计数据进行提取，得到轮速因子。

在步骤S16中，对超宽带传感器数据进行距离结算，得到超宽带因子。

如图1所示，在步骤S2中，基于三维广义点云数据以及传感器数据生成三维广义点云地图，并对三维广义点云地图进行区块划分，选取与车辆当前状态信息最接近的局部地图。

在一个实施例中，在实际应用中，三维广义点云地图可以为依据当前传感器采集生成的地图，也可以为使用测绘仪器预先扫描得到的三维地图。

在一个实施例中，通过以下步骤生成三维广义点云地图：基于三维广义点云数据，提取长隧道场景下的结构特征和纹理特征信息，结合惯性测量因子、轮速因子以及超宽带因子，生成三维广义点云地图。具体来说，基于三维广义点云数据，提取长隧道场景下的结构特征和纹理特征信息，用于预先构建三维广义点云地图。

在一个实施例中，通过以下步骤得到局部地图：根据三维广义点云地图的坐标信息进行区块划分，统计每一个区块的点云中心点，搜寻与车辆当前状态信息最接近的区块及其周围若干区块，得到局部地图。具体来说，生成的三维广义点云地图根据其坐标信息进行区块划分，统计得到每一个区块的点云中心点，搜寻与车辆当前的状态信息最接近的区块及其周围若干点云，作为局部地图。

如图1所示，在步骤S3中，对三维广义点云数据以及局部地图进行扫描匹配，得到点云匹配结果，并将点云匹配结果以及传感器数据输入到无迹卡尔曼滤波中，得到车辆当前状态信息，作为车辆定位结果。

在一个实施例中，使用三维广义点云数据，结合局部地图进行扫描匹配，获取点云匹配结果。具体来说，使用的扫描匹配算法可以为NDT（正态分布变换算法）匹配，也可以为ICP（基于最小二乘法的最优匹配算法）匹配，根据现场实际情况而定。

在一个实施例中，在进行多传感器融合定位时初次获取的地图为与当前结果最接近的地图，通过初步定位后，才可以实现动态地图加载。

在一个实施例中，多传感器融合定位数据输出可以为高频输出和低频输出，具体取决于各个传感器的频率。

在一个实施例中，一种基于动态点云的长隧道场景下多传感器融合定位方法包含：

步骤a、将车辆定位结果作为下一次扫描匹配的初始值。具体来说，多传感器融合定位模块使用惯性测量单元（IMU）进行预积分得到IMU因子，轮速计的轮速因子以及超宽带（UWB）进行距离解算得到的UWB因子，基于无迹卡尔曼滤波（UKF）的方法得到当前车辆的状态信息，作为当前车辆的定位输出，并作为下一次扫描匹配的初始值。

步骤b、基于车辆定位结果对局部地图进行更新，并重复进行扫描匹配、无迹卡尔曼滤波、步骤a、步骤b，得到车辆的定位输出，直到完成整个自动驾驶过程，从而实现长隧道场景下的多传感器融合定位。具体来说，基于当前车辆的状态信息更新局部地图，并重复扫描匹配、无迹卡尔曼滤波、步骤a、步骤b，得到车辆的定位输出，直到完成整个自动驾驶过程，从而实现弱纹理弱GNSS的长隧道场景下的多传感器融合定位。

图2显示了根据本发明的另一个实施例的一种基于动态点云的长隧道场景下多传感器融合定位方法流程图。

步骤1、多源异构传感器包括激光雷达，视觉相机，惯性测量单元（IMU），轮速计和超宽带（UWB）等传感器，需先测验其传感器精度，确定有效探测距离。

步骤2、多源异构传感器中多种传感器需进行时间同步处理和标定处理，确保各传感器之间数据的统一。

步骤3、基于标定的结果，对于采集到的传感器数据进行处理，获取三维广义点云数据（如图3所示）。

步骤4、基于三维广义点云数据，提取长隧道场景下的结构特征和纹理特征信息，用于预先构建三维广义点云地图。

步骤5、基于三维广义点云数据和惯性测量单元（IMU）预积分结果，轮速计的信息以及超宽带（UWB）的距离解算结果生成三维广义点云地图。

步骤6、生成的三维广义点云地图根据其坐标信息进行区块划分，统计得到每一个区块的点云中心点，搜寻与车辆当前的状态信息最接近的区块及其周围若干点云，作为局部地图（如图4所示）。

步骤7、使用步骤4的三维广义点云数据，结合步骤6生成的局部地图（动态地图）进行扫描匹配，获取点云匹配结果。

步骤8、使用惯性测量单元（IMU）进行预积分得到IMU因子，轮速计的轮速因子以及超宽带（UWB）进行距离解算得到的UWB因子，基于无迹卡尔曼滤波（UKF）的方法得到当前车辆的状态信息（如图5所示），作为当前车辆的定位输出，并传入到步骤7中作为下一次扫描匹配的初始值。

步骤9、将当前车辆的状态信息传入到步骤6中，得到局部地图，并重复步骤7，8，9，得到车辆的定位输出，直到完成整个自动驾驶过程，从而实现弱纹理弱GNSS的长隧道场景下的多传感器融合定位。

本发明提供的一种基于动态点云的长隧道场景下多传感器融合定位方法还可以配合一种计算机可读取的存储介质，存储介质上存储有计算机程序，执行计算机程序以运行一种基于动态点云的长隧道场景下多传感器融合定位方法。计算机程序能够运行计算机指令，计算机指令包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。

计算机可读取的存储介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器（ROM，Read-OnlyMemory）、随机存取存储器（RAM，RandomAccess Memory）、电载波信号、电信信号以及软件分发介质等。

需要说明的是，计算机可读取的存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读取的存储介质不包括电载波信号和电信信号。

图6显示了根据本发明的一个实施例的一种基于动态点云的长隧道场景下多传感器融合定位装置结构框图。

根据本发明的另一个方面，还提供了一种基于动态点云的长隧道场景下多传感器融合定位装置，执行一种基于动态点云的长隧道场景下多传感器融合定位方法，装置包含：多源异构数据采集模块、动态三维地图生成模块以及多传感器融合定位模块。

多源异构数据采集模块用于使用多源异构传感器采集长隧道场景下的多种特征数据，处理得到三维广义点云数据以及传感器数据。具体来说，多源异构数据采集模块使用携带多源异构传感器的数据采集车辆采集长隧道场景下的多种特征信息。多源异构传感器包括但不限于：激光雷达，视觉相机，惯性测量单元（IMU），轮速计和超宽带传感器（UWB）等。首先进行传感器标定，然后进行时间同步处理，最后提取隧道场景下的特征信息，得到广义点云数据，并输入到动态三维地图生成模块和多传感器融合定位模块。

动态三维地图生成模块用于基于三维广义点云数据以及传感器数据生成三维广义点云地图，并对三维广义点云地图进行区块划分，选取与车辆当前状态信息最接近的局部地图。具体来说，动态三维地图生成模块对多源异构数据采集模块得到的数据进行处理，根据三维广义点云数据生成长隧道场景下的三维广义点云地图，并通过得到的地图坐标信息，进行区块划分，得到被划分成若干块的局部地图信息，并将初始数据输入到多传感器融合定位模块。

多传感器融合定位模块用于对三维广义点云数据以及局部地图进行扫描匹配，得到点云匹配结果，并将点云匹配结果以及所述传感器数据输入到无迹卡尔曼滤波中，得到车辆当前状态信息，作为车辆定位结果。具体来说，多传感器融合定位模块根据多源异构数据采集模块输入的实时数据和动态三维地图生成模块得到的三维广义点云地图，基于扫描匹配得到匹配结果，并和其他传感器信息一并输入到无迹卡尔曼滤波（UKF）中，得到车辆的当前状态，并输入到动态三维地图生成模块中，获取与当前状态最接近的周边区域地图，作为后续广义点云扫描匹配的输入，以此循环，最终得到全过程的车辆定位结果。

综上，本发明提供的一种基于动态点云的长隧道场景下多传感器融合定位方法，与现有技术相比，具备以下有益效果：

1、本发明使用激光雷达，视觉相机，惯性测量单元（IMU），轮速计和超宽带（UWB）传感器首先构建好测试区域的高精度三维广义点云地图。并基于构建的高精度地图实现弱纹理弱GNSS长隧道场景下的自动驾驶高精定位，确保在该环境下自动驾驶的安全运行。

2、本发明使用动态地图生成的方式，可以减少点云扫描匹配的时间消耗，提高在大规模场景下的定位效率。

3、本发明提出了一种高精度的隧道场景下的定位方法，极大的提高了隧道矿井等场景下的作业安全。

应该理解的是，本发明所公开的实施例不限于这里所公开的特定结构、处理步骤或材料，而应当延伸到相关领域的普通技术人员所理解的这些特征的等同替代。还应当理解的是，在此使用的术语仅用于描述特定实施例的目的，而并不意味着限制。

在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上；术语“上”、“下”、“左”、“右”、“内”、“外”、“前端”、“后端”、“头部”、“尾部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

某些术语在本申请文件中自始至终用来指示特定系统部件。如本领域的技术人员将认识到的那样，通常可以用不同的名称来指示相同的部件，因而本申请文件不意图区别那些只是在名称上不同而不是在功能方面不同的部件。在本申请文件中，以开放的形式使用术语“包括（comprise）”、“包含（include）”和“具有（have）”，并且因此应将其解释为意指“包括但不限于…”。此外，在本文中可能使用的术语“基本上”、“实质上”或者“近似地”涉及行业所接受的对相应术语的容差。如在本文中可能采用的术语“耦合”包括直接耦合和经由另外的组件、元件、电路、或者模块的间接耦合，其中对于间接耦合来说，介于其间的组件、元件、电路、或模块不更改信号的信息但是可调整其电流水平、电压水平、和/或功率水平。推断的耦合（例如其中一个元件通过推断耦合至另一个元件）包括以与“耦合”同样的方式在两个元件之间的直接和间接的耦合。

说明书中提到的“一个实施例”或“实施例”意指结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语“一个实施例”或“实施例”并不一定均指同一个实施例。

本发明的实施例是为了示例和描述起见而给出的，而并不是无遗漏的或者将本发明限于所公开的形式。很多修改和变化对于本领域的普通技术人员而言是显而易见的。选择和描述实施例是为了更好说明本发明的原理和实际应用，并且使本领域的普通技术人员能够理解本发明从而设计适于特定用途的带有各种修改的各种实施例。

虽然本发明所公开的实施方式如上，但所述的内容只是为了便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属技术领域内的技术人员，在不脱离本发明所公开的精神和范围的前提下，可以在实施的形式上及细节上作任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。

Claims (6)

1.一种基于动态点云的长隧道场景下多传感器融合定位方法，其特征在于，所述方法包含：

使用多源异构传感器采集长隧道场景下的多种特征数据，处理得到三维广义点云数据以及传感器数据，其中，所述多源异构传感器包含视觉相机、激光雷达、惯性测量单元、轮速计以及超宽带传感器；

基于所述三维广义点云数据以及所述传感器数据生成三维广义点云地图，并对所述三维广义点云地图进行区块划分，选取与车辆当前状态信息最接近的局部地图；

对所述三维广义点云数据以及所述局部地图进行扫描匹配，得到点云匹配结果，并将所述点云匹配结果以及所述传感器数据输入到无迹卡尔曼滤波中，得到车辆当前状态信息，作为车辆定位结果；

通过以下步骤得到所述传感器数据：对惯性测量单元数据进行预积分处理，得到惯性测量因子；对轮速计数据进行处理，得到轮速因子；对超宽带传感器数据进行距离结算，得到超宽带因子；

通过以下步骤生成所述三维广义点云地图：基于所述三维广义点云数据，提取长隧道场景下的结构特征和纹理特征信息，结合所述惯性测量因子、所述轮速因子以及所述超宽带因子，生成所述三维广义点云地图；

通过以下步骤得到所述局部地图：根据所述三维广义点云地图的坐标信息进行区块划分，统计每一个区块的点云中心点，搜寻与车辆当前状态信息最接近的区块及其周围若干区块，得到所述局部地图。

2.如权利要求1所述的一种基于动态点云的长隧道场景下多传感器融合定位方法，其特征在于，通过以下步骤得到所述三维广义点云数据：

测验所述多源异构传感器中每个传感器的精度，确定有效探测距离；

对所述多源异构传感器中的多种传感器进行时间同步处理和标定处理，确保各传感器之间数据的统一；

对激光雷达数据以及视觉相机数据进行特征提取，得到所述三维广义点云数据，其中，每个三维广义点需要包含三维坐标，强度，环数，时间戳，色彩。

3.如权利要求1所述的一种基于动态点云的长隧道场景下多传感器融合定位方法，其特征在于，惯性测量单元数据为

，在/>

时刻内，预积分处理包含以下表达式：

其中：

表示的是角速度，线性加速度；/>

表示的是旋转，速度和平移量；B表示本体坐标系；W表示世界坐标系。

4.如权利要求1-3中任一项所述的一种基于动态点云的长隧道场景下多传感器融合定位方法，其特征在于，所述方法包含：

步骤a、将所述车辆定位结果作为下一次扫描匹配的初始值；

步骤b、基于所述车辆定位结果对所述局部地图进行更新，并重复进行扫描匹配、无迹卡尔曼滤波、步骤a、步骤b，得到车辆的定位输出，直到完成整个自动驾驶过程，从而实现长隧道场景下的多传感器融合定位。

5.一种存储介质，其特征在于，其包含用于执行如权利要求1-4中任一项所述的方法步骤的一系列指令。

6.一种基于动态点云的长隧道场景下多传感器融合定位装置，其特征在于，执行如权利要求1-4中任一项的方法，所述装置包含：

多源异构数据采集模块，其用于使用多源异构传感器采集长隧道场景下的多种特征数据，处理得到三维广义点云数据以及传感器数据；

动态三维地图生成模块，其用于基于所述三维广义点云数据以及所述传感器数据生成三维广义点云地图，并对所述三维广义点云地图进行区块划分，选取与车辆当前状态信息最接近的局部地图；

多传感器融合定位模块，其用于对所述三维广义点云数据以及所述局部地图进行扫描匹配，得到点云匹配结果，并将所述点云匹配结果以及所述传感器数据输入到无迹卡尔曼滤波中，得到车辆当前状态信息，作为车辆定位结果。

Images