CN114370873B

CN114370873B - 一种基于改进ukf的星光折射cns/sins组合导航方法

Info

Publication number: CN114370873B
Application number: CN202210062244.7A
Authority: CN
Inventors: 陈熙源; 耿旖堃
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2022-01-19
Filing date: 2022-01-19
Publication date: 2023-12-05
Anticipated expiration: 2042-01-19
Also published as: CN114370873A

Abstract

本发明公开了一种基于改进UKF的星光折射CNS/SINS组合导航方法；首先建立了组合导航模型，利用捷联惯性导航系统的误差方程建立组合导航状态方程，引入了导航星星点方向矢量误差、折射视高度误差和运动学约束建立量测模型；其次提出了一种改进UKF方法，引入自适应因子修正量测噪声协方差矩阵跟踪实际量测噪声的变化，对星敏感器和惯性器件解算得到的导航信息进行信息融合，修正姿态、速度、位置结果，校正陀螺仪常值漂移和加速度计的常值偏置。本方法构建的量测方程利用导航星点矢量误差、折射视高度误差和运动学约束作为量测信息校正了陀螺和加速度计的误差，提出的自适应UKF方法减少了量测噪声变化导致滤波精度下降的问题，提高了星光折射CNS/SINS组合导航精度。

Description

一种基于改进UKF的星光折射CNS/SINS组合导航方法

技术领域

本发明属于组合导航技术领域，具体涉及一种基于改进UKF的星光折射CNS/SINS组合导航方法。

背景技术

传统CNS/SINS组合导航方法将天文导航与惯性导航的姿态信息相融合，但这种组合方法无法补偿加速度计误差，在长航时任务中，组合导航系统位置误差仍然发散。为实现弹道导弹等远程长航时飞行器的高精度导航，一般采用星光折射间接敏感地平的天文/惯性组合导航方法，校正陀螺仪和加速度计的误差。

然而传统星光折射间接敏感地平CNS/SINS组合导航利用星敏感器确定的姿态信息与惯性导航系统进行组合，要求观测恒星数目不少于3颗，且姿态精度受限于导航星的几何构型，极大限制了组合导航的连续性与精度。而标准UKF中设定量测噪声协方差矩阵Rk为常值，对复杂多变的环境干扰和观测星点数目构型变化的适应能力差。因此，进一步研究星光折射间接敏感地平的天文/惯性组合导航方法十分必要，需要合理选取量测量，并利用量测噪声矩阵自适应调整的UKF方法，提高CNS/SINS系统的导航精度。

发明内容

为解决上述问题，本发明公开了一种基于改进UKF的星光折射CNS/SINS组合导航方法，利用捷联惯性导航系统的误差方程建立组合导航状态方程，利用导航星星点方向矢量误差、折射视高度误差和运动学约束作为组合导航量测信息，设计自适应无迹卡尔曼滤波器自适应调整量测噪声矩阵，利用组合导航输出结果实现陀螺仪和加速度计的误差校正，提高了星光折射间接敏感地平的天文/惯性组合导航精度。

为达到上述目的，本发明的技术方案如下：

一种基于改进UKF的星光折射CNS/SINS组合导航方法，包括以下步骤：

步骤一：获取捷联惯性导航系统与天文导航系统中的导航信息；

步骤二：建立星光折射CNS/SINS组合导航系统的状态方程和量测方程；

步骤三：基于自适应无迹卡尔曼滤波方法进行组合导航的信息融合与误差校正。

进一步地，所述步骤一中，利用天文导航系统获取导航星星点方向矢量、折射星星点方向矢量、星敏感器光轴指向的导航信息；利用惯性导航系统获取载体的实时姿态、速度、位置的导航信息，具体如下：

天文导航系统利用一个大视场星敏感器同时观测导航星和折射星。导航过程中，星敏感器实时拍摄星图，通过星图识别与匹配，捕获得到导航星，计算获得星敏感器的光轴指向。设k时刻星敏感器的拍摄星图中识别出n颗导航星，在CCD成像平面内坐标为：(u_s1,v_s1),(u_s2,v_s2),…,(u_sn,v_sn)，则导航星在星敏感器坐标系下的方向矢量表示为：

