System, method, and program for controlling work machine

Abstract

To simplify the operation of a working machine.SOLUTION: The measurement value acquisition unit acquires measurement values from a plurality of sensors. The position / posture calculation unit calculates a current posture of the work tool based on the measurement value. The target posture determination unit determines the virtual rotation axis based on the calculated current posture of the work tool when a predetermined control start condition is satisfied. The rotation amount calculation unit generates a control signal of the tilt rotator for rotating the work tool around the virtual rotation axis by a predetermined amount so that the work tool changes from the current posture to the target posture. The control signal output unit outputs the generated control signal.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present disclosure relates to a system, method, and program for controlling a work machine. [Background technology]

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

[0003] [Patent Document 1] Japanese Patent Application Publication No. 2020-125599 Summary of the Invention [Problem to be solved by the invention]

[0004] Incidentally, tiltrotators are known that support a work machine's attachment so that it can rotate around three mutually perpendicular axes. By attaching a tiltrotator to a work machine, the attachment can be oriented in any direction. Meanwhile, when loading soil or other materials onto the bed of a dump truck in a work machine such as a hydraulic excavator, there is a desire to minimize spillage of the soil during the process of moving the bucket onto the bed. However, in a hydraulic excavator equipped with multiple rotation mechanisms as described above, if the bucket is moved when its width (the direction along the cutting edge) is not horizontal, the soil loaded in the bucket is likely to spill during transport to the bed of the dump truck. Therefore, it is preferable to adjust the width of the bucket to be horizontal before moving it.

[0005] On the other hand, in a hydraulic excavator equipped with a tiltrotator, it is expected that the bucket opening direction will be aligned with the excavation surface using the tiltrotator's rotation mechanism. Therefore, in consideration of the efficiency of excavation work, there is a desire to avoid changing the bucket opening direction before and after the dump truck loading operation.

[0006] An object of the present disclosure is to provide a system, method, and program that can simplify the operation of aligning a second reference direction (e.g., the direction of the bucket cutting edge) with a predetermined plane (e.g., the vehicle body reference plane) without changing the first reference direction of the work implement (e.g., the direction of the bucket opening) in a work machine that has a work implement supported on the work machine via a tiltrotator. [Means for solving the problem]

[0007] According to one aspect of the present disclosure, a system for controlling a work machine includes a work implement operably supported on a vehicle body, a tiltrotator attached to the tip of the work implement, and a work implement supported via the tiltrotator so as to be rotatable about three axes that intersect with each other in different planes relative to the work implement, and the system includes a processor. The processor acquires measurement values ​​from multiple sensors. The processor calculates a current attitude of the work implement based on the measurement values. When a predetermined control start condition is satisfied, the processor determines a virtual rotation axis based on the calculated current attitude of the work implement. The processor generates a control signal for the tiltrotator to rotate the work implement by a predetermined amount about the virtual rotation axis so as to change the work implement from the current attitude to a target attitude. The processor outputs the generated control signal. [Effects of the Invention]

[0008] According to the above aspect, in a work machine having a work implement supported on a work machine via a tiltrotator, the operation of aligning the second reference direction with a predetermined plane can be simplified without changing the first reference direction of the work implement. [Brief explanation of the drawings]

[0009] [Figure 1] 1 is a schematic diagram showing the configuration of a work machine 100 according to a first embodiment. [Figure 2] FIG. 2 is a diagram showing the configuration of a tiltrotator 163 according to the first embodiment. [Figure 3] 1 is a diagram showing a drive system of a work machine 100 according to a first embodiment. [Figure 4] 1 is a schematic block diagram showing the configuration of a control device 200 according to a first embodiment. [Figure 5] 10 is a flowchart showing an angle adjustment function in the first embodiment. [Figure 6] FIG. 2 is a diagram illustrating details of an operating device according to the first embodiment. [Figure 7] 10A and 10B are diagrams illustrating the effects of the angle adjustment function in the first embodiment. [Figure 8] 10A and 10B are diagrams illustrating the effects of the angle adjustment function in the first embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0010] First Embodiment <<Configuration of the work machine>> Hereinafter, the embodiments will be described in detail with reference to the drawings. 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 includes a traveling body 120, a rotating body 140, a work implement 160, a cab 180, and a control device 200. The work machine 100 according to the first embodiment controls the cutting edge of a bucket 164 so that it does not exceed a design surface.

[0011] The running body 120 supports the work machine 100 so that the work machine 100 can travel. The running body 120 is, for example, a pair of left and right caterpillars. The rotating body 140 is supported by the running body 120 so as to be able to rotate around a rotation center. The work implement 160 is movably supported on the revolving unit 140. The work implement 160 is hydraulically driven. The work implement 160 includes a boom 161, an arm 162, a tiltrotator 163, and a bucket 164 that is a work implement. A base end of the boom 161 is rotatably attached to the revolving unit 140. A base end of the arm 162 is rotatably attached to the tip end of the boom 161. The tiltrotator 163 is rotatably attached to the tip end of the arm 162. The bucket 164 is attached to the tiltrotator 163. The bucket 164 is supported via the tiltrotator 163 so as to be rotatable about three axes that intersect with each other in different planes relative to the work implement 160. Here, the portion of the revolving unit 140 to which the work implement 160 is attached is referred to as the front. Furthermore, with respect to the rotating body 140, the opposite side of the front portion will be referred to as the rear portion, the left side as the left portion, and the right side as the right portion.

