CN114608517B - Gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation - Google Patents
Gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation Download PDFInfo
- Publication number
- CN114608517B CN114608517B CN202210247916.1A CN202210247916A CN114608517B CN 114608517 B CN114608517 B CN 114608517B CN 202210247916 A CN202210247916 A CN 202210247916A CN 114608517 B CN114608517 B CN 114608517B
- Authority
- CN
- China
- Prior art keywords
- calculating
- quaternion
- angular velocity
- gyroscope
- value
- 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
- 238000000034 method Methods 0.000 title claims abstract description 38
- 239000011159 matrix material Substances 0.000 claims abstract description 55
- 238000005259 measurement Methods 0.000 claims abstract description 45
- 230000001133 acceleration Effects 0.000 claims abstract description 15
- 238000001914 filtration Methods 0.000 claims abstract description 14
- 239000013598 vector Substances 0.000 claims description 41
- 238000012937 correction Methods 0.000 claims description 40
- 238000012546 transfer Methods 0.000 claims description 13
- 230000005484 gravity Effects 0.000 claims description 11
- 230000003044 adaptive effect Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 238000013461 design Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000006978 adaptation Effects 0.000 claims description 2
- 238000013178 mathematical model Methods 0.000 abstract description 2
- 230000035772 mutation Effects 0.000 abstract description 2
- 241000196324 Embryophyta Species 0.000 description 9
- 239000003814 drug Substances 0.000 description 5
- 230000000295 complement effect Effects 0.000 description 4
- 229940079593 drug Drugs 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000026676 system process Effects 0.000 description 2
- 241000607479 Yersinia pestis Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 201000010099 disease Diseases 0.000 description 1
- 208000037265 diseases, disorders, signs and symptoms Diseases 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002420 orchard Substances 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Abstract
The invention discloses a gesture resolving method applied to plant protection operation of an agricultural unmanned aerial vehicle, which aims at the problem that the accuracy of the traditional filtering method is reduced due to the influence of interference factors such as a natural wind field when the agricultural unmanned aerial vehicle performs plant protection operation outdoors, and provides a method for correcting the angular velocity output by a gyroscope by utilizing the information of acceleration and magnetic field intensity, taking gesture quaternion resolved according to the corrected angular velocity as a measurement value, taking the angular velocity information output by the gyroscope and the gesture quaternion as state quantities, establishing a mathematical model of self-adaptive extended Kalman filtering, introducing self-adaptive factors into a system noise covariance matrix and a measurement noise covariance matrix, and improving the robustness of an algorithm in state mutation and the accuracy of unmanned aerial vehicle gesture resolving.
Description
Technical Field
The invention belongs to the field of unmanned aerial vehicle control and inertial navigation, relates to measurement, strapdown inertial navigation and filtering technologies, and particularly relates to a gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation.
Background
At present, the application of the multi-rotor unmanned aerial vehicle is more and more extensive, especially in the agricultural field, and along with the development of agricultural modernization, the application of the unmanned aerial vehicle to develop plant protection operation can more efficiently help farmers to prevent and treat diseases, weeds and pests.
However, in the occasions needing targeted drug application such as mountain orchards, due to the influence of natural wind fields, drug drops sprayed by the four-rotor unmanned aerial vehicle often deviate from fruit trees needing drug application, so that the problems of uneven drug application and even off-target are caused. To solve this problem just needs to realize unmanned aerial vehicle's high accuracy control, and can know by four rotor unmanned aerial vehicle's flight characteristics, unmanned aerial vehicle's gesture can directly influence unmanned aerial vehicle's flight position, and then influences medicine drip point, therefore unmanned aerial vehicle's accurate gesture information acquisition is crucial. The accelerometer, the gyroscope and the magnetometer are main sensors for acquiring attitude information of the unmanned aerial vehicle, but the three sensors have respective defects, wherein the gyroscope is easy to be influenced by temperature, temperature drift exists, the accelerometer is easy to be influenced by high-frequency micro vibration, and the magnetometer is easy to be interfered by surrounding magnetic fields. Therefore, in order to improve the attitude resolving precision of the unmanned aerial vehicle, it is not feasible to rely on a single sensor, and the data information acquired by the three sensors needs to be fused.
The existing unmanned aerial vehicle multi-sensor data fusion algorithm mainly comprises complementary filtering, and according to noise of different frequency domain bands of an accelerometer, a gyroscope and a magnetometer, different filters are designed to perform data fusion processing on values output by sensors, so that effective expected data can be obtained. The method has a certain limitation that low-frequency noise of the gyroscope is filtered through the high-pass filter, high-frequency noise of the accelerometer is filtered through the low-pass filter, however, the switching frequency of the low-pass filter and the high-pass filter is not well determined, a large amount of experiments are required to be obtained, and meanwhile, complementary filtering is only applicable under the condition that sensor noise has different frequencies, and an agricultural unmanned aerial vehicle can be interfered by a natural wind field during plant protection operation, so that noise carried in signals output by the sensor cannot be guaranteed to have different frequencies, and the accuracy of calculated attitude data is reduced.
Disclosure of Invention
In order to solve the problems, the invention discloses a gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation, which corrects the angular velocity output by a gyroscope by utilizing the information of acceleration and magnetic field intensity, takes a gesture quaternion resolved according to the corrected angular velocity as a measurement value, establishes a mathematical model of a self-adaptive extended Kalman filter (Adaptive Extended Kalman Filtering, AEKF), introduces self-adaptive factors into a system noise covariance matrix and a measured noise covariance matrix, and improves the robustness of an algorithm in state mutation and the gesture resolving precision of the unmanned aerial vehicle.
In order to achieve the above purpose, the technical scheme of the invention is as follows:
a gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation comprises the following steps:
1) Calculating an initial attitude angle: establishing a relation between the gravity acceleration and the measured value of the accelerometer according to the direction cosine matrix, and calculating an initial roll angle, a pitch angle and a yaw angle according to the relation between the direction cosine matrix and the measured value of the magnetometer;
2) Angular velocity correction: calculating a correction vector by utilizing information output by the accelerometer and the magnetometer, obtaining a correction value of the angular speed of the gyroscope through the PI controller, and adding the correction value with data output by the gyroscope;
3) And (3) gesture resolving: according to the data output by the gyroscope, solving a four-element differential equation to obtain a posture quaternion Q g Solving a quaternion differential equation according to the corrected angular velocity to obtain a posture quaternion Q fused with data information of the accelerometer, the magnetometer and the gyroscope r ；
4) AEKF filter design: the updated gesture quaternion Q obtained in the step 3) is processed r Establishing a measurement equation as a measurement value, establishing a state equation by taking a posture quaternion and random drift of a gyroscope as state vectors, and introducing self-adaptive factors into a system noise covariance matrix and a measurement noise covariance matrix;
5) And performing time updating and measurement updating on the AEKF algorithm to obtain the quaternion after filtering correction, and converting the quaternion into an attitude angle according to the relationship between the quaternion and the attitude angle.
Further, the specific step of calculating the initial attitude angle in the step 1) includes:
(1.1) establishing a conversion relation between a navigation coordinate system and a machine body coordinate system: taking the geographic coordinate system as a navigation coordinate system n, wherein the direction is north-east-earth, and the direction of the machine body coordinate system is front-right-down, and then the direction cosine matrixCan be expressed as:
(1.2) calculating roll angle, pitch angle and yaw angle: accelerometer measurement value [ a ] can be obtained according to the direction cosine matrix bx ，a by ，a bz ] T With gravitational acceleration [0, -g] T Is the relation of:
the transverse rolling angle phi and the pitch angle theta of the unmanned aerial vehicle are as follows:
converting the value output by the magnetometer into a navigation coordinate system, and calculating the magnetic field intensity under the n system:
wherein [ m ] bx ，m by ，m bz ] T Magnetic field strength m in machine body coordinate system output by magnetometer b Thereby calculating the heading angle ψ:
further, the specific step of correcting the angular velocity in the step 2) includes:
(2.1) calculating an error vector: using the direction cosine matrix obtained in step 1)The unit gravity acceleration [0, -1 ] under the navigation coordinate system n] T Conversion to body coordinate system b to obtain k 1 ：
Based on the accelerometer measurement a and the gravity vector k in the body coordinate system 1 Calculating the correction amount alpha 1 :
α 1 ＝a×k 1
From magnetometer output m b Calculating the magnetic field strength m under n series n ：
Projecting it to the n-series horizontal plane yields β:
calculating the deviation k of the yaw angle from the horizontal 2 Calculating the correction amount alpha 2 ：
Error vector: e=α 1 +α 2 。
(2.2) calculating a correction amount: inputting the error vector e into the PI controller, and adjusting the proportionality coefficient K of the PI controller p And integral coefficient K i The output value of the PI controller is the angular velocity correction δw
According to the data output by the gyroscope, obtaining an angular velocity correction value fused with accelerometer and magnetometer information: w=w g +δw, where w g The angular velocity output for the gyroscope.
Further, the specific step of gesture resolving in the step 3) includes:
(3.1) establishing a quaternion differential equation: establishing a quaternion differential equation according to the operation relation between the quaternion Q and the body angular velocity w:
wherein: q= { Q 0 q 1 q 2 q 3 ] T
(3.2) solving for Q with angular velocity data taken into the gyroscope output g ：
Wherein,and +.>Outputting angular velocity omega for gyroscopes g And converts the initial attitude angle obtained in the step (1.2) into an initial attitude quaternion Q 0 ：
Solving the differential equation to obtain Q g 。
(3.3) solving for Q based on the output value of step 2) r : solving a quaternion differential equation according to the angular velocity after the correction in the step (2.2) to obtain Q r
The solution of the differential equation is Q r WhereinAnd +.>Is the triaxial component of the angular velocity w after correction.
Further, the step 4) of AEKF filter design specifically includes: (4.1) establishing a nonlinear discretized state equation:
x k ＝f(x k-1 ，k-1)+W k-1
wherein: x is X k The state vector of the system at the moment k is composed of an attitude quaternion and a random drift vector of a gyroscope, and X= [ q ] 0 ，q 1 ，q 2 ，q 3 ，b wx ，b wy ，b wz ] T ，b wx ，b wy ，b wz Random drift vectors of gyroscopes around a roll axis, a pitch axis and a heading axis, W k Representing system process noise.
(4.2) establishing a discretized measurement equation:
Z k ＝h(X k ，k)+V k
wherein: z is Z k An observation vector representing the system at time k, and a posture quaternion Q output by Mahony r Constitution, Z= [ q ] 0 ，q 1 ，q 2 ，q 3 ] T ，v k Representing the measured noise.
(4.3) introduced adaptation factor: observed noise covariance matrix R k The adaptive factor of (a) is:wherein s is i * ＝max{1，S ii }，S ii Represent S k Is the ith diagonal element of (S) k The method comprises the following steps:system transfer noise covariance Q k Is a self-adaptive factor of (a):wherein: />Representing the innovation sequence covariance.
Further, the specific steps of AEKF algorithm time updating and measurement updating in the step 5) include:
(5.1) initial value calculation: calculating a state initial value X 0 Initial value of error variance matrix P 0 Initial value of process noise covariance Q 0 Measuring the initial value R of the noise covariance 0 。
(5.2) time update: calculating Jacobi matrix of state transfer function:
the state one-step predicted value can be calculated according to the state transfer function:
updating system transfer noise covariance Q k-1 ：
Calculating a state vector one-step prediction covariance matrix:
(5.3) measurement update: jacobi matrix for calculating measurement function
Calculating one-step predictive value of measurement vector according to measurement function
Updating observed noise covariance matrix R k ：
s i * ＝max{1，S ii }
Calculating a state gain matrix:
estimation value of system state vector at k time:
updating a state error covariance matrix:
P k ＝(I-K k H k )P k|k-1 。
the beneficial effects of the invention are as follows:
(1) According to the invention, the attitude angle error calculated by combining the magnetometer and the accelerometer is smaller, more accurate measurement information can be provided, compared with the traditional complementary filtering method, the suppression effect of attitude divergence caused by gyroscope drift can be improved, and the accuracy of unmanned plane attitude calculation is improved;
(2) According to the angular velocity correction method provided by the invention, the angular velocity of the machine body is corrected by using the acceleration and magnetometer information, and the output attitude quaternion is used as a measurement value, so that not only can the high-frequency noise generated by vibration of the machine body be effectively filtered, but also the interference of non-gravity acceleration generated by the unmanned aerial vehicle during acceleration movement can be effectively restrained;
(3) The invention aims at the problem that in the actual process, system state noise and measurement noise are not fixed and unchanged due to the influence of a natural wind field and various disturbance factors when an agricultural unmanned aerial vehicle performs plant protection operation outdoors, and transfers a noise covariance matrix Q to a system k And an observation noise covariance matrix R k Self-adaptive factors are added, and Q can be automatically adjusted in time in the filtering process k And R is k The filtering precision is improved;
(4) Compared with the traditional complementary filtering method, the method does not need to carry out a large number of experiments to determine the proper switching frequency, improves the design efficiency and saves the time.
Drawings
FIG. 1 is a general flow chart of the method of the present invention;
FIG. 2 is a flow chart of the angular velocity correction according to the present invention;
fig. 3 is a flow chart of the AEKF algorithm in the present invention.
Detailed Description
The present invention is further illustrated in the following drawings and detailed description, which are to be understood as being merely illustrative of the invention and not limiting the scope of the invention.
As shown in fig. 1-3, the present embodiment provides a gesture resolving method applied to a plant protection operation of an agricultural unmanned aerial vehicle, the method includes the following steps:
step 1: and calculating an initial attitude angle, establishing a relationship between the gravity acceleration and the measured value of the accelerometer according to a direction cosine matrix, and calculating an initial roll angle, a pitch angle and a yaw angle according to the relationship between the direction cosine matrix and the measured value of the magnetometer.
Step 2: and correcting the angular velocity, calculating a correction vector by using information output by the accelerometer and the magnetometer, obtaining a correction value of the angular velocity of the gyroscope through the PI controller, and adding the correction value with data output by the gyroscope.
Step 3: solving the attitude and the quaternion Q by solving the quaternion differential equation according to the data output by the gyroscope g Solving a quaternion differential equation according to the corrected angular velocity to obtain a posture quaternion Q fused with data information of the accelerometer, the magnetometer and the gyroscope r 。
Step 4: AEKF filter design, and updated attitude quaternion Q obtained in step 3 r And establishing a measurement equation as a measurement value, establishing a state equation by taking a posture quaternion and random drift of a gyroscope as state vectors, and introducing self-adaptive factors into a system noise covariance matrix and a measurement noise covariance matrix.
Step 5: and performing time updating and measurement updating on the AEKF algorithm to obtain the quaternion after filtering correction, and converting the quaternion into an attitude angle according to the relationship between the quaternion and the attitude angle.
Wherein, step 1: calculating an initial attitude angle, establishing a relationship between the gravity acceleration and the measured value of the accelerometer according to a direction cosine matrix, and calculating an initial roll angle, an initial angle and an initial yaw angle according to the relationship between the direction cosine matrix and the measured value of the magnetometer; the method comprises the following steps:
1) Establishing a conversion relation between a navigation coordinate system and a machine body coordinate system: taking the geographic coordinate system as a navigation coordinate system n, wherein the direction is north-east-earth, and the direction of the machine body coordinate system is front-right-down, and then the direction cosine matrixThe method comprises the following steps:
2) Calculating a roll angle, a pitch angle and a yaw angle: obtaining accelerometer measurements from a directional cosine matrix [ a ] bx ，a by ，a bz ] T With gravitational acceleration [0, -g] T Is the relation of:
the transverse rolling angle phi and the pitch angle theta of the unmanned aerial vehicle are as follows:
the value output by the magnetometer is converted into a navigation coordinate system, and the magnetic field intensity under the n system is calculated:
wherein [ m ] bx ，m by ，m bz ] T Magnetic field strength m in machine body coordinate system output by magnetometer b Thereby calculating the heading angle ψ:
step 2: angular velocity correction, namely calculating a correction vector by utilizing information output by an accelerometer and a magnetometer, obtaining a correction value of the angular velocity of the gyroscope through a PI controller, and adding the correction value with data output by the gyroscope; the method comprises the following steps:
1) Calculating an error vector: using the direction cosine matrix obtained in step 1)The unit gravity acceleration [0, -1 ] under the navigation coordinate system n] T Conversion to body coordinate system b to obtain k 1 ：
Based on the accelerometer measurement a and the gravity vector k in the body coordinate system 1 Calculating the correction amount alpha 1 :
α 1 ＝a×k 1
From magnetometer output m b Calculating the magnetic field strength m under n series n ：
Projection onto the n-series horizontal plane yields β:
calculating the deviation sigma of the yaw angle on the horizontal plane and calculating the correction alpha 2 ：
Error vector: e=α 1 +α 2 。
2) Solving for an angular velocity correction amount: inputting the error vector e into the PI controller, and adjusting the proportionality coefficient K of the PI controller p And integral coefficient K i The PI controller output value is the angular velocity correction δw:
according to the data output by the gyroscope, obtaining an angular velocity correction value fused with accelerometer and magnetometer information: w=w g +δw, where w g The angular velocity output for the gyroscope.
Step 3: solving the attitude and the quaternion Q by solving the quaternion differential equation according to the data output by the gyroscope g Solving a quaternion differential equation according to the corrected angular velocity to obtain the accelerometer and magnetometer fusedAttitude quaternion Q of gyroscope data information r The method comprises the steps of carrying out a first treatment on the surface of the The method comprises the following steps:
1) Establishing a quaternion differential equation: establishing a quaternion differential equation according to the operation relation between the quaternion Q and the body angular velocity w:
wherein: q= { Q 0 q 1 q 2 q 3 ] T
2) Solving Q according to angular velocity data output by gyroscope g ：
Wherein,and +.>Output angular velocity w for gyroscope g The initial attitude angle obtained in the step 1 is converted into an initial attitude quaternion Q 0 ：
Solving the differential equation to obtain Q g 。
3) Solving Q according to the output value of the step 2 r : solving quaternion differential equation according to the corrected angular velocity in the step 2
The solution of the differential equation is Q r WhereinAnd +.>Is the triaxial component of the angular velocity w after correction.
Step 4: AEKF filter design, the updated gesture quaternion Q r Establishing a measurement equation as a measurement value, establishing a state equation by taking a posture quaternion and random drift of a gyroscope as state vectors, and introducing self-adaptive factors into a system noise covariance matrix and a measurement noise covariance matrix; the method comprises the following steps:
1) Establishing a nonlinear discretization state equation:
X k ＝f(X k-1 ，k-1)+W k-1
wherein: x is X k The state vector of the system at the moment k is composed of an attitude quaternion and a random drift vector of a gyroscope, and X= [ q ] 0 ，q 1 ，q 2 ，q 3 ，b wx ，b wy ，b wz ] T ，b wx ，b wy ，b wz Random drift vectors of gyroscopes around a roll axis, a pitch axis and a heading axis, W k Representing system process noise.
2) Establishing a discretized measurement equation:
Z k ＝h(X k ，k)+V k wherein:
Z k an observation vector representing the system at time k, and a posture quaternion Q output by Mahony r Constitution, Z= [ q ] 0 ，q 1 ，q 2 ，q 3 ] T ，v k Representing the measured noise.
3) The introduced adaptive factor: observed noise covariance matrix R k The adaptive factor of (a) isWherein s is i * ＝max{1，S ii }，S ii Represent S k Is the ith diagonal element of (S) k The method comprises the following steps:system transfer noise covariance Q k The adaptive factor of (a) is:
step 5: performing time updating and measurement updating on the AEKF algorithm to obtain a quaternion after filtering correction, and converting the quaternion into an attitude angle according to the relationship between the quaternion and the attitude angle; the method comprises the following steps:
1) Calculating an initial value: calculating a state initial value X 0 Error variance matrix P 0 Initial value of process noise covariance Q 0 Measuring the initial value R of the noise covariance 0 。
2) And (5) updating time: calculating Jacobi matrix of state transfer function:
the state one-step predicted value can be calculated according to the state transfer function:
updating system transfer noise covariance Q k-1 ：
Calculating a state vector one-step prediction covariance matrix:
3) And (5) measurement and update: calculating Jacobi matrix of the measurement function:
calculating one-step predictive value of measurement vector according to measurement function
Updating observed noise covariance matrix R k ：
s i * ＝max{1，S ii }
Calculating a state gain matrix:
estimation value of system state vector at k time:
updating a state error covariance matrix:
P k ＝(I-K k H k )P k|k-1 。
it should be noted that the foregoing merely illustrates the technical idea of the present invention and is not intended to limit the scope of the present invention, and that a person skilled in the art may make several improvements and modifications without departing from the principles of the present invention, which fall within the scope of the claims of the present invention.
Claims (1)
1. The gesture resolving method applied to the plant protection operation of the agricultural unmanned aerial vehicle is characterized by comprising the following steps of:
1) Calculating an initial attitude angle: establishing a relation between the gravity acceleration and the measured value of the accelerometer according to the direction cosine matrix, and calculating an initial roll angle, a pitch angle and a yaw angle according to the relation between the direction cosine matrix and the measured value of the magnetometer;
the method comprises the following specific steps:
(1.1) establishing a conversion relation between a navigation coordinate system and a machine body coordinate system: taking the geographic coordinate system as a navigation coordinate system n, wherein the direction is north-east-earth, and the direction of the machine body coordinate system is front-right-down, and then the direction cosine matrixExpressed as:
(1.2) calculating roll angle, pitch angle and yaw angle: accelerometer measurement value [ a ] can be obtained according to the direction cosine matrix bx ，a by ，a bz ] T With gravitational acceleration [0, -g] T Is the relation of:
unmanned aerial vehicle roll angle phi and pitch angle theta
Converting the value output by the magnetometer into a navigation coordinate system, and calculating the magnetic field intensity under the n system:
wherein [ m ] bx ，m by ，m bz ] T Magnetic field strength m in machine body coordinate system output by magnetometer b Thereby calculating the heading angle ψ:
2) Angular velocity correction: calculating a correction vector by utilizing information output by the accelerometer and the magnetometer, obtaining a correction value of the angular speed of the gyroscope through the PI controller, and adding the correction value with data output by the gyroscope;
the method comprises the following specific steps:
(2.1) calculating an error vector: using the direction cosine matrix obtained in step 1)The unit gravity acceleration [0, -1 ] under the navigation coordinate system n] T Conversion to body coordinate system b to obtain k 1 ：
Based on the accelerometer measurement a and the gravity vector k in the body coordinate system 1 Calculating the correction amount alpha 1 :
α 1 ＝a×k 1
From magnetometer output m b Calculating the magnetic field strength m under n series n ：
Projecting it to the n-series horizontal plane yields β:
calculating the deviation k of the yaw angle from the horizontal 2 Calculating the correction amount alpha 2 ：
Error vector: e=α 1 +α 2
(2.2) calculating a correction amount: inputting the error vector e into the PI controller, and adjusting the proportionality coefficient K of the PI controller p And integral coefficient K i The PI controller output value is the angular velocity correction δw:
according to the data output by the gyroscope, obtaining an angular velocity correction value fused with accelerometer and magnetometer information: w=w g +δw, where w g Angular velocity output for the gyroscope;
3) And (3) gesture resolving: according to the data output by the gyroscope, solving a four-element differential equation to obtain a posture quaternion Q g Solving a quaternion differential equation according to the corrected angular velocity to obtain a posture quaternion Q fused with data information of the accelerometer, the magnetometer and the gyroscope r ：
The method comprises the following specific steps:
(3.1) establishing a quaternion differential equation: establishing a quaternion differential equation according to the operation relation between the quaternion Q and the body angular velocity w:
wherein: q= [ Q ] 0 q 1 q 2 q 3 ] T
(3.2) solving for Q with angular velocity data taken into the gyroscope output g ：
Wherein,and +.>Output angular velocity w for gyroscope g The three-axis component of (2) is converted into an initial attitude quaternion Q 0 ：
Solving the differential equation to obtain Q g ，
(3.3) solving for Q based on the output value of step 2) r : solving quaternion differential equations based on the angular velocity after correction in step (2.2)
The solution of the differential equation is Q r WhereinAnd +.>A triaxial component which is the corrected angular velocity w;
4) AEKF filter design: the updated gesture quaternion Q obtained in the step 3) is processed r Establishing a measurement equation as a measurement value, establishing a state equation by taking a posture quaternion and random drift of a gyroscope as state vectors, and introducing self-adaptive factors into a system noise covariance matrix and a measurement noise covariance matrix;
the method comprises the following specific steps:
(4.1) establishing a nonlinear discretized state equation:
X k ＝f(X k-1 ，k-1)+W k-1
wherein: x is X k The state vector of the system at the moment k is composed of an attitude quaternion and a random drift vector of a gyroscope, and X= [ q ] 0 ，q 1 ，q 2 ，q 3 ，b wx ，b wy ，b wz ] T ，b wx ，b wy ，b wz Random drift vectors of gyroscopes around a roll axis, a pitch axis and a heading axis, W k Representing the noise of the process of the system,
(4.2) establishing a discretized measurement equation:
Z k ＝h(X k ，k)+V k
wherein: z is Z k An observation vector representing the system at time k, and a posture quaternion Q output by Mahony r Constitution, Z= [ q ] 0 ，q 1 ，q 2 ，q 3 ] T ，v k Indicating that the measured noise is a high level of noise,
(4.3) introduced adaptation factor: observed noise covariance matrix R k The adaptive factor of (a) is:wherein s is i * ＝max{1，S ii }，S ii Represent S k Is the ith diagonal element of (S) k The method comprises the following steps:system transfer noise covariance Q k Is a self-adaptive factor of (a):wherein: />Representing the innovation sequence covariance;
5) Performing time updating and measurement updating on the AEKF algorithm to obtain a quaternion after filtering correction, and converting the quaternion into an attitude angle according to the relationship between the quaternion and the attitude angle;
the method comprises the following specific steps:
(5.1) initial value calculation: calculating a state initial value X 0 Error variance matrix P 0 Initial value of process noise covariance Q 0 Measuring the initial value R of the noise covariance 0 ，
(5.2) time update: calculating Jacobi matrix of state transfer function:
the state one-step predicted value can be calculated according to the state transfer function:
updating system transfer noise covariance Q k-1 ：
Calculating a state vector one-step prediction covariance matrix:
(5.3) measurement update: calculating Jacobi matrix of the measurement function:
calculating one-step predictive value of measurement vector according to measurement function
Updating observed noise covariance matrix R k ：
s i * ＝max{1，S ii }
Calculating a state gain matrix:
estimation value of system state vector at k time:
updating a state error covariance matrix:
P k ＝(I-K k H k )P k|k-1 。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210247916.1A CN114608517B (en) | 2022-03-14 | 2022-03-14 | Gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210247916.1A CN114608517B (en) | 2022-03-14 | 2022-03-14 | Gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114608517A CN114608517A (en) | 2022-06-10 |
| CN114608517B true CN114608517B (en) | 2023-11-10 |
Family
ID=81862490
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210247916.1A Active CN114608517B (en) | 2022-03-14 | 2022-03-14 | Gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114608517B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115855038B (en) * | 2022-11-22 | 2024-01-09 | 哈尔滨工程大学 | Short-time high-precision posture maintaining method |
| CN117249811B (en) * | 2023-11-20 | 2024-03-29 | 中国建筑一局(集团)有限公司 | Distributed inclination monitoring system and method for super high-rise building |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107478223A (en) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | A kind of human body attitude calculation method based on quaternary number and Kalman filtering |
| US20180231374A1 (en) * | 2017-02-13 | 2018-08-16 | National Tsing Hua University | Object pose measurement system based on mems imu and method thereof |
| CN110174121A (en) * | 2019-04-30 | 2019-08-27 | 西北工业大学 | A kind of aviation attitude system attitude algorithm method based on earth's magnetic field adaptive correction |
-
2022
- 2022-03-14 CN CN202210247916.1A patent/CN114608517B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107478223A (en) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | A kind of human body attitude calculation method based on quaternary number and Kalman filtering |
| US20180231374A1 (en) * | 2017-02-13 | 2018-08-16 | National Tsing Hua University | Object pose measurement system based on mems imu and method thereof |
| CN110174121A (en) * | 2019-04-30 | 2019-08-27 | 西北工业大学 | A kind of aviation attitude system attitude algorithm method based on earth's magnetic field adaptive correction |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114608517A (en) | 2022-06-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114608517B (en) | Gesture resolving method applied to agricultural unmanned aerial vehicle plant protection operation | |
| KR102017404B1 (en) | How to update the full posture angle of agricultural machinery based on 9 axis MEMS sensor | |
| CN108225308B (en) | Quaternion-based attitude calculation method for extended Kalman filtering algorithm | |
| CN108827299B (en) | Aircraft attitude calculation method based on improved quaternion second-order complementary filtering | |
| CN104197927B (en) | Submerged structure detects robot real-time navigation system and method | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| CN106643737B (en) | Four-rotor aircraft attitude calculation method in wind power interference environment | |
| CN111351482B (en) | Multi-rotor aircraft combined navigation method based on error state Kalman filtering | |
| CN109974714B (en) | Sage-Husa self-adaptive unscented Kalman filtering attitude data fusion method | |
| WO2017063388A1 (en) | A method for initial alignment of an inertial navigation apparatus | |
| CN110887481B (en) | Carrier dynamic attitude estimation method based on MEMS inertial sensor | |
| CN107607113B (en) | Method for measuring inclination angles of two-axis attitude | |
| CN109141475B (en) | Initial alignment method for SINS robust traveling under assistance of DVL (dynamic velocity logging) | |
| CN112629538A (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
| Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
| CN106403952A (en) | Method for measuring combined attitudes of Satcom on the move with low cost | |
| CN112798021B (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
| JP2007232443A (en) | Inertia navigation system and its error correction method | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN112683269A (en) | MARG attitude calculation method with motion acceleration compensation | |
| CN110793515A (en) | Unmanned aerial vehicle attitude estimation method based on single-antenna GPS and IMU under large-mobility condition | |
| CN108871319B (en) | Attitude calculation method based on earth gravity field and earth magnetic field sequential correction | |
| CN109211232B (en) | Shell attitude estimation method based on least square filtering | |
| CN113155129B (en) | Holder attitude estimation method based on extended Kalman filtering | |
| CN111679681B (en) | Underwater robot motion path tracking method based on sliding mode robust control |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |