A multi-target waypoint planning method and system

Abstract

The application provides a multi-target waypoint planning method and system fusing an improved A* algorithm and an SPFA algorithm, and the method mainly comprises the following steps: a mobile navigator rasterizes the surrounding environment, and calibrates a target point set and a starting point of the navigator to simulate a task scene; a distance value calculation of the starting point to the target point is performed through construction of the improved A* algorithm; a negative edge weight map with point priority, direction and without negative loop is established through fusion of the improved A* algorithm and the SPFA algorithm, and the best waypoint order can be obtained according to the point priority in the negative edge weight map; the navigator moves according to the waypoint order to traverse the task target points, and global path planning and local path planning algorithms are realized. Through the application, a large number of path points and inflection points are reduced, the total path length is shortened, the total path cost is reduced, the overall efficiency is improved, and a reasonable and fast feasible path is planned in a multi-target waypoint task in a complex environment.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, and in particular to a multi-objective waypoint planning method and system that integrates the improved A* algorithm and the SPFA algorithm. Background Technology

[0002] With the development of modern science and technology, automatic navigation technology plays a crucial role in both autonomous driving and mobile navigation robots. Path planning is the most important and indispensable part of achieving automatic navigation for mobile robots. In complex environments with multiple waypoints, the order of different waypoints requires human intervention to varying degrees, making truly fully autonomous path planning and navigation impossible. Therefore, how to enable mobile navigation robots to autonomously select the optimal path in multi-waypoint tasks has become a major challenge.

[0003] Currently, commonly used algorithms for global path planning in mobile navigation robots include A*, Dijkstra's algorithm, and RRT algorithm. Local path planning algorithms commonly used in mobile navigation robots include DWA, artificial potential field, and AGV algorithms. Currently, the A* algorithm is widely used in global path planning, and due to its inherent stability in path search, it has become one of the most widely applied algorithms for path planning in mobile navigation robots. For example, in "Dynamic Path Planning for Robots by Integrating Improved A* and DWA Algorithms," Liu Jianjuan et al. disclosed a path node optimization algorithm based on the Floyd algorithm, which removes redundant nodes, reduces turns, and improves path smoothness. They also designed a dynamic window evaluation function for the DWA algorithm based on global optimization to distinguish between known obstacles and unknown dynamic and static obstacles, and extracted key points of the improved A* algorithm's planned path as temporary target points for the DWA algorithm, thus achieving the fusion of the improved A* and DWA algorithms based on global optimization.

[0004] However, the path contains a large number of redundant nodes and tortuous inflection points, the path length is not the shortest distance and the computation time is long. In addition, there are U-shaped dead ends and pseudo-forced neighbor nodes in the map, making it impossible to effectively plan a reasonable feasible path. Furthermore, the b-spline at forced neighbor nodes is not smooth enough at the corners of the planned path, so the path cannot closely resemble the travel route of the mobile navigation robot in the actual environment. Therefore, planning a reasonable and fast path in multi-objective waypoint tasks in complex environments faces certain challenges. Summary of the Invention

[0005] Based on this, the purpose of this invention is to propose a multi-objective waypoint planning method and system that integrates the improved A* algorithm and the SPFA algorithm, with the aim of planning a reasonable and fast feasible path in multi-objective waypoint tasks in complex environments.

[0006] A multi-objective waypoint planning method integrating the improved A* algorithm and the SPFA algorithm proposed according to the present invention includes:

[0007] The target environment is rasterized in two dimensions to generate a rasterized map, in which the set of starting and target points of the navigator are marked;

[0008] The traditional A* algorithm is improved to obtain the improved A* algorithm. Then, the cost value between the starting point and each target point is calculated according to the improved A* algorithm, and each cost value is recorded in the cost array.

[0009] Add weights to the cost array to obtain a directed negative weighted graph with high priority. Input the directed negative weighted graph into the SPFA algorithm to calculate the single-source shortest path from the starting point to the target point. Use the high priority as the stage endpoint to obtain the waypoint order of the target point set and record the waypoint order in the shortest path array.

[0010] Establish a current variable and a target variable, which are used as the current point index and the target point index, respectively. The current point index points to the starting point, and the target point index points to the head of the shortest path array. Input the current variable and the target variable into the raster map to perform path point calibration.

[0011] The improved A* algorithm is used to traverse between the starting point and the target point. When it is confirmed that the target variable points to the last element of the shortest path array, the multi-waypoint planning task is completed.

[0012] This invention presents a multi-objective waypoint planning method that integrates an improved A* algorithm and the SPFA algorithm. It constructs a mobile navigator to rasterize the surrounding environment and marks the navigator's target point set and starting point to simulate the task scenario, facilitating experimental verification. By constructing an improved A* algorithm to calculate the distance cost from the starting point to the target point, the cost reflects the distance between two points, reducing a large number of path points and inflection points and shortening the total path length, thus effectively reducing the total path cost. By integrating the improved A* algorithm and the SPFA algorithm, a directed, negative-weighted map with point priorities and no negative loops is established. The optimal waypoint order can be obtained based on the point priorities in the negative-weighted map, and the navigator then moves according to the waypoint order to traverse the task target points and implement a global path planning algorithm, improving overall efficiency. Furthermore, during movement, local path planning is used to avoid static obstacles, effectively conforming to the feasible trajectory of the mobile navigator. Therefore, compared with existing technologies, this invention can plan reasonable and fast feasible paths in multi-objective waypoint tasks in complex environments.

[0013] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by means of embodiments of the invention. Attached Figure Description

[0014] Figure 1 This is a flowchart of the multi-objective waypoint planning method proposed in the first embodiment of the present invention;

[0015] Figure 2 This is a schematic diagram of the center θ partitioning and 13-neighborhood expansion in the neighborhood expansion and change method of the first embodiment of the present invention, wherein (a) center θ partitioning and (b) 13-neighborhood expansion;

