TASK 3: LOCAL NAVIGATION WITH VFF

 INTRODUCTION: 

The objective of this task is to implement the logic of the VFF navigation algorithm.

The VFF consists of a type of local navigation with repulsive force in obstacles and attractive force in destination. Calculate robot situation every time. To do so, I have decided to implement VFF navigation algorithm following the next steps:

PROCESSING IMAGE:

In this section just to mention that I use getImage() and showImage(img) to see what is watching the pilot and it has helped me to debug some errors.

 FORCES: 

• ATTRACTIVE FORCE VECTOR:

As I said in the introduction, this algorithm needs an attractive force in destination. To get that I followed the next steps:

By the functions given, you can know how to get the coords (x and y) of the target we are looking for: current_target.getPose().x and current_target.getPose().y .

Besides, you can also know the coords  and the orientation of the robot (these functions are given in the statement): HAL.getPose3d().x, HAL.getPose3d().y and HAL.getPose3d().yaw.

But, it is necessary to transform those absolute coordinates into relative to know the coords of the target respect to the robot. It has dealt using the function called: absolute2relative (that function is given in the statement but return x, y instead of x and y because the second one just returns x).

At this point, we have the coords of the target respect to the robot, but, how can we get the distance to that point? 

It is calculating the vector's module:

Example:

Target's position (2, 3) (obtained from the function absolute2relative)

It has the following formula: np.sqrt(x² +y²)

 

 

The distance from robot to the target in this case is the 3.61.

This image has been done using Geogebra.  

 

Now, I assume that if the distance if less than 3, set as reached destination and I get the following target.

Also, I take into account that if the target is too far away (farther than 8) , set a constant value for the vector. I make this to make the attractive behaviour. 

Let's remember how attraction works: 

    - Far -> constant 

    - Close -> decreases

So, when it is closer than 8, I use coords target.

The attractive force has an alpha value which multiplies the attract vector. That alpha value is 0.7 which was obtained by experimenting. But, that was not enough, I could not control the force so I had to set some boundaries to eliminate that inefficient execution. That value is always the same and to behave as the VFF, it is multiplied by the alpha. 

That boundaries are set in the four possible situations ( x and y positive, x and y negative, x positive and y negative, x negative and y positive).

The only thing left to do is to represent the force in the screen. I decided to use the green arrow.  

• REPULSIVE FORCE VECTOR:

Now, the algorithm needs a repulsive force in the obstacles.  To do so. I have done the following:

 

Let's remember how repulsion works: 

    - Far -> decreases

    - Close -> increases

In this part, I need to detect the obstacles so I need the laser and process its values. I used getlaserData() and parse_laser_data(laser_data) to get an array of each laser_distance and laser_angle.

 

In parse_laser_data(laser_data) I have decided to use the one given in the statement but I changed when calculating the angle  i-90  to just i just because, is the same function given when using the vacuum and those values are familiar with me.

Now, let's vectorize the laser and that function I obtained from the vacuum but I have just modified the distance because the value was so big.

That vector mean should be modified to make a repulsive force.

 

To do so, I have created the function called get_repulsive_force(laser) which modifies the vector to work as a repulsive force; that is making a 270º rotation.

 

Example: 

Using the same point as the other example, its rotation will be:

             

 
 This image has been done using Geogebra. 

 

The repulsive force has a beta value which multiplies the repulsive vector. That beta value is 8 which was obtained by experimenting.

 

The last thing to do is to represent the force in the screen. I decided to use the red arrow. 

 

• RESULTANT FORCE VECTOR:  

This vector is just the sum of the attractive and repulsive vector and births the equation of VFF:

   

In the screen it is represented with the black arrow.

VELOCITIES:

After getting the resultant force, the last thing to do is translating that resultant force into velocities. I decided to make a conversion from cartesian to polar coordinates. 

That polar coordinate are the velocities. polar[0] = linear and polar[1] = angular. 

CONCLUSION:

The f1 takes about 5 mins to complete the lap as you can see in the video, down below.

VIDEOS:

- Complete lap video:

-Gazebo simulation:

 

I hope you liked them!