AGV path smooth planning method based on adaptive heuristic optimization under multiple constraints

Abstract

The invention relates to an AGV path smooth planning method based on adaptive heuristic optimization under multiple constraints. Comprising the following steps: 1, constructing a grid map based on a Layou graph of an intelligent manufacturing workshop, endowing each grid unit with a specific steering constraint according to a constraint condition of the operation flow direction of AGV equipment, and generating a grid steering constraint table; 2, improving a path cost evaluation function, and introducing a reverse penalty term; 3, optimizing the weight of the heuristic function, and designing a self-adaptive heuristic weight factor; and 4, introducing a secondary Bezier curve to carry out path smoothing, and finally obtaining a smooth AGV path. According to the AGV equipment operation flow direction required by an intelligent manufacturing workshop, steering constraints are given to each grid, a grid steering constraint table is generated, a reverse penalty term is added to a path cost evaluation function of a traditional A * algorithm, and a self-adaptive heuristic weight factor is designed in a heuristic function; and introducing a quadratic Bezier curve to smooth the planned initial path, and planning a smooth optimal path to realize efficient and smooth planning of the AGV path.

Description

Technical Field

[0001] The invention belongs to the technical field of AGV path planning, and in particular is an AGV path smoothing planning method of adaptive heuristic optimization under multiple constraints. Background Art

[0002] In recent years, with the acceleration of the intelligentization process of the global manufacturing industry, the traditional manufacturing model is facing unprecedented challenges. In the production workshop, a large number of non-productive operations such as material transportation, loading and unloading, etc., occupy more than 75% of the workshop time, significantly affecting the overall production efficiency. To solve this problem, companies have begun to use automatic guided vehicles (AGVs) to replace traditional manual transportation methods and traditional equipment such as forklifts and conveyor belts. Traditional AGV technologies such as fixed tracks and magnetic strip navigation have problems of poor adaptability and insufficient flexibility in practical applications, especially in complex and dynamically changing production environments, and it is difficult to meet the needs of efficient and flexible logistics and transportation.

[0003] At present, trackless navigation technology represented by QR code navigation has been widely used in AGV because it can adapt to complex environments. In the complex environment of new energy intelligent manufacturing workshops, AGV path planning faces multiple constraints, such as path flow requirements, densely distributed obstacles, and frequent traffic control. These constraints increase the difficulty of path planning, requiring AGV to not only efficiently seek feasible paths, but also ensure the smoothness of the path to avoid frequent turning or sudden braking operations that affect the stability of the system. Although the traditional A* algorithm performs well in static environments, it is prone to problems such as low search efficiency, uneven path turns, and poor reliability in multi-constrained environments, especially in complex working conditions, which may cause the AGV to be unstable or even stuck. Summary of the invention

[0004] To solve the above problems, the present invention provides an AGV path smoothing planning method with adaptive heuristic optimization under multiple constraints. According to the running direction of AGV equipment required by the intelligent manufacturing workshop, a turning constraint is assigned to each grid and a grid turning constraint table is generated. A reverse penalty term is added to the path cost evaluation function of the traditional A* algorithm, and an adaptive heuristic weight factor is designed in the heuristic function. A quadratic Bezier curve is introduced to smooth the planned initial path, so as to more efficiently plan a smooth optimal path under the constraints of the actual scene, ensure that various constraints are effectively met in the actual scene, and realize efficient and smooth planning of the AGV path.

[0005] The technical solution of the present invention is described as follows in conjunction with the accompanying drawings:

[0006] An AGV path smoothing planning method with adaptive heuristic optimization under multiple constraints includes the following steps:

[0007] Step 1: Build a grid map based on the layout diagram of the intelligent manufacturing workshop. According to the constraints of the AGV equipment running direction, assign specific turning constraints to each grid unit to generate a grid turning constraint table.

[0008] Step 2: Improve the path cost evaluation function and introduce a reverse penalty term;

[0009] Step 3: Optimize the heuristic function weight and design an adaptive heuristic weight factor;

[0010] Step 4: Introduce the second degree Bezier curve to smooth the path and finally obtain a smooth AGV path.

[0011] Furthermore, the specific method of step one is as follows:

[0012] 11) Based on the layout diagram of the intelligent manufacturing workshop and the AGV QR code navigation method, a grid map is constructed; the grid types constructed include logistics distribution channels, production lines and obstacles;

[0013] Among them, Grid∈R m×n is an m×n dimensional grid matrix, which is initialized to consist of 0 and 1. m is the number of rows and n is the number of columns. m and n are determined according to the actual environment of the smart workshop. (m, n) is the coordinate of the grid. m×n is an M*N matrix in the real number field, R is the real number field, Grid is an M*N matrix in the real number field but the matrix is ​​initialized to consist of 0.1 inside;

[0014] 12) According to the forward and reverse staggered running direction of the AGV equipment required by the intelligent manufacturing workshop, a steering constraint is assigned to each grid and a steering constraint table S is generated;

[0015] Among them, S∈R m×n It is an m×n dimensional steering constraint matrix; m is the number of rows of the grid; n is the number of columns of the grid. Each element is composed of 0, 1, 2, 3, and 4 according to the steering constraint, and each grid coordinate corresponds one to one with the grid in the grid map Grid.

[0016] Furthermore, the specific method of step 2 is as follows:

[0017] 21) Add the inverse penalty term q(n) to the traditional A* cost evaluation function f(n) to obtain the improved cost evaluation function as shown below:

[0018] f(n)=g(n)+h(n)+q(n)

[0019] Among them, q(n) is the reverse penalty term, expressed as:

[0020]

[0021] 22) Define the direction of the current grid traversal as O, and define the four directions of horizontal left, horizontal right, vertical upward, and vertical downward as: h + 、h - 、v + 、v - , b represents the traversal of the current grid point to the direction in which the AGV is allowed to run, expressed as:

[0022]

[0023] Furthermore, the specific method of step three is as follows:

[0024] 31) The total cost function from the starting point through the current grid to the target point after improvement is:

[0025] f(n)=g(n)+w h h(n)+q(n)

[0026] Among them, w h Represents the adaptive heuristic weight factor of the heuristic function h(n), expressed as:

[0027]

[0028] Where ln(x) is the natural logarithm of x.

[0029] Furthermore, the specific method of step 4 is as follows:

[0030] 41) Based on the initialization of the open list open_list and the closed list closed_list; put the task starting point into the open list open_list, and set the closed list closed_list to empty;

[0031] Among them, the open list open_list represents the grid nodes currently waiting to be explored, and the closed list closed_list represents all the grid nodes that have been processed and will not be reprocessed;

[0032] 42) Enter the main loop, obtain the global path planning from the task starting point to the task end point generated by the A* algorithm; select the node i with the smallest f value from the open list, remove it from the open list open_list, and add it to the closed list closed_list;

[0033] 43) Determine whether the traversal end condition is met. If the node i currently being processed is the task end point or the minimum value from the starting point to the end point is infinite, then exit the loop, end the search, and obtain the global path planning from the task start point to the task end point; if the traversal end condition is not met, select the adjacent node j of node i according to the AGV running direction restriction; if the adjacent node j is in the closed list closed_list or exceeds the map range, then ignore the point, otherwise calculate the g value of node j according to the open list;

[0034] 44) If the newly calculated g value is less than the current g value, set j's parent node to i and put it into the Parent matrix; where Parent∈R m×n , is the parent node matrix, used to update the parent node of each node, m is the number of rows of the grid, and n is the number of columns of the grid;

[0035] 45) Repeat the traversal until the traversal end condition is met. According to the traversal end condition, if the minimum value from the starting point to the end point is infinite, the loop ends, indicating that there is no feasible path for the current task; if the current traversal point is the task end point, obtain the initial path from the task start point to the task end point that meets the actual operation constraints;

[0036] If the initial path is obtained, the path is smoothed by checking whether the three consecutive nodes in the initial path are collinear. If not, the path is smoothed by a second-order Bezier curve. This process is repeated until an optimal and smooth path is obtained. The smooth curve is expressed as follows:

[0037]

[0038] Where B(t) is a second-order Bezier curve; t is the range of curve parameters [0,1]; P i is a control point, corresponding to three consecutive non-collinear nodes.

[0039] The beneficial effects of the present invention are:

[0040] 1) The present invention proposes an AGV path smoothing planning method with adaptive heuristic optimization under multiple constraints. The path planning method includes: according to the constraints of the AGV running flow in the intelligent manufacturing workshop, a specific turning constraint is assigned to each grid unit to ensure that the physical limitations of the workshop can be followed during the path planning process, and the path cost evaluation function is improved. In addition to considering the traditional distance cost, a reverse penalty term is added to significantly improve the accuracy of the path evaluation, and the heuristic function weight is improved. The traditional heuristic weight factor is improved into an adaptive heuristic weight factor, and a quadratic Bezier curve is introduced to smooth the initial path to ensure that the planned path is not only the shortest, most efficient and smooth, but also most consistent with the actual turning ability of the AGV and the operation logic and planning real-time of the workshop;

[0041] 2) At the AGV application level, the present invention is based on the layout diagram of the intelligent manufacturing workshop and discretizes the environment into regular grids, which is intuitive and easy to visualize. By combining the discretization characteristics of the grid map and the heuristic search ability of the A* algorithm, the optimal path can be efficiently planned in the actual scenario, thereby improving the efficiency and accuracy of material distribution. This method not only helps to optimize the production process and improve production efficiency, but also reduces the waste of human and space resources, providing important support for enterprises to pursue the goal of low cost and high efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0043] Figure 1 It is a schematic diagram of the process of the present invention;

[0044] Figure 2 The present invention provides a schematic diagram of AGV distribution flow constraints in a certain intelligent manufacturing workshop with one-way lanes staggered in forward and reverse directions;

[0045] Figure 3 This is a diagram showing the experimental results of the present invention on AGV path planning;

[0046] Figure 4 This is the experimental result of the traditional A* algorithm for AGV path planning. DETAILED DESCRIPTION

[0047] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0048] Embodiment 1

[0049] See also Figure 1 , this embodiment provides an AGV path smoothing planning method with adaptive heuristic optimization under multiple constraints, comprising the following steps:

[0050] Step 1: Build a grid map based on the layout diagram of the intelligent manufacturing workshop. According to the constraints of the AGV equipment running direction, assign specific steering constraints to each grid unit and generate a grid steering constraint table, as follows:

[0051] 11) Based on the layout diagram of the intelligent manufacturing workshop and the AGV QR code navigation method, a grid map is constructed; the grid types constructed include logistics distribution channels, production lines and obstacles;

[0052] The layout diagram (usually a CAD diagram or a floor plan) is discretized into grid units of equal size that meet the actual workshop AGV navigation requirements; different areas are defined as different types of grids, such as logistics channels, obstacles, production lines, etc.; and each grid state (passable, obstacle) is represented by a binary method.

[0053] Among them, Grid∈R m×n is an m×n dimensional grid matrix, which is initialized to consist of 0 and 1. m is the number of rows and n is the number of columns. m and n are determined according to the actual environment of the smart workshop. (m, n) is the coordinate of the grid. m×n is an M*N matrix in the real number field, R represents the real number field, Grid is an M*N matrix in the real number field but the initialization matrix is ​​composed of 0.1;

[0054] 12) According to the forward and reverse staggered running direction of the AGV equipment required by the intelligent manufacturing workshop, a steering constraint is assigned to each grid and a steering constraint table S is generated;

[0055] Among them, S∈R m×n It is an m×n dimensional steering constraint matrix; m is the number of rows of the grid; n is the number of columns of the grid. Each element is composed of 0, 1, 2, 3, and 4 according to the steering constraint, and each grid coordinate corresponds one to one with the grid in the grid map Grid.

[0056] Step 2: Improve the path cost evaluation function and introduce a reverse penalty term, as follows:

[0057] 21) Consider that in the traditional A* algorithm, the core part is the path cost evaluation function, which determines the priority of each node, thereby guiding the algorithm to search towards the target point; the path cost evaluation function is implemented by calculating the total cost from the starting point through the current grid node to the target grid node. This function is usually expressed as:

[0058] f(n)=g(n)+h(n)

[0059] In the formula, g(n) is the actual cost of the current grid point from the starting point; h(n) is the estimated distance from the current grid to the end grid;

[0060] 22) Combined with the actual AGV operation constraints in the intelligent manufacturing workshop, the heuristic function h(n) of the A* algorithm is selected to estimate the distance between the current grid and the end grid; the commonly used heuristic functions h(n) include Chebyshev distance, Manhattan distance and Euclidean distance;

[0061] Among them, Chebyshev distance is the Chebyshev distance, which refers to the diagonal distance between two grid points, that is, the current grid i coordinate (x i ,y i ) and the grid coordinates (x i+1 ,y i+1 ) is the maximum value of the absolute value of the difference between the horizontal coordinate and the vertical coordinate:

[0062] dis(i,j)=max(|x i -x j |,|y i -y j |)

[0063] Manhattan distance refers to the sum of the absolute wheelbases of two grid points in the standard coordinate system, that is, the current grid i coordinate (x i ,y i ) and the grid coordinates (x i+1 ,y i+1 ) is the sum of the absolute values ​​of the horizontal and vertical coordinate differences:

[0064] dis(i,j)=|x i -x j |+|y i -y j |

[0065] Euclidean distance is the straight-line distance between two grid points in the standard coordinate system, that is, the current grid i coordinate (x i ,y i ) and the grid coordinates (x i+1 ,y i+1 ) is the square root of the sum of the squares of the abscissa and ordinate differences:

[0066]

[0067] When the running direction of the AGV is free and unrestricted, the heuristic function h(n) selects the Euclidean distance as the distance estimate, which is more appropriate than the other two distances. When the running direction of the AGV is limited to the eight directions of southwest, southeast, northwest, northeast, north, south, east, and west, the heuristic function h(n) selects the Chebyshev distance as the distance estimate, which is more appropriate. When the running direction of the AGV is limited to the four directions of north, south, east, and west, the heuristic function h(n) selects the Manhattan distance as the distance estimate, which is more appropriate. In the intelligent manufacturing workshop, the running direction of the AGV is limited to the four directions of north, south, east, and west, so the heuristic h(n) selects the Manhattan distance as the distance estimate;

[0068] 23)Reference Figure 2 , considering that the traditional A* cost evaluation function f(n) is not applicable to the specific running flow constraints of AGV equipment required in the intelligent manufacturing workshop, in order to apply the path planning under the above constraints, the reverse penalty term q(n) is added to the traditional A* cost evaluation function f(n), and the improved cost evaluation function is obtained:

[0069] f(n)=g(n)+h(n)+q(n)

[0070] Among them, q(n) is the reverse penalty term and can be expressed as:

[0071]

[0072] The current grid traversal direction is defined as O, and the four directions of horizontal left, horizontal right, vertical upward, and vertical downward are defined as: h + 、h - 、v + 、v - , b represents the traversal of the current grid point to the direction in which the AGV is allowed to run, which can be expressed as:

[0073]

[0074] Step 3: Optimize the heuristic function weight and design an adaptive heuristic weight factor;

[0075] 31) By setting an adaptive heuristic weight factor w for the heuristic function h(n) h , when the current node is far from the end point, the weight of the heuristic function h(n) increases, the number of traversed nodes is reduced, and the search efficiency is improved. When the current node is close to the end point, the weight of the heuristic function h(n) decreases to improve the search accuracy and obtain a shortest path. The total cost function from the starting point through the current grid to the target point after improvement is:

[0076] f(n)=g(n)+wh h(n)+q(n)

[0077] Among them, w h is the adaptive heuristic weight factor of the heuristic function h(n), expressed as:

[0078]

[0079] Where ln(x) is the natural logarithm of x.

[0080] Step 4: Introduce the second degree Bezier curve to smooth the path, and finally obtain a smooth AGV path, as follows:

[0081] 41) Based on the initialization of the open list open_list and the closed list closed_list; put the task starting point into the open list open_list, and set the closed list closed_list to empty;

[0082] Among them, the open list open_list represents the grid nodes currently waiting to be explored, and the closed list closed_list represents all the grid nodes that have been processed and will not be reprocessed;

[0083] 42) Enter the main loop, obtain the global path planning from the task start point to the task end point generated by the A* algorithm; select the node i with the smallest f value from the open list, remove it from the open list open_list, and add it to the closed list closed_list; since the A* algorithm only considers simple distance optimization, it is not suitable for the multiple constraints environment mentioned in the invention. Therefore, the present invention mentions an improved A* algorithm based on multiple constraints, which is suitable for complex scenarios with multiple constraints, has better efficiency than traditional algorithms, is reliable and adaptable, and has high smoothness of the final planned path

