CN115752455A - 一种自动驾驶汽车的定位系统及方法 - Google Patents

Abstract

本发明提供一种自动驾驶汽车的定位系统及方法，所述自动驾驶汽车的定位系统包括数据获取模块，用于获取汽车的组合惯导数据、环境感知数据和所述汽车所在位置的地图数据；语义定位模块，用于根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的语义位姿数据；数据校验模块，用于对所述组合惯导数据和环境感知数据进行校验处理，以生成所述汽车的定位校验数据；以及数据滤波模块，用于根据所述组合惯导数据、所述语义位姿数据和所述定位校验数据，生成所述汽车的融合位姿数据。本发明可解决城区道路场景下车辆定位不佳的问题，实现汽车的高精度定位。

Description

一种自动驾驶汽车的定位系统及方法

技术领域

本申请涉及汽车自动驾驶融合定位技术领域，具体涉及一种自动驾驶汽车的定位系统及方法。

背景技术

高精度定位是实现汽车自动驾驶的必要条件。目前，自动驾驶汽车的定位方案包括组合惯导定位、激光雷达定位、视觉定位及多传感器融合定位。其中，组合惯导定位存在较大的局限性，像隧道、复杂立交桥、树林、高楼林立的城区等场景都会造成卫星定位信号失效，组合惯导定位无法在复杂环境中提供可靠、连续的定位服务。视觉定位存在极端天气下易受干扰、无法采集足够的特征等问题，从而导致定位不够准确。激光雷达定位以及基于激光雷达定位的多传感器融合定位，虽然定位稳定性和精度可达到很好的效果，但是系统成本过于昂贵，无法实现大规模量产。自动驾驶汽车的定位方案需再多加改进。

发明内容

鉴于以上所述现有技术的缺点，本发明提供一种自动驾驶汽车的定位系统及方法，以解决上述技术问题。

本发明提供的一种自动驾驶汽车的定位系统，包括：

数据获取模块，用于获取汽车的组合惯导数据、环境感知数据和所述汽车所在位置的地图数据；

语义定位模块，用于根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的语义位姿数据；

数据校验模块，用于对所述组合惯导数据和环境感知数据进行校验处理，以生成所述汽车的定位校验数据；以及

数据滤波模块，用于根据所述组合惯导数据、所述语义位姿数据和所述定位校验数据，生成所述汽车的融合位姿数据。

于本发明的一实施例中，所述数据获取模块包括：

语义初始化单元，用于根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的起始点数据；以及

航迹推算单元，用于基于所述起始点数据，对所述组合惯导数据和所述环境感知数据进行计算处理，以生成所述汽车的位姿推算数据。

于本发明的一实施例中，所述数据获取模块还包括：

语义匹配单元，用于基于所述环境感知数据和所述地图数据，对所述位姿推算数据进行匹配处理，以生成所述语义位姿数据。

于本发明的一实施例中，所述积分处理满足以下公式：

p_cur＝p_last+R_last·v·Δt

R_cur＝R_last·exp((ω·Δt)^∧)

其中，p_cur为当前帧的位置数据，p_last为上一帧的位置数据，R_cur为当前帧的姿态数据，R_last为上一帧的姿态数据，v为轮速，ω为角速度，Δt为两帧时间间隔数据。

于本发明的一实施例中，所述数据滤波模块包括：

滤波初始化单元，用于根据所述语义位姿数据和所述定位校验数据，生成所述汽车的初始位姿数据；以及

预测单元，用于根据所述初始位姿数据和所述组合惯导数据，生成所述汽车的预测位姿数据。

于本发明的一实施例中，所述数据滤波模块还包括：

更新单元，用于根据所述预测位姿数据、所述语义位姿数据和所述定位校验数据，生成所述融合位姿数据。

于本发明的一实施例中，所述组合惯导数据包括所述汽车的卫星定位数据、惯性测量数据和轮速数据。

本发明还提供一种自动驾驶汽车的定位方法，其特征在于，包括：

获取汽车的组合惯导数据、环境感知数据和所述汽车所在位置的地图数据；

根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的语义位姿数据；

对所述组合惯导数据和环境感知数据进行校验处理，以生成所述汽车的定位校验数据；以及

根据所述组合惯导数据、所述语义位姿数据和所述定位校验数据，生成所述汽车的融合位姿数据。

于本发明的一实施例中，所述根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成语义位姿数据的步骤，包括：

根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的起始点数据；

基于所述起始点数据，对所述组合惯导数据和所述环境感知数据进行计算处理，以生成所述汽车的位姿推算数据；以及

基于所述环境感知数据和所述地图数据，对所述位姿推算数据进行匹配处理，以生成所述语义位姿数据。

于本发明的一实施例中，所述根据所述组合惯导数据、所述语义位姿数据和所述定位校验数据，生成融合位姿数据的步骤，包括：

根据所述语义位姿数据和所述定位校验数据，生成所述汽车的初始位姿数据；

根据所述初始位姿数据和所述组合惯导数据，生成所述汽车的预测位姿数据；以及

根据所述预测位姿数据、所述语义位姿数据和所述定位校验数据，生成所述融合位姿数据。

如上，本发明提供一种自动驾驶汽车的定位系统及方法，具有成本低、定位精度高的优点，可解决城区道路场景下车辆定位不佳的问题，实现汽车的高精度定位。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术者来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。在附图中：

图1是本发明的一示例性实施例示出自动驾驶汽车的定位系统的逻辑框图；

图2是本发明的一示例性实施例示出自动驾驶汽车的定位方法的流程示意图；

图3是图2中步骤S202的一具体实施方式流程示意图；

图4是图2中步骤S204的一具体实施方式流程示意图。

元件标号说明：

100、数据获取模块；

200、语义定位模块；201、语义初始化单元；202、航迹推算单元；203、语义匹配单元；

300、数据校验模块；

400、数据滤波模块；401、滤波初始化单元；402、预测单元；403、更新单元。

具体实施方式

以下将参照附图和优选实施例来说明本发明的实施方式，本领域技术人员可由本说明书中所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。应当理解，优选实施例仅为了说明本发明，而不是为了限制本发明的保护范围。

以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

在下文描述中，探讨了大量细节，以提供对本发明实施例的更透彻的解释，然而，对本领域技术人员来说，可以在没有这些具体细节的情况下实施本发明的实施例是显而易见的，在其他实施例中，以方框图的形式而不是以细节的形式来示出公知的结构和设备，以避免使本发明的实施例难以理解。

请参阅图1～图4，本发明提供一种自动驾驶汽车的定位系统及方法，可应用于汽车自动驾驶融合定位技术领域。解决城区道路场景下车辆定位不佳的问题，实现汽车自动驾驶的高精度定位。

请参阅图1所示，图1是本发明提供的一种自动驾驶汽车的定位系统的逻辑框图。在本发明的一个实施例中，自动驾驶汽车的定位系统可包括数据获取模块100、语义定位模块200、数据校验模块300以及数据滤波模块400。其中，数据获取模块100可用于获取汽车的组合惯导数据、环境感知数据和汽车所在位置的地图数据。语义定位模块200可用于根据组合惯导数据、环境感知数据和地图数据，生成汽车的语义位姿数据。数据校验模块300可用于对组合惯导数据和环境感知数据进行校验处理，以生成汽车的定位校验数据。数据滤波模块400可用于根据组合惯导数据、语义位姿数据和定位校验数据，生成汽车的融合位姿数据。

在本发明的一个实施例中，组合惯导数据可包括由组合惯导设备提供的卫星定位数据、惯性测量数据(包括三轴角速度数据、三轴加速度数据)以及轮速传感器提供的轮速数据。其中，卫星定位数据可通过GNSS(Global Navigation Satellite System，全球导航卫星系统)获取。GNSS信号易受到时钟误差、轨道误差、电离层延迟和多路径效应的影响，定位精度只能达到米级。本行业通常使用RTK(Real-time kinematic，实时动态)服务，通过差分解算为GNSS信号提供位置补偿，使得空旷场景下的GNSS定位精度达到厘米级。但是，由于城区场景高楼、树木或隧道的遮挡，GNSS信号存在失效的情况。惯性测量数据可通过IMU(Inertial measurement unit，惯性测量单元)获取。IMU通常包括三轴陀螺仪和三轴加速度计，是测量车辆三轴姿态角和加速度的装置，在GNSS信号失效的情况下可以积分推算后续车辆位置，但是误差累积使得定位逐渐发散。本实施例中，通过组合惯导数据来定位汽车，可为汽车提供一个绝对坐标。

