Random Finite Sets for Robot Mapping and SLAM

Machines which perceive the world through the use of sensors, make computational decisions based on the sensors’ outputs and then influence the world with actuators, are broadly labelled as “Robots”. Due to the imperfect nature of all real sensors and actuators, the lack of predictability within real environments and the necessary approximations to achieve computational decisions, robotics is a science which is becoming ever more dependent on probabilistic algorithms. Autonomous robot vehicles are examples of such machines, which are now being used in areas other than the factory floors, and which therefore must operate in unstructured, and possibly previously unexplored environments. Their reliance on probabilistic algorithms, which can interpret sensory data and make decisions in the presence of uncertainty, is increasing. Therefore, mathematical interpretations of the vehicle’s environment which consider all the relevant uncertainty are of a fundamental importance to an autonomous vehicle, and its ability to function reliably within that environment. While a universal mathematical model which considers the vast complexities of the physical world remains an extremely challenging task, stochastic mathematical representations of a robots operating environment are widely adopted by the autonomous robotic community. Probability densities on the chosen map representation are often derived and then recursively propagated in time via the Bayesian framework, using appropriate measurement likelihoods.

We begin the justification for the use of RFSs by re-evaluating the basic issues of feature representation, and considering the fundamental mathematical relationship between environmental feature representations, and robot motion. We further the justification for the use of RFSs in FBRM and SLAM by considering an issue of fundamental mathematical importance in any estimation problem - estimation error.

The previous chapter provided the motivation to adopt an RFS representation for the map in both FBRM and SLAM problems. The main advantage of the RFS formulation is that the dimensions of the measurement likelihood and the predicted FBRM or SLAM state do not have to be compatible in the application of Bayes theorem, for optimal state estimation. The implementation of Bayes theorem with RFSs (equation 2.15) is therefore the subject of this chapter. It should be noted that in any realistic implementation of the vector based Bayes filter, the recursion of equation 2.13 is, in general, intractable. Hence, the well known extended Kalman filter (EKFs), unscented Kalman filter (UKFs) and higher order filters are used to approximate multi-feature, vector based densities. Unfortunately, the general RFS recursion in equation 2.15 is also mathematically intractable, since multiple integrals on the space of features are required. This chapter therefore introduces principled approximations which propagate approximations of the full multi-feature posterior density, such as the expectation of the map. Techniques borrowed from recent research in point process theory known as the probability hypothesis density (PHD) filter, cardinalised probability hypothesis density (C-PHD) filter, and the multi-target, multi-Bernoulli (MeMBer) filter, all offer principled approximations to RFS densities. A discussion on Bayesian RFS estimators will be presented, with special attention given to one of the simplest of these, the PHD filter. In the remaining chapters, variants of this filter will be explained and implemented to execute both FBRM and SLAM with simulated and real data sets.

The notion of Bayes optimality is equally as important as the Bayesian recursion of equation 2.15 itself. The following section therefore discusses optimal feature map estimation in the case of RFS based FBRM and SLAM, and once again, for clarity, makes comparisons with vector based estimators. Issues with standard estimators are demonstrated, and optimal solutions presented.

Estimating a FB map requires the joint propagation of the FB map density encapsulating uncertainty in feature number and location. This chapter addresses the joint propagation of the FB map density and leads to an optimal map estimate in the presence of unknown map size, spurious measurements, feature detection and data association uncertainty. The proposed framework further allows for the joint treatment of error in feature number and location estimates. As a proof of concept, the first-order moment recursion, the PHD filter, is implemented using both simulated and real experimental data. The feasibility of the proposed framework is demonstrated, particularly in situations of high clutter density and large data association ambiguity. This chapter establishes new tools for a more generalised representation of the FB map, which is a fundamental component of the more challenging SLAM problem, to follow in Part II.

The feature-based (FB) SLAM scenario is a vehicle moving through an environment represented by an unknown number of features. The classical problem definition is one of “a state estimation problem involving a variable number of dimensions” [28]. The SLAM problem requires a robot to navigate in an unknown environment and use its suite of on board sensors to both construct a map and localise itself within that map without the use of any a priori information. Often, in the planar navigation context, a vehicle is assumed to acquire measurements of its surrounding environment using on board range-bearing measuring sensors. This requires joint estimates of the three dimensional robot pose (Cartesian x and y coordinates, as well as the heading angle θ), the number of features in the map as well as their two dimensional Euclidean coordinates. For a real world application, this should be performed incrementally as the robot manoeuvres about the environment. As the robot motion introduces error, coupled with a feature sensing error, both localisation and mapping must be performed simultaneously [8]. As mentioned in Chapter 2, for any given sensor, an FB decision is subject to detection and data association uncertainty, spurious measurements and measurement noise, as well as bias.

This chapter proposes an alternative Bayesian framework for feature-based SLAM, again in the general case of uncertain feature number and data association. As in Chapter 5, a first order solution, coined the probability hypothesis density (PHD) SLAM filter, is used, which jointly propagates the posterior PHD of the map and the posterior distribution of the vehicle trajectory. In this chapter however, a Rao-Blackwellised (RB) implementation of the PHD-SLAM filter is proposed based on the GM PHD filter for the map and a particle filter for the vehicle trajectory, with initial results presented in [56] and further refinements in [57].

This book demonstrates that the inherent uncertainty of feature maps and feature map measurements can be naturally encapsulated by random finite set models, and subsequently in Chapter 5 proposed the multi-feature RFSSLAM framework and recursion of equations 5.5 and 5.6. The SLAM solutions presented thus far focussed on the joint propagation of the the first-order statistical moment or expectation of the RFS map, i.e. its Probability Hypothesis Density, v _k , and the vehicle trajectory. Recall from Chapter 3 that the integral of the PHD, which operates on a feature state space, gives the expected number of features in the map, at its maxima represent regions in Euclidean map space where features are most likely to exist.