CN117805866A - 基于高精地图的多传感器融合定位的方法、装置和介质 - Google Patents

Abstract

本申请描述了用于基于高精地图的多传感器融合定位的方法装置和介质。该方法包括：根据从第一传感器获取的与车辆相关联的图像来提取第一元素集合；至少部分地基于所述第一元素集合与从所述高精地图中提取的第二元素集合的语义特征匹配来执行所述车辆的第一定位；根据从第二传感器获取的定位数据来执行所述车辆的第二定位；以及至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果。还公开了众多其他方面。

Description

基于高精地图的多传感器融合定位的方法、装置和介质

技术领域

本发明涉及对车辆进行定位的技术领域，并且更具体地涉及基于高精地图的多传感器融合定位的方法、装置和介质。

背景技术

目前自动驾驶车辆主要使用预先建立的高精地图，依赖相机、激光雷达、组合导航等设备做自主定位。高精地图在建图时通常是多传感器融合建图，融合激光雷达、组合导航、视觉相机观测的数据来预先生成。基于高精地图，在车辆定位阶段，可以使用激光雷达点云进行匹配定位、基于实时动态测量技术(RTK)的组合导航定位、或者基于相机图像提取特征执行匹配定位。当前激光雷达设备价格无法满足普通乘用车的量产要求，在车辆定位上的使用不普及；基于RTK的组合导航设备易受信号遮挡干扰，且用于前述高精地图匹配定位时会存在偏置误差，无法直接用于定位；相机在定位时精度难以保证，同时，也易受光线的干扰，定位结果不稳定。

因此，亟待提出一种更加精确的基于高精地图的多传感器融合定位的方法。

发明内容

以下给出一个或多个方面的简要概述以提供对这些方面的基本理解。此概述不是所有构想到的方面的详尽综览，并且既非旨在标识出所有方面的关键性或决定性要素亦非试图界定任何或所有方面的范围。其唯一的目的是要以简化形式给出一个或多个方面的一些概念以作为稍后给出的更加详细的描述之序。

为了解决上述问题，本方面提出了一种用于基于高精地图的多传感器融合定位的方法、装置和介质。

本申请的一方面提供了一种用于基于高精地图的多传感器融合定位的方法，包括：根据从第一传感器获取的与车辆相关联的图像来提取第一元素集合；至少部分地基于所述第一元素集合与从所述高精地图中提取的第二元素集合的语义特征匹配来执行所述车辆的第一定位；根据从第二传感器获取的定位数据来执行所述车辆的第二定位；以及至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果。

优选地，所述第一元素集合与所述第二元素集合的所述语义特征匹配包括：获取第二元素集合中在地图坐标系中的一个或多个第二位置点；获取第一元素集合中与所述一个或多个第二位置点中的每个第二位置点相对应的、在相机坐标系中的一个或多个第一位置点；确定将所述一个或多个第一位置点投影到地图坐标系中的一个或多个位置结果；计算在指定时刻在地图坐标系中的所述一个或多个第二位置点中的每个第二位置点与所述一个或多个位置结果中的相应位置结果之间的一个或多个误差；以及至少部分地基于所述一个或多个误差之和来确定所述车辆在所述指定时刻在地图坐标系中的第一位姿。

优选地，执行所述车辆的第一定位包括：至少部分地基于所述车辆在所述指定时刻在地图坐标系中的第一位姿来推导所述车辆在所述指定时刻之后的在地图坐标系中的未来位姿。

优选地，根据从第二传感器获取的定位数据来执行所述车辆的第二定位包括：根据从全球卫星导航系统GNSS定位模块获取的GNSS数据来推导所述车辆的当前经纬度信息；将所述当前经纬度信息经墨卡托投影变换转换成笛卡尔平面坐标系结果；以及经转换矩阵T_g投影至地图坐标系以确定所述车辆在地图坐标系中的第二位姿。

优选地，至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果包括：使用扩展卡尔曼滤波器来融合所述第一位姿和所述第二位姿，其中所述扩展卡尔曼滤波器的状态转移矩阵

优选地，该方法进一步包括：响应于语义定位模块有输出结果，作为滤波器观测，使用第一观测矩阵H₁更新所述扩展卡尔曼滤波器的状态估计值，其中 以及响应于GNSS定位模块有输出结果，作为滤波器观测，使用第二观测矩阵H₂更新所述扩展卡尔曼滤波器的状态估计值，其中/>

优选地，该方法进一步包括：在确定所述多传感器融合定位结果后，将航位推算的误差比例系数以及GNSS转换矩阵的投影误差值作为补偿量来修正所述扩展卡尔曼滤波器的下一输入。

优选地，该方法进一步包括：使用航位推算的误差比例系数来修正航位推算模块的输出结果；以及使用GNSS转换矩阵的所述投影误差值来修正GNSS投影观测输入。

优选地，该方法进一步包括：使用激光定位结果或超声定位结果作为所述扩展卡尔曼滤波器的滤波器观测。

本申请的另一方面提供了一种用于基于高精地图的多传感器融合定位的装置，包括处理器和存储器，所述存储器存储有程序指令；所述处理器运行程序指令实现如上述方法中的任一项。

本申请的又一方面提供了一种存储指令的非瞬态计算机可读存储介质，所述指令当被计算机执行时，使所述计算机执行如上述任一项的用于基于高精地图的多传感器融合定位的方法。

本申请的又一方面提供了一种车辆，在所述车辆中运行存储的程序，其中在所述程序运行时控制所述车辆的处理器执行上述任一项的用于基于高精地图的多传感器融合定位的方法。

提供本发明内容是为了以简化的形式来介绍一些概念，这些概念将在下面的具体实施方式中进一步描述。本发明内容不旨在标识所要求保护的主题的关键特征或必要特征，也不旨在用于限制所要求保护的主题的范围。各实施例的其他方面、特征和/或优点将部分地在下面的描述中阐述，并且将部分地从描述中显而易见，或者可以通过本公开的实践来学习。

附图说明

为了能详细地理解本发明的上述特征所用的方式，可以参照各实施例来对以上简要概述的内容进行更具体的描述，其中一些方面在附图中示出。然而应该注意，附图仅示出了本发明的某些典型方面，故不应被认为限定其范围，因为该描述可以允许有其它等同有效的方面。在附图中，类似附图标记始终作类似的标识。要注意，所描述的附图只是示意性的并且是非限制性的。在附图中，一些部件的尺寸可放大并且出于解说性的目的不按比例绘制。

图1解说了根据本发明的一实施例的用于操作语义定位模块的过程流的示例。

图2解说了根据本发明的一实施例的用于操作全球卫星导航系统(GNSS)定位模块的过程流的示例。

图3解说了根据本发明的一实施例的用于基于高精地图的多传感器融合定位的方法的示例。

图4解说了根据本发明的一实施例的用于基于高精地图的多传感器融合定位的另一方法的示例。

图5解说了根据本发明的一实施例的用于基于高精地图的多传感器融合定位的示例方法的流程图。

图6解说了根据本发明的一实施例的支持用于基于高精地图的多传感器融合定位的装置的框图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。在以下详细描述中，阐述了许多具体细节以提供对所描述的示例性实施例的透彻理解。然而，对于本领域技术人员显而易见的是，可以在没有这些具体细节中的一些或全部的情况下实践所描述的实施例。在其他示例性实施例中，没有详细描述公知的结构或处理步骤，以避免不必要地模糊本公开的概念。

在本说明书中，除非另有说明，否则通过本说明书使用的术语“A或B”指的是“A和B”和“A或B”，而不是指A和B是排他性的。

本发明提出一种基于高精地图的多传感器融合定位方法。在本申请的实施例中，执行该方法的主要模块包括：高精地图制图模块、航位推算定位模块、语义定位模块、GNSS定位模块、以及多传感器融合定位模块。优选地，高精地图制图模块中，采图车辆针对需要定位的场景，使用激光雷达、组合导航、相机等设备预先创建场景的高精地图以用于后续的多传感器高精度融合定位。优选地，多传感器融合定位模块使用低成本的相机、GNSS系统、车辆轮速、惯性测量单元(IMU)等传感器，做融合定位，同时借助相机定位来修正GNSS在地图上的匹配定位误差。可实现全天候、全场景的高精度定位。优选地，该多传感器融合定位模块的输入可以包括：航位推算定位模块、语义定位模块、以及GNSS定位模块、和/或其组合、以及其他模块。

以下结合图1-图6对操作各个模块的过程流和用于基于高精地图的多传感器融合定位的方法、装置、系统、电子设备、车辆和介质以及众多其他方面进行描述。

图1解说了根据本发明的一实施例的用于操作语义定位模块的过程流的示例。

在本申请的实施例中，出于简化起见，本发明所使用的定位方案可以是二维平面定位，其定位结果为车辆(也可以是其他交通工具)在地图坐标系下的位置和姿态(简称为位姿)：横坐标x、纵坐标y和航向角θ。

在本申请的实施例中，航位推算定位模块(图1中未示出)可以用于计算车辆在两个时刻的位姿变化。假设车辆在k时刻的位姿为x_k，y_k，θ_k，可记为T_k，车辆在k+1时刻的位姿为x_k+1，y_k+1，θ_k+1，可记为T_k+1。在已知k时刻位姿的情况下，可以通过该航位推算定位模块递推出k+1时刻的位姿。推算公式如下：

