CN101963513A - 消除水下运载体捷联惯导系统杆臂效应误差的对准方法 - Google Patents

Abstract

本发明的目的在于提供消除水下运载体捷联惯导系统杆臂效应误差的对准方法。首先连接主、子惯导系统，然后将主惯导系统的初始速度参数、初始位置参数装订至子惯导系统的导航计算机中，接着粗略计算出子惯导系统的初始姿态，完成主、子惯导系统间的一步传递，利用卡尔曼滤波估计失准角，最后对子惯导系统的姿态矩阵进行修正，得到水下运载体准确的初始姿态角，完成系统的初始对准。本发明不仅能够有效地解决杆臂效应误差补偿的问题，还能提高水下运载体捷联惯导系统初始对准的对准精度。

Description

消除水下运载体捷联惯导系统杆臂效应误差的对准方法

技术领域

本发明涉及的是一种导航领域的对准方法。

背景技术

捷联惯性导航系统由于具有较高的精度、良好的可靠性、较低的成本等特点，使其在航天、航空和航海等领域得到了广泛的应用，尤其是水下运载体。水下运载体捷联惯导系统在投入使用以前首先要进行初始对准，其目的是确定载体在初始时刻的姿态。初始对准直接影响着捷联惯导系统的精度和反应时间，因此，必须快速而准确地进行捷联惯导系统的初始对准。

船舶在航行时会受到阵风、洋流、海浪等的干扰，使得水下运载体捷联惯导系统的初始对准变得非常复杂，因而很难有效地完成自对准。在现代化的船舶上通常都安装有精度较高的主惯导系统，而传递对准就是指载体在航行时，载体上需要对准的子惯导系统利用已对准好的主惯导系统的计算或测量信息进行初始对准的过程。因此，传递对准成为了解决水下运载体捷联惯导系统初始对准问题的主要方法。

在传递对准的实际应用中，认为主惯导系统安装在载体的摇摆中心，而通常子惯导系统没有安装在载体的摇摆中心，甚至距摇摆中心有较长的距离，因此主、子惯导系统中的加速度计会敏感到不同的加速度，从而引起加速度计测量差异，即传递对准中的杆臂效应。为了提高水下运载体捷联惯导系统传递对准的对准精度，必须通过一定的补偿方法对杆臂效应所引起的误差进行补偿以消除其对系统对准的影响。

发明内容

本发明的目的在于提供一种提高水下运载体捷联惯导系统初始对准的精度的消除水下运载体捷联惯导系统杆臂效应误差的对准方法。

本发明的目的是这样实现的：

本发明消除水下运载体捷联惯导系统杆臂效应误差的对准方法，其特征是：

(1)将水下运载体捷联惯导系统与船舶中心高精度的惯导系统相连接；

(2)把船舶中心高精度的惯导系统作为主惯导系统，且使其完成初始对准并进入导航状态；

(3)把水下运载体捷联惯导系统作为子惯导系统，使其采集加速度计和陀螺仪输出的数据；

(4)将主惯导系统的初始速度参数、初始位置参数装订至子惯导系统的导航计算机中；

(5)将主惯导系统输出的姿态信息传输给子惯导系统，粗略计算出子惯导系统的初始姿态，完成主、子惯导系统间的一步传递；

(6)子惯导系统根据步骤(3)采集到的加速度计和陀螺仪输出的数据以及步骤(4)与步骤(5)得到的初始速度、初始位置和初始姿态进行导航递推解算，计算子惯导系统的速度，然后计算子惯导系统Z轴方向上的姿态角；

(7)建立以主子惯导系统之间的速度误差、姿态误差以及杆臂长度为状态变量的卡尔曼滤波状态方程；

(8)将主惯导系统输出的速度进行修正后与步骤(6)得到的子惯导系统的速度作差，以及步骤(6)得到的子惯导系统Z轴方向上的姿态角与主惯导系统Z轴方向上的姿态角之间的差值作为观测量，采用卡尔曼滤波方法估计出主惯导系统导航坐标系n与子惯导系统导航坐标系n′之间的失准角；

(9)利用步骤(8)估计出来的失准角对子惯导系统的姿态矩阵进行修正，得到水下运载体准确的初始姿态角，完成系统的初始对准。

