Tracking Control for Quad-Rotor Using Velocity Field and Obstacle Avoidance Based on Hydrodynamics

: In this work, the problem of navigation control of a quad-rotor, which includes path planning (to a desired trajectory with obstacle avoidance) and tracking control, is addressed. Path planning is achieved by means of the velocity ﬁeld technique, since this method generates smooth trajectories that are suitable for this kind of vehicles. We propose a recursive method for the composition of the total velocity reference. In order to achieve a good tracking of the generated references, a saturated controller is used, namely, nested saturation. It is demonstrated that the velocity reference can be tracked with a reduction in the order of the controller, from four to three saturators, which simpliﬁes the implementation. Numerical results show that a correct tracking could be guaranteed.


Introduction
One of the inherent issues in the control of autonomous vehicles is the navigation control, which includes path planning and tracking control [1]. The velocity field could be seen as a stream that drags a vehicle in the direction of that stream at each point. The potential functions are used to generate trajectories and avoid obstacles. The velocity potential functions, obtained from hybrid potential field (HPF), have the advantage of completely eliminating local minima [2]. The velocity field method is attractive because it presents a very low computation cost [3] and is mathematically elegant [2].
Several applications have been proposed using this method. In Reference [4] this technique is used to control multiple three-wheeled robots that cooperate to achieve a task. The use of probabilistic algorithms altogether with the velocity field is developed in Reference [5] where the fuzzy logic defines the velocity to be used in the tracking control. Another application is to generate a trajectory for an unmanned aerial vehicle (UAV) to collect information from a terrain after a natural disaster [6]. In Reference [7] the velocity field is utilized for the movements of an exoskeleton. Recent work in References [8][9][10] uses velocity fields or potential fields, not necessarily to generate strictly a desired path, but for collision avoidance in the path of autonomous vehicles.
The method of velocity field for path planning arose in the early 1990s [11]. Kim and Khosla [2] proposed that this control strategy could be used for real-time control of mobile robots and manipulators. However they just presented numerical results without a dynamic model of a robot. For the purpose of a real path following, an alternative was proposed by P. Y. Li and R. Horowitz [12]; they used a vector field to define a trajectory, thus by tracking the velocity field it is possible to reach the objective. The use of velocity fields for tracking control is considered due to this technique depends on the actual position of the robot, unlike the traditional method which depends on time. The philosophy of this to illustrate the steps required to derive the equations for successive derivatives of attractive and evasive fields.

Dynamic Model
The dynamic model of the quad-rotor is obtained, representing the vehicle as a rigid body in a three-dimensional space and attached to one force and three torques. The motors dynamics and propellers flexibility are neglected [22].
Consider a quad-rotor aircraft configuration such as the one shown in Figure 1. The generalized coordinates are where ξ = (x, y, z) T ∈ R 3 denotes the quad-rotor center of mass position, relative to the inertial frame I, and η = (φ, θ, ψ) ∈ R 3 represents the orientation vector of the body frame, expressed in Euler's angles. The dynamic model of the quad-rotor is given as: where m stands for the mass of the vehicle, g is the gravitational constant, s a = sin a, c a = cos a, and whereτ is a vector with the quad-rotor torques, τ is a vector with the control inputs, J represents the inertial matrix and C denotes the Coriolis matrix. The inputs of force and torques, u and τ, respectively, are related with the velocity of the four rotors as follows. The thrust input, u, is the sum of the thrust of each rotor, expressed by where κ T > 0 is a constant coefficient and ω i denotes the angular velocity of the i-th rotor.
The torque inputs τ = [τ φ , τ θ , τ ψ ] T , are obtained through where is the distance from the rotors to the center of gravity of the quad-rotor, and τ M i denotes the gyroscopic torque produced by the motor M i . The torques τ M i are obtained from

Control Strategy
For the aircraft control, we assume that the altitude and yaw angle are fixed at some desired value.

Altitude and Yaw Control
Let us define ψ d = 0 ∀ t > 0, such that the quad-rotor maintains a zero degree yaw angle during the flight. The control is obtained through whereψ = (ψ − ψ d ), k pψ and k vψ denote the proportional and derivative constants of the PD controller, respectively. In a similar way, the control law for maintaining a constant altitude can be achieved by applying the control input wherez = z − z d represents the altitude error, k pz and k vz are positive constants related to the PD controller. The roll and pitch angles (φ, θ) must be small to avoid singularities in (8) and to handle them in the linear region. The nested saturation controller allows that these angles will remain small.