[0016] Figure 3 This is a schematic diagram of the existence of U-shaped dead corners and the existence of false forced neighbors in the first embodiment of the present invention, wherein (a) the existence of U-shaped dead corners and (b) the existence of false forced neighbors;

[0017] Figure 4 This is a schematic diagram illustrating the distinction between forced neighbors and pseudo-forced neighbors in the first embodiment of the present invention;

[0018] Figure 5 This is a schematic diagram comparing the situation with and without redundant nodes in the first embodiment of the present invention, wherein (a) redundant nodes are not removed and (b) redundant nodes are removed.

[0019] Figure 6 This is a schematic diagram comparing the before and after smoothing in the first embodiment of the present invention, wherein (a) is before smoothing and (b) is after smoothing;

[0020] Figure 7 This is a flowchart of the multi-objective waypoint planning method proposed in the second embodiment of the present invention;

[0021] Figure 8 The following is a simplified representation of the first and second examples in the third embodiment of the present invention, wherein (a) is the first example and (b) is the second example;

[0022] Figure 9 Corresponding to the third embodiment of the present invention Figure 8 The task simplification intentions of the first example and the second example, wherein, (a) first example 1-1, (b) first example 1-2, and (c) example 2-1;

[0023] Figure 10 The following is a schematic diagram of the 20*20 implementation effect in the comparative example of the present invention, wherein (a) the path generated by the traditional A* algorithm, (b) the path after the improved A* algorithm removes redundant nodes, and (c) the path after the neighboring nodes are forced to be smoothed by adding b-splines.

[0024] Figure 11The following is a schematic diagram of the 30*30 implementation effect in the comparative example of the present invention, wherein (a) the path generated by the traditional A* algorithm, (b) the path after the improved A* algorithm removes redundant nodes, and (c) the path after the neighboring nodes are forced to be smoothed by adding b-splines.

[0025] Figure 12 The following is a schematic diagram of the 40*40 implementation effect in the comparative example of the present invention, wherein (a) the path generated by the traditional A* algorithm, (b) the path after the improved A* algorithm removes redundant nodes, and (c) the path after the neighboring nodes are forced to be smoothed by adding b-splines.

[0026] Figure 13 This is a schematic diagram of the simulation effect of the first example of the simulation implementation in the comparative example of the present invention, wherein (a) the path generated by the traditional A* algorithm, (b) the path after the improved A* algorithm removes redundant nodes, and (c) the path after the b-spline is added to the neighboring nodes for smoothing.

[0027] Figure 14 This is a schematic diagram of the simulation implementation effect of the second example in the comparative example of the present invention, wherein (a) the path generated by the traditional A* algorithm, (b) the path after the improved A* algorithm removes redundant nodes, and (c) the path after forcing the neighboring nodes to add b-splines for smoothing.

[0028] Figure 15 This is a schematic diagram illustrating the first example of the actual implementation of the present invention in the comparative examples, wherein (a) is the actual mapping site, and (b) is the actual mapping site. Figure 2 3D raster electronic map;

[0029] Figure 16 This is a schematic diagram of the first example path planning effect in the comparative example of the present invention, wherein (a) the traditional A* algorithm combined with SPFA, and (b) the improved A* algorithm combined with SPFA;

[0030] Figure 17 This is a schematic diagram of the structure of the multi-objective waypoint planning method system proposed in the fourth embodiment of the present invention;

[0031] Figure 18 This is a block diagram of the physical hardware design architecture of the multi-target waypoint planning method system proposed in the fourth embodiment of the present invention;

[0032] Figure 19 This is a physical software design architecture diagram of the multi-target waypoint planning method system proposed in the fourth embodiment of the present invention. Detailed Implementation

[0033] To facilitate understanding of the present invention, a more complete description will be given below with reference to the accompanying drawings. Several embodiments of the invention are illustrated in the drawings. However, the invention can be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete.

[0034] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed items.

[0035] Please see Figure 1 The diagram shows a flowchart of a multi-target waypoint planning method in the first embodiment of the present invention. The method includes steps S01 to S05, wherein:

[0036] Step S01: The target environment is rasterized in two dimensions to generate a rasterized map, and the starting point and target point set of the navigator are marked in the rasterized map;

[0037] Specifically, a self-built mobile navigation robot can map the surrounding environment using LiDAR and the Karto-SLAM mapping algorithm. Figure 2 3D rasterization is used to rasterize the actual environment map (MAP) and to mark the starting point V of the navigation machine. start =(X start ,Y start ) and the target point set V point ={V i |i=1,2,3,.....,n}.

[0038] Step S02: Improve the traditional A* algorithm to obtain the improved A* algorithm, and then calculate the cost value between the starting point and each target point according to the improved A* algorithm, and record each cost value in the cost array;

[0039] Specifically, step S02 can be further subdivided into three steps:

[0040] Step S021: Place the navigator at the center of the rasterized map, select 24 neighborhoods around the center of the rasterized map as extended neighborhood nodes, obtain the center point of the neighborhood extension direction through the Euclidean distance slope between the current position of the navigator and the target point, and extend 5 neighborhoods to both ends with the center point θ as the extension angle centerline, forming a total of 13 neighborhood parts.

[0041] Step S022: Introduce the concept of angle into the heuristic function of the traditional A* algorithm, add local straight line cost on the basis of the neighborhood expansion angle cost, and add obstacle influence function in the neighborhood to obtain a new heuristic function with an improved idea, so as to identify and eliminate existing U-shaped dead corner points and pseudo-forced neighbor points;

[0042] Step S023: Introduce quadratic path planning to smooth the path at redundant inflection points and forced neighbor nodes. The quadratic path planning includes starting from the starting point to remove redundant nodes and performing b-spline smoothing on the marked forced neighbor nodes.

