CN114563006B - 基于参考线匹配的车辆全局定位方法及装置 - Google Patents

Abstract

本发明公开一种基于参考线匹配的车辆全局定位方法及装置，该方法步骤包括：S01.获取全局参考线的数据、车辆的GPS初始位置以及视觉指引线；S02.计算全局参考线上距离GPS初始位置最近的参考点；S03.根据最近参考点确定车体中心GPS坐标，将车辆前进方向上指定长度的全局参考线投影转换至车体坐标系下；S04.计算转换后全局参考线与视觉指引线之间的夹角，根据夹角计算车体中心离道路中心线的距离；S05.根据夹角、车体中心离道路中心线的距离以及最近参考点，得到大地坐标系下最终的定位信息输出。本发明具有实现方法简单、成本低、计算开销小、定位效率以及精度高且环境适应性强等优点。

Description

基于参考线匹配的车辆全局定位方法及装置

技术领域

本发明涉及全局定位导航技术领域定位，尤其涉及一种基于参考线匹配的车辆全局定位方法及装置。

背景技术

无人驾驶车辆或自动驾驶车辆在行驶过程中，需要自动识别出道路区域并定位出车辆在道路中的位置，进而由车辆的决策层根据车辆的位置以及道路环境规划出车辆行驶路径。全局定位即为初始位置未知，靠车辆的自身运动估计出车辆当前的位置信息。

现有技术中车辆自动驾驶通常是采用以下方式实现全局定位：

1、利用GPS信号或者组合GPS与IMU(惯导)进行全局定位，定位得到车辆当前的全局位置。但是直接使用GPS信号定位，由于地球大气层的干扰、多路径效应等会带来误差，使得定位得到的坐标与实际的坐标值之间会存在一定的偏差，且在GPS信号弱时上述偏差将更为明显，而组合导航定位方式需要使用多个传感器，实现成本较高且定位过程较为复杂。

2、利用高精度地图与实时的现场点云数据进行匹配定位的方法，即通过构建高精度地图，然后与实时对现场采集的点云数据进行特征匹配，得到全局坐标。但是高精度地图的制作成本高，点云数据特征匹配需要消耗大量的计算资源，且还会存在对周边环境频繁变化适应力差等问题，如当道路周围环境发生改变时(如道路附近进行建筑施工等)，上述方法将无法正确的实现定位。

综上，现有技术中车辆全局定位方法要么定位精度取决于GPS信号，在GPS信号弱等的环境中定位会易于产生偏差，要么需要复杂的制图、特征点匹配过程，致使定位效率低、计算开销大且环境适应性差，同时由于车辆可能行驶在各类复杂的路况下，对于曲率较大的弯道场景的定位难度较大，上述传统全局定位方法就难以精准实现大曲率弯道场景下的定位。因此亟需提供一种车辆全局定位方法，以使得能够兼顾定位精度、效率以及环境适应性，同时能够满足大曲率弯道等复杂路况下的精准定位需求。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种实现方法简单、成本低、计算开销小、定位效率以及精度高且环境适应性强的基于参考线匹配的车辆全局定位方法及装置，能够满足大曲率弯道等复杂路况下的精准定位需求。

为解决上述技术问题，本发明提出的技术方案为：

一种基于参考线匹配的车辆全局定位方法，步骤包括：

S01.获取全局参考线的数据、车辆的GPS初始位置以及视觉指引线，所述全局参考线为预先获取的目标路线的GPS信号点集，所述视觉指引线为根据车辆行驶过程中采集的道路区域栅格图的中心线生成得到；

S02.计算所述全局参考线上距离所述GPS初始位置最近的参考点，得到最近参考点；

S03.根据所述最近参考点确定车体中心GPS坐标，并根据所述车体中心GPS坐标将车辆前进方向上指定长度的全局参考线投影转换至车体坐标系下，得到转换后全局参考线；

S04.计算所述转换后全局参考线与所述视觉指引线之间的夹角，根据所述夹角计算车体中心离道路中心线的距离；

S05.根据所述夹角、所述车体中心离道路中心线的距离以及所述最近参考点，得到大地坐标系下最终的定位信息输出。

进一步的，所述步骤S02包括：在所述全局参考线上间隔取多个采样点，分别计算各所述采样点与所述GPS初始位置之间的距离，取最小距离所对应的采样点作为所述最近参考点。

进一步的，所述步骤S03中，将所述最近参考点作为车体中心GPS坐标，以Y轴为车辆前进方向、X轴平行于地面并垂直于Y轴，将车辆前进方向上指定长度的全局参考线以所述最近参考点为车体中心投影转换至车体坐标系。

进一步的，所述步骤S03中，按照下式将车辆前进方向上指定长度的全局参考线以所述最近参考点为车体中心投影转换至车体坐标系：

其中，x_r ^m、y_r ^m、分别为最近参考点的横坐标、纵坐标、大地坐标系下的偏航角，xⁱ _r,yⁱ _r,/>分别为转换前全局参考线坐标点的横坐标、纵坐标、大地坐标系下的偏航角，x_c ⁱ，y_c ⁱ分别为转换至车体坐标下的横坐标、纵坐标。

进一步的，所述步骤S04包括：

以车体坐标系下车体中心坐标作为圆点，并在投影到车体坐标系下所述转换后全局参考线上取多个参考点，以及在所述视觉指引线上取多个采样点；

分别由所述转换后全局参考线上各参考点之间形成多个第一向量，以及分别由所述视觉指引线上各采样点之间形成多个第二向量；

分别计算各所述第一向量与所述第二向量之间的夹角，综合计算的各夹角得到最终的所述转换后全局参考线与所述视觉指引线之间的夹角。

进一步的，所述步骤S04中还包括计算所述转换后全局参考线与所述视觉指引线之间的夹角的方向，步骤包括：

以垂直于地面向上为Z轴正方向构建三维坐标系；

在构建的所述三维坐标系中，对一个所述第一向量和一个所述第二向量的Z轴进行赋值以扩展为三维形式，得到扩展后第一向量、扩展后第二向量；

计算所述扩展后第一向量、扩展后第二向量的叉积，根据叉积的正负判断所述夹角的方向。

进一步的，所述步骤S05中计算车体中心与道路边界之间的距离和/或车体中心离道路中心线的距离包括：

在车体坐标系中以车体中心点坐标作为圆点，并在车体中心正前方指定距离处取一目标点C，过所述目标点C构建一条直线LC，所述直线LC的斜率KC为-tanθ，其中θ为所述转换后全局参考线与视觉指引线之间的夹角；

根据道路边界点的坐标序列计算在所述直线LC上的左右道路边界点的坐标；

根据所述左右道路边界点的坐标，分别计算出所述左右道路边界点距离所述目标点C的距离；

根据所述左右道路边界点距离所述目标点C的距离，计算出车体中心距离左右道路边界的距离和/或车体中心离道路中心线的距离。

进一步的，所述步骤S05中转换至大地坐标系时，按照式计算得到大地坐标系下横坐标x_v，按照式/>计算得到大地坐标系下纵坐标y_v，以及按照式/>计算得到大地坐标系下偏航角/>其中x_r ^m、y_r ^m、/>分别为最近参考点的横坐标、纵坐标、大地坐标系下的偏航角，xⁱ _r,yⁱ _r,/>分别为转换前全局参考线坐标点的横坐标、纵坐标、大地坐标系下的偏航角，θ为所述转换后全局参考线与所述视觉指引线之间的夹角，b为所述车体中心离道路中心线的距离。

一种基于参考线匹配的车辆全局定位装置，包括：

获取模块，用于获取全局参考线的数据、车辆的GPS初始位置以及视觉指引线，所述全局参考线为预先获取的目标路线的GPS信号点集，所述视觉指引线为根据车辆行驶过程中采集的道路区域栅格图的中心线生成得到；

最近参考点计算模块，用于计算所述全局参考线上距离所述GPS初始位置最近的参考点，得到最近参考点；

投影转换模块，用于根据所述最近参考点确定车体中心GPS坐标，并根据所述车体中心GPS坐标将车辆前进方向上指定长度的全局参考线投影转换至车体坐标系下，得到转换后全局参考线；

匹配计算模块，用于计算所述转换后全局参考线与所述视觉指引线之间的夹角，根据所述夹角计算车体中心离道路中心线的距离；

定位输出模块，用于根据所述夹角、所述车体中心离道路中心线的距离以及所述最近参考点，得到大地坐标系下最终的定位信息输出。

或者所述车辆全局定位装置包括处理器以及存储器，所述存储器用于存储计算机程序，所述处理器用于执行所述计算机程序，所述处理器用于执行如上述方法。

一种存储有计算机程序的计算机可读存储介质，所述计算机程序执行时实现如上述方法。

与现有技术相比，本发明的优点在于：

1、本发明基于全局参考线与视觉指引线信息的匹配实现全局定位优化，通过获取全局参考线信息与视觉指引线，利用GPS初始位置定位到全局参考线的最近参考点，依据最近参考点将全局参考线投影至车体坐标系，然后对视觉指引线与全局参考线信息进行匹配，以利用视觉指引线进行局部精定位，由全局参考线与视觉指引线之间夹角计算出车体中心离道路中心线的距离，进而结合最近参考点即可转换得到最终的全局定位坐标系下的定位信息，能够实现低成本、高精度以及高效率的全局定位优化，有效提高全局定位的精度，无需复杂的制图或特征匹配过程，且由于仅需GPS提供初始位置，当车辆行驶至GPS信号弱的环境中时依然可以保持精准定位输出，同时不会受环境变化的影响，可以给自动驾驶决策规划层提供精准有效的导航与定位信息。

2、本发明进一步通过分别在视觉指引线、全局参考线上取多个采样点所形成多个第一向量、第二向量，综合第一向量与第二向量之间的各夹角来最终确定一个夹角值作为全局参考线与视觉指引线之间的夹角，可以快速、简单的实现全局参考线与视觉指引线之间的夹角的计算，可以避免单一角度计算造成计量误差，有效提高计算精度，同时夹角计算的普适性强，可以适用于各类复杂场景，进一步确保各类环境下全局定位的精度以及稳定性。

