Method and device for planning obstacle avoidance path of autonomous robot in narrow bony space

Abstract

The invention provides a narrow bony space autonomous robot obstacle avoidance path planning method and device, and relates to the technical field of robot autonomous path planning. The method comprises the following steps: carrying out compact surrounding on a concave nerve root obstacle in a narrow bony space by using an Alpha-shape algorithm to obtain a surrounded obstacle; an improved APF algorithm is adopted, and an artificial potential field is constructed for the robot, the preset target point and the surrounded obstacle; and according to an improved RRT * algorithm and an artificial potential field method, obtaining an obstacle avoidance path planning result of the narrow bony space autonomous robot based on the improved RRT *-APF. According to the method, the problem that the path planned in the narrow bony space is caught in local optimum is effectively avoided, effective obstacle avoidance processing is carried out on the concave obstacle in the complex shape, and finally the feasible obstacle avoidance path is planned in the narrow bony space.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous path planning for robots, and particularly to a method and device for autonomous obstacle avoidance path planning of a robot in a narrow bony space. Background Art

[0002] Autonomous robotic surgery has the potential to provide efficacy, safety, and consistency independent of the individual skills and experience of surgeons, especially in tasks that require high-precision operations, such as autonomous discectomy in a narrow bony space. Such tasks are extremely challenging because they require fine imaging, tissue tracking, and surgical planning techniques, while precisely executing surgical actions in an unstructured and deformable environment through highly adaptive control strategies. Summary of the Invention

[0003] Two key problems in autonomous obstacle avoidance path planning in a local space are optimized: one is that the distance between the nucleus pulposus target and the nerve root obstacle is too close, resulting in the path planning algorithm falling into a local optimal solution and making the path unreachable; the other is the obstacle avoidance strategy problem of the polygon irregular surrounding obstacle method when dealing with concave obstacles. Embodiments of the present invention provide a method and device for autonomous obstacle avoidance path planning of a robot in a narrow bony space. The technical solutions are as follows:

[0004] On the one hand, a method for autonomous obstacle avoidance path planning of a robot in a narrow bony space is provided. This method is implemented by an autonomous obstacle avoidance path planning device for a robot in a narrow bony space, and the method includes:

[0005] S1. For a concave nerve root obstacle in a narrow bony space, use the Alpha-shape algorithm to surround it to obtain the surrounded obstacle.

[0006] S2. Adopt an improved APF algorithm to construct an artificial potential field for the robot, a preset target point, and the surrounded obstacle.

[0007] S3. According to the improved RRT* algorithm and the artificial potential field method, obtain the autonomous obstacle avoidance path planning result of a robot in a narrow bony space based on the improved RRT*-APF.

[0008] Optionally, the potential field function of the obstacle repulsive potential field of the improved APF algorithm in S2 is shown in the following formula (1):

[0009]

[0010] In the formula, U obs (q) represents the potential field function of the obstacle repulsive potential field, K obs represents the proportionality coefficient, ρ(q, q 0 ) represents the distance between the robot and the nerve root obstacle, q represents the position of the robot, q0 Indicates the position of the obstacle, ρ 0 Indicates the perceived distance of the obstacle, ρ(q, q g ) indicates the distance between the robot and the nucleus pulposus target position, q g Indicates the position of the nucleus pulposus.

[0011] The repulsive force F pointing from the obstacle to the robot r Is expressed as the negative gradient of the repulsive force potential field. The improved obstacle repulsive force potential field function is shown in the following formula (2):

[0012]

[0013] In the formula, F obs (q) represents the improved obstacle repulsive force potential field function, F obs1 Indicates the force from the obstacle to the robot, F obs2 Indicates the force from the robot to the target.

[0014] Among them,

[0015]

[0016] In the formula, n represents the adjustment parameter, Represents the unit vector pointing from the obstacle to the robot, Represents the unit vector pointing from the target to the robot.

[0017] Optionally, the improved RRT* algorithm in S3 includes:

[0018] S311. Initialize the starting node q start and the target node q target in the search space; define the probability threshold p target and the step size parameter ρ.

[0019] S312. Generate a random probability value p between 0 and 1; determine whether the random probability value p is greater than the probability threshold p target ; if it is greater, randomly generate a state q rand ; if it is not greater, use the target node q target as the random state q rand .

[0020] S313. Find the node q rand nearest to the state q near in the current tree.

[0021] S314. Construct the total growth direction and generate a new node q new according to the total growth direction.

[0022] S315. According to the new node q newOptimize the path cost.

[0023] S316. Add the new node q new to the tree, and determine whether the tree reaches the target node or meets the preset stop condition; if so, output the optimal path; if not, go to step S312.

[0024] Optionally, the construction of the total growth direction in S314 includes:

[0025] Calculate the random growth function as shown in the following formula (5):

[0026]

[0027] In the formula, Rd(n) represents the random growth function, ρ represents the step size parameter, q rand represents the random state, and q near represents the node closest to the state q rand .

[0028] Calculate the target gravitational component as shown in the following formula (6):

[0029]

[0030] In the formula, G(n) represents the target gravitational component, and g represents the gravitational gain coefficient.

[0031] Synthesize the total growth direction as shown in the following formula (7):

[0032] F(n) = Rd(n) + G(n) (7)

[0033] In the formula, F(n) represents the total growth direction.

[0034] Optionally, the result of the obstacle avoidance path planning for the autonomous robot in a narrow bony space based on the improved RRT*-APF obtained in S3 includes:

[0035] S321. Initialize the starting position X start and the ending position X end .

[0036] S322. Determine whether the current node X current reaches the ending position X end ; if it reaches, output the result of the obstacle avoidance path planning for the autonomous robot in a narrow bony space based on the improved RRT*-APF; if not, execute step S323.

[0037] S323. Generate a random number and determine whether an obstacle avoidance event is triggered; if not, generate the node X next1 , and execute step S324; if triggered, generate the node Xnext2 , execute step S325.

[0038] S324, determine node X next1 Is it in the feasible area? If so, move node X next1 Set as current node X current , go to step S322; if not, go to step S322.

[0039] S325, search for node X next2 The neighboring nodes within the preset range generate node X next2 The parent node of .

[0040] S326, judging through node X next2 Does it reduce the path cost as a relay? If so, reconnect the neighboring nodes to node X next2 , execute step S327; if not, execute step S327.

[0041] S327, node X next2 Add to the tree and change the node X next2 Set as current node X current , go to execute step S322.

[0042] Optionally, in S323, node X is generated next1 ,include:

[0043] Generate a random node X rand , and find the random node X in the tree rand The nearest node X near , along the nearest node X near To a random node X rand Move one step in the direction to generate node X next1 .

[0044] Optionally, the generation node X in S323 next2 ,include:

[0045] Establish an artificial potential field, calculate the total growth direction of the current node, and move one step along the total growth direction of the current node to generate node X next2 .

[0046] On the other hand, a device for obstacle avoidance path planning of an autonomous robot in a narrow bony space is provided, and the device is applied to a method for obstacle avoidance path planning of an autonomous robot in a narrow bony space, and the device comprises:

[0047] The processing module is used to enclose the concave nerve root obstacle in the narrow bony space using the Alpha-shape algorithm to obtain the enclosed obstacle.

[0048] A construction module for constructing an artificial potential field for a robot, a preset target point, and an enclosed obstacle by using an improved APF algorithm.

[0049] An output module for obtaining the autonomous robot obstacle avoidance path planning result based on the improved RRT*-APF according to the improved RRT* algorithm and the artificial potential field method.

[0050] Optionally, the potential field function of the obstacle repulsive potential field of the improved APF algorithm is as shown in the following formula (1):

[0051]

[0052] In the formula, U obs (q) represents the potential field function of the obstacle repulsive potential field, K obs represents the proportionality coefficient, ρ(q, q 0 ) represents the distance between the robot and the nerve root obstacle, q represents the position of the robot, q 0 represents the position of the obstacle, ρ 0 represents the sensing distance of the obstacle, ρ(q, q g ) represents the distance between the robot and the nucleus pulposus target position, q g represents the position of the nucleus pulposus.

[0053] The repulsive force F pointing from the obstacle to the robot r is expressed as the negative gradient of the repulsive potential field. The improved obstacle repulsive potential field function is as shown in the following formula (2):

[0054]

[0055] In the formula, F obs (q) represents the improved obstacle repulsive potential field function, F obs1 represents the force from the obstacle to the robot, F obs2 represents the force from the robot to the target.

[0056] Among them,

[0057]

[0058] In the formula, n represents the adjustment parameter, represents the unit vector pointing from the obstacle to the robot, represents the unit vector pointing from the target to the robot.

[0059] Optionally, the improved RRT* algorithm includes:

[0060] S311. Initialize the starting node q start and the target node q target in the search space; define the probability threshold p targetand step size parameter ρ.

[0061] S312. Generate a random probability value p between 0 and 1; determine whether the random probability value p is greater than the probability threshold p target ; if it is greater, randomly generate a state q rand ; if it is not greater, use the target node q target as the random state q rand .

[0062] S313. Find the node q rand nearest to the state q near in the current tree.

[0063] S314. Construct the total growth direction and generate a new node q according to the total growth direction new .

[0064] S315. Optimize the path cost according to the new node q new .

[0065] S316. Add the new node q new to the tree and determine whether the tree reaches the target node or meets the preset stop condition; if so, output the optimal path; if not, go back to execute step S312.

[0066] Optionally, the output module is further configured to:

[0067] Calculate the random growth function as shown in the following formula (5):

[0068]

[0069] In the formula, Rd(n) represents the random growth function, ρ represents the step size parameter, q rand represents the random state, and q near represents the node nearest to the state q rand .

[0070] Calculate the target gravitational component as shown in the following formula (6):

[0071]

[0072] In the formula, G(n) represents the target gravitational component and g represents the gravitational gain coefficient.

[0073] Synthesize the total growth direction as shown in the following formula (7):

[0074] F(n) = Rd(n) + G(n) (7)

[0075] In the formula, F(n) represents the total growth direction.

[0076] Optionally, the output module is further configured to:

[0077] S321. Initialize the starting position X start and the ending position X end .

[0078] S322. Determine whether the current node X current has reached the ending position X end ; if it has reached, output the obstacle avoidance path planning result of the autonomous robot in a narrow bony space based on the improved RRT*-APF; if it has not reached, execute step S323.

[0079] S323. Generate a random number and determine whether an obstacle avoidance event is triggered; if it is not triggered, generate a node X next1 , and execute step S324; if it is triggered, generate a node X next2 , and execute step S325.

[0080] S324. Determine whether the node X next1 is in the feasible region; if it is, set the node X next1 as the current node X current , and go to execute step S322; if it is not, go to execute step S322.

[0081] S325. Search for neighboring nodes within the preset range of the node X next2 , and generate the parent node of the node X next2 .

[0082] S326. Determine whether using the node X next2 as a relay reduces the path cost; if it does, reconnect the neighboring nodes to the node X next2 , and execute step S327; if it does not, execute step S327.

[0083] S327. Add the node X next2 to the tree, and set the node X next2 as the current node X current , and go to execute step S322.

[0084] Optionally, the output module is further configured to:

[0085] Generate a random node X rand , and find the node X rand in the tree that is closest to the random node X near , and move one step in the direction from the closest node X near to the random node X rand to generate a node X next1 .

[0086] Optionally, the output module is further configured to:

[0087] Construct an artificial potential field, calculate the total growth direction of the current node, and move one step along the total growth direction of the current node to generate node X next2 .

[0088] On the other hand, a narrow bony space autonomous robot obstacle avoidance path planning device is provided. The narrow bony space autonomous robot obstacle avoidance path planning device includes: a processor; a memory, and computer-readable instructions are stored on the memory. When the computer-readable instructions are executed by the processor, any one of the methods in the above-mentioned narrow bony space autonomous robot obstacle avoidance path planning method is implemented.

[0089] On the other hand, a computer-readable storage medium is provided. At least one instruction is stored in the storage medium, and the at least one instruction is loaded and executed by a processor to implement any one of the methods in the above-mentioned narrow bony space autonomous robot obstacle avoidance path planning method.

[0090] The beneficial effects brought by the technical solutions provided in the embodiments of the present invention at least include:

[0091] In the present invention, it aims to solve the reachability of local path planning and the obstacle avoidance strategy problem. Specifically, the present invention constructs an artificial potential field for the starting point, the target point, and the obstacle. On this basis, the random sampling step of the RRT* algorithm is modified, so that under the guidance of the artificial potential field, the randomly sampled points can be closer to the optimal path, and at the same time, a large number of invalid sampled points are significantly reduced. This improvement effectively avoids the problem that the path falls into local optimality, and performs efficient obstacle avoidance processing for concave complex obstacles, and finally plans a feasible local path. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0093] Figure 1 It is a flowchart of a narrow bony space autonomous robot obstacle avoidance path planning method provided by an embodiment of the present invention;

[0094] Figure 2 It is a flowchart of the RRT*-APF algorithm for the narrow bony space autonomous robot obstacle avoidance path planning in the overall local space provided by an embodiment of the present invention;

[0095] Figure 3 It is a block diagram of a narrow bony space autonomous robot obstacle avoidance path planning device provided by an embodiment of the present invention;

[0096] Figure 4 It is a schematic structural diagram of an obstacle avoidance path planning device for an autonomous robot in a narrow bony space provided by an embodiment of the present invention. Detailed implementation manners

[0097] The following describes the technical solutions in the present invention with reference to the accompanying drawings.

[0098] In the embodiments of the present invention, words such as "exemplarily" and "for example" are used to represent examples, illustrations or explanations. Any embodiment or design solution described as an "example" in the present invention should not be construed as being more preferred or having more advantages than other embodiments or design solutions. Exactly speaking, the use of the word "example" is intended to present concepts in a specific manner. In addition, in the embodiments of the present invention, the meaning expressed by "and / or" can be both, or either one of the two.

[0099] In the embodiments of the present invention, "image" and "picture" can sometimes be used interchangeably. It should be noted that when the difference is not emphasized, the meanings they express are the same. "(of)", "corresponding", and "corresponding" can sometimes be used interchangeably. It should be noted that when the difference is not emphasized, the meanings they express are the same.

[0100] In the embodiments of the present invention, sometimes subscripts such as W 1 may be written in a non-subscript form such as W1. When the difference is not emphasized, the meanings they express are the same.

[0101] To make the technical problems, technical solutions and advantages to be solved by the present invention clearer, the following will be described in detail with reference to the accompanying drawings and specific embodiments.

[0102] The embodiments of the present invention provide an obstacle avoidance path planning method for an autonomous robot in a narrow bony space. This method can be implemented by an obstacle avoidance path planning device for an autonomous robot in a narrow bony space, and this obstacle avoidance path planning device for an autonomous robot in a narrow bony space can be a terminal or a server. As Figure 1 shown in the flowchart of the obstacle avoidance path planning method for an autonomous robot in a narrow bony space, the processing flow of this method can include the following steps:

[0103] S1. For the concave nerve root obstacle in the narrow bony space, use the Alpha-shape algorithm to surround it to obtain the surrounded obstacle.

[0104] In a feasible implementation manner, use the Alpha-shape algorithm to surround the concave nerve root obstacle to prevent the surgical actuator from touching or mis-grasping the obstacle.

[0105] Specifically, the Alpha-shape algorithm is used to enclose the concave nerve root obstacle, resulting in a compact enclosure of the nerve root obstacle with an irregular polygon. The alpha_value in the algorithm is a parameter that controls the shape of the obstacle and determines the "precision" or "relaxation" of the obstacle. The Alpha-shape algorithm creates a geometric shape that approximately represents the obstacle based on the point set o of the obstacle and the parameter alpha_value. When using the obstacle avoidance detection method for this algorithm, it is to check whether the path from the starting point to the ending point collides with the given set of obstacles o. That is, the path is uniformly sampled and each point is checked to see if it overlaps with the obstacle.

[0106] Furthermore, the algorithm process:

[0107] (1) Construct a tree structure and dynamically adjust the threshold: Construct a k-d tree for the given point set. Starting from an arbitrary initial point p i , calculate the average distance to its nearest k neighbors, denoted as α. Store the current point p i and its neighboring points within 2d as a new set S. Arbitrarily select a group of points p 2 p 3 from S, and find the center of the sphere of the sphere passing through point p 1 p 2 and p 3 with a radius of α.

