[发明专利]基于恒星数目和构型的惯性/天文自适应滤波方法

权利要求书

1.一种基于恒星数目和构型的惯性/天文自适应滤波方法，其特征在于，包括以下步骤：

S1、通过星敏感器采集星光矢量，基于采集到的星光矢量数目和构型进行天文定姿误差建模；

S2、依据星光矢量对天文定姿误差的实际影响，推导基于恒星矢量数目和构型的惯性/天文自适应滤波方法；

S3、通过模拟星光矢量对惯性/天文自适应滤波方法进行有效性验证。

2.根据权利要求1所述的基于恒星数目和构型的惯性/天文自适应滤波方法，其特征在于，步骤S1包括以下步骤：

S11、天文导航定姿；

根据像点位置，计算得到恒星单位矢量在星敏感器坐标系中的位置信息，计算公式如下：

式中，s_k为第k颗恒星在星敏感器坐标系的单位向量坐标，u_k为恒星矢量投影像点的x方向坐标，v_k为恒星矢量投影像点的y方向坐标，f为焦距；

将载体坐标系记为O_b-x_by_bz_b，简记为b系，地心惯性坐标系为O_i-x_iy_iz_i，简记为i系，将星敏感器坐标系s系与载体坐标系b系视为重合，通过星敏感器即获得到恒星相对于载体坐标系的坐标为s₁、s₂、…s_n，其中s_k＝[x_sk y_sk z_sk]^T，k＝1,2,…n；与此同时，通过导航星历计算得到这些恒星相对地心惯性坐标系的坐标为v₁、v₂、…v_n，其中v_k＝[x_ik y_ik z_ik]^T，则s_k与v_k的关系如下：

式中，v_k为第k颗星相对地心惯性坐标系的坐标矢量，s_k为第k颗星相对载体坐标系的坐标矢量，矩阵为星敏感器坐标系s系到地心惯性坐标系i系的姿态转换矩阵，矩阵为载体坐标系b系到地心惯性坐标系i系的姿态转换矩阵，s系与b系重合，两个矩阵等价；

记则根据式得：

因此，当星敏感器观测到的恒星数目n≥3，通过将各个恒星观测矢量最小二乘拟合得到载体系相对于惯性系的姿态转换矩阵即

其中，

Q＝(V^TV)^-1V^T

Q是转换系数矩阵；

S12、天文定姿误差建模；

恒星相对于星敏感器坐标系的实际矢量信息应该为：

其中，ΔS即为天文定姿观测误差，为恒星相对于星敏感器坐标系的实际坐标矢量，S为恒星相对于星敏感器坐标系的理想坐标矢量；

由此解算得到的姿态转换矩阵为：

其中，

以载体坐标系b系为参考坐标系，记天文定姿误差矢量为当天文定姿误差角均为小量时，姿态转换矩阵表示为：

其中，b'表示为含误差的计算载体坐标系，为载体坐标系相对于惯性坐标系的实际姿态转换矩阵，为载体坐标系相对于惯性坐标系的实际姿态转换矩阵变化量，为实际载体坐标系到理想载体坐标系的姿态转换矩阵，为沿x轴方向旋转的误差角，为沿y轴方向旋转的误差角，为沿z轴方向旋转的误差角；

进而得：

其中，

记误差矩阵的协方差矩阵为P_Δ，则：

记矩阵M的协方差矩阵为P_M，则：

记天文定姿误差矢量的协方差矩阵为则：

进而得：

其中，P_M为矩阵M的协方差矩阵为定姿误差矢量的协方差矩阵，tr()为取矩阵的迹；

当星敏感器的测量噪声ΔS一定且为高斯白噪声时，得：

其中，P_Δ为误差矩阵的协方差矩阵，E()表示为取均值，为观测噪声ΔS的方差；

由此，天文定姿误差方差为：

其中，A＝(V^TV)^*，为3×3的方阵，是矩阵V^TV的伴随矩阵，为天文定姿误差均方差系数，det()表示取行列式；

S13、天文定姿均方差与定姿误差均方差系数ξ线性拟合

对以天文定姿均方差与定姿误差均方差系数ξ进行线性拟合，得到两者近似的线性关系

δ_c＝3.32ξ+1.36

其中，δ_c为天文定姿均方差，通过拟合出的线性关系，便实时调整观测噪声，提高组合导航精度。

3.根据权利要求1所述的基于恒星数目和构型的惯性/天文自适应滤波方法，其特征在于，步骤S2具体过程如下：

利用线性卡尔曼滤波器进行组合，惯性和天文组合导航系统的状态方程和观测方程为：

其中，X(t)为系统状态变量；为系统状态变量的导数；F(t)为系统矩阵；G(t)为系统噪声系数矩阵；W(t)为系统噪声矩阵；Z(t)为观测量；H(t)为观测矩阵；V(t)为观测噪声；

惯性/天文组合导航系统采用卡尔曼滤波的方式进行信息融合，其状态方程如下：

式中，状态变量X(t)为：

状态转移矩阵为：

噪声矩阵为：

系统噪声矢量为：

W(t)_9×1＝[ω_gx ω_gy ω_gz ω_rx ω_ry ω_rz ω_ax ω_ay ω_az]^T

其中：φ_E、φ_N、φ_U为横滚、俯仰和滚转角误差，δv_E、δv_N、δv_U表示惯性导航系统在地理坐标系下东、北、天方向的速度误差；δL、δλ、δh表示惯性导航系统在地里坐标系下经度、纬度、高度的误差；ε_bx、ε_by、ε_bz表示陀螺随机常数误差；ε_rx、ε_ry、ε_rz表示陀螺一阶马尔可夫过程随机误差；表示加速度计的一阶马尔可夫过程随机误差；A(t)_18×18为系统的状态转移矩阵；G(t)_18×9为噪声系数矩阵；W(t)_9×1为系统的白噪声矢量；

F_N为陀螺和加速度计的对应状态量变换矩阵，F_S为陀螺和加速度计状态量到误差量变换矩阵，F_M为误差量变换矩阵，O为全部元素为0的空矩阵，O_3×3为3行3列的空矩阵，O_9×3为9行3列的空矩阵，为载体坐标系到地理坐标系转换矩阵，I_3×3为三行三列的单位矩阵，T_rx、T_ry、T_rz为陀螺误差相关时间在三个轴向的分量，T_ax、T_ay、T_az为加速度计相关时间在三个轴向的分量，ω_gx、ω_gy、ω_gz为陀螺三个轴向的一阶马尔可夫过程驱动白噪声，ω_rx、ω_ry、ω_rz为陀螺三个轴向的白噪声，ω_ax、ω_ay、ω_az为加速度计三个轴向的一阶马尔可夫过程驱动白噪声；

定义观测量为天文导航测得的载体姿态角与惯性导航的姿态角之差，观测方程如下：

Z_C(t)＝H_C(t)X(t)+v_C(t)

在平台误差角为小量时，观测矩阵由以下公式表示：

式中，H_C(t)为观测矩阵，γ、θ、ψ分别为横滚角、俯仰角和航向角；

因此，观测方程表示为：

式中，γ_I、θ_I、ψ_I，γ_CNS、θ_CNS、ψ_CNS分别为惯性导航和天文导航输出的横滚、俯仰、偏航角，表示惯性导航在地理坐标系下东、北、天方向平台误差角，v_C(t)是姿态误差角观测噪声，v_C(t)将随着观测恒星矢量的变化而自适应调整大小，v_C(t)有如下表示：

其中，δ_c为噪声系数，α为自适应系数。