基于容积四元数估计的航天器姿态估计方法与流程

本发明涉及基于容积四元数估计以及星敏感器与陀螺组合的航天器姿态估计方法。

背景技术：

随着航天器和空间系统复杂性的不断加大，需要对航天器的姿态进行更加准确和鲁棒的估计。对每个航天器的功能需求正在增加，而航天器本身的尺寸却越来越小，且硬件之间是相互独立的，这就导致系统的计算能力受到很大限制。这样的系统常常被称是操作响应空间(ORS)。ORS系统的主要目标是为了航天器使用模块化的类型来建设加速任务概念推出的时间。这种模块化的系统，定制的接口和软件必须管理传送到中央处理器的所有数据。然而，潜在的危险情况是数据传播的时引入未知扰动，或部分数据在传输时丢失。

实际中，航天器系统姿态模型是一个非线性模型。在近些年中，利用一系列含有噪声的观测值对非线性系统的最优估计问题，吸引了众多学者们的广泛关注和研究。不仅在航天领域，在其他领域内对非线性系统的状态最优估计问题也取得了许多重大的成果。现有的大部分滤波理论是基于贝叶斯方法的，贝叶斯方法为状态估计问题提供了一个通用框架。贝叶斯方法是利用对在所有观测量作为条件下的系统状态的条件概率密度函数进行递归计算。在目前的应用过程中，通常将系统假设为一种特殊的情形式，即所有的概率密度函数为高斯概率密度函数,这种系统称为高斯非线性系统。针对高斯非线性系统，学者们提出了多种滤波方法，应用最为广泛的是扩展卡尔曼滤波算法(EKF)，EKF具有运算速度快，稳定性可证等优势，但是该算法存在着一定的缺陷。由于是对非线性系统通过泰勒展开进行线性逼近，所以只适用于弱非线性系统。另外，需要求解泰勒展开中的雅克比矩阵，但是有些系统的雅克比矩阵很难求解，有些甚至不存在雅克比矩阵，因此EKF应用存在一定的局限性。因此，高斯滤波框架作为一种估计高斯非线性系统的通用方法框架得到了广泛应用。高斯滤波框架通过高斯加权积分的方式代替对贝叶斯框架下概率密度函数求解。许多滤波算法都是建立在高斯滤波框架的基础上，利用不同的对高斯加权积分的近似求解方式实现的，例如高斯厄尔米特滤波器(GHQF)、无迹卡尔曼滤波算法(UKF)、差分滤波器(DDF)、容积卡尔曼滤波(CKF)。

由于航天器上的传感元件在数据传输过程中，存在着传输时滞、丢包和多种未知外来扰动的问题，其中存在乘性噪声就是一种常见的问题。例如系统参数的不确定性可由乘性噪声描述，量化作用和随机产生的传感器饱和问题可以在建模过程中由乘性噪声描述。在文献(F.Yang,Z.Wang,and Y.Hung.Robust Kalman filtering for discrete time-varying uncertain systems with multiplicative noises,IEEE Trans.on Automatic Control,47(7):1179-1183,2002)(J.Jimenez,T.Ozaki,Local linearization filters for non-linear continuous-discrete state space models with multiplicative noise,International Journal of Control,76(12):1159-1170,2003)(F.Carravetta,A.Germani,and N.Raimondi,Polynomial filtering of discrete-time stochastic linear systems with multiplicative state noise,IEEE Trans.on Automatic Control,42(8):1106-1126,1997)中，最小方差滤波器和多项式滤波器可以用来解决具有乘性噪声的线性系统。对于具有乘性噪声的非线性系统，文献(D.Spinello,D.Stilwell,Nonlinear Estimation With State-Dependent Gaussian Observation Noise,IEEE Trans.on Automatic Control,55(6):1358-1366,2010)中设计了使滤波误差的协方差最小的递归EKF算法。文献(X.Hu,Y.Hu,and B.Xu,Generalised Kalman filter tracking with multiplicative measurement noise in a wireless sensor network,IET Signal Processing,8(5):467-474,2014)提出了推广EKF(GEKF)和推广UKF(GUKF)算法，该算法不需要迭代非线性优化过程，利用条件高斯分布的性质去提高传统EKF和UKF的性能。但是所有算法都基于系统的乘性噪声和相应的加性噪声不具有相关性的前提下。

