Position determination device

Abstract

To provide a position determination device allowing calibration on a position of a vehicle position measurement device provided on a work machine to be easily and accurately carried out.SOLUTION: A position determination device 80 used for a backhoe 10 is provided with a primary antenna position determination unit 81 for 3D calibration. The primary antenna position determination unit 81 determines a sixth coordinate (X6, Y6, Z6) corresponding to a center point P1 on a primary antenna 41 in a vehicle coordinate system as a position of the primary antenna 41. The primary antenna position determination unit 81 determines the sixth coordinate (X6, Y6, Z6) based on a first original point O1, a first coordinate (X1, Y2, Z3), a pitch angle θ2, a second coordinate (S2, T2, U2), a third coordinate (S3, T3, U3), a fourth coordinate (S4, T4, U4), a fifth coordinate (S5, T5, U5), and a first azimuth angle γ1.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present invention relates to a position determination device that is applied to a work machine that is equipped with at least a movable operating tool and is configured to determine the position of a tip portion of the movable operating tool, and that determines the position of a predetermined portion of the work machine. [Background technology]

[0002] Backhoes are well known as working machines such as construction machinery. Backhoes generally include movable operating tools such as a boom, an arm, and a bucket. The movable operating tools are controlled in response to the operator's operation. In particular, it is preferable to acquire the position of the tip of the movable operating tool in order to excavate at a target position on the ground surface. For example, the acquired position of the tip can be used to guide the operator in operating the movable operating tool. In light of these circumstances, technologies for determining the position of the tip of the movable operating tool have been developed in recent years.

[0003] Patent Document 1 discloses a technology for determining the cutting edge position of a backhoe as a work machine. At least vehicle position information is used to determine the cutting edge position of the backhoe. A GNSS antenna is mounted on the backhoe to detect the vehicle position. The position of the mounted antenna is measured using an external measuring device to obtain antenna parameters. The antenna parameters are then used as calibration values to calculate the cutting edge position of the bucket. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Patent No. 5237408 (paragraphs 0063, 0079, etc.) Summary of the Invention

[0005] In Patent Document 1, the distance between the boom pin and the antenna in the vehicle coordinate system is calculated as an antenna parameter. The distance is calculated in each three-dimensional direction to form coordinates. However, the position of the boom pin varies in various ways depending on the three-dimensional tilt of the vehicle body. Unless this consideration is fully reflected in the calculation, the antenna parameters will also vary, making it difficult to expect accurate calibration. Patent Document 1 does not disclose a specific calculation process for the antenna parameters. For this reason, it can be said that there is room for improvement in terms of easily and accurately calibrating the antenna position.

[0006] In view of the above, the present invention provides a work machine that includes at least a movable operating tool and a vehicle position measuring device, and is configured to determine the position of the tip of the movable operating tool based on the vehicle position, and in a position determining device that determines the position of a predetermined part of the work machine, calibration of the position of the vehicle position measuring device can be performed easily and accurately. [Means for solving the problem]

[0007] The technical means of the present invention for solving this technical problem is characterized by the following: A position determination device of the present invention includes a traveling vehicle, a movable operating tool connected to the traveling vehicle via a connection part and extending in a direction toward a tip part separated from the connection part, the movable operating tool being rotatable relative to the traveling vehicle around the connection part, a first vehicle position measurement device mounted on a predetermined part of the traveling vehicle and measuring the mounted position, a vehicle rotation degree acquisition device acquiring the rotation degree of the traveling vehicle, and a second vehicle position measurement device for acquiring the rotation degree of the movable operating tool. and a movable operating tool rotation degree acquisition device that acquires a rotation angle of the movable operating tool, and the position of the tip portion of the movable operating tool is determined based on at least the position of the first vehicle position measurement device, the degree of rotation of the traveling vehicle, and the degree of rotation of the movable operating tool measured and acquired by the first vehicle position measurement device, the vehicle rotation degree acquisition device, and the movable operating tool rotation degree acquisition device, respectively. Both of these are applied to a work machine that is configured to be able to achieve a zero roll angle state in which a roll angle indicating the degree of rotation of the traveling vehicle around the first direction, which is acquired by the vehicle rotation degree acquisition device, is zero in the vehicle coordinate system, and which defines a second direction that passes through the connection portion along the horizontal plane, a third direction that is perpendicular to each of the first direction and the second direction and passes through the connection portion along the vertical direction, and a three-dimensional vehicle coordinate system that has the connection portion as a first origin and is composed of the first direction, the second direction, and the third direction.

[0008] The position determination device of the present invention includes the first origin in the vehicle coordinate system in the zero roll angle state, a first coordinate corresponding to a predetermined point on the movable operating device in the zero roll angle state, a pitch angle indicating the degree of rotation of the traveling vehicle around the second direction as an axis in the vehicle coordinate system in the zero roll angle state, which is acquired by the vehicle rotation degree acquisition device, and a second coordinate corresponding to the predetermined point on the movable operating device in the zero roll angle state in a three-dimensional measurement coordinate system having an arbitrary point as a second origin and configured with a fourth direction passing through the second origin along a horizontal plane, a fifth direction orthogonal to the fourth direction and passing through the second origin along the horizontal plane, and a sixth direction orthogonal to each of the fourth direction and the fifth direction and passing through the second origin along a vertical direction; a first vehicle position measuring device position determiner that determines, as the position of the first vehicle position measuring device, a sixth coordinate corresponding to the predetermined point on the first vehicle position measuring device in the vehicle coordinate system, based on: a third coordinate in the measurement coordinate system corresponding to a predetermined point on the movable operating device in the zero roll angle state, the third coordinate being different from the position of the second coordinate; a fourth coordinate in the measurement coordinate system corresponding to the predetermined point on the movable operating device corresponding to the first coordinate in the zero roll angle state; a fifth coordinate in the measurement coordinate system corresponding to the predetermined point on the first vehicle position measuring device in the zero roll angle state; and a first azimuth angle that is an angle with respect to the fourth direction on the horizontal plane obtained based on the second coordinate and the third coordinate in the zero roll angle state in the measurement coordinate system.

[0009] In the position determination device of the present invention, the work machine further includes a second vehicle position measurement device that is mounted on the traveling vehicle at a predetermined location different from the location of the first vehicle position measurement device and measures the mounted location, and in addition to the first vehicle position measurement device position determination unit, further includes a second vehicle position measurement device position determination unit that determines an eighth coordinate corresponding to a predetermined point on the second vehicle position measurement device in the vehicle coordinate system as the position of the second vehicle position measurement device based on the determined sixth coordinate, the fifth coordinate in the measurement coordinate system in the roll angle zero state, and a second azimuth angle that is an angle with respect to the fourth direction on the horizontal plane obtained based on a seventh coordinate corresponding to a predetermined point on the second vehicle position measurement device.

[0010] In the position determination device of the present invention, the work machine is a backhoe, and the movable operating tool includes a boom having one end side and the other end side defined in the first direction, the one end side being connected to the traveling vehicle via a boom pin as the connection part, and rotatable around an axis passing through the boom pin parallel to the second direction; an arm having one end side and the other end side defined in the first direction, the one end side being connected to the other end side of the boom; and a bucket having one end side and the other end side defined in the first direction, the one end side being connected to the other end side of the arm via a bucket pin, the other end side being configured as the tip part, and rotatable around an axis passing through the bucket pin parallel to the second direction. the first vehicle position measuring device position determination unit is configured to use a point on the boom pin in the vehicle coordinate system in the zero roll angle state as the first origin, to use a point on the bucket pin in the vehicle coordinate system in the zero roll angle state as the first coordinate, to use a point on the boom in the zero roll angle state in the measurement coordinate system as the second coordinate, to use a point at the tip portion of the bucket in the zero roll angle state in the measurement coordinate system as the third coordinate, and to use a point on the bucket pin in the zero roll angle state in the measurement coordinate system as the fourth coordinate.

[0011] In the position determination device of the present invention, the traveling vehicle is configured to be able to rotate integrally with the movable operating device around a predetermined axis, and is equipped with a vehicle turning center position determination unit that determines a tenth coordinate corresponding to the point of the turning center of the traveling vehicle in the vehicle coordinate system as the position of the turning center of the traveling vehicle based on the first origin, the first coordinate, the second coordinate, the third coordinate, the fourth coordinate, the pitch angle, the first azimuth angle, and a ninth coordinate on an arc locus of a predetermined point on the movable operating device in the measurement coordinate system that is drawn by the turning of the traveling vehicle in the roll angle zero state.

[0012] In the position determination device of the present invention, at least one or more of the second coordinate, the third coordinate, the fourth coordinate, the fifth coordinate, the seventh coordinate, and the ninth coordinate in the measurement coordinate system are measured using an external coordinate measuring device installed at a distance from the work machine. [Effects of the Invention]

[0013] According to the present invention, the position of a vehicle position measuring device provided on a work machine can be calibrated easily and accurately. [Brief explanation of the drawings]

[0014] [Figure 1] 1 is a side view of a work machine (backhoe) to which a position determination device according to an embodiment of the present invention is applied. [Figure 2] FIG. 2 is a top view of the work machine shown in FIG. [Figure 3] 2 is a functional block diagram showing an ECU provided in the work machine shown in FIG. 1 and various devices connected to the ECU. [Figure 4] 2A and 2B are a rear view and a side view, respectively, of the working machine shown in FIG. 1 in a state where the traveling vehicle is tilted at a roll angle θ1 and a pitch angle θ2, respectively. [Figure 5] 2 is a side view of the working machine shown in FIG. 1 in a state where the boom is tilted at a tilt angle θ3 (the rotation angle of the boom rotation shaft relative to the horizontal direction). FIG. [Figure 6]1 is a side view showing a state in which the arm of the working machine shown in FIG. 1 is tilted at an inclination angle θ4 (the rotation angle of the arm rotation shaft relative to the horizontal direction). [Figure 7] 2 is a side view of the work machine shown in FIG. 1 in a state where the wrist link is tilted at an inclination angle θ5 (the rotation angle of the wrist link rotation axis relative to the horizontal direction). FIG. [Figure 8] 2 is a side view of the working machine shown in FIG. 1 in a state where the bucket is tilted at a tilt angle θ6 (the rotation angle of the bucket rotation shaft relative to the horizontal direction). FIG. [Figure 9] 9 is a diagram showing parts corresponding to various parameters for calculating a value β used to determine the tilt angle θ6 shown in FIG. 8. FIG. [Figure 10] 2A and 2B are a rear view and a side view, respectively, of the working machine shown in FIG. 1 when the traveling vehicle is in a roll angle zero state and when the traveling vehicle is inclined at a pitch angle θ2. [Figure 11] 2 is a diagram showing the correspondence relationship between each coordinate in a measurement coordinate system and a first azimuth angle when second coordinates and third coordinates are measured by an external coordinate measuring device for the work machine shown in FIG. 1. FIG. [Figure 12] 1. FIG. 4 is a diagram showing the correspondence between each coordinate and each vector in a measurement coordinate system when a fourth coordinate is measured by an external coordinate measuring device for the work machine shown in FIG. [Figure 13] 1. FIG. 4 is a diagram showing the correspondence between each coordinate in a measurement coordinate system and a second azimuth angle when a fifth coordinate and a seventh coordinate are measured by an external coordinate measuring device for the work machine shown in FIG. [Figure 14] FIG. 2 is a diagram showing the correspondence between the arc trajectory, each coordinate, and each vector drawn by rotation in the measurement coordinate system when four ninth coordinates are measured by an external coordinate measuring device for the work machine shown in FIG. DETAILED DESCRIPTION OF THE INVENTION

[0015] Hereinafter, an embodiment of the present invention will be described with reference to the drawings.

[0016] <Work machine configuration> 1 and 2 show a work machine 10 to which a position determination device 80 according to an embodiment of the present invention is applied. In this embodiment, the work machine 10 will be described as a backhoe 10. Note that the work machine 10 is not limited to a backhoe, and may be other construction machinery, agricultural machinery, etc. that has a tip of a movable operating tool. FIGS. 1 and 2 are a side view and a top view of the backhoe 10. The configuration of the backhoe 10 will be described in detail below.

[0017] 1 and 2, the backhoe 10 includes a traveling vehicle 20, a movable operating device 30, a vehicle position detection device 40, a rotation angle detection device 50, a drive control device 70, and a position determination device 80. The drive control device 70 and the position determination device 80 form part of an ECU (Electronic Control Unit) 90.

[0018] The traveling vehicle 20 has an upper rotating body 21 and a lower running body 22. The upper rotating body 21 is mounted on the lower running body 22 so as to be able to rotate about a predetermined axis. The axis of the rotation center is parallel to the vertical direction when the lower running body 22 is positioned on a horizontal plane.

[0019] The upper rotating body 21 is equipped with a movable operating device 30, a vehicle position detection device 40, and a cabin 21a. The cabin 21a is equipped with a driver's seat, operating devices 21b (e.g., levers, switches, pedals, etc., see FIG. 3), and a display device 21c (e.g., a monitor, etc., see FIG. 3). As indicated by appropriate arrows in each drawing, when the driver is seated in the cabin 21a, the directions in front of and behind the driver, left and right of the driver, and above and below the driver are defined as front and rear, left and right, and above and below, respectively.

[0020] The lower traveling body 22 includes a traveling device 22a and a dozer device 22b. The traveling device 22a is configured as a crawler type. The dozer device 22b is provided in front of the traveling device 22a. In the upper rotating body 21, the cabin 21a is disposed on the left side, and the movable operating device 30 is disposed immediately to the right of the cabin 21a.

[0021] As shown in Fig. 1, the movable operating device 30 is connected to the upper rotating body 21 of the traveling vehicle 20 via a boom pin 31a (corresponding to a connection portion). The movable operating device 30 extends from the boom pin 31a in a direction toward the tip portion 34b of the bucket 34 that is spaced forward. The movable operating device 30 is rotatable relative to the traveling vehicle 20 around the boom pin 31a.

[0022] The movable operating device 30 includes a boom 31, an arm 33, and a bucket 34. The movable operating device 30 also includes a boom cylinder 36, an arm cylinder 38, and a bucket cylinder 39. These cylinders are, for example, rod-shaped hydraulic cylinder devices, and can be operated by the operator to extend and retract so that the linear distance from one end to the other end can be changed.

[0023] The boom 31 has one end and the other end defined in a first direction (described later), and one end is connected to the traveling vehicle 20 via a boom pin 31a serving as a connection part. The boom 31 is rotatable around an axis passing through the boom pin 31a and parallel to a second direction (described later). In this embodiment, one end of the rear lower side of the boom 31 is supported in front of the upper rotating body 21 via the boom pin 31a. One end of the boom cylinder 36 is connected to the front of the upper rotating body 21 below the boom pin 31a via a boom cylinder support shaft 36a. The other end of the boom cylinder 36 supports the boom 31 from below via a boom cylinder support shaft 36b. The boom 31 is rotatable around the boom pin 31a by the extension and retraction of the boom cylinder 36. In this embodiment, the boom 31 is configured not to be offset to the left or right. However, instead, the backhoe 10 may be an offset boom type.

[0024] The arm 33 has one end and the other end defined in the first direction, and the one end is connected to the other end of the boom 31. In this embodiment, an upper rear end of the arm 33 is supported by the other front end of the boom 31 via an arm pin 33a. One end of the arm cylinder 38 is connected to the upper side of the boom 31 via an arm cylinder spindle 38a, above and rearward of the arm pin 33a. The other end of the arm cylinder 38 supports the arm 33 from above, above and forward of the arm pin 33a, via an arm cylinder spindle 38b. The arm 33 is rotatable about the arm pin 33a by the extension and retraction drive of the arm cylinder 38.

[0025] The bucket 34 has one end and the other end defined in the first direction, and the one end is connected to the other end of the arm 33 via a bucket pin 34a. The other end of the bucket 34 is configured as a tip portion 34b, and the bucket 34 is rotatable around an axis passing through the bucket pin 34a and parallel to the second direction. Furthermore, one end of the bucket 34 is connected to the other end of the arm 33 via a wrist link 34c. In this embodiment, one end of the bucket cylinder 39 is connected to the upper side of the arm 33 via a bucket cylinder spindle 39a, forward of the arm pin 33a. The other end of the bucket cylinder 39 supports the bucket 34 via the bucket cylinder spindle 39b and the wrist link 34c, below the bucket pin 34a. The bucket 34 is rotatable around the bucket pin 34a as the bucket cylinder 39 extends and retracts. The tip portion 34b corresponds to the outermost position of the bucket 34 in the radial direction of rotation. By rotating one, two, or all of the boom 31, arm 33, and bucket 34, the tip portion 34b of the bucket 34 can move upward, downward, forward, or backward relative to the traveling vehicle 20.

[0026] The backhoe 10 is defined as having a first direction, a second direction, and a third direction. The first direction is a direction from the boom pin 31a toward the tip portion 34b along a horizontal plane. The second direction is perpendicular to the first direction, corresponds to the left-to-right direction in the vehicle width direction of the traveling vehicle 20, and is a direction passing through the boom pin 31a along a horizontal plane. The third direction is perpendicular to both the first and second directions, and is a direction passing through the boom pin 31a along the vertically upward direction. As shown in FIGS. 1 and 2, in this embodiment, the first direction corresponds to the X direction, the second direction corresponds to the Y direction, and the third direction corresponds to the Z direction. Furthermore, a three-dimensional vehicle coordinate system is defined for the backhoe 10. The vehicle coordinate system has a first origin O1 at the position of the boom pin 31a and is configured with the X direction, the Y direction, and the Z direction.

[0027] As shown in Fig. 1 and Fig. 2, the vehicle position detection device 40 includes a primary antenna 41 and a secondary antenna 42. The primary antenna 41 and the secondary antenna 42 are respectively mounted on the upper rotating body 21 of the traveling vehicle 20. In this embodiment, the primary antenna 41 is mounted behind the cabin 21a on the upper rotating body 21. The secondary antenna 42 is mounted to the right of the mounting position of the primary antenna 41 on the upper rotating body 21. The respective positions of the primary antenna 41 and the secondary antenna 42 are not limited to this and may be any position on the traveling vehicle 20.

[0028] The primary antenna 41 and the secondary antenna 42 detect the above-mentioned mounting position as the position of the traveling vehicle 20 based on receiving positioning signals from global positioning satellites. This detection signal is transmitted to the position determination device 80. The primary antenna 41 and the secondary antenna 42 may be capable of detecting their positions using a satellite positioning system such as GNSS. The primary antenna 41 corresponds to a first vehicle position measurement device, and the secondary antenna 42 corresponds to a second vehicle position measurement device. In this embodiment, the primary antenna 41 and the secondary antenna 42 are configured as separate bodies, but instead, they may be configured as an integrated body.

[0029] The rotation degree detection devices 50 are mounted at four locations on the upper rotating body 21, the boom 31, the arm 33, and the bucket 34 of the traveling vehicle 20 (see FIG. 1). These four rotation degree detection devices 50 detect the degree of rotation at each of the four locations. These detection signals are sent out to the position determination device 80. The rotation degree detection devices 50 are, for example, rotation angle detectors such as an IMU (Inertial Measurement Unit). Hereinafter, the rotation degree detection devices 50 mounted on the upper rotating body 21, the boom 31, the arm 33, and the wrist link 34c of the traveling vehicle 20, respectively, may be referred to as IMU 51, IMU 52, IMU 53, and IMU 54.

[0030] In this embodiment, the IMU 51 corresponds to a biaxial inclination sensor and is capable of acquiring the degree of rotation of the traveling vehicle 20 about an axis in the X direction and an axis in the Y direction. Here, the degree of rotation about an axis in the X direction and an axis in the Y direction are indicated as a roll angle and a pitch angle. The IMU 51 corresponds to a vehicle rotation degree acquisition device. The IMU 52, the IMU 53, and the IMU 54 each correspond to a single-axis inclination sensor and are capable of acquiring the degree of rotation of the boom 31, the arm 33, and the wrist link 34c about an axis parallel to the Y direction. The IMU 52, the IMU 53, and the IMU 54 correspond to a movable operating tool rotation degree acquisition device.

[0031] As shown in FIGS. 1 and 3, the ECU 90 is composed of a drive control device 70 and a position determination device 80. The drive control device 70 and the position determination device 80 are composed of a CPU, electric circuits, etc., and are electrically connected to the above-mentioned various devices, etc., via wired or wireless connections. Specifically, the drive control device 70 is connected to the operating device 21b, the movable operating device 30, and a power transmission system, such as a prime mover and a transmission (not shown). The position determination device 80 is connected to the vehicle position detection device 40, the rotation degree detection device 50, and the display device 21c. The position determination device 80 can also be connected to an external coordinate measuring device 100.

[0032] The drive control device 70 receives an operation signal from the operating device 21b operated by the operator. In response to the operation signal, the drive control device 70 controls the extension and retraction drive of the boom cylinder 36, arm cylinder 38, and bucket cylinder 39 of the movable operating device 30. This controls the rotational operations of the boom 31, arm 33, and bucket 34 about their respective support shafts. The drive control device 70 also controls a power transmission system, such as a prime mover and transmission (not shown), and also controls the traveling of the traveling device 22a.

[0033] The position determining device 80 includes a primary antenna position determining unit 81, a secondary antenna position determining unit 82, a vehicle turning center position determining unit 83, and a bucket tip portion position determining unit 85. When the position determining device 80 is connected to the external coordinate measuring device 100, it receives a signal corresponding to the coordinates measured by the external coordinate measuring device 100. Based on the measured coordinates, various calculations are performed in a primary antenna position determining unit 81, a secondary antenna position determining unit 82, and a vehicle turning center position determining unit 83. The calculations will be described in detail later.

[0034] The primary antenna position determiner 81 determines coordinates corresponding to a predetermined point on the primary antenna 41 in the vehicle coordinate system as the position of the primary antenna 41. The primary antenna position determiner 81 is communicably connected to the external coordinate measuring device 100 and is capable of receiving signals corresponding to coordinates measured by the external coordinate measuring device 100. The primary antenna position determiner 81 determines the coordinates on the primary antenna 41 based on the coordinates measured by the external coordinate measuring device 100, the angle acquired by the rotation degree detection device 50, etc. The primary antenna position determiner 81 corresponds to the first vehicle position measuring device position determiner.

[0035] The secondary antenna position determiner 82 determines coordinates corresponding to a predetermined point on the secondary antenna 42 in the vehicle coordinate system as the position of the secondary antenna 42. The secondary antenna position determiner 82 is communicably connected to the external coordinate measuring device 100 and is capable of receiving signals corresponding to coordinates measured by the external coordinate measuring device 100. The secondary antenna position determiner 82 determines the coordinates of the secondary antenna 42 based on the determined coordinates of the primary antenna 41, the coordinates measured by the external coordinate measuring device 100, etc. The secondary antenna position determiner 82 corresponds to a second vehicle position measuring device position determiner.

[0036] The vehicle turning center position determination unit 83 is configured to determine coordinates corresponding to the point of the turning center of the traveling vehicle 20 in the vehicle coordinate system as the position of the turning center of the traveling vehicle 20. The vehicle turning center position determination unit 83 is communicably connected to the external coordinate measuring device 100 and is capable of receiving signals corresponding to the coordinates measured by the external coordinate measuring device 100. The vehicle turning center position determination unit 83 determines the coordinates of the turning center of the traveling vehicle 20 based on the coordinates measured by the external coordinate measuring device 100, the angle acquired by the rotation degree detection device 50, etc.

[0037] The bucket tip portion position determiner 85 determines the position of the tip portion 34b of the bucket 34 of the movable operating device 30 based on the position of the traveling vehicle 20, the degree of rotation of the traveling vehicle 20, the degree of rotation of the movable operating device 30, etc. The position of the traveling vehicle 20 is measured by the vehicle position detection device 40. The degree of rotation of the traveling vehicle is acquired by the IMU 51. The degree of rotation of the movable operating device 30 is acquired by the IMUs 52, 53, and 54. More specifically, the position of the tip portion 34b may be determined by a well-known process such as the calculation described in Japanese Patent No. 5237408, and the calculation formula therefor is not limited.

[0038] Bucket tip portion position determiner 85 displays the determined position information of tip portion 34b on display device 21c. For example, a mark of tip portion 34b may be superimposed on a map of the vicinity of traveling vehicle 20 in the field on the monitor of display device 21c. This makes it possible to visually confirm the position of tip portion 34b of bucket 34 on display device 21c, and to guide the driver's operation.

[0039] On the other hand, in order to determine the position of the tip portion 34b with high accuracy, it is preferable to perform calibration in advance for the vehicle position detection device 40 (primary antenna 41 and secondary antenna 42) and the turning center of the traveling vehicle 20. Hereinafter, this calibration may be referred to as "3D calibration." 3D calibration is a method of calibrating the coordinates measured using the external coordinate measuring device 100 to the primary antenna position. The calculated values are transmitted to the vehicle turning center position determining unit 81, the secondary antenna position determining unit 82, and the vehicle turning center position determining unit 83, and are calculated by each unit.

[0040] Furthermore, in order to accurately determine the position of the tip portion 34b, it is preferable to perform calibration in advance for the rotation angle detection device 50 (IMUs 51 to 54) and the rotation angle of the bucket 34. Hereinafter, these calibrations may be referred to as "2D calibration." In this embodiment, the above-described 2D calibration and 3D calibration are performed in advance in this order before the backhoe 10 performs actual work. Note that, if dimensional measurements of each portion are required during calibration, the dimensions of each portion may be measured using a specified device, such as a convex ruler or a metal ruler. The specified device is not limited and may be any device capable of directly measuring each portion.

[0041] <2D calibration process> Next, the 2D calibration process will be described. In the 2D calibration, the following five correction amounts are determined. These correction amounts are used to compensate for errors caused by, for example, sensor installation, and to calculate the angle from the rotation axis of the bucket 34 to the tip portion 34b.

[0042] 1. Correction amounts J1 and J2 of the IMU 51, which is the tilt sensor of the traveling vehicle 20 2. Correction amount J3 of IMU52, which is the tilt sensor of boom31 3. Correction amount J4 of IMU53, which is the tilt sensor of arm 33 4. Correction amount J5 of IMU54, which is the tilt sensor of wrist link 34c 5. Correction amount J6 as the inner angle of the bucket 34

[0043] <<1. Correction amounts J1 and J2>> As shown in FIG. 4, the tilt angle of the traveling vehicle 20 with its axis in the X direction relative to the horizontal plane is defined as θ1. The tilt angle of the traveling vehicle 20 with its axis in the Y direction relative to the horizontal plane is defined as θ2. When the IMU 51 is attached to the traveling vehicle 20 and the two axes of the IMU 51 are aligned with the X and Y directions, the tilt angles θ1 and θ2 and the correction amounts J1 and J2 have the relationship shown in the following equations (1) and (2). In the following equations (1) and (2), K1 and K2 are determined by the installation direction of the IMU 51, and their absolute values are "1". α1 and α2 are the output values of the IMU 51 on each axis.

[0044] θ1=K1*α1+J1 (1) θ2=K2*α2+J2 (2)

[0045] Correction amounts J1 and J2 in this relationship are determined. More specifically, first, output values α1[1] and α1[2] of the IMU 51 are obtained in an arbitrary direction. Then, output values α2[1] and α2[2] of the IMU 51 are obtained with the traveling vehicle 20 (upper rotating body 21) rotated 180°. Then, correction amounts J1 and J2 are determined from the relationship shown in the following equations (3) and (4).

[0046] J1=((K1*α1[1]+K1*α1[2]) / 2)*(-1) ···(3) J2=((K2*α2[1]+K2*α2[2]) / 2)*(-1) ···(4)

[0047] <<2. Correction amount J3>> As shown in FIG. 5, the tilt angle of the boom 31 with respect to the horizontal plane is θ3. When the IMU 52 is attached to the boom 31, if the axis of the IMU 52 is aligned with the rotation axis of the boom, the tilt angle θ3 and the correction amount J3 have the relationship shown in the following equation (5). In the following equation (5), K3 is determined by the installation direction of the IMU 52, and its absolute value is "1". α3 is the output of the IMU 52. It is a force value.

[0048] θ3=K3*α3+J3 (5)

[0049] A correction amount J3 in this relationship is determined. More specifically, first, the traveling vehicle 20 (upper rotating body 21) is rotated so that the above-mentioned tilt angle θ1 becomes zero. While maintaining this state, the position of the boom 31 is adjusted so that the rotation axis A3 of the boom 31 and arm 33 is horizontal. At this time, a marking laser, a level, or the like is used to confirm that the boom 31 is horizontal. Next, the output value α3 of the IMU 52 in the horizontal state is obtained. Then, the correction amount J3 is determined from the relationship shown in the following equation (6).

[0050] J3=(-1)*K3*α3 (6)

[0051] <<3. Correction amount J4>> As shown in Figure 6, the tilt angle of the arm 33 with respect to the horizontal plane is θ4. When the IMU 53 is attached to the arm 33, if the axis of the IMU 53 is aligned with the rotation axis of the arm 33, the tilt angle θ4 and the correction amount J4 have the relationship shown in the following equation (7). In the following equation (7), K4 is determined by the installation direction of the IMU 53, and its absolute value is "1". α4 is the output value of the IMU 53.

[0052] θ4=K4*α4+J4 (7)

[0053] A correction amount J4 in this relationship is determined. More specifically, first, the traveling vehicle 20 (upper rotating body 21) is rotated so that the above-mentioned tilt angle θ1 becomes zero. While maintaining this state, the position of the arm 33 is adjusted so that the rotation axis A4 of the arm 33 and bucket 34 is perpendicular to the horizontal. At this time, a marking laser, a level, or the like is used to confirm that the arm 33 is perpendicular. Next, the output value α4 of the IMU 53 in the perpendicular state is obtained. Then, the correction amount J4 is determined from the relationship shown in the following equation (8).

[0054] J4=(-1)*K4*α4-90° (8)

[0055] <<4. Correction amount J5>> As shown in Figure 7, the tilt angle of wrist link 34c with respect to the horizontal plane is θ5. When attaching IMU 54 to wrist link 34c, if the axis of IMU 54 is aligned with the rotation axis of wrist link 34c, the tilt angle θ5 and correction amount J5 have the relationship shown in equation (9) below. In equation (9) below, K5 is determined by the installation direction of IMU 54, and its absolute value is "1". α5 is the output value of IMU 54.

[0056] θ5=K5*α5+J5 (9)

[0057] A correction amount J5 in this relationship is determined. More specifically, first, the traveling vehicle 20 (upper rotating body 21) is rotated so that the above-mentioned tilt angle θ1 becomes zero. While maintaining this state, the position of the bucket 34 is adjusted so that the rotation axis A5 of the wrist link 34c shown in FIG. 7 is horizontal. At this time, a marking laser, a level, or the like is used to confirm that the bucket 34 is horizontal. Next, the output value α5 of the IMU 54 in the horizontal state is obtained. Then, the correction amount J5 is determined from the relationship shown in the following equation (10).

[0058] J5=(-1)*K5*α5 (10)

[0059] <<5. Correction amount J6>> As shown in Figure 8, the inclination angle from the rotation axis of the bucket 34 to the tip portion 34b with respect to the horizontal plane is defined as θ6. The relationship between the inclination angle θ6 and the correction amount J6 as the interior angle of the bucket 34 is shown in the following equation (11). In the following equation (11), β is a value based on the various dimensions of the wrist link 34c, the inclination angles of the wrist link 34c and the arm 33, etc.

[0060] θ6=β+J6 (11)

[0061] A correction amount J6 in this relationship is determined. More specifically, first, the traveling vehicle 20 (upper rotating body 21) is rotated so that the above-mentioned tilt angle θ1 becomes zero. While maintaining this state, the position of the bucket 34 is adjusted so that the direction of the rotation axis A6 and the tip portion 34b of the bucket 34 is perpendicular to the horizontal. At this time, a marking laser, a level, or the like is used to confirm that it is perpendicular. Next, a value β in the horizontal state is obtained. Then, a correction amount J6 is determined from the relationship shown in the following equation (12). Here, the value β is determined based on the relationship shown in the following equation (13), the above-mentioned tilt angle θ4, and the values δ, μ1, and μ2.

[0062] J6=(-1)*β-90° (12) β=180°-μ1-μ2-δ-θ4 (13)

[0063] Here, the values δ, μ1, and μ2 correspond to the angles indicated by the same symbols in Figure 9 and are calculated using the following equations (14), (15), and (16). In the following equations (15) and (16), the value d is calculated using the following equation (17). In the following equations (14), (15), (16), and (17), the values L1, L2, L3, L4, and L5 correspond to the distances indicated by the same symbols in Figure 9.

[0064] δ=arcsin(L5 / L2) (14) μ1=arccos((L2*L2+d*d-L1*L1) / (2*L2*d)) ···(15) μ2=arccos((L3*L3+d*d-L4*L4) / (2*L3*d)) ···(16) d=√(L1*L1+L2*L2-2*L1*L2*cos(θ4+δ+θ5)) ···(17)

[0065] As described above, the correction amounts J1, J2, J3, J4, J5, and J6 are determined in the 2D calibration. The determined correction amounts J1 to J6 may be stored in a memory separately provided in the ECU 90, and may be read out as appropriate when the position determination device 80 performs calculations.

[0066] <3D calibration process> Next, the 3D calibration process will be described. In 3D calibration, the following three positions are determined. 3D calibration is performed after the above-mentioned 2D calibration is completed. That is, when the above-mentioned tilt angles θ1 to θ6 are used in 3D calibration, the angles are corrected using the correction amounts J1 to J6 determined in the 2D calibration.

[0067] 1. A coordinate position corresponding to a given point on the primary antenna 41 in the vehicle coordinate system 2. A coordinate position corresponding to a predetermined point on the secondary antenna 42 in the vehicle coordinate system. 3. Coordinate position corresponding to the turning center point of the traveling vehicle 20 in the vehicle coordinate system

[0068] In the 3D calibration, the coordinates corresponding to a predetermined point on the backhoe 10 are measured using an external coordinate measuring device 100. The external coordinate measuring device 100 is located away from the backhoe 10. The external coordinate measuring device 100 is installed between the workpiece and the workpiece, and measures coordinates in the measurement system coordinate system that correspond to a predetermined point on the backhoe 10. The external coordinate measuring device 100 is, for example, a total station, a 3D scanner, a laser tracker, etc., and any device, system, or method that can measure coordinates may be used.

[0069] The external coordinate measuring device 100 forms a three-dimensional measurement coordinate system. The measurement coordinate system defines a second origin O2, a fourth direction, a fifth direction, and a sixth direction. The second origin O2 is an arbitrary point within the range measurable by the external coordinate measuring device 100. The fourth direction is a direction passing through the second origin O2 along a horizontal plane. The fifth direction is a direction perpendicular to the fourth direction and passing through the second origin O2 along a horizontal plane. The sixth direction is a direction perpendicular to each of the fourth and fifth directions and passing through the second origin O2 along the vertical direction. In this embodiment, the fourth direction corresponds to the S direction, the fifth direction corresponds to the T direction, and the sixth direction corresponds to the U direction (see FIGS. 11 to 14). That is, the measurement coordinate system defines an arbitrary point as the second origin O2 and is formed by the S direction, the T direction, and the U direction.

[0070] <<1. Primary antenna coordinates>> As shown in FIG. 10, first, the traveling vehicle 20 is turned, and the turning is completed when the tilt angle θ1 (hereinafter also referred to as "roll angle θ1") about the X-axis in the vehicle coordinate system acquired by the IMU 51 becomes zero. This state is referred to as the zero roll angle state. In other words, the backhoe 10 is configured to be able to achieve the zero roll angle state. After the turning is completed, the backhoe 10 maintains the attitude in the zero roll angle state.

[0071] Next, a first coordinate (X1, Y1, Z1) corresponding to the center point of the bucket pin 34a in the vehicle coordinate system when the roll angle is zero is determined. The first coordinate (X1, Y1, Z1) is determined by the primary antenna position determination unit 81 based on, for example, the dimensions of the boom 31 and arm 33, the tilt angle acquired by the IMU 52 and IMU 53 and reflecting the correction amount, etc. Note that Y1 in the first coordinate (X1, Y1, Z1) is zero.

[0072] Next, in the vehicle coordinate system, the tilt angle θ2 (hereinafter also referred to as "pitch angle θ2") is acquired around the Y direction as an axis when the roll angle is zero, which is acquired by the IMU 51. The acquired pitch angle θ2 is transmitted from the IMU 51 to the primary antenna position determination unit 81.

[0073] As shown in FIG. 11 , next, in the measurement coordinate system, a second coordinate (S2, T2, U2) corresponding to a point on the excavation axis and a third coordinate (S3, T3, U3) corresponding to the tip portion 34b of the bucket 34 are measured in a roll angle zero state. Here, the measurement point on the excavation axis may be, for example, a point on the boom 31, and may be any point between the boom pin 31a and the bucket pin 34a. The second coordinate (S2, T2, U2) and the third coordinate (S3, T3, U3) are measured, for example, from the side of the backhoe 10 by the external coordinate measuring device 100. The measured second coordinate (S2, T2, U2) and the third coordinate (S3, T3, U3) are transmitted from the external coordinate measuring device 100 to the primary antenna position determining unit 81.

[0074] Next, a first azimuth angle γ1 in a zero roll angle state in the measurement coordinate system is acquired. The first azimuth angle γ1 is an angle formed with respect to the S direction on a horizontal plane in the measurement coordinate system. The first azimuth angle γ1 is determined by the primary antenna position determination unit 81 based on the measured second coordinates (S2, T2, U2) and third coordinates (S3, T3, U3). More specifically, for example, the first azimuth angle γ1 is determined by the second coordinates (S2, T2, U2), the third coordinates (S3, T3, U3), and the following equation (18).

[0075] γ1=arctan((T3-T2) / (S3-S2))*(180 / π) ···(18)

[0076] 12, next, a fourth coordinate (S4, T4, U4) corresponding to the center position of the bucket pin 34a in the measurement coordinate system when the roll angle is zero is measured. The fourth coordinate (S4, T4, U4) is measured, for example, from the side of the backhoe 10 by the external coordinate measuring device 100. The measured fourth coordinate (S4, T4, U4) is transmitted from the external coordinate measuring device 100 to the primary antenna position determining unit 81.

[0077] Next, vector B corresponding to the center position of the boom pin 31a in the measurement coordinate system is determined. Vector B is determined by the primary antenna position determination unit 81, for example, using vectors M, N, K, and D and the following equation (19). Here, vector M is a vector corresponding to the measured fourth coordinate (S4, T4, U4) in the measurement coordinate system. Vector N is an orientation vector of the backhoe 10 in the measurement coordinate system, and is determined, for example, using the determined first azimuth angle γ1 and the following equation (20). Vector K is a vector on the excavation axis in the measurement coordinate system, and is determined, for example, based on the measured second coordinate (S2, T2, U2). Vector D is a vector extending from the boom pin 31a to the bucket pin 34a in the measurement coordinate system, and is determined, for example, based on the determined first coordinate (X1, Y1, Z1) and pitch angle θ2. Note that vectors M, N, K, and D are determined by the primary antenna position determination unit 81.

[0078] B = (N (MK)) × N + KD (19) N=(cos(γ1),sin(γ1),0) ···(20)

[0079] Next, a fifth coordinate (S5, T5, U5) corresponding to a point on the primary antenna 41 in the measurement coordinate system when the roll angle is zero is measured. Here, for example, if the primary antenna 41 and the secondary antenna 42 are separate, the measurement point on the primary antenna 41 is the center point P1 of the primary antenna 41 in a top view (see FIGS. 2 and 13). Alternatively, for example, if the primary antenna 41 and the secondary antenna 42 are integrated, a point corresponding to a location such as a screw hole closer to the primary antenna 41 may be used as the measurement point. The fifth coordinate (S5, T5, U5) is measured, for example, from the side of the backhoe 10 by the external coordinate measuring device 100. The measured fifth coordinate (S5, T5, U5) is transmitted from the external coordinate measuring device 100 to the primary antenna position determining unit 81.

[0080] Then, primary antenna position determination unit 81 subtracts vector B determined by equation (19) from the vector corresponding to the measured fifth coordinate (S5, T5, U5). The vector obtained by this subtraction is rotated around the Z axis with the first azimuth angle γ1 determined by equation (18). The coordinate obtained by the rotation becomes sixth coordinate (X6, Y6, Z6) corresponding to the center point of primary antenna 41 in the vehicle coordinate system. In this way, primary antenna position determination unit 81 determines the sixth coordinate (X6, Y6, Z6) as the position of primary antenna 41.

[0081] <<2. Coordinate position of secondary antenna>> 13, first, in the measurement coordinate system, a fifth coordinate (S5, T5, U5) corresponding to a point on the primary antenna 41 and a seventh coordinate (S7, T7, U7) corresponding to a point on the secondary antenna 42 are measured in a state where the roll angle is zero. As the fifth coordinate (S5, T5, U5), the coordinates measured when determining the sixth coordinate (X6, Y6, Z6) of the primary antenna 41 described above may be used.

[0082] The measurement points on the secondary antenna 42 are, for example, the primary antenna 41 and the secondary antenna 42. If the primary antenna 41 and the secondary antenna 42 are separate, the measurement point is the center point P2 of the secondary antenna 42 in a top view (see FIG. 2). Alternatively, if the primary antenna 41 and the secondary antenna 42 are integrated, a point corresponding to a location such as a screw hole closer to the secondary antenna 42 may be used as the measurement point. The fifth coordinate (S5, T5, U5) and the seventh coordinate (S7, T7, U7) are measured, for example, by the external coordinate measuring device 100 from the side of the backhoe 10. The measured fifth coordinate (S5, T5, U5) and the seventh coordinate (S7, T7, U7) are transmitted from the external coordinate measuring device 100 to the secondary antenna position determining unit 82.

[0083] Next, the second azimuth angle γ2 in the measurement coordinate system in a state where the roll angle is zero is acquired. The second azimuth angle γ2 is an angle formed with respect to the S direction on the horizontal plane in the measurement coordinate system. The second azimuth angle γ2 is determined by the secondary antenna position determination unit 82 based on the measured fifth coordinates (S5, T5, U5) and seventh coordinates (S7, T7, U7). More specifically, the second azimuth angle γ2 is determined, for example, using the fifth coordinates (S5, T5, U5) and the seventh coordinates (S7, T7, U7) and the following equation (21). The second azimuth angle γ2 corresponds to the angle formed between the direction from the primary antenna 41 to the secondary antenna 42 and the X direction, as shown by the dashed line in FIG. 2.

[0084] γ2=arctan((T7-T5) / (S7-S5))*(180 / π) ···(21)

[0085] Then, the secondary antenna position determination unit 82 determines an eighth coordinate (X8, Y8, Z8) in the vehicle coordinate system that corresponds to the center point of the secondary antenna 42, based on the determined second azimuth angle γ2 and the determined sixth coordinate (X6, Y6, Z6) of the primary antenna 41. In this way, the secondary antenna position determination unit 82 determines the eighth coordinate (X8, Y8, Z8) as the position of the secondary antenna 42.

[0086] <<3. Coordinate position corresponding to the turning center point>> As shown in Fig. 14, first, in the measurement coordinate system, ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) on a circular arc locus of a predetermined point on the movable operating device 30 are measured in a roll angle zero state. The circular arc locus is described by the turning of the traveling vehicle 20. The predetermined point on the movable operating device 30 is a point corresponding to an arbitrary position, but it is preferable to use a point located farther away from the turning center.

[0087] In this embodiment, when tracing the above-described circular arc trajectory, first, the traveling vehicle 20 is rotated 90° from a predetermined posture with a zero roll angle. The state after the rotation is maintained, and a first ninth coordinate (S91, T91, U91) corresponding to a predetermined point on the movable operating device 30 is measured. Next, the traveling vehicle 20 is rotated another 90°, and a state after a 180° rotation from the initial state is maintained. In this state, a second ninth coordinate (S92, T92, U92) corresponding to a predetermined point on the movable operating device 30 is measured. Next, the traveling vehicle 20 is rotated another 90°, and a state after a 270° rotation from the initial state is maintained. In this state, a third ninth coordinate (S93, T93, U93) corresponding to a predetermined point on the movable operating device 30 is measured. Then, the traveling vehicle 20 is rotated another 90°, and a state after a 360° rotation from the initial state is maintained. In this state, the fourth ninth coordinate (S94, T94, U94) corresponding to the predetermined point on the movable operating tool 30 is measured.

[0088] These four ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) are, for example, the coordinates of the backhoe 10. The coordinates are measured from the side by the external coordinate measuring device 100. The measured ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) are transmitted from the external coordinate measuring device 100 to the vehicle turning center position determining unit 83.

[0089] Next, the vehicle turning center position determiner 83 determines the turning center point C in the measurement coordinate system based on the measured ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94). More specifically, for example, first, a surface onto which the ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) fit is found using the least squares method. Next, the ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) are projected onto the fitting surface, and an arc passing through all of these coordinates is determined. Then, the center point of the determined arc may be determined as the pivot center point C in the measurement coordinate system.

[0090] In this embodiment, four ninth coordinates are measured, but instead, the number of measurement points may be five or more. In this case, the external coordinate measuring device 100 may measure the coordinates each time the traveling vehicle 20 turns at an angle that is smaller than 90°. Also, the number of measurement points for the ninth coordinate may be three or less. In this case, the external coordinate measuring device 100 may measure the coordinates each time the traveling vehicle 20 turns at an angle that is larger than 90°.

[0091] Next, the vehicle turning center position determination unit 83 determines a vector E pointing from the center position of the boom pin 31a toward the turning center point based on the coordinates of the turning center point C in the measurement coordinate system determined above and the vector B determined by the above equation (19). As the vector B, the one determined when calculating the sixth coordinate (X6, Y6, Z6) of the primary antenna 41 described above may be used.

[0092] Then, the vehicle turning center position determination unit 83 rotates the determined vector E around the Z axis by the first azimuth angle γ1 determined by the above equation (18), and then rotates it around the Y axis by the pitch angle θ2 acquired by the IMU 51. The coordinates obtained by the rotation become tenth coordinates (X10, Y10, Z10) corresponding to the turning center point of the traveling vehicle 20 in the vehicle coordinate system. In this way, the vehicle turning center position determination unit 83 determines the tenth coordinates (X10, Y10, Z10) as the position corresponding to the turning center point of the traveling vehicle 20.

[0093] As described above, in the 3D calibration, as shown in FIGS. 1 and 2, the sixth coordinate (X6, Y6, Z6), the eighth coordinate (X8, Y9, Z8), and the tenth coordinate (X10, Y10, Z10) in the vehicle coordinate system are determined to correspond to the positions of the center point P1 of the primary antenna 41, the center point P2 of the secondary antenna 42, and the turning center point P3 of the traveling vehicle 20, respectively. The bucket tip portion position determiner 85 uses the coordinates determined in the 3D calibration and the correction amount determined in the 2D calibration to determine the position of the tip portion 34b. In other words, the tip portion 34b determined through the 2D and 3D calibrations can be displayed on the display device 21c, enabling accurate navigation of the driver's operations.

[0094] <Effects of the embodiment> As described above, the position determining device 80 according to the embodiment of the present invention is provided with the primary antenna position determining unit 81 for 3D calibration. The primary antenna position determining unit 81 determines the position of the primary antenna 41 in the vehicle coordinate system. The primary antenna position determination unit 81 determines a sixth coordinate (X6, Y6, Z6) corresponding to the center point P1 on the primary antenna 41. The primary antenna position determination unit 81 determines the sixth coordinate (X6, Y6, Z6) based on the first origin O1, the first coordinate (X1, Y2, Z3), the pitch angle θ2, the second coordinate (S2, T2, U2), the third coordinate (S3, T3, U3), the fourth coordinate (S4, T4, U4), the fifth coordinate (S5, T5, U5), and the first azimuth angle γ1.

[0095] According to this, by measuring each coordinate in the measurement system coordinate system, the sixth coordinate (X6, Y6, Z6) can be determined easily and accurately through vector calculations, etc. In other words, when determining the position of the tip portion 34b of the bucket 34, calibration of the position of the primary antenna 41 can be performed easily and accurately.

[0096] In particular, the embodiment of the present invention includes a secondary antenna position determiner 82 for 3D calibration. The secondary antenna position determiner 82 determines an eighth coordinate (X8, Y8, Z8) corresponding to a center point P2 on the secondary antenna 42 in the vehicle coordinate system as the position of the secondary antenna 42. The secondary antenna position determiner 82 determines the eighth coordinate (X8, Y8, Z8) based on the sixth coordinate (X6, Y6, Z6) and the second azimuth angle γ2. The second azimuth angle γ2 is acquired based on the fifth coordinate (S5, T5, U5) and the seventh coordinate (X7, Y7, Z7).

[0097] According to this, by measuring each coordinate in the measurement system coordinate system, the second azimuth angle γ2 can be easily and accurately obtained, and the eighth coordinate (X8, Y8, Z8) can be easily determined using the second azimuth angle γ2. Therefore, calibration can be easily and accurately performed for the position of the secondary antenna 42 in addition to the position of the primary antenna 41.

[0098] In particular, in the embodiment of the present invention, the work machine is a backhoe 10, and the movable operating device 30 includes a boom 31 connected to the traveling vehicle 20 via a boom pin 31a, an arm 33, and a bucket 34 connected to the arm 33 via a bucket pin 34a. In the primary antenna position determination unit 81, the center point on the boom pin 31a is used as the first origin O1. The center point on the bucket pin 34a is used as the first coordinate (X1, Y1, Z1). A point on the boom 31 is used as the second coordinate (S2, T2, U2). A point on the tip portion 34b of the bucket 34 is used as the third coordinate (S3, T3, U3). The center point on the bucket pin 34a is used as the fourth coordinate (S4, T4, U4).

[0099] This allows for the use of points that are easy to measure in the constituent parts of the backhoe 10 when measuring each coordinate of the measurement system coordinates. Therefore, each coordinate of the measurement system coordinates can be measured easily and accurately.

[0100] In particular, the embodiment of the present invention is provided with a vehicle turning center position determination unit 83 for 3D calibration. The vehicle turning center position determination unit 83 determines, as the position of the turning center of the traveling vehicle 20, a tenth coordinate (X10, Y10, Z10) corresponding to a turning center point P3 of the traveling vehicle 20 in the vehicle coordinate system. The vehicle turning center position determination unit 83 determines a tenth coordinate (X10, Y10, Z10) based on the first origin O1, the first coordinate (X1, Y2, Z3), the pitch angle θ2, the second coordinate (S2, T2, U2), the third coordinate (S3, T3, U3), the fourth coordinate (S4, T4, U4), the first azimuth angle γ1, and the four ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94). The four ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) are coordinates on the arc trajectory of a specified point on the movable operating device 30 that is traced by the turning of the traveling vehicle 20.

[0101] According to this, by measuring the four ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94), the turning center point C in the measurement system coordinates can be determined easily and accurately, and the tenth coordinate (X10, Y10, Z10) can be easily determined using the turning center point C. Therefore, in addition to the positions of the primary antenna 41 and the secondary antenna 42, calibration can be easily and accurately performed for the position of the turning center of the traveling vehicle 20.

[0102] In particular, in an embodiment of the present invention, the second coordinate (S2, T2, U2), the third coordinate (S3, T3, U3), the fourth coordinate (S4, T4, U4), the fifth coordinate (S5, T5, U5), the seventh coordinate (S7, T7, U7), and the four ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) are measured using an external coordinate measuring device 100 installed at a distance from the backhoe 10.

[0103] According to this, by using the external coordinate measuring device 100, it is possible to measure each coordinate in the measurement coordinate system more easily and with higher accuracy.

[0104] [Variations] In the position determination device 80 according to an embodiment of the present invention, the second coordinate (S2, T2, U2), the third coordinate (S3, T3, U3), the fourth coordinate (S4, T4, U4), the fifth coordinate (S5, T5, U5), the seventh coordinate (S7, T7, U7), and the four ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) are all measured using an external coordinate measuring device 100. Alternatively, for example, one or more of the second coordinate (S2, T2, U2), the third coordinate (S3, T3, U3), the fourth coordinate (S4, T4, U4), the fifth coordinate (S5, T5, U5), the seventh coordinate (S7, T7, U7), and the four ninth coordinates (S91, T91, U91), (S92, T92, U92), (S93, T93, U93), and (S94, T94, U94) may be measured using an external coordinate measuring device 100, rather than all of these.

[0105] The above-described embodiments disclosed herein are to be considered as illustrative in all respects and not restrictive. The scope of the present invention is defined by the claims, not by the above description, and is intended to include all modifications within the meaning and scope of the claims. [Explanation of symbols]