[0108] (2) Boundary determination: Traverse the point set S and sequentially find the set of distances from other points to the center of the sphere. If all the distance values in this set of distances are greater than or equal to the radius α, it can be determined that points p 1 p 2 and p 3 are edge contour points, and triangle p 1 p 2 p 3 is a boundary triangle; if there are values less than the radius α in the set of distances, it can be determined that it is not an edge contour point, stop traversing, and go to step 3.

[0109] (3) Select the next group of points in S and make judgments according to steps (1) and (2) until all points in S have been judged.

[0110] (4) Loop iteration: Repeat the above steps for the remaining unmarked points until all points have been processed, and finally generate the target Alpha-shape.

[0111] S2. Adopt an improved APF algorithm to construct an artificial potential field for the robot, the preset target point, and the enclosed obstacle.

[0112] In a feasible implementation, the APF algorithm is optimized to construct an artificial potential field for the robot, the target point, and the obstacle, effectively avoiding the problems of the planned path falling into a local optimum and the path being unreachable.

[0113] Specifically, the APF algorithm is optimized to effectively avoid the problems of the planned path falling into a local optimum and the path being unreachable. Since the nucleus pulposus target of the present invention is very close to the nerve root obstacle, it is easy to fall into the problems of local optimum and target unreachability. The local optimum is also the local minimum. During the movement of the robot towards the nucleus pulposus target, the following situation may occur: the repulsive force of the nerve root obstacle and the attractive force of the nucleus pulposus target are exactly equal in magnitude but opposite in direction. At this time, the resultant force pushing the robot to move becomes zero, and the robot cannot continue to move forward. The target is unreachable. When the robot approaches the nucleus pulposus target, the attractive force from the nucleus pulposus target will decrease, and when the robot approaches the nerve root obstacle, the repulsive force from the obstacle will increase. The obstacle near the target will exert a repulsive force on the robot. In this case, both insufficient attraction and excessive repulsive force may hinder the robot from moving towards the target. Therefore, the present invention optimizes the APF algorithm to solve these two major problems of the autonomous path planning of the present invention. The present invention decomposes the original repulsive force into two repulsive forces to avoid these problems, so that the resultant force of the final repulsive force and the attractive force can drive the obstacle avoidance to reach the nucleus pulposus target point. In this way, the controlled object is subjected to the repulsive force and the attractive force in the composite field composed of these two potential fields, and the resultant force of the repulsive force and the attractive force guides the movement of the controlled object to search for a collision-free obstacle avoidance path. The problem of local optimum and target unreachability is solved by improving the potential field function of the obstacle repulsive force potential field. The potential field function of the obstacle repulsive force potential field of the improved APF algorithm is expressed by the following formula:

[0114]

[0115] In the formula, U obs (q) represents the potential field function of the obstacle repulsive force potential field, K obs represents the proportionality coefficient, ρ(q,q 0 ) is a vector representing the distance between the robot and the nerve root obstacle, and the direction is from the nerve root obstacle to the Robot, q represents the position of the robot, q 0 represents the position of the obstacle, ρ 0 represents the sensing distance of the obstacle, ρ(q,q g ) is a vector representing the distance between the robot and the nucleus pulposus target position, and the vector direction is from the position of the Robot to the position of the nucleus pulposus target point, q gIndicates the position of the nucleus pulposus. The factor determining the repulsive potential field of the obstacle is the distance between the Robot and the obstacle. When the Robot has not entered the influence range of the obstacle, the potential energy value it receives is zero; after the Robot enters the influence range of the obstacle, the greater the distance between the two, the smaller the potential energy value received by the robot, and the smaller the distance, the greater the potential energy value received by the robot.

[0116] Further, the repulsive force F pointing from the obstacle to the Robot r is represented as the negative gradient of the repulsive potential field. The improved obstacle repulsive potential field function:

[0117]

[0118] In the formula, F obs (q) represents the improved obstacle repulsive potential field function, and F obs1 represents the force from the obstacle to the robot, and F obs2 represents the force from the robot to the target, but F obs2 This force is generated by the obstacle, but only the direction is the force from the Robot to the target. In this way, the problems of local optimality and target unreachability can be avoided. The obstacle generates two repulsive forces F obs1 and F obs2 on the Robot:

[0119]

[0120] In the formula, n is a user-defined parameter that plays a role in adjusting the properties of the holding force field. represents the unit vector from the obstacle to the robot, represents the unit vector from the target to the robot.

[0121] The trajectory generated by RRT* is only adjusted within the sensing distance ρ 0 range. The degree to which the trajectory is kept away from the obstacle can be adjusted by changing the parameters ρ 0 and K obs . The former adjusts the action range of the APF, and the latter adjusts the magnitude of the potential field force. When the guiding trajectory is in the free space not affected by obstacles, the APF (Artificial Potential Field Method) will not interfere with the path planning system to stably execute the established path in the obstacle-free free space.

[0122] S3. According to the improved RRT* algorithm and the artificial potential field method, the obstacle avoidance path planning result of the autonomous robot in the narrow bony space based on the improved RRT*-APF is obtained.

[0123] In a feasible implementation manner, the random sampling step of RRT* is optimized to efficiently plan the path.

