3D Path Planning of UAV Based on Improved A * Algorithm

: A 3D path planning method based on A* algorithm is proposed. This method introduces the constraints of UAV and line-of-sight communication with ground stations, and it combines the UAV's maneuverability, mission requirements and terrain information and other constraints together to avoid terrain and threat. The method of extending the child nodes to the 3D detailed position calculation is also provided. The simulation is realized by Unity 3D software, and the simulation results show that the proposed method can be used to plan the 3D path of UAV which meets UAV constraints.


Introduction
The third-rate army follows the war, the second-rate army replies the war, and the first-rate army designs the war [1].In the modern battlefield environment, in order to give full play to the UAV's advantages such as zero casualties, high mobility, low cost and so on, to complete the task of searching, locating, tracking and attacking targets accurately and efficiently, the UAV path planning is essential.The core content of path planning is to select the optimal or feasible path to meet a series of constraints and performance indexes, so that the UAV can arrive at the task area in a safe and timely manner, and as far as possible to maximize the effectiveness to complete the specified tasks [1].
Aiming at the problem of path planning, scholars have put forward many algorithms to solve the problem.The A* algorithm is widely used for its high efficiency, less memory requirement and easy code implementation.In 2000, Szczerba [2] proposed an improved A* algorithm, called Sparse A* Search(SAS), which combines the path constraints to reduce the search space for 2D path planning effectively.In 2002, Li Chunhua, Zheng Changwen [3] extended the path planning to 3D based on SAS, it makes full use of the planning environment of three-dimensional information, and can effectively avoid terrain and threat.In 2003, Zheng Changwen [4] proposed a dynamic sparse A* search (Dynamic Sparse A* Search, DSAS) algorithm.This method can realize the online real-time re-planning of the path in the flight environment with unknown threat.In 2011, Wu Jian and Yu Yuhua [5] proposed a variable step size A* method, but only for two dimensional path planning.In this paper, we add the constraint condition of UAV and ground station line-of-sight communication based on the sparse A* algorithm, and elaborate the calculation of extended child nodes coordinates.The experiment results show that this method can effectively realize 3D path planning.

Direction of Destination Approaching
Restricting the path's predetermined direction to approach the destination position.Assume that the angle between the fixed direction and the north direction is M .

2.5
The Total Length of the Path max L Restricting the length of the path must be less than or equal to a pre-set maximum distance.Considering the maneuvering, speed, wind resistance and other effects, the fuel consumption calculation is complex.This maximum path length constraint can be considered as a simplification of the maximum fuel consumption constraint.

Flight Height Constraint
Restricting the range of UAV flight height.This constrain is defined by balancing two aspects, one of which is the probability decrease of being detected by using terrain mask, and the other one is the avoidance of crash when the flight height is too low.Assuming that the path height is H , the minimum flight height is H , the constraint can be expressed as: min H H t .

Line-of-sight Communication with Ground Station Constraint
Restricting the path to be generated in the effective Fresnel zone of the ground station.This constraint ensures the line-of-sight communication of UAVs with the ground station can complete the assigned task normally.As shown in Fig. 1

Meteorological Conditions Constraints
Restricting the path generation space, so that UAV can avoid rain, snow, fog and other adverse weather conditions, and ensure the safety and communication stability.These constraints are not fixed and may change during the execution of the task.To obtain the required path, these different constraints are combined into the A * algorithm to get a search method that can be used for 3D path planning.The method can effectively reduce the search space and improve the efficiency of the search so that the algorithm can converge to the feasible solution in real time.

Path Planning Based on Sparse A * Search Algorithm
A * algorithm is a heuristic search algorithm which is widely used in artificial intelligence.SAS is an improved A * algorithm and its cost function is: The ( ) g x is the actual cost from the starting node to the current node, in other words it is the distance of the path being through.The ( ) h x means that the estimate cost from the current node to the destination node and the A* algorithm will select child nodes in accordance with the ( ) f x value.

Selection of the Cost Function
The flight status (speed, height, maneuvering, etc.) of UAV, the probability of crash or being detected and other factors are mutual coupled, the following cost function can be used: In the formula

Extension Constrains (Incorporating Constraints Into Child Nodes Extension Methods):
Minimum path length l , maximum course angle D , maximum trim angle E constrains: Assuming that the UAV's position coordinates and velocity direction are known, and ignore other interference, the place which the UAV can fly to in the next moment is on a sphere of a spherical rectangular pyramid which edge-lengths is l , pan range is 2D ,vertical angle is 2E , and this sphere is tangent with the speed direction of UAV.In order to save resources and increase the speed of planning, we divide the spherical rectangular pyramid equally into M layers according to its vertical angle and divide it equally into N regions according to its pan angle.Now the (M+1) (N+1) intersections on the sphere can be used to expand and calculate the cost value as the child nodes.The values of M and N generally take 2 -4.As shown in the Fig. 2.

