Trajectory-Biased Kidnap Recovery for AMCL

A ROS2 Humble project that helps a robot recover when it gets "kidnapped" (suddenly moved to an unknown location) in a warehouse environment.

Demo

Videos:

• Recovery Example 1
• Recovery Example 2

Videos show the robot being teleported (kidnapped) and the recovery system detecting the event and re-localizing along the trajectory.

The Problem

In warehouse environments, many areas look the same to a robot's laser scanner: rows of shelves, repeating aisles, similar corners. This is called perceptual aliasing. When a robot gets lost in such an environment, standard localization recovery struggles because multiple locations match the sensor readings equally well.

Our Solution

We exploit the fact that warehouse robots follow known, repetitive trajectories. When the robot gets lost, instead of searching the entire map, we only search along the path it was supposed to be on.

How it works:

1. Likelihood Field Construction: At startup, we build a distance transform from the occupancy grid map. This precomputes the distance to the nearest obstacle for every cell, enabling fast scan likelihood computation.

2. Kidnap Detection: We continuously compute the likelihood of the current laser scan given AMCL's believed pose. When this likelihood drops significantly (scan doesn't match where AMCL thinks we are), we also monitor AMCL's covariance. When uncertainty spikes above a threshold, we trigger recovery.

3. Trajectory-Biased Candidate Sampling: Instead of searching globally, we sample candidate positions along the robot's known trajectory. Points are sampled at regular intervals along path segments between waypoints, with additional lateral samples to cover nearby positions.

4. Scan Matching & Likelihood Scoring: For each candidate pose, we project the current laser scan endpoints and look up distances in our precomputed likelihood field. Candidates are ranked by their scan likelihood score. Higher scores mean better alignment with the map.

5. Pose Injection & Recovery Motion: We inject the best-scoring candidate into AMCL via /initialpose. The robot performs a gentle wiggle motion to help AMCL's particle filter converge around the new pose estimate.

6. Convergence Validation: We monitor the scan likelihood during recovery. Once it stabilizes above a threshold, we validate for a short period to avoid false positives, then resume navigation.

Dependencies

sudo apt install ros-humble-turtlebot3* ros-humble-navigation2 ros-humble-nav2-bringup
sudo apt install ros-humble-gazebo-ros-pkgs ros-humble-slam-toolbox
sudo apt install ros-humble-aws-robomaker-small-warehouse-world

Build

git clone https://github.com/onurcalisir27/probabilistic-robotics.git
cd ~/probabilistic-robotics/project_ws
colcon build --symlink-install
source install/setup.bash

Running the Demo

You need 4 terminals.

Terminal 1 - Start the warehouse simulation:

export GAZEBO_MODEL_PATH=/opt/ros/humble/share/aws_robomaker_small_warehouse_world/models:$GAZEBO_MODEL_PATH
export TURTLEBOT3_MODEL=waffle_pi
ros2 launch rover_sim warehouse_sim.launch.py

Terminal 2 - Start navigation:

ros2 launch rover_sim warehouse_nav.launch.py

Terminal 3 - Start the kidnap recovery system:

ros2 launch rover_sim kidnap_recovery.launch.py

Terminal 4 - Control the robot:

# Start the robot on its trajectory loop
ros2 service call /start_trajectory std_srvs/srv/Trigger

# When ready, kidnap the robot (teleports it to another spot on the trajectory)
ros2 service call /kidnap_robot std_srvs/srv/Trigger

What You Should See

1. Robot starts navigating through warehouse aisles in a loop
2. When you call /kidnap_robot, it teleports to a different location
3. Within 1-2 seconds, the system detects the kidnap (AMCL uncertainty spikes)
4. Recovery begins - you'll see candidate markers appear in RViz
5. The system tries candidates until it finds one where AMCL converges
6. Robot resumes its trajectory from the recovered position
7. If recovery fails, we call AMCL global reinitialisation

Key Parameters

In kidnap_recovery.launch.py:

• likelihood_threshold: How confident we need to be before accepting convergence (lower = easier)
• convergence_timeout: How long to wait per candidate before trying the next one
• num_candidates: How many sample points along the trajectory

Limitations

• Warehouse perceptual aliasing means sometimes wrong locations look correct
• Recovery assumes the robot is somewhere on its known trajectory
• Works best when the robot hasn't drifted far from the path