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

Abstract

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 field 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 simplifies the implementation. Numerical results show that a correct tracking could be guaranteed.

1. 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 approach is that the robot reaches the velocity defined by the vector field, thus, in that instant the robot will be in the desired trajectory. Some advantages of this method compared to the traditional tracking control, according to [13], are:

• The velocity field error is a more appropriate way to determine whether the contour is being followed or not.
• The specification of the task, encoded as vector field, and the speed at which the task is performed can be decoupled.
• The coordination aspect of the task is explicit.

Masoud [1] mentions that the reference velocity generated with HPF is followed correctly with a massless, single integrator, holonomic system, but not with a more complex system like holonomic vehicles. In order to overcome this problem, he proposes the concept of virtual velocity attractor (VVA) to successfully execute any guidance instruction to drive an UAV closer to its goal. In our work, we propose using an asymptotically stable tracking control in order to track the velocity reference generated by the velocity field. This tracking control takes the velocity error and its derivatives to zero.

Autonomous vehicles like wheeled mobile robots or fixed wing UAVs have non-holonomic constraints; then, with this kind of vehicle the velocity reference is reached by controlling the steering angle, like in References [3,8]. In this paper we use a quad-rotor that can be controlled with decoupled dynamics for pitch and roll, thus, we decompose the velocity vector in x and y components, rather than magnitude of velocity and steering angle.

Recent works [14,15,16] deal with the problem of a UAV tracking a trajectory, like a line or a circular orbit, generated by the vector field method, considering uncertainties associated with modeling errors and compensating for wind uncertainty. Nevertheless, the proposed methods neglect the presence of obstacles. In this work, we propose a trajectory generation using a velocity field similar in the definition of the attractive and evasive field to the one presented in Reference [17]. One fundamental difference is that in Reference [17], the evasive field is added or overlapped, to the global velocity field through a weighted sum of the evasive function to any obstacle. Instead, we propose a strategy, in which each evasive field modifies the previous computed field recursively. Thus the successive derivatives of the desired velocity (references for tracking control) are computed over the last resulting field in a computational recursive way.

Once the velocity field is computed, the vehicle has to track the desired velocity. In order to accomplish a perfect tracking of the velocity field, we used a nested saturation controller [18]. This control law has been proved to be asymptotically stable in regulation and tracking control [19]. Patrikar et al. [20] used the nested saturation method for guidance of UAVs tracking in a circular or elliptical trajectory; however, their work does not considered obstacles avoidance. Tran et al. [21] used nested saturations for quad-rotor trajectory tracking, also neglecting obstacle avoidance. Tran mentions that for the tracking problem it is necessary to track the reference position and its time derivative up to the fourth order. Since the objective of the controller is to track the velocity reference, in our work, we omitted the last saturation term of the nested saturation controller, that corresponds to position, reducing the order of the controller with similar results. Thus demonstrating that, in the case of tracking a velocity reference, our proposed implementation of a nested saturation controller can be simpler than in the complete form, also reducing the computational cost. The tracking form of this controller acts over the velocity error and its successive derivatives; in this case, three successive derivatives of the velocity reference must be computed. We show how to carry out the time derivative of those variables.

This document is organized as follows: in Section 2 we describe the dynamic model of a quad-rotor aerial vehicle; in Section 3 the control strategy for tracking a velocity reference is developed; in Section 4 the velocity field generation is explained; in Section 5 the hydrodynamic theory is used to develop the velocity components which will help us to achieve the obstacle avoidance. The results are shown in Section 6, while conclusions are given in Section 7. Two Appendix A and Appendix B—are included to illustrate the steps required to derive the equations for successive derivatives of attractive and evasive fields.

