CN111308532A - Gps/ins紧组合的导航方法及装置、定位*** - Google Patents

Gps/ins紧组合的导航方法及装置、定位*** Download PDF

Info

Publication number
CN111308532A
CN111308532A CN201811523127.6A CN201811523127A CN111308532A CN 111308532 A CN111308532 A CN 111308532A CN 201811523127 A CN201811523127 A CN 201811523127A CN 111308532 A CN111308532 A CN 111308532A
Authority
CN
China
Prior art keywords
filter
processing
time
navigation
representing
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
CN201811523127.6A
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.)
Qianxun Spatial Intelligence Inc
Qianxun Position Network Co Ltd
Original Assignee
Qianxun Spatial Intelligence Inc
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 Qianxun Spatial Intelligence Inc filed Critical Qianxun Spatial Intelligence Inc
Priority to CN201811523127.6A priority Critical patent/CN111308532A/zh
Publication of CN111308532A publication Critical patent/CN111308532A/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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明适用于卫星定位技术领域，提供了一种GPS/INS紧组合的导航方法及装置、定位***，所述导航方法包括：构建GPS/INS紧组合的滤波器；对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；基于所述序贯处理的滤波器输出导航数据。本发明中，对基础GPS/INS紧组合的滤波器进行降维处理，然后进行序贯处理，然后基于序贯处理的滤波器进行导航解算获得导航数据，可降低运算复杂度。

Description

GPS/INS紧组合的导航方法及装置、定位***
技术领域
本发明属于卫星定位技术领域，尤其涉及一种GPS/INS紧组合的导航方法及装置、定位***。
背景技术
GPS通过获取卫星的位置、速度以及卫星信号到接收机的传播用时，最终实现对载体的定位、定速，具有误差不随时间积累、适用范围广、用户所需成本低廉等优点，但由于涉及外部信号的处理，其精度受外部环境和载体动态的影响较大。INS(inertial navigationsystem，惯性导航)则通过对惯性器件的测量值进行积分运算，可以实现完全自主导航，受外部环境和载体动态的影响较小，但是其精度却因惯性器件误差而随时间逐渐积累。
现有技术中，为了获得较好的导航解算结果，将GPS与INS结合起来进行定位解算。利用GPS各跟踪通道输出的伪距、伪距率和星历以及INS输出的位置、速度获取导航滤波器观测量，则可以构成GPS/INS紧组合***，但前述紧组合方式涉及高维矩阵运算，计算过程复杂度高。
发明内容
本发明实施例提供了一种GPS/INS紧组合的导航方法及装置、定位***，旨在解决现有技术的紧组合导航方式的计算复杂度较高的问题。
一种GPS/INS紧组合的导航方法，包括：
构建GPS/INS紧组合的导航滤波器；
对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；基于所述序贯处理的滤波器输出导航数据。
优选地，对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器具体为：
基于两阶段卡尔曼滤波对状态方程进行降维处理，得到降维处理的滤波器。
优选地，所述降维处理的滤波器具体为：
Figure BDA0001903050830000021
其中，所述Xk表示第k时刻的状态向量，Φk，k-1表示由k-1时刻至k时刻的状态转移矩阵，Xk-1表示第k-1时刻的状态向量，Bk-1表示第k-1时刻的常值偏差向量，Bk表示第k时刻的常值偏差向量，Ψk，k-1表示由k-1时刻至k时刻的常值偏差转移矩阵，ΓK，k-1表示由k-1时刻至k时刻的状态噪声输入矩阵，Wk-1表示第k-1时刻的状态噪声向量，Zk表示第k时刻的观测向量，Vk表示第k时刻的观测噪声向量，Hk表示第k时刻的观测矩阵。
优选地，对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器包括：
对所述降维处理的滤波器进行第一滤波处理，得到状态向量的初步估计值，所述第一滤波处理为序贯滤波处理；
对所述降维处理的滤波器进行第二滤波处理，得到状态向量的最终估计值。
优选地，对所述降维处理的滤波器进行第一滤波处理，具体为：
对所述降维处理的滤波器进行无常值偏差序贯滤波处理，得到状态向量的初步估计值。
优选地，对所述降维处理的滤波器进行第二滤波处理，得到状态向量的最终估计值具体为：
对所述降维处理的滤波器进行常值偏差滤波处理，得到常值偏差向量的估计值，基于所述状态向量的初步估计值及常值偏差向量的估计值得到状态向量的最终估计值。
优选地，对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器之后还包括：
采用三元组表方式存储所述序贯处理的滤波器。
优选地，采用三元组表方式存储所述序贯处理的滤波器包括：
基于所述序贯处理的滤波器的每一矩阵的非零元素构建三元组；
按预设规则对所述三元组进行运算处理；
存储所述运算处理结果。
本发明还提供一种GPS/INS紧组合的导航装置，包括：
构建单元，用于构建GPS/INS紧组合的导航滤波器；
降维单元，用于对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
序贯处理单元，用于对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
输出单元，用于基于所述序贯处理的滤波器输出导航数据。
本发明还提供一种定位***，所述定位***包括GPS/INS紧组合的导航装置，所述导航装置包括：
构建单元，用于构建GPS/INS紧组合的导航滤波器；
降维单元，用于对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
序贯处理单元，用于对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
输出单元，用于基于所述序贯处理的滤波器输出导航数据。本发明还提供一种存储器，所述存储器存储有计算机程序，所述计算机程序被处理器执行如下步骤：
构建GPS/INS紧组合的导航滤波器；
对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
基于所述序贯处理的滤波器输出导航数据。
本发明还提供一种定位终端，包括存储器、处理器及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
构建GPS/INS紧组合的导航滤波器；
对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
基于所述序贯处理的滤波器输出导航数据。
本发明实施例中，对基础GPS/INS紧组合的导航滤波器进行降维处理，对降维处理的滤波器进一步进行序贯处理，基于经过序贯处理的滤波器进行导航解算获得导航数据，可降低运算复杂度。
附图说明
图1为本发明第一实施例提供的一种GPS/INS紧组合的导航方法的流程图；
图2为本发明第一实施例提供的一种GPS/INS紧组合的导航方法的一优选方式的流程图；
图3为本发明第一实施例提供的一种GPS/INS紧组合的导航方法的步骤S3的具体流程图；
图4为本发明第一实施例提供的一种GPS/INS紧组合的导航方法的步骤S5的具体流程图；
图5为本发明第二实施例提供的一种GPS/INS紧组合的导航装置的结构图；
图6为本发明第三实施例提供的一种定位终端的结构图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明实施例中，一种GPS/INS紧组合的导航方法，包括：构建GPS/INS紧组合的滤波器；对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；基于所述序贯处理的滤波器输出导航数据。
为了说明本发明所述的技术方案，下面通过具体实施例来进行说明。
实施例一：
图1示出了本发明第一实施例提供的一种GPS/INS紧组合的导航方法的流程图，该导航方法包括：
步骤S1，构建GPS/INS紧组合的滤波器；
具体地，首先构建GPS/INS紧组合的滤波器(导航滤波器)，该滤波器即为基础GPS/INS紧组合的导航滤波器，基于GPS/INS的组合构建，首先，导航滤波器从GPS接收机的每一跟踪通道获得卫星数据，该卫星数据可包括：可见星的伪距、伪距率、星历信息等，并通过INS获得用户的位置数据，该位置数据可包括：位置信息及速度信息，基于前述卫星数据及位置数据估计出导航滤波器状态向量的各项误差，该GPS/INS紧组合导航滤波器的状态方程具体为：
Figure BDA0001903050830000051
其中，所述k表示时刻，且k为大于0的实数，
Figure BDA0001903050830000052
表示第k时刻的传统形式的状态向量，并且
Figure BDA0001903050830000053
Figure BDA0001903050830000054
表示由第k-1时刻至第k时刻之间的传统形式的状态转移矩阵，并且
Figure BDA0001903050830000055
Figure BDA0001903050830000056
表示第k-1时刻的传统形式的状态向量，
Figure BDA0001903050830000057
表示由第k-1时刻至第k时刻之间的传统形式的状态噪声输入矩阵，并且
Figure BDA0001903050830000061
Figure BDA0001903050830000062
表示第k-1时刻的状态噪声向量，并且
Figure BDA0001903050830000063
上标T表示矩阵转置，
Figure BDA0001903050830000064
δvENU，δpLλh，εb，xyz及
Figure BDA0001903050830000065
分别表示INS的姿态误差、速度误差、位置误差、陀螺常值漂移以及加速度计常值零偏在各个坐标轴上的分量，δtu和δtru分别表示接收机的钟差、钟漂，wg，xyz和wa，xyz分别表示陀螺和加速度计的噪声，wtu和wtru分别表示钟差和钟漂的噪声，
Figure BDA0001903050830000066
表示状态转移矩阵中与INS误差状态有关的第1个子矩阵，
Figure BDA0001903050830000067
表示状态转移矩阵中与INS误差状态有关的第2个子矩阵，
Figure BDA0001903050830000068
表示状态转移矩阵中与GPS误差状态有关的第1个子矩阵，
Figure BDA0001903050830000069
表示状态噪声输入矩阵中与INS误差状态有关的第1个子矩阵，
Figure BDA00019030508300000610
表示状态噪声输入矩阵中与GPS误差状态有关的第1个子矩阵。
进一步地，将m个GPS跟踪通道输出的伪距
Figure BDA00019030508300000611
伪距率
Figure BDA00019030508300000612
与INS结合星历推算得到的伪距
Figure BDA00019030508300000613
伪距率
Figure BDA00019030508300000614
分别作差，即可得到导航滤波器的观测量，并且观测方程可以表示为：
Figure BDA00019030508300000615
其中，
Figure BDA00019030508300000616
表示第k时刻的传统形式的观测向量，并且
Figure BDA00019030508300000617
Figure BDA00019030508300000618
表示第k时刻的传统形式的观测矩阵，并且
Figure BDA00019030508300000619
Figure BDA00019030508300000620
表示第k时刻的传统形式的观测噪声向量，并且
Figure BDA0001903050830000071
δρi和
Figure BDA0001903050830000072
分别表示伪距差和伪距率差，
Figure BDA0001903050830000073
和
Figure BDA0001903050830000074
分别表示伪距、伪距率的观测噪声，这里i＝1，2，…，m，
Figure BDA0001903050830000075
表示
Figure BDA0001903050830000076
中与INS误差状态有关的第1个子矩阵，
Figure BDA0001903050830000077
表示观测矩阵中与INS误差状态有关的第2个子矩阵。
步骤S2，对滤波器的状态方程进行降维处理，得到降维处理的滤波器；
具体地，由于从前述滤波器的状态方程可看出，在15维与INS有关的状态量中，有6维状态量是不可观测的，由于INS受到自身惯性器件随机常值偏差影响，陀螺仪常值漂移和加速度计常值零偏，为了提高参数的估计精度，需要将常值偏差扩充为状态向量，也为了避免高维度矩阵运算，此时需要对前述状态方程进行降维处理，得到降维处理的滤波器。
具体地，基于两阶段卡尔曼滤波对滤波器的状态方程进行降维处理，前述滤波器变为：
Figure BDA0001903050830000078
其中，Xk表示第k时刻的状态向量，并且
Figure BDA0001903050830000079
Φk，k-1表示由k-1时刻至k时刻的状态转移矩阵，并且
Figure BDA00019030508300000710
Xk-1表示第k-1时刻的状态向量，Bk表示第k时刻的常值偏差向量，Bk-1表示第k-1时刻的常值偏差向量，并且
Figure BDA00019030508300000711
Ψk，k-1表示由k-1时刻至k时刻的常值偏差转移矩阵，并且
Figure BDA00019030508300000712
Γk，k-1表示由k-1时刻至k时刻的状态噪声输入矩阵，并且
Figure BDA00019030508300000713
Wk-1表示第k-1时刻的状态噪声向量，并且Wk-1＝[wg，xyz wa，xyz wtu wtru]T，Bk表示第k时刻的常值偏差向量，Zk表示第k时刻的观测向量，并且
Figure BDA0001903050830000081
Hk表示第k时刻的观测矩阵，并且
Figure BDA0001903050830000082
Vk表示第k时刻的观测噪声向量，并且
Figure BDA0001903050830000083
步骤S3，对降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
具体地，从前述经过降维处理的滤波器看出，其观测方程的维数与2m成比例(m为GPS跟踪通道数量，即可见星数量)，在GPS观测条件较好的开阔区域，导航滤波器在利用观测信息求解滤波增益矩阵时会出现高维矩阵求逆的问题，而矩阵求逆的计算量与矩阵维数的3次方成正比，因此需要对前述降维处理的滤波器进行改进，例如进一步对降维处理的滤波器进行序贯处理(即序贯滤波处理)，得到序贯处理的滤波器。
步骤S4，基于序贯处理的滤波器输出导航数据；
具体地，基于序贯处理的滤波器输出对应的导航数据；
在本实施例中，对基础GPS/INS紧组合的滤波器进行降维处理和序贯处理后，进行导航解算，获得导航数据，可降低运算复杂度。
在本实施例的一个优选方案中，该步骤S3之后、步骤S4之前还可包括(见图2)：
步骤S5，采用三元组表方式存储序贯处理的滤波器；
具体地，由于GPS/INS紧组合导航滤波器在工作过程中涉及到较多对稀疏矩阵的操作。例如，Hk共有22m个元素，但仅有8m个为非零值，如果在嵌入式设备中采用常规分配方法存储这些矩阵，将会产生显著的内存浪费，并且会在进行访问操作时造成大量的时间浪费，因此采用三元组表方式存储序贯处理的滤波器，以节省存储空间，减少算法耗时。
在本实施例的一个优选方案中，如图3所示，为本发明第一实施例提供的一种GPS/INS紧组合的导航方法的步骤S3的具体流程图，该步骤S3具体包括：
步骤S31，对降维处理的滤波器进行第一滤波处理，得到状态向量的初步估计值，所述第一滤波处理为序贯滤波处理；
具体地，对降维处理的滤波器进行第一滤波处理(序贯滤波处理)，得到状态向量的初步估计值，优选地，对降维处理的滤波器进行无常值偏差序贯滤波处理，例如：由于组合的各观测误差相互独立，基于序贯处理将观测更新中对观测向量的集中处理分散为对各分量的顺序处理，即得到状态向量Xk的初步估计值
Figure BDA0001903050830000091
过程如下：
Figure BDA0001903050830000092
Figure BDA0001903050830000093
令
Figure BDA0001903050830000094
及
Figure BDA0001903050830000095
再对2m个观测量依次进行循环处理。其中，第j个观测量的处理方式为：
Figure BDA0001903050830000096
Figure BDA0001903050830000097
Figure BDA0001903050830000098
其中，上标j表示第j个观测量的相应参数，j＝1，2，…，2m，循环结束后，令
Figure BDA0001903050830000099
及
Figure BDA00019030508300000910
进一步地，所述
Figure BDA00019030508300000911
表示无常值偏差序贯滤波中从第k-1时刻至第k时刻的状态向量一步预测值，
Figure BDA00019030508300000912
表示无常值偏差序贯滤波中第k-1时刻的状态向量估计值，
Figure BDA00019030508300000913
表示无常值偏差序贯滤波中从第k-1时刻至第k时刻的一步预测误差方差矩阵，
Figure BDA00019030508300000914
表示无常值偏差序贯滤波中第k-1时刻的估计误差方差矩阵，
Figure BDA00019030508300000915
表示Φk，k-1的转置矩阵，Qk-1表示第k-1时刻的状态噪声方差矩阵，
Figure BDA00019030508300000916
表示Γk，k-1的转置矩阵，
Figure BDA00019030508300000917
和
Figure BDA00019030508300000918
分别表示无常值偏差序贯滤波中
Figure BDA00019030508300000919
和
Figure BDA00019030508300000920
的初始值，
Figure BDA00019030508300000921
表示无常值偏差序贯滤波中第k时刻第j个观测量对应的状态向量估计值，
Figure BDA0001903050830000101
表示无常值偏差序贯滤波中第k-1时刻第j个观测量对应的状态向量估计值；
Figure BDA0001903050830000102
表示无常值偏差序贯滤波中第k时刻第j个观测量对应的估计误差方差矩阵，
Figure BDA0001903050830000103
表示表示无常值偏差序贯滤波中第k时刻第j-1个观测量对应的估计误差方差矩阵，
Figure BDA0001903050830000104
表示无常值偏差序贯滤波中第k时刻第j个观测量对应的增益矩阵，
Figure BDA0001903050830000105
表示无常值偏差序贯滤波中第k时刻第j个观测量对应的观测矩阵，
Figure BDA0001903050830000106
表示表示无常值偏差序贯滤波中第k时刻第j个观测量对应的观测矩阵的转置矩阵，
Figure BDA0001903050830000107
表示第k时刻第j个观测量对应的观测噪声方差，I表示单位矩阵，
Figure BDA0001903050830000108
表示无常值偏差序贯滤波中第k时刻第j个观测量。
步骤S32，对降维处理的滤波器进行第二滤波处理，得到状态向量的最终估计值；
具体地，对降维处理的滤波器进行第二滤波处理，得到常值偏差向量估计值；
优选地，首先对降维处理的滤波器进行常值偏差滤波处理，得到常值偏差向量估计值，过程如下：
Figure BDA0001903050830000109
Figure BDA00019030508300001010
Uk＝Φk，k-1Tk-1+Ψk，k-1；
Figure BDA00019030508300001011
Sk＝HkUk；
Figure BDA00019030508300001012
Figure BDA00019030508300001013
Figure BDA00019030508300001014
其中，所述
Figure BDA00019030508300001015
表示常值偏差滤波中第k-1时刻的常值偏差向量估计值，
Figure BDA00019030508300001016
表示常值偏差滤波中由第k-1时刻至第k时刻的常值偏差向量一步预测值，
Figure BDA0001903050830000111
表示常值偏差滤波中由第k-1时刻至第k时刻的常值偏差向量一步预测误差方差矩阵，
Figure BDA0001903050830000112
表示常值偏差滤波中第k-1时刻的估计误差方差矩阵，Uk、Tk、Sk为常值偏差滤波中的三个中间矩阵，
Figure BDA0001903050830000113
表示常值偏差滤波中第k时刻的增益矩阵，
Figure BDA0001903050830000114
表示Sk的转置矩阵，Rk表示第k时刻的观测噪声方差矩阵。
然后，再结合前述状态向量的初步估计值
Figure BDA0001903050830000115
和常值偏差向量的估计值
Figure BDA0001903050830000116
得到状态向量的最终估计值
Figure BDA0001903050830000117
即：
Figure BDA0001903050830000118
需要说明的是，上述第一滤波处理及第二滤波处理并无限制作用，仅用于区分序贯滤波处理及后续的滤波处理。
本实施例中，由于无常值偏差滤波和常值偏差滤波可以并行执行，当INS具有较小器件误差时，后者的更新频率可以被适当地设置为低于前者，这为在精度指标基本不变的前提下进一步提高组合***的运算速度。
在本实施例的一个优选方案中，如图4所示，为本发明第一实施例提供的一种GPS/INS紧组合的导航方法的步骤S5的具体流程图，该步骤S5具体包括：
步骤S51，基于序贯处理的滤波器的每一矩阵的非零元素构建三元组；
具体地，首先基于序贯处理的滤波器的每一矩阵的非零元素构建三元组，例如将各个矩阵的非零元素所在行、列及其值构成一个三元组(row，col，value)。
步骤S52，按预设规则对三元组进行运算处理；
具体地，根据预设规则对前述三元组进行运算处理，例如对三元组矩阵基于加减法、乘法、求逆、转置等对三元组表进行运算，得到运算结果。该预设规则可根据实际情况而设，此处对此不作限制。
步骤S53，存储运算处理结果；
在本实施例中，对基础6PS/INS紧组合的导航滤波器进行降维处理和序贯处理后，进行导航解算，获得导航数据，可降低运算复杂度。
其次，采用三元组表方式存储导航数据，可进一步降低运算复杂度并节省存储空间。
实施例二：
基于上述实施例一，如图5所示，为本发明第二实施例提供的一种GPS/INS紧组合的导航装置的结构图，该导航装置包括：构建单元1、与构建单元1连接的降维单元2、与降维单元2连接的序贯处理单元3、与序贯处理单元连接的输出单元4，其中：
构建单元1，用于构建GPS/INS紧组合的导航滤波器；
具体地，首先构建GPS/INS紧组合的滤波器(导航滤波器)，该滤波器即为基础GPS/INS紧组合的滤波器，基于GPS/INS的组合构建，首先，导航滤波器从GPS接收机的每一跟踪通道获得卫星数据，该卫星数据可包括：可见星的伪距、伪距率、星历信息等，并通过INS获得用户的位置数据，该位置数据可包括：位置信息及速度信息，基于前述卫星数据及位置数据估计出导航滤波器状态向量的各项误差，该GPS/INS紧组合滤波器的状态方程具体为：
Figure BDA0001903050830000121
其中，所述k表示时刻，且k为大于0的实数，
Figure BDA0001903050830000122
表示第k时刻的传统形式的状态向量，并且
Figure BDA0001903050830000123
Figure BDA0001903050830000124
表示由第k-1时刻至第k时刻之间的传统形式的状态转移矩阵，并且
Figure BDA0001903050830000125
Figure BDA0001903050830000126
表示第k-1时刻的传统形式的状态向量，
Figure BDA0001903050830000127
表示由第k-1时刻至第k时刻之间的传统形式的状态噪声输入矩阵，并且
Figure BDA0001903050830000131
Figure BDA0001903050830000132
表示第k-1时刻的状态噪声向量，并且
Figure BDA0001903050830000133
上标T表示矩阵转置，
Figure BDA0001903050830000134
δvENU，δpLλh，εb，xyz及
Figure BDA0001903050830000135
分别表示INS的姿态误差、速度误差、位置误差、陀螺常值漂移以及加速度计常值零偏在各个坐标轴上的分量，δtu和δtru分别表示接收机的钟差、钟漂，wg，xyz和wa，xyz分别表示陀螺和加速度计的噪声，wtu和wtru分别表示钟差和钟漂的噪声，
Figure BDA0001903050830000136
表示状态转移矩阵中与INS误差状态有关的第1个子矩阵，
Figure BDA0001903050830000137
表示状态转移矩阵中与INS误差状态有关的第2个子矩阵，
Figure BDA0001903050830000138
表示状态转移矩阵中与GPS误差状态有关的第1个子矩阵，
Figure BDA0001903050830000139
表示状态噪声输入矩阵中与INS误差状态有关的第1个子矩阵，
Figure BDA00019030508300001310
表示状态噪声输入矩阵中与GPS误差状态有关的第1个子矩阵。。
进一步地，将m个GPS跟踪通道输出的伪距
Figure BDA00019030508300001311
伪距率
Figure BDA00019030508300001312
与INS结合星历推算得到的伪距
Figure BDA00019030508300001313
伪距率
Figure BDA00019030508300001314
分别作差，即可得到导航滤波器的观测量，并且观测方程可以表示为：
Figure BDA00019030508300001315
其中，
Figure BDA00019030508300001316
表示第k时刻的传统形式的观测向量，并且
Figure BDA00019030508300001317
Figure BDA00019030508300001318
表示第k时刻的传统形式的观测矩阵，并且
Figure BDA00019030508300001319
Figure BDA00019030508300001320
表示第k时刻的传统形式的观测噪声向量，并且
Figure BDA00019030508300001321
δρi和
Figure BDA00019030508300001322
分别表示伪距差和伪距率差，
Figure BDA00019030508300001323
和
Figure BDA0001903050830000141
分别表示伪距、伪距率的观测噪声，这里i＝1，2，…，m，
Figure BDA0001903050830000142
表示
Figure BDA0001903050830000143
中与INS误差状态有关的第1个子矩阵，
Figure BDA0001903050830000144
表示观测矩阵中与INS误差状态有关的第2个子矩阵。
降维单元2，用于对滤波器的状态方程进行降维处理，得到降维处理的滤波器；
具体地，由于从前述滤波器的状态方程可看出，在15维与INS有关的状态量中，有6维状态量是不可观测的，由于INS受到自身惯性器件随机常值偏差影响，陀螺仪常值漂移和加速度计常值零偏，为了提高参数的估计精度，需要将常值偏差扩充为状态向量，也为了避免高维度矩阵运算，此时需要对前述状态方程进行降维处理，得到降维处理的滤波器。
具体地，基于两阶段卡尔曼滤波对滤波器的状态方程进行降维处理，前述滤波器变为：
Figure BDA0001903050830000145
其中，Xk表示第k时刻的状态向量，并且
Figure BDA0001903050830000146
Φk，k-1表示由k-1时刻至k时刻的状态转移矩阵，并且
Figure BDA0001903050830000147
Xk-1表示第k-1时刻的状态向量，Bk表示第k时刻的常值偏差向量，Bk-1表示第k-1时刻的常值偏差向量，并且
Figure BDA0001903050830000148
Ψk，k-1表示由k-1时刻至k时刻的常值偏差转移矩阵，并且
Figure BDA0001903050830000149
Γk，k-1表示由k-1时刻至k时刻的状态噪声输入矩阵，并且
Figure BDA00019030508300001410
Wk-1表示第k-1时刻的状态噪声向量，并且Wk-1＝[wg，xy wa，xyz wtu wtru]T，Bk表示第k时刻的常值偏差向量，Zk表示第k时刻的观测向量，并且
Figure BDA0001903050830000151
Hk表示第k时刻的观测矩阵，并且
Figure BDA0001903050830000152
Vk表示第k时刻的观测噪声向量，并且
Figure BDA0001903050830000153
序贯处理单元3，用于对降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
具体地，从前述经过降维处理的滤波器看出，其观测方程的维数与2m成比例(m为GPS跟踪通道数量，即可见星数量)，在GPS观测条件较好的开阔区域，导航滤波器在利用观测信息求解滤波增益矩阵时会出现高维矩阵求逆的问题，而矩阵求逆的计算量与矩阵维数的3次方成正比，因此需要对前述降维处理的滤波器进行改进，例如进一步对降维处理的滤波器进行序贯处理，得到序贯处理的滤波器。
输出单元4，用于基于序贯处理的滤波器输出导航数据；
具体地，基于序贯处理的滤波器输出对应的导航数据；
在本实施例中，对基础GPS/INS紧组合的导航滤波器进行降维处理后进行序贯处理，然后基于序贯处理的滤波器进行导航解算，获得导航数据，可降低运算复杂度。
在本实施例的一个优选方案中，该导航装置还包括：与降维单元2连接的序贯处理单元4，其中：
在本实施例的一个优选方案中，该导航装置还可包括：与序贯处理单元4连接的存储处理单元5，其中：
存储处理单元5，用于采用三元组表方式存储序贯处理的滤波器；
具体地，由于GPS/INS紧组合导航滤波器在工作过程中涉及到较多对稀疏矩阵的操作。例如，Hk共有22m个元素，但仅有8m个为非零值，如果在嵌入式设备中采用常规分配方法存储这些矩阵，将会产生显著的内存浪费，并且会在进行访问操作时造成大量的时间浪费，因此采用三元组表方式存储序贯处理的滤波器，以节省存储空间，减少算法耗时。
在本实施例的一个优选方案中，该序贯处理单元3具体包括第一处理子单元及与其连接的第二处理子单元，其中：
第一处理子单元，用于对降维处理的滤波器进行第一滤波处理，得到状态向量的初步估计值；
具体地，对降维处理的滤波器进行第一滤波处理，得到状态向量的初步估计值，该第一滤波处理为序贯滤波处理；
具体地，对降维处理的滤波器进行序贯滤波处理，得到状态向量的初步估计值，优选地，对降维处理的滤波器进行无常值偏差序贯滤波处理，例如：由于组合的各观测误差相互独立，基于序贯处理将观测更新中对观测向量的集中处理分散为对各分量的顺序处理，即得到状态向量Xk的初步估计值
Figure BDA00019030508300001612
过程如下：
Figure BDA0001903050830000161
Figure BDA0001903050830000162
令
Figure BDA0001903050830000163
及
Figure BDA0001903050830000164
再对2m个观测量依次进行循环处理。其中，第j个观测量的处理方式为：
Figure BDA0001903050830000165
Figure BDA0001903050830000166
Figure BDA0001903050830000167
其中，上标j表示第j个观测量的相应参数，j＝1，2，…，2m，循环结束后，令
Figure BDA0001903050830000168
及
Figure BDA0001903050830000169
进一步地，所述
Figure BDA00019030508300001610
表示无常值偏差序贯滤波中从第k-1时刻至第k时刻的状态向量一步预测值，
Figure BDA00019030508300001611
表示无常值偏差序贯滤波中第k-1时刻的状态向量估计值，
Figure BDA0001903050830000171
表示无常值偏差序贯滤波中从第k一1时刻至第k时刻的一步预测误差方差矩阵，
Figure BDA0001903050830000172
表示无常值偏差序贯滤波中第k-1时刻的估计误差方差矩阵，
Figure BDA0001903050830000173
表示Φk，k-1的转置矩阵，Qk-1表示第k-1时刻的状态噪声方差矩阵，
Figure BDA0001903050830000174
表示Γk，k-1的转置矩阵，
Figure BDA0001903050830000175
和
Figure BDA00019030508300001720
分别表示无常值偏差序贯滤波中
Figure BDA00019030508300001719
和
Figure BDA0001903050830000176
的初始值，
Figure BDA0001903050830000177
表示无常值偏差序贯滤波中第k时刻第j个观测量对应的状态向量估计值，
Figure BDA0001903050830000178
表示无常值偏差序贯滤波中第k-1时刻第j个观测量对应的状态向量估计值；
Figure BDA0001903050830000179
表示无常值偏差序贯滤波中第k时刻第j个观测量对应的估计误差方差矩阵，
Figure BDA00019030508300001710
表示表示无常值偏差序贯滤波中第k时刻第j-1个观测量对应的估计误差方差矩阵，
Figure BDA00019030508300001711
表示无常值偏差序贯滤波中第k时刻第j个观测量对应的增益矩阵，
Figure BDA00019030508300001712
表示无常值偏差序贯滤波中第k时刻第j个观测量对应的观测矩阵，
Figure BDA00019030508300001713
表示表示无常值偏差序贯滤波中第k时刻第j个观测量对应的观测矩阵的转置矩阵，
Figure BDA00019030508300001714
表示第k时刻第j个观测量对应的观测噪声方差，I表示单位矩阵，
Figure BDA00019030508300001715
表示无常值偏差序贯滤波中第k时刻第j个观测量。
第二处理子单元，用于对降维处理的滤波器进行第二滤波处理，得到状态向量的最终估计值；
具体地，对降维处理的滤波器进行第二滤波处理，得到常值偏差向量估计值；
优选地，首先对降维处理的滤波器进行常值偏差滤波处理，得到常值偏差向量估计值，过程如下：
Figure BDA00019030508300001716
Figure BDA00019030508300001717
Uk＝Φk，k-1Tk-1+Ψk，k-1；
Figure BDA00019030508300001718
Sk＝HkUk；
Figure BDA0001903050830000181
Figure BDA0001903050830000182
Figure BDA0001903050830000183
其中，所述
Figure BDA0001903050830000184
表示常值偏差滤波中第k-1时刻的常值偏差向量估计值，
Figure BDA0001903050830000185
表示常值偏差滤波中由第k-1时刻至第k时刻的常值偏差向量一步预测值，
Figure BDA0001903050830000186
表示常值偏差滤波中由第k-1时刻至第k时刻的常值偏差向量一步预测误差方差矩阵，
Figure BDA0001903050830000187
表示常值偏差滤波中第k-1时刻的估计误差方差矩阵，Uk、Tk、Sk为常值偏差滤波中的三个中间矩阵，
Figure BDA0001903050830000188
表示常值偏差滤波中第k时刻的增益矩阵，
Figure BDA0001903050830000189
表示Sk的转置矩阵，Rk表示第k时刻的观测噪声方差矩阵。
然后，再结合前述状态向量的初步估计值
Figure BDA00019030508300001810
和常值偏差向量的估计值
Figure BDA00019030508300001811
得到状态向量的最终估计值
Figure BDA00019030508300001812
即
Figure BDA00019030508300001813
在本实施例中，由于无常值偏差滤波和常值偏差滤波可以并行执行，当INS具有较小器件误差时，后者的更新频率可以被适当地设置为低于前者，这为在精度指标基本不变的前提下进一步提高组合***的运算速度。
在本实施例的一个优选方案中，该存储处理单元5具体包括：构建子单元、与构建子单元连接的运算子单元、与运算子单元连接的存储子单元，其中：
构建子单元，用于基于序贯处理的滤波器的每一矩阵的非零元素构建三元组；
具体地，首先基于序贯处理的滤波器的每一矩阵的非零元素构建三元组，例如将各个矩阵的非零元素所在行、列及其值构成一个三元组(row，col，value)。
运算子单元，用于按预设规则对三元组进行运算处理；
具体地，根据预设规则对前述三元组进行运算处理，例如对三元组矩阵基于加减法、乘法、求逆、转置等对三元组表进行运算，得到运算结果。该预设规则可根据实际情况而设，此处对此不作限制。
存储子单元，用于存储运算处理结果；
在本实施例中，对基础GPS/INS紧组合的导航滤波器进行降维处理和序贯处理后，进行导航解算，获得导航数据，可降低运算复杂度。
其次，采用三元组表方式存储导航数据，可进一步降低运算复杂度并节省存储空间。
在本发明中，还提供一种定位***，该定位***包括如上述实施例二描述的GPS/INS紧组合的导航装置，该导航装置的具体结构、工作原理及所带来的技术效果与上述实施例二的描述基本一致，此处不再赘述；
另外，该定位***还包括GPS导航单元及INS惯导单元。
实施例三：
图6示出了本发明第三实施例提供的一种定位终端的结构图，该定位终端包括：存储器(memory)61、处理器(processor)62、通信接口(Communications Interface)63和总线64，该处理器62、存储器61、通信接口63通过总线64完成相互之间的交互通信。
存储器61，用于存储各种数据；
具体地，存储器61用于存储各种数据，例如通信过程中的数据、接收的数据等，此处对此不作限制，该存储器还包括有多个计算机程序。
通信接口63，用于该定位终端的通信设备之间的信息传输；
处理器62，用于调用存储器61中的各种计算机程序，以执行上述实施例一所提供的一种GPS/INS紧组合的导航方法，例如：
构建GPS/INS紧组合的滤波器；
对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
基于所述序贯处理的滤波器输出导航数据。
本实施例中，对基础GPS/INS紧组合的导航滤波器进行降维处理后再进行序贯处理，然后基于序贯处理的滤波器进行导航解算，获得导航数据，可降低运算复杂度。
本发明还提供一种存储器，该存储器存储有多个计算机程序，该多个计算机程序被处理器调用执行上述实施例一所述的一种GPS/INS紧组合的导航方法。
本发明中，对基础GPS/INS紧组合的导航滤波器进行降维处理后再进行序贯处理，然后基于序贯处理的滤波器进行导航解算，获得导航数据，可降低运算复杂度。
其次，采用三元组表方式存储导航数据，可节省存储空间。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。
专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (12)

1.一种GPS/INS紧组合的导航方法，其特征在于，包括：
构建GPS/INS紧组合的导航滤波器；
对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
基于所述序贯处理的滤波器输出导航数据。
2.根据权利要求1所述的导航方法，其特征在于，对所述滤波器的状态方程进行降维处理，得到降维处理的状态方程具体为：
基于两阶段卡尔曼滤波对状态方程进行降维处理，得到降维处理的滤波器。
3.根据权利要求2所述的导航方法，其特征在于，所述降维处理的滤波器具体为：
Figure FDA0001903050820000011
其中，所述Xk表示第k时刻的状态向量，Φk，k-1表示由k-1时刻至k时刻的状态转移矩阵，Xk-1表示第k-1时刻的状态向量，Bk-1表示第k-1时刻的常值偏差向量，Bk表示第k时刻的常值偏差向量，Ψk，k-1表示由k-1时刻至k时刻的常值偏差转移矩阵，ΓK，k-1表示由k-1时刻至k时刻的状态噪声输入矩阵，Wk-1表示第k-1时刻的状态噪声向量，Zk表示第k时刻的观测向量，Vk表示第k时刻的观测噪声向量，Hk表示第k时刻的观测矩阵。
4.根据权利要求3所述的导航方法，其特征在于，对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器包括：
对所述降维处理的滤波器进行第一滤波处理，得到状态向量的初步估计值，所述第一滤波处理为序贯滤波处理；
对所述降维处理的滤波器进行第二滤波处理，得到状态向量的最终估计值。
5.根据权利要求4所述的导航方法，其特征在于，对所述降维处理的滤波器进行第一滤波处理，具体为：
对所述降维处理的滤波器进行无常值偏差序贯滤波处理，得到状态向量的初步估计值。
6.根据权利要求5所述的导航方法，其特征在于，对所述降维处理的滤波器进行第二滤波处理，得到状态向量的最终估计值具体为：
对所述降维处理的滤波器进行常值偏差滤波处理，得到常值偏差向量的估计值，基于所述状态向量的初步估计值及常值偏差向量的估计值得到状态向量的最终估计值。
7.根据权利要求1至6任意一项所述的导航方法，其特征在于，对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器之后还包括：
采用三元组表方式存储所述序贯处理的滤波器。
8.根据权利要求7所述的导航方法，其特征在于，采用三元组表方式存储所述序贯处理的滤波器包括：
基于所述序贯处理的滤波器的每一矩阵的非零元素构建三元组；
按预设规则对所述三元组进行运算处理；
存储所述运算处理结果。
9.一种GPS/INS紧组合的导航装置，其特征在于，包括：
构建单元，用于构建GPS/INS紧组合的导航滤波器；
降维单元，用于对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
序贯处理单元，用于对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
输出单元，用于基于所述序贯处理的滤波器输出导航数据。
10.一种定位***，其特征在于，包括如权利要求9所述的GPS/INS紧组合的导航装置。
11.一种存储器，所述存储器存储有计算机程序，其特征在于，所述计算机程序被处理器执行如下步骤：
构建GPS/INS紧组合的导航滤波器；
对所述滤波器的状态方程进行降维处理，得到降维处理的滤波器；
对所述降维处理的滤波器进行序贯处理，得到序贯处理的滤波器；
基于所述序贯处理的滤波器输出导航数据。
12.一种定位终端，包括存储器、处理器及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至8任意一项所述的GPS/INS紧组合的导航方法的步骤。
CN201811523127.6A 2018-12-12 2018-12-12 Gps/ins紧组合的导航方法及装置、定位*** Pending CN111308532A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811523127.6A CN111308532A (zh) 2018-12-12 2018-12-12 Gps/ins紧组合的导航方法及装置、定位***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811523127.6A CN111308532A (zh) 2018-12-12 2018-12-12 Gps/ins紧组合的导航方法及装置、定位***

