p-hoanganh/2d-slam

2D SLAM using an extended Kalman filter on LiDAR and INS data

2D LiDAR/INS SLAM with extended Kalman filter

Author: Jan Xu

Final Year (Masters) project carried out at CalUnmanned laboratory, Department of Civil and Environmental Engineering, University of California, Berkeley.

All code used for this project can be found in this repository, written in Python 3.

Context

As interest in autonomous robot navigation grows, Self-Localization and Mapping (SLAM) using low-cost range and inertial sensors is becoming ever-increasingly popular within the scientific community. The aim of this project was to implement SLAM algorithms by fusing odometry and pose data from an IMU with range data from a Light Detection and Ranging (LiDAR) device. A real-life experimental setup was constructed such that the sensor data is collected under conditions reflecting ground truth as close as possible. With this data, the state-space was then represented and manipulated with an extended Kalman filter, a simplified dynamical state transition model.

The system architecture for the SLAM procedure was visualized in the figure below. The blue and green blocks, “State Prediction” and “State Correction”, refer to the sensor fusion stages in the Kalman filter. Rectangular boxes with sharp corners represent physical quantities such as vectors, matrices or other types of data structures, whereas boxes with rounded corners describe various procedures.

The experimental environment used for this project was a custom-built plywood maze with a 3D-printed sensor package box, seen below:

The dimensions of the model were carefully measured and then rendered numerically to establish ground truth:

For detailed information regarding the project, please refer to the project report.

How to run

Packages required:

• Numpy
• Pandas
• Matplotlib
• Scikit-learn (if using ICP algorithm)

Usage: $ python3 main.py algorithm path-to-IMU-data path-to-LiDAR-data

Arguments:

• algorithm: LiDAR scan matching algorithms. Currently valid options: icp (Iterative Closest Point) and feature (feature-based line segment approach).
• path-to-IMU-data: local path to .csv-file containing the IMU data
• path-to-LiDAR-data: local path to .csv-file containing the LiDAR data

Example: $ python3 main.py icp sensordata/exp1_imu.csv sensordata/exp1_lidar.csv

The folder sensordata contains all the datasets for the sensors, separated in nine different experimental runs.

Output

The output from the main program will consist of a Matplotlib figure, plotting the estimated trajectory (dashed-dotted blue lines) and the experimental environment (purple dots/dashed lines). Two sample images, the first one using the ICP algorithm and the second one using the feature-based line segment approach for the scan matching procedure, are shown below.

Finally, the deviation error are printed out in terms of the root mean squared error (RMSE) in meters between estimate and ground truth for both the trajectory and the experimental environment. A Matplotlib figure showing the deviation error along the trajectory is also outputted before the program terminates:

Contact

[email protected]

Telephone: [phone]

License

MIT