[0012] FIG. 2 is a diagram showing the configuration of a tiltrotator 163 according to the first embodiment. The tiltrotator 163 is attached to the tip of an arm 162 so as to support a bucket 164. The tiltrotator 163 includes a mounting portion 1631, a tilt portion 1632, and a rotating portion 1633. The mounting portion 1631 is attached to the tip of the arm 162 so as to be rotatable about an axis extending in the left-right direction in the figure. The tilt portion 1632 is attached to the mounting portion 1631 so as to be rotatable about an axis extending in the front-rear direction in the figure. The rotating portion 1633 is attached to the tilt portion 1632 so as to be rotatable about an axis extending in the up-down direction in the figure. Ideally, the rotation axes of the mounting portion 1631, the tilt portion 1632, and the rotating portion 1633 are perpendicular to one another. The base end of the bucket 164 is fixed to the rotating portion 1633. This allows the bucket 164 to rotate relative to the arm 162 about three axes that are perpendicular to one another. However, in reality, the rotation axes of the mounting portion 1631, tilt portion 1632, and rotation portion 1633 may include design errors and may not necessarily be perpendicular to each other.

[0013] The operator's cab 180 is provided at the front of the rotating body 140. Inside the operator's cab 180, there are provided an operation device 271 that the operator uses to operate the work machine 100, and a monitor device 272 that is a man-machine interface for the control device 200. The operation device 271 receives inputs from the operator regarding the operation amount of the travel motor 304, the operation amount of the swing motor 305, the operation amount of the boom cylinder 306, the operation amount of the arm cylinder 307, the operation amount of the bucket cylinder 308, the operation amount of the tilt cylinder 309, and the operation amount of the rotation motor 310. The monitor device 272 receives inputs from the operator to set and cancel 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 rotation motor 310 to hold the attitude of the bucket 164 in the global coordinate system. The monitor device 272 is realized by, for example, a computer equipped with a touch panel.

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

[0015] <<Drive system of work machine 100>> FIG. 3 is a diagram showing the drive system of the work machine 100 according to the first embodiment. The work machine 100 is equipped with a plurality of 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 swing motor 305, a boom cylinder 306, an arm cylinder 307, a bucket cylinder 308, a tilt cylinder 309, and a rotation motor 310.

[0016] The engine 301 is a prime mover that drives the hydraulic pump 302 . The hydraulic pump 302 is driven by the engine 301 and supplies hydraulic oil to the travel motor 304 , the swing motor 305 , the boom cylinder 306 , the arm cylinder 307 and the bucket cylinder 308 via a control valve 303 . The control valve 303 controls the flow rate of hydraulic oil supplied from the hydraulic pump 302 to the travel motor 304 , the swing motor 305 , the boom cylinder 306 , the arm cylinder 307 and the bucket cylinder 308 . The traveling motor 304 is driven by hydraulic oil supplied from the hydraulic pump 302 to drive the traveling body 120 . The swing motor 305 is driven by hydraulic oil supplied from the hydraulic pump 302 and causes the swing body 140 to swing relative to the traveling body 120 .

[0017] The boom cylinder 306 is a hydraulic cylinder for driving the boom 161. A base end of the boom cylinder 306 is attached to the revolving body 140. A 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. A base end of the arm cylinder 307 is attached to the boom 161. A tip end of the arm cylinder 307 is attached to the arm 162. The bucket cylinder 308 is a hydraulic cylinder for driving the tiltrotator 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 tiltrotator 163 via a link member.

[0018] The tilt cylinder 309 is a hydraulic cylinder for driving the tilt unit 1632. The base end of the tilt cylinder 309 is attached to the attachment unit 1631. The tip end of the rod of the tilt cylinder 309 is attached to the tilt unit 1632. Rotary motor 310 is a hydraulic motor for driving rotating unit 1633. A bracket and a stator of rotary motor 310 are fixed to tilt unit 1632. A rotating shaft and a rotor of rotary motor 310 are provided to extend in the vertical direction in the figure, and are fixed to rotating unit 1633.

[0019] <<Measurement system of work machine 100>> The work machine 100 is equipped with a plurality of sensors for measuring the attitude, orientation, and position of the work machine 100. Specifically, the work machine 100 is equipped with an inclination measuring instrument 401, a position and orientation measuring instrument 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.

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

[0021] The position and orientation measuring instrument 402 uses the GNSS (Global Navigation Satellite System) to measure the position of a representative point of the revolving unit 140 and the orientation in which the revolving unit 140 is facing. The position and orientation measuring instrument 402 is equipped with, for example, two GNSS antennas (not shown) attached to the revolving unit 140, and measures the orientation perpendicular to the line connecting the positions of the two antennas as the orientation in which the work machine 100 is facing. The position and orientation measuring instrument 402 outputs measurement values, that is, position data and orientation data of the revolving unit 140, to the control device 200.

[0022] The boom angle sensor 403 measures the boom angle, which is the angle of the boom 161 relative to the revolving structure 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 a horizontal plane and the inclination of the revolving structure measured by the inclination measuring device 401. The measurement value of the boom angle sensor 403 indicates zero when, for example, the direction of a line passing through the base end and tip end of the boom 161 coincides with the fore-and-aft direction of the revolving structure 140. Note that the boom angle sensor 403 according to another embodiment may be a stroke sensor attached to the boom cylinder 306. Furthermore, the boom angle sensor 403 according to another embodiment may be a rotation sensor provided on a joint shaft that rotatably connects the revolving structure 140 and the boom 161. The boom angle sensor 403 outputs boom angle data, which is the measurement value, to the control device 200.

