Systems, methods, and computer program products for guiding vehicles. A vehicle control system detects obstacles and avoids contact between the vehicle, and the obstacles by solving a path planning problem using a four-phase system. In a parking scenario, switching control laws are used to drive the vehicle to a target line, and a forward path segment and a reverse path segment defined. The two path segments are connected along the target line to define a path for entering or exiting the parking space. Objects in a driving environment may be avoided by identifying an obstacle corner, defining an avoidance circle (