Method and system for motion control of a telescopic rotary tube mill

Abstract

The present application belongs to the technical field of drilling in soil or rock, and relates to a movement control method and system for a telescopic rotary pipe rack, which comprises the following steps: establishing a coordinate system and kinematic relationship of the end movement of the pipe rack; performing position calibration on the end of the pipe rack based on the coordinate system and determining a target movement path; performing start preparation and calculating initial movement parameters based on the target movement path; executing the end movement of the pipe rack according to the initial movement parameters and obtaining a current position; performing deviation judgment based on the current position and the target movement path and dynamically adjusting the movement path; and adjusting the movement speed of the pipe rack based on the deviation judgment result to realize positioning. By constructing the mathematical and kinematic relationship among the end position, movement speed, telescopic amount and rotation angle of the pipe rack, the present application effectively solves the problems of insufficient operation precision of the pipe rack and complicated operation caused by the lack of collaborative control of telescopic and rotary servo, the need for manual intervention in trajectory and speed setting and poor adaptability in the prior art.

Description

Technical Field

[0001] This invention relates to the field of drilling technology for soil or rock, and more specifically, to a motion control method and system for a telescopic rotary pipe laying machine. Background Technology

[0002] The pipe laying machine is a key piece of equipment in oil drilling rigs. It is responsible for transporting the drill string during operations such as string erection, tripping, and drilling. Its motion control method and positioning accuracy directly affect the quality of pipe laying and production efficiency.

[0003] Existing telescopic rotary fully automatic pipe laying machines typically achieve planar positioning through servo electric cylinders driving the telescopic arm's amplitude variation and servo motors driving the rotation mechanism's angle adjustment. Height and posture are fixed by the mechanical structure and require no dynamic control; therefore, amplitude variation and angle control are the core of their operational quality. Existing motion control has significant drawbacks: telescopic and rotational servo controls are independent, lacking a coordinated relationship, resulting in inaccurate trajectory and coordinate description; manual segmentation of dense trajectory points is required for safety; servo speeds for each trajectory segment are manually matched fixed values, leading to poor adaptability; and after equipment relocation and reinstallation, manually set data becomes invalid, requiring re-adjustment.

[0004] Therefore, existing technologies suffer from problems such as insufficient operating accuracy and cumbersome operation of pipe laying machines due to the lack of coordinated control between telescopic and rotational servos, the need for manual intervention in trajectory and speed settings, and poor adaptability. Summary of the Invention

[0005] This invention provides a motion control method and system for a telescopic rotary pipe laying machine, the purpose of which is to solve the problems of insufficient operating accuracy and cumbersome operation of existing pipe laying machines.

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

[0007] According to one aspect of the present invention, a motion control method for a telescopic rotary pipe laying machine is provided, comprising the following steps:

[0008] S1. Establish the coordinate system and kinematic relationships for the end motion of the pipe laying machine;

[0009] S2. Based on the coordinate system, the position of the end of the pipe laying machine is calibrated and the target movement path is determined;

[0010] S3. Prepare for startup based on the target motion path and calculate initial motion parameters;

[0011] S4. Execute the end motion of the pipe laying machine according to the initial motion parameters and obtain the current position;

[0012] S5. Determine the deviation between the current position and the target's movement path and dynamically adjust the movement path accordingly;

[0013] S6. Adjust the speed of the pipe laying machine based on the deviation judgment result to achieve positioning.

[0014] Furthermore, S1 includes the following steps:

[0015] S1.1. Establish a rectangular coordinate system with the center of the pipe laying machine base as the origin;

[0016] S1.2. Establish a polar coordinate system based on a rectangular coordinate system;

[0017] S1.3. Based on the mechanical transmission relationship of the pipe laying machine, establish the mathematical relationship between the end position of the pipe laying machine and the extension and rotation angle;

[0018] S1.4. Establish the kinematic relationship between the end-effector speed of the pipe laying machine and its extension and rotation speed.

[0019] Furthermore, S2 includes the following steps:

[0020] S2.1. Select the key feature position of the end of the pipe laying machine on the target movement path as the calibration position;

[0021] S2.2. Determine the coordinate data of each calibration position in the coordinate system;

[0022] S2.3. Based on the coordinate data of the calibrated position, determine the target movement path of the end of the pipe laying machine.

[0023] Furthermore, the key feature positions include at least the starting position, target position, and turning point of the pipe laying machine end on the target movement path.

[0024] Furthermore, S3 includes the following steps:

[0025] S3.1. Determine whether the initial position of the pipe laying machine's end is located on the target movement path;

[0026] S3.2. Based on the judgment results, plan the transition path that cuts into the target movement path from the initial position at the end of the pipe laying machine;

[0027] S3.3. Calculate the initial motion parameters based on the target motion path, the initial position of the pipe laying machine end, and the preset motion speed.

[0028] Furthermore, S4 includes the following steps:

[0029] S4.1. Based on the initial motion parameters, control the end of the pipe laying machine to move along the target motion path;

[0030] S4.2. During the movement, the current position of the pipe laying machine end is obtained in real time;

[0031] S4.3. Based on the current position, represent the position of the end of the pipe laying machine as coordinate values ​​in the coordinate system.

[0032] Furthermore, S5 includes the following steps:

[0033] S5.1. Compare the current position of the pipe laying machine end with the target movement path and calculate the movement deviation;

[0034] S5.2. Determine whether the motion deviation exceeds the preset allowable range;

