一种捷联惯性导航系统的任意双位置初始对准方法

1、一种捷联惯性导航系统的任意双位置初始对准方法，其特征在于包 括以下步骤：
(1)保持SINS在初始位置作为第一位置静止不动，并采集2-5分钟 陀螺输出和加速度计输出的数据；
(2)利用加速度计的输出与重力加速度的关系，计算出初始位置的俯 仰角θ1和横滚角γ1；
(3)通过任意方法将SINS绕任意轴旋转到任意一个位置作为第二位 置，并利用SINS自身的输出计算航向角的变化量α；
(4)保持SINS在第二位置静止不动，采集2-5分钟陀螺输出和加速 度计输出的数据，利用加速度计输出计算出第二个位置的俯仰角θ2和横滚角 γ2；
(5)利用两个位置上SINS的输出数据和SINS姿态的变化量，计算第 二位置的航向角2，作为系统的初始航向角，同时计算陀螺常值漂移Dx，Dy 和Dz。
2、根据权利要求1所述的一种捷联惯性导航系统的任意双位置初始对 准方法，其特征在于：所述的步骤(3)中航向角变化量α是利用SINS输出 的角速度或角增量信息，采用四元数方法进行计算得到。
3、根据权利要求1所述的一种捷联惯性导航系统的任意双位置初始对 准方法，其特征在于：所述的步骤(5)中的第二位置航向角2的计算公式 如下：航向角2
2＝1+α                                               (1)
其中，1为第一个位置时的航向角，α为从第一个位置到第二个位置转过 的角度，定义为逆时针为正；
第一个位置航向角1的计算公式为：
1＝1 1+1 2    (2)
其中，




其中，ωie为地球自转角速度，lat为纬度，
mx＝cosγ2cosα-sinθ2sinγ2sinα-cosγ1
nx＝cosγ2sinα+sinθ2sinγ2cosα-sinθ1sinγ1
my＝cosθ2cosα-cosθ1
ny＝cosθ2sinα
mz＝sinγ2cosα+sinθ2cosγ2sinα-sinγ1
nz＝sinγ2sinα-sinθ2cosγ2cosα-sinθ1cosγ1
ωx 1＝ωxo 2-ωxo 1
ωy 1＝ωyo 2-ωyo 1
ωz 1＝ωzo 2-ωzo 1
Δx＝ωiesinlat·(cosθ1sinγ1-cosθ2sinγ2)
Δy＝ωiesinlat·sinθ2-sinθ1
Δz＝ωiesinlat·(cosθ2cosγ2-cosθ1cosγ1)
其中，θ1，γ1和θ2，γ2分别为第一位置和第二位置的俯仰角、横滚角，ωxo 1、 ωyo 1和ωzo 1分别为第1个位置上SINS输出的X、Y和Z轴角增量，ωxo 2、ωyo 2和 ωzo 2分别为第2个位置上SINS输出的X、Y和Z轴角增量；
${\theta }_i=\arcsin \left(\frac{f_{\mathrm{yo}}^i}{g}\right)(i=\mathrm{1,2})---\left(7\right)$
${\gamma }_i=\arcsin \left(\frac{-f_{\mathrm{xo}}^i}{\cos {\theta }_i·g}\right)(i=\mathrm{1,2})---\left(8\right)$
其中，fxo i为第i个位置上SINS输出的X轴比力，fyo i为第i个位置上SINS 输出的Y轴比力，g为重力加速度；
根据式(3)和式(4)分别计算1 1和1 2时，需要进行主值判断，判断 方法如下：当cos1 i＞0(i＝1，2)时，式(3)和式(4)的计算结果即为1 1和 1 2的真值；当cos1 i＜0(i＝1，2)时，式(3)和式(4)的计算结果不是1 1和 1 2的真值，真值应该是π减去计算结果。
4、根据权利要求1所述的一种捷联惯性导航系统的任意双位置初始对 准方法，其特征在于：所述的步骤(5)中的陀螺常值漂移Dx，Dy和Dz的计 算公式如下：
Dx＝ωxo-(ωiecoslatsinicosγi+ωiecoslatsinθicosisinγi-ωiesinlatcosθisinγi) (9)
Dy＝ωyo-(ωiecoslatcosicosθi+ωiesinlatsinθi)                                    (10)
Dz＝ωzo-(ωiecoslatsinisinγi-ωiecoslatsinθicosicosγi+ωiesinlatcosθicosγi) (11)
其中，i＝1，2。

技术领域\n本发明涉及一种捷联惯性导航系统SINS的初始姿态确定方法，可用于 各种中高精度的捷联惯性导航系统的初始对准。\n背景技术\n捷联惯性导航系统SINS是一种完全自主的导航系统，它利用陀螺测量 的载体角速度信息和加速度计测量的加速度信息，在初始信息的基础上进行 积分运算，可以连续、实时地提供位置、速度和姿态信息，具有隐蔽性好， 不受气候条件限制等优点，因而广泛应用于航空、航天、航海等领域。根据 SINS的基本原理，SINS在开始导航定位之前必须获得初始信息，包括初始 的位置、速度和姿态。SINS的初始位置和速度较容易获得，因此如何快速 获得高精度的初始姿态信息成为了惯性导航领域一项非常重要的课题，计算 SINS初始姿态的过程称为初始对准。\n初始对准可以分为所谓的自主式和受控式两种，因为自主是惯性导航系 统最大的优点，因此自主式对准也是初始对准发展的主要方向。通常的自主 式对准方法又分为单位置和双位置对准方法，单位置初始对准方法利用陀螺 仪和加速度计输出的信息即可计算出载体的初始姿态，但是由于陀螺漂移的 存在使得单位置对准方法精度不高。双位置对准利用两个位置上陀螺仪和加 速度计输出的信息可以测量出陀螺常值漂移，因此提高了对准的精度。但是 以往的双位置对准方法要求SINS绕Z轴旋转180度或90度，这就需要将 SINS安装在一个伺服平台上，利用伺服平台实现180度或者90度的转动， 这为工程使用带来了不便，且伺服平台的精度不高，降低双位置对准的精度\n发明内容\n本发明的技术解决问题是：克服现有技术的不足，提供一种通过任意方 法将SINS绕任意轴旋转到任意位置，即可确定出SINS的初始姿态，同时 计算出陀螺的常值漂移，既保证了对准的精度，又大大方便了工程实际应用 的捷联惯性导航系统任意双位置初始对准方法。\n本发明的技术解决方案为：一种捷联惯性导航系统的任意双位置初始对 准方法，其特征在于将SINS从初始位置旋转到任意一个位置，利用两个位 置上SINS的输出与地球自转角速度和重力加速度的关系，即可确定SINS 的初始姿态，并且可以测量出陀螺仪的常值漂移，其具体步骤如下：\n(1)捷联惯性导航系统预热准备，具体准备时间根据不同的系统而不 同，SINS准备完毕，保持SINS静止在初始位置作为第一位置不动，并采 集2-5分钟陀螺输出和加速度计输出的数据，如果SINS的精度较低可适 当延长采数时间；\n(2)利用加速度计的输出与重力加速度的关系，计算出初始位置的俯 仰角θ1和横滚角γ1；\n(3)通过任意方法将SINS绕任意轴旋转到任意一个位置作为第二位 置，并利用SINS自身的输出计算航向角的变化量α；\n(4)保持SINS在第二位置静止不动，采集2-5分钟陀螺输出和加速 度计输出的数据，利用加速度计输出计算出第二个位置的俯仰角θ2和横滚角 γ2；\n(5)利用两个位置上SINS的输出数据和SINS姿态的变化量，计算第 二位置的航向角2和陀螺常值漂移Dx，Dy和Dz。\n本发明的原理是：SINS在一个位置保持静止不动时，角速度通道三个 轴的输出的角速度可以表示为：\n\n将SINS旋转到另外一个任意位置，可以建立同理的方程，\n\n利用加速度计的输出可以确定两个位置的俯仰角和航向角θ1、γ1、θ2和 γ2，利用陀螺的输出并采用四元数方法可以计算出两个位置之间的航向角变 化量α，由于转动时间较短，因此陀螺常值漂移引起的误差非常小。这样任 意双位置对准问题可转化为式(12)和(13)组成的6个方程以及2＝1+α 共7个方程求解4个未知数1、Dx、Dy和Dz的问题。\n本发明与现有技术相比的优点在于：本发明打破了传统双位置对准需要 将SINS绕Z轴旋转180度或90度的约束，通过任意方法将SINS绕任意 轴旋转到任意位置，即可确定出SINS的初始姿态，同时计算出陀螺的常值 漂移，既保证了对准的精度，又大大方便了工程实际应用。\n附图说明\n图1为本发明的任意双位置初始对准流程图；\n图2为航向角、俯仰角θ和横滚角γ的示意图，图中Oxnynzn为导航坐标 系，即东北天地理坐标系，Oxbybzb为载体坐标系。图2a表示从导航坐标系 Oxnynzn绕zn轴逆时针旋转与载体坐标系Oxbybzb重合，即为航向角；图2b 表示从导航坐标系Oxnynzn绕xn轴逆时针旋转θ与载体坐标系Oxbybzb重合，θ 即为俯仰角；图2c表示从导航坐标系Oxnynzn绕yn轴逆时针旋转γ与载体坐 标系Oxbybzb重合，γ即为横滚角。\n具体实施方式\n如图1所示，本发明的具体实施方法如下：\n1、捷联惯性导航系统的准备\nSINS开机后，进入准备状态，准备时间根据不同类系统而不同。\n2、采集初始位置，即第一位置数据\nSINS准备完毕，保持SINS静止在初始位置即第一位置不动，并采集2 分钟陀螺输出和加速度计输出的数据，如果SINS的精度较低可适当延长采 数时间。\n3、计算第一位置上SINS的俯仰角θ1和横滚角γ1，俯仰角和横滚角的定 义如图2b和图2c所示。\n利用加速度计输出与重力加速度的关系，计算出初始位置的俯仰角θ1和 横滚角γ1，计算的公式如下：\n${\theta }_1=\arcsin \left(\frac{{f_{\mathrm{yo}}}^1}{g}\right)---\left(14\right)$\n${\gamma }_1=\arcsin \left(\frac{-{f_{\mathrm{xo}}}^1}{\cos {\theta }_1·g}\right)---\left(15\right)$\n其中，fxo 1为第1个位置上SINS输出的X轴比力，fyo 1为第1个位置上 SINS输出的Y轴比力，g为重力加速度。\n4、通过任意方法将SINS绕任意轴旋转到任意位置作为第二位置\n通过任意方法将SINS绕任意的旋转轴旋转到任意一个位置作为第二位 置，利用SINS的输出角增量或角速度信息，采用四元数方法计算航向角的 变化量α，计算步骤如下：\n(1)初始化第一个位置时的姿态，俯仰角θ1和横滚角γ1通过加速度计的 输出计算得到，令航向角1＝0，计算初始姿态矩阵Cb n和四元数q，计算公式 如下：\n\n令 $C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$\n则有：\n$q_0=\pm \frac{1}{2}\sqrt{1+T_{11}+T_{22}-T_{33}}$\n$q_1=\pm \frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}$\n$q_2\pm \frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}$\n$q_3=\pm \frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}$\n(2)更新四元数和姿态矩阵\n$q(n+1)=\{(1-\frac{{\left(\Delta {\theta }_0\right)}^2}{8}+\frac{{\left(\Delta {\theta }_0\right)}^4}{384})I+(\frac{1}{2}-\frac{{\left(\Delta {\theta }_0\right)}^2}{48})\left(\mathrm{\Delta \theta }\right)\}q\left(n\right)---\left(17\right)$\n其中，\n$\mathrm{\Delta \theta }=\left[\begin{array}{cccc}0& -\Delta {\theta }_x& -\Delta {\theta }_y& -\Delta {\theta }_z\\ \Delta {\theta }_x& 0& \Delta {\theta }_z& -\Delta {\theta }_y\\ \Delta {\theta }_y& -\Delta {\theta }_z& 0& \Delta {\theta }_x\\ \Delta {\theta }_z& \Delta {\theta }_y& -\Delta {\theta }_x& 0\end{array}\right]$\n$\Delta {\theta }_0=\sqrt{\Delta {\theta }_x^2+\Delta {\theta }_y^2+\Delta {\theta }_z^2}$\n姿态矩阵Cb n的更新公式如下：\n$C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(18\right)$\n(3)计算航向角变化量\n$\alpha =t{g}^{-1}\left(\frac{T_{21}}{T_{22}}\right)---\left(19\right)$\n主值判断如下表\n    T22符号     α符号     α真值   α所在象限     +     +     α   第一象限\n    -     +     α+π   第二象限     -     -     α+π   第三象限     +     -     α+2π   第四象限\n5、采集第二位置数据\n将SINS转到第二个位置，待SINS稳定后，保持SINS在第二位置静止 不动，并采集2分钟陀螺输出和加速度计输出的数据，如果SINS的精度较 低可适当延长采数时间如5分钟。\n6、计算第二位置上SINS的俯仰角θ2和横滚角γ2\n利用第二位置上SINS的加速度计输出计算出第二个位置的俯仰角θ2和 横滚角γ2，计算公式如下：\n${\theta }_2=\mathrm{arci}\sin \left(\frac{{f_{\mathrm{yo}}}^2}{g}\right)---\left(20\right)$\n${\gamma }_2=\mathrm{arci}\sin \left(\frac{-{f_{\mathrm{xo}}}^2}{\cos {\theta }_2·g}\right)---\left(21\right)$\n其中，fxo 2为第2个位置上SINS输出的X轴比力，fyo 2为第2个位置上 SINS输出的Y轴比力，g为重力加速度。\n7、计算航向角2和陀螺常值漂移D\n利用两个位置上SINS的输出数据和SINS姿态的变化量，计算第二位 置的航向角2和陀螺常值漂移Dx，Dy和Dz，其中航向角的定义如图2a所示 计算步骤如下：\n(1)SINS三个轴向角速度输出模型\n\n其中， $\stackrel{-}{{\omega }_o}={\left[\begin{array}{ccc}{\omega }_{\mathrm{xo}}& {\omega }_{\mathrm{yo}}& {\omega }_{\mathrm{zo}}\end{array}\right]}^T\begin{array}{ccc}& & \end{array}$ 为SINS输出的角速度。\n(2)建立两个位置上的输入输出方程\nSINS在第一个位置上时有：\n\nSINS在第二个位置上时有：\n\n$+\left[\begin{array}{c}{D}_x-{\omega }_{\mathrm{ie}}\sin \mathrm{lat}\cos {\theta }_2\sin {\gamma }_2\\ D_y+{\omega }_{\mathrm{ie}}\sin \mathrm{lat}\sin {\theta }_2\\ D_z+{\omega }_{\mathrm{ie}}\sin \mathrm{lat}\cos {\theta }_2\cos {\gamma }_2\end{array}\right]---\left(24\right)$\n其中，α为航向角变化量，2＝1+α。\n(3)计算航向角2\n2＝1+α    (25)\n其中，1为第一个位置时的航向角，α为从第一个位置到第二个位置转过 的角度，定义为逆时针为正。\n第一个位置航向角1的计算公式为：\n1＝1 1+1 2   (26)\n其中，\n\n\n\n\n其中，ωie为地球自转角速度，lat为纬度，\nmx＝cosγ2cosα-sinθ2sinγ2 sinα-cosγ1\nnx＝cosγ2sinα+sinθ2sinγ2 cosα-sinθ1sinγ1\nmy＝cosθ2cosα-cosθ1\nny＝cosθ2sinα\nmz＝sinγ2cosα+sinθ2cosγ2 sinα-sinγ1\nnz＝sinγ2sinα-sinθ2cosγ2 cosα-sinθ1cosγ1\nωx′＝ωxo 2-ωxo 1\nωy′＝ωyo 2-ωyo 1\nωz′＝ωzo 2-ωzo 1\nΔx＝ωiesinlat·(cosθ1sinγ1-cosθ2sinγ2)\nΔy＝ωiesinlat·sinθ2-sinθ1\nΔz＝ωiesinlat·(cosθ2cosγ2-cosθ1cosγ1)\n其中，θ1，γ1和θ2，γ2分别为第一位置和第二位置的俯仰角、横滚角，ωxo 1、 ωyo 1和ωzo 1分别为第1个位置上SINS输出的X、Y和Z轴角增量，ωxo 2、ωyo 2和 ωzo 2分别为第2个位置上SINS输出的X、Y和Z轴角增量；\n$\theta =\arcsin \left(\frac{f_{\mathrm{yo}}}{g}\right)---\left(31\right)$\n$\gamma =\arcsin \left(\frac{-f_{\mathrm{xo}}}{\cos \theta ·g}\right)---\left(32\right)$\n其中，fxo为SINS输出的X轴比力，fyo为SINS输出的Y轴比力，g为 重力加速度；\n计算1时，需要进行主值判断，判断方法为：当cos1 i＞0(i＝1，2)时， 式(3)和式(4)的计算结果即为1 1和1 2的真值；当cos1 i＜0(i＝1，2)时， 式(3)和式(4)的计算结果不是1 1和1 2的真值，真值应该是π减去计算 结果，如下表：\n  cos1符号   1符号   1真值   1所在象限\n    +     +     1     第一象限     -     +     π-1     第二象限     -     -     π-1     第三象限     +     -     1     第四象限\n(4)陀螺常值漂移Dx，Dy和Dz的计算\nDx＝ωxo-(ωiecoslatsini cosγi+ωiecoslatsinθicosisinγi-ωiesinlatcosθisinγi)  (33)\nDy＝ωyo-(ωiecoslatcosicosθi+ωiesinlatsinθi)    (34)\nDz＝ωzo-(ωiecoslatsinisinγi-ωiecoslatsinθicosi cosγi+ωiesinlat cosθicosγi)  (35)\n其中，i＝1，2。