SYSTEM AND METHOD FOR SETTING VEHICLE BODY COORDINATE SYSTEM IN WORK MACHINE

Description

CROSS-REFERENCE TO RELATED APPLICATIONS

This application is a U.S. National stage application of International Application No. PCT/JP2023/0030262, filed on Aug. 23, 2023. This U.S. National stage application claims priority under 35 U.S.C. § 119(a) to Japanese Patent Application No. 2022-146072, filed in Japan on Sep. 14, 2022, the entire contents of which are hereby incorporated herein by reference.

BACKGROUND

Field of Invention

The present invention relates to a system and a method for setting a vehicle body coordinate system in a work machine.

Background Information

A work machine includes a second vehicle body, a first vehicle body that is rotatably connected to the second vehicle body, and a work implement that is movably attached to the first vehicle body. The first vehicle body rotates with respect to the second vehicle body and the work implement works whereby the work machine is able to perform construction work such as excavation. Conventionally, a technique is known for detecting the position of the work implement. For example, the work machine in Japanese Patent Laid-open No. 2012-233353 includes a positional sensor such as a global navigation satellite system (GNSS). The positional sensor detects a position in a global coordinate system in which the positional sensor is attached in the work machine. The positions in the global coordinate system are a coordinate system measured by GNSS and are a coordinate system based on a point of origin fixed in the world.

In addition, a controller of the work machine calculates the position of the work implement in a vehicle body coordinate system from the position of the positional sensor. The vehicle body coordinate system is a coordinate system based on the work machine. For example, the controller calculates the position of the work implement with respect to the positional sensor based on machine parameters such as the dimensions of the component parts of the vehicle body of the work machine and the angle of the work implement.

SUMMARY

In order to precisely calculate the position of the work implement as discussed above, the vehicle body coordinate system of the work machine needs to be set with precision. The following is an example of a method for setting the vehicle body coordinate system of a work machine.

Firstly, a first target prism is attached on the first vehicle body. The first vehicle body is rotated whereby a plurality of positions of the first target prism are measured by an external position measurement device. The plurality of positions of the first target prism are input to the controller and the controller calculates the rotational plane of the first vehicle body based on the plurality of positions of the first target prism.

Next, a second target prism is attached on the work implement. The work implement is moved up and down with respect to the first vehicle body whereby a plurality of positions of the second target prism are measured by the external position measurement device. The plurality of positions of the second target prism are input to the controller and the controller calculates a work implement plane that the work implement moves in based on the plurality of positions of the second target prism. The controller then calculates, based on the rotational plane and the work implement plane, the vehicle body coordinate system based on a point of origin virtually set on the first vehicle body.

However, in the abovementioned method for setting the vehicle body coordinate system in the work machine, an error in the vehicle body coordinate system would be produced due to a measurement error when rotating the first vehicle body. A large work area is required to rotate the first vehicle body. Moreover, the number of working hours increases in order to rotate the first vehicle body. An object of the present invention is to set the vehicle body coordinate system with precision without rotating the first vehicle body in a work machine.

A system according to an aspect of the present invention is a system for setting a vehicle body coordinate system in a work machine. The work machine includes a second vehicle body, a first vehicle body that is rotatably connected to the second vehicle body, and a work implement that is movably attached to the first vehicle body. The vehicle body coordinate system is a coordinate system based on the first vehicle body. The system includes an attitude sensor, a position measurement device, and a controller. The attitude sensor detects the attitude of the first vehicle body. The position measurement device measures the position of a first target part included in the work implement. The controller acquires, from the attitude sensor, the attitude of the first vehicle body while the first vehicle body is stationary with respect to the second vehicle body. The controller sets the vehicle body coordinate system based on the attitude of the first vehicle body and the position of the first target part.

A method according to another aspect of the present invention is a method for setting a vehicle body coordinate system in a work machine. The work machine includes a second vehicle body, a first vehicle body that is rotatably connected to the second vehicle body, and a work implement that is movably attached to the first vehicle body. The vehicle body coordinate system is a coordinate system based on the first vehicle body. The method includes acquiring, from an attitude sensor that detects the attitude of the first vehicle body, the attitude of the first vehicle body while the first vehicle body is stationary with respect to the second vehicle body, measuring the position of a first target part included in the work implement, and setting the vehicle body coordinate system based on the attitude of the first vehicle body and the position of the first target part.

According to the present invention, the vehicle body coordinate system is set based on the attitude of the first vehicle body detected by the attitude sensor instead of deriving a rotational plane of the first vehicle body. As a result, the vehicle body coordinate system can be set with precision without rotating the first vehicle body in a work machine.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a side view of a work machine.

FIG. 2 is a block diagram illustrating a configuration of the work machine and a control system thereof.

FIG. 3 is a schematic side view illustrating an external coordinate system and the vehicle body coordinate system of the work machine.

FIG. 4 is a schematic rear view illustrating the external coordinate system and the vehicle body coordinate system of the work machine.

FIG. 5 is a schematic top view illustrating the external coordinate system and the vehicle body coordinate system of the work machine.

FIG. 6 is a flow chart illustrating processing for calibrating machine parameters.

FIG. 7 illustrates an example of a first target part.

FIG. 8 illustrates an example of a second target part.

FIG. 9 illustrates an example of the second target part.

FIG. 10 illustrates an example of the second target part.

FIG. 11 illustrates a method for evaluating the precision of the calibration.

FIG. 12 illustrates an example of automatic control of the work machine using a detected blade tip position.

FIG. 13 illustrates an example of an assistant screen of the work machine using the detected blade tip position.

DETAILED DESCRIPTION OF EMBODIMENT(S)

A control system for a work machine 1 according to an embodiment is discussed hereinbelow with reference to the drawings. FIG. 1 is a side view of the work machine 1. In the present embodiment, the work machine 1 is an excavator such as a hydraulic shovel.

As illustrated in FIG. 1, the work machine 1 includes a body 2 and a work implement 3. The work implement 3 is attached to a front part of the body 2. The body 2 includes a first vehicle body 4 and a second vehicle body 5. The first vehicle body 4 is rotatably connected to the second vehicle body 5. A cab 6 is disposed on the first vehicle body 4. The second vehicle body 5 includes crawler belts 7. Only one of the left and right crawler belts 7 is illustrated in FIG. 1. The work machine 1 travels due to the crawler belts 7 being driven.

The work implement 3 is attached to the first vehicle body 4 so as to be able to move up and down. The work implement 3 includes a boom 11, an arm 12, and a bucket 13. The boom 11 is attached to the first vehicle body 4 so as to be rotatable around a boom pin 28. The arm 12 is attached to the boom 11 so as to be rotatable around an arm pin 29. The bucket 13 is attached to the arm 12 so as to be rotatable around a bucket pin 30.

The work implement 3 includes a boom cylinder 14, an arm cylinder 15, and a bucket cylinder 16. The boom cylinder 14, the arm cylinder 15, and the bucket cylinder 16 are, for example, hydraulic cylinders. The boom cylinder 14, the arm cylinder 15, and the bucket cylinder 16 are driven with hydraulic fluid discharged from a belowmentioned hydraulic pump 22. The boom cylinder 14 actuates the boom 11. The arm cylinder 15 actuates the arm 12. The bucket cylinder 16 actuates the bucket 13.

FIG. 2 is a block diagram illustrating a configuration of the work machine 1 and a control system of the work machine 1. As illustrated in FIG. 2, the work machine 1 includes a driving source 21, the hydraulic pump 22, a power transmission device 23, and a controller 24. The driving source 21 is controlled by instruction signals from the controller 24. The driving source 21 is, for example, an internal combustion engine. Alternatively, the driving source may include a driving source such as an electric motor or a hydrogen engine. The hydraulic pump 22 is driven by the driving source 21 and discharges hydraulic fluid. The hydraulic fluid discharged from the hydraulic pump 22 is supplied to the boom cylinder 14, the arm cylinder 15, and the bucket cylinder 16.

The work machine 1 includes a rotation motor 25. The rotation motor 25 is, for example, a hydraulic motor. The rotation motor 25 is driven by hydraulic fluid discharged from the hydraulic pump 22. Alternatively, the rotation motor 25 may be an electric motor. The rotation motor 25 causes the first vehicle body 4 to rotate. While only one hydraulic pump is illustrated in FIG. 2, a plurality of hydraulic pumps may be provided.

