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

Abstract

一种捷联惯性导航系统的任意双位置初始时准方法，本发明涉及一种用于确定捷联惯性导航系统(SINS)初始姿态的方法。其特征在于将SINS从初始位置旋转到任意一个位置，利用两个位置上SINS的输出与地球自转角速度和重力加速度的关系，即可确定SINS的初始姿态，并且可以测量出陀螺仪的常值漂移。本发明是一种自主式对准方法，具有精度高，简便易行的特点，可应用于各种捷联惯性导航系统。

Description

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

技术领域

本发明涉及一种捷联惯性导航系统SINS的初始姿态确定方法，可用于各种中高精度的捷联惯性导航系统的初始对准。

背景技术

捷联惯性导航系统SINS是一种完全自主的导航系统，它利用陀螺测量的载体角速度信息和加速度计测量的加速度信息，在初始信息的基础上进行积分运算，可以连续、实时地提供位置、速度和姿态信息，具有隐蔽性好，不受气候条件限制等优点，因而广泛应用于航空、航天、航海等领域。根据SINS的基本原理，SINS在开始导航定位之前必须获得初始信息，包括初始的位置、速度和姿态。SINS的初始位置和速度较容易获得，因此如何快速获得高精度的初始姿态信息成为了惯性导航领域一项非常重要的课题，计算SINS初始姿态的过程称为初始对准。

初始对准可以分为所谓的自主式和受控式两种，因为自主是惯性导航系统最大的优点，因此自主式对准也是初始对准发展的主要方向。通常的自主式对准方法又分为单位置和双位置对准方法，单位置初始对准方法利用陀螺仪和加速度计输出的信息即可计算出载体的初始姿态，但是由于陀螺漂移的存在使得单位置对准方法精度不高。双位置对准利用两个位置上陀螺仪和加速度计输出的信息可以测量出陀螺常值漂移，因此提高了对准的精度。但是以往的双位置对准方法要求SINS绕Z轴旋转180度或90度，这就需要将SINS安装在一个伺服平台上，利用伺服平台实现180度或者90度的转动，这为工程使用带来了不便，且伺服平台的精度不高，降低双位置对准的精度

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种通过任意方法将SINS绕任意轴旋转到任意位置，即可确定出SINS的初始姿态，同时计算出陀螺的常值漂移，既保证了对准的精度，又大大方便了工程实际应用的捷联惯性导航系统任意双位置初始对准方法。

本发明的技术解决方案为：一种捷联惯性导航系统的任意双位置初始对准方法，其特征在于将SINS从初始位置旋转到任意一个位置，利用两个位置上SINS的输出与地球自转角速度和重力加速度的关系，即可确定SINS的初始姿态，并且可以测量出陀螺仪的常值漂移，其具体步骤如下：

(1)捷联惯性导航系统预热准备，具体准备时间根据不同的系统而不同，SINS准备完毕，保持SINS静止在初始位置作为第一位置不动，并采集2-5分钟陀螺输出和加速度计输出的数据，如果SINS的精度较低可适当延长采数时间；

(2)利用加速度计的输出与重力加速度的关系，计算出初始位置的俯仰角θ₁和横滚角γ₁；

(3)通过任意方法将SINS绕任意轴旋转到任意一个位置作为第二位置，并利用SINS自身的输出计算航向角的变化量α；

(4)保持SINS在第二位置静止不动，采集2-5分钟陀螺输出和加速度计输出的数据，利用加速度计输出计算出第二个位置的俯仰角θ₂和横滚角γ₂；

(5)利用两个位置上SINS的输出数据和SINS姿态的变化量，计算第二位置的航向角₂和陀螺常值漂移D_x，D_y和D_z。

本发明的原理是：SINS在一个位置保持静止不动时，角速度通道三个轴的输出的角速度可以表示为：

将SINS旋转到另外一个任意位置，可以建立同理的方程，

