System and method for controlling a work machine

Abstract

The system includes a work implement sensor and a controller. The work implement sensor detects a roll angle and a pitch angle of a work implement. The controller acquires an actual boom length indicating a distance between a first vehicle body connection portion and a first boom connection portion. The controller calculates, when the work implement is in a first posture, a virtual position of the first boom connection portion assuming a yaw angle to be a prescribed angle, based on the roll angle and the pitch angle of the work implement. The controller calculates a virtual boom length indicating a distance between the virtual position of the first boom connection portion and the first vehicle body connection portion. The controller calculates the yaw angle of the work implement in the first posture based on a difference between the actual boom length and the virtual boom length. The controller calculates a position of a prescribed portion of the work implement based on the roll angle, the pitch angle, and the yaw angle of the work implement in the first posture.

Description

Technical Field

[0001] This invention relates to systems and methods for controlling machinery. Background Technology

[0002] The work machinery includes bulldozer blades and other workpieces. For example, in the work machinery described in Patent Document 1, the bulldozer blade is connected to the vehicle body via left and right lifting frames. Left and right pitch and tilt hydraulic cylinders are connected to the left and right lifting frames respectively. By extending and retracting the left and right pitch and tilt hydraulic cylinders respectively, the bulldozer blade tilts. The tilting action is achieved by making one end of the bulldozer blade at a different height than the other.

[0003] Prior art literature

[0004] Patent documents

[0005] Patent Document 1: Japanese Patent Application Publication No. 2014-31696 Summary of the Invention

[0006] The problem that the invention will solve

[0007] In recent years, to control work machines, the position of designated parts of the work machine is detected using the machine's controller. Therefore, work machines are equipped with work machine sensors to detect the work machine's attitude and frame sensors to detect the frame's attitude. These sensors, for example, are accelerometers that detect roll angle and pitch angle based on gravitational acceleration. When the work machine's yaw angle relative to the vehicle body is zero degrees, the controller can calculate the position of the designated parts of the work machine relative to the vehicle body based on the frame length, roll angle, pitch angle, the work machine's roll and pitch angles, and the position of the designated parts within the work machine.

[0008] However, when the bulldozer blade tilts, the heights of the left and right frames differ due to the swinging of one side. In this situation, the front ends of the left and right frames shift relative to each other in the longitudinal direction. Therefore, the yaw angle of the work machine relative to the vehicle body becomes a value different from zero degrees. In this case, it is difficult to accurately detect the position of the work machine relative to the vehicle body using the method described above.

[0009] On the other hand, based on the aforementioned accelerometer, the yaw angle of the work machine can be calculated by accumulating the angular velocity, which is zero at startup. However, its accuracy is not high, and the error increases with prolonged use. Therefore, even when detecting the yaw angle, it is not easy to accurately detect the position of the work machine relative to the vehicle body using an accelerometer. The object of the present invention is to accurately detect the position of a specified part of the work machine even if the yaw angle of the work machine changes due to its movement.

[0010] Methods for solving problems

[0011] One aspect of the present invention is a system for controlling a work machine. The work machine includes a vehicle body, a work frame, a work machine, a first actuator, and a second actuator. The work frame includes a first frame and a second frame. The first frame includes a first vehicle body connection portion connected to the vehicle body. The second frame includes a second vehicle body connection portion connected to the vehicle body. The second frame is detached from the first frame in a left-right direction.

[0012] The work machine includes a first connecting part, a second connecting part, and designated sections. The first connecting part is connected to the first frame. The second connecting part is separated from the first connecting part and arranged to the left and right. The second connecting part is connected to the second frame. A first actuator causes the first frame to move relative to the vehicle body. A second actuator causes the second frame to move relative to the vehicle body. The yaw angle of the work machine relative to the vehicle body in the first posture is different from the yaw angle of the work machine in the second posture. The second posture is different from the first posture.

[0013] The system includes a machine sensor and a controller. The machine sensor is installed on the machine. The machine sensor detects the roll angle and pitch angle of the machine. The controller obtains the actual frame length, representing the distance between the first car body connection and the first frame connection. The controller obtains the position of the first car body connection. The controller obtains the roll angle and pitch angle of the machine.

