Obstacle avoidance trajectory planning method for adding and removing drill rod manipulator under variable target conditions

Abstract

The application relates to a kind of obstacle avoidance trajectory planning methods of adding and removing drill rod manipulator under variable target conditions, the method is divided into three sections to plan, and the three section trajectories obtained are connected, to form the final trajectory;The final trajectory is a collision-free trajectory, and the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the manipulator in the trajectory are good in continuity, the motion impact is small, and the path is shorter.

Description

Technical Field

[0001] This application relates to the field of drill pipe manipulator motion trajectory planning technology, specifically, to a method for obstacle avoidance trajectory planning of a drill pipe loading and unloading manipulator under varying target conditions. Background Technology

[0002] The drill pipe loading and unloading robot is used for loading and unloading drill pipes. The robot employs a tandem robotic arm mechanism, comprising six robotic arms and a gripper mounted at the end of the sixth arm for grasping and transporting the drill pipes. Each robotic arm is connected by six rotary joints, possessing six degrees of freedom. The rotation of each joint is driven by six explosion-proof servo motors, and the gripping and releasing of the gripper is driven by a multi-way directional valve in the drilling robot's hydraulic system.

[0003] The goal of adding drill rods is to use a robotic arm to grab a drill rod from the drill rod magazine and transport it to the drill rod addition / removal position on the main unit, and vice versa for removing drill rods. During the addition / removal trajectory, the robotic arm and the drill rod in its gripper will encounter various obstacles. Because the posture of the main unit of the drilling equipment changes, the robotic arm's movement trajectory must be replanned for different main unit postures to avoid collisions. However, existing methods for planning the trajectory of addition / removal robotic arms use a teach pendant or remote controller for teaching. During the teaching process, the trajectory is adjusted to avoid obstacles, which cannot accurately achieve the goal of obstacle avoidance. Summary of the Invention

[0004] To overcome at least one deficiency in the prior art, this application provides a method for obstacle avoidance trajectory planning for a drill pipe loading and unloading robot under variable target conditions.

[0005] Firstly, a method for obstacle avoidance trajectory planning for a drill pipe loading / unloading robot under varying target conditions is provided, including:

[0006] A parametric model for loading and unloading drill pipes by a robotic arm is constructed. The parametric model for loading and unloading drill pipes by a robotic arm includes the joint motion model of the robotic arm, the joint motion model of the host machine, and the drill pipe chamber model.

[0007] In the parametric model of the robotic arm loading and unloading drill pipe, the starting point and end point of the first trajectory are set, and a straight trajectory is adopted. Based on the starting point and end point of the first trajectory, a minimum impact algorithm based on seventh-order polynomial interpolation is used in Cartesian space to determine multiple interpolation points of the first trajectory, thereby determining the first trajectory; each interpolation point includes coordinate information and attitude information.

[0008] Based on multiple interpolation points of the first trajectory, the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the manipulator corresponding to each interpolation point in the first trajectory are obtained;

[0009] Determine the start point, end point, and transition point of the second trajectory; the start point, end point, and transition point include coordinate information and attitude information; based on the joint angles of each joint of the manipulator corresponding to the start point, end point, and transition point of the second trajectory, determine the second trajectory in joint space based on the impact optimization algorithm, as well as the joint angles of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory.

[0010] Based on the joint angles and running times of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory, the angular velocity, angular acceleration, and angular jerk of each joint of the manipulator corresponding to each interpolation point of the second trajectory are determined by the derivative method.

[0011] The starting point and ending point of the third trajectory are determined by using a straight-line trajectory. Based on the starting point and ending point of the third trajectory, a minimum impact algorithm based on seventh-order polynomial interpolation is used in Cartesian space to determine multiple interpolation points of the third trajectory, thereby determining the third trajectory. Each interpolation point includes coordinate information and attitude information.

[0012] Based on multiple interpolation points of the third trajectory, the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the manipulator corresponding to each interpolation point in the third trajectory are obtained;

[0013] Connect the beginning and end of the first, second, and third trajectory segments to obtain the final trajectory. Then, obtain the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the robot corresponding to each path point in the final trajectory. Each path point includes multiple interpolation points of the first, second, and third trajectory segments.

[0014] In one embodiment, the joint angles, angular velocities, angular accelerations, and angular jerks of each joint of the manipulator corresponding to each interpolation point in the first trajectory are obtained based on multiple interpolation points of the first trajectory, including:

[0015] Each interpolation point determines a homogeneous transformation matrix;

[0016] Based on the homogeneous transformation matrix, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to each interpolation point.

[0017] The angular velocity, angular acceleration, and angular jerk of each joint of the robot are obtained by using the derivative method to calculate the joint angles and running times of each joint.

[0018] In one embodiment, based on the joint angles of each joint of the manipulator corresponding to the start point, end point, and transition point of the second trajectory, the second trajectory is determined in joint space using an impact optimization algorithm, along with the joint angles of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory, including:

[0019] Step S41: Set a transition point as the current transition point. Determine the homogeneous transformation matrix corresponding to the three points based on the starting point, ending point and current transition point of the second trajectory.

[0020] Step S42: Based on the homogeneous transformation matrices corresponding to the three points, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to the three points.

[0021] Step S43: For the joint angles of the manipulator corresponding to the three points, an improved dual-tree RRT obstacle avoidance path planning algorithm is used to plan the trajectory to obtain a preliminary trajectory; a cubic B-spline curve path optimization algorithm is used to smooth the preliminary trajectory to obtain a processed trajectory; the processed trajectory is optimized and envelope collision detection is performed using a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation to obtain an optimized trajectory, as well as the joint angles of the manipulator corresponding to the multiple interpolation points of the optimized trajectory.

[0022] Step S44: If no collision occurs between envelopes in the envelope collision detection, the optimized trajectory, i.e., the second segment trajectory, and the joint angles of each joint of the robot corresponding to multiple interpolation points of the second segment trajectory are output; if a collision occurs between envelopes in the envelope collision detection, the process returns to step S41 and the transition point is reset.

[0023] In one embodiment, the joint angles, angular velocities, angular accelerations, and angular jerks of each joint of the manipulator corresponding to each interpolation point in the third trajectory are obtained based on multiple interpolation points of the third trajectory, including:

[0024] Each interpolation point determines a homogeneous transformation matrix;

[0025] Based on the homogeneous transformation matrix, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to each interpolation point.

[0026] The angular velocity, angular acceleration, and angular jerk of each joint of the robot are obtained by using the derivative method to calculate the joint angles and running times of each joint.

[0027] In one embodiment, the transition point is a point between the two upper stabilizing devices.

[0028] In one embodiment, the starting point of the second trajectory is the ending point of the first trajectory, the ending point of the second trajectory is a point at a fixed distance from the center point of the drill pipe loading / unloading position on the host, and the line connecting the ending point of the second trajectory and the center point of the drill pipe loading / unloading position is perpendicular to the guide rail set on the host.

[0029] In one embodiment, both the first and third trajectory segments are straight-line trajectories.

[0030] Secondly, a device for planning obstacle avoidance trajectories for a drill pipe loading / unloading robot under varying target conditions is provided, comprising:

[0031] The model building module is used to build a parametric model of the manipulator loading and unloading drill pipes. The parametric model of the manipulator loading and unloading drill pipes includes the joint motion model of the manipulator, the joint motion model of the host machine, and the drill pipe chamber model.

[0032] The first trajectory planning module is used to set the start and end points of the first trajectory in the parametric model of the robot arm loading and unloading drill pipe. It adopts a straight trajectory and, based on the start and end points of the first trajectory, uses a minimum impact algorithm based on seventh-order polynomial interpolation in Cartesian space to determine multiple interpolation points of the first trajectory, thereby determining the first trajectory. Each interpolation point includes coordinate information and attitude information.

[0033] The first module for determining the joint parameters of the robotic arm is used to obtain the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the robotic arm corresponding to each interpolation point in the first trajectory based on multiple interpolation points in the first trajectory.

[0034] The second trajectory planning module is used to determine the start point, end point, and transition point of the second trajectory. The start point, end point, and transition point include coordinate information and attitude information. Based on the joint angles of each joint of the manipulator corresponding to the start point, end point, and transition point of the second trajectory, the second trajectory is determined in joint space based on the impact optimization algorithm, as well as the joint angles of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory.

[0035] The second module for determining the joint parameters of the robot arm is used to determine the angular velocity, angular acceleration, and angular jerk of each joint of the robot arm corresponding to each interpolation point of the second trajectory using the derivative method, based on the joint angle and running time of each joint of the robot arm corresponding to multiple interpolation points of the second trajectory.

[0036] The third trajectory planning module is used to determine the start and end points of the third trajectory. It adopts a straight trajectory and, based on the start and end points of the third trajectory, uses a minimum impact algorithm based on seventh-order polynomial interpolation in Cartesian space to determine multiple interpolation points of the third trajectory, thereby determining the third trajectory. Each interpolation point includes coordinate information and attitude information.

[0037] The third module for determining the joint parameters of the robotic arm is used to obtain the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the robotic arm corresponding to each interpolation point in the third trajectory based on multiple interpolation points in the third trajectory.

[0038] The trajectory synthesis module is used to connect the beginning and end of the first, second, and third trajectories to obtain the final trajectory, and to obtain the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the robot corresponding to each path point in the final trajectory; each path point includes multiple interpolation points of the first, second, and third trajectories.

[0039] Thirdly, a computer-readable storage medium is provided, which stores a computer program. When the computer program is executed by a processor, it implements the above-mentioned obstacle avoidance trajectory planning method for a drill rod loading and unloading robot under varying target conditions.

[0040] Fourthly, a computer program product is provided, including a computer program / instruction, which, when executed by a processor, implements the aforementioned obstacle avoidance trajectory planning method for a drill rod loading / unloading robot under varying target conditions.

[0041] Compared with the prior art, this application has the following beneficial effects: This application divides the trajectory into three segments for planning, and the three segments are connected end to end to form the final trajectory; the final trajectory is a collision-free trajectory, and the joint angles, angular velocities, angular accelerations and angular jerks of each joint of the robot in the trajectory are continuous, the motion impact is small, and the path is short. Attached Figure Description

[0042] This application can be better understood by referring to the description given below in conjunction with the accompanying drawings, which, together with the detailed description below, are incorporated in and form part of this specification. In the drawings:

[0043] Figure 1 A flowchart illustrating the obstacle avoidance trajectory planning method for a drill pipe loading / unloading robot under varying target conditions according to an embodiment of this application is shown.

[0044] Figure 2 A schematic diagram of trajectory planning is shown;

[0045] Figure 3 A structural block diagram of an obstacle avoidance trajectory planning device for a drill rod loading / unloading robot under varying target conditions, according to an embodiment of this application, is shown.

[0046] Figure 4 The motion path of the robot arm planned by the minimum impact algorithm is shown;

[0047] Figure 5The simulation curves of the joint operation parameters of the robot obtained by the minimum impact algorithm are shown, where (a) is the curve of the joint angle change of each joint of the robot, (b) is the curve of the angular velocity change of each joint of the robot, (c) is the curve of the angular acceleration change of each joint of the robot, and (d) is the curve of the angular jerk change of each joint of the robot.

[0048] Figure 6 The motion path of the robotic arm obtained by the combined algorithm is shown.

[0049] Figure 7 The simulation curves of the joint operation parameters of the robot obtained by the combined algorithm are shown, where (a) is the curve of the joint angle change of each joint of the robot, (b) is the curve of the angular velocity change of each joint of the robot, (c) is the curve of the angular acceleration change of each joint of the robot, and (d) is the curve of the angular jerk change of each joint of the robot. Detailed Implementation

[0050] Exemplary embodiments of the present application will be described below with reference to the accompanying drawings. For clarity and brevity, not all features of the actual embodiments are described in the specification. However, it should be understood that many embodiment-specific decisions can be made in the development of any such actual embodiment to achieve the developer’s specific objectives, and these decisions may vary as the embodiments differ.

[0051] It should also be noted that, in order to avoid obscuring this application with unnecessary details, only the device structure closely related to the solution according to this application is shown in the accompanying drawings, while other details that are not closely related to this application are omitted.

[0052] It should be understood that this application is not limited to the described embodiments by virtue of the following description with reference to the accompanying drawings. In this document, embodiments may be combined with each other, features may be substituted or borrowed between different embodiments, and one or more features may be omitted in one embodiment, where feasible.

[0053] A drill rod loading / unloading robot is used to load and unload drill rods. Loading a drill rod involves using the robot to grab a specific drill rod from the drill rod magazine and transport it to the loading / unloading position on the main unit of the drilling equipment. Unloading a drill rod is the reverse process. The drilling equipment includes a three-degree-of-freedom main unit, comprising lifting height, tilt angle, and body translation. The attitude adjustment of the main unit's body is achieved through the combination of these three degrees of freedom to meet the drilling design requirements. Since each drilling design corresponds to a change in the attitude of the main unit's body, the trajectory of the robot transporting the drill rod also changes according to the change in the body. Therefore, the obstacle avoidance trajectory planning method of the drill rod loading / unloading robot in this embodiment is designed for changing target conditions.

[0054] During the loading and unloading of drill pipes, the robotic arm and gripper will encounter various obstacles. To effectively avoid collisions, this application provides a method for obstacle avoidance trajectory planning for a robotic arm loading and unloading drill pipes under varying target conditions. Figure 1 A flowchart illustrating the obstacle avoidance trajectory planning method for a drill pipe loading / unloading robot under varying target conditions according to an embodiment of this application is shown. See also... Figure 1 The methods include:

[0055] Step S1: Construct a parametric model for the robotic arm to load and unload drill pipes. The parametric model for the robotic arm to load and unload drill pipes includes the joint motion model of the robotic arm, the joint motion model of the host machine, and the drill pipe chamber model.

[0056] Here, the joint motion models of the robot arm, the host computer, and the drill pipe magazine are established using software. The robot arm and host computer joint motion models include models of each joint and the motion relationships between them. The drill pipe magazine model contains drill pipes. A coordinate system is established with the center of the robot arm's base as the origin. Based on the reference relationships of each model in the design parameters, the three established models are placed in the same coordinate system, forming a parametric model for the robot arm to add and unload drill pipes. The following steps are all based on this parametric model. In this embodiment, the goal of adding a drill pipe is to use the robot arm to grab a specific drill pipe from the drill pipe magazine and transport it to the drill pipe addition / unloading position on the host computer; the process of unloading the drill pipe is the reverse.

[0057] Step S2: In the parameterized model of the robot arm loading and unloading drill pipe, the starting point and end point of the first trajectory are set. A straight trajectory is adopted. Based on the starting point and end point of the first trajectory, a minimum impact algorithm based on seventh-order polynomial interpolation is used in Cartesian space to determine multiple interpolation points of the first trajectory, thereby determining the first trajectory. Each interpolation point includes coordinate information and attitude information.

[0058] Figure 2 A schematic diagram of trajectory planning is shown. See also Figure 2 The trajectory planning is divided into three segments. The first and third segments are straight lines, while the second segment must avoid obstacles. Here, the goal of the first segment of the robot arm's trajectory is to grab the drill rod from the drill rod magazine and move it directly above the magazine. Therefore, the starting point of the first segment can be set to the center point of the robot arm's end effector, i.e., a point on the drill rod axis; and the ending point can be set to a point at a fixed distance directly above the starting point in the drill rod magazine.

[0059] Preferably, the first trajectory segment can be determined using a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation, based on the starting and ending points of the first trajectory segment. In robot trajectory planning, different trajectories can be generated by using different polynomial interpolation functions. Polynomial interpolation trajectory planning mainly includes cubic, quintic, and seventh-order polynomial interpolation. Here, the minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation ensures the continuity of velocity, acceleration, and jerk, achieving minimum impact trajectory planning for the robot, which helps reduce the impact of the robot's high-speed movement during drill pipe loading and unloading.

[0060] Step S3: Based on multiple interpolation points of the first trajectory, obtain the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the robot corresponding to each interpolation point in the first trajectory.

[0061] Step S4: Determine the start point, end point, and transition point of the second trajectory; the start point, end point, and transition point include coordinate information and attitude information; based on the joint angles of each joint of the manipulator corresponding to the start point, end point, and transition point of the second trajectory, determine the second trajectory in joint space based on the impact optimization algorithm, as well as the joint angles of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory.

[0062] Here, the starting point of the second trajectory segment is the ending point of the first trajectory segment. The ending point of the second trajectory segment is a fixed distance from the center point of the drill pipe loading / unloading position on the main unit. The line connecting the ending point of the second trajectory segment and the center point of the drill pipe loading / unloading position is perpendicular to the guide rail installed on the main unit. It should be noted that the center point of the drill pipe loading / unloading position here is the center point of the drill pipe loading / unloading position after the main unit has deflected according to the set degrees of freedom.

[0063] The main unit comprises three degrees of freedom: lifting height, tilt angle, and body translation. The attitude of the main unit's body is adjusted through the combination of these three degrees of freedom to meet drilling design requirements. Since each drilling design corresponds to a change in the attitude of the main unit's body, the trajectory of the robotic arm transporting the drill rod also changes according to the change in body position. Let the lifting height, tilt angle, and body translation parameters of the main unit be H, α, and L, respectively. Let the Cartesian coordinates of the initial drill rod loading / unloading position center point be (x1, y1, z1). Then the coordinates of any drill rod loading / unloading position center point are (x2, y2, z2), which are the coordinates of the drill rod loading / unloading position center point after the main unit has deflected according to the set degrees of freedom. This can be obtained using the following formula:

[0064]

[0065] Specifically, see Figure 2 The transition point is a point between the two upper stabilizing devices.

[0066] Step S5: Based on the joint angles and running time of each joint of the robot corresponding to multiple interpolation points of the second trajectory, the angular velocity, angular acceleration, and angular jerk of each joint of the robot corresponding to each interpolation point of the second trajectory are determined by the derivative method; here, the running time is the running time of the second trajectory, which is a set value.

[0067] Step S6: Determine the start and end points of the third trajectory segment. Using a straight-line trajectory, based on the start and end points of the third trajectory segment, a minimum impact algorithm based on seventh-order polynomial interpolation is used in Cartesian space to determine multiple interpolation points of the third trajectory segment, thereby determining the third trajectory segment; each interpolation point includes coordinate information and attitude information.

[0068] Here, the starting point of the third trajectory is the ending point of the second trajectory, and the ending point of the third trajectory can be set as the drill rod placement point. Similar to the method used to determine the first trajectory, the third trajectory can be determined using a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation.

[0069] Step S7: Based on multiple interpolation points of the third trajectory, obtain the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the manipulator corresponding to each interpolation point in the third trajectory;

[0070] Step S8: Connect the beginning and end of the first, second, and third trajectory segments to obtain the final trajectory, and obtain the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the robot corresponding to each path point in the final trajectory; each path point includes multiple interpolation points of the first trajectory segment, multiple interpolation points of the second trajectory segment, and multiple interpolation points of the third trajectory segment.

[0071] In this embodiment, the trajectory is planned in three segments. The joint velocities of the robot are zero at the beginning and end of the three segments, but not zero at the transition points. The final trajectory obtained by connecting the three segments end to end is a collision-free trajectory. In this trajectory, the joint angles, angular velocities, angular accelerations, and angular jerks of each joint of the robot are highly continuous, resulting in minimal motion impact and a short path.

[0072] In one embodiment, step S3, obtaining the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the manipulator corresponding to each interpolation point in the first trajectory based on multiple interpolation points in the first trajectory, may include:

[0073] Step S31: Determine a 4*4 homogeneous transformation matrix for each interpolation point; here, the homogeneous transformation matrix also includes the coordinate information and attitude information of the interpolation point.

[0074] Step S32: Based on the homogeneous transformation matrix, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to each interpolation point.

[0075] Step S33: Using the derivative method, the angular velocity, angular acceleration, and angular jerk of each joint of the robot are obtained from the joint angles and running time of each joint. Here, the running time is the running time of the first trajectory segment, which is a set value.

[0076] In one embodiment, in step S4, based on the joint angles of each joint of the manipulator corresponding to the start point, end point, and transition point of the second trajectory, the second trajectory is determined in joint space using an impact optimization algorithm, along with the joint angles of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory, including:

[0077] Step S41: Set a transition point as the current transition point. Determine the 4*4 homogeneous transformation matrix corresponding to the three points based on the starting point, ending point and current transition point of the second trajectory.

[0078] Step S42: Based on the homogeneous transformation matrices corresponding to the three points, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to the three points.

[0079] Step S43: For the joint angles of the manipulator corresponding to the three points, an improved dual-tree RRT obstacle avoidance path planning algorithm is used to plan the trajectory to obtain a preliminary trajectory; a cubic B-spline curve path optimization algorithm is used to smooth the preliminary trajectory to obtain a processed trajectory; the processed trajectory is optimized and envelope collision detection is performed using a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation to obtain an optimized trajectory, as well as the joint angles of the manipulator corresponding to the multiple interpolation points of the optimized trajectory.

[0080] Here, the impact optimization algorithm adopts a combination of algorithms, including an improved dual-tree RRT obstacle avoidance path planning algorithm, a cubic B-spline curve path optimization algorithm, and a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation.

[0081] The Rapid Expanding Random Tree (RRT) algorithm is suitable for solving obstacle avoidance path planning problems for multi-DOF serial manipulators in complex environments. Common RRT algorithms include single-tree RRT and dual-tree RRT algorithms. The dual-tree RRT algorithm can be improved by employing the nearest path asymptotic optimization method. The improved dual-tree RRT algorithm creates two root node trees simultaneously at the start and target points. To expand each tree, a configuration is randomly generated. If no collision occurs with environmental obstacles, a step is taken starting from the nearest node based on the set maximum connection distance. Then, the algorithm attempts to connect the nearest node on the opposite tree using the newly expanded tree. In this case, direct connections can be used to shorten the path without collisions. Compared to the single-tree RRT algorithm, the improved dual-tree RRT algorithm yields an optimized path with higher search speed and shorter path length.

[0082] The improved dual-tree RRT obstacle avoidance path planning algorithm described above yields initial collision-free path points. Since the paths obtained by this algorithm are typically non-smooth, a cubic B-spline curve path optimization algorithm can be used to optimize the path using a cubic uniform B-spline curve, resulting in a set of smooth path points. Finally, a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation is employed for trajectory optimization and envelope collision detection, yielding the optimized trajectory. Here, the constructed parametric model for the manipulator loading and unloading drill rod includes multiple envelopes, such as the envelope of the upper stabilizing device or the manipulator's envelope. The minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation enables envelope collision detection.

[0083] Step S44: If no collision occurs between envelopes in the envelope collision detection, the optimized trajectory, i.e., the second segment trajectory, and the joint angles of each joint of the robot corresponding to multiple interpolation points of the second segment trajectory are output; if a collision occurs between envelopes in the envelope collision detection, the process returns to step S41 and the transition point is reset.

[0084] In one embodiment, step S7, based on multiple interpolation points of the third trajectory, obtains the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the manipulator corresponding to each interpolation point in the third trajectory, including:

[0085] Step S71: Determine a 4*4 homogeneous transformation matrix for each interpolation point;

[0086] Step S72: Based on the homogeneous transformation matrix, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to each interpolation point.

[0087] Step S73: Using the derivative method, the angular velocity, angular acceleration, and angular jerk of each joint of the robot are obtained from the joint angles and running times of each joint. Here, the running time is the running time of the third trajectory segment, which is a set value.

[0088] Based on the same inventive concept as the obstacle avoidance trajectory planning method for a drill pipe loading and unloading robot under varying target conditions, this embodiment also provides a corresponding obstacle avoidance trajectory planning device for a drill pipe loading and unloading robot under varying target conditions. Figure 3 A structural block diagram of an obstacle avoidance trajectory planning device for a drill pipe loading / unloading robot under varying target conditions, according to an embodiment of this application, is shown, including:

[0089] The model building module 310 is used to build a parametric model of the manipulator loading and unloading drill pipes. The parametric model of the manipulator loading and unloading drill pipes includes the joint motion model of the manipulator, the joint motion model of the host machine, and the drill pipe chamber model.

[0090] The first trajectory planning module 320 is used to set the start and end points of the first trajectory in the parametric model of the robot arm loading and unloading drill pipe. It adopts a straight trajectory and, based on the start and end points of the first trajectory, uses a minimum impact algorithm based on seventh-order polynomial interpolation in Cartesian space to determine multiple interpolation points of the first trajectory, thereby determining the first trajectory. Each interpolation point includes coordinate information and attitude information.

[0091] The first joint parameter determination module 330 of the robot arm is used to obtain the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the robot arm corresponding to each interpolation point in the first trajectory based on multiple interpolation points of the first trajectory.

[0092] The second trajectory planning module 340 is used to determine the start point, end point, and transition point of the second trajectory. The start point, end point, and transition point include coordinate information and attitude information. Based on the joint angles of each joint of the manipulator corresponding to the start point, end point, and transition point of the second trajectory, the second trajectory is determined in joint space based on the impact optimization algorithm, as well as the joint angles of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory.

[0093] The second joint parameter determination module 350 of the robot arm is used to determine the angular velocity, angular acceleration and angular jerk of each joint of the robot arm corresponding to each interpolation point of the second trajectory using the derivative method, based on the joint angle and running time of each joint of the robot arm corresponding to multiple interpolation points of the second trajectory.

[0094] The third trajectory planning module 360 ​​is used to determine the start and end points of the third trajectory. It adopts a straight trajectory and, based on the start and end points of the third trajectory, uses a minimum impact algorithm based on seventh-order polynomial interpolation in Cartesian space to determine multiple interpolation points of the third trajectory, thereby determining the third trajectory. Each interpolation point includes coordinate information and attitude information.

[0095] The third joint parameter determination module 370 of the robot arm is used to obtain the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the robot arm corresponding to each interpolation point in the third trajectory based on multiple interpolation points of the third trajectory.

[0096] The trajectory synthesis module 380 is used to connect the beginning and end of the first, second, and third trajectories to obtain the final trajectory, and to obtain the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the robot corresponding to each path point in the final trajectory; each path point includes multiple interpolation points of the first, second, and third trajectories.

[0097] The obstacle avoidance trajectory planning device for the drill pipe loading and unloading robot under variable target conditions in this embodiment has the same inventive concept as the obstacle avoidance trajectory planning method for the drill pipe loading and unloading robot under variable target conditions described above. Therefore, the specific implementation of this device can be found in the embodiment section of the obstacle avoidance trajectory planning method for the drill pipe loading and unloading robot under variable target conditions described above, and its technical effects correspond to the technical effects of the above method, so it will not be repeated here.

[0098] To further demonstrate the effectiveness of the method in this application, the following simulation experiments were conducted.

[0099] Trajectory planning was performed using both the minimum impact algorithm and a combined algorithm. The three degrees of freedom of the main unit were adjusted with parameters of 200mm lifting, 45° tilting, and 440mm translation, and trajectory planning simulation analysis was conducted under this drilling condition. The minimum impact obstacle avoidance trajectory planning algorithm was used to plan three trajectory segments separately, with simulation time periods set to (0-5)s, (5-25)s, and (25-30)s, for a total simulation time of 30s. Figure 4 The motion path of the robot planned by the minimum impact algorithm is shown. Figure 5 The simulation curves of the operating parameters of each joint of the robot obtained by the minimum impact algorithm are shown, where (a) is the curve of the joint angle change of each joint of the robot, (b) is the curve of the angular velocity change of each joint of the robot, (c) is the curve of the angular acceleration change of each joint of the robot, and (d) is the curve of the angular jerk change of each joint of the robot.

[0100] exist Figure 4 In the second segment of the trajectory, the robotic arm's movement path is smooth but relatively long. According to... Figure 5 In the three trajectory segments, the angles, angular velocities, angular accelerations, and angular jerks of each joint of the robotic arm are continuous. However, in the second trajectory segment, the absolute values ​​of the joint angular velocities, angular accelerations, and angular jerks are very large, with the peak absolute value of the angular jerk reaching 52.481 rad / s². 3 This is highly detrimental to the operation of the robotic arm. Therefore, although this method achieves minimal impact planning, it does so by increasing parameters such as the joint speed and travel distance of the robotic arm, thereby sacrificing the robotic arm's lifespan.

[0101] By adjusting the parameters of the main unit's three degrees of freedom—lifting by 200mm, tilting at 45°, and translating by 440mm—trajectory planning simulation analysis was performed under this drilling condition. The total simulation time was set to 30 seconds. Figure 6 The motion path of the robot arm obtained by the combined algorithm is shown. Figure 7 The simulation curves of the joint operation parameters of the robot obtained by the combined algorithm are shown, where (a) is the curve of the joint angle change of each joint of the robot, (b) is the curve of the angular velocity change of each joint of the robot, (c) is the curve of the angular acceleration change of each joint of the robot, and (d) is the curve of the angular jerk change of each joint of the robot.

