[发明专利]捷联惯导/光谱红移自主组合导航系统及滤波方法

权利要求书

1.一种捷联惯导/光谱红移自主组合导航系统，其特征在于：

所述捷联惯导/光谱红移自主组合导航系统，将SINS输出的飞行器的速度、位置和姿态信息、SRS获得的速度信息和雷达高度计得到的高度信息送入组合导航滤波器，用SRS获得的速度信息对SINS进行校正，克服SINS随时间累积的导航误差，并利用雷达高度计抑制SINS高度通道的发散，得到高精度的导航信息。

2.一种利用权利要求1所述捷联惯导/光谱红移自主组合导航系统的滤波方法，其特征在于包括下述步骤：

步骤1.光谱红移导航的模型和算法；

(1)光谱红移导航

红移量由下面公式进行描述：

1+z＝(1+z₁)×(1+z₂)×(1+z₃) (1)

式(1)中z代表红移总和量，z₁、z₂和z₃分别代表三种不同机理引起的红移；其中z₂和z₃分别为引力红移，宇宙学红移，z为光谱红移传感器实际测量的红移，z₁为导航解算需要的多普勒红移；

将太阳系天体的光信号作为信息源，结合太阳系天体星历信息及飞行器惯性姿态信息，根据光谱红移效应测量获得航天器在惯性坐标系中的飞行速度，并通过积分获得航天器在惯性坐标系中的位置参数；

通过测量光谱频率的红移，可间接获得导弹的相对运动速度；根据空间向量关系，若观测的不共线天体数大于3个，则综合天体运行星历及航天器惯性姿态信息，即可确定航天器在惯性空间中的速度矢量，进而通过积分可获得航天器的位置参数；

(2)光谱红移导航的模型和算法

红移值是天体光谱的一个重要参数，蕴藏着天体的运行速度信息，红移值z定义为

式(2)中，λ₀是谱线原来的波长，λ是观测到的波长，f₀是谱线原来的频率，f是观测到的频率；

红移公式为：

式(3)中，v表示二维平面中航天器相对于光源运动的速度矢量，θ表示v和波矢方向的夹角，vcosθ表示径向速度，c为真空中的光速；

在三维立体天文导航中，将公式(2)进行变换，并应用到第一个参考天体上有：

式(4)中，v_r1为航天器相对第一个参考天体(光源)运动的径向速度，z₁为航天器相对第一个参考天体的红移值，v_p为航天器在惯性坐标系中的速度矢量，v₁表示第一个参考天体在惯性系中的速度矢量；通过光谱预处理、谱线特征提取和谱线匹配等步骤测出红移值；

选择三个参考天体，列出方程组如下：

由天体的几何关系可知，v_P与v_r1,v_r2,v_r3之间满足如下关系：

式(6)中,v₁,v₂,v₃为各天体在惯性系中的速度矢量，通过星历确定；u₁,u₂,u₃为惯性坐标系中各天体指向航天器位置矢量的单位矢量，由太阳敏感器或星敏感器测得；

建立关于速度矢量和位置矢量的状态估计方程组如下：

给定初始值后，求解方程组(7)可得航天器在惯性坐标系中的速度矢量v_P，再进行积分可得到位置矢量r_P；

步骤2：捷联惯导/光谱红移自主组合导航系统的模型

在捷联惯导/光谱红移自主组合导航系统中，将SINS作为主导航系统，SRS作为辅助导航系统，利用提出的自适应随机加权容积卡尔曼滤波算法，设计SINS/SRS自主组合导航系统；

(1)系统状态方程

选取东、北、天(East-North-Up)地理坐标系作为导航坐标系，系统状态方程为

式(8)中X(t)是系统状态向量，F(t)为状态转移阵，G(t)为噪声转移阵，W(t)为噪声阵；

式(9)中，东、北、天方向的速度误差分别为δv_E、δv_N、δv_U；纬度误差、经度误差和高度误差分别为δL、δλ、δh；数学平台失准角分别为φ_E、φ_N、φ_U；陀螺常值漂移分别为ε_x、ε_y、ε_z；分别为加速度计常值偏置；

系统的噪声转移矩阵G(t)为：

式(10)中

式(11)中，q_i(i＝1,2,3,4)为姿态四元素；

系统噪声向量由陀螺仪和加速度计的随机误差组成，表达式为：

式(12)中，是陀螺的白噪声，是加速度计的白噪声；

系统状态转移阵F(t)为：

式(13)中，F_N为导航参数矩阵，F_s为速度误差和位置误差姿态转换参数矩阵；

F_s和F_M分别为：

(2)系统量测方程

将由光谱红移得到的速度信息变换到导航坐标系中，姿态矩阵C_bⁿ如下式所示：

式(16)中，V_n表示航天器在导航坐标系中的东、北、天向的速度矢量，V_b表示航天器在机体坐标系中X、Y、Z方向的速度矢量；

取光谱红移和惯导系统输出的速度之差作为量测量，则速度量测矢量为：

式(17)中，v_E、v_N和v_U分别为由惯导系统得到的航天器的东向速度、北向速度和天向速度，v_SE、v_SN和v_SU分别为由光谱红移得到的航天器的东向速度、北向速度和天向速度，V_v(t)为速度量测噪声阵；

H_v＝[0_3×3 diag(1 1 1) 0_3×9]_3×15 (18)

为了阻尼惯性导航系统高度通道发散，引入气压高度表，由气压高度表和惯性导航系统输出的高度之差作为量测量，则高度量测矢量为：

Z_h＝[h_SINS-h_H]＝H_hX(t)+V_h(t) (19)

式(19)中，h_SINS和h_H分别为惯导系统和气压高度表输出的高度信息，V_h(t)为高度量测噪声阵；其中

H_h＝[0_3×6 I_3×3 0_3×6]_3×15^T (20)

SINS/SRS自主组合导航系统的量测方程为：

步骤3：自适应随机加权容积卡尔曼滤波(ARWCKF)算法

设计了一个SINS/SRS自主组合导航系统的高精度、非线性ARWCKF算法；

采用随机加权估计方法，根据各容积点估计误差的不同大小，将不同的权值分配到各容积点，根据各个不同时刻状态预测值、观测预测值、状态误差协方差预测值、自相关协方差预测值和互相关协方差预测值估计误差的大小，选取不同的权值v_i，i＝1,2,…m，通过调节加权因子，提高预测误差估计的精度，进而提高滤波计算的精度。

3.根据权利要求2所述的捷联惯导/光谱红移自主组合导航系统的滤波方法，其特征在于：

所述ARWCKF算法的步骤为：

①初始化

在组合滤波器前给定滤波初始状态和P₀，定义如下：

②计算容积点和时间更新：

记k-1时刻状态预测的协方差为P_k-1|k-1，经Cholesky分解，P_k-1|k-1表示为：

计算容积点如下：

式(24)中，x_i,k-1|k-1(i＝1,2,...,m)是k-1时刻第i个容积点的状态量，这里S_k-1|k-1是对角阵；

那么，从k-1到k时刻第i个容积点的状态量为

③状态预测

从k-1到k时刻第i个容积点的状态预测为

式(26)中，是q_k-1的算术平均值估计量；

相应地，的随机加权估计值为：

式(27)中，是q_k-1的随机加权估计值；

状态预测协方差阵P_k|k-1被描述为：

其中，是Q_k-1的算术平均值估计值；

相应地，P_k|k-1的随机加权估计为：

式(29)中，是Q_k-1的随机加权估计值；

④量测预测

从k-1到k时刻第i个容积点的量测量为：

从k-1到k时刻第i个容积点的量测预测为：

式(31)中，是r_k的算术平均值估计；

相应地，的随机加权估计表示为：

其中，是r_k的随机加权估计；

量测预测的自协方差阵P_zz,k|k-1为：

式(33)中，是R_k的算术平均值估计；

相应地，量测预测的自协方差阵P_zz,k|k-1的随机加权估计为：

式(34)中，是R_k的随机加权估计；

量测预测的互协方差阵P_xz,k|k-1为

相应地，量测预测的互协方差阵P_xz,k|k-1的随机加权估计为：

⑤状态更新为：

其中，K_k是滤波增益矩阵，组合导航滤波器接收到SINS、SRS和雷达高度表的参数输出值的差值，经过以上步骤的滤波计算，估计出各误差量和用SINS误差的估计值去校正SINS系统输出的导航参数，以得到导航参数的最优估计。