CN113776543B - 一种车辆融合定位方法、系统、电子设备及存储介质 - Google Patents

一种车辆融合定位方法、系统、电子设备及存储介质 Download PDF

Info

Publication number
CN113776543B
CN113776543B CN202110855287.6A CN202110855287A CN113776543B CN 113776543 B CN113776543 B CN 113776543B CN 202110855287 A CN202110855287 A CN 202110855287A CN 113776543 B CN113776543 B CN 113776543B
Authority
CN
China
Prior art keywords
pose
data
vehicle
optimization
representing
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
CN202110855287.6A
Other languages
English (en)
Other versions
CN113776543A (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.)
Heading Data Intelligence Co Ltd
Original Assignee
Heading Data Intelligence 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 Heading Data Intelligence Co Ltd filed Critical Heading Data Intelligence Co Ltd
Priority to CN202110855287.6A priority Critical patent/CN113776543B/zh
Publication of CN113776543A publication Critical patent/CN113776543A/zh
Application granted granted Critical
Publication of CN113776543B publication Critical patent/CN113776543B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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

Abstract

本发明提供一种车辆融合定位方法、系统、电子设备及存储介质，该方法包括：设置车辆位姿匹配数据对应的滑动窗口大小，设置车辆初始局部位姿到全局位姿的变换矩阵，并定义初始优化固定状态；每接收到一帧VO/VIO数据，则通过所述变换矩阵将车辆位姿转换至全局位姿，并将接收到的VO/VIO数据作为最新帧放入滑动窗口中；对滑动窗口内数据进行位姿图优化，得到节点车辆的绝对坐标；根据优化结果和原始VO/VIO数据更新所述变换矩阵，并基于优化前后的位置误差修改优化固定状态。从而实现基于GNSS和VO/VIO数据的融合定位，有效提升了非线性情况下融合定位精度，并解决了GPS长时间、大尺度漂移的问题。

Description

