Journal of Sensors

Journal of Sensors / 2016 / Article
Special Issue

Sensors Technologies and Methods for Perception Systems in Intelligent Vehicles

View this Special Issue

Research Article | Open Access

Volume 2016 |Article ID 3891865 | 23 pages | https://doi.org/10.1155/2016/3891865

FastSLAM Using Compressed Occupancy Grids

Academic Editor: Maan E. El Najjar
Received21 Oct 2015
Accepted08 May 2016
Published05 Jul 2016

Abstract

Robotic vehicles working in unknown environments require the ability to determine their location while learning about obstacles located around them. In this paper a method of solving the SLAM problem that makes use of compressed occupancy grids is presented. The presented approach is an extension of the FastSLAM algorithm which stores a compressed form of the occupancy grid to reduce the amount of memory required to store the set of occupancy grids maintained by the particle filter. The performance of the algorithm is presented using experimental results obtained using a small inexpensive ground vehicle equipped with LiDAR, compass, and downward facing camera that provides the vehicle with visual odometry measurements. The presented results demonstrate that although with our approach the occupancy grid maintained by each particle uses only of the data needed to store the uncompressed occupancy grid, we can still achieve almost identical results to the approach where each particle filter stores the full occupancy grid.

1. Introduction

One of the most important problems in the development of an unmanned vehicle (UMV) is giving it the ability to be placed in a completely unknown environment and allowing it to determine () its surroundings, based on what it “sees” using available sensors, and () its position, based on how far it has moved and what it sees. In the field of robotics this problem is referred to as Simultaneous Localization and Mapping (SLAM) or Concurrent Mapping and Localization (CML). In this paper we develop a method of solving the SLAM problem which implements a representation of the world using a compressed occupancy grid [1].

The approach we develop is an extension of the FastSLAM algorithm [2] which is one of the first solutions to the SLAM problem that makes use of a particle filter. More specifically the FastSLAM algorithm uses a Rao-Blackwellized [3, 4] particle filter to generate a position estimate of the UMV while each particle in filter maintains its copy of the occupancy grid. Based on the expected size of the environment in which a UMV operates and the level of detail required, the size of each occupancy grid can become extremely large (matrices holding hundreds of thousands of entries). Furthermore, as discussed further in Section 3.1.2, the probability that the estimate produced by the FastSLAM algorithm is accurate increases as the number of particles in the particle filter increases. For these reasons the amount of computer resources required to perform the algorithm can grow quickly. This problem is exacerbated in small form factor and low cost UMVs as the computing resources available to vehicles of this type are much less than the resources available to larger and more expensive vehicles. The approach that we develop extends the FastSLAM algorithm [2] to use compressed occupancy grids to overcome the memory restrictions that can occur when storing full occupancy grids without making drastic changes to the original algorithm.

The remainder of this paper is organized as follows. The mathematical notations used in this paper along with some mathematical preliminaries are presented in Section 2. The updated FastSLAM algorithm that makes use of compressed occupancy grids is introduced in Section 3. In Section 4 four separate approaches for reconstructing the compressed occupancy grid are examined. Section 5 presents a set of performance improvements to some components of the algorithm. In Section 6 experimental results are provided and concluding remarks are presented in Section 7.

2. Mathematical Preliminaries

In much of the existing literature, the SLAM problem is addressed in a probabilistic sense. In many cases we would like to estimate the probability, also referred to as the distribution, of some random variable and we denote the distribution of as . In many cases we use some additional information to tell us something about the random variable . In this situation is referred to as the prior probability and it is all that we know about the probability of without the inclusion of , in many cases this is shortened to prior. The distribution of with the inclusion of the data is denoted as and referred to as the posterior probability and in many cases is just referred to as the posterior.

In SLAM we are attempting to estimate the pose of a UMV and a map of the environment that surrounds the UMV at some time step . In this paper we treat the world in which the UMV operates as a two-dimensional plane; thus , where are the horizontal and vertical positions of the vehicle in some frame of reference and is the heading of the UMV with respect to the positive horizontal axis in the frame of reference. From our use of occupancy grids, the estimated map is represented by a matrix , where are the number of rows in the grid and are the number of columns. We assume that the environment that the UMV operates is static; therefore during the time in which the UMV is performing the SLAM algorithm the environment does not change. Based on this assumption, to simplify the notation, the occupancy grid is denoted with . The estimate produced by SLAM, in many cases, makes use of sensor measurements and control inputs. The set of sensor measurements denotes the full set of sensor measurements for ; thus , where , , and is the number of measurements in . In the same way the full set of control inputs are defined as , where , , and is the size of the control vector . With the notation defined and the required mathematical preliminaries provided, the FastSLAM algorithm that makes use of compressed occupancy grids is presented in the following section.

