CN105955302A - Multi-rotor unmanned aerial vehicle environment autonomous monitoring control system and method - Google Patents

Abstract

The invention discloses a multi-rotor unmanned aerial vehicle environment autonomous monitoring control system and method. The system comprises a multi-rotor unmanned aerial vehicle, a ground communication conversion system and a mobile terminal. The multi-rotor unmanned aerial vehicle comprises an unmanned aerial vehicle control system and an unmanned aerial vehicle power system, wherein the unmanned aerial vehicle control system is used for controlling operation states of a multi-rotor unmanned aerial vehicle, including rising, hovering, barrier avoiding and falling, performing multidimensional automatic monitoring on pollutants in air in an operation process and sending acquired data; and the ground communication conversion system is used for receiving the data acquired by the unmanned aerial vehicle, sending the data to the mobile terminal, receiving a control instruction from the mobile terminal and sending the control instruction to the unmanned aerial vehicle. According to the invention, under the condition that the attitude of the unmanned aerial vehicle is stable, measurement precision is guaranteed, and multidimensional concentration detection of different pollutants is realized.

Description

A kind of many rotor wing unmanned aerial vehicles environment self monitor control system and method

Technical field

The present invention relates to unmanned plane and wireless sensing field, particularly relate to a kind of many rotor wing unmanned aerial vehicles environment autonomous Monitor control system and method.

Background technology

In recent years, environmental problem receives much concern, need the carrying out to the pollutant in air to monitor in real time and Gather data, in order to analyze the formulation etc. with Improving Measurements.At present, along with science and technology growing, profit Carry out monitoring of environmental with unmanned plane gradually to popularize.The control of unmanned plane and information gathering transmission are the passes of monitoring system One of key technology.Existing environmental monitoring unmanned plane project is to use specific remote controller to carry out wireless controlled mostly System, is stored by storage card for the data collected.

Use specific remote controller to carry out unmanned aerial vehicle (UAV) control, add the development cost of system and be not easy to take Band, unstable at high-altitude collecting location, it is susceptible to skew, it is impossible to UAV Attitude is perfectly controlled System, its control accuracy and credibility also can be affected by anthropic factor.Storage card storage unmanned plane is utilized to adopt The datagraphic information that arrives of collection, can not during unmanned plane during flying real-time analysis environments data, the most just It is not easy to adjust according to ambient conditions the flight path of unmanned plane, the workload of monitoring can be increased.

Summary of the invention

The technical problem to be solved in the present invention is to gather data for Artificial Control unmanned plane in prior art Easily produce the defect of error deviation, it is provided that a kind of unmanned plane that can automatically control carries out many rotations of environmental monitoring Wing unmanned plane environment self monitor control system and method.

The technical solution adopted for the present invention to solve the technical problems is:

The present invention provides a kind of many rotor wing unmanned aerial vehicles environment self monitor control system, unmanned including many rotors Machine, ground communication converting system and mobile terminal, many rotor wing unmanned aerial vehicles include that unmanned aerial vehicle control system is with unmanned Mechanomotive force system, wherein:

Unmanned aerial vehicle control system includes that main control module, gesture stability module, radio receiving transmitting module and pollutant are surveyed Amount module, for controlling the running status of many rotor wing unmanned aerial vehicles, including rising, hovering, avoidance and whereabouts, In running, the pollutant in air are carried out various dimensions automatically to monitor, and the data collected are sent Go out；

Ground communication converting system is for according to different communication modes, including online and off-line mode, receives The data that unmanned plane collects, are sent to mobile terminal, and receive the control instruction from mobile terminal, send out Give unmanned plane；

Mobile terminal for carrying out function setting to unmanned plane, and receives and the Monitoring Data of display unmanned plane.

Further, the unmanned aerial vehicle control system of the present invention also includes the image acquisition mould being connected with main control module Block, avoidance module and navigation module.

Further, the ground communication modular converter of the present invention includes bluetooth module and radio receiving transmitting module.

Further, the gesture stability module of the present invention includes main control MCU, six axle digital sensors and three axles Numeral magnetometer.

The present invention provides a kind of many rotor wing unmanned aerial vehicles environment self monitor control method, comprises the following steps:

S1, mobile terminal send routing instruction and control instruction to unmanned plane, control unmanned plane and move to initially Coordinate, is estimated and the value of attitude control system reading sensor by UAV Attitude, carries out attitude algorithm, To the roll of unmanned plane, pitching, these attitude angle of course, and then by cascade PID algorithm, digital filtering Algorithms etc. adjust the attitude of unmanned plane, eliminate interference signal and control unmanned plane holding balance；

S2, unmanned plane monitor the pollutant data in the air of current location in real time, gather ultrasonic sensor Value carries out height PID and controls, and is automatically moved into next position according to routing instruction and is monitored, and is moved through Barrier is carried out detection avoidance by journey；

The Monitoring Data collected by being wirelessly sent to mobile terminal, is completed institute on path by S3, unmanned plane After monitoring a little, auto-returned.

Further, in step S1 of the present invention attitude algorithm method particularly as follows:

The unmanned plane according to the measurement of attitude transducer gyroscope angular velocity on x, y, z direction of principal axis, resolves Obtain quaternary number increment at short notice, obtain unmanned plane quaternary numerical value at a time by integration, Thus calculate the Eulerian angles of this moment unmanned plane body.

Further, the method controlling to adjust the attitude of unmanned plane by double PID in step S1 of the present invention Particularly as follows:

Outer shroud input is Eulerian angles angle setpoint, and the Eulerian angles obtained with current body attitude algorithm are measured Value compares, and difference is through the target angular velocity signal of outer shroud PID controller output internal ring, and and current pose The magnitude of angular velocity that sensor obtains compares, and difference calculates through internal ring PID controller, is output as being given to electricity tune Pwm signal, electricity adjust output electric current by rotor motor control unmanned plane fuselage angle fly, so Rear attitude transducer detects angle and the magnitude of angular velocity of fuselage next time, carries out cas PID control next time. Wherein inner and outer rings PID controls with the circulation of relatively short period of time interval.

Further, step S1 of the present invention obtains unmanned plane and sit calibration method particularly as follows: use karr Speed position information is merged by graceful wave filter with GPS information, obtains the navigation information of unmanned plane accurately.

Further, step S2 of the present invention is adjusted the side of the attitude of unmanned plane by the highest control algolithm Method particularly as follows:

Controller input is height set, and compares with reality measurement height value, and difference is through height PID Controller, is output as pwm control signal, is added control rotor motor with the pwm signal of gesture stability output, Thus control the height of unmanned plane body；Being then passed through 20ms, body next time measured by ultrasonic sensor Actual height, carries out Altitude control next time；In control algolithm, height will be preset and divide 300 times gradually Increase the incoming height set of 6s altogether, control the speed that unmanned plane rises, it is ensured that what one key took off stablizes Property；The every 20ms of ultrasonic sensor detects a unmanned plane height, and highly PID controller is at actual differential Add the speed that integrated acceleration obtains in Xiang, certain coefficient ratio is set, to prevent unmanned plane from rising at a key Speed when flying is higher, due to the existence of integral error, removes speed and merge item after arriving preset height.

Further, in step S3 of the present invention, the Monitoring Data collected is passed through wireless transmission by unmanned plane Specifically include to the method for mobile terminal:

Presence, mobile terminal directly utilizes wifi, gprs, real by sending instruction to Cloud Server Now to unmanned aerial vehicle (UAV) control, and by Cloud Server monitoring store unmanned plane geographical position, pollutant measurement data, Image information, is sent request of data by wifi, gprs wireless signal to Cloud Server by mobile terminal and reads Take and video data；

Off-line state, utilizes ground communication end of convert as the communication terminal of unmanned plane Yu mobile terminal, ground Face communication end of convert includes radio receiving transmitting module and bluetooth module, mobile terminal to the control information of unmanned plane by Bluetooth module transmission is to communicating in end of convert, then is transferred to unmanned plane main control chip by radio receiving transmitting module；With Sample, the positional information of unmanned plane, pollutant measurement data are passed back by the end of convert that communicates with image acquisition information Mobile terminal, for follow-up analysis.

The beneficial effect comprise that: many rotor wing unmanned aerial vehicles environment self monitor of the present invention controls system System, on the basis of the independent navigation and autonomous hovering performance of many rotor wing unmanned aerial vehicles, completes the gas of certain dimension After bulk measurement, control unmanned plane and complete independently to rise, arrive appointed place hovering, autonomous measurement, avoidance, The tasks such as whereabouts, omnidistance unattended, and can be under the conditions of UAV Attitude is stable, it is ensured that measure essence Degree, it is achieved various dimensions, the Concentration Testing of different pollutant, can be by passing shifting back in real time for monitoring result Dynamic terminal is read out display, it is simple to adjust the flight path of unmanned plane according to the situation of monitoring of environmental, permissible Reduce the workload of monitoring；This system can improve the control accuracy of unmanned plane, it is achieved environmental gas high-precision Degree monitoring automatically.

Accompanying drawing explanation

Below in conjunction with drawings and Examples, the invention will be further described, in accompanying drawing:

Fig. 1 is the structure chart of many rotor wing unmanned aerial vehicles environment self monitor control system of the embodiment of the present invention；

Fig. 2 is the control system of many rotor wing unmanned aerial vehicles environment self monitor control system of the embodiment of the present invention Block diagram；

Fig. 3 is the Android of many rotor wing unmanned aerial vehicles environment self monitor control system of the embodiment of the present invention The structured flowchart of terminal；

Fig. 4 is the gesture stability of many rotor wing unmanned aerial vehicles environment self monitor control system of the embodiment of the present invention Algorithm schematic diagram；

Fig. 5 is the Altitude control of many rotor wing unmanned aerial vehicles environment self monitor control system of the embodiment of the present invention Algorithm schematic diagram；

In figure, 1-control system, 10-main control chip, 11-attitude control system, 12-navigation system, 13- Radio receiving transmitting module, 14-avoidance module, 15-image capture module, 16-pollutant measurement module, 2-ground Communication converting system, 3-Android terminal, 110-six axle digital sensor, 111-tri-number of axle word magnetometer, 112-main control MCU, 4-unmanned plane dynamical system, 40-electricity tune, 41-brushless electric machine, 42-propeller, 43- Battery, 30-unmanned plane during flying setting coordinate, 31-ship trajectory, 32-automatic homing, 33-monitors administrative division map Picture, 34-Air Quality Analysis, 35-Cloud Server, 36-battery electric quantity shows, 37-bluetooth.

