Rule-Compliant Trajectory Repairing using Satisfiability Modulo Theories

Autonomous vehicles must comply with traffic rules. However, most motion planners do not explicitly consider all relevant traffic rules. Once traffic rule violations of an initially-planned trajectory are detected, there is often not enough time to replan the entire trajectory. To solve this problem, we propose to repair the initial trajectory by investigating the satisfiability modulo theories paradigm. This framework makes it efficient to reason whether and how the trajectory can be repaired and, at the same time, determine the part along the trajectory that can remain unchanged. Moreover, the robustness of traffic rule satisfaction is used to formulate a convex optimization problem for generating rule-compliant trajectories. We compare our approach with trajectory replanning and demonstrate its usefulness with traffic scenarios from the CommonRoad benchmark suite and recorded data. The evaluation result shows that rule-compliant trajectory repairing is computationally efficient and widely applicable.