CN101893445A - 摇摆状态下低精度捷联惯导系统快速初始对准方法 - Google Patents

Abstract

本发明的目的在于提供摇摆状态下低精度捷联惯导系统快速初始对准方法。分以下步骤：确定载体的初始位置参数；采集加速度计和陀螺仪输出的数据，采用二阶调平和方位估计法完成捷联惯导系统的粗对准，初步确定载体的姿态；估计杆臂长度，计算杆臂效应引起的干扰加速度，对加速度计的输出进行补偿；建立卡尔曼滤波状态方程及卡尔曼滤波量测方程；估计出系统的失准角，并在精对准结束时刻用其来修正系统的捷联姿态矩阵，完成初始对准。本发明消除了杆臂效应误差对捷联惯导系统初始对准的影响；缩短了系统的对准时间，提高了系统的对准精度，从而全面提高了低精度捷联惯导系统初始对准的性能。

Description

摇摆状态下低精度捷联惯导系统快速初始对准方法

技术领域

本发明涉及的是用于船舶导航的对准方法。

背景技术

随着惯性技术的不断发展，使用低精度的惯性器件构成捷联航姿系统和组合导航系统已经成为导航领域的研究热点和发展方向。捷联惯导系统初始对准的目的是确定载体在初始时刻的姿态，初始对准是捷联惯导系统的一项关键技术，它直接影响着惯导系统的精度和反应时间。对于低精度的捷联惯导系统来说，惯性测量组件的精度不高，很难有效地完成自对准，特别是方位对准。

舰船在系泊中会因为阵风海浪的干扰而处于摇摆状态，陀螺仪的输出中除了包含有地球自转角速度分量外，还包含着舰船的摇摆信息；由于惯性测量组件的安装位置偏离载体的摇摆运动中心，加速度计的输出中不仅包含有重力加速度分量外，还包含着由杆臂效应引起的干扰加速度。因此，研究解决摇摆状态下低精度捷联惯导系统的初始对准问题具有重要的意义。

发明内容

本发明的目的在于提供能够有效提高摇摆状态下低精度捷联惯导系统初始对准的对准精度并缩短对准时间的摇摆状态下低精度捷联惯导系统快速初始对准方法。

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

本发明摇摆状态下低精度捷联惯导系统快速初始对准方法，其特征是：

(1)通过GPS确定载体的初始位置参数，包括经度和纬度；

(2)采集加速度计和陀螺仪输出的数据、并对采集到的数据进行处理，采用二阶调平和方位估计法完成捷联惯导系统的粗对准，初步确定载体的姿态，即纵摇角θ、横摇角γ和航向角ψ，建立捷联惯导系统的初始捷联矩阵

$C_b^{n^{\′}}=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right];$

(3)首先，根据陀螺仪输出的角速率和和载体的姿态角信息估计杆臂长度：在两个不同的时刻t₁和t₂加速度计各测量一组比力值

和利用不同时刻解算得到的捷联矩阵

和

将其转换到计算地理坐标系，得到

$C_{b1}^{n^{\′}}{f}_1^b=-g_1^{n^{\′}}+C_{b1}^{n^{\′}}\mathrm{\δ}{f}_1^b$

$C_{b2}^{n^{\′}}{f}_2^b=-g_2^{n^{\′}}+C_{b2}^{n^{\′}}\mathrm{\δ}{f}_2^b$

其中

为干扰加速度，

为惯性测量组件的安装位置与载体的摇摆运动中心之间的距离，为陀螺仪输出的角速度，

为相应的导数，

分别为t₁、t₂时刻的重力加速度，且

令 $M=\left\{\right[{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b\×]+[{\mathrm{\ω}}_{\mathrm{ib}}^b\×\left]\right[{\mathrm{\ω}}_{\mathrm{ib}}^b\×\left]\right\},$ 即

$\mathrm{\δ}{f}^b={\mathrm{Mr}}_P^b,$

得到

$C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b=(C_{b1}^n{M}_1-C_{b2}^n{M}_2)r_P^b,$

得到杆臂长度为

$r_P^b={(C_{b1}^n{M}_1-C_{b2}^n{M}_2)}^{-1}(C_{b1}^n{f}_{b1}^b-C_{b2}^n{f}_2^b);$

其次，计算杆臂效应引起的干扰加速度：

$\mathrm{\δ}{f}^b={\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b\×r_P^b+{\mathrm{\ω}}_{\mathrm{ib}}^b\×({\mathrm{\ω}}_{\mathrm{ib}}^b\×r_P^b)=\left[\begin{array}{ccc}-({\mathrm{\ω}}_{{\mathrm{ib}}_y}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_y}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_z}& {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_z}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_y}\\ {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_y}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_z}& -({\mathrm{\ω}}_{{\mathrm{ib}}_x}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\ω}}_{{\mathrm{ib}}_y}{\mathrm{\ω}}_{{\mathrm{ib}}_z}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_x}\\ {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_z}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_y}& {\mathrm{\ω}}_{{\mathrm{ib}}_y}{\mathrm{\ω}}_{{\mathrm{ib}}_z}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_x}& -({\mathrm{\ω}}_{{\mathrm{ib}}_x}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_y}^2)\end{array}\right]\left[\begin{array}{c}{r}_{P_x}^b\\ r_{P_y}^b\\ r_{P_z}^b\end{array}\right];$

然后，对加速度计的输出进行补偿：

$f^b=f_0^b-\mathrm{\δ}{f}^b$

其中

为补偿前加速度计输出的比力，

为补偿后加速度计输出的比力；

(4)首先，建立以速度误差和姿态误差为状态变量的卡尔曼滤波状态方程：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW},$

其中X为系统的状态变量，A和B分别为系统的状态转移矩阵和噪声矩阵，W为系统噪声，系统的状态变量为

$X={\left[\begin{array}{cccccccccc}\mathrm{\δ}{V}_e& \mathrm{\δ}{V}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T,$

系统噪声为

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

其中δV_e、δV_n分别表示东向、北向速度误差，φ_e、φ_n和φ_u分别表示东向、北向和方位失准角，分别表示X轴、Y轴加速度计零偏，ε_x、ε_y和ε_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}{cccc}{A}_{11}& A_{12}& C_{2\×2}& 0_{3\×3}\\ A_{21}& A_{22}& 0_{3\×2}& C_b^{n^{\′}}\\ 0_{2\×2}& 0_{2\×3}& 0_{2\×2}& 0_{2\×3}\\ 0_{3\×2}& 0_{3\×3}& 0_{3\×2}& 0_{3\×3}\end{array}\right],$

其中

$A_{12}=\left[\begin{array}{ccc}0& -(C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b)& C_{21}{f}_x^b+C_{22}{f}_y^b+C_{23}{f}_z^b\\ C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b& 0& -(C_{11}{f}_x^b+C_{12}{f}_y^b+C_{13}{f}_z^b)\end{array}\right],$

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

为补偿后加速度计输出的比力，

为陀螺仪输出的角速度，系统噪声矩阵为

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

其中

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

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

其次，建立以速度误差和航向误差为量测量的卡尔曼滤波量测方程：

Z＝HX+V，

其中Z为系统的量测量，H为系统的量测矩阵，V为系统的量测噪声阵，系统的量测量为

Z＝[δV_e δV_n δψ]^T，

其中δψ＝ψ′-ψ为航向误差角，ψ′为捷联惯导系统解算的航向角，ψ为对磁航向传感器的输出进行修正后的航向角，系统的量测矩阵为

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& k_1& k_2& 1& 0& 0& 0& 0& 0\end{array}\right],$

其中k₁＝-tanθsinψ，k₂＝-tanθcosψ，θ、ψ分别为载体的纵摇角和航向角；

(5)利用卡尔曼滤波方法估计出系统的失准角，并在精对准结束时刻用其来修正系统的捷联姿态矩阵：

计算地理坐标系n′与当地地理坐标系n之间的转换矩阵为

$C_{n^{\′}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& -{\mathrm{\φ}}_e& 1\end{array}\right],$

载体坐标系b与当地地理坐标系n之间的转换矩阵为

$C_b^n=C_{n^{\′}}^n{C}_n^{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],$

根据

确定载体姿态，即载体的纵摇角θ、横摇角γ和航向角ψ的主值分别为

θ_主＝arcsinC′₃₂、

横摇角的定义域为(0°，360°)，纵摇角的定义域为(-90°，90°)，航向角的定义域为(-180°，180°)，得到载体姿态的真值为

θ＝θ_主，

初始对准完成。

本发明的优势在于：补偿了杆臂效应引起的干扰加速度，消除了杆臂效应误差对捷联惯导系统初始对准的影响；增加航向误差为观测量，克服了传统的以速度误差为观测量的静基座对准方法的方位失准角估计时间长、估计效果差的问题，缩短了系统的对准时间，提高了系统的对准精度，从而全面提高了低精度捷联惯导系统初始对准的性能。

附图说明

图1为本发明的流程图；

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

图3为本发明具体实施方式中东向失准角的估计误差曲线；

图4为本发明具体实施方式中北向失准角的估计误差曲线；

图5为本发明具体实施方式中方位失准角的估计误差曲线。

具体实施方式

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

结合图1～2，本发明摇摆状态下低精度捷联惯导系统快速初始对准方法分以下步骤：

(1)通过GPS确定载体的初始位置参数，包括经度和纬度；

(2)采集加速度计和陀螺仪输出的数据、并对采集到的数据进行处理，采用二阶调平和方位估计法完成捷联惯导系统的粗对准，初步确定载体的姿态，即纵摇角θ、横摇角γ和航向角ψ，建立捷联惯导系统的初始捷联矩阵

$C_b^{n^{\′}}=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right];$

(3)首先，根据陀螺仪输出的角速率和和载体的姿态角信息估计杆臂长度：在两个不同的时刻t₁和t₂加速度计各测量一组比力值

和

利用不同时刻解算得到的捷联矩阵

和

将其转换到计算地理坐标系，得到

$C_{b1}^{n^{\′}}{f}_1^b=-g_1^{n^{\′}}+C_{b1}^{n^{\′}}\mathrm{\δ}{f}_1^b$

$C_{b2}^{n^{\′}}{f}_2^b=-g_2^{n^{\′}}+C_{b2}^{n^{\′}}\mathrm{\δ}{f}_2^b$

其中为干扰加速度，为惯性测量组件的安装位置与载体的摇摆运动中心之间的距离，

为陀螺仪输出的角速度，

为相应的导数，

分别为t₁、t₂时刻的重力加速度，且

令 $M=\left\{\right[{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b\×]+[{\mathrm{\ω}}_{\mathrm{ib}}^b\×\left]\right[{\mathrm{\ω}}_{\mathrm{ib}}^b\×\left]\right\},$ 即

$\mathrm{\δ}{f}^b={\mathrm{Mr}}_P^b,$

得到

$C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b=(C_{b1}^n{M}_1-C_{b2}^n{M}_2)r_P^b,$

得到杆臂长度为

$r_P^b={(C_{b1}^n{M}_1-C_{b2}^n{M}_2)}^{-1}(C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b);$

其次，计算杆臂效应引起的干扰加速度：

$\mathrm{\δ}{f}^b={\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b\×r_P^b+{\mathrm{\ω}}_{\mathrm{ib}}^b\×({\mathrm{\ω}}_{\mathrm{ib}}^b\×r_P^b)=\left[\begin{array}{ccc}-({\mathrm{\ω}}_{{\mathrm{ib}}_y}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_y}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_z}& {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_z}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_y}\\ {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_y}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_z}& -({\mathrm{\ω}}_{{\mathrm{ib}}_x}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\ω}}_{{\mathrm{ib}}_y}{\mathrm{\ω}}_{{\mathrm{ib}}_z}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_x}\\ {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_z}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_y}& {\mathrm{\ω}}_{{\mathrm{ib}}_y}{\mathrm{\ω}}_{{\mathrm{ib}}_z}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_x}& -({\mathrm{\ω}}_{{\mathrm{ib}}_x}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_y}^2)\end{array}\right]\left[\begin{array}{c}{r}_{P_x}^b\\ r_{P_y}^b\\ r_{P_z}^b\end{array}\right];$

然后，对加速度计的输出进行补偿：

$f^b=f_0^b-\mathrm{\δ}{f}^b$

其中

为补偿前加速度计输出的比力，

为补偿后加速度计输出的比力；

(4)首先，建立以速度误差和姿态误差为状态变量的卡尔曼滤波状态方程：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW},$

其中X为系统的状态变量，A和B分别为系统的状态转移矩阵和噪声矩阵，W为系统噪声，系统的状态变量为

$X={\left[\begin{array}{cccccccccc}\mathrm{\δ}{V}_e& \mathrm{\δ}{V}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T,$

系统噪声为

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

其中δV_e、δV_n分别表示东向、北向速度误差，φ_e、φ_n和φ_u分别表示东向、北向和方位失准角，分别表示X轴、Y轴加速度计零偏，ε_x、ε_y和ε_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}{cccc}{A}_{11}& A_{12}& C_{2\×2}& 0_{3\×3}\\ A_{21}& A_{22}& 0_{3\×2}& C_b^{n^{\′}}\\ 0_{2\×2}& 0_{2\×3}& 0_{2\×2}& 0_{2\×3}\\ 0_{3\×2}& 0_{3\×3}& 0_{3\×2}& 0_{3\×3}\end{array}\right],$

其中

$A_{12}=\left[\begin{array}{ccc}0& -(C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b)& C_{21}{f}_x^b+C_{22}{f}_y^b+C_{23}{f}_z^b\\ C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b& 0& -(C_{11}{f}_x^b+C_{12}{f}_y^b+C_{13}{f}_z^b)\end{array}\right],$

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

为补偿后加速度计输出的比力，

为陀螺仪输出的角速度，系统噪声矩阵为

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

其中

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

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

其次，建立以速度误差和航向误差为量测量的卡尔曼滤波量测方程：

Z＝HX+V，

其中Z为系统的量测量，H为系统的量测矩阵，V为系统的量测噪声阵，系统的量测量为

Z＝[δV_e δV_n δψ]^T，

其中δψ＝ψ′-ψ为航向误差角，ψ′为捷联惯导系统解算的航向角，ψ为对磁航向传感器的输出进行修正后的航向角，系统的量测矩阵为

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& k_1& k_2& 1& 0& 0& 0& 0& 0\end{array}\right],$

其中k₁＝-tanθsinψ，k₂＝-tanθcosψ，θ、ψ分别为载体的纵摇角和航向角；

(5)利用卡尔曼滤波方法估计出系统的失准角，并在精对准结束时刻用其来修正系统的捷联姿态矩阵：

计算地理坐标系n′与当地地理坐标系n之间的转换矩阵为

$C_{n^{\′}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& -{\mathrm{\φ}}_e& 1\end{array}\right],$

载体坐标系b与当地地理坐标系n之间的转换矩阵为

$C_b^n=C_{n^{\′}}^n{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],$

根据

确定载体姿态，即载体的纵摇角θ、横摇角γ和航向角ψ的主值分别为

θ_主＝arcsinC′₃₂、

横摇角的定义域为(0°，360°)，纵摇角的定义域为(-90°，90°)，航向角的定义域为(-180°，180°)，得到载体姿态的真值为

θ＝θ_主，

初始对准完成。

利用本发明摇摆状态下低精度捷联惯导系统快速初始对准方法进行仿真实验：

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

$\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＝6°、γ_m＝6°、ψ_m＝5°；T_θ＝7s、T_γ＝9s、T_ψ＝8s；初始航向为ψ₀取30°；初始相位φ_θ、φ_r、φ_y均为0。

仿真时间：300s；

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

赤道半径：R_e＝6378393.0m；

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

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

杆臂长度： $r_P^b={\left[\begin{array}{ccc}0.3m& 0.4m& 0.2m\end{array}\right]}^T;$

陀螺仪常值漂移：0.1°/h；

陀螺仪随机漂移：0.01°/h；

加速度计零偏：10^-4g；

加速度计随机偏差：10^-5g；

磁航向传感器的精度：0.1°；

粗对准结束后的姿态误差角：0.5°、0.5°、1°；

利用本发明所述方法得到的东向失准角、北向失准角和方位失准角的误差曲线分别如图3、图4和图5所示。结果表明，在摇摆状态下，采用本发明的方法能够有效地缩短方位失准角的估计时间，提高低成本捷联惯导系统初始对准的对准精度。

Claims (1)

1.摇摆状态下低精度捷联惯导系统快速初始对准方法，其特征是：

(1)通过GPS确定载体的初始位置参数，包括经度和纬度；

(2)采集加速度计和陀螺仪输出的数据、并对采集到的数据进行处理，采用二阶调平和方位估计法完成捷联惯导系统的粗对准，初步确定载体的姿态，即纵摇角θ、横摇角γ和航向角ψ，建立捷联惯导系统的初始捷联矩阵

$C_b^{n^{\′}}=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right];$

(3)首先，根据陀螺仪输出的角速率和和载体的姿态角信息估计杆臂长度：在两个不同的时刻t₁和t₂加速度计各测量一组比力值

和利用不同时刻解算得到的捷联矩阵

和

将其转换到计算地理坐标系，得到

$C_{b1}^{n^{\′}}{f}_1^b=-g_1^{n^{\′}}+C_{b1}^{n^{\′}}\mathrm{\δ}{f}_1^b$

$C_{b2}^{n^{\′}}{f}_2^b=-g_2^{n^{\′}}+C_{b2}^{n^{\′}}\mathrm{\δ}{f}_2^b$

其中

为干扰加速度，

为惯性测量组件的安装位置与载体的摇摆运动中心之间的距离，

为陀螺仪输出的角速度，

为相应的导数，

分别为t₁、t₂时刻的重力加速度，且

令 $M=\left\{\right[{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b\×]+[{\mathrm{\ω}}_{\mathrm{ib}}^b\×\left]\right[{\mathrm{\ω}}_{\mathrm{ib}}^b\×\left]\right\},$ 即

$\mathrm{\δ}{f}^b={\mathrm{Mr}}_P^b,$

得到

$C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b=(C_{b1}^n{M}_1-C_{b2}^n{M}_2)r_P^b,$

得到杆臂长度为

$r_P^b={(C_{b1}^n{M}_1-C_{b2}^n{M}_2)}^{-1}(C_{b1}^n{f}_1^b-C_{b2}^n{f}_2^b);$

其次，计算杆臂效应引起的干扰加速度：

$\mathrm{\δ}{f}^b={\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ib}}^b\×r_P^b+{\mathrm{\ω}}_{\mathrm{ib}}^b\×({\mathrm{\ω}}_{\mathrm{ib}}^b\×r_P^b)=\left[\begin{array}{ccc}-({\mathrm{\ω}}_{{\mathrm{ib}}_y}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_y}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_z}& {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_z}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_y}\\ {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_y}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_z}& -({\mathrm{\ω}}_{{\mathrm{ib}}_x}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_z}^2)& {\mathrm{\ω}}_{{\mathrm{ib}}_y}{\mathrm{\ω}}_{{\mathrm{ib}}_z}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_x}\\ {\mathrm{\ω}}_{{\mathrm{ib}}_x}{\mathrm{\ω}}_{{\mathrm{ib}}_z}-{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_y}& {\mathrm{\ω}}_{{\mathrm{ib}}_y}{\mathrm{\ω}}_{{\mathrm{ib}}_z}+{\stackrel{\·}{\mathrm{\ω}}}_{{\mathrm{ib}}_x}& -({\mathrm{\ω}}_{{\mathrm{ib}}_x}^2+{\mathrm{\ω}}_{{\mathrm{ib}}_y}^2)\end{array}\right]\left[\begin{array}{c}{r}_{P_x}^b\\ r_{P_y}^b\\ r_{P_z}^b\end{array}\right];$

然后，对加速度计的输出进行补偿：

$f^b=f_0^b-\mathrm{\δ}{f}^b$

其中

为补偿前加速度计输出的比力，

为补偿后加速度计输出的比力；

(4)首先，建立以速度误差和姿态误差为状态变量的卡尔曼滤波状态方程：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW},$

其中X为系统的状态变量，A和B分别为系统的状态转移矩阵和噪声矩阵，W为系统噪声，系统的状态变量为

$X={\left[\begin{array}{cccccccccc}\mathrm{\δ}{V}_e& \mathrm{\δ}{V}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T,$

系统噪声为

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

其中δV_e、δV_n分别表示东向、北向速度误差，φ_e、φ_n和φ_u分别表示东向、北向和方位失准角，

分别表示X轴、Y轴加速度计零偏，ε_x、ε_y和ε_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}{cccc}{A}_{11}& A_{12}& C_{2\×2}& 0_{3\×3}\\ A_{21}& A_{22}& 0_{3\×2}& C_b^{n^{\′}}\\ 0_{2\×2}& 0_{2\×3}& 0_{2\×2}& 0_{2\×3}\\ 0_{3\×2}& 0_{3\×3}& 0_{3\×2}& 0_{3\×3}\end{array}\right],$

其中

$A_{12}=\left[\begin{array}{ccc}0& -(C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b)& C_{21}{f}_x^b+C_{22}{f}_y^b+C_{23}{f}_z^b\\ C_{31}{f}_x^b+C_{32}{f}_y^b+C_{33}{f}_z^b& 0& -(C_{11}{f}_x^b+C_{12}{f}_y^b+C_{13}{f}_z^b)\end{array}\right],$

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

为补偿后加速度计输出的比力，

为陀螺仪输出的角速度，系统噪声矩阵为

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

其中

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

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

其次，建立以速度误差和航向误差为量测量的卡尔曼滤波量测方程：

Z＝HX+V，

其中Z为系统的量测量，H为系统的量测矩阵，V为系统的量测噪声阵，系统的量测量为

Z＝[δV_e δV_n δψ]^T，

其中δψ＝ψ′-ψ为航向误差角，ψ′为捷联惯导系统解算的航向角，ψ为对磁航向传感器的输出进行修正后的航向角，系统的量测矩阵为

$H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& k_1& k_2& 1& 0& 0& 0& 0& 0\end{array}\right],$

其中k₁＝-tanθsinψ，k₂＝-tanθcosψ，θ、ψ分别为载体的纵摇角和航向角；

(5)利用卡尔曼滤波方法估计出系统的失准角，并在精对准结束时刻用其来修正系统的捷联姿态矩阵：

计算地理坐标系n′与当地地理坐标系n之间的转换矩阵为

$C_{n^{\′}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& -{\mathrm{\φ}}_e& 1\end{array}\right],$

载体坐标系b与当地地理坐标系n之间的转换矩阵为

$C_b^n=C_{n^{\′}}^n{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],$

根据

确定载体姿态，即载体的纵摇角θ、横摇角γ和航向角ψ的主值分别为

θ_主＝arcsinC′₃₂、

横摇角的定义域为(0°，360°)，纵摇角的定义域为(-90°，90°)，航向角的定义域为(-180°，180°)，得到载体姿态的真值为

θ＝θ_主，

初始对准完成。

Images