A Path-Following Controller for a UAV-UGV Formation Performing the Final Step of Last-Mile-Delivery

This work discusses the accomplishment of a path-following task by a formation of an unmanned aerial vehicle (UAV) and an unmanned ground vehicle (UGV). The idea behind such an application is that the UAV is returning after delivering a package, and should land on the UGV, its reference base, which continues following its route towards another delivery. A high level controller based on the virtual structure paradigm is designed to guide the formation, which also includes an obstacle avoidance algorithm to handle the possibility that the UGV, the reference for the formation, faces an obstacle in its path, such as another vehicle, for instance. After describing in details the controller designed and the obstacle avoidance algorithm, simulated and experimental results are shown and discussed, which validate the proposed controller.


I. INTRODUCTION
The use of unmanned aerial vehicles (UAVs) in tasks such as load transportation or manipulation has become a reality [1]. A single UAV or a group of them working cooperatively are options to accomplish load transportation/manipalation tasks [2]- [7]. In such a context, one activity that has received great attention is the so-called last-mile delivery [8], [9], which involves UAVs/drones delivering packages [10], and taking-off/landing from/on trucks navigating at a street network [9], [11]. Besides last-mile delivery, UGV-UAV formations have also been a feasible solution in warehouse inventory [12] and mapping using multiple robots [13], [14] (this last case calls attention to the recent news about Mars The associate editor coordinating the review of this manuscript and approving it for publication was Heng Wang . exploration, with a drone taking-off from a rover and landing back on it after acquiring images from their surroundings 1 ).
As many countries are now approving regulations that opens the possibility of using small drones for package delivery, the last-mile delivery problem using autonomous navigation of UGV/UAVs has become an important research topic [9], [11], [15]. However, to accomplish such kind of task a high level of independence is necessary. For instance, when navigating a UGV-UAV formation should be able to react to obstacles in its surroundings, in order to avoid colliding with them. To do that autonomously, a controller to guide the formation along navigation is required, which should also allow the formation to take a new and safe path to avoid any obstacle appearing in the original path. This is the case in this work, which emulates the last step of a package delivery application, when the UAV should land back in a terrestrial moving platform (a small truck, for instance), after delivering a package. To be more specific, the step right before the landing of the UAV is considered, assuming that only the UGV faces obstacles, such as stopped or slow cars, for instance, in that circumstance. In this sense, the contribution of the paper is to show the feasibility of using the paradigm of virtual structure [16] to guide the UAV to land on a static or moving platform, which corresponds to the last step of the last-mile-delivery problem, i.e., the landing of the UAV on the UGV.
Regarding the virtual structure, it has become consolidated in the literature as a control approach for multi-robot systems, with many results already reported, adopting from ground robots [17] to surface vessels [18]. In most cases, however, homogeneous formations are considered. Some results regarding UAV-UGV formations have already been reported [11], [19], [20], most times considering positioning and/or trajectory-tracking tasks. However, an important aspect, when regarding UAV-UGV teams in package delivery applications, is that the ground vehicle is affected by constraints in road/avenues/streets, such as velocity limits, red lights and the presence of other cars, thus demanding the capability of modulating its velocity, an thus the velocity of the UAV, accordingly. To guarantee this possibility, this work adopts path-following as the motion control strategy, because it allows the modulation of the velocity of the formation according to the environment, which is not possible when regarding trajectory-tracking, for instance. Therefore, this paper addresses the navigation of a UGV-UAV formation (an heterogeneous one) when following a predefined path, considering that the UAV should land on the UGV, which can be stopped or moving. The objective is to control a virtual structure associated to the formation, thus coordinating the movement of the two robots to guarantee that the UAV gets close enough to the UGV to land on it while the ground vehicle keeps moving. In addition, an obstacle avoidance approach, based on reactive local path-planning, is embedded in the proposed controller, to allow the formation to avoid obstacles in the ground. The premise is that the UGV, which coincides with the virtual robot characterizing the formation, has no previous information about obstacles, and scans its surroundings for threats. Whenever an obstacle is found, a new safe path is established around the obstacle and the UGV temporarily leaves the planned path, going ahead along a new safe path until leaving the obstacle behind. Then, as the navigation along the original path becomes safe again, the UGV resumes its original path. To implement such an obstacle avoidance strategy, an algorithm to deal with a single obstacle is designed, which is then extended to handle multiple obstacles.
Many approaches can be adopted to allow obstacle avoidance in mobile robotics. The simplest of them is to build a map of the environment, mark the positions of the obstacles and use a path-planning graph algorithm, such as the traditional Dijkstra algorithm [21], A* algorithm [22], and Bellman-Ford algorithm [23], [24], or even a more recent one, such as the one proposed in [25], based on genetic algorithm, just to mention some, in order to establish a free-of-obstacles path to be followed. Unfortunately, graph algorithms require high computational effort, not available in small vehicles. Furthermore, in many cases the robots do not have full knowledge of the environment, or the environment can change after the map building. A solution often used is to combine global, usually slow and of low-fidelity, and local, fast and of highfidelity, planning, such as in the framework proposed in [26].
Other common approaches to implement obstacle avoidance are artificial potential methods [27]- [29], with its elegant mathematical analysis, neuro-fuzzy methods [30], [31] and genetic algorithms [25], [32]. However, a potential drawback of artificial potential and genetic algorithm methods are their aptness to get the robot trapped in local minima when navigating in environments with close obstacles or narrow channels [33]. To overcome such limitations, this work has chosen to focus on a reactive path planning approach, considering that the UGV has no prior knowledge about the presence of obstacles in its path. Notice that in this case, considering the application addressed, only obstacles in the ground are considered, since the UAV flies close to the UGV, thus being free of most aerial obstacles.
Regarding path-following controllers, several works have been published, some of them in the last five years. In [34] and [35], for instance, H ∞ robust control technique is adopted to control a single ground vehicle and a vessel, respectively, when following a path. In the second case, in addition to the H ∞ robust control technique, adaptive output feedback linearization is also used. By their turn, in [36] and [37], both considering autonomous underwater vehicles, the techniques of backstepping (associated to a parameter update law neural networks) and model predictive control are adopted. Regarding path following control for multi-robot systems, [38] proposes a controller based on inverse kinematics for a group of ground vehicles, whereas [39] proposes a robust moving path following controller, including a first order sliding mode term, applied to a group of marine vessels. Other less recent articles on path-following control are available in the literature, but to the extent of the authors knowledge this is the first time a path-following controller is designed to guide a UAV-UGV formation in the last-mile-delivery problem.
The algorithm here designed to allow the UGV, and thus the whole formation, to avoid obstacles in the ground has the advantage of using only relatively simple linear algebra and demanding low computational effort, similarly to the proposal of [40]. There, a robot navigates in an environment with unknown obstacles and, as the obstacles are detected, it defines new safe paths around them. However, in spite of being quite robust, such a strategy involves both offline and online calculations, and a somewhat complex implementation, with final results quite similar to those obtained here. Complexity in terms of implementation is also an issue for the method proposed in [26], although allowing very fast navigation, theoretically speaking. Other similar work is the one reported in [41], which deals with path-following by a VOLUME 9, 2021 formation. However, it adopts a linear controller, which is too dependent on the operation point around which linearization was performed. Besides this, its collision avoidance is focused on intra-formation collision, which is not an issue here, due to the nature of the formation robots (the UAV does not fly so low as to collide with the ground vehicle).
To discuss the topics involved in our proposal, the paper is hereinafter split in four sections, starting with Section II, which characterizes the formation used in this work as well as discusses the path following controller designed to guide the formation while following the planned path, and Section III, which describes in detail the obstacle avoidance algorithm proposed in connection with the path-following controller. In the sequel, Section IV shows the results of some simulations and a real experiment run using the unicycle mobile platform Pioneer 3-DX as the UGV of the formation and the quadrotor Bebop 2 as the UAV composing the formation. Finally, Section V highlights the main conclusions of the work.

II. THE PROPOSED PATH-FOLLOWING CONTROLLER
In this section, the virtual structure correspondent to the formation is characterized and described. The design of a path-following controller to ensure the accomplishment of the navigation is also discussed, as well as its main components and some auxiliary modules, such as a dynamic compensator adopted to reduce the velocity-tracking error for the UAV.

A. FORMATION CHARACTERIZATION
The characterization of the UGV-UAV formation considered in this work is based on the paradigm of virtual structure [11], [16], [42], where the virtual structure is the 3D straight line linking the two vehicles. According to such a paradigm, a virtual robot is put on a reference point in the geometric element characterizing the formation, which can be its center of gravity, such as in [17], [43], and [38]. For the UGV-UAV formation this would correspond to the point in the middle of the line linking the two vehicles. However, taking into account that the objective here is to propose a control system that allows the UAV to land on the UGV, the point chosen to place the virtual robot was the extremity of the virtual line defining the virtual structure, which coincides with the UGV, such as in [11]. Therefore, the formation is characterized as shown in Fig. 1, with the virtual robot associated to the formation being the Pioneer 3-DX robot itself.
The variables describing the formation are also shown in  robot coincides with the UGV position, so x f y f z f T = x 1 y 1 z 1 T . Thus, two different spaces are defined, the formation space (the cluster space in [17] and in [38]) and the robots space, which are connected by two transformations: a direct one, allowing getting q from x, characterized as and an inverse one, allowing getting x from q, characterized as An important remark is that when considering the position of the UGV we are considering a value z 1 , which is always zero in this paper. To consider the variable z 1 , however, allows using this description for a formation of two UAVs with no modifications, for instance, which led to the decision to keep the position of the UGV as a 3D position.