其中，f为星敏感器的焦距。

根据星敏感器的光轴指向，依据导航星库，模拟当前时刻星敏感器拍摄到的未发生大气折射的星图。将非折射模拟星图与实际拍摄星图做星图匹配，通过计算模拟星图与实际拍摄星图中各颗星的欧氏距离，当距离最小值大于设定阈值时，判定该星为折射星，达到识别折射星的目的。

设星图中识别出的n颗折射星在CCD成像平面内坐标为：(u_r1,v_r1),(u_r2,v_r2),…,(u_rn,v_rn)，则折射星在星敏感器坐标系下的方向矢量表示为：

由此利用星敏感器完成了导航星星点方向矢量、折射星星点方向矢量、星敏感器光轴指向导航信息的获取。

进一步地，所述步骤二中，利用捷联惯性导航系统的误差方程建立组合导航状态方程；利用导航星星点方向矢量误差、折射视高度误差和运动学约束建立CNS/SINS系统的组合导航量测方程。

(1)CNS/SINS系统组合导航状态方程

星光折射CNS/SINS组合导航系统状态方程选用捷联惯性导航系统的误差方程，表示如下：

式中，[Φ_x,Φ_y,Φ_z]^T代表发射点惯性坐标系下的数学平台失准角，[δv_x,δv_y,δv_z]^T代表速度误差，[δx,δy,δz]^T代表位置误差，[ε_x,ε_y,ε_z]^T代表陀螺仪随机常值漂移，代表加速度计随机常值偏置。系统状态矩阵F和系统噪声输出矩阵G分别表示为：

其中，矩阵F_a中各系数为引力加速度对位置坐标的导数，随导弹位置的变化而变化；F_b中a_i为加速度计敏感到的视加速度，为由弹体系到发射点惯性系的姿态转移矩阵。

系统噪声矩阵为：式中为陀螺仪的随机噪声，为加速度计的随机噪声。系统噪声方差阵：

(2)CNS/SINS系统组合导航量测方程

本发明中星光折射CNS/SINS组合导航系统采用导航星星点方向矢量误差、折射视高度误差和运动学约束作为量测量，系统量测模型建立组成如下：

1)导航星星点方向矢量误差量测方程的建立

采用基于GAN神经网络的星点矢量预测方法，利用惯性导航姿态矩阵估计星点矢量预测值。利用导航星库建立一个已知姿态的星图数据集，对星点矢量预测GAN模型进行离线训练，并在组合导航过程中，每隔一段时间利用组合导航系统获得的姿态数据和天文导航拍摄的星图数据进一步训练GAN网络。导航过程中星敏感器数据有效的时刻，根据惯性导航实时解算的姿态矩阵借助星点矢量预测GAN模型获得当前时刻的星点预测矢量。

设k时刻导航星星敏感器拍摄星图中识别出n颗星，星敏感器中实际测量到的星点矢量l_s＝[l_s1,l_s2,…,l_sn]、惯性导航姿态阵估计的星点矢量预测值在导航系下分别表示为：

其中，Φ为数学平台失准角，l_ni为导航系下第i颗导航星的星点方向矢量。

导航星i的星点方向矢量误差表示为：

因此，导航星星点方向矢量误差量测方程表示为：

其中，观测矩阵量测噪声矩阵

设导航星星点像素坐标(u,v)的量测噪声方差阵为R₀，则由公式(1)公式(3)获得第i颗导航星星点方向矢量与星点像素坐标的关系：

l_si＝f_s(u_i,v_i)

则量测噪声矩阵V₁的方差阵R_k1随着量测值的输入实时更新，表示为：

2)折射星折射视高度误差量测方程的建立

星光通过大气层时发生折射，光线向地心方向偏折。20km-50km连续高度变化的大气模型中，真实折射视高度h_a表示为：

h_at＝h_ac+v_a＝57.081+2.531e^{[0.981ln(R)-8.689]}-6.441ln(R)+v_a (6)

根据几何关系，导弹上星敏感器观测到的折射光线相对地球的视高度h_a表示为：

