本发明专利技术涉及一种基于粒子滤波器的车辆自定位方法、系统、设备和介质，包括以下步骤：设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；基于采集的实车数据对预先构建的车辆运动系统模型进行训练，得到训练好的车辆运行系统模型；基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动系统模型和量测模型对自车位姿进行估计，实现对车辆的定位。本发明专利技术引入视觉模块观测的自车到左、右车道线的横向截距作为额外观测量，同时可以对自车的位置及朝向进行估计。本发明专利技术可以广泛应用于车辆自定位领域。发明专利技术可以广泛应用于车辆自定位领域。发明专利技术可以广泛应用于车辆自定位领域。
【技术实现步骤摘要】
基于粒子滤波器的车辆自定位方法、系统、设备和介质
[0001]本专利技术涉及车辆自定位领域，具体涉及一种基于量产车中传感器(全球导航卫星系统、惯性测量单元以及视觉传感器)使用粒子滤波器对车辆进行自定位的方法、系统、设备和存储介质。
技术介绍
[0002]车辆的精确自定位是实现L3(Level 3)级和更高级别的自动驾驶的前提，当前较为成熟的车辆高精度自定位方案主要有以下四种：1.GNSS(Global Navigation Satellite System，全球导航卫星系统)结合IMU(Inertial Measurement Unit，惯性测量单元)组成的组合惯性导航定位系统；2.组合惯性导航融合RTK(Real
‑
Time Kinematic)的融合定位系统；3.激光雷达结合点云高精度地图的匹配定位系统；4.视觉相机结合高精度地图的匹配定位系统。然而，由于前三种方案中要使用到的高精度的IMU、结合RTK技术的组合惯性导航以及激光雷达的成本高昂，许多汽车制造商都尽量避免在量产车上使用，所以这些方案大多数只停留于研究阶段，没有在量产车型上的实际应用。而方案4中使用的视觉传感器价格低廉而且可以获得丰富的语义信息和视觉信息，与高精度地图匹配后可以进行高精度定位，同时高精度地图的使用成本低廉，因此方案4非常适用于量产车型。
[0003]当前视觉相机结合高精度地图的匹配定位方法可以分为基于优化的方法和基于滤波的方法，下面分别进行介绍：
[0004]基于优化的方法是寻找一个视觉检测特征与地图特征之间差异最小的自车位姿，该方法首先需要从视觉信息中提取到大量视觉特征点，然后将提取到的视觉特征点与点云地图进行匹配，不断迭代计算两幅点云之间的位姿关系进而求解车辆的位置和姿态。然而提取视觉特征点大多使用到深度学习的方法，而对大数目点云进行迭代求解，这两个步骤都有较大的算力需求。
[0005]基于滤波的方法是根据车辆运动模型得到自车位姿的先验概率分布，得到观测结果后计算得到自车位姿的后验概率分布，进而对自车位姿进行估计。基于滤波的方法计算量小，然而当前流行的卡尔曼滤波算法只适用于线性观测模型的情况，但是基于视觉传感器构建的观测模型往往是非线性的，蔡豪等人使用卡尔曼滤波算法，基于视觉传感器构建出的线性观测模型，只能对车辆的位置进行估计，不能对车辆的姿态进行估计。另外，基于滤波的方法需要构建车辆的运动模型，当前普遍使用匀速匀角速度(CVCT)模型或匀加速度模型等基于运动学的运动模型，但是当采样间隔增大时存在运动模型失效的问题，例如Jae Kyu Suhr提出的视觉匹配地图定位方法将滤波频率设置为100Hz，然而很多视觉传感器的采样频率约为30Hz，因此车辆运动模型失效问题也是算法实际应用的一个障碍。
技术实现思路
[0006]针对上述问题，本专利技术的目的是提供一种基于粒子滤波器的车辆自定位方法、系统、设备和介质，利用量产车中常见传感器(GNSS、IMU和相机)结合矢量高精度地图进行车
辆的高精度定位。
[0007]为实现上述目的，本专利技术采取以下技术方案：
[0008]本专利技术的第一个方面，是提供一种基于粒子滤波器的车辆自定位方法，其包括以下步骤：
[0009]设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；
[0010]基于采集的实车数据对预先构建的车辆运动系统模型进行训练，得到训练好的车辆运行系统模型；
[0011]基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动系统模型和量测模型对自车位姿进行估计，实现对车辆的定位。
[0012]优选地，所述直接量测模型基于GPS和IMU融合形成的组合惯导，其输出为频率与IMU采样频率相同的车辆定位信息；所述设计量测模型基于视觉传感器，其输出为自车距离相邻左右车道线的横向截距。
[0013]优选地，所述基于采集的实车数据对预先构建的车辆运动系统模型进行训练的方法，包括以下步骤：
[0014]构建车辆运动系统模型，并定义该车辆运动系统模型的输入和输出；
[0015]对训练过程中采用的损失函数进行定义；
[0016]对预先获取的实车数据进行处理，并将处理后的数据作为训练集，基于确定的损失函数对构建的车辆运动系统模型进行训练，得到训练好的车辆运动系统模型。
[0017]优选地，所述车辆运动系统模型的输入为：连续n个采样时刻的时间t0，t1，...，t
n
‑1，以及对应时刻观测得到的车辆位置横坐标x、车辆位置纵坐标y、车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a；所述车辆运动系统模型的输出为：下一时刻车辆位置和车辆朝向的预测。
[0018]优选地，所述对预先获取的实车数据进行预处理时，包括：
[0019]将车辆惯性导航系统输出的基于wgs84坐标系下的经纬度坐标转换到UTM坐标系；
[0020]在一段从t0持续到t
K
的数据中，从t
i
，i∈[0，K
‑
n]开始连续n个采样时刻t
i
，t
i+1
，...，t
i+n
‑1，将各采样时刻到t
i
时刻的时间间隔来作为其采样时刻，以t
i
时刻自车的位置为坐标原点，更新各时刻的自车位置，结合车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a，得到一个输入片段；
[0021]重复上述步骤，从一段从t0持续到t
K
的数据中，得到K
‑
n+1个用于训练的片段。
[0022]优选地，所述基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动系统模型和量测模型对自车位姿进行估计，实现对车辆的定位的方法，包括以下步骤：
[0023]①
判断采样时长是否满足车辆运动系统模型的输入要求，如果不满足，则进入步骤
②
，否则进入步骤
③
；
[0024]②
当采样时刻数小于n时，利用组合惯导的观测量来生成粒子；
[0025]③
当采样时刻数大于等于n时，将自车的观测量输入到车辆运动系统模型中得到对下一时刻车辆状态的预测，并基于下一时刻车辆状态的预测生成粒子；
[0026]④
利用步骤
②
或步骤
③
得到的粒子，结合视觉传感器获取到的矢量高精度地图M计算得到粒子距左、右相邻车道的横向截距Inc
left
和Inc
right
，并基于粒子距左、右相邻车道的横向截距Inc
left
和Inc
right
对粒子权重进行更新；
[0027]⑤
计算得到粒子的权重后对粒子的权重进行归一化处理，将所有粒子的状态加权求和得到对自车位姿的估计。
[0028]本发本文档来自技高网...
【技术保护点】
【技术特征摘要】
1.一种基于粒子滤波器的车辆自定位方法，其特征在于包括以下步骤：设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；基于采集的实车数据对预先构建的车辆运动系统模型进行训练，得到训练好的车辆运行系统模型；基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动系统模型和量测模型对自车位姿进行估计，实现对车辆的定位。2.如权利要求1所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述直接量测模型基于GPS和IMU融合形成的组合惯导，其输出为频率与IMU采样频率相同的车辆定位信息；所述设计量测模型基于视觉传感器，其输出为自车距离相邻左右车道线的横向截距。3.如权利要求1所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述基于采集的实车数据对预先构建的车辆运动系统模型进行训练的方法，包括以下步骤：构建车辆运动系统模型，并定义该车辆运动系统模型的输入和输出；对训练过程中采用的损失函数进行定义；对预先获取的实车数据进行处理，并将处理后的数据作为训练集，基于确定的损失函数对构建的车辆运动系统模型进行训练，得到训练好的车辆运动系统模型。4.如权利要求3所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述车辆运动系统模型的输入为：连续n个采样时刻的时间t0，t1，...，t
n
‑1，以及对应时刻观测得到的车辆位置横坐标x、车辆位置纵坐标y、车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a；所述车辆运动系统模型的输出为：下一时刻车辆位置和车辆朝向的预测。5.如权利要求3所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述对预先获取的实车数据进行预处理时，包括：首先，将车辆惯性导航系统输出的基于wgs84坐标系下的经纬度坐标转换到UTM坐标系；然后，在一段从t0持续到t
K
的数据中，从t
i
，i∈[0，K
‑
n]开始连续n个采样时刻t
i
，t
i+1
，...，t
i+n
‑1，将各采样时刻到t
i
时刻的时间间隔来作为其采样时刻，以t
i
时刻自车的位置为坐标原点，更新各时刻的自车位置，结合车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a，得到一个输入片段；重复上述步骤，从一段从t0持续到t
K
的数据中，得到K
‑
n+1个用于训练的片段。6.如权...
【专利技术属性】
技术研发人员：江昆，杨殿阁，冯润泽，于伟光，
申请(专利权)人：清华大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。