Global Navigation using Gradient Path Planning (GPP)

-

 Global Navigation using Gradient Path Planning (GPP)

The goal of this project is to make able to find an optimal path from the current place of a Taxi to the goal place using Gradient Path Planning (GPP) algorithm in a city map:

The GPP algorithm consist of creating a field in the target position that spread among the map using a search algorithms (such as BFS, A*, etc...) without crossing the obstacles of the map. The "wave" stops when we reach the initial position.

FIRST STEP: CREATING THE GPP FIELD EXPANSION

As we are using a Grid Map (The image returned from MAP.getMap() is a numpy matrix), each of the position of the matrix represents a "node". We will start the BFS in the node (matrix position) of the clicked point. Using GUI.getTargetPose() and then MAP.rowColumn(targetPoseInWorld) we can transform the coordinates of the target point from the world to the image matrix. We will do the same with the Taxi coordinates in order to work all the time using the MAP reference.

In order to create the field that expands from the target point to the taxi position, we need to choose which search algorithm we want to use. As recommendation, if we want to simulate the wave that expands "equally in breadth" of the map, we must use the BFS algorithm (Breadth First Search).

The BFS algorithm steps are:

  1. When the user clicks in a point, push in a Queue (we will call it "Frontier") the coordinates of that point and the cost of that initial node and we will store it in a visited nodes list. That cost is defined by INITIAL_COST_NODE.
  2. While the frontier queue is not empty we will:

    1. pop item from that queue (node coordinates and coordinates)
    2. Check if it is the taxi node
    3. If it is the taxi node and EXTRA_SECS_GRADIENT_EXPAND seconds has passed, stop iteration and start the navigation algorithm.
    4. If the node is an obstacle, store it in a obstacle list
    5. If isn't, get neighbors nodes of that node and their cost. If the successor node is inside the map, update the matrix created to represent the Gradient and, if that node has not been visited, store it in the frontier and visited list with the updated cost (this is, the total cost of moving from the target position to that cost)
    6. Back to point 2

We will define the neighbors or successor nodes of a specific node as the four nodes that are at his north, east, south and west and the four that are diagonal to him, + the successors of this successors (this is, the nodes that are at 2 blocks of distance, in diagonal or in perpendicular direction of the original node). We won't consider any of this successors if they are in an invalid position. This is, if it represents an obstacle node of the global map, or if they are just 1 or 2 blocks away from any obstacle. We will explain this in detail in the next section.

NOTE: We should use an if in the main while True and do the algorithm like that, but the iterations are too slow and the gradient will expand really slowly. Using an inside while instead will make the gradient expand faster. Take this in consider if we want to apply this in a ROS scene (using a while inside the main while will be translated in other ROS nodes to be blocked).

Here's an example of how the Gradient expands:

In order to make the interface more expressive in the gradient path expansion, we will modify how the gradient looks while the expansion. We will switch the colors of the paths/obstacles, and we will represent in the gradient expansion how the cost of the successive neighbor nodes increase gradually:

SECOND STEP: ADD PENALTY COST TO NODES CLOSE TO OBSTACLES

If we want to apply this in a real scene we will need to make sure the taxi doesn't go too close to some obstacles or walls of the city. When the Gradient expansion finish, we must create a path that have a security distance to the obstacles and walls of the city. In order to do that, we will add a penalty cost to that nodes that are one block and two blocks away from an obstacle, considering this nodes as obstacles (invalid nodes). The value of that penalty is defined by the ONE_BLOCK_PENALTY and TWO_BLOCKS_PENALTY constants, respectively. To calculate this, we will create  a function called getPenalty(node).

Now, the total cost of moving to a node of the map will be:

totalSuccCost = currentCost + succCost + getPenalty(succNode)

Where currentCost is the current cost we have in that moment, the nodeCost is either ADJACENT_NODE_COST or DIAGONAL_NODE_COST and the getPenalty(succNode) is the value of penalty of that node.

For the matrix gradient representation, the color value of each of the positions of the gradient matrix will be the:

valueRepresented = succCost + getPenalty(succNode) + getDistance(succNode)

Where getDistance(succNode) is the distance from that node to the target (where the gradient starts). The reason of not using the totalSuccCost for the matrix value representation is for better visual results and comprehension. Note that valueRepresented must be less or equal to WHITE_VALUE (255). If not, we will set the value of that point to the value of the white color, 255.

Here's the final look of the gradient expansion in our program:

THIRD STEP: CREATE PATH FROM TAXI TO TARGET POINT

Once the gradient stops expanding and we have reached the taxi coordinates in the map, we will show in the gradient image the approximated path that the taxi is going to make in order to reach the target. In order to do this, we will transform the matrix used for the gradient (that was in gray scale) to a new one in BGR. We will create a new function, drawPath(), that will draw in the new colored matrix the path to be followed and will circle either the target and the taxi points in the map:

Notice that this implementation just show an APPROXIMATION of the path followed by the taxi. In no case it will influence in the final path followed by the taxi

FOURTH STEP: NAVIGATION TO THE TARGET POINT

The final step is to make able the taxi to drive over the city given the sequence of successors nodes from his current position to the target position. In order to handle correctly the direction of the car we will work with a relative coordinates system solidarity with the car. The origin will be always at the coordinates of the taxi. The orientation of the taxi will define the X and Y axis, as shown in the following picture:


Using this relative system allow us to find if we pass one of our sub-target points in an efficient way. Also, allow us to find the angle between the car and the target point really easy, using basic trigonometry.

The linear and angular velocities for the car will be the distance and the angle from the car to the sub-target point in polar coordinates. For security reasons, we will set a maximum and minimal linear and angular velocity for the car, so we avoid taking risks than can make our car turn too close from the walls.

Also, we won't use the sequence path of successors nodes from the taxi point to the target point. We will take only some of the nodes of that sequence path to make a path of less sub-targets, increasing the velocity of the car and making an optimal navigation algorithm. The SUB_TARGETS_DISTANCE will define the distance between the nodes that make-up the sequence path to take in consideration in the navigation final path.

NOTE: The navigation algorithm is also implemented in a while loop to get more reactivity in the movements of the car.

FINAL RESULTS AND ANALYSIS:

Once we have finished the program, we need to study the obtained results. Here's the most important points to take in consider:

  • As the car is not holonomic (can't turn without 'moving' of the same place) , simulating the turn of a real car makes the navigation a little bit risky if the car needs to turn in narrow streets. 

  • Increasing the MIN_LIN_VEL and MAX_LIN_VEL can make the navigation faster but also less safety. Taking a high value for the MAX_LIN_VEL in turns can make our car hit some walls during the turning-direction moment.

  • SUB_TARGETS_DISTANCE must have an well-balance value. Having a too low value can make our car to not be able to go to some paths, specially at the beginning of the navigation if we have high values for the velocities. In the other hand, using high values for this constant will affect in the transition between sub-targets, and could make the car hit the walls during the transition between changing from a sub-target after a linear path to a new sub-target that its behind a corner (the more value in this constant, before will the car start turning).

DEMONSTRATION VIDEOS:

  • Example #1:

 
Alternative link (via YouTube):

  • Example #2:



Alternative link (via YouTube):

Comments

Post a Comment

Popular posts from this blog

Vacuum Cleaner

-
Image
 Vacuum Cleaner The goal of this project is be able to make a low-end vacuum cleaner. This cleaner would have to ensure to clean as much area as possible of a predetermined map that simulates a house. Since the goal is to emulate the behavior of the ones that are in the client market (of low-end), the vacuum cleaner must cover a good area in a reasonable time period: To achieve the objective, three different functionalities are presented using finite state machines (FSM). In this post we will explain all of them and we will analyze which one got the best results. First strategy: Bump & Go In this first approximation we are using a FSM of 4 states: going forward, going backwards, and turning left or right, depending on which side the bumper hits: The robot begins with "FORWARD" as initial state. In the moment he hits a wall, he will change his state to "BACKWARD", and he would start moving with the same velocity as he had in forward state, but now with negative ...
Read more

Obstacle Avoidance using VFF

-
Image
 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 o...
Read more