Three-dimensional path planning method, device, equipment and storage medium

Abstract

This invention discloses a three-dimensional path planning method, apparatus, device, and storage medium, applied in the field of medical needle trajectory planning technology. The method includes: obtaining the puncture target position and insertion position of a hard needle; determining a target area based on the insertion position; and determining an initial area based on the puncture target position and insertion position; planning an initial search path based on the puncture target position and initial area to obtain initial search nodes; expanding the initial search nodes to determine the search areas corresponding to state nodes; planning a target search path based on the initial search path and search area; determining whether the target search path reaches the target area and obtaining a determination result; if the determination result indicates that the target search path has reached the target area, using the target search path as the three-dimensional path planning for hard needle puncture. The scheme adopted in this invention can greatly improve the success rate of puncture path planning and reduce harm to the human body.

Description

Technical Field

[0001] This invention relates to the field of medical acupuncture trajectory planning technology, and in particular to a three-dimensional path planning method, apparatus, device, and storage medium. Background Technology

[0002] Currently, the puncture procedure relies on surgeons manually planning approximate puncture data based on their experience and anatomical observations. During the procedure, scanners are continuously used to reposition the needle through repeated trials and adjustments until the target location is reached. This process requires repeated scanning of the patient, and the needle remains inside the body for an extended period. If the puncture is unsuccessful, it must be repeated, causing significant harm. Therefore, designing and planning an accurate puncture path is crucial for puncture surgery. Currently, there is no effective solution to this problem. Summary of the Invention

[0003] In view of this, embodiments of the present invention aim to provide a three-dimensional path planning method, apparatus, device, and storage medium.

[0004] The technical embodiments of this invention are implemented as follows:

[0005] This invention provides a three-dimensional path planning method applied to scenarios including hard needle puncture, the method comprising:

[0006] The target puncture location and insertion location of the hard needle are obtained, the target area is determined based on the insertion location, and the initial area is determined based on the target puncture location and the insertion location.

[0007] Based on the puncture target location and the initial area, an initial search path is planned to obtain an initial search node; the initial search node is expanded to determine the search area corresponding to the status node;

[0008] Plan the target search path based on the initial search path and the search area;

[0009] Determine whether the target search path reaches the target area and obtain the determination result; if the determination result shows that the target search path reaches the target area, use the target search path as the three-dimensional path planning for hard needle puncture.

[0010] In the above scheme, the method further includes:

[0011] If the determination result indicates that the target search path has not reached the target area, the status node is expanded in a preset direction to determine the expansion area corresponding to the new node; the preset direction represents the direction in which the status node faces the target area.

[0012] Plan a new search path based on the target search path and the expanded area;

[0013] Determine whether the new search path reaches the target area. If the new search path reaches the target area, use the new search path as the three-dimensional path planning for hard needle puncture.

[0014] In the above scheme, determining the initial area based on the puncture target location and the insertion location includes:

[0015] Obtain the initial puncture angle and initial puncture step length;

[0016] The cone is determined based on the puncture target location, the puncture position, the initial puncture angle, and the initial puncture step length;

[0017] The bottom region of the cone is taken as the initial region.

[0018] In the above scheme, the step of planning an initial search path and obtaining an initial search node based on the puncture target location and the initial area includes:

[0019] The puncture target location is used as the root node of the extended tree;

[0020] Based on the root node and the initial region, a feasible path is planned using the Random Tree Search (RRT) algorithm.

[0021] The optimal path among the feasible paths is determined based on the A* algorithm;

[0022] Determine whether the optimal path successfully avoids the obstacle; if the optimal path successfully avoids the obstacle, use the optimal path as the initial search path;

[0023] The endpoints of the initial search path in the initial region are used as the initial search nodes.

[0024] In the above scheme, expanding the initial search node to determine the search area corresponding to the state node includes:

[0025] The initial search node is expanded using the Random Tree Search (RRT) algorithm to obtain multiple random state nodes;

[0026] Multiple state nodes are determined based on the initial search node and each of the multiple random state nodes;

[0027] The optimal state node among the multiple state nodes is determined according to the A* algorithm; if the straight line of motion between the initial search node and the optimal state node is a collision-free path, the first puncture step length is determined according to the position information of the initial search node and the position information of the optimal state node.

[0028] Obtain the constraint range of the hard needle puncture angle; determine the first puncture angle of the hard needle at the optimal state node based on the hard needle puncture posture information of the initial search node and the constraint range;

[0029] The first cone is determined based on the initial search node, the optimal state node, the first puncture step size, and the first puncture angle.

[0030] The bottom region of the first cone is used as the search area.

[0031] In the above scheme, the method further includes:

[0032] If the straight line of motion between the initial search node and the optimal state node is not a collision-free path, a first state node is obtained; the first state node is any state node other than the optimal state node among the plurality of state nodes.

[0033] The second puncture step length of the first state node is determined based on the position information of the first state node and the position information of the optimal state node.

[0034] Based on the hard needle puncture posture information of the initial search node and the constraint range, the first state node is updated, and the second puncture angle of the hard needle at the updated first state node is determined.

[0035] The second cone is determined based on the updated first state node, the second puncture step size, and the second puncture angle.

[0036] The bottom region of the second cone is used as the search area.

[0037] In the above scheme, the step of planning the target search path based on the initial search path and the search area includes:

[0038] Based on the initial search path and the search area, determine the target node in the search area;

[0039] Plan the target search path based on the initial search path and the target node.

[0040] In the above scheme, determining the target node in the search area based on the initial search path and the search area includes:

[0041] The A* algorithm's evaluation function is used to estimate the path estimate between each state node in the search region and the initial search path;

[0042] The state node corresponding to the minimum value of the path estimate is determined as the target node.

[0043] This invention provides a three-dimensional path planning device applied to scenarios including hard needle puncture. The device includes: an acquisition unit, a determination unit, a planning unit, and a judgment unit, wherein:

[0044] The obtaining unit is used to obtain the puncture target position and the insertion position of the hard needle, determine the target area based on the insertion position, and determine the initial area based on the puncture target position and the insertion position.

[0045] The determining unit is used to plan an initial search path based on the puncture target location and the initial area, obtain an initial search node, and expand the initial search node to determine the search area corresponding to the state node;

[0046] The planning unit is used to plan a target search path based on the initial search path and the search area;

[0047] The judgment unit is used to determine whether the target search path reaches the target area and obtain a judgment result; if the judgment result shows that the target search path reaches the target area, the target search path is used as a three-dimensional path planning for hard needle puncture.

[0048] In the above scheme, the judgment unit is further configured to, when the judgment result indicates that the target search path has not reached the target area, expand the status node in a preset direction to determine the expansion area corresponding to the new node; the preset direction represents the direction of the status node toward the target area; plan a new search path according to the target search path and the expansion area; determine whether the new search path reaches the target area, and if the new search path reaches the target area, use the new search path as the three-dimensional path planning for hard needle puncture.

[0049] In the above scheme, the obtaining unit is further used to obtain the initial puncture angle and the initial puncture step length; determine the cone according to the puncture target position, the puncture position, the initial puncture angle and the initial puncture step length; and take the bottom region of the cone as the initial region.

[0050] In the above scheme, the obtaining unit is further configured to: use the puncture target location as the root node of the extended tree; plan a feasible path based on the root node and the initial region using the Random Tree Search (RRT) algorithm; determine the optimal path among the feasible paths according to the A* algorithm; determine whether the optimal path successfully avoids obstacles; if the optimal path successfully avoids obstacles, use the optimal path as the initial search path; and use the endpoint of the initial search path in the initial region as the initial search node.

[0051] In the above scheme, the determining unit is further configured to expand the initial search node according to the Random Tree Search (RRT) algorithm to obtain multiple random state nodes; determine multiple state nodes based on the initial search node and each of the multiple random state nodes; determine the optimal state node among the multiple state nodes according to the A* algorithm; determine a first puncture step length based on the position information of the initial search node and the position information of the optimal state node when the straight line of motion between the initial search node and the optimal state node is a collision-free path; obtain the constraint range of the hard needle puncture angle; determine the first puncture angle of the hard needle at the optimal state node based on the hard needle puncture posture information of the initial search node and the constraint range; determine a first cone based on the initial search node, the optimal state node, the first puncture step length, and the first puncture angle; and use the bottom region of the first cone as the search region.

[0052] In the above scheme, the obtaining unit is further configured to obtain a first state node when the straight line of motion between the initial search node and the optimal state node is not a collision-free path; the first state node is any state node other than the optimal state node among the plurality of state nodes.

[0053] The determining unit is further configured to: determine the second puncture step length of the first state node based on the position information of the first state node and the position information of the optimal state node; update the first state node based on the hard needle puncture posture information of the initial search node and the constraint range; determine the second puncture angle of the hard needle at the updated first state node; determine the second cone based on the updated first state node, the second puncture step length, and the second puncture angle; and use the bottom region of the second cone as the search region.

[0054] In the above scheme, the planning unit is further configured to determine the target node in the search area based on the initial search path and the search area; and plan the target search path according to the initial search path and the target node.

[0055] In the above scheme, the planning unit is further configured to use the evaluation function of the A* algorithm to estimate the path estimate of each state node in the search area and the initial search path; and determine the state node corresponding to the minimum value of the path estimate as the target node.

[0056] This invention provides a three-dimensional path planning device, including a memory and a processor. The memory stores a program that can run on the processor, and the processor executes the program to implement any step of the method described above.

[0057] This invention provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements any step of the method described above.

[0058] The three-dimensional path planning method, apparatus, device, and storage medium provided in this invention include: obtaining an initial search node by planning an initial search path based on the puncture target location and the initial region; expanding the initial search node to determine the search region corresponding to the state node; planning a target search path based on the initial search path and the search region; determining whether the target search path reaches the target region and obtaining a determination result; and if the determination result indicates that the target search path has reached the target region, using the target search path as the three-dimensional path planning for hard needle puncture. By employing the technical solution of this invention, the success rate of puncture paths is greatly improved and the harm to the human body is reduced through the design and planning of accurate puncture paths. Attached Figure Description

[0059] Figure 1 This is a schematic diagram illustrating the implementation process of the three-dimensional path planning method according to an embodiment of the present invention;

[0060] Figure 2 This is a schematic diagram of the puncture target position and the insertion position in the three-dimensional path planning method of this invention.

[0061] Figure 3 This is a schematic diagram of the initial region in the three-dimensional path planning method of this invention.

[0062] Figure 4 This is a schematic diagram of the planning process in the three-dimensional path planning method of this invention.

[0063] Figure 5 This is domain A of the three-dimensional path planning method in the embodiments of the present invention. A schematic diagram of the algorithm;

[0064] Figure 6 This is a schematic diagram of the improved RRT algorithm in the three-dimensional path planning method of this invention.

[0065] Figure 7 This is a schematic diagram illustrating an application scenario of hard needle puncture in the three-dimensional path planning method of this invention.

[0066] Figure 8 This is a schematic diagram of the composition structure of the three-dimensional path planning device according to an embodiment of the present invention;

[0067] Figure 9 This is a schematic diagram of the hardware entity structure of a three-dimensional path planning device in an embodiment of the present invention. Detailed Implementation

[0068] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the specific technical solutions of the invention will be further described in detail below with reference to the accompanying drawings of the embodiments of the present invention. The following embodiments are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0069] This embodiment proposes a three-dimensional path planning method. Figure 1 This is a schematic diagram illustrating the implementation process of the three-dimensional path planning method according to an embodiment of the present invention, as shown below. Figure 1 As shown, the method includes:

[0070] Step S101: Obtain the puncture target position and insertion position of the hard needle, determine the target area based on the insertion position, and determine the initial area based on the puncture target position and the insertion position.

[0071] It should be noted that obtaining the puncture target location of the hard needle can refer to obtaining the puncture target location of the hard needle on the patient. In practical applications, the puncture target location can be the puncture target point location, or simply the puncture target point or target point; as an example, the puncture target point can be denoted as T. Obtaining the insertion location of the hard needle can be achieved by using the puncture target location of the hard needle as a known condition, and calculating the insertion location in reverse based on the reachable area of ​​the hard needle, thus obtaining two critical points for the insertion location. Any position between these two critical points can be considered the insertion location; as an example, the insertion location can be simply referred to as the insertion point or punctureable point, and the two critical points can be denoted as point1 and point2, respectively. For ease of understanding, an example is provided here. Figure 2 This is a schematic diagram of the puncture target position and insertion position in the three-dimensional path planning method of this invention. For the puncture target T, the two critical points that the puncture needle tip can puncture are point1 and point2. If the puncture point is not between point1 and point2, the puncture target T cannot be reached.

[0072] Determining the target area based on the insertion position can be achieved by forming a curved area with the insertion position as the center and a preset length as the radius, and using this curved area as the target area. In practical applications, the target area can be assumed to be... , It is an irregular curved area on the skin with a radius of a certain length centered on a puncture point.

[0073] The initial region determined based on the puncture target location and the insertion location can be a cone drawn with the puncture target location as the vertex and the center line connecting the puncture target location and the insertion location as the center line. The cone's base serves as the initial region. The preset length and preset angle can be determined based on actual conditions and are not limited here. As an example, the preset length can be a step size, denoted as h, where h can be determined based on the spatial grid units used in the actual puncture surgery; the preset angle can be denoted as 2. , It can be kept within 10 degrees Celsius.

[0074] Step S102: Plan an initial search path based on the puncture target location and the initial area to obtain an initial search node; expand the initial search node to determine the search area corresponding to the status node.

[0075] Here, planning the initial search path based on the puncture target location and the initial area can be achieved by using a preset algorithm to plan the initial search path between the puncture target location and the initial area. The preset algorithm can be determined according to the actual situation and is not limited here. As an example, the preset algorithm can be the Random Tree Search (RRT) algorithm and the A* algorithm. In practical applications, the initial search path can be called the first workspace planning path, and can be denoted as L1. Obtaining the initial search node can be achieved by referring to the endpoint of the initial search path in the initial area as the initial search node; the initial search node can be denoted as P11. In practical applications, the initial search node can also be called the end node of the first puncture path.

[0076] Expanding the initial search node to determine the search area corresponding to the state node can be achieved by expanding the initial search node according to the improved Random Search Tree (RRT) algorithm.

[0077] Step S103: Plan the target search path based on the initial search path and the search area.

[0078] It should be noted that planning a target search path based on the initial search path and the search area can mean determining the target node in the search area based on the initial search path and the search area; and planning the target search path based on the initial search path and the target node. In practical applications, the initial search path can be called the first workspace planning path; the target node can be a node in the second workspace; planning the target search path based on the initial search path and the target node can mean planning a second workspace planning path based on the first workspace planning path and the target node, and determining the target search path based on the first workspace planning path and the second workspace planning path.

[0079] Step S104: Determine whether the target search path reaches the target area and obtain the determination result; if the determination result shows that the target search path reaches the target area, use the target search path as the three-dimensional path planning for hard needle puncture.

[0080] Here, determining whether the target search path reaches the target area can be achieved by: obtaining the endpoint of the target search path; determining the distance between the endpoint of the target search path and the piercing position; obtaining a preset threshold based on the target area; determining whether the distance is less than or equal to the preset threshold; if the distance is less than or equal to the preset threshold, obtaining a determination that the target search path has reached the target area; if the distance is greater than the preset threshold, obtaining a determination that the target search path has not reached the target area. In practical applications, the endpoint of the target search path can also be called the endpoint of the piercing planning path; the piercing position can also be called the target endpoint; the distance between the endpoint of the target search path and the piercing position can be denoted as d; the preset threshold can be determined according to the actual situation and is not limited here. As an example, the preset threshold can be a pre-set threshold, which can be denoted as... Determining whether the target search path reaches the target area can be understood as confirming whether the currently planned puncture route has reached the target area (puncture needle insertion area). This can be achieved by calculating the distance d between the endpoint of the planned puncture path and the target endpoint, and using a pre-set threshold. A comparison is made to determine whether the planned path has reached the target center. If the d value is higher than... If the value is small, the current puncture path has successfully reached the target area, the puncture path planning is complete, and the result of the current puncture path successfully reaching the target area is obtained. If d is greater than... If the planned puncture needle trajectory does not reach the target area, then the result is that the currently searched puncture path has not successfully reached the target area.

