An improved A-Star based path planning algorithm for autonomous land vehicles

This article presents a novel path planning algorithm for autonomous land vehicles. There are four main contributions: Firstly, an evaluation standard is introduced to measure the performance of different algorithms and to select appropriate parameters for the proposed algorithm. Secondly, a guideline generated by human or global planning is employed to develop the heuristic function to overcome the shortcoming of traditional A-Star algorithms. Thirdly, for improving the obstacle avoidance performance, key points around the obstacle are employed, which would guide the planning path to avoid the obstacle much earlier than the traditional one. Fourth, a novel variable-step based A-Star algorithm is also introduced to reduce the computing time of the proposed algorithm. Compared with the state-of-the-art techniques, experimental results show that the performance of the proposed algorithm is robust and stable.


Introduction
Autonomous land vehicle (ALV), as a typical robot, is widely researched recently. 1 The development of ALVs is based on core components, such as environment perception, path planning, vehicle control, position localization, and so on. Among these components, path planning is a crucial function to determine the maneuvers of ALVs because it provides the path motion operations of vehicles according to the environment perception information. 2 In the last decades, lots of techniques for motion planning strategies have emerged. These path planning algorithms are generally classified into four classes 3 : graph search algorithms, 4,5 sampling algorithms, 2 interpolating algorithms, 6 and numerical optimization algorithms. 7 Among these presented algorithms, the A-Star algorithm and its various improved algorithms are widely studied and implemented. 4,5,8,9 The idea of the A-Star algorithm is first introduced in the article, 10 derived from the Dijkstra's graph search algorithm, which introduced a heuristic function to enable a fast node search. Several successful applications in real ALVs had been implemented, such as a hybrid A-Star was applied in Junior, 4,5 and some other variations were applied both in AnnieWAY 8 and Boss, 11 all of these vehicles had joined in DARPA challenges.
The advantage of the A-Star algorithm is simple and relatively fast (compared to the Dijkstra algorithm), while its limitation is as follows: the classical A-Star is not suitable when the vehicle turns a corner, as shown in Figure 1. As we know, the heuristic function F ¼ G þ H is applied to accelerate the search process to the target. G means the cost that has finished. H ¼ disðC; T Þ, in which C means the current position, and T means the target position. The function disðA; BÞ means the distance between position A and position B. This classical heuristic function would quickly guide the search toward the target, as shown in Figure 1. Figure 1 shows a scene that the ALV would turn the right corner, the "guideline" is given by the global planning or human. The planning algorithm is expected to generate a result similar to the "ideal path." The real results planned by the classical A-Star algorithm are shown as "A-Star without obstacle" and "A-Star with obstacle," which are generated by the literature. 12 Although it is not A-Star's fault, in many scenes, indeed, the road edge is not easy to be detected by perception. Thus, both two results are unsatisfactory in Figure 1. Therefore, the classical A-Star algorithm is not suitable for application in such scenes.
Another problem with the classical A-Star algorithm is the selection of parameters. Several key parameters should be set before this algorithm is applied, such as the size of q, the length of the search step, and so on. These parameters would greatly influence the performance both on the quality and the time-consuming. For example, different lengths of the search step would bring a different result, as shown in Figure 2. Under the same condition, four different steps (step ¼ 1; 3; 6; 9) are applied by the classical A-Star algorithm offered by the literature, 12 and their results are shown in Figure 2. In Figure 2, the position and direction of the start point and the target point are fixed, and an obstacle is located between the start point and the target point. It is obvious that the parameter is not appropriate in Figure 2(d) since it brought a wrong result.
The computing time cost of the A-Star algorithm dramatically depends on the number of expanding points. 9 The number of expanding points under four different steps is listed in Table 1. It is shown that the larger the step length, the less the amount of expansion nodes.
Through the above analysis, it is difficult to choose a set of suitable parameters, though these parameters are essential for the A-Star algorithm to achieve a better outcome.
This article presents an improved A-Star based path planning algorithm for ALVs. Be aimed at those shortcomings above, several improvements are presented to raise the quality of the A-Star algorithm in this article. Firstly, a universal evaluation standard is introduced, which is employed to measure the performance of different kinds of graph search-based planning algorithms and also is used for selecting appropriate parameters. Then, a guideline-based A-Star algorithm is presented to overcome the problem of the traditional classical A-Star algorithm, in which the guideline is employed during the search process. Further, a key point-based A-Star algorithm is introduced to improve the quality during obstacle avoidance. Besides, a novel variable-step based A-Star algorithm is also introduced to reduce the computing time of the proposed algorithm. Based on these improvements, the experimental results show that the presented algorithm is valid and reliable.
The remaining of this article is organized as follows: the second section discusses some related works on path planning about ALVs, especially these graph search-based algorithms; the third section introduces the proposed evaluation standard for path planning algorithms; the fourth section describes the improved guideline-based A-Star algorithm; the fifth section describes the improved key point-based A-Star algorithm; the sixth section describes the improved variable-step based A-Star algorithm; the seventh section illustrates the results of real road environmental experiments and conclusions are drawn in the eighth section.

