Nonlinear Control for Autonomous Trajectory Tracking while Considering Collision Avoidance of UAVs Based on Geometric Relations

Trajectory tracking with collision avoidance for a multicopter is solved based on geometrical relations. In this paper, a new method is proposed for a multicopter to move from the start position to a desired destination and track a pre-planned trajectory, while avoiding collisions with obstacles. The controller consists of two parts: First, a tracking control is introduced based on the errors between the relative position of the multicopter and the reference path. Second, once the obstacles with a high possibility of collision are detected, a boundary sphere/cylinder of the obstacle is generated by the dimensions of the vehicle and the obstacles, so as to define the safety and risk areas. Afterwards, from the relation between the vehicle’s motion direction, and the tangential lines from the vehicle’s current position to the sphere/cylinder of the obstacle, a collision detection angle is computed to decide the fastest direction to take in order to avoid a collision. The obstacle/collision avoidance control is activated locally when an object is close, and null if the vehicle moves away from the obstacles. The velocity control law and the guidance law are obtained from the Lyapunov stability. In addition, a proportional controller is used at the end of vehicle’s journey to ensure the vehicle stops at the target position. A numerical simulation in different scenarios was performed to prove the effectiveness of the proposed algorithm.


Introduction
Today, multicopters are being used more frequently due to the their many practical applications, especially in places where the working environment is dangerous or human capacity is limited, such as data collection [1,2], search and rescue [3], payload carrying [4], crop spraying in agriculture [5][6][7], and military operations [8,9].In these applications, the multicopters usually have to work within a complex environment.Furthermore, the problem of avoiding collisions between a multicopter and a variety of moving objects or stationary objects is extremely important in autonomous flight.This is especially true in some applications that require the combination of trajectory tracking with collision avoidance, such as crop spraying in agriculture, which requires the multicopter to move from the start position to a desired destination point and track a pre-planned trajectory with a reference speed, while avoiding collisions with obstacles.In this case, the coordination between the tracking control and the collision avoidance control must be harmonized to ensure that the vehicle operates safely and efficiently.This problem is especially difficult and represents a significant challenge in the autonomous flight of the vehicle.Much research has been conducted in related fields.Generally, the literature can be classified into two main groups.In the first group, the research is based on a proposed navigation The combination of trajectory tracking control and obstacle/collision avoidance control in the autonomous flights of multicopters is introduced in this paper.This study presents a new method to drive the multicopters from a start position to a desired destination, whilst tracking a pre-planned trajectory and avoiding collisions with obstacles.The problem becomes simplified if the control objective is divided into two tasks, which are tracking control and collision avoidance control.Both of these were solved separately, and then combined by the movement strategy.The main advantage of this combination is to ensure that each controller focuses on one task.Furthermore, the collision avoidance control method can only work locally, and it is null if the vehicle moves away from the obstacles.Following this, the tracking control continues working without perturbations.This makes computation much faster.First, from the geometrical relation between a multicopter and the pre-planned path, the tracking errors are computed to design a tracking control law that guarantees the vehicle follows the reference path with a reference speed.Once an obstacle is detected, the safety and risk zones are generated, and then a collision detection angle is computed to determine the fastest direction for avoiding obstacles.A guidance law is presented to control the heading angle of the multicopter to steer the vehicle to the left or right to avoid an object.The overall control laws are obtained from the Lyapunov stability.During the working process, both the velocity and heading angular rate are simultaneously controlled to ensure a smooth movement of the vehicle.In addition, the proportional (P) controller is used in the end of the robot's journey to guarantee the vehicle reaches a given point and stops there.
Furthermore, this study presents a simple, practical, and effective controller in comparison with other advanced methods [54,55] with considerations of the implementation on a real-time embedded system.The full control scheme, introduced in this paper, comprises of a multi-loop architecture, which includes an outer loop and an inner loop.The presented algorithms are applied in the outer loop position and heading control to generate a set-point for velocity and heading angular rate.A conventional proportional integral derivative (PID) controller is applied in the inner loop.The performance of the proposed algorithms is verified by numerical simulations with a quadcopter.

