A tubing handling robot joint trajectory planning method

Abstract

The present application belongs to the technical field of oil pipe carrying robot joint trajectory planning, and particularly relates to an oil pipe carrying robot joint trajectory planning method. According to the task requirement, the oil pipe carrying robot joint trajectory planning method can solve the displacement, speed and acceleration of each joint movement in real time, thereby providing help for generating the joint trajectory. The oil pipe carrying robot joint trajectory planning method comprises the following steps: step 1: planning the space path of the oil pipe carrying robot joint trajectory, and calculating the space path interpolation of the oil pipe carrying robot joint trajectory; step 2: setting the objective function and constraint function of the oil pipe carrying robot joint trajectory; step 3: constructing a fusion algorithm for adjusting the oil pipe carrying robot joint trajectory planning; and step 4: based on the fusion algorithm obtained in step 3, the optimal solution of the oil pipe carrying robot joint trajectory is calculated, and the planning of the oil pipe carrying robot joint trajectory is completed.

Description

Technical Field

[0001] This invention belongs to the field of joint trajectory planning technology for tubing handling robots, and particularly relates to a method for joint trajectory planning of tubing handling robots. Background Technology

[0002] Oil pipe handling robots are widely used in oilfield pipe processing, repair, painting, and inspection. However, due to the long length and heavy weight of oil pipes, and the numerous joints in the robot's linkage mechanism, the planning of the robot's joint trajectories directly affects the quality and efficiency of task completion. Furthermore, redundant joint trajectories can unnecessarily increase the robot's energy consumption and reduce its effectiveness.

[0003] Further research revealed that, due to the direct and important relationship between trajectory planning and joint motion, and the influence of various geometric and non-geometric errors, the spatial correlation between joints is highly dynamic. Therefore, it is necessary to establish a reliable and reasonable joint trajectory planning method to improve the operating efficiency of the handling robot in each process and reduce the operating energy consumption of the handling robot. Summary of the Invention

[0004] This invention provides a joint trajectory planning method for an oil pipe handling robot. This method can calculate the displacement, velocity, and acceleration of each joint in real time according to task requirements, thereby aiding in the generation of joint trajectories. Using this method for real-time joint trajectory planning can effectively improve the motion efficiency of robot joints, reduce motion energy consumption, and ultimately optimize the robot's joint trajectories according to various needs.

[0005] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0006] A method for joint trajectory planning of an oil pipe handling robot includes the following steps:

[0007] Step 1: Plan the spatial path of the joint trajectory of the tubing handling robot and calculate the spatial path interpolation of the joint trajectory of the tubing handling robot;

[0008] Step 2: Define the objective function and constraint function for the joint trajectory of the tubing handling robot;

[0009] Step 3: Construct a fusion algorithm for adjusting the joint trajectory planning of the tubing handling robot;

[0010] Step 4: Based on the fusion algorithm obtained in Step 3, calculate the optimal solution for the joint trajectory of the tubing transport robot, and complete the planning of the joint trajectory of the tubing transport robot.

[0011] Preferably, step 1 can be specifically described as follows:

[0012] A cubic uniform B-spline curve is used to perform interpolation calculation of the spatial path of the joint trajectory of the tubing handling robot.

[0013] The mathematical expression for interpolation calculation is as follows:

[0014] (1);

[0015] In formula (1), For spline curves corresponding to parameters The The vector values ​​of the segment points u Î[0, 1]; These are the control points for the spline curve;

[0016] It is a polynomial, specifically described as:

[0017] (2);

[0018] Rewriting equation (2) in matrix form, we get:

[0019] (3);

[0020] To ensure the continuity of the B-spline trajectory, let the end vector of the previous segment of the trajectory be equal to the beginning vector of the next segment, then we can obtain:

[0021] (4);

[0022] in, These are the shape points on the trajectory;

[0023] Depend on Individual value point determination An equation is given, and boundary conditions are added to solve for all unknowns; the added boundary conditions satisfy:

[0024] (5);

[0025] Based on equation (5) The resulting B-spline curve satisfies:

[0026] (6);

[0027] Assume the control points on the B-spline curve are Then the B-spline curve on the th Joint angle changes of the tubing handling robot with segmented trajectory and changes over time They respectively satisfy:

[0028] (7).

[0029] Preferably, step 2 can be specifically described as follows:

[0030] The path of the joint trajectory of the tubing handling robot is optimized with the objective function of minimizing the joint movement time.

[0031] The mathematical model of the objective function can be expressed as:

[0032] (8);

[0033] Among them, h i T represents the running time of each segment of the tubing transport robot's joint trajectory; T represents the total time of the entire segment of the tubing transport robot's joint trajectory.

[0034] The following constraints are imposed on the speed of the joint trajectory of the tubing handling robot:

[0035] Differentiating the B-spline curve yields the velocity expression for the joints of the tubing handling robot. :

[0036] (9);

[0037] Its formula (9) can also be expressed as:

[0038] (10);

[0039] The acceleration of the joint trajectory of the tubing handling robot is constrained as follows:

[0040] The velocity expression for the joint of the tubing handling robot shown in equation (9) Differentiating the expression yields the acceleration expression for the joints of the tubing handling robot. :

[0041] (11);

[0042] Its formula (11) can also be expressed as:

[0043] (12);

[0044] Expression of joint velocity of integrated oil pipe handling robot Acceleration expression for the joints of the tubing handling robot The resulting constraint function can be expressed as:

[0045] (13).

[0046] Preferably, step 3 can be specifically described as follows:

[0047] Based on the predation mechanism of the WOA algorithm, combined with the parameters in the SMA algorithm and the fitness weights representing each slime mold individual, a fusion algorithm for adjusting the joint trajectory planning of the tubing transport robot is constructed.

[0048] The predation mechanism of the WOA algorithm includes three stages: surrounding the prey, dynamic spiral predation, and global exploration.

[0049] Preferably, the process of surrounding the prey in the WOA algorithm can be specifically described as follows:

[0050] Assume the population size involved in predation is N The dimensions of the explored space are d In the t-th iteration, the i-th individual is in the t-th iteration. d A position in 3D space can be represented as:

[0051] (14);

[0052] The best position to date is represented as the prey position, and is defined as follows:

[0053] (15);

[0054] As the number of iterations increases, the optimal position is gradually updated and replaced; because the initial population has no prior experience, whales initially choose random individual positions as their targets, and their process of searching for surrounding prey can be represented as:

[0055] (16);

[0056] in, D Let the distance between the current individual and the current best individual be the encirclement step size, which satisfies:

[0057] (17);

[0058] in, r rand A random vector in [0,1]; in equation (16) A Let be a coefficient vector, satisfying:

[0059] (18);

[0060] To adapt to complex nonlinear search processes, a Defined as:

[0061] (19).

[0062] Preferably, the dynamic spiral predation process in the WOA algorithm can be specifically described as follows:

[0063] When an individual swims around its prey, it makes a spiral motion; dynamic parameters are introduced simultaneously. v b and dynamic weights ω Based on the number of iterations, the predator's range of motion is dynamically updated, and the predator's range and speed are adaptively adjusted.

[0064] The mathematical model of this process can be represented as:

[0065] (20);

[0066] in, v b for[- a , a The vector parameter ] is b, which is a constant defining the shape of the logarithmic spiral. l A random number between [-1, 1];

[0067] D’ For the first t The distance between the current individual and the best individual in the next iteration satisfies:

[0068] (twenty one);

[0069] ω The fitness weights characterize each slime mold individual, adaptively adjusting the distance between the current individual and the optimal individual to accelerate convergence; among them, ω The value can be represented as follows:

[0070] (twenty two);

[0071] Where Firsthalf means F i The first half of the species, bF This represents the optimal fitness obtained in the current iteration. wF The sort function represents the worst fitness value obtained in the current iteration. F i ) represents the fitness sequence. r It is a random value in the range [0,1].

[0072] Preferably, the global exploration phase of the WOA algorithm can be specifically described as follows:

[0073] The global exploration phase simulates the process by which individuals randomly search for food based on each other's locations. The mathematical model for this process can be represented as:

[0074] (twenty three);

[0075] in, X t r For the randomly selected location of an individual whale, the parameter vector A Consistent with the previous settings; D’’ For the first t The current individual in the next iteration X it With random individuals X t r The distance between them satisfies:

[0076] (twenty four).

[0077] This invention provides a method for planning the joint trajectory of an oil pipe handling robot. The method specifically includes the following steps: Step 1: Planning the spatial path of the oil pipe handling robot's joint trajectory and calculating the spatial path interpolation; Step 2: Setting the objective function and constraint function of the oil pipe handling robot's joint trajectory; Step 3: Constructing a fusion algorithm for adjusting the planning of the oil pipe handling robot's joint trajectory; Step 4: Based on the fusion algorithm obtained in Step 3, calculating the optimal solution of the oil pipe handling robot's joint trajectory, thus completing the planning of the oil pipe handling robot's joint trajectory. This method, with the above-mentioned steps, plans the joint trajectory of the oil pipe handling robot and, according to task requirements, solves in real-time the displacement, velocity, and acceleration of each joint movement, thereby generating a specific joint trajectory for the oil pipe handling robot. Furthermore, this method can be used for real-time planning of the oil pipe handling robot's joint trajectory, improving the multi-motion efficiency of the oil pipe handling robot's joint trajectory, reducing motion energy consumption, and ultimately achieving the goal of optimizing the joint trajectory of the oil pipe handling robot according to various needs. Attached Figure Description

[0078] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the embodiments of the invention to explain the invention and do not constitute a limitation thereof.

[0079] Figure 1 A flowchart illustrating a joint trajectory planning method for an oil pipe handling robot provided by the present invention;

[0080] Figure 2 This is a schematic diagram of the fusion algorithm for adjusting the joint trajectory planning of the tubing handling robot, which is constructed in step 3. Detailed Implementation

[0081] This invention provides a joint trajectory planning method for an oil pipe handling robot. This method can calculate the displacement, velocity, and acceleration of each joint in real time according to task requirements, thereby aiding in the generation of joint trajectories. Using this method for real-time joint trajectory planning can effectively improve the motion efficiency of robot joints, reduce motion energy consumption, and ultimately optimize the robot's joint trajectories according to various needs.

[0082] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0083] A method for joint trajectory planning of an oil pipe handling robot, such as Figure 1 As shown, it includes the following steps:

[0084] Step 1: Plan the spatial path of the joint trajectory of the tubing handling robot and calculate the spatial path interpolation of the joint trajectory of the tubing handling robot.

[0085] It should be further explained that the spatial path planning for the joint trajectory of the tubing handling robot specifically includes the following: given the positions of the starting point, ending point, or intermediate point, determining constraints such as velocity and acceleration, and generating the motion curves of each joint of the tubing handling robot. This process employs multi-node planning.

[0086] In a preferred embodiment of the present invention, step 1 can be specifically described as follows:

[0087] A cubic uniform B-spline curve is used to perform interpolation calculation of the spatial path of the joint trajectory of the tubing handling robot.

[0088] The mathematical expression for interpolation calculation is as follows:

[0089] (1);

[0090] In formula (1), For spline curves corresponding to parameters The The vector values ​​of the segment points u Î[0, 1]; These are the control points for the spline curve;

[0091] It is a polynomial, specifically described as:

[0092] (2);

[0093] Rewriting equation (2) in matrix form, we get:

[0094] (3);

[0095] To ensure the continuity of the B-spline trajectory, let the end vector of the previous segment of the trajectory be equal to the beginning vector of the next segment, then we can obtain:

[0096] (4);

[0097] in, These are the shape points on the trajectory;

[0098] Depend on Individual value point determination An equation is given, and boundary conditions are added to solve for all unknowns; the added boundary conditions satisfy:

[0099] (5);

[0100] Based on equation (5) The resulting B-spline curve satisfies:

[0101] (6);

[0102] Assume the control points on the B-spline curve are Then the B-spline curve on the th Joint angle changes of the tubing handling robot with segmented trajectory and changes over time They respectively satisfy:

[0103] (7).

[0104] After completing step 1, proceed to step 2: set the objective function and constraint function for the joint trajectory of the tubing transport robot.

[0105] Specifically, time optimization is used as the objective function for the joint trajectory of the tubing transport robot; where time optimization can also be expressed as aiming to minimize the joint movement time of the tubing transport robot.

[0106] In a preferred embodiment of the present invention, step 2 can be specifically described as follows:

[0107] The path of the joint trajectory of the tubing handling robot is optimized with the objective function of minimizing the joint movement time.

[0108] The mathematical model of the objective function can be expressed as:

[0109] (8);

[0110] Among them, h i T represents the running time of each segment of the tubing transport robot's joint trajectory; T represents the total time of the entire segment of the tubing transport robot's joint trajectory.

