CN115979257A

CN115979257A - 一种分级融合定位方法、系统、电子设备和存储介质

Info

Publication number: CN115979257A
Application number: CN202211663251.9A
Authority: CN
Inventors: 温金水; 郑龙生; 宋舜辉
Original assignee: DeepRoute AI Ltd
Current assignee: DeepRoute AI Ltd
Priority date: 2022-12-23
Filing date: 2022-12-23
Publication date: 2023-04-18

Abstract

本发明涉及一种分级融合定位方法，包括：S1，通过第一信息采集单元、第二信息采集单元分别获取第一定位特征和第二定位特征，第一信息融合单元根据所述第一定位特征以及由所述第二定位特征得到的位姿增量值，融合计算输出第一类位姿估计值；S2，第二信息融合单元根据所述第一类位姿估计值、第二定位特征和高精度地图特征，融合计算输出第二类位姿估计值，根据第二类位姿估计值对车辆进行定位。本发明提供了一种分级融合定位方式，可充分兼容现有的组合定位设备，降低硬件设计难度和制造成本；同时提供两种定位结果，可提高系统的健壮性。

Description

一种分级融合定位方法、系统、电子设备和存储介质

技术领域

本发明涉及一种分级融合定位方法、系统、电子设备和存储介质，属于自动驾驶、定位领域。

背景技术

传统的车辆一般依靠于GNSS(全球导航卫星系统)和IMU(惯性测量单元)的组合定位算法来提供车辆位置、速度和姿态等信息。但随着智能驾驶技术的发展，GNSS/IMU组合定位算法的精度和可靠性不能满足下游感知模块和控制模块的要求，因而需要融合里程计、视觉、激光雷达、地图等信息。

中国专利CN110221328A公开了一种组合导航方法，该方法获取并根据车辆采集的卫星定位信息、角速度信息、加速度信息、视觉信息，确定车辆的第一位置信息和第一航向角信息；将第一位置信息和第一航向角信息与预置的车载地图进行匹配，确定车辆所处位置各车道线的拥堵信息，生成导航提示信息。这种方法利用了GNSS、IMU和视觉等信息生成位置与航向，但没有使用轮速和地图的信息来修正原有的位置和航向，其定位精度不高。

中国专利CN111596329A公开了一种利用GPS数据、匹配定位数据(激光雷达)和里程计进行卡尔曼滤波计算，获得目标定位信息的方法。这种方法没有利用IMU数据，姿态的精度不高；同时GPS、激光雷达、里程计数据在同一个滤波器中融合，对计算平台的算力和定位算法的健壮性要求偏高，而且不能直接使用现有的GNSS/IMU组合定位设备。

发明内容

针对上述现有技术的不足，本发明的目的在于提供一种分级融合定位方法、系统和装置，以解决现有技术中没有充分利用GNSS、IMU、里程计、视觉和激光雷达等多种传感器信息、对于融合定位算法算力要求过高、算法健壮性不足的问题。

本发明提供了一种分级融合定位方法，所述方法包括以下步骤：

S1，通过第一信息采集单元、第二信息采集单元分别获取第一定位特征和第二定位特征，第一信息融合单元根据所述第一定位特征以及由所述第二定位特征得到的位姿增量值，融合计算输出第一类位姿估计值；

S2，第二信息融合单元根据所述第一类位姿估计值、所述第二定位特征和高精度地图特征，融合计算输出第二类位姿估计值；

根据所述第二类位姿估计值对车辆进行定位。

进一步地，所述第一定位特征包括GNSS位置、GNSS航向、IMU加速度、IMU角速度、里程计速度，所述第二定位特征包括点云数据、图像信息。

进一步地，所述步骤S1中第一信息融合单元根据所述第一定位特征以及由所述第二定位特征得到的位姿增量值，融合计算输出第一类位姿估计值具体包括以下步骤：

S101，所述第二定位特征通过第一匹配算法计算得到位姿增量值；

