CN106705992A - 一种双轴光纤惯导系统快速自标定自对准方法 - Google Patents

Abstract

本发明属于惯性导航技术领域，具体涉及一种双轴光纤惯导系统快速自标定自对准方法。它包括以下步骤：步骤1、自标定自对准旋转流程；步骤2、自标定自对准误差模型建立；步骤3、粗对准算法；步骤4、精对准导航滤波；本发明的显著效果是：本发明的一种双轴光纤惯导系统快速自标定自对准方法，针对光纤陀螺标度因数和零偏等稳定性差的缺陷，在双轴光纤惯导系统启动准备过程中，实现了快速自标定和自对准，确保系统在30min内能有效估计系统的主要误差参数，同时获得较高的对准精度。

Description

一种双轴光纤惯导系统快速自标定自对准方法

技术领域

本发明属于惯性导航技术领域，具体涉及一种双轴光纤惯导系统快速自标定自对准方法。

背景技术

国内外双轴激光旋转惯导系统的研究相对较为成熟，激光陀螺标度因数稳定，受温度磁场影响较小，所以系统设计相对简单。而光纤陀螺具有可靠性高，无机抖装置利于旋转机构的控制，且陀螺随机游走误差相对较小利于惯导系统对准的快速收敛等优点。所以，双轴光纤惯导系统引起越来越多人的注意，但光纤陀螺具有自身的缺陷：对温度和磁场较为敏感，同时标度因数和零偏稳定性较差。

发明内容

本发明需要解决的技术问题为：针对光纤陀螺的特性，提供一种适用于双轴光纤惯导系统的快速自标定自对准方法。

本发明是这样实现的：一种双轴光纤惯导系统快速自标定自对准方法，包括以下步骤：

步骤1、自标定自对准旋转流程

30min系统自标定自对准的旋转流程如下所示：

X轴向旋转流程

X轴以30°/s正向旋转450°，旋转完成后停留1s，用时16s；

Z轴向旋转流程，依次进行下述旋转

Z轴以30°/s正向旋转180°，旋转完成后停留38s，用时44s；

Z轴以30°/s正向旋转180°，旋转完成后停留38s，用时44s；

Z轴以30°/s正向旋转90°，旋转完成后停留2s，用时5s；

地向旋转流程

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转90°，旋转完成后停留1s，用时4s；

Y轴向旋转流程

Y轴以30°/s正向旋转540°，旋转完成后停留1s，用时19s；

天向旋转流程

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

以上步骤中每个旋转流程循环5次，即X轴向旋转循环5次，地向旋转循环5次，天向旋转循环5次，

将以上流程循环6次，即整个步骤1循环6次，

采集三个陀螺和三个加速度计的输出数据，内环和外环的旋转角度，

步骤2、自标定自对准误差模型建立

系统状态方程和量测方程如下：

状态方程：

观测方程：Z＝HX+V

H＝[I_5×5 0_5×24]，

为的第一列，

其中，

δV_n、δV_u、δV_e为惯导北、天、东速度误差；

δL、δλ为惯导纬度和经度误差；

φ_n、φ_u、φ_e为惯导北、天、东失准角；

ε_x为惯导X轴(天向)陀螺仪零偏；

为惯导X、Y、Z轴加速度计零偏；

δK_gx、δK_gy、δK_gz为陀螺仪标度因数误差；

δG_xy、δG_yx、δG_xz、δG_zx、δG_yz、δG_zy为陀螺仪安装误差；

为X轴朝上和朝下时，等效Y、Z加速度计零偏的不对称性；

以上23个参数为待求解参数，

A_out为外环角度，该参数由外部给出，

R为地球半径，该参数为常数，由外部给出；

ω_ie为地球自转角速率，该参数为常数，由外部给出；

f_N、f_U、f_E为惯导北、天、东向加速度，该参数由外部给出；

表示的第一列和第二列；和sign是取内部数据的符号，若内部函数大于等于0取正，小于0取负

V_U天向速度、V_N北向速度、V_E动向速度，该三个参数的数值由导航系统给出，L为纬度、外部给定

请解释C₁₁～C₃₃是矩阵中的元素，f_x、f_y是惯导x和y向的加速度，其数值由加速度计输出，经过误差补偿后获得，

ω包含ω_x、ω_y、ω_z，其中ω_x、ω_y、ω_z分别为x、y、z方向的角速度，其数值由陀螺输出经误差补偿后获得，

W＝[w₁ w₂ w₃ 0 0 w₄ w₅ w₆ 0 … 0]^T为23维系统噪声列向量，w₁、w₂、w₃、w₄、w₅、w₆均为零均值随机白噪声；

V＝[v₁ v₂ v₃ v₄ v₅]^T为5维量测噪声列向量，v₁、v₂、v₃、v₄、v₅均为零均值随机白噪声，

步骤3、粗对准算法

由陀螺采集数据经误差补偿后得到的角增量更新四元数：

其中：k初值为0，表示当前时刻；k-1表示上一时刻；I为4×4阶单位矩阵；

由计算姿态矩阵的公式如下：

其中q₀～q₃用本步骤最开始的公式进行计算，计算得到中的四个元素就为q₀～q₃，

将加速度计采集的速度增量Δv(k)＝[v_xk v_yk v_xk]^T，利用姿态矩阵可求得i_b0系下速度：

与互为转置

用下述公式计算速度

T_n采样间隔，外部给定、g重力加速度、L是纬度，

计算：

取中间时刻和该中间时刻指粗对准时间的一半，以及粗对准结束时刻和可构造速度矩阵如下：

利用最后得到的计算

步骤4、精对准导航滤波

粗对准完成后，系统旋转机构开始执行自标定自对准旋转流程，系统开始进行导航解算，其中陀螺标度因数误差初始值设为1，其余各个误差系数的初始值都设定为0；同时进行“速度+位置”匹配闭环卡尔曼滤波估计，主要涉及以下几个误差的修正，

姿态误差的修正

滤波估计得到的姿态误差为则姿态修正方程为：

Tn,是采样间隔、的意义可以统一表示如下，以为例，其表示的是e坐标系相对于n坐标系的角速度的分量表示，

速度误差的修正

滤波估计得到的速度误差为δV_n,δV_u,δV_e，则速度修正方程为：

V_i＝V_i-δV_i (i＝n、u、e)

位置误差的修正

滤波估计得到的位置误差为δL和δλ，则位置修正方程为：

h＝h-δh

λ＝λ-δλ

误差参数的修正

首先更新待辨识的各误差参数，如下所示：

ε_x＝ε_x+X(9)

δK_gx＝δK_gx+X(13)

δK_gy＝δK_gy+X(14)

δK_gz＝δK_gz+X(15)

δG_xy＝δG_xy+X(16)

δG_yx＝δG_yx+X(17)

δG_xz＝δG_xz+X(18)

δG_zx＝δG_zx+X(19)

δG_yz＝δG_yz+X(20)

δG_zy＝δG_zy+X(21)

X(9)～X(23)表示X中第9～23个参数的数值，

由此修正各误差参数：

D_x＝D_x+ε_x

K_Gx＝K_Gx·δK_Gx

K_Gy＝K_Gy0·δK_Gy

K_Gz＝K_Gz0·δK_Gz

G_xy＝G_xy+δG_xy

G_yx＝G_yx+δG_yx

G_xz＝G_xz+δG_xz

G_zx＝G_zx+δG_zx

G_yz＝G_yz+δG_yz

G_zy＝G_zy+δG_zy

上述计算迭代进行，因此使用相同符号表示，

误差进行闭环修正后，各状态变量清零，

X(i)＝0，i＝1,2…23

每一个循环结束时，要对中间变量进行处理：

δG_xy＝δG_xy-δA_yx

δG_xz＝δG_xz-δA_zx

A_yx＝A_yx+δA_yx

A_zx＝A_zx+δA_zx

修正后，两状态清零，

其中，

A_yx、A_zx为惯导系统加速度计安装误差角；

B_x、B_y、B_z为惯导系统加速度计零偏值；

K_gx、K_gy、K_gz为惯导系统陀螺仪标度因数；

G_xy、G_yx、G_xz、G_zx、G_yz、G_zy为惯导系统陀螺仪安装误差角；

D_x、D_y、D_z为惯导系统陀螺仪零偏值，

在惯导系统导航计算时，利用上述参数标定值进行补偿，补偿公式如下：

其中，

N_Ax、N_Ay、N_Az为一个导航周期内加速度计输出脉冲数；

N_Gx、N_Gy、N_Gz为一个导航周期内陀螺仪输出脉冲数；

为惯导系统标定补偿后加速度值；

为惯导系统标定补偿后角速度值。

