CN116774263B - 面向组合导航系统的导航定位方法及装置 - Google Patents
面向组合导航系统的导航定位方法及装置 Download PDFInfo
- Publication number
- CN116774263B CN116774263B CN202310694378.5A CN202310694378A CN116774263B CN 116774263 B CN116774263 B CN 116774263B CN 202310694378 A CN202310694378 A CN 202310694378A CN 116774263 B CN116774263 B CN 116774263B
- Authority
- CN
- China
- Prior art keywords
- navigation
- filtering
- positioning information
- filtering model
- error
- 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 51
- 238000001914 filtration Methods 0.000 claims abstract description 97
- 238000005259 measurement Methods 0.000 claims abstract description 37
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 28
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000012937 correction Methods 0.000 claims description 14
- 238000005562 fading Methods 0.000 claims description 10
- 238000010276 construction Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 238000004590 computer program Methods 0.000 description 6
- 230000003044 adaptive effect Effects 0.000 description 5
- 230000003993 interaction Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000010295 mobile communication Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
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
-
- 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/1652—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 ranging devices, e.g. LIDAR or RADAR
-
- 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
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
- G01S13/865—Combination of radar systems with lidar systems
-
- 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
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
- G01S13/867—Combination of radar systems with 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
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
本发明提供一种面向组合导航系统的导航定位方法及装置，在该方法中，包括：基于惯导系统确定针对导航目标的第一导航定位信息，以及基于GPS系统确定针对导航目标的第二导航定位信息；GPS系统包含实时动态GPS模块和激光雷达模块；基于惯导系统的导航参数误差模型构建针对滤波模型的状态方程，以及基于惯导系统与GPS系统所输出的定位信息的偏差构建针对滤波模型的量测方程，滤波模型以导航参数误差作为状态量；基于滤波模型，确定对应量测信息的导航参数误差；滤波模型采用强跟踪卡尔曼滤波算法；基于导航参数误差，校正第一导航定位信息。由此，通过增量式自适应强跟踪卡尔曼滤波方法进行工作，实现高精度导航定位信息。
Description
技术领域
本发明涉及无人系统组合导航技术领域，尤其涉及一种面向组合导航系统的导航定位方法、装置、电子设备及存储介质。
背景技术
多传感器组合导航系统作为无人机的“眼睛”，是无人机系统最重要的组成部分之一，可为无人机提供诸如姿态、速度和位置等导航信息而，准确、可靠的导航信息是无人机安全、稳定飞行的前提。组合导航技术是指使用多个传感器来测量同一信息源，以实现更精确的导航技术。多传感器数据融合指将来自具有不同特征的传感器(例如陀螺仪，加速计，磁力计，GNSS模块和视觉传感器等)的测量信息转换到统一坐标系下，并进行时间对准，便于建立系统状态方程和量测方程，同时应用适合的估计算法得到系统状态的最优估计，以实现状态预测量和量测量之间的最优拟合。因此，多传感器数据融合过程实际上就是利用最优估计算法导出系统状态的过程。
目前绝大部分组合导航中的信息融合方法都是基于卡尔曼滤波(Kalman Filter，KF)，相关的研究文献已有很多。但是，传统卡尔曼滤波只能应用于线性系统，同时其建立在系统过程噪声与量测噪声固定不变且已知的假设之上，然而这在实际应用中是无法被满足的。
发明内容
本发明实施例提供了一种面向组合导航系统的导航定位方法、装置及存储介质，用于至少解决上述技术问题之一。
第一方面，本发明实施例提供一种面向组合导航系统的导航定位方法，包括：基于惯导系统确定针对导航目标的第一导航定位信息，以及基于GPS系统确定针对所述导航目标的第二导航定位信息；所述GPS系统包含实时动态GPS模块和激光雷达模块；基于所述惯导系统的导航参数误差模型构建针对滤波模型的状态方程，以及基于所述惯导系统与所述GPS系统所输出的定位信息的偏差构建针对所述滤波模型的量测方程，所述滤波模型以导航参数误差作为状态量；基于所述滤波模型，确定对应所述量测信息的导航参数误差；所述滤波模型采用强跟踪卡尔曼滤波算法；基于所述导航参数误差，校正所述第一导航定位信息。
优选地，所述滤波模型采用基于多重渐消因子的强跟踪卡尔曼滤波算法，其中，针对所述滤波模型的不同通道分别采用相应的渐消因子。
优选地，所述滤波模型采用增量式自适应强跟踪卡尔曼滤波算法，其用于对量测噪声协方差矩阵持续进行在线修正操作，以适应量测噪声特性的变化。
优选地，所述在线修正操作包括：
定义新息理论协方差与实测协方差的一致程度(Degree of Consistency，DoC)如下：
DoC(εk)＝Pr(k)-Cr(k) 式(1)
其中，Pr(k)、Pr(k)分别表示理论新息协方差矩阵和实测新息协方差矩阵；
其中，表示量测噪声协方差矩阵对角线的第i个元素，理想的滤波情况下DoCi(εk)＝0；若DoCi(εk)＞0，说明新息协方差理论值大于实测值，此时应该减小/>则ΔRi＜0；相反，若DoCi(εk)＜0，说明新息协方差理论值小于实测值，此时应该增大/>则ΔRi＞0；ΔR的取值为Rk大小的5％-15％。
优选地，所述自适应强跟踪卡尔曼滤波算法的计算步骤包括：
S1、一步状态预测操作：
S2、新息序列为：
S3、新息协方差一致程度检验操作：
DoC(εk)＝Pr(k)-Cr(k) 式(5)
S4、状态预测误差协方差为：
S5、量测噪声协方差自适应调整操作：
S6、滤波发散判据操作：
其中，当滤波发散判据成立时，需要切换至强跟踪卡尔曼滤波模块，此时应更新式(6)中所计算的状态预测误差协方差阵：
S7、卡尔曼增益更新操作：
S8、状态更新操作：
S9、状态更新误差协方差为：
优选地，根据新息序列εk这一特性，所构造针对所述强跟踪卡尔曼滤波模块的发散判据如下：
其中，ζ为滤波发散的边界系数。
优选地，所述基于所述滤波模型，确定对应所述量测信息的导航参数误差，
包括：
其中，X表示所确定的导航参数误差，[φE,φN,φU]为姿态角误差，[δVE,δVN,δVU]为速度误差，[δλ,δL,δh]为位置(经纬高)误差，[εgx,εgy,εgz]为导航目标坐标系三轴陀螺仪的常值漂移，为导航目标坐标系下三轴加速度计的零偏。
第二方面，本发明实施例提供一种面向组合导航系统的导航定位装置，包括：定位信息获取单元，用于基于惯导系统确定针对导航目标的第一导航定位信息，以及基于GPS系统确定针对所述导航目标的第二导航定位信息；所述GPS系统包含实时动态GPS模块和激光雷达模块；滤波模型构建单元，用于基于所述惯导系统的导航参数误差模型构建针对滤波模型的状态方程，以及基于所述惯导系统与所述GPS系统所输出的定位信息的偏差构建针对所述滤波模型的量测方程，所述滤波模型以导航参数误差作为状态量；滤波状态确定单元，用于基于所述滤波模型，确定对应所述量测信息的导航参数误差；所述滤波模型采用强跟踪卡尔曼滤波算法；定位信息校正单元，用于基于所述导航参数误差，校正所述第一导航定位信息。
第三方面，本发明实施例提供一种电子设备，其包括：至少一个处理器，以及与所述至少一个处理器通信连接的存储器，其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述方法的步骤。
第四方面，本发明实施例提供一种存储介质，所述存储介质中存储有一个或多个包括执行指令的程序，所述执行指令能够被电子设备(包括但不限于计算机，服务器，或者网络设备等)读取并执行，以用于执行本发明上述方法的步骤。
第五方面，本发明实施例还提供一种计算机程序产品，所述计算机程序产品包括存储在存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使所述计算机执行上述方法的步骤。
上述技术方案，与现有技术相比至少具有如下有益效果：
上述方案，采用间接滤波法构建GPS-RTK/INS/LiDAR组合导航系统的滤波模型，以导航参数误差(如速度误差、姿态误差、位置误差等)作为状态量，利用INS导航参数误差模型建立起状态方程，INS与其它导航子系统的输出的差作为量测建立起量测方程。组合滤波器通过增量式自适应强跟踪卡尔曼滤波方法进行工作，其输出的是导航参数误差的最优估计，用于校正INS的输出，最终得到姿态、速度、位置等高精度导航定位信息。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1示出了根据本发明实施例的面向组合导航系统的导航定位方法的一示例的流程图；
图2示出了根据本发明实施例的面向组合导航系统的导航定位方法的一示例的操作原理图；
图3为根据本发明实施例的对量测噪声协方差矩阵的增量式调节流程图；
图4为根据本发明实施例的增量式自适应强跟踪卡尔曼滤波算法流程图；
图5示出了根据本发明实施例的面向组合导航系统的导航定位装置的一示例的结构框图；
图6为本发明的电子设备的一实施例的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例的附图，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员在无需创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
除非另外定义，本发明使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。同样，“一个”、“一”或者“该”等类似词语也不表示数量限制，而是表示存在至少一个。“包括”或者“包含”等类似的词语意指出现该词前面的元件或者物件涵盖出现在该词后面列举的元件或者物件及其等同，而不排除其他元件或者物件。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。
图1示出了根据本发明实施例的面向组合导航系统的导航定位方法的一示例的流程图。
如图1所示，在步骤S110中，基于惯导系统确定针对导航目标的第一导航定位信息，以及基于GPS系统确定针对所述导航目标的第二导航定位信息，GPS系统包含实时动态GPS模块和激光雷达模块。
在步骤S120中，基于惯导系统的导航参数误差模型构建针对滤波模型的状态方程，以及基于惯导系统与GPS系统所输出的定位信息的偏差构建针对滤波模型的量测方程，滤波模型以导航参数误差作为状态量。
在步骤S130中，基于滤波模型，确定对应量测信息的导航参数误差，滤波模型采用强跟踪卡尔曼滤波算法。
在步骤S140中，基于导航参数误差，校正第一导航定位信息。
如图2所示，采用间接滤波法构建GPS-RTK/INS/LiDAR组合导航系统的滤波模型，以导航参数误差(如速度误差、姿态误差、位置误差等)作为状态量，利用INS导航参数误差模型建立起状态方程，INS与其它导航子系统的输出的差作为量测建立起量测方程。组合滤波器通过增量式自适应强跟踪卡尔曼滤波方法进行工作，其输出的是导航参数误差的最优估计，用于校正INS的输出，最终得到姿态、速度、位置等高精度导航定位信息。
具体地，增量式自适应强跟踪卡尔曼滤波方法，针对GPS-RTK/INS/LiDAR组合导航定位系统，考虑到状态方程是根据INS的力学编排建立，其系统的噪声主要取决于INS系统内部机理，其统计特性相对比较稳定，因此可以通过预先对惯性传感器进行测试分析或是采用经验值的方式，而量测噪声则主要受外部测量环境的影响，其统计特性变化相对较大；区别于Sage-Husa自适应卡尔曼滤波算法同时对系统噪声与量测噪声进行自适应估计，具有算法复杂度高的特点。本专利提出一种增量式自适应强跟踪卡尔曼滤波算法(Incremental Adaptive Strong Tracking Kalman Filter，IASTKF)，只对量测噪声统计特性进行自适应估计。一方面可以减少滤波算法估计参数的数量，有利于提升导航系统的稳定性，另一方面也减少了滤波算法的计算量，导航系统的实时性要求更容易满足。
进一步地，IASTKF算法借鉴了强跟踪卡尔曼滤波的思想，以抑制滤波发散。传统的强跟踪卡尔曼滤波利用一个标量渐消因子调节状态预测误差协方差阵，它对每个通道应用了相同的渐消因子，从某种程度上说降低了滤波器的估计精度。在本发明实施例中，充分考虑了GPS-RTK/INS/LiDAR组合导航定位系统的特性，对传统STKF算法进行了改进，使用了一种基于多重渐消因子的强跟踪卡尔曼滤波算法(Multiple Fading Factor StrongTracking Kalman Filter，MSTKF)，其具体思路是采用多个渐消因子，使各通道具有不同的渐消速率。
如图3所示，IASTKF算法采用一种增量式调节方法对量测噪声协方差矩阵不断进行在线修正，以适应量测噪声特性的变化。首先定义新息理论协方差与实测协方差的一致程度(Degree of Consistency，DoC)如下：
DoC(εk)＝Pr(k)-Cr(k) 式(1)
式中Pr(k)、Pr(k)分别表示理论新息协方差矩阵和实测新息协方差矩阵。
其中，表示量测噪声协方差矩阵对角线的第i个元素。理想的滤波情况下有DoCi(εk)＝0；若DoCi(εk)＞0，说明新息协方差理论值大于实测值，此时应该减小/>则ΔRi＜0；相反，若DoCi(εk)＜0，说明新息协方差理论值小于实测值，此时应该增大/>则ΔRi＞0。
需说明的是，IASTKF算法通过ΔR来自适应调节量测噪声协方差矩阵以增强理论值与实测值的一致程度，因此ΔR的选择至关重要。ΔR较大时能更快的适应量测噪声，但容易发生震荡；ΔR较小时自适应调节效果不明显。按照经验，一般取为Rk大小的5％-15％左右即可。
如图4所示，IASTKF算法流程以及完整计算步骤：
S1、一步状态预测
S2、新息序列为
S3、新息协方差一致程度检验
DoC(εk)＝Pr(k)-Cr(k) 式(5)
S4、状态预测误差协方差
S5、量测噪声协方差自适应调整
S6、滤波发散判据：
当滤波发散判据成立时，需要切换至强跟踪Kalman滤波器，此时应更新S4步骤公式中所计算的状态预测误差协方差阵：
S7、卡尔曼增益更新
S8、状态更新
S9、状态更新误差协方差
在应用增量式自适应强跟踪卡尔曼滤波方法之前，需考虑如何判断滤波是否存在发散的趋势。由于新息序列εk是由当前实际量测输出与预测输出之差，它在一定程度上反应了估计误差的大小，当新息较大时则意味着可能存在滤波发散的风险。根据新息序列εk这一特性，构造的滤波器发散判据如下：
式中ζ为滤波发散的边界系数，左边分子反应了当前时刻估计误差的实际大小，左边分母新息协方差的迹反应了误差的理论值，因此当实际误差一定程度上超过理论误差时(取决于边界系数ζ)，即可判定为存在发散的趋势。边界系数ζ的选取对于强跟踪卡尔曼滤波非常关键，应根据实际情况进行调整以达到较好的滤波性能。
利用增量式自适应强跟踪卡尔曼滤波方法进行误差校正。滤波器输出的导航参数误差状态的估计如下：
其中，[φE,φN,φU]为姿态角误差，[δVE,δVN,δVU]为速度误差，[δλ,δL,δh]为位置(经纬高)误差，[εgx,εgy,εgz]为机体(即，导航目标为无人机)坐标系三轴陀螺仪的常值漂移，为机体坐标系下三轴加速度计的零偏。
(1)速度与位置误差校正
直接将惯导系统输出与导航参数误差的估计值做差即可，如下式所示：
式中，和/>为惯导系统解算出的速度，/>和/>表示误差校正后的高精度速度信息；λINS、LINS和hINS为惯导系统解算出的位置信息，λcor、Lcor和hcor表示经过误差校正后的高精度位置信息。
(2)姿态角误差校正
姿态误差角为旋转矢量不能直接做相加减法，这里设/>对应的旋转矩阵和四元数分别为/>待校正的姿态角/>对应的旋转矩阵和四元数分别为/>QINS，则误差校正后的旋转矩阵如下所示：
考虑旋转矩阵与四元数的关系式：(/>为四元数/>对应的旋转矩阵)，最终可得姿态四元数误差校正公式如下：
图5示出了根据本发明实施例的面向组合导航系统的导航定位装置的一示例的结构框图。
如图5所示，面向组合导航系统的导航定位装置500包括定位信息获取单元510、滤波模型构建单元520、滤波状态确定单元530和定位信息校正单元540。
定位信息获取单元510用于基于惯导系统确定针对导航目标的第一导航定位信息，以及基于GPS系统确定针对所述导航目标的第二导航定位信息；所述GPS系统包含实时动态GPS模块和激光雷达模块.
滤波模型构建单元520用于基于所述惯导系统的导航参数误差模型构建针对滤波模型的状态方程，以及基于所述惯导系统与所述GPS系统所输出的定位信息的偏差构建针对所述滤波模型的量测方程，所述滤波模型以导航参数误差作为状态量。
滤波状态确定单元530用于基于所述滤波模型，确定对应所述量测信息的导航参数误差；所述滤波模型采用强跟踪卡尔曼滤波算法。
定位信息校正单元540用于基于所述导航参数误差，校正所述第一导航定位信息。
关于本发明实施例的面向组合导航系统的导航定位装置的更多细节，可以参照上文中结合面向组合导航系统的导航定位方法的实施例描述，并能取得相同或类似的技术效果。
需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作合并，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本发明所必须的。在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在一些实施例中，本发明实施例提供一种非易失性计算机可读存储介质，所述存储介质中存储有一个或多个包括执行指令的程序，所述执行指令能够被电子设备(包括但不限于计算机，服务器，或者网络设备等)读取并执行，以用于执行本发明上述的面向组合导航系统的导航定位方法。
在一些实施例中，本发明实施例还提供一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使所述计算机执行上述的面向组合导航系统的导航定位方法。
在一些实施例中，本发明实施例还提供一种电子设备，其包括：至少一个处理器，以及与所述至少一个处理器通信连接的存储器，其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行面向组合导航系统的导航定位方法。
图6是本发明另一实施例提供的执行面向组合导航系统的导航定位方法的电子设备的硬件结构示意图，如图6所示，该设备包括：
一个或多个处理器610以及存储器620，图6中以一个处理器610为例。
执行面向组合导航系统的导航定位方法的设备还可以包括：输入装置630和输出装置640。
处理器610、存储器620、输入装置630和输出装置640可以通过总线或者其他方式连接，图6中以通过总线连接为例。
存储器620作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本发明实施例中的面向组合导航系统的导航定位方法对应的程序指令/模块。处理器610通过运行存储在存储器620中的非易失性软件程序、指令以及模块，从而执行服务器的各种功能应用以及数据处理，即实现上述方法实施例的面向组合导航系统的导航定位方法。
存储器620可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据语音交互设备的使用所创建的数据等。此外，存储器620可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器620可选包括相对于处理器610远程设置的存储器，这些远程存储器可以通过网络连接至语音交互设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
输入装置630可接收输入的数字或字符信息，以及产生与语音交互设备的用户设置以及功能控制有关的信号。输出装置640可包括显示屏等显示设备。
所述一个或者多个模块存储在所述存储器620中，当被所述一个或者多个处理器610执行时，执行上述任意方法实施例中的面向组合导航系统的导航定位方法。
上述产品可执行本发明实施例所提供的方法，具备执行方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本发明实施例所提供的方法。
本发明实施例的电子设备以多种形式存在，包括但不限于:
(1)移动通信设备:这类设备的特点是具备移动通信功能，并且以提供话音、数据通信为主要目标。这类终端包括:智能手机、多媒体手机、功能性手机，以及低端手机等。
(2)超移动个人计算机设备:这类设备属于个人计算机的范畴，有计算和处理功能，一般也具备移动上网特性。这类终端包括:PDA、MID和UMPC设备等。
(3)便携式娱乐设备:这类设备可以显示和播放多媒体内容。该类设备包括:音频、视频播放器，掌上游戏机，电子书，以及智能玩具和便携式车载导航设备。
(4)其他具有数据交互功能的机载电子装置，例如安装上车辆上的车机装置。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (7)
1.一种面向组合导航系统的导航定位方法，包括：
基于惯导系统确定针对导航目标的第一导航定位信息，以及基于GPS系统确定针对所述导航目标的第二导航定位信息；所述GPS系统包含实时动态GPS模块和激光雷达模块；
基于所述惯导系统的导航参数误差模型构建针对滤波模型的状态方程，以及基于所述惯导系统与所述GPS系统所输出的定位信息的偏差构建针对所述滤波模型的量测方程，所述滤波模型以导航参数误差作为状态量；
基于所述滤波模型，确定对应所述量测信息的导航参数误差；所述滤波模型采用强跟踪卡尔曼滤波算法；
基于所述导航参数误差，校正所述第一导航定位信息；
其中，所述滤波模型采用增量式自适应强跟踪卡尔曼滤波算法，其用于对量测噪声协方差矩阵持续进行在线修正操作，以适应量测噪声特性的变化；
所述在线修正操作包括：
定义新息理论协方差与实测协方差的一致程度如下：
DoC(εk)＝Pr(k)-Cr(k) 式(1)
其中，Pr(k)、Pr(k)分别表示理论新息协方差矩阵和实测新息协方差矩阵；
其中，表示量测噪声协方差矩阵对角线的第i个元素，理想的滤波情况下DoCi(εk)＝0；若DoCi(εk)＞0，说明新息协方差理论值大于实测值，此时应该减小/>则ΔRi＜0；相反，若DoCi(εk)＜0，说明新息协方差理论值小于实测值，此时应该增大/>则ΔRi＞0；ΔR的取值为Rk大小的5％-15％；
所述自适应强跟踪卡尔曼滤波算法的计算步骤包括：
S1、一步状态预测操作：
S2、新息序列为：
S3、新息协方差一致程度检验操作：
DoC(εk)＝Pr(k)-Cr(k) 式(5)
S4、状态预测误差协方差为：
S5、量测噪声协方差自适应调整操作：
S6、滤波发散判据操作：
其中，当滤波发散判据成立时，需要切换至强跟踪卡尔曼滤波模块，
此时应更新式(6)中所计算的状态预测误差协方差阵：
S7、卡尔曼增益更新操作：
S8、状态更新操作：
S9、状态更新误差协方差为：
2.根据权利要求1所述的面向组合导航系统的导航定位方法，其特征在于，所述滤波模型采用基于多重渐消因子的强跟踪卡尔曼滤波算法，其中，针对所述滤波模型的不同通道分别采用相应的渐消因子。
3.根据权利要求1所述的面向组合导航系统的导航定位方法，其特征在于，根据新息序列εk这一特性，所构造针对所述强跟踪卡尔曼滤波模块的发散判据如下：
其中，ζ为滤波发散的边界系数。
4.根据权利要求3所述的面向组合导航系统的导航定位方法，其特征在于，所述基于所述滤波模型，确定对应所述量测信息的导航参数误差，包括：
X＝[φE,φN,φU,δVE,δVN,δVU,δλ,δL,δh,εgx,εgy,εgz,▽ax,▽ay,▽az]T 式(14)
其中，X表示所确定的导航参数误差，[φE,φN,φU]为姿态角误差，[δVE,δVN,δVU]为速度误差，[δλ,δL,δh]为位置(经纬高)误差，为导航目标坐标系三轴陀螺仪的常值漂移，/>为导航目标坐标系下三轴加速度计的零偏。
5.一种面向组合导航系统的导航定位装置，用于实现如权利要求1-4中任一项所述的方法，包括：
定位信息获取单元，用于基于惯导系统确定针对导航目标的第一导航定位信息，以及基于GPS系统确定针对所述导航目标的第二导航定位信息；所述GPS系统包含实时动态GPS模块和激光雷达模块；
滤波模型构建单元，用于基于所述惯导系统的导航参数误差模型构建针对滤波模型的状态方程，以及基于所述惯导系统与所述GPS系统所输出的定位信息的偏差构建针对所述滤波模型的量测方程，所述滤波模型以导航参数误差作为状态量；
滤波状态确定单元，用于基于所述滤波模型，确定对应所述量测信息的导航参数误差；所述滤波模型采用强跟踪卡尔曼滤波算法；
定位信息校正单元，用于基于所述导航参数误差，校正所述第一导航定位信息。
6.一种电子设备，其包括：至少一个处理器，以及与所述至少一个处理器通信连接的存储器，其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如上述权利要求1-4中任一项所述方法的步骤。
7.一种存储介质，所述存储介质中存储有一个或多个包括执行指令的程序，所述执行指令能够被电子设备读取并执行，以用于执行如上述权利要求1-4中任一项所述方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310694378.5A CN116774263B (zh) | 2023-06-12 | 2023-06-12 | 面向组合导航系统的导航定位方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310694378.5A CN116774263B (zh) | 2023-06-12 | 2023-06-12 | 面向组合导航系统的导航定位方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116774263A CN116774263A (zh) | 2023-09-19 |
| CN116774263B true CN116774263B (zh) | 2024-04-05 |
Family
ID=87995657
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310694378.5A Active CN116774263B (zh) | 2023-06-12 | 2023-06-12 | 面向组合导航系统的导航定位方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116774263B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105737828A (zh) * | 2016-05-09 | 2016-07-06 | 郑州航空工业管理学院 | 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法 |
| CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
| CN110823217A (zh) * | 2019-11-21 | 2020-02-21 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
| CN113048979A (zh) * | 2021-03-08 | 2021-06-29 | 中国矿业大学 | 一种组合导航滤波方法 |
| CN114396941A (zh) * | 2021-12-20 | 2022-04-26 | 东南大学 | 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法 |
-
2023
- 2023-06-12 CN CN202310694378.5A patent/CN116774263B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105737828A (zh) * | 2016-05-09 | 2016-07-06 | 郑州航空工业管理学院 | 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法 |
| CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
| CN110823217A (zh) * | 2019-11-21 | 2020-02-21 | 山东大学 | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 |
| CN113048979A (zh) * | 2021-03-08 | 2021-06-29 | 中国矿业大学 | 一种组合导航滤波方法 |
| CN114396941A (zh) * | 2021-12-20 | 2022-04-26 | 东南大学 | 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法 |
Non-Patent Citations (1)
| Title |
|---|
| A Dual-Antenna GNSS-Assisted Low-Cost SINS Initial Alignment Method Based on ASTKF;Guangyan Xu et al.;2022 China Automation Congress (CAC);20230313;881-886 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116774263A (zh) | 2023-09-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11120562B2 (en) | Posture estimation method, posture estimation apparatus and computer readable storage medium | |
| CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
| CN110398245B (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
| CN106990426B (zh) | 一种导航方法和导航装置 | |
| CN109163721A (zh) | 姿态测量方法及终端设备 | |
| CN109030867B (zh) | 使用加速度传感器和地磁传感器计算角速度的方法和设备 | |
| CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
| CN112577521A (zh) | 一种组合导航误差校准方法及电子设备 | |
| CN112798021B (zh) | 基于激光多普勒测速仪的惯导系统行进间初始对准方法 | |
| CN110926464A (zh) | 一种基于双模式的惯性导航方法及系统 | |
| CN103712598A (zh) | 一种小型无人机姿态确定系统与确定方法 | |
| CN108627152A (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
| CN110332933A (zh) | 车辆定位方法、终端及计算机可读存储介质 | |
| CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
| CN116774263B (zh) | 面向组合导航系统的导航定位方法及装置 | |
| CN106931965B (zh) | 一种确定终端姿态的方法及装置 | |
| CN113959433B (zh) | 一种组合导航方法及装置 | |
| CN114993296B (zh) | 一种制导炮弹高动态组合导航方法 | |
| CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
| CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
| CN115856922A (zh) | 一种松耦合陆地组合导航方法、装置、计算机设备和介质 | |
| CN115031731A (zh) | 一种基于相对安装位置关系的多惯导协同导航方法及装置 | |
| CN114353825B (zh) | 基于无迹卡尔曼滤波的磁力计在线校准算法、介质及系统 | |
| CN115164888B (zh) | 一种误差修正方法、装置、电子设备和存储介质 | |
| US20240085453A1 (en) | Method for evaluating sensor data, processing unit for evaluating sensor data, and sensor system |
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 |