Attitude Control of Quadrotor Using PD Plus Feedforward Controller on SO(3)

Received Aug 17, 2017 Revised Dec 8, 2017 Accepted Dec 23, 2017 This paper proposes a simple scheme of Proportional-Derivative (PD) plus Feedforward controller on SO(3) to control the attitude of a quadrotor. This controller only needs the measurement of angular velocity to calculate the exponential coordinates of the rotation matrix. With rotation matrix as an error variable of the controller, the simulation shows that the controller is able to drive the attitude of the quadrotor from hovering condition to desired attitude and from an attitude condition goes to the hovering condition, despite the system is disturbed. When the system is convergent, the rotation error matrix will be a 3x3 identity matrix. Keyword:


INTRODUCTION
A quadrotor is a six DOF Unmanned Aerial Vehicle (UAV) which consists of two pairs of counterrotating rotors. Due to some unique abilities of quadrotor such as high maneuverability (hovering and VTOL), small size, and easy to control, quadrotor has been widely used. The usage area of a quadrotor can be separated into three major parts such as: military operations (security intelligence), public application (SAR), and civil application (photography). In order to improve the ability of quadrotor, some researchers were carry out on several aspect of control, such as path planning [1], position [2], altitude or hovering [3], and attitude [4].
Attitude of quadrotor is an orientation of the body fixed frame with respect to the inertial frame. It depends on the moment and the thrust of the two pairs of motor. By varying the rotor speed, one can change the lift force to create motion. The ability of quadrotor depends on an attitude control of quadrotor. If the attitude control is correct, then the additional task can be added to the system, for example, in the navigation task such as path planning, the orientation of quadrotor is an important to set the accuracy of sensor to detect the obstacle. The set of attitudes of quadrotor (rigid body) is the set of 3x3 orthogonal matrix whose determinant is one as known as Special Orthogonal SO(3) [5].
To control the attitude of quadrotor, many of control algorithms have been developed [6]. Most of them were developed with euler angles representation, which suffer from the problem of singularities for large angle rotational maneuvers [7]- [12]. To avoid singularity in euler angles representation, some works use a quaternion method to represent the attitude of quadrotor [13]- [15]. Although the quaternions do not increase to singularities, they have double cover of the set of attitudes SO (3)  ambiguities) in the sense that each attitude corresponds to two different quaternion vectors. Another work using a combination of Euler angles and unit-quaternion to represent the attitude of the aircraft [16]. One method that can avoid from singularities and ambiguities is a geometric control method, which uses a rotation matrix representation. This representation is global and unique. In geometric control method, the rotation matrix can be developed with an exponential matrix of exponential coordinate of an attitude, which map a vector into a matrix SO(3) (Special orthogonal-3) by summing an infinite series of exponential skew-symmetric matrix. The exponential coordinate is one of the intrinsic properties of Lie group SO (3).
Several studies to control a quadrotor based on exponential coordinate have been already done. In [17], a geometric tracking control system on SO(3) was provided to avoid singularities and ambiguities that are inherent to other attitude representation. This proposed tracking control also show its performance to stabilize the attitude error without the knowledge of the inertia matrix. In [18], a robust adaptive tracking control of the attitude dynamics of a rigid body was proposed. The controller made a tracking for an attitude and angular velocity command without knowledge of the inertia matrix of a rigid body. Both of these researches merely give the attitude tracking approach without considering the control torque.
In [19], the attitude controller for a quadrotor based on exponential coordinate had been already done. The controller has been designed using TLC (trajectory linearisation control). The controller use an exponential coordinates and angular velocity as an error variable for controller. While in [19] uses a vector as an error variable, a rotation matrix error as an error variable is used as an input of the controller in [20]. In [20], a PID algorithm is used and a knowledge of angular velocity error as an input to controller is needed. This work is look like focus in response time of the rotor.
In this paper, a simple PD plus Feedforward controller on SO(3) with a rotation matrix as an error variable for the system is proposed . This controller is simpler than the controller in [20]. This controller uses a proportional-derivative algorithm and only requires the measurements of angular velocity from dynamic of quadrotor to calculate the attitude. This controller can stabilize the attitude of quadrotor in hovering condition and making a motion to a desired attitude.

RESEARCH METHOD
In this section, some mathematical tools will be given for rotation parametrization and some basic knowledge of Lie Group and Lie Algebra.