[0081] If the judgment result indicates that the target search path has reached the target area, using the target search path as the three-dimensional path planning for hard needle puncture can be understood as the current searched puncture path successfully reaching the target area, the puncture path planning is completed, and the target search path is the three-dimensional path planning for hard needle puncture.

[0082] The three-dimensional path planning method provided in this invention greatly improves the success rate of puncture paths and reduces harm to the human body by designing and planning accurate puncture paths.

[0083] In an optional embodiment of the present invention, the method further includes: if the determination result indicates that the target search path has not reached the target area, expanding the status node in a preset direction to determine the expansion area corresponding to the new node; the preset direction represents the direction in which the status node faces the target area; planning a new search path based on the target search path and the expansion area; determining whether the new search path reaches the target area, and if the new search path reaches the target area, using the new search path as a three-dimensional path for hard needle puncture.

[0084] In this embodiment, the preset direction, representing the direction in which the state node faces the target region, can be understood as the direction that guides the state node towards the target region. In practical applications, the preset direction can be implemented using a direction guidance function. This direction guidance function can be understood as incorporating a direction guidance function related to the node expansion direction and path search length into the selection factors of the next node. This direction guidance function can be denoted as... .

[0085] If the judgment result indicates that the target search path has not reached the target area, the state node can be expanded in a preset direction to determine the expansion area corresponding to the new node. This can be achieved by expanding the state node using a direction guidance function to determine the expansion area corresponding to the new node.

[0086] In an optional embodiment of the present invention, determining the initial region based on the puncture target position and the insertion position includes: obtaining an initial puncture angle and an initial puncture step length; determining a cone based on the puncture target position, the insertion position, the initial puncture angle, and the initial puncture step length; and using the bottom region of the cone as the initial region.

[0087] In this embodiment, the initial puncture angle can be denoted as: The The value can be determined according to the actual situation, and is not limited here. As an example, the stated value is... The value can be within 10 degrees. The initial puncture step length can be denoted as h, and the value of h can be determined according to the actual situation. It is not limited here. As an example, h can be determined according to the spatial grid units in the actual puncture operation.

[0088] Based on the puncture target position, the insertion position, the initial puncture angle, and the initial puncture step length, the cone can be determined to be a cone with the puncture target position as the vertex and the center line connecting the puncture target position and the insertion position as the center line, with a height equal to the initial puncture step length and a vertex angle of twice the initial puncture angle.

[0089] Using the bottom region of the cone as the initial region can mean using the conical surface at the bottom of the cone as the initial region. In practical applications, the initial region can also be understood as the region of the first search node in the expanded tree.

[0090] For ease of understanding, Figure 3 This is a schematic diagram of the initial region in the three-dimensional path planning method of this invention, as shown in the embodiment. Figure 3 As shown, point P1 is the puncture target position, point P2 is the puncture position, twice the initial puncture angle is the apex angle of the cone, and the initial region is the bottom region of the cone.

[0091] In an optional embodiment of the present invention, the step of planning an initial search path and obtaining an initial search node based on the puncture target location and the initial region includes: using the puncture target location as the root node of the extended tree; planning a feasible path based on the root node and the initial region using the Random Tree Search (RRT) algorithm; determining the optimal path among the feasible paths using the A* algorithm; determining whether the optimal path successfully avoids obstacles; if the optimal path successfully avoids obstacles, using the optimal path as the initial search path; and using the endpoint of the initial search path in the initial region as the initial search node.

[0092] In this embodiment, planning a feasible path based on the root node and the initial region using the Random Tree Search (RRT) algorithm can be performed by searching the root node using the RRT algorithm, and planning is carried out after the search tree expands to a point in the initial region to obtain a feasible path.

[0093] Determining the optimal path among the feasible paths using the A* algorithm can be achieved by using the A* algorithm's evaluation function. The evaluation function can be determined based on the specific circumstances and is not limited here. As an example, the evaluation function could be... , This represents the total cost from node n to the target point. This represents the actual cost along the path from the root node to the current path node n. This represents the estimated cost of the path from node n to the target point. In a specific puncture needle task, It is the evaluation function of node n from the starting point (puncture target point) to the ending point (puncture and needle insertion area on the skin). This represents the cost from the root node to the current node n in the working environment. It is the estimated cost of the optimal route from node n to the terminal point.

[0094] Determining whether the optimal path successfully avoids obstacles can be done using a collision detection algorithm. The collision detection algorithm can be determined based on the actual situation and is not limited here. As an example, the collision detection algorithm can be the OBBTree bounding box tree detection algorithm. The obstacles can also be determined based on the actual situation and are not limited here. As an example, the obstacles can be vital organs or tissues.

[0095] For ease of understanding, an example is provided below. Figure 4 This is a schematic diagram of the planning process in the three-dimensional path planning method of this invention, which can be combined with... Figure 4 The process involves understanding the target area of ​​a puncture needle within the workspace by determining the position and state of a node at a given moment. Then, an improved RRT algorithm and the A* algorithm are used to attempt to find the optimal path. A collision detection algorithm (using the OBBTree bounding box tree detection algorithm in this paper) is used to determine if the path collides with obstacles (such as vital organs or tissues). If the currently planned path successfully avoids obstacles, it is determined whether the node has reached the target area. If not, the node is added to the expanded search tree; otherwise, the path planning process ends. If the currently planned puncture path collides with an obstacle, a suboptimal path is replanned, and it is determined whether this path avoids obstacles. This process is repeated until the process ends.

[0096] In an optional embodiment of the present invention, the step of expanding the initial search node to determine the search area corresponding to the state node includes: expanding the initial search node according to the Random Tree Search (RRT) algorithm to obtain multiple random state nodes; determining multiple state nodes based on the initial search node and each of the multiple random state nodes; determining the optimal state node among the multiple state nodes according to the A* algorithm; determining a first puncture step length based on the position information of the initial search node and the position information of the optimal state node when the straight line of motion between the initial search node and the optimal state node is a collision-free path; obtaining a constraint range for the hard needle puncture angle; determining a first puncture angle of the hard needle at the optimal state node based on the hard needle puncture posture information of the initial search node and the constraint range; determining a first cone based on the initial search node, the optimal state node, the first puncture step length, and the first puncture angle; and using the bottom region of the first cone as the search area.

[0097] In this embodiment, expanding the initial search node according to the Random Tree Search (RRT) algorithm to obtain multiple random state nodes can be described as randomly expanding the initial search node according to the RRT algorithm to obtain multiple random state nodes. In practical applications, the initial search node can be denoted as... The random state node can be denoted as... .

[0098] Determining multiple state nodes based on the initial search node and each of the plurality of random state nodes can be achieved by using a preset formula to determine multiple state nodes based on the initial search node and each of the plurality of random state nodes; wherein, the state nodes can be denoted as... The preset formula can be determined according to the actual situation, and is not limited here. As an example, the preset formula can refer to the following formula (1):

[0099] (1)

[0100] In equation (1), , To expand the horizontal and vertical coordinates of the node, h is the single-step exploration step length of the puncture needle.

[0101] Determining the optimal state node among the multiple state nodes using the A* algorithm can be achieved by selecting an appropriate evaluation function based on an improved A* algorithm. The evaluation function can be... , This represents the total cost from node n to the target point. This represents the actual cost along the path from the root node to the current path node n. This represents the estimated cost of the path from node n to the target point. In a specific puncture needle task, It is the evaluation function of node n from the starting point (puncture target point) to the ending point (puncture and needle insertion area on the skin). This represents the cost from the root node to the current node n in the working environment. It is the estimated cost of the optimal route from node n to the terminal point.

