UAVs Path Planning based on Combination of Rapidly Exploring Random Tree and Rauch-Tung-Striebel Filter

Aiming at the problem of Unmanned Aerial Vehicle(UAV) formation path planning under complex constraints, a UAV formation path planning method based on the combination of Rapidly exploring Random Tree (RRT) and Rauch-Tung-Striebel (RTS) filter is proposed. Firstly, a path planning algorithm based on the improved RRT algorithm with adaptive step size is de-signed to solve the problem that the RRT algorithm is easy to fall into local optimum. Then, an RTS filter is introduced to smooth the trajectory planned by the improved RRT algorithm to achieve curvature continuity. Finally, taking the smooth trajectory as the reference, a UAV formation path planning algorithm over the Artificial Potential Field (APF) method is designed. The simulation results show that the designed UAV formation path planning algorithm can solve the planning problems of single trajectory and formation trajectories in complex constrained space, and can plan the formation trajectory with continuous curvature, to facilitate the UAV trajectory tracking control.


Introduction
UAVs have been extensively employed in the field of disaster rescues, such as communication support, evaluation with remote images, and small supplies delivery [1].UAVs are adopted to fulfill these tasks because of their low requirements on takeoff and landing conditions, long hover duration, and low R&D and maintenance costs.UAV smarms that can expand the capability boundary and service radius of a single UAV have become an essential field for the development and application of UAVs [2].Path planning and flight safety of swarms problem will be a multi-order NP-hard problem especially when considering constraints including flight time, UAV formation, safety, terrain, and fuel consumption [3].
There have been some researches on the path planning of UAV swarms based on certain planning methods.For example, in [4], the authors designed a multi-objective path planning method using the graph theory search algorithm.In [5], the authors reduced the re-planning by studying the shortest path problem on the dynamic road network.These path planning methods can only be executed over a graph that the destinations and feasible paths are given.Therefore, it is not practical for objects with random destinations and flexible flight areas, like UAVs.In view of the above shortcomings of the graph method, considering the characteristics of path planning in the perspective of large-scale planning, there are many kinds of research on path planning using bionics and evolutionary theory.In [6], a multi-objective optimal path is planned over Gray Wolf Optimizer (GWO).In addition, Xiande Wu et al. proposed a multi-objective UAV path planning method by hybrid of particle swarm optimization method and RTS method [7].According to a summary of these studies, it is often better to plan flight trajectories through hybrid algorithms and take advantage of the strengths of each [8].
The Rapidly Exploring Random Tree (RRT) algorithm is able to dynamically generate random positions in the planning horizons of UAVs, which will form several connected tree structures until reaching the target position [9].However, it is impossible for UAVs to follow the path generated by the RRT algorithm because it is a tree-like path that is composed of a large number of zig-zag line segments.To solve this problem, in [10], a UAV path planning method over mixed population RRT algorithm is proposed, the authors smoothed the planned trajectory to meet the UAV dynamic constraints.The result shows that after smoothing the path nodes generated by the RRT algorithm, the trajectory curvature is more continuous, which is more convenient for UAVs' tracking control.
In a review of current research on UAV path planning, we propose a planning method that combines the RRT algorithm with adaptive steps and the Rauch-Tung-Striebel (RTS) filter, which can innovatively solve the UAV formation path planning problem under complex constraints.The remainder of this paper is organized as follows.In Section 2, the UAV path planning problem models are established.Section 3 depicts the detailed design of the planning algorithm based on the Leader-Follower UAV formation that we use in this study.We will analyze the simulation results in Section 4 and summarize the conclusions in Section 5.

UAV trajectory model
The takeoff process of UAVs is not considered in this paper for it always is under control by the ground operators and is highly affected by air route, convective cloud, and some other factors, before reaching the desired flight altitude.The starting point in trajectory planning model studied is set as at ( 0 ,  0 ,  0 ) that is a certain point on the cruising altitude, and ending at (  ,   ,   ).The reference coordinate system is  − , as shown in Figure 1.
The trajectory model of UAVs can be defined as: =  −1 +     (2) where  = 1,2, … , ,  is the count of trajectory nodes, and  0 ,  0 ,  0 are the starting position of the trajectory.Where   ,   ,   indicate the position of the  th search,   =   ,   =   ,   < Δ, and   is the step-length in  th search, Δ indicates the acceptable error of cruising altitude.In the following contents, UAV cruising altitude changes won't be considered, namely,   = 0.

UAV flight constraint model
1) Flight boundary constraint: in the planning process, the upper boundary of UAVs' flight area on the X and Y axis is (  ,   ).
2) Flight range constraint: set the maximum flying range of UAVs is   , so that the sum of all step-length on trajectories should be less than   , which is: where   + indicates takeoff range and   − indicates landing range.Considering the takeoff and landing process is controlled by ground operators, so   + and   − are set to be a fixed value,   + +   − = .
3) Yaw angle constraint: the turning radius of UAVs cannot exceed the design value, so yaw angle range is [−  ,   ], as is shown in Figure 2. The yaw angle at each RRT tree node at step  + 1 is where | ⋅ | indicates the norm of a vector.Then the yaw angle constraint that should be satisfied at step  + 1 is: where ,  respectively indicate the count of nodes on trajectory  and . 5) Flight forbidden zone constraint: during path planning procedure, UAVs can't fly pass unsuitable areas caused by terrain, weather, and other factors.Supposing the enclosed area   (⋅) is the flight forbidden zone  (shown in Figure 3), then for any flight forbidden zone   (⋅) and line segment    +1 (connecting two adjacent nodes   and  +1 ) should satisfy:

Path evaluation model
Flying range and the sum of yaw angels are main factors that affect the performance of the path.In this paper, we take these two factors as the evaluation parameters, and the form of evaluation function J(x) is: where  1 and  2 are weight coefficients, and  1 +  2 = 1;  indicates the total count of trajectory nodes, the flying range of each step is   , while  indicates the yaw angle of each step.

Model of the UAV formation
The formation model keeps each UAV member in a specific shape and distance to realize collision avoidance or mission cooperation.Here, set a UAV formation composed of N UAVs with one leader UAV and N-1 followers.The configuration is kept by formation model, that is each follower relative to the leader maintains the formation.The formation model is expressed as follow: where   (, ) and (, ) respectively indicate the position information of the follower  and the leader.While   (, ) is position of the  th follower relative to leader, which means that every follower can choose an exact position in its selectable region that can be acceptable by formation configuration.

Standard RRT algorithm
In the standard RRT algorithm, the root node is taken as the planning starting point as the root node, and from there, leaf nodes are added by random sampling to generate an exploratory random tree.The tree stops searching when some of the leaf nodes cover the target point or enter the target region.It is now we can find a no-collision path composed of root nodes, from the starting point to the target location.The process of basic essential algorithm is outlined in Figure 4 as: Step 2: take a point  +1 on the line segment     , satisfying |   +1 | =   ; Step 3: repeat step 1 and step 2 until the distance between   and the new generated node is less than the threshold ; Step 4: find a path from  0 to   in the RRT tree as an alternative trajectory.

Improvement of RRT algorithm
During traditional RRT extending, the step-length is fixed  1 =  2 = ⋯ =   =  as the subtree   →  +1 →  +2 is shown in Figure 5.The extending node is too close to or cross the no-fly zone due to the fixed step-length, which will result in lots of invalid search attempts, as the subtree  +1 →  +2 passing through the flight forbidden zone  1 .The expansion process is easy to be trapped into the local minima area, especially when the number of obstacles is large or the flying area is congested, which will directly increase in invalid searches.In response to the above problem, an improvement of the fixed step-length RRT algorithm is proposed in this paper.We dynamically adjust the extending step-length of new tree node according to the no-fly zone situation, which means either   =   or   ≠   is acceptable.Figure 5 shows the variable step-length subtree   →  +1 ′ →  +2 , where the steplength   ′ =  +1 ′ .This paper studies the dynamic adjustment of the RRT expansion step-length.Therefore, the RRT tree can jump out of the local minima area and accelerate the expansion speed toward the target direction with high obstacles avoiding efficiency.

RTS filter
There are two stages in the RTS filter: Kalman forward filtering and RTS backward filtering.) where  ,−1 is the prediction result over the previous states,   ∈   indicates the status measurement of the system,  is the parameter of the measurement system,  indicates the gain,  , and  ,−1 respectively represent the covariance corresponding to the new state and the covariance corresponding to  ,−1 , and  indicates the gaussian white noise covariance.The forward filtering process over Kalman filter from node  0 to node   of the RRT tree is outlined in Figure 6.

2) RTS filter design
In the process of Kalman filter to smooth the UAV path planning results, we save the filter value  , , predictive value  ,−1 , filter error variance matrix  , , predictive error variance  ,−1 , and state transition matrix  [11].RTS smoother is then performed from the final state to initial state, and the recurrence formulas of which are: ( ) where  = 0,1,2, … , ,  |  and  ,  respectively represent the state vector and covariance after RTS smoothing, while  | and  | indicate predicted value and covariance after Kalman filtering, respectively.Furthermore,    is the smoothing gain which is expressed in formula as: The smoothing process and direction of RTS backward filter are given in Figure 7. Smoothing result is mainly calculated based-on the results of the forward Kalman filter.Thus, the RTS filtering results are positively correlated with the Kalman filtering results.

Path planning algorithm
In this paper, the standard RRT algorithm is improved with an adaptive step to improve its tendency to fall into local minima and to extend invalid nodes.To facilitate the UAV path tracking control, we combine the improved RRT algorithm with the RTS filter, and the RTS filter is used in this paper to smooth the trajectory to achieve the curvature continuity of the planned path [7].
In Figure 8, the path planning process combined the adaptive RRT algorithm with the RTS filter is outlined.The new algorithm can be divided into three stages, the first stage is the leader's path nodes generation using improved adaptive step RRT; the second stage is the leader's trajectory smoothing based on RTS filter, the path nodes that are generated by RRT are smoothed to make the path ease to track; the third stage is the UAV formation path generation, mainly using the Artificial Potential Field (APF) method to generate followers' trajectory.APF is a virtual potential field that is composed of attraction field generated by target and repulsion field generated by obstacle [12].trajectory of the leader UAV and the initial configuration of formation, here the trajectory of the leader is planned by the path planning method mentioned above.More specifically speaking, the trajectory of the followers is calculated based on the APF algorithm: using the trajectory nodes of the leading UAV as a reference from which to calculate the attraction applied to each follower.Then we calculate the repulsion of the obstacles, leading UAV, and other following UAVs, and finally, the position of each follower is determined by the resultant.The detailed steps of the APF algorithm-based UAV formation path planning method are as follows: Step 1: The trajectory nodes sequence of the leading UAV is calculated as the reference trajectory node sequence for the others.
Step 2: The expected trajectory node sequence of the followers is solved using the formation reference trajectory nodes and the formation configuration.
Step 3: According to the reference trajectory nodes and the formation configuration, the attraction fields that applied on each follower are built taking current expected trajectory node as the target, while the repulsion fields are built taking both the flight forbidden zones and the known trajectory nodes as obstacles.After establishing a potential field model, the resultants and their directions are calculated.Then following the direction of the resultant, a new trajectory node of each follower is obtained base on the last node, and step-length of the followers is consistent with leader's.
Step 4: If the current follower has not reached the target position, then turn to step 3 and keep calculating the next node; if the target position has been reached, the calculation ends.
Step 5: Check whether the path planning of all the followers is completed, if not, return to Step 2; if the calculation is completed, then check whether the planned trajectories conflict with the obstacles, if there is no conflict, output all the trajectories; if there is a conflict, then return to Step 1 to re-plan all the paths.

Simulation and verification
To verify the feasibility of the path planning method combined the RRT algorithm with adaptive step and the RTS algorithm, a simulation with a 100 × 100  2 space size is set in this section, where flight forbidden zones are randomly distributed.And four groups of UAVs with different initial positions and target locations are utilized in the simulation, and the coordination is given in Table .1.It can be seen from the simulation results in Figure 9 that the path planning algorithm proposed in this study can plan the trajectory at different starting and ending locations, by which it can be proven that the planning algorithm is well-adapted.Figure 9 illustrates the planning results of both the planned trajectory with RRT algorithm only and the planned trajectory with a combination of RRT and RTS algorithm.The solid magenta line is the path planned by the improved RRT algorithm, and the black dashed line is the path after the RTS interpolation.The curve fitted by RTS interpolation is smoother and does not have an excessive turning angle.

Conclusion
In this study, a UAV formation path planning algorithm based on a combination of RRT algorithm and RTS filter is designed.The adaptive step improvement of the RRT algorithm is carried out according to the planning needs, and then the RTS filter is applied to the planned folded trajectory, from which the curvature continuous UAV formation leading UAV trajectory is planned.Based on the leader trajectory, the APF algorithm is used to solve the follower's trajectory of the UAV formation and finally generate a smooth trajectory for the whole UAV formation.Through the construction of the simulation scenario, it is verified that the proposed algorithm can plan the smooth path of the UAV formation in the flight forbidden zone, and can provide the desired trajectory output with continuous curvature for the UAV trajectory tracking control.

Figure 2 .
Figure 2. Schematic diagram of yaw angle 4) Distance constraint: the distance between each UAV's trajectory nodes is not less than the safety distance of formation, which is:

Figure 4 .
Figure 4.The extension process of the basic RRT algorithm Step 1: generate a random sampling point   at current RRT trajectory node   , meanwhile |    | >   and −  ≤   ≤   ;Step 2: take a point  +1 on the line segment     , satisfying |   +1 | =   ;Step 3: repeat step 1 and step 2 until the distance between   and the new generated node is less than the threshold ;Step 4: find a path from  0 to   in the RRT tree as an alternative trajectory.

Figure 5 .
Figure 5. Schematic diagram of adaptive variable step-length expansion

Figure 8 .
Figure 8. Route planning algorithm based on the combination of RRT and RTS A Leader-Follower formation configuration is proposed to maintain the UAV relation of relative positions, the theoretical trajectory of each UAV member in formation is calculated over the reference

Figure 9 .
Figure 9. Result curves of path planning under different starting and ending node to demonstrate the effect of introducing RTS filter, the yaw angle of the planned Trajectory in Figure 9A is statistically compared in this section, and the results are shown in Figure 10.It can be seen that the yaw angle of the RTS-smoothed trajectory (black dashed line in the figure) is significantly better than that of the non-smoothed trajectory, which is more suitable for UAV's trajectory tracking control.

Figure 10 .
Figure 10.Comparison of track yaw angle before and after RTS smoothingIn order to verify the proposed formation path planning algorithm, the trajectory in Figure9Ais used as the trajectory of the leading UAV, and the planning result with 4-UAV formation is shown in Figure11.Where the trajectory numbered "11" corresponds to the leader trajectory, and the other trajectories correspond to the followers' trajectory respectively.It can be seen that the formation

Figure 11 .
Figure 11.4-UAV formation diagramAs shown in Figure12, curve 12, curve 13, and curve 14 represent the distance between leader and follower 2, leader and follower 3, follower 3 and follower 4, respectively.It can be seen that the distance between UAVs remains basically stable.When encountering a flight forbidden zone (as shown in the curve "34" in the figure), the shortest distance between follower 4 and follower 3 is still more than 800m, which is greater than the minimum distance constraint between UAVs.The simulation results indicate the formation configuration keeping ability of the algorithm and the flight forbidden zone processing ability of the followers' trajectory, and demonstrate the effectiveness of the proposed formation keeping strategy and the improved APF algorithm.

1) Kalman filter design
For discrete control process systems such as UAV path planning, the best estimation of the current state  , can be obtained by using Kalman filter with inputs of measured values under known conditions and current state prediction values.

Table 1 .
Different initial position and target location parameters