CN103852086A - 一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法 - Google Patents

Abstract

本发明公开了一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，属于惯性技术领域。所述方法将光纤捷联惯导系统通过工装安装在固定装置上，上电预热并且静态初始对准后，手动翻转固定装置，完成12次依次翻转。翻转前后每个位置静止3-5min，并进行卡尔曼滤波修正，根据最后一次卡尔曼滤波修正结束时得到的惯性器件误差参数估计值，对光纤捷联惯导系统光纤陀螺和加速度计的输出进行误差补偿。所述固定装置优选为正六面体。本发明所提出的方法可以在现场完成光纤捷联惯导系统21项误差参数的标定，克服了传统实验室标定的不足，提高了系统实际使用精度。

Description

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

技术领域

本发明属于惯性技术领域，涉及一种光纤捷联惯导系统的现场标定方法，可在现场没有转台和地理基准的情况下标定出光纤陀螺和加速度计各项误差参数。

背景技术

光纤陀螺具有全固态结构、启动速度快、动态范围宽、抗振动冲击及成本低等优点，已成为惯性器件重要的发展方向。近年来，光纤陀螺技术的迅猛发展推动了光纤捷联惯导系统在航空航天、航海和军事领域的应用。光纤捷联惯导系统的误差源主要来自于惯性器件，其中大部分误差可以通过标定技术进行补偿。目前在工程上应用最为广泛的是基于转台的实验室标定方法，该方法对转台设备的依赖性较高，一般只能在实验室进行，但是能标定出系统较为全面的误差参数，是光纤捷联惯导系统投入使用的重要前提。

然而，在实际应用过程中，光纤捷联惯导系统的各项误差参数并不是固定不变的，主要包括陀螺和加速度计的常值偏置、标度因数和安装误差等。由于系统的使用、器件老化、长时间贮存以及应用环境的变化等原因，这些参数会发生变化或存在残差，尤其是陀螺漂移和加速度计零偏，每次启动都不相同，时间间隔越长变化越大，通常光纤捷联惯导系统需要周期性地进行校标和测试，一般三个月或者半年一次。采用传统的方法需要将系统频繁地返回实验室标定，不但会耗费人力和成本，而且影响系统的实际使用。因此，采用现场标定技术，可以有效克服这些不足，在光纤捷联惯导系统使用现场，完成对惯性器件各项误差参数残差的辨识，提高惯导系统实际使用精度。

参考文献[1]（公开号为CN101021546A，名称为：光纤陀螺IMU的六位置旋转现场标定新方法）中，采用光纤陀螺惯性测量单元（IMU）在六个位置上进行12次旋转，然后根据光纤陀螺IMU的误差模型建立42个非线性输入输出方程，通过旋转积分和对称位置误差相消，消除方程中的非线性项，最终求出陀螺标度因数、陀螺常值漂移、陀螺安装误差和加速度计常值偏置等15个误差系数。但是该方法不能够标定出加速度计通道的标度因数和安装误差。

参考文献[2](测控技术，2011年第30卷第5期,颜开思,李岁劳,龚柏春,贾继超.基于平台和六面体的惯导系统现场标定技术,[J],106-109)中，通过翻转六面体使对称位置误差相消，并且在对准中获取姿态信息，同时精确标定出陀螺漂移和加速度计零偏。最后对理论分析结果进行了仿真验证，仿真结果表明该方案可以实现外场条件下的陀螺漂移和加速度计零偏的精确标定。但是该方法不能够标定出陀螺、加速度的标度因数和安装误差。

参考文献[3](吴赛成,秦石乔,王省书,胡春生,激光陀螺惯性测量单元系统级标定方法[J].中国惯性技术学报,2011,19(2):185-189),该文献建立了附加约束条件的陀螺和加速度计安装坐标系数学模型，根据陀螺和加速度计的输出误差方程，以速度解算误差为观测量，从惯性导航基本误差方程出发推导了惯性测量单元的系统级误差参数标定Kalman滤波模型，该模型包含了陀螺和加速度计零偏、比例因子、安装误差在内共21维标定误差状态变量。但是该方法标定步骤较多，标定时间过长，缺少实例验证。

参考文献[4]（公开号CN102607594A，捷联惯导光纤陀螺系统误差参数现场标定方法），通过姿态测量仪器给出载体姿态角，选取姿态作为观测量，标定出光纤捷联惯导系统光纤陀螺各项误差系数。但是该方法需要现场提供高精度姿态测量辅助器件，实时精确测量载体姿态角，并且要与光纤陀螺输出值保持同步，实际应用的难度很大。

发明内容

本发明的目的在于提供一种光纤捷联惯导系统现场标定的方法，减少甚至取消光纤捷联惯导系统周期性地返厂或返实验室校标，延长光纤捷联惯导系统的免标定周期并提高其实际使用精度。

本发明提供的方法具体包括如下步骤：

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

所述固定装置优选为正六面体。

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

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

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

所述翻转过程中转动角允许存在±10°误差。

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

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

本发明的有益效果：

本发明所提出的方法可以在现场完成光纤捷联惯导系统21项误差参数的标定，克服了传统实验室标定的不足，提高了系统实际使用精度。

附图说明

图1为转动过程21项标定参数卡尔曼滤波估计误差的方差收敛曲线；

图2为转动过程21项标定参数卡尔曼滤波估计值收敛曲线；

图3A和图3B分别为静态水平和摇摆情况下的现场标定补偿前后20min导航定位误差对比曲线；

图4为本发明提供的基于卡尔曼滤波的光纤捷联惯导系统现场标定方法流程图。

具体实施方式

下面结合附图和实施例对本发明进行详细说明。

本发明提供一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，如图4所示流程，具体包括如下步骤：

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

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

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

第四步：按照表1转动路径，进行手动翻转正六面体装置，完成12次依次翻转，转动角允许存在±10°误差。转动前后每个位置静止3-5min，并进行卡尔曼滤波修正，一共进行13次卡尔曼滤波修正。

表1正六面体翻转次序

所述的卡尔曼率滤波修正包含以下几个步骤：

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

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

${{\mathrm{\δ\ω}}_{\mathrm{ib}}}^b=\left[\begin{array}{c}{{\mathrm{\δ\ω}}_{\mathrm{ibx}}}^b\\ {{\mathrm{\δ\ω}}_{\mathrm{iby}}}^b\\ {{\mathrm{\δ\ω}}_{\mathrm{ibz}}}^b\end{array}\right]=\left[\begin{array}{c}{\mathrm{gB}}_x\\ {\mathrm{gB}}_y\\ {\mathrm{gB}}_z\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{gSF}}_x& {\mathrm{gMA}}_{\mathrm{xy}}& {\mathrm{gMA}}_{\mathrm{xz}}\\ {\mathrm{gMA}}_{\mathrm{yx}}& {\mathrm{gSF}}_y& {\mathrm{gMA}}_{\mathrm{yz}}\\ {\mathrm{gMA}}_{\mathrm{zx}}& {\mathrm{gMA}}_{\mathrm{zy}}& {\mathrm{gSF}}_z\end{array}\right]\left[\begin{array}{c}{{\mathrm{\ω}}_{\mathrm{ibx}}}^b\\ {{\mathrm{\ω}}_{\mathrm{iby}}}^b\\ {{\mathrm{\ω}}_{\mathrm{ibz}}}^z\end{array}\right]$

$\mathrm{\δ}{f}_{\mathrm{ib}}^b=\left[\begin{array}{c}\mathrm{\δ}{f}_{\mathrm{ibx}}^b\\ \mathrm{\δ}{f}_{\mathrm{iby}}^b\\ \mathrm{\δ}{f}_{\mathrm{ibz}}^b\end{array}\right]=\left[\begin{array}{c}{\mathrm{aB}}_x\\ {\mathrm{aB}}_y\\ {\mathrm{aB}}_z\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{aSF}}_x& 0& 0\\ {\mathrm{aMA}}_{\mathrm{yx}}& {\mathrm{aSF}}_y& 0\\ {\mathrm{aMA}}_{\mathrm{zx}}& {\mathrm{aMA}}_{\mathrm{zy}}& {\mathrm{aSF}}_z\end{array}\right]\left[\begin{array}{c}{f}_{\mathrm{ibx}}^b\\ f_{\mathrm{iby}}^b\\ f_{\mathrm{ibz}}^b\end{array}\right]$

式中δω_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：建立卡尔曼滤波器模型。

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