一种车辆融合定位方法、系统、电子设备及存储介质
技术领域
本发明属于车辆定位领域，尤其涉及一种车辆融合定位方法、系统、电子设备及存储介质。
背景技术
自动驾驶车辆一般配备有多种传感器，基于其中的GPS、IMU|相机等传感器都可以实现定位。相机可以应用视觉SLAM/VO技术进行相对定位和建图，相机和IMU组合可以应用VINS/VIO技术进行相对定位和建图，GPS可以应用GNSS技术对车体进行绝对位置定位。
由于VO/VIO是相对定位，其定位结果是相对于起始帧，而车辆的起始帧每次都不一样，无法解算出UTM坐标下的全局定位结果，同时，相机和IMU都属于内感受型传感器，随着时间的积累，VO/VIO的定位误差必然会越来越大。
为解决的相机和IMU定位的误差，常需要结合GPS来弥补。然而大多数结合GPS和VO/VIO的方案是利用EFK(扩展卡尔曼滤波)进行融合，EKF需要首先对系统模型进行线性化，因此对于非线性程度较高的系统通常结果不准确，且无法应对GPS长时间、大尺度漂移的情况。
发明内容
有鉴于此，本发明实施例提供了一种车辆融合定位方法、系统、电子设备及存储介质，用于解决现有融合定位方法在非线性情况下定位准确度低，无法应对GPS长时间、大尺度漂移的问题。
在本发明实施例的第一方面，提供了一种车辆融合定位方法，包括：
设置车辆位姿匹配数据对应的滑动窗口大小，设置车辆初始局部位姿到全局位姿的变换矩阵，并定义初始优化固定状态；
每接收到一帧VO/VIO数据，则通过所述变换矩阵将车辆位姿转换至全局位姿，并将接收到的VO/VIO数据和GPS数据作为最新帧放入滑动窗口中；
对滑动窗口内数据进行位姿图优化，得到节点车辆的绝对坐标；
根据优化结果和原始VO/VIO数据更新所述变换矩阵，并基于优化前后的位置误差修改优化固定状态。
在本发明实施例的第二方面，提供了一种车辆实时融合定位系统，包括：
窗口设置模块，用于设置车辆位姿匹配数据对应的滑动窗口大小，设置车辆初始局部位姿到全局位姿的变换矩阵，并定义初始优化固定状态；
位姿变换模块，用于每接收到一帧VO/VIO数据，则通过所述变换矩阵将车辆位姿转换至全局位姿，并将接收到的VO/VIO数据和GPS数据作为最新帧放入滑动窗口中；
位姿图优化模块，用于对滑动窗口内数据进行位姿图优化，得到节点车辆的绝对坐标；
更新模块，用于根据优化结果和原始VO/VIO数据更新所述变换矩阵，并基于优化前后的位置误差修改优化固定状态。
在本发明实施例的第三方面，提供了一种装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器运行的计算机程序，所述处理器执行所述计算机程序时实现如本发明实施例第一方面所述方法的步骤。
在本发明实施例的第四方面，提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现本发明实施例第一方面提供的所述方法的步骤。
本发明实施例中，采用位姿图优化融合GPS数据和VO/VIO，得到车辆的绝对位置，可以提高车辆定位精度，并能解决非线性模型中GPS长时间、大尺度漂移的问题。同时，基于采用滑动窗口机制对位姿数据处理，可避免长时间位姿累积导致的系统优化耗时递增的问题，提升融合定位处理效率，保障车辆定位的实时性。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单介绍，显而易见地，下面描述的附图仅仅是本发明的一些实施例，对本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获取其他附图。
图1为本发明一个实施例提供的一种车辆融合定位方法的流程示意图；
图2为本发明一个实施例提供的融合定位位姿图的结构示意图；
图3为本发明一个实施例提供的一种车辆融合定位系统的结构示意图；
图4为本发明一个实施例提供的一种电子设备的结构示意图。
具体实施方式
为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
应当理解，本发明的说明书或权利要求书及上述附图中的术语“包括”以及其他相近意思表述，意指覆盖不排他的包含，如包含一系列步骤或单元的过程、方法或系统、设备没有限定于已列出的步骤或单元。此外，“第一”“第二”用于区分不同对象，并非用于描述特定顺序。
请参阅图1，本发明实施例提供的一种车辆融合定位方法的流程示意图，包括：
S101、设置车辆位姿匹配数据对应的滑动窗口大小，设置车辆初始局部位姿到全局位姿的变换矩阵，并定义初始优化固定状态；
所述车辆位姿匹配数据为用于车辆融合定位的数据，包括IMU(惯性测量单元)、相机采集相对位姿数据，以及GPS采集的绝对定位数据。其中，相机采集的图像数据作为VO(Visual Odometry，即视觉里程计)数据，相机和IMU采集的图像、惯性数据作为VIO(Visual-Inertial Odometry，即视觉惯性里程计)作为VIO数据。
所述滑动窗口用于框选进行融合定位的VO/VIO数据帧以及GPS数据帧，基于滑动窗口内连续的数据帧，进行位姿图优化。可以使得系统能够长时间实时运行，不会因为位姿图节点的增加影响系统的实时性。
其中，初始局部位姿到全局位姿的变换矩阵表示为：
式中，表示局部位姿到全局位姿的变换矩阵，/>表示全局位姿，/>表示局部位姿。
所述初始优化固定状态用于表示滑动窗口内数据帧是否被优化，一般在进行位姿图优化前或优化未能满足预定要求，会设置为特定的状态标识，如设为False。
S102、每接收到一帧VO/VIO数据，则通过所述变换矩阵将车辆位姿转换至全局位姿，并将接收到的VO/VIO数据和GPS数据作为最新帧放入滑动窗口中；
对于车辆新采集的VO/VIO数据，可以通过变换矩阵将车辆相对位姿转换为全局位姿，即局部位姿变化至全局位姿。
其中，将新接收到的VO/VIO数据帧和GPS数据帧放入滑动窗口作为最新帧，并删除滑动窗口内最旧的帧，保持窗口大小不变。
S103、对滑动窗口内数据进行位姿图优化，得到节点车辆的绝对坐标；
所述位姿图优化是指对路标点及其对应点轨迹进行位姿和路标点优化，去除路标只考虑相机间位姿关系，进行图优化。本发明实施例中位姿图中还包括GPS数据，对位姿图进行全局约束。
在所述位姿图中，节点是滑动窗口内所有数据的全局位姿，两个相邻节点之间的边由VO/VIO局部约束构成，每个节点还包括一个GPS全局约束的边，若所有观测满足高斯分布图优化目标表示为非线性最小二乘：
式中，χ*表示局部因子，χ表示窗口内的状态量，n表示窗口数量，k表示观测数量，S表示观测值，表示窗口内t帧的第k个观测，/>表示窗口内t帧的第k个观测的观测模型。
在一个实施例中，所述位姿图的结构如图2所示，图中，Xi、Xj、Xk为位姿图的节点，表示窗口内的状态量，或称待优化的状态量，Li、Lj方块表示位姿图的边，即VO/VIO相对位姿约束，Gi、Gj、Gk方块为位姿图另一条边，表示GPS绝对位置约束。在滑动窗口内，基于VO/VIO相对位姿约束和GPS绝对位置约束可以确定节点状态，即得到节点的绝对位置。
在位姿图中，其局部因子为VO/VIO数据，由窗口内连续相邻两帧之间的位姿构成，例如，对t-1帧位姿和第/>帧位姿/>局部因子表示为：
式中，表示窗口内t帧的第l个观测，/>表示窗口内t帧的第l个观测的观测模型，χ表示窗口内的状态量，Xt-1、Xt分别表示t-1帧的状态量和t帧的状态量，/>表示t-1帧的局部位姿，/>表示t帧的局部位姿，/>表示四元数减法；
全局因子为GPS数据，将GPS的经纬高转换到UTM坐标系下，并将第一个GPS坐标设置为全局坐标系原点，GPS测量值在全局坐标系中表示为全局因子表示为：
式中，表示窗口内t帧的GPS观测，/>表示窗口内t帧的GPS观测模型，Xt表示t帧的状态量，/>表示t帧的全局坐标，/>表示t帧的观测模型。
如果VO/VIO输出的有协方差，则将其作为局部约束的协方差，否则局部约束使用统一的协方差。
所述车辆的绝对坐标是指车辆的绝对定位，包含有经纬度坐标，还可以包含有高度、航向角、速度等状态信息，其对应于位姿图中的节点，每个节点的状态表示车辆的绝对定位。
GPS观测值约束每个节点的全局位置，其协方差大小由GPS接收信号的状态位决定，接收到的卫星越多，GPS信号越好，协方差就越小。
S104、根据优化结果和原始VO/VIO数据更新所述变换矩阵，并基于优化前后的位置误差修改优化固定状态。
根据优化后局部位姿和全局位姿，结合滑动窗口中原始VO/VIO数据，对更新所述变换矩阵。
具体的，设滑动窗口内最新帧的局部位姿为全局位姿为/>则更新后的变换矩阵表示为：
若优化后的车辆位置与优化前的全局GPS坐标误差小于设定值，则修改所述优化固定状态，如优化固定状态为True，标定不对当前滑动窗口内车辆位姿数据优化。
每一次优化后都会判断窗口内每一帧数据的残差，针对误差很小的数据设置其优化固定状态为True，后续不再重复优化，进一步提高了系统运行效率。
本实施例中，采用滑动窗口，可以使得系统能够长时间实时运行，不会因为位姿图节点的增加影响系统的实时性。在每一次优化后，重新计算VO/VIO和GPS之间的变换关系，消除位姿误差，可以提高融合定位精度。基于位姿图优化结合最新的帧数据，可以提高融合定位精度，避免GPS长时间、大尺度漂移的问题。
应理解，上述实施例中各步骤的序号大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
图3为本发明实施例提供的一种车辆融合定位系统的结构示意图，该系统包括：
窗口设置模块310，用于设置车辆位姿匹配数据对应的滑动窗口大小，设置车辆初始局部位姿到全局位姿的变换矩阵，并定义初始优化固定状态；
具体的，根据公式(1)设置变换矩阵：
式中，表示变换矩阵，/>表示全局位姿，/>表示局部位姿。
位姿变换模块320，用于每接收到一帧VO/VIO数据，则通过所述变换矩阵将车辆位姿转换至全局位姿，并将接收到的VO/VIO数据作为最新帧放入滑动窗口中；
其中，将接收的VO/VIO数据帧放入滑动窗口作为最新帧，并删除窗口内最旧的帧，保持窗口大小不变。
位姿图优化模块330，用于对滑动窗口内数据进行位姿图优化，得到节点车辆的绝对坐标；
优选的，所述位姿图中两个相邻节点之间的边由VO/VIO局部约束构成，每个节点由GPS全局约束构成，优化目标表示为非线性最小二乘。
其中，所述对滑动窗口内数据进行位姿图优化包括：
将VO/VIO数据作为局部因子，由滑动窗口内相邻两帧车辆位姿构成，所述局部因子表示为：
式中，表示窗口内t帧的第l个观测，/>表示窗口内t帧的第l个观测的观测模型，χ表示窗口内的状态量，Xt-1、Xt分别表示t-1帧的状态量和t帧的状态量，/>表示t-1帧的局部位姿，/>表示t帧的局部位姿，/>表示四元数减法；
将GPS数据作为全局因子，将GPS位置转换至UMT坐标系后，在全局坐标系中标注对应的GPS位置，所述全局因子表示为：
式中，表示窗口内t帧的GPS观测，/>表示窗口内t帧的GPS观测模型，Xt表示t帧的状态量，/>表示t帧的全局坐标，/>表示t帧的观测模型。
更新模块340，用于根据优化结果和原始VO/VIO数据更新所述变换矩阵，并基于优化前后的位置误差修改优化固定状态。
可选的，若优化后的车辆位置与优化前的全局GPS坐标误差小于设定值，则修改所述优化固定状态，标定不对当前滑动窗口内车辆位姿数据优化。
所述领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置和模块的具体工作过程可以参考前述方法实施例中对应的过程，在此不再赘述。
图4是本发明一实施例提供的一种电子设备的结构示意图。所述电子设备用于车辆实时融合定位，通常为电子计算机。如图4所示，该实施例的电子设备4包括：存储器410、处理器420以及系统总线430，所述存储器410包括存储其上的可运行的程序4101，本领域技术人员可以理解，图4中示出的电子设备结构并不构成对电子设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
下面结合图4对电子设备的各个构成部件进行具体的介绍：
存储器410可用于存储软件程序以及模块，处理器420通过运行存储在存储器410的软件程序以及模块，从而执行电子设备的各种功能应用以及数据处理。存储器410可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据电子设备的使用所创建的数据(比如缓存数据)等。此外，存储器410可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
在存储器410上包含网络请求方法的可运行程序4101，所述可运行程序4101可以被分割成一个或多个模块/单元，所述一个或多个模块/单元被存储在所述存储器410中，并由处理器420执行，以实现行车数据一致性校验等，所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序4101在所述电子设备4中的执行过程。例如，所述计算机程序4101可以被分割为窗口设置模块、位姿变换模块、位姿图优化模块和更新模块。
处理器420是电子设备的控制中心，利用各种接口和线路连接整个电子设备的各个部分，通过运行或执行存储在存储器410内的软件程序和/或模块，以及调用存储在存储器410内的数据，执行电子设备的各种功能和处理数据，从而对电子设备进行整体状态监控。可选的，处理器420可包括一个或多个处理单元；优选的，处理器420可集成应用处理器和调制解调处理器，其中，应用处理器主要处理操作系统、应用程序等，调制解调处理器主要处理无线通信。可以理解的是，上述调制解调处理器也可以不集成到处理器420中。
系统总线430是用来连接计算机内部各功能部件，可以传送数据信息、地址信息、控制信息，其种类可以是例如PCI总线、ISA总线、VESA总线等。处理器420的指令通过总线传递至存储器410，存储器410反馈数据给处理器420，系统总线430负责处理器420与存储器410之间的数据、指令交互。当然系统总线430还可以接入其他设备，例如网络接口、显示设备等。
在本发明实施例中，该电子设备所包括的处理420执行的可运行程序包括：
设置车辆位姿匹配数据对应的滑动窗口大小，设置车辆初始局部位姿到全局位姿的变换矩阵，并定义初始优化固定状态；
每接收到一帧VO/VIO数据，则通过所述变换矩阵将车辆位姿转换至全局位姿，并将接收到的VO/VIO数据和GPS数据作为最新帧放入滑动窗口中；
对滑动窗口内数据进行位姿图优化，得到节点车辆的绝对坐标；
根据优化结果和原始VO/VIO数据更新所述变换矩阵，并基于优化前后的位置误差修改优化固定状态。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

