Thymio autonomous navigation stack

Final project for the EPFL course "Basics of Mobile Robotics"

Report
Code

At a glance

This is the final project for the EPFL course “Basics of Mobile Robotics” (Prof. F. Mondada), completed in Fall 2021 in collaboration with Nay Abi Akl, Mariam Hassan, and Alaa Abboud.

The goal was to build a complete navigation pipeline for the Thymio mobile robot operating on a physical map observed by an overhead camera. A user defines a target position; the robot must autonomously reach the goal while avoiding obstacles, including dynamic ones.

Our test map recreates iconic EPFL buildings using LEGO® bricks, with Thymio playing the role of a student navigating campus to attend lectures. The robot and goal positions are detected using ArUco markers.

Left: Satellite view of EPFL campus with highlighted buildings used as obstacles. Right: Our physical LEGO® map recreation for the Thymio robot.

System in a nutshell

The project follows a practical “global + local” navigation structure:

  1. Perception & mapping
    • Convert the camera view into a consistent top-down map (perspective warp).
    • Detect obstacles from the map image via contours + vertices (OpenCV).
    • Detect robot and goal using ArUco markers.
  2. Planning
    • Compute a collision-free global path using visibility graphs and an A*-based search (adapted from ExtremityPathFinder and integrated into our pipeline).
    • Inflate obstacles to account for the robot footprint and tracking margins.
  3. State estimation
    • Use an Extended Kalman Filter (EKF) with a differential-drive motion model and camera-based pose updates.
  4. Execution
    • Convert the global path into a smooth, trackable trajectory (discretization + Bezier smoothing).
    • Track the trajectory with a move-to-pose controller, producing linear/angular velocity commands and converting them to Thymio motor control.
    • When something unexpected appears, switch to a reactive obstacle avoidance behavior using proximity sensors, then resume global path following.
A complete navigation sequence: Thymio begins following the computed optimal path (left), detects a dynamic obstacle and switches to local avoidance mode (middle-left), navigates around the obstruction (middle-right), and successfully returns to its optimal trajectory toward the goal (right).

What we implemented

Global path planning (visibility graphs + A*)

We compute an optimal collision-free route by building a visibility graph from obstacle vertices (after inflation) and running A* to connect start-to-goal. This planning style works well in structured environments because it produces clean, geometrically meaningful paths.

The pipeline includes:

  • Obstacle contour extraction
  • Polygon/vertex processing
  • Obstacle inflation (safety margin)
  • Graph construction + search
  • Path extraction for downstream tracking

Camera mapping: perspective warp to top view

A key practical element is the camera geometry. We use a perspective warp that transforms an oblique camera view into a top-down representation that is consistent for:

  • Obstacle extraction
  • Robot/goal detection
  • Stable coordinate interpretation across the pipeline

The transform preserves aspect ratio and produces a top view suitable for planning and localization.

Localization with an EKF

We implemented an EKF that fuses:

  • A differential-drive motion model for prediction
  • Camera-based pose observations for correction

This gives a smoother and more reliable pose estimate than raw detections alone, and helps stabilize control (especially when the camera signal becomes noisy or briefly unreliable).

Trajectory preparation: discretization + smoothing

A shortest path is not always directly trackable by a real robot. We:

  • Discretize the planned polyline into a sequence of points
  • Smooth the resulting trajectory with a Bezier curve, reducing sharp turns and making it easier for the controller to follow

Control: move-to-pose + motor commands

We used a move-to-pose style controller that computes linear and angular commands to track the planned trajectory. Those commands are then converted into motor actions suitable for Thymio.

Local obstacle avoidance (static + dynamic)

Global planning alone is not enough in the presence of unexpected changes. We implemented reactive avoidance based on proximity sensors:

  • Detect nearby obstacles
  • Temporarily switch to a local avoidance behavior
  • Return to the planned trajectory once safe

This enables handling dynamic obstacles (including another moving Thymio entering the path).

High-level finite state machine governing Thymio's navigation behavior, coordinating global path planning with local obstacle avoidance.

Demo videos

These short demos show the robot operating on different maps and under different conditions:

  1. Thymio computes the optimal path to reach the destination, and tracks it while avoiding added obstacles

  2. Thymio can reliabily reach its destination even when static obstacles are added along its path

  3. Thymio can also react to dynamic obstacles, such as another Thymio crossing the planned path

  4. Thymio can continue following the computed trajectory even when the camera gets temporarily obstructed

Notes on robustness and design choices

A few decisions that mattered in practice:

  • Obstacle inflation: even with a perfect plan, tracking error exists; inflation provides a safety margin for execution.
  • Smoothing: the Bezier-based smoothing step makes the gap between “geometric shortest path” and “trackable motion” much smaller.
  • Estimator + controller separation: the EKF reduces noise and makes the controller less reactive to spurious pose jitter.
  • Hybrid navigation: combining global planning with reactive avoidance made the system more tolerant to dynamic changes and imperfect sensing.

Acknowledgements

Course: EPFL — Basics of Mobile Robotics (Prof. F. Mondada)
Team: Nay Abi Akl, Mariam Hassan, Alaa Abboud, Luca Zunino
Source of the cover image: OlivierMichel - Own work | CC BY-SA 4.0