CN112304312A - Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF - Google Patents
Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF Download PDFInfo
- Publication number
- CN112304312A CN112304312A CN202010982173.3A CN202010982173A CN112304312A CN 112304312 A CN112304312 A CN 112304312A CN 202010982173 A CN202010982173 A CN 202010982173A CN 112304312 A CN112304312 A CN 112304312A
- Authority
- CN
- China
- Prior art keywords
- vel
- gyro
- acc
- data
- angular velocity
- 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.)
- Granted
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 53
- 238000000034 method Methods 0.000 title claims abstract description 48
- 230000001133 acceleration Effects 0.000 claims abstract description 40
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 22
- 238000001914 filtration Methods 0.000 claims abstract description 17
- 238000012545 processing Methods 0.000 claims abstract description 11
- 238000005259 measurement Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims abstract description 9
- 238000012417 linear regression Methods 0.000 claims description 6
- 238000007781 pre-processing Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 3
- 230000035772 mutation Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 5
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000013480 data collection Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
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
-
- 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
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)
Abstract
The invention discloses a method and a system for resolving the attitude of an unmanned aerial vehicle based on a least square method and an EKF (extended Kalman Filter), comprising S100, acquiring state parameter values of the flight state of the unmanned aerial vehicle based on an IMU (inertial measurement Unit) data sensor, wherein the state parameter values comprise an angular velocity value and a linear acceleration value; s200, fitting and interpolating calculation are carried out on the angular velocity value and the linear acceleration value based on a least square method, and a state space equation about the angular velocity and the acceleration is established; s300, importing IMU data subjected to least square calculation processing into an extended Kalman filtering algorithm module, and carrying out attitude calculation on the attitude of the unmanned aerial vehicle. According to the method, based on the angular velocity value and the acceleration value processed by the least square method, a state space equation and a measurement equation about the angular velocity and the acceleration are established, and based on the equations and the corresponding Jacobian matrix, an attitude estimation module is established through an extended Kalman filtering algorithm theory, so that the attitude of the unmanned aerial vehicle is solved, and the influence of noise and mutation values in data on the accuracy and stability of the solved attitude is effectively reduced.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicles, in particular to an unmanned aerial vehicle attitude calculation method and system based on a least square method and EKF.
Background
The unmanned aerial vehicle is an airplane which does not contain a manned function and is controlled by radio or controlled by a program of the unmanned aerial vehicle. The flight control algorithm with good stability and high precision can not be widely applied in the fields of plant protection, military, geological exploration and the like. The flight control algorithm mainly comprises three algorithm modules, namely a strapdown inertial navigation system, a Kalman filtering algorithm and a flight control PID (proportion integration differentiation), and the quality of the algorithm is the key for realizing stable flight. The Kalman filtering algorithm is used for completing state parameter estimation of the unmanned aerial vehicle based on the IMU module, the GPS and the anemometer, and attitude calculation of the unmanned aerial vehicle in the state estimation is the basis for realizing stable control of the unmanned aerial vehicle.
The defects and shortcomings of the prior art are as follows:
at present, an unmanned aerial vehicle attitude resolving method mainly comprises the steps of establishing a space state equation and estimating unmanned attitude and position information based on an extended Kalman filtering algorithm. The external data from the gyroscope and the accelerometer are adopted in the extended Kalman filtering algorithm, vibration and sound of the unmanned aerial vehicle can affect the quality of IMU data acquisition, noise or unsmooth exists in output position and attitude data, and therefore stability and stability of flight of the unmanned aerial vehicle are affected, quality and safety of an operation target and an operator are hidden danger, and popularization of unmanned products are affected.
Disclosure of Invention
The unmanned aerial vehicle attitude calculation method and system based on the least square method and the EKF can solve the problems of noise and unsmooth data in attitude calculation, thereby improving the stable and stable performance of the flight state of the unmanned aerial vehicle and promoting the development and popularization of the unmanned aerial vehicle.
In order to achieve the purpose, the invention adopts the following technical scheme:
a method for unmanned aerial vehicle attitude calculation based on a least square method and EKF comprises the following steps:
the method comprises the following steps:
s100, acquiring state parameter values of the flight state of the unmanned aerial vehicle based on an IMU data sensor, wherein the state parameter values comprise an angular velocity value and a linear acceleration value;
s200, fitting and interpolating calculation are carried out on the angular velocity value and the acceleration value based on a least square method, a state space equation and a measurement equation about the angular velocity and the acceleration are established, and an attitude estimation module is built through an extended Kalman filtering algorithm theory based on the equations and a corresponding Jacobian matrix;
s300, importing IMU data subjected to least square calculation processing into an attitude estimation module, and performing attitude calculation on the attitude of the unmanned aerial vehicle.
Further, the S200 specifically includes:
step 1:
acquiring flight state parameter values of the unmanned aerial vehicle through three groups of IMU sensors, taking angular velocity acquired by a gyroscope and linear acceleration acquired by an accelerometer as state input quantity for attitude calculation, and setting a data acquisition period of an inertial sensor;
wherein,
the gyroscope acquisition data is defined as follows,
Gyro_ang_vel[3][3]＝{Vel_gyro_rx1,Vel_gyro_ry1,Vel_gyro_rz1；Vel_gyro_rx2,Vel_gyro_ry2,Vel_gyro_rz2；Vel_gyro_rx3,Vel_gyro_ry3,Vel_gyro_rz3；}
the accelerometer acquisition data is defined as follows,
Acc_line_acc[3][3]＝{Acc_x1,Acc_y1,Acc_z1；Acc_x2,Acc_y2,Acc_z2；Acc_x3,Acc_y3,Acc_z3；}
wherein the data of the first set of IMUs is defined as follows,
vel _ gyro _ rx1, namely the angular speed value of the body around the X axis of a body coordinate system, and the unit is rad/s;
vel _ gyro _ ry1, namely the angular speed value of the machine body around the Y axis of the machine body coordinate system, and the unit is rad/s;
vel _ gyro _ rz1 angular speed value of the machine body around the Z axis of the machine body coordinate system, and the unit is rad/s;
acc _ X1, the linear velocity value of the machine body along the X axis of the machine body coordinate system, the unit is m/s ^ 2;
acc _ Y1, the linear velocity value of the machine body along the Y axis of the machine body coordinate system, the unit is m/s ^ 2;
acc _ Z1, the linear velocity value of the machine body along the Z axis of the machine body coordinate system, the unit is m/s ^ 2;
dt is a data acquisition period, dt is set to be 0.02, and the unit is s;
step 2:
dividing the state parameter values of 10 periods into calculation units M, and performing data preprocessing on angular velocity values Vel _ gyro _ rx1 around the X axis of a body coordinate system;
wherein:
the value of each sensor acquired in 10 sampling periods is one calculation unit, and the mth calculation unit is defined as W ═ W (W ═ W)10*m+0,w10*m+1,w10*m+2...w10*m+9)，
Wherein:
w＝(Vel_gyro_rx,Vel_gyro_ry,Vel_gyro_rz,Acc_x,Acc_y,Acc_z)，
M＝(Vel_gyro_rx10*m+0,Vel_gyro_rx10*m+1,Vel_gyro_rx10*m+2...Vel_gyro_rx10*m+9)，Time＝(0.00,0.02,...0.18)，
N＝10；
and step 3:
a linear regression equation of M with respect to Time is calculated based on the least square method,
the regression curve equation is calculated by using the least square method for the angular velocity around the X axis in the calculation unit W as follows:
is provided with
Solving the notations by the least squares coefficients can yield:
the m group of linear regression equations is determined, then the state parameter value of the corresponding point of each time value is calculated based on the equation, the other five state parameters of the IMU are calculated as above, the data values of the other two groups are calculated as follows, the state parameter values of the three groups of IMU after least square calculation are obtained, and the following steps are referred specifically;
and 4, step 4: the calculating unit calculates the angular velocity value of the corresponding moment according to the time parameter value
Wherein:
and 5: similarly performing operation calculation in the steps 2, 3 and 4 on Y-axis angular velocity data Vel _ gyro _ ry1, Z-axis angular velocity data Vel _ gyro _ rz1, X-axis acceleration data Acc _ X1, Y-axis acceleration data Acc _ Y1 and Z-axis acceleration data Acc _ Z1 acquired by the first group of IMUs to acquire corresponding calculated state parameter values;
step 6: and (5) similarly adopting the operation rooms of the step (2), the step (3), the step (4) and the step (5) for the data of the second group and the third group to obtain the corresponding calculated state parameter values.
On the other hand, the invention also discloses an unmanned aerial vehicle attitude calculation system based on a least square method and EKF, which comprises the following units:
the data acquisition unit is used for acquiring state parameter values of the flight state of the unmanned aerial vehicle based on the IMU data sensor, wherein the state parameter values comprise an angular velocity value and a linear acceleration value;
the data processing unit is used for performing fitting and interpolation calculation on the angular velocity value and the acceleration value based on a least square method, establishing a state space equation and a measurement equation related to the angular velocity and the acceleration, and building an attitude estimation module based on the equations and a corresponding Jacobian matrix through an extended Kalman filtering algorithm theory;
and the unmanned aerial vehicle attitude calculation unit is used for importing IMU data subjected to least square calculation into the attitude estimation module and calculating the attitude of the unmanned aerial vehicle.
According to the technical scheme, the unmanned aerial vehicle attitude calculation method based on the least square method and the EKF establishes a state space equation and a measurement equation about the angular velocity and the acceleration based on the angular velocity value and the acceleration value processed by the least square method, and establishes an attitude estimation module based on the equations and the corresponding Jacobian matrix through an extended Kalman filtering algorithm theory to calculate the attitude of the unmanned aerial vehicle, so that the influence of noise and mutation values in data on the accuracy and stability of the calculated attitude is effectively reduced.
Specifically, the beneficial effects of the invention are as follows:
(1) the stability of attitude resolving of extended Kalman filtering is improved;
(2) the influence of the inaccuracy of the covariance of the extended Kalman filtering algorithm and the model of the noise error variance on the accuracy of the attitude estimation is reduced;
(3) the least square method is combined with the IMU data preprocessing and the extended Kalman algorithm, the influence of data noise on later estimation is effectively reduced, the precision and the stability are effectively improved, and the basis is set for the stable flight cushion of the unmanned aerial vehicle.
Drawings
FIG. 1 is a flow chart of the method of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention.
As shown in fig. 1, the method for resolving the attitude of the unmanned aerial vehicle based on the least square method and the EKF according to this embodiment is mainly applicable to the field of unmanned aerial vehicles with multiple rotors, and requires that the unmanned aerial vehicle has at least three sets of 6-axis IMU data acquisition capabilities, and includes the following steps:
s100, acquiring state parameter values of the flight state of the unmanned aerial vehicle based on an IMU data sensor, wherein the state parameter values comprise an angular velocity value and a linear acceleration value;
s200, fitting and interpolating calculation are carried out on the angular velocity value and the acceleration value based on a least square method, a state space equation and a measurement equation about the angular velocity and the acceleration are established, and an attitude estimation module is built through an extended Kalman filtering algorithm theory based on the equations and a corresponding Jacobian matrix;
s300, importing IMU data subjected to least square calculation processing into an attitude estimation module, and performing attitude calculation on the attitude of the unmanned aerial vehicle.
The following is a detailed description:
step 1:
acquiring angular velocity values and linear acceleration values of the flight state of the unmanned aerial vehicle based on the three groups of IMU data sensors;
the method comprises the steps that flight state parameter values of the unmanned aerial vehicle are collected through three groups of IMU sensors (namely, three-axis gyroscopes and three-axis accelerometer integrated chips), angular velocities collected by the gyroscopes and linear accelerations collected by the accelerometers are used as state input quantities for attitude calculation, and data collection periods of inertial sensors are set;
wherein,
the gyroscope acquisition data is defined as follows,
Gyro_ang_vel[3][3]＝{Vel_gyro_rx1,Vel_gyro_ry1,Vel_gyro_rz1；Vel_gyro_rx2,Vel_gyro_ry2,Vel_gyro_rz2；Vel_gyro_rx3,Vel_gyro_ry3,Vel_gyro_rz3；}
the accelerometer acquisition data is defined as follows,
Acc_line_acc[3][3]＝{Acc_x1,Acc_y1,Acc_z1；Acc_x2,Acc_y2,Acc_z2；Acc_x3,Acc_y3,Acc_z3；}
wherein the data of the first set of IMUs is defined as follows,
vel _ gyro _ rx1, namely the angular speed value of the body around the X axis of a body coordinate system, and the unit is rad/s;
vel _ gyro _ ry1, namely the angular speed value of the machine body around the Y axis of the machine body coordinate system, and the unit is rad/s;
vel _ gyro _ rz1 angular speed value of the machine body around the Z axis of the machine body coordinate system, and the unit is rad/s;
acc _ X1, the linear velocity value of the machine body along the X axis of the machine body coordinate system, the unit is m/s ^ 2;
acc _ Y1, the linear velocity value of the machine body along the Y axis of the machine body coordinate system, the unit is m/s ^ 2;
acc _ Z1, the linear velocity value of the machine body along the Z axis of the machine body coordinate system, the unit is m/s ^ 2;
dt is the data acquisition period, and dt is set to 0.02 and is expressed in s.
Step 2:
dividing the state parameter values of 10 periods into calculation units M, and performing data preprocessing on angular velocity values Vel _ gyro _ rx1 around the X axis of a body coordinate system;
wherein:
the value of each sensor acquired in 10 sampling periods is one calculation unit, and the mth calculation unit may be defined as W ═ W (W ═ W)10*m+0,w10*m+1,w10*m+2...w10*m+9)，
Wherein:
w＝(Vel_gyro_rx,Vel_gyro_ry,Vel_gyro_rz,Acc_x,Acc_y,Acc_z)，
M＝(Vel_gyro_rx10*m+0,Vel_gyro_rx10*m+1,Vel_gyro_rx10*m+2...Vel_gyro_rx10*m+9)，Time＝(0.00,0.02,...0.18)，
N＝10。
and step 3:
a linear regression equation of M with respect to Time is calculated based on the least square method,
the regression curve equation is calculated by using the least square method for the angular velocity around the X axis in the calculation unit W as follows:
is provided with
Solving the notations by the least squares coefficients can yield:
the m group of linear regression equations is determined, then the state parameter value of the corresponding point of each time value is calculated based on the equation, the other five state parameters of the IMU are calculated as above, the data values of the other two groups are calculated as follows, the state parameter values of the three groups of IMU after least square calculation are obtained, and the following steps are referred specifically;
and 4, step 4: the calculating unit calculates the angular velocity value of the corresponding moment according to the time parameter value
Wherein:
and 5: similarly performing operation calculation in the steps 2, 3 and 4 on Y-axis angular velocity data Vel _ gyro _ ry1, Z-axis angular velocity data Vel _ gyro _ rz1, X-axis acceleration data Acc _ X1, Y-axis acceleration data Acc _ Y1 and Z-axis acceleration data Acc _ Z1 acquired by the first group of IMUs to acquire corresponding calculated state parameter values;
step 6: similarly adopting the operation rooms of the step 2, the step 3, the step 4 and the step 5 for the data of the second group and the third group to obtain corresponding calculated state parameter values;
and 7: three groups of IMU data, namely Gyro _ ang _ vel [3] [3] and Acc _ line _ Acc [3] [3], which are calculated and processed by a least square method are led into the extended Kalman filtering algorithm module to realize the attitude parameter calculation of the unmanned aerial vehicle, and a foundation is provided for the stable control of the unmanned aerial vehicle.
According to the method, the state space equation and the measurement equation related to the angular velocity and the acceleration are established based on the angular velocity value and the linear acceleration value processed by the least square method, the extended Kalman filtering algorithm module is constructed based on the equations and the corresponding Jacobian matrix by adopting the extended Kalman algorithm theory, the attitude of the unmanned aerial vehicle is solved, and the influence of noise and mutation values in data on the accuracy and stability of the solved attitude is effectively reduced.
On the other hand, the invention also discloses an unmanned aerial vehicle attitude calculation system based on a least square method and EKF, which comprises the following units:
the data acquisition unit is used for acquiring state parameter values of the flight state of the unmanned aerial vehicle based on the IMU data sensor, wherein the state parameter values comprise an angular velocity value and a linear acceleration value;
the data processing unit is used for performing fitting and interpolation calculation on the angular velocity value and the acceleration value based on a least square method, establishing a state space equation and a measurement equation related to the angular velocity and the acceleration, and building an attitude estimation module based on the equations and a corresponding Jacobian matrix through an extended Kalman filtering algorithm theory;
and the unmanned aerial vehicle attitude calculation unit is used for importing IMU data subjected to least square calculation into the attitude estimation module and calculating the attitude of the unmanned aerial vehicle.
It is understood that the system provided by the embodiment of the present invention corresponds to the method provided by the embodiment of the present invention, and the explanation, the example and the beneficial effects of the related contents can refer to the corresponding parts in the method.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
The above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (3)
1. A method for resolving unmanned aerial vehicle attitude based on least square method and EKF is characterized in that: the method comprises the following steps:
s100, acquiring state parameter values of the flight state of the unmanned aerial vehicle based on an IMU data sensor, wherein the state parameter values comprise an angular velocity value and a linear acceleration value;
s200, fitting and interpolating calculation are carried out on the angular velocity value and the acceleration value based on a least square method, a state space equation and a measurement equation about the angular velocity and the acceleration are established, and an attitude estimation module is built through an extended Kalman filtering algorithm theory based on the equations and a corresponding Jacobian matrix;
s300, importing IMU data subjected to least square calculation processing into an attitude estimation module, and performing attitude calculation on the attitude of the unmanned aerial vehicle.
2. The method of unmanned aerial vehicle attitude solution based on least squares and EKF of claim 1, wherein: the S200 specifically includes:
step 1:
acquiring flight state parameter values of the unmanned aerial vehicle through three groups of IMU sensors, taking angular velocity acquired by a gyroscope and linear acceleration acquired by an accelerometer as state input quantity for attitude calculation, and setting a data acquisition period of an inertial sensor;
wherein,
the gyroscope acquisition data is defined as follows,
Gyro_ang_vel[3][3]＝{Vel_gyro_rx1,Vel_gyro_ry1,Vel_gyro_rz1；Vel_gyro_rx2,Vel_gyro_ry2,Vel_gyro_rz2；Vel_gyro_rx3,Vel_gyro_ry3,Vel_gyro_rz3；}
the accelerometer acquisition data is defined as follows,
Acc_line_acc[3][3]＝{Acc_x1,Acc_y1,Acc_z1；Acc_x2,Acc_y2,Acc_z2；Acc_x3,Acc_y3,Acc_z3；}
wherein the data of the first set of IMUs is defined as follows,
vel _ gyro _ rx1, namely the angular speed value of the body around the X axis of a body coordinate system, and the unit is rad/s;
vel _ gyro _ ry1, namely the angular speed value of the machine body around the Y axis of the machine body coordinate system, and the unit is rad/s;
vel _ gyro _ rz1 angular speed value of the machine body around the Z axis of the machine body coordinate system, and the unit is rad/s;
acc _ X1, the linear velocity value of the machine body along the X axis of the machine body coordinate system, the unit is m/s ^ 2;
acc _ Y1, the linear velocity value of the machine body along the Y axis of the machine body coordinate system, the unit is m/s ^ 2;
acc _ Z1, the linear velocity value of the machine body along the Z axis of the machine body coordinate system, the unit is m/s ^ 2;
dt is a data acquisition period, dt is set to be 0.02, and the unit is s;
step 2:
dividing the state parameter values of 10 periods into calculation units M, and performing data preprocessing on angular velocity values Vel _ gyro _ rx1 around the X axis of a body coordinate system;
wherein:
the value of each sensor acquired in 10 sampling periods is one calculation unit, and the mth calculation unit is defined as W ═ W (W ═ W)10*m+0,w10*m+1,w10*m+2...w10*m+9)，
Wherein:
w＝(Vel_gyro_rx,Vel_gyro_ry,Vel_gyro_rz,Acc_x,Acc_y,Acc_z)，
M＝(Vel_gyro_rx10*m+0,Vel_gyro_rx10*m+1,Vel_gyro_rx10*m+2...Vel_gyro_rx10*m+9)，Time＝(0.00,0.02,...0.18)，
N＝10；
and step 3:
a linear regression equation of M with respect to Time is calculated based on the least square method,
the regression curve equation is calculated by using the least square method for the angular velocity around the X axis in the calculation unit W as follows:
is provided with
Solving the notations by the least squares coefficients can yield:
the m group of linear regression equations is determined, then the state parameter value of the corresponding point of each time value is calculated based on the equation, the other five state parameters of the IMU are calculated as above, the data values of the other two groups are calculated as follows, the state parameter values of the three groups of IMU after least square calculation are obtained, and the following steps are referred specifically;
and 4, step 4: the calculating unit calculates the angular velocity value of the corresponding moment according to the time parameter value
Wherein:
and 5: similarly performing operation calculation in the steps 2, 3 and 4 on Y-axis angular velocity data Vel _ gyro _ ry1, Z-axis angular velocity data Vel _ gyro _ rz1, X-axis acceleration data Acc _ X1, Y-axis acceleration data Acc _ Y1 and Z-axis acceleration data Acc _ Z1 acquired by the first group of IMUs to acquire corresponding calculated state parameter values;
step 6: and (5) similarly adopting the operation rooms of the step (2), the step (3), the step (4) and the step (5) for the data of the second group and the third group to obtain the corresponding calculated state parameter values.
3. The utility model provides a system that unmanned aerial vehicle gesture was solved based on least square method and EKF which characterized in that: the method comprises the following units:
the data acquisition unit is used for acquiring state parameter values of the flight state of the unmanned aerial vehicle based on the IMU data sensor, wherein the state parameter values comprise an angular velocity value and a linear acceleration value;
the data processing unit is used for performing fitting and interpolation calculation on the angular velocity value and the acceleration value based on a least square method, establishing a state space equation and a measurement equation related to the angular velocity and the acceleration, and building an attitude estimation module based on the equations and a corresponding Jacobian matrix through an extended Kalman filtering algorithm theory;
and the unmanned aerial vehicle attitude calculation unit is used for importing IMU data subjected to least square calculation into the attitude estimation module and calculating the attitude of the unmanned aerial vehicle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010982173.3A CN112304312B (en) | 2020-09-17 | 2020-09-17 | Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010982173.3A CN112304312B (en) | 2020-09-17 | 2020-09-17 | Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112304312A true CN112304312A (en) | 2021-02-02 |
| CN112304312B CN112304312B (en) | 2022-09-13 |
Family
ID=74483090
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010982173.3A Active CN112304312B (en) | 2020-09-17 | 2020-09-17 | Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112304312B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114019998A (en) * | 2021-12-03 | 2022-02-08 | 河南送变电建设有限公司 | Unmanned aerial vehicle high-voltage line inspection system, method, program product and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH07239236A (en) * | 1994-02-28 | 1995-09-12 | Hitachi Ltd | Method and apparatus for measurement of quantity of state of moving body and calculation device of attitude angle of moving body |
| JP2010085196A (en) * | 2008-09-30 | 2010-04-15 | Toyota Motor Corp | Object behavior estimation method and object behavior estimating device |
| CN104316055A (en) * | 2014-09-19 | 2015-01-28 | 南京航空航天大学 | Two-wheel self-balancing robot attitude calculation method based on improved Extended Kalman Filter algorithm |
| CN107438805A (en) * | 2016-09-27 | 2017-12-05 | 深圳市大疆创新科技有限公司 | Unmanned aerial vehicle (UAV) control method and device |
| CN108693372A (en) * | 2018-04-13 | 2018-10-23 | 南京航空航天大学 | A kind of course axis angular rate method of estimation of quadrotor |
| CN110146077A (en) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | Pose of mobile robot angle calculation method |
-
2020
- 2020-09-17 CN CN202010982173.3A patent/CN112304312B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH07239236A (en) * | 1994-02-28 | 1995-09-12 | Hitachi Ltd | Method and apparatus for measurement of quantity of state of moving body and calculation device of attitude angle of moving body |
| JP2010085196A (en) * | 2008-09-30 | 2010-04-15 | Toyota Motor Corp | Object behavior estimation method and object behavior estimating device |
| CN104316055A (en) * | 2014-09-19 | 2015-01-28 | 南京航空航天大学 | Two-wheel self-balancing robot attitude calculation method based on improved Extended Kalman Filter algorithm |
| CN107438805A (en) * | 2016-09-27 | 2017-12-05 | 深圳市大疆创新科技有限公司 | Unmanned aerial vehicle (UAV) control method and device |
| CN108693372A (en) * | 2018-04-13 | 2018-10-23 | 南京航空航天大学 | A kind of course axis angular rate method of estimation of quadrotor |
| CN110146077A (en) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | Pose of mobile robot angle calculation method |
Non-Patent Citations (2)
| Title |
|---|
| ZHEYAOXU 等: "Parameter estimation of a three-axis spacecraft simulator using recursive least-squares approach with tracking differentiator and Extended Kalman Filter", 《ACTA ASTRONAUTICA》 * |
| 方根在 等: "小型飞行器姿态估计系统设计与实现", 《电子测量与仪器学报》 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114019998A (en) * | 2021-12-03 | 2022-02-08 | 河南送变电建设有限公司 | Unmanned aerial vehicle high-voltage line inspection system, method, program product and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112304312B (en) | 2022-09-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107560613B (en) | Robot indoor track tracking system and method based on nine-axis inertial sensor | |
| CN109682377B (en) | Attitude estimation method based on dynamic step gradient descent | |
| CN104198765B (en) | The coordinate system conversion method of vehicle acceleration of motion detection | |
| Wang et al. | High-order nonlinear differentiator and application to aircraft control | |
| CN109141475B (en) | Initial alignment method for SINS robust traveling under assistance of DVL (dynamic velocity logging) | |
| CN107063262A (en) | A kind of complementary filter method resolved for UAV Attitude | |
| CN106990426A (en) | A kind of air navigation aid and guider | |
| CN108871378B (en) | Online dynamic calibration method for errors of inner lever arm and outer lever arm of two sets of rotary inertial navigation systems | |
| CN112744313B (en) | Robot state estimation method and device, readable storage medium and robot | |
| CN112066985B (en) | Initialization method, device, medium and electronic equipment for combined navigation system | |
| CN105910606A (en) | Direction adjustment method based on angular velocity difference | |
| CN108981694A (en) | Attitude algorithm method and system based on wavelet neural network and EKF | |
| CN108627152B (en) | Navigation method of micro unmanned aerial vehicle based on multi-sensor data fusion | |
| CN115540860A (en) | Multi-sensor fusion pose estimation algorithm | |
| CN108871323B (en) | High-precision navigation method of low-cost inertial sensor in locomotive environment | |
| CN112665574A (en) | Underwater robot attitude acquisition method based on momentum gradient descent method | |
| CN112304312B (en) | Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF | |
| CN111238469A (en) | Unmanned aerial vehicle formation relative navigation method based on inertia/data chain | |
| Carratù et al. | IMU self-alignment in suspensions control system | |
| CN110207647B (en) | Arm ring attitude angle calculation method based on complementary Kalman filter | |
| CN116125789A (en) | Gesture algorithm parameter automatic matching system and method based on quaternion | |
| CN109506674B (en) | Acceleration correction method and device | |
| Nistler et al. | Gravity compensation in accelerometer measurements for robot navigation on inclined surfaces | |
| CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| Saputra et al. | Imu application in measurement of vehicle position and orientation for controlling a pan-tilt mechanism |
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 |