CN102735231A - 一种提高光纤陀螺寻北仪精度的方法 - Google Patents

Abstract

本发明公开了一种提高光纤陀螺寻北仪精度的方法。该方法的步骤如下：对寻北仪标定得到光纤陀螺和加速度计的标度因数和安装误差；在经过系统安装误差标定的基础上，进行四位置数据采集和预处理，然后计算基体系各轴加速度，得到寻北仪的俯仰角和横滚角；再对基体系各轴角速度进行计算和判定，最后得出寻北仪方位角。本发明消除了转位机构的转角误差和转轴方位误差所引起的寻北误差；消除了光纤陀螺和加速度计的安装误差所引起的寻北误差；降低了寻北仪工艺要求，无需再从工艺上消除安装误差，同时转位机构转过的四个位置不再需要严格正交；提高了全姿态下光纤陀螺寻北仪的寻北精度。

Description

一种提高光纤陀螺寻北仪精度的方法

技术领域

本发明涉及导航、制导及控制领域，尤其涉及一种提高光纤陀螺寻北仪精度的方法。

背景技术

在军事应用领域和工程测量领域，如导弹发射系统、地面雷达系统、隧道巷道修建等，都存在着自主定向需求。陀螺寻北仪作为一种精密惯性测量仪器，对精确定向，提高军用系统精度和工程效率有着至关重要的作用。

光纤陀螺是一种全固态陀螺，具有稳定耐冲击、启动时间短、温度范围宽、体积小、重量轻、功耗低等特点，较机械陀螺具有非常明显的优势。因此，采用光纤陀螺的寻北仪，小型化程度更高、功耗更低、性价比更高。

光纤陀螺寻北仪的方位基准由寻北仪机箱上的基准决定。如图1所示，基准面定为寻北仪左侧面（面OABC），安装面定为寻北仪底面（面OX_bY_b）。取基准面上的O点为原点建立寻北仪基体坐标系（简称基体系），记为b系。OX_b轴垂直于基准面并指向右方，OY_b轴平行于基准面和安装面并指向前方，OZ_b轴垂直于安装面并指向上方。同时，导航坐标系选取为当地地理坐标系,原点亦取O，记为n系，如图1所示。OX_n轴指向东，OY_n轴指向北，OZ_n轴指向天。

寻北仪的姿态角分别是俯仰角θ、方位角ψ、横滚角γ。如图1所示，俯仰角为OY_b轴与水平面的夹角。方位角为OY_b轴在水平面内的投影（OY_b′）与地理北向（OY_n轴方向）的夹角。横滚角为面OABC与过OY_b轴的垂面ODEF的夹角。

寻北仪的全姿态是指寻北仪姿态角处于下述定义域时的所有状态：方位角ψ的定义域为[0,2π)。俯仰角θ和横滚角γ的定义域均为(-π/2,π/2)，即寻北仪工作在正立情况下，OZ_b轴始终指向上方。另外，上述三个姿态角均以图1中所示的情况为正，例如对于方位角，以北偏东为正。

四位置寻北方法是光纤陀螺寻北仪常用的一种寻北方法。寻北仪工作时，光纤陀螺需要在四个相互正交的位置上测量。光纤陀螺在四个位置的输出分别为：

F1=K_F(ω_e·cosΦ·cosψ·cosβ₁+ω_e·sinΦ·sinβ₁)+D_F+ε_F1

F₃=K_F(ω_e·cosΦ·cos(ψ+π)·cosβ₃+ω_e·sinΦ·sinβ₃)+D_F+ε_F3    （1）

F₂=K_F(ω_e·cosΦ·cos(ψ+π/2)·cosβ₂+ω_e·sinΦ·sinβ₂)+D_F+ε_F2

F₄=K_F(ω_e·cosΦ·cos(ψ+3π/2)·cosβ₄+ω_e·sinΦ·sinβ₄)+D_F+ε_F4

其中，K_F为光纤陀螺的标度因数，D_F为光纤陀螺零位，ε_Fi为光纤陀螺随机误差，β_i为光纤陀螺的倾斜角，i＝1～4。ω_e为地球自转角速度，Φ为当地地理纬度。

使用加速度计解算出每个位置的倾斜角β_i，忽略第1和第3位置上光纤陀螺零位及随机误差的差异，并把这两位置的光纤陀螺输出值作差，可以求出方位角ψ的余弦值。同理，由第2和第4位置数据可以求出方位角ψ的正弦值。最后通过求反正切即可解算出方位角ψ。

上述这类传统的四位置寻北方法一般有以下要求：（1）转位机构转过的四个位置需要严格正交，即转位机构的转角误差需要足够小。（2）忽略转位机构相对于基体系的安装误差，该误差主要指转轴和寻北仪安装面（如底面OZ_bY_b）的不垂直程度，即转轴的方位误差。（3）光纤陀螺敏感轴和加速度计敏感轴需要平行，即光纤陀螺以及加速度计的安装误差需要严格控制，从而使加速度计测得的倾斜角为光纤陀螺的倾斜角，否则会带来寻北误差。

由此可见，传统方法存在以下缺陷：（1）无法完全消除转位机构的转角误差和转轴方位误差。（2）往往忽略由于光纤陀螺和加速度计的安装误差而引起的寻北解算误差。（3）要求寻北仪各器件和机构的加工精度高，但光纤陀螺的敏感轴是虚轴，很难通过工艺保证敏感轴的失准角，从而难以保证它与加速度计敏感轴的平行性。

理论分析与实验结果表明，对于使用光纤陀螺和加速度计作为惯性测量器件的寻北仪，工作在全姿态下时，转位机构的转角误差、光纤陀螺和加速度计的敏感轴不平行以及其他安装误差所引起的寻北误差，能够达到相当于一个密位（0.06度）的误差量级，并且会随着寻北仪倾斜角的增大而增加。因此，这些误差对于高精度全姿态寻北仪（寻北精度优于1密位）来说是必须消除的。

发明内容

针对背景技术的不足，本发明的目的在于提供一种提高光纤陀螺寻北仪精度的方法，采用全姿态下的四位置寻北方法，能够消除转位机构误差以及光纤陀螺和加速度计的安装误差造成的寻北解算误差。

本发明采用的技术方案的步骤如下：

（1）寻北仪标定

标定过程中，转位机构要求转到寻北时所需停留的各个位置。即转位机构到达第i个位置，i＝1～4，分别对光纤陀螺和加速度计进行标定。光纤陀螺经过标定，得到标度因数K_F、安装误差E_XFi、E_YFi、E_ZFi。加速度计经过标定，得到标度因数K_A、安装误差E_XAi、E_YAi、E_ZAi。最终得到标度因数共2个，安装误差共计8组24个。

（2）数据采集和预处理

将转位机构转到第i个位置，i＝1～4，测试t秒光纤陀螺输出值并取平均值，记为F_i，同时测试t秒加速度计输出值并取平均值，记为A_i。测试时间t由待测光纤陀螺的精度决定，具体通过Allan方差分析方法得到。最终得到光纤陀螺输出值4个，加速度计输出值4个。

（3）计算基体系加速度分量

基体系加速度分量A_X,A_Y,A_Z满足以下方程组，其中g₀为当地重力加速度

$\left\{\begin{array}{c}\frac{A_1-A_3}{K_A}=(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})A_X+(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})A_Y+(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})A_Z\\ \frac{A_2-A_4}{K_A}=(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})A_X+(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})A_Y+(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})A_Z\\ {A_X}^2+{A_Y}^2+{A_Z}^2={g_0}^2\end{array}\right.---\left(1\right)$

基体系加速度方程组的求解过程为

$\left\{\begin{array}{c}{A}_a=\frac{\frac{A_1-A_3}{K_A}(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-\frac{A_2-A_4}{K_A}(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})}{(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})}\\ B_a=\frac{\frac{A_1-A_3}{K_A}(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})-\frac{A_2-A_4}{K_A}(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})}{(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})-(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})}\end{array}\right.$

$\left\{\begin{array}{c}{D}_{a0}=(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})-(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})\\ E_{a0}=(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})-(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})\\ F_{a0}=(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})\end{array}\right.$

$\left\{\begin{array}{c}{D}_a=D_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\\ E_a=E_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\\ F_a=F_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\end{array}\right.---\left(2\right)$

${K_a}^{\±}=(A_a\·D_a+B_a\·E_a)\±\sqrt{{(A_a\·D_a+B_a\·E_a)}^2+({g_0}^2-{A_a}^2-{B_a}^2)}$

$\left\{\begin{array}{c}{A_X}^{\±}=-{K_a}^{\±}\·D_a+A_a\\ {A_Y}^{\±}=-{K_a}^{\±}\·E_a+B_a\\ {A_Z}^{\±}=-{K_a}^{\±}\·F_a\end{array}\right.$

本方程存在两组解，分别为A_X ⁺,A_Y ⁺,A_Z ⁺和A_X ^-,A_Y ^-,A_Z ^-，通过下式即可确定最-终解

$(A_X,A_Y,A_Z)=\left\{\begin{array}{cc}({A_X}^{+},{A_Y}^{+},{A_Z}^{+})& {A_Z}^{+}\&GreaterEqual;{A_Z}^{-}\\ ({A_X}^{-},{A_Y}^{-},{A_Z}^{-})& {A_Z}^{+}<{A_Z}^{-}\end{array}\right.---\left(3\right)$

（4）计算寻北仪的俯仰角θ、横滚角γ，其中g₀为当地重力加速度

$\mathrm{\&theta;}=\arcsin \frac{A_Y}{g_0}$ （4）

$\mathrm{\&gamma;}=\arcsin \frac{-A_X}{g_0\cos \mathrm{\&theta;}}$

（5）计算基体系角速度分量

基体系角速度分量ω_bx,ω_by,ω_bz满足以下方程组，其中ω_e为地球自转角速度

$\left\{\begin{array}{c}\frac{F_1-F_3}{K_F}=(E_{\mathrm{XF}1}-E_{\mathrm{XF}3}){\mathrm{\&omega;}}_{\mathrm{bx}}+(E_{\mathrm{YF}1}-E_{\mathrm{YF}3}){\mathrm{\&omega;}}_{\mathrm{by}}+(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3}){\mathrm{\&omega;}}_{\mathrm{bz}}\\ \frac{F_2-F_4}{K_F}=(E_{\mathrm{XF}2}-E_{\mathrm{XF}4}){\mathrm{\&omega;}}_{\mathrm{bx}}+(E_{\mathrm{YF}2}-E_{\mathrm{YF}4}){\mathrm{\&omega;}}_{\mathrm{by}}+(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4}){\mathrm{\&omega;}}_{\mathrm{bz}}\\ {{\mathrm{\&omega;}}_{\mathrm{bx}}}^2+{{\mathrm{\&omega;}}_{\mathrm{by}}}^2+{{\mathrm{\&omega;}}_{\mathrm{bz}}}^2={{\mathrm{\&omega;}}_e}^2\end{array}\right.---\left(5\right)$

基体系角速度方程组的求解过程为

$\left\{\begin{array}{c}{A}_f=\frac{\frac{F_1-F_3}{K_F}(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-\frac{F_2-F_4}{K_F}(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})}{(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})}\\ B_f=\frac{\frac{F_1-F_3}{K_F}(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})-\frac{F_2-F_4}{K_F}(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})}{(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})-(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})}\end{array}\right.$

