CN101556155A - 小卫星姿态确定系统及方法 - Google Patents

Abstract

本发明揭示一种小卫星姿态确定系统及方法，该系统包括：若干姿态测量部件、中央处理器；中央处理器用以采集上述姿态测量部件的测量数据，计算环境模型，并根据测量数据和环境模型选择相应的定姿算法进行姿态确定；其包括地平仪数据采样及处理单元、太阳敏感器数据采样及处理单元、磁强计数据采样及处理单元、环境模型计算单元、定姿选择单元。定姿选择单元根据所述地平仪数据采样及处理单元、太阳敏感器数据采样及处理单元、磁强计数据采样及处理单元、及环境模型计算单元的数据，选择相应的定姿算法进行姿态确定。本发明成本低，结构简单；同时定姿算法多样，设计了四种确定性定姿算法和四种Kalman滤波算法，各算法之间能进行有效融合，星上自主切换，提高了系统的可靠性。

Description

小卫星姿态确定系统及方法

技术领域

本发明属于航天器技术领域，涉及一种姿态确定系统及方法，尤其涉及一种针对小卫星平台的低成本、低功耗、高可靠性的姿态确定系统及方法。

背景技术

小卫星广泛应用于通讯、科学实验、环境与资源等众多领域，成为卫星技术的重要发展方向，受到世界各国的关注。卫星平台的姿态控制系统是卫星正常运行并完成飞行任务的重要保障系统之一，而卫星姿态的确定是实施姿态控制的基础，因此，针对小卫星成本低，质量轻，技术含量高等特点，研究开发低成本高可靠性的姿态确定系统具有十分重要的意义。

目前卫星上采用的姿态测量部件主要有陀螺，星敏感器，红外地平仪，太阳敏感器和磁强计等。其中，陀螺为惯性测量部件，测量卫星的角速度，但可靠性较低，寿命较短；星敏感器直接测量卫星姿态，精度较高，但价格较高；红外地平仪有扫描式和静态式两种，通过敏感地球边界获得卫星的滚动角和俯仰角，扫描式精度较高，但价格较高且存在转动机构，而静态式无转动机构，可靠性较高；太阳敏感器用于测量太阳矢量，有数字式和模拟式两种，模拟式结构简单，成本较低；磁强计用于测量地磁矢量，成本低，可靠性高。

目前卫星上采用的定姿算法主要有确定性定姿算法和状态估计法。确定性算法主要指直接测量(如星敏感器和红外地平仪)和双(或多)矢量定姿算法(如TRIAD算法，QUEST算法)，确定性算法计算简单但精度较低；状态估计法主要指最小二乘法和Kalman滤波算法，Kalman滤波一般是利用陀螺和其它姿态测量部件组合，进行高精度姿态确定，在高精度大卫星平台中应用较多，国外有采用仅利用磁强计或与太阳敏感器组合的Kalman滤波定姿算法，算法较为复杂。Kalman滤波算法计算量大，星上一般采用固定增益形式，通过地面上注方式进行增益调整，实时性差，存在发散的风险。

卫星的小型化、低成本化，迫切要求形成一套结构简单、廉价但又具有较高精度和可靠性的姿态确定系统。

发明内容

本发明所要解决的技术问题是：提供一种针对小卫星平台的低成本、低功耗、高可靠性的姿态确定系统。

另外，本发明还提供上述姿态确定系统的姿态确定方法。

为解决上述技术问题，本发明采用如下技术方案：

一种小卫星姿态确定系统，其包括：

若干姿态测量部件，用以对反映卫星姿态的信息进行测量；姿态测量部件包括红外地平仪、太阳敏感器、磁强计；

中央处理器，用以采集上述姿态测量部件测量数据，计算环境模型，并根据测量数据和环境模型选择相应的定姿算法进行姿态确定；其包括：

地平仪数据采样及处理单元，用以根据所述红外地平仪测量的数据判断红外地平仪的状态；

太阳敏感器数据采样及处理单元，用以根据所述太阳敏感器测量的数据判断太阳敏感器的状态及卫星本体坐标系中的太阳矢量；

磁强计数据采样及处理单元，用以根据所述磁强计测量的数据判断磁强计的状态及卫星本体坐标系中的地磁矢量；

环境模型计算单元，用以获取轨道坐标系下的太阳矢量、地磁矢量；

定姿选择单元，用以根据所述地平仪数据采样及处理单元、太阳敏感器数据采样及处理单元、磁强计数据采样及处理单元、及环境模型的数据，选择相应的定姿算法进行姿态确定。

作为本发明的一种优选方案，当卫星不在稳态工作模式时，定姿选择单元控制所述姿态确定系统选择非稳态定姿算法，通过获取的测量数据选择定偏航角地平仪定姿算法、不定偏航角地平仪定姿算法、双矢量定姿算法、单磁强计定姿算法中的一种；

当卫星进入稳态工作模式时，定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法，通过获取的测量数据选择红外地平仪/太阳Kalman滤波算法、红外地平仪/磁Kalman滤波算法、太阳/磁Kalman滤波算法、单磁Kalman滤波算法中的一种。

作为本发明的一种优选方案，所述定姿选择单元控制姿态确定系统选择非稳态定姿算法包括：

