CN114018274B - 车辆定位方法、装置及电子设备 - Google Patents

车辆定位方法、装置及电子设备 Download PDF

Info

Publication number
CN114018274B
CN114018274B CN202111367474.6A CN202111367474A CN114018274B CN 114018274 B CN114018274 B CN 114018274B CN 202111367474 A CN202111367474 A CN 202111367474A CN 114018274 B CN114018274 B CN 114018274B
Authority
CN
China
Prior art keywords
data
frame
sensor data
constraint
frames
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.)
Active
Application number
CN202111367474.6A
Other languages
English (en)
Other versions
CN114018274A (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.)
Apollo Intelligent Technology Beijing Co Ltd
Original Assignee
Apollo Intelligent Technology Beijing 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 Apollo Intelligent Technology Beijing Co Ltd filed Critical Apollo Intelligent Technology Beijing Co Ltd
Priority to CN202111367474.6A priority Critical patent/CN114018274B/zh
Publication of CN114018274A publication Critical patent/CN114018274A/zh
Priority to US17/931,777 priority patent/US11733398B2/en
Application granted granted Critical
Publication of CN114018274B publication Critical patent/CN114018274B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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
    • 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/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
    • 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
    • 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/53—Determining attitude
    • 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)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本公开提供了一种车辆定位方法、装置及电子设备，涉及自动驾驶技术领域，尤其涉及车辆定位技术领域。具体实现方案为：基于滑动窗口获取车辆的N帧传感器数据，所述N为正整数；基于所述N帧传感器数据构建因子图模型的目标函数，并对所述目标函数进行优化求解；基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿。

Description