x_k+1＝x_k+ds×cos(θ_k+0.5×da)

y_k+1＝y_k+ds×sin(θ_k+0.5×da)

θ_k+1＝θ_k+da 式(1)

其中ds为轮速脉冲提供的车辆两个时刻的位移变化值，da为惯性测量单元(IMU)提供的车辆两个时刻之间的航向角变化值。

在本申请的实施例中，操作语义定位模块的过程流可以如图1中所示。语义定位模块需基于由高精地图模块预先建立场景的高精地图，该高精地图存储了场景的语义元素。在本申请的实施例中，高精地图中的语义元素包括但不限于如下地面元素：道路中线、减速带、方向箭头、道路边缘、斑马线、停车位等。这些元素在现实场景中比较常见，且场景变化更新的频率较低。因此可以确保高精地图的元素丰富性和场景时效性。在本申请的实施例中，元素丰富性是指用于场景识别或车辆定位的语义元素类型与数量，数量与类型越多，场景越丰富，而场景时效性是指场景变化的频率，评估在经过一段时间后，地图对该场景的重构是否失效。为了方便后文阐述，高精地图中的语义元素可以被记为第二元素集合A。

在本申请的实施例中，操作语义定位模块的步骤如下：

步骤1：车辆通过搭载的相机，实时获取(例如，拍摄、采集)车身周围场景图像，使用深度学习算法，在图像中提取语义元素。语义元素类型同样为：道路中线、减速带、方向箭头、道路边缘、斑马线、停车位等。相机图像提取的语义元素可以被记为第一元素集合C。

步骤2：设第二元素集合A中在地图坐标系中的第二位置点为P_m，第一元素集合C中与该第二位置点相对应的、在相机坐标系中的第一位置点为P_c，记车辆在k时刻在地图中的位姿为T_mbk，即T_mbk包含车辆的二维位置和姿态，即：x_mbk，y_mbk，θ_mbk，则可以通过如下关系得到车辆在当前位置时，第一元素集合元素C中的第一位置点投影到地图上的位置结果为：

P′_m＝T_mbk·T_bkck·P_ck 式(2)

其中，P_ck是k时刻第一元素集合C中的第一位置点在相机坐标系中的坐标，P′_m是第一元素集合C中的第一位置点投影到地图上的位置结果，T_mbk是k时刻车辆在地图中的位姿，T_bkck是车辆车身坐标系到相机坐标系的固定外参关系，通过标定算法事先标定得到。

步骤3：通过步骤2可以计算在k时刻地图坐标系中的第二位置点P_m和投影到地图上的位置结果P′_m的误差，记为在位置结果足够准确的情况下，该误差应该为0。将当前时刻观测到的所有第一元素集合C投影至地图坐标系，并计算所有点的误差和，之后可以通过非线性优化方法，在最小化该误差和的同时，估计出车辆在k时刻的准确位姿x_k，y_k，θ_k，如下式所示：

其中S为所有匹配的语义元素像素点集合，通过非线性优化的方法最小化该误差和，可以迭代求解得到该时刻车辆的最优位姿解：

步骤4：基于步骤3中的准确位姿，在两个时刻k和k+1期间，使用航位推算定位模块计算车辆的位置变化，得到k+1时刻车辆的粗略位姿：x_k+1，y_k+1，θ_k+1。

步骤5：重复执行步骤2和步骤3，可以计算出车辆在k+1时刻的准确位姿

以此类推，重复步骤2-5，可以执行后续时刻的递推定位。

在本申请的实施例中，在上述步骤2中，当k为第一次定位时，需要一个初始定位的结果，可记为T_mb1，该初始定位结果可由GNSS定位模块中的GNSS数据对齐到地图上的位姿提供，如以下关于图2所描述的。

图2解说了根据本发明的一实施例的用于操作语义GNSS定位模块的过程流的示例。

在本申请的实施例中，如图2中所示，首先在GNSS定位模块中，通过GNSS数据获取车辆当前经纬度信息，随后将车辆当前经纬度信息经墨卡托投影变换，转换成笛卡尔平面坐标系结果，再经转换矩阵T_g投影至地图坐标系，得到车辆在地图坐标系下的位姿。该转换矩阵T_g记录了将墨卡托投影后笛卡尔平面坐标系下的GNSS数据转换至高精地图坐标系的转换关系，是大小为4x4的矩阵。

由于建图时GNSS信息无法确保全局高精度解状态且建图时无法全局准确一致地添加GNSS约束，因此，建立的高精地图和GNSS数据经转换矩阵后通常无法完全对齐，而GNSS定位模块在该阶段会存在定位误差，导致GNSS定位结果无法直接用于多传感器融合模块，该误差需经多传感器融合定位模块作实时的动态补偿。

