CN102114918B - 一种基于多速率敏感器组合定姿的姿控反馈回路 - Google Patents

Abstract

本发明涉及一种基于多速率敏感器组合定姿的姿控反馈回路，该回路包括姿态敏感器子系统、基于多尺度建模的姿态确定子系统和姿态控制子系统。姿态敏感器子系统由陀螺、太阳敏感器和磁强计组成，输出具有多速率特性的姿态测量信息；姿态确定子系统利用多尺度分析方法对各敏感器的测量信息在不同尺度上进行描述，基于所建立的多尺度模型对卫星姿态角进行估计；姿态控制子系统对卫星姿态进行控制，并将控制后的姿态信息反馈到姿态敏感器子系统，从而形成姿态控制回路。本发明的姿控反馈回路利用多尺度分析方法进行了多尺度建模，在降低组合定姿过程的复杂性的同时，提高了姿态确定精度。

Description

一种基于多速率敏感器组合定姿的姿控反馈回路

技术领域

本发明涉及一种基于多速率敏感器组合定姿的姿控反馈回路，属于航天器姿态确定领域。

背景技术

作为空间环境探测的主要手段，卫星技术研究尤为重要，各国都投入了大量的人力物力进行研究。随着航天科技的发展，卫星对姿态稳定指向或机动控制能力要求越来越高，系统功能也越来越复杂，运行寿命要求更长，卫星的效能越来越高，开始具备执行多任务的能力。总体上讲，多任务、高精度、长寿命、高可靠性的卫星已成为新的发展趋势。

卫星系统中最重要的技术之一是姿态控制系统的设计，它主要包括姿态确定与姿态控制。姿态确定是研究卫星相对于某个基准的姿态定位，姿态控制是指卫星在规定或预先确定的方向上定向的过程。随着对卫星高精度、长寿命、高可靠性的要求不断提高，对于卫星姿态控制系统的精度和稳定性要求越来越高，而提高姿态控制的前提是提高卫星姿态确定的精度。卫星姿态确定的精度，不仅取决于各敏感器的精度，还与姿态确定算法有关。为了保证姿态确定算法的正确性、有效性，降低卫星研制风险，需要通过大量的地面仿真实验对姿态确定算法进行验证。目前，卫星上所用姿态敏感器的输出频率不同，即具有多速率特性。

发明内容

本发明的技术解决问题是：针对卫星上所用敏感器输出频率不同的问题，利用多尺度分析方法进行姿控反馈回路中多速率敏感器组合定姿建模，在降低组合定姿复杂性的同时，提高定姿精度。

本发明的技术解决方案是：一种基于多速率敏感器组合定姿的姿控反馈回路，由姿态敏感器子系统(4)、基于多尺度建模的姿态确定子系统(5)和姿态控制子系统(6)组成。各子系统如下：

1、姿态敏感器子系统输出具有多速率特性的测量信息

姿态敏感器子系统(4)包括陀螺(1)、太阳敏感器(2)和磁强计(3)，陀螺(1)、太阳敏感器(2)和磁强计(3)分别产生卫星本体坐标系相对于惯性坐标系的姿态角速度、本体坐标系下的太阳矢量和地磁矢量。陀螺、太阳敏感器和磁强计输出测量信息的频率不同，其中陀螺和太阳敏感器的输出频率是磁强计输出频率的两倍，即姿态敏感器子系统4输出的测量信息具有多速率特性。

2、基于多尺度建模的姿态确定子系统估计卫星姿态

基于多尺度建模的姿态确定子系统(5)以四元数和陀螺漂移为状态变量；基于姿态运动学和陀螺数学模型建立姿态确定状态方程；基于太阳敏感器和磁强计的观测信息，利用多尺度分析方法建立卫星姿态确定子系统的多尺度模型；最后在多尺度建模的基础上利用UKF滤波算法，将姿态敏感器子系统(4)输出的具有多速率特性的测量信息进行融合，实现对当前卫星姿态角的估计，将卫星姿态角输出给姿态控制子系统(6)。

3、姿态控制子系统控制卫星姿态

姿态控制子系统(6)包括姿态控制器和执行机构两部分。姿态控制子系统(6)根据期望姿态角与基于多尺度建模的姿态确定子系统(5)估计的姿态角之间的偏差，通过姿态控制器和执行机构对卫星姿态进行控制，再经过卫星动力学模型和运动学模型输出卫星控制后的姿态信息，最后将姿态信息输出到姿态敏感器子系统(4)，从而形成姿控反馈回路。

本发明的原理是：卫星姿态控制反馈回路主要由姿态敏感器子系统、姿态确定子系统、姿态控制器和执行机构组成。姿态敏感器子系统用于输出多速率姿态测量信息，并基于这些信息，利用多尺度方法估计出当前姿态角；姿态控制器根据期望姿态角与估计出的当前姿态角之间的偏差产生控制信息，驱动执行机构控制卫星姿态，最后通过卫星姿态动力学和运动学计算出被控制后的姿态角，并反馈到姿态敏感器子系统中，从而形成姿控反馈回路。

本发明与现有技术相比的优点在于：利用多尺度分析方法，建立姿控反馈回路中姿态确定子系统的多尺度系统模型，在降低组合定姿复杂性的同时，提高定姿精度。

附图说明

图1为本发明的一种基于多速率敏感器组合定姿的姿控反馈回路框图；

具体实施方式

如图1所示，本发明的仿真回路由姿态敏感器子系统4、基于多尺度建模的姿态确定子系统5和姿态控制子系统6组成。具体如下：

1)、姿态敏感器子系统输出具有多速率特性的测量信息

姿态敏感器子系统4包括陀螺1、太阳敏感器2和磁强计3，分别产生本体坐标系相对于惯性坐标系的姿态角速度、卫星本体坐标系下的太阳矢量和地磁矢量。陀螺、太阳敏感器和磁强计输出测量信息的频率不同，其中陀螺和太阳敏感器的输出频率是磁强计输出频率的两倍，即姿态敏感器子系统4输出的测量信息具有多速率特性。

a.陀螺测量信息的产生

陀螺1输出的本体坐标系相对于惯性坐标系的姿态角速度，其产生数据的具体步骤如下：

①由欧拉角微分方程计算本体坐标系相对于轨道坐标系的角速度

设卫星由轨道坐标系到本体坐标系按Z-X-Y顺序旋转，令

θ和ψ分别为横滚角、俯仰角和偏航角。根据欧拉角微分方程可得卫星本体坐标系相对于轨道坐标系的姿态角速度

为：

②计算本体坐标系下的轨道角速度

轨道角速度在轨道坐标系下表示为

其中ω_o为卫星轨道坐标系相对于惯性坐标系的角速率。经过姿态矩阵

的转换得到本体坐标系下的轨道角速度

本体坐标系与轨道坐标系之间的姿态矩阵为：

则本体坐标系下的轨道角速度为：

${\mathrm{\ω}}_{\mathrm{io}}^b=T_o^b\·{\mathrm{\ω}}_{\mathrm{io}}^o---\left(3\right)$

③生成陀螺数据

陀螺输出本体坐标系相对于惯性坐标系的姿态角速度，由于陀螺存在漂移和测量噪声，所以陀螺输出ω_gyro为：

${\mathrm{\ω}}_{\mathrm{gyro}}={\mathrm{\ω}}_{\mathrm{io}}^b+{\mathrm{\ω}}_{\mathrm{ob}}^b+b+\mathrm{\ξ}---\left(4\right)$

式中，为本体坐标系下的轨道角速度，

为本体坐标系相对于轨道坐标系的姿态角速度，b为陀螺的常值漂移；ξ为陀螺的测量噪声。

b.太阳敏感器测量信息的产生

太阳敏感器2输出本体坐标坐标系下的太阳矢量Sun_b，其产生数据的具体步骤如下：

①轨道坐标系下的太阳矢量Sun_o

轨道坐标系相对于惯性坐标系的姿态矩阵为：

$T_i^o=T_p^o\·T_i^p---\left(5\right)$

式中，

为地心轨道坐标系到轨道坐标系的姿态矩阵；

为地心惯性坐标系到地心轨道坐标系的姿态矩阵。

$T_i^p=\left[\begin{array}{ccc}\cos (\mathrm{\ω}+f)& \sin (\mathrm{\ω}+f)& 0\\ -\sin (\mathrm{\ω}+f)& \cos (\mathrm{\ω}+f)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(i\right)& \sin \left(i\right)\\ 0& -\sin \left(i\right)& \cos \left(i\right)\end{array}\right]\left[\begin{array}{ccc}\cos \left(\mathrm{\Ω}\right)& \sin \left(\mathrm{\Ω}\right)& 0\\ -\sin \left(\mathrm{\Ω}\right)& \cos \left(\mathrm{\Ω}\right)& 0\\ 0& 0& 1\end{array}\right]---\left(6\right)$

