CN108827310B - 一种船用星敏感器辅助陀螺仪在线标定方法 - Google Patents

一种船用星敏感器辅助陀螺仪在线标定方法 Download PDF

Info

Publication number
CN108827310B
CN108827310B CN201810764305.8A CN201810764305A CN108827310B CN 108827310 B CN108827310 B CN 108827310B CN 201810764305 A CN201810764305 A CN 201810764305A CN 108827310 B CN108827310 B CN 108827310B
Authority
CN
China
Prior art keywords
carrier
quaternion
information
star sensor
gyroscope
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
CN201810764305.8A
Other languages
English (en)
Other versions
CN108827310A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201810764305.8A priority Critical patent/CN108827310B/zh
Publication of CN108827310A publication Critical patent/CN108827310A/zh
Application granted granted Critical
Publication of CN108827310B publication Critical patent/CN108827310B/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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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/02—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Astronomy & Astrophysics (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

本发明公开了一种船用星敏感器辅助陀螺仪在线标定方法，涉及惯性导航中惯性传感器误差标定技术领域。本发明包括以下步骤：(1)初始化星敏感器/惯性组合导航***；(2)采集惯性器件和星敏感器输出数据；(3)对惯性器件进行解算，得到姿态四元数和导航信息；(4)得到载体姿态四元数的真实值；(5)利用模糊逻辑控制方法确定简化Sage‑Husa自适应滤波方法中遗忘因子，再对载体姿态四元数信息进行滤波计算；(6)将陀螺输出误差对陀螺输出角速度信息进行补偿，并进行导航解算；(7)存储并输出导航信息。本发明增强了船用星敏感器辅助陀螺仪在线标定在复杂环境中的适用性，减小了测量噪声误差估计不充分的问题；有效提高了星敏感器的定姿精度。

Description

一种船用星敏感器辅助陀螺仪在线标定方法
技术领域
本发明涉及惯性导航中惯性传感器误差标定技术领域，具体涉及一种基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法。
背景技术
陀螺仪是惯性导航***中姿态测量的核心传感器，可以直接敏感载体系相对惯性系的角速度，但其输出误差是导致惯性导航***的导航误差主要原因之一，若可以在载体航行过程中准确的估算出陀螺输出误差，此部分导航误差就可以得到较好的抑制，因此对陀螺仪进行在线标定是重要的。传统的陀螺仪在线标定是利用GPS信号作为外部信息源估计陀螺误差，但GPS信号易受环境的干扰和欺骗而失灵。星敏感器是一种通过观测星体获得载体相对惯性系姿态的高精度传感器，其精度可达到角秒级别，具有动态稳定性好，抗干扰能力强，其测量误差不随时间累积等优点，使得在航海领域得到广泛的应用。因此，以星敏感器输出信息为基准，对陀螺仪进行在线标定，进而提高惯性导航***姿态精度，使其长期、连续输出高精度姿态信息。然而，舰船航行过程中外界环境发生变化，进而引起星敏感器测量噪声统计特性发生变化，造成滤波器工作不稳定，降低滤波精度。
为了提高滤波精度，常采用简化的Sage-Husa滤波方法对组合导航***进行滤波，该方法利用测量信息不断修正预测值，对测量噪声统计参数进行估计与修正，有效的解决了实际***中噪声统计特性发生变化的问题，提高了***的环境适应性。虽然简化的Sage-Husa滤波方法能够较好的抑制测量噪声统计特性发生变化时的滤波精度下降的问题，但是当***环境变化情况复杂时，由于量测噪声统计特性参数修正时间短，进而导致噪声统计特性未得到较好的跟踪，造成滤波发散的现象。
《***工程与电子技术》2012年第34卷第八期由高伟等人撰写的《CCD星敏感器辅助光纤陀螺在线标定技术》一文中，利用星敏感器输出高精度的姿态信息，将模型预测滤波与EKF结合可在线估计出陀螺输出误差，提高导航精度；《Sensors》2017年17卷第一期由杨延强等人撰写的《Local Observability Analysis of Star Sensor InstallationErrors in a SINS/CNS Integration System for Near-Earth Flight Vehicles》一文中，基于SINS与星敏感器之间的安装误差一直是制约SINS/CNS实际精度的主要因素之一，利用卡尔曼滤波对星敏感器安装误差进行精确估计，提高导航***精度；公开号CN101943585A的中国发明专利在2011年1月12日公开的《一种基于CCD星敏感器的标定方法》，该方法在短时间内可以达到稳定的标定结果，不需要进行任何机动措施，便可以估计出陀螺常值漂移和加速度计零偏。《宇航学报》2011年32卷第五期由杨波等人撰写的《长航时环境下高精度组合导航方法研究与仿真》一文中针对长航时环境下可能面临量测噪声统计特性的不确定问题，采用简化的Sage-Husa自适应滤波算法进行组合导航滤波设计，提高***的精度及鲁棒性；《计算机***应用》2015年第24卷第八期由孙会敏等人撰写的《有色噪声下的平方根UKF在天文自主导航中的应用》一文中针对传统的平方根UKF不能很好地解决测量噪声为有色噪声情况下的非线性滤波问题，提出了一种有色噪声情况下的平方根UKF方法，能够很好地解决估计精度低的问题。以上文献都在利用星敏感器辅助陀螺仪进行在线标定，以及抑制星敏感器测量噪声发生变化时的估计误差，并没有提及环境变化复杂时对陀螺仪在线标定精度及适应性的影响。
发明内容
本发明的目的在于解决：针对舰船长航时，自主导航过程中，外界环境的改变造成星敏感器测量噪声统计特性未知，引起标准卡尔曼滤波发散而无法进行陀螺仪在线标定的问题，提出一种基于遗传因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法。
为实现上述目的，本发明公开一种基于遗传因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法，包括以下步骤：
(1)上电，初始化星敏感器/惯性组合导航***；
(2)***实时采集惯性器件和星敏感器输出数据，包括陀螺仪输出的三轴角速度
Figure GDA0003016208480000021
加速度计输出的比力信息fb和星敏感器输出相对惯性空间的姿态信息
Figure GDA0003016208480000022
(3)利用(2)中得到的陀螺仪输出信息进行姿态解算，得到姿态四元数的计算值
Figure GDA0003016208480000023
再结合加速度计输出数据进行导航解算，得到导航信息；
(4)利用(2)中得到的星敏感器输出数据进行惯性系下姿态解算，得到载体姿态四元数的真实值q；
(5)利用模糊逻辑控制方法确定简化Sage-Husa自适应滤波方法中遗忘因子b；构造简化Sage-Husa自适应滤波器，以(3)中解算得到的四元数计算值
Figure GDA0003016208480000024
和(4)中解算得到的四元数真实值q之间的误差四元数的矢量部分δe作为观测量；误差四元数的矢量部分δe，陀螺刻度因数δk，陀螺漂移ε为状态量，进行滤波计算，估计出陀螺常值漂移、刻度因数误差；
(6)将(5)中ε和δk补偿陀螺仪输出信息
Figure GDA0003016208480000025
进行导航解算得到载体补偿后的导航信息；
(7)将(6)中得到的载体导航信息存储并输出。
步骤(1)中所述导航初始化，需初始化***以下信息：
导航信息初始化：舰船位置信息
Figure GDA0003016208480000026
λs0，单位为弧度，用rad表示，速度信息vx0、vy0，单位为米/秒，用m/s表示，以及姿态角信息φx0、φy0、φz0，单位为弧度，用rad表示，初始转换矩阵
Figure GDA0003016208480000031
初始四元数q0；
初始化常值参量：当地经纬度λ、
Figure GDA0003016208480000032
单位为弧度，用rad表示，加速度计白噪声误差σa，陀螺仪白噪声误差σw，采样时间T，单位为秒，用s表示；
简化Sage-Husa自适应滤波器参数初值：状态变量初值X0＝[δe0 δk0 ε0]T，协方差阵P0，***噪声方差阵Q0，量测噪声方差阵R0；
其中，δe0表示误差四元数矢量部分的初始值；δk0表示陀螺仪刻度因数的初始值；ε0表示陀螺仪漂移的初始值；
初始转换矩阵计算如下：
Figure GDA0003016208480000033
其中，b表示载体坐标系，n表示导航坐标系，Cb n表示b系到n系的转移矩阵；
初始化四元数q0计算如下：
令
Figure GDA0003016208480000034
则q0＝a，
Figure GDA0003016208480000035
q0＝[q0 q1 q2 q3]T；
其中，cij为矩阵
Figure GDA0003016208480000036
中第i行、第j列矩阵元素，q表示四元数的实数部分，q1、q2、q3为四元数的虚数部分。
步骤(2)中所述***实时采集惯性器件和星敏感器的输出数据，包括陀螺仪输出的三轴角速度
Figure GDA0003016208480000037
和加速度计输出的比力信息
Figure GDA0003016208480000038
星敏感器输出相对惯性空间的姿态信息
Figure GDA0003016208480000039
其中
Figure GDA00030162084800000310
为i系相对b系旋转角速度在b系投影，
Figure GDA00030162084800000311
分别为陀螺仪测量角速度在载体坐标系oxb轴、oyb轴、ozb轴上的分量，单位均为弧度/秒，用rad/s表示；
Figure GDA00030162084800000312
分别为加速度计测量加速度在载体坐标系oxb轴、oyb轴、ozb轴上的分量，单位均为米/秒，用m/s表示；i表示惯性坐标系。
步骤(3)中所述利用陀螺仪采集到的角速度
Figure GDA00030162084800000313
进行姿态解算，得到载体姿态四元数的计算值
Figure GDA00030162084800000314
结合采集到的加速度fb进行导航解算得到导航信息，具体过程如下：
角速度更新：
Figure GDA00030162084800000315
其中，e表示地球坐标系，p表示平台坐标系；
Figure GDA00030162084800000316
表示b系到p系的转换矩阵，且
Figure GDA0003016208480000041
Φx、Φy、Φz分别表示平台坐标系与导航坐标系三轴误差角；
Figure GDA0003016208480000042
表示m系相对g系旋转角速度在l系投影，m＝i,e,g＝b,p,e,l＝b,p；(·)T表示矩阵的转置；
载体姿态四元数计算值的更新：
设任意时刻载体坐标系相对平台坐标系的转动四元数为：
Figure GDA0003016208480000043
其中，
Figure GDA0003016208480000044
表示四元数；q、e1、e2、e3表示四元数的四个实数；ib、jb、kb分别表示载体坐标系oxb轴、oyb轴、ozb轴上的单位方向向量；
四元数
Figure GDA0003016208480000045
的即时修正：
Figure GDA0003016208480000046
其中，
Figure GDA0003016208480000047
分别表示平台坐标系相对载体坐标系的运动角速度在载体坐标系oxb轴、oyb轴、ozb轴上的分量；
Figure GDA0003016208480000048
分别表示q、e1、e2、e3的变化率；
根据k时刻载体坐标系相对平台坐标系的转动四元数q(k)、e1(k)、e2(k)、e3(k)，求取k时刻转动四元数的变化率为：
Figure GDA0003016208480000049
在k+1时刻载体的转动四元数具体为：
Figure GDA00030162084800000410
至此，根据上式得到载体姿态四元数的计算值
Figure GDA00030162084800000411
载体姿态、速度及位置的更新：
利用得到的q(k+1)、e1(k+1)、e2(k+1)、e3(k+1)，更新捷联矩阵
Figure GDA00030162084800000412
Figure GDA00030162084800000413
其中，上式中的q、ei，i＝1,2,3为k+1时刻载体的转动四元数中q(k+1)、ei(k+1)，i＝1,2,3，即上式中省略了(k+1)；
更新姿态信息：
Figure GDA0003016208480000051
利用转换关系式
Figure GDA0003016208480000052
将加速度计沿载体系测得的加速度信息投影转换到导航坐标系，利用下列微分方程求解载体运动速度：
Figure GDA0003016208480000053
其中，
Figure GDA0003016208480000054
分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的投影；
Figure GDA0003016208480000055
表示
Figure GDA0003016208480000056
的变化率；
Figure GDA0003016208480000057
分别表示加速度计测得的加速度信息在导航系oxn轴、oyn轴、ozn轴上的投影；g表示当地重力加速度；
根据k时刻的载体的三轴速度vx(k)、vy(k)与vz(k)，求取k时刻的载体速度变化率
Figure GDA0003016208480000058
进而得到k+1时刻载体的速度和位置：
Figure GDA0003016208480000059
其中，R表示地球半径；当k＝1时，vx(1)、vy(1)、vz(1)为步骤一中初始化***时获得的载体初始速度，
Figure GDA00030162084800000510
λ(1)为步骤一中初始化***时获得的载体初始位置；
至此，得到载体的姿态角、速度和位置。
步骤(4)中所述利用(2)中得到的星敏感器输出数据进行惯性系下姿态解算，得到载体姿态四元数的真实值q，即
Figure GDA00030162084800000511
其中，cib(i,j)表示矩阵
Figure GDA0003016208480000061
中第i行、第j列矩阵元素；qib表示四元数真实值的实数部分，eib1、eib2、eib3表示四元数真实值的虚数部分；
至此，根据上式得到载体姿态四元数的真实值q。
步骤(5)中所述的利用模糊逻辑控制方法确定k时刻的遗忘因子b，构造简化Sage-Husa自适应滤波器，具体包括以下步骤：
1)根据(3)中载体姿态四元数的计算值与(4)中载体姿态四元数的真实值解算得到误差四元数，过程如下：
Figure GDA0003016208480000062
其中，δq＝[δq δe]T表示误差四元数，δq表示误差四元数的实数部分，δe表示误差四元数的虚数部分；(·)-1表示矩阵的逆；
2)利用1)中误差四元数的第3个分量δe2的绝对值对星敏感器测量噪声统计特性进行分析，离散的将星敏感器测量噪声状态定义为若干级，形式为：
U＝{NN LN HN}
确定δe2绝对值的论域集并划分论域，基本论域为[a c]，通过量化因子转化的模糊论域为[a1 c1]，划分后的论域为：
Figure GDA0003016208480000063
其中，NN表示星敏感器无测量噪声，LN表示星敏感器测量噪声为低噪声，HN表示星敏感器测量噪声为高噪声；
3)根据δe2绝对值的实际情况建立模糊论域各元素对模糊语言的隶属度大小，即相应的隶属函数，表达形式如下：
三角形函数为：
Figure GDA0003016208480000064
梯形函数为：
Figure GDA0003016208480000071
其中，参数f,v,g,d,m,p,y,e,n分别表示模糊集合中的元素；
4)将δe2的绝对值作为模糊逻辑控制的输入信息，结合3)中隶属函数，经过模糊逻辑运算，进而推测出k时刻星敏感器测量噪声状态。其中模糊逻辑规则的具体表达式为：
Figure GDA0003016208480000072
5)依据4)中推测出的k时刻星敏感器测量噪声状态，采用重心法进行反模糊化得到b的精确值，进而选择k时刻相应的遗忘因子b，选择方法具体如下：
Figure GDA0003016208480000073
其中，b的范围为(0 1)，重心法计算式为
Figure GDA0003016208480000074
vk表示模糊集合元素，μv(vk)表示元素vk的隶属度，v0表示精确值；
6)构造简化Sage-Husa自适应滤波器，滤波状态量为X＝[δe δk ε]T,观测量为Z＝δe，滤波过程具体如下：
利用下列微方程建立***方程：
Figure GDA0003016208480000075
其中，δe为误差四元数，且δe＝[δe1 δe2 δe3]；δk为陀螺刻度因数，且δk＝[δkx δkyδkz]；ε为陀螺漂移，且ε＝[εx εy εz]；
Figure GDA0003016208480000076
分别为δe、δk、ε的变化率；δK为刻度因数导致的陀螺仪测量误差，且
Figure GDA0003016208480000077
ng为传感器测量噪声；Ο3×1为三行一列的零向量；
基于上述误差模型，采用简化Sage-Husa自适应滤波器，对陀螺误差进行在线标定，具体算法如下：
dk＝(1-b)/(1-bk+1)
Xk/k-1＝Fk,k-1Xk-1
Figure GDA0003016208480000081
其中，dk为k时刻的调节因子，Fk,k-1为k-1时刻到k时刻的状态转移矩阵，Gk为陀螺角速度信息构成的对角阵，Ι为单位阵，Ο3×3为零矩阵；
根据测量信息计算k时刻的新息，再通过指数加权法不断估计k时刻的测量噪声R阵：
vk＝Zk-HXk/k-1
Figure GDA0003016208480000082
计算滤波增益、状态估值及更新Pk：
Kk＝Pk/k-1HT(HPk/k-1HT+Rk)-1
Xk＝Xkk-1+Kkvk
Pk＝(Ι-KkH)Pk/k-1
其中，Xk为k时刻的状态量，当k＝1时，状态量X＝[δe(1) δk(1) ε(1)]为初始化***时获得的初始状态量X0。
步骤(6)中所述利用(5)中估计出的ε和δk补偿陀螺仪输出信息
Figure GDA0003016208480000083
进行导航解算得到载体补偿后的导航信息：
陀螺仪输出角速度信息补偿：
Figure GDA0003016208480000084
其中，
Figure GDA0003016208480000085
为补偿后的角速度信息，且
Figure GDA0003016208480000086
分别为在载体坐标系oxb轴、oyb轴、ozb轴上的分量；
将
Figure GDA0003016208480000087
进行角速度更新，进而得到补偿后的姿态、速度及位置信息。
本发明的有益效果在于：
本发明针对因星敏感器测量噪声统计特性发生变化而影响滤波精度的问题，利用模糊逻辑控制方法具有表达界限不清晰的定性知识与经验、推理解决常规方法难于解决的规则型模糊信息问题的优点，对自适应滤波的遗忘因子进行选择，实现陀螺误差的在线估计，既可以保证星敏感器信息的有效利用，又可实时修正了陀螺仪误差，提高舰船导航***的精度。本发明方法增强了舰船在复杂环境中星敏感器辅助陀螺仪在线标定的适用性，减小了因测量噪声导致的误差估计不充分的问题；有效提高了星敏感器的定姿精度。
附图说明
图1为本发明方法的流程图；
图2为利用本发明进行的仿真结果图，依次为陀螺漂移估计值比较曲线、刻度因数误差估计值比较曲线、补偿前后定位路径对比曲线；
图3为模糊逻辑控制方法输入量的隶属函数。
具体实施方式
下面将结合附图和实施例对本发明作进一步的详细说明。
本发明是一种基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法，方法流程如图1所示，包括以下步骤：
步骤一：接通电源，对星敏感器/惯性组合导航***进行初始化。导航初始时刻，需初始化***：
(1)导航信息初始化：舰船位置信息
Figure GDA0003016208480000091
λs0，单位为弧度，用rad表示，速度信息vx0、vy0，单位为米/秒，用m/s表示，以及姿态角信息φx0、φy0、φz0，单位为弧度，用rad表示，初始转换矩阵
Figure GDA0003016208480000092
初始四元数q0；
(2)初始化常值参量：当地经纬度λ、
Figure GDA0003016208480000093
单位为弧度，用rad表示，加速度计白噪声误差σa，陀螺仪白噪声误差σw，采样时间T，单位为秒，用s表示；
(3)简化Sage-Husa自适应滤波器参数初值：状态变量初值X0＝[δe0 δk0 ε0]T，协方差阵P0，***噪声方差阵Q0，量测噪声方差阵R0；
其中，δe0表示误差四元数矢量部分的初始值；δk0表示陀螺仪刻度因数的初始值；ε0表示陀螺仪漂移的初始值。
初始转换矩阵计算如下：
Figure GDA0003016208480000094
其中，b表示载体坐标系，n表示导航坐标系，
Figure GDA0003016208480000095
表示b系到n系的转移矩阵。
初始化四元数q0计算如下：
令
Figure GDA0003016208480000096
则q0＝a，
Figure GDA0003016208480000097
q0＝[q0 q1 q2 q3]T。
其中，cij为矩阵
Figure GDA0003016208480000101
中第i行、第j列矩阵元素，q表示四元数的实数部分，q1、q2、q3为四元数的虚数部分。
标定过程中，利用该初始信息进行更新，得到陀螺仪的角速度信息。
步骤二：***实时采集惯性器件和星敏感器的输出数据，包括陀螺仪输出的三轴角速度
Figure GDA0003016208480000102
和加速度计输出的比力信息
Figure GDA0003016208480000103
星敏感器输出相对惯性空间的姿态信息
Figure GDA0003016208480000104
其中
Figure GDA0003016208480000105
为i系相对b系旋转角速度在b系投影，
Figure GDA0003016208480000106
分别为陀螺仪测量角速度在载体坐标系oxb轴、oyb轴、ozb轴上的分量，单位均为弧度/秒，用rad/s表示；
Figure GDA0003016208480000107
分别为加速度计测量加速度在载体坐标系oxb轴、oyb轴、ozb轴上的分量，单位均为米/秒，用m/s表示；i表示惯性坐标系。
步骤三：***根据步骤二中陀螺仪采集到的角速度
Figure GDA0003016208480000108
进行姿态解算，得到载体姿态四元数的计算值；结合采集到的加速度fb进行导航解算得到导航信息，具体过程如下：
(1)角速度更新：
Figure GDA0003016208480000109
其中，e表示地球坐标系，p表示平台坐标系；
Figure GDA00030162084800001010
表示b系到p系的转换矩阵，且
Figure GDA00030162084800001011
Φx、Φy、Φz分别表示平台坐标系与导航坐标系三轴误差角；
Figure GDA00030162084800001012
表示m系相对g系旋转角速度在l系投影；(·)T表示矩阵的转置。
(2)载体姿态四元数计算值的更新：
设任意时刻载体坐标系相对平台坐标系的转动四元数为：
Figure GDA00030162084800001013
其中，
Figure GDA00030162084800001014
表示四元数；q、e1、e2、e3表示四元数的四个实数；ib、jb、kb分别表示载体坐标系oxb轴、oyb轴、ozb轴上的单位方向向量。
四元数
Figure GDA00030162084800001015
的即时修正：
Figure GDA00030162084800001016
其中，
Figure GDA00030162084800001017
分别表示平台坐标系相对载体坐标系的运动角速度在载体坐标系oxb轴、oyb轴、ozb轴上的分量；
Figure GDA00030162084800001018
分别表示q、e1、e2、e3的变化率。
根据k时刻载体坐标系相对平台坐标系的转动四元数q(k)、e1(k)、e2(k)、e3(k)，求取k时刻转动四元数的变化率为：
Figure GDA0003016208480000111
在k+1时刻载体的转动四元数具体为：
Figure GDA0003016208480000112
至此，根据上式得到载体姿态四元数的计算值
Figure GDA0003016208480000113
(3)载体姿态、速度及位置的更新：
利用得到的q(k+1)、e1(k+1)、e2(k+1)、e3(k+1)，更新捷联矩阵
Figure GDA0003016208480000114
Figure GDA0003016208480000115
其中，上式中的q、ei，i＝1,2,3为k+1时刻载体的转动四元数中q(k+1)、ei(k+1)，i＝1,2,3，上式中省略了k+1。
更新姿态信息：
Figure GDA0003016208480000116
利用转换关系式
Figure GDA0003016208480000117
将加速度计沿载体系测得的加速度信息投影转换到导航坐标系，利用下列微分方程求解载体运动速度：
Figure GDA0003016208480000118
其中，
Figure GDA0003016208480000119
分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的投影；
Figure GDA00030162084800001110
表示
Figure GDA00030162084800001111
的变化率；
Figure GDA00030162084800001112
分别表示加速度计测得的加速度信息在导航系oxn轴、oyn轴、ozn轴上的投影；g表示当地重力加速度。
根据k时刻的载体的三轴速度vx(k)、vy(k)与vz(k)，求取k时刻的载体速度变化率
Figure GDA0003016208480000127
进而得到k+1时刻载体的速度和位置：
Figure GDA0003016208480000121
其中，R表示地球半径；当k＝1时，vx(1)、vy(1)、vz(1)为步骤一中初始化***时获得的载体初始速度，
Figure GDA0003016208480000122
λ(1)为步骤一中初始化***时获得的载体初始位置。
至此得到载体的姿态角、速度和位置。
步骤四：由步骤二中星敏感器输出相对于惯性空间的姿态矩阵
Figure GDA0003016208480000123
得到载体姿态四元数的真实值，即
Figure GDA0003016208480000124
其中，cib(i,j)表示矩阵
Figure GDA0003016208480000125
中第i行、第j列矩阵元素；qib表示四元数真实值的实数部分，eib1、eib2、eib3表示四元数真实值的虚数部分。
至此，根据上式得到载体姿态四元数的真实值q。
步骤五：利用模糊逻辑控制方法确定k时刻的简化Sage-Husa自适应滤波方法中遗忘因子b，构造简化Sage-Husa自适应滤波器，具体过程如下：
(1)根据步骤三中载体姿态四元数的计算值与步骤四中载体姿态四元数的真实值解算得到误差四元数，过程如下：
Figure GDA0003016208480000126
其中，δq＝[δq δe]T表示误差四元数，δq表示误差四元数的实数部分，δe表示误差四元数的虚数部分；(·)-1表示矩阵的逆。
(2)利用(1)中误差四元数的第3个分量δe2的绝对值对星敏感器测量噪声统计特性进行分析，并将δe2的绝对值作为模糊逻辑控制器的输入，离散的将星敏感器测量噪声状态定义为若干级，形式为：
U＝{NN LN HN}
确定δe2绝对值的论域集并划分论域，基本论域为[a c]，通过量化因子转化的模糊论域为[a1 c1]，划分后的论域为：
Figure GDA0003016208480000131
其中，NN表示星敏感器无测量噪声；LN表示星敏感器测量噪声为低噪声；HN表示星敏感器测量噪声为高噪声。
(3)根据δe2绝对值的实际情况建立模糊论域各元素对模糊语言的隶属度大小，即相应的隶属函数，表达形式如下：
三角形函数为：
Figure GDA0003016208480000132
梯形函数为：
Figure GDA0003016208480000133
其中，参数f,v,g,d,m,p,y,e,n分别表示模糊集合中的元素。
(4)将δe2的绝对值作为模糊逻辑控制的输入信息，结合(3)中隶属函数，经过模糊逻辑运算，进而推测出k时刻星敏感器测量噪声状态。其中模糊逻辑控制方法采用的是“If-then”规则来形成基于数据的规则，具体表达式为：
Figure GDA0003016208480000134
(5)依据(4)中推测出的k时刻星敏感器测量噪声状态，采用重心法进行反模糊化得到b的精确值，进而选择k时刻相应的遗忘因子b，选择方法具体如下：
Figure GDA0003016208480000141
其中，b的范围为(0 1)；重心法计算式为
Figure GDA0003016208480000142
vk表示模糊集合元素，μv(vk)表示元素vk的隶属度，v0表示精确值。
(6)构造简化Sage-Husa自适应滤波器，滤波状态量为X＝[δe δk ε]T,观测量为Z＝δe，滤波过程具体如下：
利用下列微方程建立***方程：
Figure GDA0003016208480000143
其中，δe为误差四元数，且δe＝[δe1 δe2 δe3]；δk为陀螺刻度因数，且δk＝[δkx δkyδkz]；ε为陀螺漂移，且ε＝[εx εy εz]；
Figure GDA0003016208480000144
分别为δe、δk、ε的变化率；δK为刻度因数导致的陀螺仪测量误差，且
Figure GDA0003016208480000145
ng为传感器测量噪声；Ο3×1为三行一列的零向量。
根据上述误差模型，利用简化Sage-Husa自适应滤波器，对陀螺误差进行在线标定，具体算法如下：
dk＝(1-b)/(1-bk+1)
Xk/k-1＝Fk,k-1Xk-1
Figure GDA0003016208480000146
其中，dk为k时刻的调节因子，Fk,k-1为k-1时刻到k时刻的状态转移矩阵，且
Figure GDA0003016208480000147
Figure GDA0003016208480000148
为陀螺仪角速度信息构成的反对称矩阵，且
Figure GDA0003016208480000151
Gk为陀螺角速度信息构成的对角阵，且
Figure GDA0003016208480000152
Ι为单位阵，Ο3×3为零矩阵。
根据测量信息计算k时刻的新息，再通过指数加权法不断估计k时刻的测量噪声R阵：
vk＝Zk-HXk/k-1
Figure GDA0003016208480000153
计算滤波增益、状态估值及更新Pk：
Kk＝Pk/k-1HT(HPk/k-1HT+Rk)-1
Xk＝Xkk-1+Kkvk
Pk＝(Ι-KkH)Pk/k-1
其中，Xk为k时刻的状态量，当k＝1时，状态量X＝[δe(1) δk(1) ε(1)]为步骤一中初始化***时获得的初始状态量X0。再对***进行估计的同时，估计量测噪声的协方差阵R,以提高滤波精度。
步骤六：利用步骤五中估计出的ε和δk补偿陀螺仪输出信息
Figure GDA0003016208480000154
进行导航解算得到载体补偿后的导航信息：
陀螺仪输出角速度信息补偿：
Figure GDA0003016208480000155
其中，
Figure GDA0003016208480000156
为补偿后的角速度信息，且
Figure GDA0003016208480000157
分别为在载体坐标系oxb轴、oyb轴、ozb轴上的分量。
将
Figure GDA0003016208480000158
进行角速度更新，进而可以得到补偿后的姿态、速度及位置信息。
步骤七：将步骤六中得到的补偿后载体的姿态、速度及位置信息存储并输出。
对本发明有益效果进行验证方式如下：
仿真实验：
舰船以10m/s的速度匀速直线航行
当地经纬度：
Figure GDA0003016208480000159
λ0＝126.6705°
地球半径：R＝6378393.0m
由万有引力可得的地球表面重力加速度：g＝9.78049m/s2
地球自转角速度：ωie＝7.2931158×10-5rad/s
因受风浪等因素影响，载体姿态呈周期性变化，模拟载体姿态如：
Figure GDA0003016208480000161
陀螺参数：
陀螺常值漂移：0.01rad/h
陀螺刻度因数误差：5×10-4
陀螺白噪声：1×10-5
加速度计参数：
加速度计零偏：10-5g
加速度计白噪声：10-6g
星敏感器参数：
星敏感器的测量噪声看成零均值的高斯白噪声；
200s～400s时，星敏感器测量噪声方差变为5”；
600s～800s时，星敏感器测量噪声方差变为60”；
1000s～1200s时，星敏感器测量噪声方差变为30”；
1400s～1600s时，星敏感器测量噪声方差变为10”；
其他时刻星敏感器无噪声。
模糊逻辑控制方法参数设置：
基本论域：[a c]＝[0 120]
模糊论域：[a1 c1]＝[0 3]
划分后的论域：
Figure GDA0003016208480000162
不同噪声下的遗传因子：
Figure GDA0003016208480000163
简化Sage-Husa自适应滤波器参数设置：
P0＝diag([10-6 10-6 10-6 6.25×10-6 6.25×10-6 6.25×10-6 10-10 10-10 10-10]2)
Q0＝diag([2.97×10-7 2.97×10-7 2.97×10-7 0 0 0 0 0 0]2)
R0＝diag([2×10-3 2×10-3 2×10-3]2)
采样频率：1s
仿真时间：1800s
利用发明所述方法，得到星敏感器测量噪声统计特性发生变化情况下标准卡尔曼滤波和基于遗传因子自适应选择Sage-Husa滤波方法估计陀螺漂移和刻度因数的比较曲线。图2为仿真结果比较曲线，图3为δe2绝对值的隶属函数。根据图2结果可看出，利用本发明提出的基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺在线标定方法较好的跟踪测量噪声统计特性，修正滤波器的测量噪声参数，估计出了陀螺漂移和刻度因数误差，提高滤波精度，改善导航***精度，提高***的环境适用性，满足实际需求。

