CN113433576A - 一种gnss与v-slam融合定位方法及系统 - Google Patents

一种gnss与v-slam融合定位方法及系统 Download PDF

Info

Publication number
CN113433576A
CN113433576A CN202110721822.9A CN202110721822A CN113433576A CN 113433576 A CN113433576 A CN 113433576A CN 202110721822 A CN202110721822 A CN 202110721822A CN 113433576 A CN113433576 A CN 113433576A
Authority
CN
China
Prior art keywords
gnss
velocity
visual navigation
satellite
result
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
CN202110721822.9A
Other languages
English (en)
Other versions
CN113433576B (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.)
National Time Service Center of CAS
Original Assignee
National Time Service Center of CAS
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 National Time Service Center of CAS filed Critical National Time Service Center of CAS
Priority to CN202110721822.9A priority Critical patent/CN113433576B/zh
Publication of CN113433576A publication Critical patent/CN113433576A/zh
Application granted granted Critical
Publication of CN113433576B publication Critical patent/CN113433576B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种GNSS与V‑SLAM融合定位方法及系统。所述方法，包括：获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的目标载体的图像数据；基于GNSS定位方法解算GNSS伪距观测数据，得到GNSS定位结果；基于ORB‑SLAM2系统解算图像数据，得到视觉导航结果；采用卡尔曼滤波方法将GNSS伪距或GNSS定位结果和视觉导航结果进行融合，得到目标载体的位置。本发明能实现恶劣环境下的高精度、持续导航定位。

Description

