CN114252082A

CN114252082A - 车辆定位方法、装置和电子设备

Info

Publication number: CN114252082A
Application number: CN202210188968.6A
Authority: CN
Inventors: 鲍佳立; 张翔; 韩志华; 张旭
Original assignee: Zhitu Shanghai Intelligent Technology Co ltd; Suzhou Zhitu Technology Co Ltd
Current assignee: Zhitu Shanghai Intelligent Technology Co ltd; Suzhou Zhitu Technology Co Ltd
Priority date: 2022-03-01
Filing date: 2022-03-01
Publication date: 2022-03-29
Anticipated expiration: 2042-03-01
Also published as: CN114252082B

Abstract

本发明提供了一种车辆定位方法、装置和电子设备，获取摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标、与车辆经纬度坐标关联的多条地图内车道线即对应的地图内车道线经纬度坐标；将每条地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标；基于感知车道线的信息和每条地图内车道线对应的地图内车道线车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线。基于目标地图内车道线，对目标车辆进行定位。该方式可以通过摄像头采集到的感知车道线与地图内车道线，对目标车辆进行定位，降低了对卫星信号的依赖，从而可以提高对目标车辆的定位精度。

Description

车辆定位方法、装置和电子设备

技术领域

本发明涉及自动驾驶技术领域，尤其是涉及一种车辆定位方法、装置和电子设备。

背景技术

随着自动驾驶技术的发展，自动驾驶车辆的运行场景逐步从试验场、空旷场地等简单环境过渡到高速场景等更加复杂的真实环境。在高速场景下，自动驾驶车辆在进行变道或上下匝道时，需要对车辆进行准确定位，相关技术中，通常有相对定位和绝对定位两种定位方式，其中，相对定位通常依靠IMU（Inertial Measurement Unit惯性导航）计算deadreckoning(航位推算)，利用IMU的积分运算提供车辆相对于起点的相对位置，长期运行之后，由于误差的积累无法消除，该方式会导致误差的发散。绝对定位主要依靠GNSS（Global Navigation Satellite System，全球导航卫星系统），特别是RTK(Real-timekinematic，实时差分定位)等技术，均依赖于与导航卫星的通信，以提供车辆相对于地球的经纬度坐标，在结合IMU与RTK时，当车辆驶入隧道等无法接收到卫星信号的环境时，定位仍然只能依靠IMU递推，导致产生位置偏差和航向角偏差，进而导致车辆定位精度较差的问题。

发明内容

本发明的目的在于提供一种车辆定位方法、装置和电子设备，以提高车辆定位精度。

本发明提供的一种车辆定位方法，方法包括：获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标、与车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；其中，地图内车道线经纬度坐标为形成地图内车道线的多个检测点的经纬度坐标的集合；将每条地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标；其中，地图内车道线车体坐标为形成地图内车道线的多个检测点的车体坐标的集合；基于感知车道线的信息和每条地图内车道线对应的地图内车道线车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线；基于目标地图内车道线，对目标车辆进行定位。

进一步的，获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标、与车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标的步骤包括：获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标；基于车辆经纬度坐标，从地图中获取预设范围内的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标。

进一步的，将每条地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标的步骤包括：将车辆经纬度坐标转换为东北天坐标系下的车辆东北天坐标；将地图内车道线经纬度坐标转换为东北天坐标系下的地图内车道线东北天坐标；在东北天坐标系下，将每条地图内车道线对应的地图内车道线东北天坐标投影至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标。

进一步的，基于感知车道线的信息和每条地图内车道线对应的地图内车道线车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线的步骤包括：基于每条地图内车道线对应的地图内车道线车体坐标，确定每条地图内车道线对应的第一拟合曲线；基于感知车道线的信息确定感知车道线对应的第二拟合曲线，对第二拟合曲线进行采样，得到感知车道线上的多个观测点，以及每个观测点的观测点车体坐标；基于第一拟合曲线以及每个观测点的观测点车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线。

进一步的，基于第一拟合曲线以及每个观测点的观测点车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线的步骤包括：针对每条地图内车道线，计算每个观测点距离该地图内车道线的第一拟合曲线的距离；计算多个观测点对应的距离的和，并取均值，得到该地图内车道线对应的均值结果；从多条地图内车道线对应的多个均值结果中确定均值最小结果，将均值最小结果对应的地图内车道线确定为与感知车道线匹配的目标地图内车道线。

进一步的，基于目标地图内车道线，对目标车辆进行定位的步骤包括：获取目标地图内车道线对应的目标拟合曲线；确定每个观测点在目标拟合曲线上的垂点，以及垂点的垂点坐标；针对每个观测点，计算该观测点的观测点坐标与对应的垂点坐标的差值，得到该观测点对应的误差量；其中，误差量对应的函数中包括横向位移偏差量及方向角偏差量；计算误差量对横向位移偏差量及方向角偏差量的导数，得到该观测点对应的雅可比计算结果；基于每个观测点对应的误差量和雅可比计算结果，对目标车辆进行定位。

进一步的，基于每个观测点对应的误差量和雅可比计算结果，对目标车辆进行定位的步骤包括：基于每个观测点对应的误差量和雅可比计算结果，更新每个观测点对应的第一矩阵和第二矩阵；其中，每个观测点对应的更新后的第一矩阵与该观测点对应的雅可比计算结果的平方相关联；每个观测点对应的更新后的第二矩阵与该观测点对应的雅可比计算结果与误差量的乘积相关联；对多个观测点对应的更新后的第一矩阵进行加和，得到第一加和矩阵；对多个观测点对应的更新后的第二矩阵进行加和，得到第二加和矩阵；基于第一加和矩阵和第二加和矩阵，确定目标车辆的定位状态修正量；基于定位状态修正量修正目标车辆的当前定位状态量，基于修正后的当前定位状态量对目标车辆进行定位。

本发明提供的一种车辆定位装置，装置包括：获取模块，用于获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标、与车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；其中，地图内车道线经纬度坐标为形成地图内车道线的多个检测点的经纬度坐标的集合；转换模块，用于将每条地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标；其中，地图内车道线车体坐标为形成地图内车道线的多个检测点的车体坐标的集合；确定模块，用于基于感知车道线的信息和每条地图内车道线对应的地图内车道线车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线；定位模块，用于基于目标地图内车道线，对目标车辆进行定位。

本发明提供的一种电子设备，包括处理器和存储器，存储器存储有能够被处理器执行的机器可执行指令，处理器执行机器可执行指令以实现上述任一项的车辆定位方法。

本发明提供的一种机器可读存储介质，机器可读存储介质存储有机器可执行指令，机器可执行指令在被处理器调用和执行时，机器可执行指令促使处理器实现上述任一项的车辆定位方法。

本发明提供的车辆定位方法、装置和电子设备，获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标、与车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；将每条地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标；基于感知车道线的信息和每条地图内车道线对应的地图内车道线车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线。基于目标地图内车道线，对目标车辆进行定位。该方式可以通过摄像头采集到的感知车道线与地图内车道线，对目标车辆进行定位，降低了对卫星信号的依赖，从而可以提高对目标车辆的定位精度。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种车辆定位方法的流程图；

图2为本发明实施例提供的一种车道线匹配方法的流程图；

图3为本发明实施例提供的另一种车辆定位方法的流程图；

图4为本发明实施例提供的一种车辆定位装置的结构示意图；

图5为本发明实施例提供的一种电子设备的结构示意图。

具体实施方式

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

目前，当需要对自动驾驶车辆进行定位时，通常有相对定位和绝对定位两种定位方式，在利用IMU进行相对定位时，长期运行之后，由于误差的积累无法消除，会导致误差的发散。而结合IMU与RTK时，虽然可以利用全局信息抑制误差发散，但当车辆驶入高架桥下、隧道内等环境时，由于卫星信号的遮挡、折射等问题，对全局定位精度依然会产生较大的影响。具体的，当车辆驶入高架桥下等环境，导致RTK失效时，GNSS误差会增加到米级，导致车辆无法定位到正确的车道内，并产生航向角的偏差。当车辆行驶到隧道内等完全无法接收到GNSS卫星型号时，定位只依靠IMU递推，随车时间推移，会逐渐造成误差累积，产生位置偏差和航向角偏差，定位可靠度下降。基于此，本发明实施例提供了一种车辆定位方法，该技术可以应用于需要对车辆进行定位的场景中。

为便于对本实施例进行理解，首先对本发明实施例所公开的一种车辆定位方法进行详细介绍；如图1所示，该方法包括如下步骤：

步骤S102，获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标、与车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；其中，地图内车道线经纬度坐标为形成地图内车道线的多个检测点的经纬度坐标的集合。

上述感知车道线的信息可以采用智能摄像头作为数据源，检测车体坐标系下的感知车道线的信息，该信息可以包括多个参数，基于多个参数可以确定感知车道线对应的拟合曲线；上述目标车辆的车辆经纬度坐标通常是采用IMU递推出的预测位姿态，该预测位姿态通常包括位置信息和角度信息；上述多条地图内车道线可以是基于车辆经纬度坐标，从地图中获取的预设范围内的车道线；每条地图内车道线通常由多个检测点组成，每个检测点具有其对应的经纬度坐标。

