[发明专利]一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法

权利要求书

1.一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，其特征在于：包括如下步骤，

第一步：将光纤捷联惯导系统通过工装安装在固定装置上，锁紧；连接光纤捷联惯导系统、电源和采集计算机之间的线缆，并检查是否正确；

第二步：将所述固定装置置于桌面或平整的地面上，手动调整固定装置，使光纤捷联惯导系统XYZ轴初始朝向分别对应东北天方向，上电预热使光纤捷联惯导系统达到热平衡状态；

第三步：向光纤捷联惯导系统装订标定现场的地理参数，包括初始的经度、纬度和高度，然后光纤捷联惯导系统进行1min的静态初始对准，以获取导航解算的初始姿态角；

第四步：手动翻转固定装置，完成12次依次翻转；翻转前后每个位置静止3-5min，并进行卡尔曼滤波修正，一共进行13次卡尔曼滤波修正；

第五步：最后一次卡尔曼滤波修正结束时得到的惯性器件误差参数估计值，作为标定结果，包括陀螺和加速度计零偏误差、标度因数误差和安装误差一共21项误差系数；

第六步：基于建立的惯性器件误差模型和标定得到21项误差系数，对光纤捷联惯导系统光纤陀螺和加速度计的输出进行误差补偿。

2.根据权利要求1所述一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，其特征在于：所述的固定装置为正六面体。

3.根据权利要求1所述一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，其特征在于：所述的12次依次翻转，具体如下：

所述转动角的单位为度。

4.根据权利要求3所述一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，其特征在于：所述的转动角允许存在±10°误差。

5.根据权利要求1所述一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，其特征在于：所述的卡尔曼率滤波修正包含以下几个步骤：

步骤1：建立惯性器件误差模型；

惯性器件误差模型包括光纤陀螺误差模型和加速度计误差模型，分别对应如下：

δωibb=δωibxbδωibybδωibzb=gBxgBygBz+gSFxgMAxygMAxzgMAyxgSFygMAyzgMAzxgMAzygSFzωibxbωibybωibzz]]>

δfibb=δfibxbδfibybδfibzb=aBxaByaBz+aSFx00aMAyxaSFy0aMAzxaMAzyaSFzfibxbfibybfibzb]]>

式中δω_ib^b为陀螺仪的误差输出矢量；δω_ibx^b、δω_iby^b、δω_ibz^b为由陀螺误差引起的误差角速度；ω_ibx^b、ω_iby^b、ω_ibz^b分别表示三轴陀螺测量值；gSF_x、gSF_y、gSF_z分别表示三轴陀螺仪标度因数误差；gMA_xy、gMA_xz、gMA_yx、gMA_yz、gMA_zx、gMA_zy分别表示各轴间陀螺仪安装误差角；gB_x、gB_y、gB_z分别表示三轴陀螺仪零偏误差；

式中为加速度计的误差输出矢量；δf_ibx^b、δf_iby^b、δf_ibz^b分别表示由加速度计误差引起的误差加速度；f_ibx^b、f_iby^b、f_ibz^b分别表示三轴加速度计测量值；aSF_x、aSF_y、aSF_z分别为三轴加速度计标度因数误差；aB_x、aB_y、aB_z分别为三轴加速度计零偏；aMA_yx、aMA_zx、aMA_zy分别表示加速度计各轴间安装误差角；

步骤2：建立卡尔曼滤波器模型；

选取地理坐标系东北天为导航坐标系，建立系统状态方程和量测方程分别如下：

X·=F(t)X(t)+W(t)]]>

Z(t)＝HX(t)+η(t)

式中表示系统状态的微分，F(t)表示状态矩阵、X(t)表示系统状态向量、W(t)表示系统噪声、Z(t)表示系统量测矢量、H表示观测矩阵、η(t)表示量测噪声矢量；

其中，系统状态向量X(t)＝[φ δV δP X_g X_a]^T，φ表示姿态角误差φ＝[φ_E φ_N φ_U],φ_E表示俯仰角误差、φ_N表示横滚角误差、φ_U表示航向角误差；δV表示速度误差δV＝[δV_E δV_N δV_U]，δV_E表示东向速度误差、δV_N北向速度误差、δV_U表示天向速度误差；δP表示位置误差δP＝[δL δλ δh]，δL表示纬度误差、δλ表示经度误差，δh表示高度误差；X_g表示陀螺标定参数误差、X_a表示加速度计标定参数误差；

X_g＝[gSF_x gMA_xy gMA_xz gMA_yx gSF_y gMA_yz gMA_zx gMA_zy gSF_z gB_x gB_y gB_z]

X_a＝[aSF_x aMA_yx aSF_y aMA_zx aMA_zy aSF_z aB_x aB_y aB_z]