$\left\{\begin{array}{c}{D}_{f0}=(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4})-(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3})\\ E_{f0}=(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3})-(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4})\\ F_{f0}=(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})\end{array}\right.$

$\left\{\begin{array}{c}{D}_f=D_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\\ E_f=E_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\\ F_f=F_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\end{array}\right.---\left(6\right)$

${K_f}^{\&PlusMinus;}=(A_f\&CenterDot;D_f+B_f\&CenterDot;E_f)\&PlusMinus;\sqrt{{(A_f\&CenterDot;D_f+B_f\&CenterDot;E_f)}^2+({{\mathrm{\&omega;}}_e}^2-{A_f}^2-{B_f}^2)}$

$\left\{\begin{array}{c}{{\mathrm{\&omega;}}_{\mathrm{bx}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;D_f+A_f\\ {{\mathrm{\&omega;}}_{\mathrm{by}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;E_f+B_f\\ {{\mathrm{\&omega;}}_{\mathrm{bz}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;F_f\end{array}\right.$

本方程存在两组解，分别为ω_bx ⁺,ω_by ⁺,ω_bz ⁺和ω_bx ^-,ω_by ^-,ω_bz ^-，通过下式即可确定最终解

ε⁺=|ω_bx ⁺·cosθsinγ-ω_by ⁺·sinθ-ω_bz ⁺·cosθcosγ+ω_e sinΦ|

ε^-=|ω_bx ^-·cosθsinγ-ω_by ^-·sinθ-ω_bz ^-·cosθcosγ+ω_e sinΦ|    （7）

$({\mathrm{\&omega;}}_{\mathrm{bx}},{\mathrm{\&omega;}}_{\mathrm{by}},{\mathrm{\&omega;}}_{\mathrm{bz}})=\left\{\begin{array}{cc}({{\mathrm{\&omega;}}_{\mathrm{bx}}}^{+},{{\mathrm{\&omega;}}_{\mathrm{by}}}^{+},{{\mathrm{\&omega;}}_{\mathrm{bz}}}^{+})& {\mathrm{\&epsiv;}}^{+}\&le;{\mathrm{\&epsiv;}}^{-}\\ ({{\mathrm{\&omega;}}_{\mathrm{bx}}}^{-},{{\mathrm{\&omega;}}_{\mathrm{by}}}^{-},{{\mathrm{\&omega;}}_{\mathrm{bz}}}^{-})& {\mathrm{\&epsiv;}}^{+}>{\mathrm{\&epsiv;}}^{-}\end{array}\right.$

（6）求解方位角ψ

X_ψ=ω_esinΦsinθcosγ-ω_by cosγ

Y_ψ=ω_esinΦsinγ-ω_bysinθsinγ+ω_bxcosθ    （8）

ψ_主=arctan(Y_ψ/X_ψ)

上式中方位角ψ的定义域为[0,2π)，以北偏东为正，此即最终的寻北结果。

本发明具有的有益效果是：

1.消除了转位机构的转角误差和转轴方位误差所引起的寻北误差。

2.消除了光纤陀螺和加速度计的安装误差所引起的寻北误差。

3.降低了寻北仪工艺要求。无需再从工艺上消除安装误差，同时转位机构转过的四个位置不再需要严格正交。

4.提高了全姿态下光纤陀螺寻北仪的寻北精度。

附图说明

图1是寻北仪基体系和姿态角的示意图。

图2是光纤陀螺寻北仪解算方法原理图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

一种提高光纤陀螺寻北仪精度的方法，如图2所示，其原理是首先对寻北仪进行系统标定。标定过程中，转位机构要求转到寻北时所需停留的各个位置，每到一个位置，在该位置对光纤陀螺和加速度计进行标定，从而得到该位置光纤陀螺的安装误差E_XFi、E_YFi、E_ZFi以及加速度计的安装误差E_XAi、E_YAi、E_ZAi，其中i表示四个位置的位置序数，i＝1～4。标定过程中同时得到光纤陀螺的标度因数K_F以及加速度计的标度因数K_A。标定的重要意义是，得到的8组共24个安装误差事实上包含了光纤陀螺和加速度计相对于转位机构的安装误差以及转位机构的转角误差和转位机构相对于寻北仪基体系的安装误差。对于每一组安装误差，它表征了惯性测量器件的敏感轴相对于基体系的方位，实际上是惯性测量器件敏感轴和基体系各轴夹角的余弦值。由此则跳过了分析多重安装误差以及误差之间相互耦合情况的复杂思维，优化了模型，这是本方法的基础。

然后，解算基体坐标系各轴的加速度分量。

当转位机构停留在某一位置时，考虑安装误差，加速度计的输出值为：

A_i=K_A(E_XAiA_X+E_YAiA_Y+E_ZAiA_Z)+D_A+ε_Ai    （1）

其中，D_A为加速度计零位，ε_Ai为加速度计随机误差，i＝1～4。A_X,A_Y,A_Z为基体系各轴的加速度分量。

于是分别在四个位置采集加速度计的输出，并忽略经过差分之后零位和随机误差的残余量，就可以得到关于基体系加速度的一组方程，其中g₀为当地重力加速度：

$\left\{\begin{array}{c}\frac{A_1-A_3}{K_A}=(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})A_X+(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})A_Y+(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})A_Z\\ \frac{A_2-A_4}{K_A}=(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})A_X+(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})A_Y+(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})A_Z\\ {A_X}^2+{A_Y}^2+{A_Z}^2={g_0}^2\end{array}\right.---\left(2\right)$

基体系加速度方程组存在两组解，分别记为A_X ⁺,A_Y ⁺,A_Z ⁺和A_X ^-,A_Y ^-,A_Z ^-。因寻北仪工作在正立情况下，寻北仪基体系的OZ_b轴不可能朝下，故加速度在OZ_b轴的分量A_Z为正值，于是有：

$(A_X,A_Y,A_Z)=\left\{\begin{array}{cc}({A_X}^{+},{A_Y}^{+},{A_Z}^{+})& {A_Z}^{+}\&GreaterEqual;{A_Z}^{-}\\ ({A_X}^{-},{A_Y}^{-},{A_Z}^{-})& {A_Z}^{+}<{A_Z}^{-}\end{array}\right.---\left(3\right)$

使用基体系加速度分量，即可得出寻北仪的俯仰角θ和横滚角γ。

同理，解算基体坐标系各轴的角速度分量。

转位机构停留在某一位置时，考虑安装误差，光纤陀螺的输出值为：

F_i=K_F(E_XFiω_bx+E_YFiω_by+E_ZFiω_bz)+D_F+ε_Fi    （4）

其中，D_F为光纤陀螺零位，ε_Fi为光纤陀螺随机误差，i＝1～4。ω_bx,ω_by,ω_bz为基体系各轴的角速度分量。

于是分别在四个位置采集光纤陀螺的输出，并忽略经过差分之后零位和随机误差的残余量，就可以得到关于基体系角速度的一组方程，其中ω_e为地球自转角速度：

$\left\{\begin{array}{c}\frac{F_1-F_3}{K_F}=(E_{\mathrm{XF}1}-E_{\mathrm{XF}3}){\mathrm{\&omega;}}_{\mathrm{bx}}+(E_{\mathrm{YF}1}-E_{\mathrm{YF}3}){\mathrm{\&omega;}}_{\mathrm{by}}+(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3}){\mathrm{\&omega;}}_{\mathrm{bz}}\\ \frac{F_2-F_4}{K_F}=(E_{\mathrm{XF}2}-E_{\mathrm{XF}4}){\mathrm{\&omega;}}_{\mathrm{bx}}+(E_{\mathrm{YF}2}-E_{\mathrm{YF}4}){\mathrm{\&omega;}}_{\mathrm{by}}+(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4}){\mathrm{\&omega;}}_{\mathrm{bz}}\\ {{\mathrm{\&omega;}}_{\mathrm{bx}}}^2+{{\mathrm{\&omega;}}_{\mathrm{by}}}^2+{{\mathrm{\&omega;}}_{\mathrm{bz}}}^2={{\mathrm{\&omega;}}_e}^2\end{array}\right.---\left(5\right)$

分析角速度方程组和加速度方程组的建立过程，可知转位机构的转角误差以及转轴方位误差，已经包含于标定所得的光纤陀螺和加速度计的安装误差之中。因此，对于采用本方法的寻北仪，转位机构转过的四个位置不再需要严格正交。

基体系角速度方程组存在两组解，分别记为ω_bx ⁺,ω_by ⁺,ω_bz ⁺和ω_bx ^-,ω_by ^-,ω_bz ^-。可以通过使用当地地理纬度值Φ来确定合理解，过程如下：

ε⁺=|ω_bx ⁺·cosθsinγ-ω_by ⁺·sinθ-ω_bz ⁺·cosθcosγ+ω_e sinΦ|

ε^-=|ω_bx ^-·cosθsinγ-ω_by ^-·sinθ-ω_bz ^-·cosθcosγ+ω_e sinΦ|    （6）

$({\mathrm{\&omega;}}_{\mathrm{bx}},{\mathrm{\&omega;}}_{\mathrm{by}},{\mathrm{\&omega;}}_{\mathrm{bz}})=\left\{\begin{array}{cc}({{\mathrm{\&omega;}}_{\mathrm{bx}}}^{+},{{\mathrm{\&omega;}}_{\mathrm{by}}}^{+},{{\mathrm{\&omega;}}_{\mathrm{bz}}}^{+})& {\mathrm{\&epsiv;}}^{+}\&le;{\mathrm{\&epsiv;}}^{-}\\ ({{\mathrm{\&omega;}}_{\mathrm{bx}}}^{-},{{\mathrm{\&omega;}}_{\mathrm{by}}}^{-},{{\mathrm{\&omega;}}_{\mathrm{bz}}}^{-})& {\mathrm{\&epsiv;}}^{+}>{\mathrm{\&epsiv;}}^{-}\end{array}\right.$

通过上述解算过程，已经得到基体系各轴的角速度以及俯仰角θ、横滚角γ，根据下式即可得出寻北仪的方位角。方位角ψ的定义域为[0,2π)，以北偏东为正，此即最终的寻北结果。

X_ψ=ω_esinΦsinθcosγ-ω_bycosγ

Y_ψ=ω_esinΦsinγ-ω_bysinθsinγ+ω_bxcosθ    （7）

ψ_主=arctan(Y_ψ/X_ψ)

下面详细描述本发明的实施步骤，本发明的目的和效果将变得更加明显。

（1）寻北仪标定

标定过程中，转位机构要求转到寻北时所需停留的各个位置。采用本发明时，这四个位置允许存在偏差，即不再需要严格正交。转位机构到达第i个位置，i＝1～4，分别对光纤陀螺和加速度计进行标定。光纤陀螺经过标定，得到标度因数K_F、安装误差E_XFi、E_YFi、E_ZFi。加速度计经过标定，得到标度因数K_A、安装误差E_XAi、E_YAi、E_ZAi。最终得到标度因数共2个，安装误差共计8组24个。

（2）数据采集和预处理

将转位机构转到第i个位置，i＝1～4，测试t秒光纤陀螺输出值并取平均值，记为F_i，同时测试t秒加速度计输出值并取平均值，记为A_i。测试时间t由待测光纤陀螺的精度决定，具体通过Allan方差分析方法得到。最终得到光纤陀螺输出值4个，加速度计输出值4个。

（3）计算基体系加速度分量

基体系加速度分量A_X,A_Y,A_Z满足以下方程组，其中g₀为当地重力加速度

$\left\{\begin{array}{c}\frac{A_1-A_3}{K_A}=(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})A_X+(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})A_Y+(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})A_Z\\ \frac{A_2-A_4}{K_A}=(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})A_X+(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})A_Y+(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})A_Z\\ {A_X}^2+{A_Y}^2+{A_Z}^2={g_0}^2\end{array}\right.---\left(1\right)$

