Feedback motion planning approach for nonlinear control using gain scheduled RRTs

A new control strategy based on feedback motion planning is presented for solving nonlinear control problems in constrained environments. The algorithm explores the state-space using a bi-directional rapidly exploring random tree (biRRT) in order to find a feasible trajectory between an initial and goal state. By incrementally scheduling LQR controllers, it attempts to connect states so as to link the two trees. These attempts are evaluated by verifying that the connected state is inside the controllable area of an infinite time horizon controller at the goal. This allows for a rapid delineation of equivalent neighborhoods in the state-space. As a result, random exploration is terminated as soon as a feasible solution is made possible by feedback means, avoiding oversampling and partially introducing optimal actions at the neighborhood of the connection. The algorithm is demonstrated and compared against a biRRT using single-link pendulum and cart-pole swing-up tasks amongst obstacles, the latter showing a nearly order of magnitude more efficient search.