CN115523920B - 一种基于视觉惯性gnss紧耦合的无缝定位方法 - Google Patents

一种基于视觉惯性gnss紧耦合的无缝定位方法 Download PDF

Info

Publication number
CN115523920B
CN115523920B CN202211512861.9A CN202211512861A CN115523920B CN 115523920 B CN115523920 B CN 115523920B CN 202211512861 A CN202211512861 A CN 202211512861A CN 115523920 B CN115523920 B CN 115523920B
Authority
CN
China
Prior art keywords
gnss
information
residual
carrier
visual
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
CN202211512861.9A
Other languages
English (en)
Other versions
CN115523920A (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202211512861.9A priority Critical patent/CN115523920B/zh
Publication of CN115523920A publication Critical patent/CN115523920A/zh
Application granted granted Critical
Publication of CN115523920B publication Critical patent/CN115523920B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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
    • 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
    • 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
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • 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)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明涉及一种基于视觉惯性GNSS紧耦合的无缝定位方法，使用SPP算法计算载体初始绝对位置，使用多普勒频移与视觉惯性系统解算出的载体速度对齐，获得载体在ENU坐标系下的初始位置、姿态和速度。分别计算IMU残差、视觉重投影误差、伪距残差、多普勒频移残差和接收机时钟残差，将其作为因子加入到优化项中，优化求解获得载体的绝对位姿。提出一种针对GNSS拒止环境的状态切换策略，在无可用的GNSS信息时，锁定与GNSS相关的状态量并在因子图中隔离伪距残差因子、多普勒频移残差因子和接收机时钟残差因子，系统退化为视觉惯性融合的定位方法。此方法在GNSS信号弱的拒止环境下能够获得可靠精确的绝对位姿，并能在进出拒止环境时进行状态的无缝切换。

Description