1.一种车辆融合定位方法，其特征在于，包括：
设置车辆位姿匹配数据对应的滑动窗口大小，设置车辆初始局部位姿到全局位姿的变换矩阵，并定义初始优化固定状态；
每接收到一帧VO/VIO数据，则通过所述变换矩阵将车辆位姿转换至全局位姿，并将接收到的VO/VIO数据和GPS数据作为最新帧放入滑动窗口中；
对滑动窗口内数据进行位姿图优化，得到节点车辆的绝对坐标；
其中，所述位姿图中两个相邻节点之间的边由VO/VIO局部约束构成，每个节点由GPS全局约束构成，优化目标表示为非线性最小二乘；
根据优化结果和原始VO/VIO数据更新所述变换矩阵，并基于优化前后的位置误差修改优化固定状态；
其中，所述基于优化前后的位置误差修改优化固定状态包括：
若优化后的车辆位置与优化前的全局GPS坐标误差小于设定值，则修改所述优化固定状态，标定不对当前滑动窗口内车辆位姿数据优化。
2.根据权利要求1所述的方法，其特征在于，所述设置车辆初始局部位姿到全局位姿的变换矩阵具体为：
根据公式(1)设置变换矩阵：
式中，表示变换矩阵，/>表示全局位姿，/>表示局部位姿。
3.根据权利要求1所述的方法，其特征在于，所述将接收到的VO/VIO数据和GPS作为最新帧放入滑动窗口中还包括：
删除滑动窗口内最旧的帧，保持滑动窗口大小不变。
4.根据权利要求1所述的方法，其特征在于，所述对滑动窗口内数据进行位姿图优化包括：
将VO/VIO数据作为局部因子，由滑动窗口内相邻两帧车辆位姿构成，所述局部因子表示为：
式中，表示窗口内t帧的第l个观测，/>表示窗口内t帧的第l个观测的观测模型，χ表示窗口内的状态量，Xt-1、Xt分别表示t-1帧的状态量和t帧的状态量，/>表示t-1帧的局部位姿，/>表示t帧的局部位姿，/>表示四元数减法，/>表示t-1帧的局部位姿观测模型，/>表示t帧的局部位姿观测模型；
将GPS数据作为全局因子，将GPS位置转换至UMT坐标系后，在全局坐标系中标注对应的GPS位置，所述全局因子表示为：
式中，表示窗口内t帧的GPS观测，/>表示窗口内t帧的GPS观测模型，Xt表示t帧的状态量，/>表示t帧的全局坐标，/>表示t帧的观测模型。
5.一种车辆融合定位系统，其特征在于，包括：
窗口设置模块，用于设置车辆位姿匹配数据对应的滑动窗口大小，设置车辆初始局部位姿到全局位姿的变换矩阵，并定义初始优化固定状态；
位姿变换模块，用于每接收到一帧VO/VIO数据，则通过所述变换矩阵将车辆位姿转换至全局位姿，并将接收到的VO/VIO数据和GPS数据作为最新帧放入滑动窗口中；
位姿图优化模块，用于对滑动窗口内数据进行位姿图优化，得到节点车辆的绝对坐标；
其中，所述位姿图中两个相邻节点之间的边由VO/VIO局部约束构成，每个节点由GPS全局约束构成，优化目标表示为非线性最小二乘；
更新模块，用于根据优化结果和原始VO/VIO数据更新所述变换矩阵，并基于优化前后的位置误差修改优化固定状态；
其中，所述基于优化前后的位置误差修改优化固定状态包括：
若优化后的车辆位置与优化前的全局GPS坐标误差小于设定值，则修改所述优化固定状态，标定不对当前滑动窗口内车辆位姿数据优化。
6.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至4任一项所述的一种车辆融合定位方法的步骤。
7.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被执行时实现如权利要求1至4任一项所述的一种车辆融合定位方法的步骤。
CN202110855287.6A 2021-07-27 2021-07-27 一种车辆融合定位方法、系统、电子设备及存储介质 Active CN113776543B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110855287.6A CN113776543B (zh) 2021-07-27 2021-07-27 一种车辆融合定位方法、系统、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110855287.6A CN113776543B (zh) 2021-07-27 2021-07-27 一种车辆融合定位方法、系统、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN113776543A CN113776543A (zh) 2021-12-10
CN113776543B true CN113776543B (zh) 2023-11-28

