一种基于ESOQPF和UKF主从滤波的微小卫星姿态确定方法

摘要

本发明涉及一种基于ESOQPF和UKF主从滤波的微小卫星姿态确定方法。首先建立微小卫星系统状态空间模型，进行姿态确定滤波初始化；然后以姿态四元数为状态变量，将ESOQPF作为主滤波器估计姿态四元数，将估计出来的四元数转换为相应的姿态角；以陀螺漂移为状态变量，将UKF作为从滤波器估计陀螺漂移。本发明在保证姿态估计精度的同时，从两个方面降低了计算量，缩短了定姿过程。一方面，利用ESOQPF和UKF进行主从滤波，即将姿态四元数与陀螺漂移分开估计；另一方面，ESOQPF估计姿态四元数时将QPF算法与ESOQ2算法相结合，利用QPF算法计算出Davenport矩阵，然后利用ESOQ2算法直接计算出该矩阵最大特征值所对应的特征向量。该方法适用于有陀螺配置模式下，基于矢量观测的微小卫星姿态确定。

说明书

技术领域

本发明涉及一种基于ESOQPF(Estimator of the Quaternion Particle Filter，四元数粒子滤波估计器)和UKF(Unscented Kalman Filter，Unscented卡尔曼滤波)主从滤波的微小卫星姿态确定方法，适用于有陀螺配置模式下，基于矢量观测的微小卫星姿态确定。本发明属于航天器姿态确定领域。

背景技术

姿态确定是微小卫星系统技术中的重要问题之一，其精度和实时性是影响姿态控制系统精度的重要因素。姿态确定的精度和实时性不仅取决于姿态敏感器的性能，还与姿态确定方法有关。

姿态确定方法分为确定性算法和状态估计法两类。常见的确定性算法有：q_method、QUEST(Quaternion Estimator，四元数估计器)、ESOQ(Estimator of the Optimal Quaternion，最优四元数估计器)等。常见的基于状态估计的姿态确定方法有：KF(Kalman Filter，卡尔曼滤波)、EKF(Extended Kalman Filter，扩展卡尔曼滤波)、UKF和PF(Particle Filter，粒子滤波)等。其中，UKF采用U变换来处理状态方程和观测方程的非线性问题，不需要计算Jacobian矩阵，计算的状态均值和方差可以精确到三阶，但UKF对于具有较强非线性、非高斯系统的状态估计就达不到期望的效果。针对上述不足，出现了PF滤波方法。PF是一种利用随机样本或粒子来表示系统状态变量的后验概率分布的递推贝叶斯滤波方法，将PF应用于姿态确定，估计精度高。但PF存在计算量大从而使定姿过程长的缺点，为了达到要求的估计精度，需要上百个甚至上千个粒子，且随着状态维数的增加，计算量会剧增，严重影响姿态确定的实时性。

近几年，已有学者在提高姿态确定实时性方面进行了研究。其中，Yaakov Oshman提出了GA-QPF(Genetics Algorithm-Quaternion Particle Filter，遗传算法-四元数粒子滤波)算法，该算法将姿态四元数与陀螺漂移分开估计，利用QPF(Quaternion Particle Filter，四元数粒子滤波)算法估计四元数，GA(Genetics Algorithm，遗传算法)算法估计陀螺漂移，进而降低了粒子滤波状态维数，提高实时性。其中，QPF算法直接利用加权四元数粒子集进行时间更新和观测更新处理，并结合q_method计算姿态四元数。但是q_method估计姿态四元数时需要求出Davenport(达文波特)矩阵的所有特征值，计算量大，从而定姿过程长。

发明内容

本发明的技术解决问题是：(1)克服了基于粒子滤波的定姿方法计算量大从而定姿过程长，姿态确定实时性差的不足，利用ESOQPF和UKF进行主从滤波，将姿态四元数与陀螺漂移分开估计，在保证姿态确定精度的同时提高实时性；(2)克服了QPF在估计姿态四元数时采用q_method算法引起的计算量大从而定姿过程长的不足，ESOQPF在估计姿态四元数时采用ESOQ2算法，进一步降低计算量，缩短了定姿过程。

本发明的技术解决方案是：基于ESOQPF和UKF主从滤波的微小卫星姿态确定方法，将ESOQPF作为主滤波器估计姿态四元数，UKF作为从滤波器估计陀螺漂移，即姿态四元数与陀螺漂移分开估计。但是，主从滤波器之间又存在相互联系：主滤波器估计第k时刻姿态四元数时，要利用从滤波器第k-1时刻估计的陀螺漂移；从滤波器估计第k时刻陀螺漂移时，要利用主滤波器第k-1时刻估计的姿态四元数。此外，ESOQPF主滤波器估计姿态四元数时采用一种计算量小的确定性算法ESOQ2算法，进一步提高姿态确定的实时性。本发明的具体步骤如下：

(1)建立微小卫星系统状态空间模型

a.姿态四元数运动学方程

q_k+1＝Ω(ω_k)q_k

式中，q_k和q_k+1分别为第k时刻和第k+1时刻卫星本体坐标系相对于参考坐标系的姿态四元数，和q_4k分别为四元数的矢量部分和标量部分；ω_k为第k时刻本体坐标系相对参考坐标系姿态角速度。假设在采样周期Δt内ω_k保持不变，则

$\Omega \left({\omega }_k\right)={\left(\begin{array}{cc}\cos \left(0.5\right|\left|{\omega }_k\right|\left|\mathrm{\Delta t}\right)I_{3\times 3}-[{\psi }_k\times ]& {\psi }_k\\ -{\psi }_k^T& \cos \left(0.5\right|\left|{\omega }_k\right|\left|\mathrm{\Delta t}\right)\end{array}\right)}_{4\times 4}$

式中，I_3×3为3×3单位阵；[ψ_k×]为ψ_k的反对称矩阵。

b.陀螺数学模型

β_k+1＝β_k+η_u，k

式中，为陀螺的测量输出值；ω_k为真实的姿态角速度；β_k和β_k+1分别为第k时刻和第k+1时刻的陀螺漂移；η_v，k与η_u，k是互不相关的零均值高斯白噪声，η_v，k和η_u，k的方差分别为σ_v²与σ_u²。

c.矢量观测模型

b_k＝A(q_k)r_k+δb_k

式中，b_k为观测矢量；r_k为参考矢量；δb_k为测量噪声，且δb_k服从概率分布A(q_k)为参考坐标系到卫星本体坐标系的姿态转移矩阵。

(2)基于步骤(1)建立的系统状态空间模型，进行滤波初始化

滤波初始化要基于步骤(1)建立的系统状态空间模型，包括ESOQPF主滤波器初始化和UKF从滤波器初始化两部分。ESOQPF主滤波器的初始化主要是初始化N个粒子及粒子权值；UKF从滤波器的初始化主要完成陀螺漂移估计值和估计误差方差阵的初始化。

(3)以姿态四元数为状态变量，基于步骤(1)中的四元数运动学方程和矢量观测模型建立主滤波器的状态空间模型，利用ESOQPF估计姿态四元数。ESOQPF主滤波器估计姿态四元数的基本步骤包括：从第k-1时刻到第k时刻的时间更新，测量更新即粒子的权值更新，计算Davenport矩阵K，利用ESOQ2算法估计第k时刻姿态四元数此外，主滤波器估计第k时刻姿态四元数时用到从滤波器第k-1时刻估计出的陀螺漂移

(4)四元数转换成姿态角

根据四元数与姿态角之间的关系，将步骤(3)估计出的姿态四元数转换为姿态四元数对应的姿态角：横滚角俯仰角θ_k，偏航角ψ_k。

(5)以陀螺漂移为状态变量，基于步骤(1)中的陀螺漂移数学模型和矢量观测模型建立从滤波器的状态空间模型，利用UKF估计陀螺漂移。UKF从滤波器估计陀螺漂移的基本步骤包括：计算Sigma点，计算Sigma点权值，时间更新，测量更新。此外，从滤波器估计第k时刻陀螺漂移时用到主滤波器第k-1时刻估计出的姿态四元数

(6)当有新的观测值时，重复步骤(3)～(5)进行姿态确定。

