AGV multi-neighbor path planning optimization method based on laser SLAM

Abstract

The application discloses an AGV multi-neighbor path planning optimization method based on laser SLAM, comprising an AGV navigation framework, wherein the AGV navigation framework comprises a map loading module, a sensor data module, a path planning module and an AGV control module; the path planning method comprises the following steps: 1) initialization, establishing a grid map; 2) setting a starting point and an ending point; 3) an improved 32-neighbor search algorithm is used to search a path on the established grid map, and a dynamic window algorithm is used in the process to avoid obstacles in the algorithm search process; 4) some key turning points are screened, and then a B-spline method is used to perform curve optimization on a path trajectory; 5) the superiority of the obtained path is verified; and 6) a global optimal path is obtained. Under the improved algorithm, the AGV can successfully plan an optimal path from a starting point to an arbitrary specified target point, realize the AGV navigation function, complete path planning, and verify the feasibility and accuracy of the algorithm.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot path planning research, specifically an AGV multi-neighborhood path planning optimization method based on laser SLAM. Background Technology

[0002] Automated Guided Vehicles (AGVs) have enabled unmanned material handling, improving enterprise efficiency and further promoting the intelligent development of the manufacturing industry, playing a crucial role in modern intelligent manufacturing. Path planning is the core of AGV application research; however, algorithmic issues often result in non-optimal paths in terms of both space and time, failing to meet market demands. Path planning, as a vital part of mobile robot research, refers to a mobile robot planning a safe, collision-free optimal path from its starting point to its destination in obstacle-filled scenarios, based on evaluation criteria. The path planning problem is a primary issue to be addressed in various mobile intelligent agents, and solving this problem is achieved through path planning algorithms.

[0003] AMCL (Adaptive Monte Carlo Localization) is a two-dimensional probabilistic localization system for mobile robots. Its core is the particle filter algorithm. During navigation, particle filtering is used to locate the robot. It differs from SLAM localization, which simultaneously locates and maps the robot while creating the map and finally outputs the map.

[0004] When a robot encounters "hijacking" during movement, its localization fails. To address this issue, AMCL employs a resampling (Kullback-Leibler divergence, KLD) method to update randomly injected particles. The number of particles can be adjusted in real-time using KLD, accelerating computation, improving the accuracy of particle probability distribution, and resolving both localization failures and robot hijacking. The process involves moving particles by acquiring the robot's motion, using the position of each particle to simulate sensor information and compare it with observed sensor information (laser data), assigning a probability to each particle. New particles are then generated based on these probabilities; higher assigned probabilities result in a higher probability of generating new particles. Through continuous iteration, all particles gradually converge and are filtered, allowing the robot's position to be calculated, thus ensuring more accurate localization.

[0005] Autonomous navigation is a key area of ​​research in AGV motion planning, and path planning is the core of autonomous navigation, mainly including three parts: environment modeling, path search, and path smoothing. AGVs rely on LiDAR to perceive environmental information and obtain their own pose through odometer information. They use algorithms to achieve functions such as navigation and obstacle avoidance from the starting point to the target point within a constructed map. Summary of the Invention

[0006] To overcome the shortcomings of existing technologies, this invention provides a technical solution for an AGV multi-neighborhood path planning optimization method based on laser SLAM.

[0007] The laser SLAM-based AGV multi-neighborhood path planning optimization method includes an AGV navigation framework, which comprises a map loading module, a sensor data module, a path planning module, and an AGV control module.

[0008] Path planning methods include the following steps:

[0009] 1) Initialize and create a raster map;

[0010] 2) Set the start and end points;

[0011] 3) The improved 32-neighborhood search algorithm searches for paths on the established grid map. At the same time, the dynamic window algorithm is used to ensure that the algorithm avoids obstacles during the search process and prevents the algorithm from judging that it has reached the target point.

[0012] 4) To improve the A* algorithm search path, some key inflection points need to be filtered, and then the B-spline method is used to optimize the path trajectory. The dynamic window algorithm ensures real-time obstacle avoidance during the path search process.

[0013] 5) Verify the superiority of the obtained path;

[0014] 6) Obtain the globally optimal path.

[0015] Furthermore, the verification method in step 5) includes the following steps:

[0016] S1 performs mapping operations and builds a map before performing path planning;

[0017] After the S2 starts the AGV and the network connection to the AGV is successful, open the terminal to connect to the Raspberry Pi and enter ssh ubuntu@10.42.0.1. Before this, you need to select a suitable point in the actual scenario and place the AGV on the selected point. In the terminal where the connection is successful, continue to enter the command "roslaunch rikirobot_nav bringup.launch".

[0018] The S3 opens the robot keyboard control node, facilitating keyboard control of the mobile robot's navigation and mapping. In the new terminal, connect to the Raspberry Pi via SSH and run the file "roslaunch teleop_twist_keyboard teleop_twist_keyboard.launch".

[0019] The S4 uses the Gmapping package to create a map. Connect to the Raspberry Pi via SSH and run the file "roslaunch rikirobot_nav gmapping.launch".

[0020] Once the three launch files above are running correctly, open a new terminal on the Ubuntu host and run the file "roslaunch rikirobot_nav rviz.launch". In the data window, view the map and robot posture data. By selecting messages and topics in Rviz, you can see the map, laser scan points, and AGV's tf pose data.

[0021] S6 starts moving the AGV, running a closed curved trajectory, and saves the map after it is created.

[0022] Furthermore, the steps for expanding the domain in step 3) are as follows:

[0023] 1) For each adjacent point of the input point, check whether it is still within the specified workspace and determine that it has not exceeded the work environment boundary;

[0024] 2) Determine the connection points separately;

[0025] 3) Divide the search range into two layers. First, evaluate all points in the first layer (1-8 neighborhood).

[0026] 4) Then evaluate all nodes in the second layer (9-32 neighborhood);

[0027] 5) Finally, select the node with the smallest evaluation function value f in the OPEN table and save it to the CLOSE table.

[0028] Furthermore, the path planning process in step 4) includes:

[0029] 1) Set the start node and the target node, and create empty open and close sets. Put the start point into the OPEN table and the obstacle point into the CLOSE table;

[0030] 2) At the start of the search, the first grid point searched is stored in the open set, and the open set is kept non-empty. The node n that obtains the minimum estimate is stored in the CLOSE table.

[0031] 3) First determine if n is the target point. If n is the target point, the optimal path can be obtained through its forward pointer.

[0032] Assuming n is not the target point, the successor node m is generated by expanding node n according to the new expansion algorithm. The first four steps of the above expansion algorithm are used to determine all successor nodes.

[0033] 4) Create a pointer in the OPEN list that returns from the successor node m to n, and then calculate the values ​​of f(m) = g(m) + h(m) and g(m) = g(n) + g(n,m);

[0034] 5) Determine if m is in the OPEN list. If it exists, select the node with the smallest value in f as the best node. If it is not in the OPEN list, add it to the OPEN list.

[0035] 6) Reassess based on comprehensive data. n (m) <f x (m), thereby generating a successor node;

[0036] 7) Update g(m), f(m), and the forward pointer of the successor node m;

[0037] 8) Then rearrange the values ​​of f in the OPEN table from smallest to largest, and return to step 3. The robot repeatedly selects the optimal value of the evaluation function in the OPEN table to finally determine the optimal planned path.

[0038] Furthermore, the key node extraction in step S5 includes the following steps:

[0039] a. Set the node set U{P i Let the set of nodes be ,1≤i≤n, where all nodes obtained from the improved algorithm's path planning are stored. Then, an initial set of nodes is created, containing only the path's starting point P1 and ending point P2. n Using the key node set V{P1,P n} to store the key turning points after algorithm optimization;

[0040] b. Starting from point P1, draw straight lines connecting P2, P3, ..., P... m Determine P1P m Does the straight line between any two points P1P pass through an obstacle? m If there are obstacles, then the set of nodes in the path planning must include node P. m-1 Similarly, this is also a key turning point, and the key node P is... m-1 Store in the key node set V; if the connected line does not pass through any obstacle, then the nodes P2,…,P that the line passes through are... m-1 Defined as a redundant node; from Pm Continue connecting the remaining nodes in node set U until the planned path's endpoint P is reached. n Finally, all key nodes are added to the set V{P1,P... m-1 ,...,P m+k ,P n}, where m is defined as the number of key nodes searched in set V;

[0041] c. Connect all nodes in set V.

[0042] Furthermore, in step 4), the path points obtained by fitting a 32-neighborhood search with a B-spline curve are processed.

[0043] The steps for B-spline curve fitting are as follows:

[0044] Let the polygon be formed by n+1 corner points, then the expression for the fitted curve based on k iterations is:

[0045]

[0046] In the above formula, F i,k (u) is a basis function, expressed as:

[0047]

[0048] However:

[0049]

[0050] The B-spline curve can then be represented as:

[0051]

[0052] Where: u∈[0,1];

[0053] n is the order of the spline;

[0054] C i Let be the coordinates of the i-th control point. The entire curve is formed by smoothly connecting multiple B-spline curves.

[0055] Compared with the prior art, the present invention has the following advantages:

[0056] With the improved algorithm, the AGV can successfully plan an optimal path from the starting point to any specified target point, realizing the AGV navigation function and completing path planning, while verifying the feasibility and accuracy of the algorithm proposed in this paper. The fusion algorithm can ensure that the AGV can successfully plan the optimal path to avoid obstacles, and when obtaining the optimal path solution, it makes the path smoother, reduces path turns and redundant sections, and achieves multiple optimizations such as path smoothness, short search time, and shortest path distance. Attached Figure Description

[0057] Figure 1 SLAM navigation framework;

[0058] Figure 2 Traditional A* algorithm search direction;

[0059] Figure 3 32. Neighborhood search diagram;

[0060] Figure 4 Flowchart of 32-neighborhood A* algorithm for mobile robot path planning;

[0061] Figure 5 Fusion algorithm path planning design process;

[0062] Figure 6 Comparison of 8-neighborhood search and 32-neighborhood search A* algorithms;

[0063] Figure 7 The path after key point extraction;

[0064] Figure 8 Smoothed path results;

[0065] Figure 9 Traditional A* algorithm for path planning;

[0066] Figure 10 Improved A* algorithm for path planning;

[0067] Figure 11 Comparison of positional deviation results between traditional A* algorithm and improved A* algorithm;

[0068] Figure 12 Comparison of directional deviation results between the traditional A* algorithm and the improved A* algorithm;

[0069] Figure 13 Laser-guided AGV prototype model;

[0070] Figure 14 Real-world scenario diagram;

[0071] Figure 15 AGV connection successful;

[0072] Figure 16 Start the keyboard control node;

[0073] Figure 17 Rviz successfully displayed the data interface;

[0074] Figure 18 AGV path planning experiment results diagram;

[0075] Figure 19 Multi-point experiment results diagram;

[0076] Figure 20 Comparison chart of path length results for multiple experimental points. Detailed Implementation

[0077] The invention will now be further described with reference to the accompanying drawings.

[0078] As shown in the figure, as the application scenarios of mobile robots become more and more complex, although the traditional path planning algorithm can solve the navigation problem of mobile robots, it has shortcomings in terms of space, time and performance. Therefore, this paper will propose an improvement to the traditional A* algorithm for mobile robot path planning.

[0079] To verify that the A* algorithm can plan the optimal path, this paper adopts a grid method. The center of each grid stores all the information and states of the node, making it convenient to use and computationally simple. For example... Figure 2 As shown, the traditional A* algorithm has four and eight search directions. To simplify the description, the movement model is simplified; the black dots represent the robot's current position, while the black arrows represent the robot's search position during movement. The four-direction A* algorithm searches for four directions: up, down, left, and right. The robot's minimum turning angle is π / 2, and the single step size is 1. Figure 2 As shown in (a), the eight-direction A* algorithm adds four more directions—upper left, lower left, upper right, and lower right—to the existing four directions: up, down, left, and right. The minimum turning angle of the mobile robot is π / 4, and the single movement step size is 1 or... like Figure 2 As shown in (b).

[0080] Mobile robots using the traditional A* algorithm can only move and search in a maximum of 8 directions, which is not conducive to expanding the robot's path search range. This causes some key nodes to be deleted prematurely when using the algorithm to search for paths, resulting in too many turning points in the planned path and an uneven path, making the search path prone to getting stuck in local optima.

[0081] To address the above issues, this paper improves the A* algorithm by expanding its search direction. The number of neighboring nodes for each node in the A* algorithm is increased to 32, broadening the robot's search range and preventing the deletion of some critical path nodes. A diagram illustrating the 32-neighbor search area is shown below. Figure 3 As shown.

[0082] As shown in the figure, the 32 black dots represent the specific locations that the robot can search. Compared with traditional algorithms, this method has a wider search range, reduces the angle between adjacent directions, and obtains the optimal path with shorter path length, smoother path, and shorter time. In addition, it avoids the phenomenon of too many inflection points and local optima caused by the limited search range of the traditional A* algorithm, and prevents premature deletion of search nodes, which is conducive to obtaining the optimal solution.

[0083] The neighborhood expansion method is performed on the traditional A* algorithm, and the specific operation process is as follows:

[0084] 1) For each adjacent point of the input point, check whether it is still within the specified workspace and determine that it has not exceeded the work environment boundary.

[0085] 2) Judge the connection points separately.

[0086] 3) Divide the search range into two layers. First, evaluate all points in the first layer (1-8 neighborhood). If any adjacent point is a point in the first layer, determine if the adjacent point is an obstacle. If it is an obstacle, discard it; otherwise, expand the search. Then check if the adjacent point exists in the OPEN table. If it does not exist, add it to the OPEN table. If it already exists, compare it with the previously calculated evaluation function value f. If it is the node with the smallest evaluation function f, select it; otherwise, continue updating.

[0087] 4) Then, evaluate all nodes in the second layer (9-32 neighborhood). First, determine if they are obstacles. If they are obstacles, discard them. If they are not obstacles, check if 1 to 3 points in the path from the input point to the adjacent point are obstacles. If any of these nodes is an obstacle, discard the corresponding adjacent point. Then, further evaluate adjacent points from the free region formed by the path points. During this process, check if the coordinates of the point already exist in the OPEN table, and compare them with the evaluation function value obtained in the previous calculation. If the evaluation function is the smallest node, select the point. If there is no such adjacent point in the OPEN table, finally add the adjacent point to the OPEN table.

[0088] 5) Finally, select the node with the smallest evaluation function value f in the OPEN table and save it to the CLOSE table.

[0089] The 32-neighborhood method is used to search for other neighboring nodes around the current node, and the optimal neighboring search node is calculated. Although the number of search directions increases, the search angle is not limited to integer multiples of π / 4, solving the problem of the limited search range of the traditional A* algorithm, thus finding the optimal path. The specific path planning process is as follows:

[0090] 1) Set the start node and the target node, and create empty open and close sets. Put the start point into the OPEN table and the obstacle point into the CLOSE table.

[0091] 2) At the start of the search, the first grid point searched is stored in the open set, and the open set is kept non-empty. The node n that obtains the minimum estimate is stored in the CLOSE table.

[0092] 3) First determine whether n is the target point. If n is the target point, the optimal path can be obtained through its forward pointer. If n is not the target point, then the node n is expanded according to the new expansion algorithm to generate the successor node m. All successor nodes are determined according to the first four steps of the above expansion algorithm.

[0093] 4) Create a pointer in the OPEN list that returns from the successor node m to n, and then calculate the values ​​of f(m) = g(m) + h(m) and g(m) = g(n) + g(n,m).

[0094] 5) Determine if m is in the OPEN list. If it is, select the node with the smallest value in f as the best node. If it is not, add it to OPEN.

[0095] 6) Reassess based on comprehensive data. n (m) <f x (m), thereby generating the successor node.

[0096] 7) Update g(m), f(m) and the forward pointer of the successor node m.

[0097] 8) Then rearrange the values ​​of f in the OPEN table from smallest to largest, and return to step 3. The robot repeatedly selects the optimal value of the evaluation function in the OPEN table to finally determine the optimal planned path.

[0098] The flowchart of mobile robot path planning based on the 32-neighborhood A* algorithm is as follows: Figure 4 As shown.

[0099] Analysis revealed that both the 8-neighborhood A* algorithm and the improved 32-neighborhood A* algorithm resulted in numerous redundant nodes and road segments. Optimal path search is a crucial indicator for evaluating the feasibility of AGV path planning and trajectory tracking schemes, especially when planning paths at AGV turns. This requires optimizing the global path. First, key nodes are extracted, and redundant nodes in the planned paths searched by the improved 32-neighborhood A* algorithm are filtered out, retaining only the key inflection points. Then, the resulting path curve is smoothed to shorten the optimized path and reduce the number of inflection points, thus completing the path optimization process.

[0100] (1) Extract key points

[0101] Based on the path found using the 32-neighborhood search mentioned earlier, key points are extracted using the following steps:

[0102] a. Set the node set U{P i Let the set of nodes be ,1≤i≤n, where all nodes obtained from the improved algorithm's path planning are stored. Then, an initial set of nodes is created, containing only the path's starting point P1 and ending point P2. n Using the key node set V{P1,P n} to store the key turning points after algorithm optimization.

[0103] b. Starting from point P1, draw straight lines connecting P2, P3, ..., P... m Determine P1P m Does the straight line between any two points P1P pass through an obstacle? m If there are obstacles, then the set of nodes in the path planning must include node P. m-1 Similarly, this is also a key turning point, and the key node P is... m-1 Store in the key node set V; if the connected line does not pass through any obstacle, then the nodes P2,…,P that the line passes through are... m-1 Defined as a redundant node. From P m Continue connecting the remaining nodes in node set U until the planned path's endpoint P is reached. n Finally, all key nodes are added to the set V{P1,P... m-1 ,...,P m+k ,P n}, where m is defined as the number of key nodes in the set V to search.

[0104] c. Connect all nodes in set V.

[0105] (2) B-spline curve

[0106] Since AGVs have nonholonomic constraints and always move forward, the B-spline interpolation method allows for local control of the curve shape while ensuring curvature continuity. Therefore, by establishing a B-spline interpolation model and obtaining a smooth curve, it is used to update the global path in the map model. This paper uses B-spline curve fitting to the path points obtained from a 32-neighborhood search.

[0107] Let the polygon be formed by n+1 corner points, then the expression for the fitted curve based on k iterations is:

[0108]

[0109] In the above formula, F i,k (u) is a basis function, expressed as:

[0110]

[0111] However:

[0112]

[0113] The B-spline curve can then be represented as:

[0114]

[0115] Where: u∈[0,1];

[0116] n is the order of the spline;

[0117] C i Let be the coordinates of the i-th control point. The entire curve is formed by smoothly connecting multiple B-spline curves.

[0118] The path planning for the mobile robot is based on a fusion algorithm combining an improved A* algorithm and a dynamic window method. This algorithm leverages the strengths of both to accomplish different functions, while also performing trajectory smoothing, extracting key nodes, and introducing B-spline methods to achieve multiple optimizations, including shorter path length, shorter search time, fewer path turns, and smoother path flow. The specific fusion algorithm path planning design process is as follows: Figure 5 As shown.

[0119] Depend on Figure 5 As shown, the specific implementation process of the constructed fusion algorithm is as follows:

[0120] 1) Initialize and create a raster map.

[0121] 2) Set the start and end points.

[0122] 3) The improved 32-neighborhood search algorithm searches for paths on the established grid map. At the same time, the dynamic window algorithm is used to ensure that the algorithm avoids obstacles during the search process and prevents the algorithm from judging that it has reached the target point.

[0123] 4) To improve the A* algorithm search path, some key inflection points need to be filtered, and then the B-spline method is used to optimize the path trajectory. The dynamic window algorithm ensures real-time obstacle avoidance during the path search process.

[0124] 5) Obtain the globally optimal path.

[0125] To verify the performance of the 32-neighborhood A* algorithm for path planning in a mobile robot, the path search effects of the improved algorithm and the traditional A* algorithm were compared and verified in MATLAB. A 20×20 square grid map was set up, and obstacles were randomly selected. The algorithm was run on the grid map with a known environment to conduct path planning experiments from the starting point to the ending point. Point 1 and point 12 were set as starting points, black areas were obstacles, white areas were blank areas, and points 2, 3, 4, 5, 6, 7, 8, 9, 10, and 11 were path nodes. The simulation results of the 32-neighborhood A* algorithm and the traditional A* algorithm are shown below. Figure 6 As shown.

[0126] Depend on Figure 6 It can be seen that both algorithms can achieve the path planning effect of AGV from the starting point to the end point, but if... Figure 6 As shown in (a), the 8-neighborhood search A* algorithm is easily restricted by certain obstacles in the direction of movement during the path search process, and the planned path length is not optimal. Figure 6 As shown in (b), the 32-search-neighborhood A* algorithm changed in path selection, and the path performance, such as the number of turning points and path smoothness, was significantly better than the path planned by the 8-search-neighborhood A* algorithm.

[0127] Through simulation comparison and analysis, the evaluation indicators of the two algorithms in path planning performance are statistically analyzed, as shown in Table 3-3.

[0128] Table 1. Comparison of A* algorithm results for 8-neighborhood search and 32-neighborhood search.

[0129]

[0130] Where: goal_distance is the straight-line distance from the starting point to the target point (regardless of whether there are obstacles between the two points);

[0131] path_cost represents the actual path length planned by different algorithms.

[0132] As shown in Table 1, the 32-neighborhood A* algorithm, compared to the 8-neighborhood A* algorithm, can plan a shorter path in the mobile robot path planning simulation experiment. The results are as follows:

[0133] a. In terms of the number of expanded points, the 32-neighborhood A* algorithm expands the number of expanded points compared to the traditional A* algorithm because the number of search nodes increases during the search process. This indirectly solves the problem that some potential optimal nodes are prematurely deleted in the early stages of the search in the traditional A* algorithm.

[0134] b. In terms of search time, the 32-neighborhood A* algorithm has a slight time delay compared to the 8-neighborhood A* algorithm. However, in the case of many obstacles, the 32-neighborhood A* algorithm has a shorter path length and fewer inflection points than the 8-neighborhood A* algorithm. The results show that the improved 32-neighborhood A* algorithm has good path planning performance.

[0135] c. In terms of the number of inflection points, since the 32-neighborhood is the optimal search node in the path planning process, the search path is the shortest and the number of inflection points is small, which is conducive to further optimizing the global path.

[0136] To verify the reliability of the 32-neighborhood A* algorithm, the improved algorithm presented in this paper was compared with the 24-neighborhood A* and 44-neighborhood A* algorithms through MATLAB simulation. Table 2 shows the specific results for the actual path length, number of extended points, search time, and number of inflection points of the three algorithms.

[0137] Table 2 Comparison of path results for three A* neighborhood expansion algorithms

[0138]

[0139] The results of analyzing the data in Table 2 are as follows:

[0140] a. In terms of path distance, the 32-neighborhood A* algorithm and the 44-neighborhood A* algorithm have similar planned path lengths, and the path search performance of the two algorithms is significantly better than that of the 24-neighborhood A* algorithm. The reduction in path planning distance further verifies the feasibility of the neighborhood expansion algorithm proposed in this paper.

[0141] b. In terms of planning time, due to the increased number of neighborhoods expanded in a single operation, both the 32-neighborhood A* algorithm and the 44-neighborhood A* algorithm have increased search time compared to the 24-neighborhood A* algorithm. However, the 32-neighborhood A* algorithm is delayed by 0.104 seconds compared to the 24-neighborhood A* algorithm, while the 44-neighborhood A* algorithm has a much longer search time than the other two algorithms.

[0142] c. In terms of the number of expanded points, the 32-neighborhood A* algorithm expands 64 more points than the 24-neighborhood A* algorithm, but 9 fewer than the 44-neighborhood A* algorithm. Although the 32-neighborhood A* and 44-neighborhood A* algorithms significantly expand the number of expanded points compared to the 24-neighborhood A* algorithm, they solve the problems of premature deletion of the optimal node and the occurrence of local optima.

[0143] d. Specific data comparison shows that although the 44-neighborhood A* algorithm plans shorter path distances than the 32-neighborhood A* algorithm and the traditional A* algorithm, its search time is significantly increased. While the 32-neighborhood A* and 44-neighborhood A* algorithms require more search time for path planning, the 32-neighborhood A* algorithm has a shorter search time than the 44-neighborhood A* algorithm, and it also has significantly fewer inflection points than the 24-neighborhood A* and 44-neighborhood A* algorithms.

[0144] In summary, the A* algorithm based on the 32-neighborhood search yields the best path planning performance. This algorithm can search for the shortest path in the spatial path and verifies the effectiveness and reliability of the improved A* algorithm proposed in this paper, laying the foundation for path optimization in the following paper.

[0145] Based on the analysis of the improved A* algorithm above, it is known that under certain conditions, the 32-neighborhood A* algorithm for path searching suffers from problems such as too many inflection points and unsmooth paths. Therefore, to verify the accuracy and feasibility of the path optimization method, this section conducts simulation experiments based on the path searched by the 32-neighborhood A* algorithm, optimizing the trajectory curve to enable the AGV to obtain the optimal path.

[0146] (1) Key point processing

[0147] exist Figure 6 Based on (b), key points are extracted, redundant inflection points are deleted, and key nodes are retained. The simulation results are as follows: Figure 7 As shown.

[0148] Depend on Figure 7 As shown, the 32-neighborhood search A* algorithm is first used to plan the path. Then, through global optimization, the path search effect is improved. The optimized path length is shortened from 238.79 to 226.68, an improvement of 5.07%. The number of vertices is reduced from 7 to 6, an improvement of 14.29%.

[0149] In summary, while the algorithm optimizes path length and segment redundancy by extracting key nodes during path planning, it still suffers from drawbacks such as low smoothness and excessively close proximity to obstacles, thus requiring further optimization.

[0150] (2) B-spline processing

[0151] Although key points were extracted and redundant inflection points were removed, the presence of redundant nodes remains a limitation. To meet the motion requirements under various conditions and ensure smoothness at inflection points, the AGV needs to achieve optimal path optimization and more stable operation. Therefore, a B-spline algorithm is introduced to further optimize the trajectory, obtaining... Figure 7 Based on the results, further curve processing is performed.

[0152] When optimizing the path, a fusion algorithm combining an improved algorithm and a dynamic window algorithm is used for path planning. The traditional algorithm and the improved algorithm are analyzed and compared using MATLAB, and the results are as follows. Figure 8 As shown in the figure, the red line segment represents the path obtained by combining the 32-neighborhood A* algorithm with the dynamic window algorithm, and the blue line segment represents the path obtained by processing the B-spline curve. The comparison of the simulation results before and after smoothing is shown in Table 3.

[0153] Table 3 Comparison of results before and after smoothing treatment

[0154]

[0155] Depend on Figure 8 As shown in Table 3-5, the path planned using the fusion algorithm has fewer turns, continuous curvature, and higher smoothness. Compared with the path planned by the 32-neighborhood A* algorithm before optimization, the smoothed path performs better, especially at the turns. Table 3-5 shows that the path length is reduced by 5.5% after smoothing. The cumulative turning angle of the original path is 180°, while the cumulative turning angle of the curved path obtained by the fusion algorithm is 156.5°. The path search time of the 32-neighborhood A* algorithm is 0.45s, while the path search time of the fusion algorithm is 0.38s, improving time efficiency by 15%. Therefore, the fusion algorithm simultaneously achieves multiple optimization objectives, including shortest path, shortest time, and smoother path.

[0156] In summary, this paper utilizes a fusion algorithm to address the problems that AGVs easily fall into local optima, have the longest paths, and take long times during path planning. It also verifies that the improved A* smoothing algorithm has more significant advantages in path planning, is safer and more reliable, and improves both the spatial and temporal aspects of AGV path searching.

[0157] To verify the effectiveness and accuracy of the proposed algorithm, simulation experiments were conducted on AGVs using the Gazebo platform. The simulation mirrored the real-world operating environment, validating the AGV's performance under the influence of the fusion algorithm. AGV parameters were configured appropriately according to requirements to ensure stable, safe, and reliable AGV operation.

[0158] Robot Operating System (ROS) is a robot development platform that runs in the Linux environment. It can simulate the running performance of robots in system simulation, and ROS provides a wealth of tools, source code and protocols

[93] . The system simulation of robots mainly includes three parts: robot modeling (URDF), creating a simulation environment (Gazebo) and perceiving the environment (Rviz).

[0159] (1) Robot Modeling (URDF)

[0160] URDF is a unified (standardized) robot description format that can describe the structure of a robot, such as the chassis, lidar, and degrees of freedom of different joints. This file can be converted into a visual robot model and is an important component for robot simulation in ROS.

[0161] The performance of AGVs differs significantly between simulation and real-world environments. To facilitate analysis, AGVs are simplified as follows:

[0162] a. The simulator uses a physics engine instead of simulating a real physical environment.

[0163] b. The simulator constructs joint actuators, such as motors and gears, sensors and signal communication, which are all in an absolutely ideal situation. It does not currently support simulating actual hardware defects or some critical states.

[0164] c. The AGV is modeled in a simplified manner, which facilitates simulation analysis.

[0165] Before simulation, a model needs to be built according to the actual needs of the AGV, including chassis construction, adding drive wheels, omnidirectional wheels, lidar sensors, etc. After the vehicle body is successfully set up, it is saved to an xacro file, and then the URDF file is integrated into Gazebo for easy display of the model in Gazebo later.

[0166] (2) Setting up the Gazebo simulation environment

[0167] Gazebo is a plugin for ROS that can simulate complex real-world robotics and movements. Gazebo provides users and programs with more realistic physical environments, sensors, and interfaces, allowing robots to perceive their surroundings through their own sensors and perform motion control simulations, SLAM navigation, and more, just like in a real environment.

[0168] First, an indoor simulation environment for the robot is created on the Gazebo simulation platform to prepare for the next step of SLAM navigation on Gazebo. To better approximate the real environment, the specific model features of the simulation environment are set as follows:

[0169] a. The simulation environment is an indoor environment with a total length of 10m and a width of 5m, and the height of the walls is set to 2m.

[0170] b. The interior environment consists of different rooms, including corridors and obstacles such as tables and chairs.

[0171] c. Install doors with a height of 1m and a width of 0.5m between different rooms to facilitate the AGV's entry and exit from different rooms.

[0172] After editing the simulation environment, save it to the Gazebo model library. Finally, enter "roslaunchsmart_nav robot_demo.launch" in the terminal to open Gazebo and see the created runtime environment.

[0173] (3) Rviz

[0174] Rviz is a 3D visualization tool officially provided by ROS. It allows users to view and manipulate robots and obtain relevant data. Frequently used features in Rviz include LiDAR data visualization, image data visualization, and motion status display.

[0175] The robot navigation simulation is also divided into two parts for simulation experiments.

[0176] (1) SLAM simulation experiment based on Gazebo platform

[0177] First, import the existing environment model and robot model into the ROS Gazebo platform. By running the ROS SLAM algorithm program and robot motion control program, you can control the mobile robot to move in the indoor environment. The specific steps are as follows:

[0178] a. Start the simulation environment

[0179] Table 4. Configuration information for generating maps

[0180]

[0181] First, start the simulation environment by entering "roslaunch smart_nav robot_demo.launch" in the terminal.

[0182] b. Start gmapping for mapping

[0183] Then, enter "roslaunch rikirobot_nav gmapping_simulation.launch" in the terminal to create a 2D raster map. The generated map YAML data is shown in Table 4.

[0184] c. Open Rviz

[0185] Next, enter "roslaunch rikirobot_nav rviz.launch" in the terminal. After the map is created, you can view the robot's movement trajectory in the Gazebo simulation interface, and also control the AGV's navigation operation through the visualization tool Rviz.

[0186] (2) Navigation simulation experiment based on Gazebo platform

[0187] The simulation experiments in this paper were conducted on the Ubuntu 18.04 operating system, using ROS as the experimental platform. A SLAM simulation experiment based on the Gazebo platform was performed to compare and verify the improved algorithm with the traditional algorithm. The AGV's travel direction can be used to determine whether the AGV's planned path meets expectations. In the ROS system, the traditional navigation algorithm and the fusion algorithm file built based on the proposed improved algorithm were imported into the AGV and run, respectively. Then, a simulation experiment was conducted in the environment, setting the start and end points. The final path map of the burger is shown below. Figure 9 and 10 As shown.

[0188] Depend on Figure 9 and Figure 10 Simulation results show that the improved A* smoothing algorithm prioritizes the optimal node among all nodes at the target as the next position, thus the planned path generally moves in one direction. In contrast, the traditional A* algorithm prioritizes all nodes at the current node as the next position; if the evaluation function value is large, it may plan a path in the opposite direction, such as... Figure 9 As shown, the planned path has many turns and large angles when encountering curves or obstacles. Figure 9 and Figure 10 Taking the path within the yellow rectangle as an example, the tilt angle of this section is -60°, while the heading angle in the robot's previous state parameter is 90°. The robot continues path searching, and the path curve is optimized after using the improved optimization algorithm. When the AGV encounters obstacles during operation, in addition to autonomously planning its path, it can also autonomously avoid obstacles. Therefore, the fusion algorithm built based on the improved A* smoothing algorithm optimizes the AGV's planned path, effectively solving the problem of many redundant road segments in the original path, thus promoting smooth and stable AGV operation that conforms to the AGV's motion rules.

[0189] The effectiveness of two algorithms for path planning in AGV was verified using the Gazebo simulation platform. The path search results of the two algorithms are shown in Table 5.

[0190] Table 5 Search results for different algorithm paths

[0191]

[0192] As shown in Table 5, under the same road conditions, compared with the traditional algorithm, the improved A* algorithm obtains the optimal path with shorter search path length, less running time, and smaller overall distance and direction deviations. Therefore, the improved algorithm in this paper has a significantly better search effect than the traditional algorithm. The improved algorithm has higher reliability in search results, ensuring the stable operation of the AGV.

[0193] To verify the accuracy of the improved algorithm, multiple experiments were conducted under the same conditions as described above. During the path planning process, the positional deviation (absolute value) and directional deviation (absolute value) between the AGV center and the target point were recorded at the end of each path planning iteration. The comparison results of the positional and directional deviations are as follows: Figure 11 and Figure 12 As shown.

[0194] Variance and standard deviation are used to determine the stability of simulation experimental data of different algorithms. The results of position deviation and direction deviation of multiple path searches by different algorithms are shown in Figure 6.

[0195] Table 6 Comparison of Path Search Deviation Results for Different Algorithms

[0196]

[0197] As shown in Table 6, the improved algorithm produces more stable positional and directional deviations compared to the traditional algorithm, without significant fluctuations. Therefore, the robot can obtain the optimal path when using the improved algorithm for navigation, thus ensuring the stable operation of the AGV and providing a theoretical basis for trajectory tracking control in the following section.

[0198] In summary, the fusion algorithm based on the improved A* smoothing algorithm combined with the dynamic window algorithm can ensure that the AGV can successfully plan the optimal path to avoid obstacles. Moreover, it can improve the smoothness of the path when the optimal solution is obtained, reduce path turns and redundant sections, and achieve multiple optimizations such as path smoothness, short search time and shortest path distance.

[0199] Simultaneously, prototype experiments were conducted on this design, and the laser-guided AGV prototype model is as follows: Figure 13 As shown.

[0200] The specific features of the AGV prototype are as follows:

[0201] (1) The AGV prototype in this paper adopts a two-wheel differential motion method, including two driving wheels and four driven wheels, which has the ability to move at different speeds and can flexibly turn and avoid obstacles.

[0202] (2) The active wheel motor drive uses a DC motor drive system produced by Zhongma Transmission Intelligent Technology Co., Ltd. as the drive source. It has the characteristics of large torque and can maintain stable operation at low speed.

[0203] (3) Navigation uses YDLIDAR laser radar, which is mainly used for map construction. AGV can sense and search in the environment and send information.

[0204] (4) At the same time, Raspberry Pi is used as the host computer control platform to remotely connect and control the AGV to complete its movement and work.

[0205] The parameters of the AGV prototype are shown in Table 7.

[0206] Table 7 Performance parameters of AGV prototype

[0207]

[0208]

[0209] This experiment applies to AGVs operating in an indoor environment, primarily suitable for transferring materials of varying weights and for loading, unloading, and transporting materials on production lines. The experimental site was chosen to utilize a factory environment, ensuring a high degree of safety and reliability, and meeting the requirements of the experimental site. The entire indoor environment is approximately 15m long and 10m wide, with an area of ​​approximately 150m². 2 To facilitate the AGV's operation around the laboratory, stools were placed along the path to simulate obstacles, further mimicking a real-world environment. The experimental operating environment is as follows: Figure 14 As shown.

[0210] The experimental procedure is as follows:

[0211] The navigation experiment in this paper uses a map built by Gmapping, and the map building operation is performed before route planning.

[0212] a. Start the AGV. After successfully connecting to the network, open a terminal, connect to the Raspberry Pi, and enter `ssh ubuntu@10.42.0.1`. Before this, you need to select a suitable point in the actual scenario and place the AGV on the selected point. In the successfully connected terminal, continue by entering the command `roslaunch rikirobot_nav bringup.launch`. The running result is shown below. Figure 15 As shown.

[0213] b. Enable the robot keyboard control node for easy keyboard control of the mobile robot's navigation and mapping. In the new terminal, connect to the Raspberry Pi via SSH and run the file "roslaunch teleop_twist_keyboard teleop_twist_keyboard.launch". The results are shown below. Figure 16 As shown.

[0214] c. Run the Gmapping package to create the map. Connect to the Raspberry Pi via SSH and run the file "roslaunch rikirobot_nav gmapping.launch".

[0215] d. After the above three launch files run correctly, open a new terminal on the Ubuntu host and run the file "roslaunch rikirobot_nav rviz.launch". View the map and robot posture data in the data window. Message and topic selection in Rviz is as follows... Figure 17 As shown, you can see the map, laser scan points, and AGV's tf pose data.

[0216] e. Start moving the AGV, running a closed curved trajectory, and save the map after it is created.

[0217] In ROS-based AGV path planning, global path planning is first performed, using the NAVF package to calculate the global route from the AGV to the target location. Then, the robot chassis control node, navigation node, and Rviz are started sequentially, the created map is opened, and radar configuration information is also activated.

[0218] After preliminary path planning, the AGV is set to a target point using the 2D Nav Goal command tool in Rviz. The ROS system then activates the Move_base node and begins path planning based on the selected location, thus performing autonomous navigation and selecting the optimal path. To ensure the effectiveness of the improved A* smoothing algorithm over a wider range, an arbitrary set of initial and final positions was selected, and global path planning was performed on these two points. The AGV's starting position is set to point A, and the target point to point B. The slightly curved upper section represents the AGV's global path planning route, the arrows indicate the local path planning routes, and the straighter lower section represents the AGV's planned travel path. The Rviz test map showing global path planning displays two small squares representing static obstacles, a white area in the middle indicating passable areas, and a larger square representing the scanned environmental outline, indicating impassable areas. The AGV navigation experiment results are as follows. Figure 18 As shown.

[0219] To verify the algorithm's superiority, multi-point experiments were conducted, allowing the AGV to perform path planning for multiple target points in the environment. Multiple target points were set using the "publish point" tool in RViz, and the AGV navigated according to these given target points, thus achieving path optimization for multiple target points. The experimental results are as follows: Figure 19 As shown.

[0220] Depend on Figure 19 As can be seen, by selecting four target points, the AGV, under the improved algorithm, can reach the target point from the starting position and iteratively search for the optimal path. The statistical results of the path optimization experiment are shown in Table 8.

[0221] Table 8. Experimental Results of Path Planning

[0222]

[0223] To avoid randomness in the algorithm's path search, path planning experiments were repeated 30 times at each of the four target points (1, 2, 3, and 4). The results of the AGV's path planning under different algorithms were then statistically analyzed. During the path planning process, the actual path length of the AGV from the starting point to the target point was recorded for each iteration. A comparison of the path length results from multiple experiments is shown in the following figure. Figure 20 As shown in Table 9, the path planning analysis results of the multi-point experiment are presented.

[0224] Table 9. Path planning results of the multi-point experiment

[0225]

[0226] The path planning experiment shows that, with the improved algorithm, the AGV can successfully plan an optimal path from the starting point to any specified target point, realize the AGV navigation function and complete the path planning, and at the same time verify the feasibility and accuracy of the algorithm proposed in this paper.

[0227] To address the issues of long search paths, numerous inflection points, and uneven path smoothness inherent in the A* algorithm, a 32-neighborhood A* smoothing path algorithm is proposed. This algorithm extracts key path nodes by optimizing the path algorithm, introduces the B-spline method to handle curved trajectories, and combines this with a dynamic window method to construct a fusion algorithm. This results in a shorter, smoother path with fewer inflection points. The proposed optimization method effectively solves the problem of numerous redundant road segments in the original path, achieving multiple optimizations in path smoothness and minimizing path distance, ensuring stable and reliable AGV operation. It is more suitable for the needs of small and medium-sized enterprises.

[0228] The optimization method also includes neighborhood expansion. The traditional A* algorithm can only move and search in a maximum of eight directions, which is not conducive to expanding the robot's path search range. This leads to the premature deletion of some key nodes when using the algorithm to search for paths, resulting in too many turning points in the planned paths and uneven paths, making the search path prone to getting trapped in local optima. To solve these problems, this paper improves the A* algorithm by expanding its search directions, increasing the number of expanded neighborhoods for each node to 32, thus expanding the robot's search range and preventing the deletion of some key path nodes.

[0229] The optimization method also includes trajectory curve processing. Analysis revealed that both the 8-neighborhood A* algorithm and the improved 32-neighborhood A* algorithm resulted in numerous redundant nodes and road segments. Optimal path search is a crucial indicator for evaluating the feasibility of AGV path planning and trajectory tracking schemes, especially when planning paths at AGV turns. Global path optimization is necessary. First, key nodes are extracted, and redundant nodes in the planned paths searched by the improved 32-neighborhood A* algorithm are filtered out, retaining only the key inflection points. Then, the resulting path curve is smoothed to shorten the optimized path length and reduce the number of inflection points, thus completing the path optimization process.

[0230] The optimization method also includes a fusion algorithm based on the 32-neighborhood A* algorithm. The path planning of the mobile robot is based on the fusion algorithm of the improved A* algorithm and the dynamic window method. It makes full use of the advantages of the two algorithms to complete different functions, and performs trajectory smoothing, extracts key nodes, and introduces the B-spline method to achieve multiple optimizations such as short path length, shortest search time, fewer path turns and smoother path.

[0231] The optimization method also includes MATLAB simulation experiments and analysis. Based on the path searched by the 32-neighborhood A* algorithm, simulation experiments are conducted to optimize the trajectory curve, extract key points, delete redundant inflection points, retain key nodes, and introduce the B-spline algorithm to further optimize the trajectory. When optimizing the path, the improved algorithm is combined with the dynamic window algorithm to form a fusion algorithm for path planning. The traditional algorithm and the improved algorithm are analyzed and compared using MATLAB.

[0232] The optimization method also includes conducting simulation experiments on the AGV based on the Gazebo platform. By simulating the real operating environment, the AGV's operating performance is verified under the action of the fusion algorithm. AGV parameters are configured reasonably according to requirements to ensure stable, safe, and reliable AGV operation.

Claims

1. A multi-neighborhood path planning and optimization method for AGVs based on laser SLAM, characterized in that, The system includes an AGV navigation framework, which comprises a map loading module, a sensor data module, a path planning module, and an AGV control module. Path planning methods include the following steps: 1) Initialize and create a raster map; 2) Set the start and end points; 3) The improved 32-neighborhood search algorithm searches for paths on the established grid map. At the same time, the dynamic window algorithm is used to ensure that the algorithm avoids obstacles during the search process and prevents the algorithm from judging that it has reached the target point. 4) To improve the A* algorithm search path, some key inflection points need to be filtered, and then the B-spline method is used to optimize the path trajectory. The dynamic window algorithm ensures real-time obstacle avoidance during the path search process. The path points obtained by the 32-neighborhood search are fitted using B-spline curve fitting; the steps of B-spline curve fitting are as follows: Let the polygon be formed by n+1 corner points, then the expression for the fitted curve based on k iterations is: ， In the above formula, F i,k (u) is a basis function, expressed as: ， However: ， The B-spline curve can then be represented as: ， Where: u∈[0,1]; n is the order of the spline; P i Let be the coordinates of the i-th control point. The entire curve is formed by smoothly connecting multiple B-spline curves. 5) Verify the superiority of the obtained path; 6) Obtain the globally optimal path.

2. The AGV multi-neighborhood path planning optimization method based on laser SLAM according to claim 1, characterized in that, The verification method in step 5) includes the following steps: S1 performs mapping operations and builds a map before performing path planning; After the S2 starts the AGV and the network connection to the AGV is successful, open the terminal to connect to the Raspberry Pi and enter ssh ubuntu@10.42.0.

1. Before this, you need to select a suitable point in the actual scenario and place the AGV on the selected point. In the terminal where the connection is successful, continue to enter the command "roslaunch rikirobot_nav bringup.launch"; S3 opens the robot keyboard control node, which facilitates keyboard control of the mobile robot for navigation and mapping. In the new terminal, connect to the Raspberry Pi via SSH and run the file "roslaunch teleop_twist_keyboard teleop_twist_keyboard.launch". The S4 runs the Gmapping package to build maps, connects to the Raspberry Pi via SSH, and runs the file "roslaunch rikirobot_navgmapping.launch"; After the three launch files above are running normally, open a new terminal on the Ubuntu host and run the file "roslaunch rikirobot_nav rviz.launch". In the data window, view the map and robot posture data. By selecting messages and topics in Rviz, you can see the map, laser scan points, and AGV's tf pose data. S6 starts moving the AGV, running a closed curved trajectory, and saves the map after it is created.

3. The AGV multi-neighborhood path planning optimization method based on laser SLAM according to claim 1, characterized in that, The steps for expanding the domain in step 3) are as follows: 1) For each adjacent point of the input point, check whether it is still within the specified workspace and determine that it has not exceeded the work environment boundary; 2) Determine the connection points separately; 3) Divide the search range into two layers. First, evaluate all points in the first layer's 1-8 neighborhood. 4) Then evaluate all nodes in the second layer's 9-32 neighborhood; 5) Finally, select the node with the smallest evaluation function value f in the OPEN table and save it to the CLOSE table.

4. The AGV multi-neighborhood path planning optimization method based on laser SLAM according to claim 1, characterized in that, Step 4) includes the path planning process as follows: 1) Set the start node and the target node, and create empty open and close sets. Put the start point into the OPEN table and the obstacle point into the CLOSE table; 2) At the start of the search, the first grid point searched is stored in the open set, and the open set is kept non-empty. The node n that obtains the minimum estimate is stored in the CLOSE table. 3) First determine if n is the target point. If n is the target point, the optimal path can be obtained through its forward pointer. Assuming n is not the target point, the successor node m is generated by expanding node n according to the new expansion algorithm. The first four steps of the above expansion algorithm are used to determine all successor nodes. 4) Create a pointer in the OPEN list that returns from the successor node m to n, and then calculate the values ​​of f(m) = g(m) + h(m) and g(m) = g(n) + g(n,m); 5) Determine if m is in the OPEN list. If it exists, select the node with the smallest value in f as the best node. If it is not in the OPEN list, add it to the OPEN list. 6) Re-evaluate based on the comprehensive data: f(m) < f(n), where f(m) represents the estimated cost of points m and n, respectively. If f(m) < f(n), then generate a successor node. 7) Update g(m), f(m), and the forward pointer of the successor node m; 8) Then rearrange the values ​​of f in the OPEN table from smallest to largest, and return to step 3. The robot repeatedly selects the optimal value of the evaluation function in the OPEN table to finally determine the optimal planned path.

5. The AGV multi-neighborhood path planning optimization method based on laser SLAM according to claim 1, characterized in that, Step 3) involves extracting key points based on the search path, including the following steps: a. Set the node set U{P i Let the set of nodes be ,1≤i≤n, where all nodes obtained from the improved algorithm's path planning are stored. Then, an initial set of nodes is created, containing only the path's starting point P1 and ending point P2. n Using the key node set V{P1,P n } to store the key turning points after algorithm optimization; b. Starting from point P1, draw straight lines connecting P2, P3, ..., P... m Determine P1P m Does the straight line between any two points P1P pass through an obstacle? m If there are obstacles, then the set of nodes in the path planning must include node P. m-1 Similarly, this is also a key turning point, and the key node P is... m-1 Store in the key node set V; if the connected line does not pass through any obstacle, then the nodes P2,…,P that the line passes through are... m-1 Defined as a redundant node; from P m Continue connecting the remaining nodes in node set U until the planned path's endpoint P is reached. n Finally, all key nodes are added to the set V{P1,P... m-1 ,…,P m+k ,P n }, where m is defined as the number of key nodes searched in set V; c. Connect all nodes in set V.

Images