Robust online motion planning with reachable sets

In this paper we consider the problem of generating motion plans for a nonlinear dynamical system that are guaranteed to succeed despite uncertainty in the environment, parametric model uncertainty, disturbances, and/or errors in state estimation. Furthermore, we consider the case where these plans must be generated online, because constraints such as obstacles in the environment may not be known until they are perceived (with a noisy sensor) at runtime. Previous work on feedback motion planning for nonlinear systems was limited to offline planning due to the computational cost of safety verification. Here we augment the traditional trajectory library approach by designing locally stabilizing controllers for each nominal trajectory in the library and providing guarantees on the resulting closed loop systems. We leverage sums-of-squares programming to design these locally stabilizing controllers by explicitly attempting to minimize the size of the worst case reachable set of the closed-loop system subjected to bounded disturbances and uncertainty. The reachable sets associated with each trajectory in the library can be thought of as “funnels” that the system is guaranteed to remain within. The resulting funnel library is then used to sequentially compose motion plans at runtime while ensuring the safety of the robot. A major advantage of the work presented here is that by explicitly taking into account the effect of uncertainty, the robot can evaluate motion plans based on how vulnerable they are to disturbances. We demonstrate our method on a simulation of a plane flying through a two dimensional forest of polygonal trees with parametric uncertainty and disturbances in the form of a bounded “cross-wind”. Note to Practitioners—We are motivated by the need for planning algorithms for robots that are able to deal with uncertainty in the form of unknown or unmodeled dynamics, state estimation errors and obstacle positions that are unknown until runtime. Existing approaches to this problem typically either fail to provide formal guarantees on the behavior of the system subjected to disturbances and uncertainty or are unable to deal with a priori unknown environments. Our approach is to compute a set of “motion primitives” for which we can provide formal guarantees on the behavior of the closed-loop system. In particular, for each motion primitive, we can provide bounds on where the system may end up given that it starts off in some set of initial conditions. One can visuzalize these sets as “funnels” that the system is guaranteed to remain within (there is one funnel associated with each motion primitive). At runtime, when the robot encounters a novel environment, it can combine these motion primitives in order to plan its way safely through the environment. We provide an example of how this approach may be used on a model of an unmanned aerial vehicle (UAV) flying through a forest with unknown (but bounded) speed while subjected to a cross-wind. There are two main challenges that practitioners must overcome when applying this approach. First, the computational tools used for computing “funnels” can often run into numerical difficulties. Second, hardware implementation of the approach requires one to have a reasonably accurate model of the dynamics of the system and the uncertainty and Anirudha Majumdar and Russ Tedrake are with the Massachusetts Institute of Technology, 77 Massachusetts Avenue, Cambridge MA 02139. Email: {anirudha,russt}@mit.edu. disturbances it is subjected to. While not the focus here, examples of applying related algorithms to physical systems can be found in [19] and [25]. We are currently experimenting with this approach for different application domains including humanoid locomotion.