3. FastSLAM with Compressed Occupancy Grids

In general the goal of SLAM is to estimate the pose of a UMV and the map, , of the unknown environment in which the UMV is operating at some time step . We present an extension of the FastSLAM algorithm that uses compressed occupancy grids to store the map. Before presenting our approach, an overview of the FastSLAM algorithm that uses standard occupancy grids (FastSLAM OG) is provided.

3.1. FastSLAM with Occupancy Grids

As opposed to many SLAM solutions that attempt to solve the online SLAM problem that is estimating the posterior that represents the pose of a UMV and the map of the environment based on a set of control inputs and sensor measurements, the FastSLAM approach attempts to estimate the full SLAM posterior. The full SLAM posterior is given aswhich is the distribution that represents the trajectory of UMV and the map of the environment based on the set of control inputs and observations . By estimating the full SLAM posterior, (1) can be factored, using the property of conditional independence, into a pair of simpler terms which is given bywhere is the distribution that represents the UMV trajectory and is the distribution that represents the map of the environment. The FastSLAM approach estimates the factored SLAM posterior (2) using a Rao-Blackwellized particle filter. The Rao-Blackwellized particle filter is a version of a sampling importance resampling (SIR) [5] particle filter and it is the SIR particle filter that provides the format of the FastSLAM solution. The Rao-Blackwellized particle filter estimates the distribution representing a portion of the state, which in FastSLAM is the UMV trajectory , using a particle filter, and each particle in the particle filter maintains its own estimate of the remainder of the state, which is the map . As a result, each particle in the particle filter is composed of a UMV pose and a map, which the FastSLAM OG algorithm represents using an occupancy grid; thus the th particle at time is defined aswhere is the pose estimate of the th particle at time , is the occupancy grid of the environment maintained by the particle as it has been generated through time step , and is the index corresponding to the th particle where is the total number of particles used in the particle filter. The FastSLAM approach is implemented in a four-step procedure based on the use of a SIR particle filter to estimate (1) and an overview of the algorithm is presented in Algorithm 1 for time .

) procedure  FASTSLAMOG()
()  
()  for  ,   do
()    
()    
()     = OGUPDATE
()    
()  end for
()  
()  for  ,   do
()    select with probability proportional to
()    
()  end for
() end procedure
3.1.1. Motion Model Update

In the first step of the algorithm (Line () in Algorithm 1) a new particle pose is generated and added to a set of temporary particles, . The new pose for the th particle is generated by sampling from the probabilistic motion model of the UMV:The probabilistic motion model of the UMV generates a new pose based on the state transition model of the UMV, the pose of the th particle at time step , the current control input , and the characteristics of the noise on . The result is that the particles in are distributed according towhich is referred to as the proposal distribution. The actual distribution which we are attempting to estimate is referred to as the target distribution and given asThe proposal distribution is transformed to the target distribution in the last stage of the algorithm, the resampling stage (Lines ()–() in in Algorithm 1). Before the resampling process can occur, must be incorporated into the estimate, which is achieved in the calculation of the particle’s importance weight.

3.1.2. Importance Weight Calculation

In the second step of the algorithm an importance weight for each particle is generated. The need for an importance weight for each particle comes from the use of a SIR particle filter and, as seen in the previous section, we are sampling particles from a distribution different from the one that we are attempting to estimate. According to [6], if we are unable to directly sample from the distribution that we are attempting to estimate, then the importance weight for each particle is given as and particles are drawn from with replacement and added to with a probability proportional to . When drawing particles from with replacement, each particle that is added to remains in so that a given particle that is selected has the potential of being added to multiple times. After the resampling process will approximate the target distribution and the quality of the approximation will improve, the number of particles increases.

