CN115826022A - 自动驾驶车辆的定位方法、装置、可读存储介质及车辆 - Google Patents

自动驾驶车辆的定位方法、装置、可读存储介质及车辆 Download PDF

Info

Publication number
CN115826022A
CN115826022A CN202310122887.0A CN202310122887A CN115826022A CN 115826022 A CN115826022 A CN 115826022A CN 202310122887 A CN202310122887 A CN 202310122887A CN 115826022 A CN115826022 A CN 115826022A
Authority
CN
China
Prior art keywords
real
result
map matching
dynamic differential
time dynamic
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
CN202310122887.0A
Other languages
English (en)
Other versions
CN115826022B (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.)
Anhui Weilai Zhijia Technology Co Ltd
Original Assignee
Anhui Weilai Zhijia Technology Co Ltd
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 Anhui Weilai Zhijia Technology Co Ltd filed Critical Anhui Weilai Zhijia Technology Co Ltd
Priority to CN202310122887.0A priority Critical patent/CN115826022B/zh
Publication of CN115826022A publication Critical patent/CN115826022A/zh
Application granted granted Critical
Publication of CN115826022B publication Critical patent/CN115826022B/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
    • G01C21/30—Map- or contour-matching
    • 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
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/01—Determining conditions which influence positioning, e.g. radio environment, state of motion or energy consumption
    • 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/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
    • 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/50—Determining position whereby the position solution is constrained to lie upon a particular curve or surface, e.g. for locomotives on railway tracks
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明涉及自动驾驶技术领域，具体提供一种自动驾驶车辆的定位方法、控制装置、可读存储介质及车辆，旨在解决如何有效提升自动驾驶的安全性和用户的使用体验的问题。为此目的，本发明应用卡尔曼滤波算法对自动驾驶车辆的地图匹配结果和实时动态差分定位结果之间的偏差数据进行实时估计，这样就能够在自动驾驶车辆进行实时定位时，根据实时估计的偏差数据补偿地图匹配结果或实时动态差分定位结果，从而在地图匹配结果或实时动态差分定位结果其中之一缺失时，根据另一个定位结果获取缺失的数据，确保了自动驾驶车辆的全局定位能力，使得自动驾驶车辆能够在更多场景下开启自动驾驶功能，有效地提升用户的使用体验。

Description