[0023] The arm angle sensor 404 measures the arm angle, which is the angle of the arm 162 with respect 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 measurement value of the arm angle sensor 404 indicates zero when, for example, the direction of a line passing through the base end and tip end of the arm 162 coincides with the direction of a line passing through the base end and tip end of the boom 161. Note that, in another embodiment, the arm angle sensor 404 may calculate the angle by attaching a stroke sensor to the arm cylinder 307. In another embodiment, the arm angle sensor 404 may be a rotation sensor provided on a joint shaft that rotatably connects the boom 161 and the arm 162. The arm angle sensor 404 outputs arm angle data, which is the measurement value, to the control device 200.

[0024] The bucket angle sensor 405 measures the bucket angle, which is the angle of the tiltrotator 163 with respect to the arm 162. The bucket angle sensor 405 may be a stroke sensor provided in 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 measurement value of the bucket angle sensor 405 indicates zero when, for example, the direction of a line passing through the base end and cutting edge of the bucket 164 matches the direction of a line passing through the base end and tip of the arm 162. Note that the bucket angle sensor 405 according to another embodiment may be a rotation sensor provided on a joint shaft that rotatably connects the arm 162 and the mounting portion 1631 of the tiltrotator 163. Furthermore, the bucket angle sensor 405 according to another embodiment may be an IMU attached to the bucket 164. The bucket angle sensor 405 outputs bucket angle data, which is the measurement value, to the control device 200.

[0025] The tilt angle sensor 406 measures the tilt angle, which is the angle of the tilt section 1632 relative to the mounting section 1631 of the tilt rotator 163. The tilt angle sensor 406 may be a rotation sensor provided on a joint shaft that rotatably connects the mounting section 1631 and the tilt section 1632. The measurement value of the tilt angle sensor 406 indicates zero when, for example, the rotation axis of the arm 162 and the rotation axis of the rotating section 1633 are perpendicular to each other. Note that the tilt angle sensor 406 according to another embodiment may calculate the angle by attaching a stroke sensor to the tilt cylinder 309. The tilt angle sensor 406 outputs tilt angle data, which is the measurement value, to the control device 200.

[0026] 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 in the rotation motor 310. The measurement value of the tilt angle sensor 406 indicates zero, for example, when the cutting edge direction of the bucket 164 and the motion plane of the work implement 160 are perpendicular to each other. The rotation angle sensor 407 outputs rotation angle data, which is the measurement value, to the control device 200.

[0027] <Configuration of control device 200> FIG. 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 including a processor 210, a main memory 230, a storage 250, and an interface 270. The control device 200 is an example of a control system. The control device 200 receives measurement values ​​from an inclination measuring device 401, a position and orientation measuring device 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.

[0028] The storage 250 is a non-transitory tangible storage medium. Examples of the storage 250 include a magnetic disk, an optical disk, a magneto-optical disk, and a 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 an interface 270 or a communication line. The operation device 271 and the monitor device 272 are connected to the processor 210 via the interface 270.

[0029] The storage 250 stores a control program for controlling the work machine 100. The control program may be for realizing some of the functions to be performed by the control device 200. For example, the control program may be a program that performs a function in combination with other programs already stored in the storage 250 or in combination with other programs implemented in other devices. Note that 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 realized by the processor may be realized by the integrated circuit.

[0030] The storage 250 stores geometry data that represents the dimensions and center of gravity of the revolving body 140, the boom 161, the arm 162, and the bucket 164. The geometry data represents the position of an object in a predetermined coordinate system. The storage 250 also stores design surface data, which is three-dimensional data that represents the shape of the design surface of the construction site in a global coordinate system. The global coordinate system is a coordinate system consisting of X, Y, Z, and Z directions that extend in the latitude direction. g Axial, Y extending in the meridian direction g Axis, extending vertically Z g It is a coordinate system consisting of axes. The design surface data is represented by, for example, TIN (Triangular Irregular Networks) data.

[0031] <Software Configuration> By executing the control program, the processor 210 is provided with an operation signal acquisition unit 211, an input unit 212, a display control unit 213, a measurement value acquisition unit 214, a position and orientation calculation unit 215, an intervention determination unit 216, a control signal output unit 218, a target orientation determination unit 219, and a rotation amount calculation unit 220.

[0032] The operation signal acquisition unit 211 acquires an operation signal indicating the amount of operation of each actuator from the operation device 271. The input unit 212 receives operation inputs from an operator via a monitor 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 , 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 .

[0033] The position and attitude calculation unit 215 calculates the position of the work machine 100 in the global coordinate system and the vehicle body coordinate system based on the various measurement values ​​acquired by the measurement value acquisition unit 214 and the geometry data recorded in the storage 250. For example, the position and attitude 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 an orthogonal coordinate system with its origin set at a representative point of the revolving unit 140 (for example, a point passing through the center of rotation). The calculations of the position and attitude calculation unit 215 will be described later.

[0034] The intervention determination unit 216 determines whether to limit the speed of the work implement 160 based on the position of the cutting edge of the bucket 164 calculated by the position and orientation calculation unit 215 and the positional relationship between the design surface indicated by the design surface data. Hereinafter, limiting the speed of the work implement 160 by the control device 200 is also referred to as intervention control. Specifically, the intervention determination unit 216 calculates the shortest distance between the design surface and the bucket 164, and determines to perform intervention control on the work implement 160 if the shortest distance is equal to or shorter than a predetermined distance. Specifically, the intervention determination unit 216 converts the position of the design surface expressed in the global coordinate system into a position in the vehicle body coordinate system by rotating and translating the design surface data recorded in the storage 250 based on the measurement values ​​of the inclinometer 401 and the position and orientation measurement unit 402. The intervention determination unit 216 identifies, as a control point, the point closest to the design surface among the multiple contour points of the bucket 164. The intervention determination unit 216 identifies a surface (polygon) located vertically below the control point in the design surface data. The intervention determination unit 216 determines the X coordinate of the bucket coordinate system that passes through the control point. bk -Z bk A first design line is calculated as the line of intersection between a plane parallel to the plane and the identified plane. The intervention determination unit 216 determines whether the distance between the control point and the first design line is equal to or less than the intervention threshold.

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