$T_p^o=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& -1\\ -1& 0& 0\end{array}\right]---\left(7\right)$

式中，Ω为轨道升交点赤经；i为轨道倾角；ω为近地点幅角；f为真近地点。

根据太阳历模型可确定惯性坐标系下的太阳矢量Sun_i，则轨道坐标系下的太阳矢量为：

$\mathrm{Sun}\_o=T_i^o\·\mathrm{Sun}\_i---\left(8\right)$

②太阳敏感器输出数据

太阳敏感器的测量噪声是v_s，根据(2)式的姿态矩阵

可得太阳敏感器的输出：

$\mathrm{Sun}\_b=T_o^b\·\mathrm{Sun}\_o+v_s---\left(9\right)$

c.磁强计测量信息的产生

磁强计3输出本体坐标系下的地磁场矢量，其产生数据的具体步骤如下：

①轨道坐标系下的地磁矢量

根据卫星所在地球轨道坐标系的位置，利用世界地磁模型2005确定出北东地地理坐标系下的地磁场矢量Mag_t，再根据地理坐标系、惯性坐标系、轨道坐标系之间的姿态矩阵，可确定轨道坐标系下的地磁矢量Mag_o：

$\mathrm{Mag}\_o=T_i^o\·T_t^i\·\mathrm{Mag}\_t---\left(10\right)$

式中，

为(5)式中轨道坐标系相对于惯性坐标系的姿态矩阵；

为地理坐标系相对于惯性坐标系的姿态矩阵。

②磁强计输出数据

磁强计的测量噪声是v_m，根据(2)式的姿态矩阵

可得磁强计的输出：

$\mathrm{Mag}\_b=T_o^b\·\mathrm{Mag}\_o+v_m---\left(11\right)$

2、基于多尺度建模的姿态确定子系统估计卫星姿态

基于多尺度建模的姿态确定子系统5选取四元数来描述卫星本体坐标系相对于轨道坐标系的姿态；由于陀螺存在漂移，在实际应用中，陀螺输出不能直接用于姿态确定，需要在估计四元数的过程中将陀螺漂移估计出来，因此，该姿态确定子系统选取四元数和陀螺漂移为状态变量。

太阳敏感器和磁强计输出测量信息的频率不同，即两个敏感器的测量信息是对同一个卫星系统在不同尺度上获取的，具有多尺度特性。本发明利用多尺度分析方法对姿态确定子系统各敏感器的测量信息在不同尺度上进行描述和分析，把太阳敏感器和磁强计两者原本分散的观测信息依据其内在关系联系起来，建立姿态确定子系统的多尺度观测方程。

将姿态运动学方程和陀螺数学模型相结合，基于多尺度分析，建立陀螺/太阳敏感器/磁强计多速率组合定姿的多尺度状态空间模型，利用UKF滤波算法估计出卫星当前姿态角、估计误差方差阵，并将当前姿态反馈给姿态控制子系统6，同时将当前姿态估计值和估计误差方差阵保存作为下一时刻滤波解算的初始值。

具体实现步骤如下：

a.姿态确定子系统多尺度状态空间模型

①多尺度状态方程

以四元数和陀螺漂移为状态变量，即x＝[q₀ q₁ q₂ q₃ b_x b_y b_z]^T，状态变量维数为N_x＝7。根据卫星姿态运动学方程和陀螺漂移特性，有

$\stackrel{\·}{q}=\frac{1}{2}q\⊗{\mathrm{\ω}}_{\mathrm{ob}}^b,$ $\stackrel{\·}{b}=0---\left(12\right)$

式中，q＝[q₀ q₁ q₂ q₃]^T为卫星本体坐标系相对于轨道坐标系的四元数；q₀和

分别为四元数的标量部分和矢量部分；b＝[b_x b_y b_z]^T为陀螺漂移；卫星本体坐标系相对于轨道坐标系的姿态角速度是

$q\⊗=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]---\left(13\right)$

姿态确定子系统的状态方程是：

$\stackrel{\·}{x}=\left[\begin{array}{c}\stackrel{\·}{q}\\ \stackrel{\·}{b}\end{array}\right]=\mathrm{fh}(q,b)+\left[\begin{array}{c}-0.5q\⊗{\left[\begin{array}{cc}0& {\mathrm{\ξ}}^T\end{array}\right]}^T\\ O_{3\×1}\end{array}\right]---\left(14\right)$

式中，系统误差是

将式(14)离散化，可得：

x(k+1)＝f(x(k))+w(k)    (15)

式中，