2. 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 $\mathit{\xi }={(x,y,z)}^T\in {\mathbb{R}}^3$ denotes the quad-rotor center of mass position, relative to the inertial frame $\mathcal{I}$, and $\mathit{\eta }=(\varphi ,\theta ,\psi )\in {\mathbb{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:

$$\begin{array}{ccc}\hfill m\ddot{x}& =& u(c_{\psi }{s}_{\theta }{c}_{\varphi }+s_{\psi }{s}_{\varphi })\hfill \end{array}$$

(1)

$$\begin{array}{ccc}\hfill m\ddot{y}& =& -u(c_{\psi }{s}_{\varphi }-s_{\psi }{s}_{\theta }{c}_{\varphi })\hfill \end{array}$$

(2)

$$\begin{array}{ccc}\hfill m\ddot{z}& =& u{c}_{\theta }{c}_{\varphi }-mg\hfill \end{array}$$

(3)

$$\begin{array}{ccc}\hfill \ddot{\varphi }& =& \widetilde{{\tau }_{\varphi }}\hfill \end{array}$$

(4)

$$\begin{array}{ccc}\hfill \ddot{\theta }& =& \widetilde{{\tau }_{\theta }}\hfill \end{array}$$

(5)

$$\begin{array}{ccc}\hfill \ddot{\psi }& =& \widetilde{{\tau }_{\psi }},\hfill \end{array}$$

(6)

where m stands for the mass of the vehicle, g is the gravitational constant, $s_a=\sin a$, $c_a=\cos a$, and

$$\tilde{\mathit{\tau }}=\left[\begin{array}{c}\widetilde{{\tau }_{\varphi }}\\ \widetilde{{\tau }_{\theta }}\\ \widetilde{{\tau }_{\psi }}\end{array}\right]={\mathbb{J}}^{-1}(\mathit{\tau }-\mathit{C}(\mathit{\eta },\dot{\mathit{\eta }})\dot{\mathit{\eta }})$$

where $\tilde{\mathit{\tau }}$ is a vector with the quad-rotor torques, $\mathit{\tau }$ is a vector with the control inputs, $\mathbb{J}$ represents the inertial matrix and $\mathit{C}$ denotes the Coriolis matrix.

The inputs of force and torques, u and $\mathit{\tau }$, 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

$$u=f_1+f_2+f_3+f_4$$

with

$$f_i={\kappa }_T{\omega }_i^2,i=1,\dots ,4$$

where ${\kappa }_T>0$ is a constant coefficient and ${\omega }_i$ denotes the angular velocity of the i-th rotor.

The torque inputs $\mathit{\tau }={[{\tau }_{\varphi },{\tau }_{\theta },{\tau }_{\psi }]}^T$, are obtained through

$$\begin{array}{ccc}\hfill {\tau }_{\varphi }& =& [(f_1+f_4)-(f_2+f_3)]\ell \hfill \\ \hfill {\tau }_{\theta }& =& [(f_3+f_4)-(f_1+f_2)]\ell \hfill \\ \hfill {\tau }_{\psi }& =& {\tau }_{M_1}+{\tau }_{M_2}+{\tau }_{M_3}+{\tau }_{M_4},\hfill \end{array}$$

where ℓ is the distance from the rotors to the center of gravity of the quad-rotor, and ${\tau }_{M_i}$ denotes the gyroscopic torque produced by the motor $M_i$. The torques ${\tau }_{M_i}$ are obtained from

$${\tau }_{M_i}={\kappa }_Q{\omega }_i^2,i=1,\dots ,4$$

where ${\kappa }_Q>0$ is a constant coefficient. In general, ${\kappa }_T>>{\kappa }_Q$.

3. Control Strategy

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

3.1. Altitude and Yaw Control

Let us define ${\psi }_d=0\forall t>0$, such that the quad-rotor maintains a zero degree yaw angle during the flight. The control is obtained through

$${\tau }_{\psi }=-k_{p\psi }\tilde{\psi }-k_{v\psi }\dot{\psi },$$

(7)

where $\tilde{\psi }=(\psi -{\psi }_d)$, $k_{p\psi }$ and $k_{v\psi }$ 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

$$u=[(-k_{pz}\tilde{z}-k_{vz}\dot{z})+mg]\frac{1}{\cos \theta \cos \varphi },$$

(8)

where $\tilde{z}=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 ($\varphi ,\theta$) 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.

3.2. 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\to z_d$, and $\tilde{z}\to 0$, then, $\dot{z},\psi ,\dot{\psi }\to 0$ and the system (1)–(6) can be represented as:

$$\begin{array}{ccc}\hfill \ddot{x}& =& g\tan \theta ,\ddot{\theta }={\tilde{\tau }}_{\theta }\hfill \end{array}$$

(9)

$$\begin{array}{ccc}\hfill \ddot{y}& =& -g\frac{\tan \varphi }{\cos \theta },\ddot{\varphi }={\tilde{\tau }}_{\varphi },\hfill \end{array}$$

(10)

and considered to be restricted to small angles $\theta$ and $\varphi$, such that $\tan (\varphi )\approx \varphi$ and $\cos (\varphi )\approx 1$, and so for $\theta$, the system (9)–(10) can be seen as a reduced system:

$$\begin{array}{ccc}\hfill \ddot{x}& =& g\theta ,\ddot{\theta }={\tilde{\tau }}_{\theta }\hfill \end{array}$$

(11)

$$\begin{array}{ccc}\hfill \ddot{y}& =& -g\varphi ,\ddot{\varphi }={\tilde{\tau }}_{\varphi },\hfill \end{array}$$

(12)

where both subsystems are integrators in cascade

$$\frac{d}{dt}\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]=\left[\begin{array}{ccccccc}0& & 1& & 0& & 0\\ 0& & 0& & g& & 0\\ 0& & 0& & 0& & 1\\ 0& & 0& & 0& & 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]{\tilde{\tau }}_{\theta }$$

(13)

and

$$\frac{d}{dt}\left[\begin{array}{c}y\\ \dot{y}\\ \varphi \\ \dot{\varphi }\end{array}\right]=\left[\begin{array}{ccccccc}0& & 1& & 0& & 0\\ 0& & 0& & -g& & 0\\ 0& & 0& & 0& & 1\\ 0& & 0& & 0& & 0\end{array}\right]\left[\begin{array}{c}y\\ \dot{y}\\ \varphi \\ \dot{\varphi }\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]{\tilde{\tau }}_{\varphi }$$

(14)

Let us define the state variables of each subsystem as $\mathit{x}$ and $\mathit{y}$, thereby

$$\mathit{x}=\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]\mathrm{and}\mathit{y}=\left[\begin{array}{c}y\\ \dot{y}\\ \varphi \\ \dot{\varphi }\end{array}\right].$$