在本发明的一个实施例中，环境感知数据可通过环境感知传感器获取，例如相机和激光雷达等。环境感知传感器可获取汽车的相对位置。环境感知传感器存在很多局限性，例如，相机成本低廉、信息量丰富，但是环境适应性较差，易受天气光照影响；激光雷达能提供准确的三维相对位置，但是成本昂贵以致装载激光雷达的车辆无法大规模量产。本实施例中，环境感知数据只包括由前视相机获取的车道线信息。

在本发明的一个实施例中，地图数据可为适用于自动驾驶领域的高精度地图数据。高精度地图数据能够准确和全面地表征道路特征，且要求很高的实时性，其绝对位置精度可接近1m，相对位置精度可达到10cm-20cm。此外，高精度地图数据还记录驾驶行为的具体细节，包括典型驾驶行为、最佳加速点及刹车点、路况复杂程度、以及对不同路段信号接收情况的标注等。

在本发明的一个实施例中，数据获取模块100可通过三个回调函数来获取组合惯导数据、环境感知数据和地图数据。三个回调函数分别为组合惯导数据回调函数、环境感知数据回调函数和地图数据回调函数。三个函数的功能为接收各个传感器数据，并把数据保存到缓存队列。其中，组合惯导数据回调函数接收卫星定位数据、惯性测量数据和轮速数据，数据获取模块100利用第一帧锁定状态好的卫星定位数据确定局部坐标系原点，通过高斯投影将汽车位置从经纬度转换为局部坐标。需要说明的是，为了减少语义定位模块200对卫星定位数据的过度依赖，可按照每隔一秒存一帧卫星定位数据，同时高频存储惯性测量数据，分别保存在语义定位模块200和数据校验模块300的数据队列中。环境感知数据回调函数将感知车道线重构为点云形式，为后续与地图数据中高精地图车道线点云匹配做准备，重构后的数据同样保存在语义定位模块200和数据校验模块300的数据队列中。地图数据回调函数将高精地图车道线数据重构，保存在相应的结构体中，并计算汽车行驶距离，根据行驶距离控制地图接收频率，减少存储时间消耗。

请参阅图1所示，在本发明的一个实施例中，语义定位模块200可包括语义初始化单元201、航迹推算单元202以及语义匹配单元203。数据获取模块100将所获取的数据保存到语义定位模块200的数据队列之后，系统可进入语义定位模块200的处理。

在本发明的一个实施例中，语义初始化单元201可用于根据组合惯导数据、环境感知数据和地图数据，生成汽车的起始点数据。具体的，语义初始化单元201可从语义定位模块200的数据队列中提出数据。此数据队列中的数据包括三个类型，分别为卫星定位数据、惯性测量数据和轮速数据。语义初始化单元201的作用是确定航迹推算的起点，因此选点条件较为严厉。基于相关策略，语义初始化单元201可根据组合惯导数据、环境感知数据和地图数据，生成起始点数据，即确定航迹推算的起点。当起点确定后，可维护一个数据缓存区，用于存放后续航迹推算和语义匹配处理的定位数据，至此，语义定位模块初始化完成。

在本发明的一个实施例中，航迹推算单元202可用于基于起始点数据，对组合惯导数据和环境感知数据进行计算处理，以生成汽车的位姿推算数据。具体的航迹推算单元202以语义初始化单元201确定的起点，对数据缓存区中的后续数据(包括组合惯导数据和环境感知数据)进行积分处理，可生成位姿推算数据。位姿推算数据包括根据所有类型的数据获得的位姿信息。本实施例中，上述积分处理满足以下公式：

p_cur＝p_last+R_last·v·Δt

R_cur＝R_last·exp((ω·Δt)^∧)

其中，p_cur为当前帧的位置数据，p_last为上一帧的位置数据，R_cur为当前帧的姿态数据，R_last为上一帧的姿态数据，v为轮速，ω为角速度，Δt为两帧时间间隔数据。

在本发明的一个实施例中，语义匹配单元203可用于基于环境感知数据和地图数据，对位姿推算数据进行匹配处理，以生成语义位姿数据。具体的，语义类型的数据通过航迹推算获得位姿推算数据后，需进入语义匹配单元203进行优化。语义匹配单元203利用环境感知数据中的车道线信息和地图数据中的车道线信息进行ICP(Iterative ClosestPoint，迭代最近点)匹配，获得优化后的语义位姿数据。ICP算法是一种点集对点集配准方法。本实施例中，匹配优化所使用的优化库为g2o；节点为当前帧航迹推算得到的位姿数据；约束边有两条，分别为距离约束和位姿约束。数据缓存区后续数据在此帧语义位姿数据的基础上继续做航迹推算。语义匹配单元203输出语义位姿数据，可作为后续数据滤波模块400的输入。

请参阅图1所示，在本发明的一个实施例中，数据校验模块300可用于对组合惯导数据和环境感知数据进行校验处理，以生成汽车的定位校验数据。具体的，数据校验模块300的主要作用为筛选出状态好的卫星定位数据，以在后续数据滤波模块400中作为观测量使用。数据校验模块300的数据队列中保存的数据包括组合惯导数据中的卫星定位数据和环境感知数据。定位校验数据可为附带协方差的卫星定位观测量，协方差可通过前视帧和地图数据在世界坐标系下语义匹配得来的。需要说明的是，前视帧为汽车前视相机所获取一帧图片，图片包含了车道线信息。数据校验模块300首先对卫星定位数据插值处理，可得到位姿数据来为前视帧赋值；然后计算前视帧在位姿数据下的语义匹配结果，该位姿数据的协方差与匹配误差正相关，进而实现由前视帧中的车道线从前视相机坐标系转换到世界坐标系下，与地图车道线数据匹配。

请参阅图1所示，在本发明的一个实施例中，数据滤波模块400可包括滤波初始化单元401、预测单元402以及更新单元403。由于语义定位模块200受前视相机感知效果影响较大，当前视相机感知效果变差或丢失时，容易引起语义位姿数据的抖动。为了避免上述数据抖动，保证系统输出一条平滑的位姿轨迹，故需数据滤波模块400对语义位姿数据进行过滤优化处理。

在本发明的一个实施例中，滤波初始化单元401可用于根据语义位姿数据和定位校验数据，生成汽车的初始位姿数据。具体的，数据滤波模块400通过多个回调函数分别获取语义位姿数据、组合惯导数据和定位校验数据。滤波初始化单元401根据语义位姿数据和定位校验数据进行初始化处理，以生成初始位姿数据。

在本发明的一个实施例中，预测单元402可用于根据初始位姿数据和组合惯导数据，生成汽车的预测位姿数据。具体的，预测单元402基于滤波初始化单元401生成的初始位姿数据以及组合惯导数据进行位姿预测，生成预测位姿数据。

在本发明的一个实施例中，更新单元403可用于根据预测位姿数据、语义位姿数据和定位校验数据，生成融合位姿数据。具体的，更新单元403对预测位姿数据、语义位姿数据和定位校验数据，对目标位姿进行最优估计计算，生成自动驾驶汽车的融合位姿数据。本实施例中，所使用的最优估计算法可为卡尔曼滤波算法。卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。卡尔曼滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。卡尔曼滤波可利用数据校验模块300生成的定位校验数据(带协方差的卫星定位观测量)，从一系列存在测量噪声的数据中，估计动态系统的状态。卡尔曼滤波可使本系统能够通过计算机编程实现，并能够对现场采集的数据进行实时的更新和处理。

请参阅图2所示，本发明提供了一种自动驾驶汽车的定位方法，可应用在汽车自动驾驶融合定位技术领域，例如上述自动驾驶汽车的定位系统。自动驾驶汽车的定位方法可包括有如下的步骤：

步骤S201、获取汽车的组合惯导数据、环境感知数据和汽车所在位置的地图数据。

