CONTROL SYSTEM, CONTROL METHOD, AND CONTROL PROGRAM

Abstract

A work machine includes an undercarriage, a swing body swingably supported by the undercarriage, and work equipment operably supported by the swing body. A control system controls the work machine. The control system includes a processor. The processor generates a design plane that is defined by a plane on a vehicle body coordinate system of which origin is a representative point of a swing body. The processor rotationally converts the design plane around the origin of the vehicle body coordinate system in association with a swing of the swing body. The processor specifies a position of the work equipment in the vehicle body coordinate system. The processor controls the work equipment based on the specified position of the work equipment and the design plane.

Description

BACKGROUND

Technical Field

The present disclosure relates to a control system, a control method, and a control program.

Background Information

As disclosed in Japanese Patent No. 5654144, a technique is known that controls work equipment such that a bucket included in a work machine does not enter beyond a design plane showing a target shape of an excavation target.

SUMMARY

In the technique described in Japanese Patent No. 5654144, a control device can recognize the position of work equipment in a global coordinate system by using the GNSS, so that the control of the teeth with respect to the design plane can be performed. However, it is not always possible to refer to the global coordinate system depending on the visibility environment of a satellite or a configuration of the work machine. For example, in a case where the work machine works indoors, there is a case where the visibility of the satellite is poor and the GNSS cannot be referred to.

An object of the present disclosure is to provide a control system, a control method, and a control program capable of generating a design plane for controlling work equipment without referring to a global coordinate system.

According to a first aspect of the present invention, a control system controls a work machine including an undercarriage configured to travel, a swing body configured to be swingably supported by the undercarriage, and work equipment configured to be operably supported by the swing body. The control system includes a processor. The processor generates a design plane that is defined by a plane on a vehicle body coordinate system of which origin is a representative point of a swing body. The processor rotationally converts the design plane around the origin of the vehicle body coordinate system in association with a swing of the swing body. The processor specifies a position of the work equipment in the vehicle body coordinate system. The processor controls the work equipment based on the specified position of the work equipment and the design plane.

According to a second aspect of the present invention, a control method of a work machine including an undercarriage configured to travel, a swing body configured to be swingably supported by the undercarriage, and work equipment configured to be operably supported by the swing body, includes a generation step, a rotation conversion step, a specifying step, and a control step. The generation step generates a design plane defined by a plane on a vehicle body coordinate system of which origin is a representative point of the swing body. The rotation conversion step rotationally converts the design plane around the origin of the vehicle body coordinate system in association with a swing of the swing body. The specifying step specifies a position of the work equipment in the vehicle body coordinate system. The control step controls the work equipment based on the specified position of the work equipment and the design plane.

According to a third aspect of the present invention, a control program, which is a control program executed in a computer of a work machine including an undercarriage configured to travel, a swing body configured to be swingably supported by the undercarriage, and work equipment configured to be operably supported by the swing body, executes a generation step, a rotation conversion step, a specifying step, and a control step. The generation step generates a design plane defined by a plane on a vehicle body coordinate system of which origin is a representative point of the swing body. The rotation conversion step rotationally converts the design plane around the origin of the vehicle body coordinate system in association with a swing of the swing body. The specifying step specifies a position of the work equipment in the vehicle body coordinate system. The control step controls the work equipment based on the specified position of the work equipment and the design plane.

According to at least one of the aspects described above, it is possible to generate a design plane for controlling work equipment without referring to a global coordinate system.

BRIEF DESCRIPTION OF DRAWINGS

FIG. 1 is a schematic diagram showing a configuration of a work machine according to a first embodiment.

FIG. 2 is a diagram showing a drive system of the work machine according to the first embodiment.

FIG. 3 is a schematic block diagram showing a configuration of a control device according to the first embodiment.

FIG. 4 is a diagram showing an example of resetting a design plane in accordance with a swing of a swing body in the first embodiment.

FIG. 5 is a flowchart showing a method of setting the design plane according to the first embodiment.

FIG. 6 is a flowchart showing an update and intervention control of the design plane in accordance with the swing set in the first embodiment.

FIG. 7 is a flowchart showing update processing of the design plane performed by the control device according to the first embodiment.

FIG. 8 is a diagram showing a change in the design plane before and after movement of the work machine in the first embodiment.

FIG. 9 is a view showing movement of the design plane in the first embodiment.

DETAILED DESCRIPTION OF EMBODIMENT(S)

First Embodiment

(Configuration of Work Machine)

Hereinafter, an embodiment of the present invention is described in detail with reference to the drawings.

FIG. 1 is a schematic diagram showing a 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 an undercarriage 120, a swing body 140, work equipment 160, a cab 180, and a control device 200. The work machine 100 according to the first embodiment generates a planar design plane by an operation of an operator, and is controlled such that teeth do not exceed the design plane. At this time, since the design plane is set in a vehicle body coordinate system, construction using the design plane can be realized even in a case where positioning is not possible by GNSS or the like, such as a case where the work machine 100 constructs a tunnel.

The undercarriage 120 travellably 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 to be swingable around a swing center. The swing body 140 is an example of a vehicle body of the work machine 100.

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, and a bucket 163 that is a work implement. The proximal end portion of the boom 161 is rotatably attached to the swing body 140. The proximal end portion of the arm 162 is rotatably attached to the distal end portion of the boom 161. The proximal end portion of the bucket 163 is rotatably attached to the distal end portion of the arm 162. Here, the portion of the swing body 140 to which the work equipment 160 is attached is referred to as a front portion. In addition, in the swing body 140, a portion on an opposite side, a portion on a left side, and a portion on a right side with respect to the front portion are referred to as a rear portion, a left portion, and a right portion.

The cab 180 is provided at the front portion of the swing body 140. An operation device 141 for an operator to operate the work machine 100, and a monitor device 142 that is a man-machine interface of the control device 200 are provided in the cab 180. The monitor device 142 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 an operation of the operation device by the operator. The control device 200 is provided, for example, inside the cab 180.

(Drive System of Work Machine 100)

FIG. 2 is a diagram showing a drive system of the work machine 100 according to the first embodiment.

The work machine 100 includes a plurality of actuators for driving the work machine 100. Specifically, the work machine 100 includes an engine 111, a hydraulic pump 112, a control valve 113, a pair of traveling motors 114, a swing motor 115, a boom cylinder 116, an arm cylinder 117, and a bucket cylinder 118.

The engine 111 is a prime mover that drives the hydraulic pump 112.

The hydraulic pump 112 is driven by the engine 111 and supplies hydraulic oil to the traveling motor 114, the swing motor 115, the boom cylinder 116, the arm cylinder 117, and the bucket cylinder 118 via the control valve 113.

The control valve 113 controls the flow rate of the hydraulic oil to be supplied from the hydraulic pump 112 to the traveling motor 114, the swing motor 115, the boom cylinder 116, the arm cylinder 117, and the bucket cylinder 118.

The traveling motor 114 is driven by the hydraulic oil supplied from the hydraulic pump 112 and drives the undercarriage 120.

The swing motor 115 is driven by the hydraulic oil supplied from the hydraulic pump 112 and causes the swing body 140 to swing with respect to the undercarriage 120.

The boom cylinder 116 is a hydraulic cylinder that drives the boom 161. The proximal end portion of the boom cylinder 116 is attached to the swing body 140. The distal end portion of the boom cylinder 116 is attached to the boom 161.

The arm cylinder 117 is a hydraulic cylinder to drive the arm 162. The proximal end portion of the arm cylinder 117 is attached to the boom 161. The distal end portion of the arm cylinder 117 is attached to the arm 162.

The bucket cylinder 118 is a hydraulic cylinder to drive the bucket 163. The proximal end portion of the bucket cylinder 118 is attached to the arm 162. The distal end portion of the bucket cylinder 118 is attached to the bucket 163.

(Measurement System of Work Machine 100)

The work machine 100 includes a plurality of sensors to measure the posture and position of the work machine 100. Specifically, the work machine 100 includes an inclination measurer 101, a swing angle sensor 102, a boom angle sensor 103, an arm angle sensor 104, and a bucket angle sensor 105.

The inclination measurer 101 measures the posture of the swing body 140. The inclination measurer 101 measures the inclination (for example, roll angle, pitch angle, and yaw angle) of the swing body 140 with respect to a horizontal plane. As the inclination measurer 101, an inertial measurement unit (IMU) is an exemplary example. In this case, the inclination measurer 101 measures the acceleration and angular speed of the swing body 140 and calculates the inclination with respect to the horizontal plane of the swing body 140 based on the measurement result. The inclination measurer 101 is installed, for example, below the cab 180. The inclination measurer 101 outputs the posture data of the swing body 140, which is a measurement value, to the control device 200.

The swing angle sensor 102 measures the swing angle of the swing body 140 with respect to the undercarriage 120. The measurement value of the swing angle sensor 102 indicates zero, for example, when the directions of the undercarriage 120 and the swing body 140 match each other. The swing angle sensor 102 is installed, for example, at the swing center of the swing body 140. The swing angle sensor 102 outputs swing angle data, which is the measurement value, to the control device 200.

The boom angle sensor 103 measures a boom angle, which is the rotation angle of the boom 161 with respect to the swing body 140. The boom angle sensor 103 may be an IMU attached to the boom 161. In this case, the boom angle sensor 103 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 101. The measurement value of the boom angle sensor 103 indicates zero, for example, when the direction of a straight line passing through the proximal end and the distal end of the boom 161 coincides with the front to rear direction of the swing body 140. Incidentally, the boom angle sensor 103 according to another embodiment may be a stroke sensor attached to the boom cylinder 116. In addition, the boom angle sensor 103 according to another embodiment may be a rotation sensor provided on a pin that connects the swing body 140 and the boom 161. The boom angle sensor 103 outputs boom angle data, which is the measurement value, to the control device 200.

The arm angle sensor 104 measures an arm angle, which is the rotation angle of the arm 162 with respect to the boom 161. The arm angle sensor 104 may be an IMU attached to the arm 162. In this case, the arm angle sensor 104 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 103. The measurement value of the arm angle sensor 104 indicates zero, for example, when the direction of the straight line passing through the proximal end and the distal end of the arm 162 coincides with the direction of the straight line passing through the proximal end and the distal end of the boom 161. Incidentally, the arm angle sensor 104 according to another embodiment may perform angle calculation by attaching a stroke sensor to the arm cylinder 117. In addition, the arm angle sensor 104 according to another embodiment may be a rotation sensor provided on a pin that connects the boom 161 and the arm 162. The arm angle sensor 104 outputs arm angle data, which is the measurement value, to the control device 200.

The bucket angle sensor 105 measures a bucket angle, which is the rotation angle of the bucket 163 with respect to the arm 162. The bucket angle sensor 105 may be a stroke sensor provided in the bucket cylinder 118 to drive the bucket 163. In this case, the bucket angle sensor 105 measures the bucket angle based on the stroke amount of the bucket cylinder. The measurement value of the bucket angle sensor 105 indicates zero, for example, when the direction of the straight line passing through the proximal end and the teeth of the bucket 163 coincides with the direction of the straight line passing through the proximal end and the distal end of the arm 162. Incidentally, the bucket angle sensor 105 according to another embodiment may be a rotation sensor provided on a pin that connects the arm 162 and the bucket 163. In addition, the bucket angle sensor 105 according to another embodiment may be an IMU attached to the bucket 163. The bucket angle sensor 105 outputs bucket angle data, which is the measurement value, to the control device 200.

(Configuration of Control Device 200)

FIG. 3 is a schematic block diagram showing a 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 the inclination measurer 101, the swing angle sensor 102, the boom angle sensor 103, the arm angle sensor 104, and the bucket angle sensor 105.

The storage 250 is a non-transitory, tangible storage medium. As the storage 250, magnetic disks, optical disks, magneto-optical disks, semiconductor memories, or the like are exemplary examples. The storage 250 may be an internal medium that is directly connected to a bus of the control device 200 or may be an external medium connected to the control device 200 via the interface 270 or a communication line. The storage 250 stores a control program to control the work machine 100.

The control program may realize some of functions to be exhibited 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 implemented in another device. Incidentally, 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 the above configuration or instead of the above configuration. Exemplary examples of the PLD include a programmable array logic (PAL), a generic array logic (GAL), a complex programmable logic device (CPLD), and a field programmable gate array (FPGA). In this case, part or all of the functions realized by the processor may be realized by the integrated circuit.

In the storage 250, geometry data representing dimensions and positions of the centers of gravity of the swing body 140, the boom 161, the arm 162, and the bucket 163 is recorded. The geometry data is data representing the position of an object in a predetermined coordinate system.

(Software Configuration)

By executing the control program, the processor 210 includes an operation amount acquisition unit 211, an input unit 212, a display control unit 213, a measurement value acquisition unit 214, a position specifying unit 215, a generation unit 216, a rotation conversion unit 217, an intervention determination unit 218, an intervention control unit 219, a control signal output unit 220, and an update unit 221.

The operation amount acquisition unit 211 acquires an operation signal indicating an operation amount of each actuator from the operation device 141.

The input unit 212 receives an operation input by an operator from the monitor device 142.

The display control unit 213 outputs screen data to be displayed on the monitor device 142 to the monitor device 142.

The measurement value acquisition unit 214 acquires measurement values from the inclination measurer 101, the swing angle sensor 102, the boom angle sensor 103, the arm angle sensor 104, and the bucket angle sensor 105.

The position specifying unit 215 specifies the position of the teeth of the bucket 163 in 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. The vehicle body coordinate system is a Cartesian coordinate system of which origin is a representative point of the swing body 140 (for example, a point passing through the swing center). The calculation of the position specifying unit 215 will be described later.

When the input unit 212 receives a generation instruction of the design plane from the operator, the generation unit 216 calculates parameters of the design plane based on the position of the teeth of the bucket 163 specified by the position specifying unit 215. The generation unit 216 records the generated parameter of the design plane in the vehicle body coordinate system in the main memory 230.

The rotation conversion unit 217 updates the parameter of the design plane stored in the main memory 230 in association with the swing of the swing body 140. Specifically, the rotation conversion unit 217 rotationally converts the parameter of the design plane about the origin of the vehicle body coordinate system by the amount of change in the pitch angle, the roll angle, and the yaw angle that are measured by the inclination measurer 101. FIG. 4 is a diagram showing an example of the resetting a design plane in accordance with the swing of a swing body in the first embodiment. For example, as represented in FIG. 4, in a case where the swing body 140 swings after the design plane is set, the rotation conversion unit 217 calculates the amount of change in the roll angle, the pitch angle, and the yaw angle caused by the swing of the swing body 140 by referring to the measurement value of the inclination measurer 101 acquired by the measurement value acquisition unit 214, and rotationally converts the parameter of the design plane about the origin of the vehicle body coordinate system. As a result, the rotation conversion unit 217 can cancel the rotation of the design plane caused by the swing of the swing body 140.

The intervention determination unit 218 determines whether or not to limit the speed of the work equipment 160 based on the positional relationship between the teeth of the bucket 163 specified by the position specifying unit 215 and the design plane. 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 218 obtains a minimum distance between the design plane and the bucket 163, and determines to perform the intervention control on the work equipment 160 in a case where the corresponding minimum distance is equal to or less than a predetermined distance.