[0106] 10... Backhoe, 20... Traveling vehicle, 30... Movable operating device, 31... Boom, 31a... Boom pin, 33... Arm, 34... Bucket, 34a... Bucket pin, 34b... Tip portion, 34c... Wrist link, 40... Vehicle position detection device, 41... Primary antenna, 42... Secondary antenna, 50... Rotation angle detection device, 51... IMU, 52... IMU, 53... IMU, 54... IMU, 80... Position determination device, 81... Primary antenna position determination unit, 82... Secondary antenna position determination unit, 83... Vehicle turning center position determination unit, 85... Bucket tip portion position determination unit, 90... ECU, O1... First origin, O2... Second origin, (X1, Y2, Z3 )...first coordinate, (S2,T2,U2)...second coordinate, (S3,T3,U3)...third coordinate, (S4,T4,U4)...fourth coordinate, (S5,T5,U5)...fifth coordinate, (X6,Y6,Z6)...sixth coordinate, (S7,T7,U7)...seventh coordinate, (X8,Y8,Z8)...eighth coordinate, (S91,T91,U91)...ninth coordinate, (S92,T92,U92)...9th coordinate, (S93,T93,U93)...9th coordinate, (S94,T94,U94)...9th coordinate, (X10,Y10,Z10)...10th coordinate, B...vector, C...center of rotation, D...vector, E...vector, γ1...1st azimuth angle, γ2...2nd azimuth angle, θ1...roll angle, θ2...pitch angle

Claims

1. A traveling vehicle and a movable operating tool that is connected to the traveling vehicle via a connection part and extends in a direction toward a tip part that is spaced apart from the connection part, and that is rotatable relative to the traveling vehicle around the connection part; a first vehicle position measuring device mounted on a predetermined portion of the traveling vehicle and configured to measure the position of the mounted vehicle; a vehicle rotation degree acquisition device that acquires a rotation degree of the traveling vehicle; a movable manipulator rotation degree acquisition device that acquires a rotation degree of the movable manipulator; Equipped with The position of the tip portion of the movable operating tool is determined based on at least the position of the first vehicle position measuring device, the degree of rotation of the traveling vehicle, and the degree of rotation of the movable operating tool measured and acquired by the first vehicle position measuring device, the vehicle rotation degree acquiring device, and the movable operating tool rotation degree acquiring device, respectively. A work machine, a first direction along a horizontal plane from the connection portion toward the tip portion; a second direction that is perpendicular to the first direction, corresponds to a vehicle width direction of the traveling vehicle, and passes through the connection portion along the horizontal plane; a third direction that is perpendicular to the first direction and the second direction and passes through the connection portion along a vertical direction; a three-dimensional vehicle coordinate system having the connection portion as a first origin and configured by the first direction, the second direction, and the third direction; is stipulated, The vehicle rotation angle acquisition device is configured to be capable of achieving a zero roll angle state in which a roll angle indicating a degree of rotation of the traveling vehicle about an axis of the first direction, which is acquired by the vehicle rotation angle acquisition device, is zero in the vehicle coordinate system. A position determination device applied to a work machine, the first origin in the vehicle coordinate system in a state where the roll angle is zero; a first coordinate in the vehicle coordinate system corresponding to a predetermined point on the movable operating device in the zero roll angle state; a pitch angle indicating a degree of rotation of the traveling vehicle around an axis of the second direction in the vehicle coordinate system in a state where the roll angle is zero, the pitch angle being acquired by the vehicle rotation angle acquisition device; a second coordinate corresponding to a predetermined point on the movable operating tool in the zero roll angle state in a three-dimensional measurement coordinate system that has an arbitrary point as a second origin, a fourth direction that passes through the second origin along a horizontal plane, a fifth direction that is orthogonal to the fourth direction and passes through the second origin along the horizontal plane, and a sixth direction that is orthogonal to each of the fourth direction and the fifth direction and passes through the second origin along a vertical direction; a third coordinate in the measurement coordinate system corresponding to a predetermined point on the movable operating tool in the zero roll angle state, the third coordinate being different from the position of the second coordinate; a fourth coordinate in the measurement coordinate system corresponding to a predetermined point on the movable operating tool corresponding to the first coordinate in the roll angle zero state; a fifth coordinate in the measurement coordinate system corresponding to a predetermined point on the first vehicle position measuring device in the zero roll angle state; a first azimuth angle, which is an angle formed with respect to the fourth direction on the horizontal plane acquired based on the second coordinate and the third coordinate in the measurement coordinate system in a state where the roll angle is zero; Based on a first vehicle position measuring device position determining unit that determines, as the position of the first vehicle position measuring device, a sixth coordinate corresponding to a predetermined point on the first vehicle position measuring device in the vehicle coordinate system; It is equipped with At least one or more of the second coordinate, the third coordinate, the fourth coordinate, and the fifth coordinate in the measurement coordinate system are measured using an external coordinate measuring device installed away from the work machine. Positioning device.

2. 2. The position determining device according to claim 1, The work machine further comprises: a second vehicle position measuring device mounted on the traveling vehicle at a predetermined location different from the location of the first vehicle position measuring device, the second vehicle position measuring device measuring the location of the mounted location; In addition to the first vehicle position measuring device position determining unit, the determined sixth coordinate; and a second azimuth angle, which is an angle formed with respect to the fourth direction on the horizontal plane, acquired based on the fifth coordinate and a seventh coordinate corresponding to a predetermined point on the second vehicle position measuring device in the measurement coordinate system when the roll angle is zero; Based on a second vehicle position measuring device position determining unit that determines, as the position of the second vehicle position measuring device, an eighth coordinate corresponding to a predetermined point on the second vehicle position measuring device in the vehicle coordinate system; Equipped with Positioning device.

3. 3. The position determining device according to claim 2, the work machine is a backhoe, The movable operating tool is a boom having one end side and the other end side defined in the first direction, the one end side being connected to the traveling vehicle via a boom pin serving as the connection part, and the boom being rotatable around an axis passing through the boom pin that is parallel to the second direction; an arm having one end side and another end side defined in the first direction, the one end side being connected to the other end side of the boom; a bucket having one end side and the other end side defined in the first direction, the one end side being connected to the other end side of the arm via a bucket pin, the other end side being configured as the tip portion, and the bucket being rotatable around an axis passing through the bucket pin and parallel to the second direction; Equipped with The first vehicle position measuring device position determining unit As the first origin, Using a point on the boom pin in the vehicle coordinate system in the zero roll angle state, As the first coordinate, Using a point on the bucket pin in the vehicle coordinate system in the zero roll angle state, As the second coordinate, Using a point on the boom in the measurement coordinate system in the zero roll angle state, As the third coordinate, Using a point of the tip portion of the bucket in the measurement coordinate system in the zero roll angle state, As the fourth coordinate, configured to use a point on the bucket pin in the measurement coordinate system at the zero roll angle state Positioning device.

4. 4. The position determining device according to claim 3, The traveling vehicle is The movable operating tool is configured to be rotatable integrally with the movable operating tool around a predetermined axis, The first origin; the first coordinate; the second coordinate; the third coordinate; the fourth coordinate; the pitch angle; the first azimuth angle; a ninth coordinate on an arc locus of a predetermined point on the movable operating device, which is drawn by turning the traveling vehicle in the roll angle zero state, in the measurement coordinate system; Based on a vehicle turning center position determining unit that determines tenth coordinates corresponding to a point of the turning center of the traveling vehicle in the vehicle coordinate system as the position of the turning center of the traveling vehicle; Equipped with Positioning device.

5. 5. The position determining device according to claim 4, At least one of the seventh coordinate and the ninth coordinate in the measurement coordinate system is measured using an external coordinate measuring device installed away from the work machine. Positioning device.

Images