Using (7) along with the previously discussed distributions (5) and (6), the importance weight of each particle simplifies to [7]where is the probabilistic measurement model of the sensor being used by the UMV and is a normalizing factor. The probabilistic measurement model is dependent on the perceptual sensor being used by the UMV and it calculates the probability that the sensor measurement would make based on the current pose estimate and map maintained by the particle. This form of importance weight incorporates the sensor observations into the estimate which were not included in the pose sampling procedure. Once the importance weight has been calculated for each particle then the occupancy grid maintained by each particle is updated using the new particle pose and current sensor measurement.

3.1.3. Occupancy Grid Update

Once the importance weight for each particle has been calculated, the third step of the algorithm updates the occupancy grid of each particle using the current sensor measurement and pose estimate. Occupancy grids decompose an unknown environment into a finite set of cells where each cell holds the probability that the location in the world enclosed by the cell is occupied by an object. From [8], the occupancy grid for a given particle at time is obtained in a probabilistic sense by calculating the posterior over maps based on the history of sensor measurements and trajectory of a particle:By decomposing the environment into cells, the posterior calculated by the occupancy grid can be restated as a product of cellular posteriors:where contains the probability that the location in the environment represented by the cell at is occupied based on the measurement history and particle trajectory. A solution to a problem of this form is the binary Bayes filter [8]. The binary Bayes filter stores the probability of occupancy for each cell in log odds form to prevent truncation errors that can occur during the update process. The log odds form of the variable is given asand the value of can be recovered from the log odds form asThe binary Bayes filter based occupancy grid update modifies the probability of occupancy of each cell in the grid when an observation provides information about that location. As a consequence, for locations that have been observed, the probability of occupancy quickly approaches for free cells and for occupied cells. While the addition of new information to the estimate makes the probability quickly approach the limits, the probability of occupancy of each cell should never equal or as this would assume perfect knowledge about the occupancy of the cell. Depending on the data type selected to store the probability, the precision required to store the value can quickly be exceeded resulting in the probability being rounded to either or . Once this rounding has occurred any new information learned about the occupancy of a cell is lost. To overcome this problem the binary Bayes filter stores the probability of occupancy in log odds form which takes the potential probability values in and spreads them over the range . By spreading the small range of probability values over this much larger range, the data type being used to store the probability has more precision to store the probability thus preventing the truncation/rounding from occurring. The occupancy grid update algorithm based on the binary Bayes filter is provided in Algorithm 2.

) procedure OGUPDATE()
()  for  ,   do
()    for  ,   do
()      for  ,   do
()        if   lies in the perceptual range of   then
()          
()        end if
()      end for
()    end for
()  end for
() end procedure

In Algorithm 2, is the inverse sensor model of the sensor being used to generate the map and it returns the probability that the cell at is occupied based on the th sensor measurement and the pose estimate of the particle. The value of in Algorithm 2 is referred to as the prior of occupancy and is defined as and gives the probability that a cell is occupied before any sensor measurements are integrated into the map. Typically in (13) is selected to be .

3.1.4. Resampling

After the occupancy grid for each particle in has been updated, the final step of the algorithm is the resampling process. During the resampling process particles are drawn with replacement from with a probability proportional to . This resampling step takes the set of temporary particles that are distributed according to (5) and ensures that there is a high probability that is distributed according to (6).

One of the problems with this approach is the large amount of memory required to run the algorithm. The estimate generated by the algorithm improves as the number of particles increases; thus we like to have as many particles as possible. However, each particle must keep a copy of the occupancy grid and each occupancy grid can be quite large. It is easy to see that while we would like to run the particle filter with as many particles as possible we can be limited by the fact that each particle must maintain a full copy of the occupancy grid. To address this problem we propose an updated version of the algorithm that decreases the amount of memory required for each particle by storing a compressed version of the occupancy grid as opposed to the full uncompressed occupancy grid.

3.2. FastSLAM with Compressed Occupancy Grids

Before discussing the updated SLAM algorithm, we will first address the compressed form of the occupancy grid. To compress we first reshape the occupancy grid so that matrix becomes a column vector containing elements and we refer to the vector form of the occupancy grid as . The reshaping process is performed in such a way that the cell in the original occupancy grid located at can be accessed in the vector form at , where . In order to compress the occupancy grid without major degradation, we assume that the occupancy grid can be represented in an alternate basis in which it is sparse. The relationship between the two different representations is given aswhere is a transformation matrix that defines the relationship and is the set of coefficients that represent the occupancy grid in the alternate basis. In order for the occupancy grid to be compressible, must be sparse [9]; that is , where is the support operator and denotes the cardinality of the set. This means that, in order for us to be able to compress the occupancy grid, we must find a basis in which the coefficient vector is composed of a large number of zeros. If this condition is met then the occupancy grid for a given particle can be compressed usingwhere is a matrix that performs the compression by selecting a subset of coefficient data that is smaller than the amount of data in the complete signal.

The FastSLAM COG algorithm is a modification of Algorithm 1. Since we are now maintaining the compressed form of the occupancy grid, th particle in the particle set is defined asAs with FastSLAM OG, FastSLAM COG is performed in several steps and an overview of the updated algorithm is presented in Algorithm 3. As a result of using compressed occupancy grids the only changes that must be addressed are the steps of the algorithm that require the occupancy grid, the importance weight calculation, and occupancy grid update.

() procedure FASTSLAMCOG()
()   
()   for  ,   do
()     
()      = UNCOMPRESS()
()     
()      = ALPHACONSTRUCTION()
()     
()     
()   end for
()   
()   for  ,   do
()     sample with replacement from select with probability proportional to
()     
()   end for
() end procedure
3.2.1. Importance Weight Calculation

As discussed in Section 3.1.2, the importance weight for each particle is the normalized probabilistic measurement model for each particle. The probabilistic measurement model is a measure of how well the current set of sensor measurements matches up with what we expect based on the current pose and map of the particle. In calculating the probabilistic measurement model, the algorithm must have access to the raw occupancy grid values so that an expected sensor measurement can be generated. Based on this requirement, the compressed occupancy grid coefficients are first uncompressed. For now a general reconstruction operator denoted as is used; however the details of several reconstruction methods are explored in detail in Section 4. Once the compressed coefficients have been reconstructed, the occupancy grid can be extracted using (14). The importance weight calculation can be found in Algorithm 3 on Line ().

3.2.2. Compressed Occupancy Grid Update

As previously seen in Algorithm 2, the standard approach to updating the occupancy grid is to examine every cell in the occupancy grid and if a given cell falls within the perceptual range of the sensor attached to the UMV then the corresponding cell of the occupancy grid is updated according to Using Algorithm 2 as our basis, we rewrite the occupancy grid update in vector form. If we assume that the full occupancy grid is stored in vector form, then the occupancy grid update can be performed usingwhere is constructed using Algorithm 4 and is vector with every element containing .

) procedure ALPHACONSTRUCTION()
()  for  ,   do
()    
()  end for
()  for  ,   do
()    for  ,   do
()      if   in the perceptual range of   then
()        
()      end if
()    end for
()  end for
() end procedure

Algorithm 4 first initializes using . This ensures that cells that do not fall within the perceptual range of the sensor remain unchanged by the algorithm. For cells that fall into the perceptual range of the sensor, their value is set as the log odds form of the inverse sensor model and it is this value that either increases or decreases the likelihood that the cell is occupied. Equation (18) updates the occupancy grid in vector form; however we need to modify it so that it updates the occupancy grid which is stored using an alternative basis. Incorporating (14), the vector form of the occupancy grid update in the alternate basis becomeswhere is used to convert and to the alternate basis. In (19), is used to change basis as opposed to because we assume that our choice of basis is orthonormal so ; however if the alternative basis that is selected is not orthonormal then should replace in (19). We can now simplify (19) by grouping the two components that are multiplied by to reduce the number of matrix-vector multiplications; this yields

Equation (20) updates the occupancy grid in the alternate basis; hence the final step is to incorporate compression into the update. The occupancy grid maintained by each particle contains the compressed set of basis of coefficients; thus (20) can be updated using (15) to yield

Finally, the basis coefficients used to represent the occupancy grid must be recompressed in order to be stored by the particle. Including the final compression the compressed occupancy grid update can be performed according towhich is seen in Algorithm 3 Line ().

4. Compressed Signal Reconstruction Methods

As previously described we would like to store a compressed version of the occupancy grid as opposed to the complete occupancy grid to decrease the memory requirements of the algorithm. In order to make use of the compressed occupancy grid, a method for reconstructing the full occupancy grid from the compressed version must be selected. In this section several reconstruction methods are examined with the goal of selecting an approach to be used in FastSLAM COG. In FastSLAM COG we are attempting to compress the vector form of an occupancy grid; however in this section we will examine methods for reconstructing a generic discrete signal that has been compressed.

We represent using some alternate basis in which it is sparse and the relationship between and its sparse representation is given bywhere is a vector of coefficients that represent in the alternate basis and is a matrix of basis functions that determines the relationship between and . The discrete signal used in this section for testing is displayed in Figure 1(a) and the set of coefficients used to represent the signal in the discrete cosine basis [10] can be seen in Figure 1(b). The test signal is generated by evaluating at evenly spaced points in .

If is sparse then can be compressed; this sparseness can be seen by examining Figure 1(b) and noticing that a large number of the coefficients are zero. The compression is performed by selecting a subset of the coefficient information usingwhere is the compression matrix and is the size of the compressed signal with .

4.1. Reconstruction ()

One of the most simple methods for reconstructing a compressed signal is to reconstruct the signal while minimizing the error between the original and reconstructed signals as measured by norm, where norm of a signal is defined as

In examining (25), if the size of the compressed signal is the same as the original then the reconstruction can be performed according toIn reality the system is underdetermined due to . A common solution to problems of this form uses the Moore-Penrose pseudoinverse of .

The Moore-Penrose pseudoeinverse [11, 12] is used to find the solution that minimizes error for systems of the formwhere , , and . The solution to the system found using the pseudoinverse is defined aswhere is the pseudoinverse of and it minimizes error so thatfor all if is defined asfor any vector .

Using the pseudoinverse of a set of reconstructed coefficients is generated according toand the reconstructed value of the signal is generated from

The results of reconstruction using four separate amounts of signal information, , , , and , are presented in Figure 2. The form selected for needed to obtain such level of compression is an orthonormal Gaussian random matrix as it possesses the RIP property whose importance will be discussed in the following section.

4.2. Compressed Sensing Reconstruction (CS)

The second reconstruction algorithm selected is based on an approach referred to in the literature as compressed sensing [13]. Compressed sensing allows for sparse signals to be reconstructed using fewer measurements that would be required according to the Nyquist/Shannon sampling principle. In compressed sensing the full set of sparse coefficients can be reconstructed from the compressed set by solving for some sufficiently small . In (34) is a count of the number of nonzero elements in the vector . Solving (34) is NP-hard and the solution cannot be found efficiently. A key finding of compressed sensing is that if the measurement matrix, , is constructed in such a way that it obeys the Restricted Isometry Property (RIP), which is defined as form some small , then in (34) can be replaced by resulting in As opposed to being NP-hard to solve, (36) is a constrained convex optimization problem and can be solved efficiently using one of several available software packages with Lasso [14], NESTA [15], and l1 magic [16] being some examples. As in case, once the complete set of coefficients have been estimated using the compressed set, an estimate of the original signal is generated according to (33).

In order to examine the performance of the compressed sensing reconstruction approach, the test signal is reconstructed using the same four amounts of data, , , , and , with the same orthonormal Gaussian random matrix being used to perform the compression. Equation (36) is solved using the NESTA software library and the results of the reconstruction are shown in Figure 3.

4.3. Compressed Sensing Embedded Kalman Filtering (CSEKF)

In the previous section a reconstruction approach was introduced that allows for the reconstruction of compressed signals using less data than expected based on the Nyquist/Shannon sampling principle. As seen in Figure 3 this method does a better job at reconstructing a sparse signal than the approach that reconstructs the signal by minimizing norm. The authors of [17] introduce an approach that performs the compressed sensing reconstructing using an augmented Kalman filter (KF) [18] which is easy to implement, compared to the compressed sensing approach. The compressed sensing approach must solve a complex convex minimization problem, where problems of this type are typically being solved using an external software package. In order to use the KF it is assumed that the full set of coefficients evolve from time step to time step according towhere is the state transition matrix of the system which describes how the signal coefficients change between time steps, and is a zero mean Gaussian vector with covariance . The compressed form of the coefficients is generated according towhere is the same compression matrix discussed in (25) and is a zero mean Gaussian vector with covariance .

