CN102840856B - 一种动态旋转调制的陀螺仪寻北方法 - Google Patents

Abstract

本发明属于惯性测量技术或地球物理实验与仪器领域，具体涉及一种动态旋转调制的陀螺仪寻北方法。目的是在载体存在大姿态角的条件下，且无需调平、无需知道测点纬度的情况下实现快速、精确寻北测量，提高测地作业速度。它使用以角速度Ω匀速转动的转台，转台上安装1个陀螺仪，2个加速度计；首先，利用2个加速度计求解：俯仰角和横滚角；然后，利用陀螺仪求解地理纬度L和航向角；在计算得到初值之后，采用实时递推的最小二乘方法算法提高精度。本发明的优点是无需调平、无需知道测点纬度的情况下实现快速、精确寻北测量，提高测地作业速度。

Description

一种动态旋转调制的陀螺仪寻北方法

技术领域

本发明属于惯性测量技术或地球物理实验与仪器领域，主要涉及一种动态旋转调制的陀螺仪寻北方法。

背景技术

中国专利申请CN90103687.0公开了我国航空航天部第一设计研究院十五所发明的一种寻北仪。该寻北仪采用安装在转台上的一个挠性陀螺和一个加速度计作为检测元器件，通过挠性陀螺检测出地球自转角速度在多个位置上的分量，结合同一位置的加速度计输出量，解算出初始位置与北向的夹角。采用步进电机驱动转台转动，实现多测量位置的采样。在步进电机的驱动中，采用模拟电路板作为步进电机的驱动控制模块，采用光电脉冲发生器和位置记忆器记录配合使用，以确定单轴转台的零位和其他转动位置。并且每次寻北时，该寻北仪都要进行零位自动调整，即归零运行。此外，该寻北仪的零位指示线位于壳体上，因此，大都用于载体自身的寻北，很少用于对目标进行寻北。并且要实现转位机构控制惯性测量单元在较短的时间内分别停在不同的位置进行数据采集，这在工程实现上较难，因此其寻北时间较长。

中国专利号ZL200610043115.4公开中国兵器工业第二零五研究所发明的一种采用光纤陀螺作为敏感元件的单轴光纤陀螺寻北仪，该寻北仪包括作为敏感元件的单轴光纤陀螺、转轴、方向指示线、步进电机、单片机、寻北解算计算机，光纤陀螺、方向指示线均固连在转轴上，步进电机可以带动转轴转动，以此可以获得光纤陀螺在不同位置的输出信号，单片机主要完成对转轴转动的控制，寻北解算计算机主要完成光纤陀螺仪在四个等间隔位置上输出数据的采样，并利用其内置的数学模型对采样信号进行处理以及解算出寻北结果，它既可以完成单纯的寻北功能，也可以完成与目标连线相对于北向的夹角的测量。由于该寻北仪只采样四个位置的数据，较前一种寻北仪的减少了采样位置，因此在一定程度上提高了寻北速度。但减少采样位置，很难达到较高的寻北精度。并且控制陀螺分别停在四个等间隔的不同位置上，再采集陀螺数据，同样需要花费不少于5分钟的寻北时间。并且采用方向指示线输出寻北结果，精度也不会太高，无法与采用光电装置的专用测角仪——全站仪相提并论。

为了进一步提高寻北精度，缩短寻北时间，近年来兴起了一种高精度动态旋转调制寻北技术。它的基本原理是在计算机控制和电机的带动下，惯性测量单元(IMU)以恒定的角速度连续旋转，计算机同时采集陀螺仪敏感轴的数据和加速度计的数据，读取测角装置的值作为当前角度，由寻北计算机解算出偏北方位角。该方法可以将陀螺仪的漂移抑制成转速的倒数倍，并消除了陀螺仪刻度因子对寻北精度的影响，大大提高了同等指标的陀螺仪的寻北精度和速度。文献[1]、[2]、[3]、[4]、[5]、[6]、[7]是国内目前在刊物上发表的部分文章。文献[1、2]介绍了一种挠性陀螺构成的连续旋转寻北方案，对旋转状态下的寻北误差进行了分析，建立起了连续旋转式寻北仪的数学模型，并进行了仿真研究。文献[4]研究了一种旋转式激光陀螺寻北仪的算法，并指出文献[1、2]在仿真时没有给出陀螺仪的精度指标，对其仿真研究的结论表示怀疑；因此文献[4]认为该方法很难实现，转而研究多位置法来实现寻北。文献[6]、[7]对利用光纤陀螺进行动态寻北的原理和算法进行了仿真研究，其指标是寻北时间12min，寻北精度0.2°。综合以上情况，表明国内对连续旋转调制寻北方法的研究，还处于探索阶段，对其原理介绍的较多，而对其进行系统研究的还较少。

[1]邹向阳，孙谦，陈家斌，徐建华.连续旋转式寻北仪的寻北算法及信号处理[J].北京理工大学学报，2004，24(9)：801-807.

[2]徐建华，谢玲，高亚楠，陈家斌.旋转调制式寻北仪滤波技术研究[J].北京理工大学学报，2005，25(80：718-721.

[3]安道启.连续旋转式寻北仪的研制[D].北京理工大学硕士研究生论文，2006.

[4]张思将，秦石乔，王省书.旋转式激光陀螺寻北仪寻北算法研究[J].航空兵器，2006，(1)：12-14，40.

[5]王昌平.旋转式激光陀螺寻北仪误差建模与仿真研究[D].国防科学技术大学研究生院项士学位论文，2005.

[6]刘东波，刘建业，赖际舟.基于光纤陀螺的单周期快速动态寻北算法研究[J].传感器与微系统，2007，26(11)：61-64.

[7]刘东波.光纤陀螺快速高精度寻北技术与试验研究[D].南京航空航天大学，2007.

发明内容

本发明的目的是一种动态旋转调制的陀螺仪寻北方法，以便在载体存在大姿态角的条件下，且无需调平、无需知道测点纬度的情况下实现快速、精确寻北测量，提高测地作业速度。

本发明是这样实现的：一种动态旋转调制的陀螺仪寻北方法，其中，

使用以角速度Ω匀速转动的转台，转台上安装1个陀螺仪，2个加速度计；

转台以垂直方向为轴旋转；所述陀螺仪水平安装在支架安装孔内，其敏感轴线与转轴正交，且转台过零位时，陀螺测量坐标系x轴方向指向光电圆光栅尺读数头的零位线；

2只加速度计3分别沿双轴陀螺的两个敏感轴的方向，垂直安装在单轴转台支架上方互相垂直的两个安装孔内；

设东北天坐标系(OXnYnZn)(n系)；旋转台体坐标系(OXbYbZb)(b系)的Xb轴指向光栅零位，Yb轴指向光栅90°方向，Zb轴指向旋转轴；陀螺测量坐标系(OXmYmZm)(m系)的Xm轴指向加速度计1，Ym方向指向加速度计2，Zm轴指向旋转轴；当地地理纬度为L；寻北仪台体的俯仰/横滚/航向角分别为θ、γ、φ，求解过程包括如下步骤：

首先，利用加速度计求解：俯仰角和横滚角；

当惯性测量单元(IMU)相对台体的瞬时角位置为α＝Ω·t+φ₀时，其中Ω为台体转速，φ₀为初始相位，读取加速度计测量值为(f_mx、f_my)，变换到台体坐标系后，x轴、y轴上的比力为：

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{mx}}\cos \mathrm{\α}-f_{\mathrm{my}}\sin \mathrm{\α}\\ f_{\mathrm{mx}}\sin \mathrm{\α}+f_{\mathrm{my}}\cos \mathrm{\α}\end{array}\right]$

则

θ＝sin^-1(f_by/g)

γ＝sin^-1(-f_bx/g/cosθ)

然后，利用陀螺仪求解地理纬度L和航向角Φ；

本发明的粗寻北方案采用的就是正交解调法，其数学原理是：

ω_g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω_esinα

由于：+(cosθcosφcosL+sinθsinL)Ω_ecosα

设姿态角θ、γ均为0时，可以简化为：

ω_g＝(sinφcosα+cosφsinα)Ω_ecosL

＝Ω_ecosL·sin(α+φ)

令：A＝Ω_ecosL·cosφ，B＝Ω_ecosL·sinφ，

则有：

ω_g＝(Asinα+Bcosα)

分别用sinα、cosα(α＝Ω·t+φ₀)乘以上式两端然后积分可得：

$\left\{\begin{array}{c}\hat{A}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\\ \hat{B}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\end{array}\right.$

于是有： ${\widehat{\mathrm{\φ}}}_d=\mathrm{atc}\tan (\hat{B}/\hat{A}),$ $L={\cos }^{-1}\left(\sqrt{({\hat{A}}^2+{\hat{B}}^2)/2{\mathrm{\Ω}}_e}\right)$

即转台旋转一周后即可给出真北方位的概略值。

如上所述的一种动态旋转调制的陀螺仪寻北方法，其中，在计算得到初值之后，采用实时递推的最小二乘方法算法提高精度：

线性方程为

Z_k＝H_kX_k-1+N_k

其中N_k为量测噪声，H_k为量测矩阵；Z_k为量测量，X_k-1为待估计量；

观测方程如下：

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X_k＝[φ-φ₀ L-L₀ E]^T＝[Δφ ΔL E]^T

采用递推最小二乘法求解：[Δφ ΔL E]^T，E为陀螺常值漂移；

(1)设定初值X₀，P₀，

X₀由粗寻北的真北方位概略值得到；

(2)根据观测方程，计算观测矩阵H_k和观测量Z_k，并确定新观测量的加权系数W_k；

(3)递推计算

$P_k=P_{k-1}-\frac{P_{k-1}{H}_k{H}_k^T{P}_{k-1}}{W_k^{-1}+H_k^T{P}_{k-1}{H}_k}$

K_k＝P_kH_kW_k

X_k＝X_k-1+K_k(Z_k-H_kX_k-1)

得

[φ L E]^T＝[φ₀ L₀ 0]^T+[Δφ ΔL E]^T

因为式

ω_g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω_esinα+(cosθcosφcosL+sinθsinL)Ω_ecosα

是关于航向角和纬度的非线性方程，不能直接采用最小二乘方法进行求解，可以先将非线性方程在初值点泰勒展开，得到下式，即

${\mathrm{\ω}}_g^m-f({\mathrm{\φ}}_0,L_0)=\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0)}& \left(\mathrm{\φ}-{\mathrm{\φ}}_0\right)+\frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0)}& (L-L_0)+E\end{array}$

因此可令

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X_k＝[φ-φ₀ L-L₀ E]^T＝[Δφ ΔL E]^T

可得近似的线性方程为

Z_k＝H_kX_k-1+N_k

采用递推最小二乘法可以求得[Δφ ΔL E]^T，然后由

[φ L E]^T＝[φ₀ L₀ 0]^T+[Δφ ΔL E]^T

得到精确的寻北值。

本发明的优点是在解算方法上采用初值估算和精确解算相结合，初值估算模块根据转台旋转一圈时陀螺仪和加速度计的输出数据采用正交解调算法，精确寻北算法在粗寻北的基础上采用实时递推的最小二乘方法算法，减小陀螺仪、加速度计的系统误差，实现了实时解算出测点的偏北方位角、当地纬度，使得系统实现了快速、实时寻北的目的。

本发明解决了以下问题：

a.用两只加速度计测出载体姿态角，并用该数据补偿了载体姿态误差引起的寻北误差，因此无需限制陀螺敏仪感轴在近似水平状态下寻北，取消了类似陀螺稳定平台的复杂的调平机构；

b.无需外加纬度信号即可完成精确寻北；

c.在连续旋转状态下寻北，可消除陀螺及加速度计的系统误差，从而可以提高寻北精度；

d.在采用实时递推的最小二乘方法算法，减小陀螺仪、加速度计的系统误差，实现了实时解算出测点的偏北方位角、当地纬度的目的；

e.本发明不到30秒即可输出数据，且寻北精度优于10′(1δ)；每12秒刷新一次结果，精度随时间增加而提高，寻北时间2分，寻北精度优于1′；寻北时间5分，寻北精度30″，使用中可根据需要选择不同的寻北精度和时间。以上时间不含预热时间(3分钟)。

附图说明

图1是使用本发明所述的一种动态旋转调制的陀螺仪寻北方法的硬件平台结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步的说明：

下面将结合实施例进行详细说明：

如图1所示，此为寻北仪主机结构示意图；设东北天坐标系(OXnYnZn)(n系)；旋转台体坐标系(OXbYbZb)(b系)的Xb轴指向光栅零位(以此作为转台零位)，Yb轴指向光栅90°方向，Zb轴指向旋转轴；陀螺测量坐标系(OXmYmZm)(m系)的Xm轴指向加速度计1，Ym方向指向加速度计2，Zm轴指向旋转轴；当地地理纬度为L；寻北仪台体的俯仰/横滚/航向角分别为θ、γ、φ。

当惯性测量单元(IMU)相对台体的瞬时角位置为α＝Ω·t+φ₀(其中Ω为台体转速，φ₀为初始相位)旋转时。

台体各轴的角速度为

${\mathrm{\ω}}_m=T_b^m{T}_n^b{\mathrm{\ω}}_{\mathrm{ien}}=\left[\begin{array}{c}[(-\cos \mathrm{\γ}\sin \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ})\cos \mathrm{\α}-\left(\cos \mathrm{\θ}\cos \mathrm{\φ}\right)\sin \mathrm{\α}]{\mathrm{\Ω}}_e\cos L\\ ...............................+[(-\sin \mathrm{\γ}\cos \mathrm{\θ})\cos \mathrm{\α}-\left(\sin \mathrm{\θ}\right)\sin \mathrm{\α}]{\mathrm{\Ω}}_e\sin L\\ [(-\cos \mathrm{\γ}\sin \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ})\sin \mathrm{\α}+\left(\cos \mathrm{\θ}\cos \mathrm{\φ}\right)\cos \mathrm{\α}]{\mathrm{\Ω}}_e\cos L\\ ..........................+[+(-\sin \mathrm{\γ}\cos \mathrm{\θ})\sin \mathrm{\α}+\left(\sin \mathrm{\θ}\right)\sin \mathrm{\α}]{\mathrm{\Ω}}_e\sin L\\ (-\sin \mathrm{\γ}\sin \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}){\mathrm{\Ω}}_e\cos L+\left(\cos \mathrm{\γ}\cos \mathrm{\θ}\right){\mathrm{\Ω}}_e\sin L\end{array}\right]$

坐标变换矩阵：在n系上的地球自转角速度ω_ien；地球自转角速度常量：Ω_e。

陀螺敏感轴感测的角速度为

ω_g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω_esin α+(cosθcosφcosL+sinθsinL)Ω_ecosα                          (1)

此即陀螺仪敏感轴感测信号的表达式，它与台体的姿态角、航向角、陀螺敏感轴相对台体的旋转角及地理纬度有关，其中，姿态角(θ、γ)可利用加速度计测量得到。

设加速度计测量值为(f_mx、f_my)，变换到台体坐标系后，x轴、y轴上的比力为

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{mx}}\cos \mathrm{\α}-f_{\mathrm{my}}\sin \mathrm{\α}\\ f_{\mathrm{mx}}\sin \mathrm{\α}+f_{\mathrm{my}}\cos \mathrm{\α}\end{array}\right]---\left(2\right)$

则

θ＝sin^-1(f_by/g)

γ＝sin^-1(-f_bx/g/cosθ)                                  (3)

利用(3)求得台体的姿态角后，(1)中只含有航向角和纬度两个未知数，利用两个位置的陀螺测量就可以解算出航向角和纬度，而连续旋转测量使得解算结果更加精确。这就是寻北仪工作原理。

由上述过程可知，陀螺仪输出信号被调制为初始相位为φ的正弦信号。求解φ的方法有最小二乘法、正交解调法。

本发明的粗寻北方案采用的就是正交解调法，其数学原理是：

为分析方便，设姿态角θ、γ均为0时，(1)可以简化，其含义为：俯仰角和横滚角为0，寻北的条件下是用于粗寻北时是符合精度要求的，并且能提高解算速度：

ω_g＝(sinφcosα+cosφsinα)Ω_ecosL                      (4)

＝Ω_ecosL·sin(α+φ)

令：A＝Ω_ecosL·cosφ，B＝Ω_ecosL·sin φ，

则(4)改写为：

ω_g＝(Asinα+Bcosα)                              (5)

分别用sinα、cosα(α＝Ω·t+φ₀)乘以(5)两端然后积分可得：

$\left\{\begin{array}{c}\hat{A}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\\ \hat{B}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\end{array}\right.---\left(6\right)$

于是有： ${\widehat{\mathrm{\φ}}}_d=\mathrm{atc}\tan (\hat{B}/\hat{A}),$ $L={\cos }^{-1}\left(\sqrt{({\hat{A}}^2+{\hat{B}}^2)/2{\mathrm{\Ω}}_e}\right)$

即转台旋转一周后即可给出真北方位的概略值。

发明的精寻北方案采用的是实时递推的最小二乘方法算法，其数学原理是：

设线性方程为

Z_k＝H_kX_k-1+N_k                                     (7)

其中N_k为量测噪声，H_k为量测矩阵；Z_k为量测量，X_k-1为待估计量。

算法步骤

(4)设定初值X₀，P₀，X₀由粗寻北得到；

(5)根据观测方程，计算观测矩阵H_k和观测量Z_k，并确定新观测量的加权系数W_k

(6)递推计算

$P_k{=[P_{k-1}^{-1}+H_k{W}_k{H}_k^T]}^{-1}$

K_k＝P_kH_kW_k

X_k＝X_k-1+K_k(Z_k-H_kX_k-1)                            (8)

由于P_k阵的求取中需要进行两次矩阵求逆操作，为了避免矩阵求逆，采用下式递推，P₀的选择按照最小二乘法的公知选择方法进行即可，其中，初值选择越大，新息在在初期估计值中所占的比例越大，在初期没有先验知识的情况下应该以新的观测量提供的新息为主。

$P_k=P_{k-1}=\frac{P_{k-1}{H}_k{H}_k^T{P}_{k-1}}{W_k^{-1}+H_k^T{P}_{k-1}{H}_k}$

K_k＝P_kH_kW_k

X_k＝X_k-1+K_k(Z_k-H_kX_k-1)                                (9)

因为式(1)是关于航向角和纬度的非线性方程，不能直接采用最小二乘方法进行求解，可以先将非线性方程在初值点泰勒展开，即

${\mathrm{\ω}}_g^m-f({\mathrm{\φ}}_0,L_0)=\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0)}& \left(\mathrm{\φ}-{\mathrm{\φ}}_0\right)+\frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0)}& (L-L_0)+E\end{array}$

因此可令

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X_k＝[φ-φ₀ L-L₀ E]^T＝[Δφ ΔL E]^T                   (10)

可得近似的线性方程为

Z_k＝H_kX_k-1+N_k                                         (11)

采用递推最小二乘法可以求得[Δφ ΔL E]^T，得

[φ L E]^T＝[φ₀ L₀ 0]^T+[Δφ ΔL E]^T                  (12)

寻北过程中，使用显示装置接收解算数据，显示偏北角、当地纬度、陀螺漂移、寻北时间、姿态角，30秒即可输出数据，每12秒刷新一次结果，精度随时间增加提高。

Claims (2)

1.一种动态旋转调制的陀螺仪寻北方法，其特征在于：

使用以角速度Ω匀速转动的转台，转台上安装1个陀螺仪，2个加速度计；

转台以垂直方向为轴旋转；所述陀螺仪水平安装在支架安装孔内，其敏感轴线与转轴正交，且转台过零位时，陀螺测量坐标系x轴方向指向光电圆光栅尺读数头的零位线；

2只加速度计3分别沿双轴陀螺的两个敏感轴的方向，垂直安装在单轴转台支架上方互相垂直的两个安装孔内；

设东北天坐标系(OXnYnZn)(n系)；旋转台体坐标系(OXbYbZb)(b系)的Xb轴指向光栅零位，Yb轴指向光栅90°方向，Zb轴指向旋转轴；陀螺测量坐标系(OXmYmZm)(m系)的Xm轴指向加速度计1，Ym方向指向加速度计2，Zm轴指向旋转轴；当地地理纬度为L；寻北仪台体的俯仰/横滚/航向角分别为0、γ、φ，求解过程包括如下步骤：

首先，利用加速度计求解：俯仰角和横滚角；

当惯性测量单元IMU相对台体的瞬时角位置为α＝Ω·t+φ₀时，其中Ω为台体转速，φ₀为初始相位，读取加速度计测量值为f_mx、f_my，变换到台体坐标系后，x轴、y轴上的比力为：

