Implement controller having angular rate correction

• US 8,924,096 B2
• Filed: 10/31/2012
• Issued: 12/30/2014
• Est. Priority Date: 10/31/2012
• Status: Active Grant

• Current Assignee: Caterpillar Incorporated
• Original Assignee: Caterpillar Incorporated
• Inventors: Fehr, Drew Alan, Friend, Paul R.
• Primary Examiner(s): Nguyen, Kim T
• Application Number:
  US13/665,472
• Publication Number:
  US 20140121909A1
• Time in Patent Office: 790 Days
• Field of Search: 701/23, 701/26, 701/37, 701/38, 701/50
• US Class Current: 701/50
• CPC Class Codes: E02F 3/842 using electromagnetic, optical or photoelectric beams, e.g. laser beams
• CPC Class Codes: E02F 9/205 Remotely operated machines, e.g. unmanned vehicles E02F3/8866 takes precedence
• CPC Class Codes: E02F 9/265 with follow-up actions (e.g. control signals sent to actuate the work tool)
• CPC Class Codes: G01S 19/52 Determining velocity
• CPC Class Codes: G01S 19/53 Determining attitude

Abstract

A system and method of controlling an earth moving implement of a machine is disclosed. The method may include receiving a pitch rate of the machine in a body reference frame and determining a roll of the machine. The method may further include determining a turning rate of the machine and calculating a pitch rate of the machine in a gravity reference frame based on the pitch rate of the machine in the body reference frame, the roll of the machine, and the turning rate of the machine. The method may further include controlling the earth moving implement based on the pitch rate of the machine in the gravity reference frame.

Claims

1. A method of controlling an earth moving implement of a machine, comprising:

receiving a pitch rate of the machine in a body reference frame;

determining a roll of the machine;

determining a turning rate of the machine;

calculating a pitch rate of the machine in a gravity reference frame based on the pitch rate of the machine in the body reference frame, the roll of the machine, and the turning rate of the machine; and

controlling the earth moving implement based on the pitch rate of the machine in the gravity reference frame.

2. The method of claim 1, wherein the pitch rate of the machine is received from a gyroscope.

3. The method of claim 1, wherein the roll of the machine is determined based on signals received from a first GPS receiver and a second GPS receiver mounted side by side on the earth moving implement.

4. The method of claim 3, wherein the roll of the machine is calculated as:

Machine_Roll=a sin [(right_gps_elev−

left_gps_elev)/horizontal_distance_between_gps)],

where, Machine_Roll is the roll of the machine, right_gps_elev is an elevation of the first GPS receiver, left_gps_elev is an elevation of the second GPS receiver, and horizontal_distance_between_gps is a distance between the first and second GPS receivers.

5. The method of claim 1, wherein the pitch rate of the machine in the gravity reference frame is calculated as:

Pitch_Rate_Gravity=Pitch_Rate_Body−

Machine_Yaw_Rate×

sin (Machine_Roll),

where Pitch_Rate_Gravity refers to the pitch rate of the machine in the gravity reference frame, Pitch_Rate_Body refers to the pitch rate of the machine in the body reference frame, Machine_Yaw_Rate refers to the turning rate, and Machine_Roll refers to the roll.

6. The method of claim 1, wherein determining the turning rate includes:

detecting a traction device speed for a right traction device and a left traction device, and

determining the turning rate based on a difference between the traction device speeds for the right traction device and the left traction device.

7. The method of claim 6, wherein the turning rate is determined as follows:

Machine_Yaw_Rate=a sin [(right_track_spd−

left_track_spd)/distance_between_track)],

where Machine_Yaw_Rate refers to the turning rate, right_track_spd refers to the traction device speed of the right traction device, left_track_spd refers to the traction device speed of the left traction device, and distance_between_track refers to a distance between the right and left traction devices.

8. An implement controller for controlling an earth moving implement of a machine, comprising:

a pitch rate sensor configured to provide a pitch rate of the machine in a body reference frame;

a roll sensor configured to provide a signal indicative of a roll of the machine;

a turning rate sensor configured to provide a signal indicative of a turning rate of the machine; and

a controller in communication with the pitch rate sensor, the roll sensor, and the turning rate sensor, the controller configured to;

