Trajectory tracking control for quad-rotor system in the presence of velocity constraint

This article mainly considers the problem of trajectory tracking control problem of quad-rotor system with velocity constrain under the consideration of safety. A model-based nonlinear controller is proposed which can guarantee not only the asymptotical stability for control system but also the velocity under a safe range. Firstly, based on backstepping design, a position tracking controller with velocity constraint is proposed to ensure that the desired position can be tracked with velocity constrains. At the second step, considering attitude subsystem, an attitude controller is proposed to improve the attitude dynamic response performance. Finally, the validity and superiority of the design has been verified in simulation results.


Introduction
With the continuous development of industrial society, the research on robot control is becoming increasingly intense. [1][2][3][4][5] Unmanned aerial vehicle (UAV) system, as a hotspot of robotic technology, is widely employed in different kinds of fields. As one of the main types of UAV, unmanned multi-rotor aircraft has many advantages such as high mobility, great reliability, and good adaptability, which is extensively applied in military and civilian industry, such as aerial photography, freight transportation, rescue, and mapping. [6][7][8] In particular, the quad-rotor system is a 6-degree-of-freedom (DOF), nonlinear, under-actuated, and coupled system; the model is not only complex but also prone to be disturbed. 9 In the previous work, various nonlinear control strategies were employed to improve the system response performance for attitude and position control in UAV system. In the literature, 10 a robust backstepping-based approach combined with sliding mode control is proposed for trajectory tracking of a quadrotor UAV subject to external disturbances and parameter uncertainties associated with the presence of aerodynamic forces and possible wind force. An adaptive model-based predictive controller for attitude and trajectory tracking of a vertical take-off and landing (VTOL) aircraft in the simultaneous presence of model uncertainties and external disturbances is introduced in the work by Emami and Rezaeizadeh. 11 The system state of quad-rotor, especially the velocity, can be hardly constrained with hardware means. Generally, by changing the control parameters, the dynamic response can be slowed down to reduce the velocity of the flight. This kind of method has poor adaptability for different constraints, and some tracking performance is sacrificed. In recent years, some high-performance control strategies have been proposed for dynamic and steady-state performance of several kinds of state constrained systems [12][13][14][15][16][17][18][19] and for spacecraft systems. [20][21][22][23][24][25][26][27] This article focuses on designing a method of constraint control to solve the position tracking control problem for quad-rotor system with velocity constrains. Firstly, considering the position subsystem, a position tracking controller with velocity constraints is proposed to ensure that the desired position can be tracked with velocity constrains. A remarkable feature is that the nonlinear term is added into the differential feedback to adjust the control gain actively to achieve velocity constrains. Secondly, an attitude controller is proposed to improve the tracking performance of the attitude subsystem. The main contribution of this article lies in two aspects: (i) The proposed controller is simple to implement and has low complexity which reduces the processing requirement. (ii) Without cost function or other auxiliary tools, the velocity constraint is satisfied which is proved strictly in theory.

Mathematical model and problem statement
Mathematical description of quad-rotor aircraft Figure 1 gives a model of a quad-rotor aircraft with "þ" shaped rigid structure. Each propeller at the end of each arm has one motor. The lifts F i ; i ¼ 1; 2; 3; 4 generated by propellers are perpendicular upward to the rotor plane. Six DOFs are required for modeling the quad-rotor, in detail, the position P ¼ p; q; r ½ T and the attitude in Euler angles, E ¼ '; q; ½ T . The position dynamic equation of a quadrotor aircraft is described as 28 where T represents the velocity, n r ¼ 0; 0; 1 ½ T represents a unit vector in z-positive direction of the earth-fixed coordinates, F ¼ P F i ; i ¼ 1; 2; 3; 4 represents the total thrust, m represents the mass of quad-rotor, R eb represents the rotation matrix, in detail, from body-fixed coordinates to earth-fixed coordinates, and, R eb n r ¼ cos sin q cos ' þ sin sin q sin sin q cos ' À cos sin q cos ' cos q It follows from Zuo 29 that the attitude dynamic equation of a quad-rotor is given by Note that W is the orthogonal rotation matrix by the Euler angles and explained as 0 sin' sec q cos ' sec q and O ¼ ! p ; ! q ; ! r Â Ã T is the angular velocity expressed in the body-fixed coordinates, J is the inertia matrix in the body-fixed coordinates, t ¼ t p ; t q ; t r Â Ã T means three rotational forces generated by the lifts F i ; i ¼ 1; 2; 3; 4. Define O Â as a skew-symmetric matrix and given by Remark 2.1. It should be pointed out that in practice the actual control input for the quad-rotor aircraft is the four motors rather than the control forces t ¼ t p ; t q ; t r À Á and the total thrust F. Hence, there is a conversion relation between motors' speed and the rotational forces in Tayebi and McGilvray. 30 The relationship between the actual outputs F i ; i ¼ 1; ::: ; 4, and F; t ð Þ is given as follows For the sake of statement, we only consider the design of the control inputs with F; t p ; t q ; t r À Á in this article.

Control goals
This article focuses on designing a finite-time position control algorithm for a quad-rotor spacecraft such that the desired position trajectory of quad-rotor P d ¼ p d ; q d ; r d ½ T can be tracked in a finite time. At the same time, the constraints of velocity will be held to ensure the safety and stability the during the flight, that is where B p > 0; B q > 0; B r > 0 are velocity constrains in p-axis, q-axis, r-axis of the earth-fixed coordinates, respectively.
To achieve the control objective, the following assumptions are imposed. Assumption 3.1. In common sense, for the sake of guaranteeing the stable and reliable flight of the quad-rotor aircraft, the desired velocity is restricted to the velocity constraint. Hence, the following inequality will be held as the following: Controller design for quad-rotor aircraft In this section, the backstepping approach is used in the controller design for quad-rotor aircraft. To be specific, the control design method is developed in two steps.

Design of a position controller with velocity constraints
For convenience of PD controller design, a virtual control u ¼ u p ; u q ; u r Â Ã T is construed as the virtual control outputs for position subsystem, and Substituting (9) into (1), the position subsystem of quad-rotor becomes a second-order system in vector form, that is The key to serve the needs of the velocity constraints is to construct a self-regulation mechanism for velocity in the control behavior. An active nonlinear adjustment term is designed for differential gain in the controller directly. At this point, the virtual control outputs are constructed as: where a 1 > 0, a 2 > 0.
Theorem 4.1. If the nonlinear controller is constructed as (11), then the position p; q; r ½ T in system (10) will asymptotically converges to the desired position trajectory p d ; q d ; r d ½ T . In the meantime, the velocity constraints (7) are satisfied.
Proof. There is no loss of generality where only the proof about p, the position of p-axis, is presented. For the sake of statement, the process is finished into two parts, that is, global asymptotic stability and velocity constraints.
Part one: Global asymptotic stability In this part, the error dynamics equation of position subsystem (10) is provided first, and then the analysis method of Lyapunov function is used.
The position tracking error is defined as According to the position subsystem (10), the dynamic equations of (12) can be acquired, that is Obviously, the following relation holds For simplicity's sake, denote where N is defined as error-lower-bound, N error-upperbound. Thus, v p 2 ÀB p ; B p À Á is equivalent to _ f 2 N ; N À Á . By referring to the inequality (8), it yields N < 0 and N > 0.
Moreover, the active nonlinear adjustment term for differential gain can be reformed with Substituting the proposed PD position controller (11) with (16) into (13) results in a closed-loop error system: Choose the Lyapunov function whose derivative along system (17) is According to LaSalle's invariant principle, it is easy to conclude that ð f ; _ f Þ ! 0 as t ! 0.

Part two: Velocity constrains
In this part, two Propositions are used to prove that v p 2 ÀB p ; The proof of Proposition 1 is completed.
Proposition 2. There exist N 1 2 ðN ; 0Þ and N 1 2 ð0; N Þ, Proof of Proposition 2. Based on Proposition 1, It indicates that It indicates that The proof of Proposition 2 is completed. According to these two propositions, the velocity constrains can be guaranteed. The proof is completed.
Remark 4.1. It should be pointed out that the essence reason in the proposed nonlinear position controller (11) is to employ the active nonlinear adjustment term p j , which is a nonlinear function. In other words, when v p tends to +B p , this term will tend to infinity. That is to say, the differential gain p j will increase automatically, and the strength of this kind of the regulation can change actively with the desired velocity.