$\stackrel{\·}{X}=F\left(t\right)X\left(t\right)+W\left(t\right)$

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\left(t\right)=\left[\begin{array}{ccccc}{F}_{11}& F_{12}& F_{13}& -C_{\mathrm{bn}}{F}_{14}& 0_{3\×9}\\ F_{21}& F_{22}& F_{23}& 0_{3\×12}& C_{\mathrm{bn}}{F}_{25}\\ 0_{3\×3}& F_{32}& F_{33}& 0_{3\×12}& 0_{3\×9}\\ & & 0_{21\×30}& & \end{array}\right]$

$F_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\tan L& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\tan L)& 0& -\frac{V_N}{R_N+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}& \frac{V_N}{R_N+h}& 0\end{array}\right]$

$F_{12}=\left[\begin{array}{ccc}0& -\frac{1}{R_N+h}& 0\\ \frac{1}{R_E+h}& 0& 0\\ \frac{1}{R_E+h}\tan L& 0& 0\end{array}\right]$

$F_{13}=\left[\begin{array}{ccc}0& 0& \frac{V_N}{{(R_N+h)}^2}\\ 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& -\frac{V_E}{{(R_E+h)}^2}\\ 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}{\sec }^{2}L& -\frac{V_E}{{(R_E+h)}^2}\tan L\end{array}\right]$

$F_{14}=\left[\begin{array}{cccccccccccc}{\mathrm{\ω}}_{\mathrm{ibx}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{ibz}}^b& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& {\mathrm{\ω}}_{\mathrm{ibx}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{ibz}}^b& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& {\mathrm{\ω}}_{\mathrm{ibx}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{ibz}}^b& 0& 0& 1\end{array}\right]$

$F_{21}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_E& f_E& 0\end{array}\right]$

$F_{22}=\left[\begin{array}{ccc}\left(\frac{V_N\tan L-V_U}{R_E+h}\right)& (2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_E+h})& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_E+h})& -\frac{V_U}{R_N+h}& -\frac{V_N}{R_N+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})& \frac{2V_N}{R_N+h}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}0& (2{\mathrm{\ω}}_{\mathrm{ie}}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N{\sec }^{2}L}{R_E+h})& \left(\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_E+h)}^2}\right)\\ 0& -(2V_E{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{{V_E}^2{\sec }^{2}L}{R_E+h})& (\frac{V_N{V}_U}{{(R_N+h)}^2}+\frac{{V_E}^2\tan L}{{(R_E+h)}^2})\\ 0& -2V_E{\mathrm{\ω}}_{\mathrm{ie}}\text{sinL}& -(\frac{{V_N}^2}{{(R_N+h)}^2}+\frac{{V_E}^2}{{(R_E+h)}^2})\end{array}\right]$

$F_{25}=\left[\begin{array}{ccccccccc}{f}_{\mathrm{ibx}}^b& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& f_{\mathrm{ibx}}^b& b_{\mathrm{iby}}^b& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& f_{\mathrm{ibx}}^b& f_{\mathrm{iby}}^b& b_{\mathrm{ibz}}^b& 0& 0& 1\end{array}\right]$

