Fast path planning method based on collision constraint and path point optimization

Abstract

The application provides a fast path planning method based on collision constraint and path point optimization, which comprises the following steps: obtaining a path planning starting point, a target point and obstacle information; obtaining a feasible sampling point set based on collision constraint; setting a time step t as 1; at the current time step, randomly sampling a new path point from the feasible sampling point set, selecting the nearest existing path point as the parent node of the new path point, and calculating the shortest distance between the line connecting the two path points and the obstacle; performing the optimization of the reselection and reconnection mechanism of the RRT* algorithm; judging whether the current path is feasible; when the current path is not feasible, if the t has not reached the maximum iteration number, setting the t as t+1, if the t has reached the maximum iteration number, initializing the random seed of the expansion search, resetting the t as 1, and keeping the current sampled path point information; iteratively performing the above process until a feasible path is found, and the feasible path is optimized to obtain a final path. The application adopting the above scheme improves the efficiency of path planning and makes the final path smoother.

Description

Technical Field

[0001] This application relates to the field of path planning technology, and in particular to a fast path planning method based on collision constraints and waypoint optimization. Background Technology

[0002] The core objective of path planning is to devise a collision-free, safe path from the starting point to the target point within a specified time in an environment with obstacles. On one hand, the path planning result needs to ensure a sufficient safe distance between the final path and obstacles to avoid collisions between the agent and the obstacles. Simultaneously, the final path should be smooth enough to satisfy the agent's motion constraints. Furthermore, to ensure timeliness, the path planning algorithm needs to have good running speed to meet the requirements of practical simulation testing. Path planning has a wide range of applications, including automatic trajectory planning for unmanned vehicles, intelligent robot control, intelligent traffic dynamic planning, and food delivery logistics.

[0003] Typical path planning algorithms include search-based algorithms such as Dijkstra's algorithm, A* algorithm, and D* algorithm; intelligent optimization-based algorithms such as genetic algorithms, ant colony algorithms, and particle swarm optimization; and sampling-based methods such as RRT and RRT*. Search-based and intelligent optimization-based algorithms require the construction of grid maps and structured modeling, resulting in high computational costs and time complexity, which do not meet simulation testing requirements. Furthermore, intelligent optimization-based algorithms cannot guarantee optimal results. Although the sampling-based RRT* algorithm is faster than search-based and intelligent optimization-based algorithms, for agent testing in simulated environments, the path planning algorithm based on sampling only plans a feasible path, with abrupt turns at sampled path points, resulting in poor path usability. Summary of the Invention

[0004] This application aims to at least partially address one of the technical problems in the related art.

[0005] Therefore, the first objective of this application is to propose a fast path planning method based on collision constraints and path point optimization, which solves the technical problems of large computational load and poor usability of generated paths in existing methods, improves the efficiency of path planning, and makes the final path smoother.

[0006] The second objective of this application is to propose a computer device.

[0007] The third objective of this application is to provide a non-transitory computer-readable storage medium.

[0008] To achieve the above objectives, the first aspect of this application proposes a fast path planning method based on collision constraints and path point optimization, comprising:

[0009] Step S1: Obtain the starting point, target point, and obstacle information for path planning;

[0010] Step S2: Based on the collision constraints between the agent and the obstacles, obtain a set of feasible sampling points;

[0011] Step S3: Set the time step t to 1;

[0012] Step S4: At the current time step, randomly sample a new path point from the set of feasible sampling points, select the existing path point closest to the new path point as the parent node of the new path point, and calculate the shortest distance between the line connecting the two and the obstacle.

[0013] Step S5: Based on the new path points, parent nodes, and the calculated shortest distance, optimize the RRT* algorithm's reselection and reconnection mechanism;

[0014] Step S6: If the distance between the new path point and the target point is greater than the threshold, or if the shortest distance between the line connecting the new path point and the target point and the obstacle is less than the safety threshold, the path is determined to be infeasible.