图3解说了根据本发明的一实施例的用于基于高精地图的多传感器融合定位的方法的示例。

在本申请的实施例中，一般而言，多传感器融合定位模块使用扩展卡尔曼滤波器。本发明中，该滤波器的待估计状态量包括：

x，y，θ，s_w，s_v，x^G，y^G，θ^G

其中，第一状态量集合x，y，θ为车辆当前时刻在地图坐标系下的位姿；针对第二状态量集合s_w，s_v，在航位推算模块通过惯性测量单元(IMU)航向角速度积分的航向角也存在一定的误差，s_w为该误差的比例系数，同样，在航位推算阶段通过轮速脉冲计算车辆速度时会存在一定的误差，s_v为该误差的比例系数；第三状态量集合x^G，y^G，θ^G为上述GNSS定位模块转换矩阵T_g的转换误差。

在本申请的实施例中，该滤波器在工作过程中，会实时估计出车辆当前时刻的最优位姿(例如，x，y，θ)、航位推算的误差比例系数(例如，s_w，s_v)、以及GNSS经笛卡尔坐标系投影至高精地图坐标系的转换矩阵误差(例如，x^G，y^G，θ^G)。上述误差被估计后，会分别反馈给下一个输入的航位推算结果以及GNSS投影至高精地图的投影结果，以使得下一次滤波器的预测和观测输入更准确。具体步骤如下：

步骤一：通过语义定位模块结果，初始化滤波器状态量；

步骤二：通过航位推算模块使用式(3)结合图1中所描述的方法步骤2-5递推下一时刻的车辆位姿

步骤三：在递推期间，当语义定位模块有输出结果时，作为滤波器观测，使用状态转移矩阵F和第一观测矩阵H₁更新滤波器的状态估计值，如以下更详细描述的；

步骤四：在递推期间，当GNSS定位模块有输出结果时，作为滤波器观测，使用状态转移矩阵F和第二观测矩阵H₂更新滤波器的状态估计值，如以下更详细描述的。

在扩展卡尔曼滤波器中，需要求解状态转移矩阵和观测矩阵，在本发明的滤波器系统中，存在两种滤波器观测，因此需分别求解两种观测的观测矩阵。

本发明的滤波器的运动方程如下，记为式(4)：

其中，x_k,y_k,θ_k,s_wk,s_vk,为k时刻滤波器的状态量，并且x_k+1,y_k+1,θ_k+1,为k+1时刻滤波器的状态量，v_x,v_y,w_z为航位推算输出的车辆速度和角速度。在本申请实施例中，车辆只有前向速度，因此v_y为0，并且Δt为k+1时刻和k时刻的时间差。

通过上述运动方程式(4)，可求得状态转移矩阵，记为F：

语义定位模块的输出是车辆经语义定位解算的精确位姿x,y,θ。本发明的滤波器在使用语义定位的观测时，认为该观测无误差，可直接输入滤波器。同时，该观测输出的车辆位姿结果形式和滤波器的状态估计中的位姿部分形式相同，因此，该观测的第一观测矩阵H₁如下：

GNSS定位模块输出的笛卡尔平面坐标系经转换矩阵投影至高精地图后，存在误差，因此，在计算GNSS定位模块的观测矩阵时，推导如下：

θ′＝θ^G+θ

其中，x′，y′，θ′是GNSS投影到高精地图坐标系的观测位姿，x，y，θ是待估计滤波器状态量中的车辆位姿，x^G，y^G，θ^G是待估计滤波器状态量中的GNSS转换矩阵投影误差。

通过上式，可求得GNSS观测对应的第二观测矩阵H₂如下：

得到以上的F矩阵和H矩阵后，便可以使用扩展卡尔曼滤波器的预测和观测来更新公式，实现状态量的最优估计。当观测为语义定位时，H矩阵使用第一观测矩阵H₁，当观测为GNSS投影匹配定位时，H矩阵使用第二观测矩阵H₂。在本申请的实施例中，状态估计量包括了当前车辆的位姿、航位推算的误差比例系数以及GNSS转换矩阵的投影误差。因此在估计完当前时刻的最优状态后，航位推算的误差比例系数以及GNSS转换矩阵的投影误差值会作为补偿量，修正下一次滤波器的输入。具体地，航位推算的误差比例系数修正航位推算模块的输出结果，GNSS转换矩阵的投影误差值修正GNSS投影观测输入，以使得滤波器系统的输入输出更稳定。

图4解说了根据本发明的一实施例的用于基于高精地图的多传感器融合定位的另一方法的示例。

在本申请的实施例中，处理上述列举的几种传感器外，在融合定位中还可以外接更多的传感器作为观测传感器。如图4中所示，存在激光雷达或者超声传感器，可以使用激光定位结果或超声定位结果作为滤波器的附加观测输入。基于此，本申请的用于基于高精地图的多传感器融合定位具有可扩展性强的优势。应理解，虽然本申请图4中仅描绘了激光定位或超声定位，也可以使用本领域技术人员所了解的其他定位技术，而不会脱离本申请的范围。同时，虽然本申请的各实施例中仅解说了车辆，但本申请同样适用于其他交通工具的基于高精地图的多传感器融合定位。

图5解说了根据本发明的一实施例的用于基于高精地图的多传感器融合定位的示例方法的流程图。

如图5中所示，方法500包括：在步骤510，根据从第一传感器获取的与车辆相关联的图像来提取第一元素集合。在本申请的实施例中，用于基于高精地图的多传感器融合定位的车辆可以具有用于检测周围环境中的目标并且测量到那些目标对象的距离的各种类型的摄像头、雷达传感器(RADAR)、激光雷达(LIDAR)等等。还可存在各种车轮传感器和传动系传感器，诸如轮胎压力传感器、加速度计、陀螺仪和车轮旋转检测和/或计数器。在一实施例中，经由各种传感器(诸如LIDAR、RADAR、相机、GNSS和SONAR)确定的距离测量和相对位置可以与汽车大小和形状信息以及关于传感器位置的信息相结合，以确定不同车辆、障碍物的表面之间的距离和相对位置，以使得从传感器到另一车辆或两个不同传感器之间的距离或向量递增式地增大以计算传感器在每个车辆上的定位。自动驾驶系统中的摄像头和雷达通常与车辆的中央计算机连接，通过算法和模型来分析和解释从传感器收集到的数据。这些算法和模型能够识别和分类不同类型的对象，如车辆、行人、障碍物等，并计算它们与车辆的距离和相对速度。在自动驾驶过程中，摄像头和雷达的数据被用于生成车辆周围环境的三维模型，以及识别障碍物、参考线等。通过分析这些数据，自动驾驶系统能够获取与当前时刻行驶区域附近的障碍物相关联的信息以及与参考线相关联的信息。例如，可以通过车辆搭载的相机，实时获取(例如，拍摄、采集)车身周围场景图像，使用深度学习算法，在图像中提取语义元素。语义元素类型为：道路中线、减速带、方向箭头、道路边缘、斑马线、停车位等。优选地，相机图像提取的语义元素可以被记为第一元素集合。

进一步，方法500包括：在步骤520，至少部分地基于所述第一元素集合与从所述高精地图中提取的第二元素集合的语义特征匹配来执行所述车辆的第一定位。优选地，所述第一元素集合与所述第二元素集合的所述语义特征匹配包括：获取第二元素集合中在地图坐标系中的一个或多个第二位置点；获取第一元素集合中与所述一个或多个第二位置点中的每个第二位置点相对应的、在相机坐标系中的一个或多个第一位置点；确定将所述一个或多个第一位置点投影到地图坐标系中的一个或多个位置结果；计算在指定时刻在地图坐标系中的所述一个或多个第二位置点中的每个第二位置点与所述一个或多个位置结果中的相应位置结果之间的一个或多个误差；以及至少部分地基于所述一个或多个误差之和来确定所述车辆在所述指定时刻在地图坐标系中的第一位姿，如参考图1所描述的。优选地，执行所述车辆的第一定位包括：至少部分地基于所述车辆在所述指定时刻在地图坐标系中的第一位姿来推导所述车辆在所述指定时刻之后的在地图坐标系中的未来位姿。

进一步，方法500包括：在步骤530，根据从第二传感器获取的定位数据来执行所述车辆的第二定位。例如，可以根据从全球卫星导航系统GNSS定位模块获取的GNSS数据来推导所述车辆的当前经纬度信息；将所述当前经纬度信息经墨卡托投影变换转换成笛卡尔平面坐标系结果；以及经转换矩阵T_g投影至地图坐标系以确定所述车辆在地图坐标系中的第二位姿，如参考图2所描述的。

进一步，方法500包括：在步骤540，至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果。优选地，至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果包括：使用扩展卡尔曼滤波器来融合所述第一位姿和所述第二位姿，如参考图3所描述的。优选地，可以响应于语义定位模块有输出结果，作为滤波器观测，使用第一观测矩阵H₁更新所述扩展卡尔曼滤波器的状态估计值，并且响应于GNSS定位模块有输出结果，作为滤波器观测，使用第二观测矩阵H₂更新所述扩展卡尔曼滤波器的状态估计值。优选地，在确定所述多传感器融合定位结果后，将航位推算的误差比例系数以及GNSS转换矩阵的投影误差值作为补偿量来修正所述扩展卡尔曼滤波器的下一输入。优选地，可以使用航位推算的误差比例系数来修正航位推算模块的输出结果；以及使用GNSS转换矩阵的所述投影误差值来修正GNSS投影观测输入。优选地，还可以使用激光定位结果或超声定位结果作为所述扩展卡尔曼滤波器的滤波器观测。

