CN104460685A - Control system for four-rotor aircraft and control method of control system - Google Patents

Abstract

The invention discloses a control system for a four-rotor aircraft. The control system comprises a master controller, an inertial measurement unit, a geomagnetometer, a distance measuring sensor, a GPS module, a camera module, an image processing unit, a motor drive module and four motors, wherein the inertial measurement unit, the geomagnetometer, the distance measuring sensor and the GPS module are connected with the master controller, the camera module is connected with the master controller through the image processing unit, the master controller is connected with the four motors through the motor drive module, and the four motors drive four airscrews of the four-rotor aircraft respectively. The invention further comprises a control method of the control system. The control method involves an attitude control algorithm, a height determination control algorithm and a data fusion algorithm. Data fusion of the inertial measurement unit and a positioning system is adopted for a control structure and the control method, and stability and reliability of the system are improved.

Description

A kind of control system of quadrotor and control method thereof

Technical field

The invention belongs to aircraft field, particularly a kind of control system of quadrotor and control method thereof.

Background technology

Unmanned vehicle refers to does not need driver to operate, and by wireless remote control or self programmed control, utilizes aerodynamic force to carry flight and recyclable reusable aircraft.Unmanned vehicle appears at nineteen twenties, is to carry out target practice as target drone for emptying cannon army when occurring the earliest.After the Gulf War, due to the outstanding representation of unmanned plane in war, the research and development carrying out unmanned plane are all fallen over each other in countries in the world, and advanced unmanned plane can carry various detecting devices and perform reconnaissance and surveillance task, even can equip attack weapon and perform strike task.

Unmanned vehicle is divided into fixed-wing and rotary wind type two kinds, rotor wing unmanned aerial vehicle has many-sided advantage compared with fixed-wing unmanned plane, rotor wing unmanned aerial vehicle can vertical takeoff and landing, have good low-altitude low-speed flying quality and can aloft hover, break-in is aloft flexible and little to take-off and landing site requirements.In military domain, rotor wing unmanned aerial vehicle can perform scouting, monitors, lure the tasks such as shop, attack and communication relay station.In civilian and police field, rotor wing unmanned aerial vehicle can be widely used in the fields such as traffic monitoring, aeroplane mapping, Disaster Assessment and rescue, environmental protection and power circuit line walking.Quadrotor be a kind of electronic, can many rotary wind types remote control/automated spacecraft of vertical takeoff and landing, belong to non-co-shaft type disc-shaped flying craft.Compared with conventional rotary aircraft, quadrotor does not need adjustment screw propeller inclination angle to change attitude, but is carried out the attitude of change of flight device by the rotating speed changing four screw propellers, therefore compact conformation.Due to the increase of screw propeller quantity, load-carrying also becomes large thereupon, and particularly its four rotors are symmetrical, the anti-twisted moment produced is cancelled out each other, and does not therefore need extra reactive torque tail-rotor, compared with the helicopter of normal arrangement, quadrotor physical construction is simple, and cost is lower, is easy to safeguard.Its four screw propellers are symmetrical, make the maneuverability of quadrotor stronger, the stability that static state is spiraled is better, also the microminaturization of type is more easily realized, therefore be particularly suitable for carrying out the tasks such as supervision, scouting at environment near the ground such as indoor, city and jungles, there is good military and civilian prospect.

Existing small-sized quadrotor generally uses Inertial Measurement Unit to measure attitude, thus controls the flight attitude of aircraft, and positioning system obtains the positional information of aircraft, seldom Inertial Measurement Unit and positioning system is carried out information fusion.

Summary of the invention

In order to solve the technical matters that above-mentioned background technology is mentioned, the present invention aims to provide a kind of control system and control method thereof of quadrotor, adopt control structure and the method for the data fusion of Inertial Measurement Unit and positioning system, improve stability and the reliability of system.

In order to realize above-mentioned technical purpose, technical scheme of the present invention is:

