CN108981696B - 一种sins任意失准角无奇异快速传递对准方法 - Google Patents

一种sins任意失准角无奇异快速传递对准方法 Download PDF

Info

Publication number
CN108981696B
CN108981696B CN201810863248.9A CN201810863248A CN108981696B CN 108981696 B CN108981696 B CN 108981696B CN 201810863248 A CN201810863248 A CN 201810863248A CN 108981696 B CN108981696 B CN 108981696B
Authority
CN
China
Prior art keywords
inertial navigation
matrix
sub
attitude
navigation system
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
CN201810863248.9A
Other languages
English (en)
Other versions
CN108981696A (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.)
XI'AN PRECISION MEASUREMENT CONTROL Co.,Ltd.
Original Assignee
Northwestern Polytechnical University
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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201810863248.9A priority Critical patent/CN108981696B/zh
Publication of CN108981696A publication Critical patent/CN108981696A/zh
Application granted granted Critical
Publication of CN108981696B publication Critical patent/CN108981696B/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/18—Stabilised platforms, e.g. by gyroscope

Abstract

本发明提供了一种SINS任意失准角无奇异快速传递对准方法，属于惯性导航技术领域，包括：利用主、子惯导系统陀螺仪输出分别跟踪主、子惯导相对惯性空间的姿态变化矩阵
Figure DDA0001750253370000011
结合主惯导的姿态矩阵
Figure DDA0001750253370000012
和主、子惯导之间的安装矩阵
Figure DDA0001750253370000013
建立子惯导系统姿态矩阵
Figure DDA0001750253370000014
主、子惯导惯性系下的姿态更新；构造量测量；采用矩阵卡尔曼滤波估计
Figure DDA0001750253370000015
子惯导初值姿态矩阵求解，实现动基座下的传递对准。本发明的仿真结果表明，该方法适用于任意失准角下的传递对准，在摇摆运动下，可以在10s之内完成快速传递对准，水平精度达到0.02°(误差均方根)以内，航向精度达到0.03°(误差均方根)以内，在摇摆条件下，能够快速收敛，对准精度高，实现了任意失准角下的无奇异快速传递对准。

Description

