Robust Obstacle Avoidance using Tube NMPC

This work considers the problem of avoiding obstacles for general nonlinear systems subject to disturbances. Obstacle avoidance is achieved by computing disturbance invariant sets along a nominal trajectory and ensuring these invariant sets do not intersect with obstacles. We develop a novel technique to compute approximate disturbance invariant sets for general nonlinear systems using a set of finite dimensional optimizations. A bi-level NMPC optimization strategy alternates between optimizing over the nominal trajectory and finding the disturbance invariant sets. Simulation results show that the proposed algorithm is able to generate disturbance invariant sets for standard 3D aerial and planar ground vehicles models, and the NMPC algorithm successfully computes obstacle avoidance trajectories using the disturbance invariant sets.