CN110988951A - 多源数据融合实时导航定位方法及系统 - Google Patents

多源数据融合实时导航定位方法及系统 Download PDF

Info

Publication number
CN110988951A
CN110988951A CN201911241794.XA CN201911241794A CN110988951A CN 110988951 A CN110988951 A CN 110988951A CN 201911241794 A CN201911241794 A CN 201911241794A CN 110988951 A CN110988951 A CN 110988951A
Authority
CN
China
Prior art keywords
data
gnss
time
initialization
update
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
CN201911241794.XA
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.)
China University of Geosciences Beijing
Original Assignee
China University of Geosciences Beijing
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 China University of Geosciences Beijing filed Critical China University of Geosciences Beijing
Priority to CN201911241794.XA priority Critical patent/CN110988951A/zh
Publication of CN110988951A publication Critical patent/CN110988951A/zh
Pending legal-status Critical Current

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/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
    • 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • 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/40—Correcting position, velocity or attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种多源数据融合实时导航定位方法及系统。该方法包括：获取GNSS数据、由IGS播发的GNSS运行数据、IMU数据、磁强数据和里程数据；由GNSS数据和GNSS运行数据计算当前位置和速度；根据当前位置和速度实现位置和速度初始化，根据IMU数据实现姿态初始化得到位姿初始化结果；基于位姿初始化结果进行惯性导航状态更新得到位姿更新结果；检查惯性测量单元的每个测量时刻是否获取到GNSS数据、磁强数据或里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或观测更新得到参数改正数向量；基于参数改正数向量利用误差反馈理论对位姿更新结果进行修正得到导航定位信息。本发明能够提高信号失锁环境中的定位精度。

Description

多源数据融合实时导航定位方法及系统
技术领域
本发明涉及导航定位领域，特别是涉及一种多源数据融合实时导航定位方法及系统。
背景技术
目前，全球卫星导航系统(Global Navigation Satellite System，GNSS)是动态导航与位置服务领域中不可或缺的重要技术手段，其通过分布在不同轨道面上的多颗导航卫星不间断地为全球任意地面用户播发调制包含有测距码、卫星轨道和卫星钟参数、时间信息等无线电载波信号，采用精密定位算法，可为用户提供高精度的位置、速度和时间信息。相对于传统定位方法，卫星定位系统具有全球覆盖、全天候、实时、高精度、误差有界等优点。
然而，GNSS作为一种无线电导航定位手段，在应用日益广泛的同时，其本身不足也日益明显。例如当处于城市峡谷、隧道、高架桥、树荫等容易引起信号失锁的环境中，GNSS的定位精度大大降低甚至无法实现导航定位。
发明内容
本发明的目的是提供一种多源数据融合实时导航定位方法及系统，提高信号失锁环境中的定位精度。
为实现上述目的，本发明提供了如下方案：
一种多源数据融合实时导航定位方法，应用于一种多源数据融合实时导航定位装置，该定位装置包括：GNSS接收机、惯性测量单元、磁强计、里程计、网络模块和FPGA主板；所述GNSS接收机、所述惯性测量单元、所述磁强计、所述里程计和所述网络模块均与所述FPGA主板连接；
该导航定位方法包括：
获取所述GNSS接收机采集的GNSS数据、所述网络模块接收的由国际GNSS服务组织播发的GNSS运行数据、所述惯性测量单元测量的IMU数据、所述磁强计测量的磁强数据和所述里程计测量的里程数据；
基于最小二乘和序贯最小二乘理论，根据非差非组合精密单点定位理论由所述GNSS数据以及所述GNSS运行数据计算载体当前位置和当前速度；
根据所述当前位置和所述当前速度实现位置初始化和速度初始化，根据所述IMU数据实现姿态初始化，得到位姿初始化结果；
基于所述位姿初始化结果利用惯性导航系统力学编排理论进行惯性导航状态更新，得到位姿更新结果；
检查每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或卡尔曼滤波的观测更新从而得到参数改正数向量；
基于所述参数改正数向量，利用误差反馈理论对位姿更新结果进行修正，得到导航定位信息。
可选的，所述检查每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或卡尔曼滤波的观测更新从而得到参数改正数向量，具体包括：
判断每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据；
针对未获取到所述GNSS数据、所述磁强数据和所述里程数据的时刻，以所述位姿更新结果作为该时刻的导航结果，并构建卡尔曼滤波时间更新方程和协方差矩阵；所述时间更新方程的状态参数向量的分量包括位置信息、速度信息、姿态信息、零偏、比例因子误差、接收机钟差、接收机钟漂、对流层湿延迟、接收机码间偏差、载波模糊度和电离层延迟；
将所述位置信息、所述速度信息、所述姿态信息采用动力学函数模型进行描述；将所述零偏和所述比例因子误差建模为一阶高斯马尔科夫过程；将所述对流层湿延迟、所述接收机码间偏差和电离层延迟建模为随机游走过程；将所述载波模糊度建模为随机常数；将所述接收机钟差和所述接收机钟漂建模为
Figure BDA0002306462150000021
从而得到建立好的卡尔曼滤波时间更新模型；其中tr，k与tr，k-1分别为k时刻和k-1时刻的接收机钟差，
Figure BDA0002306462150000031
与
Figure BDA0002306462150000032
分别为k时刻和k-1时刻的接收机钟漂；dt为采样间隔，εt，k-1与
Figure BDA0002306462150000033
分别为接收机钟差噪声和接收机钟漂噪声；
针对获取到所述GNSS数据、所述磁强数据或所述里程数据的时刻，根据所述GNSS数据、所述GNSS运行数据、所述IMU数据、所述磁强数据、所述里程数据以及载体状态约束观测量构建观测新息向量，进而构建卡尔曼滤波观测更新方程，得到建立好的卡尔曼滤波观测更新模型；
基于扩展卡尔曼滤波参数估计理论，根据所述卡尔曼滤波时间更新模型和所述卡尔曼滤波观测更新模型对所述状态参数向量和所述协方差矩阵进行最优估计，得到最优状态参数向量和最优协方差矩阵；
基于所述最优状态参数向量和所述最优协方差矩阵计算所述参数改正数向量。
可选的，所述根据所述当前位置和所述当前速度实现位置初始化和速度初始化，根据所述IMU数据实现姿态初始化，得到位姿初始化结果，具体包括：
以所述当前位置作为位置初始化结果，以所述当前速度作为速度初始化结果；
根据所述IMU数据与地球自转角速度以及地球重力之间的姿态转换关系计算姿态方向余弦矩阵；
根据所述姿态方向余弦矩阵计算初始姿态角，得到姿态初始化结果。
可选的，在所述获取所述GNSS接收机采集的GNSS数据、所述网络模块接收的由国际GNSS服务组织播发的GNSS运行数据、所述惯性测量单元测量的IMU数据、所述磁强计测量的磁强数据和所述里程计测量的里程数据之前，还包括：
以GNSS接收机的时间为基准对所述惯性测量单元、所述磁强计和所述里程计的时间进行同步。
一种多源数据融合实时导航定位系统，应用于一种多源数据融合实时导航定位装置，该定位装置包括：GNSS接收机、惯性测量单元、磁强计、里程计、网络模块和FPGA主板；所述GNSS接收机、所述惯性测量单元、所述磁强计、所述里程计和所述网络模块均与所述FPGA主板连接；
该导航定位系统包括：
数据获取模块，用于获取所述GNSS接收机采集的GNSS数据、所述网络模块接收的由国际GNSS服务组织播发的GNSS运行数据、所述惯性测量单元测量的IMU数据、所述磁强计测量的磁强数据和所述里程计测量的里程数据；
位置初步估计模块，用于基于最小二乘和序贯最小二乘理论，根据非差非组合精密单点定位理论由所述GNSS数据以及所述GNSS运行数据计算载体当前位置和当前速度；
位姿初始化模块，用于根据所述当前位置和所述当前速度实现位置初始化和速度初始化，根据所述IMU数据实现姿态初始化，得到位姿初始化结果；
惯性导航状态更新模块，用于基于所述位姿初始化结果利用惯性导航系统力学编排理论进行惯性导航状态更新，得到位姿更新结果；
卡尔曼滤波模块，用于检查每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或卡尔曼滤波的观测更新从而得到参数改正数向量；
修正模块，用于基于所述参数改正数向量，利用误差反馈理论对位姿更新结果进行修正，得到导航定位信息。
可选的，所述卡尔曼滤波模块包括：
判断单元，用于判断每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据；
时间更新方程建立单元，用于针对未获取到所述GNSS数据、所述磁强数据和所述里程数据的时刻，以所述位姿更新结果作为该时刻的导航结果，并构建卡尔曼滤波时间更新方程和协方差矩阵；所述时间更新方程的状态参数向量的分量包括位置信息、速度信息、姿态信息、零偏、比例因子误差、接收机钟差、接收机钟漂、对流层湿延迟、接收机码间偏差、载波模糊度和电离层延迟；
状态参数向量建模单元，用于将所述位置信息、所述速度信息、所述姿态信息采用动力学函数模型进行描述；将所述零偏和所述比例因子误差建模为一阶高斯马尔科夫过程；将所述对流层湿延迟、所述接收机码间偏差和电离层延迟建模为随机游走过程；将所述载波模糊度建模为随机常数；将所述接收机钟差和所述接收机钟漂建模为
Figure BDA0002306462150000051
从而得到建立好的卡尔曼滤波时间更新模型；其中tr，k与tr，k-1分别为k时刻和k-1时刻的接收机钟差，
Figure BDA0002306462150000052
与
Figure BDA0002306462150000053
分别为k时刻和k-1时刻的接收机钟漂；dt为采样间隔，εt，k-1与
Figure BDA0002306462150000054
分别为接收机钟差噪声和接收机钟漂噪声；
观测更新方程建立单元，用于针对获取到所述GNSS数据、所述磁强数据或所述里程数据的时刻，根据所述GNSS数据、所述GNSS运行数据、所述IMU数据、所述磁强数据、所述里程数据以及载体状态约束观测量构建观测新息向量，进而构建卡尔曼滤波观测更新方程，得到建立好的卡尔曼滤波观测更新模型；
滤波单元，用于基于扩展卡尔曼滤波参数估计理论，根据所述卡尔曼滤波时间更新模型和所述卡尔曼滤波观测更新模型对所述状态参数向量和所述协方差矩阵进行最优估计，得到最优状态参数向量和最优协方差矩阵；
改正量计算单元，用于基于所述最优状态参数向量和所述最优协方差矩阵计算所述参数改正数向量。
可选的，所述位姿初始化模块包括：
位置速度初始化单元，用于以所述当前位置作为位置初始化结果，以所述当前速度作为速度初始化结果；
姿态方向余弦矩阵计算单元，用于根据所述IMU数据与地球自转角速度以及地球重力之间的姿态转换关系计算姿态方向余弦矩阵；
姿态初始化单元，用于根据所述姿态方向余弦矩阵计算初始姿态角，得到姿态初始化结果。
可选的，该导航定位系统还包括：
时间同步模块，用于以GNSS接收机的时间为基准对所述惯性测量单元、所述磁强计和所述里程计的时间进行同步。
根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明的多源数据融合实时导航定位方法及系统，将GNSS技术与惯性测量单元、磁强计、里程计相结合，首先利用GNSS数据与IMU数据对载体位姿进行初始化，并利用初始化后的数据进行INS更新，最后利用卡尔曼滤波对更新后的数据进行修正，从而能够通过修正得到准确的定位结果，即便在GNSS信号失锁的环境中也能实现精准定位，提高了号失锁环境中的定位精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所采用的多源数据融合实时导航定位装置的结构图；
图2为实施例1的多源数据融合实时导航定位方法的方法流程图；
图3为本发明实施例2的多源数据融合实时导航定位系统的系统结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
本发明的多源数据融合实时导航定位方法及系统，均应用于一种多源数据融合实时导航定位装置。
图1为本发明所采用的多源数据融合实时导航定位装置的结构图。
参见图1，该定位装置包括：GNSS接收机1、惯性测量单元(IMU，InertialMeasurement Unit)2、磁强计3、里程计(ODO，Odometer)4、网络模块5和FPGA(FieldProgrammable Gate Array，现场可编程逻辑门阵列)主板6；GNSS接收机1、惯性测量单元2、磁强计3、里程计4和网络模块5均通过馈线7与FPGA主板6连接。GNSS接收机天线为多系统GNSS接收机天线，所接收的GNSS数据包括中国第三代北斗卫星导航系统(BDS，BeiDouSatellite Navigation System)B1和B2频率数据、美国全球定位系统(GPS，GlobalPositioning System)L1和L2频率数据、俄罗斯全球导航卫星系统(GLONASS，GLObalNAvigation Satellite System)G1和G2频率数据、欧盟伽利略卫星导航系统(GSNS，Galileo Satellite Navigation System)E1和E5频率数据、日本准天顶导航卫星系统(QZSS，Quasi Zenith Satellite System)L1和L2频率数据。
惯性测量单元2采用微机械传感器(MEMS，Micro-Electro-Mechanical SystemSensor)惯性测量单元。磁强计3采用地磁导航系统(GMNS，Geo-Magnetic NavigationSystem)。
该定位装置安装于载体上，通常为车载体。
基于三维空间坐标系，在设备安装前，首先通过建立以IMU1测量中心为坐标原点的满足右手坐标系定理的空间三维载体坐标系(body frame，简称为b系)，本方案中的b系采用前-右-下坐标系(forward-right-down,简称为F-R-D坐标系，其中x轴指向IMU1前方(F)，y轴指向右侧(R)，z轴垂直指向地面朝下(D))。然后在车体安装设备时，确保IMU1和磁强计3的x轴与载体的前进方向保持一致，y轴指向载体的右侧，z轴指向地面朝下。精确量取b系下GNSS天线和里程计4对于b系坐标原点的空间相对位置(即杆臂)，其中GNSS天线相对于b系坐标原点的杆臂为
Figure BDA0002306462150000072
里程计4相对于b坐标原点的杆臂为
Figure BDA0002306462150000071
其中T表示矩阵转置运算。
GNSS接收机1用于采集的GNSS数据，网络模块5用于接收的由国际GNSS服务组织(IGS，International GNSS Service Center)播发的GNSS运行数据，惯性测量单元2用于测量IMU数据，磁强计3用于测量磁强数据，里程计4用于测量里程数据。GNSS运行数据至少包括GNSS实时精密轨道、精密钟差、实时电离层数据。
FPGA主板6内集成有导航定位程序，用于实现导航定位，即计算GNSS接收机1采集的数据和IMU/磁强计/ODO采集的数据进入FPGA主板6的时间差，以此确定IMU/磁强计/ODO采集的数据的时间，完成GNSS/IMU/GMNS/ODO数据的时间同步工作。基于IGS播发的GNSS运行数据，对同步后的GNSS/IMU/磁强计/ODO数据进行融合处理，实现广域精密导航定位。
本发明提供两个实施例，分别为多源数据融合实时导航定位方法和多源数据融合实时导航定位系统。
实施例1：
图2为实施例1的多源数据融合实时导航定位方法的方法流程图。
参见图2，该导航定位方法包括：
步骤101：以GNSS接收机的时间为基准对惯性测量单元、磁强计和里程计的时间进行同步。
基于电子信号时间同步理论，在数据传输时，以GNSS时间(tocGNSS)为基准，通过比对IMU/磁强计/ODO信号传入FPGA主板时的本地时间(reciver_tsensors，m)与GNSS数据传入FPGA主板时的本地时间(reciver_tGNSS)之间的时间差：
Figure BDA0002306462150000081
计算各传感器的时间
Figure BDA0002306462150000082
根据式(2)计算出来的以GNSS时间为基准的IMU、磁强计、ODO数据初始时间tocsensors，IMU，0、tocsensors，GMNS，0和tocsensors，ODO，0。此后，IMU、磁强计、ODO数据的在k历元的时间，可根据式(2)中的时间和各传感器的每秒脉冲数(PPS，Pulse Per Second)进行计算：
Figure BDA0002306462150000083
其中，PPS表示传感器的每秒脉冲数。对同步完的数据，根据不同传感器的数据格式进行解码，以待后续数据处理之用。
步骤102：获取GNSS接收机采集的GNSS数据、网络模块接收的由国际GNSS服务组织播发的GNSS运行数据、惯性测量单元测量的IMU数据、磁强计测量的磁强数据和里程计测量的里程数据。
该步骤102获取的数据均是经过时间同步后的数据。
步骤103：基于最小二乘和序贯最小二乘理论，根据非差非组合精密单点定位(PPP，Precise Point Positioning)理论由GNSS数据以及GNSS运行数据计算载体当前位置和当前速度。
GNSS数据至少包括各导航系统测量的双频伪距、载波、多普勒。
该步骤需要首先对非差非组合PPP理论的状态参数Xk进行估计，然后计算载体当前位置和当前速度。
状态参数Xk的估计方程为：
Figure BDA0002306462150000091
式中，Xk＝[δxr δyr δzr ctr dwet DCBr N1 N2 I1]T，表示非差非组合PPP理论的状态参数，(δxr δyr δzr) 表示GNSS接收机位置改正数，tr、dwet和DCBr分别表示接收机钟差、对流层湿延迟和接收机码间偏差，N1、N2和I1分别表示第一个频率的载波模糊度、第二个频率的载波模糊度和第一个频率上的电离层延迟；Pk表示k历元时刻观测信息向量Zk对应的先验方差，由经验模型给定；Ak表示设计系数矩阵；
Figure BDA0002306462150000092
表示k-1历元时刻状态向量Xk-1对应的方差协方差矩阵。其中，非差非组合PPP理论的观测信息向量Zk由GNSS双频伪距(Pj)、载波(Lj)、多普勒(Dj)观测方程和电离层约束方程
Figure BDA0002306462150000093
构成：
Figure BDA0002306462150000094
Figure BDA0002306462150000095
Figure BDA0002306462150000101
Figure BDA0002306462150000102
其中，Pj、Lj和Dj分别表示第j个频率(各频率分别为BDSB1/B2、GPSL1/L2、GLONASSG1/G2、GalileoE1/E5、QZSSL1/L2)上的伪距、载波和多普勒观测值；角标s和r分别表示卫星和接收机；c表示真空中的光速；ts和tr分别表示信号发射时刻卫星端钟差和信号接收时刻接收机端钟差；
Figure BDA0002306462150000105
和
Figure BDA0002306462150000106
分别表示第j个频率上的信号由卫星s传播到接收机r过程中的电离层延迟和对流层延迟；dr，j和
Figure BDA0002306462150000107
分别表示由卫星和接收机硬件引起第j个频率上伪距观测值的时间延迟；
Figure BDA0002306462150000103
和
Figure BDA0002306462150000104
分别表示由卫星轨道偏心率引起的相对论效应误差、地球自转引起的误差、卫星和接收机天线相位中心偏差、天线相位中心变化误差、地球固体潮和海洋负荷潮误差、引力误差；λj和Nj表示第j个频率载波相位的波长和整周模糊度；br，j和
Figure BDA0002306462150000108
分别表示由卫星和接收机硬件延迟引起的相位时间延迟；εP，j、εL，j和εD，j分别表示伪距、载波和多普勒观测值中未模型化误差和接收机热噪声；
Figure BDA0002306462150000109
表示由天线旋转引起的载波相位缠绕误差，静态情况下一般由卫星天线旋转引起，动态情况下则接收机天线和卫星天线都会引起该项误差；ρ和
Figure BDA00023064621500001010
分别表示信号发射时刻到信号接收时刻的几何距离和距离变化率；
Figure BDA00023064621500001011
表示变化率；STEC和f1表示GNSS数据传播路径上总电子含量和GNSS第一个信号频率；
Figure BDA00023064621500001012
表示电离层先验模型误差。
步骤104：根据当前位置和当前速度实现位置初始化和速度初始化，根据IMU数据实现姿态初始化，得到位姿初始化结果。
该步骤104具体包括：
1.以当前位置作为位置初始化结果，以当前速度作为速度初始化结果。
2.根据IMU数据与地球自转角速度以及地球重力之间的姿态转换关系计算姿态方向余弦矩阵。
根据静态条件下，IMU在b系中测量的比力和角速度数据与导航坐标系(North-East-Down，简称N-E-D，记为navigationframe，即n系)中的地球自转角速度和地球重力之间存在的姿态旋转关系，来计算姿态方向余弦矩阵
Figure BDA0002306462150000111
Figure BDA0002306462150000112
Figure BDA0002306462150000113
Figure BDA0002306462150000114
式中，
Figure BDA0002306462150000116
和
Figure BDA0002306462150000117
分别表示静态条件下比力fb和角速度
Figure BDA0002306462150000118
的均值；gn表示n系下的地球重力；
Figure BDA0002306462150000119
表示n系下地心地固坐标系下(Earth centered earth fixedframe，e系)相对于惯性系(inertialframe，i系)的旋转角速度(即地球自转角速度)；
Figure BDA00023064621500001110
和
Figure BDA00023064621500001111
表示比力和角速度向量在n系和b系下构造出的第三个向量。利用式(9)计算的姿态余弦矩阵
Figure BDA00023064621500001112
计求解初始姿态角：
Figure BDA0002306462150000115
其中，θ、φ和ψ分别表示横滚角、俯仰角和航向角；atan是C/C++语言中用于计算反正切值的函数，返回值在-π/2～+π/2之间；atan2是C/C++语言中用于计算按象限区分的反正切值的函数，其返回值在-π～+π之间；
Figure BDA00023064621500001113
表示
Figure BDA00023064621500001114
中第k1行、第k2列的元素。
3.根据姿态方向余弦矩阵计算初始姿态角，得到姿态初始化结果。
步骤105：基于位姿初始化结果利用惯性导航系统力学编排理论进行惯性导航状态更新，得到位姿更新结果。
基于INS(惯性导航系统，InertialNavigationSystem)力学编排理论和位姿初始化结果，根据惯性力学编排理论模型进行INS状态更新：
Figure BDA0002306462150000121
Figure BDA0002306462150000122
Figure BDA0002306462150000123
其中vn和pn分别表示n系下载体速度和位置；
Figure BDA0002306462150000124
和
Figure BDA0002306462150000125
表示速度、位置和姿态旋转矩阵的微分形式；vD和h表示n系下垂向速度和大地高；
Figure BDA0002306462150000126
表示n系相对于e系的旋转角速度；
Figure BDA0002306462150000127
表示n系下n系相对于i系的旋转角速度。
步骤106：检查每个惯性测量单元的测量时刻是否获取到GNSS数据、磁强数据或里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或卡尔曼滤波的观测更新从而得到参数改正数向量。由于IMU的采样率高于GNSS接收机、磁强计和ODO，因此不一定能够在惯性测量单元的每个测量时刻均能采集到GNSS数据、磁强数据或里程数据。
该步骤106具体为：
1.判断每个惯性测量单元的测量时刻是否获取到GNSS数据、磁强数据或里程数据。
2.针对未获取到GNSS数据、磁强数据和里程数据的时刻，以位姿更新结果作为该时刻的导航结果，并构建卡尔曼滤波时间更新方程和协方差矩阵；时间更新方程的状态参数向量的分量包括位置信息、速度信息、姿态信息、零偏、比例因子误差、接收机钟差、接收机钟漂、对流层湿延迟、接收机码间偏差、载波模糊度和电离层延迟；
此时式(13)～(15)的结果即为本次历元时刻的导航结果。根据卡尔曼滤波时间更新方程：
Xk＝Φk，k-1Xk-1+μk-1，μk-1～(0，Qk-1) (16)
计算对应的协方差矩阵：
Figure BDA0002306462150000131
式中Φk，k-1表示状态向量Xk的设计系数矩阵和状态转移矩阵；μk-1表示状态噪声，其先验方差为Qk-1；
Figure BDA0002306462150000137
表示k-1历元的状态参数对应的方差协方差矩阵。
在紧组合模式下，状态参数向量Xk可表示为：
Figure BDA0002306462150000138
其中，δ表示对应参数的改正数(例如：δpn表示位置改正数)，B和S表示IMU的零偏和比例因子误差，g和a则表示陀螺仪(gyroscope)和加速度计(accelerometer)；Ψ表示姿态矩阵；状态转移矩阵Φk，k-1根据状态参数的动力学函数模型确定。
3.对状态参数向量中的各个分量进行建模，从而结合卡尔曼滤波时间更新方程和协方差矩阵得到建立好的卡尔曼滤波时间更新模型。对各个分量进行建模具体包括：
1)位置、速度和姿态采用如式(19)～(21)所示的动力学函数模型进行描述：
Figure BDA0002306462150000132
Figure BDA0002306462150000133
Figure BDA0002306462150000134
2)IMU零偏和比例因子误差建模为一阶高斯马尔科夫过程：
Figure BDA0002306462150000135
式中，τ和T分别表IMU采样间隔和IMU误差对应的高斯-马尔科夫过程的相关时间；ωS，k-1和ωB，k-1分别表示比例因子误差和零偏误差的驱动噪声(传感厂家给定或者通过转台标定得到)。
3)接收机钟差和钟漂则建模为：
Figure BDA0002306462150000136
其中tr，k与tr，k-1分别为k时刻和k-1时刻的接收机钟差，
Figure BDA0002306462150000139
与
Figure BDA00023064621500001310
分别为k时刻和k-1时刻的接收机钟漂；dt为采样间隔，εt，k-1与
Figure BDA00023064621500001311
分别为接收机钟差噪声和接收机钟漂噪声。
4)dwet、DCBr和I1建模为随机游走过程：
dwet，k＝dwet，k-1+εdwet，k-1 (24)
DCBr，k＝DCBr，k-1+εDCB，k-1 (25)
I1，k＝I1，k-1+εI1，k-1 (26)
式中εdwet，k-1、εDCB，k-1和εI1，k-1分别表示接收机对流层湿延迟、接收机DCB和电离层噪声。
4)模糊度建模为随机常数：
Nk＝Nk-1 (27)
根据式(16)～(24)，可获得系数矩阵Φk，k-1。
4.针对获取到GNSS数据、磁强数据或里程数据的时刻，根据GNSS数据、GNSS运行数据、IMU数据、磁强数据、里程数据以及载体状态约束观测量构建观测新息向量，进而构建卡尔曼滤波观测更新方程，得到建立好的卡尔曼滤波观测更新模型。
首先基于地磁导航理论模型和里程计测量原理，基于磁强计数据和AHRS算法，计算姿态角信息θGMNS，φGMNS，ψGMNS；同时基于载体里程计数据，计算载体在b系下沿F方向的运动速度vODO。
然后构建卡尔曼滤波观测更新方程：
Zk＝HkXk+ηk，ηk～N(0，Rk) (28)
式中Zk、ηk、Rk分别表示紧组合中观测值新息向量、观测新息噪声、观测噪声强度；Hk表示状态向量Xk的设计系数矩阵。在紧组合模式下，观测新息向量Zk由直接观测量(GNSS观测信息、磁强计姿态信息、里程计测速信息)和载体状态约束观测量(零速修正和非完整性约束)两部分组成。其中，若有直接观测量，则根据当前IMU时刻观测到的数据，分别构建对应的观测新息向量：
若存在GNSS观测数据，则GNSS观测数据对应的新息向量为：
Figure BDA0002306462150000151
式中
Figure BDA0002306462150000155
和
Figure BDA0002306462150000156
表示由INS预测出的s卫星上第一个频率上的伪距和第二个频率上的伪距；
Figure BDA0002306462150000157
和
Figure BDA0002306462150000158
表示由GNSS接收机观测到的s卫星上第一个频率上的伪距和第二个频率上的伪距；
Figure BDA0002306462150000159
和
Figure BDA00023064621500001510
表示由INS预测出的s卫星上第一个频率上的伪距和第二个频率上的伪距；
Figure BDA00023064621500001511
和
Figure BDA00023064621500001512
表示由GNSS接收机观测到的s卫星上第一个频率上的伪距和第二个频率上的伪距；
Figure BDA00023064621500001513
和
Figure BDA00023064621500001514
表示由INS预测出的s卫星上第一个频率上的多普勒和GNSS观察侧到的多普勒；
Figure BDA00023064621500001515
和
Figure BDA00023064621500001516
表示由INS预测出的s卫星上第一个频率上的电离层延迟和全球电离层模型(GIM，Global IonosphereModel)计算的电离层延迟。
若存在磁强计姿态信息时，则磁强计观测数据对应的新息向量为：
Figure BDA0002306462150000152
式中，θINS、φINS和ψINS分别表示由INS力学编排计算得到的横滚角、俯仰角和航向角；θGMNS、φGMNS和ψGMNS分别表示根据磁强计数据计算的横滚角、俯仰角和航向角。
若存在ODO速度信息时，则ODO观测数据对应的新息向量为：
Figure BDA0002306462150000153
式中，vI NS，F、vI NS，R和vI NS，D分别表示由INS力学编排计算得到的前向速度、右向速度和垂向速度；vODO，F表示由ODO计算的前向速度。
若不存在以上直接观测信息，运动状态下，观测新息向量由非完整性约束(车体在运动状态下，只存在沿前进方向F的速度，而在车的横向和垂向不存在运动即为0)条件构成，新息观测方程为：
Figure BDA0002306462150000154
静止状态下，观测新息向量由零速修正(车体在静止状态下，其速度应为0)条件构成，新息观测方程为：
Figure BDA0002306462150000161
根据式(29)～(33)，得到多源数据融合紧组合模式的观测新息向量Zk可表示为：
Figure BDA0002306462150000162
设计系数矩阵Hk则根据误差扰动理论，通过对式(34)进行误差扰动
Figure BDA0002306462150000163
获得，式中
Figure BDA0002306462150000164
表示求偏导。
5.基于扩展卡尔曼滤波参数估计理论，根据卡尔曼滤波时间更新模型和卡尔曼滤波观测更新模型对状态参数向量和协方差矩阵进行最优估计，得到最优状态参数向量和最优协方差矩阵。
具体为：基于扩展卡尔曼滤波参数估计理论和状态转移矩阵、观测新息向量和设计系数矩阵，计算状态参数向量的最优估值
Figure BDA0002306462150000168
及其对应的方差协方差阵
Figure BDA0002306462150000169
Figure BDA0002306462150000165
Figure BDA0002306462150000166
Figure BDA0002306462150000167
式中，I表示单位矩阵。
6.基于最优状态参数向量和最优协方差矩阵计算参数改正数向量。
步骤107：基于参数改正数向量，利用误差反馈理论对位姿更新结果进行修正，得到导航定位信息。
根据以上方案，可实现载体高采样、高精度、连续的三维位置、速度和姿态信息计算，该技术方案可作为无人/辅助驾驶的技术支撑。
实施例2：
该实施例2提供一种多源数据融合实时导航定位系统。
图3为本发明实施例2的多源数据融合实时导航定位系统的系统结构图。
参见图3，该导航定位系统包括：
时间同步模块201，用于以GNSS接收机的时间为基准对惯性测量单元、磁强计和里程计的时间进行同步。
数据获取模块202，用于获取GNSS接收机采集的GNSS数据、网络模块接收的由国际GNSS服务组织播发的GNSS运行数据、惯性测量单元测量的IMU数据、磁强计测量的磁强数据和里程计测量的里程数据。
位置初步估计模块203，用于基于最小二乘和序贯最小二乘理论，根据非差非组合精密单点定位理论由GNSS数据以及GNSS运行数据计算载体当前位置和当前速度。
位姿初始化模块204，用于根据当前位置和当前速度实现位置初始化和速度初始化，根据IMU数据实现姿态初始化，得到位姿初始化结果。
惯性导航状态更新模块205，用于基于位姿初始化结果利用惯性导航系统力学编排理论进行惯性导航状态更新，得到位姿更新结果。
卡尔曼滤波模块206，用于检查每个惯性测量单元的测量时刻是否获取到GNSS数据、磁强数据或里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或卡尔曼滤波的观测更新从而得到参数改正数向量。
修正模块207，用于基于参数改正数向量，利用误差反馈理论对位姿更新结果进行修正，得到导航定位信息。
可选的，卡尔曼滤波模块206包括：
判断单元，用于判断每个惯性测量单元的测量时刻是否获取到GNSS数据、磁强数据或里程数据；
时间更新方程建立单元，用于针对未获取到GNSS数据、磁强数据和里程数据的时刻，以位姿更新结果作为该时刻的导航结果，并构建卡尔曼滤波时间更新方程和协方差矩阵；时间更新方程的状态参数向量的分量包括位置信息、速度信息、姿态信息、零偏、比例因子误差、接收机钟差、接收机钟漂、对流层湿延迟、接收机码间偏差、载波模糊度和电离层延迟；
状态参数向量建模单元，用于将位置信息、速度信息、姿态信息采用动力学函数模型进行描述；将零偏和比例因子误差建模为一阶高斯马尔科夫过程；将对流层湿延迟、接收机码间偏差和电离层延迟建模为随机游走过程；将载波模糊度建模为随机常数；将接收机钟差和接收机钟漂建模为
Figure BDA0002306462150000181
从而得到建立好的卡尔曼滤波时间更新模型；其中tr，k与tr，k-1分别为k时刻和k-1时刻的接收机钟差，
Figure BDA0002306462150000182
与
Figure BDA0002306462150000183
分别为k时刻和k-1时刻的接收机钟漂；dt为采样间隔，εt，k-1与
Figure BDA0002306462150000184
分别为接收机钟差噪声和接收机钟漂噪声；
观测更新方程建立单元，用于针对获取到GNSS数据、磁强数据或里程数据的时刻，根据GNSS数据、GNSS运行数据、IMU数据、磁强数据、里程数据以及载体状态约束观测量构建观测新息向量，进而构建卡尔曼滤波观测更新方程，得到建立好的卡尔曼滤波观测更新模型；
滤波单元，用于基于扩展卡尔曼滤波参数估计理论，根据卡尔曼滤波时间更新模型和卡尔曼滤波观测更新模型对状态参数向量和协方差矩阵进行最优估计，得到最优状态参数向量和最优协方差矩阵；
改正量计算单元，用于基于最优状态参数向量和最优协方差矩阵计算参数改正数向量。
可选的，位姿初始化模块204包括：
位置速度初始化单元，用于以当前位置作为位置初始化结果，以当前速度作为速度初始化结果；
姿态方向余弦矩阵计算单元，用于根据IMU数据与地球自转角速度以及地球重力之间的姿态转换关系计算姿态方向余弦矩阵；
姿态初始化单元，用于根据姿态方向余弦矩阵计算初始姿态角，得到姿态初始化结果。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
(1)采用多系统GNSS(BDS/GPS/GLONASS/Galileo/QZSS)，增强卫星空间分布，改善用户可观测卫星数和卫星空间几何构型；相对于GPS系统，多GNSS系统的应用将进一步增加可观测卫星数和优化卫星星座结构，使可观测卫星数从GPS时的最多14颗提高到多系统下的40多颗，这将极大改善目前GPS的定位几何精度因子(PDOP，Position Dilution OfPrecision)分布。
(2)多系统GNSS与主动导航系统(本方案采用INS)进行融合，弥补GNSS导航定位性能受观测环境影响的缺点；在给定载体初始状态信息后，INS通过惯性导航算法处理IMU采集的载体角加速度和线加速度信息，即可为用户提供载体高采样率、连续的位置、姿态、速度信息。INS的这些优点(自主导航定位、数据采样率高(数百赫兹)、短期精度高)是GNSS导航定位中所欠缺的。
(3)采用多源传感器数据和利用载体运动的特点来增强GNSS/INS组合导航定位结果的可靠性和稳定性。本方案为克服GNSS/INS组合的弱点，采用磁强计和里程计数据来增加对车体姿态和速度的观测，同时利用载体运动本身的特点来增强GNSS/INS导航结果的精度和可靠性。
(4)采用精密数据处理方法来保证导航定位结果的精度。由于GNSS/INS/磁强计/ODO组合技术中，其绝对定位精度主要受GNSS数据处理模式影响。多系统GNSS实时PPP技术采用单台接收机双频GNSS数据，利用IGS提供的卫星实时精密产品即可保证厘米级的导航定位结果。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种多源数据融合实时导航定位方法，其特征在于，应用于一种多源数据融合实时导航定位装置，该定位装置包括：GNSS接收机、惯性测量单元、磁强计、里程计、网络模块和FPGA主板；所述GNSS接收机、所述惯性测量单元、所述磁强计、所述里程计和所述网络模块均与所述FPGA主板连接；
该导航定位方法包括：
获取所述GNSS接收机采集的GNSS数据、所述网络模块接收的由国际GNSS服务组织播发的GNSS运行数据、所述惯性测量单元测量的IMU数据、所述磁强计测量的磁强数据和所述里程计测量的里程数据；
基于最小二乘和序贯最小二乘理论，根据非差非组合精密单点定位理论由所述GNSS数据以及所述GNSS运行数据计算载体当前位置和当前速度；
根据所述当前位置和所述当前速度实现位置初始化和速度初始化，根据所述IMU数据实现姿态初始化，得到位姿初始化结果；
基于所述位姿初始化结果利用惯性导航系统力学编排理论进行惯性导航状态更新，得到位姿更新结果；
检查每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或卡尔曼滤波的观测更新从而得到参数改正数向量；
基于所述参数改正数向量，利用误差反馈理论对位姿更新结果进行修正，得到导航定位信息。
2.根据权利要求1所述的多源数据融合实时导航定位方法，其特征在于，所述检查每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或卡尔曼滤波的观测更新从而得到参数改正数向量，具体包括：
判断每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据；
针对未获取到所述GNSS数据、所述磁强数据和所述里程数据的时刻，以所述位姿更新结果作为该时刻的导航结果，并构建卡尔曼滤波时间更新方程和协方差矩阵；所述时间更新方程的状态参数向量的分量包括位置信息、速度信息、姿态信息、零偏、比例因子误差、接收机钟差、接收机钟漂、对流层湿延迟、接收机码间偏差、载波模糊度和电离层延迟；
将所述位置信息、所述速度信息、所述姿态信息采用动力学函数模型进行描述；将所述零偏和所述比例因子误差建模为一阶高斯马尔科夫过程；将所述对流层湿延迟、所述接收机码间偏差和电离层延迟建模为随机游走过程；将所述载波模糊度建模为随机常数；将所述接收机钟差和所述接收机钟漂建模为
Figure FDA0002306462140000021
从而得到建立好的卡尔曼滤波时间更新模型；其中tr，k与tr，k-1分别为k时刻和k-1时刻的接收机钟差，
Figure FDA0002306462140000022
与
Figure FDA0002306462140000023
分别为k时刻和k-1时刻的接收机钟漂；dt为采样间隔，εt，k-1与
Figure FDA0002306462140000024
分别为接收机钟差噪声和接收机钟漂噪声；
针对获取到所述GNSS数据、所述磁强数据或所述里程数据的时刻，根据所述GNSS数据、所述GNSS运行数据、所述IMU数据、所述磁强数据、所述里程数据以及载体状态约束观测量构建观测新息向量，进而构建卡尔曼滤波观测更新方程，得到建立好的卡尔曼滤波观测更新模型；
基于扩展卡尔曼滤波参数估计理论，根据所述卡尔曼滤波时间更新模型和所述卡尔曼滤波观测更新模型对所述状态参数向量和所述协方差矩阵进行最优估计，得到最优状态参数向量和最优协方差矩阵；
基于所述最优状态参数向量和所述最优协方差矩阵计算所述参数改正数向量。
3.根据权利要求1所述的多源数据融合实时导航定位方法，其特征在于，所述根据所述当前位置和所述当前速度实现位置初始化和速度初始化，根据所述IMU数据实现姿态初始化，得到位姿初始化结果，具体包括：
以所述当前位置作为位置初始化结果，以所述当前速度作为速度初始化结果；
根据所述IMU数据与地球自转角速度以及地球重力之间的姿态转换关系计算姿态方向余弦矩阵；
根据所述姿态方向余弦矩阵计算初始姿态角，得到姿态初始化结果。
4.根据权利要求1所述的多源数据融合实时导航定位方法，其特征在于，在所述获取所述GNSS接收机采集的GNSS数据、所述网络模块接收的由国际GNSS服务组织播发的GNSS运行数据、所述惯性测量单元测量的IMU数据、所述磁强计测量的磁强数据和所述里程计测量的里程数据之前，还包括：
以GNSS接收机的时间为基准对所述惯性测量单元、所述磁强计和所述里程计的时间进行同步。
5.一种多源数据融合实时导航定位系统，其特征在于，应用于一种多源数据融合实时导航定位装置，该定位装置包括：GNSS接收机、惯性测量单元、磁强计、里程计、网络模块和FPGA主板；所述GNSS接收机、所述惯性测量单元、所述磁强计、所述里程计和所述网络模块均与所述FPGA主板连接；
该导航定位系统包括：
数据获取模块，用于获取所述GNSS接收机采集的GNSS数据、所述网络模块接收的由国际GNSS服务组织播发的GNSS运行数据、所述惯性测量单元测量的IMU数据、所述磁强计测量的磁强数据和所述里程计测量的里程数据；
位置初步估计模块，用于基于最小二乘和序贯最小二乘理论，根据非差非组合精密单点定位理论由所述GNSS数据以及所述GNSS运行数据计算载体当前位置和当前速度；
位姿初始化模块，用于根据所述当前位置和所述当前速度实现位置初始化和速度初始化，根据所述IMU数据实现姿态初始化，得到位姿初始化结果；
惯性导航状态更新模块，用于基于所述位姿初始化结果利用惯性导航系统力学编排理论进行惯性导航状态更新，得到位姿更新结果；
卡尔曼滤波模块，用于检查每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据，根据检查结果选择进行卡尔曼滤波的时间更新或卡尔曼滤波的观测更新从而得到参数改正数向量；
修正模块，用于基于所述参数改正数向量，利用误差反馈理论对位姿更新结果进行修正，得到导航定位信息。
6.根据权利要求5所述的多源数据融合实时导航定位系统，其特征在于，所述卡尔曼滤波模块包括：
判断单元，用于判断每个所述惯性测量单元的测量时刻是否获取到所述GNSS数据、所述磁强数据或所述里程数据；
时间更新方程建立单元，用于针对未获取到所述GNSS数据、所述磁强数据和所述里程数据的时刻，以所述位姿更新结果作为该时刻的导航结果，并构建卡尔曼滤波时间更新方程和协方差矩阵；所述时间更新方程的状态参数向量的分量包括位置信息、速度信息、姿态信息、零偏、比例因子误差、接收机钟差、接收机钟漂、对流层湿延迟、接收机码间偏差、载波模糊度和电离层延迟；
状态参数向量建模单元，用于将所述位置信息、所述速度信息、所述姿态信息采用动力学函数模型进行描述；将所述零偏和所述比例因子误差建模为一阶高斯马尔科夫过程；将所述对流层湿延迟、所述接收机码间偏差和电离层延迟建模为随机游走过程；将所述载波模糊度建模为随机常数；将所述接收机钟差和所述接收机钟漂建模为
Figure FDA0002306462140000041
从而得到建立好的卡尔曼滤波时间更新模型；其中tr，k与tr，k-1分别为k时刻和k-1时刻的接收机钟差，
Figure FDA0002306462140000042
与
Figure FDA0002306462140000043
分别为k时刻和k-1时刻的接收机钟漂；dt为采样间隔，εt，k-1与
Figure FDA0002306462140000044
分别为接收机钟差噪声和接收机钟漂噪声；
观测更新方程建立单元，用于针对获取到所述GNSS数据、所述磁强数据或所述里程数据的时刻，根据所述GNSS数据、所述GNSS运行数据、所述IMU数据、所述磁强数据、所述里程数据以及载体状态约束观测量构建观测新息向量，进而构建卡尔曼滤波观测更新方程，得到建立好的卡尔曼滤波观测更新模型；
滤波单元，用于基于扩展卡尔曼滤波参数估计理论，根据所述卡尔曼滤波时间更新模型和所述卡尔曼滤波观测更新模型对所述状态参数向量和所述协方差矩阵进行最优估计，得到最优状态参数向量和最优协方差矩阵；
改正量计算单元，用于基于所述最优状态参数向量和所述最优协方差矩阵计算所述参数改正数向量。
7.根据权利要求5所述的多源数据融合实时导航定位系统，其特征在于，所述位姿初始化模块包括：
位置速度初始化单元，用于以所述当前位置作为位置初始化结果，以所述当前速度作为速度初始化结果；
姿态方向余弦矩阵计算单元，用于根据所述IMU数据与地球自转角速度以及地球重力之间的姿态转换关系计算姿态方向余弦矩阵；
姿态初始化单元，用于根据所述姿态方向余弦矩阵计算初始姿态角，得到姿态初始化结果。
8.根据权利要求5所述的多源数据融合实时导航定位系统，其特征在于，还包括：
时间同步模块，用于以GNSS接收机的时间为基准对所述惯性测量单元、所述磁强计和所述里程计的时间进行同步。
CN201911241794.XA 2019-12-06 2019-12-06 多源数据融合实时导航定位方法及系统 Pending CN110988951A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911241794.XA CN110988951A (zh) 2019-12-06 2019-12-06 多源数据融合实时导航定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911241794.XA CN110988951A (zh) 2019-12-06 2019-12-06 多源数据融合实时导航定位方法及系统

