Autonomous driving in semi-structured environments: Mapping and planning
rapidly - exploring random trees : a new tool for path planning .