[0102] In practical applications, to reduce the randomness of the expansion method and ensure that the tree growth converges quickly towards the target point, the RRT algorithm is based on... The algorithm's core idea is to find the optimal path. During the construction of the extended tree, the number of search nodes in the extended tree is reduced, thus reducing the randomness of tree growth and making the planned piercing path close to the shortest path.

[0103] The first puncture step length can be determined based on the location information of the initial search node and the location information of the optimal state node, as shown in the following formulas (2) and (3):

[0104] (2)

[0105] (3)

[0106] In equation (2), and This represents the maximum and minimum step size values. In equation (3), This represents the position of the i-th state node. Let represent the position of the optimal state node, and si represent the error factor between the position of the i-th state node and the position of the optimal state node. This represents the maximum distance between the best state node and the other state nodes, and the iteration step size is updated based on the result of the previous iteration.

[0107] The constraint range for obtaining the hard needle puncture angle can be defined by constraint coefficients and constraint functions. The constraint coefficients and constraint functions can be determined based on actual circumstances and are not limited here. As an example, the constraint coefficients can be denoted as... The constraint function can be denoted as .

[0108] Determining the first puncture angle of the hard needle at the optimal state node based on the hard needle puncture posture information of the initial search node and the constraint range can be achieved by determining the first puncture angle of the hard needle at the optimal state node within the constraint range based on the hard needle puncture posture information of the initial search node. The posture information may include position information and state information; as an example, the state information may be the puncture needle orientation angle at the previous moment, which can be denoted as... The constraint range can be a puncture angle constraint function, for example, , The smaller the value, the smaller the deflection angle, and the fewer the turning points in the global path, thus determining the direction. In the case of minimum value This is the first puncture angle.

[0109] Based on the initial search node, the optimal state node, the first puncture step size, and the first puncture angle, the first cone can be determined by taking the initial search node as the vertex and the line connecting the initial search node and the optimal state node as the center line, and drawing a cone with a height of the first puncture step size and a vertex angle of twice the first puncture angle.

[0110] Using the bottom region of the first cone as the search region can be understood as using the bottom surface region of the first cone as the search region.

[0111] In an optional embodiment of the present invention, the method further includes: obtaining a first state node when the straight line of motion between the initial search node and the optimal state node is not a collision-free path; the first state node is any state node other than the optimal state node among the plurality of state nodes; determining a second puncture step size of the first state node based on the position information of the first state node and the position information of the optimal state node; updating the first state node based on the hard needle puncture posture information of the initial search node and the constraint range, and determining a second puncture angle of the hard needle at the updated first state node; determining a second cone based on the updated first state node, the second puncture step size, and the second puncture angle; and using the bottom region of the second cone as the search region.

[0112] In this embodiment, when the straight line of motion between the initial search node and the optimal state node is not a collision-free path, obtaining the first state node can be achieved by obtaining an extended state node at a certain moment when the straight line of motion between the initial search node and the optimal state node is not a collision-free path. For ease of understanding, the extended state node at a certain moment can be denoted as... As can be referred to in the previous formula (1), the first state node is any state node other than the best state node among the plurality of state nodes.

[0113] Determining the second puncture step size of the first state node based on the position information of the first state node and the position information of the optimal state node can be achieved by determining an error factor between the first state node and the optimal state node based on the position information of the first state node and the optimal state node, and then determining the second puncture step size of the first state node based on the error factor. For ease of understanding, an example is provided here, where the position information of the first state node can be denoted as... The location information of the optimal state node can be denoted as: The error factor can be denoted as The second puncture step length can be denoted as... The process of determining the second puncture step length can be referred to the previous formulas (2) and (3).

[0114] Based on the hard needle puncture posture information of the initial search node and the constraint range, the first state node is updated, and the second puncture angle of the hard needle at the updated first state node is determined. Alternatively, the first state node can be modified based on the hard needle puncture posture information of the initial search node and the constraint range, and the second puncture angle of the hard needle at the modified first state node can be determined. For ease of understanding, the second puncture angle can be denoted as... .

[0115] Based on the updated first state node, the second puncture step size, and the second puncture angle, the second cone can be defined as a cone with the initial search node as its vertex, the line connecting the initial search node and the updated first state node as its center line, twice the second puncture angle as its apex angle, and a height equal to the second puncture step size. For ease of understanding, the updated first state node can be denoted as... .

[0116] For ease of understanding, an example is given here, in equation (1), For the random points generated during this iteration The coordinates can be represented by a vector, which can be referred to by the following formula (4):

[0117] (4)

[0118] In equation (4), for arrive displacement, for arrive The displacement, in order to reduce the state nodes of the puncture needle. For random points Dependency, introducing vectors For state nodes Modify the puncture path to meet the requirements of the next iteration. The specific calculation process can be found in the following formulas (5), (6), (7), (8), and (9):

[0119] (5)

[0120] (6)

[0121] (7)

[0122] (8)

[0123] In equation (5), for arrive displacement, for arrive The displacement, m is arrive The Manhattan distance between them; in equation (6), t is the adjustment coefficient, It is the angle constraint coefficient; in equation (8), This is the modified state node.

[0124] (9)

[0125] Equation (9) is the puncture angle constraint function. The current angle of the puncture needle. It is the angle in which the puncture needle was pointing at the previous moment. The smaller the value, the smaller the deflection angle, and the fewer the turning points in the global path.

[0126] By modifying the state node growth mechanism, the Fast Search Random Tree (RRT) algorithm can be well applied to puncture path planning. Furthermore, based on the RRT algorithm, an improved A algorithm is incorporated during path planning. The pathfinding approach improves search efficiency, ensuring that the planned path is a suboptimal path close to the optimal one. In practical puncture testing, a region control function is proposed. The control function for this region can be referred to by the following formula (10):

[0127] (10)

[0128] In equation (10), and Indicates the coordinates of the center of the control area. Let be the radius of the base.

[0129] By estimating the cost from multiple random sampling points to the target region (target point), the optimal state node is selected each time and added to the random expansion tree, i.e., obtained from equation (8). When equation (10) is satisfied, i.e. If the iteration is successful, the node can be added. Then proceed to the next workspace puncture path planning (otherwise, start from a random point again). Generative formula (1) until the target area (target point) is finally reached; at the same time, the present invention improves the control of the expansion direction. In view of the problem that the randomness of the search direction in the RRT algorithm causes the search to easily deviate from the target area (target point), considering the actual piercing situation, in order to make the planned path more in line with the actual piercing needs, the search direction of the expanded tree node should be controlled within a certain angle, and there should be no large detours. In addition, the problem of local minima should be avoided.

[0130] In an optional embodiment of the present invention, the step of planning a target search path based on the initial search path and the search area includes: determining a target node in the search area based on the initial search path and the search area; and planning a target search path according to the initial search path and the target node.

[0131] In this embodiment, planning the target search path based on the initial search path and the target node can be achieved by connecting the end node of the initial search path with the target node to determine the path between the end node of the initial search path and the target node, and then determining the target search path from the start node of the initial search path to the target node.

[0132] In an optional embodiment of the present invention, determining the target node in the search area based on the initial search path and the search area includes: estimating the path estimate of each state node in the search area and the initial search path using the evaluation function of the A* algorithm; and determining the state node corresponding to the minimum value of the path estimate as the target node.

[0133] In this embodiment, the estimation of the valuation function can be determined based on actual circumstances. As an example, the estimation of the valuation function can be... , This represents the total cost from node n to the target point. This represents the actual cost along the path from the root node to the current path node n. This represents the estimated cost of the path from node n to the target point. In a specific puncture needle task, It is the evaluation function of node n from the starting point (puncture target point) to the ending point (puncture and needle insertion area on the skin). This represents the cost from the root node to the current node n in the working environment. It is the estimated cost of the optimal route from node n to the terminal point.

