CN106547275A - A kind of new rotor class unmanned plane is automatically positioned control method - Google Patents

Abstract

The present invention relates to unmanned air vehicle technique field, specifically disclose a kind of new rotor class unmanned plane and be automatically positioned control method, specially with the electronic equipment with direction induction function and position location functionality, many rotor class unmanned planes control the direction and position of itself according to the direction and position of electronic equipment, so as to realize direction and the position follower of rotor class unmanned plane.Dynamic relative coordinate system is constructed mainly, metrical information is changed to relative coordinate system from measurement coordinate system, the attitude and positional information of Positioning Electronic Devices are used, which employs the positioning and orientation method that AGPS, gyro add magnetic compass, the problem affected by cost and precision by relative measurement is reduced, so as to ensure that the positioning and orientation means of degree of precision.The relative position and gesture stability of multi-rotor unmanned aerial vehicle are carried out using relative information, and is finally converted to measurement coordinate system and is definitely controlled.

Description

A kind of new rotor class unmanned plane is automatically positioned control method

Technical field

The present invention relates to unmanned air vehicle technique field, particularly relates to a kind of new rotor class unmanned plane and is automatically positioned behaviour Prosecutor method.

Background technology

In recent years, many rotor class unmanned air vehicle techniques have obtained quick development, its have flight stability it is good, Easily manipulation, low noise, simple structure and little hazardness the features such as, and can be easily and effectively various of carrying Business load militarily upper achieves quite varied application, such as aviation with civilian performing various tasks Shooting, agricultural plant protection, power circuit polling and military surveillance etc..

Manipulated by remote control more than existing rotor class unmanned plane, by manipulator by the rocking bar of remote control controlling The navigation attitude angle of rotor class unmanned plane processed and position, speed etc..The difficulty of this control mode is larger, manipulator The coordinated signals by multiple passages are needed, oneself desired aircraft state is realized.In order to effectively reduce many The manipulation threshold of rotor wing unmanned aerial vehicle, by means of intelligentized Autonomous Control technology, realizes the intelligence with manipulator Interactive controlling.Meet its space relative localization control to require.Using the invention, multi-rotor unmanned aerial vehicle can be met Orientation stares being automatically obtained functionally, and related control method is not retrieved at present in the literature.

The content of the invention

It is an object of the invention to provide a kind of new rotor class unmanned plane is automatically positioned control method, it is main to apply Under the relative auxiliary positioning scene that unmanned plane orientation stares application scenarios and outdoor, the method can change existing Have such unmanned plane that the pattern of precise manual manipulation is carried out using remote control, so as to significantly facilitate rotor class nobody The use and application of machine.

To achieve these goals, present invention employs following technical scheme：A kind of new rotor class unmanned plane Control method is automatically positioned, it is including the electronic equipment with direction induction function and position location functionality, described Electronic equipment is wirelessly set up with rotor class unmanned plane and is coupled, and comprises the following steps that：

(1) electronic equipment is given at the longitude under measurement coordinate system, latitude, height and course angle, definition For λ, B, h, γ, under measurement coordinate system, position and course angle of the value for geographic coordinate system.

(2) the airborne measurements device having using unmanned gyroplane, it is possible to obtain in the geographical coordinate of measurement Longitude and latitude, height and course angle under system, orientates λ as_T,B_T,h_T,γ_T。

(3) relative coordinate system is built, relative position relation is obtained based on absolute measurement position：

Assume, in limited range, to ignore the Curvature varying of the earth, define the local frame of reference, coordinate is former For λ, B, h, x-axis point to the course angle γ direction of electronic equipment to point, and y-axis is orientated as perpendicular to the ground, upwards for Just, then z-axis constitutes right-handed coordinate system.

The geographical position being had according to electronic equipment and course, and the geographical position that multi-rotor unmanned aerial vehicle has Put and course, carry out coordinate transform, be calculated under relative coordinate system the relative position of multi-rotor unmanned aerial vehicle and Course angle.Geographic coordinate system position is carried out into the change of geocentric coordinate system, according to the signal of GPS, Which is based on U.S.'s WGS84 coordinate systems, carries out following changes in coordinates, obtains its longitude and latitude high as follows：

Basic terrestrial coordinates parameter is defined first

Earth major radius：A=6378137 ± 2 (m)；

The product of terrestrial gravitation and earth quality：GM=3986005 × 108m³/s²±0.6×108m³/s²；

Normalization second order zonal harmonic coefficient：C20=-484.16685 × 10^-6±1.3×10^-9；

Earth gravitational field second order band spherical harmonic coefficient：J2=108263 × 10^-8；

Rotational-angular velocity of the earth：ω=7292115 × 10^-11rad/s±0.150×10^-11rad/s；

Ellipticity f=0.003352810664

The geocentric coordinate system conversion of GPS receiver data is carried out based on this.

Lr=L π/180

X=(N+H) cos (B) cos (Lr) (1)

Y=(N+H) cos (B) sin (Lr)

Z=(N (1-e²)+H)sin(B)

In formula, L, B, H directly receive the signal for obtaining for receiver.It is high according to resulting longitude and latitude, it is converted into Coordinate x under geocentric coordinate system, y, z.

(4) coordinate system x obtained based on receiver according to electronic equipment_h,y_h,z_h, and the side that magnetic compass is given Parallactic angle γ, by electronic equipment wirelessly with rotor class unmanned plane UNICOM, electronic equipment can be corresponding Control signal be sent on rotor class unmanned plane.

(5) controller on rotor class unmanned plane receives directional information and its position that electronic equipment is sensed After information, the direction controlling of itself is generated on this basis given given with position control：

Rotor class unmanned plane is according to the geocentric coordinate system coordinate x obtained by self poisoning equipment_a,y_a,z_a, carry out phase To position calculation.Define earth axes as follows：Using reference electronic equipment position as zero, X-axis is pointed to and is defined by azimuth, and position is set to y-axis with the earth's core line, upwards for just, Z axis are constituted Right-handed coordinate system.

Assume plane distance electronic equipment in kilometer range, negligible earth curvature, it is believed that earth axes x Axle extends along ground.Based on the geocentric coordinates of flight, then the phase place flown relative to electronic equipment can be obtained Position relationship is as follows：

Then have can obtain flying under earth axes it is as follows relative to the dependent coordinate of electronic equipment place Ground Point：

According to the elevation information of electronic equipment itself, corrected Calculation obtain based on the relative position of electronic equipment be

After controller on rotor class unmanned plane receives the directional information that electronic equipment senses and its positional information, Generate position control to give, it is also possible to be superimposed that one variable or invariant after again as rotor class unmanned plane Direction controlling give.

(6) position with electronic equipment is as the center of circle, with rotor class unmanned plane between electronic equipment it is relative away from Make to justify from for radius；On with the position of electronic equipment as straight line a bit, the direction with electronic equipment as slope, Make a ray；The ray made and circle must have an intersection point, using the intersection point as the given of position control.

According to formula (4), the concentric radius of circle that can obtain aircraft relative to electronic equipment is：

Then there is the relative position control instruction to be：

x_rc=R, y_rc=y_r,z_rc=0 (6)

The course angle instruction of aircraft simultaneously can be calculated as：

(7) after control instruction generation, based on required control instruction, carry out the control of position and course Realize.For the position loop of general rotor class unmanned plane, its main controlling organization is motor speed, The control in orientation realized by gesture stability, then can be given and following realize step.

Position control passage can be divided into horizontal control passage and height control passage, due to height control with angle not There is coupled relation, therefore directly can carry out, obtain following controlled quentity controlled variable.

According to desired height angular velocity, the input signal of reversal phase lift, according to following rotor power Learn equation and obtain correlation.

In formula, coefficient is taken respectively for k_p3=2, k_d3=0.8.

The total life for obtaining is typically based on the number of rotor and is decomposed, according to four rotors, six rotors and eight rotations The wing carries out corresponding rotating speed decomposition.In addition also need to consider the pitching and roll angle that horizontal level control passes through It is counter to be resolved.

Horizontal level control adopts PID controller, with expectation target relative position (x_rc,z_rc) and it is actual relative Position (x_r,z_r) difference as input signal, form following control form：