With the purpose to achieve the tracking of the trajectory, it is necessary to apply the control over the error vectors, this is

$${\mathit{x}}_{\mathit{e}}=\left[\begin{array}{c}{e}_x\\ e_{\dot{x}}\\ e_{\theta }\\ e_{\dot{\theta }}\end{array}\right]=\left[\begin{array}{c}x-x_d\\ \dot{x}-{\dot{x}}_d\\ \theta -{\theta }_d\\ \dot{\theta }-{\dot{\theta }}_d\end{array}\right],{\mathit{y}}_{\mathit{e}}=\left[\begin{array}{c}{e}_y\\ e_{\dot{y}}\\ e_{\varphi }\\ e_{\dot{\varphi }}\end{array}\right]=\left[\begin{array}{c}y-y_d\\ \dot{y}-{\dot{y}}_d\\ \varphi -{\varphi }_d\\ \dot{\varphi }-{\dot{\varphi }}_d\end{array}\right].$$

(15)

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

$${\mathit{x}}_{\mathit{e}}=\left[\begin{array}{c}0\\ e_{\dot{x}}\\ e_{\theta }\\ e_{\dot{\theta }}\end{array}\right]and{\mathit{y}}_{\mathit{e}}=\left[\begin{array}{c}0\\ e_{\dot{y}}\\ e_{\varphi }\\ e_{\dot{\varphi }}\end{array}\right].$$

(16)

Each subsystem can be represented in the form $\dot{\mathit{x}}={\mathit{A}}_x\mathit{x}+{\mathit{B}}_{x}u$, as in Equation (13), with state vector $\mathit{x}\in {\mathbb{R}}^n$ as in (16), $u\in \mathbb{R}$, then a linear transformation exist ${\mathit{z}}_x={\mathit{T}}_{zx}{\mathit{x}}_{\mathit{e}}$ that maps (13) in ${\dot{\mathit{z}}}_x={\mathit{A}}_z{\mathit{z}}_x+{\mathit{B}}_{z}u$, where