在本发明的一个实施例中，当执行步骤S201时，即获取汽车的组合惯导数据、环境感知数据和汽车所在位置的地图数据。具体的，本步骤可通过数据获取模块100，使用三个回调函数来获取组合惯导数据、环境感知数据和地图数据。三个回调函数分别为组合惯导数据回调函数、环境感知数据回调函数和地图数据回调函数。三个函数的功能为接收各个传感器数据，并把数据保存到缓存队列。其中，组合惯导数据回调函数接收卫星定位数据、惯性测量数据和轮速数据，可通过数据获取模块100利用第一帧锁定状态好的卫星定位数据确定局部坐标系原点，通过高斯投影将汽车位置从经纬度转换为局部坐标。需要说明的是，为了减少步骤S202对卫星定位数据的过度依赖，可按照每隔一秒存一帧卫星定位数据，同时高频存储惯性测量数据，分别保存在语义定位模块200和数据校验模块300的数据队列中。环境感知数据回调函数将感知车道线重构为点云形式，为后续与地图数据中高精地图车道线点云匹配做准备，重构后的数据同样保存在语义定位模块200和数据校验模块300的数据队列中。地图数据回调函数将高精地图车道线数据重构，保存在相应的结构体中，并计算汽车行驶距离，根据行驶距离控制地图接收频率，减少存储时间消耗。

步骤S202、根据组合惯导数据、环境感知数据和地图数据，生成汽车的语义位姿数据。

在本发明的一个实施例中，当执行步骤S202时，即根据组合惯导数据、环境感知数据和地图数据，生成汽车的语义位姿数据。具体的，本步骤首先根据组合惯导数据、环境感知数据和地图数据，生成起始点数据，即确定航迹推算的起点。然后基于起始点数据，对组合惯导数据和环境感知数据进行积分处理，以生成位姿推算数据。最后基于环境感知数据和地图数据，对位姿推算数据进行匹配处理，以生成语义位姿数据。

步骤S203、对组合惯导数据和环境感知数据进行校验处理，以生成汽车的定位校验数据。

在本发明的一个实施例中，当执行步骤S203时，即对组合惯导数据和环境感知数据进行校验处理，以生成汽车的定位校验数据。具体的，本步骤可通过数据校验模块300对组合惯导数据和环境感知数据进行校验处理，以生成定位校验数据。本步骤的主要作用为筛选出状态好的卫星定位数据，以在后续步骤S204中作为观测量使用。数据校验模块300的数据队列中保存的数据包括组合惯导数据中的卫星定位数据和环境感知数据。定位校验数据可为附带协方差的卫星定位观测量，协方差可通过前视帧和地图数据在世界坐标系下语义匹配得来的。需要说明的是，前视帧为汽车前视相机所获取一帧图片，图片包含了车道线信息。本步骤首先对卫星定位数据插值处理，可得到位姿数据来为前视帧赋值；然后计算前视帧在位姿数据下的语义匹配结果，该位姿数据的协方差与匹配误差正相关，进而实现由前视帧中的车道线从前视相机坐标系转换到世界坐标系下，与地图车道线数据匹配。

步骤S204、根据组合惯导数据、语义位姿数据和定位校验数据，生成汽车的融合位姿数据。

在本发明的一个实施例中，当执行步骤S204时，即根据组合惯导数据、语义位姿数据和定位校验数据，生成汽车的融合位姿数据。具体的，可通过数据滤波模块400首先根据语义位姿数据和定位校验数据，生成初始位姿数据；然后再根据初始位姿数据和组合惯导数据，生成预测位姿数据；最后对预测位姿数据、语义位姿数据和定位校验数据，对目标位姿进行最优估计计算，生成自动驾驶汽车的融合位姿数据，即输出一条平滑的位姿轨迹。

请参阅图3所示，图3是图2中步骤S202的一具体实施方式流程示意图。步骤S202可包括如下的步骤流程：

步骤S301、根据组合惯导数据、环境感知数据和地图数据，生成汽车的起始点数据。

