Motion planning with invariant set trees

This paper introduces the planning algorithm Sa-feRRT, which extends the rapidly-exploring random tree (RRT) algorithm by using feedback control and positively invariant sets to guarantee collision-free closed-loop path tracking. The SafeRRT algorithm steers the output of a system from a feasible initial value to a desired goal, while satisfying input constraints and non-convex output constraints. The algorithm constructs a tree of local state-feedback controllers, each with a randomly sampled reference equilibrium and corresponding positively invariant set. The positively invariant sets indicate when it is possible to safely transition from one local controller to another without violating constraints. The tree is expanded from the desired goal until it contains the initial condition, at which point traversing the tree yields a dynamically feasible and safe closed-loop trajectory. We demonstrate SafeRRT on a spacecraft rendezvous example.