A local obstacle avoidance path planning method, system, device and storage medium

Abstract

The application discloses a local obstacle avoidance path planning method, system, device and storage medium, and comprises the following processes: S1, a path search tree is constructed, whether there is an obstacle in the path search process is judged, if not, a path is generated; S2, if there is an obstacle, a tentacle expansion strategy is adopted to avoid the obstacle, an expansion node avoiding the obstacle is added to the path search tree until the whole path search is completed, and a path is generated. The obstacle can be bypassed in advance, and the original Connect idea is improved to help two path search trees to be quickly connected. Not only is the probability of invalid expansion in a non-target direction reduced, but also the connection efficiency under the condition of an obstacle-free path is improved, and the calculation cost of the RRT-Connect algorithm in the step-by-step expansion between path search trees is reduced.

Description

Technical Field

[0001] This invention belongs to the field of path planning and relates to a local obstacle avoidance path planning method, system, device and storage medium. Background Technology

[0002] Industrial robots, due to their high precision, stability, and efficiency, have become an important component of modern manufacturing, widely used in assembly, welding, and material handling, significantly improving production automation levels, reducing labor costs, and driving the rapid development of intelligent manufacturing. Industrial robot path planning is a crucial area of ​​robotics, involving how to plan and optimize the robot's movement path to complete tasks safely and efficiently in the shortest possible time.

[0003] The Rapidly-exploring Random Tree (RRT) algorithm, proposed by LaValle in 1998, is a path planning method based on random sampling. This algorithm constructs a random tree structure and expands towards the target region from the starting point until a path connecting the starting and target points is found. It features rapid exploration capabilities, applicability to high-dimensional spaces and complex environments, and ease of implementation. However, the RRT algorithm cannot guarantee finding the optimal path and may suffer from low search efficiency in narrow passages and environments with dense obstacles. The Bidirectional Rapidly-exploring Random Trees (Bi-RRT) algorithm is an improved version of the RRT algorithm. Unlike the unidirectional expansion from the starting point to the target point in the RRT algorithm, the Bidirectional RRT algorithm simultaneously starts from both the starting and target points, constructing two random trees until they meet. It features improved search efficiency, reduced search space, and applicability to symmetrical environments. The Rapidly-exploring Random Tree Connect (RRT-Connect) algorithm is a further improvement on the bidirectional RRT algorithm, aiming to enhance the efficiency of path planning. This algorithm simultaneously constructs two random trees starting from the origin and destination points, and attempts to connect them at each step. If the connection succeeds, a path is found; if it fails, the random trees are expanded further. While the RRT-Connect algorithm improves search efficiency and is applicable to various environments and constraints, it still suffers from the drawback of getting trapped in local optima. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a local obstacle avoidance path planning method, system, device and storage medium that can avoid obstacles in advance and improve the original Connect concept to help two path search trees quickly establish a connection.

[0005] To achieve the above objectives, the present invention employs the following technical solution:

[0006] A local obstacle avoidance path planning method includes the following process:

[0007] S1, construct a path search tree, determine whether there are obstacles during the path search process, and if not, generate a path;

[0008] S2, if there are obstacles, the longhorn beetle tendril expansion strategy is used to avoid the obstacles. The expanded nodes that avoid the obstacles are added to the path search tree until the entire path search is completed and the path is generated.

[0009] Preferably, in S1, the process of constructing the path search tree is as follows: starting from the starting point... x start Build the starting tree for path search for the root node T start , with target point x goal Build a path search target tree for the root node T goal .

[0010] Furthermore, the path search process is as follows: Sampling points are generated in the workspace. x rand Searching for trees T start Mid-range x rand The most recent point x near1 Searching for trees T goal Mid-range x rand The most recent point x near2 , x rand This represents a node that is randomly generated in each iteration. x near1 Indicates the distance from the starting tree x rand The nearest point, x near2 Indicates the distance in the target tree x rand The nearest point.

[0011] Further, judgment x near1 Directly extended x near2 Check if an obstacle was encountered; if no collision occurred, then connect. x near1 and xnear2 And generate a line from x start arrive x goal The path.

[0012] Furthermore, in S2, through x near1 , x near2 and x rand Three points determine the extended plane, in x near1 Two longhorn beetle whiskers were generated at the location, in x near2 Two longhorn beetle whiskers are generated at the location, and an extension node is generated on each whisker. It is determined whether the path collides with an obstacle, and nodes that do not collide are added to their respective path search trees.

[0013] Further, calculate the distance between the extended node and another search tree. If the distance is less than a set threshold and the path does not collide, then a path is generated. Otherwise, check if the number of searches is less than a set upper limit. If so, return to S1; otherwise, end the step and the search fails.

[0014] Preferably, after the path is generated, a path shortening strategy is used to optimize the path.

[0015] A local obstacle avoidance path planning system, comprising:

[0016] The path search tree module is used to construct a path search tree and determine whether there are obstacles during the path search process. If there are no obstacles, a path is generated.

[0017] The longhorn beetle whisker module is used to avoid obstacles by using a longhorn beetle whisker expansion strategy. The expanded nodes that avoid obstacles are added to the path search tree until the entire path search is completed and a path is generated.

[0018] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the local obstacle avoidance path planning method.

[0019] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the local obstacle avoidance path planning method.

[0020] Compared with the prior art, the present invention has the following beneficial effects:

[0021] The obstacle avoidance path planning algorithm proposed in this method is inspired by the beetle's antennae mechanism, which effectively solves the problem of path search trees getting stuck when approaching obstacles. By simulating the beetle's antennae sensing scents, the algorithm generates two expansion directions. Due to the angle between the expansion directions, the expansion nodes spread out to both sides, which can bypass obstacles to a certain extent, increasing the effective sampling rate. At the same time, the difference in expansion directions helps to open up the search space and find open areas to connect to the target point. Connect is essentially about approaching the target point, but the step-by-step expansion mechanism with the same step size not only increases the computational cost but also may risk approaching obstacles. By introducing the beetle's antennae, the expansion nodes are helped to find more open areas. The Connect idea is improved by directly judging whether there is an obstacle between the nearest node and the target point. If there is, no expansion connection is made. The application of this strategy not only reduces the probability of invalid expansion in non-target directions but also improves the connection efficiency under obstacle-free path conditions, while reducing the computational cost of the RRT-Connect algorithm's step-by-step expansion between path search trees. Attached Figure Description

[0022] Figure 1 The flowchart of the local obstacle avoidance path planning method of RRT-Connect based on longhorn beetle whisker optimization of the present invention is shown below;

[0023] Figure 2 A schematic diagram illustrating the construction of the extended surface using the beetle whisker algorithm of this invention;

[0024] Figure 3 This is a schematic diagram illustrating the expansion direction of the beetle whisker algorithm of the present invention;

[0025] Figure 4 This is a schematic diagram of the Rodriguez rotation of the present invention;

[0026] Figure 5 A schematic diagram illustrating the construction of extended nodes using the beetle whisker algorithm of this invention;

[0027] Figure 6 This is a schematic diagram illustrating the obstacle avoidance algorithm of the longhorn beetle whisker of the present invention;

[0028] Figure 7 This is a schematic diagram of the Connect connection for the beetle whisker algorithm of the present invention;

[0029] Figure 8 This is a schematic diagram illustrating the path shortening algorithm of the beetle whisker algorithm of the present invention. Detailed Implementation

[0030] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0031] In the description of this invention, it should be understood that the terms "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," "clockwise," and "counterclockwise," etc., indicating orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the invention. Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, features defined with "first" and "second" may explicitly or implicitly include one or more of the stated features. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0032] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terms “installation,” “connection,” and “linkage” should be interpreted broadly, for example, as a fixed connection, a detachable connection, or an integral connection; a mechanical connection, an electrical connection, or a connection that allows communication; a direct connection or an indirect connection via an intermediate medium; or a connection within two elements or an interaction between two elements. The term “and / or” as used herein includes any and all combinations of one or more of the associated listed items. Those skilled in the art will understand the specific meaning of the above terms in this invention according to the specific circumstances. The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the invention.

[0033] The following disclosure provides many different embodiments or examples for implementing various structures of the invention. To simplify the disclosure, specific examples of components and arrangements are described below. These are merely examples and are not intended to limit the invention. Furthermore, reference numerals and / or letters may be repeated in different examples; such repetition is for simplification and clarity and does not in itself indicate a relationship between the various embodiments and / or arrangements discussed. In addition, examples of various specific processes and materials are provided in this invention, but those skilled in the art will recognize the application of other processes and / or the use of other materials.

[0034] like Figure 1 As shown, this invention provides a local obstacle avoidance path planning method for industrial robots based on an improved RRT-Connect algorithm, in order to solve the problem of how to plan a better path for industrial robots in complex environments based on the RRT-Connect algorithm.

