CN103245359A

CN103245359A - Method for calibrating fixed errors of inertial sensor in inertial navigation system in real time

Info

Publication number: CN103245359A
Application number: CN2013101427014A
Authority: CN
Inventors: 彭惠; 王东升; 张承; 许建新; 王融; 熊智; 刘建业; 赵慧; 柏青青; 王洁
Original assignee: Nanjing University of Aeronautics and Astronautics
Current assignee: Nanjing University of Aeronautics and Astronautics
Priority date: 2013-04-23
Filing date: 2013-04-23
Publication date: 2013-08-14
Anticipated expiration: 2033-04-23
Also published as: CN103245359B

Abstract

The invention discloses a real-time calibration method for the fixed error of an inertial sensor in an inertial navigation system. The method includes the following steps: firstly, a fixed error model of the inertial sensor in the dynamic flight process of an aircraft is established, and the fixed error includes an installation error and a scale factor error ; On the basis of the traditional IMU random error model and the established fixed error model, the filter state equation including the inertial sensor fixed error and the linear measurement equation of position, velocity and attitude are established; finally, in the dynamic flight process of the aircraft In the process, real-time dynamic calibration and correction are performed on the fixed error of the inertial sensor, and the navigation result of the inertial navigation system after compensation and correction of the fixed error of the inertial sensor is obtained. The method can realize the calibration and correction of the inertial sensor installation error and the scale factor error in the inertial navigation system during the dynamic flight process of the aircraft, effectively improves the performance of the inertial navigation system, and is suitable for engineering applications.

Description

A real-time calibration method for inertial sensor fixed error in inertial navigation system

技术领域technical field

本发明公开了一种惯性导航系统中惯性传感器固定误差实时标定方法，属于惯性导航惯性传感器误差标定技术领域。The invention discloses a real-time calibration method for fixed errors of inertial sensors in an inertial navigation system, belonging to the technical field of calibration of inertial sensor errors for inertial navigation.

背景技术Background technique

近年来，随着新型飞行器的研制，对导航系统性能的要求日益提高。惯性导航系统具有短时精度高、输出连续以及完全自主等突出优点必是未来新型飞行器的核心导航信息单元。In recent years, with the development of new aircraft, the requirements for the performance of the navigation system are increasing. The inertial navigation system has the outstanding advantages of high short-term accuracy, continuous output and complete autonomy, and it must be the core navigation information unit of future new aircraft.

惯性传感器（IMU-陀螺仪和加速度计）的测量误差是影响惯性导航系统精度的主要因素。传统IMU误差模型中，往往认为其固定性误差—安装误差和标度因数误差在静态标定后完全补偿，IMU误差中仅包含随机漂移误差。而在飞行器动态飞行过程中，由振动冲击、气流扰动等影响导致的机体变形将引起IMU的轴线不能与机体轴线完全重合，进而导致安装误差及标度因数误差显著增大。这些误差如果在飞行器动态飞行过程中不能进行标定补偿将在很大程度上影响导航精度。The measurement error of the inertial sensors (IMU - gyroscope and accelerometer) is the main factor affecting the accuracy of the inertial navigation system. In the traditional IMU error model, it is often considered that its fixed errors—installation errors and scale factor errors are fully compensated after static calibration, and the IMU errors only include random drift errors. During the dynamic flight of the aircraft, the deformation of the airframe caused by vibration shock, airflow disturbance, etc. will cause the axis of the IMU to not completely coincide with the axis of the airframe, which will lead to a significant increase in installation errors and scale factor errors. If these errors cannot be calibrated and compensated during the dynamic flight of the aircraft, it will greatly affect the navigation accuracy.

传统的IMU误差标定方法多利用转台在室内进行位置试验及速率试验实现对安装误差和标度因数误差的标定。但在飞行器动态飞行过程中，由于环境影响，传统利用转台的标定方法将不再适用。飞行器动态飞行过程中，传统对惯性传感器的误差修正仅针对其随机性误差，不考虑其安装误差和标度因数误差等固定性误差的影响。而由于飞行振动等影响引起的安装误差和标度因数误差将会很大程度上影响惯性导航系统精度，必须对其进行标定和补偿修正。因此，研究一种在飞行器动态飞行过程中对惯性传感器固定性误差的标定方法，能够有效提高飞行器动态飞行过程中的惯性导航系统精度，将具有突出的应用价值。The traditional IMU error calibration methods mostly use the turntable to perform position test and speed test indoors to calibrate the installation error and scale factor error. However, during the dynamic flight of the aircraft, due to the influence of the environment, the traditional calibration method using the turntable will no longer be applicable. During the dynamic flight of the aircraft, the traditional error correction of the inertial sensor is only aimed at its random error, and does not consider the influence of fixed errors such as its installation error and scale factor error. However, the installation error and scale factor error caused by flight vibration and other influences will greatly affect the accuracy of the inertial navigation system, which must be calibrated and compensated. Therefore, it will be of great application value to study a calibration method for the fixed error of the inertial sensor during the dynamic flight of the aircraft, which can effectively improve the accuracy of the inertial navigation system during the dynamic flight of the aircraft.

发明内容Contents of the invention

本发明的目的在于提出一种惯性导航系统惯性传感器的固定性误差建模及实时标定方法，以满足飞行器动态飞行过程中的对导航系统的高精度要求。The object of the present invention is to propose a fixed error modeling and real-time calibration method of an inertial sensor of an inertial navigation system, so as to meet the high-precision requirements of the navigation system during the dynamic flight of an aircraft.

本发明为解决上述技术问题采用以下技术方案，一种惯性导航系统中惯性传感器固定误差实时标定方法，所述方法具体步骤如下：In order to solve the above technical problems, the present invention adopts the following technical solutions, a real-time calibration method for inertial sensor fixed errors in an inertial navigation system, the specific steps of the method are as follows:

步骤1、建立惯性传感器的固定性误差模型，包括加速度计标度因数误差矩阵和陀螺仪的标度因数误差矩阵：Step 1. Establish the fixed error model of the inertial sensor, including the scale factor error matrix of the accelerometer and the scale factor error matrix of the gyroscope:

加速度计标度因数误差矩阵为δK_A＝diag[δK_Ax δK_Ay δK_Az]，其中，δK_Ax、δK_Ay、δK_Az分别为X轴、Y轴及Z轴加速度计的标度因数误差；加速度计安装误差矩阵为 $δA = [\begin{matrix} 0 & {δA}_{z} & - {δA}_{y} \\ - {δA}_{z} & 0 & {δA}_{x} \\ {δA}_{y} & - {δA}_{x} & 0 \end{matrix}],$ 其中，δA_x、δA_y、δA_z分别为X轴、Y轴及Z轴加速度计的安装误差角；δK_A和δA均取为随机常数，X、Y、Z三轴的加速度计误差模型相同，如下所示：The accelerometer scale factor error matrix is δK _A =diag[δK _Ax δK _Ay δK _Az ], where δK _Ax , δK _Ay , and δK _Az are the scale factor errors of the X-axis, Y-axis and Z-axis accelerometers respectively; The installation error matrix of the meter is $δA = [\begin{matrix} 0 & {δA}_{z} & - {δA}_{the y} \\ - {δA}_{z} & 0 & {δA}_{x} \\ {δA}_{the y} & - {δA}_{x} & 0 \end{matrix}],$ Among them, δA _x , δA _y , and δA _z are the installation error angles of the X-axis, Y-axis, and Z-axis accelerometers respectively; δK _A and δA are taken as random constants, and the accelerometer error models of the X, Y, and Z axes are the same ,As follows:

$\{\begin{matrix} δ δ {\overset{\cdot \cdot}{K K}}_{A A} = = 00 \\ δ δ \overset{\cdot \cdot}{A A} = = 00 \end{matrix}$

上式中，

为标度因数误差矩阵δK_A的一阶导数，为安装误差矩阵δA的一阶导数；In the above formula,

is the first derivative of the scale factor error matrix δK _A , is the first derivative of the installation error matrix δA;

陀螺仪的标度因数误差矩阵为δK_G＝diag[δK_Gx δK_Gy δK_Gz]，δK_Gx、δK_Gy、δK_Gz分别为X轴、Y轴及Z轴陀螺的标度因数误差；陀螺安装误差矩阵为 $δG = [\begin{matrix} 0 & {δG}_{z} & - {δG}_{y} \\ - {δG}_{z} & 0 & {δG}_{x} \\ {δG}_{y} & - {δG}_{x} & 0 \end{matrix}],$ δG_x、δG_y、δG_z分别为X轴、Y轴及Z轴陀螺的安装误差；标度因数误差和安装误差均取为随机常数，X、Y、Z三轴的陀螺仪误差模型相同，如下所示：The scale factor error matrix of the gyroscope is δK _G =diag[δK _Gx δK _Gy δK _Gz ], δK _Gx , δK _Gy , and δK _Gz are the scale factor errors of the X-axis, Y-axis and Z-axis gyroscopes respectively; the installation error of the gyroscope The matrix is $δ G = [\begin{matrix} 0 & {δ G}_{z} & - {δ G}_{the y} \\ - {δG}_{z} & 0 & {δ G}_{x} \\ {δG}_{the y} & - {δ G}_{x} & 0 \end{matrix}],$ δG _x , δG _y , and δG _z are the installation errors of the X-axis, Y-axis, and Z-axis gyroscopes respectively; the scale factor error and the installation error are both taken as random constants, and the gyroscope error models of the X, Y, and Z axes are the same, As follows:

$\{\begin{matrix} δ δ {\overset{\cdot \cdot}{K K}}_{G G} = = 00 \\ δ δ \overset{\cdot \cdot}{G G} = = 00 \end{matrix}$

上式中，

为标度因数误差矩阵δK_G的一阶导数，

为安装误差矩阵δG的一阶导数；In the above formula,

is the first derivative of the scale factor error matrix δK _G ,

is the first derivative of the installation error matrix δG;

步骤2、在步骤1对IMU的安装误差和标度因数误差建模的基础上，将IMU的安装误差和标度因数误差作为系统状态变量，构建基于惯性传感器全状态误差模型的滤波器状态方程和位置、速度及姿态线性化量测方程：Step 2. On the basis of modeling the installation error and scale factor error of the IMU in step 1, take the installation error and scale factor error of the IMU as system state variables, and construct the filter state equation based on the full state error model of the inertial sensor and position, velocity, and attitude linearization measurement equations:

基于惯性传感器全状态误差建模的系统状态变量X为：The system state variable X based on the inertial sensor full state error modeling is:

X＝[φ_E φ_N φ_U δV_E δV_N δV_U δL δλ δh ε_bx ε_by ε_bz ε_rx ε_ry ε_rz ▽_x ▽_y ▽_zδA_x δA_y δA_z δK_Ax δK_Ay δK_Az δG_x δG_y δG_z δK_Gx δK_Gy δK_Gz]^T X＝[φ _E φ _N φ _U δV _E δV _N δV _U δL δλ δh ε _bx ε _by ε _bz ε _rx ε _ry ε _rz ▽ _x ▽ _y ▽ _z δA _x δA _y δA _z δK _Ax δK _Ay δK _Az δG _x δG _y δG _z δK _Gx δK _Gy δK _Gz ] ^T

上式中，φ_E,φ_N,φ_U分别为惯性导航系统中的东向、北向及天向平台误差角状态量；δV_E,δV_N,δV_U分别为惯性导航系统误差中的东向、北向及天向速度误差状态量；δL,δλ,δh分别为惯性导航系统中的纬度误差状态量、经度误差状态量及高度误差状态量；ε_bx,ε_by,ε_bz分别为惯性导航系统中的X轴、Y轴、Z轴方向陀螺的常值漂移误差状态量；ε_rx,ε_ry,ε_rz分别为X轴、Y轴和Z轴方向的陀螺一阶马尔科夫漂移误差状态量；▽_x,▽_y,▽_z分别为X轴、Y轴及Z轴方向的加速度计一阶马尔科夫零偏误差状态量；δA_x,δA_y,δA_z分别为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的安装误差状态量；δK_Ax,δK_Ay,δK_Az分别为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的标度因数误差状态量；δG_x,δG_y,δG_z分别为惯性导航系统中X轴、Y轴及Z轴三个方向上陀螺的安装误差状态量；δK_Gx,δK_Gy,δK_Gz为惯性导航系统中X轴、Y轴及Z轴方向上陀螺的标度因数误差状态量；In the above formula, φ _E , φ _N , φ _U are the eastward, northward and skyward platform error angle state variables in the inertial navigation system respectively; δV _E , δV _N , δV _U are the eastward direction in the inertial navigation system , north and sky direction speed error state quantities; δL, δλ, δh are the latitude error state quantities, longitude error state quantities and altitude error state quantities in the inertial navigation system respectively; ε _bx , ε _by , ε _bz are the inertial navigation system The constant value drift error state quantity of the gyroscope in the X _- _axis , Y- _axis , and Z-axis directions in ; ▽ _x , ▽ _y , ▽ _z are the first-order Markov zero-bias error state variables of the accelerometer in the X-axis, Y-axis and Z-axis directions respectively; δA _x , δA _y , δA _z are the X-axis in the inertial navigation system , the installation error state quantity of the accelerometer on the Y-axis and the Z-axis direction; δK _Ax , δK _Ay , and δK _Az are the scale factor error state quantities of the accelerometer on the X-axis, Y-axis and Z-axis directions in the inertial navigation system, respectively; δG _x , δG _y , δG _z are the installation error state quantities of the gyroscope in the X-axis, Y-axis and Z-axis directions of the inertial navigation system respectively; δK _Gx , δK _Gy , δK _Gz are the X-axis, Y-axis The scale factor error state quantity of the gyroscope on the axis and the Z axis direction;

建立惯性导航系统的误差状态方程，如式(4)所示：Establish the error state equation of the inertial navigation system, as shown in formula (4):

$\overset{\cdot \cdot}{X x} ((t t)) = = F f ((t t)) X x ((t t)) + + G G ((t t)) W W ((t t))$

上式中，X(t)为系统状态变量；

为状态变量X(t)的一阶导数；F(t)为系统矩阵；G(t)为噪声系数矩阵；W(t)为噪声矩阵；In the above formula, X(t) is the system state variable;

is the first derivative of the state variable X(t); F(t) is the system matrix; G(t) is the noise coefficient matrix; W(t) is the noise matrix;

采用地理系下位置、速度和姿态线性化观测原理，建立地理系下卫星导航位置观测量、速度观测量及星敏感器姿态观测量和惯性导航系统误差状态量之间的线性化量测方程：Using the principle of linearized observation of position, velocity, and attitude in the geographic system, the linearized measurement equations between the satellite navigation position observations, velocity observations, star sensor attitude observations, and inertial navigation system error state quantities in the geographic system are established:

$Z Z ((t t)) = = [\begin{matrix} {Z Z}_{p p} ((t t)) \\ {Z Z}_{v v} ((t t)) \\ {Z Z}_{φ φ} ((t t)) \end{matrix}] = = [\begin{matrix} {H h}_{p p} ((t t)) \\ {H h}_{v v} ((t t)) \\ {H h}_{φ φ} ((t t)) \end{matrix}] X x ((t t)) + + [\begin{matrix} {V V}_{p p} ((t t)) \\ {V V}_{v v} ((t t)) \\ {V V}_{φ φ} ((t t)) \end{matrix}]$

上式中，Z_p(t)、Z_v(t)、Z_φ(t)分别为位置、速度及姿态量测矢量；H_p(t)、H_v(t)、H_φ(t)分别为位置、速度及姿态量测系数矩阵；V_p(t)、V_v(t)、V_φ(t)分别为位置、速度及姿态观测量噪声矩阵；In the above formula, Z _p (t), Z _v (t), Z _φ (t) are position, velocity and attitude measurement vectors respectively; H _p (t), H _v (t), H _φ (t) are respectively is the position, velocity and attitude measurement coefficient matrix; V _p (t), V _v (t), V _φ (t) are position, velocity and attitude observation noise matrix respectively;

步骤3、在步骤2的基础上，对系统状态方程和量测方程进行离散化处理以及状态量、量测量的更新，实现对IMU安装误差和标度因数误差的标定与补偿。Step 3. On the basis of step 2, discretize the system state equation and measurement equation and update the state quantity and quantity measurement to realize the calibration and compensation of the IMU installation error and scale factor error.

进一步的，步骤3中所述对IMU安装误差和标度因数误差的标定与补偿，其具体步骤为：Further, the calibration and compensation of the IMU installation error and scale factor error described in step 3, the specific steps are:

（301）将滤波器状态方程和量测方程离散化处理：(301) discretize the filter state equation and measurement equation:

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)X(k)=Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)

Z(k)＝H(k)X(k)+V(k)Z(k)=H(k)X(k)+V(k)

上式中，X(k)为t_k时刻系统状态量；X(k-1)为t_k-1时刻系统状态量；Φ(k,k-1)为t_k-1时刻至t_k时刻系统的状态转移矩阵；Γ(k,k-1)为t_k-1时刻至t_k时刻系统的噪声驱动矩阵；W(k-1)为t_k时刻系统的噪声矩阵；Z(k)为t_k时刻系统的位置、速度及姿态观测量矩阵；H(k)为t_k时刻的位置、速度及姿态量测系数矩阵；V(k)为t_k时刻的位置、速度及姿态观测量的噪声矩阵；In the above formula, X(k) is the system state quantity at time t _k ; X(k- _{1) is the system state quantity at time t k} _{-1; Φ(k,k-1} ) is _the The state transition matrix of the system; Γ(k,k-1) is the noise driving matrix of the system from time t _k-1 to time t _k ; W(k-1) is the noise matrix of the system at time t _k ; Z(k) is The position, velocity and attitude observation matrix of the system at time t _k ; H(k) is the position, velocity and attitude measurement coefficient matrix at time t _k ; V(k) is the matrix of position, velocity and attitude observations at time t _k noise matrix;

（302）按照式(8)、式(9)、式(10)、式(11)对步骤（301）部分的惯性导航系统误差状态量和协方差信息的量测更新：(302) According to formula (8), formula (9), formula (10), formula (11), the measurement update of the inertial navigation system error state quantity and covariance information in step (301):

P(k,k-1)＝Φ(k,k-1)P(k-1,k-1)Φ(k,k-1)^T+Γ(k,k-1)Q(k)Γ(k,k-1)^T P(k,k-1)=Φ(k,k-1)P(k-1,k-1)Φ(k,k-1) ^T +Γ(k,k-1)Q(k)Γ (k,k-1) ^T

K(k)＝P(k,k-1)H(k)^T[H(k)P(k,k-1)H(k)^T+R(k)]^-1 K(k)=P(k,k-1)H(k) ^T [H(k)P(k,k-1)H(k) ^T +R(k)] ^-1

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)]X(k)=X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)]

P(k,k)＝[I-K(k)H(k)]P(k,k-1)P(k,k)=[I-K(k)H(k)]P(k,k-1)

上述公式中，P(k,k-1)为t_k-1时刻至t_k时刻一步预测协方差矩阵；P(k-1,k-1)为t_k-1时刻滤波状态估计协方差矩阵；Q(k)为t_k时刻系统的噪声协方差矩阵；Κ(k)为t_k时刻滤波增益矩阵；R(k)为t_k时刻位置、速度及姿态量测噪声协方差阵；X(k,k-1)为t_k-1时刻至t_k时刻一步预测状态量；P(k,k)为t_k时刻滤波状态估计协方差矩阵；I为与P(k,k)维数相同的单位矩阵；In the above formula, P(k,k-1) is the one-step prediction covariance matrix from time t _k-1 to time t _k ; P(k-1,k-1) is the covariance matrix of filter state estimation at time t _k-1 ; Q(k) is the noise covariance matrix of the system at _tk moment; Κ(k) is the filter gain matrix at _tk moment; R(k) is the position, velocity and attitude measurement noise covariance matrix at _tk moment; X( k, k-1) is the one-step forecast state quantity from time t _k-1 to time t _k ; P(k, k) is the covariance matrix of filter state estimation at time t _k ; I is the same dimension as P(k, k) the identity matrix;

（303）对滤波输出的状态量X(k,k)进行判断，若状态量X(k,k)的预测值稳定则完成标定，得到对IMU安装误差和标度因数误差的标定结果，进入步骤（304）；若预测值不稳定，则返回步骤（302）继续对状态值进行预测估计；(303) Judging the state quantity X(k,k) output by the filter, if the predicted value of the state quantity X(k,k) is stable, the calibration is completed, and the calibration results of the IMU installation error and the scaling factor error are obtained, and enter Step (304); if the predicted value is unstable, return to step (302) to continue predicting and estimating the state value;

（304）在步骤（303）得到对IMU安装误差和标度因数误差的标定结果后，暂存标定值，利用标定值对IMU安装误差和标度因数误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：(304) After the calibration results of IMU installation error and scale factor error are obtained in step (303), the calibration value is temporarily stored, and the calibration value is used to compensate and correct the IMU installation error and scale factor error, and the correction is performed in a navigation solution Completed within a cycle, the error compensation correction algorithm is:

$\{\begin{matrix} {f f}^{b b} = = [[I I - - {δK δK}_{A A} - - δA δA]] (({\overset{~ ~}{f f}}^{b b} - - {&dtri; &dtri;}^{b b})) \\ {ω ω}_{ib ib}^{b b} = = [[I I - - {δK δK}_{G G} - - δG δG]] (({\overset{~ ~}{ω ω}}_{ib ib}^{b b} - - {ϵ ϵ}^{b b})) \end{matrix}$

上式中，

分别为误差的加速度计输出的加速度和陀螺仪输出的角速率；f^b、

分别为去除掉误差后的加速度和角速率；▽^b为加速度计的随机漂移误差，ε^b为陀螺的随机漂移误差。In the above formula,

The acceleration output by the accelerometer and the angular rate output by the gyroscope are the errors respectively; f ^b ,

are the acceleration and angular rate after removing the error; ▽ ^b is the random drift error of the accelerometer, and ε ^b is the random drift error of the gyroscope.

本发明在对惯性传感器误差建模的基础上，能够在飞行器动态飞行过程中实现对惯性传感器固定性误差的实时标定，利用标定得到的结果可以对惯性传感器误差进行实时补偿，采用本发明可以显著减小惯性导航系统误差能够有效提高惯性导航系统精度，适合工程应用。On the basis of modeling the inertial sensor error, the present invention can realize the real-time calibration of the inertial sensor fixed error during the dynamic flight process of the aircraft, and the result obtained by using the calibration can perform real-time compensation for the inertial sensor error, and the present invention can significantly Reducing the error of the inertial navigation system can effectively improve the accuracy of the inertial navigation system, which is suitable for engineering applications.

附图说明Description of drawings

图1为本发明的惯性导航系统惯性传感器固定性误差实时标定方法结构图。FIG. 1 is a structural diagram of a real-time calibration method for inertial sensor inertial errors of an inertial navigation system according to the present invention.

图2为标定所用的飞行轨迹。Figure 2 shows the flight path used for calibration.

图3为X轴、Y轴及Z轴陀螺的安装误差标定结果。Figure 3 shows the installation error calibration results of the X-axis, Y-axis and Z-axis gyroscopes.

图4为X轴、Y轴及Z轴陀螺的标度因数误差标定结果。Figure 4 shows the scaling factor error calibration results of the X-axis, Y-axis and Z-axis gyroscopes.

图5为X轴、Y轴及Z轴加速度计的安装误差标定结果。Figure 5 shows the installation error calibration results of the X-axis, Y-axis and Z-axis accelerometers.

图6为X轴、Y轴及Z轴加速度计的标度因数误差标定结果。Figure 6 shows the scaling factor error calibration results of the X-axis, Y-axis and Z-axis accelerometers.

图7为惯性传感器固定性误差标定补偿后的惯性导航姿态误差。Figure 7 shows the inertial navigation attitude error after calibration and compensation of inertial sensor fixed error.

图8为惯性传感器固定性误差标定补偿后的惯性导航速度误差。Fig. 8 shows the inertial navigation speed error after calibration and compensation of inertial sensor fixed error.

图9为惯性传感器固定性误差标定补偿后的惯性导航位置误差。Fig. 9 shows the inertial navigation position error after calibration and compensation of inertial sensor fixed error.

具体实施方式Detailed ways

下面结合附图对本发明的技术方案做进一步的详细说明：如图1所示，本发明所述的惯性传感器固定性误差实时标定方法的原理是：陀螺和加速度计的测量输出分别为

及

其中均包含了固定性和随机性误差。

及f^b是经过误差补偿修正后的角速率和加速度信息用于惯性导航系统的导航解算模块。全状态误差建模模块在传统误差模型的基础上建立IMU的安装误差和标度因数误差模型，构建惯性导航系统误差的滤波状态量及状态方程；同时，根据地理系下姿态、速度和位置线性化观测原理，建立地理系下速度、位置及姿态的线性化量测方程，实现对惯性传感器安装误差和标度因数误差的实时标定；利用标定的结果在IMU误差修正模块对IMU的固定性误差进行补偿修正，能够有效提高惯性导航系统精度。Below in conjunction with accompanying drawing, the technical scheme of the present invention is described in further detail: as shown in Figure 1, the principle of the inertial sensor fixed error real-time calibration method of the present invention is: the measurement output of gyroscope and accelerometer is respectively

and

Both include fixed and random errors.

And f ^b is the angular rate and acceleration information corrected by error compensation, which is used in the navigation calculation module of the inertial navigation system. The full state error modeling module establishes the IMU installation error and scale factor error model on the basis of the traditional error model, and constructs the filter state quantity and state equation of the inertial navigation system error; at the same time, it linearizes the attitude, velocity and position according to the geographic system Based on the principle of observation, the linearized measurement equations of velocity, position, and attitude under the geographic system are established to realize real-time calibration of inertial sensor installation errors and scale factor errors; the fixed errors of the IMU are corrected in the IMU error correction module using the calibration results Compensation and correction can effectively improve the accuracy of the inertial navigation system.

本发明的具体实施方式如下：The specific embodiment of the present invention is as follows:

1.建立惯性传感器固定性误差模型1. Establish the inertial sensor fixed error model

（1.1）加速度计误差模型(1.1) Accelerometer error model

在捷联惯性导航系统中，由于飞行器振动会导致陀螺仪和加速度计都存在安装误差及标度因数误差。忽略高阶非线性误差项，加速度计的误差模型为：In the strapdown inertial navigation system, there are installation errors and scale factor errors in both the gyroscope and the accelerometer due to aircraft vibration. Neglecting higher-order nonlinear error terms, the error model for the accelerometer is:

δf^b＝[δK_A+δA]f^b+▽^b (1)δf ^b ＝[δK _A +δA]f ^b +▽ ^b (1)

式(1)中，δK_A＝diag[δK_Ax δK_Ay δK_Az]为加速度计的标度因数误差；In formula (1), δK _A =diag[δK _Ax δK _Ay δK _Az ] is the scale factor error of the accelerometer;

▽＝[▽_x ▽_y ▽_z]^T为加速度计的零偏误差； $δA = [\begin{matrix} 0 & {δA}_{z} & - {δA}_{y} \\ - {δA}_{z} & 0 & {δA}_{x} \\ {δA}_{y} & - {δA}_{x} & 0 \end{matrix}]$ 为加速度计的安装误差角矩阵；▽＝[▽ _x ▽ _y ▽ _z ] ^T is the zero bias error of the accelerometer; $δA = [\begin{matrix} 0 & {δA}_{z} & - {δA}_{the y} \\ - {δA}_{z} & 0 & {δA}_{x} \\ {δA}_{the y} & - {δA}_{x} & 0 \end{matrix}]$ is the installation error angle matrix of the accelerometer;

加速度计的标度因数误差δK_A和安装误差δA均取为随机常数，加速度计的零偏误差▽取为一阶马尔可夫过程：且假定三个轴加速度计的误差模型相同：Both the scale factor error δK _A and the installation error δA of the accelerometer are taken as random constants, and the zero bias error ▽ of the accelerometer is taken as a first-order Markov process: and it is assumed that the error models of the three-axis accelerometers are the same:

$\{\begin{matrix} δ δ {\overset{\cdot \cdot}{K K}}_{A A} = = 00 \\ δ δ \overset{\cdot \cdot}{A A} = = 00 \\ \overset{\cdot \cdot}{&dtri; &dtri;} = = - - \frac{11}{{T T}_{a a}} &dtri; &dtri; + + {w w}_{a a} \end{matrix} - - - - - - ((22))$

（1.2）陀螺误差模型(1.2) Gyro error model

与加速度计误差建模方法类似，陀螺的误差模型为：Similar to the accelerometer error modeling method, the error model of the gyroscope is:

${δω δω}_{ib ib}^{b b} = = [[{δK δK}_{G G} + + δG δG]] {ω ω}_{ib ib}^{b b} + + {ϵ ϵ}^{b b} - - - - - - ((33))$

式(3)中，δK_G＝diag[δK_Gx δK_Gy δK_Gz]为陀螺的标度因数误差矩阵； $δG = [\begin{matrix} 0 & {δG}_{z} & - {δG}_{y} \\ - {δG}_{z} & 0 & {δG}_{x} \\ {δG}_{y} & - {δG}_{x} & 0 \end{matrix}]$ 为陀螺的安装误差角矩阵；陀螺仪的标度因数误差和In formula (3), δK _G =diag[δK _Gx δK _Gy δK _Gz ] is the scale factor error matrix of the gyroscope; $δ G = [\begin{matrix} 0 & {δG}_{z} & - {δG}_{the y} \\ - {δG}_{z} & 0 & {δG}_{x} \\ {δ G}_{the y} & - {δG}_{x} & 0 \end{matrix}]$ is the installation error angle matrix of the gyroscope; the scale factor error of the gyroscope and

安装误差均取为随机常数，假定三个轴陀螺的误差模型相同，得到：The installation errors are all taken as random constants, assuming that the error models of the three-axis gyroscopes are the same, we get:

$\{\begin{matrix} δ δ {\overset{\cdot &Center Dot;}{K K}}_{G G} = = 00 \\ δ δ \overset{\cdot &Center Dot;}{G G} = = 00 \end{matrix} - - - - - - ((44))$

陀螺漂移误差ε_b＝[ε_x ε_y ε_z]^T取为Gyro drift error ε _b = [ε _x ε _y ε _z ] ^T is taken as

ε＝ε_b+ε_r+w_g (5)ε＝ε _b +ε _r +w _g (5)

式(5)中，

为随机常数，

为一阶马尔可夫过程，w_g为白噪声。In formula (5),

is a random constant,

is a first-order Markov process, w _g is white noise.

2.建立基于全状态误差模型的卡尔曼滤波器模型2. Establish a Kalman filter model based on the full state error model

（2.1）建立惯性导航系统误差的状态量方程(2.1) Establish the state quantity equation of the inertial navigation system error

选取机体坐标系(b系)为机体的“右、前、上”(X，Y，Z)方向，选取导航坐标系(n系)为东北天(ENU)地理坐标系。基于全状态误差建模的系统状态变量为：The body coordinate system (b system) is selected as the "right, front and up" (X, Y, Z) direction of the body, and the navigation coordinate system (n system) is selected as the Northeast Heaven (ENU) geographic coordinate system. The system state variables based on full state error modeling are:

(6)(6)

式(6)中，φ_E,φ_N,φ_U分别为惯性导航系统中的东向、北向及天向平台误差误差角状态量；δV_E,δV_N,δV_U分别为惯性导航系统误差中的东向、北向及天向速度误差状态量；δL,δλ,δh表示惯性导航系统中的纬度误差状态量、经度误差状态量及高度误差状态量；ε_bx,ε_by,ε_bz分别为惯性导航系统中的X轴、Y轴、Z轴方向陀螺的常值漂移误差状态量；ε_rx,ε_ry,ε_rz分别为X轴、Y轴和Z轴方向的陀螺一阶马尔科夫漂移误差状态量；▽_x,▽_y,▽_z分别为惯性导航系统中X轴、Y轴及Z轴方向的加速度计一阶马尔科夫零偏误差状态量；δA_x,δA_y,δA_z为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的安装误差状态量；δK_Ax,δK_Ay,δK_Az为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的标度因数误差状态量；δG_x,δG_y,δG_z为惯性导航系统中X轴、Y轴及Z轴三个方向上陀螺的安装误差状态量；δK_Gx,δK_Gy,δK_Gz为惯性导航系统中X轴、Y轴及Z轴方向上陀螺的标度因数误差状态量；In formula (6), φ _E , φ _N , φ _U are the error angle state variables of the eastward, northward and skyward platform errors in the inertial navigation system respectively; δV _E , δV _N , δV _U are the δL, δλ, δh represent the latitude error state quantity, longitude error state quantity and altitude error state quantity in the inertial navigation system; ε _bx , ε _by , ε _bz are the inertial The constant value drift error state quantity of the gyroscope in the X _- axis, Y- _axis , _and Z-axis directions in the navigation system; State quantities; ▽ _x , ▽ _y , ▽ _z are the first-order Markov zero-bias error state quantities of accelerometers in the X-axis, Y-axis and Z-axis directions of the inertial navigation system; δA _x , δA _y , δA _z are inertial The installation error state quantity of the accelerometer in the X-axis, Y-axis and Z-axis directions in the navigation system; δK _Ax , δK _Ay , δK _Az are the scale factors of the accelerometer in the X-axis, Y-axis and Z-axis directions in the inertial navigation system Error state quantity; δG _x , δG _y , δG _z are the installation error state quantities of the gyroscope in the three directions of X axis, Y axis and Z axis in the inertial navigation system; δK _Gx , δK _Gy , δK _Gz are the X axis in the inertial navigation system The scale factor error state quantity of the gyroscope in the directions of Y axis, Y axis and Z axis;

建立惯性导航系统的误差状态方程，如式(7)所示：Establish the error state equation of the inertial navigation system, as shown in formula (7):

$\overset{\cdot \cdot}{X x} ((t t)) = = F f ((t t)) X x ((t t)) + + G G ((t t)) W W ((t t)) - - - - - - ((77))$

式(7)中，X(t)为系统状态变量，

为状态变量X(t)的一阶导数；F(t)为系统矩阵，G(t)为噪声系数矩阵，W(t)为噪声矩阵。In formula (7), X(t) is the system state variable,

is the first derivative of the state variable X(t); F(t) is the system matrix, G(t) is the noise coefficient matrix, and W(t) is the noise matrix.

根据步骤1中所建立的惯性传感器误差模型，与式(6)及式(7)对应的系统矩阵F(t)为：According to the inertial sensor error model established in step 1, the system matrix F(t) corresponding to formula (6) and formula (7) is:

$F f ((t t)) = = [\begin{matrix} {FN FN ((t t))}_{99 \times \times 99} & FS FS {((t t))}_{99 \times \times 99} & {Fs Fs ((t t))}_{99 \times \times 1212} \\ 00_{99 \times \times 99} & {FM FM ((t t))}_{99 \times \times 99} & 00_{99 \times \times 1212} \\ 00_{1212 \times \times 3030} \end{matrix}] - - - - - - ((88))$

式(8)中，FN(t)_9×9为对应9个惯性导航系统导航参数（姿态、速度及位置）之间的关系矩阵，其非零项元素为：In formula (8), FN(t) _9×9 is the relationship matrix corresponding to nine inertial navigation system navigation parameters (attitude, velocity and position), and its non-zero elements are:

$\{\begin{matrix} FN FN ((1,2 1,2)) = = {ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L \\ FN FN ((1,3 1,3)) = = - - (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h})) \\ FN FN ((1,5 1,5)) = = - - \frac{11}{{R R}_{M m} + + h h} \end{matrix}$ $\{\begin{matrix} FN FN ((2,1 2,1)) = = - - (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L)) \\ FN FN ((2,3 2,3)) = = - - \frac{{V V}_{N N}}{{R R}_{M m} + + h h} \\ FN FN ((2,4 2,4)) = = \frac{11}{{R R}_{N N} + + h h} \\ FN FN ((2,7 2,7)) = = - - {ω ω}_{ie ie} sin sin L L \end{matrix}$

$\{\begin{matrix} FN FN ((3,1 3,1)) = = {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} \\ FN FN ((3,2 3,2)) = = \frac{{V V}_{N N}}{{R R}_{M m} + + h h} \\ FN FN ((3,4 3,4)) = = \frac{tan the tan L L}{{R R}_{N N} + + h h} \\ FN FN ((3,7 3,7)) = = {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} {sec sec}^{22} L L \end{matrix}$

$\{\begin{matrix} FN FN ((4,2 4,2)) = = - - {f f}_{u u} \\ FN FN ((4,3 4,3)) = = {f f}_{n no} \\ FN FN ((4,4 4,4)) = = \frac{{V V}_{N N} tan the tan L L - - {V V}_{U u}}{{R R}_{N N} + + h h} \\ FN FN ((4,5 4,5)) = = {22 ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L \\ FN FN ((4,6 4,6)) = = - - ((22 {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h})) \\ FN FN ((4,7 4,7)) = = {22 ω ω}_{ie ie} (({V V}_{U u} sin sin L L + + {V V}_{N N} cos cos L L)) + + \frac{{V V}_{E E.} {V V}_{N N}}{{R R}_{N N} + + h h} {sec sec}^{22} L L \end{matrix}$

$\{\begin{matrix} FN FN ((5,1 5,1)) = = {f f}_{u u} \\ FN FN ((5,3 5,3)) = = - - {f f}_{e e} \\ FN FN ((5,4 5,4)) = = - - 22 (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L)) \\ FN FN ((5,5 5,5)) = = - - \frac{{V V}_{U u}}{{R R}_{M m} + + h h} \\ FN FN ((5,6 5,6)) = = - - \frac{{V V}_{N N}}{{R R}_{M m} + + h h} \\ FN FN ((5,7 5,7)) = = - - (({22 V V}_{E E.} {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}^{22}}{{R R}_{N N} + + h h} {sec sec}^{22} L L)) \end{matrix}$ $\{\begin{matrix} FN FN ((6,1 6,1)) = = - - {f f}_{n no} \\ FN FN ((6,2 6,2)) = = {f f}_{e e} \\ FN FN ((6,4 6,4)) = = 22 (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{N N} + + h h})) \\ FN FN ((6,5 6,5)) = = \frac{{22 V V}_{N N}}{{R R}_{M m} + + h h} \\ FN FN ((6,7 6,7)) = = - - {22 V V}_{E E.} {ω ω}_{ie ie} sin sin L L \end{matrix}$

$\{\begin{matrix} FN FN ((7,5 7,5)) = = \frac{11}{{R R}_{M m} + + h h} \\ FN FN ((8,4 8,4)) = = \frac{sec sec L L}{{R R}_{N N} + + h h} \\ FN FN ((8,7 8,7)) = = \frac{{V V}_{E E.}}{R R} \\ FN FN ((9,6 9,6)) = = 11 \end{matrix}$

R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径；f_e,f_n,f_u为东向、北向及天向的加速度。R _M is the radius of curvature of the meridian circle of the earth, R _N is the radius of curvature of the meridian circle of the earth; f _e , f _n , f _u are the accelerations in the east, north and celestial directions.

式(8)中， $FS (t) = [\begin{matrix} C_{b}^{n} & C_{b}^{n} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{n} \\ 0_{3 \times 9} \end{matrix}],$

为机体系到地理坐标系的转换矩阵In formula (8),

FS (t) = [\begin{matrix} C_{b}^{no} & C_{b}^{no} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & C_{b}^{no} \\ 0_{3 \times 9} \end{matrix}],

Transformation matrix from machine system to geographic coordinate system

${Fs Fs ((t t))}_{99 \times \times 1212} = = [\begin{matrix} 00_{33 \times \times 66} & {F f}_{11} {((t t))}_{33 \times \times 33} & {F f}_{33} {((t t))}_{33 \times \times 33} \\ {F f}_{22} {((t t))}_{33 \times \times 33} & {F f}_{44} {((t t))}_{33 \times \times 33} & 00_{33 \times \times 66} \\ 00_{33 \times \times 1212} \end{matrix}] - - - - - - ((99))$

式(9)中：In formula (9):

${F f}_{11} ((t t)) = = [\begin{matrix} - - {C C}_{1212} {ω ω}_{ibz ibz}^{b b} + + {C C}_{1313} {ω ω}_{iby iby}^{b b} & {C C}_{1111} {ω ω}_{ibz ibz}^{b b} - - {C C}_{1313} {ω ω}_{ibx ibx}^{b b} & - - {C C}_{1111} {ω ω}_{iby iby}^{b b} + + {C C}_{1212} {ω ω}_{ibx ibx}^{b b} \\ - - {C C}_{22 twenty two} {ω ω}_{ibz ibz}^{b b} + + {C C}_{23 twenty three} {ω ω}_{iby iby}^{b b} & {C C}_{21 twenty one} {ω ω}_{ibz ibz}^{b b} - - {C C}_{23 twenty three} {ω ω}_{ibx ibx}^{b b} & - - {C C}_{21 twenty one} {ω ω}_{iby iby}^{b b} + + {C C}_{22 twenty two} {ω ω}_{ibx ibx}^{b b} \\ - - {C C}_{3232} {ω ω}_{ibz ibz}^{b b} + + {C C}_{3333} {ω ω}_{iby iby}^{b b} & {C C}_{3131} {ω ω}_{ibz ibz}^{b b} - - {C C}_{3333} {ω ω}_{ibx ibx}^{b b} & - - {C C}_{3131} {ω ω}_{iby iby}^{b b} + + {C C}_{3232} {ω ω}_{ibx ibx}^{b b} \end{matrix}],,$

${F f}_{22} ((t t)) = = [\begin{matrix} {C C}_{1212} {f f}_{z z}^{b b} - - {C C}_{1313} {f f}_{y the y}^{b b} & - - {C C}_{1111} {f f}_{z z}^{b b} + + {C C}_{1313} {f f}_{x x}^{b b} & {C C}_{1111} {f f}_{y the y}^{b b} - - {C C}_{1212} {f f}_{x x}^{b b} \\ {C C}_{22 twenty two} {f f}_{z z}^{b b} - - {C C}_{23 twenty three} {f f}_{y the y}^{b b} & - - {C C}_{21 twenty one} {f f}_{z z}^{b b} + + {C C}_{23 twenty three} {f f}_{x x}^{b b} & {C C}_{21 twenty one} {f f}_{y the y}^{b b} - - {C C}_{22 twenty two} {f f}_{x x}^{b b} \\ {C C}_{3232} {f f}_{z z}^{b b} - - {C C}_{3333} {f f}_{y the y}^{b b} & - - {C C}_{3131} {f f}_{z z}^{b b} + + {C C}_{3333} {f f}_{x x}^{b b} & {C C}_{3131} {f f}_{y the y}^{b b} - - {C C}_{3232} {f f}_{x x}^{b b} \end{matrix}]$

${F f}_{33} ((t t)) = = [\begin{matrix} {- - C C}_{1111} {ω ω}_{ibx ibx}^{b b} & - - {C C}_{1212} {ω ω}_{iby iby}^{b b} & - - {C C}_{1313} {ω ω}_{ibz ibz}^{b b} \\ {- - C C}_{21 twenty one} {ω ω}_{ibx ibx}^{b b} & - - {C C}_{22 twenty two} {ω ω}_{iby iby}^{b b} & - - {C C}_{23 twenty three} {ω ω}_{ibz ibz}^{b b} \\ - - {C C}_{3131} {ω ω}_{ibx ibx}^{b b} & - - {C C}_{3232} {ω ω}_{iby iby}^{b b} & - - {C C}_{3333} {ω ω}_{ibz ibz}^{b b} \end{matrix}],,$ ${F f}_{44} ((t t)) = = [\begin{matrix} {C C}_{1111} {f f}_{x x}^{b b} & {C C}_{1212} {f f}_{y the y}^{b b} & {C C}_{1313} {f f}_{z z}^{b b} \\ {C C}_{21 twenty one} {f f}_{x x}^{b b} & {C C}_{22 twenty two} {f f}_{y the y}^{b b} & {C C}_{23 twenty three} {f f}_{z z}^{b b} \\ {C C}_{3131} {f f}_{x x}^{b b} & {C C}_{3232} {f f}_{y the y}^{b b} & {C C}_{3333} {f f}_{z z}^{b b} \end{matrix}]$

其中：3为机体系到地理坐标系的转换矩阵， $f^{b} = {[\begin{matrix} f_{x}^{b} & f_{y}^{b} & f_{z}^{b} \end{matrix}]}^{T}$ 为加速度计在机体系的输出， $ω_{ib}^{b} = {[\begin{matrix} ω_{ibx}^{b} & ω_{iby}^{b} & ω_{ibz}^{b} \end{matrix}]}^{T}$ 为陀螺输出在机体系上的投影。in: 3 is the transformation matrix from machine system to geographic coordinate system, $f^{b} = {[\begin{matrix} f_{x}^{b} & f_{the y}^{b} & f_{z}^{b} \end{matrix}]}^{T}$ is the output of the accelerometer on-board system, $ω_{ib}^{b} = {[\begin{matrix} ω_{ibx}^{b} & ω_{iby}^{b} & ω_{ibz}^{b} \end{matrix}]}^{T}$ Output the projection on the machine system for the gyro.

式(8)中，惯性传感器误差之间的关系矩阵为：In formula (8), the relationship matrix between inertial sensor errors is:

$FM FM ((t t)) = = Diag Diag [\begin{matrix} 00 & 00 & 00 & - - \frac{11}{{T T}_{gx gx}} & - - \frac{11}{{T T}_{gy gy}} & - - \frac{11}{{T T}_{gz gz}} & - - \frac{11}{{T T}_{ax ax}} & - - \frac{11}{{T T}_{ay ay}} & - - \frac{11}{{T T}_{az az}} \end{matrix}] - - - - - - ((1010))$

系统噪声矩阵为：The system noise matrix is:

W＝[w_gx w_gy w_gz w_rx w_ry w_rz w_ax w_ay w_az] (11)W＝[w _gx w _gy w _gz w _rx w _ry w _rz w _ax w _ay w _az ] (11)

误差系数矩阵：Error coefficient matrix:

$G G ((t t)) = = [\begin{matrix} {C C}_{b b}^{n no} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \\ 00_{99 \times \times 33} & 00_{99 \times \times 33} & 00_{99 \times \times 33} \\ 00_{33 \times \times 33} & {I I}_{33 \times \times 33} & 00_{33 \times \times 33} \\ 00_{33 \times \times 33} & 00_{33 \times \times 33} & {I I}_{33 \times \times 33} \end{matrix}] - - - - - - ((1212))$

（2.2）量测方程的确定(2.2) Determination of measurement equation

采用地理系下姿态、速度和位置线性化观测原理，建立地理系下卫星导航速度、位置及星敏感器姿态观测量和步骤2.1)中惯性导航系统误差状态量之间的线性化量测方程：Using the principle of linearized observation of attitude, velocity and position under the geographic system, the linearized measurement equation between the satellite navigation velocity, position and star sensor attitude observations under the geographic system and the error state quantity of the inertial navigation system in step 2.1) is established:

$Z Z ((t t)) = = [\begin{matrix} {Z Z}_{p p} ((t t)) \\ {Z Z}_{v v} ((t t)) \\ {Z Z}_{φ φ} ((t t)) \end{matrix}] = = [\begin{matrix} {H h}_{p p} ((t t)) \\ {H h}_{v v} ((t t)) \\ {H h}_{φ φ} ((t t)) \end{matrix}] X x ((t t)) + + [\begin{matrix} {V V}_{p p} ((t t)) \\ {V V}_{v v} ((t t)) \\ {V V}_{φ φ} ((t t)) \end{matrix}] - - - - - - ((1313))$

式中，Z_p(t)，Z_v(t)以及Z_φ(t)是位置、速度及姿态量测矢量。H_p(t)，H_v(t)，H_φ(t)为位置、速度及姿态量测系数矩阵，位置、速度及姿态观测量噪声矩阵为V_p(t)，V_v(t)及V_φ(t)。3.惯性传感器固定性误差的实时标定补偿In the formula, Z _p (t), Z _v (t) and Z _φ (t) are position, velocity and attitude measurement vectors. H _p (t), H _v (t), H _φ (t) is the position, velocity and attitude measurement coefficient matrix, the position, velocity and attitude measurement noise matrix is V _p (t), V _v (t) and V _φ (t). 3. Real-time calibration compensation of inertial sensor fixed error

（3.1）将滤波器状态方程和量测方程离散化处理：(3.1) Discretize the filter state equation and measurement equation:

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1) (14)X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1) (14)

Z(k)＝H(k)X(k)+V(k) (15)Z(k)＝H(k)X(k)+V(k) (15)

式(15)中，X(k)为t_k时刻系统状态量；X(k-1)为t_k-1时刻系统状态量；Φ(k,k-1)为t_k-1时刻至t_k时刻系统的状态转移矩阵；Γ(k,k-1)为t_k-1时刻至t_k时刻系统的噪声驱动矩阵；W(k-1)为t_k时刻系统的噪声矩阵；Z(k)为t_k时刻系统的位置、速度及姿态观测量矩阵；H(k)为t_k时刻的位置、速度及姿态量测系数矩阵；V(k)为t_k时刻的位置、速度及姿态观测量的噪声矩阵。In formula (15), X(k) is the state quantity of the system at time t _k ; X(k-1) is the state quantity of the system at time t _k-1 ; Φ(k,k- ₁ ) is The state transition matrix of the system at time _k ; Γ(k,k-1) is the noise driving matrix of the system from time t _k-1 to time t _k ; W(k-1) is the noise matrix of the system at time t _k ; Z(k ) is the position, velocity and attitude observation matrix of the system at time t _k ; H(k) is the position, velocity and attitude measurement coefficient matrix at time t _k ; V(k) is the position, velocity and attitude observation at time t _k Quantitative noise matrix.

（3.2）按照式(16)、式(17)、式(18)、式(19)对子步骤3.1)部分的惯导误差状态量和协方差信息的量测更新：(3.2) According to the formula (16), formula (17), formula (18), formula (19) to the measurement update of the inertial navigation error state quantity and covariance information of sub-step 3.1):