[0035] S5.3. Based on the judgment result, the current position, and the target position, dynamically adjust the remaining segments of the target's movement path;

[0036] S5.4. Based on the adjusted target motion path, recalculate the corresponding motion parameters.

[0037] Furthermore, S6 includes the following steps:

[0038] S6.1. The remaining distance between the current position and the target position of the pipe laying machine end is used as the control input.

[0039] S6.2. Adjust the movement speed of the end of the pipe laying machine according to the remaining distance;

[0040] S6.3. When the remaining distance is less than or equal to the preset positioning accuracy, it is determined that the end of the pipe laying machine has completed positioning.

[0041] Furthermore, when adjusting the movement speed based on the remaining distance, a PID control algorithm is used, with the remaining distance between the current position of the pipe laying machine end and the target position as the input, to dynamically adjust the linear speed of the end movement.

[0042] According to another aspect of the present invention, a motion control system for a telescopic rotary pipe laying machine is provided, comprising the aforementioned motion control method for a telescopic rotary pipe laying machine, including:

[0043] Control unit, human-machine interface unit, position detection unit, and drive execution unit;

[0044] The control unit is used to establish the coordinate system and kinematic relationship of the end motion of the pipe laying machine, determine the target motion path of the end motion of the pipe laying machine based on the calibration results, calculate the motion parameters corresponding to the target motion path, and dynamically adjust the target motion path according to the current position of the end motion of the pipe laying machine.

[0045] The human-machine interface unit is electrically connected to the control unit and is used to set the operating functions and motion parameters of the pipe laying machine, and to send the operating functions and motion parameters to the control unit.

[0046] The position detection unit is electrically connected to the control unit and is used to collect the extension and rotation angle of the pipe laying machine and send data to the control unit to determine the current position of the end of the pipe laying machine.

[0047] The drive execution unit is electrically connected to the control unit and is used to receive motion parameters sent by the control unit, and drive the telescopic and rotating mechanisms of the pipe laying machine to move the end of the pipe laying machine along the target motion path.

[0048] The beneficial effects of this invention are as follows:

[0049] This invention establishes a rectangular coordinate system and a matching polar coordinate system with the center of the pipe laying machine base as the origin. Combined with the mechanical transmission relationship of the pipe laying machine, it constructs the mathematical and kinematic relationship between the end position, movement speed, extension and contraction, and rotation angle of the pipe laying machine. This achieves a unified coordinate description under different installation conditions, eliminating the need to redefine the motion model and reducing the workload of debugging. At the same time, it transforms complex planar motion into radial and angular coordinated control, simplifying the modeling and control difficulty. It effectively solves the problems of needing to re-debug after equipment relocation and reinstallation, and the inability to accurately describe the trajectory and coordinates.

[0050] This invention calibrates and fits the target path by selecting key feature points on the target motion path, collects the current position of the end point in real time during operation, calculates motion deviations and dynamically adjusts the path, and combines kinematic relationships to achieve coordinated speed calculation of the telescopic axis and the rotation axis. This reduces manual intervention, avoids deviation accumulation, improves the accuracy and speed adaptability of trajectory tracking, and solves the problems of needing to manually set dense trajectory points in segments and poor servo speed adaptability of each trajectory segment.

[0051] This invention employs a PID algorithm to dynamically adjust the movement speed using the remaining distance between the current position and the target position of the pipe laying machine as the control input. Simultaneously, it achieves the coordination of path planning, motion control, and position detection through a matching closed-loop control system. Operators can easily set parameters without complex operations, which not only improves the positioning accuracy of the pipe laying machine but also reduces the difficulty of operation, thus addressing the problems of insufficient operating accuracy and cumbersome operation of existing pipe laying machines.

[0052] In summary, this invention effectively solves the problems of lack of servo coordination, inaccurate trajectory description, need for readjustment after relocation and reinstallation, excessive manual intervention, and insufficient positioning accuracy in existing equipment by combining coordinate system and kinematic modeling, dynamic path correction, PID speed adjustment, and closed-loop control technologies. Furthermore, this invention establishes a dual coordinate system based on the base center and constructs motion mathematical relationships to achieve unified coordinate description across multiple working conditions, reducing the difficulty of modeling and debugging. Key point calibration and real-time deviation correction enable dual-axis speed collaborative calculation, avoiding deviation accumulation and reducing manual operation. Simultaneously, this invention uses a PID algorithm combined with a closed-loop control system to precisely adjust the motion speed, improving positioning accuracy and simplifying the operation process. The interconnected technical aspects of this invention enhance the stability and adaptability of the pipe laying machine's motion control, effectively solving the problems of insufficient operating accuracy and cumbersome operation in existing pipe laying machines. Attached Figure Description

[0053] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained from these drawings without creative effort.

[0054] Figure 1 This is a flowchart of the method provided by the present invention;

[0055] Figure 2 This is a schematic diagram of the telescoping-rotation cooperative control model provided by the present invention;

[0056] Figure 3 This is a diagram of the control system provided by the present invention;

[0057] Figure 4 This is a schematic diagram of the method provided by the present invention;

[0058] Figure 5 This is a schematic diagram of the path planning provided by the present invention;

[0059] Figure 6 This is a trajectory diagram of an example for verifying the motion algorithm provided by this invention. Detailed Implementation

[0060] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, not all of them. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to represent selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0061] Example 1

[0062] Please refer to this as well. Figure 1-6 This embodiment provides a motion control method for a telescopic rotary pipe laying machine, including the following steps:

[0063] S1. Establish the coordinate system and kinematic relationships for the end motion of the pipe laying machine;

[0064] S2. Based on the coordinate system, the position of the end of the pipe laying machine is calibrated and the target movement path is determined;

[0065] S3. Prepare for startup based on the target motion path and calculate initial motion parameters;

[0066] S4. Execute the end motion of the pipe laying machine according to the initial motion parameters and obtain the current position;

[0067] S5. Determine the deviation between the current position and the target's movement path and dynamically adjust the movement path accordingly;

[0068] S6. Adjust the speed of the pipe laying machine based on the deviation judgment result to achieve positioning.

[0069] Furthermore, S1 includes the following steps:

[0070] S1.1. Establish a rectangular coordinate system with the center of the pipe laying machine base as the origin;

[0071] In this embodiment, a rectangular coordinate system is established with the center of the pipe laying machine base as the origin to describe the planar motion of the pipe laying machine end. The coordinate axes of the rectangular coordinate system are determined according to the installation direction of the pipe laying machine on the drilling platform, and are used to uniformly describe the positional changes of the pipe laying machine end in the plane.

[0072] By using the center of the base as the origin, the pipe laying machine can be described under a unified coordinate reference under different installation conditions and different working areas, thereby avoiding the need to redefine the motion model due to equipment relocation or changes in installation location.

[0073] S1.2. Establish a polar coordinate system based on a rectangular coordinate system;

[0074] Considering the structural characteristics of the pipe-laying machine, which involves a combination of telescopic amplitude change and planar rotation, a polar coordinate system is established on top of the rectangular coordinate system to describe the radial distance and angular position of the pipe-laying machine's end point relative to the center of the base. The radial parameter of the polar coordinate system corresponds to the amplitude change of the pipe-laying machine's telescopic mechanism, and the angular parameter corresponds to the rotation angle of the pipe-laying machine's rotation mechanism, ensuring that the positional representation of the pipe-laying machine's end point is consistent with its actual mechanical motion. By introducing the polar coordinate system, the complex planar motion problem can be transformed into a coordinated control problem in both the radial and angular directions, thereby reducing the complexity of motion modeling and control algorithms.

[0075] S1.3. Based on the mechanical transmission relationship of the pipe laying machine, establish the mathematical relationship between the end position of the pipe laying machine and the extension and rotation angle;

[0076] Based on the mechanical transmission structure and geometric relationship of the pipe laying machine, a mathematical correspondence is established between the position parameters of the pipe laying machine's end in the coordinate system and the extension / retraction amount of the telescopic mechanism and the rotation angle of the rotating mechanism. Specifically:

[0077] Based on the mechanical transmission principle and geometric relationships of the pipe laying machine, the following relationship is established:

[0078] Relationship between the projected length (amplitude) of the pipe laying machine's telescopic arm and the telescopic servo (position):

[0079]

[0080] in:

[0081]

[0082] Relationship between telescopic arm angle and rotary servo (position):

[0083]

[0084] Relationship between the projection change speed of the telescopic arm and the rotation speed of the telescopic servo:

[0085]

[0086] Relationship between telescopic boom rotation speed and rotary servo speed:

[0087]

[0088] Furthermore, the linear velocity of the telescopic arm in the direction of rotation can be obtained:

[0089]

[0090] in:

[0091] For amplitude length, For the structural parameters of the pipe laying machine,

[0092] This refers to the transmission ratio of the amplitude-changing mechanism. For the mechanical angle of the luffing electric cylinder;

[0093] For the lead of the luffing cylinder, For variable amplitude servo gear ratio;

[0094] For telescopic servo position feedback (corrected value after calibration);

[0095] The angle of rotation of the telescopic arm. The transmission ratio of the rotating mechanism;

[0096] For rotary servo gear ratio, For rotary servo position feedback (corrected value after calibration);

[0097] For variable amplitude telescoping linear velocity, For telescopic servo speed;

[0098] The rotational speed of the telescopic arm. The linear velocity of the telescopic arm's rotation;

[0099] This refers to the rotational servo speed.

[0100] Based on the above mathematical relationship, the spatial position of the pipe laying machine end in the rectangular coordinate system or polar coordinate system can be calculated when the extension and rotation angle are known, thereby realizing a unified description of the position of the pipe laying machine end. In practical applications, this mathematical relationship can be used as the basis for position inverse and forward calculations, and can be used for functions such as path tracking, deviation calculation, and positioning judgment.

[0101] S1.4. Establish the kinematic relationship between the end-effector speed of the pipe laying machine and its extension and rotation speed.

[0102] In this embodiment, based on the established positional mathematical relationship, the kinematic relationship between the linear velocity of the pipe laying machine end and the movement speed of the telescopic mechanism and the movement speed of the rotating mechanism is further established.

[0103] Analysis of the end motion of the pipe laying machine reveals that its kinematic principle is a variable-diameter circular motion. The end motion velocity can be decomposed into radial and tangential velocity components, which are respectively mapped to the motion velocities of the telescopic and rotary axes; specifically as follows:

[0104] When the linear velocity of the pipe laying machine is At that time, its radial linear velocity is The tangential linear velocity is ,and Further steps will yield results Through this kinematic relationship, the motion velocities of the telescopic shaft and the rotary shaft can be calculated collaboratively based on the linear velocity of the end target, thereby avoiding the problem of speed matching relying on human experience in the traditional independent control method.

