Trajectory Optimization and Finite-Time Control for Unmanned Helicopters Formation

A safe and rapid formation generation is of great importance for the cooperation performance of the multiple unmanned helicopters. The trajectory optimization and control is the key problem in the process of the formation generation. To achieve this goal, a novel safe formation generation strategy is proposed through trajectory optimization and tracking control. First, the rapidly-exploring random tree (RRT) algorithm is applied to obtain the initial guess to satisfy the requirement of rapid solution. Then, the Gauss pseudospectral method is used to transform the optimal control problem to the nonlinear programming (NLP) problem and the sequence quadratic programming (SQP) method is utilized to gain the optimal trajectory combined with the initial guess. Second, a finite-time sliding mode controller is proposed to ensure the finite time trajectory tracking in the presence of model parameter uncertainties and unknown external disturbances. Finally, the numerical simulation is provided to show the effectiveness of the proposed formation generation strategy for unmanned helicopters formation.


I. INTRODUCTION
With the vertical takeoff, landing, air hovering and flexible flight characteristics, unmanned helicopter has been widely used in military and civilian fields, such as target reconnaissance, disaster relief, strike power patrols and agricultural plant protection [1]- [4]. With the increase of task complexity and number, a single unmanned helicopter is hardly to satisfy the requirements. Hence, the interest in multiple unmanned helicopters has been growing strongly. The collaborative unmanned helicopters can improve the mission efficiency, flexibility and fault tolerance [5]. It should be noted that formation generation is an essential part in the process of collaboration and rapid formation generation will improve the mission efficiency and flight safety.
Therefore, it is of great significance to study the formation generation of unmanned helicopters formation. The main challenge is to guarantee the safe and rapid performance in the presence of external disturbance and model uncertainty.
The associate editor coordinating the review of this manuscript and approving it for publication was Bin Xu. The problem is solved through trajectory optimization and finite-time control in this paper. The trajectory optimization methods are mainly divided into two categories: indirect methods and direct methods [6], [7]. The trajectory optimization problem with boundary value and path constraints was converted into a Hamiltonian two-point boundary value problem by indirect method which satisfied the first-order necessary condition using the minimum principle [8]. The indirect method involving trim-reference functions was used to gain the optimal path for unmanned aerial vehicle (UAV) by Barron and Chick [9]. The direct and indirect methods were employed to compute feedforward control sequences for the flight control of a quadrotor UAV and optimization results were compared with respect to the accuracy and applicability [10]. The trajectory gained by indirect method had a high accuracy with the disadvantage of the initial values sensitivity, low convergence speed and small convergence domain. The optimization research mainly concentrated on the direct method. Direct collocation method was used to solve the trajectory optimization problem for spacecraft proximity rendezvous with path constraints by Liu [11]. Numerical simulation results demonstrated that the proposed method was not sensitive to the initial condition, however the solution speed does not meet the real-time requirements. The trajectory of a tubular launched cruising unmanned aerial vehicle was optimized using the modified direct collocation method for attacking a target at back slope under a wind gradient by Jiang, but the result could be only applied for a single UAV [12]. Taking into account the problems of traditional direct and indirect methods, the pseudospectral technique was proposed to solve the trajectory optimization problems [10]. The pseudospectral method, as an effective direct method, has attracted a lot of attention in recent 20 years. It has larger convergence region and faster convergence speed by combining the advantages of indirect method and direct method [13]. Pseudospectral method has been sussessfully applied in engineering practice due to the effectiveness and rapidity. The team of Ross [14] realized the optimization of large-angle attitude maneuver of the International Space Station under ''zero fuel'' using self-developed DIDO pseudospectral optimization software package, which verified the feasibility of the method. The pseudospectral solver was used to gain the optimal trajectory for an UAV in the presence of a wind field. The simulation showed the resulting solutions exploit regions of favorable tailwinds in order to reduce battery consumption [15]. A dynamically feasible trajectory was obtained through the solution of an optimal control problem using pseudospectral optimal control software by Grymin [16]. Tang focused on the problem of minimum time trajectory planning by pseudospectral method for helicopter UAVs [17]. Adaptive pseudospectral was used to gain the offline trajectory of formation generation for quadcopters by Zhang [18]. It can be seen from the above literature, pseudospectral method has a great performance for trajectory optimization of a single UAV. However, the collision avoidance constraints between UAVs for the formation generation problem were transformed into the problem that large number of path constraints will increase the computation time greatly.
It should be noted that suitable initial guess closely related to the computation time. In order to reduce the computation time, some optimization methods were proposed to obtain a better path as the initial guess. Yu proposed a modified wolf swarm search algorithm combined with genetic algorithm to calculate the UAV path [19]. Differential evolution algorithm was used to provide a better initial guess for space maneuver vehicle trajectory planning by Tsourdos [20], so the collision avoidance for the formation problem is not exist. RRT method was adopted to achieve real-time flight-path planning for multirotor aerial vehicles [21]. A new hybrid collision avoidance method along with a modified path following approach was proposed to reduce the computational efforts and satisfied the performance of collision avoidance [22]. A new ant colony optimization approach to path planning in dynamic environments to achieve the obstacle avoidance, however the computational time values vary from about ten to 60 seconds, which is not suitable for the helicopters [23]. A hybridization of an improved particle swarm optimization (IPSO) and an improved gravitational search algorithm (IGSA) was proposed for multi-robot path planning, which has a better performance for the different meta-heuristic algorithms such as IGSA, IPSO through the simulation, however could not satisfy the strong real-time requirements [24]. An improved RRT algorithm was proposed to gain the near optimal path that taking into account the dynamic constraints [25].
Along with the safety trajectory, a robust finite-time control strategy aiming at tracking the safety trajectory was indispensable to achieve the formation generation. Most of the trajectory tracking of unmanned helicopter adopted linear control methods. A H ∞ attitude tracking controller was designed for the attitude inner loop of unmanned helicopter and an position outer loop tracking control algorithm was designed via the dynamic inverse approach to achieve tracking control [26]. The feedback linearization and extended high gain observation methods were applied to propose a formation controller for unmanned helicopters system [27]. However, it is known that linear model can only be used for flight control around reference equilibrium points which is not suitable for the nonlinear model. It is reasonable and effective to propose a nonlinear controller suitable for the nonlinear model. Based on nonlinear control method, a smooth outer loop controller was proposed to achieve the asymptotic tracking for helicopter by Karimoddini [28]. The distributed cooperative controller based on linear sliding mode controller was designed for underactuated quadrotors when the reference signal was not available to all the vehicles by Ghommam [29]. A novel robust terminal sliding mode control algorithm was proposed to achieve the robust control avoiding the chattering phenomenon for a quadrotor UAV [30]. In recent years, finite-time theories were widely used in the UAV, spacecraft and Reusable Launch Vehicle (RLV) control [31]- [36]. A controller design method using nonsingular terminal sliding mode surface and extended state observers (ESOs) was proposed to solve the finite-time convergence problem of system states in course of the transition flight control for a small tilt rotor UAV [37]. Finite-time disturbance observer-based controllers were designed for the quadrotor model using homogeneity theory [38].
In the past few years, many collision avoidance algorithms have been designed of the formation generation process for spacecraft and UAVs. A hybrid particle swarm optimization and genetic algorithm was proposed to solve formation reconfiguration problem by Duan [39]. A semianalytic approach was developed to achieve fuel-minimized, collision-free path generation for satellites formation [40]. Potential energy function method was the most common method for formation collision avoidance control, which is more suitable for large-scale UAVs formation. An optimized artificial potential field (APF) algorithm with distance factor and jump strategy was proposed for multi-UAV operation in 3D dynamic space [41]. The controller based on the decentralized navigation function was used to achieve collision avoidance in multi-agent systems [42]. The potential function based-RRT * that incorporates the artificial potential field algorithm was proposed to improve a more efficient memory utilization and accelerated convergence rate [43].
Motivated by the aforementioned discussion, a novel safe formation generation scheme is developed for unmanned helicopters in the presence of external disturbance and model uncertainty. In this paper, the main contributions of the work are twofold. Firstly, the safe trajectory is obtained by the pseudospectral method and SQP method, in which RRT method is proposed to obtain a better initial guess. Secondly, a new finite-time tracking controller based on sliding mode method combined with the potential function method is designed to achieve the high precision control performance.
The rest of the paper is organized as follows: the model description, path constraints and control objective in the research are formulated in Sec. II. The novel formation generation scheme are given in Sec. III. The simulation results are provided in Sec. IV. Finally, conclusions remarks are summarized in Sec. V.

II. PROBLEM FORMULATION
In this paper, the problem of unmanned helicopters formation generation is investigated. The mathematical model, constraints and control objective are given as follows.

A. MATHEMATICAL MODEL
Ignoring the lift generated by lateral and longitudinal periodic variable angles in the x, y directions and coupling among lateral, longitudinal periodic variable angles and tail rotor, the equations of six-degrees-of-freedom rigid body model for helicopters can be described by the following equations [44] where are the position and velocity vector of the ith helicopter in the inertial frame, g is the acceleration due to gravity, e 3 = [0, 0, 1] T , R i is rotation matrix from body coordinate system to inertial frame, T is the attitude control vector. The rotation matrix R i is given as follows where C is cos(·), S is sin(·). Ignoring the external disturbances, the particle model using in the part of trajectory optimization for helicopters is given as followsṖ where T mi = −g + Z i w w i + Z i col δ i col is the main rotor thrust for the ith helicopters, the formula(3) can be shown as followṡ The path constraints during the process of formation generation for unmanned helicopters include state variables, boundary value and collision avoidance constraints. Taking small unmanned helicopter as controlled object, the state variables need to satisfy the following constraints: The boundary value constraints contain the state and control variables in the initial and final time points. Considering the helicopters take off from three areas, the position variables in the initial and final time points is given as follows: In the initial and final time points, the velocity vector for helicopters need to be zero, which is given as follows: In the initial time points, the Euler angle vector need to be zero. Considering the range of Euler angle vector is relatively small, the Euler angle vector in the final time can be free, which is given as follows: In addition, the collision avoidance constraints between UAVs are given in the form of linear constraints as followsčž In this paper, the formation generation process of unmanned helicopters through trajectory optimization and control is presented. In the part of trajectory optimization, the main objective is to design optimal trajectories for the unmanned helicopters formation which aim at minimize the flying time: where t f (i) is the flying time of the ith unmanned helicopters.
In the part of control, a finite-time controller is designed to achieve the high precision robust tracking control for the unmanned helicopters formation.

III. TRAJECTORY OPTIMIZATION AND CONTROL DESIGN
As shown in FIGURE 1, the structure of trajectory optimization and control are given as follows. In the part of trajectory optimization, the safe path for the unmanned helicopters formation is given through RRT algorithm as initial guess. The trajectory optimization can be discretized to a NLP problem and solved by the SQP method. In the part of control, the nonlinear model for unmanned helicopter is established. The finite-time position outer and attitude inner controller based on the terminal sliding mode are proposed to achieve the high-precision tracking control and the system stability proof is finished by Lyapunov function combined with the multi-scale principle.

A. TRAJECTORY OPTIMIZATION
Firstly, RRT method is applied to acquire safe paths as a better initial guess to increase the convergence speed. Then, the Gauss pseudospectral method is used to convert the optimal control problem into a NLP problem and solved by SQP method to gain the optimal trajectory.

1) GAUSS PSEUDOSPECTRAL METHOD
A new discrete strategy is proposed by pseudospectral method. The pseudospectral method mainly includes three methods: Gauss, Legendre and Radau pseudospectral method. Among them, Gauss pseudospectral method has the most complete proof of first-order optimality and the highest precision which is applied in the paper.
The detailed discretization steps are given as follows: Firstly, the root of Legendre polynomials located in the time domain τ ∈ [−1, 1] is chosen as the collocation point, which is different from the time domain of actual trajectory. Therefore, the time interval t of trajectory for unmanned helicopters formation is transformed into the time interval τ of the Gauss pseudospectral method by time domain transformation: where t 0 is the initial time of trajectory optimization, t f is the final time of trajectory optimization, τ is the time interval satisfying the requirement of Gauss pseudospectral method. Secondly, K is chosen as the number of discrete points, the collocation points is K -order Legendre-Gauss (LG) points, i.e. the root of K -order Legendre polynomials, where Legendre polynomial P K (τ ) is given as follows Thirdly, continuous state variable discretized by Lagrange interpolation polynomials based on discrete points is given as follows where Similarly, the discretization of control variable is expressed as follows where U(τ i ) is the τ i th discrete point of control variable. Fourthly, the terminal time point is not contained in the discrete points of Gauss pseudospectral method, the state variable need to be obtained by integration as follows The terminal state X(τ f ) is discretized by integrating the Lagrange interpolation polynomials as follows where w k is the Gaussian weight. Fifthly, the discretization approximation of the derivative parts obtained by deriving the Lagrange interpolation polynomial is given as followṡ The whole discretization process is presented from (11) to (17) and the optimal control problem is finally converted to a NLP problem.

