[发明专利]基于粒子滤波器的车辆自定位方法、系统、设备和介质

说明书

本发明涉及一种基于粒子滤波器的车辆自定位方法、系统、设备和介质，包括以下步骤：设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；基于采集的实车数据对预先构建的车辆运动系统模型进行训练，得到训练好的车辆运行系统模型；基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动系统模型和量测模型对自车位姿进行估计，实现对车辆的定位。本发明引入视觉模块观测的自车到左、右车道线的横向截距作为额外观测量，同时可以对自车的位置及朝向进行估计。本发明可以广泛应用于车辆自定位领域。

技术领域

本发明涉及车辆自定位领域，具体涉及一种基于量产车中传感器(全球导航卫星系统、惯性测量单元以及视觉传感器)使用粒子滤波器对车辆进行自定位的方法、系统、设备和存储介质。

背景技术

车辆的精确自定位是实现L3(Level 3)级和更高级别的自动驾驶的前提，当前较为成熟的车辆高精度自定位方案主要有以下四种：1.GNSS(Global Navigation SatelliteSystem，全球导航卫星系统)结合IMU(Inertial Measurement Unit，惯性测量单元)组成的组合惯性导航定位系统；2.组合惯性导航融合RTK(Real-Time Kinematic)的融合定位系统；3.激光雷达结合点云高精度地图的匹配定位系统；4.视觉相机结合高精度地图的匹配定位系统。然而，由于前三种方案中要使用到的高精度的IMU、结合RTK技术的组合惯性导航以及激光雷达的成本高昂，许多汽车制造商都尽量避免在量产车上使用，所以这些方案大多数只停留于研究阶段，没有在量产车型上的实际应用。而方案4中使用的视觉传感器价格低廉而且可以获得丰富的语义信息和视觉信息，与高精度地图匹配后可以进行高精度定位，同时高精度地图的使用成本低廉，因此方案4非常适用于量产车型。

当前视觉相机结合高精度地图的匹配定位方法可以分为基于优化的方法和基于滤波的方法，下面分别进行介绍：

基于优化的方法是寻找一个视觉检测特征与地图特征之间差异最小的自车位姿，该方法首先需要从视觉信息中提取到大量视觉特征点，然后将提取到的视觉特征点与点云地图进行匹配，不断迭代计算两幅点云之间的位姿关系进而求解车辆的位置和姿态。然而提取视觉特征点大多使用到深度学习的方法，而对大数目点云进行迭代求解，这两个步骤都有较大的算力需求。

基于滤波的方法是根据车辆运动模型得到自车位姿的先验概率分布，得到观测结果后计算得到自车位姿的后验概率分布，进而对自车位姿进行估计。基于滤波的方法计算量小，然而当前流行的卡尔曼滤波算法只适用于线性观测模型的情况，但是基于视觉传感器构建的观测模型往往是非线性的，蔡豪等人使用卡尔曼滤波算法，基于视觉传感器构建出的线性观测模型，只能对车辆的位置进行估计，不能对车辆的姿态进行估计。另外，基于滤波的方法需要构建车辆的运动模型，当前普遍使用匀速匀角速度(CVCT)模型或匀加速度模型等基于运动学的运动模型，但是当采样间隔增大时存在运动模型失效的问题，例如JaeKyu Suhr提出的视觉匹配地图定位方法将滤波频率设置为100Hz，然而很多视觉传感器的采样频率约为30Hz，因此车辆运动模型失效问题也是算法实际应用的一个障碍。

发明内容

针对上述问题，本发明的目的是提供一种基于粒子滤波器的车辆自定位方法、系统、设备和介质，利用量产车中常见传感器(GNSS、IMU和相机)结合矢量高精度地图进行车辆的高精度定位。

为实现上述目的，本发明采取以下技术方案：

本发明的第一个方面，是提供一种基于粒子滤波器的车辆自定位方法，其包括以下步骤：

设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；

基于采集的实车数据对预先构建的车辆运动系统模型进行训练，得到训练好的车辆运行系统模型；

基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动系统模型和量测模型对自车位姿进行估计，实现对车辆的定位。