The Organizations
The remainder of this article is organized as follows.Section 2 describes the mathematic model of a quadcopter, which is used for the simulation.In Section 3, the trajectory tracking control, based on the geometrical relations, is presented.In Section 4, the collision avoidance control and movement strategy are introduced.The numerical simulations with different scenarios are given in Section 5. Finally, brief conclusions and further work are discussed in Section 6.

Mathematic Model of a Quadcopter
The dynamic model of a quadcopter has been reported in many related studies [56][57][58][59][60][61][62][63][64][65][66][67].In this section, the mathematic model is briefly presented.The fixed frame, E, and body frame, B, are considered and presented in Figure 1.angular rate are simultaneously controlled to ensure a smooth movement of the vehicle.In addition, the proportional (P) controller is used in the end of the robot's journey to guarantee the vehicle reaches a given point and stops there.Furthermore, this study presents a simple, practical, and effective controller in comparison with other advanced methods [54,55] with considerations of the implementation on a real-time embedded system.The full control scheme, introduced in this paper, comprises of a multi-loop architecture, which includes an outer loop and an inner loop.The presented algorithms are applied in the outer loop position and heading control to generate a set-point for velocity and heading angular rate.A conventional proportional integral derivative (PID) controller is applied in the inner loop.The performance of the proposed algorithms is verified by numerical simulations with a quadcopter.

The Organizations
The remainder of this article is organized as follows.Section 2 describes the mathematic model of a quadcopter, which is used for the simulation.In Section 3, the trajectory tracking control, based on the geometrical relations, is presented.In Section 4, the collision avoidance control and movement strategy are introduced.The numerical simulations with different scenarios are given in Section 5. Finally, brief conclusions and further work are discussed in Section 6.
where, the first three equations of expression (1) describe the rotational dynamics, and the next three equations present the translational dynamics; U i ∈ R, i = 1, 2, 3, 4 are control inputs to control the thrust, roll, pitch, and yaw rotation of the vehicle, which are given by [60]: where F i = bΩ i represents the thrust force produced by the motor i (i = 1, . . ., 4); Ω i denotes the speed of the motor i; b and d represent the thrust coefficient and ratio coefficient from force to torque, respectively.The mathematic model of quadcopter UAVs, as shown in Equations ( 1) and ( 2), is used to verify the proposed algorithm.Let u x and u y are position controllers of x, and y axes, which can be obtained from the Equation (1).

Trajectory Tracking Control
This section introduces the trajectory tracking control algorithm.The objective of trajectory tracking is to drive the convergence of errors between the vehicle's position and a time-varying reference point of the pre-planned trajectory to zero.From Equation (3), it can be seen that [62,63]: Obviously, Equation ( 4) is used to convert the position controllers (u x , u y ) into desired reference attitude angles φ r , θ r via desired yaw (ψ r ).The values of φ r , θ r are considered as set-points of attitude controller for rotational dynamics in Equation (1).In addition, the position controllers u x , u y are indirectly generated from the velocity, which comes from the trajectory tracking controller.Furthermore, in this study, we only consider that the mission of a quadcopter is to track a pre-planned trajectory when the vehicle is stably operating at a constant altitude h.Therefore, the problem becomes a trajectory tracking control in two dimensions with x and y axes.It means that the velocity of the vehicle with dynamics model in Equation (1) can also be computed from Figure 2 [64,65].The equation form of velocity can be derived by: where v m , ω m denote the velocity and heading angular rate of the quadcopter, respectively.
obtained by:    , , e e e is computed as follows: Let R(x r , y r ), v r denote the reference point and the movement velocity of it on the reference path, respectively; ψ r , ω r represent the orientation angle (desired yaw of quadcopter) and the orientation's angular rate (desired heading angular rate of quadcopter) of the reference velocity, respectively.The movement equation of reference point can be derived as follows: Let e 1 , e 2 , e 3 denote the position tracking errors as shown in Figure 2. Its values can be obtained by: Theorem 1.The position tracking error, e i (i = 1, 2, 3), converges to zero if the velocity v m and heading angular rate ω m satisfy the controller in Equation (8). where Proof.From Equation ( 7), the derivative of e 1 , e 2 , e 3 is computed as follows: Energies 2019, 12, 1551 6 of 20 Substituting Equations ( 5) and ( 6) to .e 1 .
Considering the Lyapunov function candidate: The derivative of V 0 is obtained as follows: .
To achieve a negative value of .

