CN114370873B - 一种基于改进ukf的星光折射cns/sins组合导航方法 - Google Patents
一种基于改进ukf的星光折射cns/sins组合导航方法 Download PDFInfo
- Publication number
- CN114370873B CN114370873B CN202210062244.7A CN202210062244A CN114370873B CN 114370873 B CN114370873 B CN 114370873B CN 202210062244 A CN202210062244 A CN 202210062244A CN 114370873 B CN114370873 B CN 114370873B
- Authority
- CN
- China
- Prior art keywords
- star
- navigation
- error
- measurement
- refraction
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 238000005259 measurement Methods 0.000 claims abstract description 85
- 239000013598 vector Substances 0.000 claims abstract description 64
- 239000011159 matrix material Substances 0.000 claims abstract description 53
- 238000001914 filtration Methods 0.000 claims abstract description 10
- 230000003044 adaptive effect Effects 0.000 claims abstract description 9
- 230000004927 fusion Effects 0.000 claims abstract description 5
- 230000003287 optical effect Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 11
- 241000612151 Dodecatheon Species 0.000 claims description 6
- 235000008166 Dodecatheon meadia Nutrition 0.000 claims description 6
- 238000013256 Gubra-Amylin NASH model Methods 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 6
- 238000012549 training Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000003384 imaging method Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 230000003321 amplification Effects 0.000 claims description 3
- 238000013528 artificial neural network Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000003199 nucleic acid amplification method Methods 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 229940060587 alpha e Drugs 0.000 claims 1
- 230000008859 change Effects 0.000 abstract description 9
- 238000012546 transfer Methods 0.000 description 4
- 230000009466 transformation Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/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/02—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Automation & Control Theory (AREA)
- Life Sciences & Earth Sciences (AREA)
- Computing Systems (AREA)
- Astronomy & Astrophysics (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Operations Research (AREA)
- Probability & Statistics with Applications (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于改进UKF的星光折射CNS/SINS组合导航方法;首先建立了组合导航模型,利用捷联惯性导航系统的误差方程建立组合导航状态方程,引入了导航星星点方向矢量误差、折射视高度误差和运动学约束建立量测模型;其次提出了一种改进UKF方法,引入自适应因子修正量测噪声协方差矩阵跟踪实际量测噪声的变化,对星敏感器和惯性器件解算得到的导航信息进行信息融合,修正姿态、速度、位置结果,校正陀螺仪常值漂移和加速度计的常值偏置。本方法构建的量测方程利用导航星点矢量误差、折射视高度误差和运动学约束作为量测信息校正了陀螺和加速度计的误差,提出的自适应UKF方法减少了量测噪声变化导致滤波精度下降的问题,提高了星光折射CNS/SINS组合导航精度。
Description
技术领域
本发明属于组合导航技术领域,具体涉及一种基于改进UKF的星光折射CNS/SINS组合导航方法。
背景技术
传统CNS/SINS组合导航方法将天文导航与惯性导航的姿态信息相融合,但这种组合方法无法补偿加速度计误差,在长航时任务中,组合导航系统位置误差仍然发散。为实现弹道导弹等远程长航时飞行器的高精度导航,一般采用星光折射间接敏感地平的天文/惯性组合导航方法,校正陀螺仪和加速度计的误差。
然而传统星光折射间接敏感地平CNS/SINS组合导航利用星敏感器确定的姿态信息与惯性导航系统进行组合,要求观测恒星数目不少于3颗,且姿态精度受限于导航星的几何构型,极大限制了组合导航的连续性与精度。而标准UKF中设定量测噪声协方差矩阵Rk为常值,对复杂多变的环境干扰和观测星点数目构型变化的适应能力差。因此,进一步研究星光折射间接敏感地平的天文/惯性组合导航方法十分必要,需要合理选取量测量,并利用量测噪声矩阵自适应调整的UKF方法,提高CNS/SINS系统的导航精度。
发明内容
为解决上述问题,本发明公开了一种基于改进UKF的星光折射CNS/SINS组合导航方法,利用捷联惯性导航系统的误差方程建立组合导航状态方程,利用导航星星点方向矢量误差、折射视高度误差和运动学约束作为组合导航量测信息,设计自适应无迹卡尔曼滤波器自适应调整量测噪声矩阵,利用组合导航输出结果实现陀螺仪和加速度计的误差校正,提高了星光折射间接敏感地平的天文/惯性组合导航精度。
为达到上述目的,本发明的技术方案如下:
一种基于改进UKF的星光折射CNS/SINS组合导航方法,包括以下步骤:
步骤一:获取捷联惯性导航系统与天文导航系统中的导航信息;
步骤二:建立星光折射CNS/SINS组合导航系统的状态方程和量测方程;
步骤三:基于自适应无迹卡尔曼滤波方法进行组合导航的信息融合与误差校正。
进一步地,所述步骤一中,利用天文导航系统获取导航星星点方向矢量、折射星星点方向矢量、星敏感器光轴指向的导航信息;利用惯性导航系统获取载体的实时姿态、速度、位置的导航信息,具体如下:
天文导航系统利用一个大视场星敏感器同时观测导航星和折射星。导航过程中,星敏感器实时拍摄星图,通过星图识别与匹配,捕获得到导航星,计算获得星敏感器的光轴指向。设k时刻星敏感器的拍摄星图中识别出n颗导航星,在CCD成像平面内坐标为:(us1,vs1),(us2,vs2),…,(usn,vsn),则导航星在星敏感器坐标系下的方向矢量表示为:
其中,f为星敏感器的焦距。
根据星敏感器的光轴指向,依据导航星库,模拟当前时刻星敏感器拍摄到的未发生大气折射的星图。将非折射模拟星图与实际拍摄星图做星图匹配,通过计算模拟星图与实际拍摄星图中各颗星的欧氏距离,当距离最小值大于设定阈值时,判定该星为折射星,达到识别折射星的目的。
设星图中识别出的n颗折射星在CCD成像平面内坐标为:(ur1,vr1),(ur2,vr2),…,(urn,vrn),则折射星在星敏感器坐标系下的方向矢量表示为:
由此利用星敏感器完成了导航星星点方向矢量、折射星星点方向矢量、星敏感器光轴指向导航信息的获取。
进一步地,所述步骤二中,利用捷联惯性导航系统的误差方程建立组合导航状态方程;利用导航星星点方向矢量误差、折射视高度误差和运动学约束建立CNS/SINS系统的组合导航量测方程。
(1)CNS/SINS系统组合导航状态方程
星光折射CNS/SINS组合导航系统状态方程选用捷联惯性导航系统的误差方程,表示如下:
式中,[Φx,Φy,Φz]T代表发射点惯性坐标系下的数学平台失准角,[δvx,δvy,δvz]T代表速度误差,[δx,δy,δz]T代表位置误差,[εx,εy,εz]T代表陀螺仪随机常值漂移,代表加速度计随机常值偏置。系统状态矩阵F和系统噪声输出矩阵G分别表示为:
其中,矩阵Fa中各系数为引力加速度对位置坐标的导数,随导弹位置的变化而变化;Fb中ai为加速度计敏感到的视加速度,为由弹体系到发射点惯性系的姿态转移矩阵。
系统噪声矩阵为:式中为陀螺仪的随机噪声,为加速度计的随机噪声。系统噪声方差阵:
(2)CNS/SINS系统组合导航量测方程
本发明中星光折射CNS/SINS组合导航系统采用导航星星点方向矢量误差、折射视高度误差和运动学约束作为量测量,系统量测模型建立组成如下:
1)导航星星点方向矢量误差量测方程的建立
采用基于GAN神经网络的星点矢量预测方法,利用惯性导航姿态矩阵估计星点矢量预测值。利用导航星库建立一个已知姿态的星图数据集,对星点矢量预测GAN模型进行离线训练,并在组合导航过程中,每隔一段时间利用组合导航系统获得的姿态数据和天文导航拍摄的星图数据进一步训练GAN网络。导航过程中星敏感器数据有效的时刻,根据惯性导航实时解算的姿态矩阵借助星点矢量预测GAN模型获得当前时刻的星点预测矢量。
设k时刻导航星星敏感器拍摄星图中识别出n颗星,星敏感器中实际测量到的星点矢量ls=[ls1,ls2,…,lsn]、惯性导航姿态阵估计的星点矢量预测值在导航系下分别表示为:
其中,Φ为数学平台失准角,lni为导航系下第i颗导航星的星点方向矢量。
导航星i的星点方向矢量误差表示为:
因此,导航星星点方向矢量误差量测方程表示为:
其中,观测矩阵量测噪声矩阵
设导航星星点像素坐标(u,v)的量测噪声方差阵为R0,则由公式(1)公式(3)获得第i颗导航星星点方向矢量与星点像素坐标的关系:
lsi=fs(ui,vi)
则量测噪声矩阵V1的方差阵Rk1随着量测值的输入实时更新,表示为:
2)折射星折射视高度误差量测方程的建立
星光通过大气层时发生折射,光线向地心方向偏折。20km-50km连续高度变化的大气模型中,真实折射视高度ha表示为:
hat=hac+va=57.081+2.531e[0.981ln(R)-8.689]-6.441ln(R)+va (6)
根据几何关系,导弹上星敏感器观测到的折射光线相对地球的视高度ha表示为:
其中,R为星光折射角,表示折射前后星光方向矢量之间的夹角;表示导弹位置矢量r在折射星发生折射前的星光方向矢量上的投影,其中惯性系下的折射前的星光方向矢量根据导航星库获得;Re为地球半径;a为小量,一般忽略。
折射视高度误差表示为:
δha=hat-ha=hac-ha+va (8)
根据公式(7),折射视高度误差改写为:
式中,δr、δu均为位置误差δx、δy、δz三项的组合,式(9)改写做:
根据公式(8)和(10)折射视高度误差量测方程表示为:
其中不是白噪声,利用量测扩增方法对公式(11)进行处理使得量测噪声阵满足白噪声条件,得到折射视高度误差的量测方程:
其中va是大气密度误差和量测误差引起的视高度误差,已知大气密度百分比误差和折射角误差对视高度误差的影响,根据下式所示的折射视高度误差表达式能够实时更新va高斯白噪声的方差阵:
3)运动学约束量测方程的建立
远程弹道导弹自由段飞行时期,导弹通常状态下只受地球引力作用,处于完全失重状态,理论上加速度计输出值应为零。因此,此时加速度计的实际输出值反映了加速度计的常值偏置等误差的特性,满足:
其中,fb表示载体系下的理论加速度值,在自由段内一般fb=0;为加速度计的常值偏置误差;δfb表示加速度计其他造成实际输出值非零的误差项。
因此,远程弹道导弹的自由段中,当导弹完全失重状态时,根据运动学约束建立量测方程,表示为:
其中,观测矩阵H3=[03×12 I3×3],量测噪声矩阵V3,与加速度计除常值偏置外的误差有关。
进一步地,所述步骤三中,采用自适应无迹卡尔曼滤波方法对组合导航进行信息融合。
1.标准UKF算法
UKF算法利用UT变换处理状态一步预测估计和量测估计中均值与协方差的非线性传递,对状态方程和量测方程非线性的组合导航系统具有较高的滤波估计精度。组合导航非线性系统模型表示为:
式中,Xk和Zk分别为k时刻系统状态估计量和系统量测量;fk和hk分别为k时刻系统状态转移函数和系统量测函数;Wk和Vk分别为k时刻系统噪声和量测噪声,采用零均值高斯白噪声模型,噪声协方差矩阵分别为Qk和Rk,且满足
UKF算法的滤波计算步骤如下:
(1)参数初始化
初始化状态变量方差阵
(2)设状态变量维数n,通过UT变换获得2n+1个Sigma采样点。k-1时刻样本Sigma点和相应权值Wi为:
其中比例因子比例因子λ=α2(n+κ)-n;系数αò(0,1];系数β≥0在正态分布下一般取β=2;系数κ在状态变量多变量的高斯分布情况下取κ=0。
(3)计算k时刻一步预测状态Sigma点集的均值和方差:
则k时刻一步预测样本Sigma点表示为:
(4)根据一步预测状态Sigma点集,在获取可靠量测量的k时刻,计算量测估计Sigma点集的均值和方差:
ηi,k/k-1=hk(χi,k/k-1)
状态一步预测与量测估计的误差协方差表示为:
(5)获得状态预测和量测估计点集的均值与方差后,计算滤波增益矩阵:
(6)利用滤波增益矩阵修正状态量误差,获得k时刻状态估计值和状态估计均方差阵,完成k时刻的滤波更新:
标准UKF中设定量测噪声方差矩阵Rk为常值,对复杂多变的环境干扰和观测星点数目构型变化的适应能力差。本发明在组合导航过程中自适应修正导航星星点方向矢量和折射视高度误差的量测噪声方差矩阵Rk,使其跟踪实际测量噪声的变化。
残差εk指k时刻的系统量测值Zk与量测估计值之差,表示为:
在最优情况下,残差协方差理论值为:
由于不同时刻观测矩阵Zk维数不同,本发明中残差协方差实际值的迹表示为:
其中n为估计窗口大小,i0=k-n+1,vi表示i时刻残差εi的维数,vk表示k时刻残差εk的维数。
对UKF算法引入自适应调整因子ρk调节系统噪声协方差矩阵Rk,引入自适应因子后量测噪声方差阵改写为:
以作为参考,采用模糊推理方法得到自适应因子,使得系统测量噪声实际值大于给定值时,ρk增大;使得系统测量噪声实际值小于给定值时,ρk减小;使保持在1附近。
基于上述方法,通过自适应无迹卡尔曼滤波估计状态量并反馈给惯性导航系统,达到估计陀螺仪误差和加速度计误差的目的,实现对组合导航系统姿态、位置误差的校正,提高星光折射CNS/SINS组合导航系统的导航精度。
本发明的有益效果为:
本发明公开了一种基于改进UKF的星光折射CNS/SINS组合导航方法,与现有技术相比,利用导航星星点方向矢量误差作为量测,减少了观测星的数目与构型对导航的限制;利用视高度误差与运动学约束信息实现对加速度计误差的修正;采用自适应UKF方法调节系统噪声协方差矩阵Rk,使其跟踪实际量测噪声的变化,从而提高了星光折射CNS/SINS组合导航的精度。
附图说明
图1为一种基于改进UKF的星光折射CNS/SINS组合导航方法的结构示意图;
图2为本发明中导航星与折射星的光路示意图。
具体实施方式
下面结合附图和具体实施方式,进一步阐明本发明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
如图1所示,本发明提供了一种基于改进UKF的星光折射CNS/SINS组合导航方法,利用捷联惯性导航系统的误差方程建立组合导航状态方程,利用导航星星点方向矢量误差、折射视高度误差和运动学约束作为组合导航量测信息,设计自适应无迹卡尔曼滤波器自适应调整量测噪声矩阵,对陀螺仪和加速度计误差进行校正,提高了星光折射CNS/SINS组合导航精度。下面进行详细分析:
步骤一:获取捷联惯性导航系统与天文导航系统中的导航信息。利用惯性导航系统获取实时姿态、速度、位置信息;利用天文导航系统获取导航星与折射星的星点方向矢量、星敏感器光轴指向的导航信息。
如图2所示,星光通过大气层时发生折射,光线向地心方向偏折。天文导航系统利用一个大视场星敏感器同时观测导航星和折射星,利用捕获的导航星,计算星敏感器光轴指向。设星敏感器识别出n颗导航星,在成像平面内坐标为:(us1,vs1),(us2,vs2),…,(usn,vsn),则导航星在星敏感器坐标系下的方向矢量为:
根据星敏感器的光轴指向,依据导航星库,模拟当前时刻星敏感器拍摄到的未发生大气折射的星图。将非折射模拟星图与实际拍摄星图做星图匹配,计算模拟星图与实际拍摄星图中各颗星的欧氏距离,当距离最小值大于设定阈值时,判定该星为折射星。
步骤二:建立星光折射CNS/SINS组合导航系统的状态方程和量测方程。利用捷联惯性导航系统的误差方程建立组合导航状态方程;利用导航星星点方向矢量误差、折射视高度误差和运动学约束建立CNS/SINS系统的组合导航量测方程。
(1)CNS/SINS系统组合导航状态方程
星光折射CNS/SINS组合导航系统状态方程选用捷联惯性导航系统的误差方程,表示如下:
式中,[Φx,Φy,Φz]T代表发射点惯性坐标系下的数学平台失准角,[δvx,δvy,δvz]T代表速度误差,[δx,δy,δz]T代表位置误差,[εx,εy,εz]T代表陀螺仪随机常值漂移,代表加速度计随机常值偏置;系统噪声矩阵其中和分别表示陀螺仪和加速度计的随机噪声;方差阵
(2)CNS/SINS系统组合导航量测方程
本发明采用导航星星点方向矢量误差、折射视高度误差和运动学约束作为量测量,系统量测模型建立如下:
1)导航星星点方向矢量误差量测方程的建立
采用基于GAN神经网络的星点矢量预测方法,利用惯性导航姿态矩阵估计星点矢量预测值。利用导航星库建立一个已知姿态的星图数据集,对星点矢量预测GAN模型进行离线训练,并在组合导航过程中,每隔一段时间利用组合导航系统获得的姿态数据和天文导航拍摄的星图数据进一步训练GAN网络。导航过程中星敏感器数据有效的时刻,根据惯性导航实时解算的姿态矩阵借助星点矢量预测GAN模型获得当前时刻的星点预测矢量。
设星敏感器识别出n颗导航星,则实际星点矢量ls=[ls1,ls2,…,lsn]、惯性导航姿态阵估计的星点矢量预测值在导航系下分别为:
其中,Φ为数学平台失准角,lni为导航系下第i颗导航星的星点方向矢量,导航星i的星点方向矢量误差为:
因此,导航星星点方向矢量误差量测方程表示为:
其中,观测矩阵量测噪声矩阵V1的方差阵Rk1随着量测值的输入实时更新。
2)折射星折射视高度误差量测方程的建立
20km-50km连续高度变化的大气模型中,真实折射视高度ha表示为:
hat=hac+va=57.081+2.531e[0.981ln(R)-8.689]-6.441ln(R)+va
根据几何关系,导弹上星敏感器观测到的折射光线相对地球的视高度ha表示为:
其中,R为星光折射角;表示导弹位置矢量r在折射星发生折射前的星光方向矢量上的投影,根据导航星库获得;Re为地球半径;a为可忽略小量。
折射视高度误差:
式中,δr、δu为位置误差δx、δy、δz的组合,则折射视高度误差量测表示为:
利用量测扩增方法处理使量测噪声满足白噪声条件,得到折射视高度误差量测:
其中根据下式所示的表达式实时更新va高斯白噪声方差阵:
3)运动学约束量测方程的建立
自由段中导弹通常只受地球引力作用,处于完全失重状态,理论加速度计输出值应为零。此时加速度计的实际输出值反映了加速度计的常值偏置等误差的特性,满足:
其中,fb表示载体系下的理论加速度值,在自由段内一般fb=0;为加速度计的常值偏置误差;δfb表示加速度计其他造成实际输出值非零的误差项。
因此,自由段中导弹完全失重状态时,根据运动学约束建立量测方程:
其中,观测矩阵H3=[03×12 I3×3],量测噪声矩阵V3与加速度计的误差有关。
步骤三:基于自适应无迹卡尔曼滤波方法进行组合导航的信息融合与误差校正。
UKF算法利用UT变换处理状态一步预测估计和量测估计中均值与协方差的非线性传递,计算步骤如下:
(1)初始化状态变量方差阵
(2)设状态变量维数n,通过UT变换获得2n+1个Sigma采样点。k-1时刻样本Sigma点和相应权值Wi为:
其中比例因子λ=α2(n+κ)-n;系数αò(0,1];系数β≥0在正态分布下取β=2;系数κ取κ=0。
(3)计算k时刻一步预测状态Sigma点集的均值和方差:
则k时刻一步预测样本Sigma点表示为:
(4)在获取可靠量测量的k时刻,计算量测估计点集的均值、方差以及误差协方差:
ηi,k/k-1=hk(χi,k/k-1)
(5)获得状态预测和量测估计点集的均值与方差后,计算滤波增益矩阵:
(6)完成k时刻的滤波更新:
自适应修正导航星星点方向矢量和折射视高度误差的量测噪声方差矩阵Rk,跟踪实际测量噪声的变化。残差εk表示为:
最优情况下,残差协方差理论值为:
由于不同时刻观测矩阵Zk维数不同,本发明中残差协方差实际值的迹为:
其中n为估计窗口大小,i0=k-n+1,vi和vk分别表示i和k时刻残差εk的维数。
对UKF算法引入自适应调整因子ρk调节系统噪声方差矩阵Rk,引入自适应因子后量测噪声方差阵改写为:
以作为参考,采用模糊推理方法得到自适应因子,使得系统测量噪声实际值大于给定值时,ρk增大;使得系统测量噪声实际值小于给定值时,ρk减小;使保持在1附近。
利用自适应无迹卡尔曼滤波估计状态量并反馈给惯性导航系统,修正陀螺仪和加速度计误差,校正组合导航系统姿态、位置误差。
需要说明的是,以上内容仅仅说明了本发明的技术思想,不能以此限定本发明的保护范围,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰均落入本发明权利要求书的保护范围之内。
Claims (3)
1.一种基于改进UKF的星光折射CNS/SINS组合导航方法,其特征在于,包括以下步骤:
步骤一:获取捷联惯性导航系统与天文导航系统中的导航信息;
步骤二:建立星光折射CNS/SINS组合导航系统的状态方程和量测方程;
具体如下:
(1)CNS/SINS系统组合导航状态方程;
利用捷联惯性导航系统误差方程建立星光折射CNS/SINS组合导航系统状态方程,表示如下:
式中,[Φx,Φy,Φz]T代表发射点惯性坐标系下的数学平台失准角,[δvx,δvy,δvz]T代表速度误差,[δx,δy,δz]T代表位置误差,[εx,εy,εz]T代表陀螺仪随机常值漂移,代表加速度计随机常值偏置;系统噪声矩阵其中和分别表示陀螺仪和加速度计的随机噪声;方差阵
(2)CNS/SINS系统组合导航量测方程;
1)导航星星点方向矢量误差量测方程的建立;
设星敏感器识别出n颗导航星,则实际星点矢量ls=[ls1,ls2,…,lsn],星点矢量预测值在导航系下分别为:
其中,Φ为数学平台失准角,lni为导航系下第i颗导航星的星点方向矢量,导航星i的星点方向矢量误差为:
因此,导航星星点方向矢量误差量测方程表示为:
其中,观测矩阵量测噪声矩阵V1的方差阵Rk1随着量测值的输入实时更新;
2)折射星折射视高度误差量测方程的建立;
20km-50km连续高度变化的大气模型中,真实折射视高度ha表示为:
hat=hac+va=57.081+2.531e[0.981ln(R)-8.689]-6.441ln(R)+va
根据几何关系,导弹上星敏感器观测到的折射光线相对地球的视高度ha表示为:
其中,R为星光折射角;表示导弹位置矢量r在折射星发生折射前的星光方向矢量上的投影,根据导航星库获得;Re为地球半径;a为小量,此处忽略不计;
折射视高度误差:
式中,δr、δu为位置误差δx、δy、δz的组合,则折射视高度误差量测表示为:
利用量测扩增方法处理使量测噪声满足白噪声条件,得到折射视高度误差量测:
其中根据下式所示的表达式实时更新va高斯白噪声方差阵:
3)运动学约束量测方程的建立;
远程弹道导弹自由段飞行时期,导弹通常状态下只受地球引力作用,处于完全失重状态,理论上加速度计输出值应为零,因此,此时加速度计的实际输出值反映了加速度计的常值偏置误差的特性,满足:
其中,fb表示载体系下的理论加速度值,在自由段内fb=0;为加速度计的常值偏置误差;δfb表示加速度计其他造成实际输出值非零的误差项;
因此,自由段中导弹完全失重状态时,根据运动学约束建立量测方程:
其中,观测矩阵H3=[03×12 I3×3],量测噪声矩阵V3与加速度计的误差有关;
步骤三:基于自适应无迹卡尔曼滤波方法进行组合导航的信息融合与误差校正
计算步骤如下:
(1)初始化状态变量方差阵
(2)设状态变量维数n,通过UT变换获得2n+1个Sigma采样点;k-1时刻样本Sigma点和相应权值Wi为:
其中比例因子λ=α2(n+κ)-n;系数α∈(0,1];系数β≥0在正态分布下取β=2;系数κ取κ=0;
(3)计算k时刻一步预测状态Sigma点集的均值和方差:
k时刻一步预测样本Sigma点表示为:
(4)在获取可靠量测量的k时刻,计算量测估计Sigma点集的均值,以及状态一步预测与量测估计的误差协方差:
ηi,k/k-1=hk(χi,k/k-1)
(5)利用残差获取自适应调整因子,引入量测估计Sigma点集的方差计算,具体步骤如下:
残差εk表示为:
最优情况下,残差协方差理论值为:
由于不同时刻观测矩阵Zk维数不同,残差协方差实际值的迹为:
其中N为估计窗口大小,i0=k-n+1,vi和vk分别表示i和k时刻残差εk的维数;
以作为参考,采用模糊推理方法得到自适应因子,使得系统测量噪声实际值大于给定值时,ρk增大;系统测量噪声实际值小于给定值时,ρk减小;使保持在1附近;
为调节系统噪声协方差矩阵Rk,将自适应调整因子ρk引入量测噪声方差阵的计算:
(6)获得状态预测和量测估计点集的均值与方差后,计算滤波增益矩阵:
(7)完成k时刻的滤波更新:
利用自适应无迹卡尔曼滤波估计状态量并反馈给惯性导航系统,修正陀螺仪误差和加速度计误差,校正组合导航系统姿态、位置误差。
2.根据权利要求1所述的一种基于改进UKF的星光折射CNS/SINS组合导航方法,其特征在于,所述步骤一中,利用惯性导航系统实时获取姿态、速度、位置信息;利用天文导航系统获取导航星与折射星的星点方向矢量、星敏感器光轴指向的导航信息,天文导航信息获取具体如下:
天文导航系统利用一个大视场星敏感器同时观测导航星和折射星,利用捕获的导航星,计算星敏感器光轴指向,设星敏感器识别出n颗导航星,在成像平面内坐标为:(us1,vs1),(us2,vs2),…,(usn,vsn),则导航星在星敏感器坐标系下的方向矢量为:
根据星敏感器的光轴指向,依据导航星库,模拟当前时刻星敏感器拍摄到的未发生大气折射的星图,将非折射模拟星图与实际拍摄星图做星图匹配,计算模拟星图与实际拍摄星图中各颗星的欧氏距离,当距离最小值大于设定阈值时,判定该星为折射星。
3.根据权利要求1所述的一种基于改进UKF的星光折射CNS/SINS组合导航方法,其特征在于,利用导航星星点方向矢量误差建立CNS/SINS组合导航量测方程,
在利用惯性导航姿态阵对星点矢量预测值进行估计的过程中,采用了基于GAN神经网络的星点矢量预测方法;利用导航星库建立一个已知姿态的星图数据集,对星点矢量预测GAN模型进行离线训练;并在CNS/SINS组合导航过程中,每隔一段时间利用组合导航系统获得的姿态数据和天文导航拍摄星图数据进一步地训练GAN网络;导航过程中星敏感器获取数据有效的时刻,根据惯性导航实时解算得到的姿态矩阵借助星点矢量预测GAN模型获得当前时刻的星点预测矢量,提高星点预测矢量的估计精度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210062244.7A CN114370873B (zh) | 2022-01-19 | 2022-01-19 | 一种基于改进ukf的星光折射cns/sins组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210062244.7A CN114370873B (zh) | 2022-01-19 | 2022-01-19 | 一种基于改进ukf的星光折射cns/sins组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114370873A CN114370873A (zh) | 2022-04-19 |
CN114370873B true CN114370873B (zh) | 2023-12-05 |
Family
ID=81146105
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210062244.7A Active CN114370873B (zh) | 2022-01-19 | 2022-01-19 | 一种基于改进ukf的星光折射cns/sins组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114370873B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114923480B (zh) * | 2022-05-12 | 2024-08-13 | 中国人民解放军国防科技大学 | 弹载捷联惯性与星光折射全紧组合导航方法及装置 |
CN115638796A (zh) * | 2022-09-19 | 2023-01-24 | 北京控制工程研究所 | 一种基于折射星/非折射星信息融合与预测的快速星图识别方法 |
CN115342820A (zh) * | 2022-10-18 | 2022-11-15 | 深圳市诚王创硕科技有限公司 | 一种汽车夜间行驶导航定位系统 |
CN117367431B (zh) * | 2023-10-23 | 2024-05-14 | 哈尔滨工业大学(威海) | 带未知量测偏置的mems与uwb紧组合定位方法及系统 |
CN118052963B (zh) * | 2024-04-16 | 2024-06-25 | 山东金东数字创意股份有限公司 | 一种降低xr长时间误差积累的方法、介质及系统 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108592945A (zh) * | 2018-03-27 | 2018-09-28 | 中国人民解放军国防科技大学 | 一种惯性/天文组合系统误差的在线标定方法 |
CN111707259A (zh) * | 2020-06-16 | 2020-09-25 | 东南大学 | 一种校正加速度计误差的sins/cns组合导航方法 |
CN113324539A (zh) * | 2021-05-19 | 2021-08-31 | 西北工业大学深圳研究院 | 一种sins/srs/cns多源融合自主组合导航方法 |
CN113551668A (zh) * | 2021-07-21 | 2021-10-26 | 北京航空航天大学 | 一种航天器惯性/恒星星光矢量/星光折射组合导航方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11168984B2 (en) * | 2019-02-08 | 2021-11-09 | The Boeing Company | Celestial navigation system and method |
-
2022
- 2022-01-19 CN CN202210062244.7A patent/CN114370873B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108592945A (zh) * | 2018-03-27 | 2018-09-28 | 中国人民解放军国防科技大学 | 一种惯性/天文组合系统误差的在线标定方法 |
CN111707259A (zh) * | 2020-06-16 | 2020-09-25 | 东南大学 | 一种校正加速度计误差的sins/cns组合导航方法 |
CN113324539A (zh) * | 2021-05-19 | 2021-08-31 | 西北工业大学深圳研究院 | 一种sins/srs/cns多源融合自主组合导航方法 |
CN113551668A (zh) * | 2021-07-21 | 2021-10-26 | 北京航空航天大学 | 一种航天器惯性/恒星星光矢量/星光折射组合导航方法 |
Non-Patent Citations (2)
Title |
---|
惯性+星光折射定位组合导航技术研究;胡楠希等;《导航定位与授时》;第8卷(第1期);第90-95页 * |
星光折射辅助惯性/天文紧组合高精度导航方法;王鼎杰等;《中国惯性技术学报》;第28卷(第1期);第74-81页 * |
Also Published As
Publication number | Publication date |
---|---|
CN114370873A (zh) | 2022-04-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114370873B (zh) | 一种基于改进ukf的星光折射cns/sins组合导航方法 | |
CN111156987B (zh) | 基于残差补偿多速率ckf的惯性/天文组合导航方法 | |
CN110501024B (zh) | 一种车载ins/激光雷达组合导航系统的量测误差补偿方法 | |
CN111707259B (zh) | 一种校正加速度计误差的sins/cns组合导航方法 | |
CN109612471B (zh) | 一种基于多传感器融合的运动体姿态解算方法 | |
CN107655485B (zh) | 一种巡航段自主导航位置偏差修正方法 | |
CN111060097B (zh) | 一种提高位置误差估计精度的惯性/天文组合导航方法 | |
CN111102981B (zh) | 一种基于ukf的高精度卫星相对导航方法 | |
CN112629538A (zh) | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 | |
CN108592914A (zh) | 无gps情况下的复杂区域巡视机器人定位、导航及授时方法 | |
CN113551668B (zh) | 一种航天器惯性/恒星星光矢量/星光折射组合导航方法 | |
CN112504275B (zh) | 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法 | |
CN111238469B (zh) | 一种基于惯性/数据链的无人机编队相对导航方法 | |
CN112179334B (zh) | 基于两步Kalman滤波的星光导航方法及系统 | |
CN109633720B (zh) | 基于视频卫星的地面运动目标测量方法及其装置 | |
CN113029132B (zh) | 一种结合地面影像与恒星光行差测量的航天器导航方法 | |
CN116105730A (zh) | 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法 | |
CN110736457A (zh) | 一种基于北斗、gps和sins的组合导航方法 | |
CN107860382B (zh) | 一种在地磁异常情况下应用ahrs测量姿态的方法 | |
CN111238485B (zh) | 一种基于导航敏感器拍摄火星图像的系统误差自补偿方法 | |
CN113091731A (zh) | 一种基于恒星视线相对论效应的航天器自主导航方法 | |
CN114754798A (zh) | 一种陀螺误差特性参数在轨辨识与标定方法 | |
CN115878939A (zh) | 基于飞行器舵面偏转的高精度动态测量方法 | |
CN114608583A (zh) | 一种高超声速飞行器动态杆臂效应智能补偿方法 | |
CN111637896A (zh) | 一种基于星历约束辅助的自主天文导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |