Trajectory planning using Mixed Integer Linear Programming (MILP) is a powerful approach because vehicle dynamics and other constraints can be taken into account. However, it is currently severely limited by poor scalability. This paper presents a new approach which improves the scalability regarding the amount of obstacles and the distance between the start and goal positions. While previous approaches hit computational limits when the problem contains tens of obstacles, our approach can handle tens of thousands of polygonal obstacles successfully on a typical consumer computer. This performance is achieved by dividing the problem into many smaller MILP subproblems using two sets of heuristics. Each subproblem models a small part of the trajectory. The subproblems are solved in sequence, gradually building the desired trajectory. The first set of heuristics generate each subproblem in a way that minimizes its difficulty, while preserving stability. The second set of heuristics select a limited amount obstacles to be modeled in each subproblem, while preserving consistency. To demonstrate that this approach can scale enough to be useful in real, complex environments, it has been tested on maps of two cities with trajectories spanning over several kilometers.
展开▼
