CN113203418B - 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 - Google Patents

基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 Download PDF

Info

Publication number
CN113203418B
CN113203418B CN202110426739.9A CN202110426739A CN113203418B CN 113203418 B CN113203418 B CN 113203418B CN 202110426739 A CN202110426739 A CN 202110426739A CN 113203418 B CN113203418 B CN 113203418B
Authority
CN
China
Prior art keywords
pseudo
error
gnss
information
course
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
CN202110426739.9A
Other languages
English (en)
Other versions
CN113203418A (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.)
Tongji University
Original Assignee
Tongji 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 Tongji University filed Critical Tongji University
Priority to CN202110426739.9A priority Critical patent/CN113203418B/zh
Publication of CN113203418A publication Critical patent/CN113203418A/zh
Application granted granted Critical
Publication of CN113203418B publication Critical patent/CN113203418B/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/20—Instruments for performing navigational calculations
    • 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
    • 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
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统，该方法包括以下步骤：1)对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息，并根据速度和位置信息得到伪距和伪距率的预测值；2)获取GNSS信号中的伪距和伪距率的观测信息，与伪距和伪距率的预测值做差得到伪距误差和伪距率误差；3)采用视觉提供的航向增量，与惯性测量单元得到的航向增量做差得到航向误差；4)根据速度、位置和姿态信息、伪距误差和伪距率误差以及航向误差采用序贯卡尔曼滤波进行时间更新和量测更新，并得到误差修正信息并反馈进行信息修正。与现有技术相比，本发明具有计算量小、成本低、精度高、可广泛应用等优点。

Description

基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统
技术领域
本发明涉及车辆定位技术领域，尤其是涉及一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统。
背景技术
近年来，随着智能汽车技术的快速发展，智能车对环境感知、定位、决策、规划与控制等系统的要求越来越高，其中，定位技术为其它系统提供姿态、速度、位置等信息，是智能汽车技术中不可或缺的一环。
如何获得连续、准确、可靠的车辆位姿信息成为智能车定位技术要解决的主要问题。目前常用的车辆定位方式包括全球导航卫星系统(GNSS)定位、惯性导航系统(INS)、激光雷达(Lidar)定位、视觉定位和基于V2X技术的定位。其中，融合GNSS与INS的组合定位算法是目前最常见的定位技术。GNSS定位通过对接收到的卫星信号进行实时解算伪距、伪距率和卫星位置等信息，根据三角定位法计算接收机天线中心位置，能提供低频的、无累积偏差的绝对位置信息。INS通过处理IMU输出的加速度、角速度数据，依次进行姿态更新、比力坐标转换、有害加速度/地球自转角速度补偿、速度更新、位置更新等步骤，获得高频的、存在累积误差的位置、速度、姿态信息。为解决GNSS低频、定位精度较低、位置噪声较大和INS存在累积误差的问题，GNSS/INS组合定位通过INS进行短时间的积分，使用GNSS作为观测修正载体状态，可以提供比GNSS或INS单独定位更准确的定位结果。
GNSS/INS组合定位作为主流的定位方案，目前仍然存在两个比较显著的问题。其一，GNSS/INS系统并不是在任何运动状态下都能提供精确的航向估计。在某些运动状态下，一些状态在理论上就是难以准确估计的，这与系统的可观性有关。其二，GNSS会由于卫星信号遮挡或者GNSS异常观测导致其解算出来的位置信息存在较大误差，甚至不能解算位置。GNSS信号不佳会显著降低GNSS/INS组合定位的状态估计精度，以上两个问题会对GNSS/INS组合定位的精度和系统的鲁棒性造成不小的影响。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法及系统。
本发明的目的可以通过以下技术方案来实现：
一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，包括以下步骤：
1)对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息，并根据速度和位置信息得到伪距和伪距率的预测值；
2)获取GNSS信号中的伪距和伪距率的观测信息，经过电离层、对流层的延迟修正处理后与伪距和伪距率的预测值做差得到伪距误差和伪距率误差；
3)采用视觉提供的航向增量，与惯性测量单元得到的航向增量做差得到航向误差；
4)根据速度、位置和姿态信息、伪距误差和伪距率误差以及航向误差采用序贯卡尔曼滤波进行时间更新和量测更新，并得到误差修正信息并反馈进行信息修正。
所述的步骤1)具体为：
根据惯性测量单元测量得到的三轴加速度积分处理并进行误差补偿得到速度信息，对速度积分处理并进行误差补偿得到位置信息，根据惯性测量单元测量得到的三轴角速度积分处理并进行误差补偿得到姿态信息，并根据速度和位置信息得到伪距和伪距率的预测值。
所述的步骤3)具体为：
采用单目摄像机特征点匹配得到航向增量与惯性测量单元得到的航向增量做差得到航向误差。
所述的步骤4)中，时间更新的输入为初始状态估计向量、状态估计误差的初始均方误差阵和系统噪声，更新得到状态估计向量和状态估计误差的均方误差阵；
量测更新的输入为时间更新输出的状态估计向量、状态估计误差的均方误差阵、GNSS观测向量以及视觉航向的观测向量，更新得到状态估计向量和状态估计误差的均方误差阵。
在序贯卡尔曼滤波中，系统状态向量X为：
Figure BDA0003029863960000031
其中，φ为姿态误差角，δvn为速度误差，δp为位置误差，ε为载体坐标系下的陀螺仪零偏，▽为载体坐标系下的加速度计零偏，τ为接收机时钟误差，dτ为接收机时钟误差漂移。
在序贯卡尔曼滤波中，系统状态转移矩阵F为：
Figure BDA0003029863960000032
Figure BDA0003029863960000033
Figure BDA0003029863960000034
Figure BDA0003029863960000035
Figure BDA0003029863960000036
Figure BDA0003029863960000037
Figure BDA0003029863960000038
Figure BDA0003029863960000041
Figure BDA0003029863960000042
Figure BDA0003029863960000043
Figure BDA0003029863960000044
Figure BDA0003029863960000045
其中，
Figure BDA0003029863960000046
为导航坐标系相对于惯性坐标系的角速度在导航坐标系下的投影，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，L为载体的地理纬度，ωie为地球自转角速率，vE为东-北-天坐标系下的东向速度，vN为北向速度，vU为天向速度，
Figure BDA0003029863960000047
为载体坐标系转化至导航坐标系的姿态转移矩阵，
Figure BDA0003029863960000048
为加速度计测量的比力，h为载体重心距地面的高度，×表示反对称矩阵，
在序贯卡尔曼滤波中，GNSS测量矩阵H1为：
Figure BDA0003029863960000049
视觉航向测量矩阵H2为：
H2＝[01×2；1；014×1]
Figure BDA00030298639600000410
Figure BDA00030298639600000411
其中，LT为伪距相关的观测矩阵，LC为伪距率相关的观测矩阵，Im×n表示m×n维单位阵，0m×n表示m×n维0矩阵，λ为载体的地理经度，LOS表示地心地固坐标系下载体到不同卫星方向的单位向量，e为地球的偏心率。
在序贯卡尔曼滤波中，GNSS观测向量Z1为：
Figure BDA0003029863960000051
视觉航向观测值Z2为：
Z2＝ψmeasure-ψINS
其中，ρGNSS为GNSS的伪距观测量，δρGNSS为GNSS的伪距率观测量，ρINS为INS和星历计算的伪距预测量，δρINS表示INS与星历计算的伪距率预测量，ψmeasure为视觉惯性里程计测量得到的航向，ψINS为惯性测量单元处理模块得到的航向。
在序贯卡尔曼滤波中，计算预测及更新包括：
计算状态预测：
X(k|k-1)＝F(k-1)X(k-1)
计算协方差矩阵预测：
P1(k|k-1)＝F(k-1)P1(k-1)FT(k-1)+Q1
计算GNSS观测卡尔曼滤波增益：
K1(k)＝P1(k|k-1)H1 T(k)[H1(k)P1(k|k-1)H1 T(k)+R1]-1
计算GNSS观测状态更新：
X1(k)＝X(k|k-1)+K1(k)[Z1(k)-H1 T(k)X(k|k-1)]
计算GNSS观测协方差矩阵更新：
P1(k)＝(I-K1(k)H1(k))P1(k|k-1)
计算视觉航向观测卡尔曼滤波增益：
K2(k)＝P1(k)H2 T(k)[H2(k)P1(k)H2 T(k)+R2]-1
计算视觉航向观测状态更新：
X2(k)＝X1(k)+K1(k)[Z2(k)-H2 T(k)X1(k)]
计算视觉航向观测协方差矩阵更新：
P2(k)＝(I-K2(k)H2(k))P1(k)
其中，X(k|k-1)为系统状态预测值，F(k-1)为上一时刻系统状态转移矩阵，X(k-1)为上一时刻系统状态，P1(k|k-1)为协方差矩阵的预测值，Q1为状态方程过程噪声方差，R1为GNSS量测噪声方差，R2为视觉航向量测噪声方差，K1(k)为GNSS观测卡尔曼滤波增益，K2(k)为GNSS观测卡尔曼滤波增益，X1(k)为GNSS观测更新的状态向量，X2(k)为视觉航向观测更新的状态向量，P1(k)为GNSS观测更新的协方差矩阵的观测值，P2(k)为视觉航向更新的协方差矩阵的观测值，k和k-1分别表示当前时刻和上一时刻。
一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位系统，该系统包括：
惯性测量单元信号处理模块：通过三轴加速度计和三轴陀螺仪获取载体在三个方向的加速度和速度，通过积分更新载体的姿态、速度和位置信息，根据速度和位置信息得到伪距和伪距率的预测值，将预测值与经过GNSS信号处理模块修正后的伪距和伪距率的观测值做差，得到伪距误差和伪距率误差，并发送给融合定位模块；
GNSS信号处理模块：接收GNSS信号并进行信号处理，得到GNSS的伪距信息和伪距率的观测信息，并且对伪距和伪距率的观测信息进行电离层、对流层的延迟修正；
视觉惯性里程计信号处理模块：根据视觉提供的航向增量和惯性测量单元信号处理模块的航向增量得到航向误差，并将航向误差发送给融合定位模块；
融合定位模块：接收GNSS信号处理模块、惯性测量单元信号处理模块和视觉惯性里程计信号处理模块发生的信息并进行序贯卡尔曼滤波处理，包括时间更新模块和量测更新模块，分别进行时间更新和量测更新得到误差修正信息并反馈至惯性测量单元信号处理模块和GNSS信号处理模块进行信息修正。
与现有技术相比，本发明具有以下优点：
一、本发明仅利用惯性测量单元和GNSS传感器即可实现车辆载体的精确快速定位，成本低廉易实现。
二、本发明采用序贯卡尔曼滤波，将GNSS、INS和视觉里程计更稳定地融合，得到精度较高的车辆姿态、位置和速度信息。
三、本发明采用了GNSS和INS的紧耦合算法，较现有普遍使用的GNSS/INS松耦合算法精度更高。
四、本发明在GNSS/INS紧耦合的基础上融合了视觉里程计的特征级信息，针对紧耦合航向角弱可观的特性，有效提高了车辆的航向精度。
五、本发明在序贯卡尔曼滤波中的输入值为17维状态向量，与普遍使用的15维状态向量增加了接收机钟差和接收机钟差漂移两个信息，建模准确，估计结果精度高，鲁棒性强，应用范围广。
附图说明
图1为本发明一种基于序贯卡尔曼滤波的GNSS/INS/视觉融合定位系统的结构功能框图。
图2为本发明的序贯卡尔曼滤波的计算流程图。
图中标记说明：
1、惯性测量单元信号处理模块，2、GNSS信号处理模块，3、视觉惯性里程计信号处理模块，4、融合定位模块，5、时间更新模块，6、量测更新模块。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
如图1所示，本发明提供一种基于序贯卡尔曼滤波的GNSS/INS/视觉融合定位系统，该系统包括：
惯性测量单元信号处理模块1，对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息；
GNSS信号处理模块2，对GNSS传感器的信号和进行处理，得到伪距误差和伪距率误差的观测值，再对其进行电离层延迟和对流层延迟的修正，将修正后的数据与预测的数值做差，获得伪距误差和伪距率误差；
视觉惯性里程计信号处理模块3，采用视觉提供的航向增量与惯性导航系统积分计算的航向增量做差，获得航向误差；处理之后得到的信号发送至融合定位模块；
融合定位模块4，基于序贯卡尔曼滤波处理传感器信号处理模块发送的信号，对车辆的姿态、位置和速度进行估计。
如图2所示，融合定位模块4采用序贯卡尔曼滤波技术，具体包括时间更新模块5和量测更新模块6。
时间更新模块5的输入为初始状态向量X0、初始协方差矩阵P0和系统噪声Q1，输出为状态向量X和协方差矩阵P。
量测更新模块6具体包括GNSS观测更新和视觉航向观测更新，GNSS观测更新的输入为GNSS观测向量Z1，视觉航向观测更新的输入为视觉航向观测值Z2，输出为状态向量X和协方差矩阵P，具体计算过程如下所示：
系统状态向量：
Figure BDA0003029863960000081
系统状态转移矩阵：
Figure BDA0003029863960000082
GNSS测量矩阵：
Figure BDA0003029863960000083
视觉航向测量矩阵：
H2＝[01×2；1；014×1]
GNSS观测向量：
Figure BDA0003029863960000084
视觉航向观测值：
Z2＝ψmeasure-ψINS
计算状态预测：
X(k|k-1)＝F(k-1)X(k-1)
计算协方差矩阵预测：
P1(k|k-1)＝F(k-1)P1(k-1)FT(k-1)+Q1
计算GNSS观测卡尔曼滤波增益：
K1(k)＝P1(k|k-1)H1 T(k)[H1(k)P1(k|k-1)H1 T(k)+R1]-1
计算GNSS观测状态更新：
X1(k)＝X(k|k-1)+K1(k)[Z1(k)-H1 T(k)X(k|k-1)]
计算GNSS观测协方差矩阵更新：
P1(k)＝(I-K1(k)H1(k))P1(k|k-1)
计算视觉航向观测卡尔曼滤波增益：
K2(k)＝P1(k)H2 T(k)[H2(k)P1(k)H2 T(k)+R2]-1
计算视觉航向观测状态更新：
X2(k)＝X1(k)+K1(k)[Z2(k)-H2 T(k)X1(k)]
计算视觉航向观测协方差矩阵更新：
P2(k)＝(I-K2(k)H2(k))P1(k)
其中，
Figure BDA0003029863960000091
Figure BDA0003029863960000092
Figure BDA0003029863960000093
Figure BDA0003029863960000094
Figure BDA0003029863960000095
Figure BDA0003029863960000096
Figure BDA0003029863960000097
Figure BDA0003029863960000098
Figure BDA0003029863960000099
Figure BDA00030298639600000910
Figure BDA0003029863960000101
Figure BDA0003029863960000102
Figure BDA0003029863960000103
其中，φ为姿态误差角，δvn为速度误差，δp为位置误差，ε为载体坐标系下的陀螺零偏，▽为载体坐标系下的加速度计零偏，τ为接收机时钟误差，dτ为接收机时钟误差漂移，L为载体的地理纬度，λ是载体的地理经度，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，h是载体重心距地面的高度，e是地球的偏心率，vE是东-北-天坐标系下的东向速度，vN是北向速度，vU是天向速度，
Figure BDA0003029863960000104
是载体坐标系转化至导航坐标系的姿态转移矩阵，ωie是地球自转角速率，
Figure BDA0003029863960000105
为导航坐标系相对于惯性坐标系的角速度在导航坐标系下的投影，
Figure BDA0003029863960000106
为加速度计测量的比力，×表示反对称矩阵，X(k|k-1)表示状态预测值，F(k-1)为上一时刻系统状态转移矩阵，X(k-1)为上一时刻系统状态，P1(k|k-1)表示协方差矩阵的预测值，Q1表示状态方程过程噪声方差，R1表示GNSS量测噪声方差，R2表示视觉航向量测噪声方差，K1(k)为GNSS观测卡尔曼滤波增益，K2(k)为GNSS观测卡尔曼滤波增益，X1(k)表示GNSS观测更新的状态向量，X2(k)表示视觉航向观测更新的状态向量，P1(k)表示GNSS观测更新的协方差矩阵的观测值，P1(k)表示视觉航向更新的协方差矩阵的观测值，Im×n表示m×n维单位阵，0m×n表示m×n维0矩阵，LT为伪距相关的观测矩阵，LC为伪距率相关的观测矩阵，LOS代表地心地固坐标系下载体到不同卫星方向的单位向量，ρGNSS表示GNSS的伪距观测量，δρGNSS表示GNSS的伪距率观测量，ρINS表示INS和星历计算的伪距预测量，δρINS表示INS与星历计算的伪距率预测量，ψmeasure表示视觉惯性里程计测量得到的航向，ψINS表示惯性测量单元处理模块得到的航向。
通过融合定位模块4更新误差状态矩阵，反馈至惯性测量单元信号处理模块1修正车辆信息并输出车辆位姿、速度及位置。
基于序贯卡尔曼滤波的GNSS/INS/视觉融合定位系统，本发明采用上述融合定位系统的定位方法实现对车辆姿态、速度和位置的准确快速估计。

