一种基于蚁群pf算法的原子自旋陀螺仪和磁强计紧组合定姿方法
【专利摘要】一种基于蚁群PF算法的原子自旋陀螺仪／磁强计紧组合定姿方法，本发明涉及一种SERF原子自旋陀螺仪／磁强计组合定姿方法。该方法首先利用原子自旋陀螺仪获取惯性角速度信息，然后进行陀螺误差补偿，通过姿态解算获取载体的姿态信息；其次利用原子磁强计获得地磁量测信息，并对其进行地磁匹配，获取地磁矢量信息；最后利用蚁群(Ant?Colony)粒子滤波(Particle?Filter)算法采用紧组合方式将地磁矢量信息和载体姿态信息相融合，解决系统非线性和噪声非高斯问题，求解高精度载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移；最终实现基于SERF原子自旋陀螺仪／磁强计紧组合定姿系统陀螺随机误差的在线修正，完成对运动载体的长时间、高精度组合定姿。
【专利说明】一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法
【技术领域】
[0001]本发明涉及一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，可实现全地域、全天时、全天候、长时间、高精度的自主定姿，且自主性强、隐蔽性好、成本低，可广泛用于水下探测器、飞机、导弹、航天器等多个领域。
【背景技术】
[0002]为满足天基对地观测、武器精确打击以及空间、水下探索开发的迫切需求，各类航天器、水下探测器必须具备自主运行和管理能力，而高精度的自主定姿是航天器自主运行和管理的核心技术瓶颈。由于在无白旋交换弛豫(SERF)状态下利用原子自旋效应可同时实现超高精度惯性与磁场测量，使得SERF原子陀螺仪/磁强计组合定姿成为实现长时间高精度自主定姿的重要发展方向之一。陀螺仪/磁强计组合定姿具有能够自主、实时提供连续、全面的定姿信息，且不受气候、地域、时间等影响的特点。
[0003]目前，航天器的高精度自主定姿，无法依靠任何一种定姿手段独立实现。纯惯性定姿系统，短时精度高，但其误差随工作时间积累，难以满足航天器的长时间高精度定姿要求；地磁定姿可实时测量地磁场强度实现载体的定位，且误差不随时间积累，但受限于地磁匹配精度以及磁场测量精度等，使得目前地磁定姿精度仍然相对较低；将这两者相结合、优势互补，构成陀螺仪/磁强计组合定姿系统，是实现航天器长时间、高精度定姿的最为有效的手段。
[0004]在陀螺仪/磁强计组合定姿技术方面，以往都采用扩展卡尔曼滤波EKF (ExtendedKalman Filter)方法，但是EKF仅适用于滤波误差和预测误差很小的情况。近年来提出的Unscented卡尔曼滤波UKF(Unscented Kalman Filter)是一种EKF的改进算法,有效的解决了系统的非线性问题，但其不足是不适用于噪声非高斯分布的系统。粒子滤波(ParticleFilter, PF)由于采用蒙特卡洛采样(Monte Carlo sampling)结构而在非线性、非高斯系统状态跟踪上体现出越来越大的优越性，但其缺点是存在退化现象，消除退化现象主要依赖于两个关键技术:适当选取重要密度函数和进行重采样。对于前者的改进方法，可使用EKPF(Extented Kalman Particle Filter)、无迹粒子滤波(Unscented Particle Filter,UPF)来进行重要密度函数的选择。对于后者的改进方法，常用的重采样算法有累积分布重米样(Binary search)、系统重米样(Systematic resampling)、剩余重米样(Residualresampling)等，这些算法通过增加粒子的有效性解决了粒子的退化问题，但是在实际应用中会影响系统的鲁棒性，重采样完成后，重要度高的粒子通过重采样被多次选取，这在一定程度上丢失了粒子的多样性，由此造成的后果是一旦目标丢失或跟踪精度不够，系统自动收敛的可能性很小，为此，很多学者提出了遗传粒子滤波(GPF)算法，GPF算法虽然在保证粒子有效性的同时又增加了粒子的多样性，仍然存在滤波速度慢和鲁棒性差的问题。
【发明内容】
[0005]本发明的技术解决问题是:克服现有技术的不足，提出一种基于蚁群PF算法的原子白旋陀螺仪和磁强计紧组合定姿方法，解决系统非线性和噪声非高斯问题，以快速获得高精度的姿态信息，并能够准确地估计陀螺漂移，实现各种类型航天器长时间、高精度的组合定姿。
[0006]本发明的技术解决方案为:一种基于原子自旋效应的陀螺仪和磁强计组合定姿方法，特别是一种基于蚁群PF的滤波算法的组合定姿方法，其特点在于:利用惯性量测信息和地磁量测信息，通过蚁群(Ant Colony Algorithm) PF (粒子滤波)方法,实现航天器长时间、高精度的快速组合定姿，其实现步骤如下:
[0007](I)利用原子白旋陀螺仪获取惯性角速度信息，然后进行陀螺误差补偿，通过姿态解算获取载体的姿态信息；
[0008](2)利用原子磁强计获得地磁量测信息，并对其进行地磁匹配，获取地磁矢量信息；
[0009](3)利用地磁数据库给出的地磁匹配数据和由步骤(I)获得的姿态信息来匹配出相应的地磁矢量信息，然后与步骤(2)获取的地磁矢量信息(I)获得的姿态信息进行比较，比较结果作为测量值；
[0010](4)利用蚁群(Ant Colony)粒子滤波(Particle Filter)算法将步骤(3)得到的匹配出的相应地磁矢量信息和步骤(2)原子磁强计给出的地磁矢量信息进行比较滤波，得到误差的最优估计，利用此估计分别对原子白旋陀螺仪和磁强计的数据进行校正。
[0011](5)原子自旋陀螺仪和原子磁强计采用紧组合方式:利用原子磁强计匹配出的地磁信息去修正陀螺的漂移，具体表现为:用原子磁强计测得的磁偏角和磁倾角信息去修正滤波器输出的载体姿态信息和陀螺漂移；用原子自旋陀螺仪输出的姿态信息去修正原子磁强计的磁场信息，具体表现为:用滤波器输出的载体姿态角信息去修正地磁匹配得到的原子磁强计的相应磁偏角和磁倾角，使原子陀螺仪和原子磁强计的测量信息相互修正，实现原子陀螺仪和原子磁强计的紧组合定姿。
[0012]本发明的原理是:首先利用原子白旋陀螺仪获取惯性角速度信息，然后进行陀螺误差补偿，通过姿态解算获取载体的姿态信息；其次利用原子磁强计获得地磁量测信息，并对其采用基于“地磁熵和一种改进的基于标准方差的Hausdorff距离”的匹配方法进行地磁矢量匹配，获得载体所在位置的地磁矢量信息；再次利用蚁群(Ant Colony)粒子滤波(Particle Filter)算法采用紧组合方式将地磁信息和载体姿态信息相融合，解决系统非线性和噪声非高斯问题，求解高精度载体姿态信息，估计陀螺漂移，并反馈校正载体姿态、补偿陀螺漂移并校正原子磁强计测得的磁偏角和磁倾角；最终实现原子白旋陀螺仪/磁强计紧组合定姿系统陀螺随机误差的在线修正，完成对航天器的长时间、高精度组合定姿。
[0013]本发明与现有技术相比的优点在于:本发明克服了传统陀螺仪/磁强计组合定姿方法定姿精度低及滤波性能差的不足，利用蚁群(Ant Colony Algorithm)PF(粒子滤波)算法有效解决了系统非线性和噪声非高斯的问题，利用蚁群算法对PF(粒子滤波)进行优化，有效的提高了组合定姿的精度；采用基于地磁熵和一种改进的基于标准方差的HausdorfT距离的匹配方法进行地磁矢量匹配；采用原子陀螺仪/原子磁强计紧组合方式，将惯性量测信息和地磁量测信息相融合，进一步提高了组合定姿的精度，实现了对陀螺漂移的精确估计，满足了航天器长时间、高精度组合定姿的要求。【专利附图】
【附图说明】
[0014]图1为本发明一种基于蚁群PF的滤波算法的组合定姿方法原理图；
[0015]图2为本发明中用的基于地磁熵和一种改进的基于标准方差Hausdorff距离的地磁匹配算法流程图；
[0016]图3为本发明中用蚁群算法优化粒子的流程图。
【具体实施方式】
[0017]如图1所示，本发明的具体实施步骤如下:
[0018]步骤1、首先对原子陀螺仪测得的惯性量测信息进行补偿陀螺输出数据后，通过姿态解算，得到载体姿态信息，流程如下:
[0019]步骤1、惯性测量信息，即利用原子陀螺仪测量得到而输出的数据，利用信息融合算法估计得到的陀螺漂移f来校正补偿陀螺输出数据ω=ΚΧν+ε+ζ中的ε，之后通过姿态解算，得到载体姿态信息；所述步骤I具体包括以下步骤:
[0020]步骤1.1.设定初始姿态为θ0 η)，计算得出初始姿态四元数q(O)阵:
【权利要求】
1.一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于包括以下步骤: (1)利用原子自旋陀螺仪获取惯性角速度信息，然后进行陀螺误差补偿，通过姿态解算获取载体的姿态信息； (2)利用原子磁强计获得地磁量测信息，并对其进行地磁匹配，获取地磁矢量信息； (3)利用地磁数据库给出的地磁匹配数据和由步骤(I)获得的姿态信息来匹配出相应的地磁矢量信息，然后与步骤(2)获取的地磁矢量信息(I)获得的姿态信息进行比较，比较结果作为测量值； (4)利用蚁群(AntColony)粒子滤波(Particle Filter)算法将步骤(3)得到的匹配出的相应地磁矢量信息和步骤(2)原子磁强计给出的地磁矢量信息进行比较滤波，得到误差的最优估计，利用此估计分别对原子自旋陀螺仪和磁强计的数据进行校正； (5)原子自旋陀螺仪和原子磁强计采用紧组合方式:利用原子磁强计匹配出的地磁信息去修正陀螺的漂移，具体表现为:用原子磁强计测得的磁偏角和磁倾角信息去修正滤波器输出的载体姿态信息和陀螺漂移；用原子自旋陀螺仪输出的姿态信息去修正原子磁强计的磁场信息，具体表现为:用滤波器输出的载体姿态角信息去修正地磁匹配得到的原子磁强计的相应磁偏角和磁倾角，使原子陀螺仪和原子磁强计的测量信息相互修正，实现原子陀螺仪和原子磁强计的紧组合定姿。
2.根据权利要求1所述的一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于:所述步骤(2)中利用基于地磁熵和一种改进的基于标准方差的Hausdorff距离的方法进行地磁匹配,具体步骤为: (21)根据载体当前位置用原子磁强计测得实时地磁图； (22)将一段时间内测得的地磁强度数据按照获得时间的先后排成一个数组，按照以下公式计算实时地磁图的地磁熵:
3.根据权利要求1所述的一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于:所述步骤(2)中原子自旋陀螺仪和原子磁强计采用紧组合方式: (A)利用原子磁强计匹配出的地磁信息去修正陀螺的漂移，具体表现为:用原子磁强计测得的磁偏角和磁倾角信息去修正滤波器输出的载体姿态信息和陀螺漂移； (B)用原子自旋陀螺仪输出的姿态信息去修正原子磁强计的磁场信息，具体表现为:用滤波器输出的载体姿态角信息去修正地磁匹配得到的原子磁强计的相应磁偏角和磁倾角； 使原子陀螺仪和原子磁强计的测量信息相互修正，实现原子陀螺仪和原子磁强计的紧组合定姿。
4.根据权利要求1所述的一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于:所述步骤(3)中利用蚁群粒子滤波算法实现步骤为: (31)当初始采样时刻t=0时，初始化: 对初始的先验概率密度P GO进行采样，生成N个服从P (?)分布的粒子i = Ir..，N，其均值和方差/f满足:
5.根据权利要求4所述的一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于:所述步骤④通过蚁群算法从得到的这组新的粒子中选取优等粒子的步骤如下:a初始化 令时间t=0,迭代次数N=O,信息素τ U(O)=GC为正常数，
【文档编号】G01C25/00GK103438879SQ201310390181
【公开日】2013年12月11日 申请日期:2013年9月2日 优先权日:2013年9月2日
【发明者】全伟, 吕琳, 房建成, 龙华保, 陈熙, 刘翔, 吴双卿 申请人:北京航空航天大学, 上海航天控制工程研究所