步骤S104，将每条地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标；其中，地图内车道线车体坐标为形成地图内车道线的多个检测点的车体坐标的集合。

在获取到上述多条地图内车道线对应的地图内车道线经纬度坐标后，通常需要将地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标，以便于与感知车道线进行匹配，由于地图内车道线经纬度坐标为形成地图内车道线的多个检测点的经纬度坐标的集合，因此，在转换至车体坐标系后，地图内车道线车体坐标为形成地图内车道线的多个检测点的车体坐标的集合。

步骤S106，基于感知车道线的信息和每条地图内车道线对应的地图内车道线车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线。

当确定每条地图内车道线对应的地图内车道线车体坐标后，可以基于地图内车道线车体坐标，以及感知车道线的信息，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线，比如，可以通过拟合曲线的方式，采用匈牙利匹配算法，确定目标地图内车道线等。

步骤S108，基于目标地图内车道线，对目标车辆进行定位。

上述车辆定位方法，获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标、与车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；将每条地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标；基于感知车道线的信息和每条地图内车道线对应的地图内车道线车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线。基于目标地图内车道线，对目标车辆进行定位。该方式可以通过摄像头采集到的感知车道线与地图内车道线，对目标车辆进行定位，降低了对卫星信号的依赖，从而可以提高对目标车辆的定位精度。

本发明实施例还提供了另一种车辆定位方法，该方法在上述实施例方法的基础上实现；该方法包括如下步骤：

步骤202，获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标。

在实际实现时，通常需要先确定待优化的状态量，具体可以通过以下公式（1）-公式（3）定义，状态量x中包括了车辆的三维位置p和用四元数表示的角度姿态q。

x=[p,q] （1）

p=[p_x，p_y，p_z] （2）

Q=q(ω，x，y，z) （3）

其中，p_x、p_y和p_z为三维位置p分别在x轴方向、y轴方向和z轴方向的坐标值；q表示四元数，公式（3）中的ω、x、y和z用于表示角度姿态q。

当需要对车辆进行定位时，首先获取目标车辆的摄像头采集到的感知车道线的信息，如智能摄像头采集的车道线检测结果（车体坐标系下，xy二维坐标），还需要获取IMU递推出的预测位姿态，该预测位姿态包括：经纬度坐标系下的位置信息和角度信息；其中，该位置信息即为上述目标车辆的车辆经纬度坐标；另外，还需要获取高精度地图（经纬度坐标系）。

步骤204，基于车辆经纬度坐标，从地图中获取预设范围内的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标。

上述预设范围可以根据实际需求进行设置，比如，可以通过预测的车辆经纬度坐标，从高精度地图中获得车辆经纬度坐标周边100m范围内的地图，由于高速地图中车道线距离通常较长，通常一条车道线会被分割成多个线段，因此，利用车道线分割段的头尾两个端点，按照一定阈值，比如0.1m，如果相邻两条车道线分割段的两个端点之间的距离在0.1m内，可以认为这两条车道线分割段属于同一车道线，对属于同一车道线的多条车道线分割段进行拼接，可以得到多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标。

步骤206，将车辆经纬度坐标转换为东北天坐标系下的车辆东北天坐标；将地图内车道线经纬度坐标转换为东北天坐标系下的地图内车道线东北天坐标。

将车辆经纬度坐标根据预先设置的坐标原点，转换到东北天坐标系，如公式（4）所示，具体可以参考相关技术中的转换方式，在此不再赘述。

（4）

其中，P_vehicle表示东北天坐标系下的位置；X_vehicle 和Y_vehicle分别表示目标车辆在东北天坐标系下，x轴方向和y轴方向的坐标值；P_vehicle（longitudinal，lateral）表示目标车辆的车辆经纬度坐标；ENU（East North Up）表示东北天坐标系。

高精度地图中车道线的信息为经纬度坐标，根据设定的坐标原点，将地图内车道线经纬度坐标转换到东北天坐标系。

步骤208，在东北天坐标系下，将每条地图内车道线对应的地图内车道线东北天坐标投影至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标。

在将车辆经纬度坐标和地图内车道线经纬度坐标都转换到东北天坐标系后，可以在东北天坐标系下，通过预测的目标车辆的预测位姿态，将地图内车道线经纬度坐标向车体坐标系进行投影，如公式（5）所示，得到地图内车道线东北天坐标在车体坐标系下的坐标：

（5）

其中，

为车体坐标系与东北天坐标系之间的转换矩阵；R表示目标车辆的角度，t表示目标车辆的位置；X_map和Y_map分别表示地图内车道线上的点在车体坐标系下，x轴方向和y轴方向的坐标值；P_map（longitudinal，lateral）表示地图内车道线上的点的经纬度坐标。

步骤210，基于每条地图内车道线对应的地图内车道线车体坐标，确定每条地图内车道线对应的第一拟合曲线。

在实际实现时，针对每条地图内车道线，由于该地图内车道线对应的地图内车道线车体坐标为形成该地图内车道线的多个检测点的车体坐标的集合，因此，可以对该地图内车道线的多个检测点的车体坐标进行拟合处理，得到该地图内车道线对应的第一拟合曲线，对每条地图内车道线分别进行拟合处理，可以得到每条地图内车道线各自对应的第一拟合曲线。

步骤212，基于感知车道线的信息确定感知车道线对应的第二拟合曲线，对第二拟合曲线进行采样，得到感知车道线上的多个观测点，以及每个观测点的观测点车体坐标。

在实际实现时，获取到的感知车道线的信息通常包括多个参数，基于多个参数可以确定感知车道线对应的拟合曲线，比如，感知车道线的信息包括a、b、c、d和e五个参数，基于这五个参数可以确定第二拟合曲线为四次曲线y=ax+bx²+cx³+dx⁴+e。对该第二拟合曲线按预设的纵向采样间隔进行采样，如按0.5米的纵向采样间隔进行采样，可以得到多个观测点的集合，如公式（6）所示：

（6）

其中，P_obs表示感知车道线上的观测点；X_obs和Y_obs分别表示感知车道线上的观测点在x轴方向和y轴方向的坐标值。

步骤214，基于第一拟合曲线以及每个观测点的观测点车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线。

该步骤214可以通过下述步骤一至步骤三实现：

步骤一，针对每条地图内车道线，计算每个观测点距离该地图内车道线的第一拟合曲线的距离。

步骤二，计算多个观测点对应的距离的和，并取均值，得到该地图内车道线对应的均值结果。

步骤三，从多条地图内车道线对应的多个均值结果中确定均值最小结果，将均值最小结果对应的地图内车道线确定为与感知车道线匹配的目标地图内车道线。

在实际实现时，可以通过带权重的匈牙利算法，将智能摄像头检测的感知车道线与地图内车道线形成最佳匹配对。其中，权重通过智能摄像头检测的感知车道线上的观测点到地图内车道线的距离d来计算，如公式（7）所示：

（7）

其中x₀，y₀分别代表智能摄像头检测到的感知车道线上的观测点在x轴方向和y轴方向的坐标值；A，B，C代表地图内车道线对应的拟合曲线的系数。

具体实现时，可以针对每条地图内车道线，从预先采样得到的每个观测点向该地图内车道线的第一拟合曲线作垂线，以计算每个观测点距离该地图内车道线的第一拟合曲线的距离，然后计算多个观测点对应的距离的和，并取均值，得到均值结果，通常均值结果越小，相应的地图内车道线对应的权重越大，该地图内车道线与感知车道线的匹配度越高，最后权重最大的地图内车道线即为最匹配的目标地图内车道线。

步骤216，获取目标地图内车道线对应的目标拟合曲线。

在确定与感知车道线最匹配的目标地图内车道线后，可以继续对目标车辆的位姿进行优化，优化算法的输入有两项，分别是惯导数据（在经纬度坐标系下，预先预测的目标车辆的位置和姿态数据）和上述过程所确定的车道线匹配数据（如感知车道线a与地图内车道线甲匹配等）。优化算法基于高斯牛顿法，即计算感知车道线和车体坐标系下地图内车道线的误差量e，再寻找能够最小化该误差的状态x，从而修正目标车辆的位姿。

通常需要先定义状态量x的时间递推，具体可以通过惯导数据IMU和上一时刻的位置数据x_t-1，预测车辆在当前时刻的位置数据x_t，如公式（8）所示，即按照前一个时刻的状态推算当前时刻的状态。

x_t=f(x_t-1，imu) （8）

具体可以参考相关技术中的递推过程，在此不再赘述。

步骤218，确定每个观测点在目标拟合曲线上的垂点，以及垂点的垂点坐标。

步骤220，针对每个观测点，计算该观测点的观测点坐标与对应的垂点坐标的差值，得到该观测点对应的误差量；其中，误差量对应的函数中包括横向位移偏差量及方向角偏差量。

利用车辆状态和视觉匹配对中车道线上的每一个点，计算车辆在横向和方向角的误差量e，如公式（9）所示：

（9）