When the intervention determination unit 218 determines to perform the intervention control, the intervention control unit 219 controls the operation amount of the intervention target in the operation amounts acquired by the operation amount acquisition unit 211. In the intervention control, the intervention control unit 219 controls the operation amount of the boom 161 such that the work equipment 160 does not enter the control line. As a result, the boom 161 is driven such that the speed of the bucket 163 becomes a speed in accordance with the distance between the bucket 163 and the control line. That is, when the operator operates the arm 162 to perform excavation, the intervention control unit 219 limits the speed of the teeth of the bucket 163 by raising the boom 161 in accordance with the design plane.

The control signal output unit 220 outputs the operation amount acquired by the operation amount acquisition unit 211 or the operation amount controlled by the intervention determination unit 218 to the control valve 113.

The update unit 221 updates the parameter of the design plane stored in the main memory 230 in association with the travel of the work machine 100. Specifically, before and after the travel of the work machine 100, the operator operates the work equipment 160 and causes the teeth of the bucket 163 to touch a specific position on the site. The update unit 221 moves the design plane based on a difference in position of the teeth of the bucket 163 in the vehicle body coordinate system before and after the travel.

(Calculation of Position Specifying Unit 215)

Here, a specifying method of the position of the teeth of the bucket 163 by the position specifying unit 215 will be described. The position specifying unit 215 specifies the position of the teeth of the bucket 163 based on the various measurement values acquired by the measurement value acquisition unit 214 and the geometry data recorded in the storage 250. In the storage 250, geometry data representing dimensions of the swing body 140, the boom 161, the arm 162, and the bucket 163 is recorded.

The geometry data of the swing body 140 indicates positions (x_bm, y_bm, and z_bm) of the pin that supports the boom 161 of the swing body 140 in the vehicle body coordinate system that is the local coordinate system. The vehicle body coordinate system is a coordinate system configured by an X_sb axis extending in a front to rear direction, a Y_sb axis extending in a right to left direction, and a Z_sb axis extending in an up and down direction with the swing center of the swing body 140 as a reference. Incidentally, the up and down direction of the swing body 140 does not necessarily coincide with the vertical direction.

The geometry data of the boom 161 indicates a boom top position (x_am, y_am, and z_am) in the boom coordinate system that is the local coordinate system. The boom coordinate system is a coordinate system configured by an X_bm axis extending in a longitudinal direction, a Y_bm axis extending in a direction in which the pin extends, and a Z_bm axis orthogonal to the X_bm axis and the Y_bm axis, with the position of the pin that connects the boom 161 and the swing body 140 as a reference. The boom top is the position of the pin that connects the boom 161 and the arm 162.

The geometry data of the arm 162 indicates the arm top position (x_bk, y_bk, and z_bk) in the arm coordinate system that is the local coordinate system. The arm coordinate system is a coordinate system configured by an X_am axis extending in a longitudinal direction, a Y_am axis extending in a direction in which the pin extends, and a Z_am axis orthogonal to the X_am axis and the Y_am axis, with the position of the pin that connects the arm 162 and the boom 161 as a reference. The arm top is the position of the pin that connects the arm 162 and the bucket 163.

The geometry data of the bucket 163 indicates the position (x_ed, y_ed, and z_ed) of the teeth of the bucket 163 in the bucket coordinate system that is the local coordinate system. The bucket coordinate system is a coordinate system configured by an X_bk axis extending in a direction of the teeth, a Y_bk axis extending in a direction in which the pin extends, and a Z_bk axis orthogonal to the X_bk axis and the Y_bk axis, with the position of the pin that connects the bucket 163 and the arm 162 as a reference.

The position specifying unit 215 generates a boom-vehicle body conversion matrix T^bm_sb for the conversion from the boom coordinate system to the vehicle body coordinate system by the following Formula (1) based on the measurement value of a boom angle θ_bm acquired by the measurement value acquisition unit 214 and the geometry data of the swing body 140. The boom-vehicle body conversion matrix T^bm_sb is a matrix that performs rotation by the boom angle θ_bm around the Y_bm axis and performs movement by a deviation (x_bm, y_bm, and z_bm) between the origin of the vehicle body coordinate system and the origin of the boom coordinate system. In addition, the position specifying unit 215 obtains the position of the boom top in the vehicle body coordinate system by obtaining a product of the position of the boom top in the boom coordinate system indicated by the geometry data of the boom 161 and the boom-vehicle body conversion matrix T^bm_sb.

$\begin{array}{cc}{T}_{\mathrm{sb}}^{\mathrm{bm}}=\left[\begin{array}{cccc}\cos {\theta }_{\mathrm{bm}}& 0& \sin {\theta }_{\mathrm{bm}}& x_{\mathrm{bm}}\\ 0& 1& 0& y_{\mathrm{bm}}\\ -s\mathrm{in}{\theta }_{\mathrm{bm}}& 0& \cos {\theta }_{\mathrm{bm}}& z_{\mathrm{bm}}\\ 0& 0& 0& 1\end{array}\right]& \left(1\right)\end{array}$