本发明的原理是：微小卫星姿态确定的主要任务是利用星上的姿态敏感器测量所得到的信息，经过适当的处理，求得固连于卫星本体的坐标系相对于空间参考坐标系的姿态。由于陀螺存在漂移，无法将陀螺的输出值直接用于姿态估计，所以在四元数估计过程中要考虑陀螺漂移。本发明原理步骤是：(1)建立微小卫星系统状态空间模型；(2)滤波初始化；(3)以姿态四元数为状态变量，将ESOQPF作为主滤波器估计姿态四元数；(4)将估计出来的四元数转换为姿态角；(5)以陀螺漂移为状态变量，将UKF作为从滤波器估计陀螺漂移。此外，主从滤波器之间又存在相互联系，主滤波器估计第k时刻姿态四元数时，要利用从滤波器第k-1时刻估计的陀螺漂移；从滤波器估计第k时刻陀螺漂移时，要利用主滤波器第k-1时刻估计的姿态四元数。

本发明与现有技术相比的优点在于：本发明涉及一种基于ESOQPF和UKF主从滤波的微小卫星姿态确定方法，在保证姿态估计精度的同时，从两个方面降低了计算量，缩短了定姿过程。(1)从整体上讲，将ESOQPF作为主滤波器估计姿态四元数，UKF作为从滤波器估计陀螺漂移，即姿态四元数与陀螺漂移分开估计，避免了粒子滤波状态维数高而引起计算量大从而定姿过程长的问题，在保证姿态确定精度的同时提高姿态确定方法的实时性；(2)对于主滤波器来讲，ESOQPF估计姿态四元数时，将QPF算法与ESOQ2算法相结合，采用ESOQ2算法直接计算Davenport矩阵的最大特征值所对应的特征向量，克服了QPF中采用q_method计算Davenport矩阵所有特征值引起的计算量大从而定姿过程长的不足，进一步提高姿态确定的实时性。

附图说明

图1为本发明的实现流程图；

图2为ESOQPF与UKF主从滤波器在第k时刻的计算流程图。

具体实施方式

如图1所示，本发明采用ESOQPF和UKF主从滤波确定微小卫星姿态，具体步骤如下：

1.建立微小卫星系统状态空间模型

a.姿态运动学方程

姿态四元数运动学方程的离散形式为：

q_k+1＝Ω(ω_k)q_k                                    (1)

式中，q_k和q_k+1分别为第k时刻和第k+1时刻卫星本体坐标系相对于参考坐标系的姿态四元数，和q_4k分别为四元数的矢量部分和标量部分；ω_k是本体坐标系相对参考坐标系姿态角速度。假设在采样周期Δt内ω_k保持不变，则

$\Omega \left({\omega }_k\right)={\left(\begin{array}{cc}\cos \left(0.5\right|\left|{\omega }_k\right|\left|\mathrm{\Delta t}\right)I_{3\times 3}-[{\psi }_k\times ]& {\psi }_k\\ -{\psi }_k^T& \cos \left(0.5\right|\left|{\omega }_k\right|\left|\mathrm{\Delta t}\right)\end{array}\right)}_{4\times 4}$

式中，I_3×3为3×3单位阵；[ψ_k×]为ψ_k的反对称矩阵。

b.陀螺数学模型

陀螺的数学模型采用下式描述：

β_k+1＝β_k+η_u，k                    (2)

式中，为陀螺的测量输出值；ω_k为真实的姿态角速度；β_k和β_k+1分别为第k时刻和第k+1时刻的陀螺漂移；η_v，k与η_u，k是互不相关的零均值高斯白噪声，η_v，k和η_u，k的方差分别为σ_v²与σ_u²。

c.矢量观测模型

b_k＝A(q_k)r_k+δb_k

式中，b_k为测量矢量；r_k为参考矢量；δb_k为测量噪声，且δb_k服从概率分布A(q_k)为参考坐标系到卫星本体坐标系的姿态转移矩阵，表示为：

$A\left(q_k\right)=[{\left(q_{4k}\right)}^2-{{\bar{q}}_k}^T{\bar{q}}_k]I_{3\times 3}+2{\bar{q}}_k{{\bar{q}}_k}^T-2q_{4k}[{\bar{q}}_k\times ]---\left(3\right)$

式中，为四元数矢量部分的反对称矩阵。

2.基于步骤(1)建立的系统状态空间模型，进行滤波初始化

滤波初始化要基于步骤(1)建立的系统状态空间模型，包括ESOQPF主滤波器的初始化和UKF从滤波器的初始化两部分。

a.ESOQPF主滤波器初始化

四元数粒子初始化为q₀(i)，i＝1，…，N；各个粒子的权值初始化为W₀(i)＝1/N，i＝1，…，N；N为粒子个数。

b.UKF从滤波器初始化

${\hat{\beta }}_0=E\left[{\beta }_0\right],$$P_{{\beta }_0}=E\left[({\beta }_0-{\hat{\beta }}_0){({\beta }_0-{\hat{\beta }}_0)}^T\right]---\left(4\right)$

式中，β₀和分别为初始时刻陀螺漂移的真值和估计值；为初始估计误差方差阵；E[·]表示数学期望。

3.ESOQPF估计姿态四元数

以姿态四元数为状态变量，基于步骤(1)中的四元数运动学方程和矢量观测模型建立主滤波器的状态空间模型：

状态方程：

观测方程：b_k＝A(q_k)r_k+δb_k                                      (6)

由于陀螺存在漂移，其输出值不能直接用于姿态估计，所以在四元数估计过程中要将陀螺漂移估计出来。实际计算过程中，主滤波器估计第k时刻的姿态四元数时要利用从滤波器第k-1时刻估计的陀螺漂移。

ESOQPF主滤波器(1)估计姿态四元数基本步骤如下：

a.QPF算法计算Davenport矩阵K

利用第k-1时刻的四元数粒子q_k-1(i)、归一化的粒子权值和第k时刻的观测值b_k计算Davenport矩阵K。

1)时间更新

根据式(5)由第k-1时刻的四元数粒子q_k-1(i)求第k时刻各粒子的预测值${\hat{q}}_{k|k-1}\left(i\right),i=1,...,N.$

2)测量更新

测量更新即粒子权值更新，利用第k-1时刻的归一化的粒子权值和第k时刻的观测值b_k计算第k时刻的粒子权值W_k(i)。

$W_k\left(i\right)\propto p_{b_k|q_k}\left(b_k\right|{\hat{q}}_{k|k-1}\left(i\right)){\tilde{W}}_{k-1}\left(i\right)=p_{\delta b_k}(b_k-A\left({\hat{q}}_{k|k-1}\left(i\right)\right)r_k){\tilde{W}}_{k-1}\left(i\right)---\left(7\right)$

式中，i＝1，…，N；为的似然概率；为的概率。

将粒子权值W_k(i)归一化：

${\tilde{W}}_k\left(i\right)=W_k\left(i\right)/\underset{i=1}{\overset{N}{\Sigma }}{W}_k\left(i\right),i=1,...,N---\left(8\right)$

4)计算Davenport矩阵K

$K\stackrel{\Delta }{=}\left(\begin{array}{cc}{B}_k+{B_k}^T-I_{3\times 3}\mathrm{tr}\left(B_k\right)& z\\ z^T& \mathrm{tr}\left(B_k\right)\end{array}\right)---\left(9\right)$

其中，$B_k\stackrel{\Delta }{=}\underset{i=1}{\overset{N}{\Sigma }}{\tilde{W}}_k\left(i\right)A\left({\hat{q}}_{k|k-1}\left(i\right)\right)---\left(10\right)$

$z\stackrel{\Delta }{=}\left(\begin{array}{c}{B}_k\left(\mathrm{3,2}\right)-B_k\left(\mathrm{2,3}\right)\\ B_k\left(\mathrm{1,3}\right)-B_k\left(\mathrm{3,1}\right)\\ B_k\left(\mathrm{2,1}\right)-B_k\left(\mathrm{1,2}\right)\end{array}\right)---\left(11\right)$

式中，tr(B_k)为矩阵B_k的迹。

b.利用ESOQ2估计姿态四元数

第k时刻四元数估计值为矩阵K最大特征值所对应的特征向量，即：

$K{\hat{q}}_k={\lambda }_{\max }{\hat{q}}_k---\left(12\right)$

1)求矩阵K的最大特征值λ_max

${\lambda }_{\max }=\frac{1}{2}(\sqrt{2\sqrt{d}-b}+\sqrt{-2\sqrt{d}-b})---\left(13\right)$

式中，b＝-2tr(B_k)+tr(adj(B_k+B_k^T))-z^Tz；d＝det(K)；adj(B_k+B_k^T)为矩阵(B_k+B_k^T)的伴随矩阵；tr(adj(B_k+B_k^T))为矩阵adj(B_k+B_k^T)的迹；det(K)为矩阵K的行列式。

2)求第k时刻姿态四元数估计值

姿态四元数与欧拉轴/角的关系：

q＝[e^T sin(Φ/2) cos(Φ/2)]^T                    (14)

式中，e为旋转轴矢量，Φ为绕旋转轴的转角。

将式(14)代入式(12)并消去Φ得：

[(tr(B_k)-λ_max)S-zz^T]e＝Me＝0                  (15)

式中，S＝B_k+B_k^T-(tr(B_k)+λ_max)I_3×3，由上式可知，矩阵M为对称阵，即

$M=M^T=\left(\begin{array}{ccc}{\overrightarrow{m}}_1& {\overrightarrow{m}}_2& {\overrightarrow{m}}_3\end{array}\right)=\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)---\left(16\right)$

由式(15)可知，旋转轴e与矩阵M的各行/列向量都正交，所以可通过两向量叉乘求取旋转轴e。因此，旋转轴e的取值有三种情况：

$e_1={\overrightarrow{m}}_2\times {\overrightarrow{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)}^T$

$e_2={\overrightarrow{m}}_3\times {\overrightarrow{m}}_1---\left(17\right)$

$={\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)}^T$

$e_3={\overrightarrow{m}}_1\times {\overrightarrow{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_z}^2\end{array}\right)}^T$

式(17)中的e_i(i＝1，2，3)是相互平行的，但模值不同，选取则最大特征值λ_max对应的特征向量为：

${\tilde{q}}_k=\left(\begin{array}{c}({\lambda }_{\max }-\mathrm{tr}[B_k\left]\right)e_k\\ z^T{e}_k\end{array}\right)---\left(18\right)$

将归一化得第k时刻姿态四元数估计值为：

${\hat{q}}_k={\tilde{q}}_k/\sqrt{{{\tilde{q}}_k}^T{\tilde{q}}_k}---\left(19\right)$

4.四元数转换成姿态角

根据四元数与姿态角之间的关系，将步骤(3)估计出的姿态四元数转换为姿态四元数对应的姿态角：横滚角俯仰角θ_k，偏航角ψ_k。

5.UKF估计陀螺漂移

由于陀螺存在漂移，无法将陀螺的输出值直接用于姿态估计，所以在四元数估计过程中要将陀螺漂移估计出来。

以陀螺漂移为状态变量，基于步骤(1)中的陀螺漂移数学模型和矢量观测模型建立从滤波器的状态空间模型。

状态方程：β_k＝β_k-1+η_u，k-1                                  (20)

观测方程：$b_k=A\left(q_k\right)r_k+\delta b_k---\left(21\right)$

$=A\left(\Omega ({\tilde{\omega }}_{k-1}-{\beta }_{k-1})q_{k-1}\right)r_k+\delta b_k$

由状态空间模型可知：状态方程为线性，观测方程为非线性。从滤波器估计第k时刻陀螺漂移时要利用主滤波器第k-1时刻估计的姿态四元数。

假设状态方程和观测方程的噪声方差阵分别为Q和R，UKF从滤波器(2)估计陀螺漂移的基本步骤为：

a.计算Sigma点

$\left(\begin{array}{c}{\chi }_{0,k-1}={\hat{\beta }}_{k-1}\\ {\chi }_{i,k-1}={\hat{\beta }}_{k-1}+\sqrt{n+\tau }{\left(\sqrt{P_{{\hat{\beta }}_{k-1}}}\right)}_i\\ {\chi }_{i+n,k-1}={\hat{\beta }}_{k-1}-\sqrt{n+\tau }{\left(\sqrt{P_{{\hat{\beta }}_{k-1}}}\right)}_i\end{array}\right),i=1,...,n---\left(22\right)$

