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

摘要

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

主权项

一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法，其特征在于：包括如下步骤，第一步：将光纤捷联惯导系统通过工装安装在固定装置上，锁紧；连接光纤捷联惯导系统、电源和采集计算机之间的线缆，并检查是否正确；第二步：将所述固定装置置于桌面或平整的地面上，手动调整固定装置，使光纤捷联惯导系统XYZ轴初始朝向分别对应东北天方向，上电预热使光纤捷联惯导系统达到热平衡状态；第三步：向光纤捷联惯导系统装订标定现场的地理参数，包括初始的经度、纬度和高度，然后光纤捷联惯导系统进行1min的静态初始对准，以获取导航解算的初始姿态角；第四步：手动翻转固定装置，完成12次依次翻转；翻转前后每个位置静止3‑5min，并进行卡尔曼滤波修正，一共进行13次卡尔曼滤波修正；所述的12次依次翻转，具体如下：所述转动角的单位为度；第五步：最后一次卡尔曼滤波修正结束时得到的惯性器件误差参数估计值，作为标定结果，包括陀螺和加速度计零偏误差、标度因数误差和安装误差一共21项误差系数；第六步：基于建立的惯性器件误差模型和标定得到21项误差系数，对光纤捷联惯导系统光纤陀螺和加速度计的输出进行误差补偿；所述的卡尔曼率滤波修正包含以下几个步骤：步骤1：建立惯性器件误差模型；惯性器件误差模型包括光纤陀螺误差模型和加速度计误差模型，分别对应如下：${{\mathrm{\δ\ω}}_{ib}}^b=\left[\begin{array}{c}{{\mathrm{\δ\ω}}_{ibx}}^b\\ {{\mathrm{\δ\ω}}_{iby}}^b\\ {{\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}}_{xy}& {\mathrm{gMA}}_{xz}\\ {\mathrm{gMA}}_{yx}& {\mathrm{gSF}}_y& {\mathrm{gMA}}_{yz}\\ {\mathrm{gMA}}_{zx}& {\mathrm{gMA}}_{zy}& {\mathrm{gSF}}_z\end{array}\right]\left[\begin{array}{c}{{\mathrm{\ω}}_{ibx}}^b\\ {{\mathrm{\ω}}_{iby}}^b\\ {{\mathrm{\ω}}_{ibz}}^b\end{array}\right]$${\mathrm{\δf}}_{ib}^b=\left[\begin{array}{c}{\mathrm{\δf}}_{ibx}^b\\ {\mathrm{\δf}}_{iby}^b\\ {\mathrm{\δf}}_{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}}_{yx}& {\mathrm{aSF}}_y& 0\\ {\mathrm{aMA}}_{zx}& {\mathrm{aMA}}_{zy}& {\mathrm{aSF}}_z\end{array}\right]\left[\begin{array}{c}{f}_{ibx}^b\\ f_{iby}^b\\ f_{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_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_E+h}\tan L& -\left({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h}\right)\\ -\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_E+h}\tan L\right)& 0& -\frac{V_N}{R_N+h}\\ {\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{\ω}}_{ie}\sin L& -\frac{V_E}{{(R_E+h)}^2}\\ 0& {\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{\ω}}_{ibx}^b& {\mathrm{\ω}}_{iby}^b& {\mathrm{\ω}}_{ibz}^b& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& {\mathrm{\ω}}_{ibx}^b& {\mathrm{\ω}}_{iby}^b& {\mathrm{\ω}}_{ibz}^b& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& {\mathrm{\ω}}_{ibx}^b& {\mathrm{\ω}}_{iby}^b& {\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_N& f_E& 0\end{array}\right]$$F_{22}=\left[\begin{array}{ccc}\left(\frac{V_N\tan L-V_U}{R_E+h}\right)& \left(2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_E+h}\right)& -\left(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h}\right)\\ -2\left({\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_E+h}\right)& -\frac{V_U}{R_N+h}& -\frac{V_N}{R_N+h}\\ 2\left({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h}\right)& \frac{2V_N}{R_N+h}& 0\end{array}\right]$$F_{23}=\left[\begin{array}{ccc}0& \left(2{\mathrm{\ω}}_{ie}\left(V_U\sin L+V_N\cos L\right)+\frac{V_E{V}_N{\sec }^{2}L}{R_E+h}\right)& \left(\frac{V_E{V}_U-V_E{V}_N\tan L}{{\left(R_E+h\right)}^2}\right)\\ 0& -\left(2V_E{\mathrm{\ω}}_{ie}\cos L+\frac{{V_E}^2{\sec }^{2}L}{R_E+h}\right)& \left(\frac{V_N{V}_U}{{\left(R_N+h\right)}^2}+\frac{{V_E}^2\tan L}{{\left(R_E+h\right)}^2}\right)\\ 0& -2V_E{\mathrm{\ω}}_{ie}\sin L& -\left(\frac{{V_N}^2}{{\left(R_N+h\right)}^2}+\frac{{V_E}^2}{{\left(R_E+h\right)}^2}\right)\end{array}\right]$$F_{25}=\left[\begin{array}{ccccccccc}{f}_{ibx}^b& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& f_{ibx}^b& f_{iby}^b& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& f_{ibx}^b& f_{iby}^b& f_{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_gy和W_gz分别表示三轴陀螺在导航坐标系下的零均值白噪声，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：进行卡尔曼滤波估计。