Design of an attitude controller
For attitude subsystem, a PD control method will be designed to make sure the desired attitude can be asymptotically stable tracked in this section. In the above section, the attitude of quad-rotor E ¼ ð'; q; Þ are set as references such that the control objective of position subsystem (10) can be realized.
The desired attitude, which is denoted by Þ , is produced from virtual control outputs u. According to the relation (9), we have Considering d as a free variable, the desired yaw angle can be set as d ¼ 0 for the convenience of analysis.
Define the attitude tracking error as ¼ E d À E. Based on the attitude dynamic equation of a quadrotor (3), the error dynamics equation of attitude can be represented by Theorem 4.2. Considering the system (26), and t is designed as where b 1 > 0; b 2 > 0, then the desired attitude can be asymptotically stable tracked, that is, Proof. For sake of convenience, the proof about roll angle ' has been provided. On the basis of the attitude controller (27), Rewrite the system (26) in the following form Similar to Theorem (4.1), two parts are employed here to proof the global asymptotic stability.
Part one: Global asymptotic stability Choose the Lyapunov function whose derivative along system (28) is According to the (30), it can be known that _ S 1 0 means _ ' 0 and € ' 0, similarly, ' 0 can be obtained from (28). According to LaSalle's invariant principle the following conclusions can be drawn: ' ; _ ' ! 0 as t ! 0, that is, the system (28) can prove to be globally asymptotically stable.

Simulation results
To verify the utility of the proposed control method, this section has provided the simulation results. The main parameters for the quad-rotor are as follows: m ðkgÞ represents the total mass of quad-rotor and its value is 1:3424, g m=s 2 represents the acceleration of gravity and its value is 9:81, h m ð Þ represents the distance between the epicenter of the quad-rotor and the rotor axis and its value is 0:2, J pp kg Á m 2 ð Þrepresents the rotary inertia of p-axis and its value is 0:01175, J qq kg Á m 2 ð Þrepresents the rotary inertia of q-axis and its value is 0:01175, J rr kg Á m 2 ð Þrepresents the rotary inertia of r-axis and its value is 0:02229.
As for the quad-rotor in simulation part, the initial state is set to pð0Þ; qð0Þ; rð0Þ; 'ð0Þ; qð0Þ; ð0Þ ð Þ T ¼ 0; 0; 0; 0; 0; 0 ð Þ T The desired position trajectory is given by For the sake of illustration, three kinds of control strategies are applied to have a comparison, that is, low gains PD control without velocity constrains, PD control without velocity constrains (PDVC), and the proposed PD control without velocity constrains. The control gains of finite-time control with velocity constrains are as follows: a 1 ¼ 4:68, a 2 ¼ 3:36, g 1 ¼ 0:80, g 2 ¼ 0:89, b 1 ¼ 4:00, b 2 ¼ 3:50, r 1 ¼ 0:75, and r 2 ¼ 0:86: From the view of practice point, indoor conditions should be considered in this simulation environment. For security protection, the velocity constrains are designed by For comparison, the position tracking error curves of the four control methods are given in Figure 2. The results show that the proposed PD controller with speed limit has better convergence performance when the system reaches the equilibrium point.
Moreover, a quantitative comparative analysis was made. Table 1 describes the comparisons with the convergence time after when the position tracking error is smaller than 0.003.
For illustrate the characteristic of velocity constrained, Figure 3 shows the velocity response curves in this simulation. It can be found that the velocity responses of the controllers which have characteristics of velocity constrained, that is, PDVC, can be restricted in velocity constrains while have been exceeded in the PD controller. Besides, the velocity constrains also can be achieved in LPD, which has unsatisfied performance due to lower control gains.

Conclusions
In this article, a nonlinear position control method with velocity constrains is proposed to solve position tracking problem for quad-rotor system with velocity constrains. Firstly, a PD position tracking controller with velocity constraints is proposed to ensure that the desired position can be asymptotically stable tracked with velocity constrains.
Secondly, considering attitude subsystem, a PD attitude controller is proposed to improve the attitude dynamic response performance. Finally, simulation results proved the validity and superiority of proposed position controller with velocity constrains.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) received no financial support for the research, authorship, and/or publication of this article.