2) INITIAL GUESS
For the rapidity and safety requirements of unmanned helicopters formation generation, a better initial guess is provided by RRT algorithm to reduce the computation time of trajectory optimization.
RRT method is established by incremental methods with a tree-shaped data storage structure which can rapidly reducing the distance between randomly selected points and expected points. As shown in FIGURE 2, node expansion diagram of RRT method is given.
The specific solution steps are given as follows: Step1 (Random Path Point Selection): The target position q goal of formation generation is selected as the random path point q rand by probability p g or a random path point in the task space is selected as the random path point q rand by probability 1 − p g .
Step2 (Adjacent Path Point Selection): From current leaf nodes of the random tree, the node nearest to the random path point q rand is selected as the adjacent node q near .
Step3 (New Path Point Selection): The new path point q new is obtained by extending one step distance from the direction of q near to the direction of q rand . The collision confliction is determined during the extension process. If there is no conflict between the new path point q new and path point for the other unmanned helicopters, q new is accepted and added as a node of the random tree; if the confliction maybe occurs with the threat area, the new node is discarded and the random node q rand is selected again until the security requirements are met.
Step4 (Search Stop Judgment): Judging whether the new node reach the neighborhood of the target node. If this condition is satisfied, the random search process should be stopped; if not, back to the Step2.

3) COLLISION AVOIDANCE STRATEGY
The unmanned helicopters in formation calculate their own safety path points in turn. Considering the requirement of the fast formation generation and the same search step size between the unmanned helicopters, when judging the collision confliction conditions in the kth iteration for the ith unmanned helicopter, the path points from the k − 1th to the k + 1th iteration for the other helicopters are considered. So the safe flying for the unmanned helicopters can be achieved when the collision avoidance conditions are satisfied.
The trajectory optimization is carried out using SNOPT solver based on SQP method. SNOPT is a software package for solving large-scale optimization problems which is effective for NLP problems.

B. CONTROLLER DESIGN
When optimal trajectory is obtained as the desired command, the finite-time tracking controller for the unmanned helicopter formation is designed to realize the high-precision tracking.
Firstly, a terminal sliding mode virtual speed controller is designed to ensure accurate tracking for the position reference command. Further, the terminal outer loop controller is designed to realize the tracking of the virtual speed. In the part of attitude inner loop, the reference command of the attitude is obtained by attitude calculation. Based on the second-order sliding mode control method, the inner loop controller is designed to ensure the stable tracking control of the inner loop attitude to the reference command. The stability of outer and inner loop system are proved by combining the existing multi-time scale criteria. The specific design process is given as follows: Several assumptions and lemmas are given to prepare for the stability proofs.
Assumption 1: It is assumed that the outer loop disturbances d Vi are bounded, and there exist known positive constant k Vi such that the conditions d Vi ≤ k Vi , i = 1, ..., n hold.
Assumption 2: It is assumed that the inner loop disturbances d i and its derivatives are bounded, and there exist positive constants, k i andk i , such that the conditions d i ≤ k i and ḋ i ≤k i hold.
Lemma 1 [47]: For the following systeṁ where σ ∈ R n is system state, (t) ≤ δ, δ is a known scalar bound, there exists a range of values for the gains k 1 , k 2 , k 3 , k 4 , such that the variables σ andσ are forced to zero in finite time and remain zero for all subsequent time.
When the trajectory of each unmanned helicopter is determined, the design of trajectory controller becomes the key to achieve the formation generation. A finite-time position and a finite-time attitude controller are designed.

1) POSITION TRACKING CONTROLLER DESIGN
Position tracking error vectorē Pi is defined as follows e Pi = P di − P i (19) where P di is the desired trajectory for ith unmanned helicopter, P i is the position of ith unmanned helicopter. Define assist tracking error vector e Pi where γ a ij is the potential energy function to describe the collisions between unmanned helicopters which is given as follows where η j > 0, 0 < ε a < 1, 0 < ρ a < r a ,r a is the safe flight radius of unmanned helicopter. The virtual velocity controller V di is designed by where sig The outer controller based on the terminal sliding mode is given by (23) Then the collective pitch of main rotor can be described as follows

Finite-Time Integral Filters:
It is difficult to obtaining the derivative of the virtual controller V di , because this signal may not be practically differentiable. Based on this, we use the finite-time integral filter designed in our previous paper [41].
The finite-time integral filter of virtual and its derivative are designed aṡ Then, the output of integral filters replaces the derivative of the virtual controller.

2) ATTITUDE RESOLUTION
Because the trajectory tracking control cannot be realized by the attitude control variables, the desired attitude angle must be obtained through the attitude resolution.
Defining the desired attitude angle di = [φ di , θ di , ψ d ] T , the desired yaw angle ψ di of unmanned helicopter is given by the desired trajectory. Combined with the relationship U 1i = R i e 3 Z i col δ i col among rotation matrix R i , main rotor pitch angle δ i col and outer loop controller U 1i and the property of rotating matrix R T i R i = I ∈ R 3×3 , the desired roll angle φ di and pitch angle θ di are given as follows (26) In the following, the second derivative of the attitude controller is estimated in the same way. The finite-time integral filter of desired attitude angle, its derivative, and its second derivative are designed aṡ

3) ATTITUDE TRACKING CONTROLLER DESIGN
Define attitude tracking error vector e i = di − i . The terminal sliding mode surface s i is designed as follows where β 2i > 0. If s i = 0 holds, the attitude angle tracking error e i and its derivatives can converge to zero in finite time considering the characteristics of the terminal sliding mode, which means the output of the system i can track the desired output di . Theṡ i can be calculated bẏ Then, design the attitude tracking controller U 2i where

4) STABILITY PROOF FOR THE POSITION SYSTEM
Theorem 1: Consider the UAV system (1) satisfying Assumption 1, if the outer controller is designed as (22)-(23), and then the trajectory tracking errors e Pi will converge to zero in finite-time.
Proof: Trajectory tracking error vector e Vi is defined as follows where V i is the velocity of ith unmanned helicopter.
Choose the following candidate Lyapunov function 1 as follows The derivative of 1 iṡ According to Assumption 1, the disturbances d Vi have a bound.
is the sum filter errors, then˙ 1 ≤ −k 3Vi 3 4 1 . The velocity tracking error of ith unmanned helicopter can converge to zero in finite-time. The sum filter errorsV di − ξ 1i + ξ 1i − ξ 2i can converge to zero in finite-time according to [41].
Then, it is proved that the trajectory tracking error e Pi of ith unmanned helicopter will not diverge before the virtual velocity is traced at finite time. Consider the following candidate Lyapunov function The derivative of 2 is taking as followṡ Further, we derivė According to the assumption 3, the derivative of desired trajectory Ṗ d i , i = 1, ..., n is bounded by l 1 and the disturbances d Vi , i = 1, ..., n is bounded by l 2 , l 2 = max{k 4Vi }.
If e Vi > 1, we have e Vi ≤ e Vi 2 ≤ 2 2 , the derivative of 2 can be described as followṡ The above inequality means 2 is bounded before the virtual velocity is traced at finite time. Therefore, e Vi , ē Pi is bounded according to formula (37).
Lastly, the tracking to the desired trajectory for the virtual velocity will be proved. When the velocity error e Vi converge to zero, the Lyapunov 3 is given as follows (38) Similarly, the derivative of 3 is given aṡ If k 2Vi > Ṗ d i , we havė Therefore exists˙ 3 < 0, the trajectory tracking errore Pi can converge to zero in finite-time.

5) STABILITY PROOF FOR THE ATTITUDE SYSTEM
Theorem 2: Consider system (1) satisfying Assumption 2, if the inner loop controller is designed as (30) while the derivative of ζ i defined in (42) is bounded, then the sliding mode surface s i and its derivativeṡ i will converge to zero in finite-time and attitude tracking error e i will converge to zero in finite-time.
Proof: Combined (29) and (30), the derivative of the sliding surface is obtaineḋ Define The filter errors are converge to zero after finite time according to reference [44], then (41) becomeṡ According to Assumption 2,ζ i has a known bound. The candidate Lyapunov function 4 is chosen as follows According to [38], the inner loop attitude subsystem is finite time stable if controller parameters k m i , m = 1, 2, 3, 4 choose suitably. Further, we have e i → 0 with the time of attitude tracking error in finite time.
Remark 1: The finite time convergence characteristics of the position outer and the attitude inner loop system are clarified. Considering the unmanned helicopters formation control problem, since the control frequency of the attitude inner loop is much shorter than the position outer loop, the principle of multi-time scale can be used to prove the finite time stability for the entire closed-loop system.
Remark 2: The parameters of the tracking controller are determined based on Theorem 1 and 2. Moreover, in the simulation, we have tried several groups of data and selected better parameters.
Remark 3: The innovation of this paper is to design a virtual speed tracking controller (19) with collision avoidance, and prove the finite-time formation reconfiguration control.

IV. SIMULATION
Assume that in the process of formation generation, 10 unmanned helicopters take off from the ground, form a flight formation in three-dimensional space. The simulation is conducted on a computer with i5-2450M @2.5GHz processor and 8 GB of RAM. Firstly, the model and controller parameters of the specific helicopters used in the simulation are given in Table 1 and Table 2. Among them, the UAV model parameters including helicopter mass, gravity acceleration, proportional coefficient, moment of inertia matrix and multiple coefficient matrices are given.  The initial velocity, attitude angle, and attitude angular velocity are all zero, and the initial positions of each unmanned helicopter are set as      From FIGURE 4 to FIGURE 6, the desired position in the x, y, z direction are given, it can be seen that 10 unmanned VOLUME 7, 2019 helicopters reach the desired position at the same time and the desired position has a gentle changes with time in the range of [0, 300] m which satisfies the constraints (5).
In the simulation, the computation time for RRT method is compared with the PSO and ACO method. As shown in TABLE 3, the RRT method has 10 times faster than the PSO method and 4000 times faster than the ACO method. From the simulation, the RRT and PSO method can be used in the real-time environment and the RRT method has a faster convergence speed. The ACO method could only be used in the offline computation. As shown in FIGURE 7, a schematic diagram of the trajectories of 10 unmanned helicopters formation is given. It can be seen that the trajectory changes are gentle and the effect is good. In the current simulation environment, as shown in, the computation time of the pseudospectral method without the RRT algorithm is about 10s. By comparision, the optimal path point computation time of RRT algorithm is about 0.1s, and the time for the pseudospectral method to solve the optimal trajectory of 10 unmanned helicopters formation is about 0.9s. In general, the whole computation time is about 1s which meets the real-time requirements perfectly. Thus, the calculation efficiency can be increased by 10 times for 10 unmanned helicopters formation generation problem through the proposed method in this paper.     From FIGURE 8 to FIGURE 10, the tracking errors of the formation trajectory of the formation are shown. Under the influence of external disturbance and uncertainty, the trajectory tracking control error of the UAV formation is 10 −4 m. It means that the proposed finite-time controller satisfies the requirements of high-precision control, and at the same time in the trajectory tracking part, the convergence time is small and can be ignored.
From FIGURE 11 to FIGURE 13, the variation trend of the attitude error with time in the formation process of 10 UAV formations is given. From the figures, it can be seen that under the influence of external disturbance and uncertainty, in the existing simulation environment, the attitude control error in the θ and φ direction of the UAV  formation attitude tracking is within 0.04 rad; the attitude control error in the ψ direction of the UAV formation attitude tracking is within 0.11 rad, considering the position error in the final time, which satisfies the control requirements of the entire closed-loop system.

V. CONCLUSION
In this paper, a novel safe formation generation strategy is proposed for the multiple unmanned helicopters through trajectory optimization and tracking control. Firstly, to reduce the computation time, pseudospectral method and a NLP solver are used to obtain the optimal trajectory during which the RRT algorithm is applied to obtain the initial guess. Then, to ensure the finite time trajectory tracking in the presence of model parameter uncertainties and unknown external disturbances, a finite-time sliding mode controller is proposed through terminal sliding mode method and potential energy function approach. Finally, the efficiency of the proposed strategy is illustrated by numerical simulation.