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

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

Info

Publication number
CN114111843B
CN114111843B CN202111408645.5A CN202111408645A CN114111843B CN 114111843 B CN114111843 B CN 114111843B CN 202111408645 A CN202111408645 A CN 202111408645A CN 114111843 B CN114111843 B CN 114111843B
Authority
CN
China
Prior art keywords
matrix
navigation system
time
transformation matrix
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.)
Active
Application number
CN202111408645.5A
Other languages
English (en)
Other versions
CN114111843A (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 GDA0003741054170000021
其中导航系变换矩阵微分方程为：
Figure GDA0003741054170000022
式中，n表示导航坐标系，n(0)表示0时刻的导航坐标系，n(t)表示t时刻的导航坐标系，i表示地心惯性坐标系，
Figure GDA0003741054170000023
表示0时刻导航坐标系到t时刻导航坐标系的坐标变换矩阵，矩阵
Figure GDA0003741054170000024
是矩阵
Figure GDA0003741054170000025
的转置，
Figure GDA0003741054170000026
表示从n系到i系的旋转角速度在n系的投影，
Figure GDA0003741054170000027
表示由
Figure GDA0003741054170000028
矢量构成的反对称矩阵，
Figure GDA0003741054170000029
表示矩阵
Figure GDA00037410541700000210
的微分。
求解载体系变换矩阵微分方程，根据载体系变换矩阵微分方程得到载体系变换矩阵
Figure GDA00037410541700000211
其中载体系变换矩阵微分方程为：
Figure GDA00037410541700000212
式中，b表示载体坐标系，b(0)表示0时刻的载体坐标系，b(t)表示t时刻的载体坐标系，
Figure GDA00037410541700000213
表示0时刻载体坐标系到t时刻载体坐标系的坐标变换矩阵，矩阵
Figure GDA00037410541700000214
是矩阵
Figure GDA00037410541700000215
的转置，
Figure GDA00037410541700000216
表示陀螺仪三轴输出，
Figure GDA00037410541700000217
表示由
Figure GDA00037410541700000218
矢量构成的反对称矩阵，
Figure GDA00037410541700000219
表示矩阵
Figure GDA00037410541700000220
的微分。
根据导航系变换矩阵和载体系变换矩阵微分方程得到导航系变换矩阵
Figure GDA0003741054170000031
和载体系变换矩阵
Figure GDA0003741054170000032
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(2)的具体方法是：
根据姿态矩阵的链乘规则，比力方程写为：
Figure GDA0003741054170000033
式中，Vn为导航坐标系中速度，
Figure GDA0003741054170000034
表示Vn的微分，fb表示加速度计输出，gn为重力加速度，e表示地球坐标系，
Figure GDA0003741054170000035
表示地球自转角速度在导航坐标系中的投影，
Figure GDA0003741054170000036
表示载体线运动引起导航坐标系相对地球坐标系的角速度在导航坐标系的投影。
对式(3)进行移项，两边同乘导航系变换矩阵
Figure GDA0003741054170000037
并且积分，积分区间为[0,tk]，得到：
Figure GDA0003741054170000038
式中，
Figure GDA0003741054170000039
表示对准起始时刻的载体系到导航系的坐标变换矩阵，对式(4)进行变形得到观测矢量α(0,tk)和观测矢量β(0,tk)：
Figure GDA00037410541700000310
Figure GDA00037410541700000311
Figure GDA00037410541700000312
Figure GDA00037410541700000313
式中，α(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 GDA00037410541700000314
Figure GDA00037410541700000315
式中，T表示姿态更新周期，tk+m＝(k+m)T，表示tk+m时刻，I表示单位矩阵。
所述的一种全球导航卫星系统辅助捷联惯性导航系统的最优动基座初始对准方法，步骤(3)的具体方法是：
利用步骤(2)的新的观测矢量构造目标函数
Figure GDA0003741054170000041
Figure GDA0003741054170000042
式中，wi为权重系数，一般取为1，目标函数反映同一矢量在两个坐标系中测量值的不一致误差，min表示最小值。对目标函数变形，将求目标函数最小值问题转化为求解矩阵
Figure GDA0003741054170000043
奇异值问题，α′i和β′i表示第i个新的观测矢量α'和新的观测矢量β'，对矩阵A进行奇异值分解得到A＝UDVT，U和V都是酉矩阵，D表示矩阵A的奇异值构成的对角矩阵，得到对准起始时刻的最优姿态矩阵：
Figure GDA0003741054170000044
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(4)的具体方法是：
选择状态量
Figure GDA0003741054170000045
其中，真实载体坐标系b系到计算载体坐标系
Figure GDA0003741054170000046
系的等效旋转矢量为φ＝[φx,φy,φz]T,φx,φy,φz分别表示x轴，y轴，z轴的等效旋转矢量
Figure GDA0003741054170000047
表示陀螺仪的常值漂移，
Figure GDA0003741054170000048
分别表示x轴，y轴，z轴的陀螺仪常值漂移；
建立全球卫星导航系统辅助捷联惯性导航系统对准的状态方程：
Figure GDA0003741054170000049
Figure GDA00037410541700000410
式中，
Figure GDA00037410541700000411
表示陀螺仪的实际输出，
Figure GDA00037410541700000412
表示φ的微分，
Figure GDA00037410541700000413
表示εb的微分。
建立全球卫星导航系统辅助捷联惯性导航系统对准的量测方程：
Zk＝Hkφ+Vk (15)
Figure GDA00037410541700000414
Figure GDA00037410541700000415
式中，Zk表示量测量，Hk表示量测矩阵，Vk表示量测噪声，
Figure GDA00037410541700000416
表示t时刻的计算载体坐标系，
Figure GDA00037410541700000417
表示计算载体坐标系变换矩阵，矩阵
Figure GDA00037410541700000418
是矩阵
Figure GDA00037410541700000419
的转置。
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(5)的具体方法是：
理想情况下，新息向量近似于零均值白噪声序列，通过新息向量可以监测滤波过程中的异常情况。观察新息序列可以通过将其实际协方差矩阵
Figure GDA0003741054170000051
与理论协方差矩阵Sk的对比完成。
新息向量实际协方差矩阵
Figure GDA0003741054170000052
与理论协方差矩阵Sk分别表示为：
Figure GDA0003741054170000053
Figure GDA0003741054170000054
式中，N表示窗口大小，ei表示i时刻的新息向量，
Figure GDA0003741054170000055
表示ei的转置，Pk,k-1表示状态一步预测均方误差阵，
Figure GDA0003741054170000056
表示k时刻的量测噪声协方差矩阵，新息向量实际协方差与理论协方差的比值定义为Mk：
Figure GDA0003741054170000057
式中，tr(.)表示求迹运算。
采用Sage-husa自适应滤波器实时估计和修正量测噪声的统计特性，k时刻的量测噪声协方差矩阵
Figure GDA0003741054170000058
表示为：
Figure GDA0003741054170000059
式中，
Figure GDA00037410541700000510
表示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 GDA00037410541700000511
表示为：
Figure GDA00037410541700000512
利用估计出的等效旋转矢量φ不断修正载体系变换矩阵：
Figure GDA00037410541700000513
所述的一种捷联惯导系统最优动基座初始对准方法，步骤(6)的具体方法是：
根据姿态矩阵的链乘规则，利用步骤(5)得到的修正后的载体系变换矩阵
Figure GDA00037410541700000514
和步骤(1)得到的导航系变换矩阵
Figure GDA00037410541700000515
以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵
Figure GDA0003741054170000061
计算对准终止时刻的载体系到导航系的坐标变换矩阵
Figure GDA0003741054170000062
Figure GDA0003741054170000063
本发明的有益效果是：本发明通过滑动窗口缩短观测矢量的积分区间，减少了惯性器件误差的积累。并且，借助全球卫星导航系统提供的信息，建立状态模型和量测模型，估计陀螺仪的常值漂移和实际载体坐标系到计算载体坐标系的等效旋转矢量，并反馈到多矢量构造过程中。采用二型模糊自适应卡尔曼滤波算法解决了量测噪声不准确的问题，提高对准算法的稳定性，最终得到更精确的初始对准结果。
附图说明
图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 GDA0003741054170000071
其中导航系变换矩阵微分方程为：
Figure GDA0003741054170000072
式中，n表示导航坐标系，n(0)表示0时刻的导航坐标系，n(t)表示t时刻的导航坐标系，i表示地心惯性坐标系，
Figure GDA0003741054170000073
表示0时刻导航坐标系到t时刻导航坐标系的坐标变换矩阵，矩阵
Figure GDA0003741054170000074
是矩阵
Figure GDA0003741054170000075
的转置，
Figure GDA0003741054170000076
表示从n系到i系的旋转角速度在n系的投影，
Figure GDA0003741054170000077
表示由
Figure GDA0003741054170000078
矢量构成的反对称矩阵，
Figure GDA0003741054170000079
表示矩阵
Figure GDA00037410541700000710
的微分。
求解载体系变换矩阵微分方程，根据载体系变换矩阵微分方程得到载体系变换矩阵
Figure GDA00037410541700000711
其中载体系变换矩阵微分方程为：
Figure GDA00037410541700000712
式中，b表示载体坐标系，b(0)表示0时刻的载体坐标系，b(t)表示t时刻的载体坐标系，
Figure GDA00037410541700000713
表示0时刻载体坐标系到t时刻载体坐标系的坐标变换矩阵，矩阵
Figure GDA00037410541700000714
是矩阵
Figure GDA00037410541700000715
的转置，
Figure GDA00037410541700000716
表示陀螺仪三轴输出，
Figure GDA00037410541700000717
表示由
Figure GDA00037410541700000718
矢量构成的反对称矩阵，
Figure GDA00037410541700000719
表示矩阵
Figure GDA00037410541700000720
的微分。
根据导航系变换矩阵和载体系变换矩阵微分方程得到导航系变换矩阵
Figure GDA00037410541700000721
和载体系变换矩阵
Figure GDA00037410541700000722
步骤二、根据姿态矩阵的链乘规则，比力方程写为：
Figure GDA00037410541700000723
式中，Vn为导航坐标系中速度，
Figure GDA00037410541700000724
表示Vn的微分，fb表示加速度计输出，gn为重力加速度，e表示地球坐标系，
Figure GDA00037410541700000725
表示地球自转角速度在导航坐标系中的投影，
Figure GDA00037410541700000726
表示载体线运动引起导航坐标系相对地球坐标系的角速度在导航坐标系的投影。
对式(3)进行移项，两边同乘导航系变换矩阵
Figure GDA00037410541700000727
并且积分，积分区间为[0,tk]，得到：
Figure GDA00037410541700000728
式中，
Figure GDA00037410541700000729
表示对准起始时刻的载体系到导航系的坐标变换矩阵，对式(4)进行变形得到观测矢量α(0,tk)和观测矢量β(0,tk)：
Figure GDA0003741054170000081
Figure GDA0003741054170000082
Figure GDA0003741054170000083
Figure GDA0003741054170000084
式中，α(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 GDA0003741054170000085
Figure GDA0003741054170000086
式中，T表示姿态更新周期，取为5ms，tk+m＝(k+m)T，m取为10，表示tk+m时刻，I表示单位矩阵。
步骤三、利用步骤(2)的新的观测矢量构造目标函数
Figure GDA0003741054170000087
Figure GDA0003741054170000088
式中，wi为权重系数，一般取为1，目标函数反映同一矢量在两个坐标系中测量值的不一致误差，min表示最小值。对目标函数变形，将求目标函数最小值问题转化为求解矩阵
Figure GDA0003741054170000089
奇异值问题，α′i和β′i表示第i个新的观测矢量α'和新的观测矢量β'，对矩阵A进行奇异值分解得到A＝UDVT，U和V都是酉矩阵，D表示矩阵A的奇异值构成的对角矩阵，得到对准起始时刻的最优姿态矩阵：
Figure GDA00037410541700000810
步骤四、选择状态量
Figure GDA00037410541700000811
其中，真实载体坐标系b系到计算载体坐标系
Figure GDA0003741054170000091
系的等效旋转矢量为φ＝[φx,φy,φz]T,φx,φy,φz分别表示x轴，y轴，z轴的等效旋转矢量
Figure GDA0003741054170000092
表示陀螺仪的常值漂移，
Figure GDA0003741054170000093
分别表示x轴，y轴，z轴的陀螺仪常值漂移；
建立全球卫星导航系统辅助捷联惯性导航系统对准的状态方程：
Figure GDA0003741054170000094
Figure GDA0003741054170000095
式中，
Figure GDA0003741054170000096
表示陀螺仪的实际输出，
Figure GDA0003741054170000097
表示φ的微分，
Figure GDA0003741054170000098
表示εb的微分。
建立全球卫星导航系统辅助捷联惯性导航系统对准的量测方程：
Zk＝Hkφ+Vk (15)
Figure GDA0003741054170000099
Figure GDA00037410541700000910
式中，Zk表示量测量，Hk表示量测矩阵，Vk表示量测噪声，
Figure GDA00037410541700000911
表示t时刻的计算载体坐标系，
Figure GDA00037410541700000912
表示计算载体坐标系变换矩阵，矩阵
Figure GDA00037410541700000913
是矩阵
Figure GDA00037410541700000914
的转置。
步骤五、理想情况下，新息向量近似于零均值白噪声序列，通过新息向量可以监测滤波过程中的异常情况。观察新息序列可以通过将其实际协方差矩阵
Figure GDA00037410541700000915
与理论协方差矩阵Sk的对比完成。
新息向量实际协方差矩阵
Figure GDA00037410541700000916
与理论协方差矩阵Sk分别表示为：
Figure GDA00037410541700000917
Figure GDA00037410541700000918
式中，N表示窗口大小，ei表示i时刻的新息向量，
Figure GDA00037410541700000919
表示ei的转置，Pk,k-1表示状态一步预测均方误差阵，
Figure GDA00037410541700000920
表示k时刻的量测噪声协方差矩阵，新息向量实际协方差与理论协方差的比值定义为Mk：
Figure GDA00037410541700000921
式中，tr(.)表示求迹运算。
采用Sage-husa自适应滤波器实时估计和修正量测噪声的统计特性，k时刻的量测噪声协方差矩阵
Figure GDA00037410541700000922
表示为：
Figure GDA0003741054170000101
式中，
Figure GDA0003741054170000102
表示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 GDA0003741054170000103
表示为：
Figure GDA0003741054170000104
利用估计出的等效旋转矢量φ不断修正载体系变换矩阵：
Figure GDA0003741054170000105
步骤六、根据姿态矩阵的链乘规则，利用步骤(5)得到的修正后的载体系变换矩阵
Figure GDA0003741054170000106
和步骤(1)得到的导航系变换矩阵
Figure GDA0003741054170000107
以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵
Figure GDA0003741054170000108
计算对准终止时刻的载体系到导航系的坐标变换矩阵
Figure GDA0003741054170000109
Figure GDA00037410541700001010
仿真实验：
(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 (7)

1.一种捷联惯导系统最优动基座初始对准方法，其特征在于，该方法包括如下步骤：
(1)利用全球卫星导航系统和捷联惯性导航系统的输出信息计算导航系变换矩阵和载体系变换矩阵；
(2)采用滑动窗口构建新的观测矢量α'和新的观测矢量β'，并对新的观测矢量α'和新的观测矢量β'进行离散化处理；
(3)利用奇异值分解算法对多矢量定姿问题进行求解，求得对准起始时刻的载体系到导航系的坐标变换矩阵；
(4)利用全球卫星导航系统和捷联惯性导航系统的输出信息，以陀螺仪的常值漂移和等效旋转矢量作为状态变量，构建状态模型和量测模型；
(5)采用二型模糊自适应卡尔曼滤波算法估计等效旋转矢量和陀螺仪常值漂移，通过估计结果对步骤(1)得到的载体系变换矩阵进行修正；
(6)根据姿态矩阵的链乘规则，利用步骤(5)修正后的载体系变换矩阵和步骤(1)得到的导航系变换矩阵以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵计算对准终止时刻的载体系到导航系的坐标变换矩阵；
步骤(1)中所述计算导航系变换矩阵的具体方法是求解导航系变换矩阵微分方程，根据导航系变换矩阵微分方程得到导航系变换矩阵
Figure FDA0003640708430000011
其中导航系变换矩阵微分方程为：
Figure FDA0003640708430000012
式中，n表示导航坐标系，n(0)表示0时刻的导航坐标系，n(t)表示t时刻的导航坐标系，i表示地心惯性坐标系，
Figure FDA0003640708430000013
表示0时刻导航坐标系到t时刻导航坐标系的坐标变换矩阵，矩阵
Figure FDA0003640708430000014
是矩阵
Figure FDA0003640708430000015
的转置，
Figure FDA0003640708430000016
表示从n系到i系的旋转角速度在n系的投影，
Figure FDA0003640708430000017
表示由
Figure FDA0003640708430000018
矢量构成的反对称矩阵，
Figure FDA0003640708430000019
表示矩阵
Figure FDA00036407084300000110
的微分；
步骤(1)中所述计算载体系变换矩阵的具体方法是求解载体系变换矩阵微分方程，根据载体系变换矩阵微分方程得到载体系变换矩阵
Figure FDA00036407084300000111
其中载体系变换矩阵微分方程为：
Figure FDA00036407084300000112
式中，b表示载体坐标系，b(0)表示0时刻的载体坐标系，b(t)表示t时刻的载体坐标系，
Figure FDA00036407084300000113
表示0时刻载体坐标系到t时刻载体坐标系的坐标变换矩阵，矩阵
Figure FDA00036407084300000114
是矩阵
Figure FDA00036407084300000115
的转置，
Figure FDA00036407084300000116
表示陀螺仪三轴输出，
Figure FDA00036407084300000117
表示矩阵
Figure FDA00036407084300000118
的微分，
Figure FDA00036407084300000119
表示由
Figure FDA00036407084300000120
矢量构成的反对称矩阵；
步骤(2)的具体方法是：
根据姿态矩阵的链乘规则，比力方程写为：
Figure FDA0003640708430000021
式中，Vn为导航坐标系中速度，
Figure FDA0003640708430000022
表示Vn的微分，fb表示加速度计输出，gn为重力加速度，e表示地球坐标系，
Figure FDA0003640708430000023
表示地球自转角速度在导航坐标系中的投影，
Figure FDA0003640708430000024
表示载体线运动引起导航坐标系相对地球坐标系的角速度在导航坐标系的投影；
对式(3)进行移项，两边同乘导航系变换矩阵
Figure FDA0003640708430000025
并且积分，积分区间为[0,tk]，得到：
Figure FDA0003640708430000026
式中，
Figure FDA0003640708430000027
表示对准起始时刻的载体系到导航系的坐标变换矩阵，对式(4)进行变形得到观测矢量α(0,tk)和观测矢量β(0,tk)：
Figure FDA0003640708430000028
Figure FDA0003640708430000029
Figure FDA00036407084300000210
Figure FDA00036407084300000211
式中，α(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 FDA00036407084300000212
Figure FDA00036407084300000213
式中，T表示姿态更新周期，tk+m＝(k+m)T，表示tk+m时刻，I表示单位矩阵。
2.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(3)的具体方法是：
利用步骤(2)的新的观测矢量构造目标函数
Figure FDA0003640708430000031
Figure FDA0003640708430000032
式中，wi为权重系数，min表示最小值；
对目标函数变形，将求目标函数最小值问题转化为求解矩阵
Figure FDA0003640708430000033
奇异值问题，α′i和β′i表示第i个新的观测矢量α'和新的观测矢量β'，对矩阵A进行奇异值分解得到A＝UDVT，U和V都是酉矩阵，D表示矩阵A的奇异值构成的对角矩阵，得到对准起始时刻的最优姿态矩阵：
Figure FDA0003640708430000034
3.根据权利要求2所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，权重系数wi取为1。
4.根据权利要求1所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(4)的具体方法是：
选择状态量
Figure FDA0003640708430000035
其中，载体坐标系b系到计算载体坐标系
Figure FDA0003640708430000036
系的等效旋转矢量为φ＝[φx,φy,φz]T,φx,φy,φz分别表示x轴，y轴，z轴的等效旋转矢量，
Figure FDA0003640708430000037
表示陀螺仪的常值漂移，
Figure FDA0003640708430000038
分别表示x轴，y轴，z轴的陀螺仪常值漂移；
建立全球卫星导航系统辅助捷联惯性导航系统对准的状态方程：
Figure FDA0003640708430000039
Figure FDA00036407084300000310
式中，
Figure FDA00036407084300000311
表示陀螺仪的实际输出，
Figure FDA00036407084300000312
表示φ的微分，
Figure FDA00036407084300000313
表示εb的微分；
建立全球卫星导航系统辅助捷联惯性导航系统对准的量测方程：
Zk＝Hkφ+Vk (15)
Figure FDA00036407084300000314
Figure FDA00036407084300000315
式中，Zk表示量测量，Hk表示量测矩阵，Vk表示量测噪声，
Figure FDA00036407084300000316
表示t时刻的计算载体坐标系，
Figure FDA0003640708430000041
表示计算载体坐标系变换矩阵，矩阵
Figure FDA0003640708430000042
是矩阵
Figure FDA0003640708430000043
的转置。
5.根据权利要求4所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(5)的具体方法是：
新息向量实际协方差矩阵
Figure FDA0003640708430000044
与理论协方差矩阵Sk分别表示为：
Figure FDA0003640708430000045
Figure FDA0003640708430000046
式中，N表示窗口大小，Pk,k-1表示状态一步预测均方误差阵，ei表示第i时刻的新息向量，
Figure FDA0003640708430000047
表示ei的转置，
Figure FDA0003640708430000048
表示k时刻的量测噪声协方差矩阵，新息向量实际协方差与理论协方差的比值定义为Mk：
Figure FDA0003640708430000049
式中，tr(.)表示求迹运算；
采用Sage-husa自适应滤波器实时估计和修正量测噪声的统计特性，k时刻的量测噪声协方差矩阵
Figure FDA00036407084300000410
表示为：
Figure FDA00036407084300000411
式中，
Figure FDA00036407084300000412
表示k-1时刻的量测噪声协方差矩阵，Kk表示滤波增益，Pk表示状态估计的均方误差阵，ek表示k时刻的新息向量，dk表示当前信息的权重，dk＝(1-b)/(1-bk+1)，0<b<1，b为遗忘因子；
将Mk作为二型模糊逻辑的输入，自适应调节因子a为二型模糊逻辑的输出，通过a来调节量测噪声协方差矩阵，k+1时刻量测噪声协方差矩阵
Figure FDA00036407084300000413
表示为：
Figure FDA00036407084300000414
利用估计出的等效旋转矢量φ不断修正载体系变换矩阵：
Figure FDA00036407084300000415
6.根据权利要求5所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，所述调节量测噪声协方差矩阵的具体方法是，当Mk>1表明量测噪声偏大，应使a>1，使量测噪声协方差矩阵增大；当Mk<1表明量测噪声偏小，应使a<1，使量测噪声协方差矩阵减小；输入变量Mk的模糊集定义为{小，正常，大}，记为{NB，ZE，PB}；输出变量a的模糊集定义为{减少、保持、增大}，记为{DB、MA、IB}。
7.根据权利要求6所述一种捷联惯导系统最优动基座初始对准方法，其特征在于，步骤(6)的具体方法是：
根据姿态矩阵的链乘规则，利用步骤(5)得到的修正后的载体系变换矩阵
Figure FDA0003640708430000051
和步骤(1)得到的导航系变换矩阵
Figure FDA0003640708430000052
以及步骤(3)得到的对准起始时刻的载体系到导航系的坐标变换矩阵
Figure FDA0003640708430000053
计算对准终止时刻的载体系到导航系的坐标变换矩阵
Figure FDA0003640708430000054
Figure FDA0003640708430000055
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 CN114111843A (zh) 2022-03-01
CN114111843B true 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)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116858287A (zh) * 2023-07-06 2023-10-10 哈尔滨工程大学 一种基于地球坐标系的极区惯导初始对准方法
CN118111427A (zh) * 2024-04-30 2024-05-31 西安现代控制技术研究所 一种基于姿态预积分的弹载捷联惯导系统姿态保持方法

Family Cites Families (6)

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

Also Published As

Publication number Publication date
CN114111843A (zh) 2022-03-01

Similar Documents

Publication Publication Date Title
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN109459019B (zh) 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法
CN114111843B (zh) 一种捷联惯导系统最优动基座初始对准方法
CN109974714B (zh) 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN110440830B (zh) 动基座下车载捷联惯导系统自对准方法
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
Nourmohammadi et al. Design and experimental evaluation of indirect centralized and direct decentralized integration scheme for low-cost INS/GNSS system
Xu et al. A dynamic vector-formed information sharing algorithm based on two-state chi square detection in an adaptive federated filter
CN114877915B (zh) 一种激光陀螺惯性测量组件g敏感性误差标定装置及方法
WO2022222938A1 (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN113916226B (zh) 一种基于最小方差的组合导航系统抗扰滤波方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN111707292B (zh) 一种自适应滤波的快速传递对准方法
CN100588907C (zh) 船用光纤陀螺捷联系统初始姿态确定方法
CN110471293B (zh) 一种估计时变角速度的z轴陀螺仪滑模控制方法
CN110672127B (zh) 阵列式mems磁传感器实时标定方法
CN116772837A (zh) 基于交互式多模型的gnss/sins组合导航方法
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN110736459A (zh) 惯性量匹配对准的角形变测量误差评估方法
CN114061574B (zh) 一种基于位置不变约束及零速校正的采煤机定姿定向方法
CN114018262B (zh) 一种改进的衍生容积卡尔曼滤波组合导航方法
CN112304309A (zh) 一种基于心动阵列的高超飞行器组合导航信息解算方法
CN111290279B (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
CB03 Change of inventor or designer information
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

GR01 Patent grant
GR01 Patent grant