[0014] When the machine is in its first attitude, the controller calculates the intended position of the first frame connector when the intended yaw angle is a specified angle, based on the machine's roll and pitch angles. The controller calculates the intended frame length, representing the distance between the intended position of the first frame connector and the first vehicle body connector. Based on the difference between the actual frame length and the intended frame length, the controller calculates the yaw angle of the machine in the first attitude. Based on the machine's roll, pitch, and yaw angles in the first attitude, the controller calculates the position of a specified portion of the machine.

[0015] Another aspect of the present invention is a method for controlling a work machine. The work machine includes a vehicle body, a work frame, a work machine, a first actuator, and a second actuator. The work frame includes a first frame and a second frame. The first frame includes a first vehicle body connection portion connected to the vehicle body. The second frame includes a second vehicle body connection portion connected to the vehicle body. The second frame is detached from the first frame in a left-right direction.

[0016] The work machine includes a first connecting part, a second connecting part, and specified sections. The first connecting part is connected to the first frame. The second connecting part is separated from the first connecting part and arranged to the left and right. The second connecting part is connected to the second frame. The yaw angle of the work machine relative to the vehicle body in the first posture is different from the yaw angle of the work machine in the second posture. The second posture is different from the first posture.

[0017] The method includes: detecting the position of the first vehicle body connecting part; detecting the roll angle and pitch angle of the working machine; obtaining the actual frame length representing the distance between the first vehicle body connecting part and the first frame connecting part; when the working machine is in a first attitude, calculating the intended position of the first frame connecting part when the intended yaw angle is a specified angle based on the roll angle and pitch angle of the working machine; calculating the intended frame length representing the distance between the intended position of the first frame connecting part and the first vehicle body connecting part; calculating the yaw angle of the working machine in the first attitude based on the difference between the actual frame length and the intended frame length; and calculating the position of a specified part of the working machine based on the roll angle, pitch angle, and yaw angle of the working machine in the first attitude.

[0018] Invention Effects

[0019] According to the present invention, the yaw angle of the working machine in its first posture is calculated based on the difference between the imagined frame length and the actual frame length when the imagined yaw angle of the working machine is a specified angle. Therefore, even if the yaw angle of the working machine changes due to its movement, the position of the working machine in the working machinery can be detected with high precision. Attached Figure Description

[0020] Figure 1 It is a 3D diagram of the operating machinery.

[0021] Figure 2 It is a three-dimensional diagram of the machine and its surrounding structure.

[0022] Figure 3 It is a block diagram representing the control system of the operating machinery.

[0023] Figure 4A It is a schematic top view showing the machine and its frame in a standard orientation.

[0024] Figure 4B It is a schematic side view of the machine and its frame in a standard orientation.

[0025] Figure 4C This is a schematic rear view showing the machine and its frame in a standard orientation.

[0026] Figure 5A It is a schematic top view showing the machine and its frame in an inclined position.

[0027] Figure 5B It is a schematic side view of the machine and its frame in an inclined position.

[0028] Figure 5C This is a schematic rear view of the machine and its frame in an inclined position.

[0029] Figure 6It is a flowchart showing the process of calculating the position of a specified part of the work machine.

[0030] Figure 7 This is a diagram illustrating an example of the control of a machine. Detailed Implementation

[0031] The following describes the operating machinery for the implementation method, with reference to the attached document. Figure 1 The explanation will be provided later. Figure 1 This is a perspective view of the work machine 1 according to the embodiment. The work machine 1 in this embodiment is an excavator. The work machine 1 has a body 2, a workpiece 3, and a drive mechanism 4 for the workpiece 3.

[0032] The vehicle body 2 includes a driver's cab 5, a power compartment 6, and a running gear 7. A driver's seat (not shown) is located in the driver's cab 5. The power compartment 6 is located in front of the driver's cab 5. The running gear 7 supports the vehicle body 2. The running gear 7 includes left and right tracks 8. It should be noted that... Figure 1 In the diagram, only the track 8 on the left is shown. The working machine 1 moves by rotating the track 8.

[0033] The work machine 3 is positioned at the front of the vehicle body 2. In this embodiment, the work machine 3 is a bulldozer blade. The work machine 3 extends along the left-right direction of the work machinery 1. The work machine 3 includes a blade tip 11. The drive mechanism 4 of the work machine 3 includes a work frame 12 and multiple actuators 13-16. Figure 2 This is a 3D view of the work machine 3 and the drive mechanism 4. (See diagram below.) Figure 2 As shown, the work frame 12 supports the work machine 3. The work frame 12 includes a first frame 17 and a second frame 18. The first frame 17 and the second frame 18 extend along the front-rear direction of the work machine 1.

