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

Abstract

The invention provides a rapid path planning method based on collision constraint and path point optimization. The method comprises the following steps: acquiring a path planning starting point, a target point and obstacle information; obtaining a feasible sampling point set based on collision constraints; setting a time step t as 1; at the current time step, randomly sampling from the feasible sampling point set to obtain a new path point, selecting the nearest existing path point as a father node, and calculating the shortest distance between a connecting line of the two paths and an obstacle; reselection of an RRT * algorithm and optimization of a reconnection mechanism are carried out; judging whether the current path is feasible; if t is not feasible, if t does not reach the maximum number of iterations, t is set to be t + 1, and if t reaches the maximum number of iterations, an expansion search random seed is initialized, t is reset to be 1, and currently sampled path point information is reserved; and iteratively carrying out the process until a feasible path is found, and optimizing the feasible path to obtain a final path. By adopting the scheme, the path planning efficiency is improved, and the final path is smoother.

Description

Technical Field

[0001] The present application relates to the technical field of path planning, and in particular to a fast path planning method based on collision constraints and path point optimization. Background Art

[0002] The core purpose of path planning is to plan a collision-free and safe path from the starting point to the target point within a specified time in an environment with obstacles. On the one hand, the result of path planning needs to ensure that there is a sufficient safe distance between the final path and obstacles to avoid collisions between the agent and obstacles. At the same time, the final path should be smooth enough to meet the motion constraints of the agent. In addition, in order to ensure timeliness, the path planning algorithm needs to have a good running speed to meet the needs of actual simulation tests. Path planning has a wide range of application scenarios, including automatic trajectory planning of unmanned vehicles, intelligent robot control, dynamic planning of intelligent traffic, takeaway logistics distribution, etc.

[0003] Typical path planning algorithms include search-based Dijkstra algorithm, A* algorithm, D* algorithm, intelligent optimization-based genetic algorithm, ant colony algorithm, particle swarm algorithm, and sampling-based RRT, RRT* and other methods. Search-based algorithms and intelligent optimization-based algorithms require the construction of grid maps and structured modeling, which have large computational complexity and high algorithm time complexity. They do not meet the requirements of simulation testing, and the results of algorithms based on intelligent optimization cannot guarantee the optimality. Although the sampling-based RRT* algorithm is faster than the search-based and intelligent optimization-based algorithms, for intelligent agent testing in a simulation environment, the sampling-based path planning algorithm plans a feasible path, the sampling path point turns are abrupt, and the path availability is poor. Summary of the invention

[0004] The present application aims to solve one of the technical problems in the related art at least to some extent.

[0005] To this end, the first purpose 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 complexity and poor availability of generated paths in existing methods, improves the efficiency of path planning, and makes the final path smoother.

[0006] The second object of the present application is to provide a computer device.

[0007] A third object of the present application is to provide a non-transitory computer-readable storage medium.

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

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

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

[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 point, the parent node and the calculated shortest distance, the RRT* algorithm is reselected and the reconnection mechanism is optimized;

[0014] Step S6: When the distance between the new path point and the target point is greater than a threshold, or the shortest distance between the line connecting the new path point and the target point and the obstacle is less than a safety threshold, it is determined that the path is infeasible;

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

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

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

[0018] The fast path planning method based on collision constraints and path point optimization in the embodiment of the present application is based on the traditional RRT* algorithm, combined with the characteristics of obstacles in a two-dimensional simulation environment, to optimize the algorithm's process of handling obstacles of different shapes, thereby improving the applicability of the algorithm; based on the positional relationship between path points and obstacles, collision constraints are used to reduce the sampling space, and at the same time a mechanism for early termination of path planning is introduced, thereby further improving planning efficiency; at the same time, due to the problems of non-smooth path points and a large number of inflection points on the original path, a safety cost coefficient is introduced into the path evaluation, and a path point optimization method based on angles and collision constraints is proposed, which greatly improves the availability of the path and makes the final path smoother, thereby meeting the simulation test requirements of the intelligent body.

[0019] Optionally, in one embodiment of the present application, there is no feasible sampling point within the safety threshold range of the obstacle, wherein the set of points whose distance to the obstacle is less than the safety threshold is expressed as:

[0020]

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

[0022] The shortest distance between the new path point, the parent node connection and the obstacle is:

[0023]

[0024] Among them, i is the new path point and j is the parent node.

[0025] Optionally, in one embodiment of the present application, reselection and rewiring mechanism optimization of the RRT* algorithm are performed, including:

[0026] Calculate other path points within the radius r of the new path point i and select the min Path The parent node of c(Path∪i) 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 obstacles, 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 size of the path cost when selecting the new path point i as the parent node and the original path cost. If the cost of reaching itself through the new path point i is smaller than that of the original path, select the new path point i as the new parent node.

[0028] Optionally, in one embodiment of the present 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 the present application, optimizing the feasible path to obtain the final path includes:

[0030] Correct the path points of the feasible path, delete the redundant path points in the feasible path, and add auxiliary path points;

[0031] When the number of turning points of the feasible path is greater than a threshold, or the angle of the corner in the feasible path is greater than a threshold, reverse optimization is performed on the feasible path;

[0032] The feasible path is optimized by B-spline to obtain the final feasible path.

[0033] Optionally, in one embodiment of the present 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 obstacles, and calculate whether the turning angle between the paths meets the conditions, and record the path point combinations that meet the conditions, and select the path point combination with the largest subscript interval, and delete the path point in the middle of the selected path point combination;

[0035] Added auxiliary path points, including:

[0036] Use linear interpolation to add waypoints between consecutive waypoints at a certain distance.

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

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

[0039] To achieve the above-mentioned purpose, the second embodiment of the present invention 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, the above-mentioned fast path planning method based on collision constraints and path point optimization is implemented.

[0040] In order to achieve the above-mentioned purpose, the third aspect embodiment of the present invention proposes a non-temporary computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the above-mentioned fast path planning method based on collision constraints and path point optimization is implemented.

[0041] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become apparent from the description below, or will be learned through the practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0043] Figure 1 A flowchart of a fast path planning method based on collision constraints and path point optimization provided in Example 1 of the present application;

[0044] Figure 2 A flowchart of a fast path planning method based on collision constraints and path point optimization provided in Example 2 of the present application;

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

[0046] Figure 4 An example diagram of a path after a path point correction is performed on the original path of an embodiment of the present application;

[0047] Figure 5 This is an example diagram of a path obtained after reverse optimization of the original search path in an embodiment of the present application. DETAILED DESCRIPTION

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

[0049] The following describes a fast path planning method and device based on collision constraints and path point optimization according to an embodiment of the present application with reference to the accompanying drawings.

[0050] Figure 1 A flowchart of a fast path planning method based on collision constraints and path point optimization provided in Example 1 of the present application.

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

[0052] Step 101, obtaining the starting point, target point, and obstacle information of the path planning;

[0053] Specifically, the starting point and target point of the initial path planning, the maximum number of iterations of the RRT* algorithm, the random seed for search expansion, the radius of each path growth, the target point sampling probability, the center point position of the obstacle, and the shape information of the obstacle. This patent considers the shapes of two obstacles, circular and rectangular. The circular obstacle provides the center position and radius, and the rectangular obstacle provides the center position and length and width information.

[0054] Step 102, obtaining a set of feasible sampling points based on the collision constraints between the agent and the obstacle;

[0055] Specifically, based on the collision constraint between the agent and the obstacle, a set of points whose exclusion and obstacle are smaller than a safety threshold is obtained. If the sampled path point is located in the collision area, the path point is resampled.

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

[0057] Step 104, at the current time step, randomly sample a new path point from the feasible sampling point set, 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, reselecting and reconnecting the RRT* algorithm based on the new path point, the parent node and the calculated shortest distance;

[0059] Specifically, reselection refers to reselecting the parent node, drawing a circle with the new path point obtained in step 104 as the center and the radius, connecting the new path point with all the nodes in the circle, comparing which node is the shortest path to the new node, and selecting the node in the circle that minimizes the path cost as the parent node of the new path point. In the rewiring stage, determine whether the path cost of reaching the surrounding nodes through the new path point is less than the cost of the original path. If the cost of reaching a node through the new path point is smaller than the cost of the original path, then the parent node of the node is updated to the new path point obtained in step 104.

[0060] Step 106, when the distance between the new path point and the target point is greater than a threshold, or the shortest distance between the line connecting the new path point and the target point and the obstacle is less than a safety threshold, it is determined that the path is not feasible;

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

[0062] Step 108, iterating steps 104-107 until a feasible path is found;

[0063] Step 109, optimize the feasible path to obtain the final path.

[0064] Specifically, optimizing the feasible path includes:

[0065] Correct the path points of the searched feasible path. The path point correction includes two steps: deleting redundant path points and adding path points. Deleting path points means starting from the initial point, calculating whether the connection between other path points and the initial point meets the safety threshold between obstacles, and calculating whether the turning angles between the paths meet the conditions, recording the path point combinations that meet the conditions, and selecting the path point combination with the largest subscript interval, and deleting the middle path points. Adding path points means adding path points between two path points at a certain distance after the above operation, to facilitate subsequent reverse optimization and B-spline optimization;

[0066] Determine whether to perform reverse optimization based on the corrected path. If the path still has many turning points or large angles, perform reverse optimization. If reverse optimization is performed, the corrected path is sorted in reverse order, and the path points of the path from the target point to the initial point are corrected;

[0067] The path is optimized using B-spline, which can generate the desired trajectory for different times and obstacle configurations. The B-spline curve is smooth and continuous, so it can generate a smooth curve without sharp corners or sudden changes in direction. The default number of B-spline optimization is three times, and 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 the embodiment of the present application reduces the sampling space by utilizing the positional relationship and collision constraints between path points and obstacles, and terminates the search process when the original path is feasible, further improving the planning efficiency. At the same time, in order to address the problem that the original path is not smooth and has many turning points, a safety cost coefficient is introduced into the path evaluation, and a path point optimization method based on angles and collision constraints is proposed, which greatly improves the availability of the path.

[0069] See also Figure 2 , Figure 2 A flowchart of a fast path planning method based on collision constraints and path point optimization provided in the second embodiment of the present application is shown in FIG.

[0070] Since traditional path planning algorithms have problems such as high time complexity, many path turning points, and uneven paths, 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. Figure 3 —5, further detailed description is given of the specific implementation method of this embodiment:

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

[0072] S2: Based on the collision constraint between the agent and the obstacle, calculate the set of points whose distance from the obstacle is less than the safety threshold, that is, satisfying

[0073]

[0074] Where ∈ is the safety threshold (the default value 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 two can be calculated based on the positional relationship between the horizontal and vertical coordinates of the point and the rectangle. Figure 2 It can be seen from the paths in that there are no sampled path points within a certain range around the 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) closest to the new path point. Calculate the corresponding angle according to the radius of each path increase, and recalculate the new path point i ( Figure 3 ), calculate the shortest distance between the new path point, the line segment between the parent node and the obstacle:

[0076]

[0077] For circular obstacles, the shortest distance between the center of the circle and the line segment is calculated, and for rectangular obstacles, the shortest distance between the rectangular boundary point 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 The parent node j of c(Path∪i) and its corresponding path Path, where

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

[0080] Among them, 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 between path points i and j and the obstacles, and α is the weight coefficient of the path safety margin (the default is 0.5).

[0081] For other path points within the radius, determine the path cost when selecting the new path point i as the parent node and the original path cost. If the cost of reaching itself through i is smaller than the original path, then i is selected as the new parent node.

[0082] S5: Determine whether a feasible path is found. Calculate the distance d(i, target) and c(i, target, obstacle set) between the latest path point i and the target point. When d(i, target) is less than the threshold (the default is the radius of each path increase) and c(i, target, obstacle set) satisfies the collision constraint, the path is feasible, and go to S7; otherwise, the path is not feasible, and go to S6. It can be seen that unlike the traditional RRT* algorithm that samples multiple times and continuously iterates to optimize the path, this embodiment focuses on finding a feasible path in S5, thereby greatly reducing the running time of the RRT* algorithm;

[0083] S6: Determine whether time step t has reached the maximum number of iterations (1000). If time t has reached the maximum number of iterations, reinitialize the random seed for the expansion search (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, time step t + 1, go to S3;

[0084] S7: Modify the path points of the searched feasible path. The path point modification includes the following two steps: deleting redundant path points and adding auxiliary path points. The operation of deleting redundant path points is to use a double loop. For the path sequence [start, i1, i2, …, i n ,target], first calculate whether the path segment c(start,i2, obstacle set) between [start,i2] satisfies the collision constraint. If it satisfies the constraint, it means that the path point i1 can be deleted. Find the farthest path point i that satisfies the condition k , then [i1,i2,…,i k-1 ] are all redundant path points that need to be deleted. k As the starting point, find the farthest path point i that meets the conditions s ,Note that at this time, since [start,i k ] This path, except for c(i k ,i s , obstacle set) to meet the collision constraints, Angle(start,i k ,i s ) satisfies the angle constraint (the default angle is not less than 120°). After deleting the redundant path points, assume that the remaining path point sequence is [start,i k ,i s ,…,target]. In order to facilitate the subsequent reverse optimization and B-spline optimization, the operation of adding path points refers to [start,i k ,i s ,…,target] sequence (e.g. [start,i k],[i k ,i s ]), use linear interpolation to add path points at a certain interval (the default is 15). Assume [start,i k ]The distance between them is 30, then add a path point, and the final corrected path point sequence is as follows Figure 4 As shown by the red square points in the figure. Figure 3 The original path, the corrected path point sequence is shorter and smoother;

[0085] S8: Determine whether to perform reverse optimization. If reverse optimization is performed, the paths optimized in S7 are arranged in reverse order to obtain [target,…,i s ,i k ,start], it can be seen that at this time, the path point correction is performed on the path from the target point to the initial point. The path point sequence after the reverse is as follows Figure 4 As shown by the red square points in Figure 4 The red square point indicates that the path after reverse optimization is better; if reverse optimization is not performed, it will go directly to S9;

[0086] S9: Perform B-spline optimization on the path. Assume that the final path point sequence obtained by 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 number of B-spline optimization as Where n is 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 figure 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 optimized path is smoother and more continuous. Figure 5 Path comparison Figure 4 Better.

[0087] In order to implement the above embodiments, the present invention further 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, the method described in the above embodiments is implemented.

[0088] In order to implement the above embodiments, the present invention further proposes a non-transitory computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the method of the above embodiments is implemented.

[0089] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example" or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0090] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0091] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, fragment or portion of code comprising one or more executable instructions for implementing the steps of a custom logical function or process, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by technicians in the technical field to which the embodiments of the present application belong.

[0092] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute the instructions), or in combination with these instruction execution systems, devices or apparatuses. For the purpose of this specification, "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in combination with these instruction execution systems, devices or apparatuses. More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic device), a portable computer disk box (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), a fiber optic device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing in other suitable ways if necessary, and then stored in a computer memory.

[0093] It should be understood that the various parts of the present application can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0094] A person skilled in the art may understand that all or part of the steps in the method for implementing the above-mentioned embodiment may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, which, when executed, includes one or a combination of the steps of the method embodiment.

[0095] In addition, each functional unit in each embodiment of the present application may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0096] The storage medium mentioned above may be a read-only memory, a magnetic disk or an optical disk, etc. Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be understood as limiting the present application. A person of ordinary skill in the art may change, modify, replace and modify the above embodiments within the scope of the present application.

Claims

1. A fast path planning method based on collision constraints and path point optimization, characterized in that: include: Step S1: Obtain the starting point, target point, and obstacle information of the path planning; Step S2: Based on the collision constraints between the agent and the obstacle, a set of feasible sampling points is obtained; Step S3: Set the time step t to 1; 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; Step S5: Based on the new path point, the parent node and the calculated shortest distance, the RRT* algorithm is reselected and the reconnection mechanism is optimized; Step S6: When the distance between the new path point and the target point is greater than a threshold, or the shortest distance between the line connecting the new path point and the target point and the obstacle is less than a safety threshold, it is determined that the path is infeasible; Step S7: When the path is not feasible, determine whether the time step t reaches the maximum number of iterations. If the time step t does not reach the maximum number of iterations, set the time step t to t+1. If the time step t reaches the maximum number of iterations, initialize the random seed for the extended search, reset the time step t to 1, and retain the currently sampled path point information. Step S8: Iterate steps S4-S7 until a feasible path is found; Step S9: Optimize the feasible path to obtain a final path.

2. The method according to claim 1, characterized in that There are no feasible sampling points within the safety threshold range of the obstacle, where the set of points whose distance to the obstacle is less than the safety threshold is expressed as: Among them, d(v,i) represents the distance between obstacle v and point i, and ∈ is the safety threshold; The shortest distance between the new path point, the parent node connection and the obstacle is: Among them, i is the new path point and j is the parent node.

3. The method according to claim 1, characterized in that The reselection and rewiring mechanism optimization of the RRT* algorithm includes: Calculate other path points within the radius r of the new path point i and select the min Path The parent node of c(Path∪i) 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 obstacles, and α is the weight coefficient of the path safety margin; For other path points within the radius r of the new path point i, determine the size of the path cost when selecting the new path point i as the parent node and the original path cost. If the cost of reaching itself through the new path point i is smaller than that of the original path, select the new path point i as the new parent node.

4. The method according to claim 1, characterized in that 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).

5. The method according to claim 1, characterized in that The step of optimizing the feasible path to obtain a final path includes: Correct the path points of the feasible path, delete the redundant path points in the feasible path, and add auxiliary path points; When the number of turning points of the feasible path is greater than a threshold, or the angle of the corner in the feasible path is greater than a threshold, reverse optimization is performed on the feasible path; The feasible path is optimized by B-spline to obtain the final feasible path.

6. The method according to claim 5, characterized in that The deleting of redundant path points in the feasible path includes: Starting from the starting point, calculate whether the connection between other path points and the starting point meets the safety threshold between obstacles, and calculate whether the turning angle between the paths meets the conditions, and record the path point combinations that meet the conditions, and select the path point combination with the largest subscript interval, and delete the path point in the middle of the selected path point combination; The adding of auxiliary path points includes: Use linear interpolation to add waypoints between consecutive waypoints at a certain distance.

7. The method according to claim 1, characterized in that The B-spline optimization of the feasible path comprises: Arrange the feasible paths in reverse order and make path point corrections on the path from the target point to the starting point.

8. A computer device, characterized in that: The method comprises a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the method according to any one of claims 1 to 7 is implemented.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images