Statement Problem
Safe and stable operations are critically important issues for executing the tasks of an autonomous vehicle.A controller for collision avoidance is presented in this section.The goal of the control law is to generate the flight path through the safety zone and among obstacles.First, several important definitions, which are based on the geometrical relations, are presented as follows (Figure 3a): The value of t D and its derivative can be computed as follows: ( ) ( ) Definition 2. The risk area.Once an obstacle is detected, a boundary sphere/cylinder of the obstacle is generated to define a risk area, as shown in Figure 3a.The radius of the boundary is computed as follows: where ob R denotes the dimension of the obstacle.safe R and mc R represent the safety radius and the real radius of the vehicle, which includes the propellers, respectively.
In order to easily compute and design a controller, let us consider that safe mc R R = .

Definition 3. Collision. A collision occurs between the quadcopter and an obstacle if the relative distance t D
satisfies the following expression: where D detect represents the detection distance of the sensor; D t denotes the relative distance between the vehicle and an obstacle.Let O(x o , y o ) denote a stationary position of an obstacle in the Cartesian coordinate.The value of D t and its derivative can be computed as follows: .
Definition 2. The risk area.Once an obstacle is detected, a boundary sphere/cylinder of the obstacle is generated to define a risk area, as shown in Figure 3a.The radius of the boundary is computed as follows: where R ob denotes the dimension of the obstacle.R sa f e and R mc represent the safety radius and the real radius of the vehicle, which includes the propellers, respectively.
In order to easily compute and design a controller, let us consider that R sa f e = R mc .
Definition 3. Collision.A collision occurs between the quadcopter and an obstacle if the relative distance D t satisfies the following expression: Definition 4. Collision detection and the avoidance direction.Let β + and β − (0 represent the angles of the vectors → MN and → MP with the vector D t , respectively.
Collision detection.A collision will occur when the orientation of the velocity vector v m is in the collision cone NMP, meaning that the heading angle ψ m satisfies the following conditions: The avoidance direction.Let α denote the collision detection angle, α = (v m , D t ). where The avoidance direction of the vehicle is determined based on angle α.From Equations ( 16) and ( 17), it is easy to see that: If 0 ≤ α < β + ⇔ ψ mo ≤ ψ m < ψ mo + β + , meaning that the orientation of velocity v m is in ∠NMO.The fastest avoidance direction is the left side of the obstacle.Therefore, the vehicle turns left to avoid a collision, the collision detection angle α tracks β + .Conversely, if β − < α < 0 ⇔ ψ mo + β − < ψ m < ψ mo , the orientation of velocity v m is in ∠PMO.The fastest avoidance direction is the right side of the obstacle.Therefore, the vehicle turns right to avoid a collision, α tracks β − .Definition 5.The completion of collision avoidance.Let the vector projection of vector D t (D t 0) onto vector v m (v m 0) be given by Figure 3b: From Figure 3b and Equation (19), the collision avoidance is completely successful if cos α < 0, meaning that, angle α satisfies the following expression: Obviously, at the time successful collision avoidance, the velocity vector v m is perpendicular to vector D t , meaning that the angle α reaches ±π/2, (α = β ± = ±π/2).Remark 1.If angle α satisfies the following expression: Then, the orientation of velocity v m is outside of the collision cone NMP, which is shown in Figure 3b.This means that the collision will not occur again.Therefore, collision avoidance is considered to be basically completed.
Remark 2. As a result of Definitions 4 and 5 and Remark 1, the necessary conditions to activate the collision avoidance controller are given by: Definition 6. Sufficient conditions to activate the collision avoidance control.Let D active (0 < D active ≤ D detect ) represent the relative distance, at which the collision avoidance algorithm is activated, meaning that the controller will start to work if D t satisfies the following expression: From Remark 2 and Definition 6, the general conditions to activate the collision avoidance control are given by Equations ( 22) and ( 23).

Collision Avoidance Controller Design
The objective of the collision avoidance control is to require the angle α tracks β ± until α ≥ β + or α ≤ β − , meaning that when the tracking error angle, e = β ± − α, converges to zero, the collision avoidance process is basically completed (Remark 1).The derivative of e is computed as follows: where .β ± is obtained from Equation ( 15). .

β
± can be re-written as follows: .
The derivative of α is computed from Equations ( 17) and ( 18) as follows: .
e can be obtained as follows: .
It is easy to understand that .
e is always a continuous function when β ± satisfies the Equation ( 22).Therefore, the controller (v m , ω m ) to avoid a collision is obtained by the Theorem 2.
Theorem 2. The tracking error angle e converges to zero, meaning that the collision avoidance process is basically completed, if the velocity v m and heading angular rate ω m satisfy Equation (30): where ω avoid = v m sin e+ke D t cos β ± , k is a positive value.
Proof.Considering the Lyapunov function candidate: The derivative of V 1 is computed as follows: .
To achieve a negative value of .

Movement Strategy
In this section, the movement strategy of the vehicle with tracking control and collision avoidance control is presented.Let us assume that the vehicle moves from a start position S(x S , y S ) to a given point T(x T , y T ) and tracks a reference path.In the movement process, let C(x C , y C ) be the position at which both values of α and D t satisfy Equations ( 22) and ( 23), meaning that the collision avoidance algorithm is activated.Let N(x N , y N ) denote a position at which the collision avoidance is completed, meaning that values of α and D t do not satisfy conditions of Equations ( 22) and (23).The working path of the vehicle can be divided into two parts as shown in Figure 4.
Part 1: The curves SC, NT.The vehicle tracks the reference path with the velocity and heading angular rate, as shown in the tracking controller in Equation ( 8).
Part 2: The curve CN.This curve shows the working path of the vehicle in order to avoid an obstacle when a collision is detected.The collision avoidance algorithm is activated at point C and null at point N, as shown in the avoidance controller in Equation (30).
Method to approach the final destination T: When the vehicle closely approaches the desired position T, meaning that the relative distance ξ = (x T − x m ) 2 + (y T − y m ) 2 ≤ 2m, the velocity must be decreased to stop the vehicle at this point.Therefore, the proportional controller for position (i.e., P controller) is applied to drive the vehicle towards the destination point and stop there as follows Equation (31).
where K Pos is a positive value.
In this section, the movement strategy of the vehicle with tracking control and collision avoidance control is presented.Let us assume that the vehicle moves from a start position ( ) S x y to a given point ( ) T x y and tracks a reference path.In the movement process, let ( ) C x y be the position at which both values of a and t D satisfy Equations ( 22) and ( 23), meaning that the collision avoidance algorithm is activated.Let ( ) denote a position at which the collision avoidance is completed, meaning that values of a and t D do not satisfy conditions of Equations ( 22) and (23).The working path of the vehicle can be divided into two parts as shown in Figure 4. Part 1: The curves SC, NT.The vehicle tracks the reference path with the velocity and heading angular rate, as shown in the tracking controller in Equation ( 8).
Part 2: The curve CN.This curve shows the working path of the vehicle in order to avoid an obstacle when a collision is detected.The collision avoidance algorithm is activated at point C and null at point N, as shown in the avoidance controller in Equation (30).
Method to approach the final destination T: When the vehicle closely approaches the desired position T, meaning that the relative distance ( ) ( ) , the velocity must be decreased to stop the vehicle at this point.Therefore, the proportional controller for position (i.e., P controller) is applied to drive the vehicle towards the destination point and stop there as follows Equation (31).
where Pos K is a positive value.

Stability Analysis
As previously mentioned, the collision avoidance control only works locally, when a collision is detected, and it is null if the vehicle moves away from the obstacles.Therefore, tracking control and collision avoidance control should be separately worked to ensure that each of controller's highest focus is on the individual task.This means that at any given time, only one controller is activated, either the tracking control or the collision avoidance control.The condition of selection must follow Equations ( 22) and (23).A total Lyapunov function is considered as follows: where Energies Derivative of V is computed and proven as a negative value by Theorems 1 and 2. . If: Hence, the stability of control system in both tracking control and collision avoidance control is guaranteed by Lyapunov criterion.

Simulation Assumptions
The numerical simulations were carried out based on the mathematic model in Equations ( 1) and ( 2) to verify the performance of the proposed method with a general control structure, as shown in Figure 5. Several assumptions for the simulation platform and working environment are given as follows: First, the system's parameters of the quadcopter for the simulation are given in Table 1.Second, the mission of a quadcopter is to track a pre-planned trajectory and avoid collisions when the vehicle is stably operating in a constant altitude h.Therefore, the simulation only considers the performance of vehicle in x and y axes.The quadcopter model and proposed algorithm are implemented and simulated in the MATLAB Simulink.
,0 0, 1, 0 ,0 0, 0, 1 Derivative of V is computed and proven as a negative value by Theorems 1 and 2. If: ( ) ) If: ( ) ) Hence, the stability of control system in both tracking control and collision avoidance control is guaranteed by Lyapunov criterion.