[0043] Furthermore, the method for forming the 13 neighborhood portions in step S021 is as follows:

[0044] The current position of the mobile navigation robot is placed at the center of the grid, and the 24 neighbors surrounding the grid are selected as the extended neighborhood nodes. By introducing the concept of angle, the angular cost of neighborhood expansion is adopted. Introducing the concept of angle reduces unnecessary redundant nodes and computations. If `start` is the starting point and `end` is the target node, the best way to determine the direction is the slope of the Euclidean distance. The formula for calculating the slope to angle is expressed as:

[0045] ,

[0046] After obtaining the center point of the neighborhood expansion direction, expand the neighborhood by 5 units in both directions with the center point θ as the center line of the expansion angle. In this method, the neighborhood has a total of 13 parts.

[0047] Typically, the A* algorithm requires traversing eight neighboring nodes centered on itself, but three of these eight nodes contain invalid cost nodes. A common approach is to expand the original eight neighboring nodes to a 24-neighborhood, which speeds up the pathfinding process. However, as the neighborhood expands, the computational cost of the A* algorithm also increases. Furthermore, considering neighborhood discarding methods, the A* neighborhood expansion algorithm adds a large number of redundant nodes. To address the issue of numerous redundant nodes in the traditional 24-neighborhood expansion method of the A* algorithm, this invention proposes a new method for expanding and changing the neighborhood. Introducing the concept of angle into the A* heuristic function can accelerate the direction selection and orientation of the heuristic function. Similarly, in neighborhood expansion, the concept of angle can be introduced to reduce unnecessary redundant nodes and computations. This is achieved through a slope-to-angle calculation formula and... Figure 2 As shown in (a), the relationship between the center θ of the neighborhood expansion direction and the selection of the neighborhood within the angle range can be obtained as shown in Table 1:

[0048] Table 1. Center point θ of neighborhood expansion direction

[0049]

[0050] like Figure 2 As shown in (b), the extension center angle θ from the start point to the end point can be obtained as θ start,end =0°, expand the angle to both ends by 5 neighborhoods with 0° as the center. Introducing the angle effect in the expansion can reduce unnecessary redundant nodes and calculations.

[0051] Furthermore, the heuristic function idea for the improved A* algorithm described in step S022 is as follows:

[0052] To enhance the A* algorithm's path selection by incorporating neighborhood expansion and variation methods, and to accelerate the prediction of the next landing point, an angle cost and a local straight-line cost are further introduced into the traditional A* algorithm's heuristic function. Simultaneously, an improved heuristic function approach is proposed to address two existing path planning challenges. The H(N) formula in the traditional A* algorithm can be expressed as:

[0053] ,

[0054] The improved formula for the heuristic function H(N), which incorporates angle cost and local straight-line cost, can be expressed as:

[0055] ,

[0056] In the formula, H(N) i,j,end Represented as the H(N) formula in the traditional A* algorithm, (X i Y i ) is the current computing node, (X) i Y i ) is the parent node of the current compute node, (X) end Y end ) is the target node, tan (N) i,j,end The introduced angle cost function is DIS(N). i,j Represented as a local linear cost function, L(N) i,j It is represented as local straight-line cost, where α is the angle cost weight and β is the local straight-line cost weight;

[0057] To address the issues of U-shaped dead ends and pseudo-forced neighbors, such as Figure 3 (a) shows a U-shaped dead angle point. Figure 3 (b) contains pseudo-forced neighbor nodes. To address the issues of U-shaped dead ends and pseudo-forced neighbor nodes, an obstacle influence function within the neighborhood is added. This further improves the heuristic function formula after introducing angle cost and local straight-line cost. First, it is necessary to clarify the cases of pseudo-forced neighbors and forced neighbors, as follows: Figure 4As shown, a forced neighbor has an obstacle on only one side when the path has a diagonal line, while a pseudo-forced neighbor has obstacles on both sides. The heuristic function H(N) formula is modified after adding the obstacle influence function within the neighborhood as follows:

[0058] ,

[0059] at this time, This is the obstacle influence function within the local neighborhood. This function is mainly used to distinguish between pseudo-forced neighbors and forced neighbors. This function represents the straight-line cost of obstacles within the local neighborhood. It is mainly used to eliminate U-shaped dead-end points and selectively eliminate nodes with many surrounding obstacles.

[0060] Furthermore, the secondary path planning method described in step S023 is as follows:

[0061] The secondary path planning consists of two steps. The first step is to start from the starting point and go to redundant nodes. The second step is to perform b-spline smoothing on the marked forced neighbor nodes to make the path more consistent with the robot's actual feasible route.

[0062] The first step is to go to the redundant nodes from the starting point:

[0063] Removing redundant nodes requires two arrays: L1_point and L2_point. The path node array `open_list` obtained from A* is assigned to L1_point. Here, the dequeueing and enqueueing ideas from the SPFA algorithm are used to treat the L2_point array as an intermediate buffer array, enabling the selection of the shortest path between each point. Before removing redundant nodes, the `open_list` array needs to be traversed to force neighboring nodes to be marked. The steps for removing redundant nodes are as follows:

[0064] S0231: By improving the A* algorithm, the nodes of the entire path are placed in open_list={(X i ,Y i In the context of the current point (i=0,1,2,3,...,n), the variable NowPoint records the index position of the current point, and the variable Detection records the index position of the point to be detected.

[0065] S0232: Move the current point (X) NowPoint ,Y NowPoint Add it to queue L1_point;

[0066] S0233: Use L2_point to record the current point (X) NowPoint ,Y NowPoint ) respectively with the point to be detected (X) Detection ,YDetection The straight-line connection of the point to be detected (X) is considered acceptable if the straight-line connection does not pass through any obstacle. Detection ,Y Detection If a straight line passes through an obstacle, then the point to be detected (X) is considered valid. Detection ,Y Detection )invalid;