[0105] Furthermore, S2 includes the following steps:

[0106] S2.1. Select the key feature position of the end of the pipe laying machine on the target movement path as the calibration position;

[0107] In this embodiment, based on the specific operational function to be performed by the pipe laying machine, several representative key feature locations on the target movement path are selected as calibration locations and denoted as calibration points. .

[0108] Furthermore, the key feature locations include at least the starting position, target position, and turning points of the pipe laying machine end on the target movement path.

[0109] Key feature locations are used to define the geometry of the target's motion path. The selection principle is to fully reflect the initial state, change process, and termination state of the end-of-pipe movement trajectory of the pipe laying machine.

[0110] In some implementations, the number of key feature locations is adjusted according to the complexity of the target motion path in order to reduce the calibration workload while ensuring path accuracy. For reference, under the premise of meeting the usage requirements, the target path for calibration is preferably a straight line (requiring only two calibration points), followed by a quadratic curve or a circle (requiring three calibration points), thereby reducing the number of calibration points and reducing the calibration difficulty.

[0111] S2.2. Determine the coordinate data of each calibration position in the coordinate system;

[0112] In this embodiment, the coordinate data of each calibration position in the coordinate system is determined by manual teaching or calculation based on known geometric relationships, and the coordinate data is used as the basic input parameter for path planning.

[0113] For the calibration position obtained through manual teaching, after the end of the pipe laying machine moves to the corresponding position under manual control, the current extension and rotation angle are collected and recorded, and converted into position data in the coordinate system.

[0114] For calibration positions that can be derived through geometric relationships, the rectangular coordinate position of the pipe laying machine can be directly calculated based on the known calibration positions. and This reduces human intervention and improves calibration efficiency.

[0115] S2.3. Based on the coordinate data of the calibrated position, determine the target movement path of the end of the pipe laying machine.

[0116] In this embodiment, the target motion path is fitted or constructed based on the acquired calibration position coordinate data, so that the target motion path follows a curve. The form is determined in the coordinate system; when the target motion path can be described by a single curve, the corresponding curve form is used for path determination; when the target motion path cannot be reasonably described by a single curve, multiple sub-paths are used for combined description; by adopting a segmented path approach, the versatility and flexibility of the path planning method can be improved while ensuring path accuracy.

[0117] Furthermore, S3 includes the following steps:

[0118] S3.1. Determine whether the initial position of the pipe laying machine's end is located on the target movement path;

[0119] In this embodiment, before starting the pipe laying machine to execute the target movement path, the relationship between the initial position of the pipe laying machine's end and the target movement path is first determined to confirm whether the end of the pipe laying machine is in a state where it can directly enter the path for operation. This avoids the pipe laying machine's end starting directly if it deviates from the target path, thereby causing trajectory errors or operational risks. In some implementations, this determination can be based on the spatial distance between the initial position of the pipe laying machine's end and the target movement path.

[0120] S3.2. Based on the judgment results, plan the transition path that cuts into the target movement path from the initial position at the end of the pipe laying machine;

[0121] In this embodiment, when it is determined that the initial position of the pipe laying machine end is not on the target motion path, the pipe laying machine end is re-entered into the target motion path by adjusting the actions of the telescopic mechanism and the rotating mechanism.

[0122] This adjustment process guides the end of the pipe laying machine to a suitable starting position on the target movement path, providing stable initial conditions for subsequent path tracking. In some implementations, this adjustment process can be performed as a separate return process to reduce manual intervention.

[0123] S3.3. Calculate the initial motion parameters based on the target motion path, the initial position of the pipe laying machine end, and the preset motion speed.

[0124] In this embodiment, after the end of the pipe laying machine is on the target motion path, the initial motion parameters of the telescopic mechanism and the rotating mechanism are calculated by combining the geometric features of the target motion path, the initial position of the end of the pipe laying machine, and the preset running speed.

[0125] The initial motion parameters are used as control inputs for the pipe laying machine to start running along the target motion path, so that the end of the pipe laying machine can enter the path running state with the correct speed and direction.

[0126] This calculation process is based on the kinematic relationships established above; specifically as follows:

[0127] Based on the target linear velocity and combined The position of the pipe laying machine on the path can be calculated. radial velocity at time and tangential speed By combining the position and speed of the pipe laying machine with the transmission relationship of the servo drive, the real-time rotational speed of the telescopic servo when running at the target speed at any position on the target path can be further calculated. Real-time rotational speed of the rotary servo By dynamically controlling two servos to run at the calculated speed, the purpose of path and speed control can be achieved.

[0128] The following is a straight line along the target path from the center of the end of the pipe laying machine. Taking motion as an example, this process is explained in detail:

[0129] Set the target speed as Target line The angle between the coordinate system and the positive X-axis is Then the end of the pipe laying machine along During operation, based on the mathematical relationship established above between the end position of the pipe laying machine and the extension and rotation angles, the speed of the two-axis servo of the pipe laying machine at any position can be calculated as follows:

[0130] Rotary servo speed:

[0131] ,

[0132] Amplitude servo speed:

[0133] ,

[0134] It should be noted that because the control method has a dynamic correction function, the angle β between the target path and the positive X-axis of the coordinate system will be dynamically calculated synchronously with the dynamically adjusted path.

[0135] Furthermore, S4 includes the following steps:

[0136] S4.1. Based on the initial motion parameters, control the end of the pipe laying machine to move along the target motion path;

