CN103630146B - 一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法 - Google Patents

Abstract

本发明提供一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法，该方法结合了离散解析与系统级滤波两种标定方法的优点；首先根据激光陀螺IMU的角速度和加速度通道误差模型，采用四方位旋转速率离散解析方法，初步标定出角速度和加速度通道的全部24个标定系数；然后以标定系数的误差值为状态变量，采用带杆臂效应补偿的Kalman滤波方法估计出标定系数的误差值，并修正得到精确的标定系数。该方法消除了标定过程中减振装置变形、陀螺和加速度计漂移等随机误差而引起的误差，提高了标定系数的精度，减小了导航解算的误差。

Description

一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法

技术领域

本发明涉及一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法，可以应用于精确标定位置姿态测量系统(PositionandOrientationSystem,POS)和惯性导航系统(InertialNavigationSystem,INS)，以及INS/GPS(GlobalPositionSystem,全球定位系统)组合导航系统的标定系数，提高导航解算的精度。

背景技术

惯性测量单元(InertialMeasurementUnit,IMU)是POS、INS、INS/GPS等惯性测量系统的核心部件，惯性测量系统是以牛顿力学定律为基础，利用IMU测量载体的线运动和角运动参数，在给定初始条件下，由计算机推算得到载体的位置速度姿态参数。它是一种自主式的位置速度姿态测量单元，完全依靠机载设备自主地完成测量任务，和外界不发生任何光、电联系，具有隐蔽性好，工作不受气象条件限制的优点，因此在航空、航天和航海领域得到广泛的使用。IMU主要由陀螺仪和加速度计等惯性测量组件构成，陀螺仪的精度很大程度上决定了IMU的测量精度。由于激光陀螺具有高动态、低成本、高可靠、性能价格比高的特点，在军用、民用方面被广泛应用。但在实际应用中，激光陀螺的闭锁效应严重影响了其测量精度，通常采用抖动偏频的方法消除闭锁效应。由于存在机械抖动部件，利用激光陀螺构成的IMU中通常采用减振装置，将IMU内部的高频抖动和外界隔离。

IMU的标定系数是影响POS、INS、INS/GPS等导航系统精度的关键因素。但是在激光陀螺IMU中，由于减振装置采用橡胶材料，在IMU标定翻转的过程中减振装置会因重力和外部输入角速度、加速度等而引起变形，从而导致标定精度的下降。此外，标定过程中陀螺和加速度计的随机漂移等随机误差也同样会引入标定误差。

发明内容

本发明的技术解决问题是：提高带减振装置的激光陀螺IMU标定精度，提供一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法，该方法修正了因减振装置变形、陀螺和加速度计漂移等随机误差而引起的标定误差，提高了标定系数的精度，减小了导航解算的误差。

本发明的技术解决方案为：一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法，其特点在于包括下列步骤：

步骤(1)、根据激光陀螺及加速度计的特性，建立激光陀螺IMU角速度和加速度通道的确定性误差模型，模型参数包括：x、y、z三轴陀螺输入角速度ω_x、ω_y、ω_z，三轴输出角速度G_x、G_y、G_z，三轴陀螺零偏G_bx、G_by、G_bz，三轴陀螺标度因数S_x、S_y、S_z，三轴陀螺随机误差δG_x、δG_y、δG_z，角速度通道的六个安装误差系数E_xy、E_xz、E_yx、E_yz、E_zx、E_zy；x、y、z三轴加速度计的输入加速度a_x、a_y、a_z，三轴加速度计的输出加速度A_x、A_y、A_z，三轴加速度计零偏A_bx、A_by、A_bz，三轴加速度计标度因数K_x、K_y、K_z，三轴加速度计随机误差δA_x、δA_y、δA_z，和加速度通道的六个安装误差系数M_xy、M_xz、M_yx、M_yz、M_zx、M_zy。

步骤(2)、根据步骤(1)中建立的激光陀螺IMU角速度和加速度通道确定性误差模型，采用四方位旋转速率离散解析方法，初步标定出三轴陀螺零偏G_bx、G_by、G_bz，三轴陀螺标度因数S_x、S_y、S_z，角速度通道的六个安装误差系数E_xy、E_xz、E_yx、E_yz、E_zx、E_zy，三轴加速度计零偏A_bx、A_by、A_bz，三轴加速度计标度因数K_x、K_y、K_z，和加速度通道六个安装误差系数M_xy、M_xz、M_yx、M_yz、M_zx、M_zy共24个标定系数。

步骤(3)、建立激光陀螺IMU角速度和加速度通道的随机误差模型，模型参数包括：三轴陀螺随机误差δG_x、δG_y、δG_z，三轴陀螺随机游走ε_x、ε_y、ε_z、三轴陀螺随机游走驱动噪声三轴陀螺标度因数误差δS_x、δS_y、δS_z，角速度通道六个安装误差系数偏差δE_xy、δE_xz、δE_yx、δE_yz、δE_zx、δE_zy；三轴加速度计随机误差δA_x、δA_y、δA_z，三轴加速度计随机游走 三轴加速度计随机游走驱动噪声三轴加速度计标度因数误差δK_x、δK_y、δK_z，加速度通道安装误差系数偏差δM_xy、δM_xz、δM_yx、δM_yz、δM_zx、δM_zy。根据随机误差模型和惯性导航解算的基本原理，建立Kalman滤波的状态方程。

步骤(4)、选取东、北、天向的速度误差δV_E、δV_N、δV_U作为观测量，建立带杆臂效应补偿的Kalman滤波量测方程。

步骤(5)、用Kalman滤波估计步骤(3)中24个随机误差模型参数ε_x、ε_y、ε_z、δS_x、δS_y、δS_z、δE_xy、δE_xz、δE_yx、δE_yz、δE_zx、δE_zy、δK_x、δK_y、δK_z、δM_xy、δM_xz、δM_yx、δM_yz、δM_zx、δM_zy，并与步骤(2)中的24个标定系数对应相加进行修正。重复此步骤，直到所有的标定系数都收敛为止，收敛条件为连续十分钟内24个标定系数在修正前后相对变化小于0.05％。此时得到精确的标定系数，用于补偿激光陀螺IMU数据。

步骤(2)所述的四方位旋转速率离散解析方法中，四方位是将激光陀螺IMU分别调整到x、y、z、-x轴分别指天，其余两轴水平；当x、y、z轴分别指天时，转台的外框轴逆时针方向以5°/s—100°/s之间的任一恒定角速率匀速旋转两周；当-x指天时，转台的外框轴顺时针方向以5°/s—100°/s之间的任一恒定角速率匀速旋转两周。

步骤(3)所述的Kalman滤波状态方程中，状态变量共有30维，包括东、北、天向的速度误差δV_E、δV_N、δV_U，俯仰、横滚、航向角误差φ_E、φ_N、φ_U，以及三轴陀螺随机游走ε_x、ε_y、ε_z，陀螺标度因数误差δS_x、δS_y、δS_z，角速度通道六个安装误差系数偏差δE_xy、δE_xz、δE_yx、δE_yz、δE_zx、δE_zy，三轴加速度计随机游走三轴加速度计标度因数误差δK_x、δK_y、δK_z，加速度通道安装误差系数偏差δM_xy、δM_xz、δM_yx、δM_yz、δM_zx、δM_zy。

步骤(5)所述的带杆臂效应补偿的Kalman滤波量测方程中，量测量Z是经杆臂效应补偿后的激光陀螺IMU东、北、天向的速度误差δV_E、δV_N、δV_U。

$Z=\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ {\mathrm{\δV}}_N\\ {\mathrm{\δV}}_U\end{array}\right]=\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]-\left[\begin{array}{c}d{V}_{lE}^n\\ {\mathrm{dV}}_{lN}^n\\ {\mathrm{dV}}_{lU}^n\end{array}\right]=\left[\begin{array}{c}{V}_E-d{V}_{lE}^n\\ V_N-{\mathrm{dV}}_{lN}^n\\ V_U-{\mathrm{dV}}_{lU}^n\end{array}\right]$

其中，V_E、V_N、V_U分别是激光陀螺IMU东、北、天向的速度， 分别为杆臂效应在东、北、天向附加的速度误差。

本发明的原理是：四方位旋转速率离散解析方法是根据激光陀螺IMU的角速度和加速度通道确定性误差模型，建立包含24个标定系数的多元线性方程组，利用求解多元线性方程组解的原理计算激光陀螺IMU的所有24个标定系数；而Kalman滤波方法则是利用递推最小方差估计的原理，从量测信息中消除高斯白噪声，提取出被估计参数的信息，进而修正四方位旋转速率离散解析方法初步计算出的标定系数，最终提高标定系数精度。

本发明与现有技术相比的优点在于：本发明在离散解析标定方法的基础上，考虑了标定过程中的减振装置变形、陀螺和加速度计漂移等随机误差，并采用Kalman滤波方法估计并消除了这些随机误差，实现了带减振装置的激光陀螺IMU高精度标定，提高了标定系数的精度，减小了导航解算的误差。

附图说明

图1为本发明的原理框图。

图2为本发明的四方位旋转速率离散解析标定方案。

图3为本发明的步骤(5)的Kalman滤波及修正标定系数流程图。

图4为本发明所采用的Kalman滤波算法原理图。

具体实施方式

如图1所示，本发明的具体方法如下：

(1)根据激光陀螺及加速度计的特性，建立激光陀螺IMU角速度和加速度通道的误差模型。

角速度通道误差模型为：

$\begin{array}{c}{G}_x=G_{bx}+S_x{\mathrm{\ω}}_x+E_{xy}{\mathrm{\ω}}_y+E_{xz}{\mathrm{\ω}}_z+{\mathrm{\δG}}_x\\ G_y=G_{by}+E_{yx}{\mathrm{\ω}}_x+S_y{\mathrm{\ω}}_y+E_{yz}{\mathrm{\ω}}_z+{\mathrm{\δG}}_y\\ G_z=G_{bz}+E_{zx}{\mathrm{\ω}}_x+E_{zy}{\mathrm{\ω}}_y+S_z{\mathrm{\ω}}_z+{\mathrm{\δG}}_z\end{array}---\left(1\right)$

