6-DOF Modeling and 3D Trajectory Tracking Control of a Powered Parafoil System

The powered parafoil system is obtained by adding the propeller thrust to the unpowered parafoil system, and has coupling and nonlinear characteristics, which make its precise control more difficult than that of the unpowered parafoil system. To achieve the trajectory tracking control of the powered parafoil system in the field of precision airdrop, a mathematical model of the 6-DOF of the powered parafoil system is established first. Then, a new trajectory tracking strategy is proposed, which can overcome the limitations of the traditional guidance-based trajectory tracking strategy. We design lateral, longitudinal, and velocity controllers on the basis of the motion characteristics of the powered parafoil system. Then, we adopt the widely used PID control strategy in engineering. In response to the difficult of the PID controller parameter tuning of the powered parafoil system, we achieve the PID controller parameter tuning with the ecosystem particle swarm optimization (ESPSO) of the swarm intelligence optimization algorithm. The effectiveness of the algorithm is verified by simulation experiments. Results show that the proposed algorithm can obtain high trajectory tracking accuracy even when a deviation in the initial state and a random gust wind disturbance in the outside world occur regardless of the method of the multiphase homing or the optimal control homing adopted. The proposed trajectory tracking strategy also has strong robustness and adaptability.


I. INTRODUCTION
The powered parafoil system has the advantages of high load ratio and excellent gliding performance. It also has a wide application in military, civil, and spacecraft recovery fields. Compared with the traditional unpowered parafoil system, the powered parafoil system can achieve level flight and even climbing movement given the addition of thrust control, which greatly improve the athletic ability of the parafoil. Therefore, the longitudinal height control ability of the powered parafoil system is greatly enhanced, and it can effectively suppress the error of drop point of the parafoil system because of insufficient height. The powered parafoil system has stronger coupling and nonlinear characteristics The associate editor coordinating the review of this manuscript and approving it for publication was Shafiqul Islam . than the unpowered parafoil system given the addition of thrust, which impedes the precise control of the system.
The foundation of powered parafoil system research is to establish a mathematical model that can truly reflect its motion characteristics. At present, the common powered parafoil system models mainly include the six/eight degrees of freedom (6/8-DOF) model. Li et al. [1] established a novel framework for accurate modeling of powered parafoil. The model is developed in the following three steps: obtaining a linear dynamic model, simplifying the model structure, and estimating the model mismatch due to model variance and external disturbances factors. On the basis of the constraint of force and moment, Slegers et al. [2] built an 8-DOF dynamic model of a parafoil system and analyzed the turn command response of the open and closed loops of the system. Tan et al. [3] presented the dynamic modeling VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ process of a powered parafoil. The model is derived from the Lagrangian equations and dynamic constraints with 6-DOF of the canopy and 2-DOF of the payload. In the modeling process, the velocity, angular rate and force constraints are introduced and the detailed modeling process is provided. Zhu et al. [4] considered the relative motion of canopy and payload; two coordinates are built separately to analyze the motion properties of canopy and payload. According to the Kirchhoff motion equation, the 8-DOF dynamic model of the powered parafoil is built. The simulation focuses on the basic motions of gliding, tuning, flare landing, and the responses to wind and power. Yang et al. [5] established a simple static model, which is convenient for fast estimation or optimization design; they also discussed the relationships between the configuration parameters and the thrust. The most common method of parafoil trajectory tracking is the guidance-based trajectory tracking strategy [6], which essentially tracks the reference point on the reference trajectory that moves according to a scalar variable. Breivik and Fossen [7] proposed this strategy and proved that the trajectory error converges under certain conditions by using Lyapunov's method. Luo et al. [8] realized the parafoil track tracking using the guidance-based trajectory tracking strategy, and an accurate flight path tracking control approach for the powered parafoil combining active disturbance rejection control (ADRC) and wind feedforward compensation is proposed. Tao et al. [9] adopted a hybrid approach that combines the lateral track error and the line of sight to design guidance law; they also applied a generalized predictive control (GPC)based method for parafoil systems to track the designed trajectory to improve the control effect.
In terms of powered parafoil system control, Ochi et al. [10] designed an integral PID controller for the approximate parafoil linear model. Ward et al. [11] designed a parafoil trajectory tracking controller on the basis of model prediction. Tan et al. [12] proposed the GPC with characteristic model (CM) as a predictive model called the CM-based GPC and used it in the trajectory tracking control for parafoil systems. Sun et al. [13] proposed a hybrid control approach for powered parafoil based on ADRC. The horizontal controller in this control approach consists of the inner and outer loops unlike those in other existing ones. The outer loop is applied to accurately control the flight direction and offset the wind disturbance of the whole system. Meanwhile, the inner loop is designed to offer high control precision and dynamically compensate the unbalanced load on the actuators of the horizontal controller. Jia et al. [14] improved the controller performance by means of neuron fuzzy control. Tao et al. [15] applied ADRC controllers to solve the problem of having a large number of disturbances in actual flight environments given that traditional control methods cannot ensure the tracking accuracy. With this method, uncertain items of the model and external disturbances are estimated by an extended state observer and compensated by real-time dynamic feedback. These ADRC controllers possess better robustness and antidisturbance ability. Sun et al. [16] presented an improved ADRC. By analyzing the aerodynamic characteristic of the system, the wind disturbance is compensated previously with a feedforward compensation unit. This controller largely reduces the observation error of the extended state observer while improving the control effect and the reaction speed. In addition to the abovementioned PID and GPC control strategies, the sliding mode control strategy can overcome the uncertainty of the system and has strong robustness to disturbance and unmodeled dynamics [17]- [19]. It has great control effect for the nonlinear systems. The powered parafoil system is a second-order nonlinear system. The sliding mode control can quickly respond to the input transformation, and it is not sensitive to the parameter transformation and disturbance. The physical realization of sliding mode control is simple. Thus, it is a suitable strategy for trajectory tracking control of the powered parafoil system.
At present, research results on modeling and 3D trajectory tracking control of the powered parafoil system exist. The realization of the 3D trajectory tracking control of the powered parafoil system has some difficulty given its coupling and nonlinear characteristics. The trajectory error is easy to solve for simple geometric reference trajectories, such as straight lines or spiral curves. Thus, the traditional guidance-based 3D tracking strategy is easy to apply. However, the reference trajectory of the actual airdrop is usually complicated, and the real-time trajectory error is difficult to calculate. Thus, the traditional 3D trajectory tracking strategy is inconvenient to apply. A trajectory tracking strategy that can meet the needs of complex trajectory tracking must be used to accommodate actual airdrops. This study first establishes a 6-DOF model of the powered parafoil system as a basis for research to address the aforementioned issues. A new trajectory tracking strategy, which compensates for the shortcomings of the traditional 3D trajectory tracking strategy, is proposed. Lateral, longitudinal, and velocity channel controllers are designed to correct the trajectory error by using the PID control strategy, which is widely used in engineering, according to the motion characteristics of the powered parafoil system. Certain difficulties have emerged in the PID parameter tuning given the coupling and nonlinearity of the powered parafoil system. The trial-and-error method typically used for PID parameter tuning strongly depends on experience, and it can be realized only after long term experience accumulation. In recent years, the development of swarm intelligence algorithms has provided new ideas for PID parameter tuning. For example, some scholars [20]- [22] adopted the particle swarm optimization (PSO) algorithm to achieve the tuning of PID parameters. Bingul and Karahan [23] implemented the design of PID and FOPID controllers using PSO and artificial bee colony (ABC) algorithms, respectively, and compared the control effects of the controllers. The cuckoo search algorithm is also used in the design of PID controllers in automatic voltage regulator systems [24]. In this study, the ecosystem particle swarm optimization (ESPSO) [25] which combining the laws of the natural ecosystem with the standard PSO algorithm is used to achieve the parameter tuning of the PID controllers of the powered parafoil system. Simulation experiments verify the effectiveness and reliability of the control algorithm.
The contributions of this study are listed as follows.
1) A 6-DOF model of the powered parafoil system, which can provide a reference for the modeling of powered parafoil system, is established. The model can be used to analyze the motion characteristics of powered parafoil and accumulate experience for the powered parafoil airdrop engineering application. 2) A 3D trajectory tracking control strategy on the basis of reference point switching, which can overcome the limitations of the traditional guidance-based trajectory tracking strategy, is proposed. The proposed trajectory tracking strategy can be applied to complex trajectory tracking, and it is in line with actual engineering needs. 3) A 3D trajectory tracking controller for the powered parafoil system is designed, and the PID controller parameters are tuned by adopting the ESPSO algorithm. Compared with the standard PSO algorithm, the optimization speed is faster and the optimization accuracy is higher. It can provide a reference for the design of the trajectory tracking controller of powered parafoil system.
The rest of the paper is organized as follows. In Section II, the establishment process of the 6-DOF model of the powered parafoil system is introduced in detail, and the kinematic equation and dynamic equations of motion of the powered parafoil system are given. Section III analyzes the limitations of the traditional 3D trajectory tracking strategy, proposes a tracking control strategy on the basis of trajectory reference point switching, and designs a trajectory tracking PID controller of the powered parafoil system on the basis of the motion characteristics of the system. The coupling of powered parafoil system makes PID controller parameters difficult to tune. Thus, ESPSO is used to realize the tuning of powered parafoil trajectory tracking controller parameters. In Section IV, multiple sets of simulation experiments are conducted to verify the validity and reliability of the algorithm. In Section V, the conclusion is given.