[0034] The first frame 17 is pivotally connected to the vehicle body 2. The first frame 17 includes a first vehicle body connecting portion 23. The first frame 17 is connected to the vehicle body 2 in the first vehicle body connecting portion 23. The second frame 18 is separately arranged from the first frame 17 in a left-right direction. The second frame 18 is pivotally connected to the vehicle body 2. The second frame 18 includes a second vehicle body connecting portion 24. The second frame 18 is connected to the vehicle body 2 in the second vehicle body connecting portion 24.

[0035] The first frame 17 and the second frame 18 are oscillating relative to the vehicle body 2, at least around the lifting shaft A1. The lifting shaft A1 extends along the left-right direction of the working 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 capable of oscillating in all directions relative to the vehicle body 2. Figure 1 As shown, the first unit 17 and the second unit 18 are arranged on the outside of the traveling device 7 in the left-right direction. The first unit 17 and the second unit 18 are connected to the side of the traveling device 7.

[0036] like Figure 2 As shown, the work machine 3 includes a first frame connecting part 21 and a second frame connecting part 22. The first frame connecting part 21 and the second frame connecting part 22 are disposed on the back of the work machine 3. The first frame connecting part 21 is connected to the first frame 17. The second frame connecting part 22 is disposed separately from the first frame connecting part 21 in the left-right direction. The second frame connecting part 22 is connected to the second frame 18. The work machine 3 is rotatably supported on the first frame 17 and the second frame 18 about the first axis A2 and the second axis A3. The first axis A2 extends along the left-right direction of the work machine 1. The second axis A3 extends along the up-down direction of the work machine 1.

[0037] Multiple actuators 13-16 include a first lifting actuator 13, a second lifting actuator 14, a first pitch / tilt actuator 15, and a second pitch / tilt actuator 16. The first lifting actuator 13 and the second lifting actuator 14 are separately arranged in the left-right direction of the working machine 1. The first lifting actuator 13 and the second lifting actuator 14 are connected to the vehicle body 2 and the working machine 3. The first lifting actuator 13 and the second lifting actuator 14 are hydraulic cylinders. The first lifting actuator 13 and the second lifting actuator 14 cause the working machine frame 12 to swing up and down around the lifting shaft A1. This causes the working machine 3 to lift up and down.

[0038] The first pitch and tilt actuator 15 and the second pitch and tilt actuator 16 are configured separately in the left and right directions of the working machine 1. The first pitch and tilt actuator 15 is connected to the working machine 3 and the first frame 17. The second pitch and tilt actuator 16 is connected to the working machine 3 and the second frame 18. The first pitch and tilt actuator 15 and the second pitch and tilt actuator 16 are hydraulic cylinders.

[0039] The first pitch and tilt actuator 15, relative to the first frame 17, causes the working machine 3 to rotate about the first axis A2. The second pitch and tilt actuator 16, relative to the second frame 18, causes the working machine 3 to rotate about the first axis A2. The first pitch and tilt actuator 15 and the second pitch and tilt actuator 16 extend and retract together, thereby causing the working machine 3 to tilt forward or backward about the first axis A2. This forward and backward tilting motion of the working machine 3 is called the pitch motion.

[0040] When only one of the first pitch / tilt actuator 15 and the second pitch / tilt actuator 16 extends or retracts, the work machine 3 tilts to the left or right. For example, by extending or retracting only the first pitch / tilt actuator 15, the right end of the work machine 3 moves up and down. By extending or retracting only the second pitch / tilt actuator 16, the left end of the work machine 3 moves up and down. Thus, the work machine 3 tilts in such a way that the left and right ends of the work machine 3 are at different heights. This left-right tilting motion of the work machine 3 is called a tilting motion.

[0041] Figure 3This is a block diagram showing the configuration of the control system of the operating machine 1. For example... Figure 3 As shown, 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 can also be an electric motor. Alternatively, the power source 30 can be a hybrid power source of an internal combustion engine and an electric motor.