Publications (1)

Publication Number Publication Date
CN110988951A true CN110988951A (zh) 2020-04-10

Family

ID=70090761

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911241794.XA Pending CN110988951A (zh) 2019-12-06 2019-12-06 多源数据融合实时导航定位方法及系统

Country Status (1)

Country Link
CN (1) CN110988951A (zh)

Cited By (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111412912A (zh) * 2020-04-14 2020-07-14 上海华测导航技术股份有限公司 导航板卡、用于导航板卡的多源数据融合方法以及运载体
CN111721298A (zh) * 2020-06-24 2020-09-29 重庆赛迪奇智人工智能科技有限公司 一种slam室外大场景精准定位方法
CN111812697A (zh) * 2020-05-25 2020-10-23 浙江航天润博测控技术有限公司 基于多模式精密定位的组合定位测姿数据处理方法
CN111948686A (zh) * 2020-08-05 2020-11-17 航天恒星科技有限公司 时间同步方法及装置
CN111982106A (zh) * 2020-08-28 2020-11-24 北京信息科技大学 导航方法、装置、存储介质及电子装置
CN112097763A (zh) * 2020-08-28 2020-12-18 西北工业大学 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN112346084A (zh) * 2020-10-26 2021-02-09 上海感探号信息科技有限公司 一种汽车定位方法、系统、电子设备及存储介质
CN112985387A (zh) * 2021-02-01 2021-06-18 中北大学 一种gnss与imu的时间同步方法以及滑雪同步检测系统
CN113222092A (zh) * 2021-06-07 2021-08-06 北京迈克恒通科贸发展有限公司 轮胎全生命周期管理方法及系统
CN113434427A (zh) * 2021-07-06 2021-09-24 和芯星通科技(北京)有限公司 一种实现测试验证的方法、装置、计算机存储介质及终端
CN114167470A (zh) * 2021-10-29 2022-03-11 华为技术有限公司 一种数据处理方法和装置
FR3113943A1 (fr) * 2020-09-09 2022-03-11 Commissariat à l'Energie Atomique et aux Energies Alternatives Procédé de détermination de la position et de l’orientation d’un véhicule.
CN114295122A (zh) * 2021-12-02 2022-04-08 河北汉光重工有限责任公司 用于嵌入式系统的sins_gnss时间同步方法及系统
CN114384569A (zh) * 2022-01-12 2022-04-22 腾讯科技（深圳）有限公司 终端定位方法、装置、设备以及介质
CN114518587A (zh) * 2022-02-18 2022-05-20 中国地质大学（北京） 一种室内外无缝定位系统及方法
CN114623827A (zh) * 2022-03-04 2022-06-14 北京邮电大学 室内终端定位方法、系统、装置以及电子设备
CN114754765A (zh) * 2021-01-08 2022-07-15 上海汽车集团股份有限公司 一种车辆三维定位方法和系统
CN114877892A (zh) * 2022-07-11 2022-08-09 泉州通维科技有限责任公司 一种用于光伏机器人的融合定位方法
CN114923482A (zh) * 2022-05-17 2022-08-19 东南大学 一种基于多状态约束的uwb-imu组合室内定位方法
CN115164886A (zh) * 2022-07-22 2022-10-11 吉林大学 车载gnss/ins组合导航系统比例因子误差补偿方法
CN115166802A (zh) * 2022-08-10 2022-10-11 广东汇天航空航天科技有限公司 飞行器定位方法、装置及电子设备
CN116045976A (zh) * 2023-01-16 2023-05-02 浙江零跑科技股份有限公司 位姿确定方法及相关装置、电子设备和存储介质
CN116134293A (zh) * 2020-06-17 2023-05-16 阿斯特导航公司 磁导航设备的操作模式
CN116295436A (zh) * 2023-03-31 2023-06-23 武汉大学 一种低成本嵌入式gnss/ins组合导航时间同步算法
CN116718153A (zh) * 2023-08-07 2023-09-08 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统
CN117031513A (zh) * 2023-08-11 2023-11-10 中国地质大学（北京） 一种道路及附属物的实时监测定位方法、系统及装置
CN112946693B (zh) * 2021-02-03 2024-01-23 中国人民解放军61540部队 一种卫星导航系统的系统时间偏差的确定方法及系统
CN120232429A (zh) * 2025-04-18 2025-07-01 武汉大学 基于gnss/mems imu/磁力计组合的风力发电塔位姿监测方法及系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106969762A (zh) * 2017-01-12 2017-07-21 广州市泰斗鑫信息科技有限公司 一种用于GNSS+INS+odo的组合导航方法
CN108226985A (zh) * 2017-12-25 2018-06-29 北京交通大学 基于精密单点定位的列车组合导航方法
CN108344415A (zh) * 2018-01-30 2018-07-31 北京大学 一种组合导航信息融合方法
CN108549397A (zh) * 2018-04-19 2018-09-18 武汉大学 基于二维码和惯导辅助的无人机自主降落方法及系统
US20180328736A1 (en) * 2016-05-16 2018-11-15 Northrop Grumman Systems Corporation Vision-aided aerial navigation
US20190079534A1 (en) * 2017-09-13 2019-03-14 TuSimple Neural network architecture system for deep odometry assisted by static scene optical flow
CN109873827A (zh) * 2019-03-05 2019-06-11 长安大学 车路协同系统及其数据安全传输方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180328736A1 (en) * 2016-05-16 2018-11-15 Northrop Grumman Systems Corporation Vision-aided aerial navigation
CN106969762A (zh) * 2017-01-12 2017-07-21 广州市泰斗鑫信息科技有限公司 一种用于GNSS+INS+odo的组合导航方法
US20190079534A1 (en) * 2017-09-13 2019-03-14 TuSimple Neural network architecture system for deep odometry assisted by static scene optical flow
CN108226985A (zh) * 2017-12-25 2018-06-29 北京交通大学 基于精密单点定位的列车组合导航方法
CN108344415A (zh) * 2018-01-30 2018-07-31 北京大学 一种组合导航信息融合方法
CN108549397A (zh) * 2018-04-19 2018-09-18 武汉大学 基于二维码和惯导辅助的无人机自主降落方法及系统
CN109873827A (zh) * 2019-03-05 2019-06-11 长安大学 车路协同系统及其数据安全传输方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
WOO JUNG PARK: "MEMS 3D DR/GPS Integrated System for Land Vehicle Application Robust to GPS Outages", 《IEEE ACCESS》 *
ZHOUZHENG GAO: "Odometer and MEMS IMU enhancing PPP under weak satellite observability", 《ADVANCES IN SPACE RESEARCH》 *
刘天雄: "《卫星导航系统概论》", 30 November 2018 *
李增科: "自适应联邦滤波器在GPS-INS-Odometer组合导航的应用", 《测绘学报》 *
王世达: "GNSS/INS组合导航模型研究与性能分析", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
高周正: "多模GNSS PPP/INS组合系统算法与度用研究", 《中国博士学位论文全文数据库 基础科学辑》 *

Cited By (39)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111412912A (zh) * 2020-04-14 2020-07-14 上海华测导航技术股份有限公司 导航板卡、用于导航板卡的多源数据融合方法以及运载体
CN111812697A (zh) * 2020-05-25 2020-10-23 浙江航天润博测控技术有限公司 基于多模式精密定位的组合定位测姿数据处理方法
CN111812697B (zh) * 2020-05-25 2024-05-31 浙江航天润博测控技术有限公司 基于多模式精密定位的组合定位测姿数据处理方法
CN116134293A (zh) * 2020-06-17 2023-05-16 阿斯特导航公司 磁导航设备的操作模式
CN111721298A (zh) * 2020-06-24 2020-09-29 重庆赛迪奇智人工智能科技有限公司 一种slam室外大场景精准定位方法
CN111948686A (zh) * 2020-08-05 2020-11-17 航天恒星科技有限公司 时间同步方法及装置
CN111948686B (zh) * 2020-08-05 2024-04-26 航天恒星科技有限公司 时间同步方法及装置
CN111982106A (zh) * 2020-08-28 2020-11-24 北京信息科技大学 导航方法、装置、存储介质及电子装置
CN112097763A (zh) * 2020-08-28 2020-12-18 西北工业大学 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN112097763B (zh) * 2020-08-28 2022-07-05 西北工业大学 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
US11892296B2 (en) 2020-09-09 2024-02-06 Commissariat à l'Energie Atomique et aux Energies Alternatives Method for determining the position and the orientation of a vehicle
FR3113943A1 (fr) * 2020-09-09 2022-03-11 Commissariat à l'Energie Atomique et aux Energies Alternatives Procédé de détermination de la position et de l’orientation d’un véhicule.
EP3967975A1 (fr) 2020-09-09 2022-03-16 Commissariat à l'énergie atomique et aux énergies alternatives Procédé de détermination de la position et de l'orientation d'un véhicule
CN112346084A (zh) * 2020-10-26 2021-02-09 上海感探号信息科技有限公司 一种汽车定位方法、系统、电子设备及存储介质
CN114754765A (zh) * 2021-01-08 2022-07-15 上海汽车集团股份有限公司 一种车辆三维定位方法和系统
CN112985387B (zh) * 2021-02-01 2023-08-11 中北大学 一种gnss与imu的时间同步方法以及滑雪同步检测系统
CN112985387A (zh) * 2021-02-01 2021-06-18 中北大学 一种gnss与imu的时间同步方法以及滑雪同步检测系统
CN112946693B (zh) * 2021-02-03 2024-01-23 中国人民解放军61540部队 一种卫星导航系统的系统时间偏差的确定方法及系统
CN113222092A (zh) * 2021-06-07 2021-08-06 北京迈克恒通科贸发展有限公司 轮胎全生命周期管理方法及系统
CN113434427A (zh) * 2021-07-06 2021-09-24 和芯星通科技(北京)有限公司 一种实现测试验证的方法、装置、计算机存储介质及终端
WO2023071442A1 (zh) * 2021-10-29 2023-05-04 华为技术有限公司 一种数据处理方法和装置
CN114167470A (zh) * 2021-10-29 2022-03-11 华为技术有限公司 一种数据处理方法和装置
CN114295122B (zh) * 2021-12-02 2024-06-11 河北汉光重工有限责任公司 用于嵌入式系统的sins_gnss时间同步方法及系统
CN114295122A (zh) * 2021-12-02 2022-04-08 河北汉光重工有限责任公司 用于嵌入式系统的sins_gnss时间同步方法及系统
CN114384569A (zh) * 2022-01-12 2022-04-22 腾讯科技（深圳）有限公司 终端定位方法、装置、设备以及介质
CN114518587A (zh) * 2022-02-18 2022-05-20 中国地质大学（北京） 一种室内外无缝定位系统及方法
CN114623827B (zh) * 2022-03-04 2025-08-26 北京邮电大学 室内终端定位方法、系统、装置以及电子设备
CN114623827A (zh) * 2022-03-04 2022-06-14 北京邮电大学 室内终端定位方法、系统、装置以及电子设备
CN114923482A (zh) * 2022-05-17 2022-08-19 东南大学 一种基于多状态约束的uwb-imu组合室内定位方法
CN114877892A (zh) * 2022-07-11 2022-08-09 泉州通维科技有限责任公司 一种用于光伏机器人的融合定位方法
CN115164886B (zh) * 2022-07-22 2023-09-05 吉林大学 车载gnss/ins组合导航系统比例因子误差补偿方法
CN115164886A (zh) * 2022-07-22 2022-10-11 吉林大学 车载gnss/ins组合导航系统比例因子误差补偿方法
CN115166802A (zh) * 2022-08-10 2022-10-11 广东汇天航空航天科技有限公司 飞行器定位方法、装置及电子设备
CN116045976A (zh) * 2023-01-16 2023-05-02 浙江零跑科技股份有限公司 位姿确定方法及相关装置、电子设备和存储介质
CN116295436A (zh) * 2023-03-31 2023-06-23 武汉大学 一种低成本嵌入式gnss/ins组合导航时间同步算法
CN116718153B (zh) * 2023-08-07 2023-10-27 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统
CN116718153A (zh) * 2023-08-07 2023-09-08 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统
CN117031513A (zh) * 2023-08-11 2023-11-10 中国地质大学（北京） 一种道路及附属物的实时监测定位方法、系统及装置
CN120232429A (zh) * 2025-04-18 2025-07-01 武汉大学 基于gnss/mems imu/磁力计组合的风力发电塔位姿监测方法及系统

Similar Documents

Publication Publication Date Title
CN110988951A (zh) 多源数据融合实时导航定位方法及系统
CN114894181B (zh) 一种高精度组合导航定位方法及装置
Li et al. P ^3-VINS: Tightly-Coupled PPP/INS/Visual SLAM Based on Optimization Approach.
US9488480B2 (en) Method and apparatus for improved navigation of a moving platform
US7409290B2 (en) Positioning and navigation method and system thereof
US7292185B2 (en) Attitude determination exploiting geometry constraints
US8577606B2 (en) Vehicle guidance and sensor bias determination
CN108594283B (zh) Gnss/mems惯性组合导航系统的自由安装方法
CN109613585A (zh) 一种对基站天线超短基线gnss双天线实时测向的方法
CN106405592B (zh) 车载北斗载波相位周跳检测与修复方法及系统
US8547276B2 (en) Positioning system and method
Vana et al. Low-cost, triple-frequency, multi-GNSS PPP and MEMS IMU integration for continuous navigation in simulated urban environments
CN115683094B (zh) 一种复杂环境下车载双天线紧耦合定位方法及系统
CN112088319A (zh) 用于根据车速确定一车辆位置的方法
Wen et al. Design and evaluation of gnss/ins tightly-coupled navigation software for land vehicles
Zhang et al. Performance analysis of a tightly coupled kalman filter for the integration of un-differenced GPS and inertial data
Cheng-wei et al. Performance analysis of MADOCA-enhanced tightly coupled PPP/IMU
CN115267858B (zh) 一种区域导航系统辅助的精密单点定位方法
Du et al. Integration of PPP GPS and low cost IMU
CN114353835A (zh) 惯性轨道测量仪动态校准系统、方法及其应用
WO2005050247A1 (ja) 位置検出装置及び方法
CN114280656A (zh) 一种gnss系统的测姿方法和系统
CN116660963B (zh) 一种隧道群场景下车辆定位方法
Guo Development of a Variable Sampling Rates GNSS/INS Tightly Coupled Navigation System Based on SDR
CN117782080A (zh) 基于PPP-B2b/INS的实时空基导航系统及方法

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20200410