一种GNSS与V-SLAM融合定位方法及系统
技术领域
本发明涉及导航定位领域，特别是涉及一种GNSS与V-SLAM融合定位方法及系统。
背景技术
全球卫星导航系统(GlobalNavigation Satellite System，GNSS)在开阔环境下能够实现连续的高精度导航定位功能，但是在遮挡、干扰和欺骗的环境下，GNSS的可用性、可靠性、连续性、精度都会受到严重影响。因此，如何解决弱GNSS环境下GNSS导航定位精度和连续性受到严重影响的问题，成为目前亟待解决的问题。
发明内容
基于此，本发明实施例提供一种GNSS与V-SLAM融合定位方法及系统，以实现恶劣环境下的高精度、持续导航定位。
为实现上述目的，本发明提供了如下方案：
一种GNSS与V-SLAM融合定位方法，包括：
获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据；
基于GNSS定位方法解算所述GNSS伪距观测数据，得到GNSS定位结果；所述GNSS定位结果包括GNSS位置和GNSS速度；
基于ORB-SLAM2系统解算所述图像数据，得到视觉导航结果；所述视觉导航结果包括视觉导航位置和视觉导航速度；
采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置；所述GNSS数据为所述GNSS伪距观测数据或所述GNSS定位结果。
可选的，所述采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置，具体包括：
判断卫星数目是否小于设定数目；
若是，则建立紧组合模型，并采用卡尔曼滤波方法对所述紧组合模型进行解算，得到目标载体的位置；
若否，则建立松组合模型或紧组合模型，并采用卡尔曼滤波方法对所述松组合模型或所述紧组合模型进行解算，得到目标载体的位置；
所述松组合模型的建立方法为：
根据所述GNSS定位结果和所述视觉导航结果建立松组合模型；
所述紧组合模型的建立方法为：
根据所述GNSS伪距观测数据计算所述卫星的伪距，根据所述视觉导航结果计算相对位置变化和速度误差，并基于所述伪距、所述相对位置变化和所述速度误差，建立紧组合模型。
可选的，所述松组合模型包括：松组合模型状态方程和松组合模型观测方程；
所述松组合模型状态方程为：
XLC,k+1＝ΦLC,k+1,kXLC,k+wk；
所述松组合模型观测方程为：
zLC,k+1＝Hk+1XLC,k+1+vk+1；
其中，XLC,k+1为tk+1时刻的松组合状态向量，XLC,k为tk时刻的松组合状态向量，ΦLC,k+1,k为从tk时刻到tk+1时刻的松组合状态转移矩阵，wk为过程噪声向量，zLC,k+1为tk+1时刻的松组合观测向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的松组合观测向量
Figure BDA0003137097450000021
其中，
Figure BDA0003137097450000022
为东方向的GNSS位置，
Figure BDA0003137097450000023
为北方向的GNSS位置，
Figure BDA0003137097450000031
为高程方向的GNSS位置，
Figure BDA0003137097450000032
为东方向的视觉导航位置，
Figure BDA0003137097450000033
为北方向的视觉导航位置，
Figure BDA0003137097450000034
为高程方向的视觉导航位置，
Figure BDA0003137097450000035
为东方向的GNSS速度，
Figure BDA0003137097450000036
为北方向的GNSS速度，
Figure BDA0003137097450000037
为高程方向的GNSS速度，
Figure BDA0003137097450000038
为东方向的视觉导航速度，
Figure BDA0003137097450000039
为北方向的视觉导航速度，
Figure BDA00031370974500000310
为高程方向的视觉导航速度。
可选的，所述紧组合模型包括：紧组合模型状态方程和紧组合模型观测方程；
所述紧组合模型状态方程为：
XTC,k+1＝ΦTC,k+1,kXTC,k+wk；
所述紧组合模型观测方程为：
δzTC,k+1＝Hk+1XTC,k+1+vk+1；
其中，XTC,k+1为tk+1时刻的紧组合状态向量，XTC,k为tk时刻的紧组合状态向量，ΦTC,k+1,k为从tk时刻到tk+1时刻的紧组合状态转移矩阵，δzTC,k+1为tk+1时刻的紧组合观测向量，wk为过程噪声向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的紧组合观测向量
Figure BDA00031370974500000311
其中，P1表示第1颗卫星的伪距观测值，
Figure BDA00031370974500000312
表示根据当前估计位置计算的第1颗卫星的伪距，P2表示第2颗卫星的伪距观测值，
Figure BDA00031370974500000313
表示根据当前估计位置计算的第2颗卫星的伪距，Pn表示第n颗卫星的伪距观测值，
Figure BDA00031370974500000314
表示根据当前估计位置计算的第n颗卫星的伪距，dt表示接收机钟差的等效距离，ΔrE表示导航坐标系下东方向前后帧的相对位置变化，ΔrN表示导航坐标系下北方向前后帧的相对位置变化，ΔrU表示导航坐标系下高程方向前后帧的相对位置变化，Δt为前后帧图像的时间间隔，δ表示误差，
Figure BDA0003137097450000041
表示估计的东方向的速度误差，
Figure BDA0003137097450000042
表示估计的北方向的速度误差，
Figure BDA0003137097450000043
表示估计高程方向的速度误差，
Figure BDA0003137097450000044
表示东方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure BDA0003137097450000045
表示北方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure BDA0003137097450000046
表示高程方向的GNSS速度和视觉导航速度的组合结果的估计值，δVE导航坐标系下的东方向的速度误差，δVN导航坐标系下的北方向的速度误差，δVU为导航坐标系下的高程方向的速度误差，VE表示东方向上的GNSS速度和视觉导航速度的组合结果，VN表示北方向上的GNSS速度和视觉导航速度的组合结果，VU表示高程方向上的GNSS速度和视觉导航速度的组合结果，L表示大地坐标系的经度，B表示大地坐标系的纬度，h表示大地坐标系的大地高，δL表示大地坐标系下的经度的误差，δB表示大地坐标系下的纬度的误差，δh表示大地坐标系下的大地高的误差，δdt为接收机钟差的等效距离的误差，e1表示接收机到第1颗卫星的单位余弦矩阵，e2表示接收机到第2颗卫星的单位余弦矩阵，en表示接收机到第n颗卫星的单位余弦矩阵，ε1为第1颗卫星的观测噪声，ε2为第2颗卫星的观测噪声，εn为第n颗卫星的观测噪声，εV为视觉导航结果的噪声。
可选的，在所述获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据之后，还包括：
获取视觉传感器标定数据；
采用所述视觉传感器标定数据对所述图像数据进行畸变校正，得到校正后的图像数据；
对所述GNSS伪距观测数据进行误差的修正，得到修正后的GNSS伪距观测数据；所述误差包括相对论误差、潮汐误差、地球自转误差、卫星钟差误差、对流层误差和电离层误差。
本发明还提供了一种GNSS与V-SLAM融合定位系统，包括：
数据获取模块，用于获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据；
第一解算模块，用于基于GNSS定位方法解算所述GNSS伪距观测数据，得到GNSS定位结果；所述GNSS定位结果包括GNSS位置和GNSS速度；
第二解算模块，用于基于ORB-SLAM2系统解算所述图像数据，得到视觉导航结果；所述视觉导航结果包括视觉导航位置和视觉导航速度；
融合模块，用于采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置；所述GNSS数据为所述GNSS伪距观测数据或所述GNSS定位结果。
可选的，所述融合模块，具体包括：
判断单元，用于判断卫星数目是否小于设定数目；
第一模型解算单元，用于若卫星数目小于设定数目，则建立紧组合模型，并采用卡尔曼滤波方法对所述紧组合模型进行解算，得到目标载体的位置；
第二模型解算单元，用于若卫星数目不小于设定数目，则建立松组合模型或紧组合模型，并采用卡尔曼滤波方法对所述松组合模型或所述紧组合模型进行解算，得到目标载体的位置；
所述松组合模型的建立方法为：
根据所述GNSS定位结果和所述视觉导航结果建立松组合模型；
所述紧组合模型的建立方法为：
根据所述GNSS伪距观测数据计算所述卫星的伪距，根据所述视觉导航结果计算相对位置变化和速度误差，并基于所述伪距、所述相对位置变化和所述速度误差，建立紧组合模型。
可选的，所述第二模型解算单元中的所述松组合模型包括：松组合模型状态方程和松组合模型观测方程；
所述松组合模型状态方程为：
XLC,k+1＝ΦLC,k+1,kXLC,k+wk；
所述松组合模型观测方程为：
zLC,k+1＝Hk+1XLC,k+1+vk+1；
其中，XLC,k+1为tk+1时刻的松组合状态向量，XLC,k为tk时刻的松组合状态向量，ΦLC,k+1,k为从tk时刻到tk+1时刻的松组合状态转移矩阵，wk为过程噪声向量，zLC,k+1为tk+1时刻的松组合观测向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的松组合观测向量
Figure BDA0003137097450000061
其中，
Figure BDA0003137097450000062
为东方向的GNSS位置，
Figure BDA0003137097450000063
为北方向的GNSS位置，
Figure BDA0003137097450000064
为高程方向的GNSS位置，
Figure BDA0003137097450000065
为东方向的视觉导航位置，
Figure BDA0003137097450000066
为北方向的视觉导航位置，
Figure BDA0003137097450000067
为高程方向的视觉导航位置，
Figure BDA0003137097450000068
为东方向的GNSS速度，
Figure BDA0003137097450000069
为北方向的GNSS速度，
Figure BDA00031370974500000610
为高程方向的GNSS速度，
Figure BDA00031370974500000611
为东方向的视觉导航速度，
Figure BDA00031370974500000612
为北方向的视觉导航速度，
Figure BDA00031370974500000613
为高程方向的视觉导航速度。
可选的，所述第一模型解算单元或所述第二模型解算单元中的所述紧组合模型包括：紧组合模型状态方程和紧组合模型观测方程；
所述紧组合模型状态方程为：
XTC,k+1＝ΦTC,k+1,kXTC,k+wk；
所述紧组合模型观测方程为：
δzTC,k+1＝Hk+1XTC,k+1+vk+1；
其中，XTC,k+1为tk+1时刻的紧组合状态向量，XTC,k为tk时刻的紧组合状态向量，ΦTC,k+1,k为从tk时刻到tk+1时刻的紧组合状态转移矩阵，δzTC,k+1为tk+1时刻的紧组合观测向量，wk为过程噪声向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的紧组合观测向量
Figure BDA0003137097450000071
其中，P1表示第1颗卫星的伪距观测值，
Figure BDA0003137097450000072
表示根据当前估计位置计算的第1颗卫星的伪距，P2表示第2颗卫星的伪距观测值，
Figure BDA0003137097450000073
表示根据当前估计位置计算的第2颗卫星的伪距，Pn表示第n颗卫星的伪距观测值，
Figure BDA0003137097450000074
表示根据当前估计位置计算的第n颗卫星的伪距，dt表示接收机钟差的等效距离，ΔrE表示导航坐标系下东方向前后帧的相对位置变化，ΔrN表示导航坐标系下北方向前后帧的相对位置变化，ΔrU表示导航坐标系下高程方向前后帧的相对位置变化，Δt为前后帧图像的时间间隔，δ表示误差，
Figure BDA0003137097450000075
表示估计的东方向的速度误差，
Figure BDA0003137097450000076
表示估计的北方向的速度误差，
Figure BDA0003137097450000077
表示估计高程方向的速度误差，
Figure BDA0003137097450000078
表示东方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure BDA0003137097450000079
表示北方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure BDA00031370974500000710
表示高程方向的GNSS速度和视觉导航速度的组合结果的估计值，δVE导航坐标系下的东方向的速度误差，δVN导航坐标系下的北方向的速度误差，δVU为导航坐标系下的高程方向的速度误差，VE表示东方向上的GNSS速度和视觉导航速度的组合结果，VN表示北方向上的GNSS速度和视觉导航速度的组合结果，VU表示高程方向上的GNSS速度和视觉导航速度的组合结果，L表示大地坐标系的经度，B表示大地坐标系的纬度，h表示大地坐标系的大地高，δL表示大地坐标系下的经度的误差，δB表示大地坐标系下的纬度的误差，δh表示大地坐标系下的大地高的误差，δdt为接收机钟差的等效距离的误差，e1表示接收机到第1颗卫星的单位余弦矩阵，e2表示接收机到第2颗卫星的单位余弦矩阵，en表示接收机到第n颗卫星的单位余弦矩阵，ε1为第1颗卫星的观测噪声，ε2为第2颗卫星的观测噪声，εn为第n颗卫星的观测噪声，εV为视觉导航结果的噪声。
可选的，所述GNSS与V-SLAM融合定位系统，还包括：校正模块；
所述校正模块，具体包括：
标定数据获取单元，用于获取视觉传感器标定数据；
第一校正单元，用于采用所述视觉传感器标定数据对所述图像数据进行畸变校正，得到校正后的图像数据；
第二校正单元，用于对所述GNSS伪距观测数据进行误差的修正，得到修正后的GNSS伪距观测数据；所述误差包括相对论误差、潮汐误差、地球自转误差、卫星钟差误差、对流层误差和电离层误差。
与现有技术相比，本发明的有益效果是：
本发明实施例提出了一种GNSS与V-SLAM融合定位方法及系统，基于GNSS定位方法解算GNSS伪距观测数据，得到GNSS定位结果；基于ORB-SLAM2系统解算图像数据，得到视觉导航结果；采用卡尔曼滤波方法将GNSS伪距观测数据或GNSS定位结果和视觉导航结果进行融合，得到目标载体的位置。V-SLAM技术能在特征丰富的复杂环境下实现连续的导航定位，但其定位误差大，GNSS技术定位精度高，但是不适用于恶劣环境，本申请将在恶劣环境下能实现连续导航定位的V-SLAM与高精度的GNSS融合，实现了恶劣环境下的高精度、持续导航定位。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的GNSS与V-SLAM融合定位方法的流程图；
图2为本发明实施例提供的GNSS与视觉松组合系统框架图；
图3为本发明实施例提供的GNSS与视觉紧组合系统框架图；
图4为本发明实施例提供的GNSS与V-SLAM融合定位系统的结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
为了保证导航定位功能的连续性，常采用GNSS与其他导航方式融合的方法进行导航定位。基于特征点法的视觉同步定位与建图(Visual Simultaneous Localization andMapping，V-SLAM)技术通过获取外部环境中的特征信息估计自身的位姿变化实现相对定位，V-SLAM技术能在特征丰富的复杂环境下实现连续的导航定位，但是定位误差会随着载体的运动距离不断积累，可知GNSS与V-SLAM技术具有优劣互补的特性。
因此，将GNSS与V-SLAM技术进行融合，对实现复杂环境下的持续导航定位、改善弱GNSS环境下的导航定位精度、促进智能驾驶和无人机等新兴邻域的发展等，具有重要的理论价值和实际意义。
为了克服弱GNSS环境下GNSS导航定位精度和连续性受到严重影响的问题，本实施例提供的GNSS与V-SLAM融合定位方法的大致思路如下：采用视觉传感器采集连续的图像数据，基于ORB-SLAM2(Oriented FAST and Rotated BRIEF SLAM)系统解算出载体的视觉导航结果，从而得到载体连续的位姿变化(位置变化、速度变化)；采用GNSS接收机采集卫星数据，基于GNSS定位方法解算得到的GNSS定位结果(位置速度结果)，通过与视觉导航结果融合，进行松组合或紧组合解算，实现载体在弱GNSS环境下连续的导航定位。
下面对GNSS与V-SLAM融合定位方法进行更为详细的介绍。
图1为本发明实施例提供的GNSS与V-SLAM融合定位方法的流程图。
参见图1，本实施例的GNSS与V-SLAM融合定位方法，包括：
步骤101：获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据。
该步骤还可以获取数据处理需要的辅助数据，包括广播星历和视觉传感器标定数据等。
步骤102：基于GNSS定位方法解算所述GNSS伪距观测数据，得到GNSS定位结果；所述GNSS定位结果包括GNSS位置和GNSS速度。具体的，基于广播星历，采用GNSS定位方法解算所述GNSS伪距观测数据。
在实际应用中，在执行步骤102之前，可以进行GNSS误差修正，然后再基于GNSS定位方法计算修正后的GNSS伪距观测数据，得到GNSS定位结果。GNSS误差修正具体为：对GNSS伪距观测数据进行相对论误差、潮汐误差、地球自转误差、卫星钟差误差、对流层误差和电离层误差等修正，得到修正后的GNSS伪距观测数据。
步骤103：基于ORB-SLAM2系统解算所述图像数据，得到视觉导航结果；所述视觉导航结果包括视觉导航位置和视觉导航速度。其中，ORB-SLAM2系统是开源的，其为现有的视觉解算平台。
在实际应用中，在执行步骤103之前，可以进行图像畸变校正，然后采用校正后的图像和对应相机配置文件，基于ORB-SLAM2系统进行视觉导航结果结算，得到视觉导航结果。图像畸变校正具体为：采用视觉传感器标定数据对图像数据进行畸变校正，得到校正后的图像。
步骤104：采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置；所述GNSS数据为所述GNSS伪距观测数据或所述GNSS定位结果。
步骤104，具体包括：
根据GNSS观测卫星数目确定融合定位的模型，并采用卡尔曼滤波方法对融合定位的模型进行解算，得到目标载体的位置。具体为：
判断卫星数目是否小于设定数目。
若卫星数目小于设定数目，则建立紧组合模型，并采用卡尔曼滤波方法对所述紧组合模型进行解算，得到目标载体的位置。GNSS/视觉紧组合系统框如图3所示，原始图像数据采用图像标定数据进行畸变校正后，使用ORB-SLAM2系统解算图像数据，紧组合模型中视觉导航结果更新频率高或由于GNSS信号发生中断会出现GNSS不可用的情况，因此需要进行GNSS可用性判断，若GNSS不可用，则采用视觉导航结果进行更新，若GNSS可用，则采用视觉导航结果和GNSS伪距观测数据进行联合观测更新。因此，在实际应用中，在采用卡尔曼滤波方法对紧组合模型进行解算时，首先，需要进行GNSS可用性的判断。
若卫星数目不小于设定数目，则建立松组合模型或紧组合模型，并采用卡尔曼滤波方法对所述松组合模型或所述紧组合模型进行解算，得到目标载体的位置。GNSS/视觉松组合系统框架如图2所示。
在实际应用中，所述设定数目可以为4，若卫星数目少于4颗，则只能建立紧组合模型，若卫星数大于或等于4颗，则可以建立松组合模型或紧组合模型。采用上述松组合方法和紧组合方法即为GNSS与V-SLAM融合定位方法。
所述松组合模型的建立方法为：
根据所述GNSS定位结果和所述视觉导航结果建立松组合模型。所述松组合模型包括：松组合模型状态方程和松组合模型观测方程。
所述松组合模型状态方程为：
XLC,k+1＝ΦLC,k+1,kXLC,k+wk；
所述松组合模型观测方程为：
zLC,k+1＝Hk+1XLC,k+1+vk+1；
其中，XLC,k+1为tk+1时刻的松组合状态向量，XLC,k为tk时刻的松组合状态向量，ΦLC,k+1,k为从tk时刻到tk+1时刻的松组合状态转移矩阵，wk为过程噪声向量，zLC,k+1为tk+1时刻的松组合观测向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的松组合观测向量
Figure BDA0003137097450000121
其中，
Figure BDA0003137097450000122
为东方向的GNSS位置，
Figure BDA0003137097450000123
为北方向的GNSS位置，
Figure BDA0003137097450000124
为高程方向的GNSS位置，
Figure BDA0003137097450000125
为东方向的视觉导航位置，
Figure BDA0003137097450000126
为北方向的视觉导航位置，
Figure BDA0003137097450000127
为高程方向的视觉导航位置，
Figure BDA0003137097450000128
为东方向的GNSS速度，
Figure BDA0003137097450000129
为北方向的GNSS速度，
Figure BDA00031370974500001210
为高程方向的GNSS速度，
Figure BDA00031370974500001211
为东方向的视觉导航速度，
Figure BDA00031370974500001212
为北方向的视觉导航速度，
Figure BDA00031370974500001213
为高程方向的视觉导航速度。
其中，XLC为松组合状态向量，XLC＝[δrE δrN δrU δVE δVN δVU]T，其中δrE、δrN、δrU分别表示东北高程方向上的位置误差，rE、rN、rU分别表示东北高程方向上的GNSS位置和视觉导航位置的组合结果，δVE、δVN、δVU分别表示东北高程方向上的速度误差，VE、VN、VU分别表示东北高程方向上的GNSS速度和视觉导航速度的组合结果；位置误差和速度误差均为待估参数。
Figure BDA00031370974500001214
Δt为前后帧图像的时间间隔；
Figure BDA00031370974500001215
I3×3和03×3分别表示3×3的单位矩阵和零矩阵。
所述紧组合模型的建立方法为：
根据所述GNSS伪距观测数据计算所述卫星的伪距，根据所述视觉导航结果计算相对位置变化和速度误差，并基于所述伪距、所述相对位置变化和所述速度误差，建立紧组合模型。所述紧组合模型包括：紧组合模型状态方程和紧组合模型观测方程。
所述紧组合模型状态方程为：
XTC,k+1＝ΦTC,k+1,kXTC,k+wk；
所述紧组合模型观测方程为：
δzTC,k+1＝Hk+1XTC,k+1+vk+1；
其中，XTC,k+1为tk+1时刻的紧组合状态向量，XTC,k为tk时刻的紧组合状态向量，ΦTC,k+1,k为从tk时刻到tk+1时刻的紧组合状态转移矩阵，δzTC,k+1为tk+1时刻的紧组合观测向量，wk为过程噪声向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的紧组合观测向量
Figure BDA0003137097450000131
其中，P1表示第1颗卫星的伪距观测值，
Figure BDA0003137097450000132
表示根据当前估计位置计算的第1颗卫星的伪距，P2表示第2颗卫星的伪距观测值，
Figure BDA0003137097450000133
表示根据当前估计位置计算的第2颗卫星的伪距，Pn表示第n颗卫星的伪距观测值，
Figure BDA0003137097450000134
表示第n颗卫星的伪距，
Figure BDA0003137097450000135
是当前估计位置(GNSS位置)和卫星位置之间的距离计算得到的两者之间的距离，dt表示接收机钟差的等效距离，dt是由接收机钟差乘于光速得到的，ΔrE、ΔrN和ΔrU分别表示导航坐标系下东方向、北方向和高程方向视觉导航结果前后帧的相对位置变化，
Figure BDA0003137097450000136
和
Figure BDA0003137097450000137
分别表示估计的东方向、北方向和高程方向的速度误差，
Figure BDA0003137097450000138
分别表示东方向、北方向和高程方向的GNSS速度和视觉导航速度的组合结果的估计值，δVE、δVN和δVU分别为导航坐标系下的东方向、北方向和高程方向的速度误差，e1表示接收机到第1颗卫星的单位余弦矩阵，e2表示接收机到第2颗卫星的单位余弦矩阵，en表示接收机到第n颗卫星的单位余弦矩阵，L、B和h分别表示大地坐标系的经度、纬度和大地高，en表示接收机到第n颗卫星的单位余弦矩阵，Δt为前后帧图像的时间间隔，δ表示相应参数的误差形式，δL、δB和δh分别为大地坐标系下的经度、纬度、大地高的误差，δdt为接收机钟差的等效距离的误差，δL、δB、δh、δVE、δVN、δVU、δdt均为待估参数，ε1为第1颗卫星的观测噪声，ε2为第2颗卫星的观测噪声，εn为第n颗卫星的观测噪声，εV为视觉导航结果的噪声。
本实施例提供的GNSS与V-SLAM融合定位方法，具有如下优点：
1、改善弱GNSS环境下的导航定位精度和导航定位的持续性。
在弱GNSS环境下，GNSS定位精度会严重下降，本实施例提出的GNSS与视觉紧组合方法，利用V-SLAM短期精度高的特性，能够有效改善弱GNSS环境下的导航定位精度，同时保证了弱GNSS环境下的导航定位的持续性。该方法在恶劣环境下采用GNSS/视觉松组合和紧组合方法，在恶劣环境下GNSS信号受到干扰、遮挡、卫星数少于4颗时，仍能实现持续导航定位功能，进一步增强了导航定位的持续性能。
2、基于用户级GNSS设备和消费级视觉传感器，大大降低成本。
本发明采用的数据采集设备为用户级GNSS设备和消费级视觉传感器，消费级视觉传感器成本只有几百元，大大降低了工程成本。
本发明还提供了一种GNSS与V-SLAM融合定位系统，图2为本发明实施例提供的GNSS与V-SLAM融合定位系统的结构图。
参见图2，本实施例的GNSS与V-SLAM融合定位系统包括：
数据获取模块201，用于获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据。
第一解算模块202，用于基于GNSS定位方法解算所述GNSS伪距观测数据，得到GNSS定位结果；所述GNSS定位结果包括GNSS位置和GNSS速度。
第二解算模块203，用于基于ORB-SLAM2系统解算所述图像数据，得到视觉导航结果；所述视觉导航结果包括视觉导航位置和视觉导航速度。
融合模块204，用于采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置；所述GNSS数据为所述GNSS伪距观测数据或所述GNSS定位结果。
作为一种可选的实施方式，所述融合模块204，具体包括：
判断单元，用于判断卫星数目是否小于设定数目。
第一模型解算单元，用于若卫星数目小于设定数目，则建立紧组合模型，并采用卡尔曼滤波方法对所述紧组合模型进行解算，得到目标载体的位置。
第二模型解算单元，用于若卫星数目不小于设定数目，则建立松组合模型或紧组合模型，并采用卡尔曼滤波方法对所述松组合模型或所述紧组合模型进行解算，得到目标载体的位置。
所述松组合模型的建立方法为：
根据所述GNSS定位结果和所述视觉导航结果建立松组合模型。
所述紧组合模型的建立方法为：
根据所述GNSS伪距观测数据计算所述卫星的伪距，根据所述视觉导航结果计算相对位置变化和速度误差，并基于所述伪距、所述相对位置变化和所述速度误差，建立紧组合模型。
作为一种可选的实施方式，所述第二模型解算单元中的所述松组合模型包括：松组合模型状态方程和松组合模型观测方程。
所述松组合模型状态方程为：
XLC,k+1＝ΦLC,k+1,kXLC,k+wk；
所述松组合模型观测方程为：
zLC,k+1＝Hk+1XLC,k+1+vk+1；
其中，XLC,k+1为tk+1时刻的松组合状态向量，XLC,k为tk时刻的松组合状态向量，ΦLC,k+1,k为从tk时刻到tk+1时刻的松组合状态转移矩阵，wk为过程噪声向量，zLC,k+1为tk+1时刻的松组合观测向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的松组合观测向量
Figure BDA0003137097450000161
其中，
Figure BDA0003137097450000162
为东方向的GNSS位置，
Figure BDA0003137097450000163
为北方向的GNSS位置，
Figure BDA0003137097450000164
为高程方向的GNSS位置，
Figure BDA0003137097450000165
为东方向的视觉导航位置，
Figure BDA0003137097450000166
为北方向的视觉导航位置，
Figure BDA0003137097450000167
为高程方向的视觉导航位置，
Figure BDA0003137097450000168
为东方向的GNSS速度，
Figure BDA0003137097450000169
为北方向的GNSS速度，
Figure BDA00031370974500001610
为高程方向的GNSS速度，
Figure BDA00031370974500001611
为东方向的视觉导航速度，
Figure BDA00031370974500001612
为北方向的视觉导航速度，
Figure BDA00031370974500001613
为高程方向的视觉导航速度。
作为一种可选的实施方式，所述第一模型解算单元或所述第二模型解算单元中的所述紧组合模型包括：紧组合模型状态方程和紧组合模型观测方程。
所述紧组合模型状态方程为：
XTC,k+1＝ΦTC,k+1,kXTC,k+wk；
所述紧组合模型观测方程为：
δzTC,k+1＝Hk+1XTC,k+1+vk+1；
其中，XTC,k+1为tk+1时刻的紧组合状态向量，XTC,k为tk时刻的紧组合状态向量，ΦTC,k+1,k为从tk时刻到tk+1时刻的紧组合状态转移矩阵，δzTC,k+1为tk+1时刻的紧组合观测向量，wk为过程噪声向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的紧组合观测向量
Figure BDA00031370974500001614
其中，P1表示第1颗卫星的伪距观测值，
Figure BDA0003137097450000171
表示根据当前估计位置计算的第1颗卫星的伪距，P2表示第2颗卫星的伪距观测值，
Figure BDA0003137097450000172
表示根据当前估计位置计算的第2颗卫星的伪距，Pn表示第n颗卫星的伪距观测值，
Figure BDA0003137097450000173
表示根据当前估计位置计算的第n颗卫星的伪距，dt表示接收机钟差的等效距离，ΔrE表示导航坐标系下东方向前后帧的相对位置变化，ΔrN表示导航坐标系下北方向前后帧的相对位置变化，ΔrU表示导航坐标系下高程方向前后帧的相对位置变化，Δt为前后帧图像的时间间隔，δ表示误差，
Figure BDA0003137097450000174
表示估计的东方向的速度误差，
Figure BDA0003137097450000175
表示估计的北方向的速度误差，
Figure BDA0003137097450000176
表示估计高程方向的速度误差，
Figure BDA0003137097450000177
表示东方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure BDA0003137097450000178
表示北方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure BDA0003137097450000179
表示高程方向的GNSS速度和视觉导航速度的组合结果的估计值，δVE导航坐标系下的东方向的速度误差，δVN导航坐标系下的北方向的速度误差，δVU为导航坐标系下的高程方向的速度误差，VE表示东方向上的GNSS速度和视觉导航速度的组合结果，VN表示北方向上的GNSS速度和视觉导航速度的组合结果，VU表示高程方向上的GNSS速度和视觉导航速度的组合结果，L表示大地坐标系的经度，B表示大地坐标系的纬度，h表示大地坐标系的大地高，δL表示大地坐标系下的经度的误差，δB表示大地坐标系下的纬度的误差，δh表示大地坐标系下的大地高的误差，δdt为接收机钟差的等效距离的误差，e1表示接收机到第1颗卫星的单位余弦矩阵，e2表示接收机到第2颗卫星的单位余弦矩阵，en表示接收机到第n颗卫星的单位余弦矩阵，ε1为第1颗卫星的观测噪声，ε2为第2颗卫星的观测噪声，εn为第n颗卫星的观测噪声，εV为视觉导航结果的噪声。
作为一种可选的实施方式，所述GNSS与V-SLAM融合定位系统，还包括：校正模块。
所述校正模块，具体包括：
标定数据获取单元，用于获取视觉传感器标定数据。
第一校正单元，用于采用所述视觉传感器标定数据对所述图像数据进行畸变校正，得到校正后的图像数据。
第二校正单元，用于对所述GNSS伪距观测数据进行误差的修正，得到修正后的GNSS伪距观测数据；所述误差包括相对论误差、潮汐误差、地球自转误差、卫星钟差误差、对流层误差和电离层误差。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种GNSS与V-SLAM融合定位方法，其特征在于，包括：
获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据；
基于GNSS定位方法解算所述GNSS伪距观测数据，得到GNSS定位结果；所述GNSS定位结果包括GNSS位置和GNSS速度；
基于ORB-SLAM2系统解算所述图像数据，得到视觉导航结果；所述视觉导航结果包括视觉导航位置和视觉导航速度；
采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置；所述GNSS数据为所述GNSS伪距观测数据或所述GNSS定位结果。
2.根据权利要求1所述的一种GNSS与V-SLAM融合定位方法，其特征在于，所述采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置，具体包括：
判断卫星数目是否小于设定数目；
若是，则建立紧组合模型，并采用卡尔曼滤波方法对所述紧组合模型进行解算，得到目标载体的位置；
若否，则建立松组合模型或紧组合模型，并采用卡尔曼滤波方法对所述松组合模型或所述紧组合模型进行解算，得到目标载体的位置；
所述松组合模型的建立方法为：
根据所述GNSS定位结果和所述视觉导航结果建立松组合模型；
所述紧组合模型的建立方法为：
根据所述GNSS伪距观测数据计算所述卫星的伪距，根据所述视觉导航结果计算相对位置变化和速度误差，并基于所述伪距、所述相对位置变化和所述速度误差，建立紧组合模型。
3.根据权利要求2所述的一种GNSS与V-SLAM融合定位方法，其特征在于，所述松组合模型包括：松组合模型状态方程和松组合模型观测方程；
所述松组合模型状态方程为：
XLC,k+1＝ΦLC,k+1,kXLC,k+wk；
所述松组合模型观测方程为：
zLC,k+1＝Hk+1XLC,k+1+vk+1；
其中，XLC,k+1为tk+1时刻的松组合状态向量，XLC,k为tk时刻的松组合状态向量，ΦLC,k+1,k为从tk时刻到tk+1时刻的松组合状态转移矩阵，wk为过程噪声向量，zLC,k+1为tk+1时刻的松组合观测向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的松组合观测向量
Figure FDA0003137097440000021
其中，
Figure FDA0003137097440000022
为东方向的GNSS位置，
Figure FDA0003137097440000023
为北方向的GNSS位置，
Figure FDA0003137097440000024
为高程方向的GNSS位置，
Figure FDA0003137097440000025
为东方向的视觉导航位置，
Figure FDA0003137097440000026
为北方向的视觉导航位置，
Figure FDA0003137097440000027
为高程方向的视觉导航位置，
Figure FDA0003137097440000028
为东方向的GNSS速度，
Figure FDA0003137097440000029
为北方向的GNSS速度，
Figure FDA00031370974400000210
为高程方向的GNSS速度，
Figure FDA00031370974400000211
为东方向的视觉导航速度，
Figure FDA00031370974400000212
为北方向的视觉导航速度，
Figure FDA00031370974400000213
为高程方向的视觉导航速度。
4.根据权利要求2所述的一种GNSS与V-SLAM融合定位方法，其特征在于，所述紧组合模型包括：紧组合模型状态方程和紧组合模型观测方程；
所述紧组合模型状态方程为：
XTC,k+1＝ΦTC,k+1,kXTC,k+wk；
所述紧组合模型观测方程为：
δzTC,k+1＝Hk+1XTC,k+1+vk+1；
其中，XTC,k+1为tk+1时刻的紧组合状态向量，XTC,k为tk时刻的紧组合状态向量，ΦTC,k+1,k为从tk时刻到tk+1时刻的紧组合状态转移矩阵，δzTC,k+1为tk+1时刻的紧组合观测向量，wk为过程噪声向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的紧组合观测向量
Figure FDA0003137097440000031
其中，P1表示第1颗卫星的伪距观测值，
Figure FDA0003137097440000032
表示根据当前估计位置计算的第1颗卫星的伪距，P2表示第2颗卫星的伪距观测值，
Figure FDA0003137097440000033
表示根据当前估计位置计算的第2颗卫星的伪距，Pn表示第n颗卫星的伪距观测值，
Figure FDA0003137097440000034
表示根据当前估计位置计算的第n颗卫星的伪距，dt表示接收机钟差的等效距离，ΔrE表示导航坐标系下东方向前后帧的相对位置变化，ΔrN表示导航坐标系下北方向前后帧的相对位置变化，ΔrU表示导航坐标系下高程方向前后帧的相对位置变化，Δt为前后帧图像的时间间隔，δ表示误差，
Figure FDA0003137097440000035
表示估计的东方向的速度误差，
Figure FDA0003137097440000036
表示估计的北方向的速度误差，
Figure FDA0003137097440000037
表示估计高程方向的速度误差，
Figure FDA0003137097440000038
表示东方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure FDA0003137097440000039
表示北方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure FDA00031370974400000310
表示高程方向的GNSS速度和视觉导航速度的组合结果的估计值，δVE导航坐标系下的东方向的速度误差，δVN导航坐标系下的北方向的速度误差，δVU为导航坐标系下的高程方向的速度误差，VE表示东方向上的GNSS速度和视觉导航速度的组合结果，VN表示北方向上的GNSS速度和视觉导航速度的组合结果，VU表示高程方向上的GNSS速度和视觉导航速度的组合结果，L表示大地坐标系的经度，B表示大地坐标系的纬度，h表示大地坐标系的大地高，δL表示大地坐标系下的经度的误差，δB表示大地坐标系下的纬度的误差，δh表示大地坐标系下的大地高的误差，δdt为接收机钟差的等效距离的误差，e1表示接收机到第1颗卫星的单位余弦矩阵，e2表示接收机到第2颗卫星的单位余弦矩阵，en表示接收机到第n颗卫星的单位余弦矩阵，ε1为第1颗卫星的观测噪声，ε2为第2颗卫星的观测噪声，εn为第n颗卫星的观测噪声，εV为视觉导航结果的噪声。
5.根据权利要求1所述的一种GNSS与V-SLAM融合定位方法，其特征在于，在所述获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据之后，还包括：
获取视觉传感器标定数据；
采用所述视觉传感器标定数据对所述图像数据进行畸变校正，得到校正后的图像数据；
对所述GNSS伪距观测数据进行误差的修正，得到修正后的GNSS伪距观测数据；所述误差包括相对论误差、潮汐误差、地球自转误差、卫星钟差误差、对流层误差和电离层误差。
6.一种GNSS与V-SLAM融合定位系统，其特征在于，包括：
数据获取模块，用于获取接收机采集的目标载体的GNSS伪距观测数据和视觉传感器采集的所述目标载体的图像数据；
第一解算模块，用于基于GNSS定位方法解算所述GNSS伪距观测数据，得到GNSS定位结果；所述GNSS定位结果包括GNSS位置和GNSS速度；
第二解算模块，用于基于ORB-SLAM2系统解算所述图像数据，得到视觉导航结果；所述视觉导航结果包括视觉导航位置和视觉导航速度；
融合模块，用于采用卡尔曼滤波方法将GNSS数据和所述视觉导航结果进行融合，得到所述目标载体的位置；所述GNSS数据为所述GNSS伪距观测数据或所述GNSS定位结果。
7.根据权利要求6所述的一种GNSS与V-SLAM融合定位系统，其特征在于，所述融合模块，具体包括：
判断单元，用于判断卫星数目是否小于设定数目；
第一模型解算单元，用于若卫星数目小于设定数目，则建立紧组合模型，并采用卡尔曼滤波方法对所述紧组合模型进行解算，得到目标载体的位置；
第二模型解算单元，用于若卫星数目不小于设定数目，则建立松组合模型或紧组合模型，并采用卡尔曼滤波方法对所述松组合模型或所述紧组合模型进行解算，得到目标载体的位置；
所述松组合模型的建立方法为：
根据所述GNSS定位结果和所述视觉导航结果建立松组合模型；
所述紧组合模型的建立方法为：
根据所述GNSS伪距观测数据计算所述卫星的伪距，根据所述视觉导航结果计算相对位置变化和速度误差，并基于所述伪距、所述相对位置变化和所述速度误差，建立紧组合模型。
8.根据权利要求7所述的一种GNSS与V-SLAM融合定位系统，其特征在于，所述第二模型解算单元中的所述松组合模型包括：松组合模型状态方程和松组合模型观测方程；
所述松组合模型状态方程为：
XLC,k+1＝ΦLC,k+1,kXLC,k+wk；
所述松组合模型观测方程为：
zLC,k+1＝Hk+1XLC,k+1+vk+1；
其中，XLC,k+1为tk+1时刻的松组合状态向量，XLC,k为tk时刻的松组合状态向量，ΦLC,k+1,k为从tk时刻到tk+1时刻的松组合状态转移矩阵，wk为过程噪声向量，zLC,k+1为tk+1时刻的松组合观测向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的松组合观测向量
Figure FDA0003137097440000051
其中，
Figure FDA0003137097440000052
为东方向的GNSS位置，
Figure FDA0003137097440000053
为北方向的GNSS位置，
Figure FDA0003137097440000054
为高程方向的GNSS位置，
Figure FDA0003137097440000055
为东方向的视觉导航位置，
Figure FDA0003137097440000056
为北方向的视觉导航位置，
Figure FDA0003137097440000057
为高程方向的视觉导航位置，
Figure FDA0003137097440000058
为东方向的GNSS速度，
Figure FDA0003137097440000059
为北方向的GNSS速度，
Figure FDA00031370974400000510
为高程方向的GNSS速度，
Figure FDA00031370974400000511
为东方向的视觉导航速度，
Figure FDA0003137097440000061
为北方向的视觉导航速度，
Figure FDA0003137097440000062
为高程方向的视觉导航速度。
9.根据权利要求7所述的一种GNSS与V-SLAM融合定位系统，其特征在于，所述第一模型解算单元或所述第二模型解算单元中的所述紧组合模型包括：紧组合模型状态方程和紧组合模型观测方程；
所述紧组合模型状态方程为：
XTC,k+1＝ΦTC,k+1,kXTC,k+wk；
所述紧组合模型观测方程为：
δzTC,k+1＝Hk+1XTC,k+1+vk+1；
其中，XTC,k+1为tk+1时刻的紧组合状态向量，XTC,k为tk时刻的紧组合状态向量，ΦTC,k+1,k为从tk时刻到tk+1时刻的紧组合状态转移矩阵，δzTC,k+1为tk+1时刻的紧组合观测向量，wk为过程噪声向量，Hk+1为tk+1时刻的松组合设计矩阵，vk+1为观测噪声向量，任意时刻的紧组合观测向量
Figure FDA0003137097440000063
其中，P1表示第1颗卫星的伪距观测值，
Figure FDA0003137097440000064
表示根据当前估计位置计算的第1颗卫星的伪距，P2表示第2颗卫星的伪距观测值，
Figure FDA0003137097440000065
表示根据当前估计位置计算的第2颗卫星的伪距，Pn表示第n颗卫星的伪距观测值，
Figure FDA0003137097440000066
表示根据当前估计位置计算的第n颗卫星的伪距，dt表示接收机钟差的等效距离，ΔrE表示导航坐标系下东方向前后帧的相对位置变化，ΔrN表示导航坐标系下北方向前后帧的相对位置变化，ΔrU表示导航坐标系下高程方向前后帧的相对位置变化，Δt为前后帧图像的时间间隔，δ表示误差，
Figure FDA0003137097440000067
表示估计的东方向的速度误差，
Figure FDA0003137097440000071
表示估计的北方向的速度误差，
Figure FDA0003137097440000072
表示估计高程方向的速度误差，
Figure FDA0003137097440000073
表示东方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure FDA0003137097440000074
表示北方向的GNSS速度和视觉导航速度的组合结果的估计值，
Figure FDA0003137097440000075
表示高程方向的GNSS速度和视觉导航速度的组合结果的估计值，δVE导航坐标系下的东方向的速度误差，δVN导航坐标系下的北方向的速度误差，δVU为导航坐标系下的高程方向的速度误差，VE表示东方向上的GNSS速度和视觉导航速度的组合结果，VN表示北方向上的GNSS速度和视觉导航速度的组合结果，VU表示高程方向上的GNSS速度和视觉导航速度的组合结果，L表示大地坐标系的经度，B表示大地坐标系的纬度，h表示大地坐标系的大地高，δL表示大地坐标系下的经度的误差，δB表示大地坐标系下的纬度的误差，δh表示大地坐标系下的大地高的误差，δdt为接收机钟差的等效距离的误差，e1表示接收机到第1颗卫星的单位余弦矩阵，e2表示接收机到第2颗卫星的单位余弦矩阵，en表示接收机到第n颗卫星的单位余弦矩阵，ε1为第1颗卫星的观测噪声，ε2为第2颗卫星的观测噪声，εn为第n颗卫星的观测噪声，εV为视觉导航结果的噪声。
10.根据权利要求6所述的一种GNSS与V-SLAM融合定位系统，其特征在于，还包括：校正模块；
所述校正模块，具体包括：
标定数据获取单元，用于获取视觉传感器标定数据；
第一校正单元，用于采用所述视觉传感器标定数据对所述图像数据进行畸变校正，得到校正后的图像数据；
第二校正单元，用于对所述GNSS伪距观测数据进行误差的修正，得到修正后的GNSS伪距观测数据；所述误差包括相对论误差、潮汐误差、地球自转误差、卫星钟差误差、对流层误差和电离层误差。
CN202110721822.9A 2021-06-28 2021-06-28 一种gnss与v-slam融合定位方法及系统 Active CN113433576B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110721822.9A CN113433576B (zh) 2021-06-28 2021-06-28 一种gnss与v-slam融合定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110721822.9A CN113433576B (zh) 2021-06-28 2021-06-28 一种gnss与v-slam融合定位方法及系统