本发明的显著效果是：本发明的一种双轴光纤惯导系统快速自标定自对准方法，针对光纤陀螺标度因数和零偏等稳定性差的缺陷，在双轴光纤惯导系统启动准备过程中，实现了快速自标定和自对准，确保系统在30min内能有效估计系统的主要误差参数，同时获得较高的对准精度。

具体实施方式

一种双轴光纤惯导系统快速自标定自对准方法，包括以下步骤：

步骤1、自标定自对准旋转流程

30min系统自标定自对准的旋转流程如下所示：

X轴向旋转流程

X轴以30°/s正向旋转450°，旋转完成后停留1s，用时16s；

Z轴向旋转流程，依次进行下述旋转

Z轴以30°/s正向旋转180°，旋转完成后停留38s，用时44s；

Z轴以30°/s正向旋转180°，旋转完成后停留38s，用时44s；

Z轴以30°/s正向旋转90°，旋转完成后停留2s，用时5s；

地向旋转流程

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转90°，旋转完成后停留1s，用时4s；

Y轴向旋转流程

Y轴以30°/s正向旋转540°，旋转完成后停留1s，用时19s；

天向旋转流程

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

以上步骤中每个旋转流程循环5次，即X轴向旋转循环5次，地向旋转循环5次，天向旋转循环5次。

将以上流程循环6次，即整个步骤1循环6次。

采集三个陀螺和三个加速度计的输出数据，内环和外环的旋转角度。

步骤2、自标定自对准误差模型建立

系统状态方程和量测方程如下：

状态方程：

观测方程：Z＝HX+V

H＝[I_5×5 0_5×24]，

为的第一列。

其中，

δV_n、δV_u、δV_e为惯导北、天、东速度误差；

δL、δλ为惯导纬度和经度误差；

φ_n、φ_u、φ_e为惯导北、天、东失准角；

ε_x为惯导X轴(天向)陀螺仪零偏；

为惯导X、Y、Z轴加速度计零偏；

δK_gx、δK_gy、δK_gz为陀螺仪标度因数误差；

δG_xy、δG_yx、δG_xz、δG_zx、δG_yz、δG_zy为陀螺仪安装误差；

为X轴朝上和朝下时，等效Y、Z加速度计零偏的不对称性；

以上23个参数为待求解参数，

A_out为外环角度，该参数由外部给出。

R为地球半径，该参数为常数，由外部给出；

ω_ie为地球自转角速率，该参数为常数，由外部给出；

f_N、f_U、f_E为惯导北、天、东向加速度，该参数由外部给出；

表示的第一列和第二列；和sign是取内部数据的符号，若内部函数大于等于0取正，小于0取负

V_U天向速度、V_N北向速度、V_E动向速度，该三个参数的数值由导航系统给出，L为纬度、外部给定

请解释C₁₁～C₃₃是矩阵中的元素，f_x、f_y是惯导x和y向的加速度，其数值由加速度计输出，经过误差补偿后获得。

ω包含ω_x、ω_y、ω_z，其中ω_x、ω_y、ω_z分别为x、y、z方向的角速度，其数值由陀螺输出经误差补偿后获得。

W＝[w₁ w₂ w₃ 0 0 w₄ w₅ w₆ 0 … 0]^T为23维系统噪声列向量，w₁、w₂、w₃、w₄、w₅、w₆均为零均值随机白噪声；

V＝[v₁ v₂ v₃ v₄ v₅]^T为5维量测噪声列向量，v₁、v₂、v₃、v₄、v₅均为零均值随机白噪声。

步骤3、粗对准算法

由陀螺采集数据经误差补偿后得到的角增量更新四元数：

其中：k初值为0，表示当前时刻；k-1表示上一时刻；I为4×4阶单位矩阵；

由计算姿态矩阵的公式如下：

其中q₀～q₃用本步骤最开始的公式进行计算，计算得到中的四个元素就为q₀～q₃。

将加速度计采集的速度增量Δv(k)＝[v_xk v_yk v_xk]^T，利用姿态矩阵可求得i_b0系下速度：

与互为转置

用下述公式计算速度

T_n采样间隔，外部给定、g重力加速度、L是纬度。

计算：

取中间时刻和该中间时刻指粗对准时间的一半，以及粗对准结束时刻和可构造速度矩阵如下：

利用最后得到的计算

步骤4、精对准导航滤波

粗对准完成后，系统旋转机构开始执行自标定自对准旋转流程，系统开始进行导航解算，其中陀螺标度因数误差初始值设为1，其余各个误差系数的初始值都设定为0；同时进行“速度+位置”匹配闭环卡尔曼滤波估计，主要涉及以下几个误差的修正。

姿态误差的修正

滤波估计得到的姿态误差为则姿态修正方程为：

Tn,是采样间隔、的意义可以统一表示如下，以为例，其表示的是e坐标系相对于n坐标系的角速度的分量表示。

速度误差的修正

滤波估计得到的速度误差为δV_n,δV_u,δV_e，则速度修正方程为：

V_i＝V_i-δV_i (i＝n、u、e)

位置误差的修正

滤波估计得到的位置误差为δL和δλ，则位置修正方程为：

h＝h-δh

λ＝λ-δλ

误差参数的修正

首先更新待辨识的各误差参数，如下所示：

ε_x＝ε_x+X(9)

δK_gx＝δK_gx+X(13)

δK_gy＝δK_gy+X(14)

δK_gz＝δK_gz+X(15)

δG_xy＝δG_xy+X(16)

δG_yx＝δG_yx+X(17)

δG_xz＝δG_xz+X(18)

δG_zx＝δG_zx+X(19)

δG_yz＝δG_yz+X(20)

δG_zy＝δG_zy+X(21)

X(9)～X(23)表示X中第9～23个参数的数值。

由此修正各误差参数：

D_x＝D_x+ε_x

K_Gx＝K_Gx·δK_Gx

K_Gy＝K_Gy0·δK_Gy

K_Gz＝K_Gz0·δK_Gz

G_xy＝G_xy+δG_xy

G_yx＝G_yx+δG_yx

G_xz＝G_xz+δG_xz

G_zx＝G_zx+δG_zx

G_yz＝G_yz+δG_yz

G_zy＝G_zy+δG_zy

上述计算迭代进行，因此使用相同符号表示，

误差进行闭环修正后，各状态变量清零。

X(i)＝0，i＝1,2…23

每一个循环结束时，要对中间变量进行处理：

δG_xy＝δG_xy-δA_yx

δG_xz＝δG_xz-δA_zx

A_yx＝A_yx+δA_yx

A_zx＝A_zx+δA_zx

修正后，两状态清零。

其中，

A_yx、A_zx为惯导系统加速度计安装误差角；

B_x、B_y、B_z为惯导系统加速度计零偏值；

K_gx、K_gy、K_gz为惯导系统陀螺仪标度因数；

G_xy、G_yx、G_xz、G_zx、G_yz、G_zy为惯导系统陀螺仪安装误差角；

D_x、D_y、D_z为惯导系统陀螺仪零偏值。

在惯导系统导航计算时，利用上述参数标定值进行补偿，补偿公式如下：

其中，

N_Ax、N_Ay、N_Az为一个导航周期内加速度计输出脉冲数；

N_Gx、N_Gy、N_Gz为一个导航周期内陀螺仪输出脉冲数；

为惯导系统标定补偿后加速度值；

为惯导系统标定补偿后角速度值。

Claims (1)

1.一种双轴光纤惯导系统快速自标定自对准方法，包括以下步骤：

步骤1、自标定自对准旋转流程

30min系统自标定自对准的旋转流程如下所示：

X轴向旋转流程

X轴以30°/s正向旋转450°，旋转完成后停留1s，用时16s；

Z轴向旋转流程，依次进行下述旋转

Z轴以30°/s正向旋转180°，旋转完成后停留38s，用时44s；

Z轴以30°/s正向旋转180°，旋转完成后停留38s，用时44s；

Z轴以30°/s正向旋转90°，旋转完成后停留2s，用时5s；

地向旋转流程

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转90°，旋转完成后停留1s，用时4s；

Y轴向旋转流程

Y轴以30°/s正向旋转540°，旋转完成后停留1s，用时19s；

天向旋转流程

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以-30°/s正向旋转180°，旋转完成后停留1s，用时7s；

X轴以30°/s正向旋转180°，旋转完成后停留1s，用时7s；

以上步骤中每个旋转流程循环5次，即X轴向旋转循环5次，地向旋转循环5次，天向旋转循环5次，

将以上流程循环6次，即整个步骤1循环6次，

采集三个陀螺和三个加速度计的输出数据，内环和外环的旋转角度，