一种SINS任意失准角无奇异快速传递对准方法
技术领域
本发明属于惯性导航技术领域，具体涉及一种SINS任意失准角无奇异快速传递对准方法。
背景技术
由于现代战争对战术武器装备精确打击能力和快速反应能力的要求日益提高，许多舰载和机载武器装备都安装了捷联惯性导航系统，简称惯导系统。而由于惯导系统为战术级，传递对准成为其初始对准的主要方案，研究战术武器系统的快速高精度传递对准具有很高的军事应用价值。
传统大失准角传递对准方案主要包括两个方面：一是建立大失准角条件下的非线性误差模型；二是设计相应的非线性滤波估计算法。根据姿态描述方式不同，可以得到不同的非线性误差模型。如基于四元数非线性误差模型、基于旋转矢量非线性误差模型、基于Rodrigues参数非线性误差模型，其中，四元数非线性误差模型无奇异点，使用最为广泛，但在设计滤波算法时需要考虑其模值约束的影响。欧拉角姿态描述法存在奇异点，因此不适用于任意姿态对准，且基于欧拉角的非线性误差模型中含有状态量的正余弦函数，使得误差模型非线性增大。基于Rodrigues参数的误差模型也存在奇异点，模型非线性度也较大。
另一方面，在非线性滤波算法的选择上，常规EKF滤波算法需要求导计算Jacobian矩阵，且在处理严重非线性问题时，可能出现滤波误差增大甚至发散的现象。因此，一类基于sigma点的非线性滤波算法成为研究的热点，如UKF滤波、改进强跟踪UKF滤波、粒子滤波、Gauss-Hermite滤波，以及容积卡尔曼滤波(CKF)等。基于sigma点的非线性滤波算法，用确定性或随机采样策略逼近非线性函数的概率分布，无需对非线性模型求导，且可以通过优化采样策略，减小计算量并提高非线性函数概率分布的近似精度。总体来讲，基于sigma点的非线性滤波算法可以做到计算量与EKF滤波相当，但是存在计算量较大，且在大失准角下估计精度不高等问题。
发明内容
为了克服上述现有技术存在的不足，本发明提供了一种SINS任意失准角无奇异快速传递对准方法。
为了实现上述目的，本发明提供如下技术方案：
一种SINS任意失准角无奇异快速传递对准方法，包括以下步骤：
步骤1、利用主、子惯导系统陀螺仪输出分别跟踪主、子惯导相对惯性空间的姿态变化
Figure BDA0001750253350000021
结合主惯导的姿态矩阵
Figure BDA0001750253350000022
和主、子惯导之间的安装矩阵
Figure BDA0001750253350000023
建立子惯导系统姿态矩阵
Figure BDA0001750253350000024
子惯导系统姿态矩阵
Figure BDA0001750253350000025
的链式分解为：
Figure BDA0001750253350000026
其中，n系为实时导航坐标系，即载体时变位置东北天地理坐标系；m为主惯导载体坐标系；im为主惯导载体惯性系，与传递对准开始时刻的坐标系m重合；s为子惯导载体坐标系；is为子惯导载体惯性系，与传递对准开始时刻的坐标系s重合；
式(1)中
Figure BDA0001750253350000027
分别为主、子惯导系统相对惯性空间的姿态变化，可分别利用惯导陀螺仪输出进行跟踪，
Figure BDA0001750253350000028
是由主惯导提供的实时姿态矩阵，所以，惯性系动基座传递对准过程实际上是间接对常值姿态阵
Figure BDA0001750253350000029
的估计；
步骤2、主、子惯导惯性系下的姿态更新
由步骤1中对惯性坐标系im和is的定义可知，对准过程中姿态矩阵
Figure BDA0001750253350000031
可分别利用主、子惯导的陀螺仪输出进行跟踪，忽略主惯导误差时，即有
Figure BDA0001750253350000032
考虑子惯导陀螺随机常值误差的影响，姿态跟踪结果记为
Figure BDA0001750253350000033
则有
Figure BDA0001750253350000034
式中
Figure BDA0001750253350000035
分别为主、子惯导陀螺仪的实时输出；
Figure BDA0001750253350000036
的跟踪误差仅由子惯导陀螺仪器件误差引起，短时间对准过程中，该误差可以视为小量，记为
Figure BDA0001750253350000037
则
Figure BDA0001750253350000038
式中，εs为子惯导陀螺仪随机常值零偏；
Figure BDA0001750253350000039
为子惯导陀螺仪测量噪声；
步骤3、构造量测量
已知，主、子惯导系统敏感的比力和角速度在地心惯性系内的投影关系分别为：
Figure BDA00017502533500000310
Figure BDA00017502533500000311
式中，
Figure BDA00017502533500000312
为主惯导敏感的比力经杆臂效应补偿后的物理量；
Figure BDA00017502533500000313
为主子惯导相对角速度；
对主惯导系统在m系内的比力量测值进行杆臂补偿，记为
Figure BDA00017502533500000314
则
Figure BDA00017502533500000315
式中
Figure BDA00017502533500000316
为杆臂效应残差项；
然后，利用式(5)实时跟踪的姿态
Figure BDA00017502533500000317
将补偿后的比力量测值投影到im系，即
Figure BDA0001750253350000041
对于子惯导同理可得
Figure BDA0001750253350000042
式中
Figure BDA0001750253350000043
为子惯导加速度计常值零偏；
Figure BDA0001750253350000044
为子惯导加速度计测量噪声；
将式(12)展开整理，略去高阶小量，可得
Figure BDA0001750253350000045
由式(8)(10)(12)可得
Figure BDA0001750253350000046
对主、子惯导系统测量的角速度信息进行类似处理，可得
Figure BDA0001750253350000047
式中
Figure BDA0001750253350000048
式(14)、(15)等号两端包含了相关误差，若对其进行积分操作，则能够有效平滑相关噪声、杆臂残差以及弹性变形项，提高信噪比；
对等式(14)左端取积分，并记
Figure BDA0001750253350000049
对等式(14)右端括号内取积分，并记
Figure BDA00017502533500000410
由式(14)、(17)、(18)可得
Figure BDA0001750253350000051
对式(15)进行类似操作，同理可得
Figure BDA0001750253350000052
其中
Figure BDA0001750253350000053
Figure BDA0001750253350000054
式中，
Figure BDA0001750253350000055
可视为由弹性变形引起的量测噪声；
值得注意的是，在推导式(19)和式(20)的过程中，并未对主、子惯导系统之间的相对姿态提出任何假设；也即，无论主、子惯导系统之间相对姿态为小姿态角还是大姿态角，惯性系内得到的匹配方程具有一致性，基于此，可以在惯性系内，设计出无需区分相对姿态大小的统一传递对准模型，此外由于模型基于姿态矩阵表示，也避免了奇异点问题。下面将以比力积分加角速度积分匹配为基础，来设计基于姿态矩阵任意失准角下的无奇异快速传递对准算法；
忽略弹性变形，对准期间
Figure BDA0001750253350000056
为常值矩阵，即满足如下微分方程：
Figure BDA0001750253350000057
考虑陀螺漂移和加速度计零偏为随机常值，即
Figure BDA0001750253350000058
其中，wgk，wak均为零均值白噪声序列。
若惯性系动基座传递对准选取如下状态
Figure BDA0001750253350000059
则由式(7)、(17)、(21)、(23)、(24)可构成如下矩阵离散型状态方程
Figure BDA0001750253350000061
其中系统转移矩阵
Figure BDA0001750253350000062
为
Figure BDA0001750253350000063
Figure BDA0001750253350000064
Figure BDA0001750253350000065
Figure BDA0001750253350000066
其中，Eij为8×8矩阵，且第ij个元素为1，其余为0；
由式(19)、(20)可构成量测方程，即
Figure BDA0001750253350000067
其中，Vk+1为量测噪声矩阵，且
Figure BDA0001750253350000068
步骤4、矩阵卡尔曼滤波估计算法
步骤4.1、滤波时间更新算法
式(26)、(28)描述的系统方程和量测方程均为线性，而状态为矩阵形式，因此要利用标准卡尔曼滤波基本方程，首先要将式(26)、(28)矩阵方程向量化，为此定义如下向量化算子：
x＝vec(X) (30)
其中，
Figure BDA0001750253350000069
若定义
Figure BDA00017502533500000610
为Kronecker积，则
Figure BDA00017502533500000611
假设，在tk时刻，状态Xk和估计均方误差阵Pk已知，对式(24)进行向量化操作，并利用式(29)特性，可得
Figure BDA0001750253350000071
即等价于标准卡尔曼滤波离散状态方程
xk+1＝Ψkxk+wk (33)
则状态一步预测
Figure BDA0001750253350000072
利用算子vec的线性特性和式(31)，则上式可等价于
Figure BDA0001750253350000073
一步预测均方误差
Figure BDA0001750253350000074
其中，Qk＝cov{Wk}；
式(33)、式(34)即为矩阵滤波的时间更新；
步骤4.2、滤波量测更新算法
根据(28)描述的量测方程，推导量测更新算法；
记量测一步预测
Figure BDA0001750253350000075
残差
Figure BDA0001750253350000076
对式(38)进行向量化操作，即
Figure BDA0001750253350000077
则由标准卡尔曼滤波可得
Sk+1＝Γk+1Pk+1/k(Γk+1)T+Rk+1 (40)
Kk+1＝Pk+1/k(Γk+1)T(Sk+1)-1 (41)
Figure BDA0001750253350000081
其中，
Figure BDA0001750253350000082
将式(42)恢复到矩阵形式
Figure BDA0001750253350000083
其中
Figure BDA0001750253350000084
由式(41)得到，即
Figure BDA0001750253350000085
式(43)则为矩阵状态在时刻tk+1的更新方程；
由标准卡尔曼滤波理论，矩阵估计均方误差为
Figure BDA0001750253350000086
式(38)、(40)、(41)、(43)和(45)即为不考虑约束条件下，所设计的矩阵滤波量测更新；
步骤4中基于矩阵滤波实时估计，选取状态
Figure BDA0001750253350000087
则以姿态矩阵为姿态描述的线性状态方程为：
Figure BDA0001750253350000088
式中
Figure BDA0001750253350000089
为对准初始时刻主子惯导之间固定的安装矩阵；
Figure BDA00017502533500000810
为子惯导姿态更新误差；
Figure BDA00017502533500000811
为子惯导惯性系内比力积分误差；
Figure BDA00017502533500000812
为子惯导惯性系内角速度积分误差；εs为子惯导陀螺仪随机常值零偏；
Figure BDA0001750253350000091
为子惯导加速度计常值零偏；
Figure BDA0001750253350000092
为子惯导陀螺仪测量噪声；
Figure BDA0001750253350000093
为子惯导加速度计测量噪声；
以比力积分+角速度积分构成的线性量测方程为：
Figure BDA0001750253350000094
其中
Figure BDA0001750253350000095
式中，
Figure BDA0001750253350000096
分别为第k时刻子惯导比力积分和角速度积分；
Figure BDA0001750253350000097
分别为第k时刻主惯导比力积分和角速度积分；
Figure BDA0001750253350000098
为量测矩阵；Vk为量测噪声矩阵；
步骤5、子惯导初值姿态矩阵求解
在传递对准结束时刻，利用步骤2中主、子惯导惯性系下的姿态更新姿态矩阵
Figure BDA0001750253350000099
以及步骤3、4估计得到的常值矩阵
Figure BDA00017502533500000910
根据步骤1中的式(1)求解子惯导姿态矩阵
Figure BDA00017502533500000911
即实现动基座下的传递对准。
本发明提供的SINS任意失准角无奇异快速传递对准方法对于战术级捷联惯导系统传递对准问题，给出了一种基于姿态矩阵模型和矩阵形式卡尔曼滤波的对准算法，通过姿态矩阵对姿态的描述，系统方程和量测方程均为线性方程，避免了传统大失准角下的非线性模型和非线性滤波问题，大大降低了计算量，提高了工程实用性。仿真分析结果表明，该方法在摇摆条件下，能够快速收敛，对准精度高，实现了任意失准角下的无奇异快速传递对准。该方法利用姿态矩阵描述姿态关系，不存在奇异点和模型非线性问题，无需对失准角做任何假设约束，采用线性矩阵卡尔曼滤波，在保证对准精度和对准时间的同时，避免了繁琐非线性滤波，降低了计算量，提高了工程实用性。
附图说明
图1为本发明实施例1的SINS任意失准角无奇异快速传递对准方法的流程图；
图2为本发明实施例1提出的MKF方法与传统方法的估计误差均方根(小角度)对比图；
图3为本发明实施例1提出的MKF方法与传统方法的10s内估计均值(小角度)对比图；
图4为本发明实施例1提出的MKF方法与传统方法的估计误差均方根(大角度)对比图；
图5为本发明实施例1提出的MKF方法与传统方法的10s内估计均值(大角度)对比图。
具体实施方式
下面结合附图，对本发明的具体实施方式作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
实施例1
本发明提供了一种SINS任意失准角无奇异快速传递对准方法，具体如图1所示，包括以下步骤：
步骤1、利用主、子惯导系统陀螺仪输出分别跟踪主、子惯导相对惯性空间的姿态变化
Figure BDA0001750253350000101
结合主惯导的姿态矩阵
Figure BDA0001750253350000102
和主、子惯导之间的安装矩阵
Figure BDA0001750253350000103
建立子惯导系统姿态矩阵
Figure BDA0001750253350000104
子惯导系统姿态矩阵
Figure BDA0001750253350000105
的链式分解为：
Figure BDA0001750253350000106
其中，n系为实时导航坐标系，即载体时变位置东北天地理坐标系；m为主惯导载体坐标系；im为主惯导载体惯性系，与传递对准开始时刻的坐标系m重合；s为子惯导载体坐标系；is为子惯导载体惯性系，与传递对准开始时刻的坐标系s重合；
式(1)中
Figure BDA0001750253350000111
分别为主、子惯导系统相对惯性空间的姿态变化，可分别利用惯导陀螺仪输出进行跟踪，
Figure BDA0001750253350000112
是由主惯导提供的实时姿态矩阵，所以，惯性系动基座传递对准过程实际上是间接对常值姿态阵
Figure BDA0001750253350000113
的估计；
步骤2、主、子惯导惯性系下的姿态更新
由步骤1中对惯性坐标系im和is的定义可知，对准过程中姿态矩阵
Figure BDA0001750253350000114
可分别利用主、子惯导的陀螺仪输出进行跟踪，忽略主惯导误差时，即有
Figure BDA0001750253350000115
考虑子惯导陀螺随机常值误差的影响，姿态跟踪结果记为
Figure BDA0001750253350000116
则有
Figure BDA0001750253350000117
式中
Figure BDA0001750253350000118
分别为主、子惯导陀螺仪的实时输出；
Figure BDA0001750253350000119
的跟踪误差仅由子惯导陀螺仪器件误差引起，短时间对准过程中，该误差可以视为小量，记为
Figure BDA00017502533500001110
则
Figure BDA00017502533500001111
式中，εs为子惯导陀螺仪随机常值零偏；
Figure BDA00017502533500001112
为子惯导陀螺仪测量噪声；
步骤3、构造量测量
已知，主、子惯导系统敏感的比力和角速度在地心惯性系内的投影关系分别为：
Figure BDA00017502533500001113
Figure BDA00017502533500001114
式中，
Figure BDA0001750253350000121
为主惯导敏感的比力经杆臂效应补偿后的物理量；
Figure BDA0001750253350000122
为主子惯导相对角速度；
对主惯导系统在m系内的比力量测值进行杆臂补偿，记为
Figure BDA0001750253350000123
则
Figure BDA0001750253350000124
式中
Figure BDA0001750253350000125
为杆臂效应残差项；
然后，利用式(5)实时跟踪的姿态
Figure BDA0001750253350000126
将补偿后的比力量测值投影到im系，即
Figure BDA0001750253350000127
对于子惯导同理可得
Figure BDA0001750253350000128
式中
Figure BDA0001750253350000129
为子惯导加速度计常值零偏；
Figure BDA00017502533500001210
为子惯导加速度计测量噪声；
将式(12)展开整理，略去高阶小量，可得
Figure BDA00017502533500001211
由式(8)(10)(12)可得
Figure BDA00017502533500001212
对主、子惯导系统测量的角速度信息进行类似处理，可得
Figure BDA00017502533500001213
式中
Figure BDA00017502533500001214
式(14)、(15)等号两端包含了相关误差，若对其进行积分操作，则能够有效平滑相关噪声、杆臂残差以及弹性变形项，提高信噪比；
对等式(14)左端取积分，并记
Figure BDA0001750253350000131
对等式(14)右端括号内取积分，并记
Figure BDA0001750253350000132
由式(14)、(17)、(18)可得
Figure BDA0001750253350000133
对式(15)进行类似操作，同理可得
Figure BDA0001750253350000134
其中
Figure BDA0001750253350000135
Figure BDA0001750253350000136
式中，
Figure BDA0001750253350000137
可视为由弹性变形引起的量测噪声；
值得注意的是，在推导式(19)和式(20)的过程中，并未对主、子惯导系统之间的相对姿态提出任何假设；也即，无论主、子惯导系统之间相对姿态为小姿态角还是大姿态角，惯性系内得到的匹配方程具有一致性，基于此，可以在惯性系内，设计出无需区分相对姿态大小的统一传递对准模型，此外由于模型基于姿态矩阵表示，也避免了奇异点问题。下面将以比力积分加角速度积分匹配为基础，来设计基于姿态矩阵任意失准角下的无奇异快速传递对准算法；
忽略弹性变形，对准期间
Figure BDA0001750253350000138
为常值矩阵，即满足如下微分方程：
Figure BDA0001750253350000139
考虑陀螺漂移和加速度计零偏为随机常值，即
Figure BDA0001750253350000141
其中，wgk，wak均为零均值白噪声序列。
若惯性系动基座传递对准选取如下状态
Figure BDA0001750253350000142
则由式(7)、(17)、(21)、(23)、(24)可构成如下矩阵离散型状态方程
Figure BDA0001750253350000143
其中系统转移矩阵
Figure BDA0001750253350000144
为
Figure BDA0001750253350000145
Figure BDA0001750253350000146
Figure BDA0001750253350000147
Figure BDA0001750253350000148
其中，Eij为8×8矩阵，且第ij个元素为1，其余为0；
由式(19)、(20)可构成量测方程，即
Figure BDA0001750253350000149
其中，Vk+1为量测噪声矩阵，且
Figure BDA00017502533500001410
步骤4、矩阵卡尔曼滤波估计算法
步骤4.1、滤波时间更新算法
式(26)、(28)描述的系统方程和量测方程均为线性，而状态为矩阵形式，因此要利用标准卡尔曼滤波基本方程，首先要将式(26)、(28)矩阵方程向量化，为此定义如下向量化算子：
x＝vec(X) (30)
其中，
Figure BDA0001750253350000151
若定义
Figure BDA0001750253350000152
为Kronecker积，则
Figure BDA0001750253350000153
假设，在tk时刻，状态Xk和估计均方误差阵Pk已知，对式(24)进行向量化操作，并利用式(29)特性，可得
Figure BDA0001750253350000154
即等价于标准卡尔曼滤波离散状态方程
xk+1＝Ψkxk+wk (33)
则状态一步预测
Figure BDA0001750253350000155
利用算子vec的线性特性和式(31)，则上式可等价于
Figure BDA0001750253350000156
一步预测均方误差
Figure BDA0001750253350000157
其中，Qk＝cov{Wk}；
式(33)、式(34)即为矩阵滤波的时间更新；
步骤4.2、滤波量测更新算法
根据(28)描述的量测方程，推导量测更新算法；
记量测一步预测
Figure BDA0001750253350000158
残差
Figure BDA0001750253350000161
对式(38)进行向量化操作，即
Figure BDA0001750253350000162
则由标准卡尔曼滤波可得
Sk+1＝Γk+1Pk+1/k(Γk+1)T+Rk+1 (40)
Kk+1＝Pk+1/k(Γk+1)T(Sk+1)-1 (41)
Figure BDA0001750253350000163
其中，
Figure BDA0001750253350000164
将式(42)恢复到矩阵形式
Figure BDA0001750253350000165
其中
Figure BDA0001750253350000166
由式(41)得到，即
Figure BDA0001750253350000167
式(43)则为矩阵状态在时刻tk+1的更新方程；
由标准卡尔曼滤波理论，矩阵估计均方误差为
Figure BDA0001750253350000168
式(38)、(40)、(41)、(43)和(45)即为不考虑约束条件下，所设计的矩阵滤波量测更新；
步骤4中基于矩阵滤波实时估计，选取状态
Figure BDA0001750253350000169
则以姿态矩阵为姿态描述的线性状态方程为：
Figure BDA0001750253350000171
式中
Figure BDA0001750253350000172
为对准初始时刻主子惯导之间固定的安装矩阵；
Figure BDA0001750253350000173
为子惯导姿态更新误差；
Figure BDA0001750253350000174
为子惯导惯性系内比力积分误差；
Figure BDA0001750253350000175
为子惯导惯性系内角速度积分误差；εs为子惯导陀螺仪随机常值零偏；
Figure BDA0001750253350000176
为子惯导加速度计常值零偏；
Figure BDA0001750253350000177
为子惯导陀螺仪测量噪声；
Figure BDA0001750253350000178
为子惯导加速度计测量噪声；
以比力积分+角速度积分构成的线性量测方程为：
Figure BDA0001750253350000179
其中
Figure BDA00017502533500001710
式中，
Figure BDA00017502533500001711
分别为第k时刻子惯导比力积分和角速度积分；
Figure BDA00017502533500001712
分别为第k时刻主惯导比力积分和角速度积分；
Figure BDA00017502533500001713
为量测矩阵；Vk为量测噪声矩阵；
步骤5、子惯导初值姿态矩阵求解
在传递对准结束时刻，利用步骤2中主、子惯导惯性系下的姿态更新姿态矩阵
Figure BDA00017502533500001714
以及步骤3、4估计得到的常值矩阵
Figure BDA00017502533500001715
根据步骤1中的式(1)求解子惯导姿态矩阵
Figure BDA00017502533500001716
即实现动基座下的传递对准。
本实施例提供了一种基于姿态矩阵和矩阵卡尔曼滤波的传递对准算法，利用姿态阵分解，将子惯导姿态的估计问题转化为一个常值姿态的估计，借助经典姿态矩阵描述姿态，建立了在惯性坐标系矩阵形式的线性系统方程，量测方程也为姿态矩阵的线性模型。对于这种系统方程、量测方程均为线性模型的估计，借鉴传统线性向量卡尔曼滤波更新的思路，推导了一种基于线性矩阵卡尔曼滤波更新算法。可以认为新算法用线性滤波方式解决了大失准角非线性滤波对准问题，在计算量上远小于导航系内基于sigma点的各类非线性滤波对准算法。同时避免了经典Rodrigues参数和姿态角描述姿态的奇异性。根据上述方案，下面通过仿真试验对本发明的具体实施方式和实施效果做进一步说明。
具体描述如下：
仿真条件：本发明实施例1中，以舰船在水中的摇摆运动为例，运动轨迹可看作由一系列幅值和频率相近的正弦波来描述为：
Figure BDA0001750253350000181
其中，θ、γ、
Figure BDA0001750253350000182
分别为绕俯仰轴、横滚轴和方位轴的摇摆角度；θm、γm、
Figure BDA0001750253350000183
分别为摇摆角度幅值；ωθ、ωγ、
Figure BDA0001750253350000184
为摇摆的角频率；θ0、γ0、
Figure BDA0001750253350000185
为摇摆的初始相位，各参数设置如表1所示。
表1摇摆参数设置
Figure BDA0001750253350000186
子惯导IMU中陀螺漂移误差为5°/h、随机游走噪声为
Figure BDA0001750253350000187
加速度计零位为1mg、随机噪声为
Figure BDA0001750253350000188
惯导系统对准时间共计10s，采样周期为5ms。
失准角设置：小失准角[1° 1° 1°]和大失准角[60° 60° 150°]。对这两种姿态初值条件下的初始对准算法各进行50次的蒙特卡洛仿真，仿真结果如图2～图5所示。
图2～图5描述的是在两种失准角条件下，50次仿真得到的估计误差均方根和估计均值曲线，图中实线表示本实施例的方法得到的数据曲线，虚线表示传统方法得到的数据曲线。从图2～图5中可以得出两点结论：第一，提出的传递对准方法收敛速度很快，基本上在0.2s左右；第二，提出的传递对准方法对准精度很高，水平精度在0.02°以内，方位精度在0.03°以内。
图2给出了小失准角条件下50次仿真得到的估计误差均方根曲线。可以看出，在小失准角条件下，本实施例提出的对准方法和传统方法的估计误差均方根分别为[0.016°0.013° 0.026°]T和[0.031° 0.027° 0.037°]T。图3给出了小失准角条件下50次仿真得到的均方曲线，在对准结束时刻，本实施例提出的对准方法和传统方法的均值分别为[1.001°0.996° 0.994°]T和[1.031° 0.981° 0.987°]T。图4给出了大失准角条件下50次仿真得到的估计误差均方根曲线。可以看出，在大失准角条件下，本实施例提出的对准方法和传统方法的估计误差均方根分别为[0.0142° 0.0184° 0.0282°]T和[0.0597° 0.0349° 0.0472°]T。图5给出了大失准角条件下50次仿真得到的均方曲线，在对准结束时刻，本实施例提出的对准方法和传统方法的均值分别为[59.997° 59.984° 149.987°]T和[60.057° 59.966°149.954°]T。
以上所述实施例仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换，均属于本发明的保护范围。

Claims (2)

1.一种SINS任意失准角无奇异快速传递对准方法，其特征在于，包括以下步骤：
步骤1：利用主、子惯导系统陀螺仪输出分别跟踪主、子惯导相对惯性空间的姿态变化矩阵
Figure FDA0003240111880000011
结合主惯导的姿态矩阵
Figure FDA0003240111880000012
和主、子惯导之间的常值矩阵
Figure FDA0003240111880000013
建立子惯导系统姿态矩阵
Figure FDA0003240111880000014
子惯导系统姿态矩阵
Figure FDA0003240111880000015
的链式分解为：
Figure FDA0003240111880000016
其中，n为实时导航坐标系，即载体时变位置东北天地理坐标系，m为主惯导载体坐标系，im为主惯导载体惯性系，与传递对准开始时刻的坐标系m重合，s为子惯导载体坐标系，is为子惯导载体惯性系，与传递对准开始时刻的坐标系s重合；
步骤2：对准过程中，根据主、子惯导陀螺的输出进行惯性系下的姿态更新，实时求解姿态矩阵
Figure FDA0003240111880000017
步骤3：根据主、子惯导陀螺仪和加速度计实时输出的原始数据，结合姿态矩阵
Figure FDA0003240111880000018
在惯性系内对比力和角速度进行积分运算，得到量测量
Figure FDA0003240111880000019
和
Figure FDA00032401118800000110
其中，
Figure FDA00032401118800000111
分别为第k时刻子惯导比力积分和角速度积分；
Figure FDA00032401118800000112
分别为第k时刻主惯导比力积分和角速度积分；
步骤4：建立以
Figure FDA00032401118800000113
和
Figure FDA00032401118800000114
为量测量的线性模型，根据建立的线性模型，采用矩阵卡尔曼滤波对所求常值矩阵
Figure FDA00032401118800000115
进行实时估计；
步骤5：在传递对准结束时刻，利用步骤4估计的常值矩阵
Figure FDA00032401118800000116
通过公式(1)实现动基座下的传递对准。
2.根据权利要求1所述的SINS任意失准角无奇异快速传递对准方法，其特征在于，所述步骤4中基于矩阵卡尔曼滤波实时估计，选取状态
Figure FDA0003240111880000021
则以姿态矩阵为姿态描述的线性状态方程为：
Figure FDA0003240111880000022
式中
Figure FDA0003240111880000023
为对准初始时刻主子惯导之间固定的常值矩阵；
Figure FDA0003240111880000024
为子惯导姿态更新误差；
Figure FDA0003240111880000025
为子惯导惯性系内比力积分误差；
Figure FDA0003240111880000026
为子惯导惯性系内角速度积分误差；εs为子惯导陀螺仪随机常值零偏；
Figure FDA0003240111880000027
为子惯导加速度计常值零偏；
Figure FDA0003240111880000028
为子惯导陀螺仪测量噪声；
Figure FDA0003240111880000029
为子惯导加速度计测量噪声；
以比力积分+角速度积分构成的线性量测方程为：
Figure FDA00032401118800000210
其中
Figure FDA00032401118800000211
式中，
Figure FDA00032401118800000212
分别为第k时刻子惯导比力积分和角速度积分；
Figure FDA00032401118800000213
分别为第k时刻主惯导比力积分和角速度积分；
Figure FDA00032401118800000214
为量测矩阵；Vk为量测噪声矩阵。
CN201810863248.9A 2018-08-01 2018-08-01 一种sins任意失准角无奇异快速传递对准方法 Active CN108981696B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810863248.9A CN108981696B (zh) 2018-08-01 2018-08-01 一种sins任意失准角无奇异快速传递对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810863248.9A CN108981696B (zh) 2018-08-01 2018-08-01 一种sins任意失准角无奇异快速传递对准方法