[0067] S0234: If the point to be detected in step S0233 (X) Detection ,Y Detection If valid, then Detection = Detection + 1, and repeat step S0233. If the point to be detected (X) is valid... Detection ,Y Detection If invalid, record NowPoint in NewPoint, NowPoint=Detection, and return to step S0232;

[0068] S0235: Until NowPoint=n, at this point NewPoint={(X i ,Y i The path with redundant nodes removed is represented by the sequence |i=0,1,2,3,...,m}. A comparison diagram showing the path with and without redundant nodes is shown below. Figure 5 As shown.

[0069] The second step is to perform b-spline smoothing on the marked forced neighbor nodes to make the path more consistent with the robot's actual feasible route:

[0070] By adopting the concept of forced neighbors, the forced neighbors in the set S′ are marked. At the marked path nodes, the path is optimized by using b-spline smoothing for the forced neighbor nodes.

[0071] The formula for a b-spline curve can be expressed as:

[0072] ,

[0073] Among them, B i,k (t) is represented as:

[0074] ,

[0075] By substituting the order 3, the basis functions of the b-spline curve can be derived as follows:

[0076] ,

[0077] From the above equation, the complete equation for the b-spline curve can be derived as follows:

[0078] ,

[0079] Among them, P i B is a feature point. i,k (t) represents the K-order b-spline basis function; the diagram shows the comparison between before and after smoothing by smoothing all forced neighbor nodes in the array set S′ through b-spline curves. Figure 6 As shown.

[0080] Step S03: Add weights to the cost array to obtain a directed negative weighted graph with high priority. Input the directed negative weighted graph into the SPFA algorithm to calculate the single-source shortest path from the starting point to the target point. Use the high priority as the stage endpoint to obtain the waypoint order of the target point set and record the waypoint order in the shortest path array.

[0081] Specifically, this also includes: integrating the improved A* algorithm with the SPFA algorithm to establish a directed, negatively weighted graph with point priorities and no negative loops; using the SPFA algorithm combined with a reasonable planning algorithm to obtain the waypoint order, and the navigator will implement a global path planning algorithm between the starting point and the target point based on the waypoint order and the improved A* algorithm; during the movement, the navigator will implement local path planning based on the DWA algorithm to avoid dynamic and static obstacles.

[0082] Understandably, by improving and fusing the A* algorithm with the SPFA algorithm, a directed negative edge-weighted map with point priorities and no negative loops can be established, and the optimal waypoint order can be obtained based on the point priorities in the negative edge-weighted map.

[0083] Step S04: Establish a current variable and a target variable, which are used as the current point index and the target point index, respectively. The current point index points to the starting point, and the target point index points to the head of the shortest path array. Input the current variable and the target variable into the raster map for path point calibration.

[0084] Step S05: The improved A* algorithm is used to traverse between the starting point and the target point. When it is confirmed that the target variable points to the last element of the shortest path array, the multi-waypoint planning task is completed.

[0085] Specifically, after completing the planning of multiple target waypoints in sequence, the navigator will determine whether the last waypoint has been reached based on the target point index status. After completing the planning of multiple target waypoints, it will directly return to the target waiting point to wait for the next task.

[0086] It should be noted that the navigator moves according to waypoint order to traverse the mission objective points and implement a global path planning algorithm, thereby improving overall efficiency; and during the movement, it avoids static obstacles through local path planning, which can closely follow the feasible trajectory of the mobile navigator.

[0087] In summary, based on the aforementioned multi-objective waypoint planning method, by constructing a mobile navigator to rasterize the surrounding environment and marking the navigator's target point set and starting point to simulate the task scenario, experimental verification becomes more convenient. By constructing an improved A* algorithm to calculate the distance cost from the starting point to the target point, the cost reflects the distance between two points, reducing a large number of path points and inflection points and shortening the total path length, thereby effectively and specifically reducing the total path cost. By integrating the improved A* algorithm with the SPFA algorithm, a directed negative edge-weighted map with point priorities and no negative loops is established. The optimal waypoint order can be obtained based on the point priorities in the negative edge-weighted map, and the navigator then moves according to the waypoint order to traverse the task target points and implement a global path planning algorithm, thus improving overall efficiency. Furthermore, during the movement, local path planning is used to avoid static obstacles, which can well match the feasible trajectory of the mobile navigator. Therefore, compared with existing technologies, this invention can plan reasonable and fast feasible paths in multi-objective waypoint tasks in complex environments.

[0088] Please see Figure 7 The diagram shows a flowchart of the multi-target waypoint planning method proposed in the second embodiment of the present invention. The method includes steps S101 to S108, wherein:

[0089] Step S101: Using a mobile navigation robot, the surrounding environment is mapped using LiDAR and the Karto-SLAM mapping algorithm. Figure 2 3D rasterization enables the rasterization of real-world environmental maps (MAPs).

[0090] Step S102: Simulate the task scenario in the rasterized map and mark the target point set V. point ={V i |i=1,2,3,....,n};

[0091] Step S103: Calibrate the starting point V of the mobile navigation robot start =(X start ,Y start ), establish the target array and corresponding cost array for improving A*;

[0092] Specifically, the steps of the improved A* algorithm in step S103 are as follows:

[0093] Step S1031: In the 2D gridded map, with the current position of the mobile navigation robot at the center of the grid, select 24 neighborhoods around the grid as the extended domain nodes; by introducing the concept of angle, the domain extension angle cost is adopted; after obtaining the center point of the neighborhood extension direction through the Euclidean distance slope between the current position of the mobile navigation robot and the task target point, extend 5 neighborhoods in both directions with the center point θ as the extension angle midline. In this method, there are a total of 13 neighborhoods, so it is also called the 13-neighborhood extension method.

[0094] Step S1032: Angle cost is further introduced into the traditional heuristic function using A*, and local straight line cost is added. At the same time, an improved heuristic function is proposed to address the two existing path planning difficulties: U-shaped dead corner points and pseudo-forced neighbor points.

[0095] Step S1033: In the path obtained through step S1032, there will still be a large number of redundant inflection points and unsmooth paths at forced neighbor nodes, which the mobile navigation robot cannot pass through; the present invention introduces secondary path planning to solve the problems that there will still be a large number of inflection points and forced neighbor nodes in the path, and that the path planning at forced neighbor nodes does not fully consider the actual running trajectory of the robot.

[0096] Step S104: From the starting point V start The cost between any two points in the starting and target arrays is calculated using the improved A* algorithm and recorded in the cost array;

[0097] Step S105: Add weights to the directed graph with negative weights using the cost values ​​recorded in the cost array. Points with high priority have negative weights, and the closer the path is to the starting point, the larger the negative value.

[0098] Step S106: Input the directed graph with negative weights into the SPFA algorithm to calculate the single-source shortest path, using the highest priority as the stage endpoint, to obtain the waypoint order in the case of multiple destinations, and record it in the shortest path array Z={(X start ,Y start )|start=1,2,...,n};

[0099] It should be noted that this invention can also be implemented in multiple complex scenarios by changing the rational planning algorithm combined with the SPFA algorithm, including ward medication delivery, book inquiry and retrieval, restaurant meal delivery, sightseeing guidance, etc.

[0100] Step S107: Establish the current variable and the target variable as the current point index and the target point index respectively, with the current point index pointing to the starting point V. startThe target point index points to the head of the shortest path array Z. The current variable and the target variable are input into a raster map for pathpoint calibration. An improved A* algorithm is used to locate the starting point V. start Iterate between the two points in the target array;

[0101] Step S108: Confirm whether the target variable points to the last element of the shortest path array Z. If yes, the multi-waypoint task is completed. If no, assign the current variable to the target variable, then point the target variable to the next element of the shortest path array Z, and repeat step S107.

[0102] It should be noted that the 13-neighborhood expansion method, heuristic function improvement idea and secondary path planning method involved in the above steps S1031, S1032 and S1033 can refer to the description in Embodiment 1, but are not limited to the specific expansion range mentioned above. The domain expansion method can be designed according to the specific simulation scenario, the domain obstacle influence function can be designed according to the specific road conditions, and multiple path planning can be performed according to the path complexity.

[0103] In the third embodiment of the present invention, it should also be noted that the SPFA algorithm is combined with a rational planning algorithm, the details of which are as follows:

[0104] in, Figure 8 (a) In the first example, Start is the starting point and End is the ending point. Points A, B, C, D, E, and F are all ordinary priority points, while vip1 and vip2 are two high-priority points. Due to map path limitations, arrows indicate directional movement; travel between points is not allowed in reverse, and direct routes between points are not possible via line segments. The weights in the line segments are derived from the improved A* algorithm, representing distance values. In task planning, high-priority points are used as the endpoints of each stage, and the current endpoint is used as the starting point for the next stage. Multiple stage path plans are merged to form the final path planning point order.

[0105] in, Figure 8 The second example in (b) illustrates a scenario with multiple deliveries from a single supply point, where the point priority is immutable. Only the delivery point priority needs to be planned. However... Figure 8 In (b), there are multiple supply points and multiple delivery points. Points do not have fixed priorities; their priorities change based on the conditions met. In the diagram, start is the starting point, end is the ending point, points 1, 2, 3, 4, 5, 6, and 7 are supply points, and points I and II are delivery points. When the delivery needs of points I and II are not met, their priorities are the same. When a delivery need is met, the task priority will increase, and the task will stop along the way. Figure 8In (b), the actual route planning has no limitations. Delivery points I and II are only used for bridging in the algorithm and are not considered as repeated points in the actual route planning.

[0106] Figure 9 (a) is Figure 8 (a) is a simplified task diagram. The waypoints are set in the order of A, E, B, and VIP1. By rationally planning the algorithm details, VIP1, a high-priority point, is chosen as the first stage endpoint. The shortest distance from Start to VIP1 is calculated using the SPFA algorithm. The SPFA algorithm is used to dequeue the head of the queue and relax the other connection points, resulting in the first path: Start→A→VIP1. VIP1 is then designated as the second stage starting point. Since it passes through point A, it is no longer considered. The SPFA algorithm is used to calculate the shortest path between VIP1 and E, B, resulting in the second path: VIP1→E. Because only point B remains, and there are fewer than three task points, path planning is no longer performed. Therefore, the optimal path is: Start→A→VIP1→E→B.

[0107] Figure 9 (b) is Figure 8 (a) is a simplified task diagram. The waypoints are set in the order of A, E, F, D, VIP1, and VIP2. By carefully planning the algorithm details, either the high-priority point VIP1 or VIP2 is chosen as the first stage endpoint. The SPFA algorithm is used to calculate the shortest distances from the Start point to VIP1 and VIP2 respectively. The point with the lowest cost is selected as the first stage endpoint. After relaxation operations are performed on the other connection points, the first path is obtained as Start→A→VIP1. VIP1 is then set as the start point of the second stage (since it passes through point A, it is no longer considered), and VIP2 is set as the endpoint of the second stage. The SPFA algorithm is used to relax the other connection points, resulting in the second path as VIP1→E→F→VIP2. Since only point D remains, and there are fewer than three task points, path planning is no longer performed. Therefore, the optimal path is: Start→A→VIP1→E→F→VIP2→D.

[0108] Figure 9 (c) is Figure 8(b) is a simplified task diagram. The set waypoint order includes 2, 3, 4, 5, 7, I, and II. Based on the detailed algorithm planning in Example 2, the resupply locations for delivery point I are 2, 4, and 7, and for delivery point II, they are 2, 3, and 5. All points and weight information are input into the SPFA algorithm. The SPFA algorithm yields the optimal path for the first stage as: Start → 2 → 3 → 4. Since points 2, 3, and 4 are no longer considered, point 4 is taken as the starting point for the second stage. The remaining points and edge information are input into the SPFA algorithm. The SPFA algorithm shows that, when passing through delivery points I and II, the path to point 7 is the shortest. Because it does not meet the delivery needs of delivery points I and II, no stop is made. Therefore, the optimal path for the second stage is: 4 → 7. Since reaching point 7 will satisfy the delivery needs of delivery point 1, point 1 has high priority. In the third path planning, delivery point 1 is taken as the destination, and the path calculated using the SPFA algorithm is: 7 → 2 → 1. Because the delivery task of point 2 is not satisfied, the optimal path for the third segment is: 7 → 1. Since only one supply point 5 and one delivery point 2 remain, path planning is not performed further because the current delivery points do not meet the delivery needs and there are fewer than two supply points. Therefore, the optimal path is: Start → 2 → 3 → 4 → 7 → 1 → 5 → 2.

[0109] In the comparative examples of this invention, in order to verify the rationality of the improved A* algorithm path planning, the traditional A* algorithm, the improved A* algorithm (without smoothing), and the improved A* algorithm (with smoothing) are compared in the established simulation experimental environment.

[0110] First, obstacle maps of 20x20, 30x30, and 40x40 pixels were created. Then, the traditional A* algorithm, the improved A* algorithm (without smoothing), and the improved A* algorithm (with smoothing) were run on each of the three obstacle maps. The output results were compared, as follows. Figure 10 , 11 As shown in Figures 1 and 12.

[0111] By comparison Figure 10 and Figure 11 The three images in the figure show the path generated by the traditional A* algorithm in Figure (a), which contains a large number of redundant nodes and a tortuous path, making it unsuitable for the actual robot's travel route. After removing redundant nodes by improving the A* algorithm, as shown in Figure (b), the path becomes smoother. However, the radius of the robot itself is still not considered at the forced neighbor nodes, resulting in a deviation from the actual travel route. After adding b-splines to the forced neighbor nodes for smoothing, as shown in Figure (c), the possibility of the robot colliding with the corners of obstacles during actual travel is reduced. Figure 12It is a complex 40x40 environment map, which hides typical U-shaped dead ends and pseudo-forced neighbor nodes. Figure 12 In the traditional A* algorithm, a pseudo-forced neighbor node is encountered at coordinates (31, 13). The traditional A* algorithm considers the diagonal point as a feasible point, without considering the robot's actual operating environment. By adding an obstacle influence function to identify pseudo-forced neighbor nodes, the improved A* algorithm achieves the effect shown in Figure (b). The improved A* algorithm can avoid pseudo-forced neighbors and generate feasible paths. Figure (c) shows the result after b-spline smoothing of the forced neighbor points.

[0112] To compare the improved A* algorithm with the traditional A* algorithm, Figure 10 The data from points 11 and 12 are summarized in Tables 2, 3, and 4 below. Tables 2 and 3 show that, without encountering false forced neighbors, the improved A* algorithm reduces the number of path nodes by 84.9% and the number of inflection points by 63.6% compared to the traditional A* algorithm through secondary path planning to remove redundant nodes, resulting in an overall cost reduction of 6.4% and a shorter runtime. Table 3 shows that, even with false forced neighbors, the improved A* algorithm can avoid them and successfully plan a safe path.

[0113] Table 2. Data (20*20) of traditional A* algorithm, improved A* algorithm (without smoothing), and improved A* algorithm (with smoothing).

[0114]

[0115] Table 3. Data (30*30) of traditional A* algorithm, improved A* algorithm (without smoothing), and improved A* algorithm (with smoothing).

[0116]

[0117] Table 4. Data (40*40) of traditional A* algorithm, improved A* algorithm (without smoothing), and improved A* algorithm (with smoothing).

[0118]

[0119] This invention aims to verify that the fusion of the SPFA algorithm and the improved A* algorithm can successfully plan reasonable and efficient paths in multi-objective waypoint missions under complex environments. An experimental environment was built to verify the feasibility of the path calculation using the SPFA algorithm alone. The path results calculated by SPFA were then simultaneously applied to both the A* and improved A* algorithms, and the results are as follows. Figure 13 As shown. To obtain a visual comparison of the improved A* algorithm and the traditional A* algorithm combined with the SPFA algorithm, respectively, we will... Figure 13 The data is organized as shown in Table 5 below. Figure 13Figure (a) shows the path planning of the traditional A* algorithm in the SPFA experimental environment. There is a U-shaped dead end at the coordinate point (29,33), and the path planning of the traditional A* algorithm passes through this point. Figure 13 Figure (b) shows the improved A* algorithm's path planning in the SPFA experimental environment. The U-shaped dead corner point encountered in Figure (a) is successfully avoided in Figure (b) by adding an obstacle influence function. Furthermore, b-spline smoothing is added to the forced nodes in Figure (b), achieving the desired effect. Figure 13 As shown in Figure (c).

[0120] Table 5. Data from the traditional A* algorithm, the improved A* algorithm (without smoothing), and the improved A* algorithm (with smoothing) (SPFA)

[0121]

[0122] This invention aims to verify that the traversal order of task points can be well implemented using the improved A* algorithm in complex environments, achieving path planning between task points with multiple properties and priorities. An actual experimental environment was constructed in a simulation setting, and the task point order obtained using the SPFA solution rule was tested using both the improved A* algorithm and the traditional A* algorithm. Figure 14 As shown. Figure 14 Figure (a) shows the path planning of the traditional A* algorithm in the actual experimental site simulation environment. After removing redundant nodes by improving the A* algorithm, as shown in Figure (b), b-spline smoothing curves are used at the forced neighbor nodes in Figure (b), and the path planning is shown in Figure (c).

[0123] To further verify the feasibility of combining the improved A* algorithm with the SPFA algorithm in real-world scenarios, a practical experiment was conducted in a man-made experimental setting. First, a two-dimensional grid electronic map of the experimental scenario was constructed using a LiDAR and the Karto-Slam mapping algorithm. For example... Figure 15 As shown. Figure 15 The black lines represent the outlines of obstacles, and the red letters mark the positions of predefined path nodes. The experimental results comparing the traditional A* algorithm combined with SPFA and the improved A* algorithm combined with SPFA in this experimental environment are as follows: Figure 16 As shown. To obtain a visual comparison of the improved A* algorithm and the traditional A* algorithm combined with the SPFA algorithm, respectively, we will... Figure 16 The actual operational data from the site is summarized in Table 6 below.

[0124] Table 6. Actual field operation data of the improved A* algorithm and the traditional A* algorithm combined with the SPFA algorithm.

[0125]

[0126] Figure 16 (a) is the traditional A* combined with SPFA algorithm, which results in a large number of winding paths in the planned path, while Figure 16 (b) The improved A* combined with SPFA algorithm produces a smoother path compared to the traditional A* combined with SPFA algorithm. At obstacle corners, the obstacle expansion coefficient is reduced to make the smoothness more visually apparent. Figure 16 (b) shows the smooth path at the obstacle corner. Table 6 shows that the improved A* fusion SPFA algorithm reduces the path length by 18.6% and the overall running time by 17.5% compared to the traditional A* fusion SPFA algorithm. Furthermore, through practical field verification, the A* fusion SPFA algorithm can effectively solve the task planning problem of multi-objective waypoints in directed graphs with negative weights and point priorities in real-world engineering applications.

[0127] This invention proposes a multi-objective waypoint planning method that integrates the improved A* algorithm and the SPFA algorithm. Simulation experiments and real-world field tests were conducted to demonstrate the effectiveness of the proposed algorithm. Simulation data shows that, compared to the traditional A* algorithm, the improved A* algorithm reduces the number of path nodes by 84.9%, the number of inflection points by 66.6%, and the overall cost by 10.8% in the SPFA experimental environment, while also shortening the running time. Furthermore, the improved A* algorithm can avoid U-shaped dead ends and pseudo-forced neighbor nodes in complex environments.

[0128] Please see Figure 17 The figure shown is a schematic diagram of the multi-target waypoint planning system proposed in the third embodiment of the present invention. The system includes:

[0129] The simulation scene module 10 is used to perform two-dimensional rasterization of the target environment to generate a rasterized map, in which the starting point and target point set of the navigator are marked;

[0130] The improved algorithm module 20 is used to improve the traditional A* algorithm to obtain the improved A* algorithm, and then calculate the cost value between the starting point and each target point according to the improved A* algorithm, and record each cost value in the cost array.

[0131] The fusion algorithm module 30 is used to add weights to the cost array to obtain a directed negative weighted graph with high priority. The directed negative weighted graph is input into the SPFA algorithm to calculate the single-source shortest path from the starting point to the target point. The high priority is used as the stage endpoint to obtain the waypoint order of the target point set and the waypoint order is recorded in the shortest path array.

[0132] The path calibration module 40 is used to establish a current variable and a target variable, which are respectively used as the current point index and the target point index. The current point index points to the starting point, and the target point index points to the head of the shortest path array queue. The current variable and the target variable are input into the raster map for path point calibration.

[0133] The traversal verification module 50 is used to traverse between the starting point and the target point using the improved A* algorithm. When it is confirmed that the target variable points to the last element of the shortest path array, the multi-waypoint planning task is completed.

[0134] This invention utilizes LiDAR and Hall effect sensor data as odometry inputs, IMU data as imu_data inputs, and ultrasonic data as an additional reference layer in the local cost map. The Karto-SLAM mapping algorithm preprocesses this data, imu_data is filtered using imu_filter, and the robot_pose_ekf algorithm uses Hall effect sensor data as odometry input, imu_data, ultrasonic data, and LiDAR data as inputs. Multi-sensor data fusion is then performed to obtain the ODOM and MAP coordinates of the mobile navigation robot at its current position, along with the relative positional error between the two coordinates. The Monte Carlo localization algorithm (AMCL) is then used to relocalize and match the area around large obstacles in the 2D grid map by reading LiDAR data. The mobile navigation robot repositions itself by rotating in place. The robot can post speed-related topics via keyboard control, and the underlying control system receives these topics in bare-metal development using the ros_lib library. After receiving the control information, the drive wheels are manipulated to move forward, backward, left, and right. The movement then packages the ultrasonic, IMU, and Hall element values ​​into a single data set and sends it to the upper-level controller for data fusion and to rasterize the surrounding environment map, thus achieving a two-dimensional rasterized environment map.

[0135] The physical hardware design architecture block diagram of this invention embodiment is as follows: Figure 18 As shown, the physical construction software design architecture block diagram of this invention is as follows: Figure 19 As shown.

[0136] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0137] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A multi-objective waypoint planning method, characterized in that, The method includes: S1: The target environment is rasterized in two dimensions to generate a rasterized map, in which the set of starting and target points of the navigator are marked; S2: Improve the traditional A* algorithm to obtain the improved A* algorithm, and then calculate the cost value between the starting point and each target point according to the improved A* algorithm, and record each cost value in the cost array; The steps in S2 include: S21: Place the navigator at the center of the raster map, select 24 neighborhoods around the center of the raster map as extended neighborhood nodes, obtain the center point of the neighborhood extension direction through the Euclidean distance slope between the current position of the navigator and the target point, and extend 5 neighborhoods to both ends with the center point θ as the extension angle centerline, forming a total of 13 neighborhood parts. S22: Introducing the concept of angle into the heuristic function of the traditional A* algorithm, adding local straight line cost on the basis of the neighborhood expansion angle cost, and adding the obstacle influence function in the neighborhood, we obtain a new heuristic function with an improved approach, which can identify and eliminate existing U-shaped dead corner points and pseudo-forced neighbor points. The steps in S22 include: The formula for introducing the concept of angle into the heuristic function of the traditional A* algorithm and adding local line cost on the basis of neighborhood expansion angle cost is expressed as follows: in, Represented as the H(N) formula in the traditional A* algorithm, This represents the function improved from the H(N) formula in the traditional A* algorithm. The introduced angle cost function is α, where α is the angle cost weight and β is the weight of the impact of obstacles in the local neighborhood. Represented as a local linear cost function, , These are the x and y coordinates of the current calculation node. , These are the x and y coordinates of the parent node of the current calculation node. , Let x and y be the x and y coordinates of the target node. Represented as local linear cost; The formula for adding the influence function of obstacles in the neighborhood is expressed as follows: in, This is the obstacle influence function within the local neighborhood, used to distinguish between pseudo-forced neighbors and forced neighbors; It is the straight-line cost of obstacles in the local neighborhood, used to eliminate U-shaped dead corners and selectively eliminate nodes with many surrounding obstacles; S3: Add weights to the cost array to obtain a directed negative weighted graph with high priority. Input the directed negative weighted graph into the SPFA algorithm to calculate the single-source shortest path from the starting point to the target point. Use the high priority as the stage endpoint to obtain the waypoint order of the target point set and record the waypoint order in the shortest path array. S4: Establish a current variable and a target variable, which are used as the current point index and the target point index, respectively. The current point index points to the starting point, and the target point index points to the head of the shortest path array. Input the current variable and the target variable into the raster map to perform path point calibration. S5: The improved A* algorithm is used to traverse between the starting point and the target point. When it is confirmed that the target variable points to the last element of the shortest path array, the multi-waypoint planning task is completed.

