CN113324539A - 一种sins/srs/cns多源融合自主组合导航方法 - Google Patents

一种sins/srs/cns多源融合自主组合导航方法 Download PDF

Info

Publication number
CN113324539A
CN113324539A CN202110544188.6A CN202110544188A CN113324539A CN 113324539 A CN113324539 A CN 113324539A CN 202110544188 A CN202110544188 A CN 202110544188A CN 113324539 A CN113324539 A CN 113324539A
Authority
CN
China
Prior art keywords
sins
measurement
srs
navigation
cns
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.)
Pending
Application number
CN202110544188.6A
Other languages
English (en)
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.)
Northwestern Polytechnical University
Shenzhen Institute of Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Shenzhen Institute of 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, Shenzhen Institute of Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202110544188.6A priority Critical patent/CN113324539A/zh
Publication of CN113324539A publication Critical patent/CN113324539A/zh
Pending legal-status Critical Current

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G01C21/025—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means with the use of startrackers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • 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

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)

Abstract

本发明公开了一种SINS/SRS/CNS多源融合自主组合导航方法，构建SINS/SRS导航子系统，确定SINS/SRS导航子系统的速度量测量和高度量测量；结合速度量测量和高度量测量构建SINS/SRS导航子系统的量测方程并解算，得出第一估计值；构建SINS/CNS导航子系统，确定SINS/CNS导航子系统的姿态角量测量；基于姿态角量测量构建SINS/CNS导航子系统的量测方程并解算，得出第二估计值；融合第一估计值和第二估计值，得到SINS/SRS/CNS导航系统的全局最优估计值，并利用全局最优估计值对SINS的误差进行实时校正；本发明克服了各单一导航系统的缺陷，在性能上实现了互补，实现了在深空导航中的全自主高精度多源融合导航。

Description

