Convergent Planning

Abstract

We propose a number of “divergence metrics” to quantify the robustness of a trajectory to state uncertainty for under-actuated or under-sensed systems. These metrics are inspired by contraction analysis and we demonstrate their use to guide randomized planners towards more convergent trajectories through three extensions to the kinodynamic RRT. The first strictly thresholds action selection based on these metrics, forcing the planner to find a solution that lies within a contraction region over which all initial conditions converge exponentially to a single trajectory. However, finding such a monotonically contracting plan is not always possible. Thus, we propose a second method that relaxes these strict requirements to find “convergent” (i.e. low-divergence) plans. The third algorithm uses these metrics for post-planning path selection. Two examples test the ability of these metrics to lead the planners to more robust trajectories: a mobile robot climbing a hill and a manipulator rearranging objects on a table.