Family

ID=78836254

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110855287.6A Active CN113776543B (zh) 2021-07-27 2021-07-27 一种车辆融合定位方法、系统、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN113776543B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111238535A (zh) * 2020-02-03 2020-06-05 南京航空航天大学 一种基于因子图的imu误差在线标定方法
CN111272192A (zh) * 2020-02-11 2020-06-12 清华大学 用于滑移转向机器人的联合位姿确定方法及装置

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018125938A1 (en) * 2016-12-30 2018-07-05 DeepMap Inc. Enrichment of point cloud data for high-definition maps for autonomous vehicles
SG11201811001WA (en) * 2018-07-02 2020-02-27 Beijing Didi Infinity Technology & Development Co Ltd Vehicle navigation system using pose estimation based on point cloud
CN113906414A (zh) * 2019-03-05 2022-01-07 辉达公司 用于生成导航自主车辆用的高清晰度地图的位姿图的分布式处理

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111238535A (zh) * 2020-02-03 2020-06-05 南京航空航天大学 一种基于因子图的imu误差在线标定方法
CN111272192A (zh) * 2020-02-11 2020-06-12 清华大学 用于滑移转向机器人的联合位姿确定方法及装置

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator;Qin Tong 等;IEEE Transactions on Robotics;第34卷(第4期);第1004-1020页 *
基于RGB-D相机数据的SLAM算法;洪亮;冯常;;电子设计工程;第26卷(第09期);第147-151、157页 *
基于因子图的无人喷雾机多源融合定位;盛晨航;沈跃;刘慧;崔业民;龙友能;;江苏大学学报(自然科学版);第41卷(第03期);第281-287页 *

Also Published As

Publication number Publication date
CN113776543A (zh) 2021-12-10

Similar Documents

Publication Publication Date Title
US11105638B2 (en) Method, apparatus, and computer readable storage medium for updating electronic map
JP6688521B2 (ja) 測位方法、端末及びサーバー
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN112810625B (zh) 一种轨迹修正的方法及装置
CN112835085B (zh) 确定车辆位置的方法和装置
CN108021886B (zh) 一种无人机重复纹理影像局部显著特征点匹配方法
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
JP7356528B2 (ja) 地図データ処理方法及び装置
US20200209389A1 (en) Locating Method and Device, Storage Medium, and Electronic Device
CN110567467A (zh) 基于多传感器的地图构建方法、装置及存储介质
CN114689047A (zh) 基于深度学习的组合导航方法、装置、系统及存储介质
WO2020135183A1 (zh) 点云地图的构建方法、装置、计算机设备和存储介质
CN109655060B (zh) 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统
CN113776543B (zh) 一种车辆融合定位方法、系统、电子设备及存储介质
CN112150550B (zh) 一种融合定位方法及装置
CN115979262B (zh) 飞行器的定位方法、装置、设备及存储介质
CN110853098A (zh) 机器人定位方法、装置、设备及存储介质
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN114088104B (zh) 一种自动驾驶场景下的地图生成方法
CN115560744A (zh) 机器人以及基于多传感器的三维建图方法、存储介质
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN114814875A (zh) 一种机器人定位建图方法、装置、可读存储介质及机器人
CN112835086B (zh) 确定车辆位置的方法和装置
CN114993317A (zh) 一种基于多源融合的室内外无缝定位方法
CN113790723B (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