一种SINS/SRS/CNS多源融合自主组合导航方法
技术领域
本发明属于资助组合导航技术领域，尤其涉及一种SINS/SRS/CNS多源融合自主组合导航方法。
背景技术
惯性导航系统(Inertial Navigation System,INS)可以独立给出载体的姿态、速度和位置等导航参数，抗外界干扰能力强，短期稳定性能好，相对导航精度较高，在航空、航天和航海等军事和民用领域得到了广泛应用。但惯性导航系统的缺点是导航误差随时间积累，纯惯性导航系统难以满足飞行器高精度、长航时导航的需求。
天文导航系统是用天文方法观测星辰日月等天体来确定航行体的位置，以引导航行体沿预定航线到达目的地的一种导航方法。它是一门古老而崭新的技术，在导航技术中占有重要的位置。它不需要地面设备支持，既不受人工或自然形成的电磁干扰，也不向外辐射电磁波，自主性强，隐蔽性好。其缺点是导航数据更新率低、导航性能受制于目标天体的数量、距离和空间环境等因素，且不能直接测速。
光谱红移导航是一种隐蔽性好、自主性强的新导航体制，它不向外发射任何电磁波，任何人用任何方法都打不掉用于航天器导航的太阳系天体(太阳、木星、地球等)，充分有效地利用这些天然资源实现航天器自主导航是人类的责任和荣耀。但单一的光谱红移自主导航方法，在航天器进行姿态机动过程中，由于短时间内无法获取足够的观测信息，或由于观测信息中断而无法获得测量信息，从而使导航精度变差，甚至结果发散。此外，由于天体遮挡等原因，在某些时段会出现测量信号暂时中断的情况，无法进行导航参数的解算。
综上，现有的导航方法在深空导航中的局限性主要是或需要地面站支持，不具备自主性，或需引入航天器轨道动力学，导致大量星上复杂计算。这些局限性使深空自主导航的实现更加困难，亟待在导航方法上有所突破。
发明内容
本发明的目的是提供一种SINS/SRS/CNS多源融合自主组合导航方法，以实现在深空导航中的全自主高精度多源融合导航。
本发明采用以下技术方案：一种SINS/SRS/CNS多源融合自主组合导航方法，包括以下步骤：
构建SINS/SRS导航子系统，确定SINS/SRS导航子系统的速度量测量和高度量测量；
结合速度量测量和高度量测量构建SINS/SRS导航子系统的量测方程并解算，得出第一估计值；
构建SINS/CNS导航子系统，确定SINS/CNS导航子系统的姿态角量测量；
基于姿态角量测量构建SINS/CNS导航子系统的量测方程并解算，得出第二估计值；
融合第一估计值和第二估计值，得到SINS/SRS/CNS导航系统的全局最优估计值，并利用全局最优估计值对SINS的误差进行实时校正。
进一步地，确定SINS/SRS导航子系统的速度量测量和高度量测量包括：
通过SINS获取载体的第一速度信息，通过SRS获取载体的第二速度信息；计算第一速度信息和第二速度信息的速度差信息，并将速度差信息作为SINS/SRS导航子系统的速度量测量；
通过SINS获取载体的第一高度信息，通过高度量测元件获取载体的第二高度信息；计算第一高度信息和第二高度信息的高度差信息，并将高度差信息作为SINS/SRS导航子系统的高度量测量。
进一步地，确定SINS/CNS导航子系统的姿态角量测量包括：
通过SINS获取载体的第一姿态角信息，通过CNS获取载体的第二姿态角信息；计算第一姿态角信息和第二姿态角信息的姿态角差信息，并将姿态角差信息作为SINS/CNS导航子系统的姿态角量测量。
进一步地，SINS/SRS导航子系统的量测方程为：
Figure BDA0003072958190000031
其中，Z1(t)为第一估计值，Hv为速度测量矩阵，
Figure BDA0003072958190000032
Hh为高度测量矩阵，
Figure BDA0003072958190000033
为系统状态向量，Vv(t)为速度量测噪声，Vh(t)为高度量测噪声，H1(t)为SINS/SRS导航子系统的量测矩阵，V1(t)为SINS/SRS导航子系统的量测噪声。
进一步地，SINS/CNS导航子系统的量测方程为：
Z2(t)＝H2(t)X(t)+V2(t)，
其中，Z2(t)为第二估计值，H2(t)为SINS/CNS导航子系统的量测矩阵，V2(t)为SINS/CNS导航子系统的量测噪声。
进一步地，SINS/SRS导航子系统的量测方程和SINS/CNS导航子系统的量测方程的解算方法均采用抗差自适应Unscented粒子滤波方法；
抗差自适应Unscented粒子滤波方法包括以下步骤：
步骤S1、采用粒子算法生成k＝0时刻的N个信息值xi0，并为每个信息值赋予权值
Figure BDA0003072958190000041
其中，N为正整数，i＝1,2....,N，
Figure BDA0003072958190000042
步骤S2、在k＝1,2,...M时刻，按照下面次序计算：
步骤S2.1、计算输入的量测量的等价权
Figure BDA00030729581900000434
和自适应因子α；其中，量测量为速度量测量、高度量测量或姿态角量测量；
步骤S2.2、采用UKF算法更新信息值
Figure BDA0003072958190000043
得到
Figure BDA0003072958190000044
并结合等价权
Figure BDA0003072958190000045
和自适应因子α计算
Figure BDA0003072958190000046
对应的信息值估值
Figure BDA0003072958190000047
和方差
Figure BDA0003072958190000048
其中，
Figure BDA0003072958190000049
为(k-1)时刻的第i个信息值，
Figure BDA00030729581900000410
为
Figure BDA00030729581900000411
对应的协方差矩阵，
Figure BDA00030729581900000412
为k时刻的第i个信息值，
Figure BDA00030729581900000413
为
Figure BDA00030729581900000414
对应的协方差矩阵；
步骤S2.3、计算权值
Figure BDA00030729581900000415
并归一化为
Figure BDA00030729581900000416
其中，
Figure BDA00030729581900000417
为量测量，
Figure BDA00030729581900000418
为在已知
Figure BDA00030729581900000419
的条件下
Figure BDA00030729581900000420
的条件概率密度函数，
Figure BDA00030729581900000421
为在已知
Figure BDA00030729581900000422
的条件下
Figure BDA00030729581900000423
的条件概率密度函数，
Figure BDA00030729581900000424
为在已知
Figure BDA00030729581900000425
的条件下
Figure BDA00030729581900000426
的条件概率密度函数；
其中，
Figure BDA00030729581900000427
Figure BDA00030729581900000435
表示k时刻的预测状态量，
Figure BDA00030729581900000428
Pxklk表示状态和两侧的互协方差，
Figure BDA00030729581900000429
表示预测量测协方差的逆，
Figure BDA00030729581900000430
为k时刻的预测量测量；
步骤S2.4、计算估计式
Figure BDA00030729581900000431
将所得结果与阈值进行比较，当所得结果小于等于阈值，重复上述步骤S2.1-步骤S2.4，直至所得结果大于阈值，执行步骤S2.5；
步骤S2.5、计算状态量估计值
Figure BDA00030729581900000432
及协方差估计值
Figure BDA00030729581900000433
其中，状态量估计值为第一估计值或第二估计值。
进一步地，计算输入的量测量的等价权
Figure BDA00030729581900000510
和自适应因子α具体包括：
设等价权矩阵为
Figure BDA0003072958190000051
Figure BDA0003072958190000052
其中，k0和k1均为常值，k0∈(1,1.5)，k1∈(3,8)，Vk为量测量的残差向量，
Figure BDA0003072958190000053
Figure BDA0003072958190000054
为当前时刻载体的系统状态向量估计值，zk为量测量，Ak为量测矩阵；
自适应因子选取如下：
Figure BDA0003072958190000055
其中，c0和c1均为常数，c0∈(1,1.5)，c1∈(3,8)，
Figure BDA0003072958190000056
tr(·)表示矩阵的迹，
Figure BDA0003072958190000057
为预报值。
进一步地，计算输入的量测量的等价权
Figure BDA00030729581900000511
和自适应因子α具体包括：
设等价权矩阵为
Figure BDA0003072958190000058
Figure BDA0003072958190000059
自适应因子选取如下：
Figure BDA0003072958190000061
其中，c0和c1均为常数，c0∈(1,1.5)，c1∈(3,8)，
Figure BDA0003072958190000062
tr(·)表示矩阵的迹，
Figure BDA0003072958190000063
为预报值。
本发明的有益效果是：本申请所设计的SINS/SRS/CNS自主组合导航系统，利用先进的信息融合技术，将SINS、SRS和CNS三者进行有机结合；该系统同时兼备了SINS、SRS和CNS各自的优点，克服了各单一导航系统的缺陷，在性能上实现了互补；SINS/SRS/CNS自主组合导航系统具有各单一导航系统所不具备的优良性能；组合导航系统的姿态精度达到1′以内，速度精度达到0.1m/s，定位精度3m以内，能满足飞行器对导航系统精度的需求，有效提高了导航系统的自主性、可靠性和精度。
附图说明
图1为本发明实施例中光谱红移自主导航基本原理图；
图2为本发明实施例中SINS/SRS导航子系统的原理图；
图3为本发明实施例中SINS/CNS导航子系统的原理图；
图4为本发明实施例中SINS/SRS/CNS导航系统图；
图5为本发明实施例中SINS/SRS/CNS导航原理图；
图6为本发明验证实施例SRS/SINS/CNS多原融合自主组合导航仿真实验系统方案；
图7为本发明验证实施例中载体飞行航迹仿真图；
图8为本发明验证实施例中航向角误差比较曲线图；
图9为本发明验证实施例中东向速度误差比较曲线图；
图10为本发明验证实施例中纬度误差比较曲线图；
图11为本发明验证实施例中星敏感器无量测时SINS/SRS导航子系统的东向速度误差曲线图；
图12为本发明验证实施例中姿态误差曲线图；
图13为本发明验证实施例中速度误差曲线图；
图14为本发明验证实施例中位置误差曲线图；
具体实施方式
下面结合附图和具体实施方式对本发明进行详细说明。
本发明基于太阳系天体光谱红移测速导航原理，利用太阳系自然天体的光谱红移信息、物体运动定律和自然天体的天文信息作为导航信息源，采用非线性滤波算法和多源信息融合技术，设计全自主高精度SINS/SRS/CNS多源融合智能导航新体制，建立SINS/SRS/CNS多源智能组合导航系统的原理、方案和数学模型；设计适合该组合导航系统解算的抗差自适应Unscented粒子滤波算法；分别建立SINS/SRS组合导航子系统仿真实验系统、SINS/CNS组合导航子系统的仿真实验系统，以及SINS/SRS/CNS多源融合自主组合导航仿真实验系统，对所设计的组合导航系统的原理、方案、模型和算法进行了实验验证和分析。
为了加深和拓展对光谱红移导航方法的理解，这里将从另一角度研究光谱红移导航原理和算法，虽然和传统的方法公式推导结果相同，但本申请的方法更直观、更容易理解。
航天器光谱红移自主导航的基本原理是：将太阳系天体的光信号作为导航信息源，结合太阳系天体星历信息及航天器惯性姿态信息，根据光谱红移效应测量获得航天器在惯性坐标系中的飞行速度，并通过积分获得航天器在惯性坐标系中的位置参数，其导航原理如图1所示。
假设航天器在运行过程中，人们可探测到天体发出的光信号，则根据多普勒原理(Doppler principle，DP)，航天器接收到的光谱频率不等于该天体发出的光谱频率，且频率的变化大小与航天器相对天体的运动状态变化有关。因此，通过测量光谱频率的红移，可间接获得航天器的相对运动速度。根据空间向量关系，若观测的不共线的天体数大于3个，则综合天体运行星历及通过惯性测量元件测量到的航天器姿态信息，可确定出航天器在惯性空间中的速度信息，通过积分可获得航天器的位置信息。
根据光谱红移特征频移公式，当航天器相对光源运动时，在航天器上接收到的光源频率fm与地面标定特征频率f0的关系可表示为：
Figure BDA0003072958190000081
公式(1)中，v为惯性坐标系中航天器相对光源运动的速度矢量，θ为惯性坐标系中光源——航天器波矢(即光源指向航天器)与v的夹角，c为真空中光速。
由公式(1)可推导出：
Figure BDA0003072958190000082
进一步，根据公式(2)可推导出下式成立：
Figure BDA0003072958190000083
公式(3)实际上是航天器相对天体运动的径向速率：
Figure BDA0003072958190000091
假设航天器在轨接收并测得各参考天体的光源频率为fm1，fm2和fm3，并已知各参考天体发出的光源频率相应为f01，f02和f03，则航天器相对各天体运动的径向速率分别为：
Figure BDA0003072958190000092
式(5)中，vp为航天器在惯性系中的速度矢量。
根据天体几何关系，vp与vr1，vr2和vr3之间满足下面关系：
Figure BDA0003072958190000093
式(6)中v1、v2和v3为各天体在惯性系中的速度矢量，可通过星历确定；u1、u2和u3为惯性系中各天体指向航天器位置矢量的单位矢量，可由太阳敏感器或星敏感器测得。
由此可建立对速度矢量、位置矢量的状态估计方程组为：
Figure BDA0003072958190000094
给定初值后，求解上述方程组式(7)，可得航天器在惯性坐标系中的速度矢量vP及位置矢量rP。
本发明实施例公开了一种SINS/SRS/CNS多源融合自主组合导航方法，包括以下步骤：
构建SINS/SRS导航子系统，确定SINS/SRS导航子系统的速度量测量和高度量测量；结合速度量测量和高度量测量构建SINS/SRS导航子系统的量测方程并解算，得出第一估计值；构建SINS/CNS导航子系统，确定SINS/CNS导航子系统的姿态角量测量；基于姿态角量测量构建SINS/CNS导航子系统的量测方程并解算，得出第二估计值；融合第一估计值和第二估计值，得到SINS/SRS/CNS导航系统的全局最优估计值；利用全局最优估计值对SINS的误差进行实时校正。
本申请所设计的SINS/SRS/CNS自主组合导航系统，利用先进的信息融合技术，将SINS、SRS和CNS三者进行有机结合。该系统同时兼备了SINS、SRS和CNS各自的优点，克服了各单一导航系统的缺陷，在性能上实现了互补。SINS/SRS/CNS自主组合导航系统具有各单一导航系统所不具备的优良性能。组合导航系统的姿态精度达到1′以内，速度精度达到0.1m/s，定位精度3m以内，能满足飞行器对导航系统精度的需求，有效提高了导航系统的自主性、可靠性和精度。
为了更好的设计SINS/SRS/CNS自主组合导航系统，选用东-北-天(E-N-U)地理坐标系作为导航坐标系，分别研究SINS/SRS组合导航子系统和SINS/CNS组合导航子系统的设计问题。
具体的，在本实施例中，确定SINS/SRS导航子系统的速度量测量和高度量测量包括：
通过SINS获取载体的第一速度信息，通过SRS获取载体的第二速度信息；计算第一速度信息和第二速度信息的速度差信息，并将速度差信息作为SINS/SRS导航子系统的速度量测量；
通过SINS获取载体的第一高度信息，通过高度量测元件获取载体的第二高度信息；计算第一高度信息和第二高度信息的高度差信息，并将高度差信息作为SINS/SRS导航子系统的高度量测量。
SINS/SRS导航子系统原理如图2所示，SINS/SRS导航子系统原理是将SINS输出的飞行器的速度、位置和姿态信息、SRS获得的速度信息和雷达高度计得到的高度信息送入组合导航滤波器，用SRS获得的高精度速度信息对SINS进行校正，克服SINS随时间累积的导航误差，并利用雷达高度计抑制SINS高度通道的发散，得到高精度的导航信息。
SINS/CNS组合导航子系统原理如图3所示，在SINS/CNS组合导航系统中，惯性器件输出载体角运动信息和线运动信息，对这些导航信息进行解算可以获得载体实时三维姿态、速度和位置信息。利用CNS可以得到载体的姿态信息，用来校正SINS随时间积累的姿态误差。
CNS的星敏感器固连在载体上，在安装误差已经被标定的条件下，认为星敏感器坐标系与载体坐标重合。在图3中，星敏感器观测到的天体高度角和方位角，通过计算可以获得天体的星光单位方向矢量，利用姿态解算算法可以计算出载体系相对于惯性系的姿态矩阵
Figure BDA0003072958190000111
再根据
Figure BDA0003072958190000112
可求得载体系到导航系的姿态转换矩阵，从而获得CNS计算得到的载体系b到导航系n的姿态四元数。将惯性导航系统输出的导航信息qsins和CNS输出的qcns信息作差，送到SINS/CNS滤波器中进行滤波计算，可以获得状态的最优估计值。最后，用状态的最优估计值对SINS的导航参数误差进行校正，使SINS能够为CNS提供更加精确的数学平台基准。
具体的，确定SINS/CNS导航子系统的姿态角量测量包括：
通过SINS获取载体的第一姿态角信息，通过CNS获取载体的第二姿态角信息；计算第一姿态角信息和第二姿态角信息的姿态角差信息，并将姿态角差信息作为SINS/CNS导航子系统的姿态角量测量。
在整个导航系统中，如图4所示，根据光谱测量仪得到的光谱信号经过去除噪声和谱线分离，可计算获得光谱红移量的估计值。由光谱红移测速导航原理，结合天体星历信息和星敏感器测量的姿态信息，能够得到载体精确的速度信息，将该速度信息与SINS解算得到的速度信息之差作为量测量，将其作为观测量送到SINS/SRS子滤波器进行速度解算。
CNS以恒星为参照物，利用CCD星敏感器探测单元对天空直接捕获星图，并以数字方式存储。然后，经过数据处理单元对星图进行处理，并通过恒星质心提取、星图识别及坐标变换，将得到的恒星像点与星图库进行比较，可获得载体的姿态信息。将这些信息与来自SINS的相应信息进行比较，可得到CNS的测量偏差，将其作为观测量送到局部滤波器与其它观测值一起进行滤波解算。
信息融合中心(全局滤波器)将来自CNS的测量偏差与来自SINS的速度和方位偏差进行融合，可计算出导航误差估计，从而校正SINS并获得当前精确的导航解。这些导航解用来校正系统中的传感器误差，并为控制系统提供决策信息，从而使载机按预置航路精确飞行。
在导航系统中，由于CNS输出的姿态精度较高且误差不随时间积累，用CNS输出的姿态信息修正SINS的姿态误差，利用SRS输出的高精度的速度信息修正SINS的速度误差，因此，SINS/SRS/CNS组合系统具有很高的导航精度。为了减小组合导航系统的维数，假设SRS和CNS的量测误差为高斯白噪声，且不列为组合导航系统的状态量，只将SINS的系统误差考虑为SINS/SRS/CNS组合导航系统的状态量。
选取东北天(E-N-U)地理坐标系作为导航坐标系，根据惯性导航系统的误差方程，SINS/SRS/CNS组合导航系统的状态量x(t)选取为：
Figure BDA0003072958190000131
式(8)中，δvE,δvN和δvU分别为东向、北向和天向的速度误差，δPE,δPN和δPU分别为东向、北向和天向的位置误差，δθ，δγ和δη分别为飞行器的俯仰角误差、横滚角误差和航向角误差，εx,εy,εz是陀螺仪常值漂移，▽x,▽y,▽z是加速度零偏。
根据惯导系统的误差模型，SINS/SRS/CNS自主组合导航系统的状态方程为
Figure BDA0003072958190000132
式(9)中，f(·)是非线性函数，X(t)是系统状态向量，W(t)是系统过程噪声，G(t)是噪声系数矩阵。
系统状态函数为
Figure BDA0003072958190000133
式(10)中，Cω欧拉平台误差角矩阵，
Figure BDA0003072958190000134
是导航坐标系到计算坐标系的姿态转换矩阵，
Figure BDA0003072958190000135
是机体坐标系到计算坐标系的姿态转换矩阵，
Figure BDA0003072958190000136
和
Figure BDA0003072958190000137
分别是地球坐标系相对于惯性坐标系的转动角速率在导航坐标系中投影的估计值和真
Figure BDA0003072958190000138
实值，
Figure BDA0003072958190000139
和
Figure BDA00030729581900001310
分别是
Figure BDA00030729581900001311
和
Figure BDA00030729581900001312
的计算误差，
Figure BDA00030729581900001313
是加速度计输出的比力，是加速度计输出的比力误差，v和δv是速度和速度误差，δg是重力加速度误差，L和h分别是纬度和海拔高度，RM和RN分别为子午圈和卯酉圈的主曲率半径。
噪声系数阵G(t)为
Figure BDA0003072958190000141
系统噪声
Figure BDA0003072958190000142
其中
Figure BDA0003072958190000143
和
Figure BDA0003072958190000144
分别表示陀螺和加速度计的随机噪声。
为了克服SINS速度误差随时间积累的缺陷，利用SRS输出高精度的速度信息对SINS进行校正，利用雷达高度表提供的高度信息阻尼SINS高度通道的发散。
假设SINS和SRS输出的速度信息分别为VSINS＝(vx,vy,vz)和VSRS＝(vSx,vSy,vSz)，用VSINS和VSRS对应分量之差作为SINS/SRS组合子系统的速度量测量，即
Figure BDA0003072958190000145
用雷达高度表和SINS提供的高度信息之差作为SINS/SRS组合子系统的高度量测量
Zh＝[hSINS-hH]＝HhX(t)+Vh(t) (13)
式(13)中，hSINS和hH分别是由SINS和雷达高度表得到的高度量测量。
综合式(12)和式(13)，可得SINS/SRS导航子系统的量测方程如下：
Figure BDA0003072958190000146
其中，Z1(t)为第一估计值，Hv为速度测量矩阵，
Figure BDA0003072958190000147
Hh为高度测量矩阵，
Figure BDA0003072958190000151
为系统状态向量，Vv(t)为速度量测噪声，Vh(t)为高度量测噪声，H1(t)为SINS/SRS导航子系统的量测矩阵，V1(t)为SINS/SRS导航子系统的量测噪声。
SINS与CNS均能输出载体的姿态角信息，在导航坐标系下，将SINS输出的姿态角与CNS输出的姿态角之差作为SINS/CNS组合导航的量测Z2
Z2＝[θi-θcγi-γcηi-ηc] (15)
式(15)中，θi、γi和ηi分别为SINS输出的俯仰角、横滚角和航向角，θc、γc和ηc分别为CNS输出的俯仰角、横滚角和航向角。
SINS/CNS导航子系统的量测方程为：
Z2(t)＝H2(t)X(t)+V2(t) (16)
其中，Z2(t)为第二估计值，H2(t)为SINS/CNS导航子系统的量测矩阵，H2＝[I3× 303×303×9]9×15，V2(t)为SINS/CNS导航子系统的量测噪声，
Figure BDA0003072958190000152
ΔYS和ΔZS是星敏感器量测噪声。
进而，可得SINS/SRS/CNS多源融合自主组合导航系统的量测方程为
Figure BDA0003072958190000153
由光谱测量仪得到的光谱信号经过去噪和谱线分离，可计算获得光谱红移量的估计值。根据光谱红移导航原理，结合天体星历信息和星敏感器测量的姿态信息，能够得到载体精确的速度信息，将该速度信息与SINS解算得到的速度信息作差作为量测量。将其作为观测量送到SINS/SRS局部子滤波器进行滤波计算。将CNS获得的飞行器的姿态信息与利用SINS获得的相应信息进行比较，得到CNS的测量偏差，将其作为观测量送到SINS/CNS局部子滤波器进行滤波计算。利用全局滤波器将来自SINS/CNS局部子滤波器经滤波计算得到的航天器的姿态误差、位置误差和速度误差信息与来自SINS/SRS局部子滤波器计算得到的相应误差信息，送入全局滤波器进行多源融合自主组合导航系统滤波计算，可计算出导航误差全局最优估计值，从而校正SRS并获得当前精确的导航解。这些导航解用来校正系统误差，获得航天器高精度的导航信息。
SINS/SRS/CNS多源融合自主导航系统原理如图5所示。先设计SINS/SRS导航子滤波器和SINS/CNS导航子滤波器。
在SINS/SRS组合导航子滤波计算中，SINS为主系统，SRS为辅助系统，利用SINS可得到航天器三维连续速度、位置和姿态信息。将SRS得到的高精度的速度信息与SINS得到的速度信息之差，作为SINS/SRS导航子系统的速度量测信息，结合SINS/SRS导航子系统的状态信息，通过抗差自适应Kalman滤波算法，解算得到SINS的误差最优估计值，用来修正SINS的速度误差；
在SINS/CNS组合导航子滤波计算中，将SINS和CNS输出的姿态角信息之差作为SINS/CNS导航子系统的姿态角量测信息，将SINS与雷达高度表输出的高度信息之差作为高度量测信息，用来修正SINS的姿态角误差。
最后，利用抗差自适应Kalman滤波算法，通过计算获得组合导航系统状态的两组局部最优估计值
Figure BDA0003072958190000161
和局部最优误差协方差阵
Figure BDA0003072958190000162
然后，采用联邦滤波技术，将两组局部最优估计值送入主滤波器中进行信息融合，得到系统状态的全局最优估计值
Figure BDA0003072958190000163
和全局最优误差、协方差阵
Figure BDA0003072958190000164
最后，利用获得的状态的全局最优估计值
Figure BDA0003072958190000165
对捷联惯导系统的误差进行实时校正。
由于惯导系统高度通道发散，且SRS和CNS都不能输出载体的高度信息，因此采用雷达高度表输出载体的高度信息对SINS高度通道进行校正，以抑制SINS高度通道发散。SINS/SRS/CNS多元融合自主组合导航系统最优估计融合算法为
Figure BDA0003072958190000171
考虑如下非线性系统
Figure BDA0003072958190000172
式(19)中，xk∈Rn为k时刻系统的状态向量，zk∈Rn为量测输出，Wk∈Rn为系统噪声，其方差为Rk，Vk∈Rn为量测噪声，其方差为Qk，f(·)和h(·)是非线性函数,采样时间k＝0,1,…,N。
SINS/SRS导航子系统的量测方程和SINS/CNS导航子系统的量测方程的解算方法均采用抗差自适应Unscented粒子滤波方法；同时SINS/SRS/CNS多元融合自主组合导航系统最优估计融合算法也采用该方法。
抗差自适应Unscented粒子滤波方法包括以下步骤：
步骤S1、根据初始均值和均方差抽取N个粒子(即信息值)，在k＝0时刻，
Figure BDA0003072958190000173
i＝1,2,…,N，设置权值为
Figure BDA0003072958190000174
步骤S2、在k＝1,2,...M时刻，按照下面次序计算：
步骤S2.1、计算输入的量测量的等价权
Figure BDA0003072958190000175
和自适应因子α；其中，量测量为速度量测量、高度量测量或姿态角量测量。具体的选用IGG方案构造等价权函数，IGG法属于降权函数，即对量测值做抗差限制，若取其倒数，则定义为方差膨胀因子函数。
设等价权矩阵为
Figure BDA0003072958190000181
Figure BDA0003072958190000182
其中，k0和k1均为常值，k0∈(1,1.5)，k1∈(3,8)，Vk为所述量测量的残差向量，
Figure BDA0003072958190000183
Figure BDA0003072958190000184
为当前时刻载体的系统状态向量估计值，zk为所述量测量，Ak为量测矩阵。
在另一种实施例中，根据需要也可以采用另一种表达式
Figure BDA0003072958190000185
自适应因子选取如下：
Figure BDA0003072958190000186
其中，c0和c1均为常数，c0∈(1,1.5)，c1∈(3,8)，
Figure BDA0003072958190000187
tr(·)表示矩阵的迹，
Figure BDA0003072958190000188
为预报值，即
Figure BDA0003072958190000189
进而可以看出权函数和自适应因子构造形式基本相同，二者都是重要的调节因子.前者通过对残差的判断来选取，而后者根据状态估计值与预报值之差
Figure BDA00030729581900001810
来选取。
步骤S2.2、计算Sigma点，由UKF算法更新粒子
Figure BDA00030729581900001811
得到
Figure BDA00030729581900001813
且
Figure BDA00030729581900001812
满足
Figure BDA0003072958190000191
其中，
Figure BDA0003072958190000192
为(k-1)时刻的第i个信息值，
Figure BDA0003072958190000193
为
Figure BDA0003072958190000194
对应的协方差矩阵，
Figure BDA0003072958190000195
为k时刻的第i个信息值，
Figure BDA0003072958190000196
为
Figure BDA0003072958190000197
对应的协方差矩阵。
具体过程如下：
设新样本为
Figure BDA00030729581900001919
2N+1个Sigma点采样点为：
Figure BDA0003072958190000198
式(20)中，
Figure BDA0003072958190000199
为
Figure BDA00030729581900001910
的第j列向量，
Figure BDA00030729581900001911
表示矩阵
Figure BDA00030729581900001912
的第c列，λ＝α2(N+ν)，λ代表尺度因子，ν＝3-N，ν为二阶尺度因子，N为信息值个数，α决定信息值对预测均值的分散程度；
上述2N+1个Sigma采样点集对应的权值集合为：
Figure BDA00030729581900001913
其中，上述集合w的第j列表示第j个Sigma点的权值，满足∑wj＝1，wj表示w的第j列，j＝0,1,...,2N。
基于UKF算法，结合所述等价权
Figure BDA00030729581900001920
和自适应因子α对粒子进行预测和更新；预测和更新过程如下：
Figure BDA00030729581900001914
Figure BDA00030729581900001915
Figure BDA00030729581900001916
Figure BDA00030729581900001917
Figure BDA00030729581900001918
Figure BDA0003072958190000201
Figure BDA0003072958190000202
Figure BDA0003072958190000203
Figure BDA0003072958190000204
Figure BDA0003072958190000205
从而可以得到
Figure BDA0003072958190000206
再结合所述等价权
Figure BDA00030729581900002022
和自适应因子α计算
Figure BDA0003072958190000207
对应的信息值估值
Figure BDA0003072958190000208
和方差
Figure BDA0003072958190000209
即：
Figure BDA00030729581900002010
Figure BDA00030729581900002011
Figure BDA00030729581900002012
其中，上式表明通过因子αk和
Figure BDA00030729581900002013
可以影响和调节
Figure BDA00030729581900002014
使重要性密度函数更接近实际分布。上述三个公式得到的
Figure BDA00030729581900002015
作为粒子采样的重要性密度函数，再进行重要性采样。从上述第二个式子可以看出，当量测模型存在异常时，等价权矩阵
Figure BDA00030729581900002016
元素减小，参数估值时利用量测信息率减小，减弱了异常信息对估值的影响。反之，参数估值增加有用量测信息的利用率；同理，当状态模型存在异常时，自适应因子αk减小，参数估值时利用状态预测信息率减小，减弱模型异常的干扰，反之亦成立。若等价权
Figure BDA00030729581900002017
且α＝0时，
Figure BDA00030729581900002018
和
Figure BDA00030729581900002019
即为UKF算法得到的样本均值和方差。
步骤S2.3、计算权值
Figure BDA00030729581900002020
并归一化为
Figure BDA00030729581900002021
其中，zk为所述量测量，
Figure BDA0003072958190000211
为在已知
Figure BDA0003072958190000212
的条件下zk的条件概率密度函数，
Figure BDA0003072958190000213
为在已知
Figure BDA0003072958190000214
的条件下
Figure BDA0003072958190000215
的条件概率密度函数，
Figure BDA0003072958190000216
为在已知
Figure BDA0003072958190000217
的条件下
Figure BDA0003072958190000218
的条件概率密度函数。
其中，
Figure BDA0003072958190000219
Figure BDA00030729581900002110
表示k时刻的预测状态量，
Figure BDA00030729581900002111
Figure BDA00030729581900002112
表示状态和量测的互协方差，
Figure BDA00030729581900002113
表示预测量测协方差的逆，
Figure BDA00030729581900002114
为k时刻的预测量测量，
Figure BDA00030729581900002115
Figure BDA00030729581900002116
表示第j个Sigma样本点的预测量测量。
步骤S2.4、计算估计式
Figure BDA00030729581900002117
将所得结果与既定阙值进行比较，判断粒子退化的严重程度，
Figure BDA00030729581900002118
越小，表明退化越严重。在这种情况下，可对上面获得的后验密度进行重采样，重新得到M个新的粒子，并赋予各个粒子相同权值1/m。即当所得结果小于等于阈值，重复上述步骤S2.1-步骤S2.4，直至所得结果大于阈值，执行步骤S2.5。
步骤S2.5、计算非线性状态量估计值
Figure BDA00030729581900002119
及协方差估计值
Figure BDA00030729581900002120
其中，状态量估计值为第一估计值或第二估计值，
Figure BDA00030729581900002121
表示状态向量的估计值。
上述步骤在选取重要性密度函数时，利用了两个重要的调节因子，即等价权和自适应因子对UT变换后所得粒子采样点更合理的分配有用信息，为重要性采样过程提供更好的采样分布函数。
采用抗差自适应Unscented粒子滤波(RAUPF)算法对SINS/SRS/CNS多源融合自主组合导航系统进行滤波计算。首先，对独立导航系统SINS的数学模型进行仿真，根据仿真结果优化调整误差模型，同时针对各项误差进行误差补偿，提高SINS性能；其次，模拟仿真光谱红移测量方法，为光谱红移导航提供测量数据来源，并在此基础上对光谱红移导航的原理、模型和算法进行仿真验证，根据仿真结果优化该导航系统的模型和算法，SRS/SINS/CNS多原融合自主组合导航仿真实验系统方案如图6所示。
选取东北天(E，N，U)地理坐标系为导航坐标系，假设飞行器初始位置为北纬34.246°，东经108.997°，高度为2.5km，经过平飞、转弯、爬升和俯冲等机动飞行到达终点。飞行时间为1000s，平均飞行速度为107.5m/s，飞行轨迹如图7所示。
仿真过程中，SINS初始对准误差为0，初始速度误差为0.1m/s，初始位置误差为10m，初始姿态误差为10″；仿真中采用的传感器参数如表1所示。
表1仿真中采用的传感器参数指标
Figure BDA0003072958190000221
利用RAUPF算法对建立的SINS/SRS/CNS自主组合导航系统进行滤波解算，得到该组合导航系统输出的导航参数，并与事先设定的参考信息作差，获得SINS/SRS/CNS组合导航误差，同时与SINS、SINS/SRS子系统和SINS/CNS子系统误差进行对比，以验证SINS/SRS/CNS自主组合导航系统性能。
分别对SINS、SINS/SRS导航子系统和SINS/CNS导航子系统进行仿真计算，并对仿真结果进行比较。
对SINS、SINS/SRS导航子系统和SINS/CNS导航子系统的航向角误差、东向速度误差和纬度误差进行比较，仿真结果如图8～图11所示。如表2给出了各子导航系统误差的数值统计结果。
表2仿真结果比较
Figure BDA0003072958190000231
由仿真结果可以看出：
(1)SINS姿态误差、速度误差和位置误差随时间累积甚至发散，因此需要利用其他导航方式进行误差校正。
(2)对于SINS/CNS子系统，星敏感器可以获得较高的姿态精度，因此主要用来校正SINS的姿态误差；对于SINS/SRS组合导航系统，SRS能够获得较高的速度精度，因此校正后的速度精度较高。但是，单一的SINS/CNS组合导航系统无法满足飞行器自主组合导航定位的需求，因此需要结合两者的优点，设计SINS/SRS/CNS自主组合导航系统，以精确确定飞行器的位置。
(3)由光谱红移导航原理可以看出，光谱红移导航在利用由星敏感器获得的飞行器的姿态参数进行速度解算时，若星敏感器故障或受到遮挡，需要临时使用SINS提供的姿态数据进行速度解算以确保光谱红移导航继续工作。由图11可知，采用这种方法估计的误差完全可以满足飞行器对导航精度的需求，这说明该自主组合导航系统的抗干扰能力较强。
对SINS/SRS/CNS自主组合导航系统进行仿真计算，仿真结果如图12～图14所示。表3给出了自主组合导航系统误差的数值仿真结果。
表3SINS/SRS/CNS组合导航系统仿真结果
Figure BDA0003072958190000241
从对上述仿真结果进的分析中可知：本发明所设计的SINS/SRS/CNS自主组合导航系统，利用先进的信息融合技术，将SINS、SRS和CNS三者进行有机结合。该系统同时兼备了SINS、SRS和CNS各自的优点，克服了各单一导航系统的缺陷，在性能上实现了互补。SINS/SRS/CNS自主组合导航系统具有各单一导航系统所不具备的优良性能。组合导航系统的姿态精度达到1′以内，速度精度达到0.1m/s，定位精度3m以内，能满足飞行器对导航系统精度的需求，有效提高了导航系统的自主性、可靠性和精度。
综上可知，SINS、SRS和CNS是三个自主性很强的导航子系统，各具有自己的优点，但也存在各自的缺点。本发明基于太阳系天体光谱红移测速导航原理，利用太阳系自然天体的光谱红移信息、物体运动定律和自然天体的天文信息作为导航信息源，采用智能化技术，设计出一种具有完全自主性的SINS/SRS/CNS多源融合自主组合导航新方案。首先，研究了光谱红移自主导航的基本原理和算法；其次，设计出了SINS/SRS/CNS多源融合自主组合导航系统，给出了适合该组合导航系统解算的抗差自适应Unscented粒子滤波算法；最后，分别建立了自主组合导航子系统仿真实验和SINS/SRS/CNS多源融合自主组合导航仿真实验。实验结果表明：本发明所设计的SINS/SRS/CNS自主组合导航系统，不但自主性强，可靠性高，而且能满足飞行器对导航系统精度的需求。

Claims (8)

1.一种SINS/SRS/CNS多源融合自主组合导航方法，其特征在于，包括以下步骤：
构建SINS/SRS导航子系统，确定所述SINS/SRS导航子系统的速度量测量和高度量测量；
结合所述速度量测量和高度量测量构建所述SINS/SRS导航子系统的量测方程并解算，得出第一估计值；
构建SINS/CNS导航子系统，确定所述SINS/CNS导航子系统的姿态角量测量；
基于所述姿态角量测量构建SINS/CNS导航子系统的量测方程并解算，得出第二估计值；
融合所述第一估计值和所述第二估计值，得到SINS/SRS/CNS导航系统的全局最优估计值，并利用所述全局最优估计值对SINS的误差进行实时校正。
2.如权利要求1所述的一种SINS/SRS/CNS多源融合自主组合导航方法，其特征在于，确定所述SINS/SRS导航子系统的速度量测量和高度量测量包括：
通过SINS获取载体的第一速度信息，通过SRS获取载体的第二速度信息；计算所述第一速度信息和所述第二速度信息的速度差信息，并将所述速度差信息作为SINS/SRS导航子系统的速度量测量；
通过SINS获取所述载体的第一高度信息，通过高度量测元件获取载体的第二高度信息；计算所述第一高度信息和第二高度信息的高度差信息，并将所述高度差信息作为所述SINS/SRS导航子系统的高度量测量。
3.如权利要求2所述的一种SINS/SRS/CNS多源融合自主组合导航方法，其特征在于，确定所述SINS/CNS导航子系统的姿态角量测量包括：
通过SINS获取载体的第一姿态角信息，通过CNS获取载体的第二姿态角信息；计算所述第一姿态角信息和第二姿态角信息的姿态角差信息，并将所述姿态角差信息作为SINS/CNS导航子系统的姿态角量测量。
4.如权利要求2或3所述的一种SINS/SRS/CNS多源融合自主组合导航方法，其特征在于，所述SINS/SRS导航子系统的量测方程为：
Figure FDA0003072958180000021
其中，Z1(t)为第一估计值，Hv为速度测量矩阵，
Figure FDA0003072958180000022
Hh为高度测量矩阵，
Figure FDA0003072958180000023
X(t)为系统状态向量，Vv(t)为速度量测噪声，Vh(t)为高度量测噪声，H1(t)为所述SINS/SRS导航子系统的量测矩阵，V1(t)为所述SINS/SRS导航子系统的量测噪声。
5.如权利要求4所述的一种SINS/SRS/CNS多源融合自主组合导航方法，其特征在于，所述SINS/CNS导航子系统的量测方程为：
Z2(t)＝H2(t)X(t)+V2(t)，
其中，Z2(t)为第二估计值，H2(t)为所述SINS/CNS导航子系统的量测矩阵，V2(t)为所述SINS/CNS导航子系统的量测噪声。
6.如权利要求5所述的一种SINS/SRS/CNS多源融合自主组合导航方法，其特征在于，所述SINS/SRS导航子系统的量测方程和所述SINS/CNS导航子系统的量测方程的解算方法均采用抗差自适应Unscented粒子滤波方法；
所述抗差自适应Unscented粒子滤波方法包括以下步骤：
步骤S1、采用粒子算法生成k＝0时刻的N个信息值
Figure FDA0003072958180000024
并为每个信息值赋予权值
Figure FDA0003072958180000025
其中，N为正整数，i＝1,2....,N，
Figure FDA0003072958180000026
步骤S2、在k＝1,2,...M时刻，按照下面次序计算：
步骤S2.1、计算输入的量测量的等价权P和自适应因子α；其中，所述量测量为所述速度量测量、所述高度量测量或所述姿态角量测量；
步骤S2.2、采用UKF算法更新信息值
Figure FDA0003072958180000031
得到
Figure FDA0003072958180000032
并结合所述等价权
Figure FDA0003072958180000033
和自适应因子α计算
Figure FDA0003072958180000034
对应的信息值估值
Figure FDA0003072958180000035
和方差
Figure FDA0003072958180000036
其中，
Figure FDA0003072958180000037
为(k-1)时刻的第i个信息值，
Figure FDA0003072958180000038
为
Figure FDA0003072958180000039
对应的协方差矩阵，
Figure FDA00030729581800000310
为k时刻的第i个信息值，
Figure FDA00030729581800000311
为
Figure FDA00030729581800000312
对应的协方差矩阵；
步骤S2.3、计算权值
Figure FDA00030729581800000313
并归一化为
Figure FDA00030729581800000314
其中，zk为所述量测量，
Figure FDA00030729581800000315
为在已知
Figure FDA00030729581800000316
的条件下zk的条件概率密度函数，
Figure FDA00030729581800000317
为在已知
Figure FDA00030729581800000318
的条件下
Figure FDA00030729581800000319
的条件概率密度函数，
Figure FDA00030729581800000320
为在已知
Figure FDA00030729581800000321
的条件下
Figure FDA00030729581800000322
的条件概率密度函数；
其中，
Figure FDA00030729581800000323
Figure FDA00030729581800000324
表示k时刻的预测状态量，
Figure FDA00030729581800000325
Figure FDA00030729581800000326
表示状态和两侧的互协方差，
Figure FDA00030729581800000327
表示预测量测协方差的逆，
Figure FDA00030729581800000328
为k时刻的预测量测量；
步骤S2.4、计算估计式
Figure FDA00030729581800000329
将所得结果与阈值进行比较，当所得结果小于等于阈值，重复上述步骤S2.1-步骤S2.4，直至所得结果大于阈值，执行步骤S2.5；
步骤S2.5、计算状态量估计值
Figure FDA00030729581800000330
及协方差估计值
Figure FDA00030729581800000331
其中，所述状态量估计值为所述第一估计值或第二估计值。
7.如权利要求6所述的一种SINS/SRS/CNS多源融合自主组合导航方法，其特征在于，计算输入的量测量的等价权
Figure FDA0003072958180000041
和自适应因子α具体包括：
设等价权矩阵为
Figure FDA0003072958180000042
Figure FDA0003072958180000043
其中，k0和k1均为常值，k0∈(1,1.5)，k1∈(3,8)，Vk为所述量测量的残差向量，
Figure FDA0003072958180000044
Figure FDA0003072958180000045
为当前时刻载体的系统状态向量估计值，zk为所述量测量，Ak为量测矩阵；
自适应因子选取如下：
Figure FDA0003072958180000046
其中，c0和c1均为常数，c0∈(1,1.5)，c1∈(3,8)，
Figure FDA0003072958180000047
tr(·)表示矩阵的迹，
Figure FDA0003072958180000048
为预报值。
8.如权利要求6所述的一种SINS/SRS/CNS多源融合自主组合导航方法，其特征在于，计算输入的量测量的等价权
Figure FDA0003072958180000049
和自适应因子α具体包括：
设等价权矩阵为
Figure FDA00030729581800000410
Figure FDA00030729581800000411
自适应因子选取如下：
Figure FDA0003072958180000051
其中，c0和c1均为常数，c0∈(1,1.5)，c1∈(3,8)，
Figure FDA0003072958180000052
tr(·)表示矩阵的迹，
Figure FDA0003072958180000053
为预报值。
CN202110544188.6A 2021-05-19 2021-05-19 一种sins/srs/cns多源融合自主组合导航方法 Pending CN113324539A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110544188.6A CN113324539A (zh) 2021-05-19 2021-05-19 一种sins/srs/cns多源融合自主组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110544188.6A CN113324539A (zh) 2021-05-19 2021-05-19 一种sins/srs/cns多源融合自主组合导航方法