当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时，选择定偏航角地平仪定姿算法；

当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时，选择不定偏航角地平仪定姿算法；

当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时，选择双矢量定姿算法；

当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时，选择单磁强计定姿算法。

作为本发明的一种优选方案，所述定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法包括：

当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时，选择红外地平仪/太阳Kalman滤波算法；

当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时，选择红外地平仪/磁Kalman滤波算法；

当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时，选择太阳/磁Kalman滤波算法；

当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时，选择单磁Kalman滤波算法。

作为本发明的一种优选方案，所述红外地平仪为静态红外地平仪，太阳敏感器为模拟式太阳敏感器，磁强计为三轴磁强计。

上述姿态确定系统的姿态确定方法，该方法包括如下步骤：

红外地平仪、太阳敏感器、磁强计对反映卫星姿态的信息进行测量；

中央处理器采集上述姿态测量部件的测量数据，计算环境模型，并根据测量数据和环境模型选择相应的定姿算法进行姿态确定；

当卫星不在稳态工作模式时，定姿选择单元控制所述姿态确定系统选择非稳态定姿算法，通过获取的测量数据选择定偏航角地平仪定姿算法、不定偏航角地平仪定姿算法、双矢量定姿算法、单磁强计定姿算法中的一种；

当卫星进入稳态工作模式时，定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法，通过获取的测量数据选择红外地平仪/太阳Kalman滤波算法、红外地平仪/磁Kalman滤波算法、太阳/磁Kalman滤波算法、单磁Kalman滤波算法中的一种。

作为本发明的一种优选方案，所述定姿选择单元控制姿态确定系统选择非稳态定姿算法的过程包括：

当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时，选择定偏航角地平仪定姿算法；

当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时，选择不定偏航角地平仪定姿算法；

当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时，选择双矢量定姿算法；

当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时，选择单磁强计定姿算法。

作为本发明的一种优选方案，定偏航角/不定偏航角地平仪定姿算法包括如下步骤：红外地平仪数据直接解算出卫星的滚动角和俯仰角；根据太阳敏感器工作状态，地平仪定姿算法分为两种情形；

若太阳敏感器数据有效，利用红外地平仪确定的滚动角

和俯仰角θ，根据姿态矩阵反推卫星的偏航角ψ：

ψ＝arctan2[(k1·S_oy-k2·S_ox)，(k1·S_ox+k2·S_oy)]；

其中，k1＝cos(θ)·S_bx+sin(θ)·S_bz；

S_b＝(S_bx，S_by，S_bz)^T，S_o＝(S_ox，S_oy，S_oz)^T分别为卫星本体坐标系和轨道坐标系中的太阳矢量；

若太阳敏感器数据无效，取ψ＝0；

双矢量定姿算法包括如下步骤：利用卫星本体坐标系中的地磁矢量B_b和太阳矢量S_b，对照轨道坐标系中的地磁矢量B_o和太阳矢量S_o，采用简化后的QUEST双矢量定姿算法，确定卫星的三轴姿态；

单磁强计定姿算法包括如下步骤：当仅三轴磁强计数据有效时，利用卫星本体坐标系中的地磁矢量B_b，对照轨道坐标系中的地磁矢量B_o，近似计算卫星的俯仰角θ：θ＝arctan2[(B_oxB_bz-B_ozB_bx)，(B_ozB_bz+B_oxB_bx)]。

作为本发明的一种优选方案，所述定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法的过程包括：

当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时，选择红外地平仪/太阳Kalman滤波算法；

当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时，选择红外地平仪/磁Kalman滤波算法；

当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时，选择太阳/磁Kalman滤波算法；

当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时，选择单磁Kalman滤波算法。

作为本发明的一种优选方案，所述红外地平仪/太阳Kalman滤波算法、红外地平仪/磁Kalman滤波算法、太阳/磁Kalman滤波算法、单磁Kalman滤波算法包括如下步骤：姿态预估计步骤，状态方程建立步骤，观测量计算及观测方程建立步骤，Kalman滤波迭代计算步骤，姿态校正步骤。

本发明的有益效果在于：

(1)测量部件中不含惯性部件，无转动机构，成本低，结构简单，可靠性高，符合小卫星低成本、高可靠性的特点；

(3)定姿算法多样，设计了四种确定性定姿算法和四种Kalman滤波算法，各算法之间能进行有效融合，星上自主切换，提高了系统的可靠性；

(4)将四种完整的Kalman滤波定姿算法引入定姿系统，没有按传统形式采用固定增益的简化滤波算法，具有较高定姿精度；同时通过两项关键技术，减少了滤波算法的计算量。两项关键技术为：A、将四种Kalman滤波定姿算法设计成统一的形式，星上根据测量部件的状态自主进行重构；B、根据磁控卫星间歇控制的特点，将整个Kalman滤波定姿算法分解为两个部分，分时进行计算。

本发明提供的姿态确定系统已成功应用于小型号卫星，运行情况优异，满足中等精度小卫星平台的需要，为小卫星平台姿态确定系统的设计提供了重要参考。

附图说明

图1为本发明姿态确定系统的组成示意图。

图2为本发明姿态确定方法的流程图。

具体实施方式

下面结合附图详细说明本发明的优选实施例。

请参阅图1，本发明揭示了一种小卫星姿态确定系统，其包括：若干姿态测量部件10、中央处理器20。

姿态测量部件10用以对反映卫星姿态的信息进行测量；本实施例中，姿态测量部件包括红静态红外地平仪11、模拟式太阳敏感器12、三轴磁强计13。中央处理器20用以采集上述姿态测量部件的测量数据，计算环境模型，并根据测量数据和环境模型选择相应的定姿算法进行姿态确定。本实施例中，中央处理器20作为一星载计算机的一部分，该星载计算机还包括姿态控制系统40。

以下分别对系统各组成详细介绍。

【姿态测量部件】

本发明从小卫星低成本，低功耗、高可靠性的角度出发选择姿态测量部件。

1)静态红外地平仪

静态红外地平仪通过测量地球与太空的边界，可直接确定卫星的滚动角和俯仰角。主要指标：

测量角范围：±10°

质量：      1.3kg

功耗：      0.9W

2)模拟式太阳敏感器

模拟式太阳敏感器安装在卫星的对太阳面上，通过测量敏感器平面与太阳矢量之间的夹角，得到太阳矢量在卫星本体坐标系中的表示。主要指标：

角度分辨率：优于0.3°

测量精度：  1°

质量：      0.2kg

功耗：      0.3W

3)三轴磁强计

三轴磁强计用于测量卫星本体坐标系下三轴磁场矢量强度，不存在视场问题，是本发明系统中最可靠的测量部件，对其进行热备份，进一步提高系统可靠性。主要指标：

量程(X、Y、Z轴)：    -50000nT～+50000nT

分辨率：             2nT

测量精度：           ±4nT+0.05％测量值

质量(两探头+线路盒)：1.2kg

功耗(两探头+线路盒)：1.0W

本发明采用的三种测量部件均为低成本姿态测量部件，总质量仅为2.7kg，功耗仅为2.2W，符合小卫星的特点。对各测量部件的数据均采用多次采样进行平均的方式进行数据采集，以提高测量精度。

【中央处理器】

中央处理器20包括地平仪数据采样及处理单元21、太阳敏感器数据采样及处理单元22、磁强计数据采样及处理单元23、环境模型计算单元24、定姿选择单元25。

地平仪数据采样及处理单元21用以根据所述红外地平仪测量的数据判断红外地平仪的状态并获取红外地平仪的四路信号值。

太阳敏感器数据采样及处理单元22用以根据所述太阳敏感器测量的数据判断太阳敏感器的状态并获取卫星本体坐标系中的太阳矢量Sb。

磁强计数据采样及处理单元23用以根据所述磁强计测量的数据判断磁强计的状态并获取卫星本体坐标系中的地磁矢量Bb。

环境模型计算单元24用以获取轨道坐标系下太阳矢量So、地磁矢量Bo。

定姿选择单元25，用以根据所述地平仪数据采样及处理单元、太阳敏感器数据采样及处理单元、磁强计数据采样及处理单元、及环境模型计算单元的数据，选择相应的定姿算法进行姿态确定。

本发明采用的定姿算法包括：地平仪定姿(包含定偏航角和不定偏航角两种)，双矢量定姿和单磁强计定姿，红外地平仪/太阳Kalman滤波定姿，红外地平仪/磁Kalman滤波定姿，双矢量Kalman滤波定姿和单磁Kalman滤波定姿。中央处理器根据姿控系统的工作模式和测量部件的工作状态自主在各定姿算法间进行切换或重构。

各定姿算法重构图如图2所示。当卫星不在稳态工作模式时(条件⑥)，定姿选择单元控制所述姿态确定系统选择非稳态定姿算法，通过获取的测量数据选择定偏航角地平仪定姿算法、不定偏航角地平仪定姿算法、双矢量定姿算法、单磁强计定姿算法中的一种。

当卫星进入稳态工作模式时(条件⑤)，定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法，通过获取的测量数据选择红外地平仪/太阳Kalman滤波算法、红外地平仪/磁Kalman滤波算法、太阳/磁Kalman滤波算法、单磁Kalman滤波算法中的一种。

所述定姿选择单元控制姿态确定系统选择非稳态定姿算法的过程包括：当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时(条件②条件④)，选择定偏航角地平仪定姿算法；当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时(条件②条件③)，选择不定偏航角地平仪定姿算法；当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时(条件①条件④)，选择双矢量定姿算法；当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时(条件①条件③)，选择单磁强计定姿算法。

所述定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法的过程包括：当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时(条件②条件④)，选择红外地平仪/太阳Kalman滤波算法；当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时(条件②条件③)，选择红外地平仪/磁Kalman滤波算法；当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时(条件①条件④)，选择太阳/磁Kalman滤波算法；当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时(条件①条件③)，选择单磁Kalman滤波算法。

定姿选择单元25根据实际情况依照上述定姿确定方法选择相应定姿算法，以下具体介绍各定姿算法。

一、地平仪定姿算法

红外地平仪数据可直接解算出卫星的滚动角和俯仰角。根据太阳敏感器工作状态，地平仪定姿算法分为两种情形。若太阳敏感器数据有效，利用红外地平仪确定的滚动角

和俯仰角θ，根据姿态矩阵反推卫星的偏航角ψ：

ψ＝arctan2[(k1·S_oy-k2·S_ox)，(k1·S_ox+k2·S_oy)]    (1)

式中，k1＝cos(θ)·S_bx+sin(θ)·S_bz；

S_b＝(S_bx，S_by，S_bz)^T，S_o＝(S_ox，S_oy，S_oz)^T分别为卫星本体坐标系和轨道坐标系中的太阳矢量。

若太阳敏感器数据无效，不定卫星的偏航角，取ψ＝0。

二、双矢量定姿算法

双矢量定姿算法利用卫星本体坐标系中的地磁矢量B_b和太阳矢量S_b，对照轨道坐标系中的地磁矢量B_o和太阳矢量S_o，采用简化后的QUEST双矢量定姿算法，确定卫星的三轴姿态。

三、单磁强计定姿算法

当仅三轴磁强计数据有效时，利用卫星本体坐标系中的地磁矢量B_b，对照轨道坐标系中的地磁矢量B_o，近似计算卫星的俯仰角θ：

θ＝arctan2[(B_oxB_bz-B_ozB_bx)，(B_ozB_bz+B_oxB_bx)]    (2)

四、扩展Kalman滤波定姿算法

扩展Kalman滤波定姿算法为本发明系统的核心部分。

四种Kalman滤波算法结构一致，可细分为姿态预估计，状态方程建立，观测量计算及观测方程建立，Kalman滤波迭代计算，姿态校正五个部分。具体描述如下：

①姿态预估计

根据卫星动力学模型估计卫星角速度：

${\widehat{\mathrm{\ω}}}_{\mathrm{bi},k+1/k}={\widehat{\mathrm{\ω}}}_{\mathrm{bi},k/k}+I^{-1}[-{\mathrm{\ω}}_{\mathrm{bi},k/k}\×(I{\mathrm{\ω}}_{\mathrm{bi},k/k}+h)]\mathrm{\ΔT}---\left(3\right)$

式中， ${\widehat{\mathrm{\ω}}}_{\mathrm{bi},k/k}={[{\widehat{\mathrm{\ω}}}_{\mathrm{bix},k/k},{\widehat{\mathrm{\ω}}}_{\mathrm{biy},k/k},{\widehat{\mathrm{\ω}}}_{\mathrm{biz},k/k}]}^T$ 为第k时刻角速度滤波值，初始值 ${\widehat{\mathrm{\ω}}}_{\mathrm{bi},0/0}={[0,-{\mathrm{\ω}}_0,]}^T;$ I为卫星转动惯量矩阵，h为卫星的偏置角动量，ΔT为计算周期。

根据卫星运动学模型估计卫星姿态四元数：

${\widehat{\mathrm{\ω}}}_{\mathrm{bo},k/k}={\widehat{\mathrm{\ω}}}_{\mathrm{bi},k/k}-{\mathrm{\ω}}_0\left[\begin{array}{ccc}-2{\hat{q}}_{\mathrm{bo}3,k/k}& -1& 2{\hat{q}}_{\mathrm{bo}1,k/k}\end{array}\right]---\left(4\right)$

${\hat{q}}_{\mathrm{bo},k+1/k}={\hat{q}}_{\mathrm{bo},k/k}+\frac{1}{2}\mathrm{\Ω}\left({\widehat{\mathrm{\ω}}}_{\mathrm{bo},k/k}\right){\hat{q}}_{\mathrm{bo},k/k}\mathrm{\ΔT}---\left(5\right)$

式中， ${\hat{q}}_{\mathrm{bo},k/k}={[{\hat{q}}_{\mathrm{bo}1,k/k},{\hat{q}}_{\mathrm{bo}2,k/k},{\hat{q}}_{\mathrm{bo}3,k/k},{\hat{q}}_{\mathrm{bo}4,k/k}]}^T$ 为第k时刻姿态四元数滤波值，初始值 ${\hat{q}}_{\mathrm{bo},0/0}={\left[\mathrm{0,0,0,1}\right]}^T,$ ω₀为卫星轨道角速度； ${\widehat{\mathrm{\ω}}}_{\mathrm{bo},k/k}={[{\widehat{\mathrm{\ω}}}_{\mathrm{box},k/k},{\widehat{\mathrm{\ω}}}_{\mathrm{boy},k/k},{\widehat{\mathrm{\ω}}}_{\mathrm{boz},k/k}]}^T$ 为第k时刻卫星相对轨道系的角速度。

表示构建矩阵：

$\mathrm{\Ω}\left({\mathrm{\ω}}_{\mathrm{bo}}\right)=\left[\begin{array}{cccc}0& {\mathrm{\ω}}_{\mathrm{boz}}& -{\mathrm{\ω}}_{\mathrm{boy}}& {\mathrm{\ω}}_{\mathrm{box}}\\ -{\mathrm{\ω}}_{\mathrm{boz}}& 0& {\mathrm{\ω}}_{\mathrm{box}}& {\mathrm{\ω}}_{\mathrm{boy}}\\ {\mathrm{\ω}}_{\mathrm{boy}}& -{\mathrm{\ω}}_{\mathrm{box}}& 0& {\mathrm{\ω}}_{\mathrm{boz}}\\ -{\mathrm{\ω}}_{\mathrm{box}}& -{\mathrm{\ω}}_{\mathrm{boy}}& -{\mathrm{\ω}}_{\mathrm{boz}}& 0\end{array}\right]$

