Implementation of FastSLAM2.0 on an Embedded System and HIL Validation using Different Sensors Data

Introduction

The simultaneous localization and mapping (SLAM) aims to construct an environment map and localize a robot according to this map. Sensors data must be integrated into a given representation to map the explored environment. This is equivalent to the question: what does the environment looks like? Localization consists of estimating the robot pose relatively to the explored map. This is equivalent to the question: where am I? Localization and mapping are concurrent problems that cannot be solved independently. The robot can estimates the position of a given landmark but it needs before to know from which location this landmark was observed. At the same time, it is difficult to estimate the actual position of the robot without a map. Therefore a good map is necessary for localization while an accurate pose estimate is needed for a map reconstruction. Several algorithms have been developed to address the problem known as SLAM. EKF-SLAM (Dissanayake, New- man, Clark, Durrant-Whyte, & Csorba, 2001), FastSLAM (Montemerlo, Thrun, Koller, & Wegbreit, 2002), EIF-SLAM (Cheein, Steiner, Paina, & Carelli, 2011). These algorithms aim to enhance the quality of localization, robustness and optimize the computational cost. The EKF based approach suffers from two limiting factors. First, the computational complexity is quadratic with the state vector dimension which limits the number of landmarks that can be handled. Second, the EKF approach has a problem of data association: a false mapping of a low landmarks number can cause the filter divergence. An alternative approach to the SLAM problem is based on the particle filter. It represents the uncertainty of the robot pose by a number of different particles. Each particle is a hypothesis of the robot pose or the trajectory. It has its own map modeled by the extended Kalman filter (EKF). However, the processing task can be accelerated because the trajectory (of the particle) is known and landmarks estimations are no longer correlated. There are two main versions of SLAM algorithms based on the particle filter: the FastSLAM1.0 and the FastSLAM2.0 (Montemerlo, Thrun, Koller, & Wegbreit, 2003). The key idea of the FastSLAM1.0 is to reduce the complexity caused by the dependencies between the number of landmarks and the size of the covariance matrix encountered in EKF-SLAM. This is done by factoring the estimated robot poses and the map estimation. The principle of FastSLAM1.0 is to generate stochastically M particles representing M hypotheses of the robot pose. For each particle, landmarks are initialized and then processed using the extended Kalman filter. Each particle is then weighted according to its likelihood with measurements. The performance of the FastSLAM1.0 will decrease if the motion model is much noised according to the exteroceptive sensors. The proposal distribution (set of particles generated after integrating the control data) will be then poorly matched to the posterior (set of particles generated after integrating the observation data). A high number of particles will be deleted in the resampling step and causes the filter divergence. FastSLAM2.0 is an improved version of FastSLAM1.0 which attempts to solve this problem. When a new robot pose is sampled the resulting proposal distribution will rely on both the current control and the most recent observation. This paper is organized as follows: a state-of-art about related works is presented and our contribution is highlighted. Next, the main steps of the FastSLAM2.0 algorithm based on both a monocular camera and a laser range finder are described. We will present localization results of the FastSLAM2.0 using simulations and a real indoor dataset, a comparison of the localization accuracy of the FastSLAM2.0 with others SLAM methods is also discussed. The evaluation is based on different sensors data using the Hardware In the Loop (HIL) method. The rest of paper will be devoted to discuss the implementation of the FastSLAM2.0 on a low-cost and heterogeneous architecture. Evaluation in terms of processing times is presented.