Abstracts - 2007
Robust Range-Based Localization & Motion Planning Under Uncertainty in GPS-Denied Environments Using Ultra-Wideband Radio
Samuel Prentice & Nicholas Roy
To develop a novel method of range-based localization using ultra-wideband (UWB) radio signals to localize a mobile robot in GPS-denied environments; and to design and implement a method for planning under uncertainty, adapting the Probabilistic Roadmap algorithm to robustly handle maps with uncertain features and favor actions that minimize belief uncertainty.
The project work is two-fold: (1) we have developed a novel method of range-based localization which probabilistically models the biases of ultra-wideband radio range measurements for 2D localization; (2) we have explored a method for planning under uncertainty, extending the Probabilistic Roadmap to bias towards paths with higher certainty in the Kalman filter sense.
UWB Range-Based Localization With Hidden Bias Estimation
The Global Positioning System (GPS) provides position estimates of a user's location on the Earth to within a few meters, enabling geolocation in areas with a clear line-of-sight (LOS) to GPS satellites. However, in indoor and dense urban environments, GPS becomes unreliable or altogether unavailable. The need for accurate geolocation, however, exists in indoor and dense environments for both civil and military applications.
One approach to this problem involves placing anchor radio beacons around the target environment, enabling localization by computing range measurements from a dynamic agent to known beacons (which can geolocate via GPS). The agent emits a request signal to which the anchor beacons respond upon receipt. Ranges to each anchor beacon can be computed with a time-of-flight (TOF) calculation, measuring the round-trip time between signal emission and response signal detection. The agent can then infer its position based on range measurements from multiple sensors. Two problems limit the effectiveness of this approach when using traditional narrowband radios, namely multipath fading and LOS blockage. Multipath fading occurs when multiple signal path reflections co-interfere and are not successfully resolved by the radio receiver. Additionally, non-LOS scenarios may completely block signal reception.
The advent of ultra-wide band (UWB) technology presents a promising alternative to overcome these limitations. The spread-spectrum concept employed by UWB utilizes extremely wide transmission bandwidths, which yields desirable capabilities for solving the range-based localization problem. The fine delay resolution of UWB pulses makes them largely immune to multipath fading. Further, the low frequency components of the UWB signal enable easier penetration of building materials .
While these properties make UWB radio well suited for ranging and accurate position location, range measurements are often positively biased in non-LOS scenarios. Overestimated ranges are due to channel delays resulting from LOS occlusion or the receipt of a reflected multipath component. LOS blockage can impose a substantial propagation delay as the signal passes through building materials. Also, when a LOS path does not exist, the received signal may be a reflected multipath component, traveling a greater accumulated distance between the pair of radios. These phenomena add a positive bias to the range estimate, and thus induce uncertainty in localization.
We use a Monte Carlo method for range-based localization that is robust to hidden biases in range measurements. The goal is to accurately localize the robot, given a noisy robot motion model and positively biased range measurements from multiple anchor beacons. The typical model used to approach this problem is a dynamic Bayes network (DBN), which characterizes the evolution of controls, states, and measurements. Conventional parametric techniques for maintaining this DBN, such as Kalman filter approaches, have proven ineffective in capturing the nonlinearity of bias transitions .
Particle filters (PF)  offer a nonparametric implementation of the Bayes filter which maintains the belief distribution as a finite set of sample particles. PFs are powerful because they can approximate a broad space of distributions and functions that cannot be accurately represented by parametric models. Particles are state samples randomly drawn according to the belief posterior, each representing a belief hypothesis that accumulates weight based on measurement updates. PFs are useful for maintaining the robot pose belief in localization, accounting for nonlinear motion controls.
For the task of range-based localization, we seek to estimate the distribution of the robot pose, augmented with bias estimates to each anchor beacon. We can use PFs to maintain this belief distribution, however a major drawback of the PF is that sampling in high-dimensional spaces can be inefficient. Thus, as the number of anchor beacons grows, the PF must estimate additional biases and begins to suffer from scalability issues.
A technique known as Rao-Blackwellization has been shown to increase the efficiency and accuracy of PFs for DBN structures in which some of the state variables can be marginalized out exactly . The Rao-Blackwellized particle filter (RBPF) divides the hidden state variables into two groups, sampling particles for one group as in the standard PF, while analytically integrating out the other. This improves overall efficiency by reducing the dimensionality of the sampling space, and increases accuracy by exactly computing the set of tractable variables.
We utilize the RBPF for maintaining the bias-augmented belief state in our range-based localization problem. We sample particles for the robot pose, and integrate out the bias variables according to the bias transition probability distribution. During the course of the project work, we have experimented with different bias transition distributions, such as discrete multinomial or exponential based functions, that accurately model collected UWB range data. We follow previous work  as a starting point, which develops a probabilistic model of beacon biases resembling a notch, capturing the fact that bias estimates tend to remain constant in local spaces with common UWB channel properties, but jump significantly when crossing the threshold to an adjacent spatial locality. Preliminary results from simulation are shown in Figure 2. The robot is able to localize by probabilistically tracking range measurmement biases with an RBPF.
Figure 2 - The robot receives ranges from three UWB sensors (shown as colored circles) and estimates its pose and sensor biases. The true pose of the robot is shown in blue, and the estimated pose is shown as a black rectangle. Actual ranges are displayed as arcs with colors corresponding to the respective uwb sensor, while estimated ranges are shown for a subset of particles with highest likelihood bias multinomial bins in deeper color saturation. The robot is able to localize by probabilistically tracking range measurmement biases.
Motion Planning Under Uncertainty
The second portion of this project involves probabilistic algorithms for motion planning. The problem involves choosing actions that achieve a goal with minimal uncertainty. Robust mobile robot navigation requires reacting to unanticipated situations; the stochastic nature of robot motion results in outcomes of actions that are non-deterministic.
One solution to this problem involves the Markov Decision Process (MDP) , which generates a global control policy that prescribes the best action to take in any given situation to advance towards a desired goal state. By iteratively computing the expected rewards of transitioning between states for a given set of actions, the MDP converges to a solution where each possible action transition is assigned an expected cumulative payoff. As the robot navigates towards a goal, the optimal path is easily determined by taking the pre-defined action prescribed for the current belief state of the robot.
While this approach allows for non-deterministic actions, the MDP assumes the robot can observe the full state of the environment. For most real-world robot problems, sensor limitations play a key role and limit the certainty of the robot's belief state. The partially observable Markov decision process (POMDP)  drops the assumption that the state is fully observable. The POMDP generates a control policy that encompasses both probabilistic belief states and action transitions, but with the cost of greatly increased complexity.
In high-dimensional or continuous state spaces, it becomes intractable to compute a policy for every possible state, including those with very low likelihood. Randomized motion planners have been successful in efficiently solving complicated planning problems by limiting their scope to selected regions in the state space. The Probabilistic Roadmap (PRM) algorithm  is one such method that approximates the topology of the configuration space with a graph structure of accessible poses in free space. A preprocessing phase selects poses according to a randomized sampling strategy and builds a visibility graph between them. The PRM is capable of efficiently planning paths to a queried goal location by searching the visibility graph for an optimal path.
Success in these current approaches, however, requires a complete and accurate model of the world. This requirement is constraining and unrealistic in most real-world scenarios. Map creation is imperfect due to sensor measurement errors and is limited to a static representation of the environment. For robust performance, it is necessary to enable mobile robots to safely navigate through dynamic, uncertain environments. Incorporating uncertainty into the planning algorithm mitigates the chance of the robot becoming lost or colliding with objects.
We extend the Probabilistic Roadmap algorithm to cope with uncertain maps, making robot navigation robust in real-world scenarios. The intuition behind this approach is that it may be optimal to detour from paths with higher expected uncertainty, even if they are much shorter. We assume the true state of the world is unknown and seek paths with minimum likelihood of getting lost or colliding with objects . The task is to incorporate uncertainty into the planning process, seeking goal paths that optimize utility as a function of not only distance to the goal, but also the expected level of belief certainty (and potentially probability of collision).
Within the PRM framework, our sampled states are robot poses, forming nodes in the PRM graph structure. Possible actions are represented by the edges that stem from a given node. This optimization problem requires an estimate of the expected uncertainty of each possible action, from each possible state. The Kalman filter is widely used to maintain a stochastic Gaussian belief state, incorporating noisy updates from both measurements and control. We initially seek to quantify the expected uncertainty in Kalman filter beliefs, incorporating the covariances of both expected measurements and motion model controls . This enables the motion planner to select actions that maximize sensor information and, hence, minimize uncertainty in the belief state. Each PRM node must be extended to not only represent a 2D robot coordinate, but also a discrete uncertainty value, namely a normal distribution. Planning a path that minimizes uncertainty at a goal location then becomes an optimization problem in which the utility of each edge transition reflects its expected effect on belief covariance and distance to the goal. Figure 3 displays preliminary path-planning results from graph search minimizing predicted EKF belief at the goal location. The algorithm and simulation were developed within the CARMEN Robotics Toolkit .
Figure 3 - The path from the robot (blue rectangle) resulting in minimum covariance at the goal location (yellow circle) is shown in blue. Predicted covariances from the current robot pose to each node in the PRM graph are shown as green ellipses.
 D. Cassioli, M. Win, A. Molisch. The Ultra-Wide Bandwidth Indoor Channel: From Statistical Model to Simulations. IEEE Journal On Selected Areas In Communications, Vol. 20, No. 6, Aug. 2002.
 K.J. Astrom. Optimal Control of Markov Decision Processes with Incomplete State Estimation. Journal of Mathematical Analysis and Applications, Vol. 10, pp. 174-205, 1965.
 E. Sondik. The Optimal Control of Partially Observable Markov Decision Processes. PhD Thesis, Stanford University, 1971.
 D. Jourdan, J. Deyst, M. Win, N. Roy. Monte Carlo Localization in Dense Multipath Environments Using UWB Ranging. Proceedings of the IEEE International Conference on Ultra-Wideband, Zurich, Switzerland, Sep. 2005.
 A. Doucet, N. de Freitas, N. Gordon. Sequential Monte Carlo Methods In Practice. New York: Springer Verlag. 2001.
 A. Doucet, N. de Freitas, K. Murphy, S. Russel. Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks. Proceedings of the 16th Conference on Uncertainty in Artificial Intelligence, pp. 176 - 183, 2000.
 D. Hsu, J.C. Latombe, H. Kurniawati. On the Probabilistic Foundations of Probabilistic Roadmap Planning. The International Journal of Robotics Research, Vol. 25, No. 7, pp. 627-643, 2006.
 P. Missiuro, N. Roy. Adapting Probabilistic Roadmaps to Handle Uncertain Maps. Proceedings of the IEEE International Conference on Robotics and Automation. Orlando, May 2006.
 P. Lommel, M. McConley, N. Roy. Robust Path Planning in GPS-Denied Environments Using the Gaussian Augmented Markov Decision Process. MS Thesis, Draper Laboratory, April 2005.