图6是根据根据本发明的一实施例的用于基于高精地图的多传感器融合定位的示例装置的示图。

替换实施例可以通过组合、分离或以其他方式变化在图6中解说的框中描述的功能性来变化功能。图6的装置600解说了根据一实施例的可如何实现以上所描述的(例如，关于图1至图5)的车辆或其他交通工具的功能性。由此，用于执行图6中所解说的一个或多个框的功能性的装置可以包括车辆的硬件和/或软件组件，该车辆(如先前在图1至图5中所提及的车辆或其他交通工具)可包括在以上所描述的设备的一个或多个组件。

在框610，该功能性包括根据从第一传感器获取的与车辆相关联的图像来提取第一元素集合。优选地，可以通过车辆搭载的相机，实时获取(例如，拍摄、采集)车身周围场景图像，使用深度学习算法，在图像中提取语义元素。语义元素类型为：道路中线、减速带、方向箭头、道路边缘、斑马线、停车位等。优选地，相机图像提取的语义元素可以被记为第一元素集合。用于执行框610的功能性的提取模块可包括设备的一个或多个软件和/或硬件组件，诸如总线、(诸)处理单元、存储器和/或在设备的其他软件和/或硬件组件。

在框620，该功能性包括至少部分地基于所述第一元素集合与从所述高精地图中提取的第二元素集合的语义特征匹配来执行所述车辆的第一定位。优选地，所述第一元素集合与所述第二元素集合的所述语义特征匹配包括：获取第二元素集合中在地图坐标系中的一个或多个第二位置点；获取第一元素集合中与所述一个或多个第二位置点中的每个第二位置点相对应的、在相机坐标系中的一个或多个第一位置点；确定将所述一个或多个第一位置点投影到地图坐标系中的一个或多个位置结果；计算在指定时刻在地图坐标系中的所述一个或多个第二位置点中的每个第二位置点与所述一个或多个位置结果中的相应位置结果之间的一个或多个误差；以及至少部分地基于所述一个或多个误差之和来确定所述车辆在所述指定时刻在地图坐标系中的第一位姿，如参考图1所描述的。用于执行框620的功能性的第一定位模块包括设备的一个或多个软件和/或硬件组件，诸如总线、(诸)处理单元、存储器和/或在设备的其他软件和/或硬件组件。

在框630，该功能性包括根据从第二传感器获取的定位数据来执行所述车辆的第二定位。优选地，可以根据从全球卫星导航系统GNSS定位模块获取的GNSS数据来推导所述车辆的当前经纬度信息；将所述当前经纬度信息经墨卡托投影变换转换成笛卡尔平面坐标系结果；以及经转换矩阵T_g投影至地图坐标系以确定所述车辆在地图坐标系中的第二位姿，如参考图2所描述的。用于执行框630的功能性的第二定位模块可包括设备的一个或多个软件和/或硬件组件，诸如总线、(诸)处理单元、存储器和/或在设备的其他软件和/或硬件组件。

在框640，该功能性包括至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果。优选地，至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果包括：使用扩展卡尔曼滤波器来融合所述第一位姿和所述第二位姿，如参考图3所描述的。优选地，可以响应于语义定位模块有输出结果，作为滤波器观测，使用第一观测矩阵H₁更新所述扩展卡尔曼滤波器的状态估计值，并且响应于GNSS定位模块有输出结果，作为滤波器观测，使用第二观测矩阵H₂更新所述扩展卡尔曼滤波器的状态估计值。优选地，在确定所述多传感器融合定位结果后，将航位推算的误差比例系数以及GNSS转换矩阵的投影误差值作为补偿量来修正所述扩展卡尔曼滤波器的下一输入。优选地，可以使用航位推算的误差比例系数来修正航位推算模块的输出结果；以及使用GNSS转换矩阵的所述投影误差值来修正GNSS投影观测输入。优选地，还可以使用激光定位结果或超声定位结果作为所述扩展卡尔曼滤波器的滤波器观测。用于执行框640的功能性的融合模块可包括设备的一个或多个软件和/或硬件组件，诸如总线、(诸)处理单元、存储器和/或在设备的其他软件和/或硬件组件。

而且，本申请的实施例还公开了一种包括存储于其上的计算机可执行指令的计算机可读存储介质，计算机可执行指令在被处理器执行时使得处理器执行本文的各实施例的方法。