S102，第一信息融合单元通过将所述第二定位特征与所述位姿增量值进行融合计算输出第一类位姿估计值。

进一步地，所述步骤S101中计算位姿增量值具体步骤为，通过第一匹配算法将当前时刻的第二定位特征与前一时刻的第二定位特征进行匹配，进而获得位姿增量值。

进一步地，步骤S2具体包括以下步骤：

S201，通过第二匹配算法将当前时刻的第二定位特征、初始位姿估计值和高精度地图特征进行匹配，计算输出第二类位姿观测值；

S202，利用第二信息融合单元对第一定位特征、第二类位姿观测值进行融合计算，输出第二类位姿估计值。

进一步地，所述初始位姿估计值由第二信息融合单元对第一定位特征和第一类位姿估计值进行融合计算获得。

进一步地，所述第二信息采集单元包括激光雷达、相机，所述第一信息采集单元包括GNSS、IMU、里程计。

进一步地，所述第一信息融合单元通过第一卡尔曼滤波来计算所述第一类位姿估计值，其中所述第一卡尔曼滤波的状态向量为15维向量，所述15维向量包括IMU的姿态误差、速度误差、位置误差、角速度零偏和加速度零偏；以及

所述第二信息融合单元通过第二卡尔曼滤波计算所述第二类位姿估计值，其中所述第二类卡尔曼滤波的状态向量为18维向量，所述18维向量包括IMU的姿态误差、速度误差、位置误差、角速度零偏、加速度零偏和地图位置偏移。

一种分级融合定位系统，所述系统包括第一定位系统和第二定位系统，所述第一定位系统用于输出第一类位姿估计值，所述第一类位姿估计值提供给第二定位系统，所述第二定位系统用于为第一定位系统提供位姿增量，并输出第二类位姿估计值；

所述第一定位系统包括第一信息采集单元、第一信息融合单元，所述第一信息采集单元用于获取第一定位特征；所述第一信息融合单元用于将所述第二定位系统提供的位姿增量和第一定位特征融合，并输出第一类位姿估计值；

所述第二定位系统包括第二信息采集单元、第三信息采集单元、第一匹配单元、第二匹配单元和第二信息融合单元，所述第二信息采集单元用于获取第二定位特征，所述第三信息采集单元用于获取高精度地图特征，所述第一匹配单元用于将当前时刻的第二定位特征与前一时刻的第二定位特征进行匹配以输出位姿增量，所述第二信息融合单元用于将所述第一定位特征和位姿增量融合以输出初始位姿值，所述第二匹配单元用于将第二定位特征、初始位姿和高精度地图特征匹配以输出第二类位姿观测值，所述第二信息融合单元还用于将所述第二类位姿观测值和第一定位特征融合以输出第二类位姿估计值。

一种自动驾驶车辆，所述自动驾驶车辆包括如上所述的分级融合定位系统。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的一种分级融合定位方法的步骤。

与现有技术相比，本发明的有益效果在于：

1、本发明充分利用了GNSS、IMU、里程计、视觉和激光雷达等多种传感器，可同时输出GNSS坐标系和地图坐标系下的位置，方便下游按需使用；同时根据计算性能的不同，提供了冗余的两种定位结果，提高了定位系统的健壮性；

2、本发明提供了两个定位系统，包括第一定位系统和第二定位系统，采用分级融合的方式，其中第一定位系统可独立运行在组合定位设备中，方便兼容现有的组合定位设备，保证算法的实时性，同时设计位姿增量的输入接口，可利用位姿增量进一步提高第一类位姿估计值的精度；第二定位系统可在计算性能较强的计算平台上运行，为第一定位系统提供位姿增量的估计值，同时接收第一类位姿估计数据，结合激光雷达、相机和高精度地图的数据，计算并输出第二类位姿估计值，从而提高定位的精确度。

附图说明

图1为本发明一种分级融合定位方法的流程图；