其中，ω_x、ω_y、ω_z为x、y、z三轴陀螺输入角速度，G_x、G_y、G_z为三轴输出角速度，G_bx、G_by、G_bz为三轴陀螺零偏，S_x、S_y、S_z为陀螺标度因数，δG_x、δG_y、δG_z为三轴陀螺随机误差，E_xy、E_xz、E_yx、E_yz、E_zx、E_zy为角速度通道的六个安装误差系数。

加速度通道误差模型为：

$\begin{array}{c}{A}_x=A_{bx}+K_x{a}_x+M_{xy}{a}_y+M_{xz}{a}_z+{\mathrm{\δA}}_x\\ A_y=A_{bx}+M_{yx}{a}_x+K_y{a}_y+M_{yz}{a}_z+{\mathrm{\δA}}_y\\ A_z=A_{bz}+M_{zx}{a}_x+M_{zy}{a}_y+K_z{a}_z+{\mathrm{\δA}}_z\end{array}---\left(2\right)$

其中，a_x、a_y、a_z为x、y、z三轴输入加速度，A_x、A_y、A_z为三轴输出加速度，A_bx、A_by、A_bz为三轴加速度计零偏，K_x、K_y、K_z为标度因数，δA_x、δA_y、δA_z为三轴加速度计随机误差，M_xy、M_xz、M_yx、M_yz、M_zx、M_zy为加速度通道的六个安装误差系数。

(2)根据步骤(1)中建立的激光陀螺IMU角速度和加速度通道误差模型，采用四方位旋转速率离散解析方法，初步标定出三轴陀螺零偏G_bx、G_by、G_bz，陀螺标度因数S_x、S_y、S_z，角速度通道的六个安装误差系数E_xy、E_xz、E_yx、E_yz、E_zx、E_zy，三支加速度计零偏A_bx、A_by、A_bz，三支加速度计标度因数K_x、K_y、K_z，和加速度通道六个安装误差系数M_xy、M_xz、M_yx、M_yz、M_zx、M_zy共24个标定系数。具体步骤如下：

(a)将激光陀螺IMU通过一个专用的过渡板，与三轴转台的内框轴固连。如图2中4个方位所示，将激光陀螺IMU分别调整到x、-x、y、z轴分别指天，与转台旋转轴z_p重合，其余两轴水平。当如方位1所示x轴指天、如方位3所示y轴指天以及如方位4所示z轴指天时，转台的外框轴逆时针方向以恒定角速度Ω(5°/s＜Ω＜100°/s，通常取10°/s)匀速旋转两周；当如图2中方位2所示-x指天时，转台的外框轴顺时针方向以恒定角速度Ω(5°/s＜Ω＜100°/s，通常取10°/s)匀速旋转两周。

(b)根据上述方案可建立如下方程：

角速度通道方程:

$\left[\begin{array}{ccc}{G}_{x1}& G_{y1}& G_{z1}\\ G_{x2}& G_{y2}& G_{z2}\\ G_{x3}& G_{y3}& G_{z3}\\ G_{x4}& G_{y4}& G_{z4}\end{array}\right]=\left[\begin{array}{cccc}1& \stackrel{\‾}{\mathrm{\ω}}& 0& 0\\ 1& -\stackrel{\‾}{\mathrm{\ω}}& 0& 0\\ 1& 0& \stackrel{\‾}{\mathrm{\ω}}& 0\\ 1& 0& 0& \stackrel{\‾}{\mathrm{\ω}}\end{array}\right]\left[\begin{array}{ccc}{G}_{bx}& G_{by}& G_{bz}\\ S_x& E_{xy}& E_{xz}\\ E_{yx}& S_y& E_{yz}\\ E_{zx}& E_{zy}& S_z\end{array}\right]---\left(3\right)$

其中,G_x1、G_y1、G_z1为第1个方位下惯性系统x、y、z三轴陀螺的输出，G_x2、G_y2、G_z2为第2个方位下三轴陀螺的输出，G_x3、G_y3、G_z3为第3个方位下三轴陀螺的输出，G_x4、G_y4、G_z4为第4个方位下三轴陀螺的输出，Ω为转台输入角速率，ω_ie为地球自转角速率(°/h)，φ为当地纬度。

加速度通道方程:

$\left[\begin{array}{ccc}{A}_{x1}& A_{y1}& A_{z1}\\ A_{x2}& A_{y2}& A_{z2}\\ A_{x3}& A_{y3}& A_{z3}\\ A_{x4}& A_{y4}& A_{z4}\end{array}\right]=\left[\begin{array}{cccc}1& g& 0& 0\\ 1& -g& 0& 0\\ 1& 0& g& 0\\ 1& 0& 0& g\end{array}\right]\left[\begin{array}{ccc}{A}_{bx}& A_{by}& A_{bz}\\ K_x& M_{yx}& M_{zx}\\ M_{xy}& K_y& M_{zy}\\ M_{xz}& M_{yz}& K_z\end{array}\right]---\left(4\right)$