此外，本申请的实施例还公开了一种装置，该装置包括处理器以及存储有计算机可执行指令的存储器，计算机可执行指令在被处理器执行时使得处理器执行本文的各实施例的方法。

此外，本申请的实施例还公开了一种用于基于高精地图的多传感器融合定位的设备，该设备包括：用于根据从第一传感器获取的与车辆相关联的图像来提取第一元素集合的装置；用于至少部分地基于所述第一元素集合与从所述高精地图中提取的第二元素集合的语义特征匹配来执行所述车辆的第一定位的装置；用于根据从第二传感器获取的定位数据来执行所述车辆的第二定位的装置；以及用于至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果的装置。

此外，本申请的实施例还公开了一种车辆，在车辆中运行存储的程序，其中在这些程序运行时控制车辆的处理器中执行本文的各实施例的用于基于高精地图的多传感器融合定位的方法。

以上描述了根据本发明的基于高精地图的多传感器融合定位的方法、装置和介质，相对现有技术而言，本发明至少具有以下优点：

(1)使用车身相机提取场景语义特征与高精地图做匹配定位，并结合GNSS数据，可在提供高精度的定位的同时动态对齐GNSS数据和高精地图的匹配结果，以此来平滑语义定位结果产生的跳动，补偿GNSS传感器的观测误差，还可以补偿GNSS匹配定位的转换投影矩阵误差和航位推算求解的相对位姿误差，使滤波器的状态估计更准确、更稳定；

(2)可以在室内外场景、不同时间段内使用，在GNSS信号丢失、语义定位元素信息缺失时也可以运行，从而可实现通过低成本、低算力实现全天候、全场景、高精度定位，使得系统应用场景更丰富、算法更鲁棒；

(3)其他递推传感器、观测传感器等等可以直接接入该系统，系统具有可扩展性。

在整个说明书中已经参照“实施例”，意味着特定描述的特征、结构或特性包括在至少一个实施例中。因此，这些短语的使用可以不仅仅指代一个实施例。此外，所描述的特征、结构或特性可以在一个或多个实施例中以任何合适的方式组合。

以上描述的方法和装置的各个步骤和模块可以用硬件、软件、或其组合来实现。如果在硬件中实现，结合本公开描述的各种说明性步骤、模块、以及电路可用通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)、或其他可编程逻辑组件、硬件组件、或其任何组合来实现或执行。通用处理器可以是处理器、微处理器、控制器、微控制器、或状态机等。如果在软件中实现，则结合本公开描述的各种说明性步骤、模块可以作为一条或多条指令或代码存储在计算机可读介质上或进行传送。实现本公开的各种操作的软件模块可驻留在存储介质中，如RAM、闪存、ROM、EPROM、EEPROM、寄存器、硬盘、可移动盘、CD-ROM、云存储等。存储介质可耦合到处理器以使得该处理器能从/向该存储介质读写信息，并执行相应的程序模块以实现本公开的各个步骤。而且，基于软件的实施例可以通过适当的通信手段被上载、下载或远程地访问。这种适当的通信手段包括例如互联网、万维网、内联网、软件应用、电缆(包括光纤电缆)、磁通信、电磁通信(包括RF微波和红外通信)、电子通信或者其他这样的通信手段。

在各实施例中给出的数值仅作为示例，而不作为对本发明范围的限制。此外，作为一个整体技术方案，还存在其他没有被本发明权利要求或说明书所列举的元器件或者步骤。而且，一个元器件的单个名称不排除该元器件的其他名称。

还应注意，这些实施例可能是作为被描绘为流程图、流图、结构图、或框图的过程来描述的。尽管流程图可能会把诸操作描述为顺序过程，但是这些操作中有许多操作能够并行或并发地执行。另外，这些操作的次序可被重新安排。

所公开的方法、装置和系统不应以任何方式被限制。相反，本公开涵盖各种所公开的实施例(单独和彼此的各种组合和子组合)的所有新颖和非显而易见的特征和方面。所公开的方法、装置和系统不限于任何具体方面或特征或它们的组合，所公开的任何实施例也不要求存在任一个或多个具体优点或者解决特定或所有技术问题。

本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护范围之内。

相关领域的技术人员可以认识到，可以在没有一个或多个具体细节的情况下或者利用其他方法、资源、材料等来实践这些实施例。在其他情况下，众所周知的结构、资源，或者仅仅为了观察实施例的模糊方面而未详细示出或描述操作。

虽然已经说明和描述了实施例和应用，但是应该理解，实施例不限于上述精确配置和资源。在不脱离所要求保护的实施例的范围的情况下，可以在本文公开的方法和系统的布置，操作和细节中进行对本领域技术人员显而易见的各种修改、替换和改进。