在实际的应用中，噪声相关也是一种常见的问题。例如，动态观测过程在相同的噪声环境中，由于受到的扰动影响相同，所以产生的误差也具有相关性。在近期的相关研究中，针对噪声相关的问题学者们提出了一些理论方法，其中通过解耦重新构建系统方程的方法，在解决该问题上应用的最多，文献(G.Chang,Marginal unscented kalman filter for cross-correlated process and observation noise at the same epoch,IET Radar Sonar Navigation,8(1):54-64,2014)(A.Hermosocarazo,J.Linarespérez,Unscented Filtering from Delayed Observations with Correlated Noises,Mathematical Problems in Engineering,2009(1024-123X):266-287,2009)就是利用解耦的方法。在文献(X.Wang,Y.Liang,Q.Pan,and F.Yang,A Gaussian approximation recursive filter for nonlinear systems with correlated noises,Automatica,48(48):2290–2297,2012)中，利用两步预测的方法去更新后验概率密度函数，成功避开了量测噪声和系统噪声耦合的问题，提出了一种新型的高斯滤波框架。但在上述文献中，都是考虑系统加性噪声和量测加性噪声的耦合情况，没有考虑乘性噪声和加性噪声之间的耦合情况。

技术实现要素：

本发明的目的是为了解决现有技术存在乘性噪声及噪声相关问题，导致航天器姿态估计精度低的缺点，而提的基于容积四元数估计的航天器姿态估计方法。

基于容积四元数估计的航天器姿态估计方法包括以下步骤：

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

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

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

本发明的有益效果为：

针对现有技术各种情况的考虑，本发明针对具有乘性噪声的非线性系统且系统中乘性噪声与加性噪声相关，提出一种改进的高斯滤波框架，作为对该类系统进行最优估计的统一框架。该框架将对噪声的相关性及条件概率密度函数进行充分利用，从而对系统状态的估计精度更高。

本发明针对航天器的姿态估计时，可能出现系统乘性噪声，及乘性噪声和观测噪声相关的问题，设计了一种基于高斯滤波算法的改进高斯滤波框架。同时采用CQE作为改进高斯滤波算法的特殊实现形式，提出了CQE-MCNS算法。该算法主要针对航天器的姿态估计问题，继承了CQE算法的优势避免四元数规范约束和奇异的发生，同时又能处理乘性噪声及噪声相关的问题。通过与CQE算法进行仿真实验对比，结果表明，本发明所提出的CQE-MCNS算法相比CQE算法，角度估计误差减小0.0002°左右，陀螺漂移估计误差减小0.002°/h左右，因此更适用于具有乘性噪声及噪声相关的航天器姿态估计问题。此外，本发明所提出的改进高斯滤波框架可以作为解决具有乘性噪声及噪声相关的系统状态估计问题的通用框架应用于其他领域。

附图说明

图1为CQE角度估计误差2-范数图；

图2为CQE滚动轴角度估计误差图；

图3为CQE俯仰轴角度估计误差图；

图4为CQE偏移轴角度估计误差图；

图5为CQE陀螺漂移量估计误差2-范数图；

图6为CQE x轴陀螺漂移量估计误差图；

图7为CQE y轴陀螺漂移量估计误差图；

图8为CQE z轴陀螺漂移量估计误差图；

图9为CQE-MCNS角度估计误差2-范数图；

图10为CQE-MCNS滚动轴角度估计误差图；

图11为CQE-MCNS俯仰轴角度估计误差图；

图12为CQE-MCNS偏移轴角度估计误差图；

图13为CQE-MCNS陀螺漂移量估计误差2-范数图；

图14为CQE-MCNS x轴陀螺漂移量估计误差图；

图15为CQE-MCNS y轴陀螺漂移量估计误差图；

图16为CQE-MCNS z轴陀螺漂移量估计误差图。

具体实施方式

具体实施方式一：基于容积四元数估计的航天器姿态估计方法的具体过程为：

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

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

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

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中建立航天器姿态运动学模型和观测模型的具体过程为：

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

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维实数集；

假设1：w_k和v_k都是白噪声，且满足

其中δ_t,k是克罗内克函数，即当t＝k时，值为1，其他情况值为0。

假设2：ζ_k和v_k是同步相关的，且满足

其中S_k≠0是乘性噪声和加性噪声的互协方差；Q_ζ,k是ζ_k的方差，R_k是v_k的方差；另外，由于是同步相关，所以只有在t＝k时ζ_k和v_k才存在相关性；

假设3：初始值x₀与噪声ζ_k、w_k和v_k都是不相关的，且E[x₀]＝0,

在上述的假设条件下传统的高斯滤波框架是无法实用的，例如EKF、UKF和CKF。所以本文的目的是设计一种改进的高斯滤波框架，可以有效地对上述系统进行状态最优估计。注意1:从式(4)中可以看出，由于相关性的存在，乘性噪声和量测噪声的统计特性已经发生变化。

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

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

