Autonomous vehicle controlled by safety path planner with collision risk estimation coupled with a non-linear MPC