其中，R为星光折射角，表示折射前后星光方向矢量之间的夹角；表示导弹位置矢量r在折射星发生折射前的星光方向矢量上的投影，其中惯性系下的折射前的星光方向矢量根据导航星库获得；R_e为地球半径；a为小量，一般忽略。

折射视高度误差表示为：

δh_a＝h_at-h_a＝h_ac-h_a+v_a (8)

根据公式(7)，折射视高度误差改写为：

式中，δr、δu均为位置误差δx、δy、δz三项的组合，式(9)改写做：

根据公式(8)和(10)折射视高度误差量测方程表示为：

其中不是白噪声，利用量测扩增方法对公式(11)进行处理使得量测噪声阵满足白噪声条件，得到折射视高度误差的量测方程：

其中v_a是大气密度误差和量测误差引起的视高度误差，已知大气密度百分比误差和折射角误差对视高度误差的影响，根据下式所示的折射视高度误差表达式能够实时更新v_a高斯白噪声的方差阵：

3)运动学约束量测方程的建立

远程弹道导弹自由段飞行时期，导弹通常状态下只受地球引力作用，处于完全失重状态，理论上加速度计输出值应为零。因此，此时加速度计的实际输出值反映了加速度计的常值偏置等误差的特性，满足：

其中，f^b表示载体系下的理论加速度值，在自由段内一般f^b＝0；为加速度计的常值偏置误差；δf^b表示加速度计其他造成实际输出值非零的误差项。

因此，远程弹道导弹的自由段中，当导弹完全失重状态时，根据运动学约束建立量测方程，表示为：

其中，观测矩阵H₃＝[0_3×12 I_3×3]，量测噪声矩阵V₃，与加速度计除常值偏置外的误差有关。

进一步地，所述步骤三中，采用自适应无迹卡尔曼滤波方法对组合导航进行信息融合。

1.标准UKF算法

UKF算法利用UT变换处理状态一步预测估计和量测估计中均值与协方差的非线性传递，对状态方程和量测方程非线性的组合导航系统具有较高的滤波估计精度。组合导航非线性系统模型表示为：

式中，X_k和Z_k分别为k时刻系统状态估计量和系统量测量；f_k和h_k分别为k时刻系统状态转移函数和系统量测函数；W_k和V_k分别为k时刻系统噪声和量测噪声，采用零均值高斯白噪声模型，噪声协方差矩阵分别为Q_k和R_k，且满足

UKF算法的滤波计算步骤如下：

(1)参数初始化

初始化状态变量方差阵

(2)设状态变量维数n，通过UT变换获得2n+1个Sigma采样点。k-1时刻样本Sigma点和相应权值W_i为：

其中比例因子比例因子λ＝α²(n+κ)-n；系数αò(0,1]；系数β≥0在正态分布下一般取β＝2；系数κ在状态变量多变量的高斯分布情况下取κ＝0。

(3)计算k时刻一步预测状态Sigma点集的均值和方差：

则k时刻一步预测样本Sigma点表示为：

(4)根据一步预测状态Sigma点集，在获取可靠量测量的k时刻，计算量测估计Sigma点集的均值和方差：

η_i,k/k-1＝h_k(χ_i,k/k-1)

状态一步预测与量测估计的误差协方差表示为：

(5)获得状态预测和量测估计点集的均值与方差后，计算滤波增益矩阵：

(6)利用滤波增益矩阵修正状态量误差，获得k时刻状态估计值和状态估计均方差阵，完成k时刻的滤波更新：

标准UKF中设定量测噪声方差矩阵R_k为常值，对复杂多变的环境干扰和观测星点数目构型变化的适应能力差。本发明在组合导航过程中自适应修正导航星星点方向矢量和折射视高度误差的量测噪声方差矩阵R_k，使其跟踪实际测量噪声的变化。

残差ε_k指k时刻的系统量测值Z_k与量测估计值之差，表示为：

在最优情况下，残差协方差理论值为：

由于不同时刻观测矩阵Z_k维数不同，本发明中残差协方差实际值的迹表示为：

其中n为估计窗口大小，i₀＝k-n+1，v_i表示i时刻残差ε_i的维数，v_k表示k时刻残差ε_k的维数。

