CN102156478B

CN102156478B - 一种基于蚁群Unscented粒子滤波算法的组合定姿方法

Info

Publication number: CN102156478B
Application number: CN 201010622523
Authority: CN
Inventors: 郭雷; 全伟; 房建成; 刘翠翠; 杨照华; 崔培玲
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2010-12-28
Filing date: 2010-12-28
Publication date: 2013-11-06
Anticipated expiration: 2030-12-28
Also published as: CN102156478A

Abstract

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

Description

一种基于蚁群Unscented粒子滤波算法的组合定姿方法

技术领域

本发明涉及一种惯性/天文组合定姿方法，特别是一种基于蚁群Unscented粒子滤波(无迹粒子滤波)算法的组合定姿方法，可用于各种航天器的高精度组合定姿。

背景技术

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

在惯性/天文组合定姿技术方面，以往都采用扩展卡尔曼滤波EKF(Extended KalmanFilter)方法，但是EKF仅适用于滤波误差和预测误差很小的情况。近年来提出的Unscented卡尔曼滤波UKF(Unscented Kalman Filter)是一种EKF的改进算法，有效的解决了系统的非线性问题，但其不足是不适用于噪声非高斯分布的系统。粒子滤波PF由于采用蒙特卡洛采样(Monte Carlo sampling)结构而在非线性、非高斯系统状态跟踪上体现出越来越大的优越性，但其缺点是存在退化现象，消除退化现象主要依赖于两个关键技术：适当选取重要密度函数和进行重采样。对于前者的改进方法，可使用EKPF(Extented Kalman ParticleFilter)、UPF(Unscented Particle Filter)来进行重要密度函数的选择，其中UPF算法是利用UKF来得到粒子重要性概率密度函数的一种粒子滤波方法，由于该重要密度函数中包含了最新量测信息，因此具有更好的性能。对于后者的改进方法，常用的重采样算法有累积分布重采样(Binary search)、系统重采样(Systematic resampling)、剩余重采样(Residualresampling)等，这些算法通过增加粒子的有效性解决了粒子的退化问题，但是在实际应用中会影响系统的鲁棒性，重采样完成后，重要度高的粒子通过重采样被多次选取，这在一定程度上丢失了粒子的多样性，由此造成的后果是一旦目标丢失或跟踪精度不够，系统自动收敛的可能性很小，为此，很多学者提出了遗传粒子滤波(GPF)算法，GPF算法虽然在保证粒子有效性的同时又增加了粒子的多样性，仍然存在滤波速度慢和鲁棒性差的问题。

发明内容

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

本发明的技术解决方案为：一种基于蚁群UPF组合定姿方法，其特点在于：利用惯性量测信息和天文量测信息，通过蚁群(Ant Colony Algorithm)UPF(无迹粒子滤波)方法，实现航天器长时间、高精度的快速组合定姿，其实现步骤如下：

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

(2)利用天文量测信息，通过确定性算法，获得所需的天文姿态信息；

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

利用蚁群UPF算法进行信息融合的步骤为：

(3.1)采样时间t＝0时，初始化：

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

i＝1，…，N，生成的粒子

的均值和方差满足：

{\overset{&OverBar;}{x}}_{0}^{(i)} = E [x_{0}^{(i)}],

P_{0}^{(i)} = E [(x_{0}^{(i)} - {\overset{&OverBar;}{x}}_{0}^{(i)}) {(x_{0}^{(i)} - {\overset{&OverBar;}{x}}_{0}^{(i)})}^{T}],

其中，

为

的均值，

为

的方差，E[·]为求取[]内元素的期望，将p(x₀)分布取为均值为

方差为P₀正态分布；

(3.2)采样时间t≥1时，步骤如下：

①采样

利用(3.1)中生成的服从p(x₀)分布的粒子

进行下一时刻的采样，用Unscented卡尔曼滤波对粒子

进行估计，得到采样

得到更新的粒子

i＝1，…，N，

其中，

和

分别为k-1时刻状态对应的第i个粒子和粒子的误差方差阵，和P_{k_UKF}分别为根据k-1时刻的粒子估计的第k时刻状态估计值和估计误差方差阵，x_0：k-1为第0～k-1时刻的状态估计值，y_1：k为第1～k时刻的状态观测值，q(x_k|x_0：k-1，y_1：k)为重要性概率密度，此处选为

为均值为

方差为P_{k_UKF}的正态分布；

②利用①中UKF更新的粒子计算粒子

的权重

归一化权重：

其中，

为k时刻第i个粒子的权值，

为归一化后的权重，

为所有粒子的权值的和，

为对应于观测模型的系统状态的观测似然概率密度，为对应于系统的模型的系统状态转移概率密度，

为重要性概率密度；

③利用①中得出的粒子和②中得出的粒子的权重使用蚁群算法进行重采样，选取优等粒子(权值较大的粒子)，剔出低等(权值较小的粒子)的粒子，以解决粒子枯竭问题，利用蚁群算法进行优化的步骤如下：

首先引入如下记号：

m——蚁群中蚂蚁的数量；

d_ij——两城市i和j之间的距离；

η_ij(t)——边(i，j)的能见度，反映由城市i转移到城市j的启发程度，这个量在蚂蚁系统的运行中不改变；

τ_ij(t)——t时刻边(i，j)上的信息素轨迹强度；

Δτ_ij——蚂蚁k在边(i，j)上的留下的单位长度轨迹信息素量；

——蚂蚁k的转移概率，j为未访问的城市。

每只蚂蚁都是具有如下特征的简单主体：

I从城市i到城市j的运动过程中或是在完成一次循环后，蚂蚁在边(i，j)上释放的一种物质，称为信息素轨迹；

II蚂蚁概率的选择下一个将要访问的城市，这个概率是两城市间距离和连接两城市的路径上存有轨迹量的函数；

III为了满足问题的约束条件，在完成一次循环之前，不允许蚂蚁选择已经访问过的城市。

a.初始化

令时间t＝0，迭代次数Nc＝0，信息素τ_ij(0)＝C，C为正常数，根据具体应用进行设置，此处随意设置为C＝1，τ_ij(0)为t＝0时边(i，j)上的信息素轨迹强度，(i，j)为某时刻蚂蚁所处的位置；

b.对N个粒子的权值进行一次排序选择权值最大的点作为起点，将m只蚂蚁置于起点，各只蚂蚁，按照下列转移概率公式，采用赌轮选择方式移动，

p_{ij}^{k} = \{\begin{matrix} \frac{τ_{ij}^{α} (t) η_{ij}^{β} (t)}{\underset{s &Element; allowe d_{k}}{Σ} τ_{is}^{α} (t) η_{is}^{β} (t)} & j &Element; allowe d_{k} \\ 0 & otherwise \end{matrix}

其中，allowed_k表示蚂蚁k下一步允许走过的路径点的集合；α为启发式因子，β为期望启发式因子，分别反映了蚂蚁在运动过程中所积累的信息和启发信息在蚂蚁选择路径中的相对重要性，可设置α＝1，β＝2，城市i转移到城市j的能见度η_ij(t)＝1/d_ij(t)，令d_ij(t)为第i个粒子和第j个粒子的权重的差值；

c.按照各只蚂蚁的目标函数值F_k，并记录该次循环的最优解；选择下一个粒子(下一个目标城市)的权值

作为目标函数值F_k；

d.按照以下公式修正信息素强度：

τ_ij(t+n)＝ρτ_ij(t)+(1-ρ)Δτ_ij，

式中，参数ρ(0≤ρ≤1)为信息素残留因子，1-ρ表示信息素衰减度；表示第k只蚂蚁在本次循环中留在节点(i，j)上的信息量；常数Q是信息素强度，取Q＝100；

e.令t＝t+n，Nc＝Nc+1，经过n个时刻，完成一次循环时间t加n，循环次数Nc加1；

f.若N＜N_Cmax，则转步骤b，否则转步骤f，其中N_Cmax为循环次数；

g.输出最优解。

蚁群算法优化后的粒子为

④输出

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

{\hat{x}}_{k} = Σ_{i = 1}^{N} w_{k}^{(i^{*})} x_{k}^{(i^{*})}

p_{k} = Σ_{i = 1}^{N} w_{k}^{(i^{*})} (x_{k}^{(i^{*})} - {\hat{x}}_{k}) {(x_{k}^{(i^{*})} - {\hat{x}}_{k})}^{T},

其中，为k时刻载体姿态的最优估计，

为蚁群算法优化后的k时刻第i个粒子的权值，

为蚁群算法优化后的第k时刻第i个粒子的值，

为蚁群算法优化后的第k时刻粒子的估计值，

为从i＝1到N求和，p_k为蚁群算法优化后的第k时刻载体姿态的方差。

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

本发明与现有技术相比的优点在于：本发明克服了传统组合定姿方法在定姿精度和陀螺漂移估计精度低的不足，利用UPF有效解决了系统非线性和噪声非高斯的问题，利用蚁群算法在路径寻优方面的优势对Unscented粒子滤波的粒子进行优化，有效的解决了粒子滤波的粒子退化和粒子匮乏问题，实现了优等粒子选择的快速性和有效性，提高了组合定姿的速度和精度；将惯性量测信息和天文量测信息相融合，进一步提高了组合定姿的精度，实现了对陀螺漂移的精确估计，满足了航天器长时间、高精度组合定姿的要求。

附图说明

图1为本发明的一种基于蚁群UPF的组合定姿方法原理图；

图2为本发明中用蚁群算法优化粒子的流程图。

具体实施方式

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

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

a.设定初始姿态为

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

其中，

θ₀，γ₀分别为俯仰角、横滚角和偏航角，q(0)为0时刻的姿态四元数，cos[·]，sin[·]分别为求余弦和正弦；

b.由a中给出的初始姿态四元数阵q(0)推导出更新矩阵为：

q (n + 1) = {\cos \frac{Δφ}{2} I + \frac{\sin \frac{Δφ}{2}}{Δφ} [Δφ]} q (n),

n为第n时刻，I为单位四元数，Δφ＝[Δφ_X Δφ_Y Δφ_Z]为安装在在X，Y，Z三个轴上的陀螺输出角增量，定义[ΔΦ]为：

[ΔΦ] = [\begin{matrix} 0 & - {Δφ}_{X} & {- Δφ}_{Y} & - {Δφ}_{Z} \\ {Δφ}_{X} & 0 & {Δφ}_{Z} & - Δ φ_{Y} \\ {Δφ}_{Y} & - Δ φ_{Z} & 0 & {Δφ}_{X} \\ {Δ φ}_{Z} & {Δφ}_{Y} & {- Δφ}_{X} & 0 \end{matrix}];

c·由b中得出的姿态四元数更新矩阵q(n+1)＝[q_1，n+1 q_2，n+1 q_3，n+1 q_4，n+1]^T，计算姿态余弦阵C为：

C = [\begin{matrix} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{matrix}] = [\begin{matrix} 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{matrix}];

其中，q(n+1)为k+1时刻的姿态四元数，C₁₁～C₃₃对应公式最右边矩阵中的元素，q(n+1)＝C·q(n)；

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

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

的解算公式如下：

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

航向角

值的计算如下表所示：

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

C₁₃值判断	C₃₃值判断	横滚角γ值
			＝0	＜0	-π
＞0	＜0	atan^-1(-C₁₃/C₃₃)-π
			＞0	＝0	-π/2
任意值	＞0	atan^-1(-C₁₃/C₃₃)
			＜0	＝0	π/2
＜0	＜0	atan^-1(-C₁₃/C₃₃)+π

2、利用天文量测信息，通过确定性算法，求解天文姿态信息的步骤为：

A.定义3×3的矩阵w，v，B和S，3×1的列向量z，a和标量σ，4×1的列向量q；

其中，w＝[w₁ w₂ w₃]为k时刻观测的三颗星的星光在星敏感器坐标系中的坐标矢量，v＝[v₁ v₂ v₃]为k时刻该三颗星的星光在地心惯性坐标系中的参考矢量，

S＝B+B^T，a＝[a₁ a₂ a₃]^T为非负的加权系数，σ＝tr(B)为矩阵B的秩，q＝[q₁ q₂ q₃ q₄]^T为待求解的姿态四元数，

w = [\begin{matrix} w_{1} & w_{2} & w_{3} \end{matrix}] = [\begin{matrix} w_{1 x} & w_{2 x} & w_{3 x} \\ w_{1 y} & w_{2 y} & w_{3 y} \\ w_{1 z} & w_{2 z} & w_{3 z} \end{matrix}],

v = [\begin{matrix} v_{1} & v_{2} & v_{3} \end{matrix}] = [\begin{matrix} v_{1 x} & v_{2 x} & v_{3 x} \\ v_{1 y} & v_{2 y} & v_{3 y} \\ v_{1 z} & v_{2 z} & v_{3 z} \end{matrix}],

w = [\begin{matrix} w_{1} & w_{2} & w_{3} \end{matrix}] = [\begin{matrix} w_{1 x} & w_{2 x} & w_{3 x} \\ w_{1 y} & w_{2 y} & w_{3 y} \\ w_{1 z} & w_{2 z} & w_{3 z} \end{matrix}],

v = [\begin{matrix} v_{1} & v_{2} & v_{3} \end{matrix}] = [\begin{matrix} v_{1 x} & v_{2 x} & v_{3 x} \\ v_{1 y} & v_{2 y} & v_{3 y} \\ v_{1 z} & v_{2 z} & v_{3 z} \end{matrix}],

B = [\begin{matrix} a_{1} w_{1 x} v_{1 x} + a_{2} w_{2 x} v_{2 x} + a_{3} w_{3 x} v_{3 x} & a_{1} w_{1 x} v_{1 y} + a_{2} w_{2 x} v_{2 y} + a_{3} w_{3 x} v_{3 y} & a_{1} w_{1 x} v_{1 z} + a_{2} w_{2 x} v_{2 z} + a_{3} w_{3 x} v_{3 z} \\ a_{1} w_{1 y} v_{1 x} + a_{2} w_{2 y} v_{2 x} + a_{3} w_{3 y} v_{3 x} & a_{1} w_{1 y} v_{1 y} + a_{2} w_{2 y} v_{2 y} + a_{3} w_{3 y} v_{3 y} & a_{1} w_{1 y} v_{1 z} + a_{2} w_{2 y} v_{2 z} + a_{3} w_{3 y} v_{3 z} \\ a_{1} w_{1 z} v_{1 x} + a_{2} w_{2 z} v_{2 x} + a_{3} w_{3 z} v_{3 x} & a_{1} w_{1 z} v_{1 y} + a_{2} w_{2 z} v_{2 y} + a_{3} w_{3 z} v_{3 y} & a_{1} w_{1 z} v_{1 z} + a_{2} w_{2 z} v_{2 z} + a_{3} w_{3 z} v_{3 z} \end{matrix}]

S = 2 \times [\begin{matrix} a_{1} w_{1 x} v_{1 x} + a_{2} w_{2 x} v_{2 x} + a_{3} w_{3 x} v_{3 x} & a_{1} w_{1 x} v_{1 y} + a_{2} w_{2 x} v_{2 y} + a_{3} w_{3 x} v_{3 y} & a_{1} w_{1 x} v_{1 z} + a_{2} w_{2 x} v_{2 z} + a_{3} w_{3 x} v_{3 z} \\ a_{1} w_{1 y} v_{1 x} + a_{2} w_{2 y} v_{2 x} + a_{3} w_{3 y} v_{3 x} & a_{1} w_{1 y} v_{1 y} + a_{2} w_{2 y} v_{2 y} + a_{3} w_{3 y} v_{3 y} & a_{1} w_{1 y} v_{1 z} + a_{2} w_{2 y} v_{2 z} + a_{3} w_{3 y} v_{3 z} \\ a_{1} w_{1 z} v_{1 x} + a_{2} w_{2 z} v_{2 x} + a_{3} w_{3 z} v_{3 x} & a_{1} w_{1 z} v_{1 y} + a_{2} w_{2 z} v_{2 y} + a_{3} w_{3 z} v_{3 y} & a_{1} w_{1 z} v_{1 z} + a_{2} w_{2 z} v_{2 z} + a_{3} w_{3 z} v_{3 z} \end{matrix}]

z = Σ_{i = 1}^{3} a_{i} (w_{i} \times v_{i}) = {[\begin{matrix} B_{23} - B_{32} & B_{31} - B_{13} & B_{12} - B_{21} \end{matrix}]}^{T}

定义姿态矩阵K阵如下：

K = [\begin{matrix} S - σI & z \\ z^{T} & σ \end{matrix}],

I为单位阵，

姿态矩阵K阵的最大特征值所对应的特征矢量是最小均方差意义下的最优估计，即Kq＝λ_maxq，q为求解所得姿态四元数，λ_max为最大特征值；

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

C^{'} = [\begin{matrix} 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{matrix}];

C.由姿态余弦阵C′即可求解载体的实时高精度天文姿态信息，步骤如下：

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

的解算公式如下：

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

航向角

值的计算如下表所示：

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

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

①采样

利用(3.1)中生成的服从p(x₀)分布的粒子进行下一时刻的采样，用Unscented卡尔曼滤波对粒子

进行估计，得到

采样

得到更新的粒子

i＝1，…，N，

其中，

别为k-1时刻状态对应的第i个粒子，

为k-1时刻的粒子的误差方差阵，

和P_{k_UKF}分别为根据k-1时刻的粒子估计的第k时刻状态估计值和估计误差方差阵，x_0：k-1为第0～k-1时刻的状态估计值，y_1：k为第1～k时刻的状态观测值，q(x_k|x_0：k-1，y_1：k)为重要性概率密度，此处选为

为均值为

方差为P_{k_UKF}的正态分布；

②利用①中UKF更新的粒子

计算粒子的权重

归一化权重：

其中，

为k时刻第i个粒子的权值，为归一化后的权重，

为所有粒子的权值的和，

为对应于观测模型的系统状态的观测似然概率密度，

为对应于系统的模型的系统状态转移概率密度，

为重要性概率密度，；

首先引入如下记号：

m——蚁群中蚂蚁的数量；

d_ij——两城市i和j之间的距离；

τ_ij(t)——t时刻边(i，j)上的信息素轨迹强度；

——蚂蚁k的转移概率，j为未访问的城市。

每只蚂蚁都是具有如下特征的简单主体：

a.初始化

p_{ij}^{k} = \{\begin{matrix} \frac{τ_{ij}^{α} (t) η_{ij}^{β} (t)}{\underset{s &Element; {allowed}_{k}}{Σ} τ_{is}^{α} (t) η_{is}^{β} (t)} & j &Element; {allowed}_{k} \\ 0 & otherwise \end{matrix}

作为目标函数值F_k；

d.按照以下公式修正信息素强度：

τ_ij(t+n)＝ρτ_ij(t)+(1-ρ)Δτ_ij，

式中，参数ρ(0≤ρ≤1)为信息素残留因子，1-ρ表示信息素衰减度；

表示第k只蚂蚁在本次循环中留在节点(i，j)上的信息量；常数Q是信息素强度，取Q＝100；

g.输出最优解。

蚁群算法优化后的粒子为

④输出

{\hat{x}}_{k} = Σ_{i = 1}^{N} w_{k}^{(i^{*})} x_{k}^{(i^{*})}

p_{k} = Σ_{i = 1}^{N} w_{k}^{(i^{*})} (x_{k}^{(i^{*})} - {\hat{x}}_{k}) {(x_{k}^{(i^{*})} - {\hat{x}}_{k})}^{T} .

(3)利用蚁群Unscented粒子滤波(Unscented Particle Filter)算法将天文姿态信息和载体姿态信息相融合，解决系统非线性和噪声非高斯问题，求解高精度的载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的长时间、高精度组合定姿；

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

Claims

1.一种基于蚁群Unscented粒子滤波算法的组合定姿方法，其实现步骤如下：

（1）利用惯性量测信息补偿陀螺输出数据，通过姿态解算，得到载体姿态信息；

（2）利用天文量测信息，通过确定性算法，获得所需天文姿态信息；

（3）利用蚁群Unscented粒子滤波（Unscented Particle Filter）算法将天文姿态信息和载体姿态信息相融合，求解高精度的载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的长时间、高精度组合定姿；

所述步骤（3）中利用蚁群UPF算法步骤为：

（3.1）采样时间t＝0时，初始化：