2. The multi-objective waypoint planning method according to claim 1, characterized in that, The steps in S1 include: The target environment is rasterized into two dimensions using LiDAR and the Karto-SLAM mapping algorithm to generate a rasterized map, which is then used by a navigation robot to move within the rasterized map.

3. The multi-objective waypoint planning method of claim 1, wherein, Step S2 further includes: S23: Introduce quadratic path planning to smooth the path at redundant inflection points and forced neighbor nodes. The quadratic path planning includes starting from the starting point to remove redundant nodes and performing b-spline smoothing on the marked forced neighbor nodes.

4. The multi-objective waypoint planning method of claim 1, wherein, In step S21, the Euclidean distance slope formula is expressed as: ， In the formula, start represents the starting point, and end represents the target node. Indicates the x-coordinate of the starting point. Represents the starting point's ordinate. Represents the x-coordinate of the target node. Represents the ordinate of the target node.

5. The multi-objective waypoint planning method of claim 3, wherein, In step S23, the step of removing redundant nodes from the starting point includes: Construct the first and second arrays, and use the path node array S={(X) obtained from the improved A* algorithm. i ,Y i Assign values ​​to the first array (i=1,2,3,...,n), and use the dequeue and enqueue ideas from the SPFA algorithm to make the second array an intermediate buffer array to achieve the shortest path selection between each point.

6. The multi-objective waypoint planning method of claim 3, wherein, In step S23, the step of performing b-spline smoothing on the marked forced neighbor nodes includes: By employing the concept of forced neighbors, forced neighbors in set S′ are marked. At the marked path nodes, the path is optimized by using b-spline smoothing to make the generated path closely resemble the navigator's runnable trajectory.

7. The multi-objective waypoint planning method of claim 1, wherein, The steps in S3 include: By improving and fusing the A* algorithm with the SPFA algorithm, a directed negative weighted graph with point priority and no negative loops is constructed. By combining the SPFA algorithm with a reasonable planning algorithm, the waypoint order is obtained. The navigator will then perform a global path planning algorithm between the origin and the destination based on the waypoint order and the improved A* algorithm. During movement, the navigator performs local path planning based on the DWA algorithm to avoid dynamic and static obstacles.

8. The multi-objective waypoint planning method according to claim 1, characterized in that, The steps in S4 include: After completing the planning of multiple target waypoints in sequence, the navigator will determine whether it has reached the last waypoint based on the target point index status. After completing the planning of multiple target waypoints, it will directly return to the target waiting point to wait for the next task.

9. A multi-objective waypoint planning system, characterized in that, The multi-objective waypoint planning system is applied to the multi-objective waypoint planning method as described in claim 1, the system comprising: The simulation scene module is used to perform two-dimensional rasterization of the target environment to generate a rasterized map, in which the starting point and target point set of the navigator are marked; An improved algorithm module is used to improve the traditional A* algorithm to obtain an improved A* algorithm. Then, based on the improved A* algorithm, the cost value between the starting point and each target point is calculated, and each cost value is recorded in the cost array. The fusion algorithm module is used to add weights to the cost array to obtain a directed negative weighted graph with high priority. The directed negative weighted graph is input into the SPFA algorithm to calculate the single-source shortest path from the starting point to the target point. The high priority is used as the stage endpoint to obtain the waypoint order of the target point set and the waypoint order is recorded in the shortest path array. The path calibration module is used to establish current variables and target variables, which serve as the current point index and the target point index, respectively. The current point index points to the starting point, and the target point index points to the head of the shortest path array. The current variables and target variables are input into the raster map for path point calibration. The traversal verification module is used to traverse between the starting point and the target point using the improved A* algorithm. When it is confirmed that the target variable points to the last element of the shortest path array, the multi-waypoint planning task is completed.

Images