本发明的优势在于：不仅能够有效地解决杆臂效应误差补偿的问题，还能提高水下运载体捷联惯导系统初始对准的对准精度。

附图说明

图1为本发明的为本发明的流程图；

图2为本发明的杆臂效应误差的原理图；

图3为X轴杆臂长度的误差曲线；

图4为Y轴杆臂长度的误差曲线；

图5为Z轴杆臂长度的误差曲线；

图6为东向失准角的估计误差曲线；

图7为北向失准角的估计误差曲线；

图8为方位失准角的估计误差曲线。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1～2，本发明方法的步骤为：

1、将水下运载体捷联惯导系统与舰船中心高精度的惯导系统通过数据传输电缆相连接；

2、舰船中心高精度的惯导系统捷作为主惯导系统已经完成初始对准并进入导航状态；

3、将水下运载体捷联惯导系统作为子惯导系统，预热准备后，采集加速度计和陀螺仪输出的数据；

4、通过数据传输电缆将主惯导系统的初始速度参数、初始位置参数(包括初始的经度和纬度)装订至子惯导系统的导航计算机中；

5、将主惯导系统输出的姿态信息传输给子惯导系统，粗略计算出子惯导系统的初始姿态，完成主、子惯导系统间的一步传递；

6、子惯导系统根据步骤3采集到的加速度计和陀螺仪输出的数据以及步骤4与步骤5得到的初始速度、初始位置和初始姿态进行导航递推解算，计算子惯导系统的速度，然后计算子惯导系统Z轴方向上的姿态角；

7、建立以主子惯导系统之间的速度误差、姿态误差以及杆臂长度为状态变量的卡尔曼滤波状态方程；

卡尔曼滤波状态方程为：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)$

其中：X(t)为t时刻系统的状态变量；A(t)和B(t)分别为t时刻系统的状态转移矩阵和噪声矩阵；W(t)为系统噪声。

系统的状态变量为：

X＝[δV_e δV_n φ_e φ_n φ_u ▽_x ▽_y ε_x ε_y ε_z r_x r_y r_z]^T

系统噪声为：

$W={\left[\begin{array}{ccccccccccccc}{W}_{{\▿}_x}& W_{{\▿}_y}& W_{{\mathrm{\ϵ}}_x}& W_{{\mathrm{\ϵ}}_y}& W_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$

其中：δV_e、δV_n分别表示主子惯导系统之间的东向、北向速度误差；φ_e、φ_n和φ_u分别表示东向、北向和方位失准角；▽_x、▽_y分别表示子惯导系统X轴、Y轴的加速度计零偏；ε_x、ε_y和ε_z分别表示子惯导系统X轴、Y轴和Z轴的陀螺仪常值漂移；r_x、r_y和r_z分别表示主子惯导系统之间X轴、Y轴和Z轴的杆臂长度；

分别子惯导系统X轴、Y轴加速度计零偏随机白噪声；

和

分别表示子惯导系统X轴、Y轴和Z轴陀螺仪常值漂移随机白噪声。

设子惯导系统的捷联矩阵

为：

$C_b^{n^{\′}}=\left[\begin{array}{ccc}{C}_{11}^{\′}& C_{12}^{\′}& C_{13}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}& C_{23}^{\′}\\ C_{31}^{\′}& C_{32}^{\′}& C_{33}^{\′}\end{array}\right]$

系统的状态转移矩阵为：

$A=\left[\begin{array}{ccccc}{A}_{11}& A_{12}& C_{2\×2}& 0_{2\×3}& A_{15}\\ A_{21}& A_{22}& 0_{2\×2}& C_b^{n^{\′}}& 0_{3\×3}\\ 0_{8\×2}& 0_{8\×3}& 0_{8\×2}& 0_{8\×3}& 0_{8\×3}\end{array}\right]$

其中：

$A_{12}=\left[\begin{array}{ccc}{C}_{13}{f}_y^b-C_{12}{f}_z^b& C_{11}{f}_z^b-C_{13}{f}_x^b& C_{12}{f}_x^b-C_{11}{f}_z^b\\ C_{23}{f}_y^b-C_{22}{f}_z^b& C_{21}{f}_z^b-C_{23}{f}_x^b& C_{22}{f}_x^b-C_{21}{f}_z^b)\end{array}\right]$

