Formation tracking of target moving on natural surfaces

In this paper, we focus on formation control of a swarm of UAVs tracking a moving target (typically a climber) on a non regular mountainous surface. The main objective is to show how to dispatch the whole swarm of the quadrotors around the target with respect to the natural environment (mountain) taking advantage as much as possible from the free space around the climber. The formation shape adapts with the neighbour’s environment topology, so that it avoids collision with the environment and the occlusion of the target from the UAV’s cameras sights, and ensures as well the formation compactness and its closure to the target respecting the agent-agent and the agent-target security distances. Based on a free and safe hovering space (SHS), the proposed work proposes two algorithms to determine for each agent its desired position within the SHS according to some maximum occupation criterium. The main contribution resides in handling complex obstacle constraints while maintaining visibility of the target.

Formation tracking of targets moving on natural surfaces * H. Nadour, N. Marchand, L. Reveret

and P. Legreneur
Abstract-In this paper, we focus on formation control of a swarm of UAVs tracking a moving target (typically a climber) on a non regular mountainous surface. The main objective is to show how to dispatch the whole swarm of the quadrotors around the target with respect to the natural environment (mountain) taking advantage as much as possible from the free space around the climber. The formation shape adapts with the neighbour's environment topology, so that it avoids collision with the environment and the occlusion of the target from the UAV's cameras sights, and ensures as well the formation compactness and its closure to the target respecting the agent-agent and the agent-target security distances. Based on a free and safe hovering space (SHS), the proposed work proposes two algorithms to determine for each agent its desired position within the SHS according to some maximum occupation criterium. The main contribution resides in handling complex obstacle constraints while maintaining visibility of the target.

