Systems, methods, and programs for controlling industrial machinery.

Abstract

To simplify the operation of industrial machinery. [Solution] The measurement value acquisition unit acquires measurement values ​​from multiple sensors. The position and orientation calculation unit calculates the current orientation of the work tool based on the measurement values. The target orientation determination unit determines a virtual rotation axis based on the calculated current orientation of the work tool. The rotation amount calculation unit generates a control signal for the tilt rotator to rotate the work tool around the virtual rotation axis based on the operation signal from the operating device. The control signal output unit outputs the generated control signal.

Description

[Technical Field]

[0001] This disclosure relates to systems, methods, and programs for controlling working machines. [Background technology]

[0002] Patent Document 1 discloses a control system for a construction machine (working machine) equipped with a tiltable bucket. Thus, working machines are known that are equipped with multiple rotation mechanisms that can rotate around different axes, allowing working tools such as buckets to be rotated as desired. [Prior art documents] [Patent Documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2020-125599 [Overview of the project] [Problems that the invention aims to solve]

[0004] Incidentally, there is a component called a tilt rotator that supports the attachment of a work machine so that it can rotate around three mutually orthogonal axes. By attaching a tilt rotator to a work machine, the attachment can be directed in any direction. On the other hand, one of the most frequently performed basic operations on work machines such as hydraulic excavators is the operation of rotating the bucket along its opening direction (bucket operation). Bucket operation is an operation used when excavating or removing soil. With a normal hydraulic excavator, such an operation can be easily performed with a simple lever operation. However, in a hydraulic excavator equipped with a tilt rotator, rotating the bucket along its opening direction requires a complex operation depending on the opening direction of the bucket.

[0005] The object of this disclosure is to provide a system, method, and program that can simplify the operation of rotating a work tool supported by a work implement via a tilt rotator along its reference direction (e.g., the opening direction of a bucket) in a work machine. [Means for solving the problem]

[0006] According to one aspect of the present disclosure, the system is for controlling a work machine comprising a tilt rotator attached to the tip of the work machine and a work tool rotatably supported around three axes intersecting each other in different planes with respect to the work machine via the tilt rotator, and comprises a processor. The processor acquires measurements from a plurality of sensors. Based on the measurements, the processor calculates the current orientation of the work tool. Based on the calculated current orientation of the work tool, the processor determines a virtual axis of rotation. Based on an operation signal from an operating device, the processor generates a control signal for the tilt rotator to rotate the work tool around the virtual axis of rotation. The processor outputs the generated control signal. [Effects of the Invention]

[0007] According to the above embodiment, in a work machine equipped with a work tool supported by a work machine via a tilt rotator, the operation of rotating the work tool along its reference direction (for example, the opening direction of the bucket) can be simplified. [Brief explanation of the drawing]

[0008] [Figure 1] This is a schematic diagram showing the configuration of the work machine 100 according to the first embodiment. [Figure 2] This is a diagram showing the configuration of the tilt rotator 163 according to the first embodiment. [Figure 3] This figure shows the drive system of the work machine 100 according to the first embodiment. [Figure 4] This is a schematic block diagram showing the configuration of the control device 200 according to the first embodiment. [Figure 5]This is a flowchart showing the dump operation assist control in the first embodiment. [Figure 6] This figure shows the details of the operating device in the first embodiment. [Figure 7] This figure shows the effects of the dump operation assist control in the first embodiment. [Figure 8] This figure shows the effects of the dump operation assist control in the first embodiment. [Modes for carrying out the invention]

[0009] <First Embodiment> 《Configuration of the work machine》 The embodiments will be described in detail below with reference to the drawings. Figure 1 is a schematic diagram showing the configuration of a work machine 100 according to the first embodiment. The work machine 100 according to the first embodiment is, for example, a hydraulic excavator. The work machine 100 comprises a traveling body 120, a rotating body 140, a work machine 160, a driver's cab 180, and a control device 200. The work machine 100 according to the first embodiment controls the cutting edge of the bucket 164 so as not to exceed the design plane.

[0010] The vehicle 120 supports the work machine 100 so that it can move. The vehicle 120 is, for example, a pair of endless tracks on the left and right sides. The rotating body 140 is supported by the traveling body 120 so as to be able to rotate around the pivot point. The implement 160 is operably supported on the slewing body 140. The implement 160 is hydraulically driven. The implement 160 comprises a boom 161, an arm 162, a tilt rotator 163, and a bucket 164, which is a work tool. The base end of the boom 161 is rotatably attached to the slewing body 140. The base end of the arm 162 is rotatably attached to the tip of the boom 161. The tilt rotator 163 is rotatably attached to the tip of the arm 162. The bucket 164 is attached to the tilt rotator 163. The bucket 164 is rotatably supported on the implement 160 via the tilt rotator 163 around three axes that intersect each other in different planes. Here, the part of the slewing body 140 to which the implement 160 is attached is called the front part. Furthermore, with respect to the rotating body 140, the part opposite the front is referred to as the rear, the left side as the left section, and the right side as the right section.

[0011] Figure 2 shows the configuration of a tilt rotator 163 according to the first embodiment. The tilt rotator 163 is attached to the end of an arm 162 so as to support a bucket 164. The tilt rotator 163 comprises a mounting portion 1631, a tilt portion 1632, and a rotating portion 1633. The mounting portion 1631 is attached to the end of the arm 162 so as to be rotatable around an axis extending in the left-right direction as shown. The tilt portion 1632 is attached to the mounting portion 1631 so as to be rotatable around an axis extending in the front-back direction as shown. The rotating portion 1633 is attached to the tilt portion 1632 so as to be rotatable around an axis extending in the up-down direction as shown. Ideally, the axes of rotation of the mounting portion 1631, the tilt portion 1632, and the rotating portion 1633 are orthogonal to each other. The base end of the bucket 164 is fixed to the rotating portion 1633. This allows the bucket 164 to rotate around three axes that are orthogonal to each other with respect to the arm 162. However, in reality, the rotation axes of the mounting portion 1631, the tilt portion 1632, and the rotating portion 1633 may not be orthogonal due to design tolerances.

[0012] The operator's cab 180 is located at the front of the slewing body 140. Inside the operator's cab 180 are an operating device 271 for the operator to operate the work machine 100, and a monitoring device 272 which is the man-machine interface for the control device 200. The operating device 271 receives input from the operator regarding the control amounts of the travel motor 304, the slewing motor 305, the boom cylinder 306, the arm cylinder 307, the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310. The monitoring device 272 receives input from the operator regarding the setting and release of the bucket attitude holding mode. The bucket attitude holding mode is a mode in which the control device 200 automatically controls the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310 to maintain the attitude of the bucket 164 in the global coordinate system. The monitoring device 272 is implemented by a computer equipped with, for example, a touch panel.

[0013] The control device 200 controls the traveling body 120, the rotating body 140, and the work equipment 160 based on the operation of the control device 271 by the operator. The control device 200 is installed, for example, inside the operator's cab 180.

[0014] 《Drive System of 100 Work Machines》 Figure 3 shows the drive system of the work machine 100 according to the first embodiment. The work machine 100 is equipped with multiple actuators for driving the work machine 100. Specifically, the work machine 100 is equipped with an engine 301, a hydraulic pump 302, a control valve 303, a pair of travel motors 304, a slewing motor 305, a boom cylinder 306, an arm cylinder 307, a bucket cylinder 308, a tilt cylinder 309, and a rotary motor 310.

[0015] Engine 301 is the prime mover that drives the hydraulic pump 302. The hydraulic pump 302 is driven by the engine 301 and supplies hydraulic fluid to the travel motor 304, slewing motor 305, boom cylinder 306, arm cylinder 307, and bucket cylinder 308 via the control valve 303. The control valve 303 controls the flow rate of hydraulic fluid supplied from the hydraulic pump 302 to the travel motor 304, slewing motor 305, boom cylinder 306, arm cylinder 307, and bucket cylinder 308. The travel motor 304 is driven by hydraulic fluid supplied from the hydraulic pump 302 and drives the travel body 120. The slewing motor 305 is driven by hydraulic fluid supplied from the hydraulic pump 302, causing the slewing body 140 to rotate relative to the traveling body 120.

[0016] The boom cylinder 306 is a hydraulic cylinder for driving the boom 161. The base end of the boom cylinder 306 is attached to the slewing body 140. The tip end of the boom cylinder 306 is attached to the boom 161. The arm cylinder 307 is a hydraulic cylinder for driving the arm 162. The base end of the arm cylinder 307 is attached to the boom 161. The tip end of the arm cylinder 307 is attached to the arm 162. The bucket cylinder 308 is a hydraulic cylinder for driving the tilt rotator 163 and the bucket 164. The base end of the bucket cylinder 308 is attached to the arm 162. The tip end of the bucket cylinder 308 is attached to the tilt rotator 163 via a link member.

[0017] The tilt cylinder 309 is a hydraulic cylinder for driving the tilt section 1632. The base end of the tilt cylinder 309 is attached to the mounting section 1631. The tip of the rod of the tilt cylinder 309 is attached to the tilt section 1632. The rotary motor 310 is a hydraulic motor for driving the rotating section 1633. The bracket and stator of the rotary motor 310 are fixed to the tilt section 1632. The rotating shaft and rotor of the rotary motor 310 are provided to extend in the vertical direction as shown in the figure and are fixed to the rotating section 1633.

[0018] Measurement system for 100 working machines The work machine 100 is equipped with multiple sensors for measuring the attitude, orientation, and position of the work machine 100. Specifically, the work machine 100 is equipped with an inclination meter 401, a position and orientation meter 402, a boom angle sensor 403, an arm angle sensor 404, a bucket angle sensor 405, a tilt angle sensor 406, and a rotation angle sensor 407.

[0019] The inclination meter 401 measures the attitude of the slewing body 140. The inclination meter 401 measures the inclination of the slewing body 140 with respect to the horizontal plane (e.g., roll angle, pitch angle, and yaw angle). An example of an inclination meter 401 is an IMU (Inertial Measurement Unit). In this case, the inclination meter 401 measures the acceleration and angular velocity of the slewing body 140 and calculates the inclination of the slewing body 140 with respect to the horizontal plane based on the measurement results. The inclination meter 401 is installed, for example, below the driver's cab 180. The inclination meter 401 outputs the measured attitude data of the slewing body 140 to the control device 200.

[0020] The position and direction measuring instrument 402 measures the position of a representative point of the slewing body 140 and the direction in which the slewing body 140 is facing using GNSS (Global Navigation Satellite System). The position and direction measuring instrument 402 is equipped with, for example, two GNSS antennas (not shown) attached to the slewing body 140, and measures the direction perpendicular to the line connecting the positions of the two antennas as the direction in which the work machine 100 is facing. The position and direction measuring instrument 402 outputs the measured position data and direction data of the slewing body 140 to the control device 200.

[0021] The boom angle sensor 403 measures the boom angle, which is the angle of the boom 161 relative to the slewing body 140. The boom angle sensor 403 may be an IMU attached to the boom 161. In this case, the boom angle sensor 403 measures the boom angle based on the inclination of the boom 161 with respect to the horizontal plane and the inclination of the slewing body measured by the inclination measuring instrument 401. The measured value of the boom angle sensor 403 is zero, for example, when the direction of the straight line passing through the base and tip of the boom 161 coincides with the front-rear direction of the slewing body 140. In other embodiments, the boom angle sensor 403 may be a stroke sensor attached to the boom cylinder 306. In other embodiments, the boom angle sensor 403 may be a rotation sensor provided on the joint axis that rotatably connects the slewing body 140 and the boom 161. The boom angle sensor 403 outputs the measured boom angle data to the control device 200.

[0022] The arm angle sensor 404 measures the arm angle, which is the angle of the arm 162 relative to the boom 161. The arm angle sensor 404 may be an IMU attached to the arm 162. In this case, the arm angle sensor 404 measures the arm angle based on the inclination of the arm 162 with respect to the horizontal plane and the boom angle measured by the boom angle sensor 403. The measured value of the arm angle sensor 404 is zero, for example, when the direction of the straight line passing through the base and tip of the arm 162 coincides with the direction of the straight line passing through the base and tip of the boom 161. In other embodiments, the arm angle sensor 404 may calculate the angle by attaching a stroke sensor to the arm cylinder 307. In other embodiments, the arm angle sensor 404 may also be a rotation sensor provided on the joint axis that rotatably connects the boom 161 and the arm 162. The arm angle sensor 404 outputs the measured arm angle data to the control device 200.

[0023] The bucket angle sensor 405 measures the bucket angle, which is the angle of the tilt rotator 163 relative to the arm 162. The bucket angle sensor 405 may be a stroke sensor provided on the bucket cylinder 308. In this case, the bucket angle sensor 405 measures the bucket angle based on the stroke amount of the bucket cylinder 308. The measured value of the bucket angle sensor 405 is zero, for example, when the direction of the straight line passing through the base end and cutting edge of the bucket 164 coincides with the direction of the straight line passing through the base end and tip of the arm 162. In other embodiments, the bucket angle sensor 405 may be a rotation sensor provided on the joint axis that rotatably connects the arm 162 and the mounting portion 1631 of the tilt rotator 163. In other embodiments, the bucket angle sensor 405 may be an IMU attached to the bucket 164. The bucket angle sensor 405 outputs the measured bucket angle data to the control device 200.

[0024] The tilt angle sensor 406 measures the tilt angle, which is the angle of the tilt portion 1632 relative to the mounting portion 1631 of the tilt rotator 163. The tilt angle sensor 406 may be a rotation sensor provided on the joint axis that rotatably connects the mounting portion 1631 and the tilt portion 1632. The measured value of the tilt angle sensor 406 is zero, for example, when the rotation axis of the arm 162 and the rotation axis of the rotation portion 1633 are orthogonal. In other embodiments, the tilt angle sensor 406 may be configured to calculate the angle by attaching a stroke sensor to the tilt cylinder 309. The tilt angle sensor 406 outputs the measured tilt angle data to the control device 200.

[0025] The rotation angle sensor 407 measures the rotation angle, which is the angle of the rotating part 1633 relative to the tilt part 1632 of the tilt rotator 163. The rotation angle sensor 407 may be a rotation sensor provided on the rotary motor 310. The measured value of the tilt angle sensor 406 is zero, for example, when the cutting edge direction of the bucket 164 and the operating plane of the work machine 160 are perpendicular. The rotation angle sensor 407 outputs the measured rotation angle data to the control device 200.

[0026] Configuration of the control device 200 Figure 4 is a schematic block diagram showing the configuration of the control device 200 according to the first embodiment. The control device 200 is a computer comprising a processor 210, main memory 230, storage 250, and interface 270. The control device 200 is an example of a control system. The control device 200 receives measured values ​​from an inclination meter 401, a position and orientation meter 402, a boom angle sensor 403, an arm angle sensor 404, a bucket angle sensor 405, a tilt angle sensor 406, and a rotation angle sensor 407.

[0027] The storage 250 is a tangible, non-temporary storage medium. Examples of the storage 250 include magnetic disks, optical disks, magneto-optical disks, and semiconductor memory. The storage 250 may be an internal medium directly connected to the bus of the control device 200, or an external medium connected to the control device 200 via the interface 270 or a communication line. The operating device 271 and the monitoring device 272 are connected to the processor 210 via the interface 270.

[0028] The storage 250 stores a control program for controlling the work machine 100. The control program may be for implementing some of the functions that the control device 200 is to perform. For example, the control program may perform its functions in combination with other programs already stored in the storage 250, or in combination with other programs implemented in other devices. In other embodiments, the control device 200 may include a custom LSI (Large Scale Integrated Circuit) such as a PLD (Programmable Logic Device) in addition to, or instead of, the above configuration. Examples of PLDs include PAL (Programmable Array Logic), GAL (Generic Array Logic), CPLD (Complex Programmable Logic Device), and FPGA (Field Programmable Gate Array). In this case, some or all of the functions implemented by the processor may be implemented by the integrated circuit.

[0029] Storage 250 records geometric data representing the dimensions and center of gravity of the slewing body 140, boom 161, arm 162, and bucket 164. Geometric data represents the position of an object in a given coordinate system. Storage 250 also records design surface data, which is three-dimensional data representing the shape of the design surface of the construction site in a global coordinate system. The global coordinate system is defined by the X-axis extending in the direction of latitude. g Y-axis, extending in the direction of longitude g Axis, Z extending vertically g It is a coordinate system composed of axes. Design surface data is represented, for example, by TIN (Triangular Irregular Networks) data.

[0030] Software Configuration The processor 210, by executing a control program, includes an operation signal acquisition unit 211, an input unit 212, a display control unit 213, a measured value acquisition unit 214, a position and attitude calculation unit 215, a control signal output unit 218, a target attitude determination unit 219, and a rotation amount calculation unit 220.

[0031] The operation signal acquisition unit 211 acquires operation signals from the operation device 271 that indicate the amount of operation of each actuator. The input unit 212 receives operation input from the operator via the monitoring device 272. The display control unit 213 outputs screen data to be displayed on the monitor device 272 to the monitor device 272. The measurement value acquisition unit 214 acquires measurement values ​​from the inclination measuring instrument 401, position and orientation measuring instrument 402, boom angle sensor 403, arm angle sensor 404, bucket angle sensor 405, tilt angle sensor 406, and rotation angle sensor 407.

[0032] The position and orientation calculation unit 215 calculates the position of the work machine 100 in the global coordinate system and the vehicle body coordinate system based on various measurement values ​​acquired by the measurement value acquisition unit 214 and geometry data recorded in the storage 250. For example, the position and orientation calculation unit 215 calculates the position of the cutting edge of the bucket 164 in the global coordinate system and the vehicle body coordinate system. The vehicle body coordinate system is a Cartesian coordinate system with the origin at a representative point of the rotating body 140 (for example, a point passing through the center of rotation). The calculations performed by the position and orientation calculation unit 215 will be described later.

[0033] The control signal output unit 218 outputs control signals for each actuator (bucket cylinder 308, tilt cylinder 309, and rotary motor 310) to the control valve 303, corresponding to the manipulated amount acquired by the operation signal acquisition unit 211 or the target value calculated by the rotation amount calculation unit 220.

[0034] The functions of the target attitude determination unit 219 and the rotation amount calculation unit 220 will be explained in detail in the following description of dump operation assist control.

[0035] 《Calculation by the position and orientation calculation unit 215》 Here, a method for calculating the position of a point on the outer shell of the working machine 100 by the position and orientation calculation unit 215 will be described. The position and orientation calculation unit 215 calculates the position of a point on the outer shell based on various measurement values acquired by the measurement value acquisition unit 214 and the geometry data recorded in the storage 250. The storage 250 records geometry data representing the dimensions of the slewing body 140, the boom 161, the arm 162, the tilt rotator 163 (mounting portion 1631, tilt portion 1632, and rotation portion 1633), and the bucket 164.

[0036] The geometry data of the slewing body 140 indicates the center position (x bm , y bm , z bm ) of the joint axis by which the slewing body 140 supports the boom 161 in the vehicle body coordinate system, which is a local coordinate system. The vehicle body coordinate system is a coordinate system composed of an X sb axis extending in the front-rear direction, a Y sb axis extending in the left-right direction, and a Z sb axis extending in the up-down direction, with the center of rotation of the slewing body 140 as a reference. Note that the up-down direction of the slewing body 140 does not necessarily coincide with the vertical direction.

[0037] The geometry data of the boom 161 indicates the position (x am , y am , z am ) of the joint axis by which the boom 161 supports the arm 162 in the boom coordinate system, which is a local coordinate system. The boom coordinate system is based on the center position of the joint axis connecting the slewing body 140 and the boom 161, and is composed of an X bm axis extending in the longitudinal direction, a Y bm axis extending in the direction in which the joint axis extends, and a Z bm axis and a Y bm axis orthogonal to the X bm axis.

[0038] The geometry data of the arm 162 indicates the position (x t1 , y t1 , z t1The arm coordinate system is based on the center position of the joint axis connecting boom 161 and arm 162, with the X extending in the longitudinal direction. am The Y extends in the direction in which the axis and joint axis extend. am axis, x am Axis and Y am Z perpendicular to the axis am It is a coordinate system composed of axes.

[0039] The geometric data of the mounting portion 1631 of the tilt rotator 163 is the position (x) of the joint axis supporting the tilt portion 1632 in the first tilt rotator coordinate system, which is the local coordinate system. t2 , y t2 , z t2 ) and the tilt of the joint axis (φ t ) indicates the tilt of the joint axis φ t This is the angle related to the design error of the tilt rotator 163, and is determined by calibration of the tilt rotator 163, etc. The first tilt rotator coordinate system is based on the center position of the joint axis connecting the arm 162 and the mounting part 1631, and extends in the direction in which the joint axis connecting the arm 162 and the mounting part 1631 extends. t1 The joint axis connecting the shaft, mounting part 1631, and tilt part 1632 extends in the direction of extension of the Z axis. t1 Axis, and Y t1 Axis and Z t1 X perpendicular to the axis t1 It is a coordinate system composed of axes.

[0040] The geometry data of the tilt section 1632 of the tilt rotator 163 is the tip position (x) of the rotation axis of the rotary motor 310 in the second tilt rotator coordinate system, which is the local coordinate system. t3 , y t3 , z t3 ) and the tilt of the axis of rotation (φ r ) indicates the inclination of the axis of rotation φ rThis is the angle related to the design error of the tilt rotator 163, and is determined by calibration of the tilt rotator 163, etc. The second tilt rotator coordinate system is based on the center position of the joint axis connecting the mounting part 1631 and the tilt part 1632, and the X extends in the direction in which the joint axis connecting the mounting part 1631 and the tilt part 1632 extends. t2 The axis, the rotation axis of the rotary motor 310, extends in the direction of Z. t2 Axis, and X t2 Axis and Z t2 Y perpendicular to the axis t2 It is a coordinate system composed of axes.