[0015] Step S7: When the path is not feasible, determine whether the time step t has reached the maximum number of iterations. If the time step t has not reached the maximum number of iterations, set the time step t to t+1. If the time step t has reached the maximum number of iterations, initialize the extended search random seed, reset the time step t to 1, and retain the currently sampled path point information.

[0016] Step S8: Iterate through steps S4-S7 until a feasible path is found;

[0017] Step S9: Optimize the feasible paths to obtain the final path.

[0018] The fast path planning method based on collision constraints and path point optimization in this application is based on the traditional RRT* algorithm. However, by combining the characteristics of obstacles in a two-dimensional simulation environment, the algorithm's process for handling obstacles of different shapes is optimized, improving its applicability. Based on the positional relationship between path points and obstacles, collision constraints are used to reduce the sampling space. Simultaneously, a path planning early termination mechanism is introduced, further improving planning efficiency. Furthermore, since the original path points are not smooth and have many inflection points, a safety cost coefficient is introduced in the path evaluation, and a path point optimization method based on angle and collision constraints is proposed. This significantly improves the usability of the path, making the final path smoother, thus meeting the simulation testing requirements of intelligent agents.

[0019] Optionally, in one embodiment of this application, there are no feasible sampling points within the safety threshold range of the obstacle, wherein the set of points whose distance from the obstacle is less than the safety threshold is represented as:

[0020]

[0021] Where d(v,i) represents the distance between obstacle v and point i, and ∈ is the safety threshold;

[0022] The shortest distance between the new waypoint, the line connecting the parent node, and the obstacle is:

[0023]

[0024] Where i is the new path point and j is the parent node.

[0025] Optionally, in one embodiment of this application, the RRT* algorithm's reselection and reconnection mechanism is optimized, including:

[0026] Calculate the other path points within radius r of the new path point i, and select the minimum value. Path c(Path∪i) is the parent node of Path and its corresponding path Path, where c(Path∪i)=c(Path)+c(j,i)+αmax(0,c(j,i, obstacle set)-∈), i represents the new path point, c(Path) represents the cost of the previous path, c(j,i) represents the straight-line distance between the new path point i and the parent node j, the difference between c(j,i, obstacle set) and ∈ represents the safety margin between the line connecting the new path point i and the parent node j and the obstacle, and α is the weight coefficient of the path safety margin;

[0027] For other path points within the radius r of the new path point i, determine the difference between the path cost when selecting the new path point i as the parent node and the original path cost. If the cost to reach itself through the new path point i is less than the original path cost, select the new path point i as the new parent node.

[0028] Optionally, in one embodiment of this application, the distance between the new path point and the target point is d(i,target), and the shortest distance between the line segment formed by the line connecting the new path point and the target point and the obstacle is c(i,target, obstacle set).

[0029] Optionally, in one embodiment of this application, the feasible path is optimized to obtain the final path, including:

[0030] The feasible path is modified by removing redundant path points and adding auxiliary path points.

[0031] If the number of turning points in a feasible path exceeds a threshold, or if the angle of a turning point in a feasible path exceeds a threshold, the feasible path is optimized in reverse.

[0032] B-spline optimization is performed on the feasible paths to obtain the final feasible paths.

[0033] Optionally, in one embodiment of this application, deleting redundant path points in a feasible path includes:

[0034] Starting from the starting point, calculate whether the connection between other path points and the starting point meets the safety threshold between them and obstacles, and calculate whether the turning angle between paths meets the conditions. Record the path point combinations that meet the conditions, and select the path point combination with the largest index interval. Delete the path points in the middle of the selected path point combination.

[0035] Add auxiliary waypoints, including:

[0036] By using linear interpolation, path points are added between consecutive path points at certain intervals.

[0037] Optionally, in one embodiment of this application, B-spline optimization of the feasible path includes:

[0038] Arrange the feasible paths in reverse order and perform path point corrections on the paths from the target point to the starting point.

[0039] To achieve the above objectives, a second aspect of the present invention provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the above-described fast path planning method based on collision constraints and path point optimization.

[0040] To achieve the above objectives, a third aspect of the present invention provides a non-transitory computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described fast path planning method based on collision constraints and path point optimization.

[0041] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0042] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0043] Figure 1 This is a flowchart illustrating a fast path planning method based on collision constraints and path point optimization provided in Embodiment 1 of this application.

[0044] Figure 2 This is a flowchart illustrating a fast path planning method based on collision constraints and path point optimization provided in Embodiment 2 of this application.

[0045] Figure 3This is an example diagram of the original feasible path searched based on collision constraints and the RRT* algorithm in an embodiment of this application;

[0046] Figure 4 An example diagram of the path after a path point correction for the original path in this application embodiment;

[0047] Figure 5 This is an example diagram of the path obtained after reverse optimization of the original search path according to an embodiment of this application. Detailed Implementation

[0048] The embodiments of this application are described in detail below. Examples of these embodiments 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 intended to explain this application, and should not be construed as limiting this application.

[0049] The following description, with reference to the accompanying drawings, describes a fast path planning method and apparatus based on collision constraints and waypoint optimization according to embodiments of this application.

[0050] Figure 1 This is a flowchart illustrating a fast path planning method based on collision constraints and path point optimization provided in Embodiment 1 of this application.

[0051] like Figure 1 As shown, this fast path planning method based on collision constraints and waypoint optimization includes the following steps:

[0052] Step 101: Obtain the starting point, target point, and obstacle information for path planning;

[0053] Specifically, the initialization process includes the starting point and target point of the path planning, the maximum number of iterations of the RRT* algorithm, the random seed for search expansion, the radius of each path increase, the target point sampling probability, the center point position of obstacles, and the shape information of obstacles. This patent considers both circular and rectangular obstacle shapes; for circular obstacles, the center position and radius are provided, while for rectangular obstacles, the center position and length and width information are provided.

[0054] Step 102: Based on the collision constraints between the agent and the obstacles, obtain a set of feasible sampling points;

[0055] Specifically, based on the collision constraints between the agent and the obstacle, a set of points that are excluded or whose obstacles are less than the safety threshold is obtained. If the sampled path point is located within the collision area, the path point is resampled.

[0056] Step 103, set the time step t to 1;

[0057] Step 104: At the current time step, randomly sample a new path point from the set of feasible sampling points, select the existing path point closest to the new path point as the parent node of the new path point, and calculate the shortest distance between the line connecting the two and the obstacle.

[0058] Step 105: Based on the new path point, parent node, and calculated shortest distance, optimize the RRT* algorithm's reselection and reconnection mechanism.

[0059] Specifically, reselection refers to reselecting the parent node. A circle is drawn with the new path point obtained in step 104 as the center and radius. The new path point is connected to all nodes within the circle. The path cost to the new node is compared to see which node has the shortest cost. The node within the circle that minimizes the path cost is selected as the parent node of the new path point. In the rewiring stage, it is determined whether the path cost to the surrounding nodes through the new path point is less than the cost of the original path. If the cost to reach a node through the new path point is less than the cost of the original path, then the parent node of that node is updated to the new path point obtained in step 104.

[0060] Step 106: If the distance between the new path point and the target point is greater than the threshold, or if the shortest distance between the line connecting the new path point and the target point and the obstacle is less than the safety threshold, the path is determined to be infeasible.

[0061] Step 107: When the path is not feasible, determine whether the time step t has reached the maximum number of iterations. If the time step t has not reached the maximum number of iterations, set the time step t to t+1. If the time step t has reached the maximum number of iterations, initialize the extended search random seed, reset the time step t to 1, and retain the currently sampled path point information.

[0062] Step 108: Iterate through steps 104-107 until a feasible path is found;

[0063] Step 109: Optimize the feasible paths to obtain the final path.

[0064] Specifically, optimizing feasible paths includes:

[0065] The searched feasible paths undergo path point correction. Path point correction involves two steps: deleting redundant path points and adding path points. Deleting path points means starting from the initial point, calculating whether the lines connecting other path points to the initial point meet the safety thresholds between them and obstacles, and simultaneously calculating whether the turning angles between paths meet the conditions. Path point combinations that meet the conditions are recorded, and the path point combination with the largest index interval is selected, with the intermediate path points being deleted. Adding path points means, after the above operations, adding path points at a certain distance between two path points to facilitate subsequent backpropagation and B-spline optimization.

[0066] The system determines whether to perform reverse optimization based on the corrected path. If the path still has many turning points or large angles, reverse optimization is performed. If reverse optimization is performed, the corrected path is arranged in reverse order, and path point corrections are made for the path from the target point to the initial point.

[0067] The path is optimized using B-spline optimization. This optimization method can generate the required trajectory for different degrees and obstacle configurations. B-spline curves are smooth and continuous, so they can generate smooth curves without sharp corners or sudden changes in direction. The default number of B-spline optimizations is three, but it can also be dynamically adjusted according to the number of path points.

[0068] The fast path planning method based on collision constraints and path point optimization in this application reduces the sampling space by utilizing the positional relationship between path points and obstacles and collision constraints. Furthermore, it terminates the search process as soon as the original path becomes feasible, further improving planning efficiency. Simultaneously, addressing the issues of non-smooth original paths and numerous vertices, a safety cost coefficient is introduced into the path evaluation, and a path point optimization method based on angle and collision constraints is proposed, significantly improving path usability.

[0069] Please see Figure 2 , Figure 2 This is a flowchart illustrating a fast path planning method based on collision constraints and path point optimization, provided in Embodiment 2 of this application. The method is executed by an electronic device.

[0070] Traditional path planning algorithms suffer from high time complexity, numerous path turning points, and uneven path smoothness. This embodiment proposes a fast path planning method based on collision constraints and path point optimization. The overall process of the method is as follows: Figure 2 As shown below. (See attached diagram.) Figure 3 —5. Further detailed description of the specific implementation of the method in this embodiment is provided:

[0071] S1: Initialize the starting point of the path planning (start( Figure 3 (The red cross mark in the image), target point ( Figure 3 The map contains the black triangle points in the map, the maximum number of iterations of the RRT* algorithm (set to 1000 in this embodiment), the random seed for search expansion (default 1), the radius length of each path increase (default 50), the target point sampling probability (default 10%), the location of the obstacle center point and the shape information of the obstacle, and the default map size is 1000×1000. Figure 3 It contains red circular obstacles and blue rectangular obstacles. The information for the circular obstacles is <center, radius>, and the information for the rectangular obstacles is <center point, rectangle length, rectangle width>.

[0072] S2: Based on the collision constraints between the agent and obstacles, calculate the set of points where the collision with the obstacle is less than a safety threshold, i.e., satisfying...

[0073]

[0074] Where ∈ represents the safety threshold (default is 10). For circular obstacles, the distance between the point and the center of the circle is the shortest distance. For rectangular obstacles, the shortest distance between the point and the rectangle can be calculated based on the relationship between the point's x and y coordinates and the rectangle's position. From Figure 2 The path shows that there are no sampled path points within a certain range around circular and rectangular obstacles;

[0075] S3: Initialize time step t to 1. Sample the new path point and select the existing path point j (parent node) that is closest to the new path point. Calculate the corresponding angle based on the radius of each path increase, and recalculate the new path point i. Figure 3 (At the green inflection point in the graph), calculate the shortest distance between the new path point, the parent node, and the obstacle:

[0076]

[0077] Specifically, for circular obstacles, the shortest distance between the center of the circle and the line segment is calculated; for rectangular obstacles, the shortest distance between the boundary point of the rectangle and the line segment is calculated.

[0078] S4: Optimize the parent node reselection and reconnection mechanism of the RRT* algorithm. For the new path point i in S3, calculate other points within the radius r (default is 80), and select the minimum... Path c(Path∪i) is defined by its parent node j and its corresponding path Path.

[0079] c(Path∪i)=c(Path)+c(j,i)+αmax(0,c(j,i,set of obstacles)-∈)

[0080] Where i represents the new path point, c(Path) represents the cost of the previous path, c(j,i) represents the straight-line distance between the new path point i and its parent node j in Path, the difference between c(j,i, obstacle set) and ∈ represents the safety margin between the line connecting path points i and j and the obstacles, and α is the weighting coefficient of the path safety margin (default is 0.5).

[0081] For other path points within the radius, compare the path cost when selecting the new path point i as the parent node with the original path cost. If the cost of reaching itself through i is less than the original path cost, then select i as the new parent node.

[0082] S5: Determine if a feasible path has been found. Calculate the distances d(i,target) and c(i,target, obstacle set) between the latest path point i and the target point. If d(i,target) is less than the threshold (default is the radius of each path increase) and c(i,target, obstacle set) satisfies the collision constraint, then the path is feasible, and proceed to S7; otherwise, the path is not feasible, and proceed to S6. It can be seen that, unlike the traditional RRT* algorithm which samples multiple times and iteratively optimizes the path, this embodiment focuses on finding a feasible path in S5, thus significantly reducing the running time of the RRT* algorithm.

[0083] S6: Determine if time step t has reached the maximum number of iterations (1000). If time t has reached the maximum number of iterations, reinitialize the extended search random seed (set to 2), reset time step t = 1, and go to S3 (note that the path point information of the last sampling is retained at this time); otherwise, increase time step t by 1 and go to S3.

[0084] S7: Perform path point correction on the found feasible paths. Path point correction includes the following two steps: deleting redundant path points and adding auxiliary path points. The operation of deleting redundant path points uses a nested loop, for the path sequence [start, i1, i2, ..., i... n First, calculate whether the path segment c(start, i2, obstacle set) between [start, i2] satisfies the collision constraint. If it does, then path point i1 can be deleted. Find the farthest path point i that satisfies the condition. k Then [i1,i2,…,i k-1 These are all redundant path points that need to be deleted. Next, let's consider i... k Starting from point i, find the farthest path i that satisfies the given conditions. s Note that since [start, i] already exists at this point... k This path, except for c(i) k i s For the obstacle set to satisfy the collision constraints, Angle(start, i) also needs to be used. k i s The angle constraint is satisfied (default angle is not less than 120°). After deleting redundant path points, assume the remaining path point sequence is [start, i...]. k i s [,…,target]. To facilitate subsequent backpropagation and B-spline optimization, adding path points refers to the operation on [start,i]... k i s Between consecutive node pairs in the sequence [start, ..., target] (e.g., [start, i, ..., target]) k],[i k i s This method uses linear interpolation to add path points at certain intervals (default is 15). Assume [start, i...] k If the distance between the two is 30, then add a waypoint. The final corrected waypoint sequence is as follows: Figure 4 The red square in the image shows that, compared to... Figure 3 The original path is corrected, and the path point sequence is shorter and smoother.

[0085] S8: Determine whether to perform reverse optimization. If reverse optimization is performed, then reverse the order of the paths optimized in S7 to obtain [target,…,i s i k [,start] indicates that this step involves correcting the path points from the target point to the initial point. The reversed path point sequence is as follows: Figure 4 As shown by the red square in the image, it can be seen that relative to... Figure 4 The red square indicates a better path after reverse optimization; otherwise, proceed directly to S9.

[0086] S9: Perform B-spline optimization on the path. Assume the final path point sequence obtained in S7 is [start, i k i s [,…,target], calculate the number of path points in the sequence. If the number of path points is less than the minimum number required for B-spline optimization, then select the B-spline optimization order as […]. Where n represents the number of path points. Since B-spline curves are smooth and continuous, they can generate smooth curves without sharp corners or sudden changes in direction. Figure 4 and Figure 5 The blue dots in the diagram represent the path point sequence after B-spline optimization. It can be seen that the optimized path is smoother and more continuous, and the reverse optimization... Figure 5 Path comparison Figure 4 Better.

[0087] To implement the above embodiments, the present invention also proposes a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the methods described in the above embodiments.

[0088] To implement the above embodiments, the present invention also proposes a non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the method of the above embodiments.

[0089] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0090] 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 technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "multiple" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0091] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing custom logic functions or processes, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of this application pertain.

[0092] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0093] It should be understood that various parts of this application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0094] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0095] Furthermore, the functional units in the various embodiments of this application can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0096] The storage medium mentioned above can be a read-only memory, a disk, or an optical disk, etc. Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of this application.

Claims

1. A fast path planning method based on collision constraints and path point optimization, characterized in that, The method comprises the following steps: Step S1: obtaining a starting point, a target point and obstacle information of path planning; Step S2: obtaining a set of feasible sampling points based on collision constraints between an agent and obstacles; Step S3: Setting time step is 1; Step S4: at a current time step, randomly sampling a new path point from the set of feasible sampling points, selecting an existing path point closest to the new path point as a parent node of the new path point, and calculating a shortest distance between a line connecting the new path point and the parent node and an obstacle; Step S5: performing reselection and reconnection mechanism optimization of the RRT* algorithm based on the new path point, the parent node and the calculated shortest distance; Step S6: determining that a path is infeasible when a distance between the new path point and the target point is greater than a threshold value, or a shortest distance between a line connecting the new path point and the target point and an obstacle is less than a safety threshold value; Step S7: If the path is not feasible, judge whether the time step reaches the maximum iteration number, if the time step does not reach the maximum iteration number, set the time step to , if the time step reaches the maximum iteration number, initialize the random seed of the expansion search, reset the time step to 1, and keep the current sampled path point information; Step S8: iteratively performing steps S4-S7 until a feasible path is found; Step S9: optimizing the feasible path to obtain a final path; The reselection and reconnection mechanism optimization of the RRT* algorithm comprises: calculating a new path point radius other path points inside the circle and selecting the parent node of the new path point and its corresponding path wherein, , denotes the new path point, denotes the cost of the previous path, denotes the straight-line distance between the new path point and the parent node, the difference between and denotes the safety margin between the connecting line between the new path point and the parent node, and the obstacle, is a weight coefficient for the path safety margin; For new path point Radius Other path points inside, judge to select new path point The size of path cost when being parent node and original path cost, if through new path point The cost to reach itself is smaller than the original path, select new path point As new parent node; The optimization of the feasible path to obtain the final path comprises: performing path point correction on the feasible path, deleting redundant path points in the feasible path, and adding auxiliary path points; performing reverse optimization on the feasible path when a number of turning points of the feasible path is greater than a threshold value or an angle of a turning corner in the feasible path is greater than a threshold value; performing B-spline optimization on the feasible path to obtain the final feasible path.

2. The method of claim 1, wherein, There is no feasible sampling point within a safety threshold value range of the obstacle, wherein a set of points with a distance less than the safety threshold value from the obstacle is represented as: wherein, indicates an obstacle and the distance, of the point, is a safety threshold; The shortest distance between the line connecting the new path point and the parent node and the obstacle is: wherein, is a new path point, is a parent node.

3. The method of claim 1, wherein, The distance between the new path point and the target point is The shortest distance between the line segment formed by the new path point and the target point and the obstacle is .

4. The method of claim 1, wherein, The deletion of the redundant path points in the feasible path comprises: starting from the starting point, calculating whether a line between other path points and the starting point satisfies a safety threshold value from the obstacle, calculating whether a turning angle between paths satisfies a condition, recording path point combinations satisfying the condition, and selecting a path point combination with a maximum index interval and deleting a path point in the middle of the selected path point combination; The addition of the auxiliary path points comprises: adding path points between continuous path points at a certain distance by using linear interpolation.

5. The method of claim 1, wherein, The B-spline optimization of the feasible path comprises: arranging the feasible path in a reverse order and performing path point correction on a path from the target point to the starting point.

6. A computer device, comprising: The computer program is executed by the processor to implement the method of any one of claims 1-5.

7. A non-transitory computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the method of any one of claims 1-5.

Images