I. INTRODUCTION
The collaboration between multiple UAVs is vital in many complex tasks. Its efficiency is demonstrated in the literature [1] and deployed for various objectives, including but not limited to collaborative reconnaissance, target tracking, identification and collaborative combat as well in the process of executing combat missions [2]. Most of the collaborative tasks studies seek to maintain some desirable geometric configuration. Deep-space interferometry is an example for geometric keeping [3], where a fleet of networked air-crafts are required to perform a sequence of formation manoeuvres while maintaining a relative attitudes accurately. [4] is another good example of keeping the formation persistent and rigid in the course of its moving using distributed cohesive motion control. In [5], adaptive leader-following approach is deployed combined with artificial potential field (APF) in order to control a triangular formation a with collision avoidance using repulsive potential. However, the above mentioned works, among others, deals with a finite number of simpleshaped obstacles, for which it is intuitive to seek for rigid formation with respect to the leader or the target for the most cases. Furthermore, these works focuses mainly on tracking the target/leader, demonstrating the coordination between the agents, without discussing what reason compels the formation to take some shape or *This work was supported in part AirCap Regional project funded by French ARA Region.
H. Nadour and N. Marchand are with Grenoble-Alpes Univ., Grenoble-INP, GIPSA-lab, UMR5216, France. housseyne.nadour, nicolas.marchand@gipsa-lab.fr P. Legreneur is with University Lyon I, LIBM, France. Pierre.legreneur@univ-lyon1.fr L. Reveret is with INRIA Rhône-Alpes -Morpheo Team, lionel.reveret@inria.fr another one. In this paper, we study the possibility of having a formation that is not static and may depend upon the obstacles and the tasks of the formation.
By contrast, Our work needs the formation shape to be adapted with environment topology in which the team of UAVs is dispatching. so the formation must be adjusted automatically and continuously depending on the free space between the natural surface on which the target is moving, in order to ensure clear vision for each UAV's camera without occlusion, and no-collision, achieving some other criteria as well. Beside target tracking and formation control, this paper subject interests with other fields like surveillance and monitoring of moving objects. It is notable that most of surveillance studies [6]- [10], deal with urban environments, where the common objective is to monitor a static or moving target between urban buildings by means of single or multiple UAVs with on-board cameras. [8] proposed a novel occlusion-aware surveillance algorithm based on decomposition of the surveillance problem into a variant of the 3D art gallery problem and an instance of travelling salesman problem for Dubins vehicles, the algorithm yields a covering vantage points, each of which must be travelled by at least one UAV in a shortest path without need for coordination between agents. It is similar for [6] where he tried to keep a close line of sight from the UAV to the target in a densely populated area, his focus was to find optimal flight paths for UAVs to minimize the chance of losing the moving target under mild assumptions. In order to minimise the occlusion, the UAVs turn on circle path with the minimum turn radius and the maximum allowed altitude, the one thing to be studied is the centre of the circle for which the occlusion is minimised furthermore. Whereas [9] addressed the same problems together with other objectives like predicting the target states based on extended Kalman filter combined with probability estimation, considering the line of sight occlusion of buildings, as well as the energy consumption.
These proposed solutions, among many others, are dedicated for urban environments where it is assumed the surface consists only of the base plane and quadrangular prisms lying on the base plane (i.e the target moves in planar path between buildings), and all UAVs fly at some altitude over the buildings which makes them free from natural obstacles. In addition, as the agents are so far from the target and the terrain, these facts make the targets always under the camera sight, i.e the relative speed between the target and the UAV's eyes is low, for which, in most cases the target can be even considered as static, the same points are noticed for [10] where the author did not even need to consider the occlusion and the collision avoidance, meanwhile our issue compels the UAVs to be very close to the target (5-20 m) to monitor a target moving on natural no-planar surface (mountainous surface), this fact increases mainly the probability of occlusion of the line of sight, and the collision with the mountainous surface, which push us to exclude these solutions adapted for the usual surveillance tasks where the agents hover out of the terrain folds, especially in urban environment.
One of the current applications of UAVs, that is under study and development is the tracking of a climber moving on a surface by a swarm of mobile cameras [11]. Where the objective behind this tracking is to capture his motion while climbing the surface. Thanks to some 3D reconstruction algorithms the set of the synchronised videos are processed at the end of the experiment in order to derive the digital model of his motion that is used to study some bio-mechanical characteristics of his movements, like velocity, acceleration, his locomotion, as well as his 3D trajectory using a standard DLT (Direct Linear Transform) reconstruction [11]. The biochemical study of the climber motion can be used for multiple purposes, like enhancing his movements, or stimulating the climber movement on real robots. The process can be expanded later to capture animals movements in naturals environments.
Our objective in this paper is to show how to dispatch the whole swarm of the monitors (UAVs) around a climber target 1 with respect to the natural environment (mountainous surface) taking advantage as much as possible from the free space around the climber, for which it is necessary to take in consideration the whole geometry of the surface without neglecting its curvatures and folds by dividing the environment into framework of small entities like Octomap [12], or framing it within simple shapes like convex polyhedra [13], [14].
Approximating the environment to simple shapes is convenient with the usual objectives of path planning: (i) minimising the length of the overall path and (ii) maximising the margin of safety from obstacles [15]. By contrast, we seek in this paper, in addition to avoiding collisions, to avoid the occlusion, as well as to ensure a maximum safe view range between the two extreme agents in order to allow capturing the motion from multiple perspectives dispersed as much as possible, so that the 3D reconstruction is enhanced and the most out of biomechanical characteristics are extracted. The problem, that we focus on in this paper, is the tracking of some moving target evolving non regular  Fig.1, the typical case of study in this paper will be capturing a climber on a wall that may contain ridge, chimneys and off-width due to fissures and cracks in the rock Fig.2. The tracking is performed using embedded cameras on the UAVs and therefore the two main objectives are: avoiding obstacles and keeping the target in the field of view. We proceeded at first to make some assumptions and objectives. Consider a swarm S of m agents {a 0 , ..., a n+1 } moving in 2 space . We want them to be well spread around the climber while he is on the wall, The wall W in this example is a random continuous line, and the point c (climber), moves on the line as indicated in Fig.3.

1)
We suppose at first that the best distribution for the inter agents is when they are spread evenly around the climber, i.e they have equal inter-angles, this is an arbitrary choice. Otherwise, the choice is subjected to a cost function that must be minimised in order to maximise the amount of extracted biomechanical information.
2) It is desired to maximise the range of view, i.e to have a maximum possible angle α n+1 for the view area A in order to take advantage as much as possible of the surrounding space and therefore to monitor the target from diverse perspectives. 3) Each agent i must hover freely at some distance d i far away from the climber, d i is the trade off between the following points: • the target must be safe i.e d i must be no lesser then the safety distance d c . • the swarm formation compactness and its closure to the target must be guaranteed. • avoid collision between agents by respecting the minimum separating agent-agent distance d a . 4) The swarm is delimited by a free region A that protects the swarm from colliding with the surface and the occlusion of vision. Accordingly, we introduce two extreme fictive agents a 0 and a n+1 that are meant to delimit the boundaries of A . From these points, it turns out that the region A we are looking for is a sector, i.e a region between an arc and two radii which its vertices that are the climber c, and the two fictive agents a 0 and a n+1 , see Fig.3.