[0041] The geometric data of the rotating part 1633 of the tiltrotator 163 is the center position (x) of the mounting surface of the bucket 164 in the third tiltrotate coordinate system, which is the local coordinate system. t4 , y t4 , z t4 The third tilt-rotate coordinate system is defined as the Z coordinate system extending in the direction in which the rotation axis of the rotary motor 310 extends, with the center position of the mounting surface of the bucket 164 as the reference point. t3 X perpendicular to the axis and rotation axis t3 Axis and Y t3 It is a coordinate system composed of axes. Note that the cutting edge of bucket 164 is Y t3 It is attached to the rotating part 1633 so as to be parallel to the axis.

[0042] The geometry data of bucket 164 is the position of multiple contour points of bucket 164 in the third tilt-rotate coordinate system (x bk , y bk , z bk This shows the contour points. Examples of contour points include the positions at both ends and the center of the cutting edge of bucket 164, the positions at both ends and the center of the bottom of bucket 164, and the positions at both ends and the center of the tail of bucket 164.

[0043] The position and attitude calculation unit 215 uses the boom angle θ acquired by the measurement value acquisition unit 214. bm Based on the measured values ​​and the geometry data of the rotating body 140, the boom-to-vehicle transformation matrix T for converting from the boom coordinate system to the vehicle coordinate system is calculated using the following equation (1).bm sb Generates the boom-body transformation matrix T. bm sb Y bm Boom angle θ around the axis bm Rotate by only that much, and the deviation (x) between the origin of the vehicle coordinate system and the origin of the boom coordinate system bm , y bm , z bm This is a matrix that translates by ) units.