II. SYSTEM MODELING
The modeling of the powered parafoil system has a common 8-DOF model, which posits that no relative rolling motion exists between the canopy and the load, and only the relative pitch and relative yaw degrees of freedom are considered. This model provides a detailed description of the kinematics of the powered parafoil system. It can also accurately reflect the relative motion of the load and the canopy. However, many engineers typically use a multipoint connection between the canopy and the load which is relatively stable in projects to pursue great control effect of the powered parafoil system. In this case, the relative motion between the load FIGURE 1. Schematic of powered parafoil system. and the canopy is negligible, and the whole parafoil-load system can be regarded as a single rigid body. At this time, the 6-DOF model of the powered parafoil system can also describe the system motion characteristics well. The research of the technique of the trajectory tracking control of the powered parafoil system aims to control the motion of the centroid of the system along the desired trajectory. Therefore, the research should focus on the overall motion of the powered parafoil system.
The 6-DOF model of the powered parafoil system is based on the 6-DOF model of the unpowered parafoil system with thrust added to the load. The propeller can generate forward or backward thrust through forward and reverse rotation. The forward thrust is positive and backward is negative. The small relative movements between the canopy and the load can be ignored under the mode of multipoint connection. The parafoil-load system can be considered a single rigid body. The structural diagram of the powered parafoil system is shown in Fig. 1. Involving the canopy coordinate system and the body coordinate system. The canopy coordinate system origin is fixed in the center of mass of the canopy. The body coordinate system origin is located in the center of mass c of the parafoil-load system, the coordinate axis direction is the same as that of the geodetic inertial coordinate system. When the reference point interval is sufficiently small, the approximate trajectory is infinitely approximate to the ideal trajectory.

A. SYSTEM KINEMATICS
Assuming that the angular velocity of the canopy in the canopy coordinate system is ω, the transformation matrix from the system coordinate system to the inertial coordinate system is T ib , and the system velocity is V b cm . Then, the position of center of mass X cm , and the Euler angle [φ, θ, ψ] T of the powered parafoil system can be expressed as The expression of the transformation matrix T ib is (2), as shown at the bottom of the next page.
The expressions of the velocity V b of the center of mass of the load and the velocity V p of the center of mass of the canopy are X gb and X gp are the distance vectors from the center of mass of the powered parafoil system to the center of mass of the load and canopy respectively. Then, the acceleration of the system's center of mass is where ∧ is the cross multiplication operation,V b cm is the tangential acceleration term, and V b cm is the normal acceleration term.