III. DETERMINING THE SAFE HOVERING SPACE
The cartography is a digital model, which is an ordered list of points W = w 0 , ..., w i , ...w m t , we suppose the wall is continuous, the continuity of a digital model here will mean that the distance between any tow adjacent points is inferior then some very tiny distance e, i.e |w i − w i+1 | < e. Smoothness property can be added to avoid discontinuity phenomena. As discussed previously, the desired area A to be found, is delimited by a sector centreed at c and has a maximum possible view angle between a 0 and a n+1 , for which the space is free from possible collision or occlusion of sight, theses constraints define ca 0 and ca n+1 , see Fig. 4. If it is the case, then the safe hovering space is defined as: The two extreme points a 0 and a n+1 can be determined based on the viewshed computing [17], the viewshed V of a point c on a surface W is defined as: Considering just the area surrounding the target c, within a circle of radius l, then : That is to say the two lines ca 0 and ca n+1 , of length l, must pass through the two points v r ∈ V and v l ∈ V respecting equation (2), each of which corresponds to the maximum sight angles β r and β l resp Eq. 3.
Let the two viewshed subsets V r and V l be defined as: And let β(v) define the sight angle associated with the point v, then: The angle β is defined by the cross product and the dot product of u t , the unit vectors tangent at c, with the sight The vectors u t and cv must be extended to 3D ones with equal zcomponents in order to apply the cross-product. Notice that as arctan function, we mean the one returning values in the interval [−π, π], while the point v can make angles out of this range. Nevertheless, the angle β can be calculated using numerical integration β = δβ during the algorithm iterations.

IV. FORMATION SHAPE WITHIN SHS
Let ca 0 and ca n+1 be two vectors defined as: Each agent must be at the following position: Where a i is the position of the agent i, with F i is a transformation matrix defined as below: while: The range (angle) of A is defined by the cross-product and the dot-product of ca 0 and ca n+1 : The term d i is the agent-target distance, and must ensure the closure of the agents to the target (compact formation), that is to say the average must be minimized (equation (10)) respecting both the safety agent-target distance d c (equation (11)) and the safety agent-agent distance d a (equation (12)) by Al-Kashi Theorem ( Fig.  6(b)), as well as the boundaries limits on the left and the right (equations (13)- (14), Fig. 6(c)) : This is a nonlinear optimisation problem that can be resolved using Non Linear Programming solvers like YALMIP [18]. However the computation of non linear programming is very slow which conducts us to propose another algorithm that yields a sub-optimal solution and guaranties the formation compactness and its closure to the target respecting the agent-agent and the agent-target security distances d a and d c , as well as the boundaries limits.
The algorithms proceeds in two steps, the first one aims to rearrange the agents in a list P r that determines for each agent its degree of priority to be closer to the target Fig. 5, in other words, P r maps form a given degree of priority dg i to the number of its corresponding agent: P r : dg i → i. The second step computes, orderly and for each agent in P r , the maximum possible constrained distance among all possible constrained distances Fig.  6(a), the two steps are explained below: 1) Step 1: In this step, the group of the whole agents are separated successively multiple times until we get N groups of one agent; Firstly, the swarm (array of N elements) is divided into two new groups (arrays) based on the parity of agents positions (index), the even positions group is prior. Then, each new group itself is separated again into two new groups based on the parity of the position, this process is reproduced until we get an array P r of N groups of one agent, with ordered priorities, Fig.  5 illustrates the process of selections for five agents that results P r = (4, 2, 3, 5, 1) t . Notice here that this step can be executed once and offline, because P r does not depend on any variable but only the invariable number of agents.

2)
Step 2: This process part encompasses all agents in P r , starting orderly from the first prior one P r (1) until the last one P r (N ). For each agent i = P r (dg i ) the algorithm computes: • All possible distances d j i , constrained by prior agents j = P r (dg j < dg i ); Each d j i is computed via Al-Kashi theorem (Fig .6(b)): Obviously the chosen solution is that that verifies a i a j . If the discriminant ∆ = d 2 a − d 2 j sin 2 (δα) is negative we assign any value d c .
• The two distances d l i and d r i constrained by the left and the right boundaries; when an agent i is on contact with left or right limit then the distances are given in the two right triangles: Then it picks up the maximum distance among all constrained distances:

A. The camera set angles
Each agent holds a camera on the axis x 3 , it is clear then x 3 must shooting toward the target, let be Ψ i the angle made between the vector a i c = c − a i and x 0 of the world frame R 0 around z 0 , then the desired yaw for each agent is V. CONTROL, TRACKING A. The dynamic model of each agent In this section, we design low-level controllers for the individual agents. We first describe the dynamic modelling of quadrotor vehicles and then design control laws for trajectory tracking, which will be used to track the trajectories generated by Eq.6. Each agent is a quadrotor vehicle pushed by four intrinsic forces f i∈{1...4} generated by four rotors Fig.7. The different combinations of the forces gives four degrees of freedom. The body frame R 3   Fig. 8. The quad-rotor areal vehicles dynamic is represented in the literature by quaternion form model [19], or by the matrix form model [20], the last one takes the following from: where: p is the vehicle centre position in the inertial frame, f i is the thrust generated by the rotor i, m is the body mass, g is the gravity acceleration, ω is the angular velocity expressed in the body frame R 3 by (ω x , ω y , ω z ), I = diag (I xx , I yy , I zz ) is the inertia matrix assuming that the intrinsic axis are symmetric axis (I is diagonal), τ = (τ x , τ y , τ z ) t is the intrinsic applied torque, τ g is the gyroscopic torque: with w g = w 1 −w 2 +w 3 −w 4 , W = w g z 3 and the w i s are the rotation speed of the blades.The inputs of the system are: where f i = k l .w 2 i , with k l is the lift coefficient, and w i is the rotor i angular velocity, k d is the drag coefficient. Equations (18)-19 can be developed as below: Bouabdallah et al. approximated in [21] the attitude dynamic around the equilibrium point, i.e for θ ≈ 0 and φ ≈ 0, as below: The inputs are decoupled, and the model is easy to be handled, for that reason this approximation has been used widely in the literature [22]- [24]. The demonstration in appendix shows that the following equations are a better approximation:

B. Individual control
Each agent has two dynamics, the dynamic of the attitude whose the inputs are u 2 , u 3 and u 4 (equation (19)), and the dynamic of the translation (equation (18)) whose the inputs are u 1 , φ and θ. It is convenient to deploy cascade control that involves two controllers, one of which nestling inside the other, such that the outer controller (position controller) feeds the inner one (attitude controller) with the set references (θ * and φ * ), Fig.9. It's a cascade control. Such a system can give an improved response to disturbances [25]. Fig. 9. synoptic scheme of the controller 1) Controlling the attitude: let be w φ ,w θ ,w ψ the new inputs, such that: We get a decoupled linear system with respect to the new inputs, composed of three 2 nd order systems : We refer by φ * to the set point of φ, so if we take: then the characteristic polynomial is : where the parameters k 1 and k 2 are tuned so that the system is stable. The same strategy is applied on θ and ψ.
2) Controlling the position: As previous, let A x =ẍ, A y =ÿ, and A z =z be the new inputs of the translation dynamic. A PID controller is applied on each of these linear subsystems, i.e: where e x = x * − x, e y = y * − y, e z = z * − z. with x * , y * , and z * are the set points. It should be noticed that the inner controller must be faster than the controllers of x and y. These controllers give the necessary acceleration A (or the necessary force F = A.m) that must be applied on the body frame.
3) Extracting u 1 , θ * and φ * from the position PIDcontrollers outputs: On one hand, we have z 3 and z 0 expressed in the frame R 1 as following: which allows to rewrite equation (18) in the frame R 1 as below:p On the other hand, the position controller gives the required acceleration expressed in R 0 : Which yields according to equation (31)-33: Then we can conclude the set points for the attitude controller: The input u 1 is preferred to be concluded from the third line cos(φ) cos(θ) = Ag z in order, firstly, to avoid singularity caused by sin function around the equilibrium points φ ≈ 0 and ψ ≈ 0, and secondly, to involve directly θ and φ instead of θ * and φ * in the following equation: This makes the control of z independent from the inner controller (attitude controller), and as result the control of z is more performing.