利用加速度计的输出可以确定两个位置的俯仰角和航向角θ₁、γ₁、θ₂和γ₂，利用陀螺的输出并采用四元数方法可以计算出两个位置之间的航向角变化量α，由于转动时间较短，因此陀螺常值漂移引起的误差非常小。这样任意双位置对准问题可转化为式(12)和(13)组成的6个方程以及₂＝₁+α共7个方程求解4个未知数₁、D_x、D_y和D_z的问题。

本发明与现有技术相比的优点在于：本发明打破了传统双位置对准需要将SINS绕Z轴旋转180度或90度的约束，通过任意方法将SINS绕任意轴旋转到任意位置，即可确定出SINS的初始姿态，同时计算出陀螺的常值漂移，既保证了对准的精度，又大大方便了工程实际应用。

附图说明

图1为本发明的任意双位置初始对准流程图；

图2为航向角、俯仰角θ和横滚角γ的示意图，图中Ox_ny_nz_n为导航坐标系，即东北天地理坐标系，Ox_by_bz_b为载体坐标系。图2a表示从导航坐标系Ox_ny_nz_n绕zn轴逆时针旋转与载体坐标系Ox_by_bz_b重合，即为航向角；图2b表示从导航坐标系Ox_ny_nz_n绕x_n轴逆时针旋转θ与载体坐标系Ox_by_bz_b重合，θ即为俯仰角；图2c表示从导航坐标系Ox_ny_nz_n绕y_n轴逆时针旋转γ与载体坐标系Ox_by_bz_b重合，γ即为横滚角。

具体实施方式

如图1所示，本发明的具体实施方法如下：

1、捷联惯性导航系统的准备

SINS开机后，进入准备状态，准备时间根据不同类系统而不同。

2、采集初始位置，即第一位置数据

SINS准备完毕，保持SINS静止在初始位置即第一位置不动，并采集2分钟陀螺输出和加速度计输出的数据，如果SINS的精度较低可适当延长采数时间。

3、计算第一位置上SINS的俯仰角θ₁和横滚角γ₁，俯仰角和横滚角的定义如图2b和图2c所示。

利用加速度计输出与重力加速度的关系，计算出初始位置的俯仰角θ₁和横滚角γ₁，计算的公式如下：

${\mathrm{\θ}}_1=\arcsin \left(\frac{{f_{\mathrm{yo}}}^1}{g}\right)---\left(14\right)$

${\mathrm{\γ}}_1=\arcsin \left(\frac{-{f_{\mathrm{xo}}}^1}{\cos {\mathrm{\θ}}_1\·g}\right)---\left(15\right)$

其中，f_xo ¹为第1个位置上SINS输出的X轴比力，f^{yo 1}为第1个位置上SINS输出的Y轴比力，g为重力加速度。

4、通过任意方法将SINS绕任意轴旋转到任意位置作为第二位置

通过任意方法将SINS绕任意的旋转轴旋转到任意一个位置作为第二位置，利用SINS的输出角增量或角速度信息，采用四元数方法计算航向角的变化量α，计算步骤如下：

(1)初始化第一个位置时的姿态，俯仰角θ₁和横滚角γ₁通过加速度计的输出计算得到，令航向角₁＝0，计算初始姿态矩阵C_b ⁿ和四元数q，计算公式如下：

令 $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]$

则有：

$q_0=\±\frac{1}{2}\sqrt{1+T_{11}+T_{22}-T_{33}}$

$q_1=\±\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}$

$q_2\±\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}$

$q_3=\±\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}$

(2)更新四元数和姿态矩阵

$q(n+1)=\{(1-\frac{{\left(\mathrm{\Δ}{\mathrm{\θ}}_0\right)}^2}{8}+\frac{{\left(\mathrm{\Δ}{\mathrm{\θ}}_0\right)}^4}{384})I+(\frac{1}{2}-\frac{{\left(\mathrm{\Δ}{\mathrm{\θ}}_0\right)}^2}{48})\left(\mathrm{\Δ\θ}\right)\}q\left(n\right)---\left(17\right)$

