CN105739513A - Quadrotor flying robot non-linear trajectory tracking controller and tracking control method thereof - Google Patents

Abstract

The present invention discloses a quadrotor flying robot non-linear trajectory tracking controller and a tracking control method thereof, and belongs to the flying robot control technology field. The controller comprises a position control subsystem and an attitude control subsystem, the position control subsystem comprises the outer ring and inner ring pseudo dynamic inverters and PI controllers and a position configuration link, and the attitude control subsystem also comprises the outer ring and inner ring pseudo dynamic inverters and PI controllers and an attitude configuration link. According to the present invention, a singular point existing based on an eulerian angle controller and the insufficiency that a linear controller restricts the performance of a robot, are avoided, the design of the controller is simplified, and the system tracking precision of the controller is improved. The designed controller not only has a all-attitude trajectory tracking capability, but also is strong in anti-interference capability, is high in tracking precision, enables the maneuverability and the environment interaction capability of a quadrotor flying robot to be improved remarkably, and lays the technical foundation for the further popularization and application of the quadrotor flying robot.

Description

A kind of four rotor flying robot nonlinear loci tracking control unit and tracking and controlling methods thereof

Technical field

The invention belongs to flying robot and control technical field, relate to track automatic tracking technology, specifically, refer to the method for designing of a kind of four rotor flying robot nonlinear loci tracking control systems.

Background technology

Along with the fast development of flying robot's technology, the application of flying robot is more and more extensive.Particularly four rotor flying robots are because have VTOL function, and place is had no special requirements, and are particularly suitable for scouting, succour, the field such as take photo by plane, and obtain application in fields such as civilian and militaries gradually at present.In order to improve four rotor flying robot performance capacity and ranges of application, people are no longer satisfied with the basic demands such as four rotor flying robot energy smooth flights, and the requirement of its maneuverability and environmental interaction ability is more and more higher, this is accomplished by four rotor flying robot controllers and has significantly high tracking trajectory capacity.Four rotor flying robots are the nonlinear systems of a typical drive lacking, close coupling, and this brings challenge to the design of its path tracking controller.The four rotor flying robot path tracking controllers commonly used at present also exist following deficiency: be normally based on Eulerian angles and carry out attitude expression, Eulerian angles are adopted to carry out the controller of attitude expression, although there being attitude to express the features such as directly perceived, but when the angle of pitch is 90 degree, there is singular point, now flying robot is out of control, limits mobility and the environmental interaction ability of flying robot.Another one problem is conventional controller, the control method generally adopted is by adopting linear control method to be designed after system is carried out linearisation again, the advantage of the method is a simplified the design process of controller, but owing to four rotor flying robots itself are nonlinear systems, employing linear control method is controlled, seriously constrain the performance of its performance, particularly mobility and environmental interaction ability.

Summary of the invention

In order to solve the problems referred to above existed based on four rotor flying robot contrail trackers, the present invention proposes a kind of four rotor flying robot nonlinear loci tracking control system methods for designing, including tracking control unit and tracking and controlling method thereof.The present invention adopts the attitude based on four elements to express, by setting up four rotor flying robot six-freedom motions and kinetic model, use nonlinear control method-track linearization method of controlling specific design four rotor flying robot six degree of freedom nonlinear loci tracking control units, reach the deficiency avoiding the singular point based on the existence of Eulerian angles controller and linear controller restriction robot performance to play.Adopt the bid value through error correction controlled quentity controlled variable to replace nominal value that each control loop is designed, not only simplify the design of controller, and improve controller system tracking accuracy.Designed controller not only has full attitude tracking trajectory capacity, and capacity of resisting disturbance is strong, tracking accuracy is high, significantly improves four rotor flying robot mobility and environmental interaction ability, is that technical foundation has been established in the further genralrlization application of four rotor flying robots.

Present invention firstly provides a kind of four rotor flying robot nonlinear loci tracking control units, whole tracking control unit is made up of two parts: position control subsystem and gesture stability subsystem.Comprising again inside and outside two control loops and a position configuration link in position control subsystem, each control loop comprises a complete track linearisation control methods control structure: a pseudo-dynamic inverse and a PI controller playing calm adjustment effect.The Main Function of position configuration link is according to power instruction F_comCalculating obtains attitude command value β_comThe total pulling force T that should produce with four rotors_com.Comprising inside and outside two control loops and attitude configuration link in gesture stability subsystem, each control loop also comprises a complete track linearisation control methods control structure, and the Main Function of attitude configuration link is according to torque command M_comTotal pulling force T with the output of position control subsystem_comMap the rotating speed obtaining four rotors, thus realizing the control to four rotor flying robots.

The tracking control unit of the present invention receives position command P_com, position control subsystem the first outer shroud puppet dynamic inverse calculates datum speed v according to this value_nom, the first outer shroud PI controller is according to site error P_errCalculate site error Correction and Control amount v_ctrl, datum speed v_nomWith site error Correction and Control amount v_ctrlSum constitutes speed command v_com；Speed command v_comInput as position control subsystem internal ring.Position control subsystem the first internal ring puppet dynamic inverse is according to speed command v_comCalculate nominal force F_nom, the first internal ring PI controller is according to velocity error v_errCalculate velocity error Correction and Control amount F_ctrl, nominal force F_nomWith velocity error Correction and Control amount F_ctrlSum constitutes power instruction F_com；Power instruction F_comAs the input of position configuration link in position control subsystem, position configuration link is according to power instruction F_comValue calculates attitude command β_comWith total pulling force instruction T_com, and as the input of gesture stability subsystem；Described total pulling force instruction T_comIt is directly output to the attitude configuration link in gesture stability subsystem.Attitude command β_comAs the input quantity of the second outer shroud puppet dynamic inverse in gesture stability subsystem, the second outer shroud puppet dynamic inverse calculates nominal angle speed omega according to this attitude command_nom；Second outer shroud PI controller is according to attitude error β_errCalculate attitude error Correction and Control amount ω_ctrl, nominal angle speed omega_nomWith attitude error Correction and Control amount ω_ctrlSum constitutes angular velocity instruction ω_com；Angular velocity instruction ω_comAs the input of gesture stability subsystem internal ring, in gesture stability subsystem, the second internal ring puppet dynamic inverse is according to angular velocity instruction ω_comValue calculates nominal moment M_nom, the second internal ring PI controller is according to angular velocity error ω_errCalculate angular velocity error correction controlled quentity controlled variable M_ctrl, nominal moment M_nomWith angular velocity error correction controlled quentity controlled variable M_ctrlSum constitutes torque command M_com；Torque command M_comExport and configure link to attitude, the attitude configuration link torque command M according to input_comWith total pulling force instruction T_comCalculate the rotating speed of four four rotors of rotor flying robot, thus realizing the control to four rotor flying robots.Wherein, the measured value * of the position of four rotor flying robots, speed, attitude, angular velocity_sen(* is P, v, β, ω) is provided by integrated navigation system measurement.