determine a roll of the machine based on the signal indicative of the roll of the machine,

determine a turning rate of the machine based on the signal indicative of the turning rate of the machine,

calculate a pitch rate of the machine in a gravity reference frame based on the pitch rate of the machine in the body reference frame, the roll of the machine, and the turning rate of the machine, and

control the earth moving implement based on the pitch rate of the machine in the gravity reference frame.

9. The implement controller of claim 8, wherein the pitch rate sensor includes a gyroscope.

10. The implement controller of claim 8, wherein the roll sensor includes a first GPS receiver and a second GPS receiver mounted side by side on the earth moving implement.

11. The implement controller of claim 10, wherein the controller is configured to calculate the roll of the machine as:

Machine_Roll=_—

a sin [(right_gps_elev−

left_gps_elev)/horizontal_distance_between_gps)]

where, Machine_Roll is the roll of the machine, right_gps_elev is an elevation of the first GPS receiver, left_gps_elev is an elevation of the second GPS receiver, and horizontal_distance_between_gps is a distance between the first and second GPS receivers.

12. The implement controller of claim 8, wherein the controller is configured to calculate the pitch rate of the machine in the gravity reference frame as:

Pitch_Rate_Gravity=Pitch_Rate_Body−

Machine_Yaw_Rate×

sin (Machine_Roll),

where Pitch_Rate_Gravity refers to the pitch rate of the machine in the gravity reference frame, Pitch_Rate_Body refers to the pitch rate of the machine in the body reference frame, Machine_Yaw_Rate refers to the turning rate, and Machine_Roll refers to the roll.

13. The implement controller of claim 8, wherein the controller is configured to determine the turning rate by:

detecting a traction device speed for a right traction device and a left traction device, and

determining the turning rate based on a difference between the traction device speeds for the right traction device and the left traction device.

14. The implement controller of claim 13, wherein the controller is configured to determine the turning rate as follows:

Machine_Yaw_Rate=a sin [(right_track_spd−

left_track_spd)/distance_between_track)],

where Machine_Yaw_Rate refers to the turning rate, right_track_spd refers to the traction device speed of the right traction device, left_track_spd refers to the traction device speed of the left traction device, and distance_between_track refers to a distance between the right and left traction devices.

15. A non-transitory computer-readable storage medium storing instructions for enabling a processor to execute a method controlling an earth moving implement of a machine, the method comprising:

receiving a pitch rate of the machine in a body reference frame;

determining a roll of the machine;

determining a turning rate of the machine;

calculating a pitch rate of the machine in a gravity reference frame based on the pitch rate of the machine in the body reference frame, the roll of the machine, and the turning rate of the machine; and

controlling the earth moving implement based on the pitch rate of the machine in the gravity reference frame.

16. The non-transitory computer-readable storage medium of claim 15, wherein the pitch rate of the machine is received from a gyroscope.

17. The non-transitory computer-readable storage medium of claim 15, wherein the roll of the machine is determined based on signals received from a first GPS receiver and a second GPS receiver mounted side by side on the earth moving implement.

18. The non-transitory computer-readable storage medium of claim 17, wherein the roll of the machine is calculated as:

Machine_Roll=a sin [(right_gps_elev−

gps_elev-left_gps_elev)/horizontal_distance_between_gps)],

where Machine_Roll is the roll of the machine, right_gps_elev is an elevation of the first GPS receiver, left_gps_elev is an elevation of the second GPS receiver, and horizontal_distance_between_gps is a distance between the first and second GPS receivers.

19. The non-transitory computer-readable storage medium of claim 15, wherein the pitch rate of the machine in the gravity reference frame is calculated as:

Pitch_Rate_Gravity=Pitch_Rate_Body−

Machine_Yaw_Rate×

sin(Machine_Roll),

where Pitch_Rate_Gravity refers to the pitch rate of the machine in the gravity reference frame, Pitch_Rate_Body refers to the pitch rate of the machine in the body reference frame, Machine_Yaw_Rate refers to the turning rate, and Machine_Roll refers to the roll.

20. The non-transitory computer-readable storage medium of claim 15, wherein determining the turning rate includes:

detecting a traction device speed for a right traction device and a left traction device, and

determining the turning rate based on a difference between the tractor device speeds for the right traction device and the left traction device.