[发明专利]一种双轴光纤惯导系统快速自标定自对准方法

权利要求书

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次，

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

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

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

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

状态方程：

观测方程：Z＝HX+V

H＝[I_5×5 O_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为纬度、外部给定，

C11～C33是矩阵中的元素，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阶单位矩阵；

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

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

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

与互为转置，

用下述公式计算速度

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

计算：

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

利用最后得到的计算

步骤4、精对准导航滤波

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

姿态误差的修正

Tn是采样间隔，φ_n、φ_u、φ_e为惯导北、天、东失准角，的意义统一表示如下，以为例，其表示的是e坐标系相对于n坐标系的角速度的分量表示，

速度误差的修正

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

V_i＝V_i-δV_i，i＝n、u、e，

位置误差的修正

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

L＝L-δL

λ＝λ-δλ

误差参数的修正

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

ε_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_Gy·δK_gy

K_Gz＝K_Gz·δ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(j)＝0，j＝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为一个导航周期内陀螺仪输出脉冲数；

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

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