CN100529667C - 一种自主恢复轨控故障时的星敏感器定姿方法 - Google Patents

Abstract

一种自主恢复轨控故障时的星敏感器定姿方法，包括：(1)根据陀螺测量数据预估卫星惯性姿态；(2)根据卫星惯性姿态和星敏感器测量输出的惯性坐标系下的光轴矢量和横轴矢量计算滤波修正的新息量，并计算前后两个周期新息量的误差，用于判断星敏感器数据的一致性；(3)星敏感器数据一致性判别；(4)星敏感器双矢量定姿；(5)在星敏感器数据置过姿态估计的初值的情况下，引入星敏感器，与陀螺组合进行卫星姿态的修正。该方法可提高轨控故障恢复的可靠性，节省故障恢复的时间，保证及时、准确地恢复轨道控制。

Description

一种自主恢复轨控故障时的星敏感器定姿方法

技术领域

本发明涉及一种航天器轨控故障恢复过程中的卫星姿态重新确定方法。特别是经故障处理后，恢复轨控时，引入星敏感器信息重新定姿的方法。

背景技术

目前卫星变轨控制时，姿态确定一般都利用陀螺的测量信息进行姿态预估。若轨控过程中出现故障，则停止本次轨控，待故障排除后再次进行轨控，再次轨控时卫星的姿态估值已经过校正，与卫星的实际姿态是一致的。但这种方法需要较长的时间才能使卫星恢复轨控，影响故障恢复的时间，不能满足轨控窗口唯一性的要求(轨控唯一性要求是指轨控实施要及时，轨控姿态要准确)。若卫星在星上自主完成故障处理及故障恢复，则在卫星故障过程中，卫星姿态估计值可能已偏离实际姿态。在故障恢复过程中，若只靠陀螺数据进行姿态估计，卫星在一个错误初值基础上进行姿态估计，会使卫星的估计姿态与实际姿态相差较大，从而影响故障恢复和后续的轨控效果，难以满足唯一性(准确实施轨控)的要求；若姿态估计靠陀螺预估以及星敏感器修正组合完成，则与只用陀螺估计姿态相比，可以逐渐减小估计姿态和实际姿态的误差，但由于姿态修正量有限，消除估计误差的时间较长，使故障恢复的时间较长，同样会影响轨控窗口唯一性要求(及时轨控)的实现。

发明内容

本发明的技术解决问题：克服现有技术的不足，针对故障恢复时卫星估计姿态与卫星实际姿态相差较大或者需要较长时间恢复姿态的情况，提出了一种星上自主恢复轨控故障时的姿态确定方法。该方法可提高轨控故障恢复的可靠性，节省故障恢复的时间，保证及时、准确地恢复轨道控制。

本发明的技术解决方案：一种自主恢复轨控故障时的星敏感器定姿方法，其特征在于包括：

(1)根据陀螺测量数据预估卫星惯性姿态；

(2)根据卫星惯性姿态和星敏感器测量输出的惯性坐标系下的光轴矢量和横轴矢量计算滤波修正所需的新息量，并计算前后两个相邻周期新息量的误差，用于判断星敏感器数据的一致性；

(3)星敏感器数据一致性判别：首先判断是否用星敏感器数据置过姿态估计的初值，若已经置过，转入步骤(5)，若未置过，则判断步骤(2)中计算的新息量的误差是否至少连续两次在允许的范围之内，如果在允许的范围之内，则转入步骤(4)，若不在允许的范围内，则转入步骤(1)，继续利用陀螺数据预估卫星的惯性姿态；

(4)星敏感器双矢量定姿：利用星敏感器光轴和横轴矢量在卫星本体坐标系和惯性坐标系下的分量，计算卫星惯性姿态，并用该结果更新卫星姿态估计的初值，设置置过初值的标志，然后转入步骤(1)，进行下一个周期的姿态预估；

(5)在星敏感器数据置过姿态估计的初值的情况下，引入星敏感器，与陀螺组合进行卫星姿态的修正。

本发明与现有技术相比的优点在于：

(1)本发明将星敏感器的双矢量定姿方法和陀螺星敏感器组合滤波修正方法结合使用，可及时准确地估计卫星姿态，保证卫星准确及时地实现轨控。

(2)本发明在轨控故障恢复时，首先利用星敏感器测量数据直接进行几何定姿，确定卫星姿态初值，而后再利用常规的陀螺星敏感器组合滤波方法进行姿态估计，有效地缩短了故障恢复的过程，保证了卫星故障恢复的及时和有效性。

(3)本发明通过星敏感器数据的比对，进行星敏感器数据有效性的判别，保证了星敏感器数据的正确性。

(4)本发明的方法可靠，易于星上实现。可广泛应用于各种卫星的轨道控制。特别适用于有关键点或是唯一窗口变轨要求的航天器的轨道控制，如月球探测器的第三次近地点变轨和第一次近月点制动。

附图说明

图1为本发明自主恢复轨控故障时星敏感器定姿方法流程图；

图2为采用本发明方法的仿真曲线图；

图3为采用常规方法的仿真曲线图；

其中，图2、图3的仿真曲线中，曲线1代表qw(0)、曲线2代表qw(1)、曲线3代表qw(2)，分别为卫星实际姿态四元数的矢量部分；曲线4代表q(0)、曲线5代表q(1)、曲线6代表q(2)，分别为卫星姿态四元数矢量部分的星上估计值。

具体实施方式

本发明的自主恢复轨控故障时的星敏感器定姿方法是指变轨发生故障，由星上自主恢复时，利用星敏感器的测量信息，进行卫星的姿态确定。利用几何方法确定卫星的姿态，用此姿态更新星上的姿态估值，之后采用陀螺星敏感器组合滤波方法，进行卫星的姿态估计。为了保证星敏感器数据的正确可靠，在使用星敏感器进行定姿之前，首先进行星敏感器测量输出的一致性判别。利用连续两个采样周期中得到的星敏感器测量量---光轴矢量的误差进行判断，若连续两次以上比对光轴矢量误差在允许范围以内，则认为星敏感器数据是有效的。利用该星敏感器的光轴矢量和横轴矢量进行一次双矢量定姿计算，得到卫星该时刻的惯性姿态。将该惯性姿态更新卫星姿态估计值，用作轨控恢复过程中的姿态估计的初值；最后引入星敏感器，与陀螺组合进行卫星姿态的修正。

本发明的方法具体实施流程参见图1，具体步骤如下：

(1)陀螺预估惯性姿态：根据陀螺测量数据预估卫星惯性姿态。

a陀螺数据转换

利用三个陀螺的测量信息即可得到卫星三轴姿态角速度信息。根据速率积分陀螺的工作原理，每次采样可以得到陀螺在这一采样周期内的角度增量，经过适当转换后可以计算卫星三轴的姿态角速度。

根据参与定姿的三个陀螺(编号i，j，k)在卫星本体坐标系中的安装矩阵B，计算陀螺输出转换矩阵A＝B^-1，结合三个陀螺的测量输出Δg_i，Δg_j，Δg_k，可以得到卫星三轴姿态角增量信息Δg_x，Δg_y，Δg_z。由下式计算：

$\left[\begin{array}{c}{\mathrm{\Δg}}_x\\ {\mathrm{\Δg}}_y\\ {\mathrm{\Δg}}_z\end{array}\right]=A\left[\begin{array}{c}{\mathrm{\Δg}}_i\\ {\mathrm{\Δg}}_j\\ {\mathrm{\Δg}}_k\end{array}\right]$

b三轴绝对角速度计算

