CN114545472B - 一种gnss/ins组合***的导航方法和装置 - Google Patents
一种gnss/ins组合***的导航方法和装置 Download PDFInfo
- Publication number
- CN114545472B CN114545472B CN202210095337.XA CN202210095337A CN114545472B CN 114545472 B CN114545472 B CN 114545472B CN 202210095337 A CN202210095337 A CN 202210095337A CN 114545472 B CN114545472 B CN 114545472B
- Authority
- CN
- China
- Prior art keywords
- gnss
- carrier
- node
- ins
- zero
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 64
- 238000001514 detection method Methods 0.000 claims abstract description 64
- 230000003068 static effect Effects 0.000 claims abstract description 47
- 238000012937 correction Methods 0.000 claims abstract description 45
- 238000005259 measurement Methods 0.000 claims description 149
- 230000001133 acceleration Effects 0.000 claims description 29
- 238000012545 processing Methods 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 11
- 230000005484 gravity Effects 0.000 claims description 7
- 238000009825 accumulation Methods 0.000 claims description 6
- 230000001143 conditioned effect Effects 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 17
- 238000004364 calculation method Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 8
- 239000013598 vector Substances 0.000 description 8
- 238000004891 communication Methods 0.000 description 6
- 230000002829 reductive effect Effects 0.000 description 6
- 238000001914 filtration Methods 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 5
- 238000009499 grossing Methods 0.000 description 4
- 230000010354 integration Effects 0.000 description 4
- 238000010276 construction Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000000052 comparative effect Effects 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 239000005433 ionosphere Substances 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000036961 partial effect Effects 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
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/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
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)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明提出一种GNSS/INS组合***的导航方法和装置。所述方法包括:步骤S1、构造GNSS/INS组合***的零速检测统计量;步骤S2、构造GNSS/INS组合***的因子图,包括INS节点和GNSS节点,所述因子图表征GNSS/INS组合***在各个测量时刻的***状态量;步骤S3、基于零速检测统计量确定载体的行进状态:当载体为静止状态时,基于因子图构造零速校正因子节点,并利用零速校正因子节点对载体进行定位,以获取载体的位置和速度;当载体为非静止状态时,基于因子图对载体进行定位,以获取载体的位置和速度。
Description
技术领域
本发明属于卫星导航技术领域,尤其涉及一种GNSS(Global NavigationSatellite System,全球卫星导航***)/INS(Inertial Navigation System, 惯性导航***)组合***的导航方法和***。
背景技术
GNSS/INS紧组合模型通常采用GNSS接收机输出的原始观测信息(如伪 距、伪距率、多普勒等)作为测量值,通过贝叶斯滤波器,和INS中惯性 测量单元(InertialMeasurement Unit,IMU)输出的测量值进行融合滤 波,得到***最优的状态估计量。GNSS/INS组合导航***的紧组合模式相 比松组合模式,由于传感器之间的观测量不相关和GNSS不需要独立进行定 位解算,因此,在可见卫星数量少于4颗时仍可以工作,且具备较高的导航精度和***可用性;而相比深组合模式,又具备计算量小、实时性强、 硬件实现成本低的优点。因此紧组合模式以其适中的计算量和良好的导航 性能,成为目前GNSS/INS组合导航技术研究的热点。
然而在组合***中,存在GNSS接收机与IMU的测量数据不同步,以及 GNSS接收机测量值变差或不可用的问题,当使用传统的滤波方法时(如扩 展卡尔曼滤波、联邦卡尔曼滤波等),会出现由观测时刻不一致引起的估计 误差,或者由滤波模型不准确而出现的结果发散等问题。
2001年,Kschischang等人提出了因子图的概念,因子图是概率图模 型的一种表示方式,是一种用来表示全局函数和局部函数关系的模型图, 它被广泛应用于人工智能、信号处理、数字通信等领域,对各种复杂概率 问题进行求解。基于因子图建模的主要目的是将复杂的概率问题进行因式 化分解,即将复杂的全局函数分解为多个局部函数相乘的形式,当已知局 部函数之间的相关性时,只需要分析每个局部函数就可以得到全局复杂概率问题的解。因子图模型中含有因子节点与变量节点,两种节点通过局部 函数相连构成双向图。在组合导航***中,因子图通过各***中各传感器 的观测量建立当前状态和前一状态的联系,并构造代价函数,通过求解系 统状态量的后验概率的全局或者局部最大值,获得当前最优状态的估计值。 因子图能够解决组合导航***中各个传感器测量值的异步传输、即插即用 以及多速率数据的融合问题,同时也能解决状态方程或观测方程非线性的问题,具有良好的扩展性和灵活性。然而,随着观测量的不断增加,因子 图中产生的因子节点也会逐渐增多,每次新增因子节点,都需要重新求解 已有各个因子节点中状态量的最优估计值,这带来了巨大的计算和存储负 担,将导致***实时性不断下降。针对这个问题,有两种解决方法,第一 种方法是相同节点合并法,即通过检测算法将当前节点与过去节点进行比 较,把相同的节点合并为同一节点,从而控制图大小的增长;另一种方法 为节点移除法,将因子图中过去一些影响较小的节点进行移除,保证图的 稀疏性。上述两种方法虽然可以减缓图增大速度,但也存在不足,第一种 方法随着时间增加,检测代价会越来越高,且需要目标回到之前位置。而 第二种方式对节点移除过程比较繁琐,并且移除节点的选择本身就是一个 问题。
发明内容
为解决上述技术问题,本发明提出了一种GNSS/INS组合***的导航方 案,该方案涉及GNSS与INS组合导航***的紧组合模式下,利用测量信号 进行导航的方法和装置,特别适用于城市中行驶车辆的自动驾驶辅助导航 设备中。
该方案在扩展的增量平滑与建图方法(Incremental Smoothing and Mapping 2,iSAM2)iSAM2的基础上,结合GNSS/INS紧组合导航***;同 时在传统的利用IMU的三维加速度、姿态测量值的广义似然比零速检测的 基础上,增加了GNSS接收机的载体速度测量值作为检测量;检测到载体处 于零速时,不增加新的因子节点,有效减轻了因子图的存储量和计算量; IMU和GNSS接收机的测量值仍然对***状态进行约束和更新,保证了零速 时***状态值估计的正确性。进一步,在因子图计算过程中,将IMU测量 值进行预积分,预积分的时刻及时间间隔和GNSS接收机输出的测量值时刻 及间隔一致,保证了两种传感器输出测量数据的同步。进一步,选择观测 质量最好的一颗卫星测量值作为零速修正因子,保证了零速修正的可靠性。
本发明第一方面公开了一种GNSS/INS组合***的导航方法。所述方法 包括:
步骤S1、基于全球卫星导航***GNSS输出的测量值和惯性导航*** INS中的惯性测量单元IMU输出的测量信息,构造所述GNSS/INS组合*** 的零速检测统计量;
步骤S2、构造所述GNSS/INS组合***的因子图,所述因子图包括INS 节点和GNSS节点,所述INS节点为所述因子图的过程模型因子节点,所述 GNSS节点为所述因子图的测量模型因子节点,所述因子图表征所述 GNSS/INS组合***在各个测量时刻的***状态量;
步骤S3、基于所述零速检测统计量确定载体的行进状态:
当所述载体为静止状态时,基于所述因子图构造零速校正因子节 点,并利用所述零速校正因子节点对所述载体进行定位,以获取所述载体 的位置和速度;
当所述载体为非静止状态时,基于所述因子图对所述载体进行定 位,以获取所述载体的位置和速度。
根据本发明第一方面的方法,在所述步骤S1中,利用所述GNSS输出 的测量值计算载体速度,并基于所述载体速度和所述IMU输出的测量信息 构造如以下公式的广义似然比,以得到所述零速检测统计量;所述IMU输 出的测量信息为载体三维加速度和载体姿态信息;所述零速检测统计量的 表达式为:
其中,表示所述IMU测量的第k个三个轴向的加速度,表示所述IMU 测量的第k个三个轴向的角速度,表示由所述GNSS测量的第k个载体 速度,表示所述IMU的加速度测量方差,表示所述IMU的角速度测量 方差,表示由所述GNSS测量得到的所述载体速度测量方差,N为检测 窗口长度,g为重力常量,其中为所述检测窗口长度N内的加 速度平均值,当所述零速检测统计量小于门限γ时,所述载体处于所述静止 状态。
根据本发明第一方面的方法,在所述步骤S2中,构造所述因子图的所 述INS节点具体包括,基于所述GNSS/INS组合***在载体坐标系b下,在 测量时刻i,经过时间间隔T积分后得到的速度的变化量和姿态角的变 化量确定所述GNSS/INS组合***在地球地固坐标系e下,在测量时 刻i+1的速度估计值位置估计值和姿态角估计值以构造所 述INS节点在测量时刻i+1的状态量估计值
根据本发明第一方面的方法,在所述步骤S2中,构造所述因子图的所 述GNSS节点具体包括,基于所述GNSS节点测量得到的在所述测量时刻i 对第j颗可见卫星的伪距ρi,j、与所述伪距对应的伪距率第j颗可见卫 星在所述地球地固坐标系e下的位置和速度确定所述GNSS节点 的在所述测量时刻i的状态量
根据本发明第一方面的方法,在所述步骤S3中,当所述载体为所述静 止状态时,基于所述因子图构造所述零速校正因子节点包括,以所述载体 为所述静止状态为条件,约束所述INS节点的误差积累和所述GNSS节点的 时钟漂移,以确定所述零速校正因子节点,所述零速校正因子节点包括INS 零速校正因子节点hi ZUPT_INS和GNSS零速校正因子节点hi ZUPT_GNSS。
根据本发明第一方面的方法,在所述步骤S3中:
当所述载体为所述静止状态时,利用所述零速校正因子节点,计算所 述GNSS/INS组合***的状态量的最大后验概率值,以对所述载体进行定位;
当所述载体为非所述静止状态时,利用所述因子图中的所述INS节点 和所述GNSS节点,计算所述GNSS/INS组合***的状态量的最大后验概率 值,以对所述载体进行定位。
本发明第二方面公开了一种GNSS/INS组合***的导航装置。所述装置 包括:
第一处理单元,被配置为,基于全球卫星导航***GNSS输出的测量值 和惯性导航***INS中的惯性测量单元IMU输出的测量信息,构造所述 GNSS/INS组合***的零速检测统计量;
第二处理单元,被配置为,构造所述GNSS/INS组合***的因子图,所 述因子图包括INS节点和GNSS节点,所述INS节点为所述因子图的过程模 型因子节点,所述GNSS节点为所述因子图的测量模型因子节点,所述因子 图表征所述GNSS/INS组合***在各个测量时刻的***状态量;
第三处理单元,被配置为,基于所述零速检测统计量确定载体的行进 状态:
当所述载体为静止状态时,基于所述因子图构造零速校正因子节 点,并利用所述零速校正因子节点对所述载体进行定位,以获取所述载体 的位置和速度;
当所述载体为非静止状态时,基于所述因子图对所述载体进行定 位,以获取所述载体的位置和速度。
根据本发明第二方面的装置,所述第一处理单元具体被配置为,利用 所述GNSS输出的测量值计算载体速度,并基于所述载体速度和所述IMU输 出的测量信息构造如以下公式的广义似然比,以得到所述零速检测统计量; 所述IMU输出的测量信息为载体三维加速度和载体姿态信息;所述零速检 测统计量的表达式为:
其中,表示所述IMU测量的第k个三个轴向的加速度,表示所述IMU 测量的第k个三个轴向的角速度,表示由所述GNSS测量的第k个载体 速度,表示所述IMU的加速度测量方差,表示所述IMU的角速度测量 方差,表示由所述GNSS测量得到的所述载体速度测量方差,N为检测 窗口长度,g为重力常量,其中为所述检测窗口长度N内的加 速度平均值,当所述零速检测统计量小于门限γ时,所述载体处于所述静止 状态。
根据本发明第二方面的装置,所述第二处理单元具体被配置为,构造 所述因子图的所述INS节点具体包括,基于所述GNSS/INS组合***在载体 坐标系b下,在测量时刻i,经过时间间隔T积分后得到的速度的变化量和姿态角的变化量确定所述GNSS/INS组合***在地球地固坐标系e 下,在测量时刻i+1的速度估计值位置估计值和姿态角估计值以构造所述INS节点在测量时刻i+1的状态量估计值
根据本发明第二方面的装置,所述第二处理单元具体被配置为,构造 所述因子图的所述GNSS节点具体包括,基于所述GNSS节点测量得到的在 所述测量时刻i对第j颗可见卫星的伪距ρi,j、与所述伪距对应的伪距率 第j颗可见卫星在所述地球地固坐标系e下的位置和速度确定所述GNSS节点的在所述测量时刻i的状态量
根据本发明第二方面的装置,所述第三处理单元具体被配置为,当所 述载体为所述静止状态时,基于所述因子图构造所述零速校正因子节点包 括,以所述载体为所述静止状态为条件,约束所述INS节点的误差积累和 所述GNSS节点的时钟漂移,以确定所述零速校正因子节点,所述零速校正 因子节点包括INS零速校正因子节点hi ZUPT_INS和GNSS零速校正因子节点 hi ZUPT_GNSS。
根据本发明第二方面的装置,所述第三处理单元具体被配置为,当所 述载体为所述静止状态时,利用所述零速校正因子节点,计算所述GNSS/INS 组合***的状态量的最大后验概率值,以对所述载体进行定位;
当所述载体为非所述静止状态时,利用所述因子图中的所述INS节点 和所述GNSS节点,计算所述GNSS/INS组合***的状态量的最大后验概率 值,以对所述载体进行定位。
本发明第三方面公开了一种电子设备。所述电子设备包括存储器和处 理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时, 实现本公开第一方面中任一项所述的一种GNSS/INS组合***的导航方法中 的步骤。
本发明第四方面公开了一种计算机可读存储介质。所述计算机可读存 储介质上存储有计算机程序,所述计算机程序被处理器执行时,实现本公 开第一方面中任一项所述的一种GNSS/INS组合***的导航方法中的步骤。
本发明提供的技术方案与现有技术相比,优点在于:本发明在传统 的利用IMU输出的三维加速度、姿态测量值作为广义似然比零速检测量 的基础上,增加了GNSS接收机的载体速度测量值作为检测量,保证了检 测的稳定可靠及精确性。本发明在检测到载体处于零速时,不增加新的 因子节点,有效减轻了因子图的存储量和计算量;本发明在载体零速时, 将IMU和GNSS接收机的测量值对***状态进行约束和更新,保证了*** 状态值估计的正确性。相比传统因子图方法,本发明在组合***导航性 能不变的情况下,能有效降低因子图的存储量和计算量,特别适用于车 载GNSS/INS紧组合***在城市行驶中频繁启停的场景。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案下 面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介 绍,显而易见地,下面描述中的附图是本发明的一些实施方式,对于本 领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这 些附图获得其他的附图。
图1为根据本发明实施例的一种GNSS/INS组合***的导航方法的流 程图;
图2a为根据本发明比较例的因子图;
图2b为根据本发明实施例的因子图;
图3为根据本发明实施例的车辆和传感器的搭建示意图;
图4为根据本发明实施例的车辆行驶轨迹图;
图5为根据本发明实施例的零速检测结果示意图;
图6a为根据本发明实施例的计算时间结果比对的示意图;
图6b为根据本发明实施例的轨迹比对的示意图;
图6c为根据本发明实施例的定位精度比对的示意图;
图7为根据本发明实施例的一种GNSS/INS组合***的导航***的结 构图;
图8为根据本发明实施例的一种电子设备的结构图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本 发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描 述,显然,所描述的实施例只是本发明一部分实施例,而不是全部的实施 例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动 前提下所获得的所有其他实施例,都属于本发明保护的范围。
本发明第一方面公开了一种GNSS/INS组合***的导航方法。图1为 根据本发明实施例的一种GNSS/INS组合***的导航方法的流程图;如图 1所示,所述方法包括:
步骤S1、基于全球卫星导航***GNSS输出的测量值和惯性导航*** INS中的惯性测量单元IMU输出的测量信息,构造所述GNSS/INS组合系 统的零速检测统计量;
步骤S2、构造所述GNSS/INS组合***的因子图,所述因子图包括 INS节点和GNSS节点,所述INS节点为所述因子图的过程模型因子节点, 所述GNSS节点为所述因子图的测量模型因子节点,所述因子图表征所述 GNSS/INS组合***在各个测量时刻的***状态量;
步骤S3、基于所述零速检测统计量确定载体的行进状态:
当所述载体为静止状态时,基于所述因子图构造零速校正因子 节点,并利用所述零速校正因子节点对所述载体进行定位,以获取所述 载体的位置和速度;
当所述载体为非静止状态时,基于所述因子图对所述载体进行 定位,以获取所述载体的位置和速度。
在步骤S1,基于全球卫星导航***GNSS输出的测量值和惯性导航系 统INS中的惯性测量单元IMU输出的测量信息,构造所述GNSS/INS组合 ***的零速检测统计量。
在一些实施例中,在所述步骤S1中,利用所述GNSS输出的测量值计 算载体速度,并基于所述载体速度和所述IMU输出的测量信息构造如以下 公式的广义似然比,以得到所述零速检测统计量;所述IMU输出的测量信 息为载体三维加速度和载体姿态信息;所述零速检测统计量的表达式为:
其中,表示所述IMU测量的第k个三个轴向的加速度,表示所述IMU 测量的第k个三个轴向的角速度,表示由所述GNSS测量的第k个载 体速度,表示所述IMU的加速度测量方差,表示所述IMU的角速度 测量方差,表示由所述GNSS测量得到的所述载体速度测量方差,N 为检测窗口长度,g为重力常量,其中为所述检测窗口长度 N内的加速度平均值,当所述零速检测统计量小于门限γ时,所述载体处 于所述静止状态。
具体地,利用GNSS输出的测量值计算载体速度值,再利用IMU输出 的载体三维加速度和姿态信息,构造广义似然比公式,其中和分别表示IMU测量的第k个三个轴向的加速度、角速度测量值和由GNSS 测量的第k个载体速度,和分别为IMU的加速度、角速度测 量方差以及由GNSS接收机测量值得到的载体速度测量方差,N为检测窗 口长度,g为重力常量。其中为检测窗口长度N内的加速度 平均值,当检测量小于门限γ时认为载体处于静止状态。其中,||x||2=xTx表 示x的二范数运算,表示二范数的平方根运算。
在步骤S2,构造所述GNSS/INS组合***的因子图,所述因子图包括 INS节点和GNSS节点,所述INS节点为所述因子图的过程模型因子节点, 所述GNSS节点为所述因子图的测量模型因子节点,所述因子图表征所述 GNSS/INS组合***在各个测量时刻的***状态量。
具体地,在因子图中,包括两种节点;一种是过程模型因子节点, 即INS节点;一种是测量模型因子节点,即GNSS节点。在任意一个测量 时刻i,因子图***状态量为Xi,Xi={x1,x2…xi},X1表示第一个测量时刻, GNSS/INS组合导航***状态量,X2表示第二个测量时刻,GNSS/INS组合 导航***状态量,依次论推,则可知Xi包括了截至到时刻i GNSS/INS组合 导航***状态量的所有值。
在一些实施例中,在所述步骤S2中,构造所述因子图的所述INS节 点具体包括,基于所述GNSS/INS组合***在载体坐标系b下,在测量时 刻i,经过时间间隔T积分后得到的速度的变化量和姿态角的变化量 确定所述GNSS/INS组合***在地球地固坐标系e下,在测量时刻 i+1的速度估计值位置估计值和姿态角估计值以构造所述 INS节点在测量时刻i+1的状态量估计值
具体地,假设在测量时刻i,已知IMU在b系(body frame,载体坐 标系)的输出测量值为加速度和角度令 为IMU 在b系三个坐标轴上的加速度计测量偏差,εb i为IMU在b系三个坐标轴 上的陀螺仪测量偏差;IMU输出测量值的时间间隔为Δt。为了降低计算负 担,采用预积分的方法。选定INS因子节点的更新周期为时间间隔T,且T=M·Δt(M≥1)。时间间隔T的大小与GNSS接收机输出测量值的时间间 隔一致。在时间间隔T内对IMU测量值在b系下的速度、姿态角变化量 分别进行预积分,得到下述结果:
其中,分别为b系下在测量时刻i,经过时间间隔T积分后得到 的GNSS/INS组合导航***的速度、姿态角的变化量。由以上预积分求出 的变化量,进一步可以求得测量时刻i+1时,GNSS/INS组合导航***的 速度、位置、姿态角的估计值:
其中,分别为GNSS/INS组合导航***在e系(Earth-Centered Earth-Fixed,地球地固坐标系)下测量时刻i时的位置、速度、姿态角, 由INS递推的原理可知这些为已知量;表示从测量时刻i到测量 时刻i+1,由b系变换到e系的旋转矩阵;ge表示重力矢量,在时间间隔 T内为一个常量。值得说明的是,测量时刻i到测量时刻i+1的时间间隔 是T。
在一些实施例中,在所述步骤S2中,构造所述因子图的所述GNSS 节点具体包括,基于所述GNSS节点测量得到的在所述测量时刻i对第j 颗可见卫星的伪距ρi,j、与所述伪距对应的伪距率第j颗可见卫星在 所述地球地固坐标系e下的位置和速度确定所述GNSS节点的 在所述测量时刻i的状态量
则GNSS接收机的测量方程可以写为:
在步骤S3,基于所述零速检测统计量确定载体的行进状态:当所述 载体为静止状态时,基于所述因子图构造零速校正因子节点,并利用所 述零速校正因子节点对所述载体进行定位,以获取所述载体的位置和速 度;当所述载体为非静止状态时,基于所述因子图对所述载体进行定位, 以获取所述载体的位置和速度。
在一些实施例中,当所述载体为所述静止状态时,基于所述因子图 构造所述零速校正因子节点包括,以所述载体为所述静止状态为条件, 约束所述INS节点的误差积累和所述GNSS节点的时钟漂移,以确定所述 零速校正因子节点,所述零速校正因子节点包括INS零速校正因子节点 hi ZUPT_INS和GNSS零速校正因子节点hi ZUPT_GNSS。
具体地,假设组合导航***的载体为城市中行驶的车辆,当车辆处 于静止状态时,利用车辆速度为零的约束可以限制INS误差的积累和GNSS 接收机的时钟漂移,此时不需要进行车辆位置和速度解算,极大的降低 了因子图中计算和存储的复杂度。当检测到车辆零速时,即车体坐标系 (m系)下三个方向的速度为零,即vm=0。忽略INS安装角的偏差,认 为INS的载体坐标系(b系)与车体坐标系重合。但由于IMU测量误差, 使得INS在测量时刻i由IMU测量值实际计算得到的载体速度不为零, b系及e系下的速度矢量存在如下关系:
其中载体在i时刻e系下的速度可以由i-1时刻的速度计算得到,如 式所示。假设载体在导航坐标系(n系)下的姿态矢量为其 中αi、βi、γi分别为i时刻载体的航向角、横滚角、俯仰角。n系姿态矢量 到e系姿态矢量的转换关系为:
同时GNSS接收机与卫星***时的钟差和钟差变化率在载体静止时, 也可以用速度为零进行约束控制。选取一颗观测质量最好的卫星,在测 量时刻i得到该卫星的伪距率测量方程为:
其中为接收机与卫星之间的几何距离变化率,卫星的钟差变化率可由星 历计算得到,在测量方程中省略,其值归结到伪距率测量值中,ξi为测量 残差。为包括对流层电离层等在内的传播误差随时间的变化率,在短 时间内可以忽略。当用户静止时,接收机与卫星之间的几何距离变化率其实就是卫星的速度在卫星与接收机径向方向的大小。可得GNSS对应的 测量方程分别为:
当***检测到车辆处于静止状态时,因子图中不增加新的因子节点, 对应矩阵的大小维持不变,只使用新测量的信息更新***状态。图2a为 根据本发明比较例的因子图;图2b为根据本发明实施例的因子图。
在一些实施例中,在所述步骤S3中:
当所述载体为所述静止状态时,利用所述零速校正因子节点,计算 所述GNSS/INS组合***的状态量的最大后验概率值,以对所述载体进行 定位;
当所述载体为非所述静止状态时,利用所述因子图中的所述INS节 点和所述GNSS节点,计算所述GNSS/INS组合***的状态量的最大后验 概率值,以对所述载体进行定位。
具体地,由上述推导得,测量时刻i+1,因子图***状态量Xi+1的最 大后验概率值Xi+1 MAP的表达式为:
当检测到载体处于静止时,利用零速修正因子节点进行计算状态量 的最大后验概率值Xi+1 MAP:
利用基于平滑和建图的标准C++的库GTSAM(Georgia Tech Smoothing andMapping),采用第二版增量平滑与地图构建(Incremental Smoothing And Mapping 2,iSAM2)算法对上述公式进行求解计算,可以得到*** 导航解。
具体示例
下面将通过GNSS/INS紧组合实验对所提方法进行验证。图3为根据 本发明实施例的车辆和传感器的搭建示意图;如图3所示,GNSS接收机 与IMU分别采用诺瓦泰的ProPak6和iIMU-FSAS,其中GNSS接收机输出 速率为1Hz,IMU输出速率为10Hz。实验场景为城市道路,由于存在等待 红绿灯与礼让行人,实验期间存在多个零速时间段,图4为根据本发明实施例的车辆行驶轨迹图;如图4中带箭头曲线所示,六角星标记为目 标起始点位置。同时ProPak6和iIMU-FSAS也组成诺瓦泰SPAN紧组合导 航***,以1Hz的频率对外输出导航结果。本次实验在固定地点架设GNSS 基准站,目的是使用RTK后处理数据的轨迹作为车辆行驶的参考轨迹, GNSS基准站坐标精确已知,当目标在高架桥或建筑物遮挡区等卫星信号差的地区时,参考轨迹使用组合导航***输出的结果。
零速检测实验分析
采用广义似然比作为检测器对目标车辆进行零速检测,图5为根据 本发明实施例的零速检测结果示意图;如图5所示,其中检测结果曲线 在图中只有“0”和“15”两种取值,取值为“0”时代表检测到零速, 为“15”时表示目标处于运动状态。由于存在环境高频噪声以及静止时 汽车震动噪声,因此在检测器前加入低通滤波器以提高检测率,实验结 果表明检测率达到90%时,误检率为1.7%。
导航定位实验分析
实验条件同上,车辆在城市中行驶过程中,有频繁启停的情况,利 用本文所提方法对GNSS接收机和IMU的测量值利用因子图模型进行计 算,实时得到车辆的位置、速度和姿态等信息。图6a为根据本发明实施 例的计算时间结果比对的示意图。行驶时间为300秒,其中车辆停止时 间为80秒,从图中可以看出,在刚开始的时间段内,由于启停的次数有限,本文方法的计算时间与经典因子图的基本一致,随着时间推移,启 停次数增多后,本文方法计算时间增速较缓,相比于经典因子图在计算 代价方面优势越来越明显,实时性提升显著,而经典因子图的图增大的 速度与时间呈线性关系。对因子图中节点数量进行统计,在本次300秒 的实验时间内,主节点数量从经典方法的900个降至681个,降低24.3%。 当然,随着车辆停止的次数和时间增多,其节点数下降将更明显。
另外,图6b为根据本发明实施例的轨迹比对的示意图;图6c为根 据本发明实施例的定位精度比对的示意图;可见,本文方法与经典因子 图方法,在车辆行驶轨迹上基本重合且与参考轨迹一致,在水平定位精 度上,本文方法在车辆零速区间不更新因子图的状态节点,只是利用IMU 和GNSS接收机输出的测量值来约束***状态,总体性能水平与经典因子 图相当。
本发明第二方面公开了一种GNSS/INS组合***的导航装置。图7为根 据本发明实施例的一种GNSS/INS组合***的导航***的结构图;如图7 所示,所述装置700包括:
第一处理单元701,被配置为,基于全球卫星导航***GNSS输出的测 量值和惯性导航***INS中的惯性测量单元IMU输出的测量信息,构造所 述GNSS/INS组合***的零速检测统计量;
第二处理单元702,被配置为,构造所述GNSS/INS组合***的因子图, 所述因子图包括INS节点和GNSS节点,所述INS节点为所述因子图的过程 模型因子节点,所述GNSS节点为所述因子图的测量模型因子节点,所述因 子图表征所述GNSS/INS组合***在各个测量时刻的***状态量;
第三处理单元703,被配置为,基于所述零速检测统计量确定载体的行 进状态:
当所述载体为静止状态时,基于所述因子图构造零速校正因子节 点,并利用所述零速校正因子节点对所述载体进行定位,以获取所述载体 的位置和速度;
当所述载体为非静止状态时,基于所述因子图对所述载体进行定 位,以获取所述载体的位置和速度。
根据本发明第二方面的装置,所述第一处理单元701具体被配置为, 利用所述GNSS输出的测量值计算载体速度,并基于所述载体速度和所述IMU 输出的测量信息构造如以下公式的广义似然比,以得到所述零速检测统计 量;所述IMU输出的测量信息为载体三维加速度和载体姿态信息;所述零 速检测统计量的表达式为:
其中,表示所述IMU测量的第k个三个轴向的加速度,表示所述IMU 测量的第k个三个轴向的角速度,表示由所述GNSS测量的第k个载体 速度,表示所述IMU的加速度测量方差,表示所述IMU的角速度测量 方差,表示由所述GNSS测量得到的所述载体速度测量方差,N为检测 窗口长度,g为重力常量,其中为所述检测窗口长度N内的加 速度平均值,当所述零速检测统计量小于门限γ时,所述载体处于所述静止 状态。
根据本发明第二方面的装置,所述第二处理单元702具体被配置为, 构造所述因子图的所述INS节点具体包括,基于所述GNSS/INS组合***在 载体坐标系b下,在测量时刻i,经过时间间隔T积分后得到的速度的变化 量和姿态角的变化量确定所述GNSS/INS组合***在地球地固坐标 系e下,在测量时刻i+1的速度估计值位置估计值和姿态角估计 值以构造所述INS节点在测量时刻i+1的状态量估计值
根据本发明第二方面的装置,所述第二处理单元702具体被配置为, 构造所述因子图的所述GNSS节点具体包括,基于所述GNSS节点测量得到 的在所述测量时刻i对第j颗可见卫星的伪距ρi,j、与所述伪距对应的伪距 率第j颗可见卫星在所述地球地固坐标系e下的位置和速度确定所述GNSS节点的在所述测量时刻i的状态量
根据本发明第二方面的装置,所述第三处理单元703具体被配置为, 当所述载体为所述静止状态时,基于所述因子图构造所述零速校正因子节 点包括,以所述载体为所述静止状态为条件,约束所述INS节点的误差积 累和所述GNSS节点的时钟漂移,以确定所述零速校正因子节点,所述零速 校正因子节点包括INS零速校正因子节点hi ZUPT_INS和GNSS零速校正因子节 点hi ZUPT_GNSS。
根据本发明第二方面的装置,所述第三处理单元703具体被配置为, 当所述载体为所述静止状态时,利用所述零速校正因子节点,计算所述 GNSS/INS组合***的状态量的最大后验概率值,以对所述载体进行定位;
当所述载体为非所述静止状态时,利用所述因子图中的所述INS节点 和所述GNSS节点,计算所述GNSS/INS组合***的状态量的最大后验概率 值,以对所述载体进行定位。
本发明第三方面公开了一种电子设备。所述电子设备包括存储器和处 理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时, 实现本公开第一方面中任一项所述的一种GNSS/INS组合***的导航方法中 的步骤。
图8为根据本发明实施例的一种电子设备的结构图,如图8所示,电 子设备包括通过***总线连接的处理器、存储器、通信接口、显示屏和输 入装置。其中,该电子设备的处理器用于提供计算和控制能力。该电子设 备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储 有操作***和计算机程序。该内存储器为非易失性存储介质中的操作*** 和计算机程序的运行提供环境。该电子设备的通信接口用于与外部的终端 进行有线或无线方式的通信,无线方式可通过WIFI、运营商网络、近场通 信(NFC)或其他技术实现。该电子设备的显示屏可以是液晶显示屏或者电 子墨水显示屏,该电子设备的输入装置可以是显示屏上覆盖的触摸层,也 可以是电子设备外壳上设置的按键、轨迹球或触控板,还可以是外接的键 盘、触控板或鼠标等。
本领域技术人员可以理解,图8中示出的结构,仅仅是与本公开的技 术方案相关的部分的结构图,并不构成对本申请方案所应用于其上的电子 设备的限定,具体的电子设备可以包括比图中所示更多或更少的部件,或 者组合某些部件,或者具有不同的部件布置。
本发明第四方面公开了一种计算机可读存储介质。所述计算机可读存 储介质上存储有计算机程序,所述计算机程序被处理器执行时,实现本公 开第一方面中任一项所述的一种GNSS/INS组合***的导航方法中的步骤。
本发明提供的技术方案与现有技术相比,优点在于:本发明在传统 的利用IMU输出的三维加速度、姿态测量值作为广义似然比零速检测量 的基础上,增加了GNSS接收机的载体速度测量值作为检测量,保证了检 测的稳定可靠及精确性。本发明在检测到载体处于零速时,不增加新的 因子节点,有效减轻了因子图的存储量和计算量;本发明在载体零速时, 将IMU和GNSS接收机的测量值对***状态进行约束和更新,保证了*** 状态值估计的正确性。相比传统因子图方法,本发明在组合***导航性 能不变的情况下,能有效降低因子图的存储量和计算量,特别适用于车 载GNSS/INS紧组合***在城市行驶中频繁启停的场景。
请注意,以上实施例的各技术特征可以进行任意的组合,为使描述简 洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而, 只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。 以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细, 但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领 域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干 变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范 围应以所附权利要求为准。
Claims (10)
1.一种GNSS/INS组合***的导航方法,其特征在于,所述方法包括:
步骤S1、基于全球卫星导航***GNSS输出的测量值和惯性导航***INS中的惯性测量单元IMU输出的测量信息,构造所述GNSS/INS组合***的零速检测统计量;
步骤S2、构造所述GNSS/INS组合***的因子图,所述因子图包括INS节点和GNSS节点,所述INS节点为所述因子图的过程模型因子节点,所述GNSS节点为所述因子图的测量模型因子节点,所述因子图表征所述GNSS/INS组合***在各个测量时刻的***状态量;
步骤S3、基于所述零速检测统计量确定载体的行进状态:
当所述载体为静止状态时,基于所述因子图构造零速校正因子节点,并利用所述零速校正因子节点对所述载体进行定位,以获取所述载体的位置和速度;
当所述载体为非静止状态时,基于所述因子图对所述载体进行定位,以获取所述载体的位置和速度。
2.根据权利要求1所述的一种GNSS/INS组合***的导航方法,其特征在于,在所述步骤S1中,利用所述GNSS输出的测量值计算载体速度,并基于所述载体速度和所述IMU输出的测量信息构造如以下公式的广义似然比,以得到所述零速检测统计量;所述IMU输出的测量信息为载体三维加速度和载体姿态信息;所述零速检测统计量的表达式为:
5.根据权利要求4所述的一种GNSS/INS组合***的导航方法,其特征在于,在所述步骤S3中,当所述载体为所述静止状态时,基于所述因子图构造所述零速校正因子节点包括,以所述载体为所述静止状态为条件,约束所述INS节点的误差积累和所述GNSS节点的时钟漂移,以确定所述零速校正因子节点,所述零速校正因子节点包括INS零速校正因子节点hi ZUPT_INS和GNSS零速校正因子节点hi ZUPT_GNSS。
6.根据权利要求4所述的一种GNSS/INS组合***的导航方法,其特征在于,在所述步骤S3中:
当所述载体为所述静止状态时,利用所述零速校正因子节点,计算所述GNSS/INS组合***的状态量的最大后验概率值,以对所述载体进行定位;
当所述载体为非所述静止状态时,利用所述因子图中的所述INS节点和所述GNSS节点,计算所述GNSS/INS组合***的状态量的最大后验概率值,以对所述载体进行定位。
7.一种GNSS/INS组合***的导航装置,其特征在于,所述装置包括:
第一处理单元,被配置为,基于全球卫星导航***GNSS输出的测量值和惯性导航***INS中的惯性测量单元IMU输出的测量信息,构造所述GNSS/INS组合***的零速检测统计量;
第二处理单元,被配置为,构造所述GNSS/INS组合***的因子图,所述因子图包括INS节点和GNSS节点,所述INS节点为所述因子图的过程模型因子节点,所述GNSS节点为所述因子图的测量模型因子节点,所述因子图表征所述GNSS/INS组合***在各个测量时刻的***状态量;
第三处理单元,被配置为,基于所述零速检测统计量确定载体的行进状态:
当所述载体为静止状态时,基于所述因子图构造零速校正因子节点,并利用所述零速校正因子节点对所述载体进行定位,以获取所述载体的位置和速度;
当所述载体为非静止状态时,基于所述因子图对所述载体进行定位,以获取所述载体的位置和速度。
8.根据权利要求7所述的一种GNSS/INS组合***的导航装置,其特征在于,所述第一处理单元具体被配置为,利用所述GNSS输出的测量值计算载体速度,并基于所述载体速度和所述IMU输出的测量信息构造如以下公式的广义似然比,以得到所述零速检测统计量;所述IMU输出的测量信息为载体三维加速度和载体姿态信息;所述零速检测统计量的表达式为:
9.一种电子设备,其特征在于,所述电子设备包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时,实现权利要求1至6中任一项所述的一种GNSS/INS组合***的导航方法中的步骤。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时,实现权利要求1至6中任一项所述的一种GNSS/INS组合***的导航方法中的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210095337.XA CN114545472B (zh) | 2022-01-26 | 2022-01-26 | 一种gnss/ins组合***的导航方法和装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210095337.XA CN114545472B (zh) | 2022-01-26 | 2022-01-26 | 一种gnss/ins组合***的导航方法和装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114545472A CN114545472A (zh) | 2022-05-27 |
CN114545472B true CN114545472B (zh) | 2022-10-21 |
Family
ID=81673591
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210095337.XA Active CN114545472B (zh) | 2022-01-26 | 2022-01-26 | 一种gnss/ins组合***的导航方法和装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114545472B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115235454B (zh) * | 2022-09-15 | 2022-12-30 | 中国人民解放军国防科技大学 | 行人运动约束的视觉惯性融合定位与建图方法和装置 |
CN115685292B (zh) * | 2023-01-05 | 2023-03-21 | 中国人民解放军国防科技大学 | 一种多源融合导航***的导航方法和装置 |
CN116299576B (zh) * | 2023-05-12 | 2023-12-12 | 中国人民解放军国防科技大学 | 一种组合导航***的欺骗干扰检测方法与装置 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11313684B2 (en) * | 2016-03-28 | 2022-04-26 | Sri International | Collaborative navigation and mapping |
US10295365B2 (en) * | 2016-07-29 | 2019-05-21 | Carnegie Mellon University | State estimation for aerial vehicles using multi-sensor fusion |
CN112697138B (zh) * | 2020-12-07 | 2022-09-27 | 北方工业大学 | 一种基于因子图优化的仿生偏振同步定位与构图的方法 |
CN112946711B (zh) * | 2021-01-29 | 2022-11-25 | 中国人民解放军国防科技大学 | 一种gnss/ins组合导航***的导航方法 |
CN112985392B (zh) * | 2021-04-19 | 2021-07-30 | 中国人民解放军国防科技大学 | 基于图优化框架的行人惯性导航方法和装置 |
CN113295162B (zh) * | 2021-04-28 | 2024-06-11 | 南京航空航天大学 | 基于无人机状态信息的广义因子图融合导航方法 |
-
2022
- 2022-01-26 CN CN202210095337.XA patent/CN114545472B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN114545472A (zh) | 2022-05-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114545472B (zh) | 一种gnss/ins组合***的导航方法和装置 | |
US9360323B2 (en) | Systems and methods for estimating movements of a vehicle using a mobile device | |
KR100518852B1 (ko) | 차량의 후진 개선 추측항법 | |
US20110307171A1 (en) | GPS Location Refinement Method In Environments With Low Satellite Visibility | |
CN101762805A (zh) | 组合导航***以及导航方法 | |
CN111562603B (zh) | 基于航位推算的导航定位方法、设备及存储介质 | |
CN112505737A (zh) | 一种基于Elman神经网络在线学习辅助的GNSS/INS组合导航方法 | |
CN113252048B (zh) | 一种导航定位方法、导航定位***及计算机可读存储介质 | |
CN113063425B (zh) | 车辆定位方法、装置、电子设备及存储介质 | |
CN109579870A (zh) | 捷联惯导***的自动对准方法和组合导航装置 | |
CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
JP6248559B2 (ja) | 車両用走行軌跡算出装置 | |
CN115485527A (zh) | 将测量值量化误差建模为状态的导航设备和方法 | |
CN115407376A (zh) | 车辆定位校准方法、装置、计算机设备、存储介质 | |
CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
US12019170B1 (en) | GNSS and INS integrated navigation positioning method and system thereof | |
CN106646569A (zh) | 一种导航定位方法及设备 | |
CN117268408A (zh) | 一种激光slam定位方法及*** | |
CN114001730B (zh) | 融合定位方法、装置、计算机设备和存储介质 | |
CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
CN112461236B (zh) | 一种车载高精度容错组合导航方法及*** | |
CN116625362A (zh) | 一种室内定位方法、装置、移动终端及存储介质 | |
CN111002991B (zh) | 一种车载导航信息处理的方法、装置及计算机存储介质 | |
CN113566849B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
Elmezayen et al. | Examining the capabilities of smartphone-based high precision positioning in challenging suburban 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 |