一种基于视觉惯性GNSS紧耦合的无缝定位方法
技术领域
本发明属于制导技术领域，涉及一种视觉惯导GNSS融合定位方法，具体涉及一种基于视觉惯性GNSS紧耦合的无缝定位方法。
背景技术
同时定位与建图（Simultaneous Localization and Mapping, SLAM）是智能自主机器人独立完成任务目标的核心技术，在近年来被广泛研究并已被广泛应用于微型无人机、智能驾驶、虚拟现实以及增强现实等多个领域。
随着近年来各方的研究与开源，SLAM技术从最初的仅视觉、仅激光雷达的定位方法发展至能克服单一传感器缺点的多传感器方法。其中视觉惯性的融合方法因其重量轻、成本低的优点，成为最主流的方法。但视觉惯性融合系统因为其传感器的局限性，只能通过获取周遭环境与载体自身的信息，解算得到载体自身的相对位姿，无法得到精确的绝对位置，并且定位系统会随着时间的推移误差逐渐增大。因此引入GNSS卫星导航信息进行多传感器融合，获取载体的绝对位姿信息。但在实际应用环境中，GNSS信号容易受到遮挡、屏蔽或干扰，并不能保证接收机随时都能接收到稳定的GNSS信息，导致定位中断停滞或产生很大的漂移。
发明内容
要解决的技术问题
为了解决GNSS信号丢失而导致定位中断停滞或产生很大的漂移的问题，本发明提供一种基于视觉惯性GNSS紧耦合的实现在GNSS拒止环境与非拒止环境下无缝衔接的定位方法。
技术方案
一种基于视觉惯性GNSS紧耦合的无缝定位方法，其特征在于：实现视觉惯性GNSS紧耦合并在进入GNSS拒止环境仍能进行无缝的绝对位置的获取；步骤如下：
步骤1：对IMU信息进行预积分处理，对视觉图像信息进行特征点的提取与跟踪，使用星历信息对GNSS信息进行可靠性筛选，得到可用的传感器信息；
步骤2：进行初始化，使用仅视觉的三维运动重建获得载体的相对于初始位置的初始相对位姿，进行视觉IMU联合初始化得到IMU加速度计与陀螺仪的偏置以及重力矢量的对齐；利用SPP算法通过至少四颗以上的卫星的伪距信息计算出载体粗略的绝对位置，利用多普勒频移与视觉惯性系统得到的相对速度对齐，进行视觉惯性系统与GNSS信息的联合初始化得到载体在ENU坐标系下的绝对位姿；
步骤3：计算视觉重投影误差、IMU预积分残差和GNSS信息的伪距残差、多普勒频移残差、接收机时钟残差并分别作为因子构建因子图，构建滑动窗口对其中所有的状态量
Figure 956341DEST_PATH_IMAGE001
进行优化求解，其中n为滑动窗口中的帧数，m为滑动窗口中所有特征点的数量
Figure 670219DEST_PATH_IMAGE002
，
Figure 495961DEST_PATH_IMAGE003
分别为载体的位置、速度和姿态，
Figure 687908DEST_PATH_IMAGE004
为加速度计和陀螺仪的偏置，
Figure 33439DEST_PATH_IMAGE005
为接收机时钟钟差，
Figure 668951DEST_PATH_IMAGE006
为接收机时钟钟差变化率，
Figure 998301DEST_PATH_IMAGE007
为相机外参，
Figure 508786DEST_PATH_IMAGE008
为特征点的逆深度，
Figure 239981DEST_PATH_IMAGE009
为载体坐标系和ENU坐标系之间的偏航角；通过星历信息对卫星GNSS信息进行筛选，当无可用的GNSS信息时，锁定与GNSS相关的状态量并在因子图中隔离GNSS信息的伪距残差因子、多普勒频移残差因子和接收机时钟残差因子，切换到视觉惯性定位系统解算载体相对位姿并根据有可用GNSS信息时的先验信息推算ENU坐标系下的绝对位姿，待重新接收到可用得GNSS信息时，再对GNSS相关的状态量进行解锁，并加入伪距残差因子、多普勒频移残差因子和接收机时钟残差因子进行联合优化，令偏航角
Figure 780815DEST_PATH_IMAGE010
、接收机时钟钟差
Figure 597462DEST_PATH_IMAGE011
、接收机时钟钟差变化率
Figure 380479DEST_PATH_IMAGE012
继续被各优化项优化更新。
本发明进一步的技术方案：步骤3所述的伪距残差因子：
Figure 700602DEST_PATH_IMAGE013
其中
Figure 474654DEST_PATH_IMAGE014
为地球自转角速度，
Figure 513017DEST_PATH_IMAGE015
为卫星信号传输时间，
Figure 319299DEST_PATH_IMAGE016
为地球在卫星信号传输过程中的旋转，
Figure 14634DEST_PATH_IMAGE017
为接收机接收到卫星信号时卫星在ECEF坐标系下的实际位置，
Figure 880959DEST_PATH_IMAGE018
表示接收机的位置，c代表光速，
Figure 406618DEST_PATH_IMAGE019
为接收机时钟钟差，
Figure 32903DEST_PATH_IMAGE020
为卫星时钟误差，
Figure 62039DEST_PATH_IMAGE021
和
Figure 630423DEST_PATH_IMAGE022
分别代表对流层和电流层带来的延迟误差，
Figure 892646DEST_PATH_IMAGE023
为伪距的量测值。
本发明进一步的技术方案：步骤3所述的多普勒频移残差因子：
Figure 837469DEST_PATH_IMAGE024
其中
Figure 3002DEST_PATH_IMAGE025
表示载波信号的波长，
Figure 742288DEST_PATH_IMAGE026
表示从接收机到卫星的单位向量，
Figure 226228DEST_PATH_IMAGE027
和
Figure 443582DEST_PATH_IMAGE028
分别表示卫星和接收机的速度，
Figure 447310DEST_PATH_IMAGE029
表示接收机时钟钟差变化率，
Figure 842651DEST_PATH_IMAGE030
表示卫星时钟误差漂移率，
Figure 564619DEST_PATH_IMAGE031
为多普勒频移量测值。
本发明进一步的技术方案：步骤3所述的接收机时钟残差因子：
Figure 585665DEST_PATH_IMAGE032
其中
Figure 693167DEST_PATH_IMAGE033
和
Figure 508676DEST_PATH_IMAGE034
分别为第k时刻和第k-1时刻的接收机时钟钟差，
Figure 983520DEST_PATH_IMAGE035
为第k-1时刻的接收机时钟钟差变化率，
Figure 293410DEST_PATH_IMAGE036
为第k时刻和第k-1时刻的时间差。
本发明进一步的技术方案：步骤3所述的联合优化具体如下：
将残差项因子加入到优化项中，建立优化模型：
Figure 271730DEST_PATH_IMAGE037
其中
Figure 992561DEST_PATH_IMAGE038
表示的是z里面的第i项，z表示所有n个传感器量测信息的集合，r表示残差项；问题被建模成非线性最小二乘问题，使用梯度下降法对该优化模型进行求解，获得载体的位姿信息。
一种计算机系统，其特征在于包括：一个或多个处理器，计算机可读存储介质，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述的方法。
一种计算机可读存储介质，其特征在于存储有计算机可执行指令，所述指令在被执行时用于实现上述的方法。
有益效果
本发明提供的一种基于视觉惯性GNSS紧耦合的无缝定位方法，对载体在实际场景中进出GNSS拒止环境时的状态切换进行研究，在进入拒止环境无可用的GNSS信息时，锁定与GNSS相关的状态量偏航角
Figure 932530DEST_PATH_IMAGE039
、接收机时钟钟差
Figure 295378DEST_PATH_IMAGE040
和接收机时钟钟差变化率
Figure 128205DEST_PATH_IMAGE041
并在因子图中隔离GNSS信息的伪距残差因子、多普勒频移残差因子和接收机时钟残差因子，系统退化为视觉惯性融合的定位方法并利用偏航角推算载体的绝对位置，实现了载体进出拒止环境绝对定位的无缝衔接。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1基于视觉惯性GNSS紧耦合的无缝定位方法流程图；
图2 ECEF坐标系、ENU坐标系和Local坐标系关系图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
图1阐述了本发明的思路：首先对相机、IMU和GNSS获得的信息进行预处理，对相机图像进行特征提取与追踪，对惯性IMU数据进行预积分，对GNSS信息进行可靠性筛选；然后使用图像信息进行仅视觉的三维运动重建，后与IMU信息进行联合初始化；使用SPP方法计算初始绝对位置并使用多普勒频移与视觉惯性系统获得的速度信息对齐，完成三传感器联合初始化；在有可靠的GNSS信息时，进行三传感器联合优化求解载体位姿，在无可靠的GNSS信息时，系统退化为视觉惯性定位进行解算。
具体过程：
1、对惯性测量单元IMU信息进行预积分处理，对视觉图像信息进行特征点的提取与跟踪，使用星历信息对GNSS信息进行可靠性筛选，得到可用的传感器信息；
从GNSS接收机获取GNSS量测信息后，会对其进行筛选，首先会剔除还未收到其对应星历信息的量测信息，之后通过伪距标准差和多普勒频移标准差对不可靠的GNSS信息剔除（标准差超过设定阈值则剔除），最后利用接收机与卫星的仰角大小，去掉受到地球电离层和对流层影响较大的量测信息（仰角小于30度则剔除），剩余的可靠的量测信息用于初始化和优化。
2、进行初始化，使用仅视觉的三维运动重建获得载体的相对于初始位置的初始相对位姿，进行视觉IMU联合初始化得到IMU加速度计与陀螺仪的偏置以及重力矢量的对齐。
使用SPP（单点定位）算法，利用伪距和三角测距原理，计算获得接收机的粗略坐标，由于存在时钟钟差且不同GNSS系统的时钟钟差不同，因此需要至少N+3颗卫星的伪距信息才能计算出载体的绝对位置和时钟偏差，其中N为GNSS系统（GPS、北斗、GLONASS、Gialleo等）的数量，利用多普勒频移与视觉惯性系统得到的相对速度对齐，计算获得ENU坐标系与Local（局部导航坐标系）系的偏航角
Figure 770670DEST_PATH_IMAGE042
；利用计算获得的绝对坐标和偏航角
Figure 954527DEST_PATH_IMAGE043
获得ECEF坐标系（地心地固坐标系）、ENU坐标系（东北天坐标系）以及Local坐标系的转换关系，如图2所示，ENU系到ECEF系的转换为：
Figure 121066DEST_PATH_IMAGE044
其中
Figure 792087DEST_PATH_IMAGE045
为纬度，
Figure 385880DEST_PATH_IMAGE046
为经度，Local坐标系到ENU坐标系的转换为：
Figure 791453DEST_PATH_IMAGE047
3、通过跟踪到的特征点计算视觉重投影误差、IMU预积分残差和GNSS信息的伪距残差、多普勒频移残差、接收机时钟残差并分别作为五个因子构建因子图，构建滑动窗口对其中所有的状态量
Figure 246837DEST_PATH_IMAGE048
进行优化求解，其中n为滑动窗口中的帧数，m为滑动窗口中所有特征点的数量
Figure 54256DEST_PATH_IMAGE049
，
Figure 553370DEST_PATH_IMAGE050
分别为载体的位置、速度和姿态，
Figure 695507DEST_PATH_IMAGE051
为加速度计和陀螺仪的偏置，
Figure 938270DEST_PATH_IMAGE052
为接收机时钟钟差，
Figure 600195DEST_PATH_IMAGE053
为接收机时钟钟差变化率，
Figure 286523DEST_PATH_IMAGE055
为相机外参，
Figure 401109DEST_PATH_IMAGE056
为特征点的逆深度，
Figure 447563DEST_PATH_IMAGE057
为载体坐标系和ENU坐标系之间的偏航角。
建立伪距残差因子：
Figure 478841DEST_PATH_IMAGE058
其中
Figure 319759DEST_PATH_IMAGE059
为地球自转角速度，
Figure 921641DEST_PATH_IMAGE060
为卫星信号传输时间，
Figure 771785DEST_PATH_IMAGE061
为地球在卫星信号传输过程中的旋转，
Figure 159036DEST_PATH_IMAGE062
为接收机接收到卫星信号时卫星在ECEF坐标系下的实际位置，
Figure 436433DEST_PATH_IMAGE063
表示接收机的位置，c代表光速，
Figure 780739DEST_PATH_IMAGE064
为接收机时钟钟差，
Figure 168995DEST_PATH_IMAGE065
为卫星时钟误差，
Figure 660019DEST_PATH_IMAGE066
和
Figure 842739DEST_PATH_IMAGE067
分别代表对流层和电流层带来的延迟误差，
Figure 169946DEST_PATH_IMAGE068
为伪距的量测值。
建立多普勒频移残差因子：
Figure 627472DEST_PATH_IMAGE069
其中
Figure 707424DEST_PATH_IMAGE070
表示载波信号的波长，
Figure 841471DEST_PATH_IMAGE071
表示从接收机到卫星的单位向量，
Figure 639662DEST_PATH_IMAGE072
和
Figure 635300DEST_PATH_IMAGE073
分别表示卫星和接收机的速度，
Figure 586070DEST_PATH_IMAGE074
表示接收机时钟钟差变化率，
Figure 687756DEST_PATH_IMAGE075
表示卫星时钟误差漂移率，
Figure 504402DEST_PATH_IMAGE076
为多普勒频移量测值。
建立接收机时钟残差因子：
Figure 54463DEST_PATH_IMAGE077
其中
Figure 109007DEST_PATH_IMAGE078
和
Figure 335589DEST_PATH_IMAGE079
分别为第k时刻和第k-1时刻的接收机时钟钟差，
Figure 357640DEST_PATH_IMAGE080
为第k-1时刻的接收机时钟钟差变化率，
Figure 429502DEST_PATH_IMAGE081
为第k时刻和第k-1时刻的时间差。
将这五个因子加入到优化项中，建立优化模型：
Figure 604131DEST_PATH_IMAGE082
其中
Figure 752347DEST_PATH_IMAGE083
表示的是z里面的第i项，z表示所有n个传感器量测信息的集合，r表示残差项。问题被建模成非线性最小二乘问题，使用梯度下降法对该优化模型进行求解，获得载体的位姿信息
Figure 12427DEST_PATH_IMAGE084
。
当无可用的GNSS信息时，锁定受GNSS量测信息影响的状态量偏航角
Figure 622400DEST_PATH_IMAGE085
、接收机时钟钟差
Figure 917115DEST_PATH_IMAGE086
和接收机时钟钟差变化率
Figure 486766DEST_PATH_IMAGE087
并在因子图中隔离GNSS信息的伪距残差因子
Figure 765300DEST_PATH_IMAGE088
、多普勒频移残差因子
Figure 929697DEST_PATH_IMAGE089
和接收机时钟残差因子
Figure 78918DEST_PATH_IMAGE090
，系统退化为视觉惯性融合的定位方法并利用GNSS丢失前的偏航角
Figure 818204DEST_PATH_IMAGE091
推算载体的绝对位置，待重新接收到可用的GNSS信息时，再对GNSS相关的状态量进行解锁并加入伪距残差因子
Figure 36565DEST_PATH_IMAGE092
、多普勒频移残差因子
Figure 988340DEST_PATH_IMAGE093
和接收机时钟残差因子
Figure DEST_PATH_IMAGE094
进行联合优化，实现了载体进出拒止环境绝对定位的无缝解算输出。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。