3、本发明进一步例通过在车体中心正前方一定距离取一目标点构建直线，利用直线以及全局参考线与视觉指引线之间的夹角的信息计算出左右道路边界点的坐标，进而计算出左右道路边界点距离目标点的距离，利用该左右道路边界点距离目标点的距离计算出车体中心离道路中心线的距离，可以充分利用全局参考线与视觉指引线之间的夹角、道路边界点信息快速、准确的定位出车辆在道路区域中的位置。

附图说明

图1是本实施例基于参考线匹配的车辆全局定位方法的实现流程示意图。

图2是本实施例实现全局参考性与视觉指引线匹配定位的原理示意图。

图3是本实施例中横向距离计算的原理示意图。

图4是本发明具体应用实施例中基于参考线匹配实现车辆全局定位的详细实现流程示意图。

具体实施方式

以下结合说明书附图和具体优选的实施例对本发明作进一步描述，但并不因此而限制本发明的保护范围。

如图1所示，本实施例基于参考线匹配的车辆全局定位方法的步骤包括：

S01.获取全局参考线的数据、车辆的GPS初始位置以及视觉指引线，全局参考线为预先获取的目标路线的GPS信号点集，视觉指引线为根据车辆行驶过程中采集的道路区域栅格图的中心线生成得到；

S02.计算全局参考线上距离GPS初始位置最近的参考点，得到最近参考点；

S03.根据最近参考点确定车体中心GPS坐标，并根据车体中心GPS坐标将车辆前进方向上指定长度的全局参考线投影转换至车体坐标系下，得到转换后全局参考线；

S04.计算转换后全局参考线与视觉指引线之间的夹角，根据夹角计算车体中心离道路中心线的距离；

S05.根据夹角、车体中心离道路中心线的距离以及最近参考点，得到大地坐标系下最终的定位信息输出。

本实施例基于全局参考线与视觉指引线信息的匹配实现全局定位优化，通过获取全局参考线信息与视觉指引线，利用GPS初始位置定位到全局参考线的最近参考点，依据最近参考点将全局参考线投影至车体坐标系，然后对视觉指引线与全局参考线信息进行匹配，以利用视觉指引线进行局部精定位，由全局参考线与视觉指引线之间夹角计算出车体中心离道路中心线的距离，进而结合最近参考点即可转换得到最终的全局定位坐标系下的定位信息，能够实现低成本、高精度以及高效率的全局定位优化，有效提高全局定位的精度，无需复杂的制图或特征匹配过程，且由于仅需GPS提供初始位置，当车辆行驶至GPS信号弱的环境中时依然可以保持精准定位输出，同时由于周围环境的变化引起的特征点变化不会影响全局参考线与视觉指引线的生成，因而定位效果不会受环境变化的影响，可以适应于大曲率弯道等各类复杂路况下，从而可以给自动驾驶决策规划层持续提供精准、稳定的导航与定位信息。

本实施例步骤S01中，具体需要获取以下信息：

(1)GPS信号

本实施例大地坐标系具体为东-北-天(e-n-u)坐标系，上述x_g,y_g,conf_g分别表示在东-北-天(e-n-u)坐标系下位置点的横坐标、纵坐标、东-北-天(e-n-u)坐标系下的偏航角以及该信号的置信度。

(2)道路区域栅格图、视觉指引线坐标点以及道路边界的坐标点

首先获取车体坐标系下包含道路区域的栅格图，即在车辆行驶过程中通过摄像头采集道路环境图像进行分割，将分割出的道路区域转换为栅格图；然后根据栅格图生成道路区域的视觉指引线，视觉指引线即为根据道路区域的栅格图生成的道路中心线以作为道路指引，获取视觉指引线的坐标点以及栅格图上道路边界的坐标点。

(3)全局参考线坐标点集

全局参考线也即为路线对应的历史GPS坐标点集，其中xⁱ _r,yⁱ _r,分别表示全局参考线上坐标点的横坐标、纵坐标、东-北-天(e-n-u)坐标系下的偏航角。全局参考线坐标点集/>具体可使用高精度的GPS定位装置提前录制相应的目标路线的GPS信号点集，在具体应用实施例中生成的全局参考线如图2所示。由于周围环境的变化引起的特征点变化不会影响全局参考线与视觉指引线的生成，实际中也可使用相应低成本的GPS定位装置进行初步的初始化。

本实施例步骤S02的具体步骤包括：在全局参考线上间隔取多个采样点，分别计算各采样点与GPS初始位置之间的距离，取最小距离所对应的采样点作为最近参考点。通过采用遍历迭代计算方法来计算出GPS初始位置距离全局参考线的最近参考点，不仅计算精度高，且普适性以及鲁棒性好，对于任意的全局参考线以及GPS信号都能精准定位到最近参考点，可以适用于转弯道路或路口较多等的各类复杂场景中。

在具体应用实施例中，计算全局参考线上离GPS给出位置点最近的参考点时，首先在全局参考线上以指定长度间隔取采样点，计算出全局参考线上离GPS初始位置点最近的距离d_m，对应的参考点d_m即为最近参考点，最近参考点d_m表示为其中，x_g和y_g分别表示当前GPS信号给出的初始位置的横向坐标和纵向坐标，xⁱ _r和yⁱ _r分别表示为全局参考线上的采样点的横向坐标和纵向坐标，n表示采样点总数。

