[发明专利]基于无迹卡尔曼滤波的联合定姿方法、卫星姿态控制系统

权利要求书

1.一种基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述基于无迹卡尔曼滤波的联合定姿方法采用误差四元数和陀螺常值漂移的非线性微分状态方程的UKF的星敏感器和陀螺联合定姿算法，构建微分形式非线性状态方程，并采用四阶龙格-库塔积分法求解状态变量的时间更新；利用四元数乘性误差计算加权均值与协方差，采用无迹卡尔曼滤波算法，引入星敏感器的观测值进行滤波更新，最后利用估计误差四元数对姿态数据进行修正；

所述基于无迹卡尔曼滤波的联合定姿方法的系统状态方程和量测方程构建：

(1)陀螺的量测模型表示为式：

w_g＝w+b+d+n_g；

卫星三轴稳定，其中w_g、w分别为陀螺输出的本体坐标系相对于惯性坐标系的三轴角速度测量值和理论真实值；b为陀螺仪的常值漂移噪声，满足η_b为白噪声；d为陀螺的随机漂移噪声，满足D_τ与时间常数τ有关，η_d为白噪声；η_g为陀螺的测量噪声；

(2)利用星敏感器误差四元数以及陀螺的常值漂移误差定义系统状态变量：

其中为标量，为矢量，且满足

(3)利用姿态运动学方程和陀螺的量测模型建立非线性微分系统状态方程；基于四元数的姿态运动学方程式：

其中，

实际四元数估计四元数误差四元数满足：

联合建立非线性微分系统状态方程：

其中系统噪声服从N(0，Q)，为估计角速度，满足

(4)定义星敏感器的量测残差为星敏感器的估计值与测量值之差：

建立非线性离散量测方程：

其中z_k是误差四元数k时刻的量测残差，H＝[I_4×4 0_4×3]为观测矩阵，量测噪声v_k服从N(0，R)。

2.如权利要求1所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述基于无迹卡尔曼滤波的联合定姿方法具体包括：

输入：星敏感器量测值、陀螺的量测值；

输出：估计姿态四元数；

1)设置初始状态变量、初始状态协方差矩阵、系统噪声协方差矩阵、量测噪声协方差矩阵；

2)k+1时刻的姿态预测；

若无星敏感器测量值，则k+1时刻的姿态估计值等于k+1时刻的姿态预测值，转2)；若有星敏感器测量值，转3)进入UKF滤波；

3)对状态变量进行增广，并生成sigma点集；

4)进行时间更新，计算状态变量的统计特性；

5)量测更新，计算得到k+1时刻的误差四元数和陀螺常值漂移估计值；

6)姿态与角速度修正，利用下式对姿态四元数与陀螺角速度进行修正；之后将状态变量置零，转2)；

3.如权利要求2所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述方法中步骤2)具体包括：

首先计算k时刻的角度增量

其次利用将Δw_k转换成四元数增量

最后利用预测k+1时刻的姿态

4.如权利要求2所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述方法中步骤3)具体包括：

利用式对状态变量进行增广，并根据式生成sigma点集。

5.如权利要求2所述的基于无迹卡尔曼滤波的联合定姿方法，其特征在于，所述方法中步骤4)中具体包括：

步骤4a)时间更新

利用式进行时间更新；

步骤4b)计算陀螺常值漂移的统计特性

利用式计算陀螺常值漂移的统计特性；

步骤4c)计算星敏感器误差四元数的统计特性

构造四阶矩阵解算M的最大特征值对应的特征向量，该特征向量即为误差四元数均值

利用式计算误差四元数的协方差矩阵。