Trajectory acquisition device and mounting support device

Abstract

SOLUTION: A locus acquisition device (50) comprises: a camera (52); a storage unit (62) which stores a correspondence between a position of an upper hook (38) and a position of a prescribed portion (58) in advance; an information acquisition unit (64) which acquires angular information corresponding to each of a plurality of height positions of a lift arm while the lift arm (28) is operating in a vertical direction, acquires an image including the prescribed portion imaged by a camera when the lift arm is located at each height, and associates the angular information with the image for each height position; and a locus calculation unit (66) which calculates a three-dimensional locus of the upper hook corresponding to a rotation angle of the lift arm by using the correspondence between the plural sets of angular information and the image associated by the information acquisition unit.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to a trajectory obtaining device that obtains the trajectory of an upper hook of a hitch frame, and an attachment assistance device that assists in attaching a work implement to a tractor. [Background technology]

[0002] A work implement is attached to a tractor. Generally, a user manually attaches the work implement. For example, the user drives the tractor to bring it close to the work implement. The user then operates the work implement lifting device to move the hitch frame attached to the link mechanism (three-point link) upward. When the upper pin of the work implement is positioned in the path along which the upper hook moves, the upper hook lifts up the upper pin. The work implement then rotates around the upper pin toward the tractor, and the lower pin of the work implement fits into the lower support portion of the hitch frame. In this way, the work implement is attached to the tractor hitch frame.

[0003] Patent Document 1 discloses a technology for autonomously driving a tractor toward a work implement when the tractor is attached to the work implement. This technology uses a first camera on the tractor to capture an image of a part of the tractor, and a second camera on the tractor to capture an image of a target on the work implement, and controls the driving of the tractor using the images from each camera. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] US Patent Application Publication No. 2015 / 0077557 Summary of the Invention [Problem to be solved by the invention]

[0005] According to the technology of Patent Document 1, the user does not need to drive the tractor, but rather needs to operate the work implement lifting device and attach the work implement to the hitch frame attached to the link mechanism.

[0006] The link mechanism is made up of multiple components. Before attaching the implement to the tractor, the length of some of the components of the link mechanism, the connection positions between the components, etc. are adjusted as needed. When the link mechanism is adjusted, the trajectory of the upper hook of the hitch frame changes. In other words, the trajectory of the upper hook is not constant. For this reason, users must frequently check the position of the upper hook when attaching the implement. For these reasons, attaching the implement is not an easy task for users. Furthermore, attaching the implement takes time.

[0007] The present invention aims to solve the above-mentioned problems. [Means for solving the problem]

[0008] A first aspect of the present invention is a trajectory acquisition device that acquires the trajectory of an upper hook of a hitch frame attached to a tractor when the upper hook scoops up an upper pin of the work implement by operating the lift arm of the tractor to attach the work implement to the tractor, the trajectory acquisition device comprising: a camera attached to the tractor that photographs a predetermined portion that moves as the lift arm moves; a memory unit that pre-stores a correspondence between the position of the upper hook and the position of the predetermined portion; an information acquisition unit that acquires angle information corresponding to each of a plurality of height positions of the lift arm while the lift arm is moving in the up and down direction, and acquires images including the predetermined portion photographed by the camera when the lift arm is positioned at each height, and associates the angle information with the images for each height position; and a trajectory calculation unit that calculates the three-dimensional trajectory of the upper hook corresponding to the rotation angle of the lift arm using the correspondence between the plurality of sets of angle information and images associated by the information acquisition unit and the correspondence.

[0009] A second aspect of the present invention is an attachment assistance device that assists in the work of attaching the work machine to the tractor using the trajectory acquired by the trajectory acquisition device of the first aspect, and includes a movement calculation unit that calculates the movement direction and movement amount of the tractor, and a notification control unit that causes a notification device to notify the movement direction and movement amount calculated by the movement calculation unit, wherein the camera photographs a predetermined work machine side part of the work machine, the memory unit pre-stores a work machine side correspondence that associates the position of the work machine side part with the position of each part of the work machine, and the movement calculation unit calculates the three-dimensional position of each part of the work machine including the upper pin using an image including the work machine side part photographed by the camera and the work machine side correspondence, and calculates the movement direction and the movement amount required to match a part of the three-dimensional trajectory with the three-dimensional position of the upper pin.

[0010] A third aspect of the present invention is an attachment assistance device that assists in the work of attaching the work machine to the tractor using the trajectory acquired by the trajectory acquisition device of the first aspect, and includes a motion calculation unit that calculates the direction and amount of movement of the tractor, and a driving control unit that causes the tractor to drive autonomously based on the direction and amount of movement calculated by the motion calculation unit, wherein the camera photographs a predetermined work machine side part of the work machine, the memory unit pre-stores a work machine side correspondence that associates the positions of the work machine side part with the positions of each part of the work machine, and the motion calculation unit calculates the three-dimensional position of each part of the work machine including the upper pin using an image including the work machine side part photographed by the camera and the work machine side correspondence, and calculates the direction and amount of movement required to match a portion of the three-dimensional trajectory with the three-dimensional position of the upper pin. [Effects of the Invention]

[0011] According to the present invention, the user can easily mount the work implement on the tractor, thereby reducing the time required for the work implement mounting operation. [Brief explanation of the drawings]

[0012] [Figure 1] FIG. 1 is a side view of the rear of the tractor and the implement. [Figure 2] FIG. 2 is a side view of the rear of the tractor and the implement. [Figure 3] FIG. 3 is a configuration diagram of the trajectory acquisition device. [Figure 4] FIG. 4 is a flowchart of the trajectory acquisition process. [Figure 5] FIG. 5 is a configuration diagram of the wearing assistance device according to the first embodiment. [Figure 6] FIG. 6 is a flowchart of the guidance process according to the first embodiment. [Figure 7] FIG. 7 is a configuration diagram of a wearing assistance device according to the second embodiment. [Figure 8] FIG. 8 is a flowchart of the guidance process according to the second embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0013] [1 Configuration of tractor 10 and work implement 12] 1 and 2 are side views of the rear of the tractor 10 and the work implement 12. FIG. 1 shows a state in which the tractor 10 and the work implement 12 are spaced apart from each other. FIG. 2 shows a state in which the tractor 10 and the work implement 12 are connected to each other. In this embodiment, a combination of the tractor 10 positioned in front of the work implement 12 and the work implement 12 positioned behind the tractor 10 is assumed. However, the present invention is also applicable to a combination of the tractor 10 positioned behind the work implement 12 and the work implement 12 positioned in front of the tractor 10. In this case, the front, rear, left and right directions described below are reversed.

