CN113291493B - 一种卫星多敏感器融合姿态确定方法和系统 - Google Patents

Abstract

本发明的实施例公开一种卫星多敏感器融合姿态确定方法和系统，所述方法包括：S1、根据纳型星敏感器和微型星敏感器的信息使用陀螺角速度外推出四元数变化，对星敏四元数进行补偿，得到补偿后的四元数；根据太阳敏感器信息确定本体系太阳矢量；S2、根据所述补偿后的四元数和本体系太阳矢量及其他敏感器信息进行角速度和姿态解算，完成直接定姿；S3、根据陀螺测量角速度和各敏感器测量信息进行滤波定姿，输出滤波后的姿态、陀螺漂移和地敏测量偏差；S4、将直接定姿与滤波定姿信息进行融合确定最终卫星姿态。本发明适用于各类卫星姿态确定，成本低，容错性和适应性较强，降低了敏感器故障的影响，提高了卫星姿态确定精度。

Description

一种卫星多敏感器融合姿态确定方法和系统

技术领域

本发明涉及卫星姿态确定领域，更具体地，涉及一种卫星多敏感器融合姿态确定方法和系统。

背景技术

当今商业航天低轨互联网卫星向着低成本轻小型化方向发展，星间通信对姿态测量精度的要求提高，使用小型低成本敏感器进行高精度姿态确定成为需要解决的问题。

发明内容

有鉴于此，本发明的第一个实施例提供一种卫星多敏感器融合姿态确定方法，包括：

S1、根据纳型星敏感器和/或微型星敏感器的信息和陀螺角速度对星敏感器四元数进行补偿，计算补偿后的姿态四元数；根据太阳敏感器信息确定本体系太阳矢量；

S2、根据所述姿态四元数、本体系太阳矢量、本体系的磁场强度矢量和本体系下地球矢量解算角速度和姿态信息，进行直接定姿；

S3、根据陀螺测量角速度、微型星敏感器、纳型星敏感器、地球敏感器、太阳敏感器和磁强计测量信息构建滤波器局部估计，对所述局部估计进行融合，获得全局最优估计，对姿态四元数和角速度进行修正，并输出地球敏感器测量偏差，以完成滤波定姿；

S4、将直接定姿与滤波定姿信息进行融合确定最终卫星姿态。

在一个具体实施方式中，所述S3包括：

S31、主滤波器进行姿态预估，计算角速度预测值、四元数预测值和误差方差阵预测值；

S32、主滤波器根据误差方差阵预测值对各子滤波器进行自观测信息分配；

S33、各子滤波器根据所述自观测信息即微型星敏感器、纳型星敏感器、地球敏感器、太阳敏感器和磁强计测量信息，对所述角速度预测值和四元数预测值进行滤波计算获得局部估计；S34、主滤波器对子滤波器局部估计进行融合，获得全局最优估计；

S35、更新陀螺漂移估计误差、地球敏感器测量偏差，并对姿态四元数和角速度进行修正。

在一个具体实施方式中，所述S4包括：

若陀螺有效，直接定姿有效，则将滤波定姿结果与直接定姿结果进行比较，若在阈值范围内则将姿态更新为滤波定姿的姿态和角速度信息，若不满足阈值则不更新；

若陀螺有效，直接定姿无效，则将姿态更新为滤波定姿的姿态和角速度信息；

若陀螺无效，则滤波定姿无效，不进行姿态更新。

在一个具体实施方式中，所述角速度解算方法包括：

S21、判断陀螺数据是否有效，若有效则使用陀螺输出角速度数据，结束解算；若无效，跳至S22；

S22、判断微型星敏感器数据是否有效，若有效则使用微型星敏感器输出角速度数据，结束解算；若无效，跳至S23；

S23、判断纳型星敏感器数据是否有效，若有效则使用纳型星敏感器输出角速度数据，结束解算；若无效，跳至S24；

S24、判断当前帧和上一帧姿态是否有效，若有效则使用当前帧和上一帧姿态进行姿态差分角速度计算，结束解算；若无效，则角速度无效。

在一个具体实施方式中，所述姿态解算方法包括：

S25、判断微型星敏感器和纳型星敏感器是否有效，若有效则进行星敏定姿；若无效，跳至S26；

S26、判断太阳敏感器和地球敏感器是否有效，若有效则进行太地双矢量定姿；若无效，跳至S27；

S27、判断太阳敏感器和磁强计是否有效，若有效则进行太磁双矢量定姿；若无效，跳至S28；

S28、判断地球敏感器和磁强计是否有效，若有效则进行地磁双矢量定姿；若无效，则姿态解算无效。

在一个具体实施方式中，所述星敏定姿的方法包括：

S251、判断微型星敏感器和纳型星敏感器数据是否都有效，若否跳至S252；若是则跳至S254，

S252、判断微型星敏感器数据是否有效，若是，则进行微星敏定姿，结束定姿流程；若否跳至S253；

S253、判断纳型星敏感器数据是否有效，若是，则进行纳星敏定姿，结束定姿流程；若否，则星敏定姿无效，结束定姿流程；

S254、判断两个星敏感器的数据是否一致，若一致，则进行两星敏双矢量定姿，结束定姿流程；若不一致，跳至S255；

S255、判断陀螺数据是否有效，若有效跳至S256；若无效跳至S258；

S256、判断微型星敏感器与陀螺姿态差是否满足阈值，若是则进行微星敏定姿，结束定姿流程；若否跳至S257；

S257、判断纳型星敏感器与陀螺姿态差是否满足阈值，若是则进行纳星敏定姿，结束定姿流程；若否，则星敏定姿无效；

S258、判断微型星敏感器和纳型星敏感器数据哪一个与上一帧姿态比变化较小，若微型星敏感器变化较小，则进行微星敏定姿，结束定姿流程；若纳型星敏感器变化较小，则进行纳星敏定姿，结束定姿流程。

在一个具体实施方式中，所述太地双矢量定姿包括：

根据卫星本体系下太阳方向、卫星本体系的地球矢量、轨道系地球矢量、轨道系下太阳矢量计算得出轨道系到卫星本体系的姿态转换矩阵，进而根据姿态转换矩阵得出姿态四元数，进而得到星体相对轨道系的姿态。

在一个具体实施方式中，所述S1包括：

S11、计算星敏输出的曝光时刻与姿态解算时刻相差的星敏时延；

S12、根据陀螺角速度和所述星敏时延计算时延引起的姿态角差；

Δα为时延引起的姿态角差，Δt为星敏输出的曝光时刻与姿态解算时刻相差的星敏时延，[w_x w_y w_z]^T为陀螺测的星本体三轴角速度；

S13、计算姿态四元数变化；

其中，Δq_i为四元数变化

S14、对星敏感器姿态四元数进行补偿；

其中，q_{i_s}为星敏感器姿态四元数，q_i为补偿后的姿态四元数。

在一个具体实施方式中，所述姿态差分角速度计算方法为：

式中，

为星本体三轴角速度，

为前后两帧惯性系四元数的变化。

本发明的第二个实施例提供一种卫星多敏感器融合姿态确定系统，包括：陀螺、微型星敏感器、纳型星敏感器、太阳敏感器、地球敏感器、磁强计、陀螺微星敏子滤波器、陀螺纳星敏子滤波器、陀螺太敏子滤波器、陀螺地敏子滤波器、陀螺磁强计子滤波器和主滤波器；其中，

微型星敏感器和纳型星敏感器用于测卫星本体系姿态；

太阳敏感器用于测量卫星本体系下太阳矢量；

磁强计用于测得卫星本体系的磁场强度矢量；

地球敏感器用于测得卫星本体系下地球矢量；

根据纳型星敏感器和/或微型星敏感器的信息和陀螺角速度对星敏感器四元数进行补偿，计算补偿后的姿态四元数；根据太阳敏感器信息确定本体系太阳矢量；

根据所述姿态四元数、本体系太阳矢量、本体系的磁场强度矢量和本体系下地球矢量解算角速度和姿态信息，进行直接定姿；

各子滤波器根据陀螺角速度和相应敏感器测量信息即微型星敏感器、纳型星敏感器、地球敏感器、太阳敏感器和磁强计测量信息对姿态进行局部估计，主滤波器对所述局部估计进行融合，获得全局最优估计，对姿态四元数和角速度进行修正，以完成滤波定姿；

主滤波器将直接定姿与滤波定姿信息进行融合确定最终卫星姿态。

本发明的有益效果如下：

本发明的多敏感器融合姿态确定方法，广泛适用于各类卫星姿态确定，成本低，具有较强容错性和适应性，降低了敏感器故障对姿态确定整体功能影响，且有效提高了卫星姿态确定精度。

附图说明

下面结合附图对本发明的具体实施方式作进一步详细的说明。

图1示出本发明实施例的一种卫星多敏感器融合姿态确定系统结构图。

图2示出本发明实施例的一种卫星多敏感器融合姿态确定方法流程图。

图3示出本发明实施例的角速度解算方法流程图。

图4示出本发明实施例的姿态解算方法流程图。

图5示出本发明实施例的星敏定姿方法流程图。

图6示出本发明实施例的姿态角误差图。

图7示出本发明实施例的姿态角速度误差图。

具体实施方式

为了更清楚地说明本发明，下面结合优选实施例和附图对本发明做进一步的说明。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，不应以此限制本发明的保护范围。

本发明充分考虑该类卫星任务需求及空间环境的复杂性，形成了一套基于低成本多敏感器融合姿态确定方法。将多敏感器的量测信息构成姿态确定信息融合系统，能够自主进行故障诊断隔离故障单机，提高精度的同时也提高了姿态确定系统的可靠性，为长时高精度姿态控制提供了基础保障，更好的保证业务正常运行。

如图1所示，一种卫星多敏感器融合姿态确定系统，包括：陀螺、微型星敏感器(简称微星敏)、纳型星敏感器(简称纳星敏)、太阳敏感器(简称太敏)、地球敏感器(简称地敏)、磁强计、陀螺微星敏子滤波器、陀螺纳星敏子滤波器、陀螺太敏子滤波器、陀螺地敏子滤波器、陀螺磁强计子滤波器和主滤波器。

如图2所示，卫星多敏感器融合姿态确定方法步骤如下：

S1、根据纳型星敏感器和微型星敏感器的信息使用陀螺角速度外推出四元数变化，对星敏四元数进行补偿，得到补偿后的四元数；根据太阳敏感器信息确定本体系太阳矢量。

敏感器进一步数据处理姿态确定使用的敏感器信息虽已经过设备管理模型的处理，但在姿态确定系统中需要对部分信息进一步处理使其更精确更好用。

星敏感器信息处理：

S11、计算星敏输出的曝光时刻与姿态解算时刻相差的星敏时延；

由时间管理功能获取星上姿态解算时刻tAtt，结合星敏输出的曝光时刻tStarSen，得到星敏延时时间Δt＝tAtt-tStarSen；

S12、根据陀螺角速度和所述星敏时延计算时延引起的姿态角差；

Δα为时延引起的姿态角差，Δt为星敏输出的曝光时刻与姿态解算时刻相差的星敏时延，[w_x w_y w_z]^T为陀螺测的星本体三轴角速度；

S13、计算姿态四元数变化；

其中，Δq_i为四元数变化

S14、对星敏感器姿态四元数进行补偿；

其中，q_{i_s}为星敏感器姿态四元数，q_i为补偿后的姿态四元数。

太阳敏感器信息处理：

先将太敏各探头下的本体系太敏矢量清零处理，若探头有效，则根据探头测角信息α_ξj、α_ηj，探头到本体系转换矩阵C_bsj，解算该探头测量的本体系太阳矢量S_bj：

探头坐标系下的太阳矢量为：

本体系太阳矢量：

S_bj＝C_bsj·S_sj

S_bj＝S_bj/|S_bj|

综合有效探头个数CountN_fValid，确定本体系太阳矢量S_b

S_b＝∑S_bi/CountN_fValid

S_b＝S_b/|S_b|

S2、根据所述补偿后的四元数和本体系太阳矢量进行角速度和姿态信息解算，完成直接定姿；

直接姿态解算是未经滤波处理的由敏感器直接解算得到姿态和角速度信息。

如图3所示，角速度解算方法包括：

S21、判断陀螺数据是否有效，若有效则使用陀螺输出角速度数据，结束解算；若无效，跳至S22；

S22、判断微型星敏感器数据是否有效，若有效则使用微型星敏感器输出角速度数据，结束解算；若无效，跳至S23；