Claims (6)

1.一种基于视觉惯性GNSS紧耦合的无缝定位方法，其特征在于：实现视觉惯性GNSS紧耦合并在进入GNSS拒止环境仍能进行无缝的绝对位置的获取；步骤如下：
步骤1：对IMU信息进行预积分处理，对视觉图像信息进行特征点的提取与跟踪，使用星历信息对GNSS信息进行可靠性筛选，得到可用的传感器信息；
步骤2：进行初始化，使用仅视觉的三维运动重建获得载体的相对于初始位置的初始相对位姿，进行视觉IMU联合初始化得到IMU加速度计与陀螺仪的偏置以及重力矢量的对齐；利用单点定位SPP算法通过至少四颗以上的卫星的伪距信息计算出载体粗略的绝对位置，利用多普勒频移与视觉惯性系统得到的相对速度对齐，进行视觉惯性系统与GNSS信息的联合初始化得到载体在ENU坐标系下的绝对位姿；
步骤3：计算视觉重投影误差、IMU预积分残差和GNSS信息的伪距残差、多普勒频移残差、接收机时钟残差并分别作为因子构建因子图，构建滑动窗口对其中所有的状态量
Figure QLYQS_1
进行优化求解，其中n为滑动窗口中的帧数，m为滑动窗口中所有特征点的数量
Figure QLYQS_2
Figure QLYQS_3
分别为载体的位置、速度和姿态，ba、bω为加速度计和陀螺仪的偏置，δt为接收机时钟钟差，
Figure QLYQS_4
为接收机时钟钟差变化率，
Figure QLYQS_5
为相机外参，ρm为特征点的逆深度，ψ为载体坐标系和ENU坐标系之间的偏航角；通过星历信息对卫星GNSS信息进行筛选，当无可用的GNSS信息时，锁定与GNSS相关的状态量并在因子图中隔离GNSS信息的伪距残差因子、多普勒频移残差因子和接收机时钟残差因子，切换到视觉惯性定位系统解算载体相对位姿并根据有可用GNSS信息时的先验信息推算ENU坐标系下的绝对位姿，待重新接收到可用得GNSS信息时，再对GNSS相关的状态量进行解锁，并加入伪距残差因子、多普勒频移残差因子和接收机时钟残差因子进行联合优化，令偏航角ψ、接收机时钟钟差δt、接收机时钟钟差变化率
Figure QLYQS_6
继续被各优化项优化更新；
所述的接收机时钟残差因子：
Figure QLYQS_7
其中δtk和δtk-1分别为第k时刻和第k-1时刻的接收机时钟钟差，
Figure QLYQS_8
为第k-1时刻的接收机时钟钟差变化率，
Figure QLYQS_9
为第k时刻和第k-1时刻的时间差。
2.根据权利要求1所述的一种基于视觉惯性GNSS紧耦合的无缝定位方法，其特征在于：步骤3所述的伪距残差因子：
Figure QLYQS_10
其中ωE为地球自转角速度，tf为卫星信号传输时间，Rz(ωEtf)为地球在卫星信号传输过程中的旋转，Rz(ωEtf)ps为接收机接收到卫星信号时卫星在ECEF坐标系下的实际位置，prk表示接收机的位置，c代表光速，δtk为接收机时钟钟差，Δts为卫星时钟误差，
Figure QLYQS_11
和
Figure QLYQS_12
分别代表对流层和电流层带来的延迟误差，
Figure QLYQS_13
为伪距的量测值。
3.根据权利要求2所述的一种基于视觉惯性GNSS紧耦合的无缝定位方法，其特征在于：步骤3所述的多普勒频移残差因子：
Figure QLYQS_14
其中λ表示载波信号的波长，
Figure QLYQS_15
表示从接收机到卫星的单位向量，vs和
Figure QLYQS_16
分别表示卫星和接收机的速度，
Figure QLYQS_17
表示接收机时钟钟差变化率，
Figure QLYQS_18
表示卫星时钟误差漂移率，
Figure QLYQS_19
为多普勒频移量测值。
4.根据权利要求3所述的一种基于视觉惯性GNSS紧耦合的无缝定位方法，其特征在于：步骤3所述的联合优化具体如下：
将残差项因子加入到优化项中，建立优化模型：
Figure QLYQS_20
其中zi表示的是z里面的第i项，z表示所有n个传感器量测信息的集合，r表示残差项；问题被建模成非线性最小二乘问题，使用梯度下降法对该优化模型进行求解，获得载体的位姿信息。
5.一种计算机系统，其特征在于包括：一个或多个处理器，计算机可读存储介质，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现权利要求1所述的方法。
6.一种计算机可读存储介质，其特征在于存储有计算机可执行指令，所述指令在被执行时用于实现权利要求1所述的方法。
CN202211512861.9A 2022-11-30 2022-11-30 一种基于视觉惯性gnss紧耦合的无缝定位方法 Active CN115523920B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211512861.9A CN115523920B (zh) 2022-11-30 2022-11-30 一种基于视觉惯性gnss紧耦合的无缝定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211512861.9A CN115523920B (zh) 2022-11-30 2022-11-30 一种基于视觉惯性gnss紧耦合的无缝定位方法

Publications (2)

Publication Number Publication Date
CN115523920A CN115523920A (zh) 2022-12-27
CN115523920B true CN115523920B (zh) 2023-03-10

Family

ID=84704965

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211512861.9A Active CN115523920B (zh) 2022-11-30 2022-11-30 一种基于视觉惯性gnss紧耦合的无缝定位方法

Country Status (1)

Country Link
CN (1) CN115523920B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117760428B (zh) * 2024-02-22 2024-04-30 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112179338A (zh) * 2020-09-07 2021-01-05 西北工业大学 一种基于视觉和惯导融合的低空无人机自身定位方法
CN114719843A (zh) * 2022-06-09 2022-07-08 长沙金维信息技术有限公司 复杂环境下的高精度定位方法
CN115388884A (zh) * 2022-08-17 2022-11-25 南京航空航天大学 一种智能体位姿估计器联合初始化方法
CN115406447A (zh) * 2022-10-31 2022-11-29 南京理工大学 拒止环境下基于视觉惯性的四旋翼无人机自主定位方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8589072B2 (en) * 2011-04-13 2013-11-19 Honeywell International, Inc. Optimal combination of satellite navigation system data and inertial data
EP3236209B1 (en) * 2016-04-19 2021-06-09 Honda Research Institute Europe GmbH Navigation system and method for error correction
CN109376785B (zh) * 2018-10-31 2021-09-24 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112179338A (zh) * 2020-09-07 2021-01-05 西北工业大学 一种基于视觉和惯导融合的低空无人机自身定位方法
CN114719843A (zh) * 2022-06-09 2022-07-08 长沙金维信息技术有限公司 复杂环境下的高精度定位方法
CN115388884A (zh) * 2022-08-17 2022-11-25 南京航空航天大学 一种智能体位姿估计器联合初始化方法
CN115406447A (zh) * 2022-10-31 2022-11-29 南京理工大学 拒止环境下基于视觉惯性的四旋翼无人机自主定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"Three-dimensional coverage-based cooperative guidance law with overload constraints to intercept a hypersonic vehicle";Shuangxi Liu et al.;《Aerospace Science and Technology》;20220928;第130卷;全文 *
基于单目视觉与惯导融合的无人机位姿估计;熊敏君等;《计算机应用》;20171231;全文 *