其中，P_obs为感知车道线上采样得到的观测点；X_obs和Y_obs分别为观测点在X轴方向和Y轴方向的坐标；P_map表示地图内车道线上，与观测点对应的垂点；X_map和Y_map分别为垂点在X轴方向和Y轴方向的坐标。

步骤222，计算误差量对横向位移偏差量及方向角偏差量的导数，得到该观测点对应的雅可比计算结果。

为了计算状态量的修正方向，需要计算误差对横向位移偏差量t_x和方向角偏差量θ的导数，即雅可比函数J。基于匹配对中的感知车道线上的每一个观测点，都可以求出每个观测点对应的雅可比函数J。计算方式如公式（10）所示，通过链式求导，将雅可比函数J分解为两部分：1）误差对目标地图内车道线上的检测点的求导；2）目标地图内车道线上的检测点对横向位移偏差量t_x和方向角偏差量θ的求导：

（10）

通过误差量的公式（9），容易得到误差对于目标地图内车道线上的检测点的导数，如公式（11）所示:

（11）

目标地图内车道线上的检测点对于横向位移偏移量t_x和方向角偏移量θ的求导可以按照公式（12）求得：

（12）

其中，T表示目标车辆的位姿（包括R和t）；p表示目标地图内车道线上的点在东北天坐标系中的位置；R表示角度；t表示位移偏差量，t_x表示横向位移偏差量，t_y表示纵向位移偏差量；

表示方向角偏差量。

对θ和t_x进行求导可以确定让误差量e最快缩小的方向，把东北天坐标系下车道线上的点投影到车体坐标系。如果车辆定位有问题，把地图车道线投影到车体的结果会与感知车道线有较大差别，根据误差，调整车辆定位误差。

步骤224，基于每个观测点对应的误差量和雅可比计算结果，对目标车辆进行定位。

该步骤224可以通过步骤五至步骤九实现：

步骤五，基于每个观测点对应的误差量和雅可比计算结果，更新每个观测点对应的第一矩阵和第二矩阵；其中，每个观测点对应的更新后的第一矩阵与该观测点对应的雅可比计算结果的平方相关联；每个观测点对应的更新后的第二矩阵与该观测点对应的雅可比计算结果与误差量的乘积相关联。

对于匹配对中的每个匹配点，通过迭代，利用上述过程中计算出的每个观测点对应的雅可比计算结果和误差量，更新H和b，如公式（13）和公式（14）所示：

H=H+J*J （13）

b=b+(-J)*e （14）

其中，H为海森矩阵。

通过上述公式（13）和公式（14），可以看出，对于每个观测点来说，当得到观测点对应的误差量和雅可比计算结果后，可以通过计算雅可比计算结果的平方加上当前的第一矩阵，得到更新后的第一矩阵，即更新后的H，将雅可比计算结果取反后与误差量相乘得到的结果，加上当前的第二矩阵，得到更新后的第二矩阵，即更新后的b。

步骤六，对多个观测点对应的更新后的第一矩阵进行加和，得到第一加和矩阵。

步骤七，对多个观测点对应的更新后的第二矩阵进行加和，得到第二加和矩阵。

步骤八，基于第一加和矩阵和第二加和矩阵，确定目标车辆的定位状态修正量。

利用计算得到的每个观测点对应的更新后的第一矩阵和更新后的第二矩阵，根据高斯牛顿法，可以计算得到一个能最小化误差的状态变化量△x，如公式（15）所示，具体可以计算每个观测点对应的更新后的第一矩阵的和，以及计算每个观测点对应的更新后的第二矩阵的和，得到第一加和矩阵和第二加和矩阵，即加和之后的H和加和之后的b，将加和之后的H和加和之后的b代入下述公式15，可以得到△x。

（15）

步骤九，基于定位状态修正量修正目标车辆的当前定位状态量，基于修正后的当前定位状态量对目标车辆进行定位。

得到△x之后，更新状态量x，如公式（16）所示

（16）

系统的数据即修正后的x，根据修正后的状态量x可以对目标车辆进行精确定位。

上述车辆定位方法，通过引入高速道路的高精度地图，结合视觉技术，对定位结果进行优化在RTK失效时，依然提供可靠的定位结果。可以提高车辆定位的鲁棒性，实现在RTK信号受到影响的情况下进行车辆定位，当导致RTK、GNSS误差扩大时，通过视觉与地图修正，控制车辆横向大幅度偏差与航向角突变。

