CN100487618C - 一种基于遗传最优request和gupf的组合定姿方法 - Google Patents

Abstract

一种基于遗传最优REQUEST和GUPF的组合定姿方法，本发明涉及一种基于恒星敏感器的惯性/天文组合定姿方法。该方法首先对惯性量测信息进行补偿陀螺输出数据后，通过姿态解算，得到载体姿态信息；其次利用天文量测信息，结合补偿后的陀螺输出数据，通过基于遗传最优REQUEST方法快速获得特定间隔的高精度天文姿态信息；最后利用Genetic Unscented Particle Filter(GUPF)算法将天文姿态信息和载体姿态信息相融合，解决系统非线性和噪声非高斯问题，求解高精度载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的长时间、高精度组合定姿。

Description

一种基于遗传最优REQUEST和GUPF的组合定姿方法

技术领域

本发明涉及一种基于恒星敏感器的惯性/天文组合定姿方法，特别是一种基于遗传最优REQUEST和GUPF的组合定姿方法，可用于各种航天器的高精度组合定姿。

背景技术

为满足天基对地观测、武器精确打击以及空间探索开发的迫切需求，各类地球卫星、深空探测器、载人飞船、弹道导弹和运载火箭等航天器必须具备自主运行和管理能力，而高精度的自主定姿是航天器自主运行和管理的核心技术瓶颈。目前，航天器的高精度自主定姿，无法依靠任何一种导航手段独立实现。纯惯性导航系统能够自主、实时提供连续、全面的导航信息，短时精度高，但其误差随工作时间积累，难以满足航天器的长时间高精度定姿要求；天文导航能够提供高精度姿态信息，误差不随时间积累，但易受气候条件限制，且输出信息不连续；将这两者相结合、优势互补，构成惯性/天文组合定姿系统，是实现航天器长时间、高精度定姿的最为有效的手段。

在惯性/天文组合定姿技术方面，1996年Bar-Itzhack提出了一种REQUEST方法，它以K-矩阵为状态变量，顺序转移和传播K-矩阵，使用渐消记忆因子来处理噪声问题，但这种方法是次优的。2001年Daniel Choukroun等人在REQUEST方法的基础上，提出了最优REQUEST方法，它将REQUEST算法嵌入卡尔曼滤波的框架来计算最优增益，可达到最优，但无法估计陀螺漂移。针对这个问题，一些学者提出了基于EKF和最优REQUEST相结合的定姿方法，虽然可估计陀螺漂移，但在非线性系统和非高斯噪声情况下估计性能较差，因而存在陀螺漂移估计精度较低的缺点。专利申请号为200610165166.4中提出了一种基于最优REQUEST算法和UKF的惯性恒星罗盘组合定姿方法，该方法特别适用于非线性系统，但在非高斯噪声情况下仍存在滤波精度低和速度慢的不足。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提出一种基于遗传最优REQUEST和GUPF的组合定姿方法，解决系统非线性和噪声非高斯问题，以快速获得高精度的姿态信息，并能够准确地估计陀螺漂移，实现各种类型航天器长时间、高精度的组合定姿。

本发明的技术解决方案为：一种基于遗传最优REQUEST和GUPF的组合定姿方法，其特点在于：利用惯性量测信息和天文量测信息，通过遗传最优REQUEST(最优回归四元数估计法)和GUPF(遗传无迹粒子滤波)方法，实现航天器长时间、高精度的快速组合定姿，其实现步骤如下：

(1)对惯性量测信息进行补偿陀螺输出数据后，通过姿态解算，得到载体姿态信息；

(2)利用天文量测信息，结合补偿后的陀螺输出数据，通过遗传最优REQUEST方法快速获得所需间隔的高精度天文姿态信息；

(3)利用GUPF算法将天文姿态信息和载体姿态信息相融合，求解高精度的载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的高精度组合定姿。

