CN105890598A - Quadrotor posture resolving method combining conjugate gradient and extended Kalman filtering - Google Patents

Abstract

The invention relates to a quadrotor posture resolving method combining the conjugate gradient and extended Kalman filtering. The method comprises the steps that firstly, information of a sensor is collected to obtain the flying state of a quadrotor; then, an observation model is established with a conjugate gradient method, then a process model is established, and a quaternion obtained with the conjugate gradient method serves as a measurement value of extended Kalman filtering; finally, the optimal quaternion is obtained with an extended Kalman filtering method, and three posture angles of the quadrotor are solved. Compared with a simple gradient method and a complementary filtering method, system errors and measurement noise of the sensor are taken into consideration in the extended Kalman filtering method, and thus the estimated quaternion has higher accuracy. The observation quaternion obtained with the conjugate gradient method is applied to extended Kalman filtering, and thus a linear observation model complex in calculation can be avoided.

Description

The four rotor attitude algorithm methods that conjugate gradient is combined with EKF

Technical field

The present invention relates to a kind of four rotor wing unmanned aerial vehicle attitude course data measuring methods, particularly relate to a kind of conjugate gradient Be combined with spreading kalman to four rotor wing unmanned aerial vehicles (hereinafter referred to as four rotors) attitude algorithm method, be mainly used in military affairs and detect Examine, agricultural plant protection, electric inspection process, mapping, take photo by plane.

Background technology

Along with robot and the development of space flight and aviation technology, four rotors have been increasingly becoming Chinese scholars and have sent out with taking photo by plane Burn the study hotspot of friends.Four rotors are that a kind of many rotors with special flight performances such as VTOL and hoverings are unmanned Aircraft.In flight course, by changing the rotating speed of four rotors, thus it is possible to vary various flight attitudes.Just because of four rotors Unmanned plane has VTOL, aerial spot hover, can realize the flight performance that the flight etc. of six degree of freedom is unique so that it is Military field and civil area are widely used.

Attitude algorithm is the core of four rotors, and it is responsible for provides reliable Flight Condition Data in real time for four rotors Important task.Utilization at present is compared attitude algorithm method widely and is mainly complementary filter, Kalman filtering method and gradient descent method Method.Complementary filter method and gradient descent method calculate simple, make use of acceleration transducer and geomagnetic sensor to go to mend Repay and produce the error that drift causes due to gyro sensor in time, but all do not account for noise and the sensing of system itself The measurement error of device.Complementary filter method needs to choose dynamic weights and deacclimatizes different state of flights, major part fortune user Selecting to deacclimatize rapid flight to bigger weights, this will cause aircraft static properties not good enough.Kalman filtering Considering the measurement error of system and the measurement noise of sensor in method, comparing first two method has more preferable accuracy, and It is to use widest attitude algorithm method.General Kalman's method needs linearisation observation model, and linearisation is observed Model needs to face the problem solving Jacobin matrix, and this will bring the biggest amount of calculation.

Summary of the invention

The technical problem to be solved is, for the problem of four rotor attitude algorithms, it is provided that one can be real-time Obtain attitude and the higher attitude algorithm method of precision, thus preferably control the state of flight of four rotors.

For solving above-mentioned technical problem, the technical solution used in the present invention is:

A kind of four rotor attitude algorithm methods that conjugate gradient is combined with EKF, described four rotors comprise list Sheet machine and the gyroscope being connected with single-chip microcomputer, acceleration transducer, geomagnetic sensor, single-chip microcomputer leads to ground system host computer News connect；It is characterized in that comprising the steps:

Step S1: gather sensor information: under body axis system, is sensed by gyroscope, acceleration transducer, earth magnetism Device gathers four rotor flying status informations, and wherein, four rotor three axis angular rates measured by gyroscope, and four rotations measured by acceleration transducer The projection on three axles of the wing acceleration, geomagnetic sensor measures three axle magnetic induction of four rotor positions；Described body Coordinate system is to be fixed on the reference frame of four rotors；

Step S2: process of wafing of respectively gyroscope, accelerometer being zero-suppressed:

Step S3: use Butterworth LPF to filter body acceleration to acceleration of gravity projection on three axles Impact；Three axle magnetic induction are done zero normalization；

Step S4: utilize conjugate gradient method to set up EKF observation model；

Step S5: set up the process model of extended Kalman filter: that conjugate gradient method is obtained makes error function e_(q) Minimum quaternary number, as measured value, designs extended Kalman filter, utilizes extended Kalman filter to estimate optimum four Unit's number；

Step S6: simultaneous Euler orientation cosine matrix and Quaternion Matrix, obtains three attitudes utilizing quaternary number form to become Angle equation；Optimum quaternary number step S5 estimated is brought three attitude angle solution of equations into and is calculated three attitude angle of four rotors: Course angle γ, pitching angle theta, roll angle

In technique scheme, step S2 zero-suppress gyroscope, the accelerometer specific practice processed of wafing is by four rotations The wing keeps level, takes 200 sub-values and averages, and this meansigma methods is just zero to waft value, and the value of acquirement all deducts this and zero wafts value later.

In technique scheme, the three axle magnetic induction that geomagnetic sensor is recorded by step S3 do zero normalization Detailed process is as follows:

First, four rotor horizontal positioned of geomagnetic sensor will be installed, and the most constantly rotate four rotors and sense by earth magnetism Device rotates a ball, and in rotary course, slave computer constantly sends the data to host computer simultaneously；And then, host computer docks The data received process, and find out maximum and the minima of each single axle of x-axis, y-axis and z-axis respectively, finally make zero Normalizing calculates: zero, it is simply that the absolute value that will make the maximum of single axle and minima is equal, right on coordinate axes in other words Claim；Normalizing, it is simply that make the scope of data of two axles reach unanimity；Computing formula is as follows:

${\mathrm{Hx}}_i={\mathrm{Hsx}}_i-\frac{{\mathrm{Hsx}}_{max}+{\mathrm{Hsx}}_{\min }}{2}---\left(1\right)$

${\mathrm{Hy}}_i=\frac{{\mathrm{Hsx}}_{max}-{\mathrm{Hsx}}_{\min }}{{\mathrm{Hsy}}_{max}-{\mathrm{Hsy}}_{\min }}*({\mathrm{Hsy}}_i-\frac{{\mathrm{Hsy}}_{max}+{\mathrm{Hsy}}_{min}}{2})---\left(2\right)$

${\mathrm{Hz}}_i=\frac{{\mathrm{Hsx}}_{max}-{\mathrm{Hsx}}_{\min }}{{\mathrm{Hsz}}_{max}-{\mathrm{Hsz}}_{\min }}*({\mathrm{Hsz}}_i-\frac{{\mathrm{Hsz}}_{max}+{\mathrm{Hsz}}_{\min }}{2})---\left(3\right)$

In formula, x-axis is major axis, and y-axis is short axle；Hx_i、Hy_i、Hz_iFor each single axle of x-axis, y-axis and z-axis through zero normalizing Calibration value after process；Hsx_i、Hsy_i、Hsz_iBe illustrated respectively in x-axis, three axle magnetic induction that y-axis, z-axis collect； Hsx_max、Hsx_min、Hsy_max、Hsy_min、Hsz_max、Hsz_minBe illustrated respectively in x-axis, three axle magnetic induction that y-axis, z-axis collect strong The maximum of degree and minima.

In technique scheme, step S4 conjugate gradient method is set up to EKF observation model process such as Under: using perfect condition homogenous gravitational field and earth's magnetic field as reference direction, by quaternary number conversion under navigational coordinate system to body 3-axis acceleration [a' is obtained under coordinate system_bx a'_by a'_bz]^TWith magnetic induction [m'_bx m'_by m'_bz]^T；By be converted to 3-axis acceleration deducts the acceleration of gravity recorded under body axis system projection on three axles, and the magnetic induction that will be converted to Intensity deducts the three axle magnetic induction recorded under body axis system, obtains measurement error function e_(q), utilize conjugate gradient method to ask Measurement error of sening as an envoy to function e_(q)Minimum measurement quaternary number, i.e. utilizes conjugate gradient method to set up EKF observation Model.

In technique scheme, step S5 is set up as follows to the detailed process of the process model of EKF:

First by the differential value of quaternary numberCan be expanded the state equations of Kalman filtering with the relation of angular velocity w:

$\stackrel{\·}{q}=\mathrm{\Ω}\[w\]*q---\left(17\right)$

$\mathrm{\Ω}\[w\]=\frac{1}{2}\left[\begin{array}{cccc}0& -w_x& -w_y& -w_z\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]---\left(18\right)$

Ω [w] is state-transition matrix, w_x、w_y、w_zRepresent three axis angular rates that gyro sensor is measured respectively；Represent The differential of quaternary number.Continuous time model is converted into discrete time model, and making the systematic sampling time is T_s, discrete time model For:

q_(k+1)=exp (Ω_kT_s)*q_k (19)

Q in formula_(k)Represent kth time sampling, to exp (Ω_kT_s) carrying out Taylor series expansion reservation single order and second order term, can obtain Formula (20):

$q_{(k+1)}=\[I_{3*3}(1-\frac{{\mathrm{\Δ\θ}}^2}{8})+\frac{1}{2}{\mathrm{\Ω}}_k{T}_s\]q_k---\left(20\right)$

In formula:

Δθ²=(w_xT_s)²+(w_yT_s)²+(w_zT_s)² (21)

I_3*3Represent three-dimensional unit vector, w_x、w_y、w_zRepresent three axis angular rates that gyro sensor is measured respectively.

In technique scheme, step S5 utilizes extended Kalman filter to estimate optimum quaternary number to include walking as follows Rapid: to model the quaternary number estimating the next moment first with the process model of step S5, then carry out error covariance more Newly, the 3rd step utilizes the covariance updated to calculate Kalman gain, and the 4th step utilizes observation model in step S4 to solve Measure quaternary number and deduct the quaternary number in the next moment that the first step estimates, be multiplied by the Kalman gain that the 3rd step is obtained, Quaternary number plus a upper moment is exactly the optimum quaternary number that extended Kalman filter is obtained；Update subsequent time association afterwards Variance is prepared for calculating next time.

The EKF method of the present invention take into account the measurement noise of systematic error and sensor, therefore estimates Quaternary number has more preferable accuracy.Utilize conjugate gradient method to obtain observing quaternary number and apply in EKF, can To avoid calculating complicated linearisation observation model.

The attitude algorithm method that the conjugate gradient that the present invention selects is combined with spreading kalman, utilizes conjugate gradient method to ask Go out to observe quaternary number and apply in EKF, it is not necessary to the problem considering linearisation observation model.Conjugate gradient method Comparing gradient descent method, the step-length of decline is the dynamic value revised in real time, and precision is higher, it is easier to follow the tracks of various flight shape State.

In addition, the present invention has used the method for current most popular quaternary number to remove to represent the rotation of body, quaternary number Method is used in R⁴In space, comparing the method that Eulerian angles represent that body rotates, the method for quaternary number is easier to represent that body is any Rotation, and do not have singularity problem.

In sum, the four rotor attitude algorithm methods that the conjugate gradient method of the present invention is combined with spreading kalman, mainly Advantage has three: one utilizes Fusion, is modified low cost sensor drift problem, improves and measures essence Degree.It two make use of conjugate gradient method to set up the observation model of EKF, and conjugate gradient method is First Order Iterative Method compares other needs linearisation observation model, and the method amount of calculation of the Jacobin matrix solving complexity is greatly reduced.The side of allowing The real-time of method has had good guarantee, ready for preferably controlling the attitude of quadrotor.Its three utilizations extension Kalman's method considers the noise of system own and the measurement error of sensor, improves certainty of measurement, make use of conjugate gradient Method can adjust descent direction dynamically and step-length can well adapt to different state of flights.

Accompanying drawing explanation

Fig. 1 is the navigational coordinate system and body axis system graph of a relation that the present invention relates to；

Fig. 2 is that the navigational coordinate system that the present invention relates to turns to body axis system figure through three times；

Fig. 3 is the four rotor attitude algorithm method block diagrams that conjugate gradient of the present invention is combined with EKF；

Fig. 4 is the four rotor attitude algorithm method measurement procedures that conjugate gradient of the present invention is combined with EKF Figure；

Fig. 5 is the four rotor attitude algorithm method measurement result figures that conjugate gradient of the present invention is combined with EKF (roll angle changes over curve).

Fig. 6 is the four rotor attitude algorithm method measurement result figures that conjugate gradient of the present invention is combined with EKF (angle of pitch changes over curve).

Detailed description of the invention

Describe the present invention in detail below against accompanying drawing 1-6 and embodiment, the present invention be not restricted to accompanying drawing and reality Execute example.

The four rotor attitude algorithm methods that the conjugate gradient implemented according to the present invention is combined with EKF are overall Flow process such as Fig. 3 and 4.It is characterized in that comprising the steps:

Step S1: gather sensor information: under body axis system, is sensed by gyroscope, acceleration transducer, earth magnetism Device gathers four rotor status informations, and wherein, four rotor three axis angular rates measured by gyroscope, and acceleration transducer is measured four rotors and added Speed projection on three axles, geomagnetic sensor measures three axle magnetic induction of four rotor positions；Body axis system is It is fixed on the reference frame of four rotors；

Step S2: process of wafing of respectively gyroscope, accelerometer being zero-suppressed:

Step S3: use Butterworth LPF to filter body acceleration to acceleration of gravity projection on three axles Impact；Three axle magnetic induction are done zero normalization；

Acceleration transducer is again gravitational accelerometer, and it can measure the projection on three axles of the aircraft gravity；But The actual acceleration of gravity measured includes body acceleration, owing to body acceleration is high-frequency noise, so using Bart Butterworth low pass filter filters the impact of body acceleration；It is the Butterworth low pass of 5HZ through experimental selection cut-off frequency Ripple device；Owing to the characteristic of geomagnetic sensor itself is not full symmetric, so needing the method using zero normalizing to earth magnetism The three axle magnetic induction that sensor records process；

Step S4: using perfect condition homogenous gravitational field and earth's magnetic field as reference direction, by quaternary under navigational coordinate system Number conversion obtains 3-axis acceleration [a' under body axis system_bx a'_by a'_bz]^TWith magnetic induction [m'_bx m'_by m'_bz]^T； The 3-axis acceleration being converted to is deducted the acceleration of gravity recorded under body axis system projection on three axles, and will conversion The magnetic induction obtained deducts the three axle magnetic induction recorded under body axis system, obtains measurement error function e_(q), utilize Conjugate gradient method is obtained and is made measurement error function e_(q)Minimum quaternary number, i.e. utilizes conjugate gradient method to set up spreading kalman Filtering observation model；

Step S5: set up the process model of extended Kalman filter: that conjugate gradient method is obtained makes error function e_(q) Minimum quaternary number, as measured value, designs extended Kalman filter, utilizes extended Kalman filter to estimate optimum four Unit's number；

Step S6: simultaneous Euler orientation cosine matrix and Quaternion Matrix, obtains three attitudes utilizing quaternary number form to become Angle equation；Optimum quaternary number step S5 estimated is brought three attitude angle equations into and is obtained three attitude angle: course angle γ, bows Elevation angle theta, roll angle

In technique scheme, step S2 zero-suppress gyroscope, the accelerometer specific practice processed of wafing is by four rotations Wing unmanned plane keeps level, takes 200 sub-values and averages, and this meansigma methods is just zero to waft value, and the value of acquirement is required for subtracting later This is gone zero to waft value.

In technique scheme, the three axle magnetic induction that geomagnetic sensor is recorded by step S3 do zero normalization Detailed process is as follows:

First, four rotor horizontal positioned of geomagnetic sensor will be installed, and the most constantly rotate four rotors and sense by earth magnetism Device rotates a ball, simultaneously in rotary course slave computer (slave computer is the single-chip microcomputer on four rotors, and three large sensors are integrated In single-chip microcomputer, slave computer is connected with computer upper machine communication) constantly send the data to host computer；And then, host computer pair The data received process, and find out maximum and the minima of each single axle of x-axis, y-axis and z-axis respectively, finally return Zero normalizing calculates: zero, it is simply that the absolute value that will make the maximum of single axle and minima is equal, right on coordinate axes in other words Claim；Normalizing, it is simply that make the scope of data of two axles reach unanimity；Computing formula is as follows:

${\mathrm{Hx}}_i={\mathrm{Hsx}}_i-\frac{{\mathrm{Hsx}}_{max}+{\mathrm{Hsx}}_{min}}{2}---\left(1\right)$

${\mathrm{Hy}}_i=\frac{{\mathrm{Hsx}}_{max}-{\mathrm{Hsx}}_{min}}{{\mathrm{Hsy}}_{max}-{\mathrm{Hsy}}_{min}}*({\mathrm{Hsy}}_i-\frac{{\mathrm{Hsy}}_{max}+{\mathrm{Hsy}}_{\min }}{2})---\left(2\right)$

${\mathrm{Hz}}_i=\frac{{\mathrm{Hsx}}_{max}-{\mathrm{Hsx}}_{min}}{{\mathrm{Hsz}}_{max}-{\mathrm{Hsz}}_{min}}*({\mathrm{Hsz}}_i-\frac{{\mathrm{Hsz}}_{max}+{\mathrm{Hsz}}_{min}}{2})---\left(3\right)$

In formula, x-axis is major axis, and y-axis is short axle；Hx_i、Hy_i、Hz_iFor each single axle of x-axis, y-axis and z-axis through zero normalizing Calibration value after process；Hsx_i、Hsy_i、Hsz_iBe illustrated respectively in x-axis, three axle magnetic induction that y-axis, z-axis collect； Hsx_max、Hsx_min、Hsy_max、Hsy_min、Hsz_max、Hsz_minBe illustrated respectively in x-axis, three axle magnetic induction that y-axis, z-axis collect strong The maximum of degree and minima.

In technique scheme, step S4 utilizes conjugate gradient method to set up the tool to EKF observation model Body step is as follows:

The most in the ideal case, it is believed that the reference quantity of gravity is f_g=[0 0 g]^T, the reference quantity in earth's magnetic field is H=[H_n 0 H_d]^T；Wherein g represents acceleration of gravity, H_nAnd H_dIt is that earth's magnetic field north component in east northeast ground coordinate system divides with vertical respectively Amount；Unitization obtain

Define navigational coordinate system (n system) and body axis system (b system) as shown in Figure 1.In navigation system, body axis system phase Rotation to navigational coordinate system can represent with rotating quaternary number, it is assumed that initial state V_E=x_Ei+y_Ej+z_EK, x in formula_E、y_E、z_E Represent amount the to be rotated projection on three axles respectively；Final state V is arrived through the rotation q of four rotors_b=x'i+y'j + z'k, (is the once rotation q representing four rotors by the mode of quaternary number here, then utilizes quaternion differential equation to update New quaternary number, q cited below₀、q₁、q₂、q₃It it is all the quaternary number after quaternion differential equation updates；Due to four rotations The state being in a continuous motion of wing unmanned plane, so each cycle q is to need the differential equation by quaternary number Update, thus q also illustrate that quaternion differential equation update after quaternary number.Mono-initial value [1 00 of q is given before starting 0], after, each cycle is updated and is once updated by the differential equation.) wherein: q=q₀+q₁i+q₂j+q₃K, q₀、q₁、q₂、q₃Table Showing the component of quaternary number, i, j, k represent three vectors；, the amount throwing on three axles respectively after wherein x', y', z' represent rotation Shadow, according to the image mode that quaternary number rotates be:

$V_b=q^{*}\⊗V_E\⊗q---\left(4\right)$

Q in formula^*The identical vector of conjugate quaternion i.e. scalar for q is contrary, is expressed as q^*=q₀-q₁i-q₂j-q₃k.By above formula Launch, can be such as formula (5):

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2+q_2^2-q_1^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2+q_3^2-q_1^2-q_2^2\end{array}\right]*\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(5\right)$

According to formula (5), it would be desirable in the case of, the reference quantity of gravity and the reference quantity in earth's magnetic field are transformed into body axis system In, such as formula (6) and (7):

$\left[\begin{array}{c}{a}_{bx}^{\′}\\ a_{by}^{\′}\\ a_{bz}^{\′}\end{array}\right]=q^{*}\⊗{\hat{f}}_g\⊗q=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 1-2(q_1^2-q_2^2)\end{array}\right]---\left(6\right)$

$\left[\begin{array}{c}{m}_{bx}^{\′}\\ m_{by}^{\′}\\ m_{bz}^{\′}\end{array}\right]=q^{*}\⊗\hat{H}\⊗q=\left[\begin{array}{c}2{\hat{H}}_n(0.5-q_2^2-q_3^2)+2{\hat{H}}_d(q_1{q}_3-q_0{q}_2)\\ 2{\hat{H}}_n(q_1{q}_2-q_0{q}_3)+2{\hat{H}}_d(q_0{q}_1+q_2{q}_3)\\ 2{\hat{H}}_n(q_0{q}_2+q_1{q}_3)+2{\hat{H}}_d(0.5-q_1^2-q_2^2)\end{array}\right]---\left(7\right)$

In formula (6) and formula (7)Ideally gravitational field and magnetic induction after representation unit exist respectively Component on three axles,Respectively north component and vertical component in east northeast ground coordinate system after representation unit, q represents four Quaternary number after unit's fractional differentiation equation renewal, wherein q₀、q₁、q₂、q₃Represent the component of quaternary number, q^*Conjugate quaternion for q.Will Acceleration that formula (6) and formula (7) the data obtained and acceleration transducer and geomagnetic sensor record at body axis system and magnetic strength Answer intensity to subtract each other, measurement error function e can be obtained_(q):

e_(q)=[a'_bx-a_bx a'_by-a_by a'_bz-a_bz m'_bx-m_bx m'_by-m_by m'_bz-m_bz]^T (8)

In formula (8), a_bx、a_by、a_bz、m_bx、m_by、m_bzRepresent that acceleration transducer and geomagnetic sensor are under body axis system The acceleration recorded and magnetic induction, a'_bx、a'_by、a'_bz、m'_bx、m'_by、m'_bzIt it is the ideal situation that calculates of formula (6), (7) The acceleration of lower body and magnetic induction.Then the optimization problem to quaternary number can be exchanged into and seeks object function? The problem of little value.

Computing formula according to conjugate gradient method:

Z_(k+1)=Z_(k)+λ_(k+1)d_(k+1) (9)

In formula, λ_(k+1)For optimal step size, d_(k+1)For the direction declined, Z_(k)Represent a upper moment quaternary number, Z_(k+1)Represent The quaternary number that subsequent time estimates, k represents kth time iteration.

According to FR conjugate gradient method:

$\▿f=J^T\·e_{\left(q\right)}---\left(10\right)$

In formulaIt is an intermediate variable, J^TFor e_(q)Refined lattice than matrix, concrete launch after refined lattice than matrix such as formula (11):

$J^T={\left[\begin{array}{cccc}-2q_2& 2q_3& -2q_0& 2q_1\\ 2q_1& 2q_0& 2q_3& 2q_2\\ 0& -4q_1& 4q_2& 0\\ -2{\hat{H}}_d{q}_2& 2{\hat{H}}_d{q}_3& -4{\hat{H}}_n{q}_2-2{\hat{H}}_d{q}_0& -4{\hat{H}}_n{q}_3+2{\hat{H}}_d{q}_1\\ -2{\hat{H}}_n{q}_3+2{\hat{H}}_d{q}_1& 2{\hat{H}}_n{q}_2+2{\hat{H}}_d{q}_0& 2{\hat{H}}_n{q}_1+2{\hat{H}}_d{q}_3& -2{\hat{H}}_n{q}_0+2{\hat{H}}_d{q}_2\\ 2{\hat{H}}_n{q}_2& 2{\hat{H}}_n{q}_3-4{\hat{H}}_d{q}_1& 2{\hat{H}}_n{q}_0-4{\hat{H}}_d{q}_2& 2{\hat{H}}_n{q}_1\end{array}\right]}^T---\left(11\right)$

In formulaWithRespectively after representation unit in east northeast ground coordinate system the north to north component and vertical direction Vertical component, n represent the north to, d represents vertical direction；Wherein q₀、q₁、q₂、q₃Represent the component of quaternary number.

According to FR conjugate gradient method, formula below is utilized to select step-length, and descent direction:

d₁=-g₁ (12)

d_k+1=-g_k+1+β_k+1d_k (13)

$g_{k+1}=\▿f_{k+1}---\left(14\right)$

${\mathrm{\β}}_{k+1}=\frac{\left|\right|g_{k+1}|{|}^2}{\left|\right|g_k|{|}^2}---\left(15\right)$

${\mathrm{\λ}}_{k+1}=-\frac{g_{k+1}^T{d}_{k+1}}{d_{k+1}^T{d}_{k+1}}---16)$

5 formula are that conjugate gradient method selects suitable step-length and the core formula of the fastest descent direction, in formula above g_k+1, β_k+1It is two intermediate variables,For formula (10) through the result calculated for k+1 time, d_k+1Represent that kth declines for+1 time Step-length, λ_k+1The direction declined for+1 time for kth, d₁For the direction declined for the first time, g₁For intermediate variable g_k+1Initial value；

So far conjugate gradient method is utilized to solve the minima of error function, namely to EKF for step S4 Observation model modeling, thus obtain one group of attitude quaternion, this group quaternary number is to pass based on acceleration transducer and earth magnetism The quaternary number that sensor characterizes, as the measured value of EKF.

Step S5 is set up as follows to the detailed process of the process model of EKF:

First by the differential value of quaternary numberRelation with angular velocity w can obtain the state equations of Kalman filtering:

$\stackrel{\·}{q}=\mathrm{\Ω}\[w\]*q---\left(17\right)$

$\mathrm{\Ω}\[w\]=\frac{1}{2}\left[\begin{array}{cccc}0& -w_x& -w_y& -w_x\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]---\left(18\right)$

Ω [w] is state-transition matrix, w_x、w_y、w_zRepresent three axis angular rates that gyro sensor is measured respectively；Represent The differential of quaternary number.Continuous time model is converted into discrete time model, and making the systematic sampling time is T_s, discrete time model For:

q_(k+1)=exp (Ω_kT_s)*q_k (19)

Q in formula_(k)Represent kth time sampling, to exp (Ω_kT_s) carrying out Taylor series expansion reservation single order and second order term, can obtain Formula (20):

$q(k+1)=\[I_{3*3}(1-\frac{{\mathrm{\Δ\θ}}^2}{8})+\frac{1}{2}{\mathrm{\Ω}}_k{T}_s\]q_k---\left(20\right)$

In formula:

Δθ²=(w_xT_s)²+(w_yT_s)²+(w_zT_s)² (21)

I_3*3Represent three-dimensional unit vector, w_x、w_y、w_zRepresent three axis angular rates that gyro sensor is measured respectively, so far Complete the modeling of the process model to EKF, the modeling of process model is mainly used in the of EKF One step, namely estimates the quaternary number in next moment.Need the most exactly to utilize extended Kalman filter to estimate optimum Quaternary number: model quaternary number i.e. the formula (22) estimating the next moment first with the process model of step S5, so After carry out second step and utilize formula (23) to carry out error covariance renewal, the covariance that the 3rd step utilizes formula (24) to update calculates Going out Kalman gain, the measurement quaternary number that the 4th step utilizes the observation model of step S4 to solve deducts what the first step estimated The quaternary number in next moment, is multiplied by the Kalman gain that the 3rd step is obtained, adds that the quaternary number in a moment extends exactly The optimum quaternary number that Kalman filter is obtained, the 5th step utilizes formula (26) to update subsequent time covariance for calculate next time Prepare.Extended Kalman filter is exactly so constantly covariance recurrence, it is thus possible to obtain optimum quaternary number.Give below Go out to utilize extended Kalman filter to estimate five concrete steps of optimum quaternary number:

The first step: the k moment angular velocity measured according to gyroscope updates the state quaternary number in four rotor k+1 moment, as follows:

${\hat{x}}^{-}(k+1)=A_{\left(k\right)}{\hat{x}}_{\left(k\right)}---\left(22\right)$

The corresponding formula (20) of formula (22),Represent quaternary number, A_kRepresent exp (Ω_kT_s) single order after Taylor series expansion And second order term,Represent the k+1 moment quaternary number of prediction.

Second step: four rotor states calculate that forward error covariance updates, as follows:

P^- _(k+1)=A (k) P (k) A^T(k)+Q(k+1) (23)

In formula, P_(k)Represent the covariance matrix in k moment, Q_(k+1)Represent four-dimensional process noise,Represent A_(k)Transposition square Battle array,Represent the covariance matrix in the k+1 moment of prediction.

3rd step: solving of filter gain coefficient is as follows:

$k_{k+1}=P_{k+1}^{-}{H}_{k+1}^T{(H_{k+1}{P}_{k+1}^{-}{H}_{k+1}^T+R_{(k+1)})}^{-1}---\left(24\right)$

In formula, K_k+1Represent Kalman gain, R_(k+1)Represent four-dimensional measurement noise, H_(k+1)Representation unit matrix, Represent the covariance matrix in the k+1 moment of prediction.

4th step: obtain observing renewal equation according to conjugate gradient method is as follows:

${\hat{x}}_{k+1}={\hat{x}}_{k+1}^{-}+k_{k+1}\[Z_{k+1}-H_{k+1}{\hat{x}}_{k+1}^{-}\]---\left(25\right)$

In formula,Represent the optimum quaternary number that Kalman filter is estimated, Z_k+1Ask for conjugate gradient method in step S4 The quaternary number solved,Represent the k+1 moment quaternary number of prediction, H_(k+1)Representation unit matrix；

5th step: covariance updates, as follows:

$P_{(k+1)}=(I-k_{(k+1)}{H}_{(k+1)})P_{(k+1)}^{-}---\left(26\right)$

In formula, I is four-dimensional unit matrix,Represent the covariance matrix in the k+1 moment of prediction, H_(k+1)Representation unit square Battle array.

So far, utilize extended Kalman filter to estimate optimum quaternary number process and terminate, utilize estimation the most exactly Go out optimum quaternary number and solve the process of Eulerian angles.

In technique scheme, step S6 utilizes and estimates the detailed process of optimum quaternary number resolving attitude angle equation such as Under:

Define the angle that four rotors turn over around z-axisFor course angle；The angle γ turned over around y-axis is roll angle；Turn around x-axis The angle, θ crossed is the angle of pitch；Assume that four rotors sequentially pass through following three times to rotate and move to body axis system from navigational coordinate system: First turn over around z-axisAngle；θ angle is turned over further around y-axis；Finally turn over γ angle around x-axis；

1) rotate around z-axis(such as Fig. 2)

2) around y₁Rotate θ (such as Fig. 2)

$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\θ}& 0& -sin\mathrm{\θ}\\ 0& 1& 0\\ sin\mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$

3) around x₂Rotate γ (such as Fig. 2)

$\left[\begin{array}{c}{x}_3\\ y_3\\ z_3\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathrm{\γ}& sin\mathrm{\γ}\\ 0& -\sin \mathrm{\γ}& cos\mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]$

Here the mode of the once rotation Eulerian angles of four rotors being decomposed into three rotations, x in formula, y, z represent rotation The x of front body axis system, y, z-axis, become x through rotating body axis system around z-axis for the first time₁,y₁,z₁, through second time around y₁ Rotate body axis system and become x₂,y₂,z₂, eventually pass third time around x₂Rotate body axis system and become x₃,y₃,z₃。

Above-mentioned three rotations of simultaneous can obtain:

Then can obtain Euler orientation cosine matrix as follows:

Can obtain according to formula (5) and (28):

θ=-arctan2 (q₁q₃-q₀q₂) (30)

$\mathrm{\γ}=\arctan \frac{2(q_2{q}_3+q_0{q}_1)}{q_0^2+q_3^2-q_1^2-q_2^2}---\left(31\right)$

Fig. 5 is the four rotor attitude algorithm method measurement result figures that conjugate gradient of the present invention is combined with EKF (roll angle changes over curve).

Fig. 6 is the four rotor attitude algorithm method measurement result figures that conjugate gradient of the present invention is combined with EKF (angle of pitch changes over curve).

In Fig. 5 and 6, the longitudinal axis represents that corresponding angle changes, and transverse axis is that sampling number (changes over and samples every 4ms Once).Wherein fine line part is not carry out the attitude that blending algorithm calculates to change over curve, heavy line part For the attitude algorithm method of the present invention, this it appears that the attitude algorithm method static properties of the present invention and dynamically from figure Functional, generation drift error will not be changed over, line smoothing does not has time delay, it is possible to well follow the tracks of the change of attitude Change.

Claims (6)

1. the four rotor attitude algorithm methods that conjugate gradient is combined with EKF, described four rotors comprise monolithic Machine and the gyroscope being connected with single-chip microcomputer, acceleration transducer, geomagnetic sensor, single-chip microcomputer and ground system upper machine communication Connect；It is characterized in that comprising the steps:

Step S1: gather sensor information: under body axis system, adopted by gyroscope, acceleration transducer, geomagnetic sensor Collecting four rotor flying status informations, wherein, four rotor three axis angular rates measured by gyroscope, and acceleration transducer is measured four rotors and added Speed projection on three axles, geomagnetic sensor measures three axle magnetic induction of four rotor positions；Described body coordinate System is the reference frame being fixed on four rotors；

Step S2: process of wafing of respectively gyroscope, accelerometer being zero-suppressed:

Step S3: use Butterworth LPF to filter the body acceleration shadow to acceleration of gravity projection on three axles Ring；Three axle magnetic induction are done zero normalization；

Step S4: utilize conjugate gradient method to set up EKF observation model；

Step S5: set up the process model of extended Kalman filter: that conjugate gradient method is obtained makes error function e_(q)Minimum Quaternary number as measured value, design extended Kalman filter, utilize extended Kalman filter to estimate optimum quaternary number；

Step S6: simultaneous Euler orientation cosine matrix and Quaternion Matrix, obtains three attitude angle sides utilizing quaternary number form to become Journey；Optimum quaternary number step S5 estimated is brought three attitude angle solution of equations into and is calculated three attitude angle of four rotors: course Angle γ, pitching angle theta, roll angle

The four rotor attitude algorithm methods that conjugate gradient the most according to claim 1 is combined with EKF, its It is characterised by: step S2 zero-suppress gyroscope, the accelerometer specific practice processed of wafing is by four rotor holding levels, takes 200 sub-values are averaged, and this meansigma methods is just zero to waft value, and the value later obtained all deducts this and zero wafts value.

The four rotor attitude algorithm methods that conjugate gradient the most according to claim 2 is combined with EKF, its It is characterised by: the detailed process that the three axle magnetic induction that geomagnetic sensor is recorded by step S3 do zero normalization is as follows:

First, four rotor horizontal positioned of geomagnetic sensor will be installed, and the most constantly rotate four rotors and revolve by geomagnetic sensor Producing a ball, in rotary course, slave computer constantly sends the data to host computer simultaneously；And then, host computer is to receiving Data process, find out maximum and the minima of each single axle of x-axis, y-axis and z-axis respectively, finally carry out the normalizing that makes zero Calculate: zero, it is simply that the absolute value that will make the maximum of single axle and minima is equal, symmetrical on coordinate axes in other words；Return One, it is simply that make the scope of data of two axles reach unanimity；Computing formula is as follows:

${\mathrm{Hx}}_i={\mathrm{Hsx}}_i-\frac{{\mathrm{Hsx}}_{max}+{\mathrm{Hsx}}_{\min }}{2}---\left(1\right)$

${\mathrm{Hy}}_i=\frac{{\mathrm{Hsx}}_{max}-{\mathrm{Hsx}}_{\min }}{{\mathrm{Hsy}}_{max}-{\mathrm{Hsy}}_{\min }}*({\mathrm{Hsy}}_i-\frac{{\mathrm{Hsy}}_{max}+{\mathrm{Hsy}}_{min}}{2})---\left(2\right)$

${\mathrm{Hz}}_i=\frac{{\mathrm{Hsx}}_{max}-{\mathrm{Hsx}}_{min}}{{\mathrm{Hsz}}_{max}-{\mathrm{Hsz}}_{min}}*({\mathrm{Hsz}}_i-\frac{{\mathrm{Hsz}}_{max}+{\mathrm{Hsz}}_{\min }}{2})---\left(3\right)$

In formula, x-axis is major axis, and y-axis is short axle；Hx_i、Hy_i、Hz_iFor each single axle of x-axis, y-axis and z-axis through zero normalization After calibration value；Hsx_i、Hsy_i、Hsz_iBe illustrated respectively in x-axis, three axle magnetic induction that y-axis, z-axis collect；Hsx_max、 Hsx_min、Hsy_max、Hsy_min、Hsz_max、Hsz_minBe illustrated respectively in x-axis, three axle magnetic induction that y-axis, z-axis collect Big value and minima.

The four rotor attitude algorithm methods that conjugate gradient the most according to claim 3 is combined with EKF, its It is characterised by: step S4 conjugate gradient method is set up as follows to EKF observation model process: by perfect condition Homogenous gravitational field and earth's magnetic field, as reference direction, obtain three by quaternary number conversion under navigational coordinate system under body axis system Axle acceleration [a'_bx a'_by a'_bz]^TWith magnetic induction [m'_bx m'_by m'_bz]^T；The 3-axis acceleration being converted to is deducted The acceleration of gravity recorded under body axis system projection on three axles, and the magnetic induction being converted to is deducted body seat The three axle magnetic induction recorded under mark system, obtain measurement error function e_(q), utilize conjugate gradient method to obtain and make measurement error letter Number e_(q)Minimum measurement quaternary number, i.e. utilizes conjugate gradient method to set up EKF observation model.

The four rotor attitude algorithm methods that conjugate gradient the most according to claim 4 is combined with EKF, its It is characterised by: step S5 is set up as follows to the detailed process of the process model of EKF:

First by the differential value of quaternary numberCan be expanded the state equations of Kalman filtering with the relation of angular velocity w:

$\stackrel{\·}{q}=\mathrm{\Ω}\[w\]*q---\left(17\right)$

$\mathrm{\Ω}\[w\]=\frac{1}{2}\left[\begin{array}{cccc}0& -w_x& -w_y& -w_z\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]---\left(18\right)$

Ω [w] is state-transition matrix, w_x、w_y、w_zRepresent three axis angular rates that gyro sensor is measured respectively；Represent quaternary The differential of number；Continuous time model is converted into discrete time model, and making the systematic sampling time is T_s, discrete time model is:

q_(k+1)=exp (Ω_kT_s)*q_k (19)

Q in formula_(k)Represent kth time sampling, to exp (Ω_kT_s) carry out Taylor series expansion reservation single order and second order term, formula can be obtained (20):

$q_{(k+1)}=\[I_{3*3}(1-\frac{{\mathrm{\Δ\θ}}^2}{8})+\frac{1}{2}{\mathrm{\Ω}}_k{T}_s\]q_k---\left(20\right)$

In formula:

Δθ²=(w_xT_s)²+(w_yT_s)²+(w_zT_s)² (21)

I_3*3Represent three-dimensional unit vector, w_x、w_y、w_zRepresent three axis angular rates that gyro sensor is measured respectively.

The four rotor attitude algorithm methods that conjugate gradient the most according to claim 5 is combined with EKF, its It is characterised by: step S5 utilizes extended Kalman filter to estimate optimum quaternary number to comprise the steps: first with step The process model modeling of S5 estimates the quaternary number in next moment, then carries out error covariance renewal, and the 3rd step utilizes more New covariance calculates Kalman gain, and the measurement quaternary number that the 4th step utilizes observation model in step S4 to solve deducts The quaternary number in the next moment that one step estimates, is multiplied by the Kalman gain that the 3rd step is obtained, and adds a moment Quaternary number is exactly the optimum quaternary number that extended Kalman filter is obtained；Update subsequent time covariance afterwards for calculate next time Prepare.