An improved H-infinity unscented FastSLAM with adaptive genetic resampling

Highlights

• An improved H-Infinity unscented FastSLAM algorithm is proposed.

• The H-Infinity unscented Kalman filter algorithm is improved as the importance sampling in particle filter with an adaptive factor.

• A time varying noise estimator is introduced to estimate the process noise and the measurement noise.

• An adaptive genetic algorithm (AGA) is used to complete the resampling of particle filter.

Abstract

The FastSLAM is a typical tracking algorithm for SLAM, but it often suffers from the low tracking accuracy. To mitigate the problem, an improved H-Infinity unscented FastSLAM (IHUFastSLAM) with adaptive genetic resampling is proposed in this paper. Specifically, the H-Infinity unscented Kalman filter algorithm is improved using an adaptive factor and is employed as importance sampling in particle filter. Next, the process noise and the measurement noise are estimated by a time varying noise estimator. Moreover, an adaptive genetic algorithm is used to complete the resampling of particle filter. Finally, the improved H-Infinity UFastSLAM with adaptive genetic resampling is proposed to complete robot tracking. The proposed algorithm can track robot with good accuracy, and obtain reliable state estimation in SLAM. Simulation results reveal the validity of the proposed algorithm.

Introduction

With the development of information technology, the localization and tracking of the robot have become an active field, and have a wide range of applications involving automatic drive, augmented reality and virtual reality [1], [2]. In the absence of any global position information, some traditional robot localization algorithms may produce inaccurate and unreliable position estimation [3]. The simultaneous localization and mapping (SLAM) technique aims to solve the problem of position estimation in the unknown environment, and some algorithms have been advanced [4]. A robot accomplishes self-localization with map in an unknown environment, and builds the corresponding incremental map on the basis of self-localization in the SLAM [5].

In traditional robot localization and tracking approaches, due to the existence of the spurious observations caused by noise, it is possible to cause poor robot location performance. The Kalman filter (KF) is a well known solution for robot tracking in SLAM, where the information about robot position and map is uncertainty [6]. In the KF, system model and measurement model are used to complete state prediction and update, respectively, and the filtering algorithm is the optimal estimator when these models are linear. However, the KF may arise poor tracking effect when the observation model is nonlinear. Using the Taylor series expansion, the extended Kalman filter (EKF) transforms nonlinear system into linear one, and can emerge good tracking performance. Some nonlinear Kalman filtering algorithms for robot tracking in SLAM were developed [7], [8], [9]. In [7], the EKF was proposed to complete the robot tracking in SLAM. In [8], the SLAM system based on EKF with a laser range finder was fulfilled for robot tracking, which is the basic framework of SLAM system. In [9], the robot pose was estimated by exploiting the EKF for the first monocular vision SLAM system. Nevertheless, the EKF based SLAM algorithm has the lower tracking accuracy due to the influence of linearization error. Using the statistical linearization technique, the unscented Kalman filter (UKF) performs better than the EKF for nonlinear system, where the unscented transform (UT) uses the sampling points to estimate the mean and covariance of state variables. In [10], the UKF based SLAM algorithm was presented to handle serious nonlinear problem. In [11], the UKF based SLAM algorithm was exploited to accomplish detection mission in the large scale outdoor environment. In [12], a path planning algorithm was presented using the UKF based SLAM algorithm to guide a robot. However, the UKF has some defects in terms of stability and accuracy, which leads to the bad performance for the UKF based SLAM algorithm [13].

The FastSLAM algorithm, as a SLAM framework using particle filter (PF), was proposed [14], [15] based on the Rao-Blackwellized particle filter (RBPF). The FastSLAM has two versions: FastSLAM 1.0 and FastSLAM 2.0. The FastSLAM 1.0 algorithm estimates the vehicle pose using the general particle filter, and evaluates the position of each feature in the map utilizing an associated set of independent EKF for each particle [15]. The FastSLAM 2.0 completes robot tracking in the same linearization manner as the EKF based SLAM algorithm, and estimates the feature states using the low-dimensional EKF [15]. However, two potential drawbacks exist in FastSLAM, i.e. the derivation of Jacobian matrices and the linear approximations of nonlinear functions. It is unwelcome effort to calculate Jacobian matrices, and the estimate accuracy is degenerated on account of the inaccurate approximation to posterior covariance. The UFastSLAM [16] was proposed to overcome the drawbacks of FastSLAM. In the UFastSLAM, the UT avoids the linearization process with Jacobian calculations [17], [18], and the UKF updates mean and covariance of the feature. However, the UFastSLAM may suffer from the accuracy degradation due to poor particle diversity and nonpositive definite covariance.

To deal with poor tracking accuracy, an improved H-Infinity unscented FastSLAM (IHUFastSLAM) with adaptive genetic resampling is proposed in this paper. First, the H-Infinity unscented Kalman filter (IHUKF) algorithm is improved as importance sampling, where the H-Infinity approach can minimize the maximum variance of the state estimates. The H-Infinity UKF algorithm derives a linear-like batch-regression form [19] that preserves all the properties of the UKF when it is projected into the H-infinity framework. Then, a fading factor based on the orthogonal principle of the residual vector is exploited to adjust the gain matrix of nonlinear system and suppress the influence of state mutation for promoting the tracking ability of UKF. Next, the Huber cost function is constructed to further improve the accuracy of H-Infinity UKF algorithm. After that a time varying noise estimator [20] is introduced to estimate process noise and measurement noise at each time and deal with the uncertain noise statistics, which improves accuracy of the H-Infinity UKF. Moreover, an adaptive genetic algorithm (AGA) is used as resampling to overcome the sample impoverishment problem of UFastSLAM, where the genetic operators, i.e., selection, crossover, and mutation, are designed according to the structure characteristic of particle filter, and the mutation probability is adaptively calculated to protect effective particles and increase particle diversity. Finally, the IHUFastSLAM with adaptive genetic resampling is applied to complete robot tracking. The proposed algorithm can achieve higher tracking accuracy compared with other related algorithms and provide reliable state estimation for robot tracking in SLAM.

The rest of this paper is organized as follows. In Section 2, the FastSLAM problem is first formulated, the unscented transform of the improved H-Infinity UKF algorithm is then described, and the noise estimation is described. In Section 3, some parameters are estimated for the improved H-Infinity UKF. In Section 4, the improved H-Infinity UFastSLAM with adaptive genetic resampling is proposed. In Section 5, some simulation experiments are operated to verify the superiority of the proposed algorithm. Finally, some conclusions are drawn in Section 6.