Multi-source positioning system for work machines

• US 7,139,651 B2
• Filed: 03/05/2004
• Issued: 11/21/2006
• Est. Priority Date: 03/05/2004
• Status: Active Grant

• Current Assignee: Modular Mining Systems Incorporated (Komatsu Ltd.)
• Original Assignee: Modular Mining Systems Incorporated (Komatsu Ltd.)
• Inventors: Knowlton, Timothy J., Gay, Kenneth W.
• Primary Examiner(s): Black, Thomas
• Assistant Examiner(s): Marc, McDieunel
• Application Number:
  US10/795,198
• Publication Number:
  US 20050197755A1
• Time in Patent Office: 991 Days
• Field of Search: 701/4, 701/23, 701/45, 701/50, 701/211, 701/213, 701/214, 701/215, 701/216, 701/220, 701/225, 250/340, 250/342
• US Class Current: 701/50
• CPC Class Codes: E02F 9/2037 Coordinating the movements of the implement and of the frame
• CPC Class Codes: E02F 9/2045 Guiding machines along a predetermined path for graders E02F3/841; machines for construction of roads E01C19/004
• CPC Class Codes: E21C 47/00 Machines for obtaining or the removal of materials in open-pit mines obtaining peat E21C49/00
• CPC Class Codes: G01S 19/426 by combining or switching between position solutions or signals derived from different modes of operation in a single system
• CPC Class Codes: G01S 19/47 the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
• CPC Class Codes: G01S 19/50 whereby the position solution is constrained to lie upon a particular curve or surface, e.g. for locomotives on railway tracks
• CPC Class Codes: G01S 19/53 Determining attitude

Abstract

A three-dimensional and a two-dimensional GPS unit periodically provide coordinate positions of points on the work machine and are combined with a Kalman filter to produce a point of reference on a work machine. The point of reference is improved by combining the position with an inertial position derived from accelerometer data in another Kalman filter. Additionally, the two-dimensional GPS unit provides a heading that is combined with an inertial orientation derived by the angular rate from a gyroscope in another Kalman filter to produce a precise orientation estimate. Inclinometers provide pitch and roll of the work machine. With the point of reference, orientation estimate, pitch, roll and known geometry of the work machine, the GPS unit calculates the location and orientation of machine components and continuously tracks its movement throughout the mine. Additionally, the accelerometers and gyroscope provide position and orientation when the GPS units are inoperable.

Claims

1. A method for determining a position and an orientation of a work machine having a known geometry comprising the following steps:

(a) obtaining spatial coordinates of a point of reference on the work machine from a three-dimensional global positioning system unit;

(b) obtaining planar coordinates of said point of reference on the work machine from a two-dimensional global positioning system unit;

(c) processing the spatial coordinates and the planar coordinates from said global positioning system units in a first data fusion procedure to produce an improved position estimate of said point of reference;

(d) obtaining an orientation of the work machine from the two-dimensional global positioning system unit;

(e) obtaining pitch and roll of the work machine from an attitude sensor;

(f) calculating a position and an orientation of critical work-machine components utilizing the improved position estimate of the point of reference, the orientation, the pitch, the roll and the known geometry of the work machine; and

(g) repeating steps (a)–

(f) at predetermined time intervals to update said position and orientation of the critical work-machine components.

2. The method of claim 1, wherein the first data fusion procedure of step (c) is performed with a Kalman filter.

3. The method of claim 1, wherein step (e) is carried out utilizing inclinometers.

4. The method of claim 1, wherein step (e) is carried out by the following steps:

obtaining linear accelerations associated with the work machine from inertial sensors; and

calculating said pitch and roll from the linear accelerations.

5. The method of claim 1, further including the following steps prior to step (d):

obtaining linear accelerations associated with the work machine from inertial sensors;

calculating an inertial spatial position of said point of reference from the linear accelerations; and

processing the inertial spatial position and said improved position estimate from step (c) in a second data fusion procedure to produce a more improved position estimate.

6. The method of claim 5, wherein said first and second data fusion procedures are performed with Kalman filters.

7. The method of claim 5, wherein step (e) is carried out utilizing inclinometers.

8. The method of claim 5, wherein step (e) is carried out by calculating said pitch and roll from said linear accelerations associated with the work machine.

9. The method of claim 1, further including the following steps prior to step (e):

obtaining an angular rate of rotation of the work machine from an inertial sensor;

calculating an inertial orientation of the work machine from said angular rate of rotation; and

processing said inertial orientation and the orientation from step (d) in a second data fusion procedure to produce an improved orientation estimate.

10. The method of claim 9, wherein said first and second data fusion procedures are performed with Kalman filters.

11. The method of claim 9, wherein step (e) is carried out utilizing inclinometers.

12. The method of claim 9, wherein step (e) is carried out by the following steps:

obtaining linear accelerations associated with the work machine from inertial sensors; and

calculating said pitch and roll from the linear accelerations.

13. The method of claim 5, further including the following steps:

obtaining an angular rate of rotation of the work machine from an inertial sensor;

calculating an inertial orientation of the work machine from said angular rate of rotation; and

processing said inertial orientation and the orientation from step (d) in a third data fusion procedure to produce an improved orientation estimate.

14. The method of claim 13, wherein said first, second, and third data fusion procedures are performed Kalman filters.

15. The method of claim 13, wherein step (e) is carried out utilizing inclinometers.

16. The method of claim 13, wherein step (e) is carried out by calculating said pitch and roll from said linear accelerations associated with the work machine.

17. The method of claim 1 wherein step (c) is replaced by the following steps:

verifying whether the spatial coordinates of the point of reference from said three-dimensional global positioning system unit of step (a) are at an acceptable reliability level, and when said spatial coordinates are not at an acceptable reliability level,

obtaining linear accelerations associated with the work machine from inertial sensors;

calculating an inertial spatial position of said point of reference from the linear accelerations; and

processing the inertial spatial position and the planar coordinates from said two-dimensional global positioning system unit of step (b) in a first data fusion procedure to produce an improved position estimate of said point of reference.

18. The method of claim 17, wherein said first data fusion procedure is a Kalman filter.

19. The method of claim 17, wherein step (e) is carried out utilizing inclinometers.

20. The method of claim 17, wherein step (e) is carried out by calculating said pitch and roll from said linear accelerations associated with the work machine.

21. The method of claim 17 further including the following steps prior to step (e):

obtaining an angular rate of rotation of the work machine from an inertial sensor;

calculating an inertial orientation of the work machine from said angular rate of rotation; and

processing said inertial orientation and the orientation from step (d) in a second data fusion procedure to produce an improved orientation estimate.

22. The method of claim 21, wherein said first and second data fusion procedures are performed by Kalman filters.

23. The method of claim 21, wherein step (e) is carried out utilizing inclinometers.

24. The method of claim 21, wherein step (e) is carried out by calculating said pitch and roll from said linear accelerations associated with the work machine.

25. The method of claim 1 wherein the following steps replace steps (c–

f);

verifying whether the spatial and planar coordinates of the point of reference from said three-dimensional and two-dimensional global positioning system units, respectively, are at acceptable reliability levels, and when said spatial coordinates and said planar coordinates are not at acceptable levels,

obtaining linear accelerations associated with the work machine from an inertial sensor;

calculating an inertial spatial position of point of reference from the linear accelerations;

obtaining an angular rate of rotation of the work machine from inertial sensors;

calculating an inertial orientation of the work machine from said angular rate of rotation;

obtaining pitch and roll of the work machine from an attitude sensor; and

calculating a position and an orientation of critical work-machine components utilizing the inertial spatial position of the point of reference, the inertial orientation, the pitch, the roll and the known geometry of the work machine.

26. The method of claim 25, wherein step (e) is carried out utilizing inclinometers.

27. The method of claim 25, wherein step (e) is carried out by calculating said pitch and roll from said linear accelerations associated with the work machine.

28. An apparatus for determining a position and an orientation of a work machine having a known geometry comprising:

a three-dimensional global positioning system unit for obtaining spatial coordinates of a point of reference on the work machine;

a two-dimensional global positioning system unit for obtaining planar coordinates of said point of reference on the work machine and for obtaining an orientation of the work machine;

an attitude sensor for obtaining pitch and roll of the work machine;

a first data fusion filter for producing an improved position estimate of said point of reference from the spatial coordinates and the planar coordinates; and

a processing means for calculating a position and an orientation of critical work-machine components from the improved position estimate of the point of reference, and from the orientation, the pitch, the roll and the known geometry of the work machine;

wherein said position and orientation of critical work-machine components are updated at predetermined time-intervals.

29. The apparatus of claim 28, further comprising:

inertial sensors for obtaining linear accelerations associated with the work machine;

means for calculating an inertial spatial position of said point of reference from the linear accelerations; and

a second data fusion filter for producing a more improved position estimate from the inertial spatial position and said improved position estimate.

30. The apparatus of claim 28, further comprising:

an inertial sensor for obtaining an angular rate of rotation of the work machine;

means for calculating an inertial orientation of the work machine from said angular rate of rotation; and

a second data fusion filter for producing an improved orientation estimate from the inertial orientation and the orientation from said two-dimensional global positioning system unit.

31. The apparatus of claim 29, further comprising:

an inertial sensor for obtaining an angular rate of rotation of the work machine;

means for calculating an inertial orientation of the work machine from said angular rate of rotation; and

a third data fusion filter for producing an improved orientation estimate from the inertial orientation and the orientation from said two-dimensional global positioning system unit.

32. The apparatus of claim 28, further comprising:

inertial sensors for obtaining linear accelerations associated with the work machine;

means for calculating an inertial spatial position of said point of reference from the linear accelerations; and

a second data fusion procedure for producing a more improved position estimate from the inertial spatial position and the planar coordinate from said two-dimensional global positioning system when the spatial coordinates of the point of reference from said three-dimensional global positioning system unit are not at an acceptable reliability level.

33. The apparatus of claim 32, further comprising:

an inertial sensor for obtaining an angular rate of rotation of the work machine;

means for calculating an inertial orientation of the work machine from said angular rate of rotation; and

a third data fusion procedure for producing a best orientation estimate from the inertial orientation and the orientation from said two-dimensional global positioning system unit.

34. The apparatus of claim 28 further comprising:

inertial sensors for obtaining linear accelerations associated with the work machine;

an inertial sensor for obtaining an angular rate of rotation of the work machine;

means for calculating an inertial spatial position of said point of reference from the linear accelerations; and

means for calculating an inertial orientation of the work machine from said angular rate of rotation;

wherein the inertial spatial position of the point of reference, and the inertial orientation, the pitch, the roll and the known geometry of the work machine are used to calculate a position and an orientation of critical work-machine components when the spatial and planar coordinates of the point of reference from said three-dimensional and two-dimensional global positioning system units, respectively, are not at acceptable reliability levels.