Quick hooking structure and quick hooking control method

Abstract

The invention provides a quick hooking structure and a quick hooking control method.The quick hooking structure comprises a rack, a clamping mechanism, a first driving unit and a hooking assembly, and the rack is rotationally connected with a power mechanism through a first rotating shaft; the clamping mechanism is rotationally connected with the rack through a second rotating shaft; the clamping mechanism is driven by the first driving unit to rotate relative to the rack; the hooking assembly is rotationally connected with the power mechanism through a third rotating shaft, and the height of a hooking hook in the vertical direction and the distance of the hooking hook in the horizontal direction can be adjusted to adapt to the position of a to-be-hooked part of the agricultural implement; the clamping mechanism comprises a mechanism body, a clamping assembly and a second driving unit; under the driving of the second driving unit, the clamping assembly can do reciprocating motion so as to adapt to the position of a to-be-clamped part of the agricultural implement; in this way, the hooking structure can be adjusted in multiple degrees of freedom in the angle, the horizontal direction and the vertical direction, and rapid and accurate hooking between the hooking structure and the agricultural implement is achieved.

Description

Technical Field

[0001] The present application relates to the technical field of agricultural machinery and equipment, and in particular to a quick-hook structure and a quick-hook control method. Background Art

[0002] In the process of modern agricultural production, tractors, as core power equipment, need to be frequently connected with various agricultural machinery (such as seeders, harvesters, rotary tillers, etc.) to achieve different agricultural operation functions.

[0003] Currently, traditional hitching devices rely primarily on the forward and backward movement of the tractor and manual adjustment of direction to achieve alignment between the agricultural implement and the tractor during the hooking operation. This results in low hooking efficiency and poor hooking accuracy. For example, since the alignment of the tractor and agricultural implement can only be adjusted by moving the tractor forward and backward and turning, the operator often needs to repeatedly adjust the tractor's position and angle to achieve the appropriate relative position between the two. This makes the entire hooking process time-consuming, especially in large-scale farmland operations, which leads to longer production cycles. Traditional hitching devices generally lack multi-directional adjustment functions, making it difficult to quickly and accurately align the connection point between the agricultural implement and the tractor, resulting in difficult hooking. Summary of the Invention

[0004] In order to solve the above technical problems, the present application provides a quick-hook structure and a quick-hook control method.

[0005] In a first aspect of the present application, a quick-hook structure is provided, comprising a frame, a clamping mechanism, a first drive unit, and a hook assembly; The frame is rotatably connected to the power mechanism via a first rotating shaft; the clamping mechanism is rotatably connected to the frame via a second rotating shaft; one end of the first driving unit is rotatably connected to the frame, and the other end is rotatably connected to the clamping mechanism; under the drive of the first driving unit, the clamping mechanism is rotated relative to the frame; the hanging assembly is rotatably connected to the power mechanism via a third rotating shaft; The clamping mechanism includes a mechanism body, a clamping assembly, and a second drive unit; the clamping assembly is sleeved on the mechanism body; the second drive unit is disposed on the mechanism body and has an extended end connected to both ends of the clamping assembly; driven by the second drive unit, the clamping assembly can reciprocate to adapt to the position of the portion to be clamped of the agricultural implement; The hook assembly includes a third drive unit, a hooking hook arranged at the protruding end of the third drive unit, and a sixth drive unit; the fixed end of the sixth drive unit is rotatably connected to the power mechanism, and the protruding end is rotatably connected to the middle part of the third drive unit; under the drive of the third drive unit and the sixth drive unit, the hook can reciprocate to adapt to the position of the part to be hung of the agricultural machinery.

[0006] In some embodiments of the present application, the clamping assembly includes a mounting portion and a first clamping jaw and a second clamping jaw spaced apart from each other on the mounting portion.

[0007] In some embodiments of the present application, the mounting portion is provided with a plurality of groups of spaced-apart mounting holes; by adjusting the positions of the mounting holes adapted to the first clamping jaw and the second clamping jaw, the distance between the first clamping jaw and the second clamping jaw can be adjusted.

[0008] In some embodiments of the present application, the mechanism body includes a first body, a second body and a third body; the two ends of the first body are respectively connected to one end of the second body and one end of the third body; the other end of the second body is connected to the other end of the third body; a first hole for the clamping assembly to pass through is opened on the first body along the length direction of the first body; a second hole for installing a second rotating shaft is formed at the connection between the second body and the third body; a third hole for accommodating a frame is formed on the second body and the third body.

[0009] In some embodiments of the present application, the rack includes a rack body, a lifting adjustment assembly, and an adjustable rod assembly; The lifting adjustment assembly includes lifting assemblies symmetrically arranged on both sides of the frame body and a connecting shaft connecting the two lifting assemblies; The adjustable rod assembly is arranged on both sides of the frame body; One end of the adjustable component is arranged on the frame body and is rotatably connected to one end of the lifting component; the other end of the adjustable rod component is rotatably connected to the power mechanism.

[0010] In some embodiments of the present application, the adjustable rod assembly includes a first rod portion and a second rod portion that are detachably connected; the second rod portion is sleeved inside the first rod portion.

[0011] In some embodiments of the present application, a locking mechanism is further included; The locking mechanism includes a fourth drive unit, a locking fixed seat, a connecting structure and a locking gripper; the fixed end of the fourth drive unit is arranged on the frame, and the protruding end is connected to one end of the connecting structure; the other end of the connecting structure is movably sleeved on the locking gripper; the locking fixed seat is arranged on the frame; one end of the locking gripper is rotatably connected to the locking fixed seat; under the drive of the fourth drive unit, the locking gripper can clamp and lock the clamping mechanism.

