CN110954113A - 一种车辆位姿的修正方法和装置 - Google Patents

一种车辆位姿的修正方法和装置 Download PDF

Info

Publication number
CN110954113A
CN110954113A CN201910460548.7A CN201910460548A CN110954113A CN 110954113 A CN110954113 A CN 110954113A CN 201910460548 A CN201910460548 A CN 201910460548A CN 110954113 A CN110954113 A CN 110954113A
Authority
CN
China
Prior art keywords
particle
vehicle body
preset
target
map
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
CN201910460548.7A
Other languages
English (en)
Other versions
CN110954113B (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.)
Beijing Momenta Technology Co Ltd
Original Assignee
Beijing Chusudu 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 Beijing Chusudu Technology Co ltd filed Critical Beijing Chusudu Technology Co ltd
Priority to CN201910460548.7A priority Critical patent/CN110954113B/zh
Priority to PCT/CN2019/113486 priority patent/WO2020237996A1/zh
Publication of CN110954113A publication Critical patent/CN110954113A/zh
Application granted granted Critical
Publication of CN110954113B publication Critical patent/CN110954113B/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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Abstract

本发明实施例公开了一种车辆位姿的修正方法和装置，该方法包括：当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，先验位置通过预设定位装置得到；更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于该状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。通过采用上述技术方案，解决了使用消费级预设定位装置定位精度不高的问题，实现了对车辆进行厘米级的高精度定位的技术效果。

Description

一种车辆位姿的修正方法和装置
技术领域
本发明涉及自动驾驶技术领域，具体涉及一种车辆位姿的修正方法和装置。
背景技术
在自动驾驶领域，高精度定位至关重要。近年来，深度学习等技术的成果，极大促进了图像语义分割、图像识别领域的发展，这为高精度地图及高精度定位提供了坚实的基础。
在基于高精度地图的定位方案中，当无人驾驶车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域时，需要获得一个全局而精准的位置信息进行初始化，继而可以使用高精度地图进行精准定位，即绝对位置精度可达到厘米级。但在消费级设备，例如单点GPS(Global Positioning System，全球定位系统)、相机与廉价IMU(Inertialmeasurement unit，惯性测量单元)的定位方案中，由于单点GPS所提供的定位精度信息的误差较大，利用单点GPS所提供的位置，将高精度地图中的交通标志信息与利用深度学习感知模型感知出图像中的交通标志信息，例如车道线、路灯杆等，进行重投影匹配时，容易造成车道线左右匹配错误，路灯杆前后向匹配错误等问题。如果利用错误的匹配信息对车体位置进行修正，不仅不能利用高精地图使车体的位置精度达到厘米级，反而可能会使车体的位置与真实位置偏离更大。
现有技术在基于视觉、GPS与高精度地图融合的方法对车辆进行定位时，通常需要使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位。但路面上的箭头信息过于稀疏，无法保证连续的厘米级高精定位。
发明内容
本发明实施例公开一种车辆位姿的修正方法和装置，解决了使用消费级预设定位装置定位精度不高的问题，实现了对车辆进行厘米级的高精度定位的技术效果。
第一方面，本发明实施例公开了一种车辆位姿的修正方法，该方法包括：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
可选的，所述基于车体的先验位置进行车辆位置信息的粒子采样，包括：
在所述预设导航地图中，提取与车体的先验位置满足第一设定距离的各目标车道线；
对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离的位置处，对车辆位置信息进行2D空间的粒子采样。
可选的，所述基于车体的先验位置进行车辆位置信息的粒子采样，包括：
在与车体的先验位置满足第三设定距离的位置处，在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。
可选的，更新采样得到的粒子的位姿和每个粒子对应的权重信息，包括：
对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息；
基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上，并根据重投影残差的大小建立所述地图元素与所述感知图像中各感知元素之间的初始匹配关系；
基于所述初始匹配关系，更新每个粒子对应的权重信息。
可选的，基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上，包括：
将所述位置信息转换到相机坐标系下，并计算所述位置信息在所述相机坐标系的设定方向上对应的目标位置；
将世界坐标系中，与所述先验位置满足第四设定距离的地图元素转换到所述相机坐标系下；
在所述相机坐标系的设定方向上，将所述满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对所述满足第四设定范围的地图元素进行排序；
从所述键值对信息中，依次查找在所示设定方向上所述目标位置前方的各地图元素，并将各地图元素均投影到感知图像上。
可选的，基于所述初始匹配关系，更新每个粒子对应的权重信息，包括：
按照如下公式更新每个粒子对应的权重信息：
Figure BDA0002077937260000031
其中，
Figure BDA0002077937260000032
是k时刻的粒子i的权重；
Figure BDA0002077937260000033
是k-1时刻粒子i的权重；Ni为第i个粒子上获得的感知元素和地图元素的匹数数量；f(Ni)是Ni正相关的函数；sj为每种地图元素对应的一个归一化参数；rj第j对儿所述地图元素和所述感知元素匹配的图像重投影误差；R是图像上的观测误差；Exp指数项上除以了Ni将感知图像和导航地图匹配的数量进行了概率归一化。
可选的，根据每个目标粒子更新后的权重信息确定车体位置的状态量，包括：
按照如下公式根据每个目标粒子更新后的权重信息确定车体位置的状态量：
Figure BDA0002077937260000034
其中，xk表示k时刻车体位置的状态量；
Figure BDA0002077937260000035
是k时刻的粒子i的权重。
可选的，基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系，包括：
基于所述状态量和车体姿态，将所述预设导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与所述感知图像中对应的感知元素建立一对一的匹配关系；
将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和所述预设导航地图之间的目标匹配关系
可选的，所述方法还包括：
如果设定数目的目标粒子的位置不满足预设收敛条件，则检测所述目标粒子中的有效粒子数目是否达第二预设数目阈值；
如果所述有效粒子数目未达到所述第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
第二方面，本发明实施例还提供了一种车辆位姿的修正装置，该装置包括：
粒子采样模块，被配置为当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
权重更新模块，被配置为更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
目标匹配关系建立模块，被配置为根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
位姿优化模块，被配置为基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
可选的，所述粒子采样模块，具体被配置为：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，在所述预设导航地图中，提取与车体的先验位置满足第一设定距离的各目标车道线；
对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离的位置处，对车辆位置信息进行2D空间的粒子采样。
可选的，所述粒子采样模块，具体被配置为：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，在与车体的先验位置满足第三设定距离的位置处，在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。
可选的，所述权重更新模块，包括：
位置信息确定单元，被配置为对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息；
投影单元，被配置为基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上；
初始匹配关系建立单元，被配置为根据重投影残差的大小建立所述地图元素与所述感知图像中各感知元素之间的初始匹配关系；
权重信息更新单元，被配置为基于所述初始匹配关系，更新每个粒子对应的权重信息。
可选的，所述投影单元，具体被配置为：
将所述位置信息转换到相机坐标系下，并计算所述位置信息在所述相机坐标系的设定方向上对应的目标位置；
将世界坐标系中，与所述先验位置满足第四设定距离的地图元素转换到所述相机坐标系下；
在所述相机坐标系的设定方向上，将所述满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对所述满足第四设定范围的地图元素进行排序；
从所述键值对信息中，依次查找在所示设定方向上所述目标位置前方的各地图元素，并将各地图元素均投影到感知图像上。
可选的，所述权重信息更新单元，具体被配置为：
按照如下公式更新每个粒子对应的权重信息：
Figure BDA0002077937260000051
其中，
Figure BDA0002077937260000052
是k时刻的粒子i的权重；
Figure BDA0002077937260000053
是k-1时刻粒子i的权重；Ni为第i个粒子上获得的感知元素和地图元素的匹数数量；f(Ni)是Ni正相关的函数；sj为每种地图元素对应的一个归一化参数；rj第j对儿所述地图元素和所述感知元素匹配的图像重投影误差；R是图像上的观测误差；Exp指数项上除以了Ni将感知图像和导航地图匹配的数量进行了概率归一化。
可选的，所述目标匹配关系建立模块，具体被配置为：
根据每个目标粒子更新后的权重信息确定车体位置的状态量，基于所述状态量和车体姿态，将所述预设导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与所述感知图像中对应的感知元素建立一对一的匹配关系；
将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和所述预设导航地图之间的目标匹配关系。
可选的，所述装置还包括：
有效粒子检测模块，被配置为如果设定数目的目标粒子的位置不满足预设收敛条件，则检测所述目标粒子中的有效粒子数目是否达第二预设数目阈值；
重采样模块，被配置为如果所述有效粒子数目未达到所述第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
第三方面，本发明实施例还提供了一种车载终端，包括：
存储有可执行程序代码的存储器；
与所述存储器耦合的处理器；
所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明任意实施例所提供的一种车辆位姿的修正的部分或全部步骤。
第四方面，本发明实施例还提供了一种计算机可读存储介质，其存储计算机程序，所述计算机程序包括用于执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤的指令。
第五方面，本发明实施例还提供了一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤。
本发明实施例提供的技术方案，当车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域时，通过基于车体的先验位置进行车辆位置信息的粒子采样，以利用多个粒子状态表征车辆的当前定位结果，从而可大大提高后续感知元素和地图元素匹配的成功率和稳定性。通过对采样的粒子进行位姿和权重信息的更新，可使设定数目的目标粒子的位置收敛到一个比较小的范围，从而利用目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系，该目标匹配关系可用于对先验位置处车体的位姿进行优化。相对于现有技术提供的使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位的方式，本实施例提供的方式避免了由于路面上的箭头信息过于稀疏而无法保证连续的厘米级高精定位的问题，通过基于目标匹配关系对先验位置处车体的位姿进行优化，解决了使用消费级预设定位装置定位精度不高的问题，可使得车辆的位姿达到厘米级的定位精度。
本发明的发明点包括：
1、通过基于车体的先验位置进行车辆位置信息的粒子采样，以通过多个粒子状态表征车辆的当前定位结果，大大提高了后续感知元素和地图元素匹配的成功率和稳定性。通过对采样的粒子进行位姿和权重信息的更新，可使设定数目的目标粒子的位置收敛到一个比较小的范围，从而利用目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和导航地图之间的目标匹配关系。利用目标匹配关系可对先验位置处车体的位姿进行优化，解决了现有技术在使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位时，由于路面上的箭头信息过于稀疏而导致的无法保证连续的厘米级高精定位的问题，实现了对车辆进行厘米级的高精度定位的技术效果。
2、本发明实施例提供的技术方案，通过结合车体先验位置和预设导航地图中车道线对车体位置进行2D空间采样，解决了在三维空间采样时粒子数目较多的问题，有效减少了粒子数量，极大地提升了算法的时间效率。
3、通过利用地图元素与车体先验位置之间的相对位置关系，构建地图元素在相机坐标系下设定方向的键值对信息，可使得每个粒子根据与车体先验位置的相对关系直接从键值对信息中搜索出该粒子前方的地图元素进行投影。解决了采用直接从预设导航地图中搜索地图元素的方式所导致的无法排除不可能投影到感知图像中的地图元素的问题，有效减少了做无效重投影的次数，提升了算法时间效率。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种系统状态切换示意图；
图2a是本发明实施例提供的一种车辆位姿的修正方法的流程示意图；
图2b是本发明实施例提供的一种粒子采样示意图；
图2c是本发明实施例提供的又一种粒子采样示意图；
图3a是本发明实施例提供的一种车辆位姿的修正方法的流程示意图；
图3b是本发明实施例提供的一种地图元素的搜索范围示意图；
图4是本发明实施例提供的一种车辆位姿的修正方法的流程示意图；
图5是本发明实施例提供的一种车辆位姿的修正装置的结构示意图；
图6是本发明实施例提供的一种车载终端的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。
为了更加清楚、明白地解释各实施例的内容，下面先对本发明实施例提供的技术方案的工作原理进行简单介绍：
本发明实施例的主要目的为：无人驾驶车辆利用廉价的IMU(Inertialmeasurement unit，惯性测量单元)、相机和GPS(Global Positioning System，全球定位系统)构成的定位模块，向本系统输入10m左右绝对位置误差的先验位置。当车辆刚刚驶入预设导航地图所覆盖的与先验位置对应的区域时，系统通过一系列算法将预设导航地图中的路灯杆、交通牌、车道线以及虚线端点等地图元素与无人驾驶相机获取的图像上的路灯杆、交通牌、车道线以及虚线端点等感知元素进行一对一的匹配，输出正确的感知元素与地图元素匹配对，以形成感知图像与预设导航地图之间正确的匹配关系。通过利用正确的匹配关系优化车体位姿，可使车体位姿达到厘米级的定位精度。在上述初始化匹配完成后算法也能提供连续的高精度定位。其中，系统在执行上述过程时具体包括如下几个状态：
(1)开始初始化状态；(2)初始化中状态；(3)初始化完成状态；(4)匹配状态。请参阅图1，图1是本发明实施例提供的一种系统状态切换示意图，如图1所示：
(1)开始初始化状态，表示一个初始化匹配完整流程的起始阶段，该阶段主要判断车辆的先验位置是否在预设导航地图所覆盖的区域内，如果在地图区域内，则进行车辆状态粒子采样，并切换到初始化中状态；如果不在，则继续保持开始初始化状态。
(2)初始化中状态，表示根据图像上感知到的信息与地图中的一定范围内的路灯杆、交通牌、车道线以及虚线端点进行粒子的权重更新、重采样、更新匹配关系、检测粒子状态是否收敛等，如果粒子收敛，则将状态切换到初始化完成状态；如果粒子未收敛，则继续保持初始化中状态；如果感知到的信息与地图中元素无任何匹配关系，则将状态切换到开始初始化状态。
(3)初始化完成状态，表示粒子状态已经收敛，并检测是否可以获得匹配，即判断匹配对数量是否足够以为车辆提供六自由度约束(满足条件A)，如果满足，则将状态切换到匹配状态；如果不满足，则将状态切换到开始初始化状态。
(4)匹配状态，表示可以直接利用车体的位姿信息获得正确的感知元素与地图元素的匹配关系。但如果连续帧感知图像存在匹配错误，则将匹配状态切换到开始初始化状态。
下面，分别对本发明实施例所涉及的各个状态的具体执行过程进行详细介绍。
实施例一
请参阅图2a，图2a是本发明实施例提供的一种车辆位姿的修正方法的流程示意图。该方法应用于自动驾驶中，该方法典型的是应用于车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域范围的场景下，其主要任务是在车体位置精度不高的情况下来产生正确的感知图像与预设导航地图之间的目标匹配关系，以利用该目标匹配关系优化出厘米级定位精度的车体位姿。本实施例提供的方法可由车辆位姿的修正装置来执行，该装置可通过软件和/或硬件的方式实现，一般可集成在车载电脑、车载工业控制计算机(Industrial personal Computer，IPC)等车载终端中，本发明实施例不做限定。如图2a所示，本实施例提供的方法具体包括：
110、当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样。
其中，车体的先验位置通过预设定位装置得到。预设定位装置为单点GPS(GlobalPositioning System，全球定位系统)、IMU和相机等低精度的消费级定位设备。导航地图是指应用于自动驾驶的误差级别为厘米级的高精度导航地图。高精度导航地图中具有交通牌、路灯杆、车道线和车道线虚线端点等元素的3D位置信息。
本实施例中，预设导航地图中存在与车体的先验位置对应的覆盖区域是指在预设导航地图中，在车辆先验位置设定范围内，例如十几米的范围内，可搜索到地图元素。其中，地图元素可以为交通牌、路灯杆、车道线或车道线虚线端点等。如果在预设导航地图中未搜索到任何地图元素，则说明预设导航地图中不存在与车体的先验位置对应的覆盖区域，即当前车辆并未驶入该预设导航地图所覆盖的范围，此时需保持系统的开始初始化状态。当间隔数帧图像时间戳的间隔后再次轮循检测预设导航地图中是否存在与车体的先验位置对应的覆盖区域，如果存在，则说明车辆驶入了预设导航地图所覆盖的范围，此时可进行车辆位姿信息的粒子采样。本实施例中，采用的是间隔数帧图像后再进行预设导航地图覆盖范围的检测，这样设置相对于每次来一帧图像都做检测的方式，可以提升计算效率。
本实施例中，当检测到车辆驶入预设导航地图与车体先验位置对应的覆盖区域后，可基于车辆位置信息进行粒子采样。采样得到的每个位置是一个粒子，每个粒子作为一个车体可能的位置。在采样过程中，可保持车体的姿态不变，因为车体的姿态在GPS航向信息和IMU的重力观测信息下误差较小，误差主要发生在车辆的位置上。
作为一种可选的实施方式，本实施例中，基于车体的先验位置进行车辆位置信息的粒子采样，具体可以为：
在与车体的先验位置满足第三设定距离的位置处，可按照高斯分布模型在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。本实施例中三维采样主要应用于在预设导航地图中未检索到车道线的情况。
作为另一种可选的的实施方式，本实施例中，基于车体的先验位置进行车辆位置信息的粒子采样，具体可以为：
在预设导航地图中，提取与车体的先验位置满足第一设定距离(例如距离车体的先验位置前后左右各15米的范围)的各目标车道线；对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离(例如两米)的位置处，基于二维平面上均匀分布的概率模型，对车辆位置信息进行2D空间的粒子采样。
具体的，图2b是本发明实施例提供的一种粒子采样示意图，如图2b所示，1和2表示距离车体先验位置满足第一设定距离的目标车道线，该目标车辆线上的黑点表示车道线离散点，在距离每个离散点第二设定距离的范围内可采样得到多个粒子，作为车体可能存在的位置。
具体的，图2c是本发明实施例提供的又一种粒子采样示意图，如图2c所示，可利用车道线的位置，在每两根车道线(1和2，或2和3)的中心点处进行粒子采样，得到一系列采样粒子。
相对于3D空间的粒子采样方式，上述2D空间的粒子采样结合了车体先验位置和预设导航地图的路面信息，利用预设导航地图中的车道线在车道线离散点周围2维平面进行随机采样，有效降低了粒子采样空间的维度，减少了粒子数量，极大的提升了算法的时间效率。
具体的，由于单点GPS高程上误差有时会达到十几米，经纬度误差也在10米左右，因此若覆盖车体先验位置前后左右各10米，高程上下各15米，如果采用3D空间的粒子采样方式，按照每立方米一个粒子计算需要20*20*30需要12000个粒子。而上述2D空间的粒子采样使用了预设导航地图中的车道线，车体的高度可通过车道线的高度得到确定，在使用了车道线离散出的点来采样车体位置粒子时，无需针对高度这个维度去进行采样，因此，采样维度可从三维空间采样降到二维，大大减小了粒子数量，提升了算法的运算速率。
120、更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件。
本实施例中，由于车辆在运动，采样得到的粒子的位姿也需要得到更新。其中，粒子的姿态可采用先验位置处车辆的姿态，粒子的位置的变化是不断迭代的过程，具体可按照如下公式进行更新：
Figure BDA0002077937260000101
其中，
Figure BDA0002077937260000102
和
Figure BDA0002077937260000103
表示第i号粒子在K时刻和K-1时刻的位姿状态；ΔTk表示第K-1时刻和K时刻的相对运动信息；ΔNk表示运动噪声。
基于上述公式，可确定任意一个粒子在当前时刻的位置信息。根据该位置信息，可搜索到预设导航地图中该位置信息对应的所有地图元素，从而建立地图元素与感知图像中元素的初始匹配关系，以根据该初始匹配关系对粒子的权重信息进行更新。
示例性的，更新每个粒子对应的权重信息的过程具体可以为：
对于采样得到的任意一个粒子，基于该粒子在当前时刻的位置信息，将预设导航地图中与当前时刻粒子的位置信息满足预设距离要求的地图元素均投影到感知图像上，并根据重投影残差的大小建立地图元素与感知图像中各感知元素之间的初始匹配关系；基于初始匹配关系，更新每个粒子对应的权重信息。
其中，感知图像是利用预设感知模型对摄像头采集的包含道路信息的图像进行识别后得到的。预设感知模型可以预先采用大量标注有图像语义特征的道路样本图像对感知模型进行训练。其中，图像语义特征可包括交通牌、路灯杆、车道线、车道线虚线端点等。通过将包含有道路信息的道路图像输入至训练好的预设感知模型，基于预设感知模型的识别结果，即可得到道路图像中的图像语义特征。其中，预设感知模型可以通过以下方式得到：
构建训练样本集，该训练样本集包括多组训练样本数据，每组训练样本数据包括道路样本图像和对应的标注有图像语义特征的道路感知样本图像；基于训练样本集对搭建的初始神经网络进行训练得到预设感知模型，该预设感知模型使得每组训练样本数据中的道路样本图像与对应的标注有图像语义特征的道路感知样本图像相关联。模型输出的即可称之为感知图像。感知图像中的各种道路信息可称为感知元素。
本实施例中，在建立地图元素和感知元素的初始匹配关系时，各地图元素与对应感知元素之间的重投影残差的大小需满足一定的阈值要求。初始匹配关系建立的越准确，根据该初始匹配关系所得到的粒子的权重信息越大。同样的，基于初始匹配关系更新出的粒子的权重信息的大小，也可反映出初始匹配关系建立的准确性，权重值越大，说明初始匹配关系建立的越准确。本实施例中，可将权重值低于设定阈值的粒子及其对应的匹配关系进行过滤。通过采用不断迭代的方式更新采样粒子的位置和每个粒子对应的权重信息，可使得目标粒子的位置越来越聚拢，直到设定数目的目标粒子的位置满足预设收敛条件。其中，预设收敛条件是指目标粒子的位置方差小于设定阈值。此时，即表示完成初始化过程。
130、根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于该状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系。
其中，车辆姿态为车辆先验姿态。示例性的，可按照如下公式根据每个目标粒子更新后的权重信息确定车体位置的状态量：
Figure BDA0002077937260000111
其中，xk表示k时刻车体位置的状态量；
Figure BDA0002077937260000112
是k时刻的粒子i的权重。
示例性的，基于该状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系是指：基于当前车辆的位姿，预设导航地图中的足够数量的地图元素与感知图像中对应的感知元素均能建立一一对应的匹配关系，且每组匹配对的重投影残差均符合预设阈值要求时，可说明感知图像和预设导航地图之间建立了目标匹配关系，足够数量的匹配对可为车辆提供六自由度约束，此时，可说明系统初始化成功，系统进入匹配状态。反之，如果系统初始化不成功，则返回执行步骤110，系统重新进入开始初始化状态。
140、基于目标匹配关系对先验位置处车体的位姿进行优化。
示例性的，在匹配状态下，可基于目标匹配关系对先验位置处车体的位姿进行六自由度的优化，具体优化过程可采用非线性优化算法来实现，从而可以获得厘米级的定位精度。
进一步的，随着车辆的运动，在后续每一帧利用外部输入的运动增量和车辆系统内部维护的厘米级定位的位置，可根据感知元素和地图元素重投影残差的大小进一步获取更加精确的目标匹配关系，并不断利用获取的目标匹配关系对车辆的位姿进行优化。
进一步的，随着车辆的运动，如果连续数帧感知图像都无法与预设导航地图建立目标匹配关系，则说明车辆驶出了预设导航地图的覆盖范围，此时，可返回执行步骤110，以重新进入开始初始化状态。
本实施例提供的技术方案，当车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域时，通过基于车体的先验位置进行车辆位置信息的粒子采样，以通过多个粒子状态表征当前定位结果，由于考虑到了车体可能存在的多个位置，可大大提高了后续感知元素和地图元素匹配的成功率和稳定性。通过对采样的粒子进行位姿和权重信息的更新，可使设定数目的目标粒子的位置收敛到一个比较小的范围，从而利用目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系，以基于目标匹配关系对所述先验位置处车体的位姿进行优化。相对于现有技术提供的使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位的方式，本实施例提供的方式避免了由于路面上的箭头信息过于稀疏，无法保证连续的厘米级高精定位的问题，通过基于目标匹配关系对先验位置处车体的位姿进行优化，可使的车辆的位姿达到厘米级的定位精度。
实施例二
请参阅图3a，图3a是本发明实施例提供的一种车辆位姿的修正方法的流程示意图。本实施例在上述实施例的基础上，对投影到感知图像上的地图元素的搜索过程进行了优化。如图3a所示，该方法包括：
210、当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样。
220、对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息。
230、将世界坐标系中，与车体先验位置满足第四设定距离的地图元素转换到相机坐标系下。
其中，相机坐标系定义为x轴朝右，y轴朝下，z轴朝前。本实施例中，相机坐标系的设定方向是指相机前方Z轴朝前的方向，即Z轴的正方向。
240、在相机坐标系的设定方向上，将满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对满足第四设定范围的地图元素进行排序。
示例性的，键值对信息MAP可以为红黑树或其他二叉树数据结构。本实施例中，构建键值对信息的好处在于，可以直接搜索出在Z轴方向上粒子前方的地图元素，从而减少了后续无效重投影的次数，提升了算法的时间效率。
250、将粒子在当前时刻的位置信息信息转换到相机坐标系下，并计算位置信息在相机坐标系设定方向上对应的目标位置。
260、从键值对信息中，依次查找目标位置对应的各地图元素。
示例性的，由于沿着Z轴方向，相机只能拍摄到它前方的物体，因此，从键值对信息中查找目标位置对应的各地图元素时，可直接搜索出在Z轴方向上粒子前方的地图元素。由于地图元素的种类较多，对于不同种类的地图元素，可根据Z轴坐标值的大小设置不同的搜索范围。图3b是本发明实施例提供的一种地图元素的搜索范围示意图。如图3b所示，z1表示粒子与车体先验位置之间的相对距离，z2表示地图元素与车体先验位置之间的相对距离。对于交通牌而言，可在Z轴方向上粒子前方所有的交通牌(图3b中为未示出)。对于路灯杆而言，可选取沿Z轴方向上粒子前方距离小于d1(如图3b中方框1的宽度)的范围作为搜索范围。对于车道线虚线端点，可选取沿Z轴方向上粒子前方距离小于d2(如图3b中方框2的宽度)的范围作为搜索范围。其中，由于路灯杆的在路面中的设置相对于车道线虚线端点而言较为稀疏，因此可优选设置为d1大于d2。
270、将各地图元素投影到感知图像上，并根据重投影残差的大小建立地图元素与感知图像中各感知元素之间的初始匹配关系。
280、基于初始匹配关系，更新每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件。
290、根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和预设导航地图之间的目标匹配关系，以基于目标匹配关系对先验位置处车体的位姿进行优化。
本实施例在上述实施例的基础，通过利用地图元素与车体先验位置之间的相对位置关系，构建了地图元素在相机坐标系下设定方向的键值对信息，这样每个粒子可根据与车体先验位置的相对关系直接从键值对信息搜索出粒子前方的地图元素进行投影。相对于直接从预设导航地图中搜索地图元素的方式，本实施例这样设置排除了不可能投影到感知图像中的地图元素，从而减少了做无效重投影的次数，提升了算法时间效率。
实施例三
请参阅图4，图4是本发明实施例提供的一种车辆位姿的修正方法的流程示意图，本实施例在上述实施例的基础上，对每个粒子对应的权重信息的更新以及感知图像和预设导航地图之间的目标匹配关系的建立过程进行了优化。如图4所示，本实施例提供的方法具体包括：
310、当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样。
320、更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件。
示例性的，如果设定数目的目标粒子的位置不满足预设收敛条件，则检测目标粒子中的有效粒子数目是否达第二预设数目阈值。如果有效粒子数目达不到第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
其中，目标粒子是否为有效粒子可通过如下公式进行判断：
Figure BDA0002077937260000141
其中，
Figure BDA0002077937260000142
表示有效粒子所占目标粒子的百分比，
Figure BDA0002077937260000143
表示第i个粒子的权重，Ns表示目标粒子的总个数。
330、根据每个目标粒子更新后的权重信息确定车体位置的状态量。
340、基于车体位置的状态量和车体姿态，将导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与感知图像中对应的感知元素建立一对一的匹配关系。
350、将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和预设导航地图之间的目标匹配关系。
其中，目标匹配关系相对于上述实施例中初始匹配关系的建立，需保证匹配对的数量是足够多的，即满足第一预设数目阈值，并且每组匹配对的重投影残差也应小于初始匹配关系建立是对应的重投影残差，即都满足预设阈值要求，从而为车辆提供六自由度的约束。
360、基于目标匹配关系对先验位置处车体的位姿进行优化。
本实施例中，如果足够数量的匹配对的重投影残差均达到预设阈值要求，则确定感知图像和预设导航地图之间建立了目标匹配关系。通过目标匹配关系对先验位置处车体的位姿六自由度进行优化，可以获得厘米级的定位精度。
实施例四
请参阅图5，图5是本发明实施例提供的一种车辆位姿的修正装置的结构示意图。如图5所示，该装置包括：粒子采样模块410、权重更新模块420、目标匹配关系建立模块430和位姿优化模块440；其中，
粒子采样模块410，被配置为当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
权重更新模块420，被配置为更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
目标匹配关系建立模块430，被配置为根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
位姿优化模块440，被配置为基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
本实施例提供的技术方案，当车辆第一次驶入预设导航地图所覆盖的与先验位置对应的区域时，通过基于车体的先验位置进行车辆位置信息的粒子采样，以通过多个粒子状态表征当前定位结果，由于考虑到了车体可能存在的多个位置，可大大提高了后续感知元素和地图元素匹配的成功率和稳定性。通过对采样的粒子进行位姿和权重信息的更新，可使设定数目的目标粒子的位置收敛到一个比较小的范围，从而利用目标粒子更新后的权重信息确定车体位置的状态量，并基于状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系，以基于目标匹配关系对所述先验位置处车体的位姿进行优化。相对于现有技术提供的使用卡尔曼滤波器融合GPS和地图中的箭头信息进行组合定位的方式，本实施例提供的方式避免了由于路面上的箭头信息过于稀疏，无法保证连续的厘米级高精定位的问题，通过基于目标匹配关系对先验位置处车体的位姿进行优化，可使的车辆的位姿达到厘米级的定位精度。
可选的，所述粒子采样模块，具体被配置为：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，在所述预设导航地图中，提取与车体的先验位置满足第一设定距离的各目标车道线；
对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离的位置处，对车辆位置信息进行2D空间的粒子采样。
可选的，所述粒子采样模块，具体被配置为：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，在与车体的先验位置满足第三设定距离的位置处，在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。
可选的，所述权重更新模块，包括：
位置信息确定单元，被配置为对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息；
投影单元，被配置为基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上；
初始匹配关系建立单元，被配置为根据重投影残差的大小建立所述地图元素与所述感知图像中各感知元素之间的初始匹配关系；
权重信息更新单元，被配置为基于所述初始匹配关系，更新每个粒子对应的权重信息。
可选的，所述投影单元，具体被配置为：
将所述位置信息转换到所述相机坐标系下，并计算所述位置信息在所述设定方向上对应的目标位置；
将世界坐标系中，与所述先验位置满足第四设定距离的地图元素转换到相机坐标系下；
在所述相机坐标系的设定方向上，将所述满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对所述满足第四设定范围的地图元素进行排序；
从所述键值对信息中，依次查找在所示设定方向上所述目标位置前方的各地图元素，并将各地图元素均投影到感知图像上。
可选的，所述权重信息更新单元，具体被配置为：
按照如下公式更新每个粒子对应的权重信息：
Figure BDA0002077937260000171
其中，
Figure BDA0002077937260000172
是k时刻的粒子i的权重；
Figure BDA0002077937260000173
是k-1时刻粒子i的权重；Ni为第i个粒子上获得的感知元素和地图元素的匹数数量；f(Ni)是Ni正相关的函数；sj为每种地图元素对应的一个归一化参数；rj第j对儿所述地图元素和所述感知元素匹配的图像重投影误差；R是图像上的观测误差；Exp指数项上除以了Ni将感知图像和导航地图匹配的数量进行了概率归一化。
可选的，所述目标匹配关系建立模块，具体被配置为：
根据每个目标粒子更新后的权重信息确定车体位置的状态量，基于所述状态量和车体姿态，将所述预设导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与所述感知图像中对应的感知元素建立一对一的匹配关系；
将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和所述预设导航地图之间的目标匹配关系。
可选的，所述装置还包括：
有效粒子检测模块，被配置为如果设定数目的目标粒子的位置不满足预设收敛条件，则检测所述目标粒子中的有效粒子数目是否达第二预设数目阈值；
重采样模块，被配置为如果所述有效粒子数目未达到所述第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
本发明实施例所提供的车辆位姿的修正装置可执行本发明任意实施例所提供的车辆位姿的修正方法，具备执行方法相应的功能模块和有益效果。未在上述实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的车辆位姿的修正方法。
实施例五
请参阅图6，图6是本发明实施例提供的一种车载终端的结构示意图。如图6所示，该车载终端可以包括：
存储有可执行程序代码的存储器701；
与存储器701耦合的处理器702；
其中，处理器702调用存储器701中存储的可执行程序代码，执行本发明任意实施例所提供的车辆位姿的修正方法。
本发明实施例公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序使得计算机执行本发明任意实施例所提供的车辆位姿的修正方法。
本发明实施例公开一种计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤。
在本发明的各种实施例中，应理解，上述各过程的序号的大小并不意味着执行顺序的必然先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
在本发明所提供的实施例中，应理解，“与A相应的B”表示B与A相关联，根据A可以确定B。但还应理解，根据A确定B并不意味着仅仅根据A确定B，还可以根据A和/或其他信息确定B。
另外，在本发明各实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
上述集成的单元若以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可获取的存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或者部分，可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干请求用以使得一台计算机设备(可以为个人计算机、服务器或者网络设备等，具体可以是计算机设备中的处理器)执行本发明的各个实施例上述方法的部分或全部步骤。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。
以上对本发明实施例公开的一种车辆位姿的修正方法和装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种车辆位姿的修正方法，应用于自动驾驶，其特征在于，包括：
当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
2.根据权利要求1所述的方法，其特征在于，所述基于车体的先验位置进行车辆位置信息的粒子采样，包括：
在所述预设导航地图中，提取与车体的先验位置满足第一设定距离的各目标车道线；
对于任意一条目标车道线，在与该目标车道线的离散点满足第二设定距离的位置处，对车辆位置信息进行2D空间的粒子采样。
3.根据权利要求1所述的方法，其特征在于，所述基于车体的先验位置进行车辆位置信息的粒子采样，包括：
在与车体的先验位置满足第三设定距离的位置处，在车体所在的三维空间进行车辆位置信息的粒子采样，得到呈3D概率分布的粒子。
4.根据权利要求1所述的方法，其特征在于，更新采样得到的粒子的位姿和每个粒子对应的权重信息，包括：
对于采样得到的任意一个粒子，确定该粒子在当前时刻的位置信息；
基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上，并根据重投影残差的大小建立所述地图元素与所述感知图像中各感知元素之间的初始匹配关系；
基于所述初始匹配关系，更新每个粒子对应的权重信息。
5.根据权利要求4所述的方法，其特征在于，基于所述位置信息，将所述预设导航地图中与所述位置信息满足预设距离要求的地图元素均投影到感知图像上，包括：
将所述位置信息转换到相机坐标系下，并计算所述位置信息在所述相机坐标系的设定方向上对应的目标位置；
将世界坐标系中，与所述先验位置满足第四设定距离的地图元素转换到所述相机坐标系下；
在所述相机坐标系的设定方向上，将所述满足第四设定距离的地图元素对应的坐标轴作为键，该地图元素对应的标识作为值，构建键值对信息以对所述满足第四设定范围的地图元素进行排序；
从所述键值对信息中，依次查找在所示设定方向上所述目标位置前方的各地图元素，并将各地图元素均投影到感知图像上。
6.根据权利要求4所述的方法，其特征在于，基于所述初始匹配关系，更新每个粒子对应的权重信息，包括：
按照如下公式更新每个粒子对应的权重信息：
Figure FDA0002077937250000021
其中，
Figure FDA0002077937250000022
是k时刻的粒子i的权重；
Figure FDA0002077937250000023
是k-1时刻粒子i的权重；Ni为第i个粒子上获得的感知元素和地图元素的匹数数量；f(Ni)是Ni正相关的函数；sj为每种地图元素对应的一个归一化参数；rj第j对儿所述地图元素和所述感知元素匹配的图像重投影误差；R是图像上的观测误差；Exp指数项上除以了Ni将感知图像和导航地图匹配的数量进行了概率归一化。
7.根据权利要求1所述的方法，其特征在于，根据每个目标粒子更新后的权重信息确定车体位置的状态量，包括：
按照如下公式根据每个目标粒子更新后的权重信息确定车体位置的状态量：
Figure FDA0002077937250000024
其中，xk表示k时刻车体位置的状态量；
Figure FDA0002077937250000025
是k时刻的粒子i的权重。
8.根据权利要求1-7任一所述的方法，其特征在于，基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系，包括：
基于所述状态量和车体姿态，将所述预设导航地图中满足第一预设数目阈值的地图元素均投影到感知图像所在平面，并与所述感知图像中对应的感知元素建立一对一的匹配关系；
将任意一组存在匹配关系的地图元素和感知元素作为匹配对，如果所有匹配对对应的重投影残差均满足预设阈值要求，则确定感知图像和所述预设导航地图之间的目标匹配关系。
9.根据权利要求1-8任一所述的方法，其特征在于，所述方法还包括：
如果设定数目的目标粒子的位置不满足预设收敛条件，则检测所述目标粒子中的有效粒子数目是否达第二预设数目阈值；
如果所述有效粒子数目未达到所述第二预设数目阈值，则根据更新后的权重值进行粒子重采样，直到设定数目的目标粒子的位置满足预设收敛条件。
10.一种车辆位姿的修正装置，应用于自动驾驶，其特征在于，包括：
粒子采样模块，被配置为当检测到预设导航地图中存在与车体的先验位置对应的覆盖区域时，基于车体的先验位置进行车辆位置信息的粒子采样；其中，所述先验位置通过预设定位装置得到；
权重更新模块，被配置为更新采样得到的粒子的位姿和每个粒子对应的权重信息，以使设定数目的目标粒子的位置满足预设收敛条件；
目标匹配关系建立模块，被配置为根据每个目标粒子更新后的权重信息确定车体位置的状态量，并基于所述状态量和车体姿态获得感知图像和所述预设导航地图之间的目标匹配关系；
位姿优化模块，被配置为基于所述目标匹配关系对所述先验位置处车体的位姿进行优化。
CN201910460548.7A 2019-05-30 2019-05-30 一种车辆位姿的修正方法和装置 Active CN110954113B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201910460548.7A CN110954113B (zh) 2019-05-30 2019-05-30 一种车辆位姿的修正方法和装置
PCT/CN2019/113486 WO2020237996A1 (zh) 2019-05-30 2019-10-26 一种车辆位姿的修正方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910460548.7A CN110954113B (zh) 2019-05-30 2019-05-30 一种车辆位姿的修正方法和装置

