Guidance, navigation, and control system for a vehicle

A method for automating the operation of a self-propelled vehicle in a passageway environment, the vehicle including one or more odometric sensors, one or more rangefinder sensors mounted on said vehicle, and a microprocessor including memory storage, said one or more odometric sensors and one or more rangefinder sensors being connected to said microprocessor, said vehicle being configured for microprocessor controlled operation with said microprocessor being interfaced with the vehicle's throttle/engine system, transmission system, braking system, and steering system, said method including the steps of:

• a) teaching a vehicle path through said passageway environment by the steps of,
• i) conducting a training run of said vehicle path through said passageway environment by driving said vehicle through said passageway environment, and
• ii) acquiring raw sensor data from said one or more odometric sensors and said one or more rangefinder sensors during said training run of said vehicle path and storing said raw sensor data in a log file in said microprocessor memory storage;
• b) processing said raw sensor data contained in said log file and producing therefrom a route profile and storing said route profile in said microprocessor memory storage, said route profile including
• i) information representing a vehicle path through said passageway environment,
• ii) a sequence of local metric submaps of said passageway environment along said vehicle path, and
• iii) a speed profile defined as a function of distance along the vehicle path;
• and
• c) automatically driving said vehicle along said vehicle path by the steps of,
• i) determining longitudinal, lateral, and heading errors of said vehicle with respect to the vehicle path by comparing said raw sensor data from said odometric sensors and said rangefinder sensors to said local metric submaps and said microprocessor using said longitudinal, lateral and heading errors and knowledge of said vehicle's dynamics to control said steering system to adjust a direction of travel of said vehicle if needed for ensuring said vehicle closely tracks said vehicle path from said information representing said vehicle path stored in said route profile,
• ii) estimating a speed of said vehicle from data from said odometric sensors, and said microprocessor controlling said throttle/engine system, transmission system, and a braking system to adjust speed of travel of said vehicle if needed for ensuring said vehicle closely tracks said speed profile stored in said route profile by using said estimated speed and knowledge of said vehicle's dynamics, and
• iii) said microprocessor instructing said vehicle to stop in the event of an emergency situation arising.