Horizontal directional drilling rod changing device and control method thereof

Abstract

The invention discloses a horizontal directional drilling rod changing device and a control method thereof.The control method comprises the steps that initial state information and target state information are obtained, the initial state information comprises the numerical value of a joint variable at a set initial position, and the target state information comprises the center position coordinates of a gripper mechanism at a set target position; on the basis of the available value range and the recent value principle of the joint variable, calculating the target state information by utilizing a preset solution model to obtain the numerical value of the joint variable at the set target position; performing interpolation processing on the values of the joint variables by using quintic polynomial interpolation to obtain a position interpolation curve, a speed interpolation curve and an acceleration interpolation curve of the joint variables; and controlling the joint action according to the interpolation curve of the joint variable so as to enable the gripper mechanism to move to a set target position. According to the device, the drill rod can be automatically loaded and unloaded, the labor cost is reduced, the operation efficiency of loading and unloading the drill rod is improved, and impact and shaking in the rod replacing process can be reduced.

Description

Technical Field

[0001] The invention belongs to the technical field of horizontal directional drilling machines, and in particular relates to a horizontal directional drilling rod changing device and a control method thereof. Background Art

[0002] Horizontal directional drilling technology is a new construction technology that combines the directional drilling technology of the oil industry with traditional pipeline construction. With its fast construction speed, high precision and low cost, it is widely used in pipeline laying projects such as water supply, gas, electricity, telecommunications, natural gas, and oil. The construction process of the horizontal directional drilling rig is to make the drill bit pass through the formation by rotating and pushing and pulling the power head. The continuous columnar space formed after the drill bit passes through is the drill string. The lengthening of the drill string in space is achieved by adding drill rods in sections, and the increase in the diameter of the drill string is achieved step by step by pulling back the reaming drill bit, that is, it is necessary to remove the drill rod at the power head to achieve the reverse movement of the reaming drill bit in the drill string.

[0003] In actual use, the drilling rig needs to frequently load and unload drill rods. At present, the horizontal directional drilling rig mainly uses mechanical grippers or truck cranes to assist in loading and unloading drill rods. Specifically, when loading and unloading drill rods, workers adjust the position of the mechanical grippers or truck cranes according to the position of the drill rods to adjust the mechanical grippers or truck cranes to the drill rod position or drill rod placement position, and then grab or release the drill rods.

[0004] However, the method of using workers to adjust the position of mechanical grippers or truck cranes to load and unload drill pipes requires a large amount of manual labor, high labor costs, low operational efficiency, and poor operational safety. In addition, the manual control method has poor movement stability and continuity, which can easily cause damage to the equipment. Summary of the invention

[0005] In order to solve part or all of the technical problems existing in the above-mentioned prior art, the present invention provides a horizontal directional drilling rod changing device and a control method thereof.

[0006] The technical solution of the present invention is as follows:

[0007] In a first aspect, a rod changing device for horizontal directional drilling is provided, the rod changing device comprising:

[0008] Base;

[0009] A large arm, the large arm being rotatably mounted on the base;

[0010] A first arm section, one end of which is hinged to the upper end of the boom, and the first arm section can rotate around a hinge point between the first arm section and the boom;

[0011] A second section arm, one end of the second section arm is hinged to the other end of the first section arm, the second section arm can rotate around the hinge point between the second section arm and the first section arm, and the length of the second section arm can be telescopically adjusted;

[0012] A third arm, one end of which is hinged to the other end of the second arm, and the third arm can rotate around a hinge point between the third arm and the second arm;

[0013] A gripper support, the gripper support being rotatably mounted on the lower end of the third arm;

[0014] A gripper mechanism, the gripper mechanism being mounted at the lower end of the gripper support, the gripper mechanism comprising at least two grippers, the grippers being able to move closer to or farther from each other;

[0015] A driving mechanism, wherein the driving mechanism is capable of driving the boom to rotate, the first section arm to rotate, the second section arm to rotate, the second section arm to extend and retract, the third section arm to rotate, the gripper support to rotate, and the gripper to move;

[0016] a control unit connected to the driving mechanism, for calculating and adjusting the swing angle of the boom, the amplitude change angle of the first boom, the amplitude change angle of the second boom, the telescopic length of the second boom, the amplitude change angle of the third boom and the swing angle of the gripper support according to status information of the boom, the first boom, the second boom, the third boom, the gripper support and the gripper mechanism at a given starting position, and status information of the gripper mechanism at a given target position;

[0017] The amplitude variation angle represents the angle between the corresponding joint arm and a horizontal plane parallel to the bottom surface of the base.

[0018] In some optional embodiments, the driving mechanism includes: a first rotating assembly, a second rotating assembly, a first rotating assembly, a second rotating assembly, a third rotating assembly, a first telescopic assembly, and a first moving assembly;

[0019] The first rotating assembly can drive the upper arm to rotate in a direction perpendicular to the bottom surface of the base, the second rotating assembly can drive the gripper support to rotate in a direction perpendicular to the end surface of the third arm, the first rotating assembly can drive the first arm to rotate around its hinge point with the upper arm, the second rotating assembly can drive the second arm to rotate around its hinge point with the first arm, the third rotating assembly can drive the third arm to rotate around its hinge point with the second arm, the first telescopic assembly can drive the second arm to telescopic movement, and the first moving assembly can drive the gripper to move.

[0020] In a second aspect, a control method is also provided, which is applied to the horizontal directional drilling rod changing device, and the control method comprises:

[0021] Acquire initial state information and target state information, wherein the initial state information includes values ​​of joint variables at a set initial position, wherein the joint variables include: the rotation angle of the upper arm, the amplitude change angle of the first arm section, the amplitude change angle of the second arm section, the telescopic length of the second arm section, the amplitude change angle of the third arm section, and the rotation angle of the gripper support, and the target state information includes the center point position coordinates of the gripper mechanism at a set target position, and the center position coordinates of the two grippers in the gripper mechanism;

[0022] Based on the range of possible values ​​of the joint variables and the principle of the nearest value, the target state information is calculated and processed using a preset solution model to obtain the value of the joint variable under the set target position, and the solution model includes: a first rotation angle solution model, a second rotation angle solution model, a first amplitude angle solution model, a second amplitude angle solution model, a third amplitude angle solution model and a telescopic length solution model;

[0023] Using quintic polynomial interpolation to interpolate the values ​​of the joint variables at the set starting position and the values ​​of the number of joints at the set target position, the position interpolation curve, the velocity interpolation curve and the acceleration interpolation curve of the joint variables are obtained;

[0024] According to the position interpolation curve, velocity interpolation curve and acceleration interpolation curve of the joint variables, the upper arm rotation, the first arm rotation, the second arm rotation, the second arm extension and retraction, the third arm rotation and the gripper support rotation are controlled to move the gripper mechanism to the set target position.

[0025] In some optional implementations, the solution model is obtained by:

[0026] According to the coordinate transformation principle, a first transformation relationship model between the center point position coordinates of the gripper mechanism and the joint variables is constructed, and a second transformation relationship model between the vector between the center positions of the two grippers and the joint variables is constructed;