B. THE PATH-FOLLOWING CONTROLLER
The goal of a path following task is to reach and follow a predetermined path with no time constraint. A path is defined by a parametric function c(s) = (x(s), y(s), z(s)) ∈ R 3 , where s is the curvilinear abscissa. Notice that as the formation in this paper is based on the UGV, z(s) will always be constant and equal to zero. In addition, once defined the path is static with respect to time. However, the control layer still has to determine the point of the path that is closest to the robot and its distance, as this result will influence the behavior of the controller, since the position of the robot with respect to the path is time-dependent, due to the movement of the robot.
As mentioned, the path is not time dependent, so its definition can be done outside of the control loop, which corresponds to the Path Parametrization Layer of Fig. 2, where a flowchart of the whole control system adopted is shown.
The point of the path that is closest to the robot is defined as c n = c n x c n y c n z T , which is calculated in each iteration of the control loop. The distance between c n and x f y f z f whereas t is a unit vector tangent to the path at c n , whose orientation, in the inertial reference system, is for x(s) = c n x and y(s) = c n y . The control law for the formation, similar to the one adopted in [11], is given bẏ where L 1 and L 2 are diagonal positive definite saturation and gain matrices, respectively,q d represents the desired velocities, andq = q d − q corresponds to the formation errors. However, in [11] this controller was used to guide the formation in a trajectory-tracking task, whereas its usage in this work is to guide the formation in a path-following task.
In order to do that, an approach based on [38] was implemented. Therefore, the desired velocity for the virtual mass-less robot associated to the point of control of the formation (which coincides with the point of control of the UGV) is not defined as the time derivative of the path, which is null once the path has no time dependency. Instead, it is defined as a desired value tangent to the path, or Note that z f d has been omitted, as the virtual robot always navigates on the XY -plane (where z = 0). The formation variables ρ f d α f d β f d T have also been omitted, as they bear no influence on the path-following strategy. As for V e , it is the reference velocity input for the path-following controller, and is defined as where e c is a threshold value selected for the position error, and V d is the desired velocity of the virtual robot along the path.
In the first case, when the distance between the robot and the nearest point of the path is greater than the error e c , V e is equal to zero, which makesẋ f d andẏ f d also equal to zero, which means that the path-following task of the virtual robot associated to the formation is changed to a positioning task, with the virtual robot going to or coming back to the path. On the other hand, as the virtual robot approaches c n , the distance errorñ will eventually be less than e c , entering in the second case, where V e = V d . The virtual robot will then have a desired velocity tangential to the path, but the position error is still greater than zero, which means that there is still a residual value in the second term of the control law. This ensures that the virtual robot will converge to c n in a smooth curve, almost tangential to the path. Ultimately, the position of the virtual robot will be equal to c n , meaning that it is following the path with the desired speed V d .
Finally, as V d can be freely set regardless of the path configuration, some considerations can be made as to its feasibility. For instance, an acceptable navigation speed on a straight line or smooth curve might be too large for a sharp curve. Thus, a slightly modified version of a curvature strategy presented in [15] was implemented, consisting in reducing the velocity V d when the angle of the tangent to the path is above a certain threshold (cautious navigation). This intends to mimic the way a human driver behaves, and allows for a higher velocity to be set for the task than it would be possible if the velocity had to remain constant along the whole path.
In order to estimate such an angle, the angular velocity of the UGV,ψ 1 , is considered. Thus, the strategy consists in considering instead of a constant V d , which means that the velocity along the path decreases to follow a sharp curve. In such equation L c is a gain constant to adjust the sensibility of the described strategy.
To summarize, after defining the path to be followed and tuning the controller, the velocity V d max should be defined, which is actually the maximum velocity along the path, not a constant velocity along the path. However, the velocity along the path can be constant and equal V d max , provided that the angular velocity of the UGV does not exceed the threshold established (the path is a smooth one).
Going ahead in Fig. 2, even though the reference signals in the formation space have already been determined by (5), it is still necessary to calculate the control signals in the robots space. This is accomplished through the Jacobean of the inverse transformation in (2), which is defined as which, in turn, results in where c x and s x stand for cos(x) and sin(x), respectively. The transformation from the formation space to the robots space, when regarding the velocities the formation controller propagates to the robots, is defined as the time derivative of (2), which is given bẏ whereẋ r = ẋ 1 rẏ 1 rż 1 rẋ 2 rẏ 2 rż 2 r T , is the formation controller reference in the robots space with respect to the reference inertial system. However, the commands sent to the robot must be in their own reference system, which is done by taking where K −1 is a block diagonal matrix for which each block of the diagonal is the inverse Kinematics matrix of each one of the two robots in the formation. Thus, K −1 is defined as As for v r , it is the velocity reference generated by the formation controller in the robots own system, defined as where v r 1 is a vector representing the velocity reference for the first robot (UGV) along the three axis, and v r 2 represents the same for the second robot (UAV), in their own inertial reference systems. The controller so far considered has been based on a mass-less virtual robot and only kinematic effects have been taken into account. Initial experiments showed that the UGV presented good results navigating solely with the kinematic controller, whereas the UAV performed poorly. To improve the performance of the UAV when navigating in formation with the UGV, a dynamic compensation module, as proposed in [44], was added to consider the dynamic effects associated with the robot.
The model used by the compensator, proposed in [45], is which has been shown to be an acceptable approximation for low values of pitch and roll angles. In such a model the vector x stands for the accelerations on the global frame, whereasẋ is the vector of velocities in the frame of the vehicle. The compensator, based in [46], is then defined as where v D 2 is the output of the dynamic compensator, Such a compensator changes the dynamics of the velocity error to an asymptotically stable linear one (feedback linearization control technique [47]), guaranteeing that the error between the velocities v r 2 (the desired one) and v 2 converges to zero asymptotically. A more detailed explanation of the model and how its parameters for the present vehicle were found can be seen in [46].
At last, an orientation controller for the UAV was added in order to make it face the same direction of the UGV. This has no immediate impact on the navigation, and is done for the sole reason of future works using the camera on the front of the flying vehicle. The complete details of the controller implemented are in [15], where further discussion about it is available.