Roll and Pitch Control
For tracking a trajectory, the nested saturation controller is used. This method was proposed by Teel [18]. It is used to stabilize a chain of integrators in cascade and is asymptotically stable for both, regulation and tracking trajectory. In tracking mode, the nested saturation controller allows to reach high speeds of the vehicle while maintaining bounded control inputs, inside the limits of actuator constraints.
Consider the quad-rotor model (1)- (6), under the control inputs (7) and (8). After a finite time, z → z d , andz → 0, then,ż, ψ,ψ → 0 and the system (1)-(6) can be represented as: and considered to be restricted to small angles θ and φ, such that tan(φ) ≈ φ and cos(φ) ≈ 1, and so for θ, the system (9)-(10) can be seen as a reduced system: where both subsystems are integrators in cascade and d dt Let us define the state variables of each subsystem as x and y, thereby With the purpose to achieve the tracking of the trajectory, it is necessary to apply the control over the error vectors, this is Given that this system will follow a velocity field, the position error could be neglected of the state error vector, and still follow the desired trajectory, so the error vector could be expressed by Each subsystem can be represented in the formẋ = A x x + B x u, as in Equation (13), with state vector x ∈ R n as in (16), u ∈ R, then a linear transformation exist z x = T zx x e that maps (13) iṅ where the elements k 1 , . . . , k n = 0 ∈ R. In this particular case the transformation matrix elements in T zx matrix are with the vector x e of the Equation (16). Given that the state variable e x is set to 0, the element (1, 1) of matrix T zx can be 0, then In a similar way z y = T zy y e , is defined T zy as: with the vector y e given in Equation (16), where the control law according to the conventional nested saturation function, for the case of tracking trajectory is where b is a positive constant; therefore, the controller for the pitch angle is and for the roll angle is with b 1 , b 2 , b 3 and b 4 as the bounds of the saturation functions.
Since the objective of the vehicle is to follow the desired trajectory according to a velocity field, the error vector (16) could be reduced to With the new state error vectors (21) instead (16), it is possible to omit the state z 1 in the diffeomorphism z x = T zx x e , then T zx of Equation (17), and in the same way T zy , could be reduced to a 3 × 3 matrix Consequently, the saturation controller (19), could be reduced to only 3 saturators, as: and for the roll angle: The control law of three saturators of Equation (23) is equivalent to the control law of Equation (19) with k 1 = 0. According to Reference [19], if none of the σ i are saturated, the poles of the linearized closed loop system reside at {−k 1 , −k 2 , −k 3 , −k 4 }. This means that in the case of the three saturations controller (23), when none of the σ i are saturated, the poles of the linearized closed loop system reside at {0, −k 2 , −k 3 , −k 4 }. Both controllers are stable; however, the difference between k 1 = 0 or k 1 = 0, going to the same reference, (and in general for every constant k i ) is that for higher k i , the error rates in saturated equilibrium decrease, but the overall settling time can be higher.
In this case, it is convenient to use the transformation of Equation (22) and the control law of Equation (23), because these equations need fewer computational operations.

Velocity Field
The objective of the velocity field method in this particular case is to follow a predefined trajectory. For this method, it is necessary to calculate two vector fields, the approaching field and the tangential field. The approaching field is defined by the vectors that aim directly to the trajectory, in which each vector V ac is obtained as the normalized subtraction of the closest point to the trajectory, as proposed by Reference [17]. The trajectory to follow is a circle of radius r tr and the center at the point (o In order to calculate the approaching field, it is necessary to find the closest point from any position of the workspace to the trajectory. This closest point is calculated by where x tr and y tr are the points conforming the trajectory, that is, the set of all the points that satisfy Equation (25). For this specific case, let us define the vectors: wherex,ȳ are the difference between any point of the workspace and the center of the circular trajectory, andx,ỹ are the position errors from any point of the workspace and its closest point within the desired trajectory. The coordinates of the closest point to the trajectory are where α cl denotes the angle from the actual position to the closest point of the trajectory. Taking the Equations (28)-(29) to calculate x cl and y cl , we can define the distance between the actual position and the closest point in the trajectory as ξ = x 2 +ỹ 2 .
The angle α cl is obtained from the derivative of the distance ξ with respect to α cl , or d ξ /dα cl = 0, and it is The approaching field is defined as Let us denote the partial derivatives of x cl and y cl as v x c and v y c .
These values are necessary to generate the tangential field as follows The velocity field is obtained performing the normalized weighted sum where F 1 and F 2 are functions of the Euclidean distance between a point in space and the desired trajectory, which are given by: The aim of this function is to increase the effect of the approaching function far from the trajectory and the tangential function close to the trajectory, with γ as the parameter that controls the weighting of both vectors. In Figure 2 is shown a velocity field generated with a value γ = 0.4, o x = 40, o y = 30 and r tr = 10. The velocity field vector is expressed by (35). It represents the desired velocity that the vehicle must achieve at each instant, in order to track the trajectory. With the nested saturation controller, given by Equations (23) and (24), it is possible to track the vectors x e and y e given in (21).
Since the velocity field vector (35) is a normalized vector, then the magnitude of the velocity vector is one. In order to impose other magnitude to the velocity reference, this vector could be multiplied by a constant K v . The resulting vector could be seen as the desired value of the velocity in x and y coordinates, this is: with these desired values, the velocity error variables eẋ =ẋ −ẋ d and eẏ =ẏ −ẏ d can be computed. However, in order to compute the remaining error variables of Equation (21), are still necessary the desired values: θ d ,θ d , φ d andφ d , even more, the tracking controllers (23) and (24) need the desired variablesθ d andφ d . From Equations (13) and (14) we have thatẍ = gθ andÿ = −gθ, so θ =ẍ/g and and  Equation (38) gives us a way to obtain the desired variables to track, and it means that it is necessary to compute three successive derivatives of the potential velocity (35). Appendix A illustrates the procedure to obtain the successive derivatives of V.

Obstacle Avoidance
Obstacle avoidance is achieved by using hydrodynamic theory, specifically the flux of an ideal fluid around a cylinder as shown in Figure 3. Let us define a flux of a planar fluid in stationary state, with a vector velocity field v(p) = u(x, y) v(x, y) in the point Here Ω ⊂ R 2 is the domain occupied by the fluid, then for it to be an ideal fluid it must be incompressible, meaning vanishing divergence, and irrotational, implying a vanishing curl is a complex analytic function of z = x + iy [23]. Thus, the components u(x, y) and −v(x, y) are necessarily harmonic conjugates. The corresponding complex function (40) is known as the complex velocity of the fluid flux.
Let f (z) allow a complex anti-derivative, that is, a complex analytic function Using the Cauchy-Riemann equations for the complex derivative, Thus, ∇Φ = v, and therefore the real part Φ(x, y) of the complex function χ(z) defines the velocity potential. Consequently, χ(z) is known as the complex potential function for the fluid velocity field.
Taking advantage of this property, it is possible to use any harmonic function for the complex velocity potential and the real velocity of the fluid, ∇Φ = v, is automatically irrotational and incompressible.
Using a circular obstacle that interferes with the approaching vector field flux, where (x o , y o ) is the center position of the obstacle, with radius r o , the position error with respect to the center of the obstacle is defined asξ The proposed velocity potential function for an ideal fluid through a circular obstacle is is the angle of the velocity field vector in that point. The flux velocity around a cylinder is defined as the gradient of the Φ function, namely v = ∇Φ. Then, the x-component of the velocity vector is where cβ = cos β and sβ = sin β. The velocity function in the y-coordinate is Therefore, the velocity vector for the evasive maneuver is It could be said that the effect of the equations for evasion is to reorient the direction of the velocity vector, which was computed before considering the obstacle, redirecting the velocity reference to try to surround the obstacle. Then, it makes sense that the modified velocity field, V ev , takes the place of the previous velocity field, however, it could be also normalized as Appendix B illustrates the procedure to obtain the successive derivatives of the velocity reference V, obtained from Equation (45). These derivatives are necessary for tracking control.

Remark 1.
The method proposed to avoid obstacles allows to manage the evasive field for multiple obstacles in a recursive way. In the case of multiple obstacles, the algorithm starts from the attractive velocity field of Equation (35). The next step is to create a list of the obstacles located in the workspace, from the furthest to the closest to the vehicle. Then, considering sequentially all the obstacles, at each iteration, the previous velocity vector V is transformed, through Equation (45), in the new velocity field (considering evasion of the k-obstacle), and it will become the new reference of velocity, V , in the next iteration. After finishing all iterations, the modified velocity field achieved, becomes the actual velocity reference for Equations (38) and (39). Figure 4 shows the modification of the velocity field shown in Figure 2, resulting from the incorporation of three obstacles.

Numerical Results
The first simulation consists only of the approaching velocity field of Equation (35). The vehicle begins at position p = [0, 0] T . In Table 1 are shown the values of the controller parameters for simulation. Table 1. Parameters and their corresponding values used in the simulation.

Parameter Value
K v 2 Figure 5 shows the trajectory followed by the vehicle. The "×" symbol points out the initial position, then the vehicle is guided by the velocity field to the circular trajectory.  Figure 6 shows two plots-one for the position error, this is ξ of Equation (27), and the other one for the speed error, this is [eẋ, eẏ] T , where eẋ and eẏ are defined in Equation (15). It can be seen that the velocity field is reached after 6 seconds, then the vehicle keeps tracking the velocity field thereafter. An adequate tracking of the velocity field leads the vehicle to follow the circular trajectory which is reached around 25 seconds, according to the position error plot, and the vehicle remains in the circular trajectory thereafter at a speed of 2 m/s, according to the selected value for the constant K v (see Table 1).

Experimental Results
For the experiments, a 3DR IRIS+ quad-copter was used. This vehicle has a Pixhawk autopilot, and the position was measured with a GPS. The experiment consist in the flight from the point (0, 0), to the circular reference trajectory with center at the point (12,12), and radio = 3 m. One obstacle of radio = 1.5 m was virtually positioned in the coordinate (4,4). The Figure 9 shows the path followed by the quad-rotor. The vehicle completed the task satisfactorily and it showed an acceptable performance.  In this experimental test, the obstacle is virtual. However, real obstacles could be detected with multiple sensors like cameras on board the UAV, fixed cameras like those for motion capture, or other sensors like LIDAR . Whatever means of detection is selected, it must be ensured to provide accurate information of the width of the obstacles, to calculate the radius of the cylinder to evade.

Conclusions
The most important contributions of this work are: (i) the recursive generation of a velocity field that includes attractive field and evasive field for obstacle avoidance; (ii) the derivation of the time derivatives of the attractive and evasive velocity reference, needed to compute the desired tracking variables; and (iii) the use of a reduced asymptotically stable tracking control law (from four to three saturators), which results simpler for implementation. Simulation results show that the tracking nested saturation controller for velocity vanishes the velocity error and its derivatives, which indicate that the velocity field is perfectly tracked, showing that the combination of these strategies provides a reasonable tracking performance.
For future work, we will consider obstacles with arbitrary shapes by using the panel method. Also, a robust control for wind perturbations will be considered. Funding: This work was supported by Universidad de Sonora with internal project USO315005985.
be the numerator of the velocity field (35). Its time derivative iṡ Thus, the time derivative of the velocity field iṡ

Second and Third Derivative of Velocity Field
The derivation of the second and third derivatives implies two successive derivatives of Equations (A1)-(A10). We will just present the second and third derivative of V in order to be more concise.
The second derivative of the velocity field is

Appendix B. Derivative of Evasive Velocity Field
The evasive velocity field V is expressed by Equation (45), which requires the vector V ev of Equation (44). The next subsection shows the derivative of the evasive velocity field (45).

Appendix B.1. First Derivative of Velocity Field
The derivative of the angle β, of Equation (41), (the angle of velocity field before the addition of the considered obstacle) isβ The time derivative of V ev x , of Equation (42) iṡ where V is that of Equation (45), and V ev is expressed in Equation (44).

Appendix B.2. Second and Third Derivative of Evasive Velocity Field
The derivation of the second and third derivatives implies two successive derivatives of Equations (A13)-(A16). The second and third derivative of Equations (42) and (43) results in long equations of many terms. We will just present the second and third derivative of V in order to be more concise.
The second derivative of the velocity field is and the third derivative of this function is