可以理解的是，最近参考点的计算也可以根据实际需求采用除上述以外的其他计算方式。

本实施例步骤S03中，具体将最近参考点作为车体中心GPS坐标，以Y轴为车辆前进方向、X轴平行于地面并垂直于Y轴，将车辆前进方向上指定长度(如车辆前方40米)的全局参考线以最近参考点为车体中心投影转换至车体坐标系，即以最近参考点为中心将车辆前进方向上的部分全局参考线投影至车体坐标系，以将全局参考线转换至与视觉指引线相同的坐标系下。不在车辆前进方向上的全局参考线不需参与匹配计算，因而只取车辆前进方向上的部分全局参考线进行投影转换，可以减少不必要的转换以及匹配计算处理。

可以理解的是，在获取出最近参考点后也可以不直接作为车体中心GPS坐标，而是以最近参考点作为基准来确定车体中心GPS坐标，如在最近参考点的基础上加入一定的阈值作为车体中心GPS坐标以进一步提高精度，具体可根据实际需求确定。

如图2所示，首先根据最近参考点信息，确定出车体中心的GPS坐标；然后以Y轴为车辆前进方向(右手法则下，X轴平行于地面并垂直于Y轴)，根据当前车体中心的GPS坐标信息将车辆前进方向的全局参考线上指定长度的点投影转换到车体坐标系中，即将在东-北-天(e-n-u)坐标系下全局参考线坐标点集/>以最近参考点(x_r ^m，y_r ^m)为车体的坐标中心转换至车体坐标系中，转换过程中车体坐标系的y轴实际是沿着最近参考点位置的全局参考线的切线方向。

本实施例步骤S03中，具体按照式将车辆前进方向上指定长度的全局参考线以最近参考点为车体中心投影转换至车体坐标系，其中，x_r ^m、y_r ^m、/>分别为最近参考点的横坐标、纵坐标、大地坐标系下的偏航角，xⁱ _r,yⁱ _r,分别为转换前全局参考线坐标点的横坐标、纵坐标、大地坐标系下的偏航角，x_c ⁱ，y_c ⁱ分别为转换至车体坐标下的横坐标、纵坐标。

本实施例步骤S04的具体步骤包括：

S401.以车体坐标系下车体中心坐标作为圆点，并在投影到车体坐标系下转换后全局参考线上取多个参考点，以及在视觉指引线上取多个采样点；

S402.分别由转换后全局参考线上各参考点之间形成多个第一向量，以及分别由视觉指引线上各采样点之间形成多个第二向量；

S403.分别计算各第一向量与第二向量之间的夹角，综合计算的各夹角得到最终的转换后全局参考线与视觉指引线之间的夹角θ。

本实施例通过分别在视觉指引线、全局参考线上取多个采样点所形成多个第一向量、第二向量，综合第一向量与第二向量之间的各夹角来最终确定一个夹角值作为转换后全局参考线与视觉指引线之间的夹角θ，可以快速、简单的实现全局参考线与视觉指引线之间的夹角θ的计算，避免单一角度计算造成计量误差，有效提高计算精度，同时由于是计算转换到车体坐标系下的全局参考线与视觉指引线之间的夹角，夹角计算的普适性强，可以适用于各类复杂场景，如对于曲率较大的弯道场景也能够精准计算出上述夹角，从而可以进一步确保各类环境下全局定位的精度以及稳定性。

以分别在视觉指引线、全局参考线上取四个采样点为例，上述计算全局参考线与视觉指引线之间的夹角θ的详细步骤为：

(a)以车体坐标系车体中心(0，0)坐标为起点，向车辆前进方向取全局参考线上投影到车体坐标系的四个参考点，相对应的坐标表示为(x_c ^s1,y_c ^s1)、(x_c ^s2,y_c ^s2)、(x_c ^s3,y_c ^s3)，(x_c ^s4,y_c ^s4)，坐标距离的计算公式如式(4)所示。同理以车体坐标系车体中心(0，0)坐标为起点，在视觉指引线取四个采样点，相对应的坐标表示为(x_t ^s1,y_t ^s1)、(x_t ^s2,y_t ^s2)、(x_t ^s3,y_t ^s3)、(x_t ^s4,y_t ^s4)。

(b)将车体坐标系点(x_c ^s1,y_c ^s1)到点(x_c ^s2,y_c ^s2)的向量表示为a_c ^s1，将车体坐标系点(x_c ^s2,y_c ^s2)到点(x_c ^s3,y_c ^s3)的向量表示为a_c ^s2，将车体坐标系点(x_c ^s3,y_c ^s3)到点(x_c ^s4,y_c ^s4)的向量表示为a_c ^s3；同理将车体坐标系点(x_t ^s1,y_t ^s1)到点(x_t ^s2,y_t ^s2)的向量表示为b_t ^s1，将车体坐标系点(x_t ^s2,y_t ^s2)到点(x_t ^s3,y_t ^s3)的向量表示为b_t ^s2，将车体坐标系点(x_t ^s3,y_t ^s3)到点(x_t ^s4,y_t ^s4)的向量表示为b_t ^s3。

(c)根据向量之间的夹角计算公式，计算向量a_r ^s1和向量b_t ^s1之间的夹角为θ₁，以及计算计算向量a_r ^s2和向量b_t ^s2之间的夹角为θ₂以及计算向量a_r ^s3和向量b_t ^s3之间的夹角为θ₃，然后计算θ₁、θ₂、θ₃的平均值并作为最终的夹角θ，即为车体坐标系下参考线与视觉指引线的夹角。通过采用多个采样点计算多个角度的平均值方式，可以有效降低录制全局参考线的坐标的误差和视觉指引线某一点坐标的误差对计算车体坐标系下航向角带来的影响，提高计算精度。

上述采样点的取值数量、采样点的位置、车辆前进方向的向量等均可以根据实际需求配置，最终夹角也可以采用除均值外的其他统计值来计算，如方差等。

本实施例步骤S04中还包括计算转换后全局参考线与视觉指引线之间的夹角的方向，步骤包括：

S411.以垂直于地面向上为Z轴正方向构建三维坐标系；

S412.在构建的三维坐标系中，对一个第一向量和一个第二向量的Z轴进行赋值以扩展为三维形式，得到扩展后第一向量、扩展后第二向量；

S413.计算扩展后第一向量、扩展后第二向量的叉积，根据叉积的正负判断夹角θ的方向。

在具体应用实施例中，以上述分别在视觉指引线、全局参考线上取四个采样点为例，计算夹角θ的方向时，以垂直于地面向上为Z轴正方向构建三维坐标系，将向量a_c ^s3和向量b_t ^s3的Z轴的值赋值为0从而扩展为三维的形式，计算新构建的向量A_c ^s3和向量B_t ^s3叉积；当计算得到的叉积为正时，即夹角θ的值为正，表明车辆车头的朝向相对于路面为左，反之夹角θ的值为负，表明车辆车头的朝向相对于路面为右，即根据夹角θ的方向可以判断车辆车头的朝向。

本实施例步骤S05中计算车体中心与道路边界之间的距离和/或车体中心离道路中心线的距离包括：

S501.在车体坐标系中以车体中心点坐标作为圆点，并在车体中心正前方指定距离处取一目标点C，过目标点C构建一条直线LC，直线LC的斜率KC为-tanθ，其中θ为转换后全局参考线与视觉指引线之间的夹角；

S502.根据道路边界点的坐标序列计算在直线LC上的左右道路边界点的坐标；

S503.根据左右道路边界点的坐标，分别计算出左右道路边界点距离目标点C的距离；

S504.根据左右道路边界点距离目标点C的距离，计算出车体中心距离左右道路边界的距离、车体中心离道路中心线的距离。

本实施例通过在车体中心正前方一定距离取一目标点C构建直线LC，利用直线LC以及全局参考线与视觉指引线之间的夹角的信息计算出左右道路边界点的坐标，进而计算出左右道路边界点距离目标点C的距离，利用该左右道路边界点距离目标点C的距离即可计算出车体中心距离左右道路边界的距离、车体中心离道路中心线的距离，，可以充分利用全局参考线与视觉指引线之间的夹角、道路边界点信息快速、准确的定位出车辆在道路区域中的位置。

如图3所示，在车体坐标系上车体中心A点坐标为(0，0)，在车体中心正前方指定距离处取一目标点C，目标点C坐标即为(x_c，y_c)，其中x_c＝0，过点C作一条直线，直线的斜率为k_c，k_c的值大小为-tanθ，则根据直线的斜率和过已知点C的条件可得到直线LC的数学表达式为：y-y_c＝k_c*(x-x_c)。

由于视觉摄像头的视野范围最近处大约在车体中心前方7米处，目标点C优选的可取车体中心正前方7米处坐标点，则目标点C的坐标即为(0,7)，当然目标点C具体也可以根据实际需求取其他位置点。

根据直线LC的表达式和已知的道路边界点的坐标序列可以先计算出在直线LC上的左道路边界点F的坐标和右道路边界点G的坐标；进而根据两点之间的距离公式，可得出CF和CG的大小，分别表示为DCL和DCR，再根据(DCL+yc*sinθ)、(DCR-yc*sinθ)即可求出BF和BG的大小，即为车体中心距离左右道路边界的距离DL和DR,道路宽度D为DL和DR的和，即D＝DL+DR，进而可得到车体中心距离道路中心线的距离b为(DR-DL)/2，即b＝(DR-DL)/2。

考虑到道路边界存在误差的情况，本实施例进一步依次取车体中心正前方7.5m,8m，...米处坐标(坐标点取值具体可根据实际需求配置)重复执行上述步骤S05，求取得到相应的一系列的道路宽度值、车体中心离左右边界的距离、车体中心离道路中心线的距离，取平均值作为最终的计算结果，记为D、D_L ^a、D_R ^a和b^a，可以进一步提高计算精度，减少由于测量误差带来的影响。

本实施例中步骤S05中转换至大地坐标系时，具体按照式计算得到大地坐标系下横坐标x_v，按照式/>计算得到大地坐标系下纵坐标y_v，以及按照式/>计算得到大地坐标系下偏航角/>其中x_r ^m、y_r ^m、/>分别为最近参考点的横坐标、纵坐标、大地坐标系下的偏航角，xⁱ _r,yⁱ _r,/>分别为转换前全局参考线坐标点的横坐标、纵坐标、大地坐标系下的偏航角，θ为转换后全局参考线与视觉指引线之间的夹角，b为车体中心离道路中心线的距离。进一步还可以根据道路边界点集对应在道路语义分割推理中属于道路类别的概率值得到置信度conf_v输出。

在具体应用实施例中，根据计算得到的θ值和b^a值，将计算出的车体坐标系的值转换至东-北-天(e-n-u)坐标系下，得到新的定位输出x_v为/>y_v为/> 的值为/>conf_v的值为道路边界点集对应在道路语义分割推理中属于道路类别的概率值集的平均值。

以下以在具体应用实施例中采用本发明上述方法实现车辆全局定位为例，对本发明进行进一步说明。

如图4所示，本实施例基于参考线匹配实现车辆全局定位的详细步骤为：

步骤1：数据输入：输入GPS信号车体坐标系下包含道路区域的栅格图以及提前录制的全局参考线坐标点集/>其中x_g,y_g,,/>conf_g分别表示在东-北-天(e-n-u)坐标系下为该坐标系下位置点的横坐标、纵坐标、东-北-天(e-n-u)坐标系下的偏航角、该信号的置信度，在道路区域的栅格图中生成视觉指引线，获取栅格图上道路边界的坐标点、视觉指引线的坐标点。

步骤2；最近参考点计算：根据GPS给出的初始位置，计算全局参考线上离GPS给出初始位置点最近的参考点。

在全局参考线上以1米的距离间隔取多个采样点，按照式(1)、(2)计算出最小距离d_m，得到最近参考点

d_m＝min(d₁，d₂…d_n)i＝1，2…n (2)

步骤3；全局参考线投影：根据最近参考点d_m将全局参考线从东-北-天(e-n-u)坐标系下转换至车体坐标系。

将最近参考点信息设为车体中心的GPS坐标，以最近参考点d_m为中心，将车辆前进方向的全局参考线上前方40米的点投影转换到到车体坐标系中，其中以Y轴为车辆前进方向，X轴平行于地面并垂直于Y轴，即将在东-北-天(e-n-u)坐标系下全局参考线坐标点集/>以最近参考点(x_r ^m，y_r ^m)为车体的坐标中心转换至车体坐标系中，转换过程中车体坐标系的y轴实际是沿着最近参考点位置的全局参考线的切线方向，转换公式如式(3)所示。

其中，x_r ⁱ，y_r ⁱ和x_c ⁱ，y_c ⁱ分别表示在东-北-天(e-n-u)坐标系下全局参考线的坐标值和其投影到车体坐标系下的坐标值。

步骤4：全局参考线与视觉参考线之间的夹角计算

以车体坐标系车体中心(0，0)坐标为起点，向车辆前进方向取全局参考线上投影到车体坐标系的四个参考点，使得这四个参考点离车体中心坐标(0，0)的距离分别为10米、15米、20米，25米相对应的坐标表示为(x_c ^s1,y_c ^s1)、(x_c ^s2,y_c ^s2)、(x_c ^s3,y_c ^s3)，(x_c ^s4,y_c ^s4)坐标距离计算公式表示为式(4)。同理以车体坐标系车体中心(0，0)坐标为起点，在视觉指引线取四个采样点，使得这四个采样点离车体中心坐标(0，0)的距离分别为10米、15米、20米、25米，相对应的坐标表示为(x_t ^s1,y_t ^s1)、(x_t ^s2,y_t ^s2)、(x_tt ^s3,y_t ^s3)、(x_t ^s4,y_t ^s4)，坐标距离计算公式表示为式(5)。

将车体坐标系点(x_c ^s1,y_c ^s1)到点(x_c ^s2,y_c ^s2)的向量表示为a_c ^s1，将车体坐标系点(x_c ^s2,y_c ^s2)到点(x_c ^s3,y_c ^s3)的向量表示为a_c ^s2，将车体坐标系点(x_c ^s3,,y_c ^s3)到点(x_c ^s4,y_c ^s4)的向量表示为a_c ^s3。同理将车体坐标系点(x_t ^s1,y_t ^s1)到点(x_t ^s2,y_t ^s2)的向量表示为b_t ^s1，将车体坐标系点(x_t ^s2,y_t ^s2)到点(x_t ^s3,y_t ^s3)的向量表示为b_t ^s2，将车体坐标系点(x_t ^s3,y_t ^s3)到点(x_t ^s4,y_t ^s4)的向量表示为b_t ^s3。根据向量之间的夹角计算公式，根据式(6)计算向量a_r ^s1和向量b_t ^s1之间的夹角为θ₁，根据式(7)计算计算向量a_r ^s2和向量b_t ^s2之间的夹角为θ₂，根据式(8)计算向量a_r ^s3和向量b_t ^s3之间的夹角为θ₃。

根据公式(9)计算θ₁、θ₂、θ₃的平均值θ。