[0012] A second aspect of the present application provides a method for controlling a quick hook, the method comprising the following steps: Step S1: Capture the image of the AprilTag code and obtain the pose matrix of the clamped part of the agricultural implement relative to the camera. ; Get the camera's position relative to the frame base point , the pose matrix of the gripper relative to the frame base point ; Step S2: Calculate the position matrix of the clamped part of the agricultural implement relative to the gripper using the transformation matrix formula according to the coordinate transformation path. , the transformation matrix formula is as follows: ; Right now, ; Step S3, from the pose matrix Extract the rotation matrix from , the rotation matrix Convert to Euler angle to get yaw angle , pitch angle ; From the pose matrix Extract the translation vector t and obtain the translation vector along the X-axis and Y-axis; where the yaw angle Indicates the deflection angle of the clamping mechanism with the second axis as the rotation axis in the horizontal direction compared to the initial state; pitch angle Indicates the tilt angle of the hanging structure relative to the initial state in the vertical direction, with the first rotation axis as the rotation axis; Step S4, through the yaw angle Get the target length of the first drive unit; through the pitch angle Obtain the target length of the fifth drive unit; obtain the target length of the second drive unit through the translation vector along the X axis; obtain the target movement distance of the power mechanism along the Y axis through the translation vector along the Y axis; Step S5, obtaining the current lengths of the first drive unit, the fifth drive unit, and the second drive unit, and using a PID algorithm to obtain motion variables of the first drive unit, the fifth drive unit, and the second drive unit according to the target lengths and the current lengths of the first drive unit, the fifth drive unit, and the second drive unit; Step S6: The first drive unit, the fifth drive unit, and the second drive unit perform corresponding actions according to the motion variables; the power mechanism adjusts the corresponding distance according to the target movement distance along the Y axis; Step S7, obtaining the real-time lengths of the first driving unit, the fifth driving unit, and the second driving unit, and calculating the errors between the real-time lengths and the target lengths; If the error is smaller than the preset threshold value for multiple consecutive times, the first driving unit, the fifth driving unit and the second driving unit stop moving.

[0013] In some embodiments of the present application, in step S1, the position matrix of the clamped part of the agricultural implement relative to the camera is obtained. The specific methods are as follows: Step S11, setting AprilTag codes on both sides of a to-be-clamped portion of the agricultural implement; wherein each AprilTag code is equidistant from the to-be-clamped portion of the agricultural implement; Step S12: Perform AprilTag detection based on the visp library to obtain the pose matrix of each AprilTag code relative to the camera. and ; Step S13, create a unit rotation matrix ; Create a translation matrix based on the distance between each AprilTag code and the part of the agricultural implement to be clamped and ; in, ; Step S14, by translating the matrix 、 and the unit rotation matrix Obtain the pose matrix of the clamped part of the agricultural implement from each AprilTag code 、 ; Step S15: The position matrix of the clamped part of the agricultural implement relative to each AprilTag code is calculated. 、 , the pose matrix of each AprilTag code relative to the camera and , obtain the pose matrix of the two agricultural implements to be clamped relative to the camera ; in, = ; = ; Step S16: The position matrix of the two agricultural implements to be clamped relative to the camera Average to obtain the final pose matrix of the clamped part of the agricultural implement relative to the camera .

[0014] In some embodiments of the present application, in step S3, the pose matrix Extract the rotation matrix from , the rotation matrix Convert to Euler angle to get yaw angle , pitch angle , roll angle The specific methods are as follows: Step S31, from the pose matrix Extract the rotation matrix from , in, ; Step S32: Use the Euler angles of the fixed coordinate system ZYX sequence to configure the angles as yaw angles , pitch angle , roll angle ; Then, the rotation matrix ; in, ; ; ; Step S33, from the rotation matrix The formula for extracting Euler angles is: ; ; in, , .

[0015] Compared with the prior art, the present invention has the following advantages and beneficial effects: the quick-hanging structure of the present application includes a frame, a clamping mechanism, a first drive unit, and a hanging assembly, wherein the frame is rotatably connected to the power mechanism through a first rotating shaft; the clamping mechanism is rotatably connected to the frame through a second rotating shaft; under the drive of the first driving unit, the clamping mechanism is rotated relative to the frame; the hanging assembly is rotatably connected to the power mechanism through a third rotating shaft, and can adjust the height of the hanging hook in the vertical direction and the distance in the horizontal direction to adapt to the position of the part to be hung of the agricultural machinery; the clamping mechanism includes a mechanism body, a clamping assembly, and a second drive unit; under the drive of the second drive unit, the clamping assembly can reciprocate to adapt to the position of the part to be clamped of the agricultural machinery; in this way, the hanging structure of the present application can perform multiple degrees of freedom adjustment in angle, horizontal direction, and vertical direction to achieve fast and precise hanging with the agricultural machinery.

[0016] It is to be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] The accompanying drawings, which constitute part of this document, are intended to provide a further understanding of this document. The exemplary embodiments and descriptions herein are intended to explain this document and do not constitute an improper limitation on this document. In the accompanying drawings: Figure 1 1 is a side view of a quick-hook structure (hook state) provided by an exemplary embodiment of the present application; Figure 2 1 is a top view of a quick-hook structure (hook state) provided by an exemplary embodiment of the present application; Figure 3 It is a structural diagram of a quick-hook structure provided by an exemplary embodiment of the present application; Figure 4 is a side view of a quick hitch structure provided by an exemplary embodiment of the present application; Figure 5 is a structural schematic diagram of a locking mechanism provided by an exemplary embodiment of the present application; Figure 6 is a schematic diagram of a portion to be clamped of an agricultural implement provided by an exemplary embodiment of the present application; Figure 7 is a schematic diagram of a clamping mechanism provided by an exemplary embodiment of the present application rotating around a second rotation axis; FIG8 is a schematic structural diagram of a hanging structure provided by an exemplary embodiment of the present application rotating around a first rotation axis; FIG9 is a schematic structural diagram of a hanging structure provided by an exemplary embodiment of the present application, in which the hanging structure rotates around a third rotation axis.

[0018] In the picture: 10A, agricultural implement; 10A1, part to be clamped; 10A2, part to be attached; 10A3, April Tag; 20A, first rotating shaft; 20B, second rotating shaft; 20C, third rotating shaft; 10. Frame; 20. Clamping mechanism; 30. First drive unit; 40. Hanging assembly; 50. Camera; 60. Locking mechanism; 101, frame body; 102, lifting adjustment assembly; 1021, fifth drive unit; 1022, second lifting arm; 1023, first lifting arm; 103, adjustable rod assembly; 201, mechanism body; 2012, locking fitting portion; 202, clamping assembly; 2021, mounting portion; 2022, first clamping jaw; 2023, second clamping jaw; 203, second driving unit; 401, third driving unit; 402, hook; 403, sixth driving unit; 601. Fourth drive unit; 602. Locking fixing seat; 603. Connecting structure; 604. Locking gripper. DETAILED DESCRIPTION

[0019] In order to make the purpose, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application. It should be noted that, in the absence of conflict, the embodiments in the present application and the features in the embodiments can be combined with each other in any way.

[0020] In the process of modern agricultural production, tractors, as core power equipment, need to be frequently connected with various agricultural machinery (such as seeders, harvesters, rotary tillers, etc.) to achieve different agricultural operation functions.

[0021] Currently, traditional hitching devices rely primarily on the forward and backward movement of the tractor and manual adjustment of direction to achieve alignment between the agricultural implement and the tractor during the hooking operation. This results in low hooking efficiency and poor hooking accuracy. For example, since the alignment of the tractor and agricultural implement can only be adjusted by moving the tractor forward and backward and turning, the operator often needs to repeatedly adjust the tractor's position and angle to achieve the appropriate relative position between the two. This makes the entire hooking process time-consuming, especially in large-scale farmland operations, which leads to longer production cycles. Traditional hitching devices generally lack multi-directional adjustment functions, making it difficult to quickly and accurately align the connection point between the agricultural implement and the tractor, resulting in difficult hooking.

[0022] Based on this, an exemplary embodiment of the present application provides a quick-attachment structure and a quick-attachment control method, wherein the quick-attachment structure of the present application includes a frame, a clamping mechanism, a first drive unit, and a hanging assembly, wherein the frame is rotatably connected to the power mechanism through a first rotating shaft; the clamping mechanism is rotatably connected to the frame through a second rotating shaft; under the drive of the first driving unit, the clamping mechanism is rotated relative to the frame; the hanging assembly is rotatably connected to the power mechanism through a third rotating shaft, and can adjust the height of the hanging hook in the vertical direction and the distance in the horizontal direction to adapt to the position of the part to be hung of the agricultural machinery; the clamping mechanism includes a mechanism body, a clamping assembly, and a second drive unit; under the drive of the second drive unit, the clamping assembly can reciprocate to adapt to the position of the part to be clamped of the agricultural machinery; in this way, the hanging structure of the present application can perform multiple degrees of freedom adjustment in angle, horizontal direction, and vertical direction to achieve fast and precise attachment with the agricultural machinery.

[0023] Example 1: An exemplary embodiment of the present application provides a quick hook structure, such as Figure 1 and 2 As shown, the quick-attachment structure is used to quickly and accurately achieve a three-point connection with the agricultural implement 10A. The agricultural implement 10A has three connection points, of which the upper connection point is the part to be attached 10A2, and the lower two connection points are located on the same horizontal plane, which are the parts to be clamped 10A1. The quick-attachment structure includes a frame 10, a clamping mechanism 20, a first drive unit 30, and a hook assembly 40; the frame 10 is rotatably connected to the power mechanism via a first rotating shaft 20A; the hook structure can swing up and down with the first rotating shaft 20A as the rotation axis, which is used to adjust the height of the hook structure in the vertical direction to adapt to the height of the part to be clamped 10A1 of the agricultural implement 10A, and the power mechanism can be a tractor; the clamping mechanism 20 is rotatably connected to the frame 10 via a second rotating shaft 20B; one end of the first drive unit 30 is rotatably connected to the frame 10, and the other end is rotatably connected to the frame 10. One end is rotatably connected to the clamping mechanism 20; under the drive of the first drive unit 30, the clamping mechanism 20 can rotate relative to the frame 10 with the second rotating shaft 20B as the rotating axis, so as to adjust the angle between the clamping mechanism 20 and the to-be-clamped portion 10A1 of the agricultural implement 10A; the hanging assembly 40 is rotatably connected to the power mechanism through the third rotating shaft 20C, and the hanging assembly 40 can rotate with the third rotating shaft 20C as the rotation center, so as to adjust the height of the free end of the hanging assembly 40, and can adapt to the height of the to-be-clamped portion 10A2 of the agricultural implement 10A.

[0024] like Figures 1 to 3 As shown, the frame 10 includes a frame body 101, a lift adjustment assembly 102, and an adjustable rod assembly 103. Two adjustable rod assemblies 103 are respectively arranged on either side of the frame body 101; one end of the adjustable assembly is disposed on the frame body 101. The lift adjustment assembly 102 includes lifting assemblies symmetrically arranged on either side of the frame body 101 and a connecting shaft connecting the two lifting assemblies. The lifting assembly includes a fifth drive unit 1021, a first lifting arm 1023, and a second lifting arm 1022. One end of the fifth drive unit 1021 is rotatably connected to one end of the adjustable rod assembly 103, and the other end is rotatably connected to the middle portion of the first lifting arm 1023. One end of the first lifting arm 1023 is rotatably connected to a power mechanism, and the other end is rotatably connected to the second lifting arm 1022. The other end of the second lifting arm 1022 is rotatably connected to the other end of the adjustable rod assembly 103. The adjustable rod assembly 103 is connected to one end of the fifth drive unit 1021 and is also rotatably connected to the power mechanism. When the fifth driving unit 1021 is extended, it can drive the first lifting arm 1023 to rotate relative to the power mechanism, thereby causing the second lifting arm 1022 to drive the frame body 101 and the clamping mechanism 20 to lift in the vertical direction.

[0025] Preferably, the adjustable rod assembly 103 includes a first rod portion and a second rod portion that are detachably connected. One end of the first rod portion is fixed to the frame body 101, and the second rod portion is sleeved within the first rod portion. The first rod portion may be a hollow steel tube, and the first and second rod portions are provided with multiple sets of matching mounting holes. By adjusting the position of the second rod portion relative to the first rod portion, the overall length of the attachment structure can be adjusted to accommodate different agricultural implements 10A.

[0026] like Figure 3 As shown, the clamping mechanism 20 includes a mechanism body 201, a clamping assembly 202, and a second drive unit 203; the clamping assembly 202 is sleeved on the mechanism body 201; the second drive unit 203 is arranged on the mechanism body 201 and the protruding ends are respectively connected to the two ends of the clamping assembly 202; preferably, the second drive unit 203 is a bidirectional hydraulic cylinder. Under the drive of the second drive unit 203, the clamping assembly 202 can reciprocate relative to the mechanism body 201 to adapt to the position of the to-be-clamped portion 10A1 of the agricultural implement 10A; the clamping assembly 202 includes a mounting portion 2021 and a first clamping jaw 2022 and a second clamping jaw 2023 spaced apart on the mounting portion 2021. Preferably, the mounting portion 2021 is a steel pipe. A plurality of groups of spaced-apart mounting holes are provided on the top of the mounting portion 2021; the first clamping jaw 2022 and the second clamping jaw 2023 are installed in the mounting holes through a mounting bracket; by adjusting the positions of the mounting holes adapted to the first clamping jaw 2022 and the second clamping jaw 2023, the distance between the first clamping jaw 2022 and the second clamping jaw 2023 can be adjusted, so that the hanging structure can be adapted to different agricultural machinery 10A.

[0027] The mechanism body 201 includes a first body, a second body and a third body; the two ends of the first body are respectively connected to one end of the second body and one end of the third body; the other end of the second body is connected to the other end of the third body; a first hole for the clamping assembly 202 to pass through is opened on the first body along the length direction of the first body; a second hole for installing the second rotating shaft 20B is formed at the connection between the second body and the third body; a third hole for accommodating the frame 10 is formed on the second body and the third body, and the third hole can provide an avoidance space for the relative rotation between the clamping mechanism 20 and the frame 10 to prevent interference with the rotational motion.

[0028] The hook assembly 40 includes a third drive unit 401, a hooking hook 402 arranged at the protruding end of the third drive unit 401, and a sixth drive unit 403; the fixed end of the sixth drive unit 403 is rotationally connected to the power mechanism, and the protruding end is rotationally connected to the middle part of the third drive unit 401; under the drive of the third drive unit 401 and the sixth drive unit 403, the hooking hook 402 can rotate around the third rotating shaft 20C as the rotation center, perform reciprocating motion, and adjust the height of the hooking hook 402 in the vertical direction and the distance in the horizontal direction to adapt to the position of the part 10A2 to be hung of the agricultural machinery 10A.

[0029] Preferably, the quick-hook structure of the present application further includes a locking mechanism 60; Figure 4 and 5 As shown, the locking mechanism 60 includes a fourth drive unit 601, a locking fixed seat 602, a connecting structure 603 and a locking gripper 604; the fixed end of the fourth drive unit 601 is arranged on the frame 10, and the protruding end is connected to one end of the connecting structure 603; the other end of the connecting structure 603 is movably sleeved on the locking gripper 604; the locking fixed seat 602 is arranged on the frame 10, and preferably the locking fixed seat 602 is arranged on the top of the first rotating shaft 20A; one end of the locking gripper 604 is rotatably connected to the locking fixed seat 602; under the drive of the fourth drive unit 601, the locking gripper 604 can be opened and closed to clamp and lock or release the clamping mechanism 20. Preferably, a locking mating portion 2012 is provided on the mechanism body 201, and a locking insertion hole is formed in the locking mating portion 2012. After the first clamping jaw 2022 and the second clamping jaw 2023 clamp the to-be-clamped portion 10A1 of the agricultural implement 10A, the piston rod of the fourth drive unit 601 extends, driving the connecting structure 603 toward the locking mating portion 2012. This in turn drives the locking grip 604 toward the locking insertion hole, allowing the end of the locking grip 604 to extend into the locking insertion hole, thereby locking the clamping mechanism 20. This facilitates the attachment of the attachment portion 10A2 of the agricultural implement 10A to the attachment assembly 40. Furthermore, the stability of the clamping mechanism 20 is ensured during operation of the agricultural implement 10A.

[0030] In the present application, in order to collect the coordinates of the to-be-clamped portion 10A1 and the to-be-attached portion 10A2 of the agricultural implement 10A in real time, a camera 50 is installed on the frame body 101 .

[0031] For example, in the present application, the first drive unit 30, the second drive unit 203, the third drive unit 401, the fourth drive unit 601, the fifth drive unit 1021, and the sixth drive unit 403 are all hydraulic cylinders. The control system can control the extension and retraction of the first drive unit 30, the second drive unit 203, the third drive unit 401, the fourth drive unit 601, the fifth drive unit 1021, and the sixth drive unit 403 of the present application, so as to achieve fast and precise attachment between the attachment structure and the agricultural machinery 10A.

[0032] Example 2: An exemplary embodiment of the present application provides a control method for quick hooking, which is controlled by a control system, including a control unit, a hydraulic drive system, etc., wherein the control unit is used to process data and generate hydraulic drive instructions; the hydraulic drive system can drive the hydraulic cylinder according to the hydraulic drive instructions, that is, the first drive unit 30, the second drive unit 203, the third drive unit 401, the fourth drive unit 601, the fifth drive unit 1021, and the sixth drive unit 403; each hydraulic cylinder is equipped with a position sensor to measure the telescopic length of the hydraulic cylinder; an angle sensor is installed on the first rotating shaft 20A, the second rotating shaft 20B and the third rotating shaft 20C to detect the rotation angle of each rotating shaft.

[0033] Assume that before clamping and attaching the agricultural implement 10A, the attachment structure is in an initial state. At this time, the attachment structure is parallel to the horizontal plane, and the frame body 101 and the mounting portion 2021 are parallel to each other in the X direction.

[0034] The control method comprises the following steps: Step S1: Capture the image of the AprilTag code 10A3 and obtain the pose matrix of the clamping part 10A1 of the agricultural implement 10A relative to the camera. ; Get the camera's position relative to the 10 base points of the frame , the pose matrix of the gripper relative to the 10 base points of the frame ; Step S2: Calculate the position matrix of the gripping portion 10A1 of the agricultural implement 10A relative to the gripper using the transformation matrix formula according to the coordinate transformation path. , the transformation matrix formula is as follows: ; Right now, ; Step S3, from the pose matrix Extract the rotation matrix from , the rotation matrix Convert to Euler angle to get yaw angle , pitch angle ; From the pose matrix Extract the translation vector t and obtain the translation vector along the X-axis and Y-axis; where the yaw angle Indicates the deflection angle of the clamping mechanism 20 relative to the initial state with the second rotation axis 20B as the rotation axis in the horizontal direction; Indicates the tilt angle of the hanging structure relative to the initial state in the vertical direction, with the first rotating shaft 20A as the rotating axis; Step S4, through the yaw angle Obtain the target length of the first drive unit 30; by the pitch angle Obtain the target length of the fifth drive unit 1021; obtain the target length of the second drive unit 203 through the translation vector along the X axis; obtain the target movement distance of the power mechanism along the Y axis through the translation vector along the Y axis; Step S5: Obtain the current lengths of the first drive unit 30, the fifth drive unit 1021, and the second drive unit 203. Use a PID algorithm to obtain motion variables of the first drive unit 30, the fifth drive unit 1021, and the second drive unit 203 based on the target lengths and current lengths of the first drive unit 30, the fifth drive unit 1021, and the second drive unit 203. Step S6: the first driving unit 30, the fifth driving unit 1021, and the second driving unit 203 perform corresponding actions according to the motion variables; the power mechanism adjusts the corresponding distance according to the target moving distance along the Y axis; Step S7, obtaining the real-time lengths of the first driving unit 30, the fifth driving unit 1021, and the second driving unit 203, and calculating the errors between the real-time lengths and the target lengths; If the error is less than the preset threshold multiple times in a row, the first drive unit 30, the fifth drive unit 1021, and the second drive unit 203 stop moving, and precise attachment can be achieved. For example, the preset error threshold can be 5 mm. If the error is less than 5 mm three times in a row, it is determined that the gripper of the attachment structure has successfully docked with the to-be-clamped portion 10A1 of the agricultural implement 10A, and precise attachment can be achieved.

[0035] If the error is greater than the preset threshold, steps S1 to S7 are repeated until the error is smaller than the preset threshold for multiple times in a row.

[0036] Specifically, in step S1, the pose matrix of the to-be-clamped portion 10A1 of the agricultural implement 10A relative to the camera is obtained. The specific methods are as follows: Step S11, as Figure 6 As shown, April Tags 10A3 are respectively provided on both sides of a to-be-clamped portion 10A1 of the agricultural implement 10A; wherein, each April Tag 10A3 is equidistant from the to-be-clamped portion 10A1 of the agricultural implement 10A; Step S12: Perform AprilTag detection based on the posture estimation and other related functions of the visp library to obtain the pose matrix of each AprilTag code 10A3 relative to the camera. and ; For the convenience of calculation, the center of each AprilTag code 10A3, the center of the to-be-clamped portion 10A1 of the agricultural implement 10A, and the center of the camera are taken to calculate the corresponding pose matrix.

[0037] Step S13, create a unit rotation matrix Create a translation matrix based on the position of each AprilTag code 10A3 from the agricultural implement 10A to be clamped 10A1 and ; in, ; Indicates XX; Indicates XX; express; express; express; express; Indicates the distance in the X direction between the center of the first AprilTag code 10A3 and the center of the part to be clamped 10A1; Indicates the distance in the Y direction between the center of the first AprilTag code 10A3 and the center of the part to be clamped 10A1; Indicates the Z-direction distance between the center of the first AprilTag code 10A3 and the center of the part to be clamped 10A1; Indicates the distance in the X direction between the center of the second AprilTag code 10A3 and the center of the part to be clamped 10A1; Indicates the distance in the Y direction between the center of the second AprilTag code 10A3 and the center of the part to be clamped 10A1; Indicates the distance between the center of the second AprilTag code 10A3 and the center of the ear in the Z direction; Step S14, by translating the matrix 、 and the unit rotation matrix Obtain the pose matrix of the to-be-clamped portion 10A1 of the agricultural implement 10A from each AprilTag code 10A3 、 ; and They are 、 The translation part, the unit rotation matrix is their rotation part; we can get:

[0038]

[0039] Step S15: Based on the position matrix of the to-be-clamped portion 10A1 of the agricultural implement 10A from each AprilTag code 10A3 、 , the pose matrix of each AprilTag code 10A3 relative to the camera and , obtain the pose matrix of the clamped part 10A1 of the two agricultural implements 10A relative to the camera ; in, = ; = ; Step S16: The position matrix of the clamped parts 10A1 of the two agricultural implements 10A relative to the camera The final pose matrix of the clamped part 10A1 of the agricultural implement 10A relative to the camera is obtained by averaging. .

[0040] by For example, the matrix is ​​in 4*4 format, as follows: ; Among them, the rotation matrix ; Translation Matrix ; Extracting the rotation matrix ; And use the trace and matrix elements to derive the quaternion component method, the rotation matrix Convert to quaternion ; The conversion method is as follows: Calculation trace: ; judge Is it close to zero? like , but, , , , ; like , select the largest diagonal element, for example, if is the largest diagonal element, but , , ; Normalized quaternion: , ; Unified symbol: If , then retain ,like , then take .

[0041] Follow the above steps to get the pose matrix , the pose matrix The rotation matrix is ​​converted to a quaternion, and then the two quaternions are averaged and normalized to obtain the averaged quaternion ; The averaged quaternion will be obtained , converted to a rotation matrix ,but ; The pose matrix The translation matrix is ​​weighted averaged to obtain the translation matrix ,but ; The translation matrix and the rotation matrix Combine to get the pose matrix .

[0042] The top center of the second rotating shaft 20B is the base point of the frame 10. Since the center of the camera coincides with the rotation center of the second rotating shaft 20B, the camera has only a translation matrix relative to the base of the docking mechanism. It can be obtained based on the camera installation height and camera size. Therefore, the camera's position relative to the 10 base points of the frame is: = .

[0043] The pose matrix of the gripper relative to the 10 base points of the frame The calculation process is as follows: In the process of the gripper clamping the to-be-clamped portion 10A1 of the agricultural implement 10A, the following actions need to be performed: yaw rotation, that is, the clamping mechanism 20 rotates with the second rotating shaft 20B as the rotation axis under the drive of the first driving unit 30, and the deflection angle compared to the initial state is the yaw angle Pitch rotation, that is, the hanging structure rotates with the first rotating shaft 20A as the rotation axis under the drive of the fifth driving unit 1021, and the tilt angle compared to the initial state is the pitch angle Translation, that is, the clamping assembly 202 is translated in the horizontal direction under the drive of the second driving unit 203, and the translation amount is .

[0044] like Figure 7 As shown, when the first driving unit 30 drives the clamping mechanism 20 to rotate around the second rotation axis 20B, a triangle is formed between the two end points of the first driving unit 30 and the rotation center, wherein the variable is the length of the first drive unit 30; when the initial position is set, the mounting portion 2021 of the clamping assembly 202 is parallel to the X-axis. , the angle of ∠BAC is , and remain unchanged; Applying the law of cosines to triangle ABC, we get ; After the formula transformation, we get: ; Then the yaw transformation matrix around the second rotation axis 20B is .

[0045] As shown in FIG8 , when the fifth driving unit 1021 drives the hanging structure to rotate with the first rotating shaft 20A as the rotating axis, a triangle is formed between the two end points of the fifth driving unit 1021 and the rotation center. When the initial position is set, the extension amount of the fifth driving unit 1021 is 0. When ∠B'A'C' is ; Applying the law of cosines to triangle A'B'C', we get

[0046] Then the pitch transformation matrix around the first rotation axis 20A is .

[0047] When the mounting portion 2021 of the clamping assembly 202 is at the center position, the displacement along the X axis is set to =0, at this time, the real-time reading of the second driving unit 203 is When the second drive unit 203 drives the mounting portion 2021 to move, the real-time reading of the second drive unit 203 is ; Formulate a simple first-order linear equation:

[0048] in, is the proportionality coefficient; Translation matrix along the X axis .

[0049] The power mechanism adjusts the yaw angle , pitch angle Before the X-axis translation, a rough alignment is performed to reduce the distance between the agricultural implement 10A and the Y-axis direction by the movement of the power mechanism. Assume that the power mechanism moves a distance of , The Y-axis translation can be extracted from the pose matrix of the part to be clamped relative to the gripper and compensated by controlling the wheel speed; Then the Y-axis translation matrix ; According to the forward kinematics equation, the position matrix of the gripper relative to the 10 base points of the frame is obtained. ; ; According to the coordinate transformation path, the position matrix of the to-be-clamped part 10A1 of the agricultural implement 10A relative to the gripper is obtained. , .

[0050] In step S3, from the pose matrix Extract the rotation matrix from , the rotation matrix Convert to Euler angle to get yaw angle , pitch angle , roll angle The specific methods are as follows: Step S31, from the pose matrix Extract the rotation matrix , in, ; Step S32: Use the Euler angles of the fixed coordinate system ZYX sequence to configure the angles as yaw angles , pitch angle , roll angle ; Then, the rotation matrix ; in, ; ; ; Step S33, from the rotation matrix The formula for extracting Euler angles is: ; ; in, , ; Step S34: Obtain the rotation angle of the gripping portion 10A1 of the agricultural implement 10A relative to the gripper in the current state, wherein the yaw angle , pitch angle .

[0051] From the pose matrix of the gripping part 10A1 of the agricultural implement 10A relative to the gripper Extract the translation vector t from Axis translation ,along Axis translation ; Get the movement of the gripper along the X axis , the amount of movement along the Y axis .

[0052] After the clamping of the portion to be clamped 10A1 is completed, the hanging structure is straightened and restored to the initial state, and then the locking mechanism 60 locks the clamping mechanism 20, and then the portion to be hung 10A2 is hung.

