Invariant Set Planning for Quadrotors: Design, Analysis, Experiments

Abstract

We propose a motion planner for quadrotor unmanned aerial vehicles (UAVs) implemented as a graph search over robust positive invariant (PI) sets. We model the positional error dynamics of the quadrotor in closed loop with an onboard controller as a second-order system with polytopic uncertainty in the gains. We also consider bounded attitude tracking errors and additive input disturbances. We propose a method for computing ellipsoidal robust PI (RPI) sets using linear matrix inequalities that are expanded such that all trajectories therein remain safe, i.e., do not intersect obstacles and ensure satisfaction of UAV constraints. We construct a graph where the vertices are equilibrium positions and the edges are transitions between equilibria occurring within the PI sets. Hence, a graph search returns a sequence of setpoints steering the UAV from an initial position to a target, while remaining within the safe invariant sets. We show that subject to the properties of the graph, from any initial position within an invariant set, any RPI set in the graph is reachable in finite time. The graph construction is offline, and the online graph search and plan execution are simple and fast, thus allowing for real-time planning. We demonstrate the method in extensive simulations and in experiments with a Crazyflie 2.1 quadrotor.