本发明涉及一种卫星姿态控制单套速率陀螺的故障诊断方法。
背景技术：
当卫星只有一套速率陀螺开机运行时，仅能测得一组卫星的三轴角速率，设为ωm＝[ωmx(k)ωmy(k)ωmz(k)]t。由于没有冗余的角速率测量信息，很难对陀螺的故障进行有效诊断。然而，卫星一般配有多种角度测量敏感器，如星敏感器、太阳敏感器、地平仪等，通过多种角度敏感器的信息融合，可以较为可靠的获得卫星在参考坐标系的角度信息，设为
因此需要将单套陀螺测得的角速率信号和利用角度敏感器求得的角速率估计值进行比较，当诊断出陀螺故障后，可进行控制模式切换(如无陀螺信息的控制模式)，以免卫星姿态失稳。
技术实现要素：
本发明的技术解决问题是：
解决现有技术的缺陷，提出一种利用角速率估计信息的单套陀螺故障诊断方法，可将单套陀螺测得的角速率信号和利用角度敏感器求得的角速率估计值进行比较，以实现速率陀螺的故障诊断。
本发明的技术解决方案是：
利用角速率估计信息的单套陀螺故障诊断方法，包括以下步骤：
s1、设定卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量；
s2、根据卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量建立状态方程和测量方程；
s3、将所述状态方程和测量方程离散化；
s4、根据离散化的状态方程和测量方程对测量的卫星的姿态角进行滤波，获得当前拍的姿态角和角速度的估计值；
s5、测量卫星的惯性角速度，根据当前拍的角速度的估计值和预定阈值，来判断陀螺是否故障。
根据本发明的一个实施例，在步骤s1中，卫星的姿态动力学模型为：
根据本发明的一个实施例，在步骤s2中，状态方程为：
根据本发明的一个实施例，在步骤s3中，离散化的状态方程为：
x(k)＝φk,k-1x(k-1)+gk,k-1u(k-1)+γk,k-1wk-1，离散化的测量方程为：
z(k)＝h(k)x(k)+d(k)v(k)，其中，
根据本发明的一个实施例，计算ux、uy和uz包括以下步骤：
(1)根据等式
(2)根据等式
(3)根据等式
(4)根据等式
根据本发明的一个实施例，在步骤s4中，当前拍的滚动角测量值为
根据本发明的一个实施例，在步骤s4中，当前拍的滚动角估计值
根据本发明的一个实施例，在步骤s5中，滚动角速度测量值为ωmx(k)，俯仰角速度测量值为ωmy(k)，偏航角速度测量值为ωmz(k)，当
根据本发明的一个实施例，预定误差阈值δ为陀螺三轴噪声与对应三轴角速度估计误差的平方和的1.5倍。
一种存储了指令的计算机可读存储介质，当所述指令被至少一个计算机执行时，使得所述计算机执行以下步骤：
s1、设定卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量；
s2、根据卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量建立状态方程和测量方程；
s3、将所述状态方程和测量方程离散化；
s4、根据离散化的状态方程和测量方程对测量的卫星的姿态角进行滤波，获得当前拍的姿态角和角速度的估计值；
s5、测量卫星的惯性角速度，根据当前拍的角速度的估计值和预定阈值，来判断陀螺是否故障。
本发明与现有技术相比的优点在于：
(1)提供了冗余的角速率测量信息，对陀螺的故障进行有效诊断
(2)利用卫星上现有的多种角度测量敏感器，如星敏感器、太阳敏感器、地平仪，通过多种角度敏感器可以较为可靠的获得卫星在参考坐标系的角度信息，从而提高测量精度。
(3)利用角度信息对卫星角速率进行较为准确的估计，可提供对陀螺进行故障诊断的参考信号，以实现速率陀螺的故障诊断。
附图说明
图1是根据本发明的实施例的利用角速率估计信息的单套陀螺故障诊断方法流程图。
具体实施方式
下面根据附图详细描述本发明的实施例。
如图1所示，利用角速率估计信息的单套陀螺故障诊断方法包括以下步骤：
s1、设定卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量。
在步骤s1中，卫星的姿态动力学模型为如以下等式(1)所示：
卫星的运动学模型为如以下等式(2)所示：
卫星的状态变量为：
s2、根据卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量建立状态方程和测量方程。
在步骤s2中，状态方程为如以下等式(3)所示：
测量方程如以下等式(4)所示：
z＝hx+dv(4)
其中，
s3、将所述状态方程和测量方程离散化。
在步骤s3中，离散化的状态方程如以下等式(5)所示：
x(k)＝φk,k-1x(k-1)+gk,k-1u(k-1)+γk,k-1wk-1(5)
其中，
在本发明的实施例中，h(k)为当前拍的卫星的角动量，
其中，hx(k)为当前拍滚动轴的角动量，hx(k-1)为上一拍拍滚动轴的角动量，hy(k)为当前拍俯仰轴的角动量，hy(k-1)为上一拍拍俯仰轴的角动量，hz(k)为当前拍偏航轴的角动量，hz(k-1)为上一拍偏航轴的角动量。
当前拍卫星星体总角动量htotal(k)如以下等式(7)所示：
其中，ω(k-1)为上一拍的卫星角速度，ixx为滚动轴的惯量，ixy为滚动轴和俯仰轴的惯量积，ixz为滚动轴和偏航轴的惯量积，iyy为俯仰轴的惯量，iyz为俯仰轴和偏航轴的惯量积，izz为偏航轴的惯量，hx_total(k)为当前拍滚动轴总角动量，hy_total(k)为当前拍俯仰轴总角动量，hz_total(k)为当前拍偏航轴总角动量。
总力矩m_total如以下等式(8)所示：
其中，td(k)为当前拍的卫星的干扰力矩，m如以下等式(9)所示：
因此，根据等式(10)可求得gk,k-1u(k-1)，并因此求得x(k)。
在本发明的实施例中，离散化的测量方程如以下等式(11)所示：
z(k)＝h(k)x(k)+d(k)v(k)(11)
其中，h(k)＝d(k)＝[03×3i3×3]，v(k)为当前拍的量测噪声。
s4、根据离散化的状态方程和测量方程对测量的卫星的姿态角进行滤波，获得当前拍的姿态角和角速度的估计值。
在步骤s4中，当前拍的滚动角测量值为
根据滤波方程(12)可获得当前拍的滚动角速度估计值
其中，k1、k2和k3为滤波增益。
在本发明的实施例中，可根据方程(13)获得当前拍的滚动角估计值
其中，k4、k5和k6为滤波增益。
s5、测量卫星的惯性角速度，根据当前拍的角速度的估计值和预定阈值，来判断陀螺是否故障。
在步骤s5中，陀螺测得的滚动角速度测量值为ωmx(k)，俯仰角速度测量值为ωmy(k)，偏航角速度测量值为ωmz(k)，若下式(14)成立
则陀螺有故障，否则，陀螺无故障，其中，预定误差阈值δ为陀螺三轴噪声与对应三轴角速度估计误差的平方和的1.5倍。
本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。