Publications (2)

Publication Number Publication Date
CN110954113A true CN110954113A (zh) 2020-04-03
CN110954113B CN110954113B (zh) 2021-10-15

Family

ID=69976186

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910460548.7A Active CN110954113B (zh) 2019-05-30 2019-05-30 一种车辆位姿的修正方法和装置

Country Status (2)

Country Link
CN (1) CN110954113B (zh)
WO (1) WO2020237996A1 (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112747744A (zh) * 2020-12-22 2021-05-04 浙江大学 一种结合航位推算和多车道路网地图的车辆定位方法
CN112902987A (zh) * 2021-02-02 2021-06-04 北京三快在线科技有限公司 一种位姿修正的方法及装置
CN113447023A (zh) * 2021-06-18 2021-09-28 上海宏景智驾信息科技有限公司 一种基于吊灯的室内视觉定位方法
CN114088114A (zh) * 2021-11-19 2022-02-25 智道网联科技(北京)有限公司 车辆位姿校准方法、装置和电子设备
CN114248778A (zh) * 2020-09-22 2022-03-29 华为技术有限公司 移动设备的定位方法和定位装置
CN114323033A (zh) * 2021-12-29 2022-04-12 北京百度网讯科技有限公司 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114419590A (zh) * 2022-01-17 2022-04-29 北京百度网讯科技有限公司 高精地图的验证方法、装置、设备以及存储介质
CN115143996A (zh) * 2022-09-05 2022-10-04 北京智行者科技股份有限公司 定位信息修正方法及电子设备和存储介质
CN115248040A (zh) * 2022-09-22 2022-10-28 毫末智行科技有限公司 初始化定位方法、装置、终端及存储介质
CN115388906A (zh) * 2022-08-24 2022-11-25 上海安亭地平线智能交通技术有限公司 位姿的确定方法、装置、电子设备和存储介质
CN117485356A (zh) * 2023-12-29 2024-02-02 合众新能源汽车股份有限公司 基于风险等级评估的高精定位故障诊断方法和系统

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112558130B (zh) * 2020-12-07 2023-12-19 安徽江淮汽车集团股份有限公司 定位数据的同步方法、装置、设备及存储介质
CN113465620A (zh) * 2021-06-02 2021-10-01 上海追势科技有限公司 一种基于语义信息的停车场粒子滤波定位方法
CN114001742A (zh) * 2021-10-21 2022-02-01 广州小鹏自动驾驶科技有限公司 车辆定位方法、装置、车辆和可读存储介质
CN114111817B (zh) * 2021-11-22 2023-10-13 武汉中海庭数据技术有限公司 基于slam地图与高精度地图匹配的车辆定位方法及系统
CN114355415A (zh) * 2022-01-06 2022-04-15 上海安亭地平线智能交通技术有限公司 位姿信息的确定方法和装置、电子设备和存储介质
CN114119673B (zh) * 2022-01-25 2022-04-22 北京地平线机器人技术研发有限公司 初始位姿的确定方法和装置、电子设备和存储介质
CN115235493A (zh) * 2022-07-19 2022-10-25 合众新能源汽车有限公司 一种基于矢量地图进行自动驾驶定位的方法及装置

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101509781A (zh) * 2009-03-20 2009-08-19 同济大学 基于单目摄像头的步行机器人定位系统
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN108732603A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于定位车辆的方法和装置
CN108759833A (zh) * 2018-04-25 2018-11-06 中国科学院合肥物质科学研究院 一种基于先验地图的智能车辆定位方法
CN108871341A (zh) * 2018-07-05 2018-11-23 内江市下代互联网数据处理技术研究所 一种全局优化的并发定位与建图方法
CN109297500A (zh) * 2018-09-03 2019-02-01 武汉中海庭数据技术有限公司 基于车道线特征匹配的高精度定位装置及方法
CN109556611A (zh) * 2018-11-30 2019-04-02 广州高新兴机器人有限公司 一种基于图优化和粒子滤波的融合定位方法
US20190107396A1 (en) * 2017-10-06 2019-04-11 Cisco Technology, Inc. Collaborative localization between phone and infrastructure

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101625572B (zh) * 2009-08-10 2011-05-11 浙江大学 基于改进重采样方法和粒子选取的FastSLAM算法
KR20150058679A (ko) * 2013-11-20 2015-05-29 한국전자통신연구원 단지내 도로에서 자율주행차량의 위치 및 해딩 정보 제공 장치 및 방법
CN106940704B (zh) * 2016-11-25 2019-12-20 北京儒博科技有限公司 一种基于栅格地图的定位方法及装置
CN108053443B (zh) * 2017-11-20 2019-08-02 中国科学院空间应用工程与技术中心 一种基于粒子群优化的物体点云位姿估算方法和系统

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101509781A (zh) * 2009-03-20 2009-08-19 同济大学 基于单目摄像头的步行机器人定位系统
CN108732603A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于定位车辆的方法和装置
US20190107396A1 (en) * 2017-10-06 2019-04-11 Cisco Technology, Inc. Collaborative localization between phone and infrastructure
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN108759833A (zh) * 2018-04-25 2018-11-06 中国科学院合肥物质科学研究院 一种基于先验地图的智能车辆定位方法
CN108871341A (zh) * 2018-07-05 2018-11-23 内江市下代互联网数据处理技术研究所 一种全局优化的并发定位与建图方法
CN109297500A (zh) * 2018-09-03 2019-02-01 武汉中海庭数据技术有限公司 基于车道线特征匹配的高精度定位装置及方法
CN109556611A (zh) * 2018-11-30 2019-04-02 广州高新兴机器人有限公司 一种基于图优化和粒子滤波的融合定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
HYUN CHUL ROH; CHANG HUN SUNG; MIN TAE KANG; MYUNG JIN CHUNG: "Fast SLAM using polar scan matching and particle weight based occupancy grid map for mobile robot", 《2011 8TH INTERNATIONAL CONFERENCE ON UBIQUITOUS ROBOTS AND AMBIENT INTELLIGENCE》 *
王勇，等: "面向动态高遮挡环境的移动机器人自适应位姿跟踪算法", 《机器人》 *

Cited By (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114248778A (zh) * 2020-09-22 2022-03-29 华为技术有限公司 移动设备的定位方法和定位装置
WO2022062480A1 (zh) * 2020-09-22 2022-03-31 华为技术有限公司 移动设备的定位方法和定位装置
CN114248778B (zh) * 2020-09-22 2024-04-12 华为技术有限公司 移动设备的定位方法和定位装置
CN112747744A (zh) * 2020-12-22 2021-05-04 浙江大学 一种结合航位推算和多车道路网地图的车辆定位方法
CN112747744B (zh) * 2020-12-22 2022-11-18 浙江大学 一种结合航位推算和多车道路网地图的车辆定位方法
CN112902987A (zh) * 2021-02-02 2021-06-04 北京三快在线科技有限公司 一种位姿修正的方法及装置
CN112902987B (zh) * 2021-02-02 2022-07-15 北京三快在线科技有限公司 一种位姿修正的方法及装置
CN113447023B (zh) * 2021-06-18 2022-11-01 上海宏景智驾信息科技有限公司 一种基于吊灯的室内视觉定位方法
CN113447023A (zh) * 2021-06-18 2021-09-28 上海宏景智驾信息科技有限公司 一种基于吊灯的室内视觉定位方法
CN114088114A (zh) * 2021-11-19 2022-02-25 智道网联科技(北京)有限公司 车辆位姿校准方法、装置和电子设备
CN114088114B (zh) * 2021-11-19 2024-02-13 智道网联科技(北京)有限公司 车辆位姿校准方法、装置和电子设备
CN114323033B (zh) * 2021-12-29 2023-08-29 北京百度网讯科技有限公司 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114323033A (zh) * 2021-12-29 2022-04-12 北京百度网讯科技有限公司 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114419590A (zh) * 2022-01-17 2022-04-29 北京百度网讯科技有限公司 高精地图的验证方法、装置、设备以及存储介质
CN114419590B (zh) * 2022-01-17 2024-03-19 北京百度网讯科技有限公司 高精地图的验证方法、装置、设备以及存储介质
CN115388906A (zh) * 2022-08-24 2022-11-25 上海安亭地平线智能交通技术有限公司 位姿的确定方法、装置、电子设备和存储介质
CN115143996A (zh) * 2022-09-05 2022-10-04 北京智行者科技股份有限公司 定位信息修正方法及电子设备和存储介质
CN115248040A (zh) * 2022-09-22 2022-10-28 毫末智行科技有限公司 初始化定位方法、装置、终端及存储介质
CN115248040B (zh) * 2022-09-22 2022-12-23 毫末智行科技有限公司 初始化定位方法、装置、终端及存储介质
CN117485356A (zh) * 2023-12-29 2024-02-02 合众新能源汽车股份有限公司 基于风险等级评估的高精定位故障诊断方法和系统
CN117485356B (zh) * 2023-12-29 2024-04-05 合众新能源汽车股份有限公司 基于风险等级评估的高精定位故障诊断方法和系统

Also Published As

Publication number Publication date
CN110954113B (zh) 2021-10-15
WO2020237996A1 (zh) 2020-12-03

Similar Documents

Publication Publication Date Title
CN110954113B (zh) 一种车辆位姿的修正方法和装置
CN109214248B (zh) 用于识别无人驾驶车辆的激光点云数据的方法和装置
CN111750881B (zh) 一种基于路灯杆的车辆位姿的修正方法和装置
CN108280866B (zh) 道路点云数据处理方法及系统
US20220011117A1 (en) Positioning technology
CN111750878A (zh) 一种车辆位姿的修正方法和装置
WO2011160672A1 (en) Method for obtaining drivable road area
CN111680747B (zh) 用于占据栅格子图的闭环检测的方法和装置
CN111750882B (zh) 一种导航地图在初始化时车辆位姿的修正方法和装置
WO2023185354A1 (zh) 实景导航方法、装置、设备及存储介质、程序产品
CN111179162A (zh) 一种特殊环境下的定位初始化方法及车载终端
CN111274847A (zh) 一种定位方法
CN112067006B (zh) 一种基于语义的视觉定位方法及装置
CN113223064B (zh) 一种视觉惯性里程计尺度的估计方法和装置
CN112257668A (zh) 主辅路判断方法、装置、电子设备及存储介质
CN114758086A (zh) 一种城市道路信息模型的构建方法及装置
KR102249381B1 (ko) 3차원 영상 정보를 이용한 모바일 디바이스의 공간 정보 생성 시스템 및 방법
CN114280583B (zh) 无gps信号下激光雷达定位精度验证方法及系统
CN113052904B (zh) 一种定位方法、装置、电子设备及存储介质
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN109901589B (zh) 移动机器人控制方法和装置
CN115841514A (zh) 一种自动泊车方法、装置及设备
CN116503482B (zh) 车辆位置的获取方法、装置及电子设备
CN116878487B (zh) 一种用于建立自动驾驶地图的方法、装置、车辆和服务器
CN111684457B (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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20220309

Address after: 100083 unit 501, block AB, Dongsheng building, No. 8, Zhongguancun East Road, Haidian District, Beijing

Patentee after: BEIJING MOMENTA TECHNOLOGY Co.,Ltd.

Address before: 100083 room 28, 4 / F, block a, Dongsheng building, 8 Zhongguancun East Road, Haidian District, Beijing

Patentee before: BEIJING CHUSUDU TECHNOLOGY Co.,Ltd.