其中，A_x1、A_y1、A_z1为第1个方位下惯性系统x、y、z三轴加速度计的输出，A_x2、A_y2、A_z2为第2个方位下三轴加速度计的输出，A_x3、A_y3、A_z3为第3个方位下三轴加速度计的输出，A_x4、A_y4、A_z4为第4个方位下三轴加速度计的输出，g为当地重力加速度。

(c)求解角速度和加速度通道方程，可得标定系数。

角速度通道标定系数：

$\left[\begin{array}{cc}{G}_{bx}& S_x\\ G_{by}& E_{yx}\\ G_{bz}& E_{zx}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cc}{G}_{x2}+G_{x1}& (G_{x1}-G_{x2})/\stackrel{\‾}{\mathrm{\ω}}\\ G_{y2}+G_{y1}& (G_{y1}-G_{y2})/\stackrel{\‾}{\mathrm{\ω}}\\ G_{z2}+G_{z1}& (G_{z1}-G_{z2})/\stackrel{\‾}{\mathrm{\ω}}\end{array}\right]---\left(5\right)$

$\left[\begin{array}{cc}{E}_{xy}& E_{xz}\\ S_y& E_{yz}\\ E_{zy}& S_z\end{array}\right]=\frac{1}{\stackrel{\‾}{\mathrm{\ω}}}\left[\begin{array}{cc}{G}_{x3}-G_{bx}& G_{x4}-G_{bx}\\ G_{y3}-G_{by}& G_{y4}-G_{by}\\ G_{z3}-G_{bz}& G_{z4}-G_{bz}\end{array}\right]---\left(6\right)$

加速度通道标定系数：

$\left[\begin{array}{ccc}{A}_{bx}& A_{by}& A_{bz}\\ K_x& M_{yx}& M_{zx}\\ M_{xy}& K_y& M_{zy}\\ M_{xz}& M_{yz}& K_z\end{array}\right]=\frac{1}{2g}\left[\begin{array}{ccc}g(A_{x1}+A_{x2})& g(A_{y1}+A_{y2})& g(A_{z1}+A_{z2})\\ A_{x1}-A_{x2}& A_{y1}-A_{y2}& A_{z1}-A_{z2}\\ 2(A_{x3}-A_{bx})& 2(A_{y3}-A_{by})& 2(A_{z3}-A_{bz})\\ 2(A_{x4}-A_{bx})& 2(A_{y4}-A_{by})& 2(A_{z4}-A_{bz})\end{array}\right]---\left(7\right)$

(3)建立激光陀螺IMU角速度和加速度通道的随机误差模型，并根据随机误差模型和惯性导航解算的基本原理，建立Kalman滤波的状态方程。

(a)激光陀螺IMU角速度和加速度通道的随机误差模型。

角速度通道的随机误差模型：

$\begin{array}{c}{\mathrm{\δG}}_x={\mathrm{\δS}}_x{\mathrm{\ω}}_x+{\mathrm{\δE}}_{xy}{\mathrm{\ω}}_y+{\mathrm{\δE}}_{xz}{\mathrm{\ω}}_z+{\mathrm{\ϵ}}_x+w_{{\mathrm{\ϵ}}_x}\\ {\mathrm{\δG}}_y={\mathrm{\δE}}_{yx}{\mathrm{\ω}}_x+{\mathrm{\δS}}_y{\mathrm{\ω}}_y+{\mathrm{\δE}}_{yz}{\mathrm{\ω}}_z+{\mathrm{\ϵ}}_y+w_{{\mathrm{\ϵ}}_y}\\ {\mathrm{\δG}}_z={\mathrm{\δE}}_{zx}{\mathrm{\ω}}_x+{\mathrm{\δE}}_{zy}{\mathrm{\ω}}_y+{\mathrm{\δS}}_z{\mathrm{\ω}}_z+{\mathrm{\ϵ}}_z+w_{{\mathrm{\ϵ}}_z}\end{array}---\left(8\right)$

其中，δG_x、δG_y、δG_z表示三轴陀螺随机误差，ε_x、ε_y、ε_z为三轴陀螺随机游走，w_εx、w_εy、w_εz为随机游走驱动噪声、δS_x、δS_y、δS_z为陀螺标度因数误差，δE_xy、δE_xz、δE_yx、δE_yz、δE_zx、δE_zy角速度通道六个安装误差系数偏差。

加速度通道的随机误差模型：

$\begin{array}{c}{\mathrm{\δA}}_x={\mathrm{\δK}}_x{a}_x+{\mathrm{\δM}}_{xy}{a}_y+{\mathrm{\δM}}_{xz}{a}_z+{\▿}_x+w_{{\▿}_x}\\ {\mathrm{\δA}}_y={\mathrm{\δM}}_{yx}{a}_x+{\mathrm{\δK}}_y{a}_y+{\mathrm{\δM}}_{yz}{a}_z+{\▿}_y+w_{{\▿}_y}\\ {\mathrm{\δA}}_z={\mathrm{\δM}}_{zx}{a}_x+{\mathrm{\δM}}_{zy}{a}_y+{\mathrm{\δK}}_z{a}_z+{\▿}_z+w_{{\▿}_z}\end{array}---\left(9\right)$