图2为本发明一种分级融合定位方法中步骤S1的具体流程图；

图3为本发明一种分级融合定位方法中步骤S2的具体流程图；

图4为本发明一种分级融合定位系统的结构图；

图5为本发明一种分级融合定位装置中的第一定位模块的结构图；

图6为本发明一种分级融合定位装置中的第二定位模块的结构图。

具体实施方式

为了使本领域的技术人员更好地理解本申请中的技术方案，下面将结合本申请实施例中的附图对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

本申请中术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而包括一系列要素的过程、方法、物品或系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或系统中还存在另外的相同要素。在本申请中，“至少一个(项)”是指一个或者多个，“多个”是指两个或两个以上，“至少两个(项)”是指两个或三个及三个以上，“和/或”，用于描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：只存在A，只存在B以及同时存在A和B三种情况，其中A，B可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。“以下至少一项(个)”或其类似表达，是指这些项中的任意组合。例如，a，b或c中的至少一项(个)，可以表示：a，b，c，“a和b”，“a和c”，“b和c”，或“a和b和c”。

如图1-3所示，本发明提供了一种分级融合定位方法，具体包括以下步骤：

具体地，包括以下步骤：

S101，通过第一匹配算法将当前时刻的第二定位特征与前一时刻的第二定位特征进行匹配，进而计算得出位姿增量值；

特征匹配算法是一种内容繁杂、论文众多的算法，包括ICP(迭代最临近点)类、NDT(正态分布变换)、LOAM类、暴力匹配、快速近似最近邻(FLANN)等，其中ICP类算法又称为点云匹配算法，指的是输入两幅点云Ps和Pt，输出一个变换T使得T(Ps)和T(Pt)的重合程度尽可能高，变换包括旋转平移，点云匹配算法包括粗配准和精配准两步，粗配准值得是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，其目的主要是为精配准提供较好的变换初值，精配准则是给定一个初始变换，进一步优化得到更精确的变换。

在本发明实施例中，第二信息采集单元获取的第二定位特征为点云数据、图像信息，具体地，第二信息采集单元包括探测系统和摄像系统，探测系统可以为用于提供点云信息的雷达，如激光雷达，摄像系统为用于提供图像信息的相机，定位系统通过激光雷达获取当前时刻和前一时刻的点云数据，通过相机获取当前时刻和前一时刻的图像信息，该图像信息为车辆行驶的真实场景环境信息，该真实场景环境信息包括加油站、建筑物、车道标记、道路、树木、行人等；然后通过第一匹配算法将当前时刻的点云数据与前一时刻的点云数据，得到点云数据的位姿增量，同时，将当前时刻的图像信息与前一时刻的图像信息进行匹配，得到图像信息的位姿增量。需要说明的是，匹配范围为车辆附近特定距离内的特征。

具体地，相机提取的特征为图像信息，其特征提取方法有Haris、FAST、SIFI、SURF等方法，特征匹配算法包括暴力匹配、快速近似最近邻(FLANN)算法；激光雷达的特征匹配算法主要包括ICP类算法、NDT算法、LOAM类算法等。

S102，第一信息融合单元通过将第二定位特征与上述位姿增量值进行融合计算输出第一类位姿估计值。

具体地，第一信息采集单元包括GNSS、IMU和里程计，第一定位特征包括GNSS位置、GNSS航向、IMU加速度、IMU角速度、里程计速度，其中，GNSS可以为基于全球卫星定位系统、俄罗斯的格洛纳斯导航系统、欧盟的伽利略导航系统、中国的北斗二代导航系统和星基增强系统的多模卫星定位模块；IMU是测量物体三轴姿态角(或角速率)以及加速度的装置，其主要元件是陀螺仪以及加速度计，IMU的精度直接影响到惯性系统的精度；里程计是测量汽车行程的装置。

在本发明实施例中，可以通过GNSS接收卫星观测数据，并对卫星观测数据进行解算，以获取对车辆的卫星定位信息，具体为车辆的位置信息和航向信息；可通过IMU获取车辆的加速度和角速度信息，其中，IMU可以包括陀螺仪和加速度计，本发明对此不作具体限制；还可通过里程计采集车辆的速度信息。

在本发明实施例中，当获取了车辆的GNSS位置信息、GNSS航向信息、IMU加速度信息、IMU角速度信息以及里程计速度信息后，可以采用上述信息和点云数据、图像信息的位姿增量进行卡尔曼滤波计算，估计出车辆的位置和姿态信息，作为车辆的第一类位姿估计值提供给外界和下一步骤。将位姿增量与第一定位特征融合，可利用位姿增量进一步提高第一类位姿估计值的精度。需要说明的是，第一类位姿估计值是基于GNSS坐标系下的位姿估计。

S2，第二信息融合单元根据第一类位姿估计值、第二定位特征和高精度地图特征，融合计算输出第二类位姿估计值；根据第二类位姿估计值对车辆进行定位。

具体地，包括以下步骤：

S201，利用第二信息融合单元对第一定位特征和第一类位姿估计值进行融合计算，输出初始位姿估计值，通过第二匹配算法将当前时刻的第二定位特征、初始位姿估计值和高精度地图特征进行匹配，计算得出第二类位姿观测值。

在本发明实施例中，通过将车辆的GNSS位置信息、GNSS航向信息、IMU加速度信息、IMU角速度信息以及里程计速度信息与第一类位姿估计值进行卡尔曼滤波计算，以输出初始位姿估计值。

其中高精度地图，具体地，高精度，一方面是说高精度电子地图的绝对坐标精度更高，而绝对坐标精度指的是地图上某个目标和真实的外部世界的事物之间的精度；另一方面，高精度地图所含有的道路交通信息元素更丰富和细致。

输入初始位姿估计值作为点云或图像匹配的初始位姿；利用高精度地图与确定初始位姿的点云数据进行匹配，即基于点云中的数据点对高精度地图中的地图位置点进行匹配筛选，以确定匹配度最高的一个点，将该点作为第一目标位置点；利用高精度地图与确定初始位姿的图像信息进行匹配，即基于图像信息中的真实环境信息对高精度地图中的地图位置点进行匹配筛选，以确定匹配度最高的一个点，将该点作为第二目标位置点；第二类位姿观测值可以是第一目标位置点或(和)第二目标位置点。通过点云数据、图像信息可对传感器无法探测的部分进行补充，可较好的反馈实时状况和外部信息，并获取当前位置精准的交通状况，将两者与高精度地图进行匹配，可对车辆位置精准定位；加之初始位姿估计值，可在三维坐标上进一步提高车辆定位的精确度。

S202，利用第二信息融合单元对第一定位特征、第二类位姿观测值值进行融合计算，输出第二类位姿估计值，根据第二类位姿估计值对车辆进行定位。

在本发明实施例中，通过将车辆的GNSS位置信息、GNSS航向信息、IMU加速度信息、IMU角速度信息以及里程计速度信息与第二类位姿观测值值进行卡尔曼滤波计算，以输出第二类位姿估计值。需要说明的是，第二类位姿估计值是基于地图坐标系下的位姿估计。

基于上述介绍，本申请的车辆定位方法可结合自动驾驶技术实现自动驾驶或辅助驾驶，相比于传统定位方法，本发明充分利用了GNSS、IMU、里程计、相机、激光雷达等多种传感器，可同时输出GNSS坐标系和地图坐标系下的位置，不仅方便下游按需使用，还可有效提高定位系统的可靠性，大大降低传统定位方法中的路况误差、传感器误差、高精度地图误差和算法误差。

