[发明专利]基于容积四元数估计的航天器姿态估计方法

权利要求书

1.基于容积四元数估计的航天器姿态估计方法，所述航天器姿态估计方法的具体过程为：

步骤一：建立航天器姿态运动学模型和观测模型；

步骤二：采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型中的噪声；

步骤三：采用容积四元数姿态估计器对航天器姿态进行估计；

其特征在于：所述步骤一中建立航天器姿态运动学模型和观测模型的具体过程为：

带有乘性噪声的非线性离散系统：

x_k+1＝(I_n×n+ζ_kΦ_k)f(x_k)+w_k (1)

y_k＝h(x_k)+v_k (2)

其中，x_k∈Rⁿ是系统k时刻的状态量，y_k∈R^m是系统k时刻的观测值；f(x_k)和h(x_k)是非线性方程；Φ_k是已知的常系数矩阵，ζ_k∈R是乘性噪声，w_k∈Rⁿ和v_k∈R^m是系统加性噪声和量测加性噪声；R为实数，Rⁿ为n维实数集，R^m为m维实数集；

ζ_k和v_k是同步相关的，且满足

其中S_k≠0是乘性噪声和加性噪声的互协方差；Q_ζ,k是ζ_k的方差，R_k是v_k的方差；t＝k时ζ_k和v_k存在相关性；

采用四元数描述的航天姿态运动学模型，f(x_k)具体形式如下：

其中，q为姿态四元数，是q的导数；ω＝[ω₁ ω₂ ω₃]^T为角速度在体坐标系下的表示，[ω×]表示由ω生成的反对称矩阵表示为：

观测模型的建立：

(一)陀螺输出模型

假设陀螺固连在航天器上，并且陀螺的安装方向与航天器本体坐标系重合，测量航天器的角速度；则陀螺模型表示为：

其中，表示实际的陀螺输出值，β表示陀螺漂移值，ω表示理想情况下的陀螺输出值，η_v和η_u分别表示不相关的零均值高斯白噪声，且其协方差分别表示为和将陀螺输出和陀螺漂移方程离散化后的形式如下：

其中，Δt表示步长，N_v和N_u分别表示离散后不相关的零均值高斯白噪声，ω_k和β_k分别为公式(32)中ω和β在k时刻的值；

(二)星敏感器输出模型

假设星敏感器的安装方向与航天器本体坐标系重合，则星光矢量在航天器本体坐标系下的观测方程为：

其中，r表示星光矢量在惯性系下的单位矢量方向，表示从惯性系到航天器本体坐标系的变换矩阵；v表示敏感器的观测误差，假设有m个星敏感器同时进行观测，则在第k时刻，用四元数描述的矢量观测模型为：

其中，b_m和r_m表示第m个参考矢量分别在航天器本体坐标系和惯性坐标系下的分量；A(q)表示姿态转移矩阵，其四元数形式的描述为：

其中，姿态四元数q＝[q₁,q₂,q₃,q₄]^T，q分解为标量q₄和矢量ρ，ρ＝[q₁,q₂,q₃]^T；

展开形式是：