观测模型(包括陀螺输出模型和星敏感器输出模型)的建立：

(一)陀螺输出模型

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

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

其中，△t表示步长，N_v和N_u分别表示离散后不相关的零均值高斯白噪声；

(二)星敏感器输出模型

星敏感器是通过观测天体的方向对照星历表来确定航天器的姿态，假设星敏感器的安装方向与航天器本体坐标系重合，则星光矢量在航天器本体坐标系下的观测方程为：

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

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

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

展开形式是：

q＝[q₁,q₂,q₃,q₄]^T，ρ＝[q₁,q₂,q₃]^T。

注：

惯性坐标系

原点位于地心O_I，O_IX_I轴指向平春分点方向(天球黄道和天球赤道在天球上交叉点为春分点和秋分点)，O_IZ_I轴平行于平均地球自转轴方向，O_IY_I轴与另两轴构成右手系。

航天器本体坐标系

航天器本体坐标系与航天器固连，其原点位于航天器质心O_b，三轴与航天器惯量主轴一致，固联于星体上。O_bX_b轴指向航天器飞行方向，称为滚动轴，O_bZ_b轴指向地心方向，称为偏航轴，O_bY_b轴与另两轴构成右手直角坐标系，称为俯仰轴。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤二中采用高斯滤波算法去除步骤一建立的航天器姿态运动学模型和观测模型中的噪声的具体过程为：

用一种改进的高斯滤波框架处理噪声相关的情况，本文中所设计的滤波算法采用一步更新后验概率密度。改进的高斯滤波框架与经典高斯滤波算法类似，也分为一步预测和量测更新两个步骤，分别通过定理1和定理2描述如下。

引理1：假设向量X和Y的联合高斯分布如下

则X在Y＝y情况下的条件概率满足如下高斯分布

步骤二一：一步预测；

考虑非线性系统形式为式(1)和(2)，假设后验概率密度函数P_k-1|k-1和状态估计值是已知的，则x_k的一步预测均值和方差形式如下：

其中

其中是x_k的一步预测值，I_n×n为n×n的单位矩阵，P_k|k-1为x_k的一步预测方差，为变量a的高斯概率密度，b为a的期望，c为a的方差；为y_k-1的方差；M_k-1表示ζ_k-1的条件均值，U_k-1表示ζ_k-1的条件方差，D_k-1表示测量值的集合；

证明：首先将表示k-1时刻的式(2)代入协方差的定义中可得式(11)，然后将式(1)代入一步关于状态量x_k的一步预测均值的定义中，可得：

由于乘性噪声ζ_k-1和D_k-1中的y_k-1观测值中的v_k-1相关，因此E[ζ_k-1|D_k-1]的值不为0。ζ_k-1和y_k-1在以k-1时刻的状态估计值作为条件的联合分布如下：

根据引理1和v_k-1与D_k-2相互独立可知式(9)成立，同理可得式(10)也成立。将式(9)代入式(13)中可以得到式(7)。最后将式(1)代入P_k|k-1定义中，可得到如下

将式(15)的等式右边展开可得式(16)：

由于w_k-1和ζ_k-1不相关，且与状态x_k-1中的w_k-2也不相关，因此，可得：

最后将式(9)、(10)、(13)和(17)代入式(16)，可得到式(8)。

步骤二二：量测更新；

考虑非线性系统形式为式(1)和(2)，假设状态量x_k的预测均值和方差已经得到，则它的后验均值和方差P_k|k的形式如下：

其中为y_k的一步预测值，K_k为滤波增益，为x_k和y_k的一步预测协方差，为y_k的一步预测方差。

证明：首先将式(2)代入y_k的预测均值定义中可得

由于E[v_k|D_k-1]＝0，可直接得到式(21)。然后将式(2)代入协方差的定义中可得：

将式(25)的右侧展开可得

由于式(22)可以直接获得。最后将式(2)代入协方差的定义中可得：

将式(27)的等式右边展开可得：

由于可知式(23)成立。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤三中采用容积四元数姿态估计器对航天器姿态进行估计的具体过程为：

本发明利用容积卡尔曼滤波器(CKF)作为GF框架的实现形式，与四元数相结合估计卫星姿态，提出新的算法为乘性相关噪声系统的容积四元数姿态估计器(cubature quaternion estimator for multiplicative correlated noises system,CQE-MCNS)。为了避免采用四元数描述姿态时存在的冗余而导致滤波过程中协方差阵出现奇异的状况，将状态向量选为x＝[δp^T β^T]^T，其中δp为与误差四元数对应的修正罗德里格斯参数，形式如下：

