Collision Avoidance

We have developed an algorithm to avoid colisions in the swarm

Motivation

When many drones move in close proximity to each other, collisions are very likely to occur, which would impair the functioning of the swarm as a whole. To avoid this problem, in this section, we will present an algorithm developed to prevent collisions between drones, considering several scenarios.

Solution

In a simplified way, our algorithm divides collisions into three types. The case where one clover is stationary and the other in motion, the case where both are in motion and with parallel trajectories, and finally the case where both are in motion and with non-parallel trajectory.

For the first situation, let's call the clover that, at first, was stationary clover A and the clover that was in motion clover B. Thus, the action of the algorithm to avoid the collision is to move clover A orthognonally to the trajectory of clover B until it reaches a safe distance. After that, clover B follows its original trajectory. After it reaches a safe distance from clover A, clover A is released to return to its starting point.

For the second case, as soon as the possible collision is detected, both drones are paralyzed. After that, one of the clovers is arbitrarily chosen to move orthogonally relative to the trajectory of the other until it reaches a safe distance - let's call it clover A. After that, the clover that stood still, the clover B, follows the original trajectory. When clover B moves far enough away from clover A, the latter is released to follow its original trajectory.

Finally, for the last case described, the first action is also to paralyze both drones. Arbitrarily, one of the clovers is released to follow his original trajectory. After reaching a safe distance, the other clover is also released to follow its original trajectory.

Now, if you want to know how we did the collision avoidance algorithm, the next section will explain in detail the making of the code.

How does the algorithm treat the problem?

In the swarm_collision_avoidance.py file, a SwarmInternalCollisionAvoidance class is created, which will check and obtain information about all the drones in the swarm, leaving open the possibility of checking their respective positions.

SwarmInternalCollisionAvoidance

The constructor of this class initializes important variables for the treatment of collisions, among which we highlight:

Variables Meaning

Variables	Meaning
`dist_threshold = 1.5`	Sets the minimum safe distance to 1.5 meters.
`handling_ids = []`	Creates a vector that will be filled with a tuple of clovers IDs which have a collision being handled.
`all_clovers_ids = []`	Vector that stores the IDs of all clovers in the swarm.
`init_formation_coords = []`	A vector that stores vectors with the x, y, and z coordinates of all clovers.
`swarm = []`	Vector of instantiated objects from the SingleClover class, which represents each clover individually (will be explained in more detail later).

dist_threshold = 1.5

Sets the minimum safe distance to 1.5 meters.

handling_ids = []

Creates a vector that will be filled with a tuple of clovers IDs which have a collision being handled.

all_clovers_ids = []

Vector that stores the IDs of all clovers in the swarm.

init_formation_coords = []

A vector that stores vectors with the x, y, and z coordinates of all clovers.

swarm = []

Vector of instantiated objects from the SingleClover class, which represents each clover individually (will be explained in more detail later).

In addition, the constructor of this class also calls some methods to finish its configuration. They are:

Methods Action

Methods	Action
`__subscribeSwarmChecker()`	The method creates a ROS Subscriber that subscribes to the topic 'swarm_checker/state', being able to take the data from all the clovers and store it in the variables.
`__initTF2Listener()`	The method creates a ROS Listener that allows you to receive transformation information between reference frames.
`__getInitialFormation()`	Fills in the vector containing the x, y, and z coordinates of all clovers.
`__createCloversObjects()`	Instantiates objects from the SingleClover class and adds them to the swarm vector.

__subscribeSwarmChecker()

The method creates a ROS Subscriber that subscribes to the topic 'swarm_checker/state', being able to take the data from all the clovers and store it in the variables.

__initTF2Listener()

The method creates a ROS Listener that allows you to receive transformation information between reference frames.

__getInitialFormation()

Fills in the vector containing the x, y, and z coordinates of all clovers.

__createCloversObjects()

Instantiates objects from the SingleClover class and adds them to the swarm vector.

Since we mentioned creating objects from the SingleClover class, we'll explain how it works.

SingleClover

When we instantiate the SingleClover class on an object, we call the constructor of the class that creates important variables, which are described below. It is worth noting that the first two are received as a parameter by the constructor and are quickly populated, while the third has already been calculated in the SwarmInternalCollisionAvoidance class and is only assigned to this variable.

Variáveis Meaning

Variáveis	Meaning
`name`	Stores the name of the clover, which is of type "clover0", "clover1", and so on.
`id`	Stores the clover id.
`init_coord`	NumPy array that stores the initial coordinates of the clover.
`pose`	NumPy array that stores the current position of the clover.

name

Stores the name of the clover, which is of type "clover0", "clover1", and so on.

id

Stores the clover id.

init_coord

NumPy array that stores the initial coordinates of the clover.

pose

NumPy array that stores the current position of the clover.

Next, a single method is called: the configure() method. This method is responsible for creating a ROS Subscriber that captures the information from the topic f"{self.name}/mavros/local_position/pose" and configures the array of current positions. In addition, in this method, variables are created from ROS ServiceProxy, which allows services for clover to be called in the future.

Thus, after assembling the vector of objects of type SingleClover, we complete the constructor of the SwarmInternalCollisionAvoidance class.

Instantiation on an object and calling the method that prevents collisions

The code avoid = SwarmInternalCollisionAvoidance() instantiates this class into the avoid object, which is used to call the main method of this class: the avoidCollision() method, which puts services into actions on the clovers to prevent them from colliding.

The avoidCollision() method

This method runs a loop of repetition with a frequency of 10Hz. With each iteration of this loop, a vector is created with the current positions of all existing clovers.

