Trajectory Tracking of Quadrotor Aerial Robot Using Improved Dynamic Inversion Method

This paper presents trajectory tracking control works concerning quadrotor aerial robot with rigid cross structure. The quadrotor consists of four propellers which are two paired clockwise rotate and anticlockwise rotate. A nonlinear dynamic model of the quadrotor is provided, and a controller based on the improved dynamic inverse is synthesized for the purpose of stabilization and trajectory tracking. The proposed control strategy has been tested in simulation that can balance the deviation of model inaccuracy well.


Introduction
Since the beginning of 20 th century, many research efforts have been done to create effective flying machines with improved performance and capabilities.The quadrotor aerial robot is emerging as a kind of popular platform for unmanned aerial vehicle (UAV) research due to its simple structure, hovering ability and vertical take-off and landing (VTOL) capability.It is especially suitable for reconnaissance and surveillance missions in complex and tough environment.Because of quadrotor's complexity of dynamic and uncertainty of parameters, it is usually chosen as a testbed for validating the advanced control algorithm of nonlinear control.
The quadrotor is described as aero robot in [1,2].The core problems of quadrotor are attitude control and trajectory tracking control.The flight control unit and its control programming code are priority to its structure when treated as a testbed for control algorithm.With the emerging of open code project for autonomous control, it's more convenient to transplant the advanced control algorithms from modeling and simulation to validating test, which also provide great help to research enthusiast or aerial robot hobbyist.In [3], the differential flat control and nonlinear inverse control are proposed to process the nonlinear trajectory control for differential flatness property system.In [4], an real-time nonlinear optimal control, which is called D   technique, is employed to solve the resultant challenging problem considering the full nonlinear dynamics without gaining scheduling techniques and timescale separations, which leads to a closedform suboptimal control law.In [5,6], the back-stepping procedure is synthesized for the purposes of stabilization and trajectory tracking, which is generally robustness applied to the system of parameters uncertainty and unmodeled dynamic to some extent.The various types of aerial robot UAV based on quadrotor concept are put forward in [5][6][7][8], using the superiority of fix-wing plant and rotor-wing helicopter to enhance the VTOL ability and flying range.So, in general, the dynamic inverse method is more suitable for various types of aerial robots due to its start from original model.In [9], the dynamic inverse control with zero dynamic for non-linear affine model system is proposed.In [10], the hierarchical flight control is synthesis for rotorcraft-based UAV.In present paper, the hierarchical structure of improved dynamic inverse method for quadrotor trajectory tracking is proposed, which can balance the deviation of model inaccuracy to enhance the robustness of control system.
In this paper, we first describe the mechanism of quadrotor aerial robot in section 2. Next, we establish the dynamic model of quadrotor obtained from Newton-Euler formula in section 3.In secion 4, a hierarchical structure of improved dynamic inversion method is proposed.Furthermore, robustness with respect to parameters uncertainty and unmodeled dynamic has been observed in simulation.Finally, a trajectory tracking exam-ple is simulated and remarked.

Quadrotor Mechanism
The typical quadrotor mechanism and coordinate system is briefly showed in Figure 1, where four rotors are cross structure, providing the three required moments (roll, pith and yaw) and lifting force.For describing the flight condition of quadrotor exactly, we can respectively define flat-earth frame E and rigid body frame B according to right-hand rule.
The aerodynamic forces and moments of propeller depend on various parameters, such as the angle of attack, aerofoil and inflow ratio.We assumed that the aerodynamic forces i F and moments i T of four propellers are proportional to the square of angular velocity i  .Which is given by: where .The 1, 2, 3, 4 i  , f t k k is respectively aerodynamic force and anti-torque coefficient.Since the forces and moments are derived by modifying the speed of each rotor, then the lifting force and three moments can be given as follows: Lifting force (the sum of four rotor forces): Roll torque (right-hand rule): Pitch torque (right -hand rule): Yaw torque (right -hand rule): where L is the distance from motor position to the center of quadrotor mass.Rewrite the matrix form of Equations ( 2)-( 5) as follows:

Dynamic Model of Quadrotor
The frame E stands for earth coordinate system, and the body frame B stands for the rigid quadrotor coordinate system.The body coordinate B is attached to the center of mass of the quadrotor.The Euler angles which denote the rotation of quadrotor in the flat-earth frame coordinate E is the variables  ,  and  ,which respect- tively represents the roll, pitch and yaw angles of the quadrotor in the body-fixed coordinates.

Six DOF Model of Rigid Body
The dynamics of a rigid body under external forces applied to the center of mass expressed in the body fixed frame are in Newton-Euler formalism: where the is the velocity in the body frame, is the angular velocity in the body frame.
denote the external propeller forces acted on the rotors, the weight forces of mass center, the elastic force of landing gear only on the ground, respectively.
are the inertia matrix of quadrotor, m is the mass of quadrotor.
The 3 × 3 symmetrical inertia matrix I is defined as: and the cross product operator × is defined as : The external propeller forces b F acted on the rotors is defined as: The weight forces of mass center is as follows: where s and c are shorthand form of cosine and sine.The elastic force of landing gear on the ground is modeling the characteristics of taking off situation for quadrotor, so it disappears while flying or leaving the ground.It is modeling as follows: where l and l is elastic coefficient and elastic deformation on ground.

k z
And the function denotes: Then the Equation ( 7) can be expanded as follows:

Navigation Equations
To obtain the translational and rotational motions from the body frame to the flat earth coordinate system, the well-known navigation equations are described as follows: where the vector stand for the positions of quadrotor in flat earth coordinate, the vector stand for the attitude angles of body frame in the flat earth coordinate.The matrix R and S is translational and rotational transform matrix.The Equation (16) can be extended as follows: where s, c and t stand for sine, cosine and tangent, respectively.

Motor Dynamics
s proportional to the input voltage which is regulated by speed control unit.Input voltage signals rol system in the light of flight The revolving speed of propeller driven by the brushless DC-motor i come from the flight cont task.Assumed that the load anti-torque is proportional to the square of revolving speed and the inductance of brushless DC-motor is omitted, the dynamic model of DC-motor is described in following formula: where the coefficient 1 k , 2 k and 3 k is constant in dynamic model, ic  is the regulated signal pwm from the flight contro e e rom one waypoint to the next waypoint with desired velocity and the segments are connected l unit and the ith is the index of DC-motor.

Improved Dynamic Inverse Controller
The above airfram dynamics of typical quadrotor involves Equations ( 14), ( 15), ( 17)-( 19), including 6 equations in Equations ( 14) and (15) stand for airfram rigid-body dynamics, 6 equations in Equations ( 17) and (18) stand for navigation transformation, 4 equations in Equations ( 19) stand for motor dynamics.In these equations describe the dynamics of quadrotor, the total state variables include 16 state variables and 4 control input variables.The inherent characteristics of strong coupling in pitch-roll-yaw and underactuation are shown in state equations.For solving the coupling in pitch-roll-yaw, the dynamic inverse controller is applied to the controller.In parallel, the hierarchical structure of slow-loop and fast-loop is adopted to solve the underactuation.Nevertheless, the accurate model which is necessary in dynamic inverse method is hardly to acquire, so it's important to develop an improved dynamic inverse model to enhance the robustness of controller.The overall controller depicted in Figure 2 has fast loop as attitude control loop and slow loop as trajectory tracking loop.The details of improving dynamic inverse of hierarchical structure will be described in following process.

Slow Loop Control
The desired trajectory can be divided into sequence of segments with waypoints.So quadrotor tracks f q s c c c r acceleration constraints, then hich depends on the response of trans-2 Fast loop is attitude control loop which is required to response the change of attitude ang quency which settled by system dynamics and control unit is 50 Hz.sformed as follows: A given trajectory command signal include position command   c c c x y z and orientation command c  .
We assumed that the pitch and roll angle are s en 1 can be den mall ough to combine the formula ( 14) and ( 18).Then, symbol inv oted by the relationship of attitude angle and position derivative as follows: Trajectory control is considered to e lize the waypoint position error.Then the desired acceleration command would satisfy the relationship as follo is the damping factor and natural frequency of syst m, respectively.The factor and .From and attitude control, osition control of sl uations (20) and ( 21) ab e, the control problem of quadrotor is addressed by decoupling the position control and p ow loop provides the attitude set points for the attitude controller of fast loop.We have testified that the desired system dynamics could be perfect when damping factor is constant 0.4 -0.8.Only if the natural frequency of system required to aware, the controller factor can be quantified as follows: where the matrix inverse stands for symbol inv2 acquire In Equation ( 24), the second item is intentionally omitted in order to maintain zero dynamics while angular velocity error is zero.Thus, the model error or unmodel dynamic characteristics in dynamic inverse method can be balanced through improving the dynamic inverse model.Though the proposed model of dynamic inverse is unaccurate, the zero dynamics can be kept even if the model is not valid.We can acquire   The factor is gained in the same formula.

Motor Speed Control
Equation ( 6) can be transform as follows: where the square root stands for symbol inv3.

n Resu
the demonstrating results are presented in performances of proposed control ngle shape traparameters used for the n in Table 1.These parameters are referred to the quadrotor prototype designed by our lab, just refer Figure 3.

Simulatio lts
In this section, order to observe the principle.We considered a case of a recta jectory-tracking problem.ate the trajectory tra erforman rol strategy developed in this paper.The modeling difference between plant model and dynamic inverse controller include two aspects: no motor dynamic model in controller and 10% deviation of quadrotor body inertia and mass.But we can see the proposed controller behaves perfectly in Figure 4, except for the altitude due to quadrotor mass error.

Conclusion
The analysis presented here has show

Figure 2 .
Figure 2. Improved dynamic inverse controller of hierarchical structure for quadrotor.toapproximate the continuous trajectory well.The slow loop frequency w tional motion and position sensor is about 4 Hz.