US Patent 8392117 Using topological structure for path planning in semi-structured environments

A method of creating a lane network of a semi-structured environment for a vehicle, assigning a corresponding cost function to each of a plurality of coordinates of the semi-structured environment using the lane network and a state of the vehicle, the state of the vehicle corresponding to a coordinate location of the vehicle and an angular orientation of the vehicle, and determining an obstacle-free path on the semi-structured environment using the cost function of each of the plurality of coordinates.