为进一步理解上述实施例，下面提供如图2所示的一种车道线匹配方法的流程图，输入包括智能摄像头数据、高精度地图和状态预测值，以智能摄像头作为数据源，进行车道线检测，得到感知车道线，根据状态预测值中的预测位置，确定预测位置邻域，从高精度地图中确定预测位置邻域内的车道线信息，将车道线信息向车体系投影，得到投影结果，对感知车道线和投影结果通过匈牙利匹配算法，确定最佳车道线匹配对。

如图3所示的另一种车辆定位方法的流程图，获取车道线匹配对中的车道线检测（对应感知车道线）和地图车道线（对应目标地图内车道线），根据车道线检测确定车道线直线模型、惯导得到的状态预测值（包括位置和姿态），对地图车道线进行车体系投影，得到投影结果，基于车道线直线模型和投影结果计算雅可比，根据雅可比计算结果计算状态增量，基于状态增量对状态预测值进行修正，根据修正后的状态预测值对目标车辆进行定位。通过图2中的匹配过程可以将地图中的车道线信息与摄像头中的车道线信息进行匹配。通过图3中的定位方式可以对车辆位姿进行优化。通过匹配算法，可以确定感知数据中的车道线和地图中车道线的对应关系。在建立了正确的匹配关系后，就可以计算地图车道线与感知车道线之间的误差。

上述方式可以在卫星信号无法获得时，对车辆横向位置进行修正，具体的，在没有卫星信号时，通过车道线检测和地图数据，针对IMU递推中积累的横向误差进行修正，保持车辆在正确的车道内，对航向角误差修正，避免航向角误差的发散。该方式还充分利用了没有高程的地图信息，在地图中不含有高程的情况下，利用二维的检测数据和二维的地图信息，进行修正。相比于一般的基于惯导和GNSS的定位方法，本发明充分利用了高精度地图中的车道线信息和二维车道线检测结果，在RTK失效的情况下，起到修正定位结果，防止误差扩散的效果。

本发明实施例提供了一种车辆定位装置，如图4所示，该装置包括：获取模块40，用于获取目标车辆的摄像头采集到的感知车道线的信息、所述目标车辆的车辆经纬度坐标、与所述车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；其中，所述地图内车道线经纬度坐标为形成所述地图内车道线的多个检测点的经纬度坐标的集合；转换模块41，用于将每条所述地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条所述地图内车道线对应的地图内车道线车体坐标；其中，所述地图内车道线车体坐标为形成所述地图内车道线的多个检测点的车体坐标的集合；确定模块42，用于基于所述感知车道线的信息和每条所述地图内车道线对应的所述地图内车道线车体坐标，从多条所述地图内车道线中，确定与所述感知车道线匹配的目标地图内车道线；定位模块43，用于基于所述目标地图内车道线，对所述目标车辆进行定位。

上述车辆定位装置，获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标、与车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；将每条地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标；基于感知车道线的信息和每条地图内车道线对应的地图内车道线车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线。基于目标地图内车道线，对目标车辆进行定位。该装置可以通过摄像头采集到的感知车道线与地图内车道线，对目标车辆进行定位，降低了对卫星信号的依赖，从而可以提高对目标车辆的定位精度。

进一步的，获取模块还用于：获取目标车辆的摄像头采集到的感知车道线的信息、目标车辆的车辆经纬度坐标；基于车辆经纬度坐标，从地图中获取预设范围内的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标。

进一步的，转换模块还用于：将车辆经纬度坐标转换为东北天坐标系下的车辆东北天坐标；将地图内车道线经纬度坐标转换为东北天坐标系下的地图内车道线东北天坐标；在东北天坐标系下，将每条地图内车道线对应的地图内车道线东北天坐标投影至车体坐标系，得到每条地图内车道线对应的地图内车道线车体坐标。

进一步的，确定模块还用于：基于每条地图内车道线对应的地图内车道线车体坐标，确定每条地图内车道线对应的第一拟合曲线；基于感知车道线的信息确定感知车道线对应的第二拟合曲线，对第二拟合曲线进行采样，得到感知车道线上的多个观测点，以及每个观测点的观测点车体坐标；基于第一拟合曲线以及每个观测点的观测点车体坐标，从多条地图内车道线中，确定与感知车道线匹配的目标地图内车道线。

进一步的，确定模块还用于：针对每条地图内车道线，计算每个观测点距离该地图内车道线的第一拟合曲线的距离；计算多个观测点对应的距离的和，并取均值，得到该地图内车道线对应的均值结果；从多条地图内车道线对应的多个均值结果中确定均值最小结果，将均值最小结果对应的地图内车道线确定为与感知车道线匹配的目标地图内车道线。