Related works
Path planning is a crucial module for ALVs, which greatly decides the safety, comfort, and energy optimization of ALVs. As stated in the "Introduction" section, four typical classes 3 of path planning algorithms have emerged in the last decades.
In the early decades, the computing resource is limited on the robotic platform. Thus, the computational complexity is much considered. The main idea of a sampling-based planner is trying to solve timing constraints. The most commonly used method in robotics is the rapidly exploring random tree (RRT) method. 13 It allows fast planning in semistructured spaces by executing a random search through the navigation area. However, the resulting path is not optimal, which would significantly affect its application on ALVs. Thus, a series of developments are presented, such as RRT*. 14 In the literature, 14 two critical extensions to the traditional RRT are introduced: committed trajectories and branch-and-bound tree adaptation. Those two improvements are thought to enable the algorithm more efficiency of computation time online.
Another idea of solving the planning path problem is to build a vehicle model. In the literature, 15 a multibody modeling technique is used to develop a four-wheeled vehicle model. Two coupled controllers are proposed in that article: The first controller is designed by the Lyapunov control technique, while the second one is designed by an immersion and invariance approach. Both of the controllers aim to ensure a robust tracking of the reference trajectory while taking into account the strong coupling between the lateral and the longitudinal vehicle dynamics.
It is also easy to think that different segment road networks can be represented by interpolating known way-points with straight lines and circular shapes, such as cubic splines, 16 intrinsic splines, 17 quintic Bezier splines, 18 clothoids, 6 and so on.
In ALV applications, the planning problem is also easy to be transformed into a state space to get a path from start point A to target B. Therefore, graph search-based planners are also widely used to solve this planning path problem. 5 For example, Dijkstra's graph search algorithm, A-Star algorithm, and their various improved algorithms are extensively studied and implemented in the ALV field. 4,5,8,9 The article 5 describes a variant of the A-Star algorithm for ALVs and experimentally validated in the 2007 DARPA urban challenge. Their algorithm consists of two phases. In first phase, a hybrid-state A-Star search is used, Table 1. The number of expansion nodes under different parameters.

Parameters
Step Number of expansion nodes 62,866 10,345 8439 3408 in which a 4D search space hx; y; q; ri is represented. In the second phase, the quality of the solution is improved by nonlinear numeric optimization. The article 19 also employed the A-Star algorithm in their application on their embedded device. The motion model is considered as follows: three directions, including forwarding, turning left, and turning right model build beforehand. The most important contribution of this article is that the open-source of their A-Star algorithm is public on. 12 Recently, several new methods are emerged. 20,21 In the literature, 20 a dynamically integrated spatiotemporal-based trajectory planning and control algorithm is presented for ALVs. A two-level dynamically integrated structure is first introduced. It is said that the best trajectory is selected among a group of candidate time-parameterized trajectories in the upper level, while the trajectory controller based on the vehicle dynamics model is employed to control the vehicle to follow the desired trajectory in the lower level. In the literature, 21 a novel idea of planning algorithm by considering inverse vehicle handling dynamics is introduced. In their article, a tyre model and optimal preview time are established. And a path-tracking controller based on linear quadratic regulator method is designed and calculated for the path-tracking problem.
To address the limitation of the classical A-Star algorithm mentioned above, this article also presents an improved A-Star based path planning algorithm. An evaluation standard for evaluating each path planning algorithm is first introduced. Then, a guideline is employed to improve the heuristic function of the proposed algorithm.
A key point method is also employed to improve the ability to avoid collisions. Experimental results show that the proposed algorithms are effective and stable.

