Patent Application
20250116085
The present disclosure relates to a system and a method for controlling a work machine.
Priority is claimed on Japanese Patent Application No. 2021-161093, filed Sep. 30, 2021, the content of which is incorporated herein by reference.
Patent Document 1 discloses a technique of moving, in a work machine provided with a tilt bucket whose tooth angle can be inclined, the bucket along an inclined design surface. A tilt axis of the tilt bucket extends in an opening direction of the bucket.
Incidentally, there is known a component called a tilt rotator that rotatably supports an attachment of the work machine around three mutually orthogonal axes. By attaching the tilt rotator to the work machine, the attachment can be oriented in any direction. However, although the tilt rotator has a high degree of freedom of rotation, it is difficult for an operator to operate the tilt rotator. Patent Document 1 makes it possible to automate operations around the tilt axis, but does not disclose control of the work machine with the tilt rotator.
An object of the present disclosure is to provide a system and a method capable of assisting with an operation of a work machine provided with an attachment supported by a support portion via a tilt rotator.
According to one aspect of the present disclosure, there is provided a system for controlling a work machine including a support portion operably supported by a vehicle body, a tilt rotator attached to a tip of the support portion, and an attachment supported rotatably around three axes that intersect each other in different planes, by the support portion via the tilt rotator. The system includes a processor. The processor acquires measurement values from a plurality of sensors. The processor calculates a posture of an attachment with respect to a vehicle body based on the measurement values. The processor generates a control signal for the tilt rotator so that the posture of the attachment is held, based on an operation signal from an operation device, and outputs the generated control signal.
According to the aspect described above, the system can assist with an operation of a work machine provided with an attachment supported by a support portion via a tilt rotator.
Hereinafter, embodiments will be described in detail with reference to the drawings.
The undercarriage 120 travelably supports the work machine 100. The undercarriage 120 is, for example, a pair of left and right endless tracks.
The swing body 140 is supported by the undercarriage 120 so as to be swingable around a swing center. The swing body 140 is an example of a vehicle body. The undercarriage 120 is an example of a base portion that swingably supports the swing body 140.
The work equipment 160 is operably supported by the swing body 140. The work equipment 160 is driven by hydraulic pressure. The work equipment 160 includes a boom 161, an arm 162, a tilt rotator 163, and a bucket 164 that is an attachment. A base end portion of the boom 161 is rotatably attached to the swing body 140. A base end portion of the arm 162 is rotatably attached to a tip portion of the boom 161. The tilt rotator 163 is rotatably attached to a tip portion of the arm 162. The bucket 164 is attached to the tilt rotator 163. The bucket 164 is supported rotatably around three axes that intersect each other in different planes, by the work equipment 160 via the tilt rotator 163. Here, a portion of the swing body 140 to which the work equipment 160 is attached is referred to as a front part. In addition, regarding the swing body 140, based on the front part, an opposite part is referred to as a rear part, a left part is referred to as a left part, and a right part is referred to as a right part. The boom 161 and the arm 162 are examples of a support portion operably supported by the swing body 140.
The cab 180 is provided at the front part of the swing body 140. An operation device 271 for an operator to operate the work machine 100, and a monitor device 272 which is a human-machine interface of the control device 200 are provided in the cab 180. The operation device 271 receives, from the operator, inputs of an operation amount of a traveling motor 304, an operation amount of a swing motor 305, an operation amount of a boom cylinder 306, an operation amount of an arm cylinder 307, an operation amount of a bucket cylinder 308, an operation amount of a tilt cylinder 309, and an operation amount of a rotary motor 310. The operation device 271 outputs an operation signal indicating the operation amount of the work machine. The operation device 271 is operated by the operator and outputs operation signals for operating the boom 161 and the arm 162. The operation device 271 is operated by the operator and outputs an operation signal for swinging the swing body 140 with respect to the undercarriage 120. The operation device 271 is operated by the operator and outputs an operation signal for operating the tilt rotator 163. The monitor device 272 receives an input for setting and releasing bucket posture holding control from the operator. The bucket posture holding control means that the control device 200 automatically controls the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310 to hold a posture of the bucket 164 in a global coordinate system. The monitor device 272 is realized by, for example, a computer including a touch panel.
The control device 200 controls the undercarriage 120, the swing body 140, and the work equipment 160 based on the operation of the operation device 271 by the operator. The control device 200 is provided, for example, inside the cab 180.
The work machine 100 includes a plurality of actuators for driving the work machine 100. Specifically, the work machine 100 includes an engine 301, a hydraulic pump 302, a control valve 303, a pair of the traveling motors 304, the swing motor 305, the boom cylinder 306, the arm cylinder 307, the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310.
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 traveling motor 304, the swing motor 305, the boom cylinder 306, the arm cylinder 307, and the bucket cylinder 308 via the control valve 303.
The control valve 303 controls a flow rate of the hydraulic oil to be supplied from the hydraulic pump 302 to the traveling 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 the hydraulic oil supplied from the hydraulic pump 302 to drive the undercarriage 120.
The swing motor 305 is driven by the hydraulic oil supplied from the hydraulic pump 302 to swing the swing body 140 with respect to the undercarriage 120.
The boom cylinder 306 is a hydraulic cylinder for driving the boom 161. A base end portion of the boom cylinder 306 is attached to the swing body 140. A tip portion 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 portion of the arm cylinder 307 is attached to the boom 161. A tip portion 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. A base end portion of the bucket cylinder 308 is attached to the arm 162. A tip portion of the bucket cylinder 308 is attached to the tilt rotator 163 via a link member.
The tilt cylinder 309 is a hydraulic cylinder for driving the tilt portion 1632. A base end portion of the tilt cylinder 309 is attached to the attachment portion 1631. A tip portion of the tilt cylinder 309 is attached to the tilt portion 1632.
The rotary motor 310 is a hydraulic motor for driving the rotation portion 1633. A bracket and stator of the rotary motor 310 are fixed to the tilt portion 1632. A rotary shaft and rotor of the rotary motor 310 are provided to extend in the up-down direction in the drawing and are fixed to the rotation portion 1633.
The work machine 100 includes a plurality of sensors for measuring a posture, an azimuth direction, and a position of the work machine 100. Specifically, the work machine 100 includes an inclination measurer 401, a position and azimuth direction measurer 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.
The inclination measurer 401 measures a posture of the swing body 140. The inclination measurer 401 measures an inclination (for example, a roll angle, a pitch angle, and a yaw angle) of the swing body 140 with respect to a horizontal plane. As the inclination measurer 401, an inertial measurement unit (IMU) is an exemplary example. In this case, the inclination measurer 401 measures an acceleration and angular velocity of the swing body 140, and calculates the inclination of the swing body 140 with respect to the horizontal plane based on a measurement result. The inclination measurer 401 is installed, for example, below the cab 180. The inclination measurer 401 outputs posture data of the swing body 140 to the control device 200 as a measurement value.
The position and azimuth direction measurer 402 measures a position of a representative point of the swing body 140 and an azimuth direction in which the swing body 140 faces, by a global navigation satellite system (GNSS). The position and azimuth direction measurer 402 includes, for example, two GNSS antennas (not shown) attached to the swing body 140, and detects an azimuth direction orthogonal to a straight line connecting positions of the two antennas as the azimuth direction in which the work machine 100 faces. The position and azimuth direction measurer 402 outputs position data and azimuth direction data of the swing body 140 to the control device 200 as measurement values.
The boom angle sensor 403 measures a boom angle which is an angle of the boom 161 with respect to the swing 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 swing body measured by the inclination measurer 401. The measurement value of the boom angle sensor 403 indicates zero, for example, when a direction of a straight line passing through a base end and a tip of the boom 161 matches the front-rear direction of the swing body 140. The boom angle sensor 403 according to another embodiment may be a stroke sensor attached to the boom cylinder 306. In addition, the boom angle sensor 403 according to another embodiment may be a rotation sensor provided on a joint shaft that rotatably connects the swing body 140 and the boom 161. The boom angle sensor 403 outputs boom angle data to the control device 200 as the measurement value.
The arm angle sensor 404 measures an 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, for example, when a direction of a straight line passing through a base end and a tip of the arm 162 matches the direction of the straight line passing through the base end and the tip of the boom 161. The arm angle sensor 404 according to another embodiment may calculate an angle by attaching a stroke sensor to the arm cylinder 307. In addition, the arm angle sensor 404 according to another embodiment 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 to the control device 200 as the measurement value.
The bucket angle sensor 405 measures a bucket angle which is an angle of the tilt rotator 163 with respect 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 a stroke amount of the bucket cylinder 308. The measurement value of the bucket angle sensor 405 indicates zero, for example, when a direction of a straight line passing through a base end and teeth of the bucket 164 matches the direction of the straight line passing through the base end and the tip of the arm 162. 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 attachment portion 1631 of the tilt rotator 163. In addition, 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 to the control device 200 as the measurement value.
The tilt angle sensor 406 measures a tilt angle which is an angle of the tilt portion 1632 with respect to the attachment portion 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 attachment portion 1631 and the tilt portion 1632. The measurement value of the tilt angle sensor 406 indicates zero, for example, when the rotation axis of the arm 162 and the rotation axis of the rotation portion 1633 are orthogonal to each other. The tilt angle sensor 406 according to another embodiment may calculate an angle by attaching a stroke sensor to the tilt cylinder 309. The tilt angle sensor 406 outputs tilt angle data to the control device 200 as the measurement value.
The rotation angle sensor 407 measures a rotation angle which is an angle of the rotation portion 1633 with respect to the tilt portion 1632 of the tilt rotator 163. The rotation angle sensor 407 may be a rotation sensor provided on the rotary motor 310. The measurement value of the tilt angle sensor 406 indicates zero, for example, when a direction in which the teeth of the bucket 164 face and an operation plane of the work equipment 160 are parallel to each other. The rotation angle sensor 407 outputs rotation angle data to the control device 200 as the measurement value.
The control device 200 is a computer that includes 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 the measurement values from the inclination measurer 401, the position and azimuth direction measurer 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.
The storage 250 is a non-transitory and tangible storage medium. As the storage 250, a magnetic disk, an optical disk, a magneto-optical disk, and a semiconductor memory are exemplary examples. The storage 250 may be internal media directly connected to a bus of the control device 200, or may be external media connected to the control device 200 via the 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.
The storage 250 stores a control program for controlling the work machine 100. The control program may be used for realizing some functions to be performed by the control device 200. For example, the control program may function in combination with another program already stored in the storage 250 or in combination with another program installed in another device. In addition, in another embodiment, the control device 200 may include a custom large scale integrated circuit (LSI) such as a programmable logic device (PLD) in addition to or instead of the above configuration. As the PLD, a programmable array logic (PAL), a generic array logic (GAL), a complex programmable logic device (CPLD), and a field programmable gate array (FPGA) are exemplary examples. In this case, some or all of the functions realized by the processor may be realized by the integrated circuit.
In the storage 250, geometric data representing dimensions and positions of the center of gravity of the swing body 140, the boom 161, the arm 162, and the bucket 164 is recorded. The geometric data is data representing a position of an object in a predetermined coordinate system. In addition, design surface data, which is three-dimensional data representing a shape of the design surface of a construction site in the global coordinate system, is recorded in the storage 250. The global coordinate system is a coordinate system formed of an Xg axis extending in a latitudinal direction, a Yg axis extending in a longitudinal direction, and a Zg axis extending in a vertical direction. The design surface data is represented by, for example, Triangular Irregular Networks (TIN) data.
The processor 210 executes the control program to include an operation signal acquisition unit 211, an input unit 212, a display control unit 213, a measurement value acquisition unit 214, a position and posture calculation unit 215, an intervention determination unit 216, an intervention control unit 217, and a control signal output unit 218.
The operation signal acquisition unit 211 acquires an operation signal indicating an operation amount of each actuator from the operation device 271.
The input unit 212 receives an operation input by the operator from the 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 the measurement values from the inclination measurer 401, the position and azimuth direction measurer 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.
The position and posture calculation unit 215 calculates the position and posture of the work machine 100 in the global coordinate system and in a vehicle body coordinate system based on various measurement values acquired by the measurement value acquisition unit 214 and the geometric data recorded in the storage 250. For example, the position and posture calculation unit 215 calculates a position and posture of the teeth of the bucket 164 in the global coordinate system and in the vehicle body coordinate system. The vehicle body coordinate system is an orthogonal coordinate system whose origin is a representative point (for example, a point passing through the swing center) of the swing body 140. The calculation of the position and posture calculation unit 215 will be described below. The position and posture calculation unit 215 is an example of a posture calculation unit that calculates the posture of the bucket 164 with respect to the swing body 140.
The intervention determination unit 216 determines whether or not to limit a speed of the work equipment 160 based on a positional relationship between the position of the teeth of the bucket 164 calculated by the position and posture calculation unit 215 and the design surface indicated by the design surface data. Hereinafter, limiting the speed of the work equipment 160 by the control device 200 is also referred to as intervention control. Specifically, the intervention determination unit 216 obtains the shortest distance between the design surface and the bucket 164, and determines to perform the intervention control on the work equipment 160 in a case where the shortest distance is equal to or less than a predetermined distance. Specifically, the intervention determination unit 216 rotates and translates the design surface data recorded in the storage 250 based on the measurement values of the inclination measurer 401 and the position and azimuth direction measurer 402, thereby transforming the position of the design surface represented by the global coordinate system to the position of the vehicle body coordinate system. The intervention determination unit 216 specifies, as a control point, a contour point closest to the design surface among a plurality of contour points of the bucket 164. The intervention determination unit 216 specifies a surface (polygon) located vertically below the control point in the design surface data. The intervention determination unit 216 calculates a first design line that is an intersection line between the specified surface and a surface parallel to an Xbk-Zbk plane of a bucket coordinate system passing through the control point. The intervention determination unit 216 determines whether or not a distance between the control point and the first design line is equal to or less than an intervention threshold value. In addition, the intervention determination unit 216 determines whether or not the bucket posture holding control is set based on whether or not the input unit 212 has received an input for setting or releasing the bucket posture holding control from the monitor device 272.
In a case where the intervention determination unit 216 determines to perform the intervention control, the intervention control unit 217 controls an operation amount of an intervention target among the operation amounts acquired by the operation signal acquisition unit 211. In the intervention control, the intervention control unit 217 controls an operation amount of the boom 161 so that the work equipment 160 does not enter a design line. Accordingly, the boom 161 operates such that a speed of the bucket 164 becomes a speed corresponding to a distance between the bucket 164 and the design line. In other words, when the operator operates the arm 162 to perform excavation work, the intervention control unit 217 limits the speed of the teeth of the bucket 164 by raising the boom 161 in accordance with the design surface.
The control signal output unit 218 outputs the operation amount acquired by the operation signal acquisition unit 211 or the operation amount controlled by the intervention control unit 217 to the control valve 303.
Here, a method for calculating a position of a point on an outer shell of the work machine 100 by the position and posture calculation unit 215 will be described. The position and posture calculation unit 215 calculates the position of the point on the outer shell based on various measurement values acquired by the measurement value acquisition unit 214 and the geometric data recorded in the storage 250. In the storage 250, geometric data representing dimensions of the swing body 140, the boom 161, the arm 162, the tilt rotator 163 (the attachment portion 1631, the tilt portion 1632, and the rotation portion 1633), and the bucket 164 is recorded.
The geometric data of the swing body 140 indicates a center position (xbm, ybm, zbm) of the joint shaft by which the swing body 140 supports the boom 161 in the vehicle body coordinate system that is a local coordinate system. The vehicle body coordinate system is a coordinate system formed of an Xsb axis extending in the front-rear direction, a Ysb axis extending in the left-right direction, and a Zsb axis extending in the up-down direction based on the swing center of the swing body 140. The up-down direction of the swing body 140 does not necessarily match the vertical direction.
The geometric data of the boom 161 indicates a position (xam, yam, zam) of the joint shaft by which the boom 161 supports the arm 162 in a boom coordinate system that is a local coordinate system. The boom coordinate system is a coordinate system formed of an Xbm axis extending in a longitudinal direction, a Ybm axis extending in a direction in which the joint shaft extends, and a Zbm axis orthogonal to the Xbm axis and the Ybm axis based on the center position of the joint shaft by which the swing body 140 and the boom 161 are connected.
The geometric data of the arm 162 indicates a position (xt1, yt1, zt1) of the joint shaft by which the arm 162 supports the attachment portion 1631 of the tilt rotator 163 in an arm coordinate system that is a local coordinate system. The arm coordinate system is a coordinate system formed of an Xam axis extending in the longitudinal direction, a Yam axis extending in a direction in which the joint shaft extends, and a Zam axis orthogonal to the Xam axis and the Yam axis based on the center position of the joint shaft by which the boom 161 and the arm 162 are connected.
The geometric data of the attachment portion 1631 of the tilt rotator 163 indicates a position (xt2, yt2, zt2) of the joint shaft by which the attachment portion 1631 supports the tilt portion 1632 and an inclination (Pt) of the joint shaft in a first tilt-rotate coordinate system that is the local coordinate system. The inclination Pt of the joint shaft is an angle related to a design error of the tilt rotator 163, and is obtained by calibration of the tilt rotator 163 or the like. The first tilt-rotate coordinate system is a coordinate system formed of a Yt1 axis extending in a direction in which the joint shaft by which the arm 162 and the attachment portion 1631 are connected extends, a Zt1 axis extending in a direction in which the joint shaft by which the attachment portion 1631 and the tilt portion 1632 are connected extends, and an Xt1 axis orthogonal to the Yu axis and the Zt1 axis based on the center position of the joint shaft by which the arm 162 and the attachment portion 1631 are connected.
The geometric data of the tilt portion 1632 of the tilt rotator 163 indicates a tip position (xt3bk, yt3, zt3) of the rotary shaft of the rotary motor 310 and an inclination (φr) of the rotary shaft in a second tilt-rotate coordinate system that is the local coordinate system. The inclination or of the rotary shaft is an angle related to a design error of the tilt rotator 163, and is obtained by calibration of the tilt rotator 163 or the like. The second tilt-rotate coordinate system is a coordinate system formed of an Xt2 axis extending in a direction in which the joint shaft by which the attachment portion 1631 and the tilt portion 1632 are connected extends, a Zt2 axis extending in a direction in which the rotary shaft of the rotary motor 310 extends, and a Yt2 axis orthogonal to the Xt2 axis and the Zt2 axis based on the center position of the joint shaft by which the attachment portion 1631 and the tilt portion 1632 are connected.
The geometric data of the rotation portion 1633 of the tilt rotator 163 indicates a center position (zt4, yt4, zt4) of the attachment surface of the bucket 164 in a third tilt-rotate coordinate system that is the local coordinate system. The third tilt-rotate coordinate system is a coordinate system formed of a Zt3 axis extending in a direction in which the rotary shaft of the rotary motor 310 extends, and an Xt3 axis and a Yt3 axis orthogonal to the rotary shaft based on the center position of the attachment surface of the bucket 164. The bucket 164 is attached to the rotation portion 1633 such that the teeth are parallel to the Yt3 axis.
The geometric data of the bucket 164 indicates positions (xbk, ybk, zbk) of the plurality of contour points of the bucket 164 in the third tilt-rotate coordinate system. As the contour points, positions of both ends and the center of the teeth of the bucket 164, positions of both ends and the center of a bottom portion of the bucket 164, and positions of both ends and the center of a heel portion of the bucket 164 are exemplary examples.
The position and posture calculation unit 215 generates a boom-vehicle body transformation matrix Tbmsb for performing transformation from the boom coordinate system to the vehicle body coordinate system by using Expression (1), based on the measurement value of the boom angle θbm acquired by the measurement value acquisition unit 214 and the geometric data of the swing body 140. The boom-vehicle body transformation matrix Tbmsb is a matrix for rotation around the Ybm axis by the boom angle θbm and translation by a deviation (xbm, ybm, zbm) between the origin of the vehicle body coordinate system and the origin of the boom coordinate system.
The position and posture calculation unit 215 generates an arm-boom transformation matrix Tambm for performing transformation from the arm coordinate system to the boom coordinate system by using Expression (2), based on the measurement value of the arm angle θam acquired by the measurement value acquisition unit 214 and the geometric data of the boom 161. The arm-boom transformation matrix Tambm is a matrix for rotation by the arm angle θam around the Yam axis and translation by a deviation (xam, yam, zam) between the origin of the boom coordinate system and the origin of the arm coordinate system. In addition, the position and posture calculation unit 215 generates an arm-vehicle body transformation matrix Tamsb for performing transformation from the arm coordinate system to the vehicle body coordinate system by obtaining a product of the boom-vehicle body transformation matrix Tbmsb and the arm-boom transformation matrix Tambm.
The position and posture calculation unit 215 generates a first tilt-arm transformation matrix Tt1am for performing transformation from the first tilt-rotate coordinate system to the arm coordinate system by using Expression (3), based on the measurement value of the bucket angle θbk acquired by the measurement value acquisition unit 214 and the geometric data of the arm 162. The first tilt-arm transformation matrix Tt1am is a matrix for rotation by the bucket angle θbk around the Yt1 axis, translation by a deviation (xt1, yt1, zt1) between the origin of the arm coordinate system and the origin of the first tilt-rotate coordinate system, and further inclination by the inclination or of the joint shaft of the tilt portion 1632. In addition, the position and posture calculation unit 215 generates a first tilt-vehicle body transformation matrix Tt1sb for performing transformation from the first tilt-rotate coordinate system to the vehicle body coordinate system by obtaining a product of the arm-vehicle body transformation matrix Tamsb and the first tilt-arm transformation matrix Tt1am.
The position and posture calculation unit 215 generates a second tilt-first tilt transformation matrix Tt2t1 for performing transformation from the first tilt-rotate coordinate system to the second tilt-rotate coordinate system by using Expression (4), based on the measurement 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 Tt2t1 is a matrix for rotation by the tilt angle θt around the Xt2 axis, translation by a deviation (xt2, yt2, zt2) between the origin of the first tilt-rotate coordinate system and the origin of the second tilt-rotate coordinate system, and further inclination by the inclination or of the rotation axis of the rotation portion 1633. In addition, the position and posture calculation unit 215 generates a second tilt-vehicle body transformation matrix Tt2sb for performing transformation from the second tilt-rotate coordinate system to the vehicle body coordinate system by obtaining a product of the first tilt-vehicle body transformation matrix Tt1sb and the second tilt-first tilt transformation matrix Tt2t1.
The position and posture calculation unit 215 generates a third tilt-second tilt transformation matrix Tt3t2 for performing transformation from the second tilt-rotate coordinate system to the third tilt-rotate coordinate system by using Expression (5), based on the measurement value of the rotation angle θr acquired by the measurement value acquisition unit 214 and the geometric data of the tilt rotator 163. The third tilt-second tilt transformation matrix Tt3t2 is a matrix for rotation by the rotation angle θ; around the Z axis, and translation by a deviation (xt3, yt3, zt3) between the origin of the second tilt-rotate coordinate system and the origin of the third tilt-rotate coordinate system. In addition, the position and posture calculation unit 215 generates a third tilt-vehicle body transformation matrix Tt3sb for performing transformation from the third tilt-rotate coordinate system to the vehicle body coordinate system by obtaining a product of the second tilt-vehicle body transformation matrix Tt2sb and the third tilt-second tilt transformation matrix Tt3t2.
The position and posture calculation unit 215 can obtain the positions of the plurality of contour points of the bucket 164 in the vehicle body coordinate system by obtaining a product of a sum of the positions (xbk, ybk, zbk) of the plurality of contour points in the third tilt-rotate coordinate system indicated by the center position (xt4, yt4, zt4) of the attachment surface of the bucket 164 and the geometric data of the bucket 164, and the third tilt-vehicle body transformation matrix Tbksb.
Incidentally, the angle of the teeth of the bucket 164 with respect to a grounding surface of the work machine 100, that is, an angle formed by an Xsb-Ysb plane of the vehicle body coordinate system and the Yt3 axis of the third tilt-rotate coordinate system, is determined by the boom angle θbm, the arm angle θam, the bucket angle θbk, the tilt angle θt, and the rotation angle θr. Accordingly, as shown in
The position and posture calculation unit 215 generates a bucket-third tilt transformation matrix Tbkt3 for performing transformation from the third tilt-rotate coordinate system to the bucket coordinate system by using Expression (6), based on the geometric data of the tilt rotator 163. The bucket-third tilt transformation matrix Tbkt3 is a matrix for rotation by the inclination or of the rotary shaft around the Yt3 axis.
Hereinafter, the bucket posture holding control according to the first embodiment will be described. The bucket posture holding control is control for holding the posture of the bucket in the global coordinate system. With the bucket posture holding control, the tilt rotator 163 is controlled such that the posture of the bucket in the global coordinate system is held even in a case where at least one of the boom operation, the arm operation, and the swing operation is performed. Specifically, the bucket posture holding control is control for operating at least one of the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310 such that the axial directions of the three axes (the bucket tilt axis Xbk, the bucket pitch axis Ybk, and the bucket rotation axis Zbk) of the bucket coordinate system in the global coordinate system are held.
The measurement value acquisition unit 214 acquires the measurement values of the inclination measurer 401, the position and azimuth direction measurer 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).
The position and posture calculation unit 215 calculates the posture of the bucket in the vehicle body coordinate system based on the measurement values acquired in step S101 (step S102). The posture of the bucket in the vehicle body coordinate system is represented by a posture matrix Reur indicating the directions of respective axes (Xbk, Ybk, Zbk) of the bucket coordinate system in the vehicle body coordinate system. All translation components of the posture matrix Reur representing the posture of the bucket 164 are set to zero.
Next, the intervention determination unit 216 determines whether or not the bucket posture holding control is set (step S103). In the first embodiment, the intervention determination unit 216 determines whether or not the bucket posture holding control is set based on whether or not the input unit 212 has received an input for setting or releasing the bucket posture holding control from the monitor device 272. In a case where the bucket posture holding control is not set (step S103: NO), the control device 200 does not perform the bucket posture holding control. On the other hand, in a case where the bucket posture holding control is set (step S103: YES), the intervention determination unit 216 determines whether or not any operation of the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310 has been received, based on the operation signal from the operation device 271 acquired by the operation signal acquisition unit 211 (step S104).
In a case where any operation of the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310 has been received (step S104: YES), the control device 200 does not perform the bucket posture holding control because it is inferred that the operator has a will to operate the tilt rotator 163. On the other hand, in a case where none of the operations of the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310 have been received (step S104: NO), the intervention control unit 217 obtains a posture matrix Rman representing the posture of the bucket 164 after a unit time (control cycle), based on the operation amounts of the swing motor 305, the boom 161, and the arm 162 acquired by the operation signal acquisition unit 211 from the operation device 271, and the measurement value of the inclination measurer 401 acquired by the measurement value acquisition unit 214 (step S105).
The posture matrix Rman is represented by the current vehicle body coordinate system of the work machine 100. That is, the swinging of the swing motor 305 is reflected in the posture matrix Rman. Next, the intervention control unit 217 obtains a target value θbk_tgt of the angular velocity of the bucket cylinder 308, a target value θt_tgt of the angular velocity of the tilt cylinder 309, and a target value θr_tgt of the angular velocity of the rotary motor 310 using Expressions (7) to (9), by using the posture matrix Reur of the bucket 164 calculated in step S102 and the posture matrix Rman (step S106).
In Expression (7), ε indicates an error threshold value and K indicates a gain. Since a rotation matrix is an orthonormal matrix, a transposed matrix of the rotation matrix is equal to an inverse matrix of the rotation matrix. Therefore, according to Expressions (7) to (9), the intervention control unit 217 can obtain the angular velocities θbk_tgt, θt_tgt, and θr_tgt for offsetting a difference between the current posture Reur of the bucket 164 and the posture Rman of the bucket 164 when the actuator is actuated according to the operation amount acquired by the operation signal acquisition unit 211 from the operation device 271. The intervention control unit 217 generates control signals for the respective actuators (the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310) based on the target values of the angular velocities obtained in step S202 (step S107).
Then, the control signal output unit 218 outputs the control signals for the respective actuators (the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310) generated by the intervention control unit 217 to the control valve 303 (step S108).
In addition, according to the first embodiment, when the operator sets the posture holding control, the posture of the bucket 164 as seen from the global coordinate system can be held constant even when the swing body 140, the boom 161, and the arm 162 are operated. For example, in a case where a place sufficiently higher than the design surface is excavated, the teeth can be easily kept pointed in the excavation direction by maintaining the posture of the bucket 164. In addition, for example, in a case where an attachment such as a grapple is attached to the work equipment 160 instead of the bucket 164 to move a load, the tilt rotator is controlled such that the posture of the attachment in the global coordinate system is held, thereby preventing the load from falling due to the posture change.
In addition, when an operation signal for operating the tilt rotator 163, that is, an operation signal of any of the bucket cylinder 308, the tilt cylinder 309, and the rotary motor 310 is input, the control device 200 causes the intervention control unit 217 not to generate the control signal for the tilt rotator. The fact that the operation signal for operating the tilt rotator 163 is input by the operator means that the operator is highly likely to have a will to operate in the direction in which the bucket 164 faces. Therefore, in such a case, the control device 200 does not generate the control signal for the tilt rotator, so that the operator's operation is not hindered.
In the first embodiment described above, the bucket posture holding control is set by the input unit 212 receiving an input of the setting of the bucket posture holding control from the monitor device 272. That is, in the first embodiment, the bucket posture holding control is started by an operation performed by the operator. However, in another aspect, there is no limitation to this aspect. For example, the control device 200 according to a modification example of the first embodiment may have the following functions.
The intervention determination unit 216 according to the modification example of the first embodiment starts the bucket posture holding control when the bucket 164 comes within a predetermined distance of the ground, as a setting condition for the bucket posture holding control.
For the determination of whether the bucket 164 has come within a predetermined distance of the ground, for example, the function of the intervention determination unit 216 described above can be used. That is, the intervention determination unit 216 obtains the shortest distance between the position of the teeth of the bucket 164 and the design surface from time to time. Then, the intervention determination unit 216 determines that a predetermined control start condition is satisfied when the shortest distance, which is calculated from time to time, becomes equal to or less than a predetermined determination threshold value while the bucket 164 is descending, and starts the process of step S104.
In this way, the operator's operation can be eliminated in executing the bucket posture holding control, and the ground leveling work or the like can be further simplified.
Although one embodiment has been described above in detail with reference to the drawings, the specific configuration is not limited to the above, and various design changes and the like can be made. That is, in another embodiment, the order of the above-described processes may be appropriately changed. In addition, some of the processes may be executed in parallel.
The control device 200 according to the above-described embodiment may be configured of a single computer, or the configurations of the control device 200 may be divided and disposed in a plurality of computers, and the plurality of computers may cooperate with each other to function as the control device 200. In this case, some 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 operation device 271 and the monitor device 272 are provided remotely from the work machine 100, and the configurations of the control device 200 other than the measurement value acquisition unit 214 and the control signal output unit 218 may be provided in the remote server.
In addition, the work machine 100 is a hydraulic excavator according to the above-described embodiment, but is not limited to this. For example, the work machine 100 according to another embodiment may be a work machine that is fixed on the ground and does not travel automatically. In addition, the work machine 100 according to another embodiment may be a work machine that does not have a swing body.
The work machine 100 includes the bucket 164 as the attachment of the work equipment 160 according to the above-described embodiment, but is not limited to this. For example, the work machine 100 according to another embodiment may include a breaker, a fork, a grapple, or the like as the attachment. In this case as well, similarly to the bucket coordinate system, the control device 200 controls the tilt rotator 163 by the local coordinate system formed of the Xbk axis extending in the direction in which the teeth of the attachment face, the Ybk axis extending in the direction along the teeth, and the Zbk axis orthogonal to the Xbk axis and the Ybk axis.
In addition, in another embodiment, the axes of the tilt rotator 163 need not be orthogonal to each other as long as the axes intersect each other in different planes. Specifically, with regard to an axis AX1 related to the joint shaft by which the arm 162 and the attachment portion 1631 are connected, an axis AX2 related to the joint shaft by which the attachment portion 1631 and the tilt portion 1632 are connected, and a rotation axis AX3 of the rotary motor 310, when the tilt angle and the rotation angle of the tilt rotator 163 is zero, a plane parallel to the axes AX1 and AX2, a plane parallel to the axes AX2 and AX3, and a plane parallel to the axes AX3 and AX1 need only be different from each other.
In addition, the control device 200 according to another embodiment may not have a setting function in terms of the design surface. In this case as well, the control device 200 can automatically control the tilt rotator 163 by performing the bucket posture holding control. For example, the operator can execute simple ground leveling work without setting the design surface.
According to the aspect described above, the system can assist with an operation of a work machine provided with an attachment supported by a support portion via a tilt rotator.
| Number | Date | Country | Kind |
|---|---|---|---|
| 2021-161093 | Sep 2021 | JP | national |
| Filing Document | Filing Date | Country | Kind |
|---|---|---|---|
| PCT/JP2022/036490 | 9/29/2022 | WO |