具体地，第一信息融合单元通过第一卡尔曼滤波来计算第一类位姿估计值，第二信息融合单元通过第二卡尔曼滤波计算第二类位姿估计值。卡尔曼滤波是一种最优估计算法，利用原始传感器(GNSS、IMU、里程计)或匹配算法输出的位置/速度/姿态/角速度/加速度等运动状态相关的观测，估计出载体的位置/速度/姿态。卡尔曼滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。卡尔曼滤波，其能迭代的对某些参数进行最优估计，其融合计算公式包括空间状态方程和卡尔曼滤波算法方程。

对于空间状态方程：

X_k＝Φ_k/k-1X_k-1+w_k，

Z_k＝H_kX_k+v_k，

式中，X为状态向量，Z为观测向量，Φ_k/k-1为k-1到k时刻的状态转移矩阵，H_k为k时刻的观测矩阵，w_k为k时刻的状态噪声，v_k为k时刻的观测噪声。

卡尔曼滤波算法方程为：

P_k＝(I-K_kH_k)P_k/k-1，

式中，K为卡尔曼增益矩阵；P为误差协方差矩阵，代表状态向量X的不确定度；Q为状态噪声矩阵，代表状态向量X的噪声；R为观测噪声矩阵，代表观测Z的噪声。

具体地，对于步骤S1中的第一卡尔曼滤波器，状态向量X为15维向量，其元素分别为IMU的姿态误差、速度误差、位置误差、角速度零偏、加速度零偏。零偏是加速度或角速度校准方式之一，即传感器的测量值相对于零点的偏移量，本发明中加速度零偏、角速度零偏均为卡尔曼滤波估计获取，引入加速度零偏和角速度零偏可对位姿进一步校准以提高位姿精度。观测Z为位置误差、航向误差、速度误差和位姿增量，其中位置误差为GNSS位置与IMU位置的差值，航向误差为GNSS航向与IMU航向的差值，速度误差为里程计速度与IMU速度的差值。第一卡尔曼滤波估计出IMU的姿态误差、速度误差、位置误差、角速度零偏、加速度零偏，然后用IMU的姿态、速度、位置分别减去估计出来的误差，即为IMU位姿的估计值，认为IMU精确安装在车辆上，用IMU的位姿代表车辆位姿。

具体地，对于步骤S201中的第二卡尔曼滤波器，状态向量X的元素分别为IMU的姿态误差、速度误差、位置误差、角速度零偏、加速度零偏，观测Z为位置误差、航向误差、速度误差和第一类位姿估计值，第二卡尔曼滤波器输出初始位姿估计值。

具体地，对于步骤S202中的第二卡尔曼滤波器，状态向量X为18维状态向量，其元素分别为IMU的姿态误差、速度误差、位置误差、角速度零偏、加速度零偏和地图位置偏移，其中姿态误差、速度误差、位置误差和地图位置偏移是在东向、北向和天向的分状态，角速度零偏和加速度零偏是在X方向、Y方向和Z方向的分状态。观测Z为第一位置误差、第二位置误差、姿态误差和速度误差，其中第一位置误差包括激光雷达与地图匹配出的位置与IMU位置的差值、相机与地图匹配出的位置与IMU位置的差值中的一种或多种，第二位置误差包括GNSS位置与IMU位置的差值，姿态误差包括激光雷达与地图匹配出的姿态与IMU姿态的差值、相机与地图匹配出的姿态与IMU姿态的差值中的一种或多种，速度误差为里程计速度与IMU速度的差值，其他参数与第一卡尔曼滤波器相同。需要说明的是，地图位置偏移由第二卡尔曼滤波算法估计获得，引入地图位置偏移，可尽可能的减少地图误差对于车辆定位的影响，提高车辆行驶时在地图上的位置与真实环境位置的一致性。

