無人機自主飛行航跡規(guī)劃算法研究

無人機自主飛行航跡規(guī)劃算法研究,無人機,自主,飛行,航跡,規(guī)劃,算法,研究 Research ArticlePosition and attitude tracking control for a quadrotor UAVJing-Jing Xiongn, En-Hui ZhengCollege of Mechanical and Electrical Engineering, China Jiliang University, Hangzhou 310018, PR Chinaa r t i c l e i n f oArticle history:Received 3 November 2013Received in revised form29 December 2013Accepted 16 January 2014This paper was recommended forpublication by Jeff Pieper.Keywords:Quadrotor UAVUnderactuatedNovel robust TSMCSMCSynthesis controla b s t r a c tA synthesis control method is proposed to perform the position and attitude tracking control of thedynamical model of a small quadrotor unmanned aerial vehicle (UAV), where the dynamical model isunderactuated, highly-coupled and nonlinear. Firstly, the dynamical model is divided into a fully actuatedsubsystem and an underactuated subsystem. Secondly, a controller of the fully actuated subsystem isdesigned through a novel robust terminal sliding mode control (TSMC) algorithm, which is utilized toguarantee all state variables converge to their desired values in short time, the convergence time is sosmall that the state variables are acted as time invariants in the underactuated subsystem, and, acontroller of the underactuated subsystem is designed via sliding mode control (SMC), in addition, thestabilities of the subsystems are demonstrated by Lyapunov theory, respectively. Lastly, in order todemonstrate the robustness of the proposed control method, the aerodynamic forces and moments andair drag taken as external disturbances are taken into account, the obtained simulation results show thatthe synthesis control method has good performance in terms of position and attitude tracking whenfaced with external disturbances.& 2014 ISA. Published by Elsevier Ltd. All rights reserved.1. IntroductionThe quadrotor unmanned aerial vehicles (UAVs) are being usedin several typical missions, such as search and rescue missions,surveillance, inspection, mapping, aerial cinematography and lawenforcement 15.Considering that the dynamical model of the quadrotor is anunderactuated, highly-coupled and nonlinear system, many con-trol strategies have been developed for a class of similar systems.Among them, sliding mode control, which has drawn researchersmuch attention, has been a useful and efficient control algorithmfor handling systems with large uncertainties, time varying prop-erties, nonlinearities, and bounded external disturbances 6. Theapproach is based on defining exponentially stable sliding surfacesas a function of tracking errors and using Lyapunov theory toguarantee all state trajectories reach these surfaces in finite-time,and, since these surfaces are asymptotically stable, the statetrajectories slides along these surfaces till they reach the origin7. But, in order to obtain fast tracking error convergence, thedesired poles must be chosen far from the origin on the left half ofs-plane, simultaneously, this will, in turn, increase the gain of thecontroller, which is undesirable considering the actuator satura-tion in practical systems 8,9.Replacing the conventional linear sliding surface with the non-linear terminal sliding surface, the faster tracking error convergenceis to obtain through terminal sliding mode control (TSMC). Terminalsliding mode has been shown to be effective for providing fasterconvergence than the linear hyperplane-based sliding mode aroundthe equilibrium point 8,10,11. TSMC was proposed for uncertaindynamic systems with pure-feedback form in 12. In 13, a robustadaptive TSMC technique was developed for n-link rigid roboticmanipulators with uncertain dynamics. A global non-singular TSMCfor rigid manipulators was presented in 14. Finite-time control ofthe robot system was studied through both state feedback anddynamic output feedback control in 15. A continuous finite-timecontrol scheme for rigid robotic manipulators using a new form ofterminal sliding modes was proposed in 16. For the sake ofachieving finite-time tracking control for the rotor position in theaxial direction of a nonlinear thrust active magnetic bearing system,the robust non-singular TSMC was given in 17. However, theconventional TSMC methods are not the best in the convergencetime, the primary reason is that the convergence speed of thenonlinear sliding mode is slower than the linear sliding mode whenthe state variables are close to the equilibrium points. In 18, a novelTSMC scheme was developed using a function augmented slidinghyperplane for the guarantee that the tracking error converges tozero in finite-time, and was proposed for the uncertain single-inputand single-output (SISO) nonlinear system with unknown externaldisturbance. In the most of existing research results, the uncertainexternal disturbances are not taken into account these nonlinearsystems. In order to further demonstrate the robustness of novelContents lists available at ScienceDirectjournal homepage: Transactions0019-0578/$-see front matter & 2014 ISA. Published by Elsevier Ltd. All rights reserved.http:/dx.doi.org/10.1016/j.isatra.2014.01.004nCorresponding author.E-mail addresses: (J.-J. Xiong), (E.-H. Zheng).Please cite this article as: Xiong J-J, Zheng E-H. Position and attitude tracking control for a quadrotor UAV. ISA Transactions (2014), http:/dx.doi.org/10.1016/j.isatra.2014.01.004iISA Transactions () TSMC, the external disturbances are considered into the nonlinearsystems and are applied to the controller design.In this work, we combine two components in the proposedcontrol: a novel robust TSMC component for high accuracytracking performance in the fully actuated subsystem, and a SMCcomponent that handles the external disturbances in the under-actuated subsystem.Even though many classical, higher order and extended SMCstrategies, have been developed for the flight controller design forthe quadrotor UAV (see for instance 1923, and the list is notexhaustive), and, these strategies in the papers 1923 wereutilized to dictate a necessity to compensate for the externaldisturbances, in addition, the other control methods, such asproportionalintegraldifferential (PID) control 24,25, backstep-ping control 26,27, switching model predictive attitude control28, etc., have been proposed for the flight controller design, mostof the aforementioned control strategies have been proposed inorder to make the quadrotor stable in finite-time and the stabili-zation time of the aircraft may be too long to reflect theperformance of them. In addition, the stabilization time is essen-tial significance for the quadrotor UAV to quickly recover fromsome unexpected disturbances. For the sake of decreasing thetime, a synthesis control method based on the novel robust TSMCand SMC algorithms is applied to the dynamical model of thequadrotor UAV. The synthesis control method is proposed toguarantee all system state variables converge to their desiredvalues in short time. Furthermore, the convergence time of thestate variables are predicted via the equations derived by the novelrobust TSMC, this is demonstrated by the following sections.The organization of this work is arranged as follows. Section 2presents the dynamical model of a small quadrotor UAV. Thesynthesis control method is detailedly introduced in Section 3. InSection 4, simulation results are performed to highlight the overallvalidity and the effectiveness of the designed controllers. InSection 5, a discussion, which is based on different synthesiscontrol schemes, is presented to emphasize the performance ofthe proposed synthesis control method in this work, followed bythe concluding remarks in Section 6.2. Quadrotor modelIn order to describe the motion situations of the quadrotor modelclearly, the position coordinate is to choose. The model of thequadrotor is set up in this work by the body-frame B and the earth-frame E as presented in Fig. 1. Let the vector x,y,z denotes theposition of the center of the gravity of the quadrotor and the vector u,v,w denotes its linear velocity in the earth-frame. The vector p,q,rrepresents the quadrotors angular velocity in the body-frame. mdenotes the total mass. g represents the acceleration of gravity. ldenotes the distance from the center of each rotor to the center ofgravity.The orientation of the quadrotor is given by the rotation matrixR:E-B, where R depends on the three Euler angles ,0, whichrepresent the roll, the pitch and the yaw, respectively. AndA?=2;=2;A?=2;=2;A?;.The transformation matrix from ,0to p,q,r0is given bypqr264375 10? sin0cossincos0? sincoscos264375_266437751The dynamical model of the quadrotor can be described by thefollowing equations 5,24,29:x 1mcossincos sinsinu1?K1_xmy 1mcossinsin? sincosu1?K2_ymz 1mcoscosu1?g?K3_zm_Iy?IzIxJrIx_rlIxu2?K4lIx_ _Iz?IxIy?JrIy_rlIyu3?K5lIy_Ix?IyIz1Izu4?K6Iz_8:2where Kidenote the drag coefficients and positive constants;r1?23?4;i; stand for the angular speed of thepropeller i Ix,Iy,Izrepresent the inertias of the quadrotor;Jrdenotesthe inertia of the propeller;u1denotes the total thrust on the bodyin the z-axis;u2and u3represent the roll and pitch inputs,respectively;u4denotes a yawing moment.u1 F1F2F3F4;u2 ?F2F4; u3 ?F1F3; u4 d?F1F2F3F4=b;,where Fi b2idenote the thrust generated by four rotors and areconsidered as the real control inputs to the dynamical system, bdenotes the lift coefficient;d denotes the force to moment scalingfactor.3. Synthesis controlCompared with the brushless motor, the propeller is very light,we ignore the moment of inertia caused by the propeller. Eq. (2) isdivided into two parts:z#u1coscosm?g1Izu42435?K3m_z_Ix?IyIz?K6Iz_24353xy#u1mcossinsin? cos#cossinsin#?K1m_x?K2m_y2435#l=Ix00l=Iy#u2u3#_Iy?IzIx?K4lIx_Iz?IxIy?K5lIy_24358:4where Eq. (3) denotes the fully actuated subsystem (FAS), Eq. (4)denotes the underactuated subsystem (UAS). For the FAS, a novelrobust TSMC is used to guarantee its state variables converge totheir desired values in short time, then the state variables areregarded as time invariants, therefore, the UAS gets simplified. Forthe UAS, a sliding mode control approach is utilized. The specialsynthesis control scheme is introduced in the following sections.Fig. 1. Quadrotor UAV.J.-J. Xiong, E.-H. Zheng / ISA Transactions () 2Please cite this article as: Xiong J-J, Zheng E-H. Position and attitude tracking control for a quadrotor UAV. ISA Transactions (2014), http:/dx.doi.org/10.1016/j.isatra.2014.01.004i3.1. A novel robust TSMC for FASConsidering the symmetry of a rigid-body quadrotor, therefore,we get IxIyLet x1 z?0and x2 _z _?0. The fully actuatedsubsystem is written by_x1 x2;_x2 f1g1u1d1(5where f1 ?g 0?0;g1 coscos=m 0;0 1=Iz?;u1 u1u4?0and d1 ?K3_z=m?K6_=Iz?0:To develop the tracking control, the sliding manifolds aredefined as 18,30s2_s11s11s1m01=n016as4_s32s32s3m02=n026bwhere s1 zd?z; s3d?, Zdanddare the desired values ofstate variables Z and, respectively. In addition, the coefficients1;2;1;2 are positive, m01;m02;n01;n02are positive odd integerswith m01on01and m02on02.Let s20 and s40. The convergence time is calculated asfollows:ts1n011n01?m01ln1s10?n01?m01=n0111 !7ats3n022n02?m02ln2s30?n02?m02=n0222 !7bIn accordance with Eq. (5) and the time derivative of s2and s4, wehave_s2zd?u1mcoscosgK3m_z1_s11ddtsm01=n0118a_s4 d?1Izu4K6Iz_2_s32ddtsm02=n0238bThe controllers are designed byu1mcoscoszdg1_s11m01n01sm01?n01=n011_s11s21sm1=n12?9au4 Izd2_s32m02n02sm02?n02=n023_s32s42sm2=n24?9bwhere1,2,1, and2are positive,m1, n1, m2, and n2are positiveodd integers with m1on1and m2on2:Under the controllers, the state trajectories reach the areas(1,2) of the sliding surfaces s20 and s40 along_s2 ?1s2?01sm1=n12and_s4 ?2s4?02sm2=n24in finite-time, respectively. Thetime is defined ast01rn11n1?m1ln1s10?n1?m1=n11110at02rn22n2?m2ln2s30?n2?m2=n22210bwhere011?K3_z=m=jsm1=n12j;1 L1=jsm1=n12j1;L1 jK3_z=mjmax;140;1 fjs2jrL1=1m1=n1g022?K6_=Iz=jsm2=n24j;2 L2=jsm2=n24j2L2 jK6_=Izjmax;240;2 fjs4jrL2=2m2=n2gProof 1. In order to illustrate the subsystem is stable, here, weonly choose the state variable z as an example and Lyapunovtheory is applied.Considering the Lyapunov function candidateV1 s22=2Invoking Eqs. (8a) and (9a) the time derivative of V1is derived_V1 s2_s2 s2?1s2?1sm1=n12K3_z=m ?1s22?01sm1n1=n12Considering that (m1n1) is positive even integer, thats,_V1r0: The state trajectories of the subsystem converge to thedesired equilibrium points in finite-time. Therefore, the subsystemis asymptotically stable.3.2. A SMC approach for UASIn this section, the details about sliding mode control of aclassofunderactuatedsystemsarefoundin29.LetQ u1mcossinsin? cos#,andy1 Q?1x y?0;y2 Q?1_x_y?0;y3 ?0;y4 _?0. The underactuated subsystem is written ina cascaded form_y1 y2;_y2 f2d2;_y3 y4;_y4 f3g2u2d3:11According to Eqs. (9a) and (9b) we can select the appropriateparameters to guarantee the control law u1and yaw angleconvergetothedesired/referencevaluesinshorttime.Thats,_u1 0,becomes time invariant, then_ 0, Q is timeinvariant matrix and non-singular because u1is the total thrustand nonzero to overcome the gravity. As a resultf2 cossinsin?0;d2 Q?1diagK1=m K2=m?Qy2;f3 0;g2 diagl=Ixl=Iy?;u2 u2u3?0;d3 diag?lK4=Ix?lK5=Iy?y4Define the tracking error equationse1 yd1?y1;e2_e1_yd1?y2;e3_e2yd1?f2;e4_e3:yd1?f2y1y2f2y2f2f2y3y4?8:12where the vector yd1denotes the desired value vector.The sliding manifolds are designed ass c1e1c2e2c3e3e413where the constants ci40.By making_s ?Msgns?s, we getu2f2y3g2?1c1e2c2e3c3e4:y01d?ddtf2y1y2hi?ddtf2y2f2hi?ddtf2y3hiy4?f2y3f3d3Msgnss8:9=;14whereM c2d2c32d2jjE1jj23d4jjyjj2;1Zf2=y1;2Zf2=y2;3Zf2=y3;E1 e1e2e3?0;y y1y2y3y4?0and40;40;jjd2jjod2jjE1jj2;d2 maxK1=mK2=mjjd3jjod4jjyjj2;d4 maxlK4=IxlK5=Iy:Accordingtof2y3? sinsin?coscoscos0?;and0 of2=y3cos2coso2?, and , therefore, f2=y3is invertible.J.-J. Xiong, E.-H. Zheng / ISA Transactions () 3Please cite this article as: Xiong J-J, Zheng E-H. Position and attitude tracking control for a quadrotor UAV. ISA Transactions (2014), http:/dx.doi.org/10.1016/j.isatra.2014.01.004iProof 2. The stability of the subsystem is illustrated by Lyapunovtheory as follows.Consider the Lyapunov function candidate:V 12sTsInvoking Eqs. (13) and (14), the time derivative of V is_V sT_s sTc1_e1c2_e2c3_e3_e4? sT?Msgnsc2d2c3f2y2d2f2y3d3?o ?sTs?M?c2d2c32d2jjE1jj2?3d4jjxjj2jjsjj1 ?sTs?jjsjj1r0:Therefore, under the controllers, the subsystem state trajec-tories can reach, and, thereafter, stay on the manifold S0 infinite-time.4. Simulation results and analysisIn this section, the dynamical model of the quadrotor UAV inEq. (2) is used to test the validity and efficiency of the proposedsynthesis control scheme when faced with external disturbances.The simulations of typical position and attitude tracking areperformed on Matlab 7.1.0.246/Simulink, which is equipped witha computer comprising of a DUO E7200 2.53 GHz CPU with 2 GB ofRAM and a 100 GB solid state disk drive. Moreover, the perfor-mance of the synthesis control is demonstrated through thecomparison with the control method in 29, which used a ratebounded PID controller and a sliding mode controller for the fullyactuated subsystem, and, a SMC approach for the underactuatedsubsystem.4.1. PID control and SMCIn this section, more details of the PID control and SMC methodfor a quadrotor UAV has been introduced, meanwhile, the simula-tion results and analysis, which verify the effectiveness of thesynthesis control scheme, can be found in 29. The approximate05101520253035404550036Time ( s )z ( m )05101520253035404550-202x ( m )referencereal05101520253035404550-101y ( m )Fig. 2. The positions (x,y,z), PID control and SMC.05101520253035404550-0.02500.025 ( rad )referencereal05101520253035404550-0.0600.06 ( rad )0510152025303540455000.51Time ( s ) ( rad )X: 25.48Y: 0.5003Fig. 3. The angles (,), PID control and SMC.0510152025303540455078910111213X: 39.54Y: 9.801Time ( s )u1 ( m/s2 )Fig. 4. The controller u1, PID control and SMC.Table 1Quadrotor model parameters.VariableValueUnitsm2.0kgIxIy1.25Ns2/radIz2.2Ns2/radK1K2K30.01Ns/mK4K5K60.012Ns/ml0.20mJr1Ns2/radb2Ns2d5N ms2g9.8m/s2Table 2Controller parameters.VariableValueVariableValue11231121m015m025n017n027m11m21n13n231102101L1=jsm1=n12j12L2=jsm2=n24j210.120.1c120c222c3810.1102032J.-J. Xiong, E.-H. Zheng / ISA Transactions () 4Please cite this article as: Xiong J-J, Zheng E-H. Position and attitude tracking control for a quadrotor UAV. ISA Transactions (2014), http:/dx.doi.org/10.1016/j.isatra.2014.01.004isimulation tests are shown in Figs. 24, however, the researchobjects are slightly changed to make obvious comparisons withthe following simulation tests.4.2. Novel robust TSMC and SMCIn this section, in order to justify the effectiveness of the pro-posed synthesis control method, the position and attitude trackingof the quadrotor have been performed.The initial position and angle values of the quadrotor for thesimulation tests are 0, 0, 0m and 0, 0, 0rad. In addition, thequadrotors model variables are listed in Table 1.The desired/reference position and angle values are used insimulation tests: xd1 m, yd1 m, Zd1 m,dd0 rad,d/6.28 rad. Besides, The controller parameters are listed in Table 2.The simulation results are shown in Figs. 510.The overall control scheme managed to effectively hold thequadrotors horizontal position and attitude in finite-time, asshown in Figs. 5 and 6. The finite-time convergence of the statevariables z andis clearly faster than the other state variables,therefore, it is safe to consider the pitch angleas time invariantafter 1.163 s. In addition, the altitude z reaches its reference valueunder the controller u1after 1.779 s, thus, it is reliable to considerthe controller u1as time invariant after 1.779 s. These verify thematrix Q is time invariant in short finite-time. The other statevariables reach their desired values after about 5 s. Even thoughthe smooth curves of the state variablesandshow that theyhave certain oscillation amplitudes, the amplitudes are varyingfrom ?0.05 rad and 0.05 rad. According to the initial conditions,parameters and desired/reference values, the convergence time ofthe state variables z andis essentially consistent with the valuescalculated by invoking Eqs. (7a) and (7b) and . This demonstratesthe effectiveness of the proposed synthesis control scheme.The linear and angular velocities, displayed in Figs. 7 and 8,respectively, exhibit the same behavior as the correspondingpositions and angles. Indeed, these state variables are driven totheir steady states as expected. This, once again, demonstrates theeffectiveness of the synthesis control scheme.The behavior of the sliding variables (s2,s4and s), shown inFig. 9, follows the expectations as all the variables converge totheir sliding surfaces. Furthermore, as desired, the finite-timeconvergence of s2and s4is obviously faster than the finite-timeconvergence of s. Similarly, this exhibits the same behavior asshown in Figs. 5 and 6.Seen from Fig. 10, it can be found that the four control inputvariables converge to their steady state values (19.66,0,0,0) afterseveral seconds, respectively. Besides, u1a0, this also verifies thematrix Q is time invariant in short finite-time. In spite of the highinitial values, u1and u4are almost no oscillation amplitudes. Thisalso denotes that the time derivative of u1trends to zero. Thus,compared with the equations in 29, Eq. (11) is greatly simplified.Finally, the robustness of the proposed overall control methodis demonstrated by considering the aerodynamic forces andmoments and air drag taken as the external disturbances intothe dynamical model of the quadrotor. Furthermore, these dis-turbance terms are also applied to the controller design. As aresult, the effects of these disturbance terms are invisible on all thestate variables, sliding variables, and controllers.5. DiscussionThe extensive simulation tests have been performed to evaluatethe different synthesis control schemes which are based onposition and attitude tracking of the quadrotor UAV. It can be0510152025303540012x ( m )X: 4.977Y: 0.9909referencereal0510152