[0014] A hitch frame 20 may be used to facilitate the attachment of the work implement 12 to the tractor 10. This specification describes an embodiment in which the work implement 12 is attached to the tractor 10 via the hitch frame 20. A work implement lifting device 16, a link mechanism 18, and the hitch frame 20 are arranged at the rear of the body 14 of the tractor 10. A PTO shaft 22 protrudes rearward from the rear of the body 14. The PTO shaft 22 can supply a portion of the power of the tractor 10 to the work implement 12. The work implement 12 that requires power receives power from the tractor 10 via the PTO shaft 22. The work implement lifting device 16 has a pair of left and right lift arms 28. The work implement lifting device 16 can swing the left and right lift arms 28 up and down around a rotation axis extending left and right.

[0015] The link mechanism 18 is a three-point link and includes one top link 30, two lower links 32 (left and right), two lift rods 34 (left and right), and two tension rods 36 (left and right).

[0016] The top link 30 is located above the midpoint between the left and right lower links 32. The front end of the top link 30 is connected to the mounting part 24 at the rear of the vehicle body 14 via a ball joint. The top link 30 can swing up and down around the connection part with the mounting part 24. The rear end of the top link 30 is connected to the upper end of the hitch frame 20 via a ball joint. The top link 30 and the hitch frame 20 can swing up and down relative to each other.

[0017] The front end of each lower link 32 is connected to the rear of the vehicle body 14 via a ball joint. Each lower link 32 is capable of swinging up and down around the connection with the vehicle body 14. The rear end of each lower link 32 is connected to the hitch frame 20 via a ball joint. Each lower link 32 and the hitch frame 20 are capable of swinging up and down relative to each other.

[0018] The upper end of the left lift rod 34 is connected to the left lift arm 28. The lower end of the left lift rod 34 is connected to the left lower link 32. Similarly, the right lift rod 34 is connected to the right lift arm 28 and the right lower link 32. Each lift rod 34 has multiple parts that can be connected to the lower link 32. Each lower link 32 has multiple parts that can be connected to the lift rod 34. Therefore, the connection position between the lift rod 34 and the lower link 32 is variable. The length of the lift rod 34 may be variable.

[0019] The front end of the left tension rod 36 is connected to the vehicle body 14. The rear end of the left tension rod 36 is connected to the left lower link 32. Similarly, the right tension rod 36 is connected to the vehicle body 14 and the right lower link 32. The length of each tension rod 36 is variable. By tightening (restricting) each tension rod 36, it is possible to prevent the work implement 12 from swaying left and right relative to the tractor 10.

[0020] The hitch frame 20 is supported by one top link 30 and two lower links 32. The hitch frame 20 extends laterally from top to bottom. The hitch frame 20 has an upper hook 38 and two lower support portions 40, one on each side. The upper hook 38 is formed at the rear of the upper end of the hitch frame 20. The upper hook 38 is located at the center of the hitch frame 20 in the width direction (left-right direction). The upper hook 38 has an opening that faces approximately upward. The left lower support portion 40 is formed at the rear of the lower left end of the hitch frame 20. The right lower support portion 40 is formed at the rear of the lower right end of the hitch frame 20. Each lower support portion 40 has an opening that faces approximately upward and rearward. A locking mechanism (not shown) is provided in each lower support portion 40. The locking mechanism closes a portion of the opening in the lower support portion 40 to prevent the lower pin 48 of the work implement 12 from coming off the lower support portion 40.

[0021] A user of the tractor 10 can operate the work implement lifting device 16 using an operating device in the cabin. When the user operates the operating device to move the hitch frame 20 upward, a hydraulic mechanism (not shown) supplies hydraulic pressure for moving the hitch frame 20 upward. The left and right lift arms 28 then swing upward due to the action of the hydraulic pressure. When the lift arms 28 swing upward, the lift rods 34 connected to the lift arms 28 are pulled upward. Furthermore, the lower links 32 connected to the lift rods 34 swing upward about their front ends. This action causes the hitch frame 20 to swing upward. When the user stops the operation to move the hitch frame 20 upward, a locking mechanism is activated, and the hitch frame 20 and the attached work implement 12 maintain their stopped height. When the user operates the operating device to move the hitch frame 20 downward, the hydraulic mechanism (not shown) releases the hydraulic pressure for moving the hitch frame 20 upward. This causes the hitch frame 20 to swing downward. The lower part of the hitch frame 20 can swing around the front end of the lower link 32. The upper part of the hitch frame 20 can swing around the front end of the top link 30.

[0022] The work implement 12 is, for example, a rotary tiller. The work implement 12 is detachable from the hitch frame 20. The work implement 12 has a mast 44, an upper pin 46, and two lower pins 48 on the left and right. The mast 44 is disposed at the top of the work implement 12. The upper pin 46 is disposed on the mast 44. The upper pin 46 is a rod-shaped member that is parallel to the left-right direction of the work implement 12. Each lower pin 48 is disposed below the upper pin 46. The positional relationship between the upper pin 46 and the two lower pins 48 on the left and right matches the positional relationship between the upper hook 38 of the hitch frame 20 and the two lower support parts 40 on the left and right.

[0023] When the work implement 12 begins to be attached to the tractor 10, the upper pin 46 of the work implement 12 is positioned forward of the lower pins 48. If the upper pin 46 is positioned in the trajectory of the upper hook 38, which moves from bottom to top, the upper hook 38 can scoop up the upper pin 46 as it moves upward. The scooped up upper pin 46 is caught by the upper hook 38. Furthermore, as the upper hook 38 moves upward, the lower part of the work implement 12 rotates forward around the upper pin 46. As a result, each lower pin 48 fits into the lower support portion 40 of the hitch frame 20 located in front. At this time, the locking mechanism of the hitch frame 20 is activated, and each lower pin 48 is held in its corresponding lower support portion 40. The PTO shaft 22 of the tractor 10 is connected to the input shaft (not shown) of the work implement 12 via a universal joint or the like. In this manner, the work implement 12 is attached to the tractor 10.

[0024] [2 Orbit acquisition device 50] 3 and 4, the trajectory obtaining device 50 provided on the tractor 10 will be described. When attaching the work implement 12 to the tractor 10, the user moves the hitch frame 20 upward and uses the upper hook 38 to scoop up the upper pin 46 of the work implement 12. The trajectory obtaining device 50 obtains in advance the trajectory along which the upper hook 38 of the hitch frame 20 moves when the work implement is attached. Specifically, the trajectory obtaining device 50 obtains the three-dimensional trajectory of the tip of the upper hook 38 of the hitch frame 20. In this specification, the "three-dimensional trajectory of the tip of the upper hook 38 of the hitch frame 20" is also referred to as the "trajectory of the tip of the upper hook 38" or the "trajectory of the upper hook 38."

[0025] [2-1 Configuration] FIG. 3 is a configuration diagram of the trajectory acquisition device 50. The trajectory acquisition device 50 has a camera 52, an angle sensor 54, and a computing device 56. The trajectory acquisition device 50 also has a tractor marker 58. The tractor marker 58 has a specific mark that allows the distance from the camera 52 and the posture of the tractor marker 58 to be recognized by image recognition processing. The tractor marker 58 is attached to a location on the hitch frame 20 where the relative position with respect to the upper hook 38 does not change. For example, the tractor marker 58 is attached to the hitch frame 20.

[0026] The camera 52 is, for example, a monocular camera. However, the camera 52 may also be a stereo camera. The camera 52 is disposed in a position where it can capture an image of the tractor marker 58. For example, the camera 52 is attached to the rear end of the roof of the cabin of the tractor 10. The camera 52 transmits the captured image to the computing device 56.

[0027] The angle sensor 54 detects the rotation angle of the lift arm 28 around the rotation axis. The angle sensor 54 transmits angle information representing the detected rotation angle to the calculation device 56.

[0028] The calculation device 56 is, for example, a computer. The calculation device 56 includes a calculation unit 60 and a storage unit 62.

[0029] The calculation unit 60 has a processing circuit. The processing circuit may be a processor such as a CPU. The processing circuit may be an integrated circuit such as an ASIC or an FPGA. The processor can perform various processes by executing programs stored in the storage unit 62. The calculation unit 60 functions as an information acquisition unit 64, a trajectory calculation unit 66, and a validity determination unit 68. At least some of the processes may be performed by electronic circuits including discrete devices.

[0030] The information acquisition unit 64 acquires various pieces of information from devices external to the calculation unit 60. The information acquisition unit 64 also stores the acquired various pieces of information in the storage unit 62. For example, the information acquisition unit 64 associates images transmitted from the camera 52 with angle information transmitted from the angle sensor 54. The associated images and angle information form pairs. The information acquisition unit 64 stores the pairs of images and angle information in the storage unit 62. The trajectory calculation unit 66 calculates a three-dimensional trajectory of the tip of the upper hook 38 corresponding to the rotation angle of the lift arm 28 using multiple pairs of angle information and images and the correspondence between the position of the upper hook 38 and the position of the tractor marker 58. The validity determination unit 68 determines whether the trajectory of the upper hook 38 calculated by the trajectory calculation unit 66 is valid.

[0031] The storage unit 62 has a volatile memory and a non-volatile memory. Examples of the volatile memory include RAM. The volatile memory is used as a working memory for the processor. The volatile memory temporarily stores data required for processing or calculation. Examples of the non-volatile memory include ROM and flash memory. The non-volatile memory is used as a storage memory. The non-volatile memory stores programs, tables, maps, etc. At least a part of the storage unit 62 may be provided in the processor, integrated circuit, etc. described above.

[0032] The memory unit 62 stores information related to coordinate transformation. In this embodiment, the three three-dimensional coordinate systems shown in FIG. 1 are used. The first coordinate system C1 has its origin at the center position of the lens of the camera 52. The second coordinate system C2 has its origin at the position of a predetermined part of the tractor marker 58 (for example, the center position of the marker). The third coordinate system C3 has its origin at the position on the ground directly below the midpoint between the left and right rear wheels 11 of the tractor 10.

[0033] Of the three axes of the first coordinate system C1, the first axis (X axis) is parallel to the normal to the lens of the camera 52, the second axis (Y axis) is parallel to the left-right direction of the tractor 10, and the third axis (Z axis) is perpendicular to the first and second axes. Of the three axes of the second coordinate system C2, the first axis (X axis) is perpendicular to the marker display surface of the tractor marker 58, the second axis (Y axis) is parallel to the horizontal direction of the tractor marker 58, and the third axis (Z axis) is parallel to the vertical direction of the tractor marker 58. Of the three axes of the third coordinate system C3, the first axis (X axis) is parallel to the front-to-rear direction of the tractor 10, the second axis (Y axis) is parallel to the left-to-right direction of the tractor 10, and the third axis (Z axis) is parallel to the up-and-down direction.

[0034] In this embodiment, the trajectory of the tip of the upper hook 38 in the third coordinate system C3 is ultimately calculated using an image captured by the camera 52. However, the coordinates obtained from the image captured by the camera 52 are coordinates in the first coordinate system C1. Furthermore, the position of the tip of the upper hook 38 is uniquely determined in the second coordinate system C2. For this reason, the storage unit 62 stores a processing method (referred to as a first process) for calculating the coordinates of the tip position of the upper hook 38 in the first coordinate system C1 based on the origin position of the second coordinate system C2 in the first coordinate system C1 and the orientations of the three axes of the second coordinate system C2 in the first coordinate system C1. Furthermore, the storage unit 62 stores a processing method (referred to as a second process) for converting the coordinates in the first coordinate system C1 into coordinates in the third coordinate system C3.

[0035] [2-2 Orbit acquisition processing] 4 is a flowchart of the trajectory acquisition process. The user manually adjusts the link mechanism 18 according to the type of work implement 12 attached to the tractor 10. After adjusting the link mechanism 18, the user operates the operation device in the cabin to cause the trajectory acquisition device 50 to execute the trajectory acquisition process. Note that the trajectory acquisition process described below is performed before the work implement 12 is attached to the tractor 10.

[0036] In step S1, the user operates the work implement lifting device 16 using the operating device in the cabin. The work implement lifting device 16 swings the lift arm 28 upward or downward in response to the user's operation. Note that the work implement lifting device 16 may also swing the lift arm 28 alternately in both the upward and downward directions. The hitch frame 20 moves in response to the operation of the lift arm 28.

[0037] In step S2, the camera 52 captures an image of the tractor marker 58 attached to the hitch frame 20. The camera 52 captures an image every time a predetermined time elapses or every time the lift arm 28 moves by a predetermined angle. The information acquisition unit 64 acquires the image of the tractor marker 58 captured by the camera 52.

[0038] In step S3, the angle sensor 54 detects the rotation angle of the lift arm 28. The information acquisition unit 64 acquires the rotation angle detected by the angle sensor 54, that is, angle information.

[0039] In step S4, the information acquisition unit 64 associates the image acquired in step S2 with the angle information acquired in step S3. For example, the information acquisition unit 64 creates an image ID and associates the image ID with the angle information. The information acquisition unit 64 stores the image with the image ID in the storage unit 62, and also stores pairs of the image ID and the angle information in the storage unit 62. The storage unit 62 stores a table made up of pairs of image IDs and angle information.

[0040] In step S5, the information acquisition unit 64 determines whether or not the acquisition of information (pairs of images and angle information) has ended. For example, the information acquisition unit 64 may determine that the acquisition of information has ended when it recognizes that the rotation angle of the lift arm 28 has reached a predetermined angle. Alternatively, the information acquisition unit 64 may determine that the acquisition of information has ended when it recognizes that the operation amount of the lift arm 28 has reached a predetermined amount. Alternatively, the information acquisition unit 64 may determine that the acquisition of information has ended when it recognizes that a predetermined number of images have been acquired. Alternatively, the information acquisition unit 64 may determine that the acquisition of information has ended in response to an instruction from the user. If the acquisition of information has ended (step S5: YES), the process proceeds to step S6. At this stage, the user ends the operation of the work implement lifting device 16. On the other hand, if the acquisition of information has not ended (step S5: NO), the process returns to step S1.

[0041] In step S6, the trajectory calculation unit 66 randomly reads out multiple sets of angle information and image IDs from the table in the storage unit 62. In addition, the trajectory calculation unit 66 reads out images corresponding to each of the read out image IDs. Note that there is a possibility that the multiple images may include images in which the tractor marker 58 is not visible. The trajectory calculation unit 66 may perform image recognition processing such as pattern matching to determine whether the tractor marker 58 is visible in the read out image. Furthermore, if the tractor marker 58 is not visible in the read out image, the trajectory calculation unit 66 may discard the image.

[0042] In step S7, the trajectory calculation unit 66 calculates the trajectory of the upper hook 38 corresponding to the rotation angle of the lift arm 28. In order to calculate the trajectory of the upper hook 38, the trajectory calculation unit 66 performs the following series of processes using each of the images read out in step S6.

[0043] The trajectory calculation unit 66 calculates, from each image, the coordinates of the origin position of the second coordinate system C2 (the position of a predetermined portion of the tractor marker 58) in the first coordinate system C1 and the orientations of the three axes of the second coordinate system C2 in the first coordinate system C1. To calculate the coordinates and orientations, the trajectory calculation unit 66 uses, for example, an existing three-dimensional pose estimation method [such as PnP (Perspective-n-Point)]. The trajectory calculation unit 66 performs a first process using the calculated coordinates and the orientations of the three axes to calculate the coordinates of the tip position of the upper hook 38 in the first coordinate system C1. Next, the trajectory calculation unit 66 performs a second process to convert the coordinates of the tip position of the upper hook 38 in the first coordinate system C1 into the coordinates of the tip position of the upper hook 38 in the third coordinate system C3. The trajectory calculation unit 66 calculates the coordinates of the upper hook 38 in the third coordinate system C3 from each of the multiple images. Furthermore, the trajectory calculation unit 66 associates each of the coordinates of the upper hook 38 with a rotation angle of the lift arm 28 .

[0044] Next, the trajectory calculation unit 66 uses the calculated coordinates to calculate the trajectory of the tip of the upper hook 38. For example, the trajectory calculation unit 66 can calculate a three-dimensional trajectory by using a curve fitting method. Through the above processing, the trajectory calculation unit 66 obtains the three-dimensional trajectory of the tip of the upper hook 38.

[0045] The trajectory calculation unit 66 may use another method to calculate the trajectory of the upper hook 38. For example, the trajectory calculation unit 66 may calculate the trajectory of a predetermined portion of the tractor marker 58 and convert the obtained trajectory into the trajectory of the upper hook 38.

[0046] In step S8, the validity determination unit 68 checks whether the trajectory of the upper hook 38 calculated by the trajectory calculation unit 66 is appropriate. That is, the validity determination unit 68 determines the validity of the trajectory of the upper hook 38. The validity determination unit 68 reads multiple sets of angle information and image IDs from the table in the storage unit 62. Here, the validity determination unit 68 reads angle information and image IDs that are not used by the trajectory calculation unit 66. The validity determination unit 68 may read only one set of angle information and image IDs instead of multiple sets. In addition, the trajectory calculation unit 66 reads images corresponding to each of the read image IDs. The validity determination unit 68 calculates the coordinates of a predetermined portion of the tractor marker 58 using the method (such as a three-dimensional pose estimation method) used by the trajectory calculation unit 66 in step S7. These coordinates are referred to as determination coordinates. The validity determination unit 68 calculates the Euclidean distance between each determination coordinate and the trajectory of the upper hook 38 acquired in step S7. Furthermore, the validity determination unit 68 calculates the average value of each Euclidean distance. The validity determination unit 68 compares the average value with a threshold value for validity determination. The threshold value is, for example, approximately the width of the upper hook 38 in the left-right direction. The threshold value is pre-stored in the storage unit 62. Note that the validity determination unit 68 may calculate other statistics, such as variance or covariance, for each Euclidean distance instead of the average value.

