Research on Trajectory Planning and Tracking of Hexa-copter

In this paper, the flight control problem of hexa-copter is studied in detail from threedimensional trajectory planning to tracking. Then the cubic spline interpolation method is used to generate the trajectory by using these time marked waypoints. The flight trajectory curve produced by this method is smooth, twice differentiable, and it is easy to control implementation. The flight dynamics model of the UAV has the characteristics of multi-input multi-output, strong coupling, under-actuation, severe nonlinearity and external environmental disturbance. In order to improve the accuracy of flight trajectory and the stability of attitude control, a multi-loop sliding mode variable structure control method is proposed to achieve the hexa-copter flight trajectory tracking. The simulation results show that this method can track the predetermined flight trajectory and keep the attitude stability of the UAV normally.


Introduction
The hexa-copter is a kind of multi rotor small-size UAV.Compared with the quad-rotor UAV, the hexa-copter added a pair of rotor, as shown in Figure 1.It has better load capacity, flight stability and motion control performance [1].Therefore, the hexa-copter has been more and more applied in the field of aerial photography, logistics and environmental monitoring.In this paper, the flight control problem of the hexa-copter is focused.First, the flight trajectory planning of UAV is carried out, and then this trajectory is used for tracking control.Because the flight dynamics model of the hexa-copter has the characteristics of serious nonlinearity [2] and external environmental interference [3].A multi loop adaptive sliding mode variable structure control method is used to track the flight trajectory.

Flight trajectory planning
In order to complete the flight trajectory planning, firstly it is necessary to determine the track points that the UAV needs to pass.These track points also known as waypoints.After a series of waypoints are determined, the trajectory can be completed by the interpolation method.
Here, the number of waypoints is N, Then the i-th control point is recorded as Pci.

 
The real flight trajectory is a function that changes with time, so each waypoint needs a corresponding time mark t i .At t i time, the UAV should be located in the P ci position.As a result, the trajectory planning problem can be described as: Known N interpolation nodes t 0 , t 1 ,…t N- 1 , solving the three interpolation functions of the corresponding function value of x c0 , x c1 , … , x cN-1 ; y c0 , y c1 , … , y cN-1 ; z c0 , z c1 , … , z cN-1 .In this way, the trajectory planning problem is transformed into a function interpolation problem.
In order to facilitate the implementation of the control algorithm, the trajectory is as smooth as possible and twice differentiable.Linear interpolation, two interpolation and Hermite interpolation cannot satisfy the above two conditions at the same time, while the cubic spline interpolation method can satisfy them.Therefore, the cubic spline interpolation method is used to solve the interpolation function.The detailed implementation process can be referred to the literature [8].

Trajectory tracking based on multi loop nonlinear control method
The center of mass position vector of UAV in the inertial coordinate system is recorded as Pe, and the rotational attitude angle is recorded as Θ, and the rotational angular velocity in the body axis system is ωb.x y z In some literatures [7], when the attitude angle of UAV is not large, it is often assumed that the change rate of the attitude angle is equal to the angular velocity of the body.
In order to improve the accuracy and robustness of the flight control system, it is necessary to consider the difference between the change rate of the attitude angle and the angular velocity of the body axis system in the actual flight.In this paper, a multi loop method is proposed to design the control rate.The position subsystem is the outside ring, and the attitude subsystem is divided into the inner and outer ring.The position subsystem controller located in the outer loop uses the virtual sliding mode control input to estimate the external disturbance adaptively.It produces two intermediate instruction signals φd and θd, and pass to the attitude subsystem.The outer ring controller corresponding to the attitude subsystem produces the intermediate signal ωbd.The inner loop controller of the attitude subsystem continues to produce the final control signal Γ.In the case of no accurate UAV rotational inertia data, the tracking of all instruction signals is realized by designing the sliding mode control rate of the loop.

Attitude outer loop controller
Position controller

The Position Dynamics Model in the Inertial Coordinate System
The dynamic equation of the UAV in the inertial coordinate system [6] Here, m is the total mass of the UAV; g is the acceleration of gravity; dF represents the interference force; e3 is a vertical unit vector, e3=[0 0 1] T ; U1 represents the total lift of the six propellers.
In the upper form, the k represents the thrust factor of the propeller, and Ωi represents the revolution speed of the i-th propeller.Here, the total lift U1 is used as the input of the control quantity.

Design of Position Tracking Control System
The reference location that needs to be tracked is Ped.The tracking error is eP= Pe -Ped.The error equation of the position subsystem is UP is a virtual input to be designed.The error equation of the position system is The subsystem of the corresponding Lyapunov function is Corresponding derivative is In order to guarantee the V • 1<0, the index approach rate is adopted.
The expression of the corresponding control amount UP can be written as ( ) sgn( ) Write UP as a vector UP=[Ux Uy Uz] T , write sin to S, cos simply as C, and then.
The attitude instruction signal including the pitch angle instruction signal and the yaw angle instruction signal is obtained, as follows Finally, the corresponding control lift is designed to be

Attitude Control System
According to the characteristics of the attitude motion model, the attitude control system is subdivided into the attitude outer loop control system and the attitude inner loop control system.

Attitude Dynamics Model
The equation of attitude dynamics in the coordinate system of the body is Among them, J is the moment of inertia of the UAV; Ga is gyroscopic moment; dΓ is the disturbance moment; Γ is the control moment.Hexa-copter is axial symmetry, and its rotational inertia can be expressed as 00 00 00 The control torque can be expressed as [4][5]: (19) Among them, b is the rolling torque coefficient of the propeller, and the l is the length of the rotor arm.In the upper form, the interference force is expressed as dF=[dx dy dz] T , and the disturbance torque can be expressed as dΓ=[dψ dθ dφ] T , representing interference force and disturbance torque of representative airflow on UAV.The control torque Γ is used as the input of the control quantity.

Attitude Outer Loop Subsystem
After determining the attitude angle control target Θd in the position tracking system, the task of attitude outer loop subsystem is using the target parameter Θd and object model is given in formula(20), to determine the control amount of ωb.
Attitude angle tracking error eo is The subsystem of the corresponding Lyapunov function is The first derivative of the tracking error and Lyapunov function can be obtained.Here I 3 is the 3 order unit matrix.
Finally, the expression of the control angular velocity of the attitude outer ring can be determined.

Attitude Inner Loop Subsystem
Similarly, after the control target desired angular velocity ωbd of the body coordinate system is determined in the outer rings of the attitude, the task of attitude outer inner subsystem is using the target parameter ωbd and object model is given in formula(17), to determine the control amount of Γ. Angular velocity tracking error ei is  Because the magnitude of the gyroscopic moment Ga is small, it is ignored.

Simulation results
The mass of the UAV is 4.5kg, and the moment of inertia is J=diag[0.0150.015 0.020] kg•m 2 .
Both the interference force and the disturbance moment are set to slow time varying sine or cosine functions, as shown below The position control parameters are set as follows λ 1 = 5I 3 , ε=6, the attitude outer loop control parameters are set as η o = 5I 3 , the attitude inner loop control parameters are set as η i = 4I 3 .Set the waypoints that the UAV needs to pass as shown in Table 1.

t(s) X(m) Y(m) Z(m) t(s) X(m) Y(m) Z(m)
The cubic spline interpolation method is used to interpolate by the above 16 control points, the planned track can be obtained.,as shown in figure 3.   Figure 7 shows the change of the lift and control torque of three directions with time.Figure 8 gives the rotary speed of the six propellers.From the above results, the control amount is slightly buffeting.However, the buffeting phenomenon is expected to decrease and even eliminate by selecting the switching function and adjusting the control parameters properly.

Conclusion
Aiming at the problem of flight control of hexa-copter, this paper proposes a method based on the cubic spline interpolation to complete the flight trajectory planning of UAV, and realizes the tracking the trajectory completed by the multi-loop sliding mode variable structure control method.The simulation results show that the method can well complete the trajectory planning and tracking of hexa-copter.

Fig. 2 .
Fig. 2. Trajectory tracking control block diagram based on multi loop control method.

6 )
MATEC Web of Conferences 173, (2018) https://doi.org/10.1051/matecconf/2018173SMIMA 2018 The relationship between the attitude angle change rate Θ• of the UAV and the rotational angular velocity ωb in the body coordinate system is as follows: of the tracking error and Lyapunov function can be obtained.

Fig. 3 .
Fig. 3. Trajectory planning results using waypoints.The expected yaw angle of the UAV is always 45 degrees, and the simulation time is 16 seconds.The trajectory is tracked with the method of multi loop nonlinear control in the previous paper.The simulation results are shown as shown in the following figure.