根据陀螺数据转换的计算结果，计算一个采样周期内的三轴绝对角速度： $\widehat{\mathrm{\ω}}={\left[\begin{array}{ccc}{\widehat{\mathrm{\ω}}}_x& {\widehat{\mathrm{\ω}}}_y& {\widehat{\mathrm{\ω}}}_z\end{array}\right]}^T.$ 计算中注意减掉经过标定的陀螺常值漂移量 $\hat{b}={\left[\begin{array}{ccc}{\hat{b}}_x& {\hat{b}}_y& {\hat{b}}_z\end{array}\right]}^T,$ 单位：弧度/小时。

${\widehat{\mathrm{\ω}}}_x=\mathrm{\Δ}{g}_x/\mathrm{\Δt}-{\hat{b}}_x/3600$

${\widehat{\mathrm{\ω}}}_y=\mathrm{\Δ}{g}_y/\mathrm{\Δt}-{\hat{b}}_y/3600$

${\widehat{\mathrm{\ω}}}_z=\mathrm{\Δ}{g}_z/\mathrm{\Δt}-{\hat{b}}_z/3600$

c惯性姿态预估

根据计算得到的三轴绝对角速度，计算卫星在本周期内的三轴角速度增量，采用如下公式预估卫星的惯性姿态，以四元数形式 $\widehat{\stackrel{\‾}{q}}=\left[\begin{array}{cccc}{\hat{q}}_1& {\hat{q}}_2& {\hat{q}}_3& {\hat{q}}_4\end{array}\right]$ 给出。

$\mathrm{\Δ}\hat{g}=\widehat{\mathrm{\ω}}\·\mathrm{\Δt}$

$\widehat{\stackrel{\‾}{q}}=\widehat{\stackrel{\‾}{q}}+\frac{1}{2}\mathrm{Eq}\left(\widehat{\stackrel{\‾}{q}}\right)\mathrm{\Δ}\hat{g}$

若 ${\hat{q}}_4<0,$ 则 $\widehat{\stackrel{\&OverBar;}{q}}=-\widehat{\stackrel{\&OverBar;}{q}}$

$\widehat{\stackrel{\&OverBar;}{q}}=\widehat{\stackrel{\&OverBar;}{q}}/\mathrm{Norm}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)$

注：上述公式中Δt为采样周期。函数Eq为利用卫星姿态角速度信息预估卫星姿态四元数的计算公式。Norm为四元数取模的函数。函数定义如下：

函数a＝Norm(B)

函数名：Norm

输入：B＝[b1，b2，...，bn]T，n表示向量B的维数

输出：a

函数内容：

{

$a=\sqrt{b_1^2+b_2^2+...+b_n^2}$

}

函数A＝Eq(B)

函数名：Eq

输入：B＝[b₁，b₂，b₃，b₄]^T

输出：A＝(a_ij)_4×3

函数内容：

{

   a₁₁＝b₄

   a₁₂＝-b₃

   a₁₃＝b₂

   a₂₁＝b₃

   a₂₂＝b₄

   a₂₃＝-b₁

   a₃₁＝-b₂

   a₃₂＝b₁

   a₃₃＝b₄

   a₄₁＝-b₁

   a₄₂＝-b₂

   a₄₃＝-b₃

}

(2)星敏感器测量数据处理

根据所用星敏感器测量输出的惯性坐标系下的光轴矢量Z_I和横轴矢量X_l计算滤波修正的新息量δZ。并计算前后两个周期新息量的误差ΔZ，用于后续判断星敏感器数据的一致性。

Y₁＝Z_I×X_I

${\mathrm{\&delta;Z}}_x=X_B\&times;(\mathrm{Aq}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)\&CenterDot;X_I)$

${\mathrm{\&delta;Z}}_y=Y_B\&times;(\mathrm{Aq}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)\&CenterDot;Y_I)$

${\mathrm{\&delta;Z}}_z=Z_B\&times;(\mathrm{Aq}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)\&CenterDot;Z_I)$