其中0≤a≤1，b为尺度参数；当a＝0,b＝1式(38)退化为罗德里格参数，a＝1,b＝1时式(38)退化为修正的罗德里格参数，广义罗德里格参数的奇异点在180^°到360^°间。

(一)时间预测

已知滤波器的初值q₀，β₀，P₀，由k-1时刻(前一时刻)的状态估计和协方差阵估计P_k-1获得k时刻(下一时刻)的容积点为：

其中，δp_i,k-1和β_i,k-1分别表示姿态角误差和陀螺漂移，ξ_i表示容积点集合{ξ_i}的第i列向量。容积点集合{ξ_i}可定义为即

e_i表示单位向量，且该单位向量中第i个元素为1。

在滤波算法更新过程中，为了避免误差广义罗德里格参数描述姿态发生的奇异现象，将误差广义罗德里格参数转换成容积误差四元数为：

其中，δρ_i,k-1表示第k-1时刻误差四元数δq_i,k-1的矢量部分，表示第k-1时刻误差四元数δq_i,k-1的标量部分。

由容积误差四元数获得一步预测估计的容积四元数点集为：

容积四元数由姿态运动学方程(29)获得，采用常用的解析形式为：

其中：

陀螺的角速度估计为：

其中，i表示取值为1,2,…,n的数，表示第k-1时刻陀螺输出值，β_i,k-1表示第k-1时刻陀螺漂移值。

获得容积四元数一步预测值后，再将其转换为容积误差罗德里格参数；先计算一步预测容积误差四元数得：

再将容积误差四元数转换为容积误差罗德里格参数为：

在由步骤二所设计的高斯滤波算法，得到一步预测均值

容积点误差一步预测均值为和方差阵P_k|k-1为：

其中，是容积点传播值。

(二)量测更新

计算容积点

星敏感器生成观测容积估计点Z_i,k为：

通过以上容积点得到量测容积点均值

求取容积滤波增益K_k：

其中，协方差阵和互协方差阵P_xz,k分别表示如下：

再由式(49)得到状态量的量测更新为：

其中，

计算更新容积误差四元数和陀螺漂移；容积误差四元数δq_k更新为：

得到容积点四元数

协方差阵P_k更新为：

其它步骤及参数与具体实施方式一至三之一相同。

实施例一：

在本发明中，通过对近地轨道航天器的姿态估计来验证所设计的CQE-MCNS算法的有效性。假设航天器的本体坐标系和轨道坐标系重合，这样卫星体系相对于惯性系角速度与轨道坐标系相对于惯性系角速度相等。同时，假设航天器上装配有星敏感器，用于测量角度矢量。角速度的测量利用三轴陀螺。星敏感器对姿态矢量测量带有标准差为2角秒的高斯白噪声。陀螺采样周期为1/s，星敏感器的采样周期为10/s，仿真时间设为3个轨道周期。假设三轴角度初值有较大估计误差，分别为-45°,45°,155°，陀螺漂移初始值为0，初始方差阵P₀＝[(40°)²I_3×3(0.1°/h)²I_3×3]^T。乘性噪声ζ_k-1的均值为0，方差为Q_ζ,k-1＝0.1。相关噪声的互协方差为S_k＝0.05。下面给出文献(魏喜庆,宋申民.基于容积卡尔曼滤波的卫星姿态估计[J].宇航学报,2013,34(2):193-200)提出的CQE和所设计的CQE-MCNS的姿态估计结果。

(一)CQE估计结果

图1和图5分别给出的是角度估计误差和陀螺漂移量估计误差的2-范数曲线，图2-图4和图6-图8则给出三轴角度估计误差和陀螺漂移量估计误差曲线。

从仿真中可以看出，CQE的角度估计误差在0.002°左右，陀螺漂移估计精度达到了0.01°/h。

(二)CQE-MCNS估计结果

图9和图13分别给出的是角度估计误差和陀螺漂移量估计误差的2-范数曲线，图10-图12和图14-图16则给出三轴角度估计误差和陀螺漂移量估计误差曲线。

从仿真中可以看出，CQE-MCNS的角度估计误差在0.0005°左右，陀螺漂移估计精度达到了0.005°/h。在估计精度上，CQE-MCNS相比于CQE有了显著的提高，这是由于乘性噪声和噪声相关性的存在对CQE的估计精度产生了一定的影响。从而表明本文所设计的CQE-MCNS算法可以有效估计具有乘性状态噪声及乘性噪声与观测噪声相关的系统状，且有较高的估计精度。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。