In this proyect, I have programmed an Autonomous Taxi using Gradient Path Planning (GPP). But, you will probably think, "What is Gradient Path Planning?"
Gradient Path Planning is a global navigation technique which consists of expanding a wave from destiny to actual position of robot in the map. the grids with greater intensity will be closer to the destination, so the robot will have to navigate following the cells with greater intensity.
the project is divided into three phases:
1. Create the wave
2. Trace the route
3. Follow the route
CREATE THE WAVE
I did an example before in a different program. The frontier behaves like a FIFO queue:
The initial point is (3, 3) and we want to reach the point (5,5). The output is the following
with Frontier:
(1, 1) (1, 2) (1, 3) (1, 4) (2, 1) (2, 4) (2, 5) (3, 1) (3, 5) (4, 1) (4, 2) (4, 5) (5, 2) (5, 3) (5, 4) (5, 5)
With this idea in mind we will develop an algorithm that it will begin in the final point and it will be expanding the Wave Frontier until the taxi point is in the frontier.
def expand_frontier(frontier, ... ...):
point = frontier.pop()
# obtaining_value ....
....
expand_horizontal_vertical(point, frontier ...)
expand_diagonal(point, frontier, ...)
while objetive not in frontier:
expand_frontier(frontier, ... ...)
while cont < NUM:
# We expand a little more
expand_frontier(frontier, ..., ...)
...Once we have obtained the point in the frontier, we expand a little more to give the car movement capacity.
The algorithm is similar to BFS algorithm that uses a FIFO queue as a frontier, but instead of being a frontier of nodes we create a frontier of points and we don't have to worry about tracking parent nodes.
When we do push, we analyze the following cases:
A point will be in frontier if and only if:
- The new (x, y) point is not in frontier.
- The (x , y) point does not exceed the limits of the grid.
- The (x, y) point is not an obstacle.
- The (x, y) point is the initial point of expansion.
- the (x, y) point has already been placed with a lower value in a previous iteration.
When a point belongs to the frontier we draw it on the grid.
If a point is an obstacle, we are saving it in a list of obstacles. Once we have expanded the wave, the next step is enhance the obstacles. For that, we iterate over Obstacles List and paint each element white its neighbour points and with a depth of 2:
def enhance_obstacle(x, y, grid):
draw_obstacle(x, y, grid)
for i in range(1, 2):
draw_obstacle(x+i, y, grid)
draw_obstacle(x+i, y+i, grid)
draw_obstacle(x-i, y, grid)
... ... ...In the following video, I show you the result of the wave
2. TRACE THE ROUTE
The next objective is to follow the cells with decreasing weights. For example, let's imagine that the current position is determined by the red dot, our incremental route are the pink cells , and the route that we would expect is the the blue one. Each white cell is a point on the road, that it has a weight obtained from the previous expansion. Then, if the current point of the incremental route is the red, we must to watch around us (green cells) and looking for the point with the minimum weight (the next blue cell).
For this we have implemented a dictionary for each new neighbour point to the current point that we add to the route. We iterate over the dictionary and look for the point with the minimum weight.
We will finish the creation of the route when we reach the last point next to the objective
Once, we have the complete route, we will draw the route in the map, using the method
self.grid.setPathVal(x, y, val)Here is the result:
3. FOLLOW THE ROUTE
In this final step, We take one point out of every 7 contiguous on the route, to prevent the car from making abrupt movements. Once we have one local point (local destiny) we make an attraction vector that will guide the car to the local objective. Is similar to VFF algorithm but in this case we have not a repulsion vector. Thanks to the widening of obstacles, the car will not crash.
Once we reach a non-existent local objective, we will have reached the destination.
Here is the FINAL RESULT: