Efficient and robust feedback motion planning under uncertainty using the pontryagin difference

This paper proposes a novel application of recent research on sums-of-squares (SOS) optimization to feedback motion planning. We use nonlinear programming (NLP) to provide open-loop control and dynamic trajectories for a vehicle in segments, and then consider the problem of generating global trajectories by using a probabilistic roadmap (PRM) or a rapidly-exploring random tree (RRT). Furthermore, we compute funnels (reachable sets) using SOS optimization along the trajectory in which the vehicle's state is guaranteed to remain [1]. Considering the expensive computation of SOS, we adopt a 'funnel library' to pre-compute funnels [2]. A vehicle is subjected to disturbances due to model uncertainty and sensor noise, and the funnel library is computed without any knowledge of the severity of noise before motion planning. Therefore, we propose to use the Pontryagin difference method to shrink the funnels to account for noise-corrupted measurements, whose availability varies spatially throughout the state space. Our major contribution is to take into account the effect of measurement and model uncertainty in funnel computation, and we propose two efficient algorithms, feedback belief roadmap (FBRM) motion planning and feedback rapidly-exploring random belief trees (FRRBT) motion planning, to generate safe trajectories. Our algorithms are demonstrated in simulated experiments showing their advantages over others.