本发明的原理是：首先对惯性量测信息进行补偿陀螺输出数据后，通过姿态解算，得到载体姿态信息；其次利用天文量测信息，结合补偿后的陀螺输出数据，通过遗传最优REQUEST方法快速获得特定间隔的高精度天文姿态信息；最后利用GUPF算法将天文姿态信息和载体姿态信息相融合，解决系统非线性和噪声非高斯问题，求解高精度载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现航天器长时间、高精度的组合定姿。

本发明与现有技术相比的优点在于：本发明克服了传统组合定姿方法在定姿精度和陀螺漂移估计精度低的不足，将遗传最优REQUEST算法引入到组合定姿方法中，解决了滤波器次优的问题，提高了组合定姿的精度和速度；同时利用GUPF有效解决了系统非线性和噪声非高斯的问题，将惯性量测信息和天文量测信息相融合，进一步提高了组合定姿的精度，实现了对陀螺漂移的精确估计，满足了航天器长时间、高精度组合定姿的要求。

附图说明

图1为本发明的一种基于遗传最优REQUEST和GUPF的组合定姿方法原理图。

具体实施方式

如图1所示，本发明的具体实施步骤如下：

1、首先对惯性量测信息进行补偿陀螺输出数据后，通过姿态解算，得到载体姿态信息，流程如下：

a.设定初始姿态为

，计算得出初始姿态四元数q(0)阵：

b.更新矩阵为：

$q(n+1)=\{\cos \frac{\mathrm{\Δ\φ}}{2}I+\frac{\sin \frac{\mathrm{\Δ\φ}}{2}}{\mathrm{\Δ\φ}}\left[\mathrm{\Δ\Φ}\right]\}q\left(n\right)$

n为第n时刻，I为单位四元数，Δφ＝[Δφ_X  Δφ_Y  Δφ_Z]为陀螺输出角增量，[ΔΦ]为：

$\left[\mathrm{\Δ\Φ}\right]=\left[\begin{array}{cccc}0& {-\mathrm{\Δ\φ}}_X& {-\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_Z\\ {\mathrm{\Δ\φ}}_X& 0& {\mathrm{\Δ\φ}}_Z& {-\mathrm{\Δ\φ}}_Y\\ {\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_Z& 0& {\mathrm{\Δ\φ}}_X\\ {\mathrm{\Δ\φ}}_Z& {\mathrm{\Δ\φ}}_Y& {-\mathrm{\Δ\φ}}_X& 0\end{array}\right];$

c.由q(n+1)＝[q₁  q₂  q₃  q₄]^T，计算姿态余弦阵C为：

$C=\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[\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];$

d.由方向余弦阵C求解载体的实时姿态信息：

航向、俯仰和横滚等三姿态角(

，θ，γ)的解算公式如下：

俯仰角θ值为：θ＝sin^-1(C₂₃)；

航向角

值的计算如下表所示：

横滚角γ值的计算如下表所示：

 

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、利用遗传最优REQUEST方法，将所需间隔的天文量测信息，结合补偿后的陀螺输出数据，快速求解高精度天文姿态信息的步骤为：

A.定义3×3的矩阵B_k和S_k，3×1的列向量z和标量σ_k，k＝1...n...， $B_k=w_k{v}_k^T,$ $S_k=B_k+B_k^T,$ z＝w_k×v_k，σ_k＝tr(B_k)；其中，w_k＝[w_k，x  w_k，y  w_k，z]^T为k时刻观测的星光在星敏感器坐标系中的坐标矢量，v_k＝[v_k，x  v_k，y  v_k，z]^T为k时刻该星光在地心惯性坐标系中的参考矢量，

$B_k=\left[\begin{array}{ccc}{w}_{k,x}{v}_{k,x}& w_{k,x}{v}_{k,y}& w_{k,x}{v}_{k,z}\\ w_{k,y}{v}_{k,x}& w_{k,y}{v}_{k,y}& w_{k,y}{v}_{k,z}\\ w_{k,z}{v}_{k,x}& w_{k,z}{v}_{k,y}& w_{k,z}{v}_{k,z}\end{array}\right],$ $z=\left[\begin{array}{c}{-w}_{k,z}{v}_{k,y}+w_{k,y}{v}_{k,z}\\ w_{k,z}{v}_{k,x}-w_{k,x}{v}_{k,z}\\ {-w}_{k,y}{v}_{k,x}+w_{k,x}{v}_{k,y}\end{array}\right],$

$S_k=\left[\begin{array}{ccc}{2w}_{k,x}{v}_{k,x}& w_{k,x}{v}_{k,y}+w_{k,y}{v}_{k,x}& w_{k,x}{v}_{k,z}+w_{k,z}{v}_{k,x}\\ w_{k,y}{v}_{k,x}+w_{k,x}{v}_{k,y}& {2w}_{k,y}{v}_{k,y}& w_{k,y}{v}_{k,z}+w_{k,z}{v}_{k,y}\\ w_{k,z}{v}_{k,x}+w_{k,x}{v}_{k,z}& w_{k,z}{v}_{k,y}+w_{k,y}{v}_{k,z}& {2w}_{k,z}{v}_{k,z}\end{array}\right],$

$K_{k/k}=\frac{{\mathrm{\ρ}}_k{m}_{k-1}}{m_k}{K}_{k/k-1}+\frac{1}{m_k}{\mathrm{\δK}}_k$

上式中K_k/k-1为在k-1时刻估计k时刻的K矩阵值，m_k和m_k+1是使K_k/k的最大特征值接近于1的标量权值系数，系数ρ_k是渐消记忆因子，δK_k用第一个观测向量根据下式计算：

${\mathrm{\δK}}_k=\left[\begin{array}{cc}{S}_k-{\mathrm{\σ}}_k{I}_k& z_k\\ z_k^T& {\mathrm{\σ}}_k\end{array}\right];$

B.通过遗传算法选取最优的渐消记忆因子ρ_k，遗传算法的自适应评价函数定义为：

f(ρ_k)＝min(K_k/km_k-m_k-1K_k/k-1+δK_k)；

C.时间更新：

$K_{k+1/k}={\mathrm{\Φ}}_k{K}_{k/k}{\mathrm{\Φ}}_k^T$

$P_{k+1/k}={\mathrm{\Φ}}_k{P}_{k/k}{\mathrm{\Φ}}_k^T+Q_k$

式中，Φ_k由补偿后的陀螺输出数据ω_k计算得到，代表四元数动力学差分方程中的4×4正交转移矩阵，

${\mathrm{\Φ}}_k=\left[\begin{array}{cc}-[{\mathrm{\ω}}_k\×]& {\mathrm{\ω}}_k\\ {-\mathrm{\ω}}_k^T& 0\end{array}\right],$ ω_k＝[ω_k，1 ω_k，2 ω_k，3]^T， $[{\mathrm{\ω}}_k\×]=\left[\begin{array}{ccc}0& {-\mathrm{\ω}}_{k,3}& {\mathrm{\ω}}_{k,2}\\ {\mathrm{\ω}}_{k,3}& 0& {-\mathrm{\ω}}_{k,1}\\ {-\mathrm{\ω}}_{k,2}& {\mathrm{\ω}}_{k,1}& 0\end{array}\right];$

D.量测更新：

${\mathrm{\ρ}}_{k+1}=\frac{m_k^2\mathrm{tr}\left(P_{k+1/k}\right)}{m_k^2\mathrm{tr}\left(P_{k+1/k}\right)+{\mathrm{\δm}}_{k+1}^2\mathrm{tr}\left(R_{k+1}\right)}$

m_k+1＝(1-ρ_k+1)m_k+ρ_k+1δm_k+1

$K_{k+1/k+1}=(1-{\mathrm{\ρ}}_{k+1})\frac{m_k}{m_{k+1}}{K}_{k+1/k}+{\mathrm{\ρ}}_{k+1}\frac{{\mathrm{\δm}}_{k+1}}{m_{k+1}}{\mathrm{\δK}}_{k+1}$

$P_{k+1/k+1}={\left[(1-{\mathrm{\ρ}}_{k+1})\frac{m_k}{m_{k+1}}\right]}^2{P}_{k+1/k}+{\left({\mathrm{\ρ}}_{k+1}\frac{{\mathrm{\δm}}_{k+1}}{m_{k+1}}\right)}^2{R}_{k+1};$

E.求解天文姿态信息：

q_k+1＝Φ_kq_k

由q_k+1＝[q₁ q₂ q₃ q₄]^T，计算姿态余弦阵C′为：

$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];$

F.由方向余弦阵C′即可得出高精度天文姿态信息，求解方法同载体姿态信息的解算方法。

3、利用GUPF算法将天文姿态信息和载体姿态信息相融合，完成对航天器长时间、高精度的组合定姿步骤为：

(1)t＝0时，初始化：

对初始的先验概率密度p(x₀)进行采样，生成N个服从p(x₀)分布的粒子

其均值和方差满足

$P_0^{\left(i\right)}=E\left[(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)}){(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)})}^T\right];$

(2)t≥1时，步骤如下：

①采样步骤

用Unscented卡尔曼滤波更新粒子得到

，采样 ${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},y_{1:k})=N({\stackrel{\‾}{x}}_k^{\left(i\right)},p_k^{\left(i\right)}),$ i＝1，…，N；

②计算权重

${\tilde{w}}_k^{\left(i\right)}={\tilde{w}}_{k-1}^{\left(i\right)}\frac{p\left(y_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},y_{1:k})}$

归一化权重：   ${w_k}^{\left(i\right)}={{\tilde{w}}_K}^{\left(i\right)}/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{{\tilde{w}}_k}^{\left(i\right)};$

③重采样步骤

从离散分布的 $\{x_k^{\left(i\right)},w_k\left(x_k^{\left(i\right)}\right)\}i=1,\·\·\·,N$ 中进行N次重采样，得到一组新的粒子

仍为p(x_k|y_0:k)的近似表示；

④通过遗传算法从得到的这组新的粒子中选取优等粒子，剔出低等的粒子，以解决粒子枯竭问题，其自适应评价函数定义为：

$f=(u-\min (1,\frac{p\left(y_k\right|x_k^{*\left(i\right)})}{p\left(y_k\right|x_k^{(i*)})}))$

式中，u为0~1之间的一个随机数， $x_k^{*\left(i\right)}~p\left(x_k\right|x_{k-1}^i)$ 为从马尔科夫链中进行的采样，在此取u＝0.8；

⑤输出

按照最小方差准则，载体姿态的最优估计值就是条件分布的均值，即：