[0044]

number

[0045] The position and attitude calculation unit 215 calculates the arm angle θ acquired by the measurement value acquisition unit 214. am Based on the measured values ​​and the geometry data of boom 161, the arm-boom transformation matrix T for converting from the arm coordinate system to the boom coordinate system is calculated using the following equation (2). am bm Generates the arm-boom transformation matrix T. am bm Y am Arm angle θ around the axis am Rotate by only that much, and the deviation (x) between the origin of the boom coordinate system and the origin of the arm coordinate system am , y am , z am This is a matrix that translates by only ). Also, the position and orientation calculation unit 215 calculates the boom-body transformation matrix T bm sb and the arm-boom transformation matrix T am bm By calculating the product of these, we obtain the arm-to-vehicle transformation matrix T for transforming from the arm coordinate system to the vehicle coordinate system. am sb Generates.

[0046]

number

[0047] The position and attitude calculation unit 215 uses the bucket angle θ acquired by the measurement value acquisition unit 214. bkBased on the measured values and the geometric data of the arm 162, the first tilt - arm transformation matrix T for converting from the first tilt - rotate coordinate system to the arm coordinate system is generated according to the following formula (3). t1 am The first tilt - arm transformation matrix T t1 am is a matrix that rotates by the bucket angle θ t1 only around the Y bk axis, translates by the deviation (x t1 , y t1 , z t1 ) between the origin of the arm coordinate system and the origin of the first tilt - rotate coordinate system, and further tilts by the inclination φ t of the joint axis of the tilt part 1632. Also, the position and orientation calculation unit 215 obtains the product of the arm - vehicle body transformation matrix T am sb and the first tilt - arm transformation matrix T t1 am to generate the first tilt - vehicle body transformation matrix T t1 sb for converting from the first tilt - rotate coordinate system to the vehicle body coordinate system.

[0048]

Equation

[0049] Based on the measured value of the tilt angle θ t acquired by the measurement value acquisition unit 214 and the geometric data of the tilt rotator 163, the second tilt - first tilt transformation matrix T t2 t1 for converting from the first tilt - rotate coordinate system to the second tilt - rotate coordinate system is generated according to the following formula (4). t2 t1 The second tilt - first tilt transformation matrix T t2 is a matrix that rotates by the tilt angle θ t only around the X t2 axis, translates by the deviation (x t2 , y t2) is translated by only that much, and further the inclination of the rotation axis of the rotating part 1633 is φ r This is a matrix that tilts by only a certain amount. Furthermore, the position and orientation calculation unit 215 uses the first tilt-body transformation matrix T. t1 sb and the second tilt-first tilt transformation matrix T t2 t1 By calculating the product of these, we obtain the second tilt-to-vehicle transformation matrix T for transforming from the second tilt-rotate coordinate system to the vehicle coordinate system. t2 sb Generates.

[0050]

number

[0051] The position and orientation calculation unit 215 uses the rotation angle θ acquired by the measurement value acquisition unit 214. r Based on the measured values ​​and the geometry data of the tiltrotator 163, the third tilt-to-second tilt transformation matrix T for transforming from the second tiltrotate coordinate system to the third tiltrotate coordinate system is calculated using equation (5) below. t3 t2 This generates the third tilt-second tilt transformation matrix T. t3 t2 is, Z t3 Rotation angle θ around the axis r Rotate by only that much, and the deviation (x) between the origin of the second tilt-rotate coordinate system and the origin of the third tilt-rotate coordinate system. t3 , y t3 , z t3 This is a matrix that translates by only ). Also, the position and attitude calculation unit 215 is the second tilt-body transformation matrix T t2 sb and the 3rd tilt-2nd tilt transformation matrix T t3 t2 By calculating the product of these, we obtain the third tilt-to-vehicle transformation matrix T for transforming from the third tilt-rotate coordinate system to the vehicle coordinate system. t3 sb Generates.

[0052]

number

[0053] The position and orientation calculation unit 215 calculates the center position (x) of the mounting surface of the bucket 164. t4 , y t4 , z t4 ) and the positions of multiple contour points in the third tilt-rotate coordinate system as shown by the geometry data of bucket 164 (x bk , y bk , z bk The sum of ) and the third tilt-body transformation matrix T bk sb By calculating the product of these two factors, the positions of multiple contour points of bucket 164 in the vehicle coordinate system can be determined.

[0054] By the way, the angle of the cutting edge of the bucket 164 relative to the ground surface of the work machine 100, that is, the X coordinate system of the vehicle body coordinate system sb -Y sb Plane and the Y coordinate system of the third tilt-rotate coordinate system t3 The angle it makes with the axis is the boom angle θ. bm , arm angle θ am , bucket angle θ bk , tilt angle θ t and rotation angle θ r This is determined by the position and orientation calculation unit 215, as shown in Figure 1, identifies a bucket coordinate system that starts from the base end of the bucket 164, i.e., the center position of the mounting surface of the bucket 164 on the tilt rotator 163. The bucket coordinate system is determined by the X-axis extending in the direction that the cutting edge of the bucket 164 faces. bk axis, x bk A Y-shaped axis perpendicular to the axis and extending along the cutting edge of bucket 164. bk Axis, and X bk Axis and Y bk Z perpendicular to the axis bk It is a Cartesian coordinate system composed of axes. Hereafter, X bk The axis is the bucket tilt axis, Y bk The axis is the bucket pitch axis, Z bk The axis is also called the bucket rotation axis. Bucket tilt axis X bk Bucket pitch axis Y bk and bucket rotation axis Z bkThis is a virtual axis and is different from the joint axis of the tilt rotator 163. Note that when the tilt of the rotation axis of the rotary motor 310 is zero, the bucket coordinate system and the third tilt rotator coordinate system coincide.

[0055] The position and orientation calculation unit 215 calculates the bucket-third tilt transformation matrix T for converting from the third tilt-rotate coordinate system to the bucket coordinate system based on the geometry data of the tilt rotator 163, according to equation (6) below. bk t3 This generates the bucket-third tilt transformation matrix T. bk t3 Y t3 The inclination of the axis of rotation around the axis φ r This is a matrix that rotates by only one degree.

[0056]

number

[0057] Dump operation assist control The dump operation assist control according to this embodiment will be described in detail below with reference to the drawings. Dump operation is a control used when performing excavation work or soil removal work (dumping work), which rotates the bucket 164 along the opening direction of the bucket 164. In this embodiment, the bucket 164 is supported so as to be rotatable around three axes that intersect in different planes relative to the work machine 160 via the tilt rotator 163, so that the operator is required to perform complex operations. For this reason, the dump operation assist control uses the opening direction of the bucket 164 as the reference direction and controls the tilt rotator 163 so that the bucket 164 rotates around a virtual rotation axis determined corresponding to the reference direction. In this embodiment, the bucket tilt axis X bk The bucket pitch axis Y is perpendicular to the bucket and extends along the cutting edge of the bucket 164. bkThe Y-axis is determined as the virtual axis of rotation, and at least one of the bucket cylinder 308, tilt cylinder 309, and rotary motor 310 is controlled so that the bucket 164 rotates around the virtual axis of rotation. The dump operation assist control moves the bucket 164 by an amount corresponding to the amount of operation indicated by the operation signal from the operating device, along the bucket pitch axis Y. bk The tilt rotator 163 is controlled by calculating the amount of rotation around the three axes to make it rotate.

[0058] First, we will explain in detail the operation signal acquisition unit 211, control signal output unit 218, target attitude determination unit 219, and rotation amount calculation unit 220 shown in Figure 4.

[0059] In addition to the functions described above, the operation signal acquisition unit 211 acquires the amount of operation to a dedicated operation reception unit (hereinafter also referred to as the dump assist operation reception unit) in the operation device 271 for using dump operation assist control.

[0060] The target attitude determination unit 219 determines a target attitude, which is the attitude of the bucket 164 rotated around a virtual rotation axis by an amount corresponding to the operation amount acquired by the operation signal acquisition unit 211, from its current attitude. The virtual rotation axis is a rotation axis defined in correspondence with the opening direction of the bucket 164, and more specifically, it is a virtual rotation axis defined so that the bucket 164 traces a trajectory along its opening direction. In this embodiment, the bucket pitch axis (Y) in the bucket coordinate system described above is bk The axis (see Figure 1) is determined as the virtual axis of rotation.

[0061] The rotation amount calculation unit 220 calculates the amount of rotation for each of the multiple rotation mechanisms necessary to adjust the current position of the bucket 164 to the target position. Here, the multiple rotation mechanisms in this embodiment are the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310. As shown in Figures 1 and 2, the bucket cylinder 308 rotates the bucket 164 to a Y t1 To rotate around the axis. The tilt cylinder 309 moves the bucket 164 to X t2It is rotated around the axis. Also, the rotary motor 310 moves the bucket 164, Z t3 To rotate around an axis.

[0062] Next, with reference to Figures 5 to 8, the processing flow of dump operation assist control by the control device 200 of this embodiment will be described.

[0063] Figure 5 is a flowchart showing the dump operation assist control in the first embodiment. When the operator of the work machine 100 starts operating the work machine 100, the control device 200 executes the following control at predetermined control cycles (for example, 1000 milliseconds).

[0064] First, the measurement value acquisition unit 214 acquires the measured values ​​from the inclination measuring instrument 401, the position and orientation measuring instrument 402, the boom angle sensor 403, the arm angle sensor 404, the bucket angle sensor 405, the tilt angle sensor 406, and the rotation angle sensor 407 (step S101).

[0065] The position and attitude calculation unit 215 calculates the attitude of the bucket in the vehicle body coordinate system based on the measured values ​​obtained in step S101 (step S102). The attitude of the bucket in the vehicle body coordinate system is calculated based on each axis (X) of the bucket coordinate system in the vehicle body coordinate system. bk , Y bk , Z bk Attitude matrix R that indicates the direction of ) cur This is represented by the attitude matrix R representing the attitude of bucket 164. cur All translation components are set to zero.

[0066] The operation signal acquisition unit 211 acquires the amount of operation performed by the operator to the dump assist operation reception unit (step S103).