在此姿态确定子系统中，N_x＝7为状态变量维数；w(k)为零均值高斯白噪声，其方差为Q(k)。

由于陀螺和太阳敏感器的输出频率是磁强计输出频率的两倍，则太阳敏感器对应于细尺度，即尺度1，磁强计对应于粗尺度，即尺度2。设ΔT为磁强计的输出周期，ΔT时间间隔内两个尺度上的状态称为一个状态块，测量称为一个数据块。n表示第n个时间块，即第nΔT时间间隔。在第n个时间块内，尺度2上有一个状态节点，编号是n；尺度1上有两个状态节点，编号分别是2n和2n+1。每个ΔT内，当有了一个新的数据块时，姿态估计被更新。姿态确定子系统的多尺度状态结构满足二叉树结构，令x₂(n)表示第nΔT时间间隔内尺度2上的状态，x₁(2n)和x₁(2n+1)表示第nΔT时间间隔内尺度1上的状态。基于Haar小波，尺度2上的状态x₂(n)与尺度1上的状态x₁(2n)和x₁(2n+1)的状态空间投影关系为：

$x_2\left(n\right)=(\sqrt{2}/2)(x_1\left(2n\right)+x_1(2n+1))---\left(16\right)$

第(n+1)ΔT时间间隔内，根据式(15)，可得尺度1上的离散状态方程：

x₁(2n+2)＝f(x₁(2n+1))+w(2n+1)               (17)

则

x₁(2n+3)＝f[f(x₁(2n+1))+w(2n+1)]+w(2n+2)    (18)

令

$\stackrel{\‾}{x}\left(n\right)=\left[\begin{array}{c}{x}_1\left(2n\right)\\ x_1(2n+1)\end{array}\right],$ $\stackrel{\‾}{x}(n+1)=\left[\begin{array}{c}{x}_1(2n+2)\\ x_1(2n+3)\end{array}\right]---\left(19\right)$

其中，是2N_x×1维矢量。令

F(n+1，0)＝f(x₁(2n+1))+w(2n+1)              (20)

F(n+1，1)＝f[f(x₁(2n+1))+w(2n+1)]+w(2n+2)   (21)

可以看出，

是

的非线性函数。多尺度状态方程为：

$\stackrel{\‾}{x}(n+1)=\left[\begin{array}{c}F(n+\mathrm{1,0})\\ F(n+\mathrm{1,1})\end{array}\right]---\left(22\right)$

②多尺度测量方程

对于尺度1上的太阳敏感器，离散的测量方程是

Sun_b(2n+m₁)＝g₁(x₁(2n+m₁))+v_s(2n+m₁)    (23)

其中

g₁(x₁(2n+m₁))＝A(q(2n+m₁))·Sun_o(2n+m₁) (24)

式中，m₁＝0，1；Sun_b(2n+m₁)和Sun_o(2n+m₁)分别为2n+m₁时刻本体坐标系和轨道坐标系下的太阳矢量；A(q(2n+m₁))为2n+m₁时刻本体坐标系相对于轨道坐标系的姿态矩阵；v_s(2n+m₁)为2n+m₁时刻太阳敏感器的量测噪声，其噪声方差阵为R₁。令

$M_1\left(0\right)=\left[\begin{array}{cc}{I}_{N_x\×N_x}& O_{N_x\×N_x}\end{array}\right],$ $M_1\left(1\right)=\left[\begin{array}{cc}{O}_{N_x\×N_x}& I_{N_x\×N_x}\end{array}\right]---\left(25\right)$

式中，

表示N_x维的单位阵；

表示N_x维的零矩阵。

则

$x_1(2n+m_1)=M_1\left(m_1\right)\stackrel{\‾}{x}\left(n\right)---\left(26\right)$

$\mathrm{Sun}\_b(2n+m_1)=g_1\left(M_1\left(m_1\right)\stackrel{\‾}{x}\left(n\right)\right)+v_s(2n+m_1)---\left(27\right)$

令

$\stackrel{\‾}{S}\mathrm{un}\_b\left(n\right)=\left[\begin{array}{c}\mathrm{Sun}\_b\left(2n\right)\\ \mathrm{Sun}\_b(2n+1)\end{array}\right]---\left(28\right)$

则

$\stackrel{\‾}{S}\mathrm{un}\_b\left(n\right)=\left[\begin{array}{c}{g}_1\left(M_1\left(0\right)\stackrel{\‾}{x}\left(n\right)\right)\\ g_1\left(M_1\left(1\right)\stackrel{\‾}{x}\left(n\right)\right)\end{array}\right]+\left[\begin{array}{c}{v}_s\left(2n\right)\\ v_s(2n+1)\end{array}\right]---\left(29\right)$

