SLAM-Based Localization, Mapping, and Path Planning System for Autonomous Mobile Robots

01 · Project Information

• Institution: Cornell University

• Course: ECE 5772 - Autonomous Mobile Robots

• Period: January-May 2025

• Project Type: Course laboratories and integrated robotics project

• Focus: Robotics algorithms, environmental mapping, and system integration

• Platform: MATLAB, iRobot Create, Intel RealSense

• Core Topics: Bayesian localization, particle filtering, occupancy-grid mapping, SLAM, A*, visibility graphs, and feedback linearization

02 · Overview

This project brings together the motion-control, sensing, localization, mapping, and planning components of ECE 5772 around three fundamental questions: Where is the robot? What does its environment look like? How can it safely reach a goal?

The integrated navigation system used an iRobot Create differential-drive platform with odometry, Intel RealSense depth measurements, bump sensors, and beacon observations. MATLAB connected pose estimation, map updates, route planning, and motion control.

A separate SLAM coursework component covered EKF-SLAM and FastSLAM, examining the joint estimation of robot poses and environmental landmarks. It is presented within the same project portfolio, but not as a claim that both SLAM algorithms were deployed in the integrated navigation stack.

03 · Coursework and Project Components

• Motion Modeling and Coordinate Transforms: Established world and robot coordinate frames and applied differential-drive kinematics to represent positions, measurements, and motion consistently.

• Feedback-Linearized Control: Converted desired planar velocities into executable linear and angular velocity commands.

• Waypoint Tracking: Used position feedback and target switching to guide the robot along planned routes.

• Sensor Acquisition and Bump Response: Logged odometry, depth, beacon, and bump measurements to support environmental observation and basic reactive protection.

• Bayesian Grid Localization: Updated position probabilities using four-direction range observations and a probabilistic measurement model.

• Particle-Filter Localization: Combined motion prediction, observation weighting, and resampling to provide pose estimates for navigation and mapping.

• Occupancy-Grid Mapping: Processed depth rays and applied Log-Odds updates to build a probabilistic occupancy map.

• Explored-Area and Wall Detection: Identified observed areas and evaluated candidate walls to distinguish unknown regions from traversable space.

• Path and Task Planning: Combined obstacle inflation, visibility graphs, A*, and a greedy TSP strategy to generate feasible routes and multi-goal visitation orders.

• SLAM Coursework: Covered EKF-SLAM and FastSLAM for simultaneous localization and mapping, trajectory estimation, and uncertainty analysis.

04 · System Design

The integrated navigation workflow was organized around the following steps:

1. Acquire and log odometry, depth, bump, and beacon observations.

2. Estimate robot position and heading using a particle filter.

3. Transform depth observations into map coordinates.

4. Update occupancy and explored-area information.

5. Evaluate candidate walls using observation evidence.

6. Plan routes and goal visitation order using environmental geometry.

7. Convert paths into waypoints and motion commands.

8. Record trajectories, visited goals, and mapping outputs.

Localization and mapping had distinct responsibilities: the localization module estimated the robot pose, while Mapping updated the environment using that estimate. This architecture should not be confused with directly running EKF-SLAM or FastSLAM.

05 · Core Features

1. Differential-Drive Control and Waypoint Tracking

The feedbackLin.m module converted desired world-frame planar velocities into linear and angular velocity commands supported by the robot.

The waypoint controller used position feedback and switched targets when the robot entered a specified distance threshold. The experiments used a 0.1-meter waypoint-switching threshold; this was a controller setting, not a measured localization accuracy.

2. Multi-Sensor Logging and Bump Response

A shared dataStore recorded pose, odometry, bump, depth, and beacon measurements for debugging and analysis.

The bump laboratory used backupBump.m for a backup response after contact and limitCmds.m to constrain motion commands. Contact-based recovery provided supplementary protection rather than replacing collision checking in the planner.

3. Bayesian Localization and Particle Filtering

The stationary localization exercise predicted north, east, south, and west range measurements from a known wall map. Actual observations were then used to update the posterior probability of each candidate grid location.

The integrated system used 1,000 particles to represent possible robot poses. Motion prediction, observation weighting, and resampling updated the pose estimate supplied to the mapping module.

4. Log-Odds Occupancy-Grid Mapping

The mapping module used a 50×50 grid covering X coordinates from -3.5 to 3.5 meters and Y coordinates from -2.5 to 2.5 meters.

Using the estimated robot pose, valid depth rays contributed free-space evidence along their paths and occupied-space evidence at valid obstacle endpoints. Log-Odds updates accumulated observations over time.

This probabilistic representation retained uncertainty instead of turning each individual measurement into a permanent binary decision.

5. Explored-Area and Candidate-Wall Detection

This was my main area of responsibility in the integrated project.

My development and integration work focused on:

• logOddsDepth: Updating occupancy evidence from depth measurements.

• createExploredAreaMap: Representing observed areas.

• detectOptionalWallsWithExploredAreas: Evaluating candidate walls using coverage and occupancy evidence.

• visualizeMappingResults: Displaying mapping and wall-detection results.

The key distinction was between “no obstacle was observed” and “the area was observed and confirmed free.” Maintaining observation coverage helped prevent unknown space from being incorrectly classified as traversable.

6. Path Planning and Multi-Goal Navigation

The planning system inflated obstacles to account for robot dimensions, constructed a visibility graph, and used A* to search for feasible paths.

For multiple goals, a greedy traveling-salesperson strategy determined the visitation order. The resulting routes were executed through waypoint tracking. This heuristic produced a practical ordering without guaranteeing a globally optimal tour.

7. SLAM Coursework

The SLAM component examined a differential-drive robot moving through a wall-based environment. Inputs included linear and angular velocity commands and observations of uniquely identified wall-end landmarks.

It covered two approaches:

• EKF-SLAM: Representing robot pose, landmarks, and their correlations through a joint state and covariance.

• FastSLAM: Representing robot-path hypotheses with particles and estimating landmarks conditionally on each path.

The analysis covered robot trajectories, wall-end locations, landmark uncertainty ellipses, and pose uncertainty at selected times.

06 · My Contributions

My primary contribution was environmental mapping and module integration, alongside control and localization coursework:

• Developed feedback-linearization and waypoint-tracking routines.

• Implemented stationary Bayesian grid localization.

• Developed depth-based occupancy-grid mapping.

• Built explored-area and candidate-wall detection logic.

• Integrated particle-filter poses into the Mapping pipeline.

• Connected mapping outputs with the planning module.

• Produced mapping visualizations and analyzed recorded data.

07 · What I Learned

This project developed my understanding of the complete autonomous robotics pipeline: sensors provide noisy observations, localization estimates robot state, mapping accumulates environmental evidence, planning generates routes, and control converts those routes into motion.

It also demonstrated that reliable robotics depends on more than individual algorithms. Consistent coordinate frames, valid sensor data, estimation uncertainty, and well-defined interfaces between modules are equally important.