自动驾驶车辆的定位方法、装置、可读存储介质及车辆
技术领域
本发明涉及自动驾驶技术领域，具体提供一种自动驾驶车辆的定位方法、控制装置、可读存储介质及车辆。
背景技术
自动驾驶在进行全局定位时，需要融合多种定位传感器的数据，比如实时动态差分（Real - time kinematic，RTK）定位和地图匹配定位。但是由于地图制作工艺和法规加偏插件的问题，会导致两者定位存在横向误差，且在不同的路段的表现也不一致。目前主流的做法是融合地图匹配定位结果的横向和实时动态差分定位结果的纵向信息，但是这样会丢失实时动态差分定位结果的横向约束信息，在一些没有地图匹配的路段（如雨雪天气导致可视条件差，感知误检率高等），全局定位也就会很快丢失准确的定位能力，这不但会降低自动驾驶的安全性，也会大大降低用户的自动驾驶体验。
相应地，本领域需要一种新的自动驾驶的定位方案来解决上述问题。
发明内容
为了克服上述缺陷，提出了本发明，以提供解决或至少部分地解决如何有效提升自动驾驶的安全性和用户的使用体验的问题。
在第一方面，本发明提供一种自动驾驶车辆的定位方法，所述方法包括：
当所述自动驾驶车辆的地图匹配结果和实时动态差分定位结果都存在时，应用卡尔曼滤波算法，实时获取所述地图匹配结果和所述实时动态差分定位结果之间的偏差数据；
当所述地图匹配结果或所述实时动态差分定位结果缺失时，根据获取的所述偏差数据，获取所述自动驾驶车辆的全局定位结果。
在上述自动驾驶车辆的定位方法的一个技术方案中，所述应用卡尔曼滤波算法，实时获取所述地图匹配结果和所述实时动态差分定位结果之间的偏差数据，包括：
建立所述卡尔曼滤波算法的观测方程，所述观测方程的状态量为所述偏差数据的观测值；
根据所述地图匹配结果和实时动态差分定位结果，建立所述卡尔曼滤波算法的量测方程；
根据所述观测方程和所述量测方程，获取所述卡尔曼滤波算法的残差项；
基于所述残差项和所述卡尔曼滤波算法的观测噪声，对所述偏差数据的观测值进行迭代更新，以实时获取所述偏差数据。
在上述自动驾驶车辆的定位方法的一个技术方案中，所述方法还包括根据以下步骤获取所述卡尔曼滤波算法当前的观测噪声：
当所述自动驾驶车辆的地图匹配结果和实时动态差分定位结果均存在时，根据所述地图匹配结果和所述实时动态差分定位结果，获取所述当前的观测噪声；
当所述自动驾驶车辆的地图匹配结果或实时动态差分定位结果不存在时，根据预设的经验值或上一时刻的观测噪声，获取所述当前的观测噪声。
在上述自动驾驶车辆的定位方法的一个技术方案中，所述根据所述地图匹配结果和所述实时动态差分定位结果，获取所述当前的观测噪声，包括：
将当前时刻之前的预设历史时长内的所述地图匹配结果和所述实时动态差分定位结果分别转换至车体坐标系下；
根据所述车体坐标系下的地图匹配结果和实时动态差分定位结果，获取所述地图匹配结果和实时动态差分定位结果的偏差数据；
根据所述预设历史时长内所述偏差数据的方差，获取所述当前的观测噪声。
在上述自动驾驶车辆的定位方法的一个技术方案中，所述根据获取的所述偏差数据，获取所述自动驾驶车辆的全局定位结果，包括：
当所述地图匹配结果缺失时，根据实时动态差分定位结果和所述偏差数据，获取地图匹配结果；
根据所述实时动态差分定位结果和获取的地图匹配结果，获取所述自动驾驶车辆的全局定位结果。
在上述自动驾驶车辆的定位方法的一个技术方案中，所述根据获取的所述偏差数据，获取所述自动驾驶车辆的全局定位结果，包括：
当所述实时动态差分定位结果缺失时，根据地图匹配结果和所述偏差数据，获取实时动态差分定位结果；
根据获取的实时动态差分定位结果和所述地图匹配结果，获取所述自动驾驶车辆的全局定位结果。
在上述自动驾驶车辆的定位方法的一个技术方案中，所述偏差数据为所述地图匹配结果和所述实时动态差分定位结果之间的横向偏差。
在第二方面，提供一种控制装置，该控制装置包括处理器和存储装置，所述存储装置适于存储多条程序代码，所述程序代码适于由所述处理器加载并运行以执行上述自动驾驶车辆的定位方法的技术方案中任一项技术方案所述的自动驾驶车辆的定位方法。
在第三方面，提供一种计算机可读存储介质，该计算机可读存储介质其中存储有多条程序代码，所述程序代码适于由处理器加载并运行以执行上述自动驾驶车辆的定位方法的技术方案中任一项技术方案所述的自动驾驶车辆的定位方法。
在第四方面，提供一种车辆，所述车辆包括上述控制装置技术方案中所述的控制装置。
本发明上述一个或多个技术方案，至少具有如下一种或多种有益效果：
在实施本发明的技术方案中，本发明应用卡尔曼滤波算法对自动驾驶车辆的地图匹配结果和实时动态差分定位结果之间的偏差数据进行实时估计，这样就能够对地图匹配结果和实时动态差分定位结果之间的偏差数据进行准确地估计，当地图匹配结果或实时动态差分定位结果缺失时，可以根据实时估计的偏差数据补偿地图匹配结果或实时动态差分定位结果，从而确保了自动驾驶车辆的全局定位能力。通过上述配置方式，本发明能够有效利用地图匹配结果和实时差分定位结果之间的偏差数据，在确保自动驾驶车辆的安全性的同时，能够确保自动驾驶车辆在更多场景下开启自动驾驶功能，有效地提升用户的使用体验。
附图说明
参照附图，本发明的公开内容将变得更易理解。本领域技术人员容易理解的是：这些附图仅仅用于说明的目的，而并非意在对本发明的保护范围组成限制。其中：
图1是根据本发明的一个实施例的自动驾驶车辆的定位方法的主要步骤流程示意图；
图2是根据本发明实施例的一个实施方式的应用卡尔曼滤波算法获取偏差数据的主要步骤流程示意图；
图3是根据本发明实施例的一个实施方式的获取卡尔曼滤波算法当前的观测噪声的主要步骤流程示意图。
具体实施方式
下面参照附图来描述本发明的一些实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。
在本发明的描述中，“模块”、“处理器”可以包括硬件、软件或者两者的组合。一个模块可以包括硬件电路，各种合适的感应器，通信端口，存储器，也可以包括软件部分，比如程序代码，也可以是软件和硬件的组合。处理器可以是中央处理器、微处理器、图像处理器、数字信号处理器或者其他任何合适的处理器。处理器具有数据和/或信号处理功能。处理器可以以软件方式实现、硬件方式实现或者二者结合方式实现。非暂时性的计算机可读存储介质包括任何合适的可存储程序代码的介质，比如磁碟、硬盘、光碟、闪存、只读存储器、随机存取存储器等等。术语“A和/或B”表示所有可能的A与B的组合，比如只是A、只是B或者A和B。术语“至少一个A或B”或者“A和B中的至少一个”含义与“A和/或B”类似，可以包括只是A、只是B或者A和B。单数形式的术语“一个”、“这个”也可以包含复数形式。
参阅附图1，图1是根据本发明的一个实施例的自动驾驶车辆的定位方法的主要步骤流程示意图。如图1所示，本发明实施例中的自动驾驶车辆的定位方法主要包括下列步骤S101-步骤S102。
步骤S101：当自动驾驶车辆的地图匹配结果和实时动态差分定位结果都存在时，应用卡尔曼滤波算法，实时获取自动驾驶车辆的地图匹配结果和实时动态差分定位结果之间的偏差数据。
在本实施例中，当自动驾驶车辆的地图匹配结果和实时动态差分定位结果都存在时，可以应用卡尔曼滤波算法来对自动驾驶车辆的地图匹配结果和实时动态差分（RTK，Real - Time Kinematic）定位结果之间的偏差数据进行实时估计。由于观测结果和量测结果都可能存在误差，应用卡尔曼滤波算法基于上一状态的观测结果和当前状态的量测结果对当前状态的观测结果进行估计，能够获得更为准确的偏差数据。
一个实施方式中，偏差数据可以为地图匹配结果和实时动态差分定位结果在自动驾驶车辆的车体坐标系下的偏差数据。
一个实施方式中，偏差数据可以包括横向偏差、纵向偏差和高程偏差。其中，横向偏差可以为车体坐标系下地图匹配结果和实时动态差分定位结果在y轴方向上的差值；纵向偏差可以为车体坐标系下地图匹配结果和实时动态差分定位结果在x轴方向上的差值；高程偏差可以为车体坐标系下地图匹配结果和实时动态差分定位结果在z轴方向上的差值。
一个实施方式中，由于地图匹配结果是地图与车道线之间进行匹配获得的，因而其与实时动态差分定位结果之间的纵向偏差和高程偏差的置信度较低，因而偏差数据可以仅包括横向偏差。
步骤S102：当地图匹配结果或实时动态差分定位结果缺失时，根据获取的偏差数据，获取自动驾驶车辆的全局定位结果。
在本实施例中，当地图匹配结果或实时动态差分定位结果缺失时，可以在自动驾驶车辆进行实时定位时，根据偏差数据，来获取自动驾驶车辆的全局定位结果。
一个实施方式中，当自动驾驶车辆进行实时定位时，如出现环境因素、堵车等因素影响，导致地图匹配结果或实时动态差分定位结果不能获取时，可以应用卡尔曼滤波算法获取的偏差数据来对不能获取的定位结果进行估计，从而应用估计的定位结果和已知定位结果进行融合，获得自动驾驶车辆的全局定位结果。
基于上述步骤S101-步骤S102，本发明实施例应用卡尔曼滤波算法对自动驾驶车辆的地图匹配结果和实时动态差分定位结果之间的偏差数据进行实时估计，这样就能够对地图匹配结果和实时动态差分定位结果之间的偏差数据进行准确地估计，当地图匹配结果或实时动态差分定位结果缺失时，可以根据实时估计的偏差数据补偿地图匹配结果或实时动态差分定位结果，从而确保了自动驾驶车辆的全局定位能力。通过上述配置方式，本发明实施例能够有效利用地图匹配结果和实时差分定位结果之间的偏差数据，在确保自动驾驶车辆的安全性的同时，能够确保自动驾驶车辆在更多场景下开启自动驾驶功能，有效地提升用户的使用体验。
下面分别对步骤S101和步骤S102作进一步地说明。
参阅附图2，图2是根据本发明实施例的一个实施方式的应用卡尔曼滤波算法获取偏差数据的主要步骤流程示意图。如图2所示，在本发明实施例的一个实施方式中，步骤S101可以包括以下步骤S1011至步骤S1014：
步骤S1011：建立卡尔曼滤波算法的观测方程，观测方程的状态量为偏差数据的观测值。
在本实施方式中，可以建立卡尔曼滤波算法的观测方程，观测方程的状态量可以为偏差数据的观测值。
一个实施方式中，可以根据以下公式（1）获取观测方程：
Figure SMS_1
（1）
其中，
Figure SMS_2
，为地图匹配结果和实时差分定位结果之间的偏差数据，包括横向偏差、纵向偏差和高程偏差；
Figure SMS_3
。
步骤S1012：根据地图匹配结果和实时动态差分定位结果，建立卡尔曼滤波算法的量测方程。
在本实施方式中，可以根据地图匹配结果和实时动态差分定位结果来建立量测方程。
一个实施方式中，可以根据以下公式（2）获取量测方程：
Figure SMS_4
（2）
其中，
Figure SMS_5
为车体坐标系到全局坐标系的旋转矩阵的转置，
Figure SMS_6
为全局坐标系下的地图匹配结果，
Figure SMS_7
为全局坐标系下的实时动态差分定位结果。地图匹配结果和实时动态差分定位结果均可以根据以下公式（3）统一转换至全局坐标系下：
Figure SMS_8
（3）
其中，
Figure SMS_9
代表全局坐标系下的地图匹配结果或实时动态差分定位结果，
Figure SMS_10
为地图匹配或RTK在全局坐标系下的位置，
Figure SMS_11
为地图匹配或RTK的坐标系到全局坐标系的旋转矩阵，
Figure SMS_12
为地图匹配结果或实时动态差分定位结果在地图匹配坐标系或RTK坐标系下的位置。
步骤S1013：根据观测方程和量测方程，获取卡尔曼滤波算法的残差项。
在本实施方式中，可以根据观测方程和量测方程，获取残差项。
一个实施方式中，可以根据以下公式（4）获取残差项
Figure SMS_13
：
Figure SMS_14
(4)
一个实施方式中，卡尔曼滤波算法的雅克比矩阵
Figure SMS_15
根据以下公式（5）获得：
Figure SMS_16
(5)
步骤S1014：基于残差项和卡尔曼滤波算法的观测噪声，对偏差数据的观测值进行迭代更新，以实时获取偏差数据。
在本实施方式中，可以根据残差项和观测噪声，对偏差数据的观测值进行迭代更新，从而实时获取偏差数据。
一个实施方式中，卡尔曼滤波算法的卡尔曼方程可以根据以下公式（6）-（10）获得：
Figure SMS_17
(6)
Figure SMS_18
(7)
Figure SMS_19
(8)
Figure SMS_20
(9)
Figure SMS_21
(10)
其中，
Figure SMS_23
为k时刻的状态；
Figure SMS_26
为k时刻的预测状态（未优化）；
Figure SMS_28
为k时刻的观测值；
Figure SMS_24
为输入控制矩阵；Q为预测噪声；R为观测噪声；
Figure SMS_27
为k时刻的误差；
Figure SMS_29
为k时刻的卡尔曼增益；
Figure SMS_30
为k-1时刻外界对系统的作用；
Figure SMS_22
为k时刻的残差项
Figure SMS_25
。
一个实施方式中，参阅附图3，图3是根据本发明实施例的一个实施方式的获取卡尔曼滤波算法当前的观测噪声的主要步骤流程示意图。如图3所示，可以通过以下步骤S201和步骤S202获取卡尔曼滤波算法当前的观测噪声：
步骤S201：当自动驾驶车辆的地图匹配结果和实时动态差分定位结果均存在时，根据地图匹配结果和实时动态差分定位结果，获取当前的观测噪声。
在本实施方式中，步骤S201可以进一步包括以下步骤S2011至步骤S2013：
步骤S2011：将当前时刻之前的预设历史时长内的地图匹配结果和实时动态差分定位结果分别转换至车体坐标系下。
步骤S2012：根据车体坐标系下的地图匹配结果和实时动态差分定位结果，获取地图匹配结果和实时动态差分定位结果的偏差数据。
步骤S2013：根据预设历史时长内偏差数据的方差，获取当前的观测噪声。
在本实施方式中，可以采集当前时刻之前的一段历史时长的地图匹配结果和实时动态差分定位结果，并根据以下公式（11）和（12），将地图匹配结果和实时动态差分定位结果转换至全局坐标系下：
Figure SMS_31
（11）
其中，
Figure SMS_32
代表全局坐标系下的实时动态差分定位结果，
Figure SMS_33
为RTK在全局坐标系下的位置，
Figure SMS_34
为RTK坐标系到全局坐标系的旋转矩阵，
Figure SMS_35
为实时动态差分定位结果在RTK坐标系下的位置。
Figure SMS_36
（12）
其中，
Figure SMS_37
代表全局坐标系下的地图匹配结果，
Figure SMS_38
为地图匹配在全局坐标系下的位置，
Figure SMS_39
为地图匹配坐标系到全局坐标系的旋转矩阵，
Figure SMS_40
为地图匹配结果在地图匹配坐标系下的位置。
根据以下公式（13）获取地图匹配结果和实时动态差分定位结果的偏差数据：
Figure SMS_41
(13)
其中，
Figure SMS_42
为偏差数据，
Figure SMS_43
为全局坐标系到车体坐标系的旋转矩阵。
可以根据公式（11）至公式（13）统计预设历史时长的偏差数据，并计算偏差数据的方差，作为当前的观测噪声。
一个实施方式中，可以根据公式（11）至公式（13）统计预设历史时长的横向偏差，并计算横向偏差的方差，作为当前的观测噪声。
步骤S202：当自动驾驶的地图匹配结果或实时动态差分定位结果不存在时，根据预设的经验值或上一时刻的观测噪声，获取当前的观测噪声。
在本实施方式中，若当前的地图匹配结果或实时动态差分定位结果不存在，那么可以根据经验值，或根据上一时刻的观测噪声，确定当前的观测噪声。如，在卡尔曼滤波算法初始迭代时，其当前的观测噪声可以设置为经验值。如，在地图匹配结果或实时动态差分定位结果刚刚恢复时，可以使用上一时刻的观测噪声作为当前的观测噪声。
一个实施方式中，经验值观测噪声为R=1。
在本发明实施例的一个实施方式中，步骤S102可以包括以下步骤S1021和步骤S1022：
步骤S1021：当地图匹配结果缺失时，根据实时动态差分定位结果和偏差数据，获取地图匹配结果。
步骤S1022：根据实时动态差分定位结果和获取的地图匹配结果，获取自动驾驶车辆的全局定位结果。
在本实施方式中，在进行自动驾驶车辆的实时定位时，若地图匹配结果缺失时，则可以根据实时动态差分定位结果和应用卡尔曼滤波算法获得的偏差数据，获取地图匹配结果，从而能够实现地图匹配结果与实时动态差分定位结果之间的融合，以获取自动驾驶车辆的全局定位结果，确保了自动驾驶车辆能够开启自动驾驶功能。
在本发明实施例的一个实施方式中，步骤S102还可以包括以下步骤S1023和步骤S1024：
步骤S1023：当实时动态差分定位结果缺失时，根据地图匹配结果和偏差数据，获取实时动态差分定位结果。
步骤S1024：根据获取的实时动态差分定位结果和地图匹配结果，获取自动驾驶车辆的全局定位结果。
在本实施方式中，在进行自动驾驶车辆的实时定位时，若实时动态差分定位结果缺失时，则可以根据地图匹配结果和应用卡尔曼滤波算法获得的偏差数据，获取实时动态差分定位结果，从而能够实现地图匹配结果与实时动态差分定位结果之间的融合，以获取自动驾驶车辆的全局定位结果，确保了自动驾驶车辆能够开启自动驾驶功能。
需要指出的是，尽管上述实施例中将各个步骤按照特定的先后顺序进行了描述，但是本领域技术人员可以理解，为了实现本发明的效果，不同的步骤之间并非必须按照这样的顺序执行，其可以同时（并行）执行或以其他顺序执行，这些变化都在本发明的保护范围之内。
本领域技术人员能够理解的是，本发明实现上述一实施例的方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器、随机存取存储器、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读存储介质不包括电载波信号和电信信号。
进一步，本发明还提供了一种控制装置。在根据本发明的一个控制装置实施例中，控制装置包括处理器和存储装置，存储装置可以被配置成存储执行上述方法实施例的自动驾驶车辆的定位方法的程序，处理器可以被配置成用于执行存储装置中的程序，该程序包括但不限于执行上述方法实施例的自动驾驶车辆的定位方法的程序。为了便于说明，仅示出了与本发明实施例相关的部分，具体技术细节未揭示的，请参照本发明实施例方法部分。
在本发明实施例中控制装置可以是包括各种电子设备形成的控制装置设备。在一些可能的实施方式中，控制装置可以包括多个存储装置和多个处理器。而执行上述方法实施例的自动驾驶车辆的定位方法的程序可以被分割成多段子程序，每段子程序分别可以由处理器加载并运行以执行上述方法实施例的自动驾驶车辆的定位方法的不同步骤。具体地，每段子程序可以分别存储在不同的存储装置中，每个处理器可以被配置成用于执行一个或多个存储装置中的程序，以共同实现上述方法实施例的自动驾驶车辆的定位方法，即每个处理器分别执行上述方法实施例的自动驾驶车辆的定位方法的不同步骤，来共同实现上述方法实施例的自动驾驶车辆的定位方法。
上述多个处理器可以是部署于同一个设备上的处理器，例如上述控制装置可以是由多个处理器组成的高性能设备，上述多个处理器可以是该高性能设备上配置的处理器。此外，上述多个处理器也可以是部署于不同设备上的处理器，例如上述控制装置可以是服务器集群，上述多个处理器可以是服务器集群中不同服务器上的处理器。
进一步，本发明还提供了一种计算机可读存储介质。在根据本发明的一个计算机可读存储介质实施例中，计算机可读存储介质可以被配置成存储执行上述方法实施例的自动驾驶车辆的定位方法的程序，该程序可以由处理器加载并运行以实现上述自动驾驶车辆的定位方法。为了便于说明，仅示出了与本发明实施例相关的部分，具体技术细节未揭示的，请参照本发明实施例方法部分。该计算机可读存储介质可以是包括各种电子设备形成的存储装置设备，可选的，本发明实施例中计算机可读存储介质是非暂时性的计算机可读存储介质。
进一步，本发明还提供一种车辆。在根据本发明的一个车辆实施例中，车辆可以包括控制装置实施例中的控制装置。
进一步，应该理解的是，由于各个模块的设定仅仅是为了说明本发明的装置的功能单元，这些模块对应的物理器件可以是处理器本身，或者处理器中软件的一部分，硬件的一部分，或者软件和硬件结合的一部分。因此，图中的各个模块的数量仅仅是示意性的。
本领域技术人员能够理解的是，可以对装置中的各个模块进行适应性地拆分或合并。对具体模块的这种拆分或合并并不会导致技术方案偏离本发明的原理，因此，拆分或合并之后的技术方案都将落入本发明的保护范围内。
至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (10)