Fixed Direction of the Destination Entrance Constraint:
The A * algorithm can be extended toward the starting node along the opposite direction of the fixed entry direction from the destination point.

Judgement Constraints:
The total length of the path max L : Judge that whether the current extended child node x is satisfied with formula (3.4 , and whether they are in the smallest Fresnel zone.If so, this child node can be retained, otherwise remove it from the OPEN list. Threat source, meteorological conditions: To simplify the calculation, we consider the range of threat source as a hemisphere, the poor weather conditions as cylinders.Judge whether the distance between the current extended child node and the threat center or the horizontal meteorological center is greater than the safety distance (the threat radius).This child node can be retained if it is satisfied, otherwise remove it from the OPEN list.

The Coordinate Calculation of the Extended Child Node
A main difficulty in the proposed path planning algorithm is to calculate the coordinate of the expanded child nodes, the solution process will be described in detail.

Known Conditions:
The world coordinate system in Unity 3D is the left hand coordinate system, as shown in Fig. 3.As described above, the expansion of child node is a (M+1)×(N+1) dot matrix, and we'll give an example of a 3 × 3 dot matrix for derivation here; The maximum course angle is D and the maximum trim angle is E .

Solving Process:
Set up the UAV body coordinate system: The speed direction is Z-axis positive direction, and the X-axis positive direction is from the left of the wings to the right, perpendicular to the XOZ plane upward is the Yaxis positive direction.Think of UAV as a mass point, and ignore its roll gesture.So the body coordinate system's ZOY plane and the world coordinate system's XOZ plane are perpendicular to each other.
In order to make the derivation process more general and representative, we set the destination node on the ground, instead of being at the height min H from the ground.As shown in Fig. 4    Take the direction of ligature from destination node to the sub-destination node as the UAV body coordinate system's Z-axis positive direction, so we can get nine extended child nodes of the sub-destination node in the body of the body coordinate system as shown in Figure .6.The nine nodes in the figure are located in the same plane which parallel to the XOY plane, and the distance between the two planes is l , the point 5 is on the Z-axis., , g g g ª º ¬ ¼ .Therefore, the conversion matrix can be assumed as gb C , and we can get: , , , , Assume i r to be ig r when it is in the world coordinate system and ib r when it is in the body coordinate system.The body coordinate system has the same origin point with the world coordinate system.
Now, the body coordinate system is parallel to the world coordinate system after twice rotations.In formula (3.14), ig r is the vector of the expanded child nodes which expressed in the new body coordinate system.In formula (3.15), ig P is the vector of the extended child nodes which expressed in the world coordinate system.The vector of the body system's origin point in the world coordinate system is g R .

Algorithm Description
The following steps show the 3D path planning algorithm's specific process: Step 1: After the calculation, we put the subdestination node into the OPEN list, and empty the CLOSED.
Step 2: If the OPEN list is empty, the path search fails.And we need to increase the value of M, N appropriately to re-planning.
Step 3: Remove the least costly node from the OPEN list, take it as the current node and place it into the CLOSED list.
Step 4: If the distance between the current node and the starting point is less than l , the parent node pointer of the starting node points to the current node, the path search ends.Along the direction of the pointer, from the starting node back to the destination node, we can get the minimum cost path.
Step 5: Extending the child nodes of the current node.Construct the UAV body coordinate system by using the current node and its parent node, Z-axis positive direction is the direction of the extension.Take the Zaxis as the center benchmark, and make a quadrangular pyramid of which the horizontal section angle is 2 times of the maximum course angle, the vertical section angle is 2 times of the maximum pitch angle and the height is l .Divide the bottom of quadrangular pyramid into M regions in the vertical direction, and N regions in the horizontal direction equally (M, N generally takes 2 to 4), we can obtain (M + 1) (N + 1) child nodes.Then perform length correction to make the distance between the child nodes and the origin of the coordinate system equal to l .So we can get the world coordinates of the extended child nodes.
Calculate the substitution value of the remaining extended child nodes according to formula (3.3) and place the child nodes, according to the value from small to large, into the OPEN list.
Judge that whether each extended child node satisfies the total length constraint of the path, and then calculate the four quarter points coordinate information of the ligature between the parent node and each extended Step 6: Return to step 3.
Along the direction of the pointer, from the starting node back to the destination node, we can get the minimum cost path.

Simulation and Implementation
For now, the mainstream of the path planning simulation research is carried out in the MATLAB software.
Because of the limitation of the software performance, the planning space is usually represented as a grid in the planning process, the terrain and threat data are discretized in advance.These make the path planning extended to 3D space and the computation process complicated.In this paper, we will explore the path planning based on the Unity 3D software and the existing artificial intelligence.We simplify the terrain greatly, especially the complexity of the real terrain by making full use of its rich plug-in resources.We also make the interface much friendlier, and improve the visibility and operability a lot.The simulation is carried out by C#, which is based on the real terrain of 2.6 * 2.4 km in a certain area of Taiwan province and the threat models which are generated by simulation.In the simulation experiment, the parameters are: 1  The experimental results show that the proposed method based on Unity 3D simulation is a fast and effective method.Under the premise of satisfying many constraints, it has obvious improvement effect on the interface friendliness, visibility and operability.

Concluding
In recent years, with the development of artificial intelligence, Unity 3D becomes a very popular game development engine, which plays an important role in the exploitation of AI.Because of the gradual enrichment of artificial intelligence resources and the gradual maturity of the algorithm, certain tasks in the military field which require preciseness, efficiency and high feasibility, for example, UAV path planning, helicopter rescue etc. can be simulated and developed by using the Unity 3D totally.
(3.2), C is the Path planning cost function, n is the total number of path segments, i l means the length of path segment No.i, i T means the threat index of the path segment No.i,1 2 3  , , Z Z Z are the three kinds weight coefficient of cost value.In this article, the child nodes are expanded by a fixed step length (minimum path length).The minimum threat threshold is set, and the threat index which is under the threshold can be ignored.The safe distance can be calculated by these.Without taking the breakthrough defense into consideration, the length i l in formula(3.3)can be replaced by l , ( ) D x means the real distance that x walked through from the start node to the current node, and the threat index i T equals to 0. So the cost function can be simplified to:

Figure 3 .
Figure 3. World coordinate system in Unity 3DThe world coordinates of the destination node is 0 0 0 ( , , )x y z .The angle between the fixed destination and the north direction is M , and the positive direction is from the Z-axis to the X-axis direction; Minimum step size is l ; Minimum flight height is min H ; and Fig.5, expand the child nodes along the opposite direction of the fixed entrance direction from the destination node toward the starting node, and then we can get the inclination T between the initial path and ground, and the second point which called

Figure 4 .
Figure 4. Determination of sub-destination node

Figure 5 .
Figure 5. Inclination θ between the initial path and ground

Figure 6 .
Figure 6.Mode of child node extension Assume that the unit vector matrix of the UAV body coordinate system is 1 2 3 , , b b b ª º ¬¼ , so the vectors of the nine expanded child nodes in the body coordinate system are:

Figure 7 .Figure 8 .
Figure 7. Rotate around the Y-axis Secondly, as shown in Fig. 8, make the body coordinate system rotate T degrees around the X-axis, and we can get the second rotated matrix ' b b C : determine whether the flight height constraint and the Line-of-sight communication constraint are satisfied.If all these constraints are met, put the child node's parent node pointer towards the current node; otherwise remove it from the OPEN list.
Fig.9shows the results of the method.The left group of pictures is the result of no weather conditions.The right group of pictures is based on the experimental results of the left and the weather condition constraint is added.The red hemispheres are threat areas; the gray hemispheres are the line-of-sight communication coverage areas of the ground station; the yellow cylinders are adverse weather areas; the green line is the planned path line.The bottom left is the starting point and the right upper is the destination point.The Fig.9A1and Fig.9A2show the path display in top view.The Fig.9B1and Fig.9B2show the path display in side view.The Fig.9C1and Fig.9C2show the path display in three-dimension view.

d dt dr ht hr=Hmin h0 The minimum Fresnel zone Figure
1. Minimum Fresnel zone radiuses SL x means the linear distance from node x to the destination node.Retain this child node if it meets the condition in formula(3.4);otherwiseremove it from the OPEN list.Flight height, Line-of-sight communication with ground station: Judge that whether the current child node, the 1/4, 1/2, 3/4 path points from the parent node to the child node, of which the heights H can be satisfied with ). max ( ) ( ) D x SL x L d (3.4) ( )D x means the real distance that x walked through from the start node to the current node, and( )