System for autonomous vehicle navigation with carrier phase DGPS and laser-scanner augmentation

• US 7,228,230 B2
• Filed: 11/12/2004
• Issued: 06/05/2007
• Est. Priority Date: 11/12/2004
• Status: Active Grant

• Current Assignee: Auto Tech Group LLC
• Original Assignee: Mitsubishi Denki Kabushiki Kaisha (Mitsubishi Electric Corporation)
• Inventors: Hirokawa, Rui
• Primary Examiner(s): Chin; Gary
• Application Number:
  US10/986,205
• Publication Number:
  US 20060106533A1
• Time in Patent Office: 935 Days
• Field of Search: 701/207, 701/213, 701/214, 701/216, 701/220, 701/221, 342/357.06, 342/358
• US Class Current: 701/214
• CPC Class Codes: G01C 21/1652 with ranging devices, e.g. LIDAR or RADAR
• CPC Class Codes: G01S 19/44 Carrier phase ambiguity resolution; Floating ambiguity; LAMBDA [Least-squares AMBiguity Decorrelation Adjustment] method
• CPC Class Codes: G05D 1/024 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/027 comprising intertial navigation means, e.g. azimuth detector inertial navigation G01C21/16; inertial navigation combined with non-inertial navigation instruments G01C21/165
• CPC Class Codes: G05D 1/0272 comprising means for registering the travel distance, e.g. revolutions of wheels measuring distance traversed on the ground by vehicles, e.g. using odometers G01C22/00
• CPC Class Codes: G05D 1/0278 using satellite positioning signals, e.g. GPS

Abstract

A horizontal navigation system aided by a carrier phase differential Global Positioning System (GPS) receiver and a Laser-Scanner (LS) for an Autonomous Ground Vehicle (AGV). The high accuracy vehicle navigation system is highly demanded for advanced AGVs. Although high positioning accuracy is achievable by a high performance RTK-GPS receiver, the performance should be considerably degraded in a high-blockage environment due to tall buildings and other obstacles. The present navigation system is to provide decimetre-level positioning accuracy in such a severe environment for precise GPS positioning. The horizontal navigation system is composed of a low cost Fiber Optic Gyro (FOG) and a precise odometer. The navigation errors are estimated using a tightly coupled Extended Kalman Filter (EKF). The measurements of the EKF are double differenced code and carrier phase from a dual frequency GPS receiver and relative positions derived from laser scanner measurements.

Claims

1. A navigational device for determining a position and heading of an object, comprising:

a navigation calculation device configured to calculate the position and the heading of the object based on an output of a velocity detecting device and an output of a yaw rate detecting device; and

an estimator configured to estimate, based on a carrier phase and a pseudorange received from a global positioning satellite, (1) an error of the velocity detecting device, (2) an error of the yaw rate detecting device, (3) a position error and a heading error of the object, and (4) an integer-valued bias of a carrier,

wherein the navigation calculation device is configured to update the position and the heading of the object based on the position error and the heading error estimated by the estimator.

2. The navigational device of claim 1, wherein the estimator is configured (1) to estimate a relative range between a known landmark and the object based on a range measured by a range measuring device provided on the object and stored position data of the known landmark, and (2) to calculate an observation error based on the estimated relative range.

3. The navigational device of claim 2, wherein the estimator is configured to estimate the relative range based on geometrical data of at least two known landmarks and a known yaw angle of the range measuring device.

4. The navigational device of claim 1, wherein the navigation calculation device is configured to calculate the position and the heading of the object using the dynamical equations:

5. The navigational device of claim 1, wherein the estimator comprises:

an extended Kalman filter configured to estimate a float ambiguity vector of double-differenced carrier phase; and

an ambiguity resolution device configured to resolve ambiguity in the integer-valued bias of the carrier, using a Lambda method, based on the float ambiguity vector of double-differenced carrier phase estimated by the extended Kalman filter.

6. The navigational device of claim 1, wherein the navigation calculation device is configured to calculate the position and the heading of the object based on an output of an odometer and an output of a rate gyro.

7. A terrestrial vehicle having embedded therein the navigational device of claim 1.

8. A navigational method of determining a position and heading of an object, comprising:

calculating the position and the heading of the object based on an output of a velocity detecting device and an output of a yaw rate detecting device;