Claims (5)

1.一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，包括以下步骤：
1)对惯性单元测量得到的三轴角速度和加速度进行积分处理，得到姿态、位置和速度信息，并根据速度和位置信息得到伪距和伪距率的预测值；
2)获取GNSS信号中的伪距和伪距率的观测信息，经过电离层、对流层的延迟修正处理后与伪距和伪距率的预测值做差得到伪距误差和伪距率误差；
3)采用视觉提供的航向增量，与惯性测量单元得到的航向增量做差得到航向误差；
4)根据速度、位置和姿态信息、伪距误差和伪距率误差以及航向误差采用序贯卡尔曼滤波进行时间更新和量测更新，并得到误差修正信息并反馈进行信息修正，时间更新的输入为初始状态估计向量、状态估计误差的初始均方误差阵和系统噪声，更新得到状态估计向量和状态估计误差的均方误差阵；
量测更新的输入为时间更新输出的状态估计向量、状态估计误差的均方误差阵、GNSS观测向量以及视觉航向的观测向量，更新得到状态估计向量和状态估计误差的均方误差阵；
在序贯卡尔曼滤波中，系统状态向量X为：
Figure FDA0003636156550000011
其中，φ为姿态误差角，δvn为速度误差，δp为位置误差，ε为载体坐标系下的陀螺仪零偏，
Figure FDA0003636156550000012
为载体坐标系下的加速度计零偏，τ为接收机时钟误差，dτ为接收机时钟误差漂移；
在序贯卡尔曼滤波中，系统状态转移矩阵F为：
Figure FDA0003636156550000021
Figure FDA0003636156550000022
Figure FDA0003636156550000023
Figure FDA0003636156550000024
Figure FDA0003636156550000025
Figure FDA0003636156550000026
Figure FDA0003636156550000027
Figure FDA0003636156550000028
Figure FDA0003636156550000029
Figure FDA00036361565500000210
Figure FDA00036361565500000211
Figure FDA00036361565500000212
其中，
Figure FDA00036361565500000213
为导航坐标系相对于惯性坐标系的角速度在导航坐标系下的投影，RM为子午圈主曲率半径，RN为卯酉圈主曲率半径，L为载体的地理纬度，ωie为地球自转角速率，vE为东-北-天坐标系下的东向速度，vN为北向速度，vU为天向速度，
Figure FDA0003636156550000031
为载体坐标系转化至导航坐标系的姿态转移矩阵，
Figure FDA0003636156550000032
为加速度计测量的比力，h为载体重心距地面的高度，×表示反对称矩阵；
在序贯卡尔曼滤波中，GNSS测量矩阵H1为：
Figure FDA0003636156550000033
视觉航向测量矩阵H2为：
H2＝[01×2；1；014×1]
Figure FDA0003636156550000034
Figure FDA0003636156550000035
其中，LT为伪距相关的观测矩阵，LC为伪距率相关的观测矩阵，Im×n表示m×n维单位阵，0m×n表示m×n维0矩阵，λ为载体的地理经度，LOS表示地心地固坐标系下载体到不同卫星方向的单位向量，e为地球的偏心率；
在序贯卡尔曼滤波中，GNSS观测向量Z1为：
Figure FDA0003636156550000036
视觉航向观测值Z2为：
Z2＝ψmeasure-ψINS
其中，ρGNSS为GNSS的伪距观测量，δρGNSS为GNSS的伪距率观测量，ρINS为INS和星历计算的伪距预测量，δρINS表示INS与星历计算的伪距率预测量，ψmeasure为视觉惯性里程计测量得到的航向，ψINS为惯性测量单元处理模块得到的航向。
2.根据权利要求1所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，所述的步骤1)具体为：
根据惯性测量单元测量得到的三轴加速度积分处理并进行误差补偿得到速度信息，对速度积分处理并进行误差补偿得到位置信息，根据惯性测量单元测量得到的三轴角速度积分处理并进行误差补偿得到姿态信息，并根据速度和位置信息得到伪距和伪距率的预测值。
3.根据权利要求1所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，所述的步骤3)具体为：
采用单目摄像机特征点匹配得到航向增量与惯性测量单元得到的航向增量做差得到航向误差。
4.根据权利要求1所述的一种基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法，其特征在于，在序贯卡尔曼滤波中，计算预测及更新包括：
计算状态预测：
X(k|k-1)＝F(k-1)X(k-1)
计算协方差矩阵预测：
P1(k|k-1)＝F(k-1)P1(k-1)FT(k-1)+Q1
计算GNSS观测卡尔曼滤波增益：
K1(k)＝P1(k|k-1)H1 T(k)[H1(k)P1(k|k-1)H1 T(k)+R1]-1
计算GNSS观测状态更新：
X1(k)＝X(k|k-1)+K1(k)[Z1(k)-H1 T(k)X(k|k-1)]
计算GNSS观测协方差矩阵更新：
P1(k)＝(I-K1(k)H1(k))P1(k|k-1)
计算视觉航向观测卡尔曼滤波增益：
K2(k)＝P1(k)H2 T(k)[H2(k)P1(k)H2 T(k)+R2]-1
计算视觉航向观测状态更新：
X2(k)＝X1(k)+K1(k)[Z2(k)-H2 T(k)X1(k)]
计算视觉航向观测协方差矩阵更新：
P2(k)＝(I-K2(k)H2(k))P1(k)
其中，X(k|k-1)为系统状态预测值，F(k-1)为上一时刻系统状态转移矩阵，X(k-1)为上一时刻系统状态，P1(k|k-1)为协方差矩阵的预测值，Q1为状态方程过程噪声方差，R1为GNSS量测噪声方差，R2为视觉航向量测噪声方差，K1(k)为GNSS观测卡尔曼滤波增益，K2(k)为GNSS观测卡尔曼滤波增益，X1(k)为GNSS观测更新的状态向量，X2(k)为视觉航向观测更新的状态向量，P1(k)为GNSS观测更新的协方差矩阵的观测值，P2(k)为视觉航向更新的协方差矩阵的观测值，k和k-1分别表示当前时刻和上一时刻。
5.一种用以实现如权利要求1所述基于序贯卡尔曼滤波的GNSSINS视觉融合定位方法的系统，其特征在于，该系统包括：
惯性测量单元信号处理模块(1)：通过三轴加速度计和三轴陀螺仪获取载体在三个方向的加速度和速度，通过积分更新载体的姿态、速度和位置信息，根据速度和位置信息得到伪距和伪距率的预测值，将预测值与经过GNSS信号处理模块修正后的伪距和伪距率的观测值做差，得到伪距误差和伪距率误差，并发送给融合定位模块；
GNSS信号处理模块(2)：接收GNSS信号并进行信号处理，得到GNSS的伪距信息和伪距率的观测信息，并且对伪距和伪距率的观测信息进行电离层、对流层的延迟修正；
视觉惯性里程计信号处理模块(3)：根据视觉提供的航向增量和惯性测量单元信号处理模块的航向增量得到航向误差，并将航向误差发送给融合定位模块；
融合定位模块(4)：接收GNSS信号处理模块、惯性测量单元信号处理模块和视觉惯性里程计信号处理模块发生的信息并进行序贯卡尔曼滤波处理，包括时间更新模块(5)和量测更新模块(6)，分别进行时间更新和量测更新得到误差修正信息并反馈至惯性测量单元信号处理模块和GNSS信号处理模块进行信息修正。
CN202110426739.9A 2021-04-20 2021-04-20 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 Active CN113203418B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110426739.9A CN113203418B (zh) 2021-04-20 2021-04-20 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110426739.9A CN113203418B (zh) 2021-04-20 2021-04-20 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统

