Bi-Directional Adaptive A ∗ Algorithm Toward Optimal Path Planning for Large-Scale UAV Under Multi-Constraints

,


I. INTRODUCTION
Large-scale Unmanned Aerial Vehicle (UAV) [1] has good performance in terms of mobility and maneuverability under the complex environment. As the development of the smart devices and the algorithm, large-scale UAV is widely used in various fields, such as search and rescue [2], coastal monitoring [3], surveillance system [4], and so on. As the key technology of the large-scale UAV, path planning algorithm has recently gained great attention in the study of aerospace control community. At present, the primary algorithms of the path planning can be divided into two fields: traditional path planning algorithm and swarm intelligence optimization algorithm [5]. Artificial potential field algorithm [6] and simulated annealing [7] belong to the former, which are simplicity of operator and easy to use. However above algorithms have the problem of local minima. With the development of swarm intelligence technology, population-based evolutionary algorithms have made great progress in recent The associate editor coordinating the review of this manuscript and approving it for publication was P. K. Gupta. years [8], [9]. Common swarm intelligence optimization algorithms include Ant Colony Optimization (ACO) [10], Particle Swarm Optimization (PSO) [11], and Genetic algorithm(GA) [12]. Swarm intelligence optimization algorithms can efficiently solve the path planning problem with good robustness. However, according to the evolution principle of swarm intelligence optimization algorithms, algorithms with more uniform initial swarm distribution are easier to obtain global optimal solution [13], which means that the optimization effect of those algorithms relies on the initial distribution of the swarm. In order to avoid the local minima and reduce the dependence of the initial swarm, the samplingbased algorithms (e.g., rapidly-exploring random tree star (RRT * ) [14], A * algorithm [15]) are introduced to provide a better solution for UAV path planning problem.
Among them, the path generated by A * algorithm based on graph search can converge to the optimal solution. However, the path quality generated by A * algorithm is to a great extent depends on the number of candidate nodes in the expansion process, thus, A * algorithm converges to optimal solution very slowly due to the inefficient node expansion process.
Based on the conventional A * algorithm, a lot of works have been done by the researchers. Aimed at the dynamic obstacles, constrained A * algorithm [16] generated the safer path by creating a circular boundary for ship as a safety distance. However, constrained A * algorithm does not take into account the smoothness of the path, which means the path generated by them cannot meet the curvature constraint of ship. Considering the issue of path smooth, finite angle A * algorithm [17] explored more candidate nodes in node expansion process, which provided more abundant turning points for unmanned surface vehicles. In order to obtain a shorter and smoother path, the pre-processing stage and the post-processing stage were introduced into work [18] to reduce the redundant nodes. Meanwhile, the cost of the path generated by work [18] was optimized by reducing the local path cost. Smoothed A * algorithm [19] generated the path without 'jags' and redundant waypoints by adopting cubic spline interpolation and three path smoothers. Despite, the node expansion efficiency of the above-mentioned algorithms is low due to the node expansion process of them are similar with the conventional A * algorithm. Furthermore, additional smoothing optimization process leads to longer running time.
In order to improve the efficiency of node expansion and the convergence speed of the algorithm, researchers altered the node expansion process. By introducing difference engine into the node expansion process, work [20] proposed an improved differential A * algorithm for manipulator. Compared with conventional A * algorithm the reconfigurability and real-time capability of differential A * algorithm was superior. Word [21] proposed a multi-direction A * algorithm for ship which could generate the path similar with the real cases. Experimental result showed that multi-direction A * algorithm could enhance path safety to a large degree and improve efficiency in node expansion process by adding the penalty model into the conventional A * algorithm. Similarly, using a modified node evaluation function, the path created by light-assisted A * algorithm [22] could bypass the obstacles towards the goal in complex-shaped obstacles environment. Due to the efficient nodes expansion process, the convergence rate of light-assisted A * algorithm was improved. However above algorithms are barely considering the physical constraint of the machine which is not suitable for the actual motion.
Aimed at the problem of large-scale UAV path planning in complex environment, Bi-directional adaptive A * algorithm is proposed in this paper. In order to solve the defect of the inefficient node expansion for A * algorithm, directional search strategy is introduced into the proposed algorithm to generate the path which meet the constraint of the UAV attitude angle. Adaptive step strategy adjusts dynamically the step of A * algorithm by estimating the environment around the current node to improve the convergence rate. According to the flight demands of the UAV [23], the evaluation function is modified to generate the path which is far from obstacles and has the safe altitude. Adaptive weight strategy is used to balance the convergence rate and the path quality during the planning. Furthermore, the rewiring process is adopted to reduce the redundant nodes and the path cost. Comprehensively, comparing with the work [19] Bi-directional adaptive A * algorithm can converge to the optimal path faster.
Nomenclature P(n) = (x n , y n , z n ) Waypoint coordinate θ, φ Pitch angle, yaw angle φ max Maximum yaw angle θ max Maximum pitch angle T r = (x r 0 , y r 0 , z r 0 , R r ) The rth radar. f (P(n)) Total cost g(P(n)), h(P(n)) A * algorithm parameters P init , P goal Initial node and goal node D eff effective radius of the obstacle L, L max , L min Step of the algorithm The path generated by algorithm S Space environment S obs Obstacle area S free Obstacle-free area The main contributions of the paper are as follows: Firstly, several improvements for A * algorithm, namely directional search strategy, adaptive step strategy, adaptive weight strategy, bi-directional search strategy, rewiring process, are proposed and analyzed in this paper. Secondly, compared with conventional A * algorithm and Smoothed A * algorithm, Bi-directional adaptive A * algorithm has the better performance in convergence speed and solution optimality. Thirdly, Bi-directional adaptive A * algorithm can plan a collisionfree path, which meets UAV attitude angle and altitude constraints.
This paper is divided into following sections: Section II introduces the UAV physical constraint, and environmental representation. In Section III, the improvements and the procedure of Bi-directional adaptive A * algorithm are given. Section IV follows up with theoretical analysis of our proposed algorithm. Simulation results and comparison under different scenarios are introduced in Section V. Finally, conclusions are drawn in Section VI.