车辆定位方法、装置及电子设备
技术领域
本公开涉及自动驾驶技术领域，尤其涉及车辆定位技术领域，具体涉及一种车辆定位方法、装置及电子设备。
背景技术
高精定位是基于高精地图数据，融合摄像头、惯性测量单元(InertialMeasurement Unit，IMU)等设备的数据输入，加上车速、轮速等信息，综合产出车辆相对位置或车道线模型、高精地图信息的服务，是自动驾驶中不可或缺的功能模块。目前，主要基于单帧的感知数据与高精地图进行匹配，以获得车辆的位置信息。
发明内容
本公开提供了一种车辆定位方法、装置及电子设备。
根据本公开的第一方面，提供了一种车辆定位方法，包括：
基于滑动窗口获取车辆的N帧传感器数据，所述N为正整数；
基于所述N帧传感器数据构建因子图模型的目标函数，并对所述目标函数进行优化求解；
基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿。
根据本公开的第二方面，提供了一种车辆定位装置，包括：
获取模块，用于基于滑动窗口获取车辆的N帧传感器数据，所述N为正整数；
优化模块，用于基于所述N帧传感器数据构建因子图模型的目标函数，并对所述目标函数进行优化求解；
确定模块，用于基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿。
根据本公开的第三方面，提供了一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行第一方面所述的方法。
根据本公开的第四方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据第一方面所述的方法。
根据本公开的第五方面，提供了一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现根据第一方面所述的方法。
本公开实施提供的方案，在获取到车辆的N帧传感器数据后，对N帧传感器数据基于因子图模型构建目标函数来求解车辆的位置，这样能够有效融合传感器数据来对车辆位姿进行定位，有助于提升定位精度。
应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案，不构成对本公开的限定。其中：
图1是本公开实施例提供的一种车辆定位方法的流程图之一；
图2是本公开实施例提供的一种车辆定位方法中滑动窗口的示意图；
图3是本公开实施例提供的一种车辆定位方法中因子图模型的示意图之一；
图4是本公开实施例提供的一种车辆定位方法中因子图模型的示意图之二；
图5是本公开实施例提供的一种车辆定位方法中车道线跟踪的示意图；
图6是本公开实施例提供的一种车辆定位方法中车道线关联的示意图；
图7是本公开实施例提供的一种车辆定位方法中相邻帧车道线数据合并的示意图；
图8是本公开实施例提供的一种车辆定位方法的流程图之二；
图9本公开实施例提供的一种车辆定位装置的结构图；
图10是用来实现本公开实施例的车辆定位方法的电子设备的框图。
具体实施方式
以下结合附图对本公开的示范性实施例做出说明，其中包括本公开实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本公开的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
本公开实施例提供了一种车辆定位方法。
请参照图，图1是本公开实施例提供的一种车辆定位方法的流程图之一。如图1所示，所述车辆定位方法包括以下步骤：
步骤S101、基于滑动窗口获取车辆的N帧传感器数据，所述N为正整数。
需要说明地，本公开实施例提供的车辆定位方法可以是应用于电子设备，如手机、笔记本电脑、计算机、车载终端等，如下实施例中将以电子设备作为执行主体来对本公开的车辆定位方法进行具体说明。
本公开实施例中，电子设备基于预设的滑动窗口来获取车辆的N帧传感器数据。其中，所述滑动窗口为长度固定的能够向一个方向滑动的窗口，也即滑动窗口能够获取的传感器数据的最大帧数是固定的，例如滑动窗口最多能够获取10帧传感器数据，则N的取值也就不能超过10。
例如，请参照图2，滑动窗口在滑动过程中，每获取到一帧新的传感器数据，则会将最前的一帧传感器数据移除，进而以确保滑动窗口中的帧数是固定的。
步骤S102、基于所述N帧传感器数据构建因子图模型的目标函数，并对所述目标函数进行优化求解。
本步骤中，电子设备基于获取到的N帧传感器数据构建因子图模型的目标函数；或者，电子设备也可以是在滑动窗口中的传感器数据满足能够获取的最大帧数的情况下再构建因子图模型的目标函数，例如滑动窗口能够获取的传感器数据的最大帧数为预设数值，比如预设数值为10，则在N为10的情况下，基于这10帧传感器数据构建因子图模型的目标函数。
需要说明地，因子图是一个二部图，图中包括两类节点：变量节点和因子节点；变量节点指的是待估计的优化变量，因子节点指的是获取到的传感器数据对优化变量的约束，所有因子连乘即可得到所有变量的联合概率分布，状态估计即为找到最优变量以最大化联合概念。为了方便对因子取对数及相反数，连乘转化为求和，求最大值转化为求最小值；本公开实施例中，基于N帧传感器数据构建的因子图模型的目标函数表示为：
其中，x是优化变量，rk表示残差，T代表向量装置，Ωk是每个残差对应的信息矩阵，它是协方差矩阵Σk的逆，优化目标是
可选地，在本公开一种可选的实施方式中，每一帧传感器数据包括全球导航卫星系统(Global Navigation Satellite System，GNSS)数据、车道线数据、轮速计数据和惯性测量单元(Inertial measurement unit，IMU)数据；所述因子图模型的目标函数与GNSS约束、车道线约束、轮速计速度约束、预积分约束和边缘化约束相关；其中，所述GNSS约束基于所述GNSS数据获取，所述车道线约束基于所述车道线数据获取，所述轮速计速度约束基于所述轮速计数据获取，所述预积分约束基于所述轮速计数据和所述IMU数据获取，所述边缘化约束基于所有帧的目标传感器数据获取，所述目标传感器数据为所述GNSS数据、车道线数据、轮速计数据和IMU数据中的至少一项。
需要说明地，所述GNSS数据、车道线数据、轮速计数据和IMU数据可以分别通过对应的传感器或相关设备来观测得到，例如车道线数据可以是通过车辆上的摄像头拍摄得到，轮速计数据可以是基于车辆上的速度传感器来检测得到。
可选地，车辆的传感器数据是实时获取的，不同帧的传感器数据代表的是不同时刻的传感器数据。本公开实施例中，N可以为大于1的整数，电子设备基于N帧传感器数据来构建因子图模型的目标函数，也就会融合多个时刻的GNSS数据、IMU数据、轮速计数据和车道线数据来求解因子图模型的目标函数，多个传感器数据的融合，能够更有助于提升车辆位姿定位的准确性。
本公开实施例中采用的因子图模型如图3所示，图3中圆形表示变量节点(又称优化变量)，其他形状则表示因子节点，图中变量节点包括以下5种：位置、姿态、速度、陀螺仪零偏、轮速计尺度。图3中，圆形节点节点T(例如To、Ti、Tj)表示位置和姿态，圆形节点M(例如Mo、Mi、Mj)表示速度和陀螺仪零偏，节点sv表示轮速计尺度。由于车轮半径误差等原因导致轮速计得到的车速与实际速度存在一个比例因子，所以可以对轮速计尺度进行估计。因子节点包括以下4种：GNSS约束、预积分约束、车道线约束和边缘化约束；图2中菱形节点表示GNSS约束，五边形节点表示预积分约束(又可称轮速计/IMU约束)，正三角形节点表示车道线约束，五角星节点表示边缘化约束。可选地，为了控制外点对因子图模型优化求解的影响，可以对诸如GNSS、车道线这种会出现异常数据的传感器提供的约束添加鲁棒核函数。
如图3所示，图中的直线将因子节点与相关的变量节点相连，约束因子可直接影响与其相连的变量。另外，约束因子也可以通过与其相连的变量间接影响其他变量，例如GNSS约束和预积分约束同时约束速度，而预积分约束又与轮速计尺度相关，因此GNSS约束间接约束了轮速计尺度。由图3可知，GNSS约束可提供位置、航向角和速度，因此与GNSS约束因子直接关联的优化变量包括Tk(如图3所示，k取值范围为o……i，j，o为滑动窗口内的第一帧，j为滑动窗口内的最后一帧)和Mk；预积分约束可提供相邻帧之间的相对位姿、速度、轮速计尺度和零偏不稳定性约束，因此关联的优化变量包括Tk-1、Tk、Mk-1、Mk、sv；车道线约束关联的优化变量是两帧的位姿Th(h为k的前一帧)和Tk；边缘化约束是边缘化最老帧时相关因子对剩余变量节点的约束，关联的变量与被边缘化的因子有关。
或者，在本公开另一种可选的实施方式中，每一帧传感器数据除了包括GNSS数据、车道线数据、轮速计数据和IMU数据之外，还包括高精地图数据，该实施方式下，因子图模型的目标函数还与高精地图匹配约束相关，所述高精地图匹配约束基于所述高精地图数据和所述车道线数据获取。
请参照图4，图4是本公开实施例中可应用的因子图模型的示意图之二。如图4所示，所述因子图模型的变量节点包括以下6种：位置、姿态、速度、陀螺仪零偏、轮速计尺度和高精地图偏移；因子节点包括以下5种：GNSS约束、预积分约束、车道线约束、边缘化约束和高精地图匹配约束。图4中节点T表示位置和姿态，节点M表示速度和陀螺仪零偏，节点sv表示轮速计尺度，节点bm表示高精地图偏移，图4中菱形节点表示GNSS约束，五边形节点表示预积分约束(又可称轮速计/IMU约束)，正三角形节点表示车道线约束，五角星节点表示边缘化约束，倒三角形节点表示高精地图匹配约束。如图4所示，高精地图偏移与高精地图匹配约束有关，高精地图匹配约束与位置和姿态相关，位置和姿态又受到预积分约束、GNSS约束、边缘化约束的影响。该实施方式中，通过融合多帧传感器数据，也即融合多个时刻的GNSS数据、IMU数据、轮速计数据、车道线数据并利用高精地图进行匹配，以通过目标函数计算得到车辆的位姿，通过引入了高精地图匹配约束，使得车辆的位姿与高精地图匹配相关，以提升车辆位姿的定位精度。
可选地，本公开实施例的后续实施方式中将以图4所示的因子图模型来构建目标函数，以计算得到车辆的位姿。
步骤S103、基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿。
本公开实施例中，车辆的传感器数据包括GNSS数据、车道线数据、轮速计数据和IMU数据，基于N帧传感器数据来构建因子图模型，以得到因子图模型的目标函数，对目标函数优化求解，以得到优化求解后的目标值，所述目标值也就是车辆的位姿。其中，所述位姿包括车辆的位置和姿态。
如前所述，因子图模型包括变量节点和因子节点，本公开实施例中通过N帧传感器数据来构建因子图模型的目标函数，而N的取值与滑动窗口的长度有关，也即通过滑动窗口限定了传感器数据的数量，这样也就限定了目标函数计算中需要优化的变量个数，也就能够避免因子图模型计算中计算量的无限制增长，以简化计算量，也就能够更加快捷地得到车辆的位姿。另外，通过滑动窗口的长度来限定N的取值，也就可以基于多帧传感器数据来计算得到车辆位姿，相比于通过单帧数据来计算车辆的位姿，本公开中能够基于多帧传感器数据进行融合优化，能够利用多帧传感器数据之间的关联性来求解车辆的位姿，更有助于提升定位精度。
可选地，本公开实施例中优化求解后得到的位姿通过世界坐标系中的坐标值来表征，世界坐标系可以是定义为局部东北天坐标系，该局部东北天坐标系的坐标原点可以是基于所述N帧中的中间帧对应的GNSS数据换算得到。
为更好地理解，通过以下表格对图4所示因子图模型涉及的6种优化变量进行定义。
需要说明地，表格中的x、y、z分代表世界坐标系(也即局部东北天坐标系)中的x轴、y轴、z轴。在进行目标函数求解时，真正优化的量是各个优化变量的误差值而非名义值，名义值的自由度称为全局自由度，误差值的自由度称为局部自由度。自由变量的全局自由度和局部自由度相等，除了姿态以外，其他的优化变量均为不受约束的自由变量，更新方式可以是向量加法。姿态全局自由度为4，局部自由度为3，名义值用单位四元数(或旋转矩阵)表示，误差值是局部切空间的角度变化，优化时是通过右乘误差值指数映射的方式进行更新的。若整个滑动窗口中共有(j-o+1)个待优化的帧，每帧的自由度为3+3+3+3＝12(也即位置、姿态、速度和陀螺仪零偏的自由度之和)，再加上轮速计尺度与高精地图偏移，所有求解变量的自由度为12*(j-o+1)+4。
本公开实施例中，因子图模型的目标函数表示为：
其中，x是优化变量，rk表示残差，T代表向量装置，Ωk是每个残差对应的信息矩阵，它是协方差矩阵∑k的逆，优化目标是
可选地，上述目标函数的优化求解可以是采用高斯牛顿法、Levenberg-Marquard法或Dogleg法等非线性优化方法，这些方法避免直接求复杂的Hessian矩阵，而是对每个残差采用一阶泰勒展开做近似，展开形式可表示为rk(x*+Δx)＝rk(x*)+JkΔx，其中Jk为残差对优化变量的一阶导，也就是雅可比矩阵。因此，每个因子节点都需要获取残差、雅可比和信息矩阵。另外，M估计能够将上述目标函数转化为：
其中，ρ为鲁棒性函数，可选地，鲁棒性函数包括Huber损失函数、Cauchy损失函数等。
本公开实施例中，每种传感器数据对目标函数的优化求解是通过因子节点的方式给出的，因子节点需要明确定义对应的残差、对优化变量的雅可比以及信息矩阵。以下对因子图模型中可能影响到各因子节点的每种约束进行详细说明。
(1)GNSS约束
可以理解地，GNSS数据包括在大地坐标系下的经度、纬度和高度，还可以包括在局部东北天坐标系下的航向角和速度。经度、纬度和高度转换到局部东北天坐标系后直接提位置在局部东北天坐标系x轴、y轴、z轴三个方向的先验约束，航向角则为天向的旋转角度提供先验约束，速度也可提供x轴、y轴、z轴三个方向的先验约束。因此，位置约束维度为3，姿态约束维度为1，速度约束维度为3，进而GNSS约束下的残差是一个7维的向量，定义如下：
其中，rgnss表示GNSS残差，rp表示位置残差，rq表示姿态残差，rv表示速度残差，pk、qk、vk分别为第k帧的位置、姿态和速度，[pe，pn，pu]T为对应时刻的GNSS位置观测的向量转置，qhead为GNSS航向角观测所构造的姿态，表达式为qhead＝(cos(yaw/2)，0，0，sin(yaw/2))T，yaw为航向角，T为向量转置，[ve，vn，vu]T为GNSS在东向、北向和天向的速度观测的向量转置，下标x、y或z是取向量对应的分量。基于上述残差求得残差关于优化变量误差值(先后顺序为δpk、δθk、δvk)的雅可比，用如下公式表示：
其中，Jgnss表示GNSS约束中对应的雅可比，I为单位阵，[q]L表示单位四元数对应的左乘矩阵(4行4列)，下标brl3表示右下角的1行3列矩阵块，这里给出单位四元数的左乘矩阵[q]L和右乘矩阵[q]R分别为：
残差对应的协方差是一个对角阵，对角线元素为噪声标准差的平方，基于此得到信息矩阵为：
其中，Ωgnss表示GNSS约束中对应的信息矩阵，pe，pn，pu为GNSS在东向、北向和天向的位置观测，ve，vn，vu分别为GNSS在东向、北向和天向的速度观测，yaw为航向角，n表示相应的噪声，nyaw例如表示航向角的噪声。
(2)轮速计速度约束
可以理解地，由于车辆的速度也是状态量，轮速计可以对车辆的速度直接观测，轮速计对应的残差维度为3，用如下公式表示：
其中，rv表示轮速计残差，vwheel是轮速计在第k帧的观测值，sv是轮速计尺度真实值，qk、vk分别是车辆在第k帧的姿态和速度，轮速计残差关于优化变量误差值(先后顺序为δθk、δvk、δsv)的雅可比(维度为3×7)用如下公式表示：
其中，Jv表示轮速计速度约束中对应的雅可比，Rk表示车辆在第k帧的姿态，vk是车辆在第k帧的速度，sv是轮速计尺度真实值。
信息矩阵的对角元素分别对应三个方向的速度误差，用如下公式表示：
其中，Ωv表示轮速计速度约束中对应的信息矩阵，vx，vy，vz分别表示轮速计在东向、北向和天向的速度观测，n表示相应的噪声。
(3)预积分约束
由于俯仰角和横滚角的观测度较低，因此如果采用IMU做预积分约束，两个角度的误差以及重力加速度和加速度计零偏的耦合作用会导致平移量预积分精度变差。本公开实施例中，预积分约束采用轮速计加陀螺仪的方式约束相邻两帧的相对运动。其中，预积分约束对应的残差包括相对平移、相对旋转和陀螺仪零偏不稳定性3个部分，维度均为3，残差总计维度为9。预积分约束对应的残差通过如下公式表示：
其中，rvg表示预积分约束对应的残差，rp表示位置残差，rq表示姿态残差，rbg表示陀螺仪残差，pk表示车辆在第k帧的位置，pk-1表示车辆在第k-1帧的位置，qk表示车辆在第k帧的姿态，qk-1表示车辆在第k-1帧的姿态，bk表示陀螺仪在第k帧的零偏，bk-1表示陀螺仪在第k-1帧的零偏，α是相对平移(三维变量)预积分名义值，γ是相对旋转(旋转矩阵)预积分名义值，分别需要积分两帧之间线速度和角速度的测量值得到。残差中相关的优化变量误差值包括δpk-1、δθk-1、δpk、δθk、/>δsv，雅可比(维度为9×19)用如下公式表示：
其中，和/>(δα和δθ分别为α和γ的误差扰动)可以在预积分递推过程中得到(后续会具体说明)。
残差的协方差就是预积分量误差值的协方差，是传感器测量噪声通过预积分传递的结果，预积分约束对应的信息矩阵通过如下公式表示：
假设第k-1帧到k帧的传感器数据序列为0，1，...，m，下面推导如何基于传感器的测量值和噪声得到预积分量的名义值和协方差。
1)误差模型
预积分量的误差模型如下：
αt＝α+δα
其中，αt是平移的预积分量，α是平移的名义值，δα是平移的误差值，γt是旋转(或者也可称姿态)的预积分量，γ是旋转的名义值，δθ是旋转的误差值，是陀螺仪零偏的真实值，bg是陀螺仪零偏的真实值，δbg是陀螺仪零偏的误差值，/>是轮速计尺度的真实值，sv是轮速计尺度的名义值，δsv是轮速计尺度的误差值。
误差模型表述了真实值、名义值与误差值之间的关系。由于平移预积分时用到了轮速计尺度，因此也将其作为状态量，不过传递时始终不变。
2)测量模型
轮速计的线速度测量模型为：
其中，v是线速度测量值，vt是线速度真实值，是轮速计尺度真实值，nv是速度测量噪声。
陀螺仪的角速度测量模型为：
其中，ω是角速度测量值，ωt是角速度真实值，是陀螺仪零偏真实值，nω是角速度测量噪声。
3)预积分真实值微分方程
根据刚体运动学模型以及零偏随机游走假设，可得到真实值的微分方程为：
4)名义值与误差值微分方程
根据测量模型与误差模型，可将真实值由名义值、误差值和测量值表示，并代入到真实值微分方程中，经过推导得到名义值与误差值的微分方程，推导过程如下表所示：
5)名义值递推方程
由于传感器观测是离散的，实际采用中值积分的形式进行递推实现，递推公式表示为：
Xr+1＝Fr(Xr，vr，vr+1，ωr，ωr+1)
其中，
关于陀螺仪零偏：
r时刻的微分由r时刻与r+1时刻的差分近似，推导如下：
其中，δτr为时间差，于是可得：
因此，前后两帧的陀螺仪零偏名义值相等。
关于旋转：
r时刻的角速度由r时刻与r+1时刻的测量值取平均近似，推导如下：
可得递推公式为：
从0递推到m即可得到残差表达式中的相对旋转名义值：γ＝γm。
关于平移：
r时刻的线速度由r时刻与r+1时刻的测量值取平均近似，推导如下：
可得：
从0递推到m即可得到残差表达式中的相对平移名义值：α＝αm。
6)误差值递推方程
误差值近似用线性关系进行传递，公式如下：
δXr+1＝δFrδXr+δGrNr
其中，
其中，Nr为传感器观测噪声，知/>分别是轮速计在r时刻和r+1时刻的线性度噪声，服从相同的高斯分布，/>和/>是陀螺仪在r时刻和r+1时刻的角速度噪声，服从相同的高斯分布，/>是陀螺仪在r时刻角速度随机游走噪声，服从高斯分布，由此可得误差值协方差传递形式为：
其中，Q为Nr对应的协方差，从0递推到m即可得到残差的协方差：∑vg＝(∑m)tl99。预积分约束对应的雅可比传递形式为：
Jpre＝δFm-1…δFr+1·δFr…δF0
Jvg中的和/>分别对应了该雅可比的相关矩阵块。
关于陀螺仪零偏：
r时刻微分由差分近似，推导如下：
递推公式为：
关于旋转：
r时刻旋转差分推导如下：
递推公式为：
关于平移：
r时刻平移差分推导如下：
将δθr+1递推公式代入可得：
最终得到递推公式为：
(4)车道线约束
不同帧如果感知到了同一条车道线，那么通过两帧之间的位姿转换后，车道线应该是重合的，这也就为目标函数提供了车道线。如图5所示，同一条车道线被连续3帧看到，比较简单的操作是通过相邻帧间的匹配来约束前后两帧的位姿，如图5中(a)所示，第1帧和第2帧之间以及第2帧和第3帧之间通过对应的车道线重合部分来约束。另外，我们注意到，不仅相邻帧之间可能看到相同的车道线，非相邻帧之间同样能够看到相同车道线的重合部分，因此，帧间匹配也会提供跨帧约束，如图5中(b)所示，这样能利用更多的车道线感知数据并提供更多的约束关系，能够有助于提高目标函数优化求解的精度。
需要说明地，感知车道线会提供定义在车身x-y平面下的三次线方程，三次曲线之间的距离不方便直接计算，另外匹配时只能考虑两帧车道线的重合部分，比较恰当的方式是将后一帧的车道线离散化，按照点线距离的方式计算与前一帧车道线重合部分的残差。如何计算一个点到一条三次曲线的距离，最合理的方式是计算垂直距离，但是这种方式比较复杂且计算量较大，比较方便的方式是在三次曲线上选取邻近的两个点来构建一条直线从而近似曲线的局部特征，而点到直线的距离计算则简单很多。可以理解地，虽然车道线是二维平面上的线方程，但可假设道路是平面，点线距离z轴分量可以约束垂直方向的飘移。因此，点线距离的残差维度为3，车道线约束对应的残差通过如下公式表示：
其中，点为后一帧(例如第k帧)离散点p转换到前一帧坐标系后的位置，其转换表达式为/>pa和pb为点在前一帧的两个邻近点。该残差将平面距离与垂直距离解耦，方便设置不同的权重系数。残差关于优化变量误差值(先后顺序为δph、δθh、δpk、δθk)的雅可比(维度为3×12)通过如下公式表示：
其中，Js2s为车道线约束对应的雅可比，Rh和Rk为姿态的旋转矩阵。
信息矩阵为对角阵，对角线元素对应三个方向的距离误差，通过如下公式表示：
其中，Ωs2s为车道线约束对应的信息矩阵，nx、ny、nz分别表示在东向、北向和天向的距离噪声。
(5)高精地图匹配约束
需要说明地，感知车道线数据和高精地图车道线之间的匹配可以为车辆定位提供约束并进一步提高定位精度，同时能够补偿高精地图的制作误差。高精地图匹配误差和车道线匹配一样基于点线距离，残差维度为3，高精地图匹配约束对应的残差通过如下公式表示：
其中，点为高精地图车道线上离散点p′转换到当前帧坐标系后的位置，转换表达式为/>p′a和p′b为离散点/>在感知车道线上的两个邻近点。可以看出，残差相关的优化变量误差值包括δpk、δθk和δbm，雅可比维度为3×9，通过如下公式表示：
其中，Js2m为高精地图匹配约束对应的雅可比，Rk为姿态的旋转矩阵。
信息矩阵通过如下公式表示：
其中，Ωs2m为高精地图匹配约束对应的信息矩阵，nx、ny、nz分别表示在东向、北向和天向的噪声。
(6)边缘化约束
本公开实施例中，滑动窗口的大小是固定的，也即滑动窗口中能够承载的传感器数据帧数是固定的，当滑动窗口移动时，也就会依次移除最老帧。当滑动窗口移除最老帧时，若直接丢弃最老帧会造成信息损失，本公开实施例中通过构建边缘化约束以保留被移除的帧的相关信息。边缘化的本质是由所有变量的联合概念求剩余变量的边缘概率。假设所有优化变量为x，被边缘化掉的变量为xm，剩余的变量为xr，并假设联合概率服从多维高斯分布，表示为其中μ为均值，∑为协方差，分别表示为：
优化时采用信息形式，即其中Ω为信息矩阵，满足Ω＝∑-1，ξ为信息向量，满足ξ＝∑-1μ，分别表示为：
根据均值与信息向量的关系可知：
第二行等式两边同时减去第一行左乘可得：
第二行等式关系为：
由此得到剩余变量服从分布其中：
这样通过线性代数的消元操作，得到了剩余变量的边缘高斯分布，其中Ω′称为Ωmm的舒尔补。实际算法中只需要挑出与被边缘化的变量相关的因子构建信息矩阵和信息向量并进行舒尔补操作，即可构建边缘化约束因子。边缘化约束的残差通过如下公式表示：
rm＝xr-ur
其中μr＝(Ω′)-1ξ′，残差的维度与xr维度相同，边缘化约束的雅可比为单位阵，边缘化约束的信息矩阵为Ω′。
本公开实施例中，因子图模型的目标函数表示为：
其中，x是优化变量，rk表示残差，T代表向量装置，Ωk是每个残差对应的信息矩阵，它是协方差矩阵∑k的逆，优化目标是可选地，上述目标函数的优化求解可以是采用非线性优化方法，进而以避免直接求复杂的Hessian矩阵，而是对每个残差采用一阶泰勒展开做近似，展开形式可表示为rk(x*+Δx)＝rk(x*)+JkΔx，其中Jk为残差对优化变量的一阶导，也就是雅可比矩阵。
本公开实施例中，每种传感器数据对目标函数的优化求解是通过因子节点的方式给出的，本公开中涉及的因子节点包括GNSS约束、轮速计速度约束、预积分约束、车道线约束、高精地图匹配约束和边缘化约束，各约束对应的残差、雅可比及信息矩阵已如上所述，进而基于每个约束对应的残差、雅可比和信息矩阵，也就能够对上述因子图模型的目标函数进行优化求解，以得到车辆的位姿。
可以理解地，本公开是通过获取车辆的N帧传感器数据，也即是融合了车辆N个时刻的GNSS、IMU、轮速计和感知车道线等4种传感器数据并利用高精地图进行匹配，对因子图模型的目标函数进行优化求解得到的车辆位姿。为更好地理解，以下对本公开的实现方式进行具体说明。
可选地，所述基于滑动窗口获取车辆的N帧传感器数据，包括：
基于滑动窗口获取车辆的N帧车道线数据，并确定每一帧车道线数据的时间戳；
获取与第二目标帧车道线数据的时间戳匹配的GNSS数据、轮速计数据和IMU数据，以得到所述第二目标帧的传感器数据；其中，所述第二目标帧为所述N帧中的任一帧。
具体地，电子设备在基于滑动窗口获取到车辆的N帧车道线数据后，获取每一帧中的车道线数据的时间戳，并以该时间戳为基准，获取与车道线数据的时间戳匹配的其他传感器数据，进而以得到一帧完整的传感器数据，每一帧传感器数据也就包括车道线数据、GNSS数据、轮速计数据和IMU数据。其中，每一帧中的GNSS数据、轮速计数据和IMU数据的时间戳与该帧中车道线数据的时间戳匹配，例如时间戳相同。
本公开实施例中，以每一帧传感器数据中的车道线数据的时间戳作为基准，来获取与其时间戳匹配的其他传感器数据，使得每一帧中各传感器数据在时间上是一致的，有助于提高车辆的定位精度。
可选地，对于GNSS数据和轮速计数据，可以是通过获取与车道线数据的时间戳相同的GNSS数据和轮速计数据来实现数据同步；对于IMU数据和轮速计数据，可以是通过插值的方式来实现数据同步，例如获取上一帧到当前帧之间的所有IMU数据和轮速计数据，假设需要同步的车道线数据的时间戳为时刻t，上一帧时刻为t0，下一帧时刻为t1，那么线性插值后的数据为xt＝(1-s)×xt0+s×xt1，其中s＝(t-t0)/(t1-t0)。
需要说明地，电子设备在获取到车辆的传感器数据后，可以是先将接收到的所有传感器数据进行缓存，缓存的数据可以是按照时间戳的先后顺序以队列的形式进行数据管理。滑动窗口可以是从缓存区依次获取N个传感器数据，例如先获取最早的一帧车道线数据，然后获取与该车道线数据时间戳匹配的其他传感器数据，作为一帧完整的传感器数据，依此获取N帧传感器数据，然后基于所述N帧传感器数据构建因子图模型的目标函数，以进行目标函数的优化求解来确定车辆的位姿。
可选地，所述基于所述N帧传感器数据构建因子图模型的目标函数，包括：
判断是否已经基于所述N帧传感器数据中的前N-1帧传感器数据完成初始化定位；
在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据的第一位姿，以所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数；
在未完成初始化定位的情况下，基于所述N帧传感器数据构建因子图模型的目标函数。
需要说明地，所述初始化定位可以是指所述N帧传感器数据中已经有部分传感器数据通过构建因子图模型的目标函数，来优化求解得到了车辆的位姿。本公开实施例中，在滑动窗口填满的情况下，例如滑动窗口能够承载的最大帧数为10帧，则当获取到10帧传感器数据，则基于这10帧传感器数据构建因子图模型；假设获取到的为第1帧到第10帧传感器数据，则这10帧传感器数据还未进行因子图模型目标函数的构建，当然也就不可能得到车辆的位姿，则这10帧传感器数据也就未完成初始化定位；滑动窗口是实时滑动的，也即滑动窗口中的传感器数据会发生变化，例如当获取到第2到第11帧传感器数据，则前面9帧传感器数据，也即第2帧到第10帧传感器数据已经进行过因子图模型目标函数的构建，并优化求解得到了车辆的初始位姿(也即第一位姿)，则这前9帧传感器数据已完成初始化定位，对这第2到第11帧传感器数据构建因子图模型的目标函数，并以第一位姿作为参考对目标函数的优化求解提供优化方向，以更快地得到车辆的位姿，基于第一位姿作为参考的方式，也能够保障后续车辆定位的准确性。
可选地，所述判断是否已经基于所述N帧传感器数据中的前N-1帧传感器数据完成初始化定位之前，所述方法还包括：
基于所述N帧传感器数据获取GNSS约束、轮速计速度约束和预积分约束；
所述在未完成初始化定位的情况下，基于所述N帧传感器数据构建因子图模型的目标函数，包括：
将所述GNSS约束、所述轮速计速度约束和所述预积分约束基于因子图模型构建初始目标函数，获取所述初始目标函数优化求解后得到的初始位姿；
对所述N帧传感器数据的车道线数据进行车道线跟踪，获取车道线约束；
以所述初始位姿作为参考，基于所述GNSS约束、所述轮速计速度约束、所述预积分约束和所述车道线约束构建因子图模型的目标函数。
本公开实施例中，在未基于前N-1帧传感器数据完成初始化定位的情况下，例如N为10，也即当获取到第1帧到第10帧传感器数据时，此场景下为第一次基于N帧传感器数据来构建因子图模型的目标函数，也还未获得车辆的定位；该场景下，先获取这10帧传感器数据中的GNSS数据、轮速计数据和IMU数据，并分别获取相应的GNSS约束、轮速计速度约束和预积分约束；而后将所述GNSS约束、轮速计速度约束和预积分约束基于因子图模型构建初始目标函数，并优化求解所述初始目标函数以得到车辆的初始位姿。
需要说明地，初始目标函数相对于目标函数仅是缺少了车道线约束、高精地图匹配约束和边缘化约束，所述初始目标函数同样基于因子图模型构建得到，具有与目标函数相同的数学原理，因此初始目标函数的构建及优化求解参照上述目标函数的构建及优化求解方式，此处不再赘述。
可选地，在基于N帧传感器数据构建因子图的初始目标函数，并优化求解所述初始目标函数以得到车辆的初始位姿后，可以是基于所述N帧传感器数据构建边缘化约束并保存(边缘化约束的构建具体参照前述描述)，进而在滑动窗口移动后，边缘化约束能够应用在基于后续传感器数据构建的目标函数中，有助于更快地确定车辆位姿。
进一步地，在基于初始目标函数得到车辆的初始位姿后，对第1帧到第10帧的车道线数据进行车道线跟踪，获取这10帧的车道线约束，然后基于这10帧的GNSS约束、轮速计速度约束、预积分约束和车道线约束构建因子图模型的目标函数，进而以优化求解所述目标函数来获得车辆的位姿。
需要说明地，车道线跟踪的主要任务是跟踪同一条车道线在不同帧的车道线数据中的对应关系，实现方法是对相邻两帧之间的车道线数据做关联，并记录车道线的车道号，每条车道线有唯一的车道号。以图6为例，第一帧车道线数据包括4条车道线(实线)，车道号分别为1到4，第二帧车道线数据包括5条车道线(虚线)，通过数据关联可跟踪到右边4条车道线，车道号分别对应了1到4，但最左边的车道线未成功关联，因此将其作为新的车道线进行添加，车道号为5。每个车道线保存的信息包括其在哪些帧被观测到以及各帧中对应的车道号，比如车道线3同时被第一帧和第二帧观测到，第一帧中属于第3车道(1，3)，第二帧中属于第4车道(2，4)，依此可以得到每条车道线在哪些帧被观测到以及在各帧中对应的车道号。
数据关联问题可以抽象为带权二部图的最大匹配集求解问题，从影响定位精度的车道线数据特征进行权重换算，从整体进行指派求解，保证在数据关联阶段直接获得最优匹配集合。请参照图6所示的车道线关联的示意图，针对通过筛选后的车道线数据，本公开从两个方面对其进行权重评估：后一帧起点处横向截距误差δd和航向角误差δθ，如图6所示。若超过一定阈值则直接拒绝，例如，阈值分别为0.5米和2.0度。需要注意的是，在车道线数据关联前需要根据预测位姿将后一帧的车道线转换到前一帧。匹配权重计算公式为：
其中，δd为截距误差，δθ为航向角误差，和/>分别为截距阈值和航向角阈值，cd和cθ分别为截距和航向角对应的权重，满足cd+cθ＝1。可选地，在完成权重矩阵的构造后，可以通过Kuhn-Munkres算法求解获得最大匹配集。这样，通过车道线跟踪，也就能够同一条车道线在不同帧车道线数据中的对应关系，进而以获取N帧传感器数据对应的车道线约束。
本公开实施例中，在基于车道线跟踪获取到车道线约束后，以所述初始位姿作为参考，基于N帧传感器数据对应的GNSS约束、轮速计速度约束、预积分约束和车道线约束构建因子图模型的目标函数，进而以优化求解所述目标函数来获得车辆的位姿。其中，初始位姿能够对构建的目标函数的优化求解提供优化方向，以更快、更准确地得到车辆的位姿，
可选地，所述在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据的第一位姿，以所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数，包括：
在已完成初始化定位的情况下，判断第N帧传感器数据是否为关键帧数据；
在所述第N帧传感器数据为关键帧数据的情况下，基于预积分获取所述第N帧传感器数据的第一位姿；
将所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数。
本公开实施例中，在获取到N帧传感器数据后，若前N-1帧传感器数据已完成初始化定位，则判断第N帧传感器数据是否为关键帧数据。其中，所述关键帧数据是指该帧的传感器数据与其相邻预设帧传感器数据的差异在预设范围内，例如某一帧的传感器数据与其前一帧的传感器数据相同，则可以认为该帧传感器数据为关键帧数据；或者，某一帧的GNSS数据与其前5帧的GNSS数据的差异都小于预设阈值，则可以将该帧确定为关键帧，该帧的传感器数据为关键帧数据。
可选地，若所述第N帧传感器数据为关键帧数据，说明该帧与其前几周帧的传感器数据差异较小，则基于预积分获取第N帧传感器数据的第一位姿。可以理解地，该场景下，前N-1帧传感器数据已完成初始化定位，也即已经基于前N-1帧传感器数据计算得到了车辆的位姿，则可以是基于已计算得到的车辆位姿来获取第N帧传感器数据的第一位姿，例如基于上述预积分约束中的计算方式，通过第N帧传感器数据与已计算的车辆位姿获取第一位姿，并将第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数，以对所述目标函数的优化求解提供优化方向，以更快地计算得到车辆位姿。
可选地，所述在已完成初始化定位的情况下，判断第N帧传感器数据是否为关键帧数据，包括：
在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据相对于第N-1帧传感器数据的相对位姿；
根据所述相对位姿，将所述第N帧的车道线数据与所述第N-1帧的车道线数据关联，并移除所述第N-1帧的车道线数据；
合并所述第N帧传感器数据和所述第N-1帧传感器数据；
判断合并了所述第N-1帧传感器数据后的第N帧传感器数据是否为关键帧数据。
本公开实施例中，在已经基于前N-1帧传感器数据完成车辆的初始化定位的情况下，基于前述预积分预设中的计算方式获取第N帧传感器数据相对于第N-1帧传感器数据的相对位姿，基于该相对位姿，将第N帧的车道线数据与第N-1帧的车道线数据关联，并移除所述第N-1帧的车道线数据，相邻帧车道线数据的合并请参照图7。其中，车道线数据关联的实现方式可以是参照上述车道线跟踪的具体描述。在移除了第N-1帧的车道线数据后，将第N帧传感器数据和第N-1帧传感器数据合并，也即这两帧的GNSS数据、轮速计数据和IMU数据的合并，所述合并可以是两帧数据求平均。进一步地，判断合并了第N-1帧传感器数据后的第N帧传感器数据是否为关键帧数据。
这样，通过将第N帧的车道线数据与第N-1帧的车道线数据关联，能够利用已经完成了初始化定位的历史帧车道线数据来对最新帧的车道线数据进行匹配，以降低最新帧车道线数据误差；再通过将第N帧传感器数据和第N-1帧传感器数据合并以后再判断是否为关键帧数据，也就能够有效利用已经完成了初始化定位的历史帧数据来对最新帧数据进行调整，以减小误差，提升定位精度。
可选地，所述判断第N帧传感器数据是否为关键帧数据之后，所述方法还包括：
在所述第N帧传感器数据不为关键帧数据的情况下，基于所述第N帧传感器数据中的轮速计数据和IMU数据进行航迹推算，以确定所述车辆的位姿。
本公开实施例中，若第N帧传感器数据不为关键帧数据，则直接基于第N帧传感器数据中的轮速计数据和IMU数据进行航迹推算来确定车辆的位姿，无需再进行目标函数构建，也就能够快速获得车辆位姿。可选地，航迹推算的具体方式可以是参照相关技术中的描述，本公开不做赘述。
可选地，所述将所述N帧传感器数据基于因子图模型构建目标函数之前，所述方法还包括：
基于第一目标帧对应的GNSS数据确定目标坐标系的坐标原点，基于所述坐标原点更新每一帧的GNSS数据，所述第一目标帧为所述N帧中的任一帧；
基于更新后的每一帧的GNSS数据获取GNSS约束；
其中，所述车辆的位姿基于所述目标坐标系进行表征。
本公开实施例中，在获取到车辆的N帧传感器数据后，可以是选取其中一帧对应的GNSS数据确定目标坐标系的坐标原点，并基于该坐标原点来更新其他帧的GNSS数据。例如，第一目标帧为所述N帧中的中间帧，假设N为10，则可以是选取第5帧作为第一目标帧，基于第5帧的GNSS数据来确定目标坐标系的坐标原点，可以是以第5帧的GNSS数据对应的经度和纬度作为坐标原点，将这10帧中的其他帧基于该坐标原点来更新对应的GNSS数据，例如基于其他帧的经纬度与坐标原点对应的经纬度的差来更新其他每一帧的GNSS数据。进一步地，基于更新后的每一帧的GNSS数据来获取GNSS约束。
本公开中，滑动窗口最大能承载N帧传感器数据，而滑动窗口是实时移动的，在每获取到N帧传感器数据后，可以是重新选取这N帧传感器数据中的中间帧的GNSS数据来重新确定目标坐标系的坐标原点，基于该坐标远离来更新这N帧的GNSS数据，以获取GNSS约束。这样，也就使得每N帧传感器数据都能够基于重新确定的坐标原点来获取GNSS约束，这样能够避免所有GNSS数据都以相同的坐标原点进行计算而导致的误差，进而能够提高GNSS约束的准确性，也就能够提高车辆位姿定位的准确性。
可选地，所述目标坐标系可以是局部东北天坐标系，或者也可以是其他坐标系；本公开实施例中基于目标函数优化求解得到的目标值是基于目标坐标系来表征的，例如最终计算得到的车辆位姿是目标坐标系中的坐标值。
可选地，所述基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿之后，所述方法还包括：
根据所述第一目标帧、所述N帧传感器数据中每一帧的车道线数据、每一帧的GNSS数据与所述第一目标帧的GNSS数据之间的相对位置，确定目标地图并保存。
本公开实施例中，N帧传感器数据是基于其中第一目标帧的GNSS数据确定的坐标原点，其他帧的GNSS数据也是基于该坐标原点进行了相应的更新，也即这N帧GNSS数据是建立在目标坐标系的基础上的。在通过对N帧传感器数据构建的因子图模型的目标函数优化求解，确定车辆的位姿后，根据第一目标帧、所述N帧传感器数据中每一帧的车道线数据、每一帧的GNSS数据与所述第一目标帧的GNSS数据之间的相对位置，构建这N帧传感器数据对应的目标地图并保存。
可以理解地，针对每N帧传感器数据就能够构建得到一份目标地图，而滑动窗口是实时移动的，进而也就能够构建得到多份目标地图，多份目标地图进行拼接也就能够构建得到更大范围的地图。这样，在基于车辆传感器数据对车辆位姿进行定位的过程中，还能够同时构建车辆行驶区域对应的地图，也就能够基于车辆传感器数据来实现地图的构建和更新，例如车道线数据的更新，对于自动驾驶车辆，能够更有助于车辆行驶路径的规划。
请参照图8，图8是本申请实施例提供的一种车辆定位方法的流程图之二。如图8所示，电子设备在获取到车辆的传感器数据后，对传感器数据构建传感器约束，所述传感器约束包括GNSS约束、轮速计速度约束、预积分约束和车道线约束，传感器约束的构建具体可参照前述实施例中的具体描述，本实施例不再赘述；进一步地，判断获取到的传感器数据是否完成初始化定位，若否，则将最新帧传感器数据压入滑动窗口，判断滑动窗口是否已满，直至滑动窗口填满，基于滑动窗口中的传感器数据构因子图模型的建初始目标函数并优化求救，初始目标函数基于滑动窗口中的传感器数据对应的GNSS约束、轮速计速度约束和预积分约束构建得到，进一步判断初始化定位是否成功，也即是否能够优化求解得到车辆的初始位姿，直至初始定位成功，将滑动窗口内各帧车道线数据关联，并获取车道线约束；进一步地，将滑动窗口内的传感器数据基于因子图模型构建目标函数，并优化求解目标函数以确定车辆位姿。
可选地，上述判断获取到的传感器数据是否完成初始化定位的步骤，若已经完成初始化定位，预积分获取最新帧与次新帧的相对位姿，而后将最新帧的车道线数据与次新帧的车道线数据关联，并移除次新帧的车道线数据，然后合并最新帧的传感器数据与次新帧的传感器数据，进而以合并最新帧与次新帧的传感器约束；进一步判断合并后的最新帧是否为关键帧，若不为关键帧，则基于预积分得到的相位位姿进行航迹推算，以确定车辆位姿；若为关键帧，基于预积分获取最新帧的位姿，然后以最新帧的位姿作为参考，基于滑动窗口内的所有传感器数据构建因子图模型的目标函数，优化求解目标函数以确定车辆位姿。
可选地，在基于对目标函数优化求解得到车辆位姿后，可以是基于滑动窗口中的传感器数据构建边缘化约束并保存；进一步地，还可以基于滑动窗口中的传感器数据构建对应的局部地图并保存。
可选地，在基于当前滑动窗口内的传感器数据确定车辆位姿后，滑动窗口移动，则移除滑动窗口内的最老帧，以获取新的传感器数据，并基于新增加的传感器数据来构建因子图模型的目标函数，以确定车辆位姿。这样，也就能够基于滑动窗口的移动，不断地计算和更新车辆的位姿。
本公开实施例还提供了一种定位装置。请参照图9，图9是本公开实施例提供的一种车辆定位装置的结构图。如图9所示，所述车辆定位装置900包括：
获取模块901，用于基于滑动窗口获取车辆的N帧传感器数据，所述N为正整数；
优化模块902，用于基于所述N帧传感器数据构建因子图模型的目标函数，并对所述目标函数进行优化求解；
确定模块903，用于基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿。
可选地，每一帧传感器数据包括全球导航卫星系统GNSS数据、车道线数据、轮速计数据和惯性测量单元IMU数据；
所述因子图模型的目标函数与GNSS约束、车道线约束、轮速计速度约束、预积分约束和边缘化约束相关；
所述GNSS约束基于所述GNSS数据获取，所述车道线约束基于所述车道线数据获取，所述轮速计速度约束基于所述轮速计数据获取，所述预积分约束基于所述轮速计数据和所述IMU数据获取，所述边缘化约束基于所有帧的目标传感器数据获取，所述目标传感器数据为所述GNSS数据、车道线数据、轮速计数据和IMU数据中的至少一项。
可选地，所述优化模块902包括：
判断单元，用于判断是否已经基于所述N帧传感器数据中的前N-1帧传感器数据完成初始化定位；
构建单元，用于在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据的第一位姿，以所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数；
所述构建单元，还用于在未完成初始化定位的情况下，基于所述N帧传感器数据构建因子图模型的目标函数。
可选地，所述优化模块902还包括：
获取单元，用于基于所述N帧传感器数据获取GNSS约束、轮速计速度约束和预积分约束；
所述构建单元用于：
将所述GNSS约束、所述轮速计速度约束和所述预积分约束基于因子图模型构建初始目标函数，获取所述初始目标函数优化求解后得到的初始位姿；
对所述N帧传感器数据的车道线数据进行车道线跟踪，获取车道线约束；
以所述初始位姿作为参考，基于所述GNSS约束、所述轮速计速度约束、所述预积分约束和所述车道线约束构建因子图模型的目标函数。
可选地，所述构建单元还用于：
在已完成初始化定位的情况下，判断第N帧传感器数据是否为关键帧数据；
在所述第N帧传感器数据为关键帧数据的情况下，基于预积分获取所述第N帧传感器数据的第一位姿；
将所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数。
可选地，所述构建单元还用于：
在所述第N帧传感器数据不为关键帧数据的情况下，基于所述第N帧传感器数据中的轮速计数据和IMU数据进行航迹推算，以确定所述车辆的位姿。
可选地，所述判断单元还用于：
在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据相对于第N-1帧传感器数据的相对位姿；
根据所述相对位姿，将所述第N帧的车道线数据与所述第N-1帧的车道线数据关联，并移除所述第N-1帧的车道线数据；
合并所述第N帧传感器数据和所述第N-1帧传感器数据；
判断合并了所述第N-1帧传感器数据后的第N帧传感器数据是否为关键帧数据。
可选地，所述车辆定位装置900还包括：
更新模块，用于基于第一目标帧对应的GNSS数据确定目标坐标系的坐标原点，基于所述坐标原点更新每一帧的GNSS数据，所述第一目标帧为所述N帧中的任一帧；
所述获取模块901还用于：基于更新后的每一帧的GNSS数据获取GNSS约束；
其中，所述车辆的位姿基于所述目标坐标系进行表征。
可选地，所述车辆定位装置900还包括：
保存模块，用于根据所述第一目标帧、所述N帧传感器数据中每一帧的车道线数据、每一帧的GNSS数据与所述第一目标帧的GNSS数据之间的相对位置，确定目标地图并保存。
可选地，每一帧传感器数据还包括高精地图数据；所述因子图模型的目标函数还与高精地图匹配约束相关，所述高精地图匹配约束基于所述高精地图数据和所述车道线数据获取。
可选地，所述获取模块901还用于：
基于滑动窗口获取车辆的N帧车道线数据，并确定每一帧车道线数据的时间戳；
获取与第二目标帧车道线数据的时间戳匹配的GNSS数据、轮速计数据和IMU数据，以得到所述第二目标帧的传感器数据；
其中，所述第二目标帧为所述N帧中的任一帧。
需要说明地，本公开实施例提供的车辆定位装置900能够实现上述车辆定位方法实施例中的全部技术方案，因此至少能够实现上述车辆定位方法实施例的全部技术效果，此处不再赘述。
本公开的技术方案中，所涉及的用户个人信息的获取，存储和应用等，均符合相关法律法规的规定，且不违背公序良俗。
根据本公开的实施例，本公开还提供了一种电子设备、一种可读存储介质和一种计算机程序产品。
图10示出了可以用来实施本公开的实施例的示例电子设备1000的示意性框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本公开的实现。
如图10所示，设备1000包括计算单元1001，其可以根据存储在只读存储器(ROM)1002中的计算机程序或者从存储单元1008加载到随机访问存储器(RAM)1003中的计算机程序，来执行各种适当的动作和处理。在RAM 1003中，还可存储设备1000操作所需的各种程序和数据。计算单元1001、ROM 1002以及RAM 1003通过总线1004彼此相连。输入/输出(I/O)接口1005也连接至总线1004。
设备1000中的多个部件连接至I/O接口1005，包括：输入单元1006，例如键盘、鼠标等；输出单元1007，例如各种类型的显示器、扬声器等；存储单元1008，例如磁盘、光盘等；以及通信单元1009，例如网卡、调制解调器、无线通信收发机等。通信单元1009允许设备1000通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元1001可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元1001的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。计算单元1001执行上文所描述的各个方法和处理，例如车辆定位方法。例如，在一些实施例中，车辆定位方法可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元1008。在一些实施例中，计算机程序的部分或者全部可以经由ROM 1002和/或通信单元1009而被载入和/或安装到设备1000上。当计算机程序加载到RAM 1003并由计算单元1001执行时，可以执行上文描述车辆定位方法的一个或多个步骤。备选地，在其他实施例中，计算单元1001可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行上述车辆定位方法。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以为分布式系统的服务器，或者是结合了区块链的服务器。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本公开公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本公开保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和改进等，均应包含在本公开保护范围之内。

Claims (13)

1.一种车辆定位方法，包括：
基于滑动窗口获取车辆的N帧传感器数据，所述N为正整数；
基于所述N帧传感器数据构建因子图模型的目标函数，并对所述目标函数进行优化求解；
基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿；
所述基于所述N帧传感器数据构建因子图模型的目标函数，包括：
判断是否已经基于所述N帧传感器数据中的前N-1帧传感器数据完成初始化定位；
在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据的第一位姿，以所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数；
在未完成初始化定位的情况下，基于所述N帧传感器数据构建因子图模型的目标函数；
其中，所述因子图模型的目标函数与GNSS约束、车道线约束、轮速计速度约束、预积分约束和边缘化约束相关，每一帧传感器数据包括全球导航卫星系统GNSS数据、车道线数据、轮速计数据和惯性测量单元IMU数据；
所述判断是否已经基于所述N帧传感器数据中的前N-1帧传感器数据完成初始化定位之前，所述方法还包括：
基于所述N帧传感器数据获取GNSS约束、轮速计速度约束和预积分约束；
所述在未完成初始化定位的情况下，基于所述N帧传感器数据构建因子图模型的目标函数，包括：
将所述GNSS约束、所述轮速计速度约束和所述预积分约束基于因子图模型构建初始目标函数，获取所述初始目标函数优化求解后得到的初始位姿；
对所述N帧传感器数据的车道线数据进行车道线跟踪，获取车道线约束；
以所述初始位姿作为参考，基于所述GNSS约束、所述轮速计速度约束、所述预积分约束和所述车道线约束构建因子图模型的目标函数。
2.根据权利要求1所述的方法，其中，
所述GNSS约束基于所述GNSS数据获取，所述车道线约束基于所述车道线数据获取，所述轮速计速度约束基于所述轮速计数据获取，所述预积分约束基于所述轮速计数据和所述IMU数据获取，所述边缘化约束基于所有帧的目标传感器数据获取，所述目标传感器数据为所述GNSS数据、车道线数据、轮速计数据和IMU数据中的至少一项。
3.根据权利要求2所述的方法，其中，所述在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据的第一位姿，以所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数，包括：
在已完成初始化定位的情况下，判断第N帧传感器数据是否为关键帧数据；
在所述第N帧传感器数据为关键帧数据的情况下，基于预积分获取所述第N帧传感器数据的第一位姿；
将所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数。
4.根据权利要求3所述的方法，其中，所述判断第N帧传感器数据是否为关键帧数据之后，所述方法还包括：
在所述第N帧传感器数据不为关键帧数据的情况下，基于所述第N帧传感器数据中的轮速计数据和IMU数据进行航迹推算，以确定所述车辆的位姿。
5.根据权利要求3所述的方法，其中，所述在已完成初始化定位的情况下，判断第N帧传感器数据是否为关键帧数据，包括：
在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据相对于第N-1帧传感器数据的相对位姿；
根据所述相对位姿，将所述第N帧的车道线数据与所述第N-1帧的车道线数据关联，并移除所述第N-1帧的车道线数据；
合并所述第N帧传感器数据和所述第N-1帧传感器数据；
判断合并了所述第N-1帧传感器数据后的第N帧传感器数据是否为关键帧数据。
6.根据权利要求1-4中任一项所述的方法，其中，所述将所述N帧传感器数据基于因子图模型构建目标函数之前，所述方法还包括：
基于第一目标帧对应的GNSS数据确定目标坐标系的坐标原点，基于所述坐标原点更新每一帧的GNSS数据，所述第一目标帧为所述N帧中的任一帧；
基于更新后的每一帧的GNSS数据获取GNSS约束；
其中，所述车辆的位姿基于所述目标坐标系进行表征。
7.根据权利要求6所述的方法，其中，所述基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿之后，所述方法还包括：
根据所述第一目标帧、所述N帧传感器数据中每一帧的车道线数据、每一帧的GNSS数据与所述第一目标帧的GNSS数据之间的相对位置，确定目标地图并保存。
8.根据权利要求1-4中任一项所述的方法，其中，每一帧传感器数据还包括高精地图数据；所述因子图模型的目标函数还与高精地图匹配约束相关，所述高精地图匹配约束基于所述高精地图数据和所述车道线数据获取。
9.根据权利要求1-4中任一项所述的方法，其中，所述基于滑动窗口获取车辆的N帧传感器数据，包括：
基于滑动窗口获取车辆的N帧车道线数据，并确定每一帧车道线数据的时间戳；
获取与第二目标帧车道线数据的时间戳匹配的GNSS数据、轮速计数据和IMU数据，以得到所述第二目标帧的传感器数据；
其中，所述第二目标帧为所述N帧中的任一帧。
10.一种车辆定位装置，包括：
获取模块，用于基于滑动窗口获取车辆的N帧传感器数据，所述N为正整数；
优化模块，用于基于所述N帧传感器数据构建因子图模型的目标函数，并对所述目标函数进行优化求解；
确定模块，用于基于所述目标函数优化求解后得到的目标值确定所述车辆的位姿；
所述优化模块包括：
判断单元，用于判断是否已经基于所述N帧传感器数据中的前N-1帧传感器数据完成初始化定位；
构建单元，用于在已完成初始化定位的情况下，基于预积分获取第N帧传感器数据的第一位姿，以所述第一位姿作为参考，基于所述N帧传感器数据构建因子图模型的目标函数；
所述构建单元，还用于在未完成初始化定位的情况下，基于所述N帧传感器数据构建因子图模型的目标函数；
其中，所述因子图模型的目标函数与GNSS约束、车道线约束、轮速计速度约束、预积分约束和边缘化约束相关，每一帧传感器数据包括全球导航卫星系统GNSS数据、车道线数据、轮速计数据和惯性测量单元IMU数据；
在基于所述判断单元判断是否已经基于所述N帧传感器数据中的前N-1帧传感器数据完成初始化定位之前，所述优化模块还包括获取单元，用于基于所述N帧传感器数据获取GNSS约束、轮速计速度约束和预积分约束；
所述构建单元还用于在未完成初始化定位的情况下，基于所述N帧传感器数据构建因子图模型的目标函数，包括：
将所述GNSS约束、所述轮速计速度约束和所述预积分约束基于因子图模型构建初始目标函数，获取所述初始目标函数优化求解后得到的初始位姿；
对所述N帧传感器数据的车道线数据进行车道线跟踪，获取车道线约束；
以所述初始位姿作为参考，基于所述GNSS约束、所述轮速计速度约束、所述预积分约束和所述车道线约束构建因子图模型的目标函数。
11.一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-9中任一项所述的方法。
12.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据权利要求1-9中任一项所述的方法。
13.一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现根据权利要求1-9中任一项所述的方法。
CN202111367474.6A 2021-11-18 2021-11-18 车辆定位方法、装置及电子设备 Active CN114018274B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202111367474.6A CN114018274B (zh) 2021-11-18 2021-11-18 车辆定位方法、装置及电子设备
US17/931,777 US11733398B2 (en) 2021-11-18 2022-09-13 Vehicle positioning method for determining position of vehicle through creating target function for factor graph model

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111367474.6A CN114018274B (zh) 2021-11-18 2021-11-18 车辆定位方法、装置及电子设备

