Autonomous driving motion planning with obstacles prioritization using lexicographic optimization

There are driving situations that avoiding all obstacles is infeasible. In such situations, an autonomous vehicle should avoid vulnerable obstacles like pedestrians. In this paper, a motion planning method is presented that avoids obstacles according to their priority orders. The method utilizes a model predictive controller with obstacle constraints and applies lexicographic optimization to the controller to prioritize the constraints, and subsequently, prioritize the obstacles. The proposed method is simulated on a high fidelity CarSim vehicle model. The results show that when avoiding all obstacles is not feasible, the proposed method avoids the obstacles with the highest priority orders.