In formula, coefficient is respectively set as k_p1=2.3, k_i1=0.3, k_d1=0.8；k_p2=2.3, k_i2=0.3, k_d2=0.8；

Using the accekeration of horizontal level as anti-settlement module input information, the fortune based on multi-rotor aerocraft It is dynamic to learn and kinetics equation, do following derivation.During the hovering of many rotors with slower flight, it will be assumed that Angular velocityLess, then in kinematical equation, cross term is about zero, can be by the kinetics of barycenter and motion Learn model to be written as

Sin ψ is multiplied by 1 both sides of equation in formula (11), cos ψ are multiplied by 2 both sides of equation, and equation 1 deducts equation 2 Can obtain

And to first equation deformation in formula (11), then can obtain

Instruction (the φ of the angle of pitch, roll angle and the yaw angle of attitude inner looping can be now obtained then_c,θ_c,ψ_c), together When can also obtain basic controlled quentity controlled variable U₁。

(8) gesture stability in mobile process is carried out according to attitude command, and attitude is referred to after position reaches The whole zero setting of order, except course angle keeps command heading angle.The input after multi-axle motor fusion can then be obtained Controlled quentity controlled variable is：

Four tunnel input signal U of typical case of multi-rotor unmanned aerial vehicle can be then obtained more than comprehensive₁,U₂,U₃,U₄。

(9) the rotating speed distribution of each motor is carried out according to the configured number of rotor, with typical 4 rotor, 6 rotors is Example is illustrated：

Four rotors：According to the general configuration of four rotors, the compound mode that can obtain four motors is as follows：

According to the known signal of known control law output, the speed controling signal size of motor is reversely solved, can

Using this signal as the pulse-width-modulating signal input value of motor, then can obtain through electronic speed regulation The motor drive signal of device output.

Six rotors：The configuration of six rotors, it is only necessary to change the compound mode of motor, realizes that the decomposition of signal is driven It is dynamic.According to basic configuration, the compound mode that can obtain six motors is：

Due to there are 6 position quantities, four equations can be with additive postulate conditionThen may be used Each motor control amount is obtained in the hope of solution to be respectively：

The direction controlling that rotor class unmanned plane is generated according to more than gives and position control gives to control itself Direction and position, so as to realize direction and the position follower of rotor class unmanned plane.

The beneficial effects of the present invention is：After said apparatus, electronic equipment is wirelessly and rotor Class unmanned plane set up connection, by autonomous induction to directional information and own location information send many rotor classes to Unmanned plane, after the controller on many rotor class unmanned planes receives direction information and positional information, according to life Into direction controlling it is given and position control gives to control the direction and position of itself, so as to realize rotor class The direction of unmanned plane and position follower, it is not necessary to which manipulator directly controls the direction and position of rotor class unmanned plane, Its operating burden is significantly reduced, facilitates its manipulation to rotor class unmanned plane.It is capable of achieving outdoor auxiliary phase To positioning, and meet multi-rotor unmanned aerial vehicle and stare automatically flight, the original employing handss of change under set a distance really The method of dynamic precise control.

Description of the drawings

In order to be illustrated more clearly that embodiment of the present invention technical scheme, below will be to wanting needed for embodiment description The accompanying drawing for using is briefly described, it should be apparent that, drawings in the following description be only the present invention in order to The embodiment of the present invention or technical scheme of the prior art are illustrated more clearly that, below by p- embodiment or existing Accompanying drawing to be used needed for technology description is briefly described, for those of ordinary skill in the art, On the premise of not paying creative work, can be with according to these other accompanying drawings of accompanying drawings acquisition.

Fig. 1 is the structural representation of the present invention；

Fig. 2 is the motor combination schematic diagram of four rotor wing unmanned aerial vehicles；

Fig. 3 is the motor combination schematic diagram of six rotor wing unmanned aerial vehicles.

Specific embodiment