Also Published As

Publication number Publication date
CN115523920A (zh) 2022-12-27

Similar Documents

Publication Publication Date Title
Cao et al. GVINS: Tightly coupled GNSS–visual–inertial fusion for smooth and consistent state estimation
Wen et al. Towards robust GNSS positioning and real-time kinematic using factor graph optimization
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CA2848217C (en) Method and apparatus for navigation with nonlinear models
CN108594283B (zh) Gnss/mems惯性组合导航系统的自由安装方法
Lee et al. Tightly-coupled GNSS-aided visual-inertial localization
WO2020124624A1 (zh) 一种紧耦合的自动驾驶感知方法及系统
El-Sheimy Georeferencing component of LiDAR systems
CN115388884A (zh) 一种智能体位姿估计器联合初始化方法
CN106199668A (zh) 一种级联式gnss/sins深组合导航方法
CN115523920B (zh) 一种基于视觉惯性gnss紧耦合的无缝定位方法
Georgy Advanced nonlinear techniques for low cost land vehicle navigation
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能体位姿估计方法
Wen et al. 3D LiDAR aided GNSS real-time kinematic positioning
Beuchert et al. Factor graph fusion of raw GNSS sensing with IMU and lidar for precise robot localization without a base station
Afia et al. A low-cost gnss/imu/visual monoslam/wss integration based on federated kalman filtering for navigation in urban environments
Langel et al. Tightly coupled GPS/INS integration for differential carrier phase navigation systems using decentralized estimation
Maklouf et al. Performance evaluation of GPS\INS main integration approach
Ćwian et al. GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments
CN116105729A (zh) 一种针对野外洞穴森林环境侦察的多传感器融合定位方法
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN114646993A (zh) 一种基于gnss、视觉以及imu进行精确定位的数据融合算法
Afia Development of GNSS/INS/SLAM Algorithms for Navigation in Constrained Environments
Afia et al. A GNSS/IMU/WSS/VSLAM hybridization using an extended kalman filter

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