Global Navigation Development

 A new challenge has arrived!!. This time we have to make our car be able to navigate around the city using only its map. For this we are going to use the Gradient path planning.

The Gradient path planning is an algortihm in wich we can find the best way to reach our objective. This is made by dividing the map in cells, this cells store a value that will help us to know where is better to move. As the value is bigger, worse is that cell to complete the objective so worse is the idea to move the car to that position.

So the first thing to do is to create this grid of cells to be able to navigate. To build this grid im going to make two extra grids, one grid to indicate where are the objects, the other will be the expansion grid, both im going to explain later. The result of the sum of this two grids is the final grid which will help to move the car correctly.

Lets explain both grids:

OBSTACLE GRID

As i mentioned before this grid indicates where are the objects so in those cells we are going to assign, in the final grid more weight so the car wont choose thouse cells as an option.

To start making this grid i use the function grid.getMap() to get the map of the city. As is explained in the Jderobot web page this will return the map image, where the positions with 0 represents the obstacles and the cells with 255 are the road, the values that appears in between those are values that represents how close are you to an obstacle.

So the first i made was to fill mi obstacle grid with the information bringed by the getMap function. But to make easer im going to invert the values of the getMap, now the cells with 0 are empty cells and the ones with a number different are ocuupied. Also the cells from getMap which had a number between 0 and 255, are set as 255 in my grid, with this i make the obstacles thiker and generate more repulsion. Apart of this i expand the obstacle repulsion one cell more to avoid that the car could drive too close to a wall.

This is how it looks one of the streets of our city on the obstacle grid:

Now that we have the obstacle grid lets make the expansion grid.

EXPANSION GRID

This grid will tell us how far is the car from the traget, it will expand as a round wave, the center of this round wave is the target of the car and it will expand util it reaches the position of the car.

Its behaviour is like this:

Image from the exercise global navigation of JdeRobot

We have the wave which is going to explore new cells and assign them the gradient number, this number represents the real distance from the cell to the objetive.

For this wave i have an array with the frontier of my current point, in the next iteration i look all the position of this array and expand one cell more, this new position expanded is added to a new wave front and i remove the positions of the last frontier.

The diagonal cells of 0 have a bigger gradient becouse if the cells of up,down,right,left have a value of 1 the diagonals are:

To make this grid i consider three things:

1.The cells which  has values in the obstacle grid of 255, wont be added to the expansion wave, because in those places is inecesary calculate the gradient, the car never has to cross those cells.

2.I only added the gradient number if the cell in which im going to assign has a value of 0 or bigger than the number i want to give. This is to avoid reach local minima.

3.Once the expand wave reaches the position of the car, i expand a little bit more to prevent the car pass the grid and reach a local minima.

FINAL GRID

As i said before the final grid is the sum of the last two grids. So at the same time that i make the expansion grid im building the final grid adding both values to the final grid.

Now that we have our final grid is time to search for the best way to reach the cars objective. To build this path i start looking the values of the positions next to the car, and only save the one with the lowest value. Once i have the lowest value next to the car i search for the lowest value next to that position found. Making this recursively we will reach the target point which has the lowest value of all, 0.

Adding this values with grid.setPathFinded() we can see the path generated in colour green in our map.

Here is a demostration of how it works:

As we can see the path generated is enought separated from the walls to dont touch them.

MOVE CAR

Finally is time to move the car, to make the car reach the final destiny. To do this we cant put as an objective to the car reach the final point, becouse it will crash with everything, it doesn't now where are the obstacles. So to avoid this we have to attack the problem in smaller parts. How to do that? well, instead of try that our car reach the final target lets try to reach closer points. So we divide the hole path found in few target point that if we link all of them we will lead the car to reach our objective.

I get this points in the same function that gets the final path so for five minima points found i save one in an array that save the small target points to the car. The last position added is, of course, the final destiny.

With this we have now to know the distance from the car to this near target point. 

To do this first we have to change the point saved in the array, becouse the point of the array is in grid coordinates and we need it in the world cordinates to calculate correctly the distance.

It has not dificult using gridToWorld(gridX, gridY)  function.

Once we get this it is as easy as calculate the diference between both points, but if we want to move the car correctly we have to consider the orientation of the car, we can do this with this:

                            x_rel = dx * math.cos (-theta) - dy * math.sin (-theta)

                            y_rel = dx * math.sin (-theta) + dy * math.cos (-theta)

Where dx is the diference between the points (car position and target) in x and dy the same but in y.

Now that we now the diference between our point and the target point, i set the lineal velocity to 4 an make the angular velocity proportional to y_rel.

This are some videos of the final result:

Note: Although i have a constant lineal velocity in some zones (the most of them near a wall) the car go really slow, i dont know exactly why is this happening.