[0027] The first transformation relationship model and the second transformation relationship model are processed using an inverse solution algorithm to obtain the solution model.

[0028] In some optional implementations, the first transformation relationship model expression is:

[0029] R(x)=cos(sw1)*((A+Ext)*cos(am1+am2)-

[0030] B*sin(am1+am2+am3-E)+C*cos(am1)-D);

[0031] R(y)=sin(sw1)*((A+Ext)*cos(am1+am2)-

[0032] B*sin(am1+am2+am3-E)+C*cos(am1)-D);

[0033] R(z)=F-(A+Ext)*sin(am1+am2)-

[0034] B*cos(am1+am2+am3-E)-C*sin(am1);

[0035] The second transformation relationship model expression is:

[0036] v(1)=-sin(sw1)*cos(sw2)-cos(sw1)*sin(sw2)*cos(am1+am2+am3);

[0037] v(2)=cos(sw1)*cos(sw2)-sin(sw1)*sin(sw2)*cos(am1+am2+am3);

[0038] v(3)=sin(sw2)*sin(am1+am2+am3);

[0039] Wherein, R(x), R(y) and R(z) represent the coordinates of the center point of the gripper mechanism on the X-axis, Y-axis and Z-axis of the gripper support coordinate system, respectively; v(1), v(2) and v(3) represent the components of the vector between the center positions of the two grippers of the gripper mechanism on the X-axis, Y-axis and Z-axis of the gripper support coordinate system, respectively; sw1 represents the rotation angle of the boom; am1 represents the amplitude variation angle of the first arm section; am2 represents the amplitude variation angle of the second arm section; Ext represents the telescopic length of the second arm section; am3 represents the amplitude variation angle of the third arm section; sw2 represents the rotation angle of the gripper support; A, B, C, D, E and F represent given structural constants.

[0040] In some optional implementations, the first rotation angle solution model expression is:

[0041] sw1=atan2(R(y),R(x));

[0042] Among them, atan2 represents the four-quadrant inverse sine function calculation.

[0043] In some optional implementations, the second rotation angle solution model expression is:

[0044]

[0045] Among them, atan2 represents the four-quadrant inverse sine function calculation.

[0046] In some optional implementations, the first amplitude angle solution model expression is:

[0047]

[0048] am1=am10, sin(sw1)≠0and sin(sw2)=0;

[0049] am1=am10, sin(sw1)=0and sin(sw2)=0;

[0050] Among them, atan2 represents the four-quadrant inverse sine function calculation;

[0051] R zc =μ*sinam1+h*cosam1;

[0052] μ=(Ext+A)*cos am2+CB*sin(am2+am3-E);

[0053] h=(Ext+A)*sinam2-B*cos(am2+am3-E);

[0054] am10 represents a predetermined first amplitude angle value;

[0055] The second amplitude angle solution model expression is:

[0056] am2=am20;

[0057] Wherein, am20 represents a predetermined second amplitude variation angle value;

[0058] The third amplitude angle solution model expression is:

[0059]

[0060]

[0061] sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)=0;

[0062]

[0063] sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)≠0;

[0064]

[0065] sin(sw1)=0and sin(sw2)=0and cos(am1+am2)=0;

[0066]

[0067] sin(sw1)=0and sin(sw2)=0and cos(am1+am2)≠0;

[0068] in,

[0069]

[0070] The telescopic length solution model expression is:

[0071]

[0072] Ext=C 4z -A,sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)=0;

[0073] Ext=C 4y -A,sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)≠0;

[0074] Ext=C 5z ,sin(sw1)=0and sin(sw2)=0and cos(am1+am2)=0;

[0075] Ext=CC 6x ,sin(sw1)=0and sin(sw2)=0and cos(am1+am2)≠0;

[0076] in,

[0077] c=R y_C 2 +R z_C 2 -t 2 ;

[0078]

[0079] c2=R x_C2 +R z_C 2 -t 2 ;

[0080] t = C;

[0081]

[0082]

[0083] R z_C =FR(z)-B*cos(am1+am2+am3-E);

[0084]

[0085] In some optional embodiments, based on the position interpolation curve, velocity interpolation curve and acceleration interpolation curve of the joint variables, a dual closed-loop adjustment control method of speed closed-loop adjustment and position closed-loop adjustment is adopted to control the rotation of the upper arm, the rotation of the first arm section, the rotation of the second arm section, the extension and retraction of the second arm section, the rotation of the third arm section, and the rotation of the gripper support, so that the gripper mechanism moves to the set target position.

[0086] The main advantages of the technical solution of the present invention are as follows:

[0087] The rod changing device and control method thereof of the present invention can realize automatic loading and unloading of drill rods, reduce manual workload, reduce labor costs, improve the operational efficiency of loading and unloading drill rods, and improve operational safety; and can make the movement of the rod changing device smooth, ensure the continuity of the rod changing movement, reduce the impact and shaking of the rod changing device during the rod changing process, and reduce the use loss of the rod changing device. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] The drawings described herein are used to provide a further understanding of the embodiments of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0089] Figure 1 A schematic diagram of the structure of a rod changing device for horizontal directional drilling provided by an embodiment of the present invention;

[0090] Figure 2 A schematic diagram of establishing a coordinate system of a rod changing device for horizontal directional drilling provided by an embodiment of the present invention;

[0091] Figure 3 A flow chart of a control method for a horizontal directional drilling rod changing device provided by an embodiment of the present invention;

[0092] Figure 4A schematic diagram of a dual closed-loop regulation control provided by an embodiment of the present invention.

[0093] Description of reference numerals:

[0094] 1-base, 2-big arm, 3-first arm section, 4-second arm section, 5-third arm section, 6-grip support, 7-grip, 8-first telescopic cylinder, 9-second telescopic cylinder, 10-third telescopic cylinder, 11-fourth telescopic cylinder, 12-fifth telescopic cylinder. DETAILED DESCRIPTION

[0095] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the specific embodiments of the present invention and the corresponding drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0096] The technical solution provided by the embodiments of the present invention is described in detail below with reference to the accompanying drawings.

[0097] Embodiment 1

[0098] refer to Figure 1 , Figure 1 A schematic diagram of the structure of a rod-changing device for horizontal directional drilling provided by an embodiment of the present invention. An embodiment of the present invention provides a rod-changing device for horizontal directional drilling, the rod-changing device comprising:

[0099] Base 1; boom 2, boom 2 is rotatably mounted on base 1; first section arm 3, one end of the first section arm 3 is hinged to the upper end of boom 2, and the first section arm 3 can rotate around the hinge point between it and boom 2; second section arm 4, one end of the second section arm 4 is hinged to the other end of the first section arm 3, the second section arm 4 can rotate around the hinge point between it and the first section arm 3, and the length of the second section arm 4 can be telescopically adjusted; third section arm 5, one end of the third section arm 5 is hinged to the other end of the second section arm 4, and the third section arm 5 can rotate around the hinge point between it and the second section arm 4; gripper support 6, gripper support 6 is rotatably mounted at the lower end of the third section arm 5; gripper mechanism, the gripper mechanism is mounted at the lower end of the gripper support 6, the gripper mechanism includes at least two grippers 7, gripper 7 can approach or move away from each other; a driving mechanism, the driving mechanism can drive the boom 2 to rotate, the first boom 3 to rotate, the second boom 4 to rotate, the second boom 4 to extend and retract, the third boom 5 to rotate, the gripper support 6 to rotate, and the gripper 7 to move; a control unit, the control unit is connected to the driving mechanism, and is used to calculate and adjust the rotation angle of the boom 2, the amplitude change angle of the first boom 3, the amplitude change angle of the second boom 4, the telescopic length of the second boom 4, the amplitude change angle of the third boom 5 and the rotation angle of the gripper support 6 according to the state information of the boom 2, the first boom 3, the second boom 4, the third boom 5, the gripper support 6 and the gripper mechanism at a given starting position, and the state information of the gripper mechanism at a given target position;

[0100] The amplitude variation angle represents the angle between the corresponding boom and a horizontal plane parallel to the bottom surface of the base 1 .

[0101] In an embodiment of the present invention, when the above-mentioned rod changing device is used to put up a drill rod, the control unit adjusts the rotation angle of the boom 2, the amplitude change angle of the first boom 3, the amplitude change angle of the second boom 4, the telescopic length of the second boom 4, the amplitude change angle of the third boom 5 and the rotation angle of the gripper support 6 according to the status information of the boom 2, the first boom 3, the second boom 4, the third boom 5, the gripper support 6 and the gripper mechanism at the current starting position, and the status information of the gripper mechanism at the rod storage point position according to the pre-solved running path to move the gripper mechanism to the rod storage point position, and then controls the grippers 7 to approach each other to grab the drill rod; after completion After the drill rod is grabbed, the control unit adjusts the rotation angle of the boom 2, the amplitude change angle of the first boom 3, the amplitude change angle of the second boom 4, the telescopic length of the second boom 4, the amplitude change angle of the third boom 5 and the rotation angle of the gripper support 6 according to the status information of the boom 2, the first boom 3, the second boom 4, the third boom 5, the gripper support 6 and the gripper mechanism at the current starting position (i.e., the rod storage point position), and the status information of the gripper mechanism at the drilling rig point position of the horizontal directional drilling rig according to the pre-solved operation path to move the gripper mechanism to the drilling rig point position of the horizontal directional drilling rig, and then controls the grippers 7 to move away from each other to raise the rod.

[0102] In the embodiment of the present invention, when the above-mentioned rod changing device is used to unload the drill rod, the control unit adjusts the rotation angle of the boom 2, the amplitude change angle of the first boom 3, the amplitude change angle of the second boom 4, the telescopic length of the second boom 4, the amplitude change angle of the third boom 5 and the rotation angle of the gripper support 6 according to the state information of the boom 2, the first boom 3, the second boom 4, the third boom 5, the gripper support 6 and the gripper mechanism at the current starting position, and the state information of the gripper mechanism at the drilling point position of the horizontal directional drilling rig, according to the pre-solved operation path to move the gripper mechanism to the drilling point position of the horizontal directional drilling rig, and then controls the gripper 7 to move the gripper mechanism to the drilling point position of the horizontal directional drilling rig. They approach each other to grab the drill rod; after completing the grabbing of the drill rod, the control unit adjusts the rotation angle of the boom 2, the amplitude change angle of the first boom 3, the amplitude change angle of the second boom 4, the telescopic length of the second boom 4, the amplitude change angle of the third boom 5 and the rotation angle of the gripper support 6 according to the status information of the boom 2, the first boom 3, the second boom 4, the third boom 5, the gripper support 6 and the gripper mechanism at the current starting position (i.e., the drilling rig point position), and the status information of the gripper mechanism at the specified target position, according to the pre-solved running path to move the gripper mechanism to the specified target position, and then controls the grippers 7 to move away from each other to unload the rod.

[0103] In the embodiment of the present invention, the second joint arm 4 includes a first sub-joint arm and a second sub-joint arm which are sleeved and connected. The second sub-joint arm can move along the axial direction of the first sub-joint arm to realize the telescopic function of the second joint arm 4 .

[0104] It should be noted that when the second joint arm 4 includes a first sub-joint arm and a second sub-joint arm which are sleeved and connected, the first sub-joint arm is hinged to the first joint arm 3 , and the second sub-joint arm is hinged to the third joint arm 5 .

[0105] In the embodiment of the present invention, the control unit may be a PLC.

[0106] The horizontal directional drilling rod changing device provided by the embodiment of the present invention can realize automatic loading and unloading of drill rods, reduce manual workload, reduce labor costs, improve the operational efficiency of loading and unloading drill rods, and improve operational safety.

[0107] Further, in an optional implementation of the embodiment of the present invention, the driving mechanism includes: a first rotating assembly, a second rotating assembly, a first rotating assembly, a second rotating assembly, a third rotating assembly, a first telescopic assembly, and a first moving assembly;

[0108] The first rotating assembly can drive the upper arm 2 to rotate in a direction perpendicular to the bottom surface of the base 1, the second rotating assembly can drive the gripper support 6 to rotate in a direction perpendicular to the end surface of the third section arm 5, the first rotating assembly can drive the first section arm 3 to rotate around its hinge point with the upper arm 2, the second rotating assembly can drive the second section arm 4 to rotate around its hinge point with the first section arm 3, the third rotating assembly can drive the third section arm 5 to rotate around its hinge point with the second section arm 4, the first telescopic assembly can drive the second section arm 4 to telescopic movement, and the first movable assembly can drive the gripper 7 to move.

[0109] By setting different drive components to perform drive control on different parts, the independence of the actions of different parts can be ensured and the position adjustment accuracy of different parts can be improved.

[0110] Furthermore, in an optional implementation of an embodiment of the present invention, the first rotating component includes: a first rotating motor, a first rotating gear and a first rotating ring gear, the first rotating motor is installed on the base 1, the output end of the first rotating motor is connected to the first rotating gear, the first rotating gear is transmission-connected to the first rotating ring gear, the first rotating ring gear is rotatably installed on the base 1, and the first rotating ring gear is fixedly connected to the upper arm 2.

[0111] The second rotating assembly includes: a second rotating motor, a second rotating gear and a second rotating ring gear. The second rotating motor is installed on the third arm 5. The output end of the second rotating motor is connected to the second rotating gear. The second rotating gear is transmission-connected to the second rotating ring gear. The second rotating ring gear is rotatably installed on the third arm 5, and the second rotating ring gear is fixedly connected to the gripper support 6.

[0112] The first rotating assembly includes: a first telescopic cylinder 8 , one end of the first telescopic cylinder 8 is hinged to the big arm 2 , and the other end of the first telescopic cylinder 8 is hinged to the first section arm 3 .

[0113] The second rotating assembly includes: a second telescopic cylinder 9 , one end of the second telescopic cylinder 9 is hinged on the first boom 3 , and the other end of the second telescopic cylinder 9 is hinged on the second boom 4 .

