[发明专利]一种星敏感器与高频角位移传感器组合定姿方法及系统

权利要求书

1.一种星敏感器与高频角位移传感器组合定姿方法，其特征在于，包括以下步骤：

步骤1，构建角位移传感器测量模型如下，

ωg＝(1+Δ+Λ)ω+b+ηg

其中，ωg为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误差，Λ表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，表示卫星本体坐标系相对于惯性坐标系的三轴旋转角速度，b表示角位移传感器漂移大小，ηg为角位移传感器测量噪声；

步骤2，基于多星敏感器信息融合结果构建系统量测方程，实现如下，

设有一组正交矢量，表示正交矢量在本体坐标系下真实值，表示正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；

得到量测方程如下，

Zk＝HkXk+Vk

H k = - 2 A b i T ( q ^ ) [ l b 1 × ] 0 3 × 3 - 2 A b i T ( q ^ ) [ l b 2 × ] 0 3 × 3 - 2 A b i T ( q ^ ) [ l b 3 × ] 0 3 × 3 9 × 9 , Z k = l m i 1 - A b i T ( q ^ ) l b 1 l m i 2 - A b i T ( q ^ ) l b 2 l m i 3 - A b i T ( q ^ ) l b 3 9 × 1 ]]>

其中，Zk表示观测矢量，Hk表示观测矩阵，Xk表示状态变量，表示四元数最优估计值，表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列；

步骤3，根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方程，实现如下，

根据姿态运动学方程得到基于状态的系统状态方程如下，

X · ( t ) = F ( t ) X ( t ) + W ( t ) ]]>

F ( t ) = - [ ω ^ × ] - 0.5 I 3 × 3 0 3 × 3 0 3 × 3 ]]>

W(t)＝[-0.5ηg ηb]T

其中，表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W(t)表示系统噪声序列，表示角速度矢量叉乘，I3×3表示单位矩阵，03×3表示元素为0的矩阵，表示误差四元数矢量部分，ΔbT表示误差漂移，ηg为角位移传感器测量噪声，ηb为角位移传感器与时间相关的噪声；

对以上系统状态方程进行线性化离散化得到，

Xk＝Φk,k-1Xk-1+Γk-1Wk-1

其中，Xk、Xk-1表示tk时刻、tk-1时刻状态变量，Φk,k-1表示状态转移矩阵，Γk-1表示系统噪声驱动阵，Wk-1表示系统噪声序列；

步骤4，基于步骤2和步骤3分别构建的系统量测方程和系统状态方程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。