Publications (2)

Publication Number Publication Date
CN114018274A CN114018274A (zh) 2022-02-08
CN114018274B true CN114018274B (zh) 2024-03-26

Family

ID=80065055

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111367474.6A Active CN114018274B (zh) 2021-11-18 2021-11-18 车辆定位方法、装置及电子设备

Country Status (2)

Country Link
US (1) US11733398B2 (zh)
CN (1) CN114018274B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114639079B (zh) * 2020-12-15 2023-06-30 北京百度网讯科技有限公司 匹配车道线数据的方法、装置、设备和存储介质
CN112560684B (zh) * 2020-12-16 2023-10-24 阿波罗智联(北京)科技有限公司 车道线检测方法、装置、电子设备、存储介质以及车辆
CN114252082B (zh) * 2022-03-01 2022-05-17 苏州挚途科技有限公司 车辆定位方法、装置和电子设备
WO2023198090A1 (en) * 2022-04-14 2023-10-19 The Hong Kong Polytechnic University 3d vision aided gnss real-time kinematic positioning for autonomous systems in urban canyons
CN115116019B (zh) * 2022-07-13 2023-08-01 阿波罗智能技术(北京)有限公司 车道线处理方法、装置、设备以及存储介质
CN116503482B (zh) * 2023-06-26 2023-10-20 小米汽车科技有限公司 车辆位置的获取方法、装置及电子设备
CN116817928B (zh) * 2023-08-28 2023-12-01 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN117076824B (zh) * 2023-09-26 2024-01-12 腾讯科技（深圳）有限公司 移动终端的状态预测方法、装置、电子设备和存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108829996A (zh) * 2018-06-25 2018-11-16 禾多科技（北京）有限公司 获得车辆定位信息的方法及装置
CN111220154A (zh) * 2020-01-22 2020-06-02 北京百度网讯科技有限公司 车辆定位方法、装置、设备和介质
CN111652179A (zh) * 2020-06-15 2020-09-11 东风汽车股份有限公司 基于点线特征融合激光的语义高精地图构建与定位方法
CN111795686A (zh) * 2020-06-08 2020-10-20 南京大学 一种移动机器人定位与建图的方法
CN112212852A (zh) * 2019-07-12 2021-01-12 阿里巴巴集团控股有限公司 定位方法、移动设备及存储介质
CN113654555A (zh) * 2021-09-14 2021-11-16 上海智驾汽车科技有限公司 一种基于多传感器数据融合的自动驾驶车辆高精定位方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102015214338A1 (de) * 2015-07-29 2017-02-02 Volkswagen Aktiengesellschaft Bestimmung einer Anordnungsinformation für ein Fahrzeug
US10989542B2 (en) * 2016-03-11 2021-04-27 Kaarta, Inc. Aligning measured signal data with slam localization data and uses thereof
CN107310550B (zh) 2016-04-27 2019-09-17 腾讯科技（深圳）有限公司 道路交通工具行驶控制方法和装置
CN113945206B (zh) * 2020-07-16 2024-04-19 北京图森未来科技有限公司 一种基于多传感器融合的定位方法及装置
CN112595330B (zh) 2020-11-13 2021-10-15 禾多科技（北京）有限公司 车辆定位方法、装置、电子设备和计算机可读介质
CN115082549A (zh) * 2021-03-10 2022-09-20 北京图森智途科技有限公司 位姿估计方法及装置、以及相关设备和存储介质
CN113175938B (zh) 2021-03-16 2024-05-17 惠州市德赛西威智能交通技术研究院有限公司 一种基于高精度地图的车辆定位增强系统及方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108829996A (zh) * 2018-06-25 2018-11-16 禾多科技（北京）有限公司 获得车辆定位信息的方法及装置
CN112212852A (zh) * 2019-07-12 2021-01-12 阿里巴巴集团控股有限公司 定位方法、移动设备及存储介质
CN111220154A (zh) * 2020-01-22 2020-06-02 北京百度网讯科技有限公司 车辆定位方法、装置、设备和介质
CN111795686A (zh) * 2020-06-08 2020-10-20 南京大学 一种移动机器人定位与建图的方法
CN111652179A (zh) * 2020-06-15 2020-09-11 东风汽车股份有限公司 基于点线特征融合激光的语义高精地图构建与定位方法
CN113654555A (zh) * 2021-09-14 2021-11-16 上海智驾汽车科技有限公司 一种基于多传感器数据融合的自动驾驶车辆高精定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Dense visual-inertial odometry for tracking of aggressive motions;Ling Yonggen等;2015 IEEE International Conference on Robotics and Biomimetics;全文 *
LIO-SAM:tightly-coupled lidar inertial odometry via smoothing and mapping;Shan Tixiao等;2020 IEEE International Conference on Intelligent Robots and Systems;全文 *
Localization with sliding window factor graphs on third-party map for automated driving;Daniel Willbers等;2019 International Conference on Robotics and Automation;全文 *