[0114] It should be noted that, when the second boom 4 includes a first sub-boom and a second sub-boom which are sleeved and connected, the other end of the second telescopic cylinder 9 is hinged to the first sub-boom of the second boom 4 .

[0115] The third rotating assembly includes: a third telescopic cylinder 10 , one end of the third telescopic cylinder 10 is hinged to the second boom 4 , and the other end of the third telescopic cylinder 10 is hinged to the third boom 5 .

[0116] It should be noted that, when the second boom 4 includes a first sub-boom and a second sub-boom which are sleeved and connected, one end of the third telescopic cylinder 10 is hinged to the second sub-boom of the second boom 4 .

[0117] The first telescopic assembly includes a fourth telescopic cylinder 11 .

[0118] It should be noted that, when the second boom 4 includes a first sub-boom and a second sub-boom which are sleeved and connected, both ends of the fourth telescopic cylinder 11 are hinged to the first sub-boom and the second sub-boom respectively.

[0119] The first moving component includes: a fifth telescopic cylinder 12, which is hinged on the gripper support 6, and both ends of the fifth telescopic cylinder 12 are respectively connected to one end of the two grippers 7 of the gripper mechanism, and the fifth telescopic cylinder 12 drives the other ends of the two grippers 7 to move closer to or away from each other through telescopic movement.

[0120] The driving mechanism provided above can facilitate the motion control of the rod changing device.

[0121] Embodiment 2

[0122] refer to Figure 3 , Figure 3 A flow chart of a control method for a horizontal directional drilling rod changing device provided by an embodiment of the present invention. An embodiment of the present invention provides a control method, which is applied to the above-mentioned horizontal directional drilling rod changing device, and the control method includes the following steps 1 to 4:

[0123] Step 1: Obtain the initial state information and target state information;

[0124] In the embodiment of the present invention, the initial state information includes the values ​​of the joint variables at the set initial position, and the joint variables include: the rotation angle of the upper arm, the amplitude change angle of the first arm section, the amplitude change angle of the second arm section, the telescopic length of the second arm section, the amplitude change angle of the third arm section, and the rotation angle of the gripper support;

[0125] It should be noted that the starting position is determined according to actual conditions, and is usually the position of the rod changing device before control is performed.

[0126] In an embodiment of the present invention, the target state information includes the center point position coordinates of the gripper mechanism at the set target position, and the center position coordinates of the two grippers in the gripper mechanism;

[0127] It should be noted that the position coordinates are coordinates in a specified coordinate system.

[0128] It should be noted that the target position is determined according to the actual situation, for example, according to the actual upper rod position or lower rod position.

[0129] Step 2, based on the range of possible values ​​of the joint variables and the principle of nearest value, the target state information is calculated and processed using a preset solution model to obtain the value of the joint variable at the set target position;

[0130] In an embodiment of the present invention, the solution model includes: a first rotation angle solution model, a second rotation angle solution model, a first amplitude variation angle solution model, a second amplitude variation angle solution model, a third amplitude variation angle solution model and a telescopic length solution model.

[0131] In the embodiment of the present invention, the possible value range of each joint variable is determined according to actual conditions.

[0132] In an embodiment of the present invention, the nearest value principle indicates that: when the calculated value of a joint variable is outside the acceptable range of the joint variable, the value of the joint variable at the set target position is taken as a value within the acceptable range of the joint variable that is closest to the calculated value; when the calculated value of a joint variable includes two or more values ​​within the acceptable range of the joint variable, the value of the joint variable at the set target position is taken as a calculated value that is closest to the value of the joint variable at the set starting position and within the acceptable range of the joint variable.

[0133] Step 3, using quintic polynomial interpolation to interpolate the values ​​of the joint variables at the set starting position and the values ​​of the number of joints at the set target position, to obtain a position interpolation curve, a velocity interpolation curve, and an acceleration interpolation curve of the joint variables;

[0134] It should be noted that when performing the fifth-order polynomial interpolation processing, the speed and acceleration of the upper arm at the starting position and the target position are set to 0, the speed and acceleration of the first section arm at the starting position and the target position are set to 0, the speed and acceleration of the second section arm at the starting position and the target position are set to 0, the speed and acceleration of the third section arm at the starting position and the target position are set to 0, and the speed and acceleration of the gripper support at the starting position and the target position are set to 0.

[0135] Step 4, according to the position interpolation curve, velocity interpolation curve and acceleration interpolation curve of the joint variables, the upper arm rotation, the first arm rotation, the second arm rotation, the second arm extension, the third arm rotation and the gripper support rotation are controlled to move the gripper mechanism to the set target position.

[0136] By implementing the control method provided by the embodiment of the present invention in the horizontal directional drilling rod changing device described in the above embodiment 1, it is possible to realize automatic loading and unloading of drill rods, reduce manual workload, reduce labor costs, improve the operational efficiency of loading and unloading drill rods, and improve operational safety; and it is possible to make the movement of the rod changing device smooth, ensure the continuity of the rod changing action, reduce the impact and jitter of the rod changing device during the rod changing process, and reduce the use loss of the rod changing device.

[0137] Furthermore, in an optional implementation of the embodiment of the present invention, the solution model is obtained by:

[0138] According to the coordinate transformation principle, a first transformation relationship model between the center point position coordinates of the gripper mechanism and the joint variables is constructed, and a second transformation relationship model between the vector between the center positions of the two grippers and the joint variables is constructed;

[0139] The first transformation relationship model and the second transformation relationship model are processed using an inverse solution algorithm to obtain a solution model.

[0140] refer to Figure 2 , Figure 2 A schematic diagram of the establishment of a coordinate system of a horizontal directional drilling rod changing device provided in an embodiment of the present invention. In the embodiment of the present invention, for the structural type of the above-mentioned rod changing device, the connecting rod coordinate systems O1X1Y1Z1, O2X2Y2Z2, O3X3Y3Z3, O4X4Y4Z4, O5X5Y5Z5, and O6X6Y6Z6 are established according to the standard DH method (SDH). Among them, the coordinate system O1X1Y1Z1 is the base coordinate system, O2X2Y2Z2 is the boom end coordinate system, O3X3Y3Z3 is the first boom end coordinate system, O4X4Y4Z4 is the second boom telescopic end coordinate system, O5X5Y5Z5 is the second boom end coordinate system, and O6X6Y6Z6 is the gripper support coordinate system.

[0141] In the embodiment of the present invention, the first transformation relationship model expression is:

[0142] R(x)=cos(sw1)*((A+Ext)*cos(am1+am2)-

[0143] B*sin(am1+am2+am3-E)+C*cos(am1)-D);

[0144] R(y)=sin(sw1)*((A+Ext)*cos(am1+am2)-

[0145] B*sin(am1+am2+am3-E)+C*cos(am1)-D);

[0146] R(z)=F-(A+Ext)*sin(am1+am2)-

[0147] B*cos(am1+am2+am3-E)-C*sin(am1);

[0148] The second transformation relationship model expression is:

[0149] v(1)=-sin(sw1)*cos(sw2)-cos(sw1)*sin(sw2)*cos(am1+am2+am3);

[0150] v(2)=cos(sw1)*cos(sw2)-sin(sw1)*sin(sw2)*cos(am1+am2+am3);

[0151] v(3)=sin(sw2)*sin(am1+am2+am3);

[0152] Wherein, R(x), R(y) and R(z) represent the coordinates of the center point of the gripper mechanism on the X-axis, Y-axis and Z-axis of the gripper support coordinate system, respectively; v(1), v(2) and v(3) represent the components of the vector between the center positions of the two grippers of the gripper mechanism on the X-axis, Y-axis and Z-axis of the gripper support coordinate system, respectively; sw1 represents the rotation angle of the boom; am1 represents the amplitude variation angle of the first arm section; am2 represents the amplitude variation angle of the second arm section; Ext represents the telescopic length of the second arm section; am3 represents the amplitude variation angle of the third arm section; sw2 represents the rotation angle of the gripper support; A, B, C, D, E and F represent given structural constants.

[0153] In the embodiment of the present invention, the structural constants A, B, C, D, E and F are obtained in advance according to the standard DH method (SDH) based on the specific structural type and specific dimensional parameter information of the rod changing device.

[0154] In the embodiment of the present invention, the first rotation angle solution model expression is:

[0155] sw1=atan2(R(y),R(x));

[0156] The second rotation angle solution model expression is:

[0157]

[0158] Among them, atan2 represents the four-quadrant inverse sine function calculation.

[0159] It should be noted that the calculation of the four-quadrant inverse sine function is specifically expressed as:

[0160]

[0161] Among them, arctan represents the inverse tangent trigonometric function calculation.

[0162] The first amplitude angle solution model expression is:

[0163]

[0164] am1=am10, sin(sw1)≠0and sin(sw2)=0;

[0165] am1=am10, sin(sw1)=0and sin(sw2)=0;

[0166] Among them, atan2 represents the four-quadrant inverse sine function calculation;

[0167]

[0168] μ=(Ext+A)*cos am2+CB*sin(am2+am3-E);

[0169] h=(Ext+A)*sinam2-B*cos(am2+am3-E);

[0170] am10 represents a predetermined first amplitude angle value.

[0171] The second amplitude angle solution model expression is:

[0172] am2=am20;

[0173] Here, am20 represents a predetermined second amplitude variation angle value.

[0174] The third amplitude angle solution model expression is:

[0175]

[0176]

[0177] sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)=0;

[0178]

[0179] sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)≠0;

[0180]

[0181] sin(sw1)=0and sin(sw2)=0and cos(am1+am2)=0;

[0182]

[0183] sin(sw1)=0and sin(sw2)=0and cos(am1+am2)≠0;

[0184] in,

[0185]

[0186] The expression of the telescopic length solution model is:

[0187]

[0188] Ext=C 4z -A,sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)=0;

[0189] Ext=C 4y -A,sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)≠0;

[0190] Ext=C 5z ,sin(sw1)=0and sin(sw2)=0and cos(am1+am2)=0;

[0191] Ext=CC 6x ,sin(sw1)=0and sin(sw2)=0and cos(am1+am2)≠0;

[0192] in,

[0193] c=R y_c 2 +R z_C 2 -t 2 ;

[0194]

[0195] c2=R x_c 2 +R z_c 2 -t 2 ;

[0196] t = C;

[0197]

[0198]

[0199] Further, in the embodiment of the present invention, the process of using the inverse solution algorithm to process the first transformation relationship model and the second transformation relationship model to obtain the above-mentioned solution model is described as follows:

[0200] According to the first formula and the second formula in the first transformation relationship model, dividing the first formula by the second formula can yield: R(y) / R(x)=sin(sw1) / cos(sw1).

[0201] By converting and calculating according to the formula R(y) / R(x)=sin(sw1) / cos(sw1), we can obtain the first rotation angle solution model expression: sw1=atan2(R(y),R(x)).

[0202] By multiplying both sides of the first formula in the second transformation relationship model by sin(sw1), and by multiplying both sides of the second formula in the second transformation relationship model by cos(sw1), we can obtain:

[0203] v(1)*sin(sw1)=-sin(sw1) 2 *cos(sw2)-

[0204] sin(sw1)*cos(sw1)*sin(sw2)*cos(am1+am2+am3-E);

[0205] v(2)*cos(sw1)=cos(sw1) 2 *cos(sw2)-

[0206] cos(sw1)*sin(sw1)*sin(sw2)*cos(am1+am2+am3-E).

[0207] By simplifying the two formulas obtained above, we can get:

[0208] v(2)*cos(sw1)-v(1)*sin(sw1)=cos(sw2).

[0209] According to the formula v(2)*cos(sw1)-v(1)*sin(sw1)=cos(sw2), sw2 has two solutions. Since cos(sw2) and sin(sw2) are not 0 at the same time, the second rotation angle solution model expression can be obtained:

[0210]

[0211] It should be noted that The possible values ​​are: or

[0212] According to the second and third formulas in the second transformation relationship model, we can get:

[0213]

[0214] In the above two formulas, since sin(sw1) and sin(sw2) are used as denominators, it is necessary to determine whether sin(sw1) and sin(sw2) are equal to 0.

[0215] ①sin(sw1)≠0and sin(sw2)≠0

[0216] Specifically, if sin(sw1)≠0and sin(sw2)≠0and(v(3) and cos(sw1)*cos(sw2)-v(2) are not 0 at the same time), then according to the two formulas obtained above, am1+am2+am3 can be expressed as:

[0217]

[0218] If sin(sw1)≠0and sin(sw2)≠0and(v(3) and cos(sw1)*cos(sw2)-v(2) are both 0), then the above first transformation relationship model can be transformed into the following form:

[0219] R(x)=cos(sw1)*((A+Ext)*cos(am1+am2)-

[0220] B*sin(am1+am2+am3-E)+C*cos(am1)-D);

[0221] R(y)=sin(sw1)*((A+Ext)*cos(am1+am2)-

[0222] B*sin(am1+am2+am3-E)+C*cos(am1)-D);

[0223] R(z)=F-(A+Ext)*sin(am1+am2)-

[0224] B*cos(am1+am2+am3-E)-C*sin(am1);

[0225] The above second transformation relationship model can be transformed into the following form:

[0226] v(1)=-sin(sw1)*cos(sw2)-cos(sw1)*sin(sw2)*cos(am1+am2+am3);

[0227] 0=sin(sw1)*sin(sw2)*cos(am1+am2+am3);

[0228] 0=sin(sw2)*sin(am1+am2+am3).

[0229] Since sin(sw1)≠0and sin(sw2)≠0, according to the second and third formulas in the transformed second transformation relationship model, we can obtain cos(am1+am2+am3)=0 and sin(am1+am2+am3)=0. Since cos(am1+am2+am3)=0 and sin(am1+am2+am3)=0 cannot be satisfied at the same time, sin(sw1)≠0and sin(sw2)≠0and the situation where (v(3) and cos(sw1)*cos(sw2)-v(2) are 0 at the same time) does not exist.

[0230] Furthermore, the above first transformation relationship model is transformed into the following form:

[0231] R(x)=cos(sw1)*((A+Ext)*cos(am1+am2)-

[0232] B*sin(am1+am2+am3-E)+C*cos(am1)-D);

[0233]

[0234] (A+Ext)*sin(am1+am2)+C*sin(am1)=FR(z)-B*cos(am1+am2+am3-E);

[0235] The above second transformation relationship model is transformed into the following form:

[0236] v(1)=-sin(sw1)*cos(sw2)-cos(sw1)*sin(sw2)*cos(am1+am2+am3);

[0237] v(2)=cos(sw1)*cos(sw2)-sin(sw1)*sin(sw2)*cos(am1+am2+am3);

[0238] v(3)=sin(sw2)*sin(am1+am2+am3).

[0239] Fix am2, that is, give an am2 value to make am2 a known quantity, then the second amplitude angle solution model expression under the current conditions is: am2=am20; am20 represents the predetermined second amplitude angle value.

[0240] Define the following parameters:

[0241]

[0242] Based on the above defined parameters, the second and third formulas in the transformed first transformation relationship model are replaced by parameters to obtain:

[0243]

[0244] By squaring and summing the two equations, we can get:

[0245] d 2 +t 2 +2dt*(cosαcosβ+sinαsinβ)=R y_C 2 +R z_C 2 .

[0246] Further simplifying the obtained formula, we can get:

[0247] d 2 +2*t*cosφ*d=R y_C 2 +R z_C 2 -t 2 .

[0248] Define the following parameters:

[0249]

[0250] Based on the above defined parameters, formula d 2 +2*t*cosφ*d=R y_C 2 +R z_c 2 -t 2 It can be expressed as a quadratic equation:

[0251] d 2 +b1*d-c1=0;

[0252] By solving the above quadratic equation, we can get d and Ext:

[0253]

[0254] Therefore, the expression of the expansion length solution model under the current conditions is:

[0255]

[0256] Furthermore, by simplifying the third formula in the transformed first transformation relationship model, we can obtain:

[0257] FR(z)=((Ext+A)*cosam2+CB*sin(am2+am3-E))*

[0258] sinam1+((Ext+A)*sinam2-B*cos(am2+am3-E))*cosam1.

[0259] Define the following parameters:

[0260]

[0261] Based on the above defined parameters, the simplified formula can be expressed as:

[0262]

[0263] According to the formula By solving, we can get the first amplitude angle solution model expression under the current conditions:

[0264]

[0265] It should be noted that Indicates that the possible values ​​are or

[0266]

[0267] According to the formula By solving, we can get the third amplitude angle solution model expression under the current conditions:

[0268]

[0269] ②sin(sw1)=0and sin(sw2)≠0

[0270] If sin(sw1)=0 and sin(sw2)≠0, then cos(sw1)≠0, and the above first transformation relationship model can be transformed into the following form:

[0271] R(x) / cos(sw1)=(A+Ext)*cos(am1+am2)-

[0272] B*sin(am1+am2+am3-E)+C*cos(am1)-D;

[0273] R(y)=0;

[0274] R(z)=F-(A+Ext)*sin(am1+am2)-

[0275] B*cos(am1+am2+am3-E)-C*sin(am1);

[0276] The above second transformation relationship model can be transformed into the following form:

[0277] v(1)=-cos(sw1)*sin(sw2)*cos(am1+am2+am3);

[0278] v(2)=cos(sw1)*cos(sw2);

[0279] v(3)=sin(sw2)*sin(am1+am2+am3).

[0280] According to the first and third formulas in the transformed second transformation relationship model, we can get:

[0281]

[0282] According to the two formulas, since sin(sw2)≠0and cos(sw1)≠0, if v(1) and v(3) are both 0, then cos(am1+am2+am3)=0and sin(am1+am2+am3)=0, but the above situation does not exist, so v(1) and v(3) are not 0 at the same time. Therefore, we can get:

[0283]

[0284] Furthermore, the above first transformation relationship model is transformed into the following form:

[0285] (A+Ext)*cos(am1+am2)+C*cos(am1)=C 3x ;

[0286]

[0287] R(y)=0;

[0288] (A+Ext)*sin(am1+am2)+C*sin(am1)=C 3z ;

[0289] C 3z =FR(z)-B*cos(am1+am2+am3-E);

[0290] The above second transformation relationship model is transformed into the following form:

[0291] v(1)=-cos(sw1)*sin(sw2)*cos(am1+am2+am3);

[0292] v(2)=cos(sw1)*cos(sw2);

[0293] v(3)=sin(sw2)*sin(am1+am2+am3).

[0294] Fix am2, that is, give an am2 value to make am2 a known quantity, then the second amplitude angle solution model expression under the current conditions is: am2=am20; am20 represents the predetermined second amplitude angle value.

[0295] Define the following parameters:

[0296]

[0297] Based on the above defined parameters, the first and third formulas in the transformed first transformation relationship model are replaced by parameters to obtain:

[0298]

[0299] By squaring and summing the two equations, we can get:

[0300] d 2 +t 2 +2dt*(cosαcosβ+sinαsinβ)=R x_C 2 +R z_c 2 .

[0301] Further simplifying the obtained formula, we can get:

[0302]

[0303] Define the following parameters:

[0304]

[0305] Based on the above defined parameters, the formula It can be expressed as a quadratic equation:

[0306] d 2 +b2*d-c2=0;

[0307] By solving the above quadratic equation, we can get d and Ext:

[0308]

[0309] Therefore, the expression of the expansion length solution model under the current conditions is:

[0310]

[0311] Furthermore, by simplifying the third formula in the transformed first transformation relationship model, we can obtain:

[0312] FR(z)=((Ext+A)*cosam2+CB*sin(am2+am3-E))*sinam1+

[0313] ((Ext+A)*sinam2-B*cos(am2+am3-E))*cosam1.

[0314] Define the following parameters:

[0315]

[0316] Based on the above defined parameters, the simplified formula can be expressed as:

[0317]

[0318] According to the formula By solving, we can get the first amplitude angle solution model expression under the current conditions:

[0319]

[0320] It should be noted that Indicates that the possible values ​​are or

[0321]

[0322] According to the formula By solving, we can get the third amplitude angle solution model expression under the current conditions:

[0323]

[0324] ③sin(sw1)≠0and sin(sw2)=0

[0325] If sin(sw1)≠0and sin(sw2)=0, the above first transformation relationship model can be transformed into the following form:

[0326] R(x)=cos(sw1)*((A+Ext)*cos(am1+am2)-

[0327] B*sin(am1+am2+am3-E)+C*cos(am1)-D);

[0328] R(y) / sin(sw1)=(A+Ext)*cos(am1+am2)-

[0329] B*sin(am1+am2+am3-E)+C*cos(am1)-D;

[0330] R(z)=F-(A+Ext)*sin(am1+am2)-

[0331] B*cos(am1+am2+am3-E)-C*sin(am1);

[0332] The above second transformation relationship model can be transformed into the following form:

