Abstract
To address the problems of the traditional A algorithm in solving paths with many expansion nodes, high memory overhead, low operation efficiency, and many path corners, this paper improved the traditional A algorithm by combining jump point search strategy and adaptive arc optimization strategy. Firstly, to improve the safety of our paths, the risk area of the obstacles was expanded. Then, the A algorithm was combined with the jump point search strategy to achieve the subnode jump search, reducing the calculation scale and memory overhead, and improving search efficiency. Considering the influence of the density of obstacles on search efficiency, the heuristic function was enhanced according to the special effects of the density of obstacles. Finally, the redundant jump point and adaptive arc optimization strategies were used to shorten the path length further and enhance the initial path’s smoothness. Simulation results showed that our algorithm outperforms traditional A and literature algorithms in path length, security, and smoothness, and then was further validated and applied in large-scale marine environments and realistic settings.
1. Introduction
With the continuous improvement of automation, mobile robots have been widely used in various scenarios, such as warehouse handling robots, service robots, disinfection robots, and uncrewed express delivery vehicles. Path planning is one of the critical technologies in mobile robots, and the purpose is to find a collision-free optimal or suboptimal path for the robot from the starting position to the target position [1].
Many algorithms for path planning have been proposed by scholars, such as the A algorithm [2], Dijkstra algorithm [3] as the representative of geometric model search algorithm and particle swarm optimization (PSO) algorithm [4], and ant colony (ACO) algorithm [5] as the representative of the intelligent optimization algorithm. The A algorithm has been widely studied and applied due to its relatively small computational effort, high search efficiency, and relatively optimal planning path compared with other algorithms [6].
In response to the problems of the traditional A algorithm, such as too many traversing nodes, large memory occupation, and poor path smoothness, some better improvement strategies have been proposed by researchers. Yang et al. [7] extended the search method of the traditional A algorithm from eight directions in eight neighborhoods to sixteen directions in sixteen neighborhoods at the expense of the search efficiency of the algorithm, which improved the smoothness and optimality of the path. Wang et al. [8] used a two-way search strategy to improve the A algorithm, and the simultaneous iterative search in both positive and negative directions improved the search efficiency, but it ignored the problem of large memory occupation in large-scale map paths. Zafar et al. [9] used the jump point search method to improve the node expansion method of the A algorithm, which solved the memory problem of high overhead and low search efficiency in large-scale maps [8], but the smoothness of the paths is not satisfactory yet. Tang et al. [10] used third-degree B-spline to smooth the paths planned by the A algorithm but ignored the constraint problem of the obstacle region, which is prone to cause the smoothed paths to traverse obstacles in a narrow area. Saeed et al. [11] solved the problem of Tang et al. [10] by inserting additional path nodes between the original path points until they could satisfy and generate a smoothed path that does not occur to traverse obstacles. Song et al. [12] designed three path smoothers to optimize the paths, but the time complexity of this algorithm is vulnerable to the scale of the number of nodes. For the safety of robot movement in realistic environments, Zhang et al. [13] considered the distance of nodes from obstacles in the A expansion node, using the threat generation value as part of the evaluation of the node cost. That algorithm ensures path safety but does not apply to the large-scale complex map environments in a natural setting.
The above-improved algorithms [7–13] improve the search efficiency, path smoothing, and security of the traditional A algorithm to some degree. Considering the above issues comprehensively, the main contributions of this paper are as follows: firstly, the obstacles modeled by the grid method are puffed, and the risk areas are defined, and then, our improved A does not search the risk region during the search path so that we reduce the computation of the algorithm and ensure the safety of the planned path. Then, we integrate the jump point search strategy with the A algorithm to avoid expending unnecessary nodes in the search process of A, so as to reduce the computational scale and the computer memory usage. In addition, we introduce the distribution density of obstacles to adaptively change the weights of the evaluation function of our enhanced A, which further improves the search efficiency of the algorithm. Finally, we utilize the redundant jump point deletion strategy and the adaptive arc optimization strategy to secondary optimize the planning path to reduce the turning points and circularize the corner path to improve the path quality further.
2. Environmental Modeling
The grid method [14] is widely used to build mobile robots for various working environments as it has a simple structure and is easy to implement. Traditional grid modeling methods ignore the mobile robot’s volume constraints and kinematic constraints, leading to some planned paths colliding with the edges of obstacles, which increases the risk of collision in reality [15].
To solve the above problems, we add the identification processing of risk area based on the modeling approach of Tsardoulias et al. [16]. Firstly, the actual presence of irregular obstacles in the environment is considered. In the process of environment modeling with the traditional (0,1) grid method, the obstacle areas that do not occupy the whole-cell grid need to be puffed and filled, and then are considered obstacle areas that are forbidden to pass, after which they are represented by “1” in the matrix map. The area without filling is the free passage area and is characterized by “0” in the matrix . Next, the outer grid area of the obstacle grid is determined to be the risk area, and the risk area is represented as “2’ in the matrix . The steps for identifying risk areas are as follows: Step 1 Iterate the coordinate positions of all obstacles in the grid matrix . Step 2 Judge whether the obstacle grid is located in the nonboundary range according to equation (1). If it is satisfied, the free grid near it will be judged as the risk grid according to equation (2), and the corresponding matrix value will be changed from “1” to “2.” where represents the number of obstacles; represents the maximum horizontal coordinate of the ; represents the maximum vertical coordinate of the ; represents the original grid of obstacle in the eight fields in the grid matrix ; and is the grid matrix after satisfying the judgment condition, where .
Figure 1 is a schematic diagram of our grid method, and the orange triangular area in Figure 1(a) is an obstacle in the actual environment; the black area represents the expanded grid area that the robot cannot pass and corresponds to “1” in Figure 1(b); the gray area is the expanded risk area and corresponds to “2” in Figure 1(b); the white area is the passable area and corresponds to “0” in Figure 1(b).
(a)
(b)
While developing the improved A algorithm to generate path, we avoid extending the search nodes to the risk region, enhancing algorithm search efficiency and making the initially planned paths maintain a certain safety distance from the obstacles. We only consider the distance between the path and the original obstacle in the path optimization stage. Our method guarantees the optimality and safety of the path to a great extent compared with some methods in the literature and reduces the performance requirements of the mobile robot for dynamic obstacle avoidance.
3. Improved A Algorithm Based on Jump Point Search (JPS-A)
3.1. Standard A Algorithm
The A algorithm is a classical heuristic search algorithm with a wide range of applications in solving optimal paths in two-dimensional static environments [17]. The algorithm combines the heuristic idea of the Breadth-First Search algorithm and the shortest path search method of Dijkstra to select the optimal node sequentially by evaluating the cost of each extended node, and the evaluation function of the current node is as follows:where denotes the total cost of the current node, denotes the actual cost from the starting node to the current node, denotes the heuristic distance of the current node from the target node.the The heuristic function of the standard A∗ algorithm uses the Manhattan distance (4) and the Euclidean distance of equation (5), respectively, which can be expressed as follows:where is the coordinates of the current node, and is the coordinates of the target node. The A algorithm based on Manhattan distance can only achieve the search of four neighborhoods and four directions. The A algorithm based on Euclidean distance can investigate eight neighborhoods and eight directions, so in this paper, we use the Euclidean distance to calculate to ensure the optimal path. As shown in Figure 2, both methods will inevitably search for multiple symmetric paths with the same start point, endpoint, and path length, which differ only in the order of the nodes, thus theoretically causing redundant computations in the algorithm search process [18].
(a)
(b)
The A algorithm creates two lists when searching the path: for the current node’s neighboring nodes and for the visited nodes. Then, the algorithm starts from the starting point and expands to the neighboring nodes, adds the nodes that A ∗ considers for expansion to the, calculates their generation values, sorts them according to their generation values, takes out the node with the smallest generation value as the next parent node, and adds the previous parent node to the , then repeats the above search process until it reaches the target node. The search strategy of the traditional A algorithm makes a large number of nodes constantly maintained and accessed, which leads to the defects of high computation, serious memory consumption, and low efficiency when performing path planning in a large-scale map environment.
3.2. Improving A Algorithm Based on Jump Point Search Strategy
To solve the problems mentioned earlier, such as many extended nodes, high memory consumption, and low search efficiency in a large-scale map environment, we use the jump point search strategy [19] to improve the conventional A algorithm.
The jump point search strategy breaks the symmetry of paths and reduces the computational scale by giving up evaluating a large number of nodes during path search and only expanding the nodes that conform to the jump point rules. The jump point search strategy will prefilter out the non-natural neighbor nodes in the map and then search the suitable nodes from the remaining nodes as jump points. The following are the rules for defining non-natural neighbor nodes, forced neighbor nodes, and jump points in this paper.
3.2.1. Non-Natural Neighbor Nodes
Figure 3 shows the filtering rules for non-natural neighbor nodes, where is the parent node of node ; the direction of the parent node pointing to is the direction of the algorithm extension node; and the yellow grid is the non-natural neighbor nodes that do not need to be considered in the search path process.
(a)
(b)
As shown in Figure 3(a), starting from the parent node , there are shortest or equivalent paths to all yellow nodes without going through node x. Therefore, all yellow nodes in the straight-line direction are nodes that do not need to be computed, and for such nodes in the straight-line path that satisfy equation (6), we define them as non-natural neighbor nodes. All the yellow nodes in Figure 3(b) are directly reachable by with a minimum path score value, and the filtering rule for non-natural neighbor nodes in the diagonal direction is in equation (7)as follows:where represents the generation value of the path; represents the path from parent to node via node ; and denotes the path from parent to node directly without going through node .
3.2.2. Forced Neighbor Nodes
For pathfinding with obstacle disturbances like Figure 4, there does not exist an optimal path from without going through node to node directly in the both linear and diagonal directions. Therefore, node is defined as a forced neighbor node of node x with the following filtering rule.(1)Node satisfies the definition of non-natural neighbor node.(2)Node satisfies the rule of equation (8).
(a)
(b)
3.2.3. Jump Point
A nodeis defined as a jump point of nodeif there exists a minimum value such that the formulaholds and one of the following three conditions is satisfied [20].(1)Node is the target node.(2)At least one forced neighbor node of node .(3)If is the diagonal direction and there exists a node at the same time ( is the diagonal horizontal and vertical decomposition vector direction), node can move steps in that direction to reach node , while node is the defined jump point by condition (1) or condition (2).
The jump point search starts by finding jump points along the straight line and diagonal directions, adding the nodes that meet the definition of jump points to the OpenList, and then selecting the node with the lowest generated value as the new extended node for calculation up to the target point. As shown in Figure 5, it is the starting point, T is the target point, the green grids represent the nodes extended to in the path search, the red grids represent the forced neighbor nodes of the jump point search, and the solid black line is the final path. In the same map environment, both algorithms can plan a safe path.
(a)
(b)
However, jump point search prunes a large number of non-natural neighbor nodes in the process of expanding nodes and expands only natural neighbor nodes as well as forced neighbor nodes. Compared with the A algorithm, it dramatically reduces the computation of nodes and speeds up the search time. It is evident from Figure 5 that the number of green search grid points of our algorithm is small, and the jump point search can achieve a long-distance jump between two nonadjacent jump points according to the definition of the jump point, which means the computational scale is small. In summary, the improved algorithm can significantly reduce the computational complexity and thus the time scale. Also, the number of path nodes is further reduced, which is a more pronounced improvement in large-scale maps, as demonstrated in subsequent sections 5.1 and 5.2.
Our jump point search strategy is suitable for 2D static planar path planning. It considers only distance cost and greatly outperforms the traditional A algorithm since it removes many neighboring nodes with suboptimal spaces. Moreover, it does not apply to 2D environments considering nonflat terrain [13] because it focuses on the distance constraint of the node rather than other factors, such as the terrain height difference between that node and the surrounding nodes.
3.3. Improving the Heuristic Function of JPS-A Algorithm
The heuristic function of the traditional A algorithm is shown in equation (3), which does not consider the influence of obstacle density on search efficiency. Since there are usually many obstacles between the starting point and the target point, the algorithm will occupy ample memory space and reduce the efficiency during the search process, and generate more redundant nodes [21]. When the number of obstacles in the environment space is small, the value of the heuristic function of A is closer to the actual distance value, so the weight of the heuristic function can be increased appropriately to reduce the algorithm’s search space and improve the search efficiency. When the number of obstacles in the region is more complex, the heuristic function value of the algorithm will be smaller than the actual distance value, so the weight of the heuristic function can be reduced appropriately to increase the number of expansion nodes of the algorithm, which will improve the search capability of the optimal path by expanding the search space.
To this effect, equations (9) and (10) are used to calculate the obstacle density P within the rectangular region composed of the current node and the target node, and the obstacle density P is introduced into the heuristic function for improving A.where is the coordinates of the current node; is the number of obstacles in the rectangular region composed of the current node and the target node; and and are the lengths of the boundaries in the horizontal and vertical axis directions of the map, respectively. The improved heuristic function enables the algorithm to adaptively adjust the weights of the heuristic function according to the obstacle rates in different regions.
To verify the effectiveness of the improved heuristic function, we use the heuristic function as the only variable. It is evident from Figure 6 that the number of extended nodes of the improved heuristic function is reduced to 28 compared with 41 under the traditional method for both planning path lengths of 20.9706 m. The 31.7% reduction in the expansion nodes improves the search speed of the algorithm by 32.8%. The main reason is that the algorithm searches in the region with a low obstacle rate by reducing the search space and thus improving the search efficiency.
(a)
(b)
4. Path Optimization Strategy
The improved A algorithm based on jump point search can significantly improve the search efficiency, but the planned path still has more turning points and the robot needs to make several unnecessary attitude adjustments to track path [22]. In order to satisfy the nonholonomic constraints of mobile robots, we propose a redundant jump point deletion strategy and an adaptive arc optimization strategy to optimize the path further.
4.1. Redundant Jump Point Removal Strategy
The path generated by the traditional A algorithm is composed of a continuous grid containing more turning points. Jump point search can achieve long-distance jumps between nodes, but the smoothness of the path is not improved more significantly, so we propose a redundant jump point removal strategy to eliminate the redundant turning points.
4.1.1. Step 1 Extract key jump points
Extract three consecutive jump points , , and from the initial path compare the slope of the line formed by and with the slope of the line formed by and . If the slopes are equal, we delete jump point . We iterate through all jump points, delete all common jump points, and generate a path sequence containing only the start point, key jump points, and endpoint.
4.1.2. Step 2. Remove redundant jump points
The path sequence generated after Step 1 is and connects the point with the point , and if the line segment does not cross the obstacle area, then we continue to connect until the line segment crosses the obstacle area, and then, we determine , which are the redundant jump points, and the corresponding path sequence is deleted. We repeat the above operation from point again until all the jump points in the path have been detected.
As shown in Figure 7, the path optimized by the redundant jump point removal strategy breaks through the limitation of eight neighborhood search directions and can realize the connection between nodes in noneight directions. The length of the path is further shortened, and the number of turning points is also reduced. But the same problem of an unsmooth path at the turning point still exists [23].
4.2. Adaptive Arc Optimization Strategy
As shown in Figure 8, the starting position of the mobile robot is , and the end position is . The algorithm starts from the starting point and circularizes the corner areas in the path until the endpoint [24], and the specific steps are as follows:
Step 1. Compare the lengths of the line segments and , choose the short side of them as the initial tangent side, whose endpoint is the initial tangent point, and make a vertical line through the initial tangent point to intersect with the angle bisector of at the point ; then, the center of the circle of the tangent circle is as follows:The radius of the tangent circle is as follows:The equation of the tangent circle is as follows:where is the slope of the short side, and is the slope of the angle bisector.
Step 2. Judge whether the tangent circle intersects the long side at a point . If yes, we execute Step (3); otherwise, we go to Step (4).
Step 3. Determine whether the arc crosses the obstacle area. If yes, then execute Step (4); otherwise, we replace the fold line with the arc and go to Step (5).
Step 4. The tangent point moves to the point along the line segment, and can be expressed as follows:where is set according to the actual situation, while is set as the initial tangent point, and return to Step (1).
Step 5. Determine whether all the jump points in the path are traversed. If yes, the algorithm is finished; otherwise, we return to Step (1). The result of adaptive arc optimization is shown in Figure 9.
4.3. Algorithm Flow
The flow chart of the pathfinding optimization based on our improved A algorithm is shown in Figure 10.
5. Simulation Experiment
5.1. Small- and Medium-Scale Map Simulation
To verify the effectiveness of our improved algorithm, the following experiments were conducted, and the 30 × 30, 50 × 50, and 100 × 100 grid environments of Li et al. [25] were used as test maps for simulation and compared with the traditional ACO, improved ACO [26], traditional A algorithm, secure A algorithm, bidirectional A smoothing algorithm [25], and PRM, and experimental environment such as Windows 10 (64 bit), AMD Ryzen5-4600H, CPU 3Ghz, memory 16 GB, and Matlab 2019a. Simulation experimental results are shown in Table 1 and Figures 11–16.
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
As shown in Figures 11 and 12, the paths planned by traditional ACO and traditional A both collide with obstacles and do not meet the safety requirements for robot motion. Based on the environment modeling approach we proposed, the safe A algorithm, the improved A [25], PRM, and our algorithm can all plan safe paths as shown in Figures 13–16, and the paths obtained by our algorithm outperform the safe A algorithm, the improved A [25], and PRM in terms of length. As shown in Figures 14–16, in terms of smoothness, we combine adaptive circular arc optimization and achieve similar smoothness as Li et al. [25], who use Bézier curves for optimization, but our optimization in terms of distance length is significantly better. Since the paths planned by PRM have some turning points, the smoothness of its planned paths is worse. Besides, the time spent by our algorithm is significantly better than PRM and slightly better than the improved A [25].
(a)
(b)
(c)
From the experimental results in Figure 11, the IACO of Yang et al. [26] also guarantees the path security, but the length of the planned paths, the smoothness, and the running time of that algorithm is not as good as ours in the 30 × 30, 50 × 50, and 100 × 100 environments. In addition, the number of nodes extended in this paper is significantly better than the traditional A algorithm and slightly better than the improvedA [25], so it is also the best among the compared algorithms in terms of computational efficiency and the algorithm search time.
Based on the statistical tests, we draw a conclusion that the comprehensive performance of our algorithm is better than that of the other algorithms in the experiments.
5.2. Large-Scale Marine Environment Simulation
To verify the performance of the improved algorithm in the super-scale map scenario, Figure 17(a) of the area around Zhubu Island (7 km7 km) in Qingdao, Shandong Province, China, and Figure 18(a) of the area around Changhai County (64 km48 km) in Liaoning Province, China, were selected as the sea maps for testing [27]. The satellite map of the area was first acquired, binarized, and then modeled the ultra-large-scale grid map with our method, where the accuracy is 10 m. The two areas’ binarization maps are shown in Figure 17(b) and 18(b). Due to the limited performance of our computer, this experiment again divided three groups of maps: Map_1 with a simple obstacle environment but a more extensive map size of 7 km7 km, and Map_2 and Map_3 with a slightly smaller scale of 5 km5 km. Three environments are in increasing order of complexity, and we compared our algorithm with the A. The experimental results and data are shown in Figures 19 and 20 and Tables 2 and 3, where the yellow grid area in Figure 19 is the expanded node area of A.
(a)
(b)
(a)
(b)
(a)
(b)
(c)
(a)
(b)
(c)
In Map_1, both algorithms can successfully plan paths, and our improved algorithm achieves significant results in this type of large-scale map. The algorithm in this paper is 771.752 m shorter and 1268 times less than the A algorithm in terms of path length and the number of extended nodes, respectively, and the number of nodes visiting the obstacle is also significantly better than A. In Map_2 and Map_3, which have higher complexity, both our algorithm and A perform three sets of path planning. From Tables 2 and 3, we can see that our algorithm still has apparent advantages in path length, the number of extended nodes, and the number of nodes visiting the obstacle. The paths of our algorithm in path_1 of Map_2 are smoother when traversing narrow areas, and the comparative indexes are more prominent. We do not consider the secondary path optimization in the experiments in Figure 20 due to the broader sea area, and the number of path turns is less, but it is still better than A from the results. To reflect the effect of adaptive arc optimization, we optimize path_3 with the path length of 3472.792 m in Map_3 as the target and get a smooth path with the length of 3406.679 m, as shown in Figure 21.
5.3. ROS Cart Experiment
To verify the applicability of the improved algorithm in a natural environment, a two-wheel differential speed mobile robot was composed of a McNamee wheel chassis, an STM32 underlying control board, a Lidar, and equipped with an Ubuntu 16.04 system including ROS-kinetic. The ROS cart experiment was conducted in an environment 1.5 m long and 0.5 m wide, and the experimental conditions are shown in Figure 22.
(a)
(b)
Figure 23 shows our observation of the ROS vehicle in motion, in which the red color is the outline of the obstacle scanned by lidar; the black area is the risk area extension; the white box is the lidar detection range; and the solid green line is the global route planned by our A algorithm. From Figure 23, it can be seen that a collision-free global path is preplanned for the ROS cart by our algorithm, and then, the cart tracks the global path to move. From the experimental data in Figure 24, it can be seen that due to the expansion of the risk region and the optimization strategy of adaptive arc, the position change and speed change amplitude of the trolley in the driving process are relatively gentle, and the ROS cart finally avoids the obstacles safely and smoothly to reach the target position.
(a)
(b)
(a)
(b)
6. Conclusion
To solve the problems of smoothness, safety, and memory occupation of the path planned by the traditional A algorithm, we improve it by combining the jump point search strategy and the adaptive arc optimization strategy. To improve the safety of the planned paths, we expand the risk area of the obstacles. Combining the efficient search method of jump points, we improve the node expansion strategy of A and improve the heuristic function by combining the obstacle density effects of the environment to enhance the execution efficiency further and reduce the computer memory requirement. Considering the actual motion requirements of the mobile robot, the redundant jump point optimization strategy and the adaptive arc optimization strategy are used to shorten the path length and improve the smoothness. Comparative experiments with the traditional A algorithm and other algorithms [25,26] in small- to medium-scale environments validate the effectiveness of our algorithm in optimizing path length, smoothness, and safety. Simulation tests in mega-island environments demonstrate the advantages of our algorithm in terms of path metrics and low memory usage under large-scale maps. Finally, path planning experiments for mobile robot in a natural environment based on the ROS platform further verify that our improved algorithm can design a safe and smooth route, and meet the actual robot motion requirements.
Data Availability
No data were used in this study.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this article.
Authors’ Contributions
Z.Y. and L.Y. conceptualized the data; Z.Y. performed methodology; Z.Y. contributed to software; Z.Y. and L.L. validated the document; H.C. performed resources; L.Y. visualized the study; Z.Y. wrote original draft; and L.L. and L.Y. reviewed and edited the manuscript. All authors have read and agree to the published version of this manuscript.
Acknowledgments
This work was supported by the National Natural Science Foundation of China (61163051).
Supplementary Materials
The following supporting information can be downloaded. (1) We expose the code for the adaptive arc optimization algorithm: https://github.com/akuyh/Adaptive-arc-optimization, Figure 9. (2) The large-scale sea area map we use is from Wang et al. [27], and we disclose the map data Map_1, Map_2, and Map_3 used in this paper for other scholars to study in their research: https://github.com/akuyh/map, Figures 19(a)–19(c). (Supplementary Materials)