Saurav Agarwal

Saurav Agarwal, Amirhossein Tamjidi, Suman Chakravorty

This paper presents a method for motion planning under uncertainty to deal with situations where ambiguous data associations result in a multimodal hypothesis on the robot state. In the global localization problem, sometimes referred to as the "lost or kidnapped robot problem", given little to no a priori pose information, the localization algorithm should recover the correct pose of a mobile robot with respect to a global reference frame. We present a Receding Horizon approach, to plan actions that sequentially disambiguate a multimodal belief to achieve tight localization on the correct pose in finite time, i.e., converge to a unimodal belief. Experimental results are presented using a physical ground robot operating in an artificial maze-like environment. We demonstrate two runs wherein the robot is given no a priori information about its initial pose and the planner is tasked to localize the robot.

Ali-akbar Agha-mohammadi, Saurav Agarwal, Sung-Kyun Kim et al.

Simultaneous localization and Planning (SLAP) is a crucial ability for an autonomous robot operating under uncertainty. In its most general form, SLAP induces a continuous POMDP (partially-observable Markov decision process), which needs to be repeatedly solved online. This paper addresses this problem and proposes a dynamic replanning scheme in belief space. The underlying POMDP, which is continuous in state, action, and observation space, is approximated offline via sampling-based methods, but operates in a replanning loop online to admit local improvements to the coarse offline policy. This construct enables the proposed method to combat changing environments and large localization errors, even when the change alters the homotopy class of the optimal trajectory. It further outperforms the state-of-the-art FIRM (Feedback-based Information RoadMap) method by eliminating unnecessary stabilization steps. Applying belief space planning to physical systems brings with it a plethora of challenges. A key focus of this paper is to implement the proposed planner on a physical robot and show the SLAP solution performance under uncertainty, in changing environments and in the presence of large disturbances, such as a kidnapped robot situation.

Saurav Agarwal, Amirhossein Tamjidi, Suman Chakravorty

Planning under uncertainty is a key requirement for physical systems due to the noisy nature of actuators and sensors. Using a belief space approach, planning solutions tend to generate actions that result in information seeking behavior which reduce state uncertainty. While recent work has dealt with planning for Gaussian beliefs, for many cases, a multi-modal belief is a more accurate representation of the underlying belief. This is particularly true in environments with information symmetry that cause uncertain data associations which naturally lead to a multi-modal hypothesis on the state. Thus, a planner cannot simply base actions on the most-likely state. We propose an algorithm that uses a Receding Horizon Planning approach to plan actions that sequentially disambiguate the multi-modal belief to a uni-modal Gaussian and achieve tight localization on the true state, called a Multi-Modal Motion Planner (M3P). By combining a Gaussian sampling-based belief space planner with M3P, and introducing a switching behavior in the planner and belief representation, we present a holistic end-to-end solution for the belief space planning problem. Simulation results for a 2D ground robot navigation problem are presented that demonstrate our method's performance.