The position specifying unit 215 generates an arm-boom conversion matrix T^am_bm for the conversion from the arm coordinate system into the boom coordinate system by the following Formula (2) based on the measurement value of the arm angle θ_am acquired by the measurement value acquisition unit 214 and the geometry data of the boom 161. The arm-boom conversion matrix T^am_bm is a matrix that performs rotation by the arm angle θ_am around the Y_am axis and performs movement by a deviation (x_am, y_am, and z_am) between the origin of the boom coordinate system and the origin of the arm coordinate system. In addition, the position specifying unit 215 generates an arm-vehicle body conversion matrix T^am_sb for the conversion from the arm coordinate system into the vehicle body coordinate system by obtaining a product of the boom-vehicle body conversion matrix T^bm_sb and the arm-boom conversion matrix T^am_bm. In addition, the position specifying unit 215 obtains the position of the arm top in the vehicle body coordinate system by obtaining the product of the position of the arm top in the arm coordinate system indicated by the geometry data of the arm 162 and the arm-vehicle body conversion matrix T^am_sb.

$\begin{array}{cc}{T}_{\mathrm{bm}}^{\mathrm{am}}=\left[\begin{array}{cccc}\cos {\theta }_{\mathrm{am}}& 0& \sin {\theta }_{\mathrm{am}}& x_{\mathrm{am}}\\ 0& 1& 0& y_{\mathrm{am}}\\ -\sin {\theta }_{\mathrm{am}}& 0& \cos {\theta }_{\mathrm{am}}& z_{\mathrm{am}}\\ 0& 0& 0& 1\end{array}\right]& \left(2\right)\end{array}$

The position specifying unit 215 generates a bucket-arm conversion matrix T^bk_am for the conversion from the bucket coordinate system into the arm coordinate system by the following Formula (3) based on the measurement value of a bucket angle θ_bk acquired by the measurement value acquisition unit 214 and the geometry data of the arm 162. The bucket-arm conversion matrix T^bk_am is a matrix that performs rotation by the bucket angle θ_bk around the Y_bk axis and performs movement by a deviation (x_bk, y_bk, and z_bk) between the origin of the arm coordinate system and the origin of the bucket coordinate system. In addition, the position specifying unit 215 generates a bucket-vehicle body conversion matrix T^bk_sb for the conversion from the bucket coordinate system into the vehicle body coordinate system by obtaining a product of the arm-vehicle body conversion matrix T^am_sb and the bucket-arm conversion matrix T^bk_am.

$\begin{array}{cc}{T}_{\mathrm{am}}^{bk}=\left[\begin{array}{cccc}\cos {\theta }_{\mathrm{bk}}& 0& \sin {\theta }_{bk}& x_{\mathrm{bk}}\\ 0& 1& 0& y_{\mathrm{bk}}\\ -\sin {\theta }_{\mathrm{bk}}& 0& \cos {\theta }_{\mathrm{bk}}& z_{\mathrm{bk}}\\ 0& 0& 0& 1\end{array}\right]& \left(3\right)\end{array}$

The position specifying unit 215 obtains the position of the teeth of the bucket 163 in the vehicle body coordinate system by obtaining the product of the position of the teeth in the bucket coordinate system indicated by the geometry data of the bucket 163 and the bucket-vehicle body conversion matrix T^bk_sb.

(Control Method of Work Machine 100)

Hereinafter, a control method of the work machine 100 according to the first embodiment will be described.

First, the operator of the work machine 100 operates the monitor device 142 and sets the design plane.

(Setting of Design Plane)

FIG. 5 is a flowchart showing a method of setting the design plane according to the first embodiment.

When the input unit 212 receives an instruction for setting the design plane from the monitor device 142, the display control unit 213 displays a guidance screen including a distance input field, an inclination angle input field, and a setting button on the monitor device 142 (step S101). On the guidance screen, the instructions to move the teeth of the bucket 163 above the point at which the design plane is to be set, to input a distance from the teeth to the design plane in the distance input field and an inclination angle of the design plane in the inclination angle input field, and to operate the setting button are displayed. As initial values, a distance of 0 meters, a pitch angle of 0 degrees, and a roll angle of 0 degrees are input in the distance input field and the inclination angle input field. Hereinafter, the distance input in the distance input field is referred to as an input distance, and the inclination angle input in the inclination angle input field is referred to as an input inclination angle (input pitch angle and input roll angle). The operator operates the work machine 100, moves the teeth of the bucket 163 to a desired position, and then operates the setting button. The input unit 212 receives inputs into the distance input field and the inclination angle input field and the operation of the setting button from the monitor device 142 (step S102). The input unit 212 acquires the values of the distance input field and the inclination angle input field at the time point when the setting button is operated (step S103). Incidentally, the input inclination angle is an inclination angle with the vertical direction and the front of the work machine 100 when the design plane is set as a reference. That is, the input pitch angle and the input roll angle are the inclination of the normal line of the design plane with respect to the vertical axis.

The measurement value acquisition unit 214 acquires the measurement values of the inclination measurer 101, the swing angle sensor 102, the boom angle sensor 103, the arm angle sensor 104, and the bucket angle sensor 105 at the time point when the setting button is operated (step S104). The position specifying unit 215 specifies the position of the teeth of the bucket 163 in the vehicle body coordinate system based on the acquired measurement value (step S105).

The generation unit 216 calculates the parameters of the design plane based on the roll angle and the pitch angle (measurement roll angle and measurement pitch angle) acquired from the inclination measurer 101 in the step S104, the position of the teeth obtained in the step S105, and the input distance and the input inclination angle acquired in the step S103. The generation unit 216 obtains a vertical vector in the vehicle body coordinate system by rotating a vector in which the value of the X_sb axis is 0, the value of the Y_sb axis is 0, and the value of the Z_sb axis is 1 only by the measurement roll angle and the measurement pitch angle (step S106). The generation unit 216 obtains a position vector of the design plane by obtaining a sum of the vector indicating the position of the teeth obtained in the step S104 and a depth vector obtained by multiplying the vertical vector by the distance (step S107). In addition, the generation unit 216 obtains a normal vector of the design plane based on the vertical vector and the input inclination angle (step S108). Specifically, the generation unit 216 specifies a vertical coordinate system, which is a Cartesian coordinate system, that shares an origin with the vehicle body coordinate system and is configured by a Z_v axis extending in the vertical direction, an X_v axis that coincides with the X_sb axis of the vehicle body coordinate system when the measurement roll angle and the measurement pitch angle are zero, and a Y_v axis that coincides with the Y_sb axis of the vehicle body coordinate system when the measurement roll angle and the measurement pitch angle are zero. That is, the vertical coordinate system coincides with the vehicle body coordinate system when the measurement roll angle and the measurement pitch angle are zero. The generation unit 216 rotates the vertical coordinate system around the Y_v axis by an input pitch angle. In addition, the generation unit 216 rotates the vertical coordinate system around the X_v axis by an input roll angle. The generation unit 216 obtains the normal vector of the design plane in the vertical coordinate system by obtaining an outer product of a unit vector extending in the X_v axis direction of the vertical coordinate system rotated around the Y_v axis and a unit vector extending in the Y_v axis direction of the vertical coordinate system rotated around the X_v axis. The generation unit 216 obtains the normal vector of the design plane in the vehicle body coordinate system by rotating the normal vector in the vertical coordinate system only by the measurement roll angle and the measurement pitch angle.

The generation unit 216 records the parameters (normal vector and position vector) of the generated design plane in the main memory 230 (step S109). Incidentally, in a case where the parameter of the design plane has already been recorded in the main memory 230, the old parameter is overwritten with a new parameter.

(Update and Intervention Control of Design Plane in Accordance with Swing)

The work machine 100 can perform work within a range that the work equipment 160 reaches by swinging the swing body 140. Therefore, an operator usually swings the work machine 100 when work such as excavation is performed. Since the vehicle body coordinate system considers the swing body 140 as a reference, a positional relationship between the design plane set in the vehicle body coordinate system and the work equipment 160 does not change as the work machine 100 swings. Therefore, in a case where the design plane is not updated while being set in the above procedure, the design plane behaves to move by following the swing of the swing body 140 as viewed from the viewpoint of the global coordinate system. For example, when a design plane that becomes a down slope when viewed from the swing body 140 is generated, the inclination direction of the design plane as viewed from the swing body 140 is always maintained to be a down slope regardless of how the swing body 140 is swung.

Therefore, the control device 200 according to the first embodiment performs rotation conversion processing of the design plane to maintain the position of the design plane in the global coordinate system before and after the work machine 100 swings.

FIG. 6 is a flowchart showing the update and intervention control of the design plane in accordance with the swing set in the first embodiment. When the operator of the work machine 100 sets the design plane by operating the monitor device 142, the control device 200 starts the controls represented below.

The operation amount acquisition unit 211 acquires operation signals of the boom 161, the arm 162, the bucket 163, and the swing body 140 from the operation device 141 (step S201). The measurement value acquisition unit 214 acquires measurement values of the inclination measurer 101, the swing angle sensor 102, the boom angle sensor 103, the arm angle sensor 104, and the bucket angle sensor 105 (step S202).

The rotation conversion unit 217 rotationally converts and updates the design plane stored in the main memory 230 based on the roll angle, the pitch angle, and the yaw angle of the swing body 140 acquired from the inclination measurer 101 in the step S202 (step S203).

The position specifying unit 215 calculates the position of the teeth of the bucket 163 in the vehicle body coordinate system based on the measurement value acquired in the step S202 (step S204). The intervention determination unit 218 specifies a cross-section that passes through the position of the teeth calculated in the step S204 and is parallel to the X_sb-Z_sb plane of the vehicle body coordinate system (step S205).

The intervention determination unit 218 calculates an intersection line between the cross-section and the design plane generated in the step S205 as a control line (step S206). The intervention determination unit 218 obtains the distance between the teeth of the bucket 163 and the control line (step S207). The intervention determination unit 218 determines whether or not the distance between the teeth and the control line is longer than an intervention start distance (step S208). When the distance is longer than the intervention start distance (step S208: YES), the intervention control unit 219 does not perform the intervention control on the work equipment 160.

On the other hand, when the shortest distance is equal to or less than the intervention start distance (step S208: NO), the intervention control unit 219 calculates the target speed of the boom 161, the arm 162, and the bucket 163 based on the operation signals of the boom 161, the arm 162, and the bucket 163 acquired in the step S201 (step S209). The intervention control unit 219 calculates the moving speed of the teeth of the bucket 163 based on the target speeds of the boom 161, the arm 162, and the bucket 163 and the geometry data (step S210).

The intervention control unit 219 specifies the speed limit of the teeth of the bucket 163 based on the distance calculated in the step S207 and the predetermined speed limit table (step S211). The angular speed limit table is a function indicating a relationship between the distance between the teeth and the control line and the speed limit of the teeth, and is a function in which the shorter the distance, the smaller the speed limit. The intervention control unit 219 determines whether or not the speed of the teeth calculated in the step S210 exceeds the speed limit (step S212). When the speed of the teeth exceeds the speed limit (step S212: YES), the intervention control unit 219 calculates the speed of the boom 161 for matching the speed of the teeth with the speed limit and sets the target speed of the boom (step S213). When the speed of the teeth does not exceed the speed limit (step S212: NO), the intervention control unit 219 does not perform the intervention control on the work equipment 160.

The control signal output unit 220 generates a control signal based on the target speeds of the boom 161, the arm 162, and the bucket 163 and the target angular speed of the swing body 140, and outputs the control signal to the control valve 113 (step S214).

(Update of Design Plane in Accordance with Movement)

The construction site of the work machine 100 does not usually fit within a range that the work equipment 160 reaches by the swing of the swing body 140. Therefore, the operator causes the work machine 100 to travel and perform the construction of the site while a position of the work machine 100 is moved. Since the design plane according to the first embodiment is set in the vehicle body coordinate system, in a case where the position of the work machine 100 moves, the design plane behaves to move by following the swing body 140 when viewed from the viewpoint of the global coordinate system. For example, in a case where a pitch angle θ is set on the design plane, the height of the design plane should change by tan θ every meter, but the height of the design plane does not change although the work machine 100 moves by 1 meter.

Therefore, the control device 200 according to the first embodiment performs the update processing of the design plane represented in FIG. 7 to maintain the position of the design plane in the global coordinate system before and after the work machine 100 is moved.

FIG. 7 is a flowchart showing the update processing of the design plane performed by the control device according to the first embodiment.

The operator operates the monitor device 142 and inputs an execution instruction of the update processing when the work machine 100 is moved during the construction of the design plane. When the input unit 212 of the control device 200 receives an execution instruction of the update processing from the monitor device 142, the display control unit 213 displays a first guidance screen including the setting button on the monitor device 142 (step S301). On the guidance screen, the instructions to assign the teeth of the bucket 163 to the target object that can be touched by the bucket 163 in common before and after the movement and to operate the setting button are displayed. The operator operates the work machine 100, assigns the teeth of the bucket 163 on the target object, and then operates the setting button. The input unit 212 receives the operation of the setting button from the monitor device 142 (step S302).

The measurement value acquisition unit 214 acquires the measurement values of the inclination measurer 101, the swing angle sensor 102, the boom angle sensor 103, the arm angle sensor 104, and the bucket angle sensor 105 at the time point (first time) when the setting button of the first guidance screen is operated (step S303). The position specifying unit 215 specifies the position of the teeth of the bucket 163 in the vehicle body coordinate system based on the acquired measurement value (step S304). That is, the position specifying unit 215 specifies the position of the target object in the vehicle body coordinate system at the first time. The position specifying unit 215 records the specified position of the teeth in the main memory 230.

Next, the display control unit 213 displays a second guidance screen including the setting button on the monitor device 142 (step S305). On the guidance screen, the instructions to travel the work machine 100 to move to a desired position, to assign the teeth of the bucket 163 to the same target object, and to operate the setting button are displayed. The operator operates the work machine 100 and causes the work machine 100 to travel.

While the operator operates the work machine 100, the measurement value acquisition unit 214 acquires measurement values of the inclination measurer 101, the swing angle sensor 102, the boom angle sensor 103, the arm angle sensor 104, and the bucket angle sensor 105 (step S306). The update unit 221 determines whether or not the setting button has been operated (step S307). When the setting button has not been operated (step S307: NO), that is, when the movement to the desired position has not been completed, the rotation conversion unit 217 rotationally converts and updates the design plane stored in the main memory 230 based on the measurement value of the inclination measurer 101 (step S308). Then, the control device 200 returns the processing to the step S306, and repeats the processing until the setting button is operated.

When the setting button is operated (step S307: YES), that is, when the movement to the desired position is completed, the position specifying unit 215 specifies the position of the teeth of the bucket 163 in the vehicle body coordinate system based on the measurement value acquired by the measurement value acquisition unit 214 (step S309). That is, the position specifying unit 215 specifies the position of the target object in the vehicle body coordinate system at the time point (second time) when the setting button of the second guidance screen is operated.

Next, the update unit 221 calculates a translation vector that is a difference between the position vector indicating the position of the teeth specified in the step S304 and the position vector indicating the position of the teeth specified in the step S309 (step S310). The update unit 221 moves and updates the design plane stored in the main memory 230 by using the calculated translation vector (step S311). As a result, the update unit 221 can maintain the position of the design plane in the global coordinate system before and after traveling.

(Actions and Effects)