[0042] The hydraulic pump 31 is driven by the power source 30 to discharge working oil. The working oil discharged from the hydraulic pump 31 is supplied to the lifting actuators 13 and 14, and the pitch and tilt actuators 15 and 16. It should be noted that in Figure 3 The diagram shows a hydraulic pump 31, but multiple hydraulic pumps can also be provided.

[0043] The power transmission device 32 transmits the driving force of the power source 30 to the driving 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 with multiple gears.

[0044] The machine tool 1 has a controller 33 and a control valve 34. The controller 33 is programmed to control the machine tool 1 based on 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 or an auxiliary storage device. The storage device 35 can also be, for example, RAM or ROM. The storage device 35 can also 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 that can be executed by the processor 36 for controlling the machine tool 1.

[0045] Control valve 34 is controlled by a command signal from controller 33. Control valve 34 is positioned between actuators 13-16 and hydraulic pump 31. Control valve 34 controls the flow rate of working oil supplied from hydraulic pump 31 to lifting actuators 13 and 14. Control valve 34 also controls the flow rate of working oil supplied from hydraulic pump 31 to pitch and tilt actuators 15 and 16.

[0046] The operating machine 1 has an operating device 37 and an input device 38. The operating device 37 may include, for example, a lever. Alternatively, the operating device 37 may also include a pedal or a switch. The operator can use the operating device 37 to manually operate the movement of the operating machine 1 and the operation of the machine 3. For example, the operating device 37 can perform lifting, pitching, and tilting movements of the machine 3. The operating device 37 outputs an operating signal indicating the operation of the operating device 37. The controller 33 receives the operating signal from the operating device 37.

[0047] Input device 38 may include, for example, a touch panel. However, input device 38 may also include other devices such as switches. The operator can use operating device 37 to set the control parameters for the machine tool 1. Input device 38 outputs input signals indicating input to it. Controller 33 receives input signals from input device 38.

[0048] The working machine 1 includes a vehicle body sensor 41, a frame sensor 42, and a working machine sensor 43. The vehicle body sensor 41 is mounted on the vehicle body 2. The vehicle body sensor 41 detects the attitude of the vehicle body 2. The frame sensor 42 is mounted on the working machine frame 12. The frame sensor 42 detects the attitude of the working machine frame 12. The working machine sensor 43 is mounted on the working machine 3. The working machine sensor 43 detects the attitude of the working machine 3.

[0049] The vehicle body sensor 41, frame sensor 42, and machine sensor 43 are, for example, acceleration sensors such as IMUs (Inertial Measurement Units). However, the vehicle body sensor 41, frame sensor 42, and machine sensor 43 are not limited to IMUs and can also be other acceleration sensors.

[0050] Vehicle body sensor 41 detects the pitch angle, roll angle, and yaw angle of vehicle body 2. Frame sensor 42 detects the pitch angle, roll angle, and yaw angle of work machine frame 12. Specifically, frame sensor 42 is installed on the second frame 18. Frame sensor 42 detects the pitch angle, roll angle, and yaw angle of the second frame 18. Work machine sensor 43 detects the pitch angle, roll angle, and yaw angle of work machine 3.

[0051] Each sensor detects pitch and roll angles using gravitational acceleration. Additionally, each sensor starts at 0° and detects yaw angle by accumulating angular velocities. The vehicle body sensor 41, frame sensor 42, and work machine sensor 43 each output detection signals representing the detected angles.

[0052] Based on the angles detected by the sensors 41-43 and the shape data of the working machine 1, the controller 33 detects the position of a specified portion of the working machine 3 relative to the vehicle body 2. The shape data of the working machine 1 is stored in the controller 33, representing the positional relationship of each part of the working machine 1. The method for detecting the position of the specified portion of the working machine 3 will be described below.

[0053] from Figures 4A to 4C This is a schematic diagram showing the work machine 3 and the work machine frame 12 in their standard posture. Figure 4A It is a top view. Figure 4B It is a side view. Figure 4C This is the rear view. (For example...) Figure 4CAs shown, in the standard posture, the cutting edge 11 of the work machine 3 is horizontal, and the first end 51 and the second end 52 of the cutting edge 11 are at the same height. Additionally, the first frame 17 and the second frame 18 are at the same height. The forward and backward direction of the work machine 3 is consistent with the forward and backward direction of the vehicle body 2, and the yaw angle of the work machine 3 relative to the vehicle body 2 is zero degrees.

[0054] The first end 51 and the second end 52 are the left and right ends of the blade tip 11. For example... Figure 4A As shown, the first end 51 is the right end of the blade tip 11, and the second end 52 is the left end of the blade tip 11. However, the first end 51 and the second end 52 can also be set to be opposite in left and right.

[0055] The shape data includes the positions of the first vehicle body connecting part 23 and the second vehicle body connecting part 24 in the vehicle body 2. For example, the positions of the first vehicle body connecting part 23 and the second vehicle body connecting part 24 are represented by coordinates in a vehicle body coordinate system with the vehicle body 2 as the reference. The shape data includes a first actual frame length and a second actual frame length. The first actual frame length represents the distance between the first vehicle body connecting part 23 and the first frame connecting part 21. The second actual frame length represents the distance between the second vehicle body connecting part 24 and the second frame connecting part 22.

[0056] The shape data includes machine data. The machine data indicates the positional relationship between the second frame connecting part 22 and a designated portion of the machine 3. The designated portion is, for example, located on the blade tip 11 of the machine 3. In this embodiment, the designated portion includes the first end portion 51 and the second end portion 52 of the machine 3.

[0057] With the work machine 3 in its standard posture, the controller 33 calculates the positions of the first end 51 and the second end 52 of the work machine 3 as follows. The controller 33 calculates the position of the second frame connection 22 based on the position of the second vehicle body connection 24, the pitch angle, roll angle, yaw angle of the second frame 18, and the actual length of the second frame. The controller 33 calculates the position of the first end 51 of the work machine 3 based on the position of the second frame connection 22, the pitch angle, roll angle, yaw angle of the work machine 3, and the work machine data. Here, the yaw angle of the work machine 3 relative to the vehicle body 2 is zero degrees. Furthermore, the controller 33 calculates the position of the second end 52 of the work machine 3 based on the position of the second frame connection 22, the pitch angle, roll angle, yaw angle of the work machine 3, and the work machine data.

[0058] from Figures 5A to 5C This is a schematic diagram showing the work machine 3 and the work machine frame 12 in an inclined posture. Figure 5A This is a top view. Figure 5B This is a side view. Figure 5C This is the rear view. (Example) Figure 5CAs shown, in an inclined posture, the heights of the first end 51 and the second end 52 of the working machine 3 are different. The first frame 17 and the second frame 18 are located at different heights. In this case, utilizing the structure of the working machine 1, as... Figure 5A As shown, the yaw angle of the work machine 3 in an inclined attitude Unlike the yaw angle of the work machine 3 in its standard attitude, this is a value other than zero degrees. It should be noted that... Figure 5A In the middle, the dashed line 3' represents the yaw angle in a tilted attitude. Machine 3 operating at zero degrees Celsius.

[0059] Next, the method for calculating the position of a specified part of the work machine 3 in an inclined posture will be explained. For example... Figure 6 As shown, in step S101, the controller 33 calculates the intended position of the first vehicle body connection portion 23. For example... Figure 5A As shown, the controller 33 calculates the intended position 21' of the first connecting part 21 when the intended yaw angle φ is zero, based on the roll angle and pitch angle of the working machine 3. For example, the controller 33 calculates the intended position of the first connecting part 21 using the following formula (1).

[0060] P21'=Rx(δ)Ry(θ)Rz(0)P21s··· (1)

[0061] P21' is the coordinate of the intended position 21' of the first connecting part 21. P21s is the coordinate of the first connecting part 21 in the standard attitude. δ and θ are the changes in roll angle and pitch angle from the standard attitude, respectively. Rx(δ), Ry(θ), and Rz(0) are the rotation columns of the roll angle, pitch angle, and yaw angle of the working machine 3, respectively.

[0062] In step S102, the controller 33 calculates the design frame length L1'. The design frame length L1' represents the distance between the design position 21' of the first frame connecting part 21 and the first vehicle body connecting part 23. The controller 33 calculates the design frame length L1' based on the coordinates of the design position 21' of the first frame connecting part 21 and the coordinates of the position of the first vehicle body connecting part 23.

