CN103245357A - 一种船用捷联惯导***二次快速对准方法 - Google Patents

Abstract

本发明提供的是一种船用捷联惯性导航***二次快速对准方法。***预热后连续采集光纤陀螺仪和石英挠性加速度计输出数据，对采集数据进行处理完成捷联惯导***的粗对准。粗对准结束后进入第一次精对准，建立船用捷联惯性导航***误差方程，利用速度误差作为观测量进行卡尔曼滤波获得失准角估计值，并对船体姿态进行补偿。在第一次精对准的基础上，利用已补偿的姿态作为参考姿态，将残余失准角扩充为新的状态变量，采取速度加失准角误差匹配方法，进一步完成水平和方位对准。采用本发明的方法可以在保证对准精度和快速性的要求下，实现对水平加速度计零偏的准确估计。

Description

一种船用捷联惯导***二次快速对准方法

技术领域

本发明涉及的是一种船用捷联惯性导航***对准方法，特别是涉及一种利用速度加失准角误差匹配的两时间段的二次快速对准方法。

背景技术

初始对准是惯性导航***导航工作的前提，初始对准误差是惯性导航***主要的误差源之一，其对***误差的影响不仅表现在姿态上，而且表现在速度和位置信息的获取上，对准精度直接影响着导航精度。因此，在导航前得到精确的初始姿态尤为重要。

捷联惯性导航是现在导航技术中较热门的技术，受到了越来越多的导航界人士的青睐，而且通过捷联惯导***（SINS）与其他导航***的组合，可以提高导航***初始对准的精度，常用的组合有SINS/多普勒计程仪（DVL），SINS/全球定位***（GPS），SINS/天文导航（CNS）等。针对船用光纤陀螺捷联惯性导航***，以速度为外观测量的初始对准多采用SINS/DVL的组合方式。实际中广泛应用以卡尔曼滤波为代表的最优控制方法进行动基座对准，它不但可以克服经典控制理论中船体加速度对对准性能的不利影响，适用于多种运动情况下的初始对准，还可以在估计船体姿态信息的同时估计惯性器件的误差，实现初始对准过程中的测漂，从而实现对准精度的提高。例如哈尔滨工程大学周广涛的专利“基于滤波的光线陀螺捷联惯导***两位置初始对准方法”（申请公布号CN101246022）。该发明公开了一种基于滤波的光线陀螺捷联惯导***两位置初始对准方法，通过载体从第一位置绕方位轴旋转到第二位置，在两个位置进行滤波估计，利用估计值对陀螺的逐次启动误差进行修正，估计出平台失准角，该发明能克服地理系等效陀螺漂移对方位失准角估算精度的影响，但是需要载体能够绕方位轴旋转，而且在两个位置上静止不动。再例如北京航空航天大学崔培玲的专利“一种基于MRUPF的捷联惯导***大方位失准角初始对准方法”（申请公布号CN101975585A）。该发明公开了一种基于MRUPF的捷联惯导***大方位失准角初始对准方法，建立大方位失准角条件下捷联惯导***静基座初始对准的状态空间模型，利用UPF滤波算法进行初始对准的状态估计，通过多分辨方法减少UPF滤波的粒子数目，降低了计算量，在保证初始对准精度的同时提高了大方位失准角下捷联惯导***初始对准的实时性。但是该方法只适用于静基座初始对准，而且忽略了采用滤波估计存在残余失准角的问题。

发明内容

本发明的目的在于提供一种在保证对准精度和快速性的要求下，实现对水平加速度计零偏的准确估计的船用捷联惯导***二次快速对准方法。

本发明的目的是这样实现的，主要包括如下步骤：

步骤1：启动捷联惯性导航***使其进行充分预热，连续采集光纤陀螺仪和石英挠性加速度计输出数据；

步骤2：对采集到的光纤陀螺仪和石英挠性加速度计的数据进行处理，采用惯性系对准方法完成捷联惯性导航***的粗对准；

步骤3：粗对准完毕后进入第一次精对准阶段，继续采集光纤陀螺仪和石英挠性加速度计输出数据，进行导航解算获得船体速度、姿态、位置等信息，通过多普勒计程仪测得船体的速度，并把该速度作为船体的真实速度；