Publications (2)

Publication Number Publication Date
CN108981696A CN108981696A (zh) 2018-12-11
CN108981696B true CN108981696B (zh) 2022-02-18

Family

ID=64550506

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810863248.9A Active CN108981696B (zh) 2018-08-01 2018-08-01 一种sins任意失准角无奇异快速传递对准方法

Country Status (1)

Country Link
CN (1) CN108981696B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110398257B (zh) * 2019-07-17 2022-08-02 哈尔滨工程大学 Gps辅助的sins系统快速动基座初始对准方法
CN110702143B (zh) * 2019-10-19 2021-07-30 北京工业大学 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN111076721B (zh) * 2020-01-19 2023-03-28 浙江融芯导航科技有限公司 一种快速收敛的惯性测量单元安装姿态估计方法
CN112097728B (zh) * 2020-09-17 2021-07-30 中国人民解放军国防科技大学 基于反向解算组合惯性导航系统的惯性双矢量匹配形变测量方法
CN112525191B (zh) * 2021-02-08 2021-06-08 北京航空航天大学 一种基于相对捷联解算的机载分布式pos传递对准方法
CN115727875B (zh) * 2022-11-29 2023-09-01 哈尔滨理工大学 一种基于修正罗德里格斯参数的无奇异传递对准方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101915578A (zh) * 2010-07-09 2010-12-15 哈尔滨工程大学 基于光纤捷联惯性系统测量船上任意两位置间距离的方法
CN103047999A (zh) * 2012-12-18 2013-04-17 东南大学 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法
CN104897170A (zh) * 2015-05-29 2015-09-09 西北工业大学 一种基于罗德里格参数和二阶非线性量测的滤波对准算法
CN105066996A (zh) * 2015-07-20 2015-11-18 东南大学 自适应矩阵卡尔曼滤波姿态估计方法
CN105973268A (zh) * 2016-05-06 2016-09-28 哈尔滨工程大学 一种基于共基座安装的传递对准精度定量评估方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101915578A (zh) * 2010-07-09 2010-12-15 哈尔滨工程大学 基于光纤捷联惯性系统测量船上任意两位置间距离的方法
CN103047999A (zh) * 2012-12-18 2013-04-17 东南大学 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法
CN104897170A (zh) * 2015-05-29 2015-09-09 西北工业大学 一种基于罗德里格参数和二阶非线性量测的滤波对准算法
CN105066996A (zh) * 2015-07-20 2015-11-18 东南大学 自适应矩阵卡尔曼滤波姿态估计方法
CN105973268A (zh) * 2016-05-06 2016-09-28 哈尔滨工程大学 一种基于共基座安装的传递对准精度定量评估方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于Rodrigues 参数和量测非线性的SINS/GPS 动基座对准;梅春波等;《中国惯性技术学报》;20151231;第23卷(第6期);第739-745页 *