gSF_x、gSF_y、gSF_z分别表示三轴陀螺仪标度因数误差；gMA_xy、gMA_xz、gMA_yx、gMA_yz、gMA_zx、gMA_zy分别表示各轴陀螺仪间安装误差角；gB_x、gB_y、gB_z分别表示三轴陀螺仪零偏误差；aSF_x、aSF_y、aSF_z分别为三轴加速度计标度因数误差；aB_x、aB_y、aB_z分别表示三轴加速度计零偏；aMA_yx、aMA_zx、aMA_zy分别表示各轴加速度计间安装误差角；

状态矩阵F(t)=F11F12F13-CbnF1403×9F21F22F2303×12CbnF2503×3F32F3303×1203×9021×30]]>

F11=0ωiesinL+VERE+htanL-(ωiecosL+VERE+h)-(ωiesinL+VERE+htanL)0-VNRN+hωiecosL+VERE+hVNRN+h0]]>

F12=0-1RN+h01RE+h001RE+htanL00]]>

F13=00VN(RN+h)20-ωiesinL-VE(RE+h)20ωiecosL+VERE+hsec2L-VE(RE+h)2tanL]]>

F14=ωibxbωibybωibzb000000100000ωibxbωibybωibzb000010000000ωibxbωibybωibzb001]]>

F21=0-fUfNfU0-fE-fEfE0]]>

F22=(VNtanL-VURE+h)(2ωiesinL+VEtanLRE+h)-(2ωiecosL+VERE+h)-2(ωiesinL+VEtanLRE+h)-VURN+h-VNRN+h2(ωiecosL+VERE+h)2VNRN+h0]]>

F23=0(2ωie(VUsinL+VNcosL)+VEVNsec2LRE+h)(VEVU-VEVNtanL(RE+h)2)0-(2VEωiecosL+VE2sec2LRE+h)(VNVU(RN+h)2+VE2tanL(RE+h)2)0-2VEωiesinL-(VN2(RN+h)2+VE2(RE+h)2)]]>

F25=fibxb000001000fibxbbibyb000010000fibxbfibybbibzb001]]>

F32=01(RN+h)0secL(RE+h)00001]]>

F33=00-VN(RN+h)2VEtanLsecL(RE+h)0-VEsecL(RE+h)2000]]>

式中ω_ie表示地球自转角速率；L表示系统所在位置的地理纬度；h表示系统所在位置的海拔高度；R_E表示当地子午面主曲率半径；R_N表示当地卯酉面主曲率半径；V_E、V_N和V_U分别表示系统东向、北向和天向速度；f_E、f_N和f_U分别表示导航坐标系下系统的比力信息；ω_ibx^b、ω_iby^b和ω_ibz^b分别表示三轴陀螺测量值；f_ibx^b、f_iby^b和f_ibz^b分别表示三轴加速度计测量值；

系统噪声W(t)＝[W_gx W_gy W_gz W_ax W_ay W_az 0_1×24]^T，W_gx、W_gx和W_gx分别表示三轴陀螺在导航坐标系下的零均值白噪声，W_ax、W_ay和W_az分别表示三轴加速度计在导航坐标系下的零均值白噪声；

系统量测矢量Z(t)＝[V(t)-V_obs P(t)-P_obs]^T，式中V(t)为系统输出的东北天速度信息，V_obs为速度观测信息，静止状态下为0，P(t)为系统输出的纬度、经度和高度信息，P_obs为位置观测信息，静止状态下P_obs为系统初始位置信息；

系统观测矩阵H＝[0_6×3 I_6×6 0_6×21]，式中I_6×6表示六阶单位阵；

步骤3：对系统状态方程进行离散化；

步骤4：进行卡尔曼滤波估计。

6.根据权利要求1所述一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，其特征在于：第六步所述的误差补偿方法如下：

wxwywz=(I+gSFxgMAxygMAxzgMAyxgSFygMAyzgMAzxgMAzygSFz)ωibxb-gBxωibyb-gByωibzb-gBz]]>

fxfyfz=(I+aSFx00aMAyxaSFy0aMAzxaMAzyaSFz)fibxb-aBxfibyb-aByfibzb-aBz]]>

式中gSF_x、gSF_y、gSF_z分别表示三轴陀螺仪标度因数标定结果；gMA_xy、gMA_xz、gMA_yx、gMA_yz、gMA_zx、gMA_zy分别表示各轴陀螺仪间安装误差角标定结果；gB_x、gB_y、gB_z分别表示三轴陀螺仪零偏误差标定结果；aSF_x、aSF_y、aSF_z分别为三轴加速度计标度因数误差标定结果；aB_x、aB_y、aB_z分别为三轴加速度计零偏结果；aMA_yx、aMA_zx、aMA_zy分别表示各轴加速度计间安装误差角标定结果；ω_ibx^b、ω_iby^b、ω_ibz^b分别表示系统三轴陀螺原测量值；f_ibx^b、f_iby^b、f_ibz^b分别表示系统三轴加速度计原测量值；I表示三阶单位阵；w_x、w_y、w_z分别表示系统三轴陀螺补偿后测量值；f_x、f_y、f_z分别表示系统三轴加速度计补偿后测量值。