[0333] v(1)=-sin(sw1)*cos(sw2);

[0334] v(2)=cos(sw1)*cos(sw2);

[0335] v(3)=0.

[0336] According to the second and third formulas in the transformed first transformation relationship model, we can obtain:

[0337]

[0338]

[0339] Fix am1 and am2, that is, give an am1 value to make am1 a known value, and give an am2 value to make am2 a known value, then the first amplitude angle solution model expression under the current condition is: am1 = am10; am10 represents the first amplitude angle value given in advance. The second amplitude angle solution model expression under the current condition is: am2 = am20; am20 represents the second amplitude angle value given in advance.

[0340] By formula It can be seen that it is necessary to determine whether cos(am1+am2) is equal to 0.

[0341] If cos(am1+am2)=0, then simplifying the second formula in the transformed first transformation relationship model yields:

[0342]

[0343] According to the formula Solving it, we can get:

[0344]

[0345] According to the formula By solving, we can get the third amplitude angle solution model expression under the current conditions:

[0346]

[0347] Since cos(am1+am2)=0, sin(am1+am2)≠0, substitute the solution of am3 into the formula By solving the problem, we can get the expression of the telescopic length solution model under the current conditions:

[0348] Ext=C 4z -A.

[0349] According to the above analysis, when sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)=0, [sw1,am1,am2,Ext,am3,sw2] has multiple solutions, including:

[0350] When sw2=0, [sw1,am1,am2,Ext,am3,sw2] has the following two solutions:

[0351]

[0352] When sw2=π, [sw1,am1,am2,Ext,am3,sw2] has the following two sets of solutions:

[0353]

[0354] When sw2=-π, [sw1,am1,am2,Ext,am3,sw2] has the following two sets of solutions:

[0355]

[0356] If cos(am1+am2)≠0, then according to the second and third formulas in the transformed first transformation relationship model, we can obtain:

[0357]

[0358] FR(z)-C*sin(am1)-B*cos(am1+am2+am3-E).

[0359] Further simplifying the obtained formula, we can get:

[0360]

[0361] Solving according to the simplified formula, we can get:

[0362]

[0363] According to the obtained formula, the solution model expression of the third amplitude angle under the current conditions can be obtained as follows:

[0364]

[0365] Since cos(am1+am2)≠0, substitute the solution of am3 into the formula By solving the problem, we can get the expression of the telescopic length solution model under the current conditions:

[0366] Ext=C 4y -A.

[0367] ④sin(sw1)=0and sin(sw2)=0

[0368] If sin(sw1)=0 and sin(sw2)=0, the above first transformation relationship model can be transformed into the following form:

[0369] R(x) / cos(sw1)=

[0370] (A+Ext)*cos(am1+am2)-B*sin(am1+am2+am3-E)+C*cos(am1)-D;

[0371] R(y)=0;

[0372] R(z)=

[0373] F-(A+Ext)*sin(am1+am2)-B*cos(am1+am2+am3-E)-C*sin(am1);

[0374] The above second transformation relationship model can be transformed into the following form:

[0375] v(1)=0;

[0376] v(2)=cos(sw1)*cos(sw2);

[0377] v(3)=0.

[0378] According to the first formula in the transformed first transformation relationship model, we can get:

[0379]

[0380] Fix am1 and am2, that is, give an am1 value to make am1 a known value, and give an am2 value to make am2 a known value, then the first amplitude angle solution model expression under the current condition is: am1 = am10; am10 represents the first amplitude angle value given in advance. The second amplitude angle solution model expression under the current condition is: am2 = am20; am20 represents the second amplitude angle value given in advance.

[0381] By formula It can be seen that it is necessary to determine whether cos(am1+am2) is equal to 0.

[0382] If cos(am1+am2)=0, the first formula in the transformed first transformation relationship model can be simplified to obtain:

[0383]

[0384] According to the formula Solving it, we can get:

[0385]

[0386] According to the formula By solving, we can get the third amplitude angle solution model expression under the current conditions:

[0387]

[0388] Because cos(am1+am2)=0, sin(am1+am2)≠0, substitute the solution of am3 into the third formula in the transformed first transformation relationship model to solve it, and the solution model expression of the telescopic length under the current conditions can be obtained:

[0389]

[0390] According to the above analysis, when sin(sw1)=0and sin(sw2)=0and cos(am1+am2)=0, [sw1,am1,am2,Ext,am3,sw2] has two sets of solutions, including:

[0391]

[0392] If cos(am1+am2)≠0, then according to the first and third formulas in the transformed first transformation relationship model, we can get:

[0393]

[0394] Solving the obtained formula, we can get:

[0395]

[0396] According to the obtained formula, the solution model expression of the third amplitude angle under the current conditions can be obtained as follows:

[0397]

[0398]

[0399] Substituting the solution result of am3 into the first formula in the transformed first transformation relationship model for solution, the expression of the telescopic length solution model under the current conditions can be obtained as follows:

[0400]

[0401] Furthermore, in an optional implementation of an embodiment of the present invention, in the above step 4, according to the position interpolation curve, velocity interpolation curve and acceleration interpolation curve of the joint variables, a dual closed-loop adjustment control method of speed closed-loop adjustment and position closed-loop adjustment is adopted to control the rotation of the upper arm, the rotation of the first arm section, the rotation of the second arm section, the extension and retraction of the second arm section, the rotation of the third arm section, and the rotation of the gripper support, so that the gripper mechanism moves to the set target position.

[0402] refer to Figure 4 , Figure 4 A schematic diagram of a dual closed-loop regulation control provided by an embodiment of the present invention. In the embodiment of the present invention, the dual closed-loop regulation control is mainly speed closed-loop regulation, and is supplemented by position closed-loop regulation.

[0403] In the embodiment of the present invention, by adopting a double closed-loop adjustment control method, the impact and vibration of the rod changing device during the rod changing process can be further reduced, thereby ensuring the movement stability.

[0404] It should be noted that, in this article, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In addition, "front", "back", "left", "right", "upper" and "lower" in this article are all referenced to the placement state shown in the accompanying drawings.

[0405] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A rod changing device for horizontal directional drilling, characterized in that: The rod changing device comprises: Base; A large arm, the large arm being rotatably mounted on the base; A first arm section, one end of which is hinged to the upper end of the boom, and the first arm section can rotate around a hinge point between the first arm section and the boom; A second section arm, one end of the second section arm is hinged to the other end of the first section arm, the second section arm can rotate around the hinge point between the second section arm and the first section arm, and the length of the second section arm can be telescopically adjusted; A third arm, one end of which is hinged to the other end of the second arm, and the third arm can rotate around a hinge point between the third arm and the second arm; A gripper support, the gripper support being rotatably mounted on the lower end of the third arm; A gripper mechanism, the gripper mechanism being mounted at the lower end of the gripper support, the gripper mechanism comprising at least two grippers, the grippers being able to move closer to or farther from each other; A driving mechanism, wherein the driving mechanism is capable of driving the boom to rotate, the first section arm to rotate, the second section arm to rotate, the second section arm to extend and retract, the third section arm to rotate, the gripper support to rotate, and the gripper to move; a control unit connected to the driving mechanism, for calculating and adjusting the swing angle of the boom, the amplitude change angle of the first boom, the amplitude change angle of the second boom, the telescopic length of the second boom, the amplitude change angle of the third boom and the swing angle of the gripper support according to status information of the boom, the first boom, the second boom, the third boom, the gripper support and the gripper mechanism at a given starting position, and status information of the gripper mechanism at a given target position; The amplitude variation angle represents the angle between the corresponding joint arm and a horizontal plane parallel to the bottom surface of the base.