1.一种自动驾驶车辆的定位方法，其特征在于，所述方法包括：
当所述自动驾驶车辆的地图匹配结果和实时动态差分定位结果都存在时，应用卡尔曼滤波算法，实时获取所述地图匹配结果和所述实时动态差分定位结果之间的偏差数据；
当所述地图匹配结果或所述实时动态差分定位结果缺失时，根据获取的所述偏差数据，获取所述自动驾驶车辆的全局定位结果。
2.根据权利要求1所述的自动驾驶车辆的定位方法，其特征在于，
所述应用卡尔曼滤波算法，实时获取所述地图匹配结果和所述实时动态差分定位结果之间的偏差数据，包括：
建立所述卡尔曼滤波算法的观测方程，所述观测方程的状态量为所述偏差数据的观测值；
根据所述地图匹配结果和实时动态差分定位结果，建立所述卡尔曼滤波算法的量测方程；
根据所述观测方程和所述量测方程，获取所述卡尔曼滤波算法的残差项；
基于所述残差项和所述卡尔曼滤波算法的观测噪声，对所述偏差数据的观测值进行迭代更新，以实时获取所述偏差数据。
3.根据权利要求2所述的自动驾驶车辆的定位方法，其特征在于，所述方法还包括根据以下步骤获取所述卡尔曼滤波算法当前的观测噪声：
当所述自动驾驶车辆的地图匹配结果和实时动态差分定位结果均存在时，根据所述地图匹配结果和所述实时动态差分定位结果，获取所述当前的观测噪声；
当所述自动驾驶车辆的地图匹配结果或实时动态差分定位结果不存在时，根据预设的经验值或上一时刻的观测噪声，获取所述当前的观测噪声。
4.根据权利要求3所述的自动驾驶车辆的定位方法，其特征在于，
所述根据所述地图匹配结果和所述实时动态差分定位结果，获取所述当前的观测噪声，包括：
将当前时刻之前的预设历史时长内的所述地图匹配结果和所述实时动态差分定位结果分别转换至车体坐标系下；
根据所述车体坐标系下的地图匹配结果和实时动态差分定位结果，获取所述地图匹配结果和实时动态差分定位结果的偏差数据；
根据所述预设历史时长内所述偏差数据的方差，获取所述当前的观测噪声。
5.根据权利要求1所述的自动驾驶车辆的定位方法，其特征在于，所述根据获取的所述偏差数据，获取所述自动驾驶车辆的全局定位结果，包括：
当所述地图匹配结果缺失时，根据实时动态差分定位结果和所述偏差数据，获取地图匹配结果；
根据所述实时动态差分定位结果和获取的地图匹配结果，获取所述自动驾驶车辆的全局定位结果。
6.根据权利要求1所述的自动驾驶车辆的定位方法，其特征在于，所述根据获取的所述偏差数据，获取所述自动驾驶车辆的全局定位结果，包括：
当所述实时动态差分定位结果缺失时，根据地图匹配结果和所述偏差数据，获取实时动态差分定位结果；
根据获取的实时动态差分定位结果和所述地图匹配结果，获取所述自动驾驶车辆的全局定位结果。
7.根据权利要求1至6中任一项所述的自动驾驶车辆的定位方法，其特征在于，所述偏差数据为所述地图匹配结果和所述实时动态差分定位结果之间的横向偏差。
8.一种控制装置，包括处理器和存储装置，所述存储装置适于存储多条程序代码，其特征在于，所述程序代码适于由所述处理器加载并运行以执行权利要求1至7中任一项所述的自动驾驶车辆的定位方法。
9.一种计算机可读存储介质，其中存储有多条程序代码，其特征在于，所述程序代码适于由处理器加载并运行以执行权利要求1至7中任一项所述的自动驾驶车辆的定位方法。
10.一种车辆，其特征在于，所述车辆包括权利要求8所述的控制装置。
CN202310122887.0A 2023-02-16 2023-02-16 自动驾驶车辆的定位方法、装置、可读存储介质及车辆 Active CN115826022B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310122887.0A CN115826022B (zh) 2023-02-16 2023-02-16 自动驾驶车辆的定位方法、装置、可读存储介质及车辆

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310122887.0A CN115826022B (zh) 2023-02-16 2023-02-16 自动驾驶车辆的定位方法、装置、可读存储介质及车辆

