Limiting Computation Levels in Prioritized Trajectory Planning with Safety Guarantees

In prioritized planning for vehicles, vehicles plan trajectories in parallel or in sequence. In parallel prioritized planning, the computation time remains approximately constant with an increasing number of vehicles, but it is difficult to guarantee collision-free trajectories. Although sequential prioritized planning can guarantee collision-free trajectories, the computation time increases with the number of sequentially computing vehicles, which we call computation levels. This number is determined by the directed coupling graph which results from the coupling and prioritization of vehicles. This work's contribution is twofold. First, we guarantee safe trajectories in parallel planning through reachability analysis. Although these trajectories are collision-free, they tend to be conservative. Second, we address this conservativeness by planning with a subset of vehicles in sequence. We formulate the problem of selecting this subset as a graph partitioning problem, in which we limit the size of the resulting subgraphs. Consequently, we can choose the number of computation levels independently from the directed coupling graph, and thus are able to limit the computation time in prioritized planning. In our simulations, we reduce the number of computation levels to approximately 64% compared to sequential prioritized planning while maintaining the solution quality. Video youtu.be/di6X6XTGt88 Code github.com/embedded-software-laboratory/p-dmpc