System and method for controlling a work machine

Abstract

To provide a system and a method for controlling a work machine with fewer limitation on facilities to carry out calibration.SOLUTION: A controller acquires a roll angle and a pitch angle of a vehicle body detected with a vehicle body sensor. The controller acquires a roll angle of a work machine detected with a work machine sensor. The controller calculates error of a yaw angle of the vehicle sensor corresponding to the vehicle body based on difference between the vehicle roll angle and the roll angle of the work machine and the pitch angle of the vehicle body. The controller calibrates the vehicle body sensor using the error of the yaw angle of the vehicle body sensor.SELECTED DRAWING: Figure 7

Description

Technical Field

[0001] The present invention relates to a system and method for controlling a work machine.

Background Art

[0002] Some work machines are equipped with sensors for detecting the attitude of the work machine. For example, the work machine of Patent Document 1 includes a vehicle body, a work implement attached to the vehicle body, and an attitude sensor. The attitude sensor is attached to the vehicle body. The attitude sensor detects the pitch angle and roll angle of the vehicle body indicating the attitude of the vehicle body.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] In order to accurately detect the roll angle and pitch angle of the vehicle body by the above-described attitude sensor, the attitude sensor needs to be attached to the vehicle body so that the yaw angle of the attitude sensor coincides with the yaw angle of the vehicle body. That is, the front direction of the attitude sensor needs to coincide with the front direction of the vehicle body. However, when the yaw angle of the attitude sensor is deviated from the yaw angle of the vehicle body due to the mounting error of the attitude sensor, an error occurs in the roll angle and pitch angle of the vehicle body detected by the attitude sensor on an inclined ground.

[0005] As a method for calibrating such an error of the attitude sensor, for example, the following is available. First, the work machine is placed on an inclined plane with a known inclination angle. At this time, the work machine is placed so that the direction of the work machine coincides with the inclined plane. If the yaw angle of the attitude sensor is not deviated from the yaw angle of the vehicle body, in this state, the roll angle of the vehicle body of the work machine becomes zero degrees.

[0006] Therefore, when the roll angle of the vehicle body is not zero, an error occurs in the roll angle due to the yaw angle error of the attitude sensor. Therefore, the yaw angle error of the attitude sensor can be obtained by calculating the yaw angle of the vehicle body such that the roll angle of the vehicle body detected by the attitude sensor in the above state becomes zero.

[0007] However, in the calibration method as described above, an inclined plane with a known inclination angle is required. Therefore, the facilities or terrain for calibration are limited. In addition, it is necessary to arrange the work machine on the inclined plane so that the direction of the work machine accurately matches the inclined plane. Such precise operation of the work machine is not easy and takes a lot of time. An object of the present invention is to provide a system and method for controlling a work machine with fewer restrictions on the facilities for calibration.

Means for Solving the Problems

[0008] A system according to an aspect of the present invention is a system for controlling a work machine. The work machine includes a vehicle body and a work implement. The work implement is supported by the vehicle body. The system includes a vehicle body sensor, a work implement sensor, and a controller. The vehicle body sensor is attached to the vehicle body. The vehicle body sensor detects the roll angle, pitch angle, and yaw angle of the vehicle body. The work implement sensor is attached to the work implement. The work implement sensor detects the roll angle of the work implement.

[0009] The controller acquires the roll angle and pitch angle of the vehicle body detected by the vehicle body sensor. The controller acquires the roll angle of the work implement detected by the work implement sensor. The controller calculates the yaw angle error of the vehicle body sensor with respect to the vehicle body based on the difference between the roll angle of the vehicle body and the roll angle of the work implement and the pitch angle of the vehicle body. The controller calibrates the vehicle body sensor using the yaw angle error of the vehicle body sensor.

[0010] A method according to another aspect of the present invention is a method executed by a controller that controls a work machine. The work machine includes a vehicle body, a work implement, a vehicle body sensor, and a work implement sensor. The work implement is supported by the vehicle body. The vehicle body sensor is attached to the vehicle body. The vehicle body sensor detects a roll angle, a pitch angle, and a yaw angle of the vehicle body. The work implement sensor is attached to the work implement. The work implement sensor detects a roll angle of the work implement.