如本文中所使用的术语“和”、“或”以及“和/或”可包括还预期至少部分地取决于使用此类术语的上下文的各种含义。通常，“或”如果被用于关联一列表，诸如A、B或C，则旨在表示A、B和C(这里使用的是包含性的含义)以及A、B或C(这里使用的是排他性的含义)。另外，本文所使用的术语“一个或多个”可用于描述单数形式的任何特征、结构或特性，或者可用于描述多个特征、结构或特征或其某种其他组合。但是，应注意，这仅是说明性示例，并且所要求保护的主题内容不限于此示例。

虽然已经解说并描述了目前被认为是示例特征的内容，但是本领域技术人员将理解，在不脱离所要求保护的主题的情况下，可以进行各种其他修改，并且可以替换等同物。附加地，可以作出许多修改以使特定场景适应于要求保护的主题内容的教导，而不脱离本文所描述的中心概念。

Claims (12)

1.一种用于基于高精地图的多传感器融合定位的方法，包括：

根据从第一传感器获取的与车辆相关联的图像来提取第一元素集合；

至少部分地基于所述第一元素集合与从所述高精地图中提取的第二元素集合的语义特征匹配来执行所述车辆的第一定位；

根据从第二传感器获取的定位数据来执行所述车辆的第二定位；以及

至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果。

2.如权利要求1所述的方法，其特征在于，所述第一元素集合与所述第二元素集合的所述语义特征匹配包括：

获取第二元素集合中在地图坐标系中的一个或多个第二位置点；

获取第一元素集合中与所述一个或多个第二位置点中的每个第二位置点相对应的、在相机坐标系中的一个或多个第一位置点；

确定将所述一个或多个第一位置点投影到地图坐标系中的一个或多个位置结果；

计算在指定时刻在地图坐标系中的所述一个或多个第二位置点中的每个第二位置点与所述一个或多个位置结果中的相应位置结果之间的一个或多个误差；以及

至少部分地基于所述一个或多个误差之和来确定所述车辆在所述指定时刻在地图坐标系中的第一位姿。

3.如权利要求2所述的方法，其特征在于，执行所述车辆的第一定位包括：

至少部分地基于所述车辆在所述指定时刻在地图坐标系中的第一位姿来推导所述车辆在所述指定时刻之后的在地图坐标系中的未来位姿。

4.如权利要求3所述的方法，其特征在于，根据从第二传感器获取的定位数据来执行所述车辆的第二定位包括：

根据从全球卫星导航系统GNSS定位模块获取的GNSS数据来推导所述车辆的当前经纬度信息；

将所述当前经纬度信息经墨卡托投影变换转换成笛卡尔平面坐标系结果；以及

经转换矩阵T_g投影至地图坐标系以确定所述车辆在地图坐标系中的第二位姿。

5.如权利要求4所述的方法，其特征在于，至少部分地基于所述第一定位和所述第二定位来获得多传感器融合定位结果包括：

使用扩展卡尔曼滤波器来融合所述第一位姿和所述第二位姿，其中所述扩展卡尔曼滤波器的状态转移矩阵

6.如权利要求5所述的方法，进一步包括：

响应于语义定位模块有输出结果，作为滤波器观测，使用第一观测矩阵H₁更新所述扩展卡尔曼滤波器的状态估计值，其中

以及

响应于GNSS定位模块有输出结果，作为滤波器观测，使用第二观测矩阵H₂更新所述扩展卡尔曼滤波器的状态估计值，其中

7.如权利要求6所述的方法，其特征在于，进一步包括：

在确定所述多传感器融合定位结果后，将航位推算的误差比例系数以及GNSS转换矩阵的投影误差值作为补偿量来修正所述扩展卡尔曼滤波器的下一输入。

8.如权利要求7所述的方法，其特征在于，进一步包括：

使用航位推算的误差比例系数来修正航位推算模块的输出结果；以及

使用GNSS转换矩阵的所述投影误差值来修正GNSS投影观测输入。

9.如权利要求5所述的方法，进一步包括：

使用激光定位结果或超声定位结果作为所述扩展卡尔曼滤波器的滤波器观测。

10.一种用于基于高精地图的多传感器融合定位的装置，包括处理器和存储器，所述存储器存储有程序指令；所述处理器运行程序指令实现如权利要求1至权利要求9中任一项的方法。

11.一种存储指令的非瞬态计算机可读存储介质，所述指令当被计算机执行时，使所述计算机执行如权利要求1至权利要求9中任一项的用于基于高精地图的多传感器融合定位的方法。

12.一种车辆，在所述车辆中运行存储的程序，其中在所述程序运行时控制所述车辆的处理器执行权利要求1至权利要求9中任一项的用于基于高精地图的多传感器融合定位的方法。