[0035] The RRT-Connect algorithm improves upon the basic RRT algorithm's long search time and tendency to get trapped in local optima. However, its rapid expansion approach can cause the path search tree to approach obstacles, limiting the effective expansion direction and requiring more sampling to escape the obstacle region, increasing the number of samplings and computational cost. Furthermore, the Connect algorithm's expansion connection idea is essentially to quickly approach the target point, but it can get trapped in local optima when obstacles are present. This method introduces the idea of ​​the beetle whisker search algorithm to bypass obstacles in advance and improves upon the original Connect idea to help two path search trees quickly establish a connection.

[0036] The path search steps of the RRT-Connect algorithm based on longhorn beetle whiskers optimization are as follows:

[0037] (1) with x start Build a path search tree for the root node T start ,by x goal Build a path search tree for the root node T goal .

[0038] (2) Generate sampling points in the workspace x rand Searching for trees T start Mid-range x rand The most recent point x near1 Searching for trees T goal Mid-range xrand The most recent point x near2 .

[0039] (3) Judgment x near1 Directly extended x near2 Check if an obstacle was encountered; if no collision occurred, then connect. x near1 and x near2 And generate a line from x start arrive x goal If the initial path is correct, proceed to step (7); otherwise, proceed to step (4).

[0040] (4) Through x near1 , x near2 and x rand Three points determine the extended plane, in x near1 Two longhorn beetle whiskers were generated at the location, in x near2 Two longhorn beetle whiskers are generated at the location, and an extension node is generated on each longhorn beetle whisker. It is determined whether the path collides with the obstacle, and the nodes that do not collide are added to their respective path search trees.

[0041] (5) Calculate the distance between the extended node and another search tree. If the distance is less than the set threshold and the path does not collide, generate the initial path and proceed to step (7); otherwise, proceed to step (6).

[0042] (6) If the number of searches is less than the set limit, return to step (2); otherwise, end the step and the search fails.

[0043] (7) Shorten and optimize the initial path to find x start Connect x goal The trajectory path.

[0044] in: x start Indicates the starting point. x goal Indicates the target point. T start This represents a path search tree originating from the starting point. T goal This represents a path search tree starting from the target point. x rand This represents a node that is randomly generated in each iteration. xnear1 Indicates the distance from the starting tree x rand The nearest point, x near2 Indicates the distance in the target tree x rand The nearest point.

[0045] In step (4), the longhorn beetle must extend its strategy, which includes the following process:

[0046] 1) Determine the extension plane

[0047] In traditional RRT algorithms, the nearest node of each path search tree expands towards the target point, and the expansion direction is the nearest target point. There is no need to determine the expansion plane. The RRT-Connect algorithm based on beetle whisker optimization uses two beetle whiskers to determine the expansion direction, so the plane in which the expansion direction is located needs to be considered.

[0048] like Figure 2 As shown, a random sampling point is first generated. x rand Then in x start Path search tree with root node T start Calculate the nearest node x near1 In the x goal Path search tree with root node T goal Calculate the nearest node x near2 Calculated by formula x near1 point to x rand vector By calculating x near1 Point to x near2 vector Through calculation x near1 , x near2 and x rand The unit normal vector corresponding to the plane containing the three points .

[0049] (1)

[0050] (2) (3)

[0051] Assumption =( a,b,c )and x near1 =( x 0, y 0, z 0), then finally, the point-normal expression is expressed through the point. x near1 And perpendicular to the vector The plane, i.e., the node x near1 expansion surface β .

[0052] (4)

[0053] 2) Determine the direction of expansion

[0054] Compared to traditional algorithms that generate only one extended node per expansion, the RRT-Connect algorithm based on beetle whisker optimization determines two expansion directions within the expansion plane during the iteration process, and further expands into two new nodes based on these directions.

[0055] like Figure 3 As shown, in the x start In the path search tree with the root node as the root node, starting from the node x near point to x rand Using the initial direction as an example, deflect to both sides within the extended plane. θ Angles are used to generate the left and right antennae of the longhorn beetle, which are then used as nodes. x near The two directions of extension are calculated using Rodriguez's rotation formula, as shown in the following equation:

[0056] (5)

[0057] The vector can be obtained by calculating using the Rodrigues rotation formula. Around a unit vector Rotation θ New vector after angle This calculation uses the rotation axis. and rotation angle θ A defined rotation matrix parameterizes the angle and direction of rotation. Figure 4 This is a schematic diagram of Rodriguez's rotation.

[0058] According to Rodriguez's rotation formula, with the unit normal vector Using the axis of rotation, the vector Rotate to the left θ The direction of expansion is obtained after the angle. , will vector Rotate in the same direction -θ The direction of expansion is obtained after the angle. .

[0059] (6)

[0060] (7)

[0061] 3) Expanding new nodes

[0062] like Figure 5 As shown, from the nearest node x near Initially, the step size is... λ Generate new extended nodes along the left beetle whiskers. x left Generate new extended nodes along the right whiskers of the longhorn beetle. x right .

[0063] First, the extended vector of the longhorn beetle's whiskers is standardized as follows:

[0064] (8)

[0065] (9)

[0066] Then, from x near Move forward a distance along a unit vector λ This results in two extended nodes:

[0067] (10)

[0068] (11)

[0069] Finally, determine the node. x near Check if the connection to the extended node collides with an obstacle. If no collision occurs, the node is successfully extended and added to the path search tree; otherwise, discard the current extended node.

[0070] In the path search process of the RRT-Connect algorithm, the "Connect" concept is introduced to facilitate the rapid expansion of the random search tree towards the target point, thereby reducing search time. However, if the algorithm encounters obstacles during the expansion process, the nodes of the search tree will become adjacent to the obstacles, potentially leading to frequent and inefficient expansion attempts in an attempt to escape the area surrounding the obstacles. While this mechanism speeds up the connection of the search tree, it also increases the time cost of invalid sampling.

[0071] The obstacle avoidance path planning algorithm proposed in this method is inspired by the antennae mechanism of longhorn beetles, which effectively solves the problem of path search trees getting stuck when approaching obstacles. By simulating the behavior of longhorn beetle antennae sensing scents, the algorithm generates two expansion directions. Due to the certain angle between the expansion directions, the expansion nodes spread out to both sides, which can bypass obstacles to a certain extent and increase the effective sampling rate. At the same time, the difference in expansion directions helps to open up the search space and help find open areas to connect to the target point. Connect is essentially about approaching the target point, but the stepwise expansion mechanism with the same length not only increases the computational cost but also may have the risk of approaching obstacles. By introducing longhorn beetle antennae, the expansion nodes are helped to find more open areas. The Connect idea is improved by directly determining whether there is an obstacle between the nearest node and the target point. If there is, no expansion connection is made.

[0072] Figure 6 This demonstrates that when there is an obstacle between the nearest node and the random sampling point, the beetle's beard algorithm, due to the angle between its antennae, ensures that the two expansion directions do not all directly point to the obstacle. This gives it a higher probability that one expansion direction will bypass the obstacle, thus reducing the generation of invalid sampling points. Figure 7 As shown, when there are no obstacles between the nearest nodes of two trees, the algorithm attempts to directly connect these two nodes with a certain probability. This aims to optimize the rapid establishment of paths and improve algorithm efficiency. The application of this strategy not only reduces the probability of invalid expansion in non-target directions but also improves connection efficiency under obstacle-free path conditions, while simultaneously reducing the computational cost of the RRT-Connect algorithm for equal-step expansion between path search trees.

[0073] In step (7), the specific process of path shortening optimization is as follows:

[0074] The RRT-Connect algorithm incorporates a beetle whisker search strategy, enhancing its ability to avoid obstacles and find optimal paths. However, the resulting paths are often quite tortuous, indicating room for further optimization and path shortening.

[0075] The path shortening strategy is used to optimize the path. The specific steps of the algorithm are as follows:

[0076] (1) In the generated initial path, the nodes in the path are numbered sequentially to obtain the path sequence. x 1, x 2, .. . , x n ];

[0077] (2) Traverse the nodes in the sequence in turn, starting with the current node. x i Centered on, with λ Use the specified radius to search for the neighboring nodes of the current node;

[0078] (3) When node x i When both its previous and next neighbors are within its search range, select the previous neighbor that is furthest away. x front and its farthest next neighbor node x behind If node x front and nodes x behind If the line connecting the two nodes does not collide with an obstacle, then remove the other nodes between them;

[0079] (4) Update the path sequence and finally obtain the new path.

[0080] like Figure 8 As shown, the initial path sequence from the initial point to the target point is [ x 1, x 2, x 3, x 4, x 5, x 6, x 7] After applying the path shortening strategy, a new path sequence is obtained. x 1, x 2, x 3, x 4, x 5, x 6, x [7] The new path is shorter than the initial path.

[0081] The following are embodiments of the apparatus of the present invention, which can be used to execute embodiments of the method of the present invention. For details not omitted in the apparatus embodiments, please refer to the embodiments of the method of the present invention.