9＝(θ₁+θ₂+θ₃)/3＝(acrcos(cosθ₁)+acrcos(cosθ₂)+acrcos(cosθ₃))/3 (9)

根据上述式(9)计算得到的θ值即为所求的车体坐标系下参考线与视觉指引线的夹角θ。

步骤5:夹角θ的方向计算

以垂直于地面向上为Z轴正方向构建三维坐标系，将向量a_c ^s3和向量b_t ^s3的Z轴的值赋值为0从而扩展为三维形式，计算新构建的向量A_c ^s3和向量B_t ^s3叉积，计算公式如下所示：

当计算得到的叉积为正时，θ的值为正，表明车辆车头的朝向相对于路面为左，反之θ的值为负，表明车辆车头的朝向相对于路面为右。

步骤6：道路宽度和车体中心距离道路中心线左右边界的距离计算。

在车体坐标系上车体中心A点坐标为(0，0)，取车体中心正前方7米处坐标C点(0，7)，过点C作一条直线，直线的斜率为K_C,K_C的值大小为-tanθ，根据直线的斜率和过已知点C的条件可求直线L_C的数学表达式(11)：

y-y_c＝k_c*(x-x_c) (11)

根据式(11)和已知的道路边界点的坐标序列求在直线L_C上的左道路边界点F的坐标和右道路边界点G的坐标；进而根据两点之间的距离公式，得出CF和CG的大小表示为D_CL和D_CR，进而求出BF和BG的大小即为车体中心离左右边界的距离D_L和D_R,D_L等于(D_CL+7sinθ),D_R等于(D_CR-7sinθ)；道路宽度为D_L和D_R的和，即为D＝D_L+D_R,进而得到车体中心离道路中心线的距离为b＝(D_R-D_L)/2；进一步依次取车体中心正前方多个点坐标，求取得到一系列的道路宽度值、车体中心离左右边界的距离、车体中心离道路中心线的距离，求平均值作为最终的结果D、D_L ^a、D_R ^a和b^a。

步骤7：定位输出：车体坐标系的相应值转换至东-北-天(e-n-u)坐标系计算输出新的定位输出

根据计算得到的θ值和b^a值，计算输出的x_v为计算输出的y_v为/>以及计算输出的/>的值为/>输出的conf_v的值具体取道路边界点集对应在道路语义分割推理中属于道路类别的概率值集的平均值。

本实施例中基于参考线匹配的车辆全局定位装置包括：

获取模块，用于获取全局参考线的数据、车辆的GPS初始位置以及视觉指引线，所述全局参考线为预先获取的目标路线的GPS信号点集，所述视觉指引线为根据车辆行驶过程中采集的道路区域栅格图的中心线生成得到；

最近参考点计算模块，用于计算所述全局参考线上距离所述GPS初始位置最近的参考点，得到最近参考点；

投影转换模块，用于根据所述最近参考点确定车体中心GPS坐标，并根据所述车体中心GPS坐标将车辆前进方向上指定长度的全局参考线投影转换至车体坐标系下，得到转换后全局参考线；

匹配计算模块，用于计算所述转换后全局参考线与所述视觉指引线之间的夹角，根据所述夹角计算车体中心离道路中心线的距离；

定位输出模块，用于根据所述夹角、所述车体中心离道路中心线的距离以及所述最近参考点，得到大地坐标系下最终的定位信息输出。

本实施例中基于参考线匹配的车辆全局定位装置与上述基于参考线匹配的车辆全局定位方法为一一对应，在此不再一一赘述。

本实施例基于参考线匹配的车辆全局定位装置，还可以为：包括处理器以及存储器，存储器用于存储计算机程序，处理器用于执行计算机程序，处理器用于执行计算机程序以执行如上述基于参考线匹配的车辆全局定位方法。

本实施例还提供存储有计算机程序的计算机可读存储介质，计算机程序执行时实现如上述的方法。

上述只是本发明的较佳实施例，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。因此，凡是未脱离本发明技术方案的内容，依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均应落在本发明技术方案保护的范围内。

Claims (11)

1.一种基于参考线匹配的车辆全局定位方法，其特征在于，步骤包括：

S01.获取全局参考线的数据、车辆的GPS初始位置以及视觉指引线，所述全局参考线为预先获取的目标路线的GPS信号点集，所述视觉指引线为根据车辆行驶过程中采集的道路区域栅格图的中心线生成得到；

S02.计算所述全局参考线上距离所述GPS初始位置最近的参考点，得到最近参考点；

S03.根据所述最近参考点确定车体中心GPS坐标，并根据所述车体中心GPS坐标将车辆前进方向上指定长度的全局参考线投影转换至车体坐标系下，得到转换后全局参考线；

S04.计算所述转换后全局参考线与所述视觉指引线之间的夹角，根据所述夹角计算车体中心离道路中心线的距离；

S05.根据所述夹角、所述车体中心离道路中心线的距离以及所述最近参考点，得到大地坐标系下最终的定位信息输出。

2.根据权利要求1所述的基于参考线匹配的车辆全局定位方法，其特征在于，所述步骤S02包括：在所述全局参考线上间隔取多个采样点，分别计算各所述采样点与所述GPS初始位置之间的距离，取最小距离所对应的采样点作为所述最近参考点。