Simulation Assumptions
The numerical simulations were carried out based on the mathematic model in Equations ( 1) and ( 2) to verify the performance of the proposed method with a general control structure, as shown in Figure 5. Several assumptions for the simulation platform and working environment are given as follows: First, the system's parameters of the quadcopter for the simulation are given in Table 1.Second, the mission of a quadcopter is to track a pre-planned trajectory and avoid collisions when the vehicle is stably operating in a constant altitude h.Therefore, the simulation only considers the performance of vehicle in x and y axes.The quadcopter model and proposed algorithm are implemented and simulated in the MATLAB Simulink.The simulation presents two examples.The first example is performed with a straight-line trajectory and other parameters of the simulation environment are shown in Table 2.In the second  2. In the second example, the simulation is performed with a spline trajectory as shown in Figure 6, and other parameters are given in Table 3.  6) 0 rad/s Reference angular rate in AB, CD, EF (as Figure 6)

Simulation Results
Example 1: The reference path is a straight-line.In this section, the first simulation was performed with obstacles at positions: O1 (6.5, 4) and O2 (11, 8.5).
The results of the simulation are shown in Figure 7.As soon as the tracking control was activated 0 < t < t 2 = 2.4 s (Figure 7h), the quadcopter moved toward the desired position T from the starting point S with an initial heading angle of tan −1 ((y T − y S )/(x T − x S )) = 35.54degrees (Figure 7b), and the heading angular rate was zero (Figure 7f).The vehicle tracked the reference line with the convergence of velocity v m to v r , as shown in Figure 7e.The position tracking errors e 1 , e 2 , e 3 converged to zero (Figure 7d).(Figure 7b), and the heading angular rate was zero (Figure 7f).The vehicle tracked the reference line with the convergence of velocity m v to r v , as shown in Figure 7e.The position tracking errors , , e e e converged to zero (Figure 7d).At time t = t 1 , D t ≤ D detect , an obstacle O1 was detected, and afterward at t 2 ≤ t ≤ t 3 , D t ≤ D active , the collision avoidance control was activated (Figure 7h).Because of α ≥ 0, therefore α tracked β + (Figure 7a).The heading angle and heading angular rate increase to steer the vehicle toward the left side for avoiding a collision with obstacle O1, as shown in Figure 7b,f,g.The angular tracking error e converged to zero (Figure 7c).At time t 3 ≤ t ≤ t 4 , β + ≤ α ≤ π/2 (Figure 7a), the vehicle moved outside the collision cone.Therefore, from this point onward a collision will not occur, and the collision avoidance control stopped working.Instead of this, the vehicle began to track the reference line with the tracking controller.In this working process, the angle α reached π/2 at t = t 4 .Collision avoidance was a success with obstacle O1 when t > t 4 , (α > π/2).
At time t = t 5 , D t ≤ D detect , the obstacle O2 was detected (Figure 7h), and at time t 6 ≤ t < t 7 , D t ≤ D active , the collision avoidance controller was activated again.In this case, α < 0, therefore α tracked β − (Figure 7a).The heading angle and heading angular rate increased toward a negative direction to steer the vehicle to the right side to avoid a collision with obstacle O2, as shown in Figure 7b,f,g.The angular tracking error e converged to zero (Figure 7c).At time t 7 ≤ t ≤ t 8 , −π/2 ≤ α ≤ β − (Figure 7a), the vehicle moved outside the collision cone.Therefore, from this point onward, a collision will not occur, and the collision avoidance control stopped working.Instead of this, the vehicle began to track the reference line with the tracking controller.In this working process, the angle α reached −π/2 at t = t 8 .The collision avoidance was a success with obstacle O2 when t > t 8 , (α < −π/2).
From the time t ≥ t 8 , the vehicle continued to track the reference line with e 1 , e 2 , e 3 converged to zero (Figure 7d).Once the vehicle approached the desired position, the velocity v m was decreased to stop the vehicle at the destination, which is shown in Figure 7e.The performance of the working path in 2D is shown in Figure 7g.During the moving process, the relative distance between the vehicle and the obstacle was always greater than the boundary radius of the obstacle D t ≥ R, which is illustrated in Figure 7h.
In the first example, the simulation to be executed with a reference trajectory was a straight-line.Obviously, the working path of the vehicle tracked the reference line well and once the obstacle with a high possibility of a collision was detected, the collision avoidance algorithm was activated and determined the best direction, which could be the left side or right side to avoid an obstacle.In order to strongly prove the effectiveness of the proposed method, the second simulation with a reference path that was a spline was carried out as following second example.
Example 2: The reference path is a spline.The simulations were performed in two different obstacles at position: O1 (5, 8) and O2 (7,14).The other parameters are listed in Table 3.
The results of the simulation are shown in Figure 8.Similarly, to first example, as soon as the tracking control was activated 0 < t < t 2 = 5.6 s, the vehicle moved from the start position S toward the desired position T with an initial heading angle of zero, and ψ m tracked ψ r (Figure 8b).The vehicle tracked the reference path with the tracking errors e 1 , e 2 , e 3 that converged to zero as seen in Figure 8d.Until the obstacle O1 was detected at t = t 1 , D t ≤ D detect , and afterward the relative distance satisfied D t ≤ D active at time t 2 ≤ t < t 3 (Figure 8h), the collision avoidance controller was activated.Angle α tracked β + (because of α ≥ 0 at t = t 2 ), as shown in Figure 8a.The heading angle and heading angular rate increased to turn the vehicle to the left in order to avoid a collision with obstacle O1 (Figure 8b,f,g).At time t = t 3 , α ≥ β + , the collision avoidance was completed with obstacle O1, the tracking control was activated again to drive the vehicle tracks to the reference path.The position of tracking errors e 1 , e 2 , e 3 gradually converged to zero (Figure 8d).However, at time t = t 5 , the second obstacle O2 was detected (Figure 8h).After that, the collision avoidance algorithm was activated again at t = t 6 , at this time angle α < 0 resulting in α tracking β − , which can be seen in Figure 8a.The heading angle and heading angular rate increased towards the negative direction to steer the vehicle to the right in order to avoid a collision with obstacle O2 (Figure 8b,f,g).After completion of the collision avoidance with obstacle O2 at t ≥ t 7 , (α < β − ) the vehicle continued to track the reference path with e 1 , e 2 , e 3 converging to zero (Figure 8d).When the vehicle approaches to the target point, the velocity v m decreased to stop the vehicle at the desired position, as in Figure 8e.The working path of the vehicle in 2D is shown in Figure 8g.In this movement process, the relative distance D t always satisfied the condition D t ≥ R (Figure 8h).The three-dimension performances of example 1 and 2 are shown in Figure 9.
to track the reference path with 1 2 3 , , e e e converging to zero (Figure 8d).When the vehicle approaches to the target point, the velocity m v decreased to stop the vehicle at the desired position, as in Figure 8e.The working path of the vehicle in 2D is shown in Figure 8g.In this movement process, the relative distance  always satisfied the condition t D R ≥ (Figure 8h).The three-dimension performances of example 1 and 2 are shown in Figure 9.  From the simulation results of the two examples with different scenarios, we can conclude the following: First, the quadcopter can track a pre-planned arbitrary curve.The tracking control worked well both before and after completion of the collision avoidance process and without any perturbations.Second, the vehicle always determined the nearest and fastest direction to avoid a collision in order to minimize the moving time from the start position to the target point.Third, the proposed method is simple and requires low computation.Therefore, it can be used for the multicopters in applications that require trajectory tracking, while avoiding collision in a complex environment.From the simulation results of the two examples with different scenarios, we can conclude the following: First, the quadcopter can track a pre-planned arbitrary curve.The tracking control worked well both before and after completion of the collision avoidance process and without any perturbations.Second, the vehicle always determined the nearest and fastest direction to avoid a collision in order to minimize the moving time from the start position to the target point.Third, the proposed method is simple and requires low computation.Therefore, it can be used for the multicopters in applications that require trajectory tracking, while avoiding collision in a complex environment.

Conclusions
A nonlinear control method for autonomous trajectory tracking with collision avoidance of a multicopters based on geometrical relations was introduced in this study.The combination of tracking control and collision avoidance control, via movement strategy, ensures the highest priority to perform the subtask.The controllers of the velocity and heading angular rate, to drive the convergence of errors to zero, were obtained from the Lyapunov stability in order to guarantee the vehicle tracks the reference path.In addition, the collision avoidance algorithm is presented to ensure the safe movement and stable operation of the vehicle.A boundary sphere/cylinder of obstacle is defined to generate the risk and safety areas when an obstacle is detected, and then the fastest avoidance direction is determined to avoid a collision.
We performed numerical simulations in various scenarios, and the good results prove the effectiveness of the proposed method.However, the study did not consider trajectory tracking with collision avoidance in a dynamic environment.Future will look into the same problems with moving obstacles.

Figure 1 .
Figure 1.The quadcopter configuration.Let φ m , θ m , φ m ∈ (−π/2, π/2), θ m ∈ (−π/2, π/2) and ψ m , ψ m ∈ (−π, π) represent roll, pitch, and yaw angles of a quadcopter, respectively.The position of vehicle in a fixed frame E is denoted by vector (x m , y m , z m ).l represents the arm length of the vehicle.Let I xx , I yy , I zz represent the moment of inertia along x, y, z axes of the quadcopter, respectively; m is the total mass.The mathematic model of the vehicle is described as follows [60]:

Figure 2 .Theorem 1 .
Figure 2. The geometry of describing the kinematic equation of the quadcopter and the reference point.Theorem 1.The position tracking error, ( 1,2,3) i e i = , converges to zero if the velocity m v and heading angular rate m w satisfy the controller in Equation (8).

Figure 2 .
Figure 2. The geometry of describing the kinematic equation of the quadcopter and the reference point.

Figure 3 .Definition 1 .
Figure 3.The geometrical relations of the quadcopter and an obstacle: (a) Fastest direction for collision avoidance; (b) The completion of collision avoidance.Definition 1.The obstacle detection.An obstacle is detected if the relative distance t D satisfies the following expression:

Figure 3 .Definition 1 .
Figure 3.The geometrical relations of the quadcopter and an obstacle: (a) Fastest direction for collision avoidance; (b) The completion of collision avoidance.Definition 1.The obstacle detection.An obstacle is detected if the relative distance D t satisfies the following expression: D t ≤ D detect (9)

Figure 4 .
Figure 4.The movement strategy of the vehicle.Figure 4. The movement strategy of the vehicle.

Figure 4 .
Figure 4.The movement strategy of the vehicle.Figure 4. The movement strategy of the vehicle.

Figure 5 .
Figure 5.General block diagram of the proposed algorithm.