$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{mx}}\cos \mathrm{\α}-f_{\mathrm{my}}\sin \mathrm{\α}\\ f_{\mathrm{mx}}\sin \mathrm{\α}+f_{\mathrm{my}}\cos \mathrm{\α}\end{array}\right]$

则

θ＝sin^-1(f_by/g)

γ＝sin^-1(-f_bx/g/cosθ)

然后，利用陀螺仪求解地理纬度L和航向角Φ；

所述寻北方法的粗寻北方案采用的就是正交解调法，其数学原理是：

ω_g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω_esinα

+(cosθcosφcosL+sinθsinL)Ω_ecosα

由于：设姿态角θ、γ均为0时，可以简化为：

ω_g＝(sinφcosα+cosφsinα)Ω_ecosL

＝Ω_ecosL·sin(α+φ)

令：A＝Ω_ecosL·cosφ，B＝Ω_ecosL·sinφ，

则有：

ω_g＝(Asinα+Bcosα)

分别用sinα、cosα(α＝Ω·t+φ₀)乘以上式，然后两端积分可得：

$\left\{\begin{array}{c}\hat{A}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\\ \hat{B}=\frac{\mathrm{\Ω}}{4}{\∫}_0^T{\mathrm{\ω}}_g\mathrm{sgn}\left[\cos \left(\mathrm{\Ωt}\right)\right]\mathrm{dt}\end{array}\right.$

于是有： ${\widehat{\mathrm{\φ}}}_d=\arctan (\hat{B}/\hat{A}),L={\cos }^{-1}\left(\sqrt{({\hat{A}}^2+{\hat{B}}^2)/2{\mathrm{\Ω}}_e}\right)$

即转台旋转一周后即可给出真北方位的概略值。

2.如权利要求1所述的一种动态旋转调制的陀螺仪寻北方法，其特征在于：在计算得到初值之后，采用实时递推的最小二乘方法算法提高精度：

线性方程为

Z_k＝H_kX_k-1+N_k

其中N_k为量测噪声，H_k为量测矩阵；Z_k为量测量，X_k-1为待估计量；

观测方程如下：

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X_k＝[φ-φ₀ L-L₀ E]^T＝[Δφ ΔL E]^T

采用递推最小二乘法求解：[Δφ ΔL E]^T，E为陀螺常值漂移；

(1)设定初值X₀，P₀，

X₀由粗寻北的真北方位概略值得到；

(2)根据观测方程，计算观测矩阵H_k和观测量Z_k，并确定新观测量的加权系数W_k；

(3)递推计算

$P_k=P_{k-1}-\frac{P_{k-1}{H}_k{H}_k^T{P}_{k-1}}{W_k^{-1}+H_k^T{P}_{k-1}{H}_k}$

K_k＝P_kH_kW_k

X_k＝X_k-1+K_k(Z_k-H_kX_K-1)

得

[φ L E]^T＝[φ₀ L₀ 0]^T+[Δφ ΔL E]^T

因为公式

ω_g＝-[(-cosγsinφ+sinγsinθcosφ)cosL-sinγcosθsinL]Ω_esinα+(cosθcosφcosL+sinθsinL)Ω_ecosα

是关于航向角和纬度的非线性方程，不能直接采用最小二乘方法进行求解，可以先将非线性方程在初值点泰勒展开，得到下式，即

${\mathrm{\ω}}_g^m-f({\mathrm{\φ}}_0,L_0)=\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0)}(\mathrm{\φ}-{\mathrm{\φ}}_0)+\frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0)}(L-L_0)+E$

因此可令

$Z_k={\mathrm{\ω}}_g^m\left(k\right)-f({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k);$

$H_k=\left[\begin{array}{ccc}\frac{\∂f}{\∂\mathrm{\φ}}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& \frac{\∂f}{\∂L}{|}_{({\mathrm{\φ}}_0,L_0,{\mathrm{\α}}_k)}& 1\end{array}\right]$

X_k＝[φ-φ₀ L-L₀ E]^T＝[Δφ ΔL E]^T

可得近似的线性方程为

Z_k＝H_kX_k-1+N_k

采用递推最小二乘法可以求得[Δφ ΔL E]^T，然后由

[φ L E]^T＝[φ₀ L₀ 0]^T+[Δφ ΔL E]^T

得到精确的寻北值。