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 .
| ||||||||||||||||||||||||||||
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.
| ||||||||||||||||||||||
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.
| ||||||||||||||||||||||||||||||||
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 .
| ||||||||||||||||||||||||
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 .
(a)
(b)
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.
(a)
(b)
(c)
(d)
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.
(a)
(b)
(c)
(d)
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.
| ||||||||||||||||||||||||
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 .
(a)
(b)
(c)
(d)
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.
| ||||||||||||||||||||||||||||||||||||||
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.
(a)
(b)
(c)
(d)
4.5. Algorithm Comparison
Before deciding which reconstruction algorithm should be used in the FastSLAM COG algorithm we examine the performance of each of the algorithms in two key areas: accuracy and run time. First, we examine how well each algorithm reproduces the original signal based on the amount of information that is stored in the compressed signal. To compare how well each algorithm reconstructs the signal, each algorithm was run for equally spaced percentages in . The results for each of the algorithms are shown in Figure 7 which plots the MSE as a function of the amount of data stored. As seen in the results, in general each of the compressed sensing type approaches produces less of an error for a given data percentage than approach.
The second key characteristic of each algorithm examined is the time it takes for each algorithm to run. During the error performance testing described above, the time taken to run each algorithm was recorded. The run time as a function of the amount of compressed data used to represent the signal can be seen in Figure 8. As seen in the timing results the two algorithms that use the augmented Kalman filter have run times that are significantly larger than the other approaches. This increased run time is due to the number of matrix-matrix multiplications required by the Kalman filter which are computationally heavy, from [22] having . The remaining two algorithms, and compressed sensing, can be implemented completely using only matrix-vector multiplications, which can be performed more quickly than matrix-matrix multiplications.
5. Performance Improvements
Our overall goal is to augment the FastSLAM algorithm to use compressed occupancy grids. In order for us to accomplish this task the reconstruction approach selected must run thousands or hundreds of thousands of times during the operation of a UMV. By using a Rao-Blackwellized particle filter, we can increase the probability that our SLAM posterior estimate matches the true SLAM posterior by increasing the number of particles maintained by the particle filter. As seen in Algorithm 3 the reconstruction process must be performed by each particle at each iteration; thus the execution time of the reconstruction process is important in the overall performance of the algorithm.
In Section 4 four different algorithms were presented that can be used to reconstruct an occupancy grid from the compressed form. From the timing results (Figure 8), the two approaches that make use of the Kalman filter (CSEKF and CSEKF-) take much longer time to run than and CS. The longer run times of the Kalman filter based approaches are due to the large number of matrix-matrix multiplications. Based on their performance the CSEKF and CSEKF- algorithms will not be used in FastSLAM COG. We will now present a method that can be used to improve the accuracy of the remaining two algorithms and a method that improves the run time of approach.
5.1. Updated Compression Step
As seen in Section 3.2 the compressed occupancy grid is generated by selecting a subset of the information stored in the coefficients. This compression is performed according to (15) which makes use of to perform the compression. As stated in Section 4.2 we would like for this compression matrix to obey the RIP property. Several common matrices that are used in compressed sensing applications are the discrete Fourier transform [23], the discrete cosine transform [10], the Hadamard transform [24], and noiselet transform [25]. Each of these matrices is constructed in such a way that each of the elements stored in the compressed vector stores a small amount of information about each of the coefficients used to represent the original signal and this information can be recovered by knowing . These types of matrices are useful when there is no knowledge about the sparse signal that is being compressed; however we are constructing the sparse signal thus we have full knowledge of the signal before the compression is performed. Using this knowledge we present an alternative form of that allows for better reconstruction using a smaller amount of information. The form of selected comes from the definition of the square error defined according to norm. For some discrete signal the square of the error between the signal and the approximation of the signal generated using elements is defined aswhere is the standard inner product. The discrete signal can be represented in some alternate basis so that a single element can be expressed asRecalling that denotes the compressed set of coefficients (25), a single element of the compressed signal can be expressed in the alternate basis aswhere
While this is a valid approach for compressing a signal, we would like to simplify the compression process for performance reasons. Instead of generating a new set of basis function it is easier to keep the basis functions the same at all times and just select a subset of the coefficients to represent the signal. In order to decide which coefficients should be selected that minimize the error based on norm, we define several sets. The set is the full set of indices of the discrete vector being compressed. The set is the set of indices that are used to represent the compressed signal, . Finally, the set is the set of indices of coefficients not used in the compressed signal, . Using these sets and the fact that a constant basis is used to represent the uncompressed and compressed signal an element in the reconstructed signal can be written asThe square of the error in norm for a single element becomesand if we assume that the set of basis function is orthonormal, which is common, then error for the entire compressed signal becomesThis tells us that the best way to minimize the error in the reconstructed signal is to select the coefficients that have the largest magnitudes. Using this knowledge a new compression algorithm that does not require matrix-vector multiplications and selects only coefficients with the largest magnitudes was developed and shown in Algorithm 7.
| ||||||||||||||
To examine how the use of the new compression algorithms affects the performance of the reconstruction algorithms, the occupancy grid shown in Figure 9(a) is reconstructed from the compressed form. The compression matrix selected for the baseline results is different from the orthonormal Gaussian random matrix used in Section 4; in its place a normalized Hadamard matrix was used. Each of the two algorithms was then run using each of the two compression methods for separate compression percentages in . The performance results of each of the two algorithms are provided in Figure 10. As seen from the results for each of the two reconstruction algorithms the occupancy grid is reconstructed with a smaller MSE using the compression matrix that keeps most significant coefficients as opposed to the compression step using the normalized Hadamard matrix using the same amount of data.
(a)
(b)
5.2. Updated Reconstruction
In Section 4.1 a method for reconstructing a compressed signal was introduced that minimizes the error according to norm. The method makes uses of the Moore-Penrose pseudoinverse to reconstruct the set of coefficients from their compressed form according toThis method of reconstructing the signal is made up of two steps where the performance of each step is based on the size of the signal (). The first step in the process is the calculation of the pseudoinverse of the compression matrix . Many common software implementations of this calculation, for example, Matlab’s function [26], makes use of the Singular Value Decomposition (SVD) of the matrix. The process of calculating the SVD of a matrix is worse from a performance perspective than that of matrix-matrix multiplication which is [27] for matrix.
The second step of the algorithm is a matrix-vector multiplication operation which in a naive implementation has a performance of . We would like to improve the performance of the reconstruction algorithm to get rid of these computationally intensive steps. By making use of the compression matrix developed in the previous section that just selects coefficients with the largest magnitudes, the performance of the reconstruction process can be improved. Each of the rows in the compression matrix is orthogonal; that is, for two rows in the selection matrices and the discrete inner product is zero; . A second property is that each row of is normalized; ; thus is orthonormal. The pseudoinverse of a orthonormal matrix can be easily computed without the computationally intensive SVD process according to where is the conjugate transpose of and is found by taking the transpose of followed by the complex conjugate of each element in . Since the compression matrix is composed of elements containing just or the pseudoinverse is just the transpose .
Finally, in (67) we can replace the pseudoinverse of with the transpose; thus the reconstruction process becomesWe can improve the performance even more if we assume that the compression is performed as discussed above, that is, where coefficients with the largest magnitudes are selected. In that case then is full of zeros except for elements that correspond to the coefficients with largest magnitudes. Using this information we simplify reconstruction algorithm to just generate a vector of zeros and then copy the stored coefficients into their proper locations in the reconstructed vector. This replaces the matrix-vector multiplication with value copies. The updated form of reconstruction algorithm that does not require the potentially expensive calculation of the matrix pseudoinverse or matrix-vector multiplication is presented in Algorithm 8.
| ||||||||||||||||
To examine the performance benefit of the new reconstruction algorithm a compressed form of the occupancy grid seen in Figure 11 is reconstructed using the original algorithm explained in Section 4.1 and the new algorithm described in Algorithm 8. The algorithm is run for varying compressed data sizes in and the timing results are seen in Figure 11. As seen from the results the run time for the new reconstruction remains small as the size of the compressed signal grows while the original reconstruction approach slows down as the size of the compressed signal grows.
6. Experimental Results
We would now like to examine how using compressed occupancy grids affects the FastSLAM OG algorithm. Based on the timing results presented in Section 4 only two of the uncompression methods, (Section 4.1) and CS (Section 4.2), were selected to use in the FastSLAM COG algorithm. Both of the algorithms are compared using data captured by a small unmanned ground vehicle (UGV). The UGV is equipped with Mecanum wheels [28] which allow the vehicle to move in any direction without the constraints that are typical with many ground vehicles. The use of Mecanum wheels makes it difficult to use standard wheel encoders for localization purposes; to overcome this a downward facing camera is attached to the UGV which provides visual odometry data for localization using the approach described in [29]. Other sensors attached to the UGV include a digital compass to provide the global heading of the vehicle and a Hokuyo UTM-30LX LiDAR sensor, with reported accuracy of to m ± 30 mm and to m ± 50 mm, to provide range and bearing measurements to objects in the environment. In order to provide a “ground truth” trajectory for our test, a Hagisonic StarGazer indoor localization system was used. This sensor uses an upward facing camera and static landmarks placed on the ceiling to triangulate a robotic vehicle in an environment. This sensor has a precision between mm and mm based on how many landmarks the sensor can see at a given time. This accuracy was much higher than the visual odometry algorithm that provided the odometry input to the SLAM algorithm; thus this trajectory was used to compare the effect of using compressed occupancy grids in our FastSLAM algorithm as opposed to the full occupancy grid.
For comparison purposes, the standard FastSLAM OG algorithm described in Algorithm 1 was used to generate a baseline vehicle path and occupancy grid map. Once the baseline was generated the FastSLAM COG algorithm described in Algorithm 3 was run using the same data used by the standard approach. For each of the two reconstruction algorithms the compression method used is the optimized approach described in Section 5.1 and approach is implemented as described in Section 5.2. For the CS reconstruction algorithm the convex optimization problem is solved using the NESTA library.
The UGV was remotely driven around a small enclosed 10 ft × 12 ft environment while all of the sensor data was logged to memory on a small attached computer. The data was then postprocessed to produce a baseline vehicle path and an occupancy grid representing the testing environment. The baseline vehicle path, along with the true vehicle path, can be seen in Figure 12(a) and the occupancy grid with a 0.1 m cell size is shown in Figure 12(b). The environment used to generate the baseline shown in Figures 12(a) and 12(b) is a simple rectangular indoor environment. Our choice of this simple environment, as opposed to a more complex environment, was made for two reasons. First, in order to properly compare how the use of compressed occupancy grids affected the localization and mapping an accurate baseline was required for comparison purposes. Based on the sensors that we had at our disposal, an indoor testing was more ideally suited for this task. Secondly, the goal of this research was not to examine how well an occupancy grid based SLAM algorithm performs in complex environment that was beyond our scope and addressed by others; rather our goal was to examine how the performance of the approach was affected by compressing the occupancy grid used to represent the environment, thus testing in complex environment was not attempted for this initial research.
(a)
(b)
The vehicle path estimate generated by FastSLAM COG algorithm using reconstruction can be seen in Figure 13(a) and the reconstructed occupancy grid is shown in Figure 13(b). The generated path estimate using compressed sensing reconstruction approach is shown in Figure 14(a) and the reconstructed occupancy grid is shown in Figure 14(b). For each of the algorithms of the amount of data used to represent the full occupancy grid is used. Figure 15 shows the mean square error (MSE) in the path estimate and reconstructed occupancy grid between the uncompressed form of the algorithm and the compressed form of the algorithm. As Figure 15 shows using more than of the data does not decrease the MSE any more than the error that exists when using of the data.
(a)
(b)
(a)
(b)
(a)
(b)
The plots provided in Figure 16 illustrate how a decrease in the amount of stored data decreases the quality of the reconstructed occupancy grid. This set of plots shows a series of occupancy grids that were generated as the final map of by the FastSLAM COG algorithm using reconstruction approach for decreasing amounts of data. As can be seen from the first several grids, up to of the data show no decrease in quality; however as the amount of data used continues to decrease the amount of error in the final occupancy grid increases until the grid becomes unusable. A similar set of plots are provided in Figure 17 for the FastSLAM COG algorithm using the compressed sensing reconstruction approach and similar results can be seen.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)
By examining Figure 15 it can be seen that there is a constant error that appears in both the path estimate and the occupancy grid reconstruction from using the compressed sensing approach. This error comes from the parameters that are chosen for the NESTA library. Of most significance is the value of from (36) which is the amount of error that is allowed between the coefficients that represent the compressed form of the occupancy grid and the value of those coefficients in the reconstructed occupancy grid. For the presented results the value of was chosen to be which is quite small; however it does allow for small errors to occur in the reconstructed occupancy grid. These errors can be reduced or removed completely, from the estimate by choosing a smaller value of and adjusting the parameters used by the NESTA library. However, as previously discussed, the accuracy of the grid reconstruction is not the only property of importance to us when deciding which approach should be used for storing and reconstructing the compressed occupancy grid. The other significant property of each algorithm that must be investigated is the time it takes to execute the algorithm. A comparison of the average time it takes to run a single iteration of the FastSLAM COG algorithm using approach and the compressed sensing approach for varying amounts of data is shown in Figure 18. As seen in these results the average run time of the FastSLAM COG algorithm using the compressed sensing approach is significantly longer at each compression level than the algorithm using reconstruction approach.
6.1. How Much Compression Can Be Achieved?
There are two key components that affect how much an occupancy grid can be compressed; first is the complexity of the environment in which the UMV is operating. If the environment is simple, such as a rectangular room, then a relatively small amount of data is needed to represent it with respect to a more complex environment. The second component that affects how much an occupancy grid can be compressed is the cell size used. If a large cell size is selected then there are fewer cells to represent the environment compared to when a smaller cell size is selected. It follows that when a larger cell size is selected a larger percentage of the cells must be used to store the information about the environment, even for simple environments so the grid cannot be compressed as much. To illustrate these two components, three separate environments of increasing complexity where generated and a compressed occupancy grid was generated for each environment using four separate cell sizes (1.0 m, 0.5 m, 0.2 m, and 0.1 m) and varying amounts of compression. The first environment is a simple rectangular room and an occupancy grid for this environment generated using the full set of data with the smallest cell size is shown in Figure 19. The MSE as function of data stored for this environment is shown in Figure 20 for each of the four cell sizes. As expected the environment with the largest cell size, when fewer coefficients are needed to store all of the environment information, has the largest error when a small amount of data is used to represent the environment. It can also can be seen that, for each of the four cell sizes, as more information is stored, the error in the reconstructed form of the occupancy grid decreases.
A second environment with more complexity is shown in Figure 21 which is a rectangular room with complex corners. The MSE between the true occupancy grid and the reconstructed grid as a function of the amount of data stored is shown in Figure 22 for each of the cell sizes. The overall behavior of the error is the same as in the simple case with the largest cell size having the largest error when small amounts of data are used. However, because of the added complexity in this environment, the errors at the lower data percentages are larger than the simple case which is what we expected because more information is required to store the more complex environment.
Finally, a complex environment of a rectangular room with complex corners along with rectangular objects located throughout the rectangular room is shown in Figure 23. As with the previous two environments the MSE between the true occupancy grid and the reconstructed occupancy as a function of data is shown in Figure 24 for each cell size. As expected the use of larger cell sizes causes more errors in the reconstructed form of the grid when small amounts of data are used. Also, the additional complexity causes the errors at lower data percent to be larger than in the more simple environments. It can be seen from the presented results that there appears to be a “magic” compression level of at which, no matter the complexity, the compressed occupancy can be reconstructed with significant accuracy.
7. Conclusions
In this paper an approach for solving the SLAM problem was presented that makes use of compressed occupancy grids. The approach is based on an extension to the FastSLAM algorithm that represents an unknown environment by using occupancy grids. A modified form of the SLAM algorithm, FastSLAM COG, was presented in Section 3 which makes use of a generic compression and reconstruction method to solve the SLAM problem. Four specific reconstruction methods were examined in Section 4 in order to see how well each of the approaches could reconstruct a compressed signal along with an analysis of the time needed for each algorithm to complete. Based on the reconstruction time, two of the four algorithms were used to replace the generic reconstruction method in the FastSLAM COG algorithm and their performance was examined in Section 6 using experimental data. From the presented experimental results it was concluded that the FastSLAM COG algorithm using reconstruction approach could solve the SLAM problem with almost no errors compared to the FastSLAM OG algorithm while storing only of the data required to store the complete occupancy grid. The experimental results presented in this paper were achieved by exploring how compressing the occupancy grid affects the performance of the algorithm in simple environments. In order to further examine our approach, a next step would be to examine how well this approach performs in more complex and unstructured environments. Also, further investigation into the effect of using more complex compression and reconstruction approaches as well as the possibility of dynamically resizing the cell size could lead to higher data compression rates without inducing errors into the SLAM solution.
Competing Interests
The authors declare that there are no competing interests regarding the publication of this paper.
Acknowledgments
This material is based upon work supported by (while serving at) the National Science Foundation of the United States.