[0053] It may also include step S8, obtaining the real-time length of the third driving unit 401 , the real-time length of the sixth drive unit 403 According to the forward kinematics equation, the position matrix of the hook 402 relative to the base point of the frame 10 is obtained ; Using the transformation matrix formula, obtain the posture matrix of the agricultural implement 10A's hook 402 relative to the part to be hooked 10A2 ; From the pose matrix Extract the rotation matrix and convert it into Euler angle to get the pitch angle ; From the pose matrix Extract the translation vector and obtain the translation amount along the Y axis; use the PID algorithm according to the real-time length of the third driving unit 401 , the real-time length of the sixth drive unit 403 , obtain the motion variables of the third drive unit 401 and the sixth drive unit 403; the third drive unit 401 and the sixth drive unit 403 perform corresponding actions based on the motion variables; obtain the real-time lengths of the third drive unit 401 and the sixth drive unit 403, and calculate the error between the real-time lengths and the target lengths. If the error is less than a preset threshold value multiple times in a row, the third drive unit 401 and the sixth drive unit 403 stop moving. For example, the preset error threshold may be 5 mm. If the error is less than 5 mm three times in a row, it is determined that the attachment structure has successfully docked with the to-be-attached portion 10A2 of the agricultural implement 10A, achieving precise attachment.

[0054] The relationship between the part to be clamped 10A1 and the part to be attached 10A2 of the agricultural implement 10A is a fixed and known translation offset, which is a design constant and can be measured from the model of the agricultural implement 10A; therefore, the pose matrix of the part to be attached 10A2 relative to the part to be clamped 10A1 is defined as , the pose matrix is ​​a pure translation; the pose matrix of the part to be attached 10A2 relative to the base point of the frame 10 is calculated by matrix transformation , ; in, is the position matrix of the part to be clamped 10A1 relative to the base point of the frame 10; Since the gripper has already gripped the portion to be gripped 10A1, ; ; The pose matrix of the coupling hook 402 of the agricultural implement 10A relative to the part to be coupled 10A2 is: ; in, is the pose matrix of the hitch 402 of the agricultural implement 10A relative to the base point of the frame 10.

[0055] The pose matrix of the hitch 402 of the agricultural implement 10A relative to the base point of the frame 10 , obtained by the following method: The third drive unit 401 rotates around the third shaft 20C, that is, around the rotation center D; the sixth drive unit 403 rotates around the hinge point F with the power mechanism, that is, around the rotation center E; therefore, the third drive unit 401, the sixth drive unit 403 and the power mechanism form a triangle as shown in Figure 9.

[0056] set up is the real-time length of the sixth driving unit 403, which can be measured by a displacement sensor; is the distance between the rotation center E and the rotation center D; is the distance from the rotation center D to the hinge point between the sixth drive unit 403 and the third drive unit 401; =0; is the rotation angle of DF relative to the initial position; Applying the law of cosines to △DEF: ; After transformation, we can get: ; Get the pitch transformation matrix ; Set the distance that the hook 402 needs to extend to ; is the real-time length of the third driving unit 401, which can be measured by the displacement sensor; when the extended distance When the length of the hydraulic cylinder is ; The proportional coefficient is , used to describe the relationship between the change in the length of the hydraulic cylinder and the actual extension distance; but ; Get the scaling and translation matrix ; After the clamping of the part to be clamped 10A1 is completed, the hanging structure is corrected. At this time, the position of the clamping claw relative to the base point of the frame 10 can be obtained. ; Obtain the pose matrix of the hook 402 of the agricultural implement 10A relative to the base point of the frame 10 :

[0057] = .

[0058] In this application, the terms "comprises," "comprising," or any other variations thereof are intended to encompass non-exclusive inclusion, such that an article or device comprising a list of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such article or device. In the absence of further limitations, an element defined by the phrase "comprising..." does not exclude the presence of additional identical elements in the article or device comprising the element.

[0059] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.

[0060] Obviously, those skilled in the art may make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if such changes and modifications of the present application fall within the scope of the claims of the present application and their equivalents, the present application is intended to include such changes and modifications.

Claims

1. A quick hooking structure, characterized in that: It includes a frame, a clamping mechanism, a first driving unit, and a hanging assembly; The frame is rotatably connected to the power mechanism via a first rotating shaft; the clamping mechanism is rotatably connected to the frame via a second rotating shaft; one end of the first driving unit is rotatably connected to the frame, and the other end is rotatably connected to the clamping mechanism; under the drive of the first driving unit, the clamping mechanism is rotated relative to the frame; the hanging assembly is rotatably connected to the power mechanism via a third rotating shaft; The clamping mechanism includes a mechanism body, a clamping assembly, and a second driving unit; the clamping assembly is sleeved on the mechanism body; the second driving unit is arranged on the mechanism body and its protruding ends are respectively connected to the two ends of the clamping assembly; Under the drive of the second driving unit, the clamping assembly can reciprocate to adapt to the position of the to-be-clamped portion of the agricultural implement; The hook assembly includes a third drive unit, a hook provided at an extended end of the third drive unit, and a sixth drive unit; the fixed end of the sixth drive unit is rotatably connected to the power mechanism, and the extended end is rotatably connected to the middle portion of the third drive unit; Driven by the third driving unit and the sixth driving unit, the coupling hook can reciprocate to adapt to the position of the to-be-coupled portion of the agricultural implement.

2. The quick-hook structure according to claim 1, characterized in that: The clamping assembly includes a mounting portion and a first clamping jaw and a second clamping jaw spaced apart from each other on the mounting portion.

3. The quick-hook structure according to claim 2, characterized in that: The mounting portion is provided with a plurality of groups of mounting holes arranged at intervals; by adjusting the positions of the mounting holes adapted to the first clamping jaw and the second clamping jaw, the distance between the first clamping jaw and the second clamping jaw can be adjusted.

4. The quick-hook structure according to claim 1, characterized in that: The mechanism body includes a first body, a second body and a third body; the two ends of the first body are respectively connected to one end of the second body and one end of the third body; the other end of the second body is connected to the other end of the third body; a first hole for the clamping assembly to pass through is opened on the first body along the length direction of the first body; a second hole for installing a second rotating shaft is formed at the connection between the second body and the third body; a third hole for accommodating a frame is formed on the second body and the third body.

5. The quick-hook structure according to claim 1, characterized in that: The frame includes a frame body, a lifting adjustment assembly and an adjustable rod assembly; The lifting adjustment assembly includes lifting assemblies symmetrically arranged on both sides of the frame body and a connecting shaft connecting the two lifting assemblies; The adjustable rod assembly is arranged on both sides of the frame body; One end of the adjustable component is arranged on the frame body and is rotatably connected to one end of the lifting component; the other end of the adjustable rod component is rotatably connected to the power mechanism.

6. The quick-hook structure according to claim 5, characterized in that: The adjustable rod assembly comprises a first rod portion and a second rod portion which are detachably connected; the second rod portion is sleeved in the first rod portion.

7. The quick-hook structure according to claim 1, characterized in that: Also included is a locking mechanism; The locking mechanism includes a fourth drive unit, a locking fixing seat, a connecting structure, and a locking gripper; the fixed end of the fourth drive unit is arranged on the frame, and the extended end is connected to one end of the connecting structure; the other end of the connecting structure is movably sleeved on the locking gripper; the locking fixing seat is arranged on the frame; one end of the locking gripper is rotatably connected to the locking fixing seat; Driven by the fourth driving unit, the locking gripper can clamp and lock the clamping mechanism.

8. A method for controlling quick hooking, characterized in that: The control method comprises the following steps: Step S1: Capture the image of the AprilTag code and obtain the pose matrix of the clamped part of the agricultural implement relative to the camera. ; Get the camera's position relative to the base point of the frame , the pose matrix of the gripper relative to the frame base point ; Step S2: Calculate the position matrix of the clamped part of the agricultural implement relative to the gripper using the transformation matrix formula according to the coordinate transformation path. , the transformation matrix formula is as follows: ； Right now, ; Step S3, from the pose matrix Extract the rotation matrix , the rotation matrix Convert to Euler angle to get yaw angle , pitch angle ; From the pose matrix Extract the translation vector t and obtain the translation vector along the X-axis and Y-axis; where the yaw angle Indicates the deflection angle of the clamping mechanism with the second axis as the rotation axis in the horizontal direction compared to the initial state; pitch angle Indicates the tilt angle of the hanging structure relative to the initial state in the vertical direction, with the first rotation axis as the rotation axis; Step S4, through the yaw angle Get the target length of the first drive unit; through the pitch angle Obtain the target length of the fifth drive unit; obtain the target length of the second drive unit through the translation vector along the X axis; obtain the target movement distance of the power mechanism along the Y axis through the translation vector along the Y axis; Step S5, obtaining the current lengths of the first drive unit, the fifth drive unit, and the second drive unit, and using a PID algorithm to obtain motion variables of the first drive unit, the fifth drive unit, and the second drive unit according to the target lengths and the current lengths of the first drive unit, the fifth drive unit, and the second drive unit; Step S6: The first drive unit, the fifth drive unit, and the second drive unit perform corresponding actions according to the motion variables; the power mechanism adjusts the corresponding distance according to the target movement distance along the Y axis; Step S7, obtaining the real-time lengths of the first driving unit, the fifth driving unit, and the second driving unit, and calculating the errors between the real-time lengths and the target lengths; If the error is smaller than the preset threshold value for multiple consecutive times, the first driving unit, the fifth driving unit and the second driving unit stop moving.

9. The method for controlling quick hooking according to claim 8, characterized in that: In step S1, the pose matrix of the clamped part of the agricultural implement relative to the camera is obtained. The specific methods are as follows: Step S11, setting AprilTag codes on both sides of a to-be-clamped portion of the agricultural implement; wherein each AprilTag code is equidistant from the to-be-clamped portion of the agricultural implement; Step S12: Perform AprilTag detection based on the visp library to obtain the pose matrix of each AprilTag code relative to the camera. and ; Step S13, create a unit rotation matrix ; Create a translation matrix based on the distance between each AprilTag code and the part of the agricultural implement to be clamped and ; in, ; Step S14, by translating the matrix 、 and the unit rotation matrix Obtain the pose matrix of the clamped part of the agricultural implement from each AprilTag code 、 ; Step S15: The position matrix of the clamped part of the agricultural implement relative to each AprilTag code is calculated. 、 , the pose matrix of each AprilTag code relative to the camera and , obtain the pose matrix of the two agricultural implements to be clamped relative to the camera ; in, = ; = ; Step S16: The position matrix of the two agricultural implements to be clamped relative to the camera Average to obtain the final pose matrix of the clamped part of the agricultural implement relative to the camera .

10. The method for controlling quick hooking according to claim 8, characterized in that: In step S3, from the pose matrix Extract the rotation matrix , the rotation matrix Convert to Euler angle to get yaw angle , pitch angle , roll angle The specific methods are as follows: Step S31, from the pose matrix Extract the rotation matrix , in, ; Step S32: Use the Euler angles of the fixed coordinate system ZYX sequence to configure the angles as yaw angles , pitch angle , roll angle ; Then, the rotation matrix ; in, ; ； ； Step S33, from the rotation matrix The formula for extracting Euler angles is: ； ； in, , .

Images