[发明专利]一种基于滚动时域估计的航天器角速度的估计方法

权利要求书

1.一种基于滚动时域估计的航天器角速度的估计方法，其特征在于，包括以下步骤：

S1：基于特殊正交群SO(3)描述方式下的航天器动力学模型，构建航天器角速度的状态方程及观测方程；

所述特殊正交群SO(3)描述方式下的航天器动力学模型为：

R_k+1＝R_kF_k (2)

其中，h为采样时间间隔；上标T表示矩阵转置；下标k表示第k个采样时刻；矩阵R_k,F_k∈SO(3)是旋转矩阵，表示航天器欧拉姿态角描述的旋转矩阵，表示两个采样时刻之间的航天器欧拉姿态误差角描述的旋转矩阵，F_k为隐式方程(1)的解；表示航天器角动量，为航天器的惯性矩阵，为第k个采样时刻的航天器角速度；表示航天器所受到的由包括重力梯度力矩、气动阻力、太阳光压的外部干扰引起的外部干扰力矩；表示航天器姿态机动或稳定所需的控制力矩；正定阵表示非标准的惯性矩阵，它与惯性矩阵J有关；是关于实向量的斜反对称矩阵，对于任意一个向量其对应的斜反对称阵x^×形式如下：

基于航天器动力学模型构建用于滚动时域估计的状态方程和观测方程如下：

取系统状态量为航天器角速度ω＝(ω_x,ω_y,ω_z)^T，则状态方程为

取系统观测量为冗余光纤陀螺仪组合数据处理后的角速度测量数据输出值y＝(y_x,y_y,y_z)^T，则观测方程为

Jy_k＝CJω_k+ν_k (5)

其中，y_x,y_y,y_z为航天器各惯性主轴的角速率测量值；和分别为系统外部干扰和测量噪声；为测量矩阵；

S2：利用权值基于故障失效因子进行分配的新的加权数据融合方法，对冗余光纤陀螺仪组合的四路十二维角速率测量信息进行简化处理，得到航天器三轴角速率测量数据；

所述权值基于故障失效因子进行分配的新的加权数据融合方法为：

其中，为冗余光纤陀螺仪中第i个光纤陀螺仪所敏感到的航天器角速度信息，为四路十二维角速率测量信息；α_i,i＝1,2,3,4为光纤陀螺仪的安装偏转角，表示第i个光纤陀螺仪的轴向在xoy平面上的投影与ox轴的夹角；β_i,i＝1,2,3,4为光纤陀螺仪的安装高低角，表示第i个光纤陀螺仪的轴向与xoy平面的夹角；为加权因子，受第i个光纤陀螺仪的故障失效因子ρ_i∈[0,1]影响，且需满足约束τ₁+τ₂+τ₃+τ₄＝1；

S3：基于步骤S1中构建的状态方程和观测方程及步骤S2中得到的航天器三轴角速率测量数据，同时考虑航天器角速度约束和控制力矩约束，利用滚动时域估计策略及多重打靶法进行在线估计，得到当前时刻的航天器最优角速度的估计值；

所述利用滚动时域估计策略及多重打靶法进行在线估计的具体步骤为：

1)假设已知系统的初始状态的先验估计初始状态误差的协方差阵为P₀，系统外部干扰和测量噪声的权值矩阵分别为Q₁,Q₂，且P₀,Q₁,Q₂均为正定矩阵，滚动时域长度为N；

2)在T时刻，由滚动时域估计定义的优化问题如下：

其中，两种情况下目标函数中的第一项均表示每个采样时刻系统外部干扰的代价和，第二项均表示每个采样时刻测量噪声的代价和，第三项在T≤N时表示对先验估计的信任程度，在T>N时，为到达代价函数，表示其余测量数据对估计的影响，采用无约束线性系统的到达代价函数来近似有约束的非线性系统的代价函数，即，近似到达代价函数的形式如下：

式中，起始状态估计服从正态分布P_T-N为估计误差协方差，其可以由下式(8)进行更新；

同时，目标函数要满足航天器动力学、航天器角速度时域约束和控制力矩约束，具体的数学表达形式如下：

角速度时域约束：