[0102] Depend on Figure 6 It can be seen that, compared to the path in the minimum impact algorithm, the path curve is shorter, due to... Figure 7 As shown in (d), the maximum peak absolute value of the angular acceleration is 0.957 rad / s². 3 Compared to the minimum impact algorithm, this algorithm reduces the impact by 98.2%. Compared to the existing minimum impact trajectory planning algorithm, the joint angles, angular velocities, angular accelerations and angular jerks of the robot are relatively continuous, the path is short and the peak value of angular jerk is very small, which is beneficial to the long-term operational stability of the robot and improves its service life.

[0103] This application provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, it implements the obstacle avoidance trajectory planning method for a drill rod loading and unloading robot under varying target conditions.

[0104] This application provides a computer program product, including a computer program / instruction. When the computer program / instruction is executed by a processor, it implements the obstacle avoidance trajectory planning method for a drill rod loading / unloading robot under varying target conditions.

[0105] The above descriptions are merely various embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for obstacle avoidance trajectory planning for a drill pipe loading / unloading robot under varying target conditions, characterized in that, include: A parametric model for loading and unloading drill pipes by a robotic arm is constructed. The parametric model for loading and unloading drill pipes by a robotic arm includes the joint motion model of the robotic arm, the joint motion model of the host machine, and the drill pipe chamber model. In the parametric model of the robotic arm loading and unloading drill pipe, the starting point and the end point of the first trajectory are set, and a straight trajectory is adopted. Based on the starting point and the end point of the first trajectory, a minimum impact algorithm based on seventh-order polynomial interpolation is used in Cartesian space to determine multiple interpolation points of the first trajectory, thereby determining the first trajectory; each interpolation point includes coordinate information and attitude information. Based on multiple interpolation points of the first trajectory segment, the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the manipulator corresponding to each interpolation point in the first trajectory segment are obtained; The starting point, ending point, and transition point of the second trajectory segment are determined; the starting point, ending point, and transition point include coordinate information and attitude information; based on the joint angles of each joint of the manipulator corresponding to the starting point, ending point, and transition point of the second trajectory segment, the second trajectory segment is determined in joint space based on an impact optimization algorithm, as well as the joint angles of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory segment; the impact optimization algorithm adopts a combined algorithm, including an improved dual-tree RRT obstacle avoidance path planning algorithm, a cubic B-spline curve path optimization algorithm, and a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation; Based on the joint angles and running times of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory, the angular velocity, angular acceleration, and angular jerk of each joint of the manipulator corresponding to each interpolation point of the second trajectory are determined by the derivative method. The starting point and ending point of the third trajectory are determined using a straight-line trajectory. Based on the starting point and ending point of the third trajectory, a minimum impact algorithm based on seventh-order polynomial interpolation is used in Cartesian space to determine multiple interpolation points of the third trajectory, thereby determining the third trajectory. Each interpolation point includes coordinate information and attitude information. Based on multiple interpolation points of the third trajectory, the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the manipulator corresponding to each interpolation point in the third trajectory are obtained; Connect the first, second, and third trajectory segments end to end to obtain the final trajectory, and obtain the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the robot corresponding to each path point in the final trajectory; each path point includes multiple interpolation points of the first trajectory segment, multiple interpolation points of the second trajectory segment, and multiple interpolation points of the third trajectory segment.

2. The method as described in claim 1, characterized in that, in, Based on multiple interpolation points of the first trajectory segment, the joint angles, angular velocities, angular accelerations, and angular jerks of each joint of the manipulator corresponding to each interpolation point in the first trajectory segment are obtained, including: Each interpolation point defines a homogeneous transformation matrix; Based on the homogeneous transformation matrix, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to each interpolation point. The angular velocity, angular acceleration, and angular jerk of each joint of the robot are obtained by using the derivative method to calculate the joint angles and running times of each joint of the robot.

3. The method as described in claim 1, characterized in that, in, Based on the joint angles of the manipulator at the start, end, and transition points of the second trajectory, the second trajectory is determined in joint space using an impact optimization algorithm. The joint angles of the manipulator at multiple interpolation points of the second trajectory, including: Step S41: Set a transition point as the current transition point, and determine the homogeneous transformation matrix corresponding to the three points based on the starting point, ending point and current transition point of the second trajectory. Step S42: Based on the homogeneous transformation matrices corresponding to the three points, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to the three points. Step S43: For the joint angles of the manipulator corresponding to the three points, an improved dual-tree RRT obstacle avoidance path planning algorithm is used to plan the trajectory to obtain a preliminary trajectory; and a cubic B-spline curve path optimization algorithm is used to smooth the preliminary trajectory to obtain a processed trajectory; the processed trajectory is then optimized and envelope collision detection is performed using a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation to obtain an optimized trajectory, and the joint angles of the manipulator corresponding to the multiple interpolation points of the optimized trajectory. Step S44: If no collision occurs between envelopes in the envelope collision detection, the optimized trajectory, i.e., the second segment trajectory, and the joint angles of each joint of the robot corresponding to the multiple interpolation points of the second segment trajectory are output; if a collision occurs between envelopes in the envelope collision detection, the process returns to step S41 and the transition point is reset.

