MOBILE ROBOTIC
Robotic and software engineering URJC
GLOBAL NAVIGATION
In this exercise our target is arrive at a destination selected with our robot, that in this case will be a taxi. We have to move it with the algorithm of Gradient Path Planning (GPP).
This algorithm consists in 3 phases. The first two, which are create the grid and calculate the path, will be executed only once, and the third one, which is move the taxi, will be iterative.
FIRST PHASE
First of all, we have to see the map as a grid. This grid will be filled with numbers, starting in the destination, that will have a value of 0, and finishing on the taxi position, with a higher value.
​
To obtain the final grid, we have to take into account two grids: the expansion grid and the obstacles grid.
EXPANSION GRID
The expansion grid is the gradient expansion, which give meaning to this algorithm. To do this expansion, we have to start in the destination. This cell will have the value of 0. Now, we have to expand this value to all the neighbors, adding 1 to the current value if the neighbor is up, down, left or right, and adding √2 if are diagonals, because there is more distance.
This first expansion will look like this:
Then, we will expand the 8 cells added, then the neighbors of these cells, increasing the wave front, which are the current cells that we have to expand, and increasing the values of the grid.
We have to think in two possible situations if we are going to expand in a cell that has been already written. Let's see both in these pictures to see clearly.
We are going to expand first √2 of the left up corner. At the right neighbor, we would have to wrote √2+1, but the value that is already in the cell is lower: we don't have to change it, because it means that there is already a shortest way to arrive to that point. It's the same with the down and right down corner.
Once we have finished with the corner, let's expand the upper cell:
In this case, we see that the value to write un the upper cell of the red one (1+1) is lower than the current (√2+√2), so we are going to modify it, because, as we don't have to change if is higher for the same reason, this means that there is a shortest way to arrive that cell.
Once we have already understand this, we can finish the expansion, which would look like this:
But... when should we stop? We have to stop when we have arrived to the taxi's cell. To give it more security, we can expand an extra number of wave front just in case the car has other orientation.
OBSTACLES GRID
The next grid we have to do is the obstacles grid. Apart from have where the obstacles are, cause we can't calculate a possible path if there is any, we have to "expand" these obstacles to be sure that our taxi won't collide and will have enough space to move and turn without any problems. To do this, starting from the image of the map, we are going to set as 255 all the obstacles that we will find in this image, and expanding those 3 more times.
At the left we can see the initial image of the map, only with the obstacles, and at right the map after apply the obstacles expansion:
FINAL GRID
Finally, we have to join the two grids into the final one, where we will have the gradient expansion with the obstacles expansion, which will let us calculate the path for the taxi.
SECOND PHASE
In the second phase we have to calculate the path. Having the final grid, is easy to calculate. Starting in the taxi's cell, we only have to save the neighbor with the minimum value in the path array, so will let us arrive to the destination, which have the value of 0, in the shortest way.
We can see the result, which is the shortest path in these examples. In addition, we can also see the grid expansion in the grid field window, where the whitest color is represented with the highest values of the grid.
THIRD PHASE
When we already have the path, we only have to move the taxi. To do this, we have to know that the coordinates of the path that we have, are in the grid world, and our robot works in the real world, so first of all, we have to convert each position of the path into the real world. We have a path with all the cells we have to go to arrive our destination. Since the cells are neighbors to each other, we can skip one to give more fluency to the movement. To be sure that we arrive to the final point and not the previous one, we will always add the last point of the path array.
​
We have the target point, which will be the first of the path array, and will happen to be next when it is reached. now we have to focus on how should we move the taxi. We have the position x, position y and the orientation of the taxi, and the position x and position y of the target in absolute coordinates.
​
Knowing the absolute coordinates we can transform them into relatives to taxi’s system, so will let us calculate the angular velocity. Calculating the direction vector the direction vector, we will be able to calculate how much distance is between the taxi and the target.
So, since the horizontal axis are y, and the vertical one x, we are interested of how much distance, in the horizontal axis is the target. The more distance, the more value to the angular velocity, because this means that the angle is bigger. We can set a constant linear velocity and a proportional to y to the angular velocity, setting it the maximum value for the angular multiplied by this difference.
​
​
We already have the grid, the path, and the movement. I've added and extra situation, where we chose as an destination an obstacle: in this situation we notify the error and exit:
​
Once finished, we can execute the code and see the results: