Home | Members | Research | Facilities | Publications | Software | News | Contact


 

   

Robust Simultaneous Planning, Localization and Mapping

The problem of simultaneous localization and mapping (SLAM) or additionally simultaneous planning, localization and mapping (SPLAM) has received considerable attention in the Robotics community in the past several years . The generic SLAM problem consists of an autonomous system navigating in an unknown environment, which it is trying to map while simultaneously localizing itself with respect to the map that it is building. This creates a chicken and egg problem which leads to a very high dimensional estimation problem. In most SLAM techniques, the localization and mapping problem is posed as a Bayesian filtering problem wherein the environment is considered to be a fixed but unknown parameter. There are two basic approaches to solving the Bayes filtering problem. The first alternative is to use the Kalman filtering technique which is applicable to linear-Gaussian systems. However, this method cannot accommodate cases where the distributions are non-Gaussian and cannot provide a solution to the so called “data association” problem . The second method consists of solving the Bayesian filtering problem using particle filtering techniques which can handle the data-association problem in a graceful fashion. The basic drawback with the Bayesian formulation of the SLAM problem is that the estimates of the various environmental components (features) become correlated even though their measurements are mutually independent. This is a basic structural property of the Bayesian formulation of the SLAM problem. Thus, the filter has to estimate a very high dimensional probability distribution on the environment which becomes increasingly computationally intractable as the size of the environment increases.

The addition of planning to the SLAM problem, resulting in the SPLAM problem, adds further to the complexity of the problem . In fact, the planning problem on its own is computationally quite intractable under uncertainty . In this proposal, an alternative to the Bayesian formulation of the SPLAM problem is proposed. In this formulation, the environment is modeled as a stationary but unknown random process that has to be estimated as the autonomous system moves through it and makes observations of the environment. It is assumed that the robot localizes in the environment based on a few landmarks located throughout the environment (which is the Bayesian SLAM problem but with very few parameters compared to case when the whole environment is considered to be an unknown parameter), and then maps the rest of the environment based on this estimate. This problem formulation ensures that the estimates of the landmarks and the environmental components never get correlated. In addition, each individual environmental component can be estimated completely independently of the other components. This drastically reduces the computational burden of the SPLAM problem as the correlation of the various features in the environment need not be considered. The environmental components are estimated using stochastic approximation algorithms. Further, it can be shown that there is a separation principle between the mapping and the planning problems, in that the mapping algorithms can be designed completely independently of the planning algorithms and still be guaranteed to converge.

A hierarchical approach to the planning problem is proposed here. In this approach, the local environment variables and their associated uncertainties are abstracted to global environment variables through the definition of suitable feature vectors, which allows for the definition of “meaningful” approximate Markov Decision Processes (MDP) at the global level. This allows for the maintenance of maps with suitable description of hierarchical environmental variables and their attendant uncertainties at the various levels, which in turn aids in the design of the planning algorithms at the various levels of the hierarchy. In particular, the approach described separates the concern between uncertainties at different time and length scales and provides a hybrid framework to bridge the gap between the discrete and continuous planning methods of motion planning and trajectory generation. The real-time aspect of the problem is addressed by implementing the motion planning algorithms using anytime computational models that incrementally improves the algorithm performance with available computational time

Hierarchical, Multi-Scale System Architecture for Robust Motion Planning.

In the Robust Anytime Motion Planning (RAMP) architecture, the motion-planning problem is decomposed into three layers as illustrated in Figure above:

  1. Global Planning Layer: At the topmost level, the dynamics of the environment are represented at a low spatial and temporal resolution (using coarse discrete variables). The vehicle dynamics are completely ignored at this level. The output of the global planner is high level commands in terms of discrete waypoints. Coarse grained information from the local layer is used to periodically update the model of the environment. Figure 3 illustrates low resolution spatial discretization of the global environment. Regions of high hazard are estimated using coarse variables. The optimal path from START to END is determined by solving a stochastic dynamic programming problem. The output of the global planner is a set of waypoints (in feedback form) that avoids the regions of high hazard and defines the optimal path from START to END.

Global Planning with Coarse Variables
Figure 3:  Global Planning with Coarse Variables

  1. Local Planning Layer: At the intermediate level, the nominal dynamics of the vehicle and local changes in the environment are considered. This middle layer uses the discrete waypoints from the top layer to form continuous reference trajectories that are robust to local environmental uncertainty (i.e. the vehicle will avoid obstacles) and satisfy the dynamical constraints of the vehicle. Sensor information describing the surrounding of the vehicle is sent to the global layer, which uses it to modify the global environmental model. Figure 4 illustrates the functionality of the local planning layer. In this example, the global planner issues waypoints situated on the extremes of the diagonal. The local planner generates a trajectory connecting the two waypoints, avoiding local obstacles and threats, and satisfying the system dynamics.