${\hat{x}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i$

$p_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i(x_k^i-{\hat{x}}_k){(x_k^i-{\hat{x}}_k)}^T.$

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

Claims (4)

1、一种基于遗传最优REQUEST和GUPF的组合定姿方法，其特征在于包括以下步骤：

(1)对惯性量测信息进行补偿陀螺输出数据后，通过姿态解算，得到载体姿态信息；

(2)利用天文量测信息，结合补偿后的陀螺输出数据，通过遗传最优REQUEST方法快速获得所需间隔的天文姿态信息；

(3)利用GUPF算法将天文姿态信息和载体姿态信息相融合，求解载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的组合定姿。

2、根据权利要求1所述的一种基于遗传最优REQUEST和GUPF的组合定姿方法，其特征在于：所述步骤(1)中的实时的载体姿态信息解算如下：

a.设定初始姿态为

计算得出初始姿态四元数q(0)阵：

b.更新矩阵为：

$q(n+1)=\{\cos \frac{\mathrm{\Δ\φ}}{2}I+\frac{\sin \frac{\mathrm{\Δ\φ}}{2}}{\mathrm{\Δ\φ}}\left[\mathrm{\Δ\Φ}\right]\}q\left(n\right)$

n为第n时刻，I为单位四元数，Δφ＝[Δφ_X Δφ_Y Δφ_Z]为陀螺输出角增量，[ΔΦ]为：

$\left[\mathrm{\Δ\Φ}\right]=\left[\begin{array}{cccc}0& -{\mathrm{\Δ\φ}}_X& -{\mathrm{\Δ\φ}}_Y& -{\mathrm{\Δ\φ}}_Z\\ {\mathrm{\Δ\φ}}_X& 0& {\mathrm{\Δ\φ}}_Z& -{\mathrm{\Δ\φ}}_Y\\ {\mathrm{\Δ\φ}}_Y& -{\mathrm{\Δ\φ}}_Z& 0& {\mathrm{\Δ\φ}}_X\\ {\mathrm{\Δ\φ}}_Z& {\mathrm{\Δ\φ}}_Y& -{\mathrm{\Δ\φ}}_X& 0\end{array}\right];$

c.由q(n+1)＝[q₁ q₂ q₃ q₄]^T，计算姿态余弦阵C为：

$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];$

d.由方向余弦阵C即可得出实时载体的姿态信息。

3、根据权利要求1所述的一种基于遗传最优REQUEST和GUPF的组合定姿方法，其特征在于：所述步骤(2)中利用遗传最优REQUEST方法，将所需间隔的天文量测信息，结合补偿后的陀螺输出数据，快速求解天文姿态信息的步骤为：

a.定义3×3的矩阵B_k和S_k，3×1的列向量z和标量σ_k，k＝1...n...， $B_k=w_k{v}_k^T,$ $S_k=B_k+B_k^T,$ z＝w_k×v_k，σ_k＝tr(B_k)；其中，w_k为k时刻观测的星光在星敏感器坐标系中的坐标矢量，v_k为k时刻该星光在地心惯性坐标系中的参考矢量，

$K_{k/k}=\frac{{\mathrm{\ρ}}_k{m}_{k-1}}{m_k}{K}_{k/k-1}+\frac{1}{m_k}\mathrm{\δ}{K}_k$

上式中K_k/k-1为在k-1时刻估计k时刻的K矩阵值，m_k和m_k+1是使K_k/k的最大特征值接近于1的标量权值系数，系数ρ_k是渐消记忆因子，δK_k用第一个观测向量根据下式计算：

${\mathrm{\δK}}_k=\left[\begin{array}{cc}{S}_k-{\mathrm{\σ}}_k{I}_k& z_k\\ z_k^T& {\mathrm{\σ}}_k\end{array}\right];$

b.通过遗传算法选取最优的渐消记忆因子ρ_k，遗传算法的自适应评价函数定义为：f(ρ_k)＝min(K_k/km_k-m_k-1K_k/k-1+δK_k)；

c.时间更新：

$K_{k+1/k}={\mathrm{\Φ}}_k{K}_{k/k}{\mathrm{\Φ}}_k^T$

$P_{k+1/k}={\mathrm{\Φ}}_k{P}_{k/k}{\mathrm{\Φ}}_k^T+Q_k$

式中，Φ_k由补偿后的陀螺输出数据ω_k计算得到，代表四元数动力学差分方程中的4×4正交转移矩阵，

${\mathrm{\Φ}}_k=\left[\begin{array}{cc}-[{\mathrm{\ω}}_k\×]& {\mathrm{\ω}}_k\\ {-\mathrm{\ω}}_k^T& 0\end{array}\right],$ ω_k＝[ω_k，1ω_k，2ω_k，3]^T，

