Preliminary results in range-only localization and mapping

This paper presents methods of localization using cooperating landmarks (beacons) that provide the ability to measure range only. Recent advances in radio frequency technology make it possible to measure range between inexpensive beacons and a transponder Such a method has tremendous benefit since line of sight is not required between the beacons and the transponder and because the data association problem can be completely avoided. If the positions of the beacons are known, measurements from multiple beacons can be combined using probability grids to provide an accurate estimate of robot location. This estimate can be improved by using Monte Carlo techniques and Kalman filters to incorporate odometry data. Similar methods can be used to solve the simultaneous localization and mapping problem (SLAM) when beacon locations are uncertain. Experimental results are presented for robot localization. Tracking and SLAM algorithms are demonstrated in simulation.


Introduction
In this paper, we present a system of active beacons as a solution to the problem of mobile robot localization. The beacons, which return a range estimate and unique beacon identification number when queried from a mobile robot, form a de facto local positioning system when distributed in an environment. The beacons are self-contained, small, and inexpensive. They do not require line-of-sight to the robot, and when used with the methods presented here they do not need to be accurately placed. The result is a lowcost, easily installed system that can be used to localize a mobile robot in both indoor and outdoor environments.
The ability of a robot localize itself is a fundamental problem for mobile robots. Not surprisingly, many technologies and techniques for robot localization can be found in the literature (eg. [l, 8, 9, 141). While there are many different variations of the localization problem, we concentrate on three: static localization, position tracking, and simultaneous localization and mapping (SLAM). Static localization requires a robot to obtain an accurate estimate of its global position based only sensor readings. For po- sition tracking, the robot starts with an initial position estimate that is assumed to be "close" to the actual location.
The robot must then keep track of its position as it moves about, using sensory information continually improve its location estimate and correct for odometry errors [l]. For both position tracking and global localization, it is generally assumed that the robot has a map of its environment, i.e. that the locations of the landmarks used for localization are known. The SLAM problem does not rely on this assumption: the robot must use sensor information to simultaneously localize itself and build a map of its environment. The SLAM problem was first studied by Smith, Self, and Cheeseman [ 101 in 1990 and has been the subject of of significant recent activity [2, 3,7, 131. Typically, the work in SLAM supposes that a robot is able to measure both bearing and range to landmarks in the environment [3,7,131, but there is some work in trying to use sensors that measure only bearing [2, 121. This work is related to a body of literature in the computer vision community known as structure from motion, where egomotion and the locations of sparse landmarks are simultaneously extracted from a sequence of images [4]. In contrast to these methods, we examine methods where only range to the landmarks is measured. The most prevalent case of localization using range only is the use of GPS, which has been successfully used for mobile robot localization in outdoor experiments [15]. GPS essentially measures the time taken for signals broadcast from satellites to reach a receiver with the presumption that the locations of the satellites are known with high accuracy. The fact that GPS works only outdoors is a significant drawback. Pseudolites that act as stand-ins for GPS satellites have been used to allow GPS receivers to operate indoors [6], however this solution is undesirable due to the cost and size of the required infrastructure.
For most range-only sensors, the problem of data registration poses a serious obstacle for localization and mapping; the sensors give range to some object without identifying the object. Because beacons in the system we use transmit a unique ID number as well as range, data registration is solved trivially.
In this paper we employ probabilistic methods to generate position estimates from sensor data. We use three methods that have been applied in the past with success: Kalman filtering, Markov methods, and Monte Carlo localization. All three of these methods estimate robot position as a distribution of probabilities over the space of possible robot positions. Originally introduced in 1960, the Kalman filter assumes a multivariate Gaussian distribution [5]. The Kalman filter has the advantage that the representation of the distribution is compact; a Gaussian distribution can be represented by a mean and a covariance matrix. Recent extensions to Kalman filtering allow for non-Gaussian, multimodal probability distributions through multiple hypothesis tracking [SI. The result is a more versatile estimation technique that still preserves many of the computational advantages of the Kalman filter. Markov methods provide another means of estimation [9]. Here, the space of possible robot positions in discretized (often into a "probability grid") and the probability distribution is approximated by assigning a real number to each point in the discretization. Successive grids or grids arising from independent measurements are combined to create a new grid using Bayes' rule. Markov methods have the advantage of flexibility, but the size of the discretization can become prohibitive for large areas, small grid resolutions, or problems such as SLAM where the system state has high dimension. Monte Carlo localization provides yet another method of representing multimodal distributions for position estimation [14]. Also known as particle filtering, Monte Carlo localization approximates a distribution using a finite number of weighted samples. The estimated distribution is updated using importance sampling: new samples are drawn from the old distribution at random, propagated in accordance with robot odometry, and then weighted according to available sensor information. One advantage of Monte Carlo localization is that the computational requirements can be scaled as needed by adjusting the number of samples used to represent the distribution.
The paper is arranged as follows: In Section 2, we investigate the problem of robot localization in an environment with known beacon locations using Markovian probability grids. Experimental results are presented. Section 3 extends these ideas to position tracking using Kalman filtering and Monte Carlo localization. Section 4 addresses the problem of localization in an environment with uncertain beacon locations. Here we present a SLAM algorithm that combines intuition with Kalman filtering. Simulation results are given for position tracking and SLAM algorithms.

Static Localization
In this section, we address the problem of robot localization based solely on current beacon readings. We term this static localization because this method does not use past sensor readings or past estimates of position to determine an estimate of the current position. We assume that the positions of the beacons are known and fixed.
For perfect measurements, determining position from range information is a matter of simple geometry. A robot at distance T from a beacon must be located on a circle of radius T centered at the beacon. Determining location from multiple range measurements is just a matter of finding where the corresponding circles intersect. Unfortunately, perfect measurements are difficult to achieve in the real world. The commercially available beacons we use here provide range measurements with an expected error of about 6 feet. We use probabilistic methods to estimate robot position in the face of these uncertainties.

Characterizing Range Measurements
In order to apply probabilistic methods to the localization problem, we first obtain a set of probability distribution functions (pdfs) to characterize the range data provided by the system. Because of measurement discretization and noise, the actual range T associated with a measurement mi is a random variable. We denote the pdf describing the distribution of r given mi as p(rlmi).
A pdf was experimentally determined for each measurement. The resulting pdfs are plotted in Figure la.

Creating Probability Grids
The pdfs generated in the previous section give a probabilistic description of the range between the antenna and beacon. In order to be used for robot localization in a plane, these one-dimensional range distributions must be converted to two-dimensional position distributions. Probability grids provide one method of accomplishing this.
To construct a probability grid, the space of interest is discretized into a grid of desired size and resolution. For our purposes, the 50' x 50' test area was divided into a Cartesian grid of 1' x 1' squares. Then each square is assigned a real number equal to the probability that the robot resides in that square given a measurement mi from a beacon at known location 26. We approximate this probability via the following steps: 1. For each square on the grid, compute the value where T , = 11x6z, 1) and 2, is the location of the center of the square.  2. Assign to each square the probability 73 P, = -.

CY
Here, step 1 assigns a relative probability to each square while step 2 rescales the relative probabilities to make sure that the overall probability is one.

Combining Probability Grids
Probability grids arising from measurements to multiple beacons can be combined to produce an estimate of robot location. To combine two probability grids, we simply multiply them in a pointwise manner and scale the result so that the sum over the squares is one. Figure l b depicts this merging process. Note that the number of squares where the robot is likely to be is reduced, providing a better estimate of robot position. This process can be repeated for multiple beacons to yield better results. Figure IC depicts a probability grid that results from combining measurements from eight beacons.
To get the position estimate 2 from a probability grid, we take a weighted average of grid locations: N.
(1) s=1 The covariance matrix C associated with this estimate is computed as where X is the 2 x Ns matrix whose sth column is

Experimental Results
We used the technique described above to estimate robot location at approximately 100 points distributed over the 50 x 50 test area with 8 active beacons. The average estimate error over this sample was 1.62 feet. This result is significant considering that the expected error in the range measurements ranges from 5.82 to 7.18 feet.

Position Tracking
Here we present two methods that take advantage of past position estimates and odometry data to continuously track a robot's position.

Extended Kalman Filtering
When the robot can read data from 3 or more beacons, the resulting combined probability grid usually has only one peak. In these cases, the distribution resulting from the static localization algorithm is reasonably well approximated as Gaussian and we can use a type of extended Kalman filtering algorithm to solve the position tracking problem.
For this discussion, we assume an omnidirectional robot with x and y velocities as inputs'. We also assume some uncertainty in this model, which conceptually models phenomena like wheel slippage and other unmodeled disturbances. Given robot position z ( k ) and inputs (q (IC), u2 (IC)) at time t, the robot location at time t + T will be where w ( k ) is an identically independently distributed (id) Gaussian random vector with zero mean and constant covariance matrix R. 'This assumption is made for clarity. More complicated robot models such as that of a differentially steered robot can be accommodated for with minor adjustments to the approach given here Given a range measurement m from a beacon located at x b , we seek to approximate the annular distribution that results with a Gaussian distribution. Generally, this is not possible, but if we have a prior estimate, 2 of robot position we can "linearize" the annular distribution around the estimate. The variance in the direction radial to the annulus, U,., is chosen to be the same as the variance of the range measurement. The variance in the direction tangent to the annulus, ut, is chosen to be very large, reflecting the fact that the range measurement provides little information in that direction. In practice, we choose this variance to be 10 times the variance of the range measurement. The resulting covariance matrix C to has principal variances TI, . and vt in the axial and radial directions respectively. The mean of this approximate distribution, 2, is chosen to lie on same radial line with the 2 with the distance between the beacon and 2 equal to expected value associated with the measurement m. This approximation process is depicted graphically in Figure 2. The ellipse plotted in this figure is the variance ellipse associated with C.
If we let 0 be the angle between the z axis and the line through i that points radially away from 2 6 , then we can express the mean 2 and covariance matrix C of the approximate distribution as

(4)
where Fm and U,. are the mean and variance respectively of the pdf associated with the measurement m, and sin0 cos0 1 ' cos0 -sin0 @ = [ In the notation of Kalman filtering, the estimate at the kth time step is denoted i ( k l k ) and its associated covari-ancematrixisP(klk). Given2(klk),P(klk), aninput vector u(k), and a collection of measurements mi from a set of N b beacons located at X b i , i = 1,2,3,. . . , N b . the next estimate 2( k + 1 1 IC + l), and covariance P ( k + 1 I k + 1)) are computed as follows: 1. Compute predicted next estimate according to robot model:

Fori
To start the algorithm, initial position and covariance estimates (2( 1 1 1) and P( 1 1 l), respectively) are found using the static localization method presented in Section 2. In cases where the initialization does not produce a unimodal distribution (eg. when only two beacons are visible), multiple hypothesis testing [8] can be used to solve the tracking problem.

Monte Carlo Methods
In Monte Carlo localization, a probability distribution is represented by a finite collection of samples. The samples, often referred to as pum'cles, represent possible robot locations. The basic idea of Monte Carlo localization is to propagate the particles so that they all converge to likely robot locations. Intuitively, the process is not all that different from predictionhpdate process of Kalman filtering.
ZFor more complicated robot models, this step is more difficult.

See [l I] for compounding when robot orientation is taken into account
There is a "prediction" step where each particle is propagated according to some robot model, the current inputs, and a random noise selected from an appropriate distribution. There is then an "update" step where the collection of predictions is merged with measurement data. This merging is accomplished through a technique called importance sampling, where particles are weighted according to the pdf associated with the measurement and then resampled. As a result, particles with large weights are likely to be chosen multiple times while particles with small weights are likely not to be chosen at all. In this manner, particles that are in unlikely robot locations are replaced by particles in more likely locations. 2. For each p assign a weight to the pth particle according to the pdf associated with m(k) and zg(k): where rp is the distance between zb(k) and ZP(k). 3. Rescale the weights so that Crz, = 1.

4.
For each p, randomly choose z p ( k + 1) from the predicted collection. The probability that the particle &(k) is selected during any choice is ~( i ) .

Results
Both the Kalman and Monte Carlo methods were tested in simulation using the robot model given in Equation 3 and the set of pdfs determined from experimental data in Section 2. Both systems were simulated with identical beacon locations, beacon returns, and robot trajectories. The covariance matrix for the noise vector w was chosen to be The results are plotted in Figures 3a and 3b

Simultaneous Localization and Mapping
The algorithms presented in Sections 2 and 3 require that the positions of the beacons are known exactly. Algorithms that can cope with uncertain beacon positions will make the proposed positioning system easier to install because the beacons will not need to be placed carefully. The problem of simultaneously determining robot position and idenwing the locations of the beacons used to navigate is known as simultaneous localization and mapping (SLAM).
The scenario where initial robot and beacon locations are approximately known is a reasonable one. Good (but not perfect) beacon locations can be obtained through crude measurement or even through estimating location on a building blueprint. Here we adapt the extended Kalman filtering algorithm presented in Section 3.1 to apply it to the SLAM problem. At each step, we use the current estimates of robot and beacon positions together with Equations 4 and 5 to translate each range measurement mi, i = 1,2, . . . , Nb into an estimate of the relative displacement between the robot, s, and the ith beacon, xh. The estimated relative displacement Zi can be thought of as a measurement with zero-mean Gaussian noise with covariance C i . The remaining SLAM problem can then be solved according to [lo]. Specifically, we define the system state to include both robot and beacon locations, write down dynamic equations for the state (which is easy since the beacons do not move), write the outputs as a function of the state (which are just the differences between robot and beacon locations), and use a Kalman filter to provide an estimate of the state. Figure 3c depicts the performance of this technique. In this simulation, we assume variance of the range measurement noise is = 1. The variances of the initial robot and beacon estimates were set to 25, meaning that the initial guesses have an expected error of 5 feet. In this simulation, the average error of the initial estimate for robot and beacons was 5.13 feet. The average error of the estimates at the end of the simulation improved to 0.77 feet.

Conclusion
We have presented some robot localization algorithms that employ range only data obtained from active beacons in the environment. Range measurements with expected error on the order of 6 feet were used to generate position estimates with expected error on the order of a foot or less. The beacon technology used in this paper is con-  We also presented an algorithm based on Kalman filtering to solve the SLAM problem when the beacon positions are approximately known. Here we use the linearization step described in Figure 2 to put the range-only SLAM problem into a form that can be solved with standard Kalman-based techniques. Conceptually, Markov and Monte Carlo methods should provide a solution for completely unknown beacons, however the high dimension of the SLAM state space renders these methods computationally intractable. Range-only SLAM with completely unknown beacon locations is left as a topic for future research.