VI. SIMULATION
The algorithms are validated using ROS together with SimuLink. The swarm consists of 5 agents trying to capture the movement of the climber where he is moving horizontally on 2D surface at a desired constant height z = 20 Fig .10.
The PID controllers parameters are tuned so that each vehicle followed its assigned position a i (t) (equation (6)) as quickly as possible 11-12, the swarm respected the desired specifications and was able to maintain its desire form i.e. to be inside SHS, within compact formation while respecting the security distances. As one might expect, the curvature discontinuity increases greatly the discontinuity of the set trajectory a i assigned to the agent i, and consequently: • the rotational movements θ and φ are increased and become so dynamic which effects the camera shooting quality on the target, it appears in form of noises. • in some cases, some agents are outside of SHS while they are trying to track it and be within it, this fact is natural when the swarm has a dynamic and cannot move instantly contrary to SHS.

VII. CONCLUSIONS AND PERSPECTIVES
We presented the basic idea for controlling a formation of UAVs monitoring a target moving on a non regular mountainous surface. we showed mainly how to determine the safe hovering space SHS where the whole agents can be dispatched around the target (climber) taking advantage as much as possible from the free neighbour space around the climber, so that the formation shape adapts with the neighbour environment topology, in order to avoid both collision with the wall and occlusion of the line of sight. Besides, In order to ensure the formation compactness respecting the the limits of A together with the agent-agent and the agent-target security distances, a sub-solution was proposed and tested with sufficiency, instead of using Non Linear Programming that might not be practical. This work represents one ring among many others that will be put together to accomplish the final protocol to monitor the target autonomously, for instance: • The strategy will be extended for 3D environment. • An intermediate safe corridor spaces between free regions A are necessary to remedy some problem due to the curvature discontinuity of the wall. • The safe hovering space A could be in some cases smaller then the minim required surface to store the whole formation, which compels to add a complementary protocol to full this gap. • A protocol that guides each agent to the target neighbourhood within the SHS is necessary before assigning for each agent its suitable position (α i and d i ) within SHS. • The path given to each agent should be redressed (optimised), in order to reduce some effects due the curvature discontinuity like high perturbation on the attitude of the agents which effects the quality of monitoring the climber. • Replacing the hypothesis of predefined cartography by sensors, this point will change the algorithm to compute SHS and holds technical difficulties. • Integrating a bio-mechanical cost-function that disperses the agents on the angular coordinates instead of taking arbitrary choices equation (8). • Generalising the idea of computing SHS for discontinuous surfaces.
Validation of the approximate model (24): The two models (23) and (24) are tested in this section using the matrix form inverse dynamic model defined as: With ε = (ψ, θ, φ), while f is a function that maps from ε to its corresponding rotation matrix R (in matlab f = eul2rotm). The idea behind this validation is to generate a rotational movements ε * so that at some moment t = t 0 we have: φ * (t 0 ) = 0 and θ * (t 0 ) = 0 ψ * (t 0 ).θ * (t 0 ).φ * (t 0 ) = 0 (43) The following angular movement is sufficient: The inverse dynamic model Eq.42 is introduced to calculate the reference torque τ * which is necessary to regenerate the desired rotational movements using the approximated direct dynamic models Eq.23 and Eq.24. So each approximated model is fed with the Eular angular velocitiesε * and the torque τ * as it is illustrated in Fig.13. The comparison is done between the outputs Fig. 13. Simulation schema; (B) model = Eq.23, (C) model= Eq.24, angles * = ε * ; dangles * =ε * , d2angles * =ε * (i.e the regenerated Euler angular accelerationε) of the approximated direct dynamic models and the reference Euler angular acceleration vectorε * , the validity of each model is confirmed if the following errors: • are null for t = 0.5 • they are closed to be null at the neighbourhood of t = 0.5. The errors E ψ,θ,φ b,c are plotted in Fig.14. The errors of the model Eq.24 are null for t 0 = 0.5 and closed to be null at the neighbourhood, contrary to the model Eq.23. Hence the validity of the established model in this paper.