[0111] The following constraints are imposed on the speed of the joint trajectory of the tubing handling robot:

[0112] Differentiating the B-spline curve yields the velocity expression for the joints of the tubing handling robot. :

[0113] (9);

[0114] Its formula (9) can also be expressed as:

[0115] (10);

[0116] The acceleration of the joint trajectory of the tubing handling robot is constrained as follows:

[0117] The velocity expression for the joint of the tubing handling robot shown in equation (9) Differentiating the expression yields the acceleration expression for the joints of the tubing handling robot. :

[0118] (11);

[0119] Its formula (11) can also be expressed as:

[0120] (12);

[0121] Expression of joint velocity of integrated oil pipe handling robot Acceleration expression for the joints of the tubing handling robot The resulting constraint function can be expressed as:

[0122] (13).

[0123] After completing step 2, proceed to step 3: construct a fusion algorithm for adjusting the joint trajectory planning of the tubing handling robot.

[0124] This invention specifically combines the advantages of two optimization algorithms: the whale optimization algorithm (WOA) and the slime mold algorithm (SMA). Specifically, by adjusting parameters and introducing weights, the mechanism of SMA and the WOA algorithm are integrated to create a novel joint trajectory planning algorithm (SMWOA) for tubing transport robots, aiming to enhance global optimization capabilities.

[0125] It should be noted that step 3 above can be specifically described as follows:

[0126] Based on the predation mechanism of the WOA algorithm, combined with the parameters in the SMA algorithm and the fitness weights representing each slime mold individual, a fusion algorithm for adjusting the joint trajectory planning of the tubing handling robot is constructed.

[0127] The predation mechanism of the WOA algorithm includes three stages: surrounding the prey, dynamic spiral predation, and global exploration.

[0128] The process of surrounding the prey in the WOA algorithm can be specifically described as follows:

[0129] Assume the population size involved in predation is N The dimensions of the explored space are d In the t-th iteration, the i-th individual is in the t-th iteration. d A position in 3D space can be represented as:

[0130] (14);

[0131] The best position to date is represented as the prey position, and is defined as follows:

[0132] (15);

[0133] As the number of iterations increases, the optimal position is gradually updated and replaced; because the initial population has no prior experience, whales initially choose random individual positions as their targets, and their process of searching for surrounding prey can be represented as:

[0134] (16);

[0135] in, D Let the distance between the current individual and the current best individual be the encirclement step size, which satisfies:

[0136] (17);

[0137] in, rrand A random vector in [0,1]; in equation (16) A Let be a coefficient vector, satisfying:

[0138] (18);

[0139] To adapt to complex nonlinear search processes, a Defined as:

[0140] (19).

[0141] The dynamic spiral predation process in the WOA algorithm can be specifically described as follows:

[0142] When an individual swims around its prey, it makes a spiral motion; dynamic parameters are introduced simultaneously. v b and dynamic weights ω Based on the number of iterations, the predator's range of motion is dynamically updated, and the predator's range and speed are adaptively adjusted.

[0143] The mathematical model of this process can be represented as:

[0144] (20);

[0145] in, v b for[- a , a The vector parameter ] is b, which is a constant defining the shape of the logarithmic spiral. l A random number between [-1, 1];

[0146] D’ For the first t The distance between the current individual and the best individual in the next iteration satisfies:

[0147] (twenty one);

[0148] ω The fitness weights characterize each slime mold individual, adaptively adjusting the distance between the current individual and the optimal individual to accelerate convergence; among them, ω The value can be represented as follows:

[0149] (twenty two);

[0150] Where Firsthalf means F i The first half of the species, bF This represents the optimal fitness obtained in the current iteration. wFThe sort function represents the worst fitness value obtained in the current iteration. F i ) represents the fitness sequence. r It is a random value in the range [0,1].

[0151] The global exploration phase of the WOA algorithm can be specifically described as follows:

[0152] The global exploration phase simulates the process by which individuals randomly search for food based on each other's locations. The mathematical model for this process can be represented as:

[0153] (twenty three);

[0154] in, X t r For the randomly selected location of an individual whale, the parameter vector A Consistent with the previous settings; D’’ For the first t The current individual in the next iteration X it With random individuals X t r The distance between them satisfies:

[0155] (twenty four).

[0156] One point that needs further explanation is that the prey-encircling and dynamic spiral predation phases belong to the development phase of the metaheuristic optimization algorithm. The selection probabilities of the two mechanisms are set to be equal, each accounting for 50%, determined by the parameters. P Confirmed. If P ≥ 0.5, then it directly enters the dynamic spiral predation mode; if P < 0.5, then when | A When |<1, individuals tend to surround their prey; when | A When |≥1, the individual selects a global random exploration mechanism to search for a better solution. The specific implementation process of the fusion algorithm can be found in [reference needed]. Figure 2 As shown.

[0157] like Figure 2 As shown, firstly, the population-related parameters and variables for the two algorithms are defined, and an initial population is created. Then, the fitness values ​​of all individuals in the population are calculated and evaluated to determine the best and worst individuals. Parameters are updated according to the corresponding formulas. When P ≥ 0.5, the position is updated using formula (20). When P < 0.5, and | A When |<1, use formula (16) for position update; when P<0.5, and | A When |≥1, the position is updated using formula (23).

[0158] After completing step 3, proceed to step 4: based on the fusion algorithm obtained in step 3, calculate the optimal solution for the joint trajectory of the tubing transport robot, and complete the planning of the joint trajectory of the tubing transport robot.

[0159] The working process of the joint trajectory planning method for an oil pipe handling robot provided by this invention can be summarized as follows: First, based on the workspace, task requirements, and obstacle information of the oil pipe handling robot's robotic arm, several pose points that the oil pipe handling robot joints must pass through in Cartesian space during task execution are determined. Then, through inverse kinematics, the corresponding joint space point positions of the oil pipe handling robot are obtained, transforming the trajectory in the task space into the joint space. An interpolation function is used to interpolate the angles of each joint in the joint space. With time as the objective, an optimization objective function is determined. For each joint, its angular velocity, angular acceleration, and impact constraints are set and input into the aforementioned fusion algorithm to solve for the optimal objective function. Finally, trajectory planning is performed based on the optimal solution obtained by the fusion algorithm.

[0160] This invention provides a method for planning the joint trajectory of an oil pipe handling robot. The method specifically includes the following steps: Step 1: Planning the spatial path of the oil pipe handling robot's joint trajectory and calculating the spatial path interpolation; Step 2: Setting the objective function and constraint function of the oil pipe handling robot's joint trajectory; Step 3: Constructing a fusion algorithm for adjusting the planning of the oil pipe handling robot's joint trajectory; Step 4: Based on the fusion algorithm obtained in Step 3, calculating the optimal solution of the oil pipe handling robot's joint trajectory, thus completing the planning of the oil pipe handling robot's joint trajectory. This method, with the above-mentioned steps, plans the joint trajectory of the oil pipe handling robot and, according to task requirements, solves in real-time the displacement, velocity, and acceleration of each joint movement, thereby generating a specific joint trajectory for the oil pipe handling robot. Furthermore, this method can be used for real-time planning of the oil pipe handling robot's joint trajectory, improving the multi-motion efficiency of the oil pipe handling robot's joint trajectory, reducing motion energy consumption, and ultimately achieving the goal of optimizing the joint trajectory of the oil pipe handling robot according to various needs.