II. PROBLEM DEFINITION A. PHYSICAL CONSTRAINTS
In order to obtain a feasible path, the following physical constraints need to be satisfied when UAV is in operation.

1) THE CONSTRAIN OF MAXIMUM YAW ANGLE
The maximum yaw angle is used to limit the turning angle of the UAV. Let P(n) = (x n , y n , z n ) denote waypoint coordinate. Let p n = (x n − x n−1 , y n − y n−1 , z n − z n−1 ) denote the path vector between waypoints P(n) = (x n , y n , z n ) and P(n − 1) = (x n−1 , y n−1 , z n−1 ). Maximum yaw angle is represented by φ max . Yaw angle of UAV should meet the following condition,

2) THE CONSTRAIN OF MAXIMUM PITCH ANGLE
The maximum pitch angle is used to restrict the climb/dive angle of the UAV. Let θ max denote maximum pitch angle. Pitch angle of the UAV should meet the following condition, In military applications, UAV not only needs to avoid the threat of the mountain, but also needs to stay away from radar, thus radar threat as obstacle is introduced into this paper. Let T r = (x r 0 , y r 0 , z r 0 , R r ) denote the rth radar. As an omnidirectional detection device, the mathematical model of radar is simplified to the follow, where (x r 0 , y r 0 , z r 0 ) is the center location coordinate of the rth radar, R r is the effective radius of the rth radar.
It is necessary for UAV path planning algorithm to consider the effects of physical constraint and the threat to improve stability and safety of the UAV, thus distance cost, obstacle threat, altitude, and so on, are integrated into Bi-directional adaptive A * algorithm.

III. BI-DIRECTIONAL ADAPTIVE
As a heuristic method, A * algorithm based on the evaluation function seeks the shortest path consisted by a finite set of waypoints P init , P (1) , · · · , P goal between the initial node P init and the goal node P goal . During each iteration, A * algorithm calculates total cost f (P(n)) for each node P(n), where f (P(n)) is calculated by the follow equation: where g(P(n)) denote the distance cost from the current node P(n) to the initial node P init , h(P(n)) is the estimated cost of the path from the current node P(n) to the goal node P goal . In order to obtain the shortest path between the initial node and the goal node, A * algorithm seeks the node owning the minimum f (P(n)) value at each iteration process. And that process will continue till the algorithm returns an available path.

B. DIRECTIONAL SEARCH STRATEGYA
Based on the difference of candidate nodes number in the exploration process, conventional A * algorithm based on the grid method can be divided into 4-direction A * algorithm, 8direction A * algorithm as follow in Fig.1. As the number of candidate nodes increases, the performance of multi-direction A * algorithm can be improved in the yaw angle. However, due to more candidate nodes are available, multi-direction A * algorithm will inevitably increase computation and sacrifice memory.
Multi-direction A * algorithm is advantageous to improve the exploration ability; however, the invalid candidate nodes would increase the computational time. Therefore, a more efficient extension method needs to be introduced to reduce the computation. Combining the direction vector from the current node to the goal node in obstacle-free space with constrain of pitch angle θ and yaw angle φ of the UAV, the candidate nodes of the current node can be confirmed by directional search strategy.
It can be seen from Fig.2, the candidate nodes obtained by directional search strategy are distributed on the arc surface which satisfy the constrain of UAV pitch angle θ and yaw angle φ. Directional search strategy not only guarantees the exploration ability of the proposed algorithm, but improves the efficiency of seeking candidate nodes. Due to the constrains of UAV pitch angle θ and yaw angle φ are considered in directional search strategy, the extra smoothing process is not necessary for the path generated by it.

C. ADAPTIVE STEP STRATEGY
Conventional A * algorithm connects the current node P(n) and the child node P child (n) with fixed short step, which could delay the convergence of it in obstacle-free. In order to overcome this defect, an adaptive step strategy is proposed in this paper to adaptively adjust step while the current node is under the different environment, such as obstruction area and safe area.
The calculation equation of adaptive step strategy is shown below: where L is the step, L max and L min represent the maximum step and the minimum step, respectively, let D denote the VOLUME 8, 2020 distance between the current node to its nearest obstacle. D eff is the effective radius of the obstruction. As shown in Fig.3, when the UAV is far from the obstacle, the UAV is considered as in the safe area (D ≥ D eff ). In this case, the large step is allowed to accelerate convergence. As shown in Fig.4, if the UAV is estimated in the area of obstruction (D < D eff ), the small step size is used to guarantee the granularity of the path.

D. EVALUATION FUNCTION
Considering the special constraint of the UAV, such as distance cost, collision-free path and safe flight altitude, g(P(n)) is modified to the following: g(P(n)) = ω 1 g obs (P(n)) + ω 2 g alt (P(n)) + ω 3 g cost (P(n)) where g obs (P(n)) represent the cost from the current position P(n) to its nearest obstacle, g alt (P(n)) is the flight altitude cost from the current position P(n) to the reference altitude, g cost (P(n)) denote the Euclidean distance cost from the current position P(n) to the initial node P init . ω 1 , ω 2 , and ω 3 are the weight of each parameter, respectively. Different from the conventional A * algorithm, heuristic function h(P(n)) is calculated in terms of Manhattan distance [24] instead of Euclidean metric in this paper, due to the optimization effect is different when the adopted geometric measurement of heuristic function h(P(n)) is different. It can be seen from Fig.5(a), heuristic function h(P(n)) based on Euclidean distance will explore more space, which sacrifices the memory. Meanwhile the expensive square root in Euclidean metric increases the computation. On the contrary, heuristic function h(P(n)) based on Manhattan distance conducts more accurate space exploration with less redundant nodes. Manhattan distance adopted by this paper guarantees the efficiency of our proposed algorithm and the path quality.
h (P (n)) = P x (n) − P goal_x + P y (n) − P goal_y where (P x (n) , P y (n) , P z (n)) denote the coordinate of node P(n), (P goal_x , P goal_y , P goal_z ) is the coordinate of goal node P goal .

