一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法.pdf

一种基于蚁群PF算法的原子自旋陀螺仪／磁强计紧组合定姿方法，本发明涉及一种SERF原子自旋陀螺仪／磁强计组合定姿方法。该方法首先利用原子自旋陀螺仪获取惯性角速度信息，然后进行陀螺误差补偿，通过姿态解算获取载体的姿态信息；其次利用原子磁强计获得地磁量测信息，并对其进行地磁匹配，获取地磁矢量信息；最后利用蚁群(Ant Colony)粒子滤波(Particle Filter)算法采用紧组合方式将地磁矢量信息和载体姿态信息相融合，解决系统非线性和噪声非高斯问题，求解高精度载体姿态信息，估计陀螺漂移，并反馈校正载体姿态和补偿陀螺漂移；最终实现基于SERF原子自旋陀螺仪／磁强计紧组合定姿系统陀螺随机误差的在线修正，完成对运动载体的长时间、高精度组合定姿。

权利要求书
1.  一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于包括以下步骤：
(1)利用原子自旋陀螺仪获取惯性角速度信息，然后进行陀螺误差补偿，通过姿态解算获取载体的姿态信息；
(2)利用原子磁强计获得地磁量测信息，并对其进行地磁匹配，获取地磁矢量信息；
(3)利用地磁数据库给出的地磁匹配数据和由步骤(1)获得的姿态信息来匹配出相应的地磁矢量信息，然后与步骤(2)获取的地磁矢量信息(1)获得的姿态信息进行比较，比较结果作为测量值；
(4)利用蚁群(Ant Colony)粒子滤波(Particle Filter)算法将步骤(3)得到的匹配出的相应地磁矢量信息和步骤(2)原子磁强计给出的地磁矢量信息进行比较滤波，得到误差的最优估计，利用此估计分别对原子自旋陀螺仪和磁强计的数据进行校正；
(5)原子自旋陀螺仪和原子磁强计采用紧组合方式：利用原子磁强计匹配出的地磁信息去修正陀螺的漂移，具体表现为：用原子磁强计测得的磁偏角和磁倾角信息去修正滤波器输出的载体姿态信息和陀螺漂移；用原子自旋陀螺仪输出的姿态信息去修正原子磁强计的磁场信息，具体表现为：用滤波器输出的载体姿态角信息去修正地磁匹配得到的原子磁强计的相应磁偏角和磁倾角，使原子陀螺仪和原子磁强计的测量信息相互修正，实现原子陀螺仪和原子磁强计的紧组合定姿。

2.  根据权利要求1所述的一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于：所述步骤(2)中利用基于地磁熵和一种改进的基于标准方差的Hausdorff距离的方法进行地磁匹配，具体步骤为：
(21)根据载体当前位置用原子磁强计测得实时地磁图；
(22)将一段时间内测得的地磁强度数据按照获得时间的先后排成一个数组，按照以下公式计算实时地磁图的地磁熵：
Hf=-Σi=1npi(pi-1)pi=g(i)Σi=1ng(i)]]>
其中，p(i)为每个地磁值出现的概率，g(i)为第i个点的磁场强度值；
(23)判断粗匹配的指标是否满足地磁熵算法的判决门限，若满足则得到粗匹配点，执行步骤(24)；不满足则继续执行步骤(23)；
(24)确定精匹配的范围和步长；
(25)根据实时地磁图和基准地磁图计算基于标准方差的改进的Hausdorff距离，通过引入点集间距离的标准方差，加入了点集间的分布信息，定义如下：
hSTMHD(A,B)=1NAΣa∈Aminb∈B||a-b||+kS(A,B)]]>
hSTMHD(B,A)=1NBΣa∈Bminb∈A||b-a||+kS(B,A)]]>
其中，a为点集中的元素，b为点集中的元素，k为加权系数，用来调节点的分部信息在距离计算中所占的比重；S(A，B)表示点集A中一点到点集B中最远点的距离的标准方差，S(B，A)表示点集B中最远点到点集A中的最近点的标准方差，则可由下式求得基于标准方差的Hausdorff距离：
S(A,B)=Σa∈A[minb∈B||a-b||-1NAΣa∈Aminb∈B||a-b||]2]]>
S(B,A)=Σb∈B[minb∈B||b-a||-1NBΣb∈Bmina∈A||b-a||]2]]>
HSTMHD(A，B)=max{hSTMHD(A，B)，hSTMHD(B，A)}
(26)将基于标准方差的Hausdorff距离从小到大排序，排除个别较大 值对剩下的基于标准方差的Hausdorff距离值求取平均值，从而确定两点之间的距离基于标准方差的改进的Hausdorff距离；
(27)输出最终地磁矢量信息。

3.  根据权利要求1所述的一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于：所述步骤(2)中原子自旋陀螺仪和原子磁强计采用紧组合方式：
(A)利用原子磁强计匹配出的地磁信息去修正陀螺的漂移，具体表现为：用原子磁强计测得的磁偏角和磁倾角信息去修正滤波器输出的载体姿态信息和陀螺漂移；
(B)用原子自旋陀螺仪输出的姿态信息去修正原子磁强计的磁场信息，具体表现为：用滤波器输出的载体姿态角信息去修正地磁匹配得到的原子磁强计的相应磁偏角和磁倾角；
使原子陀螺仪和原子磁强计的测量信息相互修正，实现原子陀螺仪和原子磁强计的紧组合定姿。

4.  根据权利要求1所述的一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于：所述步骤(3)中利用蚁群粒子滤波算法实现步骤为：
(31)当初始采样时刻t=0时，初始化：
对初始的先验概率密度p(x0)进行采样，生成N个服从p(x0)分布的粒子i＝1，…，N，其均值和方差满足：

P0(i)=E[(x0(i)-x‾0(i))(x0(i)-x‾0(i))T];]]>
(32)t≥1时，步骤如下：
①采样步骤
用卡尔曼滤波更新粒子得到采样x^k(i)~]]>q(xk(i)|x0:k-1(i),y1:k)=N(x‾k(i),Pk(i)),]]>i=1，…，N；
②计算权重w~k(i)=w~k-1(i)p(yk|x^k(i))p(x^k(i)|xk-1(i))q(x^k(i)|x0:k-1(i),y1:k),]]>归一化权重：wk(i)=w~k(i)/Σi=1Nw~k(i);]]>
③重采样步骤
从离散分布的i＝1，…，N中进行N次重采样，得到一组新的粒子仍为p(xk|y0：k)的近似表示；
④通过蚁群算法从得到的这组新的粒子中选取优等粒子，剔出低等的粒子，以解决粒子枯竭问题；
⑤输出
按照最小方差准则，载体姿态的最优估计就是条件分布的均值，即：
x^k=Σi=1Nwkixki]]>
pk=Σi=1Nwki(xki-x^k)(xki-x^k)T.]]>

5.  根据权利要求4所述的一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，其特征在于：所述步骤④通过蚁群算法从得到的这组新的粒子中选取优等粒子的步骤如下：
a初始化
令时间t=0，迭代次数N=0，信息素τij(0)=C，C为正常数，τij(0)为0时刻节点(i，j)的信息素强度；
b将m只蚂蚁置于起点，各只蚂蚁，按照下列转移概率公式，采用赌轮选择方式移动，
pijk=τijα(t)ηijβ(t)Σs∈allowedkτisα(t)ηisβ(t)j∈allowedk0otherwise]]>
allowedk表示蚂蚁k下一步允许走过的路径点的集合；为t时刻节点(i，j)的信息素强度；α为启发式因子，β为期望启发式因子，ηij(t)为节点(i，j) 上的能见度；
c按照各只蚂蚁的目标函数值Fk(选择粒子权值作为目标函数值Fk)，并记录该次循环的最优解；
d按照以下公式修正信息素强度：
τij(t+n)=ρτij(t)+(1-ρ)Δτij，

式中，参数ρ(0≤ρ≤1)为信息素残留因子，1-ρ表示信息素衰减度；Δ表示第k只蚂蚁在本次循环中留在节点(i，j)上的信息量；Δτij表示本次循环汇总节点(i，j)上的信息素的增量，常数Q是信息素强度；
e令t←t+n，N←N+1；
f若N<NCmax，则转b，否则转f，其中NCmax为最大迭代次数；
g输出最优解；
⑤输出
按照最小方差准则，载体姿态的最优估计就是条件分布的均值，即：
x^k=Σi=1Nwkixki]]>
pk=Σi=1Nwki(xki-x^k)(xki-x^k)T.]]>

说明书一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法
技术领域
本发明涉及一种基于蚁群PF算法的原子自旋陀螺仪和磁强计紧组合定姿方法，可实现全地域、全天时、全天候、长时间、高精度的自主定姿，且自主性强、隐蔽性好、成本低，可广泛用于水下探测器、飞机、导弹、航天器等多个领域。
背景技术
为满足天基对地观测、武器精确打击以及空间、水下探索开发的迫切需求，各类航天器、水下探测器必须具备自主运行和管理能力，而高精度的自主定姿是航天器自主运行和管理的核心技术瓶颈。由于在无白旋交换弛豫(SERF)状态下利用原子自旋效应可同时实现超高精度惯性与磁场测量，使得SERF原子陀螺仪／磁强计组合定姿成为实现长时间高精度自主定姿的重要发展方向之一。陀螺仪／磁强计组合定姿具有能够自主、实时提供连续、全面的定姿信息，且不受气候、地域、时间等影响的特点。
目前，航天器的高精度自主定姿，无法依靠任何一种定姿手段独立实现。纯惯性定姿系统，短时精度高，但其误差随工作时间积累，难以满足航天器的长时间高精度定姿要求；地磁定姿可实时测量地磁场强度实现载体的定位，且误差不随时间积累，但受限于地磁匹配精度以及磁场测量精度等，使得目前地磁定姿精度仍然相对较低；将这两者相结合、优势互补，构成陀螺仪／磁强计组合定姿系统，是实现航天器长时间、高精度定姿的最为有效的手段。
在陀螺仪／磁强计组合定姿技术方面，以往都采用扩展卡尔曼滤波EKF(Extended Kalman Filter)方法，但是EKF仅适用于滤波误差和预测误差 很小的情况。近年来提出的Unscented卡尔曼滤波UKF(Unscented Kalman Filter)是一种EKF的改进算法，有效的解决了系统的非线性问题，但其不足是不适用于噪声非高斯分布的系统。粒子滤波(Particle Filter，PF)由于采用蒙特卡洛采样(Monte Carlo sampling)结构而在非线性、非高斯系统状态跟踪上体现出越来越大的优越性，但其缺点是存在退化现象，消除退化现象主要依赖于两个关键技术：适当选取重要密度函数和进行重采样。对于前者的改进方法，可使用EKPF(Extented Kalman Particle Filter)、无迹粒子滤波(Unscented Particle Filter，UPF)来进行重要密度函数的选择。对于后者的改进方法，常用的重采样算法有累积分布重采样(Binary search)、系统重采样(Systematic resampling)、剩余重采样(Residual resampling)等，这些算法通过增加粒子的有效性解决了粒子的退化问题，但是在实际应用中会影响系统的鲁棒性，重采样完成后，重要度高的粒子通过重采样被多次选取，这在一定程度上丢失了粒子的多样性，由此造成的后果是一旦目标丢失或跟踪精度不够，系统自动收敛的可能性很小，为此，很多学者提出了遗传粒子滤波(GPF)算法，GPF算法虽然在保证粒子有效性的同时又增加了粒子的多样性，仍然存在滤波速度慢和鲁棒性差的问题。
发明内容
本发明的技术解决问题是：克服现有技术的不足，提出一种基于蚁群PF算法的原子白旋陀螺仪和磁强计紧组合定姿方法，解决系统非线性和噪声非高斯问题，以快速获得高精度的姿态信息，并能够准确地估计陀螺漂移，实现各种类型航天器长时间、高精度的组合定姿。
本发明的技术解决方案为：一种基于原子自旋效应的陀螺仪和磁强计组合定姿方法，特别是一种基于蚁群PF的滤波算法的组合定姿方法，其特点在于：利用惯性量测信息和地磁量测信息，通过蚁群(Ant Colony Algorithm)PF(粒子滤波)方法，实现航天器长时间、高精度的快速组合定姿，其实现步骤如下：
(1)利用原子白旋陀螺仪获取惯性角速度信息，然后进行陀螺误差补偿，通过姿态解算获取载体的姿态信息；
(2)利用原子磁强计获得地磁量测信息，并对其进行地磁匹配，获取地磁矢量信息；
(3)利用地磁数据库给出的地磁匹配数据和由步骤(1)获得的姿态信息来匹配出相应的地磁矢量信息，然后与步骤(2)获取的地磁矢量信息(1)获得的姿态信息进行比较，比较结果作为测量值；
(4)利用蚁群(Ant Colony)粒子滤波(Particle Filter)算法将步骤(3)得到的匹配出的相应地磁矢量信息和步骤(2)原子磁强计给出的地磁矢量信息进行比较滤波，得到误差的最优估计，利用此估计分别对原子白旋陀螺仪和磁强计的数据进行校正。
(5)原子自旋陀螺仪和原子磁强计采用紧组合方式：利用原子磁强计匹配出的地磁信息去修正陀螺的漂移，具体表现为：用原子磁强计测得的磁偏角和磁倾角信息去修正滤波器输出的载体姿态信息和陀螺漂移；用原子自旋陀螺仪输出的姿态信息去修正原子磁强计的磁场信息，具体表现为：用滤波器输出的载体姿态角信息去修正地磁匹配得到的原子磁强计的相应磁偏角和磁倾角，使原子陀螺仪和原子磁强计的测量信息相互修正，实现原子陀螺仪和原子磁强计的紧组合定姿。
本发明的原理是：首先利用原子白旋陀螺仪获取惯性角速度信息，然后进行陀螺误差补偿，通过姿态解算获取载体的姿态信息；其次利用原子磁强计获得地磁量测信息，并对其采用基于“地磁熵和一种改进的基于标准方差的Hausdorff距离”的匹配方法进行地磁矢量匹配，获得载体所在位置的地磁矢量信息；再次利用蚁群(Ant Colony)粒子滤波(Particle Filter)算法采用紧组合方式将地磁信息和载体姿态信息相融合，解决系统非线性和噪声非高斯问题，求解高精度载体姿态信息，估计陀螺漂移，并反馈校正载体姿态、补偿陀螺漂移并校正原子磁强计测得的磁偏角和磁倾角；最终实现 原子白旋陀螺仪／磁强计紧组合定姿系统陀螺随机误差的在线修正，完成对航天器的长时间、高精度组合定姿。
本发明与现有技术相比的优点在于：本发明克服了传统陀螺仪／磁强计组合定姿方法定姿精度低及滤波性能差的不足，利用蚁群(Ant Colony Algorithm)PF(粒子滤波)算法有效解决了系统非线性和噪声非高斯的问题，利用蚁群算法对PF(粒子滤波)进行优化，有效的提高了组合定姿的精度；采用基于地磁熵和一种改进的基于标准方差的Hausdorff距离的匹配方法进行地磁矢量匹配；采用原子陀螺仪／原子磁强计紧组合方式，将惯性量测信息和地磁量测信息相融合，进一步提高了组合定姿的精度，实现了对陀螺漂移的精确估计，满足了航天器长时间、高精度组合定姿的要求。
附图说明
图1为本发明一种基于蚁群PF的滤波算法的组合定姿方法原理图；
图2为本发明中用的基于地磁熵和一种改进的基于标准方差Hausdorff距离的地磁匹配算法流程图；
图3为本发明中用蚁群算法优化粒子的流程图。
具体实施方式
如图1所示，本发明的具体实施步骤如下：
步骤1、首先对原子陀螺仪测得的惯性量测信息进行补偿陀螺输出数据后，通过姿态解算，得到载体姿态信息，流程如下：
步骤1、惯性测量信息，即利用原子陀螺仪测量得到而输出的数据，利用信息融合算法估计得到的陀螺漂移来校正补偿陀螺输出数据ω=K×V+ε+ζ中的ε，之后通过姿态解算，得到载体姿态信息；所述步骤1具体包括以下步骤：
步骤1.1.设定初始姿态为计算得出初始姿态四元数q(0)阵：

步骤1.2.更新姿态四元数矩阵为：
q(n+1)={cosΔφ2I+sinΔφ2Δφ[ΔΦ]}q(n)]]>
其中，n为第n时刻，I为单位四元数，Δφ=[ΔφX  ΔφY  ΔφZ]为陀螺输出角增量，[ΔΦ]为：
[ΔΦ]=0-ΔφX-ΔφY-ΔφZΔφX0ΔφZ-ΔφYΔφY-ΔφZ0ΔφXΔφZΔφY-ΔφX0;]]>
步骤1.3.由q(n+1)=[q1  q2  q3  q4]T，计算姿态余弦阵C为：
C=C11C12C13C21C22C23C31C32C33=q42+q12-q22-q322(q1q2+q4q3)2(q1q3-q4q2)2(q1q2-q4q3)q42-q12+q22-q322(q2q3+q4q1)2(q1q3+q4q2)2(q2q3-q4q1)q42-q12-q22+q32;]]>
步骤1.4.由姿态余弦阵C求解载体的实时姿态信息，所述载体的实时姿态信息包括航向、俯仰和横滚三姿态角信息：
航向、俯仰和横滚三姿态角的解算公式如下：
俯仰角θ值为：θ=sin-1(C23)
航向角值的计算如下表所示：


横滚角γ值的计算如下表所示：
C13值判断C33值判断横滚角γ值=0<0-π>0<0atan-1(-C13/C33)-π>0=0-π/2任意值>0atan-1(-C13/C33)<0=0π/2<0<0atan-1(-C13/C33)+π
步骤2、利用原子磁强计获得地磁量测信息，并对其进行地磁矢量匹配。利用基于地磁熵和一种改进的基于标准方差的Hausdorff距离的方法进行地磁匹配,如图2所示，为本发明的基于地磁熵和一种改进的基于标准方差的Hausdorff距离的匹配方法流程图，步骤如下：
a.根据载体当前位置用原子磁强计测得实时地磁图；
b.按照以下公式计算实时地磁图的地磁熵；
Hf=-Σi=1npi(pi-1)pi=g(i)Σi=1ng(i)]]>
其中，p(i)为每个地磁值出现的概率，g(i)为第i个点的磁场强度值;
c.判断粗匹配的指标是否满足地磁熵算法的判决门限，若满足则得到粗匹配点，执行步骤4），不满足则继续执行步骤3）；
d.确定精匹配的范围和步长；
e.根据实时地磁图和基准地磁图计算基于标准方差的改进的Hausdorff距离，通过引入点集间距离的标准方差，加入了点集间的分布信息，定义如下：
hSTMHD(A,B)=1NAΣa&Element;Aminb&Element;B||a-b||+kS(A,B)]]>
hSTMHD(B,A)=1NBΣa&Element;Bminb&Element;A||b-a||+kS(B,A)]]>
其中，a为点集中的元素，b为点集中的元素，k为加权系数，用来调节点的分部信息在距离计算中所占的比重；S(A，B)表示点集A中一点到点集B中最远点的距离的标准方差，S(B，A)表示点集B中最远点到点集A中的最近点的标准方差，则可由下式求得基于标准方差的Hausdorff距离：
S(A,B)=Σa&Element;A[minb&Element;B||a-b||-1NAΣa&Element;Aminb&Element;B||a-b||]2]]>
S(B,A)=Σb&Element;B[minb&Element;B||b-a||-1NBΣb&Element;Bmina&Element;A||b-a||]2]]>
HSTMHD(A，B)＝max{hSTMHD(A，B)，hSTMHD(B，A)}
f.将基于标准方差的Hausdorff距离从小到大排序，排除个别较大值对剩下的基于标准方差的Hausdorff距离值求取平均值，从而确定两点之间的距离基于标准方差的改进的Hausdorff距离；
g.输出最终的地磁矢量信息。
步骤3、原子自旋陀螺仪和原子磁强计采用紧组合方式：
1)利用原子磁强计匹配出的地磁信息去修正陀螺的漂移，具体表现为：用原子磁强计测得的磁偏角和磁倾角信息去修正滤波器输出的载体姿态角信息和陀螺漂移；
2)用姿态角信息去修正原子磁强计的磁场信息，具体表现为：用滤波器输出的载体姿态角信息去修正地磁匹配得到的原子磁强计的相应磁偏角 和磁倾角；
使原子陀螺仪和原子磁强计的测量信息相互修正，实现原子陀螺仪和原子磁强计的紧组合定姿。
步骤4、如图3所示，为本发明的蚁群算法优化粒子的流程图，利用蚁群算法优化的PF算法将天文姿态信息和载体姿态信息相融合，完成对航天器长时间、高精度的组合定姿步骤为：
(1)t＝0时，初始化：
对初始的先验概率密度p(x0)进行采样，生成N个服从p(x0)分布的粒子其均值和方差满足

P0(i)=E[(x0(i)-x&OverBar;0(i))(x0(i)-x&OverBar;0(i))T];]]>
(2)t≥1时，步骤如下：
①采样步骤
用卡尔曼滤波更新粒子得到采样x^k(i)~]]>q(xk(i)|x0:k-1(i),y1:k)=N(x&OverBar;k(i),Pk(i)),i=1,...,N;]]>
②计算权重
w~k(i)=w~k-1(i)p(yk|x^k(i))p(x^k(i)|xk-1(i))q(x^k(i)|x0:k-1,(i)y1:k)]]>
归一化权重：wk(i)=w~k(i)/Σi=1Nw~k(i);]]>
③重采样步骤
从离散分布的中进行N次重采样，得到一组新的粒子仍为的近似表示；
④通过蚁群算法从得到的这组新的粒子中选取优等粒子，剔出低等的粒子，以解决粒子枯竭问题，利用蚁群算法进行优化的步骤如下：
a初始化
令时间t＝0，迭代次数N＝0，信息素τij(0)＝C，C为正常数，τij(0)为0时刻节点(i，j)的信息素强度；
b将m只蚂蚁置于起点，各只蚂蚁，按照下列转移概率公式，采用赌轮选择方式移动，
pijk=τijα(t)ηijβ(t)Σs&Element;allowedkτisα(t)ηisβ(t)j&Element;allowedk0otherwise]]>
allowedk表示蚂蚁k下一步允许走过的路径点的集合；为t时刻节点(i，j)的信息素强度；α为启发式因子，表示轨迹的相对重要性；β为期望启发式因子，表示能见度的相对重要性；ηij(t)为节点(i，j)上的能见度；
c按照各只蚂蚁的目标函数值Fk(选择粒子权值作为目标函数Fk)，并记录该次循环的最优解；
d按照以下公式修正信息素强度：
τij(t+n)=ρτij(t)+(1-ρ)Δτij,Δτij=Σk=1mΔτijk]]>

式中，参数ρ(0≤ρ≤1)为信息素残留因子，表示轨迹的持久性；1-ρ表示信息素衰减度；表示第k只蚂蚁在本次循环中留在节点(i，j)上的信息量；Δτij表示本次循环汇总节点(i，j)上的信息素的增量。常数Q是信息素强度，体现了蚂蚁所留轨迹数量；
e令t←t+n，N←N+1；
f若N＜NCmax，则转b，否则转f，其中NCmax为最大迭代次数；
g输出最优解。
⑤输出
按照最小方差准则，载体姿态的最优估计值就是条件分布的均值，即：
x^k=Σi=1Nwkixki]]>
pk=Σi=1Nwki(xki-x^k)(xki-x^k)T.]]>
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。