利用角速率估计信息的单套陀螺故障诊断方法

摘要

利用角速率估计信息的单套陀螺故障诊断方法，包括以下步骤：S1、设定卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量；S2、根据卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量建立状态方程和测量方程；S3、将所述状态方程和测量方程离散化；S4、根据离散化的状态方程和测量方程对测量的卫星的姿态角进行滤波，获得当前拍的姿态角和角速度的估计值；S5、测量卫星的惯性角速度，根据当前拍的角速度的估计值和预定阈值，来判断陀螺是否故障。

说明书

技术领域

本发明涉及一种卫星姿态控制单套速率陀螺的故障诊断方法。

背景技术

当卫星只有一套速率陀螺开机运行时，仅能测得一组卫星的三轴角速率，设为ω_m＝[ω_mx(k)ω_my(k)ω_mz(k)]^T。由于没有冗余的角速率测量信息，很难对陀螺的故障进行有效诊断。然而，卫星一般配有多种角度测量敏感器，如星敏感器、太阳敏感器、地平仪等，通过多种角度敏感器的信息融合，可以较为可靠的获得卫星在参考坐标系的角度信息，设为θ_m、ψ_m。利用角度信息对卫星角速率进行较为准确的估计，得到卫星在参考坐标系的角速率，设为

因此需要将单套陀螺测得的角速率信号和利用角度敏感器求得的角速率估计值进行比较，当诊断出陀螺故障后，可进行控制模式切换(如无陀螺信息的控制模式)，以免卫星姿态失稳。

发明内容

本发明的技术解决问题是：

解决现有技术的缺陷，提出一种利用角速率估计信息的单套陀螺故障诊断方法，可将单套陀螺测得的角速率信号和利用角度敏感器求得的角速率估计值进行比较，以实现速率陀螺的故障诊断。

本发明的技术解决方案是：

利用角速率估计信息的单套陀螺故障诊断方法，包括以下步骤：

S1、设定卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量；

S2、根据卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量建立状态方程和测量方程；

S3、将所述状态方程和测量方程离散化；

S4、根据离散化的状态方程和测量方程对测量的卫星的姿态角进行滤波，获得当前拍的姿态角和角速度的估计值；

S5、测量卫星的惯性角速度，根据当前拍的角速度的估计值和预定阈值，来判断陀螺是否故障。

根据本发明的一个实施例，在步骤S1中，卫星的姿态动力学模型为：卫星的运动学模型为：卫星的状态变量为：

其中，I为卫星的惯量矩阵，为卫星的角速度，为卫星的角速度的一阶导数，ω_bix为卫星的滚动角速度，ω_biy为卫星的俯仰角速度，ω_biz为卫星的偏航角速度，h为卫星的角动量，为卫星的角动量的一阶导数，ω₀为卫星轨道角速度，为卫星的滚动角，θ为卫星的俯仰角，ψ为卫星的偏航角，为卫星的滚动角的一阶导数，为卫星的俯仰角的一阶导数，为卫星的偏航角的一阶导数，T_d为卫星的干扰力矩。

根据本发明的一个实施例，在步骤S2中，状态方程为：

测量方程为Z＝HX+DV，其中，为卫星的滚动角速度的一阶导数，为卫星的俯仰角速度的一阶导数，为卫星的偏航角速度的一阶导数，U为控制量输入，E为单位矩阵，W是输入噪声，H＝D＝[0_3×3E_3×3]，V为量测噪声，

根据本发明的一个实施例，在步骤S3中，离散化的状态方程为：

X(k)＝Φ_k,k-1X(k-1)+G_k,k-1u(k-1)+Γ_k,k-1W_k-1，离散化的测量方程为：

Z(k)＝H(k)X(k)+D(k)V(k)，其中，

X(k)为当前拍的状态变量，X(k-1)为上一拍的状态变量，Z(k)为当前拍的卫星的姿态角，W_k-1为上一拍的输入噪声，ΔT为采样周期，U_x为滚动轴控制量输入，U_y为俯仰轴控制量输入，U_z为偏航轴控制量输入，为上一拍滚动角速度估计值，为上一拍俯仰角速度估计值，为上一拍偏航角速度估计值，为上一拍偏航角估计值，为上一拍滚动角估计值，为上一拍偏航角估计值，H(k)＝D(k)＝[0_3×3>3×3]，V(k)为当前拍的量测噪声。