令

${\stackrel{\‾}{G}}_1\left(\stackrel{\‾}{x}\left(n\right)\right)=\left[\begin{array}{c}{g}_1\left(M_1\left(0\right)\stackrel{\‾}{x}\left(n\right)\right)\\ g_1\left(M_1\left(1\right)\stackrel{\‾}{x}\left(n\right)\right)\end{array}\right],$ ${\stackrel{\‾}{v}}_s\left(n\right)=\left[\begin{array}{c}{v}_s\left(2n\right)\\ v_s(2n+1)\end{array}\right]---\left(30\right)$

是的非线性函数，

的方差为

${\stackrel{\‾}{R}}_1=\left[\begin{array}{cc}{R}_1& O_{3\×3}\\ O_{3\×3}& R_1\end{array}\right]---\left(31\right)$

式中，O_3×3为3×3的零矩阵。则

$\stackrel{\‾}{S}\mathrm{un}\_b\left(n\right)={\stackrel{\‾}{G}}_1\left(\stackrel{\‾}{x}\left(n\right)\right)+{\stackrel{\‾}{v}}_s\left(n\right)---\left(32\right)$

对于尺度2上的磁强计，离散的测量方程是

Mag_b(n)＝g₂(x₂(n))+v_m(n)    (33)

式中，g₂(x₂(n))＝A(q(n))·Mag_o(n)；Mag_b(n)和Mag_o(n)分别为第n个时间块本体坐标系和轨道坐标系下的地磁矢量；A(q(n))为第n个时间块本体坐标系相对于轨道坐标系的姿态矩阵；v_m(n)为第n个时间块磁强计的量测噪声，其噪声方差阵为R₂。令

$M_2=\left[\begin{array}{cc}(\sqrt{2}/2)I_{N_x\×N_x}& (\sqrt{2}/2)I_{N_x\×N_x}\end{array}\right]---\left(34\right)$

则

$x_2\left(n\right)=M_2\stackrel{\‾}{x}\left(n\right)---\left(35\right)$

则

$\mathrm{Mag}\_b\left(n\right)=g_2\left(M_2\stackrel{\‾}{x}\left(n\right)\right)+v_m\left(n\right)---\left(36\right)$

令

$\stackrel{\‾}{M}\mathrm{ag}\_b\left(n\right)=\mathrm{Mag}\_b\left(n\right),$ ${\stackrel{\‾}{G}}_2\left(\stackrel{\‾}{x}\left(n\right)\right)=g_2\left(M_2\stackrel{\‾}{x}\left(n\right)\right),$ ${\stackrel{\‾}{v}}_m\left(n\right)=v_m\left(n\right)---\left(37\right)$

是的非线性函数，

的方差是

则

$\stackrel{\‾}{M}\mathrm{ag}\_b\left(n\right)\left(n\right)={\stackrel{\‾}{G}}_2\left(\stackrel{\‾}{x}\left(n\right)\right)+{\stackrel{\‾}{v}}_m\left(n\right)---\left(38\right)$

令

$\stackrel{\‾}{z}\left(n\right)=\left[\begin{array}{c}\stackrel{\‾}{M}\mathrm{ag}\_b\left(n\right)\\ \stackrel{\‾}{S}\mathrm{un}\_b\left(n\right)\end{array}\right],$ $\stackrel{\‾}{G}\left(\stackrel{\‾}{x}\left(n\right)\right)=\left[\begin{array}{c}{\stackrel{\‾}{G}}_2\left(\stackrel{\‾}{x}\left(n\right)\right)\\ {\stackrel{\‾}{G}}_1\left(\stackrel{\‾}{x}\left(n\right)\right)\end{array}\right],$ $\stackrel{\‾}{v}\left(n\right)=\left[\begin{array}{c}{\stackrel{\‾}{v}}_m\left(n\right)\\ {\stackrel{\‾}{v}}_s\left(n\right)\end{array}\right]---\left(39\right)$

可得多尺度测量方程为：

$\stackrel{\‾}{z}\left(n\right)=\stackrel{\‾}{G}\left(\stackrel{\‾}{x}\left(n\right)\right)+\stackrel{\‾}{v}\left(n\right)---\left(40\right)$

是

的非线性函数；白噪声的方差是

