CN105043348A - Accelerometer gyroscope horizontal angle measurement method based on Kalman filtering - Google Patents

Abstract

Belonging to the technical field of measurement of dynamic equipment systems, the invention relates to an accelerometer gyroscope horizontal angle measurement method based on Kalman filtering so as to solve the problem of poor precision in accelerometer gyroscope horizontal angle measurement under a dynamic measurement environment. The method includes: 1. utilizing an accelerometer and a gyroscope to collect data about X axis so as to obtain the acceleration and angular rate; 2. subjecting the obtained acceleration and angular rate to digital filtering so as to filter out high-frequency interference generated in the transmission process; 3. converting the filtered acceleration into angular speed; 4. according to the conversion angle, introducing a temperature compensation factor to obtain a compensated angle; and 5. subjecting the compensated angle and filtered angular rate to Kalman filtering, and performing data fusion so as to obtain a horizontal angle optimal estimation value for correction in a control system.

Description

Based on the accelerometer gyroscope level angle measuring method of Kalman filtering

Technical field

The present invention relates to the level inclination measuring method based on Kalman filtering, belong to the field of measuring technique of dynamic device system.

Background technology

In the controls, horizontal angle measure is extremely important measuring-signal, usually used as the feedback signal of control system, for system corrects, therefore its precision and stability of crucial importance.Level angle measuring sensor common mostly at present is obliquity sensor, and conventional obliquity measurement sensor is divided into solid type, liquid-type and gas pendulum three major types, and they are all developed based on Newton second law.Measure under quiescent conditions, they all can obtain higher precision.But work as carrier in the dynamic case, because the interference of acceleration can add component in the sensitive axes of sensor, thus cause the error in measurement, the measuring accuracy of system is reduced, and then certain impact can be produced on the control of system.The obliquity sensor of gravity swing type cannot accurately be measured in the dynamic case, and in principle, is applicable to the angular velocity of dynamic measurement system with the gyroscope utilizing gyroscopic inertia principle to make.When static state, accelerometer is directly measured object acceleration of gravity and the formula scales of inverse trigonometric function can be used to become the angle of inclination of system, but it is larger to be interfered in the dynamic case.And gyrostatic output valve be its party upwards relative to the angular speed of sensitive axis, utilize the integration of angular speed to the time to obtain its angle value, dynamic is good, but can passing accumulation drift error in time.

Summary of the invention

The object of the invention is the problem measuring level angle low precision under kinetic measurement environment in order to solve accelerometer gyroscope, the invention provides a kind of accelerometer gyroscope level angle measuring method based on Kalman filtering.

Accelerometer gyroscope level angle measuring method based on Kalman filtering of the present invention,

Described method comprises the steps:

Step one: utilize accelerometer and gyroscope collection about the data of X-axis, obtain acceleration and angular speed;

Step 2: digital filtering is carried out, the high frequency interference that filtering produces in transmitting procedure to the acceleration obtained and angular speed;

Step 3: convert filtered acceleration to angle;

Step 4: according to the angle of conversion, substitutes into temperature compensation factor, the angle after being compensated;

Step 5: the angle after compensation and filtered angular speed are carried out Kalman filtering, and carries out data fusion, obtains the optimal estimation value of level angle.

The process of described step 5 comprises:

Step May Day: the initial value of parameters:

Matrix $A=\left[\begin{array}{cc}1& -dt\\ 0& 1\end{array}\right],$ T represents current time, and dt represents integration, matrix $B=\left[\begin{array}{c}dt\\ 0\end{array}\right],$ The covariance matrix of process noise $Q=\left[\begin{array}{cc}{q}_a& 0\\ 0& q_g\end{array}\right],$ Q _arepresent the measurement covariance of accelerometer, q _grepresent gyrostatic measurement covariance, R=[r _a], r _arepresent the covariance of measuring error, the covariance initial value of level angle estimated value $P(0/0)=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right],$ Level angle estimates initial value $X(0/0)=\left[\begin{array}{c}0\\ 0\end{array}\right];$

Step 5 two: prediction level angle estimation value covariance P (t|t-1):

P(t|t-1)＝AP(t-1|t-1)A ^T+Q；

The covariance that P (t|t-1) is X (t|t-1);

Step 5 three: according to P (t|t-1), calculates kalman gain Kg (t):

Kg(t)＝P(t|t-1)H ^T/[HP(t|t-1)H ^T+R]；

Observing matrix H=[10];