E. ADAPTIVE WEIGHT STRATEGY
When the weight of h(P(n)) is smaller, A * algorithm approaches to Dijkstra algorithm, which seeks the shortest path from the initial node P init to the goal node P goal by conduct abundant space exploration with slower convergence speed. When the weight of g(P(n)) is smaller, A * algorithm translates into Best-First-Search algorithm (BFS), finding a path quickly, however this path is not the optimal for UAV. Thus, A * algorithm based on the Dijkstra algorithm and the BFS algorithm can balance the rate of convergence and the quality of the path by adjusting the weight of g(P(n)) and h(P(n)) dynamically. The evaluation function of our proposed algorithm is calculated by the following: f (P(n)) = ω g g(P(n)) + ω h h(P(n)) (8) where ω g and ω h are the weight factor of g(P(n)) and h(P(n)), respectively. The calculation equations for ω g and ω h are as follow: where ω max and ω min are respectively the maximum and minimum preset value for ω h . It can be seen from Equ. (9), the value of ω h will be set as the maximum value to accelerate the rate of convergence in the obstacle-free area (D ≥ D eff ). Meanwhile, according to Equ. (10), ω g will be set as the minimum value to avoid the useless exploration. When UAV drive into the obstacles area (D < D eff ), accurate space exploration is needed to generate a high-quality path. Therefore, the value of ω g is adaptively increased to find an optimal path in obstacles area. 85434 VOLUME 8, 2020

F. BI-DIRECTIONAL SEARCH STRATEGY
In order to improve the exploration ability of the A * algorithm, bi-directional search strategy is introduced into our proposed algorithm which performs both forward and reverse searches at the same time. When the forward search path A * a rooted at the initial node P init and the reverse search path A * b initialized at the goal node P goal explore the same child node, algorithm returns a feasible path.

G. REWIRING PROCESS
The path generated by the above path exploration strategy consists of a number of short line segments linking a mass of path nodes, might including redundant nodes. In order to reduce the path length and the useless turning nodes, the rewiring process is introduced in this paper to delete the redundant nodes.
For each optimization process, rewiring process checks all path nodes successively in a groups of three continuous path nodes and above three continuous nodes are named as follows, (1) let node A denote the current checking node; (2) let node B denote the child node of the current checking node A in path set; (3) let node C denote the child node of the node B. If nodes A, B, and C satisfy Equ. (11) and Equ. (12), the original path A − B − C will turns into A − C, and the redundant node B will be removed. Fig.6 illustrates the principle of the rewiring process.
where S obs denote the obstacle area, • is the Euclidean norm, C-A represent the Euclidean distance between C to A. C-B and B-A are the same definition as above. Fig.6 illustrate how this method works, where A − B − C − D is the path to be optimized, A − C − D is the optimized path. Clearly, invalid turning node B increases path length and the instability of the UAV, thus, rewiring process is proposed to optimize the path in distance cost and the angular variation of the UAV.

H. ALGORITHM PROCESS
The detailed steps of Bi-directional adaptive A * algorithm are given in this section.
Step 1: Initialize three-dimensional solution space. Construct radars by Equ.  Step 2: The node in the open list with the minimum value f (·) is marked as the current node by the calculation of Equ. (8). If current nodes of A * a and A * b are accessible for each other, then return a feasible path and go to Step 6.
Step 3: Remove current node from open list and add it to close list. Calculate successor node by the directional search strategy and adaptive step strategy.
Step 4: If successor node is absent in the open list, add it into the open list. Set the current node as its father node and calculate {g (P(n)) , h (P(n)) , f (P(n))} by Eqs. (6)∼(8), respectively. Return to Step 2.
Step 5: If successor node is present in the open list and the cost is less than the current node cost, update cost of successor node. Return to Step 2.
Step 6: The optimal path is returned by the rewiring process.

A. PROBABILISTIC COMPLETENESS
Let D a k (p) denote the random variable of the minimum distance from node p ∈ S free to the latest node of the first path A * a , VOLUME 8, 2020 where A * a is defined as above. d a k (p) is the value of the random variable D a k (p), where k is the number of iterations, S free is the obstacle-free space. Similarly, let D b k (p) denote the random variable of the minimum distance from node p ∈ S free to the latest node of the second path A * b and A * b is defined as above.
where p a and p b is the latest nodes of path A * a and path A * b , respectively. d k (p a , p b ) is the value of the random variable D k (p a , p b ).
Theorem: Bi-directional adaptive A * algorithm is a probabilistically complete algorithm. For any space S, when the number of iterations approach infinity, the distance between p a and p b is less than a constant ε. Thus, the probability of searching a path goes to one.
Proof: Let O k a (p a ) denote the closed ball of radius ε > 0 centered at p a ∈ A * a in kth iteration. Similarly, O k b (p b ) represent the closed ball of radius ε > 0 centered at LetȮ k a (p a ) denote the intersection of the closed ball O k a (p a ) and the obstacle-free space S free .Ȯ k b (p b ) is defined as same as theȮ k a (p a ).
For the kth iteration, it is assumed that the latest node of the path A * a is outside the ballȮ Similarly, let us assume that the second path A * b and the ball O k a (p a ) have no intersection, For the next iteration, due to the node exploration process adopts the directional search strategy, path A * a is expanded toward the direction of the latest node of the path A * b . The mean of D a k+1 (p b ) is given by following, Similarly, the directional search strategy act on the second path A * b . The mean of D b k+1 (p a ) is expressed below, One can see from Equ. (20) and Equ. (21) that, for the k + 1th iteration, the distance between the path A * a and the path Thus, as the number of iterations k goes to infinity, the probability of searching a path goes to one.

V. SIMULATION RESULTS
In order to evaluate the performance of our proposed algorithm, several comparisons are carried out among Bi-directional adaptive A * algorithm (BAA * ), Smoothed A * algorithm (SA * ), and Conventional A * algorithm (CA * ) in this section. Three different maps in Fig.7 are used to test the performance of BAA * , where radars are added as the threaten area in the maps. The characteristic parameters of BAA * are shown in Table 2. The parameters of the threaten area are given in Table 3.      The simulation results of above algorithm running in three different maps are shown in Fig.8-11, where the simulation results of BAA * are shown in different visual angles. The data of simulation results are illustrated in Table 4. Furthermore, the data of attitude angle for these different A * algorithm are given and analyzed below, meanwhile the altitude comparison of them are also carried out. The related results of attitude angle and altitude are shown in Tables 5 and Fig.12-16.

A. OPTIMALITY COMPARISON
It can be seen from Table 4, the algorithm optimality includes the path cost, computational time and the number of the search grids, where the distance cost represents the basic performance of the algorithm, computational time and the number of the search grids represent the computing power of the algorithm.
The simulation results of CA * in different environments are shown in Fig.8. Based on the 8-direction grid search strategy, CA * explores a mass of candidate nodes (Nodes = 44, 40, 42), which sacrifices the memory and decreases the convergence speed (Time = 50.308, 50.854, 53.541). Meanwhile, the path generated by CA * climbs over the obstacles to avoid the collision. Due to the climb process of the UAV, the path cost can be increased (Path Cost = 51.455, 59.094, 55.351) which lead to the increase in fuel consumption of UAV.
As seen from Fig.9, comparing with the CA * , SA * can generate smoother path with less jags which improve the VOLUME 8, 2020

