[发明专利]一种基于蚁群Unscented粒子滤波算法的组合定姿方法

摘要

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

主权项

1.一种基于蚁群Unscented粒子滤波算法的组合定姿方法，其实现步骤如下：(1)利用惯性量测信息补偿陀螺输出数据，通过姿态解算，得到载体姿态信息；(2)利用天文量测信息，通过确定性算法，获得所需天文姿态信息；(3)利用蚁群Unscented粒子滤波(Unscented Particle Filter)算法将天文姿态信息和载体姿态信息相融合，求解高精度的载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移补偿；最终实现基于天文量测信息实时消除惯性/天文组合导航系统陀螺随机误差的在线修正，完成对航天器的长时间、高精度组合定姿；所述步骤(3)中利用蚁群UPF算法步骤为：(3.1)采样时间t＝0时，初始化：对初始的先验概率密度p(x₀)进行采样，生成N个服从p(x₀)分布的粒子i＝1，…，N，生成的粒子的均值和方差满足：x‾0=E[x0(i)],]]>P0=E[(x0(i)-x‾0(i))(x0(i)-x‾0(i))T],]]>其中，为的均值，P₀为的方差，E[·]为求取[]内元素的期望，将p(x₀)分布取为均值为方差为P₀正态分布；(3.2)采样时间t≥1时，步骤如下：①采样利用(3.1)中生成的服从p(x₀)分布的粒子进行下一时刻的采样，用Unscented卡尔曼滤波对粒子进行估计，得到采样得到更新的粒子i＝1，…，N，其中，别为k-1时刻状态对应的第i个粒子，为k-1时刻的粒子的误差方差阵，和P_{k_UKF}分别为利用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个粒子的权重，为归一化后的权重，为所有粒子的权重的和，为对应于观测模型的系统状态的观测似然概率密度，对应于系统的模型的系统状态转移概率密度，为重要性概率密度，初始时刻粒子的权值i＝1，…，N；③利用①中得出的粒子和②中得出的粒子的权重使用蚁群算法进行重采样，选取优等粒子(权值较大的粒子)，剔出低等(权值较小的粒子)的粒子，以解决粒子枯竭问题，蚁群算法优化后的粒子为④输出按照最小方差准则，载体姿态的最优估计值就是条件分布的均值，即：x^k=Σi=1Nwk(i*)xk(i*)]]>pk=Σi=1Nwk(i*)(xk(i*)-x^k)(xk(i*)-x^k)T,]]>其中，为k时刻载体姿态的最优估计，为蚁群算法优化后的k时刻第i个粒子的权值，为蚁群算法优化后的第k时刻第i个粒子的值，为蚁群算法优化后的第k时刻粒子的估计值，为从i＝1到N求和，p_k为蚁群算法优化后的第k时刻载体姿态的方差。