根据本发明的一个实施例，计算U_x、U_y和U_z包括以下步骤：

(1)根据等式计算当前拍的卫星的角动量的一阶导数其中，h(k)为当前拍的卫星的角动量，h_x(k)为当前拍滚动轴的角动量，h_x(k-1)为上一拍拍滚动轴的角动量，h_y(k)为当前拍俯仰轴的角动量，h_y(k-1)为上一拍拍俯仰轴的角动量，h_z(k)为当前拍偏航轴的角动量，h_z(k-1)为上一拍偏航轴的角动量；

(2)根据等式

计算当前拍卫星星体总角动量H_total(k)，其中，ω(k-1)为上一拍的卫星角速度，I_xx为滚动轴的惯量，I_xy为滚动轴和俯仰轴的惯量积，I_xz为滚动轴和偏航轴的惯量积，I_yy为俯仰轴的惯量，I_yz为俯仰轴和偏航轴的惯量积，I_zz为偏航轴的惯量，H_{x_total}(k)为当前拍滚动轴总角动量，H_{y_total}(k)为当前拍俯仰轴总角动量，H_{z_total}(k)为当前拍偏航轴总角动量；

(3)根据等式计算总力矩M_total，其中，T_d(k)为当前拍的卫星的干扰力矩，M根据等式获得；

(4)根据等式获得U_x、U_y和U_z。

根据本发明的一个实施例，在步骤S4中，当前拍的滚动角测量值为当前拍的俯仰角测量值为θ_m，当前拍的偏航角测量值为ψ_m，根据滤波方程获得当前拍的滚动角速度估计值当前拍的俯仰角速度估计值和当前拍的偏航角速度估计值K₁、K₂和K₃为滤波增益。

根据本发明的一个实施例，在步骤S4中，当前拍的滚动角估计值当前拍的俯仰角估计值当前拍的偏航角估计值根据等式

获得。

根据本发明的一个实施例，在步骤S5中，滚动角速度测量值为ω_mx(k)，俯仰角速度测量值为ω_my(k)，偏航角速度测量值为ω_mz(k)，当

时，陀螺有故障，否则，陀螺无故障，其中，δ为预定误差阈值。

根据本发明的一个实施例，预定误差阈值δ为陀螺三轴噪声与对应三轴角速度估计误差的平方和的1.5倍。

一种存储了指令的计算机可读存储介质，当所述指令被至少一个计算机执行时，使得所述计算机执行以下步骤：

S1、设定卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量；

S2、根据卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量建立状态方程和测量方程；

S3、将所述状态方程和测量方程离散化；

S4、根据离散化的状态方程和测量方程对测量的卫星的姿态角进行滤波，获得当前拍的姿态角和角速度的估计值；

S5、测量卫星的惯性角速度，根据当前拍的角速度的估计值和预定阈值，来判断陀螺是否故障。

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

(1)提供了冗余的角速率测量信息，对陀螺的故障进行有效诊断

(2)利用卫星上现有的多种角度测量敏感器，如星敏感器、太阳敏感器、地平仪，通过多种角度敏感器可以较为可靠的获得卫星在参考坐标系的角度信息，从而提高测量精度。

(3)利用角度信息对卫星角速率进行较为准确的估计，可提供对陀螺进行故障诊断的参考信号，以实现速率陀螺的故障诊断。

附图说明

图1是根据本发明的实施例的利用角速率估计信息的单套陀螺故障诊断方法流程图。

具体实施方式

下面根据附图详细描述本发明的实施例。

如图1所示，利用角速率估计信息的单套陀螺故障诊断方法包括以下步骤：

S1、设定卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量。

在步骤S1中，卫星的姿态动力学模型为如以下等式(1)所示：

卫星的运动学模型为如以下等式(2)所示：

卫星的状态变量为：其中，I为卫星的惯量矩阵，为卫星的角速度，为卫星的角速度的一阶导数，ω_bix为卫星的滚动角速度，ω_biy为卫星的俯仰角速度，ω_biz为卫星的偏航角速度，h为卫星的角动量，为卫星的角动量的一阶导数，ω₀为卫星轨道角速度，为卫星的滚动角，θ为卫星的俯仰角，ψ为卫星的偏航角，为卫星的滚动角的一阶导数，为卫星的俯仰角的一阶导数，为卫星的偏航角的一阶导数，T_d为卫星的干扰力矩。