[0067] In this embodiment, the operating device 271 includes, for example, two levers 2710 and 2711 as shown in Figure 6. In this embodiment, the work machine 100, like a normal work machine, allows the operator to control the rotation of the slewing body 140 and the boom angle θ by tilting the two levers 2710 and 2711 in the forward / backward and left / right directions.bm , arm angle θ am and bucket angle θ bk Each lever can be operated individually. Furthermore, the operator can control the tilt angle θ via the tilt rotator 163 by operating the control input (button, slide switch, dial, proportional roller switch, etc.) located on the upper surface of each lever 2710, 2711. t and rotation angle θ r These can be controlled individually.

[0068] Furthermore, the operating device 271 according to this embodiment has a dump assist operation receiving section 2710a on the lever 2710. The dump assist operation receiving section 2710a is, for example, a proportional roller type switch that is biased by a spring to return to the central position. The operator can adjust the rotation direction and rotation speed (angular velocity) along the opening direction of the bucket 164 by the operating direction and amount of the dump assist operation receiving section 2710a.

[0069] Returning to Figure 5, next, the target attitude determination unit 219 determines the bucket pitch axis Y bk The virtual rotation axis is determined, and the bucket pitch axis Y is determined according to the manipulated amount obtained in step S103. bk Target value θ of angular velocity around bk_p_tgt Identify (Step S104). For example, the target attitude determination unit 219 determines the bucket pitch axis Y as the amount of manipulation increases. bk Target value θ of angular velocity around bk_p_tgt The value of is specified to be large. Note that the bucket pitch axis Y bk Target value θ of angular velocity around bk_p_tgt Identifying this is equivalent to determining the target attitude of bucket 164 after a unit of time has elapsed from the present moment.

[0070] Next, the rotation amount calculation unit 220 calculates the target value θ identified by the target attitude determination unit 219. bk_p_tgt Based on this, the target values ​​for the amount of rotation for each of the multiple rotating mechanisms required to adjust the current position of bucket 164 to the target position are calculated (step S105). Specifically, the rotation amount calculation unit 220 calculates the target value θ of angular velocity in the following equation (7). bk_t_tgt By substituting this, the bucket pitch axis Y in the bucket coordinate system bk Rotation matrix R representing the rotation around bk_p bk Create.

[0071]

number

[0072] The rotation amount calculation unit 220 calculates a matrix R representing the current attitude of the bucket 164. cur Next, the rotation matrix R in equation (7) bk_p bk By multiplying by this, the target attitude R of bucket 164 after a unit of time is obtained. tgt The rotation amount calculation unit 220 then calculates the current attitude R of the bucket 164. cur And the target attitude R of bucket 164 after a unit of time tgt Based on this, the bucket angle θ is given by equations (8), (9), and (10) below. bk , tilt angle θ t and rotation angle θ r Each target value (θ) bk_tgt ,θ t_tgt ,θ r_tgt )

[0073]

number

number

number

[0074] As described above, the target value of the angular velocity around a single virtual rotation axis (θ) is obtained by matrix transformation. bk_p_tgt ) is the target value of the angular velocity around the three machine axes (θ bk_tgt ,θ t_tgt ,θ r_tgt It will be converted to ).

[0075] Next, the control signal output unit 218 outputs the bucket angle θ bk , tilt angle θ t and rotation angle θ r Each target value (θ) bk_tgt ,θ t_tgt ,θ r_tgt Control signals for each actuator (bucket cylinder 308, tilt cylinder 309, and rotary motor 310) are generated according to the specified value, and the control signals for each actuator are output to the control valve 303 (step S106).

[0076] Action / Effect Next, with reference to Figures 7 and 8, the effects and benefits of the dump operation assist control in the first embodiment will be described. Figures 7 and 8 show the work machine 100 viewed from the same angle. In Figure 7, the opening direction of bucket 164 faces the front of the paper, and the virtual rotation axis is the bucket pitch axis Y. bk The bucket is positioned parallel to the plane of the paper. In this case, when the operator operates the dump assist operation receiving unit 2710a (see Figure 6), the bucket 164 moves along the bucket pitch axis Y, which is parallel to the plane of the paper. bk As a result of rotating around the page, it will rotate either towards the front of the page or towards the back of the page. Similarly, in Figure 8, the opening direction of bucket 164 is facing to the right of the page, and the virtual rotation axis is the bucket pitch axis Y. bk The bucket is positioned perpendicular to the plane of the paper. The other angles are the same as in Figure 7. In this case, when the operator operates the dump assist operation receiving unit 2710a (see Figure 6), the bucket 164 moves along the bucket pitch axis Y, which is perpendicular to the plane of the paper. bk As a result of the rotation, it will rotate either to the left or to the right of the paper.

[0077] As described above, according to the control device 200 of the work machine 100 according to this embodiment, regardless of the direction in which the opening direction of the bucket 164 is facing the vehicle body (slewing body 140), the operator can rotate the bucket 164 along its opening direction simply by performing an operation on a predetermined operation receiving unit 2710a. In other words, conventionally, the operator had to operate each machine axis (θ) to operate the bucket 164 in its opening direction. bk ,θ t ,θ r While complex three-axis operation was required for the operation of the control unit 2710a, in the work machine 100 according to this embodiment, this can be done with a single-axis operation of the control unit 2710a. This makes it possible to easily perform dumping (soil removal) and excavation operations.

[0078] As described above, according to the control device 200 of the first embodiment, in a work machine 100 that includes a work machine 160 in which a plurality of rotating mechanisms (bucket cylinder 308, tilt cylinder 309, and rotary motor 310) and a bucket 164 are connected, the operation of rotating the bucket 164 along its reference direction (opening direction) can be simplified.