S23、判断纳型星敏感器数据是否有效，若有效则使用纳型星敏感器输出角速度数据，结束解算；若无效，跳至S24；

S24、判断当前帧和上一帧姿态是否有效，若有效则使用当前帧和上一帧姿态进行姿态差分角速度计算，结束解算；若无效，则角速度无效。

如图4所示，姿态解算方法包括：

S25、判断微型星敏感器和纳型星敏感器是否有效，若有效则进行星敏定姿；若无效，跳至S26；

如图5所示，星敏定姿的流程：

S251、判断微型星敏感器和纳型星敏感器数据是否都有效，若否跳至S252；若是则跳至S254，

S252、判断微型星敏感器数据是否有效，若是，则进行微星敏定姿，结束定姿流程；若否跳至S253；

S253、判断纳型星敏感器数据是否有效，若是，则进行纳星敏定姿，结束定姿流程；若否，则星敏定姿无效，结束定姿流程；

S254、判断两个星敏感器的数据是否一致，若一致，则进行两星敏双矢量定姿，结束定姿流程；若不一致，跳至S255；

S255、判断陀螺数据是否有效，若有效跳至S256；若无效跳至S258；

S256、判断微型星敏感器与陀螺姿态差是否满足阈值，若是则进行微星敏定姿，结束定姿流程；若否跳至S257；

S257、判断纳型星敏感器与陀螺姿态差是否满足阈值，若是则进行纳星敏定姿，结束定姿流程；若否，则星敏定姿无效；

S258、判断微型星敏感器和纳型星敏感器数据哪一个与上一帧姿态比变化较小，若微型星敏感器变化较小，则进行微星敏定姿，结束定姿流程；若纳型星敏感器变化较小，则进行纳星敏定姿，结束定姿流程。

S26、判断太阳敏感器和地球敏感器是否有效，若有效则进行太地双矢量定姿；若无效，跳至S27；

S27、判断太阳敏感器和磁强计是否有效，若有效则进行太磁双矢量定姿；若无效，跳至S28；

S28、判断地球敏感器和磁强计是否有效，若有效则进行地磁双矢量定姿；若无效，则姿态解算无效。

1)单星敏(微星敏或纳星敏)定姿：

设星敏时间补偿后的姿态四元数q_i，根据轨道信息可以计算得到惯性系到轨道系的四元数为q_oi，则根据星敏信息得到的轨道系下卫星姿态q为：

2)双星敏(微星敏和纳星敏)定姿：

设两星敏的光轴均为Z轴，根据星敏测量姿态四元数和星敏安装布局可以得到惯性到星敏系的四元数，进而可转化得到惯性系到星敏系的坐标转换矩阵，由转换矩阵可得到两星敏Z轴在惯性系的坐标矢量

根据两星敏的安装布局会得到两星敏Z轴在本体系的坐标

两星敏的Z轴构成了双矢量，采用双矢量定姿原理即可得到惯性系到本体系的转换矩阵，根据四元数与姿态转换矩阵的关系得出惯性系的姿态四元数q_i。根据轨道确定，可以得出星体相对轨道系的姿态q。

3)双矢量定姿

太敏测得本体系的太阳矢量，磁强计测得本体系的磁场强度矢量，地敏可得本体系地球矢量，由星历结合轨道信息可以得到轨道系的太阳矢量，应用世界地磁场理论模型结合轨道信息可以得到轨道系地磁场矢量，轨道系的地球矢量即为[0；0；1]。上述三矢量中任意两者有效均可根据双矢量原理进行姿态确定，三者均有效，优先使用太地双矢量确定姿态。

a、太地双矢量定姿

太敏能够测量本体系下太阳方向S_b，通过地敏输出可直接获得轨道系到本体系的滚转角和俯仰角分别为

和θ_e，得出本体系的地球矢量：

根据星历并结合轨道信息给出轨道系下太阳矢量S_o，轨道系地球矢量为E_o＝[0；0；1]。太阳矢量、地球矢量可形成双矢量定姿，进而确定卫星的姿态。

双矢量定姿原理如下：

根据上述信息，中间变量如下：

B_o＝S_o×A_o

B_b＝S_b×A_b

F_P＝[S_o A_o B_o]

F_Q＝[S_b A_b B_b]

得出轨道系到本体系的转换矩阵：

即可根据四元数与姿态转换矩阵的关系得出姿态四元数q。

b、太磁双矢量定姿

太敏能够测量本体系下太阳方向S_b，磁强计能够输出本体系地磁场强度矢量B_b，根据星历并结合轨道信息给出轨道系下太阳矢量S_o，应用WMM地磁场模型计算地磁场在轨道系下的磁场矢量B_o。太阳矢量、地磁场矢量可形成双矢量定姿，进而确定卫星的姿态。原理与上述太地双矢量定姿类似。

c、地磁双矢量定姿

原理与太磁双矢量定姿定姿类似，定姿双矢量为地磁场矢量、地球矢量。

S3、根据陀螺测量角速度和各敏感器测量信息构建滤波器，进行局部估计，对所述局部估计进行融合，获得全局最优估计，对姿态四元数和角速度进行修正，并输出地球敏感器测量偏差，以完成滤波定姿；

以陀螺输出的姿态误差四元数矢量部分、陀螺漂移估计误差、地敏测量偏差估计误差作为系统状态变量。各子滤波器利用各自敏感器(微型敏感器、纳型敏感器、地球敏感器、太阳敏感器和磁强计)测量信息获得状态变量的局部估计并输入到主滤波器，主滤波器将局部估计进行融合得到陀螺姿态误差的全局估计，然后再校正陀螺姿态参数，更新卫星姿态。

1)系统状态方程

取状态变量为姿态误差四元数的矢量部分Δq₁₃＝[Δq₁ Δq₂ Δq₃]^T、陀螺常值漂移估计误差Δb＝[Δb₁ Δb₂ Δb₃]^T、地敏测量偏差估计误差

和Δθ_bia，即

结合四元数的运动学结合姿态动力学描述整个姿态运动，则系统状态方程可表示为：

其中，

W是系统状态模型误差，为零均值高斯白噪声，其中v_g为陀螺的测量噪声满足

v_b为陀螺漂移白噪声满足

v_bia为地敏偏差白噪声满足

对方程进行离散，离散化后的状态方程为

X_k＝Φ_k,k-1·X_k-1+Γ_k,k-1·W

式中Φ_k/k-1≈I₈+F(t)·Δt，Γ_k/k-1＝G(t)·Δt，Δt为采样时间周期。

2)主滤波器进行姿态预估

角速度预测值

其中w_gyro,k为陀螺测量输出的角速度，

为陀螺常值漂移估计值，计算四元数预测值

其中dq由角速度预测值

一步积分得到，第一次进滤波时

初值为上帧直接姿态解算的四元数。

误差方差阵预测值计算也在主滤波器完成：

Q为系统状态模型误差W离散化后对应的方差阵，

3)主滤波器向各子滤波器分配信息

信息分配因子β_i,k通过计算局部误差方差阵的迹自适应确定，按下式对各子滤波器进行信息分配，下标i对应第i个子滤波器。

4)各子滤波器获得各自观测信息后，完成相应的量测更新。

4-1)陀螺微星敏子滤波器1

微星敏有效，测量四元数为q_m，陀螺姿态预测值为

定义观测量为

则微星敏的测量方程为

Z_m＝Δq_m13＝H_mΔq₁₃+v_m

H_m＝I_3×3

v_m为微星敏测量噪声，满足

H_m是微星敏量测矩阵，I_3X3是三阶单位阵。

微星敏无效，则Z_m＝0_3×1，H_m＝0_3×3。

子滤波器1量测方程为Z₁＝H₁X₁+n₁，状态量X₁＝X，Z₁＝Z_m，H₁＝[H_m 0_3×5]_3×8，n₁＝v_m。

4-2)陀螺纳星敏子滤波器2

纳星敏有效，测量四元数为q_n，陀螺姿态预测值为

定义观测量为

则纳星敏的测量方程为

Z_n＝Δq_n13＝H_nΔq₁₃+v_n

H_n＝I_3×3

v_n为纳星敏测量噪声，满足

纳星敏无效，则Z_n＝0_3×1，H_n＝0_3×3。

子滤波器2量测方程为Z₂＝H₂X₂+n₂，状态量X₂＝X，Z₂＝Z_n，H₂＝[H_n 0_3×5]_3×8，n₂＝v_n。

4-3)陀螺太敏子滤波器3

太敏有效，测得本体系下太阳矢量S_b＝[S_bx S_by S_bz]，由星历和轨道信息得到轨道系下太阳矢量S_o，根据陀螺预测

可以得到太阳矢量估计值

将太阳矢量的测量值与预测值的差值作为观测量，同时根据矢量测量原理可得

v_s为太敏测量噪声，满足

太敏无效，则Z_s＝0_3×1，H_s＝0_3×3。

子滤波器3量测方程为Z₃＝H₃X₃+n₃，状态量X₃＝X，Z₃＝Z_s，H₃＝[H_s 0_3×5]_3×8，n₃＝v_s。

4-4)陀螺地敏子滤波器4

地敏有效直接测量输出卫星的滚动角和俯仰角，地敏测量方程为：

式中，

和θ_e为地敏的测量值，

和θ为卫星真实的滚转角和俯仰角，

和θ_bia为地敏常值测量偏差，v_e为测量噪声，满足

通过陀螺姿态预测估计得到的滚动角和俯仰角为

和

为陀螺预测姿态角，

δθ为陀螺预测的姿态角误差，考虑姿态角误差为小角度，则

δθ＝2Δq₂。

将地敏修正后的测量输出与陀螺预测估计的姿态角的差值作为观测量，

地敏无效，则Z_e＝0_2×1，H_e＝0_2×3，H_bia＝0_2×2。

子滤波器4量测方程为Z₄＝H₄X₄+n₄，状态量X₄＝X，Z₄＝Z_e，H₄＝[H_e 0_2×3 H_bia]_2×8，n₄＝v_e。

4-5)陀螺磁强计子滤波器5

磁强计有效，测量的本体系磁强度矢量为B_b，通过世界地磁场模型结合陀螺姿态预测

可以得到地磁场估计值为

将地磁场矢量的测量值与预测估计值的差值作为观测量，同时根据矢量测量原理可得

v_c为磁测量噪声，满足

磁强计无效，则Z_c＝0_3×1，H_c＝0_3×3。

子滤波器5量测方程为Z₅＝H₅X₅+n₅，状态量X₅＝X，Z₅＝Z_c，H₅＝[H_c 0_3×5]_3×8，n₅＝v_c。

5)子滤波器滤波，计算流程如下：

R_i为敏感器测量噪声方差阵，H_i为观测矩阵，K_i,k为增益矩阵。

6)主滤波器对子滤波器局部估计进行融合，获得全局最优估计

7)主滤波器更新陀螺漂移、地敏测量偏差，并对姿态四元数、角速度进行修正

8)重复第2)到第7)步，进行下一周期的滤波解算。

S4、将直接定姿与滤波定姿信息进行融合确定最终卫星姿态。

若陀螺有效，直接定姿有效，则将滤波定姿结果与直接定姿结果进行比较，若在阈值范围内则将姿态更新为滤波定姿的姿态和角速度信息，若不满足阈值则不更新；

若陀螺有效，直接定姿无效，则将姿态更新为滤波定姿的姿态和角速度信息；

若陀螺无效，则滤波定姿无效，不进行姿态更新。

一个实施例，轨道高度1175km、倾角86.5°、周期108.8min。卫星装备有1套陀螺、2套星敏(微星敏、纳星敏)、1套太敏、1套地敏和1套磁强计作为姿态测量敏感器。

陀螺常值漂移1°/h，测量噪声均方差σ_g＝0.1°/h，漂移白噪声均方差σ_b＝0.01°/h，微星敏测量噪声均方差σ_m＝[5" 5" 25"]，纳星敏σ_n＝[10" 10" 50"]，太敏测量噪声均方差σ_s＝0.1°，地敏常值测量偏差0.15°，测量噪声均方差σ_e＝0.1°，测量偏差估计误差噪声均方差σ_bia＝0.001°，磁强计测量噪声均方差σ_c＝100nT。

仿真时长为一个轨道周期t＝6528s，姿轨控计算周期Δt＝0.25s，在三轴对地模式下，设三轴初始姿态角为[5°；0°；0°]，三轴角速度均为0°/s。在仿真时间t＝500s后开启滤波解算，t＝1500s后微星敏无效，t＝2500s后纳星敏无效，t＝3500s后地敏无效，t＝4500s后太敏无效，t＝5500s后磁强计无效。

