Localized Vacuum Cleaner

 

Localized Vacuum Cleaner — Practice Report

Objective

The objective of this project is to implement and test a navigation algorithm for an autonomous vacuum cleaner capable of covering the largest possible area of a house using localization and mapping techniques.

 

Implementation Steps

1. Map and Obstacles

• Before running the navigation algorithm, I preprocessed the map to make obstacles thicker and improve the robot’s safety margins.
  I implemented the function inflate_obstacle_numpy() using NumPy, which expands all dark areas in the grayscale map to simulate thicker walls. 

 Steps I followed:

        Binary mask

First, I created a binary mask of the map. Each pixel darker than threshold (obstacle)        is marked by 1, while the rest are 0. 

"mask = (img[:, :, 0] < threshold).astype(np.uint8)"

        Expanding obstacles:

It loops over every offset (dx, dy) inside of the defined radius. For each offset I shift the entire mask in that direction and merge it with the previous mask. This way all the points of the mask with 1 end up taking every pixel around them with the radius established.

         Merge shifted masks:

The merging is done using np.maximum(), which acts as a logical OR between the current inflated mask and each shifted one:

 

This means that if a pixel is marked as 1 in any shifted mask, it remains 1 in the final combined version. In other words, all the nearby regions around each obstacle become part of a single, unified obstacle area.

         Applying the Inflation to the Map Image:

Finally the inflated mask is applied to the Map Image.

img[inflated == 1] = [0, 0, 0].

This means that wherever the mask has a 1. The corresponding pixel is going to turn black.

The rest of the pixels remain unchanged.

The resulting image now has thicker obstacles, making path planning safer for the robot.

2. Localization and Transformation

• To correctly relate the robot’s real position in Gazebo with its corresponding position on the 2D map, I created a transformation system based on linear algebra.
  
  Reference Points and Least Squares Fitting
  
  First, I manually selected 10 pairs of corresponding points:
  

  • The pixel coordinates on the map image (obtained by clicking specific locations).

  • The real-world coordinates of those same points in Gazebo.

    Each point was represented in homogeneous coordinates (x, y, 1) to allow both rotation and translation to be captured by a single matrix.
    
    Then, using the least squares method (np.linalg.lstsq), I computed the transformation matrix M that best fits the relationship between the Gazebo coordinates and the map pixels:
    "M, _, _, _ = np.linalg.lstsq(coords, pixels, rcond=None)"
    "M_inv = np.linalg.pinv(M.T)" 
     
     The matrix "M" converts real coordinates into map pixels, while it's pseudo-inverse "M_inv" performs the inverse transformation. 
    I implemented two helper functions: "coord_to_pixel", "pixel_to_coord".
    You call them with a pixel or a coordinate and it returns the conversion.

     

    They use the transposed matrix because map’s X and Y axes were inverted.

3. Grid Representation

The cell size chosen is a little less of the robot's size so when it goes through a cell the cell is fully cleaned.

Using the array that gets the map information I divided it's height and width into the cell's size wanted to get the number of rows and columns I would have. Then I looped over each cell and in every loop I looped again over every pixel on each cell.

If there was only one black pixel (meaning there's an obstacle) in the cell, then the whole cell turns into an obstacle and breaks the loop. After every loop, if the variable obstacle is True then the cell in the grid with it's location row and column will turn black as if it's an obstacle.

The "show_grid()" function visualizes the grid map at a larger scale.
It duplicates each grid cell by a fixed scale factor using np.repeat(), effectively converting the logical occupancy grid into a pixel-based image that can be displayed with WebGUI.showNumpy().

This allows me to observe in real time how the robot plans and explores the environment, marking free, occupied, and visited cells. 

 

4. Path Planning and Coverage

• To determine how the robot should explore the environment, I implemented a coverage algorithm based on prioritized movements and a BFS (Breadth-First Search) fallback strategy.
  
  The movement priority is fixed as follows:

  1. UP
  2. RIGHT
  3. DOWN
  4. LEFT

  The robot always tries to move in the highest-priority direction.
  If the next cell in that direction is an obstacle, it switches to the next one in the list.
  When the robot becomes completely blocked (no free cells around), it navigates back to the last uncleaned cell it stored earlier.
  
  While moving, the robot continuously stores all free and unvisited cells around its current position in a list called uncleaned_point.
  Whenever it gets stuck, it pops the last element from that list and uses my bfs(start, goal) function to find the shortest path to that cell.
  
  The BFS algorithm explores the map layer by layer, ensuring the robot finds an efficient and collision-free route.
  Once the robot reaches the target, it resumes its normal movement priority routine.
  Each visited cell is marked as “planned” and stored in a path list for later use by the control system.

            

5. Robot Control

• The robot movement is governed by a PID controller, which guides it to each waypoint defined in the path list.
  For every point in the path, the controller adjusts the robot’s linear and angular velocities based on the error between the robot’s current orientation and the target direction.

The angular velocity (w) is computed using the PID formula, while the linear velocity (v) is dynamically adjusted according to the angular error:

 

 v = vmax - kv * abs(error)

 

This simple yet effective equation ensures smoother turns:
when the angular error is large, the robot slows down to rotate precisely without drifting away from its path; when the robot is well aligned, it speeds up to move efficiently.

Typical parameters used:
vmax = 0.32 m/s
kv = 0.2
kp = 0.9, kd = 0.35, ki = 0.0
Finally, once the entire planned_path is completed, the robot stops by setting both V and W to zero. 

6. Visualization

• To visualize the robot’s state and progress, I used WebGUI.showNumpy() to display a monochrome grid:

  Black cells → Obstacles

Gray cells → Visited areas

White cells → Free space

 

This visualization allowed me to debug in real time, checking how the map was being explored and how the robot reacted to the navigation strategy, local obstructions or dead ends.

---

Problems Encountered

• Adjusting velocity: Initially, the robot moved in jerky motions because I used fixed speeds depending on error intervals.
  Solution: Implement a velocity proportional to the error.
  
  Incorrect coordinate transformation: The robot appeared out of bounds.
  Solution: Transpose the matrix, as X and Y axes were inverted.
  
  Obstacle inflation performance: The first implementation manually iterated over each pixel, which took minutes.
  Solution: Use NumPy to create a mask, shift the entire mask in the specified radius, and merge masks for faster processing.
  
  Robot stuck between virtual obstacles:
  Solution: Fine-tune the PID to brake correctly in turns, slightly increase obstacle inflation, and reduce cell size slightly.
  
  Slow map cleaning and oscillation:
  Solution: Adjust PID constants to reduce oscillation and increase speed, reducing coverage time from 30 to 20 minutes.

---

Demonstration Video