$\stackrel{\‾}{R}=\left[\begin{array}{cc}{\stackrel{\‾}{R}}_2& O_{3\×9}\\ P_{9\×3}& {\stackrel{\‾}{R}}_1\end{array}\right]---\left(14\right)$

式中，O_3×9和O_9×3分别为3×9和9×3的零矩阵。

将式(22)和式(40)写在一起，得到姿态确定子系统的多尺度状态空间模型：

$\left\{\begin{array}{c}\stackrel{\‾}{x}(n+1)=\left[\begin{array}{c}F(n+\mathrm{1,0})\\ F(n+\mathrm{1,1})\end{array}\right]\\ \stackrel{\‾}{z}\left(n\right)=\stackrel{\‾}{G}\left(\stackrel{\‾}{x}\left(n\right)\right)+\stackrel{\‾}{v}\left(n\right)\end{array}\right.---\left(42\right)$

b.估计姿态角

式(42)所表示的状态空间模型是一组非线性方程。利用UKF，可以得到

的估计值，从而得到了第nΔT时间间隔内的四元数。根据四元数与姿态角之间的关系，将四元数转换为对应的姿态角估计值

3、姿态控制子系统控制卫星姿态

本发明姿态控制子系统6包括姿态控制器和执行机构。设期望姿态角为其中

θ_p、ψ_p分别为期望横滚角、俯仰角和偏航角，则姿态角误差为：

姿态控制器采用PID线性控制律，则控制器输出力矩指令为：

$G\left(s\right)=\mathrm{diag}\left(\right[K_{\mathrm{px}}+s{K}_{\mathrm{dx}}+\frac{K_{\mathrm{ix}}}{s},K_{\mathrm{py}}+s{K}_{\mathrm{dy}}+\frac{K_{\mathrm{iy}}}{s},K_{\mathrm{pz}}+s{K}_{\mathrm{dz}}+\frac{K_{\mathrm{iz}}}{s}\left]\right)---\left(45\right)$

式中，G(s)为姿态控制器的传递函数；diag(·)表示对角阵；K_px，K_py，K_pz为比例系数；K_dx，K_dy，K_dz为微分系数；K_ix，K_iy，K_iz为积分系数；s为拉普拉斯算子。

采用飞轮作为执行机构，且飞轮的安装方式为三个反作用轮沿星体主轴正交安装。则加到卫星三轴上的力矩为：

T_cr＝C·F(s)·D·T_c    (46)

式中，C是飞轮的安装矩阵，为三阶单位阵；F(s)是飞轮传递函数；D是力矩指令分配矩阵，为三阶单位阵。T_c是控制器输出力矩指令。

卫星动力学模型为：

$I{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b+{\mathrm{\ω}}_{\mathrm{ib}}^b\×(I{\mathrm{\ω}}_{\mathrm{ib}}^b+\mathrm{J\Ω})=T_{\mathrm{cr}}+T_n---\left(47\right)$

式中，I为卫星转动惯量矩阵；J，Ω分别为飞轮系统的转动惯量阵和角速度矢量；T_n为干扰力矩；

为卫星本体坐标系相对于惯性坐标系的姿态角速度。

用欧拉角描述卫星姿态时，卫星姿态运动学模型为：

式中，ω_o为卫星轨道坐标系相对于惯性坐标系的角速率，

θ和ψ分别为横滚角、俯仰角和偏航角。

由(47)式和(48)式给出的卫星动力学模型和运动学模型可计算出卫星被控后的姿态信息，并将姿态信息输出到姿态敏感器子系统4，从而形成姿态控制仿真回路。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (2)

1.一种基于多速率敏感器组合定姿的姿控反馈回路，其特征在于：由姿态敏感器子系统(4)、基于多尺度建模的姿态确定子系统(5)和姿态控制子系统(6)组成，其中：

①姿态敏感器子系统(4)包括陀螺(1)、太阳敏感器(2)和磁强计(3)，陀螺(1)、太阳敏感器(2)和磁强计(3)分别产生卫星本体坐标系相对于惯性坐标系的姿态角速度、本体坐标系下的太阳矢量和地磁矢量；陀螺、太阳敏感器和磁强计输出测量信息的频率不同，其中陀螺和太阳敏感器的输出频率是磁强计输出频率的两倍，即姿态敏感器子系统(4)输出的测量信息具有多速率特性；

②基于多尺度建模的姿态确定子系统(5)用于估计卫星姿态角，先利用多尺度分析方法建立卫星姿态确定子系统多尺度模型，然后在多尺度建模的基础上利用UKF滤波算法，将姿态敏感器子系统(4)输出的具有多速率特性的测量信息进行融合，实现对当前卫星姿态角的估计，最后将卫星姿态角输出给姿态控制子系统(6)；

③姿态控制子系统(6)根据期望姿态角与基于多尺度建模的姿态确定子系统(5)估计的姿态角之间的偏差，通过姿态控制器和执行机构对卫星姿态进行控制，再经过卫星动力学模型和运动学模型输出卫星控制后的姿态信息，最后将姿态信息输出到姿态敏感器子系统(4)，从而形成姿控反馈回路。

2.根据权利要求1所述的一种基于多速率敏感器组合定姿的姿控反馈回路，其特征在于：所述基于多尺度建模的姿态确定子系统(5)利用多尺度分析方法建立卫星姿态确定子系统多尺度模型的过程如下：

a.多尺度状态空间投影以四元数和陀螺漂移为状态变量，x＝[q₀ q₁ q₂ q₃ b_x by b_z]^T，其中q₀和分别为四元数的标量部分和矢量部分，b＝[b_x b_y b_z]^T为陀螺XYZ三个轴上的漂移，状态变量的维数N_x＝7；由于陀螺和太阳敏感器的输出频率是磁强计输出频率的两倍，则太阳敏感器对应于细尺度，即尺度1，磁强计对应于粗尺度，即尺度2；设ΔT为磁强计的输出周期，ΔT时间间隔内两个尺度上的状态称为一个状态块，ΔT时间间隔内两个尺度上敏感器的量测量称为一个数据块；用n表示第n个时间块，即第nΔT时间间隔；x₁(2n)和x₁(2n+1)表示第nΔT时间间隔内尺度1上两个状态节点的状态，x₂(n)表示第nΔT时间间隔内尺度2上的状态；姿态确定子系统的多尺度状态结构满足二叉树结构，基于Haar小波，尺度2上的状态x₂(n)与尺度1上的状态x₁(2n)和x₁(2n+1)的状态空间投影关系为：

$x_2\left(n\right)=(\sqrt{2}/2)(x_1\left(2n\right)+x_1(2n+1))---\left(1\right);$

b.建立多尺度状态方程

基于Haar小波的姿态确定子系统多尺度状态方程为：

$\stackrel{\‾}{x}(n+1)=\left[\begin{array}{c}F(n+\mathrm{1,0})\\ F(n+\mathrm{1,1})\end{array}\right]---\left(2\right)$

其中：

$\stackrel{\‾}{x}(n+1)=\left[\begin{array}{c}{x}_1(2n+2)\\ x_1(2n+3)\end{array}\right]---\left(3\right)$

F(n+1，0)＝f(x₁(2n+1))+w(2n+1)    (4)

F(n+1，1)＝f[f(x₁(2n+1))+w(2n+1)]+w(2n+2)    (5)

式中，x₁(2n+2)和x₁(2n+3)表示第(n+1)ΔT时间间隔内尺度1上两个状态节点的状态；x₁(2n+1)表示第nΔT时间间隔内尺度1上状态节点的状态；f(·)为尺度1上状态方程的非线性项；w(·)为系统噪声；

c.建立多尺度测量方程

对于尺度1上的太阳敏感器，离散的测量方程是：

Sun_b(2n+m₁)＝g₁(x₁(2n+m₁))+v_s(2n+m₁)    (6)

其中：

g₁(x₁(2n+m₁))＝A(q(2n+m₁))·Sun_o(2n+m₁)    (7)

式中，m₁＝0，1；Sun_b(2n+m₁)和Sun_o(2n+m₁)分别为2n+m₁时刻本体坐标系和轨道坐标系下的太阳矢量；A(q(2n+m₁))为2n+m₁时刻本体坐标系相对于轨道坐标系的姿态矩阵；v_s(2n+m₁)为2n+m₁时刻太阳敏感器的量测噪声，其噪声方差阵为R₁；

对于尺度2上的磁强计，离散的测量方程是：

Mag_b(n)＝g₂(x₂(n))+v_m(n)    (8)

其中：

g₂(x₂(n))＝A(q(n))·Mag_o(n)    (9)

式中，Mag_b(n)和Mag_o(n)分别为第n个时间块本体坐标系和轨道坐标系下的地磁矢量，维数分别为3；A(q(n))为第n个时间块本体坐标系相对于轨道坐标系的姿态矩阵；v_m(n)为第n个时间块磁强计的量测噪声，其噪声方差阵为R₂；

根据太阳敏感器的测量方程(6)、磁强计的测量方程(8)以及尺度1与尺度2上的状态空间投影关系式(1)，多尺度观测方程为：

$\stackrel{\‾}{z}\left(n\right)=\stackrel{\‾}{G}\left(\stackrel{\‾}{x}\left(n\right)\right)+\stackrel{\‾}{v}\left(n\right)---\left(10\right)$

其中，式(10)中的

如下

$\stackrel{\‾}{z}\left(n\right)=\left[\begin{array}{c}\stackrel{\‾}{M}\mathrm{ag}\_b\left(n\right)\\ \stackrel{\‾}{S}\mathrm{un}\_b\left(n\right)\end{array}\right]---\left(11\right)$

式中，

表示第nΔT时间间隔内尺度2上的磁强计的测量信息；设太阳敏感器在第nΔT时间间隔内尺度1上的状态x₁(2n)和x₁(2n+1)对应的测量信息分别为Sun_b(2n)和Sun_b(2n+1)，则 $\stackrel{\‾}{S}\mathrm{un}\_b\left(n\right)=\left[\begin{array}{c}\mathrm{Sun}\_b\left(2n\right)\\ \mathrm{Sun}\_b(2n+1)\end{array}\right];$

为9×1维矢量；

式(10)中的

如下

$\stackrel{\‾}{G}\left(\stackrel{\‾}{x}n\right)=\left[\begin{array}{c}{\stackrel{\‾}{G}}_2\left(\stackrel{\‾}{x}\left(n\right)\right)\\ {\stackrel{\‾}{G}}_1\left(\stackrel{\‾}{x}\left(n\right)\right)\end{array}\right]---\left(12\right)$

${\stackrel{\‾}{G}}_1\left(\stackrel{\‾}{x}\left(n\right)\right)=\left[\begin{array}{c}{g}_1\left(M_1\left(0\right)\stackrel{\‾}{x}\left(n\right)\right)\\ g_1\left(M_1\left(1\right)\stackrel{\‾}{x}\left(n\right)\right)\end{array}\right]---\left(13\right)$

${\stackrel{\‾}{G}}_2\left(\stackrel{\‾}{x}\left(n\right)\right)=g_2\left(M_2\stackrel{\‾}{x}\left(n\right)\right)---\left(14\right)$

$M_1\left(0\right)=\left[\begin{array}{cc}{I}_{N_x\×N_x}& O_{N_x\×N_x}\end{array}\right],$ $M_1\left(1\right)=\left[\begin{array}{cc}{O}_{N_x\×N_x}& I_{N_x\×N_x}\end{array}\right]---\left(15\right)$

$M_2=\left[\begin{array}{cc}(\sqrt{2}/2)I_{N_x\×N_x}& (\sqrt{2}/2)I_{N_x\×N_x}\end{array}\right]---\left(16\right)$

式中，M₁(0)和M₁(1)分别表示第nΔT时间间隔内尺度1上的状态节点x₁(2n)和x₁(2n+1)与之间的变换矩阵；M₂表示第nΔT时间间隔内尺度2上的状态节点x₂(n)与

之间的变换矩阵；

表示N_x维的单位阵；

表示N_x维的零矩阵；

式(10)中的

表示多尺度观测方程的观测噪声，具体为：

$\stackrel{\‾}{v}\left(n\right)=\left[\begin{array}{c}{\stackrel{\‾}{v}}_m\left(n\right)\\ {\stackrel{\‾}{v}}_s\left(n\right)\end{array}\right]---\left(17\right)$

式中， ${\stackrel{\‾}{v}}_s\left(n\right)=\left[\begin{array}{c}{v}_s\left(2n\right)\\ v_s(2n+1)\end{array}\right],$ 其中v_s(2n)和v_s(2n+1)分别表示第nΔT时间间隔内尺度1上两个状态节点x₁(2n)和x₁(2n+1)的量测噪声；

表示第nΔT时间间隔内尺度2上状态节点x₂(n)的量测噪声；

将多尺度状态方程(2)和多尺度观测方程(10)写在一起，得到姿态确定子系统的多尺度状态模型：

$\left\{\begin{array}{c}\stackrel{\‾}{x}(n+1)=\left[\begin{array}{c}F(n+\mathrm{1,0})\\ F(n+\mathrm{1,1})\end{array}\right]\\ \stackrel{\‾}{z}\left(n\right)=\stackrel{\‾}{G}\left(\stackrel{\‾}{x}\left(n\right)\right)+\stackrel{\‾}{v}\left(n\right)\end{array}\right.---\left(18\right).$

Images