The hydraulic pump 22 is a variable displacement pump. A pump control device 26 is connected to the hydraulic pump 22. The pump control device 26 controls the tilt angle of the hydraulic pump 22. The pump control device 26 includes, for example, an electromagnetic valve and is controlled by instruction signals from the controller 24. The controller 24 controls the displacement of the hydraulic pump 22 by controlling the pump control device 26.

The work machine 1 includes a control valve 27. The hydraulic pump 22, the cylinders 14 to 16, and the rotation motor 25 are connected to each other by a hydraulic circuit via the control valve 27. The control valve 27 is controlled by instruction signals from the controller 24. The control valve 27 controls the flow rate of the hydraulic fluid supplied from the hydraulic pump 22 to the cylinders 14 to 16 and to the rotation motor 25. The controller 24 controls the operation of the work implement 3 by controlling the control valve 27. The controller 24 controls the rotation of the first vehicle body 4 by controlling the control valve 27. The cylinders 14 to 16 are not limited to hydraulic cylinders and may be mechanical cylinders driven by an electric motor.

The power transmission device 23 transmits the driving power of the driving source 21 to the second vehicle body 5. The crawler belts 7 are driven by the driving power from the power transmission device 23 whereby the work machine 1 is made to travel. The power transmission device 23, for example, may be a transmission having a torque converter or a plurality of speed change gears. Alternatively, the power transmission device 23 may be a transmission of another type such as a hydrostatic transmission (HST) or a hydraulic mechanical transmission (HMT).

The controller 24 includes a processor 31 such as a CPU. The processor 31 performs processing for controlling the work machine 1. The controller 24 includes a storage device 32. The storage device 32 includes a memory such as a RAM or a ROM, and an auxiliary storage device such as a hard disk drive (HDD) or a solid state drive (SSD). The storage device 32 stores data and programs for the control of the work machine 1.

The control system includes an operating device 33. The operating device 33 is operable by an operator. The operating device 33 includes, for example, a lever, a pedal, or a switch and the like. The operating device 33 outputs, to the controller 24, an operation signal corresponding to an operation by the operator. The controller 24 controls the control valve 27 so as to actuate the work implement 3 in response to the operation of the operating device 33 by the operator. The controller 24 controls the control valve 27 so as to cause the first vehicle body 4 to rotate in response to an operation of the operating device 33 by the operator. The controller 24 controls the driving source 21 and the power transmission device 23 so as to cause the work machine 1 to travel in response to the operation of the operating device 33 by the operator.

The control system includes an input device 34 and a display 35. The input device 34 is operable by the operator. The input device 34 is, for example, a touchscreen. However, the input device 34 may include hardware keys. The operator inputs various settings related to the work machine 1 by operating the input device 34. The input device 34 outputs an input signal corresponding to the operation by the operator. The display 35 is an LCD, an OELD, or another type of display. The display 35 displays a screen in accordance with the display signals from the controller 24.

The control system includes a positional sensor 36, an attitude sensor 37, and a work implement sensor 38. The positional sensor 36 detects the position of the body 2. The position of the body 2 is represented by an external coordinate system. The external coordinate system is a coordinate system based on the outside of the work machine 1. The external coordinate system is, for example, a global coordinate system based on a global navigation satellite system (GNSS). Alternatively, the external coordinate system may be a local coordinate system within the work site where the work machine 1 is used.

FIG. 3 is a schematic side view illustrating the work machine 1, an external coordinate system X1-Y1-Z1, and a vehicle body coordinate system X2-Y2-Z2. FIG. 4 is a schematic rear view illustrating the work machine 1, the external coordinate system X1-Y1-Z1, and the vehicle body coordinate system X2-Y2-Z2. FIG. 5 is a schematic top view illustrating the work machine 1, the external coordinate system X1-Y1-Z1, and the vehicle body coordinate system X2-Y2-Z2.

As illustrated in FIG. 3, the positional sensor 36 includes an antenna 41 and a receiver 42. The antenna 41 is attached to the body 2. The receiver 42 detects a position (referred to below as “antenna position”) P1 of the antenna 41 in the external coordinate system X1-Y1-Z1. The receiver 42 outputs antenna position data indicating the antenna position P1 in the external coordinate system X1-Y1-Z1. There may be a plurality of antenna positions.

The attitude sensor 37 is attached to the first vehicle body 4. The attitude sensor 37 detects the attitude of the first vehicle body 4. The attitude of the first vehicle body 4 includes a yaw angle θ_y1, a pitch angle θ_p1, and a roll angle θ_r1 of the first vehicle body 4. As illustrated in FIG. 3, the pitch angle θ_p1 of the first vehicle body 4 is an inclination angle in the front-back direction of the first vehicle body 4. As illustrated in FIG. 4, the roll angle θ_r1 of the first vehicle body 4 is an inclination angle in the left-right direction of the first vehicle body 4. As illustrated in FIG. 5, the yaw angle θ_y1 of the first vehicle body 4 is the bearing in the front-back direction of the first vehicle body 4. The attitude sensor 37 includes, for example, an inertial measurement unit (IMU). The attitude sensor 37 outputs first attitude data that represents the attitude of the first vehicle body 4.

The work implement sensor 38 detects the attitude of the work implement 3. The work implement 3 is attached to the first vehicle body 4 in a manner that allows movement within a work implement plane 50 illustrated in FIGS. 4 and 5. The work implement plane 50 is a plane in which the work implement 3 passes through when moving. For example, the work implement plane 50 is a virtual plane that passes through the center of the work implement 3 in the left-right direction and extends in the up-down direction and the front-back direction of the first vehicle body 4. The work implement plane 50 is not limited to passing through the center in the left-right direction of the work implement 3 and may pass through a position offset from the center in the left or right direction. The boom 11, the arm 12, and the bucket 13 rotate within the work implement plane 50 whereby the attitude of the work implement 3 changes.

As illustrated in FIG. 3, the attitude of the work implement 3 includes the boom angle θ1, the arm angle θ1, and a bucket angle θ3. The work implement sensor 38 outputs second attitude data which indicates the boom angle θ1, the arm angle θ2, and the bucket angle θ3. The boom angle θ1 is the angle of the boom 11 with respect to the up-down direction of the vehicle body coordinate system of the vehicle body 2. The arm angle θ2 is the angle of the arm 12 with respect to the boom 11. The bucket angle θ3 is the angle of the bucket 13 with respect to the arm 12.

Specifically, the work implement sensor 38 includes a boom angle sensor 38A, an arm angle sensor 38B, and a bucket angle sensor 38C as illustrated in FIG. 3. The boom angle sensor 38A detects the boom angle θ1. The arm angle sensor 38B detects the arm angle θ2. The bucket angle sensor 38C detects the bucket angle θ3.

Specifically, the boom angle sensor 38A detects the stroke length of the boom cylinder 14. The arm angle sensor 38B detects the stroke length of the arm cylinder 15. The bucket angle sensor 38C detects the stroke length of the bucket cylinder 16. The respective rotation angles θ1 to θ3 of the boom 11, the arm 12, and the bucket 13 are calculated from the stroke lengths of the cylinders 14 to 16. Alternatively, the boom angle sensor 38A, the arm angle sensor 38B, and the bucket angle sensor 38C may be sensors that directly detect the respective rotation angles θ1 to θ3 of the boom 11, the arm 12, and the bucket 13. Alternatively, the boom angle sensor 38A, the arm angle sensor 38B, and the bucket angle sensor 38C may be IMUs.

The controller 24 acquires operation signals from the operating device 33. The controller 24 receives input signals from the input device 34. The controller 24 outputs display signals to the display 35. The controller 24 receives the antenna position data from the positional sensor 36. The controller 24 receives the first attitude data from the attitude sensor 37. The controller 24 receives the second attitude data from the work implement sensor 38.

The controller 24 calculates the position of the work implement 3 based on the received data and machine parameters. Specifically, the controller 24 calculates the position (referred to below as “blade tip position”) P2 of the blade tip of the bucket 13 based on the received data and the machine parameters. The controller 24 calculates the blade tip position P2 in the abovementioned external coordinate system X1-Y1-Z1.

The machine parameters are recorded in the storage device 32. The machine parameters define the positional relationship between the antenna 41 and the blade tip position P2 of the work implement 3 in the vehicle body coordinate system X2-Y2-Z2. As illustrated in FIGS. 3 to 5, the vehicle body coordinate system X2-Y2-Z2 is a coordinate system based on a virtual point of origin provided to the first vehicle body 4. The X2 axis of the vehicle body coordinate system X2-Y2-Z2 indicates the front-back direction of the first vehicle body 4. The Y2 axis of the vehicle body coordinate system X2-Y2-Z2 indicates the left-right direction of the first vehicle body 4. The Z2 axis of the vehicle body coordinate system X2-Y2-Z2 indicates the up-down direction of the first vehicle body 4.

The machine parameters include an antenna parameter and a work implement parameter. The antenna parameter indicates the relative position of the antenna position P1 with respect to a reference position such as the boom pin 28 in the vehicle body coordinate system X2-Y2-Z2. The reference position is not limited to the boom pin 28 and may be another position. As illustrated in FIGS. 3 and 4, the antenna parameter includes a distance Lx between the antenna position P1 and the boom pin 28 in the axis X2 direction, a distance Ly in the Y2 axis direction, and a distance Lz in the Z2 axis direction of the vehicle body coordinate system X2-Y2-Z2.

As illustrated in FIG. 3, the work implement parameter includes the length L1 of the boom 11, the length L2 of the arm 12, and the length L3 of the bucket 13. Specifically the length L1 of the boom 11 is the distance between the boom pin 28 and the arm pin 29. The length L2 of the arm 12 is the distance between the arm pin 29 and the bucket pin 30. The length of the bucket 13 is the distance between the bucket pin 30 and the blade tip of the bucket 13.

The controller 24 calculates the blade tip position P2 in the external coordinate system X1-Y1-Z1 based on the antenna position P1 in the external coordinate system X1-Y1-Z1, the machine parameters, the first attitude data, and the second attitude data. For example, the controller 24 calculates the positional relationship between the antenna position P1 and the blade tip position P2 in the vehicle body coordinate system X2-Y2-Z2 based on the machine parameters and the second attitude data. The controller 24 calculates the positional relationship between the external coordinate system X1-Y1-Z1 and the vehicle body coordinate system X2-Y2-Z2 from the first attitude data. The controller 24 then converts the blade tip position P2 in the vehicle body coordinate system X2-Y2-Z2 to the blade tip position P2 in the external coordinate system X1-Y1-Z1 from the positional relationship between the antenna position P1 and the blade tip position P2 in the vehicle body coordinate system X2-Y2-Z2 and the positional relationship between the external coordinate system X1-Y1-Z1 and the vehicle body coordinate system X2-Y2-Z2.

As described above, the controller 24 calculates the blade tip position P2 in the external coordinate system X1-Y1-Z1 from the antenna position P1 in the external coordinate system X1-Y1-Z1 detected by the positional sensor 36.

Next, processing for calibrating the machine parameters used to calculate the blade tip position P2 will be explained. FIG. 6 is a flow chart illustrating processing for calibrating the machine parameters. The parameters to be calibrated may be all of the abovementioned machine parameters or may be a portion thereof.

Firstly, in steps S101 to S104, the controller 24 sets the vehicle body coordinate system X2-Y2-Z2 in the work machine 1. As illustrated in FIG. 6, the controller 24 acquires the position of a first target part in step S101. The first target part is a portion to be measured for setting the vehicle body coordinate system X2-Y2-Z2 and is included in the work implement 3. As illustrated in FIG. 7, the first target part is, for example, the bucket pin 30. However, the first target part may be the arm pin 29. Alternatively, the first target part may be a specific portion included in the boom 11, the arm 12, or the bucket 13. The controller 24 acquires at least three mutually different positions of the bucket pin 30 as the first target part.

The positions of the first target part are measured by an external position measurement device 45. The position measurement device 45 measures positions in the external coordinate system X1-Y1-Z1 to be measured. The position measurement device 45 is, for example, a laser tracker. Alternatively, the position measurement device 45 may be another position measurement device such as a total station or a stereo camera. The operator of the work machine 1 moves the work implement 3 within the work implement plane 50 with respect to the first vehicle body 4 while the first vehicle body 4 is not rotated with respect to the second vehicle body 5 and is stationary. At this time, a plurality of mutually different positions of the first target part are measured by the position measurement device 45.