进一步的，定位模块还用于：获取目标地图内车道线对应的目标拟合曲线；确定每个观测点在目标拟合曲线上的垂点，以及垂点的垂点坐标；针对每个观测点，计算该观测点的观测点坐标与对应的垂点坐标的差值，得到该观测点对应的误差量；其中，误差量对应的函数中包括横向位移偏差量及方向角偏差量；计算误差量对横向位移偏差量及方向角偏差量的导数，得到该观测点对应的雅可比计算结果；基于每个观测点对应的误差量和雅可比计算结果，对目标车辆进行定位。

进一步的，定位模块还用于：基于每个观测点对应的误差量和雅可比计算结果，更新每个观测点对应的第一矩阵和第二矩阵；其中，每个观测点对应的更新后的第一矩阵与该观测点对应的雅可比计算结果的平方相关联；每个观测点对应的更新后的第二矩阵与该观测点对应的雅可比计算结果与误差量的乘积相关联；对多个观测点对应的更新后的第一矩阵进行加和，得到第一加和矩阵；对多个观测点对应的更新后的第二矩阵进行加和，得到第二加和矩阵；基于第一加和矩阵和第二加和矩阵，确定目标车辆的定位状态修正量；基于定位状态修正量修正目标车辆的当前定位状态量，基于修正后的当前定位状态量对目标车辆进行定位。

本发明实施例所提供的车辆定位装置，其实现原理及产生的技术效果和前述车辆定位方法实施例相同，为简要描述，车辆定位装置实施例部分未提及之处，可参考前述车辆定位方法实施例中相应内容。

本发明实施例还提供了一种电子设备，参见图5所示，该电子设备包括处理器130和存储器131，该存储器131存储有能够被处理器130执行的机器可执行指令，该处理器130执行机器可执行指令以实现上述车辆定位方法。

进一步地，图5所示的电子设备还包括总线132和通信接口133，处理器130、通信接口133和存储器131通过总线132连接。

其中，存储器131可能包含高速随机存取存储器（RAM，Random Access Memory），也可能还包括非不稳定的存储器（non-volatile memory），例如至少一个磁盘存储器。通过至少一个通信接口133（可以是有线或者无线）实现该系统网元与至少一个其他网元之间的通信连接，可以使用互联网，广域网，本地网，城域网等。总线132可以是ISA总线、PCI总线或EISA总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示，图5中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。