Also Published As

Publication number Publication date
CN114018274A (zh) 2022-02-08
US11733398B2 (en) 2023-08-22
US20230043236A1 (en) 2023-02-09

Similar Documents

Publication Publication Date Title
CN114018274B (zh) 车辆定位方法、装置及电子设备
CN111721289B (zh) 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
US20230194265A1 (en) Square-Root Multi-State Constraint Kalman Filter for Vision-Aided Inertial Navigation System
JP7299261B2 (ja) 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム
CN107748569B (zh) 用于无人机的运动控制方法、装置及无人机系统
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN113048980B (zh) 位姿优化方法、装置、电子设备及存储介质
CN106767767A (zh) 一种微纳多模星敏感器系统及其数据融合方法
WO2020135183A1 (zh) 点云地图的构建方法、装置、计算机设备和存储介质
CN114111776A (zh) 定位方法及相关装置
CN114440877B (zh) 一种异步多相机视觉惯性里程计定位方法
CN117232499A (zh) 多传感器融合的点云地图构建方法、装置、设备及介质
CN114323007A (zh) 一种载体运动状态估计方法及装置
Taghizadeh et al. A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN115900697A (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN115984463A (zh) 一种适用于狭窄巷道的三维重建方法和系统
CN113946151A (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN114111769A (zh) 一种视觉惯性定位方法、装置及自动驾驶装置
CN111811500A (zh) 目标对象的位姿估计方法、装置、存储介质和电子设备
CN116817928B (zh) 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN117058430B (zh) 用于视场匹配的方法、装置、电子设备和存储介质
US20150092985A1 (en) Updating filter parameters of a system

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