[0036] The functions of the target attitude determination unit 219 and the rotation amount calculation unit 220 will be explained in detail in the explanation of the angle matching function below.

[0037] <<Calculation by the position and orientation calculation unit 215>> Here, a method for calculating the positions of points on the hull of the work machine 100 by the position and orientation calculation unit 215 will be described. The position and orientation calculation unit 215 calculates the positions of points on the hull based on various measurement values ​​acquired by the measurement value acquisition unit 214 and geometry data recorded in the storage 250. Geometry data representing the dimensions of the revolving unit 140, boom 161, arm 162, tilt rotator 163 (mounting unit 1631, tilt unit 1632, and rotating unit 1633), and bucket 164 is recorded in the storage 250.

[0038] The geometry data of the revolving unit 140 is the center position (x bm , y bm , z bm The vehicle body coordinate system is an X coordinate system that extends in the front-rear direction with the center of rotation of the rotating body 140 as the reference. sb axis, Y extending left and right sb Axis, extending vertically Z sb It should be noted that the up-down direction of the rotating body 140 does not necessarily coincide with the vertical direction.

[0039] The geometry data of the boom 161 is the position (x) of the joint axis where the boom 161 supports the arm 162 in the boom coordinate system, which is a local coordinate system. am , y am , z am The boom coordinate system is based on the center position of the joint shaft connecting the rotating body 140 and the boom 161, and is defined by an X axis extending in the longitudinal direction. bm Y axis extends in the direction in which the joint axis extends bm axis, X bm Axis and Y bm Z perpendicular to the axis bm It is a coordinate system consisting of axes.

[0040] The geometry data of the arm 162 is the position (x) of the joint axis where the arm 162 supports the mounting part 1631 of the tiltrotator 163 in the arm coordinate system, which is a local coordinate system. t1 , y t1 , z t1The arm coordinate system is based on the center position of the joint axis connecting the boom 161 and the arm 162, and is defined by an X axis extending in the longitudinal direction. am Y axis extends in the direction in which the joint axis extends am axis, X am Axis and Y am Z perpendicular to the axis am It is a coordinate system consisting of axes.

[0041] The geometry data of the mounting part 1631 of the tiltrotator 163 is the position (x t2 , y t2 , z t2 ) and the inclination of the joint axis (φ t ) The tilt of the joint axis φ t is an angle related to the design error of the tiltrotator 163, and can be obtained by calibration of the tiltrotator 163, etc. The first tiltrotate coordinate system is based on the center position of the joint axis connecting the arm 162 and the attachment part 1631, and is defined by a Y t1 The Z axis extends in the direction in which the joint axis connecting the mounting portion 1631 and the tilt portion 1632 extends. t1 axis, and Y t1 axis and Z t1 Orthogonal to the X axis t1 It is a coordinate system consisting of axes.

[0042] The geometry data of the tilt unit 1632 of the tilt rotator 163 is the tip position (x t3 , y t3 , z t3 ) and the tilt of the rotation axis (φ r ) The tilt of the rotation axis φ ris an angle related to the design error of the tiltrotator 163, and can be obtained by calibration of the tiltrotator 163, etc. The second tiltrotate coordinate system is based on the center position of the joint axis connecting the mounting part 1631 and the tilt part 1632, and is an X axis extending in the direction in which the joint axis connecting the mounting part 1631 and the tilt part 1632 extends. t2 The Z axis extends in the direction in which the rotation axis of the rotary motor 310 extends. t2 axis, and X t2 axis and Z t2 Y perpendicular to the axis t2 It is a coordinate system consisting of axes.

[0043] The geometry data of the rotating part 1633 of the tiltrotator 163 is the center position (x t4 , y t4 , z t4 The third tilt-rotate coordinate system is based on the center position of the mounting surface of the bucket 164 and is defined by a Z axis extending in the direction in which the rotation shaft of the rotation motor 310 extends. t3 X axis, perpendicular to the rotation axis t3 axis and Y t3 The bucket 164 has a cutting edge that is aligned along the Y axis. t3 It is attached to the rotating part 1633 so that it is parallel to the axis.

[0044] The geometry data of the bucket 164 includes the positions (x bk , y bk , z bk ) are shown. Examples of contour points include both ends and the center of the cutting edge of the bucket 164, both ends and the center of the bottom of the bucket 164, and both ends and the center of the butt of the bucket 164.

[0045] The position and orientation calculation unit 215 calculates the boom angle θ bm Based on the measurement values ​​and the geometry data of the rotating body 140, a boom-to-body transformation matrix T for transforming from the boom coordinate system to the body coordinate system is calculated using the following equation (1):bm sb Generate the boom-body transformation matrix T bm sb is Y bm Boom angle θ around the axis bm and the deviation between the origin of the vehicle coordinate system and the origin of the boom coordinate system (x bm , y bm , z bm ) is a matrix that translates the

[0046]

number

[0047] The position and orientation calculation unit 215 calculates the arm angle θ am Based on the measurement values ​​and the geometry data of the boom 161, an arm-boom transformation matrix T for transforming from the arm coordinate system to the boom coordinate system is calculated by the following formula (2): am bm The arm-boom transformation matrix T am bm is Y am Arm angle θ around the axis am and the deviation between the origin of the boom coordinate system and the origin of the arm coordinate system (x am , y am , z am ) is a matrix that translates the boom-vehicle body transformation matrix T bm sb and the arm-boom transformation matrix T am bm The arm-body transformation matrix T is used to transform from the arm coordinate system to the body coordinate system. am sb Generate.

[0048]

number

[0049] The position and orientation calculation unit 215 calculates the bucket angle θ bkBased on the measurement values ​​and the geometry data of the arm 162, a first tilt-arm transformation matrix T for transforming from the first tilt-rotated coordinate system to the arm coordinate system is calculated by the following equation (3): t1 am The first tilt-arm transformation matrix T t1 am is Y t1 Bucket angle θ around the axis bk and the deviation between the origin of the arm coordinate system and the origin of the first tilt-rotate coordinate system (x t1 , y t1 , z t1 ), and then the tilt angle φ of the joint axis of the tilt unit 1632 is t The position and orientation calculation unit 215 also calculates the arm-vehicle body transformation matrix T am sb and the first tilt-arm transformation matrix T t1 am The first tilt-body transformation matrix T is used to transform from the first tilt-rotated coordinate system to the body coordinate system. t1 sb Generate.

[0050]

number

[0051] The position and orientation calculation unit 215 calculates the tilt angle θ t Based on the measurement values ​​of the tiltrotator 163 and the geometry data of the tiltrotator 163, a second tilt-first tilt transformation matrix T for transforming from the first tiltrotated coordinate system to the second tiltrotated coordinate system is calculated by the following equation (4): t2 t1 The second tilt-first tilt transformation matrix T t2 t1 is X t2 Tilt angle θ around the axis t and the deviation of the origin of the first tilt-rotated coordinate system from the origin of the second tilt-rotated coordinate system (x t2 , y t2 , z t2), and then the tilt of the rotation axis of the rotating part 1633 is φ r The position and orientation calculation unit 215 also calculates the first tilt-vehicle body transformation matrix T t1 sb and the second tilt-first tilt transformation matrix T t2 t1 The second tilt-body transformation matrix T is used to transform from the second tilt-rotated coordinate system to the body coordinate system. t2 sb Generate.

[0052]

number

[0053] The position and orientation calculation unit 215 calculates the rotation angle θ r Based on the measurement values ​​of the tiltrotator 163 and the geometry data of the tiltrotator 163, a third tilt-second tilt transformation matrix T for transforming from the second tiltrotated coordinate system to the third tiltrotated coordinate system is calculated by the following equation (5): t3 t2 The third tilt-second tilt transformation matrix T t3 t2 is Z t3 Rotation angle θ around the axis r and the deviation between the origin of the second tilt-rotated coordinate system and the origin of the third tilt-rotated coordinate system (x t3 , y t3 , z t3 ) is a matrix that translates the vehicle body by the second tilt-vehicle body transformation matrix T t2 sb and the third tilt-second tilt transformation matrix T t3 t2 The third tilt-body transformation matrix T is used to transform from the third tilt-rotated coordinate system to the body coordinate system. t3 sb Generate.

[0054]

number

[0055] The position and orientation calculation unit 215 calculates the center position (x t4 , y t4 , z t4 ) and the positions (x bk , y bk , z bk ) and the third tilt-body transformation matrix T bk sb By calculating the product of these, the positions of a plurality of contour points of the bucket 164 in the vehicle body coordinate system can be calculated.

[0056] Incidentally, the angle of the blade tip of the bucket 164 with respect to the ground surface of the work machine 100, i.e., the X sb -Y sb Y of the plane and the third tilt-rotated coordinate system t3 The angle between the axis is the boom angle θ bm , arm angle θ am , bucket angle θ bk , tilt angle θ t and rotation angle θ r Therefore, as shown in FIG. 1, the position and orientation calculation unit 215 specifies a bucket coordinate system that has its origin at the base end of the bucket 164, that is, the center position of the mounting surface of the bucket 164 on the tiltrotator 163. The bucket coordinate system is defined by an X bk axis, X bk Y perpendicular to the axis and extending along the cutting edge of the bucket 164 bk axis, and X bk axis and Y bk Z perpendicular to the axis bk It is a Cartesian coordinate system consisting of the X axis. bk axis as bucket tilt axis, Y bk axis as 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 bkis a virtual axis and is different from the joint axis of the tiltrotator 163. When the tilt of the rotation axis of the rotation motor 310 is zero, the bucket coordinate system and the third tiltrotate coordinate system coincide with each other.

[0057] The position and orientation calculation unit 215 calculates a bucket-third tilt transformation matrix T for transforming from the third tilt rotated coordinate system to the bucket coordinate system using the following equation (6) based on the geometry data of the tilt rotator 163: bk t3 Generate the bucket-third tilt transformation matrix T bk t3 is Y t3 Inclination of the rotation axis around the axis φ r is a matrix that rotates by

[0058]

number

[0059] <Angle adjustment function> Hereinafter, the angle adjustment function according to this embodiment will be described in detail with reference to the drawings. Here, "angle adjustment" means adjusting the angle of the bucket tilt axis (X bk The bucket 164 is rotated around the bucket pitch axis (Y bk This refers to the operation of setting the X axis at a predetermined angle relative to the vehicle body reference plane. sb Axis-Y sb The predetermined angle is the angle at which the cutting edge is parallel to the vehicle body reference plane (see FIG. 1). In this embodiment, the predetermined angle is the angle at which the cutting edge is parallel to the vehicle body reference plane. This operation makes it possible to make the cutting edge parallel to the vehicle body reference plane without changing the opening direction of the bucket 164 (bucket tilt axis direction). Note that the predetermined angle is not limited to the angle at which the cutting edge is parallel to the vehicle body reference plane, but may be an angle arbitrarily determined by the operator.

[0060] First, the operation signal acquisition unit 211, the control signal output unit 218, the target attitude determination unit 219, and the rotation amount calculation unit 220 in FIG. 4 will be described in detail.

[0061] In addition to the above-mentioned functions, the operation signal acquisition unit 211 acquires an operation signal to an operation reception unit (hereinafter also referred to as angle adjustment operation reception unit) dedicated to using the angle adjustment function in the operation device 271.

[0062] When the target attitude determination unit 219 receives an operation signal for angle adjustment from the operation reception unit, it determines a target attitude, which is an attitude obtained by rotating the bucket 164 by a predetermined amount from the current attitude around a virtual rotation axis. The virtual rotation axis is a virtual rotation axis that faces the opening direction of the bucket 164. In this embodiment, the bucket tilt axis (X bk The target attitude is an attitude in which a reference axis perpendicular to the virtual rotation axis forms a predetermined angle with respect to a predetermined plane. The reference axis is an axis extending along the cutting edge of the bucket 164, and in this embodiment, it is the bucket pitch axis (Y bk axis, see Figure 1). The specified plane is the vehicle body reference plane.

[0063] The rotation amount calculation unit 220 calculates the amount of rotation for each of the plurality of rotation mechanisms required to align the current attitude of the bucket 164 with the target attitude. Here, the plurality of rotation mechanisms in this embodiment are the bucket cylinder 308, tilt cylinder 309, and rotation motor 310. As shown in FIGS. 1 and 2, the bucket cylinder 308 rotates the bucket 164 in a manner such that Y t1 The tilt cylinder 309 rotates the bucket 164 around the X axis. t2 The rotation motor 310 rotates the bucket 164 around the Z axis. t3 Rotate it around the axis.

[0064] Next, the flow of processing of the angle adjustment function by the control device 200 of this embodiment will be described with reference to FIGS.

[0065] 5 is a flowchart showing the angle adjustment function 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 intervals (for example, 1000 milliseconds).

[0066] First, the measurement value acquisition unit 214 acquires the measurement values ​​of 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).

[0067] The position and attitude calculation unit 215 calculates the attitude of the bucket in the vehicle body coordinate system based on the measurement values ​​acquired in step S101 (step S102). The attitude of the bucket in the vehicle body coordinate system is calculated using the coordinates of each axis (X bk , Y bk , Z bk ) orientation matrix R cur The orientation matrix R representing the orientation of the bucket 164 is expressed as follows: cur All translation components of are set to zero.

[0068] The operation signal acquisition unit 211 acquires an operation signal from the angle adjustment operation acceptance unit by the operator (step S103).

[0069] In this embodiment, the operating device 271 is equipped with, for example, two levers 2710, 2711 as shown in Fig. 6. In the construction machine 100 according to this embodiment, like a normal construction machine, the operator tilts the two levers 2710, 2711 in the front-rear and left-right directions to perform the rotation operation of the rotating body 140 and the boom angle θ bm , arm angle θ am and bucket angle θ bk The operator can also operate the operation reception units (buttons, slide switches, dials, proportional roller switches) provided on the top surfaces of the levers 2710 and 2711 to adjust the tilt angle θ via the tilt rotator 163. t and rotation angle θr can be controlled individually.

[0070] Furthermore, the operating device 271 according to this embodiment has an angle adjustment operation receiving unit 2710b on the lever 2710. The angle adjustment operation receiving unit 2710b is, for example, a push-down mechanical switch. By pressing the switch, the operator can execute angle adjustment control at a desired timing. When the processor receives a signal from the angle adjustment operation receiving unit 2710b, it determines that a predetermined control start condition has been met, and proceeds to the processing of step S104.

[0071] Returning to FIG. 9, next, the target attitude determination unit 219 calculates the bucket tilt axis X bk is determined as the virtual rotation axis, and the bucket tilt axis X bk Target value of angular velocity θ bk_t_tgt (Step S104). bk_t_tgt may be a preset fixed value. bk Target value of angular velocity θ bk_t_tgt Specifying the target attitude of the bucket 164 is equivalent to determining the target attitude of the bucket 164 that should be reached after a unit time has elapsed from the present time.

[0072] Next, the rotation amount calculation unit 220 calculates the target value θ bk_t_tgt Based on this, a target value of the rotation amount for each of the plurality of rotation mechanisms required to adjust the current posture of the bucket 164 to the target posture is calculated (step S105). Specifically, the rotation amount calculation unit 220 calculates the target value θ of the angular velocity in the following equation (7): bk_t_tgt By substituting the above, the bucket tilt axis X in the bucket coordinate system bk Rotation matrix R representing the rotation around bk_t bk Create a.

[0073]

number

[0074] The rotation amount calculation unit 220 calculates a matrix R that represents the current orientation of the bucket 164. cur Then, the rotation matrix R in Eq. (7) bk_p bk By multiplying this, the target posture R of the bucket 164 after a unit time is obtained. tgt Then, the rotation amount calculation unit 220 calculates the current orientation R of the bucket 164. cur and the target attitude R of bucket 164 after a unit time. tgt Based on this, the bucket angle θ is calculated using the following equations (8), (9), and (10). bk , tilt angle θ t and rotation angle θ r The target values ​​of each (θ bk_tgt ,θ t_tgt ,θ r_tgt ) is found.

[0075]

number

number

number

[0076] As described above, the target value (θ bk_t_tgt ) are the target values ​​of the angular velocity around the three mechanical axes (θ bk_tgt ,θ t_tgt ,θ r_tgt )

[0077] Next, the control signal output unit 218 outputs the bucket angle θ bk , tilt angle θ t and rotation angle θ r The target values ​​of each (θ bk_tgt ,θ t_tgt ,θ r_tgt ) and outputs the control signals for the actuators (bucket cylinder 308, tilt cylinder 309 and rotation motor 310) to the control valve 303 (step S106).

[0078] The control signal output unit 218 outputs control signals for each actuator to the control valve 303, thereby actually changing the attitude of the bucket 164. At this time, the target attitude determination unit 219 determines the current attitude R after the change. cur is acquired, and it is determined whether the bucket pitch axis is parallel to the vehicle body reference plane (step S107). Bucket pitch axis (Y bk If the bucket tilt axis (X axis) is not parallel to the vehicle body reference plane (step S107; NO), the process returns to step S104 and the bucket tilt axis (X axis) is adjusted again. bk The target value of the angular velocity around the axis (θ bk_t_tgt As a result, the process of step S105 by the rotation amount calculation unit 220 and the process of step S106 by the control signal output unit 218 are executed again. On the other hand, the bucket pitch axis (Y bk If the rotational axis) becomes parallel to the vehicle body reference plane (step S107; YES), the target attitude determination unit 219, the rotation amount calculation unit 220, and the control signal output unit 218 end the processing. This completes the automatic angle adjustment control by the control device 200.

[0079] Actions and Effects Next, the effect of the angle adjustment function will be described with reference to FIGS. 7 and 8 show the work machine 100 as viewed from the same angle. Here, FIG. 7 shows the cutting edge direction of the bucket 164 (bucket pitch axis (Y bk axis)) is the vehicle reference plane (X sb -Y sb The figure shows the state immediately after excavation (scooping) has been performed while the work machine 100 is inclined relative to the horizontal plane. From this state, the operator of the work machine 100 loads the work machine 100 onto a dump truck.

[0080] To load the scooped soil into a dump truck, the operator operates the boom 161 and arm 162 to lift the bucket 164 upward. However, if the cutting edge of the bucket 164 remains tilted, some of the scooped soil will spill out of the bucket 164. Therefore, the operator presses down on the operation receiving unit 2710b (see FIG. 6) while operating the boom 161 and arm 162 via the levers 2710 and 2711. This automatically moves the bucket 164 in the direction of the bucket tilt axis (X bk The bucket rotates around the pitch axis (Y bk axis) is controlled so that it is parallel to the vehicle reference plane.

[0081] FIG. 8 shows the state immediately after the automatic angle adjustment control is completed. As shown in FIG. 8, the cutting edge direction of the bucket 164 (the bucket pitch axis (Y bk The opening direction of the bucket 164 (the bucket tilt axis (X bk Therefore, when the bucket 164 is returned to the excavation surface after discharging soil into the dump truck, the opening surface remains aligned with the excavation surface.

[0082] As described above, the control device 200 according to the first embodiment can simplify the operation of making the cutting edge direction of the bucket 164 parallel to the vehicle body reference plane in a work machine 100 equipped with a work implement 160 formed by connecting multiple rotation mechanisms (bucket cylinder 308, tilt cylinder 309, and rotation motor 310) and a bucket 164.

[0083] (Modification of the first embodiment) In the above-described first embodiment, it has been described that the angle adjustment control can be started at a desired timing by the operator pressing the operation receiving unit 2710b. That is, in the first embodiment, the condition for starting control of the angle adjustment function is an operation by the operator (pressing a button). However, other embodiments are not limited to this. For example, the control device 200 according to a modification of the first embodiment may have the following functions.

[0084] The target attitude determination unit 219 according to the modification of the first embodiment starts the process of determining the target attitude when the bucket 164 is separated from the ground by a predetermined distance as a control start condition for the angle adjustment function.

[0085] The determination of whether bucket 164 has moved a predetermined distance away from the ground can be made by, for example, using the function of intervention determination unit 216 described above. That is, desired attitude determination unit 219 constantly determines the shortest distance between the position of the cutting edge of bucket 164 and the design surface via intervention determination unit 216. Then, while bucket 164 is ascending after scooping, desired attitude determination unit 219 determines that the predetermined control start condition has been satisfied when the shortest distance, which is constantly being calculated, becomes equal to or greater than a predetermined determination threshold, and starts the processing of step S104.

[0086] In this way, the operation by the operator can be eliminated when performing the angle adjustment control, and the loading work onto the loading platform can be further simplified.

[0087] Other Embodiments Although one embodiment has been described in detail above with reference to the drawings, the specific configuration is not limited to the above, and various design modifications are possible. That is, in other embodiments, the order of the above-described processes may be changed as appropriate. Furthermore, some processes may be executed in parallel. The control device 200 according to the embodiment described above may be configured by a single computer, or the configuration of the control device 200 may be divided among multiple computers that work together to function as the control device 200. In this case, some of the computers that make up 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 operation device 271 and the monitor device 272 may be provided remotely from the work machine 100, and the configuration of the control device 200 other than the measurement value acquisition unit 214 and the control signal output unit 218 may be provided in a remote server.

[0088] Furthermore, although the work machine 100 according to the embodiment described above is a hydraulic excavator, this is not limited to this. For example, the work machine 100 according to another embodiment may be a work machine that is fixed to the ground and is not self-propelled. Furthermore, the work machine 100 according to another embodiment may be a work machine that does not have a rotating bed.

[0089] The work machine 100 according to the embodiment described above is equipped with a bucket 164 as an attachment for the work implement 160, but this is not limited to this. For example, the work machine 100 according to other embodiments may be equipped with a breaker, fork, grapple, or the like as an attachment. In this case as well, the control device 200 uses the X coordinate system, which extends in the direction in which the cutting edge of the attachment faces, in the same way as the bucket coordinate system. bk Y extending along the axis and cutting edge bk axis and X bk axis and Y bk Z perpendicular to the axis bk The tiltrotator 163 is controlled by a local coordinate system consisting of the axes.

[0090] In another embodiment, the axes of the tiltrotator 163 do not have to be orthogonal to each other as long as they intersect on different planes. Specifically, for axis AX1 related to the joint axis connecting the arm 162 and the attachment portion 1631, axis AX2 related to the joint axis connecting the attachment portion 1631 and the tilt portion 1632, and rotation axis AX3 of the rotation motor 310, when the tilt angle and rotation angle of the tiltrotator 163 are zero, the planes parallel to the axis AX1 and the axis AX2, the planes parallel to the axis AX2 and the axis AX3, and the planes parallel to the axis AX3 and the axis AX1 may all be different.

[0091] Furthermore, the control device 200 according to another embodiment may not have a function for setting a design surface. In this case, the control device 200 can automatically control the tiltrotator 163 by performing bucket attitude maintenance control. For example, the operator can perform simple ground leveling work without setting a design surface. [Explanation of symbols]

[0092] DESCRIPTION OF SYMBOLS 100...Work machine 120...Traveling body 140...Swinging body 160...Work machine 161...Boom 162...Arm 163...Tilt rotator 1631...Mounting section 1632...Tilt section 1633...Rotating section 164...Bucket 180...Operator's cab 200...Control device 210...Processor 211...Operation signal acquisition section 212...Input section 213...Display control section 214...Measurement value acquisition section 215...Position and attitude calculation section 216...Intervention determination section 218...Control signal output section 219...Desired attitude determination section 220...Rotation amount calculation section 230...Main memory 250...Storage 270...Interface 271...Operation device 272...Monitor device 301...Engine 302...Hydraulic pump 303...Control valve 304...Travel motor 305...Swing motor 306...Boom cylinder 307...Arm cylinder 308...Bucket cylinder 309...Tilt cylinder 310...Rotation motor 401...Inclination measuring device 402...Position and direction measuring device 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 including a work implement operably supported on a vehicle body, a tiltrotator attached to a tip of the work implement, and a work implement supported via the tiltrotator so as to be rotatable about three axes intersecting with each other in different planes relative to the work implement, a processor; The processor: Acquire measurements from multiple sensors, calculating a current attitude of the work implement based on the measurements; determining a virtual axis of rotation based on the calculated current attitude of the work implement when a predetermined control start condition is satisfied; generating a control signal for the tiltrotator to rotate the work implement by a predetermined amount about the virtual axis of rotation so as to move from the current attitude to a target attitude; outputting the generated control signal; system.

2. the work implement has a cutting edge; The virtual rotation axis is an axis extending in a direction in which the cutting edge of the work tool faces. The system of claim 1 .

3. The processor: determining a reference axis that is perpendicular to the virtual axis of rotation and extends along the cutting edge of the work implement; A posture in which the reference axis and the vehicle body reference plane are parallel to each other is defined as the target posture. The system of claim 2 .

4. The processor: A predetermined operation signal is received from an operator, when the predetermined operation signal is received as the control start condition, a control signal for the tiltrotator is generated to rotate the work implement by a predetermined amount around the virtual rotation axis so that the current attitude of the work implement becomes the target attitude. A system according to any one of claims 1 to 3.

5. the processor determines the target attitude when the work implement is separated from the ground by a predetermined distance as the control start condition.

10. A system according to claim 1 or claim 4.

6. the virtual rotation axis is different from the joint axis of the tiltrotator, A system according to any one of claims 1 to 5.

7. A method for controlling a work machine including a work implement operably supported on a vehicle body, a tiltrotator attached to a tip of the work implement, and a work implement supported via the tiltrotator so as to be rotatable about three axes intersecting with each other in different planes relative to the work implement, acquiring measurements from a plurality of sensors; calculating a current attitude of the work implement based on the measurements; determining a virtual axis of rotation based on the calculated current attitude of the work implement when a predetermined control initiation condition is satisfied; generating a control signal for the tiltrotator to rotate the work implement a predetermined amount about the virtual axis of rotation from the current attitude to a target attitude; outputting the generated control signal; A method for providing

8. A computer of a control system for a work machine including a work machine operably supported on a vehicle body, a tiltrotator attached to a tip of the work machine, and a work implement supported via the tiltrotator so as to be rotatable about three axes intersecting with each other in different planes relative to the work machine, acquiring measurements from a plurality of sensors; calculating a current attitude of the work implement based on the measurements; determining a virtual axis of rotation based on the calculated current attitude of the work implement when a predetermined control initiation condition is satisfied; generating a control signal for the tiltrotator to rotate the work implement a predetermined amount about the virtual axis of rotation from the current attitude to a target attitude; outputting the generated control signal; A program that executes the following.

Images