Evaluation standard
Path planning algorithm is required as safety, comfort, and energy optimization, but there is a lack of evaluation rule to quantitative analysis of those requirements. To estimate various of path planning algorithms and their results, a novel evaluation standard algorithm is first introduced as follows.
The path planning algorithm offered by the literature 12 is employed to describe the usage of the presented evaluation method. The parameter of the search step is employed to show its influence. As mentioned in the article, 12 three directions, including turn left, turn right, and go straight, are used during the search process. Thus, the mapping relationship between path curve and virtual speed is given in Table 2: suppose the virtual speed from straight road to straight road is set unit 1, the virtual speed from straight road to turn left or turn right is set a, and the virtual speed from turn left to turn left or from turn right to turn right is also set a. The virtual speed from turn left to turn right or from turn right to turn left is set b, where 0 < b < a < 1.
In this estimate process, we set a ¼ 0:5 and b ¼ 0:25, nine different search steps (step ¼ 1 À 9) are employed, and their results are shown in Figure 3. The cost time T and their middle results are listed in Table 3. In Table 3, "Epoints N" means the number of expansion points, and "Ppoints" means the number of path points. It is obvious that under the parameter step ¼ 4, the cost time is the smallest, which means it gets the best performance under the parameter step ¼ 4.
The advantage of the proposed evaluation standard is as follows: first, the cost time T that a vehicle passed the planned path is employed to measure the ability of these planning algorithms, which has a physical meaning and is consistent with the real one. Second, virtual distance and virtual speed are used in the proposed algorithm to overcome the difficulty of getting the vehicle model but keeping the tendency consistently with the real one.

Guideline-based A-Star algorithm
The classical A-Star algorithm is not suitable when directly applied in ALVs, such as when the vehicle turns a corner Algorithm 1. Evaluation standard for path planning algorithms. In these cases, the purposes of the human driver are not well implemented. Thus, a guideline-based A-Star algorithm is presented in this article to overcome those problems. Guideline, which was generated by a global planner or by humans, expressed the driver's intention. Thus, in our guideline-based algorithm, the guideline is employed to develop the heuristic function to express the driver's intention. The improved heuristic function is defined as follows  The heuristic function F is combined by two parts, as the same as in classical one: G means the cost that had finished, and H means an estimated value from the current position to the target. In our algorithm, H is divided into two components: H 1 is the distance between the point i and the guideline, and H 2 means the distance from gðiÞ to the target, where gðiÞ is on the guideline and is decided by point i. gðiÞ is computed as follows: when the point i is fixed, distance between the point i and each point in guideline is computed. Then, the minimum distance and its corresponding point on guideline are found, marked as gðiÞ.
The physical meaning of H 1 and H 2 is shown in Figure 4. a1 and a2 are coefficients.
Several compare experiments are carried out to show the performance of the proposed guideline-based A-Star algorithm. The classical algorithm offered by the article 12 is employed as a comparison, named as "Autoware A-Star." Our algorithm is developed based on the "Autoware A-Star," named as "guideline-based A-Star." Experimental results are shown in Figure 5. Two scenes, a crossroad scene and a curve road scene, are employed to test the quality of these two algorithms. In Figure 5(a), two results, with obstacles and without obstacles, are both listed by the "Autoware A-Star," since maybe roadside would be detected or not. The scene in Figure 5(b) shows a typical curve road, which is very common in both country road and highway. In both these scenes, the results offered by the "Autoware A-Star" are not suitable, even would bring danger to the ALV. Contrary to "Autoware A-Star," the proposed algorithm is well done in these testing scenes.