B. SYSTEM DYNAMICS
If the powered parafoil system is regarded as a particle and the mass of the parafoil ropes is ignored, then the force on the centroid of the powered parafoil system includes the gravity of the canopy and the load, the thrust of the propeller, the aerodynamic force of the canopy, and the aerodynamic force of the load. Only the momentum and momentum moment of the object change when the object moves at a variable speed in a vacuum. However, the momentum and momentum moment of the fluid driven by the object also change when the object moves at a variable speed in the fluid, except the momentum and momentum moment of the object with the change. The mass of this part of the fluid is called additional mass. When the density of the object is far greater than the density of the fluid, only the mass of the object itself needs to be considered when establishing the dynamic equation, and the influence of the additional mass can be ignored. The additional mass can greatly affect the motion model when the density of the object is close to or less than the density of the fluid. Thus, the additional mass of the object must be considered in this case. Given that the density of the load is much greater than that of the air but the density of the canopy is similar to that of the air. The additional mass forces are generated during the relative motion of the canopy and the air flow. Moreover, the tension of the parafoil rope belongs to the internal force for the whole powered parafoil system. Therefore, according to Newton's law of motion, the dynamic equation of the powered parafoil system can be expressed as where M is the total mass of the powered parafoil system, W b is the total gravity, and F p A and F b A are the aerodynamic force acting on the canopy and load, respectively. F app is the additional mass force vector of the canopy. T b is the propeller thrust vector, and the propeller directly acts on the load. The thrust T b is defined as Assuming that m b and m g are the mass of the load and the canopy, respectively, the total gravity of the system is (m b + m g )g, and the direction points vertically to the earth. Projecting it to the body coordinate system to obtain the expression of the total gravity W b : The aerodynamic forces acting on the load and the canopy are In Equation (9), V p and V b are the actual velocity vectors of the canopy and load, respectively. ρ is the atmospheric density. S p is the area of the canopy. C L and C p D are the lift and drag coefficients of the parafoil, respectively. Their expression is where α P is the angle of attack, and its calculation formula (1)). Atmospheric density ρ is a quantity related to altitude, and its expression is In Equation (11), ρ 0 and g 0 represent the atmospheric density and acceleration of gravity at sea level, respectively. R e represents the radius of the earth, and H represents the altitude. The calculation formula of the additional mass force vector F app of the parafoil is where M F is the additional mass term, which is calculated according to the following formula given by Lissaman and Brown [26]: A, B, and C are calculated according to the following three empirical formulas: T ib =   cos θ cos ψ cos θ sin ψ − sin θ sin φ sin θ cos ψ − cos φ sin ψ sin φ sin θ sin ψ + cos φ cos ψ sin φ cos θ cos φ sin θ cos ψ + sin φ sin ψ cos φ sin θ sin ψ − sin φ cos ψ cos φ cos θ   .
(2) VOLUME 8, 2020 where b is the canopy span, c is the canopy chord length, AR = b c is the canopy span ratio, a is the canopy arc altitude, and t is the canopy thickness. The force balance equation of the system can be obtained by combining Equations (6) and (12) as follows: The moment balance equation of the powered parafoil system is derived. The expression of the moment of inertia matrix of the canopy is Similar to the additional mass, the moment of inertia matrix of additional mass is calculated by the following formula given by Lissaman and Brown [26]: For the flat canopy, the moments of inertia I A , I B ,and I C of additional mass are defined as follows: According to the dynamic characteristics, the moment balance equation of the powered parafoil system about the center of mass can be written as In Formula (19), M p A is the aerodynamic moment. M app is the moment generated by the additional mass force. X cp is the relative distance vector from the center of mass c of the system to the center of mass of the canopy in the parafoil body coordinate system. X cb is the relative distance vector from the center of mass c of the system to the center of mass of the load in the load body coordinate system.
A , X gp ∧ F app , and X cb ∧ T b are the moments of aerodynamic force of canopy and load, additional mass force, and propeller thrust acting at the center of mass of the system, respectively. The abovementioned moments satisfy the following relations: The expressions of R cp and R cb are The expression of aerodynamic moment M p A that causes the canopy to roll, pitch, and yaw is where C l p and C l φ are the coefficients of the rolling moment, C m q , C m 0 , and C m α are the coefficients of the pitching moment, and C n r is the coefficient of the yaw moment. The expression of the additional mass force moment vector M app is where the expression of p is In combination with Formulas (19), (20), and (23), the moment balance equation of the powered parafoil system can be obtained as follows: In addition to the propeller thrust effect, the powered parafoil system can also be controlled by the symmetrical flaps deflection δ s and the differential flaps deflection δ a , which is defined as where δ Left and δ Right are the downward deflections quantity of the left and right flaps of the parafoil, respectively. The unit is radian. δ s and δ a influences both longitudinal and lateral aerodynamics coefficients of the parafoil, which results in a variation of aerodynamic force F p a and a variation of where the expression of L is (28), as shown at the bottom of the next page where sign(·) is the sign function, and C L δa , C D δa , C L δs , and C D δs are the lift and drag coefficients of the parafoil induced by symmetrical and differential flap deflection, respectively.

C. DYNAMIC EQUATIONS OF THE SYSTEM
In combination with (1), (2), (15), (25), and (28), the powered parafoil system motion equation can be obtained: where (30), as shown at the bottom of the next page.
The expressions of the center of mass velocity and the Euler angular velocity of the powered parafoil system on the control input of flap deflection and the control input of thrust are obtained, which make up the state equation of the powered parafoil system.

III. 3D TRAJECTORY TRACKING CONTROL A. TRACKING STRATEGY
Trajectory tracking control is the key to realizing the precision airdrop of the powered parafoil system. It is required to control the flight of the system along the planned trajectory by controlling several control variables of the powered parafoil system until it reaches the landing. At the same time, certain tracking accuracy and robustness need to be met. Equation (31) shows that the position of the center of mass of the powered parafoil system is affected by the flap deflection and propeller thrust. Among them, the asymmetric flap deflection control mainly affects the lateral aerodynamic performance of the powered parafoil system, which causes the parafoil to turn. With the increase in asymmetric flap deflection control, the yaw rate of the powered parafoil system increases. The symmetrical flap deflection mainly affects the longitudinal aerodynamic performance of the powered parafoil system, which is shown to affect the flight speed of the parafoil system. The propeller thrust acting on the powered parafoil system can change the flight speed of the system. A positive thrust increases the forward flight speed. Otherwise, it reduces the forward speed of the powered parafoil system. If the forward speed of the parafoil increases, then the lift generated by the canopy also increases. When the speed increases to a certain extent and makes the lift generated by the canopy equal to the total gravity of the system, the parafoil can realize level flight. Then, it continues to increase the thrust, and the parafoil's forward speed further increases. The parafoil can climb when the lift is greater than the total gravity of the system. At the same time, Equation (31) demonstrates a certain coupling between the control variables, and the control ability of the control variables are limited. The external environment can also easily interfere with the powered parafoil system. Thus, the realization of the trajectory tracking control technology of the powered parafoil system is always difficult. The traditional trajectory tracking method is mainly the guidance-based trajectory tracking strategy. The core idea is to track the reference point which on the ideal trajectory that moves according to the scalar variable. In the geodetic coordinate system, the point P r is the reference point on the ideal trajectory, and the point P C is the actual position of the parafoil system's center of mass at the current time. ϕ r and ϕ c are the reference and the actual course angles of the powered parafoil system, respectively. ϕ d is the angle between − −− → P C P 0 and the X e -axis of the geodetic coordinate system. γ r and γ c are the reference and the actual glide slope angles, respectively.
Breivik and Fossen [7] proposed the guidance-based 3D path tracking strategy. In the inertial coordinate system, the position of the center of mass of the parafoil system is represented as 3 , and the size of actual speed vector of the parafoil system is U C = |ṗ c | 2 = ṗ T cṗ c 1 2 . The course and glide slope angles are expressed as The powered parafoil tracks the reference point P r = [x r , y r , z r ] T ∈ R 3 on the reference trajectory, and the reference speed is U r = |ṗ r | 2 = ṗ T rṗ r 1 2 . Assuming that the reference point position is updated according to the scalar variable µ, the reference point position can be expressed as The reference course and reference glide slope angles can be expressed as ).
(33) VOLUME 8, 2020 The transformation matrix of the inertial coordinate system to the reference track coordinate system is where R p,z (ϕ r ) and R p,y (γ r ) are the rotation matrices of coordinate system around the Z P -axis and Y P -axis, respectively. The error between the actual position and the reference point can be expressed as The error ε = [s, e, h] T ∈ R 3 , where s is the along-track error, e is the cross-track error, and h is the vertical-track error. The three errors are the projections of − − → P c P r on the X P -axis, Y Paxis, and Z P -axis of the reference track coordinate system.
Breivik and Fossen [7] used Lyapunov's method to prove that, as long as the scalar variable µ is updated according to Equation (36): and the difference of course angle ϕ error and the difference of glide slope angle γ error between the actual and ideal velocities satisfy Formula (37): Then, the error vector ε = [s, e, h] T ∈ R 3 is globally uniformly progressively stable and locally exponentially stable. τ is a coefficient greater than 0. k e and k h are adjustable parameters greater than 0.
The abovementioned guidance-based trajectory tracking strategy requires parafoil tracking the point which moves according to the scalar variable µ on the reference trajectory. µ is easy to select and the guidance-based trajectory tracking control strategy is easy to implement for simple trajectories, such as straight lines or spiral curves. However, the reference trajectory of airdrop is usually relatively complex. In this case, finding a suitable scalar variable µ is often difficult. Thus, the implementation of the guidance-based trajectory tracking strategy becomes increasingly difficult. Other trajectory tracking strategies have the similar problems.
In response to the aforementioned problems, this study proposes a new trajectory tracking strategy on the basis of reference point switching. As shown in Fig. 2, the ideal trajectory is discretized to obtain a series of trajectory reference points arranged in sequence: Each reference point should include the position coordinate (x r , y r , z r ), reference speed V r , reference course angle ϕ r , and reference glide slope angle γ r . The powered parafoil system tracks an approximate trajectory constrained by a trajectory reference point within a certain period. Each approximate trajectory is a line segment whose end point is the current reference point P r i x r i , y r i , z r i . The direction of the approximate trajectory is the direction of the extension of the reference velocity vector. The starting point of each approximate trajectory is the intersection of the approximate trajectory and the switching plane. The switching plane passes through the previous reference point P r i−1 x r i−1 , y r i−1 , z r i−1 , and it is perpendicular to the reference course ϕ r i−1 of the previous reference point. When the actual position of the parafoil passes the switching plane, the target reference point is switched to the next reference point. Then, the new approximate trajectory is tracked. Repeat the above steps until the parafoil system reach the target point. This trajectory tracking strategy perfectly avoids the problem of difficulty in selecting scalar variable when applying the traditional guidance-based trajectory tracking strategy. The approximate trajectory is infinitely approximate to the ideal trajectory when the distance between the reference points is sufficiently small.

B. CONTROLLER DESIGN
According to the trajectory tracking strategy on the basis of reference point switching proposed in Section III.A, the trajectory tracking controller needs to be designed to realize the trajectory tracking of the powered parafoil system. PID is currently the most widely used control strategy. It has the characteristics of simple algorithm, good robustness, and high reliability. It is widely used in process control and motion control. Therefore, the PID control strategy that is used to achieve the trajectory tracking control of the powered parafoil system is a good choice. The basic idea of PID control is to take the error between the ideal and the actual outputs as the input and eliminate the error through the effects of proportional, integral, and differential links. Corresponding to the parafoil trajectory tracking system, the trajectory error can be taken as the controller input, and the control input can be calculated through the controller to eliminate the trajectory tracking error. Therefore, the definition of the trajectory error based on the trajectory reference point should be clearly given first. Then, the trajectory error expression based on the trajectory reference point should be derived. As shown in Fig. 2 (a), the horizontal error is defined as where L xy and ϕ error are the cross track distance and the error of course, respectively. Their expressions are where D xy is the distance between the actual position and the current reference point, ϕ L xy is the angle between −−→ P c P r i and current segment of the approximate trajectory.
The longitudinal height error of the system, as shown in Fig. 2 (b), is defined as where H error and γ error are the height and glide slope angle errors, respectively. Their expressions are Thus, the total trajectory error is The design of the PID controller is conducted. For the 2D trajectory error ε xy = L xy , ϕ error T , the design of the powered parafoil course controller is where k 1 and k 2 are the weighting coefficients. Given that L xy and ϕ error have different dimensions, they need to be unified through coefficients. The selection of the coefficient should follow this principle that the position error is the dominant error when the cross track distance L xy is large, and play the leading role of eliminating errors. When L xy is small, the angle error ϕ error plays a leading role to keep the course stable. L xy and ϕ error can be eliminated by choosing appropriate coefficients. Similarly, for the longitudinal height error, the design of the controller is H error and γ error can also be eliminated by choosing appropriate coefficients. The responding to the control input in time is difficult during the rapid change of the reference course given the high speed of the powered parafoil system. Thus, the tracking error can easily increase. It is not easy to lead to the increase of the tracking error when the reference course is relatively stable even the speed of powered parafoil system is fast. Thus, the speed of the powered parafoil needs to be controlled to suppress the tracking error caused by the abovementioned reason for improving the tracking accuracy. The specific design idea is to control the deceleration of the parafoil when the trajectory error is large to ensure that the powered parafoil system has sufficient reaction time to respond to the control input and eliminate tracking errors. When the trajectory error is small, the system speed can be increased so that to guarantee the parafoil can reach the target point as soon as possible. Thus, the designed velocity controller is Given the limited control ability, saturation limiting processing must be performed on the abovementioned several control inputs.
The control method of the powered parafoil system is mainly performed by controlling the trailing edge flap deflection and propeller thrust. The flap deflection is divided into symmetric flap deflection δ s and differential flaps deflection δ a . The 2D trajectory error can be eliminated by controlling the yaw angle ψ c of the powered parafoil system. The longitudinal height error can be eliminated by controlling the flight altitude h c of the powered parafoil system. The velocity control is mainly for forward velocity control. According to the 6-DOF model of the powered parafoil system, the yaw angle ψ cm , flight altitude h cm , and the forward velocityẋ cm of the parafoil system can be expressed by the following second order equations: where ω dis1 , ω dis2 , and ω dis3 are their total disturbances, respectively, which including external and internal disturbances. f 1 , f 2 , f 3 , f 4 , f 5 , f 6 , ω dis1 , ω dis2 ,and ω dis3 cannot obtain accurate mathematical expressions.
PID control does not heavily depend on the model, and it has strong adaptability to nonlinear systems. Therefore, it is suitable for the trajectory tracking control of the powered parafoil system. Thus, the control input expressions of the powered parafoil system are where the differential flaps deflection δ a affects the lateral aerodynamic performance of the parafoil, which results in the steering movement of the parafoil. The symmetrical flaps deflection δ s affects the longitudinal aerodynamics coefficient of the parafoil, which affecting the air drag of the parafoil. Then, the forward velocity of the parafoil is changed. The propeller thrust greatly improves the longitudinal height control ability of the parafoil system. Thus, it can be used as the control input for the flight altitude control of the powered parafoil system. The deflection control inputs of the left and right flaps of the parafoil should not exceed a certain range, otherwise it may cause system instability. The control inputs of flaps deflection meet δ Left , δ Right ∈ [0, 1] (rad), which means that the flaps can only deflect downward relative to the reference position. It is obviously disadvantageous to the speed control of the parafoil system. δ Left 0 = δ Right 0 = 0.5 (rad) is set to meet the actual control needs and improve control ability of the flaps so that the powered parafoil system can realize acceleration and deceleration motions by controlling the flap deflection. Then, Therefore, the expressions of the deflection control inputs of the left and right flaps of the parafoil are The thrust generated by the propeller should be coordinated with the total mass of the powered parafoil system. It will not provide enough powered to correct the altitude error when the thrust generated by the propeller is too small. At the same time, the propeller thrust is limited by the system power and has a certain upper limit. The maximum thrust of propeller is set as 800 N according to the parameters of the powered parafoil system researched in this article. Thus, the expression of thrust control input of the powered parafoil is The structure of the powered parafoil trajectory tracking control system is shown in Fig. 3. The total controller is divided into three channels: lateral, longitudinal, and velocity. The input of the lateral channel is u 1 , and the output of the differential flaps deflection δ a is used to control the yaw angle of the powered parafoil system and eliminate the horizontal error. The input of the longitudinal channel is u 2 .The output thrust T b x is used to control the flight altitude of the parafoil, which is used to eliminate the longitudinal height error. The velocity channel is used as an auxiliary control channel with an input of u 3 . By controlling the forward velocity of the power parafoil system, the trajectory tracking error is suppressed.
The flowchart of the 3D trajectory tracking control algorithm of the powered parafoil system proposed in this study is shown in Fig. 4. After trajectory planning, an ideal trajectory is obtained. The ideal trajectory is discretized to obtain an orderly arrangement of trajectory reference points. Then, the powered parafoil system state is initialized. Next, whether the parafoil has landed is determined. If it has landed, then the control ends. If it has not landed, then the trajectory tracking control starts, the trajectory error is calculated, and the controller calculates the corresponding control input. Under the action of the parafoil system actuator, the position of the parafoil system is adjusted. Thereafter, whether the powered parafoil system meets the reference point switching condition is determined, that is, whether the absolute value of the ϕ L xy is greater than 90 • (or whether it passes through the switching plane). If it does not meet the condition, then the reference point is not switched. Otherwise, the next reference point is switched. Finally, the loop returns to the step that determines whether the parafoil has landed. The trajectory tracking control continues until the parafoil system has landed.

C. THE PID CONTROLLER PARAMETER TUNING BASED ON ESPSO
The effects of proportional, integral, and derivative of the three controls must be adjusted to form mutual coordination and mutual restriction in the input of control for ensuring good control effects of the PID control. In practical applications, setting the PID parameters is difficult, which is often due to the complexity of the controlled object. The most commonly used PID parameter tuning method is the trial-anderror method. However, the trial-and-error method is strongly dependent on experience and often requires years of accumulated experience to complete. The powered parafoil system is coupled and needs to meet the tuning of three groups of PID controller parameters at the same time. Adjusting the PID parameters by the trial-and-error method is undoubtedly very difficult. Thus, adopting other effective methods is necessary to adjust the PID parameters for improving the control effect.
In recent years, the development of swarm intelligence algorithms has opened up a new way to achieve PID parameter tuning. For example, PSO, genetic algorithm (GA), crowd search algorithm, ABC algorithm [23], and cuckoo search algorithm [24] can quickly implement PID parameter tuning. These algorithms are robust and do not depend on human experience. This study adopts ESPSO to realize the tuning of the PID parameters of the power parafoil system.
The standard PSO has wide adaptability, and it shows good convergence for PID parameter optimization. PSO is an optimization algorithm that simulates bird predation behavior. Compared with other intelligent algorithms, they all initialize a set of random solutions and iteratively search for the optimal solution. The difference is that iterative calculations of PSO follow the principle of survival of the fittest, and uses three simple rules to operate on individual particles, namely, flying away from the nearest individual to avoid collisions, flying to the target, and flying to the center of the group. PSO expresses each possible solution as a particle in the group. Each particle has its own position vector, velocity vector, and a fitness determined by the objective function. All particles fly at a certain speed in the search space, and the global optimal value is determined by following the currently searched optimal value.
PSO assumes that a group of m particles exist in an S-dimensional target search space, and the i-th particle is represented as a vector in the S-dimensional space. The particle position and flight speed are expressed as The position of the particle represents the potential solution, and the fitness can be obtained by input the information of the particle into the objective function. The pros and cons of the solution can be evaluated according to the fitness. The optimal position searched by the i-th particle so far and the optimal position searched by the entire particle swarm are recorded as: − → P is = (P is , P is , · · · , P is ) − → P gs = P gs , P gs , · · · , P gs .
Assuming the objective function is f (x), the current optimal position of particle i is determined by the following formula: The operation of the particles is based on the following formula: where ω is the inertia weight, which controls the effect of the previous speed on the current speed. c 1 and c 2 are the VOLUME 8, 2020 learning factors, which are non-negative constants. r 1 and r 2 are independent pseudo-random numbers, which are subject to uniform distribution on [0, 1].
For the trajectory tracking control of the powered parafoil system, the fitness function is defined as where L xy i and H error i are the horizontal error and longitudinal height error of the i-th sample point, respectively. Then, the fitness function actually means the total error of each trajectory tracking simulation process. Therefore, the constructed PSO algorithm is PSO_PID = (Fitness, N , c 1 , c 2 where N is the dimension, MaxDT is the maximum iteration step, and MinFit is the predetermined minimum adaptation threshold. Lower and Upper are the lower and upper bounds of the PID parameters, respectively. By combining the laws of the natural ecosystem with the standard PSO algorithm, Liu et al. [25] proposed an ESPSO algorithm. The algorithm adopts three mechanisms, called the ecosystem mechanism, the reproduction and mutation mechanism and the full-information mechanism, respectively.
A circular food chain structure is constructed to increase species diversity in the ecosystem mechanism. The particle swarm is divided into sn populations. Each population can prey on another population, and at the same time it will be preyed by another population. It will be k individuals with the best fitness in each population as advertised, and the rest of the particles in the population will randomly select one of them for learning. At the same time, in order to avoid predation of a population, the advertised in the population will learn from the best dimension of the individual in the predator with a certain probability P c . The learning strategy is as follows: where fit s is the fitness of the species, fit predator is the fitness of the predator, and fit max and fit min are the best and worst fitness of the entire species, respectively. The mechanism of reproduction and mutation is similar to the genetic and mutation operations in the GA. The reproduction and mutation in ESPSO work in the same population, which increases the randomness of the particles and avoids falling into the local optimum. It will undergo reproduction and mutation when a random number between 0 and 1 is less than the reproduction probability P b , as follows: where, i is the current particle, j is a random individual particle. Upper and Lower are the upper and lower bounds of N -dimensional space, respectively. pbest i will be replaced by x N reproduction when the fitness of x N reproduction is better than pbest i .
The full-information mechanism is mainly used for information exchange between different species. The species will move closer to a certain best point as the iteration progresses. The Adding of the full-information strategy can make full use of the information in different species and share it among different populations. The strategies for this problem are: where M is the number of particles in the neighborhood of the current particle, exemplar is the advertised particles which be selected. In the ecosystem particle swarm, once xbest is determined, xbest and C i will remain unchanged within a certain evolutionary generation t, which is called the number of stagnations. The iterative formula of the particle flight speed of the algorithm is: Among these three strategies, the ecosystem mechanism is the dominant strategy, and the other two mechanisms are supplemented. The reproduction and mutation mechanism can prevent the algorithm from falling into the local optimum. The full-information strategy improves the optimization efficiency of the algorithm to a certain extent. Therefore, the constructed ESPSO algorithm is ESPSO_PID = (Fitness, N , c, w, k, P b , sn MaxDT , MinFit, Lower, Upper), (62) The algorithm terminates when the maximum iteration step has been reached or the optimal position searched by the particle swarm has met the preset minimum adaptation threshold. The algorithm of parameter setting of the powered parafoil system PID controller based on the PSO or ESPSO is shown in Fig. 5.   The position of each particle in the particle swarm represents the parameters of the PID controller. The generated PID parameters are passed to the powered parafoil control system for conducting the simulation experiments of the trajectory tracking control. The parameters are optimized through iterations. When the fitness of particles gradually decreases and converges to a certain value which means that the trajectory tracking error is convergent. The position of the globally optimal particle can be seen as the optimal parameters of the PID controller if the accuracy meets the requirements. Then, the tuning of the PID controller is completed.

IV. SIMULATION AND ANALYSIS
The physical and aerodynamic parameters of the 6-DOF powered parafoil model in this study are shown in Tables 1 and 2, respectively. The parameters are from relevant literature records and can be adjusted according to actual data. The appropriate control law parameters need to be selected to unify the dimensions of position and angle errors. The weight coefficient determines the size of the role of each error in the trajectory tracking control process. For example, the tracking error can be eliminated quickly by increasing the weight coefficient of position error, or the stability of system motion direction can be improved by increasing the weight coefficient of angle error. The weight coefficient cannot be increased without limit, otherwise the stability of the system will be destroyed. Thus, the selected control law parameters are shown in Table 3.
At present, the mainstream homing methods of the parafoil system include the multiphase homing and the optimal control homing. The trajectory of the multiphase homing is a combination of circular arcs and straight lines. The multiphase homing trajectory must meet the accuracy requirements of the landing point, and the course is against the wind at the end of the homing process [28], [29]. In addition to the two constraints of the multiphase homing, the optimal control homing must satisfy the minimum energy consumption of the control. With the help of the optimization algorithm and the optimal control algorithm, the trajectory planning of the multiphase homing and the optimal control homing can be achieved. In this study, the GA and Gaussian pseudospectrum method are used to realize the trajectory planning of the multiphase homing and the optimal control homing. Given that trajectory planning is not the focus of this research, it will not be described in detail here.
According to the 3D trajectory tracking control algorithm introduced in Section III.B, the planning and discretization of the trajectory are needed to obtain a series of trajectory reference points arranged in sequence. The initial point coordinate (1000, 800, 2000) is set. The target point is the origin of the geodetic coordinate system. The initial course angle is 60 • . The course in the target point is to point to the negative direction of the X e -axis of the geodetic coordinate system (against the wind). The glide slope angle is −25 • . The basic reference forward speed V 0 is set to 10 m/s according to the actual condition. After optimization and discretization of the continuous ideal trajectory, an ideal trajectory which made up of a series of reference points arranged in sequence is obtained, as shown in Fig. 6. Fig. 6 shows the ideal trajectory reference points obtained after algorithm optimization and discretization. Fig. 6 (a) and Fig. 6 (b) are the top and the 3D views, respectively. In the figure, the trajectory composed of red dots is the ideal trajectory of the multiphase homing, and the trajectory composed of blue dots is the ideal trajectory of the optimal control homing. VOLUME 8, 2020 FIGURE 6. Ideal trajectory. The curve formed by the red dots is the ideal trajectory of the multiphase homing, and the curve formed by the blue dots is the ideal trajectory of the optimal control homing.
In Section III.C, the ESPSO-based PID parameter tuning algorithm has been introduced. The learning factor c 1 is the step size which adjust the particle to fly to its own best position, and c 2 is the step size which adjust the particle to fly to the global best position, in the ESPSO algorithm. The dynamic constant ω determines the influence of the previous speed on the current speed. The influence of the previous speed on the current speed is great, and the global search ability is strong, when ω is large. Otherwise, the influence of the previous speed on the current speed is small, and the local search ability is strong, when ω is small. To jump out of the local minimum by adjusting the size of ω. The roughly range of PID controller parameters can be determined by the trial-and-error method. On the basis of experience, we set the dimension N = 9 (three PID controllers, a total of nine parameters), the number of particles is 50, the number of populations sn = 5, the number of advertised individuals k = 4, the reproduction probability P b = 0.3, learning factor c 1 = c 2 = 2, the dynamic constant ω = 0.6, the maximum iteration step MaxDT = 100, the minimum adaptation threshold MinFit = 0, the lower bounds Lower = 0, and the upper bounds Upper = 50. The simulation effects are shown in Fig. 7. Fig. 7 shows that the value of the fitness function decreases continuously and achieve convergence eventually as the   Table. number of iterations increases. The ESPSO converged to the minimum when iterated to 14th time, while the PSO converged when iterated to 47th time. Thus, the ESPSO has faster solving speed than the PSO. From the final optimization results, the ESPSO is slightly better than the PSO, which indicating that ESPSO has better global optimization capabilities. The final optimization result of the PID controller parameters are shown in Table 4.
Next, the simulation of trajectory tracking control of the powered parafoil system is performed. The initial state (925, 863, 2079), speed V = 9 m/s, course angle ϕ 0 =30 • , and glide slope angle γ 0 = −24 • of the parafoil is set. A random gust wind disturbance with a mean value of 0 and a mean square error of 1 appeared between 180 and 230 seconds, and superimpose it on the velocity of the powered parafoil system. The simulation time is set to 600 seconds, and the simulation step size is set to 0.01 seconds. The simulation effects using the two homing methods are shown in Fig. 8. Fig. 8 shows that the control effect is relatively ideal in the case of deviations in the initial position, speed, course, and glide slope angle of the powered parafoil system, under random wind disturbance. Under the action of the controller, the system may adopt either the multiphase homing or the optimal control homing. The powered parafoil system realizes the tracking of the reference trajectory with high accuracy. Given that the reference trajectory of the optimal control homing is relatively smooth, the trajectory tracking effect which using the optimal control homing is better than that using the multiphase homing.
The control input is shown in Fig. 9. The flaps deflection control inputs often saturate in the multiphase homing process. However, the flaps deflection control inputs are small in the optimal control homing process, and no saturation occurs. The input of thrust in the optimal control homing process is also slightly smaller than that in the multiphase homing process.
The trajectory error and velocity change curve in the trajectory tracking process are shown in Fig. 10. Fig. 10(a) and Fig. 10(b) intuitively show that the trajectory tracking error is large with the multiphase homing. Significant jump is observed given that the curvature of the ideal trajectory in several transition stages is large. However, the trajectory tracking error is small when the optimal control homing method is adopted. No obvious jump is observed given the smooth reference trajectory. Fig. 10(c) shows the speed of powered parafoil system. It can be clearly seen that the parafoil speed fluctuates greatly, and the lateral offset and flightaltitude deviation increase accordingly between 180 and 230 seconds due to random wind disturbance. The trajectory tracking accuracy is high, and the parafoil system is not out of control even if there is random gust wind disturbance. It shows that the algorithm proposed in this article has high tracking accuracy and good robustness.
To verify the reliability of the trajectory tracking control algorithm proposed in this study, six groups of different initial  states of the powered parafoil system are selected to perform trajectory tracking control simulations. Random wind disturbance is the same as the previous simulation experiments. The initial states of each case are shown in Table 5.
The results of the simulation experiments are shown in Fig. 11. It can be seen from Fig. 11 that the 3D trajectory tracking control strategy proposed in this study has achieved a relatively ideal trajectory tracking effect under different initial state and homing methods. The simulation experiments have fully proven the effectiveness and reliability of the 3D trajectory control algorithm proposed in this study.   The following comparative simulation experiments are conducted, that is, to conduct experiments in the cases of with or without the velocity control channel, to explain the role of the velocity controller. The results of the experimental are shown in Fig. 12.
The multiphase homing causes a large trajectory tracking error given the curvature of the ideal trajectory is large in several transition stages. After the velocity controller is introduced, the tracking accuracy is effectively improved, especially in several transition stages of the ideal trajectory. The influence mechanism of the velocity controller on the trajectory tracking of the system is mainly manifested in two aspects. First, when the actual position of the system has deviated far from the ideal trajectory, the velocity controller controls the powered parafoil system decelerate, which allows the parafoil to have sufficient reaction time to respond to the control inputs and adjust the course. Then, the powered parafoil system retracks to the ideal trajectory within a short distance. Second, when the powered parafoil system is about to deviate from the ideal trajectory because of the external disturbances or the sharp turns, the velocity controller controls the powered parafoil system to slow down, it can effectively suppress the increase of the trajectory error. When the track error is small, the velocity controller can control the powered parafoil system accelerate and fly at a high speed. This allows the parafoil to reach the target point as soon as possible.

V. CONCLUSION
In this study, we investigate a powered parafoil system. A 6-DOF model of the powered parafoil system is established as the basis of the research to explore the trajectory tracking control of the powered parafoil system. The limitations of traditional guidance-based trajectory tracking strategy are analyzed. A new trajectory tracking strategy on the basis of reference point switching is proposed, that is, discretizing the continuous ideal trajectory, and obtain a series of trajectory reference points arranged in sequence. Then, realizing the trajectory tracking control by switching reference points continuously. This trajectory tracking strategy avoids the difficulty in selecting the scalar variable µ of the traditional guidancebased trajectory tracking strategy, and it is more in line with the needs of actual engineering application. The lateral, longitudinal, and velocity controllers are designed according to the dynamic characteristics of the powered parafoil system. The differential flaps deflection control input is used to control the course of the powered parafoil system to correct the horizontal error. The symmetric flaps deflection control input is used to control the powered parafoil velocity for suppressing trajectory tracking error. Then, the longitudinal height error is corrected by controlling the thrust. The control inputs of the powered parafoil is coupling and nonlinear, and the PID controller parameter is not easy to tune. Thus, the ESPSO algorithm is adopted to achieve the tuning of the PID controller parameter. The ESPSO algorithm has faster solving speed and optimization accuracy compared with the PSO. Finally, the effectiveness and reliability of the 3D trajectory tracking control algorithm of the powered parafoil system proposed in this study is verified by simulation experiments. It is suitable for the multiphase homing and the optimal control homing, and has good trajectory tracking control effects.
There are some limitations in this study. First, the main purpose of this research is to provide a tracking strategy that can overcome the limitations of the traditional guidancebased trajectory tracking strategy and is suitable for complex trajectory tracking. Thus, we discretize the parafoil trajectory to obtain ordered trajectory reference points, and achieve the trajectory tracking by switching the reference points. Based on this idea, and to reducing the difficulty of engineering realization, we use a combination of distance and angle errors to design the PID controller. The PID controller is only adopted to verify the effectiveness of the trajectory tracking strategy proposed in this article. Thus, the trajectory tracking accuracy may not be very high. In the future, the further theoretical research will be carried out in the controller design to design more complex and powerful controllers to obtain higher control accuracy. For example, the feedback linearization control and the sliding mode control strategies can be adopted to improve the trajectory tracking accuracy. Second, the algorithm of the PID controller parameter in this study takes a little long time given the number of PID controller parameters is large and the powered parafoil system is nonlinear and coupling. The improving of the algorithm speed will be further studied in the future. Last, the influence of obstacles such as mountains is not considered in this study. In the future, the trajectory tracking control of the powered parafoil system in complex environments will be further studied.
The trajectory tracking strategy on the basis of trajectory reference point switching proposed in this article has a broad vision of application, it is not only suitable for trajectory tracking control of the powered parafoil system, but also has broad application prospects in the field of driverless and flight vehicle navigation.
MIN YAO received the B.Sc., M.Sc., and Ph.D. degrees from the Nanjing University of Aeronautics and Astronautics, China, in 1997China, in , 2002China, in , and 2008, respectively. She is currently an Associate Professor with the Nanjing University of Aeronautics and Astronautics. Her research interests include computer measurement, control and UAVs task assignment, data and signal processing, and algorithm optimization.
QI CHEN received the B.S. degree in communication engineering from the Xi'an University of Architecture and Technology, the master's degree in control theory and control engineering from Jiangnan University, and the Ph.D. degree from the College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing, China, in 2019. His main research interests include multi-parafoils trajectories planning and collaborative control, multi-agents cooperative control, and so on.
RUIPENG GUO received the Ph.D. degree in instrument science and technology from Shanghai Jiao Tong University, Shanghai, China, in 2011. She is currently an Associate Professor with the Nanjing University of Aeronautics and Astronautics. Her research interests include non-destructive testing, computer measurement, and control and signal processing.
TONG SUN was born in Huaian, Jiangsu, China, in 1995. He received the B.Sc. degree from Yangzhou University, China, in 2014. He is currently a Graduate Student with the Nanjing University of Aeronautics and Astronautics. His research interests include positron nondestructive testing, algorithm optimization, and image processing.
TAO JIANG was born in Anqing, Anhui, China, in 1995. He received the B.Sc. degree from the Beijing Information Science & Technology University, China, in 2014. He is currently a Graduate Student with the Nanjing University of Aeronautics and Astronautics. His research interests include positron nondestructive testing, algorithm optimization, and image processing.
ZENGHAO ZHAO received the B.Sc. degree from Jinling University of Science and Technology, in 2018. He is currently a Graduate Student with the Nanjing University of Aeronautics and Astronautics. His main research interests include detection technology and signal processing. VOLUME 8, 2020