CN113483769A - 基于粒子滤波器的车辆自定位方法、***、设备和介质 - Google Patents

基于粒子滤波器的车辆自定位方法、***、设备和介质 Download PDF

Info

Publication number
CN113483769A
CN113483769A CN202110941721.2A CN202110941721A CN113483769A CN 113483769 A CN113483769 A CN 113483769A CN 202110941721 A CN202110941721 A CN 202110941721A CN 113483769 A CN113483769 A CN 113483769A
Authority
CN
China
Prior art keywords
vehicle
model
self
motion system
particle filter
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110941721.2A
Other languages
English (en)
Other versions
CN113483769B (zh
Inventor
江昆
杨殿阁
冯润泽
于伟光
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tsinghua University
Original Assignee
Tsinghua University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tsinghua University filed Critical Tsinghua University
Priority to CN202110941721.2A priority Critical patent/CN113483769B/zh
Publication of CN113483769A publication Critical patent/CN113483769A/zh
Application granted granted Critical
Publication of CN113483769B publication Critical patent/CN113483769B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/044—Recurrent networks, e.g. Hopfield networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Biophysics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Biomedical Technology (AREA)
  • Computational Linguistics (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Navigation (AREA)

Abstract

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

Description

基于粒子滤波器的车辆自定位方法、***、设备和介质
技术领域
本发明涉及车辆自定位领域，具体涉及一种基于量产车中传感器(全球导航卫星***、惯性测量单元以及视觉传感器)使用粒子滤波器对车辆进行自定位的方法、***、设备和存储介质。
背景技术
车辆的精确自定位是实现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和相机)结合矢量高精度地图进行车辆的高精度定位。
为实现上述目的，本发明采取以下技术方案：
本发明的第一个方面，是提供一种基于粒子滤波器的车辆自定位方法，其包括以下步骤：
设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；
基于采集的实车数据对预先构建的车辆运动***模型进行训练，得到训练好的车辆运行***模型；
基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动***模型和量测模型对自车位姿进行估计，实现对车辆的定位。
优选地，所述直接量测模型基于GPS和IMU融合形成的组合惯导，其输出为频率与IMU采样频率相同的车辆定位信息；所述设计量测模型基于视觉传感器，其输出为自车距离相邻左右车道线的横向截距。
优选地，所述基于采集的实车数据对预先构建的车辆运动***模型进行训练的方法，包括以下步骤：
构建车辆运动***模型，并定义该车辆运动***模型的输入和输出；
对训练过程中采用的损失函数进行定义；
对预先获取的实车数据进行处理，并将处理后的数据作为训练集，基于确定的损失函数对构建的车辆运动***模型进行训练，得到训练好的车辆运动***模型。
优选地，所述车辆运动***模型的输入为：连续n个采样时刻的时间t0，t1，...，tn-1，以及对应时刻观测得到的车辆位置横坐标x、车辆位置纵坐标y、车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a；所述车辆运动***模型的输出为：下一时刻车辆位置和车辆朝向的预测。
优选地，所述对预先获取的实车数据进行预处理时，包括：
将车辆惯性导航***输出的基于wgs84坐标系下的经纬度坐标转换到UTM坐标系；
在一段从t0持续到tK的数据中，从ti，i∈[0，K-n]开始连续n个采样时刻ti，ti+1，...，ti+n-1，将各采样时刻到ti时刻的时间间隔来作为其采样时刻，以ti时刻自车的位置为坐标原点，更新各时刻的自车位置，结合车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a，得到一个输入片段；
重复上述步骤，从一段从t0持续到tK的数据中，得到K-n+1个用于训练的片段。
优选地，所述基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动***模型和量测模型对自车位姿进行估计，实现对车辆的定位的方法，包括以下步骤：
①判断采样时长是否满足车辆运动***模型的输入要求，如果不满足，则进入步骤②，否则进入步骤③；
②当采样时刻数小于n时，利用组合惯导的观测量来生成粒子；
③当采样时刻数大于等于n时，将自车的观测量输入到车辆运动***模型中得到对下一时刻车辆状态的预测，并基于下一时刻车辆状态的预测生成粒子；
④利用步骤②或步骤③得到的粒子，结合视觉传感器获取到的矢量高精度地图M计算得到粒子距左、右相邻车道的横向截距Incleft和Incright，并基于粒子距左、右相邻车道的横向截距Incleft和Incright对粒子权重进行更新；
⑤计算得到粒子的权重后对粒子的权重进行归一化处理，将所有粒子的状态加权求和得到对自车位姿的估计。
本发明的第二个方面，是提供一种基于粒子滤波器的车辆自定位***，包括：
量测模型设计模块，用于设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；
车辆运动***模型训练模块，用于基于采集的实车数据对预先构建的车辆运动***模型进行训练，得到训练好的车辆运行***模型；
自车位姿估计模块，用于基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动***模型和量测模型对自车位姿进行估计，实现对车辆的定位。
优选地，所述直接量测模型基于GPS和IMU融合形成的组合惯导，该模型输出为频率与IMU采样频率相同的车辆定位信息；所述设计量测模型基于视觉传感器，其模型输出为自车距离相邻左右车道线的横向截距。
本发明的第三个方面，是提供一种处理设备，所述处理设备至少包括处理器和存储器，所述存储器上存储有计算机程序，所述处理器运行所述计算机程序时执行以实现所述基于粒子滤波器的车辆自定位方法的步骤。
本发明的第四个方面，是提供一种计算机存储介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行以实现所述基于粒子滤波器的车辆自定位方法的步骤。
本发明由于采取以上技术方案，其具有以下优点：
1.本发明引入视觉模块观测的自车到左、右车道线的横向截距作为额外观测量，同时可以对自车的位置及朝向进行估计。
2.本发明利用循环神经网络对自车运动***模型进行构建，解决了使用基于运动学的车辆模型作为车辆运动模型在滤波频率低时滤波器失效的问题。
3.本发明使用粒子滤波器算法，适配于非线性的量测模型和自车运动***模型，利用量产车传感器实现了车辆的精确自定位。
因此，本发明可以广范应用于车辆自定位领域。
附图说明
图1是本发明实现车辆自定位的算法流程图；
图2是本发明中所设计的量测模型图；
图3是本发明中用于预测自车状态的网络模型；
图4是本发明中计算粒子到车道线的左、右横向截距的模型图.
具体实施方式
下面结合附图和实施例对本发明进行详细的描述。
本发明提供的一种车辆自定位方法，基于粒子滤波器的算法，使用视觉模块观测的自车到左、右车道线的横向截距作为额外观测量，利用循环神经网络对自车运动***进行建模，在滤波频率低时仍有较好的滤波效果，解决了普通滤波器不适用于非线性量测模型的问题。整体上相较于基于优化的匹配定位算法，计算量小，解决了量产车低成本高精度自定位问题。
实施例1
如图1所示，本实施例提供一种基于粒子滤波器的车辆自定位方法，该方法包括以下步骤：
1)设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型。
如图2所示，直接量测模型基于GPS和IMU融合形成的组合惯导，该直接量测模型的输出为频率与IMU采样频率相同的车辆定位信息(经纬度和朝向)；
设计量测模型基于视觉传感器(在本实施例中为MobileyeTM ADAS相机)，该设计量测模型的输出为自车距离相邻左右车道线的横向截距。
2)基于采集的实车数据对预先构建的车辆运动***模型进行训练，得到训练好的车辆运行***模型，使之能够尽量拟合车辆的实际运动***模型。
具体的，基于采集的实车数据对预先构建的车辆运动***模型进行训练的方法，包括以下步骤：
2.1)构建车辆运动***模型，并定义该车辆运动***模型的输入和输出。
由于RNN(Recurrent Neural Network，循环神经网络)适用于提取序列化信息的特征并预测趋势，因此，本实施例基于RNN网络串联全连接网络构建车辆运动***模型。
如图3所示，在本实施例中，构建的车辆运动***模型使用5层LSTM网络连接2层全连接网络，其中，5层LSTM网络的输入为：连续n个采样时刻的时间t0，t1，...，tn-1，以及对应时刻观测得到的车辆位置横坐标x、车辆位置纵坐标y、车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a，输出为最后一个时刻得到的隐藏层信息ht；2层全连接网络的输入为LSTM网络最后一个时刻得到的隐藏层信息ht，输出为tn时刻车辆位置和车辆朝向的预测。
2.2)对训练过程中采用的损失函数进行定义。
本实施例中，车辆运动***模型的损失函数采用均方损失函数，即
Figure BDA0003215128640000051
式中，y代表网络预测的车辆状态，
Figure BDA0003215128640000052
代表车辆状态的真值；yi为y的第i维分量；
Figure BDA0003215128640000053
为
Figure BDA0003215128640000054
的第i维分量；N为y，
Figure BDA0003215128640000055
的维度，在本实施例中N＝3(横坐标、纵坐标和朝向3个维度)。
2.3)对预先获取的实车数据进行处理，并将处理后的实车数据作为训练集，基于步骤2.2)确定的损失函数对步骤2.1)构建的车辆运动***模型进行训练，得到最终的车辆运动***模型。
对实车数据进行预处理时，方法为：
首先，将车辆惯性导航***输出的基于wgs84坐标系下的经纬度坐标转换到UTM(Universal Transverse Mercator Grid System，通用横墨卡托格网***)坐标系；
然后，在一段从t0持续到tK的数据中，从ti(i∈[0，K-n])开始连续n个采样时刻(ti，ti+1，...，ti+n-1)，将各采样时刻到ti时刻的时间间隔来作为其采样时刻，另外以ti时刻自车的位置为坐标原点，更新各时刻的自车位置，结合车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a，作为网络输入，而将ti+n时刻车辆的位置横、纵坐标和朝向作为预测真值。这样，便可以从一段从t0持续到tK的数据中，得到K-n+1个可用于训练的片段。
3)基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动***模型和量测模型对自车位姿进行估计，实现对车辆的定位。
具体的，包括以下步骤：
3.1)判断采样时长是否满足步骤2)中车辆运动***模型的输入要求，如果不满足，则进入步骤和3.2)，否则进入步骤3.3)；
3.2)算法启动阶段：当采样时刻数小于n时，利用组合惯导的观测量来生成粒子，生成的粒子状态如下：
Figure BDA0003215128640000061
其中，xt，yt，θt分别代表粒子在t时刻的横坐标、纵坐标和朝向角；
Figure BDA0003215128640000062
分别代表组合惯导在t时刻的观测量；
Figure BDA0003215128640000063
分别代表均值为0，方差为
Figure BDA0003215128640000064
Figure BDA0003215128640000065
的高斯噪声。
3.3)算法运行阶段，当采样时刻数大于等于n时，将自车的观测量输入到车辆运动***模型中得到对下一时刻车辆状态的预测，并基于下一时刻车辆状态的预测生成粒子，粒子状态如下：
Figure BDA0003215128640000066
式中，xt，yt，θt分别代表t时刻生成粒子的横坐标、纵坐标和朝向角；
Figure BDA0003215128640000067
θ^tpred分别代表通过车辆运动***模型对t时刻车辆状态的预测。N(0，v2)代表均值为0，方差为v2的高斯噪声；vpos为位置的观测噪声，本实施例中vpos＝1m；vθ为朝向的观测噪声，本实施例中vθ＝0.1rad；本实施例中，生成的粒子数目为P＝400。
3.4)利用步骤3.2)或步骤3.3)得到的粒子状态，结合视觉传感器获取到的矢量高精度地图M计算得到粒子距左、右相邻车道的横向截距Incleft和Incright，并基于粒子距左、右相邻车道的横向截距Incleft和Incright对粒子权重进行更新。
其中，矢量高精度地图M的组成如下：
M＝{L1，L2，...，Ln} (4)
Li＝{P1，P2，...，Pn} (5)
Pj＝{xj，yj} (6)
式中，Li代表第i条车道线，Pj代表同一车道线上的第j个点，Pj的位置为(xj，yj)。
如图4所示，将矢量高精度地图M中的点变换到以粒子的位置为坐标原点和以粒子朝向为纵轴正方向的坐标系下，搜索得到在此坐标系下各象限距离粒子最近的地图点，一、三象限的最近点相连后与横坐标交点到粒子的距离即为左横向截距Incleft，二、四象限的最近点相连后与横坐标交点到粒子的距离即为右横向截距Incright。
得到粒子的状态量后结合设计量测模型的观测量对粒子进行权值更新，第k(k∈[0，K])个粒子的权重可从下式计算得到：
Figure BDA0003215128640000071
式中，vInc代表MobileyeTM ADAS相机对横向截距的观测误差，在本实施例中vInc＝0.1m。
3.5)计算得到粒子的权重后对粒子的权重进行归一化处理，将所有粒子的状态加权求和得到对自车位姿的估计。
实施例2
上述实施例1提供了一种基于粒子滤波器的车辆自定位方法，与之相对应地，本实施例提供一种车辆自定位***。本实施例提供的车辆自定位***可以实施实施例1的基于粒子滤波器的车辆自定位方法，该识别***可以通过软件、硬件或软硬结合的方式来实现。例如，该识别***可以包括集成的或分开的功能模块或功能单元来执行实施例1各方法中的对应步骤。由于本实施例的识别***基本相似于方法实施例，所以本实施例描述过程比较简单，相关之处可以参见实施例1的部分说明即可，本实施例的车辆自定位***的实施例仅仅是示意性的。
本实施例提供的一种基于粒子滤波器的车辆自定位***，其包括：
量测模型设计模块，用于设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；
车辆运动***模型训练模块，用于基于采集的实车数据对预先构建的车辆运动***模型进行训练，得到训练好的车辆运行***模型；
自车位姿估计模块，用于基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动***模型和量测模型对自车位姿进行估计，实现对车辆的定位。
优选地，量测模型设计模块中，直接量测模型基于GPS和IMU融合形成的组合惯导，该模型输出为频率与IMU采样频率相同的车辆定位信息；设计量测模型基于视觉传感器，其模型输出为自车距离相邻左右车道线的横向截距。
优选地，车辆运动***模型训练模块，包括：
模型构建模块，用于构建车辆运动***模型，并定义该车辆运动***模型的输入和输出；
损失函数定义模块，用于对训练过程中采用的损失函数进行定义；
模型训练模块，用于对预先获取的实车数据进行处理，并将处理后的数据作为训练集，基于确定的损失函数和超参数对构建的车辆运动***模型进行训练，得到最终的车辆运动模型。
优选地，自车位姿估计模块，包括：
采样时长判断模块，用于判断采样时长是否满足车辆运动***模型的输入要求，如果不满足则发送信号到第一粒子生成模块，否则发送信号到第二粒子生成模块；
第一粒子生成模块，用于利用组合惯导的观测量来生成粒子；
第二粒子生成模块，用于将自车的观测量输入到车辆运动***模型中得到对下一时刻车辆状态的预测，并基于下一时刻车辆状态的预测生成粒子；
粒子权重更新模块，用于利用得到的粒子，结合矢量高精度地图M计算得到粒子距左、右相邻车道的横向截距Incleft和Incright，并基于粒子距左、右相邻车道的横向截距Incleft和Incright对粒子权重进行更新；
估计模块，用于计算得到粒子的权重后对粒子的权重进行归一化处理，将所有粒子的状态加权求和得到对自车位姿的估计。
实施例3
本实施例提供一种与本实施例1所提供的基于粒子滤波器的车辆自定位方法对应的处理设备，处理设备可以是用于客户端的处理设备，例如手机、笔记本电脑、平板电脑、台式机电脑等，以执行实施例1的基于粒子滤波器的车辆自定位方法。
所述处理设备包括处理器、存储器、通信接口和总线，处理器、存储器和通信接口通过总线连接，以完成相互间的通信。存储器中存储有可在所述处理器上运行的计算机程序，所述处理器运行所述计算机程序时执行本实施例1所提供的车辆自定位方法。
在一些实现中，存储器可以是高速随机存取存储器(RAM：Random AccessMemory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。
在另一些实现中，处理器可以为中央处理器(CPU)、数字信号处理器(DSP)等各种类型通用处理器，在此不做限定。
实施例4
本实施例1的车辆自定位方法可被具体实现为一种计算机程序产品，计算机程序产品可以包括计算机可读存储介质，其上载有用于执行本实施例1所述的基于粒子滤波器的车辆自定位方法的计算机可读程序指令。
计算机可读存储介质可以是保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意组合。
实施例5
本实施例结合实车数据，对本发明提出的基于相机和矢量高精度地图的车辆自定位方法与传统粒子滤波器进行性能评测。
传统的粒子滤波定位方法仅使用自车位置和朝向作为观测量，使用匀速匀角速度模型作为车辆运动模型。结合一段实车采集数据，采用本实施例中参数后，在滤波频率为10Hz的情况下对本算法和传统粒子滤波算法进行比较，实验结果(如表1)表明在观测的平均绝对距离误差为1.25m时，传统粒子滤波算法的平均绝对距离误差为1.75m，而本算法的平均角度距离误差为0.39m；在观测的平均绝对角度误差为0.08rad时，传统粒子滤波算法的平均绝对距离误差为0.18rad，而本算法的平均角度距离误差为0.07rad。实验结果表明在采样频率为10Hz的情况下传统滤波器的误差大于直接观测误差，已经失效，但是本算法在定位精度上较原始观测有较大的提升。
表1本算法与传统粒子滤波算法性能比较
平均绝对距离误差(m) 平均绝对角度误差(rad)
观测量 1.25 0.08
传统粒子滤波算法 1.75 0.18
本发明算法 0.39 0.07
需要说明的是，附图中的流程图和框图显示了根据本申请的多个实施例的***、方法和计算机程序产品的可能实现的体系架构、功能和操作。流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。
上述各实施例仅用于说明本发明，其中各部件的结构、连接方式和制作工艺等都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变换和改进，均不应排除在本发明的保护范围之外。

Claims (10)

1.一种基于粒子滤波器的车辆自定位方法，其特征在于包括以下步骤：
设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；
基于采集的实车数据对预先构建的车辆运动***模型进行训练，得到训练好的车辆运行***模型；
基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动***模型和量测模型对自车位姿进行估计，实现对车辆的定位。
2.如权利要求1所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述直接量测模型基于GPS和IMU融合形成的组合惯导，其输出为频率与IMU采样频率相同的车辆定位信息；所述设计量测模型基于视觉传感器，其输出为自车距离相邻左右车道线的横向截距。
3.如权利要求1所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述基于采集的实车数据对预先构建的车辆运动***模型进行训练的方法，包括以下步骤：
构建车辆运动***模型，并定义该车辆运动***模型的输入和输出；
对训练过程中采用的损失函数进行定义；
对预先获取的实车数据进行处理，并将处理后的数据作为训练集，基于确定的损失函数对构建的车辆运动***模型进行训练，得到训练好的车辆运动***模型。
4.如权利要求3所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述车辆运动***模型的输入为：连续n个采样时刻的时间t0，t1，...，tn-1，以及对应时刻观测得到的车辆位置横坐标x、车辆位置纵坐标y、车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a；所述车辆运动***模型的输出为：下一时刻车辆位置和车辆朝向的预测。
5.如权利要求3所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述对预先获取的实车数据进行预处理时，包括：
首先，将车辆惯性导航***输出的基于wgs84坐标系下的经纬度坐标转换到UTM坐标系；
然后，在一段从t0持续到tK的数据中，从ti，i∈[0，K-n]开始连续n个采样时刻ti，ti+1，...，ti+n-1，将各采样时刻到ti时刻的时间间隔来作为其采样时刻，以ti时刻自车的位置为坐标原点，更新各时刻的自车位置，结合车辆朝向与正北方向的夹角θ、车辆速度v以及车辆加速度沿车头方向的分量a，得到一个输入片段；重复上述步骤，从一段从t0持续到tK的数据中，得到K-n+1个用于训练的片段。
6.如权利要求1所述的一种基于粒子滤波器的车辆自定位方法，其特征在于：所述基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动***模型和量测模型对自车位姿进行估计，实现对车辆的定位的方法，包括以下步骤：
①判断采样时长是否满足车辆运动***模型的输入要求，如果不满足，则进入步骤②，否则进入步骤③；
②当采样时刻数小于n时，利用组合惯导的观测量来生成粒子；
③当采样时刻数大于等于n时，将自车的观测量输入到车辆运动***模型中得到对下一时刻车辆状态的预测，并基于下一时刻车辆状态的预测生成粒子；
④利用步骤②或步骤③得到的粒子，结合视觉传感器获取到的矢量高精度地图M计算得到粒子距左、右相邻车道的横向截距Incleft和Incright，并基于粒子距左、右相邻车道的横向截距Incleft和Incright对粒子权重进行更新；
⑤计算得到粒子的权重后对粒子的权重进行归一化处理，将所有粒子的状态加权求和得到对自车位姿的估计。
7.一种基于粒子滤波器的车辆自定位***，其特征在于，包括：
量测模型设计模块，用于设计粒子滤波器的量测模型，该量测模型包括直接量测模型和设计量测模型；
车辆运动***模型训练模块，用于基于采集的实车数据对预先构建的车辆运动***模型进行训练，得到训练好的车辆运行***模型；
自车位姿估计模块，用于基于组合惯导和视觉传感器对车辆运动状态进行采集，并基于车辆运动***模型和量测模型对自车位姿进行估计，实现对车辆的定位。
8.如权利要求7所述的一种基于粒子滤波器的车辆自定位***，其特征在于：所述直接量测模型基于GPS和IMU融合形成的组合惯导，该模型输出为频率与IMU采样频率相同的车辆定位信息；所述设计量测模型基于视觉传感器，其模型输出为自车距离相邻左右车道线的横向截距。
9.一种处理设备，所述处理设备至少包括处理器和存储器，所述存储器上存储有计算机程序，其特征在于，所述处理器运行所述计算机程序时执行以实现权利要求1到6任一项所述基于粒子滤波器的车辆自定位方法的步骤。
10.一种计算机存储介质，其特征在于，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行以实现根据权利要求1到6任一项所述基于粒子滤波器的车辆自定位方法的步骤。
CN202110941721.2A 2021-08-17 2021-08-17 基于粒子滤波器的车辆自定位方法、***、设备和介质 Active CN113483769B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110941721.2A CN113483769B (zh) 2021-08-17 2021-08-17 基于粒子滤波器的车辆自定位方法、***、设备和介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110941721.2A CN113483769B (zh) 2021-08-17 2021-08-17 基于粒子滤波器的车辆自定位方法、***、设备和介质

Publications (2)

Publication Number Publication Date
CN113483769A true CN113483769A (zh) 2021-10-08
CN113483769B CN113483769B (zh) 2024-03-29

Family

ID=77946643

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110941721.2A Active CN113483769B (zh) 2021-08-17 2021-08-17 基于粒子滤波器的车辆自定位方法、***、设备和介质

Country Status (1)

Country Link
CN (1) CN113483769B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114323020A (zh) * 2021-12-06 2022-04-12 纵目科技（上海）股份有限公司 车辆的定位方法、***、设备及计算机可读存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110095116A (zh) * 2019-04-29 2019-08-06 桂林电子科技大学 一种基于lift的视觉定位和惯性导航组合的定位方法
CN110865403A (zh) * 2019-10-18 2020-03-06 浙江天尚元科技有限公司 一种基于神经网络预学习和轮式里程计融合的定位方法
US20200124421A1 (en) * 2018-10-19 2020-04-23 Samsung Electronics Co., Ltd. Method and apparatus for estimating position
CN111174782A (zh) * 2019-12-31 2020-05-19 智车优行科技（上海）有限公司 位姿估计方法、装置、电子设备及计算机可读存储介质
CN111862672A (zh) * 2020-06-24 2020-10-30 北京易航远智科技有限公司 基于顶视图的停车场车辆自定位及地图构建方法
CN113075713A (zh) * 2021-03-29 2021-07-06 北京理工大学重庆创新中心 一种车辆相对位姿测量方法、***、设备及存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200124421A1 (en) * 2018-10-19 2020-04-23 Samsung Electronics Co., Ltd. Method and apparatus for estimating position
CN110095116A (zh) * 2019-04-29 2019-08-06 桂林电子科技大学 一种基于lift的视觉定位和惯性导航组合的定位方法
CN110865403A (zh) * 2019-10-18 2020-03-06 浙江天尚元科技有限公司 一种基于神经网络预学习和轮式里程计融合的定位方法
CN111174782A (zh) * 2019-12-31 2020-05-19 智车优行科技（上海）有限公司 位姿估计方法、装置、电子设备及计算机可读存储介质
CN111862672A (zh) * 2020-06-24 2020-10-30 北京易航远智科技有限公司 基于顶视图的停车场车辆自定位及地图构建方法
CN113075713A (zh) * 2021-03-29 2021-07-06 北京理工大学重庆创新中心 一种车辆相对位姿测量方法、***、设备及存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
彭文正;敖银辉;黄晓涛;王鹏飞;: "多传感器信息融合的自动驾驶车辆定位与速度估计", 传感技术学报, no. 08 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114323020A (zh) * 2021-12-06 2022-04-12 纵目科技（上海）股份有限公司 车辆的定位方法、***、设备及计算机可读存储介质
CN114323020B (zh) * 2021-12-06 2024-02-06 纵目科技（上海）股份有限公司 车辆的定位方法、***、设备及计算机可读存储介质

Also Published As

Publication number Publication date
CN113483769B (zh) 2024-03-29

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
KR102292277B1 (ko) 자율 주행 차량에서 3d cnn 네트워크를 사용하여 솔루션을 추론하는 lidar 위치 추정
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
Li et al. Deep sensor fusion between 2D laser scanner and IMU for mobile robot localization
Scherer et al. River mapping from a flying robot: state estimation, river detection, and obstacle mapping
Chen et al. Milestones in autonomous driving and intelligent vehicles—part ii: Perception and planning
Engel et al. Deeplocalization: Landmark-based self-localization with deep neural networks
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
Farag Real-time autonomous vehicle localization based on particle and unscented kalman filters
Li et al. Robust localization for intelligent vehicles based on compressed road scene map in urban environments
US20240094343A1 (en) Method, device, system, and storage medium for tracking moving target
WO2022021661A1 (zh) 一种基于高斯过程的视觉定位方法、***及存储介质
Zheng et al. A novel visual measurement framework for land vehicle positioning based on multimodule cascaded deep neural network
CN113759364A (zh) 一种基于激光地图的毫米波雷达连续定位方法及装置
Liu et al. Precise Positioning and Prediction System for Autonomous Driving Based on Generative Artificial Intelligence
Son et al. Synthetic deep neural network design for lidar-inertial odometry based on CNN and LSTM
Chen et al. Continuous occupancy mapping in dynamic environments using particles
CN113483769B (zh) 基于粒子滤波器的车辆自定位方法、***、设备和介质
Jo et al. Mixture density-PoseNet and its application to monocular camera-based global localization
Georgy et al. Unconstrained underwater multi-target tracking in passive sonar systems using two-stage PF-based technique
Farag Bayesian localization in real-time using probabilistic maps and unscented-Kalman-filters
CN115061499B (zh) 无人机控制方法及无人机控制装置
Xu et al. Vision-aided intelligent and adaptive vehicle pose estimation during GNSS outages
Soleimani et al. A disaster invariant feature for localization
Farag Self-driving vehicle localization using probabilistic maps and Unscented-Kalman filters

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant