CN105890598B - Quadrotor attitude algorithm method of the conjugate gradient in conjunction with Extended Kalman filter - Google Patents
Quadrotor attitude algorithm method of the conjugate gradient in conjunction with Extended Kalman filter Download PDFInfo
- Publication number
- CN105890598B CN105890598B CN201610216667.4A CN201610216667A CN105890598B CN 105890598 B CN105890598 B CN 105890598B CN 201610216667 A CN201610216667 A CN 201610216667A CN 105890598 B CN105890598 B CN 105890598B
- Authority
- CN
- China
- Prior art keywords
- axis
- quadrotor
- kalman filter
- extended kalman
- quaternary number
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
The quadrotor attitude algorithm method that the present invention relates to conjugate gradients in conjunction with Extended Kalman filter, acquisition sensor information first obtains quadrotor state of flight, then model modeling is observed using conjugate gradient method, and then process model modeling is carried out, the quaternary number that conjugate gradient method is obtained is as the measured value of Extended Kalman filter；Optimal quaternary number finally is obtained using Extended Kalman filter method, calculates three attitude angles of quadrotor.Compared to simple gradient method and complementary filter method, Extended Kalman filter method of the invention considers the measurement noise of systematic error and sensor, therefore the quaternary number estimated has better accuracy.Observation quaternary number is obtained using conjugate gradient method to apply in Extended Kalman filter, it can be to avoid the linearisation observation model for calculating complexity.
Description
Technical field
The present invention relates to a kind of quadrotor drone posture course data measurement methods, more particularly to a kind of conjugate gradient
In conjunction with spreading kalman to quadrotor drone (hereinafter referred to as quadrotor) attitude algorithm method, be mainly used for military affairs and detect
It examines, agricultural plant protection, electric inspection process, surveys and draws, take photo by plane.
Background technique
With the continuous development of robot and space flight and aviation technology, quadrotor has been increasingly becoming domestic and foreign scholars and hair of taking photo by plane
Burn the research hotspot of friends.Quadrotor be a kind of more rotors with the special flight performance such as VTOL and hovering nobody
Aircraft.In flight course, by the revolving speed for changing four rotors, thus it is possible to vary various flight attitudes.Just because of quadrotor
Unmanned plane has VTOL, aerial spot hover, the unique flight performance such as flight that six degree of freedom may be implemented, make its
Military field and civil field are widely used.
Attitude algorithm is the core of quadrotor, is responsible for and provides reliable Flight Condition Data in real time for quadrotor
Important task.It is mainly at present complementary filter, Kalman filtering method and gradient descent method with more extensive attitude algorithm method
Method.Complementary filter method and gradient descent method calculate simply, and acceleration transducer is utilized and geomagnetic sensor goes to mend
It repays since gyro sensor generates error caused by drift at any time, but does not all account for the noise and sensing of system itself
The measurement error of device.Complementary filter method needs to choose dynamic weight and deacclimatizes different state of flights, most of to transport user
It selects to deacclimatize rapid flight to a bigger weight, it is not good enough that this will will lead to aircraft static properties.Kalman filtering
The measurement error of system and the measurement noise of sensor are considered in method, have better accuracy compared to first two method, and
It is with widest attitude algorithm method.It needs to linearize observation model in general Kalman's method, and linearizes observation
Model needs face the problem of solving Jacobin matrix, this will bring very big calculation amount.
Summary of the invention
The technical problem to be solved by the present invention is to aiming at the problem that quadrotor attitude algorithm, providing one kind can be real-time
Posture and the higher attitude algorithm method of precision are obtained, to preferably control the state of flight of quadrotor.
In order to solve the above technical problems, the technical solution adopted by the present invention is that:
A kind of quadrotor attitude algorithm method of conjugate gradient in conjunction with Extended Kalman filter, the quadrotor include single
Piece machine and the gyroscope being connected with single-chip microcontroller, acceleration transducer, geomagnetic sensor, single-chip microcontroller and ground system host computer are logical
News connection；It is characterized by comprising following steps:
Step S1: it acquisition sensor information: under body coordinate system, is sensed by gyroscope, acceleration transducer, earth magnetism
Device acquires quadrotor state of flight information, wherein gyroscope measures three axis angular rate of quadrotor, four rotation of acceleration transducer measurement
Projection of the wing acceleration on three axis, geomagnetic sensor measure three axis magnetic induction intensity of quadrotor position；The body
Coordinate system is the reference frame for being fixed on quadrotor；
Step S2: it zero-suppresses processing of floaing to gyroscope, accelerometer respectively:
Step S3: projection of the body acceleration to acceleration of gravity on three axis is filtered out using Butterworth LPF
Influence；Zero normalization is done to three axis magnetic induction intensity；
Step S4: it is established using conjugate gradient method to Extended Kalman filter observation model；
Step S5: it establishes the process model of extended Kalman filter: making error function e for what conjugate gradient method was found out(q)
The smallest quaternary number designs extended Kalman filter, estimates optimal four using extended Kalman filter as measured value
First number；
Step S6: simultaneous Euler orientation cosine matrix and quaternionic matrix, obtain three using quaternary number forms at posture
Angle equation；The optimal quaternary number that step S5 is estimated brings three attitude angles that three attitude angle solution of equations calculate quadrotor into:
Course angle γ, pitching angle theta, roll angle
In above-mentioned technical proposal, step S2 zero-suppress to gyroscope, accelerometer float processing specific practice be by four rotation
The wing keeps horizontal, and 200 sub-values is taken to be averaged, this average value is just zero to float value, and the value obtained later all subtracts this and zero floats value.
In above-mentioned technical proposal, step S3 does zero normalization to the three axis magnetic induction intensity that geomagnetic sensor measures
Detailed process is as follows:
Firstly, the quadrotor for being equipped with geomagnetic sensor is horizontally arranged, then constantly rotation quadrotor allows earth magnetism to sense
Device rotates out of a ball, while slave computer constantly sends the data to host computer in rotary course；In turn, host computer docks
The data received are handled, and are found out the maximum value and minimum value of each single axis of x-axis, y-axis and z-axis respectively, are finally zeroed
Normalizing calculates: zero, the absolute value for seeking to the maximum value for making single axis and minimum value is equal, right in reference axis in other words
Claim；Normalizing exactly makes the data area of two axis reach unanimity；Calculation formula is as follows:
In formula, x-axis is long axis, and y-axis is short axle；Hxi、Hyi、HziIt is each single axis of x-axis, y-axis and z-axis by zero normalizing
Treated calibration value；Hsxi、Hsyi、HsziIt is illustrated respectively in the collected three axis magnetic induction intensity of x-axis, y-axis, z-axis；
Hsxmax、Hsxmin、Hsymax、Hsymin、Hszmax、HszminIt is strong to be illustrated respectively in the collected three axis magnetic induction of x-axis, y-axis, z-axis
The maximum value and minimum value of degree.
In above-mentioned technical proposal, step S4 is established to Extended Kalman filter observation model process such as with conjugate gradient method
Under: using perfect condition homogenous gravitational field and earth's magnetic field as reference direction, body is transformed by quaternary number under navigational coordinate system
3-axis acceleration [a' is obtained under coordinate systembx a'by a'bz]TWith magnetic induction intensity [m'bx m'by m'bz]T；By what is be converted to
3-axis acceleration subtracts projection of the acceleration of gravity measured under body coordinate system on three axis, and the magnetic induction that will be converted to
Intensity subtracts the three axis magnetic induction intensity measured under body coordinate system, obtains measurement error function e(q), asked using conjugate gradient method
Measurement error of sening as an envoy to function e(q)The smallest measurement quaternary number is established Extended Kalman filter using conjugate gradient method and is observed
Model.
In above-mentioned technical proposal, detailed process is as follows for process model of the step S5 foundation to Extended Kalman filter:
First by the differential value of quaternary numberWith the state equations of the available Extended Kalman filter of relationship of angular speed w:
Ω [w] is state-transition matrix, wx、wy、wzRespectively indicate three axis angular rates of gyro sensor measurement；It indicates
The differential of quaternary number.Discrete time model is converted by continuous time model, enabling the systematic sampling time is Ts, discrete time model
Are as follows:
q(k+1)=exp (ΩkTs)*qk (19)
Q in formula(k)Kth time sampling is indicated, to exp (ΩkTs) Taylor series expansion reservation single order and second order term are carried out, it can obtain
Formula (20):
In formula:
Δθ2=(wxTs)2+(wyTs)2+(wzTs)2 (21)
I3*3Indicate three-dimensional unit vector, wx、wy、wzRespectively indicate three axis angular rates of gyro sensor measurement.
In above-mentioned technical proposal, it includes following step that step S5, which estimates optimal quaternary number using extended Kalman filter,
It is rapid: to estimate the quaternary number at next moment first with the process model modeling of step S5, then carry out error covariance more
Newly, third step calculates kalman gain using the covariance of update, and the 4th step is solved using observation model in step S4
Measurement quaternary number subtracts the quaternary number at next moment that the first step estimates, and is multiplied by the kalman gain that third step is found out,
In addition the quaternary number at a upper moment is exactly the optimal quaternary number that extended Kalman filter is found out；Subsequent time association is updated later
Variance is to calculate to prepare next time.
Extended Kalman filter method of the invention considers the measurement noise of systematic error and sensor, therefore estimate
Quaternary number has better accuracy.Observation quaternary number is obtained using conjugate gradient method to apply in Extended Kalman filter, it can
To avoid the linearisation observation model for calculating complexity.
Attitude algorithm method of the conjugate gradient that the present invention selects in conjunction with spreading kalman, is asked using conjugate gradient method
Out observation quaternary number apply in Extended Kalman filter, without the concern for linearisation observation model the problem of.Conjugate gradient method
Compared to gradient descent method, the step-length of decline is real-time modified dynamic value, and precision is higher, it is easier to track various flight shapes
State.
In addition to this, the present invention goes to indicate the rotation of body, quaternary number with the method for current most popular quaternary number
Method is used in R4In space, compared to the method that Eulerian angles indicate body rotation, the method for quaternary number is easier to indicate that body is any
Rotation, and be not in singularity problem.
In conclusion the quadrotor attitude algorithm method of conjugate gradient method of the invention in conjunction with spreading kalman, mainly
There are three advantages: one utilizes Fusion, is modified to inexpensive sensor drift problem, improves measurement essence
Degree.Secondly the observation model that conjugate gradient method establishes Extended Kalman filter is utilized, conjugate gradient method is First Order Iterative
Method needs to linearize observation model compared to other, and the method calculation amount for solving complicated Jacobin matrix greatly reduces.The side of allowing
The real-time of method has good guarantee, and the posture for preferably control quadrotor is ready.Thirdly with extension
Kalman's method considers the measurement error of system noise itself and sensor, improves measurement accuracy, conjugate gradient is utilized
Method can dynamically adjust descent direction and step-length and can be very good to adapt to different state of flights.
Detailed description of the invention
Fig. 1 is navigational coordinate system of the present invention and body coordinate system relational graph；
Fig. 2 is navigational coordinate system of the present invention by turning to body coordinate system figure three times；
Fig. 3 is quadrotor attitude algorithm method block diagram of the conjugate gradient of the present invention in conjunction with Extended Kalman filter；
Fig. 4 is quadrotor attitude algorithm method measurement procedure of the conjugate gradient of the present invention in conjunction with Extended Kalman filter
Figure；
Fig. 5 is quadrotor attitude algorithm method measurement result figure of the conjugate gradient of the present invention in conjunction with Extended Kalman filter
(roll angle changes over time curve).
Fig. 6 is quadrotor attitude algorithm method measurement result figure of the conjugate gradient of the present invention in conjunction with Extended Kalman filter
(pitch angle changes over time curve).
Specific embodiment
Below against attached drawing 1-6 and embodiment, the present invention will be described in detail, and the present invention is not restricted to given attached drawing and reality
Apply example.
Quadrotor attitude algorithm method of the conjugate gradient implemented according to the present invention in conjunction with Extended Kalman filter is overall
Process such as Fig. 3 and 4.It is characterized by comprising following steps:
Step S1: it acquisition sensor information: under body coordinate system, is sensed by gyroscope, acceleration transducer, earth magnetism
Device acquires quadrotor status information, wherein gyroscope measures three axis angular rate of quadrotor, and acceleration transducer measurement quadrotor adds
Projection of the speed on three axis, geomagnetic sensor measure three axis magnetic induction intensity of quadrotor position；Body coordinate system is
It is fixed on the reference frame of quadrotor；
Step S2: it zero-suppresses processing of floaing to gyroscope, accelerometer respectively:
Step S3: projection of the body acceleration to acceleration of gravity on three axis is filtered out using Butterworth LPF
Influence；Zero normalization is done to three axis magnetic induction intensity；
Acceleration transducer is called gravitational accelerometer, it can measure projection of the aircraft gravity on three axis；But
It include organism acceleration in the acceleration of gravity of actual measurement, since body acceleration is high-frequency noise, so using Bart
Butterworth low-pass filter filters out the influence of body acceleration；The Butterworth low pass for being 5HZ by experimental selection cutoff frequency
Wave device；Since the characteristic of geomagnetic sensor itself is not full symmetric, so needing using the method for zero normalizing to earth magnetism
The three axis magnetic induction intensity that sensor measures are handled；
Step S4: using perfect condition homogenous gravitational field and earth's magnetic field as reference direction, pass through quaternary under navigational coordinate system
Number, which is transformed under body coordinate system, obtains 3-axis acceleration [a'bx a'by a'bz]TWith magnetic induction intensity [m'bx m'by m'bz]T；
The 3-axis acceleration being converted to is subtracted to projection of the acceleration of gravity measured under body coordinate system on three axis, and will conversion
Obtained magnetic induction intensity subtracts the three axis magnetic induction intensity measured under body coordinate system, obtains measurement error function e(q), utilize
Conjugate gradient method, which is found out, makes measurement error function e(q)The smallest quaternary number, i.e., establish spreading kalman using conjugate gradient method
Filter observation model；
Step S5: it establishes the process model of extended Kalman filter: making error function e for what conjugate gradient method was found out(q)
The smallest quaternary number designs extended Kalman filter, estimates optimal four using extended Kalman filter as measured value
First number；
Step S6: simultaneous Euler orientation cosine matrix and quaternionic matrix, obtain three using quaternary number forms at posture
Angle equation；The optimal quaternary number that step S5 is estimated brings three posture angle equations into and finds out three attitude angles: course angle γ bows
Elevation angle theta, roll angle
In above-mentioned technical proposal, step S2 zero-suppress to gyroscope, accelerometer float processing specific practice be by four rotation
Wing unmanned plane keeps horizontal, and 200 sub-values is taken to be averaged, this average value is just zero to float value, and the value obtained later requires to subtract
This is gone zero to float value.
In above-mentioned technical proposal, step S3 does zero normalization to the three axis magnetic induction intensity that geomagnetic sensor measures
Detailed process is as follows:
Firstly, the quadrotor for being equipped with geomagnetic sensor is horizontally arranged, then constantly rotation quadrotor allows earth magnetism to sense
Device rotates out of a ball, while (slave computer is the single-chip microcontroller in quadrotor to slave computer, and three large sensors are integrated in rotary course
In single-chip microcontroller, slave computer and computer host computer communication connection) constantly send the data to host computer；In turn, host computer pair
The data received are handled, and are found out the maximum value and minimum value of each single axis of x-axis, y-axis and z-axis respectively, are finally returned
Zero normalizing calculates: zero, the absolute value for seeking to the maximum value for making single axis and minimum value is equal, right in reference axis in other words
Claim；Normalizing exactly makes the data area of two axis reach unanimity；Calculation formula is as follows:
In formula, x-axis is long axis, and y-axis is short axle；Hxi、Hyi、HziIt is each single axis of x-axis, y-axis and z-axis by zero normalizing
Treated calibration value；Hsxi、Hsyi、HsziIt is illustrated respectively in the collected three axis magnetic induction intensity of x-axis, y-axis, z-axis；
Hsxmax、Hsxmin、Hsymax、Hsymin、Hszmax、HszminIt is strong to be illustrated respectively in the collected three axis magnetic induction of x-axis, y-axis, z-axis
The maximum value and minimum value of degree.
In above-mentioned technical proposal, step S4 establishes the tool to Extended Kalman filter observation model using conjugate gradient method
Steps are as follows for body:
First in the ideal case, it is believed that the reference quantity of gravity is fg=[0 0 g]T, the reference quantity in earth's magnetic field is H=[Hn
0 Hd]T；Wherein g indicates acceleration of gravity, HnAnd HdIt is earth's magnetic field in east northeast north component and vertical point in coordinate system respectively
Amount；It is unitization to obtain
Navigational coordinate system (n system) and body coordinate system (b system) are defined as shown in Figure 1.In navigation system, body coordinate system phase
The rotation of navigational coordinate system can be indicated with rotation quaternary number, it is assumed that initial state VE=xEi+yEj+zEK, x in formulaE、yE、zE
Respectively indicate the projection of amount to be rotated on three axis；Rotation q by a quadrotor reaches final state Vb=x'i+y'j
+ z'k (is the primary rotation q for indicating quadrotor with the mode of quaternary number here, is then updated using quaternion differential equation
New quaternary number, q cited below0、q1、q2、q3It is all by the updated quaternary number of quaternion differential equation；Due to four rotations
Wing unmanned plane is in a state constantly moved, so each period q is the differential equation needed through quaternary number
Come what is updated, thus q also illustrates that the updated quaternary number of quaternion differential equation.Mono- initial value [1 00 of q is given before starting
0], each period updates and is once updated by the differential equation behind.) wherein: q=q0+q1i+q2j+q3K, q0、q1、q2、q3Table
Show that the component of quaternary number, i, j, k indicate three vectors；, wherein x', y', z' indicate the amount throwing on three axis respectively after rotation
Shadow, the image mode rotated according to quaternary number are as follows:
Q in formula*For q the identical vector of conjugate quaternion, that is, scalar on the contrary, being expressed as q*=q0-q1i-q2j-q3k.By above formula
Expansion can be obtained such as formula (5):
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 coordinate system
In, such as formula (6) and (7):
In formula (6) and formula (7)Respectively indicate it is unitization after ideally gravitational field and magnetic induction intensity exist
Component on three axis,North component and vertical component, q indicate four in coordinate system with respectively indicating unitization rear east northeast
First updated quaternary number of fractional differentiation equation, wherein q0、q1、q2、q3Indicate the component of quaternary number, q*For the conjugate quaternion of q.It will
The acceleration and magnetic strength that formula (6) and formula (7) the data obtained and acceleration transducer and geomagnetic sensor are measured in body coordinate system
It answers intensity to subtract each other, measurement error function e can be obtained(q):
e(q)=[a'bx-abx a'by-aby a'bz-abz m'bx-mbx m'by-mby m'bz-mbz]T (8)
In formula (8), abx、aby、abz、mbx、mby、mbzIndicate acceleration transducer and geomagnetic sensor under body coordinate system
The acceleration and magnetic induction intensity measured, a'bx、a'by、a'bz、m'bx、m'by、m'bzIt is formula (6), (7) calculated ideal situation
The acceleration and magnetic induction intensity of lower body.Then the optimization problem of quaternary number can be exchanged into and seek objective functionMost
The problem of small value.
According to the calculation formula of 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 of decline, Z(k)Indicate last moment quaternary number, Z(k+1)It indicates
The quaternary number that subsequent time estimates, k indicate kth time iteration.
According to FR conjugate gradient method:
In formulaFor an intermediate variable, JTFor e(q)Refined lattice than matrix, the refined lattice after specific expansion are than matrix such as formula
(11):
In formulaWithRespectively indicate it is unitization after east northeast in coordinate system the north to north component and vertical direction
Vertical component, n indicate the north to d indicates vertical direction；Wherein q0、q1、q2、q3Indicate the component of quaternary number.
According to FR conjugate gradient method, step-length and descent direction are selected using following formula:
d1=-g1 (12)
dk+1=-gk+1+βk+1dk (13)
5 formula are the core formula that conjugate gradient method selects suitable step-length and most fast descent direction above, in formula
gk+1, βk+1For two intermediate variables,It is formula (10) by calculating for k+1 times as a result, dk+1Indicate kth+1 time decline
Step-length, λk+1The direction declined for kth+1 time, d1For the direction of first time decline, g1For intermediate variable gk+1Initial value；
So far the minimum value of error function is solved using conjugate gradient method for step S4, that is, to Extended Kalman filter
Observation model modeling, one group of attitude quaternion obtained from, this group of quaternary number is passed based on acceleration transducer and earth magnetism
The quaternary number that sensor characterizes, the measured value as Extended Kalman filter.
Detailed process is as follows for process model of the step S5 foundation to Extended Kalman filter:
First by the differential value of quaternary numberWith the state equations of the available Kalman filtering of relationship of angular speed w:
Ω [w] is state-transition matrix, wx、wy、wzRespectively indicate three axis angular rates of gyro sensor measurement；It indicates
The differential of quaternary number.Discrete time model is converted by continuous time model, enabling the systematic sampling time is Ts, discrete time model
Are as follows:
q(k+1)=exp (ΩkTs)*qk (19)
Q in formula(k)Kth time sampling is indicated, to exp (ΩkTs) Taylor series expansion reservation single order and second order term are carried out, it can obtain
Formula (20):
In formula:
Δθ2=(wxTs)2+(wyTs)2+(wzTs)2 (21)
I3*3Indicate three-dimensional unit vector, wx、wy、wzThree axis angular rates of gyro sensor measurement are respectively indicated, so far
It completes to model the process model of Extended Kalman filter, the of Extended Kalman filter is mainly used for the modeling of process model
One step, that is, estimate the quaternary number at next moment.Be exactly later need to estimate using extended Kalman filter it is optimal
Quaternary number: the quaternary number i.e. formula (22) at next moment are estimated first with the process model modeling of step S5, so
Second step is carried out afterwards and carries out error covariance update using formula (23), and third step is calculated using the covariance that formula (24) update
Kalman gain out, the measurement quaternary number that the 4th step is solved using the observation model of step S4 subtract what the first step estimated
The quaternary number at next moment is multiplied by the kalman gain that third step is found out, in addition the quaternary number at a upper moment is exactly to extend
The optimal quaternary number that Kalman filter is found out, it is to calculate next time that the 5th step, which updates subsequent time covariance using formula (26),
It prepares.Extended Kalman filter is exactly so constantly covariance recurrence, so as to find out optimal quaternary number.It gives below
Five specific steps of optimal quaternary number are estimated using extended Kalman filter out:
Step 1: the state quaternary number at quadrotor k+1 moment is updated according to the k moment angular speed of gyroscope measurement, it is as follows:
Formula (22) is corresponding formula (20),Indicate quaternary number, AkIndicate exp (ΩkTs) single order after Taylor series expansion
And second order term,Indicate the k+1 moment quaternary number of prediction.
Step 2: quadrotor state calculates that forward error covariance updates, it is as follows:
P- (k+1)=A (k) P (k) AT(k)+Q(k+1) (23)
In formula, P(k)Indicate the covariance matrix at k moment, Q(k+1)Indicate four-dimensional process noise,Indicate A(k)Transposition square
Battle array,Indicate the covariance matrix at the k+1 moment of prediction.
Step 3: the solution of filter gain coefficient, as follows:
In formula, Kk+1Indicate kalman gain, R(k+1)Indicate four-dimensional measurement noise, H(k+1)Indicate unit matrix,
Indicate the covariance matrix at the k+1 moment of prediction.
Step 4: observation renewal equation is obtained according to conjugate gradient method, it is as follows:
In formula,Indicate the optimal quaternary number of Kalman filter estimation, Zk+1It is asked for conjugate gradient method in step S4
The quaternary number solved,Indicate the k+1 moment quaternary number of prediction, H(k+1)Indicate unit matrix；
Step 5: covariance updates, it is as follows:
In formula, I is four-dimensional unit matrix,Indicate the covariance matrix at the k+1 moment of prediction, H(k+1)Indicate unit square
Battle array.
So far, estimating optimal quaternary number process using extended Kalman filter terminates, and is next exactly to utilize estimation
Optimal quaternary number solves the process of Eulerian angles out.
In above-mentioned technical proposal, step S6 resolves the detailed process of posture angle equation such as using optimal quaternary number is estimated
Under:
Define the angle that quadrotor is turned over around z-axisFor course angle；The angle γ turned over around y-axis is roll angle；Turn around x-axis
The angle, θ crossed is pitch angle；Assuming that quadrotor successively moves to body coordinate system by following rotate three times from navigational coordinate system:
First turned over around z-axisAngle；θ angle is turned over further around y-axis；Finally γ angle is turned over around x-axis；
1) it is rotated around z-axis(such as Fig. 2)
2) around y1It rotates θ (such as Fig. 2)
3) around x2It rotates γ (such as Fig. 2)
Here the mode of the primary rotation Eulerian angles of quadrotor is decomposed into and is rotated three times, x in formula, y, z indicate rotation
The x of preceding body coordinate system, y, z-axis become x around z-axis rotation body coordinate system by first time1,y1,z1, by second around y1
Rotation body coordinate system becomes x2,y2,z2, finally by third time around x2Rotation body coordinate system becomes x3,y3,z3。
The above-mentioned rotation three times of simultaneous can obtain:
It is as follows can then Euler orientation cosine matrix to be obtained:
It can be obtained according to formula (5) and (28):
θ=- arctan2 (q1q3-q0q2) (30)
Fig. 5 is quadrotor attitude algorithm method measurement result figure of the conjugate gradient of the present invention in conjunction with Extended Kalman filter
(roll angle changes over time curve).
Fig. 6 is quadrotor attitude algorithm method measurement result figure of the conjugate gradient of the present invention in conjunction with Extended Kalman filter
(pitch angle changes over time curve).
In Figures 5 and 6, the longitudinal axis indicates corresponding angle change, and horizontal axis is that sampling number (is changed over time and sampled every 4ms
Once).Wherein fine line part is to change over time curve, heavy line part without carrying out the posture that blending algorithm calculates
For attitude algorithm method of the invention, this it appears that attitude algorithm method static properties of the invention and dynamic from figure
It is functional, generation drift error will not be changed over time, line smoothing is not delayed, and can be good at the change for tracking posture
Change.
Claims (3)
1. a kind of quadrotor attitude algorithm method of conjugate gradient in conjunction with Extended Kalman filter, the quadrotor includes monolithic
Machine and the gyroscope being connected with single-chip microcontroller, acceleration transducer, geomagnetic sensor, single-chip microcontroller and ground system host computer communicate
Connection；It is characterized by comprising following steps:
Step S1: it acquisition sensor information: under body coordinate system, is adopted by gyroscope, acceleration transducer, geomagnetic sensor
Collect quadrotor state of flight information, wherein gyroscope measures three axis angular rate of quadrotor, and acceleration transducer measurement quadrotor adds
Projection of the speed on three axis, geomagnetic sensor measure three axis magnetic induction intensity of quadrotor position；The body coordinate
System is the reference frame for being fixed on quadrotor；
Step S2: it zero-suppresses processing of floaing to gyroscope, accelerometer respectively:
Step S3: the shadow of projection of the body acceleration to acceleration of gravity on three axis is filtered out using Butterworth LPF
It rings；Zero normalization is done to three axis magnetic induction intensity；
Step S4: it is established using conjugate gradient method to Extended Kalman filter observation model；
Step S5: it establishes the process model of extended Kalman filter: making error function e for what conjugate gradient method was found out(q)It is minimum
Quaternary number as measured value, design extended Kalman filter, estimate optimal quaternary number using extended Kalman filter；
Step S6: simultaneous Euler orientation cosine matrix and quaternionic matrix, obtain three using quaternary number forms at attitude angle side
Journey；The optimal quaternary number that step S5 is estimated brings three attitude angles that three attitude angle solution of equations calculate quadrotor: course into
Angle γ, pitching angle theta, roll angle
Step S2 zero-suppress to gyroscope, accelerometer float processing specific practice be quadrotor holding is horizontal, take 200 sub-values
It is averaged, this average value is just zero to float value, and the value obtained later all subtracts this and zero floats value；
Step S3 does zero normalization to the three axis magnetic induction intensity that geomagnetic sensor measures, and detailed process is as follows:
Firstly, the quadrotor for being equipped with geomagnetic sensor is horizontally arranged, then constantly rotation quadrotor allows geomagnetic sensor to revolve
A ball is produced, while slave computer constantly sends the data to host computer in rotary course；In turn, host computer is to receiving
Data handled, find out the maximum value and minimum value of each single axis of x-axis, y-axis and z-axis respectively, finally carry out zero normalizing
Calculate: zero, the absolute value for seeking to the maximum value for making single axis and minimum value is equal, symmetrical in reference axis in other words；Return
One, so that the data area of two axis is reached unanimity；Calculation formula is as follows:
In formula, x-axis is long axis, and y-axis is short axle；Hxi、Hyi、HziIt is each single axis of x-axis, y-axis and z-axis by zero normalization
Calibration value afterwards；Hsxi、Hsyi、HsziIt is illustrated respectively in the collected three axis magnetic induction intensity of x-axis, y-axis, z-axis；Hsxmax、
Hsxmin、Hsymax、Hsymin、Hszmax、HszminIt is illustrated respectively in the collected three axis magnetic induction intensity of x-axis, y-axis, z-axis most
Big value and minimum value,
Step S4 is established as follows to Extended Kalman filter observation model process with conjugate gradient method: perfect condition is uniformly weighed
The field of force and earth's magnetic field are transformed under body coordinate system by quaternary number under navigational coordinate system as reference direction and obtain the acceleration of three axis
Spend [a'bx a'by a'bz]TWith magnetic induction intensity [m'bx m'by m'bz]T；The 3-axis acceleration being converted to is subtracted body to sit
Projection of the acceleration of gravity measured under mark system on three axis, and the magnetic induction intensity being converted to is subtracted under body coordinate system
The three axis magnetic induction intensity measured, obtain measurement error function e(q), being found out using conjugate gradient method makes measurement error function e(q)
The smallest measurement quaternary number, i.e., establish Extended Kalman filter observation model using conjugate gradient method.
2. quadrotor attitude algorithm method of the conjugate gradient according to claim 1 in conjunction with Extended Kalman filter,
Be characterized in that: detailed process is as follows for process model of the step S5 foundation to Extended Kalman filter:
First by the state equations of the differential value q of quaternary number and the available Extended Kalman filter of relationship of angular speed w:
Ω [w] is state-transition matrix, wx、wy、wzRespectively indicate three axis angular rates of gyro sensor measurement；Q indicates quaternary
Several differential；Discrete time model is converted by continuous time model, enabling the systematic sampling time is Ts, discrete time model are as follows:
q(k+1)=exp (ΩkTs)*qk (19)
Q in formula(k)Kth time sampling is indicated, to exp (ΩkTs) Taylor series expansion reservation single order and second order term are carried out, formula can be obtained
(20):
In formula:
Δθ2=(wxTs)2+(wyTs)2+(wzTs)2 (21)
I3*3Indicate three-dimensional unit vector, wx、wy、wzRespectively indicate three axis angular rates of gyro sensor measurement.
3. quadrotor attitude algorithm method of the conjugate gradient according to claim 2 in conjunction with Extended Kalman filter,
Be characterized in that: step S5 estimates optimal quaternary number using extended Kalman filter and includes the following steps: first with step
The process model modeling of S5 estimates the quaternary number at next moment, then carries out error covariance update, third step is using more
New covariance calculates kalman gain, and the measurement quaternary number that the 4th step is solved using observation model in step S4 subtracts
The quaternary number at next moment that one step estimates is multiplied by the kalman gain that third step is found out, in addition a upper moment
Quaternary number is exactly the optimal quaternary number that extended Kalman filter is found out；Updating subsequent time covariance later is to calculate next time
It prepares.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610216667.4A CN105890598B (en) | 2016-04-08 | 2016-04-08 | Quadrotor attitude algorithm method of the conjugate gradient in conjunction with Extended Kalman filter |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610216667.4A CN105890598B (en) | 2016-04-08 | 2016-04-08 | Quadrotor attitude algorithm method of the conjugate gradient in conjunction with Extended Kalman filter |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105890598A CN105890598A (en) | 2016-08-24 |
| CN105890598B true CN105890598B (en) | 2019-04-09 |
Family
ID=57012235
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610216667.4A Active CN105890598B (en) | 2016-04-08 | 2016-04-08 | Quadrotor attitude algorithm method of the conjugate gradient in conjunction with Extended Kalman filter |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105890598B (en) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113804191B (en) * | 2016-11-17 | 2024-03-19 | 格兰菲智能科技有限公司 | Mobile device and method for calculating gesture of mobile device |
| CN108693496A (en) * | 2018-05-07 | 2018-10-23 | 国网山东省电力公司电力科学研究院 | A kind of intelligent electric energy meter error predictor method based on parameter degeneration equation |
| CN109724602A (en) * | 2018-12-17 | 2019-05-07 | 南京理工大学 | A kind of attitude algorithm system and its calculation method based on hardware FPU |
| CN109631895B (en) * | 2019-01-04 | 2023-03-31 | 京东方科技集团股份有限公司 | Object pose estimation method and device |
| CN110319840A (en) * | 2019-07-05 | 2019-10-11 | 东北大学秦皇岛分校 | Conjugate gradient attitude algorithm method towards abnormal gait identification |
| CN110531613A (en) * | 2019-09-05 | 2019-12-03 | 广东工业大学 | Two axis unmanned aerial vehicle (UAV) control method, apparatus, equipment and computer readable storage medium |
| CN111141283A (en) * | 2020-01-19 | 2020-05-12 | 杭州十域科技有限公司 | Method for judging advancing direction through geomagnetic data |
| CN114234970A (en) * | 2021-12-21 | 2022-03-25 | 华南农业大学 | Real-time measurement method and device for attitude of motion carrier |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101915580A (en) * | 2010-07-14 | 2010-12-15 | 中国科学院自动化研究所 | Self-adaptation three-dimensional attitude positioning method based on microinertia and geomagnetic technology |
| WO2012068359A2 (en) * | 2010-11-17 | 2012-05-24 | Hillcrest Laboratories, Inc. | Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic |
| CN104215244A (en) * | 2014-08-22 | 2014-12-17 | 南京航空航天大学 | Aerospace vehicle combined navigation robust filtering method based on launching inertia coordinate system |
| CN104898681A (en) * | 2015-05-04 | 2015-09-09 | 浙江工业大学 | Tetra-rotor aircraft attitude obtaining method by use of three-order approximation Picard quaternion |
-
2016
- 2016-04-08 CN CN201610216667.4A patent/CN105890598B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101915580A (en) * | 2010-07-14 | 2010-12-15 | 中国科学院自动化研究所 | Self-adaptation three-dimensional attitude positioning method based on microinertia and geomagnetic technology |
| WO2012068359A2 (en) * | 2010-11-17 | 2012-05-24 | Hillcrest Laboratories, Inc. | Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic |
| CN104215244A (en) * | 2014-08-22 | 2014-12-17 | 南京航空航天大学 | Aerospace vehicle combined navigation robust filtering method based on launching inertia coordinate system |
| CN104898681A (en) * | 2015-05-04 | 2015-09-09 | 浙江工业大学 | Tetra-rotor aircraft attitude obtaining method by use of three-order approximation Picard quaternion |
Non-Patent Citations (1)
| Title |
|---|
| 基于共轭梯度法和互补滤波相结合的姿态解算算法;孙金秋等;《传感技术学报》;20140430;第27卷(第4期);第524-528页 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105890598A (en) | 2016-08-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105890598B (en) | Quadrotor attitude algorithm method of the conjugate gradient in conjunction with Extended Kalman filter | |
| CN106708066B (en) | View-based access control model/inertial navigation unmanned plane independent landing method | |
| CN103285599B (en) | A kind of method by remote control means navigator's target drone directly perceived | |
| Wang et al. | Nonlinear double-integral observer and application to quadrotor aircraft | |
| CN111351482B (en) | Multi-rotor aircraft combined navigation method based on error state Kalman filtering | |
| CN108225308A (en) | A kind of attitude algorithm method of the expanded Kalman filtration algorithm based on quaternary number | |
| CN107314718A (en) | High speed rotating missile Attitude estimation method based on magnetic survey rolling angular rate information | |
| Cheviron et al. | Robust nonlinear fusion of inertial and visual data for position, velocity and attitude estimation of UAV | |
| CN107478223A (en) | A kind of human body attitude calculation method based on quaternary number and Kalman filtering | |
| CN109506646A (en) | A kind of the UAV Attitude calculation method and system of dual controller | |
| CN105094138A (en) | Low-altitude autonomous navigation system for rotary-wing unmanned plane | |
| CN105973238B (en) | A kind of attitude of flight vehicle method of estimation based on norm constraint volume Kalman filtering | |
| CN108827313A (en) | Multi-mode rotor craft Attitude estimation method based on extended Kalman filter | |
| CN105929836B (en) | Control method for quadrotor | |
| CN108318038A (en) | A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method | |
| Poddar et al. | PSO aided adaptive complementary filter for attitude estimation | |
| CN108170154A (en) | A kind of unmanned plane multisensor forward direction photography, which tilts, flies control adjustment method | |
| CN107782309A (en) | Noninertial system vision and double tops instrument multi tate CKF fusion attitude measurement methods | |
| CN113670314B (en) | Unmanned aerial vehicle attitude estimation method based on PI self-adaptive two-stage Kalman filtering | |
| CN110377056A (en) | Unmanned plane course angle Initialization Algorithms and unmanned plane | |
| CN108534772A (en) | Attitude angle acquisition methods and device | |
| CN106352897A (en) | Silicon MEMS (micro-electromechanical system) gyroscope error estimating and correcting method based on monocular visual sensor | |
| Agarwal et al. | Evaluation of a commercially available autonomous visual inertial odometry solution for indoor navigation | |
| CN113022898A (en) | State estimation method for flexible attachment system in weak gravity environment | |
| Ramirez et al. | Stability analysis of a vision-based UAV controller: An application to autonomous road following missions |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |