OBSTACLE AVOIDANCE USING VFF NAVIGATION ALGORITHM

In this practice we will learn how to make a Formula 1 able to avoid obstacles in his path implementing the logic of the Virtual Force Field navigation algorithm (VFF):

The VFF (Virtual Force Field) is a navigation algorithm in which the trajectory of the robot (in this case, the formula1) is determined by virtual forces. We can differentiate 3 components:

ATTRACTION FORCE: Force that push the robot toward an objective.
REPULSIVE FORCE: Make up by the obstacles in the path (other cars, walls, etc...) which we don't want to crash into.
AVERAGE FORCE: Linear combination of both the attraction and repulsive forces, each multiply by a different coefficient that adjust the weight of each force.
FIRST STEP: FINDING OUT THE VFF VECTORS:
The first thing we need to do is to find out the components of that vectors:
- REPULSIVE FORCE: Using the lidar sensor from the F1 we can estimate the distance from the car to each of the obstacle points with angle i (i in the lidar range, [0-180º]) using the getLaserData() function. That function returns an array with the information for each of the "i"'s. To determinate a single point that represents the "mass center of the obstacles", we use the laserVectorize() function that returns the mean of the points in the list that are at a distance less than an DISTANCE_THRESHOLD value, in order to ignore the walls of the circuit when they are relative far from the car . Also, we will multiply the vector by a REPULSIVE_MULTIPLIER value to get a representative repulsive vector in the GUI.
- ATTRACTION FORCE: In our circuit, we have different checkpoints we need to achieve. Each of that checkpoints are the objective of our car. We can find out that checkpoints coordinates using the currentTarget = GUI.map.getNextTarget() sentence from the given API. The coordinates of the currentTarget are given in absolute coordinates system (the origin is located in (0,0), which is exactly where the F1 starts the lap). We will need to change that coordinates to a relative system, where the origin is the car position. For this reason, we define the function absolute2relative().
Notice that onces we have reach the target, we will need to change and update the algorithm to follow up to reach the next target we can mark a visited target using the currentTarget.setReached(True).
- AVARAGE FORCE: The resultant average force (the one that determines the direction of the F1) is determined by the following expression:
Faverage = 𝛂Fattraction + 𝝱Frepulsive
where 𝛂 and 𝝱 are coefficients that we need to balance in order to get an optimal output based in shyness and caution.
SECOND STEP: ADJUST THE VALUE OF THE FORCE VECTORS
Notice that the vectors of this forces can have a really big module (length of the vector) if, for example, the target its too far from the car. Notice that this is a big problem cause we are having a force component too big (pushing the car towards the target) and the obstacle force compared to this will be really small and won't take any effect. Also, the resultant average force will have also a big module, and this traduced to velocity will cause an excessive velocity in the F1.
In order to fix this problems, we will enclose the maximum value of this forces by the following graphs:
- ATTRACTION FORCE:
- REPULSIVE FORCE:
THIRD STEP: FROM FORCE TO VELOCITY VALUES:
Once we have the force value of the average force, we need to transform that data into velocity values for the wheels of the F1.
For the linear velocity, we will use the resultant module value of the average vector. As the module can be so small when the obstacle is too close from the F1 (and also it could be even negative!), I decided to implement a MIN_LINAR_VEL and MAX_LINEAR_VEL so the car doesn't take velocities out of that range. For the angular velocity, we will use the value of the y component from that vector (notice that if the y value is negative, the average vector is pointing to the left and we will turn to the left; and if the y value is positive the vector is pointing to the right and we will turn to the right). In line with the linear velocity, I set a MAX_ANGULAR_VEL so the car doesn't turn too fast.
In addition, this is the time when we need to find out which values of 𝛂 and 𝝱 are the optimal for our algorithm.
In mi case, I used the following strategy to find out the best values for that coefficients: While the execution program, I printed in every iteration the coordinates from the 3 vectors (using 𝛂 and 𝝱 initial values = 1). When the F1 was about to crash into another car, I stop the program and I check how the vectors where drawn, and I choose a point where I wanted to be my average Vector. Here is an example:

Solving the equation system of 2 variables we get the values of 𝛂 and 𝝱 for this particular case.
We can repeat this for another points of the circuit and get some more pairs of values for 𝛂 and 𝝱. Then, we can get an approximation of all the values using methods like least squares.
To get a first approximation of the program, I tried to run the program and see if the F1 was able to finish a lap. I was able to complete the circuit in ~6 minutes, but there was some points of the circuit that the car was having some troubles with some obstacles. Even if I was able to complete the circuit, the car grazed some moments into some cars:
In order to be able to complete the circuit not touching any obstacle, I introduced the following changes:
- Instead of getting all the values of the lidar range (0-180º), configure that range with MIN_LASER_RANGE and MAX_ANGLE_RANGE. In my case I used MIN_LASER_RANGE=15 and MAX_ANGLE_RANGE=165 to take only the data from that range
- When the obstacle vector module is less than the MAX_REPULSIVE_FORCE, that means that the obstacle is not close and we will multiply the linear velocity by a MULTIPLIER_LINEAR_VEL (in my case 1.5) to increase the velocity of the car.
FOURTH STEP: RESULTS ANALYSIS AND CONCLUSIONS:
After adjusting some of the parameters, and the lidar range we want to take values, I was able to complete the circuit in less time, getting a mayor security distance with the obstacles and not touching any of them (See video demonstration at the end of this post).
However, I was forced to decrease the MIN_LINEAR_VEL to guarantee security with the obstacles, so the car reduce a lot the linear velocity when he is too close of an obstacle.
This is the conclusions I got from my program:
- Key parameters such as 𝛂 and 𝝱 and the Lidar range for taking values are too important to adjust in order to get an optimal result in the average force and obstacle force.
- Setting a high DISTANCE_THRESHOLD for the lidar would make the sensor take the walls of the circuit as obstacles even if we are in the red line in a straight path, and that would make the Average force to have less module.
- The GUI Frequency value and the RTF have a huge impact in the result of the program simulation.
- The range of linear velocities for the car is also important. Using a small MIN_LINEAR_VEL is translated in slow laps, but also using a big value for MIN_LINEAR_VEL could also be negative when we are reaching some obstacles, specially if we apply this is real life, because deceleration time when we are about to crash into an obstacle will increase notably (due to inertia)
  VIDEO DEMONSTRATION OF THE CIRCUIT LAP
Alternative Link (via YouTube): https://youtu.be/rfKpM4dveDo