A control system for parking a vehicle uses a directed graph representing a traffic network within parking space to determine a coarse path subsequently refined to control the vehicle according to refined path. The directed graph includes waypoints connected by edges, a waypoint defines a position and an orientation, while edge defines a collision free path for the vehicle. The coarse path is selected as a shortest route of waypoints connecting an initial waypoint on the directed graph closest to the initial state of the vehicle with a target waypoint on the directed graph closest to the target state of the vehicle. The refined path is a path on a tree constructed to reduce a deviation of a cost of a motion defined by a refined path connecting the initial node with the target node of the tree from a cost of the coarse path defining the route of waypoints.