The advantage of a kind of four rotor flying robot Trajectory Tracking Control System methods for designing of the present invention is in that:

1, adopt quaternary usually to express four rotor flying robot attitudes and be controlled device design, be prevented effectively from the deficiency that there is singular point based on Eulerian angles traditional controller.

2, track linearisation nonlinear control method planned course tracking control unit is adopted, equation need not be carried out microvariations linearization process in the design, thus avoid the error brought in general Design of Flight Control due to linearisation, and there is automatic decoupling ability, it is effectively increased the control accuracy of controller, avoid linearizing deficiency, thus laying a good foundation for the high maneuvering flight of flying robot and the raising of environmental interaction ability.

3, on controller architecture, by adopting the bid value through error control amount correction to replace nominal value as the input of next control loop, not only simplify design procedure, also improve controller track following performance.

Accompanying drawing explanation

Fig. 1 is the four rotor flying robot coordinate systems applied of the present invention and configuration figure；

Fig. 2 is the present invention four rotor flying robot nonlinear loci tracking control unit overall construction drawing；

Fig. 3 is the present invention four rotor flying robot nonlinear loci tracking control unit position tracking effect figure；

Fig. 4 is the present invention four rotor flying robot nonlinear loci tracking control unit speed Tracking design sketch；

Fig. 5 is the present invention four rotor flying robot nonlinear loci tracking control unit Attitude Tracking design sketch；

Fig. 6 is the present invention four rotor flying robot nonlinear loci tracking control unit angular velocity tracking effect figure.

Detailed description of the invention

Below in conjunction with accompanying drawing, the present invention is elaborated.

The present invention provides a kind of four rotor flying robot nonlinear loci tracking control unit and control methods thereof, bid value is adopted to replace nominal value to be controlled loop design, as in figure 2 it is shown, tracking control unit provided by the invention is made up of two parts: position control subsystem and gesture stability subsystem.Comprising the first outer shroud puppet dynamic inverse, the first outer shroud PI controller, the first internal ring puppet dynamic inverse, the first internal ring PI controller and position configuration link in described position control subsystem, position configuration link is according to power instruction F_comCalculating obtains attitude command value β_comThe total pulling force T that should produce with four rotors_com, output is to gesture stability subsystem.Described gesture stability subsystem comprises the second outer shroud puppet dynamic inverse, the second outer shroud PI controller, the second internal ring puppet dynamic inverse, the second internal ring PI controller and attitude configuration link.Attitude configuration link is according to torque command M_comTotal pulling force T with the output of position control subsystem_comMap the rotating speed obtaining four rotors, thus realizing the control to four rotor flying robots.

The tracking control unit of the present invention receives position command P_com, position control subsystem the first outer shroud puppet dynamic inverse calculates datum speed v according to this value_nom, the first outer shroud PI controller is according to site error P_errCalculate site error Correction and Control amount v_ctrl, datum speed v_nomWith site error Correction and Control amount v_ctrlSum constitutes speed command v_com；Speed command v_comInput as position control subsystem internal ring.Position control subsystem the first internal ring puppet dynamic inverse is according to speed command v_comCalculate nominal force F_nom, the first internal ring PI controller is according to velocity error v_errCalculate velocity error Correction and Control amount F_ctrl, nominal force F_nomWith velocity error Correction and Control amount F_ctrlSum constitutes power instruction F_com；Power instruction F_comAs the input of position configuration link in position control subsystem, position configuration link is according to power instruction F_comValue calculates attitude command β_comWith total pulling force instruction T_com, and as the input of gesture stability subsystem；Described total pulling force instruction T_comIt is directly output to the attitude configuration link in gesture stability subsystem.Attitude command β_comAs the input quantity of the second outer shroud puppet dynamic inverse in gesture stability subsystem, the second outer shroud puppet dynamic inverse calculates nominal angle speed omega according to this attitude command_nom；Second outer shroud PI controller is according to attitude error β_errCalculate attitude error Correction and Control amount ω_ctrl, nominal angle speed omega_nomWith attitude error Correction and Control amount ω_ctrlSum constitutes angular velocity instruction ω_com；Angular velocity instruction ω_comAs the input of gesture stability subsystem internal ring, in gesture stability subsystem, the second internal ring puppet dynamic inverse is according to angular velocity instruction ω_comValue calculates nominal moment M_nom, the second internal ring PI controller is according to angular velocity error ω_errCalculate angular velocity error correction controlled quentity controlled variable M_ctrl, nominal moment M_nomWith angular velocity error correction controlled quentity controlled variable M_ctrlSum constitutes torque command M_com；Torque command M_comExport and configure link to attitude, the attitude configuration link torque command M according to input_comWith total pulling force instruction T_comCalculate the rotating speed of four four rotors of rotor flying robot, thus realizing the control to four rotor flying robots.Wherein, the measured value * of the position of four rotor flying robots, speed, attitude, angular velocity_sen(* is P, v, β, ω) is provided by integrated navigation system measurement.

Based on described tracking control unit, the present invention also provides for a kind of four rotor flying robot nonlinear loci tracking and controlling methods, comprises the following steps:

The first step, models based on four element four rotor flying robots, including Kinematic Model and Dynamic Modeling；

Owing to the quality of rotor can be ignored relative to four rotor flying robots, now four rotor flying robots can be regarded as the rigid body of a 6DOF, its motion can be decomposed into translational motion and rotary motion, and the coordinate system of foundation and configuration are as shown in Figure 1.Σ in Fig. 1_ERepresent earth axes O_DX_NY_EZ_D, Σ_bFor body axis system xyz, T₁、T₂、T₃、T₄Representing pulling force produced by four rotors respectively, total pulling force is T.Use Ω₁、Ω₂、Ω₃、Ω₄Represent the rotating speed of four rotors of four rotor flying robots respectively, represent the position vector of flying robot's center of gravity under earth axes, v=(v with P_x,v_y,v_z) represent the velocity of flying robot's center of gravity under earth axes, F represents the total power of bonding force suffered by flying robot (including gravity) under earth axes, M represents the resultant couple of flying robot under body axis system, and ω represents the angular velocity vector under body axis systemRepresenting the body axis system attitude spin matrix relative to earth axes, be the matrix of 3 × 3, concrete calculating sees formula (20)～(25), usually represents that by quaternary attitude spin matrix is then:

$R_b^e=R\left(\mathrm{\β}\right)=\left[\begin{array}{ccc}{a}^2+b^2-c^2-d^2& 2(bc-ad)& 2(bd+ac)\\ 2(bc+ad)& a^2-b^2+c^2-d^2& 2(cd-ab)\\ 2(bd-ac)& 2(cd+ab)& a^2-b^2-c^2+d^2\end{array}\right]---\left(1\right)$

Wherein a, b, c, d are four components of four element β, and four rotor flying robots kinematical equation of translational motion under earth axes is:

$\stackrel{\·}{P}=v---\left(2\right)$

Kinetics equation is:

$\stackrel{\·}{v}=\frac{1}{m}F---\left(3\right)$

M in formula is the quality of four rotor flying robots, and the kinematical equation based on four rotor flying robot rotary motions of four elements is:

$\begin{array}{c}\stackrel{\·}{\mathrm{\β}}=C\left(\mathrm{\β}\right)\mathrm{\ω}\\ =\frac{1}{2}\left[\begin{array}{ccc}-b& -c& -d\\ a& -d& c\\ d& a& -b\\ -c& b& a\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\end{array}---\left(4\right)$

Wherein p, q, r are three components of angular velocity omega, represent four rotor flying robot rolls under body axis system, pitching and course angle speed respectively, $C\left(\mathrm{\β}\right)=\frac{1}{2}\left[\begin{array}{ccc}-b& -c& -d\\ a& -d& c\\ d& a& -b\\ -c& b& a\end{array}\right].$

The spin dynamics equation of four rotor flying robots being approximately rigid body is:

$\stackrel{\·}{\mathrm{\ω}}=J^{-1}(M-\mathrm{\ω}\×J\mathrm{\ω})---\left(5\right)$

In above formula, J is the inertial tensor that four rotor flying robots describe under body axis system, defines as follows:

$J=\left(\begin{array}{ccc}{J}_{xx}& -J_{xy}& -J_{zx}\\ -J_{xy}& J_{yy}& -J_{yz}\\ -J_{zx}& -J_{yz}& J_{zz}\end{array}\right)$

Wherein, J_xx、J_yy、J_zz、J_zx、J_yz、J_xyThe rotary inertia described under body axis system for flying robot and the product of inertia.Due to the symmetry of four rotor flying robots, there is J_xy=J_yz=0, by arranging, formula (5) can be rewritten as follows with the ω state equation being state variable:

$\stackrel{\·}{\mathrm{\ω}}=\left[\begin{array}{c}{J}_{pq}^{p}pq+J_{qr}^{p}qr\\ J_{pp}^q{p}^2+J_{rr}^q{r}^2+J_{pr}^{q}pr\\ J_{pq}^{r}pq+J_{qr}^{r}qr\end{array}\right]+\left[\begin{array}{ccc}{g}_l^p& 0& g_n^p\\ 0& g_m^q& 0\\ g_l^r& 0& g_n^r\end{array}\right]M---\left(6\right)$

Wherein,WithIt is relevant with flying robot's inertial parameter under body axis system,WithJ can be used_xx, J_yy, J_zz, J_xzRepresent, embody as follows:

$\left.\begin{array}{c}{J}_{pq}^p=J_{xz}(J_{yy}-J_{zz}-J_{xx})D=-J_{qr}^r\\ J_{qr}^p=({J_{zz}}^2-J_{yy}{J}_{zz}+{J_{xx}}^2)D\\ J_{pp}^q=-\frac{J_{xz}}{J_{yy}}=-J_{rr}^q\\ J_{pr}^q=\frac{(J_{zz}-J_{xx})}{J_{yy}}\\ J_{pq}^r=(J_{xx}{J}_{yy}-{J_{xz}}^2-{J_{xx}}^2)D\\ g_l^p=-J_{zz}D\\ g_n^p=-J_{xz}D=g_l^r\\ g_m^q=\frac{1}{J_{yy}}\\ g_n^r=-J_{xx}D\\ D=\frac{1}{({J_{xz}}^2-J_{xx}{J}_{zz})}\end{array}\right\}$

The moment M that four rotor flying robot four rotors produce_aWith total pulling force T it is:

$\begin{array}{c}\left[\begin{array}{c}{M}_a\\ T\end{array}\right]=L\left[\begin{array}{c}{\mathrm{\Ω}}_1^2\\ {\mathrm{\Ω}}_2^2\\ {\mathrm{\Ω}}_3^2\\ {\mathrm{\Ω}}_4^2\end{array}\right]\\ =\left[\begin{array}{cccc}-\frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t\\ \frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t\\ -k_d& k_d& -k_d& k_d\\ k_t& k_t& k_t& k_t\end{array}\right]\left[\begin{array}{c}{\mathrm{\Ω}}_1^2\\ {\mathrm{\Ω}}_2^2\\ {\mathrm{\Ω}}_3^2\\ {\mathrm{\Ω}}_4^2\end{array}\right]\end{array}---\left(7\right)$

Wherein, k_tAnd k_dRespectively rotor produces the coefficient of pulling force and moment of torsion, d_rIt is the distance between two diagonal angle rotors,

$L=\left[\begin{array}{cccc}-\frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t\\ \frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t\\ -k_d& k_d& -k_d& k_d\\ k_t& k_t& k_t& k_t\end{array}\right].$

