Fully Coupled Six-DoF Nonlinear Suboptimal Control of a Quadrotor: Application to Variable-Pitch Rotor Design

Depósito de Investigación de la Universidad de Sevilla https://idus.us.es/ This version of the conference paper has been accepted for publication, after peer review and is subject to Springer Nature’s AM terms of use, but is not the Version of Record and does not reflect post-acceptance improvements, or any corrections. The Version of Record is available online at: http://dx.doi.org/10.1007/978-3-030-36150-1_7 Preprint version of: Nekoo, S. R., Acosta, J. Á., & Ollero, A. (2019, November). Fully Coupled Six-DoF Nonlinear Suboptimal Control of a Quadrotor: Application to Variable-Pitch Rotor Design. In Fourth Iberian Robotics conference (ROBOT 2019), pp. 7283, Springer, Cham. Fully Coupled Six-DoF Nonlinear Suboptimal Control of a Quadrotor: Application to Variable-pitch Rotor Design S. R. Nekoo, J. Á. Acosta and A. Ollero GRVC Robotics Lab., Departamento de Ingeniería de Sistemas y Automática, Escuela Técnica Superior de Ingeniería, Universidad de Sevilla, 41092 Seville, Spain [email protected], {jaar, aollero}@us.es Abstract. In this work, a fully coupled six degree-of-freedom (DoF) nonlinear suboptimal control of a variable-pitch quadrotor is studied using a state-depend- ent Riccati equation (SDRE) controller. The quadrotor control has been widely considered for attitude control; however, the position control is an uncontrollable problem with the common design of the SDRE. Due to the under-actuated nature of a quadrotor, the state-dependent coefficient (SDC) parameterization of statespace representation of a nonlinear system leads to an uncontrollable SDC pair. The control law is divided into two sections of position and attitude control. The position control provides the main thrust. A virtual constraint is regarded to provide stabilization for the quadrotor in attitude control. Two methods were designed for selection of a state vector or in other words, selection of feedback. The first one uses the position and orientation and their derivatives in global coordinate. The second one uses position and orientation in global and their velocities in local coordinate. The dynamics of a variable-pitch propeller quadrotor was imported to the problem and compared with a fixed-pitch propeller system. The simulation of the systems shows that the SDRE is capable of controlling the system with both fixed- and variable-pitch rotor dynamics. Keywords: Quadrotor; Nonlinear Optimal Control; SDRE; Virtual Constraint. 1 Introduction The state-dependent Riccati equation was applied to quadrotors by Voos for the first time in 2006 [1]. The control problem was limited to attitude control of a system in a sub-control unit, and control of the velocity. So, the regulation was not possible since the quadrotor was moving in space with a constant velocity. The combination of the SDRE controller with the neural network was provided to control the velocity vector towards zero [2]. Navabi and Mirzaei presented  -D based nonlinear tracking control of a quadcopter using the state-dependent Riccati equation [3]. Due to under-actuation, only Z direction was controlled. Babaie and Ehyaie proposed a robust SDRE, based 2 on sliding mode design [4]. The position control was addressed through Lyapunov criteria for stabilization of sliding surfaces. Chipofya and Lee presented the position control of a quadrotor via SDRE controller employing a Kalman filter for estimation [5]. In the design, the planar motion of the quadrotor was not included in the simulation. This problem was visible in most control problems of quadrotors using the SDRE. At the beginning of the quadrotor control research, most of the cases used fixedpitch propeller systems to simplify the design, increase the stability of the quadcopter, and employ common methodology in the literature. Fixed-pitch propellers limited the inputs to angular velocities of rotors. The focus of this work is to explore the variablepitch propellers quadrotor control within the framework of the SDRE. The variablepitch design decreases stability; hence, a more agile maneuver could be expected. Inverted flight and flip during the motion were also highlighted in the literature [6]. The use of variable-pitch blades in quadrotors was reported by Bristeau et al. [7], and later on by Cutler [6]. Fresk and Nikolakopoulos presented experimental model derivation and control of a variable pitch propeller for a quadrotor [8]. Sheng and Sun focused on the energy consumption of the variable pitch quadrotor control [9]. Panizza et al. presented data-driven attitude control of the system [10]. Chipade et al. presented control of variable pitch quadrotor for payload delivery with focus on mechanism design [11]. Rotor dynamic in variable pitch imposed a nonlinear algebraic equation which required control allocation [12], or other techniques to find a solution. Turning the nonlinear relation to a first-order differential equation was regarded to solve that issue [12]. An extra differential equation in addition to complex nonlinear dynamics of a multirotor enhances the complexity. The main contribution of this current research is to apply the state-dependent Riccati equation controller for a variable-pitch quadrotor, considering fully coupled six-DoF nonlinear dynamics. The main step of the SDRE control design is state-dependent coefficient parameterization of a nonlinear vector. The SDC matrices must be controllable and observable to guarantee a solution to the related SDRE. The equation of motion of a quadrotor in SDC form does not release a controllable pair of SDC matrices since there is only one actuator for translation control. To overcome this issue, the design of the translation control has been done assuming that three virtual inputs are available for XYZ directions. Then, virtual constraints were designed to find the relations between thrust (actuator in Z ) and desired orientation angles. The three virtual inputs have been transformed into one input (thrust) capable of controlling position vector. The main contribution of this research is fully coupled six-DoF control of a variable-pitch quadrotor using state-dependent Riccati equation introducing virtual constraint; for both fixed- and variable pitch rotor design. Two sets of states were chosen to generate state-space representations for the system. The first one uses the position and orientation of the quadrotor in global coordinates and their velocities in local one. This point of view requires the assumption of small deviations in rotational movement which is common in regulation and tracking of a multirotor system. The second one uses all the position and orientation with their velocities in global coordinate. Both methods were simulated and analyzed to assess the effect of different feedback selection. 3 2 The State-dependent Riccati Equation Consider a nonlinear system ( ) ( ( )) ( ) ( ( )) ( ),t t t t tx A x x B x u (1) where () n tx is a state vector and () m tu is an input vector. ( ( )): n n n t Ax and ( ( )): n n m t Bx are state-dependent coefficient parameterization of a nonlinear system, consists of piecewise-continuous vector-valued functions that satisfy Lipschitz condition. The intention of optimal control is to minimize the cost functional integral [13]:   0 1 () ( ) ( ( )) ( ) ( ) ( ( )) ( ) d , 2 TT J t t t t t t t     x Q x x u R x u where ( ( )): n n n t Qx penalizes the states (symmetric positive semi-definite) and ( ( )): n m m t Rx penalizes the inputs (symmetric positive definite). Controllability condition: The pair of { ( ( )), ( ( ))}ttA x B x is a completely controllable parameterization of a nonlinear system (1) with its condition [14]. Observability condition: The pair of 1/2 { ( ( )), ( ( ))}ttA x Q x is a completely observable parameterization of a nonlinear system (1) with its condition [14]. A standard form of the SDRE control law is 1( ) ( ) ( )( ), T u R x B x K x x where the symmetric positive definite suboptimal gain 22 [ ( ( ))] nn t Kx , is a solution to the statedependent Riccati equation [14]: 1 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) . TT    A x K x K x A x K x B x R x B x K x Q x 0 3 State-space Representation 3.1 Global Position and Local Velocity The absolute linear position vector of a quadrotor in the inertial frame is 1 c c c ( ) [ ( ), ( ), ( )] ( ) T t x t y t z t mξ in which subscript “c” stands for center-of-mass, the three Euler angles in an inertial frame, roll-pitch-yaw, are set in a vector 2( ) [ ( ), ( ), ( )] (rad) T t t t t    ξ , linear velocity vector in body frame is 1( ) [ ( ), ( ), ( )] (m/s) T t u t v t w tυ , and angular velocity vector in body frame is 2( ) [ ( ), ( ), ( )] (rad/s) T t p t q t r tυ , Fig. 1. 4 Fig. 1. Fixed and moving reference frame. The following kinematics relations are held between inertial and body frame [15]: 1 2 1 ( ) , ZYX ξ R ξ υ (2) 2 2 2 ( ) ,ξ T ξ υ (3) where 2 () ZYX Rξ is found based on the multiplication of the three rotation matrices around three main axes: 2 ( ) , ZYX c c s s c c s c s c s s c s s s s c c c s s s c s s c c c                                        Rξ 2 1 ( ) 0 , 0 / / s t c t cs s c c c                Tξ in which e.g. cos( ( ))ct   and tan( ( ))tt   . There is one input force (thrust) B( ) (N),Tt acting in direction of w on CoM of the quadrotor (local moving coordinate), and an input torque vector B( ) ( ) ( ) ( ) (N.m) T t t t t         τ , acting against three Euler angles { ( ), ( ), ( )}.t t t    B()Tt is defined in body frame and B()tτ is set on the inertial frame. In this point of view, the position and attitude of the quadrotor are considered and measured in the inertial frame, and the linear and angular velocity of that are measured in body frame. This consideration is valid through a simplification 22 ( ) ( )ttξυ that holds for small angular motions [16], and 11 ( ) ( )ttξυ . This assumption leads to two more approximations 11 ( ) ( )ttυξ and 22 ( ) ( )ttυξ . Based on that assumption, the state vector of the system is assembled as 1 2 1 2 c c c ( ) [ ( ), ( ), ( ), ( )] [ , , , , , , , , , , , ] . T T T T T T t t t t t x y z u v w p q r    xξ ξ υ υ (4) Considering state-vector (4), the upper half of the state-space representation of the system uses kinematics relations (2) and (3); and the lower part of that extracts 1 ξ and 2 ξ from equation of motion: X Y ENU Z ψ ϕ θ v u w (xc,yc,zc) p q r ξ1 f1, rotor 1 f4, rotor 4 f3, rotor 3 f2, rotor 2 ω3 ω1 ω2 ω4 5 21 1 22 2 3 3 ,3 2 B 3 1 1 1 22 B 2 2 2 () () () () ( ) , 1/ ( ) () () ( ) ( , ) ZYX ZYX t t tm T mg t t                        Rξυ ξTξυ ξ xIR ξ e Dξ υ υJξ τ C ξ ξ ξ (5) where ,3 2 () ZYX Rξ is the third column of 2 () ZYX Rξ and 3[0,0,1]T e , and 2 2 2 ( ) ( ) ( ) T Jξ W ξ IW ξ , 2 ()Wξ is the inverse of 2 ()Tξ and vector 2 2 2 [ ( , ) ]Cξ ξ ξ includes Coriolis and centrifugal terms. The aerodynamics effect is incorporated into the dynamics of the system thorough diag( , , ) (kg/s) x y z D D DD matrix [17]. That is the result of drag force caused by air resistance in which ,, x y z D D D are drag coefficients in ( , , )X Y Z inertial frame. 3.2 Global Position and Global Velocity In this section, the position and attitude of the quadrotor, and the linear and angular velocity of that are considered and measured in the inertial frame. This consideration does not need any assumption or approximation. So, the state vector of the system is assembled as 1 2 1 2 c c c c c c ( ) [ ( ), ( ), ( ), ( )] [ , , , , , , , , , , , ] . T T T T T T t t t t t x y z x y z       xξξξξ (6) Considering state-vector (6), the modified representation of (5) is found: 1 1 2 2 3 3 ,3 2 B 3 1 1 1 22 B 2 2 2 () () () () ( ) . 1/ ( ) () () ( ) ( , ) ZYX t tt t tm T mg t t                         ξ ξ ξ ξ xIR ξ e Dξ ξ ξJξ τ C ξ ξ ξ (7) The state-space representation in (7) is based on the measurement of all the states in the inertial frame; however, the representation in (5) computes the half of the states in body frame. So, the choice of state-space representations might be restricted to the sensor selection or practical limitation. One should note that “global position and local velocity (GPLV)” representation has an approximation though “global position and global velocity (GPGV)” form was generated without any simplification or approximation. 4 Variable-pitch Rotor Dynamics Variable-pitch propeller quadrotors provide the option of additional inputs to the problem; hence, the angle of a blade could be considered as input. More maneuverability, upright or inverted flight, detach/attach equipment in out of reach positions and negative thrust deceleration could be listed as the advantages of the variable-pitch design. 6 The challenges are also complexity in rotor mechanism design, control approach and reducing the flight stability of the quadrotor. The blade is defined in terms of thrust coefficient as [12]: TT l 6 ( ) ( ) 3 ( ) , 22 ii i C t C t tC    where i  and Ti C are blade angle and thrust coefficient of i-th rotor with respect, l C  is airfoil lift curve slope, b Nc R   in which b N is number of the blades in each rotor, (m)c is rotor’s chord length and (m)R is the radius of the rotor. Based on the structure of the quadrotor (plus shape), the thrust coefficient is related to force/moment inputs of a quadrotor [18]:      4 2 3 1 1 2 3 4 4 B T T T T T 1 3/2 3/2 3/2 3/2 T T T T ( ) ( ), ( ) ( ) ( ) , ( ) ( ) ( ) , ( ) ( ) ( ) ( ) ( ) , 2 i i T t K C t t lK C t C t t lK C t C t KR t C t C t C t C t                     where  is 1 for normal flight and -1 for inverted flight and 42 ss KR    in which 3 (kg/m )  is air density and ss (rad/s)  is a constant angular velocity of the rotors; ss  is considered constant in variable-pitch flight mode. Considering variable  in K is also possible and increases the flexibility and complexity of the system; in this work, constant angular velocity is chosen for the variable-pitch system. 5 The State-dependent Coefficient Parameterization The transformation of the state-space representation of the dynamics into apparent (extended) linearization is called state-dependent coefficient parameterization. Since two representations were given for the dynamics of the quadrotor, two sets of SDC are generated. With regard to the state-space equation (5), the first point of view, GPLV, provides: 3 3 2 t 3 3 3 3 () ( ) , 1/ ZYX m       0Rξ Ax 0 I D 33 t 33 , 1/m      0 BI (8) 3 3 2 o1 3 3 2 2 2 () ( ) , ( ) ( , )        0Tξ Ax 0Jξ C ξ ξ 33 o12 ( ) , ()      0 Bx Jξ (9) where index “t” stands for translation and “o” for orientation. With regard to the statespace equation (7), the second point of view, GPGV, provides: 3 3 3 3 t 3 3 3 3 , 1/m       0I A0 I D 3 3 3 3 o1 3 3 2 2 2 ( ) , ( ) ( , )        0I Ax 0Jξ C ξ ξ moreover, t B and o()Bx are similar to (8) and (9). 7 Controllability: It can be easily checked that an arbitrary pair of n n n n n n n n      0I A00 and nn nn      0 BI is controllable and a pair of 1/2 { , }AQ , with nn QI is observable. Considering that the diagonal elements of 2 () ZYX Rξ , 2 ()Tξ and 12 ()  Jξ possess multiplication of “cosine” functions, they hardly meet zero. So, the controllability and observability of the proposed SDC parameterization are guaranteed, except for ( ( ), ( ), ( )) (2 1) / 2, t t t k k        . The orientation of ( ), ( ), ( )t t t    is not supposed to reach /2  due to stability of the quadrotor for normal flight; in case of inverted flight, GPGV must be used. Note: It should be noted that t B in (8) should have been a 31 vector since the thrust input is a scalar value. However, the SDC design based on   t31 B , will result in an uncontrollable system. So, the design of (8) was done assuming a fully-actuated system. In Section 6, the necessary modification to incorporate 3 virtual inputs to one actual thrust B T will be established. 6 Virtual Constraint Under-actuation Compensation A quadrotor is an under-actuated system, possessing six-DoF motion in space and four input actuators. One could divide the six-DoF system into two subsystems, three translational motion c c c ( , , )x y z , and three rotational one ( , , )    . The contribution of the input B()tτ in the lower sets of state-space equation (5) or (7) provides a stable controller. However, the contribution of input thrust B()Tt through ,3 2 () ZYX Rξ in the upper sets of the system (5) or (7) only controls c z direction. In order to control cc ,xy directions, additional constraints should be provided to link the motions in a meaningful manner. The error of the state vector is defined as des ( ) ( )tte x x . A stable control law is proposed (assuming the system is not under-actuated): 1 t t t t ( ) ( ) ( ) , T U R x B x K x e (10) where 6 3 3 t( ):  Rx is the weighting matrix for inputs, t 1 1,des 1 1,des ,T T T T T      eξ ξ υ ξ (GPLV) or t 1 1,des 1 1,des ,T T T T T      eξ ξ ξ ξ (GPGV) is an error vector including translational states, 6 6 6 t( ):  Kx is the symmetric positive definite solution to the SDRE (dedicated for translational control): 1 t t t t t t t t t t ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) . TT    A x K x K x A x K x B x R x B x K x Q x 0 Replacing 1 ξ from the equation of motion with U results in [15]: 8 0 0. B c s c s s T c s s s c m g c c                          U (11) Changing (11) to     1 2 3 2 ( ) 0 0 / , TT ZYX B U U U g T mRξ and multiplying 2 () T ZYX Rξ from left side provides ( 2 () ZYX Rξ is orthogonal):     2 1 2 3 ( ) 0 0 / . TT T ZYX B U U U g T mRξ (12) From Eq. (12), two relations could be found as constraints for determining desired values for  and  [15]: 11 des 2 des des 3 cos sin ( ) tan , UU tUg        (13) 11 des 2 des des 2 2 2 1 2 3 sin cos ( ) sin . () UU tU U U g           (14) Equations (13) and (14) are found based on cascade design [15]. So, the desired vector 2,des()tξ , is defined as   2,des des des des ( ) ( ) ( ) ( ) , T t t t t    ξ (15) where desired des()t  , in (13)-(15), could be independently set. Consequently, the problem of under-actuation is solved and the thrust is in the form of:   B ,3 2 1 ,3 2 2 ,3 2 3 1 2 3 1 2 3 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) . ZYX ZYX ZYX T t m U U U g m c s c s s U c s s s c U c c U g                                     Rξ R ξ R ξ The design of the rotational control of the quadrotor is straightforward (similar to (10)): 1 B o o o o ( ) ( ) ( ) , T τ R x B x K x e where 6 3 3 o( ):  Rx is the weighting matrix for inputs, o 2 2,des 2 2,des ,T T T T T      eξ ξ υ ξ (GPLV) or o 2 2,des 2 2,des ,T T T T T      eξ ξ ξ ξ (GPGV) is error vector including rotational states, 6 6 6 o( ):  Kx is the symmetric positive definite solution to the SDRE (dedicated for rotational control): 1 o o o o o o o o o o ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) . TT    A x K x K x A x K x B x R x B x K x Q x 0 7 Simulations In this section, both cases of GPLV and GPGV are simulated and compared. The parameters of the quadrotor are based on the model in Ref. [17]. A regulation case study is regarded to analyze the modeling and the controllers. The initial condition of the system was set as equilibrium point and the desired position was