其中，δA_x、δA_y、δA_z表示三轴加速度随机误差，为加速度计随机游走，为随机游走驱动噪声，δK_x、δK_y、δK_z为加速度计标度因数误差，δM_xy、δM_xz、δM_yx、δM_yz、δM_zx、δM_zy为加速度通道安装误差系数偏差。

(b)根据激光陀螺随机误差模型以及惯性导航解算的基本原理，建立Kalman滤波的状态方程。

选取东、北、天向的速度误差δV_E、δV_N、δV_U，俯仰、横滚、航向角误差φ_E、φ_N、φ_U，以及步骤(3)中随机误差模型参数ε_x、ε_y、ε_z、δS_x、δS_y、δS_z、δE_xy、δE_xz、δE_yx、δE_yz、δE_zx、δE_zy、δK_x、δK_y、δK_z、δM_xy、δM_xz、δM_yx、δM_yz、δM_zx、δM_zy为状态变量X，X的维数为30维。

状态方程为：

$\stackrel{\·}{X}=F\·X+G\·w---\left(10\right)$

其中，X为系统状态变量：

$\begin{array}{c}X=\[\begin{array}{cccccccccccccccccc}{\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\δK}}_x& {\mathrm{\δK}}_y& {\mathrm{\δK}}_z& {\mathrm{\δM}}_{xy}& {\mathrm{\δM}}_{xz}& {\mathrm{\δM}}_{yx}& {\mathrm{\δM}}_{yz}& {\mathrm{\δM}}_{zx}& {\mathrm{\δM}}_{zy}\end{array}\\ \begin{array}{cccccccccccc}{\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\mathrm{\δS}}_x& {\mathrm{\δS}}_y& {\mathrm{\δS}}_z& {\mathrm{\δE}}_{xy}& {\mathrm{\δE}}_{xz}& {\mathrm{\δE}}_{yx}& {\mathrm{\δE}}_{yz}& {\mathrm{\δE}}_{zx}& {\mathrm{\δE}}_{zy}{\]}^T\end{array}\end{array}---\left(11\right)$

w为系统噪声矢量：

w＝[w_axw_ayw_azw_gxw_gyw_gz]^T(12)

G为噪声阵：

$G=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{24\×3}& 0_{24\×3}\end{array}\right]---\left(13\right)$

是从IMU载体系到导航系的姿态转移矩阵，分别用θ、φ、γ表示IMU的航向、俯仰、横滚角，则:

$C_b^n=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\γ}\cos \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\φ}\\ -\cos \mathrm{\γ}\sin \mathrm{\φ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}& -\sin \mathrm{\γ}\sin \mathrm{\φ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(14\right)$

T_ij(i,j＝1,2,3)表示姿态转移矩阵的9个元素。

F为系统转移矩阵：

$F=\left[\begin{array}{ccc}{A}_{3\×6}& C_{3\×12}& 0_{3\×12}\\ B_{3\×6}& 0_{3\×12}& D_{3\×12}\\ 0_{24\×6}& 0_{24\×12}& 0_{24\×12}\end{array}\right]---\left(15\right)$

$A_{3\×6}=\left[\begin{array}{cccccc}\frac{V_N}{R}\tan L& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R}& -2{\mathrm{\ω}}_{ie}\cos L-\frac{V_E}{R}& 0& g& 0\\ -2{\mathrm{\ω}}_{ie}\sin L+\frac{2V_E\tan L}{R}& -\frac{V_U}{R}& -\frac{V_N}{R}& -g& 0& 0\\ 2{\mathrm{\ω}}_{ie}\cos L+\frac{2V_E}{R}& \frac{2V_N}{R}& 0& 0& 0& 0\end{array}\right]---\left(16\right)$

其中，V_E、V_N、V_U分别是激光陀螺IMU东、北、天向的速度，L是当地纬度，R为地球半径，ω_ie为地球自转角速率，g为地球重力加速度。