本发明实施例提出了一种分级融合定位方式，通过获取并根据车辆采集的点云数据信息和图像信息，利用匹配算法获得对应的位姿增量值，然后获取车辆的GNSS的位置和航向信息、IMU的加速度和角速度信息、里程计速度信息，并将上述信息和位姿增量进行卡尔曼滤波算法融合，输出第一类位姿估计；将第一类位姿估计输入卡尔曼滤波算法中以获取初始位姿，然后利用匹配算法将点云数据特征、图像信息特征、初始位姿估计与高精度地图进行匹配以获取第二类位姿观测值，然后将第二类位姿观测值与第一定位特征进行融合，以输出第二类位姿估计值。通过该分级融合定位方式，可同时输出GNSS坐标系和地图坐标系下的车辆位置，方便下游按需使用；同时根据硬件计算性能的不同，同时运行两套定位算法，提供两种定位结果，提高了定位系统的健壮性。

需要说明的是，对于方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明实施例并不受所描述的动作顺序的限制，因为依据本发明实施例，某些步骤可以采取其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作并不一定是本发明实施例所必须的。

如图4所示，示出了本发明的一种分级融合定位系统实施例的结构图，该系统具体可以包括第一定位系统和第二定位系统，第一定位系统用于输出第一类位姿估计值，第一类位姿估计分别提供给外界和第二定位系统，第二定位系统用于为第一定位系统提供位姿增量，并向外界输出第二类位姿估计值。

本定位系统可分为两个部分：第一定位系统和第二定位系统。其中第一定位系统可独立得运行在组合定位设备中，方便兼容现有的组合定位设备，既保证了算法的实时性，又降低了硬件设计的难度和制造的成本；设计了位姿增量的输入接口，可利用位姿增量进一步提高第一定位系统输出的第一类位姿估计值的精度。第二定位系统在计算性能较强的计算平台上运行，为第一定位系统提供位姿增量的估计值；同时接收第一类位姿估计值数据，结合激光雷达、相机和高精度地图的数据，计算出第二类位姿估计值。本系统同时向外界输出第一类位姿估计值和第二类位姿估计值进行定位，通过设置两种定位结果，可提高定位系统的健壮性。

进一步地，第一定位系统包括第一信息采集单元、第一信息融合单元，第一信息采集单元包括GNSS、IMU、里程计，用于获取第一定位特征；第一信息融合单元用于将第二定位系统提供的位姿增量和第二定位特征融合，并输出第一类位姿估计值。

具体地，第一定位系统中，GNSS输出车辆位置和航向的观测，IMU输出车辆角速度和加速度的数据，里程计输出车辆前进的速度。第一卡尔曼滤波器接收来自GNSS/IMU/里程计以及外部提供的位姿增量数据，估计出车辆的位置、速度和姿态，作为第一类位姿估计(GNSS坐标系下)提供给第二定位系统。

进一步地，第二定位系统包括第二信息采集单元、第三信息采集单元、第一匹配单元、第二匹配单元和第二信息融合单元，第二信息采集单元包括激光雷达、相机，用于获取第二定位特征，第三信息采集单元用于获取高精度地图特征，第一匹配单元用于将当前时刻的第一定位特征与前一时刻的第二定位特征进行匹配以输出位姿增量值，第二信息融合单元用于将所述第一定位特征和第一类位姿估计融合以输出初始位姿估计值，第二匹配单元用于将第二定位特征、初始位姿估计值和高精度地图特征匹配以输出第二类位姿观测值，第二信息融合单元还用于将所述第二类位姿观测值和第一定位特征融合以输出第二类位姿估计值。

具体地，第二定位系统中，根据卡尔曼滤波估计的位姿、激光雷达/相机的数据、高精度地图数据，此模块利用第二匹配算法计算车辆在地图坐标下的位姿，作为第二类位姿观测值；第二卡尔曼滤波器使用GNSS、IMU、里程计和第二类位姿观测值，计算出IMU的位姿估计数值，将其加上地图偏置，输出第二定位系统的位姿结果(地图坐标系下)。

对于系统实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

如图5-6所示的一种分级融合定位装置，该装置包括第一定位模块和第二定位模块，第一定位模块和第二定位模块通信连接；