$\mathrm{\&delta;Z}=\frac{1}{2}({\mathrm{\&delta;Z}}_x+\mathrm{\&delta;}{Z}_y+\mathrm{\&delta;}{Z}_z)$

ΔZ＝|δZ_pst-δZ|

δZ_pst＝δZ(记录的本周期计算的信息量，用于下一周期计算的比对)

其中，Z_I为星敏感器测量得到的星敏感器光轴矢量在地心惯性坐标系下的表示；X_I为星敏感器测量得到的星敏感器横轴矢量在地心惯性坐标系下的表示；X_B，Y_B，Z_B分别为星敏感器坐标系的三个坐标轴(横轴(X_B)，光轴(Z_B)，Y_B与X_B，Z_B满足右手定则，Y_B＝Z_B×X_B)在卫星本体坐标系下的表示，根据星敏感器在卫星上的具体安装位置确定。Aq(·)为根据卫星估计姿态计算的卫星本体坐标系相对于惯性坐标系的姿态转换矩阵。δZ_pst为记录的前一周期的新息量。

(3)星敏感器数据一致性判别

首先判断是否利用星敏感器测量数据置过姿态估计的初值。若已经置过，则转入步骤(5)，进行后续的星敏感器滤波修正。若未置过姿态估计的初值，则判断新息误差量ΔZ是否连续两次以上均在允许的范围之内。若条件满足，则转入步骤(4)，进行双矢量定姿，并用定姿结果更新卫星姿态估计值。

(4)星敏感器双矢量定姿

利用星敏感器光轴和横轴矢量在卫星本体坐标系和惯性坐标系下的分量，计算卫星本体坐标系相对于惯性坐标系的姿态转换矩阵A：A＝Avv(X₁，Z_I，X_B，Z_B)。此函数为用两个矢量分别在卫星本体坐标系和惯性坐标系的分量进行双矢量定姿，计算卫星本体坐标系相对于惯性坐标系的姿态四元数的函数，函数定义如下：

  函数A＝Avv(X_1I，X_2I，X_1b，X_2b)

  函数名Avv

  输入：X_1I，X_2I，X_1b，X_2b(均为单位矢量)

  输出：A＝(a_ij)_3×3

  函数内容：

{

     V_2I＝X_1I×X_2I；

     V_2I＝V_2I/Norm(V_2I)；

     V_3I＝X_1I×V_2I；

     V_2b＝X_1b×X_2b；

     V_2b＝V_2b/Norm(V_2b)；

     V₃＝X_1b×V_2b；

$A=X_{1b}{X}_{1I}^T+V_{2b}{V}_{2I}^T+V_{3b}{V}_{3I}^T$

}

根据双矢量定姿确定的姿态转换矩阵A，计算卫星惯性姿态四元数： $\stackrel{\&OverBar;}{q}=\mathrm{Qa}\left(A\right).$ 此函数为用姿态转换矩阵计算卫星四元数的通用公式，函数定义如下：

函数A＝Qa(B)

函数名：Qa

输入：B＝(b_ij)_3×3

输出：A＝[a₁，a₂，a₃，a₄]·^T

函数内容：

   if(b₁₁+b₂₂+b₃₃+1≥0.004)

   {

$a_4=\left(\sqrt{b_{11}+b_{22}+b_{33}+1}\right)/2$

           a₁＝(b₂₃-b₃₂)/(4·a₄)

           a₂＝(b₃₁-b₁₃)/(4·a₄)

           a₃＝(b₁₂-b₂₁)/(4·a₄)

   }

   else if(b₁₁+b₂₂+b₃₃+1＜0.004且1-b₁₁+b₂₂-b₃₃≥0.004)

   {

$a_2=\left(\sqrt{1-b_{11}+b_{22}-b_{33}}\right)/2\&CenterDot;\mathrm{sgn}(b_{31}-b_{13})$

          (若b₃₁-b₁₃＝0，则取sgn(b₃₁-b₁₃)＝1)

          a₁＝(b₂₁+b₁₂)/(4·a₂)

          a₃＝(b₃₂+b₂₃)/(4·a₂)

          a₄＝(b₃₁-b₁₃)/(4·a₂)

}