基体系加速度方程组的求解过程为

$\left\{\begin{array}{c}{A}_a=\frac{\frac{A_1-A_3}{K_A}(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-\frac{A_2-A_4}{K_A}(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})}{(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})}\\ B_a=\frac{\frac{A_1-A_3}{K_A}(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})-\frac{A_2-A_4}{K_A}(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})}{(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})-(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})}\end{array}\right.$

$\left\{\begin{array}{c}{D}_{a0}=(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})-(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})\\ E_{a0}=(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})-(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})\\ F_{a0}=(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})\end{array}\right.$

$\left\{\begin{array}{c}{D}_a=D_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\\ E_a=E_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\\ F_a=F_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\end{array}\right.---\left(2\right)$

${K_a}^{\&PlusMinus;}=(A_a\&CenterDot;D_a+B_a\&CenterDot;E_a)\&PlusMinus;\sqrt{{(A_a\&CenterDot;D_a+B_a\&CenterDot;E_a)}^2+({g_0}^2-{A_a}^2-{B_a}^2)}$

$\left\{\begin{array}{c}{A_X}^{\&PlusMinus;}=-{K_a}^{\&PlusMinus;}\&CenterDot;D_a+A_a\\ {A_Y}^{\&PlusMinus;}=-{K_a}^{\&PlusMinus;}\&CenterDot;E_a+B_a\\ {A_Z}^{\&PlusMinus;}=-{K_a}^{\&PlusMinus;}\&CenterDot;F_a\end{array}\right.$

本方程存在两组解，分别为A_X ⁺,A_Y ⁺,A_Z ⁺和A_X ^-,A_Y ^-,A_Z ^-，通过下式即可确定最终解

$(A_X,A_Y,A_Z)=\left\{\begin{array}{cc}({A_X}^{+},{A_Y}^{+},{A_Z}^{+})& {A_Z}^{+}\&GreaterEqual;{A_Z}^{-}\\ ({A_X}^{-},{A_Y}^{-},{A_Z}^{-})& {A_Z}^{+}<{A_Z}^{-}\end{array}\right.---\left(3\right)$

（4）计算寻北仪的俯仰角θ、横滚角γ，其中g₀为当地重力加速度

$\mathrm{\&theta;}=\arcsin \frac{A_Y}{g_0}$ （4）

$\mathrm{\&gamma;}=\arcsin \frac{-A_X}{g_0\cos \mathrm{\&theta;}}$

（5）计算基体系角速度分量

基体系角速度分量ω_bx,ω_by,ω_bz满足以下方程组，其中ω_e为地球自转角速度

$\left\{\begin{array}{c}\frac{F_1-F_3}{K_F}=(E_{\mathrm{XF}1}-E_{\mathrm{XF}3}){\mathrm{\&omega;}}_{\mathrm{bx}}+(E_{\mathrm{YF}1}-E_{\mathrm{YF}3}){\mathrm{\&omega;}}_{\mathrm{by}}+(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3}){\mathrm{\&omega;}}_{\mathrm{bz}}\\ \frac{F_2-F_4}{K_F}=(E_{\mathrm{XF}2}-E_{\mathrm{XF}4}){\mathrm{\&omega;}}_{\mathrm{bx}}+(E_{\mathrm{YF}2}-E_{\mathrm{YF}4}){\mathrm{\&omega;}}_{\mathrm{by}}+(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4}){\mathrm{\&omega;}}_{\mathrm{bz}}\\ {{\mathrm{\&omega;}}_{\mathrm{bx}}}^2+{{\mathrm{\&omega;}}_{\mathrm{by}}}^2+{{\mathrm{\&omega;}}_{\mathrm{bz}}}^2={{\mathrm{\&omega;}}_e}^2\end{array}\right.---\left(5\right)$

基体系角速度方程组的求解过程为

$\left\{\begin{array}{c}{A}_f=\frac{\frac{F_1-F_3}{K_F}(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-\frac{F_2-F_4}{K_F}(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})}{(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})}\\ B_f=\frac{\frac{F_1-F_3}{K_F}(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})-\frac{F_2-F_4}{K_F}(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})}{(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})-(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})}\end{array}\right.$