$F_{32}=\left[\begin{array}{ccc}0& \frac{1}{(R_N+h)}& 0\\ \frac{\sec L}{(R_E+h)}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{33}=\left[\begin{array}{ccc}0& 0& -\frac{V_N}{{(R_N+h)}^2}\\ \frac{V_E\tan L\sec L}{(R_E+h)}& 0& -\frac{V_E\sec L}{{(R_E+h)}^2}\\ 0& 0& 0\end{array}\right]$

式中ω_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：对系统状态方程进行离散化。

采用泰勒级数展开：

$\mathrm{\Φ}(k+1,k)I+\mathrm{TF}+\frac{T^2}{2!}{F}^2+\frac{T^3}{3!}{F}^3+...$

其中Φ(k+1,k)为状态一步转移矩阵，I为30阶单位阵，F为状态转移矩阵，T为滤波周期。

离散系统噪声方差为：

$Q\left(k\right)=\mathrm{QT}+[\mathrm{FQ}+{\left(\mathrm{FQ}\right)}^T]\frac{T^2}{2!}\left\{F\right[\mathrm{FQ}+{\left(\mathrm{FQ}\right)}^T]+F{\left[(\mathrm{FQ}+{\mathrm{QF}}^T)\right]}^T\}\frac{T^3}{3!}+...$

其中Q(k)为离散系统噪声方差强度阵，Q为连续系统噪声方差阵，F为状态转移矩阵。

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

第k+1时刻的量测值为Z_k，则X_k的卡尔曼滤波估计值按下述方程求解：

获取系统状态向量的一步预测：

获取预测误差的方差阵： $P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

获取卡尔曼滤波增益： $K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$

获取系统状态卡尔曼滤波估计值： ${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

获取系统的状态估计误差方差： $P_k=P_{k/k-1}-P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}{H}_k{P}_{k/k-1}$

卡尔曼滤波采用闭环校正，估计结果有姿态角误差φ、速度误差δV、位置误差δP、陀螺标定参数误差X_g和加速度计标定参数误差X_a，利用φ、δV、δP对导航解算姿态、速度和位置进行校正，利用X_g、X_a对原惯性器件测量值进行校正。

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

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

误差补偿方法如下：

$\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=(I+\left[\begin{array}{ccc}{\mathrm{gSF}}_x& {\mathrm{gMA}}_{\mathrm{xy}}& {\mathrm{gMA}}_{\mathrm{xz}}\\ {\mathrm{gMA}}_{\mathrm{yx}}& {\mathrm{gSF}}_y& {\mathrm{gMA}}_{\mathrm{yz}}\\ {\mathrm{gMA}}_{\mathrm{zx}}& {\mathrm{gMA}}_{\mathrm{zy}}& {\mathrm{gSF}}_z\end{array}\right])\left[\begin{array}{c}{{\mathrm{\ω}}_{\mathrm{ibx}}}^b-{\mathrm{gB}}_x\\ {{\mathrm{\ω}}_{\mathrm{iby}}}^b-{\mathrm{gB}}_y\\ {{\mathrm{\ω}}_{\mathrm{ibz}}}^b-{\mathrm{gB}}_z\end{array}\right]$

$\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=(I+\left[\begin{array}{ccc}{\mathrm{aSF}}_x& 0& 0\\ {\mathrm{aMA}}_{\mathrm{yx}}& {\mathrm{aSF}}_y& 0\\ {\mathrm{aMA}}_{\mathrm{zx}}& {\mathrm{aMA}}_{\mathrm{zy}}& {\mathrm{aSF}}_z\end{array}\right])\left[\begin{array}{c}{f}_{\mathrm{ibx}}^b-a{B}_x\\ f_{\mathrm{iby}}^b-a{B}_y\\ f_{\mathrm{ibz}}^b-a{B}_z\end{array}\right]$

式中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分别表示系统三轴加速度计补偿后测量值。

实施例

第一步：选取某型光纤捷联惯导系统，该系统三个月前在实验室精密双轴转台上通过位置实验和速率实验已初步标定完毕。

第二步：将该光纤捷联惯导系统通过工装安装在六面体装置上，锁紧。连接光纤捷联惯导系统、电源、采集计算机之间的线缆，并检查正确。

第三步：将六面体装置置于平稳桌面上，上电预热使光纤捷联惯导系统达到热平衡状态，并装订光纤捷联惯导系统的初始位置参数，包括初始的经度、纬度和高度。

第四步：调整六面体装置，使光纤捷联惯导系统XYZ轴初始朝向对应东北天，采用静态解析式粗对准1-3min，获取光纤捷联惯导系统的初始姿态角。

第五步：按照表1转动路径，进行手动翻转六面体装置，完成12次连续转动。转动前后每个位置静止3-5min，并进行卡尔曼滤波修正，一共进行13次。

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

第七步：将光纤捷联惯导系统断电，1天后重新启动光纤捷联惯导系统，首先静态采集23min惯性器件数据，然后再将光纤捷联惯导系统断电3-5h，将光纤捷联惯导系统安装于双轴转台上，先静态3min再摇摆20min，一共采集23min惯性器件数据。

第八步：离线处理两组惯性器件数据（分别为静态惯性器件数据和摇摆状态下惯性器件数据），将第五步得到的现场标定结果对两组惯性器件数据分别进行补偿，采用解析式粗对准3min和纯惯性导航，对比补偿前后两组惯性器件数据的纯惯性导航结果。

结果及分析：

（1）卡尔曼滤波修正过程中，21个标定参数估计误差的方差收敛曲线如图1所示，21个标定参数估计值收敛曲线如图2所示。从图1和图2可以看出，随着光纤捷联惯导系统的连续转动，所有参数估计误差的方差值逐渐收敛接近零，参数估计值渐近收敛接近至真值。卡尔曼滤波修正最终参数估计值，即现场标定结果，具体值如表2所示。

表2现场标定结果

（2）对比现场标定补偿前后的数据导航结果如图3A、图3B所示。图3A是20min静态水平定位误差对比曲线，图3B是摇摆情况下水平定位误差对比曲线。从图3中可以看出，不管是静态还是动态情况下，光纤捷联惯导系统的导航定位误差减小了1倍以上，因此采用本发明提供的现场标定方法补偿后的数据精度更高。

可得到如下分析结论：在仅采用六面体装置的环境下，本发明设计的连续转动路径和卡尔曼滤波器，能有效标定出光纤捷联惯导系统21项误差参数，提高了系统实际使用精度，20min导航定位精度提高了1倍以上。

Claims (6)

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

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

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

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

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

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

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

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

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

所述转动角的单位为度。

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

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

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

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

${{\mathrm{\δ\ω}}_{\mathrm{ib}}}^b=\left[\begin{array}{c}{{\mathrm{\δ\ω}}_{\mathrm{ibx}}}^b\\ {{\mathrm{\δ\ω}}_{\mathrm{iby}}}^b\\ {{\mathrm{\δ\ω}}_{\mathrm{ibz}}}^b\end{array}\right]=\left[\begin{array}{c}{\mathrm{gB}}_x\\ {\mathrm{gB}}_y\\ {\mathrm{gB}}_z\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{gSF}}_x& {\mathrm{gMA}}_{\mathrm{xy}}& {\mathrm{gMA}}_{\mathrm{xz}}\\ {\mathrm{gMA}}_{\mathrm{yx}}& {\mathrm{gSF}}_y& {\mathrm{gMA}}_{\mathrm{yz}}\\ {\mathrm{gMA}}_{\mathrm{zx}}& {\mathrm{gMA}}_{\mathrm{zy}}& {\mathrm{gSF}}_z\end{array}\right]\left[\begin{array}{c}{{\mathrm{\ω}}_{\mathrm{ibx}}}^b\\ {{\mathrm{\ω}}_{\mathrm{iby}}}^b\\ {{\mathrm{\ω}}_{\mathrm{ibz}}}^z\end{array}\right]$

$\mathrm{\δ}{f}_{\mathrm{ib}}^b=\left[\begin{array}{c}\mathrm{\δ}{f}_{\mathrm{ibx}}^b\\ \mathrm{\δ}{f}_{\mathrm{iby}}^b\\ \mathrm{\δ}{f}_{\mathrm{ibz}}^b\end{array}\right]=\left[\begin{array}{c}{\mathrm{aB}}_x\\ {\mathrm{aB}}_y\\ {\mathrm{aB}}_z\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{aSF}}_x& 0& 0\\ {\mathrm{aMA}}_{\mathrm{yx}}& {\mathrm{aSF}}_y& 0\\ {\mathrm{aMA}}_{\mathrm{zx}}& {\mathrm{aMA}}_{\mathrm{zy}}& {\mathrm{aSF}}_z\end{array}\right]\left[\begin{array}{c}{f}_{\mathrm{ibx}}^b\\ f_{\mathrm{iby}}^b\\ f_{\mathrm{ibz}}^b\end{array}\right]$

式中δω_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：建立卡尔曼滤波器模型；

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

$\stackrel{\·}{X}=F\left(t\right)X\left(t\right)+W\left(t\right)$

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\left(t\right)=\left[\begin{array}{ccccc}{F}_{11}& F_{12}& F_{13}& -C_{\mathrm{bn}}{F}_{14}& 0_{3\×9}\\ F_{21}& F_{22}& F_{23}& 0_{3\×12}& C_{\mathrm{bn}}{F}_{25}\\ 0_{3\×3}& F_{32}& F_{33}& 0_{3\×12}& 0_{3\×9}\\ & & 0_{21\×30}& & \end{array}\right]$

$F_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\tan L& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\tan L)& 0& -\frac{V_N}{R_N+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}& \frac{V_N}{R_N+h}& 0\end{array}\right]$

$F_{12}=\left[\begin{array}{ccc}0& -\frac{1}{R_N+h}& 0\\ \frac{1}{R_E+h}& 0& 0\\ \frac{1}{R_E+h}\tan L& 0& 0\end{array}\right]$

