[发明专利]一种任意二位置捷联寻北方法

摘要

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

主权项

一种任意二位置捷联寻北方法,其特征在于：包括以下步骤：步骤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\·C_1^2\·C_n^1\\ =\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\θ}& \sin \mathrm{\α}\cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ \cos \mathrm{\α}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}& \cos \mathrm{\θ}\sin \mathrm{\γ}\\ \cos \mathrm{\α}\sin \mathrm{\θ}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\θ}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\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{\→}{\mathrm{\ω}}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=C_n^b{\stackrel{\→}{\mathrm{\ω}}}_{ie}^n+{\stackrel{\→}{\mathrm{\ϵ}}}_0+\stackrel{\→}{\mathrm{\ϵ}}=C_n^b\left[\begin{array}{c}{\mathrm{\ω}}_N\\ 0\\ {\mathrm{\ω}}_T\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ϵ}}_0^x\\ {\mathrm{\ϵ}}_0^y\\ {\mathrm{\ϵ}}_0^z\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ϵ}}^x\\ {\mathrm{\ϵ}}^y\\ {\mathrm{\ϵ}}^z\end{array}\right]---\left(2\right)$式中分别表示陀螺x、y、z敏感轴的常值漂移，ε^x、ε^y、ε^z分别表示陀螺x、y、z敏感轴的随机漂移；把(1)式代入(2)式可得陀螺在位置1(初始位置)测得的角速度为：${\mathrm{\ω}}_{x\left(1\right)}={\mathrm{\ω}}_{N}cos\mathrm{\α}cos\mathrm{\θ}-{\mathrm{\ω}}_{T}sin\mathrm{\θ}+{\mathrm{\ϵ}}_0^x+{\mathrm{\ϵ}}_{\left(1\right)}^x---\left(3\right)$${\mathrm{\ω}}_{y\left(1\right)}={\mathrm{\ω}}_N(cos\mathrm{\α}sin\mathrm{\θ}sin\mathrm{\γ}-sin\mathrm{\α}cos\mathrm{\γ})+{\mathrm{\ω}}_{T}cos\mathrm{\θ}sin\mathrm{\γ}+{\mathrm{\ϵ}}_0^y+{\mathrm{\ϵ}}_{\left(1\right)}^y---\left(4\right)$式中分别表示陀螺x、y敏感轴在位置1测量数据的随机漂移；步骤2.4：同理可得加速度计在位置1的测得值为：$a_{x\left(1\right)}=gsin\mathrm{\θ}+{\mathrm{\ξ}}_0^x+{\mathrm{\ξ}}_{\left(1\right)}^x---\left(5\right)$$a_{y\left(1\right)}=-gcos\mathrm{\θ}sin\mathrm{\γ}+{\mathrm{\ξ}}_0^y+{\mathrm{\ξ}}_{\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{\μ}& sin\mathrm{\μ}& 0\\ -sin\mathrm{\μ}& \cos \mathrm{\μ}& 0\\ 0& 0& 1\end{array}\right]$则可得位置2陀螺的理论输出角速度为：${\stackrel{\→}{\mathrm{\ω}}}_{m^1}=C_m^{m^1}\·C_n^b\·{\stackrel{\→}{\mathrm{\ω}}}_{ie}^n=C_m^{m^1}\·C_n^b\left[\begin{array}{c}{\mathrm{\ω}}_N\\ 0\\ {\mathrm{\ω}}_T\end{array}\right]$步骤3.2：考虑陀螺常值漂移和随机漂移项，可得陀螺在位置2测得的角速度为：$\begin{array}{c}{\mathrm{\ω}}_{x\left(2\right)}=\cos \mathrm{\μ}({\mathrm{\ω}}_N\cos \mathrm{\α}\cos \mathrm{\θ}-{\mathrm{\ω}}_T\sin \mathrm{\θ})+\\ \sin \mathrm{\μ}\[{\mathrm{\ω}}_N(\cos \mathrm{\α}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ})+{\mathrm{\ω}}_T\cos \mathrm{\θ}\sin \mathrm{\γ}\]+{\mathrm{\ϵ}}_0^x+{\mathrm{\ϵ}}_{\left(2\right)}^x\end{array}---\left(7\right)$$\begin{array}{c}{\mathrm{\ω}}_{y\left(2\right)}=-\sin \mathrm{\μ}({\mathrm{\ω}}_N\cos \mathrm{\α}\cos \mathrm{\θ}-{\mathrm{\ω}}_T\sin \mathrm{\θ})+\\ \cos \mathrm{\μ}\[{\mathrm{\ω}}_N(\cos \mathrm{\α}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ})+{\mathrm{\ω}}_T\cos \mathrm{\θ}\sin \mathrm{\γ}\]+{\mathrm{\ϵ}}_0^y+{\mathrm{\ϵ}}_{\left(2\right)}^y\end{array}---\left(8\right)$式中分别表示陀螺x、y敏感轴在位置2测量数据的随机漂移；步骤3.3：同理可得加速度计在位置2测得的值为：$a_{x\left(2\right)}=gsin\mathrm{\θ}cos\mathrm{\μ}-gcos\mathrm{\θ}sin\mathrm{\γ}sin\mathrm{\μ}+{\mathrm{\ξ}}_0^x+{\mathrm{\ξ}}_{\left(2\right)}^x---\left(9\right)$$a_{y\left(2\right)}=-gsin\mathrm{\θ}sin\mathrm{\μ}-gcos\mathrm{\θ}sin\mathrm{\γ}cos\mathrm{\μ}+{\mathrm{\ξ}}_0^y+{\mathrm{\ξ}}_{\left(2\right)}^y---\left(10\right)$式中分别表示加速度计A_x、A_y在位置2测量数据的随机漂移；步骤4：对陀螺常值漂移和加速度计零偏进行补偿步骤4.1：陀螺常值漂移的补偿；把式(3)、(4)和式(7)、(8)分别对应相减并忽略随机漂移可得：$\begin{array}{c}{\mathrm{\ω}}_x={\mathrm{\ω}}_{x\left(1\right)}-{\mathrm{\ω}}_{x\left(2\right)}={\mathrm{\ω}}_N\cos \mathrm{\α}\cos \mathrm{\θ}-{\mathrm{\ω}}_T\sin \mathrm{\θ}-\\ \{\cos \mathrm{\μ}({\mathrm{\ω}}_N\cos \mathrm{\α}\cos \mathrm{\θ}-{\mathrm{\ω}}_T\sin \mathrm{\θ})+\sin \mathrm{\μ}\[{\mathrm{\ω}}_N(\cos \mathrm{\α}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ})+{\mathrm{\ω}}_T\cos \mathrm{\θ}\sin \mathrm{\γ}\]\}\end{array}---\left(11\right)$$\begin{array}{c}{\mathrm{\ω}}_y={\mathrm{\ω}}_{y\left(1\right)}-{\mathrm{\ω}}_{y\left(2\right)}={\mathrm{\ω}}_N(\cos \mathrm{\α}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ})+{\mathrm{\ω}}_T\cos \mathrm{\θ}\sin \mathrm{\γ}-\\ \{-\sin \mathrm{\μ}({\mathrm{\ω}}_N\cos \mathrm{\α}\cos \mathrm{\θ}-{\mathrm{\ω}}_T\sin \mathrm{\θ})+\cos \mathrm{\μ}\[{\mathrm{\ω}}_N(\cos \mathrm{\α}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\α}\cos \mathrm{\γ})+{\mathrm{\ω}}_T\cos \mathrm{\θ}\sin \mathrm{\γ}\]\}\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{\α}=\frac{{\mathrm{\ω}}_x(\sin \mathrm{\θ}\sin \mathrm{\γ}+\sin \mathrm{\μ}\cos \mathrm{\θ}-\cos \mathrm{\μ}\sin \mathrm{\θ}\sin \mathrm{\γ})-{\mathrm{\ω}}_y(\cos \mathrm{\θ}-\cos \mathrm{\μ}\cos \mathrm{\θ}-\sin \mathrm{\μ}\sin \mathrm{\θ}\sin \mathrm{\γ})+2{\mathrm{\ω}}_T\sin \mathrm{\γ}(1-\cos \mathrm{\μ})}{2{\mathrm{\ω}}_N\cos \mathrm{\θ}\cos \mathrm{\γ}(1-\cos \mathrm{\μ})}$$\cos \mathrm{\α}=\frac{{\mathrm{\ω}}_x(\cos \mathrm{\μ}-1)-{\mathrm{\ω}}_y\sin \mathrm{\μ}+2{\mathrm{\ω}}_T\sin \mathrm{\θ}(\cos \mathrm{\μ}-1)}{2{\mathrm{\ω}}_N\cos \mathrm{\θ}(\cos \mathrm{\μ}-1)}$故可得方位角：$\tan \mathrm{\α}=\frac{{\mathrm{\ω}}_x(\sin \mathrm{\θ}\sin \mathrm{\γ}+\sin \mathrm{\μ}\cos \mathrm{\θ}-\cos \mathrm{\μ}\sin \mathrm{\θ}\sin \mathrm{\γ})-{\mathrm{\ω}}_y(\cos \mathrm{\θ}-\cos \mathrm{\μ}\cos \mathrm{\θ}-\sin \mathrm{\μ}\sin \mathrm{\θ}\sin \mathrm{\γ})+2{\mathrm{\ω}}_T\sin \mathrm{\γ}(1-\cos \mathrm{\μ})}{-\[{\mathrm{\ω}}_x(\cos \mathrm{\μ}-1)-{\mathrm{\ω}}_y\sin \mathrm{\μ}+2{\mathrm{\ω}}_T\sin \mathrm{\θ}(\cos \mathrm{\μ}-1)\]\cos \mathrm{\γ}}$$\mathrm{\α}=\arctan \left\{\frac{{\mathrm{\ω}}_x(\sin \mathrm{\θ}\sin \mathrm{\γ}+\sin \mathrm{\μ}\cos \mathrm{\θ}-\cos \mathrm{\μ}\sin \mathrm{\θ}\sin \mathrm{\γ})-{\mathrm{\ω}}_y(\cos \mathrm{\θ}-\cos \mathrm{\μ}\cos \mathrm{\θ}-\sin \mathrm{\μ}\sin \mathrm{\θ}\sin \mathrm{\γ})+2{\mathrm{\ω}}_T\sin \mathrm{\γ}(1-\cos \mathrm{\μ})}{-\[{\mathrm{\ω}}_x(\cos \mathrm{\μ}-1)-{\mathrm{\ω}}_y\sin \mathrm{\μ}+2{\mathrm{\ω}}_T\sin \mathrm{\θ}(\cos \mathrm{\μ}-1)\]\cos \mathrm{\γ}}\right\}---\left(15\right)$式中ω_x、ω_y分别为陀螺x、y敏感轴在任意两个位置上的测量数据之差；θ、γ分别表示俯仰角和横滚角，可由式(13)、(14)联立求的；对于下述三种特殊情况有：(1)θ＝0、γ＝0(水平状态下)；$\mathrm{\α}=\arctan \[\frac{{\mathrm{\ω}}_{x}sin\mathrm{\μ}-{\mathrm{\ω}}_y(1-cos\mathrm{\μ})}{{\mathrm{\ω}}_x(1-cos\mathrm{\μ})+{\mathrm{\ω}}_{y}sin\mathrm{\μ}}\]$(2)μ＝180°(对径180°两位置测量，即传统二位置法)；$\mathrm{\α}=\arctan \[\frac{{\mathrm{\ω}}_{x}sin\mathrm{\θ}sin\mathrm{\γ}-{\mathrm{\ω}}_{y}cos\mathrm{\θ}+2{\mathrm{\ω}}_{T}sin\mathrm{\γ}}{({\mathrm{\ω}}_x+2{\mathrm{\ω}}_{T}sin\mathrm{\θ})cos\mathrm{\γ}}\]$(3)θ＝0、γ＝0且μ＝180°(水平状态下的传统二位置法)；$\mathrm{\α}=\arctan (-\frac{{\mathrm{\ω}}_y}{{\mathrm{\ω}}_x}).$