[0161] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for joint trajectory planning of an oil pipe handling robot, characterized in that, The steps include the following: Step 1: Plan the spatial path of the joint trajectory of the tubing handling robot and calculate the spatial path interpolation of the joint trajectory of the tubing handling robot; Step 2: Define the objective function and constraint function for the joint trajectory of the tubing handling robot; Step 3: Construct a fusion algorithm for adjusting the joint trajectory planning of the tubing handling robot; Step 4: Based on the fusion algorithm obtained in Step 3, calculate the optimal solution for the joint trajectory of the tubing handling robot and complete the planning of the joint trajectory of the tubing handling robot; Step 3 can be specifically described as follows: Based on the predation mechanism of the WOA algorithm, combined with the parameters in the SMA algorithm and the fitness weights representing each slime mold individual, a fusion algorithm for adjusting the joint trajectory planning of the tubing transport robot is constructed. The predation mechanism of the WOA algorithm includes three stages: surrounding the prey, dynamic spiral predation, and global exploration. The process of surrounding prey in the WOA algorithm can be specifically described as follows: Assume the population size involved in predation is N The dimensions of the explored space are d In the t-th iteration, the i-th individual is in the t-th iteration. d A position in 3D space can be represented as: （14）； The best position to date is represented as the prey position, and is defined as follows: （15）； As the number of iterations increases, the optimal position is gradually updated and replaced; because the initial population has no prior experience, whales initially choose random individual positions as their targets, and their process of searching for surrounding prey can be represented as: （16）； in, D Let the distance between the current individual and the current best individual be the encirclement step size, which satisfies: （17）； in, r rand A random vector in [0,1]; in equation (16) A Let be a coefficient vector, satisfying: （18）； To adapt to complex nonlinear search processes, a Defined as: （19）； The dynamic spiral predation process in the WOA algorithm can be specifically described as follows: When an individual swims around its prey, it makes a spiral motion; dynamic parameters are introduced simultaneously. v b and dynamic weights ω Based on the number of iterations, the predator's range of motion is dynamically updated, and the predator's range and speed are adaptively adjusted. The mathematical model of this process can be represented as: （20）； in, v b for[- a , a The vector parameter ] is b, which is a constant defining the shape of the logarithmic spiral. l A random number between [-1, 1]; D’ For the first t The distance between the current individual and the best individual in the next iteration satisfies: （21）； ω The fitness weights characterize each slime mold individual, adaptively adjusting the distance between the current individual and the optimal individual to accelerate convergence; among them, ω The value can be represented as follows: （22）； Where Firsthalf means F i The first half of the species, bF This represents the optimal fitness obtained in the current iteration. wF The sort function represents the worst fitness value obtained in the current iteration. F i ) represents the fitness sequence. r A random value in the range [0,1]. The process of the global exploration phase in the WOA algorithm can be specifically described as follows: The global exploration phase simulates the process by which individuals randomly search for food based on each other's locations. The mathematical model for this process can be represented as: （23）； in, X t r For the randomly selected location of an individual whale, the parameter vector A Consistent with the previous settings; D’’ For the first t The current individual in the next iteration X it With random individuals X t r The distance between them satisfies: （24）。 2. The method for joint trajectory planning of an oil pipe handling robot according to claim 1, characterized in that, Step 1 can be specifically described as follows: A cubic uniform B-spline curve is used to perform interpolation calculation of the spatial path of the joint trajectory of the tubing handling robot. The mathematical expression for interpolation calculation is as follows: （1）； In formula (1), For spline curves corresponding to parameters The The vector values ​​of the segment points u Î[0, 1]; These are the control points for the spline curve; It is a polynomial, specifically described as: （2）； Rewriting equation (2) in matrix form, we get: （3）； To ensure the continuity of the B-spline trajectory, let the end vector of the previous segment of the trajectory be equal to the beginning vector of the next segment, then we can obtain: （4）； in, These are the shape points on the trajectory; Depend on Individual value point determination An equation is given, and boundary conditions are added to solve for all unknowns; the added boundary conditions satisfy: （5）； Based on equation (5) The resulting B-spline curve satisfies: （6）； Assume the control points on the B-spline curve are Then the B-spline curve on the th Joint angle changes of the tubing handling robot with segmented trajectory and changes over time They respectively satisfy: （7）。 3. The method for joint trajectory planning of an oil pipe handling robot according to claim 1, characterized in that, Step 2 can be specifically described as follows: The path of the joint trajectory of the tubing handling robot is optimized with the objective function of minimizing the joint movement time. The mathematical model of the objective function can be expressed as: （8）； Among them, h i T represents the running time of each segment of the tubing transport robot's joint trajectory; T represents the total time of the entire segment of the tubing transport robot's joint trajectory. The following constraints are imposed on the speed of the joint trajectory of the tubing handling robot: Differentiating the B-spline curve yields the velocity expression for the joints of the tubing handling robot. : （9）； Its formula (9) can also be expressed as: （10）； The acceleration of the joint trajectory of the tubing handling robot is constrained as follows: The velocity expression for the joint of the tubing handling robot shown in equation (9) Differentiating the expression yields the acceleration expression for the joints of the tubing handling robot. : （11）； Its formula (11) can also be expressed as: （12）； Expression of joint velocity of integrated oil pipe handling robot Acceleration expression for the joints of the tubing handling robot The resulting constraint function can be expressed as: （13）。

Images