$\left\{\begin{array}{c}{D}_{f0}=(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4})-(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3})\\ E_{f0}=(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3})-(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4})\\ F_{f0}=(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})\end{array}\right.$

$\left\{\begin{array}{c}{D}_f=D_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\\ E_f=E_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\\ F_f=F_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\end{array}\right.---\left(6\right)$

${K_f}^{\&PlusMinus;}=(A_f\&CenterDot;D_f+B_f\&CenterDot;E_f)\&PlusMinus;\sqrt{{(A_f\&CenterDot;D_f+B_f\&CenterDot;E_f)}^2+({{\mathrm{\&omega;}}_e}^2-{A_f}^2-{B_f}^2)}$

$\left\{\begin{array}{c}{{\mathrm{\&omega;}}_{\mathrm{bx}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;D_f+A_f\\ {{\mathrm{\&omega;}}_{\mathrm{by}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;E_f+B_f\\ {{\mathrm{\&omega;}}_{\mathrm{bz}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;F_f\end{array}\right.$

本方程存在两组解，分别为ω_bx ⁺,ω_by ⁺,ω_bz ⁺和ω_bx ^-,ω_by ^-,ω_bz ^-，通过下式即可确定最终解

ε⁺=|ω_bx ⁺·cosθsinγ-ω_by ⁺·sinθ-ω_bz ⁺·cosθcosγ+ω_e sinΦ|

ε^-=|ω_bx ^-·cosθsinγ-ω_by ^-·sinθ-ω_bz ^-·cosθcosγ+ω_e sinΦ|    （7）

$({\mathrm{\&omega;}}_{\mathrm{bx}},{\mathrm{\&omega;}}_{\mathrm{by}},{\mathrm{\&omega;}}_{\mathrm{bz}})=\left\{\begin{array}{cc}({{\mathrm{\&omega;}}_{\mathrm{bx}}}^{+},{{\mathrm{\&omega;}}_{\mathrm{by}}}^{+},{{\mathrm{\&omega;}}_{\mathrm{bz}}}^{+})& {\mathrm{\&epsiv;}}^{+}\&le;{\mathrm{\&epsiv;}}^{-}\\ ({{\mathrm{\&omega;}}_{\mathrm{bx}}}^{-},{{\mathrm{\&omega;}}_{\mathrm{by}}}^{-},{{\mathrm{\&omega;}}_{\mathrm{bz}}}^{-})& {\mathrm{\&epsiv;}}^{+}>{\mathrm{\&epsiv;}}^{-}\end{array}\right.$

（6）求解方位角ψ

X_ψ=ω_esinΦsinθcosγ-ω_by cosγ

Y_ψ=ω_esinΦsinγ-ω_bysinθsinγ+ω_bxcosθ    （8）

ψ_主=arctan(Y_ψ/X_ψ)

上式中方位角ψ的定义域为[0,2π)，以北偏东为正，此即最终的寻北结果。

下面叙述本发明的一个实施例，从而进一步说明本发明的特点和效果。

本实施例中的寻北仪采用精度为0.01°/h的光纤陀螺和精度为2e-5g₀的微机械压电平衡式加速度计，经过系统标定后，得到惯性测量器件的安装误差以及标度因数如表1至表3所示。

表1加速度计安装误差

位置序数i	EXAi	EYAi	EZAi
				1	0.00560951	0.99995520	-0.00762407
2	-0.99994531	0.00905911	-0.00522533
				3	-0.00341379	-0.99996025	-0.00823702
4	0.99995384	-0.00507633	-0.00815771

表2光纤陀螺安装误差

位置序数i	EXFi	EYFi	EZFi
				1	0.001629934	0.999998566	0.000459034
2	-0.999997961	0.001853627	0.000801485
				3	-0.001913566	-0.999997900	0.000734111
4	0.999998489	-0.001669753	0.000484649

表3标度因数

加速度计标度因数KA	64208.87bit/g0
		光纤陀螺标度因数KF	43375696.62bit/(°/s)

实验过程中，寻北仪放置于带有基准靠块的大理石平台之上，此基准靠块与北向的夹角已知，为30.5906度，并采用楔形垫块改变寻北仪的俯仰角。当地地理纬度为北纬30.265499度。寻北过程中，寻北仪在四个位置的数据采集时间为60s，总寻北时间小于5min，并采用传统寻北方法和本方法分别计算方位角。五次实验中得到的惯性测量器件数据如表4所示，最终的寻北结果如表5所示，可见采用本方法能够有效提高全姿态下光纤陀螺寻北仪的寻北精度。

表4惯性测量器件数据

寻北次数	A1	A2	A3	A4	F1	F2	F3	F4
									1	21470.52	-146.12	-22485.80	-634.47	180707.36	-56587.10	-135056.01	102335.25
2	16115.49	-204.52	-17159.93	-620.22	176695.76	-56595.63	-131074.34	102304.39
									3	10637.85	-259.78	-11700.45	-603.35	171428.99	-56586.47	-125780.01	102213.42
4	5078.47	-313.39	-6152.72	-580.17	165048.15	-56411.93	-119394.27	102196.70
									5	-520.36	-366.84	-558.21	-553.64	157793.98	-56553.47	-112016.58	102187.85

表5光纤陀螺寻北仪实验结果

Claims (1)

1.一种提高光纤陀螺寻北仪精度的方法，其特征在于，该方法的步骤如下：

（1）寻北仪标定

标定过程中，转位机构要求转到寻北时所需停留的位置；即转位机构到达第i个位置，i＝1～4，分别对光纤陀螺和加速度计进行标定；光纤陀螺经过标定，得到标度因数K_F、安装误差E_XFi、E_YFi、E_ZFi；加速度计经过标定，得到标度因数K_A、安装误差E_XAi、E_YAi、E_ZAi；最终得到标度因数共2个，安装误差共计8组24个；

（2）数据采集和预处理

将转位机构转到第i个位置，i＝1～4，测试t秒光纤陀螺输出值并取平均值，记为F_i，同时测试t秒加速度计输出值并取平均值，记为A_i，测试时间t由待测光纤陀螺的精度决定，具体通过Allan方差分析方法得到，最终得到4个光纤陀螺输出值，4个加速度计输出值；

（3）计算基体系加速度分量

基体系加速度分量A_X,A_Y,A_Z满足以下方程组，其中g₀为当地重力加速度，

$\left\{\begin{array}{c}\frac{A_1-A_3}{K_A}=(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})A_X+(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})A_Y+(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})A_Z\\ \frac{A_2-A_4}{K_A}=(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})A_X+(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})A_Y+(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})A_Z\\ {A_X}^2+{A_Y}^2+{A_Z}^2={g_0}^2\end{array}\right.---\left(1\right)$

基体系加速度方程组的求解过程为

$\left\{\begin{array}{c}{A}_a=\frac{\frac{A_1-A_3}{K_A}(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-\frac{A_2-A_4}{K_A}(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})}{(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})}\\ B_a=\frac{\frac{A_1-A_3}{K_A}(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})-\frac{A_2-A_4}{K_A}(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})}{(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})-(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})}\end{array}\right.$

