一种利用二阶卡尔曼滤波算法估计卫星姿态控制系统执行机构加性故障大小的方法

摘要

一种利用二阶卡尔曼滤波算法估计卫星姿态控制系统执行机构加性故障大小的方法，它涉及利用二阶卡尔曼滤波算法估计卫星姿态控制系统执行机构加性故障大小的方法，本发明是要解决现有的卫星姿态控制系统中采用卡尔曼滤波算法建模不能真实地反映飞轮产生的故障大小的问题。本发明方法通过如下步骤来实现：一、根据动力学方程和运动学方程建立离散的控制系统模型；二、对离散的控制系统模型中的噪声向量wk和vk在实际运行过程中进行标定；三、建立含有执行机构加性故障的离散控制系统数学模型；四、测出执行机构输出力矩中的白噪声向量五、利用二阶卡尔曼滤波算法估计执行机构加性故障的大小。本发明可用于航天器姿态控制领域。

说明书

技术领域

本专利涉及航天器姿态控制系统技术领域，尤其涉及利用二阶卡尔曼滤波算法估计卫星 姿态控制系统执行机构加性故障大小的方法。

背景技术

卫星姿态控制系统中的执行机构飞轮作为一个长时间机械转动的部件，在卫星姿态控制 系统中是比较容易产生故障的部件。而且飞轮在长期运转后，飞轮中会产生较大的摩擦力矩， 这个摩擦力矩能够使飞轮输出力矩与期望输出力矩产生一个常值偏差，这个偏差能够对卫星 姿态控制系统的控制精度产生影响。执行机构飞轮中的晶体管失效或者遗漏指令故障引起的 失效，也能够使飞轮输出力矩与期望输出力矩产生偏差。如果能够利用算法检测到这个偏差， 便可以采取一些措施来修正这个误差，防止造成更严重的后果，并且提高控制系统的可靠性 与精度。

二阶卡尔曼滤波算法与卡尔曼滤波算法相比较，能够避免由于状态维数较高时所产生的 数值病态问题和克服增广状态卡尔曼滤波计算量大的缺点，在实际应用中拥有诸多优点。 M.Hadi Amoozgar利用二阶卡尔曼滤波算法估计了四旋翼无人机执行机构发生乘性故障时的 故障诊断问题。由于卫星姿态控制系统稳定的时候，系统参数和执行机构的输出力矩接近于 零，乘性的故障建模方式并不能真实的反映飞轮产生的故障大小，而加性的故障建模方式具 有具体的物理意义，能够反映飞轮的真实故障。

发明内容

本发明是要解决现有卫星姿态控制系统中采用卡尔曼滤波算法建模不能真实地反映飞轮 产生的故障大小的问题，而提出一种利用二阶卡尔曼滤波算法估计卫星姿态控制系统执行机 构加性故障大小的方法。

一种利用二阶卡尔曼滤波算法估计卫星姿态控制系统执行机构加性故障大小的方法按以 下步骤进行：

一、根据卫星姿态动力学方程和运动学方程建立离散的卫星姿态控制系统模型；

二、对离散的卫星姿态控制系统模型中的噪声向量w_k和v_k在实际运行过程中进行标定；

三、建立含有执行机构加性故障的离散卫星姿态控制系统数学模型；

四、对执行机构的输出力矩进行分析，测出执行机构输出力矩中的白噪声向量

五、将上述模型转化为二阶卡尔曼滤波算法处理模型的标准形式，利用二阶卡尔曼滤波 算法估计执行机构加性故障的大小。

本发明包括以下有益效果：

1、卫星姿态控制系统中的执行机构飞轮作为一个长时间机械转动的部件，在卫星长期运 行时很容易产生摩擦力矩，这是一个加性故障，利用该方法可以准确的估计飞轮加性故障大 小；

2、当卫星姿态稳定时，卫星转动角速度接近于零，飞轮输出大小也接近于零，干扰力矩 大小与飞轮输出力矩大小较为接近时，乘性故障建模方法估计结果很不准确，而加性故障建 模方法能够克服这个缺点，也就是在稳定情况下能够准确的估计出飞轮加性故障的大小；

3、当执行机构飞轮输出力矩精度比干扰力矩大小更小时，还可以利用本发明方法来估计 干扰力矩的大小；

本发明的具体实验效果请参见具体实施方式后的仿真实验部分。

附图说明

图1为本发明方法的流程图；图2为仿真实验中执行机构故障时执行机构的加性故障图； 图3为仿真实验中执行机构故障时估计出来的故障图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合图1和具体实施方式 对本发明作进一步详细的说明。

具体实施方式一：一种利用二阶卡尔曼滤波算法估计卫星姿态控制系统执行机构加性故 障大小的方法

按以下步骤进行：

一、根据卫星姿态动力学方程和运动学方程建立离散的卫星姿态控制系统模型；

二、对离散的卫星姿态控制系统模型中的噪声向量w_k和v_k在实际运行过程中进行标定；

三、建立含有执行机构加性故障的离散卫星姿态控制系统数学模型；

四、对执行机构的输出力矩进行分析，测出执行机构输出力矩中的白噪声向量

五、将上述模型转化为二阶卡尔曼滤波算法处理模型的标准形式，利用二阶卡尔曼滤波 算法估计执行机构加性故障的大小。

本发明包括以下有益效果：

1、卫星姿态控制系统中的执行机构飞轮作为一个长时间机械转动的部件，在卫星长期运 行时很容易产生摩擦力矩，这是一个加性故障，利用该方法可以准确的估计飞轮加性故障大 小；

2、当卫星姿态稳定时，卫星转动角速度接近于零，飞轮输出大小也接近于零，干扰力矩 大小与飞轮输出力矩大小较为接近时，乘性故障建模方法估计结果很不准确，而加性故障建 模方法能够克服这个缺点，也就是在稳定情况下能够准确的估计出飞轮加性故障的大小；

3、当执行机构飞轮输出力矩精度比干扰力矩大小更小时，还可以利用本发明方法来估计 干扰力矩的大小；

本发明的具体实验效果请参见具体实施方式后的仿真实验部分。

具体实施方式二：本实施方式是对具体实施方式一的进一步说明，步骤一的具体过程为：

一.一、根据卫星姿态动力学方程和运动学方程建立相对于轨道坐标系的卫星姿态控制系 统状态方程为：

$\left(\begin{array}{c}\stackrel{·}{x}=\mathrm{Ax}+\mathrm{Bu}\\ y=\mathrm{Cx}\end{array}\right);$

其中x是卫星姿态控制系统状态方程的状态向量，是卫星姿态控制系统状态方程的状 态向量的导数，u是控制输入力矩向量，y是输出向量；A，B和C分别为系数矩阵、输入矩 阵和输出矩阵；

一.二、对上述状态方程进行离散化，建立离散的卫星姿态控制系统状态空间方程为：

$\left(\begin{array}{c}{x}_{k+1}={\mathrm{\Phi x}}_k+\Theta u_k\\ y_{k+1}={\mathrm{Cx}}_k\end{array};\right)$

其中Φ、Θ和C分别为离散控制系统的系统矩阵、输入矩阵和输出矩阵，x_k与y_k是k 时刻卫星姿态控制系统的状态向量和输出向量，u_k是k时刻控制输入力矩向量，x_k+1与y_k+1是 k+1时刻卫星姿态控制系统的状态向量和输出向量。

具体实施方式三：本实施方式是对具体实施方式一的进一步说明，步骤二的具体过程为：

二.一、在离散的控制系统状态方程中引入零均值的白噪声向量w_k和v_k，并且满足： E{w_k}＝0，E{v_k}＝0，E{v_kv_j}＝Rδ_kj，其中δ_kj是克罗内克符 号，E为求期望的运算符号，w_k为k时刻系统状态向量的白噪声，v_k为k时刻测量向量的白 噪声，w_j为j时刻系统状态向量的白噪声，v_j为j时刻测量向量的白噪声，Q_x为系统状态向 量的白噪声协方差矩阵，R为测量向量的白噪声协方差矩阵；

二.二、通过实际运行完成对w_k和v_k这两个白噪声的标定，根据公式和 E{v_kv_j}＝Rδ_kj从而确定Q_x与R的值；

二.三、将标定的白噪声向量w_k和v_k加入到步骤一.二中得到的离散的卫星姿态控制系统 状态空间方程后，得到方程：

$\left(\begin{array}{c}{x}_{k+1}={\mathrm{\Phi x}}_k+{\mathrm{\Theta u}}_k+w_k\\ y_{k+1}={\mathrm{Cx}}_k+v_k\end{array}.\right)$

具体实施方式四：本实施方式是对具体实施方式一的进一步说明，步骤三中的含有加性 执行机构故障的卫星姿态控制系统状态空间方程为：

$\left(\begin{array}{c}{x}_{k+1}={\mathrm{\Phi x}}_k+\Theta u_k+\Theta f_k+w_k\\ y_{k+1}={\mathrm{Cx}}_k+v_k\end{array}\right);$

其中f_k是k时刻执行机构飞轮的加性故障向量。

具体实施方式五：本实施方式是对具体实施方式一的进一步说明，步骤四的具体过程为：

四.一、对执行机构的输出力矩进行分析，测出卫星飞轮输出力矩白噪声向量过程 为控制飞轮输出一个恒定大小的力矩，测得实际输出力矩大小，然后用实际输出力矩减去恒 定值就测得卫星飞轮输出力矩白噪声向量按时序重复进行，就可以得到卫星飞轮输出力 矩中含有零均值白噪声向量序列；

四.二、求出Q_f：根据公式求出Q_f，其中Q_f为卫星飞轮输出力矩白 噪声协方差矩阵。

具体实施方式六：本实施方式是对具体实施方式一的进一步说明，步骤五的具体过程为：

五.一、二阶卡尔曼滤波模型通过步骤三所得状态方程引申而来：

$\left(\begin{array}{c}{x}_{k+1}={\mathrm{\Phi x}}_k+{\mathrm{\Phi u}}_k+\Theta f_k+w_k\\ f_{k+1}=f_k+w_k^f\\ y_{k+1}={\mathrm{Cx}}_k+v_k\end{array}\right);$且应满足如下条件：初始值x₀，f₀为高 斯随机变量，满足$E\left\{x_0\right\}=\bar{x},$$E\left\{(x_0-{\bar{x}}_0){(x_0-{\bar{x}}_0)}^T\right\}=P_0^x>0,$$E\left\{f_0\right\}={\bar{f}}_0,$$E\left\{(f_0-{\bar{f}}_0){(f_0-{\bar{f}}_0)}^T\right\}=P_0^f>0,$$E\left\{(x_0-{\bar{x}}_0){(x_0-{\bar{x}}_0)}^T\right\}=P_0^x>0,$$E\left\{f_0\right\}={\bar{f}}_0,$其中分别是初始时刻系统状态向量误差和执行机构 飞轮输出力矩误差的协方差矩阵，为初始时刻系统状态向量误差与执行机构飞轮输出力 矩误差的协方差；

五.二、二阶卡尔曼滤波算法为：

最优偏差估计器是：

${\hat{f}}_{k+1|k}={\hat{f}}_{k|k}$

$P_{k+1|k}^f=P_{k|k}^f+Q_f$

${\hat{f}}_{k+1|k+1}={\hat{f}}_{k+1|k}+K_{k+1}^f(y_{k+1}-C{\tilde{x}}_{k+1|k}-H_{k+1|k}{\hat{f}}_{k|k});$

$K_{k+1}^f=P_{k+1|k}^f{H}_{k+1|k}^T{(H_{k+1|k}{P}_{k+1|k}^f{H}_{k+1|k}^T+C{\tilde{P}}_{k+1|k}^x{C}^T+R)}^{-1}$

$P_{k+1|k+1}^f=(I-K_{k+1}^f{H}_{k+1|k})P_{k+1|k}^f$