[0137] Based on the calculated initial motion parameters, the telescopic mechanism and the rotating mechanism are controlled to work together to make the end of the pipe laying machine start running along the target motion path; by coordinating the control of the telescopic shaft and the rotating shaft, the actual motion trajectory of the end of the pipe laying machine is kept consistent with the target motion path.

[0138] S4.2. During the movement, the current position of the pipe laying machine end is obtained in real time;

[0139] During the movement of the pipe-laying machine's end point along the target path, the extension and rotation angles of the machine are collected in real time by setting telescopic position encoders and rotary position encoders. These data are used to reflect the real-time motion state of the pipe-laying machine's end point, and the real-time position of the end point is recorded as follows: The collected data provides a basis for subsequent motion deviation judgment and dynamic path adjustment.

[0140] S4.3. Based on the current position, represent the position of the end of the pipe laying machine as coordinate values ​​in the coordinate system.

[0141] Based on the real-time collected extension and rotation angles, and combined with the previously established positional mathematical relationships, the current position of the pipe laying machine end is converted into coordinate values ​​in the coordinate system, so that subsequent deviation calculations and path adjustments can be completed under a unified spatial model.

[0142] in:

[0143] ;

[0144] ;

[0145] Furthermore, S5 includes the following steps:

[0146] S5.1. Compare the current position of the pipe laying machine end with the target movement path and calculate the movement deviation;

[0147] In this embodiment, the real-time position of the pipe laying machine end is compared with the target movement path to calculate the movement deviation of the pipe laying machine end relative to the target movement path, so as to reflect the degree of deviation of the pipe laying machine's running trajectory.

[0148] S5.2. Determine whether the motion deviation exceeds the preset allowable range;

[0149] In this embodiment, the calculated actual deviation Deviation from the preset limit A comparison is made to determine whether the current operating status of the pipe laying machine meets the control requirements; this determination is used to distinguish between a state that can continue to operate through correction and an abnormal state that needs to be stopped.

[0150] S5.3. Based on the judgment result, the current position, and the target position, dynamically adjust the remaining segments of the target's movement path;

[0151] like If the vehicle has deviated from its trajectory, it will stop, issue a warning, and cease all corrections.

[0152] When the motion deviation is within the allowable range, that is < Based on the current position of the pipe laying machine's end point, the target position, and the remaining portion of the original target movement path, the subsequent movement path is replanned to prevent deviations from accumulating gradually during operation; specifically as follows:

[0153] From current location Current location and the original target path The remaining segments are used as path calibration points to replan the movement path. Then execute the motion algorithm.

[0154] refer to Figure 6 straight line L AB For the initially planned straight path segment, point C is the theoretical position of the pipe laying machine's end point at a certain moment as it moves along this path. CB This corresponds to the remaining theoretical path segment; due to motion deviation, the actual position of the pipe laying machine end at this moment is C', which has a positional deviation δ from the theoretical position C.

[0155] When the position deviation δ does not exceed the preset allowable deviation range (which can be set during the equipment design phase according to the control accuracy requirements of the application scenario), the current actual position of the pipe laying machine end, the target position, and the preset path curve type are used. Figure 6 Using the straight line as the planning baseline, the remaining target paths are replanned, and the remaining path segments are updated to L. C'B By repeatedly executing deviation judgment and path replanning operations, dynamic deviation correction control of the end motion of the pipe laying machine can be achieved.

[0156] S5.4. Based on the adjusted target motion path, recalculate the corresponding motion parameters.

[0157] After the target motion path is updated, the motion parameters of the telescopic mechanism and the rotary mechanism are recalculated based on the adjusted path and used for subsequent drive control.

[0158] Furthermore, S6 includes the following steps:

[0159] S6.1. The remaining distance between the current position and the target position of the pipe laying machine end is used as the control input.

[0160] In this embodiment, the current position of the pipe laying machine end is... With the target location The remaining distance between As a control input for adjusting the movement speed, it is used to reflect the degree to which the pipe laying machine approaches the target position.

[0161] S6.2. Adjust the movement speed of the end of the pipe laying machine according to the remaining distance;

[0162] Furthermore, when adjusting the movement speed based on the remaining distance, a PID control algorithm is used, with the remaining distance between the current position of the pipe laying machine end and the target position as the input, to dynamically adjust the linear speed of the end movement.

[0163] In this embodiment, the running speed of the pipe laying machine's end point is dynamically adjusted based on the change in the remaining distance, causing the pipe laying machine to gradually decelerate as it approaches the target position, thereby improving positioning accuracy; specifically as follows:

[0164] The PID algorithm is used to determine the current position of the pipe laying machine's end point. With the target location Distance between The target linear velocity of the pipe laying machine is dynamically controlled as the input to solve the positioning accuracy problem. The control equation is as follows: , where parameters , , The parameters are PID control parameters. This method is existing technology and will not be further explained here.

[0165] S6.3. When the remaining distance is less than or equal to the preset positioning accuracy, it is determined that the end of the pipe laying machine has completed positioning.

[0166] In this embodiment, the remaining distance between the current position of the pipe laying machine's end point and the target position... Meets the preset positioning accuracy When required, determine that the end of the pipe laying machine has completed positioning and end the current movement process.

[0167] Example 2

[0168] Please refer to this as well. Figure 1-6 Based on Embodiment 1, this embodiment provides a motion control system for a telescopic rotary pipe laying machine, using the aforementioned motion control method for a telescopic rotary pipe laying machine. The system includes:

[0169] Control unit, human-machine interface unit, position detection unit, and drive execution unit;

[0170] The control unit is used to establish the coordinate system and kinematic relationship of the end motion of the pipe laying machine, determine the target motion path of the end motion of the pipe laying machine based on the calibration results, calculate the motion parameters corresponding to the target motion path, and dynamically adjust the target motion path according to the current position of the end motion of the pipe laying machine.

[0171] The human-machine interface unit is electrically connected to the control unit and is used to set the operating functions and motion parameters of the pipe laying machine, and to send the operating functions and motion parameters to the control unit.

[0172] The position detection unit is electrically connected to the control unit and is used to collect the extension and rotation angle of the pipe laying machine and send data to the control unit to determine the current position of the end of the pipe laying machine.

[0173] The drive execution unit is electrically connected to the control unit and is used to receive motion parameters sent by the control unit and drive the telescopic and rotating mechanisms of the pipe laying machine to move the end of the pipe laying machine along the target motion path.

[0174] The control unit judges the motion deviation of the pipe laying machine end based on the current position data sent by the position detection unit, and realizes the positioning of the pipe laying machine end by adjusting the motion parameters sent to the drive execution unit.

[0175] The control unit is used to establish the coordinate system and kinematic relationship of the end motion of the pipe laying machine, determine the target motion path of the end motion of the pipe laying machine based on the calibration results, calculate the motion parameters corresponding to the target motion path, and dynamically adjust the target motion path according to the current position of the end motion of the pipe laying machine.

[0176] The control unit pre-establishes a coordinate system model with the center of the pipe laying machine base as the origin, and combines the mechanical transmission relationship of the pipe laying machine's telescopic and rotating mechanisms to complete the mathematical modeling of the relationship between the end position of the pipe laying machine and its motion parameters. After receiving calibration data, the control unit determines the target motion path of the pipe laying machine's end based on the calibration position, and dynamically updates the target motion path based on real-time position feedback during operation, thereby realizing closed-loop control of the pipe laying machine's end motion trajectory; preferably, the control unit is a PLC controller.

[0177] The human-machine interface unit is electrically connected to the control unit and is used to set the operating functions and motion parameters of the pipe laying machine, and to send the operating functions and motion parameters to the control unit.

[0178] The human-machine interaction unit is preferably an operation screen, which provides an interactive interface between the operator and the motion control system of the pipe laying machine. The operator can select the specific operation function that the pipe laying machine needs to perform and set the corresponding operating parameters through the human-machine interaction unit.

[0179] Operating parameters include, but are not limited to, the operating speed and target position of the pipe laying machine's end. The human-machine interface unit sends the operating functions and motion parameters to the control unit, which then completes subsequent path planning and motion control based on this information.

[0180] The position detection unit is electrically connected to the control unit and is used to collect the extension and rotation angle of the pipe laying machine and send data to the control unit to determine the current position of the end of the pipe laying machine.

[0181] In this embodiment, the position detection unit includes a telescopic position encoder and a rotary position encoder, which are used to acquire the telescopic amount of the pipe laying machine's telescopic mechanism and the rotation angle of the rotary mechanism in real time, and send the collected data to the control unit.

[0182] Based on the extension and rotation angle data, combined with the established coordinate system and positional mathematical relationships, the control unit calculates the current position of the pipe laying machine end in the coordinate system, providing a data basis for motion deviation judgment, path dynamic adjustment and positioning control.

[0183] The drive execution unit is electrically connected to the control unit and is used to receive motion parameters sent by the control unit and drive the telescopic and rotating mechanisms of the pipe laying machine to move the end of the pipe laying machine along the target motion path.

[0184] In this embodiment, the drive execution unit includes a telescopic servo driver and a rotary servo driver. Based on the motion parameters calculated by the control unit, the telescopic mechanism and the rotary mechanism of the pipe laying machine are driven and controlled respectively, so that the two work together to achieve continuous movement of the end of the pipe laying machine along the target motion path.

[0185] By having the control unit uniformly calculate and distribute motion parameters, the problem of speed matching between the telescopic mechanism and the rotating mechanism relying on manual settings is avoided, thus improving the trajectory consistency and positioning accuracy during the operation of the pipe laying machine.

[0186] The control unit judges the motion deviation of the pipe laying machine end based on the current position data sent by the position detection unit, and realizes the positioning of the pipe laying machine end by adjusting the motion parameters sent to the drive execution unit.

[0187] In this embodiment, the control unit continuously receives current position data sent by the position detection unit during the operation of the pipe laying machine, and compares the current position with the target motion path to determine whether there is a motion deviation at the end of the pipe laying machine. When the motion deviation is within the allowable range, the control unit dynamically adjusts the target motion path based on the current position and the target position, and recalculates the corresponding motion parameters; when the end of the pipe laying machine approaches the target position, the control unit adjusts the motion parameters to gradually decelerate the end of the pipe laying machine, thereby achieving precise positioning.

[0188] It is worth noting that, for reference Figure 3 In this embodiment, the signal input terminal of the PLC controller, in addition to communicating with the human-machine interaction unit and the position detection unit to receive operation commands issued by the operation screen and detection data collected by the position detection unit, also connects to various peripheral status detection signals of the pipe laying machine. These include extension servo limit signals, rotation servo limit signals, clamping cylinder limit signals, clamping mechanism limit signals, support cylinder position signals, lifting cylinder position signals, tilting cylinder position signals, clamp head detection of material, clamp head successful material removal signal, clamp head successful material release signal, and finger beam stop bar position signals, etc., to enable real-time... The PLC controller acquires the operating status of each actuator and working component of the pipe laying machine, providing comprehensive status data for the motion control at the end of the machine and ensuring the safety and continuity of the motion control process. In addition to connecting to the telescopic and rotary servo drives of the drive unit to output motion control parameters for the telescopic and rotary mechanisms, the PLC controller's signal output terminals also communicate with other actuator drive components of the pipe laying machine, enabling coordinated control of various working components such as cylinders, clamps, and finger beams, ensuring the coordinated operation of the end-of-line motion control and the overall work process. The PLC controller's access methods for various peripheral status detection signals, signal processing logic, and control methods and linkage logic for other actuators are all existing mature technologies. This invention does not improve upon these technologies; it merely relies on these existing mature technologies to provide supporting operational status and overall process coordination for the motion control at the end of the pipe laying machine.

[0189] To better illustrate this, the operating principle of the system of the present invention will be explained below:

[0190] After the system is powered on, the PLC controller first initializes the internal control program, coordinate system model, and communication with the operator panel, position encoder, and servo drive, and then enters standby mode to wait for operation instructions from the operator panel.

[0191] When the system is in standby mode, operators can manually control the pipe laying machine via the control panel. Manual operation includes two modes: single-action mode and linkage mode. In single-action mode, the control panel sends individual telescopic or rotation control commands to the PLC controller. The PLC controller then sends control signals to the telescopic servo driver or the rotation servo driver, causing the pipe laying machine to perform only telescopic or rotational movements. This allows operators to easily move the pipe laying machine to the designated position according to the site conditions.

[0192] In linkage mode, the PLC controller, based on a pre-established Cartesian coordinate system and kinematic model, calculates the target motion direction of the pipe laying machine's end in the coordinate system in real time according to the linkage operation commands issued by the operation panel. It also coordinates and controls the telescopic servo drive and the rotary servo drive to make the pipe laying machine's end move linearly along the X or Y direction of the Cartesian coordinate system. The motion trajectory and calibration point in this linkage mode are automatically set by the PLC controller, requiring no manual calibration. Its motion process also follows the closed-loop control and dynamic correction logic described later.

[0193] Before executing the automatic operation function, the PLC controller first completes the position calibration of the pipe laying machine's end position. Position calibration can be performed in two ways: First, the operator manually controls the pipe laying machine to move to the calibration position via the control panel. The telescopic position encoder and rotary position encoder respectively collect the current telescopic amount and rotation angle of the pipe laying machine and send the collected data to the PLC controller, which stores the corresponding end position information as calibration data. Second, the PLC controller calculates the corresponding calibration position data based on the geometric relationship between the position to be calibrated and the already calibrated position. Obtaining the calibration point through calculation significantly reduces manual operation and improves the ease of use of the system.

[0194] After the position calibration is completed, the PLC controller automatically plans the initial motion path of the pipe laying machine end based on the set of calibration points corresponding to different operation functions, and uses this path as the reference path for subsequent motion state judgment and trajectory correction.

[0195] Before initiating automatic operation, the PLC controller determines whether the end of the pipe laying machine is located on the initial motion path corresponding to the function to be performed, based on the current position data fed back by the telescopic position encoder. When the end of the pipe laying machine is not on the path, the PLC controller disables the positioning function and controls the telescopic servo drive and rotary servo drive to automatically return the end of the pipe laying machine to the initial motion path through a preset safety return program, thereby reducing manual intervention; when the end of the pipe laying machine is on the initial motion path, the PLC controller allows automatic operation to be initiated.

[0196] When automatic operation is started, the operator sets the running speed of the pipe laying machine's end point and selects the corresponding operation function through the operation screen. After receiving the start command, the PLC controller first re-determines the initial position of the pipe laying machine's end point to confirm that it is still on the initial motion path. Then, based on the set running speed, the initial motion path, the initial position of the pipe laying machine's end point, and the transmission relationship between the position of the pipe laying machine's end point and the servo drive, it calculates the initial running speed of the telescopic servo and the rotary servo.

[0197] The PLC controller sends the initial running speed to the telescopic servo drive and the rotary servo drive respectively. The servo drives drive the telescopic and rotary mechanisms of the pipe laying machine to move synchronously, so that the end of the pipe laying machine starts to move along the initial motion path.

[0198] During the operation of the pipe laying machine, the telescopic position encoder and the rotary position encoder continuously collect the telescopic amount and rotation angle of the pipe laying machine, and send the collected data to the PLC controller in real time. Based on the feedback data and the established coordinate system model and position mathematical relationship, the PLC controller calculates the current position of the end of the pipe laying machine in the rectangular coordinate system in real time.

[0199] The PLC controller compares the calculated current position with the initial motion path to determine the motion deviation at the end of the pipe laying machine. When the motion deviation exceeds a preset threshold, the PLC controller can control the system to stop and issue an alarm; when the motion deviation is within the allowable range, the PLC controller updates the target motion path based on the current position of the pipe laying machine end, the target position, and the remaining part of the initial motion path, and recalculates the corresponding motion parameters based on the updated target motion path.

[0200] The aforementioned position determination, path update, and motion parameter calculation processes are all executed periodically by the PLC controller in a timed interrupt routine to ensure the real-time performance and stability of the control process. The PLC controller sends the real-time calculated motion parameters to the telescopic servo drive and the rotary servo drive to drive the pipe laying machine to continue moving.

[0201] By continuously repeating the closed-loop control process of position acquisition, position judgment, path update and drive execution, the dynamic correction of the end motion trajectory of the pipe laying machine is realized, so that the actual motion trajectory of the end of the pipe laying machine continuously approaches the planned path, thereby ensuring the trajectory accuracy during operation.

[0202] When the end of the pipe laying machine approaches the target position, the PLC controller uses the remaining distance between the current position of the end of the pipe laying machine and the target position as the control input. Through the PID control algorithm, the target running linear speed of the end of the pipe laying machine is adjusted, so that the end of the pipe laying machine gradually decelerates, and finally achieves accurate positioning that meets the preset positioning accuracy requirements.

[0203] The above description is not intended to limit the present invention in any way. Although the present invention has been disclosed above through embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A motion control method for a telescopic rotary pipe laying machine, characterized in that, Includes the following steps: S1. Establish the coordinate system and kinematic relationships for the end motion of the pipe laying machine; S2. Based on the coordinate system, the position of the end of the pipe laying machine is calibrated and the target movement path is determined; S3. Prepare for startup based on the target motion path and calculate initial motion parameters; S4. Execute the end motion of the pipe laying machine according to the initial motion parameters and obtain the current position; S5. Determine the deviation between the current position and the target's movement path and dynamically adjust the movement path accordingly; S6. Adjust the speed of the pipe laying machine based on the deviation judgment result to achieve positioning.

2. The method according to claim 1, characterized in that, S1 includes the following steps: S1.

1. Establish a rectangular coordinate system with the center of the pipe laying machine base as the origin; S1.

2. Establish a polar coordinate system based on a rectangular coordinate system; S1.

3. Based on the mechanical transmission relationship of the pipe laying machine, establish the mathematical relationship between the end position of the pipe laying machine and the extension and rotation angle; S1.

4. Establish the kinematic relationship between the end-effector speed of the pipe laying machine and its extension and rotation speed.

3. The method according to claim 2, characterized in that, S2 includes the following steps: S2.

1. Select the key feature position of the end of the pipe laying machine on the target movement path as the calibration position; S2.

2. Determine the coordinate data of each calibration position in the coordinate system; S2.

3. Based on the coordinate data of the calibrated position, determine the target movement path of the end of the pipe laying machine.

4. The method according to claim 3, characterized in that, The key feature locations include at least the starting position, target position, and turning points of the pipe laying machine end on the target movement path.

5. The method according to claim 4, characterized in that, S3 includes the following steps: S3.

1. Determine whether the initial position of the pipe laying machine's end is located on the target movement path; S3.

2. Based on the judgment results, plan the transition path that cuts into the target movement path from the initial position at the end of the pipe laying machine; S3.

3. Calculate the initial motion parameters based on the target motion path, the initial position of the pipe laying machine end, and the preset motion speed.

6. The method according to claim 5, characterized in that, S4 includes the following steps: S4.

1. Based on the initial motion parameters, control the end of the pipe laying machine to move along the target motion path; S4.

2. During the movement, the current position of the pipe laying machine end is obtained in real time; S4.

3. Based on the current position, represent the position of the end of the pipe laying machine as coordinate values ​​in the coordinate system.

7. The method according to claim 6, characterized in that, S5 includes the following steps: S5.

1. Compare the current position of the pipe laying machine end with the target movement path and calculate the movement deviation; S5.

2. Determine whether the motion deviation exceeds the preset allowable range; S5.

3. Based on the judgment result, the current position, and the target position, dynamically adjust the remaining segments of the target's movement path; S5.

4. Based on the adjusted target motion path, recalculate the corresponding motion parameters.

8. The method according to claim 7, characterized in that, S6 includes the following steps: S6.

1. The remaining distance between the current position and the target position of the pipe laying machine end is used as the control input. S6.

2. Adjust the movement speed of the end of the pipe laying machine according to the remaining distance; S6.

3. When the remaining distance is less than or equal to the preset positioning accuracy, it is determined that the end of the pipe laying machine has completed positioning.

9. The method according to claim 8, characterized in that, When adjusting the movement speed based on the remaining distance, a PID control algorithm is used, with the remaining distance between the current position and the target position of the pipe laying machine as the input, to dynamically adjust the linear speed of the end movement.

10. A motion control system for a telescopic rotary pipe laying machine, using the method described in any one of claims 1-9, characterized in that, include: Control unit, human-machine interface unit, position detection unit, and drive execution unit; The control unit is used to establish the coordinate system and kinematic relationship of the end motion of the pipe laying machine, determine the target motion path of the end motion of the pipe laying machine based on the calibration results, calculate the motion parameters corresponding to the target motion path, and dynamically adjust the target motion path according to the current position of the end motion of the pipe laying machine. The human-machine interface unit is electrically connected to the control unit and is used to set the operating functions and motion parameters of the pipe laying machine, and to send the operating functions and motion parameters to the control unit. The position detection unit is electrically connected to the control unit and is used to collect the extension and rotation angle of the pipe laying machine and send data to the control unit to determine the current position of the end of the pipe laying machine. The drive execution unit is electrically connected to the control unit and is used to receive motion parameters sent by the control unit, and drive the telescopic and rotating mechanisms of the pipe laying machine to move the end of the pipe laying machine along the target motion path.

Images