The distance between each pair of clovers is analyzed by a method called __analyseDistance(). This method creates an n x n matrix, where n is the number of clovers and the a_ij element is the distance between the clovers i and j.

Furthermore, if one of the calculated distances is less than 1.5, the algorithm calls the function __checkParalellTrajectory() to determine whether the trajectory between the two clovers is parallel or not. It does this by analyzing the angle between the trajectories, obtained through the following mathematical expression, considering that one of the vectors is the trajectory vector of clover i and the other, the trajectory vector of clover j. All these operations are performed with functions of the NumPy library.

\theta = \arccos \left( \frac{\vec{a} \cdot \vec{b}}{\left\lVert \vec{a} \right\rVert \left\lVert \vec{b} \right\rVert} \right)

After the calculation, the angle is converted to degrees. If it is less than 20º or greater than 160º, the crossing between the clovers is parallel. Otherwise, it is non-parallel.

Thus, after the return of this function, a tuple containing the IDs of the nearby clovers is added at the end of the vector near_ids and a Boolean that indicates whether the trajectories between i and j are parallel with true or not parallel with false.

Finally, the __analyseDistance() returns the distance matrix and the tuple vector near_ids.

The next step of the loop is to summon collision handling for all elements of the near_ids vector. Thus, if the collision is no longer being handled, which is verified by the presence of i and j in the vector handling_ids, a Thread is declared and started that executes the __handleTwoNearClovers method for each element of the vector. We will now open a specific section to explain this method that is the center of the actions that will prevent collisions between drones.

The __handleTwoNearClovers method

The first step is to add the IDs of the clovers in thehandling_ids vector to make it clear to the other simultaneous processes that the collision between these two specific drones is already being dealt with.

After that, a test is done to check if one of the clovers is stopped. If this statement is true, it is called the __handleStationaryClover method. Otherwise, if the trajectory is parallel or non-parallel, the functions __handleParallelClovers and __handleNonParallelClovers are called, respectively.

Métodos Ação

Métodos	Ação
`__handleStationaryClover`	The method receives the IDs of the clovers and first saves in one vector the point where the stopped clover (A) is, and in another vector the trajectory of the moving clover (B). Next, the clover that was in motion is requested to stop with the initialization of a Thread that executes the`__stopClover` method. Then a vector orthogonal to the trajectory vector of B is calculated and clover A is sent into that trajectory with the function`__goToLastTarget` and, after reaching a distance greater than the `dist_threshold` parameter, the clover B follows its original trajectory using this same function. After this command and the minimum safe distance is reached, the clover A returns to its original position by making use of that function.
`__handleParallelClovers`	The method takes the IDs of the clovers as a parameter and quickly saves in two vectors the trajectory of both clovers. Then, both drones are paralyzed by the start of two Threads that launch the execution of the `__stopClover` function. After this step, arbitrarily one of the clovers is chosen to follow in trajectory orthogonal to the trajectory originates from the other. This trajectory is calculated and the chosen clover is sent to it by means of `__goToLastTarget` and thus, when it reaches a safe distance, the clover that remained stationary follows its original trajectory through the same function. After that, the displaced clover follows its original trajectory also using this method.
`__handleNonParallelClovers`	The method receives the IDs of the two clovers. Trajectories are saved and threads are started for the execution of the `__stopClover` method, paralyzing them. Arbitrarily, one of the clovers is chosen to follow his original trajectory with `__goToLastTarget`, and after that drone distances itself far enough, the other is allowed to do the same process.

__handleStationaryClover

The method receives the IDs of the clovers and first saves in one vector the point where the stopped clover (A) is, and in another vector the trajectory of the moving clover (B). Next, the clover that was in motion is requested to stop with the initialization of a Thread that executes the__stopClover method. Then a vector orthogonal to the trajectory vector of B is calculated and clover A is sent into that trajectory with the function__goToLastTarget and, after reaching a distance greater than the dist_threshold parameter, the clover B follows its original trajectory using this same function. After this command and the minimum safe distance is reached, the clover A returns to its original position by making use of that function.

__handleParallelClovers

The method takes the IDs of the clovers as a parameter and quickly saves in two vectors the trajectory of both clovers. Then, both drones are paralyzed by the start of two Threads that launch the execution of the __stopClover function. After this step, arbitrarily one of the clovers is chosen to follow in trajectory orthogonal to the trajectory originates from the other. This trajectory is calculated and the chosen clover is sent to it by means of __goToLastTarget and thus, when it reaches a safe distance, the clover that remained stationary follows its original trajectory through the same function. After that, the displaced clover follows its original trajectory also using this method.

__handleNonParallelClovers

The method receives the IDs of the two clovers. Trajectories are saved and threads are started for the execution of the __stopClover method, paralyzing them. Arbitrarily, one of the clovers is chosen to follow his original trajectory with __goToLastTarget, and after that drone distances itself far enough, the other is allowed to do the same process.

Observations

The __goToLastTarget method uses the navigate service to move the clover to a certain point;
The __stopClover method uses the navigate service from the navigateWait function to keep the clover in its current position for a period of time.
It is worth remembering that the services were given to the object through the 'ROS ServiceProxy' in the definition of each object of the SingleClover class.

Finally, after the collision treatment, the tuple with the pair of IDs that had the collision treated is removed from thehandling_ids vector. This removal must be done because these two drones can be re-approached again and this possible collision cannot be blocked from being treated.

PreviousTransformations NextLED Effects

Last updated 12 months ago