Local Planning with Fine Variables
Figure 4:  Local Planning with Fine Variables
The method described has been applied to the problem of a mobile agent navigating an urban terrain, the results are shown in Fig. 5. These methods for single agents can then be generalized through the use of decentralized planning methodologies such as dec-MDP and dec-POMDP [37,38].
Hierarchical Planning using DP and RRT with global 5x5 grids
Figure 5: Hierarchical Planning using DP and RRT with global 5x5 grids.

The multi-layer approach to motion planning has also been implemented [40] using tools from optimal control theory in the receding horizon framework. Figure 6(a) shows the decomposition of an obstacle rich environment into convex feasible sets and fig.6(b) shows the trajectories obtained by repeatedly solving  optimal control problems. The advantage of the RHC based method is that the trajectories obtained satisfy the dynamics of the vehicle. The trajectories shown in fig.6(b) are for a 6DOF UAV like model.


6DOF UAV like model
Figure 6(a): Optimal Path in Coarse Variables

6DOF UAV like model
Figure 6(b): RHC based motion planning using overlapping sequence of convex feasible sets

PIs: Raktim Bhattacharya, Suman Chakravorty, Tamas Kalmar-Nagy.

 

 

Related Publications:

Near Time Optimal Waypoint Tracking of a 3-DOF Model Helicopter - B. Singh, R. Bhattacharya, submitted, AIAA GNC 2007.
Multi-Layer Approach for Motion Planning in Obstacle Rich Environments – S. H. Kim, R. Bhattacharya, submitted, AIAA GNC 2007.
Nonlinear Trajectory Generation Using Global-Local Approximations - R. Bhattacharya, P. Singla, IEEE Conference on Decision and Control, San Diego, 2006.
OPTRAGEN: A MATLAB Toolbox for Optimal Trajectory Generation - R. Bhattacharya, IEEE Conference on Decision and Control, San Diego, 2006.
Use of Parametric Approximation in Real–Time Nonlinear Trajectory Generation - Tamer Inanc, Raktim Bhattacharya, and Mehmet K. Muezzinoglu, IEEE Conference on Decision and Control, San Diego, 2006.
Nonlinear Receding Horizon Control of an F-16 Aircraft - R. Bhattacharya, G. J. Balas, M. Alpay Kaya, A. Packard, Journal of Guidance, Control, and Dynamics, Vol. 25, No. 5, pp. 924-931, 2002
• S. Chakravorty and J. L. Junkins, “ Motion Planning in Unknown Environments using vision-like sensors,” in review for Automatica
• J Davis and S. Chakravorty, “Motion Planning under Uncertainty: Application to an unmanned Helicopter,” in review for Journal of Guidance, Control and Dynamics
• S. Chakravorty and J. L. Junkins, “A Methodology for Intelligent Path Planning in Unknown Environments”, Proceedings of the IEEE International Symposium on Intelligent Control, Cyprus, 2005
• S. Chakravorty and J. L. Junkins, “Intelligent Exploration of Unknown Environments with Vision like Sensors”, Proceedings of the 2005 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, July 2005, Monterey, CA
• J Davis and S. Chakravorty, “Intelligent Path Planning for Autonomous Agents: Application to an unmanned Helicopter,” Proceedings of the 2006 IEEE Conference on Decision and Control
• S. Chakravorty, “Mapping and Planning under Uncertainty”, submitted to IEEE Transactions on Automatic Control
• S. Chakravorty, “Mapping and Planning under Uncertainty I”, submitted to the 2007 Robotics: Systems and Science Conference, Atlanta, GA
• S. Chakravorty, “Mapping and Planning under Uncertainty II”, submitted to the 2007 Robotics: Systems and Science Conference, Atlanta, GA
• S. Chakravorty and R. Saha, “Hierarchical Motion Planning under Uncertainty”, submitted to the IEEE Transactions on Robotics
• S. Chakravorty and R. Saha, “Hierarchical Motion Planning under Uncertainty”, submitted to the 2007 IEEE Conference on Decision and Control
• Doebbler, James, Gesting, Paul, and Valasek, John, “Real-Time Path Planning and Terrain Obstacle Avoidance for Aircraft,”AIAA-2005-5825, Proceedings of the AIAA Guidance, Navigation, and Control Conference, San Francisco, CA, 15-18 August 2005.

 

 

 



 


© Aerospace Engineering, Texas A&M University