Publications (2)

Publication Number Publication Date
CN113203418A CN113203418A (zh) 2021-08-03
CN113203418B true CN113203418B (zh) 2022-09-16

Family

ID=77027568

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110426739.9A Active CN113203418B (zh) 2021-04-20 2021-04-20 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统

Country Status (1)

Country Link
CN (1) CN113203418B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114136315B (zh) * 2021-11-30 2024-04-16 山东天星北斗信息科技有限公司 一种基于单目视觉辅助惯性组合导航方法及系统
CN114353786B (zh) * 2021-11-30 2023-07-07 安徽海博智能科技有限责任公司 一种基于改进卡尔曼滤波器的无人矿卡融合定位方法
CN114279449A (zh) * 2022-01-01 2022-04-05 南昌智能新能源汽车研究院 一种考虑加速度计温度漂移误差的姿态估计方法
CN114114369B (zh) * 2022-01-27 2022-07-15 智道网联科技(北京)有限公司 自动驾驶车辆定位方法和装置、电子设备和存储介质
CN115113226B (zh) * 2022-06-27 2024-05-17 桂林理工大学 一种用于机载LiDAR装置的高频POS系统
CN116718153B (zh) * 2023-08-07 2023-10-27 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统
CN117687414B (zh) * 2023-12-12 2024-08-13 东南大学 一种草坪修护机器人定位定向方法及系统
CN117521018B (zh) * 2024-01-08 2024-03-26 鹏城实验室 基于扩展观测的融合估计方法、装置、设备及存储介质
CN117972637B (zh) * 2024-03-28 2024-06-21 天津大学 角振动台的角速度数据融合方法和角速度数据融合装置

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106679648B (zh) * 2016-12-08 2019-12-10 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN107643534B (zh) * 2017-09-11 2019-07-12 东南大学 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法
CN108981692B (zh) * 2018-06-14 2021-04-30 兰州晨阳启创信息科技有限公司 一种基于惯导/视觉里程计的列车定位方法及系统
CN108873038B (zh) * 2018-09-10 2020-11-06 芜湖盟博科技有限公司 自主泊车定位方法及定位系统
CN109471144B (zh) * 2018-12-13 2023-04-28 北京交通大学 基于伪距/伪距率的多传感器紧组合列车组合定位方法
CN111121767B (zh) * 2019-12-18 2023-06-30 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
复杂城市环境下GNSS/INS组合导航可观测度分析及鲁棒滤波方法;沈凯等;《仪器仪表学报》;20200930;第41卷(第9期);第252-261页 *