零偏差状态估计器是：

${\tilde{x}}_{k+1|k}=\Phi {\tilde{x}}_{k|k}+\Theta u_k+W_k{\hat{f}}_{k|k}-V_{k+1|k}{\hat{f}}_{k|k}$

${\tilde{P}}_{k+1|k}^x=\Phi {\tilde{P}}_{k|k}^x{\Phi }^T+Q_x+W_k{P}_{k|k}^f{W}_k^T-V_{k+1|k}{P}_{k+1|k}^f{V}_{k+1|k}{V_{k+1|k}}^T$

${\tilde{x}}_{k+1|k+1}={\tilde{x}}_{k+1|k}+{\tilde{K}}_{k+1}^x(y_{k+1}-C{\tilde{x}}_{k+1|k});$

${\tilde{K}}_{k+1}^x={\tilde{P}}_{k+1|k}^x{C}^T{(C_k{\tilde{P}}_{k+1|k}^x{C}^T+R)}^{-1}$

${\tilde{P}}_{k+1|k+1}^x=(I-{\tilde{K}}_{k+1}^{x}C){\tilde{P}}_{k+1|k}^x$

执行机构故障参数f与状态变量之间的耦合关系是：

W_k＝ΦV_k|k-Θ

$V_{k+1|k}=W_k{P}_{k|k}^f{\left(P_{k+1|k}^f\right)}^{-1}$

H_k+1|k＝CV_k+1|k；

H_k+1|k+1＝CV_k+1|k+1

$V_{k+1|k+1}=V_{k+1|k}-{\tilde{K}}_{k+1}^x{H}_{k+1|k}$

最优状态估计为：

$\left(\begin{array}{c}{\hat{x}}_{k+1|k+1}={\tilde{x}}_{k+1|k+1}+V_{k+1|k+1}{\hat{f}}_{k+1|k+1}\\ P_{k+1|k+1}^x={\tilde{P}}_{k+1|k+1}^x+V_{k+1|k+1}{P}_{k+1|k+1}^f{V}_{k+1|k+1}^T\end{array}\right);$

式中下标k|k表示第k步的数据，下标k+1|k+1表示第k+1步的数据，k+1|k表示利用 第k步数据计算第k+1步的中间结果；表示第k步故障估计结果；I是单位矩阵；H、V 和K为二阶卡尔曼滤波算法的中间变量符号；上式中的和代入初值f₀，x₀和之后，便可以迭代递推计算后计算出的便是执行机构加性故障f的大小。

仿真实验过程：

仿真实验所基于的技术方案为：

一种利用二阶卡尔曼滤波算法估计卫星姿态控制系统执行机构加性故障大小的方法，其 特征在于它是通过以下步骤实现的：

一、根据卫星姿态动力学方程和运动学方程建立离散的卫星姿态控制系统模型；

二、对离散的卫星姿态控制系统模型中的噪声向量w_k和v_k在实际运行过程中进行标定；

三、建立含有执行机构加性故障的离散卫星姿态控制系统数学模型；

四、对执行机构的输出力矩进行分析，测出执行机构输出力矩中的白噪声向量

五、将上述模型转化为二阶卡尔曼滤波算法处理模型的标准形式，利用二阶卡尔曼滤波 算法估计执行机构加性故障的大小。

步骤一的具体过程为：

一.一、根据卫星姿态动力学方程和运动学方程建立相对于轨道坐标系的卫星姿态控制系 统状态方程为：

$\left(\begin{array}{c}\stackrel{·}{x}=\mathrm{Ax}+\mathrm{Bu}\\ y=\mathrm{Cx}\end{array}\right);$

其中x是卫星姿态控制系统状态方程的状态向量，是卫星姿态控制系统状态方程的状 态向量的导数，u是控制输入力矩向量，y是输出向量；A，B和C分别为系数矩阵、输入矩 阵和输出矩阵；

一.二、对上述状态方程进行离散化，建立离散的卫星姿态控制系统状态空间方程为：

$\left(\begin{array}{c}{x}_{k+1}={\mathrm{\Phi x}}_k+\Theta u_k\\ y_{k+1}={\mathrm{Cx}}_k\end{array};\right)$

其中Φ、Θ和C分别为离散控制系统的系统矩阵、输入矩阵和输出矩阵，x_k与y_k是k 时刻卫星姿态控制系统的状态向量和输出向量，u_k是k时刻控制输入力矩向量，x_k+1与y_k+1是 k+1时刻卫星姿态控制系统的状态向量和输出向量。

步骤二的具体过程为：

二.一、在离散的控制系统状态方程中引入零均值的白噪声向量w_k和v_k，并且满足： E{w_k}＝0，E{v_k}＝0，E{v_kv_j}＝Rδ_kj，其中δ_kj是克罗内克符 号，E为求期望的运算符号，w_k为k时刻系统状态向量的白噪声，v_k为k时刻测量向量的白 噪声，w_j为j时刻系统状态向量的白噪声，v_j为j时刻测量向量的白噪声，Q_x为系统状态向 量的白噪声协方差矩阵，R为测量向量的白噪声协方差矩阵；

二.二、通过实际运行完成对w_k和v_k这两个白噪声的标定，根据公式和 E{v_kv_j}＝Rδ_kj从而确定Q_x与R的值；

二.三、将标定的白噪声向量w_k和v_k加入到步骤一.二中得到的离散的卫星姿态控制系统 状态空间方程后，得到方程：

$\left(\begin{array}{c}{x}_{k+1}={\mathrm{\Phi x}}_k+{\mathrm{\Theta u}}_k+w_k\\ y_{k+1}={\mathrm{Cx}}_k+v_k\end{array}.\right)$

步骤三中的含有加性执行机构故障的卫星姿态控制系统状态空间方程为：

$\left(\begin{array}{c}{x}_{k+1}={\mathrm{\Phi x}}_k+\Theta u_k+\Theta f_k+w_k\\ y_{k+1}={\mathrm{Cx}}_k+v_k\end{array}\right);$

其中f_k是k时刻执行机构飞轮的加性故障向量。

步骤四的具体过程为：

四.一、对执行机构的输出力矩进行分析，测出卫星飞轮输出力矩白噪声向量过程 为控制飞轮输出一个恒定大小的力矩，测得实际输出力矩大小，然后用实际输出力矩减去恒 定值就测得卫星飞轮输出力矩白噪声向量按时序重复进行，就可以得到卫星飞轮输出力 矩中含有零均值白噪声向量序列；

四.二、求出Q_f：根据公式求出Q_f，其中Q_f为卫星飞轮输出力矩白 噪声协方差矩阵。

步骤五的具体过程为：

五.一、二阶卡尔曼滤波模型通过步骤三所得状态方程引申而来：

$\left(\begin{array}{c}{x}_{k+1}={\mathrm{\Phi x}}_k+{\mathrm{\Phi u}}_k+\Theta f_k+w_k\\ f_{k+1}=f_k+w_k^f\\ y_{k+1}={\mathrm{Cx}}_k+v_k\end{array}\right);$且应满足如下条件：初始值x₀，f₀为高 斯随机变量，满足$E\left\{x_0\right\}=\bar{x},$$E\left\{(x_0-{\bar{x}}_0){(x_0-{\bar{x}}_0)}^T\right\}=P_0^x>0,$$E\left\{f_0\right\}={\bar{f}}_0,$$E\left\{(f_0-{\bar{f}}_0){(f_0-{\bar{f}}_0)}^T\right\}=P_0^f>0,$$E\left\{(x_0-{\bar{x}}_0){(x_0-{\bar{x}}_0)}^T\right\}=P_0^x>0,$$E\left\{f_0\right\}={\bar{f}}_0,$其中分别是初始时刻系统状态向量误差和执行机构 飞轮输出力矩误差的协方差矩阵，为初始时刻系统状态向量误差与执行机构飞轮输出力 矩误差的协方差；

五.二、二阶卡尔曼滤波算法为：

最优偏差估计器是：

${\hat{f}}_{k+1|k}={\hat{f}}_{k|k}$

$P_{k+1|k}^f=P_{k|k}^f+Q_f$

${\hat{f}}_{k+1|k+1}={\hat{f}}_{k+1|k}+K_{k+1}^f(y_{k+1}-C{\tilde{x}}_{k+1|k}-H_{k+1|k}{\hat{f}}_{k|k});$

$K_{k+1}^f=P_{k+1|k}^f{H}_{k+1|k}^T{(H_{k+1|k}{P}_{k+1|k}^f{H}_{k+1|k}^T+C{\tilde{P}}_{k+1|k}^x{C}^T+R)}^{-1}$

$P_{k+1|k+1}^f=(I-K_{k+1}^f{H}_{k+1|k})P_{k+1|k}^f$

零偏差状态估计器是：

${\tilde{x}}_{k+1|k}=\Phi {\tilde{x}}_{k|k}+\Theta u_k+W_k{\hat{f}}_{k|k}-V_{k+1|k}{\hat{f}}_{k|k}$

${\tilde{P}}_{k+1|k}^x=\Phi {\tilde{P}}_{k|k}^x{\Phi }^T+Q_x+W_k{P}_{k|k}^f{W}_k^T-V_{k+1|k}{P}_{k+1|k}^f{V_{k+1|k}}^T$

${\tilde{x}}_{k+1|k+1}={\tilde{x}}_{k+1|k}+{\tilde{K}}_{k+1}^x(y_{k+1}-C{\tilde{x}}_{k+1|k})$

${\tilde{K}}_{k+1}^x={\tilde{P}}_{k+1|k}^x{C}^T{(C_k{\tilde{P}}_{k+1|k}^x{C}^T+R)}^{-1}$

${\tilde{P}}_{k+1|k+1}^x=(I-{\tilde{K}}_{k+1}^{x}C){\tilde{P}}_{k+1|k}^x$

执行机构故障参数f与状态变量之间的耦合关系是：

W_k＝ΦV_k|k-Θ

$V_{k+1|k}=W_k{P}_{k|k}^f{\left(P_{k+1|k}^f\right)}^{-1}$

H_k+1|k＝CV_k+1|k

H_k+1|k+1＝CV_k+1|k+1

$V_{k+1|k+1}=V_{k+1|k}-{\tilde{K}}_{k+1}^x{H}_{k+1|k}$

最优状态估计为：

${\hat{x}}_{k+1|k+1}={\tilde{x}}_{k+1|k+1}+V_{k+1|k+1}{\hat{f}}_{k+1|k+1}$

$P_{k+1|k+1}^x={\tilde{P}}_{k+1|k+1}^x+V_{k+1|k+1}{P}_{k+1|k+1}^f{V}_{k+1|k+1}^T$

式中下标k|k表示第k步的数据，下标k+1|k+1表示第k+1步的数据，k+1|k表示利用 第k步数据计算第k+1步的中间结果；表示第k步故障估计结果；I是单位矩阵；H、V 和K为二阶卡尔曼滤波算法的中间变量符号；上式中的和代入初值f₀，x₀和之后，便可以迭代递推计算后计算出的便是执行机构加性故障f的大小。

仿真实验中卫星的执行机构的故障具体表现为执行机构输入命令与实际输出之间的差 别；仿真采用的飞轮为最为普遍的反作用飞轮，加性故障体现为摩擦力矩的突然增大、电子 元器件失效等导致的飞轮输出力矩的增大或者减小。

仿真中使用的反作用飞轮的故障输出见图2所示。由图2可知仿真中共设置了两个故障， 第一次的故障出现在x轴，大小为5×10^-3N·m，时间是从55s到250s；第二次的故障出现在y 轴，大小为3×10^-3N·m，时间是从300s到450s。图3是仿真中利用二阶卡尔曼滤波算法估 计的故障的大小。对比图2与图3可知，本发明方法可以准确的估计出来执行机构故障的大 小，并且能够区分出发生故障的轴向。