其中，

$\mathrm{\Δ\θ}=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\θ}}_x& -\mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_z\\ \mathrm{\Δ}{\mathrm{\θ}}_x& 0& \mathrm{\Δ}{\mathrm{\θ}}_z& -\mathrm{\Δ}{\mathrm{\θ}}_y\\ \mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_z& 0& \mathrm{\Δ}{\mathrm{\θ}}_x\\ \mathrm{\Δ}{\mathrm{\θ}}_z& \mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_x& 0\end{array}\right]$

$\mathrm{\Δ}{\mathrm{\θ}}_0=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_x^2+\mathrm{\Δ}{\mathrm{\θ}}_y^2+\mathrm{\Δ}{\mathrm{\θ}}_z^2}$

姿态矩阵C_b ⁿ的更新公式如下：

$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)$

(3)计算航向角变化量

$\mathrm{\α}=t{g}^{-1}\left(\frac{T_{21}}{T_{22}}\right)---\left(19\right)$

主值判断如下表

T22符号	α符号	α真值	α所在象限
				+	+	α	第一象限

-	+	α+π	第二象限
				-	-	α+π	第三象限
+	-	α+2π	第四象限

5、采集第二位置数据

将SINS转到第二个位置，待SINS稳定后，保持SINS在第二位置静止不动，并采集2分钟陀螺输出和加速度计输出的数据，如果SINS的精度较低可适当延长采数时间如5分钟。

6、计算第二位置上SINS的俯仰角θ₂和横滚角γ₂

利用第二位置上SINS的加速度计输出计算出第二个位置的俯仰角θ₂和横滚角γ₂，计算公式如下：

${\mathrm{\θ}}_2=\mathrm{arci}\sin \left(\frac{{f_{\mathrm{yo}}}^2}{g}\right)---\left(20\right)$

${\mathrm{\γ}}_2=\mathrm{arci}\sin \left(\frac{-{f_{\mathrm{xo}}}^2}{\cos {\mathrm{\θ}}_2\·g}\right)---\left(21\right)$

其中，f_xo ²为第2个位置上SINS输出的X轴比力，f_yo ²为第2个位置上SINS输出的Y轴比力，g为重力加速度。

7、计算航向角₂和陀螺常值漂移D

利用两个位置上SINS的输出数据和SINS姿态的变化量，计算第二位置的航向角₂和陀螺常值漂移D_x，D_y和D_z，其中航向角的定义如图2a所示计算步骤如下：

(1)SINS三个轴向角速度输出模型

其中， $\stackrel{-}{{\mathrm{\ω}}_o}={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{xo}}& {\mathrm{\ω}}_{\mathrm{yo}}& {\mathrm{\ω}}_{\mathrm{zo}}\end{array}\right]}^T\begin{array}{ccc}& & \end{array}$ 为SINS输出的角速度。

(2)建立两个位置上的输入输出方程

SINS在第一个位置上时有：

SINS在第二个位置上时有：