Figure 5 .
Figure 5.General block diagram of the proposed algorithm.The simulation presents two examples.The first example is performed with a straight-line trajectory and other parameters of the simulation environment are shown in Table 2.In the second example, the simulation is performed with a spline trajectory as shown in Figure6, and other parameters are given in Table3.

Figure 6 .
Figure 6.The spline for the second example simulation.Figure 6.The spline for the second example simulation.

Figure 6 .
Figure 6.The spline for the second example simulation.Figure 6.The spline for the second example simulation.
), the quadcopter moved toward the desired position T from the starting point S with an initial heading angle of

Figure 7 .
Figure 7. Example 1, the reference path is a straight-line and obstacles were placed at O1 (6.5, 4) and O2 (11, 8.5): (a) Angle α tracking β ± ; (b) Attitude performance; (c) The angular tracking error e; (d) The position tracking error e1, e2, e3; (e) The velocity performance of the vehicle; (f) Heading angular rate; (g) The performance of the working path in 2D; (h) The relative distance between the vehicle and the obstacle.

Figure 7 .
Figure 7. Example 1, the reference path is a straight-line and obstacles were placed at O1 (6.5, 4) and O2 (11, 8.5): (a) Angle α tracking β ± ; (b) Attitude performance; (c) The angular tracking error e; (d) The position tracking error e1, e2, e3; (e) The velocity performance of the vehicle; (f) Heading angular rate; (g) The performance of the working path in 2D; (h) The relative distance between the vehicle and the obstacle.

Figure 8 .
Figure 8. Example 2, the reference path was a spline and an obstacle was placed at O1 (5, 8) and O2 (7, 14): (a) Angle α tracking β ± ; (b) Attitude performance; (c) The angular tracking error e; (d) The position tracking error e1, e2, e3; (e) The velocity performance of the vehicle; (f) Heading angular rate; (g) The performance of the working path in 2D; (h) The relative distance between the vehicle and the obstacle.

Figure 8 .
Figure 8. Example 2, the reference path was a spline and an obstacle was placed at O1 (5, 8) and O2 (7, 14): (a) Angle α tracking β ± ; (b) Attitude performance; (c) The angular tracking error e; (d) The position tracking error e1, e2, e3; (e) The velocity performance of the vehicle; (f) Heading angular rate; (g) The performance of the working path in 2D; (h) The relative distance between the vehicle and the obstacle.

Figure 9 .
Figure 9. Three-dimension performance of simulations: (a) First example with a straight-line trajectory; (b) Second example with a spline trajectory.

Table 1 .
System's parameters of quadcopter for simulation.

Table 2 .
Parameters of the first example simulation.

Table 3 .
Parameters of the second example simulation.

Table 2 .
Parameters of the first example simulation.

Table 3 .
Parameters of the second example simulation.
wr Reference angular rate in SA, BC, DE, FT (as Figure