Skip to content

README.md

Latest commit

 

History

History
63 lines (49 loc) · 3.25 KB

README.md

File metadata and controls

63 lines (49 loc) · 3.25 KB
Raw

Localization: Monte Carlo Localization (MCL)

Determining a robot's position and orientation within a known map is a foundational problem in mobile robotics. This package solves it with Monte Carlo Localization (a particle filter): a swarm of pose hypotheses ("particles") is propagated forward by a motion model on each odometry update, then reweighted and resampled against a sensor model each time a new LIDAR scan arrives. Over time the particle cloud collapses around the robot's true pose.

Modules

  • localization/motion_model.py — propagates each particle's [x, y, theta] by the odometry delta [dx, dy, dtheta] (rotated into the particle's frame), with injected Gaussian noise so the cloud spreads to reflect motion uncertainty.

  • localization/sensor_model.py — precomputes a discretized beam sensor model table (a weighted mixture of a Gaussian "hit" term, a "short reading" term, a max-range spike, and a uniform random term — see Thrun/Fox/Burgard's Probabilistic Robotics), then scores each particle by ray-casting its expected LIDAR scan against the map and comparing to the real scan via table lookup.

  • localization/scan_simulator_2d.pyx — a Cython/C++ 2D ray-casting engine used to compute the expected scan from a given pose against the occupancy grid (shared with racecar_simulator).

  • localization/particle_filter.py — the ROS2 node tying it together: propagates particles on odometry, reweights/resamples on scans, publishes the mean pose estimate to /pf/pose/odom, and publishes the particle cloud to /particles_vis for RViz. Clicking a point in RViz (/clicked_point) reseeds the particle cloud around that point for manual re-initialization (the "kidnapped robot" case).

Running

ros2 launch localization localize.launch.xml
ros2 launch racecar_simulator simulate.launch.xml

The simulator publishes ground truth position, so it's a good way to validate the filter before testing on the real car — inject noise into the odometry and compare the filter's estimate against ground truth.

Unit tests

# ====== motion model ======
ros2 launch localization motion_model_test.launch.py
# ==========================
# ====== sensor model ======
ros2 launch localization sensor_model_test.launch.py
# this will wait for you to run test_map.launch.xml in another terminal
ros2 launch localization test_map.launch.xml
# ==========================

See localization/test/*.py for what's being checked.

References

  1. S. Thrun, D. Fox, W. Burgard and F. Dellaert. "Robust Monte Carlo Localization for Mobile Robots." Artificial Intelligence Journal. 2001.
  2. D. Fox, W. Burgard, and S. Thrun. "Markov localization for mobile robots in dynamic environments." Journal of Artificial Intelligence Research, 1999.
  3. D. Fox. "KLD-sampling: Adaptive particle filters." NIPS 2002.
  4. C. Walsh and S. Karaman. "CDDT: Fast Approximate 2D Ray Casting for Accelerated Localization."