A complete autonomous-navigation pipeline for a Thymio robot, capable of reaching a goal through mapped obstacles, reacting to unexpected obstacles, maintaining a pose estimate, and recovering after being moved to a new position.
We placed a Thymio robot on a map covered with obstacles and tasked it with reaching a target while avoiding both known obstacles visible to an overhead camera and unknown obstacles introduced during the run. The robot also needed to localize itself continuously and recover after being "kidnapped" and moved to a new position.
ArUco markers identified the robot, goal, obstacles, and workspace references. Five integrated modules handled computer vision, global path planning, state estimation, local obstacle avoidance, and motion control inside one asynchronous control loop.
We used OpenCV's ArUco marker detection to identify the robot, goal, and known obstacles from an overhead camera feed. Marker IDs classified each object, while detected corners and centers mapped pixel coordinates onto a discretized representation of the workspace.
Obstacle regions were inflated by a safety margin based on the Thymio's physical footprint, preventing the planner from producing paths that grazed obstacle edges. Three measured reference markers provided a pixel-to-millimeter scale factor, allowing wheel odometry to be expressed consistently in camera coordinates.
Once the occupancy grid was built, we used A* search with a Manhattan-distance heuristic on an 8-connected grid to compute a collision-free path from the robot's current cell to the goal.
The raw path often contained dozens of adjacent grid cells. A simplification stage retained only points where the travel direction changed, reducing the result to a compact sequence of meaningful waypoints that the motion controller could follow directly.
We implemented an Extended Kalman Filter around a differential-drive odometry model to maintain the robot's position and heading. The prediction step used left and right motor speeds with the Thymio's wheel geometry, while the correction step used camera measurements whenever the robot's ArUco marker was visible.
When camera measurements were unavailable, the filter continued open-loop using the motion model. Process-noise and measurement-noise covariance matrices were tuned empirically until the estimated trajectory closely followed the camera trajectory during normal operation and remained stable through brief occlusions.
| EKF Stage | Input |
|---|---|
| Prediction | Left and right motor speeds, wheel geometry, elapsed time |
| Correction | ArUco position and heading from the overhead camera |
| State | Robot position and orientation |
| Tuning | Empirical process and measurement covariance adjustment |
Unknown obstacles added during a run could not appear in the global camera map in advance. We therefore used the Thymio's five front infrared proximity sensors as a reactive local layer.
Sensor readings were scaled and combined through a fixed Braitenberg-style weight matrix to produce left and right motor corrections. Crossing a proximity threshold switched the robot from waypoint tracking into avoidance mode. Once every sensor cleared the threshold, the controller returned automatically to the planned path.
We implemented a modified Astolfi controller to drive the robot through the simplified waypoint list. For each target, the controller computed heading error from the current estimated state and scaled it with the angular gain k_alpha.
Linear speed remained zero until heading error fell below a threshold. This forced the Thymio to orient itself before moving forward, avoiding wide arcs and improving waypoint precision. Once aligned, the controller applied a fixed forward speed while continuing to correct heading.
All modules ran inside one asynchronous main loop using tdmclient for communication with the robot. Each cycle coordinated perception, planning, estimation, local navigation, and motor commands.
The integrated system built a map from the overhead camera, planned a collision-free route, navigated through known obstacles, reacted to obstacles that were invisible to the camera, reached the goal, and recovered after the robot was moved to a different position.
The remaining unresolved case was long-duration navigation with the camera fully hidden. A state-tracking bug prevented the camera-occlusion flag from updating correctly, so the Kalman filter's open-loop prediction was not exercised as intended. The root cause was identified, but the project timeline ended before the correction could be validated.