CN102288197B

CN102288197B - Low-cost denoising, null-shift preventing and distortion preventing method for gyroscope

Info

Publication number: CN102288197B
Application number: CN 201010612733
Authority: CN
Inventors: 涂超; 郭盖华; 李一鹏
Original assignee: Dongguan Robstep Robot Co ltd
Current assignee: Dongguan Robstep Robot Co ltd
Priority date: 2010-12-30
Filing date: 2010-12-30
Publication date: 2012-12-12
Anticipated expiration: 2030-12-30
Also published as: CN102288197A

Abstract

The invention discloses a low-cost denoising, null-shift preventing and distortion preventing method for a gyroscope, which comprises the following steps: step one, denoising an output signal of the gyroscope by using a low-pass filter; step two, correcting data outputted by the low-pass filter through a time window in real time when the gyroscope is at a null shift; and step three, inputting data outputted after the data is corrected by the time window in real time into a kalman filter and judging whether the gyroscope is distorted or not through filter residual of the kalman filter. Through matching of the steps in the low-cost denoising, null-shift preventing and distortion preventing method for the gyroscope disclosed by the invention, noise reduction can be realized, drifting is lowered and the problem of fusion distortion is solved.

Description

The method of the anti-drift anti-distortion of a kind of low-cost gyroscope denoising

Technical field

Patent of the present invention relates to the method for the anti-drift anti-distortion of a kind of low-cost gyroscope denoising.

Background technology

Gyroscope is a kind of important inertial sensor spare; Be used for responsive relative inertness Space Angle motion is measured; It is the core component of systems such as navigation, guidance, stable, aiming; Be widely used in fields such as strategy and tactics weapon, spacecraft, aircraft, automotive safety, industrial automation and consumer electronics, the precision of gyro will directly have influence on the effect of system's attitude measurement.Receive the restriction of manufacturing process and precision level; Compare the inertial sensor that conventional fabrication processes is made; The random noise of gyroscope output data is bigger; Thereby before using gyroscope, must set up rational random noise model, gyrostatic error is estimated and compensation, to reduce its influence system accuracy.

See also Fig. 1 to Fig. 4; Gyrostatic output error, remove alignment error, calibration factor error etc. negligible outside, mainly contain three kinds: stochastic error, constant value drift, temperature effect; Visible by Fig. 1; The output from Gyroscope noise is very big, and wild value is more, and maximum open country value reaches positive and negative 3.5; Visible by Fig. 2, the gyro output data under temperature effect zero drift occurred near 30000 o'clock; Visible by Fig. 3, gyroscope constant value drift (curve b), (curve a) continues to increase angle behind the integration; Visible by Fig. 4, because the dynamic property of acceleration transducer is bad, when inertial measurement system quickened suddenly, acc sensing (curve c) can oppositely be fallen, thereby made also reverse direction skew of fusion angle (curve d), and is opposite with true angle, distortion occurs.

Can know by figure; Constant value drift is bigger to the contribution that gyro error produces; Its processing usually when intelligent apparatus such as robot or two-wheeled balance electric car start but also do not work, is averaged the gyrostatic drift data of gathering, when using, in observation data, it is deducted then; Secondly, be exactly by stochastic error and temperature effect.The anti-drift of common low-cost gyroscope denoising is to adopt time series to set up the method for error model, however this method face and have model error, noise is big when applying to complicated high maneuvering system, drift easily.

Summary of the invention

The defective that The present invention be directed to the existence of above-mentioned background technology provides the method for the anti-drift anti-distortion of a kind of low-cost gyroscope denoising, and this method can realize reducing noise, reduces drift, has solved the fusion problem of dtmf distortion DTMF.

For realizing above-mentioned purpose, the invention discloses the method for the anti-drift anti-distortion of a kind of low-cost gyroscope denoising, it comprises the steps:

Step 1 is passed through the low-pass filter denoising with gyrostatic output signal;

Step 2 is proofreaied and correct the gyro drift with the data of low-pass filter output through time window in real time;

Step 3, whether the output data input kalman wave filter after will proofreading and correct in real time through time window judges the gyroscope distortion through kalman filtering residual error.

Further, the current output valve of the said low-pass filter weights that equal a current real output value and a preceding output valve with.

Further, the weights of a said current real output value and a preceding output valve are 0.7 and 0.3 or 0.6 and 0.4.

Further, the filter times of said step 1 is 2～5 times.

Further, said step 2 adopts zero-speed to detect correcting algorithm in real time.

Further, said step 2 is averaged through the mode of time window for the data of low-pass filter being exported through the fifo queue algorithm, and with this mean value input kalman wave filter.

In sum, the cooperation of the method for drift anti-distortion through above-mentioned each step prevented in the low-cost gyroscope denoising of the present invention, thereby can realize reducing noise, reduces drift, solved the fusion problem of dtmf distortion DTMF.

Description of drawings

The data plot of Fig. 1 for exporting under the gyroscope static state.

Fig. 2 is the data plot of the gyroscope output under temperature effect.

Angle variation diagram when Fig. 3 is gyroscope static state behind constant value drift and the integration.

Output data figure when Fig. 4 is gyroscope and acceleration sensor fusion.

Fig. 5 is kalman filtering residual error and sensor fusion graph of a relation.

Fig. 6 takes fifo queue for the present invention, gets 15 points and carries out the curve map after algorithm improves for big or small time window.

Fig. 7 for the noise of gyroscope output at the front and back comparison diagram that uses denoising method of the present invention.

Fig. 8 for the dynamic property of gyroscope output at the front and back comparison diagram that uses denoising method of the present invention.

Fig. 9 is the front and back comparison diagram of gyroscopic drift curve in use denoising method of the present invention.

Figure 10 is the graph of relation of the angular velocity that uses fusion angle and angular-rate sensor behind the present invention.

Embodiment

For further understanding characteristic of the present invention, technological means and the specific purposes that reached, function, the present invention is described in further detail below in conjunction with accompanying drawing and embodiment.

See also Fig. 5 to Figure 10; The method of the anti-drift anti-distortion of a kind of low-cost gyroscope denoising of the present invention comprises the steps: step 1; Gyrostatic output signal is carried out 3 denoisings through low-pass filter; The filtering algorithm of said low-pass filter is: the weights that current output valve equals a current real output value and a preceding output valve with, wherein, the weights of a said current real output value and a preceding output valve are 0.7 and 0.3.

Step 2; The data of low-pass filter output are proofreaied and correct the gyro drift in real time through time window; In the present embodiment; It detects correcting algorithm in real time for adopting zero-speed, promptly when gyrostatic speed is zero, through fifo queue algorithm (FIFO algorithm) data of low-pass filter output is averaged through the mode of time window, and this mean value is imported the kalman wave filter as eigenwert.

Step 3, whether the output data input kalman wave filter after will proofreading and correct in real time through time window judges the gyroscope distortion through kalman filtering residual error.Owing to comprise progressive and random component in the original signal of gyroscopic drift, propose to obtain Kalman filtering residual error r behind the progressive item through fitting of a polynomial _k=Z _k-H _kX _{K, k-1}, Kalman filtering residual error is represented the additional information of the available new observation of wave filter, and the data that static characteristics is different with prior imformation will embody in residual vector.Because said formula: r _k=Z _k-H _kX _{K, k-1}By those skilled in the art are understood, repeat no more at this.

Residual error is represented the information of new observer, with self-adaptation most relevance is arranged, and can accurately, promptly embody the deviation size of observed quantity and estimated value.Therefore, based on fuzzy algorithm of residual error structure, used the measurement data of rejecting abnormalities, weighting adjustment estimated value makes the wave filter residual error keep zero-mean, reaches optimal estimation.Mould value according to the filtering residual error varies in size, and directly fuzzy adjustment filtering variance is strengthened the noise adaptation ability of filtering algorithm to target maneuver.

See also Fig. 5 to Figure 10, curve f is the curve of output of acceleration transducer among Fig. 5, and curve e is a kalman filtering residual values, and curve g is the sensor fusion angle, and by finding out among the figure, the filtering residual error can reflect that truly whether the sensor fusion angle.Fig. 6 takes fifo queue (fifo queue); Get 15 points and carry out the curve map after algorithm improves for big or small time window; Curve k is the output valve curve of acceleration transducer, and curve h is the curve of filtering residual error variance yields, and curve i is the curve of sensor fusion.Can find out significantly among Fig. 6 that before 3000 of the time shafts, sensor fusion is normal; Curve h is (near 0) steadily, time shaft 3500 points, 3800 points; About 4200, acceleration transducer is owing to thrashing, and output valve is undesired; Curve h detects very delicately, and real-time is very good, and its amplitude has shown the degree of sensor malfunction.

Fig. 7 has shown the noise of gyroscope output after using preceding (curve m) back (curve n) contrast of denoising method of the present invention, using denoising method of the present invention, and gyrostatic output data precision is doubled, and has eliminated wild value, and noise reduces nearly 50%.Among Fig. 8, curve p is a raw data, and curve q is for using the output data after the denoising method of the present invention, and when it showed significantly noise reduction, the gyroscope dynamic property was good.Curve s is the curve map after the drift compensation among Fig. 9, and curve r is the curve map before the drift compensation, can be known by figure and use denoising method drift of the present invention to reduce 57%.Figure 10 shows, uses denoising method of the present invention after the fusion angle when the acceleration transducer dynamic property is bad, undistorted fully.

The above embodiment has only expressed one embodiment of the present invention, and it describes comparatively concrete and detailed, but can not therefore be interpreted as limitation of the scope of the invention.Should be pointed out that for the person of ordinary skill of the art under the prerequisite that does not break away from the present invention's design, can also make some distortion and improvement, these all belong to protection scope of the present invention.Therefore, protection scope of the present invention should be as the criterion with accompanying claims.

Claims

1. the method for the anti-drift anti-distortion of a low-cost gyroscope denoising, it comprises the steps:

Step 1; Gyrostatic output signal is passed through the low-pass filter denoising; The filtering algorithm of said low-pass filter be the current output valve weights that equal a current real output value and a preceding output valve with, and the weights of a current real output value and a preceding output valve are 0.7 and 0.3 or 0.6 and 0.4;

Step 2; Adopt zero-speed to detect correcting algorithm in real time; When gyrostatic speed is zero, the data of low-pass filter output are averaged through the mode of time window, the data of low-pass filter output are proofreaied and correct the gyro drift in real time through time window through the fifo queue algorithm;

2. the method for the anti-drift anti-distortion of low-cost gyroscope denoising according to claim 1, it is characterized in that: the filter times of said step 1 is 2～5 times.