B. ATTITUDE ANGLE COMPARISON
As the change of attitude angle is closely related to the UAV stability and is a main index of path quality for UAV, attitude angle comparisons among different A * algorithm are given and analyzed with the combination of Fig.12-14 and tables 5.
It can be seen from Fig.12-13, since only the basic requirements of UAV path planning, such as obstacle avoidance and distance cost, are considered, the yaw angle of CA * inevitably exceeds the constraint when UAV avoids obstacles. The larger attitude angle change leads to the instability and insecurity of UAV. Comparing with CA * , the attitude angle of the path created by SA * is optimized by introduced three individual path smoothers. But there are some unbearable jags in the path while the UAV passes by the threat.
Thanks to the introduction of attitude angle constraint during the exploration process, the path generated by BAA * satisfies the angle constraint of UAV, which guarantees the stability during flight. Let γ ya and γ pa represent the sum of UAV yaw and pitch angle change, respectively. γ ya and γ pa are calculated by the follow, where V denote the number of attitude angle change, γ y_a and γ p_a denote the set of yaw and pitch angle, respectively. It can be seen from Table 5 and Fig.14, compared with CA * , the sum changes of yaw angle are reduced by 72.210%, 73.187%, and 48.300% while BAA * is run in map1, map2, and map3, respectively. For the pitch angle, compared with CA * , the path generated by BAA * in three different maps reduce it by 66.558%, 70.301%, and 48.109%, respectively.

C. ALTITUDE COMPARISON
It can be seen from Fig.15, due to altitude constraint is not introduced in CA * , the climbing process is used to avoid obstacles. The maximum altitude of CA * in map1, map2, and map3 reach to 4.1km, 3.1km, and 3.1km, respectively. Further, flight altitude standard deviations (SD) is shown in Fig.16, which reflects the stationarity of the UAV during the flying process. The flight altitude standard deviations of CA * is 1.076km, 1.048km, and 0.873km in three different maps respectively, which means the paths generated by CA * are unstable. Meanwhile, too much energy is wasted during the climbing process. Similarly, for SA * , the maximum altitude in map1, map2, and map3 is 4.1km, 3.1km, and 3.1km, respectively. However, due to the cubic spline interpolation function is introduces into SA * , the standard deviations of flight altitude are 0.994km, 0.747km, and 0.779km, which means the stability and smoothness of the path. However, similar to the conventional A * , the node growth process of SA * also ignores the altitude limitation of UAV, resulting in the path generated by SA * has the climbing process to avoid the obstacles.
By introducing the information of the obstacles and the altitude limit into the evaluation function, the path generated by BAA * can bypass the obstacles without the climbing process. As shown in Fig.15, the maximum altitude of the path in map1, map2, and map3 is 1.661km, 2.357km, 1.807km, respectively. And the standard deviations of flight altitude are 0.698km, 0.709km, 0.530km, which guarantees the safety and the stability of the UAV.

VI. CONCLUSION
A novel path planning algorithm based on A * algorithm underling multi-constraints is proposed in this paper namely Bi-directional adaptive A * algorithm. The directional search strategy, adaptive step strategy, modified evaluation function, adaptive weight strategy, bi-directional search strategy, and rewiring process are respectively introduced and analyzed in this paper. The main findings are summarized as follows: (1) Due to the adoption of the directional search strategy based on the attitude angle, the proposed algorithm generates the path meeting the constraint condition of large-scale UAV and increases the rate of convergence comparing with other A * algorithm. (2) Adaptive step strategy and the adaptive weight strategy are used to balance exploration speed and the path planning quality by analyzing the environmental conditions of the current node. (3) The altitude limitation and the obstacles information are introduced into the evaluation function to create the safety path. (4) Rewiring process is used to reduce the redundant nodes which decreases the path cost further. In order to verify the feasibility of Bi-directional adaptive A * algorithm, the proof of probabilistic completeness is given in section IV. The simulation comparison results given in section V indicate that comparing with conventional A * algorithm and smoothed A * algorithm [19], Bi-directional adaptive A * algorithm has superiorities for path planning problem of large-scale UAV in term of the path quality and the rate of convergence.