Below in conjunction with the accompanying drawing in the embodiment of the present invention, the technical scheme in the embodiment of the present invention is carried out clearly Chu, it is fully described by, it is clear that described embodiment is only a part of embodiment of the invention, rather than Whole embodiments.Based on the embodiment in the present invention, those of ordinary skill in the art are not making creation Property work under the premise of the every other embodiment that obtained, belong to the scope of protection of the invention.

A kind of new rotor class unmanned plane is automatically positioned control method, including fixed with direction induction function and position The electronic equipment of bit function, the electronic equipment are wirelessly set up with rotor class unmanned plane and are coupled, tool Body step is as follows：

(1) electronic equipment is given at the longitude under measurement coordinate system, latitude, height and course angle, definition For λ, B, h, γ, under measurement coordinate system, position and course angle of the value for geographic coordinate system.

(2) the airborne measurements device having using unmanned gyroplane, it is possible to obtain in the geographical coordinate of measurement Longitude and latitude, height and course angle under system, orientates λ as_T,B_T,h_T,γ_T。

(3) relative coordinate system is built, relative position relation is obtained based on absolute measurement position：

Assume, in limited range, to ignore the Curvature varying of the earth, define the local frame of reference, coordinate is former For λ, B, h, x-axis point to the course angle γ direction of electronic equipment to point, and y-axis is orientated as perpendicular to the ground, upwards for Just, then z-axis constitutes right-handed coordinate system.

The geographical position being had according to electronic equipment and course, and the geographical position that multi-rotor unmanned aerial vehicle has Put and course, carry out coordinate transform, be calculated under relative coordinate system the relative position of multi-rotor unmanned aerial vehicle and Course angle.Geographic coordinate system position is carried out into the change of geocentric coordinate system, according to the signal of GPS, Which is based on U.S.'s WGS84 coordinate systems, carries out following changes in coordinates, obtains its longitude and latitude high as follows：

Basic terrestrial coordinates parameter is defined first

Earth major radius：A=6378137 ± 2 (m)；

The product of terrestrial gravitation and earth quality：GM=3986005 × 108m³/s²±0.6×108m³/s²；

Normalization second order zonal harmonic coefficient：C20=-484.16685 × 10^-6±1.3×10^-9；

Earth gravitational field second order band spherical harmonic coefficient：J2=108263 × 10^-8；

Rotational-angular velocity of the earth：ω=7292115 × 10^-11rad/s±0.150×10^-11rad/s；

Ellipticity f=0.003352810664

The geocentric coordinate system conversion of GPS receiver data is carried out based on this.

Lr=L π/180

X=(N+H) cos (B) cos (Lr) (1)

Y=(N+H) cos (B) sin (Lr)

Z=(N (1-e²)+H)sin(B)

In formula, L, B, H directly receive the signal for obtaining for receiver.It is high according to resulting longitude and latitude, it is converted into Coordinate x under geocentric coordinate system, y, z.

(4) coordinate system x obtained based on receiver according to electronic equipment_h,y_h,z_h, and the side that magnetic compass is given Parallactic angle γ, by electronic equipment wirelessly with rotor class unmanned plane UNICOM, electronic equipment can be corresponding Control signal be sent on rotor class unmanned plane.

(5) controller on rotor class unmanned plane receives directional information and its position that electronic equipment is sensed After information, the direction controlling of itself is generated on this basis given given with position control：

Rotor class unmanned plane is according to the geocentric coordinate system coordinate x obtained by self poisoning equipment_a,y_a,z_a, carry out phase To position calculation.Define earth axes as follows：Using reference electronic equipment position as zero, X-axis is pointed to and is defined by azimuth, and position is set to y-axis with the earth's core line, upwards for just, Z axis are constituted Right-handed coordinate system.

Assume plane distance electronic equipment in kilometer range, negligible earth curvature, it is believed that earth axes x Axle extends along ground.Based on the geocentric coordinates of flight, then the phase place flown relative to electronic equipment can be obtained Position relationship is as follows：

Then have can obtain flying under earth axes it is as follows relative to the dependent coordinate of electronic equipment place Ground Point：

According to the elevation information of electronic equipment itself, corrected Calculation obtain based on the relative position of electronic equipment be

After controller on rotor class unmanned plane receives the directional information that electronic equipment senses and its positional information, Generate position control to give, it is also possible to be superimposed that one variable or invariant after again as rotor class unmanned plane Direction controlling give.

(6) as shown in figure 1, with the position of electronic equipment as the center of circle, with the same electronic equipment of rotor class unmanned plane Between relative distance be radius make justify；A bit on the position of electronic equipment as straight line, with electronic equipment Direction be slope, make a ray；The ray made and circle must have an intersection point, using the intersection point as position Put the given of control.

According to formula (4), the concentric radius of circle that can obtain aircraft relative to electronic equipment is：

Then there is the relative position control instruction to be：

x_rc=R, y_rc=y_r,z_rc=0 (6)

The course angle instruction of aircraft simultaneously can be calculated as：

(7) after control instruction generation, based on required control instruction, carry out the control of position and course Realize.For the position loop of general rotor class unmanned plane, its main controlling organization is motor speed, The control in orientation realized by gesture stability, then can be given and following realize step.

Position control passage can be divided into horizontal control passage and height control passage, due to height control with angle not There is coupled relation, therefore directly can carry out, obtain following controlled quentity controlled variable.

According to desired height angular velocity, the input signal of reversal phase lift, according to following rotor power Learn equation and obtain correlation.

In formula, coefficient is taken respectively for k_p3=2, k_d3=0.8.

The total life for obtaining is typically based on the number of rotor and is decomposed, according to four rotors, six rotors and eight rotations The wing carries out corresponding rotating speed decomposition.In addition also need to consider the pitching and roll angle that horizontal level control passes through It is counter to be resolved.

Horizontal level control adopts PID controller, with expectation target relative position (x_rc,z_rc) and it is actual relative Position (x_r,z_r) difference as input signal, form following control form：

In formula, coefficient is respectively set as k_p1=2.3, k_i1=0.3, k_d1=0.8；k_p2=2.3, k_i2=0.3, k_d2=0.8；

Using the accekeration of horizontal level as anti-settlement module input information, the fortune based on multi-rotor aerocraft It is dynamic to learn and kinetics equation, do following derivation.During the hovering of many rotors with slower flight, it will be assumed that Angular velocityLess, then in kinematical equation, cross term is about zero, can be by the kinetics of barycenter and motion Learn model to be written as

Sin ψ is multiplied by 1 both sides of equation in formula (11), cos ψ are multiplied by 2 both sides of equation, and equation 1 deducts equation 2 Can obtain

And to first equation deformation in formula (11), then can obtain

Instruction (the φ of the angle of pitch, roll angle and the yaw angle of attitude inner looping can be now obtained then_c,θ_c,ψ_c), together When can also obtain basic controlled quentity controlled variable U₁。

(8) gesture stability in mobile process is carried out according to attitude command, and attitude is referred to after position reaches The whole zero setting of order, except course angle keeps command heading angle.The input after multi-axle motor fusion can then be obtained Controlled quentity controlled variable is：

Four tunnel input signal U of typical case of multi-rotor unmanned aerial vehicle can be then obtained more than comprehensive₁,U₂,U₃,U₄。

(9) the rotating speed distribution of each motor is carried out according to the configured number of rotor, with typical 4 rotor, 6 rotors is Example is illustrated：

Four rotors：With reference to Fig. 2, according to the general configuration of four rotors, the compound mode of four motors can be obtained such as Under：

According to the known signal of known control law output, the speed controling signal size of motor is reversely solved, can

Using this signal as the pulse-width-modulating signal input value of motor, then can obtain through electronic speed regulation The motor drive signal of device output.

Six rotors：With reference to Fig. 3, the configuration of six rotors, it is only necessary to change the compound mode of motor, realize signal Decomposition drive.According to basic configuration, the compound mode that can obtain six motors is：

Due to there are 6 position quantities, four equations can be with additive postulate conditionThen may be used Each motor control amount is obtained in the hope of solution to be respectively：

The direction controlling that rotor class unmanned plane is generated according to more than gives and position control gives to control itself Direction and position, so as to realize direction and the position follower of rotor class unmanned plane.

Presently preferred embodiments of the present invention is the foregoing is only, it is not to limit the present invention, all at this Within bright spirit and principle, any modification, equivalent substitution and improvements made etc. should be included in this Within bright protection domain.

Claims (1)

1. a kind of new rotor class unmanned plane is automatically positioned control method, it is characterised in that：Including with side To inducing function and the electronic equipment of position location functionality, the electronic equipment wirelessly with rotor class Unmanned plane sets up connection, comprises the following steps that：

(1) electronic equipment is given at the longitude under measurement coordinate system, latitude, height and course angle, definition For λ, B, h, γ, under measurement coordinate system, position and course angle of the value for geographic coordinate system；

(2) the airborne measurements device having using unmanned gyroplane, it is possible to obtain in the geographical coordinate of measurement Longitude and latitude, height and course angle under system, orientates λ as_T,B_T,h_T,γ_T；

(3) relative coordinate system is built, relative position relation is obtained based on absolute measurement position：

Assume, in limited range, to ignore the Curvature varying of the earth, define the local frame of reference, coordinate Origin is λ, B, h, and x-axis points to the course angle γ direction of electronic equipment, and y-axis is orientated as perpendicular to the ground, upwards For just, then z-axis constitutes right-handed coordinate system；

The geographical position being had according to electronic equipment and course, and the geography that multi-rotor unmanned aerial vehicle has Position and course, carry out coordinate transform, are calculated the relative position of multi-rotor unmanned aerial vehicle under relative coordinate system And course angle；Geographic coordinate system position is carried out into the change of geocentric coordinate system, according to the signal of GPS, Which is based on U.S.'s WGS84 coordinate systems, carries out following changes in coordinates, obtains its longitude and latitude high as follows：

Basic terrestrial coordinates parameter is defined first

Earth major radius：A=6378137 ± 2 (m)；

The product of terrestrial gravitation and earth quality：GM=3986005 × 108m³/s²±0.6×108m³/s²；

Normalization second order zonal harmonic coefficient：C20=-484.16685 × 10^-6±1.3×10^-9；

Earth gravitational field second order band spherical harmonic coefficient：J2=108263 × 10^-8；

Rotational-angular velocity of the earth：ω=7292115 × 10^-11rad/s±0.150×10^-11rad/s；

Ellipticity f=0.003352810664

The geocentric coordinate system conversion of GPS receiver data is carried out based on this；

Lr=L π/180

$N=a/\sqrt{1-e^{2}sin{\left(B\right)}^2}$

X=(N+H) cos (B) cos (Lr) (1)

Y=(N+H) cos (B) sin (Lr)

Z=(N (1-e²)+H)sin(B)

In formula, L, B, H directly receive the signal for obtaining for receiver.It is high according to resulting longitude and latitude, it is converted into Coordinate x under geocentric coordinate system, y, z；

(4) coordinate system x obtained based on receiver according to electronic equipment_h,y_h,z_h, and the side that magnetic compass is given Parallactic angle γ, by electronic equipment wirelessly with rotor class unmanned plane UNICOM, electronic equipment can be corresponding Control signal be sent on rotor class unmanned plane；

(5) controller on rotor class unmanned plane receives directional information and its position that electronic equipment is sensed After information, the direction controlling of itself is generated on this basis given given with position control：

Rotor class unmanned plane is according to the geocentric coordinate system coordinate x obtained by self poisoning equipment_a,y_a,z_a, carry out Relative position is calculated；Define earth axes as follows：Using reference electronic equipment position as zero, X-axis is pointed to and is defined by azimuth, and position is set to y-axis with the earth's core line, upwards for just, Z axis are constituted Right-handed coordinate system；

Assume plane distance electronic equipment in kilometer range, negligible earth curvature, it is believed that earth axes X-axis extends along ground；Based on the geocentric coordinates of flight, then the phase flown relative to electronic equipment can be obtained Position position relationship is as follows：

$G=\left[\begin{array}{ccc}-\sin \mathrm{\γ}\sin L-\cos \mathrm{\γ}\sin B\cos L& \sin \mathrm{\γ}\cos L-\cos \mathrm{\γ}\sin B\sin L& \cos \mathrm{\γ}\cos B\\ \cos B\cos L& \cos B\sin L& \sin B\\ -\cos \mathrm{\γ}\sin L+\sin \mathrm{\γ}\sin B\cos L& \cos \mathrm{\γ}\cos L+\sin \mathrm{\γ}\sin B\sin L& -\sin \mathrm{\γ}\cos B\end{array}\right]---\left(2\right)$

Then have can obtain flying under earth axes it is as follows relative to the dependent coordinate of electronic equipment place Ground Point：

$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=G\left[\begin{array}{c}{x}_a\\ y_a\\ z_a\end{array}\right]---\left(3\right)$

According to the elevation information of electronic equipment itself, corrected Calculation obtain based on the relative position of electronic equipment be

$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=\left[\begin{array}{c}x\\ y-H\\ z\end{array}\right]---\left(4\right)$

It is after controller on rotor class unmanned plane receives the directional information that electronic equipment senses and its positional information, raw It is given into position control, it is also possible to be superimposed that one variable or invariant after again as rotor class unmanned plane Direction controlling gives；

(6) position with electronic equipment is as the center of circle, with rotor class unmanned plane between electronic equipment it is relative away from Make to justify from for radius；On with the position of electronic equipment as straight line a bit, the direction with electronic equipment as slope, Make a ray；The ray made and circle must have an intersection point, using the intersection point as the given of position control；

According to formula (4), the concentric radius of circle that can obtain aircraft relative to electronic equipment is：

$R=\sqrt{x_r^2+y_r^2+z_r^2}---\left(5\right)$

Then there is the relative position control instruction to be：

x_rc=R, y_rc=y_r,z_rc=0 (6)

The course angle instruction of aircraft simultaneously can be calculated as：