步骤2、自标定自对准误差模型建立

系统状态方程和量测方程如下：

状态方程： $\stackrel{\·}{X}=AX+W$

观测方程：Z＝HX+V

X＝[δV_n、δV_u、δV_e、δL、δλ、φ_n、φ_u、φ_e、ε_x、▽_x、▽_y、▽_z、

δK_gx、δK_gy、δK_gz、δG_xy、δG_yx、δG_xz、δG_zx、δG_yz、δG_zy、d▽_y、d▽_z]，

$A=\left[\begin{array}{cccccc}F{1}_{3\×8}& 0_{3\×1}& C_b^n& 0_{3\×3}& 0_{3\×6}& sign\left(\cos \left(A_{out}\right)\right)C_b^n\left(:,1:2\right)\\ F{3}_{2\×8}& 0_{2\×1}& 0_{2\×3}& 0_{2\×3}& 0_{2\×6}& 0_{2\×2}\\ F{4}_{3\×8}& -C_b^n\left(;,1\right)& 0_{3\×3}& F{5}_{3\×3}& F{6}_{3\×6}& 0_{3\×2}\\ 0_{15\×8}& 0_{15\×1}& 0_{15\×3}& 0_{15\×3}& 0_{15\×6}& 0_{15\×2}\end{array}\right],$

$F1=\left[\begin{array}{cccccccc}\frac{-V_U}{R}& \frac{-V_N}{R}& -2\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L\right)& -\left(2V_E{\mathrm{\ω}}_{ie}\cos L+\frac{V_E^2}{R}{\sec }^{2}L\right)& 0& 0& -f_E& f_U\\ \frac{2V_N}{R}& 0& \left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}\right)& -2V_E{\mathrm{\ω}}_{ie}\sin L& 0& f_E& 0& -f_N\\ 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R}\tan L& -\left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}\right)& \frac{V_N\tan L-V_U}{R}& 2{\mathrm{\ω}}_{ie}\left(V_U\sin L+V_N\cos L\right)+\frac{V_E{V}_N}{R}{\sec }^{2}L& 0& -f_U& f_N& 0\end{array}\right],$

$F2=\left[\begin{array}{cccccc}{C}_{11}{f}_x& C_{12}{f}_y& C_{13}{f}_z& C_{12}{f}_x& C_{13}{f}_x& C_{13}{f}_y\\ C_{21}{f}_x& C_{22}{f}_y& C_{23}{f}_z& C_{22}{f}_x& C_{23}{f}_x& C_{23}{f}_y\\ C_{31}{f}_x& C_{32}{f}_y& C_{33}{f}_z& C_{32}{f}_x& C_{33}{f}_x& C_{33}{f}_y\end{array}\right],$

$F3=\left[\begin{array}{cccccccc}\frac{1}{R}& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& \frac{\sec L}{R}& \frac{V_E}{R}\tan L\sec L& 0& 0& 0& 0\end{array}\right],$

$F4=\left[\begin{array}{cccccccc}0& 0& \frac{1}{R}& -\mathrm{\ω}\sin L& 0& 0& -\frac{V_N}{R}& -\left(\mathrm{\ω}\sin L+\frac{V_E}{R}\tan L\right)\\ 0& 0& \frac{\tan L}{R}& \mathrm{\ω}\cos L+\frac{V_E}{R}{\sec }^{2}L& 0& \frac{V_N}{R}& 0& \mathrm{\ω}\cos L+\frac{V_E}{R}\\ -\frac{1}{R}& 0& 0& 0& 0& \mathrm{\ω}\sin L+\frac{V_E}{R}\tan L& -\left(\mathrm{\ω}\cos L+\frac{V_E}{R}\right)& 0\end{array}\right],$

$F5=\left[\begin{array}{ccc}-C_{11}{\mathrm{\ω}}_x& -C_{12}{\mathrm{\ω}}_y& -C_{13}{\mathrm{\ω}}_z\\ -C_{21}{\mathrm{\ω}}_x& -C_{22}{\mathrm{\ω}}_y& -C_{23}{\mathrm{\ω}}_z\\ -C_{31}{\mathrm{\ω}}_x& -C_{32}{\mathrm{\ω}}_y& -C_{33}{\mathrm{\ω}}_z\end{array}\right],$