III. OBSTACLE AVOIDANCE ALGORITHM
This section discusses the algorithm developed in this work to allow the UAV-UGV formation to overcome obstacles found during the path-following task guided by the controller proposed. As a first step it will be discussed the avoidance procedure for a single obstacle. In the sequel the algorithm will be extended to handle multiple obstacles. In this last case, however, if the multiple obstacles are distributed in such a way that the formation can resume the original path being followed after overcoming an obstacle, then they can be dealt with as several single obstacles. Therefore, the algorithm for multiple obstacles is used just when the obstacles are close to each other. Furthermore, as in this work the obstacles are ground-based, the analysis will be focused on the virtual robot that characterizes the formation (remember that the formation control paradigm adopted is the one based on virtual structures [16], for which the entire formation is dealt with as a single virtual robot), which, for the formation considered, coincides with the UGV.

A. AVOIDING A SINGLE OBSTACLE
When navigating along a path, a robot may face an obstacle blocking its way so that if the robot continues on its planned route a collision will occur. In such a situation, it is desirable that the robot leave its original path and follow an alternative safe path, until it is safe to resume the original path.
The goal of the method presented here is to be a simple solution easily understood and implemented for this problem. As such, it is an algorithm that employs simple geometry in order to determine safe paths around the obstacle, and uses the same path-following controller discussed in Section II-B to navigate along this new path. Thus, the implemented strategy is an online path planning, not a controller-based one.
The proposed algorithm assumes that the position of each obstacle in the world is known as long as it has been detected by the robot. The zone of detection is a radius centered on the robot, whose position is given by x f , the position of the formation, and has the value of r det . Additionally, each obstacle will be modeled as a circumference with a radius large enough to encompass all of its physical boundaries and a safety zone, which will allow enough space for the body of the robot to navigate. This circumference is called the avoidance zone, and its radius is defined as r avd . It is assumed that r det is larger than r avd for all obstacles, as illustrated in Fig. 3. Once the robot detects an obstacle, two special points in the original path c o (s) are defined. The first one is the first point in the path that intersects the circumference of radius r avd centered on the center of the circumference that describes the obstacle, defined as x o i (for the i-th obstacle), whereas the second one is the second point of the original path which is over the same circumference. Notice in Fig. 3 that this means that the path being followed by the robot passes inside the safety zone of the obstacle, thus demanding a maneuver to avoid such obstacle. These two points are called c avd f i and c avd l i , respectively. If none of such points exist, i.e., the obstacle is close enough to be detected by r det , but far enough such that r avd does not touch the path, then the obstacle is not considered to be a threat to navigation and no action is taken. Furthermore, some approximations may be made in determining c avd f i and c avd l i depending on the resolution of the path, but common resolutions used are enough to ensure that such approximations have little impact.
As the distance between x o i and c avd f i is r avd , and likewise for x o i and c avd l i , they fall exactly on the border of the circumference defined by the radius r avd and centered in x o i . The strategy adopted, then, consists in guiding the robot to temporarily abandon its original path c o (s) and navigate tangentially to this circumference, thus preventing any collision with the obstacle. Moreover, as c avd f i and c avd l i both belong to c o (s), the continuity between the paths is assured, i.e., the robot will meet its new path by following the original one, and once the avoidance maneuver is over, it will return to c o (s) only after leaving the obstacle behind.
Of course, the robot might choose any direction, left or right, to navigate the new path. This choice could be random or follow any other criteria, such as the direction that the robot currently faces or the shorter path, for instance. For this work, the shorter path criterion was selected, and the new path c avd i (s), when regarding the example depicted in Fig. 3, is the one marked in the figure.
While at first it might seem a simple problem to determine c avd i (s) function as a circumference arc, this is somewhat complex because c avd f i , x o i and c avd l i might be in any position and quadrant in the world reference frame W f . This leads to a complicated quadrant analysis of how to determine the curvilinear abscissa for the circumference arc. This is further compounded by the discontinuity present in the region of [−π, π].
In order to tackle this problem, a new reference frame O f is defined. The origin of this new frame is the center of the circumference describing the obstacle (x o i ) and its x-axis points in the same direction of the line that connects c avd f i and x o i . This is accomplished through the homogeneous transformation matrix. Thus, the matrix H w o represents the position and orientation of the new frame O f with respect to the world frame W f , and is written as where and ψ o is the angle that x o i makes with the reference to c avd f i , defined as However, to transform points in the world reference frame W f to their corresponding points in the obstacle reference frame O f , the inverse transformation H o w is needed. Fortunately, due to the Homogeneous Transformation properties it is possible to state that  and thus f , is right on top of the discontinuity, which means that the arc function will never pass through it, but rather always begin on it. Which value ψ o f will assume, π or −π, will depend on the direction desired for the arc, what will be explored shortly. Fig. 4 shows the situation presented in Fig. 3 where and inc is the increment of the function and is dependent on the resolution one wants for the path. This condition ensures that, if ψ o l is negative, as in Fig. 4, the robot will follow the bottom arc, and if the angle is positive the robot will follow the top arc, thus always following the shorter path. Following this, the function that defines the shorter arc is where the z-coordinates are zero because the function is defined on the xy-plane. The function defined, however, is with respect to the O f frame. In order to establish this function in the world coordinates, where our navigation is taking place, the inverse homogeneous transformation is applied, resulting in Thus, as the robot encounters an obstacle when following its original path, the intersection of the path with the avoidance zone of the obstacle is determined. A new avoidance path will then be calculated once by the algorithm, beginning on the original path and following along the limits of the avoidance zone, on the shortest path available. When the avoidance path ends, it connects with the original path, and only when the robot reaches this point the obstacle is considered as left behind, and the robot resumes its navigation following the original path.

