CN105021197A - ILOPF-based quadrotor attitude estimation method - Google Patents

Abstract

The invention provides an ILOPF-based quadrotor attitude estimation method. The method comprises the following steps: 1, initializing a particle set, and randomly generating N sampling points according to the value domain of a state to be estimated in order to form a particle set; 2, updating the particle set by using an EKF technology, adopting the obtained approximate posteriori density as an importance density function, and generating a new particle set; 3, carrying out weight updating on the new particle set generated in step 2; 4, dividing the weight updated particle set into a replication group and a discarding group according to a distinguishing threshold, and respectively normalizing weights; and 5, re-sampling the weight normalized replication group by using a roulette technology to obtain the estimation result. The method improves the diversity of particles, meets a real-time requirement, improves the algorithm efficiency, and can effectively inhibit disturbance through rapid filtering treatment.

Description

A kind of four rotor Attitude estimation methods based on ILOPF

Technical field

The invention belongs to the field of measuring technique of quadrotor attitude angular rate, be specifically related to a kind of four rotor Attitude estimation methods based on ILOPF.

Background technology

Quadrotor is typical multiple coupled, Nonlinear Underactuated System, and in recent years, because its flying condition is low, physics such as easily to realize at the feature, becomes the new focus of control field research.Multinational research institution makes for it carries out mathematical modeling, Controller gain variations and flying article.In order to realize effective control of aircraft, need the flight attitude of accurate description aircraft.Acceleration in dynamic process owing to being subject to the impact of dither, the attitude angle in space cannot be calculated separately, therefore, the attitude information of quadrotor is obtained via information fusion algorithm by 3-axis acceleration and tri-axis angular rate, gyroscope in measuring process due to be disturbed and self measurement noise impact, angular rate data can produce high-frequency fluctuation, and directly using such data to carry out merging the attitude information obtained cannot be used for control system.Conventional anti-interference method has Kalman filtering algorithm, hypercomplex number integration technology and other mechanical resistance to shake measure, wherein Kalman filter or complementary filter can not must be estimated the nonlinear element of quadrotor very well, Quaternion Algorithm can solve in solving of attitude the problem occurring singular point, but can not must resist noise very well, mechanical measure design difficulty is large, realizes cost high.

Summary of the invention

One of the object of the invention is that providing a kind of implements the four rotor Attitude estimation methods based on ILOPF simple, filtration efficiency is high, real-time is high, precision is high.

A kind of four rotor Attitude estimation methods based on ILOPF provided by the invention, comprise the steps:

Step S1: initialization particle collection, the codomain scope of the state estimated as required, the N number of sampled point of stochastic generation, constituent particle collection;

Step S2: utilize EKF method to upgrade particle collection, using the approximate posterior density that obtains as the importance density function, finally produces new particle collection;

Step S3: right value update is carried out to the new particle collection produced in described step S2;

Step S4: the particle collection after right value update be divided into copy group according to distinguishing threshold value and abandon group, and difference normalization weights;

Step S5: adopt roulette method to carry out resampling to the copy group after normalization weights, and draw this result estimated.

Further, described four rotor Attitude estimation methods also comprise step S6: the sampled point adopting roulette method to get the copy group in described step S4 respectively and abandon in group, utilize following formula to form a new sampled point,

X _n=x _α+ randn (x _α-x _s), wherein randn is random number;

Circulation N time, produces the particle collection of N number of sampled point as estimated service life next time.

Beneficial effect of the present invention is, the invention solves the problems such as sample degeneracy that the comparatively large and high-frequency resampling strategy of importance weight deviation in elementary particle filtering algorithm brings and diversity scarcity; By introducing the linear optimization strategy simplified, improve the diversity of particle, meeting requirement of real-time, improve efficiency of algorithm, by filtering process fast, can effective disturbance suppression.

Accompanying drawing explanation

Figure 1 shows that the four rotor Attitude estimation method flow diagrams that the present invention is based on ILOPF.

Embodiment

Hereafter will describe the present invention in detail in conjunction with specific embodiments.It should be noted that the combination of technical characteristic or the technical characteristic described in following embodiment should not be considered to isolated, they can mutually be combined thus be reached better technique effect.

As shown in Figure 1, one provided by the invention comprises the steps: based on four rotor Attitude estimation methods of improvement linear optimization particle filter algorithm (Improved LinearOptimization ofParticleFilter is called for short ILOPF)

Step S1: initialization particle collection, the codomain scope of the state estimated as required, the N number of sampled point of stochastic generation, constituent particle collection.

Step S2: utilize EKF (Extended Kalman Filter, be called for short EKF) method to upgrade particle collection, using the approximate posterior density that obtains as the importance density function, finally produces new particle collection.

EKF method is similar to posterior probability according to formula (1) in each moment,

$p\left(x_k\right|z_k)\≈N({\hat{x}}_k,\hat{P})\text{---}\left(1\right)$

Wherein for the state estimation in k moment; for the estimation variance in k moment.

By EKF method, particle collection is upgraded, using the approximate posterior density that finally obtains as the importance density function, namely produce new particle collection thus.

Step S3: right value update is carried out to the new particle collection produced in described step S2.

Step S4: the particle collection after right value update be divided into copy group according to distinguishing threshold value and abandon group, and difference normalization weights.

When certain sampled point of needs repeated acquisition, produce a new sampled point by sampled point and abandoned sampled point are carried out suitable linear combination, the mode of linear combination is:

x _n＝x _α+L(x _α-x _s) (2)

Wherein, x _nthe new sampled point produced by array mode, x _αfor being repeated the sampled point of selection; x _sabandoned sampled point, L=(Nw) ^-1/m, N is particle number, and m is sample space dimension, and w is the distribution probability of sampled point in any sampled point neighborhood space, and dividing particle is abandon group and the threshold value of copy group is:

${\mathrm{\ω}}_{Thr}=\frac{max\left({\left\{{\mathrm{\ω}}_k^i\right\}}_{i=1}^N\right)}{K}---\left(3\right)$

With reference to above-mentioned linear optimization process, propose a kind of improvement array mode increasing degree of randomness, main thought is: in a certain group of estimation is carried out, according to distinguishing threshold value ω _thrparticle collection be divided into copy group and abandon group, and difference normalization weights.

Step S5: adopt roulette method to carry out resampling to the copy group after normalization weights, and draw this result estimated;

Described four rotor Attitude estimation methods also comprise step S6: the sampled point adopting roulette method to get the copy group in described step S4 respectively and abandon in group, utilize following formula to form a new sampled point,

x _n＝x _α+randn(x _α-x _s) (4)

Wherein randn is random number; Circulation like this N time, produces the particle collection of N number of sampled point as estimated service life next time, then goes to step S2 and reappraise.

Particle filter is after the resampling process of classics, and the diversity of particle reduces, and candidate's particle is divided into and abandons group and copy group by linear optimization method, after resampling process completes, the particle abandoning group and copy group is carried out linear combination, forms new particle collection, enter and estimate next time.The advantage of this method is, the particle after using new particle to replace single resampling, reduces the repetition rate of particle set particle, and the distribution of particles produced in this way is closer to real Posterior probability distribution.The improvement linear optimization particle filter algorithm that the present invention proposes has carried out suitable adjustment on above-mentioned idea basis, for the attitude angular rate estimation problem of quadrotor, to reduce algorithm complex and to improve for the purpose of real-time.

The invention solves the problems such as sample degeneracy that the comparatively large and high-frequency resampling strategy of importance weight deviation in elementary particle filtering algorithm brings and diversity scarcity; By introducing the linear optimization strategy simplified, improve the diversity of particle, meeting requirement of real-time, improve efficiency of algorithm, by filtering process fast, can effective disturbance suppression.

Although give some embodiments of the present invention, it will be understood by those of skill in the art that without departing from the spirit of the invention herein, can change embodiment herein.Above-described embodiment is exemplary, should using embodiment herein as the restriction of interest field of the present invention.

Claims (2)

1., based on four rotor Attitude estimation methods of ILOPF, it is characterized in that, comprise the steps:

Step S1: initialization particle collection: the codomain scope of the state estimated as required, the N number of sampled point of stochastic generation, constituent particle collection;

Step S2: utilize EKF method to upgrade particle collection, using the approximate posterior density that obtains as the importance density function, finally produces new particle collection;

Step S3: right value update is carried out to the new particle collection produced in described step S2;

Step S4: the particle collection after right value update be divided into copy group according to distinguishing threshold value and abandon group, and difference normalization weights;

Step S5: adopt roulette method to carry out resampling to the copy group after normalization weights, and draw this result estimated.

2. a kind of four rotor Attitude estimation methods based on ILOPF as claimed in claim 1, it is characterized in that, described four rotor Attitude estimation methods also comprise step S6: adopt the sampled point that roulette method is got the copy group in described step S4 respectively and abandoned in group, following formula is utilized to form a new sampled point

X _n=x _α+ randn (x _α-x _s), wherein randn is random number;

Circulation N time, produces the particle collection of N number of sampled point as estimated service life next time.