$F6=\left[\begin{array}{cccccc}-C_{11}{\mathrm{\ω}}_y& -C_{12}{\mathrm{\ω}}_x& -C_{11}{\mathrm{\ω}}_z& -C_{13}{\mathrm{\ω}}_x& -C_{12}{\mathrm{\ω}}_z& -C_{13}{\mathrm{\ω}}_y\\ -C_{21}{\mathrm{\ω}}_y& -C_{22}{\mathrm{\ω}}_x& -C_{21}{\mathrm{\ω}}_z& -C_{23}{\mathrm{\ω}}_x& -C_{22}{\mathrm{\ω}}_z& -C_{23}{\mathrm{\ω}}_y\\ -C_{31}{\mathrm{\ω}}_y& -C_{32}{\mathrm{\ω}}_x& -C_{31}{\mathrm{\ω}}_z& -C_{33}{\mathrm{\ω}}_x& -C_{32}{\mathrm{\ω}}_z& -C_{33}{\mathrm{\ω}}_y\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],$

H＝[I_5×5 0_5×24]，

为的第一列，

其中，

δV_n、δV_u、δV_e为惯导北、天、东速度误差；

δL、δλ为惯导纬度和经度误差；

φ_n、φ_u、φ_e为惯导北、天、东失准角；

ε_x为惯导X轴(天向)陀螺仪零偏；

▽_x、▽_y、▽_z为惯导X、Y、Z轴加速度计零偏；

δK_gx、δK_gy、δK_gz为陀螺仪标度因数误差；

δG_xy、δG_yx、δG_xz、δG_zx、δG_yz、δG_zy为陀螺仪安装误差；

d▽_y、d▽_z为X轴朝上和朝下时，等效Y、Z加速度计零偏的不对称性；

以上23个参数为待求解参数，

A_out为外环角度，该参数由外部给出，

R为地球半径，该参数为常数，由外部给出；

ω_ie为地球自转角速率，该参数为常数，由外部给出；

f_N、f_U、f_E为惯导北、天、东向加速度，该参数由外部给出；

表示的第一列和第二列；和sign是取内部数据的符号，若内部函数大于等于0取正，小于0取负

V_U天向速度、V_N北向速度、V_E动向速度，该三个参数的数值由导航系统给出，L为纬度、外部给定

请解释C₁₁～C₃₃是矩阵中的元素，f_x、f_y是惯导x和y向的加速度，其数值由加速度计输出，经过误差补偿后获得，

ω包含ω_x、ω_y、ω_z，其中ω_x、ω_y、ω_z分别为x、y、z方向的角速度，其数值由陀螺输出经误差补偿后获得，

W＝[w₁ w₂ w₃ 0 0 w₄ w₅ w₆ 0 … 0]^T为23维系统噪声列向量，w₁、w₂、w₃、w₄、w₅、w₆均为零均值随机白噪声；

V＝[v₁ v₂ v₃ v₄ v₅]^T为5维量测噪声列向量，v₁、v₂、v₃、v₄、v₅均为零均值随机白噪声，

步骤3、粗对准算法

由陀螺采集数据经误差补偿后得到的角增量更新四元数：

$q_b^{i_{b0}}\left(k\right)=\left\{\cos \frac{{\mathrm{\Δ\θ}}_0}{2}I+\frac{sin\frac{{\mathrm{\Δ\θ}}_0}{2}}{{\mathrm{\Δ\θ}}_0}\[\mathrm{\Δ}\mathrm{\θ}\]\right\}{q}_b^{i_{b0}}(k-1)$

$q_b^{i_{b0}}\left(0\right)={\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right]}^T$

其中：k初值为0，表示当前时刻；k-1表示上一时刻；I为4×4阶单位矩阵；

${\mathrm{\Δ\θ}}_0=\sqrt{{\mathrm{\Δ\θ}}_{nbx}^2+{\mathrm{\Δ\θ}}_{nby}^2+{\mathrm{\Δ\θ}}_{nbz}^2},$

$\[\mathrm{\Δ}\mathrm{\θ}\]=\left[\begin{array}{cccc}0& -{\mathrm{\Δ\θ}}_{nbx}^b& -{\mathrm{\Δ\θ}}_{nby}^b& -{\mathrm{\Δ\θ}}_{nbz}^b\\ {\mathrm{\Δ\θ}}_{nbx}^b& 0& {\mathrm{\Δ\θ}}_{nbz}^b& -{\mathrm{\Δ\θ}}_{nby}^b\\ {\mathrm{\Δ\θ}}_{nby}^b& -{\mathrm{\Δ\θ}}_{nbz}^b& 0& {\mathrm{\Δ\θ}}_{nbx}^b\\ {\mathrm{\Δ\θ}}_{nbz}^b& {\mathrm{\Δ\θ}}_{nby}^b& -{\mathrm{\Δ\θ}}_{nbx}^b& 0\end{array}\right],$

由计算姿态矩阵的公式如下：

$C_{i_{b0}}^b\left(k\right)=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2+q_0{q}_3\right)& 2\left(q_1{q}_3-q_0{q}_2\right)\\ 2\left(q_1{q}_2-q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3+q_0{q}_1\right)\\ 2\left(q_1{q}_3+q_0{q}_2\right)& 2\left(q_2{q}_3-q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

其中q₀～q₃用本步骤最开始的公式进行计算，计算得到中的四个元素就为q₀～q₃，

将加速度计采集的速度增量Δv(k)＝[v_xk v_yk v_xk]^T，利用姿态矩阵可求得i_b0系下速度：

$V^{i_{b0}}\left(k\right)=V^{i_{b0}}(k-1)+C_b^{i_{b0}}\left(k\right)\mathrm{\Δ}v\left(k\right)$

与互为转置

用下述公式计算速度

$V_t^{i_0}\left(k\right)={\left[\begin{array}{ccc}g\sin {\mathrm{LkT}}_n& \frac{g\cos L}{{\mathrm{\ω}}_{iE}}\sin \left({\mathrm{\ω}}_{iE}{\mathrm{kT}}_n\right)& \frac{g\cos L}{{\mathrm{\ω}}_{iE}}\left(1-\cos \left({\mathrm{\ω}}_{iE}{\mathrm{kT}}_n\right)\right)\end{array}\right]}^T$

T_n采样间隔，外部给定、g重力加速度、L是纬度，

计算：

${\mathrm{SV}}^{i_{b0}}\left(k\right)=31/32\×{\mathrm{SV}}^{i_{b0}}(k-1)+1/32\×V^{i_{b0}}\left(k\right)$

${\mathrm{SV}}^{i_0}\left(k\right)=31/32\×{\mathrm{SV}}^{i_0}(k-1)+1/32\×V^{i_0}\left(k\right)$

取中间时刻和该中间时刻指粗对准时间的一半，以及粗对准结束时刻和可构造速度矩阵如下：

${\stackrel{\‾}{V}}^{i_{b0}}={\left[\begin{array}{ccc}{\mathrm{SV}}_{end}^{i_{b0}}& {\mathrm{SV}}_{end}^{i_{b0}}\×{\mathrm{SV}}_{mid}^{i_{b0}}& {\mathrm{SV}}_{end}^{i_{b0}}\×{\mathrm{SV}}_{mid}^{i_{b0}}\×{\mathrm{SV}}_{end}^{i_{b0}}\end{array}\right]}^T$

${\stackrel{\‾}{V}}^{i_0}={\left[\begin{array}{ccc}{\mathrm{SV}}_{end}^{i_0}& {\mathrm{SV}}_{end}^{i_0}\×{\mathrm{SV}}_{mid}^{i_0}& {\mathrm{SV}}_{end}^{i_0}\×{\mathrm{SV}}_{mid}^{i_0}\×{\mathrm{SV}}_{end}^{i_0}\end{array}\right]}^T$

利用最后得到的计算 $C_{i_0}^{i_{b0}}={\stackrel{\‾}{V}}^{i_{b0}}\left(k\right){\stackrel{\‾}{V}}^{i_0}{\left(k\right)}^{-1}$

步骤4、精对准导航滤波

粗对准完成后，系统旋转机构开始执行自标定自对准旋转流程，系统开始进行导航解算，其中陀螺标度因数误差初始值设为1，其余各个误差系数的初始值都设定为0；同时进行“速度+位置”匹配闭环卡尔曼滤波估计，主要涉及以下几个误差的修正，

姿态误差的修正

滤波估计得到的姿态误差为φ_n，φ_u，φ_e，则姿态修正方程为：

${\mathrm{\ω}}_{in}^n={\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n-\frac{1}{T_n}\left[\begin{array}{c}{\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\\ {\mathrm{\φ}}_e\end{array}\right]$

Tn,是采样间隔、的意义可以统一表示如下，以为例，其表示的是e坐标系相对于n坐标系的角速度的分量表示，

速度误差的修正

滤波估计得到的速度误差为δV_n,δV_u,δV_e，则速度修正方程为：

V_i＝V_i-δV_i(i＝n、u、e)

位置误差的修正

滤波估计得到的位置误差为δL和δλ，则位置修正方程为：

h＝h-δh

λ＝λ-δλ

误差参数的修正

首先更新待辨识的各误差参数，如下所示：

ε_x＝ε_x+X(9)

▽_x＝▽_x+X(10)

▽_y＝▽_y+X(11)

▽_z＝▽_z+X(12)

δK_gx＝δK_gx+X(13)

δK_gy＝δK_gy+X(14)

δK_gz＝δK_gz+X(15)

δG_xy＝δG_xy+X(16)

δG_yx＝δG_yx+X(17)

δG_xz＝δG_xz+X(18)

δG_zx＝δG_zx+X(19)

δG_yz＝δG_yz+X(20)

δG_zy＝δG_zy+X(21)

d▽_y＝d▽_y+X(22)

d▽_z＝d▽_z+X(23)

X(9)～X(23)表示X中第9～23个参数的数值，

由此修正各误差参数：

D_x＝D_x+ε_x

B_x＝B_x+▽_x

B_y＝B_y+▽_y

B_z＝B_z+▽_z

K_Gx＝K_Gx·δK_Gx

K_Gy＝K_Gy0·δK_Gy

K_Gz＝K_Gz0·δK_Gz

G_xy＝G_xy+δG_xy

G_yx＝G_yx+δG_yx

G_xz＝G_xz+δG_xz

G_zx＝G_zx+δG_zx

G_yz＝G_yz+δG_yz

G_zy＝G_zy+δG_zy

上述计算迭代进行，因此使用相同符号表示，

误差进行闭环修正后，各状态变量清零，

X(i)＝0，i＝1,2…23

每一个循环结束时，要对中间变量d▽_y、d▽_z进行处理：δA_yx＝-d▽_y/9.8

δA_zx＝-d▽_z/9.8

δG_xy＝δG_xy-δA_yx

δG_xz＝δG_xz-δA_zx

A_yx＝A_yx+δA_yx

A_zx＝A_zx+δA_zx

修正后，两状态清零，

d▽_y＝0；d▽_z＝0

其中，

A_yx、A_zx为惯导系统加速度计安装误差角；

B_x、B_y、B_z为惯导系统加速度计零偏值；

K_gx、K_gy、K_gz为惯导系统陀螺仪标度因数；

G_xy、G_yx、G_xz、G_zx、G_yz、G_zy为惯导系统陀螺仪安装误差角；

D_x、D_y、D_z为惯导系统陀螺仪零偏值，

在惯导系统导航计算时，利用上述参数标定值进行补偿，补偿公式如下：

${\tilde{f}}_{ibx}^b=K_{Ax}\·N_{Ax}-B_x$

${\tilde{f}}_{iby}^b=-A_{yx}\·(K_{Ax}\·N_{Ax})+K_{Ay}\·N_{Ay}-B_x$

${\tilde{f}}_{ibz}^b=-A_{zx}\·(K_{Ax}\·N_{Ax})-A_{zy}\·(K_{Ay}\·N_{Ay})+K_{Az}\·N_{Az}-B_z$

${\widetilde{\mathrm{\ω}}}_{ibx}^b=K_{gx}\·N_{Gx}-G_{xy}\·(K_{gy}\·N_{Gy})-G_{xz}\·(K_{gz}\·N_{Gz})-D_x$

${\widetilde{\mathrm{\ω}}}_{iby}^b=-G_{yx}\·(K_{gx}\·N_{Gx})+K_{gy}\·N_{Gy}-G_{yz}\·(K_{gz}\·N_{Gz})-D_y$

${\widetilde{\mathrm{\ω}}}_{ibz}^b=-G_{zx}\·(K_{gx}\·N_{Gx})-G_{zy}\·(K_{gy}\·N_{Gy})+K_{gz}\·N_{Gz}-D_z$

其中，

N_Ax、N_Ay、N_Az为一个导航周期内加速度计输出脉冲数；

N_Gx、N_Gy、N_Gz为一个导航周期内陀螺仪输出脉冲数；

为惯导系统标定补偿后加速度值；

为惯导系统标定补偿后角速度值。