其中，为滚动时域估计得到的角速率估计值，分别为航天器各主轴所允许的最小和最大角速率，其数值由安装在航天器上的包括太阳帆板、敏感器的物理器件的性质所决定；

控制力矩约束：

又因为航天器系统为离散系统形式，故角加速度可用如下形式来近似表示：

则控制力矩约束可近似写成：

化简上式，可将控制力矩约束转化为角速度约束的形式，具体形式如下：

其中，分别为航天器执行机构在各主轴方向上所能提供的最小和最大控制力矩；为前一时刻航天器角速率的最优估计值；T_s为离散系统采样时间间隔；

结合角速度约束式(9)和控制力矩约束式(13)，可将两个约束条件简化为：

此时，带约束的滚动时域优化问题可写成如下形式：

3)在T时刻，若数据处理后的冗余光纤陀螺仪组合角速度测量输出序列或和航天器姿态控制力矩序列或已知，其中对于任意变量序列表示则可利用多重打靶法来求解优化问题(15)，解得唯一存在的最优解

4)在T时刻，利用多重打靶法求解优化问题(15)的具体步骤如下：

首先，这里用航天器角动量Π_k＝Jω_k来替换航天器角速度ω_k，故优化问题(15)可以等效变形为如下优化问题(16)的形式：

其中，Y_k＝Jy_k为k时刻由航天器角速度测量值y_k计算得到的航天器角动量；3为k时刻航天器各主轴角动量估计值；分别为航天器各主轴最小和最大角动量；式(16c)为求解辅助变量的隐式方程，

考虑系统动力学等式约束(16b)和角动量时域不等式约束(16d)，定义増广目标函数：

其中，为拉格朗日乘子，μ＞0且为罚参数，为对应不等式约束(16d)的罚函数，其具体表达形式如下：

当T＞N时，利用变分法对増广代价函数进行变分，其变分表达式如下：

其中，为罚函数对的一阶变分和，变分系数如下所示：

其中，e_i,i＝1,2,3分别为航天器各惯量主轴的单位向量；

令δΦ_Ta＝0可得优化问题(16)的一阶必要条件如下：

其中，

对式(21)进行变形可得如下改进型一阶必要条件：

其中，为一阶必要条件的待求向量，由拉格朗日乘子序列和航天器角动量估计序列组成；

此时，求解优化问题(16)可以等效转化为求解一阶必要条件(22)的根，即一阶必要条件的根与优化问题的最优解同解，采用多重打靶法对式(22)进行数值迭代求解，即要找到一组特定的序列s^*使得F_s(s^*)＝0_6N+3成立，若一阶必要条件式(22)有根且唯一，那么该根即为优化问题(16)的最优解；

多重打靶法中的求根迭代关系式如下：

s_i+1＝s_i+κΔ(s)_i (23)

其中，s_i+1为第i+1个迭代解，s_i为第i个迭代解，κ∈(0,1]为迭代步长，为迭代下降方向，Δ(s)_i中的为式(22)的雅克比矩阵；

根据式(22)的变分表达式：

其中，各式对应的系数矩阵如下：

故根据式(24)-式(28)可得式(22)的雅克比矩阵如下：

其中，

算法流程中的迭代终止条件为：

||F_s(s)||≤σ (30)

其中，σ为所能接受的迭代精度误差；

至此，由上述步骤可解得一阶必要条件(22)的根再通过关系式λ_k＝Q₁w_k可得优化问题(16)的最优解再进而通过关系式Π_k＝Jω_k可得优化问题(15)的最优解

当T≤N时，也即当考虑全信息滚动时域估计时，只需令上述算法步骤中所有的T-N＝0即可求解一阶必要条件(22)的根进而通过关系式λ_k＝Q₁w_k可得优化问题(16)的最优解再进而通过关系式Π_k＝Jω_k可得优化问题(15)的最优解

5)在T时刻，可将步骤4)的最优解或和姿态控制力矩序列或代入离散航天器姿态动力学和运动学方程迭代递推得到当前时刻航天器角速度的最优估计值

6)根据式(8)计算下一时刻估计误差协方差P_T-N+1；

7)在下一个T+1时刻，用最新测量值和最新控制力矩更新数据序列，并定义T:＝T+1，返回步骤2。