$${\mathit{A}}_z=\left[\begin{array}{ccccccccc}0& & k_2& & k_3& & \dots & & k_n\\ 0& & 0& & k_3& & \dots & & k_n\\ ⋮& & & & & & & & ⋮\\ 0& & 0& & 0& & \dots & & k_n\\ 0& & 0& & 0& & \dots & & 0\end{array}\right],{\mathit{B}}_z=\left[\begin{array}{c}1\\ 1\\ ⋮\\ 1\\ 1\end{array}\right],$$

where the elements $k_1,\dots ,k_n\ne 0\in \mathbb{R}$.

In this particular case the transformation matrix elements in ${\mathit{T}}_{zx}$ matrix are

$${\mathit{T}}_{zx}=\left[\begin{array}{cccc}{\displaystyle \frac{k_2{k}_3{k}_4}{g}}& {\displaystyle \frac{k_2{k}_3+k_2{k}_4+k_3{k}_4}{g}}& k_2+k_3+k_4& 1\\ 0& {\displaystyle \frac{k_3{k}_4}{g}}& k_3+k_4& 1\\ 0& 0& k_4& 1\\ 0& 0& 0& 1\end{array}\right]$$

with the vector ${\mathit{x}}_e$ of the Equation(16). Given that the state variable $e_x$ is set to 0, the element (1, 1) of matrix ${\mathit{T}}_{zx}$ can be 0, then

$${\mathit{T}}_{zx}=\left[\begin{array}{cccc}0& {\displaystyle \frac{k_2{k}_3+k_2{k}_4+k_3{k}_4}{g}}& k_2+k_3+k_4& 1\\ 0& {\displaystyle \frac{k_3{k}_4}{g}}& k_3+k_4& 1\\ 0& 0& k_4& 1\\ 0& 0& 0& 1\end{array}\right].$$

(17)

In a similar way ${\mathit{z}}_y={\mathit{T}}_{zy}{\mathit{y}}_e$, is defined ${\mathit{T}}_{zy}$ as:

$${\mathit{T}}_{zy}=\left[\begin{array}{cccc}0& -{\displaystyle \frac{k_2{k}_3+k_2{k}_4+k_3{k}_4}{g}}& k_2+k_3+k_4& 1\\ 0& -{\displaystyle \frac{k_3{k}_4}{g}}& k_3+k_4& 1\\ 0& 0& k_4& 1\\ 0& 0& 0& 1\end{array}\right]$$

(18)

with the vector ${\mathit{y}}_e$ given in Equation (16), where the control law according to the conventional nested saturation function, for the case of tracking trajectory is

$$u=x_d^{(n)}-{\sigma }_n(k_n{z}_n+{\sigma }_{n-1}(k_{n-1}{z}_{n-1}+\dots +{\sigma }_1(k_1{z}_1))),$$

with