The gyroscopic effect that each rotor produces is:

$M_G=\underset{i=1}{\overset{4}{\Σ}}{J}_r(\mathrm{\ω}\×e_z){(-1)}^{i+1}{\mathrm{\Ω}}_i---\left(8\right)$

Wherein J_rIt is electric machine rotation inertia, e_z=(0,0,1)^T, then the resultant couple M suffered by four rotor flying robots is:

M=M_a+M_G(9)

Four rotor flying robots total power F suffered by under earth axes is:

$F=R_b^e\left[\begin{array}{c}0\\ 0\\ -T\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ mg\end{array}\right]-\left[\begin{array}{c}{C}_{Dv}\left|v_x\right|v_x\\ C_{Dv}\left|v_y\right|v_y\\ C_{Dv}\left|v_z\right|v_z\end{array}\right]---\left(10\right)$

Wherein,Representing attitude spin matrix, m is the quality of four rotor flying robots, and g is gravity constant, v_x、v_y, v_zIt is the velocity component in four rotor flying robots, three directions under earth axes respectively, C_DvIt it is resistance coefficient.

Second step, based on motion and the kinetic model of above four set up rotor flying robots, adopts track linearization method of controlling to be controlled the specific design of device.

Position control subsystem is according to position command P_com, calculate the attitude command β of four rotor flying robots_comWith total pulling force instruction T_com, and export to gesture stability subsystem as instruction.

As in figure 2 it is shown, the outer shroud of position control subsystem is on the control structure, it is made up of the first outer shroud puppet dynamic inverse and the first outer shroud PI controller.Datum speed v under earth axes_nomPseudoinverse is asked to obtain by equation (2).

$v_{nom}={\stackrel{\·}{P}}_{com}---\left(11\right)$

P in formula_comIt is the position command under earth axes, in order to ensureCausality,By position command P_comSecond order puppet differentiation operator is utilized to obtain, shown in second order puppet differentiation operator such as following formula transmission function:

$G_d\left(s\right)=\frac{{\mathrm{\ω}}_{n,d}^{2}s}{s^2+2{\mathrm{\ζ}}_d{\mathrm{\ω}}_{n,d}s+{\mathrm{\ω}}_{n,d}^2}---\left(12\right)$

Wherein, ζ_d、ω_n,dBeing pseudo-differentiation operator damping ratio and bandwidth frequency respectively, s is the symbol of transmission function, multiple parameter.

Position tracking error P_errIt is defined as:

P_err=P_sen-P_com(13)

Wherein, P_senIt it is the measured value of four rotor flying robot positions of sensor measurement.

The kinetics equation of position tracking error is:

${\stackrel{\·}{P}}_{err}=v_{ctr1}---\left(14\right)$

Wherein v_ctrlFor site error Correction and Control amount, according to the linearizing method for designing of track, the control rate of position control subsystem the first outer shroud PI controller is:

v_ctrl=-K_P1(t)P_err-K_I1(t⁾∫P_err(15)

Wherein,

$K_{P1}\left(t\right)=-A_{cl12}=\left[\begin{array}{ccc}{\mathrm{\α}}_{112}& 0& 0\\ 0& {\mathrm{\α}}_{122}& 0\\ 0& 0& {\mathrm{\α}}_{132}\end{array}\right]---\left(16\right)$

$K_{I1}\left(t\right)=-A_{cl11}=\left[\begin{array}{ccc}{\mathrm{\α}}_{111}& 0& 0\\ 0& {\mathrm{\α}}_{121}& 0\\ 0& 0& {\mathrm{\α}}_{131}\end{array}\right]---\left(17\right)$

A_cl11=diag{-α_1j1},A_cl12=diag{-α_1j2And α_1jk(j=1,2,3；K=1,2) it is control parameter, according to PD spectral theory, factor alpha_1jk(j=1,2,3；K=1,2) obtained by desired closed loop system damping ratio and bandwidth frequency, circular is:

${\mathrm{\α}}_{1j1}\left(t\right)={\mathrm{\ω}}_{n,j}^2\left(t\right),{\mathrm{\α}}_{1j2}\left(t\right)=2{\mathrm{\ζ}}_j{\mathrm{\ω}}_{n,j}\left(t\right)-\frac{{\stackrel{\·}{\mathrm{\ω}}}_{n,j}\left(t\right)}{{\mathrm{\ω}}_{n,j}\left(t\right)}---\left(18\right)$

Wherein, ω_n,jT () is bandwidth frequency, ζ_jBeing closed loop system damping ratio, t is the time.The outer shroud output of position control subsystem is speed value v_com, computing formula is:

v_com=v_nom+v_ctrl(19)

Due to each control loop design process basic simlarity, just repeat no more, distribute link to two separately below and be designed.

Position configuration link is the power instruction F that the internal ring of position control subsystem is exported_comAs input value, calculate output four rotor flying robot gesture commands β_comWith the total pulling force instruction T along flying robot's body axis system Z axis_com.The attitude spin matrix of four rotor flying robotsThe form being expressed as direction cosine matrix is:

$R_b^e=\left[\begin{array}{ccc}{r}_1& r_2& r_3\end{array}\right]=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]---\left(20\right)$

Wherein, r₁、r₂、r₃Represent three column vectors of attitude spin matrix, c respectively_ijFor direction cosine matrix parameter, i=1,2,3；J=1,2,3.

Above formula (20) is substituted into formula (10), it is possible to calculate and obtain r₃:

Note $r_0=\left[\begin{array}{c}{F}_{x,\mathrm{com}}+C_{\mathrm{Dv}}\left|v_x\right|v_x\\ F_{y,\mathrm{com}}+C_{\mathrm{Dv}}\left|v_y\right|v_y\\ F_{z,\mathrm{com}}-\mathrm{mg}+C_{\mathrm{Dv}}\left|v_z\right|v_z\end{array}\right]$

Then $r_3=-\frac{r_0}{\left|\right|r_0\left|\right|}---\left(21\right)$

Wherein, F_com=[F_x,comF_y,comF_z,com], F_x,com、F_y,com、F_z,comRespectively power instruction component on three directions of earth axes.

Making a concerted effort always in the Z-direction of body axis system from Fig. 1 it will be seen that produced by four rotors of four rotor flying robots, be not rely on the course of flying robot, therefore, course angle could be arranged to arbitrary value.But considering that engineering is actual, generally allow the x-axis of body axis system point in the tangential direction of position command track all the time, now, course angle ψ is calculated by following formula:

$\mathrm{\ψ}=arctan\left(\frac{{\stackrel{\·}{Y}}_{com}}{{\stackrel{\·}{X}}_{com}}\right)---\left(22\right)$

Wherein X_comAnd Y_comIt it is the command position of four rotor flying robots.

Now, the r of attitude spin matrix₁Column vector projects to plane of reference X_NO_DY_EOn unit vector can be expressed as:

H=[cos (ψ), sin (ψ), 0]^T(23)

Consider unit vector r₂It is orthogonal to vector h and r₃The plane constituted, therefore r₂The computing formula of column vector is:

$r_2=\frac{r_3\×h}{\left|\right|r_3\×h\left|\right|}---\left(24\right)$

Owing to direction cosine matrix is an orthogonal matrix, according to the right-hand rule, unit column vector r₂Computing formula be:

r₁=r₂×r₃(25)

Attitude spin matrix then can be calculated by formula (21), (24) and (25)For low-angle rotation, it is used for representing four element β of attitude nominal value_nomEach component a_nom、b_nom、c_nom、d_nomWith attitude spin matrix R_b ^eParameter c_ijRelational expression between (i=1,2,3, j=1,2,3) is:

${\mathrm{\β}}_{nom}=\left[\begin{array}{c}{a}_{nom}\\ b_{nom}\\ c_{nom}\\ d_{nom}\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}{(c_{11}+c_{22}+c_{33})}^{1/2}\\ \frac{1}{4a_{nom}}(c_{32}-c_{23})\\ \frac{1}{4a_{nom}}(c_{13}-c_{31})\\ \frac{1}{4a_{nom}}(c_{21}-c_{12})\end{array}\right]---\left(26\right)$