[0063] In step S103, the controller 33 calculates the yaw angle φ of the machine 3 in an inclined attitude. The controller 33 calculates the yaw angle φ of the machine 3 in an inclined attitude based on the difference between the first actual frame length L1 and the imagined frame length L1'. The controller 33 calculates the yaw angle φ of the machine 3 in an inclined attitude using the following formula (2).

[0064]

[0065] W represents the width of machine 3.

[0066] In step S104, the controller 33 calculates the position of a specified portion of the work machine 3. In this embodiment, the specified portion is the first end 51 and the second end 52 of the work machine 3. When the work machine 3 is in an inclined posture, the controller 33 calculates the position of the first end 51 and the second end 52 of the work machine 3 based on the roll angle δ and pitch angle θ of the work machine 3 in the inclined posture. For example, the controller 33 calculates the position of the second end 52 of the work machine 3 using the following formula (3).

[0067]

[0068] P52t represents the position of the second end 52 of the machine 3 in an inclined posture. P52s represents the position of the second end 52 of the machine 3 in a standard posture. It should be noted that, similarly, the position of the first end 51 of the machine 3 in an inclined posture can also be calculated based on the position of the first end 51 of the machine 3 in a standard posture.

[0069] In the control system of the working machine 1 of this embodiment described above, the yaw angle of the working machine 3 when it is in an inclined attitude is calculated based on the difference between the assumed frame length L1' of the first frame 17 when the yaw angle of the working machine 3 is assumed to be zero degrees and the actual frame length L1. Therefore, even if the yaw angle of the machine 3 changes due to tilting motion, the position of the specified part of the machine 3 in the machine 1 can be detected with high precision.

[0070] It should be noted that the controller 33 can also control the operation of the machine 3 based on the position of a specified part detected as described above. For example... Figure 7 As shown, the controller 33 can also acquire the target design terrain 60. The controller 33 can also control the movement of a predetermined portion of the working machine 3 according to the target design terrain 60. For example, the controller 33 can also acquire the target design terrain 60 via the input device 38. The controller 33 can also automatically generate the target design terrain 60.

[0071] The above describes one embodiment of the present invention, but the present invention is not limited to the above embodiment, and various modifications can be made without departing from the spirit of the invention.

[0072] The working machine 1 is not limited to an excavator, but can also be other vehicles such as a wheel loader or motorized grader. The controller 33 can also have multiple independent controllers. The working machine 1 can also be operated remotely. In this case, the controller 33, operating device 37, and input device 38 can also be configured externally to the working machine 1. The controller 33 can also control the working machine 1 via wireless communication.

[0073] The processing of controller 33 is not limited to the above-described embodiment and can be modified. A portion of the processing of controller 33 may be omitted. Alternatively, a portion of the above-described processing may be modified.

[0074] For example, in the above embodiment, the sensor 42 is mounted on the second frame 18. However, the sensor 42 may also be mounted on the first frame 17. Alternatively, the sensor 42 may be mounted on both the first frame 17 and the second frame 18. In this case, the controller 33 may also calculate the position of a specified part of the work machine 3 based on the position of the first frame connection 21.

[0075] In the above embodiment, the yaw angle of the working machine 3 when it is in an inclined attitude is calculated based on the difference between the assumed frame length L1' and the actual frame length L1 of the first frame 17 when the assumed yaw angle is zero degrees. However, the yaw angle of the working machine 3 when it is in an inclined attitude can also be calculated based on the difference between the assumed frame length and the actual frame length of the second frame 18 when the assumed yaw angle is zero degrees.

[0076] The specified angle can also be an angle other than zero degrees. The attitude of the working machine 3 used to calculate the yaw angle is not limited to the above standard attitude or tilt attitude, as long as there are two or more attitudes with different yaw angles.

[0077] The specified part is not limited to the first end 51 and the second end 52 of the blade tip 11 of the working machine 3, but can also be another part. For example, the specified part can also be the center of the blade tip 11.

[0078] Industrial availability

[0079] According to the present invention, even if the yaw angle of the machine changes due to tilting motion, the position of a specified part of the machine in the machine can be detected with high precision.

[0080] Explanation of reference numerals in the attached figures

[0081] 1: Working machine, 2: Vehicle body, 3: Working machine, 12: Working machine frame, 15: First pitch and tilt actuator, 16: Second pitch and tilt actuator, 17: First frame, 18: Second frame, 21: First frame connection, 21': Desired position of the first frame connection 21, 22: Second frame connection, 23: First vehicle body connection, 24: Second vehicle body connection, 33: Controller, 43: Working machine sensor, 51: First end, 52: Second end, L1: First actual frame length, L1': Desired frame length.