[0134] Determining the state node corresponding to the minimum value of the path estimate as the target node can be done by selecting the minimum value of the path estimate from the path estimates and setting the state node corresponding to the minimum value as the target node.

[0135] The three-dimensional path planning method provided in this invention combines the idea of ​​an improved fast search random tree algorithm with the application scenario of puncture path planning. Based on the principles and construction method of the RRT algorithm, it modifies the growth method and control expansion direction of state nodes, and adds an improved A The approach involves pathfinding and avoiding local minima during the search process.

[0136] For ease of understanding, here is an example of a 3D path planning method based on an improved RRT algorithm and A. Algorithm for three-dimensional path planning

[0137] Rapid-exploration Random Tree (RRT) is a commonly used path planning algorithm for robots. It primarily addresses how to autonomously explore a path from a given starting point to a suitable endpoint in an obstacle-filled space, safely and without collisions by using a globally optimized path. RRT can search for feasible paths from the root node to the endpoint that avoid obstacles. However, the basic RRT algorithm does not consider the actual puncture situation, and the searched path does not conform to the puncture trajectory of the needle. The RRT algorithm exhibits significant randomness and blindness in the selection of sampling points during the search process, resulting in a large discrepancy between the planned path and the optimal path. Furthermore, the search results obtained directly cannot be applied to the puncture needle operation; it is necessary to optimize the search results to obtain a near-straight puncture route. To address the randomness and blindness of the RRT algorithm, improvements are made to the basic algorithm to meet the actual requirements of puncture.

[0138] This invention combines the idea of ​​an improved fast search random tree algorithm with the application of puncture path planning. Based on the principles and construction method of the RRT algorithm, it modifies the growth method and control of the expansion direction of the state nodes, and adds an improved A The pathfinding idea is to avoid local minima during the search process. The principle of the RRT algorithm is based on the previous formulas (1) to (10), and will not be repeated here.

[0139] By estimating the cost from multiple random sampling points to the target region (target point), the optimal state node is selected each time and added to the random expansion tree, which is obtained from the previous formula (8). When the above formula (10) is satisfied, that is... If the iteration is successful, the node can be added. Then proceed to the next workspace puncture path planning (otherwise, start from a random point again). Generative formula (1) until the target area (target point) is finally reached; at the same time, the present invention improves the control of the expansion direction. In view of the problem that the randomness of the search direction in the RRT algorithm causes the search to easily deviate from the target area (target point), considering the actual piercing situation, in order to make the planned path more in line with the actual piercing needs, the search direction of the expanded tree node should be controlled within a certain angle, and there should be no large detours. In addition, the problem of local minima should be avoided.

[0140] The three-dimensional trajectory planning process of the puncture needle is as follows:

[0141] Currently, puncture surgery cannot determine the puncture area and relies solely on the surgeon's experience. The three-dimensional trajectory planning of the puncture needle also includes the selection of the epidermal insertion point. The selection of the needle tip insertion point can be viewed as the inverse problem of solving the reachable range of the puncture needle tip. With the target location as known, the area of ​​the needle tip insertion point can be calculated inversely based on the reachable area of ​​the puncture needle. For example... Figure 2 As shown, for the target point T, the two critical points that the puncture needle tip can penetrate are point1 and point2. If the puncture point is not between point1 and point2, the target point T cannot be reached.

[0142] The overall path planning task for puncture needles mainly involves the following four steps.

[0143] 1. Determine the first search node region. Discretize the entire search working area into a three-dimensional mesh space, assuming the target region is... , This refers to an irregular curved surface region on the skin with a radius of a certain length, centered on a puncture point. The process then begins by searching and acquiring puncture path information within the three-dimensional working area. The search is performed with the puncture target point as the root node of an expanded search tree. Once the search tree expands to a certain point, planning is performed, as shown in the flowchart below. Figure 4 As shown. The first state node of the puncture path is selected on the straight line between the puncture termination point and the root node. Within a 10-degree range, the distance to the target point is one step size h (h is determined based on the spatial grid unit in the actual puncture surgery). For example... Figure 3 As shown, with the target point P1 as the vertex and the line connecting the target point and the puncture starting point P2 as the center line, a boundary is drawn with a height of one step and a vertex angle of 2. The cone-shaped area at its base (light-colored) forms the region of the first search node in the expanded tree. For example... Figure 4 As shown, if each workspace node can be successfully added (i.e., equation (8) satisfies) If the target area or target point (the initial needle insertion point on the skin) is not found, the current node is discarded and the planning continues from the parent node of the current tree node until a feasible puncture path is planned, thus completing the planning from the initial point to the target area.

[0144] exist Figure 4 In this process, the reachable area of ​​the puncture needle in the workspace can be determined by the position and state of a certain state node at a certain moment. Then, the improved RRT algorithm and A* algorithm are used to try to find the optimal path, and a collision detection algorithm (OBBTree bounding box tree detection algorithm is used in this paper) is used to determine whether the path collides with obstacles (such as important organs and tissues). If the currently planned path successfully avoids obstacles, it is determined whether the node has reached the target area. If not, the node is added to the expanded search tree; otherwise, the path planning process ends. If the currently planned puncture path collides with an obstacle, a suboptimal path is replanned, and it is determined whether the path avoids obstacles. The above steps are repeated until the process ends.

[0145] 2. Plan the search path for the first workspace. In this paper, to reduce the randomness of the expansion method and ensure that the tree growth converges quickly towards the target point, the RRT algorithm is based on... The algorithm's core idea is to find the optimal path. During the construction of the extended tree, the number of search nodes in the extended tree is reduced, thus reducing the randomness of tree growth and making the planned piercing path close to the shortest path. The algorithm is a typical heuristic algorithm that searches for the optimal path in practical applications by selecting an appropriate evaluation function. The algorithm's evaluation function is , This represents the total cost from node n to the target point. This represents the actual cost along the path from the root node to the current path node n. This represents the estimated cost of the path from node n to the target point. In a specific puncture needle task, It is the evaluation function of node n from the starting point (puncture target point) to the ending point (puncture and needle insertion area on the skin). This represents the cost from the root node to the current node n in the working environment. It is the estimated cost of the optimal route from node n to the terminal point.

[0146] The following introduces the introduction of A into the search space. The pathfinding approach, by optimizing the storage of the Open table and the direction of node search, improves the algorithm's efficiency and solves for the shortest path. This is achieved through the evaluation function. It can be seen The choice of path determines the result of finding the optimal path. In order to better apply A... The algorithm first needs to increase the search direction; the traditional A algorithm... Algorithm 8's neighborhood expansion only allows movement up, down, left, and right, and cannot obtain the optimal path. In a 3D working environment, to better plan the path, the search space is expanded to 26 neighborhoods. During the search process, unvisited nodes to be examined are added to the expanded list Open (as shown in step 4 below), and the node f-values ​​are sorted using a min-heap storage method to reduce the time cost of traversing nodes and speed up convergence. The specific process is as follows:

[0147] (1) Establish Open and Close lists, put obstacle points into Close list, and start point into Open list. Check the input adjacent points to determine that they are within the working environment range and use min-heap storage structure.

[0148] (2) For the first-level neighborhood points (1-8 neighborhood), check whether the adjacent points are in the Close list or whether they are obstacle points. If they are not obstacle points, they can be expanded. Divide and conquer the Open list. If the length of the Open list is not 0, use the min-heap storage divide and conquer method to find the minimum cost node M, remove the node from the Open list and insert it into the Close list, and update the parent node of the adjacent points as appropriate. If the Open list does not contain this point, use the open_insert method to add this adjacent point to the Open list.

[0149] (3) For all nodes in the second layer (9-26 neighborhood), determine whether the point is an obstacle point. If it is not an obstacle point, check whether the path from the starting point to this adjacent point passes through an obstacle point. If there is an obstacle point, discard the adjacent point. If the path is a free area, check whether the adjacent point exists in the Open table. If the length of the Open table is not 0, divide and conquer the Open table and use a min-heap to find the minimum cost node M. If the point does not exist in the Open table, use the open_insert method to add the adjacent point to the Open table.

[0150] (4) Perform layer-by-layer judgment on the adjacent points (all adjacent points in the two layers centered on the input point), and determine whether the 26 adjacent points of the node to be expanded x can be added to the Open table of the expanded node, that is, include the non-obstacle points or free area points along the way into the expanded node, so as to ensure that the 26 neighborhoods centered on the input point are covered.

[0151] (5) The node with the smallest f-value in the Open table is selected as the optimal node and added to the Close table using the min-heap divide-and-conquer method;

[0152] (6) If the minimum cost node M is the target node, it means that the path finding is successful. Backtrack from the best node to the original node and exit. Otherwise, jump to step (2).

[0153] By analyzing A The algorithm is improved, increasing the number of selectable neighbors from 8 to 26 during each node expansion, adding more continuous search directions. Simultaneously, it improves efficiency by using a min-heap for divide-and-conquer storage. In the algorithm design, the sub-function `open_insert()` inserts selectable nodes into the `Open` list, `min_f()` represents the node with the smallest value in the `Open` list, `Delete()` removes the node from the `Open` list and adds it to the `Close` list, `T(M)` represents the state of node M, and `b(N) = M` indicates that the forward pointer of node M is node N. For ease of understanding, Figure 5 This is domain A of the three-dimensional path planning method in the embodiments of the present invention. The algorithm diagram shows that by inputting an initial input point and a target area, it can output a planned path from the initial point to the destination.

[0154] In the three-dimensional search space, when planning the search path using the RRT algorithm, new leaf nodes are added. Introducing an improved A method The algorithm (i.e., expanding the 26-neighborhood and using an open table to divide and conquer to find the minimum-cost node) selects each new node by calculating the estimated value from each leaf node to the next state node. During the tree expansion, random sampling points are used. A series of temporary candidate nodes can be calculated. And store it in the Open table, and calculate it according to formula (11).

[0155] (11)

[0156] in, It is the last new leaf node added in the current expanded tree, i.e. The parent node. The node with the lowest estimated cost to the next state node is added to the Close table. For state node n, if obtained from equation (8)... If equation (10) is satisfied, the node is added to the extended tree for the next spatial path planning.

[0157] 3. Determine if the planned puncture route has reached the target area (needle insertion area). This is done by calculating the distance d between the endpoint of the planned puncture path and the target endpoint, and using a pre-set threshold. A comparison is made to determine whether the planned path has reached the target center. If the d value is higher than... If the value is small, the currently searched puncture path has successfully reached the target area, and the puncture path planning is complete. If d is greater than... If the planned puncture needle trajectory does not reach the target area, proceed to step 4.

[0158] 4. After step 3, if the current puncture path has not yet reached the target area, an expanded tree path search is needed to determine if the endpoint has been reached, by adding a directional guidance function. To avoid local minima, the specific steps are as follows (a, b, c, d).