Publications (1)

Publication Number Publication Date
CN111308532A true CN111308532A (zh) 2020-06-19

Family

ID=71149259

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811523127.6A Pending CN111308532A (zh) 2018-12-12 2018-12-12 Gps/ins紧组合的导航方法及装置、定位***

Country Status (1)

Country Link
CN (1) CN111308532A (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102096086A (zh) * 2010-11-22 2011-06-15 北京航空航天大学 一种基于gps/ins组合导航***不同测量特性的自适应滤波方法
CN102830415A (zh) * 2012-08-31 2012-12-19 西北工业大学 一种降维度的基于Carlson滤波算法的快速组合导航方法
CN103792561A (zh) * 2014-02-21 2014-05-14 南京理工大学 一种基于gnss通道差分的紧组合降维滤波方法
CN107036598A (zh) * 2017-03-30 2017-08-11 南京航空航天大学 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航***的自适应卡尔曼滤波方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102096086A (zh) * 2010-11-22 2011-06-15 北京航空航天大学 一种基于gps/ins组合导航***不同测量特性的自适应滤波方法
CN102830415A (zh) * 2012-08-31 2012-12-19 西北工业大学 一种降维度的基于Carlson滤波算法的快速组合导航方法
CN103792561A (zh) * 2014-02-21 2014-05-14 南京理工大学 一种基于gnss通道差分的紧组合降维滤波方法
CN107036598A (zh) * 2017-03-30 2017-08-11 南京航空航天大学 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航***的自适应卡尔曼滤波方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
丁维福 等: "基于两阶段卡尔曼滤波的多传感器信息融合", 《西南民族大学学报·自然科学版》 *
张希 等: "GPS/INS超紧组合导航滤波器的优化设计", 《北京理工大学学报》 *
高胜峰 等: "SINS/LBL组合导航序贯滤波方法", 《仪器仪表学报》 *

Similar Documents

Publication Publication Date Title
CN105783922B (zh) 用于确定针对具有磁力计辅助的混合导航***的航向的方法
CN109163721B (zh) 姿态测量方法及终端设备
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考***算法
JP5046687B2 (ja) ナビゲーション・システム内での反復拡張カルマン・フィルタを実施する方法及び装置
CN106597507B (zh) Gnss/sins紧组合滤波平滑的高精度快速算法
CN108507568B (zh) 补偿温度漂移误差的方法、装置和组合导航***
EP3408688A1 (en) Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
CN108562290B (zh) 导航数据的滤波方法、装置、计算机设备及存储介质
WO2013080183A9 (en) A quasi tightly coupled gnss-ins integration process
CN111076722A (zh) 基于自适应的四元数的姿态估计方法及装置
CN112798021A (zh) 基于激光多普勒测速仪的惯导***行进间初始对准方法
Guerrier et al. Wavelet-based improvements for inertial sensor error modeling
CN113984054A (zh) 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备
Han et al. Precise positioning with machine learning based Kalman filter using GNSS/IMU measurements from android smartphone
Liu et al. An improved GNSS/INS navigation method based on cubature Kalman filter for occluded environment
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
CN116734864B (zh) 一种常值观测偏差条件下航天器自主相对导航方法
US11280617B2 (en) High performance dual mode mixing inertial navigation system
CN111308532A (zh) Gps/ins紧组合的导航方法及装置、定位***
Yu et al. Data fusion for a GPS/INS tightly coupled positioning system with equality and inequality constraints using an aggregate constraint unscented Kalman filter
Souto et al. A robust extended Kalman filter for discrete-time systems with uncertain dynamics, measurements and correlated noise
Zhou et al. INS/GPS for High‐Dynamic UAV‐Based Applications
CN115031731A (zh) 一种基于相对安装位置关系的多惯导协同导航方法及装置

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: 20200619