Also Published As

Publication number Publication date
CN108981696A (zh) 2018-12-11

Similar Documents

Publication Publication Date Title
CN108981696B (zh) 一种sins任意失准角无奇异快速传递对准方法
CN107990910B (zh) 一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN104567871B (zh) 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法
WO2020087845A1 (zh) 基于gpr与改进的srckf的sins初始对准方法
Aydemir et al. Characterization and calibration of MEMS inertial sensors for state and parameter estimation applications
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN108759870B (zh) 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN107479076B (zh) 一种动基座下联合滤波初始对准方法
CN110567455B (zh) 一种求积更新容积卡尔曼滤波的紧组合导航方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN108344409B (zh) 提高卫星姿态确定精度的方法
CN104374405A (zh) 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法
CN103776449A (zh) 一种提高鲁棒性的动基座初始对准方法
CN105004351A (zh) 基于自适应upf的sins大方位失准角初始对准方法
CN109840517A (zh) 一种mems陀螺噪声估计和滤波方法
CN112857398A (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN108731702B (zh) 一种基于Huber方法的大失准角传递对准方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
CN111795708A (zh) 晃动基座条件下陆用惯性导航系统的自适应初始对准方法
Lou et al. Robust partially strong tracking extended consider Kalman filtering for INS/GNSS integrated navigation
CN110736459B (zh) 惯性量匹配对准的角形变测量误差评估方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN110345942A (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
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20221123

Address after: 710065 A-312, Tsinghua Science Park, No. 65, Keji Second Road, Hi tech Zone, Xi'an, Shaanxi

Patentee after: XI'AN PRECISION MEASUREMENT CONTROL Co.,Ltd.

Address before: Beilin District Shaanxi province Xi'an City friendship road 710072 No. 127

Patentee before: Northwestern Polytechnical University