Detailed description of the invention

In order to make the purpose of the present invention, technical scheme and advantage clearer, below in conjunction with accompanying drawing and reality Execute example, the present invention is further elaborated.Only should be appreciated that specific embodiment described herein In order to explain the present invention, it is not intended to limit the present invention.

As it is shown in figure 1, many rotor wing unmanned aerial vehicles environment self monitor control system of the embodiment of the present invention, including Many rotor wing unmanned aerial vehicles, ground communication converting system and mobile terminal, many rotor wing unmanned aerial vehicles include unmanned aerial vehicle (UAV) control System and unmanned plane dynamical system, wherein:

Unmanned aerial vehicle control system includes that main control module, gesture stability module, radio receiving transmitting module and pollutant are surveyed Amount module, for controlling the running status of many rotor wing unmanned aerial vehicles, including rising, hovering, avoidance and whereabouts, In running, the pollutant in air are carried out various dimensions automatically to monitor, and the data collected are sent Go out；

Ground communication converting system is for according to different communication modes, including online and off-line mode, receives The data that unmanned plane collects, are sent to mobile terminal, and receive the control instruction from mobile terminal, send out Give unmanned plane；

Mobile terminal for carrying out function setting to unmanned plane, and receives and the Monitoring Data of display unmanned plane.

Unmanned aerial vehicle control system also includes image capture module, avoidance module and the navigation being connected with main control module Module.Ground communication modular converter includes bluetooth module and radio receiving transmitting module.Gesture stability module includes main Control MCU, six axle digital sensors and three number of axle word magnetometers.Unmanned plane dynamical system includes many groups and master control The rotor module that MCU is connected, often group rotor module all includes electricity tune, brushless electric machine and the spiral being sequentially connected Oar.

As in figure 2 it is shown, in another specific embodiment of the present invention, many rotor wing unmanned aerial vehicles environment is independently supervised Survey control system, by many rotor wing unmanned aerial vehicles, radio communication end and Android terminal three parts composition, many Rotor wing unmanned aerial vehicle is mainly made up of control system and dynamical system two parts, and control system is based on microprocessor Control chip realizes complete machine control, mainly includes attitude control system, navigation system, radio receiving transmitting module, keeps away Barrier module, image capture module and atmosphere pollution measurement module；Dynamical system is mainly adjusted by electricity, brushless electricity Machine, propeller and model airplane battery composition.

Radio communication end, according to the online of system and off-line mode, has two kinds of different communication modes, the One is under presence, and Android terminal directly utilizes wifi, gprs, by sending to Cloud Server Instruction realizes unmanned aerial vehicle (UAV) control, and is surveyed by Cloud Server monitoring storage unmanned plane geographical position, pollutant Amount data, image information, sent to Cloud Server by described wifi, gprs wireless signal by Android Request of data reads and video data；The second is under off-line state, utilizes ground communication end of convert as nothing The man-machine communication terminal with Android terminal, described ground communication end of convert include radio receiving transmitting module and Bluetooth module, after have corresponding data conversion module, described radio receiving transmitting module to be responsible for unmanned plane between the two With the communication of ground end of convert, the bluetooth module in the end of convert of described ground and Android terminal realize bluetooth The control information of unmanned plane is extremely communicated in end of convert by communication, i.e. Android terminal by bluetooth module transmission, Unmanned plane main control chip is transferred to again by radio receiving transmitting module；Equally, the positional information of unmanned plane, pollutant Measurement data passes Android terminal with image acquisition information back by the end of convert that communicates, it is simple to follow-up analysis.

As it is shown on figure 3, Andriod terminal, it is possible to realize the setting coordinate to unmanned plane, automatic homing control System, receives and display environment Monitoring Data, monitored space area image, ship trajectory and unmanned plane battery electric quantity, And according to the pollution level of pollutant measurement data analysis differing heights, measurement data image and the situation of analysis Can synchronize to preserve to Android terminal SD card and Cloud Server, described Cloud Server, in system When using line model, can survey with positional information, the differing heights pollutant that real-time reception is sent from unmanned plane The data of amount, image information, and be stored in Cloud Server, after Android terminal checking user profile, Send data request signal by wireless signal to Cloud Server, Cloud Server differentiate request signal type it Data needed for Android terminal sends afterwards.

As in figure 2 it is shown, attitude control system, blend with six axle digital sensors and three number of axle word magnetometers, The balance using cas PID control to realize many rotor wing unmanned aerial vehicles controls and other required calculating.Many rotations Wing unmanned plane uses six axle digital sensors 110 and three number of axle word magnetometers 111 to blend to carry out attitude and estimate Meter, described six number of axle word sensor internal integrated tri-axial acceleration meter and three-axis gyroscopes, can be with 200Hz Frequency required acceleration and gyro data are provided, three number of axle word magnetometers can provide with the frequency of 50Hz Desirably magnetic direction data, thus obtain UAV Attitude accurately, it is simple to carry out attitude algorithm.

Navigation system 12 uses modern airmanship.Use a new generation low-power consumption, high-precision GPS module, And support China's Beidou navigation.The speed position information of many rotor wing unmanned aerial vehicles controls system by many rotor wing unmanned aerial vehicles The attitude estimator of system obtains.Kalman filter is used to be merged with GPS information by speed position information, The navigation information of available unmanned plane accurately.Then the flight information set according to navigation information and unmanned plane Positioning many rotor wing unmanned aerial vehicles, described location information can show on navigation map in real time.

Radio receiving transmitting module 13, enables when using off-line mode corresponding to system, is operable with exempting from licence 2.4G ISM band, high-power, high sensitivity, long distance for data, and weight only has tens grams. It is respectively arranged in many rotor wing unmanned aerial vehicles and ground communication end of convert, for telecommunication between the two, many The master control system of rotor wing unmanned aerial vehicle only need to use SPI interface to be connected with this wireless module, can carry out data Transmitting-receiving.

Ground communication converting system 2 is as it is shown in figure 1, be the communication mode under system off-line pattern, describedly Face communication end of convert as the communication terminal of unmanned plane Yu Android terminal, including radio receiving transmitting module with Bluetooth module.The step that under off-line mode, Android terminal sends instruction to unmanned plane is as follows:

Step one: set the control information to unmanned plane in Android terminal, is sent to logical by bluetooth module Letter end of convert；

Step 2: turned, by the bluetooth in communication end of convert, the information that step one receives by wireless module and send extremely The radio receiving transmitting module of unmanned plane；

Step 3: unmanned plane microprocessor main control chip reads the information of radio receiving transmitting module and controls unmanned plane.

The monitoring information of unmanned plane is passed Android terminal back and is inversely performed according to above-mentioned steps, and Android is eventually The communication with unmanned plane is held to carry out in real time.

Communication under communication end of convert line model accesses internet mainly by wifi, gprs module, Using Cloud Server to realize the intercommunication of Android terminal and unmanned plane, above-mentioned three is transmitted across in data Carrying out transceiving data in journey as a data frame, the content of described Frame includes frame head, function word, length Degree, data, check bit etc..Frame head mainly differentiates the transmitting terminal of Frame；Function word is mainly transmitting terminal The flag bit specially set for obtaining appointment data；The length of data in length i.e. Frame；Data are real Border receiving terminal data to be processed；Check bit is primarily to prevent from occurring number during wireless data transceiving Make receiving terminal receive wrong data according to LOF, described check bit passes through special algorithm, it is ensured that Between Android terminal, unmanned plane and Cloud Server, the accuracy of data transmission, enhances anti-interference.

Avoidance module 14, enables when being affected bigger when the flight of many rotor wing unmanned aerial vehicles by weather condition, When wind-force is higher than 3 grades, due to the impact of wind-force, unmanned plane can occur the situation of drift, may cause it Collide with building.So need install range-measurement system for measure unmanned plane distance barrier away from From.The feature of ultrasonic wave module has to comply with the requirement that detection angle is sufficiently large, measure the moderate range of distance. Owing to unmanned plane body is little, can closely fly near circuit, the measuring distance of selection be 6-10 rice. Ultrasonic emitting, reception and unmanned plane main control chip control three parts and constitute ultrasonic distance measuring module. Ultrasonic distance measuring module is controlled by main control chip and couples in cascaded fashion.At unmanned plane It has been respectively mounted ultrasonic distance measuring module, once three ultrasonic transceiver modules in left and right, first three orientation Opening, the range information in record three orientation will be sent to control system by panel simultaneously, controls system System carries out extraction and analysis immediately to differentiate whether unmanned plane excessively connects with barrier to three azran signals Closely.

Image capture module 15, is in order to record the atmospheric pollution information of surveillance area and to use high definition to take the photograph As head gathers the image of air, simultaneously by image information by ground communication converting system 2 or cloud service Device is transferred to Android terminal 3, in order to testing staff understands situation and the surrounding of on-the-spot air in real time, And the image transmitted by many rotor wing unmanned aerial vehicles uses the SD card of Android terminal or Cloud Server to deposit Storage, is convenient for measuring the later stage inspection of personnel and processes.

Atmosphere pollution measurement module 16, by using corresponding sensor, it is possible to achieve to atmospheric pollution (based on the particulate matter of PM2.5, PM10, the oxygen sulfur compound based on sulfur dioxide, with titanium dioxide for thing Nitrogen is main oxynitride etc.) and air in harmful gas precisely detect.About sensor Installation can select the sensor of multiple difference in functionality can also only install single sensor, it is achieved in air Different types of pollutant are selectively monitored.

The signal that sensor uses supporting microprocessor digital circuit blocks to be detected is analyzed, then It is connected with unmanned plane main control chip by serial ports, the pollutant levels that output records, changed by ground communication System 2 or Cloud Server are passed Android terminal back and are analyzed display.Described sensor is the least Type, it is easy to safeguard, reduces body weight and use cost.

The dynamical system 4 of many rotor wing unmanned aerial vehicles of the present invention as in figure 2 it is shown, make electricity consumption adjust 40, three-phase without Brush motor 41, propeller 42 and model airplane battery 43.Brushless electric machine is owing to eliminating brush and diverter composition Mechanical contact configuration, there is no commutation spark and mechanical friction, there is efficiency high, without electromagnetic interference, life-span The advantage such as long, reliable, uses the electricity supporting with brushless electric machine to be in harmonious proportion propeller, it is ensured that dynamical system The superperformance of system.The battery request of many rotor wing unmanned aerial vehicles has large current discharging capability, the most also needs volume Little, lightweight, select model airplane battery.

Android terminal 3, is everyone mobile product of carrying with, can be equipped with Android The mobile phone of system or PC, it is not limited to this.Carried out in described Android terminal by operator The instruction of unmanned plane sends, and adjusts the flight of unmanned plane in real time according to received information.It mainly realizes Function is as shown in Figure 3:

Android terminal arranges the wanted monitored area of unmanned plane by unmanned plane during flying setting coordinate module 30 Longitude and latitude and space coordinates, can show the throttle size of unmanned plane and rudder relative to position, right In the flight speed of unmanned plane, flight time, flying height also has corresponding coordinate and shows.

Ship trajectory 31, can show the ship trajectory of unmanned plane institute monitored area on map in real time, can To combine the distribution situation of monitored area air quality, unmanned plane during flying setting coordinate module 30 change accordingly Become monitoring position coordinates, it is achieved analyze in real time, measure flexibly.

Automatic homing module 32 complete can make unmanned plane automatic steady return in unmanned plane monitoring.

The interior aerography picture that can check unmanned plane monitored area of monitored space area image module 33 and about ring Border.

Air Quality Analysis module 34, can to Bluetooth receptions to atmospheric pollution data message at Reason is analyzed, the analytical standard of contrast country air quality, the air quality of display institute monitored area.

Cloud Server 35, stores the ship trajectory of unmanned plane, monitoring administrative division map under line model in real time Picture and pollutant measurement situation etc..User can log in and check.

Battery electric quantity display module 36, is used for showing the electricity remaining sum in many rotor wing unmanned aerial vehicles flight course.

Bluetooth module 37 is attached with communication transfer interface module 2, for sending out to unmanned plane in disconnection mode Send control instruction, the data of the image information, ship trajectory and the atmosphere pollution that are simultaneously monitored by unmanned plane Information returns Android and controls terminal and stored by SD card.

Many rotor wing unmanned aerial vehicles environment self monitor control method of the embodiment of the present invention, comprises the following steps:

S1, mobile terminal send routing instruction and control instruction to unmanned plane, control unmanned plane and move to initially Coordinate, is estimated and the value of attitude control system reading sensor by UAV Attitude, carries out attitude algorithm, To the roll of unmanned plane, pitching, these attitude angle of course, and then by cascade PID algorithm, digital filtering Algorithms etc. adjust the attitude of unmanned plane, and control unmanned plane holding balance；

S2, unmanned plane monitor the pollutant data in the air of current location in real time, gather ultrasonic sensor Value carries out height PID and controls, and is automatically moved into next position according to routing instruction and is monitored, and is moved through Barrier is carried out detection avoidance by journey；

The Monitoring Data collected by being wirelessly sent to mobile terminal, is completed institute on path by S3, unmanned plane After monitoring a little, auto-returned.

In step S1 attitude algorithm method particularly as follows:

The unmanned plane according to the measurement of attitude transducer gyroscope angular velocity on x, y, z direction of principal axis, resolves Obtain quaternary number increment at short notice, obtain unmanned plane quaternary numerical value at a time by integration, Thus calculate the Eulerian angles of this moment unmanned plane body.Final gained Eulerian angles are used to describe rigid body three The anglec of rotation of coordinate axes on dimension space.Define three anglecs of rotation:

Roll angle Rol: the angle rotated around y-axis, is just counterclockwise；

Angle of pitch Pit: the angle rotated around x-axis, is just counterclockwise；

Course angle Yaw: the angle rotated around z-axis, is just counterclockwise.

Body axis system b can regard as by geographic coordinate system g by rotation Eulerian angles on three coordinate axess Obtain.Definition anglec of rotation γ is that on unmanned plane, 1 A rotates around the outer fixed point O of fuselage, vector OA For initial position vector, elapsed time t rotational angle moves to some B, A and a B and is generally aligned in the same plane.

Definition from quaternary number Q:

Q(q₀,q₁,q₂,q₃)=q₀+q₁i+q₂j+q₃k (1)

$Q=cos\frac{\mathrm{\γ}}{2}+lsin\frac{\mathrm{\γ}}{2}i+m\sin \frac{\mathrm{\γ}}{2}j+nsin\frac{\mathrm{\γ}}{2}k=cos\frac{\mathrm{\γ}}{2}+u^{g}sin\frac{\mathrm{\γ}}{2}---\left(2\right)$

Wherein q₀、q₁、q₂、q₃For real number, i, j, k are mutually orthogonal unit vectors.

Quaternary number Q derivation can be obtained:

$\frac{dQ}{dt}=-\frac{\mathrm{\γ}}{2}sin\frac{\mathrm{\γ}}{2}+u^g\frac{\mathrm{\γ}}{2}cos\frac{\mathrm{\γ}}{2}+sin\frac{\mathrm{\γ}}{2}\frac{{\mathrm{du}}^g}{dt}---\left(3\right)$

Then according to Coriolis Theorem:

$\frac{{\mathrm{du}}^g}{dt}=C_b^g\frac{{\mathrm{du}}^b}{dt}+{\mathrm{\ω}}_{gb}^g\×u^g---\left(4\right)$

Wherein:

$\frac{{\mathrm{du}}^b}{dt}=0,{\mathrm{\ω}}_{gb}^g=\frac{d\mathrm{\γ}}{dt}{u}^g---\left(5\right)$