4. The method as described in claim 1, characterized in that, in, Based on multiple interpolation points of the third trajectory segment, the joint angles, angular velocities, angular accelerations, and angular jerks of each joint of the manipulator corresponding to each interpolation point in the third trajectory segment are obtained, including: Each interpolation point defines a homogeneous transformation matrix; Based on the homogeneous transformation matrix, the inverse kinematics solution of the manipulator is used to obtain the joint angles of each joint of the manipulator corresponding to each interpolation point. The angular velocity, angular acceleration, and angular jerk of each joint of the robot are obtained by using the derivative method to calculate the joint angles and running times of each joint of the robot.

5. The method as described in claim 1, characterized in that, The transition point is a point between the two upper stabilizing devices.

6. The method as described in claim 1, characterized in that, The starting point of the second trajectory is the ending point of the first trajectory. The ending point of the second trajectory is a point at a fixed distance from the center point of the drill rod loading / unloading position on the host machine. The line connecting the ending point of the second trajectory and the center point of the drill rod loading / unloading position is perpendicular to the guide rail installed on the host machine.

7. The method as described in claim 1, characterized in that, Both the first and third segments of the trajectory are straight-line trajectories.

8. A trajectory planning device for a drill pipe loading / unloading robot under varying target conditions, characterized in that, include: The model building module is used to build a parametric model of the manipulator loading and unloading drill pipes. The parametric model of the manipulator loading and unloading drill pipes includes the joint motion model of the manipulator, the joint motion model of the host machine, and the drill pipe chamber model. The first trajectory planning module is used to set the start and end points of the first trajectory in the parameterized model of the manipulator loading and unloading drill pipe. It adopts a straight trajectory and, based on the start and end points of the first trajectory, uses a minimum impact algorithm based on seventh-order polynomial interpolation in Cartesian space to determine multiple interpolation points of the first trajectory, thereby determining the first trajectory. Each interpolation point includes coordinate information and attitude information. The first joint parameter determination module for the robotic arm is used to obtain the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the robotic arm corresponding to each interpolation point in the first trajectory based on multiple interpolation points in the first trajectory. The second trajectory planning module is used to determine the start point, end point, and transition point of the second trajectory. The start point, end point, and transition point include coordinate information and attitude information. Based on the joint angles of each joint of the manipulator corresponding to the start point, end point, and transition point of the second trajectory, the second trajectory is determined in joint space using an impact optimization algorithm, as well as the joint angles of each joint of the manipulator corresponding to multiple interpolation points of the second trajectory. The impact optimization algorithm is a combined algorithm, including an improved dual-tree RRT obstacle avoidance path planning algorithm, a cubic B-spline curve path optimization algorithm, and a minimum impact obstacle avoidance trajectory planning method based on seventh-order polynomial interpolation. The second module for determining the joint parameters of the robotic arm is used to determine the angular velocity, angular acceleration, and angular jerk of each joint of the robotic arm corresponding to each interpolation point of the second trajectory using the derivative method, based on the joint angle and running time of each joint of the robotic arm corresponding to multiple interpolation points of the second trajectory. The third trajectory planning module is used to determine the start and end points of the third trajectory. It adopts a straight-line trajectory and, based on the start and end points of the third trajectory, uses a minimum impact algorithm based on seventh-order polynomial interpolation in Cartesian space to determine multiple interpolation points of the third trajectory, thereby determining the third trajectory. Each interpolation point includes coordinate information and attitude information. The third module for determining the joint parameters of the robotic arm is used to obtain the joint angle, angular velocity, angular acceleration and angular jerk of each joint of the robotic arm corresponding to each interpolation point in the third trajectory based on multiple interpolation points in the third trajectory. The trajectory synthesis module is used to connect the first segment of the trajectory, the second segment of the trajectory, and the third segment of the trajectory to obtain the final trajectory, and to obtain the joint angle, angular velocity, angular acceleration, and angular jerk of each joint of the robot corresponding to each path point in the final trajectory; each path point includes multiple interpolation points of the first segment of the trajectory, multiple interpolation points of the second segment of the trajectory, and multiple interpolation points of the third segment of the trajectory.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the obstacle avoidance trajectory planning method for a drill rod loading / unloading manipulator under varying target conditions as described in any one of claims 1-7.

10. A computer program product, characterized in that, It includes a computer program / instruction, which, when executed by a processor, implements the obstacle avoidance trajectory planning method for a drill rod loading / unloading manipulator under varying target conditions as described in any one of claims 1-7.

Images