1️⃣Search-based Path Planning

Dijkstra's Algorithm

Let say we have a 8x8 Grid Map with Obstacles, where S is start, G is the goal, 1 is free space, X is obstacle.

S 1 1 1 1 1 1 1
1 X 1 X 1 1 1 1
1 X 1 X 1 X X 1
1 1 1 1 1 X 1 1
X X X X 1 X 1 1
1 1 1 X 1 1 1 1
1 X 1 1 1 X X 1
1 1 1 X 1 1 1 G
Python
import heapq

def dijkstra(grid, start, end):
    n = len(grid)
    m = len(grid[0])
    
    # Directions: up, down, left, right
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
    
    # Priority queue (min-heap) to store the nodes to explore
    pq = [(0, start)]  # (cost, (row, col))
    
    # Dictionary to store the minimum cost to reach each point
    costs = {start: 0}
    
    # Dictionary to store the parent of each node for path reconstruction
    parents = {start: None}
    
    while pq:
        current_cost, (current_row, current_col) = heapq.heappop(pq)
        
        # If we have reached the end point, reconstruct the path and return the cost and path
        if (current_row, current_col) == end:
            path = []
            node = end
            while node is not None:
                path.append(node)
                node = parents[node]
            path.reverse()
            return current_cost, path
        
        # Explore the neighbors
        for dr, dc in directions:
            next_row, next_col = current_row + dr, current_col + dc
            
            if 0 <= next_row < n and 0 <= next_col < m:
                if grid[next_row][next_col] != float('inf'):
                    next_cost = current_cost + grid[next_row][next_col]
                    if (next_row, next_col) not in costs or next_cost < costs[(next_row, next_col)]:
                        costs[(next_row, next_col)] = next_cost
                        parents[(next_row, next_col)] = (current_row, current_col)
                        heapq.heappush(pq, (next_cost, (next_row, next_col)))
    
    # If the end point is not reachable, return infinity or an indicator value
    return float('inf'), []

# Define the grid (8x8) with obstacles
grid = [
    [0, 1, 1, 1, 1, 1, 1, 1],
    [1, float('inf'), 1, float('inf'), 1, 1, 1, 1],
    [1, float('inf'), 1, float('inf'), 1, float('inf'), float('inf'), 1],
    [1, 1, 1, 1, 1, float('inf'), 1, 1],
    [float('inf'), float('inf'), float('inf'), float('inf'), 1, float('inf'), 1, 0],
    [1, 1, 1, float('inf'), 1, 1, 1, 1],
    [1, float('inf'), 1, 1, 1, float('inf'), float('inf'), 1],
    [1, 1, 1, float('inf'), 1, 1, 1, 1]
]

# Define start and end points
start = (0, 0)  # Top-left corner
end = (4, 7)    # Bottom-right corner

# Find the optimal path cost and path
optimal_cost, path = dijkstra(grid, start, end)
if optimal_cost == float('inf'):
    print(f"No path found from {start} to {end}.")
else:
    print(f"The optimal path cost from {start} to {end} is {optimal_cost}.")
    print("The path is:", path)
CPP
#include <iostream>
#include <vector>
#include <queue>
#include <stack>

using namespace std;

typedef pair<int, pair<int, int>> Node;

int dijkstra(const vector<vector<int>>& grid, pair<int, int> start, pair<int, int> end, vector<pair<int, int>>& path) {
    int m = grid.size();
    int n = grid[0].size();

    vector<pair<int, int>> directions = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}};

    // Priority queue min-heap 
    priority_queue<Node, vector<Node>, greater<Node>> pq;

    pq.push({0, start});

    // cost, initially very big INT_MAX
    vector<vector<int>> costs(n, vector<int>(m, INT_MAX));

    costs[start.first][start.second] = 0;

    vector<vector<bool>> visited(n, vector<bool>(m, false));

    

    vector<vector<pair<int, int>>> parents(n, vector<pair<int, int>>(m, {-1, -1}));

    while(!pq.empty()){
        int current_cost = pq.top().first;
        int current_row = pq.top().second.first;
        int current_col = pq.top().second.second;

        visited[current_row][current_col] = true;

        pq.pop();

        //if reach the end, reconstruct the path
        if(make_pair(current_row, current_col) == end){
            stack<pair<int, int>> s;
            pair<int, int> p =end;

            while( p != start){
                s.push(p);
                p = parents[p.first][p.second];
            }

            s.push(start);

            while(!s.empty()){
                path.push_back(s.top());
                s.pop();
            }

            return current_cost;
        }

        // Explore the neighbors
        for(const auto& direction : directions) {
            int next_row = current_row + direction.first;
            int next_col = current_col + direction.second;

            if(next_row >= 0 && next_row < n && next_col >= 0 && next_col < m && !visited[next_row][next_col]){
                if(grid[next_row][next_col] != INT_MAX) {
                    // in free space
                    int next_cost = current_cost + grid[next_row][next_col];
                    if(next_cost < costs[next_row][next_col]){
                        costs[next_row][next_col] = next_cost;
                        parents[next_row][next_col] = {current_row, current_col};
                        pq.push({next_cost, {next_row, next_col}});
                    }

                }
            }
        }

    }

    return INT_MAX;


}


int main() {
    // Define the grid (8x8) with obstacles
    vector<vector<int>> grid = {
        {0, 1, 1, 1, 1, 1, 1, 1},
        {1, INT_MAX, 1, INT_MAX, 1, 1, 1, 1},
        {1, INT_MAX, 1, INT_MAX, 1, INT_MAX, INT_MAX, 1},
        {1, 1, 1, 1, 1, INT_MAX, 1, 1},
        {INT_MAX, INT_MAX, INT_MAX, INT_MAX, 1, INT_MAX, 1, 1},
        {1, 1, 1, INT_MAX, 1, 1, 1, 1},
        {1, INT_MAX, 1, 1, 1, INT_MAX, INT_MAX, 1},
        {1, 1, 1, INT_MAX, 1, 1, 1, 0}
    };
    
    // Define start and end points
    pair<int, int> start = {0, 0};  // Top-left corner
    pair<int, int> end = {7, 7};    // Bottom-right corner
    
    // Vector to store the optimal path
    vector<pair<int, int>> path;
    
    // Find the optimal path cost
    int optimal_cost = dijkstra(grid, start, end, path);
    if (optimal_cost == INT_MAX) {
        cout << "No path found from (" << start.first << ", " << start.second << ") to (" << end.first << ", " << end.second << ")." << endl;
    } else {
        cout << "The optimal path cost from (" << start.first << ", " << start.second << ") to (" << end.first << ", " << end.second << ") is " << optimal_cost << "." << endl;
        cout << "The path is: ";
        for (const auto& p : path) {
            cout << "(" << p.first << ", " << p.second << ") ";
        }
        cout << endl;
    }
    
    return 0;
}

Pros

  • Complete and optimal

Cons

  • Can only see the cost accumulated so fat, thus exploring next state in every "direction"

  • No information about goal location

Complete vs Optimal

Complete

A path planning algorithm is complete if it is guaranteed to find a path if one exists. This means that if there is a way to reach the goal from the start, the algorithm will eventually discover it, regardless of the complexity of the environment or the obstacles present. If no path exists, a complete algorithm will confirm that as well.

Characteristics of Completeness:

  • Guarantee of Solution: If a path exists, the algorithm will find it.

  • Exhaustive Search: The algorithm explores the entire search space, ensuring that no possible paths are overlooked.

Optimal

A path planning algorithm is optimal if it is guaranteed to find the best possible path according to a specified metric, such as the shortest distance, the minimum time, or the least cost. The "best" path is defined by the optimization criterion chosen for the problem.

Characteristics of Optimality:

  • Best Path: The algorithm finds the path with the lowest cost according to the given cost function.

  • Comparative Evaluation: It evaluates multiple paths and selects the one that best meets the optimization criterion.

Examples

To illustrate, consider the following two popular algorithms:

  1. A Algorithm*:

    • Complete: A* is complete because it will eventually find a path if one exists, as long as the graph is finite and the cost of each step is positive.

    • Optimal: A* is optimal if the heuristic used is admissible (never overestimates the true cost) and consistent (satisfies the triangle inequality).

  2. Depth-First Search (DFS):

    • Complete: DFS is not complete in general, especially if the search space is infinite. It might get stuck in an infinite branch and never find the solution even if one exists.

    • Optimal: DFS is not optimal because it does not consider path costs and may find a longer or more expensive path when a shorter or cheaper one exists.

Summary

  • Complete Algorithm: Guarantees finding a path if one exists.

  • Optimal Algorithm: Guarantees finding the best path according to a specified metric.

  • Complete and Optimal Algorithm: Guarantees finding the best path if a path exists, combining the properties of both completeness and optimality.

A Star

CPP
#include <iostream>
#include <vector>
#include <queue>
#include <stack>

using namespace std;

typedef pair<int, pair<int, int>> Node;

int heuristic(pair<int, int> a, pair<int, int> b)
{
    return abs(a.first - b.first) + abs(a.second - b.second);
}

int a_star(const vector<vector<int>>& grid, pair<int, int> start, pair<int, int> goal, vector<pair<int, int>>& path) 
{
    int n = grid.size();
    int m = grid[0].size();

    vector<vector<int>> costs(n, vector<int>(m, INT_MAX));
    costs[start.first][start.second] = 0;

    vector<vector<bool>> visited(n, vector<bool>(m, false));

    priority_queue<Node, vector<Node>, greater<Node>> pq;
    pq.push({heuristic(start, goal), start });

    vector<pair<int, int>> directions = { {-1, 0}, {1, 0}, {0, -1}, {0, 1} };
    vector<vector<pair<int, int>>> parents(n, vector<pair<int, int>>(m, {-1, -1}));

    while (!pq.empty()) {
        
        int current_cost = pq.top().first;
        int current_row = pq.top().second.first;
        int current_col = pq.top().second.second;

        visited[current_row][current_col] = true;

        pq.pop();

        // if reach the goal
        if (make_pair(current_row, current_col) == goal) {
            stack < pair<int, int> > s;

            pair<int, int> n = { current_row, current_col };
            while (n != start) {
                s.push(n);
                n = parents[n.first][n.second];
            }

            s.push(start);
            while (!s.empty()) {
                path.push_back(s.top());
                s.pop();
            }
            return current_cost;
        }

        //explore neighbors
        for (auto direction : directions) {
            int next_row = current_row + direction.first;
            int next_col = current_col + direction.second;

            if (next_row >= 0 && next_row < n && next_col >= 0 && next_col < m && !visited[next_row][next_col]) 
            {
                if (grid[next_row][next_col] != INT_MAX) 
                {
                
                    // in free space
                    int next_cost = current_cost + grid[next_row][next_col] + heuristic(make_pair(next_row, next_col), goal);
                    if (next_cost < costs[next_row][next_col]) 
                    {
                        pq.push({ next_cost, {next_row, next_col} });
                        costs[next_row][next_col] = next_cost;
                        parents[next_row][next_col] = { current_row, current_col };
                    }

                }
            }    
        }
    }
    // no path found
    return INT_MAX;
}

int main()
{
    vector<vector<int>> grid = { {0, 1, 1, 1},
                                 {1, 1, INT_MAX, 1},
                                 {1, 1, 1, 1},
                                 {1, INT_MAX, 1, 0} };
    pair<int, int> start = { 0, 0 };
    pair<int, int> goal = { 3, 3 };

    vector<pair<int, int>> path;
    int optimal_cost = a_star(grid, start, goal, path);

    if (optimal_cost != INT_MAX) {
        cout << "optimal path found" << endl;

        for (auto& n : path) {
            cout << " (" << n.first << ", " << n.second << ") ";
        }
        cout << endl;
    }
    else 
    {
        cout << "no path found" << endl;
    }

    return 0;
}

Jump Point Search

Jump Point Search (JPS) is an optimization technique for improving the efficiency of the A* algorithm in grid-based pathfinding, particularly in motion planning. Here’s a detailed explanation of how it works and its significance:

Overview

In grid-based motion planning, the objective is to find the shortest path from a start point to a goal point while navigating around obstacles. The A* algorithm is commonly used for this purpose due to its optimality and efficiency. However, A* can be slow on large grids because it evaluates many nodes, especially in open areas. JPS addresses this inefficiency.

Key Concepts

  1. Pruning Unnecessary Nodes: JPS reduces the number of nodes A* needs to evaluate by "jumping" over multiple nodes in straight lines or diagonals until it reaches a node that requires evaluation. This significantly reduces the search space.

  2. Jump Points: A jump point is a critical node in the path. It's a node where a decision needs to be made, such as changing direction or encountering an obstacle. JPS skips nodes between jump points, thus accelerating the search process.

  3. Forced Neighbors: These are nodes that need to be considered because they represent potential paths around obstacles. When a node has forced neighbors, it becomes a jump point.

How Jump Point Search Works

  1. Straight-line Jumping: From a given node, JPS attempts to move in a straight line (either horizontally, vertically, or diagonally) until it reaches:

    • The goal node.

    • An obstacle.

    • A node with forced neighbors.

  2. Recursive Search: If a jump point is reached, JPS recursively searches from this new point. The recursion continues until the goal is found or no more jump points are available.

  3. Path Reconstruction: Similar to A*, JPS uses a parent pointer for each node to reconstruct the path once the goal is reached.

Benefits of Jump Point Search

  1. Efficiency: By reducing the number of nodes evaluated, JPS can significantly speed up the pathfinding process, especially in large, open areas.

  2. Optimality: JPS preserves the optimality of A*. It still guarantees finding the shortest path.

  3. Simplicity: Despite its sophisticated approach, JPS is relatively easy to implement and integrates well with existing A* frameworks.

Example

Consider a grid where we need to find the shortest path from the top-left corner to the bottom-right corner. Traditional A* would evaluate many nodes, including those in straight lines and around obstacles. JPS would jump directly along straight lines, only evaluating nodes where direction changes or obstacles are encountered, thereby reducing the number of evaluations and speeding up the search.

Applications

JPS is particularly useful in applications involving real-time pathfinding, such as:

  • Video games and simulations.

  • Robotics, for efficient navigation in known grid-like environments.

  • Geographic information systems (GIS) for route planning.

PreviousRule-basedNextSampling-based Path Planning

Last updated