为观测量计算做准备，对卫星姿态矩阵进行预估计：

$A=\left[\begin{array}{ccc}1& 2{\hat{q}}_{\mathrm{bo}3,k+1/k}& -2{\hat{q}}_{\mathrm{bo}2,k+1/k}\\ -2{\hat{q}}_{\mathrm{bo}3,k+1/k}& 1& 2{\hat{q}}_{\mathrm{bo}1,k+1/k}\\ 2{\hat{q}}_{\mathrm{bo}2,k+1/k}& -2{\hat{q}}_{\mathrm{bo}1,k+1/k}& 1\end{array}\right]---\left(6\right)$

②状态方程建立

分别定义卫星角速度ω_bi和姿态四元数q_bo的真实值与估计值

的误差δω_bi＝[δω_bix，δω_biy，δω_biz]^T和δq_bo＝[δq_bo1，δq_bo2，δq_bo3，δq_bo4]^T，满足：

$q_{\mathrm{bo}}={\hat{q}}_{\mathrm{bo}}\⊗\mathrm{\Δ}{q}_{\mathrm{bo}},$ ${\mathrm{\ω}}_{\mathrm{bi}}={\widehat{\mathrm{\ω}}}_{\mathrm{bi}}+\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{bi}}$

取δω_bi和δq_bo的矢量部分作为滤波状态量X：

X＝[δω_bix δω_biy δω_biz δq_bo1 δq_bo2 δq_bo3]^T

连续滤波状态方程为：

$\stackrel{\·}{X}=F\left(t\right)X+W\left(t\right)---\left(7\right)$

式中， $F\left(t\right)=\left[\begin{array}{cc}{F}_{11(3\×3)}& 0_{3\×3}\\ 0.5I_{3\×3}& 0_{3\×3}\end{array}\right],$ $F_{11}=I^{-1}\left[\right[(I{\widehat{\mathrm{\ω}}}_{\mathrm{bi}}+h)\×]-[{\widehat{\mathrm{\ω}}}_{\mathrm{bi}}\×\left]I\right];$

I_3×3为3维单位矩阵。

将连续模型离散化：

X_k+1＝Ф_k+1/kX_k+W_k    (8)

式中，Ф_k+1/k＝I_6×6+F(t_k)ΔT，I_6×6为6维单位矩阵。

③观测量计算及观测方程建立

Kalman滤波观测方程离散形式为：

Y_k+1＝H_k+1X_k+1+V_k+1    (9)

四种Kalman滤波算法，滤波观测量Y_k+1，观测矩阵H_k+1和噪声V_k+1的取值各不相同，通过适当处理将其设计成统一的形式，具体如下：

地平仪+太阳EKF

滤波观测量Y_k+1：Y＝[δS_bx δS_by δS_bz δφ δθ 0]^T

式中，δS_b＝[δS_bx δS_by δS_bz]^T＝[(AS_o)×]S_b；

$\mathrm{\δ\θ}=\mathrm{\θ}-2{\hat{q}}_{\mathrm{bo}2},$

和θ为红外地平仪确定的滚动角和俯仰角。

滤波观测矩阵H_k+1： $H=\left[\begin{array}{cc}{0}_{3\×3}& H_{12}\\ 0_{3\×3}& H_{22}\end{array}\right]$

式中， $H_{12}=\left[\begin{array}{ccc}-2(S_{\mathrm{oy}}^2+S_{\mathrm{oz}}^2)& 0& 0\\ 0& -2(S_{\mathrm{ox}}^2+S_{\mathrm{oz}}^2)& 0\\ 0& 0& -2(S_{\mathrm{ox}}^2+S_{\mathrm{oy}}^2)\end{array}\right],$ $H_{22}=\left[\begin{array}{ccc}2& 0& 0\\ 0& 2& 0\\ 0& 0& 0\end{array}\right].$

测量噪声V_k+1的方差阵R取为：

$R=\left[\begin{array}{cccccc}{\mathrm{\σ}}_s^2& 0& 0& 0& 0& 0\\ 0& {\mathrm{\σ}}_s^2& 0& 0& 0& 0\\ 0& 0& {\mathrm{\σ}}_s^2& 0& 0& 0\\ 0& 0& 0& {\mathrm{\σ}}_h^2& 0& 0\\ 0& 0& 0& 0& {\mathrm{\σ}}_h^2& 0\\ 0& 0& 0& 0& 0& {\mathrm{\σ}}_h^2\end{array}\right]$

式中，σ_s，σ_h分别为太阳敏感器和红外地平仪的测量误差方差。

地平仪+磁EKF

滤波观测量Y_k+1：Y＝[δB_bx δB_by δB_bz δφ δθ 0]^T

