Mapping & Path Planning in Mobile Robotics: Webots Python Tutorial

Introduction to Mapping and Path Planning

Mapping and path planning are core competencies in modern robotics, enabling autonomous navigation in structured environments. Whether it's a delivery robot in a warehouse or a drone surveying a disaster zone, the ability to build a map and compute efficient routes is essential. In this tutorial, we'll explore these concepts using the Webots 2023b simulator and Python, focusing on the FAIRIS robot platform. You'll learn how to integrate encoder, compass, lidar, and camera data to create a world model, localize the robot, and plan paths with the Wavefront Planner algorithm.

Sensor Integration for Environmental Awareness

To navigate effectively, a robot must perceive its surroundings. The FAIRIS robot provides four key sensors: encoders for odometry, a compass for orientation, lidar for 360° distance measurements, and a camera with object recognition. Each sensor contributes unique data: encoders track wheel rotations to estimate displacement, the compass gives heading relative to the world frame, lidar builds a map of obstacles, and the camera identifies landmarks like colored cylinders. In a real-world scenario, think of this as similar to how a delivery drone uses GPS (compass), cameras, and LIDAR to avoid trees and find your doorstep. By combining these sensors, the robot can correct drift and build a reliable map.

Odometry and Compass: Dead Reckoning with Correction

Start by using encoders to compute the robot's position change. Each encoder tick corresponds to a known wheel rotation, allowing you to calculate distance traveled. The compass provides absolute orientation, which helps correct heading errors. For example, if your robot turns 90° but the compass shows 88°, you can adjust. This is like using a step counter and a compass while hiking—useful but prone to drift. To improve accuracy, fuse encoder and compass data with a simple weighted average or a Kalman filter.

# Example: Compute position from encoders and compass
import math
# Assume left and right encoder distances (in meters)
d_left = 0.1
d_right = 0.1
# Robot wheelbase (meters)
L = 0.3
# Compute change in orientation and displacement
d_theta = (d_right - d_left) / L
distance = (d_left + d_right) / 2
# Update position (x, y) relative to robot frame
x += distance * math.cos(theta)
y += distance * math.sin(theta)
theta += d_theta
# Correct with compass reading
compass_heading = getCompass()  # in radians
theta = 0.9 * theta + 0.1 * compass_heading  # simple correction

Lidar for Obstacle Detection and Mapping

Lidar returns a list of distances at various angles. By converting these polar coordinates to Cartesian coordinates in the world frame, you can mark occupied cells in a grid map. For instance, if lidar detects an obstacle at 1.5 meters at 45°, you can compute its world coordinates and mark the corresponding grid cell as occupied. This is similar to how self-driving cars use lidar to create high-definition maps. In Webots, the lidar provides a 360° view; you can filter out noisy readings and update a 2D occupancy grid.

# Pseudo-code for updating occupancy grid from lidar
for angle, distance in lidar_readings:
    if distance < max_range and distance > min_range:
        # Compute world coordinates
        world_x = robot_x + distance * math.cos(robot_theta + angle)
        world_y = robot_y + distance * math.sin(robot_theta + angle)
        # Convert to grid cell indices
        cell_x = int((world_x - grid_origin_x) / cell_size)
        cell_y = int((world_y - grid_origin_y) / cell_size)
        # Mark as occupied
        grid[cell_y][cell_x] = 1

Camera and Object Recognition for Landmarks

The camera with object recognition helps identify specific landmarks, like colored cylinders, that can serve as goal locations or reference points. For example, a red cylinder might be the goal. By detecting its position in the image and using camera calibration, you can estimate its relative location. This is analogous to how a robot in a warehouse might look for QR codes on shelves. In the lab, you'll use these landmarks to confirm the robot's position and navigate to them.

Grid-Based World Representation

The arena is a 5×5 grid of 1m×1m cells, numbered 1 to 25. The global reference frame has its origin at the center, with positive x east and positive y north. To represent walls, you need a data structure that stores for each cell whether walls exist on the west, north, east, and south sides. A 25×4 matrix (rows for cells, columns for WNES) works well. For instance, cell 12 might have walls on north and east. This structure allows efficient checking of connectivity during path planning. You can initialize it based on the known arena layout or build it dynamically using lidar.

# Example: Wall configuration matrix (25 cells, 4 directions: W,N,E,S)
# 0 = open, 1 = wall
wall_matrix = [
    [1,1,0,0],  # cell 1: walls on West and North
    [0,1,0,0],  # cell 2: wall on North
    ...
]

