专利名称：一种基于蚁群Unscented粒子滤波算法的组合定姿方法
技术领域：
本发明涉及一种惯性/天文组合定姿方法，特别是一种基于蚁群Unscented粒子滤波 (无迹粒子滤波)算法的组合定姿方法，可用于各种航天器的高精度组合定姿。
背景技术：
为满足天基对地观测、武器精确打击以及空间探索开发的迫切需求，各类地球卫星、深空探测器、载人飞船、弹道导弹和运载火箭等航天器必须具备自主运行和管理能力， 而高精度的自主定姿是航天器自主运行和管理的核心技术瓶颈。目前，航天器的高精度自主定姿，无法依靠任何一种导航手段独立实现。纯惯性导航系统能够自主、实时提供连续、 全面的导航信息，短时精度高，但其误差随工作时间积累，难以满足航天器的长时间高精度定姿要求；天文导航能够提供高精度姿态信息，误差不随时间积累，但易受气候条件限制， 且输出信息不连续；将这两者相结合、优势互补，构成惯性/天文组合定姿系统，是实现航天器长时间、高精度定姿的最为有效的手段。在惯性/天文组合定姿技术方面，以往都采用扩展卡尔曼滤波EKF(Extended Kalman Filter)方法，但是EKF仅适用于滤波误差和预测误差很小的情况。近年来提出的 Unscented卡尔曼滤波UKF(Unscented Kalman Filter)是一种EKF的改进算法，有效的解决了系统的非线性问题，但其不足是不适用于噪声非高斯分布的系统。粒子滤波PF由于采用蒙特卡洛采样(Monte Carlo sampling)结构而在非线性、非高斯系统状态跟踪上体现出越来越大的优越性，但其缺点是存在退化现象，消除退化现象主要依赖于两个关键技术适当选取重要密度函数和进行重采样。对于前者的改进方法，可使用EKPF(Extented Kalman Particle Filter)、UPF(Unscented Particle Filter)来进行重要密度函数的选择，其中 UPF算法是利用UKF来得到粒子重要性概率密度函数的一种粒子滤波方法，由于该重要密度函数中包含了最新量测信息，因此具有更好的性能。对于后者的改进方法，常用的重采样算法有累积分布重采样(Binary search)、系统重采样(Systematic resampling)、剩余重采样(Residual resampling)等，这些算法通过增加粒子的有效性解决了粒子的退化问题， 但是在实际应用中会影响系统的鲁棒性，重采样完成后，重要度高的粒子通过重采样被多次选取，这在一定程度上丢失了粒子的多样性，由此造成的后果是一旦目标丢失或跟踪精度不够，系统自动收敛的可能性很小，为此，很多学者提出了遗传粒子滤波(GPF)算法，GPF 算法虽然在保证粒子有效性的同时又增加了粒子的多样性，仍然存在滤波速度慢和鲁棒性差的问题。
发明内容
本发明的技术解决问题是克服现有技术的不足，提出一种基于蚁群UPF的组合定姿方法，解决系统非线性和噪声非高斯问题，以快速获得高精度的姿态信息，并能够准确地估计陀螺漂移，实现各种类型航天器长时间、高精度的组合定姿。本发明的技术解决方案为一种基于蚁群UPF组合定姿方法，其特点在于利用惯性量测信息和天文量测信息，通过蚁群(Ant Colony Algorithm)UPF(无迹粒子滤波)方法，实现航天器长时间、高精度的快速组合定姿，其实现步骤如下(1)利用惯性量测信息补偿陀螺输出数据，通过姿态解算，得到载体姿态信息；(2)利用天文量测信息，通过确定性算法，获得所需的天文姿态信息；
(3)禾l」ML (Ant Colony Algorithm)Unscented|4^iit^ (Unscented Particle Filter)算法将天文姿态信息和载体姿态信息相融合，求解高精度的载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的高精度组合定姿；利用蚁群UPF算法进行信息融合的步骤为(3. 1)采样时间t = 0时，初始化对初始的先验概率密度ρ (Xtl)进行采样，生成N个服从ρ (Xtl)分布的粒子= 1，…，N，生成的粒子xf的均值和方差满足J。(')=五[4')]，々')=五[(4')-野))(4')-对〒]’其中，苟K的均值，W为<的方差，Ε[·]为求取口内元素的期望，将ρ (Xtl)分布取为均值为4，方差为Ptl正态分布；(3. 2)采样时间t彡1时，步骤如下①采样利用(3.1)中生成的服从P(Xtl)分布的粒子<进行下一时刻的采样，用 Unscented卡尔曼滤波对粒子IKDj进行估计，得到丨，采样#
^k |、卜1’ )=N^k_UKF'Pk_UKF^ ’得到更新的粒子{4')，f)} 4-1 = PkJJKF ’ i = 1，…，N，其中，^和^f1分别为k-Ι时刻状态对应的第i个粒子和粒子的误差方差阵， 和Piukf分别为根据k-Ι时刻的粒子估计的第k时刻状态估计值和估计误差方差阵，
X0 :k-!为第0 k-Ι时刻的状态估计值，y1:k为第1 k时刻的状态观测值，q(xkI Xtl Md1J 为重要性概率密度，此处选为—M^I-MW^为均值为、，方差为 PkMF的正态分布；②利用①中UKF更新的粒子Rlifl，计算粒子丨j^^fl的权重 < =-‘——,归一化权重(,_) .(0 , N
N 广、其中，为k时刻第i个粒子的权值，w 为归一化后的权重，g为所有粒子的
vvkk/=I
权值的和，为对应于观测模型的系统状态的观测似然概率密度，树χΓ|4'」,)为对应
于系统的模型的系统状态转移概率密度，拟I^tl,m)为重要性概率密度；③利用①中得出的粒子和②中得出的粒子的权重使用蚁群算法进行重采样，选取优等粒子(权值较大的粒子)，剔出低等(权值较小的粒子)的粒子，以解决粒子枯竭问题，利用蚁群算法进行优化的步骤如下首先引入如下记号
m——蚁群中蚂蚁的数量；
Clij——两城市i和j之间的距离；n.ja)——边(i，j)的能见度，反映由城市i转移到城市j的启发程度，这个量在蚂蚁系统的运行中不改变；τ u.a) — t时刻边(i，j)上的信息素轨迹强度；Δ τ u—蚂蚁k在边(i，j)上的留下的单位长度轨迹信息素量；p\——蚂蚁k的转移概率，j为未访问的城市。每只蚂蚁都是具有如下特征的简单主体I从城市i到城市j的运动过程中或是在完成一次循环后，蚂蚁在边(i，j)上释放的一种物质，称为信息素轨迹；II蚂蚁概率的选择下一个将要访问的城市，这个概率是两城市间距离和连接两城市的路径上存有轨迹量的函数；III为了满足问题的约束条件，在完成一次循环之前，不允许蚂蚁选择已经访问过的城市。a.初始化令时间t = 0，迭代次数Nc = 0，信息素τ u (0) = C，C为正常数，根据具体应用进行设置，此处随意设置为C = 1，Tij(O) ^Jt = O时边(i，j)上的信息素轨迹强度，(i，j) 为某时刻蚂蚁所处的位置；b.对N个粒子的权值进行一次排序选择权值最大的点作为起点，将m只蚂蚁置于起点，各只蚂蚁，按照下列转移概率公式，采用赌轮选择方式移动，
权利要求
1. 一种基于蚁群Unscented粒子滤波算法的组合定姿方法，其实现步骤如下(1)利用惯性量测信息补偿陀螺输出数据，通过姿态解算，得到载体姿态信息；(2)利用天文量测信息，通过确定性算法，获得所需天文姿态信息；(3)利用蚁群Unscented粒子滤波(UnscentedParticle Filter)算法将天文姿态信息和载体姿态信息相融合，求解高精度的载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的长时间、高精度组合定姿；所述步骤(3)中利用蚁群UPF算法步骤为 (3. 1)采样时间t = 0时，初始化对初始的先验概率密度P OO进行采样，生成N个服从ρ ( )分布的粒子<，i = 1，···， N，生成的粒子xf的均值和方差满足
2.根据要求1所述的基于蚁群Unscented粒子滤波算法的组合定姿方法，其特征在于 所述利用蚁群算法实现步骤如下 首先引入如下符号 m——蚁群中蚂蚁的数量； Clij——两城市i和j之间的距离；η^-ω—边(i，j)的能见度，反映由城市i转移到城市j的启发程度，这个量在蚂蚁系统的运行中不改变；τ ^t)——t时刻边(i，j)上的信息素轨迹强度； Δ τ u—蚂蚁k在边(i，j)上的留下的单位长度轨迹信息素量； Py——蚂蚁k的转移概率，j为未访问的城市； 每只蚂蚁都是具有如下特征的简单主体I从城市i到城市j的运动过程中或是在完成一次循环后，蚂蚁在边(i，j)上释放的一种物质，称为信息素轨迹；II蚂蚁概率的选择下一个将要访问的城市，这个概率是两城市间距离和连接两城市的路径上存有轨迹量的函数；III为了满足问题的约束条件，在完成一次循环之前，不允许蚂蚁选择已经访问过的城市；具体实现过程为a.初始化令时间t = 0，迭代次数Nc = 0，信息素τ。_(0) = C，C为正常数，根据具体应用进行设置，此处随意设置为C = 1，Tij(O) ^t = O时边(i，j)上的信息素轨迹强度，(i，j)为某时刻蚂蚁所处的位置；b.对N个粒子的权值进行一次排序选择权值最大的点作为起点，将m只蚂蚁置于起点， 各只蚂蚁，按照下列转移概率公式，采用赌轮选择方式移动，
3.根据要求1所述的基于蚁群Unscented粒子滤波算法的组合定姿方法，其特征在于所述确定性算法实现如下A.定义3X3的矩阵w，v，B和S，3X1的列向量z，a，标量σ，4X1的列向量q; 其中，w= [Wi W2 w3]为k时刻观测的三颗星的星光在星敏感器坐标系中的坐标矢量，ν = [V1 V2 V3]为k时刻该三颗星的星光在地心惯性坐标系中的参考矢量
全文摘要
一种基于蚁群Unscented粒子滤波算法的组合定姿方法，本发明涉及一种惯性/天文组合定姿方法。该方法首先利用惯性量测信息进行补偿陀螺输出数据，通过姿态解算，得到载体姿态信息；然后利用天文量测信息，通过确定性算法，获得所需天文姿态信息；最后利用蚁群Unscented粒子滤波(Unscented Particle Filter)算法将天文姿态信息和载体姿态信息相融合，解决系统非线性和噪声非高斯问题，求解高精度载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的长时间、高精度组合定姿。
文档编号G05D1/08GK102156478SQ20101062252
公开日2011年8月17日 申请日期2010年12月28日 优先权日2010年12月28日
发明者全伟, 刘翠翠, 崔培玲, 房建成, 杨照华, 郭雷 申请人:北京航空航天大学