Hierarchical trajectory optimization for a class of hybrid dynamical systems

We discuss trajectory optimization for a class of hybrid systems with a natural, hierarchical separation of discrete and continuous dynamics, where the continuous dynamics do not change with the discrete state. The trajectory optimization problem considered requires that a discrete state sequence and a continuous state trajectory must be both determined to minimize a single cost function, such that the discrete state sequence also solves a symbolic planning problem. We model this symbolic planning problem as a search on a planning graph, and we introduce a family of graphs called lifted planning graphs parametrized by an integer H. We define a family of continuous state trajectory optimization problems and associate them with edge costs in the lifted planning graphs. Next, we present an algorithm for finding an optimal solution to the hybrid trajectory optimization problem, which includes mapping paths in the lifted planning graphs to discrete state sequences and continuous state trajectories. We show that the cost of optimal hybrid trajectories is a nonincreasing function of H, and that there exists a finite H for which this cost attains a minimum. We illustrate the proposed algorithm with numerical simulation results for two application examples: an autonomous mobile vehicle and an autonomous robotic manipulator.