$A_{15}=\left[\begin{array}{ccc}-({\mathrm{\ω}}_y^2+{\mathrm{\ω}}_z^2)& {\mathrm{\ω}}_x{\mathrm{\ω}}_y-{\stackrel{\·}{\mathrm{\ω}}}_z& {\mathrm{\ω}}_y{\mathrm{\ω}}_z+{\stackrel{\·}{\mathrm{\ω}}}_y\\ {\mathrm{\ω}}_x{\mathrm{\ω}}_y+{\stackrel{\·}{\mathrm{\ω}}}_z& -({\mathrm{\ω}}_x^2+{\mathrm{\ω}}_z^2)& {\mathrm{\ω}}_z{\mathrm{\ω}}_y-{\stackrel{\·}{\mathrm{\ω}}}_x\end{array}\right]$

$C_{2\×2}=\left[\begin{array}{cc}{C}_{11}^{\′}& C_{12}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}\end{array}\right]$

式中：ω_ie为地球自转角速度；R_e为地球半径；

为当地纬度；V_e、V_n分别表示子惯导系统的东向、北向速度；

为子惯导系统加速度计输出的比力；

为子惯导系统陀螺仪输出的角速度，

为相应的导数。

系统噪声矩阵为：

$B=\left[\begin{array}{ccc}{C}_{2\×2}& 0_{2\×3}& 0_{2\×8}\\ 0_{3\×2}& C_b^{n^{\′}}& 0_{3\×8}\\ 0_{8\×2}& 0_{8\×3}& 0_{8\×8}\end{array}\right]$

其中：

$C_{2\×2}=\left[\begin{array}{cc}{C}_{11}^{\′}& C_{12}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}\end{array}\right]$

8、将主惯导系统输出的速度进行修正后与步骤6得到的子惯导系统的速度作差，以及步骤6得到的子惯导系统Z轴方向上的姿态角与主惯导系统Z轴方向上的姿态角之间的差值作为观测量，采用卡尔曼滤波方法估计出主惯导系统导航坐标系n与子惯导系统导航坐标系n′之间的失准角；

卡尔曼滤波量测方程为：

Z(t)＝H(t)X(t)+V(t)

其中：Z(t)为t时刻系统的量测量；H(t)为t时刻系统的量测矩阵；V(t)为t时刻系统的量测噪声阵。

系统的量测量为：

$Z={\left[\begin{array}{cc}{Z}_V& Z_{\mathrm{\ψ}}\end{array}\right]}^T={\left[\begin{array}{cc}\mathrm{\δV}-{\mathrm{\ω}}_{\mathrm{eb}}^n\×r^n& \mathrm{\δ\ψ}\end{array}\right]}^T$

其中：

δV＝[δV_e δV_n]^T；

为b系相对于e系的旋转角速度在n系的投影；

rⁿ为主、子惯导系统之间的杆臂长度在n系的投影；

δψ＝ψ_s-ψ_m，ψ_s和ψ_m分别为主、子惯导系统Z轴方向上的姿态角。

系统的量测矩阵为：

$H=\left[\begin{array}{cccc}{I}_{2\×2}& 0_{2\×3}& 0_{2\×5}& H_{14}\\ 0_{1\×2}& H_{22}& 0_{1\×5}& 0_{1\×3}\end{array}\right]$

其中：

$H_{22}=\left[\begin{array}{ccc}\frac{C_{12}{C}_{32}}{C_{12}^2+C_{22}^2}& \frac{C_{22}{C}_{32}}{C_{12}^2+C_{22}^2}& -1\end{array}\right]$

式中：C_ij为主惯导系统捷联矩阵中第i行j列的元素。

9、利用步骤8估计出来的失准角对子惯导系统的姿态矩阵进行修正，得到水下运载体准确的初始姿态角，完成系统的初始对准。

初始姿态角的主值分别为：

式中：

为修正后子惯导系统捷联矩阵

中第i行j列的元素。

横摇角γ的定义域为(0°，360°)，纵摇角θ的定义域为(-90°，90°)，航向角ψ的定义域为(-180°，180°)，得到初始姿态的真值为：

θ＝θ_主

初始对准完成，可以进入导航状态。

