Saurav Agarwal

Robust online belief space planning in changing environments: Application to physical mobile robots

Motion planning in belief space (under motion and sensing uncertainty) is a challenging problem due to the computational intractability of its exact solution. The Feedback-based Information RoadMap (FIRM) framework made an important theoretical step toward enabling roadmap-based planning in belief space and provided a computationally tractable version of belief space planning. However, there are still challenges in applying belief space planners to physical systems, such as the discrepancy between computational models and real physical models. In this paper, we propose a dynamic replanning scheme in belief space to address such challenges. Moreover, we present techniques to cope with changes in the environment (e.g., changes in the obstacle map), as well as unforeseen large deviations in the robots location (e.g., the kidnapped robot problem). We then utilize these techniques to implement the first online replanning scheme in belief space on a physical mobile robot that is robust to changes in the environment and large disturbances. This method demonstrates that belief space planning is a practical tool for robot motion planning.

Periodic-Node Graph-Based Framework for Stochastic Control of Small Aerial Vehicles

This paper presents a strategy for stochastic control of small aerial vehicles under uncertainty using graph-based methods. In planning with graph-based methods, such as the probabilistic roadmap method (PRM) in state space or the information roadmaps (IRM) in information-state (belief) space, the local planners (along the edges) are responsible to drive the state/belief to the final node of the edge. However, for aerial vehicles with minimum velocity constraints, driving the system belief to a sampled belief is a challenge. In this paper, we propose a novel method based on periodic controllers, in which instead of stabilizing the belief to a predefined probability distribution, the belief is stabilized to an orbit (periodic path) of probability distributions. Choosing nodes along these orbits, the node reachability in belief space is achieved and we can form a graph in belief space that can handle higher order dynamics or nonstoppable systems (whose velocity cannot be zero), such as fixed-wing aircraft. The proposed method takes obstacles into account and provides a query-independent graph, since its edge costs are independent of each other. Thus, it satisfies the principle of optimality. Therefore, dynamic programming (DP) can be utilized to compute the best feedback on the graph. We demonstrate the methods performance on a unicycle robot and a six degrees of freedom (DoF) small aerial vehicle.

RFM-SLAM: Exploiting relative feature measurements to separate orientation and position estimation in SLAM

The SLAM problem is known to have a special property that when robot orientation is known, estimating the history of robot poses and feature locations can be posed as a standard linear least squares problem. In this work, we develop a SLAM framework that uses relative feature-to-feature measurements to exploit this structural property of SLAM. Relative feature measurements are used to pose a linear estimation problem for pose-to-pose orientation constraints. This is followed by solving an iterative non-linear on-manifold optimization problem to compute the maximum likelihood estimate for robot orientation given relative rotation constraints. Once the robot orientation is computed, we solve a linear problem for robot position and map estimation. Our approach reduces the computational complexity of non-linear optimization by posing a smaller optimization problem as compared to standard graph-based methods for feature-based SLAM. Further, empirical results show our method avoids catastrophic failures that arise in existing methods due to using odometery as an initial guess for non-linear optimization, while its accuracy degrades gracefully as sensor noise is increased. We demonstrate our method through extensive simulations and comparisons with an existing state-of-the-art solver.