Remote puncture robot path planning method based on improved RRT algorithm

Abstract

The application provides a remote puncture robot path planning method based on an improved RRT algorithm, which specifically comprises the following steps: puncture robot parameter setting and initialization, target bias strategy, new node generation, collision detection, exploration path and path pruning processing, etc. By introducing the target bias strategy, the dynamic space sampling strategy and the path pruning strategy, the planning time is shortened, the path length is reduced, the planning efficiency and success rate are improved, and good results can be achieved in two-dimensional and three-dimensional obstacle environments. The remote puncture robot path planning method is based on the improved RRT algorithm, laser radar and X-ray positioning, and can autonomously plan a collision-free path from the starting point of the puncture robot to the needle insertion point without human intervention, thereby improving the accuracy in remote surgery.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of medical robots, and particularly relates to a remote puncture robot path planning method based on an improved RRT algorithm. BACKGROUND

[0002] With the rapid development of hardware technology and computer technology, robots are widely used in various fields, especially in the medical field. Among them, there is an automatic puncture robot used in percutaneous vertebroplasty under the guidance of X-ray film. The robot end effector is a puncture kirschner wire clamping mechanism. At present, in most cases, the doctor adjusts the angle of the automatic puncture robot end effector and places it in the appropriate position near the needle insertion point. The puncture robot will perform puncture and needle insertion until it reaches the lesion site.

[0003] The path planning of the robot is to search for a set of reasonable path points under the condition of given starting point and target point, and to convert these path points into a corresponding joint angle set. By using this set of joint angle set, the robot is driven to move from the initial pose to the target pose without collision. The Rapidly-exploring Random Tree (RRT) is one of the many path planning algorithms. RRT is a sampling-based algorithm that obtains path nodes through collision detection of state space nodes, stores path nodes in a tree structure, guides the search to the blank area, and obtains a collision-free path. This method does not need to model the space, can effectively solve the path planning problem of high-dimensional space and complex constraints, and is widely used in the field of robot motion planning.

[0004] The traditional RRT algorithm for planning path also has some shortcomings: the global uniform random sampling strategy leads to lack of directionality of the algorithm, slow convergence speed, and high time consumption; the random tree for searching path is generated from random sampling points in the map, which may not be the optimal path. Therefore, an improved path planning algorithm is needed to solve the problems of long time consumption, low efficiency and non-optimal planning path. SUMMARY

[0005] The present application provides a remote puncture robot path planning method based on an improved RRT algorithm, which is used to solve the path planning problem of the existing automatic puncture robot moving from the initial state to the needle insertion point. By introducing a target bias strategy, the blindness of global sampling is reduced, the directivity is increased, sampling is performed in a dynamic sampling space, the planning speed of the path is improved, the planning time is shortened, and finally the initial path is pruned to reduce the path length and plan an optimized path.

[0006] In order to achieve the above purpose, the technical scheme adopted by the present application is as follows:

[0007] A remote puncture robot path planning method based on an improved RRT algorithm, comprising the following steps:

[0008] S1, puncture robot parameter setting and initialization:

[0009] S11, establishing a kinematic model of the puncture robot;

[0010] S12, obtaining specific position information of the starting point X start , target point X goal and obstacle environment information of the puncture robot, and obtaining the joint angle of the puncture robot corresponding to the starting point X start and the target point X goal by inverse kinematics solution;

[0011] S13, setting the initial values of the target bias probability threshold q, the search step length step, the allowed error Thr and the maximum iteration number iterMax according to the actual working condition, and initializing the random tree T, setting the current iteration number iter=1 and the initial bias probability p

[0012] S2, target bias strategy: determining the sampling method according to the size relationship between the target bias probability threshold q and the bias probability p, and updating the bias probability p after each collision detection;

[0013] S21, if p sample is equal to the target point X goal ;

[0014] S22, if p sample is obtained by random sampling in the dynamic sampling space;

[0015] S3, generating a new node:

[0016] S31, finding the point closest to the sampling point X sample in the random tree T as the parent node X nearest ;

[0017] S32, generating a new node X new in the direction of the connecting line from the parent node X nearest to the sampling point X sample with the search step length step, and the expansion formula of the new node X new is:

[0018]

[0019] S4, collision detection: performing collision detection on the new node X new and updating the value of the bias probability p;

[0020] S41. If a collision occurs, let p ≥ q, and jump back to step S22, that is, perform random sampling in the dynamic sampling space and discard the new node X. new Adjustments are made based on the current bias probability p until a new node X is found. new ;