第一定位模块包括GNSS板卡、IMU模块、微控制单元、第一存储单元、电源转换单元、总线通信模块、雷达数据接口和相机数据接口，GNSS板卡用于提供车辆位置、速度、航向的观测信息，IMU模块用于提供加速度、角速度和温度等信息，微控制单元主要作用是对GNSS、IMU、里程计、雷达\相机匹配后的位姿增量数据，进行卡尔曼滤波融合处理，输出可靠的位姿信息提供给外界；总线通信模块包括天线接口和总线接口，天线接口连接外界的GNSS天线，用于为GNSS板卡提供原始电信号，总线接口用于接收里程计、GNSS差分修正，同时输出定位算法估计的位姿数据；存储单元用来存储定位算法的一些配置文件、外部输入的原始数据等内容

第二定位模块为车载计算平台，包括计算单元和第二存储单元，第二定位模块通过网络交换机与外界进行数据传输。

本发明实施例提供一种自动驾驶车辆，所述自动驾驶车辆包括如上所述的分级融合定位系统。

一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现分级融合定位方法的步骤。

本公开上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等等，或者上述的任意合适的组合。

上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。

可以以一种或多种程序设计语言或其组合来编写用于执行本公开的操作的计算机程序代码，上述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(AN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

前述对本发明的具体示例性实施方案的描述是为了说明和例证的目的。这些描述并非想将本发明限定为所公开的精确形式，并且很显然，根据上述教导，可以进行很多改变和变化。对示例性实施例进行选择和描述的目的在于解释本发明的特定原理及其实际应用，从而使得本领域的技术人员能够实现并利用本发明的各种不同的示例性实施方案以及各种不同的选择和改变。本发明的范围意在由权利要求书及其等同形式所限定。

Claims

1.一种分级融合定位方法，其特征在于，所述方法包括以下步骤：

根据所述第二类位姿估计值对车辆进行定位。

2.根据权利要求1所述的一种分级融合定位方法，其特征在于，所述第一定位特征包括GNSS位置、GNSS航向、IMU加速度、IMU角速度、里程计速度，所述第二定位特征包括点云数据、图像信息。

3.根据权利要求1所述的一种分级融合定位方法，其特征在于，所述步骤S1中第一信息融合单元根据所述第一定位特征以及由所述第二定位特征得到的位姿增量值，融合计算输出第一类位姿估计值具体包括以下步骤：

4.根据权利要求3所述的一种分级融合定位方法，其特征在于，所述步骤S101中计算位姿增量值具体步骤为，通过第一匹配算法将当前时刻的第二定位特征与前一时刻的第二定位特征进行匹配，进而获得位姿增量值。

5.根据权利要求1所述的一种分级融合定位方法，其特征在于，步骤S2具体包括以下步骤：

6.根据权利要求5所述的一种分级融合定位方法，其特征在于，所述初始位姿估计值由第二信息融合单元对第一定位特征和第一类位姿估计值进行融合计算获得。

7.根据权利要求1所述的一种分级融合定位方法，其特征在于，所述第二信息采集单元包括激光雷达、相机，所述第一信息采集单元包括GNSS、IMU、里程计。

8.根据权利要求1所述的一种分级融合定位方法，其特征在于，所述第一信息融合单元通过第一卡尔曼滤波来计算所述第一类位姿估计值，其中所述第一卡尔曼滤波的状态向量为15维向量，所述15维向量包括IMU的姿态误差、速度误差、位置误差、角速度零偏和加速度零偏；以及

9.一种分级融合定位系统，其特征在于，所述系统包括第一定位系统和第二定位系统，所述第一定位系统用于输出第一类位姿估计值，所述第一类位姿估计值提供给第二定位系统，所述第二定位系统用于为第一定位系统提供位姿增量，并输出第二类位姿估计值；

10.一种自动驾驶车辆，其特征在于，所述自动驾驶车辆包括如权利要求9所述的分级融合定位系统。

11.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-8中任一项所述的一种分级融合定位方法的步骤。