System and method for simultaneous localization and map building

• US 8,340,852 B2
• Filed: 04/29/2009
• Issued: 12/25/2012
• Est. Priority Date: 04/29/2009
• Status: Active Grant

• Current Assignee: Honeywell International Inc.
• Original Assignee: Honeywell International Inc.
• Inventors: Bageshwar, Vibhor L., Ariyur, Kartik B.
• Primary Examiner(s): Morris, Lesley D
• Assistant Examiner(s): KNUTSON, JACOB D
• Application Number:
  US12/432,026
• Publication Number:
  US 20100280699A1
• Time in Patent Office: 1,336 Days
• Field of Search: 701/23, 701/28, 701/200, 701/207, 701/217, 340/995.24, 340/995.25
• US Class Current: 701/26
• CPC Class Codes: B64G 1/244 Spacecraft control systems
• CPC Class Codes: G05D 1/0231 using optical position detecting means position-fixing by using electromagnetic waves other than radio waves, e.g. optical position detecting means G01S5/16
• CPC Class Codes: G05D 1/024 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0251 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision stereoscopic image analysis H04N13/00; depth recovery from images G06T7/593
• CPC Class Codes: G05D 1/0255 using acoustic signals, e.g. ultra-sonic singals sonar systems designed for anti-collision purposes G01S15/93
• CPC Class Codes: G05D 1/0257 using a radar radar systems designed for anti-collision purposes between land vehicles or between land vehicle and fixed obstacles G01S13/931
• CPC Class Codes: G05D 1/0875 specially adapted to water vehicles

Abstract

An autonomous vehicle comprises at least one image sensor to provide measurements of landmark position for a plurality of landmarks; and processing functionality to estimate the position of the plurality of landmarks in a global frame and in the autonomous vehicle'"'"'s frame, and to estimate the kinematic state of the autonomous vehicle in a global frame based, at least in part, on the measurements of landmark position from the at least one image sensor. The processing functionality is further operable to calculate errors in the estimated positions of the plurality of landmarks in the global frame and in the estimate of the kinematic state of the autonomous vehicle in the global frame by using a plurality of unit projection vectors between the estimated positions of the plurality landmarks in the autonomous vehicle'"'"'s frame and a plurality of unit projection vectors between the estimated positions of the plurality of landmarks in the global frame.

Claims

1. An autonomous vehicle comprising:

at least one image sensor to provide measurements of landmark position for a plurality of landmarks; and

processor to estimate the position of the plurality of landmarks in a global frame and in the autonomous vehicle'"'"'s frame, and to estimate the kinematic state of the autonomous vehicle in a global frame based, at least in part, on the measurements of landmark position from the at least one image sensor;

wherein the processor is further operable to calculate errors in the estimated positions of the plurality of landmarks in the global frame and in the estimate of the kinematic state of the autonomous vehicle in the global frame by using a plurality of unit projection vectors between the estimated positions of the plurality landmarks in the autonomous vehicle'"'"'s frame and a plurality of unit projection vectors between the estimated positions of the plurality of landmarks in the global frame.

2. The autonomous vehicle of claim 1, wherein the at least one image sensor comprises one or more of stereo vision cameras, a Laser Detection and Ranging (LADAR) sensor, a milli-meter wave Radio Detection and Ranging (RADAR) sensor, and an ultrasonic range finder.

3. The autonomous vehicle of claim 1, further comprising an inertial measurement unit (IMU) to provide measurements related to the kinematic state of the autonomous vehicle in the autonomous vehicle'"'"'s frame.

4. The autonomous vehicle of claim 1, wherein the processor is operable to calculate an optimal solution to a constrained optimization problem using the unit projection vectors from the autonomous vehicle'"'"'s frame and the unit projection vectors from the global frame to calculate heading angle drift errors, and to calculate the errors in the estimated positions of the plurality of landmarks in the global frame and in the estimate of the kinematic state of the autonomous vehicle in the global frame.

5. The autonomous vehicle of claim 4, wherein the processor is operable to update the estimated positions of the plurality of landmarks in the global frame and the estimate of the kinematic state of the autonomous vehicle in the global frame based on the calculated heading angle drift errors.

6. The autonomous vehicle of claim 5, further comprising:

at least one actuator responsive to the processor and operable to control movement of the autonomous vehicle;

wherein the processor is operable to provide control signals to the at least one actuator based on the updated estimates of the positions of the plurality of landmarks in the global frame and the updated estimate of the kinematic state of the autonomous vehicle in the global frame.

7. The autonomous vehicle of claim 1, wherein the autonomous vehicle is one of an unmanned land vehicle, an unmanned aerial vehicle, a lunar lander, a planetary rover, and an unmanned underwater vehicle.