RESILIENT AERIAL AUTONOMY THROUGH EXTENDED HIGH-GAIN OBSERVERS By Connor James Boss A DISSERTATION Submitted to Michigan State University in partial fulfillment of the requirements for the degree of Electrical Engineering โ€“ Doctor of Philosophy 2021 ABSTRACT RESILIENT AERIAL AUTONOMY THROUGH EXTENDED HIGH-GAIN OBSERVERS By Connor James Boss For a growing number of flight operations, human piloted aircraft are being replaced by au- tonomous uncrewed aerial vehicles (UAVs) which can provide equivalent service with drastically reduced operating costs. The UAVs that are deployed in mission critical applications, such as search and rescue, medical deliveries to remote locations, infrastructure inspection, and reconnaissance and surveillance, must be extremely resilient as the loss of a vehicle poses significant threats to financial, security, or personnel interests. In order to rely on uncrewed systems in these mission critical situations, we must have confidence in their ability to perform their duties as reliably as possible. Recent advancements in hardware, software, and control design have increased the re- liability of these small inexpensive aircraft. The focus of this dissertation is to further improve multi-rotor reliability in the presence of a broad class of disturbances, while providing a unifying framework that can be extended to multiple applications. The methods we present in this work are based on a feedback linearizing control strategy in which the controller is augmented with an extended high-gain observer. The addition of the extended high-gain observer allows us to overcome the typical drawbacks associated with using a feedback linearization control approach; primarily that we must have an excellent model of the system, and we must know any disturbances that are affecting the system. The extended high-gain observer not only provides estimates of any model uncertainties or external disturbances, but also any unmeasured states for use in output feedback control. This estimation and control strategy enables the multi-rotor to robustly track a trajectory in the presence of a broad class of unmodeled disturbances and without needing an extremely accurate system model. This method forms the base technology applied throughout this dissertation. We extend our estimation and control strategy in a number of ways in the coming chapters. We begin by extending the observer dynamics further to incorporate real-time trajectory estimation for a reference system which may have partially known or completely unknown dynamics, and for which we assume we only have access to the position of the reference system. The observer is then able to provide estimates of all higher-order trajectory terms required for feedforward control, improving transient tracking performance. We further extend this strategy to enable both the detection and classification of a complete actuator failure of a multi-rotor during flight. The identification subsequently enables a control reconfiguration and a recovery from the loss of an actuator to resume flight operations. This extension provides the ability to not only detect and correctly identify a failure, but to discern the failure from other external disturbances affecting the system during flight. Finally, we extend our estimation and control strategy to enable robust trajectory tracking for a novel form of multi-body multi-rotor systems. This type of system consists of a large carrier UAV which suspends a small platform which is itself actuated by a pair of rotors. The platform is equipped with a manipulator which enables long reach aerial manipulation. One advantage of this configuration is the workspace will not be disturbed by downwash from the carrier UAV. Each of these methods are rigorously analyzed to guarantee closed-loop stability and provide insights into the trade-offs that arise during the design process for these control systems utilizing extended high-gain observers. Copyright by CONNOR JAMES BOSS 2021 to my family v ACKNOWLEDGEMENTS First, I would like to thank my advisor, Dr. Vaibhav Srivastava, for his continuous mentorship, guidance, encouragement, and support throughout my graduate career. My technical writing and mathematical analysis have improved markedly under his guidance. I would additionally like to thank my graduate committee members, Dr. Ranjan Mukherjee, Dr. Xiaobo Tan, and Dr. Shaunak Bopardikar for their encouragement and guidance throughout my graduate career. I thank Dr. Hassan K. Khalil for his insights into extended high-gain observer design and analysis. I thank my parents, Jim and Linda, and my brother, Ryder, for fostering my creativity through the years and for all of the support and encouragement throughout my education. I thank my girlfriend, Marissa, for sharing this journey with me as we both pursue PhDs in engineering. I also want to thank my cousin, Cody, and his wife, Stephanie, for their continued encouragement and inspiring me to pursue a graduate degree in the first place. Finally, I would like to thank my lab-mates, Sanders, Krissy, Mahmoud, Nilay, and Sheryl for the constant sense of camaraderie in the lab and the tight friendships we have developed. The work reported in this publication was supported by The National Science Foundation through Award IIS-1734272, and in part by funding provided by the National Aeronautics and Space Administration (NASA), under award number 80NSSC20M0124, Michigan Space Grant Consortium (MSGC). vi TABLE OF CONTENTS LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x CHAPTER 1 INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.1 Trajectory Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.2 In-flight Actuator Failure Recovery . . . . . . . . . . . . . . . . . . . . . . . . . . 5 1.3 Long Reach Aerial Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.4 Organization and Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 CHAPTER 2 TRACKING AN UNKNOWN TRAJECTORY . . . . . . . . . . . . . . . . 13 2.1 System Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.1.1 Rotational Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.1.2 Translational Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.1.3 Reference System Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.1.4 Actuator Dynamics and Mapping to Inputs . . . . . . . . . . . . . . . . . . 17 2.2 Control and Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 2.2.1 Rotational Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 2.2.2 Translational Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 2.2.3 Extended High-Gain Observer Design . . . . . . . . . . . . . . . . . . . . 21 2.2.4 Output Feedback Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.3 Stability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 2.3.1 Restricting Domain of Operation . . . . . . . . . . . . . . . . . . . . . . . 26 2.3.2 Stability Under State Feedback . . . . . . . . . . . . . . . . . . . . . . . . 28 2.3.3 Convergence of Observer Estimates . . . . . . . . . . . . . . . . . . . . . 29 2.3.4 Stability Under Output Feedback . . . . . . . . . . . . . . . . . . . . . . . 33 2.4 Numerical Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 2.5 Experimental Validation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 2.5.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 2.5.2 Experimental Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 2.6 Extended High-Gain Observers on ๐‘†๐‘‚ (3) . . . . . . . . . . . . . . . . . . . . . . 41 2.6.1 Rotational Dynamics on ๐‘†๐‘‚ (3) . . . . . . . . . . . . . . . . . . . . . . . . 41 2.6.2 Rotational State Feedback Control on ๐‘†๐‘‚ (3) . . . . . . . . . . . . . . . . . 43 2.6.3 Extended High-Gain Observer Design . . . . . . . . . . . . . . . . . . . . 43 2.6.4 Translational Output Feedback Control . . . . . . . . . . . . . . . . . . . . 46 2.6.5 Numerical Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 2.7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 CHAPTER 3 IN-FLIGHT ACTUATOR FAILURE RECOVERY . . . . . . . . . . . . . . 52 3.1 System Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.1.1 Rotational Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 vii 3.1.2 Translational Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 3.1.3 Failure Modes and Mapping Actuator Speeds to Inputs . . . . . . . . . . . 54 3.2 Output Feedback Control Design . . . . . . . . . . . . . . . . . . . . . . . . . . . 56 3.2.1 Extended High-Gain Observer Design . . . . . . . . . . . . . . . . . . . . 56 3.2.2 Output Feedback Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 58 3.3 Failure Recovery Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60 3.3.1 Estimating the Lyapunov Derivative . . . . . . . . . . . . . . . . . . . . . 60 3.3.2 Estimating Disturbance and Failures Simultaneously . . . . . . . . . . . . 61 3.4 Stability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 3.5 Numerical Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 3.6 Experimental Validation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68 3.7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71 CHAPTER 4 TOWARDS MULTI-BODY MULTI-ROTORS FOR LONG REACH MA- NIPULATION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 4.1 System Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 4.1.1 Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 4.1.2 Differential flatness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 4.1.3 State-Space Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 4.2 Control Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 4.2.1 State Feedback Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 4.2.2 Extended High-Gain Observer Design . . . . . . . . . . . . . . . . . . . . 78 4.2.3 Output Feedback Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 80 4.3 Stability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80 4.3.1 Restricting Domain of Operation . . . . . . . . . . . . . . . . . . . . . . . 80 4.3.2 Stability under State Feedback . . . . . . . . . . . . . . . . . . . . . . . . 82 4.3.3 Stability under Output Feedback . . . . . . . . . . . . . . . . . . . . . . . 82 4.4 Numerical Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83 4.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84 CHAPTER 5 CONCLUSIONS AND FUTURE DIRECTIONS . . . . . . . . . . . . . . . 87 APPENDICES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 APPENDIX A EXPERIMENTAL SYSTEM DESIGN . . . . . . . . . . . . . . . . 90 APPENDIX B STABILITY OF A GENERALIZED CASCADE SYSTEM . . . . . 109 APPENDIX C PEAKING PHENOMENON . . . . . . . . . . . . . . . . . . . . . 111 APPENDIX D ROTATION MATRIX PROJECTIONS BACK TO ๐‘†๐‘‚ (3) . . . . . . 112 APPENDIX E CONTROLLABILITY OF HEXROTOR CONFIGURATION AF- TER ACTUATOR FAILURE . . . . . . . . . . . . . . . . . . . . . 113 BIBLIOGRAPHY . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116 viii LIST OF TABLES Table 3.1: System parameters used in simulation and experiment. . . . . . . . . . . . . . . 66 ix LIST OF FIGURES Figure 2.1: Simulated rotational system response with and without actuator dynamics in- cluded in the EHGO. The disturbance estimate, ๐œŽ ห† 2 , when actuator dynamics are omitted oscillates between the saturation bounds (top), inducing oscilla- tions in the tracking performance of ๐œ™2 (bottom). The disturbance estimate, ห† 1 , and tracking, ๐œ™1 , show excellent performance when actuator dynamics ๐œŽ are included in the EHGO for this example. . . . . . . . . . . . . . . . . . . . . 25 Figure 2.2: The trajectory of the multi-rotor UAV (dashed) and the trajectory of the ground vehicle (solid). The red points are the initial conditions and the green point signifies the occurrence of the landing. . . . . . . . . . . . . . . . . . . . . . . 35 Figure 2.3: Experimental multi-rotor on ground vehicle landing platform. . . . . . . . . . . 36 Figure 2.4: Experimental landing on moving ground vehicle. . . . . . . . . . . . . . . . . . 36 Figure 2.5: Estimates of the total rotational disturbance affecting the hexrotor during an experimental flight. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 Figure 2.6: Estimates of the total translational disturbance affecting the hexrotor during an experimental flight. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 Figure 2.7: Multiple experimental landing trajectories showing the multi-rotor trajectory (dashed) and the ground vehicle trajectory (solid). The red dots correspond to the initial conditions of the system when a landing was commanded. . . . . . 39 Figure 2.8: Trajectory tracking utilizing the almost-globally stabilizing control and ex- tended high-gain observer presented in Section 2.6.3. . . . . . . . . . . . . . . 47 Figure 2.9: Disturbance estimates from the extended high-gain observer presented in Section 2.6.3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48 Figure 2.10: Trajectory tracking utilizing the almost-globally stabilizing control and ex- tended high-gain observer presented in Section 2.6.3. The multi-rotor starts upside down and flips to return to hover at the desired position. . . . . . . . . . 49 Figure 2.11: Disturbance estimates from the extended high-gain observer presented in Sec- tion 2.6.3, where system-observer space mismatch errors enter the disturbance estimate. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 Figure 2.12: Rotational tracking error. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 x Figure 3.1: Norm of disturbance estimates for all failure case models and Lyapunov derivative estimate during a simulated in-flight actuator failure. . . . . . . . . . 67 Figure 3.2: Hexrotor position tracking recovery after simulated in-flight actuator failure using multiple models to recover performance. . . . . . . . . . . . . . . . . . . 68 Figure 3.3: Health estimate of each rotor as estimated using the Extended Kalman Filter method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69 Figure 3.4: Norm of disturbance estimates for all failure case models and Lyapunov derivative estimate during an experimental in-flight actuator failure. . . . . . . . 70 Figure 3.5: Hexrotor position tracking recovery after experimental in-flight actuator failure. 71 Figure 4.1: Carrier UAV with suspended bi-rotor actuated platform. . . . . . . . . . . . . . 74 Figure 4.2: Trajectory tracking for ๐‘ฅ, ๐‘ฆ, ๐›ผ (platform pose) and ๐›ฝ (UAV orientation) under output feedback control in the presence of disturbances. . . . . . . . . . . . . . 84 Figure 4.3: Simulation tracking the same trajectory, showing a potential application using an attached two-link manipulator to grasp an object. . . . . . . . . . . . . . . . 85 Figure A.1: Suspending the multi-rotor to measure the moment of inertia about each principle axis using the bifilar pendulum method. . . . . . . . . . . . . . . . . 91 Figure A.2: Standard quadrotor, hexrotor, and octorotor configurations with rotation di- rection of each rotor. These multi-rotors are in the โ€˜Xโ€™ configuration, meaning that there are two rotors symmetric about the body-fixed ๐‘ฅ-axis at the front and rear of the airframe. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92 Figure A.3: Examples of the two extremes for the PWM signals that can be sent to an ESC. Both signals are applying the maximum pulse width of 2000๐œ‡๐‘ , corresponding to maximum throttle for the ESC. . . . . . . . . . . . . . . . . . . . . . . . . . 97 Figure A.4: Photo tachometer rotor speed test stand. . . . . . . . . . . . . . . . . . . . . . 98 Figure A.5: Actuator dynamic response. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 Figure A.6: Mapping commanded ESC Pulse Width Modulation (PWM) to measured rotor speed in forward and reverse directions. . . . . . . . . . . . . . . . . . . . 100 Figure A.7: Mapping measured rotor speed to force in forward and reverse directions for carbon fiber 10x4.5 rotors, which are designed for single direction use and generate reduced thrust in reverse by a factor of five. . . . . . . . . . . . . . . . 101 xi Figure A.8: Time required to stop and reverse rotor direction with the magnitude of rotor speed shown to aid in estimating time. A complete reversal takes approximately 450ms. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102 Figure A.9: PX4 Serial Read Simulinkยฎ block, used to read from serial ports on the Pixhawk 4 FMU hardware. Specifically in this case, devttyS1 corresponds to the port labeled TELEM1 on the FMU. . . . . . . . . . . . . . . . . . . . . . . 103 Figure A.10: Pixhawk 4 FMU, Raspberry Pi, GPS receiver, RC receiver, and Vicon markers for motion tracking shown mounted on the hexrotor airframe. . . . . . . . . . . 104 Figure A.11: Simulinkยฎ uORB Read blocks. Used to access the vehicle_attitude bus and rc_channels bus on the uORB message system in PX4 firmware. . . . . . . . . 106 Figure A.12: Example implementation of a Matlabยฎ function running within Simulinkยฎ . Data is routed in and out through bus signals. . . . . . . . . . . . . . . . . . . . 106 Figure A.13: Writing desired information to the uORB Bus in the PX4 firmware. This example will play a tune over the piezo speaker on the FMU, which is useful to the operator for audible feedback. . . . . . . . . . . . . . . . . . . . . . . . 107 Figure A.14: Block for logging telemetry data to the on-board SD card on the FMU. User defined file name and path and the enable input must be 1 to log data, and transition to 0 to close the file and save at the end of flight. . . . . . . . . . . . . 108 Figure A.15: Block for writing PWM signals to the ESCs. User specified number of channels and the datatype must be uint16. . . . . . . . . . . . . . . . . . . . . 108 Figure E.1: Standard body configuration of a hexrotor. . . . . . . . . . . . . . . . . . . . . 113 Figure E.2: Candidate reconfiguration strategy for a hexrotor UAV. Disabling the actuator across the center of mass from the failed actuator results in a non-standard quadrotor configuration. This configuration is not controllable. . . . . . . . . . 115 Figure E.3: Candidate reconfiguration strategy for a hexrotor UAV. Enabling the actuator across the center of mass from the failed actuator to rotate in either direction so as to apply both upward and downward forces results in a fully controllable system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115 xii CHAPTER 1 INTRODUCTION As uncrewed aerial vehicles (UAVs) continue to achieve higher performance, increased reli- ability, and new capabilities, many applications which previously relied on manned aircraft are making the shift to these smaller, cheaper, uncrewed vehicles. With increased dependence on UAVs in a variety of mission critical applications, the demand for increasingly reliable vehicles is growing. Medical deliveries are now being performed by these uncrewed systems in remote parts of Rwanda [1] to overcome medical infrastructure deficits. Applications to search and rescue in both natural disaster scenarios [63] and mountain search and rescue operations [38] are also being investigated. The use of UAVs to perform infrastructure inspections [5] has also been investigated, and implemented specifically for bridge inspections [82]. With the broadening of applications in which UAVs are being deployed, performance improve- ments must be made through hardware, software, and control design to increase flight performance and robustness to unpredictable flight conditions. Much work has been devoted to studying multi- rotor UAV control design; see [49, 72] for a survey. Specifically, efforts have been focused on control advancements to enable aggressive maneuvers [31, 60, 91], react to external disturbances and potential modeling error [20, 32, 51, 65], and to design feasible flight trajectories [14, 56, 68]. Numerous linear and nonlinear control approaches have been applied to multi-rotors, including PID [31, 80], feedback linearization [50, 65, 95], backstepping [16, 59], adaptive control [18, 20, 32], and model predictive control (MPC) [13, 91] to name a few. Linear methods are effective near the hover configuration, but can experience degraded performance during aggressive maneuvers. Feedback linearization is sensitive to sensor noise as well as model uncertainty [50], but results in a linear system which is simple to analyze and tune. Adaptive control and backstepping are often combined to enable a nonlinear adaptive control design [32, 51]. These methods enable unknown system parameters to be estimated during operation to reduce the effect of model uncertainty. MPC enables excellent tracking performance and can achieve aggressive maneuvers [91], but at the cost 1 of computational complexity. Many linear and nonlinear approaches are also subject to reduced performance in the presence of model uncertainty and external disturbances [88]. Thus motivating the development of robust methods which can overcome certain classes of disturbances utilizing observers [46, 88] or adaptive approaches to estimate model parameters online [18, 51]. The magnitude of disturbance that can be canceled may also depend on the control gains, which can be tuned using an adaptive gain scheduling approach [88]. While disturbance observers have been used in multi-rotor control [27, 37], extended high-gain observers are not typically implemented on highly dynamic systems of this nature due to high sample rate requirements and possibility of measurement noise amplification. The methods presented in this dissertation serve to show that even at a relatively low sample rate of 100Hz, extended high-gain observers can be implemented on highly dynamic systems in practice. Furthermore, extended high-gain observers afford the ability to estimate and cancel a broader class of disturbances than standard disturbance observers [77]. Several methods of trajectory generation have also been developed for multi-rotors. Methods applying motion primitives to generate feasible trajectories [56, 68] have been explored and extended specifically to utilize reinforcement learning [56]. Computationally efficient methods which can be implemented and updated in real-time have been developed to design trajectories that also meet dynamic feasibility constraints [14]. These methods of generating a trajectory for multi-rotor UAVs still require a control method to ensure robust tracking of the desired trajectory. Additionally, stochastic methods have been developed to implement path planning for mobile robots which may have motion uncertainty and imperfect state information. Linear-quadratic Gaussian motion planning [92] is one such method in which probabilistic distributions are used to generate feasible paths for the robot a priori. This method utilizes a linear quadratic regulation (LQR) control in parallel with a Kalman filter for state estimation. To control a partially observable system with sensor noise, the system can be framed in belief space to apply conventional planning and control techniques, such as LQG [76]. These stochastic methods, as well as many of the linear and nonlinear strategies presented above that do not explicitly consider disturbance, are designed 2 to be passively robust to disturbances. The work presented in the coming chapters, by contrast, involves an estimation strategy to estimate and actively cancel disturbances through the feedback control. Since the estimation and control strategy does not rely on aggressive control gains to cancel disturbance, but rather utilizes the estimate to ensure the disturbance is aggressively canceled, the desired transient performance of the system can be retained while remaining robust to a broad class of disturbances. In the coming sections, we will detail a number of specific areas of multi-rotor control develop- ment. These include trajectory estimation and tracking, specifically the case of landing a multi-rotor on a moving ground vehicle, in-flight actuator failure recovery, and the use of multi-rotors for long reach aerial manipulation. These specific areas of interest are relevant to the work presented in the coming chapters in which we design a robust output feedback linearizing control structure through the use of extended high-gain observers. The methods proposed herein address several of the design challenges presented above. Specif- ically, since our method relies on a feedback linearizing control approach, with the addition of an extended high-gain observer we can overcome the associated challenges by estimating model errors and external disturbances to improve tracking performance in real-world flight conditions. Our base estimation and control strategy is extended to enable the estimation of all higher-order terms of a reference system, which may have partially known or completely unknown dynamics, for use as a desired flight trajectory. We also present a novel method for designing an extended high-gain observer for estimating higher-order states and disturbances for a system evolving on ๐‘†๐‘‚ (3). The estimation and control strategy is then further extended to enable the detection, classifi- cation, and recovery from an in-flight actuator failure through the implementation of a family of extended high-gain observers. Once detected, the observer estimates are used to identify which actuator has failed. Finally, a new controller is selected to re-stabilize the system. An actuator failure is an extremely dynamic event, and the detection, classification, and control switch must occur in a fraction of a second to recover stability. Finally, we apply the estimation and control strategy to a novel multi-body multi-rotor airframe 3 design to enable long-reach aerial manipulation. Our proposed configuration avoids the typical downdraft issues associated with using a large carrier UAV with a smaller suspended body which supports the manipulator. 1.1 Trajectory Tracking One of the key focuses of this dissertation is to develop an extended high-gain observer based control strategy that is capable of robustly tracking a trajectory in the presence of unknown and unmodeled disturbances. While other multi-rotor trajectory tracking control strategies may be susceptible to modeling error, external disturbances, or unknown reference system dynamics, we provide a unified framework which can ensure flight performance in the presence of all of these uncertainties. We design a robust feedback linearizing control strategy that can achieve excellent transient tracking performance even in the presence of these disturbances. Our framework enables real-time trajectory generation based on position information of a reference system which may have partially known or completely unknown dynamics. Furthermore, the magnitude of disturbance this method can overcome is not dependent on control gains, but rather on observer gains. This allows total freedom in assigning control gains, which can be chosen to shape the transient response as we recover the performance of a desired linear system. To showcase the performance of our method, we will apply our technique to the problem of landing a multi-rotor on a mobile platform [25, 47]. Multiple control methodologies have been applied to this problem, including model predictive control [21, 58], PI control [29, 79, 83], and feedback linearizing control [30]. Many approaches either do not consider modeling error and external disturbances, or consider them to be constant or slowly time-varying [29, 83]. In contrast, our approach only requires that any uncertainty be bounded and continuously differentiable. State estimators such as a Kalman filter have been used to estimate the dynamic state of the mobile platform [43, 79] under the assumptions that the dynamic model of the mobile platform is known and it travels with unknown, but constant, velocity. Through our extended high-gain observer design, these assumptions are relaxed, requiring no information about the dynamics or 4 input of the mobile platform. An alternative approach to estimate the state of the mobile platform uses optical flow data [29, 83], or visual cue data [39] in which a dynamic model of the mobile platform is not required. In these cases the relative velocity is estimated through the optical flow algorithms and is minimized in the control to ensure tracking. Our proposed approach relies on relative position measurements and is complimentary to optical flow methods. In Chapter 2, we design and rigorously analyze an extended high-gain observer based feedback linearizing control method that incorporates estimation of a reference trajectory from an unknown, or partially known, reference dynamic system. The extended high-gain observer enables estima- tion of all states for output feedback control, as well as estimating modeling error and external disturbances, enabling the design of a robust feedback linearizing control strategy. The proposed method can recover performance of the desired linear system under a broad class of disturbances. Existing multi-rotor control techniques require the rotational dynamics to be sufficiently faster than the translational dynamics such that the tracked rotational trajectory can serve as virtual control for the translational subsystem. The proposed extended high-gain observer design treats the transient of the rotational subsystem as a disturbance, estimates it, and actively compensates for it in the ro- tational controller. Thus, the proposed controller does not require the timescale separation between rotational and translational subsystems. A key challenge in implementing an extended high-gain observer based output feedback controller is that the dynamics of commercial electronic speed controllers evolve at the same timescale as the extended high-gain observer and should be included in extended high-gain observer design. We illustrate the influence of inclusion/non-inclusion of these dynamics on output feedback performance. 1.2 In-flight Actuator Failure Recovery Actuator failure is of particular interest when it comes to reliability, as conventional multi-rotor UAVs will crash, or at least require an emergency landing, in the event of a failure. The main challenges in recovering from an actuator failure during flight are the ability to quickly detect the failure and to reconfigure the system while preserving stability. A complete actuator failure will 5 cause a large rotational torque, which in turn causes the UAV to roll and pitch rapidly. If action is not taken extremely quickly the UAV can arrive at a configuration where it simply cannot recover. The area of fault detection and isolation has been investigated for generalized systems [6, 7, 15, 33], as well as for multi-rotor UAVs, including quadrotors [4, 19, 57, 67, 97], hexrotors [91, 93, 94, 96], and octorotors [2, 3]. A quadrotor with failure of one actuator or two opposing actuators can be stabilized, but loses yaw controllability [67]. A failure on an octorotor can result in dramatically reduced thrust capability [2, 3]. Interestingly, a standard hexrotor is fully vulnerable to any single actuator failure because the total moment generated by opposed rotors are collinear [62], resulting in an uncontrollable system. For details on the controllability analysis of a standard hexrotor, see Appendix E. In order to overcome this vulnerability, a variety of modifications have been proposed. One option is to use a different pattern of rotor rotation directions [93, 94], thus making the moment of certain pairs of opposed rotors non-collinear. This will maintain controllability under failure, however, the asymmetry restricts this method to only recover if one of four specific rotors fail. Another option [62, 96] involves a modified airframe design where the actuators are canted off plane to enable force application in all six degrees of freedom. This method supports loss of any one actuator, however, given the orientation of the rotors, the configuration is not efficient for nominal flight. A third option is to enable the rotors to rotate in either direction [91]. This method maintains controllability if any one of the actuators fail, while preserving efficiency during nominal flight. Thus, we will utilize this hardware reconfiguration strategy as well. The detection strategies used in the methods described above include a linear observer [94], estimating actuator forces using a sliding-mode differentiator on IMU data [57], and an EKF based rotor health estimator [91], while others have left the detection strategy to future work [2, 3, 67]. In contrast, we utilize a multiple-model multiple extended high-gain observer output feedback linearizing control approach, in which estimates from the extended high-gain observers are used to detect and classify a failure. The extended high-gain observers not only allow us to detect failures and select the appropriate reconfiguration, but are fully integrated in our control strategy to provide 6 estimates of unmeasured states and disturbances. These estimates are used before, during, and after failure to improve tracking performance in the presence of a broad class of disturbances. The failure recovery methods described thus far do not consider disturbances affecting the system. Our method explicitly incorporates disturbances and can differentiate between disturbances and an actuator failure, depending on disturbance levels. The analysis presented in Chapter 3 not only guarantees stability, but provides insights into what levels of disturbances can be distinguished from an actuator failure and how much time can elapse between failure and switching models before risking losing control. 1.3 Long Reach Aerial Manipulation In certain applications, such as remote crop sampling, novel multi-rotor systems are required which can actively interact with the environment and perform challenging tasks in unstructured environments. Augmenting a multi-rotor with another multi-body system, for example, a manipu- lator, significantly increases its functionality. Several UAV-manipulator systems, often referred to as aerial manipulators, have been designed for aerial pick and place [24], avian inspired grasping and perching [89, 90], mobile manipulation [28, 45, 71], assembly [36], valve turning [48], and aerial phytobiopsy [70]. Having a manipulator connected to a UAV through an actuated joint is a typical design for such systems. However, this limits the range of operation of the end-effector with respect to the UAV. In recent years a variety of alternate designs, in which a UAV connected to another system through a passive joint, have been considered. Using cables to suspend a load from a UAV is one such example. Transporting a load using suspended cables has been considered both in single- UAV [17, 75, 84] as well as multi-UAV scenarios [54, 61, 73, 78]. Research efforts have also been devoted to considering using a passive spherical joint to attach a rigid link to the UAV. For example, the work presented in [74] considers a rigid rod suspended at one end from a UAV through a spherical joint and studies scenarios with both a torque-actuated and an actuation-free joint. In [69] a spherically connected multiquadrotor (SmQ) platform is designed in which multiple quadrotors 7 are used as rotating thrust generators for the platform. Aerial manipulators specifically designed for long reach manipulation have recently gained attention. In [85], a dual arm connected at the end of a flexible link attached to a UAV is designed. In [44] and [87], a manipulator is connected at the end of a rod and a platform, respectively, which in turn are attached to the UAV through a passive spherical joint and act as a pendulum. In [64, 81], two methods of designing a suspended platform from a larger aerial carrier using cables are presented. The platforms are actuated through winches and rotors and have manipulators mounted on the platforms to be used for manipulation tasks. Another approach [86] shows a dual arm manipulator suspended below a carrier UAV for long reach manipulation and inspection tasks. In these approaches, the manipulator is suspended directly below the carrier UAV, whereas our approach allows the manipulator platform to swing out from under the carrier UAV to reach areas unaffected by downdraft. Large UAVs may be required in situations where heavy lift capabilities are needed. However, several applications require access to locations where a large UAV is not well suited to operate in close proximity to the manipulated object. This can be due to factors such as heavy downdraft generated by the UAV, confined work spaces, or higher required operational accuracy. One such example is remote crop sampling, in which the downdraft from the UAV disturbs the crops and makes sampling difficult. Cleaning high-rise windows or solar panels is another example where it can be difficult to get close enough with a large UAV, but heavy lift capabilities are necessary to carry the cleaning products. In Chapter 4, we address these challenges by presenting a novel multi-body multi-rotor based aerial manipulator system. The proposed system consists of a bi-rotor actuated platform connected to a rigid rod which is suspended from a carrier UAV through a passive revolute joint. The presence of the passive revolute joint and the actuation of the platform using rotor thrust form the novel aspects of our design. The proposed aerial manipulator can be used for the long reach manipulation tasks presented above in which a larger UAV is required, however may not be well suited. Consider the case of remote crop sampling. Our design would allow the small suspended 8 manipulator platform to swing out to the side below the large carrier UAV to escape the downdraft. The horizontal bi-rotor actuation of the platform has the added benefit that it only produces air currents perpendicular to the manipulator, leaving the work space free from induced disturbances. We analyze and control the system assuming its operations are restricted to a plane. The system dynamics are formulated using the Lagrangian approach and it is shown that the bi-rotor platform pose, i.e., position and orientation, act as differentially flat outputs. An extended high-gain observer is designed to estimate states and disturbances acting on the system. An output feedback linearizing control law is designed for flat output trajectory tracking considering the coupled UAV-platform dynamics. The result is a feedback linearizing control approach that is robust to modeling error and disturbances. The system is rigorously analyzed to prove stability. 1.4 Organization and Contributions In this section, we present the organization of the remainder of the dissertation as well as the contributions of the work presented in the coming chapters. In Chapter 2, we design the base estimation and control strategy that will be used throughout this dissertation. The strategy utilizes a feedback linearizing controller that is augmented with an extended high-gain observer to provide estimates of unmeasured states and unmodeled disturbances. We further extend the observer dynamics to include the dynamics of a reference system, which may only be partially known, or completely unknown. As a result, the observer is able to estimate the the higher-order terms of the reference trajectory to enable feedforward control for improved transient tracking performance. The only information about the reference system that is needed to generate the trajectory information is the reference system position. The proposed estimation and control strategy is then rigorously analyzed to guarantee stability of the overall output feedback system. Finally, the method is then applied to the problem of tracking and landing on a moving ground vehicle with a multi-rotor UAV, which is realized in both simulation and experimental results to show the application of this method to a physical system. During the experiment, the ground vehicle is tele-operated by a human to ensure the trajectory is not known apriori. Several 9 different trajectories are traversed by the ground vehicle and the multi-rotor is able to track and land on the ground vehicle for each. We additionally propose a novel method for designing an extended high-gain observer for a system whose dynamics evolve on ๐‘†๐‘‚ (3). The contributions of this work presented in Chapter 2 are as follows. We design and analyze a robust feedback linearizing controller that mitigates modeling errors and external disturbances using their estimates. We design and rigorously analyze an extended high-gain observer to estimate modeling error and external disturbances, feedforward terms for trajectory tracking, and multi- rotor states for output feedback control. We show that the dynamics of commercial electronic speed controllers, which regulate the rotational rate of the rotors, evolve on the same timescale as the extended high-gain observer and therefore must be included in the dynamic model of the extended high-gain observer. We further illustrate the influence of inclusion/non-inclusion of these dynamics on output feedback performance through simulation. We rigorously characterize the stability of the overall output feedback system consisting of our feedback linearizing controller, extended high- gain observer, and show the incorporation of the multi-rotor actuator dynamics in our observer design. We illustrate the effectiveness of our output feedback controller through simulation and experimental results using the example of a multi-rotor landing on a mobile platform. Finally, we present a novel method for designing an extended high-gain observer for a system evolving on ๐‘†๐‘‚ (3) enabling output feedback control design for a multi-rotor which can avoid the singularities which are present in designing a control system using Euler angles. The observer also enables disturbance estimation to improve tracking performance. In Chapter 3, we extend the estimation and control strategy presented in Chapter 2 to enable detection, classification, and recovery from a complete failure of any one actuator for a hexrotor UAV during flight. We employ a family of extended high-gain observers, one for the nominal flight configuration, and one to model each failure mode. The observer estimates are still used in the feedback control to estimate unmeasured states and unmodeled disturbances, and are also used to estimate a Lypaunov function derivative of the system in real-time to detect an actuator failure. Once a failure is detected, the specific actuator that has failed is identified by comparing the norm of 10 the disturbance estimates from all of the observers, enabling the selection of a new control mapping to stabilize the system after failure. We rigorously analyze this method to guarantee closed-loop stability, selection of the appropriate model even in the presence of other disturbances, and we are able to establish a theoretical bound on the amount of time which can elapse before the model switch must occur to recover stability. The proposed method is validated through simulation results as well as experimental implementation on a physical system. The contributions of the work presented in Chapter 3 are as follows. We devise a failure recovery strategy that enables recovery of any single actuator failure on a hexrotor UAV, even in the presence of a broad class of disturbances. We rigorously analyze the proposed method to guarantee stability, guarantee correct model selection, and provide bounds on the maximum model switching time following an actuator failure. We further show that for an observer gain that is high enough, a model switch need not occur. The observer can estimate the large disturbance caused by the failure and correct for it, however this method is not practical to implement on a physical system as it requires a very high sample rate and would drastically amplify any noise present in the measurements. Finally, we illustrate the effectiveness of the proposed approach through simulation and experimental results. In Chapter 4, we design a novel multi-body multi-rotor UAV to be used for long reach manip- ulation tasks. Our proposed design incorporates a large carrier UAV which has a small platform, which itself is actuated by two rotors, suspended at the end of a long rigid rod. The platform can be outfitted with different types of manipulators depending on the task at hand. The rod is connected to the carrier UAV through a passive revolute joint. Since the platform is actuated at the end of the lever arm, the forces required are relatively small. This design allows for an arbitrarily long rod to attach the two bodies, thus allowing the manipulator to enter confined spaces that would not be possible with only a large UAV. We restrict the motion of the overall system to a plane and design a similar feedback linearizing output feedback control strategy that again utilizes extended high-gain observers to estimate unmeasured states and disturbances. The overall output feedback system is rigorously analyzed to prove stability. The proposed method is implemented in simulation in which 11 disturbances are applied, estimated, and canceled, resulting in excellent tracking performance. The contributions of the work presented Chapter 4 are as follows. We design a novel airframe configuration consisting of two bodies, both actuated by multiple rotors, which are connected through a rigid rod. We overcome the inherent underactuation present in the proposed airframe design through control of the differentially flat outputs to form a cascade connection between two systems. We adopt an extended high-gain observer to estimate a broad class of unmodeled disturbances and modeling errors to be canceled in the feedback control design. We rigorously analyze the system to guarantee stability of the closed-loop system under output feedback. Finally, we illustrate the effectiveness of the proposed approach through simulation. In Appendix A, we provide a detailed treatment of the steps necessary to implement these control systems on physical hardware. This includes system parameter identification, gain tuning, actuator dynamic response analysis, mapping rotor speed commands to rotor speeds and their associated forces, software implementation, and mapping control inputs in the form of body-fixed torques and collective thrust to required actuator forces. Appendix B shows how to prove stability of a generalized cascade system. The peaking phenomenon which is present in extended high-gain observers during the transient is discussed in Appendix C. The projections necessary to project the estimates from the extended high-gain observer back to ๐‘†๐‘‚ (3) are shown in Appendix D. Finally, a detailed controllability analysis of a hexrotor under actuator failure is shown in Appendix E. 12 CHAPTER 2 TRACKING AN UNKNOWN TRAJECTORY In this chapter, we lay the groundwork for the base estimation and control technology which enables robust trajectory tracking for a multi-rotor UAV in the presence of modeling error and external disturbances. We then immediately extend the estimation and control strategy to study the problem of estimating and tracking an unknown trajectory. The reference trajectory is unknown and generated from a reference system with unknown or partially known dynamics. We assume the only measurements that are available are the position and orientation of the multi-rotor and the position of the reference system. We adopt an extended high-gain observer (EHGO) estimation framework to estimate the unmeasured multi-rotor states, modeling error, external disturbances, and the reference trajectory. We design a robust output feedback controller for trajectory tracking that comprises a feedback linearizing controller and the EHGO. The proposed control method is rigorously analyzed to establish its stability properties. Finally, we illustrate our theoretical results through numerical simulation and experimental validation in which a multi-rotor tracks a moving ground vehicle with unknown trajectory and dynamics and successfully lands on the vehicle while in motion. This chapter draws from [10, 12]. Furthermore, we present a novel method to design an extended high-gain observer for a multi- rotor which has dynamics evolving on ๐‘†๐‘‚ (3). We begin by interpreting the rotation matrix, ๐‘…, and its derivative, ๐‘…,ยค as variables in R18 instead of interpreting them as variables in ๐‘†๐‘‚ (3) and its tangent space. This enables the design of the extended high-gain observer which can provide estimates of the higher-order states as well as any modeling error and external disturbances. The estimates from the observer must be projected back onto ๐‘†๐‘‚ (3) to be used in the control law (see Appendix D). Additionally, we design a second observer to estimate the desired rotational trajectory from a desired orientation. This enables the design of a translational control to prescribe a desired orientation, while preserving excellent tracking performance through the estimated feedforward terms in the rotational control. 13 The remainder of the chapter is organized as follows. The system dynamics are introduced in Section 2.1 with the control and observer design in Section 2.2. The controller is analyzed in Section 2.3 and is validated through simulation in Section 2.4 with experimental results presented in Section 2.5. Extended high-gain observer design on ๐‘†๐‘‚ (3) is presented in Section 2.6, and conclusions are presented in Section 2.7. 2.1 System Dynamics In this section, we review the dynamics of the different subsystems of a multi-rotor UAV and reference system. 2.1.1 Rotational Dynamics The rotational dynamics of the multi-rotor are ๐‰ = ๐ฝฮฉยค + ฮฉ ร— ๐ฝฮฉ, (2.1) where ๐ฝ โˆˆ R3ร—03 is the inertia matrix, and R3ร—03 denotes a 3ร—3 positive definite matrix. Additionally, ๐‰ โˆˆ R3 is the torque applied to the multi-rotor body and ฮฉ โˆˆ R3 is the angular velocity, each expressed in the body-fixed frame [55]. Consider the orientation of the multi-rotor expressed in terms of Z-Y-X Euler angles ๐‘1 = [๐œ™ ๐œƒ ๐œ“] > โˆˆ (โˆ’ ๐œ‹2 , ๐œ‹2 ) 2 ร— (โˆ’๐œ‹, ๐œ‹]. The angular velocity ฮฉ is related to the Euler angle rates ๐‘2 = [ ๐œ™ยค ๐œƒยค ๐œ“] ยค > โˆˆ R3 in the inertial frame as ๏ฃฎ ๏ฃน ๏ฃฏ1 ๐‘  ๐œ™ ๐‘ก ๐œƒ ๐‘ ๐œ™ ๐‘ก ๐œƒ ๏ฃบ ๏ฃฏ ๏ฃบ ฮฉ = ฮจโˆ’1 ๐‘2 , ๏ฃฏ ๏ฃบ ๐‘2 = ฮจฮฉ, ฮจ = ๏ฃฏ0 ๐‘ ๐œ™ ๏ฃฏ โˆ’๐‘  ๐œ™ ๏ฃบ๏ฃบ , ๏ฃฏ ๏ฃบ ๏ฃฏ0 ๐‘  ๐œ™ /๐‘ ๐œƒ ๐‘ ๐œ™ /๐‘ ๐œƒ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป where ๐‘ (ยท) , ๐‘  (ยท) , ๐‘ก (ยท) denote cos(ยท), sin(ยท), tan(ยท), respectively. The rotational dynamics can be equivalently written in terms of Euler angles as ๐‘ยค 1 = ๐‘2 , (2.2) ๐‘ยค 2 = ฮจฮจ ยค โˆ’1 ๐‘2 โˆ’ ฮจ๐ฝ โˆ’1 (ฮจโˆ’1 ๐‘2 ร— ๐ฝฮจโˆ’1 ๐‘2 ) + ฮจ๐ฝ โˆ’1 ๐‰ + ๐ˆ ๐œ‰ , 14 where ๐ˆ ๐œ‰ โˆˆ R3 is an added term to represent the lumped rotational disturbance and satisfies the following assumption. Definition 2.1 (Prime Canonical Form). A control system in the โ€œprime canonical formโ€ [66], for state ๐’™ โˆˆ R๐‘› , control input ๐‘ข โˆˆ R, and disturbance ๐œŽ โˆˆ R, has the following representation ๐’™ยค = ๐ดprm ๐’™ + ๐ตprm ๐‘“prm (๐‘ก, ๐’™, ๐‘ข), ๐‘ฆ = ๐ถprm ๐’™, (2.3) where ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ0๐‘›โˆ’1ร—1 ๐ผ๐‘›โˆ’1 ๏ฃบ ๏ฃฏ0๐‘›โˆ’1ร—1 ๏ฃบ =๏ฃฏ ๏ฃบ , ๐ตprm = ๏ฃฏ ๏ฃบ , ๐‘“prm : Rโ‰ฅ0 ร— R๐‘› ร— R โ†’ R, ๐ถprm = [1 01ร—๐‘›โˆ’1 ], ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐ดprm ๏ฃฏ 0 01ร—๐‘›โˆ’1 ๏ฃบ๏ฃบ ๏ฃฏ 1 ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป 0 ๐‘ร—๐‘ž is a matrix of zeros with dimension ๐‘ ร— ๐‘ž, ๐ผ ๐‘ is the identity matrix of dimension ๐‘, and ๐‘ฆ โˆˆ R is the measurement. Assumption 2.1 (Disturbance Properties). The dynamics of the various subsystems in this chapter take the prime canonical form perturbed by a disturbance term. We assume that the disturbance enters the RHS of (2.3) as ๐ต ๐‘‘ ๐œŽ where ๐ต ๐‘‘ = ๐ตprm , ๐œŽ is continuously differentiable, and its partial derivatives with respect to states are bounded on compact sets of those states for all ๐‘ก โ‰ฅ 0. Let ๐‘๐‘Ÿ = [๐œ™๐‘Ÿ ๐œƒ ๐‘Ÿ ๐œ“๐‘Ÿ ] > โˆˆ (โˆ’ ๐œ‹2 , ๐œ‹2 ) 2 ร— (โˆ’๐œ‹, ๐œ‹] and ๐‘ยค ๐‘Ÿ = [ ๐œ™ยค๐‘Ÿ ๐œƒยค๐‘Ÿ ๐œ“ยค ๐‘Ÿ ] > โˆˆ R3 be the rotational reference signals. Define the rotational tracking error variables ๐ƒ 1 = ๐‘ 1 โˆ’ ๐‘๐‘Ÿ , ๐ƒ 2 = ๐ƒยค1 = ๐‘2 โˆ’ ๐‘ยค ๐‘Ÿ , ๐ƒ = [๐ƒ > > > 1 ๐ƒ2 ] . The rotational dynamics (2.2) can now be written in terms of tracking error ๐ƒยค 1 = ๐ƒ 2 , (2.4) ๐ƒยค 2 = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยค ๐‘Ÿ ) + ๐บ (๐‘1 )๐‰ + ๐ˆ ๐œ‰ โˆ’ ๐‘ยฅ ๐‘Ÿ , where ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยค ๐‘Ÿ ) = ฮจฮจยค โˆ’1 (๐ƒ 2 + ๐‘ยค ๐‘Ÿ ) โˆ’ ฮจ๐ฝ โˆ’1 (ฮจโˆ’1 (๐ƒ 2 + ๐‘ยค ๐‘Ÿ ) ร— ๐ฝฮจโˆ’1 (๐ƒ 2 + ๐‘ยค ๐‘Ÿ )), ๐บ (๐‘1 ) = ฮจ๐ฝ โˆ’1 . 15 Suppose that only ๐‘ยคฬ„ ๐‘Ÿ , an estimate of ๐‘ยค ๐‘Ÿ , is known. Then (2.4) can be rewritten as ๐ƒยค 1 = ๐ƒ 2 , (2.5) ๐ƒยค 2 = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) + ๐บ (๐‘1 )๐‰ + ๐‡ ๐œ‰ , where ๐‡ ๐œ‰ = ๐ˆ ๐œ‰ โˆ’ ๐‘ยฅ ๐‘Ÿ + [ ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยค ๐‘Ÿ ) โˆ’ ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ )], which also satisfies Assumption 2.1 based on the properties of ๐‘“ and by assuming the reference trajectory is third-order continuously differentiable. 2.1.2 Translational Dynamics Let ๐’‘ 1 = [๐‘ฅ ๐‘ฆ ๐‘ง] > โˆˆ R3 and ๐’‘ 2 = [๐‘ฅยค ๐‘ฆยค ๐‘งยค] > โˆˆ R3 , respectively, be the position and velocity of the multi-rotor center of mass expressed in the inertial frame. Let the thrust generated by the ๐‘–-th rotor ร๐‘› ยฏ be ๐‘“ยฏ๐‘– โˆˆ R, and the total thrust force, ๐‘ข ๐‘“ = ๐‘–= 1 ๐‘“๐‘– โˆˆ R serves as the input to the translational system. Let the mass of the aerial platform be ๐‘š โˆˆ R0 , ๐‘” be the gravitational constant, ๐’† ๐‘ง = [0 0 1] > , and ๐ˆ ๐œŒ โˆˆ R3 be the lumped translational disturbance term which satisfies Assumption 2.1. Then, the translational dynamics [55] are ๐’‘ยค 1 = ๐’‘ 2 , ๐‘ข๐‘“ (2.6) ๐’‘ยค 2 = โˆ’ ๐‘…3 (๐‘1 ) + ๐‘”๐’† ๐‘ง + ๐ˆ ๐œŒ , ๐‘š where ๏ฃฎ ๏ฃน ๏ฃฏ ๐‘ ๐œ™ ๐‘  ๐œƒ ๐‘ ๐œ“ + ๐‘  ๐œ™ ๐‘ ๐œ“ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐‘…3 (๐‘1 ) = ๏ฃฏ๐‘ ๐œ™ ๐‘ ๐œƒ ๐‘ ๐œ“ โˆ’ ๐‘  ๐œ™ ๐‘ ๐œ“ ๏ฃบ๏ฃบ . ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‘๐œ™ ๐‘๐œƒ ๏ฃบ ๏ฃฐ ๏ฃป Let ๐’‘๐‘Ÿ = [๐‘ฅ๐‘Ÿ ๐‘ฆ๐‘Ÿ ๐‘ง๐‘Ÿ ] > 3 > 3 โˆˆ R and ๐’‘ยค ๐‘Ÿ = [๐‘ฅยค๐‘Ÿ ๐‘ฆยค ๐‘Ÿ ๐‘งยค๐‘Ÿ ] โˆˆ R be the translational reference signals. Define the translational error variables ๐†1 = ๐’‘ 1 โˆ’ ๐’‘๐‘Ÿ , ๐† 2 = ๐†ยค1 = ๐’‘ 2 โˆ’ ๐’‘ยค ๐‘Ÿ , ๐† = [๐† > > > 1 ๐†2 ] . The translational dynamics (2.6) can now be written in terms of tracking error as ๐†ยค 1 = ๐† 2 , ๐‘ข๐‘“ (2.7) ๐†ยค 2 = โˆ’ ๐‘…3 (๐‘1 ) + ๐‘”๐’† ๐‘ง + ๐ˆ ๐œŒ โˆ’ ๐’‘ยฅ ๐‘Ÿ . ๐‘š 16 2.1.3 Reference System Dynamics We assume that the reference trajectory that the multi-rotor UAV will track is generated by the system ๐’™ยค ๐‘1 = ๐’™ ๐‘2 , (2.8) ๐’™ยค ๐‘2 = ๐‘“๐‘ (๐’™ ๐‘ , ๐’– ๐‘ ), where ๐’™ ๐‘1 = [๐‘ฅ ๐‘ ๐‘ฆ ๐‘ ๐‘ง ๐‘ ] > โˆˆ R3 and ๐’™ยค ๐‘1 = [ ๐‘ฅยค๐‘ ๐‘ฆยค ๐‘ ๐‘งยค๐‘ ] > โˆˆ R3 are the position and velocity of the reference system, ๐’™ ๐‘ = [๐’™ > > > ๐‘ 1 , ๐’™ ๐‘ 2 ] is the system state, ๐’– ๐‘ is the unknown system input, and ๐‘“๐‘ (๐’™ ๐‘ , ๐’– ๐‘ ) is some unknown function. We take the system input ๐’– ๐‘ = ๐‘”๐‘ (๐‘ก, ๐’™ ๐‘ ) and let ๐œ• ๐‘“ยฏ๐‘ (๐‘ก,๐’™ ๐‘ ) ๐‘“ยฏ๐‘ (๐‘ก, ๐’™ ๐‘ ) = ๐‘“๐‘ (๐’™ ๐‘ , ๐’– ๐‘ ). We assume that ๐œ•๐’™ ๐‘ ๐’™ ยค๐‘ satisfies Assumption 2.1. In the case of tracking a moving ground vehicle, the reference signals will be taken as the reference system state, ๐’™ ๐‘ , and will be estimated using measurements of the ground vehicle position. 2.1.4 Actuator Dynamics and Mapping to Inputs The system dynamics, (2.4) and (2.7), take body-fixed torques, ๐‰, and total thrust force, ๐‘ข ๐‘“ , as inputs. The thrust and torques are generated by applying forces with each actuator. The force generated by rotor ๐‘– โˆˆ {1, . . . , ๐‘›} is ๐‘“ยฏ๐‘– = ๐‘๐œ”๐‘–2 , where ๐‘ โˆˆ R0 is a constant relating angular rate to force and ๐œ”๐‘– โˆˆ R0 is the ๐‘–-th rotor angular rate. These individual actuator forces are then mapped through a matrix, ๐‘€ โˆˆ R4ร—๐‘› , based on the geometry of the multi-rotor aerial platform, allowing the squared rotor angular rates to be treated as the system input through ๏ฃฎ ๏ฃน ๏ฃฏ๐‘ข ๐‘“ ๏ฃบ > ๏ฃฏ ๏ฃบ = ๐‘๐‘€๐Ž ๐‘  , ๐Ž ๐‘  = ๐œ”21 , . . . , ๐œ”2๐‘› .  (2.9) ๏ฃฏ ๏ฃบ ๏ฃฏ๐‰๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป The actuators typically used on multi-rotor UAVs are Brushless DC (BLDC) motors, which require electronic speed controllers (ESCs). Let the vector of desired rotor angular rates be ๐Ždes โˆˆ R๐‘› and ๐Ž โˆˆ R๐‘› be the vector of rotor angular rates. Due to the internal use of PI control, the ESCs introduce dynamic delays [22] of the following form ๐œ๐‘š ๐Ž ยค = (๐Ždes โˆ’ ๐Ž), (2.10) 17 where ๐œ๐‘š โˆˆ R0 is the time constant of the actuator system, see Appendix A for details. Typically the actuator dynamics are ignored in multi-rotor control design as they are sufficiently fast as compared with the rotational and translational dynamics and the control law. We also ignore the actuator dynamics in our control design, however, they are crucial in the dynamics of the EHGO used for output feedback control (see Remark 2.1 below). The actuator dynamics evolve on the same time-scale as the EHGO dynamics, and therefore cannot be ignored in EHGO design. Since feedback of the rotor angular rates is not available, they can be simulated by the following system ๐œ๐‘š ๐Žยคฬ‚ = (๐Ždes โˆ’ ๐Ž), ห† ห† ๐œ”(0) = 0๐‘›ร—1 , (2.11) where ๐Ž ห† โˆˆ R๐‘› is a vector of simulated rotor angular rates, and 0๐‘›ร—1 โˆˆ R๐‘›ร—1 is a vector of zeros. We will show in Section 2.3 that the use of simulated rotor speeds in place of measured rotor speeds still results in an exponentially stable closed-loop system. 2.2 Control and Observer Design A multi-rotor UAV is an underactuated mechanical system. While there can be ๐‘› โˆˆ {4, 6, 8, ...} rotors, only four degrees of freedom can be controlled in the classic configuration with co-planar rotors. To overcome the underactuation, as discussed below, the rotational dynamics are controlled to create a virtual control input for the translational dynamics. We begin by designing a trajectory tracking feedback linearizing controller for the rotational subsystem. The rotational trajectory is subsequently used to design a trajectory tracking controller for the translational subsystem in the presence of tracking errors in the rotational system. The controllers are designed under state feedback which requires the assumption that we not only have access to all states, but know the system disturbances exactly. This assumption is relaxed through the design of an EHGO to estimate states, disturbances, and the reference trajectory for use in output feedback control. 18 2.2.1 Rotational Control The rotational control feedback linearizes the rotational tracking error dynamics (2.4) by se- lecting the desired torque ๐‰ as ๐‰ ๐‘‘ = ๐บ โˆ’1 (๐‘1 ) [ ๐’‡ ๐‘Ÿ โˆ’ ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ )], (2.12) where ๐’‡ ๐‘Ÿ = โˆ’๐›ฝ1 ๐ƒ 1 โˆ’ ๐›ฝ2 ๐ƒ 2 โˆ’ ๐‡ ๐œ‰ , and ๐›ฝ1 , ๐›ฝ2 โˆˆ R0 are constant gains. Using (2.12) results in the following closed-loop rotational tracking error system ๐ƒยค 1 = ๐ƒ 2 , (2.13) ๐ƒยค 2 = โˆ’๐›ฝ1 ๐ƒ 1 โˆ’ ๐›ฝ2 ๐ƒ 2 . 2.2.2 Translational Control The translational control uses the total thrust, ๐‘ข ๐‘“ , as the direct control input and the desired roll and pitch trajectories, ๐œ™๐‘Ÿ and ๐œƒ ๐‘Ÿ , as virtual control inputs. The translational control is designed in view of potential roll and pitch trajectory tracking errors, leading to the following modification of the translational error dynamics (2.7) ๐†ยค 1 = ๐† 2 , ๐‘ข๐‘“ (2.14) ๐†ยค 2 = โˆ’ ๐‘…3 (๐‘๐‘Ÿ + ๐ƒ 1 ) + ๐‘”๐’† ๐‘ง + ๐ˆ ๐œŒ โˆ’ ๐’‘ยฅ ๐‘Ÿ . ๐‘š Define the perturbation due to rotational tracking error by ๐‘ข๐‘“ ๐’† ๐œ— (๐‘ก, ๐ƒ 1 ) = โˆ’ (๐‘…3 (๐‘๐‘Ÿ + ๐ƒ 1 ) โˆ’ ๐‘…3 (๐‘๐‘Ÿ )). (2.15) ๐‘š Then, (2.14) can be written as ๐†ยค 1 = ๐† 2 , ๐‘ข๐‘“ (2.16) ๐†ยค 2 = โˆ’ ๐‘…3 (๐‘๐‘Ÿ ) + ๐‘”๐’† ๐‘ง + ๐ˆ ๐œŒ โˆ’ ๐’‘ยฅ ๐‘Ÿ + ๐’† ๐œ— (๐‘ก, ๐ƒ 1 ). ๐‘š 19 Let ๐’‡ ๐‘ก = [ ๐‘“๐‘ฅ ๐‘“ ๐‘ฆ ๐‘“๐‘ง ] > be defined by ๐’‡ ๐‘ก = โˆ’๐›พ1 ๐† 1 โˆ’ ๐›พ2 ๐† 2 โˆ’ ๐ˆ ๐œŒ + ๐’‘ยฅ ๐‘Ÿ โˆ’ ๐‘”๐’† ๐‘ง , where ๐›พ1 , ๐›พ2 โˆˆ R0 are constant gains. Define the desired rotational references and desired total thrust by ยฉ โˆ’ ๐‘“๐‘ฆ ยช ๐œ™๐‘Ÿ = tanโˆ’1 ยญยญ q ยฎ , ๐œ“๐‘Ÿ = 0, ยฎ ๐‘“๐‘ฅ2 + ๐‘“๐‘ง2 (2.17) ยซ  ยฌ ๐‘“ ๐‘š ๐‘“๐‘ง ๐œƒ ๐‘Ÿ = tanโˆ’1 ๐‘ฅ , ๐‘ข๐‘“๐‘‘ = โˆ’ . ๐‘“๐‘ง ๐‘ ๐œ™๐‘Ÿ ๐‘ ๐œƒ ๐‘Ÿ ๐‘ข Then, โˆ’ ๐‘š๐‘“ ๐‘…3 (๐‘๐‘Ÿ ) = ๐’‡ ๐‘ก . Thus, using (2.17) leads to the following closed-loop translational subsystem with the inclusion of tracking error (2.15) from the rotational subsystem ๐†ยค 1 = ๐† 2 , (2.18) ๐†ยค 2 = โˆ’๐›พ1 ๐† 1 โˆ’ ๐›พ2 ๐† 2 + ๐’† ๐œ— (๐‘ก, ๐ƒ 1 ). Note that the rotational controller (2.12) requires the estimate ๐‘ยคฬ„ ๐‘Ÿ , however, only ๐‘๐‘Ÿ is given by the translational controller (2.17). The derivative of the reference trajectory ๐‘ยค ๐‘Ÿ can be computed analytically from the translational controller as ๐‘“ ๐‘ฆ ๐‘“ยค๐‘ฅ ๐‘“๐‘ฅ + ๐‘“ยค๐‘ง ๐‘“๐‘ง โˆ’ ๐‘“ยค๐‘ฆ ๐‘“๐‘ฅ2 + ๐‘“๐‘ง2   ๐œ™ยค๐‘Ÿ =  1/2 2  , ๐‘“๐‘ฅ2 + ๐‘“๐‘ง2 ๐‘“๐‘ฅ + ๐‘“ ๐‘ฆ2 + ๐‘“๐‘ง2 ๐‘“ยค๐‘ฅ ๐‘“๐‘ง โˆ’ ๐‘“๐‘ฅ ๐‘“ยค๐‘ง (2.19) ๐œƒยค๐‘Ÿ = , ๐‘“๐‘ฅ2 + ๐‘“๐‘ง2 ๐œ“ยค ๐‘Ÿ = 0, > where ยค๐’‡ ๐‘ก = ๐‘“ยค๐‘ฅ ๐‘“ยค๐‘ฆ ๐‘“ยค๐‘ง and  ยค๐’‡ = โˆ’๐›พ1 ๐† โˆ’ ๐›พ2 โˆ’ ๐‘ข ๐‘“ ๐‘…3 (๐‘1 ) + ๐‘”๐’† ๐‘ง + ๐ˆ ๐œŒ โˆ’ ๐’‘ยฅ โˆ’ ๐ˆ h i ๐‘ก 2 ๐‘Ÿ ยค ๐œŒ + ๐’‘๐‘Ÿ(3) . (2.20) ๐‘š The estimate ๐‘ยคฬ„ ๐‘Ÿ is obtained by setting ๐ˆ ยค ๐œŒ = 0 in the expression for ๐‘ยค ๐‘Ÿ . While the substitution (2.20) requires the third order derivative of the translational reference, it is shown in the EHGO design that the translational reference must be sixth order differentiable to be sufficiently smooth for estimation. 20 2.2.3 Extended High-Gain Observer Design A multi-input multi-output EHGO is designed similar to [52, 53] to estimate higher-order states of the error dynamic systems (2.4) and (2.7), uncertainties arising from modeling error and external disturbances, as well as the reference trajectory based on the reference system dynamics (2.8). It is shown in [9] that the actuator dynamics must be included in the dynamic model in the EHGO design. The dynamics (2.2), (2.6), and (2.8) can be combined into one set of equations for the observer where the state space is extended to include unknown disturbance dynamics. Since the third derivative of the reference trajectory is required by (2.20), the dynamics of the reference system are extended to include the third derivative of its position for estimation ๐†ยค 1 = ๐† 2 , ๐‘ข๐‘“ ๐†ยค 2 = โˆ’ ๐‘…3 (๐‘1 ) + ๐‘”๐’† ๐‘ง + ๐ˆ ๐œŒ โˆ’ ๐’‘ยฅ ๐‘Ÿ , ๐‘š ๐ˆยค ๐œŒ = ๐œ‘ ๐œŒ (๐‘ก, ๐†), ๐ƒยค 1 = ๐ƒ 2 , ๐ƒยค 2 = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) + ๐บ (๐‘1 )๐‰ + ๐‡ ๐œ‰ , (2.21) ๐‡ยค ๐œ‰ = ๐œ‘๐œ‰ (๐‘ก, ๐ƒ), ๐’™ยค ๐‘1 = ๐’™ ๐‘2 , ๐’™ยค ๐‘2 = ๐’™ ๐‘3 , ๐’™ยค ๐‘3 = ๐ˆ ๐‘ฅ๐‘ , ยค ๐‘ฅ๐‘ = ๐œ‘๐‘ฅ๐‘ (๐‘ก, ๐’™ ๐‘ ), ๐ˆ ๐œ• ๐‘“ยฏ๐‘ (๐‘ก,๐’™ ๐‘ ) where ๐ˆ ๐‘ฅ๐‘ = ๐œ•๐’™ ๐‘ ๐’™ ยค ๐‘. Since the reference system dynamics may not be known, they have been absorbed by the disturbance term in their entirety. If the reference system dynamics are partially known, then the nominal component can be included in the ๐’™ยค ๐‘3 expression. The estimated reference system states will be taken as the reference trajectory for the output feedback control. 21 We now define the state vectors ๐’’ = [๐† > > > > > 1 ๐†2 ๐ƒ 1 ๐ƒ 2 ] , ๐Œ1 = [๐† > > > > 1 ๐†2 ๐ˆ ๐œŒ ] , > > > > ๐Œ2 = [๐ƒ > > > > 1 ๐ƒ2 ๐‡๐œ‰ ] , ๐Œ3 = [๐’™ > ๐‘ 1 ๐’™ ๐‘ 2 ๐’™ ๐‘ 3 ๐ˆ ๐‘ฅ๐‘ ] , ๐Œ = [ ๐Œ> > > > 1 ๐Œ2 ๐Œ3 ] .  > Define ๐œ‘(๐‘ก, ๐’’, ๐’™ ๐‘ ) = ๐œ‘ ๐œŒ (๐‘ก, ๐†) ๐œ‘๐œ‰ (๐‘ก, ๐ƒ) ๐œ‘๐‘ฅ๐‘ (๐‘ก, ๐’™ ๐‘ ) , a vector of unknown functions describing the disturbance dynamics. Assumption 2.2 (Disturbance Dynamics). It is assumed ๐œ‘(๐‘ก, ๐’’, ๐’™ ๐‘ ) is continuous and bounded on any compact set containing ๐’’ and ๐’™ ๐‘ . Note that the second order derivative of the reference trajectory, ๐‘ยฅ ๐‘Ÿ , is lumped into the distur- bance ๐‡ ๐œ‰ . To ensure ๐‡ ๐œ‰ satisfies Assumption 2.1, ๐‘ยฅ ๐‘Ÿ must be differentiable, therefore by (2.19) and (2.20) the translational reference signals must be sixth order differentiable to be sufficiently smooth, however our design only requires estimates up to the third derivative. The observer system with extended states and a vector of simulated squared rotor speeds, ห† 21 , . . . , ๐œ” ห† ๐‘  = [๐œ” ๐Ž ห† 2๐‘› ] > from the system (2.11), as the control input through the mapping (2.9) is h i ยคฬ‚ = ๐ด ๐Œ ๐Œ ห† + ๐ต ๐‘“ยฏ( ๐ƒ, ห† ๐’™ห† ๐‘ , ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) + ๐บยฏ (๐‘1 ) ๐Ž ห† ๐‘  + ๐ป๐Œ ห† ๐‘’, 3 (2.22) ห†๐‘’ = ๐ถ( ๐Œ โˆ’ ๐Œ ๐Œ ห† ), where 3 3 3 3 ๐ด = โŠ•๐‘–= 1 ๐ด๐‘– , ๐ต = โŠ•๐‘–= 1 ๐ต๐‘– , ๐ถ = โŠ•๐‘–= 1 ๐ถ๐‘– , ๐ป = โŠ•๐‘–= 1 ๐ป๐‘– , ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ03 ๐ผ3 03 ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๐›ผ1 /๐œ– ๐ผ3 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐ด๐‘– = ๏ฃฏ๏ฃฏ03 03 ๐ผ3 ๏ฃบ๏ฃบ , ๐ต๐‘– = ๏ฃฏ๏ฃฏ ๐ผ3 ๏ฃบ๏ฃบ , ๐ป๐‘– = ๏ฃฏ๏ฃฏ๐›ผ2 /๐œ– 2 ๐ผ3 ๏ฃบ๏ฃบ , ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ03 03 03 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ 3 ๏ฃฏ๐›ผ3 /๐œ– ๐ผ3 ๏ฃบ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป h i ๐ถ๐‘– = ๐ผ3 03 03 , for ๐‘– โˆˆ {1, 2}, 22 ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ03 ๐ผ3 03 03 ๏ฃบ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๐›ผ1 /๐œ– ๐ผ3 ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ03 03 ๐ผ3 03 ๏ฃบ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ๐›ผ2 /๐œ– 2 ๐ผ3 ๏ฃบ ๐ด3 = ๏ฃฏ ๏ฃบ , ๐ต 3 = ๏ฃฏ ๏ฃบ , ๐ป3 = ๏ฃฏ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃบ, 3 ๏ฃฏ๐›ผ3 /๐œ– ๐ผ3 ๏ฃบ ๏ฃฏ0 03 03 ๐ผ3 ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ 3 ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃฏ03 03 03 03 ๏ฃบ ๏ฃฏ ๏ฃบ 0 ๏ฃฏ 4 ๐›ผ /๐œ– ๐ผ3 ๏ฃบ ๏ฃบ ๏ฃฐ 3๏ฃป ๏ฃฐ 4 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃฐ ๏ฃป ๏ฃป h i ๐ถ3 = ๐ผ3 03 03 03 , ๏ฃฎ ๏ฃน ๏ฃฎ โˆ’๐‘…3 (๐‘1 ) ๏ฃน ๏ฃฏ ๐‘”๐’† ๐‘ง โˆ’ ๐’™ห† ๐‘3 ๏ฃบ ๏ฃฏ 0 3 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‘š ๏ฃบ ๐‘“ยฏ( ๐ƒ, ยคฬ„ ห† ๐’™ห† ๐‘ , ๐‘1 , ๐‘๐‘Ÿ ) = ๏ฃฏ ๐‘“ ( ๐ƒ, ๏ฃฏ ยคฬ„ ๏ฃบ ๏ฃบ ยฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ห† ๐‘1 , ๐‘๐‘Ÿ ) ๏ฃบ , ๐บ (๐‘1 ) = ๐‘ ๏ฃฏ 03ร—1 ๐บ (๐‘1 ) ๏ฃบ ๐‘€, ๏ฃฏ ๏ฃบ 3 ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ 03ร—1 ๏ฃฏ 03ร—1 03 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป where โŠ• denotes the matrix direct sum, ๐ผ๐‘› โˆˆ R ๐‘›ร—๐‘› is the identity matrix of dimension ๐‘›, 0๐‘› โˆˆ R๐‘›ร—๐‘› is a square matrix of zeros, and ๐ป is designed by choosing ๐›ผ๐‘–๐‘— such that ๐‘  ๐œš๐‘– + ๐›ผ1๐‘– ๐‘  ๐œš๐‘– โˆ’1 + ยท ยท ยท + ๐›ผ๐‘–๐œš๐‘– โˆ’1 ๐‘  + ๐›ผ ๐œš๐‘– , (2.23) is Hurwitz, [ ๐œš 1 ๐œš2 ๐œš3 ] > = [3 3 4] > , and ๐œ– โˆˆ R0 is a positive constant that is chosen small enough. 2.2.4 Output Feedback Control For use in output feedback control, the estimates, ๐Œ ห† , must be saturated outside a compact set of interest to overcome the peaking phenomenon (see Appendix C). The following saturation function is used to saturate each estimate individually if |๐‘ฆ| โ‰ค 1,   ๏ฃฑ ๐œ’ห†๐‘– ๏ฃฒ ๐‘ฆ, ๏ฃด ๏ฃด ๐œ’ห†๐‘–๐‘  = ๐‘˜ ๐œ’๐‘– sat , sat(๐‘ฆ) = (2.24) ๐‘˜ ๐œ’๐‘– ๏ฃด sign(๐‘ฆ), ๏ฃด if |๐‘ฆ| > 1, ๏ฃณ for 1 โ‰ค ๐‘– โ‰ค 30, where the saturation bounds ๐‘˜ ๐œ’๐‘– are chosen such that the saturation functions will not be invoked under state feedback. The state feedback controllers (2.12) and (2.17) are rewritten as output feedback controllers using the saturated estimates h i ๐‰ห† ๐‘‘ = ๐บ โˆ’1 (๐‘1 ) ห†๐’‡ ๐‘Ÿ โˆ’ ๐‘“ ( ๐ƒ, ห† ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) , (2.25) 23 where ห†๐’‡ ๐‘Ÿ = โˆ’๐›ฝ1 ๐ƒห† 1 โˆ’ ๐›ฝ2 ๐ƒห† 2 โˆ’ ๐‡ห† ๐œ‰ and ยฉ โˆ’ ๐‘“ห†๐‘ฆ ยช ๐œ™ห†๐‘Ÿ = tanโˆ’1 ยญยญ q ยฎ , ๐œ“ห†๐‘Ÿ = 0, ยฎ ห†๐‘ฅ2 + ๐‘“ห†๐‘ง2 ๐‘“ ยซ ! ยฌ (2.26) ๐‘“ห†๐‘ฅ ๐‘š ๐‘“ห†๐‘ง ๐œƒห†๐‘Ÿ = tanโˆ’1 , ๐‘ขห† ๐‘“ ๐‘‘ = โˆ’ , ๐‘“ห†๐‘ง ๐‘ ๐œ™ห†๐‘Ÿ ๐‘ ๐œƒห†๐‘Ÿ where ห†๐’‡ ๐‘ก = โˆ’๐›พ1 ๐† ห† 1 โˆ’ ๐›พ2 ๐† ห†2 โˆ’ ๐ˆ ห† ๐œŒ + ๐’™ห† ๐‘3 โˆ’ ๐‘”๐’† ๐‘ง . Furthermore, these control inputs can be mapped to desired squared rotor speeds, ๐Ž ๐‘ ๐‘‘ โˆˆ R๐‘› , from the output feedback linearizing control signals ๐‘ขห† ๐‘“ ๐‘‘ and ๐‰ห† ๐‘‘ . For ๐‘› > 4, the inverse of (2.9) is an over-determined system which admits infinitely many solutions. In this case, we focus on the minimum energy solution ๏ฃฎ ๏ฃน 1 โ€  ๏ฃฏ๏ฃฏ๐‘ขห† ๐‘“ ๐‘‘ ๏ฃบ๏ฃบ ๐Ž ๐‘ ๐‘‘ = ๐‘€ ๏ฃฏ ๏ฃบ, where ๐‘€ โ€  = ๐‘€ > (๐‘€ ๐‘€ > ) โˆ’1 . (2.27) ๐‘ ๏ฃฏ ๐‰ห† ๏ฃบ ๏ฃฏ ๐‘‘๏ฃบ ๏ฃฐ ๏ฃป The square root of each component of ๐Ž ๐‘ ๐‘‘ acts as the reference signal, ๐Ždes , in (2.11) for the associated rotor, which in turn can be applied directly to the physical system. Remark 2.1 (Inclusion of Actuator Dynamics in EHGO). The actuator dynamics evolve on the same time-scale as the EHGO. Now, consider the case of an EHGO without actuator dynamics. While the actuators are changing their rotational rates according to (2.10) to apply the desired control input, the EHGO, with no knowledge of these relatively slow dynamics, will observe this delayed application of control as a large disturbance. In an effort to cancel this perceived disturbance, a larger control action is commanded. This causes the system to overshoot the reference dramatically. The opposite action occurs in trying to correct for the overshoot, resulting in aggressive oscillations that can destabilize the system. An example of this behavior is shown in Fig. 2.1, where the rotational subsystem is simulated with and without actuator dynamics in the observer. There is no nominal disturbance applied to the system, however, the disturbance estimate from the observer without actuator dynamics oscillates quickly between its saturation bounds. In this case the saturation bounds were chosen small 24 10 5 0 -5 -10 10 11 12 13 14 15 16 0.5 0 -0.5 10 11 12 13 14 15 16 Figure 2.1: Simulated rotational system response with and without actuator dynamics included in the EHGO. The disturbance estimate, ๐œŽ ห† 2 , when actuator dynamics are omitted oscillates between the saturation bounds (top), inducing oscillations in the tracking performance of ๐œ™2 (bottom). The disturbance estimate, ๐œŽ ห† 1 , and tracking, ๐œ™1 , show excellent performance when actuator dynamics are included in the EHGO for this example. enough to prevent the system from becoming unstable to illustrate the oscillatory behavior induced by the omission of the actuator dynamics. When the EHGO has a model of how the actuators are dynamically applying the desired control action, there is no longer a perceived disturbance due to the actuator delay, and the system functions nominally. 2.3 Stability Analysis In this section, we will derive the requirements of the initial conditions that ensure that the proposed controller is well defined throughout operation. We then establish stability of the state feedback control, observer estimates, and output feedback control. 25 2.3.1 Restricting Domain of Operation The domain of operation must be restricted in order to ensure that the rotational feedback linearizing control law remains well defined. To ensure the expressions in (2.17) are well-defined, we introduce the following assumption. Assumption 2.3. The rotational reference signals remain in the set {|๐œ™๐‘Ÿ | < ๐œ‹2 โˆ’๐›ฟ, |๐œƒ ๐‘Ÿ | < ๐œ‹2 โˆ’๐›ฟ, |๐œ“๐‘Ÿ | < ๐œ‹ 2 โˆ’ ๐›ฟ}, where 0 < ๐›ฟ < ๐œ‹2 . To ensure the rotational tracking error is well defined, i.e., the magnitude of each entry of ๐ƒ 1 is smaller than ๐œ‹2 , and to ensure singularities of the Z-Y-X Euler angle representation at ๐œƒ = ยฑ ๐œ‹2 , the rotational states must remain in the set {|๐œ™| < ๐œ‹2 , |๐œƒ| < ๐œ‹2 , |๐œ“| < ๐œ‹2 , | ๐œ™| ยค < ๐‘Ž ๐œƒ , | ๐œ“| ยค < ๐‘Ž ๐œƒ , | ๐œƒ| ยค < ๐‘Ž ๐œƒ }, ๐œ‹ where ๐‘Ž ๐œƒ is some positive constant. The magnitude of each entry of ๐ƒ 1 should be smaller than 2 to ensure that the rotational error is well-defined. We will now establish that for sufficiently small initial tracking error, ๐ƒ (0), the tracking error k๐ƒ 1 (๐‘ก) k < ๐›ฟ for all ๐‘ก > 0. A Lyapunov function in the rotational error dynamics is ๐‘‰๐œ‰ = ๐ƒ > ๐‘ƒ๐œ‰ ๐ƒ, where ๐‘ƒ๐œ‰ ๐ด๐œ‰ + ๐ด๐œ‰> ๐‘ƒ๐œ‰ = โˆ’๐ผ6 , (2.28) ๏ฃฎ ๏ฃน ๏ฃฏ 03 ๐ผ 3 ๏ฃบ ๐ด๐œ‰ = ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃบ. ๏ฃฏโˆ’๐›ฝ ๐ผ โˆ’๐›ฝ ๐ผ ๏ฃบ ๏ฃฏ 1 3 2 3๏ฃบ ๏ฃฐ ๏ฃป A Lyapunov function in the translational error dynamics is ๐‘‰๐œŒ = ๐† > ๐‘ƒ ๐œŒ ๐†, where ๐‘ƒ ๐œŒ ๐ด ๐œŒ + ๐ด>๐œŒ ๐‘ƒ ๐œŒ = โˆ’๐ผ6 , (2.29) ๏ฃฎ ๏ฃน ๏ฃฏ 03 ๐ผ 3 ๏ฃบ ๐ด๐œŒ = ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃบ. ๏ฃฏโˆ’๐›พ ๐ผ โˆ’๐›พ ๐ผ ๏ฃบ ๏ฃฏ 1 3 2 3๏ฃบ ๏ฃฐ ๏ฃป > > Solving ๐‘ƒ๐œ‰ ๐ด๐œ‰ + ๐ด๐œ‰ ๐‘ƒ๐œ‰ = โˆ’๐ผ6 for ๐‘ƒ๐œ‰ and ๐‘ƒ ๐œŒ ๐ด ๐œŒ + ๐ด ๐œŒ ๐‘ƒ ๐œŒ = โˆ’๐ผ6 for ๐‘ƒ ๐œŒ yields ๏ฃฎ ๐›ฝ2 +๐›ฝ1 +๐›ฝ2 ๏ฃน ๏ฃฎ ๐›พ 2 +๐›พ1 +๐›พ 2 ๏ฃน ๏ฃฏ 1 2 1 ๏ฃฏ 1 2 1 ๏ฃฏ 2 ๐›ฝ1 ๐›ฝ2 ๐ผ 3 2 ๐›ฝ1 ๐ผ 3 ๏ฃฏ 2 ๐›พ1 ๐›พ2 ๐ผ 3 2 ๐›พ1 ๐ผ 3 ๏ฃบ ๏ฃบ ๐‘ƒ๐œ‰ = ๏ฃฏ ๐‘ƒ๐œŒ = ๏ฃฏ ๏ฃบ ๏ฃบ ๏ฃบ, ๏ฃบ. ๐›ฝ 1 +1 ๐›พ 1 +1 ๏ฃบ ๏ฃฏ 21๐›ฝ1 ๐ผ3 ๏ฃฏ 21๐›พ1 ๐ผ3 ๏ฃฏ ๐ผ ๏ฃบ ๏ฃฏ ๐ผ ๏ฃฐ 2 ๐›ฝ1 ๐›ฝ2 3 ๏ฃบ๏ฃป ๏ฃฐ 2๐›พ1 ๐›พ2 3 ๏ฃบ๏ฃป 26 Let ๐‘ ๐œ‰ โˆˆ R0 be chosen such that ๐‘ ๐œ‰ < (๐›ฝ1 + 1)๐›ฟ2 /(2๐›ฝ2 ), and let ฮฉ๐œ‰ = {๐‘‰๐œ‰ < ๐‘ ๐œ‰ }. Since ๐’† ๐œ— (๐‘ก, ๐ƒ 1 ) and its partial derivatives are continuous on ฮฉ๐œ‰ , and ๐’† ๐œ— is uniformly bounded in time, it is locally Lip- schitz in ฮฉ๐œ‰ and let ๐ฟ ๐‘’ be the associated Lipschitz constant. Take ๐‘ ๐œŒ > ๐œ†max (๐‘ƒ ๐œŒ )(2๐ฟ ๐‘’ ๐›ฟ๐œ†max (๐‘ƒ ๐œŒ )) 2 , where ๐œ†max (ยท) is the maximum eigenvalue of the argument, and let ฮฉ ๐œŒ = {๐‘‰๐œŒ < ๐‘ ๐œŒ }. Define the domain of operation ฮฉ๐‘ž = ฮฉ๐œ‰ ร— ฮฉ ๐œŒ . Lemma 2.1 (Restricting the Domain of Operation). For the feedback linearized rotational error dynamics (2.13) with initial conditions ๐ƒ (0) โˆˆ ฮฉ๐œ‰ the system state ๐ƒ (๐‘ก) remains in the set k๐ƒ 1 (๐‘ก) k < ๐›ฟ for all ๐‘ก > 0. Similarly, the feedback linearized translational error dynamics (2.18) with initial conditions ๐†(0) โˆˆ ฮฉ ๐œŒ , the system state ๐†(๐‘ก) remains in ฮฉ ๐œŒ , for all ๐‘ก > 0. Proof. Substituting ๐‘ƒ๐œ‰ in (2.28), the rotational tracking error Lyapunov function can be written as (๐›ฝ1 + 1)๐ƒ > 1 ๐ƒ1 ๐›ฝ1 ๐ƒ > > 2 ๐ƒ 2 + (๐›ฝ2 ๐ƒ 1 + ๐ƒ 2 ) (๐›ฝ2 ๐ƒ 1 + ๐ƒ 2 ) ๐‘‰๐œ‰ = + . 2๐›ฝ2 2๐›ฝ1 ๐›ฝ2 Taking the bound on the Lyapunov function (๐›ฝ1 + 1)๐ƒ > 1 ๐ƒ1 ๐‘‰๐œ‰ โ‰ค ๐‘ ๐œ‰ โ‡’ โ‰ค ๐‘๐œ‰ , 2๐›ฝ2 and choosing ๐‘ ๐œ‰ in the following manner (๐›ฝ1 + 1)๐›ฟ2 ๐‘๐œ‰ < โ‡’ k๐ƒ 1 (๐‘ก)k < ๐›ฟ, 2๐›ฝ2 over the set ฮฉ๐œ‰ . The Lyapunov function (2.28) also satisfies the following inequalities ๐œ† min (๐‘ƒ๐œ‰ ) k๐ƒ k 2 โ‰ค ๐‘‰๐œ‰ โ‰ค ๐œ† max (๐‘ƒ๐œ‰ ) k๐ƒ k 2 , ๐‘‰ยค๐œ‰ โ‰ค โˆ’ k๐ƒ k 2 , where ๐œ† min (ยท) is the minimum eigenvalue of the argument, showing that ฮฉ๐œ‰ is positively invariant. In view of potential rotational tracking errors, the translational tracking error Lyapunov function (2.29) satisfies the following inequalities ๐œ† min (๐‘ƒ ๐œŒ ) k ๐†k 2 โ‰ค ๐‘‰๐œŒ โ‰ค ๐œ† max (๐‘ƒ ๐œŒ ) k ๐†k 2 , ๐‘‰ยค ๐œŒ โ‰ค โˆ’ k ๐†k 2 + 2[03ร—1 ๐‘’ ๐œ— (๐‘ก, ๐ƒ 1 ) > ] > ๐‘ƒ ๐œŒ ๐†. 27 Since ๐‘’ ๐œ— (๐‘ก, ๐ƒ 1 ) and its partial derivatives are continuous on ฮฉ๐œ‰ , and ๐‘’ ๐œ— is uniformly bounded in time, ๐‘’ ๐œ— is Lipschitz in ๐ƒ 1 on ฮฉ๐œ‰ . We can now define k๐‘’ ๐œ— (๐‘ก, ๐ƒ 1 ) โˆ’ ๐‘’ ๐œ— (๐‘ก, 0) k โ‰ค ๐ฟ ๐‘’ k๐ƒ 1 k โ‰ค ๐ฟ ๐‘’ ๐›ฟ, for the Lipschitz constant, ๐ฟ ๐‘’ . We can then bound the translational Lyapunov function derivative by ๐‘‰ยค ๐œŒ โ‰ค โˆ’ k ๐†k 2 + 2๐ฟ ๐‘’ ๐›ฟ๐œ†max (๐‘ƒ ๐œŒ ) k ๐†k . For k ๐†k > 2๐ฟ ๐‘’ ๐›ฟ๐œ† max (๐‘ƒ ๐œŒ ), ๐‘‰ยค ๐œŒ < 0. Since ๐‘‰๐œŒ โ‰ค ๐œ† max (๐‘ƒ ๐œŒ ) k ๐†k 2 we can choose ๐‘ ๐œŒ > ๐œ†max (๐‘ƒ ๐œŒ )(2๐ฟ ๐‘’ ๐›ฟ๐œ†max (๐‘ƒ ๐œŒ )) 2 . (2.30) By this choice, ๐‘‰ยค ๐œŒ < 0 for ๐‘‰๐œŒ โ‰ฅ ๐‘ ๐œŒ , hence ฮฉ ๐œŒ is compact and positively invariant. Thus, the domain of operation ฮฉ๐‘ž = ฮฉ๐œ‰ ร— ฮฉ ๐œŒ is positively invariant.  Remark 2.2. By Lemma 2.1 and Assumption 2.3, the rotational states remain in the set {|๐œ™| < ๐œ‹ < ๐œ‹2 , | ๐œ™| ยค < ๐‘Ž ๐œƒ }, where ๐‘Ž ๐œƒ is some positive constant. Thereby ensuring singularities ยค < ๐‘Ž ๐œƒ , | ๐œƒ| 2 , |๐œƒ| in the Euler angles are avoided and the feedback linearizing controllers (2.12) and (2.17) remain well defined. Furthermore, we will restrict the domain of operation of the reference system by defining the set ฮฉ๐‘ฅ ๐‘ = {k๐’™ ๐‘1 k < ๐‘Ž 1 , k๐’™ ๐‘2 k < ๐‘Ž 2 , k๐’™ ๐‘3 k < ๐‘Ž 3 } for ๐‘Ž 1 , ๐‘Ž 2 , ๐‘Ž 3 โˆˆ R0 . 2.3.2 Stability Under State Feedback Theorem 2.1 (Stability Under State Feedback). For the closed-loop state feedback rotational and translational subsystems, (2.13) and (2.18), if the initial conditions (๐ƒ (0), ๐†(0)) โˆˆ ฮฉ๐‘ž , the system states (๐ƒ (๐‘ก), ๐†(๐‘ก)) โˆˆ ฮฉ๐‘ž for all ๐‘ก > 0. Additionally, the states will exponentially converge to the origin. 28 Proof. The translational and rotational closed-loop systems can be written as a cascaded system in the following form ๐†ยค 1 = ๐† 2 , ๐†ยค 1 = ๐† 2 , ๐†ยค 2 = โˆ’๐›พ1 ๐† 1 โˆ’ ๐›พ2 ๐† 2 + ๐’† ๐œ— (๐‘ก, ๐ƒ 1 ), ๐†ยค 2 = ๐‘“1 (๐‘ก, ๐†, ๐ƒ), โ‡’ ๐ƒยค 1 = ๐ƒ 2 , ๐ƒยค 1 = ๐ƒ 2 , ๐ƒยค 2 = โˆ’๐›ฝ1 ๐ƒ 1 โˆ’ ๐›ฝ2 ๐ƒ 2 , ๐ƒยค 2 = ๐‘“2 (๐ƒ). Taking the Lyapunov functions for the rotational and translational subsystems, (2.28) and (2.29), a composite Lyapunov function can be written ๐‘‰๐‘  ๐‘“ = ๐‘‘1๐‘‰๐œŒ + ๐‘‰๐œ‰ , ๐‘‘1 > 0. (2.31) Since ๐‘‰๐œŒ satisfies (B.3) on ฮฉ ๐œŒ , ๐‘‰๐œ‰ satisfies (B.2) on ฮฉ๐œ‰ , and ๐‘“1 (๐‘ก, ๐†, ๐ƒ) is Lipshitz in ๐ƒ on ฮฉ๐œ‰ , it can be shown following the generalized proof in Appendix B that for ๐‘‘1 small enough, the entire closed-loop state feedback system converges exponentially to the origin for any trajectory starting within the domain of operation, ฮฉ๐‘ž .  2.3.3 Convergence of Observer Estimates The scaled error dynamics of the EHGO are written by making the following change of variables ( ๐œ’๐‘–๐‘— โˆ’ ๐œ’ห†๐‘–๐‘— ) ๐œ‚๐‘–๐‘— = , หœ ๐‘  = ๐Ž๐‘  โˆ’ ๐Ž ๐Ž ห† ๐‘ , (2.32) ๐œ– ๐œš๐‘– โˆ’ ๐‘— where ๐œ’๐‘–๐‘— is the ๐‘—-th element of ๐Œ๐‘– for 1 โ‰ค ๐‘– โ‰ค 3 and 1 โ‰ค ๐‘— โ‰ค ๐œš๐‘– , and ๐œ’ห†๐‘–๐‘— is the estimate of ๐œ’๐‘–๐‘— obtained using the EGHO. In the new variables, the scaled EHGO estimation error dynamics become ๐œ– ๐œผยค ๐‘– = ๐น๐‘– ๐œผ๐‘– + ๐ต๐‘–1 ฮ” ๐‘“ยฏ๐‘– + ๐บยฏ ๐‘– (๐‘1 ) ๐Ž   หœ ๐‘  + ๐œ– ๐ต๐‘–2 ๐œ‘๐‘– (๐‘ก, ๐’’, ๐’™ ๐‘ ), (2.33) 29 where ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ โˆ’๐›ผ1๐‘– ๐ผ3 ๐ผ3 ยท ยท ยท 03 ๏ฃบ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ .. .. .. ๏ฃบ ๏ฃบ ๏ฃฏ.๏ฃบ ๏ฃฏ .. ๏ฃบ ๏ฃฏ . . .๏ฃบ ๐‘– ๐น๐‘– = ๏ฃฏ = ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃบ , ๐ต 1 ๏ฃฏ ๏ฃบ, ๏ฃฏโˆ’๐›ผ๐‘– ๐ผ 0 ยท ยท ยท ๐ผ3 ๏ฃบ ๏ฃบ ๏ฃฏ๐ผ ๏ฃบ ๏ฃฏ ๐œš ๐‘– โˆ’1 3 3 ๏ฃฏ 3๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ โˆ’๐›ผ๐‘–๐œš๐‘– ๐ผ3 03 ยท ยท ยท 03 ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป > > h i > ๐ต๐‘–2 = [03 ยท ยท ยท 03 ๐ผ3 ] > , ๐œผ๐‘– = ๐œผ๐‘–1 ยท ยท ยท ๐œผ๐‘–๐œš๐‘– , ห† ๐’™ห† ๐‘ , ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) and ๐‘“ยฏ๐‘– , ๐บยฏ ๐‘– , and ๐œ‘๐‘– correspond to rows 3๐‘– โˆ’ 2 to 3๐‘– and ฮ” ๐‘“ยฏ๐‘– = ๐‘“ยฏ๐‘– (๐ƒ, ๐’™ ๐‘3 , ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) โˆ’ ๐‘“ยฏ๐‘– ( ๐ƒ, 3 of ๐‘“ยฏ, ๐บ, ยฏ and ๐œ‘, respectively. Note (2.33) is an ๐‘‚ (๐œ–) perturbation of ๐œ– ๐œผยค ๐‘– = ๐น๐‘– ๐œผ๐‘– + ๐ต๐‘–1 ฮ” ๐‘“ยฏ๐‘– + ๐บยฏ ๐‘– (๐‘1 ) ๐Ž   หœ๐‘  . (2.34) The actuator error dynamics in terms of the error in squared rotor angular rate, ๐Ž หœ ๐‘  , and rotor angular rate error, ๐Ž หœ =๐Žโˆ’๐Ž ห† can be written as ๐œ๐‘š ๐Ž ยคฬƒ ๐‘  = โˆ’2๐Ž หœ ๐‘  + 2๐‘Šdes ๐Ž, หœ (2.35) ๐œ๐‘š ๐Žยคฬƒ = โˆ’๐Ž, หœ โˆš where ๐‘Šdes = diag[ ๐œ” ๐‘ ๐‘‘๐‘– ] โˆˆ R๐‘›ร—๐‘› for ๐‘– โˆˆ {1, . . . , ๐‘›} is time-varying. By exploiting the fact that โˆš ๐‘Šdes is bounded, i.e., ๐œ” ๐‘ ๐‘‘๐‘– โ‰ค ๐œ”max , for each i, where ๐œ”max โˆˆ R is the maximum achievable rotor angular rate, the actuator error dynamics (2.35) can be analyzed as a cascaded system with the Lyapunov functions ๐‘‰๐œ”หœ๐‘  = ๐Ž หœ>๐‘ ๐Žหœ ๐‘ , ๐‘‰๐œ”หœ = ๐Ž หœ > ๐Ž, หœ (2.36) and the composite Lyapunov function ๐‘‰๐œ” = ๐‘‘2๐‘‰๐œ”หœ๐‘  + ๐‘‰๐œ”หœ , ๐‘‘2 > 0, (2.37) where ๐‘‘2 is sufficiently small (see Appendix B for details). Define the set ฮฉ๐œ” = {๐‘‰๐œ” โ‰ค ๐‘ ๐œ” } where ๐‘ ๐œ” โˆˆ R0 is an arbitrary constant. โˆš Lemma 2.2 (Stability of Actuator Dynamics). For bounded input, ๐Ž ๐‘ ๐‘‘๐‘– for ๐‘– โˆˆ {1, . . . , ๐‘›}, the actuator error dynamics (2.35) will globally exponentially converge to the origin. Therefore, the simulated rotor angular rates, ๐Ž, ห† exponentially converge to the actual rotor angular rates, ๐Ž. 30 Proof. The Lyapunov functions for the actuator dynamics (2.35) are ๐‘‰๐œ”หœ๐‘  and ๐‘‰๐œ”หœ from (2.36), with the composite Lyapunov function (2.37). Since ๐‘‰๐œ”หœ๐‘  satisfies (B.3) globally, ๐‘‰๐œ”หœ satisfies (B.2) globally, and ๐Ž ยคฬƒ ๐‘  is globally Lipschitz in ๐Ž หœ since ๐‘Šdes is bounded, using the general result for cascaded systems in Appendix B, it can be shown that the origin is globally exponentially stable when ๐‘‘2 is chosen small enough.  The systems (2.34) and (2.35) form the cascaded system ๐œ– ๐œผยค ๐‘– = ๐น๐‘– ๐œผ๐‘– + ๐ต๐‘–1 ฮ” ๐‘“ยฏ๐‘– + ๐บยฏ ๐‘– (๐‘1 ) ๐Ž   หœ๐‘  , ยคฬƒ ๐‘  = โˆ’2๐Ž ๐œ๐‘š ๐Ž หœ ๐‘  + 2๐‘Šdes ๐Ž, หœ (2.38) ๐œ๐‘š ๐Žยคฬƒ = โˆ’๐Ž. หœ We now define the state vector of scaled observer error and actuator error as ๐šซ = [๐œผ1 ๐œผ2 ๐œผ3 ๐Ž หœ >. หœ ๐‘  ๐Ž] In comparison with a standard EHGO, (2.38) has additional vanishing perturbation terms with associated dynamics. In the following theorem, we establish that these perturbation terms do not affect the convergence of the EHGO. Furthermore, the perturbation term in (2.33) is continuous and can be bounded by ๐œ– ๐œ‘(๐‘ก, ๐’’, ๐’™ ๐‘ ) โ‰ค ๐œ– ๐œ… for ๐œ… โˆˆ R0 , and can be treated as a nonvanishing perturbation. Using [40, Lemma 9.2], it can be shown that the perturbed observer error dynamics converge to an ๐‘‚ (๐œ– ๐œ…) neighborhood of the origin. A Lyapunov function for the EHGO error system (2.34) with the input, ๐Ž หœ ๐‘  , set to zero is ร• 3 ๐œ–๐‘‰๐œ‚ = (๐œผ๐‘– ) > ๐‘ƒ๐œ‚๐‘– ๐œผ๐‘– , ๐‘ƒ๐œ‚๐‘– ๐น๐‘– + ๐น๐‘–> ๐‘ƒ๐œ‚๐‘– = โˆ’๐ผ3 ๐œš๐‘– . (2.39) ๐‘–=1 A composite Lyapunov function for (2.38) is ๐‘‰ฮ” = ๐‘‘3๐‘‰๐œ‚ + ๐‘‰๐œ” , ๐‘‘3 > 0, (2.40) where ๐‘‘3 is sufficiently small (see Appendix B for details). Recall that ( ๐Œ21 , ๐Œ22 ) = (๐ƒ 1 , ๐ƒ 2 ). Also, the estimates of (๐ƒ 1 , ๐ƒ 2 ) can be expressed as ๐ƒห† 1 = ๐ƒ 1 โˆ’ ๐œ– 2 ๐œผ21 and ๐ƒห† 2 = ๐ƒ2 โˆ’ ๐œ–๐œผ22 . Consider a strict subset of ฮฉ๐œ‰ , defined by ฮฉsub ๐œ‰ โŠ‚ ฮฉ๐œ‰ . Define ฮฉ๐œ‚ = {(๐œผ1 , ๐œผ2 , ๐œผ3 ) โˆˆ R10 | ( ๐ƒห† 1 , ๐ƒห† 2 ) โˆˆ ฮฉ๐œ‰ , โˆ€๐ƒ โˆˆ ฮฉsub ๐œ‰ }. Let ๐‘ ฮ” โˆˆ R0 be the largest constant such that ฮฉฮ” = {๐‘‰ฮ” โ‰ค ๐‘ ฮ” } is contained in ฮฉ๐œ‚ ร— ฮฉ๐œ” . 31 Theorem 2.2 (Convergence of EHGO Estimates). There exists sufficiently small ๐œ– โˆ— such that for all ๐œ– โˆˆ (0, ๐œ– โˆ— ), ฮฉฮ” is positively invariant, and for each ๐šซ(0) โˆˆ ฮฉฮ” , ๐šซ(๐‘ก) converges exponentially to an ๐‘‚ (๐œ– ๐œ…) neighborhood of the origin. Proof. The Lyapunov function for the actuator error system is (2.37) and the Lyapunov function for the EHGO error system with the input, ๐Ž หœ ๐‘  , set to zero is (2.39). A composite Lyapunov function for the cascaded system (2.38) is (2.40). The function ๐‘“ยฏ๐‘– (๐ƒ, ๐’™ ๐‘3 , ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) is Lipschitz in ๐ƒ and ๐’™ ๐‘3 on ฮฉ๐œ‰ ร— ฮฉ๐‘ฅ ๐‘ and ( ๐ƒห† 1 , ๐ƒห† 2 ) โˆˆ ฮฉ๐œ‰ . Thus, ฮ” ๐‘“ยฏ๐‘– can be bounded by ฮ” ๐‘“ยฏ๐‘– โ‰ค ๐ฟ ๐œ‚ ๐Œ๐‘– โˆ’ ๐Œ ห† ๐‘– โ‡’ ฮ” ๐‘“ยฏ๐‘– โ‰ค ๐œ– ๐ฟ ๐œ‚ ๐œผ๐‘– , leading to the following bound on the derivative of the Lyapunov function 3   ๐‘– 2 ๐‘– 2 ร• ๐œ– ๐‘‰ยค๐œ‚ โ‰ค โˆ’ ๐œผ + 2๐œ– ๐ฟ ๐œ‚ ๐œผ ๐‘ƒ๐œ‚๐‘– ๐ต๐‘–1 , ๐‘–=1 ๐œ– ๐‘‰ยค๐œ‚ โ‰ค โˆ’ k๐œผk 2 + 2๐œ– ๐ฟ ๐œ‚ k๐‘ k k๐œผk 2 , where the elements of the diagonal matrix ๐‘ are ๐‘๐‘– = ๐‘ƒ๐œ‚๐‘– ๐ต๐‘–1 . Since ๐›ผ๐‘–๐‘— are tunable and ๐œ– is a 1 design parameter, pick ๐œ– such that 2๐œ– ๐ฟ ๐œ‚ k๐‘ k โ‰ค 2 resulting in the following inequality 1 ๐œ– ๐‘‰ยค๐œ‚ โ‰ค โˆ’ k๐œผk 2 . (2.41) 2 The composite Lyapunov function (2.40) consists of ๐‘‰๐œ‚ and ๐‘‰๐œ” , where ๐‘‰๐œ‚ satisfies (B.3) on ฮฉ๐œ‚ , ๐‘‰๐œ” satisfies (B.2) on ฮฉ๐œ” , and ๐œ– ๐œผยค is Lipschitz in ๐Ž หœ ๐‘  on ฮฉ๐œ” . Following Appendix B, the origin of (2.38) is exponentially stable for any trajectory starting in ฮฉฮ” . Furthermore, the cascade connection of the complete scaled observer error system (2.33) and the actuator error dynamics (2.35) is the same as (2.38) with perturbation. The perturbation is bounded by ๐œ– ๐œ‘(๐‘ก, ๐’’, ๐’™ ๐‘ ) < ๐œ– ๐œ… and is continuous, therefore it can be treated as a nonvanishing perturbation. Following [40, Lemma 9.2], the estimation error of the EHGO converges exponentially to an ๐‘‚ (๐œ– ๐œ…) neighborhood of the origin. Furthermore, ฮฉฮ” will remain invariant under the nonvanishing perturbation.  32 Remark 2.3 (Identification of ๐œ– โˆ— ). The Lyapunov analysis in the proof of Theorem 2.2 provides an estimate of ๐œ– โˆ— , however this estimate is usually quite conservative. In practice, ๐œ– is chosen through empirical tuning on the system on which it will be implemented. 2.3.4 Stability Under Output Feedback The system under output feedback is a singularly perturbed system which can be split into two time-scales. The multi-rotor dynamics and control reside in the slow time-scale while the observer and actuator dynamics reside in the fast time-scale. We now establish the stability of the overall output feedback system. Theorem 2.3 (Stability Under Output Feedback). For the output feedback system defined by (2.5), (2.7), (2.11), (2.22), (2.25), (2.26), and (2.27), the following statements hold i. given any compact subset ฮฉ ๐ด โŠ‚ ฮฉ๐‘ž ร— ฮฉฮ” , there exists a sufficiently small ๐œ– โˆ— such that for any ๐œ– โˆˆ (0, ๐œ– โˆ— ), ฮฉ ๐ด is a positively invariant set; ii. for ๐œ– โˆˆ (0, ๐œ– โˆ— ) the trajectories of the output feedback system exponentially converge to an ๐‘‚ (๐œ– ๐œ…) neighborhood of the origin with ฮฉ ๐ด as a subset of its region of attraction. Proof. The existence of sufficiently small ๐œ– โˆ— such that ฮฉ ๐ด is invariant can be established analogously to [40, Theorem 14.6]. The entire output feedback closed-loop system can now be written in singularly perturbed form ๐’’ยค = ๐ด๐‘ ๐’’ + ฮ”(๐œผ), (2.42a) ๐œ– ๐œผยค ๐‘– = ๐น๐‘– ๐œผ๐‘– + ๐ต๐‘–1 ฮ” ๐‘“ยฏ๐‘– + ๐บยฏ ๐‘– (๐‘1 ) ๐Ž   หœ ๐‘  + ๐œ– ๐ต๐‘–2 ๐œ‘๐‘– (๐‘ก, ๐’’, ๐’™ ๐‘ ), (2.42b) ยคฬƒ ๐‘  = โˆ’2๐Ž ๐œ๐‘š ๐Ž หœ ๐‘  + 2๐‘Šdes ๐Ž, หœ (2.42c) ๐œ๐‘š ๐Žยคฬƒ = โˆ’๐Ž,หœ (2.42d) where ๏ฃฎ ๏ฃน ๏ฃฏ ๐ด ๐œŒ 06 ๏ฃบ ๐ด๐‘ = ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃบ. ๏ฃฏ0 ๐ด ๏ฃบ ๏ฃฏ 6 ๐œ‰๏ฃบ ๏ฃฐ ๏ฃป 33 The term ฮ”(๐œผ) is due to estimation errors and is ๐‘‚ (๐œ– ๐œ…) and can be defined by ๏ฃฎ ๏ฃน ๏ฃฏ ๏ฃฏ 03ร—1 ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐›พ1 ๐œ– 2 ๐œผ1 + ๐›พ2 ๐œ–๐œผ1 + ๐œผ1 โˆ’ ๐œผ3 ๏ฃบ 1 2 3 3 ฮ”(๐œผ) = ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃบ. ๏ฃฏ ๏ฃฏ 03ร—1 ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ 2 2 ๏ฃฏ ๐›ฝ1 ๐œ– ๐œผ1 + ๐›ฝ2 ๐œ–๐œผ22 + ๐œผ23 + ๐œ– ๐ฟ ๐œ‚ ๐œผ22 ๏ฃบ ๏ฃบ ๏ฃฐ ๏ฃป First, we ignore the last term, ๐œ– ๐ต๐‘– ๐œ‘๐‘– (๐‘ก, ๐’’, ๐’™ ๐‘ ), in the ๐œผ dynamics. In this case, the closed-loop system has a two-time-scale structure because ๐œ– and ๐œ๐‘š are small. Since the effect of ฮ”(๐œผ) in (2.42a) vanishes as ๐œ– is pushed to zero, the boundary layer system can be taken as (2.42b)โ€“(2.42d) and the slow dynamics can be taken as (2.42a). From Theorem 2.2, the origin of the boundary layer system is an exponentially stable equilibrium point as ๐œ– โ†’ 0, and from Theorem 2.1, the origin of the slow system is an exponentially stable equilibrium point. With the inclusion of ๐œ– ๐ต๐‘– ๐œ‘๐‘– (๐‘ก, ๐’’, ๐’™ ๐‘ ) in the ๐œผ dynamics, the overall system is an ๐‘‚ (๐œ– ๐‘˜) pertur- bation of an exponentially stable system. Therefore, similar to [40, Lemma 9.2], it can be shown that the entire closed-loop system with output feedback control (2.42) will converge to an ๐‘‚ (๐œ– ๐œ…) neighborhood of the origin for any trajectory starting in ฮฉ๐‘ž ร— ฮฉฮ” .  2.4 Numerical Simulation The proposed method is simulated with the reference system taken as a moving ground vehicle on which the multi-rotor will land. However, since the multi-rotor may initially be far from the ground vehicle, i.e., ๐’‘ 1 โˆ’ ๐’™ ๐‘1 may be large, we will bound the estimate of this error to prevent overly aggressive maneuvers by saturating ๐† ห† 1 as ห† 1๐‘  = ๐›ฟ ๐œŒ tanh( ๐† ๐† ห† 1 /๐›ฟ ๐‘ ), (2.43) where ๐›ฟ ๐œŒ โˆˆ R is chosen to determine the rate of convergence of the multi-rotor position, ๐’‘ 1 , and the ground vehicle position, ๐’™ ๐‘1 . The saturated estimate is then used in the output feedback control (2.26). The controller (2.26) using ๐† ห† 1๐‘  loses exponential stability outside a certain region around the 34 origin. This can be avoided by scheduling the proportional gain, ๐›พ1 , in (2.26). However, saturation is a more natural choice and our simulations suggested that it yields superior performance. Figure 2.2: The trajectory of the multi-rotor UAV (dashed) and the trajectory of the ground vehicle (solid). The red points are the initial conditions and the green point signifies the occurrence of the landing. The initial position of the multi-rotor is ๐’‘ 1 (0) = [โˆ’10, 1, โˆ’5] > and the initial position of the ground vehicle is ๐’™ ๐‘1 (0) = [2, 0, โˆ’0.5] > . The ground vehicle follows the trajectory ๐’™ ๐‘1 (๐‘ก) = [๐‘ก, 2 cos(๐‘ก), โˆ’0.5] > . While only having a position measurement of the ground vehicle, with added noise, the multi-rotor is able to track and land on the vehicle, as shown in Fig. 2.2. The multi-rotor is able to make this landing while canceling disturbances in both the rotational and translational subsystems, ๐ˆ ๐œ‰ = [sin(๐‘ก) cos(๐‘ก) sin(๐‘ก)] > and ๐ˆ ๐œŒ = [cos(๐‘ก) sin(๐‘ก) cos(๐‘ก)] > , respectively. Gaussian white noise is added to all measurement signals. 35 Figure 2.3: Experimental multi-rotor on ground vehicle landing platform. Figure 2.4: Experimental landing on moving ground vehicle. 2.5 Experimental Validation The proposed estimation and control method is implemented on an experimental platform to validate performance and show the practical application of this control methodology to landing a multi-rotor on a small moving ground vehicle. 36 0 -10 -20 8 10 12 14 16 18 0 -20 -40 8 10 12 14 16 18 40 20 0 8 10 12 14 16 18 Figure 2.5: Estimates of the total rotational disturbance affecting the hexrotor during an experimental flight. 2.5.1 Hardware The experimental multi-rotor platform is built on a 550mm hexrotor frame with 920kV motors and 10x4.5 carbon fiber rotors. Six 30A electronic speed controllers (ESCs) are used for motor control and the system is powered by a 5000mAh 4s LiPo battery. The model parameters for the experimental platform were found to be ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ 1 1 1 1 1 1 ๏ฃบ๏ฃบ ๏ฃฏ0.0228 0 0 ๏ฃบ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃฏ โˆ’ ๐‘Ÿ โˆ’๐‘Ÿ โˆ’ ๐‘Ÿ ๐‘Ÿ ๐‘Ÿ 2๐‘Ÿ ๏ฃบ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ 2 2 2 ๐ฝ = ๏ฃฏ๏ฃฏ 0 0.0241 0 ๏ฃบ๏ฃบ , ๐‘€=๏ฃฏ โˆš โˆš โˆš โˆš ๏ฃบ, ๏ฃฏ ๏ฃบ ๏ฃฏ๐‘Ÿ 3 0 โˆ’๐‘Ÿ 3 โˆ’๐‘Ÿ 3 ๐‘Ÿ 3๏ฃบ 0 2 ๏ฃบ ๏ฃฏ 2 2 2 ๏ฃฏ 0 0 0.0446๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฏ ๐‘ โˆ’๐‘ ๐‘ โˆ’๐‘ ๐‘ โˆ’๐‘ ๏ฃบ ๏ฃฐ ๏ฃป ๐‘š = 1.824๐‘˜๐‘”, ๐‘ = 0.1, ๐‘ = 1.8182๐‘’ โˆ’ 05, ๐‘Ÿ = 0.275๐‘š, ๐œ๐‘š = 0.059. The moment of inertia matrix, ๐ฝ, was measured using the bifilar pendulum approach [35]. The mapping matrix, ๐‘€, is derived from the geometry of the airframe, in this case a hexrotor with x 37 0.5 0 -0.5 -1 8 10 12 14 16 18 1 0 -1 8 10 12 14 16 18 2 1.5 1 8 10 12 14 16 18 Figure 2.6: Estimates of the total translational disturbance affecting the hexrotor during an experimental flight. geometry with rotors numbered clockwise starting from the front right. The aerodynamic drag of the rotors, ๐‘, and the constant mapping squared actuator speed to force, ๐‘, were obtained using a photo-tachometer to measure rotor angular rate and a load cell to measure the forces generated at a range of speeds. Similarly, the actuator time constant, ๐œ๐‘š , was measured by applying several step inputs of varying magnitude to the rotor, measuring the response with the photo-tachometer, and fitting a first-order system to the data, see Appendix A for details. The control method is implemented on a Pixhawk 4 Flight Management Unit (FMU) in discrete time at 100Hz using Mathworks Simulinkยฎ through the PX4 Autopilots Support from Embedded Coderยฎ package [34]. This enables the control method to be integrated with the PX4 firmware to run on the Pixhawk 4 hardware. As a result, we can access fused estimates of the vehicle orientation from the EKF running in the PX4 firmware. The position estimates of both the multi-rotor and ground vehicle are pulled from a Vicon server at 100Hz. The estimates are sent over a UDP connection to a Raspberry Pi Zero that is running onboard the multi-rotor. The Raspberry Pi Zero 38 Figure 2.7: Multiple experimental landing trajectories showing the multi-rotor trajectory (dashed) and the ground vehicle trajectory (solid). The red dots correspond to the initial conditions of the system when a landing was commanded. then relays the position information to the FMU over a serial connection, see Appendix A for details. The ground vehicle is a Quanser QBot2 with a landing platform attached as shown with the multi-rotor on the landing platform in Fig. 2.3. The ground vehicle is manually tele-operated using a joystick through Simulinkยฎ . This ensures that no prior information about the trajectory is known, as the trajectory is generated in real-time by the operator. 39 2.5.2 Experimental Procedure The hexrotor initially ascends to a fixed altitude and holds position until commanded to track and land on the ground vehicle. Once a landing command is sent, the hexrotor begins converging on the position of the ground vehicle while the ground vehicle is being manually tele-operated around the area until the hexrotor successfully lands. To ensure the large initial position error does not result in overly aggressive control action, the same bounding function (2.43) is used to bound the position error vector ๐† 1 . Furthermore, to ensure the multi-rotor approaches the ground vehicle from above, an offset is added to the ๐‘ง component of the reference system. Once the multi-rotor is within some pre-defined radius of the center of the ground vehicle, in this case 4๐‘๐‘š, the offset is removed so the hexrotor will commence landing on the ground vehicle. Multiple experimental test flights were conducted with different initial conditions for both the hexrotor and ground vehicle. Each test was also performed with different ground vehicle trajectories. These experiments show the ability of the algorithm to successfully land regardless of differences in initial conditions or different reference trajectories. The ground vehicle trajectories and the hexrotor trajectories are shown for four different exper- imental flights in Fig. 2.7. The estimates of the disturbances affecting the system in the rotational and translational dynamics for one such flight are shown in Fig. 2.5 and Fig. 2.6, respectively. Notice that the translational disturbance estimate, specifically ๐ˆห† ๐œŒ (3) in Fig. 2.6, contains a constant offset. This offset is a result of the charge state of the battery. As the battery voltage decreases, the thrust applied by the rotors for a given commanded speed decreases. Also, large rotational disturbances arise in Fig. 2.5, which can be caused by unmodeled aerodynamic effects, inaccuracies in the inertia matrix, or differences between speed controllers. We do not model these discrep- ancies, however, the observer is able to estimate and compensate for these uncertainties in the control to result in excellent tracking performance. A video of the experiments can be found at https://youtu.be/oWcl4ydNLDs For a highly detailed treatment of the experimental multi-rotor system, including parameter 40 identification, rotor dynamics and force characteristics, and software implementation of these control methods, see Appendix A. 2.6 Extended High-Gain Observers on ๐‘†๐‘‚ (3) We propose a further extension of this work to enable extended high-gain observers to be designed for a system evolving on ๐‘†๐‘‚ (3). This would allow the use of existing almost-globally stabilizing feedback linearizing control strategies [55] to be extended to incorporate model un- certainties and external disturbances, and thus, increase their robustness. Furthermore, this also enables output feedback by estimating unmeasured states. The ability to design an extended high- gain observer for an almost-globally stabilizing control strategy on ๐‘†๐‘‚ (3) would alleviate the singularities which are present in control strategies that utilize Euler angles, as was the case in the work presented earlier in this chapter. Additionally, this extension would enable aggressive aero- batic flight maneuvers that are not possible when utilizing Euler angles. For example, a multi-rotor with this proposed control strategy would be capable of inverting and using the thrust generated by the rotors to accelerate downward faster than the acceleration due to gravity alone. This section details the mathematical framework of this proposed extension. 2.6.1 Rotational Dynamics on ๐‘†๐‘‚ (3) We begin by writing the rotational dynamics, no longer in terms of Euler angles, but instead in terms of a rotation matrix. Let ๐‘…๐‘๐ผ โˆˆ ๐‘†๐‘‚ (3) be the rotation matrix from the body frame to the inertial frame, which we shall denote as ๐‘… from here on to simplify notation. We further define ฮฉ ๐ผ โˆˆ R3 as the angular velocity of the body in the inertial frame, and ๐ฝ ๐ผ โˆˆ R3ร—03 as the inertia matrix of the body in the inertial frame. The body-fixed inertia matrix, ๐ฝ๐‘ โˆˆ R3ร—03 , can be mapped to ๐ฝ ๐ผ through ๐ฝ ๐ผ = ๐‘…๐ฝ๐‘ ๐‘… > . Taking ๐‰ โˆˆ R3 as a vector of body-fixed torques, we can now write the rotational dynamics on ๐‘†๐‘‚ (3) as ๐‘…ยค = ๐‘†(ฮฉ ๐ผ )๐‘…, (2.44) ยค ๐ผ = ๐ฝ โˆ’1 (โˆ’ฮฉ ๐ผ ร— ๐ฝ ๐ผ ฮฉ ๐ผ + ๐‘…๐‰) + ๐ˆ ๐ผ , ฮฉ ๐ผ 41 where ๐‘†(ยท) is the skew symmetric mapping from R3 to ๐”ฐ๐”ฌ(3), and ๐ˆ ๐ผ โˆˆ R3 is a lumped rotational disturbance term affecting the system in the inertial frame. Let ๐’“ 1 โˆˆ R3 , ๐’“ 2 โˆˆ R3 , and ๐’“ 3 โˆˆ R3 be the columns of the rotation matrix, ๐‘…. Then, ๐‘…ยค = ๐‘†(ฮฉ ๐ผ )๐‘… can be written as ๐’“ยค ๐‘– = ฮฉ๐ผ ร— ๐’“ ๐‘– , (2.45) for ๐‘– โˆˆ {1, 2, 3}. Using Lagrangeโ€™s formula for vector triple product, it follows that ๐’“ ๐‘– ร— ๐’“ยค ๐‘– = ๐’“ ๐‘– ร— (ฮฉ ๐ผ ร— ๐’“ ๐‘– ), (2.46) = (๐’“ ๐‘– ยท ๐’“ ๐‘– )ฮฉ ๐ผ โˆ’ (๐’“ ๐‘– ยท ฮฉ ๐ผ ) ๐’“ ๐‘– , Since we can write ฮฉ ๐ผ = [(๐’“ 1 ยท ฮฉ ๐ผ ) ๐’“ 1 + (๐’“ 2 ยท ฮฉ ๐ผ ) ๐’“ 2 + (๐’“ 3 ยท ฮฉ ๐ผ ) ๐’“ 3 ], (2.47) it follows that ๐’“ 1 ร— ๐’“ยค 1 + ๐’“ 2 ร— ๐’“ยค 2 + ๐’“ 3 ร— ๐’“ยค 3 = 3ฮฉ ๐ผ โˆ’ [(๐’“ 1 ยท ฮฉ ๐ผ ) ๐’“ 1 + (๐’“ 2 ยท ฮฉ ๐ผ ) ๐’“ 2 + (๐’“ 3 ยท ฮฉ ๐ผ ) ๐’“ 3 ], (2.48) = 2ฮฉ ๐ผ . Thus, ๐’“ 1 ร— ๐’“ยค 1 + ๐’“ 2 ร— ๐’“ยค 2 + ๐’“ 3 ร— ๐’“ยค 3 ฮฉ๐ผ = . (2.49) 2 Letting ๐’“ ๐‘– = ๐’“ ๐‘–1 and ๐’“ยค ๐‘– = ๐’“ ๐‘–2 , we can now write the columns of the rotation matrix, ๐‘…, and its derivative, ๐‘…,ยค in the following prime canoncial form ๐’“ยค ๐‘–1 = ฮฉ๐ผ ร— ๐’“ ๐‘–1 , ยค ๐ผ ร— ๐’“ 1 + ฮฉ๐ผ ร— ๐’“ 2 , ๐’“ยค ๐‘–2 = ฮฉ (2.50) ๐‘– ๐‘– = ๐ฝ ๐ผโˆ’1 (โˆ’ฮฉ ๐ผ ร— ๐ฝ ๐ผ ฮฉ ๐ผ + ๐‘…๐‰) ร— ๐’“ ๐‘–1 + ฮฉ ๐ผ ร— ๐’“ ๐‘–2 + ๐ˆ ๐ผ ร— ๐’“ ๐‘–1 . Substituting for ฮฉ ๐ผ , and taking ๐‡ ๐‘– = ๐ˆ ๐ผ ร— ๐’“ ๐‘–1 as the total additive disturbance term yields the following dynamic system ๐’“ยค ๐‘–1 = ๐’“ ๐‘–2 , (2.51) ๐’“ยค ๐‘–2 = ๐‘“ (๐’“ ๐‘–1 , ๐’“ ๐‘–2 ) + ๐‘”(๐’“ ๐‘–1 )๐‰ + ๐‡ ๐‘– , 42 where " 3 # " 3 #! " 3 # 1 ร• ร• 1 ร• ๐‘“ (๐’“ ๐‘–1 , ๐’“ ๐‘–2 ) = ๐ฝ ๐ผโˆ’1 โˆ’ ๐’“ 1๐‘— ร— ๐’“ 2๐‘— ร— ๐ฝ ๐ผ ๐’“ 1๐‘— ร— ๐’“ 2๐‘— ร— ๐’“ ๐‘–1 + ๐’“ 1๐‘— ร— ๐’“ 2๐‘— ร— ๐’“ ๐‘–2 , 2 2 ๐‘—=1 ๐‘—=1 ๐‘—=1 (2.52) ๐‘”(๐’“ ๐‘–1 ) = โˆ’๐‘†(๐’“ ๐‘–1 )๐ฝ ๐ผโˆ’1 ๐‘…. 2.6.2 Rotational State Feedback Control on ๐‘†๐‘‚ (3) In [55], an almost globally stabilizing state feedback controller has been designed on ๐‘†๐‘‚ (3) to track an arbitrary smooth reference orientation defined by ๐‘…๐‘‘ (๐‘ก) โˆˆ ๐‘†๐‘‚ (3). In addition to ๐‘…๐‘‘ (๐‘ก), this controller requires the desired body-fixed angular velocity, ฮฉ๐‘๐‘‘ (๐‘ก) โˆˆ R3 , and desired body-fixed angular acceleration, ฮฉ ยค ๐‘‘ (๐‘ก) โˆˆ R3 . Recall that the body-fixed angular velocity is related to the ๐‘ inertial angular velocity by ฮฉ๐‘ = ๐‘… > ฮฉ ๐ผ . We further add the disturbance compensation term to the control law design in [55], with the appropriate mapping, to ensure disturbance rejection and robust performance. This leads to the following rotational control law ๐‰ = ๐ฝ๐‘ ๐‘… > (โˆ’๐‘˜ ๐‘… ๐’† ๐‘… โˆ’ ๐‘˜ ฮฉ ๐’† ฮฉ ) + ฮฉ๐‘ ร— ๐ฝ๐‘ ฮฉ๐‘ โˆ’ ๐ฝ๐‘ (ฮฉ๐‘ ร— ๐‘… > ๐‘…๐‘‘ ฮฉ๐‘๐‘‘ โˆ’ ๐‘… > ๐‘…๐‘‘ ฮฉ ยค ๐‘‘ ) โˆ’ ๐ฝ๐‘ ๐‘… > ๐ˆ ๐ผ , (2.53) ๐‘ where 1 ๐’† ๐‘… = ๐‘† โˆ’1 (๐‘…๐‘‘> ๐‘… โˆ’ ๐‘… > ๐‘…๐‘‘ ), and ๐’† ฮฉ = ฮฉ๐‘ โˆ’ ๐‘… > ๐‘…๐‘‘ ฮฉ๐‘๐‘‘ , (2.54) 2 are the attitude error and angular velocity error, respectively, and ๐‘˜ ๐‘… โˆˆ R0 and ๐‘˜ ฮฉ โˆˆ R0 are the control gains. 2.6.3 Extended High-Gain Observer Design To design an extended high-gain observer for a rotation matrix and its derivative, we interpret ๐‘… and ๐‘…ยค as variables in R18 instead of interpreting them as variables in ๐‘†๐‘‚ (3) and its tangent space. The observer provides an estimate of ๐‘… and ๐‘…ยค in R18 each time, which we project back to ๐‘†๐‘‚ (3) and its tangent space using the projections shown in Appendix D. 43 To design the extended high-gain observer, we begin by extending the state space to include the unknown dynamics of the disturbance ๐’“ยค ๐‘–1 = ๐’“ ๐‘–2 , ๐’“ยค ๐‘–2 = ๐‘“ (๐’“ ๐‘–1 , ๐’“ ๐‘–2 ) + ๐‘”(๐’“ ๐‘–1 )๐‰ + ๐‡ ๐‘– , (2.55) ๐‡ยค ๐‘– = ๐œ‘๐‘– (๐‘ก, ๐’“ ๐‘–1 , ๐’“ ๐‘–2 ), where ๐œ‘๐‘– (๐‘ก, ๐’“ ๐‘–1 , ๐’“ ๐‘–2 ) โˆˆ R3 is a vector of unknown functions describing the disturbance dynamics. In order to estimate the higher order terms of the ๐’“ ๐‘– dynamics, and the disturbance term, ๐‡ ๐‘– , we can design an extended high-gain observer using the extended dynamics (2.55) as follows ๐›ผ1 ๐’“ยคฬ‚ ๐‘–1 = ห†๐’“ ๐‘–2 + (๐’“ ๐‘–1 โˆ’ ห†๐’“ ๐‘–1 ), ๐œ– ๐›ผ2 ๐’“ยคฬ‚ ๐‘–2 = ๐‘“ (ห†๐’“ ๐‘–1 , ห†๐’“ ๐‘–2 ) + ๐‘”(ห†๐’“ ๐‘–1 )๐‰ + ๐‡ห† ๐‘– + 2 (๐’“ ๐‘–1 โˆ’ ห†๐’“ ๐‘–1 ), (2.56) ๐œ– ๐›ผ 3 ๐‡ยคฬ‚ ๐‘– = 3 (๐’“ ๐‘–1 โˆ’ ห†๐’“ ๐‘–1 ), ๐œ– where, as before, ๐›ผ๐‘– for ๐‘– โˆˆ {1, 2, 3} are the observer gains, chosen to ensure the polynomial ๐‘ 3 + ๐›ผ1 ๐‘ 2 + ๐›ผ2 ๐‘  + ๐›ผ3 , is Hurwitz, and ๐œ– โˆˆ R0 is a positive constant that is chosen small enough. Recall, the disturbance term being estimated is actually an estimate of ๐‡ ๐‘– = ๐ˆ ๐ผ ร— ๐’“ ๐‘–1 , however, we require an estimate of ๐ˆ ๐ผ for the control law. We can solve for ๐ˆ ห† ๐ผ as follows ๐’“ 1 ร— ๐‡ห† 1 + ๐’“ 2 ร— ๐‡ห† 2 + ๐’“ 3 ร— ๐‡ห† 3 ๐ˆห†๐ผ = . (2.57) 2 It is noteworthy that the mismatch between the space in which the system dynamics and observer dynamics evolve appears in the disturbance estimates in the extended high-gain observer. However, since within ๐‘‡ (๐œ–) time, where lim๐œ–โ†’0 ๐‘‡ (๐œ–) = 0, the observer will bring the error between the true states and the estimated states to ๐‘‚ (๐œ–). As a result, the error associated with projected ๐‘… and ๐‘…ยค will be ๐‘‚ (๐œ–) as well. This ensures that the disturbance due to the above mismatch is small in magnitude, as will be shown through simulation in Fig. 2.11. 44 Additionally, we need an estimate of ฮฉ๐‘ for use in the feedback control. This can be computed from the observer estimates as ห†๐’“ 11 ร— ห†๐’“ 21 + ห†๐’“ 12 ร— ห†๐’“ 22 + ห†๐’“ 13 ร— ห†๐’“ 23 ฮฉฬ‚ ๐ผ = , and ฮฉฬ‚๐‘ = ๐‘…ห†> ฮฉฬ‚ ๐ผ . (2.58) 2 Recall that the state feedback attitude control presented in Section 2.6.2 requires not only a desired orientation, ๐‘…๐‘‘ , but the desired body-fixed angular rates, ฮฉ๐‘๐‘‘ , and the desired body-fixed angular acceleration, ฮฉ ยค ๐‘‘ . As was shown in the case for the attitude control designed using Euler ๐‘ angles, the reference trajectory for the attitude control may be designed through virtual inputs in a translational controller. In this case, we do not have access to the higher-order terms of the new reference trajectory. To overcome this deficit, we propose the use of a second extended high-gain observer to estimate these higher-order reference trajectory terms. Letting ๐’“ ๐‘‘๐‘– = ๐’“ 1๐‘‘๐‘– , ๐’“ยค ๐‘‘๐‘– = ๐’“ 2๐‘‘๐‘– , and ๐’“ยฅ ๐‘‘๐‘– = ๐’“ 3๐‘‘๐‘– , the observer can be designed for the desired orientation, ๐‘…๐‘‘ , as follows ๐›ผ1 ๐’“ยคฬ‚ 1๐‘‘๐‘– = ห†๐’“ 2๐‘‘๐‘– + (๐’“ 1๐‘‘๐‘– โˆ’ ห†๐’“ 1๐‘‘๐‘– ), ๐œ– ๐›ผ2 ๐’“ยคฬ‚ 2๐‘‘๐‘– = ห†๐’“ 3๐‘‘๐‘– + 2 (๐’“ 1๐‘‘๐‘– โˆ’ ห†๐’“ 1๐‘‘๐‘– ), (2.59) ๐œ– ๐›ผ3 ๐’“ยคฬ‚ 3๐‘‘๐‘– = 3 (๐’“ 1๐‘‘๐‘– โˆ’ ห†๐’“ 1๐‘‘๐‘– ), ๐œ– for ๐‘– โˆˆ {1, 2, 3}. The estimated desired rotation matrix and the higher-order derivatives can now be written as ๐‘…ห†๐‘‘ = ห†๐’“ 1๐‘‘1 ห†๐’“ 1๐‘‘2 ห†๐’“ 1๐‘‘3 , ๐‘…ห†๐‘‘2 = ห†๐’“ 2๐‘‘1 ห†๐’“ 2๐‘‘2 ห†๐’“ 2๐‘‘3 , ๐‘…ห†๐‘‘3 = ห†๐’“ 3๐‘‘1 ห†๐’“ 3๐‘‘2 ห†๐’“ 3๐‘‘3 .       We can now map these reference estimates to desired body-fixed angular rate and angular acceler- ation by ยคฬ‚     โˆ’1 ห† 1 > ห†2 โˆ’1 ห†2 > ห†2 ห† 1 ห† 3 ฮฉฬ‚๐‘๐‘‘ =๐‘† ( ๐‘…๐‘‘ ) ๐‘…๐‘‘ , and ๐‘‘ ฮฉ๐‘ = ๐‘† ( ๐‘…๐‘‘ ) ๐‘…๐‘‘ + ( ๐‘…๐‘‘ )> ๐‘…๐‘‘ . (2.60) Putting everything together, we obtain the following output feedback rotational control law ๐‰ = ๐ฝ๐‘ ๐‘…ห†> (โˆ’๐‘˜ ๐‘… ห†๐’† ๐‘… โˆ’ ๐‘˜ ฮฉ ห†๐’† ฮฉ ) + ฮฉฬ‚๐‘ ร— ๐ฝ๐‘ ฮฉฬ‚๐‘ โˆ’ ๐ฝ๐‘ ( ฮฉฬ‚๐‘ ร— ๐‘…ห†> ๐‘…ห†๐‘‘ ฮฉฬ‚๐‘๐‘‘ โˆ’ ๐‘…ห†> ๐‘…ห†๐‘‘ ฮฉ ยคฬ‚ ๐‘‘ ) โˆ’ ๐ฝ ๐‘…ห†> ๐ˆ ห† ๐ผ, (2.61) ๐‘ ๐‘ where 1 ห†๐’† ๐‘… = ๐‘† โˆ’1 ( ๐‘…ห†๐‘‘> ๐‘…ห† โˆ’ ๐‘…ห†> ๐‘…ห†๐‘‘ ), and ห†๐’† ฮฉ = ฮฉฬ‚๐‘ โˆ’ ๐‘…ห†> ๐‘…ห†๐‘‘ ฮฉฬ‚๐‘๐‘‘ . (2.62) 2 45 2.6.4 Translational Output Feedback Control An almost-globally asymptotically stabilizing translational control that was originally developed in [55], will now be presented. We can utilize the same translational error dynamics and extended high-gain observer as presented in Section 2.2.3, with the exception of replacing ๐‘…3 (๐‘1 ) in (2.7) by the third column of the rotation matrix, ๐‘…๐’† ๐‘ง , from this section. Utilizing the state and disturbance estimates, ๐†ห† 1, ๐† ห† ๐œŒ , and the reference signal, ๐’‘ยฅ ๐‘Ÿ , we can immediately write the translational ห† 2, ๐ˆ output feedback controller. Similar to the rotational control, to facilitate robust performance, we augment the control law in [55] with the estimated translational disturbances and obtain the third column of ๐‘…๐‘‘ as โˆ— โˆ’๐›พ1 ๐† ห† 1 โˆ’ ๐›พ2 ๐† ห†2 โˆ’ ๐ˆ ห† ๐œŒ + ๐‘š ๐’‘ยฅ ๐‘Ÿ โˆ’ ๐‘š๐‘”๐’† ๐‘ง ๐’“ 1๐‘‘3 = โˆ’ . (2.63) || โˆ’ ๐›พ1 ๐†ห† 1 โˆ’ ๐›พ2 ๐† ห†2 โˆ’ ๐ˆ ห† ๐œŒ + ๐‘š ๐’‘ยฅ ๐‘Ÿ โˆ’ ๐‘š๐‘”๐’† ๐‘ง || We now choose a point on the multi-rotor, in this case we chose ๐’‘ = [1 0 0] > , and project that point to create the first column of ๐‘…๐‘‘ as โˆ— โˆ— โˆ— ๐’“ 1๐‘‘3 ร— (๐’“ 1๐‘‘3 ร— ๐’‘) ๐’“ 1๐‘‘1 =โˆ’ โˆ— โˆ— . (2.64) ||๐’“ 1๐‘‘3 ร— (๐’“ 1๐‘‘3 ร— ๐’‘)|| This choice of ๐‘ will ensure the multi-rotor heading angle is aligned with the inertial frame during flight. We can now compute the second column of ๐‘…๐‘‘ as โˆ— โˆ— ๐’“ 1๐‘‘3 ร— ๐’‘ ๐’“ ๐‘–๐‘‘2 = โˆ— . (2.65) ||๐’“ 1๐‘‘3 ร— ๐’‘|| Thus, we have a desired rotation matrix that ensures that the desired translation is achieved. 2.6.5 Numerical Simulation The method described in this section is simulated to show trajectory tracking in the presence of large disturbances. The system is simulated in discrete time at 100Hz using only position and orientation measurements. The multi-rotor is commanded to track a circular trajectory and maintain altitude where ๐’‘๐‘Ÿ (๐‘ก) = [sin(๐‘ก) 1 โˆ’ cos(๐‘ก) 0] > . The rotational disturbances, ๐ˆ ๐ผ (๐‘ก) = 5[sin(๐‘ก) cos(3๐‘ก) sin(๐‘ก)] > , are applied to the system and are estimated by the extended high-gain 46 1 0 -1 0 5 10 15 20 2 1 0 0 5 10 15 20 0.04 0.02 0 0 5 10 15 20 Figure 2.8: Trajectory tracking utilizing the almost-globally stabilizing control and extended high-gain observer presented in Section 2.6.3. observer. No translational disturbances were applied for this simulation. The system is simulated with the same parameters from the hexrotor used in the experiment and the following initial conditions are used ๐‘…(0) = ๐ผ3 , ฮฉ ๐ผ = [0 0 0] > , ๐’‘ 1 (0) = [0 0 0] > , ๐’‘ 2 (0) = [0 0 0] > . The tracking performance is shown in Fig. 2.8 and the disturbance estimates, ๐ˆ ห† ๐ผ , are shown in Fig. 2.9. To further showcase the ability of this method to perform in a flight scenario which would not be feasible utilizing the Euler angle based control approach due to singularities, the system is simulated from an initial condition where it is upside down. In this case the multi-rotor flips over and returns to a stable hover configuration. The desired trajectory is simply to hold the initial position, ๐’‘๐‘Ÿ (๐‘ก) = [0 0 0] > , in the presence of the disturbances ๐ˆ ๐ผ (๐‘ก) = [sin(๐‘ก) cos(3๐‘ก) sin(๐‘ก)] > . 47 5 0 -5 0 5 10 15 20 5 0 -5 0 5 10 15 20 5 0 -5 0 5 10 15 20 Figure 2.9: Disturbance estimates from the extended high-gain observer presented in Section 2.6.3. The same initial conditions are used with the exception of the initial orientation, which is [55] ๏ฃฎ ๏ฃน ๏ฃฏ1 0 0 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐‘…(0) = ๏ฃฏ0 โˆ’0.9995 โˆ’0.0314๏ฃบ๏ฃบ . ๏ฃฏ (2.66) ๏ฃฏ ๏ฃบ ๏ฃฏ0 0.0314 โˆ’0.9995๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป The multi-rotor successfully flips over and returns to the desired position, as shown in Fig. 2.10 with the disturbance estimates shown in Fig. 2.11. Furthermore, to show the rotational tracking performance we can calculate the rotational tracking error 1  ฮจ(๐‘…, ๐‘…๐‘‘ ) = tr ๐ผ3 โˆ’ ๐‘… > ๐‘…๐‘‘ ,  (2.67) 2 where tr[ยท] is the trace of the argument. The tracking error function starts off large and quickly converges to show the convergence to the desired orientation, despite the large initial orientation error, as shown in Fig. 2.12. 48 0 -0.5 -1 0 2 4 6 8 10 1 0.5 0 0 2 4 6 8 10 2 1 0 0 2 4 6 8 10 Figure 2.10: Trajectory tracking utilizing the almost-globally stabilizing control and extended high-gain observer presented in Section 2.6.3. The multi-rotor starts upside down and flips to return to hover at the desired position. 2.7 Conclusions In this chapter, we studied a real-time trajectory estimation and tracking problem for a multi- rotor in the presence of modeling error and external disturbances. The unknown trajectory is generated from a dynamical system with unknown or partially known dynamics. We designed and rigorously analyzed an extended high-gain observer based output feedback controller to guarantee stable operation of the overall system. This included the modeling of actuator dynamics and their integration in the observer design, as well as estimating feed-forward terms for trajectory tracking and multi-rotor states for use in the output feedback control design. Furthermore, we introduced a novel method for designing an extended high-gain observer for a system whose dynamics evolve on ๐‘†๐‘‚ (3). The proposed method involves interpreting the dynamics on ๐‘†๐‘‚ (3) in Euclidean space to write the dynamics in an prime canonical form and design an extended high-gain observer to estimate unmeasured states, disturbances, and the higher-order terms of the rotational reference 49 1 0 -1 0 2 4 6 8 10 2 1 0 -1 0 2 4 6 8 10 1 0 -1 0 2 4 6 8 10 Figure 2.11: Disturbance estimates from the extended high-gain observer presented in Section 2.6.3, where system-observer space mismatch errors enter the disturbance estimate. trajectory. The capability of the control and estimation strategy is illustrated using the example of landing a multi-rotor on a moving ground vehicle. The multi-rotor landing is shown in simulation with noise and disturbances added, as well as implemented experimentally on a hexrotor platform. Multiple initial conditions and unknown trajectories were tested experimentally and shown to result in successful landings. 50 2 1.8 1.6 1.4 1.2 1 0.8 0.6 0.4 0.2 0 0 2 4 6 8 10 Figure 2.12: Rotational tracking error. 51 CHAPTER 3 IN-FLIGHT ACTUATOR FAILURE RECOVERY In this chapter, we extend the extended high-gain observer based estimation and control strategy presented in Chapter 2 to enable detection and recovery from a complete actuator failure for a hexrotor UAV during flight. The hexrotor may experience external disturbances and modeling error, which are accounted for in the control design and distinguished from an actuator failure. A failure of any one actuator occurs during flight and must be identified quickly and accurately. This is achieved through the use of a multiple-model, multiple extended high-gain observer based output feedback control strategy. The family of extended high-gain observers are responsible for estimating states, disturbances, and are used to select the appropriate model based on the system dynamics after a failure has occurred. The proposed method is theoretically analyzed and validated through simulations and experiments. This chapter draws from [11], which was originally published in IEEE Robotics and Automation Letters ยฉ2021 IEEE. Reprinted, with permission, from Connor J. Boss, Vaibhav Srivastava, In-flight actuator failure recovery of a hexrotor via multiple models and extended high-gain observers, June 2021. The remainder of the chapter is organized as follows. The system dynamics are introduced in Section 3.1, and the control law is designed in Section 3.2. The failure recovery strategy is presented in Section 3.3, with stability analysis in Section 3.4. Simulation and experimental results are presented in Section 3.5 and Section 3.6, respectively, with conclusions in Section 3.7. 3.1 System Dynamics A hexrotor UAV is an underactuated mechanical system. To overcome the underactuation, the dynamics are split into two subsystems: the rotational dynamics and the translational dynamics. 52 3.1.1 Rotational Dynamics Let ๐‘1 = [๐œ™ ๐œƒ ๐œ“] > โˆˆ (โˆ’ ๐œ‹2 , ๐œ‹2 ) 2 ร— (โˆ’๐œ‹, ๐œ‹] be the Euler angles describing the hexrotor orientation in the inertial frame, and let ๐‘2 = [ ๐œ™ยค ๐œƒยค ๐œ“] ยค > โˆˆ R3 be the associated angular rates. Let ๐‘๐‘Ÿ = [๐œ™๐‘Ÿ ๐œƒ ๐‘Ÿ ๐œ“๐‘Ÿ ] > โˆˆ (โˆ’ ๐œ‹2 , ๐œ‹2 ) 2 ร— (โˆ’๐œ‹, ๐œ‹] and ๐‘ยค ๐‘Ÿ = [ ๐œ™ยค๐‘Ÿ ๐œ—ยค๐‘Ÿ ๐œ“ยค ๐‘Ÿ ] > โˆˆ R3 be the rotational reference signals. Define the rotational tracking error, ๐ƒ, by ๐ƒ 1 = ๐‘ 1 โˆ’ ๐‘๐‘Ÿ , ๐ƒ 2 = ๐ƒยค1 = ๐‘2 โˆ’ ๐‘ยค ๐‘Ÿ , ๐ƒ = [๐ƒ > > > 1 ๐ƒ2 ] . Defining the inertia matrix, ๐ฝ โˆˆ R3ร—03 , where R3ร—03 denotes a 3 ร— 3 positive definite matrix, a matrix ฮจ โˆˆ R3ร—3 which transforms body-fixed angular velocity to Euler angular rates [12], and its associated derivative, ฮจ ยค โˆˆ R3ร—3 , the rotational tracking error dynamics are ๐ƒยค 1 = ๐ƒ 2 , (3.1) ๐ƒยค 2 = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) + ๐บ (๐‘1 )๐‰ + ๐‡ ๐œ‰ , where ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ ) = ฮจฮจยค โˆ’1 (๐ƒ 2 + ๐‘ยคฬ„ ๐‘Ÿ ) โˆ’ ฮจ๐ฝ โˆ’1 (ฮจโˆ’1 (๐ƒ 2 + ๐‘ยคฬ„ ๐‘Ÿ ) ร— ๐ฝฮจโˆ’1 (๐ƒ 2 + ๐‘ยคฬ„ ๐‘Ÿ )), ๐บ (๐‘1 ) = ฮจ๐ฝ โˆ’1 , ๐‘ยคฬ„ ๐‘Ÿ is some approximation of ๐‘ยค ๐‘Ÿ , ๐‰ โˆˆ R3 is a vector of body-fixed torques, ๐‡ ๐œ‰ = ๐ˆ ๐œ‰ โˆ’ ๐‘ยฅ ๐‘Ÿ + [ ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยค ๐‘Ÿ ) โˆ’ ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ )] โˆˆ R3 is an added term to represent the lumped rotational disturbance which satisfies Assumption 3.1 (stated below), and ๐ˆ ๐œ‰ โˆˆ R3 is the nominal rotational disturbance term [12] in the original rotational dynamics with a generic control input. Definition 3.1 (Prime Canonical Form). A control system in the โ€œprime canonical formโ€ [66], for state ๐’™ โˆˆ R๐‘› , control input ๐‘ข โˆˆ R, and disturbance ๐œŽ โˆˆ R, has the following representation ๐’™ยค = ๐ดprm ๐’™ + ๐ตprm ๐‘“prm (๐‘ก, ๐’™, ๐‘ข), ๐‘ฆ = ๐ถprm ๐’™, (3.2) where ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ0๐‘›โˆ’1ร—1 ๐ผ๐‘›โˆ’1 ๏ฃบ ๏ฃฏ0๐‘›โˆ’1ร—1 ๏ฃบ =๏ฃฏ ๏ฃบ , ๐ตprm = ๏ฃฏ ๏ฃบ , ๐‘“prm : Rโ‰ฅ0 ร— R๐‘› ร— R โ†’ R, ๐ถprm = [1 01ร—๐‘›โˆ’1 ], ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐ดprm ๏ฃฏ 0 01ร—๐‘›โˆ’1 ๏ฃบ ๏ฃฏ 1 ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป 53 0 ๐‘ร—๐‘ž is a matrix of zeros with dimension ๐‘ ร— ๐‘ž, ๐ผ ๐‘ is the identity matrix of dimension ๐‘, and ๐‘ฆ โˆˆ R is the measurement. Assumption 3.1 (Disturbance Properties). The dynamics of the various subsystems in this chapter take the prime canonical form perturbed by a disturbance term. We assume that the disturbance enters the RHS of (3.2) as ๐ต ๐‘‘ ๐œŽ where ๐ต ๐‘‘ = ๐ตprm , ๐œŽ is continuously differentiable, and its partial derivatives with respect to states are bounded on compact sets of those states for all ๐‘ก โ‰ฅ 0. 3.1.2 Translational Dynamics Let ๐’‘ 1 = [๐‘ฅ ๐‘ฆ ๐‘ง] > โˆˆ R3 and ๐’‘ 2 = [ ๐‘ฅยค ๐‘ฆยค ๐‘งยค] > โˆˆ R3 be the position and velocity of the hexrotor center of mass. Let ๐’‘๐‘Ÿ = [๐‘ฅ๐‘Ÿ ๐‘ฆ๐‘Ÿ ๐‘ง๐‘Ÿ ] > โˆˆ R3 and ๐’‘ยค ๐‘Ÿ = [๐‘ฅยค๐‘Ÿ ๐‘ฆยค ๐‘Ÿ ๐‘งยค๐‘Ÿ ] > โˆˆ R3 be the translational reference signals. Define the translational tracking error, ๐†, by ๐†1 = ๐’‘ 1 โˆ’ ๐’‘๐‘Ÿ , ๐† 2 = ๐†ยค1 = ๐’‘ 2 โˆ’ ๐’‘ยค ๐‘Ÿ , ๐† = [๐† > > > 1 ๐†2 ] . Taking the third column of the rotation matrix describing the hexrotor orientation in the inertial frame as ๐‘…3 (๐‘1 ) โˆˆ R3 , as in [12], ๐‘” as the gravitational constant, ๐‘ข ๐‘“ โˆˆ R as the total thrust input, ๐‘š โˆˆ R0 as the mass, and defining ๐’† ๐‘ง = [0 0 1] ๐‘‡ , the translational tracking error dynamics are ๐†ยค 1 = ๐† 2 , ๐‘ข๐‘“ (3.3) ๐†ยค 2 = โˆ’ ๐‘…3 (๐‘1 ) + ๐‘”๐’† ๐‘ง + ๐ˆ ๐œŒ โˆ’ ๐’‘ยฅ ๐‘Ÿ , ๐‘š where ๐ˆ ๐œŒ โˆˆ R3 is an added term to represent the lumped translational disturbance which also satisfies Assumption 3.1. 3.1.3 Failure Modes and Mapping Actuator Speeds to Inputs We will now consider how the system inputs in the form of body-torques, ๐‰, and thrust force, ๐‘ข ๐‘“ , are applied by the actuators, and how this changes during a failure. Remark 3.1 (Bidirectional Rotor Rotation). Bidirectional rotors are a requirement for a model switching failure recovery based on the controllability of the system, see Appendix E. 54 Since we require bidirectional rotor rotation, and the rotors are designed for efficient operation in only one direction, we define a pair of thrust coefficients, ๐‘ + โˆˆ R0 for normal operation and ๐‘ โˆ’ โˆˆ R0 for reverse operation. These coefficients relate rotor speed, ๐œ” โˆˆ R, to force, ๐‘“ยฏ โˆˆ R, as ๏ฃฑ ๏ฃฒ ๐‘ + ๐œ”2๐‘— , ๏ฃด for ๐œ” ๐‘— โ‰ฅ 0, ๏ฃด ๏ฃด ๏ฃด ๐‘“ยฏ๐‘— = for ๐‘— โˆˆ {1, . . . , 6}. (3.4) ๏ฃด โˆ’๐‘ โˆ’ ๐œ”2๐‘— , ๏ฃด ๏ฃด ๏ฃด for ๐œ” ๐‘— < 0, ๏ฃณ Let ๐‘– โˆˆ {0, . . . , 6} denote failure modes such that ๐‘– = 0 corresponds to no failure and ๐‘– โ‰  0 corresponds to the failure of the ๐‘–-th rotor. Let F (๐‘–) โˆˆ R6ร—6 be the failure matrix associated with failure mode ๐‘–, defined by F (0) = ๐ผ6 and ๏ฃฎ ๏ฃน ๏ฃฏ๐‘‘1 ๏ฃบ ๏ฃฑ ๏ฃด ๏ฃฒ 0, for ๐‘— = ๐‘–, ๏ฃฏ ๏ฃบ ๏ฃด ๏ฃด ๏ฃด F (๐‘–) ๏ฃฏ . ๏ฃบ = ๏ฃฏ๏ฃฏ .. ๏ฃบ, ๏ฃบ with ๐‘‘ ๐‘— = (3.5) ๏ฃด ๏ฃด 1, otherwise, ๏ฃฏ ๏ฃบ ๏ฃด ๏ฃด ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‘‘6 ๏ฃบ ๏ฃณ ๏ฃฐ ๏ฃป for ๐‘— โˆˆ {1, . . . , 6}. Let ๐‘€ โˆˆ R4ร—6 be the mapping between actuator forces and system inputs and be defined by ๏ฃฎ ๏ฃน ๏ฃฏ 1 1 1 1 1 1 ๏ฃบ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏโˆ’๐‘Ÿ โˆ’๐‘Ÿ โˆ’ 2๐‘Ÿ ๐‘Ÿ ๐‘Ÿ ๐‘Ÿ ๏ฃบ ๏ฃฏ 2 2 2 ๏ฃบ ๐‘€=๏ฃฏ โˆš โˆš โˆš โˆš ๏ฃบ, (3.6) ๏ฃฏ๐‘Ÿ 3 ๐‘Ÿ 3 ๐‘Ÿ 3 0 โˆ’ 2 โˆ’ 2 0 2 ๏ฃบ ๐‘Ÿ 3๏ฃบ ๏ฃฏ 2 ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‘ โˆ’๐‘ ๐‘ โˆ’๐‘ ๐‘ โˆ’๐‘ ๏ฃบ ๏ฃฐ ๏ฃป where ๐‘Ÿ โˆˆ R0 is the distance from the hexrotor center of mass to the center of an actuator, and ๐‘ โˆˆ R0 is the aerodynamic drag coefficient of a rotor. Let ๐‘–๐‘กโˆ— be the true failure mode at time ๐‘ก, ๐‘–๐‘ก be the failure mode that is selected at time ๐‘ก, and ๐‘ก ๐‘“ be the time of failure. The system inputs for the nominal model and all failure models are mapped to a vector of squared rotor speeds, ๐Ž ๐‘  = [๐œ”21 , . . . , ๐œ”26 ] > โˆˆ R6โ‰ฅ0 , through ๏ฃฎ ๏ฃน " # ๏ฃฏ๐‘ข ๐‘“ ๏ฃบ ๐œˆ1 ๐‘ ๐œˆ1 ๏ฃฏ ๏ฃบ = ๐‘€BF (๐‘–๐‘ก ) ๐Ž ๐‘  , B= .. (3.7) ๏ฃฏ ๏ฃบ . , ๏ฃฏ๐‰๏ฃบ ๐œˆ6 ๐‘ ๐œˆ6 ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป where ๐œˆ ๐‘— โˆˆ {โˆ’, +} is the sign of ๐œ” ๐‘— . 55 Assumption 3.2 (Single Failure Occurrence). We assume the configuration ๐‘–๐‘กโˆ— = ๐‘–๐‘ก = 0 for ๐‘ก < ๐‘ก ๐‘“ . At the time of failure ๐‘–๐‘กโˆ—๐‘“ = 0 โ†’ ๐‘–๐‘กโˆ— โˆˆ {1, . . . , 6} for ๐‘ก > ๐‘ก ๐‘“ . Since our platform is a hexrotor, we focus on a single actuator failure to ensure the system retains full controllability, see Appendix E. Failure of more than one actuator, in specific cases, can result in a system that retains controllability. However, in these configurations, only two of the actuators would be responsible for generating the total lifting thrust, with the others providing small correctional forces and torques. Consequently, due to limited actuator power, the hexrotor would not be able to maintain altitude. 3.2 Output Feedback Control Design In this section, an output feedback estimation and control strategy is designed as in [12]. We utilize the same control and observer design, while extending our previous work to incorporate a family of EHGOs to estimate not only modeling error and external disturbances, but errors due to the failure of any one actuator, as well as enabling the detection of a failure through the use of the observer estimates. As such, each observer will correspond to a possible plant configuration, i.e., a nominal model and six failure models. 3.2.1 Extended High-Gain Observer Design A family of multi-input multi-output EHGOs is designed to estimate higher-order states of the error dynamic systems (3.1) and (3.3), and uncertainties arising from modeling error, external disturbances, and actuator failure [42]. It is shown in [9, 12] that it is necessary to include actuator dynamics in the multi-rotor model for EHGO design. For a desired rotor speed, ๐œ”des , the actuators can be modeled as a first-order system with time constant, ๐œ๐‘š โˆˆ R0 , given by ๐œ๐‘š ๐œ”ยค ๐‘— = (๐œ”des ๐‘— โˆ’ ๐œ” ๐‘— ), for ๐‘— โˆˆ {1, . . . , 6}. The actuator dynamics must be included in the EHGO because in practice ๐œ๐‘š and ๐œ– are of similar magnitude, so both reside in the same time-scale. These dynamics then form the input to the EHGO as in [12]. The rotational and translational tracking error dynamics are combined and the state-space is extended to estimate disturbance vectors for both subsystems. For the purposes of writing the 56 observer under the ๐‘–-th failure mode, we write the extended system dynamics in the following form ๐†ยค 1 = ๐† 2 , ๐‘ข๐‘“ ๐†ยค 2 = โˆ’ ๐‘…3 (๐‘1 ) + ๐‘”๐’† ๐‘ง + ๐ˆ ๐œŒ โˆ’ ๐’‘ยฅ ๐‘Ÿ , ๐‘š ๐ˆยค ๐œŒ = ๐œ‘ ๐œŒ (๐‘ก, ๐†), (3.8) ๐ƒยค 1 = ๐ƒ 2 , ๐ƒยค 2 = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ(๐‘–๐‘ก ) ) + ๐บหœ ๐‘€BF (๐‘–) ๐Ž ๐‘ (๐‘–๐‘ก ) + ๐‡ยฏ ๐œ‰(๐‘–) , ๐‡ยคฬ„ ๐œ‰(๐‘–) = ๐œ‘๐œ‰(๐‘–) (๐‘ก, ๐‡ยฏ ๐œ‰(๐‘–) ), โˆ— where ๐‡ยฏ ๐œ‰(๐‘–) = ๐ˆ ๐‘š(๐‘–) + ๐‡ ๐œ‰ , and ๐ˆ ๐‘š(๐‘–) = ๐บหœ ๐‘€B (F (๐‘–๐‘ก ) โˆ’ F (๐‘–) )๐Ž ๐‘ (๐‘–๐‘ก ) is the error resulting from an incorrect โˆ— (๐‘– ) model, ๐‘–๐‘ก โ‰  ๐‘–๐‘กโˆ— . Finally, ๐บหœ = [03ร—1 ๐บ (๐‘1 )] and ๐ˆ ๐‘š๐‘ก = 0. Here, ๐œ‘ ๐œŒ (๐‘ก, ๐†) and ๐œ‘๐œ‰(๐‘–) (๐‘ก, ๐‡ยฏ ๐œ‰(๐‘–) ) are unknown functions describing the translational and rotational disturbance dynamics. Assumption 3.3 (Disturbance Dynamics). It is assumed that ๐œ‘ ๐œŒ (๐‘ก, ๐†) and ๐œ‘๐œ‰(๐‘–) (๐‘ก, ๐‡ยฏ ๐œ‰(๐‘–) ) are contin- uous and bounded on any compact set in the domain of ๐† and ๐‡ยฏ ๐œ‰(๐‘–) , respectively. The observer system is written with extended states and squared rotor speeds as the input, ๐Ž ๐‘ (๐‘–๐‘ก ) . Defining the following state vectors > > > > ๐Œ1 = [๐† > > > > 1 ๐† 2 ๐ˆ ๐œŒ ] , ๐Œ 2 = [๐ƒ 1 ๐ƒ 2 ๐‡ ยฏ ๐œ‰ ] , ๐Œ = [ ๐Œ> > > 1 ๐Œ2 ] , we can write the EHGOs in a compact form as h i ยคฬ‚ (๐‘–) = ๐ด ๐Œ ๐Œ ห† (๐‘–) + ๐ต ๐‘“ยฏ( ๐Œ ห† (๐‘–) , ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ(๐‘–๐‘ก ) ) + ๐บยฏ (๐‘–) (๐‘1 )๐Ž ๐‘ (๐‘–๐‘ก ) + ๐ป ๐Œห† ๐‘’(๐‘–) , (3.9) (๐‘–) ห† ๐‘’(๐‘–) ๐Œ = ๐ถ( ๐Œ โˆ’ ๐Œ ห† ), (๐‘–)> (๐‘–)> (๐‘–)> > h i where ๐Œห† (๐‘–) = ๐† ห† 1(๐‘–)> ๐†ห† 2(๐‘–)> ๐ˆ ห† (๐‘–)> ๐œŒ ห† ๐ƒ 1 ห† ๐ƒ 2 ห† ๐‡ ยฏ ๐œ‰ is the estimate of ๐Œ under the model with failure ๐‘–, and ๐ด = โŠ•2๐‘—=1 ๐ด ๐‘— , ๐ต = โŠ•2๐‘—=1 ๐ต ๐‘— , ๐ถ = โŠ•2๐‘—=1๐ถ ๐‘— , ๐ป = โŠ•2๐‘—=1 ๐ป ๐‘— , 57 ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ03 ๐ผ3 03 ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๐›ผ1 /๐œ– ๐ผ3 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐ด ๐‘— = ๏ฃฏ03 03 ๐ผ3 ๏ฃบ , ๐ต ๐‘— = ๏ฃฏ ๐ผ3 ๏ฃบ , ๐ป ๐‘— = ๏ฃฏ๐›ผ2 /๐œ– 2 ๐ผ3 ๏ฃบ๏ฃบ , ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ03 03 03 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ 3 ๏ฃฏ๐›ผ3 /๐œ– ๐ผ3 ๏ฃบ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป h i ๐ถ ๐‘— = ๐ผ3 03 03 , for ๐‘— โˆˆ {1, 2}, ๏ฃฎ ๏ฃน ๏ฃฎ โˆ’๐‘… (๐‘ ) ๏ฃน ๏ฃฏ ๐‘”๐’† ๐‘ง โˆ’ ๐’‘ยฅ ๏ฃบ ๏ฃฏ 3 1 03 ๏ฃบ ห† (๐‘–) , ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ(๐‘–๐‘ก ) ) = ๏ฃฏ ๐‘“ยฏ( ๐Œ ยฏ (๐‘–) (๐‘1 ) = ๏ฃฏ๏ฃฏ ๐‘š ๐‘Ÿ ๏ฃบ ๐‘€BF (๐‘–) , ๏ฃฏ ๏ฃบ ๏ฃบ , ๐บ ๏ฃฏ ๐‘“ ( ๐ƒห† (๐‘–) , ๐‘ , ๐‘ยคฬ„ (๐‘–๐‘ก ) ) ๏ฃบ ๏ฃบ ๏ฃฏ 3ร—1 ๐บ (๐‘1 ) ๏ฃบ ๏ฃฏ 0 ๏ฃบ ๏ฃฏ 1 ๐‘Ÿ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป where โŠ• denotes the matrix direct sum, ๐ป is designed by choosing ๐›ผ1 , ๐›ผ2 , ๐›ผ3 โˆˆ R0 such that the polynomial ๐‘ 3 + ๐›ผ1 ๐‘ 2 + ๐›ผ2 ๐‘  + ๐›ผ3 , (3.10) is Hurwitz [42] and ๐œ– โˆˆ R0 is a tuning parameter that must be chosen small enough. In practice, ๐œ– is tuned empirically to achieve a balance between convergence rate of the observer and noise amplification. All EHGO estimates must also be saturated outside a compact set of interest to avoid peaking, see Appendix C. 3.2.2 Output Feedback Control The output feedback controllers are written using the estimates from the corresponding EHGO, ห† (๐‘–) . The family of translational output feedback controllers, induced by rotational reference signals ๐Œ and total thrust, become โˆ’ ๐‘“ห†๐‘ฆ(๐‘–) ๐œ™ห†๐‘Ÿ(๐‘–) = tanโˆ’1 ยญยญ q ยฎ , ๐œ“ห†๐‘Ÿ(๐‘–) = 0, ยฉ ยช ยฎ ( ห†๐‘ฅ(๐‘–) ) 2 + ( ๐‘“ห†๐‘ง(๐‘–) โˆ’ ๐‘”) 2 ๐‘“ ยซ ! ยฌ ๐‘“ห†๐‘ฅ(๐‘–) ๐‘š( ๐‘“ห†๐‘ง(๐‘–) โˆ’ ๐‘”) ๐œƒห†๐‘Ÿ(๐‘–) = tan โˆ’1 (๐‘–) , ๐‘ขห† ๐‘“ ๐‘‘ = โˆ’ , ๐‘“ห†๐‘ง(๐‘–) โˆ’ ๐‘” cos ๐œ™ห†๐‘Ÿ(๐‘–) cos ๐œƒห†๐‘Ÿ(๐‘–) (๐‘–) where ห†๐’‡ ๐‘ก = [ ๐‘“ห†๐‘ฅ(๐‘–) ๐‘“ห†๐‘ฆ(๐‘–) ๐‘“ห†๐‘ง(๐‘–) ] > = โˆ’๐›พ1 ๐† ห† 1(๐‘–) โˆ’ ๐›พ2 ๐† ห† 2(๐‘–) โˆ’ ๐ˆ ห† (๐‘–) ๐œŒ + ๐’‘ ยฅ ๐‘Ÿ . Here, desired heading, ๐œ“ห†๐‘Ÿ(๐‘–) , is set to zero to simplify the control equations; see [9] for control equations with arbitrary ๐œ“ห†๐‘Ÿ(๐‘–) . The family of rotational output feedback controllers become h (๐‘–) (๐‘–) i ๐‰ห† ๐‘‘(๐‘–) = ๐บ โˆ’1 (๐‘1 ) ห†๐’‡ ๐‘Ÿ โˆ’ ๐‘“ ( ๐ƒห† , ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ(๐‘–๐‘ก ) ) , 58 (๐‘–) (๐‘–) (๐‘–) where ห†๐’‡ ๐‘Ÿ = โˆ’๐›ฝ1 ๐ƒห† 1 โˆ’ ๐›ฝ2 ๐ƒห† 2 โˆ’ ๐‡ห†ยฏ ๐œ‰(๐‘–) . Note that the rotational reference signal estimates ( ๐œ™ห†๐‘Ÿ(๐‘–๐‘ก ) , ๐œƒห†๐‘Ÿ(๐‘–๐‘ก ) , ๐œ“ห†๐‘Ÿ(๐‘–๐‘ก ) ) are used to estimate ๐‘ยคฬ„ ๐‘Ÿ(๐‘–๐‘ก ) in ๐‰ห† ๐‘‘(๐‘–) (see [12] for details). We then arrive at the family of commanded squared rotor speeds ๏ฃฎ (๐‘–) ๏ฃน ๏ฃฏ๐‘ขห† ๏ฃบ ๐Ž ๐‘ (๐‘–) = (๐‘€BF (๐‘–) ) โ€  ๐’– ห† (๐‘–) , ห† (๐‘–) ๐’– = ๏ฃฏ ๐‘“ ๐‘‘๏ฃบ , (3.11) ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‰ห† (๐‘–) ๏ฃบ ๏ฃฏ ๐‘‘ ๏ฃบ ๏ฃฐ ๏ฃป where (ยท) โ€  = (ยท) > ((ยท)(ยท) > ) โˆ’1 is the minimum energy pseudo-inverse of the argument. The rotational closed-loop system under input ๐Ž ๐‘ (๐‘–๐‘ก ) for any ๐‘–๐‘ก , regardless of ๐‘–๐‘กโˆ— , reduces to the following perturbed linear system, since the mismatch is captured by ๐‡ยฏ ๐œ‰(๐‘–) ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ 03 ๐ผ3 ๏ฃบ๏ฃบ ๏ฃฏ03 ๏ฃบ ๐ƒยค = ๐ด๐œ‰ ๐ƒ + ๐œ– ๐ต1 ๐œน (๐‘–) , ๐ด๐œ‰ = ๏ฃฏ ๐ต1 = ๏ฃฏ ๏ฃบ , (3.12) ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃบ, ๏ฃฏโˆ’๐›ฝ ๐ผ โˆ’๐›ฝ ๐ผ ๏ฃบ ๏ฃฏ๐ผ ๏ฃบ ๏ฃฏ 1 3 2 3๏ฃบ ๏ฃฏ 3๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป where (๐‘–) (๐‘–) ๐œ– ๐œน (๐‘–) = ๐‡ ๐œ‰ + ๐ˆ ๐‘š(๐‘–) โˆ’ ๐‡ห†ยฏ ๐œ‰(๐‘–) + ๐›ฝ1 (๐ƒ 1 โˆ’ ๐ƒห† 1 ) + ๐›ฝ2 (๐ƒ 2 โˆ’ ๐ƒห† 2 ) + ฮ” ๐‘“ (๐‘–) , (3.13) (๐‘–) ฮ” ๐‘“ (๐‘–) = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ(๐‘–๐‘ก ) ) โˆ’ ๐‘“ ( ๐ƒห† , ๐‘1 , ๐‘ยคฬ„ ๐‘Ÿ(๐‘–๐‘ก ) ). The ability to write the mismatched closed-loop system as (3.12) means that if ๐œ– is chosen small enough, the system under output feedback will recover performance of the desired linear system, even in the presence of an actuator failure without requiring a model switch. Remark 3.2 (Multiple Possible Recovery Strategies). For the small constants, ๐œ–1 , ๐œ–2 โˆˆ R0 , where ๐œ–1  ๐œ–2 , if we choose ๐œ– โˆˆ (0, ๐œ–1 ) a recovery can be achieved without requiring a model switch. If we choose ๐œ– โˆˆ (๐œ–1 , ๐œ–2 ), nominal disturbances can be compensated, however, the large disturbance, ๐ˆ ๐‘š(๐‘–) , can result in large estimation error. Due to practical constraints on ๐œ– when it comes to implementation, such as sample rate and noise, we must choose ๐œ– โˆˆ (๐œ– 1 , ๐œ–2 ). This motivates our use of multiple models and multiple observers for recovery (see Theorem 1 for details). Furthermore, we can arrive at approximate values of ๐œ–1 โ‰ˆ 0.002 and ๐œ–2 โ‰ˆ 0.06 through simulation. Remark 3.3 (Domain of Operation). We define the domain of operation as the region in which singularities are avoided in the feedback linearizing control design [12]. Since ๐ˆ ๐‘š(๐‘–) = 0 for ๐‘– = ๐‘–๐‘กโˆ— , 59 for any single actuator failure, with ๐œ– โˆˆ (0, ๐œ–2 ) and ๐‘–๐‘ก = ๐‘–๐‘กโˆ— , the closed-loop rotational subsystem becomes the linearized system (3.12) with a small perturbation ๐œ– ๐œน (๐‘–) . Therefore, the domain of operation is the same for all ๐‘–๐‘ก = ๐‘–๐‘กโˆ— . 3.3 Failure Recovery Strategy The most common external disturbances experienced by a multi-rotor during flight are aerody- namic (wind gusts, drag, etc.), and therefore primarily affect the translational dynamics. During an actuator failure, a large rotational torque is generated. This large torque appears as a high magnitude disturbance, ๐ˆ ๐‘š(๐‘–) , in the rotational dynamics, thus we monitor the rotational subsystem for actuator failure detection. For ๐œ– โˆˆ (๐œ–1 , ๐œ–2 ), when ๐‘–๐‘ก โ‰  ๐‘–๐‘กโˆ— immediately after failure, the perturbation, ๐œ– ๐œน (๐‘–๐‘ก ) , is no longer small, and may make (3.12) leave the domain of operation. This behavior can be identified by monitoring an estimate of the derivative of a Lyapunov function for the rotational subsystem, using the method presented in [23]. Therefore we can detect the failure, and then switch models to recover stability. Defining ๐‘ก ๐‘  as the time of control switching, we can define a maximum switching time, ๐‘ก ๐‘ max , such that ๐‘ก ๐‘  < ๐‘ก ๐‘ max ensures recovery from the failure before (3.12) leaves the domain of operation (see the proof of Theorem 1 for an estimate of ๐‘ก ๐‘ max ). 3.3.1 Estimating the Lyapunov Derivative Since the derivative of the Lyapunov function is not available, it will be estimated using estimates from the EHGOs, similar to [23]. Following Assumption 3.2, the system begins in the nominal operating regime, ๐‘–๐‘กโˆ— = 0, therefore only the nominal Lyapunov function derivative must be estimated ( 0)> ( 0) ๐‘‰ยคฬ‚๐œ‰ = ๐ƒห† ๐‘ƒ๐œ‰ ๐ƒยคฬ‚ (0) + ๐ƒยคฬ‚ (0)> ๐‘ƒ๐œ‰ ๐ƒห† , (3.14) where ๐‘ƒ๐œ‰ ๐ด๐œ‰ + ๐ด๐œ‰> ๐‘ƒ๐œ‰ = โˆ’๐ผ6 . We use this estimate to test the following inequality to detect an actuator failure ( 0) 2 ๐‘‰ยคฬ‚๐œ‰ โ‰ค ๐‘Ž 0 โˆ’ ๐ƒห† , (3.15) 60 where ๐‘Ž 0 โˆˆ R0 is a small constant introduced to overcome the ๐‘‚ (๐œ–) estimation errors and is tuned empirically through simulation and experiments. For example, choosing ๐‘Ž 0 too small would result in detecting false positives, and too large would increase time until failure detection, potentially past ๐‘ก ๐‘ max . Once (3.15) is violated, a new model must be selected. Since the hexrotor is a symmetric system, if any actuator fails the dynamic response is the same, simply in a different direction. As a result, only one threshold, ๐‘Ž 0 , need be computed which will successfully detect the occurrence of any one actuator failure. 3.3.2 Estimating Disturbance and Failures Simultaneously Since all disturbance estimates contain any discrepancies between the modeled response and the response of the physical system, the total rotational disturbance estimated by the ๐‘–-th observer, ๐‡ห†ยฏ ๐œ‰(๐‘–) , is an estimate of ๐ˆ ๐‘š(๐‘–) + ๐‡ ๐œ‰ . In order to select the appropriate model after a failure has been detected using (3.15), we utilize the magnitude of the disturbance estimates from each observer to appropriately select ๐‘–๐‘ก = ๐‘–๐‘กโˆ— as n o ๐‘–๐‘ก = arg min ๐‡ห†ยฏ ๐œ‰(๐‘–) . (3.16) ๐‘–โˆˆ{1,...,6} Following Assumption 3.2, (3.16) is a minimization across failure modes, excluding the nominal model. 3.4 Stability Analysis Following the stability arguments in [12] and the Theorems therein, the proposed output feed- back control design presented here meets the same stability guarantees. We can show that these stability guarantees are also met under actuator failure without switching models when ๐œ– โˆˆ (0, ๐œ–1 ), and also hold for ๐œ– โˆˆ (๐œ–1 , ๐œ–2 ) so long as ๐‘–๐‘ก = ๐‘–๐‘กโˆ— . We must now investigate the stability of the system during an actuator failure. Define the scaled observer error for the rotational system, ๐œผ (๐‘–) = [๐œผ1(๐‘–) ๐œผ2(๐‘–) ๐œผ3(๐‘–) ] > โˆˆ R9 , by (๐‘–) (๐‘–) ๐ƒ 1 โˆ’ ๐ƒห† 1 ๐ƒ 2 โˆ’ ๐ƒห† 2 ๐œผ1(๐‘–) = , ๐œผ2(๐‘–) = , ๐œผ3(๐‘–) = ๐‡ ๐œ‰ + ๐ˆ ๐‘š(๐‘–) โˆ’ ๐‡ห†ยฏ ๐œ‰(๐‘–) . ๐œ–2 ๐œ– 61 The entire rotational output feedback closed-loop system can now be written as the singularly perturbed system ๐ƒยค = ๐ด๐œ‰ ๐ƒ + ๐œ– ๐ต1 ๐œน (๐‘–) , (3.17a)  (๐‘–)  ๐œ– ๐œผยค (๐‘–) = ฮ›๐œผ (๐‘–) + ๐œ– ๐ต2 ฮ” ๐‘“๐œ– + ๐ต3 (๐œ‘๐œ‰(๐‘–) (๐‘ก, ๐‡ยฏ ๐œ‰(๐‘–) ) + ๐ˆ ยค ๐‘š(๐‘–) ) , (3.17b) where the system dynamics (3.17a) are the slow variables, the observer error (3.17b) are the fast variables, and ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏโˆ’๐›ผ1 ๐ผ3 ๐ผ3 03 ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ฮ› = ๏ฃฏ๏ฃฏโˆ’๐›ผ2 ๐ผ3 03 ๐ผ3 ๏ฃบ๏ฃบ , ๐ต2 = ๏ฃฏ๏ฃฏ ๐ผ3 ๏ฃบ๏ฃบ , ๐ต3 = ๏ฃฏ๏ฃฏ03 ๏ฃบ๏ฃบ . ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏโˆ’๐›ผ3 ๐ผ3 03 03 ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐ผ3 ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป By Assumption 3.3, ๐œ‘๐œ‰(๐‘–) (๐‘ก, ๐‡ยฏ ๐œ‰(๐‘–) ) is continuous and bounded, and it can be shown that ๐ˆ ยค ๐‘š(๐‘–) is also continuous and bounded, so we can bound the sum as, ๐œ‘๐œ‰(๐‘–) (๐‘ก, ๐‡ยฏ ๐œ‰(๐‘–) ) + ๐ˆ ยค ๐‘š(๐‘–) โ‰ค ฮ”max (๐‘–) . From [12], ฮ” ๐‘“ (๐‘–) is Lipschitz in ๐ƒ over the domain of operation and ฮ” ๐‘“ (๐‘–) can be bounded by ฮ” ๐‘“ (๐‘–) โ‰ค ๐œ– ๐ฟ ๐œ‚ ๐œผ (๐‘–) , for the Lipschitz constant, ๐ฟ ๐œ‚ . We can write a Lyapunov function for the scaled observer error system (3.17b) as ๐‘‰๐œ‚(๐‘–) = (๐œผ (๐‘–) ) > ๐‘ƒ๐œ‚ ๐œผ (๐‘–) , where ๐‘ƒ๐œ‚ ฮ› + ฮ›> ๐‘ƒ๐œ‚ = โˆ’๐ผ9 . (3.18) Lemma 3.1 (Bounds on Observer Error). Let the observer error at the time of failure, ๐‘ก ๐‘“ , be ๐œผ (๐‘–) (๐‘ก ๐‘“ ). Then, the observer error for ๐‘ก > ๐‘ก ๐‘“ can be bounded by q   (๐‘–) (๐‘–) ๐œ… โˆ’ ๐‘ (๐‘กโˆ’๐‘ก ๐‘“ ) ๐œ… q ๐œผ (๐‘ก) โ‰ค ๐‘‰๐œ‚ (๐‘ก ๐‘“ ) โˆ’ ๐œ– ๐‘’ ๐œ– + ๐œ– / ๐œ† min (๐‘ƒ๐œ‚ ), ๐‘ ๐‘   (๐‘–) ๐œ† (๐‘ƒ ๐œ‚ )๐œ– ๐ฟ ๐œ‚ ๐œ† max (๐‘ƒ ๐œ‚ )ฮ”max ๐‘ = ๐œ†max1(๐‘ƒ ๐œ‚ ) โˆ’ max ๐œ† min (๐‘ƒ ๐œ‚ ) , ๐œ… = โˆš , ๐œ† min (๐‘ƒ ๐œ‚ ) where ๐œ† min (ยท) and ๐œ† max (ยท) are the minimum and maximum eigenvalues of the argument, respectively. Proof. Taking the Lyapunov function (3.18) and computing the derivative with the scaled observer error system (3.17b) yields  (๐‘–)  ๐œ– ๐‘‰ยค๐œ‚(๐‘–) = โˆ’(๐œผ (๐‘–) ) > ๐œผ (๐‘–) + 2๐œ– (๐œผ (๐‘–) ) > ๐‘ƒ๐œ‚ ๐ต2 ฮ” ๐‘“๐œ– + ๐ต3 (๐œ‘๐œ‰(๐‘–) (๐‘ก, ๐‡ยฏ ๐œ‰(๐‘–) ) + ๐ˆ ยค ๐‘š(๐‘–) ) , 62 which can be bounded by q ๐œ– ๐‘‰ยค๐œ‚(๐‘–) โ‰ค โˆ’ 2๐‘ ๐‘‰๐œ‚(๐‘–) + 2๐œ– ๐œ… ๐‘‰๐œ‚(๐‘–) ,   (๐‘–) 1 ๐œ† (๐‘ƒ ๐œ‚ )๐œ– ๐ฟ ๐œ‚ ๐œ† max (๐‘ƒ ๐œ‚ )ฮ”max ๐‘= ๐œ† max (๐‘ƒ ๐œ‚ ) โˆ’ max ๐œ† min (๐‘ƒ ๐œ‚ ) , ๐œ… = โˆš . ๐œ† min (๐‘ƒ ๐œ‚ ) q Taking ๐‘Š๐œ‚(๐‘–) = ๐‘‰๐œ‚(๐‘–) , the bound becomes ๐‘Šยค ๐œ‚(๐‘–) โ‰ค โˆ’๐‘๐‘Š๐œ‚(๐‘–) + ๐œ– ๐œ…. By the Comparison Lemma [40, Lemma 3.4], ๐‘Š๐œ‚(๐‘–) (๐‘ก) is upper bounded by  ๐œ… ๐‘ ๐œ… ๐‘Š๐œ‚(๐‘–) (๐‘ก) โ‰ค ๐‘Š๐œ‚(๐‘–) (๐‘ก ๐‘“ ) โˆ’ ๐œ– ๐‘’ โˆ’ ๐œ– (๐‘กโˆ’๐‘ก ๐‘“ ) + ๐œ– , ๐‘ ๐‘ leading to the bound on scaled observer error q ๐œผ (๐‘–) (๐‘ก) โ‰ค ๐‘Š๐œ‚(๐‘–) (๐‘ก)/ ๐œ† min (๐‘ƒ๐œ‚ ).  We can now write (3.13) as ๐œ– ๐œน (๐‘–) = ๐œ– 2 ๐›ฝ1 (๐œผ1(๐‘–) ) + ๐œ– ๐›ฝ2 (๐œผ2(๐‘–) ) + ๐œผ3(๐‘–) + ฮ” ๐‘“ (๐‘–) , (3.19) which can be bounded in terms of observer error as (๐‘–) ๐œ– ๐œน (๐‘–) โ‰ค ๐œนmax (๐‘ก) = (๐œ– 2 ๐›ฝ1 + ๐œ– (๐›ฝ2 + ๐ฟ ๐œ‚ ) + 1) ๐œผ (๐‘–) (๐‘ก) . (3.20) (๐‘–) Lemma 3.1 shows that estimation error, ๐œผ (๐‘–) , converges to an ๐‘‚ (๐œ–ฮ”max ) neighborhood of the origin within ๐‘‚ (๐œ–) time. Actuator failure is significantly more dynamic than external disturbance, (๐‘– โˆ— ) (๐‘– โˆ— ) i.e., ๐‡ยค ๐œ‰ is relatively small as compared with ๐ˆ ยค ๐‘š(๐‘–) . Thus, ฮ”max ๐‘ก  ฮ”max (๐‘–) for ๐‘– โ‰  ๐‘–๐‘กโˆ— , since ๐ˆ ยค ๐‘š๐‘ก = 0. Therefore, as stated in Remark 3.2, ๐œ– can be chosen larger for the correct model than for any incorrect model, motivating the use of multiple models to reduce the total system disturbance. In order to select the appropriate model after failure, as long as ๐ˆ ๐‘š(๐‘–) , ๐‡ ๐œ‰ , and ๐œผ3(๐‘–) (๐‘ก ๐‘  ) satisfy, (๐‘– โˆ— ) ๐ˆ ๐‘š(๐‘–) โ‰ฅ 2 ๐‡ ๐œ‰ + ๐œผ3(๐‘–) (๐‘ก ๐‘  ) + ๐œผ3 ๐‘ก (๐‘ก ๐‘  ) , (3.21) (๐‘– โˆ— ) for each ๐‘– โˆˆ {1, . . . , 6} \ {๐‘–๐‘กโˆ— }, the observer estimate, ๐‡ห†ยฏ ๐œ‰ ๐‘ก , will be the smallest in magnitude at ๐‘ก ๐‘  , therefore, (3.16) will select the appropriate model. 63 Proposition 3.1 (Correct Model Selection). Under the control input, ๐Ž ๐‘ (๐‘–๐‘ก ) , the family of observers (3.9) will produce disturbance estimates, ๐‡ห†ยฏ ๐œ‰(๐‘–) , for ๐‘– โˆˆ {0, . . . , 6}. If ๐ˆ ๐‘š(๐‘–) , ๐‡ ๐œ‰ , and ๐œผ3(๐‘–) (๐‘ก ๐‘  ) satisfy (๐‘– โˆ— ) (3.21), the estimate ๐‡ห†ยฏ ๐œ‰ ๐‘ก will be the smallest in magnitude and (3.16) will select the correct model. Proof. Suppose the modeling and external disturbances, ๐‡ ๐œ‰ , the disturbance resulting from incorrect model selection, ๐ˆ ๐‘š(๐‘–) , and the scaled observer error, ๐œผ3(๐‘–) (๐‘ก ๐‘  ) satisfy (3.21), then ๐‡ห†ยฏ ๐œ‰(๐‘–) = ๐‡ ๐œ‰ + ๐ˆ ๐‘š(๐‘–) โˆ’ ๐œผ3(๐‘–) โ‰ฅ k๐ˆ ๐‘š(๐‘–) k โˆ’ k๐‡ ๐œ‰ k + k๐œผ3(๐‘–) k  (๐‘– โˆ— )  (๐‘– โˆ— ) (๐‘– โˆ— ) โ‰ฅ k๐‡ ๐œ‰ k + k๐œผ3 ๐‘ก k โ‰ฅ ๐‡ ๐œ‰ โˆ’ ๐œผ3 ๐‘ก = ๐‡ห†ยฏ ๐œ‰ ๐‘ก , using (3.21) โˆ— (๐‘– ) โˆ— (๐‘– ) where the last equality holds since ๐ˆ ๐‘š๐‘ก = 0. Thus, the estimated disturbance, ๐‡ห†ยฏ ๐œ‰ ๐‘ก , will be the smallest in magnitude at ๐‘ก ๐‘  , and the solution to (3.16) will be the correct model.  Remark 3.4 (Minimum Switching Time). At ๐‘ก ๐‘“ , ๐œผ3(๐‘–) (๐‘ก ๐‘“ ) may be large, but will decay to an (๐‘–) ๐‘‚ (๐œ–ฮ”max ) neighborhood of the origin in ๐‘‚ (๐œ–) time. Therefore, there is some ๐‘‚ (๐œ–) time we must (๐‘– โˆ— ) wait to switch for the observer estimates to converge. Furthermore, ๐œผ3 ๐‘ก (๐‘ก) will decay to an โˆ— (๐‘– ๐‘ก ) โˆ— (๐‘– ) ๐‘‚ (๐œ–ฮ”max ) neighborhood of the origin, further reducing ๐‡ห†ยฏ ๐œ‰ ๐‘ก as compared with the other model (๐‘– โˆ— ) (๐‘–) estimates, since ฮ”max ๐‘ก  ฮ”max . Theorem 3.1 (Stability During Actuator Failure). Let the state of the system (3.12) at the time of failure, ๐‘ก ๐‘“ , be such that ๐‘‰๐œ‰ (๐‘ก ๐‘“ ) < ๐‘Ž for some sufficiently small ๐‘Ž > 0. Then, there exist ๐œ–1 , ๐œ–2 > 0, and maximum switching time, ๐‘ก ๐‘ max > ๐‘ก ๐‘“ , such that the state, ๐ƒ, will remain within the domain of operation during the failure transient, and will recover tracking performance i. after the transient, when ๐œ– โˆˆ (0, ๐œ–1 ); ii. if the correct model, ๐‘–๐‘ก = ๐‘–๐‘กโˆ— , is selected before ๐‘ก ๐‘ max , when ๐œ– โˆˆ (๐œ–1 , ๐œ–2 ). Proof. A common Lyapunov function, ๐‘‰๐œ‰ , for the feedback linearized rotational subsystem for each ๐‘– โˆˆ {0, . . . , 6} and ๐œ– โ†’ 0 is given by ๐‘‰๐œ‰ = ๐ƒ > ๐‘ƒ๐œ‰ ๐ƒ, where ๐‘ƒ๐œ‰ ๐ด๐œ‰ + ๐ด๐œ‰> ๐‘ƒ๐œ‰ = โˆ’๐ผ6 . (3.22) 64 Let ฮฉ๐œ‰ = {๐‘‰๐œ‰ < ๐‘ ๐œ‰ } be an estimate of the domain of operation of the controller designed in Section 3.2.2 for ๐‘ ๐œ‰ โˆˆ R0 (see [12] for more details). For simplicity, we use the estimate of the domain of operation for ๐œ– โ†’ 0, wherein the states and disturbances are estimated perfectly. For small ๐œ– > 0, the obtained domain of operation can be shrunk to ฮฉ0๐œ‰ = {๐‘‰๐œ‰ < ๐‘0๐œ‰ }, with ๐‘0๐œ‰ < ๐‘ ๐œ‰ , to incorporate the effect of estimation error [10]. Using singular perturbation [40, Theorem 11.4] and non-vanishing perturbation [40, Lemma (๐‘–) 9.2], it can be shown that (3.17) converges to an ๐‘‚ (๐œ–ฮ”max ) neighborhood of the origin for ๐œ– โˆˆ (0, ๐œ–2 ). (๐‘–) For ๐œ– โˆˆ (0, ๐œ–1 ) the neighborhood ๐‘‚ (๐œ–ฮ”max ) is small enough for reasonable tracking performance. For ๐œ– โˆˆ (๐œ–1 , ๐œ–2 ), the large estimation error can make the trajectory leave the domain of operation and the system may diverge, thus requiring a model switch. Taking the Lyapunov function (3.22), and computing its derivative with the rotational closed- loop system (3.12) yields ๐‘‰ยค๐œ‰ = โˆ’๐ƒ > ๐ƒ + 2๐ƒ > ๐‘ƒ๐œ‰ ๐œ– ๐ต1 ๐œน (๐‘–) . (3.23) p By the change of variables ๐‘Š๐œ‰ = ๐‘‰๐œ‰ , and the arguments in the proof of Lemma 1, we can immediately upper bound ๐‘Š๐œ‰ (๐‘ก) by โˆ’(๐‘กโˆ’๐‘ก ๐‘“ ) ๐œ† max (๐‘ƒ๐œ‰ ) (๐‘–) โˆซ ๐‘ก โˆ’(๐‘กโˆ’๐œ) 2๐œ†max ( ๐‘ƒ ๐œ‰ ) ๐‘Š๐œ‰ (๐‘ก ๐‘“ )๐‘’ + ๐‘’ 2๐œ†max ( ๐‘ƒ ๐œ‰ ) p ๐œนmax (๐œ)๐‘‘๐œ. ๐‘ก๐‘“ ๐œ† min (๐‘ƒ๐œ‰ ) Let ๐‘ก = ๐‘ก ๐‘ max be the unique solution to the equation โˆ’(๐‘กโˆ’๐‘ก ๐‘“ ) โˆš ๐œ† max (๐‘ƒ๐œ‰ ) (๐‘–) โˆซ ๐‘ก โˆ’(๐‘กโˆ’๐œ) 2๐œ†max ( ๐‘ƒ ๐œ‰ ) ๐‘Ž๐‘’ + ๐‘’ 2๐œ†max ( ๐‘ƒ ๐œ‰ ) p ๐œนmax (๐œ)๐‘‘๐œ = ๐‘ ๐œ‰ . ๐‘ก๐‘“ ๐œ† min (๐‘ƒ๐œ‰ ) The theorem follows immediately from the definition of ๐‘ก ๐‘ max .  Remark 3.5 (Identification of ๐œ–1 and ๐œ–2 ). In principle, the Lyapunov arguments in the singular perturbation analysis [40] can be used to estimate ๐œ– 1 and ๐œ–2 , however, these estimates are usually quite conservative. In practice, these values are chosen through empirical tuning on the system on which it will be implemented. For this system, we arrive at two empirically tuned estimates of ๐œ–1 โ‰ˆ 0.002 and ๐œ– 2 โ‰ˆ 0.06 through simulation. 65 ๐œ– ๐‘+ ๐‘Ÿ ๐‘ ๐›พ1 ๐›ฝ1 ๐›ผ1 ๐›ผ3 0.025 1.8182e-5 0.275m 0.1 2 40 3 0.6 ๐œ๐‘š ๐‘โˆ’ ๐‘š ๐‘Ž0 ๐›พ2 ๐›ฝ2 ๐›ผ2 T 0.059 3.6364e-6 1.824kg 2 1.5 20 3 0.01s Table 3.1: System parameters used in simulation and experiment. 3.5 Numerical Simulation The proposed method is simulated for a hexrotor system tracking a trajectory generated by a 9-th order polynomial to ensure sufficient smoothness, shown in Fig. 3.2. The system is simulated in discrete-time with sample time, ๐‘‡ = 0.01๐‘ , while using position and orientation measurements with added white Gaussian noise to replicate the experimental system. The hexrotor is able to track the reference trajectory, suffer an actuator failure at 14 seconds into flight, and recover to resume tracking the trajectory after switching controllers. The system is simulated with large external rotational disturbances, ๐ˆ ๐œ‰ = 12[sin(๐‘ก) cos(๐‘ก) sin(๐‘ก)] > , and translational disturbances, ๐ˆ ๐œŒ = [sin(๐‘ก) cos(๐‘ก) sin(๐‘ก)] > . To facilitate tuning the parameters, for example ๐œ–, ๐‘Ž 0 , and control gains, we use the same parameters in simulation as in the experiment. The parameters are given in Table 3.1, and the inertia matrix is ๏ฃฎ ๏ฃน ๏ฃฏ0.0228 0 0 ๏ฃบ ๏ฃฏ ๏ฃบ 2 ๏ฃฏ ๏ฃบ ๐ฝ=๏ฃฏ 0๏ฃฏ 0.0241 0 ๏ฃบ๏ฃบ kg m . ๏ฃฏ ๏ฃบ ๏ฃฏ 0 0 0.0446๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป The estimated Lyapunov function derivative, ๐‘‰ยคฬ‚๐œ‰ , is monitored to determine when the failure occurs. The magnitudes of the estimated disturbances for all six failure modes, as well as the Lyapunov derivative estimate, are shown in Fig. 3.1. At the time of detection, ๐‡ห†ยฏ ๐œ‰(4) has the smallest magnitude, indicating a failure of actuator four. The dashed vertical line in Fig. 3.1 shows the time when a failure is induced, ๐‘ก ๐‘“ , and the solid vertical line shows when the switch occurs, ๐‘ก ๐‘  . Fig. 3.2 shows the hexrotor briefly breaking from tracking the reference trajectory as the failure occurs at ๐‘ก ๐‘“ , and after the controller is switched, the hexrotor successfully resumes tracking the 66 80 70 60 50 40 30 20 10 13.9 13.95 14 14.05 14.1 14.15 14.2 3 2 1 0 13.9 13.95 14 14.05 14.1 14.15 14.2 Figure 3.1: Norm of disturbance estimates for all failure case models and Lyapunov derivative estimate during a simulated in-flight actuator failure. trajectory. The tracking performance is slightly degraded due to the large external disturbances present in this simulation, however, a successful recovery is still achieved. A potential problem with not considering disturbance is the false identification of failures. We illustrate this with the rotor health estimation approach [91]. This method utilizes an Extended Kalman Filter which estimates the health of each rotor, โ„Ž ๐‘— , for ๐‘— โˆˆ {1, ...6}. Utilizing the same dynamics and a detection cutoff on the health of each rotor of 0.5, as was shown to work well experimentally in [91], we simulate this method. The same flight parameters and large disturbances are again applied to the system with the EHGO based failure detection method replaced by the EKF method. The EKF method does not consider disturbances, and Fig. 3.3 shows that a failure of actuator two is falsely detected just under two seconds into flight. In principle, the EKF could be augmented with a disturbance model to improve this performance, however, that would require a model of the expected disturbances [77]. The EHGO can accommodate a wide range of disturbances with unknown dynamics. We also investigated lowering the cutoff threshold for 67 2 1 0 0 5 10 15 20 25 1 0.5 0 0 5 10 15 20 25 0 -0.2 -0.4 0 5 10 15 20 25 Figure 3.2: Hexrotor position tracking recovery after simulated in-flight actuator failure using multiple models to recover performance. failure detection, however, this results in longer detection times. In summary, depending on flight conditions, considering disturbance in the failure recovery strategy becomes important. 3.6 Experimental Validation The proposed multiple-model estimation and control method is implemented on an experimental platform to validate performance and show recovery from an actuator failure during flight. The experimental platform is built on a 550mm hexrotor frame with 920kV motors and 10x4.5 carbon fiber rotors. Six 35A ESCs with bidirectional capability are used, and the system is powered by a 5000mAh 4s LiPo battery. The control method is implemented on a Pixhawk 4 FMU in discrete time at 100Hz using Mathworks Simulink through the PX4 Autopilots Support from Embedded Coder package. All sensing and computation is done on-board the vehicle, with the exception of utilizing translational position data from a Vicon motion capture system in lieu of GPS. 68 1 1 0.5 0.5 1 2 3 4 1 2 3 4 1 1 0.5 0.5 1 2 3 4 1 2 3 4 1 1 0.5 0.5 1 2 3 4 1 2 3 4 Figure 3.3: Health estimate of each rotor as estimated using the Extended Kalman Filter method. Once the failure is detected and the controller is switched, the rotor opposite the failed rotor will be commanded to reverse directions to apply a large downward force to counteract the roll and pitch errors. Once the system returns to level flight, using the pseudo-inverse to compute desired rotor speeds results in the opposite rotor being commanded to apply small forces in either direction, thus requiring the rotor to change directions rapidly. During experimental testing it became clear that the opposite rotor could not change directions quickly enough to stabilize the system. To restrict the opposite rotor to only generate downward force for a detected failure, ๐‘–, we impose force (๐‘–, ๐‘—) (๐‘–, ๐‘—) constraints, ๐‘“min , ๐‘“max < 0 for the opposite rotor, ๐‘—, defined by ๏ฃฑ ๏ฃด ๏ฃฒ๐‘– + 3, 1 โ‰ค ๐‘– โ‰ค 3, ๏ฃด ๏ฃด ๏ฃด ๐‘—= ๏ฃด ๏ฃด๐‘– โˆ’ 3, 4 โ‰ค ๐‘– โ‰ค 6, ๏ฃด ๏ฃด ๏ฃณ (๐‘–, ๐‘—) (๐‘–, ๐‘—) and ๐‘“min , ๐‘“max โ‰ฅ 0 for all remaining ๐‘—. These constraints ensure only a single directional change โˆ— will be commanded when the model is switched. Let ยฏ๐’‡ be the solution to the following optimization 69 70 60 50 40 30 20 89.5 89.55 89.6 89.65 89.7 89.75 89.8 89.85 3 2 1 0 89.5 89.55 89.6 89.65 89.7 89.75 89.8 89.85 Figure 3.4: Norm of disturbance estimates for all failure case models and Lyapunov derivative estimate during an experimental in-flight actuator failure. problem with above discussed constraints under the selected model ๐‘–๐‘ก   2 2 minimize ยฏ๐’‡ + ๐œ† ๐‘Š๐‘ฃ (๐‘€ F ยฏ๐’‡ โˆ’ ๐’– (๐‘– ๐‘ก ) (๐‘– ห† ) ๐‘ก ) ยฏ๐’‡ (๐‘–, ๐‘—) (๐‘–, ๐‘—) subject to ๐‘“min โ‰ค ๐‘“ยฏ๐‘–,โˆ—๐‘— โ‰ค ๐‘“max , where ๐œ† โˆˆ R0 is chosen to be large to ensure we achieve applied forces and torques as close as possible to the desired ๐’– ห† (๐‘–๐‘ก ) . We also take advantage of the diagonal weighting matrix, ๐‘Š๐‘ฃ โˆˆ R4ร—4 , to prioritize the total thrust and the roll and pitch torques, allowing for lower performance in yaw 2 tracking since the former are integral for achieving a successful recovery. The additional ยฏ๐’‡ term โˆ— is used to simultaneously select a solution with lower energy. The solution, ยฏ๐’‡ , is then mapped to squared rotor speeds through the inverse of (3.4). The optimization problem is solved by the active-set algorithm proposed in [26]. The experimental system is flown along the same trajectory as in simulation and a failure of actuator four is induced algorithmically. The norm of the experimental disturbance estimates for 70 3 2 1 0 80 85 90 95 100 1 0.5 0 80 85 90 95 100 0 -0.2 -0.4 80 85 90 95 100 Figure 3.5: Hexrotor position tracking recovery after experimental in-flight actuator failure. each failure model, and the Lyapunov derivative estimate, are shown in Fig. 3.4. The dashed vertical line in Fig. 3.4 corresponds to the time when a failure is induced, ๐‘ก ๐‘“ , and the solid vertical line shows when the detection and switch occurs, ๐‘ก ๐‘  . The correct model for a failure of actuator four is selected and the resulting tracking performance before and after recovery are shown in Fig. 3.5. The tracking performance after failure is slightly degraded due to the use of non-ideal control inputs, ยฏ๐’‡ โˆ— . A video of the experiments can be found at https://youtu.be/8fQMrca49os For a highly detailed treatment of the experimental multi-rotor system, including parameter identifica- tion, rotor dynamics and force characteristics, and software implementation of these control methods, see Appendix A. 3.7 Conclusions In this chapter, we studied a trajectory tracking problem for a hexrotor in the presence of modeling error and external disturbances, while simultaneously enabling in-flight recovery of a complete actuator failure. A multiple-model, multiple extended high-gain observer based output feedback control framework is used 71 to enable this extended functionality. The framework is rigorously analyzed to provide stability guarantees and bounds on the maximum switching time for recovery. Simulation and experimental flight data show the successful application of the method on a physical system. 72 CHAPTER 4 TOWARDS MULTI-BODY MULTI-ROTORS FOR LONG REACH MANIPULATION In this chapter, we design a novel multi-body multi-rotor UAV to be used for long reach manipulation tasks and extend the base control technology presented in Chapter 2 to enable robust trajectory tracking performance on this novel airframe design. We consider the modeling and control of a multi-body multi- rotor system in which a horizontally actuated bi-rotor platform is suspended from a larger multi-rotor through a passive revolute joint. We provide dynamic modeling, feedback linearizing control through the use of flat outputs, and an extended high-gain observer based output feedback control design assuming that the system operates in a plane. Simulation results are provided to show the long reach manipulator platform tracking a trajectory. The methods are rigorously analyzed and stability of the closed-loop system under output feedback is proven. The work presented in this chapter was a collaborative effort with fellow graduate student Vishal Abhishek. This work originally appeared in the proceedings of the 2021 American Control Conference [8]. The remainder of the chapter is organized in the following manner. Section 4.1 gives the system dynamics and modeling of the aerial manipulator whereas Section 4.2 details the control design using the extended high-gain observer. Section 4.3 proves stability of the system, and Section 4.4 provides simulation results. Finally, Section 4.5 concludes the chapter. 4.1 System Dynamics We study a rigid platform with bi-rotor actuation, rigidly attached to a rigid rod connected at the other end to a carrier UAV through a passive revolute joint. The suspended platform is actuated with two rotors so as to provide a net thrust perpendicular to the rod with no net moment. The two rotors attached at both ends of the platform are operated with angular velocity in opposite directions to generate viscous torque in opposite directions while generating thrust in the same direction. Thus, when operated at the same speed, the rotors generate a net thrust along the axis of the platform with no net moment. The resulting multi-body system is shown in Fig. 4.1. For the analysis going forward, we restrict our case to planar motion in the vertical plane. 73 ๐‘ข1 ๐›ฝ (๐‘ฅ๐‘„ , ๐‘ฆ๐‘„ ) ๐‘ข2 ๐›ผ ๐ฟ ๐‘ข3 (๐‘ฅ๐‘ƒ , ๐‘ฆ๐‘ƒ ) ๐‘Œ0 ๐‘‚0 ๐‘‹0 Figure 4.1: Carrier UAV with suspended bi-rotor actuated platform. 4.1.1 Dynamic Model We begin by introducing the following notation. Unless otherwise stated, all coordinates are expressed in the inertial frame. The mass of the carrier UAV is ๐‘š ๐‘„ โˆˆ R 0 , the mass of the platform is ๐‘š ๐‘ƒ โˆˆ R 0 , and ๐‘” is the acceleration due to gravity. The length of the connecting rod is ๐ฟ โˆˆ R 0 , with the ๐‘ฅ and ๐‘ฆ positions of the carrier UAV center of mass, ๐‘ฅ ๐‘„ โˆˆ R and ๐‘ฆ ๐‘„ โˆˆ R, and the position of the platform center of mass, ๐‘ฅ ๐‘ƒ โˆˆ R and ๐‘ฆ ๐‘ƒ โˆˆ R. The angular position of the platform is ๐›ผ โˆˆ R and the angular position of the carrier UAV is ๐›ฝ โˆˆ (โˆ’๐œ‹/2, ๐œ‹/2). The moment of inertia of the carrier UAV in the body-frame is ๐ผ๐‘„ โˆˆ R 0 and the moment of inertia of the platform in the body-frame is ๐ผ ๐‘ƒ โˆˆ R 0 . The control inputs are the total thrust of the carrier UAV, ๐‘ข 1 โˆˆ R 0 , the torque on the carrier UAV about its center of mass, ๐‘ข 2 โˆˆ R, and the total platform thrust, ๐‘ข 3 โˆˆ R 0 . We assume the revolute joint and the connecting rod are rigidly attached to the platform and are massless. The centers of mass of the carrier UAV and the platform are assumed to be at their geometric centers and are the same as the two end-points of the connecting rod. The actuator forces are the total carrier UAV thrust, ๐‘ข 1 , the carrier UAV moment, ๐‘ข 2 , and the suspended platform thrust, ๐‘ข 3 . These forces are shown in Fig. 4.1. The equations of motion are written using ๐‘ฅ ๐‘ƒ , ๐‘ฆ ๐‘ƒ , ๐›ผ, and ๐›ฝ as generalized coordinates. The kinetic 74 energy, ๐‘‡, and the potential energy, ๐‘ˆ, are given by 1 1 1 ๐‘‡= (๐‘š ๐‘ƒ + ๐‘š ๐‘„ ) ( ๐‘ฅยค 2๐‘ƒ + ๐‘ฆยค 2๐‘ƒ ) โˆ’ ๐‘š ๐‘„ ๐ฟ ( ๐‘ฅยค ๐‘ƒ ๐›ผ๐‘ ยค ๐›ผ ) + ๐ผ๐‘„ ๐›ฝยค2 + (๐ผ ๐‘ƒ + ๐‘š ๐‘„ ๐ฟ 2 ) ๐›ผยค 2 , ยค ๐›ผ + ๐‘ฆยค ๐‘ƒ ๐›ผ๐‘  2 2 2 ๐‘ˆ = ๐‘š ๐‘„ ๐‘”(๐‘ฆ ๐‘ƒ + ๐ฟ๐‘ ๐›ผ ) + ๐‘š ๐‘ƒ ๐‘”๐‘ฆ ๐‘ƒ . The corresponding generalized forces are F๐‘ฅ ๐‘ƒ = ๐‘ข 3 ๐‘ ๐›ผ โˆ’ ๐‘ข 1 ๐‘  ๐›ฝ , F๐‘ฆ๐‘ƒ = ๐‘ข 3 ๐‘  ๐›ผ + ๐‘ข 1 ๐‘ ๐›ฝ , F๐›ผ = ๐‘ข 1 ๐ฟ๐‘  (๐›ฝโˆ’๐›ผ) , F๐›ฝ = ๐‘ข 2 . Here, ๐‘ ๐›ผ and ๐‘  ๐›ผ represent cos ๐›ผ and sin ๐›ผ respectively. The equations of motion can be written compactly as ๐‰ = ๐‘€ (๐’’ 1 ) ๐’’ยฅ 1 + ๐ถ (๐’’ 1 , ๐’’ยค 1 ) ๐’’ยค 1 + ๐บ (๐’’ 1 ), (4.1a) ๐‘ข 2 = ๐ผ๐‘„ ๐›ฝ, ยฅ (4.1b) where, ๐’’ 1 := [๐‘ฅ ๐‘ƒ , ๐‘ฆ ๐‘ƒ , ๐›ผ] > , ๐‰ := [F๐‘ฅ ๐‘ƒ , F๐‘ฆ๐‘ƒ , F๐›ผ ] > , and ๏ฃฎ ๏ฃน ๏ฃฏ (๐‘š ๐‘ƒ + ๐‘š ๐‘„ ) 0 โˆ’๐‘š ๐‘„ ๐ฟ๐‘ ๐›ผ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐‘€=๏ฃฏ ๏ฃฏ 0 (๐‘š ๐‘ƒ + ๐‘š ๐‘„ ) โˆ’๐‘š ๐‘„ ๐ฟ๐‘  ๐›ผ ๏ฃบ๏ฃบ , ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃฏ โˆ’๐‘š ๐‘„ ๐ฟ๐‘ ๐›ผ โˆ’๐‘š ๐‘„ ๐ฟ๐‘  ๐›ผ (๐ผ ๐‘ƒ + ๐‘š ๐‘„ ๐ฟ ) ๏ฃบ 2 ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฎ ๏ฃน ๏ฃฏ0 0 ๐‘š ๐‘„ ๐ฟ ๐›ผ๐‘  ๏ฃฏ ยค ๐›ผ ๏ฃบ๏ฃบ (4.2) ๏ฃฏ ๏ฃบ ๐ถ = ๏ฃฏ๏ฃฏ0 0 โˆ’๐‘š ๐‘„ ๐ฟ ๐›ผ๐‘ ยค ๐›ผ ๏ฃบ๏ฃบ , ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ0 0 0 ๏ฃบ ๏ฃฐ ๏ฃป  > ๐บ = 0 (๐‘š ๐‘ƒ + ๐‘š ๐‘„ )๐‘” โˆ’๐‘š ๐‘„ ๐‘”๐ฟ๐‘  ๐›ผ . 4.1.2 Differential flatness The system has four generalized coordinates at the position level: ๐‘ฅ ๐‘ƒ , ๐‘ฆ ๐‘ƒ , ๐›ผ, and ๐›ฝ, but only three independent control inputs: ๐‘ข 1 , ๐‘ข 2 , and ๐‘ข 3 , thus resulting in one degree of underactuation. It can be seen from (4.1a) and the expression for ๐‰ that designing a trajectory in ๐’’ 1 uniquely determines the trajectories of ๐‘ข 1 , ๐‘ข 3 , and ๐›ฝ. Furthermore, a trajectory in ๐›ฝ uniquely determines ๐‘ข 2 . Thus, a trajectory in ๐’’ 1 uniquely determines all state trajectories and control inputs. As a result, ๐’’ 1 are the differentially flat outputs. 75 4.1.3 State-Space Model The dynamics can now be written as two subsystems in state-space form. Taking ๐’’ 2 = ๐’’ยค 1 , ๐’’ = [๐’’ > > > 1 , ๐’’2 ] , the platform dynamics become ๐’’ยค 1 = ๐’’ 2 , (4.3) ๐’’ยค 2 = ๐‘€ (๐’’ 1 ) โˆ’1 [โˆ’๐ถ (๐’’)๐’’ 2 โˆ’ ๐บ (๐’’ 1 )] + ๐‘€ (๐’’ 1 ) โˆ’1 ๐‰ + ๐ˆ ๐‘ž , where ๐ˆ ๐‘ž โˆˆ R3 is an added term to represent the lumped disturbance in the platform subsystem, which satisfies the following assumption. Definition 4.1 (Prime Canonical Form). A control system in the โ€œprime canonical formโ€ [66], for state ๐’™ โˆˆ R๐‘› , control input ๐‘ข โˆˆ R, and disturbance ๐œŽ โˆˆ R, has the following representation ๐’™ยค = ๐ดprm ๐’™ + ๐ตprm ๐‘“prm (๐‘ก, ๐’™, ๐‘ข), ๐‘ฆ = ๐ถprm ๐’™, (4.4) where ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ0๐‘›โˆ’1ร—1 ๐ผ๐‘›โˆ’1 ๏ฃบ ๏ฃฏ0๐‘›โˆ’1ร—1 ๏ฃบ =๏ฃฏ ๏ฃบ , ๐ตprm = ๏ฃฏ ๏ฃบ , ๐‘“prm : R โ‰ฅ0 ร— R๐‘› ร— R โ†’ R, ๐ถprm = [1 01ร—๐‘›โˆ’1 ], ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐ดprm ๏ฃฏ 0 01ร—๐‘›โˆ’1 ๏ฃบ ๏ฃฏ 1 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป 0 ๐‘ร—๐‘ž is a matrix of zeros with dimension ๐‘ ร— ๐‘ž, ๐ผ ๐‘ is the identity matrix of dimension ๐‘, and ๐‘ฆ โˆˆ R is the measurement. Assumption 4.1 (Disturbance Properties). The dynamics of the various subsystems in this chapter take the prime canonical form perturbed by a disturbance term. We assume that the disturbance enters the RHS of (4.4) as ๐ต ๐‘‘ ๐œŽ where ๐ต ๐‘‘ = ๐ตprm , ๐œŽ is continuously differentiable, and its partial derivatives with respect to states are bounded on compact sets of those states for all ๐‘ก โ‰ฅ 0. Taking ๐›ฝ1 = ๐›ฝ, ๐›ฝ2 = ๐›ฝ,ยค and ๐œท = [๐›ฝ1 , ๐›ฝ2 ] > the carrier UAV orientation dynamics become ๐›ฝยค1 = ๐›ฝ2 , ๐‘ข2 (4.5) ๐›ฝยค2 = + ๐œŽ๐›ฝ , ๐ผ๐‘„ where ๐œŽ๐›ฝ โˆˆ R is an added term to represent the lumped disturbance in the carrier UAV subsystem, which also satisfies Assumption 4.1. 76 Let ๐’’ ๐‘‘ = [๐‘ฅ ๐‘‘๐‘ƒ , ๐‘ฆ ๐‘‘๐‘ƒ , ๐›ผ ๐‘‘ ] > be the desired output trajectory and let ๐›ฝ ๐‘ be the desired orientation of the carrier UAV. By making the following change of variables ๐’†1 = ๐’’1 โˆ’ ๐’’ ๐‘‘ , ๐’† 2 = ๐’†ยค 1 = ๐’’ 2 โˆ’ ๐’’ยค ๐‘‘ , ๐’† = [๐’† > > > 1 , ๐’†2 ] , ๐›ฝหœ1 = ๐›ฝ1 โˆ’ ๐›ฝ ๐‘ , ๐›ฝหœ2 = ๐›ฝยคฬƒ1 = ๐›ฝ2 โˆ’ ๐›ฝยค๐‘ , หœ = [ ๐›ฝหœ1 , ๐›ฝหœ2 ] > , ๐œท the system can be written in terms of tracking error as ๐’†ยค 1 = ๐’† 2 , ๐’†ยค 2 = ๐‘“ (๐’’ 1 , ๐’†, ๐’’ยค ๐‘‘ ) + ๐‘€ (๐’’ 1 ) โˆ’1 ๐‰ + ๐ˆ ๐‘ž โˆ’ ๐’’ยฅ ๐‘‘ , (4.6) ๐›ฝยคฬƒ1 = ๐›ฝหœ2 , ๐›ฝยคฬƒ2 = ๐‘ข2 + ๐œ๐›ฝ, ๐ผ๐‘„ where ๐‘“ (๐’’ 1 , ๐’†, ๐’’ยค ๐‘‘ ) = ๐‘€ (๐’’ 1 ) โˆ’1 [โˆ’๐ถ (๐’’ 1 , ๐’† 2 + ๐’’ยค ๐‘‘ ) (๐’† 2 + ๐’’ยค ๐‘‘ ) โˆ’ ๐บ (๐’’ 1 )] and ๐›ฝยฅ๐‘ is lumped into the disturbance term ๐œ ๐›ฝ = ๐œŽ๐›ฝ โˆ’ ๐›ฝยฅ๐‘ . The advantage of this modification is we no longer require higher-order derivatives of ๐›ฝ ๐‘ , however ๐›ฝ ๐‘ must be third order differentiable to ensure ๐œ ๐›ฝ satisfies Assumption 4.1. 4.2 Control Design We begin by designing a state feedback control law for each subsystem using feedback linearization, thus requiring the assumption that all states and disturbances are known. This assumption will then be relaxed by introducing an extended high-gain observer to estimate all states and disturbances to arrive at an output feedback control law. 4.2.1 State Feedback Control Using the differentially flat outputs, ๐’’ 1 , we design a trajectory tracking controller by taking the control input ๐‰ ๐‘ = ๐‘€ (๐’’ 1 ) [โˆ’๐‘˜ 1 ๐’† 1 โˆ’ ๐‘˜ 2 ๐’† 2 โˆ’ ๐ˆ ๐‘ž + ๐’’ยฅ ๐‘‘ โˆ’ ๐‘“ (๐’’ 1 , ๐’†, ๐’’ยค ๐‘‘ )]. Take ๐›ฝ ๐‘ as the desired orientation of the carrier UAV, and use the expression for ๐‰ at ๐›ฝ = ๐›ฝ ๐‘ to compute ๐‘ข 3๐‘ , ๐‘ข 1๐‘ , and ๐›ฝ ๐‘ from the equation ๐‰ (๐›ฝ=๐›ฝ ๐‘ ) = ๐‰ ๐‘ = [๐œ1๐‘ , ๐œ2๐‘ , ๐œ3๐‘ ] > . This leads to the following feedback linearizing control equations ๐‘ข 3๐‘ = ๐œ1๐‘ ๐‘ ๐›ผ + ๐œ2๐‘ ๐‘  ๐›ผ + ๐œ3๐‘ /๐ฟ, q ๐‘ข 1๐‘ = (๐‘ข 3๐‘ ๐‘ ๐›ผ โˆ’ ๐œ1๐‘ ) 2 + (๐œ2๐‘ โˆ’ ๐‘ข 3๐‘ ๐‘  ๐›ผ ) 2 , (4.7) ๐›ฝ ๐‘ = atan2((๐‘ข 3๐‘ ๐‘ ๐›ผ โˆ’ ๐œ1๐‘ ), (๐œ2๐‘ โˆ’ ๐‘ข 3๐‘ ๐‘  ๐›ผ )). 77 Note that for ๐›ฝ ๐‘ to be third order differentiable, we require the desired trajectory, ๐’’ ๐‘‘ , to be fifth order differentiable. If we set the control actions ๐‘ข 1 = ๐‘ข 1๐‘ , ๐‘ข 3 = ๐‘ข 3๐‘ , and ๐‘ข 2 = ๐ผ๐‘„ (โˆ’๐‘˜ 3 ๐›ฝหœ1 โˆ’ ๐‘˜ 4 ๐›ฝหœ2 โˆ’ ๐œ ๐›ฝ ) the closed-loop system dynamics become ๏ฃฎ ๏ฃน ๏ฃฏ 03ร—1 ๏ฃบ ๐’†ยค = ๐ด๐’† ๐’† + ๏ฃฏ (4.8a) ๏ฃฏ ๏ฃบ ๏ฃบ, ๏ฃฏ ๐’† ๐›ฝ (๐‘ก, ๐’’ 1 , ๐›ฝหœ1 ) ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป ยคฬƒ๐œท = ๐ด ๐œท,หœ (4.8b) ๐œทหœ where ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ 03 ๐ผ3 ๏ฃบ๏ฃบ ๏ฃฏ 0 1 ๏ฃบ๏ฃบ ๐ด๐’† = ๏ฃฏ ๏ฃบ , ๐ด ๐œทหœ = ๏ฃฏ ๏ฃฏ ๏ฃฏ ๏ฃบ, ๏ฃฏโˆ’๐‘˜ 1 ๐ผ3 โˆ’๐‘˜ 2 ๐ผ3 ๏ฃบ ๏ฃฏโˆ’๐‘˜ 3 โˆ’๐‘˜ 4 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฎ ๏ฃน ๏ฃฏ ๏ฃฏ ๐‘ข 1๐‘ (๐‘  ๐›ฝ โˆ’ ๐‘  ๐›ฝ ๐‘ ) ๏ฃบ ๏ฃบ หœ โˆ’1 ๏ฃฏ ๏ฃบ ๐’† ๐›ฝ (๐‘ก, ๐’’ 1 , ๐›ฝ1 ) = ๐‘€ (๐’’ 1 ) ๏ฃฏ ๏ฃฏ ๐‘ข 1 (๐‘ ๐›ฝ ๐‘ โˆ’ ๐‘ ๐›ฝ ) ๐‘ ๏ฃบ, ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‘ ๏ฃบ ๏ฃฏ๐‘ข 1 ๐ฟ(๐‘  (๐›ฝ ๐‘ โˆ’๐›ผ) โˆ’ ๐‘  (๐›ฝโˆ’๐›ผ) ) ๏ฃบ ๏ฃฐ ๏ฃป and ๐ผ3 โˆˆ R3ร—3 is the identity matrix, 03 โˆˆ R3ร—3 is a matrix of zeros, and 03ร—1 โˆˆ R3 is a vector of zeros. As a result, the two closed-loop subsystems form a cascade connection through ๐’† ๐›ฝ and the fact that ๐’† ๐›ฝ (๐‘ก, ๐’’ 1 , 0) = 0 is important when analyzing stability of the cascaded system. Note that ๐›ฝหœ2 and ๐œ ๐›ฝ contain ๐›ฝยค๐‘ and ๐›ฝยฅ๐‘ respectively. We do not compute these derivatives directly, however the terms containing them are estimated completely by the extneded high-gain observer in the output feedback control design. 4.2.2 Extended High-Gain Observer Design We now present the design of an extended high-gain observer to estimate unmeasured states as well as uncertainties in the form of modeling error and external disturbances. These estimates will be used to cancel out the uncertainties in the control design, resulting in improved performance of the feedback linearizing controllers. For the extended high-gain observer design we assume only the position level states can be 78 measured. We begin by extending the tracking error dynamics (4.6) to include disturbance dynamics ๐’†ยค 1 = ๐’† 2 , ๐’†ยค 2 = ๐‘“ (๐’’ 1 , ๐’†, ๐’’ยค ๐‘‘ ) + ๐‘€ (๐’’ 1 ) โˆ’1 ๐‰ + ๐ˆ ๐‘ž โˆ’ ๐’’ยฅ ๐‘‘ , ๐ˆยค ๐‘ž = ๐œ‘๐‘ž (๐‘ก, ๐’†), (4.9) ๐›ฝยคฬƒ1 = ๐›ฝหœ2 , ๐›ฝยคฬƒ2 = ๐‘ข2 + ๐œ๐›ฝ, ๐ผ๐‘„ ยค ๐›ฝ = ๐œ‘ ๐›ฝ (๐‘ก, ๐œท), ๐œŽ หœ where ๐œ‘๐‘ž (๐‘ก, ๐’†) and ๐œ‘ ๐›ฝ (๐‘ก, ๐œท) หœ are unknown functions describing the disturbance dynamics and are assumed to be continuous and bounded on any compact set in ๐’† and ๐œท หœ to ensure Assumption 4.1 is satisfied. To facilitate writing the observer dynamics in a compact form, we define the state vector ๐Œ = [๐’† > > > หœ หœ > หœ > หœ > 1 , ๐’† 2 , ๐ˆ ๐‘ž , ๐›ฝ1 , ๐›ฝ2 , ๐œŽ๐›ฝ ] , and the disturbance function vector ๐œ‘(๐‘ก, ๐’†, ๐œท) = [๐œ‘ ๐‘ž (๐‘ก, ๐’†) , ๐œ‘ ๐›ฝ (๐‘ก, ๐œท)] , leading to the observer dynamics ยคฬ‚ = ๐ด ๐Œ ห† + ๐ต ๐‘“ยฏ(๐’’ 1 , ห†๐’† , ๐’’ยค ๐‘‘ ) + ๐บยฏ (๐’’ 1 )๐’– + ๐ป ๐Œ   ๐Œ ห† ๐‘’, (4.10) ห†๐‘’ = ๐ถ( ๐Œ โˆ’ ๐Œ ๐Œ ห† ), where 2 2 2 2 ๐ด = โŠ•๐‘–=1 ๐ด๐‘– , ๐ต = โŠ•๐‘–=1 ๐ต๐‘– , ๐ถ = โŠ•๐‘–=1 ๐ถ๐‘– , ๐ป = โŠ•๐‘–=1 ๐ป๐‘– , ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ03 ๐ผ3 03 ๏ฃบ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ ๐œŒ1 /๐œ– ๐ผ3 ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ   ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐ด1 = ๏ฃฏ๏ฃฏ03 03 ๏ฃฏ ๏ฃบ ๏ฃฏ 2 ๐ผ3 ๏ฃบ , ๐ต1 = ๏ฃฏ ๐ผ3 ๏ฃบ , ๐ป1 = ๏ฃฏ ๐œŒ2 /๐œ– ๐ผ3 ๏ฃบ , ๐ถ1 = ๐ผ3 03 03 , ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃฏ03 03 03 ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ03 ๏ฃบ ๏ฃฏ 3 ๏ฃฏ ๐œŒ3 /๐œ– ๐ผ3 ๏ฃบ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ0 1 0๏ฃบ๏ฃบ ๏ฃฏ0๏ฃบ ๏ฃฏ ๐œŒ1 /๐œ– ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ   ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๐ด2 = ๏ฃฏ๏ฃฏ0 0 1๏ฃบ , ๐ต2 = ๏ฃฏ1๏ฃบ , ๐ป2 = ๏ฃฏ ๐œŒ2 /๐œ– ๏ฃบ , ๐ถ2 = 1 0 0 , ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ 2 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃฏ0 0 0๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ0๏ฃบ ๏ฃฏ 3 ๏ฃฏ ๐œŒ3 /๐œ– ๏ฃบ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ ๐‘“ (๐’’ , ห†๐’† , ๐’’ยค ๐‘‘ ) ๏ฃบ ๏ฃฏ ๐‘€ (๐’’ ) โˆ’1 03ร—1 ๏ฃบ ๏ฃฏ๐‰ ๐‘ ๏ฃบ 1 1 ๐‘“ยฏ(๐’’ 1 , ห†๐’† , ๐’’ยค ๐‘‘ ) = ๏ฃฏ ๏ฃบ , ๐บยฏ (๐’’ 1 ) = ๏ฃฏ ๏ฃบ, ๐’–= ๏ฃฏ ๏ฃบ, ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ 0 ๏ฃฏ 01ร—3 1/๐ผ๐‘„ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ๐‘ข2 ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป where โŠ• denotes the matrix direct sum, ๐ป is designed by choosing ๐œŒ such that the polynomial ๐‘ 3 + ๐œŒ1 ๐‘ 2 + ๐œŒ2 ๐‘  + ๐œŒ3 , is Hurwitz and ๐œ– โˆˆ R 0 is a sufficiently small tuning parameter. 79 4.2.3 Output Feedback Control We now define an output feedback controller to ensure tracking of the desired trajectory, ๐’’ ๐‘‘ . The output feedback controller is designed by replacing the states and disturbances in the state feedback controller by their estimates from the extended high-gain observer ๐‰ห† ๐‘ = ๐‘€ (๐’’ 1 ) [โˆ’๐‘˜ 1 ห†๐’† 1 โˆ’ ๐‘˜ 2 ห†๐’† 2 โˆ’ ๐ˆห† ๐‘ž + ๐’’ยฅ ๐‘‘ โˆ’ ๐‘“ (๐’’ 1 , ห†๐’† , ๐’’ยค ๐‘‘ )]. This leads to the following output feedback control equations ๐‘ขห†3๐‘ = ๐œห†1๐‘ ๐‘ ๐›ผ + ๐œห†2๐‘ ๐‘  ๐›ผ + ๐œห†3๐‘ /๐ฟ, (4.11a) q ๐‘ขห†1๐‘ = ( ๐‘ขห†3๐‘ ๐‘ ๐›ผ โˆ’ ๐œห†1๐‘ ) 2 + ( ๐‘ขห†3๐‘ ๐‘  ๐›ผ โˆ’ ๐œห†2๐‘ ) 2 , (4.11b) ๐›ฝห†๐‘ = atan2(( ๐‘ขห†3๐‘ ๐‘ ๐›ผ โˆ’ ๐œห†1๐‘ ), ( ๐œห†2๐‘ โˆ’ ๐‘ขห†3๐‘ ๐‘  ๐›ผ )), (4.11c) ๐‘ขห†2 = ๐ผ๐‘„ (โˆ’๐‘˜ 3 ๐›ฝห†หœ1 โˆ’ ๐‘˜ 4 ๐›ฝห†หœ2 โˆ’ ๐œห†๐›ฝ ). (4.11d) The state and disturbance estimates must be saturated outside a compact set of interest to overcome the peaking phenomenon present in extended high-gain observers, see Appendix C. 4.3 Stability Analysis The stability of the state feedback control, observer estimates, and output feedback controller will now be proven. We begin by restricting the domain of operation by establishing a compact positively invariant set in which the system will operate. We then prove stability of the closed-loop system under state feedback, convergence of the observer estimates, and finally prove stability of the closed-loop system under output feedback. 4.3.1 Restricting Domain of Operation To ensure the angular position of the carrier UAV, ๐›ฝ1 , satisfies โˆ’๐œ‹/2 < ๐›ฝ1 < ๐œ‹/2, we restrict the domain of operation and make the following assumption. Assumption 4.2. The rotational reference signal ๐›ฝ ๐‘ remains in the set {|๐›ฝ ๐‘ | < ๐œ‹/2 โˆ’ ๐›ฟ}, where 0 < ๐›ฟ < ๐œ‹/2. It can now be shown that for sufficiently small initial tracking error, ๐œท(0), หœ the tracking error | ๐›ฝหœ1 (๐‘ก)| < ๐›ฟ for all ๐‘ก > 0. Together with Assumption 4.2, this ensures that |๐›ฝ1 | < ๐œ‹/2. 80 A Lyapunov function in the rotational error dynamics is taken as ๐‘‰๐›ฝหœ = ๐œทหœ > ๐‘ƒ หœ ๐œท,หœ where ๐‘ƒ ๐›ฝหœ ๐ด ๐›ฝหœ + ๐ด>๐›ฝหœ ๐‘ƒ ๐›ฝหœ = โˆ’๐ผ2 . (4.12) ๐›ฝ Since ๐ด ๐›ฝหœ is Hurwitz, a symmetric positive definite ๐‘ƒ ๐›ฝหœ as defined above exists, and 2 2 2 ๐œ†min (๐‘ƒ ๐›ฝหœ) ๐œท หœ โ‰ค ๐‘‰๐›ฝหœ โ‰ค ๐œ†max (๐‘ƒ ๐›ฝหœ) ๐œท หœ , หœ ๐‘‰ยค ๐›ฝหœ โ‰ค โˆ’ ๐œท . Define the positively invariant set ฮฉ๐›ฝหœ = {๐‘‰๐›ฝหœ โ‰ค ๐‘ ๐›ฝหœ}. Choosing the positive constant ๐‘ ๐›ฝหœ = ๐œ† min (๐‘ƒ ๐›ฝหœ)๐›ฟ2 implies that ๐œท(๐‘ก)หœ โ‰ค ๐›ฟ and hence | ๐›ฝหœ1 (๐‘ก)| < ๐›ฟ as required. A Lyapunov function for the platform tracking error can be taken as ๐‘‰๐’† = ๐’† > ๐‘ƒ๐’† ๐’†, where ๐‘ƒ๐’† ๐ด๐’† + ๐ด๐’†> ๐‘ƒ๐’† = โˆ’๐ผ6 . (4.13) Taking the derivative of (4.13) and considering the potential for tracking error in ๐œท yields ๐‘‰ยค๐’† = โˆ’ k๐’†k 2 + 2[0> > 3ร—1 , ๐’† ๐›ฝ ]๐‘ƒ๐’† ๐’†, (4.14) 2 โ‰ค โˆ’ k๐’†k + 2๐œ†max (๐‘ƒ๐’† ) ๐’† ๐›ฝ k๐’†k . Since ๐’† ๐›ฝ (๐‘ก, ๐’’ 1 , ๐›ฝหœ1 ) and its partial derivatives are continuous on ฮฉ๐›ฝหœ, and ๐’† ๐›ฝ is uniformly bounded in time, ๐’† ๐›ฝ is locally Lipschitz in ๐›ฝหœ1 on ฮฉ๐›ฝหœ. Hence, in ฮฉ๐›ฝหœ ๐’† ๐›ฝ (๐‘ก, ๐’’ 1 , ๐›ฝหœ1 ) โˆ’ ๐’† ๐›ฝ (๐‘ก, ๐’’ 1 , 0) โ‰ค ๐ฟ ๐›ฝหœ1 โ‰ค ๐ฟ๐›ฟ. (4.15) This enables us to bound the Lyapunov derivative (4.14) by ๐‘‰ยค๐’† โ‰ค โˆ’ k๐’†k 2 + 2๐œ† max (๐‘ƒ๐’† )๐ฟ๐›ฟ k๐’†k . (4.16) Therefore, for k๐’†k > 2๐œ†max (๐‘ƒ๐’† )๐ฟ๐›ฟ, ๐‘‰ยค๐’† < 0. Using the bounds ๐œ†min (๐‘ƒ๐’† ) k๐’†k 2 โ‰ค ๐‘‰๐’† โ‰ค ๐œ†max (๐‘ƒ๐’† ) k๐’†k 2 , we can ensure ฮฉ๐’† = {๐‘‰๐’† โ‰ค ๐‘ ๐’† } is a positively invariant set with the following choice of ๐‘ ๐’† ๐‘ ๐’† > ๐œ†max (๐‘ƒ๐’† ) (2๐œ† max (๐‘ƒ๐’† )๐ฟ๐›ฟ) 2 . (4.17) This leads to the domain of operation ฮฉ = ฮฉ๐›ฝหœ ร— ฮฉ๐’† which is compact and positively invariant under state feedback. 81 4.3.2 Stability under State Feedback Lemma 4.1 (Stability under State Feedback). For the closed-loop system under state feedback (4.8), with หœ initial tracking error (๐’†(0), ๐œท(0)) โˆˆ ฮฉ, the system states (๐’†(๐‘ก), ๐œท(๐‘ก)) หœ remain in ฮฉ for all ๐‘ก > 0 and exponentially converge to the origin. Proof. The closed-loop system (4.8) is a cascade system of the form ๐’†ยค 1 = ๐’† 2 , ๐’†ยค 2 = โˆ’๐‘˜ 1 ๐’† 1 โˆ’ ๐‘˜ 2 ๐’† 2 + ๐’† ๐›ฝ (๐‘ก, ๐’’ 1 , ๐›ฝหœ1 ), ๐›ฝยคฬƒ1 = ๐›ฝหœ2 , ๐›ฝยคฬƒ2 = โˆ’๐‘˜ 3 ๐›ฝหœ1 โˆ’ ๐‘˜ 4 ๐›ฝหœ2 . The Lyapunov functions for the platform tracking error (4.13) and the carrier UAV rotational tracking error (4.12) can be combined to form a composite Lyapunov function ๐‘‰ = ๐‘‘๐‘‰๐‘’ + ๐‘‰๐›ฝหœ, ๐‘‘ > 0. (4.18) Following the generalized proof for cascaded system stability in Appendix B, the closed-loop state feedback system converges exponentially to the origin and ฮฉ is compact and positively invariant for ๐‘‘ chosen small enough.  4.3.3 Stability under Output Feedback The system under output feedback is a singularly perturbed system which can be separated into two time-scales. The system dynamics and control reside in the slow time-scale while the observer resides in the fast time-scale. Lemma 4.2 (Convergence of EHGO Estimates). The observer estimates, ๐Œ ห† , converge to an ๐‘‚ (๐œ–) neighbor- hood of the true state, ๐Œ, for any ๐œ– โˆˆ (0, ๐œ– โˆ— ) for sufficiently small ๐œ– โˆ— > 0. Proof. The lemma follows from standard high-gain observer analysis [42].  Remark 4.1 (Identification of ๐œ– โˆ— ). The Lyapunov analysis in the proof of Lemma 4.2 provides an estimate of ๐œ– โˆ— , however this estimate is usually quite conservative. In practice, ๐œ– is chosen through empirical tuning on the system on which it will be implemented. 82 Remark 4.2. It follows from standard EHGO analysis that the estimation error enters an invariant set contained inside a ball of radius ๐œ– ๐‘ after some short time ๐‘‡ (๐œ–), where lim ๐œ– โ†’0 ๐‘‡ (๐œ–) = 0 and ๐‘ โˆˆ R 0 . Since the initial state resides on the interior of ฮฉ, choosing ๐œ– sufficiently small ensures that the states will not leave ฮฉ during the interval [0, ๐‘‡ (๐œ–)], thus the system state will remain inside the domain of operation under output feedback while the observer converges. Theorem 4.1 (Stability under Output Feedback). The closed-loop tracking error system under output feed- back, with initial conditions on the interior of ฮฉ, exponentially converges to an ๐‘‚ (๐œ–) neighborhood of the origin when ๐œ– is chosen small enough. Proof. The output feedback closed-loop system can be written in singularly perturbed form. The system dynamics form the reduced system and the observer dynamics form the boundary layer system. This system has a two time-scale structure as ๐œ– becomes small. The boundary layer system has an exponentially stable equilibrium point at the origin from Lemma 4.2. The reduced system also has an exponentially stable equilibrium point at the origin as shown in Lemma 4.1. Following [40, Theorem 11.4], it can be shown that the entire closed-loop output feedback system is exponentially stable when ๐œ– is chosen sufficiently small.  4.4 Numerical Simulation The estimation and control strategy presented above is simulated to perform trajectory tracking in the flat outputs, ๐’’ 1 , i.e, platform pose. A simple task is chosen where the platform is taken from an initial position and orientation, ๐’’ 1 = [0, 0, 0] > , to a final position and orientation, ๐’’ 1 = [1, 1, 1] > . The system then returns back to the initial configuration. As the controller requires continuous derivatives of the desired trajectory, ๐’’ ๐‘‘ , up to fifth order, the desired trajectory ๐’’ ๐‘‘ is taken as a 9-th order polynomial with given initial and final values: ๐’’ ๐‘‘ (0) = [0, 0, 0] > , and ๐’’ ๐‘‘ (5) = [1, 1, 1] > . The derivatives up to 5-th order are set to zero at ๐‘ก = 0 and ๐‘ก = 5. The return trajectory is generated in the same manner, but in the opposite direction. The simulation is conducted with the disturbances ๐ˆ ๐‘ž = [sin(2๐‘ก), sin(3๐‘ก), sin(๐‘ก)] > and ๐œŽ๐›ฝ = sin(4๐‘ก) applied to the system dynamics. The desired and actual trajectories under output feedback control are shown in Fig. 4.2. To showcase an application and aid in interpretation, we simulated the system tracking the same trajectory in the presence of the same disturbances and included a two-link manipulator attached to the birotor platform. We animated the simulation to show the birotor platform swinging up and grasping an 83 (a) Desired and actual trajectories: ๐‘ฅ (b) Desired and actual trajectories: ๐‘ฆ (c) Desired and actual trajectories: ๐›ผ (d) Desired and actual trajectories: ๐›ฝ Figure 4.2: Trajectory tracking for ๐‘ฅ, ๐‘ฆ, ๐›ผ (platform pose) and ๐›ฝ (UAV orientation) under output feedback control in the presence of disturbances. object with the two-link manipulator, then returning to the initial configuration. Frames from the animation are shown in Fig. 4.3. 4.5 Conclusions In this chapter, we designed a novel long reach aerial manipulation system in which a small horizontally actuated platform is suspended from a larger carrier UAV. The system dynamics are modeled and a feedback linearizing control is initially designed under state feedback. An extended high-gain observer is designed to estimate unmeasured states as well as any modeling error and external disturbances, which are then canceled in the output feedback control design. The method is rigorously analyzed to guarantee stability of the overall 84 2 2 1.5 1.5 1 1 0.5 0.5 0 0 -0.5 -0.5 -0.5 0 0.5 1 1.5 2 -0.5 0 0.5 1 1.5 2 (a) Initial configuration (b) Approach object 2 2 1.5 1.5 1 1 0.5 0.5 0 0 -0.5 -0.5 -0.5 0 0.5 1 1.5 2 -0.5 0 0.5 1 1.5 2 (c) Grasp object (d) Return 2 2 1.5 1.5 1 1 0.5 0.5 0 0 -0.5 -0.5 -0.5 0 0.5 1 1.5 2 -0.5 0 0.5 1 1.5 2 (e) Return (f) Arrive at final configuration Figure 4.3: Simulation tracking the same trajectory, showing a potential application using an attached two-link manipulator to grasp an object. 85 closed-loop system under output feedback. The system is simulated with added disturbances which are estimated and canceled through the control design to show excellent tracking performance. 86 CHAPTER 5 CONCLUSIONS AND FUTURE DIRECTIONS We presented a robust output feedback linearizing control strategy that utilizes extended high-gain observers for use in developing methods enabling resilient autonomy for multi-rotor systems. The base control technology is further extended to enable trajectory estimation, in-flight actuator failure detection, and implementation on a novel multi-body multi-rotor airframe design. The estimation and control methods are rigorously analyzed to provide stability guarantees and the provide insights into system tuning and the maximum control switching time to recover from an actuator failure. Furthermore, the methods presented are shown to not only work in theory and simulation, but they can be applied to experimental systems at reasonable sample rates with inexpensive sensors and computation. Chapter 2 demonstrated the base estimation and control strategy that is utilized throughout this work. A feedback linearizing controller, which alone would result in poor closed-loop performance, is augmented with an extended high-gain observer. The combination yields a system that is robust to external disturbances as well as modeling error, while still providing intuitive gain tuning to allow shaping of the transient response. The observer also provides estimates of any unmeasured system states for use in the output feedback control strategy. The overall system is rigorously analyzed and proven stable. The estimation and control strategy is also validated through simulation and experimental results where we focus on the problem of landing a multi-rotor on a moving ground vehicle. Furthermore, we present a novel method for designing an extended high-gain observer for a system evolving on ๐‘†๐‘‚ (3). The observer enables estimation of higher-order states, disturbances, and higher-order derivatives of the reference trajectory. In Chapter 3, we demonstrated that a family of extended high-gain observers can be utilized in an output feedback control strategy to incorporate the detection and identification of a complete actuator failure during flight on a hexrotor UAV. Our analysis provides insight into the magnitude of disturbances that can be affecting the system while still being able to detect and recover from an actuator failure. We are also able to provide a theoretical bound on the amount of time that can elapse before switching models to guarantee stability is recovered. The system is again validated through simulation and experimental results. Chapter 4 presented a novel multi-body multi-rotor airframe design to enable long reach aerial manip- 87 ulation. The feedback linearizing control strategy, augmented with an extended high-gain observer, is also applied on this novel airframe design. The proposed strategy is rigorously analyzed to provide a domain of operation in which the control law is valid, as well as guaranteeing stability of the overall closed-loop system under output feedback. The system is simulated and shown to result in excellent tracking performance even in the presence of disturbances. The work presented in this dissertation can be extended in a number of ways. In principle, the methods presented above result in a robust trajectory tracking control design. These controllers could be paired with a high-level control strategy which would be responsible for generating the reference trajectory. For example, the use of a reinforcement learning control strategy could be used to generate the reference trajectory. Furthermore, the disturbance estimates could be fed into the reinforcement learning strategy to influence the design of a more efficient reference trajectory in the presence of the expected disturbances. Our estimation and control strategy can also be extended to consider control optimality. The feedback linearizing control could be replaced with an optimal control strategy, such as model predictive control. Furthermore, the estimates of disturbance from the extended high-gain observer could be used to parameterize a disturbance model online for use in control design. When considering actuator failure, our method could be extended to multiple failures for general ๐‘›-rotors. Assuming that system controllability is retained despite multiple failures, the proposed approach could be applied in an hierarchical way, wherein a new set of models are considered after each failure detection. Analysis of such an approach under mutual interactions of multiple failures is an interesting direction of future investigation. Furthermore, the multi-body multi-rotor control design presented in Chapter 4 could be extended to three dimensional motion. One way to achieve this would be to connect the passive revolute joint to an actuated joint below the carrier UAV as opposed to connecting it directly to the carrier UAV. The actuated joint could be controlled to remain parallel to the inertial ๐‘ฅ, ๐‘ฆ plane enabling the carrier UAV to rotate as necessary for translation, while keeping the plane in which the suspended bi-rotor platform operates perpendicular to the inertial ๐‘ฅ, ๐‘ฆ plane. 88 APPENDICES 89 APPENDIX A EXPERIMENTAL SYSTEM DESIGN In this appendix, we will detail the methods necessary to take the control and estimation strategies proposed throughout this dissertation and implement them on a physical system. In Section A.1, we begin by showing the identification of the necessary physical parameters, and associated mappings, for the multi-rotor model. We also describe the gain tuning for the control, observer, and the failure identification threshold. In Section A.2, we identify the parameters and mappings associated with generating a desired force with a motor and rotor pair, including the dynamic response of the rotor speeds to a commanded input, which as we have shown must be included in the extended high-gain observer design. Finally, in Section A.3, we show the methods used to generate integrated flight software to run on the Pixhawk 4 flight management unit (FMU). The flight software is implemented in Simulinkยฎ and codegened through the PX4 Autopilots Support from Embedded Coder package [34]. A.1 Multi-rotor Parameter Identification and Tuning In this section, the necessary physical parameters that will be identified consist of the mass moment of inertia of the multi-rotor, the mass of the multi-rotor, and the mapping of the desired body-fixed torques, ๐‰, and the total thrust, ๐‘ข ๐‘“ , to rotor forces. We further detail tuning the control and estimation parameters to arrive at a system which exhibits excellent tracking performance even in the presence of large disturbances during flight. The tuning will be focused on control performance, as well as showcasing the trade-offs between measurement noise amplification and convergence rate of the observer in choosing the estimation gains. Tuning the threshold for actuator failure detection will also be detailed in this section. Through this tuning process, we will arrive at a set of real-world parameters which can be directly implemented on an experimental system. A.1.1 Moment of Inertia Measurement To measure the inertia matrix of our experimental platform we used the bifilar pendulum method [35]. This method involves suspending the multi-rotor by two cables that are attached at points equidistant from 90 (a) ๐‘ฅ-axis (b) ๐‘ฆ-axis (c) ๐‘ง-axis Figure A.1: Suspending the multi-rotor to measure the moment of inertia about each principle axis using the bifilar pendulum method. the center of mass and rotating the multi-rotor about the vertical axis as shown in Fig. A.1. Here gravity, ๐‘”, provides the restoring force and we will measure the period of oscillation, ๐œ. Taking ๐‘š โˆˆ R 0 as the mass of the multi-rotor, ๐‘Ÿ โˆˆ R 0 as the distance between the center of mass and the connection point of the cables, and ๐ฟ โˆˆ R 0 as the length of the cables suspending the multi-rotor, we use ๐‘š๐‘”๐‘Ÿ 2 ๐œ 2 ๐ฝ= , (A.1) 4๐œ‹ 2 ๐ฟ to compute the moment of inertia about the vertical axis. The multi-rotor must then be suspended in the other two principle orientations to determine the moment of inertia about each of the three body-fixed axes. For simplicity, we assume that the off-diagonal elements of the inertia matrix are negligible. The inertia matrix that was measured for our experimental hexrotor system is ๏ฃฎ ๏ฃน ๏ฃฏ0.0228 0 0 ๏ฃบ๏ฃบ ๏ฃฏ 2 ๏ฃฏ ๏ฃบ ๐ฝ = ๏ฃฏ๏ฃฏ 0 0.0241 0 ๏ฃบ๏ฃบ kg m . ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ 0 0 0.0446๏ฃบ ๏ฃฐ ๏ฃป 91 Figure A.2: Standard quadrotor, hexrotor, and octorotor configurations with rotation direction of each rotor. These multi-rotors are in the โ€˜Xโ€™ configuration, meaning that there are two rotors symmetric about the body-fixed ๐‘ฅ-axis at the front and rear of the airframe. A.1.2 Mapping Actuator Forces to Body Torques and Collective Thrust The control inputs that are designed through our output feedback linearizing control law are a vector of body-fixed torques, ๐‰, and the collective thrust, ๐‘ข ๐‘“ . In this section we will describe the mapping from these control signals to forces required of each actuator for the most common quadrotor, hexrotor, and octorotor configurations. This will enable the body-torques and collective thrust to be applied, as designed by the control law, to the physical system which can only produce forces with the actuators. We will begin by considering the quadrotor configuration shown in Fig. A.2. Let ๐‘Ÿ โˆˆ R 0 be the radius of the airframe, or the distance from the center of mass to the center of each actuator. Let ๐‘ โˆˆ R 0 be the quad quad quad rotational aerodynamic drag on the rotor, and finally, let ยฏ๐’‡ = [ ๐‘“ยฏ1 , . . . , ๐‘“ยฏ4 ] > be a vector of the forces applied by each actuator and define the following mapping matrix ๏ฃฏ1 1 1 1๏ฃบ ๏ฃฎ ๏ฃน ๏ฃฏ ๏ฃบ ๏ฃฏ โˆ’๐‘Ÿ โˆ’๐‘Ÿ ๏ฃบ ๏ฃฏโˆš โˆš โˆš๐‘Ÿ ๐‘Ÿ โˆš ๏ฃบ quad =๏ฃฏ 2 2 2 2๏ฃบ . (A.2) ๏ฃฏ ๐‘€ ๏ฃบ ๏ฃฏ โˆš๐‘Ÿ โˆ’๐‘Ÿ โˆš โˆ’๐‘Ÿ โˆš โˆš๐‘Ÿ ๏ฃบ ๏ฃบ ๏ฃฏ 2 2 2 2๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ๐‘ ๏ฃฐ โˆ’๐‘ ๐‘ โˆ’๐‘๏ฃบ๏ฃป The vector of control inputs can now be written in terms of the forces generated by each rotor through ๏ฃฎ ๏ฃน ๏ฃฏ๐‘ข ๐‘“ ๏ฃบ quad ๏ฃฏ ๏ฃบ = ๐‘€ quad ยฏ๐’‡ (A.3) ๏ฃฏ ๏ฃบ . ๏ฃฏ๐‰๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป 92 When it comes to the physical implementation of the control law, we need an inverse mapping so we can map the desired body-fixed torques and collective thrust from the control law to actuator forces. This can be solved for the quadrotor case by simply taking the inverse of the mapping matrix ๏ฃฎ ๏ฃน ๏ฃฏ๐‘ข ๐‘“ ๏ฃบ ยฏ๐’‡ quad quad โˆ’1 = (๐‘€ ) ๏ฃฏ ๏ฃบ. (A.4) ๏ฃฏ ๏ฃบ ๏ฃฏ๐‰๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป hex The mapping matrix for the hexrotor configuration shown in Fig. A.2 is very similar. Let ยฏ๐’‡ = [ ๐‘“ยฏ1hex , . . . , ๐‘“ยฏ6hex ] > be a vector of the forces applied by each actuator and define the following mapping matrix ๏ฃฏ 1 1 1 1 1 1 ๏ฃบ ๏ฃฎ ๏ฃน ๏ฃฏ ๏ฃบ ๏ฃฏ โˆ’๐‘Ÿ โˆ’๐‘Ÿ ๐‘Ÿ ๐‘Ÿ ๏ฃบ ๏ฃบ hex ๏ฃฏ ๏ฃฏ 2 โˆ’๐‘Ÿ 2 2 ๐‘Ÿ 2 ๏ฃบ ๐‘€ =๏ฃฏ โˆš โˆš โˆš โˆš ๏ฃบ. (A.5) ๏ฃฏ๐‘Ÿ 3 โˆ’๐‘Ÿ 3 โˆ’๐‘Ÿ 3 ๐‘Ÿ 3๏ฃบ ๏ฃฏ 2 0 2 2 0 2 ๏ฃบ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‘ ๏ฃฐ โˆ’๐‘ ๐‘ โˆ’๐‘ ๐‘ โˆ’๐‘ ๏ฃบ๏ฃป The vector of control inputs can now be written in terms of the forces generated by each rotor through ๏ฃฎ ๏ฃน ๏ฃฏ๐‘ข ๐‘“ ๏ฃบ hex ๏ฃฏ ๏ฃบ = ๐‘€ hex ยฏ๐’‡ . (A.6) ๏ฃฏ ๏ฃบ ๏ฃฏ๐‰๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป When it comes to the inverse mapping, since the mapping matrix, ๐‘€ hex , is not square, we will instead use the minimum energy pseudo-inverse, leading to the following mapping ๏ฃฎ ๏ฃน ๏ฃฏ๐‘ข ๏ฃบ ยฏ๐’‡ hex = (๐‘€ hex ) > ((๐‘€ hex ) (๐‘€ hex ) > ) โˆ’1 ๏ฃฏ๏ฃฏ ๐‘“ ๏ฃบ๏ฃบ . (A.7) ๏ฃฏ๐‰๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป oct The mapping matrix for the hexrotor configuration shown in Fig. A.2 is again very similar. Let ยฏ๐’‡ = [ ๐‘“ยฏ1oct , . . . , ๐‘“ยฏ8oct ] > be a vector of the forces applied by each actuator and define the following mapping matrix 1 1 1 1 1 1 1 1 ๏ฃฎ ๏ฃน ๏ฃฏ ๏ฃบ ๏ฃฏ โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš โˆš ๏ฃบ๏ฃบ ๏ฃฏ โˆ’๐‘Ÿ 2โˆ’ 2 โˆ’๐‘Ÿ 2+ 2 โˆ’๐‘Ÿ 2+ 2 โˆ’๐‘Ÿ 2โˆ’ 2 ๐‘Ÿ 2โˆ’ 2 ๐‘Ÿ 2+ 2 ๐‘Ÿ 2+ 2 ๐‘Ÿ 2โˆ’ 2 ๏ฃบ ๏ฃฏ 2 ๐‘€ oct =๏ฃฏ โˆš โˆš ๏ฃฏ โˆš2 โˆš โˆš2 โˆš โˆš2 โˆš โˆš2 โˆš โˆš2 โˆš โˆš2 โˆš โˆš 2 โˆš ๏ฃบ๏ฃบ . (A.8) ๏ฃฏ ๐‘Ÿ 2+ 2 ๐‘Ÿ 2โˆ’ 2 โˆ’๐‘Ÿ 2โˆ’ 2 โˆ’๐‘Ÿ 2+ 2 โˆ’๐‘Ÿ 2+ 2 โˆ’๐‘Ÿ 2โˆ’ 2 ๐‘Ÿ 2โˆ’ 2 ๐‘Ÿ 2+ 2 ๏ฃบ ๏ฃฏ ๏ฃฏ 2 2 2 2 2 2 2 2 ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ โˆ’๐‘ ๐‘ โˆ’๐‘ ๐‘ โˆ’๐‘ ๐‘ โˆ’๐‘ ๐‘ ๏ฃบ๏ฃป ๏ฃฐ Again, since the mapping matrix, ๐‘€ oct , is not square, we will use the minimum energy pseudo-inverse, leading to the following mapping ๏ฃฎ ๏ฃน ๏ฃฏ๐‘ข ๐‘“ ๏ฃบ ยฏ oct ๐’‡ = (๐‘€ oct ) > ((๐‘€ oct ) (๐‘€ oct ) > ) โˆ’1 ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ. (A.9) ๏ฃฏ๐‰๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป 93 Now that all of these mappings have been defined, the control actions can be mapped to actuator forces and back to ensure the control action can be applied to the experimental platform. In Section A.2, a detailed analysis of the rotor dynamics and thrust characteristics are presented. This will enable the final piece of the control application chain by detailing a mapping from desired actuator forces to the digital PWM signals that will be sent to the electronic speed controllers driving the actuators. Therefore, by applying the mappings presented in this section, and the mappings presented in Section A.2, we are able to map our desired body-fixed torques and collective thrust directly to actuator signals. A.1.3 Tuning Control Gains The control method that is employed throughout this work is a feedback linearizing control strategy. Typically, feedback linearization is not utilized in real-world control applications because this method relies on having exact model knowledge, knowledge of any disturbances effecting the system, and can be adversely affected by measurement noise. When we augment a feedback linearizing control strategy with an extended high-gain observer, we can relax these requirements as the observer can provide an estimate of any errors between the physical system and our mathematical model, including an estimate of any disturbances affecting the system. The rotational dynamics will behave as a linear system under the output feedback control presented in Chapter 2. Similarly, the translational dynamics will also behave as a linear system under the output feedback control, however with a slight perturbation resulting from any tracking error present in the rotational control. The rotational and translational control will also experience slight perturbations due to estimation error when utilizing the observer estimates in the output feedback control. In principle, since the closed-loop systems behave as linear systems, we can employ any linear system gain tuning approach to shape the transient response and ensure certain performance criteria. The rotational and translational closed-loop systems form a cascade connection as the translational control uses the reference signals of the rotational control as virtual control inputs. As a result, the translational control can only approach the behavior of the desired linear system since the closed-loop behavior is dependent on the tracking error of the rotational system. If the rotational system is tracking perfectly, then the translational system reduces to the desired linear system, however, in practice, perfect rotational tracking will not be achieved. 94 In order to achieve excellent closed-loop tracking performance, we want to reduce the magnitude of the rotational tracking error as much as possible. Since we are able to write the closed-loop system as a cascade connection between the rotational and translational subsystems, the control gains for the two systems can be tuned independently and in theory do not require the time-scale separation arguments that are typically made in this type of control structure. However, if the gains for both systems are chosen close to each other, the system will experience degraded tracking performance as the perturbations in the translational closed-loop system, resulting from the tracking error in the rotational closed-loop system, become large. For practical performance we choose the rotational gains decently higher than the translational gains which improves the overall closed-loop tracking performance of the system. The gains that were found to work well on our system consist of the rotational gains, ๐›ฝ1 = 40, ๐›ฝ2 = 20, and the translational gains, ๐›พ1 = 2, and ๐›พ2 = 1.5. A.1.4 Tuning the Extended High-Gain Observer and Actuator Failure Detection Threshold When tuning the gains for an extended high-gain observer for use in nonlinear feedback control, there are a few trade-offs that must be considered. The primary argument against using extended high-gain observers in practical applications, specifically on highly dynamic systems such as multi-rotor UAVs, is the effect of measurement noise and the high sample-rates required. Measurement noise can have a drastic effect on extended high-gain observer performance as we are relying on position measurements to estimate velocity and acceleration for use in output feedback control. This means that any measurement noise will be amplified dramatically which can result in severely degraded performance. An illustration on the effect of measurement noise on the ultimate bound on observer error and the appropriate choice of the observer gain ๐œ– is shown in [42, Figure 1.11]. On our system, the value ๐œ– = 0.025 was found to work well. When considering using the estimates from the extended high-gain observer to detect an actuator failure, as detailed in Chapter 3, there are some trade-offs that must be considered. The estimate of the Lyapunov derivative will be corrupted by measurement noise and will only be in an ๐‘‚ (๐œ–) neighborhood of the actual Lyapunov derivative. This is important when selecting ๐‘Ž 0 for our detection threshold through (0) ๐‘‰ยคฬ‚ ๐œ‰ โ‰ค ๐‘Ž 0 โˆ’ || ๐ƒห† || 2 , as choosing ๐‘Ž 0 too small will result in falsely detecting an actuator failure. Similarly, choosing ๐‘Ž 0 too large will result in longer delays before detecting the failure, which can impact stability. In practice, we found the 95 value ๐‘Ž 0 = 2 to work well. A.2 Rotor Speed and Force Calibration The way in which we impart the control forces and torques prescribed by the output feedback linearizing control law is through varying the speed of the rotors on the multi-rotor vehicle. Without taking other small aerodynamic effects into consideration, the force generated by a rotor is directly proportional to the square of the rotational rate of the rotor. For a generic ๐‘›-rotor, the force generated by rotor ๐‘– โˆˆ {1, . . . , ๐‘›} is ๐‘“ยฏ๐‘– = ๐‘๐œ”2๐‘– , where ๐‘ โˆˆ R 0 is a constant relating angular rate to force and ๐œ”๐‘– โˆˆ R 0 is the ๐‘–-th rotor angular rate. The actuators used on the majority of multi-rotors are brushless DC (BLDC) motors. These motors require an electronic speed controller (ESC) to generate a rotating magnetic field. The ESC has an internal PI controller to regulate the speed of the rotating magnetic field, and in turn the speed of the actuator and rotor. The control signal that a standard ESC expects is a standard servo signal. This signal consists of a PWM signal which has a frequency of 50Hz with a pulse width between 1000๐œ‡๐‘  and 2000๐œ‡๐‘ , corresponding to no rotation and maximum rotational rate, respectively. In practice, most modern ESCs allow the signal frequency to be increased to increase responsiveness of the actuators so long as the total pulse width can be maintained with a small low signal in between. This places the operating range of the PWM signals for these ESCs between 50Hz and 400Hz, as shown in Fig. A.3. Other even faster ESC communication protocols exist, such as DSHOT 600 and DSHOT 1200, which can further improve the responsiveness of the actuators, however we did not consider these protocols because we run all of our experiments with a sample rate of 100Hz. Ultimately, we will need a mapping between the servo signal command and the rotational speed of the rotor. This mapping will be discussed in detail in Section A.2.3. A.2.1 Photo-tachometer Actuator Test Stand In order to measure the rotational rate of the rotors for a given commanded input, we constructed a test stand that uses an infrared photo diode and an infrared LED. The photo diode is placed in the top arm of the test stand shown in Fig. A.4 aiming down so as to limit interference from external sources. The infrared LED resides in the bottom arm pointing up forming a beam of infrared light, which when interrupted by the rotor passing between the arms, can be detected using a microcontroller. The rotor is run at a range of speeds 96 1 0.5 0 -0.5 -1 0 0.01 0.02 0.03 0.04 0.05 1 0.5 0 -0.5 -1 0 0.01 0.02 0.03 0.04 0.05 Figure A.3: Examples of the two extremes for the PWM signals that can be sent to an ESC. Both signals are applying the maximum pulse width of 2000๐œ‡๐‘ , corresponding to maximum throttle for the ESC. and as it interrupts the infrared light, the time at which that occurs is recorded, which is used to resolve a rotational speed of the rotor. Taking these speed measurements enables both the calibration of the relation between rotor angular rate and the PWM speed command sent to the ESC, as well as dynamic information used to model the response of the actuators, which is shown in the next section. To characterize the rotor thrust for a given speed we place the rotor speed test stand on a load cell, flip the rotor over to generate downward force, and run the rotor at a range of speeds. The force measurement data and the rotor speed data can then be used to generate a functional mapping between rotor speed and force. These mappings will be shown in detail in Section A.2.3. A.2.2 Actuator Dynamic Response The actuators typically used in multi-rotor construction are brushless DC (BLDC) motors which require electronic speed controllers (ESCs). The ESCs have internal PI controllers which introduce dynamic delays, resulting in a first-order response of the rotor speed to a commanded input. The first order model is written 97 Figure A.4: Photo tachometer rotor speed test stand. as 1 ยค = ๐Ž (๐Ždes โˆ’ ๐Ž), (A.10) ๐œ๐‘š where ๐Ž โˆˆ R๐‘› is a vector of rotor angular rates, ๐Ždes โˆˆ R๐‘› is a vector of desired rotor angular rates, and the associated time constant of the system is ๐œ๐‘š โˆˆ R 0 . Through experimental testing and fitting a model as shown in Fig. A.5, the time constant is found to be ๐œ๐‘š = 0.059. The experimental system which was tested consisted of a 4S Lithium polymer battery, Nidici 35A ESCs running BLHeli_32 firmware, 2212 920Kv brushless motors, and 10x4.5 carbon fiber rotors. The rotor speed is measured using a custom built photo tachometer rotor test stand, shown in Fig. A.4. Even though this is an open loop model, any errors in rotor speed can be pulled out of the dynamics as an additive disturbance and then compensated for with the output feedback control. Recall the rotational tracking error dynamics with squared rotor speed input ๐ƒยค 1 = ๐ƒ 2 , (A.11) ๐ƒยค 2 = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยค ๐‘Ÿ ) + ๐‘๐บ (๐‘1 ) ๐‘€๐Ž ๐‘  + ๐‡ ๐œ‰ . 98 800 750 700 650 600 550 500 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6 2.8 3 Figure A.5: Actuator dynamic response. We can modify the actuator dynamic model, where the actual speed of each rotor has some error, ๐œน = [๐›ฟ1 , . . . , ๐›ฟ ๐‘› ] > , where ๐›ฟ๐‘– is the error between the commanded and actual angular rate of rotor ๐‘–, as compared with the commanded rotor speed as 1 ๐Žยคฬ„ = (๐Ždes โˆ’ (๐Ž + ๐œน)). (A.12) ๐œ๐‘š Taking ๐Ž ยฏ ๐‘  = [๐œ” ยฏ 21 , . . . , ๐œ”ยฏ 2๐‘› ] > as the input to the multi-rotor dynamics results in the following dynamics ๐ƒยค 1 = ๐ƒ 2 , (A.13) ๐ƒยค 2 = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยค ๐‘Ÿ ) + ๐‘๐บ (๐‘1 ) ๐‘€ ๐Ž ยฏ๐‘  + ๐‡๐œ‰. The error, ๐œน, can be pulled out of the dynamics as an additive term and lumped in with the rotational disturbance as ๐ƒยค 1 = ๐ƒ 2 , (A.14) ๐ƒยค 2 = ๐‘“ (๐ƒ, ๐‘1 , ๐‘ยค ๐‘Ÿ ) + ๐‘๐บ (๐‘1 ) ๐‘€๐Ž ๐‘  + ๐‡ยฏ ๐œ‰ , where ๐‡ยฏ ๐œ‰ = ๐‘๐บ (๐‘1 )๐‘€ (2๐Ž๐œน + ๐œน ๐‘  ) + ๐‡ ๐œ‰ , and ๐œน ๐‘  = [๐›ฟ12 , . . . , ๐›ฟ2๐‘› ] > . 99 The additional disturbance can then be estimated by the extended high-gain observer, and canceled through the output feedback control. A.2.3 Reversible Rotors and Their Effect on Performance As established in Chapter 3, in order to recover stability after an actuator failure on a standard hexrotor UAV, the rotors need to be able to rotate in both the forward and reverse directions. However, the airfoil design of the rotors that are used on multi-rotors are asymmetric to improve their efficiency in the direction of rotation of normal operation. As a result, when a rotor is commanded to run in reverse during a failure, the performance is drastically reduced, requiring much larger rotor speeds to achieve the same force as would be applied in the nominal direction. For the 10x4.5 carbon fiber rotors used on our experimental vehicle, this deficit was found to require five times the rotor rotational rate to generate the same force in the reverse direction. To enable bi-directional operation, the firmware on the ESCs is updated. As a result, the input range is still a pulse between 1000๐œ‡๐‘  and 2000๐œ‡๐‘ , however this now corresponds to full speed in reverse and full speed in the forward direction, respectively. Therefore, 1500๐œ‡๐‘  becomes the new zero throttle point. 1900 1800 1700 1600 1500 0 100 200 300 400 500 600 700 1500 1400 1300 1200 1100 -800 -700 -600 -500 -400 -300 -200 -100 0 Figure A.6: Mapping commanded ESC Pulse Width Modulation (PWM) to measured rotor speed in forward and reverse directions. 100 10 8 6 4 2 0 200 250 300 350 400 450 500 550 600 650 700 0 -0.5 -1 -1.5 -2 -800 -700 -600 -500 -400 -300 -200 Figure A.7: Mapping measured rotor speed to force in forward and reverse directions for carbon fiber 10x4.5 rotors, which are designed for single direction use and generate reduced thrust in reverse by a factor of five. The rotor test stand (Fig. A.4) is used to measure the angular rate at a number of speed settings to arrive at a mapping from desired rotor speed in radians per second to a necessary PWM signal value in microseconds. The following mappings were found for forward and reverse speeds respectively ๏ฃฑ ๏ฃฒ (3.305e-4)๐œ”2๐‘— + 0.218๐œ” ๐‘— + 1528 ๏ฃด ๏ฃด ๏ฃด ๏ฃด for ๐œ” ๐‘— โ‰ฅ 0, PWM ๐‘— = (A.15) ๏ฃด ๏ฃด (โˆ’2.457e-4)๐œ”2๐‘— + 0.267๐œ” ๐‘— + 1474 for ๐œ” ๐‘— < 0, ๏ฃด ๏ฃด ๏ฃณ for ๐‘— โˆˆ {1, . . . , 6}, with the measured and fitted data shown in Fig. A.6. We can extend the previous relation between rotor thrust force and rotor angular rate to incorporate the difference in efficiency in the forward and reverse directions. We define a pair of thrust coefficients, ๐‘ + โˆˆ R 0 for normal operation and ๐‘ โˆ’ โˆˆ R 0 for reverse operation. These coefficients relate rotor speed, ๐œ” โˆˆ R, to force, ๐‘“ยฏ โˆˆ R, as ๏ฃฑ ๏ฃฒ ๐‘ + ๐œ”2๐‘— , ๏ฃด ๏ฃด ๏ฃด ๏ฃด for ๐œ” ๐‘— โ‰ฅ 0, ๐‘“ยฏ๐‘— = (A.16) ๏ฃด ๏ฃด โˆ’๐‘ โˆ’ ๐œ”2๐‘— , for ๐œ” ๐‘— < 0, ๏ฃด ๏ฃด ๏ฃณ 101 400 350 300 250 200 150 100 50 2.5 3 3.5 4 4.5 5 5.5 Figure A.8: Time required to stop and reverse rotor direction with the magnitude of rotor speed shown to aid in estimating time. A complete reversal takes approximately 450ms. for ๐‘— โˆˆ {1, . . . , 6}. In order to calibrate this mapping for our experimental system, we need to utilize the data from our rotor test stand where we measure the steady-state force generated at a range of rotor speeds. The coefficients were found to be ๐‘ + = 1.8182e-5, and ๐‘ โˆ’ = 3.6364e-6, and the measured and fitted data are shown in Fig. A.7. Additionally, in order to reverse directions, a longer dynamic delay is present, as shown in Fig. A.8. As discussed in Chapter 3, the delay to change directions on the rotors is too long to change directions multiple times during flight. The ESC has to slow the actuator down, and cause a smooth transition between forward and reverse directions so as to keep the actuator magnetically locked to the rotating magnetic field. If the motor de-synchronizes from the magnetic field, the motor will stop and have to be restarted, resulting in extremely long delays. A.3 Flight Software Implementation There are several layers to our software implementation to ensure timely communication between all 102 Figure A.9: PX4 Serial Read Simulinkยฎ block, used to read from serial ports on the Pixhawk 4 FMU hardware. Specifically in this case, devttyS1 corresponds to the port labeled TELEM1 on the FMU. systems including: the Vicon motion capture server, a local instance of Matlabยฎ running to parse the Vicon position estimates, wireless transmission of that data to a Raspberry Pi runnning on the hexrotor, and finally serial communication to transmit the position information to the flight controller. A.3.1 Position Estimation and Transmission As stated above, the hexrotor receives position estimates from a Vicon motion capture system. This is not necessary in general to apply our control methods as we have shown [9], however, since we are operating in a GPS denied environment, we must rely on the motion capture system for position measurements. To route the position information to the hexrotor, there are a few steps we go through. First, the Vicon server is accessed from a desktop compute through the supplied ViconSDK in Matlabยฎ , and the position values are read in to the running Matlabยฎ script. Next, the three position values are sent through a UDP connection on a local WiFi network to a Raspberry Pi Zero W on the hexrotor. Then the Raspberry Pi reads the position data from the network, computes a checksum for the data, and sends it out over a serial connection to the Pixhawk which is operating at a baud rate of 115200bits/s. Finally, the serial data is read by the Pixhawk within our Simulinkยฎ code through the dedicated PX4 Serial Read block shown in Fig. A.9. The Data output of this block is a vector of all bytes received in the serial buffer since the last loop iteration. A.3.2 Hexrotor On-board Data Processing and Control Implementation The estimation and control strategy is implemented in discrete time and simulated in Matlabยฎ for all of the simulation results presented in this dissertation. In order to run the control strategy on the Pixhawk 4 flight management unit (FMU), the standard PX4 firmware will be modified to replace the standard flight controller with the one we have designed. This is done through use of the use of the Simulinkยฎ package 103 Figure A.10: Pixhawk 4 FMU, Raspberry Pi, GPS receiver, RC receiver, and Vicon markers for motion tracking shown mounted on the hexrotor airframe. 104 Embedded Coderยฎ Support Package for PX4ยฎ Autopilots. The FMU hardware, along with the Raspberry Pi, GPS receiver, and RC receiver are shown mounted on the hexrotor airframe in Fig. A.10. The Embedded Coderยฎ Support Package for PX4ยฎ Autopilots makes implementing custom control software on the FMU relatively simple. Since the Simulinkยฎ code is codegened and integrated with the existing PX4 firmware, we still have access to all of the on-board measurements and state estimates, as well as dedicated Simulinkยฎ blocks to interact with the FMU hardware directly. We will begin by discussing pulling data from the PX4 firmware into our Simulinkยฎ code. The Simulinkยฎ block that is used to interact with all state busses on the Pixhawk is the PX4 uORB Read block. In the PX4 firmware, uORB is a messaging API which allows us to access all messages running on the FMU. We utilize this block to access the estimate of the vehicle attitude as well as read the signals coming in on from the RC receiver, as shown in Fig. A.11. The Msg output of these blocks contains a bus signal which can be broken out into its principle channels using a Bus Selector block. To change which uORB message is being accessed, the block can be opened and the user is provided with a list of messages that are accessible. In order to quickly implement our estimation and control strategy, we utilize the Matlabยฎ function block in Simulinkยฎ . For example, Fig. A.12 shows a function block in which we compute the estimates from our translational extended high-gain observer, as well as our rotor speeds, omegaOut, through the first order actuator dynamics. The inputs to the function are vectors that are being pulled from busses which are written to by other blocks in our system. Bus signals can be used to lump data together into a single signal wire to simplify the code layout. As in Fig. A.12, we grouped our signals into busses with logical names to facility code organization. Furthermore, we use memory blocks to feed the outputs of our function block, omegaOut and rhoHatOut, back into the function for use in the next loop iteration. Finally, the outputs are written back into their specified busses as shown with the small bus write blocks shown on the right hand side of the figure. We will now discuss how data is written to different portions of the FMU hardware, primarily writing back to a uORB message, writing telemetry data to the on-board SD card, and sending PWM signals out to the ESCs. An example of writing data to a uORB message is shown in Fig. A.13. In this example, we access the tune_control message to play an audible tune over the piezo speaker on the FMU. This is useful for providing user feedback. In our implementation, we had this tune play whenever a new set of parameters were loaded and verified prior to takeoff to ensure all data was received correctly. 105 Figure A.11: Simulinkยฎ uORB Read blocks. Used to access the vehicle_attitude bus and rc_channels bus on the uORB message system in PX4 firmware. Figure A.12: Example implementation of a Matlabยฎ function running within Simulinkยฎ . Data is routed in and out through bus signals. 106 Figure A.13: Writing desired information to the uORB Bus in the PX4 firmware. This example will play a tune over the piezo speaker on the FMU, which is useful to the operator for audible feedback. In order to record the telemetry data from each flight we use the PX4 SD Card Logger block, shown in Fig. A.14. This block accepts a vector of datatype single in the input port labeled data. The file name and path can be updated by double clicking on the block. One important aspect of using this block is the en (or enable) input. This input must be set to 1 to begin logging data, and must be returned to zero to close the file and save the data at the end of the flight, before shutting down the FMU. Through our implementation, we were able to log 67 channels of telemetry during flight at 100Hz. Finally, we come to enacting our control action in the form of commanded rotor PWM signals to be sent to the ESCs, calculated using the mappings shown in (A.15) and (A.16). This is done through the PX4 PWM block, shown in Fig. A.15. The channel numbers correspond to the hardware pin number on the FMU, and each signal should be of datatype uint16, with values in microseconds from 1000 to 2000. The information presented in this appendix will provide the base knowledge and building blocks to implement advanced control systems on an experimental multi-rotor. We presented the tools necessary to measure all necessary physical parameters as well as defining all mappings necessary to apply the control torques and collective thrust through a set of rotors. We further detail the characterization of the actuator dynamics and their thrust and speed relations. The topography of the system is laid out and the necessary 107 Figure A.14: Block for logging telemetry data to the on-board SD card on the FMU. User defined file name and path and the enable input must be 1 to log data, and transition to 0 to close the file and save at the end of flight. Figure A.15: Block for writing PWM signals to the ESCs. User specified number of channels and the datatype must be uint16. software tools for implementing these control methods on a Pixhawk 4 FMU using Simulinkยฎ were shown. 108 APPENDIX B STABILITY OF A GENERALIZED CASCADE SYSTEM A generalized stability proof for cascade systems is adapted from [41, Appendix C.1]. Consider the cascade connection of two systems ๐œ‚ยค = ๐‘“1 (๐‘ก, ๐œ‚, ๐œ‰), ๐œ‰ยค = ๐‘“2 (๐œ‰), (B.1) where ๐‘“1 and ๐‘“2 are locally Lipschitz and ๐‘“1 (๐‘ก, 0, 0) = 0, ๐‘“2 (0) = 0. Assuming the origin of ๐œ‰ยค = ๐‘“2 (๐œ‰) is exponentially stable, there is a continuously differentiable Lyapunov function, ๐‘‰2 (๐œ‰), that satisfies the following inequalities ๐‘ 1 k๐œ‰ k 2 โ‰ค ๐‘‰2 (๐œ‰) โ‰ค ๐‘ 2 k๐œ‰ k 2 , (B.2a) ๐œ•๐‘‰2 (๐œ‰) ๐‘“2 (๐œ‰) โ‰ค โˆ’๐‘ 3 k๐œ‰ k 2 , (B.2b) ๐œ•๐œ‰ ๐œ•๐‘‰2 (๐œ‰) โ‰ค ๐‘ 4 k๐œ‰ k , (B.2c) ๐œ•๐œ‰ over the set ฮฉ2 = {๐‘‰2 (๐œ‰) < ๐‘ 5 } for some ๐‘ 5 โˆˆ R 0 . Now, suppose there is a continuously differentiable Lyapunov function, ๐‘‰1 (๐œ‚), that satisfies the inequal- ities ๐œ•๐‘‰1 (๐œ‚) ๐œ•๐‘‰1 (๐œ‚) ๐‘“1 (๐‘ก, ๐œ‚, 0) โ‰ค โˆ’๐‘ k๐œ‚k 2 , โ‰ค ๐‘˜ k๐œ‚k , (B.3) ๐œ•๐œ‚ ๐œ•๐œ‚ over the set ฮฉ1 = {๐‘‰1 (๐œ‚) < ๐‘ 6 } for some ๐‘ 6 โˆˆ R 0 . Take a composite Lyapunov function for the cascaded system as ๐‘‰ (๐œ‚, ๐œ‰) = ๐‘๐‘‰1 (๐œ‚) + ๐‘‰2 (๐œ‰), ๐‘ > 0, (B.4) in which ๐‘ can be arbitrarily chosen. The derivative, ๐‘‰, ยค satisfies ๐œ•๐‘‰1 (๐œ‚) ๐œ•๐‘‰1 (๐œ‚) ๐œ•๐‘‰2 (๐œ‰) ๐‘‰ยค (๐œ‚, ๐œ‰) = ๐‘ ๐‘“1 (๐‘ก, ๐œ‚, 0) + ๐‘ [ ๐‘“1 (๐‘ก, ๐œ‚, ๐œ‰) โˆ’ ๐‘“1 (๐‘ก, ๐œ‚, 0)] + ๐‘“2 (๐œ‰), ๐œ•๐œ‚ ๐œ•๐œ‚ ๐œ•๐œ‰ ๐‘‰ยค (๐œ‚, ๐œ‰) โ‰ค โˆ’๐‘๐‘ k๐œ‚k 2 + ๐‘๐‘˜ ๐ฟ k๐œ‚k k๐œ‰ k โˆ’ ๐‘ 3 k๐œ‰ k 2 , where ๐‘“1 is Lipschitz in ๐œ‰ on ฮฉ2 , and ๐ฟ is the associated Lipschitz constant. 109 The inequality can be written in quadratic form as ๏ฃฎ ๏ฃน> ๏ฃฎ ๏ฃน๏ฃฎ โˆ’๐‘๐‘˜ ๐ฟ ๏ฃบ ๏ฃฏ ๏ฃน ๏ฃฏ k๐œ‚k ๏ฃบ ๏ฃฏ ๐‘๐‘ k๐œ‚k ๏ฃบ๏ฃบ 2 ๐‘‰ยค (๐œ‚, ๐œ‰) โ‰ค โˆ’ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ๏ฃฏ ๏ฃบ, ๏ฃฏ k๐œ‰ k ๏ฃบ ๏ฃฏ โˆ’๐‘๐‘˜ ๐ฟ k๐œ‰ k ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ๏ฃฏ ๏ฃบ ๐‘ 3 ๏ฃป ๏ฃฐ 2 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป๏ฃฐ ๏ฃป ๏ฃฎ ๏ฃน> ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน 2 ๏ฃฏ k๐œ‚k ๏ฃบ ๏ฃฏ k๐œ‚k ๏ฃบ ๏ฃฏ k๐œ‚k ๏ฃบ =โˆ’๏ฃฏ ๏ฃบ โ‰ค โˆ’๐œ†min (๐‘„) ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃบ ๐‘„๏ฃฏ ๏ฃบ , ๏ฃฏ k๐œ‰ k ๏ฃบ ๏ฃฏ k๐œ‰ k ๏ฃบ ๏ฃฏ k๐œ‰ k ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป where ๐‘ is chosen such that ๐‘ < 4๐‘๐‘ 3 /(๐‘˜ ๐ฟ) 2 to ensure ๐‘„ is positive definite. The foregoing analysis shows that the origin of (B.1) is exponentially stable on the set ฮฉ = ฮฉ1 ร— ฮฉ2 . 110 APPENDIX C PEAKING PHENOMENON For a system of order ๐œš the EHGO estimation error ๐œ’หœ๐‘– = ๐œ’๐‘– โˆ’ ๐œ’ห†๐‘– , with the observer gain, ๐œ–, and initial estimation error ๐Œ หœ (0), can be bounded by ๐‘ | ๐œ’หœ๐‘– | โ‰ค k ๐Œ(0)k หœ ๐‘’ โˆ’๐‘Ž๐‘ก/๐œ– , (C.1) ๐œ– ๐œšโˆ’1 for some positive constants ๐‘Ž and ๐‘, by [42, Theorem 2.1]. Initially, the estimation error can be very large, i.e., ๐‘‚ (1/๐œ– ๐œšโˆ’1 ), but will decay rapidly. To prevent the peaking of the estimates from entering the plant during the initial transient, the output feedback controller needs to be saturated. This is done by saturating the individual estimates outside a compact set of interest using the following saturation function ๏ฃฑ if |๐‘ฆ| โ‰ค 1,   ๏ฃด ๐œ’ห†๐‘– ๏ฃฒ ๐‘ฆ, ๏ฃด ๏ฃด ๐œ’ห†๐‘–๐‘  = ๐‘˜ ๐œ’๐‘– sat , sat(๐‘ฆ) = (C.2) ๐‘˜ ๐œ’๐‘– if |๐‘ฆ| > 1, ๏ฃด sign(๐‘ฆ), ๏ฃด ๏ฃด ๏ฃณ where ๐‘˜ ๐œ’๐‘– is chosen to ensure the saturation is not active during normal state feedback operation. Taking a Lypaunov function for the observer error, ๐‘‰๐œ’หœ , there is some set {๐‘‰๐œ’หœ โ‰ค ๐œ– 2 ๐‘} for some ๐‘ โˆˆ R 0 that the estimation error will enter after some short time, ๐‘‡ (๐œ–), where lim ๐œ– โ†’0 ๐‘‡ (๐œ–) = 0. Suppose for some system state, ๐’’ โˆˆ R๐‘› , the initial state, ๐’’(0), resides on the interior of a compact set of interest, ฮฉ. Choosing the observer gain, ๐œ–, small enough will ensure that the system state, ๐’’(๐‘ก), will not leave ฮฉ during the interval [0, ๐‘‡ (๐œ–)]. This establishes the boundedness of all states. 111 APPENDIX D ROTATION MATRIX PROJECTIONS BACK TO ๐‘†๐‘‚ (3) Due to numerical precision errors during computation, we must ensure the rotation matrix remains on ๐‘†๐‘‚ (3) by ensuring the following conditions are satisfied (i) ๐’“ 11 ยท ๐’“ 12 = ๐’“ 12 ยท ๐’“ 13 = ๐’“ 11 ยท ๐’“ 13 = 0, (ii) ๐’“ 11 ยท (๐’“ 12 ร— ๐’“ 13 ) = 1, (D.1) (iii) ๐’“ 2๐‘– โŠฅ ๐’“ 1๐‘– , for ๐‘– โˆˆ {1, 2, 3}. We utilize the following projection to ensure conditions (i) and (ii) are met ๐‘…ยฏ = exp(skew(log(๐‘…))), (D.2) where ๐‘…ยฏ is the rotation matrix projected back onto ๐‘†๐‘‚ (3). Further computing ยฏ๐’“ 21 = ๐’“ 21 โˆ’ (๐’“ 21 ยท ๐’“ 11 ) ยท ๐’“ 11 , ยฏ๐’“ 22 = ๐’“ 22 โˆ’ (๐’“ 22 ยท ๐’“ 12 ) ยท ๐’“ 12 , (D.3) ยฏ๐’“ 23 = ๐’“ 23 โˆ’ (๐’“ 23 ยท ๐’“ 13 ) ยท ๐’“ 13 , and taking ๐‘…ยค = [ยฏ๐’“ 21 ยฏ๐’“ 22 ยฏ๐’“ 23 ] ensures that (iii) is met. 112 APPENDIX E CONTROLLABILITY OF HEXROTOR CONFIGURATION AFTER ACTUATOR FAILURE For the purposes of this work, we will examine the controllability of the standard hexrotor configuration shown in Fig. E.1. The body torques, ๐‰, and total thrust, ๐‘ข ๐‘“ , are generated by applying forces with each rotor. These forces are mapped through a matrix to convert individual motor forces to torques and total thrust based on the hexrotor geometry ๏ฃฏ 1 1 1 1 1 1 ๏ฃบ ๏ฃฎ ๏ฃน ๏ฃฏ ๏ฃบ ๏ฃฏ โˆ’๐‘Ÿ โˆ’๐‘Ÿ ๐‘Ÿ ๐‘Ÿ ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃฏ 2 โˆ’๐‘Ÿ 2 2 ๐‘Ÿ 2 ๏ฃบ ๐‘€=๏ฃฏ โˆš โˆš โˆš โˆš ๏ฃบ, (E.1) ๏ฃฏ๐‘Ÿ 3 โˆ’๐‘Ÿ 3 โˆ’๐‘Ÿ 3 ๐‘Ÿ 3๏ฃบ ๏ฃฏ 2 0 2 2 0 2 ๏ฃบ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‘ ๏ฃฐ โˆ’๐‘ ๐‘ โˆ’๐‘ ๐‘ โˆ’๐‘ ๏ฃบ๏ฃป where ๐‘Ÿ โˆˆ R 0 is the distance from the center of mass to each rotor and ๐‘ โˆˆ R 0 is the rotational aerodynamic drag coefficient of each rotor. If we look at the case of a failure of actuator four, and consider simply turning off actuator one and continuing operation as a quadrotor, as shown in Fig. E.2, the system will not be controllable. This is because of the specific rotor rotational directions. Notice that the rotor rotational Figure E.1: Standard body configuration of a hexrotor. 113 directions for the hexrotor with actuators four and one disabled are not the same as the rotor rotational directions of the standard quadrotor (see Fig. A.2). The loss of controllability can be seen if the system is linearized about the hover equilibrium as ๐‘ยค = ๐ด๐‘ + ๐ต๐‰, (E.2) where ๏ฃฎ ๏ฃน ๏ฃฎ ๏ฃน ๏ฃฏ03 ๐ผ3 ๏ฃบ ๏ฃฏ 03 ๏ฃบ ๐ด=๏ฃฏ ๐ต= ๏ฃฏ ๏ฃบ, (E.3) ๏ฃฏ ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃบ, ๏ฃฏ โˆ’1 ๏ฃบ ๏ฃฏ03 03 ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ๐ฝ ๏ฃบ ๏ฃฐ ๏ฃป ๏ฃฐ ๏ฃป and ๐‘ = [๐œ™ ๐œƒ ๐œ“ ๐œ™ยค ๐œƒยค ๐œ“]ยค > is a vector of the Euler angles and Euler angular rates. Further mapping the squared rotor speeds to the body torque, ๐‰, through ๏ฃฎ ๏ฃน ๏ฃฏ๐‘ข ๐‘“ ๏ฃบ > ๐Ž ๐‘  = ๐œ”21 , . . . , ๐œ”2๐‘› ,  ๏ฃฏ ๏ฃบ = ๐‘๐‘€๐Ž ๐‘  , (E.4) ๏ฃฏ ๏ฃบ ๏ฃฏ๐‰๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฐ ๏ฃป allows us to investigate controllability under the loss of any actuator by modifying ๐‘€. To investigate the case shown in Fig. E.2, where actuator four has failed and we simply shut down actuator one, we zero out the corresponding columns of ๐‘€ as ๏ฃฏ0 1 1 0 1 1 ๏ฃบ ๏ฃฎ ๏ฃน ๏ฃฏ ๏ฃบ ๏ฃฏ โˆ’๐‘Ÿ ๐‘Ÿ ๏ฃบ ๏ฃบ ๏ฃฏ0 โˆ’๐‘Ÿ 0 ๐‘Ÿ 2 2 ๏ฃบ ๐‘€=๏ฃฏ (E.5) ๏ฃฏ โˆš โˆš ๏ฃบ. โˆ’๐‘Ÿ 3 ๐‘Ÿ 3๏ฃบ ๏ฃฏ0 0 0 0 ๏ฃฏ ๏ฃฏ 2 2 ๏ฃบ๏ฃบ ๏ฃฏ ๏ฃบ ๏ฃฏ0 โˆ’๐‘ ๐‘ 0 ๐‘ โˆ’๐‘ ๏ฃบ๏ฃป ๏ฃฐ The controllability matrix is computed for this case and is not full rank, showing that this configuration will not result in a controllable system, and is therefore infeasible for recovery. Now, if we instead consider no longer using actuator one to generate lifting thrust and do not shut it down completely, but instead run it at lower speeds (in either direction) to apply some reaction torque to the system, it can be shown that the controllability matrix for this configuration will be full rank, where ๏ฃฏ 0 1 1 0 1 1 ๏ฃบ ๏ฃฎ ๏ฃน ๏ฃฏ ๏ฃบ ๏ฃฏ โˆ’๐‘Ÿ โˆ’๐‘Ÿ ๐‘Ÿ ๏ฃบ ๏ฃบ ๏ฃฏ ๏ฃฏ 2 โˆ’๐‘Ÿ 2 0 ๐‘Ÿ 2 ๏ฃบ ๐‘€=๏ฃฏ โˆš โˆš โˆš ๏ฃบ. (E.6) ๏ฃฏ๐‘Ÿ 3 โˆ’๐‘Ÿ 3 ๐‘Ÿ 3๏ฃบ ๏ฃฏ 2 0 2 0 0 2 ๏ฃบ๏ฃบ ๏ฃฏ ๏ฃฏ ๏ฃบ ๏ฃฏ ๐‘ ๏ฃฐ โˆ’๐‘ ๐‘ 0 ๐‘ โˆ’๐‘ ๏ฃบ๏ฃป This configuration is shown in Fig. E.3, and is used to generate a feasible reconfiguration strategy and controller for the standard hexrotor under actuator failure. 114 Figure E.2: Candidate reconfiguration strategy for a hexrotor UAV. Disabling the actuator across the center of mass from the failed actuator results in a non-standard quadrotor configuration. This configuration is not controllable. Figure E.3: Candidate reconfiguration strategy for a hexrotor UAV. Enabling the actuator across the center of mass from the failed actuator to rotate in either direction so as to apply both upward and downward forces results in a fully controllable system. 115 BIBLIOGRAPHY 116 BIBLIOGRAPHY [1] Evan Ackerman and Eliza Strickland. Medical delivery drones take flight in east africa. IEEE Spectrum, 55(1):34โ€“35, 2018. [2] Halim Alwi and Christopher Edwards. Fault tolerant control of octorotor using sliding mode control allocation. In Proceedings of the EuroGNC Specialist Conference on Guidance, Navigation & Control, pages 1404โ€“1423, 2013. [3] Halim Alwi and Christopher Edwards. Sliding mode fault-tolerant control of an octorotor using linear parameter varying-based schemes. IET Control Theory & Applications, 9(4):618โ€“636, 2015. [4] Trevor Avant, Unsik Lee, Brian Katona, and Kristi Morgansen. Dynamics, hover configurations, and rotor failure restabilization of a morphing quadrotor. In American Control Conference (ACC), pages 4855โ€“4862. IEEE, 2018. [5] Juan A Besada, Luca Bergesio, Ivรกn Campaรฑa, Diego Vaquero-Melchor, Jaime Lรณpez-Araquistain, Ana M Bernardos, and Josรฉ R Casar. Drone mission definition and implementation for automated infrastructure inspection using airborne sensors. Sensors, 18(4):1170, 2018. [6] Jovan D Boskovic and Raman K Mehra. Stable multiple model adaptive flight control for accommoda- tion of a large class of control effector failures. In Proceedings of American Control Conference, pages 1920โ€“1924. IEEE, 1999. [7] Jovan D Boskovic, Ssu-Hsin Yu, and Raman K Mehra. A stable scheme for automatic control recon- figuration in the presence of actuator failures. In Proceedings of American Control Conference, pages 2455โ€“2459. IEEE, 1998. [8] Connor J. Boss, Vishal Abhishek, and Vaibhav Srivastava. Towards multi-body multi-rotors for long reach manipulation. In American Control Conference, 2021. [9] Connor J. Boss, Joonho Lee, and Jongeun Choi. Uncertainty and disturbance estimation for quadrotor control using extended high-gain observers: Experimental implementation. In ASME Dynamic Systems and Control Conference, 2017. [10] Connor J. Boss and Vaibhav Srivastava. A high-gain observer approach to robust trajectory estimation and tracking for a multi-rotor UAV. arXiv preprint arXiv:2103.13429, 2021. [11] Connor J. Boss and Vaibhav Srivastava. In-flight actuator failure recovery of a hexrotor via multiple models and extended high-gain observers. IEEE Robotics and Automation Letters, 2021. doi: 10.1109/LRA.2021.3090991. [12] Connor J. Boss, Vaibhav Srivastava, and Hassan K. Khalil. Robust tracking of an unknown trajectory with a multi-rotor UAV: A high-gain observer approach. In American Control Conference, pages 1429โ€“1434, Denver, CO, July 2020. [13] Patrick Bouffard, Anil Aswani, and Claire Tomlin. Learning-based model predictive control on a quadrotor: Onboard implementation and experimental results. In International Conference on Robotics and Automation, pages 279โ€“284. IEEE, 2012. 117 [14] Dario Brescianini and Raffaello Dโ€™Andrea. Computationally efficient trajectory generation for fully actuated multirotor vehicles. IEEE Transactions on Robotics, 34(3):555โ€“571, 2018. [15] Arghya Chakravarty, Tousif Khan Nizami, Indrani Kar, and Chitralekha Mahanta. Adaptive compen- sation of actuator failures using multiple models. IFAC World Congress, 50(1):10350โ€“10356, 2017. [16] Vilas K Chitrakaran, Darren M Dawson, Jian Chen, and Matthew Feemster. Vision assisted autonomous landing of an unmanned aerial vehicle. In Conference on Decision and Control, pages 1465โ€“1470. IEEE, 2005. [17] Patricio J Cruz and Rafael Fierro. Cable-suspended load lifting by a quadrotor UAV: hybrid model, trajectory generation, and control. Autonomous Robots, 41(8):1629โ€“1643, 2017. [18] Chen Diao, Bin Xian, Qiang Yin, Wei Zeng, Haotao Li, and Yungao Yang. A nonlinear adaptive control approach for quadrotor UAVs. In Asian Control Conference (ASCC), pages 223โ€“228. IEEE, 2011. [19] Zachary T Dydek, Anuradha M Annaswamy, and Eugene Lavretsky. Adaptive control of quadrotor UAVs: A design trade study with flight evaluations. IEEE Transactions on Control Systems Technology, 21(4):1400โ€“1406, 2012. [20] Zheng Fang and Weinan Gao. Adaptive integral backstepping control of a micro-quadrotor. In International Conference on Intelligent Control and Information Processing, volume 2, pages 910โ€“ 915. IEEE, 2011. [21] Yi Feng, Cong Zhang, Stanley Baek, Samir Rawashdeh, and Alireza Mohammadi. Autonomous landing of a UAV on a moving platform using model predictive control. Drones, 2(4):34, 2018. [22] Antonio Franchi and Anthony Mallet. Adaptive closed-loop speed control of BLDC motors with applications to multi-rotor aerial vehicles. In International Conference on Robotics and Automation (ICRA), pages 5203โ€“5208. IEEE, 2017. [23] Leonid B Freidovich and Hassan K Khalil. Lyapunov-based switching control of nonlinear systems using high-gain observers. Automatica, 43(1):150โ€“157, 2007. [24] Gowtham Garimella and Marin Kobilarov. Towards model-predictive control for aerial pick-and-place. In International Conference on Robotics and Automation (ICRA), pages 4692โ€“4697. IEEE, 2015. [25] Alvika Gautam, PB Sujit, and Srikanth Saripalli. A survey of autonomous landing techniques for UAVs. In International Conference on Unmanned Aircraft Systems, pages 1210โ€“1218, 2014. [26] Ola Harkegard. Efficient active set algorithms for solving constrained least squares problems in aircraft control allocation. In Conference on Decision and Control, pages 1295โ€“1300. IEEE, 2002. [27] Daniel Hentzen, Thomas Stastny, Roland Siegwart, and Roland Brockers. Disturbance estimation and rejection for high-precision multirotor position control. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 2797โ€“2804, 2019. doi:10.1109/IROS40897.2019. 8968471. [28] Guillermo Heredia, AE Jimenez-Cano, I Sanchez, Domingo Llorente, V Vega, J Braga, JA Acosta, and Anรญbal Ollero. Control of a multirotor outdoor aerial manipulator. In International Conference on Intelligent Robots and Systems (IROS), pages 3417โ€“3422. IEEE, 2014. 118 [29] Bruno Herissรฉ, Tarek Hamel, Robert Mahony, and Franรงois-Xavier Russotto. Landing a VTOL unmanned aerial vehicle on a moving platform using optical flow. IEEE Transactions on Robotics, 28(1):77โ€“89, 2011. [30] Tru Hoang, Enkhmurun Bayasgalan, Ziyin Wang, Gavriil Tsechpenakis, and Dimitra Panagou. Vision- based target tracking and autonomous landing of a quadrotor on a ground vehicle. In American Control Conference, pages 5580โ€“5585, 2017. [31] Haomiao Huang, Gabriel M Hoffmann, Steven L Waslander, and Claire J Tomlin. Aerodynamics and control of autonomous quadrotor helicopters in aggressive maneuvering. In International Conference on Robotics and Automation, pages 3277โ€“3282. IEEE, 2009. [32] Mu Huang, Bin Xian, Chen Diao, Kaiyan Yang, and Yu Feng. Adaptive tracking control of underac- tuated quadrotor unmanned aerial vehicles via backstepping. In American Control Conference, pages 2076โ€“2081. IEEE, 2010. [33] Inseok Hwang, Sungwan Kim, Youdan Kim, and Chze Eng Seah. A survey of fault detection, isolation, and reconfiguration methods. IEEE Transactions on Control Systems Technology, 18(3):636โ€“653, 2009. [34] The MathWorks Inc. PX4 Autopilots Support from Embedded Coder. Natick, Massachusetts, United State, 2020. URL: https://www.mathworks.com/hardware-support/px4-autopilots.html. [35] Matthew Jardin and Eric Mueller. Optimized measurements of UAV mass moment of inertia with a bifilar pendulum. In AIAA Guidance, Navigation and Control Conference, page 6822, 2007. [36] AE Jimenez-Cano, Jesรบs Martin, Guillermo Heredia, Anรญbal Ollero, and R Cano. Control of an aerial robot with multi-link arm for assembly tasks. In International Conference on Robotics and Automation (ICRA), pages 4916โ€“4921. IEEE, 2013. [37] Mina Kamel, Thomas Stastny, Kostas Alexis, and Roland Siegwart. Model predictive control for trajectory tracking of unmanned aerial vehicles using robot operating system. In Robot Operating System (ROS), pages 3โ€“39. Springer, 2017. [38] Yunus Karaca, Mustafa Cicek, Ozgur Tatli, Aynur Sahin, Sinan Pasli, Muhammed Fatih Beser, and Suleyman Turedi. The potential use of unmanned aircraft systems (drones) in mountain search and rescue operations. The American journal of emergency medicine, 36(4):583โ€“588, 2018. [39] Farid Kendoul. Four-dimensional guidance and control of movement using time-to-contact: Application to automated docking and landing of unmanned rotorcraft systems. The International Journal of Robotics Research, 33(2):237โ€“267, 2014. [40] Hassan K Khalil. Nonlinear Systems. Upper Saddle River, 2002. [41] Hassan K Khalil. Nonlinear Control. Pearson New York, 2015. [42] Hassan K Khalil. High-Gain Observers in Nonlinear Feedback Control. Philadelphia, PA, USA: SIAM, 2017. [43] JeongWoon Kim, Yeondeuk Jung, Dasol Lee, and David Hyunchul Shim. Outdoor autonomous landing on a moving platform for quadrotors using an omnidirectional camera. In International Conference on Unmanned Aircraft Systems, pages 1243โ€“1252, 2014. 119 [44] Min Jun Kim, Jianjie Lin, Konstantin Kondak, Dongheui Lee, and Christian Ott. Oscillation damp- ing control of pendulum-like manipulation platform using moving masses. IFAC-PapersOnLine, 51(22):465โ€“470, 2018. [45] Suseong Kim, Seungwon Choi, and H Jin Kim. Aerial manipulation using a quadrotor with a two DOF robotic arm. In International Conference on Intelligent Robots and Systems, pages 4990โ€“4995. IEEE, 2013. [46] Suseong Kim, Seungwon Choi, Hyeonggeun Kim, Jongho Shin, Hyungbo Shim, and H Jin Kim. Robust control of an equipment-added multirotor using disturbance observer. Transactions on Control Systems Technology, 26(4):1524โ€“1531, 2018. [47] Weiwei Kong, Dianle Zhou, Daibing Zhang, and Jianwei Zhang. Vision-based autonomous landing system for unmanned aerial vehicle: A survey. In International Conference on Multisensor Fusion & Information integration for Intelligent Systems, pages 1โ€“8, 2014. [48] Christopher Korpela, Matko Orsag, and Paul Oh. Towards valve turning using a dual-arm aerial manipulator. In International Conference on Intelligent Robots and Systems (IROS), pages 3411โ€“3416. IEEE, 2014. [49] Vฤณay Kumar and Nathan Michael. Opportunities and challenges with autonomous micro aerial vehicles. The International Journal of Robotics Research, 31(11):1279โ€“1291, 2012. [50] Daewon Lee, H Jin Kim, and Shankar Sastry. Feedback linearization vs. adaptive sliding mode control for a quadrotor helicopter. International Journal of Control, Automation and Systems, 7(3):419โ€“428, 2009. [51] DongBin Lee, C Nataraj, Timothy C Burg, and Darren M Dawson. Adaptive tracking control of an underactuated aerial vehicle. In American Control Conference, pages 2326โ€“2331. IEEE, 2011. [52] Joonho Lee, Ranjan Mukherjee, and Hassan K Khalil. Control design for a helicopter using dynamic inversion and extended high gain observers. In ASME Dynamic Systems and Control Conference, pages 653โ€“660, 2012. [53] Joonho Lee, Ranjan Mukherjee, and Hassan K Khalil. Output feedback performance recovery in the presence of uncertainties. Systems & Control Letters, 90:31โ€“37, 2016. [54] Taeyoung Lee. Geometric control of quadrotor UAVs transporting a cable-suspended rigid body. IEEE Transactions on Control Systems Technology, 26(1):255โ€“264, 2017. [55] Taeyoung Lee, Melvin Leok, and N Harris McClamroch. Geometric tracking control of a quadrotor UAV on SE(3). In Conference on Decision and Control, pages 5420โ€“5425. IEEE, 2010. [56] Yilan Li, Hossein Eslamiat, Ningshan Wang, Ziyi Zhao, Amit K Sanyal, and Qinru Qiu. Autonomous waypoints planning and trajectory generation for multi-rotor UAVs. In Proceedings of the Workshop on Design Automation for CPS and IoT, pages 31โ€“40, 2019. [57] Peng Lu and Erik-Jan van Kampen. Active fault-tolerant control for quadrotors subjected to a complete rotor failure. In International Conference on Intelligent Robots and Systems (IROS), pages 4698โ€“4703. IEEE, 2015. 120 [58] Josรฉ A Macรฉs-Hernรกndez, Franรงois Defaรฟ, and Corentin Chauffaut. Autonomous landing of an UAV on a moving platform using model predictive control. In Asian Control Conference, pages 2298โ€“2303, 2017. [59] Tarek Madani and Abdelaziz Benallegue. Backstepping control with exact 2-sliding mode estimation for a quadrotor unmanned aerial vehicle. In International Conference on Intelligent Robots and Systems, pages 141โ€“146. IEEE, 2007. [60] Daniel Mellinger, Nathan Michael, and Vฤณay Kumar. Trajectory generation and control for precise aggressive maneuvers with quadrotors. The International Journal of Robotics Research, 31(5):664โ€“674, 2012. [61] Nathan Michael, Jonathan Fink, and Vฤณay Kumar. Cooperative manipulation and transportation with aerial robots. Autonomous Robots, 30(1):73โ€“86, 2011. [62] Giulia Michieletto, Markus Ryll, and Antonio Franchi. Fundamental actuation properties of multirotors: Forceโ€“moment decoupling and failโ€“safe robustness. Transactions on Robotics, 34(3):702โ€“715, 2018. [63] Balmukund Mishra, Deepak Garg, Pratik Narang, and Vipul Mishra. Drone-surveillance for search and rescue in natural disaster. Computer Communications, 156:1โ€“10, 2020. [64] Ryo Miyazaki, Hannibal Paul, Takamasa Kominami, and Kazuhiro Shimonomura. Wire-suspended device control based on wireless communication with multirotor for long reach-aerial manipulation. IEEE Access, 8:172096โ€“172104, 2020. [65] Abdellah Mokhtari, Abdelaziz Benallegue, and Boubaker Daachi. Robust feedback linearization and GHโˆž controller for a quadrotor unmanned aerial vehicle. In International Conference on Intelligent Robots and Systems, pages 1198โ€“1203. IEEE, 2005. [66] A Stephen Morse. Structural invariants of linear multivariable systems. SIAM Journal on Control, 11(3):446โ€“465, 1973. [67] Mark W Mueller and Raffaello Dโ€™Andrea. Stability and control of a quadrocopter despite the complete loss of one, two, or three propellers. In International Conference on Robotics and Automation (ICRA), pages 45โ€“52. IEEE, 2014. [68] Mark W Mueller, Markus Hehn, and Raffaello Dโ€™Andrea. A computationally efficient motion primitive for quadrocopter trajectory generation. IEEE Transactions on Robotics, 31(6):1294โ€“1310, 2015. [69] Hai-Nguyen Nguyen, Sangyul Park, Junyoung Park, and Dongjun Lee. A novel robotic platform for aerial manipulation using quadrotors as rotating thrust generators. IEEE Transactions on Robotics, 34(2):353โ€“369, 2018. [70] Daniel Orol, Jnaneshwar Das, Lukas Vacek, Isabella Orr, Mathews Paret, Camillo J Taylor, and Vฤณay Kumar. An aerial phytobiopsy system: Design, evaluation, and lessons learned. In International Conference on Unmanned Aircraft Systems (ICUAS), pages 188โ€“195. IEEE, 2017. [71] Matko Orsag, Christopher Korpela, and Paul Oh. Modeling and control of MM-UAV: Mobile manip- ulating unmanned aerial vehicle. Journal of Intelligent & Robotic Systems, 69(1-4):227โ€“240, 2013. [72] Christos Papachristos, Tung Dang, Shehryar Khattak, Frank Mascarich, Nikhil Khedekar, and Kostas Alexis. Modeling, control, state estimation and path planning methods for autonomous multirotor aerial robots. Foundations and Trends in Robotics, 7(3):180โ€“250, 2018. 121 [73] Pedro O Pereira and Dimos V Dimarogonas. Nonlinear pose tracking controller for bar tethered to two aerial vehicles with bounded linear and angular accelerations. In Conference on Decision and Control (CDC), pages 4260โ€“4265. IEEE, 2017. [74] Pedro O Pereira and Dimos V Dimarogonas. Pose and position trajectory tracking for aerial transporta- tion of a rod-like object. Automatica, 109:108547, 2019. [75] Pedro O Pereira, Manuel Herzog, and Dimos V Dimarogonas. Slung load transportation with a single aerial vehicle and disturbance removal. In Mediterranean Conference on Control and Automation (MED), pages 671โ€“676. IEEE, 2016. [76] Robert Platt Jr, Russ Tedrake, Leslie Kaelbling, and Tomas Lozano-Perez. Belief space planning assuming maximum likelihood observations. Robotics: Science and Systems, 2010. [77] Aaron Radke and Zhiqiang Gao. A survey of state and disturbance observers for practitioners. In American Control Conference, pages 6โ€“pp. IEEE, 2006. [78] Dario Sanalitro, Heitor J Savino, Marco Tognon, Juan Cortรฉs, and Antonio Franchi. Full-pose manip- ulation control of a cable-suspended load with multiple UAVs under uncertainties. IEEE Robotics and Automation Letters, 2020. [79] Jose Luis Sanchez-Lopez, Jesus Pestana, Srikanth Saripalli, and Pascual Campoy. An approach toward visual autonomous ship board landing of a VTOL UAV. Journal of Intelligent & Robotic Systems, 74(1-2):113โ€“127, 2014. [80] Theerasak Sangyam, Pined Laohapiengsak, Wonlop Chongcharoen, and Itthisek Nilkhamhang. Au- tonomous path tracking and disturbance force rejection of UAV using fuzzy based auto-tuning PID controller. In International Conference on Electrical Engineering/Electronics, Computer, Telecommu- nications and Information Technology, pages 528โ€“531. IEEE, 2010. [81] Yuri S Sarkisov, Min Jun Kim, Davide Bicego, Dzmitry Tsetserukou, Christian Ott, Antonio Franchi, and Konstantin Kondak. Development of SAM: Cable-suspended aerial manipulator. In International Conference on Robotics and Automation (ICRA), pages 5323โ€“5329. IEEE, 2019. [82] Junwon Seo, Luis Duque, and Jim Wacker. Drone-enabled bridge inspection methodology and appli- cation. Automation in Construction, 94:112โ€“126, 2018. [83] Pedro Serra, Rita Cunha, Tarek Hamel, David Cabecinhas, and Carlos Silvestre. Landing of a quadrotor on a moving target using dynamic image-based visual servo control. IEEE Transactions on Robotics, 32(6):1524โ€“1535, 2016. [84] Koushil Sreenath, Taeyoung Lee, and Vฤณay Kumar. Geometric control and differential flatness of a quadrotor UAV with a cable-suspended load. In Conference on Decision and Control (CDC), pages 2269โ€“2274. IEEE, 2013. [85] A Suarez, AM Giordano, Konstantin Kondak, Guillermo Heredia, and Anรญbal Ollero. Flexible link long reach manipulator with lightweight dual arm: Soft-collision detection, reaction, and obstacle localization. In International Conference on Soft Robotics (RoboSoft), pages 406โ€“411. IEEE, 2018. [86] Alejandro Suarez, Fran Real, Victor M Vega, Guillermo Heredia, Angel Rodriguez-Castaรฑo, and Anibal Ollero. Compliant bimanual aerial manipulation: Standard and long reach configurations. IEEE Access, 8:88844โ€“88865, 2020. 122 [87] Alejandro Suรกrez, P Sanchez-Cuevas, M Fernandez, M Perez, Guillermo Heredia, and Anibal Ollero. Lightweight and compliant long reach aerial manipulator for inspection operations. In International Conference on Intelligent Robots and Systems (IROS), pages 6746โ€“6752. IEEE, 2018. [88] Ha Le Nhu Ngoc Thanh and Sung Kyung Hong. Quadcopter robust adaptive second order sliding mode control based on PID sliding surface. IEEE Access, 6:66850โ€“66860, 2018. [89] Justin Thomas, Giuseppe Loianno, Koushil Sreenath, and Vฤณay Kumar. Toward image based visual servoing for aerial grasping and perching. In International Conference on Robotics and Automation (ICRA), pages 2113โ€“2118. IEEE, 2014. [90] Justin Thomas, Joe Polin, Koushil Sreenath, and Vฤณay Kumar. Avian-inspired grasping for quadro- tor micro UAVs. In International Design Engineering Technical Conferences and Computers and Information in Engineering Conference. ASME, 2013. [91] D. Tzoumanikas, Q. Yan, and S. Leutenegger. Nonlinear MPC with motor failure identification and recovery for safe and aggressive multicopter flight. In International Conference on Robotics and Automation (ICRA), pages 8538โ€“8544, 2020. doi:10.1109/ICRA40945.2020.9196690. [92] Jur Van Den Berg, Pieter Abbeel, and Ken Goldberg. LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information. The International Journal of Robotics Research, 30(7):895โ€“913, 2011. [93] Daniel Vey and Jan Lunze. Structural reconfigurability analysis of multirotor UAVs after actuator failures. In Conference on Decision and Control, pages 5097โ€“5104. IEEE, 2015. [94] Daniel Vey and Jan Lunze. Experimental evaluation of an active fault-tolerant control scheme for multirotor UAVs. In Conference on Control and Fault-Tolerant Systems (SysTol), pages 125โ€“132. IEEE, 2016. [95] Holger Voos. Nonlinear control of a quadrotor micro-UAV using feedback-linearization. In Interna- tional Conference on Mechatronics, pages 1โ€“6. IEEE, 2009. [96] Richard Voyles and Guangying Jiang. Hexrotor UAV platform enabling dextrous interaction with structuresโ€”Preliminary work. In International Symposium on Safety, Security, and Rescue Robotics (SSRR), pages 1โ€“7. IEEE, 2012. [97] Dan Xu, James Ferris Whidborne, and Alastair Cooke. Fault tolerant control of a quadrotor using L1 adaptive control. International Journal of Intelligent Unmanned Systems, 4(1):43โ€“66, 2016. 123