Building a drone navigation system using matplotlib and A* algorithm

Have you ever wondered how drones navigate through complex environments? In this blog, we’ll create a simple drone navigation system using Python, Matplotlib, and the A* algorithm. By the end, you’ll have a working system that visualizes a drone solving a maze!

What You'll Learn

Basic AI terminologies like "agent" and "environment."
How to create and visualize a maze with Python.
How the A* algorithm works to solve navigation problems.
How to implement and visualize the drone's path.

Introduction

To build our drone navigation system, we need the following:

An agent: The drone 🛸.
A path: A 2D maze that the drone will navigate through 🛣️.
A search algorithm: The A* algorithm ⭐.

But first, let’s quickly review some basic AI terms for those who are new.

Key AI Terms

Agent: An entity (like our drone) that perceives its environment (maze) and takes actions to achieve a goal (reaching the end of the maze).
Environment: The world in which the agent operates, here represented as a 2D maze.
Heuristic: A rule of thumb or an estimate used to guide the search (like measuring distance to the goal).

The System Design

Our drone will navigate a 2D maze. The maze will consist of:

Walls (impassable regions represented by 1s).
Paths (open spaces represented by 0s).

The drone’s objectives:

Avoid walls.🧱
Reach the end of the path.🔚

Here’s what the maze looks like:

Step 1: Setting Up the Maze

Import Required Libraries

First, install and import the required libraries:

import matplotlib.pyplot as plt
import numpy as np
import random
import math
from heapq import heappop, heappush

Define Maze Dimensions

Let’s define the maze size:
python WIDTH, HEIGHT = 22, 22

Set Directions and Weights

In real-world navigation, movement in different directions can have varying costs. For example, moving north might be harder than moving east.

DIRECTIONAL_WEIGHTS = {'N': 1.2, 'S': 1.0, 'E': 1.5, 'W': 1.3}
DIRECTIONS = {'N': (-1, 0), 'S': (1, 0), 'E': (0, 1), 'W': (0, -1)}

Initialize the Maze Grid

We start with a grid filled with walls (1s):

maze = np.ones((2 * WIDTH + 1, 2 * HEIGHT + 1), dtype=int)

The numpy. ones() function is used to create a new array of given shape and type, filled with ones... useful in initializing an array with default values.

Step 2: Carving the Maze

Now let's define a function that will "carve" out paths in your maze which is right now initialized with just walls

def carve(x, y):
    maze[2 * x + 1, 2 * y + 1] = 0  # Mark current cell as a path
    directions = list(DIRECTIONS.items())
    random.shuffle(directions)  # Randomize directions

    for _, (dx, dy) in directions:
        nx, ny = x + dx, y + dy
        if 0 <= nx < WIDTH and 0 <= ny < HEIGHT and maze[2 * nx + 1, 2 * ny + 1] == 1:
            maze[2 * x + 1 + dx, 2 * y + 1 + dy] = 0
            carve(nx, ny)

carve(0, 0)  # Start carving from the top-left corner

Define Start and End Points

start = (1, 1)
end = (2 * WIDTH - 1, 2 * HEIGHT - 1)
maze[start] = 0
maze[end] = 0

Step 3: Visualizing the Maze

Use Matplotlib to display the maze:

fig, ax = plt.subplots(figsize=(8, 6))
ax.imshow(maze, cmap='binary', interpolation='nearest')
ax.set_title("2D Maze")
plt.show()

Step 4: Solving the Maze with A*

The A* algorithm finds the shortest path in a weighted maze using a combination of path cost and heuristic.

Define the Heuristic

We use the Euclidean distance as our heuristic:

def heuristic(a, b):
    return math.sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2)

**A* Algorithm Implementation**

def a_star(maze, start, end):
    open_set = []
    heappush(open_set, (0, start))
    came_from = {}
    cost_so_far = {start: 0}

    while open_set:
        _, current = heappop(open_set)

        if current == end:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            return path[::-1]

        for direction, (dx, dy) in DIRECTIONS.items():
            nx, ny = current[0] + 2 * dx, current[1] + 2 * dy
            mx, my = current[0] + dx, current[1] + dy
            new_position = (nx, ny)

            if (0 <= nx < maze.shape[0] and 0 <= ny < maze.shape[1] and
                    maze[nx, ny] == 0 and maze[mx, my] == 0):
                new_cost = cost_so_far[current] + DIRECTIONAL_WEIGHTS[direction]

                if new_position not in cost_so_far or new_cost < cost_so_far[new_position]:
                    cost_so_far[new_position] = new_cost
                    priority = new_cost + heuristic(new_position, end)
                    heappush(open_set, (priority, new_position))
                    came_from[new_position] = current

    return None  # Return None if no path is found

Step 5: Visualizing the Solution

We've got the maze but you can't yet see the drone's path yet.
Lets visualize the drone’s path:

path = a_star(maze, start, end)

if path:
    path_x, path_y = zip(*path)
    fig, ax = plt.subplots(figsize=(8, 6))
    ax.imshow(maze, cmap='binary', interpolation='nearest')
    ax.plot(path_y, path_x, color="blue", linewidth=2, label="Drone Path")
    ax.legend()
    plt.show()

Conclusion

Congratulations! 🎉 You’ve built a working drone navigation system that:

Generates a 2D maze.
Solves it using the A* algorithm.
Visualizes the shortest path.

Next Steps

Experiment with different maze sizes and weights.
Try other heuristics like Manhattan distance.
Visualize a 3D maze for more complexity!

Feel free to share your results or ask questions in the comments below.
To infinity and beyond 🛸

Blog