步骤4：建立船用捷联惯性导航***误差方程，以多普勒测量速度与惯性导航***解算速度之差作为观测量，采用卡尔曼滤波对失准角进行估计；

步骤5：利用第一次精对准估计所得失准角对惯性导航***姿态进行补偿，并作为第二次精对准的参考姿态，将残余失准角扩充为新的状态变量，采用速度加失准角误差匹配方法，进一步完成水平和方位对准；

步骤6：利用第二次精对准获得失准角估计值，完成精确初始对准，同时获得水平加速度计零偏估计值。

所述的将残余失准角扩充为新的状态变量的方法如下：

将残余失准角ξ作为状态变量引入到状态空间模型中，则***的状态变量扩充为十三维 $X={\left[\begin{array}{ccccccccccccc}{\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\mathrm{\ξ}}_e& {\mathrm{\ξ}}_n& {\mathrm{\ξ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T,$ 结合惯性导航***误差方程，此时的状态空间模型如下：

其中，φ_e,n,u为失准角，ξ_e,n,u为残余失准角，

δλ为纬度和经度误差，δv_e,δv_n为地理坐标系下东向和北向速度误差，v_e,v_n为东向和北向速度，f_e,f_n,f_u为东向、北向和天向石英扰性加速度计输出，ω_ie为地球自转角速率，

为船体所在当地纬度，R为地球半径，g为当地重力加速度，为地理坐标系下东向和北向石英扰性加速度计零偏，ε_e,n,u为地理坐标系下东向、北向和天向陀螺漂移，

为载体坐标系下x,y轴的石英扰性加速度计零偏，ε_x,y,z为载体坐标系下x,y,z轴的陀螺漂移，

用到的***状态方程和量测方程如下：

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

Z=HX+V

状态变量扩充为十三维：

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

W=[w_ax w_ay w_gx w_gy w_gz 0 0 0 0 0 0 0 0]^T

其中，w_ax,w_ay为加速度计白噪声误差，w_gx,w_gy,w_gz为陀螺白噪声误差；

$A=\left[\begin{array}{ccc}{F}_{5\×5}& 0_{5\×3}& T_{5\×5}\\ 0_{3\×5}& 0_{3\×3}& 0_{3\×5}\\ 0_{5\×5}& 0_{5\×3}& 0_{5\×5}\end{array}\right],$ $F_{5\×5}=\left[\begin{array}{cc}{F}_{2\×2}& F_{2\×3}\\ F_{3\×2}& F_{3\×3}\end{array}\right],$ $B=\left[\begin{array}{cc}{T}_{5\×5}& 0_{5\×8}\\ 0_{8\×5}& 0_{8\×8}\end{array}\right]$

$F_{2\×3}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& {-f}_e\end{array}\right],$

$T_{5\×5}=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& 0& 0& 0\\ C_{21}& C_{22}& 0& 0& 0\\ 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& C_{31}& C_{32}& C_{33}\end{array}\right],$ 0_m×n表示元素都为零的m行n列矩阵，

观测模型如下：

$E=I_{3\×3}-{\hat{C}}_b^n{C}_{n^{\′}}^b=I_{3\×3}-(I_{3\×3}+(\mathrm{\ξ}\×)-(\mathrm{\φ}\×))=(\mathrm{\φ}-\mathrm{\ξ})\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& {-a}_1\\ -a_2& a_1& 0\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],$ I_3×3表示3阶单位矩阵， $(\mathrm{\φ}\×)=\left[\begin{array}{ccc}0& {-\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& {-\mathrm{\φ}}_e\\ {-\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right]$ 为φ=[φ_e φ_n φ_u]^T的反对称阵， $(\mathrm{\ξ}\×)=\left[\begin{array}{ccc}0& {-\mathrm{\ξ}}_u& {\mathrm{\ξ}}_n\\ {\mathrm{\ξ}}_u& 0& -{\mathrm{\ξ}}_e\\ -{\mathrm{\ξ}}_n& {\mathrm{\ξ}}_e& 0\end{array}\right]$ 为ξ=[ξ_e ξ_n ξ_u]^T的反对称阵，(φ-ξ)×为φ-ξ的反对称阵，a₁,a₂,a₃为(φ-ξ)×中元素，

从观测模型的反对称阵中提取姿态观测量可表示为：

$Z_a=\left[\begin{array}{c}{\mathrm{\φ}}_e-{\mathrm{\ξ}}_e\\ {\mathrm{\φ}}_n-{\mathrm{\ξ}}_n\\ {\mathrm{\φ}}_n-{\mathrm{\ξ}}_n\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$

其中，a₁,a₂,a₃为从反对称矩阵

中提取的元素，将上式中的三个失准角误差扩充到观测量中，则二次对准的速度加失准角误差观测量为Z=[δv_e δv_n φ_e-ξ_e φ_n-ξ_n φ_u-ξ_u]^T，因此，***的量测矩阵H可表示为：

$H=\left[\begin{array}{cccc}{I}_{2\×2}& & 0_{2\×11}& \\ 0_{3\×2}& I_{3\×3}& -I_{3\×3}& 0_{3\×5}\end{array}\right]$

其中，I_n×n表示n阶单位矩阵，0_m×n表示元素都为零的m行n列矩阵。

本发明的方法具有如下优点：（1）有效地克服了一次精对准中由于水平加速度计和东向陀螺漂移等不可观测状态限制失准角估计精度的缺点。（2）在实现更快速更高精度的水平和方位对准的同时，估计出水平加速度计零偏，能够进一步抑制器件误差对舰船导航参数的影响。

为了验证本发明的实用性，进行了Matlab仿真实验。

陀螺常值漂移为：ε_x=0.01°/h,ε_y=0.03°/h,ε_z=0.02°/h，陀螺随机游走为：ε_ri=0.005°/h(i=x,y,z)，刻度因数误差为：δK_gi=10ppm(i=x,y,z)；

加速度计零偏为： ${\▿}_x=1.0\×{10}^{-4}g,$ ${\▿}_y=3.0\×{10}^{-4}g,$ ${\▿}_z=2.0\×{10}^{-4}g,$ 加速度计噪声为：

(i=x,y,z)，刻度因数误差为：δK_ai=10ppm(i=x,y,z)；

初始位置为：经度为126.6705°，纬度为45.7796°；

初始失准角为：φ(0)=[0.1° 0.1° 0.5°]^T；

仿真时间为400秒，忽略粗对准时间，第一次精对准时间为100秒，第二次精对准时间为300秒，第一次精对准结束后利用所估计失准角对解算姿态进行补偿，补偿后姿态作为第二次精对准时的参考姿态。

仿真结果表明船用捷联惯性导航***在进行精对准时，采用本发明的方法可以获得较一次精对准更高的精度，且在保证对准精度和快速性的要求下实现对水平加速度计零偏的估计。

附图说明

图1为本发明的流程图；

图2为东向失准角误差曲线；

图3为北向失准角误差曲线；

图4为方位失准角误差曲线；

图5为水平加速度计零偏估计效果。

具体实施方式

下面结合附图对本发明作进一步描述：

结合图1。本发明包括以下步骤：

步骤1：启动捷联惯性导航***使其进行充分预热，连续采集光纤陀螺仪和石英挠性加速度计输出数据。

步骤2：对采集到的光纤陀螺仪和石英挠性加速度计的数据进行处理，采用惯性系对准方法完成捷联惯导***的粗对准，确定此时船体的姿态信息（纵摇角θ，横摇角γ，航向角ψ）。

步骤3：粗对准完毕后进入第一次精对准阶段，继续采集光纤陀螺仪和石英挠性加速度计输出数据，进行导航解算获得船体速度、姿态、位置等信息，通过多普勒计程仪测得船体的速度，并把该速度作为船体的真实速度。

步骤4：建立船用捷联惯性导航***误差方程。不考虑垂直通道，通常的增广状态为七维误差向量，误差方程如下：

其中，φ_e,n,u为失准角，

δλ为纬度和经度误差，δv_e,δv_n为地理坐标系下东向和北向速度误差，v_e,v_n为东向和北向速度，f_e,f_n为东向、北向和天向加速度计输出，ω_ie为地球自转角速率，

为船体所在当地纬度和经度，R为地球半径，g为当地重力加速度，

为地理坐标系下东向和北向加速度计零偏，ε_e,n,u为地理坐标系下东向、北向和天向陀螺漂移。

为载体坐标系下x,y轴的加速度计零偏，ε_x,y,z为载体坐标系下x,y,z轴的陀螺漂移。

以多普勒测量速度与惯性导航***解算速度之差作为观测量，采用卡尔曼滤波对失准角进行估计。所用到的状态方程和量测方程如下：

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

Z=HX+V

对于船用捷联惯导***来说，***状态变量选取十维：

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

W=[w_ax w_ay w_gx w_gy w_gz 0 0 0 0 0]^T

观测量选择如下：

Z=[δv_e δv_n]^T

其中，

$A=\left[\begin{array}{cc}{F}_{5\×5}& T_{5\×5}\\ 0_{5\×5}& 0_{5\×5}\end{array}\right],$ $F_{5\×5}=\left[\begin{array}{cc}{F}_{2\×2}& F_{2\×3}\\ F_{3\×2}& F_{3\×3}\end{array}\right],$ $B=\left[\begin{array}{cc}{T}_{5\×5}& 0_{5\×5}\\ 0_{5\×5}& 0_{5\×5}\end{array}\right]$

$F_{2\×3}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& {-f}_e\end{array}\right],$

$T_{5\×5}=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& 0& 0& 0\\ C_{21}& C_{22}& 0& 0& 0\\ 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& C_{31}& C_{32}& C_{33}\end{array}\right],$ 0_m×n表示元素都为零的m行n列矩阵。

***观测矩阵： $H\left(t\right)=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

步骤5：利用第一次精对准估计所得失准角对惯性导航***姿态进行补偿，并作为第二次精对准的参考姿态，将失准角估计误差扩充为新的状态变量，采用速度加残余失准角匹配方法，进一步完成水平和方位对准。

将残余失准角ξ作为状态变量引入到状态空间模型中，则***的状态变量扩充为十三维 $X={\left[\begin{array}{ccccccccccccc}{\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\mathrm{\ξ}}_e& {\mathrm{\ξ}}_n& {\mathrm{\ξ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T.$ 结合步骤4中的惯性导航***误差方程，此时的状态空间模型入下：

其中，φ_e,n,u为失准角，ξ_e,n,u为残余失准角，

δλ为纬度和经度误差，δv_e,δv_n为地理坐标系下东向和北向速度误差，v_e,v_n为东向和北向速度，f_e,f_n为东向、北向和天向加速度计输出，ω_ie为地球自转角速率，

为船体所在当地纬度和经度，R为地球半径，g为当地重力加速度，

为地理坐标系下东向和北向加速度计零偏，ε_e,n,u为地理坐标系下东向、北向和天向陀螺漂移。

为载体坐标系下x,y轴的加速度计零偏，ε_x,y,z为载体坐标系下x,y,z轴的陀螺漂移。

步骤5中用到的***状态方程和量测方程如下：

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

Z=HX+V

状态变量扩充为十三维：

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

W=[w_ax w_ay w_gx w_gy w_gz 0 0 0 0 0 0 0 0]^T

其中，w_ax,w_ay为加速度计白噪声误差，w_gx,w_gy,w_gz为陀螺白噪声误差；

$A=\left[\begin{array}{ccc}{F}_{5\×5}& 0_{5\×3}& T_{5\×5}\\ 0_{3\×5}& 0_{3\×3}& 0_{3\×5}\\ 0_{5\×5}& 0_{5\×3}& 0_{5\×5}\end{array}\right],$ $F_{5\×5}=\left[\begin{array}{cc}{F}_{2\×2}& F_{2\×3}\\ F_{3\×2}& F_{3\×3}\end{array}\right],$ $B=\left[\begin{array}{cc}{T}_{5\×5}& 0_{5\×8}\\ 0_{8\×5}& 0_{8\×8}\end{array}\right]$

$F_{2\×3}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& {-f}_e\end{array}\right],$

$T_{5\×5}=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& 0& 0& 0\\ C_{21}& C_{22}& 0& 0& 0\\ 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& C_{31}& C_{32}& C_{33}\end{array}\right],$ 0_m×n表示元素都为零的m行n列矩阵。

观测模型如下：

$E=I_{3\×3}-{\hat{C}}_b^n{C}_{n^{\′}}^b=I_{3\×3}-(I_{3\×3}+(\mathrm{\ξ}\×)-(\mathrm{\φ}\×))=(\mathrm{\φ}-\mathrm{\ξ})\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& {-a}_1\\ -a_2& a_1& 0\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],$ I_3×3表示3阶单位矩阵， $(\mathrm{\φ}\×)=\left[\begin{array}{ccc}0& {-\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& {-\mathrm{\φ}}_e\\ {-\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right]$ 为φ=[φ_e φ_n φ_u]^T的反对称阵， $(\mathrm{\ξ}\×)=\left[\begin{array}{ccc}0& {-\mathrm{\ξ}}_u& {\mathrm{\ξ}}_n\\ {\mathrm{\ξ}}_u& 0& -{\mathrm{\ξ}}_e\\ -{\mathrm{\ξ}}_n& {\mathrm{\ξ}}_e& 0\end{array}\right]$ 为ξ=[ξ_e ξ_n ξ_u]^T的反对称阵，(φ-ξ)×为φ-ξ的反对称阵，a₁,a₂,a₃为(φ-ξ)×中元素。

由观测模型的反对称阵中提取姿态观测量可表示为：

$Z_a=\left[\begin{array}{c}{\mathrm{\φ}}_e-{\mathrm{\ξ}}_e\\ {\mathrm{\φ}}_n-{\mathrm{\ξ}}_n\\ {\mathrm{\φ}}_n-{\mathrm{\ξ}}_n\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$

其中，a₁,a₂,a₃为从反对称矩阵

中提取的元素。将上式中的三个失准角误差扩充到观测量中，则二次对准的速度加失准角误差观测量为Z=[δv_e δv_n φ_e-ξ_e φ_n-ξ_n φ_u-ξ_u]^T。因此，***的量测矩阵H可表示为：

$H=\left[\begin{array}{cccc}{I}_{2\×2}& & 0_{2\×11}& \\ 0_{3\×2}& I_{3\×3}& -I_{3\×3}& 0_{3\×5}\end{array}\right]$

其中，I_n×n表示n阶单位矩阵，0_m×n表示元素都为零的m行n列矩阵。

步骤6：利用第二次精对准获得失准角估计值，完成精确初始对准，同时获得水平加速度计零偏估计值。

结合图2、图3和图4分别表示东向、北向和方位失准角误差曲线，结果表明采用本发明的方法可以获得较一次精对准更高的精度，方位失准角误差由一次精对准的0.16°减小到0.1°。

结合图5。水平加速度计零偏估计曲线表明采用本发明的方法可以使不可观测的水平加速度计零偏变为可观测。

Claims (2)

1.一种船用捷联惯性导航***二次快速对准方法，其特征是：

步骤1：启动捷联惯性导航***使其进行充分预热，连续采集光纤陀螺仪和石英挠性加速度计输出数据；

步骤2：对采集到的光纤陀螺仪和石英挠性加速度计的数据进行处理，采用惯性系对准方法完成捷联惯性导航***的粗对准；

步骤3：粗对准完毕后进入第一次精对准阶段，继续采集光纤陀螺仪和石英挠性加速度计输出数据，进行导航解算获得船体速度、姿态、位置等信息，通过多普勒计程仪测得船体的速度，并把该速度作为船体的真实速度；

步骤4：建立船用捷联惯性导航***误差方程，以多普勒测量速度与惯性导航***解算速度之差作为观测量，采用卡尔曼滤波对失准角进行估计；

步骤5：利用第一次精对准估计所得失准角对惯性导航***姿态进行补偿，并作为第二次精对准的参考姿态，将残余失准角扩充为新的状态变量，采用速度加失准角误差匹配方法，进一步完成水平和方位对准；

步骤6：利用第二次精对准获得失准角估计值，完成精确初始对准，同时获得水平加速度计零偏估计值。

2.根据权利要求书1所述的船用捷联惯性导航***二次快速对准方法，其特征是：所述的将残余失准角扩充为新的状态变量的方法如下：

将残余失准角ξ作为状态变量引入到状态空间模型中，则***的状态变量扩充为十三维 $X={\left[\begin{array}{ccccccccccccc}{\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\mathrm{\ξ}}_e& {\mathrm{\ξ}}_n& {\mathrm{\ξ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T,$ 结合惯性导航***误差方程，此时的状态空间模型如下：

其中，φ_e,n,u为失准角，ξ_e,n,u为残余失准角，

δλ为纬度和经度误差，δv_e,δv_n为地理坐标系下东向和北向速度误差，v_e,v_n为东向和北向速度，f_e,f_n,f_u为东向、北向和天向石英扰性加速度计输出，ω_ie为地球自转角速率，

为船体所在当地纬度，R为地球半径，g为当地重力加速度，

为地理坐标系下东向和北向石英扰性加速度计零偏，ε_e,n,u为地理坐标系下东向、北向和天向陀螺漂移，为载体坐标系下x,y轴的石英扰性加速度计零偏，ε_x,y,z为载体坐标系下x,y,z轴的陀螺漂移，

***状态方程和量测方程如下：

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

Z=HX+V

状态变量扩充为十三维：

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

W=[w_ax w_ay w_gx w_gy w_gz 0 0 0 0 0 0 0 0]^T

其中，w_ax,w_ay为加速度计白噪声误差，w_gx,w_gy,w_gz为陀螺白噪声误差；

$A=\left[\begin{array}{ccc}{F}_{5\×5}& 0_{5\×3}& T_{5\×5}\\ 0_{3\×5}& 0_{3\×3}& 0_{3\×5}\\ 0_{5\×5}& 0_{5\×3}& 0_{5\×5}\end{array}\right],$ $F_{5\×5}=\left[\begin{array}{cc}{F}_{2\×2}& F_{2\×3}\\ F_{3\×2}& F_{3\×3}\end{array}\right],$ $B=\left[\begin{array}{cc}{T}_{5\×5}& 0_{5\×8}\\ 0_{8\×5}& 0_{8\×8}\end{array}\right]$

$F_{2\×3}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& {-f}_e\end{array}\right],$

$T_{5\×5}=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& 0& 0& 0\\ C_{21}& C_{22}& 0& 0& 0\\ 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& C_{31}& C_{32}& C_{33}\end{array}\right],$ 0_m×n表示元素都为零的m行n列矩阵，

观测模型如下：

$E=I_{3\×3}-{\hat{C}}_b^n{C}_{n^{\′}}^b=I_{3\×3}-(I_{3\×3}+(\mathrm{\ξ}\×)-(\mathrm{\φ}\×))=(\mathrm{\φ}-\mathrm{\ξ})\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& {-a}_1\\ -a_2& a_1& 0\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],$ I_3×3表示3阶单位矩阵， $(\mathrm{\φ}\×)=\left[\begin{array}{ccc}0& {-\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 0& {-\mathrm{\φ}}_e\\ {-\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 0\end{array}\right]$ 为φ=[φ_e φ_n φ_u]^T的反对称阵， $(\mathrm{\ξ}\×)=\left[\begin{array}{ccc}0& {-\mathrm{\ξ}}_u& {\mathrm{\ξ}}_n\\ {\mathrm{\ξ}}_u& 0& -{\mathrm{\ξ}}_e\\ -{\mathrm{\ξ}}_n& {\mathrm{\ξ}}_e& 0\end{array}\right]$ 为ξ=[ξ_e ξ_n ξ_u]^T的反对称阵，(φ-ξ)×为φ-ξ的反对称阵，a₁,a₂,a₃为(φ-ξ)×中元素，

从观测模型的反对称阵中提取姿态观测量可表示为：

$Z_a=\left[\begin{array}{c}{\mathrm{\φ}}_e-{\mathrm{\ξ}}_e\\ {\mathrm{\φ}}_n-{\mathrm{\ξ}}_n\\ {\mathrm{\φ}}_n-{\mathrm{\ξ}}_n\end{array}\right]=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$

其中，a₁,a₂,a₃为从反对称矩阵

中提取的元素，将上式中的三个失准角误差扩充到观测量中，则二次对准的速度加失准角误差观测量为Z=[δv_e δv_n φ_e-ξ_e φ_n-ξ_n φ_u-ξ_u]^T，因此，***的量测矩阵H可表示为：

$H=\left[\begin{array}{cccc}{I}_{2\×2}& & 0_{2\×11}& \\ 0_{3\×2}& I_{3\×3}& -I_{3\×3}& 0_{3\×5}\end{array}\right]$

其中，I_n×n表示n阶单位矩阵，0_m×n表示元素都为零的m行n列矩阵。

Images