[0021] S42. If no collision occurs, then let p < q, that is, continue to expand the random tree towards the target point, and set the new node X. new Add to the random tree T;

[0022] S5. Exploring the path: Repeat steps S2 to S4 to expand the random tree. In each iteration, increment iter by 1 until a new node X is obtained. new Satisfy |X new -X goal If | < Thr and iter < iterMax, then the target point is considered found, the path planning is successful, the loop ends, and the planned path is found by reverse search based on the parent-child relationship of each node.

[0023] S6. Path pruning:

[0024] The path obtained in step S5 is pruned to reduce its length. The points on this path are converted into the corresponding joint angles of the robot and then converted into instructions sent to the host computer. Under the control of the host computer, the puncture robot moves from the starting point to the target point along the planned path.

[0025] Preferably, in step S1, the target point X goal This is the needle insertion point.

[0026] Preferably, in step S2, the target bias probability threshold q is a fixed value preset according to the obstacle environment, and the bias probability p (0 < p < 1) is a variable value that is updated after each collision detection.

[0027] Preferably, in step S2, in the target bias strategy, in the initial state, to accelerate the growth of the random tree, the end effector of the piercing robot explores the path towards the target point from the beginning, that is, the sampling point is set as the target point and a new node is generated. Collision detection is performed on the new node. If a collision occurs, it means that there is an obstacle between the nearest node on the random tree and the target point. Then, p≥q is set, that is, random sampling is performed again in the dynamic sampling space to obtain the sampling point X. sample If no collision occurs, it means there is no obstacle between the nearest node in the random tree and the target point. Therefore, let p < q, and do not perform random sampling, taking the target point X as the reference. goal As sampling point X sample .

[0028] It can be preferred that in the step S22, the dynamic sampling space in the target bias strategy is determined by the target point X goal and the nearest point X near together, and the specific expression is as follows:

[0029] The dynamic sampling space (x, y) in the two-dimensional space:

[0030]

[0031] The dynamic sampling space (x, y, z) in the three-dimensional space:

[0032]

[0033] It can be preferred that in the step S22, when a random point in the dynamic sampling space is taken as a sampling point and a new node is generated in the target bias strategy, collision detection is performed on the new node. If collision occurs, considering that sampling is performed through the step S21 in the last round of sampling, but collision detection occurs after collision, and p≥q, in order to avoid repeated collision detection failure in the next round, the sampling is still performed in the dynamic space when p≥q; if no collision occurs, it means that a new node close to the target point is added to the random tree, so p<q, and the target point X goal is taken as the sampling point X sample , and the path exploration towards the target point is continued.

[0034] It can be preferred that in the step S22, the dynamic space sampling strategy indicates that the sampling space at each sampling time is dynamically changed, the dynamic sampling space is between the target point and the nearest node to the target point in the random tree, and the sampling space is dynamically updated with the update of the random tree, so as to ensure that the sampling point always grows towards the target point.

[0035] It can be preferred that the step S5 further includes the following case: when iter=iterMax, no new node X new satisfying the condition is found, the path planning is ended.

[0036] It can be preferred that the pruning processing in the step S6 specifically includes the following steps:

[0037] S61, the starting point is added to the new path, and the starting point is set as a detection point;

[0038] S62, it is judged whether collision occurs between the starting point and the second point in the initial path. If no collision occurs, it is continued to detect whether collision occurs between the starting point and the third point, and so on, until collision occurs.

[0039] S63, add a node before the collision occurs to the new path, and set the node as a detection point;

[0040] S64, repeat step S62 and step S63 until the target point X goal is detected, that is, a new path is obtained.

[0041] Compared with the prior art, the technical effects of the present application are:

[0042] (1) The remote puncture robot path planning method based on the improved RRT algorithm provided by the present application improves the selection mode of the random point on the basis of the traditional RRT algorithm, adopts a target bias strategy, makes the random tree grow purposefully towards the target point, introduces a dynamic sampling space, reduces the sampling range of the random point, reduces the blindness of random sampling of the basic RRT algorithm, improves the planning efficiency of the algorithm, introduces a path pruning strategy after obtaining the initial planning path, and eliminates path points with excessively large path lengths, so as to obtain a path with smaller path length and more gentle.

[0043] (2) The remote puncture robot path planning method based on the improved RRT algorithm provided by the present application can obtain good planning results in two-dimensional and three-dimensional obstacle environments, shorten the time required for path planning, reduce the path length, improve the efficiency and success rate of path planning, and quickly obtain a relatively optimal path, and then control the puncture robot to move according to the planned path.

[0044] (3) The remote puncture robot path planning method based on the improved RRT algorithm provided by the present application is applied to remote puncture surgery, plans a collision-free path from the starting point of the puncture robot to the needle insertion point by the improved RRT algorithm, converts the path into an instruction and sends the instruction to the puncture robot, controls the end of the puncture robot to reach the needle insertion point along the planned path and avoid obstacles, and can autonomously plan a reasonable path without human intervention. BRIEF DESCRIPTION OF DRAWINGS

[0045] Figure 1 It is a flowchart of the remote puncture robot path planning method based on the improved RRT algorithm;

[0046] Figure 2 It is a flowchart of the improved RRT algorithm;

[0047] Figure 3 It is a link coordinate system of the puncture robot;

[0048] Figure 4 It is a dynamic sampling space diagram of the improved RRT algorithm in a two-dimensional environment;

[0049] Figure 5Fig. 1 is a schematic diagram of a dynamic sampling space in a three-dimensional environment for improving the RRT algorithm;

[0050] Figure 6 Fig. 2 is a schematic diagram of generating a new node by advancing in a search step;

[0051] Figure 7 Fig. 3 is a schematic diagram of an optimized path obtained by path pruning of the improved RRT algorithm;

[0052] Fig. 8(a) and Fig. 8(b) are respectively simulation results of the basic RRT algorithm and the improved RRT algorithm in a two-dimensional environment;

[0053] Fig. 9(a) and Fig. 9(b) are respectively simulation results of the basic RRT algorithm and the improved RRT algorithm in a three-dimensional environment. DETAILED DESCRIPTION

[0054] The specific structure and working principle of the present application will be further described in detail below in combination with the drawings and examples.

[0055] In order to make the purpose, technical scheme and advantages of the method more clear and explicit, the present application will be further described in detail below in combination with the drawings. It should be understood that the specific implementation examples described herein are only used to explain and not to limit the present application.

[0056] As shown in Figure 1 , the implementation process of the remote puncture robot path planning method based on the improved RRT algorithm of the present application is as follows: first, a hardware device and a software control system of an automatic puncture robot are built, obstacle information around an operating table is obtained through laser radar scanning, and specific starting point and target point information are obtained in combination with X-ray films, then the position and attitude corresponding to the end of the puncture robot are obtained through inverse kinematics solving, an improved RRT algorithm is used for path planning to obtain a collision-free path from the starting point to the target point, the path is converted into instructions and sent to a host computer, and the host computer controls the puncture robot to move according to the given path.

[0057] As shown in Figure 2 , the specific implementation process of the improved RRT algorithm is as follows:

[0058] S1, puncture robot parameter setting and initialization: the puncture robot parameters include the starting point X start , the target point X goal , and the joint angle of the puncture robot corresponding thereto, and the initialization means that a kinematics model of the puncture robot is established, and the initial values of the related parameters in the improved RRT algorithm are set according to the actual working conditions.

[0059] S11, as shown in Figure 3As shown, a kinematic model of the puncture robot is established. Let axis i represent the axis of the joint connecting link i-1 to link i. The following are the principles for establishing the link coordinate system:

[0060] Select axis z along the direction of joint i+1. i ;

[0061] Origin O i Positioned on axis z i With axis z i-1 and z i Similarly, the intersection of the common perpendiculars, O i' Positioned at the common perpendicular and axis z i-1 The intersection point;

[0062] Along axis z i-1 and z i The common perpendicular line is selected from the x-axis. i The direction is from joint i to i+1;

[0063] Select axis y i To form a right-handed system;

[0064] Once the link coordinate system is established, the position and orientation of coordinate system i with respect to coordinate system i-1 are completely given by the following parameters:

[0065] α i :O i and O i' The distance between them;

[0066] a i :O i' Along z i-1 The coordinates;

[0067] θ i : axis z i-1 and axis z i The angle between them, when about the x-axis i When rotating counterclockwise, the positive direction is taken.

[0068] d i : axis x i-1 and axis x i The angle between them, when about the z-axis i-1 When rotating counterclockwise, the positive direction is taken.

[0069] Based on the above principles, a DH parameter table for the puncture robot is established, as shown in Table 1.

[0070] Table 1 DH Parameter Table

[0071]

[0072] Based on the parameters in the table, the forward kinematic equations of the puncture robot are calculated as follows:

[0073]

[0074] wherein:

[0075]

[0076]

[0077]

[0078]

[0079] In the above formula, c represents cos, and s represents sin.

[0080] S12, obtaining specific position information of the starting point X start and the target point X goal and obstacle environment information, and obtaining joint angles of the puncture robot corresponding to the starting point X start and the target point X goal by inverse kinematics. In a specific embodiment, the specific position information of the starting point X start and the target point X goal and the obstacle environment information around the operating table are obtained in combination with X-ray films and laser radars, and the position and attitude of the end of the puncture robot corresponding to the starting point X start and the target point X goal are obtained by inverse kinematics.

[0081] S13, initializing the target bias probability threshold q, the search step length step, the allowed error Thr, and the maximum number of iterations iterMax, and adding the starting point X start to the random tree T, setting the current number of iterations iter = 1 and the initial bias probability p < q.

[0082] S2, target bias strategy: determining the sampling method according to the size relationship between the target bias probability threshold q and the bias probability p, and updating the bias probability p after each collision detection;

[0083] S21, if p < q, do not perform random sampling, and set the sampling point X sample equal to the target point X goal ;

[0084] S22, if p ≥ q, random sampling is performed in the dynamic sampling space to obtain the sampling point X sample ;

[0085] Further, the specific process of step S2 is analyzed and described as follows:

[0086] Sampling point X sample The selection method is determined by the relationship between the target bias probability threshold q and the bias probability p. The target bias probability threshold q is a fixed value preset based on the obstacle environment. The more complex the obstacle, the smaller the value of the target bias probability threshold q. The value of the bias probability p is updated in each collision detection, and it is divided into the following two cases:

[0087] (1) When p < q: In the initial state, let the initial bias probability p < q, that is, initially explore the path towards the target point. If the newly generated node X at this time new If no collision occurs after collision detection, then the new node X will be... new If added to the random tree, then the random tree now has a starting point X. start and new node X new Two nodes, with starting point X start New node X new and target point X goal If three points lie on a straight line and no collision occurs, it means they are on the first half of this straight line, i.e., the starting point X. start With new node X new There is no interference with obstacles. Considering that the path can be found more quickly and the search step size is relatively small, it is assumed that the path will be found at the new node X. new Continue towards target point X goal Expanding by one step increases the probability of avoiding a collision, so we continue to set the bias probability p < q, and then move towards the target point X. goal Expand to a new node; if the new node X generated at this time... new If a collision occurs after collision detection, it means that the collision occurred at the starting point X. start With new node X new If interference occurs between nodes and obstacles, and the node continues to expand towards the target point in the next iteration, the same collision detection process will be repeated. To avoid this, the bias probability p ≥ q is set. In the next sampling, a point is randomly selected from the dynamic sampling space, i.e., the random tree is expanded in any direction in the sampling space, and the newly generated node X is discarded. new Resampling is performed based on the relationship between the currently updated bias probability p and the target bias probability threshold q.

[0088] (2) When p≥q: At this time, a point is randomly selected in the dynamic space. If the newly generated node X is... new If no collision occurs after collision detection, then the new node X will be... new Adding it to the random tree, to make the random tree expand as close to the target point as possible, we set the bias probability p < q, that is, the target point is used as the random point for the next round; if the new node X generated at this timenew If collision occurs after collision detection, it means that at this time the starting point X start interferes with the obstacle between the new node X new , and considering that p≥q in this round may be obtained from the second case in case (1), that is, in the last round of sampling, the target point is taken as the sampling point, but collision occurs when collision detection is performed, so the bias probability is updated to p≥q, which also leads to random sampling in the dynamic sampling space in this round. Since collision occurs in the last round and this round, no new node is added to the random tree. If p<q in the next round, it is equivalent to returning to the last round and performing the same collision detection to jump back to this round. In order to avoid this repeated operation, p≥q is set, and random sampling is performed in the dynamic sampling space next time, which improves the sampling efficiency.

[0089] Further, the specific process of sampling the dynamic sampling space is analyzed and described as follows:

[0090] The dynamic sampling space strategy indicates that the sampling space at each sampling time is dynamically changed. The dynamic space is between the target point and the node in the random tree closest to the target point. With the update of the random tree, the sampling space is also dynamically updated to ensure that the sampling point always grows towards the target point, avoiding the phenomenon of expanding towards the root of the random tree, causing redundant points or increasing the path length.

[0091] In a two-dimensional environment, as shown in Figure 4 , with the exploration of the path, the node closest to the target point in the random tree can always be found, that is, the nearest point in the figure. When p≥q, that is, when random sampling is performed in the dynamic sampling space, the sampling space is the S1, S2 and S3 regions in the figure, and the probability of taking a point in the S2 region is set to 0.5, and the probabilities of taking points in the S1 and S2 regions are 0.25 respectively;

[0092] The same principle applies in a three-dimensional environment, as shown in Figure 5 , when p≥q, the dynamic sampling space is the S1, S2 and S3 regions, and the proportions of the sampling points falling in these three regions are 0.25, 0.5 and 0.25 respectively. With the continuous update of the random tree, the position of the nearest point changes constantly, so the sampling space also dynamically changes, but always moves closer to the target point.

[0093] S3, a new node is generated

[0094] S31, find the nearest point in the random tree T to the sampling point X sample as the parent node X nearest ;

[0095] S32, between the parent node X nearest and the sampling point Xsample The new node X is generated by advancing in the direction of the connecting line of the parent node X with a search step step new new The expansion formula is:

[0096]

[0097] As shown in Fig. 1, the node X is a new node obtained by advancing in the direction of the parent node X with a search step step. Figure 6 new nearest

[0098] S4, collision detection: the new node X is subjected to collision detection and the value of the bias probability p is updated; new

[0099] S41, if collision occurs, p≥q is set, and the step S22 is jumped back, i.e. random sampling is performed in the dynamic sampling space, and the new node X is discarded, and adjustment is made according to the value of the current bias probability p until the new node X is found; new new

[0100] S42, if collision does not occur, p<q is still set, i.e. the random tree is continuously expanded towards the target point, and the new node X is added to the random tree T; new

[0101] S5, exploration path: steps S2-S4 are repeated to expand the random tree, and iter=iter+1 is set each time the loop is executed until the new node X is obtained, which satisfies |X new new goal |<Thr and iter<iterMax, the target point is considered to be found, the path planning is successful, the loop is ended, and the planning path is found by reverse search according to the parent-child relationship of each node.

[0102] Further, in step S5, the following case exists: until iter=iterMax, the new node X satisfying the condition is not found, and the path planning is ended. new

[0103] S6, path pruning processing:

[0104] The path obtained in step S5 is subjected to pruning processing, the path length is reduced according to the principle that the sum of two sides of a triangle is greater than the third side, the points on the path are converted into corresponding joint angles of the robot, and the joint angles are converted into instructions and sent to the upper computer, and the robot is controlled by the upper computer to move from the starting point to the target point according to the planned path.

[0105] ​​​​​​​​​​​​Furthermore, the specific process of step S6 is analyzed and described as follows:

[0106] S61. Add the starting point to the new path and set the starting point as the detection point;

[0107] S62. Determine whether there is a collision between it and the second point in the initial path. If there is no collision, continue to check whether there is a collision between the starting point and the third point, and so on, until a collision occurs.

[0108] S63. Add a node that occurred before the collision to the new path and set that node as a detection point;

[0109] S64. Repeat steps S62 and S63 until the target point X is detected. goal Until then, a new path has been obtained.

[0110] like Figure 7 As shown in the diagram, black dots represent path points, solid lines represent the initially planned path, and dashed lines represent the optimized path obtained after path pruning. Point 1 is added to the new path, and point 1 is used as the detection point. No collisions occur between point 1 and point 2, point 1 and point 3, or point 1 and point 4. A collision occurs between point 1 and point 5, so point 4 is added to the new path. Then, using point 4 as the path, no collisions occur between point 4 and point 5, or point 4 and point 6, so point 6 is added to the path. Therefore, the optimized path is from point 1 to point 4 and then to point 6.

[0111] The embodiments of the present invention will be described in detail below using specific experiments.