[0079] <Other Embodiments> Although one embodiment has been described in detail above with reference to the drawings, the specific configuration is not limited to that described above, and various design changes are possible. In other embodiments, the order of the above-described processes may be changed as appropriate. Also, some processes may be executed in parallel. The control device 200 according to the above embodiment may be composed of a single computer, or the configuration of the control device 200 may be divided among multiple computers, and the multiple computers may cooperate with each other to function as the control device 200. In this case, some of the computers constituting the control device 200 may be mounted inside the work machine, and other computers may be provided outside the work machine. For example, in another embodiment, the operating device 271 and the monitoring device 272 may be provided remotely from the work machine 100, and the components of the control device 200 other than the measurement value acquisition unit 214 and the control signal output unit 218 may be provided on a remote server.

[0080] Furthermore, while the work machine 100 in the above-described embodiment is a hydraulic excavator, it is not limited to this. For example, the work machine 100 in another embodiment may be a non-self-propelled work machine fixed to the ground. Also, the work machine 100 in another embodiment may be a work machine that does not have a rotating body.

[0081] The work machine 100 according to the above embodiment includes a bucket 164 as an attachment to the work machine 160, but is not limited to this. For example, the work machine 100 according to other embodiments may include a breaker, fork, grapple, etc. as attachments. In this case as well, the control device 200 extends in the direction in which the cutting edge of the attachment faces, similar to the bucket coordinate system. bk The axis and the Y extending in the direction along the cutting edge bk Axis and X bk Axis and Y bk Z perpendicular to the axis bk The tilt rotator 163 is controlled by a local coordinate system consisting of axes.

[0082] In another embodiment, the axes of the tilt rotator 163 do not have to be orthogonal, as long as they intersect in different planes. Specifically, with respect to axis AX1, which is the joint axis connecting the arm 162 and the mounting part 1631, axis AX2, which is the joint axis connecting the mounting part 1631 and the tilt part 1632, and the rotation axis AX3 of the rotary motor 310, it is sufficient that when the tilt angle and rotation angle of the tilt rotator 163 are zero, the planes parallel to axes AX1 and AX2, the planes parallel to axes AX2 and AX3, and the planes parallel to axes AX3 and AX1 are all different.

[0083] Furthermore, the control device 200 according to other embodiments may not have a design surface setting function. In this case as well, the control device 200 can automatically control the tilt rotator 163 by performing bucket attitude holding control. For example, the operator can perform simple leveling work without setting a design surface. [Explanation of symbols]

[0084] 100…Work machine 120…Traveling body 140…Slewing body 160…Work machine 161…Boom 162…Arm 163…Tilt rotator 1631…Mounting part 1632…Tilt part 1633…Rotating part 164…Bucket 180…Operator's cab 200…Control device 210…Processor 211…Operation signal acquisition unit 212…Input unit 213…Display control unit 214…Measurement value acquisition unit 215…Position and attitude calculation unit 218…Control signal output unit 219…Target attitude determination unit 220…Rotation amount calculation unit 230…Main memory 250…Storage 270…Interface 271…Operating device 272…Monitor device 301…Engine 302…Hydraulic pump 303…Control valve 304…Travel motor 305…Slewing motor 306…Boom cylinder 307…Arm cylinder 308…Bucket cylinder 309…Tilt cylinder 310…Rotation motor 401…Incline measuring instrument 402…Position and orientation measuring instrument 403…Boom angle sensor 404…Arm angle sensor 405…Bucket angle sensor 406…Tilt angle sensor 407…Rotation angle sensor

Claims

1. A system for controlling a work machine comprising a tilt rotator attached to the tip of the work machine, and a work tool supported via the tilt rotator so as to be rotatable around three axes that intersect each other in different planes relative to the work machine, Equipped with a processor, The aforementioned processor, By acquiring measured values ​​from multiple sensors, Based on the measured values, the current position of the work tool is calculated. Based on the calculated current position of the work tool, the virtual axis of rotation is determined. Based on the operation signal from the operating device, a control signal for the tilt rotator is generated to rotate the work tool around the virtual rotation axis. Output the generated control signal. system.

2. The aforementioned processor, The amount of rotation around the three axes is calculated so that the work tool rotates around the virtual rotation axis by an amount corresponding to the amount of operation indicated by the operation signal. Based on the amount of rotation around the three axes, a control signal for the tilt rotator is generated. The system according to claim 1.

3. The aforementioned work tool has a cutting edge, The virtual axis of rotation is an axis that is perpendicular to the direction in which the cutting edge of the work tool faces and extends along the cutting edge of the work tool. The system according to claim 1 or 2.

4. The aforementioned virtual rotation axis is different from the joint axis of the tilt rotator. The system according to any one of claims 1 to 3.

5. A method for controlling a work machine comprising a tilt rotator attached to the tip of the work machine, and a work tool supported via the tilt rotator so as to be rotatable around three axes that intersect each other in different planes relative to the work machine, Steps include acquiring measured values ​​from multiple sensors, The steps include determining the current posture of the work machine based on the measured values, The steps include determining a virtual axis of rotation corresponding to the reference direction of the work tool based on the current posture determined, The steps include generating a control signal for the tilt rotator to rotate the work tool around the virtual axis of rotation based on an operation signal from the operating device, The steps include outputting the generated control signal, A method for providing this.

6. A computer in a control system for a work machine comprising a tilt rotator attached to the tip of the work machine and a work tool supported so as to be rotatable around three axes that intersect each other in different planes with respect to the work machine via the tilt rotator, Steps include acquiring measured values ​​from multiple sensors, The steps include determining the current posture of the work machine based on the measured values, The steps include determining a virtual axis of rotation corresponding to the reference direction of the work tool based on the current posture determined, The steps include generating a control signal for the tilt rotator to rotate the work tool around the virtual axis of rotation based on an operation signal from the operating device, The steps include outputting the generated control signal, A program that executes the command.

Images