else if(b₁₁+b₂₂+b₃₃+1＜0.004且1-b₁₁+b₂₂-b₃₃＜0.004

           且1+b₁₁-b₂₂-b₃₃≥0.004)

{

$a_1=\left(\sqrt{1+b_{11}-b_{22}-b_{33}}\right)/2\&CenterDot;\mathrm{sgn}(b_{23}-b_{32})$

               (若b₂₃-b₃₂＝0，则取sgn(b₂₃-b₃₂)＝1)

               a₂＝(b₂₁+b₁₂)/(4·a₁)

               a₃＝(b₁₃+b₃₁)/(4·a₁)

               a₄＝(b₂₃-b₃₂)/(4·a₁)

   }

   else

   {

$a_3=\left(\sqrt{1-b_{11}-b_{22}+b_{33}}\right)/2\&CenterDot;\mathrm{sgn}(b_{12}-b_{21})$

           (若b₁₂-b₂₁＝0，则取sgn(b₁₂-b₂₁)＝1)

           a₁＝(b₁₃+b₃₁)/(4·a₃)

           a₂＝(b₂₃+b₃₂)/(4·a₃)

           a₄＝(b₁₂-b₂₁)/(4·a₃)

    }

用此计算结果更新卫星星上的四元数估计值： $\widehat{\stackrel{\&OverBar;}{q}}=\stackrel{\&OverBar;}{q}$ (

为卫星中估计的姿态四元数)，并且设置一个用星敏感器设置过卫星姿态估计初值的标志，用于判断后续程序是否还需要继续判断星敏感器数据的一致性。

(5)引入星敏感器姿态修正方法

卫星姿态预估初值设置以后，采用常规的星敏感器滤波算法进行实时姿态修正。滤波算法设计按照经典的Kalman滤波原理进行即可。月球探测器中采用的滤波算法如下：

$\left[\begin{array}{c}\mathrm{\&delta;}\hat{q}\\ \mathrm{\&Delta;}\hat{b}\end{array}\right]=K_s\mathrm{\&delta;Z}$

$\widehat{\stackrel{\&OverBar;}{q}}=\widehat{\stackrel{\&OverBar;}{q}}+\mathrm{Eq}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)\&CenterDot;\mathrm{\&delta;}\hat{q}$

若 ${\hat{q}}_4<0,$ 则 $\widehat{\stackrel{\&OverBar;}{q}}=-\widehat{\stackrel{\&OverBar;}{q}}$

$\widehat{\stackrel{\&OverBar;}{q}}=\widehat{\stackrel{\&OverBar;}{q}}/\mathrm{Norm}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)$

$\hat{b}=\hat{b}+\mathrm{\&Delta;}\hat{b}$

这里K_s为滤波修正系数，

为陀螺常值漂移的标定量。

根据上述实施步骤，针对某月球探测卫星进行了仿真试验，仿真曲线如图2所示。仿真过程为：轨控点火过程中，卫星出现故障，导致卫星姿态偏离目标点火姿态，在仿真时间272秒时由轨控定向模式自主转入速率阻尼模式。在速率阻尼模式中，引入星敏感器数据进行双矢量定姿，利用定姿结果，设置了卫星的姿态估计值，此后继续使用星敏感器和陀螺联合滤波方法进行姿态估计。从仿真曲线上看，故障过程中卫星的估计姿态已经偏离实际姿态，由于星敏感器数据定姿结果的引入，使卫星的估计姿态很快收敛于真实姿态，卫星姿态误差小于0.0005(rad)。