Here, design plane update processing performed by the update unit 221 will be described with reference to the drawings. FIG. 8 is a diagram showing a change in the design plane before and after the movement of the work machine 100 in the first embodiment. In the example represented in FIG. 8, the design plane has a pitch angle. The operator assigns the teeth of the bucket 163 to a target object tgt at a time t1 and then causes the work machine 100 to travel rearward by a distance L. Since a design plane s is defined in the vehicle body coordinate system, the relative positional relationship between the swing body 140 and the design plane s is maintained although the work machine 100 moves. Therefore, in the viewpoint of the global coordinate system, a deviation occurs between a design plane s1 before the movement of the work machine 100 and a design plane s2 after the movement. At this time, the relative positional relationship between the position of the teeth of the bucket 163 and the swing body 140 recorded at the time t1 is also maintained similarly to the design plane s.

FIG. 9 is a view showing the movement of the design plane in the first embodiment. Thereafter, at a time t2, the operator assigns the teeth of the bucket 163 to the target object tgt again. The update unit 221 calculates a translation vector v showing the amount of change in the position of the teeth from the position of the teeth at the time t1 and the position of the teeth at the time t2. The translation vector v corresponds to a movement amount of the work machine 100 as represented in FIG. 8. Therefore, the update unit 221 updates the design plane s2 to a design plane s3 by moving the design plane s2 after the movement by using the translation vector v. The design plane s3 after the movement is equal to the design plane s1 before the movement of the work machine 100, from the viewpoint of the global coordinate system.

In this manner, the control device 200 according to the first embodiment moves the design plane based on a difference between the position of the teeth in the vehicle body coordinate system when the teeth of the bucket 163 are positioned at a reference point (for example, the target object) at the site at the first time, and the position of the teeth when the teeth are positioned at the reference point at the second time. As a result, the control device 200 can maintain the position of the design plane in the global coordinate system although the position of the work machine 100 changes due to traveling.

In addition, the control device 200 according to the first embodiment rotationally converts the design plane between the first time and the second time, based on the measurement value of the posture of the swing body 140. As a result, the control device 200 can maintain the position of the design plane in the global coordinate system although the posture of the work machine 100 changes due to the movement of the work machine 100. Incidentally, in another embodiment, when the work machine 100 can always maintain the same posture, the control device 200 may not perform the rotation conversion of the design plane. As the work machine 100 that always maintains the same posture, the work machine 100 or the like that travels on a rail of a straight line shape without twisting is an exemplary example.

Another Embodiment

The embodiments have been described above in detail with reference to the drawings: however, the specific configurations are not limited to the above-described configurations, and various design changes or the like can be made. That is, in another embodiment, the order of the above-described processing may be appropriately changed. In addition, some of the processing may be executed in parallel.

The control device 200 according to the above-described embodiment may be configured by a single computer, or the configuration of the control device 200 may be disposed to be divided into a plurality of computers, and the plurality of computers may function as the control device 200 by cooperating with each other. In this case, a part of the computers constituting the control device 200 may be mounted inside the work machine 100, and another computer may be provided outside the work machine 100.

According to at least one of the aspects described above, it is possible to generate a design plane for controlling work equipment without referring to a global coordinate system.

Claims

1. A control system that controls a work machine including an undercarriage configured to travel, a swing body configured to be swingably supported by the undercarriage, and work equipment configured to be operably supported by the swing body, the control system comprising: a processor that generates a design plane that is defined by a plane on a vehicle body coordinate system of which origin is a representative point of the swing body,rotationally converts the design plane around the origin of the vehicle body coordinate system in association with a swing of the swing body,specifies a position of the work equipment in the vehicle body coordinate system, andcontrols the work equipment based on the specified position of the work equipment and the design plane.

2. The control system according to claim 1, wherein the processor acquires a measurement value from an inclination measurer that measures the posture of the swing body,calculates an amount of change in a posture caused by the swing of the swing body based on the measurement value, androtationally converts the design plane based on the amount of change in the posture.

3. The control system according to claim 1, wherein the processor moves the design plane based on a difference between a first position that is a position of the work equipment in the vehicle body coordinate system when the work equipment is positioned at a reference point of a site at a first time, anda second position that is a position of the work equipment in the vehicle body coordinate system when the work equipment is positioned at the reference point at a second time.

4. The control system according to claim 3, wherein the first time is a time before traveling by the undercarriage, andthe second time is a time after traveling by the undercarriage.

5. A control method of a work machine including an undercarriage configured to travel, a swing body configured to be swingably supported by the undercarriage, and work equipment configured to be operably supported by the swing body, the control method comprising: generating a design plane that is defined by a plane on a vehicle body coordinate system of which origin is a representative point of the swing body;rotationally converting the design plane around the origin of the vehicle body coordinate system in association with a swing of the swing body;specifying a position of the work equipment in the vehicle body coordinate system; andcontrolling the work equipment based on the specified position of the work equipment and the design plane.

6. A control program causing a computer of a work machine including an undercarriage configured to travel, a swing body configured to be swingably supported by the undercarriage, and work equipment configured to be operably supported by the swing body, to execute: a step of generating a design plane that is defined by a plane on a vehicle body coordinate system of which origin is a representative point of the swing body;a step of rotationally converting the design plane around the origin of the vehicle body coordinate system in association with a swing of the swing body;a step of specifying a position of the work equipment in the vehicle body coordinate system; anda step of controlling the work equipment based on the specified position of the work equipment and the design plane.