[发明专利]一种激光陀螺惯导系统的系统级温度误差补偿方法

权利要求书

1.一种激光陀螺惯导系统的系统级温度误差补偿方法，采用的测试装置包括：温度变化试验箱、惯性元件数据采集系统、导航解算计算机和三轴转台，其特征在于，该方法步骤如下：

S1：将激光陀螺惯导系统安装在带温度变化试验箱的三轴转台上，将惯导系统上电，设定温度变化试验箱初始温度为0℃；

S2：待惯导系统中激光陀螺和加速度计的温度监测值波动范围小于0.1℃后，控制三轴转台寻零后静置1200s，进行惯导系统初始对准：包括解析法粗对准和卡尔曼滤波精对准，其中解析法粗对准用时120s，卡尔曼滤波精对准用时1080s；

S3：设置温度变化试验箱按+10℃/小时的温度变化速率从0℃升温至50℃；在升温过程中控制三轴转台，使惯导系统按照下列19位置标定路径循环运动两次：1.绕中框旋转-90°，转动结束后置停180s；2.绕中框旋转-180°，转动结束后置停180s；3.绕中框旋转-180°，转动结束后置停180s；4.绕内框旋转-90°，转动结束后置停180s；5.绕内框旋转-180°，转动结束后置停180s；6.绕内框旋转-180°，转动结束后置停180s；7.绕中框旋转-90°，转动结束后置停180s；8.绕中框旋转-180°，转动结束后置停180s；9.绕中框旋转-180°，转动结束后置停180s；10.绕中框旋转-90°，转动结束后置停180s；11.绕中框旋转-90°，转动结束后置停180s；12.绕中框旋转-90°，转动结束后置停180s；13.绕内框旋转-90°，转动结束后置停180s；14.绕内框旋转-90°，转动结束后置停180s；15.绕内框旋转-90°，转动结束后置停180s；16.绕中框旋转-90°，转动结束后置停180s；17.绕中框旋转-90°，转动结束后置停180s；18.绕中框旋转-90°，转动结束后置停180s；选取加速度计表头作为温度采样点，利用惯性元件数据采集系统采集加速度计的温度值，采集激光陀螺角增量和加速度计比力增量信息，将激光陀螺角增量和加速度计比力增量信息输入导航解算计算机进行导航解算，输出速度误差和位置误差；

S4：构建激光陀螺和加速度计误差模型，构建一个42维卡尔曼滤波器，并将S3输出的速度误差和位置误差作为观测量输入卡尔曼滤波器，滤波估计激光陀螺和加速度计的常值零偏、标度因数误差、安装误差，以及加速度计的误差温度系数；

具体步骤如下：

S4.1建立激光陀螺和加速度计的误差模型：

定义i系为地心惯性系，b系为载体系，其坐标原点位于载体的质心处，坐标轴构成右手直角坐标系，n系为导航系，x轴、y轴、z轴分别指向北、东、地方向；

激光陀螺的误差模型为：

加速度计的误差模型为：

在式(1)和式(2)中，下标x、y、z分别表示沿x轴、y轴、z轴方向的分量，为简便起见，后面在定义时，带下标x、y、z的参数都没有分开定义，f^b分别为激光陀螺角增量和加速度计比力增量的理论值在b系下的分量，δf^b分别为激光陀螺角增量和加速度计比力增量的误差量在b系下的分量，B_g为激光陀螺常值零偏，B_a为加速度计常值零偏，δK_g为激光陀螺标度因数误差，δK_a为加速度计标度因数误差，δM_gyx为y轴激光陀螺与载体系xy平面的安装误差角，δM_gzx为z轴激光陀螺与载体系xz平面的安装误差角，δM_gzy为z轴激光陀螺与载体系yz平面的安装误差角，δM_ayx为y轴加速度计与载体系xy平面的安装误差角，δM_azx为z轴加速度计与载体系xz平面的安装误差角，δM_axy为x轴加速度计与载体系xy平面的安装误差角，δM_azy为z轴加速度计与载体系yz平面的安装误差角，δM_axz为x轴加速度计与载体系xz平面的安装误差角，δM_ayz为y轴加速度计与载体系yz平面的安装误差角，TB_a为加速度计零偏一阶温度系数，TK_a为加速度计标度因数误差一阶温度系数，TM_ayx为y轴加速度计与载体系xy平面的安装误差角的一阶温度系数，TM_azx为z轴加速度计与载体系xz平面的安装误差角的一阶温度系数，TM_axy为x轴加速度计与载体系xy平面的安装误差角的一阶温度系数，TM_azy为z轴加速度计与载体系yz平面的安装误差角的一阶温度系数，TM_axz为x轴加速度计与载体系xz平面的安装误差角的一阶温度系数，TM_ayz为y轴加速度计与载体系yz平面的安装误差角的一阶温度系数，T_a为加速度计的温度值；

S4.2建立卡尔曼滤波器的系统状态方程和观测方程：

S4.2.1包含北、东、地三个方向的姿态误差速度误差δV_N,δV_E,δV_D，纬度、经度、高度误差δL,δλ,δh和激光陀螺、加速度计误差模型中33个误差参数的42维卡尔曼滤波器状态方程为：

其中，42维状态向量X为：

为42维状态向量X的微分；

F为状态转移矩阵，其表达式如下：

其中各分块矩阵的表达式为：

式(6)至式(24)中，ω_ie为地球自转角速率，V_N,V_E,V_D分别为北向、天向和垂向速度，L为纬度，h为高度，R_N,R_E分别为地球子午圈、卯酉圈曲率半径，f_N,f_E,f_D分别为加速度计输出比力在导航系北向、东向和垂向的投影，为b系至n系的姿态变换矩阵，表示姿态变换矩阵的第i行第j列元素；