为了比较本发明方法达到的效果，图3给出了采用常规的方法进行故障恢复定姿的仿真曲线。仿真过程与上述的仿真过程一致，只是未引入星敏感器定姿数据设置卫星的姿态估值。从仿真曲线来看，由于未引入星敏感器双矢量定姿结果，故障恢复后，卫星星上预估的姿态四元数仍然与卫星真实姿态相差较大，卫星姿态误差在0.08(rad)左右。

以上所描述的系统只是本发明的一种情况，本领域技术人员可以根据不同的要求和设计参数在不偏离本发明的情况下进行各种增补、改进和更换，因此，本发明是广泛的。

Claims (3)

1、一种自主恢复轨控故障时的星敏感器定姿方法，其特征在于包括：

(1)根据陀螺测量数据预估卫星惯性姿态；

(2)根据卫星惯性姿态和星敏感器测量输出的惯性坐标系下的光轴矢量和横轴矢量计算滤波修正所需的新息量，并计算前后两个相邻周期新息量的误差，用于判断星敏感器数据的一致性；

(3)星敏感器数据一致性判别：首先判断是否用星敏感器数据置过姿态估计的初值，若已经置过，转入步骤(5)，若未置过，则判断步骤(2)中计算的新息量的误差是否至少连续两次在允许的范围之内，如果在允许的范围之内，则转入步骤(4)，若不在允许的范围内，则转入步骤(1)，继续利用陀螺数据预估卫星的惯性姿态；

(4)星敏感器双矢量定姿：利用星敏感器光轴和横轴矢量在卫星本体坐标系和惯性坐标系下的分量，计算卫星惯性姿态，并用该结果更新卫星姿态估计的初值，设置置过初值的标志，然后转入步骤(1)，进行下一个周期的姿态预估；

(5)在星敏感器数据置过姿态估计的初值的情况下，引入星敏感器，与陀螺组合进行卫星姿态的修正。

2、根据权利要求1所述的一种自主恢复轨控故障时的星敏感器定姿方法，其特征在于所述步骤(1)中预估卫星惯性姿态的方法是：首先根据陀螺的测量信息计算卫星的姿态角速度，然后计算三轴绝对角速度，最后根据绝对角速度，预估卫星的惯性姿态；

3、根据权利要求1所述的一种自主恢复轨控故障时的星敏感器定姿方法，其特征在于：所述步骤(2)中计算滤波修正的新息量δZ和新息量的误差ΔZ的方法是：

Y_I＝Z_I×X_I

$\mathrm{\&delta;}{Z}_x=X_B\&times;(\mathrm{Aq}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)\&CenterDot;X_I)$

$\mathrm{\&delta;}{Z}_y=Y_B\&times;(\mathrm{Aq}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)\&CenterDot;Y_I)$

$\mathrm{\&delta;}{Z}_z=Z_B\&times;(\mathrm{Aq}\left(\widehat{\stackrel{\&OverBar;}{q}}\right)\&CenterDot;Z_I)$

$\mathrm{\&delta;Z}=\frac{1}{2}(\mathrm{\&delta;}{Z}_x+\mathrm{\&delta;}{Z}_y+\mathrm{\&delta;}{Z}_z)$

ΔZ＝|δZ_pst-δZ|

δZ_pst＝δZ

其中，Z_I为星敏感器测量得到的星敏感器光轴矢量在地心惯性坐标系下的表示；X_I为星敏感器测量得到的星敏感器横轴矢量在地心惯性坐标系下的表示；X_B，Y_B，Z_B分别为星敏感器坐标系的三个坐标轴在卫星本体坐标系下的表示，即横轴X_B，光轴Z_B，Y_B与X_B，Z_B满足右手定则，Y_B＝Z_B×X_B，根据星敏感器在卫星上的具体安装位置确定，Aq(·)为根据卫星估计姿态计算的卫星本体坐标系相对于惯性坐标系的姿态转换矩阵，δZ_pst为记录前一周期的新息量。

Images