3.根据权利要求1所述的基于参考线匹配的车辆全局定位方法，其特征在于，所述步骤S03中，将所述最近参考点作为车体中心GPS坐标，以Y轴为车辆前进方向、X轴平行于地面并垂直于Y轴，将车辆前进方向上指定长度的全局参考线以所述最近参考点为车体中心投影转换至车体坐标系。

4.根据权利要求3所述的基于参考线匹配的车辆全局定位方法，其特征在于，所述步骤S03中，按照下式将车辆前进方向上指定长度的全局参考线以所述最近参考点为车体中心投影转换至车体坐标系：

其中，x_r ^m、y_r ^m、φ^m _r分别为最近参考点的横坐标、纵坐标、大地坐标系下的偏航角，xⁱ _r ,yⁱ _r ,φⁱ _r分别为转换前全局参考线坐标点的横坐标、纵坐标、大地坐标系下的偏航角，x_c ⁱ，y_c ⁱ分别为转换至车体坐标下的横坐标、纵坐标。

5.根据权利要求1所述的基于参考线匹配的车辆全局定位方法，其特征在于，所述步骤S04包括：

以车体坐标系下车体中心坐标作为圆点，并在投影到车体坐标系下所述转换后全局参考线上取多个参考点，以及在所述视觉指引线上取多个采样点；

分别由所述转换后全局参考线上各参考点之间形成多个第一向量，以及分别由所述视觉指引线上各采样点之间形成多个第二向量；

分别计算各所述第一向量与所述第二向量之间的夹角，综合计算的各夹角得到最终的所述转换后全局参考线与所述视觉指引线之间的夹角。

6.根据权利要求5所述的基于参考线匹配的车辆全局定位方法，其特征在于，所述步骤S04中还包括计算所述转换后全局参考线与所述视觉指引线之间的夹角的方向，步骤包括：

以垂直于地面向上为Z轴正方向构建三维坐标系；

在构建的所述三维坐标系中，对一个所述第一向量和一个所述第二向量的Z轴进行赋值以扩展为三维形式，得到扩展后第一向量、扩展后第二向量；

计算所述扩展后第一向量、扩展后第二向量的叉积，根据叉积的正负判断所述夹角的方向。

7.根据权利要求1～6中任意一项所述的基于参考线匹配的车辆全局定位方法，其特征在于，所述步骤S05中计算车体中心与道路边界之间的距离和/或车体中心离道路中心线的距离包括：

在车体坐标系中以车体中心点坐标作为圆点，并在车体中心正前方指定距离处取一目标点C，过所述目标点C构建一条直线LC，所述直线LC的斜率KC为-tanθ，其中θ为所述转换后全局参考线与视觉指引线之间的夹角；

根据道路边界点的坐标序列计算在所述直线LC上的左右道路边界点的坐标；

根据所述左右道路边界点的坐标，分别计算出所述左右道路边界点距离所述目标点C的距离；

根据所述左右道路边界点距离所述目标点C的距离，计算出车体中心距离左右道路边界的距离和/或车体中心离道路中心线的距离。

8. 根据权利要求1～6中任意一项所述的基于参考线匹配的车辆全局定位方法，其特征在于，所述步骤S05中转换至大地坐标系时，按照式计算得到大地坐标系下横坐标x_v，按照式/>计算得到大地坐标系下纵坐标y_v ，以及按照式（φ^m _r-θ）计算得到大地坐标系下偏航角φ_v，其中x_r ^m、y_r ^m、φ^m _r分别为最近参考点的横坐标、纵坐标、大地坐标系下的偏航角，θ为所述转换后全局参考线与所述视觉指引线之间的夹角，b为所述车体中心离道路中心线的距离。

9.一种基于参考线匹配的车辆全局定位装置，其特征在于，包括：

获取模块，用于获取全局参考线的数据、车辆的GPS初始位置以及视觉指引线，所述全局参考线为预先获取的目标路线的GPS信号点集，所述视觉指引线为根据车辆行驶过程中采集的道路区域栅格图的中心线生成得到；

最近参考点计算模块，用于计算所述全局参考线上距离所述GPS初始位置最近的参考点，得到最近参考点；

投影转换模块，用于根据所述最近参考点确定车体中心GPS坐标，并根据所述车体中心GPS坐标将车辆前进方向上指定长度的全局参考线投影转换至车体坐标系下，得到转换后全局参考线；

匹配计算模块，用于计算所述转换后全局参考线与所述视觉指引线之间的夹角，根据所述夹角计算车体中心离道路中心线的距离；

定位输出模块，用于根据所述夹角、所述车体中心离道路中心线的距离以及所述最近参考点，得到大地坐标系下最终的定位信息输出。

10.一种基于参考线匹配的车辆全局定位装置，包括处理器以及存储器，所述存储器用于存储计算机程序，其特征在于，所述处理器用于执行如权利要求1～8中任意一项所述方法。

11.一种存储有计算机程序的计算机可读存储介质，其特征在于，所述计算机程序执行时实现如权利要求1～8中任意一项所述的方法。