Safe motion planning for autonomous driving