Key point-based A-Star algorithm
The proposed guideline-based A-Star algorithm would work well on crossroad and curve road as analyzed above, but it would take troubles when the guideline cross an obstacle. The guideline would lead the potential path toward the obstacle according to the improved heuristic function F, which is not only dangerous to the ALV but also contrary to the human driver's intention. Thus, a key point-based idea is presented to develop the guidelinebased algorithm in this section. The main idea is that when  the guideline is on an obstacle, then find a new guideline that not on obstacles. The proposed key point-based A-Star algorithm is described as follows: Step 1: Set the test scene, including the size of search space, guideline, the start point and target point, obstacle map, and so on; Step 2: Check the guideline from the start position to the target, find whether there are obstacles on the guideline; Step 3: If there is no obstacle on the guideline, go to return, otherwise, go to next step; Step 4: Find all candidate key points according to those obstacles crossed by the guideline. Two sides of the obstacle are found as candidate key points. In order to describe this process more clear, an example is shown in Figure 6: there are three obstacles on the guideline, and "P1-P6" are six candidate points; Step 5: Generate "n-order (n ¼ 2)" candidate guidelines as follows: link these candidate points with the start point and the target point to generate new lines, such as the line "S-P6-T" means the line from the start point to key point P6, and finally to the target point. Thus, there are six "2-order" candidate guidelines: "S-P1-T, S-P2-T, S-P3-T, S-P4-T, S-P5-T, S-P6-T." The "n-order" means there are "nÀ1" key points between the start to target. For example, "two-order" candidate guideline means there is one key point, such as the line "S-P1-T," "3-order" candidate guideline means there are two key points, such as the line "S-P5-P1-T," and so on; Step 6: Check whether there are obstacles on these new candidate guidelines, and compute their cost times by the proposed evaluation standard; Step 7: To find a candidate line, first, meet the requirement that there is no obstacle to it, and then the cost time is as short as possible. If there is no candidate line that meets the condition, go to next step; otherwise, this new candidate guideline would instead of the original one, then go to return; In this example shown in Figure 6, only one line "S-P4-T" meets the requirement that there is no obstacle on it. Thus, P4 is the key point, and line "S-P4-T" would become the new guideline.
Step 8: Make n ¼ n þ 1, then generate "n-order" candidate guidelines, and go to step 6; Return: Apply the guideline-based A-Star algorithm and return the result.
Several compared experiments are carried out to show the performance of the proposed key point-based A-Star algorithm. The "Autoware A-Star" algorithm is employed as a comparison again, and the guideline-based A-Star algorithm is employed as a comparison, too. All of the parameters in three algorithms are set as the same, and their search step is fixed as step ¼ 4, which is thought the optimum one. The experimental result is shown in Figure 7, and their quality estimated by the proposed estimate standard is listed in Table 4. In Table 4, "Epoints N" means the number of expansion points, and "Points" means the

Variable-step based A-Star algorithm
The proposed key point-based A-Star algorithm could optimize the planning path for ALVs, while the drawback of high time and space complexity is still existence, which is the main limitation of real application for the A-Star algorithm. Under the same search space, the number of expanding points means the time complexity, which is smaller, the performance is better. To reduce the time complexity, a variable-step based A-Star algorithm, based on the proposed key point one, is presented in this article. In the variable-step based A-Star algorithm, the search step is variable according to the distribution of obstacles. The advantage of this algorithm is that the search step can be big and stride in the open area, and the search step can be smaller, avoiding during near obstacles. The strategy of variable step is introduced in Algorithm 2 as follows.
To rise the computational efficiency, the article 12 offered a calculation template for computing expanding point during the initialization of the algorithm in its open source. This method would greatly reduce these sin and cos computing operation repeatedly. Therefore, a discrete variable step strategy is used in the proposed algorithm. For example, MinStepValue ¼ 1 and MaxStepValue ¼ 9 are set for the biggest and smallest search step, and ReT ¼ roundðdis Â gÞ, where the function roundðaÞ means to take an integer of a. Therefore, ReT 2 ½2; 3; 4; 5; 6; 7; 8. Similar to the article, 12 calculation templates under all of the different variable steps are firstly generated during the initialization stage.
The same compared experiment is also carried out to show the performance of the proposed variable-step based A-Star algorithm. The result is shown in Figure 8, and details are listed in Table 5. In Table 5, "Epoints N" means the number of expansion points, and "Points" means the number of path points. Though the "cost time" of the proposed variable-step based A-Star algorithm is a little less than that of the key point-based A-Star algorithm ð280:6477 : 275:2890Þ, the computational efficiency is greatly increased ð1426 : 11102Þ. Thus, this improvement would be meaningful when the algorithm is applied under a limited computing resource environment or that require real-time performance.

Experimental results and analysis
To evaluate the proposed algorithm, experiments are designed in various kinds of environments. Our experiments are composed of three parts: In part I, an open square with several fixed obstacles is employed to compare the proposed algorithm with the traditional A-Star algorithm. In part II, a real structured city scenario is employed to analyze the effectiveness of the presented algorithm. Sequence performances are also listed to check its ability to avoid obstacles. In part III, the proposed algorithm is widely applied in city scenarios with long distances to test its robustness.

Experimental results in an open square
An open square with several obstacles is designed to verify the performance of the proposed algorithm. The scene is designed as follows (shown in Figure 9): global line is generated in an open square, and then two parts of obstacles are placed on the global line (which is represented by the black triangle). The position of the vehicle is marked as a green circle, and the guideline front of the vehicle is marked as a red circle. The distribution of obstacles of two parts is shown in Figure 9(b) and (c). In Figure 9(b), three groups of obstacles are S bending distributed, which is often used to check the performance of planning algorithms. In Figure 9(c), a group of obstacles is placed at a turn position to check the turning performance of the proposed planning algorithm. Figure 10 shows the results by the proposed algorithm and by the compared one. 12 Their trajectories are listed in Figure 10(a) by red trajectory (generated by the proposed algorithm) and by pink trajectory (generated by the compared algorithm), where the blue trajectory is the guideline. Figure 10

