Integrated trajectory control and collision avoidance for automated driving