SBP-Guided MPC to Overcome Local Minima in Trajectory Planning

Trajectory planning is often a difficult task for highdimensional systems, especially those with non-linear dynamics. Two common methods for trajectory planning in non-linear systems are Model Predictive Control (MPC) and kinodynamic Sampling-Based Planning (SBP). In this paper, we focus on a variant of MPC called the iterative Linear Quadratic Gaussian (iLQG) algorithm[1]. iLQG has been shown to be fast and effective for generating trajectories to reach a goal. However, when optimizing for non-linear dynamics, it runs the risk of falling into local minima. Unlike iLQG, SBP methods such as Rapidly-exploring Random Trees (RRT) [2] can be robust to local minima because they explore by taking random actions instead of following a gradient. However, for similar reasons, SBP often produces inefficient trajectories. We combine these two algorithms to take advantage of the specific strengths of each. We show that by using an RRT-produced trajectory as a warm start for iLQG, we can overcome local minima while still producing an efficient trajectory. On a specific system model (a robot snake), we show that the combination of these two algorithms allows the robot to reach a goal faster than the use of either algorithm alone in most cases. In general terms, we define our problem as follows. Consider a robot model with state space x and action space u. The dynamics (or forward) model f determines the behavior of the system in discrete time steps as f(xt,ut) = xt+1. The goal is to determine an action sequence U = {u0...uT } such that the final state of the robot xT matches a desired configuration xd. We note that xd might only specify desired values for a subset of x, leaving the rest of the state variables unconstrained.