A system and a method for determining an intermediate parking goal of an autonomous vehicle. The autonomous vehicle includes a controller, the controller has a processor and a storage device storing computer executable code. The computer executable code, when executed at the processor, is configured to: prepare a driving plan toward a final parking goal and a parking plan to park at the final parking goal; drive the autonomous vehicle toward the final parking goal according to the driving plan; switch from the driving plan to the parking plan when the autonomous vehicle is within a predetermined distance to the final parking goal; and when the final parking goal is inaccessible: determine the intermediate parking goal, drive and park the autonomous vehicle at the intermediate parking goal.