Therefore:

$\frac{{\mathrm{du}}^g}{dt}=0---\left(6\right)$

So:

$\frac{dQ}{dt}=-\frac{\mathrm{\γ}}{2}sin\frac{\mathrm{\γ}}{2}+u^g\frac{\mathrm{\γ}}{2}cos\frac{\mathrm{\γ}}{2}---\left(7\right)$

Due to:

$\frac{d\mathrm{\γ}}{2dt}{u}^g\⊗Q=\frac{d\mathrm{\γ}}{2dt}{u}^{g}cos\frac{\mathrm{\γ}}{2}-\frac{d\mathrm{\γ}}{2dt}sin\frac{\mathrm{\γ}}{2}---\left(8\right)$

Can obtain:

$\frac{dQ}{dt}=\frac{d\mathrm{\γ}}{2dt}{u}^g\⊗Q=\frac{1}{2}{\mathrm{\ω}}_{gb}^g\⊗Q---\left(9\right)$

The angular velocity measured due to attitude transducer gyroscope on unmanned plane carrier is body axis system fortune Dynamic angular velocity.And:

${\mathrm{\ω}}_{gb}^g=Q\⊗{\mathrm{\ω}}_{gb}^b\⊗Q*---\left(10\right)$

Can obtain

$\frac{dQ}{dt}=\frac{1}{2}{\mathrm{\ω}}_{gb}^g\⊗Q=\frac{1}{2}Q\⊗{\mathrm{\ω}}_{gb}^b\⊗Q*\⊗Q=\frac{1}{2}Q\⊗{\mathrm{\ω}}_{gb}^b---\left(11\right)$

Can be obtained by the multiplication formula of quaternary number:

$\left[\begin{array}{c}\frac{{\mathrm{dq}}_0}{dt}\\ \frac{{\mathrm{dq}}_1}{dt}\\ \frac{{\mathrm{dq}}_2}{dt}\\ \frac{{\mathrm{dq}}_3}{dt}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(12\right)$

Therefore the unmanned plane angular velocity on x, y, z direction of principal axis can measured according to attitude transducer gyroscope, Resolve and obtain quaternary number increment at short notice, obtain unmanned plane quaternary number at a time by integration Value, the relation of the Eulerian angles changed when quaternary numerical value and unmanned plane during flying is as follows:

$\left[\begin{array}{c}\mathrm{\θ}\\ \mathrm{\β}\\ \mathrm{\α}\end{array}\right]=\left[\begin{array}{c}arctan\frac{2(q_2{q}_3+q_0{q}_1)}{1-2(q_1^2+q_2^2)}\\ arcsin2(q_0{q}_2-q_1{q}_3)\\ \arctan \frac{2(q_1{q}_2+q_0{q}_3)}{1-2(q_2^2+q_3^2)}\end{array}\right]---\left(13\right)$

Wherein: θ, β, α represent three anglecs of rotation of unmanned plane respectively, and quaternary numerical value flies with unmanned plane Eulerian angles relation derivation during row is not introduced at this.

The Eulerian angles of this moment unmanned plane body can be calculated according to formula (12) and formula (13).It follows that work as Obtain the value of the quaternary number of four rotor wing unmanned aerial vehicles, just can resolve obtain the roll of unmanned plane, pitching, course this A little attitude angle, thus control the motion of unmanned plane.

But owing to there is integral operation during attitude algorithm, when the angular velocity signal of gyroscope detection exists During error, along with the effect of integration, error can be increasing, may cause the angle signal distortion obtained. At this moment it is accomplished by a kind of sensor that can believe for a long time and angle signal is provided, here it is the effect of accelerometer. Actual acceleration suffered on x, y, z direction of principal axis in body axis system b is sat at body with acceleration of gravity In mark system b, on three axles, the acceleration of projection compares, and obtains the two attitude error, then by this error value product Lease making is crossed the data obtained with gyroscope and is carried out complementary filter, corrects the integral error of gyroscope.Thus obtain Stablize reliable attitude angle data.During complementary filter, complementary filter coefficient can control gyroscope Degree of faith respective with accelerometer, if fully according to this attitude error by the integral error of gyroscope Compensate, trust accelerometer the most completely.Due to effect of noise on accelerometer, complementary filter coefficient needs root Depending on practical situation.

In step S1 by cascade PID algorithm controls adjust unmanned plane attitude method particularly as follows:

Cas PID control is the lifting that single loop PID controls quality, is the attitude control of four rotor wing unmanned aerial vehicles The core of system.When control system exist multiple influence factor output is had an impact time, single loop PID control The system that can not well reach controls requirement.All influence factors are considered to control loop, energy by cascade PID Effectively realize control performance, improve dynamic responding speed and the stability of system.

Fig. 4 is the cas PID control structure chart of four rotor wing unmanned aerial vehicles.Outer shroud input is Eulerian angles angle initialization Value, and compare with current body Eulerian angles measured value, difference is through the mesh of outer shroud PID controller output internal ring Marking angular velocity signal, and the magnitude of angular velocity obtained with current pose sensor compares, difference is through internal ring PID Controller calculates, and is output as being given to the pwm signal of electricity tune, and electricity adjusts output electric current to control nothing by rotor motor Man-machine fuselage angle is flown, and then attitude transducer detects angle and the angular velocity of fuselage next time Value, carries out cas PID control next time.Wherein inner and outer rings PID controls with the circulation of relatively short period of time interval.

In step S1 by digital filtering algorithm adjust unmanned plane attitude method particularly as follows:

Using the first-order low-pass ripple in digital filtering algorithm, amount of calculation is little, and the suitability is strong.Single order simulation is low Bandpass filter is hardware RC filtering, and deriving according to circuit to obtain its differential equation, then the filter of single order digital lowpass Ripple difference equation represents its differential equation, just can get its formula and is:

Y_n=a × X_n+(1-a)×Y_n-1 (14)

Wherein Y_nThe output valve filtered for this；A is filter factor, and its value is generally much less than 1；X_nFor this Secondary sample input value；Y_n-1For Y_nPrevious filtering output value.

The relation of the filter factor a and cut-off frequency f of this algorithm is:

$f=\frac{a}{2\mathrm{\π}T}---\left(15\right)$

Wherein π is pi, and T is the sampling interval duration of input value.

From above formula, output valve Y of this algorithm filtering_nIt is more likely to believe output valve Y filtered last time_n-1, and Sampled value X of this filtering_nImpact on this filtering output value is relatively small, and this is the most to a certain extent Filter the interference of this filtering high frequency signal.When input variable low frequency variations, this filtering algorithm is imitated Rate is at a relatively high, and good wave filtering effect.But, this filtering algorithm can not effectively filter out and adopt higher than 1/2 The interference signal of sample frequency, therefore this interference signal demand takes other suitable filtering algorithms.But for this Systematic difference, first-order low-pass ripple fully meets the control system requirement to filtering performance.

Step S1 obtains unmanned plane and sits calibration method particularly as follows: use Kalman filter by velocity location Information merges with GPS information, obtains the navigation information of unmanned plane accurately.

In step S2 by the highest control algolithm adjust unmanned plane attitude method particularly as follows:

Fig. 5 is the Altitude control structure chart of four rotor wing unmanned aerial vehicles.Controller input is height set, and with Actual height value of measuring compares, and difference, through height PID controller, is output as pwm control signal, with appearance State controls the pwm signal of output and is added control rotor motor, thus controls the height of unmanned plane body.Then Through 20ms, body actual height next time measured by ultrasonic sensor, carries out Altitude control next time. In control algolithm, height point will be preset and be gradually increased the incoming height set of common 6s for 300 times, control The speed that unmanned plane rises, it is ensured that the stability that one key takes off；Ultrasonic sensor every 20ms detection one Secondary unmanned plane height, highly PID controller add the speed that integrated acceleration obtains in actual differential term, Certain coefficient ratio is set, to prevent the unmanned plane speed when a key takes off higher, due to integral error Exist, after arriving preset height, remove speed merge item.

In step S2 unmanned plane carry out the method that automatically moves and monitor particularly as follows:

Unmanned plane completes independently to rise according to predetermined route, arrives appointed place and hovers, independently measures, keeps away Barrier, whereabouts task, and measurement data and the image of return are analyzed, and show institute at mobile terminal The image of monitored area, the ship trajectory of aircraft, model airplane battery electricity remaining sum and air quality, accordingly Monitoring information also can synchronized transmission in Cloud Server, make aircraft automatic homing monitoring is complete.

In step S3, the Monitoring Data collected is had by unmanned plane by the method being wirelessly sent to mobile terminal Body includes:

Presence, mobile terminal directly utilizes wifi, gprs, real by sending instruction to Cloud Server Now to unmanned aerial vehicle (UAV) control, and by Cloud Server monitoring store unmanned plane geographical position, pollutant measurement data, Image information, is sent request of data by wifi, gprs wireless signal to Cloud Server by mobile terminal and reads Take and video data；

Off-line state, utilizes ground communication end of convert as the communication terminal of unmanned plane Yu mobile terminal, ground Face communication end of convert includes radio receiving transmitting module and bluetooth module, mobile terminal to the control information of unmanned plane by Bluetooth module transmission is to communicating in end of convert, then is transferred to unmanned plane main control chip by radio receiving transmitting module；With Sample, the positional information of unmanned plane, pollutant measurement data are passed back by the end of convert that communicates with image acquisition information Mobile terminal, for follow-up analysis.

It should be appreciated that for those of ordinary skills, can be improved according to the above description Or conversion, and all these modifications and variations all should belong to the protection domain of claims of the present invention.

Claims (10)

1. rotor wing unmanned aerial vehicle environment self monitor control system more than a kind, it is characterised in that include many rotors Unmanned plane, ground communication converting system and mobile terminal, many rotor wing unmanned aerial vehicles include unmanned aerial vehicle control system and Unmanned plane dynamical system, wherein:

Unmanned aerial vehicle control system includes that main control module, gesture stability module, radio receiving transmitting module and pollutant are surveyed Amount module, for controlling the running status of many rotor wing unmanned aerial vehicles, including rising, hovering, avoidance and whereabouts, In running, the pollutant in air are carried out various dimensions automatically to monitor, and the data collected are sent Go out；

Ground communication converting system is for according to different communication modes, including online and off-line mode, receives The data that unmanned plane collects, are sent to mobile terminal, and receive the control instruction from mobile terminal, send out Give unmanned plane；

Mobile terminal for carrying out function setting to unmanned plane, and receives and the Monitoring Data of display unmanned plane.

Many rotor wing unmanned aerial vehicles environment self monitor control system the most according to claim 1, its feature Being, unmanned aerial vehicle control system also includes the image capture module being connected with main control module, avoidance module and leads Model plane block.

Many rotor wing unmanned aerial vehicles environment self monitor control system the most according to claim 1, its feature Being, ground communication modular converter includes bluetooth module and radio receiving transmitting module.

Many rotor wing unmanned aerial vehicles environment self monitor control system the most according to claim 1, its feature Being, gesture stability module includes main control MCU, six axle digital sensors and three number of axle word magnetometers.

5. a control method for many rotor wing unmanned aerial vehicles environment self monitor control system described in claim 1, It is characterized in that, comprise the following steps:

S1, mobile terminal send routing instruction and control instruction to unmanned plane, control unmanned plane and move to initially Coordinate, is estimated and the value of attitude control system reading sensor by UAV Attitude, carries out attitude algorithm, To the roll of unmanned plane, pitching, these attitude angle of course, and then by cascade PID algorithm, digital filtering Algorithms etc. adjust the attitude of unmanned plane, eliminate interference signal and control unmanned plane holding balance；

S2, unmanned plane monitor the pollutant data in the air of current location in real time, gather ultrasonic sensor Value carries out height PID and controls, and is automatically moved into next position according to routing instruction and is monitored, and is moved through Barrier is carried out detection avoidance by journey；

The Monitoring Data collected by being wirelessly sent to mobile terminal, is completed institute on path by S3, unmanned plane After monitoring a little, auto-returned.

Many rotor wing unmanned aerial vehicles environment self monitor control method the most according to claim 5, its feature Be, in step S1 attitude algorithm method particularly as follows:

The unmanned plane according to the measurement of attitude transducer gyroscope angular velocity on x, y, z direction of principal axis, resolves Obtain quaternary number increment at short notice, obtain unmanned plane quaternary numerical value at a time by integration, Thus calculate the Eulerian angles of this moment unmanned plane body.

Many rotor wing unmanned aerial vehicles environment self monitor control method the most according to claim 5, its feature Be, step S1 controls to adjust the method for the attitude of unmanned planes by double PID particularly as follows:

Outer shroud input is Eulerian angles angle setpoint, and the Eulerian angles obtained with current body attitude algorithm are measured Value compares, and difference is through the target angular velocity signal of outer shroud PID controller output internal ring, and and current pose The magnitude of angular velocity that sensor obtains compares, and difference calculates through internal ring PID controller, is output as being given to electricity tune Pwm signal, electricity adjust output electric current by rotor motor control unmanned plane fuselage angle fly, so Rear attitude transducer detects angle and the magnitude of angular velocity of fuselage next time, carries out cas PID control next time. Wherein inner and outer rings PID controls with the circulation of relatively short period of time interval.

Many rotor wing unmanned aerial vehicles environment self monitor control method the most according to claim 5, its feature It is, step S1 obtains unmanned plane and sits calibration method particularly as follows: use Kalman filter by velocity potential Confidence breath merges with GPS information, obtains the navigation information of unmanned plane accurately.

Many rotor wing unmanned aerial vehicles environment self monitor control method the most according to claim 5, its feature Be, in step S2 by the highest control algolithm adjust unmanned plane attitude method particularly as follows:

Controller input is height set, and compares with reality measurement height value, and difference is through height PID Controller, is output as pwm control signal, is added control rotor motor with the pwm signal of gesture stability output, Thus control the height of unmanned plane body；Being then passed through 20ms, body next time measured by ultrasonic sensor Actual height, carries out Altitude control next time；In control algolithm, height will be preset and divide 300 times gradually Increase the incoming height set of 6s altogether, control the speed that unmanned plane rises, it is ensured that what one key took off stablizes Property；The every 20ms of ultrasonic sensor detects a unmanned plane height, and highly PID controller is at actual differential Add the speed that integrated acceleration obtains in Xiang, certain coefficient ratio is set, to prevent unmanned plane from rising at a key Speed when flying is higher, due to the existence of integral error, removes speed and merge item after arriving preset height.

Many rotor wing unmanned aerial vehicles environment self monitor control method the most according to claim 5, its feature Be, in step S3 unmanned plane by the Monitoring Data collected by the method being wirelessly sent to mobile terminal Specifically include:

Presence, mobile terminal directly utilizes wifi, gprs, real by sending instruction to Cloud Server Now to unmanned aerial vehicle (UAV) control, and by Cloud Server monitoring store unmanned plane geographical position, pollutant measurement data, Image information, is sent request of data by wifi, gprs wireless signal to Cloud Server by mobile terminal and reads Take and video data；

Off-line state, utilizes ground communication end of convert as the communication terminal of unmanned plane Yu mobile terminal, ground Face communication end of convert includes radio receiving transmitting module and bluetooth module, mobile terminal to the control information of unmanned plane by Bluetooth module transmission is to communicating in end of convert, then is transferred to unmanned plane main control chip by radio receiving transmitting module；With Sample, the positional information of unmanned plane, pollutant measurement data are passed back by the end of convert that communicates with image acquisition information Mobile terminal, for follow-up analysis.