Wherein, c_ijIt it is the attitude spin matrix represented with direction cosines form defined by formula (20)Parameter.

It will be seen that position configuration link is not required to individually calculate attitude error from control structure block diagram 2, therefore, position control subsystem exports the nominal value that the attitude command to gesture stability subsystem is exactly attitude, i.e. β_com=β_nom.Total pulling force instruction T_comIt is directly output to the attitude configuration link of gesture stability subsystem, owing to the direction of total pulling force is always along on the Z axis of the body axis system of four rotor flying robots, T_comIt is sized to:

$T_{com}=-\sqrt{F_x^2+F_y^2+F_z^2}---\left(27\right)$

Wherein F_x、F_y、F_zIt is given by:

F_x=F_x,com+C_Dv|v_x,com|v_x,com

F_y=F_y,com+C_Dv|v_y,com|v_y,com

F_z=F_z,com-mg+C_Dv|v_z,com|v_z,com

v_x,com、v_y,com、v_z,comThree components that respectively speed command is fastened at geographical coordinates.

So far, completing the design of whole position control subsystem, position control subsystem, using position command as input value, exports attitude and total pulling force command value, and as the input quantity of gesture stability subsystem.

3rd step, gesture stability subsystem receives the total pulling force T of the internal ring output of position control subsystem_comWith attitude command β_com, attitude configuration link is in conjunction with total pulling force T_comWith moment M_com, calculate the rotary speed instruction Ω of output four four rotors of rotor flying robot_i,com(i=1,2,3,4, represent four rotors respectively), thus realizing the flight of four rotor flying robots is controlled.

The design of the attitude configuration link of gesture stability subsystem is very simple, directly equation (7) is inverted, there it can be seen that based on the linearizing control method of track, have the function of direct decoupling, simplify design process.Four rotor rotary speed instruction computing formula are:

$\left[\begin{array}{c}{Q}_{1,com}^2\\ Q_{2,com}^2\\ Q_{3,com}^2\\ Q_{4,com}^2\end{array}\right]=L^{-1}\left[\begin{array}{c}{M}_{com}\\ T_{com}\end{array}\right]---\left(28\right)$

So far, fly modeling and the design based on four element attitudes expression and the non-linear full attitude flight controller of track linearization method of controlling of whole four rotor flying robots are completed.

Adopting this control method that the practical flight tracking effect of four rotor flying robots is tested, it is as shown in table 1 that the damped coefficient used and angular frequency etc. control parameter.

Table 1 controls parameter

Tracking effect is such as shown in Fig. 3～Fig. 6, and wherein Fig. 3 is that position is followed the tracks of, and Fig. 4 is speed Tracking, and Fig. 5 is Attitude Tracking, and Fig. 6 is that angular velocity is followed the tracks of, wherein *_com、*_sen(* is P, v, γ, ω) represents command value and measured value respectively.In Attitude Tracking design sketch, owing to the attitude based on four elements is expressed directly perceived not, when mapping, utilize the transformational relation between four elements and Eulerian angles, be converted to Eulerian angles, but in whole control process, the form with four elements that is still that is to express the attitude of flying robot, as it is shown in figure 5, Ф, θ, ψ represent roll, pitching and deflection respectively.It will be seen that four tracking errors controlling loop are all very little from these design sketchs, it was shown that adopt four rotor flying robot six degree of freedom nonlinear loci tracking control units designed by control method of the present invention to have good tracking trajectory capacity.

Claims (3)

1. a rotor flying robot nonlinear loci tracking control unit, it is characterised in that: include position control subsystem and gesture stability subsystem；Comprising the first outer shroud puppet dynamic inverse, the first outer shroud PI controller, the first internal ring puppet dynamic inverse, the first internal ring PI controller and position configuration link in described position control subsystem, position configuration link is according to power instruction F_comCalculating obtains attitude command value β_comThe total pulling force T that should produce with four rotors_com, output is to gesture stability subsystem；Described gesture stability subsystem comprises the second outer shroud puppet dynamic inverse, the second outer shroud PI controller, the second internal ring puppet dynamic inverse, the second internal ring PI controller and attitude configuration link；Attitude configuration link is according to torque command M_comTotal pulling force T with the output of position control subsystem_comMap the rotating speed obtaining four rotors, thus realizing the control to four rotor flying robots.

