CN103487053B - 一种任意二位置捷联寻北方法 - Google Patents

Abstract

本发明属于陀螺定向寻北技术领域，涉及一种任意两个位置捷联式快速寻北方法。包括以下步骤：“寻北仪通电、系统粗调平”；“采集陀螺及加速度计在位置2的输出信号”；“对陀螺常值漂移和加速度计零偏进行补偿”；“计算真北方位角”；五大步骤，本发明同以前的传统捷联寻北方法相比，要求陀螺仪的数据采集位置只有两位置，较前数集采集量大为减少，且不要求陀螺仪必须在相差固定的位置上进行数据测量二位置的180°、四位置的90°采集，而且减少了由于测量位置过多、相差过大以及精确地位置定位不仅操作过程繁琐，大大减少了捷联寻北系统的定向时间。

Description

一种任意二位置捷联寻北方法

技术领域

本发明属于陀螺定向寻北技术领域，涉及一种任意两个位置捷联式快速寻北方法。

背景技术

捷联式陀螺寻北仪是一种能在静态下全天候自动指示方位的高精度惯性仪表，在军事和民用部门均有广阔的应用前景，军事上的导弹、火炮等武器系统的快速精确定位定向以及民用中的精密大地测量、矿井工程及贯通测量都需要非常精确的方位基准。

目前国内外研制生产的捷联寻北仪多采用的是二位置、四位置以及多位置寻北方案，如中国北方自动控制技术研究所研制的二位置(相差180°)数字捷联寻北仪；中国科学院长春光学精密机械与物理研究所研制的基于动调陀螺的多位置捷联寻北仪；航天发射技术研究所研制的TXC-2陀螺指北仪；西安测绘研究所研制的基于旋转调制技术的自动陀螺寻北仪等等。基于这些寻北方案而研制的寻北仪有各自优点，但大都存在寻北定向时间过久的问题，这大大增加了寻北仪应用领域的技术准备时间；单位置寻北方法寻北速度很快，但是由于陀螺常值漂移的影响精度太低，没有实用价值。为此，通过研究一种新的捷联式全方位快速寻北方法，来简化寻北过程的复杂程度，在保证一定精度的前提下缩短寻北时间是非常有必要的。在本发明以前的传统捷联寻北方法中，要么要求陀螺仪的数据采集位置过多(四位置、多位置等)；要么要求陀螺仪必须在相差固定的位置上进行数据测量(二位置的180°、四位置的90°)。测量位置过多、相差过大以及精确地位置定位不仅操作过程繁琐，而且耗时较长，大大增加了捷联寻北系统的定向时间。

发明内容

针对现有的捷联寻北方法存在的不足,本发明提供一种基于陀螺漂移特性的任意二位置捷联式全方位快速寻北方法，其设计目的在于：通过在任意两个位置上对陀螺和加速度计的输出信号进行采集，在有效补偿陀螺常值漂移和加速度计零偏对寻北精度影响的前提下，一方面，本发明相对于传统二位置法可以在相差较小(<180°)的两个位置进行寻北测量，从而减少转台转动时间，在一定程度上节省整个寻北过程的时间；另一方面，本发明在相差任意角度的两个位置上对陀螺进行采样，取消了准确的转台定位系统，只需准确测量转台转动的角度，操作简单，避免了需要精确定位而引进的误差源。

为达成上述发明目的，现将本发明技术解决方案叙述如下：

本发明一种任意二位置捷联寻北方法,其特征在于：包括以下步骤：

步骤1：寻北仪通电、系统粗调平；

捷联寻北仪的结构主体主要由一个陀螺仪G和两个加速度计A_x、A_y组成(如图1所示其中：G—陀螺仪；A_x—加速度计1；A_y—加速度计2；M_z—力矩电机；R—转台；L—测角装置；x_by_bz_b—陀螺载体坐标系；)，陀螺仪G用于敏感地球自转角速度分量，加速度计A_x、A_y用于敏感陀螺x轴、y轴的倾斜角，即图3中的俯仰角θ和横滚角γ；寻北系统工作时，先供电一段时间，根据加速度计A_x和A_y的输出信号对整个系统进行粗略调平；

步骤2：采集陀螺及加速度计在位置1(初始位置)的输出信号；

步骤2.1：确定惯性坐标系与地理坐标系之间的方位关系：

如图2所示为惯性坐标系与地理坐标系之间的方位关系示意图，惯性坐标系Ox_iy_iz_i记为i系，地理坐标系Ox_ny_nz_n(ONWT)记为n系，ON轴指北，OW轴指西，OT轴指天；图中ω_ie表示地球自转角速度，ω_N和ω_T分别表示地球自转角速度在n系中北向分量和天顶分量：表示当地纬度；

步骤2.2：确定地理坐标系与载体坐标系之间的方位关系：

如图3所示，b系为陀螺载体坐标系，初始时与地理坐标系重合，载体的姿态角α、θ、γ分别表示方位角、俯仰角和横滚角，它表示n系先以角速度绕z_n旋转α角到坐标系Ox₁y₁z_n，然后再分别以角速度和绕y₁和x₁轴旋转θ和γ角得到b系，故可得地理坐标系到陀螺载体坐标系的转换矩阵：

$\begin{array}{c}{C}_n^b=C_2^b\&CenterDot;C_1^2\&CenterDot;C_n^1\\ =\left[\begin{array}{ccc}\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}& \sin \mathrm{\&alpha;}\cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ \cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& \sin \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\\ \cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&alpha;}\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}\end{array}\right]\end{array}---\left(1\right)$