处理器130可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器130中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器130可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(DigitalSignal Processor，简称DSP)、专用集成电路(Application Specific IntegratedCircuit，简称ASIC)、现场可编程门阵列(Field-Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器131，处理器130读取存储器131中的信息，结合其硬件完成前述实施例的方法的步骤。

本发明实施例还提供了一种机器可读存储介质，该机器可读存储介质存储有机器可执行指令，该机器可执行指令在被处理器调用和执行时，该机器可执行指令促使处理器实现上述车辆定位方法，具体实现可参见方法实施例，在此不再赘述。

本发明实施例所提供的车辆定位方法、装置和电子设备的计算机程序产品，包括存储了程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见方法实施例，在此不再赘述。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims

1.一种车辆定位方法，其特征在于，所述方法包括：

获取目标车辆的摄像头采集到的感知车道线的信息、所述目标车辆的车辆经纬度坐标、与所述车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；其中，所述地图内车道线经纬度坐标为形成所述地图内车道线的多个检测点的经纬度坐标的集合；

将每条所述地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条所述地图内车道线对应的地图内车道线车体坐标；其中，所述地图内车道线车体坐标为形成所述地图内车道线的多个检测点的车体坐标的集合；

基于所述感知车道线的信息和每条所述地图内车道线对应的所述地图内车道线车体坐标，从多条所述地图内车道线中，确定与所述感知车道线匹配的目标地图内车道线；

基于所述目标地图内车道线，对所述目标车辆进行定位。

2.根据权利要求1所述的方法，其特征在于，所述获取目标车辆的摄像头采集到的感知车道线的信息、所述目标车辆的车辆经纬度坐标、与所述车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标的步骤包括：

获取所述目标车辆的摄像头采集到的感知车道线的信息、所述目标车辆的车辆经纬度坐标；

基于所述车辆经纬度坐标，从地图中获取预设范围内的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标。

3.根据权利要求1所述的方法，其特征在于，所述将每条所述地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条所述地图内车道线对应的地图内车道线车体坐标的步骤包括：

将所述车辆经纬度坐标转换为东北天坐标系下的车辆东北天坐标；将所述地图内车道线经纬度坐标转换为东北天坐标系下的地图内车道线东北天坐标；

在所述东北天坐标系下，将每条所述地图内车道线对应的地图内车道线东北天坐标投影至车体坐标系，得到每条所述地图内车道线对应的地图内车道线车体坐标。

4.根据权利要求1所述的方法，其特征在于，所述基于所述感知车道线的信息和每条所述地图内车道线对应的所述地图内车道线车体坐标，从多条所述地图内车道线中，确定与所述感知车道线匹配的目标地图内车道线的步骤包括：

基于每条所述地图内车道线对应的地图内车道线车体坐标，确定每条所述地图内车道线对应的第一拟合曲线；

基于所述感知车道线的信息确定所述感知车道线对应的第二拟合曲线，对所述第二拟合曲线进行采样，得到所述感知车道线上的多个观测点，以及每个所述观测点的观测点车体坐标；

基于所述第一拟合曲线以及每个所述观测点的观测点车体坐标，从多条所述地图内车道线中，确定与所述感知车道线匹配的目标地图内车道线。

5.根据权利要求4所述的方法，其特征在于，所述基于所述第一拟合曲线以及每个所述观测点的观测点车体坐标，从多条所述地图内车道线中，确定与所述感知车道线匹配的目标地图内车道线的步骤包括：

针对每条地图内车道线，计算每个所述观测点距离该地图内车道线的所述第一拟合曲线的距离；

计算多个所述观测点对应的距离的和，并取均值，得到该地图内车道线对应的均值结果；

从多条地图内车道线对应的多个均值结果中确定均值最小结果，将所述均值最小结果对应的地图内车道线确定为与所述感知车道线匹配的目标地图内车道线。

6.根据权利要求4所述的方法，其特征在于，所述基于所述目标地图内车道线，对所述目标车辆进行定位的步骤包括：

获取所述目标地图内车道线对应的目标拟合曲线；

确定每个所述观测点在所述目标拟合曲线上的垂点，以及所述垂点的垂点坐标；

针对每个所述观测点，计算该观测点的观测点坐标与对应的垂点坐标的差值，得到该观测点对应的误差量；其中，所述误差量对应的函数中包括横向位移偏差量及方向角偏差量；

计算所述误差量对所述横向位移偏差量及所述方向角偏差量的导数，得到该观测点对应的雅可比计算结果；

基于每个所述观测点对应的所述误差量和所述雅可比计算结果，对所述目标车辆进行定位。

7.根据权利要求6所述的方法，其特征在于，基于每个所述观测点对应的所述误差量和所述雅可比计算结果，对所述目标车辆进行定位的步骤包括：

基于每个所述观测点对应的所述误差量和所述雅可比计算结果，更新每个所述观测点对应的第一矩阵和第二矩阵；其中，每个所述观测点对应的更新后的第一矩阵与该观测点对应的所述雅可比计算结果的平方相关联；每个所述观测点对应的更新后第二矩阵与该观测点对应的雅可比计算结果与误差量的乘积相关联；

对多个所述观测点对应的更新后的第一矩阵进行加和，得到第一加和矩阵；

对多个所述观测点对应的更新后的第二矩阵进行加和，得到第二加和矩阵；

基于所述第一加和矩阵和所述第二加和矩阵，确定所述目标车辆的定位状态修正量；

基于所述定位状态修正量修正所述目标车辆的当前定位状态量，基于修正后的当前定位状态量对所述目标车辆进行定位。

8.一种车辆定位装置，其特征在于，所述装置包括：

获取模块，用于获取目标车辆的摄像头采集到的感知车道线的信息、所述目标车辆的车辆经纬度坐标、与所述车辆经纬度坐标关联的多条地图内车道线，以及每条地图内车道线对应的地图内车道线经纬度坐标；其中，所述地图内车道线经纬度坐标为形成所述地图内车道线的多个检测点的经纬度坐标的集合；

转换模块，用于将每条所述地图内车道线对应的地图内车道线经纬度坐标转换至车体坐标系，得到每条所述地图内车道线对应的地图内车道线车体坐标；其中，所述地图内车道线车体坐标为形成所述地图内车道线的多个检测点的车体坐标的集合；

确定模块，用于基于所述感知车道线的信息和每条所述地图内车道线对应的所述地图内车道线车体坐标，从多条所述地图内车道线中，确定与所述感知车道线匹配的目标地图内车道线；

定位模块，用于基于所述目标地图内车道线，对所述目标车辆进行定位。

9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的机器可执行指令，所述处理器执行所述机器可执行指令以实现权利要求1-7任一项所述的车辆定位方法。

10.一种机器可读存储介质，其特征在于，所述机器可读存储介质存储有机器可执行指令，所述机器可执行指令在被处理器调用和执行时，所述机器可执行指令促使所述处理器实现权利要求1-7任一项所述的车辆定位方法。