[0047] If the average value is less than the threshold value (step S9: YES), the validity determination unit 68 determines that the trajectory of the upper hook 38 is valid. In this case, the process proceeds to step S10. On the other hand, if the average value is equal to or greater than the threshold value (step S9: NO), the validity determination unit 68 determines that the trajectory of the upper hook 38 is invalid. In this case, the process returns to step S6. Note that in step S6 from the second time onwards, the trajectory calculation unit 66 reads out a set of angle information and an image ID different from the set of angle information and an image ID read out in step S6 before the previous time, in order to prevent a recurrence of the trajectory of the upper hook 38 whose average value was determined to be equal to or greater than the threshold value in step S9.

[0048] In step S10, the trajectory calculation unit 66 stores the trajectory of the upper hook 38 that was determined to be valid in steps S8 and S9 in the storage unit 62. This completes the trajectory acquisition process.

[0049] In this manner, the trajectory acquisition device 50 acquires the three-dimensional trajectory of the upper hook 38 corresponding to the rotation angle of the lift arm 28 before the work of attaching the work implement 12 to the tractor 10. A rearview monitor is provided in the cabin of the tractor 10. The trajectory acquisition device 50 displays an image captured by the camera 52 or another camera on the rearview monitor. The trajectory acquisition device 50 may superimpose the trajectory of the upper hook 38 on the image displayed on the rearview monitor. The user can move the tractor 10 so that the upper pin 46 of the work implement 12 is positioned on this trajectory and operate the work implement lifting device 16, so that the upper hook 38 of the hitch frame 20 can lift up the upper pin 46 of the work implement 12. In this way, if the user can know the trajectory of the upper hook 38 of the hitch frame 20 in advance, the burden of the work of attaching the work implement 12 is reduced. In other words, the trajectory acquisition device 50 allows the user to easily attach the work implement 12 to the tractor 10. Furthermore, the trajectory obtaining device 50 can reduce the time required to install the work implement 12.

[0050] [3 Wearing Assist Device 70] 5 to 8, the mounting assistance device 70 provided on the tractor 10 will be described. The mounting assistance device 70 guides the tractor 10 to a position where the upper hook 38 can scoop up the upper pin 46.

[0051] 3-1 Wearing assistance device 70 of the first embodiment [3-1-1 Configuration of the wearing assistance device 70] 5 is a configuration diagram of a mounting assistance device 70 according to the first embodiment. The mounting assistance device 70 according to the first embodiment guides the tractor 10 to a position directly in front of the work implement 12 by informing the user of the direction and amount of movement of the tractor 10. The mounting assistance device 70 includes a trajectory obtaining device 50. For this reason, among the components of the mounting assistance device 70, components that are the same as those of the trajectory obtaining device 50 are given the same reference numerals as those of the trajectory obtaining device 50, and detailed descriptions thereof will be omitted.

[0052] The mounting assistance device 70 has a camera 52, an angle sensor 54, an inertial measurement unit 72, a computing device 56, and an alarm device 74. The mounting assistance device 70 also has a tractor marker 58 and a work implement marker 76. Like the tractor marker 58, the work implement marker 76 has a specific mark that allows the distance from the camera 52 and the posture of the work implement marker 76 to be recognized by image recognition processing. The work implement marker 76 is attached to a location on the work implement 12 where the relative position with respect to the upper pin 46 does not change. For example, the work implement marker 76 is attached to the mast 44. The work implement marker 76 is determined for each type of work implement 12.

[0053] The inertial measurement unit 72 has a three-axis gyro and a three-directional acceleration sensor. The inertial measurement unit 72 measures the acceleration and turning angular velocity of the tractor 10. The inertial measurement unit 72 transmits the measurement data to the calculation device 56.

[0054] The calculation unit 60 functions as an information acquisition unit 64, a trajectory calculation unit 66, a validity determination unit 68, a movement calculation unit 78, a notification control unit 80, and a position determination unit 82. The movement calculation unit 78 performs various calculations to match a portion of the trajectory calculated by the trajectory calculation unit 66 with the position of the upper pin 46 of the work implement 12. The notification control unit 80 outputs a notification instruction to the notification device 74. The position determination unit 82 determines whether the tractor 10 has reached the target position.

[0055] The memory unit 62 stores information related to coordinate transformation. In this embodiment, five three-dimensional coordinate systems shown in FIG. 1 are used. The first coordinate system C1 to the third coordinate system C3 are the same as the three coordinate systems used by the trajectory acquisition device 50. The fourth coordinate system C4 has its origin at the position of a predetermined part of the work implement marker 76 (for example, the marker center position). The fifth coordinate system C5 has its origin at the position on the ground directly below the center of the upper pin 46 (the center of the work implement 12 in the left-right direction), in other words, the position where the center of the upper pin 46 is projected onto the ground.

[0056] When the work implement 12 is in an upright position, of the three axes of the fourth coordinate system C4, the first axis (X axis) is perpendicular to the marker display surface of the work implement marker 76, the second axis (Y axis) is parallel to the horizontal direction of the work implement marker 76, and the third axis (Z axis) is parallel to the vertical direction of the work implement marker 76. Of the three axes of the fifth coordinate system C5, the first axis (X axis) is parallel to the front-to-rear direction of the work implement 12, the second axis (Y axis) is parallel to the left-to-right direction of the work implement 12, and the third axis (Z axis) is parallel to the up-and-down direction.

[0057] In the first embodiment, an image captured by the camera 52 is used to ultimately calculate the target position of the tractor 10 in the fifth coordinate system C5 and the orientation of the tractor 10. The storage unit 62 stores a processing method (referred to as a third process) for calculating the coordinates of the center position of the upper pin 46 in the first coordinate system C1 and the orientation of the upper pin 46 based on the origin position of the fourth coordinate system C4 in the first coordinate system C1 and the orientation of the three axes of the fourth coordinate system C4 in the first coordinate system C1. The storage unit 62 also stores the height of the upper pin 46 in the upright work implement 12 as information for calculating the origin position of the fifth coordinate system C5 from the origin position of the fourth coordinate system C4.

[0058] The notification device 74 has, for example, one or both of a display device including a display etc. and an audio device including a speaker etc. The notification device 74 issues a notification in response to a notification instruction output by the notification control unit 80.

[0059] [3-1-2 Guidance Processing] 6 is a flowchart of the guidance process of the first embodiment. The user operates the operation device in the cabin to cause the wearing assistance device 70 to execute the guidance process.

[0060] In step S11, the mounting assistance device 70 executes a series of trajectory acquisition processes shown in Fig. 4. As a result, the trajectory calculation unit 66 acquires the three-dimensional trajectory of the tip of the upper hook 38 of the hitch frame 20 in the third coordinate system C3.

[0061] In step S12, the camera 52 captures an image of the work implement marker 76 attached to the mast 44. The information acquisition unit 64 acquires the image captured by the camera 52. It is recommended that the user move the tractor 10 close to the work implement 12 before step S12 so that the work implement marker 76 is within the angle of view of the camera 52.

[0062] In step S13, the movement calculation unit 78 uses the image acquired in step S12 to calculate the origin position of the fourth coordinate system C4, i.e., the coordinates of a predetermined part of the work implement marker 76. For example, the movement calculation unit 78 calculates the coordinates of the origin position (coordinates of a predetermined part of the work implement marker 76) of the fourth coordinate system C4 in the first coordinate system C1 and the orientations of the three axes of the fourth coordinate system C4 in the first coordinate system C1 by the method (three-dimensional pose estimation method, etc.) performed by the trajectory calculation unit 66 in step S7 of Fig. 4.

[0063] In step S14, the movement calculation unit 78 calculates the current position of the tractor 10 in the fifth coordinate system C5 and the orientation of the tractor 10. The movement calculation unit 78 performs the following processing.

[0064] The motion calculation unit 78 performs a third process using the coordinates and the orientations of the three axes calculated in step S13 to calculate the coordinates of the center position of the upper pin 46 in the first coordinate system C1 and the orientation of the upper pin 46. Next, the motion calculation unit 78 calculates the coordinates of the origin of the fifth coordinate system C5 in the first coordinate system C1 and the orientations of the three axes of the fifth coordinate system C5 from the coordinates of the origin of the fourth coordinate system C4 and the height of the upper pin 46. Next, the motion calculation unit 78 performs a second process to convert the coordinates of the origin of the fifth coordinate system C5 in the first coordinate system C1 into coordinates of the third coordinate system C3. As a result, the origin of the fifth coordinate system C5 is determined as the coordinates of the third coordinate system C3. Next, the motion calculation unit 78 determines the origin of the third coordinate system C3 as the coordinates of the fifth coordinate system C5 based on the coordinates of the origin of the third coordinate system C3 and the coordinates of the origin of the fifth coordinate system C5. Furthermore, the movement calculation unit 78 calculates the orientation of the three axes of the third coordinate system C3 in the fifth coordinate system C5. Through the above processing, the current position of the tractor 10 relative to the work implement 12 and the orientation of the tractor 10 are identified.

[0065] In step S15, the movement calculation unit 78 sets a target position to be reached by the tractor 10. In this embodiment, the movement calculation unit 78 sets a target position of the tractor 10 in the fifth coordinate system C5 of the work implement 12 in order to move the tractor 10 to a position where the upper hook 38 of the hitch frame 20 of the tractor 10 can scoop up the upper pin 46 of the work implement 12. At the target position, the fore-and-aft direction of the tractor 10 and the fore-and-aft direction of the work implement 12 are parallel. The movement calculation unit 78 performs the following processing to set the target position.

[0066] The origin of the third coordinate system C3 and the origin of the fifth coordinate system C5 are both located on the ground. In other words, the height coordinate in the third coordinate system C3 and the height coordinate in the fifth coordinate system C5 are the same. The movement calculation unit 78 acquires a coordinate that matches the height of the upper pin 46 from the trajectory of the tip of the upper hook 38 in the third coordinate system C3. This is the coordinate of the position where the upper hook 38 scoops up the upper pin 46, and is the coordinate in the third coordinate system C3. This position is referred to as the hook position. The origin of the fifth coordinate system C5 is located directly below the hook position. In other words, the front-rear coordinate and the left-right coordinate of the hook position are the same as the origin of the fifth coordinate system C5. The movement calculation unit 78 calculates the front-rear coordinate and the left-right coordinate of the origin of the third coordinate system C3 in the fifth coordinate system C5 from the front-rear coordinate and the left-right coordinate of the hook position in the third coordinate system C3. The motion calculation unit 78 sets these coordinates as the target position in the fifth coordinate system C5.

[0067] In step S16, the movement calculation unit 78 calculates the direction and amount of movement of the tractor 10. Here, the movement calculation unit 78 calculates the direction and amount of movement required to guide the tractor 10 from the current position to the target position. The movement calculation unit 78 sets the current position of the tractor 10 calculated in step S14 as the starting position of the tractor 10. Furthermore, the movement calculation unit 78 calculates the actual amount and direction of movement of the tractor 10 using measurement data acquired from the inertial measurement unit 72, and continues to update the current position and current orientation of the tractor 10.

[0068] In step S17, the notification control unit 80 converts the movement direction and movement amount of the tractor 10 calculated in step S16 into a steering operation direction and operation amount. The notification control unit 80 outputs the steering operation direction and operation amount to the notification device 74 as a notification instruction to notify the user. The notification device 74 notifies the user of the steering operation direction and operation amount in accordance with the notification instruction. The user can drive the tractor 10 in accordance with the notification content of the notification device 74, thereby causing the tractor 10 to travel toward the target position.

[0069] When traveling the tractor 10 toward a target position, the user adjusts the height position of the hitch frame 20. Specifically, the user adjusts the rotation angle of the lift arm 28 so that the upper hook 38 of the hitch frame 20 is positioned lower than the upper pin 46 of the work implement 12. This prevents the upper hook 38 from hitting the upper pin 46 even when the tractor 10 approaches the work implement 12. The calculation device 56 may adjust the rotation angle of the lift arm 28. The rotation angle of the lift arm 28 may be stored in the memory unit 62. The rotation angle of the lift arm 28 may also be determined depending on the type of work implement 12.

[0070] In step S18, the position determination unit 82 compares the current position of the tractor 10 with the target position. The position determination unit 82 also compares the current orientation of the tractor 10 with the target orientation of the tractor 10 at the target position. If the current position of the tractor 10 matches the target position and the current orientation of the tractor 10 matches the target orientation of the tractor 10 (step S18: YES), the position determination unit 82 determines that the tractor 10 has reached the target position. In this case, the processing proceeds to step S19. On the other hand, if the current position of the tractor 10 does not match the target position or if the current orientation of the tractor 10 does not match the target orientation of the tractor 10 (step S18: NO), the position determination unit 82 determines that the tractor 10 has not reached the target position. In this case, the processing returns to step S16.

[0071] In step S19, the notification control unit 80 outputs a notification instruction to the notification device 74 to notify the user that the tractor 10 has reached the target position. The notification device 74 notifies the user that the tractor 10 has reached the target position in accordance with the notification instruction. At this stage, the user stops the tractor 10. This completes the guidance process.

[0072] When the guidance process is completed, the user operates the work implement lifting device 16 to mount the work implement 12 on the tractor 10. The computing device 56 may perform the mounting of the work implement 12 on the tractor 10. The rotation angle of the lift arm 28 during the mounting operation may be stored in the memory unit 62. The rotation angle of the lift arm 28 during the mounting operation may also be determined depending on the type of work implement 12.

[0073] In this way, the mounting assistance device 70 guides the tractor 10 to the front of the work implement 12. The mounting assistance device 70 allows the user to easily mount the work implement 12 on the tractor 10. Furthermore, the trajectory acquisition device 50 can reduce the time required for mounting the work implement 12.

[0074] 3-2 Second embodiment of the wearing assistance device 70 [3-2-1 Configuration of the wearing assistance device 70] FIG. 7 is a configuration diagram of a mounting assistance device 70 according to the second embodiment. The mounting assistance device 70 according to the second embodiment guides the tractor 10 to a position in front of the work implement 12 by autonomously driving the tractor 10. As with the first embodiment, the mounting assistance device 70 includes a trajectory obtaining device 50. Therefore, of the components of the mounting assistance device 70, the same components as those of the trajectory obtaining device 50 are given the same reference numerals as those of the trajectory obtaining device 50, and detailed descriptions thereof will be omitted. Furthermore, the same components as those of the first embodiment are given the same reference numerals as those of the first embodiment, and detailed descriptions thereof will be omitted.

[0075] The mounting assistance device 70 includes a camera 52 , an angle sensor 54 , an inertial measurement unit 72 , a computing unit 56 , a steering unit 84 , a driving unit 86 , and a braking unit 88 .

[0076] The calculation unit 60 functions as an information acquisition unit 64, a trajectory calculation unit 66, a validity determination unit 68, a motion calculation unit 78, a travel control unit 90, and a position determination unit 82. The travel control unit 90 outputs a steering instruction for the tractor 10 to the steering device 84. The travel control unit 90 also outputs an acceleration instruction for the tractor 10 to the drive device 86. The travel control unit 90 also outputs a deceleration and stop instruction for the tractor 10 to the braking device 88.

[0077] The steering device 84 steers the tractor 10 in accordance with a steering command output by the travel control unit 90. The drive device 86 accelerates the tractor 10 in accordance with an acceleration command output by the travel control unit 90. The braking device 88 decelerates or stops the tractor 10 in accordance with a deceleration / stop command output by the travel control unit 90.

[0078] [3-2-2 Guidance Processing] 8 is a flowchart of the guidance process of the second embodiment. The user operates the operating device in the cabin to cause the wearing assistance device 70 to execute the guidance process. The processes of steps S21 to S26 shown in FIG. 8 are the same as the processes of steps S11 to S16 shown in FIG. 6. The processes from step S27 onwards will be described below.

[0079] In step S27, the travel control unit 90 calculates the steering amount, acceleration, and deceleration of the tractor 10 based on the direction and amount of movement of the tractor 10 calculated in step S26. The travel control unit 90 outputs a steering command to the steering device 84 according to the steering amount. The travel control unit 90 also outputs an acceleration command to the drive device 86 according to the acceleration. The travel control unit 90 also outputs a deceleration command or a stop command to the braking device 88 according to the deceleration. This allows the tractor 10 to autonomously travel toward the target position.

[0080] The processing of step S28 is the same as the processing of step S18. If the current position of the tractor 10 matches the target position and the current orientation of the tractor 10 matches the target orientation of the tractor 10 (step S28: YES), the processing proceeds to step S29. On the other hand, if the current position of the tractor 10 does not match the target position or if the current orientation of the tractor 10 does not match the target orientation of the tractor 10 (step S28: NO), the processing returns to step S26.

[0081] In step S29, the travel control unit 90 outputs a stop command to the braking device 88. In response to the stop command, the braking device 88 stops the tractor 10 at the target position. This completes the guidance process.

[0082] When the guidance process is completed, the user operates the work implement lifting device 16 to mount the work implement 12 on the tractor 10.

[0083] In this way, the mounting assistance device 70 guides the tractor 10 to the front of the work implement 12. The mounting assistance device 70 allows the user to easily mount the work implement 12 on the tractor 10. Furthermore, the trajectory acquisition device 50 can reduce the time required for mounting the work implement 12.

[0084] [3-3 Other Embodiments of Wearing Assist Device 70] In the first and second embodiments, a specific work implement 12 is attached to the tractor 10. Alternatively, an unspecified work implement 12 may be attached to the tractor 10. For example, the memory unit 62 may store a table that associates the feature amount of the work implement marker 76 with information specific to the work implement 12 (height and width of the upper pin 46). In this case, the motion calculation unit 78 may calculate the feature amount of the work implement marker 76 by image recognition, and identify the information specific to the work implement 12 that corresponds to the feature amount.

[0085] A server external to the tractor 10 may store a table that associates the feature amounts of the work implement marker 76 with information specific to the work implement 12. In this case, it is preferable that the tractor 10 and the server be able to wirelessly communicate with each other.

[0086] [4 Inventions Obtained from the Embodiments] The invention that can be understood from the above embodiment will be described below.