其中表示从n系旋转到坐标系Ox₁y₁z_n的方向余弦矩阵，表示从坐标系Ox₁y₁z_n旋转到坐标系Ox_by₁z₁的方向余弦矩阵，表示从坐标系Ox_by₁z₁旋转到b系的方向余弦矩阵。

步骤2.3：考虑陀螺常值漂移和随机漂移项待系统输出稳定后，可得陀螺测得的角速度在陀螺载体坐标系的投影向量为：

${\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}^b=\left[\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right]=C_n^b{\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{ie}^n+{\stackrel{\&RightArrow;}{\mathrm{\&epsiv;}}}_0+\stackrel{\&RightArrow;}{\mathrm{\&epsiv;}}=C_n^b\left[\begin{array}{c}{\mathrm{\&omega;}}_N\\ 0\\ {\mathrm{\&omega;}}_T\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&epsiv;}}_0^x\\ {\mathrm{\&epsiv;}}_0^y\\ {\mathrm{\&epsiv;}}_0^z\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&epsiv;}}^x\\ {\mathrm{\&epsiv;}}^y\\ {\mathrm{\&epsiv;}}^z\end{array}\right]---\left(2\right)$

式中分别表示陀螺x、y、z敏感轴的常值漂移，ε^x、ε^y、ε^z分别表示陀螺x、y、z敏感轴的随机漂移；把(1)式代入(2)式可得陀螺在位置1(初始位置)测得的角速度为：

${\mathrm{\&omega;}}_{x\left(1\right)}={\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}cos\mathrm{\&theta;}-{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;}+{\mathrm{\&epsiv;}}_0^x+{\mathrm{\&epsiv;}}_{\left(1\right)}^x---\left(3\right)$

${\mathrm{\&omega;}}_{y\left(1\right)}={\mathrm{\&omega;}}_N(cos\mathrm{\&alpha;}sin\mathrm{\&theta;}sin\mathrm{\&gamma;}-sin\mathrm{\&alpha;}cos\mathrm{\&gamma;})+{\mathrm{\&omega;}}_{T}cos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&epsiv;}}_0^y+{\mathrm{\&epsiv;}}_{\left(1\right)}^y---\left(4\right)$

式中分别表示陀螺x、y敏感轴在位置1测量数据的随机漂移。

步骤2.4：同理可得加速度计在位置1(初始位置)的测得值为：

$a_{x\left(1\right)}=gsin\mathrm{\&theta;}+{\mathrm{\&xi;}}_0^x+{\mathrm{\&xi;}}_{\left(1\right)}^x---\left(5\right)$

$a_{y\left(1\right)}=-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&xi;}}_0^y+{\mathrm{\&xi;}}_{\left(1\right)}^y---\left(6\right)$

式中分别表示加速度计A_x、A_y的零偏，分别表示加速度计A_x、A_y在位置1测量数据的随机漂移。

步骤3：采集陀螺及加速度计在位置2的输出信号；

步骤3.1：对初始位置陀螺和加速度计的输出信号采集完毕后，通过力矩电机Mz控制转台R绕z_b轴转动任意角度μ，设在初始状态下机械转动系为m系，与载体坐标系b系重合，转动后的机械转动系为m¹系(如图4所示)，则m系到m¹系之间的方向余弦矩阵为：

$C_m^{m^1}=\left[\begin{array}{ccc}cos\mathrm{\&mu;}& sin\mathrm{\&mu;}& 0\\ -sin\mathrm{\&mu;}& \cos \mathrm{\&mu;}& 0\\ 0& 0& 1\end{array}\right]$

则可得位置2陀螺的理论输出角速度为：

${\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{m^1}=C_m^{m^1}\&CenterDot;C_n^b\&CenterDot;{\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{ie}^n=C_m^{m^1}\&CenterDot;C_n^b\left[\begin{array}{c}{\mathrm{\&omega;}}_N\\ 0\\ {\mathrm{\&omega;}}_T\end{array}\right]$

步骤3.2：考虑陀螺常值漂移和随机漂移项，故可得陀螺在位置2测得的角速度为：

$\begin{array}{c}{\mathrm{\&omega;}}_{x\left(2\right)}=\cos \mathrm{\&mu;}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\\ \sin \mathrm{\&mu;}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;+{\mathrm{\&epsiv;}}_0^x+{\mathrm{\&epsiv;}}_{\left(2\right)}^x\end{array}---\left(7\right)$

$\begin{array}{c}{\mathrm{\&omega;}}_{y\left(2\right)}=-\sin \mathrm{\&mu;}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\\ \cos \mathrm{\&mu;}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;+{\mathrm{\&epsiv;}}_0^y+{\mathrm{\&epsiv;}}_{\left(2\right)}^y\end{array}---\left(8\right)$

式中分别表示陀螺x、y敏感轴在位置2测量数据的随机漂移。

步骤3.3：同理可得加速度计在位置2测得的值为：

$a_{x\left(2\right)}=gsin\mathrm{\&theta;}cos\mathrm{\&mu;}-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}\sin \mathrm{\&mu;}+{\mathrm{\&xi;}}_0^x+{\mathrm{\&xi;}}_{\left(2\right)}^x---\left(9\right)$

$a_{y\left(2\right)}=-gsin\mathrm{\&theta;}sin\mathrm{\&mu;}-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}cos\mathrm{\&mu;}+{\mathrm{\&xi;}}_0^y+{\mathrm{\&xi;}}_{\left(2\right)}^y---\left(10\right)$

式中分别表示加速度计A_x、A_y在位置2测量数据的随机漂移。

步骤4：对陀螺常值漂移和加速度计零偏进行补偿；

步骤4.1：陀螺常值漂移的补偿。把式(3)、(4)和式(7)、(8)分别对应相减并忽略随机漂移可得

$\begin{array}{c}{\mathrm{\&omega;}}_x={\mathrm{\&omega;}}_{x\left(1\right)}-{\mathrm{\&omega;}}_{x\left(2\right)}={\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}-\\ \{\cos \mathrm{\&mu;}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\sin \mathrm{\&mu;}[{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\left]\right\}\end{array}---\left(11\right)$

$\begin{array}{c}{\mathrm{\&omega;}}_y={\mathrm{\&omega;}}_{y\left(1\right)}-{\mathrm{\&omega;}}_{y\left(2\right)}={\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\\ \{-\sin \mathrm{\&mu;}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\cos \mathrm{\&mu;}[{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\left]\right\}\end{array}---\left(12\right)$

步骤4.2：加速度计零偏的补偿。把式(5)、(6)和式(9)、(10)分别对应相减并忽略随机漂移可得

α_x＝α_x(1)-α_x(2)＝g sinθ-(g sinθcosμ-g cosθsinγsinμ) (13)

α_y＝α_y(1)-α_y(2)＝-g cosθsinγ-(-g sinθsinμ-g cosθsinγcosμ) (14)

这说明任意二位置测量方法能够有效消除陀螺常值漂移和加速度计零偏对寻北精度的影响。

步骤5：计算真北方位角；

根据上述分析，联立式(11)、(12)可解得

$\sin \mathrm{\&alpha;}=\frac{{\mathrm{\&omega;}}_x(\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&mu;}\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})-{\mathrm{\&omega;}}_y(\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\cos \mathrm{\&theta;}-\sin \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})+2{\mathrm{\&omega;}}_T\sin \mathrm{\&gamma;}(1-\cos \mathrm{\&mu;})}{2{\mathrm{\&omega;}}_N\cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}(1-\cos \mathrm{\&mu;})}$

$cos\mathrm{\&alpha;}=\frac{{\mathrm{\&omega;}}_x(cos\mathrm{\&mu;}-1)-{\mathrm{\&omega;}}_{y}sin\mathrm{\&mu;}+2{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;}(cos\mathrm{\&mu;}-1)}{2{\mathrm{\&omega;}}_N\cos \mathrm{\&theta;}(\cos \mathrm{\&mu;}-1)}$

故可得方位角：

$\begin{array}{c}\tan \mathrm{\&alpha;}=\frac{{\mathrm{\&omega;}}_x(\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&mu;}\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})-{\mathrm{\&omega;}}_y(\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\cos \mathrm{\&theta;}-\sin \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})+2{\mathrm{\&omega;}}_T\sin \mathrm{\&gamma;}(1-\cos \mathrm{\&mu;})}{-\&lsqb;{\mathrm{\&omega;}}_x(\cos \mathrm{\&mu;}-1)-{\mathrm{\&omega;}}_y\sin \mathrm{\&mu;}+2{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}(\cos \mathrm{\&mu;}-1)\&rsqb;\cos \mathrm{\&gamma;}}\\ \mathrm{\&alpha;}=\arctan \left\{\frac{{\mathrm{\&omega;}}_x(\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&mu;}\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})-{\mathrm{\&omega;}}_y(\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\cos \mathrm{\&theta;}-\sin \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})+2{\mathrm{\&omega;}}_T\sin \mathrm{\&gamma;}(1-\cos \mathrm{\&mu;})}{-\&lsqb;{\mathrm{\&omega;}}_x(\cos \mathrm{\&mu;}-1)-{\mathrm{\&omega;}}_y\sin \mathrm{\&mu;}+2{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}(\cos \mathrm{\&mu;}-1)\&rsqb;\cos \mathrm{\&gamma;}}\right\}\end{array}---\left(15\right)$

式中ω_x、ω_y分别为陀螺x、y敏感轴在任意两个位置上的测量数据之差；θ、γ分别表示俯仰角和横滚角，可由式(13)、(14)联立求的。

对于下述三种特殊情况有：

(1)θ＝0、γ＝0(水平状态下)

$\mathrm{\&alpha;}=arctan\&lsqb;\frac{{\mathrm{\&omega;}}_{x}sin\mathrm{\&mu;}-{\mathrm{\&omega;}}_y(1-cos\mathrm{\&mu;})}{{\mathrm{\&omega;}}_x(1-cos\mathrm{\&mu;})+{\mathrm{\&omega;}}_{y}sin\mathrm{\&mu;}}\&rsqb;$

(2)μ＝180°(对径180°两位置测量，即传统二位置法)

$\mathrm{\&alpha;}=arctan\&lsqb;\frac{{\mathrm{\&omega;}}_{x}sin\mathrm{\&theta;}sin\mathrm{\&gamma;}-{\mathrm{\&omega;}}_{y}cos\mathrm{\&theta;}+2{\mathrm{\&omega;}}_{T}sin\mathrm{\&gamma;}}{({\mathrm{\&omega;}}_x+2{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;})cos\mathrm{\&gamma;}}\&rsqb;$

(3)θ＝0、γ＝0且μ＝180°(水平状态下的传统二位置法)

$\mathrm{\&alpha;}=arctan(-\frac{{\mathrm{\&omega;}}_y}{{\mathrm{\&omega;}}_x})$

在实际寻北工作中，可灵活选择两个测量位置间的相位差，在消除陀螺常值漂移和加速度计零偏的同时，有效缩短寻北时间。

附图说明

图1为捷联式陀螺寻北仪结构简图

图2为惯性坐标系与地理坐标系之间的方位关系示意图

图3为地理坐标系与载体坐标系之间的方位关系示意图

图4为机械转动系m与m¹之间的方位关系示意图

具体实施方式

现结合附图对本发明的具体实施方式作进一步说明：

实施例1：

如图4所示，设转动角度μ＝30°(即在相差30°的两位置进行寻北解算)，参见图1、2、3、4，本发明任意二位置捷联寻北解算方法具体步骤为：

步骤1：寻北仪通电、系统粗调平；

根据上面步骤1的说明和图1所示，对寻北仪进行通电，根据加速度计A_x和A_y的输出信号对整个系统进行粗略调平。

步骤2：采集陀螺及加速度计在位置1(初始位置)的输出信号；

通过前面的分析，结合图2与图3，由(3)～(6)式可得陀螺及加速度计在位置1(初始位置)的输出信号为：

${\mathrm{\&omega;}}_{x\left(1\right)}={\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}cos\mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}+{\mathrm{\&epsiv;}}_0^x+{\mathrm{\&epsiv;}}_{\left(1\right)}^x$

${\mathrm{\&omega;}}_{y\left(1\right)}={\mathrm{\&omega;}}_N(cos\mathrm{\&alpha;}sin\mathrm{\&theta;}sin\mathrm{\&gamma;}-sin\mathrm{\&alpha;}cos\mathrm{\&gamma;})+{\mathrm{\&omega;}}_{T}cos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&epsiv;}}_0^y+{\mathrm{\&epsiv;}}_{\left(1\right)}^y$

$a_{x\left(1\right)}=gsin\mathrm{\&theta;}+{\mathrm{\&xi;}}_0^x+{\mathrm{\&xi;}}_{\left(1\right)}^x$

$a_{y\left(1\right)}=-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&xi;}}_0^y+{\mathrm{\&xi;}}_{\left(1\right)}^y$

步骤3：采集陀螺及加速度计在位置2的输出信号；

通过力矩电机Mz控制转台R绕z_b轴转动任意角度30°，结合图4(其中：Ox_by_bz_b—陀螺载体坐标系；Ox_my_mz_m—位置1的机械转动系m；—位置2的机械转动系m¹；μ—转台转动角)和前面的分析，将假设条件代入(7)～(10)式可得陀螺及加速度计在位置2的输出信号为：

${\mathrm{\&omega;}}_{x\left(2\right)}=\frac{\sqrt{3}}{2}({\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}cos\mathrm{\&theta;}-{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;})+\frac{1}{2}\&lsqb;{\mathrm{\&omega;}}_N(cos\mathrm{\&alpha;}sin\mathrm{\&theta;}sin\mathrm{\&gamma;}-sin\mathrm{\&alpha;}cos\mathrm{\&gamma;})+{\mathrm{\&omega;}}_{T}cos\mathrm{\&theta;}sin\mathrm{\&gamma;}\&rsqb;+{\mathrm{\&epsiv;}}_{x0}+{\mathrm{\&epsiv;}}_{\left(2\right)}^x$

${\mathrm{\&omega;}}_{y\left(2\right)}=-\frac{1}{2}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\frac{\sqrt{3}}{2}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;+{\mathrm{\&epsiv;}}_{y0}+{\mathrm{\&epsiv;}}_{\left(2\right)}^y$

$a_{x\left(2\right)}=\frac{\sqrt{3}}{2}gsin\mathrm{\&theta;}-\frac{1}{2}gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&xi;}}_0^x+{\mathrm{\&xi;}}_{\left(2\right)}^x$

$a_{y\left(2\right)}=-\frac{1}{2}gsin\mathrm{\&theta;}-\frac{\sqrt{3}}{2}gcos\mathrm{\&theta;}\sin \mathrm{\&gamma;}+{\mathrm{\&xi;}}_0^y+{\mathrm{\&xi;}}_{\left(2\right)}^y$

步骤4：对陀螺常值漂移和加速度计零偏进行补偿；

步骤4.1：陀螺常值漂移的补偿。根据陀螺常值漂移数值不变特性，把上述所测陀螺在两个位置的输出信号代入式(11)、(12)可得

$\begin{array}{c}{\mathrm{\&omega;}}_x={\mathrm{\&omega;}}_{x\left(1\right)}-{\mathrm{\&omega;}}_{x\left(2\right)}={\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}-\\ \{\frac{\sqrt{3}}{2}({\mathrm{\&omega;}}_N\cos \cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\frac{1}{2}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;\}\end{array}$

$\begin{array}{c}{\mathrm{\&omega;}}_y={\mathrm{\&omega;}}_{y\left(1\right)}-{\mathrm{\&omega;}}_{y\left(2\right)}={\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\\ \{-\frac{1}{2}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\frac{\sqrt{3}}{2}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;\}\end{array}$

步骤4.2：加速度计零偏的补偿。根据陀螺常值漂移数值不变特性，把上述所测加速度计在两个位置的输出信号代入式(13)、(14)可得

${\mathrm{\&alpha;}}_x={\mathrm{\&alpha;}}_{x\left(1\right)}-{\mathrm{\&alpha;}}_{x\left(2\right)}=gsin\mathrm{\&theta;}-(\frac{\sqrt{3}}{2}gsin\mathrm{\&theta;}-\frac{1}{2}gcos\mathrm{\&theta;}sin\mathrm{\&gamma;})$

${\mathrm{\&alpha;}}_y={\mathrm{\&alpha;}}_{y\left(1\right)}-{\mathrm{\&alpha;}}_{y\left(2\right)}=-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}-(-\frac{1}{2}gsin\mathrm{\&theta;}-\frac{\sqrt{3}}{2}gcos\mathrm{\&theta;}sin\mathrm{\&gamma;})$

步骤5：计算真北方位角；

根据上面的分析，结合式(15)，可得真北方位角

$\mathrm{\&alpha;}=arctan\left\{\frac{{\mathrm{\&omega;}}_x(sin\mathrm{\&theta;}sin\mathrm{\&gamma;}+\frac{1}{2}cos\mathrm{\&theta;}-\frac{\sqrt{3}}{2}sin\mathrm{\&theta;}sin\mathrm{\&gamma;})-{\mathrm{\&omega;}}_y(cos\mathrm{\&theta;}-\frac{\sqrt{3}}{2}cos\mathrm{\&theta;}-\frac{1}{2}sin\mathrm{\&theta;}sin\mathrm{\&gamma;})+{\mathrm{\&omega;}}_{T}sin\mathrm{\&gamma;}(2-\sqrt{3})}{\&lsqb;(1-\frac{\sqrt{3}}{2}){\mathrm{\&omega;}}_x+\frac{1}{2}{\mathrm{\&omega;}}_y+(2-\sqrt{3}){\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;}\&rsqb;cos\mathrm{\&gamma;}}\right\}$

实施例2：

如图4所示，设转动角度μ＝60°(即在相差60°的两位置进行寻北解算)，参见图1、2、3、4，本发明任意二位置捷联寻北解算方法具体步骤为：

步骤1：寻北仪通电、系统粗调平；

根据上面步骤1的说明和图1所示，对寻北仪进行通电，根据加速度计A_x和A_y的输出信号对整个系统进行粗略调平。

步骤2：采集陀螺及加速度计在位置1(初始位置)的输出信号；

通过前面的分析，结合图2与图3，由(3)～(6)式可得陀螺及加速度计在位置1(初始位置)的输出信号为：

${\mathrm{\&omega;}}_{x\left(1\right)}={\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}cos\mathrm{\&theta;}-{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;}+{\mathrm{\&epsiv;}}_0^x+{\mathrm{\&epsiv;}}_{\left(1\right)}^x$

${\mathrm{\&omega;}}_{y\left(1\right)}={\mathrm{\&omega;}}_N(cos\mathrm{\&alpha;}sin\mathrm{\&theta;}sin\mathrm{\&gamma;}-sin\mathrm{\&alpha;}cos\mathrm{\&gamma;})+{\mathrm{\&omega;}}_{T}cos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&epsiv;}}_0^y+{\mathrm{\&epsiv;}}_{\left(1\right)}^y$

$a_{x\left(1\right)}=gsin\mathrm{\&theta;}+{\mathrm{\&xi;}}_0^x+{\mathrm{\&xi;}}_{\left(1\right)}^x$

$a_{y\left(1\right)}=-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&xi;}}_0^y+{\mathrm{\&xi;}}_{\left(1\right)}^y$

步骤3：采集陀螺及加速度计在位置2的输出信号；

通过力矩电机Mz控制转台R绕z_b轴转动任意角度60°，结合图4和前面的分析，将假设条件代入(7)～(10)式可得陀螺及加速度计在位置2的输出信号为：

${\mathrm{\&omega;}}_{x\left(2\right)}=\frac{1}{2}({\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}cos\mathrm{\&theta;}-{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;})+\frac{\sqrt{3}}{2}\&lsqb;{\mathrm{\&omega;}}_N(cos\mathrm{\&alpha;}sin\mathrm{\&theta;}\sin \mathrm{\&gamma;}-sin\mathrm{\&alpha;}cos\mathrm{\&gamma;})+{\mathrm{\&omega;}}_{T}cos\mathrm{\&theta;}sin\mathrm{\&gamma;}\&rsqb;+{\mathrm{\&epsiv;}}_{x0}+{\mathrm{\&epsiv;}}_{\left(2\right)}^x$

${\mathrm{\&omega;}}_{y\left(2\right)}=-\frac{\sqrt{3}}{2}({\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}cos\mathrm{\&theta;}-{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;})+\frac{1}{2}\&lsqb;{\mathrm{\&omega;}}_N(cos\mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}cos\mathrm{\&gamma;})+{\mathrm{\&omega;}}_{T}cos\mathrm{\&theta;}sin\mathrm{\&gamma;}\&rsqb;+{\mathrm{\&epsiv;}}_{y0}+{\mathrm{\&epsiv;}}_{\left(2\right)}^y$

$a_{x\left(2\right)}=\frac{1}{2}gsin\mathrm{\&theta;}-\frac{\sqrt{3}}{2}gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&xi;}}_0^x+{\mathrm{\&xi;}}_{\left(2\right)}^x$

$a_{y\left(2\right)}=-\frac{\sqrt{3}}{2}gsin\mathrm{\&theta;}-\frac{1}{2}gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&xi;}}_0^y+{\mathrm{\&xi;}}_{\left(2\right)}^y$

步骤4：对陀螺常值漂移和加速度计零偏进行补偿；

步骤4.1：陀螺常值漂移的补偿。根据陀螺常值漂移数值不变特性，把上述所测陀螺在两个位置的输出信号代入式(11)、(12)可得

$\begin{array}{c}{\mathrm{\&omega;}}_y={\mathrm{\&omega;}}_{x\left(1\right)}-{\mathrm{\&omega;}}_{x\left(2\right)}={\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;}-\\ \{\frac{1}{2}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\frac{\sqrt{3}}{2}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;\}\end{array}$

$\begin{array}{c}{\mathrm{\&omega;}}_y={\mathrm{\&omega;}}_{y\left(1\right)}-{\mathrm{\&omega;}}_{y\left(2\right)}={\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\\ \{-\frac{\sqrt{3}}{2}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\frac{1}{2}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;\}\end{array}$

步骤4.2：加速度计零偏的补偿。根据陀螺常值漂移数值不变特性，把上述所测加速度计在两个位置的输出信号代入式(13)、(14)可得

${\mathrm{\&alpha;}}_x={\mathrm{\&alpha;}}_{x\left(1\right)}-{\mathrm{\&alpha;}}_{x\left(2\right)}=gsin\mathrm{\&theta;}-(\frac{1}{2}gsin\mathrm{\&theta;}-\frac{\sqrt{3}}{2}gcos\mathrm{\&theta;}sin\mathrm{\&gamma;})$

${\mathrm{\&alpha;}}_y={\mathrm{\&alpha;}}_{y\left(1\right)}-{\mathrm{\&alpha;}}_{y\left(2\right)}=-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}-(-\frac{\sqrt{3}}{2}g\sin \mathrm{\&theta;}-\frac{1}{2}gcos\mathrm{\&theta;}sin\mathrm{\&gamma;})$

步骤5：计算真北方位角；

根据上面的分析，结合式(15)，可得真北方位角

$\mathrm{\&alpha;}=arctan\left\{\frac{{\mathrm{\&omega;}}_x(\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\frac{\sqrt{3}}{2}\cos \mathrm{\&theta;}-\frac{1}{2}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})-{\mathrm{\&omega;}}_y(\cos \mathrm{\&theta;}-\frac{1}{2}\cos \mathrm{\&theta;}-\frac{\sqrt{3}}{2}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\sin \mathrm{\&gamma;}}{(\frac{1}{2}{\mathrm{\&omega;}}_x+\frac{\sqrt{3}}{2}{\mathrm{\&omega;}}_y+{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})\cos \mathrm{\&gamma;}}\right\}$

实施例3：

如图2、4所示，设俯仰角θ＝0、横滚角γ＝0、转动角度μ＝90°(即水平状态下，在相差90°的两个位置进行寻北测量)，参见图1、2、3、4，本发明任意二位置捷联寻北解算方法具体步骤为：

步骤1：寻北仪通电、系统粗调平；

根据上面步骤1的说明和图1所示，对寻北仪进行通电，由于假设条件俯仰角θ＝0、横滚角γ＝0，故无需再调平。

步骤2：采集陀螺在位置1(初始位置)的输出信号；

由于假设条件中已包含姿态角，这里可不需对加速度计进行采集信号，只需采集陀螺的输出信号，通过前面的分析，结合图2与图3，将假设条件代入(3)、(4)式可得

${\mathrm{\&omega;}}_{x\left(1\right)}={\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}+{\mathrm{\&epsiv;}}_0^x+{\mathrm{\&epsiv;}}_{\left(1\right)}^x$

${\mathrm{\&omega;}}_{y\left(1\right)}=-{\mathrm{\&omega;}}_{N}sin\mathrm{\&alpha;}+{\mathrm{\&epsiv;}}_0^y+{\mathrm{\&epsiv;}}_{\left(1\right)}^y$

步骤3：采集陀螺在位置2的输出信号；

通过力矩电机Mz控制转台R绕z_b轴转动任意角度90°，结合图4，将假设条件代入(7)、(8)式可得

${\mathrm{\&omega;}}_{x\left(2\right)}=\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}-\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_{N}sin\mathrm{\&alpha;}+{\mathrm{\&epsiv;}}_{x0}+{\mathrm{\&epsiv;}}_{\left(2\right)}^x$

${\mathrm{\&omega;}}_{y\left(2\right)}=-\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}-\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_{N}sin\mathrm{\&alpha;}+{\mathrm{\&epsiv;}}_{y0}+{\mathrm{\&epsiv;}}_{\left(2\right)}^y$

步骤4：对陀螺常值漂移进行补偿；

根据陀螺常值漂移数值不变特性，把上述所测陀螺输出信号代入式(11)、(12)可得

${\mathrm{\&omega;}}_x={\mathrm{\&omega;}}_{x\left(1\right)}-{\mathrm{\&omega;}}_{x\left(2\right)}={\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}-\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}+\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_{N}sin\mathrm{\&alpha;}$

${\mathrm{\&omega;}}_y={\mathrm{\&omega;}}_{y\left(1\right)}-{\mathrm{\&omega;}}_{y\left(2\right)}=-{\mathrm{\&omega;}}_{N}sin\mathrm{\&alpha;}+\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}+\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_{N}sin\mathrm{\&alpha;}$

步骤5：计算真北方位角；

结合式(15)，联立步骤4的两式可得真北方位角

$\mathrm{\&alpha;}=\arctan \&lsqb;\frac{\frac{\sqrt{2}}{2}{\mathrm{\&omega;}}_x-(1-\frac{\sqrt{2}}{2}){\mathrm{\&omega;}}_y}{(1-\frac{\sqrt{2}}{2}){\mathrm{\&omega;}}_x}\&rsqb;$

Claims (1)

1.一种任意二位置捷联寻北方法,其特征在于：包括以下步骤：

步骤1：寻北仪通电、系统粗调平；

步骤2：采集陀螺及加速度计在位置1的输出信号；

步骤2.1：确定惯性坐标系与地理坐标系之间的方位关系：

惯性坐标系Ox_iy_iz_i记为i系，地理坐标系Ox_ny_nz_n(ONWT)记为n系，ON轴指北，OW轴指西，OT轴指天；ω_ie表示地球自转角速度，ω_N和ω_T分别表示地球自转角速度在n系中北向分量和天顶分量： 表示当地纬度；

步骤2.2：确定地理坐标系与载体坐标系之间的方位关系：

$\begin{array}{c}{C}_n^b=C_2^b\&CenterDot;C_1^2\&CenterDot;C_n^1\\ =\left[\begin{array}{ccc}\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}& \sin \mathrm{\&alpha;}\cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ \cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& \sin \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\\ \cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&alpha;}\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}\end{array}\right]\end{array}---\left(1\right)$

其中表示从n系旋转到坐标系Ox₁y₁z_n的方向余弦矩阵，表示从坐标系Ox₁y₁z_n旋转到坐标系Ox_by₁z₁的方向余弦矩阵，表示从坐标系Ox_by₁z₁旋转到b系的方向余弦矩阵；

步骤2.3：陀螺测得的角速度在陀螺载体坐标系的投影向量为：

${\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}^b=\left[\begin{array}{c}{\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z\end{array}\right]=C_n^b{\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{ie}^n+{\stackrel{\&RightArrow;}{\mathrm{\&epsiv;}}}_0+\stackrel{\&RightArrow;}{\mathrm{\&epsiv;}}=C_n^b\left[\begin{array}{c}{\mathrm{\&omega;}}_N\\ 0\\ {\mathrm{\&omega;}}_T\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&epsiv;}}_0^x\\ {\mathrm{\&epsiv;}}_0^y\\ {\mathrm{\&epsiv;}}_0^z\end{array}\right]+\left[\begin{array}{c}{\mathrm{\&epsiv;}}^x\\ {\mathrm{\&epsiv;}}^y\\ {\mathrm{\&epsiv;}}^z\end{array}\right]---\left(2\right)$

式中分别表示陀螺x、y、z敏感轴的常值漂移，ε^x、ε^y、ε^z分别表示陀螺x、y、z敏感轴的随机漂移；把(1)式代入(2)式可得陀螺在位置1(初始位置)测得的角速度为：

${\mathrm{\&omega;}}_{x\left(1\right)}={\mathrm{\&omega;}}_{N}cos\mathrm{\&alpha;}cos\mathrm{\&theta;}-{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;}+{\mathrm{\&epsiv;}}_0^x+{\mathrm{\&epsiv;}}_{\left(1\right)}^x---\left(3\right)$

${\mathrm{\&omega;}}_{y\left(1\right)}={\mathrm{\&omega;}}_N(cos\mathrm{\&alpha;}sin\mathrm{\&theta;}sin\mathrm{\&gamma;}-sin\mathrm{\&alpha;}cos\mathrm{\&gamma;})+{\mathrm{\&omega;}}_{T}cos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&epsiv;}}_0^y+{\mathrm{\&epsiv;}}_{\left(1\right)}^y---\left(4\right)$

式中分别表示陀螺x、y敏感轴在位置1测量数据的随机漂移；

步骤2.4：同理可得加速度计在位置1的测得值为：

$a_{x\left(1\right)}=gsin\mathrm{\&theta;}+{\mathrm{\&xi;}}_0^x+{\mathrm{\&xi;}}_{\left(1\right)}^x---\left(5\right)$

$a_{y\left(1\right)}=-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}+{\mathrm{\&xi;}}_0^y+{\mathrm{\&xi;}}_{\left(1\right)}^y---\left(6\right)$

式中分别表示加速度计A_x、A_y的零偏，分别表示加速度计A_x、A_y在位置1测量数据的随机漂移；

步骤3：采集陀螺及加速度计在位置2的输出信号；

步骤3.1：对初始位置陀螺和加速度计的输出信号采集完毕后，通过力矩电机Mz控制转台R绕z_b轴转动任意角度μ，设在初始状态下机械转动系为m系，与载体坐标系b系重合，转动后的机械转动系为m¹系，则m系到m¹系之间的方向余弦矩阵为：

$C_m^{m^1}=\left[\begin{array}{ccc}cos\mathrm{\&mu;}& sin\mathrm{\&mu;}& 0\\ -sin\mathrm{\&mu;}& \cos \mathrm{\&mu;}& 0\\ 0& 0& 1\end{array}\right]$

则可得位置2陀螺的理论输出角速度为：

${\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{m^1}=C_m^{m^1}\&CenterDot;C_n^b\&CenterDot;{\stackrel{\&RightArrow;}{\mathrm{\&omega;}}}_{ie}^n=C_m^{m^1}\&CenterDot;C_n^b\left[\begin{array}{c}{\mathrm{\&omega;}}_N\\ 0\\ {\mathrm{\&omega;}}_T\end{array}\right]$

步骤3.2：考虑陀螺常值漂移和随机漂移项，可得陀螺在位置2测得的角速度为：

$\begin{array}{c}{\mathrm{\&omega;}}_{x\left(2\right)}=\cos \mathrm{\&mu;}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\\ \sin \mathrm{\&mu;}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;+{\mathrm{\&epsiv;}}_0^x+{\mathrm{\&epsiv;}}_{\left(2\right)}^x\end{array}---\left(7\right)$

$\begin{array}{c}{\mathrm{\&omega;}}_{y\left(2\right)}=-\sin \mathrm{\&mu;}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\\ \cos \mathrm{\&mu;}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;+{\mathrm{\&epsiv;}}_0^y+{\mathrm{\&epsiv;}}_{\left(2\right)}^y\end{array}---\left(8\right)$

式中分别表示陀螺x、y敏感轴在位置2测量数据的随机漂移；

步骤3.3：同理可得加速度计在位置2测得的值为：

$a_{x\left(2\right)}=gsin\mathrm{\&theta;}cos\mathrm{\&mu;}-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}sin\mathrm{\&mu;}+{\mathrm{\&xi;}}_0^x+{\mathrm{\&xi;}}_{\left(2\right)}^x---\left(9\right)$

$a_{y\left(2\right)}=-gsin\mathrm{\&theta;}sin\mathrm{\&mu;}-gcos\mathrm{\&theta;}sin\mathrm{\&gamma;}cos\mathrm{\&mu;}+{\mathrm{\&xi;}}_0^y+{\mathrm{\&xi;}}_{\left(2\right)}^y---\left(10\right)$

式中分别表示加速度计A_x、A_y在位置2测量数据的随机漂移；

步骤4：对陀螺常值漂移和加速度计零偏进行补偿

步骤4.1：陀螺常值漂移的补偿；把式(3)、(4)和式(7)、(8)分别对应相减并忽略随机漂移可得：

$\begin{array}{c}{\mathrm{\&omega;}}_x={\mathrm{\&omega;}}_{x\left(1\right)}-{\mathrm{\&omega;}}_{x\left(2\right)}={\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}-\\ \{\cos \mathrm{\&mu;}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\sin \mathrm{\&mu;}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;\}\end{array}---\left(11\right)$

$\begin{array}{c}{\mathrm{\&omega;}}_y={\mathrm{\&omega;}}_{y\left(1\right)}-{\mathrm{\&omega;}}_{y\left(2\right)}={\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\\ \{-\sin \mathrm{\&mu;}({\mathrm{\&omega;}}_N\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}-{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;})+\cos \mathrm{\&mu;}\&lsqb;{\mathrm{\&omega;}}_N(\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}-\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;})+{\mathrm{\&omega;}}_T\cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}\&rsqb;\}\end{array}---\left(12\right)$

步骤4.2：加速度计零偏的补偿；把式(5)、(6)和式(9)、(10)分别对应相减并忽略随机漂移可得

α_x＝α_x(1)-α_x(2)＝g sinθ-(g sinθcosμ-g cosθsinγsinμ) (13)

α_y＝α_y(1)-α_y(2)＝-g cosθsinγ-(-g sinθsinμ-g cosθsinγcosμ) (14)

步骤5：计算真北方位角；

$\sin \mathrm{\&alpha;}=\frac{{\mathrm{\&omega;}}_x(\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&mu;}\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})-{\mathrm{\&omega;}}_y(\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\cos \mathrm{\&theta;}-\sin \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})+2{\mathrm{\&omega;}}_T\sin \mathrm{\&gamma;}(1-\cos \mathrm{\&mu;})}{2{\mathrm{\&omega;}}_N\cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}(1-\cos \mathrm{\&mu;})}$

$\cos \mathrm{\&alpha;}=\frac{{\mathrm{\&omega;}}_x(\cos \mathrm{\&mu;}-1)-{\mathrm{\&omega;}}_y\sin \mathrm{\&mu;}+2{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}(\cos \mathrm{\&mu;}-1)}{2{\mathrm{\&omega;}}_N\cos \mathrm{\&theta;}(\cos \mathrm{\&mu;}-1)}$

故可得方位角：

$\tan \mathrm{\&alpha;}=\frac{{\mathrm{\&omega;}}_x(\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&mu;}\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})-{\mathrm{\&omega;}}_y(\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\cos \mathrm{\&theta;}-\sin \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})+2{\mathrm{\&omega;}}_T\sin \mathrm{\&gamma;}(1-\cos \mathrm{\&mu;})}{-\&lsqb;{\mathrm{\&omega;}}_x(\cos \mathrm{\&mu;}-1)-{\mathrm{\&omega;}}_y\sin \mathrm{\&mu;}+2{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}(\cos \mathrm{\&mu;}-1)\&rsqb;\cos \mathrm{\&gamma;}}$

$\mathrm{\&alpha;}=\arctan \left\{\frac{{\mathrm{\&omega;}}_x(\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&mu;}\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})-{\mathrm{\&omega;}}_y(\cos \mathrm{\&theta;}-\cos \mathrm{\&mu;}\cos \mathrm{\&theta;}-\sin \mathrm{\&mu;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;})+2{\mathrm{\&omega;}}_T\sin \mathrm{\&gamma;}(1-\cos \mathrm{\&mu;})}{-\&lsqb;{\mathrm{\&omega;}}_x(\cos \mathrm{\&mu;}-1)-{\mathrm{\&omega;}}_y\sin \mathrm{\&mu;}+2{\mathrm{\&omega;}}_T\sin \mathrm{\&theta;}(\cos \mathrm{\&mu;}-1)\&rsqb;\cos \mathrm{\&gamma;}}\right\}---\left(15\right)$

式中ω_x、ω_y分别为陀螺x、y敏感轴在任意两个位置上的测量数据之差；θ、γ分别表示俯仰角和横滚角，可由式(13)、(14)联立求的；

对于下述三种特殊情况有：

(1)θ＝0、γ＝0(水平状态下)；

$\mathrm{\&alpha;}=\arctan \&lsqb;\frac{{\mathrm{\&omega;}}_{x}sin\mathrm{\&mu;}-{\mathrm{\&omega;}}_y(1-cos\mathrm{\&mu;})}{{\mathrm{\&omega;}}_x(1-cos\mathrm{\&mu;})+{\mathrm{\&omega;}}_{y}sin\mathrm{\&mu;}}\&rsqb;$

(2)μ＝180°(对径180°两位置测量，即传统二位置法)；

$\mathrm{\&alpha;}=\arctan \&lsqb;\frac{{\mathrm{\&omega;}}_{x}sin\mathrm{\&theta;}sin\mathrm{\&gamma;}-{\mathrm{\&omega;}}_{y}cos\mathrm{\&theta;}+2{\mathrm{\&omega;}}_{T}sin\mathrm{\&gamma;}}{({\mathrm{\&omega;}}_x+2{\mathrm{\&omega;}}_{T}sin\mathrm{\&theta;})cos\mathrm{\&gamma;}}\&rsqb;$

(3)θ＝0、γ＝0且μ＝180°(水平状态下的传统二位置法)；

$\mathrm{\&alpha;}=\arctan (-\frac{{\mathrm{\&omega;}}_y}{{\mathrm{\&omega;}}_x}).$