滤波初值：X₀＝[0 0 0 0 0 0 0 0]^T，

P₀＝diag([0.01²；0.01²；0.01²；0.01²；0.01²；0.01²])，β_i,0＝1/5，直接定姿与滤波定姿的姿态误差阈值为0.1°。

按本发明实施例步骤进行姿态解算，姿态角误差如图6所示，自上而下分别对应滚动角，俯仰角和航偏角；角速度误差如图7所示，自上而下分别对应滚动角，俯仰角和航偏角；可以看出开启滤波后姿态精度有了一定提高，利用的敏感器信息越多，获得的姿态信息越精确。

本发明的多敏感器融合姿态确定方法，给出了直接定姿解算、滤波定姿解算及姿态融合的详细步骤，广泛适用于各类卫星姿态确定，成本低，具有较强容错性和适应性，降低了敏感器故障对姿态确定整体功能影响，且有效提高了卫星姿态确定精度。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (10)

1.一种卫星多敏感器融合姿态确定方法，其特征在于，包括：

S1、根据纳型星敏感器和/或微型星敏感器的信息和陀螺角速度对星敏感器四元数进行补偿，计算补偿后的姿态四元数；根据太阳敏感器信息确定本体系太阳矢量；

S2、根据所述姿态四元数、本体系太阳矢量、本体系的磁场强度矢量和本体系下地球矢量解算角速度和姿态信息，进行直接定姿；

S3、根据陀螺测量角速度、微型星敏感器、纳型星敏感器、地球敏感器、太阳敏感器和磁强计测量信息构建滤波器进行局部估计，对所述局部估计进行融合，获得全局最优估计，对姿态四元数和角速度进行修正，并输出地球敏感器测量偏差，以完成滤波定姿；

S4、将直接定姿与滤波定姿信息进行融合确定最终卫星姿态。

2.根据权利要求1所述的方法，其特征在于，所述S3包括：

S31、主滤波器进行姿态预估，计算角速度预测值、四元数预测值和误差方差阵预测值；

S32、主滤波器根据误差方差阵预测值对各子滤波器进行自观测信息分配；

S33、各子滤波器根据所述自观测信息即微型星敏感器、纳型星敏感器、地球敏感器、太阳敏感器和磁强计测量信息，对所述角速度预测值和四元数预测值进行滤波计算获得局部估计；

S34、主滤波器对子滤波器局部估计进行融合，获得全局最优估计；

S35、更新陀螺漂移估计误差、地球敏感器测量偏差，并对姿态四元数和角速度进行修正。

3.根据权利要求1所述的方法，其特征在于，所述S4包括：

若陀螺有效，直接定姿有效，则将滤波定姿结果与直接定姿结果进行比较，若在阈值范围内则将姿态更新为滤波定姿的姿态和角速度信息，若不满足阈值则不更新；

若陀螺有效，直接定姿无效，则将姿态更新为滤波定姿的姿态和角速度信息；

若陀螺无效，则滤波定姿无效，不进行姿态更新。

4.根据权利要求1所述的方法，其特征在于，所述角速度解算方法包括：

S21、判断陀螺数据是否有效，若有效则使用陀螺输出角速度数据，结束解算；若无效，跳至S22；

S22、判断微型星敏感器数据是否有效，若有效则使用微型星敏感器输出角速度数据，结束解算；若无效，跳至S23；

S23、判断纳型星敏感器数据是否有效，若有效则使用纳型星敏感器输出角速度数据，结束解算；若无效，跳至S24；

S24、判断当前帧和上一帧姿态是否有效，若有效则使用当前帧和上一帧姿态进行姿态差分角速度计算，结束解算；若无效，则角速度无效。

5.根据权利要求1所述的方法，其特征在于，所述姿态解算方法包括：

S25、判断微型星敏感器和纳型星敏感器是否有效，若有效则进行星敏定姿；若无效，跳至S26；

S26、判断太阳敏感器和地球敏感器是否有效，若有效则进行太地双矢量定姿；若无效，跳至S27；