对UKF算法引入自适应调整因子ρ_k调节系统噪声协方差矩阵R_k，引入自适应因子后量测噪声方差阵改写为：

以作为参考，采用模糊推理方法得到自适应因子，使得系统测量噪声实际值大于给定值时，ρ_k增大；使得系统测量噪声实际值小于给定值时，ρ_k减小；使保持在1附近。

基于上述方法，通过自适应无迹卡尔曼滤波估计状态量并反馈给惯性导航系统，达到估计陀螺仪误差和加速度计误差的目的，实现对组合导航系统姿态、位置误差的校正，提高星光折射CNS/SINS组合导航系统的导航精度。

本发明的有益效果为：

本发明公开了一种基于改进UKF的星光折射CNS/SINS组合导航方法，与现有技术相比，利用导航星星点方向矢量误差作为量测，减少了观测星的数目与构型对导航的限制；利用视高度误差与运动学约束信息实现对加速度计误差的修正；采用自适应UKF方法调节系统噪声协方差矩阵R_k，使其跟踪实际量测噪声的变化，从而提高了星光折射CNS/SINS组合导航的精度。

附图说明

图1为一种基于改进UKF的星光折射CNS/SINS组合导航方法的结构示意图；

图2为本发明中导航星与折射星的光路示意图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图1所示，本发明提供了一种基于改进UKF的星光折射CNS/SINS组合导航方法，利用捷联惯性导航系统的误差方程建立组合导航状态方程，利用导航星星点方向矢量误差、折射视高度误差和运动学约束作为组合导航量测信息，设计自适应无迹卡尔曼滤波器自适应调整量测噪声矩阵，对陀螺仪和加速度计误差进行校正，提高了星光折射CNS/SINS组合导航精度。下面进行详细分析：

步骤一：获取捷联惯性导航系统与天文导航系统中的导航信息。利用惯性导航系统获取实时姿态、速度、位置信息；利用天文导航系统获取导航星与折射星的星点方向矢量、星敏感器光轴指向的导航信息。

如图2所示，星光通过大气层时发生折射，光线向地心方向偏折。天文导航系统利用一个大视场星敏感器同时观测导航星和折射星，利用捕获的导航星，计算星敏感器光轴指向。设星敏感器识别出n颗导航星，在成像平面内坐标为：(u_s1,v_s1),(u_s2,v_s2),…,(u_sn,v_sn)，则导航星在星敏感器坐标系下的方向矢量为：

根据星敏感器的光轴指向，依据导航星库，模拟当前时刻星敏感器拍摄到的未发生大气折射的星图。将非折射模拟星图与实际拍摄星图做星图匹配，计算模拟星图与实际拍摄星图中各颗星的欧氏距离，当距离最小值大于设定阈值时，判定该星为折射星。

步骤二：建立星光折射CNS/SINS组合导航系统的状态方程和量测方程。利用捷联惯性导航系统的误差方程建立组合导航状态方程；利用导航星星点方向矢量误差、折射视高度误差和运动学约束建立CNS/SINS系统的组合导航量测方程。

(1)CNS/SINS系统组合导航状态方程

式中，[Φ_x,Φ_y,Φ_z]^T代表发射点惯性坐标系下的数学平台失准角，[δv_x,δv_y,δv_z]^T代表速度误差，[δx,δy,δz]^T代表位置误差，[ε_x,ε_y,ε_z]^T代表陀螺仪随机常值漂移，代表加速度计随机常值偏置；系统噪声矩阵其中和分别表示陀螺仪和加速度计的随机噪声；方差阵

(2)CNS/SINS系统组合导航量测方程

本发明采用导航星星点方向矢量误差、折射视高度误差和运动学约束作为量测量，系统量测模型建立如下：

1)导航星星点方向矢量误差量测方程的建立

设星敏感器识别出n颗导航星，则实际星点矢量l_s＝[l_s1,l_s2,…,l_sn]、惯性导航姿态阵估计的星点矢量预测值在导航系下分别为：

其中，Φ为数学平台失准角，l_ni为导航系下第i颗导航星的星点方向矢量，导航星i的星点方向矢量误差为：

因此，导航星星点方向矢量误差量测方程表示为：

其中，观测矩阵量测噪声矩阵V₁的方差阵R_k1随着量测值的输入实时更新。

2)折射星折射视高度误差量测方程的建立

20km-50km连续高度变化的大气模型中，真实折射视高度h_a表示为：

h_at＝h_ac+v_a＝57.081+2.531e^{[0.981ln(R)-8.689]}-6.441ln(R)+v_a

其中，R为星光折射角；表示导弹位置矢量r在折射星发生折射前的星光方向矢量上的投影，根据导航星库获得；R_e为地球半径；a为可忽略小量。

折射视高度误差：

式中，δr、δu为位置误差δx、δy、δz的组合，则折射视高度误差量测表示为：

利用量测扩增方法处理使量测噪声满足白噪声条件，得到折射视高度误差量测：

其中根据下式所示的表达式实时更新v_a高斯白噪声方差阵：

3)运动学约束量测方程的建立

自由段中导弹通常只受地球引力作用，处于完全失重状态，理论加速度计输出值应为零。此时加速度计的实际输出值反映了加速度计的常值偏置等误差的特性，满足：

因此，自由段中导弹完全失重状态时，根据运动学约束建立量测方程：

其中，观测矩阵H₃＝[0_3×12 I_3×3]，量测噪声矩阵V₃与加速度计的误差有关。

UKF算法利用UT变换处理状态一步预测估计和量测估计中均值与协方差的非线性传递，计算步骤如下：

(1)初始化状态变量方差阵

其中比例因子λ＝α²(n+κ)-n；系数αò(0,1]；系数β≥0在正态分布下取β＝2；系数κ取κ＝0。

(3)计算k时刻一步预测状态Sigma点集的均值和方差：

则k时刻一步预测样本Sigma点表示为：

(4)在获取可靠量测量的k时刻，计算量测估计点集的均值、方差以及误差协方差：

η_i,k/k-1＝h_k(χ_i,k/k-1)

(6)完成k时刻的滤波更新：

自适应修正导航星星点方向矢量和折射视高度误差的量测噪声方差矩阵R_k，跟踪实际测量噪声的变化。残差ε_k表示为：

最优情况下，残差协方差理论值为：

由于不同时刻观测矩阵Z_k维数不同，本发明中残差协方差实际值的迹为：

其中n为估计窗口大小，i₀＝k-n+1，v_i和v_k分别表示i和k时刻残差ε_k的维数。

对UKF算法引入自适应调整因子ρ_k调节系统噪声方差矩阵R_k，引入自适应因子后量测噪声方差阵改写为：

利用自适应无迹卡尔曼滤波估计状态量并反馈给惯性导航系统，修正陀螺仪和加速度计误差，校正组合导航系统姿态、位置误差。

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims

1.一种基于改进UKF的星光折射CNS/SINS组合导航方法，其特征在于，包括以下步骤：

具体如下：

(1)CNS/SINS系统组合导航状态方程；

利用捷联惯性导航系统误差方程建立星光折射CNS/SINS组合导航系统状态方程，表示如下：

(2)CNS/SINS系统组合导航量测方程；

1)导航星星点方向矢量误差量测方程的建立；

设星敏感器识别出n颗导航星，则实际星点矢量l_s＝[l_s1,l_s2,…,l_sn]，星点矢量预测值在导航系下分别为：

因此，导航星星点方向矢量误差量测方程表示为：

其中，观测矩阵量测噪声矩阵V₁的方差阵R_k1随着量测值的输入实时更新；

2)折射星折射视高度误差量测方程的建立；

h_at＝h_ac+v_a＝57.081+2.531e^{[0.981ln(R)-8.689]}-6.441ln(R)+v_a

其中，R为星光折射角；表示导弹位置矢量r在折射星发生折射前的星光方向矢量上的投影，根据导航星库获得；R_e为地球半径；a为小量，此处忽略不计；

折射视高度误差：

其中根据下式所示的表达式实时更新v_a高斯白噪声方差阵：

3)运动学约束量测方程的建立；

远程弹道导弹自由段飞行时期，导弹通常状态下只受地球引力作用，处于完全失重状态，理论上加速度计输出值应为零，因此，此时加速度计的实际输出值反映了加速度计的常值偏置误差的特性，满足：

其中，f^b表示载体系下的理论加速度值，在自由段内f^b＝0；为加速度计的常值偏置误差；δf^b表示加速度计其他造成实际输出值非零的误差项；

其中，观测矩阵H₃＝[0_3×12 I_3×3]，量测噪声矩阵V₃与加速度计的误差有关；

步骤三：基于自适应无迹卡尔曼滤波方法进行组合导航的信息融合与误差校正

计算步骤如下：

(1)初始化状态变量方差阵

(2)设状态变量维数n，通过UT变换获得2n+1个Sigma采样点；k-1时刻样本Sigma点和相应权值W_i为：

其中比例因子λ＝α²(n+κ)-n；系数α∈(0,1]；系数β≥0在正态分布下取β＝2；系数κ取κ＝0；

(3)计算k时刻一步预测状态Sigma点集的均值和方差：

k时刻一步预测样本Sigma点表示为：

(4)在获取可靠量测量的k时刻，计算量测估计Sigma点集的均值，以及状态一步预测与量测估计的误差协方差：

η_i,k/k-1＝h_k(χ_i,k/k-1)

(5)利用残差获取自适应调整因子，引入量测估计Sigma点集的方差计算，具体步骤如下：

残差ε_k表示为：

最优情况下，残差协方差理论值为：

由于不同时刻观测矩阵Z_k维数不同，残差协方差实际值的迹为：

其中N为估计窗口大小，i₀＝k-n+1，v_i和v_k分别表示i和k时刻残差ε_k的维数；

以作为参考，采用模糊推理方法得到自适应因子，使得系统测量噪声实际值大于给定值时，ρ_k增大；系统测量噪声实际值小于给定值时，ρ_k减小；使保持在1附近；

为调节系统噪声协方差矩阵R_k，将自适应调整因子ρ_k引入量测噪声方差阵的计算：

(6)获得状态预测和量测估计点集的均值与方差后，计算滤波增益矩阵：

(7)完成k时刻的滤波更新：

利用自适应无迹卡尔曼滤波估计状态量并反馈给惯性导航系统，修正陀螺仪误差和加速度计误差，校正组合导航系统姿态、位置误差。

2.根据权利要求1所述的一种基于改进UKF的星光折射CNS/SINS组合导航方法，其特征在于，所述步骤一中，利用惯性导航系统实时获取姿态、速度、位置信息；利用天文导航系统获取导航星与折射星的星点方向矢量、星敏感器光轴指向的导航信息，天文导航信息获取具体如下：

天文导航系统利用一个大视场星敏感器同时观测导航星和折射星，利用捕获的导航星，计算星敏感器光轴指向，设星敏感器识别出n颗导航星，在成像平面内坐标为：(u_s1,v_s1),(u_s2,v_s2),…,(u_sn,v_sn)，则导航星在星敏感器坐标系下的方向矢量为：

根据星敏感器的光轴指向，依据导航星库，模拟当前时刻星敏感器拍摄到的未发生大气折射的星图，将非折射模拟星图与实际拍摄星图做星图匹配，计算模拟星图与实际拍摄星图中各颗星的欧氏距离，当距离最小值大于设定阈值时，判定该星为折射星。

3.根据权利要求1所述的一种基于改进UKF的星光折射CNS/SINS组合导航方法，其特征在于，利用导航星星点方向矢量误差建立CNS/SINS组合导航量测方程，

在利用惯性导航姿态阵对星点矢量预测值进行估计的过程中，采用了基于GAN神经网络的星点矢量预测方法；利用导航星库建立一个已知姿态的星图数据集，对星点矢量预测GAN模型进行离线训练；并在CNS/SINS组合导航过程中，每隔一段时间利用组合导航系统获得的姿态数据和天文导航拍摄星图数据进一步地训练GAN网络；导航过程中星敏感器获取数据有效的时刻，根据惯性导航实时解算得到的姿态矩阵借助星点矢量预测GAN模型获得当前时刻的星点预测矢量，提高星点预测矢量的估计精度。