Patent Application
20240295096
The present invention relates to a system and a method for controlling a work machine.
Some work machines are equipped with a work implement such as a blade. For example, in the work machine disclosed in Japanese Patent Application Publication No. 2014-31696, the blade and the vehicle body are connected by left and right lift frames. Left and right pitch/tilt cylinders are connected to the left and right lift frames, respectively. The blade performs a tilt motion when the left and right pitch/tilt cylinders expand and contract, respectively. The tilt motion is a motion in which the blade tilts from side to side so that the height of one of the left and right ends of the blade is different from the height of the other.
In recent years, in order to control the work implement, the position of a predetermined portion of the work implement is detected by a controller of the work machine. Therefore, the work machine is equipped with a work implement sensor that detects the posture of the work implement and a frame sensor that detects the posture of the frame. Each sensor is, for example, an acceleration sensor, and detects a roll angle and a pitch angle from the gravitational acceleration. If the work implement's yaw angle relative to the vehicle body is zero degrees, the controller can calculate the position of the predetermined portion of the work implement with respect to the vehicle body based on the length, the roll angle, and the pitch angle of the frame, the roll angle and the pitch angle of the work implement, and the position of the predetermined portion in the work implement.
However, when the blade tilts, one of the left and right frames swings, so that the heights of the left and right frames become different from each other. In that case, the positions of the front ends of the left and right frames are shifted from each other in the front-back direction. Therefore, the yaw angle of the work implement with respect to the vehicle body has a value different from zero degrees. In that case, with the method described above, it is difficult to accurately detect the position of the work implement with respect to the vehicle body.
On the other hand, according to the above-mentioned acceleration sensor, the yaw angle of the work implement can be calculated by integrating the angular velocities, assuming that the yaw angle at the starting time is 0. However, the accuracy is not high, and the error becomes large when used for a long time. Therefore, even when the yaw angle is detected using an acceleration sensor, it is not easy to accurately detect the position of the work implement with respect to the vehicle body. An object of the present invention is to accurately detect the position of a predetermined portion of a work implement in a work machine even if the yaw angle of the work implement changes due to the movement of the work implement.
A system according to one aspect of the present invention is a system for controlling a work machine. The work machine includes a vehicle body, a work implement frame, a work implement, a first actuator, and a second actuator. The work implement frame includes a first frame and a second frame. The first frame includes a first body connecting portion connected to the vehicle body. The second frame includes a second body connecting portion connected to the vehicle body. The second frame is disposed apart from the first frame in the left-right direction.
The work implement includes a first frame connecting portion, a second frame connecting portion, and a predetermined portion. The first frame connecting portion is connected to the first frame. The second frame connecting portion is disposed apart from the first frame connecting portion in the left-right direction. The second frame connecting portion is connected to the second frame. The first actuator moves the first frame relative to the vehicle body. The second actuator moves the second frame relative to the vehicle body. The yaw angle of the work implement with respect to the vehicle body when the work implement is in a first posture is different from the yaw angle of the work implement when the work implement is in a second posture. The second posture is different from the first posture.
The system includes a work implement sensor and a controller. The work implement sensor is attached to the work implement. The work implement sensor detects the roll angle and the pitch angle of the work implement. The controller acquires an actual frame length indicating the distance between the first body connecting portion and the first frame connecting portion. The controller acquires the position of the first body connecting portion. The controller acquires the roll angle and the pitch angle of the work implement.
When the work implement is in the first posture, the controller calculates an assumed position of the first frame connecting portion when the yaw angle is assumed to be a predetermined angle based on the roll angle and the pitch angle of the work implement. The controller calculates an assumed frame length indicating the distance between the assumed position of the first frame connecting portion and the first body connecting portion. The controller calculates the yaw angle of the work implement in the first posture based on the difference between the actual frame length and the assumed frame length. The controller calculates the position of the predetermined portion of the work implement based on the roll angle and the pitch angle of the work implement and the yaw angle of the work implement in the first posture.
A method according to another aspect of the present invention is a method for controlling a work machine. The work machine includes a vehicle body, a work implement frame, a work implement, a first actuator, and a second actuator. The work implement frame includes a first frame and a second frame. The first frame includes a first body connecting portion connected to the vehicle body. The second frame includes a second body connecting portion connected to the vehicle body. The second frame is disposed apart from the first frame in the left-right direction.
The work implement includes a first frame connecting portion, a second frame connecting portion, and a predetermined portion. The first frame connecting portion is connected to the first frame. The second frame connecting portion is disposed apart from the first frame connecting portion in the left-right direction. The second frame connecting portion is connected to the second frame. The yaw angle of the work implement with respect to the vehicle body when the work implement is in a first posture is different from the yaw angle of the work implement when the work implement is in a second posture. The second posture is different from the first posture.
The method includes detecting a position of the first body connecting portion: detecting the roll angle and the pitch angle of the work implement: acquiring an actual frame length indicating a distance between the first body connecting portion and the first frame connecting portion: calculating an assumed position of the first frame connecting portion when the yaw angle is assumed to be a predetermined angle based on the roll angle and the pitch angle of the work implement when the work implement is in the first posture: calculating an assumed frame length indicating the distance between the assumed position of the first frame connecting portion and the first body connecting portion: calculating the yaw angle of the work implement in the first posture based on the difference between the actual frame length and the assumed frame length; and calculating the position of the predetermined position of the work implement based on the roll angle and the pitch angle of the work implement, and the yaw angle of the work implement in the first posture.
According to the present invention, the yaw angle when the work implement is in the first posture is calculated from the difference between the assumed frame length and the actual frame length when the yaw angle of the work implement is assumed to be a predetermined angle. As a result, even if the yaw angle of work implement changes due to the movement of the work implement, the position of work implement can be accurately detected on the work machine.
Hereinafter, a work machine according to an embodiment will be described with reference to the drawings.
The vehicle body 2 includes a cab 5, a power compartment 6, and a traveling device 7. A driver's seat (not shown) is disposed in the cab 5. The power compartment 6 is disposed in front of the cab 5. The traveling device 7 supports the vehicle body 2. The traveling device 7 includes left and right crawler tracks 8. Note that in
The work implement 3 is disposed in front of the vehicle body 2. In this embodiment, the work implement 3 is a blade. The work implement 3 extends in the left-right direction of the work machine 1. The work implement 3 includes a cutting edge 11. The drive mechanism 4 for the work implement 3 includes a work implement frame 12 and a plurality of actuators 13 to 16.
The first frame 17 is swingably connected to the vehicle body 2. The first frame 17 includes a first body connecting portion 23. The first frame 17 is connected to the vehicle body 2 at the first body connecting portion 23. The second frame 18 is disposed apart from the first frame 17 in the left-right direction. The second frame 18 is swingably connected to the vehicle body 2. The second frame 18 includes a second body connecting portion 24. The second frame 18 is connected to the vehicle body 2 at the second body connecting portion 24.
The first frame 17 and the second frame 18 swing at least about the lift axis A1 with respect to the vehicle body 2. The lift axis A1 extends in the left-right direction of the work machine 1. Specifically, the first frame 17 and the second frame 18 are connected to the vehicle body 2 via a ball joint 19, and are swingable in all directions relative to the vehicle body 2. As shown in
As shown in
The plurality of actuators 13 to 16 include a first lift actuator 13, a second lift actuator 14, a first pitch/tilt actuator 15, and a second pitch/tilt actuator 16. The first lift actuator 13 and the second lift actuator 14 are disposed apart from each other in the left-right direction of the work machine 1. The first lift actuator 13 and the second lift actuator 14 are connected to the vehicle body 2 and the work implement 3. The first lift actuator 13 and the second lift actuator 14 are hydraulic cylinders. The first lift actuator 13 and the second lift actuator 14 swing the work implement frame 12 up and down about the lift axis A1. Thereby, the work implement 3 is lifted up and down.
The first pitch/tilt actuator 15 and the second pitch/tilt actuator 16 are disposed apart from each other in the left-right direction of the work machine 1. The first pitch/tilt actuator 15 is connected to the work implement 3 and the first frame 17. The second pitch/tilt actuator 16 is connected to the work implement 3 and the second frame 18. The first pitch/tilt actuator 15 and the second pitch/tilt actuator 16 are hydraulic cylinders.
The first pitch/tilt actuator 15 rotates the work implement 3 about the first axis A2 with respect to the first frame 17. The second pitch/tilt actuator 16 rotates the work implement 3 about the first axis A2 with respect to the second frame 18. By expanding and contracting both the first pitch/tilt actuator 15 and the second pitch/tilt actuator 16, the work implement 3 tilts forward or backward about the first axis A2. This tilting motion of the work implement 3 back and forth is called a pitch motion.
When only one of the first pitch/tilt actuator 15 and the second pitch/tilt actuator 16 expands or contracts, the work implement 3 tilts to the left or right. For example, by expanding and contracting only the first pitch/tilt actuator 15, the right end of the work implement 3 moves up and down. By expanding and contracting only the second pitch/tilt actuator 16, the left end of the work implement 3 moves up and down. As a result, the work implement 3 is tilted so that the heights of the left end and the right end of the work implement 3 are different from each other. This left and right tilting motion of the work implement 3 is called a tilt motion.
The hydraulic pump 31 is driven by the power source 30 and discharges hydraulic fluid. The hydraulic fluid discharged from the hydraulic pump 31 is supplied to the lift actuators 13 and 14 and the pitch/tilt actuators 15 and 16. Note that although one hydraulic pump 31 is illustrated in
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 multiple speed change gears.
The work machine 1 includes a controller 33 and a control valve 34. The controller 33 is programmed to control the work 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, a hard disk, or the like. The storage device 35 is an example of a non-transitory computer-readable recording medium. The storage device 35 records computer instructions executable by processor 36 to control the work machine 1.
The control valve 34 is controlled by a command signal from the controller 33. The control valve 34 is disposed between the actuators 13 to 16 and the hydraulic pump 31. The control valve 34 controls the flow rate of hydraulic fluid supplied from the hydraulic pump 31 to the lift actuators 13 and 14. The control valve 34 controls the flow rate of hydraulic fluid supplied from the hydraulic pump 31 to the pitch/tilt actuators 15 and 16.
The work 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. An operator can use the operating device 37 to manually control the running of the work machine 1 and the operation of the work implement 3. For example, the operating device 37 can operate a lift motion, a pitch motion, and a tilt motion of the work 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.
The input device 38 includes, for example, a touch screen. However, input device 38 may include other devices such as a switch. The operator can use the operating device 37 to set the control of the work machine 1. The input device 38 outputs an input signal indicating an input to input device 38. The controller 33 receives the input signal from input device 38.
The work machine 1 includes a vehicle body sensor 41, a frame sensor 42, and a work implement sensor 43. The vehicle body sensor 41 is attached to the vehicle body 2. The vehicle body sensor 41 detects the posture of the vehicle body 2. The frame sensor 42 is attached to the work implement frame 12. The frame sensor 42 detects the posture of the work implement frame 12. The work implement sensor 43 is attached to the work implement 3. The work implement sensor 43 detects the posture of the work implement 3.
The vehicle body sensor 41, the frame sensor 42, and the work implement sensor 43 are, for example, acceleration sensors such as IMUs (Inertial Measurement Units). However, the vehicle body sensor 41, the frame sensor 42, and the work implement sensor 43 are not limited to the IMU, and may be other acceleration sensors.
The vehicle body sensor 41 detects the pitch angle, the roll angle, and the yaw angle of the vehicle body 2. The frame sensor 42 detects the pitch angle, the roll angle, and the yaw angle of the work implement frame 12. Specifically, the frame sensor 42 is attached to the second frame 18. The frame sensor 42 detects the pitch angle, the roll angle, and the yaw angle of the second frame 18. The work implement sensor 43 detects the pitch angle, the roll angle, and the yaw angle of the work implement 3.
Each sensor detects the pitch angle and the roll angle based on gravitational acceleration. Furthermore, each sensor detects the yaw angle by integrating the angular velocity, with the yaw angle at the starting time is 0. The vehicle body sensor 41, the frame sensor 42, and the work implement sensor 43 each output a detection signal indicating the detected angle.
The controller 33 detects the position of a predetermined portion of the work implement 3 with respect to the vehicle body 2 based on the angles detected by the above-mentioned sensors 41 to 43 and a shape data of the work machine 1. The shape data of the work machine 1 is stored in the controller 33 and indicates the positional relationship of each part of the work machine 1. Hereinafter, a method for detecting the position of a predetermined portion of the work implement 3 will be explained.
The first end 51 and the second end 52 are the left and right ends of the cutting edge 11. As shown in
The shape data includes the position of the first body connecting portion 23 and the position of the second body connecting portion 24 in the vehicle body 2. For example, the position of the first body connecting portion 23 and the position of the second body connecting portion 24 are indicated by coordinates in a vehicle body coordinate system with the vehicle body 2 as a reference. The shape data includes a first actual frame length and a second actual frame length. The first actual frame length indicates the distance between the first body connecting portion 23 and the first frame connecting portion 21. The second actual frame length indicates the distance between the second body connecting portion 24 and the second frame connecting portion 22.
The shape data includes work implement data. The work implement data indicates the positional relationship between the second frame connecting portion 22 and a predetermined portion of the work implement 3. The predetermined portion is located, for example, on the cutting edge 11 of the work implement 3. In this embodiment, the predetermined portion includes the first end 51 and the second end 52 of the work implement 3.
When the work implement 3 is in the standard posture, the controller 33 calculates the positions of the first end 51 and the second end 52 of the work implement 3 as follows. The controller 33 calculates the position of the second frame connecting portion 22 from the position of the second body connecting portion 24 based on the pitch angle, the roll angle, and the yaw angle of the second frame 18 and the second actual frame length. The controller 33 calculates the position of the first end 51 of the work implement 3 from the position of the second frame connecting portion 22 based on the pitch angle, the roll angle, and the yaw angle of the work implement 3, and the work implement data. Here, the yaw angle of the work implement 3 with respect to the vehicle body 2 is zero degrees. Further, the controller 33 determines the position of the second end 52 of the work implement 3 from the position of the second frame connecting portion 22 based on the pitch angle, the roll angle, and the yaw angle of the work implement 3, and the work implement data.
Next, a method for calculating the position of the predetermined portion of the work implement 3 when the work implement 3 is in the tilted posture will be described. As shown in
P21′=Rx(δ)Ry(θ)Rz(0)P21s (1)
P21′ is the coordinate of the assumed position 21′ of the first frame connecting portion 21. P21s is the coordinate of the first frame connecting portion 21 in the standard posture. δ and θ are the amounts of change in the roll angle and the pitch angle, respectively, from the standard posture. Rx(δ), Ry(θ), and Rz(0) are rotation matrices of the roll angle, the pitch angle, and the yaw angle of the work implement 3, respectively.
In step S102, the controller 33 calculates an assumed frame length L1′. The assumed frame length L1′ indicates the distance between the assumed position 21′ of the first frame connecting portion 21 and the first body connecting portion 23. The controller 33 calculates the assumed frame length L1′ from the coordinates of the assumed position 21′ of the first frame connecting portion 21 and the coordinates of the position of the first body connecting portion 23.
In step S103, the controller 33 calculates the yaw angle φ of the work implement 3 in the tilted posture. The controller 33 calculates the yaw angle φ of the work implement 3 in the tilted posture based on the difference between the first actual frame length L1 and the assumed frame length L1′. The controller 33 calculates the yaw angle φ of the work implement 3 in the tilted posture using the following equation (2).
φ=arcsin{(L1−L1′)/W} (2)
W is the width of the work implement 3.
In step S104, the controller 33 calculates the positions of the predetermined portions of the work implement 3. In this embodiment, the predetermined portions are the first end 51 and the second end 52 of the work implement 3. When the work implement 3 is in the tilted posture, the controller 33 calculates the positions of the first end 51 and the second end 52 of the work implement 3 based on the roll angle δ and the pitch angle θ of the work implement 3, and the yaw angle φ in the tilted posture. For example, the controller 33 calculates the position of the second end 52 of the work implement 3 using the following equation (3).
P52t=Rx(δ)Ry(θ)Rz(φ)P52s (3)
P52t is the position of the second end 52 of the work implement 3 in the tilted posture. P52s is the position of the second end 52 of the work implement 3 in the standard posture. Note that, in the same manner as described above, the position of the first end 51 of the work implement 3 in the tilted posture may be calculated from the position of the first end 51 of the work implement 3 in the standard posture.
In the control system for the work machine 1 according to the present embodiment described above, the difference between the assumed frame length L1′ of the first frame 17 and the actual frame length L1 when the yaw angle of the work implement 3 is assumed to be zero degrees. From this, the yaw angle φ when the work implement 3 is in the tilted posture is calculated. Thereby, even if the yaw angle of the work implement 3 changes due to the tilt motion, the position of the predetermined portion of the work implement 3 in the work machine 1 can be detected with high accuracy.
Note that the controller 33 may control the work implement 3 to move based on the position of the predetermined portion of the work implement 3 detected as described above. For example, as shown in
Although one embodiment of the present invention has been described above, the present invention is not limited to the above embodiment, and various changes can be made without departing from the gist of the invention.
The work machine 1 is not limited to a bulldozer, but may be another vehicle such as a wheel loader or a motor grader. The controller 33 may include a plurality of controllers that are separate from each other. The work machine 1 may be operable remotely. In that case, the controller 33, the operating device 37, and the input device 38 may be disposed outside the work machine 1. The controller 33 may control the work machine 1 by wireless communication with the work machine 1.
The processing by the controller 33 is not limited to that of the above embodiment, and may be modified. A part of the processing by the controller 33 may be omitted. Alternatively, part of the process described above may be changed.
For example, in the embodiment described above, the frame sensor 42 is attached to the second frame 18. However, the frame sensor 42 may be attached to the first frame 17. Alternatively, the frame sensor 42 may be attached to each of the first frame 17 and the second frame 18. In that case, the controller 33 may calculate the position of the predetermined portion of the work implement 3 from the position of the first frame connecting portion 21.
In the above embodiment, the yaw angle when the work implement 3 is in the tilted posture is calculated from the difference between the actual frame length L1 and the assumed frame length L1′ of the first frame 17 when the yaw angle is assumed to be zero degrees. However, the yaw angle when the work implement 3 is in the tilted posture may be calculated from the difference between the actual frame length and the assumed frame length of the second frame 18 when the yaw angle is assumed to be zero degrees.
The predetermined angle may be an angle other than zero degrees. The posture of the work implement 3 used for calculating the yaw angle is not limited to the standard posture and the tilted posture described above, but may be any two or more postures with mutually different yaw angles.
The predetermined portion is not limited to the first end 51 and the second end 52 of the cutting edge 11 of the work implement 3, but may be another portion. For example, the predetermined portion may be the center of the cutting edge 11.
According to the present invention, even if the yaw angle of the work implement changes due to the tilt motion, the position of the predetermined portion of the work implement can be detected with high accuracy in the work machine.
| Number | Date | Country | Kind |
|---|---|---|---|
| 2021-161624 | Sep 2021 | JP | national |
This application is a U.S. National stage application of International Application No. PCT/JP2022/028962, filed on Jul. 27, 2022. This U.S. National stage application claims priority under 35 U.S.C. § 119(a) to Japanese Patent Application No. 2021-161624, filed in Japan on Sep. 30, 2021, the entire contents of which are hereby incorporated herein by reference.
| Filing Document | Filing Date | Country | Kind |
|---|---|---|---|
| PCT/JP2022/028962 | 7/27/2022 | WO |