$${\sigma }_n(s)=\left\{\begin{array}{cc}-b,\hfill & \mathrm{if}s<-b\hfill \\ s,\hfill & \mathrm{if}-b\le s\le b\hfill \\ b,\hfill & \mathrm{if}s>b\hfill \end{array}\right.,$$

where b is a positive constant; therefore, the controller for the pitch angle is

$${\tau }_{\theta }={\ddot{\theta }}_d-{\sigma }_4(k_4{z}_{4x}+{\sigma }_3(k_3{z}_{3x}+{\sigma }_2(k_2{z}_{2x}+{\sigma }_1(k_1{z}_{1x}))))$$

(19)

and for the roll angle is

$${\tau }_{\varphi }={\ddot{\varphi }}_d-{\sigma }_4(k_4{z}_{4y}+{\sigma }_3(k_3{z}_{3y}+{\sigma }_2(k_2{z}_{2y}+{\sigma }_1(k_1{z}_{1y}))))$$

(20)

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

$${\mathit{x}}_{\mathit{e}}=\left[\begin{array}{c}{e}_{\dot{x}}\\ e_{\theta }\\ e_{\dot{\theta }}\end{array}\right]\mathrm{and}{\mathit{y}}_{\mathit{e}}=\left[\begin{array}{c}{e}_{\dot{y}}\\ e_{\varphi }\\ e_{\dot{\varphi }}\end{array}\right].$$

(21)

With the new state error vectors (21) instead (16), it is possible to omit the state $z_1$ in the diffeomorphism ${\mathit{z}}_x={\mathit{T}}_{zx}{\mathit{x}}_e$, then ${\mathit{T}}_{zx}$ of Equation (17), and in the same way ${\mathit{T}}_{zy}$, could be reduced to a $3\times 3$ matrix

$${\mathit{T}}_{zx}=\left[\begin{array}{ccc}{\displaystyle \frac{k_3{k}_4}{g}}& k_3+k_4& 1\\ 0& k_4& 1\\ 0& 0& 1\end{array}\right].$$

(22)

Consequently, the saturation controller (19), could be reduced to only 3 saturators, as:

$${\tau }_{\theta }={\ddot{\theta }}_d-{\sigma }_4(k_4{z}_{4x}+{\sigma }_3(k_3{z}_{3x}+{\sigma }_2(k_2{z}_{2x})))$$

(23)

and for the roll angle:

$${\tau }_{\varphi }={\ddot{\varphi }}_d-{\sigma }_4(k_4{z}_{4y}+{\sigma }_3(k_3{z}_{3y}+{\sigma }_2(k_2{z}_{2y}))).$$

(24)

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 ${\sigma }_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 ${\sigma }_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\ne 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.

4. 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 ${\mathit{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_x,o_y)$, expressed by

$${(x-o_x)}^2+{(y-o_y)}^2=r_{tr}^2.$$

(25)

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

$$\min \left(\sqrt{{(x-x_{tr})}^2+{(y-y_{tr})}^2}\right),$$

(26)

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:

$$\overline{\mathit{\xi }}=\left[\begin{array}{c}\overline{x}\\ \overline{y}\end{array}\right]=\left[\begin{array}{c}x-o_x\\ y-o_y\end{array}\right],\tilde{\mathit{\xi }}=\left[\begin{array}{c}\tilde{x}\\ \tilde{y}\end{array}\right]=\left[\begin{array}{c}{x}_{cl}-x\\ y_{cl}-y\end{array}\right],$$

(27)

where $\overline{x},\overline{y}$ are the difference between any point of the workspace and the center of the circular trajectory, and $\tilde{x},\tilde{y}$ 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

$$\begin{array}{ccc}\hfill x_{cl}& =& o_x+r_{tr}\cos ({\alpha }_{cl})\hfill \end{array}$$

(28)

$$\begin{array}{ccc}\hfill y_{cl}& =& o_y+r_{tr}\sin ({\alpha }_{cl}),\hfill \end{array}$$

(29)

where ${\alpha }_{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

$$\parallel \tilde{\mathit{\xi }}\parallel =\sqrt{{\tilde{x}}^2+{\tilde{y}}^2}.$$

(30)

The angle ${\alpha }_{cl}$ is obtained from the derivative of the distance $\parallel \tilde{\mathit{\xi }}\parallel$ with respect to ${\alpha }_{cl}$, or $d\parallel \tilde{\mathit{\xi }}\parallel /d{\alpha }_{cl}=0$, and it is

$${\alpha }_{cl}=\mathrm{atan}2(\overline{y},\overline{x}).$$

(31)

The approaching field is defined as

$${\mathit{V}}_{ac}=\frac{\tilde{\mathit{\xi }}}{\Vert \tilde{\mathit{\xi }}\Vert }.$$

(32)

Let us denote the partial derivatives of $x_{cl}$ and $y_{cl}$ as $v_{x_c}$ and $v_{y_c}$.

$${\mathit{v}}_c=\left[\begin{array}{c}{v}_{x_c}\\ v_{y_c}\end{array}\right]=\left[\begin{array}{c}\partial x_{cl}/\partial {\alpha }_{cl}\\ \partial y_{cl}/\partial {\alpha }_{cl}\end{array}\right]=\left[\begin{array}{c}-r_{tr}\sin {\alpha }_{cl}\\ r_{tr}\cos {\alpha }_{cl}\end{array}\right].$$

(33)

These values are necessary to generate the tangential field as follows

$${\mathit{V}}_{tg}=\frac{{\mathit{v}}_c}{\Vert {\mathit{v}}_c\Vert }.$$

(34)

The velocity field is obtained performing the normalized weighted sum

$$\mathit{V}=\frac{F_1{\mathit{V}}_{ac}+F_2{\mathit{V}}_{tg}}{\Vert F_1{\mathit{V}}_{ac}+F_2{\mathit{V}}_{tg}\Vert },$$

(35)

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:

$$F_1=\frac{2}{1+e^{-\gamma \parallel \tilde{\mathit{\xi }}\parallel }}-1,F_2=1-F_1$$

(36)

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 $\gamma$ as the parameter that controls the weighting of both vectors. In Figure 2 is shown a velocity field generated with a value $\gamma =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 ${\mathit{x}}_e$ and ${\mathit{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:

$$\left[\begin{array}{c}{\dot{x}}_d\\ {\dot{y}}_d\end{array}\right]=K_v\mathit{V}=\left[\begin{array}{c}{K}_v{V}_x\\ K_v{V}_y\end{array}\right],$$

(37)

with these desired values, the velocity error variables $e_{\dot{x}}=\dot{x}-{\dot{x}}_d$ and $e_{\dot{y}}=\dot{y}-{\dot{y}}_d$ can be computed. However, in order to compute the remaining error variables of Equation (21), are still necessary the desired values: ${\theta }_d,{\dot{\theta }}_d,{\varphi }_d$ and ${\dot{\varphi }}_d$, even more, the tracking controllers (23) and (24) need the desired variables ${\ddot{\theta }}_d$ and ${\ddot{\varphi }}_d$. From Equations (13) and (14) we have that $\ddot{x}=g\theta$ and $\ddot{y}=-g\theta$, so $\theta =\ddot{x}/g$ and $\varphi =-\ddot{y}/g$, then

$$\left[\begin{array}{c}{\dot{x}}_d\\ {\theta }_d\\ {\dot{\theta }}_d\\ {\ddot{\theta }}_d\end{array}\right]=\left[\begin{array}{c}{\dot{x}}_d\\ {\ddot{x}}_d/g\\ {\stackrel{⃛}{x}}_d/g\\ {\stackrel{⃜}{x}}_d/g\end{array}\right]=\left[\begin{array}{c}{K}_v{V}_x\\ K_v{\dot{V}}_x/g\\ K_v{\ddot{V}}_x/g\\ K_v{\stackrel{⃛}{V}}_x/g\end{array}\right]$$

(38)

and

$$\left[\begin{array}{c}{\dot{y}}_d\\ {\varphi }_d\\ {\dot{\varphi }}_d\\ {\ddot{\varphi }}_d\end{array}\right]=\left[\begin{array}{c}{K}_v{V}_y\\ -K_v{\dot{V}}_y/g\\ -K_v{\ddot{V}}_y/g\\ -K_v{\stackrel{⃛}{V}}_y/g\end{array}\right].$$

(39)

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 $\mathit{V}$.

5. 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

$$\mathit{v}(\mathit{p})=\left[\begin{array}{c}u(x,y)\\ v(x,y)\end{array}\right]$$

in the point

$$\mathit{p}=\left[\begin{array}{c}x\\ y\end{array}\right]\in \Omega .$$

Here $\Omega \subset {\mathbb{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

$$\nabla ·\mathit{v}=\frac{\partial u}{\partial x}+\frac{\partial v}{\partial y}=0,\nabla \times \mathit{v}=\frac{\partial v}{\partial x}-\frac{\partial u}{\partial y}=0.$$

Theorem 1.

The velocity vector field $\mathit{v}={[u(x,y),v(x,y)]}^T$ induces a flux of ideal fluid if and only if

$$f(z)=u(x,y)-\mathrm{i}v(x,y)$$

(40)

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

$$\chi (z)=\Phi (x,y)+\mathrm{i}\Psi (x,y) \mathrm{that} \mathrm{satisfies} \frac{\mathrm{d}\chi }{\mathrm{d}z}=f(z).$$

Using the Cauchy-Riemann equations for the complex derivative,

$$\frac{\mathrm{d}\chi }{\mathrm{d}z}=\frac{\partial \Phi }{\partial x}-\mathrm{i}\frac{\partial \Phi }{\partial y}=u-\mathrm{i}v,\mathrm{then}\frac{\partial \Phi }{\partial x}=u,\frac{\partial \Phi }{\partial y}=v.$$

Thus, $\nabla \Phi =\mathit{v}$, and therefore the real part $\Phi (x,y)$ of the complex function $\chi (z)$ defines the velocity potential. Consequently, $\chi (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, $\nabla \Phi =\mathit{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

$${\tilde{\mathit{\xi }}}_o=\left[\begin{array}{c}x-x_o\\ y-y_o\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o\\ {\tilde{y}}_o\end{array}\right].$$

The proposed velocity potential function for an ideal fluid through a circular obstacle is

$$\Phi ({\tilde{\mathit{\xi }}}_o)=\left(1+\frac{r_o^2}{{\tilde{x}}_o^2+{\tilde{y}}_o^2}\right)({\tilde{x}}_o\cos \beta +{\tilde{y}}_o\sin \beta ),$$

where

$$\beta =\mathrm{atan}2(V_y,V_x)$$

(41)

is the angle of the velocity field vector in that point. The flux velocity around a cylinder is defined as the gradient of the $\Phi$ function, namely $\mathit{v}=\nabla \Phi$.

Then, the x-component of the velocity vector is

$$V_{e{v}_x}=\frac{-2r_o^2}{{({\tilde{x}}_o^2+{\tilde{y}}_o^2)}^2}({\tilde{x}}_{o}c\beta +{\tilde{y}}_{o}s\beta ){\tilde{x}}_o+\left(1+\frac{r_o^2}{{\tilde{x}}_o^2+{\tilde{y}}_o^2}\right)c\beta ,$$

(42)

where $c\beta =\cos \beta$ and $s\beta =\sin \beta$. The velocity function in the y-coordinate is

$$V_{e{v}_y}=\frac{-2r_o^2}{{({\tilde{x}}_o^2+{\tilde{y}}_o^2)}^2}({\tilde{x}}_{o}c\beta +{\tilde{y}}_{o}s\beta ){\tilde{y}}_o+\left(1+\frac{r_o^2}{{\tilde{x}}_o^2+{\tilde{y}}_o^2}\right)s\beta .$$

(43)

Therefore, the velocity vector for the evasive maneuver is

$${\mathit{V}}_{ev}=\left[\begin{array}{c}{V}_{e{v}_x}\\ V_{e{v}_y}\end{array}\right].$$

(44)

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, ${\mathit{V}}_{ev}$, takes the place of the previous velocity field, however, it could be also normalized as

$$\mathit{V}=\frac{{\mathit{V}}_{ev}}{\parallel {\mathit{V}}_{ev}\parallel }.$$

(45)

Appendix B illustrates the procedure to obtain the successive derivatives of the velocity reference $\mathit{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 $\mathit{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, $\mathit{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.

6. Results

6.1. Numerical Results

The first simulation consists only of the approaching velocity field of Equation (35). The vehicle begins at position $\mathit{p}={[0,0]}^T$. In

Table 1. Parameters and their corresponding values used in the simulation.

Parameter	Value
$b_2$	0.2
$b_3$	0.4
$b_4$	0.8
$k_2$	2
$k_3$	2
$k_4$	2
$K_v$	2

are shown the values of the controller parameters for simulation.

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 $\parallel \tilde{\mathit{\xi }}\parallel$ of Equation (27), and the other one for the speed error, this is $\parallel {[e_{\dot{x}},e_{\dot{y}}]}^T\parallel$, where $e_{\dot{x}}$ and $e_{\dot{y}}$ 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).

6.2. Obstacle Avoidance Strategy

Figure 7 depicts the trajectory followed by the vehicle in the presence of three obstacles. The “×” symbol shows the initial position. It is shown how the vehicle avoids the obstacles guided by the evasive velocity field.

Figure 8 illustrates two plots—one for the position error, this is $\parallel \tilde{\mathit{\xi }}\parallel$ of Equation (27), and the other one for the speed error, this is $\parallel {[e_{\dot{x}},e_{\dot{y}}]}^T\parallel$. It can be seen that the vehicle reaches the velocity field and the desired trajectory almost at the same time of the simulation without obstacles, however, in this case, it could be seen two little humps in the speed error plot. The greater hump (up to 0.03 m/s) occurs around 16.5 s and the other one around 18 s; this occurs after the evasion of the second obstacle.

6.3. 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.

7. 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.