Publications (1)

Publication Number Publication Date
CN113324539A true CN113324539A (zh) 2021-08-31

Family

ID=77415981

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110544188.6A Pending CN113324539A (zh) 2021-05-19 2021-05-19 一种sins/srs/cns多源融合自主组合导航方法

Country Status (1)

Country Link
CN (1) CN113324539A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114370873A (zh) * 2022-01-19 2022-04-19 东南大学 一种基于改进ukf的星光折射cns/sins组合导航方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107589748A (zh) * 2017-08-21 2018-01-16 江苏科技大学 基于UnscentedFastSLAM算法的AUV自主导航方法
CN111156986A (zh) * 2020-01-13 2020-05-15 西北工业大学 一种基于抗差自适应ukf的光谱红移自主组合导航方法
CN112179347A (zh) * 2020-09-18 2021-01-05 西北工业大学 一种基于光谱红移误差观测方程的组合导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107589748A (zh) * 2017-08-21 2018-01-16 江苏科技大学 基于UnscentedFastSLAM算法的AUV自主导航方法
CN111156986A (zh) * 2020-01-13 2020-05-15 西北工业大学 一种基于抗差自适应ukf的光谱红移自主组合导航方法
CN112179347A (zh) * 2020-09-18 2021-01-05 西北工业大学 一种基于光谱红移误差观测方程的组合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
高怡: "运载器组合导航高性能滤波算法研究", 中国博士学位论文全文数据库 (信息科技辑), 15 July 2015 (2015-07-15) *
高朝晖;慕德俊;魏文辉;高社生;张晶;: "光谱红移/SINS自主组合导航方法", 弹箭与制导学报, no. 04 *
高朝辉: "SINS/SRS/CNS自主导航系统设计与CKF算法拓展研究", 中国博士学位论文全文数据库 (工程科技Ⅱ辑), no. 04, pages 27 - 74 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114370873A (zh) * 2022-01-19 2022-04-19 东南大学 一种基于改进ukf的星光折射cns/sins组合导航方法
CN114370873B (zh) * 2022-01-19 2023-12-05 东南大学 一种基于改进ukf的星光折射cns/sins组合导航方法