$B_{3\×6}=\left[\begin{array}{cccccc}0& -\frac{1}{R}& 0& 0& {\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R}& -{\mathrm{\ω}}_{ie}\cos L-\frac{V_E}{R}\\ \frac{1}{R}& 0& 0& -{\mathrm{\ω}}_{ie}\sin L-\frac{V_E\tan L}{R}& 0& -\frac{V_N}{R}\\ \frac{1}{R}\tan L& 0& 0& {\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0\end{array}\right]---\left(17\right)$

$C_{3\×12}=\left[\begin{array}{cccccccccccc}{T}_{11}& T_{12}& T_{13}& T_{11}{a}_x& T_{12}{a}_y& T_{13}{a}_z& T_{11}{a}_y& T_{11}{a}_z& T_{12}{a}_x& T_{12}{a}_z& T_{13}{a}_x& T_{13}{a}_y\\ T_{21}& T_{22}& T_{23}& T_{21}{a}_x& T_{22}{a}_y& T_{23}{a}_z& T_{21}{a}_y& T_{21}{a}_z& T_{22}{a}_x& T_{22}{a}_z& T_{23}{a}_x& T_{23}{a}_y\\ T_{31}& T_{32}& T_{33}& T_{31}{a}_x& T_{32}{a}_y& T_{33}{a}_z& T_{31}{a}_y& T_{31}{a}_z& T_{32}{a}_x& T_{32}{a}_z& T_{33}{a}_x& T_{33}{a}_y\end{array}\right]---\left(18\right)$

$D_{3\×12}=\left[\begin{array}{cccccccccccc}{T}_{11}& T_{12}& T_{13}& T_{11}{\mathrm{\ω}}_x& T_{12}{\mathrm{\ω}}_y& T_{13}{\mathrm{\ω}}_z& T_{11}{\mathrm{\ω}}_y& T_{11}{\mathrm{\ω}}_z& T_{12}{\mathrm{\ω}}_x& T_{12}{\mathrm{\ω}}_z& T_{13}{\mathrm{\ω}}_x& T_{13}{\mathrm{\ω}}_y\\ T_{21}& T_{22}& T_{23}& T_{21}{\mathrm{\ω}}_x& T_{22}{\mathrm{\ω}}_y& T_{23}{\mathrm{\ω}}_z& T_{21}{\mathrm{\ω}}_y& T_{21}{\mathrm{\ω}}_z& T_{22}{\mathrm{\ω}}_x& T_{22}{\mathrm{\ω}}_z& T_{23}{\mathrm{\ω}}_x& T_{23}{\mathrm{\ω}}_y\\ T_{31}& T_{32}& T_{33}& T_{31}{\mathrm{\ω}}_x& T_{32}{\mathrm{\ω}}_y& T_{33}{\mathrm{\ω}}_z& T_{31}{\mathrm{\ω}}_y& T_{31}{\mathrm{\ω}}_z& T_{32}{\mathrm{\ω}}_x& T_{32}{\mathrm{\ω}}_z& T_{33}{\mathrm{\ω}}_x& T_{33}{\mathrm{\ω}}_y\end{array}\right]---\left(19\right)$

(4)选取δV_E、δV_N、δV_U作为观测量Z，建立带杆臂效应补偿的Kalman滤波量测方程。

在实际的标定过程中，由于激光陀螺IMU与转台的中心不完全重合，即存在杆臂。在转台旋转的时候，激光陀螺IMU会产生一个附加的线速度。因此，需要将激光陀螺IMU在导航坐标系下因杆臂效应附加的线速度进行补偿。ω_b为载体坐标系下的旋转角速度，r_b是IMU敏感中心到转台旋转中心的位置矢量，是从载体坐标系到导航坐标系的转移矩阵。

r_b＝[r_xr_yr_z]^Tω_b＝[ω_xω_yω_z]^T(20)

由杆臂效应引起的速度误差在导航坐标系下可表示为：

${\mathrm{dV}}_l^n=\left[\begin{array}{c}d{V}_{lE}^n\\ {\mathrm{dV}}_{lN}^n\\ {\mathrm{dV}}_{lU}^n\end{array}\right]=C_b^n({\mathrm{\ω}}_b\×r_b)---\left(21\right)$

系统的量测方程:

Z＝HX+η(22)

其中，Z为观测矢量，H为观测矩阵，η为量测噪声。

$Z=\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ {\mathrm{\δV}}_N\\ {\mathrm{\δV}}_U\end{array}\right]=\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]-\left[\begin{array}{c}d{V}_{lE}^n\\ {\mathrm{dV}}_{lN}^n\\ {\mathrm{dV}}_{lU}^n\end{array}\right]=\left[\begin{array}{c}{V}_E-d{V}_{lE}^n\\ V_N-{\mathrm{dV}}_{lN}^n\\ V_U-{\mathrm{dV}}_{lU}^n\end{array}\right]---\left(23\right)$

H＝[I_3×30_3×27](24)

η＝[η_Eη_Nη_U]^T(25)

(5)用Kalman滤波估计随机误差模型参数，并进行修正标定系数。

如图3所示，首先用当前24个标定系数补偿IMU数据，并带入捷联惯导解算求解出IMU在东、北、天向的速度；其次利用步骤(4)中的杆臂效应补偿方法补偿IMU东、北、天向的速度；最后采用Kalman滤波来估计步骤(3)中的24个随机误差模型参数，并与步骤(2)中的24个标定系数对应相加进行修正。

Kalman滤波基本算法编排如图4所示:

状态一步预测方程:

${\stackrel{\mathrm{\Λ}}{X}}_{k/k-1}={\mathrm{\φ}}_{k,k-1}{\stackrel{\mathrm{\Λ}}{X}}_{k-1}---\left(26\right)$

状态估值计算方程:

${\stackrel{\mathrm{\Λ}}{X}}_k={\stackrel{\mathrm{\Λ}}{X}}_{k/k-1}+K_k(Z_k-H_k{\stackrel{\mathrm{\Λ}}{X}}_{k/k-1})---\left(27\right)$

滤波增量方程:

${\stackrel{\mathrm{\Λ}}{X}}_k={\stackrel{\mathrm{\Λ}}{P}}_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(28\right)$

一步预测均方误差方程:

${\stackrel{\mathrm{\Λ}}{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---\left(29\right)$

估计均方误差方程:

${\stackrel{\mathrm{\Λ}}{P}}_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(30\right)$

Kalman滤波估计出的24个随机误差模型参数按以下方法修正步骤(2)中的24个标定系数：

角速度通道标定系数修正:

$\left[\begin{array}{ccc}{\stackrel{\‾}{G}}_{bx}& {\stackrel{\‾}{G}}_{by}& {\stackrel{\‾}{G}}_{bz}\\ {\stackrel{\‾}{S}}_x& {\stackrel{\‾}{E}}_{yx}& {\stackrel{\‾}{E}}_{zx}\\ {\stackrel{\‾}{E}}_{xy}& {\stackrel{\‾}{S}}_y& {\stackrel{\‾}{E}}_{zy}\\ {\stackrel{\‾}{E}}_{xz}& {\stackrel{\‾}{E}}_{yz}& {\stackrel{\‾}{S}}_z\end{array}\right]=\left[\begin{array}{ccc}{G}_{bx}& G_{by}& G_{bz}\\ S_x& E_{yx}& E_{zx}\\ E_{xy}& S_y& E_{zy}\\ E_{xz}& E_{yz}& S_z\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{\ϵ}}_{gx}& {\mathrm{\ϵ}}_{gy}& {\mathrm{\ϵ}}_{gz}\\ {\mathrm{\δS}}_x& {\mathrm{\δE}}_{yx}& {\mathrm{\δE}}_{zx}\\ {\mathrm{\δE}}_{xy}& {\mathrm{\δS}}_y& {\mathrm{\δE}}_{zy}\\ {\mathrm{\δE}}_{xz}& {\mathrm{\δE}}_{yz}& {\mathrm{\δS}}_z\end{array}\right]---\left(31\right)$

其中，为修正后得到的三轴陀螺零偏，为修正后的陀螺标度因数，为修正后的六个角速度通道安装误差系数。

加速度通道标定系数修正:

$\left[\begin{array}{ccc}{\stackrel{\‾}{A}}_{bx}& {\stackrel{\‾}{A}}_{by}& {\stackrel{\‾}{A}}_{bz}\\ {\stackrel{\‾}{K}}_x& {\stackrel{\‾}{M}}_{yx}& {\stackrel{\‾}{M}}_{zx}\\ {\stackrel{\‾}{M}}_{xy}& {\stackrel{\‾}{K}}_y& {\stackrel{\‾}{M}}_{zy}\\ {\stackrel{\‾}{M}}_{xz}& {\stackrel{\‾}{M}}_{yz}& {\stackrel{\‾}{K}}_z\end{array}\right]=\left[\begin{array}{ccc}{A}_{bx}& A_{by}& A_{bz}\\ K_x& M_{yx}& M_{zx}\\ M_{xy}& K_y& M_{zy}\\ M_{xz}& M_{yz}& K_z\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{\ϵ}}_{ax}& {\mathrm{\ϵ}}_{ay}& {\mathrm{\ϵ}}_{az}\\ {\mathrm{\δK}}_x& {\mathrm{\δM}}_{yx}& {\mathrm{\δM}}_{zx}\\ {\mathrm{\δM}}_{xy}& {\mathrm{\δK}}_y& {\mathrm{\δM}}_{zy}\\ {\mathrm{\δM}}_{xz}& {\mathrm{\δM}}_{yz}& {\mathrm{\δK}}_z\end{array}\right]---\left(32\right)$

其中，为修正后得到的三轴加速度计零偏，为修正后的加速度计标度因数，为修正后的六个加速度通道安装误差系数。

重复步骤(5)，直到所有24个标定系数都收敛为止，收敛条件为连续十分钟内24个标定系数在修正前后相对变化小于0.05％。此时得到精确的标定系数，用于补偿激光陀螺IMU数据。

Claims (3)

1.一种离散解析与Kalman滤波结合的激光陀螺IMU标定方法，其特征在于包括下列步骤：

步骤(1)、根据激光陀螺及加速度计的特性，建立激光陀螺IMU角速度和加速度通道的确定性误差模型，模型参数包括：x、y、z三轴陀螺输入角速度ω_x、ω_y、ω_z，三轴陀螺输出角速度G_x、G_y、G_z，三轴陀螺零偏G_bx、G_by、G_bz，三轴陀螺标度因数S_x、S_y、S_z，三轴陀螺随机误差δG_x、δG_y、δG_z，角速度通道的六个安装误差系数E_xy、E_xz、E_yx、E_yz、E_zx、E_zy；x、y、z三轴加速度计的输入加速度a_x、a_y、a_z，三轴加速度计输出加速度A_x、A_y、A_z，三轴加速度计零偏A_bx、A_by、A_bz，三轴加速度计标度因数K_x、K_y、K_z，三轴加速度计随机误差δA_x、δA_y、δA_z和加速度通道的六个安装误差系数M_xy、M_xz、M_yx、M_yz、M_zx、M_zy；

