CN114111843A - 一种捷联惯导航系统最优动基座初始对准方法 - Google Patents

一种捷联惯导航系统最优动基座初始对准方法 Download PDF

Info

Publication number
CN114111843A
CN114111843A CN202111408645.5A CN202111408645A CN114111843A CN 114111843 A CN114111843 A CN 114111843A CN 202111408645 A CN202111408645 A CN 202111408645A CN 114111843 A CN114111843 A CN 114111843A
Authority
CN
China
Prior art keywords
matrix
navigation system
time
transformation matrix
carrier
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.)
Granted
Application number
CN202111408645.5A
Other languages
English (en)
Other versions
CN114111843B (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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN202111408645.5A priority Critical patent/CN114111843B/zh
Publication of CN114111843A publication Critical patent/CN114111843A/zh
Application granted granted Critical
Publication of CN114111843B publication Critical patent/CN114111843B/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

本发明公开一种捷联惯导系统最优动基座初始对准方法，属于捷联惯性导航领域。所述方法主要包括以下步骤：首先利用全球卫星导航系统数据和捷联惯性导航系统数据计算导航系变换矩阵和载体系变换矩阵，将对准起始时刻的姿态矩阵确定问题转化为多矢量定姿问题，采用滑动窗口构建新的观测矢量，利用奇异值分解算法求解多矢量定姿问题，然后结合全球卫星导航系统和捷联惯性导航系统的输出信息，建立状态模型和量测模型，最后采用二型模糊自适应卡尔曼滤波算法估计状态量并不断修正姿态矩阵，实现捷联惯性导航系统动基座初始对准，提高了初始对准的精度和稳定性，应用前景广阔。

Description

一种捷联惯导航系统最优动基座初始对准方法
技术领域
本发明涉及一种捷联惯导系统最优动基座初始对准方法，属于捷联惯性导航初始对准技术领域。
背景技术
捷联式惯性导航系统(Strapdown Inertial Navigation System,SINS)可以根据陀螺仪输出的角速率和加速度计输出的比力计算载体的姿态、速度和位置，广泛应用于各种军事和民用领域。捷联惯导系统需要在导航前进行初始对准，初始姿态矩阵的确定是导航算法成功运行的前提，理想的初始对准算法应具有较快的收敛速度和较高的收敛精度。初始对准可分为两个主要过程：粗对准和精对准。粗对准是在短时间内提供一个大致已知的初始姿态矩阵，为精对准做准备。精对准通常能准确地描述捷联惯导系统中误差的传播规律，以卡尔曼滤波及其改进方法为数学工具，利用精确的误差传播模型对捷联惯导系统的误差进行估计，通过误差补偿得到了精确的对准结果，精对准的结果需要通过精确的粗对准结果来保证。
代表性粗对准方法主要包括静基座或准静基座下的解析法和动态环境下的基于优化的对准方法。解析法利用地球自转角速度和重力加速度作为信息源。虽然解析法原理简单，计算量小，但其信息利用率不高，对准精度较低。因此，通过构建观测矢量组将对准问题转化为多矢量定姿问题，采用最优对准方法(Optimization-Based Alignment,OBA)可提高信息利用率，从而提高对准精度，但是OBA方法的主要缺点是无法抵抗器件误差带来的影响。而在OBA方法基础上，学者通过建立非线性状态方程和量测方程估计惯性器件误差，提高了初始对准精度，但是增加了计算难度。
基于此，研究一种可以抵抗惯性器件误差影响，且适应性强、算法简单的动基座初始对准算法，同时算法可以不受初始姿态角误差大小的影响，成为了行业发展的方向。
发明内容
本发明的目的是针对传统动基座粗对准方法精度低、适应性差的问题，提供一种捷联惯导系统最优动基座初始对准方法，该方法通过缩短观测矢量的积分间隔，减少惯性器件的积累，并借助全球导航卫星系统提供的信息建立状态方程和量测方程，利用二型模糊自适应卡尔曼滤波算法进行估计与补偿，减小惯性器件误差带来的影响，从而得到更加精确的初始对准结果。
上述的目的通过以下技术方案实现：
一种捷联惯导系统最优动基座初始对准方法，该方法包括如下步骤：
(1)利用全球卫星导航系统和捷联惯性导航系统的输出信息计算导航系变换矩阵和载体系变换矩阵；
(2)采用滑动窗口构建新的观测矢量α'和新的观测矢量β'，并对新的观测矢量α'和新的观测矢量β'进行离散化处理；
(3)利用奇异值分解算法对多矢量定姿问题进行求解，求得对准起始时刻的载体系到导航系的坐标变换矩阵；
(4)利用全球卫星导航系统和捷联惯性导航系统的输出信息，以陀螺仪的常值漂移和等效旋转矢量作为状态变量，构建状态模型和量测模型；
(5)采用二型模糊自适应卡尔曼滤波算法估计等效旋转矢量和陀螺仪常值漂移，通过估计结果对步骤(1)得到的载体系变换矩阵进行修正；
(6)根据姿态矩阵的链乘规则，利用步骤(5)修正后的载体系变换矩阵和步骤(1)得到的导航系变换矩阵以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵计算对准终止时刻的载体系到导航系的坐标变换矩阵。
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(1)的具体方法是：
求解导航系变换矩阵微分方程，根据导航系变换矩阵微分方程得到导航系变换矩阵
Figure BDA0003373163900000021
其中导航系变换矩阵微分方程为：
Figure BDA0003373163900000022
式中，n表示导航坐标系，n(0)表示0时刻的导航坐标系，n(t)表示t时刻的导航坐标系，i表示地心惯性坐标系，
Figure BDA0003373163900000023
表示0时刻导航坐标系到t时刻导航坐标系的坐标变换矩阵，矩阵
Figure BDA0003373163900000024
是矩阵
Figure BDA0003373163900000025
的转置，
Figure BDA0003373163900000026
表示从n系到i系的旋转角速度在n系的投影，
Figure BDA0003373163900000027
表示由
Figure BDA0003373163900000028
矢量构成的反对称矩阵，
Figure BDA0003373163900000029
表示矩阵
Figure BDA00033731639000000210
的微分。
求解载体系变换矩阵微分方程，根据载体系变换矩阵微分方程得到载体系变换矩阵
Figure BDA00033731639000000211
其中载体系变换矩阵微分方程为：
Figure BDA00033731639000000212
式中，b表示载体坐标系，b(0)表示0时刻的载体坐标系，b(t)表示t时刻的载体坐标系，
Figure BDA00033731639000000213
表示0时刻载体坐标系到t时刻载体坐标系的坐标变换矩阵，矩阵
Figure BDA00033731639000000214
是矩阵
Figure BDA00033731639000000215
的转置，
Figure BDA00033731639000000216
表示陀螺仪三轴输出，
Figure BDA00033731639000000217
表示由
Figure BDA00033731639000000218
矢量构成的反对称矩阵，
Figure BDA00033731639000000219
表示矩阵
Figure BDA00033731639000000220
的微分。
根据导航系变换矩阵和载体系变换矩阵微分方程得到导航系变换矩阵
Figure BDA0003373163900000031
和载体系变换矩阵
Figure BDA0003373163900000032
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(2)的具体方法是：
根据姿态矩阵的链乘规则，比力方程写为：
Figure BDA0003373163900000033
式中，Vn为导航坐标系中速度，
Figure BDA0003373163900000034
表示Vn的微分，fb表示加速度计输出，gn为重力加速度，e表示地球坐标系，
Figure BDA0003373163900000035
表示地球自转角速度在导航坐标系中的投影，
Figure BDA0003373163900000036
表示载体线运动引起导航坐标系相对地球坐标系的角速度在导航坐标系的投影。
对式(3)进行移项，两边同乘导航系变换矩阵
Figure BDA0003373163900000037
并且积分，积分区间为[0,tk]，得到：
Figure BDA0003373163900000038
式中，
Figure BDA0003373163900000039
表示对准起始时刻的载体系到导航系的坐标变换矩阵，对式(4)进行变形得到观测矢量α(0,tk)和观测矢量β(0,tk)：
Figure BDA00033731639000000310
Figure BDA00033731639000000311
Figure BDA00033731639000000312
Figure BDA00033731639000000313
式中，α(0,tk)表示从时间0到时间tk的观测矢量，α(tk,tk+m)表示从时间tk到时间tk+m的观测矢量，β(0,tk)表示从时间0到时间tk的观测矢量，β(tk,tk+m)表示从时间tk到时间tk+m的观测矢量。将α(tk,tk+m)和β(tk,tk+m)作为新的观测矢量α'和新的观测矢量β'，[tk，tk+m]是一个固定间隔的滑动窗口。新观测矢量的积分区间是[tk，tk+m]，而不是[0，tk+m]，这可以削弱惯性器件测量引起的累积误差。
对新的观测矢量α'和新的观测矢量β'进行离散化处理：
Figure BDA00033731639000000314
Figure BDA00033731639000000315
式中，T表示姿态更新周期，tk+m＝(k+m)T，表示tk+m时刻，I表示单位矩阵。
所述的一种全球导航卫星系统辅助捷联惯性导航系统的最优动基座初始对准方法，步骤(3)的具体方法是：
利用步骤(2)的新的观测矢量构造目标函数
Figure BDA0003373163900000041
Figure BDA0003373163900000042
式中，wi为权重系数，一般取为1，目标函数反映同一矢量在两个坐标系中测量值的不一致误差，min表示最小值。对目标函数变形，将求目标函数最小值问题转化为求解矩阵
Figure BDA0003373163900000043
奇异值问题，α′i和β′i表示第i个新的观测矢量α'和新的观测矢量β'，对矩阵A进行奇异值分解得到A＝UDVT，U和V都是酉矩阵，D表示矩阵A的奇异值构成的对角矩阵，得到对准起始时刻的最优姿态矩阵：
Figure BDA0003373163900000044
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(4)的具体方法是：
选择状态量
Figure BDA0003373163900000045
其中，真实载体坐标系b系到计算载体坐标系
Figure BDA0003373163900000046
系的等效旋转矢量为φ＝[φx,φy,φz]T,φx,φy,φz分别表示x轴，y轴，z轴的等效旋转矢量
Figure BDA0003373163900000047
表示陀螺仪的常值漂移，
Figure BDA0003373163900000048
分别表示x轴，y轴，z轴的陀螺仪常值漂移；
建立全球卫星导航系统辅助捷联惯性导航系统对准的状态方程：
Figure BDA0003373163900000049
Figure BDA00033731639000000410
式中，
Figure BDA00033731639000000411
表示陀螺仪的实际输出，
Figure BDA00033731639000000412
表示φ的微分，
Figure BDA00033731639000000413
表示εb的微分。
建立全球卫星导航系统辅助捷联惯性导航系统对准的量测方程：
Zk＝Hkφ+Vk (15)
Figure BDA00033731639000000414
Figure BDA00033731639000000415
式中，Zk表示量测量，Hk表示量测矩阵，Vk表示量测噪声，
Figure BDA00033731639000000416
表示t时刻的计算载体坐标系，
Figure BDA00033731639000000417
表示计算载体坐标系变换矩阵，矩阵
Figure BDA00033731639000000418
是矩阵
Figure BDA00033731639000000419
的转置。
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(5)的具体方法是：
理想情况下，新息向量近似于零均值白噪声序列，通过新息向量可以监测滤波过程中的异常情况。观察新息序列可以通过将其实际协方差矩阵
Figure BDA0003373163900000051
与理论协方差矩阵Sk的对比完成。
新息向量实际协方差矩阵
Figure BDA0003373163900000052
与理论协方差矩阵Sk分别表示为：
Figure BDA0003373163900000053
Figure BDA0003373163900000054
式中，N表示窗口大小，ei表示i时刻的新息向量，
Figure BDA0003373163900000055
表示ei的转置，Pk,k-1表示状态一步预测均方误差阵，
Figure BDA0003373163900000056
表示k时刻的量测噪声协方差矩阵，新息向量实际协方差与理论协方差的比值定义为Mk：
Figure BDA0003373163900000057
式中，tr(.)表示求迹运算。
采用Sage-husa自适应滤波器实时估计和修正量测噪声的统计特性，k时刻的量测噪声协方差矩阵
Figure BDA0003373163900000058
表示为：
Figure BDA0003373163900000059
式中，
Figure BDA00033731639000000510
表示k-1时刻的量测噪声协方差矩阵，Kk表示滤波增益，Pk表示状态估计的均方误差阵，ek表示k时刻的新息向量，dk表示当前信息的权重，dk＝(1-b)/(1-bk+1)，0<b<1，b为遗忘因子；
将Mk作为二型模糊逻辑的输入，自适应调节因子a为二型模糊逻辑的输出，通过a来调节量测噪声协方差矩阵。
Mk>1表明量测噪声偏大，应使a>1，使量测噪声协方差矩阵增大；Mk<1表明量测噪声偏小，应使a<1，使量测噪声协方差矩阵减小；输入变量Mk的模糊集定义为{小，正常，大}，记为{NB，ZE，PB}；输出变量a的模糊集定义为{减少、保持、增大}，记为{DB、MA、IB}。k+1时刻量测噪声协方差矩阵
Figure BDA00033731639000000511
表示为：
Figure BDA00033731639000000512
利用估计出的等效旋转矢量φ不断修正载体系变换矩阵：
Figure BDA00033731639000000513
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(6)的具体方法是：
根据姿态矩阵的链乘规则，利用步骤(5)得到的修正后的载体系变换矩阵
Figure BDA00033731639000000514
和步骤(1)得到的导航系变换矩阵
Figure BDA00033731639000000515
以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵
Figure BDA0003373163900000061
计算对准终止时刻的载体系到导航系的坐标变换矩阵
Figure BDA0003373163900000062
Figure BDA0003373163900000063
本发明的有益效果是：本发明通过滑动窗口缩短观测矢量的积分区间，减少了惯性器件误差的积累。并且，借助全球卫星导航系统提供的信息，建立状态模型和量测模型，估计陀螺仪的常值漂移和实际载体坐标系到计算载体坐标系的等效旋转矢量，并反馈到多矢量构造过程中。采用二型模糊自适应卡尔曼滤波算法解决了量测噪声不准确的问题，提高对准算法的稳定性，最终得到更精确的初始对准结果。
附图说明
图1本发明原理示意图；
图2仿真运动轨迹；
图3仿真参考姿态角变化，图3中(a)是参考航向角变化，图3中(b)是参考俯仰角变化，图3中(c)是参考横滚角变化；
图4不同算法初始对准姿态角仿真误差对比曲线，图4中(a)是航向角误差对比曲线，图4中(b)是俯仰角误差对比曲线，图4中(c)是横滚角误差对比曲线；
图5不同算法初始对准姿态角半物理误差对比曲线，图5中(a)是航向角误差对比曲线，图5中(b)是俯仰角误差对比曲线，图5中(c)是横滚角误差对比曲线；图4-图5中OBA点划线表示传统的基于优化的初始对准算法，IFA-VIF实线表示基于速度观测矢量的优化初始对准算法，AE-IKF虚线表示基于间接卡尔曼滤波的优化初始对准算法，虚线+圆标记表示本发明所提出的捷联惯导系统最优动基座初始对准方法。
具体实施方式
如图1所示，本发明提供了一种捷联惯导系统最优动基座初始对准方法，包括如下步骤：
(1)利用全球卫星导航系统和捷联惯性导航系统的输出信息计算导航系变换矩阵和载体系变换矩阵；
(2)采用滑动窗口构建新的观测矢量α'和新的观测矢量β'，并对新的观测矢量α'和新的观测矢量β'进行离散化处理；
(3)利用奇异值分解算法对多矢量定姿问题进行求解，求得对准起始时刻的载体系到导航系的坐标变换矩阵；
(4)利用全球卫星导航系统和捷联惯性导航系统的输出信息，以陀螺仪的常值漂移和等效旋转矢量作为状态变量，构建状态模型和量测模型；
(5)采用二型模糊自适应卡尔曼滤波算法估计等效旋转矢量和陀螺仪常值漂移，通过估计结果对步骤(1)得到的载体系变换矩阵进行修正；
(6)根据姿态矩阵的链乘规则，利用步骤(5)修正后的载体系变换矩阵和步骤(1)得到的导航系变换矩阵以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵计算对准终止时刻的载体系到导航系的坐标变换矩阵。
步骤一、求解导航系变换矩阵微分方程，根据导航系变换矩阵微分方程得到导航系变换矩阵
Figure BDA0003373163900000071
其中导航系变换矩阵微分方程为：
Figure BDA0003373163900000072
式中，n表示导航坐标系，n(0)表示0时刻的导航坐标系，n(t)表示t时刻的导航坐标系，i表示地心惯性坐标系，
Figure BDA0003373163900000073
表示0时刻导航坐标系到t时刻导航坐标系的坐标变换矩阵，矩阵
Figure BDA0003373163900000074
是矩阵
Figure BDA0003373163900000075
的转置，
Figure BDA0003373163900000076
表示从n系到i系的旋转角速度在n系的投影，
Figure BDA0003373163900000077
表示由
Figure BDA0003373163900000078
矢量构成的反对称矩阵，
Figure BDA0003373163900000079
表示矩阵
Figure BDA00033731639000000710
的微分。
求解载体系变换矩阵微分方程，根据载体系变换矩阵微分方程得到载体系变换矩阵
Figure BDA00033731639000000711
其中载体系变换矩阵微分方程为：
Figure BDA00033731639000000712
式中，b表示载体坐标系，b(0)表示0时刻的载体坐标系，b(t)表示t时刻的载体坐标系，
Figure BDA00033731639000000713
表示0时刻载体坐标系到t时刻载体坐标系的坐标变换矩阵，矩阵
Figure BDA00033731639000000714
是矩阵
Figure BDA00033731639000000715
的转置，
Figure BDA00033731639000000716
表示陀螺仪三轴输出，
Figure BDA00033731639000000717
表示由
Figure BDA00033731639000000718
矢量构成的反对称矩阵，
Figure BDA00033731639000000719
表示矩阵
Figure BDA00033731639000000720
的微分。
根据导航系变换矩阵和载体系变换矩阵微分方程得到导航系变换矩阵
Figure BDA00033731639000000721
和载体系变换矩阵
Figure BDA00033731639000000722
步骤二、根据姿态矩阵的链乘规则，比力方程写为：
Figure BDA00033731639000000723
式中，Vn为导航坐标系中速度，
Figure BDA00033731639000000724
表示Vn的微分，fb表示加速度计输出，gn为重力加速度，e表示地球坐标系，
Figure BDA00033731639000000725
表示地球自转角速度在导航坐标系中的投影，
Figure BDA00033731639000000726
表示载体线运动引起导航坐标系相对地球坐标系的角速度在导航坐标系的投影。
对式(3)进行移项，两边同乘导航系变换矩阵
Figure BDA00033731639000000727
并且积分，积分区间为[0,tk]，得到：
Figure BDA00033731639000000728
式中，
Figure BDA00033731639000000729
表示对准起始时刻的载体系到导航系的坐标变换矩阵，对式(4)进行变形得到观测矢量α(0,tk)和观测矢量β(0,tk)：
Figure BDA0003373163900000081
Figure BDA0003373163900000082
Figure BDA0003373163900000083
Figure BDA0003373163900000084
式中，α(0,tk)表示从时间0到时间tk的观测矢量，α(tk,tk+m)表示从时间tk到时间tk+m的观测矢量，β(0,tk)表示从时间0到时间tk的观测矢量，β(tk,tk+m)表示从时间tk到时间tk+m的观测矢量。将α(tk,tk+m)和β(tk,tk+m)作为新的观测矢量α'和新的观测矢量β'，[tk，tk+m]是一个固定间隔的滑动窗口。新观测矢量的积分区间是[tk，tk+m]，而不是[0，tk+m]，这可以削弱惯性器件测量引起的累积误差。
对新的观测矢量α'和新的观测矢量β'进行离散化处理：
Figure BDA0003373163900000085
Figure BDA0003373163900000086
式中，T表示姿态更新周期，取为5ms，tk+m＝(k+m)T，m取为10，表示tk+m时刻，I表示单位矩阵。
步骤三、利用步骤(2)的新的观测矢量构造目标函数
Figure BDA0003373163900000087
Figure BDA0003373163900000088
式中，wi为权重系数，一般取为1，目标函数反映同一矢量在两个坐标系中测量值的不一致误差，min表示最小值。对目标函数变形，将求目标函数最小值问题转化为求解矩阵
Figure BDA0003373163900000089
奇异值问题，α′i和β′i表示第i个新的观测矢量α'和新的观测矢量β'，对矩阵A进行奇异值分解得到A＝UDVT，U和V都是酉矩阵，D表示矩阵A的奇异值构成的对角矩阵，得到对准起始时刻的最优姿态矩阵：
Figure BDA00033731639000000810
步骤四、选择状态量
Figure BDA00033731639000000811
其中，真实载体坐标系b系到计算载体坐标系
Figure BDA0003373163900000091
系的等效旋转矢量为φ＝[φx,φy,φz]T,φx,φy,φz分别表示x轴，y轴，z轴的等效旋转矢量
Figure BDA0003373163900000092
表示陀螺仪的常值漂移，
Figure BDA0003373163900000093
分别表示x轴，y轴，z轴的陀螺仪常值漂移；
建立全球卫星导航系统辅助捷联惯性导航系统对准的状态方程：
Figure BDA0003373163900000094
Figure BDA0003373163900000095
式中，
Figure BDA0003373163900000096
表示陀螺仪的实际输出，
Figure BDA0003373163900000097
表示φ的微分，
Figure BDA0003373163900000098
表示εb的微分。
建立全球卫星导航系统辅助捷联惯性导航系统对准的量测方程：
Zk＝Hkφ+Vk (15)
Figure BDA0003373163900000099
Figure BDA00033731639000000910
式中，Zk表示量测量，Hk表示量测矩阵，Vk表示量测噪声，
Figure BDA00033731639000000911
表示t时刻的计算载体坐标系，
Figure BDA00033731639000000912
表示计算载体坐标系变换矩阵，矩阵
Figure BDA00033731639000000913
是矩阵
Figure BDA00033731639000000914
的转置。
步骤五、理想情况下，新息向量近似于零均值白噪声序列，通过新息向量可以监测滤波过程中的异常情况。观察新息序列可以通过将其实际协方差矩阵
Figure BDA00033731639000000915
与理论协方差矩阵Sk的对比完成。
新息向量实际协方差矩阵
Figure BDA00033731639000000916
与理论协方差矩阵Sk分别表示为：
Figure BDA00033731639000000917
Figure BDA00033731639000000918
式中，N表示窗口大小，ei表示i时刻的新息向量，
Figure BDA00033731639000000919
表示ei的转置，Pk,k-1表示状态一步预测均方误差阵，
Figure BDA00033731639000000920
表示k时刻的量测噪声协方差矩阵，新息向量实际协方差与理论协方差的比值定义为Mk：
Figure BDA00033731639000000921
式中，tr(.)表示求迹运算。
采用Sage-husa自适应滤波器实时估计和修正量测噪声的统计特性，k时刻的量测噪声协方差矩阵
Figure BDA00033731639000000922
表示为：
Figure BDA0003373163900000101
式中，
Figure BDA0003373163900000102
表示k-1时刻的量测噪声协方差矩阵，Kk表示滤波增益，Pk表示状态估计的均方误差阵，ek表示k时刻的新息向量，dk表示当前信息的权重，dk＝(1-b)/(1-bk+1)，0<b<1，b为遗忘因子；
将Mk作为二型模糊逻辑的输入，自适应调节因子a为二型模糊逻辑的输出，通过a来调节量测噪声协方差矩阵。
Mk>1表明量测噪声偏大，应使a>1，使量测噪声协方差矩阵增大。Mk<1表明量测噪声偏小，应使a<1，使量测噪声协方差矩阵减小。使Mk始终保持在1附近，以满足新息向量的实际协方差近似等于理论协方差的滤波器稳定性条件。输入变量Mk的模糊集定义为{小，正常，大}，记为{NB，ZE，PB}。输出变量a的模糊集定义为{减少、保持、增大}，记为{DB、MA、IB}。k+1时刻量测噪声协方差矩阵
Figure BDA0003373163900000103
表示为：
Figure BDA0003373163900000104
利用估计出的等效旋转矢量φ不断修正载体系变换矩阵：
Figure BDA0003373163900000105
步骤六、根据姿态矩阵的链乘规则，利用步骤(5)得到的修正后的载体系变换矩阵
Figure BDA0003373163900000106
和步骤(1)得到的导航系变换矩阵
Figure BDA0003373163900000107
以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵
Figure BDA0003373163900000108
计算对准终止时刻的载体系到导航系的坐标变换矩阵
Figure BDA0003373163900000109
Figure BDA00033731639000001010
仿真实验：
(1)惯性器件采样频率为200Hz，对准时间为500s，初始经度为106.6906°，初始纬度为26.5019°，陀螺常值漂移各方向均设为0.01°/h，陀螺随机噪声为0.005°/h1/2，加速度计的常值偏置为0.1mg，加速度计的随机噪声为0.1mg/Hz1/2。在仿真中，载体进行匀速运动、加速运动、减速运动、转弯运动和晃动运动。航向角、俯仰角和滚转角的振动幅度分别为1°、2°和3°，振动角频率分别为π/5rad/s、3π/10rad/s和2π/5rad/s。载体仿真运动轨迹如图2所示，仿真参考姿态角变化如图3所示，对准时间为500s。载体的初始速度为4m/s，在5s～25s内进行加速度为0.3m/s2的加速运动，载体在26s～74s内作匀速运动，加速度为-0.1m/s2的减速运动在75s～85s内完成。载体从95s开始拐弯，拐弯角度大小为45°，拐弯角速度为1°/s，然后，载体沿直线运动，并从300s开始拐弯，转向角度大小为45°，拐弯角速度为2°/s，最后进行匀速直线运动。不同算法初始对准姿态角仿真误差对比曲线如图4所示。
(2)利用跑车数据进行半物理实验，不同算法初始对准姿态角半物理误差对比曲线如图5所示。
由图4可以看出，在对准过程的最后100秒内，本发明所提方法的航向误差收敛到0.025°以内，而其他三种方法的航向误差分别收敛到0.9°、0.4°和0.035°以内。传统OBA方法的收敛速度相对较慢，而IFA-VIF算法、AE-IKF算法和本发明所提算法的收敛速度相对较快。然而，IFA-VIF算法不考虑惯性器件误差对对准结果的影响，惯性器件的常值误差将随时间积累。本发明所提算法与AE-IKF算法相比，引入自适应滤波算法对量测噪声进行估计，有助于提高系统的稳定性。由图4与图5可以看出，与OBA算法、IFA-VIF算法和AE-IKF算法相比，本发明动基座下初始对准性能是最优的。
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (10)

1.一种捷联惯导系统最优动基座初始对准方法，其特征在于，该方法包括如下步骤：
(1)利用全球卫星导航系统和捷联惯性导航系统的输出信息计算导航系变换矩阵和载体系变换矩阵；
(2)采用滑动窗口构建新的观测矢量α'和新的观测矢量β'，并对新的观测矢量α'和新的观测矢量β'进行离散化处理；
(3)利用奇异值分解算法对多矢量定姿问题进行求解，求得对准起始时刻的载体系到导航系的坐标变换矩阵；
(4)利用全球卫星导航系统和捷联惯性导航系统的输出信息，以陀螺仪的常值漂移和等效旋转矢量作为状态变量，构建状态模型和量测模型；
(5)采用二型模糊自适应卡尔曼滤波算法估计等效旋转矢量和陀螺仪常值漂移，通过估计结果对步骤(1)得到的载体系变换矩阵进行修正；
(6)根据姿态矩阵的链乘规则，利用步骤(5)修正后的载体系变换矩阵和步骤(1)得到的导航系变换矩阵以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵计算对准终止时刻的载体系到导航系的坐标变换矩阵。
2.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(1)中所述计算导航系变换矩阵的具体方法是求解导航系变换矩阵微分方程，根据导航系变换矩阵微分方程得到导航系变换矩阵
Figure FDA0003373163890000011
其中导航系变换矩阵微分方程为：
Figure FDA0003373163890000012
式中，n表示导航坐标系，n(0)表示0时刻的导航坐标系，n(t)表示t时刻的导航坐标系，i表示地心惯性坐标系，
Figure FDA0003373163890000013
表示0时刻导航坐标系到t时刻导航坐标系的坐标变换矩阵，矩阵
Figure FDA0003373163890000014
是矩阵
Figure FDA0003373163890000015
的转置，
Figure FDA0003373163890000016
表示从n系到i系的旋转角速度在n系的投影，
Figure FDA0003373163890000017
表示由
Figure FDA0003373163890000018
矢量构成的反对称矩阵，
Figure FDA0003373163890000019
表示矩阵
Figure FDA00033731638900000110
的微分。
3.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(1)中所述计算载体系变换矩阵的具体方法是求解载体系变换矩阵微分方程，根据载体系变换矩阵微分方程得到载体系变换矩阵
Figure FDA00033731638900000111
其中载体系变换矩阵微分方程为：
Figure FDA00033731638900000112
式中，b表示载体坐标系，b(0)表示0时刻的载体坐标系，b(t)表示t时刻的载体坐标系，
Figure FDA0003373163890000021
表示0时刻载体坐标系到t时刻载体坐标系的坐标变换矩阵，矩阵
Figure FDA0003373163890000022
是矩阵
Figure FDA0003373163890000023
的转置，
Figure FDA0003373163890000024
表示陀螺仪三轴输出，
Figure FDA0003373163890000025
表示矩阵
Figure FDA0003373163890000026
的微分，
Figure FDA0003373163890000027
表示由
Figure FDA0003373163890000028
矢量构成的反对称矩阵。
4.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(2)的具体方法是：
根据姿态矩阵的链乘规则，比力方程写为：
Figure FDA0003373163890000029
式中，Vn为导航坐标系中速度，
Figure FDA00033731638900000210
表示Vn的微分，fb表示加速度计输出，gn为重力加速度，e表示地球坐标系，
Figure FDA00033731638900000211
表示地球自转角速度在导航坐标系中的投影，
Figure FDA00033731638900000212
表示载体线运动引起导航坐标系相对地球坐标系的角速度在导航坐标系的投影；
对式(3)进行移项，两边同乘导航系变换矩阵
Figure FDA00033731638900000213
并且积分，积分区间为[0,tk]，得到：
Figure FDA00033731638900000214
式中，
Figure FDA00033731638900000215
表示对准起始时刻的载体系到导航系的坐标变换矩阵，对式(4)进行变形得到观测矢量α(0,tk)和观测矢量β(0,tk)：
Figure FDA00033731638900000216
Figure FDA00033731638900000217
Figure FDA00033731638900000218
Figure FDA00033731638900000219
式中，α(0,tk)表示从时间0到时间tk的观测矢量，α(tk,tk+m)表示从时间tk到时间tk+m的观测矢量，β(0,tk)表示从时间0到时间tk的观测矢量，β(tk,tk+m)表示从时间tk到时间tk+m的观测矢量，将α(tk,tk+m)和β(tk,tk+m)作为新的观测矢量α'和新的观测矢量β'，[tk，tk+m]是一个固定间隔的滑动窗口；
对新的观测矢量α'和新的观测矢量β'进行离散化处理：
Figure FDA00033731638900000220
Figure FDA0003373163890000031
式中，T表示姿态更新周期，tk+m＝(k+m)T，表示tk+m时刻，I表示单位矩阵。
5.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(3)的具体方法是：
利用步骤(2)的新的观测矢量构造目标函数
Figure FDA0003373163890000032
Figure FDA0003373163890000033
式中，wi为权重系数，min表示最小值；
对目标函数变形，将求目标函数最小值问题转化为求解矩阵
Figure FDA0003373163890000034
奇异值问题，α′i和β′i表示第i个新的观测矢量α'和新的观测矢量β'，对矩阵A进行奇异值分解得到A＝UDVT，U和V都是酉矩阵，D表示矩阵A的奇异值构成的对角矩阵，得到对准起始时刻的最优姿态矩阵：
Figure FDA0003373163890000035
6.根据权利要求5所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，权重系数wi取为1。
7.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(4)的具体方法是：
选择状态量
Figure FDA0003373163890000036
其中，载体坐标系b系到计算载体坐标系
Figure FDA0003373163890000037
系的等效旋转矢量为φ＝[φx,φy,φz]T,φx,φy,φz分别表示x轴，y轴，z轴的等效旋转矢量，
Figure FDA0003373163890000038
表示陀螺仪的常值漂移，
Figure FDA0003373163890000039
分别表示x轴，y轴，z轴的陀螺仪常值漂移；
建立全球卫星导航系统辅助捷联惯性导航系统对准的状态方程：
Figure FDA00033731638900000310
Figure FDA00033731638900000311
式中，
Figure FDA00033731638900000312
表示陀螺仪的实际输出，
Figure FDA00033731638900000313
表示φ的微分，
Figure FDA00033731638900000314
表示εb的微分；
建立全球卫星导航系统辅助捷联惯性导航系统对准的量测方程：
Zk＝Hkφ+Vk (15)
Figure FDA0003373163890000041
Figure FDA0003373163890000042
式中，Zk表示量测量，Hk表示量测矩阵，Vk表示量测噪声，
Figure FDA0003373163890000043
表示t时刻的计算载体坐标系，
Figure FDA0003373163890000044
表示计算载体坐标系变换矩阵，矩阵
Figure FDA0003373163890000045
是矩阵
Figure FDA0003373163890000046
的转置。
8.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(5)的具体方法是：
新息向量实际协方差矩阵
Figure FDA0003373163890000047
与理论协方差矩阵Sk分别表示为：
Figure FDA0003373163890000048
Figure FDA0003373163890000049
式中，N表示窗口大小，Pk,k-1表示状态一步预测均方误差阵，ei表示第i时刻的新息向量，
Figure FDA00033731638900000410
表示ei的转置，
Figure FDA00033731638900000411
表示k时刻的量测噪声协方差矩阵，新息向量实际协方差与理论协方差的比值定义为Mk：
Figure FDA00033731638900000412
式中，tr(.)表示求迹运算。
采用Sage-husa自适应滤波器实时估计和修正量测噪声的统计特性，k时刻的量测噪声协方差矩阵
Figure FDA00033731638900000413
表示为：
Figure FDA00033731638900000414
式中，
Figure FDA00033731638900000415
表示k-1时刻的量测噪声协方差矩阵，Kk表示滤波增益，Pk表示状态估计的均方误差阵，ek表示k时刻的新息向量，dk表示当前信息的权重，dk＝(1-b)/(1-bk+1)，0<b<1，b为遗忘因子；
将Mk作为二型模糊逻辑的输入，自适应调节因子a为二型模糊逻辑的输出，通过a来调节量测噪声协方差矩阵，k+1时刻量测噪声协方差矩阵
Figure FDA00033731638900000416
表示为：
Figure FDA00033731638900000417
利用估计出的等效旋转矢量φ不断修正载体系变换矩阵：
Figure FDA0003373163890000051
9.根据权利要求8所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，所述调节量测噪声协方差矩阵的具体方法是，当Mk>1表明量测噪声偏大，应使a>1，使量测噪声协方差矩阵增大；当Mk<1表明量测噪声偏小，应使a<1，使量测噪声协方差矩阵减小；输入变量Mk的模糊集定义为{小，正常，大}，记为{NB，ZE，PB}；输出变量a的模糊集定义为{减少、保持、增大}，记为{DB、MA、IB}。
10.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(6)的具体方法是：
根据姿态矩阵的链乘规则，利用步骤(5)得到的修正后的载体系变换矩阵
Figure FDA0003373163890000052
和步骤(1)得到的导航系变换矩阵
Figure FDA0003373163890000053
以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵
Figure FDA0003373163890000054
计算对准终止时刻的载体系到导航系的坐标变换矩阵
Figure FDA0003373163890000055
Figure FDA0003373163890000056
CN202111408645.5A 2021-11-24 2021-11-24 一种捷联惯导系统最优动基座初始对准方法 Active CN114111843B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111408645.5A CN114111843B (zh) 2021-11-24 2021-11-24 一种捷联惯导系统最优动基座初始对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111408645.5A CN114111843B (zh) 2021-11-24 2021-11-24 一种捷联惯导系统最优动基座初始对准方法

Publications (2)

Publication Number Publication Date
CN114111843A true CN114111843A (zh) 2022-03-01
CN114111843B CN114111843B (zh) 2022-09-02

Family

ID=80372572

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111408645.5A Active CN114111843B (zh) 2021-11-24 2021-11-24 一种捷联惯导系统最优动基座初始对准方法

Country Status (1)

Country Link
CN (1) CN114111843B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115655308A (zh) * 2022-10-21 2023-01-31 湖南科众兄弟科技有限公司 一种组合导航系统空中自对准方法及装置
CN116858287A (zh) * 2023-07-06 2023-10-10 哈尔滨工程大学 一种基于地球坐标系的极区惯导初始对准方法
CN118111427A (zh) * 2024-04-30 2024-05-31 西安现代控制技术研究所 一种基于姿态预积分的弹载捷联惯导系统姿态保持方法
CN118500390A (zh) * 2024-04-26 2024-08-16 江苏科技大学 一种鲁棒抗干扰的捷联惯导系统行进间对准方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103727940A (zh) * 2014-01-15 2014-04-16 东南大学 基于重力加速度矢量匹配的非线性初始对准方法
WO2019122987A1 (en) * 2017-12-22 2019-06-27 Shakibay Senobari Mohammad Accurate initialization of strapdown inertial navigation system
CN109945859A (zh) * 2019-04-01 2019-06-28 东南大学 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN110702143A (zh) * 2019-10-19 2020-01-17 北京工业大学 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111707292A (zh) * 2020-07-18 2020-09-25 东南大学 一种自适应滤波的快速传递对准方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103727940A (zh) * 2014-01-15 2014-04-16 东南大学 基于重力加速度矢量匹配的非线性初始对准方法
WO2019122987A1 (en) * 2017-12-22 2019-06-27 Shakibay Senobari Mohammad Accurate initialization of strapdown inertial navigation system
CN109945859A (zh) * 2019-04-01 2019-06-28 东南大学 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN110702143A (zh) * 2019-10-19 2020-01-17 北京工业大学 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111707292A (zh) * 2020-07-18 2020-09-25 东南大学 一种自适应滤波的快速传递对准方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
SAEED KHANKALANTARY等: "An Adaptive Constrained Type-2 Fuzzy Hammerstein Neural Network Data Fusion Scheme for Low-cost SINS/GNSS Navigation System", 《APPLIED SOFT COMPUTEING》 *
刘文倩等: "基于多矢量定姿的动基座最优化对准算法", 《飞控与探测》 *
刘超等: "基于模糊测度与累积前景理论的区间二型模糊多准则决策方法", 《运筹与管理》 *
李瑶等: "一种纬度未知条件下捷联惯导抗扰动自对准算法", 《中国惯性技术学报》 *
武萌等: "基于多新息强跟踪SINS大失准角初始对准研究", 《压电与声光》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115655308A (zh) * 2022-10-21 2023-01-31 湖南科众兄弟科技有限公司 一种组合导航系统空中自对准方法及装置
CN116858287A (zh) * 2023-07-06 2023-10-10 哈尔滨工程大学 一种基于地球坐标系的极区惯导初始对准方法
CN118500390A (zh) * 2024-04-26 2024-08-16 江苏科技大学 一种鲁棒抗干扰的捷联惯导系统行进间对准方法
CN118111427A (zh) * 2024-04-30 2024-05-31 西安现代控制技术研究所 一种基于姿态预积分的弹载捷联惯导系统姿态保持方法

Also Published As

Publication number Publication date
CN114111843B (zh) 2022-09-02

Similar Documents

Publication Publication Date Title
CN114111843B (zh) 一种捷联惯导系统最优动基座初始对准方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
US11187535B1 (en) Attitude measurement method
CN109631913A (zh) 基于非线性预测强跟踪无迹卡尔曼滤波的x射线脉冲星导航定位方法及系统
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN100541135C (zh) 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110440830B (zh) 动基座下车载捷联惯导系统自对准方法
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN109000642A (zh) 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN106052686B (zh) 基于dsptms320f28335的全自主捷联惯性导航系统
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN102252677A (zh) 一种基于时间序列分析的变比例自适应联邦滤波方法
CN113175926B (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN105806365A (zh) 一种基于自抗扰控制的车载惯导行进间快速初始对准方法
Xue et al. In-motion alignment algorithm for vehicle carried sins based on odometer aiding
US12061086B2 (en) Self-adaptive horizontal attitude measurement method based on motion state monitoring
CN110262237A (zh) 基于双反馈模糊神经网络的微陀螺仪超扭曲滑模控制方法
CN114877915B (zh) 一种激光陀螺惯性测量组件g敏感性误差标定装置及方法
CN113916226B (zh) 一种基于最小方差的组合导航系统抗扰滤波方法
CN110471293A (zh) 一种估计时变角速度的z轴陀螺仪滑模控制方法
CN100588907C (zh) 船用光纤陀螺捷联系统初始姿态确定方法
CN113447019A (zh) Ins/cns组合导航方法、系统、存储介质及设备
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems

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
CB03 Change of inventor or designer information

Inventor after: Cheng Xianghong

Inventor after: Liu Wenqian

Inventor after: Luo Xinchen

Inventor before: Cheng Xianghong

Inventor before: Liu Wenqian

CB03 Change of inventor or designer information
GR01 Patent grant
GR01 Patent grant