对本发明有益的效果说明如下：

在Matlab仿真条件下，对该方法进行仿真实验：

载体的三轴摇摆运动为分别绕纵摇轴、横摇轴和航向轴以正弦规律的运动，其数学模型为：

$\left\{\begin{array}{c}{\mathrm{\θ}}^{\′}={\mathrm{\θ}}_m\sin (2\mathrm{\π}/T_{\mathrm{\θ}}+{\mathrm{\φ}}_{\mathrm{\θ}})\\ {\mathrm{\γ}}^{\′}={\mathrm{\γ}}_m\sin (2\mathrm{\π}/T_{\mathrm{\γ}}+{\mathrm{\φ}}_{\mathrm{\γ}})\\ {\mathrm{\ψ}}^{\′}={\mathrm{\ψ}}_m\sin (2\mathrm{\π}/T_{\mathrm{\ψ}}+{\mathrm{\φ}}_{\mathrm{\ψ}})+{\mathrm{\ψ}}_0\end{array}\right.$

其中：θ_m、γ_m、ψ_m分别为载体纵摇、横摇、艏摇的摇摆幅值；T_θ、T_γ、T_ψ分别为纵摇、横摇、艏摇的摇摆周期；φ_θ、φ_γ、φ_ψ为初始相位；ψ₀为初始航向角。仿真时，θ_m＝5°、γ_m＝6°、ψ_m＝5°；T_θ＝6s、T_γ＝9s、T_ψ＝8s；初始航向ψ₀取30°；初始相位φ_θ、φ_γ、φ_y均为0。

仿真时间：400s；

载体的初始位置：北纬45.7796°，东经126.6705°；

赤道半径：R_e＝6378393.0m；

地球自转角速度：ω_ie＝7.2921158e-5rad/s；

地球表面重力加速度：g＝9.78049m/s²；

子惯导系统陀螺仪常值漂移：0.01°/h；

子惯导系统陀螺仪随机漂移：0.005°/h；

子惯导系统加速度计零偏：10^-4g；

子惯导系统加速度计随机偏差：10^-5g；

杆臂长度：r^b＝[3m 4m 1m]^T

初始姿态误差角：0.1°、0.1°、0.5°；

利用本发明所述方法得到的杆臂长度的误差曲线分别如图3、图4和图5所示；东向失准角、北向失准角和方位失准角的误差曲线分别如图6、图7和图8所示；。结果表明，本发明的方法在实现杆臂长度估计的同时消除了杆臂效应对水下运载体捷联惯导系统初始对准的影响。

Claims (1)

1.消除水下运载体捷联惯导系统杆臂效应误差的对准方法，其特征是：

(1)将水下运载体捷联惯导系统与船舶中心高精度的惯导系统相连接；

(2)将船舶中心高精度的惯导系统作为主惯导系统，且使其完成初始对准并进入导航状态；

(3)将水下运载体捷联惯导系统作为子惯导系统，使其采集加速度计和陀螺仪输出的数据；

(4)将主惯导系统的初始速度参数、初始位置参数装订至子惯导系统的导航计算机中；

(5)将主惯导系统输出的姿态信息传输给子惯导系统，粗略计算出子惯导系统的初始姿态，完成主、子惯导系统间的一步传递；

(6)子惯导系统根据步骤(3)采集到的加速度计和陀螺仪输出的数据以及步骤(4)与步骤(5)得到的初始速度、初始位置和初始姿态进行导航递推解算，计算子惯导系统的速度，然后计算子惯导系统Z轴方向上的姿态角；

(7)建立以主子惯导系统之间的速度误差、姿态误差以及杆臂长度为状态变量的卡尔曼滤波状态方程；

(8)将主惯导系统输出的速度进行修正后与步骤(6)得到的子惯导系统的速度作差，以及步骤(6)得到的子惯导系统Z轴方向上的姿态角与主惯导系统Z轴方向上的姿态角之间的差值作为观测量，采用卡尔曼滤波方法估计出主惯导系统导航坐标系n与子惯导系统导航坐标系n′之间的失准角；

(9)利用步骤(8)估计出来的失准角对子惯导系统的姿态矩阵进行修正，得到水下运载体准确的初始姿态角，完成系统的初始对准。

Images