步骤(2)、根据步骤(1)中建立的激光陀螺IMU角速度和加速度通道确定性误差模型，采用四方位旋转速率离散解析方法，初步标定出三轴陀螺零偏G_bx、G_by、G_bz，三轴陀螺标度因数S_x、S_y、S_z，角速度通道的六个安装误差系数E_xy、E_xz、E_yx、E_yz、E_zx、E_zy，三轴加速度计零偏A_bx、A_by、A_bz，三轴加速度计标度因数K_x、K_y、K_z和加速度通道六个安装误差系数M_xy、M_xz、M_yx、M_yz、M_zx、M_zy共24个标定系数；

步骤(3)、建立激光陀螺IMU角速度和加速度通道的随机误差模型，模型参数包括：三轴陀螺随机误差δG_x、δG_y、δG_z，三轴陀螺随机游走ε_x、ε_y、ε_z、三轴陀螺随机游走驱动噪声三轴陀螺标度因数误差δS_x、δS_y、δS_z，角速度通道六个安装误差系数偏差δE_xy、δE_xz、δE_yx、δE_yz、δE_zx、δE_zy；三轴加速度计随机误差δA_x、δA_y、δA_z，三轴加速度计随机游走 三轴加速度计随机游走驱动噪声三轴加速度计标度因数误差δK_x、δK_y、δK_z，加速度通道安装误差系数偏差δM_xy、δM_xz、δM_yx、δM_yz、δM_zx、δM_zy，根据随机误差模型和惯性导航解算的基本原理，建立Kalman滤波的状态方程；

步骤(4)、选取东、北、天向的速度误差δV_E、δV_N、δV_U作为观测量，建立带杆臂效应补偿的Kalman滤波量测方程；

步骤(5)、用Kalman滤波估计步骤(3)中24个随机误差模型参数ε_x、ε_y、ε_z、δS_x、δS_y、δS_z、δE_xy、δE_xz、δE_yx、δE_yz、δE_zx、δE_zy、δK_x、δK_y、δK_z、δM_xy、δM_xz、δM_yx、δM_yz、δM_zx、δM_zy，并与步骤(2)中的24个标定系数对应相加进行修正，重复此步骤，直到所有的标定系数都收敛为止，收敛条件为连续十分钟内24个标定系数在修正前后相对变化小于0.05％；所述的带杆臂效应补偿的Kalman滤波量测方程中，量测量Z是经杆臂效应补偿后的激光陀螺IMU东、北、天向的速度误差δV_E、δV_N、δV_U；

$Z=\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ {\mathrm{\δV}}_N\\ {\mathrm{\δV}}_U\end{array}\right]=\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]-\left[\begin{array}{c}d{V}_{lE}^n\\ {\mathrm{dV}}_{lN}^n\\ {\mathrm{dV}}_{lU}^n\end{array}\right]=\left[\begin{array}{c}{V}_E-d{V}_{lE}^n\\ V_N-{\mathrm{dV}}_{lN}^n\\ V_U-{\mathrm{dV}}_{lU}^n\end{array}\right]$

其中，V_E、V_N、V_U分别是激光陀螺IMU东、北、天向的速度，分别为杆臂效应在东、北、天向附加的速度误差。

2.根据权利要求1所述的离散解析与Kalman滤波结合的激光陀螺IMU标定方法，其特征在于：步骤(2)所述的四方位旋转速率离散解析方法中，四方位是将激光陀螺IMU分别调整到x、y、z、-x轴分别指天，其余两轴水平；当x、y、z轴分别指天时，转台的外框轴逆时针方向以5°/s—100°/s之间的任一恒定角速率匀速旋转两周；当-x指天时，转台的外框轴顺时针方向以5°/s—100°/s之间的任一恒定角速率匀速旋转两周。

3.根据权利要求1所述的离散解析与Kalman滤波结合的激光陀螺IMU标定方法，其特征在于：步骤(3)所述的Kalman滤波状态方程中，状态变量共有30维，包括东、北、天向的速度误差δV_E、δV_N、δV_U，俯仰、横滚、航向角误差φ_E、φ_N、φ_U，以及三轴陀螺随机游走ε_x、ε_y、ε_z，三轴陀螺标度因数误差δS_x、δS_y、δS_z，角速度通道六个安装误差系数偏差δE_xy、δE_xz、δE_yx、δE_yz、δE_zx、δE_zy，三轴加速度计随机游走三轴加速度计标度因数误差δK_x、δK_y、δK_z，加速度通道安装误差系数偏差δM_xy、δM_xz、δM_yx、δM_yz、δM_zx、δM_zy。