[0011] The method includes obtaining the roll angle and pitch angle of the vehicle body detected by the vehicle body sensor, obtaining the roll angle of the work implement detected by the work implement sensor, calculating a yaw angle error of the vehicle body sensor with respect to the vehicle body based on a difference between the roll angle of the vehicle body and the roll angle of the work implement and the pitch angle of the vehicle body, and calibrating the vehicle body sensor using the yaw angle error of the vehicle body sensor.

Advantages of the Invention

[0012] According to the present invention, a yaw angle error of the vehicle body sensor is calculated using a difference between the roll angle of the work implement detected by the work implement sensor and the roll angle of the vehicle body detected by the vehicle body sensor. Thereby, even on a slope where the inclination angle is unknown and even if the orientation of the vehicle body with respect to the slope does not exactly match, the vehicle body sensor can be accurately calibrated. Therefore, according to the present invention, there are few restrictions on the equipment for calibration. Also, the time for calibration is shortened.

Brief Description of the Drawings

[0013]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Mode for Carrying Out the Invention

[0014] Hereinafter, a working machine according to an embodiment will be described with reference to the drawings. FIG. 1 is a side view showing a working machine 1 according to the embodiment. FIG. 2 is a top view of the working machine 1. The working machine 1 according to the present embodiment is a bulldozer. The working machine 1 includes a vehicle body 2 and a working device 3.

[0015] The vehicle body 2 includes a driver's cab 5, a power chamber 6, and a traveling device 7. A driver's seat (not shown) is arranged in the driver's cab 5. The power chamber 6 is arranged in front of the driver's cab 5. The traveling device 7 supports the vehicle body 2. The traveling device 7 includes left and right crawlers 8a, 8b. The working machine 1 travels by the rotation of the crawlers 8a, 8b.

[0016] The working device 3 is arranged in front of the vehicle body 2. The working device 3 is swingably supported around a lift shaft A1. The lift shaft A1 extends in the left - right direction of the vehicle body 2. The working device 3 includes a working tool 10 and a working device frame 11. In the present embodiment, the working tool 10 is a blade. The working device frame 11 supports the working tool 10. As shown in FIG. 2, the working device frame 11 includes a first frame 12, a second frame 13, and a connecting portion 14.

[0017] The first frame 12 and the second frame 13 extend in the longitudinal direction of the working machine 1. The first frame 12 and the second frame 13 are arranged apart from each other in the left - right direction. The first frame 12 and the second frame 13 are supported by the vehicle body 2 so as to be swingable about the lift axis A1. The connecting portion 14 connects the first frame 12 and the second frame 13. The connecting portion 14 is connected to the working implement 10. The first frame 12, the second frame 13, and the connecting portion 14 swing integrally about the lift axis A1.

[0018] The working machine 3 includes a plurality of actuators 15, 16. The plurality of actuators 15, 16 includes a first lift actuator 15 and a second lift actuator 16. The first lift actuator 15 and the second lift actuator 16 are arranged apart from each other in the left - right direction of the working machine 1.

[0019] The first lift actuator 15 is connected to the vehicle body 2 and the first frame 12. The second lift actuator 16 is connected to the vehicle body 2 and the second frame 13. The first lift actuator 15 and the second lift actuator 16 are hydraulic cylinders. The first lift actuator 15 and the second lift actuator 16 swing the working machine frame 11 up and down about the lift axis A1. Thereby, the working machine 3 performs a lifting operation up and down.

[0020] Figure 3 is a block diagram showing the configuration of the control system of the working machine 1. As shown in Figure 3, the working machine 1 includes a power source 30, a hydraulic pump 31, and a power transmission device 32. The power source 30 is, for example, an internal combustion engine. However, the power source 30 may be an electric motor. Alternatively, the power source 30 may be a hybrid of an internal combustion engine and an electric motor.

[0021] The hydraulic pump 31 is driven by the power source 30 and discharges hydraulic oil. The hydraulic oil discharged from the hydraulic pump 31 is supplied to the lift actuators 15, 16. Although one hydraulic pump 31 is shown in Figure 3, a plurality of hydraulic pumps may be provided.

[0022] The power transmission device 32 transmits the driving force of the power source 30 to the traveling device 7. The power transmission device 32 may be, for example, an HST (Hydro Static Transmission). Alternatively, the power transmission device 32 may be, for example, a torque converter or a transmission having a plurality of speed change gears.

[0023] The working machine 1 includes a controller 33 and a control valve 34. The controller 33 is programmed to control the working machine 1 based on the acquired data. The controller 33 includes a storage device 35 and a processor 36. The processor 36 includes, for example, a CPU. The storage device 35 includes, for example, a memory and an auxiliary storage device. The storage device 35 may be, for example, a RAM or a ROM. The storage device 35 may be a semiconductor memory or a hard disk. The storage device 35 is an example of a non-transitory computer-readable recording medium. The storage device 35 records computer instructions executable by the processor 36 to control the working machine 1.

[0024] The control valve 34 is controlled by a command signal from the controller 33. The control valve 34 is disposed between the actuators 15, 16 and the hydraulic pump 31. The control valve 34 controls the flow rate of the hydraulic oil supplied from the hydraulic pump 31 to the lift actuators 15, 16.

[0025] The working machine 1 includes an operating device 37 and an input device 38. The operating device 37 includes, for example, a lever. Alternatively, the operating device 37 may include a pedal or a switch. The operator can manually operate the traveling of the working machine 1 and the operation of the working implement 3 using the operating device 37. For example, the operating device 37 can operate the lift operation of the working implement 3. The operating device 37 outputs an operation signal indicating the operation of the operating device 37. The controller 33 receives the operation signal from the operating device 37.

[0026] The input device 38 includes, for example, a touch panel. However, the input device 38 may include other devices such as switches. The operator can use the operating device 37 to set the control of the working machine 1. The input device 38 outputs an input signal indicating the input to the input device 38. The controller 33 receives the input signal from the input device 38.

[0027] The working machine 1 includes a vehicle body sensor 41, a frame sensor 42, and a working implement sensor 43. The vehicle body sensor 41 is attached to the vehicle body 2. The vehicle body sensor 41 detects the attitude of the vehicle body 2. The frame sensor 42 is attached to the work machine frame 11. The frame sensor 42 detects the attitude of the work machine frame 11. The working implement sensor 43 is attached to the working implement 3. The working implement sensor 43 detects the attitude of the working implement 10.

[0028] The vehicle body sensor 41 detects the pitch angle, roll angle, and yaw angle of the vehicle body 2. The frame sensor 42 detects the pitch angle, roll angle, and yaw angle of the work machine frame 11. The working implement sensor 43 detects the pitch angle, roll angle, and yaw angle of the working implement 10.

[0029] The vehicle body sensor 41, the frame sensor 42, and the working implement sensor 43 are acceleration sensors such as, for example, an IMU (Inertial Measurement Unit). However, the vehicle body sensor 41, the frame sensor 42, and the working implement sensor 43 are not limited to IMUs and may be other acceleration sensors. Each of the sensors 41 - 43 detects the pitch angle and roll angle based on the gravitational acceleration. Also, each of the sensors 41 - 43 detects the yaw angle by integrating the angular velocity with 0 at startup.

[0030] The controller 33 is communicably connected to the vehicle body sensor 41, the frame sensor 42, and the working implement sensor 43, either by wire or wirelessly. The controller 33 acquires vehicle body attitude data from the vehicle body sensor 41. The vehicle body attitude data indicates the pitch angle, roll angle, and yaw angle of the vehicle body 2. The controller 33 acquires frame attitude data from the frame sensor 42. The frame attitude data indicates the pitch angle, roll angle, and yaw angle of the work implement frame 11. The controller 33 acquires working implement attitude data from the working implement sensor 43. The working implement attitude data indicates the pitch angle, roll angle, and yaw angle of the working implement 10.

[0031] The controller 33 controls the working machine 1 based on the vehicle body attitude data, the frame attitude data, and the working implement attitude data. For example, the controller 33 calculates the position of the working implement 20 based on the vehicle body attitude data, the frame attitude data, and the working implement attitude data. The controller 33 may control the control valve 34 so as to perform a desired operation of the working implement 20 based on the position of the working implement 20.

[0032] Next, a method for calibrating the vehicle body sensor 41 will be described. When the vehicle body sensor 41 is attached to the vehicle body 2 in the correct orientation, as shown in FIG. 4, the x-axis of the vehicle body sensor 41 coincides with the forward direction of the vehicle body 2. That is, the yaw angle error between the vehicle body sensor 41 and the vehicle body 2 is zero degrees. In this case, as shown in FIG. 5, when the vehicle body 2 is tilted at the pitch angle θp, the vehicle body sensor 41 detects the pitch angle θp due to the x-component gsinθp of the gravitational acceleration g. Also, as shown in FIG. 6, when the vehicle body 2 is tilted at the roll angle θr, the vehicle body sensor 41 detects the roll angle θr due to the y-component gsinθr of the gravitational acceleration g. In the drawings, X, Y, and Z indicate the x-axis, Y-axis, and Z-axis of the vehicle body sensor 41, respectively. However, as shown in FIG. 7, the X-axis of the vehicle body sensor 41 may be offset from the forward direction of the vehicle body 2. In this case, assuming that the yaw angle error between the X-axis of the vehicle body sensor 41 and the forward direction of the vehicle body 2 is φ, the vehicle body sensor 41 detects the x component gsinθpcosφ of the gravitational acceleration g. However, this value is smaller than gsinθp that indicates the appropriate pitch angle of the vehicle body 2. Therefore, an error occurs in the pitch angle of the vehicle body 2 detected by the vehicle body sensor 41.

[0033] Also, in this case, as shown in FIG. 8, even if the roll angle of the vehicle body 2 is zero, the vehicle body sensor 41 detects the y component gsinθpsinφ of the gravitational acceleration g. Therefore, an error occurs in the roll angle of the vehicle body 2 detected by the vehicle body sensor 41. Assuming that the error in the roll angle is β, the following equation (1) holds. gsinθpsinφ = gsinβ ···(1) The controller 33 calculates the yaw angle error φ by the following equation (2) φ = sin -1 (sin(θr - θR) / sinθp) ···(2) θR is the roll angle of the work implement frame 11 detected by the frame sensor 42. That is, the controller 33 calculates the yaw angle error φ of the vehicle body sensor 41 with respect to the vehicle body 2 based on the difference between the roll angle of the vehicle body 2 and the roll angle of the work implement frame 11 and the pitch angle of the vehicle body 2. The controller 33 calibrates the pitch angle, roll angle, and yaw angle detected by the vehicle body sensor 41 using the yaw angle error φ of the vehicle body sensor 41.

[0034] Note that it is assumed that the roll angle and pitch angle detected by the vehicle body sensor 41 are calibrated. Also, it is assumed that the roll angle, pitch angle, and yaw angle detected by the frame sensor 42 are calibrated.

[0035] The work implement frame 11 is attached to the vehicle body 2 so as not to be rotatable about the roll axis. The roll axis extends in the longitudinal direction of the vehicle body 2. Therefore, the roll angle of the work implement frame 11 coincides with the roll angle of the vehicle body 2. In the above equation (2), by regarding the roll angle θR of the work implement frame 11 as the correct roll angle of the vehicle body 2 and using it, based on the error of the roll angle of the vehicle body 2 and the pitch angle of the vehicle body 2, the yaw angle error φ of the vehicle body sensor 41 with respect to the vehicle body 2 is calculated.

[0036] In the control system of the working machine 1 according to the present embodiment described above, the yaw angle error φ of the vehicle body sensor 41 is calculated using the difference between the roll angle of the work implement frame 11 detected by the frame sensor 42 and the roll angle of the vehicle body 2 detected by the vehicle body sensor 41. Thereby, even on a slope where the inclination angle is unknown, and even if the forward direction of the vehicle body 2 does not exactly match the slope, the vehicle body sensor 41 can be accurately calibrated. Therefore, in the control system according to the present embodiment, there are few restrictions on the equipment for calibration. Also, the time for calibration is reduced.

[0037] Although one embodiment of the present invention has been described above, the present invention is not limited to the above embodiment, and various modifications are possible without departing from the gist of the invention.

[0038] The working machine 1 is not limited to a bulldozer, and may be other vehicles such as a wheel loader or a motor grader. In the above embodiment, the work implement frame 11 is arranged inside the crawlers 8a, 8b. However, the work implement frame 11 may be arranged outside the crawlers 8a, 8b.

[0039] The controller 33 may have a plurality of controllers that are separate from each other. The working machine 1 may be operable remotely. In that case, the controller 33, the operating device 37, and the input device 38 may be arranged outside the working machine 1. The controller 33 may control the working machine 1 by performing wireless communication with the working machine 1.

[0040] The processing by the controller 33 is not limited to that of the above-described embodiment and may be changed. A part of the processing by the controller 33 may be omitted. Alternatively, a part of the above-described processing may be changed.

[0041] For example, the work implement sensor may be the work tool sensor 43 instead of the frame sensor 42. That is, the controller 33 may calculate the yaw angle error φ of the vehicle body sensor 41 with respect to the vehicle body 2 based on the difference between the roll angle of the vehicle body 2 and the roll angle of the work tool 10 and the pitch angle of the vehicle body 2.

[0042] In the above-described embodiment, the frame sensor 42 is assumed to be calibrated. However, the controller 33 may calibrate the yaw angle of the frame sensor 42 by the following method. The controller 33 may calculate the yaw angle error of the frame sensor 42 with respect to the work implement frame 11 such that the change in the roll angle of the work implement frame 11 becomes 0 while swinging the work implement frame 11 around the lift axis A1. The controller 33 may calibrate the frame sensor 42 using the yaw angle error of the frame sensor 42.

[0043] When the yaw angle error of the frame sensor 42 is zero, the pitch angle of the work implement 3 changes in response to the lift operation of the work implement 3, but the roll angle of the work implement 3 does not change. Therefore, the controller 33 can calculate, as the yaw angle error, the yaw angle of the frame sensor 42 such that the roll angle of the work implement 3 becomes zero even when the work implement 3 performs a lift operation. In this way, the yaw angle error of the frame sensor 42 can be calculated easily and accurately by the lift operation of the work implement 3.

Industrial Applicability

[0044] According to the present invention, there are provided a system and a method for controlling a work machine that have few restrictions on facilities for calibration and can be calibrated in a short time.

Explanation of Signs

[0045] 1: Work machine 2: Vehicle body 3: Working machine 33: Controller 41: Vehicle body sensor 42: Frame sensor 43: Working tool sensor

Claims

1. A system for controlling a work machine, wherein the work machine includes a vehicle body, a work implement supported by the vehicle body, and the system includes a vehicle body sensor attached to the vehicle body for detecting a roll angle, a pitch angle, and a yaw angle of the vehicle body, a work implement sensor attached to the work implement for detecting a roll angle of the work implement, and a controller communicably connected to the vehicle body sensor and the work implement sensor, wherein the controller acquires the roll angle and the pitch angle of the vehicle body detected by the vehicle body sensor, acquires the roll angle of the work implement detected by the work implement sensor, calculates a yaw angle error of the vehicle body sensor with respect to the vehicle body based on a difference between the roll angle of the vehicle body and the roll angle of the work implement and the pitch angle of the vehicle body, and calibrates the vehicle body sensor using the yaw angle error of the vehicle body sensor. A system.

2. The work implement is swingably supported about a lift axis extending in a left-right direction of the vehicle body, The system according to claim 1.

3. The controller calculates a yaw angle error of the work implement sensor with respect to the work implement such that a change in the roll angle of the work implement becomes zero while swinging the work implement about the lift axis, and calibrates the work implement sensor using the yaw angle error of the work implement sensor. The system according to claim 2.

4. A method executed by a controller for controlling a work machine, wherein the work machine includes a vehicle body, a work implement supported by the vehicle body, a vehicle body sensor attached to the vehicle body for detecting a roll angle, a pitch angle, and a yaw angle of the vehicle body, and a work implement sensor attached to the work implement for detecting a roll angle of the work implement, and the method includes acquiring the roll angle and the pitch angle of the vehicle body detected by the vehicle body sensor, acquiring the roll angle of the work implement detected by the work implement sensor, calculating a yaw angle error of the vehicle body sensor with respect to the vehicle body based on a difference between the roll angle of the vehicle body and the roll angle of the work implement and the pitch angle of the vehicle body, and calibrating the vehicle body sensor using the yaw angle error of the vehicle body sensor. A method comprising the above steps.

5. The work implement is swingably supported about a lift axis extending in a left-right direction of the vehicle body, The method according to claim 4.

6. ​ ​ ​ Calculating a yaw angle error of the work implement sensor with respect to the work implement while swinging the work implement around the lift axis so that a change in the roll angle of the work implement becomes zero; Calibrating the work implement sensor using the yaw angle error of the work implement sensor; The method according to claim 5, further comprising.

Images