Experimental results in real structured city scenario
Another experiment is carried out in a structured city scenario to test the performance of the proposed algorithm, as shown in Figure 11, which is a part of the street block. Two algorithms, the proposed algorithm and the literature, 12 are employed again. The street scenario is a very common scene, and there are people, cars, trees, and edges beside the road in various forms (it may be not easy to be detected by computers), and the ALV would usually drive among these environments. The results of both algorithms are also shown in Figure 11. Figure 11(a) shows the scene and trajectories of both algorithms, in which the blue trajectory is the guideline, the pink and green trajectories are generated by the compared algorithm, the red and cyan trajectories are generated by our algorithm. Two groups of obstacles are placed in the middle of the road to verify the avoiding ability of the planning algorithm, as shown in Figure 11(b). In this experiment, both algorithms are applied two times, and their results are listed by different colors in Figure 11(a). In these trajectories, the pink one, which is generated by the compared algorithm, is broken off at position P1. It was found that the compared algorithm would take a shortcut from area A1, which is marked in Figure 11(a). Figure 11(c) is the corresponding place of area A1, and Figure 11(e) shows the planning result that generated by the compared algorithm at position P1. It is not the traditional A-Star's fault since the perception not checks out the roadside correctly. But in a real application, these cases are very common that edges beside the road are not correctly detected. Thus, the traditional A-Star algorithm is not suitable under these cases. Under the condition that some obstacle was placed at area A1, the compared algorithm generated a new result, as shown in the green trajectory. From this experiment, we can see that the proposed algorithm in this article would deal with the problem that edges beside the road are not correctly detected, and it is better to show driver's purpose than the compared one (The blue trajectory and the cyan trajectory are much closer than the green one to the guideline). It is also found that the traditional A-Star algorithm has higher requirements for roadside detection than the proposed one.
In this designed scene, two groups of obstacles are specially placed in the middle of the lane, as shown in Figure 11(b). A series of planning results for avoiding these obstacles generated by our algorithm is shown in Figure 12(a) to (j). In Figure 12, the blue trajectory means the guideline, and the green trajectory is the planning result, and the red block means obstacles. Figure 12(a) to (j) shows the process that the vehicle passed obstacles. It is obvious that our algorithm is well done to avoid these obstacles.

Long-distance application in real environments
To verify the stable and reliable of the proposed algorithm, long-distance applications in structure environments are carried out. Lots of open scenes are employed, such as streets, campuses, villages, residential quarters, and so on. Among these scenes, pedestrians and moving vehicles are common, which would test the performance of our algorithm. Two typical results are shown in Figures 13  and 14. Figure 13 is a typical campus, and its total mileage is more than 5 km. In Figure 13, the blue line is the guideline, and the black one means obstacles. The ALV platform drives twice under the proposed algorithm, and their trajectories are shown as a red line and green line. Figure 14 is a typical industrial park, and its total mileage is more than 2 km. In Figure 14, the guideline is also shown as the blue one, and the real trajectory is shown as the red one. From those experiments, it is obvious that our algorithm is well done, stability, and reliability.

Conclusion
This article presents an improved A-Star based path planning algorithm for ALVs. On-road motion planning for ALVs is a challenging problem, and the traditional  A-Star algorithm is insufficient when road edge is not correctly detected, especially in turning a corner. Based on the traditional A-Star algorithm, four improvements are presented in this article. To measure the performance of algorithms and to choose the most appropriate parameter, an evaluation standard is first introduced. Then, a guidelinebased A-Star algorithm is presented, in which the guideline is employed to develop the heuristic function to overcome the shortcoming of the traditional A-Star algorithm. Further, for improving the avoidance performance, the idea of key points besides obstacles is employed, which would guide the planning path to avoid the obstacle much earlier and more effective. In addition, a novel variable-step based A-Star algorithm is presented to reduce the computing time of the proposed algorithm. By combination of these improvements, this improved A-Star based path planning algorithm is well done by lots of experiments. Experimental results show that the proposed algorithm is robust and stable. Compared with the state-of-the-art techniques, the performance is better and more suitable in the real application.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported in part by the National Natural Science Foundation of China under grant nos. 61803380 and 61790565.