$\left\{\begin{array}{c}r+\mathrm{\&pi;},\mathrm{\&gamma;}<\mathrm{\&pi;}\\ \mathrm{\&gamma;}-\mathrm{\&pi;},\mathrm{\&gamma;}\&GreaterEqual;\mathrm{\&pi;}\end{array}\right.---\left(7\right)$

(7) after control instruction generation, based on required control instruction, carry out the control of position and course Realize.For the position loop of general rotor class unmanned plane, its main controlling organization is motor speed, The control in orientation realized by gesture stability, then can be given and following realize step；

Position control passage can be divided into horizontal control passage and height control passage, due to height control and angle There is no coupled relation, therefore directly can carry out, obtain following controlled quentity controlled variable；

$\stackrel{\&CenterDot;\&CenterDot;}{y}=k_{p3}(y_{rc}-y_r)+k_{d3}\frac{d(y_{rc}-y_r)}{dt}---\left(8\right)$

According to desired height angular velocity, the input signal of reversal phase lift is dynamic according to following rotor Mechanical equation obtains correlation；

$U_1=\frac{1}{\cos \mathrm{\&phi;}\cos \mathrm{\&theta;}}\left(k_{p3}\right(y_{rc}-y_r)+k_{d3}({\stackrel{\&CenterDot;}{y}}_{rc}-{\stackrel{\&CenterDot;}{y}}_r)+mg)---\left(9\right)$

In formula, coefficient is taken respectively for k_p3=2, k_d3=0.8；

The total life for obtaining is typically based on the number of rotor and is decomposed, according to four rotors, six rotors and eight Rotor carries out corresponding rotating speed decomposition.In addition also need to consider the pitching and rolling that horizontal level control passes through Angle is counter to be resolved；

Horizontal level control adopts PID controller, with expectation target relative position (x_rc,z_rc) and actual phase To position (x_r,z_r) difference as input signal, form following control form：

$\stackrel{\&CenterDot;\&CenterDot;}{x}=k_{p1}(x_{rc}-x_r)+k_{i1}\&Integral;\left(x_{rc}-x_r\right)dt+k_{d1}({\stackrel{\&CenterDot;}{x}}_{rc}-{\stackrel{\&CenterDot;}{x}}_r)$

(10)

$\stackrel{\&CenterDot;\&CenterDot;}{z}=k_{p1}(z_{rc}-z_r)+k_{i1}\&Integral;\left(z_{rc}-z_r\right)dt+k_{d1}({\stackrel{\&CenterDot;}{z}}_{rc}-{\stackrel{\&CenterDot;}{z}}_r)$

In formula, coefficient is respectively set as k_p1=2.3, k_i1=0.3, k_d1=0.8；k_p2=2.3, k_i2=0.3, k_d2=0.8；

Using the accekeration of horizontal level as anti-settlement module input information, based on multi-rotor aerocraft Kinesiology and kinetics equation, do following derivation.During the hovering of many rotors with slower flight, can be false If angular velocityLess, then in kinematical equation, cross term is about zero, can be by the kinetics of barycenter and fortune Dynamic model of learning is written as

$\stackrel{\&CenterDot;\&CenterDot;}{x}=(sin\mathrm{\&theta;}cos\mathrm{\&phi;}cos\mathrm{\&psi;}+sin\mathrm{\&phi;}sin\mathrm{\&psi;})\frac{U_1}{m}$

(11)

$\stackrel{\&CenterDot;\&CenterDot;}{z}=(sin\mathrm{\&theta;}cos\mathrm{\&phi;}sin\mathrm{\&psi;}-sin\mathrm{\&phi;}cos\mathrm{\&psi;})\frac{U_1}{m}$

Sin ψ is multiplied by 1 both sides of equation in formula (11), cos ψ are multiplied by 2 both sides of equation, and equation 1 deducts equation 2 Can obtain

$m\stackrel{\&CenterDot;\&CenterDot;}{x}sin\mathrm{\&psi;}-m\stackrel{\&CenterDot;\&CenterDot;}{z}cos\mathrm{\&psi;}=U_{1}sin\mathrm{\&phi;}$

${\mathrm{\&phi;}}_d=arcsin\&lsqb;\frac{m}{U_1}(\stackrel{\&CenterDot;\&CenterDot;}{x}sin\mathrm{\&psi;}-\stackrel{\&CenterDot;\&CenterDot;}{y}cos\mathrm{\&psi;})\&rsqb;---\left(12\right)$

And to first equation deformation in formula (11), then can obtain

$sin\mathrm{\&theta;}=\&lsqb;\frac{m\stackrel{\&CenterDot;\&CenterDot;}{x}}{U_1}-sin\mathrm{\&phi;}\sin \mathrm{\&psi;}\&rsqb;/cos\mathrm{\&phi;}cos\mathrm{\&psi;}$

(13)

Instruction (the φ of the angle of pitch, roll angle and the yaw angle of attitude inner looping can be now obtained then_c,θ_c,ψ_c), Basic controlled quentity controlled variable U can also be obtained simultaneously₁；

(8) gesture stability in mobile process is carried out according to attitude command, and by attitude after position reaches The whole zero setting of instruction, except course angle keeps command heading angle；Can then obtain defeated after multi-axle motor fusion Entering controlled quentity controlled variable is：

$U_2=k_{p\mathrm{\&phi;}}{e}_{\mathrm{\&phi;}}+k_{d\mathrm{\&phi;}}\frac{{\mathrm{de}}_{\mathrm{\&phi;}}}{dt}+k_{i\mathrm{\&phi;}}\&Integral;e_{\mathrm{\&phi;}}$

$U_3=k_{p\mathrm{\&theta;}}{e}_{\mathrm{\&theta;}}+k_{d\mathrm{\&theta;}}\frac{{\mathrm{de}}_{\mathrm{\&theta;}}}{dt}+k_{i\mathrm{\&theta;}}\&Integral;e_{\mathrm{\&theta;}}---\left(14\right)$

$U_4=k_{p\mathrm{\&psi;}}{e}_{\mathrm{\&psi;}}+k_{d\mathrm{\&psi;}}\frac{{\mathrm{de}}_{\mathrm{\&psi;}}}{dt}+k_{i\mathrm{\&psi;}}\&Integral;e_{\mathrm{\&psi;}}$

Four tunnel input signal U of typical case of multi-rotor unmanned aerial vehicle can be then obtained more than comprehensive₁,U₂,U₃,U₄。