式中，χ_0，k-1，χ_i，k-1，χ_i+n，k-1分别是对应的第0个、第i个、第i+n个Sigma点，对应的Sigma点共有2n+1个；和分别为第k-1时刻陀螺漂移的估计值和估计误差方差阵；n为状态维数，这里n＝3；τ为合成比例参数；将估计误差方差阵分解表示A的第i列。

b.计算Sigma点对应的权值

$\left(\begin{array}{c}{W}_0=\tau /(n+\tau )\\ W_i=1/\left[2(n+\tau )\right]\\ W_{i+n}=1/\left[2(n+\tau )\right]\end{array}\right)i=1,...,n---\left(23\right)$

式中，W₀，W_i，W_i+n分别是第0个、第i个、第i+n个Sigma点对应的权值。

c.时间更新

第k时刻各Sigma点的预测值

χ_i，k|k-1＝χ_i，k-1，i＝0，1，…，2n                (24)

第k时刻陀螺漂移的预测均值和预测误差方差阵分别为：

${\hat{\beta }}_{\bar{k}}=\underset{i=0}{\overset{2n}{\Sigma }}{W}_i{\chi }_{i,k|k-1},$$P_{{\stackrel{\widehat{‾}}{\beta }}_k}=P_{{\hat{\beta }}_{k-1}}+Q---\left(25\right)$

式中，为第k-1时刻陀螺漂移的预测误差方差阵。

第k时刻各Sigma点对应的测量预测值

${\hat{b}}_{k|k-1}\left(i\right)=A\left(\Omega ({\tilde{\omega }}_{k-1}-{\chi }_{i,k|k-1}){\hat{q}}_{k-1}\right)r_k,i=\mathrm{0,1},...,2n---\left(26\right)$

测量的预测均值为：

${\hat{b}}_{\bar{k}}=\underset{i=0}{\overset{2n}{\Sigma }}{W}_i{\hat{b}}_{k|k-1}\left(i\right)---\left(27\right)$

d.测量更新

测量的预测误差方差阵和测量与状态变量之间的协方差阵分别为：

$P_{b_k{b}_k}=\underset{i=0}{\overset{2n}{\Sigma }}{W}_i({\hat{b}}_{k|k-1}\left(i\right)-{\hat{b}}_{\bar{k}}){({\hat{b}}_{k|k-1}\left(i\right)-{\hat{b}}_{\bar{k}})}^T+R---\left(28\right)$

$P_{{\beta }_k{b}_k}=\underset{i=0}{\overset{2n}{\Sigma }}{W}_i({\chi }_{i,k|k-1}-{\hat{\beta }}_{\bar{k}}){({\hat{b}}_{k|k-1}\left(i\right)-{\hat{b}}_{\bar{k}})}^T---\left(29\right)$

增益矩阵：$K_k=P_{{\beta }_k{b}_k}{P}_{b_k{b}_k}^{-1}---\left(30\right)$

则第k时刻陀螺漂移的估计值和陀螺漂移估计误差方差阵分别为：

${\hat{\beta }}_k={\hat{\beta }}_{\bar{k}}+K_k(b_k-{\hat{b}}_{\bar{k}})---\left(31\right)$

$P_{{\hat{\beta }}_k}=P_{{\stackrel{\widehat{‾}}{\beta }}_k}-K_k{P}_{b_k{b}_k}{K_k}^T---\left(32\right)$

6.第k+1时刻，当有新的观测值时，重复步骤3～5进行姿态确定。

一种基于ESOQPF和UKF主从滤波的微小卫星姿态确定方法，该方法适用于有陀螺配置模式下，基于矢量观测的微小卫星姿态确定。在保证姿态估计精度的同时，从两个方面降低了计算量，缩短了定姿过程。一方面，将姿态四元数与陀螺漂移分开估计，另一方面，在ESOQPF主滤波器中估计姿态四元数时采用一种计算量小的确定性算法。

本发明说明书中未作详细描述的内容属于本专业领域技术人员公知的现有技术。