CN106767797B - 一种基于对偶四元数的惯性/gps组合导航方法 - Google Patents

一种基于对偶四元数的惯性/gps组合导航方法 Download PDF

Info

Publication number
CN106767797B
CN106767797B CN201710178159.6A CN201710178159A CN106767797B CN 106767797 B CN106767797 B CN 106767797B CN 201710178159 A CN201710178159 A CN 201710178159A CN 106767797 B CN106767797 B CN 106767797B
Authority
CN
China
Prior art keywords
quaternion
matrix
dual
multiplication
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
Application number
CN201710178159.6A
Other languages
English (en)
Other versions
CN106767797A (zh
Inventor
闵艳玲
熊智
邢丽
刘建业
王融
殷德全
许建新
施丽娟
景羿铭
万众
鲍雪
黄欣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201710178159.6A priority Critical patent/CN106767797B/zh
Publication of CN106767797A publication Critical patent/CN106767797A/zh
Application granted granted Critical
Publication of CN106767797B publication Critical patent/CN106767797B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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

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

本发明公开了一种基于对偶四元数的惯性/GPS组合导航方法，属于飞行器组合导航技术领域。该方法首先建立陀螺和加速度计误差模型，并将陀螺及加速度计误差扩展为***状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；随后，结合GPS的量测信息及对偶四元数捷联惯导算法的计算值，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；最后，对***状态方程和量测方程进行离散化处理，采用卡尔曼滤波进行闭环估计并对惯导算法中的推力速度对偶四元数、引力速度对偶四元数、位置对偶四元数进行修正，从而得到载体的导航信息。本方法能够有效利用GPS的输出信息对惯导解算误差进行修正，提高惯性导航***性能，适用于工程应用。

Description

一种基于对偶四元数的惯性/GPS组合导航方法
技术领域
本发明涉及一种基于对偶四元数的惯性/GPS组合导航方法，属于飞行器组合导航技术领域。
背景技术
近年来，随着高超声速飞行器等高动态飞行器的研制发展，对导航***性能提出了更高的要求。惯性导航***具有短时精度高、输出连续以及完全自主等突出优点，但其误差会随时间累积，需要其他导航手段加以辅助。
GPS全球定位***是一种高精度的全球三维实时卫星***，其导航定位的全球性和高精度使其成为一种先进的导航设备。但是GPS全球定位***也存在一些不足之处，主要是GPS在受到遮挡的情况下容易信号丢失，且容易受到人为控制和干扰，因此主要作为一种辅助导航设备使用。惯性/GPS组合克服了各自缺点，取长补短，使组合后的导航精度高于两个***单独工作的精度。
对偶四元数捷联惯性导航算法将载体的旋转和平移统一考虑，以最简洁的形式表示一般的刚体运动，在高动态环境下，具有比传统捷联惯导算法更高的精度，更能满足高动态飞行器对高精度导航性能的要求。传统的惯性导航算法误差模型通常是基于数学平台失准角的线性误差方程，但是该模型仅适用于平台失准角为小量的情况，当载体的姿态角误差较大时，在组合导航算法中使用该模型会使滤波精度大大降低，收敛时间变长甚至会导致发散。基于对偶四元数的捷联惯性导航算法可以直接利用对偶四元数建立线性误差模型，即使在大失准角的情况也同样能具有较快的收敛速度，较好的滤波精度，从而提高了组合导航***性能。因此，研究基于对偶四元数的惯性/GPS组合导航算法具有重要的研究意义。
发明内容
本发明提出了一种基于对偶四元数的惯性/GPS组合导航方法，在飞行器动态飞行过程中有效利用GPS提供的速度位置信息，对惯性导航解算参数误差进行修正，显著提高惯性导航***精度。
本发明为解决其技术问题采用如下技术方案：
一种基于对偶四元数的惯性/GPS组合导航方法，包括如下步骤：
步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪声；
步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂移误差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为***状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；
步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得的地理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度及地球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；
步骤4，对***状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。
步骤1所述陀螺和加速度计误差模型为：
Figure BDA0001252914680000021
Figure BDA0001252914680000022
其中，
Figure BDA0001252914680000031
为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，ωg为白噪声；δfB为加速度计误差，
Figure BDA0001252914680000032
为加速度计一阶马尔科夫过程随机噪声；
对上式中的εb、εr、
Figure BDA0001252914680000033
进行求导后可得到以下数学表达式：
Figure BDA0001252914680000034
Figure BDA0001252914680000035
Figure BDA0001252914680000036
其中，
Figure BDA0001252914680000037
为εb的一阶导数；
Figure BDA0001252914680000038
为εr的一阶导数；
Figure BDA0001252914680000039
为
Figure BDA00012529146800000310
的一阶导数；Tg为陀螺一阶马尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。
步骤2所述基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程为：
Figure BDA00012529146800000311
其中X∈R28×1为***状态量，F∈R28×28为***矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1为***噪声向量，
Figure BDA00012529146800000312
为X的一阶导数，各矩阵分别表示为：
Figure BDA00012529146800000313
Figure BDA0001252914680000041
Figure BDA0001252914680000042
***矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们定义矩阵
Figure BDA0001252914680000043
为q在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000044
为q在四元数乘法中的后乘矩阵，其具体可表示为：
Figure BDA0001252914680000045
***矩阵F和噪声系数矩阵G中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数。δqIT为推力速度对偶四元数误差的实数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU为位置对偶四元数误差的对偶部分；
Figure BDA0001252914680000046
为陀螺输出信息，
Figure BDA0001252914680000047
为
Figure BDA0001252914680000048
在四元数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，
Figure BDA0001252914680000049
为qIT在四元数乘法中的前乘矩阵；q′IT为推力对偶四元数的对偶部分，
Figure BDA0001252914680000051
为q′IT在四元数乘法中的前乘矩阵；
Figure BDA0001252914680000052
为地球自转角速度，
Figure BDA0001252914680000053
为
Figure BDA0001252914680000054
在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，
Figure BDA0001252914680000055
为其在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000056
为其在四元数乘法中的后乘矩阵；q* IT为qIT的共轭四元数，
Figure BDA0001252914680000057
为其在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000058
为其在四元数乘法中的后乘矩阵；
***矩阵F和噪声系数矩阵G中M1、M2、M3可分别表示为：
Figure BDA0001252914680000059
步骤3所述的GPS输出的地理系速度、地球系位置测量误差模型为：
VG＝Vn+δV，
RG＝Re+δR，
其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为GPS的位置测量误差，将其建模为白噪声；
利用如下公式将VG转换到惯性系：
Figure BDA00012529146800000510
其中，VGI为VG转换到惯性系下的值，
Figure BDA00012529146800000513
为地球系到惯性系的转换矩阵，ωie为四元数
Figure BDA00012529146800000511
的矢量部分，
Figure BDA00012529146800000512
为地理系到地球系的转换矩阵；
由此可建立步骤3所述基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程：
Z＝HX+v，
其中，
Figure BDA0001252914680000061
为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为***状态量，
Figure BDA0001252914680000062
为***量测噪声阵；
量测系数矩阵H的具体表达式为：
Figure BDA0001252914680000063
其中，qIG为引力速度对偶四元数的实数部分，
Figure BDA0001252914680000064
为qIG的共轭四元数，
Figure BDA0001252914680000065
为q* IG在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，* IU为qIU的共轭四元数，
Figure BDA0001252914680000066
为q* IU在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000067
为q′IT在四元数乘法中的前乘矩阵。
所述步骤4的具体过程为：
(401)将***状态方程和量测方程离散化处理：
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，
Zk＝HkXk+Vk，
其中，Xk为tk时刻***状态量,Xk-1为tk-1时刻***状态量，Φk,k-1为tk-1时刻至tk时刻***的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻***的噪声驱动矩阵，Wk-1为tk-1时刻***的噪声矩阵，Zk为tk时刻***的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测量噪声矩阵；
(402)采用卡尔曼滤波对状态量进行闭环估计：
Figure BDA0001252914680000068
Figure BDA0001252914680000069
Figure BDA00012529146800000610
Figure BDA00012529146800000611
Figure BDA00012529146800000612
其中，
Figure BDA00012529146800000613
是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩阵,Qk-1为tk-1时刻***噪声协方差矩阵,
Figure BDA0001252914680000071
为Φk,k-1的转置，
Figure BDA0001252914680000072
为Γk,k-1的转置，Pk,k-1为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk时刻滤波增益矩阵,
Figure BDA0001252914680000073
为Hk的转置，
Figure BDA0001252914680000074
为状态量Xk的卡尔曼滤波估值，I为单位矩阵，
Figure BDA0001252914680000075
为Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；
(403)在(402)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：
Figure BDA0001252914680000076
Figure BDA0001252914680000077
Figure BDA0001252914680000078
Figure BDA0001252914680000079
上式中，
Figure BDA00012529146800000710
为δqIT的估计值，
Figure BDA00012529146800000711
为修正后的推力速度对偶四元数的实数部分；
Figure BDA00012529146800000712
为δq′IT的估计值，
Figure BDA00012529146800000713
为修正后的推力速度对偶四元数的对偶部分；
Figure BDA00012529146800000714
为δq′IG的估计值，
Figure BDA00012529146800000715
为修正后的引力速度对偶四元数的对偶部分；
Figure BDA00012529146800000716
为δq′IU的估计值，
Figure BDA00012529146800000717
为修正后的位置对偶四元数的对偶部分。
本发明的有益效果如下：
1、基于对偶四元数的惯性/GPS算法可以直接利用对偶四元数建立线性误差模型，使得该组合导航***即使在大失准角的情况下也同样能具有较快的滤波收敛速度和较好的滤波精度，从而提高了组合导航***性能，适合工程应用。
2、本发明方法给出了组合前GPS输出信息的转换方法，并给出了坐标转化后的***量测噪声模型，该模型可以有效提高对偶四元数惯性/GPS组合导航精度，适合工程应用。
附图说明
图1是本发明基于对偶四元数的惯性/GPS组合导航方法的架构图。
具体实施方式
下面结合附图对本发明创造做进一步详细说明。
如图1所示，本发明所述基于对偶四元数的惯性/GPS组合导航方法的原理是：GPS输出地理系速度及地球系位置信息，将其输出的地理系速度经坐标转换为惯性系速度。同时，对偶四元数捷联惯导算法也可以相应得到载体在惯性系下的速度信息及地球系下的位置信息。利用卡尔曼滤波器融合GPS和对偶四元数捷联惯导算法的地球系位置、惯性系速度，在线估计得到对偶四元数误差参数，利用该信息对对偶四元数进行补偿修正，以此提高惯性导航***性能。
本发明的具体实施方式如下：
1、建立陀螺、加速度计误差模型
陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声及白噪声误差，加速度计误差建模为一阶马尔科夫过程随机噪声，具体误差模型如下：
Figure BDA0001252914680000081
Figure BDA0001252914680000082
其中，
Figure BDA0001252914680000083
为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，ωg为白噪声；δfB为加速度计误差，
Figure BDA0001252914680000084
为加速度计一阶马尔科夫过程随机噪声；
对上式中的εb、εr、
Figure BDA0001252914680000085
进行求导后可得到以下数学表达式：
Figure BDA0001252914680000086
Figure BDA0001252914680000087
Figure BDA0001252914680000088
其中，
Figure BDA0001252914680000091
为εb的一阶导数；
Figure BDA0001252914680000092
为εr的一阶导数；
Figure BDA0001252914680000093
为
Figure BDA0001252914680000094
的一阶导数；Tg为陀螺一阶马尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。
2、建立基于陀螺、加速度计误差模型的卡尔曼滤波状态方程
状态方程如下所示：
Figure BDA0001252914680000095
其中X∈R28×1为***状态量，F∈R28×28为***矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1为***噪声向量，
Figure BDA0001252914680000096
为X的一阶导数，各矩阵分别表示为：
Figure BDA0001252914680000097
Figure BDA0001252914680000098
Figure BDA0001252914680000099
Figure BDA0001252914680000101
***矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们定义矩阵
Figure BDA0001252914680000102
为q在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000103
为q在四元数乘法中的后乘矩阵，其具体可表示为：
Figure BDA0001252914680000104
Figure BDA0001252914680000105
式(7)～(10)中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数。δqIT为推力速度对偶四元数误差的实数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU为位置对偶四元数误差的对偶部分；
Figure BDA0001252914680000106
为陀螺输出信息，
Figure BDA0001252914680000107
为
Figure BDA0001252914680000108
在四元数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，
Figure BDA0001252914680000109
为qIT在四元数乘法中的前乘矩阵；q′IT为推力对偶四元数的对偶部分，
Figure BDA00012529146800001010
为q′IT在四元数乘法中的前乘矩阵；
Figure BDA00012529146800001011
为地球自转角速度，
Figure BDA00012529146800001012
为
Figure BDA00012529146800001013
在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，
Figure BDA00012529146800001014
为其在四元数乘法中的前乘矩阵，
Figure BDA00012529146800001015
为其在四元数乘法中的后乘矩阵；q* IT为qIT的共轭四元数，
Figure BDA0001252914680000111
为其在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000112
为其在四元数乘法中的后乘矩阵；
式(9)和式(10)中M1、M2、M3可分别表示为：
Figure BDA0001252914680000113
Figure BDA0001252914680000114
Figure BDA0001252914680000115
3、建立基于GPS测量误差模型的卡尔曼滤波量测方程
(3.1)GPS测量误差模型
GPS测量误差模型为：
VG＝Vn+δV (16)
RG＝Re+δR (17)
其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为GPS的位置测量误差，将其建模为白噪声；
利用如下公式将VG转换到惯性系：
Figure BDA0001252914680000116
其中，VGI为VG转换到惯性系下的值，
Figure BDA0001252914680000121
为地球系到惯性系的转换矩阵，ωie为四元数
Figure BDA0001252914680000122
的矢量部分，
Figure BDA0001252914680000123
为地理系到地球系的转换矩阵；
(3.2)建立卡尔曼滤波测量方程
量测方程如下所示：
Z＝HX+v (19)
其中，
Figure BDA0001252914680000124
为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为***状态量，
Figure BDA0001252914680000125
为***量测噪声阵；
量测系数矩阵H的具体表达式为：
Figure BDA0001252914680000126
其中，qIG为引力速度对偶四元数的实数部分，q* IG为qIG的共轭四元数，
Figure BDA0001252914680000127
为q* IG在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，q* IU为qIU的共轭四元数，
Figure BDA0001252914680000128
为q* IU在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000129
为q′IT在四元数乘法中的前乘矩阵。
4、对偶四元数误差在线估计与补偿修正
(4.1)***状态方程和量测方程离散化处理
将***状态方程和量测方程进行离散化处理：
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1 (21)
Zk＝HkXk+Vk (22)
其中，Xk为tk时刻***状态量,Xk-1为tk-1时刻***状态量，Φk,k-1为tk-1时刻至tk时刻***的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻***的噪声驱动矩阵，Wk-1为tk-1时刻***的噪声矩阵，Zk为tk时刻***的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测量噪声矩阵；
(4.2)卡尔曼滤波闭环估计
采用卡尔曼滤波对状态量进行闭环估计：
Figure BDA0001252914680000131
Figure BDA0001252914680000132
Figure BDA0001252914680000133
Figure BDA0001252914680000134
Figure BDA0001252914680000135
其中，
Figure BDA0001252914680000136
是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩阵,Qk-1为tk-1时刻***噪声协方差矩阵,
Figure BDA0001252914680000137
为Φk,k-1的转置，
Figure BDA0001252914680000138
为Γk,k-1的转置，Pk,k-1为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk时刻滤波增益矩阵,
Figure BDA0001252914680000139
为Hk的转置，
Figure BDA00012529146800001310
为状态量Xk的卡尔曼滤波估值，I为单位矩阵，
Figure BDA00012529146800001311
为Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；
(4.3)对偶四元数误差在线补偿修正
由(4.2)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：
Figure BDA00012529146800001312
Figure BDA00012529146800001313
Figure BDA00012529146800001314
Figure BDA00012529146800001315
上式中，
Figure BDA00012529146800001316
为δqIT的估计值，
Figure BDA00012529146800001317
为修正后的推力速度对偶四元数的实数部分；
Figure BDA00012529146800001318
为δq′IT的估计值，
Figure BDA00012529146800001319
为修正后的推力速度对偶四元数的对偶部分；
Figure BDA00012529146800001320
为δq′IG的估计值，
Figure BDA00012529146800001321
为修正后的引力速度对偶四元数的对偶部分；
Figure BDA00012529146800001322
为δq′IU的估计值，
Figure BDA00012529146800001323
为修正后的位置对偶四元数的对偶部分。

Claims (5)

1.一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，包括如下步骤：
步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪声；
步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂移误差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为***状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；
步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得的地理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度及地球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；
步骤4，对***状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。
2.根据权利要求1所述的一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步骤1所述陀螺和加速度计误差模型为：
Figure FDA0002253368750000011
δfB＝▽a，
其中，
Figure FDA0002253368750000012
为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，ωg为白噪声；δfB为加速度计误差，▽a为加速度计一阶马尔科夫过程随机噪声；
对上式中的εb、εr、▽a进行求导后可得到以下数学表达式：
Figure FDA0002253368750000013
Figure FDA0002253368750000021
Figure FDA0002253368750000022
其中，
Figure FDA0002253368750000023
为εb的一阶导数；
Figure FDA0002253368750000024
为εr的一阶导数；
Figure FDA0002253368750000025
为▽a的一阶导数；Tg为陀螺一阶马尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。
3.根据权利要求2所述的一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步骤2所述基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程为：
Figure FDA0002253368750000026
其中X∈R28×1为***状态量，F∈R28×28为***矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1为***噪声向量，
Figure FDA0002253368750000027
为X的一阶导数，各矩阵分别表示为：
Figure FDA0002253368750000028
Figure FDA0002253368750000029
Figure FDA0002253368750000031
***矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们定义矩阵
Figure FDA0002253368750000032
为q在四元数乘法中的前乘矩阵，
Figure FDA0002253368750000033
为q在四元数乘法中的后乘矩阵，其具体可表示为：
Figure FDA0002253368750000034
***矩阵F和噪声系数矩阵G中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数；δqIT为推力速度对偶四元数误差的实数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU为位置对偶四元数误差的对偶部分；
Figure FDA0002253368750000035
为陀螺输出信息，
Figure FDA0002253368750000036
为
Figure FDA0002253368750000037
在四元数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，
Figure FDA0002253368750000038
为qIT在四元数乘法中的前乘矩阵；q′IT为推力对偶四元数的对偶部分，
Figure FDA0002253368750000039
为q′IT在四元数乘法中的前乘矩阵；
Figure FDA00022533687500000310
为地球自转角速度，
Figure FDA00022533687500000311
为
Figure FDA00022533687500000312
在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，
Figure FDA00022533687500000313
为其在四元数乘法中的前乘矩阵，
Figure FDA00022533687500000314
为其在四元数乘法中的后乘矩阵；q* IT为qIT的共轭四元数，
Figure FDA00022533687500000315
为其在四元数乘法中的前乘矩阵，
Figure FDA00022533687500000316
为其在四元数乘法中的后乘矩阵；
***矩阵F和噪声系数矩阵G中M1、M2、M3可分别表示为：
Figure FDA0002253368750000041
4.根据权利要求3所述一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步骤3所述的GPS输出的地理系速度、地球系位置测量误差模型为：
VG＝Vn+δV，
RG＝Re+δR，
其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为GPS的位置测量误差，将其建模为白噪声；
利用如下公式将VG转换到惯性系：
Figure FDA0002253368750000042
其中，VGI为VG转换到惯性系下的值，
Figure FDA0002253368750000043
为地球系到惯性系的转换矩阵，ωie为四元数
Figure FDA0002253368750000044
的矢量部分，
Figure FDA0002253368750000045
为地理系到地球系的转换矩阵；
由此可建立步骤3所述基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程：
Z＝HX+v，
其中，
Figure FDA0002253368750000046
为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为***状态量，
Figure FDA0002253368750000047
为***量测噪声阵；
量测系数矩阵H的具体表达式为：
Figure FDA0002253368750000048
其中，qIG为引力速度对偶四元数的实数部分，q* IG为qIG的共轭四元数，
Figure FDA0002253368750000051
为q* IG在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，q* IU为qIU的共轭四元数，
Figure FDA0002253368750000052
为q* IU在四元数乘法中的前乘矩阵，
Figure FDA0002253368750000053
为q′IT在四元数乘法中的前乘矩阵。
5.根据权利要求3所述一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，所述步骤4的具体过程为：
(401)将***状态方程和量测方程离散化处理：
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，
Zk＝HkXk+Vk，
其中，Xk为tk时刻***状态量,Xk-1为tk-1时刻***状态量，Φk,k-1为tk-1时刻至tk时刻***的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻***的噪声驱动矩阵，Wk-1为tk-1时刻***的噪声矩阵，Zk为tk时刻***的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测量噪声矩阵；
(402)采用卡尔曼滤波对状态量进行闭环估计：
Figure FDA0002253368750000054
Figure FDA0002253368750000055
Figure FDA0002253368750000056
Figure FDA0002253368750000057
Figure FDA0002253368750000058
其中，
Figure FDA0002253368750000059
是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩阵,Qk-1为tk-1时刻***噪声协方差矩阵,
Figure FDA00022533687500000510
为Φk,k-1的转置，
Figure FDA00022533687500000511
为Γk,k-1的转置，Pk,k-1为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk时刻滤波增益矩阵,
Figure FDA00022533687500000512
为Hk的转置，
Figure FDA00022533687500000513
为状态量Xk的卡尔曼滤波估值，I为单位矩阵，
Figure FDA00022533687500000514
为Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；
(403)在(402)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：
Figure FDA0002253368750000061
Figure FDA0002253368750000062
Figure FDA0002253368750000063
Figure FDA0002253368750000064
上式中，
Figure FDA0002253368750000065
为δqIT的估计值，
Figure FDA0002253368750000066
为修正后的推力速度对偶四元数的实数部分；
Figure FDA0002253368750000067
为δq′IT的估计值，
Figure FDA0002253368750000068
为修正后的推力速度对偶四元数的对偶部分；
Figure FDA0002253368750000069
为δq′IG的估计值，
Figure FDA00022533687500000610
为修正后的引力速度对偶四元数的对偶部分；
Figure FDA00022533687500000611
为δq′IU的估计值，
Figure FDA00022533687500000612
为修正后的位置对偶四元数的对偶部分。
CN201710178159.6A 2017-03-23 2017-03-23 一种基于对偶四元数的惯性/gps组合导航方法 Active CN106767797B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710178159.6A CN106767797B (zh) 2017-03-23 2017-03-23 一种基于对偶四元数的惯性/gps组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710178159.6A CN106767797B (zh) 2017-03-23 2017-03-23 一种基于对偶四元数的惯性/gps组合导航方法

Publications (2)

Publication Number Publication Date
CN106767797A CN106767797A (zh) 2017-05-31
CN106767797B true CN106767797B (zh) 2020-03-17

Family

ID=58966597

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710178159.6A Active CN106767797B (zh) 2017-03-23 2017-03-23 一种基于对偶四元数的惯性/gps组合导航方法

Country Status (1)

Country Link
CN (1) CN106767797B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108387918A (zh) * 2018-01-18 2018-08-10 和芯星通（上海）科技有限公司 一种行人导航方法和云***服务器、存储介质、电子设备
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
CN109470251A (zh) * 2018-12-21 2019-03-15 陕西航天时代导航设备有限公司 一种用于组合导航***中的部分反馈滤波方法
CN110285804B (zh) * 2019-06-26 2022-06-17 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
CN111351482B (zh) * 2020-03-19 2023-08-18 南京理工大学 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法
CN111982126B (zh) * 2020-08-31 2023-03-17 郑州轻工业大学 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN113051757B (zh) * 2021-03-23 2022-08-09 中国人民解放军海军工程大学 一种捷联惯导广义psi角误差模型构建方法
CN113091754B (zh) * 2021-03-30 2023-02-28 北京航空航天大学 一种非合作航天器位姿一体化估计和惯性参数确定方法
CN117555036B (zh) * 2023-11-17 2024-07-09 中国地质大学（北京） 惯性稳定平台航空重力信号提取方法及装置
CN117606474B (zh) * 2024-01-24 2024-03-29 北京神导科技股份有限公司 基于四元数二阶插值的惯性天文组合导航时间同步方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104181574B (zh) * 2013-05-25 2016-08-10 成都国星通信有限公司 一种捷联惯导***/全球导航卫星***组合导航滤波***及方法
CN103700082B (zh) * 2013-12-23 2016-09-07 南京航空航天大学 基于对偶四元数相对定向的图像拼接方法
CN105512391B (zh) * 2015-12-04 2018-09-25 上海新跃仪表厂 基于对偶四元数的多星姿轨动力学建模方法及其验证***
CN106052716B (zh) * 2016-05-25 2019-04-05 南京航空航天大学 惯性系下基于星光信息辅助的陀螺误差在线标定方法

Also Published As

Publication number Publication date
CN106767797A (zh) 2017-05-31

Similar Documents

Publication Publication Date Title
CN106767797B (zh) 一种基于对偶四元数的惯性/gps组合导航方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN102608596B (zh) 一种用于机载惯性/多普勒雷达组合导航***的信息融合方法
CN110702143B (zh) 基于李群描述的sins捷联惯性导航***动基座快速初始对准方法
CN103235328B (zh) 一种gnss与mems组合导航的方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航***初始对准方法
CN111024070A (zh) 一种基于航向自观测的惯性足绑式行人定位方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN110595503B (zh) 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法
CN102809377A (zh) 飞行器惯性/气动模型组合导航方法
CN104697520B (zh) 一体化无陀螺捷联惯导***与gps***组合导航方法
CN108168548B (zh) 一种通过机器学习算法与模型辅助的行人惯性导航***和方法
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN107036598A (zh) 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法
CN112146655A (zh) 一种BeiDou/SINS紧组合导航***弹性模型设计方法
CN112325886A (zh) 一种基于重力梯度仪和陀螺仪组合的航天器自主定姿***
CN103674059A (zh) 一种基于外测速度信息的sins水平姿态误差修正方法
CN110926465A (zh) 一种mems/gps松组合导航方法
Bai et al. Improved preintegration method for GNSS/IMU/in-vehicle sensors navigation using graph optimization
CN105606093A (zh) 基于重力实时补偿的惯性导航方法及装置
CN110926499A (zh) 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法
CN108981691B (zh) 一种天空偏振光组合导航在线滤波与平滑方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法

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