Similar Documents

Publication Publication Date Title
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
Hasan et al. A review of navigation systems (integration and algorithms)
Fang et al. Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensation
CN111221018B (zh) 一种用于抑制海上多路径的gnss多源信息融合导航方法
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
RU2380656C1 (ru) Комплексированная бесплатформенная инерциально-спутниковая система навигации на "грубых" чувствительных элементах
KR20200011002A (ko) 항법 시스템
CN109708663B (zh) 基于空天飞机sins辅助的星敏感器在线标定方法
CN112378399A (zh) 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
Yoo et al. Improvement of TERCOM aided inertial navigation system by velocity correction
Wang et al. Attitude determination method by fusing single antenna GPS and low cost MEMS sensors using intelligent Kalman filter algorithm
Canciani et al. An analysis of the benefits and difficulties of aerial magnetic vector navigation
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
CN112525188B (zh) 一种基于联邦滤波的组合导航方法
Park et al. Implementation of vehicle navigation system using GNSS, INS, odometer and barometer
CN113324539A (zh) 一种sins/srs/cns多源融合自主组合导航方法
Turan et al. Particle filter studies on terrain referenced navigation
Gong et al. Airborne earth observation positioning and orientation by SINS/GPS integration using CD RTS smoothing
Omerbashich Integrated INS/GPS navigation from a popular perspective
Yu et al. SINS/CNS nonlinear integrated navigation algorithm for hypersonic vehicle

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