Perfoming 2D Slam with a Differential Drive Mobile Robot geared with a Range Sensor

Project for Probabilistic Robotics, Univ. La Sapienza Roma, 2020.

Approach

The goal of this project is to optimize the trajectory and the map generated by a unicycle-like robot. The robots moves in the map and it's equipped with a range sensor which can sense the radial distance of landmarks. We know the matching range-landmark_id but the global position of the robot and the global position of the landmark must be corrected using a multipoint ICP.

The Algorithm follows these phases:

First we need to generate an initial guess of the position in world frame of the landmark, so a triangulation procces is launched. This will give a "guess" on the position of the landmarks.
Second phase is the ICP algorithm. This is a repetitive optimization algorithm which aims to converge both robot poses and landmark position based on the range measurements.
Generation of the resulting plot.