[0124] Specifically, in the improved RRT* algorithm, by introducing a probability factor in the random tree expansion step, the convergence efficiency of the path planning algorithm is significantly improved. Specifically, a certain gravitational mechanism is added during the expansion of the random tree to guide the branches to grow preferentially towards the target node, thus accelerating the search process. At the same time, a repulsive force field is constructed near the obstacles to limit the expansion range of the search tree in the obstacle area, thereby reducing the randomness of path planning and improving the efficiency and stability of the algorithm. This target-guided RRT* algorithm adopts a probability-driven search strategy to further optimize the search speed and path quality. First, the target-biased RRT* algorithm proposes a target partial probability threshold P target , and then obtains a random probability value P between 0 and 1. When P > P target , a random state q rand is obtained in the search space, otherwise q rand is equal to q target . This algorithm maintains the characteristics of the original algorithm and speeds up the convergence to the target node.

[0125] This improved rapidly-exploring random tree RRT* algorithm incorporates the concept of artificial potential field (APF). By increasing the factor of target attraction, it guides the growth direction of the random tree, making it more inclined to expand towards the target point while reducing randomness. In the algorithm, a gravitational potential field is introduced as the guiding force for newly generated nodes, thereby changing the expansion trajectory of the tree to make it preferentially tend towards the target point rather than relying solely on random expansion. The gravitational potential field is mainly related to the distance between the Robot and the nucleus pulposus target position. The greater the distance, the greater the potential energy value the Robot receives; the smaller the distance, the smaller the potential energy value the robot receives. This improvement aims to enhance the efficiency and target-oriented characteristics of path planning. Artificial potential field (APF), as a local path planning method, regards the robot workspace as an artificial force field. In this force field, the target point exerts an attractive force on the robot, while the obstacles exert a repulsive force. The robot moves under the guidance of the resultant force of the target attraction and the obstacle repulsion and finally advances along the optimized path.

[0126] Optionally, the basic process of the improved RRT* algorithm:

[0127] S311. Initialize the starting node q start and the target node q target in the search space; define the probability threshold p target and the step size parameter ρ. Set the potential field parameters, such as the gravitational gain coefficient g.

[0128] S312. Generate a random probability value p between 0 and 1; determine whether the random probability value p is greater than the probability threshold p target ; if it is greater, randomly generate a state q rand; If it is not greater than, directly use the target node as the random state, i.e., q rand = q target .

[0129] S313. Find the node q rand nearest to the state q near in the current tree.

[0130] S314. Construct the total growth direction and generate a new node q new according to the total growth direction.

[0131] In a feasible implementation, step S314 is the combined action of the gravitational component and the random component.

[0132] Specifically, calculate the random growth function as shown in the following formula (5):

[0133]

[0134] In the formula, Rd(n) represents the random growth function, n is a part of the function name used to represent these functions, ρ represents the step size parameter, q rand represents the random state, q near represents the node nearest to the state q rand .

[0135] Calculate the target gravitational component (if q rand ≠ q target , this item can also be retained) as shown in the following formula (6):

[0136]

[0137] In the formula, G(n) represents the target gravitational component, and g represents the gravitational gain coefficient.

[0138] Synthesize the total growth direction as shown in the following formula (7):

[0139] F(n) = Rd(n) + G(n) (7)

[0140] In the formula, F(n) represents the total growth direction.

[0141] S315. Optimize the path cost according to the new node q new .

[0142] In a feasible implementation, the path optimization algorithm used is RRT*. After adding q new , reconnect the tree according to the optimality principle to optimize the path cost. The specific method is to recalculate the cost of q new and its neighboring nodes, and select a parent node with the minimum cost.

[0143] S316. Add the new node q new to the tree, and determine whether the tree reaches the target node or meets the preset stop condition; if so, output the optimal path; if not, go to step S312 for execution.

[0144] Add the new node q new to the tree, and repeat steps S312 - S315 until the tree reaches the target node or meets the preset stop condition (such as the maximum number of iterations or search time).

[0145] Further, after the search is completed, backtrack the path from q target to q start and output the final optimal path.

[0146] In order to obtain an algorithm that combines the advantages of RRT* and APF and can make up for each other's disadvantages, the present invention attempts to fuse the two algorithms. Taking the improved RRT* algorithm as the main framework, an improved APF algorithm is introduced to guide the path selection, so as to solve some defects in the path selection of RRT*, such as path irregularity and high time and distance costs. The improved APF is defined in the search space of the RRT* algorithm, so each point moves towards the direction of the resultant force with a certain probability. While enhancing the target directivity, this method still retains the randomly generated nodes, and these random nodes can help the algorithm jump out of the local optimal solution. Finally, the optimal planned path can be found.

[0147] Optionally, as Figure 2 shown, the above step S3 may include the following steps S321 - S327:

[0148] S321. Initialize the starting point and the ending point.

[0149] Start the process, input the starting point position X start and the ending point position X end .

[0150] S322. Check whether the current node is the ending point.

[0151] Judge whether the current node X current reaches the ending point position X end ; if it reaches, then find the global path, end the algorithm, and output the obstacle avoidance path planning result of the autonomous robot in a narrow bony space based on the improved RRT*-APF; if not, execute step S323.

[0152] S323. Random point generation and navigation strategy.

[0153] Generate a random number and determine whether an obstacle avoidance event is triggered; if not, generate the node X next1 , and execute step S324; if triggered, generate the node Xnext2 , execute step S325.

[0154] In a feasible implementation, generate a random number and determine whether to use the potential field method to generate X next .

[0155] Obstacle avoidance event triggered: Establish an artificial potential field, calculate the resultant force direction (gravitational force + repulsive force) at the current point, and move one step along this direction to generate X next .

[0156] Obstacle avoidance event not triggered: Generate a random point X rand , and find the point X rand closest to X in the tree near , and move one step along the direction from X near to X rand to generate X next .

[0157] S324. Check the feasibility of Xnext.

[0158] Judge whether the node X next1 is in the feasible region (no collision and satisfying the constraints); if so, set the node X next1 as the current node X current , and go to execute step S322; if not, go to execute step S322.

[0159] S325. Find the best parent node.

[0160] Search for neighboring nodes within a certain range around the generated X next .

[0161] Calculate the path cost from each neighboring node to X next , and select the node with the lowest cost as the parent node of X next .

[0162] S326. Update the connection of neighboring nodes.

[0163] Check the neighboring nodes around X next , and judge whether the cost can be reduced by using X next as a relay.

[0164] If the path cost is reduced, reconnect these nodes to X next .

[0165] S327. If X next is a valid node, add it to the tree and set it as the current node X current .

[0166] Repeat the above steps until the termination condition is met (finding from Xstart to X end of the globally optimal path).

[0167] Path planning can be divided into global path planning and local path planning. This invention focuses on local path planning for the application scenario. Local path planning is based on a more one-sided and microscopic perspective. It conducts a local search within a limited range around the robot to find a good path and the positions of surrounding obstacles. When the position information of the obstacles is mastered and the specific situation of the obstacles is judged, the obstacles can be avoided and a feasible path can be found. This invention proposes an improved algorithm combining RRT (Rapidly Exploring Random Tree) and APF (Artificial Potential Field) to solve the problems of random sampling and low efficiency in the traditional RRT path planning algorithm. Aiming at the deficiencies of the sampling path planning algorithm, this invention uses the concept of the target bias algorithm and adds a probability value p to the RRT* algorithm in the basic algorithm for improvement. The RRT* algorithm can optimize the path length during the continuous expansion process and make fine-tuning near the end point. The improved algorithm further improves the search efficiency and path quality. The algorithm is further improved using the APF method to solve the problem that the planned path gets stuck in a local optimum and the path is unreachable. The concept of obstacle repulsion introduced in APF is applied to the target-biased RRT* algorithm to guide the local random tree to grow away from the obstacles.

[0168] In the embodiments of this invention, it aims to solve the reachability and obstacle avoidance strategy problems of local path planning. Specifically, this invention constructs an artificial potential field for the starting point, target point, and obstacles. On this basis, the random sampling step of the RRT* algorithm is modified so that under the guidance of the artificial potential field, the random sampling points can be closer to the optimal path, and at the same time, a large number of invalid sampling points are significantly reduced. This improvement effectively avoids the problem that the path gets stuck in a local optimum and conducts efficient obstacle avoidance for concave complex obstacles, and finally plans a feasible local path.

[0169] Figure 3 is a block diagram of an obstacle avoidance path planning device for a narrow bony space autonomous robot shown according to an exemplary embodiment. This device is used for the obstacle avoidance path planning method of a narrow bony space autonomous robot. Referring to Figure 3 , this device includes a processing module 310, a construction module 320, and an output module 330. Among them:

[0170] The processing module 310 is used to enclose the concave nerve root obstacle in a narrow bony space using the Alpha-shape algorithm to obtain the enclosed obstacle.

[0171] The construction module 320 is used to construct an artificial potential field for the robot, the preset target point, and the surrounded obstacles by using an improved APF algorithm.

[0172] The output module 330 is used to obtain the obstacle avoidance path planning result of the autonomous robot in a narrow bony space based on the improved RRT*-APF according to the improved RRT* algorithm and the artificial potential field method.

[0173] In the embodiment of the present invention, it aims to solve the problems of reachability and obstacle avoidance strategy in local path planning. Specifically, the present invention constructs an artificial potential field for the starting point, the target point, and the obstacles. On this basis, the random sampling step of the RRT* algorithm is modified, so that under the guidance of the artificial potential field, the randomly sampled points can be closer to the optimal path, and at the same time, a large number of invalid sampled points are significantly reduced. This improvement effectively avoids the problem of the path falling into local optimum, and performs efficient obstacle avoidance processing for concave complex obstacles, and finally plans a feasible local path.

[0174] Figure 4 It is a schematic structural diagram of an obstacle avoidance path planning device for an autonomous robot in a narrow bony space, as Figure 4 shown, the obstacle avoidance path planning device for an autonomous robot in a narrow bony space may include the above-mentioned Figure 3 shown obstacle avoidance path planning device for an autonomous robot in a narrow bony space. Optionally, the obstacle avoidance path planning device 410 for an autonomous robot in a narrow bony space may include a first processor 2001.

[0175] Optionally, the obstacle avoidance path planning device 410 for an autonomous robot in a narrow bony space may further include a memory 2002 and a transceiver 2003.

[0176] Wherein, the first processor 2001 is connected to the memory 2002 and the transceiver 2003, such as through a communication bus.

[0177] Next, in combination with Figure 4 each component of the obstacle avoidance path planning device 410 for an autonomous robot in a narrow bony space will be specifically introduced:

[0178] Among them, the first processor 2001 is the control center of the autonomous robot obstacle avoidance path planning device 410 for narrow bony spaces, which can be a single processor or a collective term for multiple processing elements. For example, the first processor 2001 is one or more central processing units (CPUs), or can be an application specific integrated circuit (ASIC), or one or more integrated circuits configured to implement the embodiments of the present invention. For example: one or more digital signal processors (DSPs), or one or more field programmable gate arrays (FPGAs).

[0179] Optionally, the first processor 2001 can execute various functions of the autonomous robot obstacle avoidance path planning device 410 for narrow bony spaces by running or executing software programs stored in the memory 2002 and calling data stored in the memory 2002.

[0180] In a specific implementation, as an embodiment, the first processor 2001 can include one or more CPUs, such as Figure 4 the CPU0 and CPU1 shown in

[0181] In a specific implementation, as an embodiment, the autonomous robot obstacle avoidance path planning device 410 for narrow bony spaces can also include multiple processors, such as Figure 4 the first processor 2001 and the second processor 2004 shown in

[0182] Among them, the memory 2002 is used to store software programs for implementing the solution of the present invention and is controlled by the first processor 2001 for execution. The specific implementation manner can refer to the above method embodiments and will not be elaborated here.

[0183] Optionally, the memory 2002 may be a read-only memory (ROM) or other type of static storage device that can store static information and instructions, a random access memory (RAM) or other type of dynamic storage device that can store information and instructions, or may also be an electrically erasable programmable read-only memory (EEPROM), a compact disc read-only memory (CD-ROM), or other optical disc storage, optical disc storage (including compact discs, laser discs, optical discs, digital versatile discs, Blu-ray discs, etc.), magnetic disk storage media, or other magnetic storage devices, or any other medium that can be used to carry or store the desired program code in the form of instructions or data structures and can be accessed by a computer, but is not limited thereto. The memory 2002 may be integrated with the first processor 2001 or may exist independently and be coupled to the first processor 2001 through an interface circuit ( Figure 4 not shown) of the narrow bony space autonomous robot obstacle avoidance path planning device 410. The embodiments of the present invention do not make specific limitations in this regard.

[0184] The transceiver 2003 is used to communicate with a network device or with a terminal device.

[0185] Optionally, the transceiver 2003 may include a receiver and a transmitter ( Figure 4 not shown separately). Among them, the receiver is used to implement the receiving function, and the transmitter is used to implement the sending function.

[0186] Optionally, the transceiver 2003 may be integrated with the first processor 2001 or may exist independently and be coupled to the first processor 2001 through an interface circuit ( Figure 4 not shown) of the narrow bony space autonomous robot obstacle avoidance path planning device 410. The embodiments of the present invention do not make specific limitations in this regard.

[0187] It should be noted that Figure 4 the structure of the narrow bony space autonomous robot obstacle avoidance path planning device 410 shown in does not constitute a limitation on the router. The actual knowledge structure recognition device may include more or fewer components than shown in the figure, or combine certain components, or have different component arrangements.

[0188] In addition, the technical effects of the narrow bony space autonomous robot obstacle avoidance path planning device 410 may refer to the technical effects of the narrow bony space autonomous robot obstacle avoidance path planning method described in the above method embodiments, and will not be elaborated here.

[0189] It should be understood that the first processor 2001 in the embodiments of the present invention may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field programmable gate arrays (FPGAs) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0190] It should also be understood that the memory in the embodiments of the present invention may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memories. Among them, the non-volatile memory may be a read-only memory (ROM), a programmable ROM (PROM), an erasable PROM (EPROM), an electrically erasable PROM (EEPROM), or a flash memory. The volatile memory may be a random access memory (RAM), which is used as an external cache. By way of example but not limitation, many forms of random access memory (RAM) are available, such as static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced SDRAM (ESDRAM), synchlink DRAM (SLDRAM), and direct rambus RAM (DR RAM).

[0191] The above embodiments can be implemented in whole or in part by software, hardware (such as circuits), firmware, or any combination thereof. When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, the processes or functions described in the embodiments of the present invention are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (such as infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or a data center that contains one or more collections of available media. The available media can be magnetic media (such as floppy disks, hard disks, magnetic tapes), optical media (such as DVDs), or semiconductor media. The semiconductor media can be a solid-state drive.

[0192] It should be understood that the term "and / or" in this document is merely a description of the association relationship between associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist simultaneously, and B exists alone. Here, A and B can be singular or plural. Additionally, the character " / " in this document generally represents an "or" relationship between the associated objects before and after, but it may also represent an "and / or" relationship, which can be specifically understood with reference to the context before and after.

[0193] In the present invention, "at least one" means one or more, and "a plurality" means two or more. "At least one of the following" or its similar expressions refer to any combination of these items, including any combination of single items or plural items. For example, at least one of a, b, or c can represent: a, b, c, a - b, a - c, b - c, or a - b - c, where a, b, and c can be single or multiple.

[0194] It should be understood that in various embodiments of the present invention, the magnitudes of the sequence numbers of the above processes do not imply the order of execution. The order of execution of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present invention.

[0195] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in connection with the embodiments disclosed herein can be implemented in electronic hardware, or in a combination of computer software and electronic hardware. Whether these functions are executed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled professionals can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of the present invention.

[0196] Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the devices, apparatuses, and units described above can refer to the corresponding processes in the foregoing method embodiments and will not be elaborated herein.

[0197] In several embodiments provided by the present invention, it should be understood that the disclosed devices, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative. For example, the division of the units is only a logical function division, and there can be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another device, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections to each other can be through some interfaces, and the indirect couplings or communication connections of the devices or units can be in electrical, mechanical, or other forms.

[0198] The units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they can be located in one place, or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0199] In addition, the functional units in each embodiment of the present invention can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit.

[0200] When the above-mentioned functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical discs that can store program codes.

[0201] As described above, the above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention can easily think of changes or substitutions, which should all be covered by the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. A method for obstacle avoidance path planning of an autonomous robot in a narrow bony space, characterized in that: The method comprises: S1. For the concave nerve root obstacle in the narrow bony space, the Alpha-shape algorithm is used to enclose it to obtain the enclosed obstacle; S2. Using an improved APF algorithm, an artificial potential field is constructed for the robot, the preset target point and the surrounded obstacles; S3. According to the improved RRT* algorithm and artificial potential field method, the obstacle avoidance path planning result of the autonomous robot in narrow bony space based on the improved RRT*-APF is obtained.

2. The method for obstacle avoidance path planning of an autonomous robot in a narrow bony space according to claim 1, characterized in that: The potential field function of the obstacle repulsion potential field of the improved APF algorithm in S2 is shown in the following formula (1): Where U obs (q) represents the potential field function of the obstacle repulsive potential field, K obs represents the positive proportional coefficient, ρ(q,q0) represents the distance between the robot and the nerve root obstacle, q represents the robot position, q0 represents the obstacle position, ρ0 represents the perceived distance of the obstacle, ρ(q,q g ) represents the distance between the robot and the target position of the nucleus pulposus, q g Indicates the location of the nucleus pulposus; The repulsive force F directed from the obstacle to the robot r Expressed as the negative gradient of the repulsive potential field, the improved obstacle repulsive potential field function is shown in the following formula (2): In the formula, F obs (q) represents the improved obstacle repulsion potential field function, F obs1 represents the force of the obstacle pointing to the robot, F obs2 represents the force of the robot pointing to the target; in, In the formula, n represents the adjustment parameter, represents the unit vector pointing from the obstacle to the robot, Represents the unit vector of the target pointing toward the robot.

3. The narrow bony space autonomous robot obstacle avoidance path planning method according to claim 1, characterized in that: The improved RRT* algorithm in S3 includes: S311, initialize the starting node q in the search space start and the target node q target ; Define the probability threshold p target and step size parameter ρ; S312, generate a random probability value p between 0 and 1; determine whether the random probability value p is greater than the probability threshold p target ; If it is greater than, a state q is randomly generated rand ; If not, the target node q target As a random state q rand ; S313, find the state q in the current tree rand The nearest node q near ; S314: construct a total growth direction, and generate a new node q according to the total growth direction new ; S315, according to the new node q new Optimize path cost; S316: The new node q new Add to the tree and determine whether the tree reaches the target node or meets the preset stop condition; if so, output the optimal path; if not, go to step S312.

4. The narrow bony space autonomous robot obstacle avoidance path planning method according to claim 3, characterized in that: The construction of the overall growth direction in S314 includes: Calculate the random growth function as shown in the following formula (5): In the formula, Rd(n) represents the random growth function, ρ represents the step size parameter, and q rand represents a random state, q near Indicates the state q rand The nearest node; Calculate the target gravity component as shown in the following formula (6): In the formula, G(n) represents the target gravity component, and g represents the gravity gain coefficient; The total growth direction is shown in the following formula (7): F(n)=Rd(n)+G(n) (7) Where F(n) represents the total growth direction.

5. The narrow bony space autonomous robot obstacle avoidance path planning method according to claim 1, characterized in that: The step S3 obtains the obstacle avoidance path planning result of the autonomous robot in a narrow bony space based on the improved RRT*-APF according to the artificial potential field and the improved RRT* algorithm, including: S321, initialization starting point position X start and the end position X end ; S322, determine the current node X current Whether the end position X is reached end ; If it is reached, then output the obstacle avoidance path planning result of the autonomous robot in a narrow bony space based on the improved RRT*-APF; if it is not reached, execute step S323; S323, generate a random number to determine whether the obstacle avoidance event is triggered; if not, generate node X next1 , execute step S324; if triggered, generate node X next2 , execute step S325; S324, determine the node X next1 Is it in the feasible area? If so, the node X next1 Set as current node X current , go to step S322; if not, go to step S322; S325: Search the node X next2 The neighboring nodes within the preset range generate the node X next2 The parent node of S326, judging through the node X next2 Whether to reduce the path cost as a relay; if so, reconnect the neighboring nodes to the node X next2 , execute step S327; if not, execute step S327; S327, the node X next2 Add to the tree and change the node X next2 Set as current node X current , go to execute step S322.

6. The method for obstacle avoidance path planning of an autonomous robot in a narrow bony space according to claim 5, characterized in that: The node X is generated in S323 next1 ,include: Generate a random node X rand , and find the random node X in the tree rand The nearest node X near , along the nearest node X near To the random node X rand Move one step in the direction to generate node X next1 .

7. The method for obstacle avoidance path planning of an autonomous robot in a narrow bony space according to claim 5, characterized in that: The generation node X in S323 next2 ,include: Establish an artificial potential field, calculate the total growth direction of the current node, and move one step along the total growth direction of the current node to generate node X next2 .

8. An obstacle avoidance path planning device for an autonomous robot in a narrow bony space, the obstacle avoidance path planning device for an autonomous robot in a narrow bony space is used to implement the obstacle avoidance path planning method for an autonomous robot in a narrow bony space as claimed in any one of claims 1 to 7, characterized in that: The device comprises: A processing module is used to enclose a concave nerve root obstacle in a narrow bony space using an Alpha-shape algorithm to obtain the enclosed obstacle; A construction module is used to construct an artificial potential field for the robot, the preset target point and the surrounded obstacles by using an improved APF algorithm; The output module is used to obtain the obstacle avoidance path planning result of the autonomous robot in the narrow bony space based on the improved RRT*-APF according to the improved RRT* algorithm and the artificial potential field method.

9. An autonomous robot obstacle avoidance path planning device for narrow bony spaces, characterized in that: The narrow bony space autonomous robot obstacle avoidance path planning device comprises: processor; A memory having computer-readable instructions stored thereon, wherein when the computer-readable instructions are executed by the processor, the method according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores program codes, which can be called by a processor to execute the method according to any one of claims 1 to 7.

Images