式中，δB_b＝[δB_bx δB_by δB_bz]^T＝[(AB_o)×]B_b；

$\mathrm{\δ\θ}=\mathrm{\θ}\_h-2{\hat{q}}_{\mathrm{bo}2.}$

滤波观测矩阵H_k+1： $H=\left[\begin{array}{cc}{0}_{3\×3}& H_{12}\\ 0_{3\×3}& H_{22}\end{array}\right]$

式中， $H_{12}=\left[\begin{array}{ccc}-2(B_{\mathrm{oy}}^2+B_{\mathrm{oz}}^2)& 0& 0\\ 0& -2(B_{\mathrm{ox}}^2+B_{\mathrm{oz}}^2)& 0\\ 0& 0& -2(B_{\mathrm{ox}}^2+B_{\mathrm{oy}}^2)\end{array}\right],$ $H_{22}=\left[\begin{array}{ccc}2& 0& 0\\ 0& 2& 0\\ 0& 0& 0\end{array}\right].$

测量噪声V_k+1的方差阵R取为：

$R=\left[\begin{array}{cccccc}{\mathrm{\σ}}_b^2& 0& 0& 0& 0& 0\\ 0& {\mathrm{\σ}}_b^2& 0& 0& 0& 0\\ 0& 0& {\mathrm{\σ}}_b^2& 0& 0& 0\\ 0& 0& 0& {\mathrm{\σ}}_h^2& 0& 0\\ 0& 0& 0& 0& {\mathrm{\σ}}_h^2& 0\\ 0& 0& 0& 0& 0& {\mathrm{\σ}}_h^2\end{array}\right]$

式中，σ_b为磁强计测量误差方差。

双矢量EKF

滤波观测量Y_k+1：Y＝[δB_bx δB_by δB_bz δS_bx δS_by δS_bz]^T

式中，δB_b＝[δB_bx δB_by δB_bz]^T＝[(AB_o)×]B_b；

δS_b＝[δS_bx δS_by δS_bz]^T＝[(AS_o)×]S_b。

滤波观测矩阵H_k+1： $H=\left[\begin{array}{cc}{0}_{3\×3}& H_{12}\\ 0_{3\×3}& H_{22}\end{array}\right]$

式中， $H_{12}=\left[\begin{array}{ccc}-2(B_{\mathrm{oy}}^2+B_{\mathrm{oz}}^2)& 0& 0\\ 0& -2(B_{\mathrm{ox}}^2+B_{\mathrm{oz}}^2)& 0\\ 0& 0& -2(B_{\mathrm{ox}}^2+B_{\mathrm{oy}}^2)\end{array}\right],$

$H_{22}=\left[\begin{array}{ccc}-2(S_{\mathrm{oy}}^2+S_{\mathrm{oz}}^2)& 0& 0\\ 0& -2(S_{\mathrm{ox}}^2+S_{\mathrm{oz}}^2)& 0\\ 0& 0& -2(S_{\mathrm{ox}}^2+S_{\mathrm{oy}}^2)\end{array}\right].$

测量噪声V_k+1的方差阵R取为：

$R=\left[\begin{array}{cccccc}{\mathrm{\σ}}_b^2& 0& 0& 0& 0& 0\\ 0& {\mathrm{\σ}}_b^2& 0& 0& 0& 0\\ 0& 0& {\mathrm{\σ}}_b^2& 0& 0& 0\\ 0& 0& 0& {\mathrm{\σ}}_s^2& 0& 0\\ 0& 0& 0& 0& {\mathrm{\σ}}_s^2& 0\\ 0& 0& 0& 0& 0& {\mathrm{\σ}}_s^2\end{array}\right]$

单磁强计EKF

滤波观测量Y_k+1：Y＝[δB_bx δB_by δB_bz 0 0 0]^T

式中，δB_b＝[δB_bx δB_by δB_bz]^T＝[(AB_o)×]B_b。

滤波观测矩阵H_k+1：

$H=\left[\begin{array}{cc}{0}_{3\×3}& H_{12}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]$

式中， $H_{12}=\left[\begin{array}{ccc}-2(B_{\mathrm{oy}}^2+B_{\mathrm{oz}}^2)& 0& 0\\ 0& -2(B_{\mathrm{ox}}^2+B_{\mathrm{oz}}^2)& 0\\ 0& 0& -2(B_{\mathrm{ox}}^2+B_{\mathrm{oy}}^2)\end{array}\right].$

测量噪声V_k+1的方差阵R取为：

$R=\left[\begin{array}{cccccc}{\mathrm{\σ}}_b^2& 0& 0& 0& 0& 0\\ 0& {\mathrm{\σ}}_b^2& 0& 0& 0& 0\\ 0& 0& {\mathrm{\σ}}_b^2& 0& 0& 0\\ 0& 0& 0& {\mathrm{\σ}}_b^2& 0& 0\\ 0& 0& 0& 0& {\mathrm{\σ}}_b^2& 0\\ 0& 0& 0& 0& 0& {\mathrm{\σ}}_b^2\end{array}\right]$

④Kalman滤波迭代计算

联立Kalman滤波状态方程和观测方程：

$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{\Φ}}_{k+1/k}{X}_k+W_k\\ Y_{k+1}=H_{k+1}{X}_{k+1}+V_{k+1}\end{array}\right.---\left(10\right)$

按下述简化的Kalman滤波迭代公式进行迭代计算：

$\left\{\begin{array}{c}{P}_{k+1/k}={\mathrm{\Φ}}_{k+1/k}{P}_{k/k}{\mathrm{\Φ}}_{k+1/k}^T+Q\\ K_{k+1}=P_{k+1/k}{H}_{k+1}^T{(H_{k+1}{P}_{k+1/k}{H}_{k+1}^T+R)}^{-1}\\ P_{k+1/k+1}=(I_{6\×6}-K_{k+1}{H}_{k+1})P_{k+1/k}\\ X_{k+1/k+1}=K_{k+1}{Y}_{k+1}\end{array}\right.---\left(11\right)$

式中，P_k/k为滤波误差方差阵，Q为系统误差方差阵。

⑤姿态校正

利用滤波值X_k+1/k+1对预估计值进行校正得到最终的姿态：

角速度校正：

${\widehat{\mathrm{\ω}}}_{\mathrm{bi},k+1/k+1}={\widehat{\mathrm{\ω}}}_{\mathrm{bi},k+1/k}+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{bi},k+1/k+1}---\left(12\right)$

姿态四元数校正：