$F_{13}=\left[\begin{array}{ccc}0& 0& \frac{V_N}{{(R_N+h)}^2}\\ 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& -\frac{V_E}{{(R_E+h)}^2}\\ 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}{\sec }^{2}L& -\frac{V_E}{{(R_E+h)}^2}\tan L\end{array}\right]$

$F_{14}=\left[\begin{array}{cccccccccccc}{\mathrm{\ω}}_{\mathrm{ibx}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{ibz}}^b& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& {\mathrm{\ω}}_{\mathrm{ibx}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{ibz}}^b& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& {\mathrm{\ω}}_{\mathrm{ibx}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{ibz}}^b& 0& 0& 1\end{array}\right]$

$F_{21}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_E& f_E& 0\end{array}\right]$

$F_{22}=\left[\begin{array}{ccc}\left(\frac{V_N\tan L-V_U}{R_E+h}\right)& (2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_E+h})& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_E+h})& -\frac{V_U}{R_N+h}& -\frac{V_N}{R_N+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})& \frac{2V_N}{R_N+h}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}0& (2{\mathrm{\ω}}_{\mathrm{ie}}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N{\sec }^{2}L}{R_E+h})& \left(\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_E+h)}^2}\right)\\ 0& -(2V_E{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{{V_E}^2{\sec }^{2}L}{R_E+h})& (\frac{V_N{V}_U}{{(R_N+h)}^2}+\frac{{V_E}^2\tan L}{{(R_E+h)}^2})\\ 0& -2V_E{\mathrm{\ω}}_{\mathrm{ie}}\text{sinL}& -(\frac{{V_N}^2}{{(R_N+h)}^2}+\frac{{V_E}^2}{{(R_E+h)}^2})\end{array}\right]$

$F_{25}=\left[\begin{array}{ccccccccc}{f}_{\mathrm{ibx}}^b& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& f_{\mathrm{ibx}}^b& b_{\mathrm{iby}}^b& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& f_{\mathrm{ibx}}^b& f_{\mathrm{iby}}^b& b_{\mathrm{ibz}}^b& 0& 0& 1\end{array}\right]$

$F_{32}=\left[\begin{array}{ccc}0& \frac{1}{(R_N+h)}& 0\\ \frac{\sec L}{(R_E+h)}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{33}=\left[\begin{array}{ccc}0& 0& -\frac{V_N}{{(R_N+h)}^2}\\ \frac{V_E\tan L\sec L}{(R_E+h)}& 0& -\frac{V_E\sec L}{{(R_E+h)}^2}\\ 0& 0& 0\end{array}\right]$

式中ω_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所述一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，其特征在于：第六步所述的误差补偿方法如下：

$\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=(I+\left[\begin{array}{ccc}{\mathrm{gSF}}_x& {\mathrm{gMA}}_{\mathrm{xy}}& {\mathrm{gMA}}_{\mathrm{xz}}\\ {\mathrm{gMA}}_{\mathrm{yx}}& {\mathrm{gSF}}_y& {\mathrm{gMA}}_{\mathrm{yz}}\\ {\mathrm{gMA}}_{\mathrm{zx}}& {\mathrm{gMA}}_{\mathrm{zy}}& {\mathrm{gSF}}_z\end{array}\right])\left[\begin{array}{c}{{\mathrm{\ω}}_{\mathrm{ibx}}}^b-{\mathrm{gB}}_x\\ {{\mathrm{\ω}}_{\mathrm{iby}}}^b-{\mathrm{gB}}_y\\ {{\mathrm{\ω}}_{\mathrm{ibz}}}^b-{\mathrm{gB}}_z\end{array}\right]$

$\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=(I+\left[\begin{array}{ccc}{\mathrm{aSF}}_x& 0& 0\\ {\mathrm{aMA}}_{\mathrm{yx}}& {\mathrm{aSF}}_y& 0\\ {\mathrm{aMA}}_{\mathrm{zx}}& {\mathrm{aMA}}_{\mathrm{zy}}& {\mathrm{aSF}}_z\end{array}\right])\left[\begin{array}{c}{f}_{\mathrm{ibx}}^b-a{B}_x\\ f_{\mathrm{iby}}^b-a{B}_y\\ f_{\mathrm{ibz}}^b-a{B}_z\end{array}\right]$

式中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分别表示系统三轴加速度计补偿后测量值。

Images