The plurality of measured positions of the first target part are input to the controller 24. The position measurement device 45 is able to communicate with the controller 24 and transmits data indicating the plurality of positions of the first target part to the controller 24. Alternatively, the data indicating the plurality of positions of the first target part may be input to the controller 24 by manual input via the input device 34.

In step S102, the controller 24 acquires the first attitude data. The first attitude data includes the pitch angle θ_p1 of the first vehicle body 4 detected by the attitude sensor 37 as discussed above.

In step S103, the controller 24 calculates the work implement plane 50. The work implement plane 50 includes the plurality of positions of the first target part. When the plurality of positions of the first target part are four or more points, the work implement plane 50 may be a least square plane calculated from the plurality of positions. The work implement plane 50 is expressed below in equation (1). In the following equation (1), a₁, b₁, and c₁ indicate normal vector in the work implement plane 50. The controller 24 calculates the normal vector of the work implement plane 50 based on the plurality of positions of the first target part.

a 1 ⁢ x + b 1 ⁢ y + c 1 ⁢ z + d = 0 ( 1 )

In step S104, the controller 24 sets the vehicle body coordinate system X2-Y2-Z2. The controller 24 determines the direction of the Y2 axis in the vehicle body coordinate system X2-Y2-Z2 based on the normal vector of the work implement plane 50. The controller 24 determines the direction of the X2 axis in the vehicle body coordinate system X2-Y2-Z2 based on the pitch angle θ_p1 of the first vehicle body 4 detected by the attitude sensor 37. The controller 24 then determines the direction of the Z2 axis in the vehicle body coordinate system X2-Y2-Z2 based on the direction of the Y2 axis and the direction of the X2 axis. Specifically, the controller 24 sets the vehicle body coordinate system X2-Y2-Z2 by calculating a rotational matrix R of the vehicle body coordinate system X2-Y2-Z2 with respect to the external coordinate system X1-Y1-Z1 as indicated below.

The rotational matrix R of the vehicle body coordinate system X2-Y2-Z2 is expressed by the following equation (2) using the yaw angle θ_Y2,the pitch angle θ_p2, and the roll angle θ_r2 as Euler angles that indicate the rotational attitude of the vehicle body coordinate system X2-Y2-Z2.

R = [ cos ⁢ θ y ⁢ 2 ⁢ cos ⁢ θ p ⁢ 2 - sin ⁢ θ y ⁢ 2 ⁢ cos ⁢ θ r ⁢ 2 + cos ⁢ θ y ⁢ 2 ⁢ sin ⁢ θ p ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 sin ⁢ θ y ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 + cos ⁢ θ y ⁢ 2 ⁢ sin ⁢ θ p ⁢ 2 ⁢ cos ⁢ θ r ⁢ 2 sin ⁢ θ y ⁢ 2 ⁢ cos ⁢ θ p ⁢ 2 cos ⁢ θ y ⁢ 2 ⁢ cos ⁢ θ r ⁢ 2 + sin ⁢ θ y ⁢ 2 ⁢ sin ⁢ θ p ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 - cos ⁢ θ y ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 + sin ⁢ θ y ⁢ 2 ⁢ sin ⁢ θ p ⁢ 2 ⁢ cos ⁢ θ r ⁢ 2 - sin ⁢ θ p ⁢ 2 cos ⁢ θ p ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 cos ⁢ θ p ⁢ 2 ⁢ cos ⁢ θ r ⁢ 2 ] ( 2 )

The direction vector of the Y2 axis in the vehicle body coordinate system X2-Y2-Z2 matches the abovementioned normal vector of the work implement plane 50. Therefore, the following equation (3) is established from the direction vector of the Y2 axis in equation (2) and the normal vector in the work implement plane 50.

[ - sin ⁢ θ y ⁢ 2 ⁢ cos ⁢ θ r ⁢ 2 + cos ⁢ θ y ⁢ 2 ⁢ sin ⁢ θ p ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 cos ⁢ θ y ⁢ 2 ⁢ cos ⁢ θ r ⁢ 2 + sin ⁢ θ y ⁢ 2 ⁢ sin ⁢ θ p ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 cos ⁢ θ p ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 ] = [ a 1 b 1 c 1 ] ( 3 )

The pitch angle θ_p2 of the vehicle body coordinate system X2-Y2-Z2 matches the pitch angle θ_p1 detected by the attitude sensor 37. Therefore, by expanding the equation (3) in which the pitch angle θ_p1 detected by the attitude sensor 37 is substituted by the pitch angle θ_p2 in equation (3), the pitch angle θ_p2, the roll angle θ_r2, and the yaw angle θ_y2 can each be derived as Euler angles that indicate the rotational attitude of the vehicle body coordinate system X2-Y2-Z2 as expressed in the following equations (4), (5), and (6).

θ p ⁢ 2 = θ p ⁢ 1 ( 4 ) θ r ⁢ 2 = arcsin ⁢ c 1 cos ⁢ θ p ⁢ 2 ( 5 ) θ y ⁢ 2 = arcsin ( - a 1 ⁢ cos ⁢ θ r ⁢ 2 + b 1 ⁢ sin ⁢ θ p ⁢ 2 ⁢ sin ⁢ θ r ⁢ 2 cos ⁢ θ r ⁢ 2 2 + sin ⁢ θ p ⁢ 2 2 ⁢ sin ⁢ θ r ⁢ 2 2 ) ( 6 )

The rotational matrix R of the vehicle body coordinate system X2-Y2-Z2 is found by substituting the respective yaw angle θ_y2, the pitch angle θ_p2, and the roll angle θ_r2 in the above equation (2).

As illustrated in FIG. 6, the controller 24 next uses the set vehicle body coordinate system X2-Y2-Z2 to calibrate the machine parameters in steps S105 and S106. In step S105, the controller 24 acquires the position of a second target part. The second target part is a portion to be measured for calibrating the machine parameters.

As illustrated in FIG. 8, the second target part includes the boom pin 28 and the antenna position P1. The controller 24 acquires the boom pin 28 and the antenna position P1. As illustrated in FIG. 9, the second target part includes the bucket pin 30. The operator moves the work implement 3 thereby acquiring a plurality of mutually different positions of the bucket pin 30. As illustrated in FIG. 10, the second target part includes the blade tip position P2. The operator moves the work implement 3 thereby acquiring a plurality of mutually different blade tip positions P2. However, the second target part may be a portion other than the abovementioned portions. For example, the second target part may include the arm pin 29. The controller 24 may acquire the plurality of positions of the second target part with one attitude of the work implement 3.

The positions of the second target part are measured by the external position measurement device 45. The position measurement device 45 includes a camera 46 and locks onto and measures the positions to be measured captured by the camera 46. The controller 24 calculates the positions of the second target part in the vehicle body coordinate system X2-Y2-Z2, uses the set vehicle body coordinate system X2-Y2-Z2 to convert the positions of the second target part in the vehicle body coordinate system X2-Y2-Z2 to the positions of the second target part in the external coordinate system X1-Y1-Z1, and transmits the positions to the position measurement device 45. The position measurement device 45 points the camera 46 to the positions received from the controller 24 thereby locking onto and measuring the positions of the second target part. The position measurement device 45 transmits the measured positions (referred to below as “external measurement values”) of the second target part to the controller 24.

The abovementioned positions of the second target part transmitted from the controller 24 to the position measurement device 45 are calculated using the machine parameters before calibration and therefore are estimated positions with low precision. However, the position measurement device 45 is able to automatically lock onto the second target part with the camera 46 even if the second target part has moved based on the estimated positions of the second target part. Consequently, the position measurement device 45 is able to automatically switch the second target part to be locked onto. For example, the position measurement device 45 is able to automatically switch the second target part to be locked onto from the arm pin 29 to the bucket pin 30.

In step S106, the controller 24 executes calibration of the machine parameters. The controller 24 calculates the positions (referred to below as “arithmetic values”) of the second target part in the external coordinate system X1-Y1-Z1 based on the first attitude data, the second attitude data, and the machine parameters. The controller 24 calibrates the machine parameters based on an error between the external measurement values and the arithmetic values of the positions of the second target part. Alternatively, the controller 24 may calibrate the machine parameters based on the external measurement values of the positions of the second target part. A well-known calibration method may be used as the specific calibration method for the machine parameters. For example, the positions of the arm pin 29 and the bucket pin 30 may be measured and the length L2 of the arm 12 may be calibrated by using the distance calculated from the measured positions of the arm pin 29 and the bucket pin 30. Alternatively, the length of the arm 12 may be calibrated such that an error evaluation function of the measured values and the arithmetic values of the positions of the arm pin 29 in a plurality of attitudes of the work implement 3 is minimized.

In step S107, the controller 24 then evaluates the precision of the calibration. As illustrated in FIG. 11, the position measurement device 45 measures the blade tip position P2 and the antenna position P1 and transmits the measurements to the controller 24. The controller 24 calculates the vector (referred to below as “external measurement value”) in the direction from the antenna position P1 to the blade tip position P2 based on the blade tip position P2 and the antenna position P1 received from the position measurement device 45.

Conversely, the controller 24 calculates the vector in the direction from the antenna position P1 to the blade tip position P2 by using the vehicle body coordinate system X2-Y2-Z2. The controller 24 calculates the vector (referred to below as “arithmetic value”) in the direction from the antenna position P1 to the blade tip position P2 with the first attitude data, the second attitude data, and the machine parameters without using the position data detected by the positional sensor 36. The controller 24 calculates the error by comparing the external measurement value and the arithmetic value of the vector in the direction from the antenna position P1 to the blade tip position P2.

The controller 24 evaluates the precision of the calibration based on the error between the external measurement value and the arithmetic value of the vector in the direction from the antenna position P1 to the blade tip position P2. For example, the controller 24 may display a message or an image indicating that the calibration was successful when the error is equal to or less than a threshold. The controller 24 may display a message or an image urging the calibration to be redone when the error is greater than the threshold.

In the control system of the work machine 1 according to the present embodiment discussed above, the vehicle body coordinate system X2-Y2-Z2 is set based on the attitude of the first vehicle body 4 detected by the attitude sensor 37 instead of deriving the rotational plane of the first vehicle body 4. As a result, the vehicle body coordinate system X2-Y2-Z2 can be set with precision without rotating the first vehicle body 4 in the work machine 1.

Although an embodiment of the present invention has been described so far, the present invention is not limited to the above embodiment and various modifications may be made within the scope of the invention.

The work machine 1 is not limited to a hydraulic shovel and may be another type of work machine that includes a first vehicle body and a second vehicle body that is rotatably connected to the first vehicle body such as a crane, a wheel loader, or a motor grader. The configuration of the work machine 1 is not limited to the configuration described above and may be changed to other attachments.

The controller 24 may include a plurality of processors. The aforementioned processing may be distributed among the plurality of processors and executed. The controller 24 is not limited to one controller and the above processing may be distributed and executed by a plurality of controllers. For example, the controller 24 may be disposed outside the work machine 1. The machine parameters are not limited to the above embodiment and may be changed, omitted, or other parameters may be added.

The work machine 1 may be remotely operated. In this case, the operating device 33 may be disposed outside the work machine 1. The work machine 1 may be operated automatically. In this case, the operating device 33 may be omitted.

The controller 24 may perform automatic control of the work machine 1 using the calculated blade tip position P2. For example, as illustrated in FIG. 12, the controller 24 acquires actual topography data that indicates an actual topography 51 to be excavated. The controller 24 acquires a target locus 52 of the blade tip of the bucket 13 for performing construction work such as excavation. The controller 24 automatically controls the work implement 3 so that the calculated blade tip position P2 moves along the target locus 52.

Alternatively, the controller 24 may display an assistant screen on a display by using the calculated blade tip position P2. For example, as illustrated in FIG. 13, an assistant screen 53 includes an image representing an actual topography 54 and an image representing a target topography 55. The controller 24 displays images representing the work machine 1 and the blade tip position P2 at positions corresponding to the actual topography 54 and the target topography 55 based on the calculated blade tip position P2.

According to the present invention, the vehicle body coordinate system can be set with precision without rotating the first vehicle body in the work machine.