Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers

Depósito de investigación de la Universidad de Sevilla https://idus.us.es/ This is an Accepted Manuscript of an article published International Journal of Control available at: 10.1109/ICUAS54217.2022.9836135 © 2021 Taylor & Francis. Personal use of this material is permitted. Permission from Taylor & Francis must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other Works” This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 1 Geometric control using the state-dependent Riccati equation: Application to aerial-acrobatic maneuvers Saeed Rafee Nekoo*, José Ángel Acosta and Anibal Ollero GRVC Robotics Lab., Depto de Ingeniería de Sistemas y Automática, Escuela Técnica Superior de Ingeniería, Universidad de Sevilla, Seville, Spain [email protected], {jaar, aollero}@us.es *Corresponding Author Abstract Acrobatic flip is one of the most challenging representatives of aggressive maneuvers to test the performance of an aerial system’s capability or a controller. A variable-pitch rotor quadcopter generates thrust in both vertical directions for the special design of the rotor’s actuation mechanism. Flip maneuver sets the rotation matrix of the copter in a singularity that makes the action impossible for common designs based on the hovering dynamics model. Sudden flip was a solution to pass the singularities though that imposes an agile maneuver and excessive energy consumption. This research proposes two possible solutions for the flip: a regulation solution based on the geometric control approach; and tracking a predefined optimal smooth trajectory covering a turnover. The first solution uses a geometric control approach that is immune to singular points since the rotation matrix is integrated into another manifold on SO(3). The second solution proposes an optimal trajectory generation for flip maneuver using open-loop optimal control, two-point boundary value problem (TPBVP) approach. Since generated open-loop state information is not applicable without a controller, the statedependent differential Riccati equation (SDDRE) is chosen for trajectory tracking. The backward integration method is employed to provide a solution to the SDDRE with a final boundary condition. The dynamics equation of the variable pitch rotor was considered in the simulation to compute the angle of the blades and thrust coefficients. The flip maneuver has been successfully implemented and the optimal trajectory for the maneuver has been obtained and tracked by the nonlinear closed-loop optimal controller. For the regulation case, the geometric control resulted in a smoother flip rather than the abrupt turning of the conventional design. This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 2 Keywords: Quadcopter; variable-pitch; geometric control; open-loop optimal control; flip maneuver; SDDRE. 1. Introduction Inspection and maintenance is the key point for the safety of industrial plants and refineries. The size of the refineries forces the designers to keep the infrastructure outdoor exposed to sun and rain, or in other words, degradation and corrosion. HYbrid FLying-rollIng with-snakE- aRm robot for contact inSpection (HYFLIERS) is an innovation and research action of EU Horizon 2020 program to inspect and perform maintenance via advanced robot capabilities (https://www.oulu.fi/hyfliers/). One branch of the research is devoted to an inspection task of the pipes by rotating around it with a multi-rotor drone. The drone keeps the inspection device near the pipe during the rotary motion. The common multi-rotor drones are working in hovering conditions due to the singularity of the rotation matrix for a large role angle. This current research presents a geometric design to control the system in an aggressive role angle, without the problem of singularity. The representative of this aggressive motion is a flip maneuver. So, the elementary step for solving the singularity problem for rotating around the pipe is followed in this current research. This work contributes to 1) setting up and formalizing the statedependent Riccati equation equipped by geometric control; 2) presenting a TPBVP for quadrotor path-planning including flip maneuver. A comparative study analyzes the proposed methods in this aggressive flight. The use of variable-pitch quadrotors has been increased in the research due to their capability in flight such as agile and aggressive maneuver, flip, and inverted flight (Pretorius & Boje, 2014). Agile and aggressive maneuvers always enforce the quadrotor to undergo sudden motions and excessive energy consumption; moreover, such maneuvers are not usually optimized with smooth and practical trajectory. Constrained input path generation and control for variable-pitch UAVs were presented (Cutler & How, 2012). An algorithm was designed to assign the flip in minimum time when the time was not mentioned in the acrobatic motion, and polynomials were designed to perform the flip in predefined trajectories. The flip maneuver was simulated for attitude control (Pretorius & Boje, 2014). Adaptive control was investigated based on quaternion design for package delivery by a variable-pitch unmanned system (Shastry, Bhargavapuri, Kothari, & Sahoo, 2018). A nonlinear adaptive controller was used to control the load uncertainty and agile flight. During the trajectory tracking in a circular motion, This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 3 a flip was performed to illustrate the performance of the variable-pitch rotors. The flip was done fast to reduce the unstable situation during the acrobatic motion. The sudden flip during the flight highlights the potential of the variable-pitch rotor unmanned aerial vehicle (UAV); however, unstable situations, power consumption, and possible actuator saturation are the disadvantages of the fast acrobatic maneuver which is not optimized. Moreover, the trajectory during the flip is unpredictable. This current research proposes two methods to solve this issue: a regulation method based on a geometric control approach and trajectory tracking of an optimal predefined path for the flip. The first approach avoids singularity via geometric control, moving to another manifold in a special group in SO(3). Although the geometric control was used for different cases such as airplane flight dynamics (Hassan & Taha, 2017), tilted-rotor UAV (Invernizzi & Lovera, 2017), haptic-virtual control (Hernández-Díez, Méndez-Barrios, Mondié, Niculescu, & González-Galván, 2018), UAV trajectory tracking (Shi, Zhang, & Zhou, 2015), and control of a UAV transporting a payload (F. A. Goodarzi, Lee, & Lee, 2015); the power of the method is more visible in performing acrobatic motions (F. Goodarzi, Lee, & Lee, 2013; Loianno, Brunner, McGrath, & Kumar, 2016; Raj, Banavar, & Kothari, 2017). The common form of the controller for the geometric approach is PD/PID plus a nonlinear part for compensating the rotational dynamics (Fernando, Chandiramani, Lee, & Gutierrez, 2011; Lee, 2012). An attitude tracking control was presented with the exponential stability on SO(3) for large-scale rotational movements (Lee, 2012). The robustness and adaptive characteristics were mainly involved in the controllers though the dominating parts were the PD or PID terms (Gamagedara, Bisheban, Kaufman, & Lee, 2019). Here we introduce the state-dependent Riccati equation equipped with the geometric control to achieve a singularity-free nonlinear optimal controller for aggressive maneuvers. For the second solution, open-loop optimal control is expressed for trajectory generation for a six-DoF fully coupled quadrotor considering flip maneuver. The flip time, initial, and desired position of the UAV are adjustable. The generated motion is smooth and admissible; also better to emphasize that the trajectory is optimal, a consequence of a quadratic performance criterion. The solution is found by solving a two-point boundary value problem. The trajectory generation, in the optimal domain, was investigated in different fields such as under-actuated pendulum (Boscariol & Richiedei, 2018), robotic manipulators for loadcarrying (M. H. Korayem & Nikoobin, 2008), and hovercraft motion planning (Seguchi & This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 4 Ohtsuka, 2003); although, for the best knowledge of authors, optimal trajectory generation by TPBVP for flip maneuver of a quadrotor has not been studied. Finding the optimal trajectory for the flip is half of the design and trajectory tracking by a closed-loop controller is essential to complete the task. The tracking controller must be effective to reduce the error for both the orientation and position of the system. The position and attitude of the system are coupled and this specific design removes the cascade control in closed-loop tracking. So, the incompatibility or lack of precision in one part (attitude or position) imposes a large error on the output. The state-dependent differential Riccati equation has been selected to track the trajectory. There are several methods to solve the SDDRE tracking problem though backward integration is the most precise method among them (M. H. Korayem & Nekoo, 2015a, 2015b). Backward integration provides a two-round solution; in the first round, the system sweeps the trajectory backward to generate the gains, and in the forward one, the system tracks the system with the obtained gain, forward in time (Beeler, Tran, & Banks, 2003; Nekoo, 2020). Approximate SDRE tracking control was presented considering the feed-forward tracking vector (Cimen, 2007). The design of a suboptimal tracking controller was studied for a class of nonlinear systems (Batmani, Davoodi, & Meskin, 2016). Backward integration might seem impractical because of the two-round solution; however, the different applications showed otherwise such as satellite control (Abdelrahman & Park, 2013), robotics (A. H. Korayem, Nekoo, & Korayem, 2019), multi-motor driving unit (Wang, Dong, Ren, & Chen, 2020), and hardware-in-the-loop satellite control (Jung, Park, Kim, Eun, & Chang, 2013). The main contributions: (a) introducing the geometric control embedded within the nonlinear optimal control problem, the SDRE, to release a singularity-free method for acrobatic and aggressive maneuver (including flip) for variable-pitch quadcopters. (b) Proposing an open-loop optimal control approach for path planning of a quadrotor in six-DoF considering flip maneuver with adjustable maneuver time. Then tracking the optimal trajectory with the SDDRE controller using the backward integration solution. The sudden flip (in regulation) for a quadrotor using SDRE was reported (Nekoo, Acosta, Gomez-Tamm, & Ollero, 2019), though the novelty of this current work is highlighted by the smoothness of the optimal trajectory for the flip, defining the desired time of the flip, and the differential form of the tracking controller. Section 2 expresses the dynamics of a variable-pitch rotor quadcopter. Section 3 presents the geometric approach for the flip maneuver using the SDRE design. Section 4 presents the This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 5 open-loop optimal control trajectory generation by TPBVP. Section 5 is dedicated to the SDDRE tracking control design. Section 6 illustrates the simulations and Section 7 states the conclusion. 2. Variable-pitch quadrotor dynamics The fixed (inertial or Earth) frame is the first reference frame. The 𝑍 axis points from the center of Earth towards the surface, 𝑌 pointing to North and 𝑋 pointing to East, see Fig. 1. The body frame is set on to the center-of-mass (CoM) of a UAV, presented in Fig. 2; the position variables are 𝛏1=[𝑥c,𝑦c,𝑧c]𝑇(m), and Euler angles in the inertial frame, roll-pitch-yaw, are 𝛏2=[𝜙,𝜃,𝜓]𝑇(rad). The derivatives of generalized coordinates 𝛏1 and 𝛏2 are named as linear velocity vector in the inertial frame 𝛏󰇗1=[𝑥󰇗c,𝑦󰇗c,𝑧󰇗c]𝑇(m/s), and angular velocity vector in the inertial frame 𝛏󰇗2=[𝜙󰇗,𝜃󰇗,𝜓󰇗]𝑇(rad/s). Fig. 1. Fixed reference coordinate on earth. Fig. 2. The schematic view of the variable-pitch quadcopter. 𝜔 1 𝜔 2 𝜔 3 𝜔 4 𝑋 𝑌 𝑍 𝜙 𝜃 𝜓 𝛏 1 𝑝 𝑞 𝑟 𝑢 𝑣 𝑤 𝐶 𝑇,1 𝐶 𝑇,4 𝐶 𝑇,2 𝐶 𝑇,3 East, X North, Y Up, Z This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 6 The total thrust 𝑇B (N) is aligned with 𝑧c on the CoM of the UAV. The input torque vector is 𝛕B=[𝜏𝜙𝜏𝜃𝜏𝜓]𝑇,(Nm); its direction is against Euler angles {𝜙,𝜃,𝜓}. 𝑇B is set on body frame, and 𝛕B acts on the inertial frame. To make the coordinates identical, the input force is transformed from the body coordinate to the inertial one: 𝐅(𝛏2)=𝐑𝑍𝑌𝑋(𝛏2)[00 𝑇B], (1) where 𝐑𝑍𝑌𝑋(𝛏2)=[𝑐𝜓𝑐𝜃−𝑐𝜙𝑠𝜓+𝑠𝜙𝑠𝜃𝑐𝜓𝑠𝜙𝑠𝜓+𝑐𝜙𝑠𝜃𝑐𝜓 𝑠𝜓𝑐𝜃𝑐𝜙𝑐𝜓+𝑠𝜙𝑠𝜃𝑠𝜓−𝑠𝜙𝑐𝜓+𝑐𝜙𝑠𝜃𝑠𝜓 −𝑠𝜃𝑐𝜃𝑠𝜙𝑐𝜃𝑐𝜙]. The purpose of using two rotational matrices in this work is to compare the geometric control with conventional methods for aggressive maneuvers. It should be noted that 𝐑𝑍𝑌𝑋(𝛏2(𝑡)):ℝ3→ℝ3×3 in Eq. (1) is only used for TPBVP (Section 4 and 5). For the geometric control (Section 3), a new numerical 𝐑(𝑡)∈SO(3) is used: SO(3)={𝐑(𝑡)∈ℝ3×3|𝐑𝑇(𝑡)𝐑(𝑡)=𝐈3×3,det[𝐑(𝑡)]=1}, 𝐑󰇗(𝑡)=𝐑(𝑡)𝛀 (𝑡), (2) where [hat] map (∙) :ℝ3→𝔰𝔬(3) changes any vector to a skew-symmetric matrix: 𝛀 (𝑡)=[ 0 −𝜓󰇗(𝑡) 𝜃󰇗(𝑡) 𝜓󰇗(𝑡) 0 −𝜙󰇗(𝑡) −𝜃󰇗(𝑡) 𝜙󰇗(𝑡) 0 ]. 𝐑𝑍𝑌𝑋(𝛏2(𝑡)), in Eq. (1), is the common form of a rotation matrix used in kinematics and dynamics of the flying systems, found by three consecutive rotations on an orthogonal coordinate. To avoid the singularities in 𝐑𝑍𝑌𝑋(𝛏2(𝑡)), a new rotation matrix is used 𝐑(𝑡), with specifications in Eq. (2), which results in geometric control. Using the Euler-Lagrange equation, the dynamics equation of the system is found, which the translation section provides: 𝑚𝛏󰇘1+𝑚[00𝑔]=[𝑠𝜙𝑠𝜓+𝑐𝜙𝑠𝜃𝑐𝜓 −𝑠𝜙𝑐𝜓+𝑐𝜙𝑠𝜃𝑠𝜓 𝑐𝜃𝑐𝜙]𝑇B, (3) where 𝑚(kg) represents the total mass of the system and 𝑔(m/s2) indicates gravity acceleration. The orientation dynamic results in: 𝐉(𝛏2)𝛏󰇘2+𝐂(𝛏2,𝛏󰇗2)𝛏󰇗2=𝛕B. (4) This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 7 Generalized coordinates and their velocities are set in the state vector 𝐱=[𝛏1𝑇,𝛏2𝑇,𝛏󰇗1𝑇,𝛏󰇗2𝑇]𝑇=[𝑥c,𝑦c,𝑧c,𝜙,𝜃,𝜓,𝑥󰇗c,𝑦󰇗c,𝑧󰇗c,𝜙󰇗,𝜃󰇗,𝜓󰇗]𝑇. (5) Considering state-vector (5), and equation of motion (3) and (4), the state-space equation of the UAV is found: 𝐱󰇗= [ 𝛏󰇗1 𝛏󰇗2 𝛏󰇘1 𝛏󰇘2 ] = [ 𝛏󰇗1 𝛏󰇗2 1/𝑚𝐈3×3[𝐑𝑍𝑌𝑋,3(𝛏2)𝑇B−𝑚𝑔𝐞3−𝐃𝛏󰇗1] 𝐉−1(𝛏2)[𝛕B−𝐂(𝛏2,𝛏󰇗2)𝛏󰇗2] ] , (6) in which 𝐞3=[0,0,1]𝑇, 𝐑𝑍𝑌𝑋,3(𝛏2) represents the last column of 𝐑𝑍𝑌𝑋(𝛏2), and details of 𝐂(𝛏2,𝛏󰇗2) could be found in Ref. (Nekoo, Acosta, Gomez-Tamm, et al., 2019). The aerodynamics effect is regarded in UAV dynamics (6) by definition of 𝐃= diag(𝐷𝑥,𝐷𝑦,𝐷𝑧) (kg/s) (Luukkonen, 2011). The corresponding state-space system for geometric control is also presented with a different rotation matrix: 𝐱󰇗= [ 𝛏󰇗1 𝛏󰇗2 𝛏󰇘1 𝛏󰇘2 ] = [ 𝛏󰇗1 𝛏󰇗2 1/𝑚𝐈3×3[𝐑3𝑇B−𝑚𝑔𝐞3−𝐃𝛏󰇗1] 𝐉−1(𝛏2)[𝛕B−𝐂(𝛏2,𝛏󰇗2)𝛏󰇗2] ] , (7) in which 𝐑3(𝑡) presents the last column of 𝐑(𝑡). The dynamics of the quadrotor is balanced by generalized force including total thrust 𝑇B and moments 𝛕B. The state-space representation of the system is nonlinear (either of Eqs. (6) or (7)), coupled and under-actuated. The computation of dynamics and control is done by the generalized force-moments. The blade angle and angular velocity of four rotors of the quadcopter are related to the generalized force-moment through a kinematic relation. Following definitions are considered to find the thrust factors 𝐂T(𝑡): 1) Thrust 𝑇B(𝑡): If all the blades have the same angle, the force 𝑇B(𝑡) (in body frame) is generated. An increase in blade angle (positive direction) moves the quadrotor up and a decrease towards down. If the blade angle sets in the negative direction, the negative thrust is generated. A specific constant blade angle holds the quadrotor steady in the air. 2) Roll 𝜏𝜙(𝑡): If the blade angles of rotor 2 and 4 differ, a rotation around 𝑥c coupled with lateral motion occurs, represented by the reactive moment 𝜏𝜙(𝑡). This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 8 3) Pitch 𝜏𝜃(𝑡): If the blade angles of rotor 1 and 3 differ, a rotation around 𝑦c coupled with lateral motion occurs, represented by the reactive moment 𝜏𝜃(𝑡). 4) Yaw 𝜏𝜓(𝑡): If a differential blade angle sets between rotors (1,3) and (2,4), a rotation around 𝑧c happens, represented by the reactive moment 𝜏𝜓(𝑡). The pair of blade angles (1,3) changes with the same angle and the pair of blade angles (2,4) changes with the same one in the opposite direction. The difference between the two pairs provides yaw. The total thrust is (Bhargavapuri, Sahoo, & Kothari, 2019): 𝑇B=𝐾∑𝐶𝑇𝑖, 4 𝑖=1 (8) where 𝐾=𝜌𝜋𝑅4𝜔ss 2, 𝜌(kg/m3) is the air density, 𝑅(m) is the tip radius of the blade, 𝜔ss(rad/s) is the steady-state angular velocity of the blades, and 𝐶𝑇𝑖(𝑡) indicates a coefficient of the thrust of 𝑖-th variable-pitch blade. 𝜔ss can be defined more than the minimum angular velocity of the quadcopter; the minimum value for flying is 𝜔min=√𝑚𝑔/(4𝑘) (rad/s), in which 𝑘(Ns2/rad2) is the lift constant for a special steady-state blade angle. Roll and pitch moments are 𝜏𝜙=𝐾𝐿[−𝐶𝑇2+𝐶𝑇4], (9) 𝜏𝜃=𝐾𝐿[−𝐶𝑇1+𝐶𝑇3], (10) where 𝐿 is the distance between a rotor and CoM of the quadrotor. Yaw moment is 𝜏𝜓=𝑅𝐾 √2[−|𝐶𝑇1|3/2+|𝐶𝑇2|3/2−|𝐶𝑇3|3/2+|𝐶𝑇4|3/2]. (11) Equations (8)-(11) form a nonlinear relation between input force/moments and thrust coefficients: [𝑇B 𝜏𝜙 𝜏𝜃 𝜏𝜓]= [ 𝐾 𝐾 𝐾 𝐾 0 −𝐿𝐾 0𝐿𝐾 −𝐿𝐾 0𝐿𝐾 0 −𝑅𝐾 √2√|𝐶𝑇1|𝑅𝐾 √2√|𝐶𝑇2| −𝑅𝐾 √2√|𝐶𝑇3|𝑅𝐾 √2√|𝐶𝑇4| ] [ 𝐶𝑇1 𝐶𝑇2 𝐶𝑇3 𝐶𝑇4 ] . (12) Several methods were presented to solve this nonlinear equation such as using first-order dynamics (Bhargavapuri et al., 2019), Mean Value Theorem, Pseudo, augmented Pseudo inverse approach, and null-space of Pseudo inverse approach (Nekoo, Acosta, Gomez-Tamm, et al., 2019). In this work, to solve the nonlinear relation in thrust allocation which leads to a This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 15 problem. Setting the final (𝐱∗(𝑡f)=𝛌∗(𝑡f)=𝐱𝑡f) and initial one (𝐱∗(𝟎)=𝛌∗(𝟎)=𝐱0) boundary conditions for (35) and (36), results in a two-point boundary value problem. The presented model of the quadrotor defines a state-vector 𝐱(𝑡)∈ℝ12 and a co-state vector 𝛌(𝑡)∈ℝ12; consequently, the system to be solved is a set of differential equation 𝛘(𝑡)= [𝐱𝑇(𝑡),𝛌𝑇(𝑡)]24×1 𝑇. The control law is also 𝐮(𝑡)=[𝑇B(𝑡) 𝛕B 𝑇(𝑡)]4×1 𝑇. Point-to-point motion is the goal of this study, so the initial and final velocities of states are set zeros 𝐱∗(0)=𝛌∗(0)= [𝛏1𝑇(0),𝛏2𝑇(0),𝟎,𝟎]𝑇 and 𝐱∗(𝑡f)=𝛌∗(𝑡f)=[𝛏1𝑇(𝑡f),𝛏2𝑇(𝑡f),𝟎,𝟎]𝑇. 𝛏1 and 𝛏2 define the translation and rotation states of the quadrotor with respect. The orientation dynamics (4) is fully actuated and controllable; however, the translation dynamics (3) is under-actuated and common designs will not result in an admissible solution. Suppose 𝐔3×1 is a stable control law. Replacing 𝛏󰇘1 from (3) with 𝐔 results in (Zuo, 2010): 𝐔+[00𝑔]=[𝑠𝜙𝑠𝜓+𝑐𝜙𝑠𝜃𝑐𝜓 −𝑠𝜙𝑐𝜓+𝑐𝜙𝑠𝜃𝑠𝜓 𝑐𝜃𝑐𝜙]𝑇B 𝑚. (38) Changing (38) to [𝑈1 𝑈2 𝑈3+𝑔]=[𝑐𝜓𝑐𝜃−𝑐𝜙𝑠𝜓+𝑠𝜙𝑠𝜃𝑐𝜓𝑠𝜙𝑠𝜓+𝑐𝜙𝑠𝜃𝑐𝜓 𝑠𝜓𝑐𝜃𝑐𝜙𝑐𝜓+𝑠𝜙𝑠𝜃𝑠𝜓−𝑠𝜙𝑐𝜓+𝑐𝜙𝑠𝜃𝑠𝜓 −𝑠𝜃𝑐𝜃𝑠𝜙𝑐𝜃𝑐𝜙][ 00 𝑇B𝑚 ⁄], and multiplying 𝐑𝑍𝑌𝑋 𝑇(𝛏2) from the left side provides: [𝑐𝜓𝑐𝜃𝑠𝜓𝑐𝜃−𝑠𝜃 −𝑐𝜙𝑠𝜓+𝑠𝜙𝑠𝜃𝑐𝜓𝑐𝜙𝑐𝜓+𝑠𝜙𝑠𝜃𝑠𝜓𝑐𝜃𝑠𝜙 𝑠𝜙𝑠𝜓+𝑐𝜙𝑠𝜃𝑐𝜓−𝑠𝜙𝑐𝜓+𝑐𝜙𝑠𝜃𝑠𝜓𝑐𝜃𝑐𝜙][ 𝑈1 𝑈2 𝑈3+𝑔]=[ 00 𝑇B𝑚 ⁄]. (39) From the last row of (39), the final form of thrust is found (Zuo, 2010): 𝑇B(𝑡)=𝑚{[𝐑𝑍𝑌𝑋,3(𝛏2)]1𝑈1+[𝐑𝑍𝑌𝑋,3(𝛏2)]2𝑈2+[𝐑𝑍𝑌𝑋,3(𝛏2)]3(𝑈3+𝑔)} =𝑚[(𝑠𝜙𝑠𝜓+𝑐𝜙𝑠𝜃𝑐𝜓)𝑈1+(−𝑠𝜙𝑐𝜓+𝑐𝜙𝑠𝜃𝑠𝜓)𝑈2+𝑐𝜃𝑐𝜙(𝑈3 +𝑔)]. (40) For generating the co-state dynamics and control law in off-line (parametric) form, the control law 𝐔 and 𝑇B must be considered in the Hamiltonian before derivation of (36) and (37). Equation (36) generates 12 co-state equation and Eq. (37) results in 6 control signals, 𝐔 and 𝛕B, off-line and symbolic. Then, the main system of differential equations 𝛘(𝑡)= [𝐱𝑇(𝑡),𝛌𝑇(𝑡)]24×1 𝑇, is solvable with cascaded control law (40) and input moments, 𝐮(𝑡)= [𝑇B(𝑡) 𝛕B 𝑇(𝑡)]𝑇. This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 16 5. The SDDRE tracking controller: Backward integration solution Consider nonlinear system (31) in SDC parameterization form with output vector 𝐱󰇗(𝑡)=𝐀(𝐱(𝑡))𝐱(𝑡)+𝐁(𝐱(𝑡))𝐮(𝑡), (41) 𝐲(𝑡)=𝐂(𝐱(𝑡))𝐱(𝑡), (42) where output vector is 𝐲(𝑡)∈ℝ𝑝; and 𝐂(𝐱(𝑡)):ℝ𝑛→ℝ𝑝×𝑛, 𝐁(𝐱(𝑡)):ℝ𝑛→ℝ𝑛×𝑚 and 𝐀(𝐱(𝑡)):ℝ𝑛→ℝ𝑛×𝑛 are held. For second-order Lagrangian systems, 𝑛 is usually an even number greater than 1 and 𝑛/2≤𝑝≤𝑛. The {𝐀(𝐱(𝑡)),𝐁(𝐱(𝑡))} pair is a completely controllable SDC design and the {𝐀(𝐱(𝑡)),𝐂(𝐱(𝑡))} pair is a completely observable design of the system (41) and output (42). The cost function of the SDDRE tracking is in the form of 𝐽s=12{𝐞𝑇(𝑡f)𝐅𝐞(𝑡f)+∫(𝐮𝑇(𝑡)𝐑(𝐱(𝑡))𝐮(𝑡)+𝐞𝑇(𝑡)𝐐(𝐱(𝑡))𝐞(𝑡))d𝑡 𝑡f 0}, where the error vector is 𝐞(𝑡)=𝐲(𝑡)−𝐫(𝑡), 𝐫(𝑡)∈ℝ𝑝 is desired trajectory, the weighting matrix for the error vector is 𝐐(𝐱(𝑡)):ℝ𝑛→ℝ𝑝×𝑝, and a weighting matrix for inputs is 𝐑(𝐱(𝑡)):ℝ𝑛→ℝ𝑚×𝑚; both of them symmetric positive definite. The weighting matrix for states is 𝐅∈ℝ𝑝×𝑝 imposing penalty at the final time. The Hamiltonian is specified as 𝐻s(𝐱(𝑡),𝛌(𝑡),𝐮(𝑡),𝑡) =12{𝐮𝑇(𝑡)𝐑(𝐱(𝑡))𝐮(𝑡)+𝐞𝑇(𝑡)𝐐(𝐱(𝑡))𝐞(𝑡)} +𝛌𝑇(𝑡)[𝐀(𝐱(𝑡))𝐱(𝑡)+𝐁(𝐱(𝑡))𝐮(𝑡)]. Using the optimality condition (34) for 𝐻s results in control law 𝐮(𝑡)=−𝐑−1(𝐱(𝑡))𝐁𝑇(𝐱(𝑡))𝛌(𝑡), (43) in which 𝛌(𝑡)=𝐊(𝐱(𝑡))𝐱(𝑡)+𝐬(𝐱(𝑡)) is the co-state equation where 𝐊(𝐱(𝑡)):ℝ𝑛→ℝ𝑛×𝑛 is optimal gain and the feed-forward vector is 𝐬(𝐱(𝑡)):ℝ𝑛→ℝ𝑛. Importing co-state in (43) provides: 𝐮(𝑡)=−𝐑−1(𝐱(𝑡))𝐁𝑇(𝐱(𝑡)){𝐊(𝐱(𝑡))𝐱(𝑡)+𝐬(𝐱(𝑡))}. (44) Computing derivative of co-state vector, 𝛌󰇗(𝑡), and using condition (33), input (44), and mathematical operation, a system of differential equations is found (M. H. Korayem & Nekoo, 2015a): This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 17 { 𝐊󰇗+𝐊𝐀+𝐀𝑇𝐊+𝐂𝑇𝐐𝐂−𝐊𝐁𝐑−1𝐁𝑇𝐊+ ([𝜕𝐀 𝜕𝐱𝐱]𝑇−12[𝜕𝐑 𝜕𝐱𝐮]𝑇𝐊𝐁𝐑−1𝐁𝑇+[𝜕𝐁 𝜕𝐱𝐮]𝑇)𝐊+ (12[𝜕𝐐 𝜕𝐱[𝐂𝐱−𝐫]]𝑇+[𝜕𝐂 𝜕𝐱𝐱]𝑇𝐐)𝐂 } 𝐱 + { 𝐬󰇗+(𝐀𝑇+[𝜕𝐀 𝜕𝐱𝐱]𝑇−12[𝜕𝐑 𝜕𝐱𝐮]𝑇𝐊𝐁𝐑−1𝐁𝑇+[𝜕𝐁 𝜕𝐱𝐮]𝑇−𝐊𝐁𝐑−1𝐁𝑇)𝐬− (𝐂𝑇𝐐+12[𝜕𝐐 𝜕𝐱[𝐂𝐱−𝐫]]𝑇+[𝜕𝐂 𝜕𝐱𝐱]𝑇𝐐)𝐫 } =𝟎. (45) The final boundary condition [12𝜕𝐞𝑇(𝑡f)𝐅𝐞(𝑡f) 𝜕𝐱 −𝛌(𝑡f)]𝑇𝛿𝐱f+[𝜕𝐻s(𝐱(𝑡f),𝛌(𝑡f),𝐮(𝑡f),𝑡f)+12𝜕𝐞𝑇(𝑡f)𝐅𝐞(𝑡f) 𝜕𝑡 ]𝛿𝑡f=0, concerning 𝛿𝑡f=0, also defines 𝐊(𝑡f)=[𝐂(𝐱(𝑡f))+𝜕𝐂(𝐱(𝑡f)) 𝜕𝐱 𝐱(𝑡f)]𝑇𝐅𝐂(𝐱(𝑡f)), (46) 𝐬(𝑡f)=−[𝐂(𝐱(𝑡f))+𝜕𝐂(𝐱(𝑡f)) 𝜕𝐱 𝐱(𝑡f)]𝑇𝐅𝐫(𝑡f). (47) Excluding the derivative terms from (45)-(47), the SDDRE and feed-forward equation along with final boundary condition are found: 𝐊󰇗=−𝐊𝐀−𝐀𝑇𝐊−𝐂𝑇𝐐𝐂+𝐊𝐁𝐑−1𝐁𝑇𝐊, (48) 𝐬󰇗=−𝐀s 𝑇𝐬+𝐂𝑇𝐐𝐫, (49) 𝐊(𝑡f)=𝐂𝑇(𝐱(𝑡f))𝐅𝐂(𝐱(𝑡f)), (50) 𝐬(𝑡f)=−𝐂𝑇(𝐱(𝑡f))𝐅𝐫(𝑡f), (51) where 𝐀s=𝐀−𝐁𝐑−1𝐁𝑇𝐊 is a stable closed-loop matrix. Equation (50) is a boundary condition for the SDDRE (48) and Eq. (51) is the one for feed-forward Eq. (49). In the first round, Eq. (48) and (49) are solved backward from 𝑡f→𝑡0 and after storing feedforward vector 𝐬(𝐱(𝑡)) and optimal gain 𝐊(𝐱(𝑡)), the forward solution begins. Further details on stability and solution to tracking case of SDDRE are referred to (M. H. Korayem & Nekoo, 2015b). To implement the closed-loop tracking, the translational dynamic is controlled separately although the interaction with the rotational dynamic is preserved; assuming three virtual inputs for translation control. Then, the three virtual controls are set into one total thrust, such as (40). This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 18 The rotational dynamic has already fully actuated with three inputs; so, overall control is a vector 𝐮(𝑡)=[𝑇B(𝑡) 𝛕B 𝑇(𝑡)]𝑇. The summary of the tracking solution for the flip is as follows. First, design the openloop optimal trajectory based on the TPBVP in Section 4, then track the trajectory with the SDDRE nonlinear optimal controller, Section 5. So for the second solution for acrobatic flip maneuver, tracking an optimal smooth trajectory has been employed. 6. Simulation 6-1. Acrobatic flip with optimal trajectory tracking The two-point boundary value problem is intended to perform a flip maneuver in the desired period. The time of simulation is set 3 seconds, and weighting matrices for TPBVP are 𝐐= diag[𝟏1×9,𝟏𝟎1×3] and 𝐑=diag[𝟏1×3,𝟎.𝟏1×3]. The model details are presented in Tables 1 and 2. The initial position is set on the equilibrium point and the desired position is (−3,2,1) m. The initial orientation of the quadrotor is zero for three angles and the final orientation is (𝜋,0,0) rad representing flip in 𝜙 direction. The SDDRE tracking parameters were also chosen as follows. The weighting matrices were selected as 𝐑o=0.1×𝐈3×3, 𝐑t=0.1×𝐈3×3, 𝐐o=1000×𝐈3×3, 𝐅o=𝐈3×3, 𝐐t= 100000×𝐈3×3 and 𝐅t=𝐈3×3. The output vector also defined by 𝐂o=𝐂t=[𝐈3×3,𝟎3×3]. The translation and orientation states of the system are presented in Fig. 3 and Fig. 4. The UAV’s linear and angular velocities are illustrated in Fig. 5 and Fig. 6. The optimal trajectory and configuration of the variable-pitch quadrotor are presented in Fig. 7. The thrust of the system and input moments of the quadrotor are illustrated in Fig. 8 and Fig. 9. The error of SDDRE tracking is shown in Fig. 10. The angles of the blades are illustrated in Fig. 11. This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 19 Table 1. Parameters of the quadrotor (Luukkonen, 2011). para. values units definition 𝑙 0.225 m dist. between motor and CoM of UAV 𝑅 0.075 m radi. of the propeller 𝑘 2.98×10−5 Ns2/rad2 lift constant - thrust factor (fixed-pitch case) 𝐼𝑥𝑥 4.856×10−3 kgm2 mom. of iner. of UAV around X axis 𝐼𝑦𝑦 4.856×10−3 kgm2 mom. of iner. of UAV around Y axis 𝐼𝑧𝑧 8.801×10−3 kgm2 mom. of iner. of UAV around Z axis 𝑚 0.468 kg mass of the UAV 𝐷 [0.25 0 0 0 0.25 0 0 0 0.25] kg/s drag matrix 𝑔 9.81 m/s2 gravity constant Table 2. Parameters of the variable-pitch rotor dynamics. para. values units definition 𝜌 1.225 kg/m3 density of air 𝜔min 196.2 rad/s minimum constant rotor angular velocity 𝜔ss 392.5 rad/s steady-state constant rotor angular velocity 𝑁b 2 - number of the blades in each rotor 𝑐 0.03 m chord length of the rotor 𝐶l𝛼 5.73 1/rad airfoil lift curve slope (Bhargavapuri et al., 2019) 𝛼max,min ±𝜋/6 rad maximum and minimum bound of blade angles 𝑣0 0.5 m/s air flow velocity Fig. 3. Translation states of the UAV in the Cartesian coordinate. This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 20 Fig. 4. The UAV’s orientation states. Fig. 5. Linear state velocities. Fig. 6. Angular velocities of the variable-pitch quadrotor. This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 21 Fig. 7. The optimal trajectory tracking and configuration of the quadrotor during the flip. Fig. 8. Total thrust, SDDRE. Fig. 9. Input torque, SDDRE. This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 22 Fig. 10. The error of the SDDRE closed-loop tracking. Fig. 11. Blade angles of the system in tracking simulation. 6-2. Acrobatic flip with geometric control: Regulation This section shows the acrobatic flip maneuver by a regulation approach based on geometric control, presented in Section 3. The first difference between the regulation and tracking approach is the time of the flip. In the tracking problem, the time of the flip and motion between the initial and final conditions was set 3 seconds. For this regulation, almost 6 seconds is necessary to complete the point-to-point motion. So, the time of simulation is defined as 6 seconds. Regulation embeds the flip inside the task, between 𝑡1=1s and 𝑡2=2s, therefore the time for the flip is 1s. The initial and final conditions are similar to Section 6-1. The weighting matrices were selected as 𝐑o=𝐈3×3, 𝐑t=𝐈3×3, 𝐐o=diag[𝟏1×3,𝟎1×3], and 𝐐t= diag[𝟓1×3,𝟐.𝟓1×3]. The initial condition for the rotation matrix is 𝐑(0)=𝐈3×3. The position This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 23 variables of the UAV are presented in Fig. 12. The results are also compared with the proposed approach for a sudden flip in Ref. (Nekoo, Acosta, Gomez-Tamm, et al., 2019), known as GPGV (global position global velocity). The orientation of the quadcopter is presented in Fig. 13, by rotational generalized coordinates in the global frame. The generated trajectories and configuration of the UAVs are presented in Fig. 14 controlled by geometric control and GPGV. Total thrusts of the UAVs are illustrated in Fig. 15. The input moments of geometric control are demonstrated in Fig. 16. The blade angles are shown in Fig. 17. Checking the determinant of the rotation matrix during numerical integration is important, presented in Fig. 18 to emphasize correct integration. The error function is also presented in Fig. 19. Fig. 12. Cartesian coordinates of the quadcopter in regulation problem; in this graph, geometric control is compared with GPGV (global position global velocity) in Ref. (Nekoo, Acosta, Gomez-Tamm, et al., 2019). Fig. 13. The orientation of the system for both geometric approach, and GPGV (Nekoo, Acosta, Gomez- Tamm, et al., 2019). 𝜋 −𝜋 This is an Accepted Manuscript of an article published by Taylor & Francis in International Journal of Control on 9 Feb. 2021, available online: https://www.tandfonline.com/doi/10.1080/00207179.2021.1881165 This is a preprint version of: Rafee Nekoo, Saeed, José Ángel Acosta, and Anibal Ollero. "Geometric control using the state-dependent Riccati equation: application to aerial-acrobatic maneuvers." International Journal of Control (2021): 1-13. 24 (a) (b) Fig. 14. Flip maneuver and configuration of the UAV by (a) geometric control, and (b) GPGV (Nekoo, Acosta, Gomez-Tamm, et al., 2019). Fig. 15. Total thrust of the UAVs in flip, controlled by regulation approach. Fig. 16. Input moments of the geometric control approach.