在本发明的一个实施例中，当执行步骤S301时，即根据组合惯导数据、环境感知数据和地图数据，生成汽车的起始点数据。具体的，可通过语义初始化单元201根据组合惯导数据、环境感知数据和地图数据，生成起始点数据。语义初始化单元201可从语义定位模块200的数据队列中提出数据。此数据队列中的数据包括三个类型，分别为卫星定位数据、惯性测量数据和轮速数据。步骤S301的作用是确定航迹推算的起点，因此选点条件较为严厉。基于相关策略，可根据组合惯导数据、环境感知数据和地图数据，生成起始点数据，即确定航迹推算的起点。当起点确定后，可维护一个数据缓存区，用于存放后续航迹推算和语义匹配处理的定位数据，至此，语义定位初始化完成。

步骤S302、基于起始点数据，对组合惯导数据和环境感知数据进行计算处理，以生成汽车的位姿推算数据。

在本发明的一个实施例中，当执行步骤S302时，即基于起始点数据，对组合惯导数据和环境感知数据进行计算处理，以生成汽车的位姿推算数据。具体的，可基于起始点数据，对组合惯导数据和环境感知数据进行积分处理，以生成位姿推算数据。具体的本步骤以步骤S301确定的起点，对数据缓存区中的后续数据(包括组合惯导数据和环境感知数据)进行积分处理，可生成位姿推算数据。位姿推算数据包括根据所有类型的数据获得的位姿信息。

步骤S303、基于环境感知数据和地图数据，对位姿推算数据进行匹配处理，以生成语义位姿数据。

在本发明的一个实施例中，当执行步骤S303时，即基于环境感知数据和地图数据，对位姿推算数据进行匹配处理，以生成语义位姿数据。具体的，可通过语义匹配单元203基于环境感知数据和地图数据，对位姿推算数据进行匹配处理，以生成语义位姿数据。语义类型的数据通过航迹推算获得位姿推算数据后，需进入语义匹配单元203进行优化。语义匹配单元203利用环境感知数据中的车道线信息和地图数据中的车道线信息进行ICP(Iterative Closest Point，迭代最近点)匹配，获得优化后的语义位姿数据。ICP算法是一种点集对点集配准方法。本实施例中，匹配优化所使用的优化库为g2o；节点为当前帧航迹推算得到的位姿数据；约束边有两条，分别为距离约束和位姿约束。数据缓存区后续数据在此帧语义位姿数据的基础上继续做航迹推算。本步骤输出语义位姿数据，可作为后续步骤S204的输入。

请参阅图4所示，图4是图3中步骤S204的一具体实施方式流程示意图。步骤S204可包括如下的步骤流程：

步骤S401、根据语义位姿数据和定位校验数据，生成汽车的初始位姿数据。

在本发明的一个实施例中，当执行步骤S401时，即根据语义位姿数据和定位校验数据，生成汽车的初始位姿数据。具体的，可通过滤波初始化单元401根据语义位姿数据和定位校验数据，生成初始位姿数据。步骤S204通过多个回调函数分别获取语义位姿数据、组合惯导数据和定位校验数据。本步骤根据语义位姿数据和定位校验数据进行初始化处理，以生成初始位姿数据。

步骤S402、根据初始位姿数据和组合惯导数据，生成汽车的预测位姿数据。

在本发明的一个实施例中，当执行步骤S402时，即根据初始位姿数据和组合惯导数据，生成汽车的预测位姿数据。具体的，可通过预测单元402根据初始位姿数据和组合惯导数据，生成预测位姿数据。本步骤基于步骤S401生成的初始位姿数据以及组合惯导数据进行位姿预测，生成预测位姿数据。

步骤S403、根据预测位姿数据、语义位姿数据和定位校验数据，生成融合位姿数据。

在本发明的一个实施例中，当执行步骤S403时，即根据预测位姿数据、语义位姿数据和定位校验数据，生成融合位姿数据。具体的，可通过更新单元403根据预测位姿数据、语义位姿数据和定位校验数据，生成融合位姿数据。本步骤对预测位姿数据、语义位姿数据和定位校验数据，对目标位姿进行最优估计计算，生成自动驾驶汽车的融合位姿数据。本实施例中，所使用的最优估计算法可为卡尔曼滤波算法。卡尔曼滤波可利用步骤S203生成的定位校验数据，从一系列存在测量噪声的数据中，估计动态系统的状态。