Publications (2)

Publication Number Publication Date
CN115826022A true CN115826022A (zh) 2023-03-21
CN115826022B CN115826022B (zh) 2023-06-06

Family

ID=85521607

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310122887.0A Active CN115826022B (zh) 2023-02-16 2023-02-16 自动驾驶车辆的定位方法、装置、可读存储介质及车辆

Country Status (1)

Country Link
CN (1) CN115826022B (zh)

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1455268A (zh) * 2003-03-27 2003-11-12 第二炮兵工程学院对外技术服务部 一种基于地图匹配的卫星定位误差处理方法
CN101696886A (zh) * 2009-10-29 2010-04-21 哈尔滨工业大学 一种在gps盲区内的电子地图辅助惯性导航方法
JP2017062167A (ja) * 2015-09-24 2017-03-30 カシオ計算機株式会社 電子機器、測位制御方法及びプログラム
JP2018189463A (ja) * 2017-05-01 2018-11-29 株式会社Ｓｏｋｅｎ 車両位置推定装置及びプログラム
CN110687562A (zh) * 2019-09-12 2020-01-14 华为技术有限公司 一种定位方法及车载装置
CN111721289A (zh) * 2020-06-28 2020-09-29 北京百度网讯科技有限公司 车辆定位方法、装置、设备、存储介质及车辆
CN111862659A (zh) * 2020-06-30 2020-10-30 中冶智诚(武汉)工程技术有限公司 一种gps轨迹数据匹配和补全的方法
US20210156711A1 (en) * 2019-11-27 2021-05-27 Honda Research Institute Europe Gmbh Analysis of localization errors in a mobile object
CN113447033A (zh) * 2021-05-17 2021-09-28 山东科技大学 一种车道级地图匹配方法及系统
CN115046546A (zh) * 2022-05-10 2022-09-13 南京航空航天大学 一种基于车道线识别的自动驾驶汽车定位系统及方法
CN115542307A (zh) * 2022-09-15 2022-12-30 河北省交通规划设计研究院有限公司 一种基于高精度地图的高速场景多雷达航迹融合方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1455268A (zh) * 2003-03-27 2003-11-12 第二炮兵工程学院对外技术服务部 一种基于地图匹配的卫星定位误差处理方法
CN101696886A (zh) * 2009-10-29 2010-04-21 哈尔滨工业大学 一种在gps盲区内的电子地图辅助惯性导航方法
JP2017062167A (ja) * 2015-09-24 2017-03-30 カシオ計算機株式会社 電子機器、測位制御方法及びプログラム
JP2018189463A (ja) * 2017-05-01 2018-11-29 株式会社Ｓｏｋｅｎ 車両位置推定装置及びプログラム
CN110687562A (zh) * 2019-09-12 2020-01-14 华为技术有限公司 一种定位方法及车载装置
US20210156711A1 (en) * 2019-11-27 2021-05-27 Honda Research Institute Europe Gmbh Analysis of localization errors in a mobile object
CN111721289A (zh) * 2020-06-28 2020-09-29 北京百度网讯科技有限公司 车辆定位方法、装置、设备、存储介质及车辆
CN111862659A (zh) * 2020-06-30 2020-10-30 中冶智诚(武汉)工程技术有限公司 一种gps轨迹数据匹配和补全的方法
CN113447033A (zh) * 2021-05-17 2021-09-28 山东科技大学 一种车道级地图匹配方法及系统
CN115046546A (zh) * 2022-05-10 2022-09-13 南京航空航天大学 一种基于车道线识别的自动驾驶汽车定位系统及方法
CN115542307A (zh) * 2022-09-15 2022-12-30 河北省交通规划设计研究院有限公司 一种基于高精度地图的高速场景多雷达航迹融合方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
YUKI KATO 等: "Autonomous Robot Navigation System Without Grid Maps Based on Double Deep Q-Network and RTK-GNSS Localization in Outdoor Environments", 《2019 IEEE/SICE INTERNATIONAL SYMPOSIUM ON SYSTEM INTEGRATION (SII)》, pages 346 - 351 *
滕志军 等: "交叉路段背景下改进的D-S证据理论地图匹配算法", 《中国惯性技术学报》, vol. 28, no. 3, pages 380 - 385 *
陈光武;刘昊;李少远;杨菊花;魏宗寿;: "动态模型偏差校正在列车组合定位中的应用", vol. 45, no. 12, pages 2281 - 2293 *
马连韬;王亚沙;彭广举;赵宇昕;何远舵;高敬月;: "基于公交车轨迹数据的道路GPS环境友好性评估", 计算机研究与发展, no. 12, pages 2694 - 2707 *

Also Published As

Publication number Publication date
CN115826022B (zh) 2023-06-06

Similar Documents

Publication Publication Date Title
CN113155139B (zh) 车辆轨迹纠偏方法、装置及电子设备
CN106610294B (zh) 定位方法及装置
CN108225341B (zh) 车辆定位方法
CN110411440B (zh) 一种道路采集方法、装置、服务器及存储介质
CN110888434A (zh) 自动驾驶方法、装置、计算机设备和计算机可读存储介质
CN110530392B (zh) 一种基于传统地图和高精地图结合的路径规划方法及装置
CN112016190A (zh) 动力电池温度修正方法、装置及计算机可读存储介质
CN114280582A (zh) 激光雷达的标定校准方法、装置、存储介质及电子设备
CN110926498B (zh) 车辆里程补偿方法、装置、计算机设备
CN113050660B (zh) 误差补偿方法、装置、计算机设备及存储介质
CN115826022A (zh) 自动驾驶车辆的定位方法、装置、可读存储介质及车辆
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN112444820B (zh) 一种机器人位姿确定方法、装置、可读存储介质及机器人
CN115790615B (zh) 基于自动驾驶的多传感器融合方法、装置、介质及车辆
DE102018109126A1 (de) Statistisch abgeleitete GNSS-ABS-Geschwindigkeitsmesserkalibrierung
CN112558036B (zh) 用于输出信息的方法和装置
CN109656245B (zh) 刹车位置的确定方法和装置
CN115792943A (zh) 自动驾驶车辆的定位方法、装置、可读存储介质及车辆
CN115265566A (zh) 自动驾驶定位数据收集处理方法、装置、介质及车辆
CN115909252A (zh) 车载多相机目标检测方法、控制装置、存储介质及车辆
CN115683139A (zh) 一种车载地图路径规划方法、系统、电子设备及存储介质
CN113075713B (zh) 一种车辆相对位姿测量方法、系统、设备及存储介质
CN110032172B (zh) 一种车辆驾驶控制系统精度检测方法及装置
CN112595330B (zh) 车辆定位方法、装置、电子设备和计算机可读介质
CN108709553A (zh) 云端估算隧道内任意点通过速度模型的方法和装置

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