2. The horizontal directional drilling rod changing device according to claim 1, characterized in that: The driving mechanism comprises: a first rotating assembly, a second rotating assembly, a first rotating assembly, a second rotating assembly, a third rotating assembly, a first telescopic assembly, and a first moving assembly; The first rotating assembly can drive the upper arm to rotate in a direction perpendicular to the bottom surface of the base, the second rotating assembly can drive the gripper support to rotate in a direction perpendicular to the end surface of the third arm, the first rotating assembly can drive the first arm to rotate around its hinge point with the upper arm, the second rotating assembly can drive the second arm to rotate around its hinge point with the first arm, the third rotating assembly can drive the third arm to rotate around its hinge point with the second arm, the first telescopic assembly can drive the second arm to telescopic movement, and the first moving assembly can drive the gripper to move.

3. A control method, characterized in that: Applied to the horizontal directional drilling rod changing device according to claim 1 or 2, the control method comprises: Acquire initial state information and target state information, wherein the initial state information includes values ​​of joint variables at a set initial position, wherein the joint variables include: the rotation angle of the upper arm, the amplitude change angle of the first arm section, the amplitude change angle of the second arm section, the telescopic length of the second arm section, the amplitude change angle of the third arm section, and the rotation angle of the gripper support, and the target state information includes the center point position coordinates of the gripper mechanism at a set target position, and the center position coordinates of the two grippers in the gripper mechanism; Based on the range of possible values ​​of the joint variables and the principle of the nearest value, the target state information is calculated and processed using a preset solution model to obtain the value of the joint variable under the set target position, and the solution model includes: a first rotation angle solution model, a second rotation angle solution model, a first amplitude angle solution model, a second amplitude angle solution model, a third amplitude angle solution model and a telescopic length solution model; Using quintic polynomial interpolation to interpolate the values ​​of the joint variables at the set starting position and the values ​​of the number of joints at the set target position, the position interpolation curve, the velocity interpolation curve and the acceleration interpolation curve of the joint variables are obtained; According to the position interpolation curve, velocity interpolation curve and acceleration interpolation curve of the joint variables, the upper arm rotation, the first arm rotation, the second arm rotation, the second arm extension and retraction, the third arm rotation and the gripper support rotation are controlled to move the gripper mechanism to the set target position.

4. The control method according to claim 3, characterized in that: The solution model is obtained in the following way: According to the coordinate transformation principle, a first transformation relationship model between the center point position coordinates of the gripper mechanism and the joint variables is constructed, and a second transformation relationship model between the vector between the center positions of the two grippers and the joint variables is constructed; The first transformation relationship model and the second transformation relationship model are processed using an inverse solution algorithm to obtain the solution model.

5. The control method according to claim 4, characterized in that: The first transformation relationship model expression is: R(x)=cos(sw1)*((A+Ext)*cos(am1+am2)- B*sin(am1+am2+am3-E)+C*cos(am1)-D); R(y)=sin(sw1)*((A+Ext)*cos(am1+am2)- B*sin(am1+am2+am3-E)+C*cos(am1)-D); R(z)=F-(A+Ext)*sin(am1+am2)- B*cos(am1+am2+am3-E)-C*sin(am1); The second transformation relationship model expression is: v(1)=-sin(sw1)*cos(sw2)-cos(sw1)*sin(sw2)*cos(am1+am2+am3); v(2)=cos(sw1)*cos(sw2)-sin(sw1)*sin(sw2)*cos(am1+am2+am3); v(3)=sin(sw2)*sin(am1+am2+am3); Wherein, R(x), R(y) and R(z) represent the coordinates of the center point of the gripper mechanism on the X-axis, Y-axis and Z-axis of the gripper support coordinate system, respectively; v(1), v(2) and v(3) represent the components of the vector between the center positions of the two grippers of the gripper mechanism on the X-axis, Y-axis and Z-axis of the gripper support coordinate system, respectively; sw1 represents the rotation angle of the boom; am1 represents the amplitude variation angle of the first arm section; am2 represents the amplitude variation angle of the second arm section; Ext represents the telescopic length of the second arm section; am3 represents the amplitude variation angle of the third arm section; sw2 represents the rotation angle of the gripper support; A, B, C, D, E and F represent given structural constants.

6. The control method according to claim 5, characterized in that: The first rotation angle solution model expression is: sw1=atan2(R(y),R(x)); Among them, atan2 represents the four-quadrant inverse sine function calculation.

7. The control method according to claim 5, characterized in that: The second rotation angle solution model expression is: Among them, atan2 represents the four-quadrant inverse sine function calculation.

8. The control method according to claim 5, characterized in that: The first amplitude angle solution model expression is: am1=am10, sin(sw1)≠0and sin(sw2)=0; am1=am10, sin(sw1)=0 and sin(sw2)=0; Among them, atan2 represents the four-quadrant inverse sine function calculation; R zC =μ*sin am1+h*cos am1; μ=(Ext+A)*cosam2+CB*sin(am2+am3-E); h=(Ext+A)*sinam2-B*cos(am2+am3-E); am10 represents a predetermined first amplitude angle value; The second amplitude angle solution model expression is: am2=am20; Wherein, am20 represents a predetermined second amplitude variation angle value; The third amplitude angle solution model expression is: sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)=0; sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)≠0; sin(sw1)=0and sin(sw2)=0and cos(am1+am2)=0; sin(sw1)=0and sin(sw2)=0and cos(am1+am2)≠0; in, The telescopic length solution model expression is: Ext=C 4z -A,sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)=0; Ext=C 4y -A,sin(sw1)≠0and sin(sw2)=0and cos(am1+am2)≠0; Ext=C 5z ,sin(sw1)=0and sin(sw2)=0and cos(am1+am2)=0; Ext=CC 6x ,sin(sw1)=0and sin(sw2)=0and cos(am1+am2)≠0; in, c=R y_C 2 +R z_C 2 -t 2 ; c2=R x_C 2 +R z_c 2 -t 2 ; t = C; R z_C =F-R(z)-B*cos(am1+am2+am3-E); 9. The control method according to claim 3, characterized in that: According to the position interpolation curve, velocity interpolation curve and acceleration interpolation curve of the joint variables, a dual closed-loop adjustment control method of speed closed-loop adjustment and position closed-loop adjustment is adopted to control the rotation of the upper arm, the rotation of the first section arm, the rotation of the second section arm, the extension and retraction of the second section arm, the rotation of the third section arm, and the rotation of the gripper support, so that the gripper mechanism moves to the set target position.

Images