[0159] a. Continue searching for the next puncture path segment. Similar to the selection of the first path state node and the path search method, the first step is to determine the region space of the next extended search tree state node, i.e., the line connecting the end node of the first path segment and the target point (target region). Area within the range of degrees ( Figure 3 () represents the selection range for the next puncture path node.

[0160] b. Avoid local minima. Local minima can easily occur during path search. To address this, a directional guidance function related to the node expansion direction and path search length is added to the selection factors for the next node. (Formula (14)) While considering the expansion area and path length, the path with the smallest turning angle is always selected to reach the next node. Introducing this function into the RRT algorithm makes it always move towards the target area when expanding the tree. According to Formulas (12) and (13), when selecting a new node b, the path search node closest to the next state node is always selected.

[0161] (12)

[0162] (13)

[0163] Equation (14) middle, This is the direction guiding function. Let be the function influence coefficient, and . It is a function of angle and distance, reflecting the degree of vector correlation in choosing the direction of travel to the next node.

[0164] (14)

[0165] and The turning angles of the previous and next nodes. , Let be one side of the set of edges.

[0166] In this article, if inequalities If the condition is met, then the node with the largest possible value of b is selected into the solution constructed by the random tree according to equation (12); otherwise, the next node b is selected according to equation (13). This planned path avoids obstacles while also preventing local minima. When searching in the target direction, the planned path will extend as far as possible into unexplored open areas. For ease of understanding, the specific algorithm steps are as follows: Figure 6 As shown, Figure 6 This is a schematic diagram of the improved RRT algorithm in the three-dimensional path planning method of this invention; where the return value Success indicates that the target area has been reached. Failure indicates that the search for a new leaf node failed. c. Route search for the next workspace. The planning method for this space is consistent with that of the previous search space.

[0167] d. Determine if the currently planned puncture path has reached the target puncture area (puncture needle insertion area). If the target area has not been reached, return to step a above and replan the path. If the target area has been reached, the puncture path planning task is complete.

[0168] For ease of understanding, a schematic diagram illustrating an application scenario of hard needle puncture in the three-dimensional path planning method of this invention is provided below. Figure 7 This is a schematic diagram illustrating an application scenario of hard needle puncture in the three-dimensional path planning method of this invention, as shown in the embodiment of the invention. Figure 7 As shown, we need to plan an optimal puncture path from point T to a certain threshold interval of the target point P. We start planning from point p1, with L1 being the first segment of the workspace planning path and L2 being the second segment.

[0169] The first working interval is the angle between the starting point and the target point. Within an initial 10 degrees, draw a vertex with a radius of 2, at a distance of one step h (h is determined based on the spatial grid unit in the actual puncture surgery) from the starting node. A cone of degree, the base of which is the selected range of the last node of the first puncture path;

[0170] The puncture range of the nodes in the second working interval is similar to that of the first working interval. The last node of the first segment serves as the starting node for the second working interval, and the path is planned by calculating a series of random state nodes according to the RRT extended algorithm. If the motion lines between the starting point and the state node are collision-free, then the angle can be calculated. (Formula (9)) and step size h (Formula (2)) are used to draw a vertex angle of 2. The step length in the linear direction of motion is h, representing the height of the vertebra. The closer the position of the i-th node is to the optimal position, the smaller the step length; the farther the position of the i-th node is from the optimal position, the larger the step length. We should try to keep the value of formula (9) as small as possible; the smaller the value, the smaller the path deflection cost.

[0171] Within each workspace, extension nodes are selected based on RRT calculations. Formula (1) represents the coordinates of the extension nodes, determined by the region control function. If the node expression satisfies equation (10), then the node is added to the extended tree for the next spatial path search.

[0172] Introduce improvements during tree growth. Algorithm, adding new leaf nodes Introducing an improved A method The algorithm (i.e., expanding the 26-neighborhood and using an open table to divide and conquer to find the minimum cost node) uses random sampling points. A series of temporary candidate nodes can be calculated. And store it in the Open table (formula (11)), and add the node with the smallest cost estimate to the next state node to the Close table. For state nodes If obtained from equation (8) If equation (10) is satisfied, it can be added to the extension tree.

[0173] If the added node Once the target area is reached, the planning is complete.

[0174] If the target region is not reached, backtrack the expanded tree to avoid local minima.

[0175] Incorporate a directional guidance function related to the node expansion direction and path search length into the factors considering the selection of the next node. (Formula (14)) While considering the expansion area and path length, the path with the smallest turning angle is always selected to reach the next node. Introducing this function into the RRT algorithm ensures that when expanding the tree, it always moves towards the target area. According to equations (12) and (13), when selecting a new node b, the path search node closest to the next state node is always selected. If the inequality is not true... If the condition is met, then the node with the largest possible value of b is selected into the solution constructed by the random tree according to equation (12); otherwise, the next node b is selected according to equation (13). This continues until the target region is reached.

[0176] The technical solution adopted in this invention has the following technical effects: First, it proposes a guided RRT path planning algorithm and solves the local minima problem during the search process. Second, it is the first time that this improved algorithm has been applied to hard needle puncture path planning. Third, it introduces A... The algorithm is improved to achieve rapid convergence of the target area, reduce the number of searches, reduce search randomness, and make the planning result close to the shortest puncture path.

[0177] This embodiment proposes a three-dimensional path planning device. Figure 8 This is a schematic diagram of the composition structure of the three-dimensional path planning device according to an embodiment of the present invention, as shown below. Figure 8 As shown, the device 200, applied to scenarios including hard needle puncture, includes: an acquisition unit 201, a determination unit 202, a planning unit 203, and a judgment unit 204, wherein:

[0178] The obtaining unit 201 is used to obtain the puncture target position and the insertion position of the hard needle, determine the target area based on the insertion position, and determine the initial area based on the puncture target position and the insertion position.

[0179] The determining unit 202 is used to plan an initial search path based on the puncture target location and the initial area to obtain an initial search node; and to expand the initial search node to determine the search area corresponding to the state node.

[0180] The planning unit 203 is used to plan a target search path based on the initial search path and the search area;

[0181] The judgment unit 204 is used to determine whether the target search path reaches the target area and obtain a judgment result; if the judgment result shows that the target search path reaches the target area, the target search path is used as the three-dimensional path planning for hard needle puncture.

[0182] In other embodiments, the judgment unit 204 is further configured to, when the judgment result indicates that the target search path has not reached the target area, expand the status node in a preset direction to determine the expansion area corresponding to the new node; the preset direction represents the direction in which the status node faces the target area; plan a new search path according to the target search path and the expansion area; determine whether the new search path reaches the target area, and if the new search path reaches the target area, plan the new search path as a three-dimensional path for hard needle puncture.

[0183] In other embodiments, the obtaining unit 201 is further configured to obtain an initial puncture angle and an initial puncture step length; determine a cone based on the puncture target position, the insertion position, the initial puncture angle, and the initial puncture step length; and use the bottom region of the cone as the initial region.

[0184] In other embodiments, the obtaining unit 201 is further configured to: use the puncture target location as the root node of the extended tree; plan a feasible path based on the root node and the initial region using the Random Tree Search (RRT) algorithm; determine the optimal path among the feasible paths according to the A* algorithm; determine whether the optimal path successfully avoids obstacles; if the optimal path successfully avoids obstacles, use the optimal path as the initial search path; and use the endpoint of the initial search path in the initial region as the initial search node.

[0185] In other embodiments, the determining unit 202 is further configured to: expand the initial search node according to the Random Tree Search (RRT) algorithm to obtain multiple random state nodes; determine multiple state nodes based on the initial search node and each of the multiple random state nodes; determine the optimal state node among the multiple state nodes according to the A* algorithm; determine a first puncture step length based on the position information of the initial search node and the position information of the optimal state node when the straight line of motion between the initial search node and the optimal state node is a collision-free path; obtain a constraint range for the hard needle puncture angle; determine the first puncture angle of the hard needle at the optimal state node based on the hard needle puncture posture information of the initial search node and the constraint range; determine a first cone based on the initial search node, the optimal state node, the first puncture step length, and the first puncture angle; and use the bottom region of the first cone as the search region.

[0186] In other embodiments, the obtaining unit 201 is further configured to obtain a first state node when the straight line of motion between the initial search node and the optimal state node is not a collision-free path; the first state node is any state node other than the optimal state node among the plurality of state nodes.

[0187] The determining unit 202 is further configured to: determine the second puncture step length of the first state node based on the position information of the first state node and the position information of the optimal state node; update the first state node based on the hard needle puncture posture information of the initial search node and the constraint range; determine the second puncture angle of the hard needle at the updated first state node; determine the second cone based on the updated first state node, the second puncture step length and the second puncture angle; and use the bottom region of the second cone as the search region.

[0188] In other embodiments, the planning unit 203 is further configured to determine a target node in the search area based on the initial search path and the search area; and to plan a target search path based on the initial search path and the target node.

[0189] In other embodiments, the planning unit 203 is further configured to use the evaluation function of the A* algorithm to estimate the path estimate of each state node in the search area and the initial search path; and determine the state node corresponding to the minimum value of the path estimate as the target node.

[0190] The description of the above device embodiments is similar to that of the above method embodiments, and has similar beneficial effects. For technical details not disclosed in the device embodiments of the present invention, please refer to the description of the method embodiments of the present invention for understanding.

[0191] It should be noted that, in the embodiments of the present invention, if the above-described three-dimensional path planning method is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. Based on this understanding, the technical embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a control server (which may be a personal computer, a server, or a network server, etc.) to execute all or part of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), magnetic disks, or optical disks. Thus, the embodiments of the present invention are not limited to any specific hardware and software combination.

[0192] Correspondingly, embodiments of the present invention provide a three-dimensional path planning device, including a memory and a processor. The memory stores a computer program that can run on the processor, and when the processor executes the program, it implements the steps in the control method provided in the above embodiments.

[0193] Correspondingly, embodiments of the present invention provide a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the control method provided in the above embodiments.

[0194] It should be noted that the descriptions of the storage medium and device embodiments above are similar to those of the method embodiments above, and have similar beneficial effects. For technical details not disclosed in the storage medium and device embodiments of the present invention, please refer to the descriptions of the method embodiments of the present invention for understanding.

[0195] It should be noted that, Figure 9 This is a schematic diagram of a hardware entity structure of a three-dimensional path planning device in an embodiment of the present invention, such as... Figure 9 As shown, the hardware entity of the three-dimensional path planning device 300 includes a processor 301 and a memory 303. Optionally, the three-dimensional path planning device 300 may also include a communication interface 302.

[0196] It is understood that memory 303 can be volatile memory or non-volatile memory, or both. Non-volatile memory can be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), ferromagnetic random access memory (FRAM), flash memory, magnetic surface memory, optical disc, or compact disc read-only memory (CD-ROM); magnetic surface memory can be disk storage or magnetic tape storage. Volatile memory can be random access memory (RAM), which is used as an external cache. By way of example, but not limitation, many forms of RAM are available, such as Static Random Access Memory (SRAM), Synchronous Static Random Access Memory (SSRAM), Dynamic Random Access Memory (DRAM), Synchronous Dynamic Random Access Memory (SDRAM), Double Data Rate Synchronous Dynamic Random Access Memory (DDRSDRAM), Enhanced Synchronous Dynamic Random Access Memory (ESDRAM), SyncLink Dynamic Random Access Memory (SLDRAM), and Direct Rambus Random Access Memory (DRRAM).The memory 303 described in this embodiment of the invention is intended to include, but is not limited to, these and any other suitable types of memory.

[0197] The methods disclosed in the above embodiments of the present invention can be applied to processor 301, or implemented by processor 301. Processor 301 may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the above method can be completed by the integrated logic circuit of the hardware in processor 301 or by instructions in software form. The processor 301 may be a general-purpose processor, a digital signal processor (DSP), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. Processor 301 can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of the present invention. The general-purpose processor may be a microprocessor or any conventional processor, etc. The steps of the methods disclosed in the embodiments of the present invention can be directly manifested as being executed by a hardware decoding processor, or being executed by a combination of hardware and software modules in the decoding processor. The software modules may be located in a storage medium, which is located in memory 303. Processor 301 reads the information in memory 303 and combines its hardware to complete the steps of the aforementioned method.

[0198] In an exemplary embodiment, the device may be implemented by one or more application-specific integrated circuits (ASICs), DSPs, programmable logic devices (PLDs), complex programmable logic devices (CPLDs), field-programmable gate arrays (FPGAs), general-purpose processors, controllers, microcontrollers (MCUs), microprocessors, or other electronic components to perform the aforementioned method.