Mathematics of Rotation in SO(3)
Rotation matrix is a 3x3 matrix that represents the transformation vector from a fixed-body frame to an inertial frame. Rotation matrix is a Lie group which has an algebraic group structure based on matrix multiplication as the group operation. According to the Euler theorem for rotates body, any 3-dimensional rotation of a rigid body can be represented by a rotation of a given axis by an angle. Suppose that is a unit vector that specifying the rotation axis of a rigid body, and is a rotation angle in radians. The position of a point at the rigid body as a function of time is denoted by q(t). If the body is rotated at constant unit velocity about the axis , the velocity of the point can be written as The hat operator maps a vector in to a skew-symmetric matrix, defined as The inverse operation of the hat operator is denoted as a vee operator , which recover a vector from a skew-symmetric matrix ̂ . The skew-symmetric matrix ̂ satisfies ̂ ̂ . All such matrices forms a vector space denoted as so (3): The exponential map transforms a skew-symmetric matrices into an orthogonal matrices. Geometrically, the skew-symmetric matrix corresponds to the axis of rotation (via the mapping ̂, and the exponential map generates the rotation corresponding to the rotation about the axis by a specified amount . So the exponential map will maps a skew-symmetric matrix ̂ to an orthogonal matrix where I is a 3x3 identity matrix, is a rotation angle, and is a rotation axis. The rotation matrix is a three-dimensional special orthogonal matrix that satisfies (5) Equation (4) is an infinite series formula. To obtain a closed-form expression for ̂ , by referred to Rodrigues' Formula, an efficient method for computing ̂ is stated as where [ ] , with ‖ ‖ is the rotation axis, and is a rotation angle. The vector is the exponential coordinates for , therefore, a rotation matrix can be calculated by an exponential matrix of a skew-symmetric matrix of exponential coordinate , . ( The inverse mapping from an orthogonal matrix SO (3) to a skew-symmetric matrix so (3) is done by the logarithmic map which is defined as (8) where is ( ) and | | , and is the sum of the elements on the main diagonal of rotation matrix. If , the rotation axis can be chosen arbitrarily. And if , the rotation axis can be calculated by , that is (9) An illustration of a rotation with an axis and angle of rotation can be seen in Figure 1, where [ ] is a reference configuration, and [ ] is a present configuration. From Figure 1, a reference configuration p will be rotated by an angle to a present configuration t with a rotation axis r.

Quadrotor Model
As shown in Figure 2, the inertial frame and body frame are denoted as E and B. The base of frame B can be represented using the axes of frame E, with coordinates , where is a 3x1 dimensional-vector. In Figure 2, the quadrotor's attitude is described using roll ( ), pitch ( ), and yaw ( ) angle, written as [ ] . The angular velocity is described in body frame axes, denoted as [ ] . The inertia matrix of the quadrotor is as follows: where the indices x, y, and z denote x-, y-, and z-axis in the body frame, respectively. If quadrotor is assumed to be symmetric to its x-and y-axis, the inertia matrix in (10) can be written as The inputs of quadrotor's system are the squared angular speeds of its four rotors, denoted by [ ] . The rotors generate the torque [ ] and thrust T. The torques and thrust are generated by manipulating the angular speed as (12) , where d, b, k are respectively the length of the quadrotor's arm, propeller thrust coefficient, and propeller torque coefficient. The equation of motion of this quadrotor are given by where , is the inertia matrix in the body fixed frame; , is the angular velocity in the body frame; , is the rotation matrix as a transform from the body frame to the inertial frame; , is the control torque generated by the actuator of the robot. The dynamics of quadrotor in (14) can be rewritten as and the quadrotor's kinematics in (13), referred from [21], this differential equation can be rewritten in terms of the angle as where ⁄ ⁄ is a body angular velocity, and is an exponential coordinate of rotation matrix. Therefore, the second order system of quadrotor UAV in SO (3) is defined as

PD Plus Feed-forward Control on SO(3)
Based on the works of Bullo in [22], for a simple first order system on SO(3), a dynamical system with state evolves following where G is a Lie Group matrix, is the configuration of system, and is the body velocity. Assume that the quantity of can be directly controlled to any desired value (i.e. the system is fully actuated). Then the proportional control action is . (19) Now, consider the stabilization problem for second order systems, that is for the system where have full control over forces (accelerations) rather than velocities. A second order system on SO(3) has the form where is the configuration of the system, is the internal drift, and is the control input. To regulate the configuration g to the identity matrix , the proportional action will be coupled with a derivative term, i.e. with a term proportional to the velocity . Let and be symmetric, positive definite gains. Then the control law is , (21) exponentially stabilizes the state g at from any initial condition ( ) and for all and such that where is the minimum eigenvalue of . Now, if the system is assumed as a quadrotor. And than, from the formulation above, if is replaced with , with , and with , then the control law for quadrotor is , where and are proportional and derivative constant, respectively. With this error term, if the is exponential coordinate of a rotation matrix , the Equation (24) should be written as (25) The block diagram of this controller is shown in Figure 3. From the block diagram of controller, the difference between rotation target matrix and current rotation matrix will be calculated in multiplication of matrix to get an error rotation matrix. With this multiply operation, the result is still in the form of a rotation matrix, which satisfy and . The rotation current matrix is calculated with the given angular velocity from dynamic model. From [21], given body angular velocity , the which consist of exponential coordinate of rotation matrix can be calculated using (7).

RESULTS AND ANALYSIS
Here, the controller was simulated in 3 cases. In the first case, the controller is used to drive the attitude from an attitude condition to hovering condition. In the second case, the controller is used to control the attitude from hovering condition to a desired attitude. And in the last case, the simulation shows the ability of the controller to compensate an attitude disturbance.
The initial and target of rotation matrix can be an any 3x3 matrix, where the matrix satisfies the condition in (5). The controller was simulated in 6 DOF euler angles with model parameter of quadrotor are created by Corke in [23], i.e. .

Case 1
In this case, the simulation will show the performance of controller to drive an attitude toward a hovering condition ( ). In hovering condition, will be a identity matrix. In this case, the initial attitude of quadrotor is The error of attitude can be calculated by multiplying the transpose matrix of desired attitude with the initial attitude of quadrotor, that is The exponential coordinates of the error of attitude become an input to the controller, to calculate the required torque for the system to achieve the desired attitude. The result of the simulation is shown in Figure 4. In Figure 4b, the exponential coordinate of this case becomes [ ] when the system is convergent. This exponential coordinates vector will be transformed into a skew-symmetric matrix by operator , and the attitude of this condition can be calculated by exponential matrix of the skew-symmetric matrix of exponential coordinates as shown in (7).
The condition describes that the quadrotor has no rotation or in hovering condition, the same as the rotation target in this case. The rotation angle is 0 rad as shown in Figure 4a. In this condition, while the attitude , the rotation axis can be chosen arbitrarily. When the system meets convergence, the angular velocity for all axis are [ ] as shown in Figure 4c. The steady state in this system is reached in 3 sec with the rise time is about 1 sec.

Case 2
In the second case, the simulation will show the performance of controller to control the quadrotor's attitude from hovering condition to a desired attitude. In this case, the initial attitude is a identity matrix and the desired attitude is

[ ]
The error of attitude in this second case can be calculated by multiplying the transpose matrix of desired attitude with the initial attitude of quadrotor , that is The result of the second case is shown in Figure 5. When the system is convergent, the value of exponential coordinate are [ ] as shown in Figure 5b, and the attitude can be calculated by the same way as shown in Case 1, that is As shown in Figure 5a and the value of , the rotation axis can be calculated by (9), that is [ ] . This means that the quadrotor is rotated with this rotation axis, and rotates about 1.287 rad (73.4 degree). The value of angular velocity in Figure 5c indicates that there is no alteration of rotation angle when the system is convergent.

Case 3
In the third case, the simulation is done to test the controller's ability to compensate the disturbance of the system. The controller is simulated with disturbance by [ ] in attitude current , with the initial and target of attitude is the same as in Case 2. In this case, this controller can preserve the value of attitude current in 3.03 sec as shown in Figure 6a.
When a disturbance is added to the system, the rotation error raises by 0.06 point in x-axis and the controller drives the system to make the error of attitude become zero in 3.03 sec as shown in Figure 6b. As long as the system is added by a disturbance, the controller preserves error in zero point. When the disturbance is eliminated from the system, rotation error in this system is reduced from zero point to point -0.06. In about 3 sec, the controller can drive the the system to the desired attitude, and the rotation error becomes zero when the system meets convergence. Based on all of the cases, rotation error matrix is a 3x3 identity matrix when the system is convergent. The steady state time is about 3 sec and the rise time is about 1 sec for all cases. 6a 6b Figure 6. Simulation result of case 3

CONCLUSION
A new simple scheme of PD plus Feedforward controller on SO(3) to control the attitude of quadrotor is proposed in this paper. This controller uses a rotation matrix error as a variable error of the system. The controller is able to drive the attitude of quadrotor from hovering condition to desired attitude and from an attitude condition goes to the hovering condition, with the convergence time of about 3 sec. The value of angular velocity will be zero when the desired equilibrium is achieved and the rotation error matrix will be a 3x3 identity matrix. This controller can also drive the system to the desired attitude despite the system is disturbed. For the future work, this controller will be implemented to control the maneuvering of quadrotor in the real quadrotor.