[0112] This embodiment simulates in both a two-dimensional obstacle environment and a three-dimensional obstacle environment. The map size for the two-dimensional environment is [10, 10], and the map size for the three-dimensional environment is [1000, 1000, 1000]. Simulation environment: Lenovo Legion, Intel Core i5-7300HQ processor, MATLAB 2019b. Parameter settings: In the two-dimensional environment: target bias probability threshold q = 0.5, search step size step = 0.3, allowable error Thr = 0.5, maximum number of iterations iterMax = 10000, current iteration number iter = 1, initial bias probability p = q - 0.1, starting point X start = (1,1), target point X goal = (9,9); In a 3D environment: target bias probability threshold q = 0.5, search step size step = 5, allowable error Thr = 10, maximum number of iterations iterMax = 10000, current iteration number iter = 1, initial bias probability p = q - 0.1, starting point X start = (50, 50, 50), target point Xgoal = (900, 900, 900).

[0113] Fig. 8 (a) and Fig. 8 (b) are comparison diagrams of simulation results of the improved RRT algorithm and the basic RRT algorithm based on a two-dimensional obstacle environment, in order to avoid the contingency of one experiment, the average value of 100 experiments of the two algorithms is counted, and the specific data is shown in Table 2. It can be known from Fig. 8 (a), Fig. 8 (b) and Table 2 that compared with the basic RRT algorithm, the improved RRT algorithm has shorter path planning time, smaller path length, fewer path points, fewer nodes in the random tree, and the same success rate of path planning. Specifically, compared with the basic RRT algorithm, the planning time of the improved RRT algorithm is shortened by 81.564%, the path length is reduced by 18.273%, the path points are reduced by 81.013%, the node number is reduced by 91.445%, and the success rate is increased by 96.078%.

[0114] Table 2 Simulation experiment data

[0115]

[0116]

[0117] Fig. 9 (a) and Fig. 9 (b) are comparison diagrams of simulation results of the improved RRT algorithm and the basic RRT algorithm based on a three-dimensional obstacle environment, in order to avoid the contingency of one experiment, the average value of 100 experiments of the two algorithms is counted, and the specific data is shown in Table 3. It can be known from Fig. 9 (a), Fig. 9 (b) and Table 3 that compared with the basic RRT algorithm, the improved RRT algorithm has shorter path planning time, smaller path length, fewer path points, fewer nodes in the random tree, and higher success rate of path planning. Specifically, compared with the basic RRT algorithm, the planning time of the improved RRT algorithm is shortened by 89.248%, the path length is reduced by 20.31%, the path points are reduced by 97.455%, the node number is reduced by 84.91%, and the success rate is increased by 96.078%.

[0118] Table 3 Simulation experiment data

[0119]

[0120] The improved RRT algorithm-based remote puncture robot path planning method provided by the application is applied to a remote puncture operation, an improved RRT algorithm is used to plan a collision-free path from a starting point of a puncture robot to a needle insertion point, the path is converted into instructions and sent to the puncture robot, and the end of the puncture robot is controlled to reach the needle insertion point along the planned path and avoid obstacles, so that a reasonable path can be autonomously planned without human intervention, and the puncture robot is remotely controlled.

[0121] The above embodiments are only to describe the preferred embodiments of the present application, and are not intended to limit the scope of the present application. Various modifications and improvements to the technical solutions of the present application made by those of ordinary skill in the art without departing from the design spirit of the present application shall fall within the protection scope of the present application as defined by the claims.

Claims

1. A path planning method for a remote puncture robot based on an improved RRT algorithm, characterized in that, It includes the following steps: S1. Puncture robot parameter settings and initialization: S11. Establish the kinematic model of the puncture robot; S12. Obtain the starting point X of the puncture robot. start Target point X goal The specific location information and obstacle environment information are obtained, and the starting point X is obtained by solving inverse kinematics. start and target point X goal The corresponding joint angles of the puncture robot; S13. Set the initial values ​​of the target bias probability threshold q, search step size step, allowable error Thr and maximum iteration number iterMax according to the actual working conditions, and initialize the random tree T, setting the current iteration number iter = 1 and the initial bias probability p < q. S2. Target bias strategy: The sampling method is determined based on the relationship between the target bias probability threshold q and the bias probability p. The bias probability p is updated after each collision detection. S21. If p < q, do not perform random sampling, let the sampling point X sample Equal to target point X goal ; S22. If p ≥ q, then random sampling is performed in the dynamic sampling space to obtain sampling point X. sample ; S3, Generate a new node: S31. Find the distance sampling point X in the random tree T. sample The most recent point is used as the parent node X nearest ; S32, at parent node X nearest To sampling point X sample New node X is generated by moving forward in the direction of the connection with a search step size step. new New node X new The extended formula is: S4. Collision Detection: For the new node X new Perform collision detection and update the value of the bias probability p; S41. If a collision occurs, let p ≥ q, and jump back to step S22, that is, perform random sampling in the dynamic sampling space and discard the new node X. new Adjustments are made based on the current bias probability p until a new node X is found. new ; S42. If no collision occurs, then let p < q, that is, continue to expand the random tree towards the target point, and set the new node X. new Add to the random tree T; S5. Exploring the path: Repeat steps S2 to S4 to expand the random tree. In each iteration, increment iter by 1 until a new node X is obtained. new Satisfy |X new -X goal If | < Thr and iter < iterMax, then the target point is considered found, the path planning is successful, the loop ends, and the planned path is found by reverse search based on the parent-child relationship of each node. S6. Path pruning: The path obtained in step S5 is pruned to reduce its length. The points on the path are converted into joint angles corresponding to the robot, and these angles are converted into commands and sent to the host computer. Under the control of the host computer, the puncture robot moves from the starting point to the target point along the planned path.

