CN116242373A - 一种融合多源数据的高精度导航定位方法及系统 - Google Patents

一种融合多源数据的高精度导航定位方法及系统 Download PDF

Info

Publication number
CN116242373A
CN116242373A CN202310208302.7A CN202310208302A CN116242373A CN 116242373 A CN116242373 A CN 116242373A CN 202310208302 A CN202310208302 A CN 202310208302A CN 116242373 A CN116242373 A CN 116242373A
Authority
CN
China
Prior art keywords
source data
observation model
navigation positioning
data
determining
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.)
Pending
Application number
CN202310208302.7A
Other languages
English (en)
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.)
Aerospace Hongtu Information Technology Co Ltd
Original Assignee
Aerospace Hongtu Information 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 Aerospace Hongtu Information Technology Co Ltd filed Critical Aerospace Hongtu Information Technology Co Ltd
Priority to CN202310208302.7A priority Critical patent/CN116242373A/zh
Publication of CN116242373A publication Critical patent/CN116242373A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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

本发明提供了一种融合多源数据的高精度导航定位方法及系统，涉及定位导航的技术领域，包括：获取待定位对象的多源数据，其中，多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；基于多源数据和优化真值求差算法，构建待定位对象的状态向量；基于状态向量、多源数据和频率加权因子算法，确定出待定位对象的量测方程；基于Kalman滤波参数估计算法、量测方程和状态向量，确定出待定位对象的导航定位参数，其中，导航定位参数包括：位置参数，速度参数和姿态参数，解决了现有融合多源数据的高精度导航定位方法的导航定位解算效率和准确率较低的技术问题。

Description