Also Published As

Publication number Publication date
CN113203418A (zh) 2021-08-03

Similar Documents

Publication Publication Date Title
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN106405670B (zh) 一种适用于捷联式海洋重力仪的重力异常数据处理方法
US6496778B1 (en) Real-time integrated vehicle positioning method and system with differential GPS
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN112378400A (zh) 一种双天线gnss辅助的捷联惯导组合导航方法
CN108344415B (zh) 一种组合导航信息融合方法
EP1221586A2 (en) Position and heading error-correction method and apparatus for vehicle navigation systems
CN111854746A (zh) Mimu/csac/高度计辅助卫星接收机的定位方法
CN109613585A (zh) 一种对基站天线超短基线gnss双天线实时测向的方法
CN108709552A (zh) 一种基于mems的imu和gps紧组合导航方法
Georgy et al. Vehicle navigator using a mixture particle filter for inertial sensors/odometer/map data/GPS integration
CN106767787A (zh) 一种紧耦合gnss/ins组合导航装置
CN107121141A (zh) 一种适用于定位导航授时微系统的数据融合方法
CN110221332A (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
WO2011120029A2 (en) Pedal navigation using leo signals and body-mounted sensors
CN110133692B (zh) 惯导技术辅助的高精度gnss动态倾斜测量系统及方法
KR100443550B1 (ko) 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법
CN113253325B (zh) 惯性卫星序贯紧组合李群滤波方法
US9243914B2 (en) Correction of navigation position estimate based on the geometry of passively measured and estimated bearings to near earth objects (NEOS)
CN111965685A (zh) 一种基于多普勒信息的低轨卫星/惯性组合导航定位方法
CN115388884A (zh) 一种智能体位姿估计器联合初始化方法
CN113419265B (zh) 一种基于多传感器融合的定位方法、装置与电子设备
CN117804443A (zh) 一种北斗卫星多模融合定位监测方法及系统
Afia et al. A low-cost gnss/imu/visual monoslam/wss integration based on federated kalman filtering for navigation in urban environments

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