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 library provides access to mathematical functions and constants.

import math

This library 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.

MAP_URL = '/resources/exercises/global_navigation/images/cityLargenBin.png'
city_map = GUI.getMap(MAP_URL)

def normalize_cost(cost_grid, robot_map, target_map):
    ... 
    normalized_grid = np.full_like(cost_grid, np.inf)
    normalized_grid[finite_mask] = (cost_grid[finite_mask] / max_cost) * 255

    return normalized_grid

def show_cost(target_map, robot_map, city_map):
    cost_grid = np.full(city_map.shape, np.inf)
    cost_grid[target_map[0], target_map[1]] = 0
    queue = [(target_map[0], target_map[1])]

    ...

    # Normalize
    normalized_grid = normalize_cost(cost_grid, robot_map, target_map)
    return normalized_grid

def expand_obstacle(cost_grid, city_map, cushion_size=3, cushion_cost=255):
    ...

    return expanded_grid

Navigation

The car navigates based on the gradient, it obtains the cost map and analyzes its 8 neighboring cells. These neighbors can range from unreachable (not expanded or obstacles) to cells with varying costs. Among the latter, the taxi will move toward the cell with the lowest cost. This makes the navigation semi-reactive.

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]])

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

        while distance > 0.3:
            ...
            target_grid_x, target_grid_y = next_grid
            direction = math.atan2(distance_y, distance_x)
            angle_diff = math.atan2(math.sin(direction - robot_orientation), math.cos(direction - robot_orientation))

            linear_speed = 6 if abs(angle_diff) < 0.2 else 0
            angular_speed = 0 if abs(angle_diff) < 0.2 else angle_diff * 2

            HAL.setV(linear_speed)
            HAL.setW(angular_speed)
        ...

        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: https://youtu.be/wn9l0Gxg-0M)

Test long ride

I placed the first target in the bottom-left corner and the second one in the top-right corner to observe the taxi's behavior during a very long ride.

(In case the video doesn´t play try this link: https://youtu.be/45cWqGK-cH4)

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)

Blog