A kind of control system of quadrotor, comprise master controller and the Inertial Measurement Unit be attached thereto respectively, magnetometer, distance measuring sensor and GPS module, also comprise camera module, graphics processing unit, motor drive module and 4 motors, camera module is connected with master controller through graphics processing unit, master controller is connected with 4 motors respectively through motor drive module, and described 4 motors drive 4 screw propellers of quadrotor respectively; Described Inertial Measurement Unit, magnetometer, distance measuring sensor and GPS module detect the inertial data of quadrotor, bearing data, altitude information and position data respectively and send these data to master controller, master controller produces throttle signal according to aforementioned data and sends throttle signal to motor drive module, motor drive module drives 4 motor rotations according to throttle signal, and described camera module collection position picture signal also obtains positional information by graphics processing unit and sends positional information to master controller.

Wherein, above-mentioned control system also comprises bluetooth module, and described bluetooth module connects master controller, and master controller realizes the data interaction of itself and ground control station by this bluetooth module.

Wherein, the model of above-mentioned Inertial Measurement Unit is MPU6050, and it comprises 3-axis acceleration sensor and three-axis gyroscope; The model of magnetometer is HMC5983; Distance measuring sensor is HC-SR04 ultrasonic sensor; The model of camera module is OV7670; The model of GPS module is HOLUX M-89; The model of master controller is TIVA M4.

The present invention also comprises the control method of the control system based on above-mentioned a kind of quadrotor, comprise gesture stability algorithm, described gesture stability algorithm adopts cascade pid algorithm, cascade pid algorithm comprises outer shroud and inner ring, outer shroud is that the attitude data of Inertial Measurement Unit and magnetometer collection is converted to hypercomplex number, again hypercomplex number is converted to Eulerian angle, the Eulerian angle of these Eulerian angle and expectation are carried out PID arithmetic, Output speed, inner ring is as the angular velocity expected using the angular velocity of outer shroud PID arithmetic output, PID arithmetic is carried out with the angular velocity of Inertial Measurement Unit collection, export the pwm signal of 4 motors.

The present invention also comprises the control method of the control system based on above-mentioned a kind of quadrotor, comprise surely high control algolithm, described fixed high control algolithm adopts cascade pid algorithm, cascade pid algorithm comprises outer shroud and inner ring, outer shroud is that the height value of the height value of distance measuring sensor collection and expectation is carried out PID arithmetic, output speed, inner ring is as the speed expected using the speed of outer shroud PID output, the speed obtained through First-order Integral with the acceleration of Inertial Measurement Unit collection carries out PID arithmetic, exports the pwm signal of 4 motors.

The present invention also comprises the control method of the control system based on above-mentioned a kind of quadrotor, comprise data anastomosing algorithm, data fusion is carried out by Federated Filters, described Federated Filters comprises 1 senior filter and the first ~ three subfilter, Inertial Measurement Unit is as common reference system, the data of Inertial Measurement Unit and GPS module collection input the first subfilter and carry out position data fusion, the data of Inertial Measurement Unit and distance measuring sensor collection input the second subfilter and carry out altitude information fusion, the data of Inertial Measurement Unit and magnetometer collection input the 3rd subfilter and carry out bearing data fusion, give senior filter by the data that 3 subfilters export again and carry out data fusion, its concrete steps are as follows:

(1) during initialization, common condition distributes once to subfilter by senior filter, and then each subfilter works independently;

(2) each subfilter independently carries out Kalman filtering according to respective system equation and measurement equation, namely carries out time renewal and measure upgrading, and the filter result of synchronization is sent into senior filter by each subfilter;

(3) to the filter result that each subfilter is sent here, senior filter only directly merges common condition and corresponding evaluated error covariance, retains the value after merging, until next merges the moment.

Adopt the beneficial effect that technique scheme is brought:

(1) in the present invention each sensor under the coordination of master controller, mutual cooperation, stablize each attitude parameter of quadrotor, thus make aircraft can complete various psychomotor task by stability and high efficiency, when system initialization, utilize the information of GPS module collection to carry out initial alignment to Inertial Measurement Unit, make it to be operated in optimum condition;

(2) the present invention adopts data fusion method, and by building Federated Filters, makes the data that collect more accurately and reliably;

(3) control method of the present invention comprises gesture stability algorithm, fixed high control algolithm etc., and these control algolithms adopt the pid control algorithm of cascade, make the control of system more stable.

Accompanying drawing explanation

Fig. 1 is system architecture diagram of the present invention;

Fig. 2 is gesture stability algorithm schematic diagram of the present invention;

Fig. 3 is fixed high control algolithm schematic diagram of the present invention;

Fig. 4 is data anastomosing algorithm schematic diagram of the present invention.

Embodiment

Below with reference to accompanying drawing, technical scheme of the present invention is described in detail.

System architecture diagram of the present invention as shown in Figure 1, a kind of control system of quadrotor, comprise master controller and the Inertial Measurement Unit be attached thereto respectively, magnetometer, distance measuring sensor and GPS module, also comprise camera module, graphics processing unit, motor drive module and 4 motors, camera module is connected with master controller through graphics processing unit, master controller is connected with 4 motors respectively through motor drive module, and described 4 motors drive 4 screw propellers of quadrotor respectively; Described Inertial Measurement Unit, magnetometer, distance measuring sensor and GPS module detect the inertial data of quadrotor, bearing data, altitude information and position data respectively and send these data to master controller, master controller produces throttle signal according to aforementioned data and sends throttle signal to motor drive module, motor drive module drives 4 motor rotations according to throttle signal, and described camera module collection position picture signal also obtains positional information by graphics processing unit and sends positional information to master controller.

In the present embodiment, control system also comprises bluetooth module, and described bluetooth module connects master controller, and master controller realizes the data interaction of itself and ground control station by this bluetooth module.The model of Inertial Measurement Unit is MPU6050, and it comprises 3-axis acceleration sensor and three-axis gyroscope; The model of magnetometer is HMC5983; Distance measuring sensor is HC-SR04 ultrasonic sensor; The model of camera module is OV7670; The model of GPS module is HOLUX M-89; The model of master controller is TIVA M4.Inertial Measurement Unit MPU6050 needs to carry out initial alignment before operation, and GPS module HOLUX M-89 obtains the initial position of quadrotor, speed and attitude information and is directly passed to MPU6050, and MPU6050 carries out initial alignment according to these information.

The present invention also comprises the control method based on above-mentioned four-rotor aircraft control system, comprises gesture stability algorithm, fixed high control algolithm, Orientation control algorithm and data anastomosing algorithm.

Before analysis gesture stability algorithm, first founding mathematical models, needs to do following hypothesis:

1. quadrotor is rigid body symmetrically;

2. the initial point of inertial coordinates system E and aircraft geometric center and barycenter are positioned at same position;

3. resistance suffered by quadrotor and gravity do not affect by factors such as flying heights, always remain unchanged;

4. the pulling force of quadrotor all directions and thruster rotating speed is square in direct ratio.

Be defined as follows again:

Crab angle ψ: Ox at the projection of OXY plane and X-axis angle;

Pitching angle theta: Oz is at the projection of OXZ plane and Z axis angle;

Roll angle φ: Oy at the projection of OYZ plane and Y-axis angle.

According to Newton second law and Formula of Coordinate System Transformation etc., derive kinetics equation:

In formula (1), [x y z] ^tfor the displacement of the lines of quadrotor under navigational coordinate system, for acceleration of motion, m is vehicle mass, θ, φ are respectively the crab angle of body, the angle of pitch and roll angle, and l is the distance of center, rotor face to quadrotor barycenter, I _x, I _y, I _zfor axial principal moment of inertia.Adding the descriptive equation to the momentum moment by above-mentioned kinetic equation, the equation that algorithm directly relies on can obtained.

$\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]=\left[\begin{array}{c}{F}_1+F_2+F_3+F_4\\ F_4-F_2\\ F_3-F_1\\ F_2+F_4-F_3-F_1\end{array}\right]=\left[\begin{array}{c}{k}_t\underset{i=1}{\overset{4}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^2\\ k_t({\mathrm{\ω}}_4^2-{\mathrm{\ω}}_2^2)\\ k_t({\mathrm{\ω}}_3^2-{\mathrm{\ω}}_1^2)\\ k_d({\mathrm{\ω}}_1^2-{\mathrm{\ω}}_2^2+{\mathrm{\ω}}_3^2-{\mathrm{\ω}}_4^2)\end{array}\right]---\left(2\right)$

In formula (2), U ₁for vertical speed controlled quentity controlled variable, U ₂for rolling input control amount, U ₃for pitch control subsystem amount, U ₄for driftage controlled quentity controlled variable, ω is gyroplane rotate speed, F _ipulling force (i=1,2,3,4) suffered by rotor.Due to throttle (namely exporting PWM dutycycle) and F _ibecome approximate linear relationship, i.e. F _i=k*ai (ai is four motor PWM dutycycles), then relation between the amount of being under control ai and U1, U2, U3, U4.This mathematical model directly establishes four-way PWM dutycycle to the control of flight attitude, is the theoretical foundation of gesture stability.

Gesture stability algorithm schematic diagram of the present invention as shown in Figure 2, gesture stability algorithm adopts cascade pid algorithm, cascade pid algorithm comprises outer shroud and inner ring, outer shroud is that the attitude data of Inertial Measurement Unit and magnetometer collection is converted to hypercomplex number, again hypercomplex number is converted to Eulerian angle, the Eulerian angle of these Eulerian angle and expectation are carried out PID arithmetic, Output speed, inner ring is as the angular velocity expected using the angular velocity of outer shroud PID arithmetic output, PID arithmetic is carried out with the angular velocity of Inertial Measurement Unit collection, export the pwm signal of 4 motors, thus the attitude of adjustment quadrotor.Wherein, hypercomplex number (x, y, z, w) is simple supercomplex, is the number that expression conventional in 3D graphics rotates, utilizes hypercomplex number original attitude data can be converted to the matrix of a hypercomplex number, conveniently store attitude data, reduce storage area.Hypercomplex number is an intermediate quantity.The conversion formula that can obtain hypercomplex number and Eulerian angle under cartesian coordinate system is:

Fixed high control algolithm schematic diagram of the present invention as shown in Figure 3, high control algolithm adopts cascade pid algorithm, cascade pid algorithm comprises outer shroud and inner ring, outer shroud is that the height value of the height value of distance measuring sensor collection and expectation is carried out PID arithmetic, output speed, inner ring is as the speed expected using the speed of outer shroud PID output, the speed obtained through First-order Integral with the acceleration of Inertial Measurement Unit collection carries out PID arithmetic, export the pwm signal of 4 motors, thus determine the flying height of quadrotor.

Because acceleration transducer can not catch the change that uniform motion brings to position, therefore introduce OV7670 camera, auxiliary positioning.OV7670 camera collection view data, and given graphics processing unit, graphics processing unit analysis of image data, obtain quadrotor current location information, primary controller is given by positional information, primary controller carries out PID arithmetic, the expectation of adjustment attitude angle, thus makes quadrotor arrive stable position.

Data anastomosing algorithm schematic diagram of the present invention as shown in Figure 4, data fusion is carried out by Federated Filters, this Federated Filters comprises 1 senior filter MF and 3 subfilter LF1, LF2, LF3, Inertial Measurement Unit is as common reference system, the data of Inertial Measurement Unit and GPS module collection input subfilter LF1 and carry out position data fusion, the data of Inertial Measurement Unit and distance measuring sensor collection input subfilter LF2 and carry out altitude information fusion, the data of Inertial Measurement Unit and magnetometer collection input subfilter LF3 and carry out bearing data fusion, give senior filter by the data that 3 subfilters export again and carry out data fusion,

Wherein, 3 subfilters adopt common Kalman filter, and the information that 3 subfilters export directly merges by senior filter, and do not need filtering, its concrete steps are as follows:

(1) during initialization, common condition distributes once to subfilter by senior filter, then each subfilter works independently: original allocation primary information, because inertial navigation system take part in the filtering of three subfilters simultaneously, inertial navigation state is common condition, and its information is directly distributed in each subfilter according to information conservation principle:

$\left\{\begin{array}{cc}{\hat{X}}_{\mathrm{ci}}={\hat{X}}_{\mathrm{cg}}& \\ P_{\mathrm{ci}}^{-1}={\mathrm{\β}}_i{P}_{\mathrm{cg}}^{-1}& i=\mathrm{1,2,3}\\ Q_{\mathrm{ci}}^{-1}={\mathrm{\β}}_i{Q}_{\mathrm{cg}}^{-1}& \end{array}\right.---\left(4\right)$

Wherein, β ₁, β ₂and β ₃for information sharing scheme, meet β ₁+ β ₂+ β ₃=1, its value can be selected according to actual conditions.Q is interference noise (process noise) the variance intensity battle array of common condition, and subscript " c " represents common condition, is inertial system state." g " represents that the overall situation is estimated.

The value principle of following recommended information partition factor and the distribution principle of common reference information.

Be located in Federated Filters, involved navigation subsystem, except inertial navigation, also has N number of non-similar navigational subsystem.Because inertial navigation is common reference system, it take part in the filtering of the N number of subfilter be made up of this N number of subsystem and inertial navigation, so the information of inertial navigation should be distributed between this N number of subfilter.According to information conservation principle, partition factor should meet β _iit is the information sharing scheme that i-th subfilter obtains.If the mean squared error matrix of the evaluated error of X is the estimated quality that P, P describe to X, and P ^-1for information matrix.P is larger, and the estimated quality of X is poorer, information matrix P ^-1less; Otherwise P is less, the estimated quality of X is better, information matrix P ^-1larger.By formula can find out, do to distribute to the information of inertial navigation system, in fact exactly the estimation mean squared error matrix Pc of the inertial navigation of participation i-th sub-filter filtering is expanded 1/ β _idoubly.Therefrom can find out, β _iless, then the multiple of Pc expansion is larger.Because Kalman filter can do the utilization of weighted, automatically so β according to the quality of information quality _iless, utilize weight lower to inertial navigation information, the filtering accuracy of this subfilter depends primarily on the information quality of i-th subsystem, and the output information role of inertial navigation system reduces relatively; Vice versa.

In sum, the rule that inertial navigation information is distributed in subfilter: subsystem precision is poorer, the partition factor of inertial navigation information should be larger; Subsystem precision is higher, and the partition factor of inertial navigation information should suitably obtain less.Because subsystem precision is higher, corresponding subfilter filtering accuracy is by β _iimpact less, now, β _iobtain the less inertial navigation information making total amount limited can play one's part to the full in the subfilter at lower accuracy subsystem place.

(2) each subfilter independently carries out Kalman filtering according to respective system equation and measurement equation, namely carries out time renewal and measure upgrading, and the filter result of synchronization is sent into senior filter by each subfilter.

(3) to the filter result that each subfilter is sent here, senior filter only directly merges common condition and corresponding evaluated error covariance, retains the value after merging, until next merges the moment.The senior filter data fusion cycle is identical with the subfilter filtering cycle, 3 subfilters is often walked in the result of filtering, takes out common condition p _ci, i=1,2,3, pass to senior filter.Senior filter completes the optimal synthesis of information according to formula (5), forms the integrated information of global system p _cg:

$\left\{\begin{array}{c}{P}_{\mathrm{cg}}^{-1}=P_{c1}^{-1}+P_{c2}^{-1}+P_{c3}^{-1}\\ {\hat{X}}_{\mathrm{cg}}\left(k\right)=P_{\mathrm{cg}}[P_{c1}^{-1}{\hat{X}}_{c1}\left(k\right)+P_{c2}^{-1}{\hat{X}}_{c2}\left(k\right)+P_{c3}^{-1}{\hat{X}}_{c3}\left(k\right)]\end{array}\right.---\left(5\right)$

Above embodiment is only and technological thought of the present invention is described, can not limit protection scope of the present invention with this, and every technological thought proposed according to the present invention, any change that technical scheme basis is done, all falls within scope.

Claims (6)

1. the control system of a quadrotor, it is characterized in that: comprise master controller and the Inertial Measurement Unit be attached thereto respectively, magnetometer, distance measuring sensor and GPS module, also comprise camera module, graphics processing unit, motor drive module and 4 motors, camera module is connected with master controller through graphics processing unit, master controller is connected with 4 motors respectively through motor drive module, and described 4 motors drive 4 screw propellers of quadrotor respectively; Described Inertial Measurement Unit, magnetometer, distance measuring sensor and GPS module detect the inertial data of quadrotor, bearing data, altitude information and position data respectively and send these data to master controller, master controller produces throttle signal according to aforementioned data and sends throttle signal to motor drive module, motor drive module drives 4 motor rotations according to throttle signal, and described camera module collection position picture signal also obtains positional information by graphics processing unit and sends positional information to master controller.

2. the control system of a kind of quadrotor according to claim 1, is characterized in that: also comprise bluetooth module, and described bluetooth module connects master controller, and master controller realizes the data interaction of itself and ground control station by this bluetooth module.

3. the control system of a kind of quadrotor according to claim 1, is characterized in that: the model of described Inertial Measurement Unit is MPU6050, and it comprises 3-axis acceleration sensor and three-axis gyroscope; The model of described magnetometer is HMC5983; Described distance measuring sensor is HC-SR04 ultrasonic sensor; The model of described camera module is OV7670; The model of described GPS module is HOLUX M-89; The model of described master controller is TIVA M4.

4. based on a kind of described in claim 1 control method of control system of quadrotor, it is characterized in that: comprise gesture stability algorithm described in gesture stability algorithm and adopt cascade pid algorithm, cascade pid algorithm comprises outer shroud and inner ring, outer shroud is that the attitude data of Inertial Measurement Unit and magnetometer collection is converted to hypercomplex number, again hypercomplex number is converted to Eulerian angle, the Eulerian angle of these Eulerian angle and expectation are carried out PID arithmetic, Output speed, inner ring is as the angular velocity expected using the angular velocity of outer shroud PID arithmetic output, PID arithmetic is carried out with the angular velocity of Inertial Measurement Unit collection, export the pwm signal of 4 motors.

5. based on a kind of described in claim 1 control method of control system of quadrotor, it is characterized in that: comprise surely high control algolithm, described fixed high control algolithm adopts cascade pid algorithm, cascade pid algorithm comprises outer shroud and inner ring, outer shroud is that the height value of the height value of distance measuring sensor collection and expectation is carried out PID arithmetic, output speed, inner ring is as the speed expected using the speed of outer shroud PID output, the speed obtained through First-order Integral with the acceleration of Inertial Measurement Unit collection carries out PID arithmetic, exports the pwm signal of 4 motors.

6. based on a kind of described in claim 1 control method of control system of quadrotor, it is characterized in that: comprise data anastomosing algorithm, data fusion is carried out by Federated Filters, described Federated Filters comprises 1 senior filter and the first ~ three subfilter, Inertial Measurement Unit is as common reference system, the data of Inertial Measurement Unit and GPS module collection input the first subfilter and carry out position data fusion, the data of Inertial Measurement Unit and distance measuring sensor collection input the second subfilter and carry out altitude information fusion, the data of Inertial Measurement Unit and magnetometer collection input the 3rd subfilter and carry out bearing data fusion, give senior filter by the data that 3 subfilters export again and carry out data fusion, its concrete steps are as follows:

(1) during initialization, common condition distributes once to subfilter by senior filter, and then each subfilter works independently;

(2) each subfilter independently carries out Kalman filtering according to respective system equation and measurement equation, namely carries out time renewal and measure upgrading, and the filter result of synchronization is sent into senior filter by each subfilter;

(3) to the filter result that each subfilter is sent here, senior filter only directly merges common condition and corresponding evaluated error covariance, retains the value after merging, until next merges the moment.