[0084] 43) Determine whether the traversal end condition is met. If the node i currently being processed is the task end point or the minimum value from the starting point to the end point is infinite, then exit the loop, end the search, and obtain the global path planning from the task start point to the task end point; if the traversal end condition is not met, select the adjacent node j of node i according to the AGV running direction restriction; if the adjacent node j is in the closed list closed_list or exceeds the map range, then ignore the point, otherwise calculate the g value of node j (g(n) value in the total cost function) according to the open list;

[0085] 44) If the newly calculated g value is less than the current g value, set j's parent node to i and put it into the Parent matrix; where Parent∈R m×n, is the parent node matrix, which is a matrix in the initialization step of the A* algorithm. It is used to update the parent node of each node and trace back to the complete path after the traversal is completed. m is the number of rows in the grid, and n is the number of columns in the grid.

[0086] 45) Repeat the traversal until the traversal end condition is met. According to the traversal end condition, if the minimum value from the starting point to the end point is infinite, the loop ends, indicating that there is no feasible path for the current task; if the current traversal point is the task end point, obtain the initial path from the task start point to the task end point that meets the actual operation constraints;

[0087] If the initial path is obtained, the path is smoothed by checking whether the three consecutive nodes in the initial path are collinear. If not, the path is smoothed by a second-order Bezier curve. This process is repeated until an optimal and smooth path is obtained. The smooth curve is expressed as follows:

[0088]

[0089] Where B(t) is a second-order Bezier curve; t is the range of curve parameters [0,1]; P i is a control point, corresponding to three consecutive non-collinear nodes.

[0090] Embodiment 2

[0091] In order to verify the effectiveness and superiority of the method proposed in the invention, the improved A* algorithm was experimentally verified by using MATLAB simulation software.

[0092] Five tasks are randomly generated, and simulation experiments are performed on these five tasks using the traditional A* algorithm and the improved A* algorithm proposed in this invention. The starting point and end point of the task represent the stop position along the line, and the path planning results are shown in Table 1 and Table 2. The grid number is composed of rows and columns, and the grid number of the lower left corner is 1, and the grid number of the upper right corner is 2500 (m×n).

[0093] Table 1 Experimental results of traditional A* algorithm path planning

[0094]

[0095]

[0096] Table 2 Experimental results of AGV path smoothing planning with adaptive heuristic optimization under multiple constraints in the present invention

[0097]

[0098]

[0099]

[0100]

[0101] By comparing Table 1 and Table 2, it can be seen that only the optimal path planned by the traditional A* method in Task 2 is the same as the optimal path planned by the method of the present invention. Figure 2 The schematic diagram of the AGV distribution flow direction of a one-way road in a smart manufacturing workshop is analyzed: the connection direction of the starting point and the end point of task 2 conforms to the flow direction between the lines where the end point is located. Therefore, the optimal path obtained by the traditional A* algorithm just meets the AGV distribution flow direction in the smart manufacturing workshop, which is consistent with the optimal path planned by the method of the present invention. For tasks 1, 3, 4, and 5, the path planning results and Figure 2 The comparison shows that the reliability of the method of the present invention in planning the optimal path has been verified.

[0102] Figure 3 This is an AGV path smoothing planning method with adaptive heuristic optimization under multiple constraints in the present invention. The experimental results of Task 1 are shown in the figure. Figure 4 This is the experimental result diagram of the traditional A* algorithm for task 1, combined with Figure 2 It can be intuitively demonstrated that the method designed by the present invention ensures that the optimal distribution path is output under the constraint of AGV distribution flow direction that is unidirectionally staggered between workshop lines.

[0103] The above experimental results show that the AGV path smoothing planning method with adaptive heuristic optimization under multiple constraints described in the present invention can efficiently plan the optimal path that meets the specific path constraints between line bodies for any task under the grid map.

[0104] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. An AGV path smoothing planning method with adaptive heuristic optimization under multiple constraints, characterized in that: The following steps are involved: Step 1: Build a grid map based on the layout diagram of the intelligent manufacturing workshop. According to the constraints of the AGV equipment running direction, assign specific turning constraints to each grid unit to generate a grid turning constraint table. Step 2: Improve the path cost evaluation function and introduce a reverse penalty term; Step 3: Optimize the heuristic function weight and design an adaptive heuristic weight factor; Step 4: Introduce the second degree Bezier curve to smooth the path and finally obtain a smooth AGV path.

2. According to the method of adaptive heuristic optimization of AGV path smoothing planning under multiple constraints in claim 1, it is characterized in that: The specific method of step one is as follows: 11) Based on the layout diagram of the intelligent manufacturing workshop and the AGV QR code navigation method, a grid map is constructed; the grid types constructed include logistics distribution channels, production lines and obstacles; Where Grid∈R m×n is an m×n dimensional grid matrix, which is initialized to consist of 0 and 1. m is the number of rows and n is the number of columns. m and n are determined according to the actual environment of the smart workshop. (m, n) is the coordinate of the grid. m×n is an M*N matrix in the real number field, R is the real number field, Grid is an M*N matrix in the real number field but the matrix is ​​initialized to consist of 0.1 inside; 12) According to the forward and reverse staggered running direction of the AGV equipment required by the intelligent manufacturing workshop, a steering constraint is assigned to each grid and a steering constraint table S is generated; Among them, S∈R m×n It is an m×n dimensional steering constraint matrix; m is the number of rows of the grid; n is the number of columns of the grid. Each element is composed of 0, 1, 2, 3, and 4 according to the steering constraint, and each grid coordinate corresponds one to one with the grid in the grid map Grid.

3. The AGV path smoothing planning method of adaptive heuristic optimization under multiple constraints according to claim 1 is characterized in that: The specific method of step 2 is as follows: 21) Add the inverse penalty term q(n) to the traditional A* cost evaluation function f(n) to obtain the improved cost evaluation function as shown below: f(n)=g(n)+h(n)+q(n) Among them, q(n) is the reverse penalty term, expressed as: 22) Define the direction of the current grid traversal as O, and define the four directions of horizontal left, horizontal right, vertical upward, and vertical downward as: h + 、h - 、v + 、v - , b represents the traversal of the current grid point to the direction in which the AGV is allowed to run, expressed as:

4. The AGV path smoothing planning method of adaptive heuristic optimization under multiple constraints according to claim 1 is characterized in that: The specific method of step three is as follows: 31) The total cost function from the starting point through the current grid to the target point after improvement is: f(n)=g(n)+w h h(n)+q(n) Among them, w h Represents the adaptive heuristic weight factor of the heuristic function h(n), expressed as: Where ln(x) is the natural logarithm of x.

5. The AGV path smoothing planning method of adaptive heuristic optimization under multiple constraints according to claim 1 is characterized in that: The specific method of step 4 is as follows: 41) Based on the initialization of the open list open_list and the closed list closed_list; put the task starting point into the open list open_list, and set the closed list closed_list to empty; Among them, the open list open_list represents the grid nodes currently waiting to be explored, and the closed list closed_list represents all the grid nodes that have been processed and will not be reprocessed; 42) Enter the main loop, obtain the global path planning from the task starting point to the task end point generated by the A* algorithm; select the node i with the smallest f value from the open list, remove it from the open list open_list, and add it to the closed list closed_list; 43) Determine whether the traversal end condition is met. If the node i currently being processed is the task end point or the minimum value from the starting point to the end point is infinite, then exit the loop, end the search, and obtain the global path planning from the task start point to the task end point; if the traversal end condition is not met, select the adjacent node j of node i according to the AGV running direction restriction; if the adjacent node j is in the closed list closed_list or exceeds the map range, then ignore the point, otherwise calculate the g value of node j according to the open list; 44) If the newly calculated g value is less than the current g value, set j's parent node to i and put it into the Parent matrix; where Parent∈R m×n , is the parent node matrix, used to update the parent node of each node, m is the number of rows of the grid, and n is the number of columns of the grid; 45) Repeat the traversal until the traversal end condition is met. According to the traversal end condition, if the minimum value from the starting point to the end point is infinite, the loop ends, indicating that there is no feasible path for the current task; if the current traversal point is the task end point, obtain the initial path from the task start point to the task end point that meets the actual operation constraints; If the initial path is obtained, the path is smoothed by checking whether the three consecutive nodes in the initial path are collinear. If not, the path is smoothed by a second-order Bezier curve. This process is repeated until an optimal and smooth path is obtained. The smooth curve is expressed as follows: Where B(t) is a second-order Bezier curve; t is the range of curve parameters [0,1]; P i is a control point, corresponding to three consecutive non-collinear nodes.

Images