Claims (6)

1.一种基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法，其特征在于，包括以下步骤：
(1)上电，初始化星敏感器/惯性组合导航***；
(2)***实时采集惯性器件和星敏感器输出数据，包括陀螺仪输出的三轴角速度
Figure FDA0003025941720000011
加速度计输出的比力信息
Figure FDA0003025941720000017
和星敏感器输出相对惯性空间的姿态信息
Figure FDA0003025941720000012
(3)利用(2)中得到的陀螺仪输出信息进行姿态解算，得到姿态四元数的计算值
Figure FDA0003025941720000013
再结合加速度计输出数据进行导航解算，得到导航信息；
(4)利用(2)中得到的星敏感器输出数据进行惯性系下姿态解算，得到载体姿态四元数的真实值
Figure FDA0003025941720000018
(5)利用模糊逻辑控制方法确定简化Sage-Husa自适应滤波方法中遗忘因子
Figure FDA0003025941720000019
构造简化Sage-Husa自适应滤波器，以(3)中解算得到的四元数计算值
Figure FDA0003025941720000014
和(4)中解算得到的四元数真实值
Figure FDA00030259417200000110
之间的误差四元数的矢量部分
Figure FDA00030259417200000111
作为观测量；误差四元数的矢量部分
Figure FDA00030259417200000112
陀螺刻度因数
Figure FDA00030259417200000113
陀螺漂移
Figure FDA00030259417200000114
为状态量，进行滤波计算，估计出陀螺常值漂移、刻度因数误差；
(6)将(5)中
Figure FDA00030259417200000115
和
Figure FDA00030259417200000116
补偿陀螺仪输出信息
Figure FDA0003025941720000015
进行导航解算得到载体补偿后的导航信息；
(7)将(6)中得到的载体导航信息存储并输出；
利用模糊逻辑控制方法确定
Figure FDA00030259417200000117
时刻的遗忘因子
Figure FDA00030259417200000118
构造简化Sage-Husa自适应滤波器，具体包括以下步骤：
1)根据步骤(3)中载体姿态四元数的计算值与步骤(4)中载体姿态四元数的真实值解算得到误差四元数，过程如下：
Figure FDA0003025941720000016
其中，
Figure FDA00030259417200000119
表示误差四元数，
Figure FDA00030259417200000120
表示误差四元数的实数部分，
Figure FDA00030259417200000121
表示误差四元数的虚数部分；
Figure FDA00030259417200000122
表示矩阵的逆；
2)利用1)中误差四元数的第3个分量
Figure FDA00030259417200000123
的绝对值对星敏感器测量噪声统计特性进行分析，离散的将星敏感器测量噪声状态定义为若干级，形式为：
Figure FDA00030259417200000124
确定
Figure FDA00030259417200000125
绝对值的论域集并划分论域，基本论域为
Figure FDA00030259417200000126
通过量化因子转化的模糊论域为
Figure FDA00030259417200000127
划分后的论域为：
Figure FDA0003025941720000021
其中，
Figure FDA0003025941720000027
表示星敏感器无测量噪声，
Figure FDA0003025941720000028
表示星敏感器测量噪声为低噪声，
Figure FDA0003025941720000029
表示星敏感器测量噪声为高噪声；
3)根据
Figure FDA00030259417200000210
绝对值的实际情况建立模糊论域各元素对模糊语言的隶属度大小，即相应的隶属函数，表达形式如下：
三角形函数为：
Figure FDA0003025941720000022
梯形函数为：
Figure FDA0003025941720000023
其中，参数
Figure FDA00030259417200000211
分别表示模糊集合中的元素；
4)将
Figure FDA00030259417200000212
的绝对值作为模糊逻辑控制的输入信息，结合3)中隶属函数，经过模糊逻辑运算，进而推测出
Figure FDA00030259417200000222
时刻星敏感器测量噪声状态；其中模糊逻辑规则的具体表达式为：
Figure FDA0003025941720000024
5)依据4)中推测出的
Figure FDA00030259417200000214
时刻星敏感器测量噪声状态，采用重心法进行反模糊化得到
Figure FDA00030259417200000213
的精确值，进而选择
Figure FDA00030259417200000215
时刻相应的遗忘因子
Figure FDA00030259417200000216
选择方法具体如下：
Figure FDA0003025941720000025
其中，
Figure FDA00030259417200000217
的范围为(0 1)，重心法计算式为
Figure FDA0003025941720000026
Figure FDA00030259417200000218
表示模糊集合元素，
Figure FDA00030259417200000219
表示元素
Figure FDA00030259417200000220
的隶属度，
Figure FDA00030259417200000221
表示精确值；
6)构造简化Sage-Husa自适应滤波器，滤波状态量为
Figure FDA0003025941720000037
观测量为
Figure FDA0003025941720000038
滤波过程具体如下：
利用下列微方程建立***方程：
Figure FDA0003025941720000031
其中
Figure FDA00030259417200000312
为陀螺刻度因数，且
Figure FDA00030259417200000311
为陀螺漂移，且
Figure FDA00030259417200000313
Figure FDA0003025941720000032
分别为
Figure FDA00030259417200000314
的变化率；
Figure FDA00030259417200000315
为刻度因数导致的陀螺仪测量误差，且
Figure FDA0003025941720000033
Figure FDA00030259417200000316
为传感器测量噪声；
Figure FDA00030259417200000317
为三行一列的零向量；
Figure FDA0003025941720000034
分别为陀螺仪测量角速度在载体坐标系
Figure FDA00030259417200000318
轴、
Figure FDA00030259417200000319
轴、
Figure FDA00030259417200000320
轴上的分量；
基于上述误差模型，采用简化Sage-Husa自适应滤波器，对陀螺误差进行在线标定，具体算法如下：
Figure FDA00030259417200000321
Figure FDA00030259417200000322
Figure FDA0003025941720000035
其中，
Figure FDA00030259417200000323
为
Figure FDA00030259417200000324
时刻的调节因子，
Figure FDA00030259417200000325
为
Figure FDA00030259417200000326
时刻到
Figure FDA00030259417200000327
时刻的状态转移矩阵，
Figure FDA00030259417200000328
为陀螺角速度信息构成的对角阵，
Figure FDA00030259417200000329
为单位阵，
Figure FDA00030259417200000330
为零矩阵；
根据测量信息计算
Figure FDA00030259417200000331
时刻的新息，再通过指数加权法不断估计
Figure FDA00030259417200000332
时刻的测量噪声
Figure FDA00030259417200000333
阵：
Figure FDA00030259417200000334
Figure FDA0003025941720000036
计算滤波增益、状态估值及更新
Figure FDA00030259417200000335
：
Figure FDA00030259417200000336
Figure FDA00030259417200000337
Figure FDA00030259417200000338
其中，
Figure FDA00030259417200000339
为
Figure FDA00030259417200000340
时刻的状态量，当
Figure FDA00030259417200000341
时，状态量
Figure FDA00030259417200000342
为初始化***时获得的初始状态量
Figure FDA00030259417200000343
2.根据权利要求1所述的一种基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法，其特征在于：步骤(1)中初始化星敏感器/惯性组合导航***，需初始化***以下信息：
导航信息初始化：舰船位置信息
Figure FDA00030259417200000416
单位为弧度，用
Figure FDA00030259417200000456
表示，速度信息
Figure FDA00030259417200000417
单位为米/秒，用
Figure FDA00030259417200000455
表示，以及姿态角信息
Figure FDA00030259417200000418
单位为弧度，用
Figure FDA00030259417200000457
表示，初始转换矩阵
Figure FDA0003025941720000042
初始四元数
Figure FDA00030259417200000419
初始化常值参量：当地经纬度
Figure FDA00030259417200000420
单位为弧度，用rad表示，加速度计白噪声误差
Figure FDA00030259417200000421
陀螺仪白噪声误差
Figure FDA00030259417200000422
采样时间
Figure FDA00030259417200000423
单位为秒，用
Figure FDA00030259417200000424
表示；
简化Sage-Husa自适应滤波器参数初值：状态变量初值
Figure FDA00030259417200000425
协方差阵
Figure FDA00030259417200000426
***噪声方差阵
Figure FDA00030259417200000427
量测噪声方差阵
Figure FDA00030259417200000428
其中，
Figure FDA00030259417200000429
表示误差四元数矢量部分的初始值；
Figure FDA00030259417200000430
表示陀螺仪刻度因数的初始值；
Figure FDA00030259417200000431
表示陀螺仪漂移的初始值；
初始转换矩阵计算如下：
Figure FDA0003025941720000044
其中，
Figure FDA00030259417200000432
表示载体坐标系，
Figure FDA00030259417200000433
表示导航坐标系，
Figure FDA0003025941720000045
表示
Figure FDA00030259417200000434
系到
Figure FDA00030259417200000435
系的转移矩阵；
初始化四元数
Figure FDA00030259417200000436
计算如下：
令
Figure FDA0003025941720000046
则
Figure FDA00030259417200000437
Figure FDA0003025941720000047
Figure FDA00030259417200000438
其中，
Figure FDA00030259417200000439
为矩阵
Figure FDA0003025941720000048
中第
Figure FDA00030259417200000440
行、第
Figure FDA00030259417200000441
列矩阵元素，
Figure FDA00030259417200000442
表示四元数的实数部分，
Figure FDA00030259417200000443
为四元数的虚数部分。
3.根据权利要求2所述的一种基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法，其特征在于：步骤(2)中所述***实时采集惯性器件和星敏感器的输出数据，包括陀螺仪输出的三轴角速度
Figure FDA0003025941720000049
和加速度计输出的比力信息
Figure FDA00030259417200000410
星敏感器输出相对惯性空间的姿态信息
Figure FDA00030259417200000411
其中
Figure FDA00030259417200000412
为
Figure FDA00030259417200000444
系相对
Figure FDA00030259417200000445
系旋转角速度在
Figure FDA00030259417200000446
系投影，
Figure FDA00030259417200000413
分别为陀螺仪测量角速度在载体坐标系
Figure FDA00030259417200000447
轴、
Figure FDA00030259417200000448
轴、
Figure FDA00030259417200000449
轴上的分量，单位均为弧度/秒，用
Figure FDA00030259417200000458
表示；
Figure FDA00030259417200000414
分别为加速度计测量加速度在载体坐标系
Figure FDA00030259417200000450
轴、
Figure FDA00030259417200000451
轴、
Figure FDA00030259417200000452
轴上的分量，单位均为米/秒，用
Figure FDA00030259417200000453
表示；
Figure FDA00030259417200000454
表示惯性坐标系。
4.根据权利要求3所述的一种基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法，其特征在于：步骤(3)中利用陀螺仪采集到的角速度
Figure FDA00030259417200000415
进行姿态解算，得到载体姿态四元数的计算值
Figure FDA0003025941720000051
结合采集到的加速度
Figure FDA00030259417200000515
进行导航解算得到导航信息，具体过程如下：
角速度更新：
Figure FDA0003025941720000052
其中，
Figure FDA00030259417200000516
表示地球坐标系，
Figure FDA00030259417200000517
表示平台坐标系；
Figure FDA0003025941720000053
表示
Figure FDA00030259417200000518
系到
Figure FDA00030259417200000519
系的转换矩阵，且
Figure FDA0003025941720000054
Figure FDA00030259417200000520
分别表示平台坐标系与导航坐标系三轴误差角；
Figure FDA0003025941720000055
表示
Figure FDA00030259417200000521
系相对
Figure FDA00030259417200000522
系旋转角速度在
Figure FDA00030259417200000523
系投影，
Figure FDA00030259417200000524
表示矩阵的转置；
载体姿态四元数计算值的更新：
设任意时刻载体坐标系相对平台坐标系的转动四元数为：
Figure FDA0003025941720000056
其中，
Figure FDA0003025941720000057
表示四元数；
Figure FDA00030259417200000525
表示四元数的四个实数；
Figure FDA00030259417200000526
分别表示载体坐标系
Figure FDA00030259417200000527
轴、
Figure FDA00030259417200000528
轴、
Figure FDA00030259417200000529
轴上的单位方向向量；
四元数
Figure FDA0003025941720000058
的即时修正：
Figure FDA0003025941720000059
其中，
Figure FDA00030259417200000510
分别表示平台坐标系相对载体坐标系的运动角速度在载体坐标系
Figure FDA00030259417200000530
轴、
Figure FDA00030259417200000531
轴、
Figure FDA00030259417200000532
轴上的分量；
Figure FDA00030259417200000511
分别表示
Figure FDA00030259417200000533
的变化率；
根据
Figure FDA00030259417200000534
时刻载体坐标系相对平台坐标系的转动四元数
Figure FDA00030259417200000535
求取
Figure FDA00030259417200000536
时刻转动四元数的变化率为：
Figure FDA00030259417200000512
在
Figure FDA00030259417200000537
时刻载体的转动四元数具体为：
Figure FDA00030259417200000513
至此，根据上式得到载体姿态四元数的计算值
Figure FDA00030259417200000514
载体姿态、速度及位置的更新：
利用得到的
Figure FDA00030259417200000613
更新捷联矩阵
Figure FDA0003025941720000061
Figure FDA0003025941720000062
其中，上式中的
Figure FDA00030259417200000614
为
Figure FDA00030259417200000615
时刻载体的转动四元数中
Figure FDA00030259417200000616
即上式中省略了
Figure FDA00030259417200000617
更新姿态信息：
Figure FDA0003025941720000063
利用转换关系式
Figure FDA0003025941720000064
将加速度计沿载体系测得的加速度信息投影转换到导航坐标系，利用下列微分方程求解载体运动速度：
Figure FDA0003025941720000065
其中，
Figure FDA0003025941720000066
分别表示解算载体速度在导航系
Figure FDA00030259417200000618
轴、
Figure FDA00030259417200000619
轴、
Figure FDA00030259417200000620
轴上的投影；
Figure FDA0003025941720000067
表示
Figure FDA0003025941720000068
的变化率；
Figure FDA0003025941720000069
分别表示加速度计测得的加速度信息在导航系
Figure FDA00030259417200000621
轴、
Figure FDA00030259417200000622
轴、
Figure FDA00030259417200000623
轴上的投影；
Figure FDA00030259417200000624
表示当地重力加速度；
根据
Figure FDA00030259417200000625
时刻的载体的三轴速度
Figure FDA00030259417200000626
与
Figure FDA00030259417200000627
求取
Figure FDA00030259417200000628
时刻的载体速度变化率
Figure FDA00030259417200000610
进而得到
Figure FDA00030259417200000629
时刻载体的速度和位置：
Figure FDA00030259417200000611
其中，
Figure FDA00030259417200000630
表示地球半径；当
Figure FDA00030259417200000631
时，
Figure FDA00030259417200000632
为步骤(1)中初始化***时获得的载体初始速度，
Figure FDA00030259417200000633
为步骤一中初始化***时获得的载体初始位置；
至此，得到载体的姿态角、速度和位置。
5.根据权利要求1所述的一种基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法，其特征在于：(4)中所述利用(2)中得到的星敏感器输出数据进行惯性系下姿态解算，得到载体姿态四元数的真实值
Figure FDA00030259417200000634
即
Figure FDA0003025941720000071
其中，
Figure FDA0003025941720000078
表示矩阵
Figure FDA0003025941720000072
中第
Figure FDA0003025941720000079
行、第
Figure FDA00030259417200000710
列矩阵元素；
Figure FDA00030259417200000711
表示四元数真实值的实数部分，
Figure FDA00030259417200000712
表示四元数真实值的虚数部分；
至此，根据上式得到载体姿态四元数的真实值
Figure FDA00030259417200000713
6.根据权利要求5所述的一种基于遗忘因子自适应选择Sage-Husa滤波的船用星敏感器辅助陀螺仪在线标定方法，其特征在于，步骤(6)中估计出的
Figure FDA00030259417200000714
和
Figure FDA00030259417200000715
补偿陀螺仪输出信息
Figure FDA0003025941720000073
进行导航解算得到载体补偿后的导航信息：
陀螺仪输出角速度信息补偿：
Figure FDA0003025941720000074
其中，
Figure FDA0003025941720000075
为补偿后的角速度信息，且
Figure FDA0003025941720000076
分别为在载体坐标系
Figure FDA00030259417200000716
轴、
Figure FDA00030259417200000717
轴、
Figure FDA00030259417200000718
轴上的分量；
将
Figure FDA0003025941720000077
代入角速度更新，进而得到补偿后的姿态、速度及位置信息。
CN201810764305.8A 2018-07-12 2018-07-12 一种船用星敏感器辅助陀螺仪在线标定方法 Active CN108827310B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810764305.8A CN108827310B (zh) 2018-07-12 2018-07-12 一种船用星敏感器辅助陀螺仪在线标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810764305.8A CN108827310B (zh) 2018-07-12 2018-07-12 一种船用星敏感器辅助陀螺仪在线标定方法