The CSEKF algorithm first performs a standard KF estimation that is carried out in a two parts, a prediction phase and a correction phase. The KF estimates as a Gaussian distribution that can be represented using a mean vector and covariance matrix . The KF generates the estimate using a standard set of five equations [18]:where in (43) is dimensional identity matrix. The estimate produced by the KF minimizes the error, using norm, between the estimate and the actual set of coefficientshowever, this is not what we are trying to solve according to the compressed sensing approach. The CSEKF presented by the authors replaces the classic compressed sensing convex optimization problem (36) with the dual problem:They solve this constrained optimization problem by iteratively applying the pseudoobservation method presented in [19] to the estimate generated by the KF. According to this approach a pseudoobservation is generated using the constraint being applied to the estimate which is defined aswhere the constraining value is now treated as a Gaussian noise on the pseudomeasurement with a covariance of . Using the definition of the pseudomeasurement can be written aswhere the observation matrix of the pseudoobservation is defined as

The constraint is then iteratively applied times to the estimate produced by the KF using the standard KF correction equations (41)–(43). First, a new Kalman matrix is generated using the observation matrix of the pseudomeasurement and the covariance of the constraint:Next, the mean of the estimate is updated using the standard mean update equation with the assumption that the measured value is . Using this assumption the mean update equation can be simplified toFinally, the covariance of the estimate is constrained using the standard covariance measurement update equationThe full algorithm that combines all of these separate components to reconstruct the compressed signal using the CSEKF is shown in Algorithm 5.

) procedure CSEKF
()   
()   
()   
()   
()   for  ,   do
()     
()     
()     
()     
()   end for
() end procedure

The reconstructed test function for the same four test percentages as in the previous two algorithms is shown in Figure 4. The algorithm is implemented as shown in Algorithm 5 with the tuning parameters being chosen as , , , and . Since the input vector is static the initial estimate of the coefficient vector was zero, , the state transition matrix is chosen as , and the initial covariance matrix of the estimate is chosen to be .

4.4. Kalman Filter Constrained with Quasinorm (CSEKF-)

In the previous section an approach was presented that solves a problem similar to the standard compressed sensing problem (36) using a KF that is constrained by iteratively applying a pseudoobservation to the estimate generated by the KF. The standard approach to solving the compressed sensing problem replaces in (34) with . A new approach has been presented by the authors of [20] that replaces the zero norm with -norm which for is defined asThis approach has been shown to yield better accuracy than using in some cases. Using this approach, we would like to constrain the estimate produced by the KF with the quasinorm as opposed to .

Just as in the previous section the initial estimate is produced using the standard KF equations. The pseudoobservation used to apply the constraint to the KF estimate using is defined aswhere is again treated as noise on the pseudoobservation with a covariance of . Unlike in the previous case where the constraint was linear and could be applied using the pseudoobservation matrix, the constraint applied by -norm is nonlinear and can be rewritten aswhereSince the constraint using -norm is nonlinear the constraint is applied to the KF estimate by iteratively performing a measurement update using (54) and the Extended Kalman Filter (EKF) [21] update equations. Just as in the linear case the first step in applying the constraint is to construct the Kalman matrix according towhere is now the Jacobian of and is defined aswhere . The constraint is applied to the mean of the estimate using the standard EKF mean measurement update equation, again with the assumption that the measured value is zero, which when simplified is written asThe final step in applying the constraint is to update the covariance of the estimate using the standard EKF covariance measurement update equationThe complete algorithm for reconstructing a compressed signal using the estimate produced by the KF constrained using -norm is shown in Algorithm 6.

) procedure CSEKFP
()   
()   
()   
()   
()   for  ,   do
()     for  ,   do
()       if    then
()        
()       else
()         
()       end if
()     end for
()     
()     
()     
()     
()   end for
() end procedure

The reconstruction results for the compressed sensing Kalman filter using a quasinorm to constrain the estimate are presented in Figure 5 for the same four data sizes used for the three previous algorithms. A parameter that greatly effects the performance of the algorithm is the value chosen for . To determine the best value of the algorithm was run for a single data percentage, , for equally spaced values of . A plot of the mean square error (MSE) between the compressed signal and original signal can be seen in Figure 6. From this graph the value of was chosen to be . The remainder of the tuning parameters for the algorithm were selected to be the same as those used for the CSEKF reconstruction.