一种融合多源数据的高精度导航定位方法及系统
技术领域
本发明涉及定位导航的技术领域，尤其是涉及一种融合多源数据的高精度导航定位方法及系统。
背景技术
随着汽车行业的产业升级，自动驾驶技术作为汽车发展的一个方向，已成为一个研究热点。高精度的位置信息获取是实现自动驾驶的先决条件。随着卫星导航、惯性导航、视觉导航、激光导航等技术的不断发展，获取高精度位置信息的方式不断多样化。
卫星导航技术通过接收四颗及以上导航卫星信号实现实时导航定位，但在多遮挡环境下，容易出现信号失锁继而无法进行导航定位；惯性导航技术不受环境影响，能进行持续导航定位，但随着时间过长，会出现误差累计，严重影响定位精度；视觉导航通过相邻关键帧特征点的位移与旋转确定姿态转移矩阵，从而实现导航定位，特征点的高效提取影响着实时导航定位的精度；激光导航基于激光雷达高精度的测距信息进行实时导航定位，但其定位精度受光照条件、点云稀疏影响较大。
针对上述问题，还未提出有效的解决方案。
发明内容
有鉴于此，本发明的目的在于提供一种融合多源数据的高精度导航定位方法及系统，以缓解了现有融合多源数据的高精度导航定位方法的导航定位解算效率和准确率较低的技术问题。
第一方面，本发明实施例提供了一种融合多源数据的高精度导航定位方法，包括：获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数。
进一步的，基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量，包括：确定出所述多源数据对应的观测模型，其中，所述观测模型包括：GNSS观测模型、惯性导航系统观测模型、视觉观测模型和激光观测模型；基于所述观测模型和所述多源数据，确定出所述导航定位参数真值；基于所述导航定位参数真值，构建所述待定位对象的状态向量。
进一步的，基于所述观测模型和所述多源数据，确定出所述导航定位参数真值，包括：基于所述观测模型和所述多源数据，分别确定出所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数；基于所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数，确定出所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数的方差；将所述方差中的最小值对应的第一导航定位参数确定为所述导航定位参数真值。
进一步的，所述状态向量为x＝[δP,δv,δR,δba,δbg]，其中，
Figure BDA0004111684910000031
δP为位置误差，P为真实位置，PINS为惯性导航位置估计值，δv为速度误差，v为真实速度，vINS为惯性导航速度估计值，δR为姿态误差，R为真实姿态，RINS为惯性导航姿态估计，ln为对数映射，()V为反对称矩阵对应的向量，δba为加速度计零偏误差，ba为真实加速度零偏，/>
Figure BDA0004111684910000032
为加速度计零偏估计值，δbg为陀螺仪零偏误差，bg为真实陀螺仪零偏，/>
Figure BDA0004111684910000033
为陀螺仪零偏估计值。
进一步的，基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程，包括：基于所述多源数据的采样频率，计算出频率加权数；基于所述多源数据，计算出精度因子；基于所述频率加权数和所述精度因子，计算出加权精度因子；基于所述加权精度因子，确定出所述待定位对象的量测方程。
进一步的，将所述待定位对象的导航定位参数与高精度地图的路网信息进行匹配，为所述待定位对象进行高精度导航。
第二方面，本发明实施例还提供了一种融合多源数据的高精度导航定位系统，包括：获取单元，用于获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；构建单元，用于基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；确定单元，用于基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；定位单元，用于基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数。
进一步的，所述构建单元，用于：确定出所述多源数据对应的观测模型，其中，所述观测模型包括：GNSS观测模型、惯性导航系统观测模型、视觉观测模型和激光观测模型；基于所述观测模型和所述多源数据，确定出所述导航定位参数真值；基于所述导航定位参数真值，构建所述待定位对象的状态向量。
第三方面，本发明实施例还提供了一种电子设备，包括存储器以及处理器，所述存储器用于存储支持处理器执行上述第一方面中所述方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。
第四方面，本发明实施例还提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序。
在本发明实施例中，通过获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数，达到了对融合多源数据的导航定位参数进行高精度和高效率的解算的目的，进而解决了现有融合多源数据的高精度导航定位方法的导航定位解算效率和准确率较低的技术问题，从而实现了提高融合多源数据的高精度导航定位的准确率和效率的技术效果。
本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种融合多源数据的高精度导航定位方法的流程图；
图2为本发明实施例提供的一种融合多源数据的高精度导航定位方法系统的示意图；
图3为本发明实施例提供的一种电子设别的示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例一：
根据本发明实施例，提供了一种融合多源数据的高精度导航定位的实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
图1是根据本发明实施例的融合多源数据的高精度导航定位的流程图，如图1所示，该方法包括如下步骤：
步骤S102，获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；
步骤S104，基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；
步骤S106，基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；
步骤S108，基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数。
在本发明实施例中，通过获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数，达到了对融合多源数据的导航定位参数进行高精度和高效率的解算的目的，进而解决了现有融合多源数据的高精度导航定位方法的导航定位解算效率和准确率较低的技术问题，从而实现了提高融合多源数据的高精度导航定位的准确率和效率的技术效果。
在本发明实施例中，步骤S104包括如下步骤：
步骤S201，确定出所述多源数据对应的观测模型，其中，所述观测模型包括：GNSS观测模型、惯性导航系统观测模型、视觉观测模型和激光观测模型；
步骤S202，基于所述观测模型和所述多源数据，确定出所述导航定位参数真值；
步骤S203，基于所述导航定位参数真值，构建所述待定位对象的状态向量。
具体的，在本发明实施例中，步骤S202包括如下步骤：
基于所述观测模型和所述多源数据，分别确定出所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数；
基于所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数，确定出所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数的方差；
将所述方差中的最小值对应的第一导航定位参数确定为所述导航定位参数真值。
在本发明实施例中，获取GNSS观测数据、惯性导航系统观测数据、视觉图像数据、激光雷达数据之后，通过优化真值求差法构建状态向量。
首先，确定各导航定位观测数据的观测模型：
GNSS相位观测模型：
Figure BDA0004111684910000071
式中，
Figure BDA0004111684910000072
为GNSS相位观测数据；/>
Figure BDA0004111684910000073
为接收机到卫星的几何距离；c为真空中光速；dtr为接收机钟差；dts为卫星钟差；λ为载波波长；/>
Figure BDA0004111684910000074
为载波相位整周模糊度；br为接收机端非校正相位硬件延迟；bs为卫星端非校正相位硬件延迟；/>
Figure BDA0004111684910000075
为电离层延迟量；/>
Figure BDA0004111684910000076
为对流层延迟量；/>
Figure BDA0004111684910000077
为载波相位观测噪声。
惯性导航观测模型：
姿态微分方程：
Figure BDA0004111684910000078
/>
式中，
Figure BDA0004111684910000079
为/>
Figure BDA00041116849100000710
的反对称矩阵；/>
Figure BDA00041116849100000712
为从载体坐标系旋转到地心地固系的四元数形式姿态矩阵；/>
Figure BDA00041116849100000711
Figure BDA0004111684910000081
为陀螺仪输出角速度，/>
Figure BDA0004111684910000082
为/>
Figure BDA0004111684910000083
的共轭四元数，/>
Figure BDA0004111684910000084
为地球自转角速度。
速度微分方程：
Figure BDA0004111684910000085
式中，
Figure BDA0004111684910000086
为加速度计输出的比力；ve为地速；ge为地心地固坐标系下的重力矢量。
位置微分方程：
Figure BDA00041116849100000812
激光观测模型：
Figure BDA0004111684910000087
Figure BDA0004111684910000088
式中，yR,k、yP,k分别为姿态增量和位置增量；
Figure BDA0004111684910000089
为k-1时刻姿态最优估计；Rk为k时刻的姿态矩阵；κR,k为姿态增量观测噪声；Δt为时间增量；Pk为k时刻位置观测量；/>
Figure BDA00041116849100000810
为k-1时刻位置最优估计量；ηP,k为位置增量观测噪声；exp()为指数映射；()^为向量的反对称矩阵。
视觉观测模型：
视觉观测模型中的姿态模型和位置模型与激光观测模型一致，速度模型为：
Figure BDA00041116849100000811
式中，yv,k为速度增量；vk为k时刻的速度观测量；γv,k为速度增量观测噪声。
然后，根据GNSS相位观测模型、激光观测模型和视觉观测模型分别求解各观测量对应的速度、位置与姿态为vs、Ps、Rs，其中s＝(GNSS,LADIR,VO)，vs为GNSS相位观测模型、激光观测模型和视觉观测模型对应的速度，Ps为GNSS相位观测模型、激光观测模型和视觉观测模型对应的速度的位置，Rs为GNSS相位观测模型、激光观测模型和视觉观测模型对应的姿态，并求解相应的方差Ds，s含义与上面一致。将最小方差对应的速度、位置与姿态作为真实的速度、位置与姿态(即，导航定位参数真值)，可快速有效确定真值。
最后，在确定出导航定位参数真值之后，以位置误差、速度误差、姿态误差、加速度计零偏误差、陀螺仪零偏误差构建状态向量，关键在于对真值的确定，在多种观测数据解算的参数值中优化选择，选定的真值更接近于实际真值，可有效减小状态向量误差，提高时间更新精度，状态向量可表示为：x＝[δP,δv,δR,δba,δbg]，其中，
Figure BDA0004111684910000091
δP为位置误差；P为真实位置；PINS为惯性导航位置估计值；δv为速度误差；v为真实速度；vINS为惯性导航速度估计值；δR为姿态误差；R为真实姿态；RINS为惯性导航姿态估计；ln为对数映射；()V为反对称矩阵对应的向量；δba为加速度计零偏误差；ba为真实加速度零偏；/>
Figure BDA0004111684910000092
为加速度计零偏估计值；δbg为陀螺仪零偏误差；bg为真实陀螺仪零偏；/>
Figure BDA0004111684910000093
为陀螺仪零偏估计值。
在本发明实施例中，步骤S106包括如下步骤：
基于所述多源数据的采样频率，计算出频率加权数；
基于所述多源数据，计算出精度因子；
基于所述频率加权数和所述精度因子，计算出加权精度因子；
基于所述加权精度因子，确定出所述待定位对象的量测方程。
在本发明实施例中，在利用多源观测数据进行实时导航定位时，将惯性导航数据作为时间更新，GNSS观测数据、激光雷达数据、视觉图像数据作为量测更新。量测更新的精度和稳定性深刻影响着最终的导航定位精度，通过频率加权精度因子法确定最优量测方程，进行量测更新，可保障选择最优精度与稳定性的观测数据，从而抑制时间更新的误差漂移，可有效提高导航定位精度，主要步骤如下：
第一步，计算频率加权数：
Figure BDA0004111684910000101
式中，β为放大因子；Kw为观测数据采样频率，其中w＝(GNSS,LADIR,VO)，分别表示GNSS观测数据采样频率、激光雷达观测数据采样频率、视觉图像采样频率；K0为参考频率。
第二步，计算精度因子：
Figure BDA0004111684910000102
式中，
Figure BDA00041116849100001010
为精度因子，其中/>
Figure BDA00041116849100001011
分别表示GNSS观测数据、激光雷达观测数据、视觉图像观测数据；n为连续采样个数；/>
Figure BDA0004111684910000103
为观测数据，其中/>
Figure BDA00041116849100001012
与上述表示含义一致。
第三步，利用频率加权精度因子确定最优量测方程：
根据第一步计算的频率加权数和第二步计算的精度因子，计算频率加权精度因子：
Figure BDA0004111684910000104
式中，Cυ为频率加权精度因子，其中υ＝(GNSS,LADIR,VO)，分别表示GNSS观测数据、激光雷达观测数据、视觉图像观测数据。
第四步，根据频率加权精度因子，取最小频率加权精度因子对应观测数据的量测方程为导航定位解算的量测方程：
Figure BDA0004111684910000105
式中，
Figure BDA0004111684910000106
为k时刻观测向量；/>
Figure BDA0004111684910000107
为k时刻观测矩阵；/>
Figure BDA0004111684910000108
为k时刻噪声矩阵；/>
Figure BDA0004111684910000109
为k时刻观测噪声；ε＝(GNSS,LADIR,VO)，分别表示GNSS观测数据、激光雷达观测数据、视觉图像观测数据，ε取最小频率加权精度因子对应的观测数据。
下面对步骤S108进行详细说明。
根据量测方程和状态向量利用经典Kalman滤波参数估计方法进行导航定位参数估计。量测方程所解算的参数精度，直接影响这状态向量的修正精度。用频率加权精度因子法优化确定量测方程，构建动态量测方程Kalman滤波器进行参数估计，可有效提高最终导航定位参数解算精度。
详细处理流程为：以惯性导航观测模型作为状态方程，确定k时刻状态向量xk，通过自适应确定动态量测方程，得到有效的观测量zk，将xk和zk输入到Kalman滤波器中，最终获取最优估计结果xk，然后迭代到k+1时刻，重复上述操作，随着时间递推，不断输出导航定位参数。
最后，在确定出导航定位参数之后，实时输出待定位对象的位置、速度和姿态，并将高精度地图的路网信息与导航定位参数实时匹配，实时修正定位参数，从而实现高精度导航。
实时高精度导航定位是实现自动驾驶的重要条件。目前汽车上搭载了GNSS、惯性导航器件、激光雷达、相机等多种传感器用于汽车的导航定位。GNSS技术通过接收四颗及以上导航卫星信号实现实时导航定位，但在多遮挡环境下，容易出现信号失锁继而无法进行导航定位；惯性导航技术不受环境影响，能进行持续导航定位，但随着时间过长，会出现误差累计，严重影响定位精度；视觉导航通过相邻关键帧特征点的位移与旋转确定姿态转移矩阵，从而实现导航定位，特征点的高效提取影响着实时导航定位的精度；激光导航基于激光雷达高精度的测距信息进行实时导航定位，但其定位精度受光照条件、点云稀疏影响较大。本发明实施例提出了一种融合多源数据的高精度导航定位方法及系统。基于GNSS观测数据、INS数据、视觉图像数据、激光雷达数据，利用优化真值求差法，构建状态向量；利用频率加权精度因子法自适应确定最优量测方程，进行量测更新，可保障选择最优精度与稳定性的观测数据，从而抑制时间更新的误差漂移；构建基于动态量测方程的Kalman滤波器，实时求解导航定位参数，可以显著提高导航定位参数解算精度；最后利用高精度地图信息对定位参数进行修正，以实现实时高精度导航。
实施例二：
本发明实施例还提供了一种融合多源数据的高精度导航定位系统，该融合多源数据的高精度导航定位系统用于执行本发明实施例上述内容所提供的融合多源数据的高精度导航定位方法，以下是本发明实施例提供的融合多源数据的高精度导航定位系统的具体介绍。
如图2所示，图2为上述融合多源数据的高精度导航定位系统的示意图，该融合多源数据的高精度导航定位系统包括：
获取单元10，用于获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；
构建单元20，用于基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；
确定单元30，用于基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；
定位单元40，用于基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数。
在本发明实施例中，通过获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数，达到了对融合多源数据的导航定位参数进行高精度和高效率的解算的目的，进而解决了现有融合多源数据的高精度导航定位方法的导航定位解算效率和准确率较低的技术问题，从而实现了提高融合多源数据的高精度导航定位的准确率和效率的技术效果。
实施例三：
本发明实施例还提供了一种电子设备，包括存储器以及处理器，所述存储器用于存储支持处理器执行上述实施例一中所述方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。
参见图3，本发明实施例还提供一种电子设备100，包括：处理器60，存储器61，总线62和通信接口63，所述处理器60、通信接口63和存储器61通过总线62连接；处理器60用于执行存储器61中存储的可执行模块，例如计算机程序。
其中，存储器61可能包含高速随机存取存储器(RAM，Random Access Memory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。通过至少一个通信接口63(可以是有线或者无线)实现该系统网元与至少一个其他网元之间的通信连接，可以使用互联网，广域网，本地网，城域网等。
总线62可以是ISA总线、PCI总线或EISA总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示，图3中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。
其中，存储器61用于存储程序，所述处理器60在接收到执行指令后，执行所述程序，前述本发明实施例任一实施例揭示的流过程定义的装置所执行的方法可以应用于处理器60中，或者由处理器60实现。
处理器60可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器60中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器60可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(Digital SignalProcessing，简称DSP)、专用集成电路(Application Specific Integrated Circuit，简称ASIC)、现成可编程门阵列(Field-Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器61，处理器60读取存储器61中的信息，结合其硬件完成上述方法的步骤。
实施例四：
本发明实施例还提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器运行时执行上述实施例一中所述方法的步骤。
另外，在本发明实施例的描述中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (10)

1.一种融合多源数据的高精度导航定位方法，其特征在于，包括：
获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；
基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；
基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；
基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数。
2.根据权利要求1所述的方法，其特征在于，基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量，包括：
确定出所述多源数据对应的观测模型，其中，所述观测模型包括：GNSS观测模型、惯性导航系统观测模型、视觉观测模型和激光观测模型；
基于所述观测模型和所述多源数据，确定出导航定位参数真值；
基于所述导航定位参数真值，构建所述待定位对象的状态向量。
3.根据权利要求2所述的方法，其特征在于，基于所述观测模型和所述多源数据，确定出导航定位参数真值，包括：
基于所述观测模型和所述多源数据，分别确定出所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数；
基于所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数，确定出所述GNSS观测模型、所述视觉观测模型和所述激光观测模型对应的第一导航定位参数的方差；
将所述方差中的最小值对应的第一导航定位参数确定为所述导航定位参数真值。
4.根据权利要求2所述的方法，其特征在于，
所述状态向量为x＝[δP,δv,δR,δba,bg]，其中，
Figure FDA0004111684890000021
δP为位置误差，P为真实位置，PINS为惯性导航位置估计值，δv为速度误差，v为真实速度，vINS为惯性导航速度估计值，δR为姿态误差，R为真实姿态，RINS为惯性导航姿态估计，ln为对数映射，()V为反对称矩阵对应的向量，δba为加速度计零偏误差，ba为真实加速度零偏，/>
Figure FDA0004111684890000022
为加速度计零偏估计值，δbg为陀螺仪零偏误差，bg为真实陀螺仪零偏，/>
Figure FDA0004111684890000023
为陀螺仪零偏估计值。
5.根据权利要求1所述的方法，其特征在于，基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程，包括：
基于所述多源数据的采样频率，计算出频率加权数；
基于所述多源数据，计算出精度因子；
基于所述频率加权数和所述精度因子，计算出加权精度因子；
基于所述加权精度因子，确定出所述待定位对象的量测方程。
6.根据权利要求1所述的方法，其特征在于，所述方法还包括：
将所述待定位对象的导航定位参数与高精度地图的路网信息进行匹配，为所述待定位对象进行高精度导航。
7.一种融合多源数据的高精度导航定位系统，其特征在于，包括：
获取单元，用于获取待定位对象的多源数据，其中，所述多源数据包括：GNSS观测数据、惯性导航系统观测数据、视觉图像数据和激光雷达数据；
构建单元，用于基于所述多源数据和优化真值求差算法，构建所述待定位对象的状态向量；
确定单元，用于基于所述状态向量、所述多源数据和频率加权因子算法，确定出所述待定位对象的量测方程；
定位单元，用于基于Kalman滤波参数估计算法、所述量测方程和所述状态向量，确定出所述待定位对象的导航定位参数，其中，所述导航定位参数包括：位置参数，速度参数和姿态参数。
8.根据权利要求7所述的系统，其特征在于，所述构建单元，用于：
确定出所述多源数据对应的观测模型，其中，所述观测模型包括：GNSS观测模型、惯性导航系统观测模型、视觉观测模型和激光观测模型；
基于所述观测模型和所述多源数据，确定出所述导航定位参数真值；
基于所述导航定位参数真值，构建所述待定位对象的状态向量。
9.一种电子设备，其特征在于，包括存储器以及处理器，所述存储器用于存储支持处理器执行权利要求1至6任一项所述方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。
10.一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，其特征在于，计算机程序被处理器运行时执行上述权利要求1至6任一项所述方法的步骤。
CN202310208302.7A 2023-02-27 2023-02-27 一种融合多源数据的高精度导航定位方法及系统 Pending CN116242373A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310208302.7A CN116242373A (zh) 2023-02-27 2023-02-27 一种融合多源数据的高精度导航定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310208302.7A CN116242373A (zh) 2023-02-27 2023-02-27 一种融合多源数据的高精度导航定位方法及系统

Publications (1)

Publication Number Publication Date
CN116242373A true CN116242373A (zh) 2023-06-09

Family

ID=86627572

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310208302.7A Pending CN116242373A (zh) 2023-02-27 2023-02-27 一种融合多源数据的高精度导航定位方法及系统

Country Status (1)

Country Link
CN (1) CN116242373A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117606470A (zh) * 2024-01-24 2024-02-27 航天宏图信息技术股份有限公司 高精导航图线状要素智能自适追索生成方法、装置及设备

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117606470A (zh) * 2024-01-24 2024-02-27 航天宏图信息技术股份有限公司 高精导航图线状要素智能自适追索生成方法、装置及设备
CN117606470B (zh) * 2024-01-24 2024-04-16 航天宏图信息技术股份有限公司 高精导航图线状要素智能自适追索生成方法、装置及设备

Similar Documents

Publication Publication Date Title
JP6831414B2 (ja) 測位のための方法、測位のための装置、デバイス及びコンピュータ読み取り可能な記憶媒体
CN111121768B (zh) 一种机器人位姿估计方法、装置、可读存储介质及机器人
CN114018274B (zh) 车辆定位方法、装置及电子设备
CN112577521B (zh) 一种组合导航误差校准方法及电子设备
WO2022127532A1 (zh) 一种激光雷达与imu的外参标定方法、装置及设备
CN113701745B (zh) 一种外参变化检测方法、装置、电子设备及检测系统
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN116242373A (zh) 一种融合多源数据的高精度导航定位方法及系统
CN114111775A (zh) 一种多传感器融合定位方法、装置、存储介质及电子设备
CN116007620A (zh) 一种组合导航滤波方法、系统、电子设备及存储介质
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN113865586B (zh) 安装角度的估计方法、装置和自动驾驶系统
CN115560744A (zh) 机器人以及基于多传感器的三维建图方法、存储介质
CN113495281B (zh) 可移动平台的实时定位方法及装置
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN115900697A (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN115236714A (zh) 多源数据融合定位方法、装置、设备及计算机存储介质
CN117999207A (zh) 运动状态估计方法及装置
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN114019182A (zh) 零速状态检测方法、装置及电子设备
CN116718153B (zh) 一种基于gnss和ins的形变监测方法及系统
CN116045976A (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