Publications (2)

Publication Number Publication Date
CN113433576A true CN113433576A (zh) 2021-09-24
CN113433576B CN113433576B (zh) 2023-09-01

Family

ID=77755143

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110721822.9A Active CN113433576B (zh) 2021-06-28 2021-06-28 一种gnss与v-slam融合定位方法及系统

Country Status (1)

Country Link
CN (1) CN113433576B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114928881A (zh) * 2022-04-18 2022-08-19 上海大学 基于超宽带和视觉智能装置的协同定位系统及定位方法
WO2023185215A1 (zh) * 2022-04-02 2023-10-05 北京三快在线科技有限公司 数据校准

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110412635A (zh) * 2019-07-22 2019-11-05 武汉大学 一种环境信标支持下的gnss/sins/视觉紧组合方法
CN111505685A (zh) * 2020-04-15 2020-08-07 中国科学院国家授时中心 一种基于改正系统间偏差的多系统组合rtk模型的定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110412635A (zh) * 2019-07-22 2019-11-05 武汉大学 一种环境信标支持下的gnss/sins/视觉紧组合方法
CN111505685A (zh) * 2020-04-15 2020-08-07 中国科学院国家授时中心 一种基于改正系统间偏差的多系统组合rtk模型的定位方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
冯?;涂锐;韩军强;侯福荣;洪菊;刘金海;王星星;: "双目视觉辅助GNSS在恶劣环境下导航定位", 全球定位系统, no. 03, pages 48 - 50 *
冯祎等: "双目视觉辅助ＧＮＳＳ在恶劣环境下导航定位", 全球定位系统, vol. 45, no. 3, pages 48 - 50 *
曾庆喜;邱文旗;冯玉朋;刘德辉;贾丰源;: "GNSS/VO组合导航研究现状及发展趋势", 导航定位学报, no. 02 *
李帅鑫;李广云;周阳林;李明磊;王力;: "改进的单目视觉实时定位与测图方法", 仪器仪表学报, no. 11 *
黎蓉;: "卡尔曼滤波在组合导航数据处理中的应用", 电子测量技术, no. 03 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023185215A1 (zh) * 2022-04-02 2023-10-05 北京三快在线科技有限公司 数据校准
CN114928881A (zh) * 2022-04-18 2022-08-19 上海大学 基于超宽带和视觉智能装置的协同定位系统及定位方法
CN114928881B (zh) * 2022-04-18 2024-05-14 上海大学 基于超宽带和视觉智能装置的协同定位系统及定位方法

Also Published As

Publication number Publication date
CN113433576B (zh) 2023-09-01

Similar Documents

Publication Publication Date Title
CN108873038B (zh) 自主泊车定位方法及定位系统
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN108594283B (zh) Gnss/mems惯性组合导航系统的自由安装方法
CN109059751B (zh) 一种形变数据监测方法及系统
CN109471146B (zh) 一种基于ls-svm的自适应容错gps/ins组合导航方法
CN109471144B (zh) 基于伪距/伪距率的多传感器紧组合列车组合定位方法
CN110988951A (zh) 多源数据融合实时导航定位方法及系统
JP3875714B2 (ja) 移動体加速度・距離推定回路、移動体測位装置及び移動体測位方法
CN113433576B (zh) 一种gnss与v-slam融合定位方法及系统
Li et al. Review of PPP–RTK: Achievements, challenges, and opportunities
CN111913201B (zh) 一种gnss差分定位方法、装置及计算机可读存储介质
CN109507706B (zh) 一种gps信号丢失的预测定位方法
WO2020124624A1 (zh) 一种紧耦合的自动驾驶感知方法及系统
Sun et al. An adaptive weighting strategy for multi sensor integrated navigation in urban areas
CN113447971A (zh) 一种leo与gnss融合精密单点定位方法及系统
JP2005195395A (ja) 移動体加速度・距離推定回路、測位航法用擬似距離推定回路、移動体測位装置及び移動体測位方法
CN114396943A (zh) 一种融合定位方法与终端
CN115683094A (zh) 一种复杂环境下车载双天线紧耦合定位方法及系统
CN113419265B (zh) 一种基于多传感器融合的定位方法、装置与电子设备
CN116753948A (zh) 一种基于视觉惯性gnss ppp耦合的定位方法
CN115900732A (zh) 基于路侧相机和车载单元的组合导航方法及系统
CN112229401B (zh) 适用于ins-gps伪距融合的量测信息同步外推方法及系统
CN115097508A (zh) 一种带有多径误差估计器的卫星/惯性深耦合方法
CN116380119A (zh) 组合导航的校准方法、装置和系统
CN113031040A (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