S27、判断太阳敏感器和磁强计是否有效，若有效则进行太磁双矢量定姿；若无效，跳至S28；

S28、判断地球敏感器和磁强计是否有效，若有效则进行地磁双矢量定姿；若无效，则姿态解算无效。

6.根据权利要求5所述的方法，其特征在于，所述星敏定姿的方法包括：

S251、判断微型星敏感器和纳型星敏感器数据是否都有效，若否跳至S252；若是则跳至S254，

S252、判断微型星敏感器数据是否有效，若是，则进行微星敏定姿，结束定姿流程；若否跳至S253；

S253、判断纳型星敏感器数据是否有效，若是，则进行纳星敏定姿，结束定姿流程；若否，则星敏定姿无效，结束定姿流程；

S254、判断两个星敏感器的数据是否一致，若一致，则进行两星敏双矢量定姿，结束定姿流程；若不一致，跳至S255；

S255、判断陀螺数据是否有效，若有效跳至S256；若无效跳至S258；

S256、判断微型星敏感器与陀螺姿态差是否满足阈值，若是则进行微星敏定姿，结束定姿流程；若否跳至S257；

S257、判断纳型星敏感器与陀螺姿态差是否满足阈值，若是则进行纳星敏定姿，结束定姿流程；若否，则星敏定姿无效；

S258、判断微型星敏感器和纳型星敏感器数据哪一个与上一帧姿态比变化较小，若微型星敏感器变化较小，则进行微星敏定姿，结束定姿流程；若纳型星敏感器变化较小，则进行纳星敏定姿，结束定姿流程。

7.权利要求5所述的方法，其特征在于，所述太地双矢量定姿包括：

根据卫星本体系下太阳方向、卫星本体系的地球矢量、轨道系地球矢量、轨道系下太阳矢量计算得出轨道系到卫星本体系的姿态转换矩阵，进而根据姿态转换矩阵得出姿态四元数，进而得到星体相对轨道系的姿态。

8.权利要求1所述的方法，其特征在于，所述S1包括：

S11、计算星敏输出的曝光时刻与姿态解算时刻相差的星敏时延；

S12、根据陀螺角速度和所述星敏时延计算时延引起的姿态角差；

Δα为时延引起的姿态角差，Δt为星敏输出的曝光时刻与姿态解算时刻相差的星敏时延，[w_x w_y w_z]^T为陀螺测的星本体三轴角速度；

S13、计算姿态四元数变化；

其中，Δq_i为四元数变化

S14、对星敏感器姿态四元数进行补偿；

其中，q_{i_s}为星敏感器姿态四元数，q_i为补偿后的姿态四元数。

9.根据权利要求4所述的方法，其特征在于，所述姿态差分角速度计算方法为：

式中，

为星本体三轴角速度，

为前后两帧惯性系四元数的变化。

10.一种卫星多敏感器融合姿态确定系统，其特征在于，包括：陀螺、微型星敏感器、纳型星敏感器、太阳敏感器、地球敏感器、磁强计、陀螺微星敏子滤波器、陀螺纳星敏子滤波器、陀螺太敏子滤波器、陀螺地敏子滤波器、陀螺磁强计子滤波器和主滤波器；其中，

微型星敏感器和纳型星敏感器用于测卫星姿态；

太阳敏感器用于测量卫星本体系下太阳矢量；

磁强计用于测得卫星本体系的磁场强度矢量；

地球敏感器用于测得卫星本体系下地球矢量；

根据纳型星敏感器和/或微型星敏感器的信息和陀螺角速度对星敏感器四元数进行补偿，计算补偿后的姿态四元数；根据太阳敏感器信息确定本体系太阳矢量；

根据所述姿态四元数、本体系太阳矢量、本体系的磁场强度矢量和本体系下地球矢量解算角速度和姿态信息，进行直接定姿；

各子滤波器根据陀螺输出角速度和相应敏感器测量信息即微型星敏感器、纳型星敏感器、地球敏感器、太阳敏感器和磁强计测量信息对姿态进行局部估计，主滤波器对所述局部估计进行融合，获得全局最优估计，对姿态四元数和角速度进行修正，以完成滤波定姿；

主滤波器将直接定姿与滤波定姿信息进行融合确定最终卫星姿态。

Images