B. AVOIDING MULTIPLE OBSTACLES
The algorithm described in the last section covers all the cases of single obstacles that the robot may find in its navigation along a path defined in the real world. However, what would happen if the robot faced a new obstacle during its navigation through the temporary path generated to avoid a previous obstacle? In the last section, one of the parameters used was c avd f i , which provided continuity between the original path and the new one. However, now that the robot is not on the path, it makes no sense to use c avd f i . Thus, a slightly modified version of the algorithm of the previous subsection is applied when the robot faces a new obstacle whilst avoiding a previous one. Notice that this is a real possibility when there are multiple obstacles in the navigation environment.
It will be assumed that the robot is facing a new obstacle x o i+1 when the distance between the robot, whose current position is x f , and the center of the circumference characterizing the new obstacle, x o i+1 , becomes smaller than the distance between x f and x o i , the center of the circumference characterizing the previous obstacle. Then, new parameters will be calculated and a procedure similar to the previous one will be run. Fig. 5 illustrates the situation and the relevant parameters, considering two close obstacles. As previously mentioned, if the two obstacles were not close to one another, they would be dealt with as single obstacles.
The detection region encompassed by r det has been omitted for clarity. Another detail about the figure is that the robot has not yet detected the second obstacle when it starts avoiding the first one. Indeed, it will detect the second obstacle after entering the temporary path adopted to avoid the first obstacle, characterized as c avd i (s). Moreover, the points c avd f i+1 and c avd l i+1 have been determined in a way similar to the one in the previous subsection, with the important difference that c avd f i+1 is determined over the path the robot is currently following, i.e., c avd i (s), and not over the original path c o (s). The goal is then to determine c avd i+1 (s), which will be the new safe path that the robot will follow, as c avd i (s) has been compromised due to the new obstacle.
However, as one can see in Fig. 5, were the algorithm for multiple obstacles to use the same criterion previously adopted, the shortest path, the arc chosen would take the robot towards the previous obstacle. This means that it is not guaranteed that the algorithm should always choose the shortest path. It can take the largest one, as illustrated in the figure. As one can easily verify, if the second obstacle were moved upwards and placed horizontally above the first one, then the robot would really take the shortest path when avoiding the two obstacles. It is also easy to verify that one path (short or long) will always lead the robot back to the previous obstacle, while the other will be safe for it.
In order to solve the problem of which arc should the robot take, a new criterion was adopted. After following the same procedure shown in the last subsection the algorithm calculates the shortest arc function, now identified as c avd s i+1 (s), with the superscript s on top of avd to identify it as the short path.
To calculate the longer arc, the analysis remain the same, with the difference that now the curvilinear abscissa will be the complement of the previous one. This means that, for the long arc (24) remains the same, but (25) will be modified to and After defining both, the short and the long paths, the algorithm will calculate the distance of each of their points to the previous obstacle x o i , and the minimum distance of each path will be identified as d min s and d min l , for the short and long paths, respectively. Then, the new safe path will be defined as In other words, the path chosen will be the one farthest from the previous obstacle.
Furthermore, although this discussion focused on two obstacles, whenever the robot finds a new obstacle on its way when avoiding a previously detected obstacle, this algorithm can be applied again. The difference is that it is now taking into account x o i+1 and x o i+2 , up until n obstacles. Cases with more than two obstacles will be presented in the simulations and experiments discussed ahead.