2. The path planning method for a remote puncture robot based on the improved RRT algorithm according to claim 1, characterized in that, In step S1, the target point X of the puncture robot goal This is the needle insertion point.

3. The path planning method for a remote puncture robot based on the improved RRT algorithm according to claim 1, characterized in that, In step S2, the target bias probability threshold q is a fixed value preset based on the obstacle environment, and the bias probability p (0 < p < 1) is a variable value that is updated after each collision detection.

4. The path planning method for a remote puncture robot based on the improved RRT algorithm according to claim 1, characterized in that, In the target bias strategy of step S2, in the initial state, to accelerate the growth of the random tree, the end effector of the puncture robot explores the path towards the target point from the beginning. That is, the sampling point is set as the target point and a new node is generated. Collision detection is performed on the new node. If a collision occurs, it means that there is an obstacle between the nearest node on the random tree and the target point. Then, p≥q is set, that is, random sampling is performed again in the dynamic sampling space to obtain the sampling point X. sample If no collision occurs, it means there is no obstacle between the nearest node in the random tree and the target point. Therefore, let p < q, and do not perform random sampling, taking the target point X as the reference. goal As sampling point X sample .

5. The remote puncture robot path planning method based on the improved RRT algorithm according to claim 1, characterized in that, In step S22, the dynamic sampling space in the target bias strategy is defined by the target point X. goal And the nearest point X to the target point in the random tree near The decision is made jointly, and the specific expression is as follows: Dynamic sampling space (x,y) in two-dimensional space: Dynamic sampling space (x, y, z) in three-dimensional space:

6. The path planning method for a remote puncture robot based on the improved RRT algorithm according to claim 5, characterized in that, In step S22, in the target bias strategy, when a random point in the dynamic sampling space is used as a sampling point and a new node is generated, collision detection is performed on the new node. If a collision occurs, considering that sampling was performed in step S21 in the previous round of sampling, but a collision occurred after the collision detection, resulting in p≥q, in order to avoid jumping to step S21 sampling again in the next round and causing repeated collision detection failures, p≥q is set, and sampling is still performed in the dynamic space. If no collision occurs, it means a new node closer to the target point has been added to the random tree. Therefore, let p < q, and let the target point X... goal As sampling point X sample Continue exploring the path towards the target point.

7. The remote puncture robot path planning method based on the improved RRT algorithm according to claim 5, characterized in that, In step S22, the dynamic sampling space sampling strategy means that the sampling space changes dynamically each time a sample is taken. The dynamic sampling space is located between the target point and the node in the random tree that is closest to the target point. As the random tree is updated, the sampling space is updated dynamically to ensure that the sampling point always grows in the direction of the target point.

8. The path planning method for a remote puncture robot based on the improved RRT algorithm according to claim 1, characterized in that, Step S5 also includes the following situations: No new node X satisfying the conditions was found until iter = iterMax. new If the path planning ends, then the path planning process is complete.

9. The path planning method for a remote puncture robot based on the improved RRT algorithm according to claim 1, characterized in that, The pruning process in step S6 specifically includes the following steps: S61. Add the starting point to the new path and set the starting point as the detection point; S62. Determine whether there is a collision between it and the second point in the initial path. If there is no collision, continue to check whether there is a collision between the starting point and the third point, and so on, until a collision occurs. S63. Add a node that occurred before the collision to the new path and set that node as a detection point; S64. Repeat steps S62 and S63 until the target point X is detected. goal Until then, a new path has been obtained.

Images