$[{\mathrm{\ω}}_k\×]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{k,3}& {\mathrm{\ω}}_{k,2}\\ {\mathrm{\ω}}_{k,3}& 0& -{\mathrm{\ω}}_{k,1}\\ -{\mathrm{\ω}}_{k,2}& {\mathrm{\ω}}_{k,1}& 0\end{array}\right];$

d.量测更新：

${\mathrm{\ρ}}_{k+1}=\frac{m_k^2\mathrm{tr}\left(P_{k+1/k}\right)}{m_k^2\mathrm{tr}\left(P_{k+1/k}\right)+\mathrm{\δ}{m}_{k+1}^2\mathrm{tr}\left(R_{k+1}\right)}$

m_k+1＝(1-ρ_k+1)m_k+ρ_k+1δm_k+1

$K_{k+1/k+1}=(1-{\mathrm{\ρ}}_{k+1})\frac{m_k}{m_{k+1}}{K}_{k+1/k}+{\mathrm{\ρ}}_{k+1}\frac{{\mathrm{\δm}}_{k+1}}{m_{k+1}}{\mathrm{\δK}}_{k+1}$

$P_{k+1/k+1}={\left[(1-{\mathrm{\ρ}}_{k+1})\frac{m_k}{m_{k+1}}\right]}^2{P}_{k+1/k}+{\left({\mathrm{\ρ}}_{k+1}\frac{{\mathrm{\δm}}_{k+1}}{m_{k+1}}\right)}^2{R}_{k+1};$

e.求解天文姿态信息：

q_k+1＝Φ_kq_k

由q_k+1＝[q₁ q₂ q₃ q₄]^T，计算姿态余弦阵C为：

$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];$

f.由方向余弦阵C即可得出天文姿态信息。

4、根据权利要求1所述的一种基于遗传最优REQUEST和GUPF的组合定姿方法，其特征在于：所述步骤(3)中利用GUPF算法将天文姿态信息和载体姿态信息相融合，完成对航天器组合定姿步骤为：

a.t＝0时，初始化：

对初始的先验概率密度p(x₀)进行采样，生成N个服从p(x₀)分布的粒子 $x_0^{\left(i\right)},i=1,\·\·\·,N,$ 其均值和方差满足

$P_0^{\left(i\right)}=E\left[(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)}){(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^i)}^T\right];$

b.t≥1时，步骤如下：

①采样步骤

用Unscented卡尔曼滤波更新粒子

得到

采样 ${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},y_{1:k})=N({\stackrel{\‾}{x}}_k^{\left(i\right)},P_k^{\left(i\right)}),$ i＝1，…，N；

②计算权重 ${\tilde{w}}_k^{\left(i\right)}={\tilde{w}}_{k-1}^{\left(i\right)}\frac{p\left(y_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},y_{1:k})},$ 归一化权重： ${w_k}^{\left(i\right)}={{\tilde{w}}_k}^{\left(i\right)}/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{{\tilde{w}}_k}^{\left(i\right)};$

③重采样步骤

从离散分布的 $\{x_k^{\left(i\right)},w_k\left(x_k^{\left(i\right)}\right)\}i=1,\·\·\·,N$ 中进行N次重采样，得到一组新的粒子

仍为p(x_k|y_0：k)的近似表示；

④通过遗传算法从得到的这组新的粒子中选取优等粒子，剔出低等的粒子，以解决粒子枯竭问题，其自适应评价函数定义为：

$f=(u-\min (1,\frac{p\left(y_k\right|x_k^{*\left(i\right)})}{p\left(y_k\right|x_k^{(i*)})})),$

式中，u为0～1之间的一个随机数， $x_k^{*\left(i\right)}~p\left(x_k\right|x_{k-1}^i)$ 为从马尔科夫链中进行的采样；

⑤输出

按照最小方差准则，载体姿态的最优估计值就是条件分布的均值，即：

${\hat{x}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i$

$p_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i(x_k^i-{\hat{x}}_k){(x_k^i-{\hat{x}}_k)}^T.$

Images