$+\left[\begin{array}{c}{D}_x-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{lat}\cos {\mathrm{\θ}}_2\sin {\mathrm{\γ}}_2\\ D_y+{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{lat}\sin {\mathrm{\θ}}_2\\ D_z+{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{lat}\cos {\mathrm{\θ}}_2\cos {\mathrm{\γ}}_2\end{array}\right]---\left(24\right)$

其中，α为航向角变化量，₂＝₁+α。

(3)计算航向角₂

₂＝₁+α    (25)

其中，₁为第一个位置时的航向角，α为从第一个位置到第二个位置转过的角度，定义为逆时针为正。

第一个位置航向角₁的计算公式为：

₁＝₁ ¹+₁ ²   (26)

其中，

其中，ω_ie为地球自转角速度，lat为纬度，

m_x＝cosγ₂cosα-sinθ₂sinγ₂ sinα-cosγ₁

n_x＝cosγ₂sinα+sinθ2sinγ₂ cosα-sinθ₁sinγ₁

m_y＝cos_θ2cosα-cosθ₁

n_y＝cosθ₂sinα

m_z＝sinγ₂cosα+sinθ₂cosγ₂ sinα-sinγ₁

n_z＝sinγ₂sinα-sinθ₂cosγ₂ cosα-sinθ₁cosγ₁

ω_x′＝ω_xo ²-ω_xo ¹

ω_y′＝ω_yo ²-ω_yo ¹

ω_z′＝ω_zo ²-ω_zo ¹

Δ_x＝ω_iesinlat·(cosθ₁sinγ₁-cosθ₂sinγ₂)

Δ_y＝ω_iesinlat·sinθ₂-sinθ₁

Δ_z＝ω_iesinlat·(cosθ₂cosγ₂-cosθ₁cosγ₁)

其中，θ₁，γ₁和θ₂，γ₂分别为第一位置和第二位置的俯仰角、横滚角，ω_xo ¹、ω_yo ¹和ω_zo ¹分别为第1个位置上SINS输出的X、Y和Z轴角增量，ω_xo ²、ω_yo ²和ω_zo ²分别为第2个位置上SINS输出的X、Y和Z轴角增量；

$\mathrm{\θ}=\arcsin \left(\frac{f_{\mathrm{yo}}}{g}\right)---\left(31\right)$

$\mathrm{\γ}=\arcsin \left(\frac{-f_{\mathrm{xo}}}{\cos \mathrm{\θ}\·g}\right)---\left(32\right)$

其中，f_xo为SINS输出的X轴比力，f_yo为SINS输出的Y轴比力，g为重力加速度；

计算₁时，需要进行主值判断，判断方法为：当cos₁ ⁱ＞0(i＝1，2)时，式(3)和式(4)的计算结果即为₁ ¹和₁ ²的真值；当cos₁ ⁱ＜0(i＝1，2)时，式(3)和式(4)的计算结果不是₁ ¹和₁ ²的真值，真值应该是π减去计算结果，如下表：

cos1符号

1符号

1真值

1所在象限

+	+	1	第一象限
				-	+	π-1	第二象限
-	-	π-1	第三象限
				+	-	1	第四象限

(4)陀螺常值漂移D_x，D_y和D_z的计算

D_x＝ω_xo-(ω_iecoslatsin_i cosγ_i+ω_iecoslatsinθ_icos_isinγ_i-ω_iesinlatcosθ_isinγ_i)  (33)

D_y＝ω_yo-(ω_iecoslatcos_icosθ_i+ω_iesinlatsinθ_i)    (34)

D_z＝ω_zo-(ω_iecoslatsin_isinγ_i-ω_iecoslatsinθ_icos_i cosγ_i+ω_iesinlat cosθ_icosγ_i)  (35)

其中，i＝1，2。

Claims (4)

1、一种捷联惯性导航系统的任意双位置初始对准方法，其特征在于包括以下步骤：

(1)保持SINS在初始位置作为第一位置静止不动，并采集2-5分钟陀螺输出和加速度计输出的数据；

(2)利用加速度计的输出与重力加速度的关系，计算出初始位置的俯仰角θ₁和横滚角γ₁；

(3)通过任意方法将SINS绕任意轴旋转到任意一个位置作为第二位置，并利用SINS自身的输出计算航向角的变化量α；

(4)保持SINS在第二位置静止不动，采集2-5分钟陀螺输出和加速度计输出的数据，利用加速度计输出计算出第二个位置的俯仰角θ₂和横滚角γ₂；

(5)利用两个位置上SINS的输出数据和SINS姿态的变化量，计算第二位置的航向角₂，作为系统的初始航向角，同时计算陀螺常值漂移D_x，D_y和D_z。

2、根据权利要求1所述的一种捷联惯性导航系统的任意双位置初始对准方法，其特征在于：所述的步骤(3)中航向角变化量α是利用SINS输出的角速度或角增量信息，采用四元数方法进行计算得到。

3、根据权利要求1所述的一种捷联惯性导航系统的任意双位置初始对准方法，其特征在于：所述的步骤(5)中的第二位置航向角₂的计算公式如下：航向角₂

₂＝₁+α                                               (1)

其中，₁为第一个位置时的航向角，α为从第一个位置到第二个位置转过的角度，定义为逆时针为正；

第一个位置航向角₁的计算公式为：

₁＝₁ ¹+₁ ²    (2)

其中，

其中，ω_ie为地球自转角速度，lat为纬度，

m_x＝cosγ₂cosα-sinθ₂sinγ₂sinα-cosγ₁

n_x＝cosγ₂sinα+sinθ₂sinγ₂cosα-sinθ₁sinγ₁

m_y＝cosθ₂cosα-cosθ₁

n_y＝cosθ₂sinα

m_z＝sinγ₂cosα+sinθ₂cosγ₂sinα-sinγ₁

n_z＝sinγ₂sinα-sinθ₂cosγ₂cosα-sinθ₁cosγ₁

ω_x ¹＝ω_xo ²-ω_xo ¹

ω_y ¹＝ω_yo ²-ω_yo ¹

ω_z ¹＝ω_zo ²-ω_zo ¹

Δ_x＝ω_iesinlat·(cosθ₁sinγ₁-cosθ₂sinγ₂)

Δ_y＝ω_iesinlat·sinθ₂-sinθ₁

Δ_z＝ω_iesinlat·(cosθ₂cosγ₂-cosθ₁cosγ₁)

其中，θ₁，γ₁和θ₂，γ₂分别为第一位置和第二位置的俯仰角、横滚角，ω_xo ¹、ω_yo ¹和ω_zo ¹分别为第1个位置上SINS输出的X、Y和Z轴角增量，ω_xo ²、ω_yo ²和ω_zo ²分别为第2个位置上SINS输出的X、Y和Z轴角增量；

${\mathrm{\θ}}_i=\arcsin \left(\frac{f_{\mathrm{yo}}^i}{g}\right)(i=\mathrm{1,2})---\left(7\right)$

${\mathrm{\γ}}_i=\arcsin \left(\frac{-f_{\mathrm{xo}}^i}{\cos {\mathrm{\θ}}_i\·g}\right)(i=\mathrm{1,2})---\left(8\right)$

其中，f_xo ⁱ为第i个位置上SINS输出的X轴比力，f_yo ⁱ为第i个位置上SINS输出的Y轴比力，g为重力加速度；

根据式(3)和式(4)分别计算₁ ¹和₁ ²时，需要进行主值判断，判断方法如下：当cos₁ ⁱ＞0(i＝1，2)时，式(3)和式(4)的计算结果即为₁ ¹和₁ ²的真值；当cos₁ ⁱ＜0(i＝1，2)时，式(3)和式(4)的计算结果不是₁ ¹和₁ ²的真值，真值应该是π减去计算结果。

4、根据权利要求1所述的一种捷联惯性导航系统的任意双位置初始对准方法，其特征在于：所述的步骤(5)中的陀螺常值漂移D_x，D_y和D_z的计算公式如下：

D_x＝ω_xo-(ω_iecoslatsin_icosγ_i+ω_iecoslatsinθ_icos_isinγ_i-ω_iesinlatcosθ_isinγ_i) (9)

D_y＝ω_yo-(ω_iecoslatcos_icosθ_i+ω_iesinlatsinθ_i)                                    (10)

D_z＝ω_zo-(ω_iecoslatsin_isinγ_i-ω_iecoslatsinθ_icos_icosγ_i+ω_iesinlatcosθ_icosγ_i) (11)

其中，i＝1，2。