式(3)中，G为系统噪声驱动矩阵，其表达式为：

W(t)为系统噪声矩阵，其表达式为：

式(26)中，W_g(t)为激光陀螺输出白噪声，W_a(t)为加速度计输出白噪声，设Q_g为激光陀螺输出白噪声方差，Q_a为加速度计输出白噪声方差，满足如下条件：

S4.2.2以速度误差和位置误差作为观测量构建卡尔曼滤波器的观测方程：

Z＝HX+v(t) (29)

Z为包含速度误差和位置误差的观测量，其表达式为：

Z＝[δV_N δV_E δV_D δL δλ δh]^T (30)

H为观测矩阵，其表达式为：

v(t)为观测噪声矩阵，其表达式为：

ν(t)＝[ν_VN(t) ν_VE(t) ν_VD(t) ν_L(t) ν_λ(t) ν_h(t)]^T (32)

式(32)中，ν_VN(t),ν_VE(t),ν_VD(t)分别为北向、东向、垂向速度观测噪声，ν_L(t),ν_λ(t),ν_h(t)分别为纬度L、经度λ和高度h位置观测噪声，设R_V为速度观测白噪声方差，R_p为位置观测白噪声方差，满足如下条件：

S4.3利用卡尔曼滤波算法估计状态参量：

S4.2构建了连续条件下的卡尔曼滤波器的系统状态方程和观测方程，为了能够使用计算机编程实现，需要将卡尔曼滤波器离散化；状态转移矩阵F的离散化过程为：

式中，F_k/k-1表示离散化的从k-1时刻至k时刻的状态转移矩阵，为描述简便，此处进行如下定义：带下标k-1、k的参数分别表示该参数在k-1时刻和k时刻的值，带下标k/k-1的参数表示该参数从k-1时刻至k时刻的一步预测；I为单位矩阵，T为滤波周期；

根据离散卡尔曼滤波器的基本原理，在已知k-1时刻的状态量估计值的基础上，可以估计出k时刻的状态量，由此可得，在给定初始时刻的状态量后，可以递推估计出任意时刻的状态量，状态量的估计过程表示为：

在式(36)中，表示状态量的估计值，K为滤波增益；滤波增益K的计算方式为：

在式(37)中，P为滤波估计的误差方差矩阵，P的更新过程满足递推算法，在给定滤波估计的误差方差矩阵P的初始值P₀后，可以递推估计出任意时刻的P；Q₀为激光陀螺和加速度计输出白噪声的均方误差矩阵的初始值，R₀为观测噪声方差阵的初始值；

综上，在给定系统状态量初始值X₀、滤波估计的误差方差矩阵初始值P₀、激光陀螺和加速度计输出白噪声的方差矩阵初始值Q₀和观测噪声方差阵初始值R₀，并且确定状态转移矩阵F、观测量Z和观测矩阵H具体形式的条件下，由式(35)至(37)就可以递推估计出任意时刻的状态量，即可以得到激光陀螺和加速度计的常值零偏、标度因数误差、安装误差，以及加速度计的误差温度系数在任意时刻的滤波估计值；

S5：建立激光陀螺和加速度计的标定参数误差反馈模型；将S4卡尔曼滤波器估计得到的激光陀螺和加速度计的常值零偏、标度因数误差、安装误差，以及加速度计的误差温度系数代入标定参数误差反馈模型，完成误差修正；

具体步骤如下：

S5.1建立激光陀螺和加速度计的标定参数误差反馈模型：

激光陀螺的标度因数误差反馈模型的表达式为：

为激光陀螺的误差修正前的标度因数，K_g为误差修正后的标度因数；

激光陀螺的安装关系矩阵误差反馈模型的表达式为：

为激光陀螺的误差修正前的安装关系矩阵，M_g为误差修正后的安装关系矩阵；

加速度计的标度因数误差反馈模型的表达式为：

为加速度计的误差修正前的标度因数，K_a为误差修正后的标度因数，ΔT_a为加速度计的温度变化率值；

加速度计的安装关系矩阵误差反馈模型的表达式为：

为加速度计的误差修正前的安装关系矩阵，M_a为修正后的安装关系矩阵；

加速度计常值零偏误差反馈模型的表达式为：

为误差修正前的加速度计常值零偏，B_a为经过误差修正后的加速度计常值零偏；

S5.2将S4卡尔曼滤波器估计得到的激光陀螺标度因数误差δK_g，激光陀螺安装误差角δM_g，加速度计标度因数误差δK_a，加速度计安装误差角δM_a，以及加速度计标度因数误差一阶温度系数TK_a，加速度计安装误差一阶温度系数TM_a和加速度计零偏一阶温度系数TB_a代入S5.1所建立的标定参数误差反馈模型，完成误差修正。

2.一种根据权利要求1所述激光陀螺惯导系统的系统级温度误差补偿方法，其特征在于：S4中，系统状态量初始值设置为X₀＝[0_1×42]^T；

滤波估计的误差方差矩阵P的初始值P₀设置为：

激光陀螺和加速度计输出白噪声的均方误差矩阵Q的初始值Q₀设置为：

Q₀＝diag{(0.0005°/h)²,(0.0005°/h)²,(0.0005°/h)²,(1ug)²,(1ug)²,(1ug)²}

观测噪声方差阵R的初始值R₀设置为：

R₀＝diag{(0.01m/s)²,(0.01m/s)²,(0.01m/s)²,(0.001°)²,(0.001°)²,(0.001°)²}。