CN102853834A - High-precision scheme of IMU for rotating carrier and denoising method - Google Patents

Abstract

The invention relates to an inertial unit (IMU) for a rotating carrier, which mainly designs a high-performance micro-miniature inertial instrument scheme (mainly composed of four MEMS gyroscopes to form three groups of orthogonal gyroscopes, three orthogonal quartz digital accelerometers, and a structural body , secondary power supply, signal processing and communication interface, etc.), the UKF Kalman filter is designed to effectively compensate the random drift error of the MEMS gyroscope. The invention has the characteristics of wide measuring range, small size, high dynamics and light weight, and is suitable for real-time measurement of the flight attitude of a high-speed rotating projectile during flight, and is suitable for stable attitude of long-range rockets, medium and short-range terminal guided munitions, and ground weapons system, aerial bomb flight control system and UAV attitude system, position and speed. It can also be applied to nuclear submarine strapdown positioning combination, environmental swing and tilt measurement system, inertial motion recording system of various vehicles and ships, etc.

Description

High-precision scheme and noise reduction method of IMU for rotating carrier

technical field

The invention relates to a high-performance micro-miniature inertial instrument scheme and design of UKF Kalman filter to compensate random drift of MEMS gyro to realize accurate measurement of flight attitude, position and speed of rotating bombs. It belongs to the field of navigation and guidance.

Background technique

The inertial instrument combined with a gyroscope and an accelerometer is a key component for autonomously measuring the carrier's motion acceleration and attitude information, and calculating the carrier's immediate velocity, position, angular velocity, and angular position through calculations, so that the weapon system can achieve precise strikes. The rotary bomb system is widely used in army ammunition at home and abroad. Its biggest advantage is that it uses high-speed rotation to maintain its flight stability and reduce the landing point error caused by the asymmetry of the aerodynamic shape and mass eccentricity. Small to gun bullets, large to long-range rockets are all rotary bombs. The biggest disadvantage of traditional uncontrolled ammunition is that it has a relatively large drop point spread, poor shooting accuracy and projectile density, and it is difficult to achieve precise killing effects. Therefore, conventional rockets and artillery shells need to introduce trajectory control technology. In this way, the advantages of dense firepower and good maneuverability of rotating bombs can be used, and the shortcomings of insufficient accuracy can be effectively solved. It has become a common development trend at home and abroad.

To increase the trajectory control of the rotary projectile, the first thing is to add an inertial measurement device to realize the measurement of the flight attitude of the rotary projectile, and then the control system corrects its trajectory according to the measurement results. In view of the limited installation space inside the rotary bomb, the inertial measurement device for the rotary bomb can only be combined with a miniaturized inertia. However, since the rotating bomb has a rotational speed around the longitudinal axis of the bomb body as high as 10r/s, the scale factor error of the existing gyroscope is worse than 5PPM at such a high rotational speed, which greatly restricts the use of inertial navigation technology on the rotating bomb. Applications. Therefore, there is an urgent need for a strapdown inertial measurement device that can be directly applied to existing rotary bombs, and can directly measure the flight attitude, position and velocity of the rotary bomb without providing a stable platform for the rotary bomb.

Due to the lack of gyroscopes sensitive to high-speed roll angular rate, the current strapdown inertial combination used for rotating projectiles is adopting various methods to prevent the strapdown inertial measurement unit from being subjected to the rotating environment of the projectile. For example, the GMLRS227mm long-range guided rocket jointly developed by five countries headed by the United States, in order to prevent the inertial measurement unit from being subjected to a rotating environment, a sliding bearing is added to isolate the rotation of the engine, and the guidance cabin is kept tilted and stable through the air rudder. The disadvantage of this method is that the system is complex, the ineffective weight is increased, and part of the rudder resources are sacrificed. The United States is researching an angular rate gyroscope with a measurement range of 3600°/s and a scale factor error of less than 5PPM, but the cost problem is difficult to solve in the short term.

Contents of the invention

The technical problem to be solved by the present invention is aimed at the guidance transformation needs of high-speed rotating bombs, on the basis of existing research, to realize the miniaturization of low-cost gyroscopes and accelerometers, the high-performance strapdown inertial combination scheme, and make full use of existing The advanced filtering technology realizes the random error compensation of the MEMS gyroscope, so as to realize the direct and accurate measurement of the flight attitude, position and speed of the rotating projectile by the IMU at high speed.

In order to achieve the above object, the present invention adopts the following technical solutions: the inertial unit (IMU) and the rotating bomb are fixed together, and three sets of MEMS orthogonal gyroscopes and three orthogonal high dynamic accelerometers are used. The main axis (corresponding to the rotation axis of the carrier) adopts two MEMS gyroscopes to improve the accuracy, each of the other two axes uses a MEMS gyroscope, and each of the three orthogonal axes uses a high dynamic accelerometer, and a temperature sensor is added to solve the problem caused by temperature changes. MEMS gyro and accelerometer measurement error, the power supply design uses a secondary power supply, the circuit part includes signal conditioning circuit, 16-bit A/D acquisition, ARM main control computer and communication interface, etc. In order to solve the random drift error of MEMS gyro, UKF is used here Filtering implements compensation for random drift errors.

The high-precision scheme and noise reduction method of the IMU for the rotating carrier are implemented as follows:

In the first step, in the case of high-speed rotation of the carrier, four gyroscopes (including two main axes) and three high-dynamic accelerometers and temperature sensors output signals, and the signal conditioning circuit adjusts the zero bias and coupling of the signals, and adjusts the signal amplification etc.

The second step is to set the frequency and use 16-bit A/D to collect the conditioned signal.

The third step is to use the main control computer (ARM) to process the signal, including using the UKF algorithm to compensate the random drift error of the MEMS gyroscope (including temperature compensation), and to compensate the error of the accelerometer measurement caused by the temperature, etc., and the processed data is the real flight attitude, position and velocity of the rotating bomb.

The fourth step is to output the real flight attitude, position and speed of the rotating bomb.

Description of drawings

Figure 1 is a schematic diagram of the structure diagram of the high-performance micro-miniature inertial instrument scheme

Figure 2 is a schematic diagram of three groups of MEMS orthogonal gyroscopes

Figure 3 is the original signal diagram of the MEMS gyroscope

Figure 4 is the signal diagram of the MEMS gyroscope after data preprocessing

Figure 5 is a schematic diagram of the MEMS gyroscope static random noise compensation process

Figure 6 is a static random noise compensation diagram of the MEMS gyroscope

Figure 7 is a schematic diagram of the dynamic noise compensation process of the MEMS gyroscope

Figure 8 is the noise compensation diagram of MEMS gyroscope rotating at a constant speed

Figure 9 is a noise compensation diagram of MEMS gyroscope with simultaneous changes in angle and direction

Figure 10 is a partial enlarged view of Figure 9

Detailed ways

(1) See Figure 1 for the solution of high-performance miniature inertial instrument

The inertia combination adopts an integrated design according to the requirements of miniaturization, and is mainly composed of three sets of MEMS orthogonal gyroscopes, three orthogonal high dynamic accelerometers, a structural body, a secondary power supply, signal processing and communication interfaces, etc., as shown in Figure 1 shown. Among them, the main shaft (corresponding to the rotation axis of the carrier) gyroscope adopts two MEMS gyroscopes to improve the accuracy.

(2)MEMS gyro

The MEMS gyroscope chip is an outsourced part, as shown in Figure 2. After signal processing, the chip is packaged into a single-axis gyroscope.

3. Error analysis and compensation

3.1 Error sources

Inertial devices are small in size, low in cost, light in weight, but low in precision, so the main error of this inertial combination comes from the random drift error of the MEMS gyroscope.

3.2 Random error compensation

3.2.1 Modeling based on time series methods

The content of time series analysis and modeling includes: data collection, statistical analysis and preprocessing of data, determination of model order, estimation of model parameters, model applicability test and other issues.

(1) Data collection

Samples the MEMS gyro output y-axis output signal. Figure 3 is the original drift and random drift signal of the MEMS gyroscope

(2) Data preprocessing

Before establishing a model for the MEMS gyroscope random drift signal, data preprocessing is required to make it a zero-mean, stable, normal time series signal, and then a mathematical model can be established for the processed signal. Signal preprocessing includes:

1) Elimination of outliers: The so-called outliers refer to a small amount of abnormal data caused by the sudden abnormality of experimental conditions or test instruments, and the negligence of observers. They occur as observations that are far from the majority and must therefore be eliminated.

2) Zero mean value processing: Calculate the mean value of the finite time series, and after calculating the mean value, subtract the mean value from the gyroscope at each moment to obtain the data after zero mean value processing.

3) Extract trend items: Since the gyro random drift data sequence in actual measurement is often a non-stationary random sequence, it is sometimes necessary to remove one of the linear or slowly changing trends.

4) Differential processing: If the sequence after extracting the trend item is still a non-stationary column, it is necessary to perform differential processing on the data until a stationary time series is obtained.

5) Data inspection: data inspection includes stationarity inspection, normality inspection, and zero-mean inspection. The stationarity test is used to test whether the drift data sequence has statistical characteristics that do not change over time, the normality test is used to judge whether the gyro random drift data has the characteristics of a normal distribution, and the zero-mean test is to test whether the mean value of the time series is Zero, the method of data inspection is very mature.

(4) Model identification of gyro random drift

In practical engineering applications, time series models can generally be divided into autoregressive (AR) models, moving average (MA) models, and autoregressive moving average (ARMA) models. Because the order of the gyro drift model is relatively low, generally no more than 2 to 3 orders, and for the actual system, the autoregressive order of the random ARMA model is greater than or equal to the moving average order, so the error model is generally in the range of AR(1), AR (2), AR(3), ARMA(1,1) and ARMA(2,1). After inspection, the sequence after data preprocessing has reached the requirements of being stable, normal, and zero mean. Figure 4 is the preprocessed signal of the Y-axis output data of the MEMS gyroscope.

According to the preprocessed data, different AR or ARMA models are established, and then the AIC values of different models are compared, and the most ideal random drift error model is selected from them. The results are shown in Table 1.

AR(1) AR(2) AR(3) ARMA(1,1) ARMA(2,1) a1 0.4150 0.5412 0.6114 -0.1531 -0.1515 a2 0 0.3016 0.4270 0 0.0141 a3 0 0 0.2303 0 0 b1 0 0 0 -0.9418 -0.9472 AIC -7.8186 -7.9151 -7.9702 -8.1547 -8.1551

Table 1 MEMS gyroscope random drift model parameters and AIC value

It can be seen from Table 1 that the model with the smallest AIC value is the ARMA (2, 1) model, so the ARMA (2, 1) model is selected as the mathematical model of the random drift of the MEMS gyroscope. The mathematical expression of the model is:

x _k ＝0.1515x _k-1 -0.0141x _k-2 +e _k -0.9472e _k-1 (1)

Where e _t (t=1,...n,n+1,...) is mutually independent white noise, x _k (k=1,...n,n+1,..) is a state variable .

3.2.2 State space implementation of time series mathematical model

Because UKF is to be used to realize one-step prediction of MEMS gyro random drift, the number of system variables in the state space is set to 2, and the time series is transformed into a state space model here.

Let this time series model be:

The state space model is:

X _k ＝AX _k-1 +BW _k (3)

Z _k ＝HX _k +V _k (4)

Here, X _k =[X _k X _k-1 ] ^T , W _k =e _k (system noise variable), V _k is a scalar (measurement noise), Z _k is a scalar (measurement value), where

This state-space model directly brings the autoregressive moving average parameters of the time series into the state space without any intermediate operations and the model satisfies observability (the system can output the predicted random drift if the system satisfies the observability, which is one step in practice. The predicted random drift is necessary for real-time compensation).

The state space model obtained by bringing the parameters in (1) into this state space model is as follows:

$\left[\begin{array}{c}{x}_k\\ x_{K-1}\end{array}\right]=\left[\begin{array}{cc}0.1515& 1\\ -0.0141& 0\end{array}\right]\left[\begin{array}{c}{x}_{k-1}\\ x_{k-2}\end{array}\right]+\left[\begin{array}{c}1\\ -0.9472\end{array}\right]W_k$ (6)

${\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\\ {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}$

Among them, V _k (k=1, 2, n, . . . ) is the measurement noise, which can be set according to the actual situation, and W _k is the process noise, which can be approximated by calculating the sample standard deviation of x _k .

3.2.3UKF filter method for error compensation

(1) UKF filter equation

Consider the following nonlinear discrete system

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Gamma;</span>}{\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

Among them, x _i is the n-dimensional state vector, z _i is the measurement vector, u _i is the known external input; w _i is the zero-mean system process noise, which is Gaussian white noise with variance R; v _i is the zero-mean quantity The measurement noise is Gaussian white noise with variance Q. The filtering process is as follows:

1) Initialization

则：The state vector x is an n-dimensional random variable whose mean is

but:

(8)

(8)

2) Sigma point sampling

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; =</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}$

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>\sqrt{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&lambda;</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; )</span>}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\sqrt{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&lambda;</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; P</span>}}<span\; class="notranslate">\; )</span>}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span>\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; no</span>}$

Corresponding weights:

周围Sigma点分布状态；k是一个标量，用于控制每个点到均值的距离；调节参数β可以提高方差的精度，

是矩阵

的第i列。Where λ=α ² (n+k)-n is a scaling factor, α determines

The distribution state of the surrounding Sigma points; k is a scalar used to control the distance from each point to the mean; adjusting the parameter β can improve the accuracy of the variance,

is the matrix

The ith column of .

3) Time update

Each Sigma point is nonlinearly transformed by the state equation:

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

A one-step predictor of the state:

${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Right\; Arrow;</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 12</span>}<span\; class="notranslate">\; )</span>$

The one-step prediction equation for the state:

${\mathrm{<span\; class="notranslate">\; p</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Right\; Arrow;</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; l</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; )</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$

Each Sigma point is nonlinearly transformed by the observation equation:

z _{i, k|k-1} = h(x _{i, k|k-1} ) (14)

The predicted output value of the system:

${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Right\; Arrow;</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$

4) Measurement update

Compute the theoretical variance matrix of the system output:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Right\; Arrow;</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; l</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; )</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 16</span>}<span\; class="notranslate">\; )</span>$

Compute the covariance:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Right\; Arrow;</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}^{\mathrm{<span\; class="notranslate">\; l</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; )</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; z</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 17</span>}<span\; class="notranslate">\; )</span>$

Compute the filter gain matrix:

$\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 18</span>}<span\; class="notranslate">\; )</span>$

Filtered value after state update:

${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 19</span>}<span\; class="notranslate">\; )</span>$

Solve the state posterior variance matrix:

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}{\mathrm{<span\; class="notranslate">\; P</span>}}_{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 20</span>}<span\; class="notranslate">\; )</span>$

3.2.4 MEMS gyroscope static random error compensation

The compensation process of the static random error is shown in Figure 5, and the compensation result is shown in Figure 6. The red color is the noise before compensation, and the blue color is the data after compensation. Obviously, the standard deviation of the compensated data is much smaller than that before compensation. The compensation effect of UKF on static random noise is very good.

3.2.5 MEMS gyroscope dynamic random error compensation

The process of dynamic random error compensation is shown in Figure 7. It is different from the static noise compensation method in that the data is differentiated, and the data before the difference and the data after K times filtering are subtracted to reduce the noise. The principle is: in the system In the data generation, it is considered that the data at K time and K-1 time should be equal. If they are not equal, it is considered an error (this error is considered to be noise). After the error is filtered and corrected, the data before the difference is subtracted by K times Filter the data to remove noise.

1) Rotate the gyroscope at an angular velocity of 5°/s, multiply the input data by the proportional coefficient, and perform the dynamic noise compensation method shown in Figure 7, and adjust the appropriate k value as shown in Figure 8, and Figure 8 shows that the constant speed The noise compensation effect of the rotating MEMS gyroscope is relatively good.

2) Considering an extreme situation, take the gyroscope at the speed of 10°, 20°, 30°, 50°, 75°, 90°, 100°/s respectively, and perform the output data as shown in Figure 7 The processing shown in Fig. 9 after noise compensation, and Fig. 10 is a partially enlarged view. It can be seen from Figures 9 and 10 that the error is greatly reduced after compensation.

In summary, the accuracy of MEMS gyroscope random error compensation meets the requirements.

In short, the high-performance miniature inertial instrument scheme and UKF filter algorithm adopted by the IMU for the rotating carrier can realize the accurate measurement of the flight attitude, position and velocity of the rotating projectile.

Claims (2)

1. the inertia combination (IMU) used of rotating carrier is accurately measured flight attitude, position and the speed in the body flight course, may further comprise the steps:

The first step, in the situation of carrier High Rotation Speed, design high-performance microminiature inertial instrument scheme both four MEMS gyros (wherein main shaft is two) and three high dynamic accelerations is taken into account temperature sensor output signal, signal conditioning circuit is partially zero to signal, coupling is nursed one's health, and signal amplified etc.

Second step arranges sample frequency, adopts 16 A/D that the signal of nursing one's health is gathered.

The 3rd step, utilize main control computer (ARM) that signal is processed, comprise that design UKF algorithm is to the compensation (comprising temperature compensation) of MEMS Gyro random error, and the error of the accelerometer measures that causes of compensation temperature etc., the data after the processing are Live Flying attitude, position and the speed of rotating missile.

The 4th step, Live Flying attitude, position and the speed of output rotating missile.

2. as follows according to its special character of step of flight attitude, position and speed in claims 1 described inertia combination (IMU) measurement body flight course:

(1) step 1 proposes high-performance microminiature inertial instrument scheme, this conceptual design the quartzy digital accelerometer of three groups of MEMS quadrature gyros, three quadratures, wherein main shaft (turning axle of corresponding carrier) adopts two MEMS gyros to improve precision, and it also has structural body, secondary power supply, signal processing and communication interface etc. partly to form in addition.

(2) step 3 utilizes main control computer (ARM) that signal is processed, and its special character is to design the UKF Kalman Algorithm Random Drift Error of MEMS gyro is compensated.

Images