Global Navigation + Gradient Path Planning

This is the blog of the P4-RM, where I programmed the behavior of a taxi based on Gradient Path Planning.

Objetive

The objective of this practice was to program the behavior of a taxi using gradient path planning, which generates a cost map and moves following the path of least cost until reaching a target.

Coding

Libraries

• This provides functions for working in domain of linear algebra, fourier transform, matrices and arrays.
  

import numpy as np

Implementation

Cost Map

The creation of the cost map consists of three distinct functions: one that creates the simple cost map, another that uses this same cost map to expand the obstacles and penalize the taxi for getting too close to them, and a third function that normalizes the costs.

def navigate_to_target(cost_grid, robot_map, target_map):

    ...



    while robot_map != target_map:

        # Neighbors of the robot

        neighbors = [(robot_map[0] - 1, robot_map[1]), (robot_map[0] + 1, robot_map[1]),

                     (robot_map[0], robot_map[1] - 1), (robot_map[0], robot_map[1] + 1),

                     (robot_map[0] - 1, robot_map[1] - 1), (robot_map[0] + 1, robot_map[1] + 1),

                     (robot_map[0] - 1, robot_map[1] + 1), (robot_map[0] + 1, robot_map[1] - 1)]



        ...



        next_step = min(valid_neighbors, key=lambda x: cost_grid[x[0], x[1]])



        ...

        world_point = GUI.gridToWorld((next_step[1], next_step[0]))



        while True:

            ...

            target_x, target_y = world_point

            direction = np.arctan2(target_y - robot_y, target_x - robot_x)

            angle_diff = direction - robot_theta

            angle_diff = np.arctan2(np.sin(angle_diff), np.cos(angle_diff))



            if abs(angle_diff) > 0.1:

                HAL.setW(angle_diff * 2)

                HAL.setV(0)

            else:

                HAL.setW(0)

                HAL.setV(5)

        ...



        robot_map = next_step

Difficulties

• One of the main problems I encountered was during navigation. There were times when two adjacent cells had the same cost or were surrounded by higher-cost cells, which caused the robot to get stuck in a constant loop. I solved this by prohibiting the robot from returning to a previous cell.


• Another problem I encountered was that when converting from world to grid, the x and y coordinates were inverted, as x referred to the columns and y to the rows.

Logbook

• 
  Day 1: Test the world and implement conversions from world coordinates to grid.


• 
  Day 2: Create a simple cost map.


• 
  Day 3: Enhance the cost map by expanding obstacles.


• 
  Day 4: Normalize the cost map and finalize its generation when it reaches the taxi.


• 
  Day 5: Implement navigation.

Functionality

Simple test

(In case the video doesn´t play try this link: )

Changing target in the middle of the ride

The taxi reaches the first target before changing to the next one.





(In case the video doesn´t play try this link: https://youtu.be/hGz6yQENSe0)