2. a kind of four rotor flying robot nonlinear loci tracking control units according to claim 1, it is characterised in that: tracking control unit receives position command P_com, position control subsystem the first outer shroud puppet dynamic inverse calculates datum speed v according to this value_nom, the first outer shroud PI controller is according to site error P_errCalculate site error Correction and Control amount v_ctrl, datum speed v_nomWith site error Correction and Control amount v_ctrlSum constitutes speed command v_com；Speed command v_comInput as position control subsystem internal ring；Position control subsystem the first internal ring puppet dynamic inverse is according to speed command v_comCalculate nominal force F_nom, the first internal ring PI controller is according to velocity error v_errCalculate velocity error Correction and Control amount F_ctrl, nominal force F_nomWith velocity error Correction and Control amount F_ctrlSum constitutes power instruction F_com；Power instruction F_comAs the input of position configuration link in position control subsystem, position configuration link is according to power instruction F_comValue calculates attitude command β_comWith total pulling force instruction T_com, and as the input of gesture stability subsystem；Described total pulling force instruction T_comIt is directly output to the attitude configuration link in gesture stability subsystem；Attitude command β_comAs the input quantity of the second outer shroud puppet dynamic inverse in gesture stability subsystem, the second outer shroud puppet dynamic inverse calculates nominal angle speed omega according to this attitude command_nom；Second outer shroud PI controller is according to attitude error β_errCalculate attitude error Correction and Control amount ω_ctrl, nominal angle speed omega_nomWith attitude error Correction and Control amount ω_ctrlSum constitutes angular velocity instruction ω_com；Angular velocity instruction ω_comAs the input of gesture stability subsystem internal ring, in gesture stability subsystem, the second internal ring puppet dynamic inverse is according to angular velocity instruction ω_comValue calculates nominal moment M_nom, the second internal ring PI controller is according to angular velocity error ω_errCalculate angular velocity error correction controlled quentity controlled variable M_ctrl, nominal moment M_nomWith angular velocity error correction controlled quentity controlled variable M_ctrlSum constitutes torque command M_com；Torque command M_comExport and configure link to attitude, the attitude configuration link torque command M according to input_comWith total pulling force instruction T_comCalculate the rotating speed of four four rotors of rotor flying robot, thus realizing the control to four rotor flying robots；Wherein, the measured value * of the position of four rotor flying robots, speed, attitude, angular velocity_senBeing provided by integrated navigation system measurement, * is P, v, β, ω.

3. a four rotor flying robot nonlinear loci tracking and controlling method, comprises the following steps:

The first step, models based on four element four rotor flying robots, including Kinematic Model and Dynamic Modeling；

Four rotor flying robots being regarded as the rigid body of a 6DOF, its Kinematic Decomposition is translational motion and rotary motion, the coordinate system of foundation, Σ_ERepresent earth axes O_DX_NY_EZ_D, Σ_bFor body axis system xyz, T₁、T₂、T₃、T₄Representing pulling force produced by four rotors respectively, total pulling force is T, uses Ω₁、Ω₂、Ω₃、Ω₄Represent the rotating speed of four rotors of four rotor flying robots respectively, represent the position vector of flying robot's center of gravity under earth axes, v=(v with P_x,v_y,v_z) representing the velocity of flying robot's center of gravity under earth axes, F represents the total power of bonding force suffered by flying robot under earth axes, and M represents the resultant couple of flying robot under body axis system, and ω represents the angular velocity vector under body axis system,Represent that body axis system is relative to the attitude spin matrix of earth axes, is the matrix of 3 × 3, usually represents that by quaternary attitude spin matrix is then:

$R_b^e=R\left(\mathrm{\β}\right)=\left[\begin{array}{ccc}{a}^2+b^2-c^2-d^2& 2(bc-ad)& 2(bd+ac)\\ 2(bc+ad)& a^2-b^2+c^2-d^2& 2(cd-ab)\\ 2(bd-ac)& 2(cd+ab)& a^2-b^2-c^2+d^2\end{array}\right]---\left(1\right)$

Wherein a, b, c, d are four components of four element β, and four rotor flying robots kinematical equation of translational motion under earth axes is:

$\stackrel{\·}{P}=v---\left(2\right)$

Kinetics equation is:

$\stackrel{\·}{v}=\frac{1}{m}F---\left(3\right)$

M in formula is the quality of four rotor flying robots, and the kinematical equation based on four rotor flying robot rotary motions of four elements is:

$\begin{array}{c}\stackrel{\·}{\mathrm{\β}}=C\left(\mathrm{\β}\right)\mathrm{\ω}\\ =\frac{1}{2}\left[\begin{array}{ccc}-b& -c& -d\\ a& -d& c\\ d& a& -b\\ -c& b& a\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\end{array}---\left(4\right)$

Wherein p, q, r are three components of angular velocity omega, represent four rotor flying robot rolls under body axis system, pitching and course angle speed respectively, $C\left(\mathrm{\β}\right)=\frac{1}{2}\left[\begin{array}{ccc}-b& -c& -d\\ a& -d& c\\ d& a& -b\\ -c& b& a\end{array}\right];$

The spin dynamics equation of four rotor flying robots is:

$\stackrel{\·}{\mathrm{\ω}}=J^{-1}(M-\mathrm{\ω}\×J\mathrm{\ω})---\left(5\right)$

In above formula, J is the inertial tensor that four rotor flying robots describe under body axis system, defines as follows:

$J=\left(\begin{array}{ccc}{J}_{xx}& -J_{xy}& -J_{zx}\\ -J_{xy}& J_{yy}& -J_{yz}\\ -J_{zx}& -J_{yz}& J_{zz}\end{array}\right)$

Wherein, J_xx、J_yy、J_zz、J_zx、J_yz、J_xyThe rotary inertia described under body axis system for flying robot and the product of inertia；

Due to the symmetry of four rotor flying robots, there is J_xy=J_yz=0, by arranging, formula (5) is rewritten as follows with the ω state equation being state variable:

$\stackrel{\·}{\mathrm{\ω}}=\left[\begin{array}{c}{J}_{pq}^{p}pq+J_{qr}^{p}qr\\ J_{pp}^q{p}^2+J_{rr}^q{r}^2+J_{pr}^{q}pr\\ J_{pq}^{r}pq+J_{qr}^{r}qr\end{array}\right]+\left[\begin{array}{ccc}{g}_l^p& 0& g_n^p\\ 0& g_m^q& 0\\ g_l^r& 0& g_n^r\end{array}\right]M---\left(6\right)$

Wherein,WithIt is relevant with flying robot's inertial parameter under body axis system,WithUse J_xx, J_yy, J_zz, J_xzRepresent, embody as follows:

$\left.\begin{array}{c}{J}_{pq}^p=J_{xz}\left(J_{yy}-J_{zz}-J_{xx}\right)D=-J_{qr}^r\\ J_{qr}^p=\left({J_{zz}}^2-J_{yy}{J}_{zz}+{J_{xx}}^2\right)D\\ J_{pp}^q=-\frac{J_{xz}}{J_{yy}}=-J_{rr}^q\\ J_{pr}^q=\frac{\left(J_{zz}-J_{xx}\right)}{J_{yy}}\\ J_{pq}^r=\left(J_{xx}{J}_{yy}-{J_{xz}}^2-{J_{xx}}^2\right)D\\ g_l^p=-J_{zz}D\\ g_n^p=-J_{xz}D=g_l^r\\ g_m^q=\frac{1}{J_{yy}}\\ g_n^r=-J_{xx}D\\ D=\frac{1}{\left({J_{xz}}^2-J_{xx}{J}_{zz}\right)}\end{array}\right\}$

The moment M that four rotor flying robot four rotors produce_aWith total pulling force T it is:

$\begin{array}{c}\left[\begin{array}{c}{M}_a\\ T\end{array}\right]=L\left[\begin{array}{c}{\mathrm{\Ω}}_1^2\\ {\mathrm{\Ω}}_2^2\\ {\mathrm{\Ω}}_3^2\\ {\mathrm{\Ω}}_4^2\end{array}\right]\\ =\left[\begin{array}{cccc}-\frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t\\ \frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t\\ -k_d& k_d& -k_d& k_d\\ k_t& k_t& k_t& k_t\end{array}\right]\left[\begin{array}{c}{\mathrm{\Ω}}_1^2\\ {\mathrm{\Ω}}_2^2\\ {\mathrm{\Ω}}_3^2\\ {\mathrm{\Ω}}_4^2\end{array}\right]\end{array}---\left(7\right)$

Wherein, k_tAnd k_dRespectively rotor produces the coefficient of pulling force and moment of torsion, d_rIt is the distance between two diagonal angle rotors,

$L=\left[\begin{array}{cccc}-\frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t\\ \frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& -\frac{\sqrt{2}}{4}{d}_r{k}_t& \frac{\sqrt{2}}{4}{d}_r{k}_t\\ -k_d& k_d& -k_d& k_d\\ k_t& k_t& k_t& k_t\end{array}\right].$

The gyroscopic effect that each rotor produces is:

$M_G=\underset{i=1}{\overset{4}{\mathrm{\Σ}}}{J}_r(\mathrm{\ω}\×e_z){(-1)}^{i+1}{\mathrm{\Ω}}_i---\left(8\right)$

Wherein J_rIt is electric machine rotation inertia, e_z=(0,0,1)^T, then the resultant couple M suffered by four rotor flying robots is:

M=M_a+M_G(9)

Four rotor flying robots total power F suffered by under earth axes is:

$F=R_b^e\left[\begin{array}{c}0\\ 0\\ -T\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ mg\end{array}\right]-\left[\begin{array}{c}{C}_{Dv}\left|v_x\right|v_x\\ C_{Dv}\left|v_y\right|v_y\\ C_{Dv}\left|v_z\right|v_z\end{array}\right]---\left(10\right)$

Wherein,Representing attitude spin matrix, m is the quality of four rotor flying robots, and g is gravity constant, v_x、v_y, v_zIt is the velocity component in four rotor flying robots, three directions under earth axes respectively, C_DvIt it is resistance coefficient；

Second step, based on motion and the kinetic model of above four set up rotor flying robots, adopts track linearization method of controlling to be controlled the specific design of device；

Position control subsystem is according to position command P_com, calculate the attitude command β of four rotor flying robots_comWith total pulling force instruction T_com, and export to gesture stability subsystem as instruction；

The outer shroud of position control subsystem on the control structure, is made up of the first outer shroud puppet dynamic inverse and the first outer shroud PI controller, the datum speed v under earth axes_nomPseudoinverse is asked to obtain by equation (2)；

$v_{nom}={\stackrel{\·}{P}}_{com}--\left(11\right)$

P in formula_comIt is the position command under earth axes,By position command P_comSecond order puppet differentiation operator is utilized to obtain, shown in second order puppet differentiation operator such as following formula transmission function:

$G_d\left(s\right)=\frac{{\mathrm{\ω}}_{n,d}^{2}s}{s^2+2{\mathrm{\ζ}}_d{\mathrm{\ω}}_{n,d}s+{\mathrm{\ω}}_{n,d}^2}---\left(12\right)$

Wherein, ζ_d、ω_n,dBeing pseudo-differentiation operator damping ratio and bandwidth frequency respectively, s is the symbol of transmission function, multiple parameter；

Position tracking error P_errIt is defined as:

P_err=P_sen-P_com(13)

Wherein, P_senIt it is the measured value of four rotor flying robot positions of sensor measurement；

The kinetics equation of position tracking error is:

${\stackrel{\·}{P}}_{err}=v_{ctrl}---\left(14\right)$

Wherein v_ctrlFor site error Correction and Control amount, according to the linearizing method for designing of track, the control rate of position control subsystem the first outer shroud PI controller is:

v_ctrl=-K_P1(t)P_err-K_I1(t)∫P_err(15)

Wherein,

$K_{P1}\left(t\right)=-A_{cl12}=\left[\begin{array}{ccc}{\mathrm{\α}}_{112}& 0& 0\\ 0& {\mathrm{\α}}_{122}& 0\\ 0& 0& {\mathrm{\α}}_{132}\end{array}\right]---\left(16\right)$

$K_{I1}\left(t\right)=-A_{cl11}=\left[\begin{array}{ccc}{\mathrm{\α}}_{111}& 0& 0\\ 0& {\mathrm{\α}}_{121}& 0\\ 0& 0& {\mathrm{\α}}_{131}\end{array}\right]---\left(17\right)$

A_cl11=diag{-α_1j1},A_cl12=diag{-α_1j2And α_1jkIt is control parameter, according to PD spectral theory, factor alpha_1jkObtained by desired closed loop system damping ratio and bandwidth frequency, j=1,2,3；K=1,2, circular is:

${\mathrm{\α}}_{1j1}\left(t\right)={\mathrm{\ω}}_{n,j}^2\left(t\right),{\mathrm{\α}}_{1j2}\left(t\right)=2{\mathrm{\ζ}}_j{\mathrm{\ω}}_{n,j}\left(t\right)-\frac{{\stackrel{\·}{\mathrm{\ω}}}_{n,j}\left(t\right)}{{\mathrm{\ω}}_{n,j}\left(t\right)}---\left(18\right)$

Wherein, ω_n,jT () is bandwidth frequency, ζ_jBeing closed loop system damping ratio, t is the time, and the outer shroud output of position control subsystem is speed value v_com, computing formula is:

v_com=v_nom+v_ctrl(19)

Position configuration link is the power instruction F that the internal ring of position control subsystem is exported_comAs input value, calculate output four rotor flying robot gesture commands β_comWith the total pulling force instruction T along flying robot's body axis system Z axis_com, the attitude spin matrix of four rotor flying robotsThe form being expressed as direction cosine matrix is:

$R_b^e=\left[\begin{array}{ccc}{r}_1& r_2& r_3\end{array}\right]=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]---\left(20\right)$

Wherein, r₁、r₂、r₃Represent three column vectors of attitude spin matrix, c respectively_ijFor direction cosine matrix parameter, i=1,2,3；J=1,2,3；

Above formula (20) is substituted into formula (10), calculates and obtain r₃:

Note $r_0=\left[\begin{array}{c}{F}_{x,com}+C_{Dv}\left|v_x\right|v_x\\ F_{y,com}+C_{Dv}\left|v_y\right|v_y\\ F_{z,com}-mg+C_{Dv}\left|v_z\right|v_z\end{array}\right]$

Then $r_3=-\frac{r_0}{\left|\right|r_0\left|\right|}---\left(21\right)$

Wherein, F_com=[F_x,comF_y,comF_z,com], F_x,com、F_y,com、F_z,comRespectively power instruction component on three directions of earth axes；

The x-axis allowing body axis system is pointed in the tangential direction of position command track all the time, and now, course angle ψ is calculated by following formula:

$\mathrm{\ψ}=\arctan \left(\frac{{\stackrel{\·}{Y}}_{com}}{{\stackrel{\·}{X}}_{com}}\right)---\left(22\right)$

Wherein X_comAnd Y_comIt it is the command position of four rotor flying robots；

Now, the r of attitude spin matrix₁Column vector projects to plane of reference X_NO_DY_EOn unit vector be expressed as:

H=[cos (ψ), sin (ψ), 0]^T(23)

Consider unit vector r₂It is orthogonal to vector h and r₃The plane constituted, therefore r₂The computing formula of column vector is:

$r_2=\frac{r_3\×h}{\left|\right|r_3\×h\left|\right|}---\left(24\right)$

Owing to direction cosine matrix is an orthogonal matrix, according to the right-hand rule, unit column vector r₂Computing formula be:

r₁=r₂×r₃(25)

Attitude spin matrix is calculated by formula (21), (24) and (25)For low-angle rotation, it is used for representing four element β of attitude nominal value_nomEach component a_nom、b_nom、c_nom、d_nomWith attitude spin matrixParameter c_ijBetween relational expression be:

${\mathrm{\β}}_{nom}=\left[\begin{array}{c}{a}_{nom}\\ b_{nom}\\ c_{nom}\\ d_{nom}\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}{\left(c_{11}+c_{22}+c_{33}\right)}^{1/2}\\ \frac{1}{4a_{nom}}\left(c_{32}-c_{23}\right)\\ \frac{1}{4a_{nom}}\left(c_{13}-c_{31}\right)\\ \frac{1}{4a_{nom}}\left(c_{21}-c_{12}\right)\end{array}\right]---\left(26\right)$

Total pulling force instruction T_comIt is directly output to the attitude configuration link of gesture stability subsystem, owing to the direction of total pulling force is always along on the Z axis of the body axis system of four rotor flying robots, T_comIt is sized to:

$T_{com}=-\sqrt{F_x^2+F_y^2+F_z^2}---\left(27\right)$

Wherein F_x、F_y、F_zIt is given by:

F_x=F_x,com+C_Dv|v_x,com|v_x,com

F_y=F_y,com+C_Dv|v_y,com|v_y,com

F_z=F_z,com-mg+C_Dv|v_z,com|v_z,com

v_x,com、v_y,com、v_z,comThree components that respectively speed command is fastened at geographical coordinates；

So far, position control subsystem, using position command as input value, exports attitude and total pulling force command value, and as the input quantity of gesture stability subsystem；

3rd step, gesture stability subsystem receives the total pulling force T of the internal ring output of position control subsystem_comWith attitude command β_com, attitude configuration link is in conjunction with total pulling force T_comWith moment M_com, calculate the rotary speed instruction Ω of output four four rotors of rotor flying robot_i,com, i=1,2,3,4, thus realizing the flight of four rotor flying robots is controlled；

Four rotor rotary speed instruction computing formula are:

$\left[\begin{array}{c}{\mathrm{\Ω}}_{1,com}^2\\ {\mathrm{\Ω}}_{2,com}^2\\ {\mathrm{\Ω}}_{3,com}^2\\ {\mathrm{\Ω}}_{4,com}^2\end{array}\right]=L^{-1}\left[\begin{array}{c}{M}_{com}\\ T_{com}\end{array}\right]---\left(28\right).$