In accordance with an example embodiment, a vehicle path planning system and a method for planning a path of a vehicle having a work tool are provided. The method includes determining an actual path through a work area, modifying the actual path with a margin to determine a modified path plan through the work area, and passing the work tool through the work area with the vehicle along the modified path plan.
