Integral Lqr-based 6dof Autonomous Quadrocopter Balancing System Control

—This paper presents an LQR-Based 6DOF control of an unmanned aerial vehicles (UAV), namely a small-scale quadrocopter. Due to its high nonlinearity and a high degree of coupling system, the control of an UAV is very challenging. quadrocopter trajectory tracking in a 3D space is greatly affected by the quadrocopter balancing around its roll-pitch-yaw frame. Lack of precise tracking control about the body frame may result in inaccurate localization with respect to a fixed frame. Thus, the present paper provides a high dynamic control tracking balancing system response. An integral LQR-based controller is proposed to enhance the dynamic system response balancing on roll, pitch and yaw. The control on the hovering angles consists of two-cascaded loops. Namely, an inner loop for the angular speed control of each angular motion around the body frame axes, and an outer loop for the desired position control. In general, the proposed balancing control system on roll, pitch and yaw, has six control loops. The proposed control approach is implemented utilizing an embedded ATMega2560 microcontroller system. Practical results obtained from the proposed control approach exhibits fast and robust control response and high disturbance rejection.


INTRODUCTION
Control of Unmanned Aerial Vehicles, known as UAVs, has been considered as one of the difficult and complicated challenges, especially controlling those that can perform vertical take-off and landing (VTOL).quadrocopters have become very popular recently due to their simple structure design compared with what they can perform even in a complex environment.Moreover, recent development in high density power storage, integrated miniature actuators and MEMS technology sensors have made the autonomous flying robots possible.However, the quadrocopter is a very nonlinear system; that makes the control of this vehicle not easy to be accomplished.
The movement of the vehicle's body frame with respect to the inertial earth frame is controlled by adjusting the angular speed of the propellers properly.Control laws will lead the vehicle to act in a particular manner as desired.Several control methods have been tested on the quadrocopter to stabilize and track control.
In [1] & [2], authors compared between PID, Sliding mode, LQR and Backstepping, for attitude control only for the quadrocopter.Authors in [1], propose an Integral Backstepping for full position control of the quadrocopter system.LQG controller is applied to control the attitude of quadrocopter [3].
Feedback linearization based technique is proposed to deduce the control law for quadrocopter attitude control, [4].In [5], a modified Backstepping approach is proposed to control attitude and position of the quadrocopter, where the main contribution was to reduce the number of gains in the control laws.
In [6], a nonlinear control design is combined with an online parameter estimation to develop the control law, in presence of parameter uncertainties and compared with sliding mode control.In [7], Fuzzy Backstepping Sliding Mode Controllers are designed for quadrocopter.However, this control technique is a Backstepping technique where the error signals were determined as the sliding manifold, moreover, a Fuzzy Controler was added instead of sign function in the control law.
The remaining sections are, section II, which briefs the quadrocopter system description.The quadrocopter LQR Controller is discussed in section III.The dynamic model of quadrocopter is given in section IV.Section V explains the design of LQR optimal control design.Practical implementation and results are given in section VI.Section VII discusses the concluded practical results.www.ijarai.thesai.org

II. QUADROCOPTER SYSTEM DESCRIPTION
The UAV system used for implementation, is shown in Fig. 2. It is aimed to make an autonomous quadrocopter, which can balance itself while flying.To have it done, an Inertial Measurement Unit (IMU) and an altimeter (altitude sensor) are used.The IMU is an MEMS type that measures and reports the quadrocopter's orientation and gravitational forces, using a combination of accelerometer, gyroscope and magnetometer.The altimeter is a fusion sensor that measures the altitude of the quadrocopter.Four BLDC motors, out-runner type, are used.Fig. 2 shows an image of the quadrocopter under investigation in the Lab.The quadrocopter is equipped with an embedded fight controller board, which consists of an ATMega2560 microcontroller and MPU6050 integrated 6-axis motion tracking device, which combines 3-axis gyroscope and 3-axis accelerometer with its dedicated I2C sensor bus.It directly accepts inputs from an external 3-axis compass to provide a complete 9-axis Motion Fusion.Fig. 3 shows the flight controller board.The board also consists of HMC5883L  PROPOSED QUADROCOPTER LQR CONTROL Fig. 4 shows the proposed entire control system for qaudrocopter balancing and localization.The main goal of the paper is to develop an integral LQR controller to enhance the dynamic control response of the quadrocopter with respect to the quadrocopter body frame.As seen from Fig. 4, the proposed control system consists of two control levels.This includes a low-level control (body frame control), and a highlevel control.The main task of the low level control, is to improve the dynamic performance of the quadrocopter on the body frame F B .The low-level control receives the desired command signals,  and  from the high-level control.While, the control law for the orientation angle  considered a high-level control.It is wise mentioning that the goal of the high-level control is to precisely localize the quadrocopter at a desired posture with respect to a fixed earth frame F E [3].

IV. DYNAMIC MODEL OF A QUADROCOPTER
The non-linear dynamic model of the quadrocopter is provided [8].It is of 6DOF system, which includes, the position vector  with respect to a fixed frame and the orientation vector  of the body frame F B relative to a fixed origin of the earth frame F E .Fig. 5 shows the quadrocopter system with its body frame referenced to a fixed frame [9] & [10].
(3) Equation ( 3) represents the joint system state space vector of the 6DOF quadrocopter system, which includes the position and the orientation vectors ( 1) and ( 2).The linear and the angular velocity vectors are given by ( 4) and ( 5), respectively.
The rotation matrix R from body frame F B to earth frame F E , and the transformation matrix for angular speeds from F B to F E , are given by ( 6) and ( 7), respectively.
In which, And the rotation matrix is orthogonal, i.e., R -1 =R T .The physical structure of the quadrocopter is symmetrical about all axes; hence, the inertial matrix is defined as in (8).
When rotor i rotates, it generates a lift force f i , which causes a vertical motion, and an angular torque i around z-axis  Mi as given in ( 9) and (10).
As a result, three forces and one torque affect the quadrocopter body and are determined as given by ( 11), ( 12), ( 13) and (14).

A. Newton-Euler Equations
It is assumed the quadrocopter to be a rigid body; hence, Newton-Euler equations can be used to describe its dynamics.In the body frame F B , the force required for the acceleration of mass B mV  , and the centrifugal force ) ( B mV υ are equal to gravity G R  and the total thrust of the rotors B T , which is defined in (15), the outcome equation is seen in (18).
In F E , the centrifugal force is nullified.Therefore, only the gravitational force and the magnitude and direction of the thrust are contributing in the acceleration of the quadrocopter, as in (19) and (20).
In F B , the angular acceleration of the inertia υ I , the cen- tripetal forces ) (Iυ υ and the gyroscopic forces Γ are equal to the external torque τ , as in ( 21), ( 22) and (23).
In which, Consequently, the resulted equation can be written as in (25).(25)

B. Nonlinear Dynamic Model Simplification
The transformation between ] , , [ for rotational dynamics, is very complex, since it includes many trigonometric functions; therefore, simplification is needed.It is assumed that if perturbations from hover condition are small, body angular rates and rate of change of Euler angles are equal for small values of  and  , the relation between body angular rates and rate of change of Euler angles becomes as in (28) [10].
According to previous, the complete dynamics of the vehicle is described in (29).
LQR OPTIMAL CONTROLLER DESIGN

A. Dynamic Model Linearization
To achieve optimal control algorithm [11] & [12], Linear Quadratic Regular (LQR), the dynamic model, described in (29), must be linearized around a trim condition, which is chosen to be hover condition.
as the state vector of the attitude dynamics whereas, , as the input vector.The state space representation of the dynamics can be given by ) where, To linearize the system given in (30), and from the Jacobian matrices described in (31) and (32), the linearized system will become as in (33).
) , ( 1 To stabilize the quadrocopter, the system state, described in (33), will be divided into subsystems as follows: This model could be optimally stabilized using the classical result of LQR.

B. Optimal stabilization of subsystem 
First, the subsystem in (34) is written in a space state form as seen in (37).
Fig. 6 shows the suggested LQR controller structure, where a feed forward loop has been added to obtain stabilization at steady state, which supports the control law to maintain the desired angle at its commanded value, and improve the system tracking response.The structure of this control regime was implemented to enhance the quadrocopter balancing system on both roll and pitch angles.It consists of LQR and Integral LQR type controllers.Different control approach structure was proposed for the yaw angle control.Fig. 7 explains the structure of the proposed LQR controller.The system given in (37) should be extended to apply the desired controller, and the extended system is shown in (38).

VI. PRACTICAL IMPLEMENTATION AND RESULTS
Practical results obtained from the test bench of the quadrocopter system, built in Lab, were demonstrated to validate the proposed technique.Intensive practical results were obtained, which demonstrated the applicability of the proposed control approach to work in a robust manner with the ability of disturbance rejection.The practical results were presented in three different categories; angular speed and position control response on roll axis; angular speed and position control response on pitch axis; and angular speed and position control response on yaw axis.To prove the stable high performance balancing control systems on roll, pitch and yaw axes, different desired angular position command on each axis was applied.

A. Practical Results on Roll-Axis
This subsection shows the practical results for the balancing control system response on roll axis.It is presumed that there were no motion on the pitch axis i.e., the control balancing system on pitch axis tries to maintain 0      as well as no orientation around yaw axis i.e., 0      .-Black‖ is the command signal, and -Blue‖ is the actual quadrocopter response.As seen the quadrocopter balancing system exhibited fast tracking response and zero steady state error.As noticed at time t=0t=10 sec, the system was balancing at zero roll angle.

B. Practical Results on Pitch-Axis
This section explains the dynamic performance of the quadrocopter balancing for pitch angle control.Fig. 9. Balancing Response on pitch-axis Fig. 9 shows the response of the quadrocopter control for a sinusoidal changes in the desired command angle.As seen from Fig. 9, -black‖ is the command and -blue‖ is the actual system response.There is a very fast command tracking.It is worth mentioning that the peak variation of the quadrocopter around pitch axis is 60 o .Hence, the quadrocopter system exhibites a wavey motion with no lack of stability.Fig. 10 shows the inner angular speed control response.As seen, the integral LQR conroller provides fast angular speed tracking for which the outer position control loop tracks fast the desired position command.Fig. 11 shows the control for the case when controlling the pitch angle, as seen by the angle command profile depicted in Fig. 9.The control law of Fig. 11 is to be sent to the four quadrocopter's rotors, for which stabilization and balancing as desired on pitch axis is achieved.

C. Practical Results on Yaw-Axis
The following section provides the experimental results of the quadrocopter balancing system response for yaw angle control.As seen, Fig. 12 and Fig. 13 show the system control response for the outer position control loop and the inner angular speed control loop respectively.-Black‖ is the command and -blue‖ is the actual system response.As shown, there is a robust control tracking in both the inner and the outer control loops.Fig. 14 shows the LQR control law generated to maitain stable operation and exact yaw angle tracking as it is desired.Fig. 13 shows the test rig of the quadrocopter balancing control system.It has been completely developed by the graduate students in the integration of Mechatronics Systems Lab.As seen, the quadrocopter is linked with a four yellow straps.This is a safety procedure as the quadrocoper is running indoor.This procedure has no effect on the balancing system, as the system is due to work on the body frame angles control and has no motion with respect to the reference fixed earth frame.

VII. CONCLUSION
In this paper, it has been presented a practical implementation of a two-cascaded LQR control loops for position control of the quadrocopter's body frame angles.There are six control loops for the whole quadrocopter balancing control.An LQR and an integral LQR have been proposed for the outer loop (position loop).It has been shown that the proposed control approach, tracks fast the desired commands for roll, pitch and yaw angles in the body frame.It has been also noted that the proposed control approach, exhibits an inherited decoupling control action, for which the control of one axis angle has relieved the dynamic coupling effect on the other two axes.Furthermore, intensive practical results have demonstrated the robustness of the proposed controller.Future work is dedicated for the development of quadrocopter motion control on the Cartesian domain with respect to a fixed frame i.e., high-level control.Referencing to Fig. 4, a vision system assisted-novel SLAM technique is proposed to control and track the localization of the quadrocopter system.Real time implementation of the proposed SLAM technique is done utilizing an Intel® Core TM i5-480M computer machine with a wireless communication system.The communication protocol system is to provide a bi-directional paired communication between the quadrocopter on board control system and the SLAM approach hosted on the computer machine.
The prismatic speed on z unit vector in the body frame v deg/s The angular speed vector in the body frame p deg/s The angular speed on x unit vector in the body frame q deg/s The angular speed on y unit vector in the body frame r deg/s The angular speed on z unit vector in the body frame R None The transformation matrix from the body frame to the earth frame R -1 None The transformation matrix from the earth frame to the body frame 1   W None The transformation matrix of angular speed from the body frame to the earth frame m kg Vehicle flight mass T B m/s Vector of total force acting on vehicle expressed in the body frame G N.m Vector force due to gravity acting on vehicle expressed in the body frame I kg.m 2 Vehicle flight moment of inertia tensor w.r.t.Vector thrust of rotor i expressed in the body fram b N.s 2

Fig. 5 .
Fig. 5. Reference frames F B and F E , and forces and torques generated by the rotors of the quadrocopter 29) www.ijarai.thesai.orgWhereas,

Fig. 6 .
Fig. 6.Proposed optimal controller for roll and pitch angles control

Fig. 7 .
Fig.7.The applied optimal controller to control yaw angle

Fig. 8 .
Fig. 8. Balancing Response on roll-axis Fig. 8 shows the balancing system response on the roll axis.-Black‖ is the command signal, and -Blue‖ is the actual quadrocopter response.As seen the quadrocopter balancing system exhibited fast tracking response and zero steady state error.As noticed at time t=0t=10 sec, the system was balancing at zero roll angle.

TABLE I
3-axis digital magnetometer