[0199] In the several embodiments provided by this invention, it should be understood that the disclosed methods and apparatus can be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the division of units is only a logical functional division, and in actual implementation, there may be other division methods, such as: multiple units or components may be combined, or integrated into another observation, or some features may be ignored or not executed. In addition, the communication connections between the various components shown or discussed may be through some interfaces, indirect coupling or communication connections between devices or units, and may be electrical, mechanical, or other forms.

[0200] The units described above as separate components may or may not be physically separate. The components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the units may be selected to achieve the purpose of this embodiment according to actual needs.

[0201] Those skilled in the art will understand that all or part of the steps of the above method embodiments can be implemented by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it performs the steps of the above method embodiments. The aforementioned storage medium includes various media that can store program code, such as mobile storage devices, read-only memory (ROM), magnetic disks, or optical disks.

[0202] Alternatively, if the integrated units described above in the embodiments of the present invention are implemented as software functional units and sold or used as independent products, they can also be stored in a computer-readable storage medium. Based on this understanding, the technical embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a device (which may be a personal computer, server, or network device, etc.) to execute all or part of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as mobile storage devices, ROMs, magnetic disks, or optical disks.

[0203] The three-dimensional path planning method, apparatus, device, and storage medium described in this invention are only examples of the embodiments of this invention, but are not limited thereto. Any three-dimensional path planning method, apparatus, device, and storage medium involved are within the protection scope of this invention.

[0204] It should be understood that the phrase "one embodiment" or "an embodiment" throughout the specification means that a specific feature, structure, or characteristic related to the embodiment is included in at least one embodiment of the invention. Therefore, "in one embodiment" or "in an embodiment" appearing throughout the specification does not necessarily refer to the same embodiment. Furthermore, these specific features, structures, or characteristics can be combined in any suitable manner in one or more embodiments. It should be understood that in the various embodiments of the invention, the sequence numbers of the above-described processes do not imply a sequential order of execution; the execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the invention. The sequence numbers of the above-described embodiments of the invention are merely descriptive and do not represent the superiority or inferiority of the embodiments.

[0205] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0206] The methods disclosed in the several method embodiments provided by this invention can be arbitrarily combined without conflict to obtain new method embodiments.

[0207] The features disclosed in the several product embodiments provided by this invention can be arbitrarily combined without conflict to obtain new product embodiments.

[0208] The features disclosed in the several method or device embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new method or device embodiments.

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

Claims

1. A method of three-dimensional path planning, characterized by, Applied to scenarios including hard needle puncture, the method includes: Obtain the puncture target position and insertion position of the hard needle, determine the target area based on the insertion position, and determine the initial area based on the puncture target position and the insertion position; The puncture target location is taken as the root node of the extended tree; a feasible path is planned based on the root node and the initial region using the Random Tree Search (RRT) algorithm; the optimal path among the feasible paths is determined according to the A* algorithm; when it is determined that the optimal path successfully avoids the obstacle, the optimal path is taken as the initial search path; the endpoint of the initial search path in the initial region is taken as the initial search node. The initial search node is expanded to determine the search area corresponding to the state node; wherein, when planning the feasible path using the RRT algorithm, an improved A* algorithm is introduced to add new leaf nodes. The improved A* algorithm expands multiple neighborhoods and uses an open table divide-and-conquer to find the minimum cost node. For the selection of each new leaf node, the estimated value from each new leaf node to the next state node is calculated as the selection criterion. Plan the target search path based on the initial search path and the search area; Determine whether the target search path reaches the target area and obtain a determination result; if the determination result indicates that the target search path has not reached the target area, use the direction guidance function related to the node expansion direction and path search length as the selection factor for the next state node, and select the path with the smallest turning angle to reach the next state node based on the direction guidance function; if the determination result indicates that the target search path has reached the target area, use the target search path as the three-dimensional path planning for the hard needle puncture.

2. The method of claim 1, wherein, The method further includes: Plan a new search path based on the target search path and the next state node; Determine whether the new search path reaches the target area. If the new search path reaches the target area, use the new search path as the three-dimensional path planning for hard needle puncture.

3. The method of claim 1, wherein, Determining the initial region based on the puncture target location and the insertion location includes: Obtain the initial puncture angle and initial puncture step length; The cone is determined based on the puncture target location, the puncture position, the initial puncture angle, and the initial puncture step length; The bottom region of the cone is taken as the initial region.

4. The method of claim 1, wherein, The step of expanding the initial search node to determine the search area corresponding to the state node includes: The initial search node is expanded using the Random Tree Search (RRT) algorithm to obtain multiple random state nodes; Multiple state nodes are determined based on the initial search node and each of the multiple random state nodes; The optimal state node among the multiple state nodes is determined according to the A* algorithm; if the straight line of motion between the initial search node and the optimal state node is a collision-free path, the first puncture step length is determined according to the position information of the initial search node and the position information of the optimal state node. Obtain the constraint range of the hard needle puncture angle; determine the first puncture angle of the hard needle at the optimal state node based on the hard needle puncture posture information of the initial search node and the constraint range; The first cone is determined based on the initial search node, the optimal state node, the first puncture step size, and the first puncture angle. The bottom region of the first cone is used as the search area.

5. The method according to claim 4, characterized in that, The method further includes: If the straight line of motion between the initial search node and the optimal state node is not a collision-free path, a first state node is obtained; the first state node is any state node other than the optimal state node among the plurality of state nodes. The second puncture step length of the first state node is determined based on the position information of the first state node and the position information of the optimal state node. Based on the hard needle puncture posture information of the initial search node and the constraint range, the first state node is updated, and the second puncture angle of the hard needle at the updated first state node is determined. The second cone is determined based on the updated first state node, the second puncture step size, and the second puncture angle. The bottom region of the second cone is used as the search area.

6. The method of claim 1, wherein, The step of planning a target search path based on the initial search path and the search area includes: Based on the initial search path and the search area, determine the target node in the search area; Plan the target search path based on the initial search path and the target node.

7. The method of claim 6, wherein, The step of determining the target node in the search area based on the initial search path and the search area includes: The A* algorithm's evaluation function is used to estimate the path estimate between each state node in the search region and the initial search path; The state node corresponding to the minimum value of the path estimate is determined as the target node.

8. A three-dimensional path planning apparatus characterized by comprising: Applied to scenarios including hard needle puncture, the device includes: an acquisition unit, a determination unit, a planning unit, and a judgment unit, wherein: The obtaining unit is used to obtain the puncture target position and the insertion position of the hard needle, determine the target area based on the insertion position, and determine the initial area based on the puncture target position and the insertion position. The determining unit is configured to: use the puncture target location as the root node of the extended tree; plan feasible paths using the Random Tree Search (RRT) algorithm based on the root node and the initial region; determine the optimal path among the feasible paths using the A* algorithm; when it is determined that the optimal path successfully avoids obstacles, use the optimal path as the initial search path; use the endpoint of the initial search path in the initial region as the initial search node; expand the initial search node to determine the search region corresponding to the state node; wherein, when using the RRT algorithm to plan the feasible path, an improved A* algorithm is introduced to add new leaf nodes. The improved A* algorithm expands multiple neighborhoods and uses an open table divide-and-conquer method to find the minimum cost node. For the selection of each new leaf node, the estimated value from each new leaf node to the next state node is calculated as the selection criterion. The planning unit is used to plan a target search path based on the initial search path and the search area; The judgment unit is used to determine whether the target search path has reached the target area and obtain a judgment result; if the judgment result indicates that the target search path has not reached the target area, the directional guidance function related to the node expansion direction and path search length is used as the selection factor for the next state node, and based on the directional guidance function, the path with the smallest turning angle is selected to reach the next state node; if the judgment result indicates that the target search path has reached the target area, the target search path is used as the three-dimensional path planning for the hard needle puncture.

9. A three-dimensional path planning device comprising a memory and a processor, the memory storing a program operable on the processor, characterized in that, When the processor executes the program, it implements the steps of the method according to any one of claims 1 to 7.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 7.

Images