CN100559125C - 一种基于Euler-q算法和DD2滤波的航天器姿态确定方法 - Google Patents

Abstract

一种基于Euler-q(Euler-quaternion)算法和DD2(Divided Difference2)滤波的航天器姿态确定方法，本发明涉及一种基于矢量观测的状态估计姿态确定方法。其特征在于根据参考星历和由星敏感器观测得到的参考矢量和观测矢量，通过Euler-q算法获得航天器姿态四元数，经过DD2滤波，一方面反馈校正陀螺解算获得的姿态阵，另一方面，修正三轴陀螺角速度输出，对陀螺仪的常值漂移进行补偿。这样既可以得到高精度实时的航天器姿态信息，又可以在线标定陀螺仪的常值漂移。本发明是一种自主式姿态确定方法，具有精度高，实时性好，简便易行的特点，可应用于各种航天器的姿态确定系统。

Description

一种基于Euler-q算法和DD2滤波的航天器姿态确定方法

技术领域

本发明涉及一种航天器的姿态确定方法，特别是一种基于Euler-q(Euler-quaternion)算法和DD2(Divided Difference 2)滤波的航天器姿态确定方法，用于各种中高精度的惯性/天文组合导航系统的姿态确定。

背景技术

航天器的姿态确定任务是利用航天器上的姿态敏感器测量所得到的信息，经过适当的处理，求得固连于航天器本体坐标系相对空间某一参考坐标系的信息，航天器由于各种任务的要求，需要高精度姿态信息，这是航天器发展和广泛应用的关键技术之一。

惯性/天文，即INS(Inertial Navigation System)/CNS(CelestialNavigation System)姿态确定系统是一种完全自主的姿态确定导航系统，它利用陀螺测量的载体角速度信息和星敏感器测量视场内的星光信息，在初始信息的基础上进行姿态解算和滤波，可以连续、实时地提供姿态，具有自主性强、隐蔽性好、不受气候条件限制等优点，因而广泛应用于航天等领域。目前使用的航天器姿态确定模型，大多以姿态四元数为状态，由于三轴姿态的自由度为3，因此姿态四元数存在冗余，而如果单纯取姿态四元数的矢量为估计的状态，则必须对估计模型线性化简化，造成模型的精度降低。应用于航天器姿态确定方法中，单纯使用确定性矢量观测，例如Davenport q方法由于需要计算特征值和特征矩阵，在线计算困难，因而难以得到应用。而在Davenport q方法上改进的QUEST(Quaternion Estimator)方法，虽然在算法上得到了极大的改进，但是仍然存在过于依赖星敏感器精度且不能修正陀螺漂移并输出实时姿态的缺点。SVD(the Singular ValueDecomposition)鲁棒性很强但却需要奇异值分解，由于奇异值分解是一项运算量很大的复杂工作，因此不是很有效。FOAM(A Fast Optimal MatrixAlgurithm)算法也不能估计陀螺漂移使陀螺输出获得在线标定；而使用其它状态估计法，例如，扩展卡尔曼滤波EKF(Extend Kalman Filter)，由于使用泰勒展开的一阶非线性处理，虽然可以估计非线性模型，但是对非线性模型估计精度不高，而UKF(Unsented Kalman Filter)则利用一系列近似高斯分布的采样点，通过Unscented变换来进行状态与误差协方差的递推和更新，虽然精度有所提高，但是计算繁琐，实时性不高，因而很难满足姿态确定实时性的要求。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种基于Euler-q算法和DD2滤波的航天器姿态确定方法，该方法由Euler-q算法计算矢量观测得到的航天器姿态，同时由DD2滤波估计姿态误差和陀螺的常值漂移，不仅提供了高精度的姿态信息，而且校正了陀螺角速率输出。

本发明的技术解决方案为：一种基于Euler-q算法和DD2滤波的航天器姿态确定方法，其特征在于使用Euler-q算法和DD2滤波估计模型，利用星敏感器和陀螺仪，既可得到高精度实时的航天器姿态，又可以估计出陀螺仪的常值漂移，其具体步骤如下：

(1)由陀螺仪敏感航天器姿态，输出三轴角速率，并使用角增量算法更新姿态阵，获得航天器在地心惯性坐标系下的实时姿态：航向角

俯仰角θ，横滚角γ；

(2)利用星敏感器敏感星光，经过星图预处理得到观测星光矢量s在航天器本体坐标系下的坐标；并经过星图匹配识别之后得到与之对应的参考星光矢量v在地心惯性坐标系下的坐标；

(3)使用Euler-q算法，利用观测星光矢量和与之对应的参考星光矢量，获得航天器姿态四元数；

(4)以星光观测得到的航天器姿态四元数与步骤(1)中陀螺仪解算的航天器姿态之差作为观测量，对航天器姿态误差和陀螺漂移进行DD2滤波，状态量为：(Δq₁ Δq₂ Δq₃ b₁ b₂ b₃)；

(5)由DD2滤波得到的航天器姿态误差和陀螺仪常值漂移，反馈校正陀螺仪输出姿态和陀螺仪输出角速率，得到高精度的航天器姿态以及高精度的陀螺输出；

(6)反馈校正后，重复(1)-(5)步骤。

本发明的原理是：使用陀螺仪输出角速率解算姿态作为短期参考，提供实时的姿态，短时间内有较高的精度，但是姿态信息不断发散，由星敏感器获得的星光矢量通过Euler-q算法以一定的频率提供高精度的长期参考航天器姿态，用这个长期参考姿态作为观测量，通过DD2滤波器，修正陀螺输出姿态，纠正发散信息，同时估计陀螺常值漂移，反馈校正陀螺输出，对陀螺进行在线标定，保持高精度的姿态实时输出。

本发明与现有技术相比的优点在于：本发明提供由Euler轴和Euler角的旋转特性来计算姿态阵的Euler q方法，计算速度更快，同时采用以姿态四元数矢量和陀螺误差为状态的模型，提高了模型的精度，避免了计算中的奇异性；并且在组合定姿滤波过程中采用DD2滤波估计四元数矢量误差和陀螺漂移，以多项式逼近的新的滤波器考虑了新模型状态估计的不确定性，更容易实现且不需要求导，精度优于EKF且不低于UKF，在保证定姿精度的同时大大提高的计算速度，同时估计出陀螺的常值漂移，大大方便了工程实际应用。

附图说明

图1为本发明的使用Euler-q算法和DD2滤波的航天器姿态确定方法流程图；

图2为航天器本体坐标系与地心惯性坐标系之间的关系。

具体实施方式

如图1、2所示，本发明的具体实施方法如下：

1、由陀螺仪敏感航天器姿态，输出三轴角速率，并使用角增量算法更新姿态阵，获得航天器在地心惯性坐标系，即地球地心为原点，指向天顶为z_i，指向春分点为x_i，y_i与x_i、z_i成右手螺旋下的实时姿态：航向角

俯仰角θ，横滚角γ。陀螺输出姿态频率由实际陀螺采样速率决定，本例中采用输出频率为100HZ；

由初始姿态计算得出初始姿态四元数q阵[q₁ q₂ q₃ q₄]，下标0为初始值：

更新矩阵为：

$q(n+1)=\{\cos \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}I+\frac{\sin \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}}{{\mathrm{\Δ\θ}}_0}[\mathrm{\Δ\θ}\left]\right\}q\left(n\right)---\left(6\right)$

Δθ＝[Δθ₁ Δθ₂ Δθ₃]为陀螺输出角增量。Δθ为Δθ的斜对称阵：

$\left[\begin{array}{ccc}0& {\mathrm{\Δ\θ}}_2& -{\mathrm{\Δ\θ}}_3\\ -{\mathrm{\Δ\θ}}_2& 0& {\mathrm{\Δ\θ}}_1\\ {\mathrm{\Δ\θ}}_3& -{\mathrm{\Δ\θ}}_1& 0\end{array}\right]$

计算姿态余弦阵为：

$C=\left[\begin{array}{ccc}{q}_4^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_1{q}_2-q_4{q}_3)& q_4^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_1{q}_3+q_4{q}_2)& 2(q_2{q}_3-q_4{q}_1)& q_4^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]---\left(7\right)$

由方向余弦阵计算得出地心惯性坐标系下的实时姿态角。

姿态角解算公式如下：

俯仰角为：

θ＝sin^-1(C₂₃)                                        (8)

偏航角如下表：

C<sub>21</sub>值判断	C<sub>22</sub>值判断	偏航角
			＝0	＞0	0
＜0	＞0	atan<sup>-1</sup>(-C<sub>21</sub>/C<sub>22</sub>)
			＜0	＝0	π/2
＜0	＜0	atan<sup>-1</sup>(-C<sub>21</sub>/C<sub>22</sub>)+π
			＝0	＜0	π
＞0	＜0	atan<sup>-1</sup>(-C<sub>21</sub>/C<sub>22</sub>)+π
			＞0	＝0	3π/2
＞0	＞0	atan<sup>-1</sup>(-C<sub>21</sub>/C<sub>22</sub>)+2π

横滚角值如下表：

C<sub>13</sub>值判断	C<sub>33</sub>值判断	横滚角
			＝0	＜0	-π
＞0	＜0	atan<sup>-1</sup>(-C<sub>13</sub>/C<sub>33</sub>)-π
			＞0	＝0	-π/2
任意值	＞0	atan<sup>-1</sup>(-C<sub>13</sub>/C<sub>33</sub>)
			＜0	＝0	π/2
＜0	＜0	atan<sup>-1</sup>(-C<sub>13</sub>/C<sub>33</sub>)+π

2、利用星敏感器敏感星光，经过星图预处理得到观测星光矢量s在航天器本体坐标系下的坐标，即航天器质心为原点，与地心连线反向为z_b轴，与天纬相切指向东为x_b，y_b与z_b、x_b成右手螺旋；并经过星图匹配识别之后得到与之对应的参考星光矢量v在地心惯性坐标系下的坐标；

3、使用Euler-q算法，利用观测星光矢量和与之对应的参考星光矢量，获得航天器姿态四元数；

星敏感器相关精度： ${\mathrm{\α}}_i=\frac{1}{{\mathrm{\β}}_i\underset{k=1}{\overset{n}{\mathrm{\Σ}}}1/{\mathrm{\β}}_k}---\left(9\right)$

计算B矩阵： $B=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\α}}_i{s}_i{v}_i^T=\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right]---\left(10\right)$

计算向量Z：z＝{b₂₃-b₃₂ b₃₁-b₁₃ b₁₂-b₂₁}^T    (11)

计算相关权重： ${\mathrm{\ξ}}_i=\frac{1}{{\mathrm{\β}}_i\underset{k=1}{\overset{n}{\mathrm{\Σ}}}1/{\mathrm{\β}}_k}---\left(12\right)$

其中下标i为第i个星光，β_i为星敏感器观测第i个星光时的精度，n为星光个数，以星图识别星光的个数为准。

引入单位向量： $d_i=\frac{v_i-s_i}{\left|\right|v_i-s_i\left|\right|}---\left(13\right)$

计算对称矩阵： $H=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{d}_i{d}_i^T=\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ h_{21}& h_{22}& h_{23}\\ h_{31}& h_{32}& h_{33}\end{array}\right]---\left(14\right)$

Euler轴即为H阵的最小特征值的特征向量。H阵的最小特征值的特征向量的计算公式如下：

$H{\stackrel{\→}{e}}_{\mathrm{opt}}={\mathrm{\λ}}_{\min }{\stackrel{\→}{e}}_{\mathrm{opt}}---\left(15\right)$

λ_min为最小特征值，

为最优特征矢量。

可以直接从H阵3阶特征等式中直接解算出来：

λ³+aλ²+bλ+c＝0                    (16)

这里a，b，c系数可以根据H阵的元素来表示：

a＝-tr[H]＝-h₁₁-h₂₂-h₃₃

$b=-\mathrm{tr}\left[\mathrm{adjH}\right]=h_{11}{h}_{22}+h_{11}{h}_{33}+h_{22}{h}_{33}-h_{12}^2-h_{13}^2-h_{23}^2---\left(17\right)$

$c=-\det \left[H\right]=h_{11}{h}_{22}{h}_{33}+2h_{12}{h}_{13}{h}_{23}-h_{22}{h}_{13}^2-h_{11}{h}_{23}^2-h_{33}{h}_{12}^2$

设定参数为：

p²＝(a/3)²-(b/3)

q＝[(b/3)-(a/3)²](a/3)-(c/3)                       (18)

$w=\frac{1}{3}{\cos }^{-1}(q/p^3)$

三个实根解为：

${\mathrm{\λ}}_1=-p(\sqrt{3}\sin w+\cos w)-a/3$

${\mathrm{\λ}}_2=p(\sqrt{3}\sin w-\cos w)-a/3---\left(19\right)$

λ₃＝2pcosw-a/3

由于0≤w≤π/3，得出λ_i满足条件：

0≤λ₁≤λ₂≤λ₃     (20)

λ_min即为最小λ值。一下计算λ_min的特征向量，即Euler q轴：

$(H-\mathrm{\λI}){\stackrel{\→}{e}}_{\mathrm{opt}}=M{\stackrel{\→}{e}}_{\mathrm{opt}}=\left[\begin{array}{c}{\stackrel{\→}{M}}_1\\ {\stackrel{\→}{M}}_2\\ {\stackrel{\→}{M}}_3\end{array}\right]{\stackrel{\→}{e}}_{\mathrm{opt}}=\left[\begin{array}{ccc}{m}_a& m_x& m_y\\ m_x& m_b& m_z\\ m_y& m_z& m_c\end{array}\right]{\stackrel{\→}{e}}_{\mathrm{opt}}=0---\left(21\right)$

给出三个可选择的解：

${\stackrel{\→}{e}}_1={\stackrel{\→}{M}}_2\×{\stackrel{\→}{M}}_3=\left\{\begin{array}{ccc}{m}_b{m}_c-m_z^2& m_y{m}_z-m_x{m}_c& m_x{m}_z-m_y{m}_b\end{array}\right\}$

${\stackrel{\→}{e}}_2={\stackrel{\→}{M}}_3\×{\stackrel{\→}{M}}_1=\left\{\begin{array}{ccc}{m}_y{m}_z-m_x{m}_c& m_a{m}_c-m_y^2& m_x{m}_y-m_z{m}_a\end{array}\right\}---\left(22\right)$

${\stackrel{\→}{e}}_3={\stackrel{\→}{M}}_1\×{\stackrel{\→}{M}}_2=\left\{\begin{array}{ccc}{m}_x{m}_z-{m_{y}m}_b& m_x{m}_y-m_z{m}_a& m_a{m}_b-m_x^2\end{array}\right\}$

所有的

(k＝1，2，3)都是平行的，计算各个矢量的模为：

$p_1=|m_b{m}_c-m_z^2|$

$p_2=|m_a{m}_c-m_y^2|---\left(23\right)$

$p_3=|m_a{m}_b-m_x^2|$

选择最接近单位值的p_k(k＝1，2，3)，然后由 ${\stackrel{\→}{e}}_{\mathrm{opt}}={\stackrel{\→}{e}}_k/\left|\right|{\stackrel{\→}{e}}_k\left|\right|$ 得出最优的Euler-q轴。

计算得出相关Euler-q角Φ_opt：

$\sin {\mathrm{\Φ}}_{\mathrm{opt}}=(1/D)Z^T{\stackrel{\→}{e}}_{\mathrm{opt}}---\left(24\right)$

$\cos {\mathrm{\Φ}}_{\mathrm{opt}}=(1/D)\left(\mathrm{tr}\left(B\right)\right)-{\stackrel{\→}{e}}_{\mathrm{opt}}^{T}B{\stackrel{\→}{e}}_{\mathrm{opt}}$

其中： $D^2=\left(Z^T{\stackrel{\→}{e}}_{\mathrm{opt}}\right)+{(\mathrm{tr}\left(B\right)-{\stackrel{\→}{e}}_{\mathrm{opt}}^T{\mathrm{Be}}_{\mathrm{opt}})}^2$

航天器姿态矩阵A可以由Euler-q轴和Euler-q角来表示：

$A=I\cos {\mathrm{\Φ}}_{\mathrm{opt}}+(1-\cos \mathrm{\Φ}){\stackrel{\→}{e}}_{\mathrm{opt}}{{\stackrel{\→}{e}}_{\mathrm{opt}}}^T-\tilde{e}\sin {\mathrm{\Φ}}_{\mathrm{opt}}---\left(25\right)$

是的叉乘斜对称矩阵。由此解算航天器姿态。

4、以星光观测得到的航天器姿态四元数与陀螺仪解算的航天器姿态之差，作为观测量，对航天器姿态误差和陀螺漂移进行DD2滤波，滤波周期由星图识别间隔时间决定，本例中频率为1HZ，状态量为：(δq₁ δq₂ δq₃ b₁ b₂ b₃)

状态方程：

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\stackrel{\→}{q}}\\ \stackrel{\·}{\stackrel{\→}{b}}\end{array}\right]=\left[\begin{array}{c}-[w\×]\mathrm{\δ}\stackrel{\→}{q}-\frac{1}{2}[{\mathrm{\δq}}_0{I}_3+\mathrm{\δ}\stackrel{\→}{q}]\stackrel{\→}{b}-\frac{1}{2}[{\mathrm{\δq}}_0{I}_3+\mathrm{\δ}\stackrel{\→}{q}]{\mathrm{\ϵ}}_1\\ {\mathrm{\ϵ}}_2\end{array}\right]---\left(26\right)$

观测方程：

y＝[I₃ 0]X+w                                    (27)

离散化后可得：

x_k+1＝f(x_k，v_k)                                 (28)

y_k＝g(x_k，w_k)                                   (29)

以下各式中，上标-为量的均值，上标^为量的估值，上标~为量的误差。

引入四个cholesky因子分解：

$Q=S_v{S}_v^T$ $R=S_w{S}_w^T$

                       (30)

$\stackrel{\‾}{p}={\stackrel{\‾}{S}}_x{\stackrel{\‾}{S}}_x^T$ $\hat{P}={\hat{S}}_x{\hat{S}}_x^T$

其中， $\stackrel{\‾}{P}=E\left[(x_k-{\stackrel{\‾}{x}}_k){(x_k-{\stackrel{\‾}{x}}_k)}^T\right|Y^{k-1}],$ $\hat{P}=E\left[(x_k-{\stackrel{\‾}{x}}_k){(x_k-{\stackrel{\‾}{x}}_k)}^T\right|Y^k]=\stackrel{\‾}{P}-K_k{P}_y{K}_k^T,$

Q为状态噪声方差阵，R为量测噪声方差阵，Y^k-1＝[y₀ y₁…y_k-1]，为过去的量测矩阵。

使用S_x，j表示S_x矩阵的j列元素。

可以得到以下各量：

$S_{\mathrm{xx}}^{\left(1\right)}\left(k\right)=\left\{(f_i({\hat{x}}_k+h{\hat{S}}_{x,j},{\stackrel{\‾}{v}}_k)+f_i({\hat{x}}_k-h{\hat{S}}_{x,j},{\stackrel{\‾}{v}}_k)/\left(2h\right))\right\}$

$S_{\mathrm{xv}}^{\left(1\right)}\left(k\right)=\left\{(f_i({\hat{x}}_k{,\stackrel{\‾}{v}}_k+h{S}_{v,j})+f_i({\hat{x}}_k-h{S}_{x,j},{\stackrel{\‾}{v}}_k)/\left(2h\right))\right\}$

         (31)

$S_{y\hat{x}}^{\left(1\right)}\left(k\right)=\left\{(g_i({\stackrel{\‾}{x}}_k+h{\stackrel{\‾}{S}}_{x,j},{\stackrel{\‾}{w}}_k)+g_i({\stackrel{\‾}{x}}_k-h{\stackrel{\‾}{S}}_{x,j},{\stackrel{\‾}{w}}_k)/\left(2h\right))\right\}$

$S_{\mathrm{yw}}^{\left(1\right)}\left(k\right)=\left\{(g_i({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k+h{S}_{w,j})+g_i({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k-h{S}_{w,j})/\left(2h\right))\right\}$

$S_{\mathrm{xx}}^{\left(2\right)}\left(k\right)=\left\{\frac{\sqrt{h^2-1}}{{2h}^2}(f_i({\hat{x}}_k+h{\hat{S}}_{x,j},{\stackrel{\‾}{v}}_k)+f_i({\hat{x}}_k-h{\hat{S}}_{x,j},{\stackrel{\‾}{v}}_k)-2f_i({\hat{x}}_k,{\stackrel{\‾}{v}}_k))\right\}$

$S_{\mathrm{xv}}^{\left(2\right)}\left(k\right)=\left\{\frac{\sqrt{h^2-1}}{{2h}^2}(f_i({\hat{x}}_k,{\stackrel{\‾}{v}}_k+h{S}_{v,j})+f_i({\hat{x}}_k-h{S}_{x,j},{\stackrel{\‾}{v}}_k)-2f_i({\hat{x}}_k,{\stackrel{\‾}{v}}_k))\right\}$

        (32)

$S_{y\hat{x}}^{\left(2\right)}\left(k\right)=\left\{\frac{\sqrt{h^2-1}}{{2h}^2}(g_i({\stackrel{\‾}{x}}_k+h{\stackrel{\‾}{S}}_{x,j},{\stackrel{\‾}{w}}_k)+g_i({\stackrel{\‾}{x}}_k-h{\stackrel{\‾}{S}}_{x,j},{\stackrel{\‾}{w}}_k)-2g_i({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k))\right\}$

$S_{\mathrm{yw}}^{\left(2\right)}\left(k\right)=\left\{\frac{\sqrt{h^2-1}}{{2h}^2}(g_i({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k+h{S}_{w,j})+g_i({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k-h{S}_{w,j})-2g_i({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k))\right\}$

假设估计误差是高斯且无偏的，设间隔长度为h²＝3。一步更新：

${\stackrel{\‾}{x}}_{k+1}=\frac{h^2-n_x-n_v}{h^2}f({\hat{x}}_k,{\stackrel{\‾}{v}}_k)+\frac{1}{{2h}^2}\underset{p=1}{\overset{n_x}{\mathrm{\Σ}}}(f({\hat{x}}_k+h{\hat{S}}_{x,p},{\stackrel{\‾}{v}}_k)+f({\hat{x}}_k-h{\hat{S}}_{x,p},{\stackrel{\‾}{v}}_k))$

       (33)

$\frac{1}{{2h}^2}\underset{p=1}{\overset{n_v}{\mathrm{\Σ}}}(f({\hat{x}}_k,{\stackrel{\‾}{v}}_k+h{S}_{v,p})+f({\hat{x}}_k,{\stackrel{\‾}{v}}_k-h{S}_{v,p}))$

$y_k=\frac{h^2-n_x-n_w}{h^2}f({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k)+\frac{1}{{2h}^2}\underset{p=1}{\overset{n_x}{\mathrm{\Σ}}}(g({\stackrel{\‾}{x}}_k+h{\stackrel{\‾}{S}}_{x,p},{\stackrel{\‾}{w}}_k)+g({\stackrel{\‾}{x}}_k-h{\stackrel{\‾}{S}}_{x,p},{\stackrel{\‾}{w}}_k))$

       (34)

$\frac{1}{{2h}^2}\underset{p=1}{\overset{n_v}{\mathrm{\Σ}}}(g({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k+h{S}_{w,p})+g({\stackrel{\‾}{x}}_k,{\stackrel{\‾}{w}}_k-h{S}_{w,p}))$

从复合矩阵中得到：

$S_x\left(k\right)=\left[\begin{array}{cccc}{S}_{x\hat{x}}^{\left(1\right)}\left(k\right)& S_{\mathrm{xv}}^{\left(1\right)}\left(k\right)& S_{x\hat{x}}^{\left(2\right)}\left(k\right)& S_{\mathrm{xv}}^{\left(2\right)}\left(k\right)\end{array}\right]---\left(35\right)$

$S_y\left(k\right)=\left[\begin{array}{cccc}{S}_{y\hat{x}}^{\left(1\right)}\left(k\right)& S_{\mathrm{yw}}^{\left(1\right)}\left(k\right)& S_{y\hat{x}}^{\left(2\right)}\left(k\right)& S_{\mathrm{yw}}^{\left(2\right)}\left(k\right)\end{array}\right]---\left(36\right)$

$P_{\mathrm{xy}}\left(k\right)={\stackrel{\‾}{S}}_x\left(k\right)S_{y\hat{x}}{\left(k\right)}^T---\left(37\right)$

增益矩阵为：

K_k＝P_xy(k)[S_y(k)S_y(k)^T]^-1                                      (38)

${\hat{x}}_k={\stackrel{\‾}{x}}_k+K_k(y_k-{\stackrel{\‾}{y}}_k)---\left(39\right)$

协方差阵：

$\hat{P}=({\stackrel{\‾}{S}}_x-{\mathrm{KS}}_{\mathrm{yx}}^{\left(1\right)}){({\stackrel{\‾}{S}}_x-{\mathrm{KS}}_{\mathrm{yx}}^{\left(1\right)})}^T+{\mathrm{KS}}_{\mathrm{yw}}^{\left(1\right)}{\left({\mathrm{KS}}_{\mathrm{yw}}^{\left(1\right)}\right)}^T+{\mathrm{KS}}_{\mathrm{yx}}^{\left(2\right)}{\left({\mathrm{KS}}_{\mathrm{yx}}^{\left(2\right)}\right)}^T+{\mathrm{KS}}_{\mathrm{yw}}^{\left(2\right)}{\left({\mathrm{KS}}_{\mathrm{yw}}^{\left(2\right)}\right)}^T---\left(40\right)$

${\hat{S}}_x\left(k\right)=\left[\begin{array}{cccc}{\stackrel{\‾}{S}}_x\left(k\right)-{\mathrm{KS}}_{\mathrm{yx}}^{\left(1\right)}\left(k\right)& {\mathrm{KS}}_{\mathrm{yw}}^{\left(1\right)}\left(k\right)& {\mathrm{KS}}_{\mathrm{yx}}^{\left(2\right)}\left(k\right)& {\mathrm{KS}}_{\mathrm{yw}}^{\left(2\right)}\left(k\right)\end{array}\right]---\left(41\right)$

5、由DD2滤波得到的航天器姿态误差，反馈校正陀螺仪输出姿态，得到高精度的航天器姿态；由滤波得到的陀螺仪常值漂移，反馈校正陀螺仪输出角速率，得到校正后的陀螺输出：

由滤波得出的状态中[δq₁ δq₂ δq₃]，获得误差四元数：

$\mathrm{\δq}=\left[\begin{array}{cccc}{\mathrm{\δq}}_1& {\mathrm{\δq}}_2& {\mathrm{\δq}}_3& \mathrm{sqrt}(1-{\mathrm{\δq}}_1^2-{\mathrm{\δq}}_2^2-{\mathrm{\δq}}_3^2)\end{array}\right],$ 计算出姿态误差阵C_g ^c，修正由陀螺输出的姿态阵： $C_{\mathrm{end}}^c=C_g^{c}C---\left(42\right)$

由此计算得出反馈校正后的姿态。

由滤波得出状态中的 $\stackrel{\→}{b}=\left[\begin{array}{ccc}{b}_1& b_2& b_3\end{array}\right],$ 修正陀螺输出：

$w_{\mathrm{end}}=w_{\mathrm{out}}-\stackrel{\→}{b}---\left(43\right)$

式中，w_out为修正前的陀螺输出。

6、反馈校正后，重复1-5步骤。

Claims (3)

1、一种基于Euler-q算法和DD2滤波的航天器姿态确定方法，其特征在于包括以下步骤：

步骤a：由陀螺仪敏感航天器姿态，输出三轴角速率，获得航天器在地心惯性坐标系下的实时姿态；

步骤b：利用星敏感器敏感星光，经过星图预处理得到观测星光矢量s在航天器本体坐标系下的坐标；并经过星图匹配识别之后得到与之对应的参考星光矢量v在地心惯性坐标系下的坐标；

步骤c：使用Euler-q算法，利用观测星光矢量和与之对应的参考星光矢量，获得航天器姿态四元数；

步骤d：以星光观测得到的航天器姿态四元数与陀螺仪解算的航天器姿态之差，作为观测量，对航天器姿态误差和陀螺漂移进行DD2滤波；

步骤e：由DD2滤波得到的航天器姿态误差和陀螺仪常值漂移，反馈校正陀螺仪输出姿态和陀螺仪输出角速率，得到航天器本体坐标系相对于地心惯性坐标系的姿态和陀螺输出；

步骤f：反馈校正后，重复步骤a-步骤e。

2、根据权利要求1所述的基于Euler-q算法和DD2滤波的航天器姿态确定方法，其特征在于：所述的步骤d中，对航天器姿态误差和陀螺漂移的估计是通过DD2滤波器实现的，且姿态确定模型如下：

状态量为：(δq₁ δq₂ δq₃ b₁ b₂ b₃)，其中δq₁ δq₂ δq₃是姿态误差四元数的矢量部分，b₁ b₂ b₃分别是陀螺三轴输出的常值漂移；

状态方程，观测方程分别为：

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\stackrel{\→}{q}}\\ \stackrel{\·}{\stackrel{\→}{b}}\end{array}\right]\left[\begin{array}{c}-[w\×]\mathrm{\δ}\stackrel{\→}{q}-\frac{1}{2}[\mathrm{\δ}{q}_0{I}_3+\mathrm{\δ}\stackrel{\→}{q}]\stackrel{\→}{b}-\frac{1}{2}[\mathrm{\δ}{q}_0{I}_3+\mathrm{\δ}\stackrel{\→}{q}]{\mathrm{\ϵ}}_1\\ {\mathrm{\ϵ}}_2\end{array}\right]---\left(1\right)$

y＝[I₃ 0]X+W    (2)

其中，y为观测量，ε₁，ε₂为高斯白噪声，w为陀螺输出角速率，W为量测噪声，I₃为3×3单位矢量。

3、根据权利要求1所述的基于Euler-q算法和DD2滤波的航天器姿态确定方法，其特征在于：所述的步骤e中，对航天器姿态误差和陀螺漂移的反馈校正是通过以下步骤实现的：

由滤波得出的状态中[δq₁ δq₂ δq₃]，获得误差四元数：

$\mathrm{\δq}=\left[\begin{array}{cccc}\mathrm{\δ}{q}_1& \mathrm{\δ}{q}_2& \mathrm{\δ}{q}_3& \mathrm{sqrt}(1-\mathrm{\δ}{q}_1^2-\mathrm{\δ}{q}_2^2-\mathrm{\δ}{q}_3^2)\end{array}\right],$ 计算出姿态误差阵C_g ^c，修正由陀螺输出的姿态阵C，得到反馈校正后的姿态阵：

$C_{\mathrm{end}}^c=C_g^{c}C---\left(3\right)$

由滤波得出状态中的 $\stackrel{\→}{b}=\left[\begin{array}{ccc}{b}_1& b_2& b_3\end{array}\right],$ 修正陀螺输出：

$w_{\mathrm{end}}=w_{\mathrm{out}}-\stackrel{\→}{b}---\left(4\right)$

式中，w_out为修正前的陀螺输出。

Images