(16)(16)

K(k)＝P(k,k-1)H(k)^T[H(k)P(k,k-1)H(k)^T+R(k)]^-1 (17)K(k)=P(k,k-1)H(k) ^T [H(k)P(k,k-1)H(k) ^T +R(k)] ^-1 (17)

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)] (18)X(k)=X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)] (18)

P(k,k)＝[I-K(k)H(k)]P(k,k-1) (19)P(k,k)＝[I-K(k)H(k)]P(k,k-1) (19)

其中，P(k,k-1)为t_k-1时刻至t_k时刻一步预测协方差矩阵；P(k-1,k-1)为t_k-1时刻滤波状态估计协方差矩阵；Q(k)为t_k时刻系统的噪声协方差矩阵；Κ(k)为t_k时刻滤波增益矩阵；R(k)为t_k时刻位置、速度及姿态量测噪声协方差阵；X(k,k-1)为t_k-1时刻至t_k时刻一步预测状态量；P(k,k)为t_k时刻滤波状态估计协方差矩阵；Among them, P(k,k-1) is the one-step prediction covariance matrix from time t _k-1 to time t _k ; P(k-1,k-1) is the covariance matrix of filter state estimation at time t _k-1 ; Q (k) is the noise covariance matrix of the system at time t _k ; Κ(k) is the filter gain matrix at time t _k ; R(k) is the noise covariance matrix of position, velocity and attitude measurement at time t _k ; X(k, k-1) is the one-step forecast state quantity from time t _k-1 to time t _k ; P(k,k) is the covariance matrix of filter state estimation at time t _k ;

I为与P(k,k)维数相同的单位阵；I is an identity matrix with the same dimension as P(k,k);

（3.3）对滤波输出的状态量X(k,k)进行判断，若状态量的预测值稳定，则完成标定进入步骤（3.4）若预测值不稳定，则返回步骤（3.2）继续对状态值进行预测估计。(3.3) Judge the state quantity X(k,k) output by the filter. If the predicted value of the state quantity is stable, complete the calibration and enter step (3.4). If the predicted value is unstable, return to step (3.2) and continue to check the state value Make forecast estimates.

（3.4）在步骤（3.3）得到对IMU安装误差和标度因数误差的标定结果后，暂存标定值，利用该标定值对IMU安装误差和标度因数误差进行补偿校正，校正在一个导航解算周期内完成。误差补偿校正算法为：(3.4) After obtaining the calibration results of the IMU installation error and scale factor error in step (3.3), temporarily store the calibration value, and use the calibration value to compensate and correct the IMU installation error and scale factor error. completed within the calculation period. The error compensation correction algorithm is:

$\{\begin{matrix} {f f}^{b b} = = [[I I - - {δK δK}_{A A} - - δA δA]] (({\overset{~ ~}{f f}}^{b b} - - {&dtri; &dtri;}^{b b})) \\ {ω ω}_{ib ib}^{b b} = = [[I I - - {δK δK}_{G G} - - δG δ G]] (({\overset{~ ~}{ω ω}}_{ib ib}^{b b} - - {ϵ ϵ}^{b b})) \end{matrix} - - - - - - ((2020))$

式(20)中，δK_A、δA、▽^b分别为加速度计的标度因数误差、安装误差和零偏，δK_G、δG、ε^b为陀螺的标度因数误差、安装误差及随机零偏，均由标定过程中获得。In formula (20), δK _A , δA, ▽ ^b are the scale factor error, installation error and zero bias of the accelerometer respectively, and δK _G , δG, ε ^b are the scale factor error, installation error and random zero bias of the gyroscope , are obtained during the calibration process.

（3.5）在步骤（3.4）对IMU的安装误差和标度因数误差校正补偿完成后，不再使用滤波器进入纯惯性导航系统工作模式。(3.5) After the correction and compensation of the installation error and scale factor error of the IMU in step (3.4), the filter is no longer used to enter the pure inertial navigation system working mode.

为了验证发明所提出的惯性导航系统惯性传感器的固定性误差建模及标定方法的正确性及有效性，采用本发明方法建立模型，进行matlab仿真验证。图2为验证时采用的飞行航迹，对惯性传感器的安装误差及标度因数误差的标定结果如图3、图4、图5、图6所示。In order to verify the correctness and effectiveness of the fixed error modeling and calibration method of the inertial navigation system inertial sensor proposed by the invention, the method of the invention is used to establish a model and perform matlab simulation verification. Figure 2 shows the flight path used in the verification, and the calibration results of the installation error and scale factor error of the inertial sensor are shown in Figure 3, Figure 4, Figure 5, and Figure 6.

在飞行器动态飞行过程中利用标定结果对IMU安装误差和标度因数误差进行补偿修正，补偿后的惯性导航结果与未经过补偿的惯性导航导航结果进行对比验证，验证结果如图7、图8、图9所示。During the dynamic flight process of the aircraft, the calibration results are used to compensate and correct the IMU installation error and scale factor error. The compensated inertial navigation results are compared with the uncompensated inertial navigation navigation results. The verification results are shown in Figure 7, Figure 8, Figure 9 shows.

图3、图4、图5、图6中黑色实线代表本发明的标定结果，红色点划线为设定值。从图中可以看出对IMU安装误差和标度因数误差标定所用时间在20s左右，标定结果与设定值一致。图7、图8、图9中黑色实线代表未补偿IMU固定性误差的纯惯导导航结果，红色点划线代表补偿IMU固定性误差后的纯惯导导航结果。从图7、图8、图9中可以看出，利用对IMU安装误差和标度因数误差的标定结果对IMU的安装误差和标度因数误差进行校正后，惯性导航系统误差明显减小，具有有益的工程应用价值。In Fig. 3, Fig. 4, Fig. 5 and Fig. 6, the black solid line represents the calibration result of the present invention, and the red dotted line is the set value. It can be seen from the figure that it takes about 20s to calibrate the IMU installation error and scale factor error, and the calibration result is consistent with the set value. In Figure 7, Figure 8, and Figure 9, the black solid line represents the pure inertial navigation result without compensating the IMU fixed error, and the red dotted line represents the pure inertial navigation result after compensating the IMU fixed error. It can be seen from Fig. 7, Fig. 8, and Fig. 9 that after correcting the installation error and scale factor error of the IMU by using the calibration results of the IMU installation error and scale factor error, the error of the inertial navigation system is significantly reduced, with Beneficial engineering application value.

Claims

1. inertial sensor fixed error real-time calibration method in the inertial navigation system is characterized in that described method concrete steps are as follows:

Step 1, set up the solid error model of inertial sensor, comprise accelerometer constant multiplier error matrix and gyrostatic constant multiplier error matrix:

Accelerometer constant multiplier error matrix is δ K _A=diag[δ K _Axδ K _Ayδ K _Az], wherein, δ K _Ax, δ K _Ay, δ K _AzBe respectively the constant multiplier error of X-axis, Y-axis and Z axis accelerometer; Accelerometer alignment error matrix is

δA = [\begin{matrix} 0 & {δA}_{z} & - {δA}_{y} \\ - {δA}_{z} & 0 & {δA}_{x} \\ {δA}_{y} & - {δA}_{x} & 0 \end{matrix}],

Wherein, δ A _x, δ A _y, δ A _zBe respectively the alignment error angle of X-axis, Y-axis and Z axis accelerometer; δ K _AAll be taken as arbitrary constant with δ A, the accelerometer error model that X, Y, Z are three is identical, as follows:

\{\begin{matrix} δ {\overset{\cdot}{K}}_{A} = 0 \\ δ \overset{\cdot}{A} = 0 \end{matrix}

In the following formula,

Be constant multiplier error matrix δ K _AFirst order derivative,

First order derivative for alignment error matrix delta A;

Gyrostatic constant multiplier error matrix is δ K _G=diag[δ K _Gxδ K _Gyδ K _Gz], δ K _Gx, δ K _Gy, δ K _GzBe respectively the constant multiplier error of X-axis, Y-axis and Z axle gyro; The gyro misalignment matrix is

δG = [\begin{matrix} 0 & {δG}_{z} & - {δG}_{y} \\ - {δG}_{z} & 0 & {δG}_{x} \\ {δG}_{y} & - {δG}_{x} & 0 \end{matrix}],

δ G _x, δ G _y, δ G _zBe respectively the alignment error of X-axis, Y-axis and Z axle gyro; Constant multiplier sum of errors alignment error all is taken as arbitrary constant, and the gyro error model that X, Y, Z are three is identical, as follows:

\{\begin{matrix} δ {\overset{\cdot}{K}}_{G} = 0 \\ δ \overset{\cdot}{G} = 0 \end{matrix}

In the following formula,

Be constant multiplier error matrix δ K _GFirst order derivative,

First order derivative for alignment error matrix delta G;

Step 2, on the basis of the alignment error of step 1 couple IMU and constant multiplier error modeling, the alignment error of IMU and constant multiplier error as system state variables, are made up filter status equation and position, speed and attitude linearization measurement equation based on inertial sensor total state error model:

System state variables X based on inertial sensor total state error modeling is:

X＝[φ _E φ _N φ _U δV _E δV _N δV _U δL δλ δh ε _bx ε _by ε _bz ε _rx ε _ry ε _rz ▽ _x ▽ _y ▽ _z δA _x δA _y δA _z δK _Ax δK _Ay δK _Az δG _x δG _y δG _z δK _Gx δK _Gy δK _Gz] ^T

In the following formula, φ _E, φ _N, φ _UBe respectively east orientation in the inertial navigation system, north orientation and day to platform error angle quantity of state; δ V _E, δ V _N, δ V _UBe respectively east orientation in the INS errors, north orientation and day to the velocity error quantity of state; δ L, δ λ, δ h are respectively latitude error quantity of state, longitude error quantity of state and the height error quantity of state in the inertial navigation system; ε _Bx, ε _By, ε _BzBe respectively the constant value drift error state amount of X-axis in the inertial navigation system, Y-axis, Z-direction gyro; ε _Rx, ε _Ry, ε _RzBe respectively the gyro single order Markov drift error quantity of state of X-axis, Y-axis and Z-direction; ▽ _x, ▽ _y, ▽ _zBe respectively the accelerometer single order Markov zero inclined to one side error state amount of X-axis, Y-axis and Z-direction; δ A _x, δ A _y, δ A _zBe respectively the alignment error quantity of state of accelerometer on X-axis in the inertial navigation system, Y-axis and the Z-direction; δ K _Ax, δ K _Ay, δ K _AzBe respectively the constant multiplier error state amount of accelerometer on X-axis in the inertial navigation system, Y-axis and the Z-direction; δ G _x, δ G _y, δ G _zBe respectively the alignment error quantity of state of gyro on X-axis in the inertial navigation system, Y-axis and three directions of Z axle; δ K _Gx, δ K _Gy, δ K _GzConstant multiplier error state amount for gyro on X-axis, Y-axis and the Z-direction in the inertial navigation system;

Set up the error state equation of inertial navigation system, as the formula (4):

\overset{\cdot}{X} (t) = F (t) X (t) + G (t) W (t)

In the following formula, X (t) is system state variables;

First order derivative for state variable X (t); F (t) is system matrix; G (t) is the noise figure matrix; W (t) is noise matrix;

Adopt Department of Geography's upper/lower positions, speed and attitude linearization observation principle, set up the linearization measurement equation between satellite navigation position detection amount, speed observed quantity and the observed quantity of star sensor attitude and INS errors quantity of state under the Department of Geography:

Z (t) = [\begin{matrix} Z_{p} (t) \\ Z_{v} (t) \\ Z_{φ} (t) \end{matrix}] = [\begin{matrix} H_{p} (t) \\ H_{v} (t) \\ H_{φ} (t) \end{matrix}] X (t) + [\begin{matrix} V_{p} (t) \\ V_{v} (t) \\ V_{φ} (t) \end{matrix}]

In the following formula, Z _p(t), Z _v(t), Z _φ(t) be respectively position, speed and attitude and measure vector; H _p(t), H _v(t), H _φ(t) be respectively position, speed and attitude and measure matrix of coefficients; V _p(t), V _v(t), V _φ(t) be respectively position, speed and attitude observed quantity noise matrix;

Step 3, on the basis of step 2, system state equation and measurement equation are carried out the renewal of discretize processing and quantity of state, measurement amount, realize demarcation and compensation to IMU alignment error and constant multiplier error.

2. inertial sensor fixed error real-time calibration method in a kind of inertial navigation system as claimed in claim 1 is characterized in that to demarcation and the compensation of IMU alignment error and constant multiplier error, its concrete steps are described in the step 3:

(301) filter status equation and measurement equation discretize are handled:

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)

Z(k)＝H(k)X(k)+V(k)

In the following formula, X (k) is t _kMoment system state amount; X (k-1) is t _K-1Moment system state amount; (k k-1) is t to Φ _K-1Constantly to t _kThe time etching system state-transition matrix; (k k-1) is t to Γ _K-1Constantly to t _kThe time etching system noise drive matrix; W (k-1) is t _kThe time etching system noise matrix; Z (k) is t _kThe time etching system position, speed and attitude observed quantity matrix; H (k) is t _kPosition constantly, speed and attitude measure matrix of coefficients; V (k) is t _kThe noise matrix of position, speed and attitude observed quantity constantly;

(302) according to formula (8), formula (9), formula (10), formula (11) the INS errors quantity of state of step (301) part and the measurement of covariance information are upgraded:

P(k,k-1)＝Φ(k,k-1)P(k-1,k-1)Φ(k,k-1) ^T+Γ(k,k-1)Q(k)Γ(k,k-1) ^T

K(k)＝P(k,k-1)H(k) ^T[H(k)P(k,k-1)H(k) ^T+R(k)] ^-1

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)]

P(k,k)＝[I-K(k)H(k)]P(k,k-1)

In the above-mentioned formula, (k k-1) is t to P _K-1Constantly to t _kMoment one-step prediction covariance matrix; (k-1 k-1) is t to P _K-1Moment filter state estimate covariance matrix; Q (k) is t _kThe time etching system noise covariance matrix; Κ (k) is t _kMoment filter gain matrix; R (k) is t _kMoment position, speed and attitude measurement noise covariance matrix; (k k-1) is t to X _K-1Constantly to t _kMoment one-step prediction quantity of state; (k k) is t to P _kMoment filter state estimate covariance matrix; I is and P (k, k) the identical unit matrix of dimension;

(303) to the quantity of state X of filtering output (k k) judges, if quantity of state X (k, predicted value k) is stable then finishes demarcation, obtains the calibration result to IMU alignment error and constant multiplier error, enters step (304); If the predicted value instability is then returned step (302) and is continued state value is predicted estimation;

(304) after step (303) obtains calibration result to IMU alignment error and constant multiplier error, temporary calibration value, utilize calibration value that IMU alignment error and constant multiplier error are compensated correction, proofread and correct and finish in the cycle at a navigation calculation, the error compensation correcting algorithm is:

\{\begin{matrix} f^{b} = [I - {δK}_{A} - δA] ({\tilde{f}}^{b} - {&dtri;}^{b}) \\ ω_{ib}^{b} = [I - {δK}_{G} - δG] ({\tilde{ω}}_{ib}^{b} - ϵ^{b}) \end{matrix}

In the following formula,

Be respectively the acceleration of accelerometer output of error and the angular speed of gyroscope output; f ^b,

Be respectively acceleration and the angular speed got rid of after the error; ▽ ^bBe the Random Drift Error of accelerometer, ε ^bRandom Drift Error for gyro.