Step the May 4th: prediction level angle estimation value X (t|t-1):

X(t|t-1)＝AX(t-1|t-1)+BU(t)；

X (t|t-1) represents the level angle estimated value according to the prediction t in t-1 moment, and U (t) represents the controlled quentity controlled variable of t;

Step 5 five: according to angular rate measurement value Z (t) of current time, in conjunction with X (t|t-1) and kg (t), obtains the optimal estimation value X (t|t) of level angle:

X(t|t)＝X(t|t-1)+Kg(t)[Z(t)-HX(t|t-1)]；

Step 5 six: according to X (t|t), obtains covariance P (t|t):

P(t|t)＝[I _n-Kg(t)H]P(t|t-1)；

I _nrepresent n rank unit matrix, namely diagonal entry is the matrix of 1 entirely;

Proceed to step 5 two, the covariance of prediction subsequent time.

Beneficial effect of the present invention is, in the present invention, first slip weighted filtering is carried out to the data collected from accelerometer and gyroscope, the high frequency interference produced in filtered signal transmitting procedure, again the angle processed and angle rate signal are resolved carrying out angle, adopt Kalman's Data fusion technique, the measurement data of degree of will speed up meter and gyro carries out data fusion.Finally, the optimal estimation value the present invention obtaining real-time level angle adopts the change of the angular speed precognition angle of gyroscope survey, carries out the measurement dynamically.Come thus to have complementary advantages to dynamic inclination measurement, ensure that measuring accuracy.

Accompanying drawing explanation

Fig. 1 is the schematic flow sheet of the described accelerometer gyroscope level angle measuring method based on Kalman filtering of embodiment one.

Embodiment

Embodiment one: composition graphs 1 illustrates present embodiment, the accelerometer gyroscope level angle measuring method based on Kalman filtering described in present embodiment, described method comprises the steps:

Step one: utilize accelerometer and gyroscope collection about the data of X-axis, obtain acceleration and angular speed;

Step 2: digital filtering is carried out, the high frequency interference that filtering produces in transmitting procedure to the acceleration obtained and angular speed;

Step 3: convert filtered acceleration to angle;

Step 4: according to the angle of conversion, substitutes into temperature compensation factor, the angle after being compensated;

Step 5: the angle after compensation and filtered angular speed are carried out Kalman filtering, and carries out data fusion, obtains the optimal estimation value of level angle.

The process of described step 5 comprises:

Step May Day: the initial value of parameters:

Matrix $A=\left[\begin{array}{cc}1& -dt\\ 0& 1\end{array}\right],$ T represents current time, and dt represents integration, matrix $B=\left[\begin{array}{c}dt\\ 0\end{array}\right],$ The covariance matrix of process noise $Q=\left[\begin{array}{cc}{q}_a& 0\\ 0& q_g\end{array}\right],$ Q _arepresent the measurement covariance of accelerometer, q _grepresent gyrostatic measurement covariance, R=[r _a], r _arepresent the covariance of measuring error, the covariance initial value of level angle estimated value $P(0/0)=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right],$ Level angle estimates initial value $X(0/0)=\left[\begin{array}{c}0\\ 0\end{array}\right];$

Step 5 two: prediction level angle estimation value covariance P (t|t-1):

P(t|t-1)＝AP(t-1|t-1)A ^T+Q；

The covariance that P (t|t-1) is X (t|t-1);

Step 5 three: according to P (t|t-1), calculates kalman gain Kg (t):

Kg(t)＝P(t|t-1)H ^T/[HP(t|t-1)H ^T+R]；

Observing matrix H=[10];

Step the May 4th: prediction level angle estimation value X (t|t-1):

X(t|t-1)＝AX(t-1|t-1)+BU(t)；

X (t|t-1) represents the level angle estimated value according to the prediction t in t-1 moment, and U (t) represents the controlled quentity controlled variable of t;

Step 5 five: according to angular rate measurement value Z (t) of current time, in conjunction with X (t|t-1) and Kg (t), obtains the optimal estimation value X (t|t) of level angle:

X(t|t)＝X(t|t-1)+Kg(t)[Z(t)-HX(t|t-1)]；

Step 5 six: according to X (t|t), obtains covariance P (t|t):

P(t|t)＝[I _n-Kg(t)H]P(t|t-1)；

I _nrepresent n rank unit matrix, namely diagonal entry is the matrix of 1 entirely;

Proceed to step 5 two, the covariance of prediction subsequent time.

In present embodiment, based on the level angle measuring system of Kalman filtering using STM32F103C8 as controller, based on its minimum system, comprise reset circuit, crystal oscillating circuit, power circuit, LM1117 line style source of stable pressure, TTL turns USB downloader and inside embeds accelerometer and gyrostatic MPU6050.

When control system works, in digital signal input process, that comes with measurement data also has various Noise and Interference, in order to ensure the precision of measuring system, needing to eliminate these interference and noises, in present embodiment, adopting slip weighted filtering.In sampling, if the proportion of n the numerical value obtained of sampling is identical, for time varying signal, after introducing some, and n value is larger, seriously delayed.Therefore, in order to suitable minimizing hysteresis phenomenon, weights can be added in the process calculated, so increase the sampled value that newly obtains for filter value impact shared by proportion.More close to now, corresponding weights are also larger.The controlled device large for lag time constant and sampling period short system, use slip weighted filtering algorithm better can suppress the noise of system.

$Y=\underset{k=1}{\overset{n}{\Σ}}{C}_{k}X\left(k\right)$

Wherein, C meets:

$\left\{\begin{array}{c}0<C_1<C_2<...C_n\\ C_1+C_2+...C_n=1\end{array}\right.$

Also be have various method to choosing of constant C.Conventional method has weighting factor method, utilizes the pure delay time of object to select C value.

The mathematical model expression formula of slip weighted filtering is as follows:

Y _k＝(B _kX _k+B _k-1X _k-1+B _k-2X _k-2+…+B ₀X ₀)/A

Wherein and B ₀< B ₁< B ₂< ... < B _k.The method to set up of constant term B is with conventional weighting factor method, supposes that τ is object pure delay time, then now

δ＝1+e ^-τ+e ^-2τ+…+e ^-(n-1)τ

Wherein, $B_0=\frac{1}{\mathrm{\&delta;}},B_1=\frac{e^{-\mathrm{\&tau;}}}{\mathrm{\&delta;}},\mathrm{..},B_{n-1}=\frac{e^{-(n-1)\mathrm{\&tau;}}}{\mathrm{\&delta;}}.$

It can thus be appreciated that, when τ is larger, δ is less, the weight coefficient of new sampled value is also larger, and the weight coefficient of historical data is less, so, slip weighted filtering is used in this system had compared with large time constant, the proportion shared by value that its new instance sample obtains is larger, effectively can suppress the hysteresis phenomenon on measuring.

For a dynamic inclination measuring system, if only use accelerometer to measure carrier inclination angle, then can bring very large deviation, because the acceleration of the static state that accelerometer not only can be experienced, can also feel to the dynamic acceleration suffered by carrier, only need during measuring angle to take away under static state, the acceleration therefore dynamically caused has become a kind of interference, needs to compensate.Select collocation angular rate gyroscope to be because it can predict the change of angle, carry out the measurement dynamically.Come thus to have complementary advantages to dynamic inclination measurement.In the present invention, adopt Kalman filtering method to carry out data fusion to accelerometer and gyrostatic measurement data, obtain the optimal estimation value of level angle.

Here sets up the state space equation group of this system.Consider that the angular speed at inclination angle and inclination angle exists the relation of derivative, so can be set it as a state variable.Another one shape body variables choice is gyrostatic constant value deviation b.

Obtaining state equation is:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\\ \stackrel{\&CenterDot;}{b}\end{array}\right]=\left[\begin{array}{cc}0& -1\\ 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\&theta;}\\ b\end{array}\right]+\left[\begin{array}{c}1\\ 0\end{array}\right]w_{gyro}+\left[\begin{array}{c}{w}_g\\ 0\end{array}\right]$

W in formula _gfor the noise error of gyro, w _gyrorepresent the output angle speed of gyro, this state equation is to obtain angular velocity to set up to angle differentiate.

Next be observation equation:

W in formula _abe expressed as the noise that accelerometer measures produces, then represent that accelerometer converts the angle obtained.

System is represented discretize, gyro angular velocity integral formula in the discrete case and the observation equation of accelerometer can be obtained thus:

θ(t)＝θ(t-1)+[w _gyro(t-1)-b+w _g(t)]dt

The measurement data of degree of will speed up meter and gyro connects thus, utilizes accelerometer to revise inclination angle as observed quantity.So can obtain:

By above formula passable to, to try to achieve the inclination angle of t, just must estimate according to the inclination angle in t-1 moment, Kalman filter carries out recursive operation thus, estimates optimal corner angle value.Above formula is compared with Kalman's space equation, so can obtain:

$A=\left[\begin{array}{cc}1& -dt\\ 0& 1\end{array}\right],B=\left[\begin{array}{c}dt\\ 0\end{array}\right],$ W _gyroas the input signal of state space, observing matrix H=[10], wherein unit matrix $I=\left[\begin{array}{c}1\\ 1\end{array}\right].$

Also remaining P, Q, R battle array needs to confirm in addition.Q battle array is the covariance of systematic procedure noise, and R battle array is the error covariance of systematic survey.

$Q=\left[\begin{array}{cc}{q}_a& 0\\ 0& q_g\end{array}\right],$ R=[r _a], wherein q _aand q _grepresent the measurement covariance of accelerometer and gyro, it represents the trusting degree to sensing data, and numerical value is less, higher to the trusting degree of sensor.Set Q and R battle array according to the theory of engineering design and experience, to make after debugging to obtain the good filter effect of system.P battle array is defined as the covariance matrix of state variable error.Known according to definition, if know definite initial position, just can provide $P(0/0)=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right];$ If do not know its initial position, then can be initialized to the matrix of a diagonal form, and diagonal line be got a suitable larger number.

In this process, Kalman filter can be found out in the process of recursion according to minimum covariance, estimate optimum inclination value.Only need the optimal value at a moment, system deviation, prediction deviation, just can carry out the calculating advancing capable optimal value this moment, in computing, very large burden can not be caused to system.

Claims (2)

1., based on the accelerometer gyroscope level angle measuring method of Kalman filtering, it is characterized in that, described method comprises the steps:

Step one: utilize accelerometer and gyroscope collection about the data of X-axis, obtain acceleration and angular speed;

Step 2: digital filtering is carried out, the high frequency interference that filtering produces in transmitting procedure to the acceleration obtained and angular speed;

Step 3: convert filtered acceleration to angle;

Step 4: according to the angle of conversion, substitutes into temperature compensation factor, the angle after being compensated;

Step 5: the angle after compensation and filtered angular speed are carried out Kalman filtering, and carries out data fusion, obtains the optimal estimation value of level angle.

2. the accelerometer gyroscope level angle measuring method based on Kalman filtering according to claim 1, it is characterized in that, the process of described step 5 comprises:

Step May Day: the initial value of parameters:

Matrix $A=\left[\begin{array}{cc}1& -dt\\ 0& 1\end{array}\right],$ T represents current time, and dt represents integration, matrix $B=\left[\begin{array}{c}dt\\ 0\end{array}\right],$ The covariance matrix of process noise $Q=\left[\begin{array}{cc}{q}_a& 0\\ 0& q_g\end{array}\right],$ Q _arepresent the measurement covariance of accelerometer, q _grepresent gyrostatic measurement covariance, R=[r _a], r _arepresent the covariance of measuring error, the covariance initial value of level angle estimated value $P(0/0)=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right],$ Level angle estimates initial value $X(0/0)=\left[\begin{array}{c}0\\ 0\end{array}\right];$

Step 5 two: prediction level angle estimation value covariance P (t|t-1):

P(t|t-1)＝AP(t-1|t-1)A ^T+Q；

The covariance that P (t|t-1) is X (t|t-1);

Step 5 three: according to P (t|t-1), calculates kalman gain Kg (t):

Kg(t)＝P(t|t-1)H ^T/[HP(t|t-1)H ^T+R]；

Observing matrix H=[10];

Step the May 4th: prediction level angle estimation value X (t|t-1):

X(t|t-1)＝AX(t-1|t-1)+BU(t)；

X (t|t-1) represents the level angle estimated value according to the prediction t in t-1 moment, and U (t) represents the controlled quentity controlled variable of t;

Step 5 five: according to angular rate measurement value Z (t) of current time, in conjunction with X (t|t-1) and Kg (t), obtains the optimal estimation value X (t|t) of level angle:

X(t|t)＝X(t|t-1)+Kg(t)[Z(t)-HX(t|t-1)]；

Step 5 six: according to X (t|t), obtains covariance P (t|t):

P(t|t)＝[I _n-Kg(t)H]P(t|t-1)；

I _nrepresent n rank unit matrix, namely diagonal entry is the matrix of 1 entirely;

Proceed to step 5 two, the covariance of prediction subsequent time.