[0087] A first aspect of the present invention is a trajectory acquisition device (50) that acquires the trajectory of an upper hook (38) of a hitch frame (20) attached to a tractor (10) when the upper hook (38) of the hitch frame (20) attached to the tractor lifts up an upper pin (46) of the work implement by operating the lift arm (28) of the tractor in order to mount the work implement on the tractor, the trajectory acquisition device (50) comprising: a camera (52) attached to the tractor that photographs a predetermined portion (58) that moves with the operation of the lift arm; and a storage unit ( an information acquisition unit (64) that acquires angle information corresponding to each of a plurality of height positions of the lift arm while the lift arm is moving in the up and down direction, and acquires images including the predetermined part photographed by the camera when the lift arm is positioned at each height, and associates the angle information with the images for each height position; and a trajectory calculation unit (66) that calculates the three-dimensional trajectory of the upper hook corresponding to the rotation angle of the lift arm using the plurality of sets of angle information and images associated by the information acquisition unit and the correspondence.

[0088] In a first aspect, the trajectory acquisition device further includes a validity determination unit (68) that determines whether the trajectory is valid, and the trajectory calculation unit calculates the three-dimensional trajectory of the upper hook using the angle information and the image of some of the multiple pairs of pairs associated by the information acquisition unit and the correspondence relationship, and the validity determination unit determines whether the three-dimensional position of the upper hook determined based on one or more pairs of angle information and the image not used by the trajectory calculation unit is within a range that is a predetermined distance away from the trajectory, and determines that the trajectory is valid if the three-dimensional position of the upper hook is within the range.

[0089] A second aspect of the present invention is an attachment assistance device (70) that assists in the work of attaching the work machine to the tractor using the trajectory acquired by a trajectory acquisition device, and includes a motion calculation unit (78) that calculates the direction and amount of movement of the tractor, and a notification control unit (80) that causes a notification device (74) to notify the direction and amount of movement calculated by the motion calculation unit, wherein the camera photographs a predetermined work machine side part (76) of the work machine, the memory unit pre-stores a work machine side correspondence that associates the position of the work machine side part with the position of each part of the work machine, and the motion calculation unit calculates the three-dimensional position of each part of the work machine including the upper pin using an image including the work machine side part photographed by the camera and the work machine side correspondence, and calculates the direction and amount of movement required to match a part of the three-dimensional trajectory with the three-dimensional position of the upper pin.

[0090] A third aspect of the present invention is an attachment assistance device that assists in the work of attaching the work machine to the tractor using the trajectory acquired by a trajectory acquisition device, and includes a motion calculation unit that calculates the direction and amount of movement of the tractor, and a travel control unit (90) that causes the tractor to travel autonomously based on the direction and amount of movement calculated by the motion calculation unit, wherein the camera photographs a predetermined work machine side part of the work machine, the memory unit pre-stores a work machine side correspondence that associates the positions of the work machine side part with the positions of each part of the work machine, and the motion calculation unit calculates the three-dimensional position of each part of the work machine including the upper pin using an image including the work machine side part photographed by the camera and the work machine side correspondence, and calculates the direction and amount of movement required to match a part of the three-dimensional trajectory with the three-dimensional position of the upper pin. [Explanation of symbols]

[0091] 10...Tractor 12...Work equipment 28...Lift arm 38...Upper hook 46...Upper pin 50...Trajectory acquisition device 52...Camera 58...Tractor marker (predetermined part) 62...Storage unit 64...Information acquisition unit 66...Trajectory calculation section 68...Validity judgment section 70... Wearing support device 74... Notification device 76... Work machine marker (work machine side part) 78... Motion calculation unit 80... Notification control unit 90... Driving control unit

Claims

1. A trajectory acquisition device that acquires a trajectory of an upper hook of a hitch frame attached to a tractor when the upper hook lifts up an upper pin of the work implement by operating a lift arm of the tractor to attach the work implement to the tractor, a camera attached to the tractor for capturing an image of a predetermined portion that moves in response to the operation of the lift arm; a storage unit that stores in advance a correspondence relationship between the position of the upper hook and the position of the predetermined portion; an information acquisition unit that acquires angle information corresponding to each of a plurality of height positions of the lift arm while the lift arm is moving in the up and down direction, acquires images including the predetermined portion photographed by the camera when the lift arm is positioned at each height, and associates the angle information with the images for each height position; a trajectory calculation unit that calculates the three-dimensional trajectory of the upper hook corresponding to the rotation angle of the lift arm using the plurality of sets of the angle information and the images associated by the information acquisition unit and the correspondence relationship; A trajectory acquisition device comprising:

2. The trajectory acquisition device according to claim 1, a validity determination unit that determines whether the trajectory is valid, the trajectory calculation unit calculates the three-dimensional trajectory of the upper hook using the angle information and the images and the correspondence relationship of some of the plurality of pairs associated by the information acquisition unit, The validity determination unit determines whether the three-dimensional position of the upper hook, which is determined based on one or more sets of angle information and the image not used by the trajectory calculation unit, is within a range that is a predetermined distance from the trajectory, and determines that the trajectory is valid if the three-dimensional position of the upper hook is within the range.

3. 3. A mounting assistance device that supports an operation of mounting the work implement to the tractor using the trajectory acquired by the trajectory acquisition device according to claim 1 or 2, a movement calculation unit that calculates the direction and amount of movement of the tractor; a notification control unit that causes a notification device to notify the movement direction and the movement amount calculated by the motion calculation unit; Equipped with The camera photographs a predetermined work machine side portion of the work machine, the storage unit stores in advance a work machine side correspondence relationship that associates the positions of the work machine side parts with the positions of each part of the work machine; The motion calculation unit uses an image including the work machine side parts captured by the camera and the work machine side correspondence relationship to calculate the three-dimensional position of each part of the work machine including the upper pin, and calculates the movement direction and the movement amount required to match a portion of the three-dimensional trajectory with the three-dimensional position of the upper pin. This is an attachment assistance device.

4. 3. A mounting assistance device that supports an operation of mounting the work implement to the tractor using the trajectory acquired by the trajectory acquisition device according to claim 1 or 2, a movement calculation unit that calculates the direction and amount of movement of the tractor; a travel control unit that causes the tractor to travel autonomously based on the movement direction and the movement amount calculated by the motion calculation unit; Equipped with The camera photographs a predetermined work machine side portion of the work machine, the storage unit stores in advance a work machine side correspondence relationship that associates the positions of the work machine side parts with the positions of each part of the work machine; The motion calculation unit uses an image including the work machine side parts captured by the camera and the work machine side correspondence relationship to calculate the three-dimensional position of each part of the work machine including the upper pin, and calculates the movement direction and the movement amount required to match a portion of the three-dimensional trajectory with the three-dimensional position of the upper pin. This is an attachment assistance device.

Images