[0082] In another embodiment of the present invention, a local obstacle avoidance path planning system is provided. This local obstacle avoidance path planning system can be used to implement the above-mentioned local obstacle avoidance path planning method. Specifically, the local obstacle avoidance path planning system includes a path search tree module and a longhorn beetle whisker module.

[0083] The path search tree module is used to construct a path search tree and determine whether there are obstacles during the path search process. If there are no obstacles, a path is generated.

[0084] The longhorn beetle whisker module is used to avoid obstacles by employing a longhorn beetle whisker expansion strategy. The expanded nodes that avoid obstacles are added to the path search tree until the entire path search is completed and a path is generated.

[0085] In another embodiment of the present invention, a terminal device is provided, comprising a processor and a memory. The memory stores a computer program, which includes program instructions. The processor executes the program instructions stored in the computer storage medium. The processor may be a Central Processing Unit (CPU), or 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. It is the computing and control core of the terminal, suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions to achieve a corresponding method flow or corresponding function. The processor described in this embodiment can be used for the operation of a local obstacle avoidance path planning method, including: S1, constructing a path search tree, determining whether there are obstacles during the path search process, and generating a path if none exist; S2, if obstacles exist, using a longhorn beetle-like expansion strategy to avoid the obstacles, adding the expanded nodes that avoid the obstacles to the path search tree, until the entire path search is completed and a path is generated.

[0086] In another embodiment, the present invention also provides a computer-readable storage medium (Memory), which is a memory device in a terminal device for storing programs and data. It is understood that the computer-readable storage medium here may include both the built-in storage medium in the terminal device and extended storage media supported by the terminal device. The computer-readable storage medium provides storage space that stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for loading and execution by a processor, which may be one or more computer programs (including program code). It should be noted that the computer-readable storage medium here may be high-speed RAM or non-volatile memory, such as at least one disk storage device.

[0087] One or more instructions stored in a computer-readable storage medium can be loaded and executed by the processor to implement the corresponding steps of the local obstacle avoidance path planning method in the above embodiments; one or more instructions in the computer-readable storage medium are loaded and executed by the processor to perform the following steps: S1, construct a path search tree, determine whether there are obstacles during the path search process, and if not, generate a path; S2, if there are obstacles, use the longhorn beetle whisker expansion strategy to avoid the obstacles, add the expansion nodes that avoid the obstacles to the path search tree, until the entire path search is completed and a path is generated.

[0088] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0089] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0090] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0091] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1The steps of the function specified in one or more boxes.

[0092] The sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0093] In the above embodiments of this application, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0094] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. The device embodiments described above are merely illustrative; for example, the division of units can be a logical functional division, and in actual implementation, there may be other division methods. For instance, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the displayed or discussed mutual coupling, direct coupling, or communication connection may be through some interfaces; the indirect coupling or communication connection between units or modules may be electrical or other forms.

[0095] The units described 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 units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0096] The above description is only a preferred embodiment of this application. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of this application, and these improvements and modifications should also be considered within the scope of protection of this application.

[0097] It should be understood that the above description is for illustrative purposes and not for limitation. Many embodiments and applications beyond the provided examples will be apparent to those skilled in the art upon reading the above description. Therefore, the scope of this patent should not be determined by reference to the above description, but rather by reference to the foregoing claims and the full scope of their equivalents. For purposes of completeness, all articles and references, including patent applications and publications, are incorporated herein by reference. The omission of any aspect of the subject matter disclosed herein in the foregoing claims is not intended as a waiver of that subject matter, nor should it be construed as an indication that the applicant has not considered that subject matter as part of the disclosed inventive subject matter.

Claims

1. A local obstacle avoidance path planning method, characterized in that, Includes the following processes: S1, construct a path search tree, determine whether there are obstacles during the path search process, and if not, generate a path; The process of constructing a path search tree is as follows: starting from the starting point x start Build the starting tree for path search for the root node T start , with target point x goal Build a path search target tree for the root node T goal ; The path search process is as follows: generate sampling points in the workspace. x rand Searching for trees T start Mid-range x rand The most recent point x near1 Searching for trees T goal Mid-range x rand The most recent point x near2 , x rand This represents a node that is randomly generated in each iteration. x near1 Indicates the distance from the starting tree x rand The nearest point, x near2 Indicates the distance in the target tree x rand The nearest point; Calculate x near1 point to x rand vector By calculating x near1 Point to x near2 vector Through calculation x near1 , x near2 and x rand The unit normal vector corresponding to the plane containing the three points ; Assumption =( a,b,c )and x near1 =( x 0, y 0, z 0), then finally, the point-normal expression is expressed through the point. x near1 And perpendicular to the vector The plane, i.e., the node x near1 expansion surface β ; judge x near1 Directly extended x near2 Check if an obstacle was encountered; if no collision occurred, then connect. x near1 and x near2 And generate a line from x start arrive x goal The path; S2, If there are obstacles, the longhorn beetle whisker expansion strategy is used to avoid the obstacles. The expanded nodes that avoid the obstacles are added to the path search tree until the entire path search is completed and the path is generated. pass x near1 , x near2 and x rand Three points determine the extended plane, in x near1 Two longhorn beetle whiskers were generated at the location, in x near2 Two longhorn beetle whiskers are generated at the location, and an extension node is generated on each longhorn beetle whisker. It is determined whether the path collides with the obstacle, and the nodes that do not collide are added to their respective path search trees. In x start In the path search tree with the root node as the root node, starting from the node x near point to x rand Using the initial direction as an example, deflect to both sides within the extended plane. θ Angles are used to generate the left and right whiskers of the longhorn beetle, which are then used as nodes. x near The two extension directions are calculated using Rodrigues' rotation formula: The vector can be obtained by calculating using the Rodrigues rotation formula. Around a unit vector Rotation θ New vector after angle This calculation uses the rotation axis and rotation angle θ A defined rotation matrix parameterizes the angle and direction of rotation.

2. The local obstacle avoidance path planning method according to claim 1, characterized in that, Calculate the distance between the expanded node and another search tree. If the distance is less than a set threshold and the path does not collide, generate a path. Otherwise, check if the number of searches is less than a set upper limit. If so, return to S1; otherwise, end the step and the search fails.

3. The local obstacle avoidance path planning method according to claim 1, characterized in that, After the path is generated, a path shortening strategy is used to optimize the path.

4. A local obstacle avoidance path planning system, characterized in that, include: The path search tree module is used to construct a path search tree and determine whether there are obstacles during the path search process. If there are no obstacles, a path is generated. The process of constructing a path search tree is as follows: starting from the starting point x start Build the starting tree for path search for the root node T start , with target point x goal Build a path search target tree for the root node T goal ; The path search process is as follows: generate sampling points in the workspace. x rand Searching for trees T start Mid-range x rand The most recent point x near1 Searching for trees T goal Mid-range x rand The most recent point x near2 , x rand This represents a node that is randomly generated in each iteration. x near1 Indicates the distance from the starting tree x rand The nearest point, x near2 Indicates the distance in the target tree x rand The nearest point; Calculate x near1 point to x rand vector By calculating x near1 Point to x near2 vector Through calculation x near1 , x near2 and x rand The unit normal vector corresponding to the plane containing the three points ; Assumption =( a,b,c )and x near1 =( x 0, y 0, z 0), then finally, the point-normal expression is expressed through the point. x near1 And perpendicular to the vector The plane, i.e., the node x near1 expansion surface β ; judge x near1 Directly extended x near2 Check if an obstacle was encountered; if no collision occurred, then connect. x near1 and x near2 And generate a line from x start arrive x goal The path; The longhorn beetle whisker module is used to avoid obstacles by using a longhorn beetle whisker expansion strategy. The expanded nodes that avoid obstacles are added to the path search tree until the entire path search is completed and a path is generated. pass x near1 , x near2 and x rand Three points determine the extended plane, in x near1 Two longhorn beetle whiskers were generated at the location, in x near2 Two longhorn beetle whiskers are generated at the location, and an extension node is generated on each longhorn beetle whisker. It is determined whether the path collides with the obstacle, and the nodes that do not collide are added to their respective path search trees. In x start In the path search tree with the root node as the root node, starting from the node x near point to x rand Using the initial direction as an example, deflect to both sides within the extended plane. θ Angles are used to generate the left and right whiskers of the longhorn beetle, which are then used as nodes. x near The two extension directions are calculated using Rodrigues' rotation formula: The vector can be obtained by calculating using the Rodrigues rotation formula. Around a unit vector Rotation θ New vector after angle This calculation uses the rotation axis and rotation angle θ A defined rotation matrix parameterizes the angle and direction of rotation.

5. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the local obstacle avoidance path planning method as described in any one of claims 1 to 3.

6. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the local obstacle avoidance path planning method as described in any one of claims 1 to 3.

Images