Publications (2)

Publication Number Publication Date
CN108827310A CN108827310A (zh) 2018-11-16
CN108827310B true CN108827310B (zh) 2021-07-23

Family

ID=64136975

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810764305.8A Active CN108827310B (zh) 2018-07-12 2018-07-12 一种船用星敏感器辅助陀螺仪在线标定方法

Country Status (1)

Country Link
CN (1) CN108827310B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109489689B (zh) * 2018-11-20 2020-11-03 上海航天控制技术研究所 一种基于α-β滤波的星矢量测量误差在轨估计方法
CN110132271B (zh) * 2019-01-02 2022-04-12 中国船舶重工集团公司第七0七研究所 一种自适应卡尔曼滤波姿态估计算法
CN110440827B (zh) * 2019-08-01 2022-05-24 北京神导科讯科技发展有限公司 一种参数误差的标定方法、装置及存储介质
CN110705002B (zh) * 2019-08-13 2022-11-11 北京机电工程研究所 一种仿真试验的补偿***及方法
CN110836664B (zh) * 2019-09-29 2021-06-08 渤海造船厂集团有限公司 一种船台统一基准建立方法及装置
CN110672127B (zh) * 2019-11-01 2021-10-19 苏州大学 阵列式mems磁传感器实时标定方法
CN111044082B (zh) * 2020-01-15 2021-07-06 北京航空航天大学 一种基于星敏感器辅助的陀螺误差参数在轨快速标定方法
CN111207776B (zh) * 2020-02-25 2022-04-12 上海航天控制技术研究所 一种适用于火星探测的星敏感器与陀螺联合标定方法
CN111504310B (zh) * 2020-04-30 2022-12-27 东南大学 一种新的弹载ins/cns组合导航***建模方法
CN112665570B (zh) * 2020-11-30 2022-11-22 北京电子工程总体研究所 一种基于星敏感器的mems陀螺零偏在轨简化工程计算方法
CN112649007A (zh) * 2021-01-13 2021-04-13 中国科学院微小卫星创新研究院 一种姿态敏感器的一体化设计方法
CN113008272B (zh) * 2021-03-08 2022-04-19 航天科工空间工程发展有限公司 一种用于微小卫星的mems陀螺在轨常值漂移标定方法和***
CN113091740B (zh) * 2021-03-22 2023-05-16 浙江兆晟科技股份有限公司 一种基于深度学习的稳定云台陀螺仪漂移实时修正方法
CN113418536B (zh) * 2021-06-28 2022-08-12 北京控制工程研究所 一种基于相关信号对消的陀螺仪在轨精度评价方法和***
CN114166203B (zh) * 2021-11-16 2024-02-09 哈尔滨工程大学 一种基于改进的s-h自适应联邦滤波的智能水下机器人多源组合导航方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102545225A (zh) * 2012-01-16 2012-07-04 广西电网公司电力科学研究院 高压并联混合型有源电力滤波器及带遗忘因子的迭代学习控制方法
CN103398713A (zh) * 2013-04-26 2013-11-20 哈尔滨工程大学 一种星敏感器/光纤惯性设备量测数据同步方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102545225A (zh) * 2012-01-16 2012-07-04 广西电网公司电力科学研究院 高压并联混合型有源电力滤波器及带遗忘因子的迭代学习控制方法
CN103398713A (zh) * 2013-04-26 2013-11-20 哈尔滨工程大学 一种星敏感器/光纤惯性设备量测数据同步方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"Application of Fuzzy Adaptive Kalman Filtering on GPS/INS Integrated Navigation System";Mingwei Liu;《Advanced Materials Research Vols 317-319 (2011)》;20110816;正文第1512-1517页 *
"基于自适应滤波的捷联惯性/星光组合导航技术";曾威;《惯性技术发展动态发展方向研讨会文集》;20121231;正文第68-73页 *

Also Published As

Publication number Publication date
CN108827310A (zh) 2018-11-16

Similar Documents

Publication Publication Date Title
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
WO2020062807A1 (zh) 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
Soken et al. UKF-based reconfigurable attitude parameters estimation and magnetometer calibration
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
EP1585939A2 (en) Attitude change kalman filter measurement apparatus and method
CN112146655B (zh) 一种BeiDou/SINS紧组合导航***弹性模型设计方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航***及方法
CN108613674A (zh) 一种基于自适应差分进化bp神经网络的姿态误差抑制方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导***行进间初始对准方法
CN104697520B (zh) 一体化无陀螺捷联惯导***与gps***组合导航方法
Huang et al. Variational Bayesian-based filter for inaccurate input in underwater navigation
CN110567455B (zh) 一种求积更新容积卡尔曼滤波的紧组合导航方法
CN109708663B (zh) 基于空天飞机sins辅助的星敏感器在线标定方法
CN111750865A (zh) 一种用于双功能深海无人潜器导航***的自适应滤波导航方法
Ben et al. A dual-state filter for a relative velocity aiding strapdown inertial navigation system
Jørgensen et al. Underwater position and attitude estimation using acoustic, inertial, and depth measurements
CN104634348B (zh) 组合导航中的姿态角计算方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN111854741B (zh) 一种gnss/ins紧组合滤波器及导航方法
CN110375773B (zh) Mems惯导***姿态初始化方法
CN113503891B (zh) 一种sinsdvl对准校正方法、***、介质及设备
CN113434806B (zh) 一种抗差自适应多模型滤波方法
Jiang et al. Study on Shipboard Navigation Method Based on MEMS/GNSS Integration
Gao et al. Online calibration method of DVL error based on real-time deformation compensation

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