S2、根据卫星的姿态动力学模型、卫星的运动学模型和卫星的状态变量建立状态方程和测量方程。

在步骤S2中，状态方程为如以下等式(3)所示：

测量方程如以下等式(4)所示：

Z＝HX+DV (4)

其中，为卫星的滚动角速度的一阶导数，为卫星的俯仰角速度的一阶导数，为卫星的偏航角速度的一阶导数，U为控制量输入，W是输入噪声，H＝D＝[0_3×3>3×3]，V为量测噪声，

S3、将所述状态方程和测量方程离散化。

在步骤S3中，离散化的状态方程如以下等式(5)所示：

X(k)＝Φ_k,k-1X(k-1)+G_k,k-1u(k-1)+Γ_k,k-1W_k-1>

其中，X(k)为当前拍的状态变量，X(k-1)为上一拍的状态变量，W_k-1为上一拍的输入噪声，ΔT为采样周期，U_x为滚动轴控制量输入，U_y为俯仰轴控制量输入，U_z为偏航轴控制量输入，为上一拍滚动角速度估计值，为上一拍俯仰角速度估计值，为上一拍偏航角速度估计值，为上一拍偏航角估计值，为上一拍滚动角估计值，为上一拍偏航角估计值。

在本发明的实施例中，h(k)为当前拍的卫星的角动量，为当前拍的卫星的角动量的一阶导数，根据求导法则，由以下等式(6)来确定：

其中，h_x(k)为当前拍滚动轴的角动量，h_x(k-1)为上一拍拍滚动轴的角动量，h_y(k)为当前拍俯仰轴的角动量，h_y(k-1)为上一拍拍俯仰轴的角动量，h_z(k)为当前拍偏航轴的角动量，h_z(k-1)为上一拍偏航轴的角动量。

当前拍卫星星体总角动量H_total(k)如以下等式(7)所示：

其中，ω(k-1)为上一拍的卫星角速度，I_xx为滚动轴的惯量，I_xy为滚动轴和俯仰轴的惯量积，I_xz为滚动轴和偏航轴的惯量积，I_yy为俯仰轴的惯量，I_yz为俯仰轴和偏航轴的惯量积，I_zz为偏航轴的惯量，H_{x_total}(k)为当前拍滚动轴总角动量，H_{y_total}(k)为当前拍俯仰轴总角动量，H_{z_total}(k)为当前拍偏航轴总角动量。

总力矩M_total如以下等式(8)所示：

其中，T_d(k)为当前拍的卫星的干扰力矩，M如以下等式(9)所示：

如以下等式(10)所示：

因此，根据等式(10)可求得G_k,k-1u(k-1)，并因此求得X(k)。

在本发明的实施例中，离散化的测量方程如以下等式(11)所示：

Z(k)＝H(k)X(k)+D(k)V(k) (11)

其中，H(k)＝D(k)＝[0_3×3>3×3]，V(k)为当前拍的量测噪声。

S4、根据离散化的状态方程和测量方程对测量的卫星的姿态角进行滤波，获得当前拍的姿态角和角速度的估计值。

在步骤S4中，当前拍的滚动角测量值为当前拍的俯仰角测量值为θ_m，当前拍的偏航角测量值为ψ_m。

根据滤波方程(12)可获得当前拍的滚动角速度估计值当前拍的俯仰角速度估计值和当前拍的偏航角速度估计值

其中，K₁、K₂和K₃为滤波增益。

在本发明的实施例中，可根据方程(13)获得当前拍的滚动角估计值当前拍的俯仰角估计值当前拍的偏航角估计值

其中，K₄、K₅和K₆为滤波增益。

S5、测量卫星的惯性角速度，根据当前拍的角速度的估计值和预定阈值，来判断陀螺是否故障。

在步骤S5中，陀螺测得的滚动角速度测量值为ω_mx(k)，俯仰角速度测量值为ω_my(k)，偏航角速度测量值为ω_mz(k)，若下式(14)成立

则陀螺有故障，否则，陀螺无故障，其中，预定误差阈值δ为陀螺三轴噪声与对应三轴角速度估计误差的平方和的1.5倍。

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