$\left\{\begin{array}{c}{D}_{a0}=(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})-(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})\\ E_{a0}=(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{ZA}1}-E_{\mathrm{ZA}3})-(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{ZA}2}-E_{\mathrm{ZA}4})\\ F_{a0}=(E_{\mathrm{XA}1}-E_{\mathrm{XA}3})(E_{\mathrm{YA}2}-E_{\mathrm{YA}4})-(E_{\mathrm{XA}2}-E_{\mathrm{XA}4})(E_{\mathrm{YA}1}-E_{\mathrm{YA}3})\end{array}\right.$

$\left\{\begin{array}{c}{D}_a=D_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\\ E_a=E_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\\ F_a=F_{a0}/\sqrt{D_{a0}^2+E_{a0}^2+F_{a0}^2}\end{array}\right.---\left(2\right)$

${K_a}^{\&PlusMinus;}=(A_a\&CenterDot;D_a+B_a\&CenterDot;E_a)\&PlusMinus;\sqrt{{(A_a\&CenterDot;D_a+B_a\&CenterDot;E_a)}^2+({g_0}^2-{A_a}^2-{B_a}^2)}$

$\left\{\begin{array}{c}{A_X}^{\&PlusMinus;}=-{K_a}^{\&PlusMinus;}\&CenterDot;D_a+A_a\\ {A_Y}^{\&PlusMinus;}=-{K_a}^{\&PlusMinus;}\&CenterDot;E_a+B_a\\ {A_Z}^{\&PlusMinus;}=-{K_a}^{\&PlusMinus;}\&CenterDot;F_a\end{array}\right.$

本方程存在两组解，分别为A_X ⁺,A_Y ⁺,A_Z ⁺和A_X ^-,A_Y ^-,A_Z ^-，通过下式即可确定最终解

$(A_X,A_Y,A_Z)=\left\{\begin{array}{cc}({A_X}^{+},{A_Y}^{+},{A_Z}^{+})& {A_Z}^{+}\&GreaterEqual;{A_Z}^{-}\\ ({A_X}^{-},{A_Y}^{-},{A_Z}^{-})& {A_Z}^{+}<{A_Z}^{-}\end{array}\right.---\left(3\right)$

（4）计算寻北仪的俯仰角θ、横滚角γ，其中g₀为当地重力加速度

$\mathrm{\&theta;}=\arcsin \frac{A_Y}{g_0}$ （4）

$\mathrm{\&gamma;}=\arcsin \frac{-A_X}{g_0\cos \mathrm{\&theta;}}$

（5）计算基体系角速度分量

基体系角速度分量ω_bx,ω_by,ω_bz满足以下方程组，其中ω_e为地球自转角速度，

$\left\{\begin{array}{c}\frac{F_1-F_3}{K_F}=(E_{\mathrm{XF}1}-E_{\mathrm{XF}3}){\mathrm{\&omega;}}_{\mathrm{bx}}+(E_{\mathrm{YF}1}-E_{\mathrm{YF}3}){\mathrm{\&omega;}}_{\mathrm{by}}+(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3}){\mathrm{\&omega;}}_{\mathrm{bz}}\\ \frac{F_2-F_4}{K_F}=(E_{\mathrm{XF}2}-E_{\mathrm{XF}4}){\mathrm{\&omega;}}_{\mathrm{bx}}+(E_{\mathrm{YF}2}-E_{\mathrm{YF}4}){\mathrm{\&omega;}}_{\mathrm{by}}+(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4}){\mathrm{\&omega;}}_{\mathrm{bz}}\\ {{\mathrm{\&omega;}}_{\mathrm{bx}}}^2+{{\mathrm{\&omega;}}_{\mathrm{by}}}^2+{{\mathrm{\&omega;}}_{\mathrm{bz}}}^2={{\mathrm{\&omega;}}_e}^2\end{array}\right.---\left(5\right)$

基体系角速度方程组的求解过程为

$\left\{\begin{array}{c}{A}_f=\frac{\frac{F_1-F_3}{K_F}(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-\frac{F_2-F_4}{K_F}(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})}{(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})}\\ B_f=\frac{\frac{F_1-F_3}{K_F}(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})-\frac{F_2-F_4}{K_F}(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})}{(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})-(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})}\end{array}\right.$

$\left\{\begin{array}{c}{D}_{f0}=(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4})-(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3})\\ E_{f0}=(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{ZF}1}-E_{\mathrm{ZF}3})-(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{ZF}2}-E_{\mathrm{ZF}4})\\ F_{f0}=(E_{\mathrm{XF}1}-E_{\mathrm{XF}3})(E_{\mathrm{YF}2}-E_{\mathrm{YF}4})-(E_{\mathrm{XF}2}-E_{\mathrm{XF}4})(E_{\mathrm{YF}1}-E_{\mathrm{YF}3})\end{array}\right.$

$\left\{\begin{array}{c}{D}_f=D_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\\ E_f=E_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\\ F_f=F_{f0}/\sqrt{D_{f0}^2+E_{f0}^2+F_{f0}^2}\end{array}\right.---\left(6\right)$

${K_f}^{\&PlusMinus;}=(A_f\&CenterDot;D_f+B_f\&CenterDot;E_f)\&PlusMinus;\sqrt{{(A_f\&CenterDot;D_f+B_f\&CenterDot;E_f)}^2+({{\mathrm{\&omega;}}_e}^2-{A_f}^2-{B_f}^2)}$

$\left\{\begin{array}{c}{{\mathrm{\&omega;}}_{\mathrm{bx}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;D_f+A_f\\ {{\mathrm{\&omega;}}_{\mathrm{by}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;E_f+B_f\\ {{\mathrm{\&omega;}}_{\mathrm{bz}}}^{\&PlusMinus;}=-{K_f}^{\&PlusMinus;}\&CenterDot;F_f\end{array}\right.$

本方程存在两组解，分别为ω_bx ⁺,ω_by ⁺,ω_bz ⁺和ω_bx ^-,ω_by ^-,ω_bz ^-，通过下式即可确定最终解

ε⁺=|ω_bx ⁺·cosθsinγ-ω_by ⁺·sinθ-ω_bz ⁺·cosθcosγ+ω_esinΦ|

ε^-=|ω_bx ^-·cosθsinγ-ω_by ^-·sinθ-ω_bz^-·cosθcosγ+ω_esinΦ|    （7）

$({\mathrm{\&omega;}}_{\mathrm{bx}},{\mathrm{\&omega;}}_{\mathrm{by}},{\mathrm{\&omega;}}_{\mathrm{bz}})=\left\{\begin{array}{cc}({{\mathrm{\&omega;}}_{\mathrm{bx}}}^{+},{{\mathrm{\&omega;}}_{\mathrm{by}}}^{+},{{\mathrm{\&omega;}}_{\mathrm{bz}}}^{+})& {\mathrm{\&epsiv;}}^{+}\&le;{\mathrm{\&epsiv;}}^{-}\\ ({{\mathrm{\&omega;}}_{\mathrm{bx}}}^{-},{{\mathrm{\&omega;}}_{\mathrm{by}}}^{-},{{\mathrm{\&omega;}}_{\mathrm{bz}}}^{-})& {\mathrm{\&epsiv;}}^{+}>{\mathrm{\&epsiv;}}^{-}\end{array}\right.$

（6）求解方位角ψ

X_ψ＝ω_e sinΦsinθcosγ-ω_by cosγ

Y_ψ＝ω_e sinΦsinγ-ω_bysinθsinγ+ω_bx cosθ    （8）

ψ_主＝arctan(Y_ψ/X_ψ)

上式中方位角ψ的定义域为[0,2π)，以北偏东为正，此即最终的寻北结果。

Images