综上所述，本发明提供一种自动驾驶汽车的定位系统及方法，可应用于汽车自动驾驶融合定位技术领域。本发明所使用的传感器主要包括组合惯导设备和前视相机，具有设备成本低的优点，易于实现大规模量产。同时，本发明还对多传感器的定位数据进行滤波融合，可输出一条平滑的位姿轨迹，解决了城区道路场景下车辆定位不佳的问题，实现汽车的高精度定位。

上述实施例仅示例性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，但凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (10)

1.一种自动驾驶汽车的定位系统，其特征在于，包括：

数据获取模块，用于获取汽车的组合惯导数据、环境感知数据和所述汽车所在位置的地图数据；

语义定位模块，用于根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的语义位姿数据；

数据校验模块，用于对所述组合惯导数据和环境感知数据进行校验处理，以生成所述汽车的定位校验数据；以及

数据滤波模块，用于根据所述组合惯导数据、所述语义位姿数据和所述定位校验数据，生成所述汽车的融合位姿数据。

2.根据权利要求1所述的自动驾驶汽车的定位系统，其特征在于，所述数据获取模块包括：

语义初始化单元，用于根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的起始点数据；以及

航迹推算单元，用于基于所述起始点数据，对所述组合惯导数据和所述环境感知数据进行计算处理，以生成所述汽车的位姿推算数据。

3.根据权利要求2所述的自动驾驶汽车的定位系统，其特征在于，所述数据获取模块还包括：

语义匹配单元，用于基于所述环境感知数据和所述地图数据，对所述位姿推算数据进行匹配处理，以生成所述语义位姿数据。

4.根据权利要求2所述的自动驾驶汽车的定位系统，其特征在于，所述积分处理满足以下公式：

p_cur＝p_last+R_last·v·Δt

R_cur＝R_last·exp((ω·Δt)^)

其中，p_cur为当前帧的位置数据，p_last为上一帧的位置数据，R_cur为当前帧的姿态数据，R_last为上一帧的姿态数据，v为轮速，ω为角速度，Δt为两帧时间间隔数据。

5.根据权利要求1所述的自动驾驶汽车的定位系统，其特征在于，所述数据滤波模块包括：

滤波初始化单元，用于根据所述语义位姿数据和所述定位校验数据，生成所述汽车的初始位姿数据；以及

预测单元，用于根据所述初始位姿数据和所述组合惯导数据，生成所述汽车的预测位姿数据。

6.根据权利要求5所述的自动驾驶汽车的定位系统，其特征在于，所述数据滤波模块还包括：

更新单元，用于根据所述预测位姿数据、所述语义位姿数据和所述定位校验数据，生成所述融合位姿数据。

7.根据权利要求1所述的自动驾驶汽车的定位系统，其特征在于，所述组合惯导数据包括所述汽车的卫星定位数据、惯性测量数据和轮速数据。

8.一种自动驾驶汽车的定位方法，其特征在于，包括：

获取汽车的组合惯导数据、环境感知数据和所述汽车所在位置的地图数据；

根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的语义位姿数据；

对所述组合惯导数据和环境感知数据进行校验处理，以生成所述汽车的定位校验数据；以及

根据所述组合惯导数据、所述语义位姿数据和所述定位校验数据，生成所述汽车的融合位姿数据。

9.根据权利要求8所述的自动驾驶汽车的定位方法，其特征在于，所述根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成语义位姿数据的步骤，包括：

根据所述组合惯导数据、所述环境感知数据和所述地图数据，生成所述汽车的起始点数据；

基于所述起始点数据，对所述组合惯导数据和所述环境感知数据进行计算处理，以生成所述汽车的位姿推算数据；以及

基于所述环境感知数据和所述地图数据，对所述位姿推算数据进行匹配处理，以生成所述语义位姿数据。

10.根据权利要求9所述的自动驾驶汽车的定位方法，其特征在于，所述根据所述组合惯导数据、所述语义位姿数据和所述定位校验数据，生成融合位姿数据的步骤，包括：

根据所述语义位姿数据和所述定位校验数据，生成所述汽车的初始位姿数据；

根据所述初始位姿数据和所述组合惯导数据，生成所述汽车的预测位姿数据；以及

根据所述预测位姿数据、所述语义位姿数据和所述定位校验数据，生成所述融合位姿数据。

Images