Claims

1. A system for controlling machinery, characterized in that, The operating machinery includes: Vehicle body; The work frame includes a first frame and a second frame. The first frame has a first vehicle body connection portion connected to the vehicle body, and the second frame has a second vehicle body connection portion connected to the vehicle body. The second frame is separated from the first frame in a left-right direction. The work machine includes a first frame connecting part, a second frame connecting part, and a designated part. The first frame connecting part is connected to the first frame, and the second frame connecting part is separated from the first frame connecting part and connected to the second frame in a left-right direction. The designated part is located on the blade tip of the work machine. A first actuator causes the first frame to move relative to the vehicle body; A second actuator causes the second frame to move relative to the vehicle body; The yaw angle of the work machine relative to the vehicle body when it is in the first posture is different from the yaw angle of the work machine when it is in the second posture, which is different from the first posture. The system has the following features: A machine sensor, installed on the machine, detects the roll angle and pitch angle of the machine; Controller; The controller performs the following controls: Obtain the actual frame length, which represents the distance between the first vehicle body connection and the first frame connection; Obtain the position of the first vehicle body connecting part; Obtain the roll angle and pitch angle of the machine. When the machine is in the first posture, the assumed position of the first frame connection is calculated based on the roll angle and pitch angle of the machine, assuming that the yaw angle is a specified angle; Calculate the length of the imaginary frame, which represents the distance between the imaginary position of the first frame connection and the first vehicle body connection; Based on the difference between the actual frame length and the hypothetical frame length, the yaw angle of the machine in the first attitude is calculated; Based on the roll angle, pitch angle, and yaw angle of the machine in the first attitude, the position of the specified part of the machine is calculated.

2. The system as described in claim 1, characterized in that, The controller controls the machine based on the position of the specified portion of the machine.

3. The system as described in claim 1, characterized in that, The vehicle body also has a driving device that enables the operating machinery to move. The first and second frames are arranged on the outside of the traveling device in a left-right direction.

4. The system as described in claim 1, characterized in that, The sensor in the machine is an accelerometer.

5. The system as described in claim 1, characterized in that, In the first posture, the left and right ends of the machine are at different heights. In the second posture, the left and right ends of the machine are at the same height.

6. The system as described in claim 1, characterized in that, The specified angle is zero degrees.

7. A method for controlling working machinery, characterized in that, The operating machinery includes: Vehicle body; The work frame includes a first frame and a second frame. The first frame has a first vehicle body connection portion connected to the vehicle body, and the second frame has a second vehicle body connection portion connected to the vehicle body. The second frame is separated from the first frame in a left-right direction. The work machine includes a first frame connecting part, a second frame connecting part, and a designated part. The first frame connecting part is connected to the first frame, and the second frame connecting part is separated from the first frame connecting part and connected to the second frame in a left-right direction. The designated part is located on the blade tip of the work machine. The yaw angle of the work machine relative to the vehicle body when it is in the first posture is different from the yaw angle of the work machine when it is in the second posture, which is different from the first posture. The method includes: Obtain the position of the first vehicle body connecting part; The roll angle and pitch angle of the machine are detected. Obtain the actual frame length, which represents the distance between the first vehicle body connection and the first frame connection; When the machine is in the first posture, the assumed position of the first frame connection is calculated based on the roll angle and pitch angle of the machine, assuming that the yaw angle is a specified angle; Calculate the length of the imaginary frame, which represents the distance between the imaginary position of the first frame connection and the first vehicle body connection; Based on the difference between the actual frame length and the hypothetical frame length, the yaw angle of the machine in the first attitude is calculated; When the machine is in the first posture, the position of the specified part of the machine is calculated based on the roll angle, pitch angle and yaw angle of the machine in the first posture.

8. The method as described in claim 7, characterized in that, It also has the capability to control the work machine based on the position of the specified portion of the work machine.

9. The method as described in claim 7, characterized in that, In the first posture, the left and right ends of the machine are at different heights. In the second posture, the left and right ends of the machine are at the same height.

10. The method as described in claim 7, characterized in that, The specified angle is zero degrees.

Images