estimating, based on a carrier phase and a pseudorange received from a global positioning satellite, (1) an error of the velocity detecting device, (2) an error of the yaw rate detecting device, (3) a position error and a heading error of the object, and (4) an integer-valued bias of a carrier; and

updating the position and the heading of the object based on the estimated position error and the estimated heading error.

9. The method of claim 8, wherein the estimating step comprises:

estimating a relative range between a known landmark and the object based on a range measured by a range measuring device provided on the object and stored position data of the known landmark; and

calculating an observation error based on the estimated relative range.

10. The method of claim 9, wherein the estimating step comprises:

estimating the relative range based on geometrical data of at least two known landmarks and a known yaw angle of the range measuring device.

11. The method of claim 9, wherein the estimating step comprises:

estimating the relative range based on a range measured by a laser scanner.

12. The method of claim 8, wherein the calculating step comprises:

calculating the position and the heading of the object using the dynamical equations;

13. The method of claim 8, wherein the estimating step comprises:

estimating a float ambiguity vector of double-differenced carrier phase using an extended Kalman filter; and

resolving ambiguity in the integer-valued bias of the carrier, using a Lambda method, based on the estimated float ambiguity vector of double-differenced carrier phase estimated by the extended Kalman filter.

14. The method of claim 8, wherein the calculating step comprises:

calculating the position and the heading of the object based on outputs of an odometer and a rate gyro.

15. A navigational system for determining a position and heading of a vehicle using inertial and satellite navigation, comprising:

a velocity detecting device configured to detect a velocity of the vehicle;

a yaw rate detecting device configured to detect a yaw rate of the vehicle;

a landmark database configured to store position data of a known landmark;

a range measuring device attached to the vehicle, the range measuring device configured to measure a range from the vehicle to the known landmark;

a navigation calculation device configured to calculate the position and the heading of the vehicle based on the velocity detected by the velocity detecting device and the yaw rate detected by the yaw rate detecting device; and

an estimator configured to estimate, based on a carrier phase and a pseudorange received from a global positioning satellite, (1) an error of the velocity detecting device, (2) an error of the yaw rate detecting device, (3) a position error and a heading error of the vehicle, and (4) an integer-valued bias of a carrier,

wherein the navigation calculation device is configured to update the position and the heading of the vehicle based on the position error and the heading error estimated by the estimator.

16. The navigational system of claim 15, wherein the estimator is configured (1) to estimate a relative range between the known landmark and the vehicle based on a range measured by a range measuring device provided on the vehicle and position data of the known landmark stored in the landmark database, and (2) to calculate an observation error based on the estimated relative range.

17. The navigational system of claim 16, wherein the estimator is configured to estimate the relative range based on geometrical data of at least two known landmarks stored in the landmark database and a known yaw angle of the range measuring device.

18. The navigational system of claim 15, wherein the navigation calculation device is configured to calculate the position and the heading of the vehicle using the dynamical equations:

19. The navigational system of claim 15, wherein the estimator comprises:

an extended Kalman filter configured to estimate a float ambiguity vector of double-differenced carrier phase; and

an ambiguity resolution device configured to resolve ambiguity in the integer-valued bias of the carrier, using a Lambda method, based on the float ambiguity vector of double-differenced carrier phase estimated by the extended Kalman filter.

20. A vehicle comprising:

a propulsion system configured to propel the vehicle; and

a navigational system for determining a position and heading of the vehicle using inertial and satellite navigation, the navigational system including;

a velocity detecting device configured to detect a velocity of the vehicle;

a yaw rate detecting device configured to detect a yaw rate of the vehicle;

a landmark database configured to store position data of a known landmark;

a range measuring device attached to the vehicle, the range measuring device configured to measure a range from the vehicle to the known landmark;

a navigation calculation device configured to calculate the position and the heading of the vehicle based on the velocity detected by the velocity detecting device and the yaw rate detected by the yaw rate detecting device; and

an estimator configured to estimate, based on a carrier phase and a pseudorange received from a global positioning satellite, (1) an error of the velocity detecting device, (2) an error of the yaw rate detecting device, (3) a position error and a heading error of the vehicle, and (4) an integer-valued bias of a carrier,

wherein the navigation calculation device is configured to update the position and the heading of the vehicle based on the position error and the heading error estimated by the estimator.