$\left\{\begin{array}{c}\mathrm{\δ}{q}_{\mathrm{bo}4,k+1/k+1}=\sqrt{1-(\mathrm{\δ}{q}_{\mathrm{bo}1,k+1/k+1}^2+{\mathrm{\δq}}_{\mathrm{bo}2,k+1/k+1}^2+\mathrm{\δ}{q}_{\mathrm{bo}3,k+1/k+1}^2)}\\ {\hat{q}}_{\mathrm{bo},k+1/k+1=}{\hat{q}}_{\mathrm{bo},k+1/k}\⊗\mathrm{\δ}{q}_{\mathrm{bo},k+1/k+1}\\ {\hat{q}}_{\mathrm{bo},k+1/k+1}={\hat{q}}_{\mathrm{bo},k+1/k+1}/\left|{\hat{q}}_{\mathrm{bo},k+1/k+1}\right|\end{array}\right.---\left(13\right)$

姿态四元数q转化为姿态角

θ，ψ：

通过对滤波观测量的处理，四种Kalman滤波算法可采用一套算法实现。

Kalman滤波递推计算耗时很长，尤其是式(11)涉及六维矩阵间的四则运算，在实际计算过程中，采取将六维矩阵拆成三维矩阵，将常数阵转化为矢量等方式进行计算，有效的减少了计算量。式(11)中的第三式滤波误差方差阵P_k+1/k+1的计算是为了下一拍滤波计算做准备，不参与该步定姿。而主动磁控卫星采取一拍控，一拍不控的方式对卫星施加控制力矩，即在不控的一拍是不需要姿态信息的，所以将式(11)中的第三式从Kalman算法中分离至不控的那一拍进行计算，计算结果设置为全局变量，作为完整Klaman滤波算法的输入量，从而进一步减少了Kalman滤波定姿算法的计算量。

这里本发明的描述和应用是说明性的，并非想将本发明的范围限制在上述实施例中。这里所披露的实施例的变形和改变是可能的，对于那些本领域的普通技术人员来说实施例的替换和等效的各种部件是公知的。本领域技术人员应该清楚的是，在不脱离本发明的精神或本质特征的情况下，本发明可以以其它形式、结构、布置、比例，以及用其它组件、材料和部件来实现。在不脱离本发明范围和精神的情况下，可以对这里所披露的实施例进行其它变形和改变。

Claims (10)

1、一种小卫星姿态确定系统，其特征在于，其包括：

若干姿态测量部件，用以对反映卫星姿态的信息进行测量；姿态测量部件包括红外地平仪、太阳敏感器、磁强计；

中央处理器，用以采集上述姿态测量部件的测量数据，并计算环境模型，然后根据测量数据及环境模型选择相应的定姿算法进行姿态确定；其包括

地平仪数据采样及处理单元，用以根据所述红外地平仪测量的数据判断红外地平仪的状态并获取处理后的红外地平仪测量数据；

太阳敏感器数据采样及处理单元，用以根据所述太阳敏感器测量的数据判断太阳敏感器的状态并获取卫星本体坐标系中的太阳矢量；

磁强计数据采样及处理单元，用以根据所述磁强计测量的数据判断磁强计的状态并获取卫星本体坐标系中的地磁矢量；

环境模型计算单元，用以获取轨道坐标系下的太阳矢量、地磁矢量；

定姿选择单元，用以根据所述地平仪数据采样及处理单元、太阳敏感器数据采样及处理单元、磁强计数据采样及处理单元、环境模型计算单元，选择相应的定姿算法进行姿态确定。

2、根据权利要求1所述的小卫星姿态确定系统，其特征在于：

当卫星不在稳态工作模式时，定姿选择单元控制所述姿态确定系统选择非稳态定姿算法，通过获取的测量数据选择定偏航角地平仪定姿算法、不定偏航角地平仪定姿算法、双矢量定姿算法、单磁强计定姿算法中的一种；

当卫星进入稳态工作模式时，定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法，通过获取的测量数据选择红外地平仪/太阳Kalman滤波算法、红外地平仪/磁Kalman滤波算法、太阳/磁Kalman滤波算法、单磁Kalman滤波算法中的一种。

3、根据权利要求2所述的小卫星姿态确定系统，其特征在于：

所述定姿选择单元控制姿态确定系统选择非稳态定姿算法包括：

当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时，选择定偏航角地平仪定姿算法；

当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时，选择不定偏航角地平仪定姿算法；

当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时，选择双矢量定姿算法；

当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时，选择单磁强计定姿算法。

4、根据权利要求2所述的小卫星姿态确定系统，其特征在于：

所述定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法包括：

当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时，选择红外地平仪/太阳Kalman滤波算法；

当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时，选择红外地平仪/磁Kalman滤波算法；

当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时，选择太阳/磁Kalman滤波算法；

当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时，选择单磁Kalman滤波算法。

5、根据权利要求1至4任意一项所述的小卫星姿态确定系统，其特征在于：

所述红外地平仪为静态红外地平仪，太阳敏感器为模拟式太阳敏感器，磁强计为三轴磁强计。

6、一种权利要求1至5任意一项所述姿态确定系统的姿态确定方法，其特征在于，该方法包括如下步骤：

红外地平仪、太阳敏感器、磁强计对反映卫星姿态的信息进行测量；

中央处理器采集上述姿态测量部件的测量数据，计算环境模型，并根据测量数据和环境模型选择相应的定姿算法进行姿态确定；

当卫星不在稳态控制模式时，定姿选择单元控制所述姿态确定系统选择非稳态定姿算法，通过获取的测量数据选择定偏航角地平仪定姿算法、不定偏航角地平仪定姿算法、双矢量定姿算法、单磁强计定姿算法中的一种；

当卫星进入稳态工作模式时，定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法，通过获取的测量数据选择红外地平仪/太阳Kalman滤波算法、红外地平仪/磁Kalman滤波算法、太阳/磁Kalman滤波算法、单磁Kalman滤波算法中的一种。

7、根据权利要求6所述的姿态确定方法，其特征在于：

所述定姿选择单元控制姿态确定系统选择非稳态定姿算法的过程包括：

当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时，选择定偏航角地平仪定姿算法；

当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时，选择不定偏航角地平仪定姿算法；

当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时，选择双矢量定姿算法；

当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时，选择单磁强计定姿算法。

8、根据权利要求7所述的姿态确定方法，其特征在于：

定偏航角/不定偏航角地平仪定姿算法包括如下步骤：红外地平仪数据直接解算出卫星的滚动角和俯仰角；根据太阳敏感器工作状态，地平仪定姿算法分为两种情形；

若太阳敏感器数据有效，利用红外地平仪确定的滚动角

和俯仰角θ，根据姿态矩阵反推卫星的偏航角ψ：

ψ＝arctan2[(k1·S_oy-k2·S_ox)，(k1·S_ox+k2·S_oy)]；

其中，k1＝cos(θ)·S_bx+sin(θ)·S_bz；

S_b＝(S_bx，S_by，S_bz)^T，S_o＝(S_ox，S_oy，S_oz)^T分别为卫星本体坐标系和轨道坐标系中的太阳矢量；

若太阳敏感器数据无效，取ψ＝0；

双矢量定姿算法包括如下步骤：利用卫星本体坐标系中的地磁矢量B_b和太阳矢量S_b，对照轨道坐标系中的地磁矢量B_o和太阳矢量S_o，采用简化后的QUEST双矢量定姿算法，确定卫星的三轴姿态；

单磁强计定姿算法包括如下步骤：当仅三轴磁强计数据有效时，利用卫星本体坐标系中的地磁矢量B_b，对照轨道坐标系中的地磁矢量B_o，近似计算卫星的俯仰角θ：θ＝arctan2[(B_oxB_bz-B_ozB_bx)，(B_ozB_bz+B_oxB_bx)]。

9、根据权利要求6所述的姿态确定方法，其特征在于：

所述定姿选择单元控制所述姿态确定系统选择稳态EKF定姿算法的过程包括：

当地平仪有效、数据正常，且卫星位于阳照区、太阳敏感器正常时，选择红外地平仪/太阳Kalman滤波算法；

当地平仪有效、数据正常，且卫星位于阴影区或太阳敏感器故障时，选择红外地平仪/磁Kalman滤波算法；

当地平仪失效或故障，且卫星位于阳照区、太阳敏感器正常时，选择太阳/磁Kalman滤波算法；

当地平仪失效或故障，且卫星位于阴影区或太阳敏感器故障时，选择单磁Kalman滤波算法。

10、根据权利要求9所述的姿态确定方法，其特征在于：

所述红外地平仪/太阳Kalman滤波算法、红外地平仪/磁Kalman滤波算法、太阳/磁Kalman滤波算法、单磁Kalman滤波算法包括如下步骤：

姿态预估计步骤，状态方程建立步骤，观测量计算及观测方程建立步骤，Kalman滤波迭代计算步骤，姿态校正步骤。

Images