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

权利要求书

1.一种基于粒子滤波器的车辆自定位方法，其特征在于包括以下步骤：

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

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

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

2.如权利要求1所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述直接量测模型基于GPS和IMU融合形成的组合惯导，其输出为频率与IMU采样频率相同的车辆定位信息；所述设计量测模型基于视觉传感器，其输出为自车距离相邻左右车道线的横向截距。

3.如权利要求1所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述基于采集的实车数据对预先构建的车辆运动系统模型进行训练的方法，包括以下步骤：

构建车辆运动系统模型，并定义该车辆运动系统模型的输入和输出；

对训练过程中采用的损失函数进行定义；

对预先获取的实车数据进行处理，并将处理后的数据作为训练集，基于确定的损失函数对构建的车辆运动系统模型进行训练，得到训练好的车辆运动系统模型。

4.如权利要求3所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述车辆运动系统模型的输入为：连续n个采样时刻的时间t₀，t₁，...，t_n-1，以及对应时刻观测得到的车辆位置横坐标x、车辆位置纵坐标y、车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a；所述车辆运动系统模型的输出为：下一时刻车辆位置和车辆朝向的预测。

5.如权利要求3所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述对预先获取的实车数据进行预处理时，包括：

首先，将车辆惯性导航系统输出的基于wgs84坐标系下的经纬度坐标转换到UTM坐标系；

然后，在一段从t₀持续到t_K的数据中，从t_i，i∈[0，K-n]开始连续n个采样时刻t_i，t_i+1，...，t_i+n-1，将各采样时刻到t_i时刻的时间间隔来作为其采样时刻，以t_i时刻自车的位置为坐标原点，更新各时刻的自车位置，结合车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a，得到一个输入片段；重复上述步骤，从一段从t₀持续到t_K的数据中，得到K-n+1个用于训练的片段。

6.如权利要求1所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动系统模型和量测模型对自车位姿进行估计，实现对车辆的定位的方法，包括以下步骤：

①判断采样时长是否满足车辆运动系统模型的输入要求，如果不满足，则进入步骤②，否则进入步骤③；

②当采样时刻数小于n时，利用组合惯导的观测量来生成粒子；

③当采样时刻数大于等于n时，将自车的观测量输入到车辆运动系统模型中得到对下一时刻车辆状态的预测，并基于下一时刻车辆状态的预测生成粒子；

④利用步骤②或步骤③得到的粒子，结合视觉传感器获取到的矢量高精度地图M计算得到粒子距左、右相邻车道的横向截距Inc^left和Inc^right，并基于粒子距左、右相邻车道的横向截距Inc^left和Inc^right对粒子权重进行更新；

⑤计算得到粒子的权重后对粒子的权重进行归一化处理，将所有粒子的状态加权求和得到对自车位姿的估计。