IV. RESULTS
In order to test whether the proposed algorithm is a valid solution to the obstacle avoidance problem, simulations were run. After successful simulations, real world experiments were performed to further check the efficiency of the algorithm and also its robustness. This section presents and discusses the results obtained in the simulations and the real experiments.

A. SIMULATIONS
The simulations goal is to validate the strategy proposed to accomplish obstacle avoidance. To achieve this, eight scenarios were devised to show the main challenges and variations of obstacles that the formation may face. The desired velocity of the formation in each case was V d max = 0.4 m/s with the gains tuned to L 1 = diag 1.
The desired formation in given by for whatever path c(s) that is currently being followed in each scenario.
As for the obstacle avoidance algorithm parameters, the radii of detection and of the avoidance zone were defined as r det = 1.2 m and r avd = 0.8 m. One should notice, however, that a zone of avoidance should be tied to each obstacle and that it might be different when regarding different obstacles. Although this is effectively true and easy enough to be handled by the algorithm as long as all r avd are smaller than r det , for the sake of simplifying the discussion it was deemed better to use a single value for all obstacles.
The scenarios devised are shown in Fig. 6. The first column depicts the cases where the formation only encounters one obstacle at time, i.e., even in the cases where there are multiple obstacles in the navigation, they are far enough apart that they may be treated separately. These cases will be discussed first, and then the second column, where the formation face multiple obstacles at a time, will be discussed after.
The first figure shows the formation facing just a single obstacle, represented by the red circle, when following a straight path along the x axis. The UGV starts from a point on top of the desired path whereas the UAV starts from a point at the UGVs right. They quickly converge to the desired formation, avoid the obstacle while maintaining the formation, and reach the end of the path.
It is readily apparent that the avoidance path is the shortest arc around the obstacle, as the center of the obstacle is below the path (y o is a negative number), and the formation navigates through positive values of y. In addition, while it may seem that the value of r avd is unnecessarily large for the size of the obstacle, one has to keep in mind that only the control point of the UGV is being shown. In reality the robot has a non-zero size and it has to be taken into account, so the safe zone is made large enough to allow the whole body of the robot to navigate (this will become clearer in the experiments).
The second scenario is much like the first, except now there are two obstacles, one below and one above the path. Again, the algorithm is shown to yield the shortest arc of circumference around each obstacle for the formation to continue following the path. In other words, for the obstacle that is below the path the formation navigates above it, and for the obstacle that is above the path, the formation navigates below it.
The third scenario shows a more complex path where the algorithm has to determine which is the shortest path around the obstacles. The very first obstacle is an interesting case: at a first glance it would be better for the formation to steer to the left (in relation to where the formation is currently facing, which is downwards) as the obstacle is to the right of the path. However, after the obstacle the path makes a right turn, and, because of that, the shorter arc is actually to the right of the obstacle. This means that the algorithm is shown to be performing quite well in this somewhat complex and generic case. The other obstacles are similarly avoided.
So far, all the cases discussed have depicted the formation avoiding all the obstacles it faced by leaving the path, circumventing the obstacle following the shortest circumference arc and returning back to the original path beyond the danger and at a safe distance. This is in accordance to the expected behavior of the algorithm for single obstacles (or obstacles far enough in relation to the others). Now, the cases presented in the second column, where multiple obstacles appear in the path followed by the formation close to one another, require the use of the extended algorithm. The first scenario, shown in the upper-right figure, depicts two obstacles in close proximity. Much like its single-obstacle counterpart, when facing the first obstacle, the formation avoids it using the shortest circumference arc available. However, in the middle of the avoidance maneuver, the robots face a second obstacle. Then, if the formation chose the shortest arc once more, it is easy to see that it would collide with the first obstacle. In such case, it is shown that the algorithm chooses the longest circumference arc, because it is the safest one. After that, the formation completes its navigation.
In the second scenario of multiple obstacles (middle figure at the right column), the formation has to deal with three close obstacles. Now, it is possible to observe that in each case, the robot followed the safest arc. The choices will depend on the relative positions of the obstacles, formation and path, as discussed, but it is clear that the algorithm yielded the right ones.
Finally, the last scenario shows two clumps of obstacles in close proximity. To avoid the first clump, the formation initially follows the shortest arc around the first obstacle and then, when faced with the next obstacle it chooses the longest arc to return to the path. Midway through its navigation, the formation is faced with the third obstacle of the clump, and it readily changes direction to avoid it. At the second clump, a similar process happens and the formation is able to navigate around it.
Thus, it has been shown that the algorithm will behave as expected from the theoretical analysis. It will avoid the first obstacle it sees following the shortest circumference arc and, upon facing a new obstacle, it will follow the safest circumference arc around the obstacle, as many times as necessary to return to the original path.
These simulation results show that the proposed strategy accomplishes the obstacle avoidance task in several different scenarios with unique challenges, indicating that it can deal with generic obstacle distributions in the workspace of the formation.

