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

摘要

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

主权项

1.一种基于ESOQPF和UKF主从滤波的微小卫星姿态确定方法，其特征在于包括以下步骤：(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.陀螺数学模型${\tilde{\omega }}_k={\omega }_k+{\beta }_k+{\eta }_{v,k},$β_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主滤波器估计第k时刻的姿态四元数此外，主滤波器估计第k时刻姿态四元数时，要利用从滤波器第k-1时刻估计的陀螺漂移(4)四元数转换为姿态角根据四元数与姿态角之间的关系，将步骤(3)估计出的姿态四元数转换为姿态四元数对应的姿态角：横滚角俯仰角θ_k，偏航角ψ_k；(5)以陀螺漂移为状态变量，基于步骤(1)中的陀螺漂移数学模型和矢量观测模型建立从滤波器的状态空间模型，利用UKF从滤波器估计第k时刻的陀螺漂移此外，从滤波器估计第k时刻陀螺漂移时，要利用主滤波器第k-1时刻估计的姿态四元数(6)当有新的观测值时，重复步骤(3)～(5)进行姿态确定，得到姿态角估计值和陀螺漂移估计值；所述步骤(3)中ESOQPF主滤波器将QPF算法与ESOQ2算法相结合来估计姿态四元数；ESOQPF主滤波器首先利用QPF算法计算出Davenport矩阵，然后利用ESOQ2算法直接计算出该矩阵的最大特征值，最后计算出最大特征值对应的特征向量，即姿态四元数；具体实现步骤如下：(31)QPF算法计算Davenport矩阵K利用第k-1时刻的四元数粒子q_k-1(i)、归一化的粒子权值陀螺漂移估计值和第k时刻的观测值b_k计算Davenport矩阵K；a.时间更新根据姿态四元数运动学方程，由第k-1时刻的四元数粒子q_k-1(i)求各粒子第k时刻的预测值i＝1，...，N；b.测量更新测量更新即粒子权值更新，利用第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)$式中，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，...，Nc.计算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]$其中，$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)$$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]$式中，tr(B_k)为矩阵B_k的迹；(32)ESOQ2算法估计姿态四元数a.求矩阵K的最大特征值λ_max${\lambda }_{\max }=\frac{1}{2}(\sqrt{2\sqrt{d}-b}+\sqrt{-2\sqrt{d}-b})$式中，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的行列式；b.求姿态四元数估计值矩阵K最大特征值λ_max对应的特征向量为：${\tilde{q}}_k=\left[\begin{array}{c}({\lambda }_{\max }-\mathrm{tr}\left(B_k\right))e_k\\ z^T{e}_k\end{array}\right]$式中，e_k为四元数对应的欧拉旋转轴矢量；将归一化，求得第k时刻姿态四元数估计值为：${\hat{q}}_k={\tilde{q}}_k/\sqrt{{{\tilde{q}}_k}^T{\tilde{q}}_k}$估计出的姿态四元数将在步骤(4)转换为姿态四元数对应的姿态角。