CN106249745B - The control method of four axis unmanned planes - Google Patents
The control method of four axis unmanned planes Download PDFInfo
- Publication number
- CN106249745B CN106249745B CN201610531882.3A CN201610531882A CN106249745B CN 106249745 B CN106249745 B CN 106249745B CN 201610531882 A CN201610531882 A CN 201610531882A CN 106249745 B CN106249745 B CN 106249745B
- Authority
- CN
- China
- Prior art keywords
- attitude
- angle
- unmanned aerial
- aerial vehicle
- control
- 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 33
- 230000004927 fusion Effects 0.000 claims abstract description 11
- 238000004422 calculation algorithm Methods 0.000 claims description 29
- 238000001914 filtration Methods 0.000 claims description 27
- 230000005484 gravity Effects 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 13
- 230000001133 acceleration Effects 0.000 claims description 11
- 230000000295 complement effect Effects 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims 1
- 238000005259 measurement Methods 0.000 abstract description 7
- 230000033001 locomotion Effects 0.000 description 22
- 230000006872 improvement Effects 0.000 description 8
- 230000008859 change Effects 0.000 description 5
- 238000012937 correction Methods 0.000 description 5
- 238000005096 rolling process Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 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 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000001568 sexual effect Effects 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 230000035899 viability Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a kind of control methods of four axis unmanned planes, include: S1, obtain the attitude data of attitude transducer in four axis unmanned planes and be filtered fusion, current posture information is obtained, attitude data includes the current corresponding angular speed of attitude angle and attitude angle；S2, four axis unmanned planes are controlled by cascade double loop PID control method, using the deviation of current pose information and targeted attitude information as the input quantity of angle PID controller, according to the difference input angular velocity PID controller of current angular velocity and angle PID controller output quantity, angular speed PID controller output control motor, until four axis unmanned planes reach targeted attitude.The present invention can obtain more accurate measurement data, improve the accuracy of attitude data；Using cascade double loop PID control, the anti-interference of system is enhanced, so that four axis unmanned planes have stronger adaptability.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicle control, in particular to a control method of a four-axis unmanned aerial vehicle.
Background
An Unmanned Aerial Vehicle (UAV), also called an Unmanned Aerial Vehicle for short, is an Unmanned Aerial Vehicle that is controlled by a radio remote control and an onboard program controller, unlike a conventional manned aircraft. It appeared in the 20 th century for the first time, and was only used as a target in military training, and gradually turned to various multipurpose fields such as detection and attack after the development of the last hundred years. Compared with a manned airplane, the unmanned airplane has the advantages of low cost, strong viability, no casualty risk, convenient use and the like, so that the unmanned airplane can play an important role in military affairs and has wide application prospect in the civil field.
Classify according to unmanned aerial vehicle's overall structure, can divide unmanned aerial vehicle into these three kinds of mainstream structures of fixed wing unmanned aerial vehicle, helicopter unmanned aerial vehicle and many rotor unmanned aerial vehicle, in addition, there are less numerous unmanned aerial vehicle structures such as umbrella wing unmanned aerial vehicle, flapping wing unmanned aerial vehicle and unmanned airship, but practical application is less. Wherein many rotor unmanned aerial vehicle possess 4 or more rotors and provide power, compare fixed wing unmanned aerial vehicle, can realize nimble maneuver such as in-situ VTOL and hover in the air, and mechanical structure is simpler again than helicopter unmanned aerial vehicle, and the price is cheaper, and environmental adaptation ability is stronger, can be used to part for military use and most civilian, consumption field, and development prospect receives the most attention. Among the multi-rotor drones, the simplest and most common is a quad-rotor drone (Quadrotor UAV), also known as a quad-rotor drone, whose flight power is provided by driving a propeller by four rotor-type flight engines, respectively, and stable flight can be achieved by adjusting the engine speed to change the lift and torque generated by the rotation of the four rotors.
According to the classification of unmanned aerial vehicle size, most four-axis unmanned aerial vehicles on the market at present basically belong to microminiature unmanned aerial vehicle's category, and microminiature four-axis unmanned aerial vehicle comprises frame, motor, screw, battery, remote controller, motor drive ware and flight control panel. Because microminiature four-axis unmanned aerial vehicle has flight gesture flexibility, mobility is strong, mechanical structure is comparatively simple, easy to detach maintains, characteristics such as sexual valence are higher for it has comparatively extensive platform adaptability, can be applied to low-altitude military reconnaissance, power line and patrol and examine, mountain and forest disaster condition search and rescue, amusement movie & TV are taken photo by plane multiple different fields.
However, the microminiature four-axis unmanned aerial vehicle has smaller size, lighter weight and lower flying speed, so that an aerodynamic model is difficult to accurately establish, and in addition, the load and the inertia of the microminiature four-axis unmanned aerial vehicle are both smaller, and the microminiature four-axis unmanned aerial vehicle is very easy to be influenced by airflow during low-speed flying, so that the microminiature four-axis unmanned aerial vehicle has higher requirements on the control precision and the flexibility of a flying controller.
Therefore, in order to solve the above technical problem, it is necessary to provide a control method for a four-axis drone.
Disclosure of Invention
In view of this, the present invention provides a control method for a four-axis drone.
In order to achieve the above purpose, the technical solutions provided by the embodiments of the present invention are as follows:
a control method of a four-axis unmanned aerial vehicle, the control method comprising:
s1, acquiring attitude data of an attitude sensor in the four-axis unmanned aerial vehicle, and performing filtering fusion to obtain current attitude information, wherein the attitude data comprises a current attitude angle and an angular velocity corresponding to the attitude angle;
and S2, controlling the four-axis unmanned aerial vehicle by a cascade double-ring PID control method, taking the deviation value of the current attitude information and the target attitude information as the input quantity of the angle PID controller, inputting the angular velocity PID controller according to the difference value of the current angular velocity and the output quantity of the angle PID controller, and outputting a control motor by the angular velocity PID controller until the four-axis unmanned aerial vehicle reaches the target attitude.
As a further improvement of the invention, the attitude angle comprises a pitch angle, a roll angle and a course angle.
As a further improvement of the present invention, the attitude information in step S1 is represented by one of euler angles, quaternions, matrices, and axial angles.
As a further improvement of the present invention, the attitude information calculation formula in step S1 is:
wherein,respectively are attitude angles of the current four-axis unmanned aerial vehicle,theta, psi being Euler angles, omega, obtained in the last resolving periodx、ωy、ωzIs the three-axis angular velocity of the four-axis unmanned plane.
As a further improvement of the present invention, the attitude information calculation formula in step S1 is:
wherein,respectively the attitude angle, q, of the current four-axis unmanned aerial vehicle0、q1、q2、q3Quaternions derived for the previous resolution cycle.
As a further improvement of the present invention, the filtering in step S1 is fused as follows:
and performing filter fusion of the attitude data through a Mahony complementary filter algorithm.
As a further improvement of the present invention, the step S1 specifically includes:
1) calculating the initial attitude of the body in the Euler angle form through the data measured by the accelerometer and the magnetometer, and initializing the quaternion;
2) obtaining current gravity acceleration components ax, ay and az, angular velocity components gx, gy and gz and magnetic field strength components mx, my and mz of the body through an accelerometer, a gyroscope and a magnetometer, and normalizing the gravity acceleration components and the magnetic field strength components to obtain a unit value;
3) calculating the estimated values of the gravity acceleration component and the magnetic field intensity component under the current body coordinate system by using the quaternion obtained in the previous resolving period according to the relationship between the quaternion and the Euler angle;
4) vector cross multiplication is carried out on the measured values of the gravity vector and the earth magnetic field and an estimated value calculated by utilizing the last attitude calculation result to obtain an error between the measured values and the earth magnetic field, and the error is accumulated;
5) correcting drift errors of the gyroscope in a PI (proportional-integral) adjustment mode by using accumulated errors obtained by vector cross multiplication;
6) and finally, solving a differential equation set of the quaternion, updating the quaternion and unitizing the quaternion.
As a further improvement of the present invention, the PID control method in step S2 specifically includes:
and calculating the proportion P, the integral I and the differential D of the error between the current attitude information and the target attitude information, and superposing and outputting the proportions.
As a further improvement of the present invention, the output of the PID control method in the step S2Wherein:
output P of proportional controlout＝Kpe(t)，KpIs a parameter of proportional control, e is an input error signal;
output of integral controlKiIs a parameter of proportional control, e is an input error signal;
output of differential controlKdFor the parameter of the proportional control, e is the input error signal.
The invention has the beneficial effects that:
resolving the surrounding unmanned aerial vehicle attitude can be realized only by acquiring the three-axis angular velocity of the body, and meanwhile, the measurement data is subjected to auxiliary correction through filtering fusion, so that more accurate measurement data can be obtained, and the accuracy of the attitude data is improved;
by using cascade dual-ring PID control, the anti-interference performance of the system is enhanced, so that the four-axis unmanned aerial vehicle has stronger adaptability.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments described in the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
Fig. 1 is a schematic flow chart of a control method of a four-axis unmanned aerial vehicle according to the present invention;
FIG. 2 is a power model diagram of a four-axis unmanned aerial vehicle according to the present invention;
FIG. 3 is a schematic view of a four-axis unmanned aerial vehicle flight motion mode according to the present invention;
FIG. 4 is a schematic diagram of a geographic coordinate system of the present invention;
FIG. 5 is a schematic diagram illustrating Euler angles of Pitch, Roll, and Yaw in accordance with an embodiment of the present invention;
FIG. 6 is a schematic flow chart of prior art angular single loop PID control;
fig. 7 is a schematic flow chart of cascade dual-loop PID control according to an embodiment of the present invention.
Detailed Description
In order to make those skilled in the art better understand the technical solution of the present invention, the technical solution in the embodiment of the present invention will be clearly and completely described below with reference to the drawings in the embodiment of the present invention, and it is obvious that the described embodiment is only a part of the embodiment of the present invention, and not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Referring to fig. 1, the invention discloses a control method of a four-axis unmanned aerial vehicle, comprising the following steps:
s1, acquiring attitude data of an attitude sensor in the four-axis unmanned aerial vehicle, and performing filtering fusion to obtain current attitude information, wherein the attitude data comprises a current attitude angle and an angular velocity corresponding to the attitude angle;
and S2, controlling the four-axis unmanned aerial vehicle by a cascade double-ring PID control method, taking the deviation value of the current attitude information and the target attitude information as the input quantity of the angle PID controller, inputting the angular velocity PID controller according to the difference value of the current angular velocity and the output quantity of the angle PID controller, and outputting a control motor by the angular velocity PID controller until the four-axis unmanned aerial vehicle reaches the target attitude.
The present invention will be further described with reference to the following embodiments.
The 'four-axis' in the microminiature four-axis unmanned aerial vehicle means that the power of the aircraft is provided by four rotor wing type flight engines driving propellers, and the power model of the aircraft is shown in figure 2. The four rotors are divided into two groups, respectively located at the cantilever ends of the cross-symmetric airframe, and two rotors located at opposite corners are used as one group. The rotary directions of the rotors in the same group are the same, and the rotary directions of the rotors in different groups are opposite. Under the condition of the same rotating speed, due to the symmetry of the structure, the torques generated by the rotation of the four rotors can be mutually offset, so that the fuselage can be kept stable on a horizontal plane.
The adjustment of the flight attitude of the four-axis unmanned aerial vehicle can be realized by adjusting the rotating speeds of the four rotors, so that the four-axis unmanned aerial vehicle can complete basic movements such as pitching, rolling, yawing, lifting and the like. The four basic movement modes are shown in fig. 3, the four-axis drone in fig. 3 works in a "+" mode, the head of the drone faces the positive direction of the X axis, the motors 1 and 3 are in one group and rotate in the counterclockwise direction, the motors 2 and 4 are in one group and rotate in the clockwise direction, the upward arrow indicates the increase of the rotation speed, and the downward arrow indicates the decrease of the rotation speed. The flying motion mode comprises the following steps:
1) pitching motion: the movement mode is as shown in fig. 3a, the rotation speeds of motors No. 2 and 4 are kept unchanged, the rotation speeds of motors No. 1 and 3 are changed, and the trend of the change of the rotation speeds is opposite, so that the four-axis unmanned aerial vehicle can rotate around the Y axis by a certain angle to perform pitching motion, and the angle of rotation around the Y axis is also called a Pitch angle (Pitch); the front and back lifting forces of the unmanned aerial vehicle body are inconsistent due to different rotating speeds of the motors No. 1 and No. 3, so that the unmanned aerial vehicle body moves back and forth in the X-axis direction of the horizontal plane, and the forward and backward movement of the four-axis unmanned aerial vehicle is realized;
2) and (3) rolling movement: the movement mode is as shown in fig. 3b, opposite to the pitching movement, the rotation speed of the motors 1 and 3 is kept unchanged, the rotation speed of the motors 2 and 4 is changed, and the trend of the change of the rotation speed is opposite, so that the four-axis unmanned aerial vehicle rotates around the X axis by a certain angle to perform a rolling movement, and the angle of the rotation around the X axis is also called a rolling angle (Roll); 2. the difference of left and right lifting forces of the unmanned aerial vehicle body is caused by the difference of the rotating speeds of the No. 4 motor, so that the unmanned aerial vehicle body moves left and right in the Y-axis direction of the horizontal plane, and the left movement and the right movement of the four-axis unmanned aerial vehicle are realized;
3) yaw movement: the motion mode is as shown in fig. 3c, keeps the rotational speed of diagonal motors consistent, changes the rotational speed of two sets of diagonal motors with opposite trend respectively, can make four-axis unmanned aerial vehicle rotate certain angle around the Z axle, accomplishes the yaw motion, and then changes the orientation of aircraft nose. The angle of rotation about the Z axis is also known as Yaw angle (Yaw);
4) lifting movement: the motion mode is as shown in fig. 3d, it makes four-axis unmanned aerial vehicle can keep steady to keep four motor rotational speeds the same, then promotes or reduces their rotational speed simultaneously, just can realize the displacement of four-axis unmanned aerial vehicle in the Z axle direction, lifting motion promptly.
The analysis on the flying principle of the micro-miniature four-axis unmanned aerial vehicle shows that the control on the flying motion mode of the micro-miniature four-axis unmanned aerial vehicle not only controls the flying attitude, namely three attitude angles of a Pitch angle Pitch, a Roll angle Roll and a Yaw angle Yaw, but also finally converts the control into the control on the rotating speed of a corresponding motor. In order to quantitatively realize such a control mode, a closed-loop control mode of 'observation- > control- > re-observation- > re-control' circulation is generally adopted, that is, the current attitude angle of the machine body is continuously observed and calculated through a sensor device, then a certain control algorithm is used, the rotating speed of the motor is controlled to make the machine body change towards the target attitude, and the process is continuously repeated until the machine body reaches the target attitude.
In the above control mode, the step of "observation" is also called attitude resolution, and is responsible for fusing data of attitude sensors such as accelerometers, gyroscopes and magnetometers and outputting current attitude information; the control part adopts a PID control algorithm and is responsible for converting the target attitude into the control of the rotating speed of the motor. The present embodiment will be specifically described below in terms of both the attitude calculation method and the PID control method.
First, attitude solution
The flight attitude of a four-axis drone describes in practice the angular relationship between the coordinate system in which the airframe is located and the geographic coordinate system. The geographic coordinate system is established with the earth as a reference, and the true east, true north and true up directions are used as X, Y and Z axes of the coordinate system, which is shown in FIG. 4. The body coordinate system is shown in fig. 2, the geometric center of the body is used as the origin, the axis perpendicular to the plane of the four motors is used as the Z axis, the X, Y axes are located on the plane of the motors and are perpendicular to each other, and the specific direction is determined by the flight mode used. Under low altitude and near ground conditions, it is considered that both the gravity acceleration vector and the geomagnetic vector in the geographic coordinate system are fixed, and the posture of the body can be determined by using this feature, using sensors such as an accelerometer and a magnetometer.
The relation between the body coordinate system and the geographic coordinate system can be expressed by euler angles, quaternions, matrixes, shaft angles and the like, wherein the euler angle expression and quaternion expression are most commonly used.
1) Euler angle representation
The euler angles are used to describe the orientation of one coordinate system relative to another in three-dimensional euler space. Here we use the fixed geographic coordinate system as the reference coordinate system, and any orientation of the coordinate system of the body in the same space can be obtained by using the reference coordinate system to make three rotations around some coordinate axis of the reference coordinate system in a certain order, and the angle of each rotation is called euler angle.
The representation of the Euler angles is different according to the rotation sequence and the selection of the rotation axis, and the representation of Tait-Bryan angle is adopted to obtain the Euler angles as shown in FIG. 5Theta, psi. The geographic coordinate system referenced by O-xyz in FIG. 5 is first rotated by an angle ψ about the z-axis to obtain O-x 'y' z ', then rotated by an angle θ about the y' axis to obtain O-x "y" z ", and finally rotated about the x" axisAngle to obtain O-XYZ body coordinate system with Euler angleAnd theta and psi respectively correspond to Pitch, Roll and Yaw.
Using the Euler angles obtained from the last solution cycle when using the Euler angles to represent attitudeTheta, psi, plus the triaxial angular velocity omega in the body coordinate system measured by the gyroscopex、ωy、ωzThe current new attitude angle can be obtained through Euler angle kinematics differential equationThe formula is as follows:
through the method, the values of Pitch, Roll and Yaw can be directly solved, the values are relatively easy to understand, but when theta reaches 90 degrees, the problem of gimbal deadlock called GimbalLock is generated, and the full-attitude solution cannot be realized.
2) Quaternion representation
Quaternions are understood to be hypercomplexes consisting of a real number and a three-dimensional vector, which are expressed as follows:
a unit quaternion can be conveniently used to represent a rotation in three-dimensional space, where the vector part is used to represent the axis of rotation and the real part is used to represent the angle of rotation about the axis of rotation.
When the posture is expressed by using the quaternion, it is necessary to add the quaternion to the body first when the body is stationaryCalculating Euler angle of body at the moment by data measured by speedometer and magnetometerθ0、ψ0Then, the initial quaternion q is obtained by the following formula0’、q1’、q2’、q3’：
Then, the differential equation set of the quaternion can be solved through a first-order Picard algorithm, and the posture updating formula of the quaternion is obtained as follows:
in equation 4, the equation left side is the updated quaternion, q on the right side of the equation0、q1、q2、q3Quaternions derived for the previous resolution cycle. Compared with the Euler angle algorithm, the algorithm reduces the calculation of the trigonometric function, improves the attitude calculation speed, does not have the problem of universal joint deadlock in the Euler angle algorithm, and can be used for calculating the full attitude.
Although the attitude of the aircraft is easy to calculate, the quaternion mode is not intuitive enough, so after the attitude is solved, the updated quaternion needs to be converted into an Euler angle and provided for a subsequent attitude control algorithm, and the formula is shown as follows:
as can be seen from formulas 3, 4 and 5, under the condition of knowing the initial attitude of the body, the body attitude can be resolved theoretically only by the three-axis angular velocity of the body acquired by the gyroscope. However, due to the existence of error noise and the like, the data measured by the gyroscope can only keep relative accuracy in a short time, and especially after the gyroscope is operated for a period of time, the accumulation of the integral error can cause the obtained attitude to be far from the actual attitude, so that a large drift is generated, and other sensors are necessary to perform auxiliary correction on the measured data of the gyroscope. This process of combining different sensor measurements to obtain more accurate measurements is called filtering fusion of sensor data.
Common filtering fusion algorithms in the four-axis unmanned aerial vehicle include a complementary filtering algorithm, a Kalman filtering algorithm and the like. The complementary filtering algorithm is based on the idea that more accurate output data is obtained by utilizing complementary characteristics of different sensor output data on a frequency domain and adopting ways such as weighted fusion and the like; the kalman filtering algorithm is an algorithm for performing optimal estimation on a system state by inputting and outputting observation data through a system using a linear system state equation, and the optimal estimation can also be regarded as a filtering process because the observation data includes the influence of noise and interference in the system.
Kalman filtering is better than complementary filtering in response speed, but the Kalman filtering has higher requirement on the accuracy of target modeling, is relatively complex to realize and has relatively large calculated amount; the calculation amount of complementary filtering is relatively small, the implementation difficulty is low, the difference between the overall filtering effect and Kalman filtering is not very large, and the attitude calculation for the four-axis unmanned aerial vehicle is enough.
In the present embodiment, an algorithm called Mahony complementary filtering is used to perform the filtering fusion of the attitude sensor data. The algorithm is based on the idea of complementary filtering, the drift of the gyroscope is corrected by using acceleration data which has more high-frequency noise, more stable low-frequency long-term signals and no long-term drift, and magnetometer data are added to correct the Yaw angle Yaw aiming at the characteristic that the accelerometer cannot measure the rotation motion on the gravity axis.
In the embodiment, quaternion is adopted to solve the attitude, a Mahony complementary filtering algorithm is used to filter the attitude data, and a quaternion-based complementary filtering attitude calculation algorithm is mutually combined, and the execution flow is as follows:
1) when the algorithm is executed for the first time, the initial attitude (euler angle form) of the body is calculated from the data measured by the accelerometer and the magnetometer, and the quaternion is initialized by equation 3.
2) Obtaining the current gravity acceleration component a of the body through an accelerometer, a gyroscope and a magnetometerx、ay、azComponent of angular velocity gx、gy、gzAnd a magnetic field strength component mx、my、mz. And the gravitational acceleration component and the magnetic field strength component are normalized (each component is divided by the modulus of the vector) to obtain their unit values.
3) Through the relation between quaternion and Euler angle, the quaternion (obtained by integrating the gyroscope) obtained in the last resolving period is used for calculating the estimated values of the gravity acceleration component and the magnetic field strength component in the current body coordinate system, and the related algorithm is as follows:
hx＝2.0f*(mx*(0.5f-q2q2-q3q3)+my*(q1q2-q0q3)+mz*(q1q3+q0q2))；
hy＝2.0f*(mx*(q1q2+q0q3)+my*(0.5f-q1q1-q3q3)+mz*(q2q3-q0q1))；
hz＝2.0f*mx*(q1q3-q0q2)+2.0f*my*(q2q3+q0q1)+2.0f*mz*(0.5f-q1q1-q2q2)；
bx＝sqrt(hx*hx+hy*hy)；
bz＝hz；
v/estimated magnetic field orientation Using last attitude solution
halfwx＝bx*(0.5f-q2q2-q3q3)+bz*(q1q3-q0q2)；
halfwy＝bx*(q1q2-q0q3)+bz*(q0q1+q2q3)；
halfwz＝bx*(q0q2+q1q3)+bz*(0.5f-q1q1-q2q2)；
Estimating gravity field direction using last attitude solution
halfvx＝q1q3-q0q2；
halfvy＝q0q1+q2q3；
halfvz＝q0q0-0.5f+q3q3；
4) And carrying out vector cross multiplication on the measured values of the gravity vector and the earth magnetic field and an estimated value obtained by using the last attitude calculation result to obtain an error between the measured values and the estimated value, and accumulating the error, wherein a correlation algorithm is as follows:
v/calculating and accumulating the error between the estimated and the measured direction of the magnetic field
halfex+＝(my*halfwz-mz*halfwy)；
halfey+＝(mz*halfwx-mx*halfwz)；
halfez+＝(mx*halfwy-my*halfwx)；
halfvz＝q0q0-0.5f+q3q3；
V/calculating and accumulating errors between the gravity estimated direction and the measured direction
halfex+＝ay*halfvz-az*halfvy；
halfey+＝az*halfvx-ax*halfvz；
halfez+＝ax*halfvy-ay*halfvx；
5) The drift error of the gyroscope is corrected by using the accumulated error obtained by vector cross multiplication in a PI regulation mode, and a related algorithm is as follows, wherein two parameters Kp and Ki are used for controlling the correction speed of the accelerometer and the magnetometer on the integral attitude of the gyroscope.
// error integral
gyro_bias[0]+＝twoKi*halfex*dt；
gyro_bias[1]+＝twoKi*halfey*dt；
gyro_bias[2]+＝twoKi*halfez*dt；
// drift correction
gx+＝(twoKp*halfex+gyro_bias[0])；
gy+＝(twoKp*halfey+gyro_bias[1])；
gz+＝(twoKp*halfez+gyro_bias[2])；
6) And finally, solving a differential equation set of the quaternion by using the formula 4, updating the quaternion, unitizing the quaternion so as to facilitate the calculation of the next resolving period, wherein a related algorithm is as follows:
dq0＝0.5f*(-q1*gx-q2*gy-q3*gz)；
dq1＝0.5f*(q0*gx+q2*gz-q3*gy)；
dq2＝0.5f*(q0*gy-q1*gz+q3*gx)；
dq3＝0.5f*(q0*gz+q1*gy-q2*gx)；
q0+＝dt*dq0；
q1+＝dt*dq1；
q2+＝dt*dq2；
q3+＝dt*dq3；
v/unitizing quaternions
recipNorm＝invSqrt(q0*q0+q1*q1+q2*q2+q3*q3)；
q0*＝recipNorm；
q1*＝recipNorm；
q2*＝recipNorm；
q3*＝recipNorm；
Therefore, resolving of the four-axis unmanned aerial vehicle attitude is completed, and the current attitude of the body is obtained.
Second, PID control method
After the current flight attitude of the four-axis unmanned aerial vehicle is obtained, the rotating speed of the corresponding motor needs to be controlled in a certain control mode, so that the four-axis unmanned aerial vehicle reaches the target attitude. Some Control methods proposed so far include PID Control (Proportional-Integral-Derivative Control), BackStepping Control (BackStepping Control), Sliding Mode Control (Sliding Mode Control), and Linear quadratic modulator (Linear quadratic regulator). The PID control method is relatively the most traditional, but because of low requirements on system models, simple coding implementation and strong adaptability of the control method, the PID control method is widely applied to engineering practice. The invention adopts a cascade double-ring PID algorithm to realize the control of the attitude angle of the four-axis unmanned aerial vehicle.
The PID control algorithm is the most common and widely applied negative feedback automatic control algorithm. The method utilizes the proportion (P, Proport), Integral (I, Integral) and Derivative (D) of the error between the observed value and the target value to carry out corresponding calculation and superposition output, thereby realizing the control of the target. Wherein:
ratio control (P): the proportional control is to control and output the input error signal according to a certain proportion. When only proportional control is available, the system output will have a steady-state error, i.e., the output will deviate from the target value when stable. Output P of proportional controloutAs shown in formula 6, wherein KpFor the parameter of the proportional control, e is the input error signal.
Pout＝Kpe (t) (formula 6)
As can be seen from equation 6, the coefficient KpThe larger the value of (a), the larger the proportional control output, the quicker the error is corrected, but the system stability is also reduced.
Integral control (I): the integral control introduces the integral of an error signal to time, and the output of the controller is in a direct proportion relation with the integral term. Since the integral term increases with time, combining it with proportional control to form PI control eliminates steady-state errors in proportional control, but the output suffers some time lag. The integral term output is expressed by equation 7, whereKiFor the parameter of the proportional control, e is the input error signal.
Differential control (D): in differential control, the output of the controller is proportional to the differential of the error signal over time. The differential regulation is to control the change rate of the error, can realize the advanced control of the controlled quantity, increase the damping coefficient of the system and increase the stability of the system. The output equation of the derivative term is shown in equation 8, where Kd is a parameter for proportional control, and e is the input error signal.
P, I, D, the combination of the three regulation modes becomes PID control, and the mathematical expression is as follows:
by adjusting K in PID controllersp、KiAnd KdThe three parameters can realize stable, quick and accurate control on the controlled object.
In the prior art, for a four-axis unmanned aerial vehicle system, after the current three attitude angles of a vehicle body are calculated, the deviation values between the current three attitude angles and a target attitude angle can be used as input quantities, and a corresponding PID controller is designed to realize the control of a pitch angle, a roll angle and a course angle, wherein the control flow is shown in fig. 6.
The angle single-ring PID control algorithm only uses attitude angle information of the four-axis unmanned aerial vehicle, in order to increase the stability and control quality of control, the control on the angular speed of the four-axis unmanned aerial vehicle is further added in the embodiment, and a cascade double-ring PID control algorithm consisting of an angle ring and an angular speed ring is formed.
In this embodiment, the four-axis drone is controlled by a cascade dual-loop PID control method, a deviation value between current attitude information and target attitude information is used as an input quantity of an angle PID controller, the angular velocity PID controller is input according to a difference value between the current angular velocity and an output quantity of the angle PID controller, and the angular velocity PID controller outputs a control motor until the four-axis drone reaches a target attitude.
Specifically, the composition structure of the outer ring angular velocity ring is similar to that shown in fig. 6, except that the control output thereof is no longer directly supplied to the motor for rotation speed adjustment, but is taken as the desired value of the inner ring angular velocity ring; the angular velocity data measured by the gyroscope is matched to obtain the difference value of the two angular velocity data, the difference value is input into an inner ring angular velocity PID controller, the output of the inner ring angular velocity PID controller is used for controlling a motor, and the control flow is shown in figure 7.
According to the technical scheme, the invention has the following beneficial effects:
resolving the surrounding unmanned aerial vehicle attitude can be realized only by acquiring the three-axis angular velocity of the body, and meanwhile, the measurement data is subjected to auxiliary correction through filtering fusion, so that more accurate measurement data can be obtained, and the accuracy of the attitude data is improved;
by using cascade dual-ring PID control, the anti-interference performance of the system is enhanced, so that the four-axis unmanned aerial vehicle has stronger adaptability.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms without departing from the spirit or essential attributes thereof. The present embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim concerned.
Furthermore, it should be understood that although the present description refers to embodiments, not every embodiment may contain only a single embodiment, and such description is for clarity only, and those skilled in the art should integrate the description, and the embodiments may be combined as appropriate to form other embodiments understood by those skilled in the art.
Claims (3)
1. A control method of a four-axis unmanned aerial vehicle is characterized by comprising the following steps:
s1, acquiring attitude data of an attitude sensor in the four-axis unmanned aerial vehicle, filtering and fusing the attitude data to obtain current attitude information, wherein the attitude data comprises a current attitude angle and an angular velocity corresponding to the attitude angle, the attitude angle comprises a pitch angle, a roll angle and a course angle, and the attitude information is represented by one of an Euler angle, a quaternion, a matrix and an axial angle;
s2, controlling the four-axis unmanned aerial vehicle by a cascade double-ring PID control method, taking the deviation value of the current attitude information and the target attitude information as the input quantity of an angle PID controller, inputting the angular velocity PID controller according to the difference value of the current angular velocity and the output quantity of the angle PID controller, and outputting a control motor by the angular velocity PID controller until the four-axis unmanned aerial vehicle reaches the target attitude;
the attitude information calculation formula is as follows:
wherein,respectively the attitude angle, q, of the current four-axis unmanned aerial vehicle0、q1、q2、q3Quaternion obtained for the last resolving period;
the filtering fusion in step S1 is:
filtering and fusing the attitude data through a Mahony complementary filtering algorithm;
the step S1 specifically includes:
1) calculating the initial attitude of the body in the Euler angle form through the data measured by the accelerometer and the magnetometer, and initializing the quaternion;
2) obtaining current gravity acceleration components ax, ay and az, angular velocity components gx, gy and gz and magnetic field strength components mx, my and mz of the body through an accelerometer, a gyroscope and a magnetometer, and normalizing the gravity acceleration components and the magnetic field strength components to obtain a unit value;
3) calculating the estimated values of the gravity acceleration component and the magnetic field intensity component under the current body coordinate system by using the quaternion obtained in the previous resolving period according to the relationship between the quaternion and the Euler angle;
4) vector cross multiplication is carried out on the measured values of the gravity vector and the earth magnetic field and an estimated value calculated by utilizing the last attitude calculation result to obtain an error between the gravity vector and the earth magnetic field, and the error is accumulated;
5) correcting drift errors of the gyroscope in a PI (proportional-integral) adjustment mode by using accumulated errors obtained by vector cross multiplication;
6) and finally, solving a differential equation set of the quaternion, updating the quaternion and unitizing the quaternion.
2. The control method according to claim 1, wherein the PID control method in step S2 is specifically:
and calculating the proportion P, the integral I and the differential D of the error between the current attitude information and the target attitude information, and superposing and outputting the proportions.
3. Control method according to claim 2, characterized in that the output of the PID control method in step S2Wherein:
output P of proportional controlout＝Kpe(t)，KpIs a parameter of proportional control, e is an input error signal;
output of integral controlKiIs a parameter of proportional control, e is an input error signal;
output of differential controlKdFor the parameter of the proportional control, e is the input error signal.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610531882.3A CN106249745B (en) | 2016-07-07 | 2016-07-07 | The control method of four axis unmanned planes |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610531882.3A CN106249745B (en) | 2016-07-07 | 2016-07-07 | The control method of four axis unmanned planes |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106249745A CN106249745A (en) | 2016-12-21 |
| CN106249745B true CN106249745B (en) | 2019-02-19 |
Family
ID=57613940
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610531882.3A Active CN106249745B (en) | 2016-07-07 | 2016-07-07 | The control method of four axis unmanned planes |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106249745B (en) |
Families Citing this family (39)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106774378B (en) * | 2017-03-01 | 2019-08-09 | 杭州谷航科技有限公司 | A kind of UAV Flight Control and localization method |
| CN108475066B (en) * | 2017-04-21 | 2021-02-19 | 深圳市大疆创新科技有限公司 | Unmanned aerial vehicle attitude calculation method, flight controller and unmanned aerial vehicle |
| CN107145155A (en) * | 2017-07-06 | 2017-09-08 | 深圳潜行创新科技有限公司 | It is a kind of from steady system and underwater vehicle |
| CN109388146A (en) * | 2017-08-04 | 2019-02-26 | 上海裕芮信息技术有限公司 | A kind of unmanned plane core flight control system |
| CN107807375B (en) * | 2017-09-18 | 2021-04-06 | 南京邮电大学 | Unmanned aerial vehicle attitude tracking method and system based on multiple GPS receivers |
| WO2019071390A1 (en) * | 2017-10-09 | 2019-04-18 | 深圳市大疆灵眸科技有限公司 | Mechanical angle detecting method, pan/tilt head and machine readable storage medium |
| CN107817821A (en) * | 2017-10-27 | 2018-03-20 | 成都鼎信精控科技有限公司 | A kind of stable head and control method based on MEMS gyroscope combination |
| CN107943070B (en) * | 2017-11-01 | 2020-07-28 | 杭州睿杰智能空中机器人科技有限公司 | Method for controlling active disturbance rejection flight speed and attitude of unmanned helicopter |
| CN108181915A (en) * | 2017-12-19 | 2018-06-19 | 广东省航空航天装备技术研究所 | A kind of flight attitude regulation and control method of quadrotor unmanned plane |
| WO2019119441A1 (en) | 2017-12-22 | 2019-06-27 | 深圳市大疆创新科技有限公司 | Method for controlling pan-tilt by means of somatosensory controller, and pan-tilt, somatosensory controller and system |
| CN108592918A (en) * | 2018-04-26 | 2018-09-28 | 清华大学 | The full attitude algorithm method of MEMS IMU under swaying base |
| CN108983795A (en) * | 2018-05-07 | 2018-12-11 | 长江大学 | A kind of three-axis attitude bearing calibration and equipment |
| CN108549399B (en) * | 2018-05-23 | 2020-08-21 | 深圳市道通智能航空技术有限公司 | Aircraft yaw angle correction method and device and aircraft |
| CN108488572A (en) * | 2018-05-23 | 2018-09-04 | 高新兴科技集团股份有限公司 | A kind of active stabilization holder and its control method |
| CN109030862A (en) * | 2018-06-20 | 2018-12-18 | 南京信息工程大学 | A kind of gyroscope balancing device and its angle adaptive equalization adjusting method for ultrasonic aerovane |
| CN109194912A (en) * | 2018-08-06 | 2019-01-11 | 陈彦楠 | A kind of real-time head behaviortrace monitoring system and its control method |
| CN109398686B (en) * | 2018-09-14 | 2022-11-04 | 上海歌尔泰克机器人有限公司 | Rotor unmanned aerial vehicle and attitude control method thereof |
| CN109108936A (en) * | 2018-10-24 | 2019-01-01 | 电子科技大学 | A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion |
| CN109931929A (en) * | 2019-01-25 | 2019-06-25 | 南京薄幕软件科技有限公司 | A kind of UAV Attitude calculation method based on quaternary number |
| CN109674480B (en) * | 2019-02-02 | 2020-10-30 | 北京理工大学 | Human motion attitude calculation method based on improved complementary filtering |
| CN111580542B (en) * | 2019-02-15 | 2024-07-19 | 北京京东乾石科技有限公司 | Dynamic unmanned aerial vehicle formation control method, device and storage medium |
| CN109813308A (en) * | 2019-03-07 | 2019-05-28 | 京东方科技集团股份有限公司 | Attitude estimation method, apparatus and computer readable storage medium |
| CN110209182A (en) * | 2019-05-21 | 2019-09-06 | 云南民族大学 | A kind of quadrotor based on AVR single chip |
| CN110737194B (en) * | 2019-10-12 | 2022-10-25 | 江苏科技大学 | Multi-MEMS sensor combination attitude measurement method |
| WO2021134645A1 (en) * | 2019-12-31 | 2021-07-08 | 深圳市大疆创新科技有限公司 | Gimbal control method and gimbal |
| CN111141283A (en) * | 2020-01-19 | 2020-05-12 | 杭州十域科技有限公司 | Method for judging advancing direction through geomagnetic data |
| CN111459188B (en) * | 2020-04-29 | 2022-07-19 | 南京理工大学 | Quaternion-based multi-rotor nonlinear flight control method |
| CN111693019B (en) * | 2020-05-20 | 2021-04-20 | 西安交通大学 | Attitude sensing device and data fusion and attitude calculation method |
| CN111947654A (en) * | 2020-08-13 | 2020-11-17 | 杭州北斗东芯科技有限公司 | Navigation and control integrated chip and control method thereof |
| CN112180957A (en) * | 2020-09-15 | 2021-01-05 | 东北大学 | Three-redundancy arbitration switching method and device for unmanned aerial vehicle and computer equipment |
| CN112754473A (en) * | 2021-01-08 | 2021-05-07 | 华中科技大学鄂州工业技术研究院 | Spontaneous heating downloading and method for falling detection by fusing transfer learning algorithm |
| CN112873164A (en) * | 2021-01-14 | 2021-06-01 | 电子科技大学 | Automatic material handling robot |
| CN112873163A (en) * | 2021-01-14 | 2021-06-01 | 电子科技大学 | Automatic material carrying robot system and control method thereof |
| CN112965511B (en) * | 2021-02-08 | 2022-06-14 | 电子科技大学 | Simplified attitude control method suitable for flight control of quad-rotor unmanned aerial vehicle |
| CN113759706B (en) * | 2021-11-05 | 2022-02-08 | 普宙科技(深圳)有限公司 | Multi-rotor attitude active disturbance rejection control method and system based on axial angle vector |
| CN114114897B (en) * | 2021-11-27 | 2024-06-28 | 中国南方电网有限责任公司超高压输电公司大理局 | Unmanned aerial vehicle wind resistance control method and device, electronic equipment and storage medium |
| CN114610075A (en) * | 2022-05-10 | 2022-06-10 | 之江实验室 | Many rotor crafts of verting flight control system and many rotor unmanned aerial vehicle |
| CN115218898A (en) * | 2022-07-18 | 2022-10-21 | 中国航空工业集团公司沈阳空气动力研究所 | Offshore sailing ship operation measurement system and measurement method |
| CN117724540B (en) * | 2024-02-18 | 2024-04-19 | 成都航空职业技术学院 | Automatic control method for aircraft motor |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8645063B2 (en) * | 2010-12-22 | 2014-02-04 | Custom Sensors & Technologies, Inc. | Method and system for initial quaternion and attitude estimation |
| CN104898681B (en) * | 2015-05-04 | 2017-07-28 | 浙江工业大学 | A kind of quadrotor attitude acquisition method for approximately finishing card quaternary number using three ranks |
| CN105259750A (en) * | 2015-10-27 | 2016-01-20 | 四川豪斯特电子技术有限责任公司 | Method for balancing body of four-rotor aircraft |
-
2016
- 2016-07-07 CN CN201610531882.3A patent/CN106249745B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN106249745A (en) | 2016-12-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106249745B (en) | The control method of four axis unmanned planes | |
| CN107368091B (en) | A kind of stabilized flight control method of more rotor unmanned aircrafts based on finite time neurodynamics | |
| CN107957730B (en) | Unmanned aerial vehicle stable flight control method | |
| Derafa et al. | Super twisting control algorithm for the attitude tracking of a four rotors UAV | |
| Lozano | Unmanned aerial vehicles: Embedded control | |
| CN102830622B (en) | Auto-disturbance-rejection automatic flight control method for four-rotor aircraft | |
| CN111351482B (en) | Multi-rotor aircraft combined navigation method based on error state Kalman filtering | |
| Tayebi et al. | Attitude stabilization of a VTOL quadrotor aircraft | |
| Rodić et al. | The modeling and simulation of an autonomous quad-rotor microcopter in a virtual outdoor scenario | |
| Rodić et al. | Modeling and simulation of quad-rotor dynamics and spatial navigation | |
| Derafa et al. | Super twisting control algorithm for the four rotors helicopter attitude tracking problem | |
| CN108803639A (en) | A kind of quadrotor flight control method based on Backstepping | |
| Omari et al. | Hardware and software architecture for nonlinear control of multirotor helicopters | |
| CN104765272A (en) | Four-rotor aircraft control method based on PID neural network (PIDNN) control | |
| CN106155076A (en) | A kind of stabilized flight control method of many rotor unmanned aircrafts | |
| CN103837151A (en) | Pneumatic model-assisted navigation method for four-rotor-wing air vehicle | |
| CN107992084B (en) | Unmanned aerial vehicle robust attitude control method and device independent of angular velocity feedback | |
| CN106043695B (en) | A kind of dynamic multi-rotor unmanned aerial vehicle fixed pitch variable speed system of oil and control technology | |
| CN114489094A (en) | Unmanned gyroplane wind-disturbance-resistant control method based on acceleration feedback enhancement | |
| Emran et al. | Adaptive neural network control of quadrotor system under the presence of actuator constraints | |
| Etemadi | Mathematical dynamics, kinematics modeling and PID equation controller of quadcopter | |
| CN111857171B (en) | Unmanned aerial vehicle control method based on quality self-adaptive multilayer neural network | |
| Fang et al. | Design and nonlinear control of an indoor quadrotor flying robot | |
| Lembono et al. | Orientation filter and angular rates estimation in monocopter using accelerometers and magnetometer with the extended kalman filter | |
| Derafa et al. | Non-linear control algorithm for the four rotors UAV attitude tracking problem |
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 | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220921 Address after: 510670 Room 518, 91 Kefeng Road, Huangpu District, Guangzhou City, Guangdong Province Patentee after: Guangzhou Fangwei Information Technology Co.,Ltd. Address before: 215123 No. 199 benevolence Road, Suzhou Industrial Park, Jiangsu, China Patentee before: SOOCHOW University Effective date of registration: 20220921 Address after: 314500 02, No. 4, South Zaoqiang street, No. 1, Nanmen Gongnong Road, Chongfu Town, Tongxiang City, Jiaxing City, Zhejiang Province Patentee after: Jiaxing Zhixu Information Technology Co.,Ltd. Address before: 510670 Room 518, 91 Kefeng Road, Huangpu District, Guangzhou City, Guangdong Province Patentee before: Guangzhou Fangwei Information Technology Co.,Ltd. |
|
| TR01 | Transfer of patent right |