B. REAL WORLD EXPERIMENT
This section presents and analyzes a real experiment in which the UAV-UGV formation should follow a path correspondent to a lemniscate of Bernoulli, along which it faces some obstacles. The experiment was run using the setup described on Fig. 7. As one can see in the figure, the UGV adopted was the nonholonomic unicycle Pioneer 3-DX mobile platform, and the UAV is the Bebop 2 quadrotor. To measure the vehicle positions of the vehicles along their navigation the motion capture system OptiTrack was used, configured with eight cameras spread around the room. In order to communicate with the robots the Robot Operating System -ROS was used. As for the high-level control code, it was run using Matlab. The communication between ROS and Matlab was done through a proprietary Matlab library, which simulates the necessary ROS nodes and topics. The obstacles were represented by physical objects, which also had their positions captured using the OptiTrack system.  The desired formation velocity, the gains and the parameters of the obstacle avoidance algorithm were configured with the same values used in the simulations. The path to be followed was defined as As for the desired formation, it was established as being q d n = c n x c n y c n z 1.5 m 0 rad π 2 rad T , for the part of the experiment in which the two vehicles are navigating as a formation, and q d l = c n x c n y c n z 0.8 m (ψ 1 + π) rad 4π 9 rad T , (36) for the part of the experiment in which the UAV is preparing to land on the UGV. The task to be accomplished by the formation was to follow the path, reacting to any obstacle appearing in its way. During the first lap no obstacles were placed, in order to establish the baseline for normal navigation. In the second lap, two obstacles were placed in the way of the formation, far from each other. The objective was to show the algorithm working on avoiding single obstacles. Finally, in the third and last lap of the formation along the lemniscate of Bernoulli curve, the formation faced three obstacles placed near each other, in positions randomly selected along the navigation. The operation of the path-following controller and the obstacle avoidance algorithm can be watched in the video of the full experiment, which can be seen at the link https:// youtu.be/KfihOrF4xmE.
The results of the whole experiment are synthesized in Fig. 8, where, in order to make easier to understand the performance of the proposed system, the navigation was split into three parts, albeit being just a single experiment, each part consisting of a lap around the path.
In the first lap the two robots quickly converge to the positions correspondent to the establishment of the desired formation, and then they follow the path without problems, as depicted in the first subfigure. In the second lap, the formation faces an obstacle just about the second turn-right maneuver, as depicted in the second subfigure. As one can see in the figure, the formation avoids the obstacle, following a temporary safe path. Afterwards, just after turning left, a new obstacle forces the formation to follow a new safe path generated by the obstacle avoidance algorithm, which results in the avoidance of the obstacle.
In the last part of Fig. 8, the third lap was supposed to start at the same spot where the second lap finishes in the previous figure. However, as this was an obstacle free zone and the landing happened at this region, the beginning of the third lap was omitted, and a delayed starting point is shown in the graph. The formation starts following the lemniscate of Bernoulli in the third lap normally until facing three obstacles, just before turning left the first time. Then it uses the safe paths generated by the obstacle avoidance algorithm and navigates safely to a point in the path beyond the obstacles, from where it continues following the prescribed path.
Finally, at the end of the navigation, the UAV lands on the still moving UGV, and then the navigation as a whole finishes.
The formation errors during the navigation are shown in Fig. 9, where the starting points of the second and third laps are represented. Regardingx andỹ, it is apparent that the error peaks are smaller at the first lap, slightly larger in the second lap and larger in the third lap. This happens because the sudden change of the desired path to avoid the obstacles increases the error momentarily. However, one can see that the controller is capable of reducing those errors and that the maximum magnitude is of such errors is in the order of 20 centimeters. The error in the z-axis,z, is always zero because the virtual robot correspondent to the reference for the formation, which coincides with the UGV, is considered to be navigating always on the x − y plane, where z = 0.
Additionally, one can compare the maximum errorsx andỹ with the error bounds present in [40] for a similar speed value. There, the error bound can get as high as 50 centimeters.
The second column in Fig. 9 depicts the errors regarding the shape of the formation. The error associated with the length of the 3D Line,ρ, is quickly reduced to a value close to zero, which corresponds to the composition of the desired formation, and then is kept under control. Accordingly, the error β exhibits the same behaviour. The errorα, however, is much larger. This happens because when β = pi 2 , as it is the case in q d n , α is actually in a singularity point, and thus any value at all will accomplish the task. Because of this, the errorα can be disregarded in this situation (a more thorough explanation about such a singularity and why it can be disregarded is given in [15]).
Finally, Fig. 10 compares the desired velocities of the UGV, which represents the virtual robot that is the reference for the formation, with the measured values. The first graph shows  The results show that the tracking of the desired speed is close to perfection, since the three curves are very similar to one another. Even so, one can note slight delays whenever there is a sudden drop in the reference velocity, caused by the strategy adopted to make severe turns, discussed in Section II. This happens because of the lack of a dynamic compensator for the UGV, which would deal exactly with the imperfections in the velocity tracking, but even without such a compensator, as it is the case here, the delay is still small enough to be disregarded in this application.
Due to the nonholonomic restriction of the UGV, the angular velocity is not freely set, as the linear velocity is. It is a consequence of the direction of the latter one, since the robot must always face the direction of its movement. Because of this, there is no reference value for the angular velocity, but nonetheless it is vital that it is correctly controlled, otherwise the robot will not be able to follow the desired direction. Fortunately, it can be seen that the robot successfully tracked the controller references for its angular velocity. Thus, as the robot has successfully tracked both the desired linear velocity and angular velocities, it ensures that both the magnitude and direction of the desired velocity along the path being followed are correct.

V. CONCLUSION
This paper proposed an obstacle avoidance algorithm embedded in a controller designed to guide an heterogeneous formation of robots (a UAV and a UGV) in a path-following task. In order to do that, the formation was characterized and the path following controller was adapted to embed the obstacle avoidance algorithm. Still considering the proposed path-following controller, a strategy was applied to adjust the desired velocity along the path, which allows the user to set a maximum velocity, instead of a constant one, which may increase efficiency when completing the task. In this sense, an important detail is that the proposed controller does not have any singularity in terms of the velocity along the path, which allows varying the velocity along the path in any way, including stopping the formation when necessary.
Afterwards, the obstacle avoidance algorithm was formalized, first considering just single obstacles, and then was extended to handle multiple obstacles at once. This is done through establishing safe zones around the obstacles, shaped as circles, and then navigating along temporary safe paths generated by the algorithm until reaching another point on the path located after the obstacles. In the case of single obstacles, the safe path is always the shortest arc of the circle delimiting the safe zone, as both circumference arcs are equally safe. On the other hand, for multiple obstacles the safe path will be the safest circumference arc, whether it is the shortest or the longest one, and it will be established in a case by case basis in real time. VOLUME 9, 2021 From the results presented, involving both simulations and experiments, it is possible to claim that the controller is effective in guiding the formation along a path, and that, by utilizing the proposed obstacle avoidance algorithm, the formation is able to navigate safely in an environment with previously unknown hazards.
As future work, it is under study to modify the multiple obstacles algorithm to model the multiple obstacles as a single ellipsis, instead of modelling separate safe zones as circles. The idea is that this might increase efficiency in the navigation. Also, the approach used to bring the points of interest to a new frame centered on the obstacle already makes such modification easier, because while the circle is immune to rotation around its center, an ellipsis is not, so its definition, and that of the shortest and longest arcs, are simpler when it is located at the origin with no rotation. As a second part, it is being studied how to handle moving obstacles, and the ability of the path controller to freely change the formations velocity is a great advantage, where in certain cases the most natural approach may be to stop the robots and wait until the moving obstacles do not represent a problem for the formation, or to change the formations velocity in such a way that the trajectory of the formation and of the obstacles do not cross over one another at the same time.