Path Planning with the Wavefront Planner

The Wavefront Planner is a simple yet effective grid-based pathfinding algorithm. It works by propagating values from the goal cell outward, similar to a flood fill. Each cell gets a value representing the number of steps to the goal. The robot then moves from its start cell to neighboring cells with decreasing values until it reaches the goal. This algorithm assumes static obstacles and is ideal for small grids.

Algorithm Steps

Initialize grid: Mark all cells as unvisited (value 0). Set goal cell to 0. Set start cell to 2. Set obstacle cells to 1.
Wave expansion: Enqueue the goal cell. While queue not empty, pop a cell. For each neighbor (4-connected), if neighbor is unvisited and not an obstacle, set its value to current cell's value + 1 and enqueue it.
Path extraction: Starting from start cell, move to neighbor with the lowest value (but higher than start? Actually lower than current) until goal is reached.

def wavefront_planner(grid, start, goal):
    # grid: 2D list, 0=free, 1=obstacle
    # start, goal: (row, col)
    rows, cols = len(grid), len(grid[0])
    values = [[0]*cols for _ in range(rows)]
    # Mark obstacles with high value (e.g., -1 or 999)
    for r in range(rows):
        for c in range(cols):
            if grid[r][c] == 1:
                values[r][c] = -1  # obstacle
    # Initialize goal and start values
    values[goal[0]][goal[1]] = 0
    values[start[0]][start[1]] = 2
    # BFS from goal
    from collections import deque
    queue = deque()
    queue.append(goal)
    while queue:
        r, c = queue.popleft()
        for dr, dc in [(0,1),(0,-1),(1,0),(-1,0)]:
            nr, nc = r+dr, c+dc
            if 0 <= nr < rows and 0 <= nc < cols:
                if values[nr][nc] == 0 and grid[nr][nc] != 1:
                    values[nr][nc] = values[r][c] + 1
                    queue.append((nr, nc))
    # Extract path
    path = [start]
    r, c = start
    while (r, c) != goal:
        min_val = float('inf')
        next_cell = None
        for dr, dc in [(0,1),(0,-1),(1,0),(-1,0)]:
            nr, nc = r+dr, c+dc
            if 0 <= nr < rows and 0 <= nc < cols:
                if values[nr][nc] > 0 and values[nr][nc] < min_val:
                    min_val = values[nr][nc]
                    next_cell = (nr, nc)
        if next_cell is None:
            break  # no path
        r, c = next_cell
        path.append((r, c))
    return path

Integrating Mapping and Path Planning

In practice, the robot first explores the arena to build the wall matrix. Using lidar and odometry, it can detect walls and update the matrix. Then, given a goal cell (e.g., where a red cylinder is detected), the robot runs the Wavefront Planner to compute a path. The path is a sequence of grid cells; the robot must execute motion commands to traverse from one cell center to the next. This involves turning to face the next cell and moving forward. The compass helps maintain heading, and encoders measure distance. If obstacles are encountered (e.g., a wall not in the map), the robot should update the map and replan.

Example: Navigating to a Landmark

Imagine the robot starts at cell 13 (center) and the goal is cell 25 (north-east corner) where a blue cylinder is detected. The Wavefront Planner generates a path: 13 → 14 → 15 → 20 → 25. The robot then moves: turn to east, move 1m to cell 14, turn to east again, move to cell 15, turn north, move to cell 20, turn north, move to cell 25. During movement, lidar checks for unexpected obstacles; if a wall is detected where none was expected, the robot updates the matrix and recalculates the path.

Real-World Connections and Trends

Mapping and path planning are not just academic—they power the autonomous vehicles and delivery robots we see today. For instance, during the 2026 FIFA World Cup, autonomous shuttles might transport fans using similar algorithms. In e-commerce, robots like those from Amazon Robotics navigate warehouses using grid maps and path planning to retrieve items. Even in video games, NPCs use pathfinding algorithms like A* (an evolution of Wavefront) to move through levels. The skills you learn here are directly applicable to these exciting fields.

Conclusion

This tutorial covered the essential steps for mapping and path planning in mobile robotics using Webots and Python. By integrating encoders, compass, lidar, and camera data, you can build a wall matrix and plan paths with the Wavefront Planner. Practice by modifying the grid size, adding dynamic obstacles, or implementing A* for comparison. These fundamentals will serve you well in advanced robotics courses and real-world applications.