NONLINEAR CONTROL OF ROBOTIC FISH By Maria L. Castaño A DISSERTATION Submitted to Michigan State University in partial fulfillment of the requirements for the degree of Electrical Engineering – Doctor of Philosophy 2021 ABSTRACT NONLINEAR CONTROL OF ROBOTIC FISH By Maria L. Castaño In the past few decades, robots that propel and maneuver themselves like fish, known as robotic fish, have received substantial attention due to their efficiency, maneuverability, and lifelike features. Their agile locomotion can be partially attributed to their bio-inspired propulsion methods, which range from tail (caudal) and dorsal to paired pectoral fins. While these characteristics make robotic fish an attractive choice for a myriad of aquatic applications, their highly nonlinear, often under- actuated dynamics and actuator constraints present significant challenges in control design. The goal of this dissertation is to develop systematic model-based control approaches that guarantee closed-loop system stability, accommodate input constraints, and are computationally viable for robotic fish. We first propose a nonlinear model predictive control (NMPC) approach for path-following of a tail-actuated robotic fish, where the control design is based on an averaged dynamic model. The bias and the amplitude of the tail oscillation are treated as physical variables to be manipulated and are related to the control inputs via a nonlinear map. A control projection method is introduced to accommodate the inputs constraints while minimizing the optimization complexity in solving the NMPC problem. Both simulation and experimental results on a tail-actuated robotic fish support the efficacy of the proposed approach and its advantages over alternative approaches. Although NMPC is a promising candidate for tracking control, its computational complexity poses significant challenges in its implementation on resource-constrained robotic fish. We thus pro- pose a backstepping-based trajectory tracking control scheme that is computationally inexpensive and guarantees closed-loop stability. We demonstrate how the control scheme can be synthesized to handle input constraints and establish via singular perturbation analysis the ultimate boundedness of three tracking errors (2D-position and orientation) despite the under-actuated nature of the robot. The effectiveness of this approach is supported by both simulation and experimental results on a tail-actuated robotic fish. We then turn our attention to pectoral fin-actuated robotic fish. Despite its benefits in achieving agile maneuvering at low swimming speeds, the range constraint of pectoral fin movement presents challenges in control. To overcome these challenges, we propose two different backstepping-based control approaches to achieve trajectory tracking and quick-maneuvering control, respectively. We first propose a scaling-based approach to develop a control-affine nonlinear dynamic average model for a pectoral fin-actuated robotic fish, which is validated via both simulation and experiments. The utility of the developed average dynamic model is then demonstrated via the synthesis of a dual-loop backstepping-based trajectory tracking controller. Cyclic actuation can often limit precise manipulation of the fin movements and the full ex- ploitation of the maneuverability of pectoral fin-actuated robotic fish. To achieve quick velocity maneuvering control, we propose a dual-loop control approach composed of a backstepping-based controller in the outer loop and a fin movement-planning algorithm in the inner loop. Simulation results are presented to demonstrate the performance of the proposed scheme via comparison with a nonlinear model predictive controller. Copyright by MARIA L. CASTAÑO 2021 To my beloved parents, Mary and Luis Javier, for their love, support and encouragement. To the loving memory of my brother, Wilson Eduardo. v ACKNOWLEDGEMENTS Throughout my Ph.D. and academic journey, I have received a great deal of support and assistance from many people. First, I would like to sincerely thank my advisor Dr. Xiaobo Tan for his continuous support, patience, motivation, and guidance. I feel very honored to have had the opportunity to do my graduate research under his tutelage and will forever be thankful for his patient guidance throughout my Ph.D. journey. I would also like to thank the rest of my thesis committee: Dr. Hassan Khalil, Dr. Guoming Zhu, Dr. Vaibhav Srivastava, and Dr. Lixin Dong, for kindly joining my advisory committee and for their insightful comments, suggestions, and advice. I would like to express my gratitude to the technical and administrative staff of the Electrical and Computer Engineering Department for their assistance throughout the years. My deepest gratitude to John Thon for his fabrication expertise, especially his help in ensuring that the robots were waterproof. I am incredibly grateful for his willingness to go far and beyond to help me during times of urgency and duress. I am grateful to my friends at Michigan State University and my fellow labmates at Smart Microsystems Lab for their support, friendship, memories, and all the fun we have had in the last few years. Special thanks to Dr. Pratap Bhanu Solanki and Dr. Jason Greenberg. Thank you for the unforgettable memories of late-night study and research sessions. I also want to acknowledge the funding support from my National Science Foundation Graduate Research Program fellowship (DGE1424871), the Graduate School, and other National Science Foundation grants (IIP 1343413, IIS 1319602, CCF 1331852, ECCS 1446793, IIS 1715714, IIS 1848945) that made this work possible. I would like to thank all my uncles, aunts, cousins, and close friends for their love and support. A special thanks to my aunts Patricia and Delia and my cousin Jorge Humberto who have supported me during different transitional and challenging periods of my life. Many thanks to my high school vi best friend, Shawny Espinoza, for her emotional support, love, and motivation all these years. I am foremost and forever thankful to my source of inspiration, my parents Mary and Luis Javier, for their unconditional love, emotional and financial support, motivation, and encouragement. It is needless to say that without them, none of my accomplishments would be possible. Words can never fully express my appreciation for everything they have done and sacrificed for me. Thank you for being brave and for taking hard steps to seek better opportunities for our future. I love you both. Finally, I want to praise and thank God, who has granted me countless blessings, knowledge, health, and opportunity throughout my life. vii TABLE OF CONTENTS LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xi LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xii CHAPTER 1 INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.1 Control of Robotic Fish . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.1.1 Control of Body and Caudal Fin (BCF)-actuated Robotic Fish . . . . . . . 2 1.1.2 Control of Pectoral Fin-actuated Robotic Fish . . . . . . . . . . . . . . . . 3 1.2 Overview of Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.2.1 Model Predictive Control-based Path Following for Tail-actuated Robotic Fish 6 1.2.2 Backstepping Control-based Trajectory Tracking for Tail-actuated Robotic Fish . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.2.3 Averaged Modeling and Trajectory Tracking Control of Pectoral Fin- actuated Robotic Fish . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.2.4 Rapid Maneuvering Control of Pectoral Fin-actuated Robotic Fish . . . . . 10 CHAPTER 2 MODEL PREDICTIVE CONTROL-BASED PATH FOLLOWING FOR TAIL-ACTUATED ROBOTIC FISH . . . . . . . . . . . . . . . . . . . . . 12 2.1 Tail-actuated Robotic Fish Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.1.1 Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.2 Path-following Control Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.2.1 Path-following Error Coordinates . . . . . . . . . . . . . . . . . . . . . . . 16 2.2.2 Path-following Control Design . . . . . . . . . . . . . . . . . . . . . . . . 19 2.2.3 Control Projection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 2.3 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 2.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 2.4.1 Model Parameter Identification . . . . . . . . . . . . . . . . . . . . . . . . 32 2.4.2 Experimental Results on Path-Following . . . . . . . . . . . . . . . . . . . 35 2.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 CHAPTER 3 BACKSTEPPING CONTROL-BASED TRAJECTORY TRACKING FOR TAIL-ACTUATED ROBOTIC FISH . . . . . . . . . . . . . . . . . . . . . 40 3.1 System Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 3.2 Trajectory Tracking Control Algorithm . . . . . . . . . . . . . . . . . . . . . . . . 44 3.2.1 Trajectory Tracking Error Coordinates . . . . . . . . . . . . . . . . . . . . 44 3.2.2 Backstepping Control Design . . . . . . . . . . . . . . . . . . . . . . . . 45 3.3 Stability Analysis of the Closed-Loop System . . . . . . . . . . . . . . . . . . . . 53 3.3.1 Asymptotic Stability of Multi-Time-Scale Systems: The Setup . . . . . . . 54 3.3.2 Lyapunov Function Candidates and Two-Time-Scale Analysis . . . . . . . . 59 3.3.2.1 Asymptotic Stability of the Closed-Loop Perturbed System . . . . 61 3.4 Backstepping Control of Robotic Fish . . . . . . . . . . . . . . . . . . . . . . . . 63 viii 3.4.1 Dynamic Model of Robotic Fish . . . . . . . . . . . . . . . . . . . . . . . 63 3.4.2 Backstepping Control Design . . . . . . . . . . . . . . . . . . . . . . . . . 65 3.4.2.1 Control Synthesis . . . . . . . . . . . . . . . . . . . . . . . . . 66 3.4.2.2 Control Synthesis Incorporating Input Constraints . . . . . . . . 68 3.4.3 Stability Analysis of Closed-Loop Robotic Fish System . . . . . . . . . . . 71 3.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72 3.6 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 3.7 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78 CHAPTER 4 AVERAGED MODELING AND TRACKING CONTROL OF PECTORAL FIN-ACTUATED ROBOTIC FISH . . . . . . . . . . . . . . . . . . . . . . 82 4.1 Dynamic Model For Pectoral Fin-actuated Robotic Fish . . . . . . . . . . . . . . . 84 4.1.1 Rigid Body Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84 4.1.2 Drag and Lift on the Robot Body . . . . . . . . . . . . . . . . . . . . . . . 86 4.1.3 Hydrodynamic Forces from Rowing Pectoral Fins . . . . . . . . . . . . . . 86 4.2 Averaging with Scaled Forcing . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 4.2.1 Averaged Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 4.3 Simulation and Experimental Model Validation . . . . . . . . . . . . . . . . . . . 91 4.3.1 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92 4.3.2 Original Dynamics Parameter Identification . . . . . . . . . . . . . . . . . 92 4.3.3 Identification of Scaling Functions . . . . . . . . . . . . . . . . . . . . . . 96 4.3.4 Validation of the Averaged Model . . . . . . . . . . . . . . . . . . . . . . 99 4.4 Average Dynamic Model-based Trajectory Tracking Control . . . . . . . . . . . . 101 4.4.1 Trajectory Tracking Problem . . . . . . . . . . . . . . . . . . . . . . . . . 101 4.4.2 Trajectory Tracking Control Algorithm . . . . . . . . . . . . . . . . . . . . 102 4.5 Trajectory Tracking Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . 107 4.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 CHAPTER 5 RAPID MANEUVERING CONTROL OF PECTORAL FIN-ACTUATED ROBOTIC FISH . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110 5.1 Dynamic Model of Pectoral Fin-actuated Robotic Fish . . . . . . . . . . . . . . . . 111 5.2 Dual-Loop Fin Control Scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112 5.2.1 Velocity Tracking Problem . . . . . . . . . . . . . . . . . . . . . . . . . . 112 5.2.2 Velocity Tracking Control Algorithm . . . . . . . . . . . . . . . . . . . . . 113 5.2.3 Outer-loop Tracking Control Synthesis . . . . . . . . . . . . . . . . . . . . 113 5.2.4 Pectoral Fin Control Algorithm . . . . . . . . . . . . . . . . . . . . . . . . 115 5.3 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 5.4 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123 CHAPTER 6 SUMMARY AND FUTURE WORK . . . . . . . . . . . . . . . . . . . . . 124 6.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 APPENDICES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128 APPENDIX A PROOF OF THE ROBOTIC FISH’S INPUT-TO-STATE STABILITY129 ix APPENDIX B PROOF OF STABILITY OF THE PERTURBATION-FREE ROBOTIC FISH CLOSED-LOOP SYSTEM . . . . . . . . . . . . . . . . . . . 133 APPENDIX C PROOF OF STABILITY OF THE ROBOTIC FISH’S PERTURBED CLOSED-LOOP SYSTEM . . . . . . . . . . . . . . . . . . . . . . 138 BIBLIOGRAPHY . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142 x LIST OF TABLES Table 2.1: Identified parameters for the robotic fish used in this work. . . . . . . . . . . . . 32 Table 2.2: Model validation results: relative model prediction error for turning radius and turning period, when the tail beats at 1.5 Hz. . . . . . . . . . . . . . . . . . 35 Table 3.1: Comparison functions and inequalities that guarantee the asymptotic stability requirements for the two-time scale system. . . . . . . . . . . . . . . . . . . . . 58 Table 3.2: Parameters of the robotic fish. . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 Table 4.1: Identified model parameters for the robotic fish. . . . . . . . . . . . . . . . . . . 95 Table 4.2: Model validation results: relative model prediction error for turning radius and turning period. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96 Table 4.3: Model validation results: surge velocities predicted by the original model and measured from experiments, and their relative error. . . . . . . . . . . . . . . . 96 Table 4.4: Averaged model validation results: relative model prediction error between the original and averaged models for turning radius and turning period. . . . . . 99 xi LIST OF FIGURES Figure 1.1: Examples of reported robotic fish prototypes. . . . . . . . . . . . . . . . . . . . 1 Figure 1.2: The main types of pectoral fin motion (adapted from [1]). The rotation axes for the rowing, feathering, and flapping motions are vertical, transverse, and longitudinal, respectively. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 Figure 2.1: Top view of the tail-actuated robotic fish undergoing planar motion. . . . . . . . 13 Figure 2.2: Illustration of the path-following problem for robotic fish. . . . . . . . . . . . . 16 Figure 2.3: Illustration of the admissible input control set for robotic fish. . . . . . . . . . . 22 Figure 2.4: Illustration of a rectangular section chosen from the admissible control set. . . . 23 Figure 2.5: Boxed section encompassing the admissible control set. . . . . . . . . . . . . . 23 Figure 2.6: Simulation: line-tracking results for NMPC with the proposed control pro- jection method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 Figure 2.7: Simulation: line-tracking results for NMPC with boxed constraints inside the admissible sector-shaped control region. . . . . . . . . . . . . . . . . . . . . . 27 Figure 2.8: Simulation: line-tracking results for NMPC with direct nonlinear control constraints. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 Figure 2.9: Simulation: arc-tracking results for NMPC with the proposed control projec- tion method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 Figure 2.10: Simulation: arc-tracking results for NMPC with boxed constraints inside the admissible sector-shaped control region. . . . . . . . . . . . . . . . . . . . . . 29 Figure 2.11: Simulation: arc-tracking results for NMPC with nonlinear control constraints. . 29 Figure 2.12: The experimental setup. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 Figure 2.13: Tail-actuated robotic fish develop at Smart Microsystems Lab. . . . . . . . . . 31 Figure 2.14: Experiments: line-tracking results for NMPC with the proposed control pro- jection method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 xii Figure 2.15: Experiments: line-tracking results for NMPC with boxed constraints inside the admissible sector-shaped control region. . . . . . . . . . . . . . . . . . . . 37 Figure 2.16: Experiments: computed NMPC physical inputs and their projected values. . . . 37 Figure 2.17: Experiments: arc-tracking results for NMPC with the proposed control pro- jection method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 Figure 2.18: Experiments: arc-tracking results for NMPC with boxed constraints inside the admissible sector-shaped control region. . . . . . . . . . . . . . . . . . . . 38 Figure 3.1: Top view of an aquatic robotic system undergoing planar motion. . . . . . . . . 42 Figure 3.2: The bottom-up approach to the analysis of two-time-scale system. . . . . . . . . 55 Figure 3.3: Simulation: line and circle-tracking trajectory results, respectively, for backstepping- based and PI control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 Figure 3.4: Simulation: line-tracking position error for backstepping-based and PI con- trol. “STD” curves represent the standard deviation envelopes for the average error under each controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 Figure 3.5: Simulation: circle-tracking position error for backstepping-based and PI con- trol. “STD” curves represent the standard deviation envelopes for the average error under each controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 Figure 3.6: Simulation: line-tracking angle error for backstepping-based and PI control. “STD” curves represent the standard deviation envelopes for the average error under each controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 Figure 3.7: Simulation: circle-tracking angle error for backstepping-based and PI control. “STD” curves represent the standard deviation envelopes for the average error under each controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 Figure 3.8: Experiments: line and circle-tracking trajectory results, respectively, for backstepping-based and PI control. . . . . . . . . . . . . . . . . . . . . . . . . 78 Figure 3.9: A snapshot of an experimental run for line and circle-tracking using backstepping- based (a) and PI control (b), respectively. . . . . . . . . . . . . . . . . . . . . . 79 xiii Figure 3.10: Experiments: line-tracking (3.10a) and circle-tracking (3.10b) averaged po- sition error for backstepping-based control and PI control. The solid blue line with circular markers represents the averaged position error obtained for backstepping-based control, while the dashed blue lines represent its standard deviation. Similarly, the solid magenta line represents the averaged position error obtained for PI control, while the dotted-dashed magenta lines represent its standard deviation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80 Figure 3.11: Experiments: line-tracking (3.11a) and circle-tracking (3.11b) averaged an- gle error for backstepping-based control and PI control. The solid blue line with circular markers represents the averaged angle error obtained for backstepping-based control, while the dashed blue lines represent its stan- dard deviation. Similarly, the solid magenta line represents the averaged angle error obtained for PI control, while the dotted-dashed magenta lines represent its standard deviation. . . . . . . . . . . . . . . . . . . . . . . . . . 81 Figure 4.1: (a) Top view of the pectoral fin-actuated robotic fish undergoing planar mo- tion; (b) side view and blade element of the right pectoral fin with its param- eters and variables; (c) top view of the pectoral fin with its associated forces and angles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84 Figure 4.2: The experimental setup. During experiments the pectoral-fin actuated robotic fish swims within the enclosed area (denoted by the yellow lines) in the tank, and the overhead Logitech camera captures a video of the robot swimming. An image processing algorithm detects the red and blue markers placed on top of the robot to localize it and determine its heading. . . . . . . . . . . . . . 93 Figure 4.3: Obtained scaling values 𝐾 𝑓𝑥 𝑅 , 𝐾 𝑓 𝑦 𝑅 , 𝐾𝑚𝑅 versus the amplitude 𝛾 𝐴 𝑅 and bias 𝛾0 𝑅 , for fixed actuation period 𝑇𝑝 𝑅 = 1 s and ratio 𝜁 = 2. . . . . . . . . . . . . 98 Figure 4.4: Circular trajectories captured in experiments and predicted by the original and average dynamic models. In this case the right fin remains still and the left fin undergoes actuation with the following parameters fixed: 𝛾 𝐴𝐿 = 22◦ , 𝛾0𝐿 = 85◦ , 𝑇𝑃𝐿 = 1 and 𝜁 = 4. . . . . . . . . . . . . . . . . . . . . . . . . . . 100 Figure 4.5: Comparison between experimental results and the predictions of the steady- state forward swimming speed based on the average model and the original dynamic model. In this study the left and right fins undergo symmetric actuation with the following parameters fixed: 𝛾 𝐴 = 15◦ , 𝛾0 = 95◦ and 𝜁 = 4. . 101 Figure 4.6: Illustration of the proposed dual-loop pectoral fin-actuated robotic fish control scheme. The green dashed line encompasses the outer loop trajectory tracking controller, while the inner red dotted and dashed line encompasses the fin parameter optimization algorithm. . . . . . . . . . . . . . . . . . . . . . . . . 103 xiv Figure 4.7: Simulation: position, heading angle and velocity tracking for the line-tracking case. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 Figure 4.8: Simulation: position, heading angle and velocity tracking for the circle- tracking case. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 Figure 4.9: Simulation: desired input versus achievable inputs for the line tracking case. . . 109 Figure 5.1: Illustration of the proposed dual-loop pectoral fin-actuated robotic fish control scheme. The green dashed line encompasses the outer loop velocity tracking controller, while the inner red dotted and dashed line encompasses the fin movement planning and control algorithm. . . . . . . . . . . . . . . . . . . . 112 Figure 5.2: Example of different possible fin acceleration, velocity and position trajecto- ries generated within a planning interval 𝑇𝑃 that is divided into three sub-intervals.116 Figure 5.3: Example of weights for each value of 𝛾¥ 𝑅𝑖 within a given allowed range. The vertical dotted red lines indicate the maximum and minimum values allowed for 𝛾¥ 𝑅𝑖 in the 𝑖th interval, while the green solid line represents the desired value. 118 Figure 5.4: Simulation: velocity-tracking trajectories results for the proposed scheme (a) and the alternative scheme with NMPC for the inner loop controller (b). . . . . 121 Figure 5.5: Simulation: fin position trajectories resulting from the proposed scheme (a) and the alternative scheme with NMPC for the inner loop controller (b). The dashed blue lines depict the fin’s position constraints. . . . . . . . . . . . . . . 122 xv CHAPTER 1 INTRODUCTION The efficiency, astounding agility, and remarkable feats in swimming of real fish have motivated and inspired scientific interest in developing a class of autonomous underwater vehicles called robotic fish. As illustrated in Figure 1.1, various bio-inspired actuation mechanisms, ranging from oscillating tail (caudal) and paired pectoral fins [2, 3], a combination of both movements [4], to undulatory motion of the entire body [5], have been explored in the design of these robots. In addition to being promising platforms in applications such as aquatic environmental monitoring and search and rescue, robotic fish are also useful tools to study robot-animal interactions [6–9]. Their efficiency, maneuverability, and stealth are some of the characteristics that have made robotic fish an attractive choice over traditional propeller-driven underwater vehicles [10–12]. (a) Ghostswimmer, a robotic fish developed (b) Robotic fish developed by Delft University of Tech- by Boston Engineering Corporation [13]. nology [14]. (c) A robotic fish developed at New York (d) A robotic fish developed by the Smart Microsys- University [9]. tems Lab at Michigan State University. Figure 1.1: Examples of reported robotic fish prototypes. 1 1.1 Control of Robotic Fish To be suitable for aquatic applications and effectively carry out monitoring and sensing tasks, accurate trajectory tracking is essential for robotic fish. For example, trajectory tracking is critical when exploring and sampling specific areas, seeking pollutant sources, and mapping the whole aquatic environment. However, input constraints, and highly nonlinear and often under-actuated dynamics of these robots present significant challenges in trajectory control. Much of the work done on robotic fish has been focused on robot development, prototyping, [1, 5, 10, 15–31] and high-fidelity modeling [32–38]. There has also been extensive work on motion control of robotic fish, which has mainly been focused on the generation of coordinated movements of the actuation components to produce some fish-like swimming gaits. In the case of swimming gait generation, several kinematics and dynamics-based schemes [39–47], as well as bio-inspired approaches, such as central pattern generators [48–53], have been used to produce fish-like swimming. However, these approaches are typically open-loop in nature. Although some works have examined closed-loop approaches to drive the robot to achieve some desired motion [31, 54, 55], they have mainly been focused on heading or depth control. 1.1.1 Control of Body and Caudal Fin (BCF)-actuated Robotic Fish There has been additional work done on model-based closed-loop motion control of body and caudal fin-actuated robotic fish to achieve maneuvering, speed and orientation control, path following, point-to-point tracking, and line-of-sight (LOS) control [56–63]. In [56] a point-to-point control of a four-link robotic fish with an oscillating tail was implemented, where a classical proportional- integral-derivative controller combined with a fuzzy logic controller was designed for speed and orientation control. The authors in [57] devised a control strategy using motion primitives to achieve maneuvering of an aquatic vehicle using an oscillating foil. The strategy consists of an optimal off-line primitive planning step and an online feedback control step composed of a cascade of finite-time, time-scalable linear quadratic control, and input-output linearization, in combination 2 with a sliding mode controller. Furthermore, in [58] a target-tracking and collision-avoidance algorithm for two autonomous robotic fish was implemented via a situated-behavior-based and motion primitive decentralized control approach, in combination with an attractive force toward a target and a repulsive force for collision avoidance. In [59] three simplified linearized models of the decoupled fish dynamics were used for the design of linear quadratic regulators to achieve speed and orientation control and to stabilize the pitch and roll. Furthermore, a line-of-sight guidance scheme was implemented for way-point tracking using linearized controllers. Zou et al. developed a neural-network-based sliding mode control algorithm for cooperative trajectory tracking of multiple robotic fish [60]. The authors in [61] designed a sliding mode controller for swimming, orientating, and LOS way-point tracking of robotic fish in three-dimensional motion. In [62], the authors proposed a target-tracking hybrid controller that consists of an open-loop turning controller and a closed-loop backstepping controller to drive a robotic fish to a specified target location. Finally, in [63], the authors proposed a backstepping controller for tracking control of a nonholonomic fish robot with dynamics that can be expressed in a chained form. Despite the aforementioned progress in the control of robotic fish, most of the work focused on speed, orientation, and way-point tacking control. Furthermore, it has relied on utilizing either motion primitives, which can be system-specific, or linearized methods, which only provide local results (i.e., stability is only guaranteed in a neighborhood of the selected operating points). A unified, systematic control approach that accommodates input constraints and has stability guarantees is in general lacking for such robots. 1.1.2 Control of Pectoral Fin-actuated Robotic Fish While caudal fins have proven to be an efficient propulsion mode at higher speeds, pectoral fins are vital in assisting propulsion and achieving agile maneuvering at low swimming speeds [25, 64, 65]. As shown in Figure 1.2, pectoral fin motions can generally be classified into three modes based on the axis of rotation: rowing, feathering, and flapping, where the axes of rotation are vertical, transverse, and longitudinal, respectively. Rowing motion is classified as a “drag-based” swimming 3 Figure 1.2: The main types of pectoral fin motion (adapted from [1]). The rotation axes for the rowing, feathering, and flapping motions are vertical, transverse, and longitudinal, respectively. mechanism, where the drag element of fluid dynamics generates the thrust, and it is often regarded as an effective type of motion for achieving a number of in-plane locomotion and maneuvering tasks, such as forward swimming, side-way swimming, and turning [66,67]. The rowing motion of pectoral fins comprises two sub-movements during the fin-beat cycle: a power stroke and a recovery stroke. During the power stroke, the pectoral fin moves backward to produce thrust through induced drag on the pectoral fin surface, while during the recovery stroke, the fin moves toward the front of the body, ideally with minimal loading, to get ready for the next fin-beat cycle. Although beneficial in maneuvering, utilizing rowing motion for propulsion gives rise to chal- lenges for the control of robotic fish. The challenge lies with the actuation constraints (i.e., angular position, velocity, and acceleration limitations) and the mechanism in which the “drag-based” swimming method is used to generate thrust. For example, forward thrust can only be generated during the power stroke; however, when the pectoral fin reaches its maximum angular position, it has to recover in order to be able to generate forward thrust again. During the recovery phase, the fin will produce a “negative” thrust, thus opposing the objective of producing forward thrust in the first place. While such “backing-up” behavior seems natural to human understanding and is widely used by live fish, it is challenging to incorporate this behavior through systematic, rigorous controller synthesis. Significant work has been done on the modeling of pectoral fin-actuated robotic fish [3,5,24–30, 68–70]. Some of these works have focused on developing Computational Fluid Dynamics (CFD) models to carry out numerical analysis of the robot’s hydrodynamic characteristics and the force 4 generation of the fins. Despite being instrumental in studying pectoral fins’ propulsive mechanism, CFD-based models are not suitable for control design. Some efforts have also gone into developing analytical models to study the propulsive mechanism and gait analysis. For example, [71] and [72] utilized blade element theory (BET) to evaluate the quasi-static hydrodynamic forces generated by undulating and rowing fins, respectively, and [73] utilized Euler-Lagrange equation methods to develop a dynamic model for batoid swimming robots. Others have focused on developing modeling frameworks for analyzing the effects of different pectoral fin designs and materials on the robot’s swimming performance and mechanical efficiency [2,25,26,74,75]; however, these models tend to be complex and are not amenable to controller design. Control-affine models that easily allow studying the effect of the input parameters on the dynamics and fins movement are essential for control design and are lacking for these types of robots. Limited work has been reported on the control of pectoral fin-actuated robotic fish. Some of the work in this area has focused on open-loop motion control, i.e., the generation of coordinated movements of the actuation components to produce some fish-like swimming gaits [4, 76–78]. In terms of closed-loop control, several authors have proposed sensory-feedback Central Pattern Generators (CPGs) for target tracking or obstacle avoidance control [79, 80]. Similarly, in [81] the authors proposed a control strategy composed of two layers: an upper decision-making layer that uses a finite state machine to determine the proper swimming gait and a layer that uses a CPG to implement the desired gait. Fuzzy rule-based control laws were proposed in [82] to control fin-beat parameters to drive a robotic fish to perform rendezvous and docking in a three- dimensional workspace. The authors in [31] implemented geometric control methods for closed- loop depth control of a robotic fish using pectoral fins undergoing feathering motion. All of the aforementioned work is based on motion primitives, which can be system-specific, or have focused mainly on heading, depth, and velocity tracking for robotic fish using pectoral fins undergoing feathering or lead-lag motion. Systematic trajectory tracking control approaches that accommodate input constraints and have closed-loop stability guarantees have not been proposed for robotic fish with pectoral fins undergoing rowing motion. 5 The limited work on control of pectoral fin-actuated robotic fish utilized cyclic fin actuation for the lead-lag or feathering motion of the fins while treating the fin-beat parameters or the phase differences between the fins as the control variables for the fish locomotion control. However, such cyclic fin movement limits precise manipulation of the fin movements and the thrust profile that can be generated, which impedes full exploitation of the maneuverability of pectoral fin- actuated robotic fish. Acyclic thrust or moments could be more instrumental for generating a quick maneuvering response, which can be valuable in scenarios like counteracting disturbances or avoiding fast obstacles. 1.2 Overview of Contributions The contributions of this dissertation reside in the development of systematic model-based control approaches that guarantee closed-loop system stability, accommodate input constraints, and are computationally viable for robotic fish. In particular, we present two nonlinear model- based control approaches for path-following and trajectory tracking of a tail-actuated robotic fish. Furthermore, to exploit the benefits of pectoral fins and overcome their challenges in actuation, we propose two different backstepping-based control approaches to achieve trajectory tracking and quick-maneuvering control, respectively. The contributions of this research are further summarized in the following subsections. 1.2.1 Model Predictive Control-based Path Following for Tail-actuated Robotic Fish First, we propose a nonlinear model predictive control (NMPC) approach to path-following of a tail-actuated robotic fish that accommodates the nonlinear dynamics and actuation constraints while minimizing the control effort. Although there is extensive work on NMPC for path-following of mobile robots [83–88], little work has been reported on its application to control of robotic fish. In this controller design, a high-fidelity averaged nonlinear dynamic model developed in [89] is used. The physical control inputs consist of two of the tail-beat parameters, the bias and the amplitude, while the angular frequency of the tail beat is kept constant. We propose a scheme to 6 address the nonlinear input constraints. Specifically, to maximize the use of the admissible control and handle the nonlinear control constraints in a computationally efficient manner, we employ an analytical projection scheme for the control inputs. We further propose a novel estimation scheme to identify some critical unknown parameters in the robotic fish model. In particular, inspired by the work in [89], we develop a parameter estimation method to empirically identify the hydrodynamic and scaling coefficients of the model instead of utilizing time-consuming CFD simulations, or relying on trial-and-error data-fitting between dynamic simulation and experimental measurement. To implement the controller in real-time, we employ the ACADO Toolkit [90] to solve repeatedly the optimal control problem, and an image processing algorithm using OpenCV to provide feedback. Some preliminary results of this work were presented at the 2016 ASME Dynamic Systems and Control Conference [91]. More comprehensive results were published in ASME Journal of Dynamic Systems and Control [92], where we proposed a computation-efficient method to deal with the nonlinear constraints, formulated a parameter estimation scheme to identify crucial parameters in the model, and reported experimental results on a tail-actuated robotic fish. The proposed NMPC scheme was compared with two alternative approaches. In one approach, the nonlinear input constraints were directly defined, and in the other boxed constraints were used. Overall, the tracking simulation and experimental results showed that the proposed NMPC scheme with projection resulted in faster convergence to the desired path and smaller path error than the alternative approaches. Chapter 2 describes the details of this work. 1.2.2 Backstepping Control-based Trajectory Tracking for Tail-actuated Robotic Fish Although NMPC is a promising candidate for tracking control, its computational complexity poses significant challenges in implementing such controllers on resource-constrained robotic fish. Backstepping-based control design presents a practical and promising approach for trajectory tracking with stability guarantees. In particular, it is computationally inexpensive, especially when compared to methods such as NMPC, offers a systematic framework for the design of the feedback control law, guarantees the stability of the system, and allows the accommodation of input 7 constraints. Some limited work has been reported on backstepping-based control of robotic fish [62, 63]. In [62], the authors proposed a target-tracking hybrid controller that consists of an open-loop turning controller and a closed-loop backstepping controller to drive a robotic fish to a specified target location. In [63], the authors proposed a backstepping controller for tracking control of a nonholonomic fish robot with dynamics that can be expressed in a chained form. The proposed approach guaranteed asymptotic convergence to a desired trajectory generated by a reference fish robot, but limitations were placed on the desired trajectory since it had to be generated by dynamics in the same chained form. Furthermore, input constraints were not accommodated. In this work, we address the trajectory tracking control problem for a general class of autonomous under-actuated planar aquatic robots. In particular, we present a systematic method to synthesize a backstepping-based continuous tracking controller to ultimately bound the position tracking errors and the orientation error between the robot’s heading and a vector tangential to the reference. Via multi-time-scale analysis of perturbed systems, we prove how the control scheme achieves boundedness and convergence of the tracking errors to a neighborhood of the origin. The scheme proposed in this work is inspired by the approach in [93]. In particular, we utilize the idea of introducing an additional error coordinate that couples the lateral position error and the heading angle error. In this manner, by stabilizing the longitudinal tracking error along with this new state, we can guarantee that the whole error system is stabilized at a neighborhood of the origin. Furthermore, we establish a systematic method that provides necessary conditions to be considered when designing this augmenting state for a general class of systems. The proposed control approach is implemented in a tail-actuated robotic fish, where the con- troller design uses a high-fidelity averaged nonlinear dynamic model [89]. In addition, the physical control inputs consist of the tail amplitude, and bias with the angular frequency kept constant. Furthermore, an auxiliary system is designed to accommodate the input constraints such that the controller compensates for the error due to the difference between the feasible and nominal inputs. Finally, simulation and experimental results demonstrate the effectiveness of the controller. 8 Preliminary simulation results were presented at the 2019 International Conference on Advanced Intelligent Mechatronics [94]. The details of this work are provided in Chapter 3. 1.2.3 Averaged Modeling and Trajectory Tracking Control of Pectoral Fin-actuated Robotic Fish Pectoral fins have become a useful actuation mechanism for robotic fish [25, 64, 65]. Utilizing pectoral fins for locomotion improves maneuverability and can allow robotic fish to achieve tra- jectories that could not otherwise be possible with just a tail fin, such as circular trajectories with smaller radii, and sharp, aggressive turning. However, despite their benefit, pectoral fins give rise to challenges in control. Given the rhythmic nature of the robotic fish’s body and fin movements, averaging has proven to be a useful approach in obtaining control-affine models [95, 96] and studying the effect of the input parameters on its dynamics and fins movement [31, 54, 89]. This makes an averaged model best suited for trajectory planning and tracking control. To the best of our knowledge, the use of averaging has not been explored in developing control-affine models for pectoral fin-actuated robotic fish. In this work, we present a nonlinear dynamic average model for robotic fish propelled by a pair of rigid pectoral fins undergoing rowing motion. In particular, we consider the robot undergoing planar motion, with its original dynamics incorporating pectoral fin-generated hydrodynamic forces evaluated via the blade element theory. Inspired by the work in [89], which deals with averaged dynamics for tail-actuated robotic fish, we seek scaling factors, as functions of fin-beat parameters, for the original hydrodynamic forces and moment. These scaling factors are determined such that when classical averaging is applied to the resulting modified dynamics, the obtained average model produces locomotion behaviors close to those of the original dynamic model. One fundamental step in identifying the scaling functions is estimating the scaling values for a given fin-beat pattern. [89] used a trial-and-error approach for the tail-actuated robotic fish, which is time-consuming. We propose a novel systematic approach to finding optimal scaling values by formulating a nonlinear 9 model-predictive control (NMPC) problem, which can be readily solved with NMPC packages instead. Once the scaling values are found for a set of fin-beat patterns, nonlinear regression is used to determine the scaling functions with minimal complexity. Simulation comparison between the averaged model and the original dynamic model, under fin-beat patterns not used in identifying the scaling functions, supports the efficacy of the developed averaged model. Furthermore, we conduct experiments on a pectoral fin-actuated robotic fish and compare the experimental results with simulation predictions when considering the forward swimming motion, where both fins are actuated symmetrically. Finally, to demonstrate the utility of the dynamic average model, we design a controller for trajectory tracking of a pectoral fin-actuated robotic fish. In particular, the proposed scheme uses a backstepping-based controller that finds the needed inputs for the robot to track the desired trajectory based upon the averaged model. In this design, the physical control inputs involve two of the fin-beat parameters, the bias, and the amplitude, while the other parameters (angular frequency and power/recover stroke ratio) are kept constant. We further use a multi-variable minimization solver to determine the optimal fin-beat parameters such that the achieved inputs are closed to the needed values. Simulation results are presented to demonstrate the effectiveness of the proposed model-based tracking control scheme. Preliminary experimental results demonstrating the effectiveness of the proposed average model will be presented at the 2021 Modeling, Estimation and Control Conference (MECC) [97]. Chap- ter 4 provides more detail of this work. 1.2.4 Rapid Maneuvering Control of Pectoral Fin-actuated Robotic Fish Cyclic fin movement can limit precise manipulation of the fin movements and the thrust profile that can be generated which can be detrimental in tasks like maneuvering. Acyclic actuation could be more instrumental in such scenarios. In this work, we propose a systematic approach to the control of pectoral fins that naturally accommodates the fins’ constraints and automatically generates “intelligent” behavior (e.g., “backing-up” when required) to produce acyclic thrust for 10 quick maneuvering. In particular, we consider the velocity tracking problem as an example to illustrate the challenges and propose a dual-loop control structure to drive the velocity tracking error to a neighborhood of the origin. The outer loop of the proposed scheme is composed of a backstepping-based velocity-tracking controller that finds the needed thrust and moment for the robot’s velocity to track the desired velocity profile. The inner loop is composed of a randomized, model-predictive fin planning algorithm, which determines a feasible sequence for the fins’ angular accelerations such that the thrust and moments generated are close to desired values. To demonstrate the effectiveness of the proposed scheme, we present simulation results on tracking forward and angular velocities that are abruptly changing. The proposed scheme is compared with an alternative approach, in which the inner loop is implemented with a nonlinear model predictive controller (NMPC). Preliminary simulation results were presented at the 2021 International Conference on Advanced Intelligent Mechatronics [98]. Further details of this work are presented in Chapter 5. 11 CHAPTER 2 MODEL PREDICTIVE CONTROL-BASED PATH FOLLOWING FOR TAIL-ACTUATED ROBOTIC FISH To be suitable for monitoring and sensing aquatic environments, accurate path-following control is vital for robotic fish. In this chapter we propose a nonlinear model predictive control (NMPC) approach for path-following of a tail-actuated robotic fish. In this controller design, a high-fidelity averaged nonlinear dynamic model is used, and the physical control inputs consist of two of the tail- beat parameters, the bias and the amplitude, while the other parameter (angular frequency) is kept constant. To address the nonlinear input constraints and maximize the use of the admissible control in a computationally efficient manner, we employ an analytical projection scheme. Furthermore, we present a novel parameter estimation scheme to empirically identify unknown parameters present in the robotic fish model instead of utilizing time-consuming CFD simulations or relying on trial- and-error data fitting between dynamic simulation and experimental measurement. To implement real-time control, we employ ACADO Toolkit [90] to solve repeatedly the optimal control problem, and an image processing algorithm using OpenCV to provide feedback. This chapter is organized as follows. In section 2.1, the dynamic and scaled averaging models of the tail-actuated robotic fish, followed by a simplified averaged model, are reviewed. In Section 2.2 the path-following problem formulation is presented, followed by the NMPC design and the proposed control projection scheme. In Section 2.3 simulation results are discussed, and in Section 2.4 the experimental setup, the proposed parameter estimation scheme and the experimental results are presented. 2.1 Tail-actuated Robotic Fish Model 2.1.1 Dynamic Model As presented in [89], the tail-actuated robotic fish is modeled as a rigid body with a rigid tail that is actuated at its base, and it is assumed that the robot operates in an inviscid, irrotational, 12 and incompressible fluid within an infinite domain. Let [X,Y,Z]𝑇 and [x, y, z] 𝑇 be the inertial coordinate system and the body-fixed coordinate system, respectively, as illustrated in Figure 2.1. The velocity of the center of mass in the body-fixed coordinates is expressed as V𝑐 = [𝑉𝑐𝑥 , 𝑉𝑐 𝑦 , 𝑉𝑐 𝑧 ], where 𝑉𝑐𝑥 , 𝑉𝑐 𝑦 , and 𝑉𝑐 𝑧 denote the surge, sway, and heave velocities, respectively. Furthermore, let 𝛽 denote the angle of attack, formed by the direction of V𝑐 with respect to the x-axis, and 𝜓 denote the heading angle, formed by the x-axis relative to the X-axis. The angular velocity expressed in the body-fixed coordinate system is given by 𝜔 = [𝜔𝑥 , 𝜔 𝑦 , 𝜔 𝑧 ], which is composed of roll (𝜔𝑥 ), pitch (𝜔 𝑦 ), and yaw (𝜔 𝑧 ). Finally, let 𝛼 denote the tail deflection angle with respect to the negative x-axis. Y c KD y D C L Robotic Fish Body Tail Fin Z X Figure 2.1: Top view of the tail-actuated robotic fish undergoing planar motion. We only consider the planar motion and further assume that the body is symmetric with respect to the xz-plane and that the tail moves in the xy-plane. As a result, the system has three degrees of freedom, surge (𝑉𝑐𝑥 ), sway (𝑉𝑐 𝑦 ), and yaw (𝜔 𝑧 ). It is further assumed that the inertial coupling between yaw, sway and surge motions is negligible, which leads to the following equations of planar 13 motion (𝑚 𝑏 − 𝑚 𝑎 𝑥 )𝑉¤𝑐𝑥 = (𝑚 𝑏 − 𝑚 𝑎 𝑦 )𝑉𝑐 𝑦 𝜔 𝑧 + 𝑓𝑥 (2.1) (𝑚 𝑏 − 𝑚 𝑎 𝑦 )𝑉¤𝑐 𝑦 = −(𝑚 𝑏 − 𝑚 𝑎 𝑥 )𝑉𝑐𝑥 𝜔 𝑧 + 𝑓 𝑦 (2.2) (𝐽𝑏𝑧 − 𝐽𝑎 𝑧 ) 𝜔¤ 𝑧 = (𝑚 𝑎 𝑦 − 𝑚 𝑎 𝑥 )𝑉𝑐𝑥 𝑉𝑐 𝑦 + 𝑀𝑧 (2.3) where 𝑚 𝑏 is the mass of the body, 𝐽𝑏𝑧 is the inertia of the body about the z-axis, 𝑚 𝑎 𝑥 and 𝑚 𝑎 𝑦 are the hydrodynamic derivatives that represent the added masses of the robotic fish along the x and y directions, respectively, and 𝐽𝑎 𝑧 represents the added inertia effect of the body about the z-axis. The hydrodynamic forces and moment due to tail fin actuation and the interaction of the body itself with the fluid are captured by 𝑓𝑥 , 𝑓 𝑦 , and 𝑀𝑧 . To evaluate the hydrodynamic forces exerted by the tail, Lighthill’s large amplitude elongated body theory is used [89]. The kinematic equations for the robotic fish are given by 𝑋¤ = 𝑉𝑐𝑥 cos 𝜓 − 𝑉𝑐 𝑦 sin 𝜓 (2.4) 𝑌¤ = 𝑉𝑐𝑥 sin 𝜓 + 𝑉𝑐 𝑦 cos 𝜓 (2.5) 𝜓¤ = 𝜔 𝑧 (2.6) Given the rhythmic nature of the robotic fish movement and the periodic tail actuation, averaging has proven to be a useful approach in studying the effect of the input parameters on the dynamics of the robotic fish [89]. Furthermore, in practical applications, it is more natural to control the parameters for periodic fin beats than to directly control the fin position at every moment. Therefore, an averaged model is best suited for trajectory planning and tracking control. We next review the averaged model proposed in [89], where the following periodic pattern for the tail deflection angle is considered: 𝛼(𝑡) = 𝛼0 + 𝛼𝑎 sin(𝜔𝛼 𝑡) (2.7) where 𝛼0 , 𝛼𝑎 , and 𝜔𝛼 represent the bias, amplitude, and frequency of the tail beat, respectively. The original hydrodynamic force and moment terms in Eqs. (2.1)-(2.3) are scaled by some functions 14 dependent on the tail beat parameters, 𝛼0 , 𝛼𝑎 , and 𝜔𝛼 , and classical averaging is then conducted over these scaled dynamics. In particular, we define the states 𝑥 1 = 𝑉𝑐𝑥 , 𝑥 2 = 𝑉𝑐 𝑦 and 𝑥3 = 𝜔 𝑧 , so that the averaged dynamics takes the following form 𝑥¤1 = 𝑓1 (𝑥 1 , 𝑥2 , 𝑥3 ) + 𝐾 𝑓 𝑓¯4 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) (2.8) 𝑥¤2 = 𝑓2 (𝑥 1 , 𝑥2 , 𝑥3 ) + 𝐾 𝑓 𝑓¯5 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) (2.9) 𝑥¤3 = 𝑓3 (𝑥 1 , 𝑥2 , 𝑥3 ) + 𝐾𝑚 𝑓¯6 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) (2.10) with q q 𝑚2 𝑐1 𝑐2 𝑥 𝑓1 (𝑥1 , 𝑥2 , 𝑥3 ) = 𝑥 2 𝑥 3 − 2 𝑥1 𝑥1 + 𝑥2 + 2 𝑥 2 𝑥 12 + 𝑥22 arctan( 2 ) (2.11) 𝑚1 𝑚1 𝑚1 𝑥1 q q 𝑚 𝑐 𝑐 𝑥 𝑓2 (𝑥1 , 𝑥2 , 𝑥3 ) = − 1 𝑥1 𝑥 3 − 1 𝑥 2 𝑥 12 + 𝑥 22 − 2 𝑥1 𝑥 12 + 𝑥 22 arctan( 2 ) (2.12) 𝑚2 𝑚2 𝑚2 𝑥1 𝑓3 (𝑥1 , 𝑥2 , 𝑥3 ) =(𝑚 1 − 𝑚 2 )𝑥 1 𝑥2 − 𝑐 4 𝜔2𝑧 sgn(𝜔 𝑧 ) (2.13) 𝑚 3 3 𝑓¯4 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) = 𝜔2𝛼 𝛼𝑎2 (3 − 𝛼02 − 𝛼𝑎2 ) (2.14) 12𝑚 1 𝐿 2 2 8 𝑚 𝑓¯5 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) = 𝜔2𝛼 𝛼𝑎2 𝛼0 (2.15) 4𝑚 2 𝐿 2 𝑚 𝑓¯6 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) = − c𝜔2𝛼 𝛼𝑎2 𝛼0 (2.16) 4𝐽3 𝐿 2 1 1 where 𝑚 1 = 𝑚 𝑏 − 𝑚 𝑎 𝑥 , 𝑚 2 = 𝑚 𝑏 − 𝑚 𝑎 𝑦 , 𝐽3 = 𝐽𝑏𝑧 − 𝐽𝑎 𝑧 , 𝑐 1 = 𝜌𝑆𝐶 𝐷 , 𝑐 2 = 𝜌𝑆𝐶 𝐿 , 𝑐 3 = 2 2 1 2 1 1 2 1 3 𝑚𝐿 , 𝑐 4 = 𝐾 , 𝑐 = 𝐿 𝑚𝑐, and 𝑐 6 = 𝐿 𝑚. Here 𝑆 denotes the reference 2 (𝐽3 ) 𝐷 5 (2𝐽3 ) (3𝐽3 ) surface area for the robot body, 𝐶 𝐷 , 𝐶 𝐿 and 𝐾 𝐷 represent the drag force coefficient, lift coefficient, and drag moment coefficient, respectively, 𝜌 is the density of water, 𝐿 is the tail length, 𝑐 is the distance from the body center to the pivot point of the actuated tail and 𝑚 represents the mass of water displaced by the tail per unit length and is approximated by 𝜋4 𝜌𝑑 2 with 𝑑 denoting the tail depth. 𝐾 𝑓 is a scaling constant, and 𝐾𝑚 (𝛼0 ) is a scaling function affine in 𝛼0 . To further facilitate control design, in this work 𝐾𝑚 is considered as a constant during the NMPC design. This term is found by taking the average of 𝐾𝑚 for a given range of 𝛼0 . The resulting model is called the simplified averaged model in this dissertation. 15 2.2 Path-following Control Algorithm It is important to design a controller that is able to meet performance objectives such as minimizing the path-tracking error while accommodating input constraints. We are thus motivated to develop an NMPC scheme for path-following. NMPC is an attractive choice because it allows explicit consideration of state and input constraints, is capable of handling nonlinear models, and can optimize control performance [99, 100]. 2.2.1 Path-following Error Coordinates In contrast to trajectory tracking, in path-following one is interested in following a geometric reference parametrized by some scalar without any specified timing. The kinematic model of the robotic fish is expressed in a Frenet-Serret frame {F} that moves along the reference path according to some desired function of time. Figure 2.2 illustrates the path-following problem. Vc Y y {B} Desired Robotic θc Fish Path C Body Yp p {F} p(s) C̄ P̄ Z X Figure 2.2: Illustration of the path-following problem for robotic fish. Assume that the reference path is a twice continuously differentiable geometric curve that is 16 defined as a set of points 𝑷 parametrized by the scalar 𝑠, P = {𝑷 ∈ R2 |𝑷 = 𝒑(𝑠), ∀𝑠 ∈ [0, 𝑙 𝑝 ]} (2.17) where 𝑙 𝑝 denotes the length of the path, and the function 𝒑 : R1 → R2 is twice differentiable. Let P denote a point on the path to be followed, 𝜃 𝑝 the tangential angle of the path at point P, and 𝜃 𝑐 the angle between the robotic fish velocity vector V𝑐 and the inertial X-axis, while the coordinate axes x 𝑝 and y 𝑝 are directed along the tangential and normal directions at point P. We let point C denote the center of the robotic fish, and the vectors C̄ and P̄ describe the positions of C and P in the 3-D inertial frame {I}. Note that since we are only considering the planar case, the third component of the position vectors is taken as 0. Let r = [𝑋𝑒 ; 𝑌𝑒 ; 0] 𝑇 denote the position of the robotic fish center C with respect to the point P on the path expressed in {F}. Let 𝐼 𝑹 𝐹 denote the rotation matrix from {I} to {F} and 𝐹 𝑹 𝐼 denote the rotation matrix from {F} to {I}, with  cos 𝜃 𝑝 sin 𝜃 𝑝 0   𝐼𝑹 =    𝐹  − sin 𝜃 𝑝 cos 𝜃 𝑝 0   (2.18)    0 0 1  Define 𝜃¤ 𝑝 = 𝐶 𝑝 (𝑠) 𝑠¤, where 𝐶 𝑝 (𝑠) is the path’s curvature. One can express C̄ = P̄ +𝐹 𝑹 𝐼 r (2.19) The velocity of C̄ in {I} is given by  dC̄   dP̄   dr  = +𝐹 𝑹 𝐼 +𝐹 𝑹 𝐼 (𝝎 𝑝 × r) (2.20) d𝑡 {𝐼} d𝑡 {𝐼} d𝑡 {𝐹} where 0     𝝎 𝑝 =  0  (2.21) 𝜃¤ 𝑝      Multiplying the equation above on the left by 𝐼 𝑹 𝐹 gives the velocity of C expressed in {F}:  dC̄   dP̄   dr  𝐼𝑹 = + + (𝝎 𝑝 × r) (2.22) 𝐹 d𝑡 {𝐼} d𝑡 {𝐹} d𝑡 {𝐹} 17 where  ¤  dC̄  𝑋    = 𝑌¤  (2.23) d𝑡 {𝐼}   0    𝑠¤     dP̄    = 0 (2.24) d𝑡 {𝐹}   0   ¤   dr   𝑋𝑒    =  𝑌¤𝑒  (2.25) d𝑡 {𝐹}   0    0   𝑋𝑒  −𝐶 𝑝 (𝑠) 𝑠¤𝑌𝑒              𝝎 𝑝 × r =  0  ×  𝑌𝑒  =  𝐶 𝑝 (𝑠) 𝑠¤ 𝑋𝑒  (2.26)       𝐶 𝑝 (𝑠) 𝑠¤  0   0        After rearranging and solving for 𝑋¤ 𝑒 and 𝑌¤𝑒 from Eq. (2.22), we get the following expression " # " # 𝑋¤ 𝑒 𝑋¤ cos 𝜃 𝑝 + 𝑌¤ sin 𝜃 𝑝 − 𝑠¤ + 𝐶 𝑝 (𝑠) 𝑠¤𝑌𝑒 = (2.27) 𝑌¤𝑒 − 𝑋¤ sin 𝜃 𝑝 + 𝑌¤ cos 𝜃 𝑝 − 𝐶 𝑝 (𝑠) 𝑠¤ 𝑋𝑒 Let 𝛼𝑒 = 𝜓 − 𝜃 𝑝 , and further expand 𝑋¤ and 𝑌¤ with Eq. (2.4) and Eq. (2.5), respectively, which results in the following error state model © 𝑋¤ 𝑒 ª ©𝑉𝑐 cos(𝛼𝑒 + 𝛽) − 𝑠¤ + 𝐶 𝑝 (𝑠) 𝑠¤𝑌𝑒 ª ­ ® ­ ® ­ 𝑌¤𝑒 ® = ­ 𝑉𝑐 sin(𝛼𝑒 + 𝛽) − 𝐶 𝑝 (𝑠) 𝑠¤ 𝑋𝑒 ® (2.28) ­ ® ­ ® ­ ® ­ ® 𝛼¤ 𝑒 𝜔 𝑧 − 𝐶 𝑝 (𝑠) 𝑠¤ « ¬ « ¬ where 𝑉𝑐 is the magnitude of the robotic fish’s translational velocity. The dynamical model of the robotic fish in the error state is then obtained by augmenting the equations above with the simplified averaged scaled dynamics as seen in Eqs. (2.8)-(2.10). Ideally, one would like the robotic fish to not only converge to a desired path, but also move along the path with some desired surge velocity and desired angular velocity. Let 𝑉𝑑𝑥 be the desired surge velocity, and let the velocity error states be defined by 𝜂 𝑒 = 𝑉𝑐𝑥 − 𝑉𝑑𝑥 , 𝜔 𝑒 = 𝜔 𝑧 − 𝐶 𝑝 (𝑠) 𝑠¤, and 𝜁 𝑒 = 𝑠¤ − 𝑉𝑐 cos(𝛼𝑒 + 𝛽) so that 18 © 𝜂¤𝑒 ª © 𝑉¤𝑐𝑥 − 𝑉¤𝑑𝑥 ª ­ ® ­ ® ­𝜔¤ 𝑒 ® = ­ 𝜔¤ − 𝐶 (𝑠) ¥ 𝑠 − 𝑔 (𝑠) ¤ 𝑠 2 ® (2.29) ­ ® ­ 𝑧 𝑝 𝑐 ® ¤ ¤ ¤ ­ ® ­ ® 𝜁𝑒 𝑠¥ − 𝑉𝑐 cos(𝛼𝑒 + 𝛽) + 𝑉𝑐 sin(𝛼𝑒 + 𝛽)( 𝛼¤ 𝑒 + 𝛽) « ¬ « ¬ d𝐶 𝑝(𝑠) where 𝑔𝑐 (𝑠) = d𝑠 . Since we are interested in steering the robotic fish such that 𝑉𝑐𝑥 = 𝑉𝑑𝑥 , and 𝜔 𝑧 = 𝐶 𝑝 (𝑠) 𝑠¤, by doing a change of variables on Eqs. (2.8)-(2.10) using the above definitions, we can express the robotic fish dynamic equations in terms of the error velocity states. Note that in this work we assume 𝑉¤𝑑𝑥 = 0. The error state vector is then given by © 𝑋𝑒 ª ­ ® ­ 𝑌𝑒 ® ­ ® ­ ® ­ 𝛼𝑒 ® 𝛀𝑒 = ­­ ®® (2.30) ­ 𝜂𝑒 ® ­ ® ­𝜔 𝑒 ® ­ ® ­ ® 𝜁𝑒 « ¬ Since we have formulated the problem with respect to the error dynamics, and have shifted the equilibrium point of the dynamic equations, our control objective has become a stabilization problem for the resultant error dynamics. 2.2.2 Path-following Control Design To steer the robotic fish to the desired path, and drive the error state vector 𝛀𝑒 to zero, we utilize an NMPC scheme using the robot’s simplified averaged model. NMPC is an optimization-based method for feedback control of nonlinear systems, where the basic idea is to repeatedly solve a finite-horizon optimization problem subject to state and input constraints. At a given time 𝑡, measurements are obtained, and using a model of the process, the controller predicts the behavior of the system over a prediction horizon 𝑇𝑝 and then determines over the control horizon 𝑇𝑐 the input necessary to maximize the performance objective. The first part of the optimal control obtained is implemented until the next sampling instant, and then a new measurement is obtained and the process repeats [100]. 19 To design the controller, we consider the robot’s simplified averaged model in which the control represents functions of the actual control variables, namely, the tail-beat pattern parameters 𝛼0 , 𝛼𝑎 , and 𝜔𝛼 . By choosing the control in this manner, we allow the control inputs to appear linearly in the dynamic equation. In particular, we have chosen our control inputs as 3 3 𝑢 𝑓1 = 𝛼𝑎2 (3 − 𝛼02 − 𝛼𝑎2 ) (2.31) 2 8 𝑢 𝑓2 = 𝛼𝑎2 𝛼0 (2.32) which are present in functions 𝑓4 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) to 𝑓6 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) in Eqs. (2.8)-(2.10). To simplify discussion, it is assumed that the robotic fish uses a fixed tail-beat frequency 𝜔𝛼 . Furthermore, since the system dynamics are expressed in terms of the velocity errors states, by doing a change of variables we have essentially shifted the equilibrium point of the dynamical system, which means that there is also a shift on the control inputs 𝑢 𝑓1 and 𝑢 𝑓2 . Let 𝑢 2𝑠𝑠 and 𝑢 3𝑠𝑠 represent the shifted control values, which are defined as follows 6𝜌𝑠𝐶 𝐷 𝑉𝑑2 𝑥 𝑢 2𝑠𝑠 = (2.33) 𝐾 𝑓 𝑚𝐿 𝜔𝛼 2 2 4𝐾 𝐷 𝐶𝑃 (𝑠)𝑉𝑑2 𝑥 𝑢 3𝑠𝑠 = (2.34) 2 𝐾𝑚 𝑚𝐿 𝑐𝜔𝛼 2 In order to satisfy the condition that 𝛀 ¤ 𝑒 = 0 when 𝛀𝑒 = 0 and 𝒖 𝑒 = 0, we define the control inputs as follows ©𝑢 𝑒1 ª ©𝑠¥ − 𝑉¤𝑐 cos(𝛼𝑒 + 𝛽) + 𝑉𝑐 sin(𝛼𝑒 + 𝛽)( 𝛼¤ 𝑒 + 𝛽) ¤ª ­ ® ­ ® 𝒖 𝑒 = ­­𝑢 𝑒2 ®® = ­­ 𝑢 𝑓1 − 𝑢 2𝑠𝑠 ® ® (2.35) ­ ® ­ ® 𝑢 𝑒3 𝑢 𝑓2 − 𝑢 3𝑠𝑠 « ¬ « ¬ where 𝑢 𝑒1 is essentially 𝜁¤𝑒 as seen in (2.29). Since one is interested in steering the robotic fish to the desired path, we employ a stage cost that is a function of the error state vector 𝛀𝑒 . Furthermore, to minimize the control effort a weighting term on the control inputs is introduced. The following quadratic cost is chosen: 𝐹 (𝛀𝑒 , 𝒖 𝑒 ) = (𝛀𝑒 )𝑇 𝑄(𝛀𝑒 ) + (𝒖 𝑒 )𝑇 𝑅(𝒖 𝑒 ) (2.36) 20 where 𝑄 and 𝑅 are positive definite weighting matrices that penalize deviations from the desired values. Furthermore, to guarantee closed-loop stability and convergence, we utilize the terminal penalty, and the fictitious terminal control law 𝜋(𝛀𝑒 ) as proposed in [101], where a polytopic linear differential inclusion (PLDI)-based method is employed to obtain the weighting matrix 𝑄𝑇 for a terminal penalty of the following form 𝐸 (𝛀𝑒 (𝑡 + 𝑇𝑝 )) = (𝛀𝑒 (𝑡 + 𝑇𝑝 ))𝑇 𝑄𝑇 (𝛀𝑒 (𝑡 + 𝑇𝑝 )) (2.37) The reader is referred to [101] for details on how to obtain this weighting matrix. By solving the optimal control problem we obtain the optimal control sequence for 𝑢 𝑒1 , 𝑢 𝑒2 , and 𝑢 𝑒 . From 𝑢 𝑒 , we can obtain 𝜁¤𝑒 , and thus the state 𝜁 𝑒 from which we can then solve for 𝑠¤. 3 1 Furthermore, from 𝑢 𝑒2 and 𝑢 𝑒3 , along with Eqs.(2.31)-(2.34), one can solve for the actual robotic fish control variables 𝛼0 and 𝛼𝑎 . 2.2.3 Control Projection Given that the NMPC inputs, 𝑢 𝑒2 and 𝑢 𝑒3 , consist of functions of the actual robotic fish control variables 𝛼0 and 𝛼𝑎 , the NMPC input constraints are nonlinear in nature. As an illustration, Figure 2.3 plots the admissible control inputs in terms of 𝑢 𝑓1 and 𝑢 𝑓2 when the tail beat bias and amplitude have the following limits: 𝛼0min = −40◦ , 𝛼0max = 40◦ , 𝛼𝑎 min = 0◦ , 𝛼𝑎 max = 30◦ where 𝛼0min , 𝛼0max , 𝛼𝑎 min and 𝛼𝑎 max are the physical limits on the tail-beat bias and amplitude, respectively. Although NMPC is able to handle nonlinear control constraints, defining the constraints in this manner leads to an increase in computational time and complexity which in turn makes it challenging to implement in real time. It is thus desirable to define boxed-constraints since this can reduce significantly the complexity of the optimization problem and thus lower the computational time. 21 Figure 2.3: Illustration of the admissible input control set for robotic fish. One way of handling the irregular sector-shaped admissible control region shown in Figure 2.3, is to choose a rectangular area that lies inside this sector-shaped region as depicted by the light gray box in Figure 2.4. However, this deprives one of fully utilizing the admissible control. To overcome this problem, we propose to employ a projection method, where we define the NMPC boxed constraint to be such that it encompasses the admissible control region as depicted in Figure 2.5, and then project the computed values onto the true region depicted by the red sector-shaped section. Let 𝑧 denote a control point anywhere in this rectangular region, and let the sector-shaped set be represented by 𝑈. We can then project the point 𝑧 onto the convex set 𝑈 such that 𝑃𝑟𝑜 𝑗𝑈 (𝑧) , arg min k𝑧 − 𝑢k (2.38) 𝑢∈𝑈 Given that 𝑈 is convex, this problem is then well defined and 𝑃𝑟𝑜 𝑗𝑈 (𝑧) is unique. Instead of relying on an iterative optimization algorithm to determine the projected value, one can directly obtain an analytical solution that will simplify the projection and minimize computational complex- ity, as explained next. By taking advantage of the symmetry of the admissible control set, one can restrict the analysis to the left-half plane. To characterize the boundaries of the admissible control 22 Figure 2.4: Illustration of a rectangular section chosen from the admissible control set. Figure 2.5: Boxed section encompassing the admissible control set. region, we obtain the relationship between 𝑢 𝑓1 , 𝑢 𝑓2 , and 𝛼0 by solving for 𝛼𝑎 from Eq. (2.32) and then substituting that into Eq. (2.31). Similarly, by solving for 𝛼0 from Eq. (2.32), we can obtain an equation that captures the relationship between 𝑢 𝑓1 , 𝑢 𝑓2 , and 𝛼𝑎 .These equations are given as 23 follows 3 3 𝜒1 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼0 ) , −𝛼02 𝑢 𝑓1 + (3 − 𝛼02 )𝛼0 𝑢 𝑓2 − 𝑢 2𝑓 = 0 (2.39) 2 8 2 3 3 𝜒2 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼𝑎 ) , −𝛼𝑎2 𝑢 𝑓1 − 𝑢 2𝑓 + 3𝛼𝑎4 − 𝛼𝑎2 = 0 (2.40) 2 2 8 where Eq. (2.39) represents the left boundary when 𝛼0 = 𝛼0𝑚𝑖𝑛 and Eq. (2.40) represents the arc at the top when 𝛼𝑎 = 𝛼𝑎 max . To implement the projection scheme, the following cases are considered:   ( 𝐴) 𝜒1 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼0min ) ≤ 0 and 𝜒2 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼𝑎 max ) ≤ 0          (𝐵) 𝜒1 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼0min ) ≤ 0 and 𝜒2 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼𝑎 max ) > 0         (𝐶) 𝜒1 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼0min ) > 0 and 𝜒2 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼𝑎 max ) ≤ 0      (𝐷) 𝜒1 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼0min ) > 0 and 𝜒2 (𝑢 𝑓1 , 𝑢 𝑓2 , 𝛼𝑎 max ) > 0    For case ( 𝐴), the point to be projected is inside or on the boundary of the convex set 𝑈 and no projection is needed. For case (D), the point to be projected would be outside of the box encompassing the constraint set 𝑈 and thus does not need to be considered. For case (𝐵), the point to be projected is above the arc, and therefore 𝑃𝑟𝑜 𝑗𝑈 (𝑧) can be found by finding the minimum distance from the point 𝑧 to the arc described by Eq. (2.40). Let 𝑧 = ( 𝑝, 𝑞) and 𝑢 ∗ = (𝑢 ∗𝑓 , 𝑢 ∗𝑓 ) = 𝑃𝑟𝑜 𝑗𝑈 (𝑧). 1 2 ∗ ∗ The relationship between 𝑢 𝑓 and 𝑢 𝑓 is given by 1 2 1 −3 ∗ 2 3 𝑢 ∗𝑓 = ( 𝑢 𝑓 + 3𝛼𝑎4max − 𝛼𝑎6max ) (2.41) 1 𝛼𝑎2max 2 2 8 and the distance between 𝑧 and 𝑢 ∗ is then given by 𝑔2 (𝑢 ∗𝑓 ) =(𝑢 ∗𝑓 − 𝑝) 2 + (𝑢 ∗𝑓 − 𝑞) 2 2 2 1 1 −3 3 (2.42) =(𝑢 ∗𝑓 − 𝑝) 2 + ( ( 𝑢 ∗𝑓 2 + 3𝛼𝑎4max − 𝛼𝑎6max ) − 𝑞) 2 2 𝛼𝑎2 max 2 2 8 By taking the partial derivative of 𝑔2 (𝑢 ∗𝑓 ) with respect to 𝑢 ∗𝑓 , one obtains the following 2 2 𝜕𝑔(𝑢 ∗𝑓 ) 1 3 ∗ 3 −3 2 = 2(𝑢 ∗𝑓 − 𝑝) + 2( ( 𝑢 𝑓 + 3𝛼𝑎4 − 𝛼𝑎6 ) − 𝑞)(2( )) = 0 (2.43) 𝜕𝑢 ∗𝑓 2 𝛼𝑎2 2 2 8 2𝛼𝑎2 2 24 After simplifying Eq. (2.43), we obtain 9𝑢 ∗𝑓 3 18𝛼𝑎2 6𝑞 2 + (18 − − + 2)𝑢 ∗𝑓 − 2𝑝 = 0 (2.44) 𝛼𝑎4 8 2 𝛼𝑎 2 and by setting Eq (2.44) to zero, we can obtain a unique real root for 𝑢 ∗𝑓 that would minimize this 2 distance. Finally, 𝑢 ∗𝑓 is obtained with Eq. (2.41). 1 For case (𝐶), the point to be projected is below the left boundary. In this case, 𝑢 ∗𝑓 and 𝑢 ∗𝑓 are 1 2 related by 1 −3𝛼03 3 𝑢 ∗𝑓 = ( min ∗ 𝑢𝑓 + 3𝛼0min 𝑢 ∗𝑓 − 𝑢 ∗𝑓 2 ) (2.45) 1 𝛼02 2 2 2 8 2 min and the distance between 𝑧 ∗ and 𝑢 can be captured by: 𝑔3 (𝑢 ∗𝑓 ) =(𝑢 ∗𝑓 − 𝑝) 2 + (𝑢 ∗𝑓 − 𝑞) 2 2 2 1 −3𝛼02 3𝑢 ∗𝑓 3 (2.46) ∗ =(𝑢 𝑓 − 𝑝) + ( 2 min ∗ 𝑢𝑓 + 2 − 𝑢 ∗𝑓 2 − 𝑞) 2 2 2 2 𝛼0 min 8 2 By taking the partial derivative of 𝑔3 (𝑢 ∗𝑓 ) with respect to 𝑢 ∗𝑓 as follows: 2 2 𝜕𝑔3 (𝑢 ∗𝑓 ) −6 ∗ 6 ∗ 6 ∗ 2 −3 3 6𝑢 ∗𝑓 2 2 2 = 2(𝑢 ∗𝑓 − 𝑝) + ( 𝑢 𝑓 𝛼0 + 𝑢 𝑓 − 𝑢 − 𝑞)( + − ) = 0 (2.47) 𝜕𝑢 ∗𝑓 2 2 2 𝛼0 2 8𝛼2 𝑓2 2 𝛼0 8𝛼 2 2 0 0 and by setting it to zero, we can obtain a unique real root for 𝑢 ∗𝑓 , and consequently 𝑢 ∗𝑓 with 2 1 Eq. (2.45). 2.3 Simulation Results To evaluate the effectiveness of the designed controller, simulation was carried out using ACADO Model Predictive Control Toolkit. The parameters used (Table 2.1) were based on a robotic fish developed by Smart Microsystems Lab at Michigan State University. Furthermore, while the input constraints are the same as those presented in the experiment section, the parameters used to solve the optimization problem and implement the NMPC are as follows: Length of optimization horizon : 𝑇𝑐 = 𝑇𝑝 = 12 s Sampling interval : 𝑡 𝑠 = 1 s 25 Weighting matrix : 𝑄 = 𝑑𝑖𝑎𝑔(7, 7, 0.3, 1, 1, 7) Control weighting matrix : 𝑅 = 0.001𝑰3 𝑉𝑐 max = 0.04 m/sec 𝑠¤max = 0.04 m/sec 𝛼0min = −40◦ 𝛼0max = 40◦ 𝛼𝑎 min = 0◦ 𝛼𝑎 max = 30◦ where 𝑉𝑐 max is the maximum velocity the robotic fish can achieve, 𝑠¤𝑚𝑎𝑥 is the maximum speed the point 𝑠 can move along the path with, and 𝛼0min , 𝛼0max , 𝛼𝑎 min and 𝛼𝑎 max are the physical limits on the tail-beat bias and amplitude, respectively. Note that all of the following simulation was run with the same set of parameters and initial conditions. Furthermore, the terminal penalty weighting matrix was determined as described in Section 2.2.2. Though the controller was designed using the simplified averaged model, the simulation was performed using the original dynamic model. In other words, the model of the process was based on the simplified averaged dynamics as described by Eqs.(2.8)-(2.10), and the inputs obtained from solving the optimization problem were applied to the system described by Eqs. (2.1)-(2.3). We first considered the following path 𝑥𝑝 = 𝑠 (2.48) 𝑦𝑝 = 0 where 𝑥 𝑝 and 𝑦 𝑝 represent the position of the point P in the {I} frame. This path has a curvature of 𝑐 𝑝 (𝑠) = 0, and we chose to require the robotic fish to move with a constant velocity 𝑉𝑐 = 0.03 m/s. In Figures 2.6-2.8 we compare the desired path and the closed-loop trajectory of the robotic fish for three cases. In particular, Figure 2.6 shows the simulation results of the NMPC utilizing 26 Figure 2.6: Simulation: line-tracking results for NMPC with the proposed control projection method. the projection scheme, while Figure 2.7 shows the results for the case when no projection was employed and a boxed constraint within 𝑈 was chosen instead (as shown in Figure 2.4). Finally, Figure 2.8 shows the results when the nonlinear constraints for the set 𝑈 were directly defined. Note that in this work the blue dashed line represents the closed-loop trajectory of the robotic fish while the solid red line represents the desired path, and the arrowheads point in the direction of progression. Furthermore, the red diamond represents the starting position of the robotic fish, the green dot represents the starting point of the path, and the magenta box represents the imaginary boundaries of the fish tank. Figure 2.7: Simulation: line-tracking results for NMPC with boxed constraints inside the admis- sible sector-shaped control region. 27 Figure 2.8: Simulation: line-tracking results for NMPC with direct nonlinear control constraints. Similarly, we considered the following circular path 𝑥 𝑝 = 0.3 sin(𝑠) (2.49) 𝑦 𝑝 = 0.3 cos(𝑠) which has a constant curvature of 𝑐 𝑝 (𝑠) = 3.03. In Figures 2.9-2.11 we compare the desired path trajectory with those obtained by the robotic fish using the three aforementioned control schemes, respectively. Figure 2.9: Simulation: arc-tracking results for NMPC with the proposed control projection method. From the simulation results, one can see that the proposed NMPC scheme with projection outperforms the other two schemes in both line-tracking and arc-tracking cases; in particular, it results in smaller tracking error at the steady state. Compared with the case with boxed constraint within the set 𝑈, the proposed scheme offers larger control authority. The better tracking results from the proposed scheme compared to the case using direct, nonlinear constraints, however, were 28 Figure 2.10: Simulation: arc-tracking results for NMPC with boxed constraints inside the admis- sible sector-shaped control region. Figure 2.11: Simulation: arc-tracking results for NMPC with nonlinear control constraints. somewhat surprising. We conjecture that this is because the latter algorithm cannot reach an optimal solution within the allotted computing time. In particular, directly defining the nonlinear constraints requires the optimization algorithm to conduct more iterations in order to find the solution, which also makes it difficult to implement in real time. 2.4 Experimental Results In order to evaluate the effectiveness of the designed controller, experiments were carried out us- ing the robotic fish depicted in Figure 2.13. The robot consisted of a rigid-shell body and a relatively rigid tail, which were both 3D-printed. The tail was actuated using a Hitec digital micro waterproof servo (HS-5086WP), while a Microchip Digital Signal Processors and Controller (DSPIC30F6014) was used to control the tail actuation. Furthermore, an XBee-PRO module was used for commu- nication with a computer. Two Tenergy Li-Ion rechargeable batteries(7.4V, 3350mAh) were used 29 to power the robot. For the experiments, the robotic fish was run in an approximately 1.4 m by 0.8 m tank equipped with an overhead Logitech camera as seen in Figure 2.12. Furthermore, to obtain the robotic fish’s position and orientation in the tank, two markers were attached to the anterior and posterior of the robotic fish body. We then captured an overhead video of the robotic fish swimming in the tank using the camera, and utilized Visual C++ and the OpenCV library to implement an image processing algorithm. The algorithm detected the positions of the two markers and then used their average to obtain the center position of the robotic fish. The heading angle of the robot was estimated using the positions of the two markers. Additionally, the Kalman filter function in OpenCV was used to estimate the linear and angular velocities of the robot based on the measured position and heading. During every sampling time 𝑡 𝑠 , the OpenCV algorithm was used to obtain measurements for NMPC, which were then passed to the nonlinear optimization tool ACADO to solve the optimal control problem. In particular, we ran the software on a Surface Pro tablet with an Intel(R) Core(TM) i5 CPU @ 2.50 GHz with 4.0 GB of DDR3 RAM. Once the control inputs were calculated, the bias and amplitude values for the tail beat were obtained and then transmitted to the robotic fish wirelessly, and the process was repeated. 30 Camera Robotic Fish Figure 2.12: The experimental setup. Position Markers Tail Fin Figure 2.13: Tail-actuated robotic fish develop at Smart Microsystems Lab. 31 2.4.1 Model Parameter Identification Table 2.1: Identified parameters for the robotic fish used in this work. PARAMETER VALUE 𝑚𝑏 0.725 kg 𝑚 𝑎𝑥 -0.217 kg 𝑚 𝑎𝑦 -0.7888 kg 𝐽𝑏𝑧 2.66×10−3 kg · m2 𝐽𝑎𝑧 -7.93×10−4 kg · m2 𝐿 0.071 m 𝑑 0.04 m 𝑐 0.105 m 𝜌 1000 kg/m3 𝑆 0.03 m2 𝐶𝐷 0.97 𝐶𝐿 3.9047 𝐾𝐷 4.5 ×10−3 kg·m2 𝐾𝑓 0.7 𝐾𝑚 (averaged) 0.45 The robotic fish mass and tail fin dimensions were measured, the values of which are as shown in Table 2.1. Furthermore, the added masses, added inertia, and wetted surface were calculated based on a prolate spheroid approximation of the robotic fish body [102]. Identification of the hydrodynamic parameters (such as 𝐶 𝐷 , 𝐶 𝐿 , and 𝐾 𝐷 ) of the robotic fish model (2.8)-(2.16) typically requires extensive effort in fitting dynamic simulation data to experimental data by scanning the parameter space [35]. Furthermore, the determination of the scaling coefficients of 𝐾 𝑓 and 𝐾𝑚 requires scanning the parameter space for multiple sets of tail beat patterns and matching the simulated average model data to the simulated dynamic model [89], which is time-consuming. In this work, we propose an efficient and systematic way to identify the model parameters by exploiting the approximate, analytical relationship between the steady-state turning parameter (turning radius, turning period, etc.) and the model parameters established in [89]. With the assumption 𝑥 1  𝑥 2 , which is reasonable in general, we can obtain the unique equilibrium of the system (2.8)-(2.10), 32 under a given tail beat pattern, as v u u 3 3 𝐾 𝑓 𝑚𝐿 2 𝜔2𝛼 𝛼𝑎 (3 − 𝛼02 − 𝛼𝑎2 ) t 𝑥¯1 = 2 8 (2.50) 6𝜌𝑆𝐶 𝐷 v u t 6𝑚𝜌𝑆𝐶 𝐷 𝐿 2 𝜔2𝛼 𝛼𝑎2 𝛼02 u u 𝐾𝑓 𝑥¯2 = + 2𝜌𝑆(𝐶 𝐷 + 𝐶 𝐿 ) 3 3 𝐾 𝑓 (3 − 𝛼02 − 𝛼𝑎2 ) 2 8 (2.51) s 2𝑚 1 𝐾𝑚 𝑚𝐿 2 𝑐𝜔2𝛼 𝛼𝑎2 𝛼0 𝜌𝑆(𝐶 𝐷 + 𝐶 𝐿 ) 4𝐾 𝐷 s 𝐾𝑚 𝑚𝐿 2 𝑐𝜔2𝛼 𝛼𝑎2 𝛼0 𝑥¯3 = − (2.52) 4𝐾 𝐷 And the steady-state turning period 𝑇𝑝 (i.e., how long the robot takes to complete a full orbit), turning radius 𝑅, and angle of attack 𝛽 can be expressed as 𝑇𝑝 = 2𝜋/ 𝑥¯3 (2.53) q 𝑅 = ( 𝑥¯12 + 𝑥¯22 )/ 𝑥¯3 (2.54) 𝛽 = arctan( 𝑥¯2 /𝑥¯1 ) (2.55) Using Eqs. (2.50)-(2.55), we formulate the following algorithm to obtain the hydrodynamic coeffi- cients 𝐶 𝐷 , 𝐶 𝐿 and 𝐾 𝐷 , as well as the scaling coefficients 𝐾𝑚 and 𝐾 𝑓 for the averaged model. Let 𝐾𝑓 𝐾𝑓 𝑅1 = . By solving for the ratio from Eq. (2.50), we obtain 𝐶𝐷 𝐶𝐷 6𝜌𝑆 𝑥¯12 𝑅1 = (2.56) 3 3 𝑚𝐿 2 𝜔2𝛼 𝛼𝑎 (3 − 𝛼02 − 𝛼𝑎2 ) 2 8 Using the equation above, one can obtain the numerical value of the ratio 𝑅1 for a given set of tail beat parameters and the corresponding measured 𝑥¯1 . In particular, we found this ratio by averaging the different values obtained for each set of measurements. Furthermore, let 𝐾𝑚 = 𝐾0 + 𝐾1 𝛼0 . By 𝐾𝑚 solving for 𝐾 from Eq. (2.52), and using the definition 𝜃 0 = 𝐾0 /𝐾 𝐷 and 𝜃 1 = 𝐾1 /𝐾 𝐷 , one gets 𝐷 𝑥¯32 𝜃 0 + 𝛼0 𝜃 1 = (2.57) 𝑚𝐿 2 𝑐𝜔2𝛼 𝛼𝑎2 𝛼0 33 Using Eq. (2.57) one can then estimate the numerical values for 𝜃 0 and 𝜃 1 by utilizing, for example, the constrained linear least squares (𝑙𝑠𝑞𝑙𝑖𝑛) function in MATLAB, based on the tail beat parameters and the corresponding measurement of 𝑥¯3 for a set of experiments. 𝐾𝑚 By considering 𝑅1 and the ratio 𝐾 , we have reduced the number of parameters to be estimated 𝐷 𝐾𝑚 from 5 (𝐾 𝑓 , 𝐾𝑚 , 𝐶 𝐷 , 𝐶 𝐿 , 𝐾 𝐷 ) to 3 (𝑅1 , 𝐾 , 𝐶 𝐿 ). In order to obtain the particular values for 𝐶 𝐷 , 𝐷 𝐾 𝑓 , 𝐾𝑚 , and 𝐾 𝐷 , and to estimate the remaining parameter 𝐶 𝐿 , we utilize Eqs. (2.56) and (2.57) along with Eqs. (2.51) and (2.54). By letting 𝑐 0 = 𝐶 𝐷 + 𝐶 𝐿 and substituting 𝑅1 , 𝜃 0 and 𝜃 1 into (2.54), one obtains 𝑑2 2   2 𝐶 𝑅2 𝑥¯3 = 𝑥¯12 + 𝐷 𝑑1 + (2.58) 𝑐0 𝑐0 where v u t 6𝑚𝜌𝑆𝐿 2 𝜔2𝛼 𝛼𝑎2 𝛼02 u u 𝑑 1 = 𝑅1 (2.59) 3 3 𝑅1 (3 − 𝛼02 − 𝛼𝑎2 ) s 2 8 2𝑚 1 (𝜃 1 + 𝜃 2 𝛼0 )𝑚𝐿 2 𝑐𝜔2𝛼 𝛼𝑎2 𝛼0 𝑑2 = (2.60) 𝜌𝑆 4 Using Eq. (2.58) and letting 𝑐 1 = 𝑐1 , we can obtain the following: 0 𝑦 = 𝑐21 𝑑22 + 𝑑12𝐶 𝐷2 𝑐 2 + 2𝑐 𝑑 𝑑 𝐶 1 1 2 1 𝐷 (2.61) where 𝑦 = 𝑅 2 | 𝑥¯3 | 2 − 𝑥¯12 . Letting 𝜙1 = 𝑐21 , 𝜙2 = 𝐶 𝐷 2 𝑐 2 , and 𝜙 = 𝑐 2 𝐶 , one can rewrite the above 1 3 1 𝐷 expression as 𝑦 = 𝜙1 𝑑22 + 𝑑12 𝜙2 + 2𝑑2 𝑑1 𝜙3 (2.62) With a set of collected data, the parameters 𝜙1 through 𝜙3 can be estimated readily using techniques, such as the constrained linear-least square method. We can then solve for 𝐶 𝐷 , 𝐶 𝐿 and 𝐾 𝑓 using the definitions established above. Since the proposed estimation method only provides 𝐾𝑚 the ratio 𝐾 , to obtain the values for 𝐾𝑚 and 𝐾 𝐷 , we run simulations with the original dynamical 𝐷 model and choose 𝐾 𝐷 such that the angular velocity of the dynamic model matches that of the averaged model. 34 Table 2.2: Model validation results: relative model prediction error for turning radius and turning period, when the tail beats at 1.5 Hz. (𝛼𝑎 , 𝛼0 ) Turning Radius Error(%) Turning Period Error(%) (15◦ , 45◦ ) 11.23 8.80 (15◦ , 50◦ ) 7.57 11.65 (20◦ , 45◦ ) 8.46 3.82 (20◦ , 50◦ ) 8.74 12.55 (25◦ , 45◦ ) 2.90 0.97 (25◦ , 50◦ ) 11.07 15.88 For the implementation of the above parameter estimation scheme, we first ran experiments to obtain the steady-state turning radii and periods for different tail biases (0◦ , 25◦ , 40◦ ), and amplitudes (15◦ , 20◦ , 25◦ ) while holding the frequency at 1 Hz. The values obtained for the parameters 𝐾 𝑓 , 𝐾𝑚 , 𝐶 𝐷 , 𝐶 𝐿 and 𝐾 𝐷 are listed in Table 2.1. Furthermore, to validate the models we ran experiments with the same set of biases and amplitudes as previously stated while holding the frequency at 1.5 Hz. Table 2.2 lists the errors in turning radius and period between those obtained from experiments and those obtained from simulation using the parameters estimated above. The comparison indicates that the estimated model has acceptable accuracy. 2.4.2 Experimental Results on Path-Following The parameters used to solve the optimization problem and implement the NMPC were as follows: Length of optimization horizon : 𝑇𝑐 = 𝑇𝑝 = 7𝑠 Sampling interval: 𝑡 𝑠 = 1𝑠 Weighting matrix: 𝑄 = 0.9𝑰5 Control weighting matrix: 𝑅 = 0.001𝑰3 𝑉𝑐 max = 0.04 m/s 𝑠¤max = 0.04 m/s 35 𝛼0min = −40◦ 𝛼0max = 40◦ 𝛼𝑎 max = 30◦ 𝛼𝑎 min = 0◦ The following were the inputs constraints used for the case implementing projection: −1.810 ≤ 𝑢 𝑒1 ≤ 0.190       0  ≤ 𝑢 𝑓1 ≤ 0.794   (2.63)   −0.191 ≤ 𝑢 𝑓 2 ≤ 0.191   For the case using boxed constraints (without projection), we considered the following input constraints:  −1.81 ≤ 𝑢 𝑒1 ≤ 0.1900      0.366 ≤ 𝑢 ≤ 0.794   𝑓1  (2.64)   −0.115 ≤ 𝑢 𝑓 2 ≤ 0.115    We first considered the following path 𝑥𝑝 = 𝑠 (2.65) 𝑦𝑝 = 0 where 𝑥 𝑝 and 𝑦 𝑝 represent the position of the point P in the {I} frame. The desired velocity for the robotic fish was set to be 0.03 m/s. In Figures 2.14 and 2.15 we compare the desired path and the closed-loop robotic fish trajectory, obtained by using the NMPC with the proposed control projection scheme, and with a boxed constraint inside the nonlinear constraint set 𝑈, respectively. We do not report the case of NMPC with nonlinear constraints 𝑈 directly, because it could not be implemented in real time due to its long computation time. Figure 2.16 shows the computed physical inputs from solving the NMPC with the larger boxed constraint and their final values after the proposed projection. Furthermore, it shows the desired surge velocity 𝑉𝑑𝑥 versus the robot’s linear velocity’s 𝑉𝑐𝑥 . 36 Figure 2.14: Experiments: line-tracking results for NMPC with the proposed control projection method. Figure 2.15: Experiments: line-tracking results for NMPC with boxed constraints inside the admissible sector-shaped control region. Figure 2.16: Experiments: computed NMPC physical inputs and their projected values. 37 Similarly, we considered the following circular path 𝑥 𝑝 = 0.3 sin(𝑠) (2.66) 𝑦 𝑝 = 0.3 cos(𝑠) and Figure 2.17 and Figure 2.18 show the path-following results for NMPC with the proposed projection and for NMPC with boxed constraints inside 𝑈, respectively. Figure 2.17: Experiments: arc-tracking results for NMPC with the proposed control projection method. Figure 2.18: Experiments: arc-tracking results for NMPC with boxed constraints inside the admissible sector-shaped control region. Overall, the tracking results shown in Figures 2.14-2.18, one can see that, consistent with the simulation results, the proposed NMPC scheme with projection resulted in faster convergence to the desired path and smaller path error, due to the availability of larger control authority. 38 2.5 Chapter Summary In this chapter, we proposed and implemented in real time a path-following NMPC scheme for a tail-actuated robotic fish. A high-fidelity averaged nonlinear dynamic model was used for controller design. A parameter estimation scheme was employed to empirically identify the hydrodynamic parameters and scaling coefficients of the model. Furthermore, given that the control inputs were functions of two of the tail-beat parameters, specifically the tail bias and tail amplitude, a control projection strategy was implemented to handle these nonlinear input constraints and maximize the use of the admissible control region in a computationally efficient manner. Finally, simulation and experimental results demonstrated the effectiveness of the proposed scheme. 39 CHAPTER 3 BACKSTEPPING CONTROL-BASED TRAJECTORY TRACKING FOR TAIL-ACTUATED ROBOTIC FISH Although NMPC offered a systematic framework for control, its computational complexity poses challenges on implementation in resource-constrained robotic fish. Furthermore, the effect of the proposed projection scheme on stability is unclear and challenging to analyze. We are thus inspired to explore alternative control approaches that can handle input constraints and provide stability analysis of the closed-loop system. Backstepping-based control design presents a practical and systematic approach to the trajectory tracking problem as it is computationally inexpensive, especially when compared to methods such as nonlinear model predictive control (NMPC), and provides stability guarantees. In this chapter, we first consider the trajectory tracking control problem for a general class of autonomous under-actuated planar aquatic robots and then implement the proposed schemed on a tail-actuated robotic fish. In the case of under-actuated marine robotic systems confined to a horizontal plane, several groups have proposed nonlinear control methods to achieve tracking of desired trajectories. How- ever, some of these works have been limited to tracking a restrictive class of trajectories with reference angular velocities bigger than zero [103–112]. Others have only focused on regulating the position error while ignoring the vehicle’s orientation, which in some applications, e.g., in the case of the robotic fish, can often lead to unstable configurations where no proper control can be applied due to the actuator constraints [83, 113–120]. Several authors have designed backstepping and Lyapunov-based controllers that have achieved both position and orientation tracking [121–127]. However, these approaches suffer from the drawback that the position and orientation tracking controllers only allow the vehicles to track pre-specified feasible state-space trajectories that are generated using the model of the vehicle. In this work, we address the trajectory tracking control problem for autonomous under-actuated planar aquatic robots and vehicles. In particular, we present a systematic method to design a 40 backstepping-based continuous tracking controller that yields global ultimate boundedness of the position tracking errors and the orientation error. Furthermore, we demonstrate its effectiveness via simulation and experiments on a tail-actuated robotic fish. The control scheme proposed in this work was inspired by [93] (chapter 5), where the control of an under-actuated ship is considered. In particular, we utilize the idea of introducing an augmented error coordinate to address the under-actuation issue and stabilize the heading, lateral and longitudinal tracking errors. While [93] focuses on the dynamic model of a ship, in this work we consider a general class of nonlinear systems with under-actuated planar dynamics and input coupling where each input could affect multiple velocity states. Furthermore, unlike [93] and other work previously reported in literature we do not impose that the reference trajectory be generated from a nominal dynamics model, i.e., we do not require that the desired body-fixed velocities are generated from a virtual vehicle with the same dynamics, nor do we assume that the reference angular velocity is persistently exciting or decays to zero as in [93]. Another key difference between our work and [93] is that, while [93] uses a particular form for the augmented error state, we provide a systematic approach to the synthesis of this state that guarantees the global boundedness and convergence of the position and heading tracking errors to a neighborhood of the origin. We establish the theoretical results via multi-time-scale analysis of singularly perturbed systems. The proposed scheme is illustrated and validated via the trajectory tracking control of a tail- actuated robotic fish, as the latter is a good example of highly nonlinear, under-actuated robotic systems that exhibit input coupling [128]. By considering the cyclic nature of tail actuation, an averaged dynamic model for the robotic fish is used, where the bias and amplitude of the tail oscillation are treated as physical variables to be manipulated, while the tail beat frequency is fixed. Experimental results demonstrate the effectiveness of the proposed scheme and show its advantages over a well-tuned PI controller. The rest of the chapter is organized as follows. We first describe the dynamic model for the class of under-actuated robots considered in this work in Section 3.1. In Section 3.2, the problem 41 formulation for trajectory tracking is presented, followed by the backstepping control design. The closed-loop system stability analysis is shown in Section 3.3. In Section 3.4 the backstepping controller is designed for a robotic fish. In Section 3.5 simulation results are presented. Finally, in Section 3.6 the experimental set-up and experimental results for the robotic fish are discussed. Finally, we provide some concluding remarks in Section 3.7. 3.1 System Model Figure 3.1: Top view of an aquatic robotic system undergoing planar motion. Consider an under-actuated robotic system modeled as a 3-DOF rigid body with two inputs. Let {I} denote the inertial coordinate frame and {B} the body-fixed reference frame attached to the center of mass of the robot. In particular, let [X, Y, Z] 𝑇 and [ 𝑥, ˆ 𝑦ˆ , 𝑧ˆ] 𝑇 denote the inertial and the body-fixed coordinate systems, respectively, as illustrated in Figure 3.1. Assume that the robot undergoes planar motion such that its configuration ( 𝐵 𝑹 𝐼 , 𝒑) is an element of the Special Euclidean group 𝑆𝐸 (2): = 𝑆𝑂 (2) × R2 where 𝐵 𝑹 𝐼 ∈ R2×2 :{ 𝐵 𝑹 𝐼 𝐵 𝑹𝑇𝐼 = I2 , det( 𝐵 𝑹 𝐼 ) = +1} is a rotation matrix that maps the vehicle’s body coordinates into the inertial coordinates, and 𝒑 = [𝑥 𝑝 , 𝑦 𝑝 ] 𝑇 is the position of the origin of {B} in {I}. Let 𝒗 = [𝑣 1 𝑣 2 ] 𝑇 denote the body-fixed linear velocity, 42 where 𝑣 1 and 𝑣 2 denote the surge and sway velocities, respectively. The angular velocity expressed in the body-fixed coordinates is given by 𝜔 ∈ R. Furthermore, let 𝛽 denote the angle of attack, formed by the direction of 𝒗 with respect to the body-fixed 𝑥-axis, ˆ and let 𝜓 denote the heading angle, formed by the body-fixed 𝑥-axis ˆ relative to the inertial X-axis. The robot’s kinematics are thus expressed as 𝒑¤ = 𝐵 𝑹 𝐼 𝒗 (3.1a) 𝐵 𝑹¤ = 𝐵 𝑹 𝐼 𝑺(𝜔) (3.1b) 𝐼 where   𝐵𝑹 cos 𝜓 − sin 𝜓  𝐼 =    (3.2)  sin 𝜓 cos 𝜓     and 𝑺(·) is the skew-symmetric matrix defined as    0 −𝜔 𝑺(𝜔) =    (3.3) 𝜔 0     Let 𝜼 = [𝑣 1 𝑣 2 𝜔], and consider the class of under-actuated aquatic robot models with the following dynamic equations of motion: 𝜼¤ = 𝐻 (𝜼) + 𝐵𝑈 (3.4) where  ℎ1 (𝜼)       𝑏1 𝑏2        𝑢 1  𝐻 (𝜼) =  ℎ2 (𝜼)  , 𝐵 = 𝑏 3 𝑏 4  , 𝑈 =         𝑢 2   ℎ3 (𝜼)  𝑏 5 𝑏 6        𝑢 1 , 𝑢 2 represent the inputs, 𝑏 1 − 𝑏 6 are constants, and ℎ1 (·), ℎ2 (·), ℎ3 (·) can be nonlinear functions of the velocity states. Assumption 1 The dynamic system described by Eq. (3.4) is input-to-state stable. Assumption 2 The sway velocity is bounded such that |𝑣 2 | ≤ 𝜗¯ for some constant 𝜗¯ > 0. 43 Assumption 3 The submatrix    𝑏1 𝑏2  𝑏¯ =    (3.5) 𝑏 5 𝑏 6    is not singular. 3.2 Trajectory Tracking Control Algorithm 3.2.1 Trajectory Tracking Error Coordinates The trajectory tracking problem involves the design of a control law that enables the robot to track a desired reference trajectory in the inertial frame, i.e., a geometric path with a specified time law, such that the robot’s position and pose coincide with those of the desired trajectory. Figure 3.1 depicts the general idea. Consider a robot with dynamics given by Eq. (3.1) and (3.4), and let the vectors P̄(𝑡) and T̄(𝑡) denote the robot’s actual and desired position and pose with respect to the inertial frame {I} at a given time 𝑡, respectively, such that     𝑥 𝑝   𝑥𝑟      P̄ =  𝑦 𝑝  , T̄ =  𝑦𝑟    (3.6)     𝜓 𝜓 𝑟      For ease of presentation, the time-dependence notation is omitted for the remainder of the paper. Assume that the time evolution of T̄ is given by  𝑥¤𝑟  𝑣 𝑟 cos 𝜓𝑟      dT̄       =  𝑦¤ 𝑟  =  𝑣 𝑟 sin 𝜓𝑟  (3.7) d𝑡 𝜓¤ 𝑟   𝜔𝑟          where 𝑣 𝑟 and 𝜔𝑟 are some desired surge and angular velocities, respectively. Note that in this work, we are interested in aligning the robot’s heading angle 𝜓 with the velocity vector 𝒗 𝑟 = [ 𝑥¤𝑟 , 𝑦¤ 𝑟 ], 𝑦¤ 𝑟 which lies tangent to the reference trajectory, i.e., 𝜓𝑟 = arctan . 𝑥¤𝑟 𝑇 Furthermore, let 𝒆 = [𝑥 𝑒 𝑦 𝑒 𝜓 𝑒 ] denote the tracking error vector expressed in the body-fixed frame such that 𝐼 𝑹 02×1  𝐵 𝒆= ( P̄ − T̄) (3.8) 01×2 1 44 where 𝐼 𝑹 𝐵 denotes the rotation matrix from the inertial frame {I} to the body-fixed frame {B}. The derivative of 𝒆 (expressed in {B}) is given by 𝐼 𝑹 𝐼 𝑹 d𝒆 𝐵 02×1 𝐵 02×1 dP̄ dT̄    = 𝑺¯ ( P̄ − T̄) + − (3.9) d𝑡 01×2 1 01×2 1 d𝑡 d𝑡 where ! −𝑺(𝜔) 02×0 𝑺¯ = (3.10) 01×2 0 d𝒆 By solving for from Eq. (3.9), and by augmenting the error state model with the robot’s dt dynamics (3.4), one can obtain the following error state model  𝑥¤ 𝑒   𝑣 1 − 𝑣 𝑟 cos(𝜓 𝑒 ) + 𝜔𝑦 𝑒           𝑦¤ 𝑒   𝑣 + 𝑣 𝑟 sin(𝜓 𝑒 ) − 𝜔𝑥 𝑒     2  𝜓¤ 𝑒       𝜔 − 𝜔𝑟   =  (3.11)  𝑣¤ 1   ℎ1 (𝑣 1 , 𝑣 2 , 𝜔) + 𝑏 1 𝑢 1 + 𝑏 2 𝑢 2           𝑣¤   ℎ (𝑣 , 𝑣 , 𝜔) + 𝑏 𝑢 + 𝑏 𝑢   2  2 1 2 3 1 4 2      𝜔¤   ℎ3 (𝑣 1 , 𝑣 2 , 𝜔) + 𝑏 5 𝑢 1 + 𝑏 6 𝑢 2      Assumption 4 The reference velocities 𝑣 𝑟 and 𝜔𝑟 are bounded and differentiable with bounded derivatives 𝑣¤ 𝑟 and 𝜔¤ 𝑟 . In particular, we assume |𝜔𝑟 | ≤ 𝜔𝑟𝑚𝑎𝑥 and 0 < 𝑣 𝑟 ≤ 𝑣 𝑟𝑚𝑎𝑥 . By formulating the tracking problem in terms of the error dynamics, the trajectory tracking control problem has become a stabilization problem. In particular, the trajectory-tracking problem is to find a control law such that, for an arbitrary initial error, the states (𝑥 𝑒 , 𝑦 𝑒 , 𝜓 𝑒 ) of system (3.11) converges to a neighborhood of the origin. 3.2.2 Backstepping Control Design In this subsection we synthesize the backstepping-based controller such that the tracking error states, 𝑥 𝑒 , 𝑦 𝑒 , and 𝜓 𝑒 , are stabilized at a neighborhood of the origin. 45 Let 𝑒 = [𝑥 𝑒 , 𝑦 𝑒 , 𝜓 𝑒 ] 𝑇 such that (3.11) is expressed as 𝒆¤ =𝐹 (𝑡, 𝒆) + 𝐿(𝒆)𝜼 (3.12) 𝜼¤ =𝐻 (𝜼) + 𝐵𝑈 (3.13) where −𝑣 𝑟 cos(𝜓 𝑒 )   1 0 𝑦𝑒          𝐹 (𝑡, 𝒆) =  𝑣 𝑟 sin(𝜓 𝑒 )  , 𝐿 (𝒆) =  0 1 −𝑥 𝑒  (3.14)       −𝜔 𝑟    0 0 1    Note that the time dependence in the system above stems from the time-varying reference trajectory. From (3.12)-(3.13), one can see that this system can be viewed as a cascade connection of two subsystems, where the first subsystem is given by Eq. (3.12) with 𝜼 as input, and the second subsystem is given by Eq. (3.13) with 𝑈 as input. By treating 𝜼 as a virtual control input for the stabilization of 𝒆, the objective of backstepping control is to design a state feedback control law, such that 𝒆 → 0 as 𝑡 → ∞. Ideally one would seek a state feedback law 𝜼 = 𝜙(𝒆) such that the system 𝒆¤ = 𝐹 (𝑡, 𝒆) + 𝐿(𝒆)𝜙(𝒆) is asymptotically stable. However, although one could find a choice of 𝜙 that would stabilize 𝒆 at the origin, this would entail utilizing three virtual inputs (𝑣 1 , 𝑣 2 , 𝜔). Given the under-actuated nature of the dynamics of 𝜼, this would lead to an over-constrained problem as one would be attempting to manipulate three virtual inputs with only two actual inputs 𝑢 1 and 𝑢 2 . The under-actuation of the dynamics thus implies that only two virtual inputs are available to stabilize 𝒆. Alternatively, one could attempt to stabilize only two of the states at the origin, for example, 𝑥 𝑒 and 𝑦 𝑒 ; however, the third state, in this case, 𝜓 𝑒 , is not guaranteed to converge to the origin or even be bounded. The choice of virtual inputs should also consider practicality. For example, choosing the sway velocity 𝑣 2 as a virtual input is impractical in reality because either it can lead to unstable whirling of the robot or it is not directly actuated (when 𝑏 3 and 𝑏 4 are zero); therefore, the most viable virtual input choices are 𝑣 1 and 𝜔. It is natural to use 𝑣 1 to regulate the error in the longitudinal 46 axis (𝑥 𝑒 ), which leaves 𝜔 as a virtual input to drive the lateral (𝑦 𝑒 ) and heading angle (𝜓 𝑒 ) errors to the origin. The control scheme proposed in [93] for a ship, overcame a similar limitation by introducing a new error state that couples the lateral and heading angle errors. By stabilizing the longitudinal tracking error along with this new state, the authors were able to stabilize the whole error system at the origin. Inspired by this work, we propose a similar approach; however, whereas [93] only provided a specific form of this new error state, in this work, we provide a systematic approach to its design for a general class of under-actuated dynamics. In particular, we establish the sufficient conditions needed to achieve closed-loop stability and via time-scale analysis, we provide further insight into the structure that this new error state needs in order to achieve ultimate boundedness of the error state system. We define a new state, referred to as a “modified error” 𝑧 𝜉 , which is a function of 𝑦 𝑒 and 𝜓 𝑒 and couples them in such a way that the convergence of one implies the convergence of the other. This new state can be viewed intuitively as a “correction angle”, where essentially the heading angle error 𝜓 𝑒 is “corrected” with an angle that is correlated to the magnitude of the error 𝑦 𝑒 , such that it determines the rotation needed for the robot to point towards the desired trajectory. Following this intuition, let the “modified error” state be defined as 𝑧 𝜉 = 𝑞(𝑡, 𝑦 𝑒 , 𝜓 𝑒 ) = 𝑞 1 (𝑡, 𝑦 𝑒 ) + 𝑞 2 (𝑡, 𝜓 𝑒 ). Let 𝑞(·, ·, ·) satisfy the following assumption: Assumption 5 The function 𝑞(𝑡, 𝑦 𝑒 , 𝜓 𝑒 ) is continuously differentiable with respect to all its argu- ments, 𝑞 2 (𝑡, 𝜓 𝑒 ) is invertible with respect to 𝜓 𝑒 , the range of 𝑞 1 is bounded, and 𝑞(𝑡, 0, 0) = 0. Furthermore, write the time derivative of 𝑞(·, ·, ·) as d𝑞 𝜕𝑞 1 (𝑡, 𝑦 𝑒 ) 𝜕𝑞 (𝑡, 𝜓 𝑒 ) 𝜕𝑞(𝑡, 𝑦 𝑒 , 𝜓 𝑒 ) = (𝑣 2 + 𝑣 𝑟 sin(𝜓 𝑒 ) − 𝜔𝑥 𝑒 ) + 2 (𝜔 − 𝜔𝑟 ) + d𝑡 𝜕𝑦 𝑒 𝜕𝜓 𝑒 𝜕𝑡 d𝑞 =𝜔𝑘 (𝑡, 𝒆) + 𝑝(𝑡, 𝒆, 𝑣 2 ) d𝑡 𝜕𝑞 1 (𝑡, 𝑦 𝑒 ) 𝜕𝑞 (𝑡, 𝜓 𝑒 ) 𝜕𝑞(𝑡, 𝑦 𝑒 , 𝜓 𝑒 ) where 𝑝(𝑡, 𝒆, 𝑣 2 ) = (𝑣 2 + 𝑣 𝑟 sin(𝜓 𝑒 )) − 2 (𝜔𝑟 ) + , 𝜕𝑦 𝑒 𝜕𝜓 𝑒 𝜕𝑡 𝜕𝑞 2 (𝑡, 𝜓 𝑒 ) 𝜕𝑞 1 (𝑡, 𝑦 𝑒 ) 𝑘 (𝑡, 𝒆) = − 𝑥 𝑒 . It is assumed that 𝑘 (𝑡, 𝒆) ≠ 0 ∀ 𝑡 ≥ 0, ∀ 𝒆 ∈ 𝐷, in which 𝜕𝜓 𝑒 𝜕𝑦 𝑒 𝐷 ⊂ R𝑛 is a domain that contains the origin and the initial error state 𝑒(0). 47 Using this new error variable, the error system (3.12)-(3.13) is augmented to the following 𝒆¤ =𝐹 (𝑡, 𝒆) + 𝐿 (𝒆)𝜼 (3.15a) 𝑧¤𝜉 =𝑝(𝑡, 𝒆, 𝑣 2 ) + 𝜔𝑘 (𝑡, 𝒆) (3.15b) 𝜼¤ =𝐻 (𝜼) + 𝐵𝑈 (3.15c) The function 𝑞(·, ·, ·) should be defined such that driving 𝑧 𝜉 and 𝑥 𝑒 to a neighborhood of the origin guarantees that the error system 𝒆 converges to a neighborhood of the origin. The reason and mechanism to why this is true will be evident later with the time-scale analysis. To guarantee that the dynamics of 𝑥¤ 𝑒 and 𝑧¤𝜉 are asymptotically stable, a state feedback law 𝜙(·) needs to be designed. Let 𝜼 = 𝜙(·) = [𝜙1 0 𝜙3 ] 𝑇 , where 𝜙1 and 𝜙3 will be determined shorty. Let the candidate Lyapunov function for the 𝑥¤ 𝑒 , 𝑧¤𝜉 system be chosen as 1 1 𝑉 (𝑥 𝑒 , 𝑧 𝜉 ) = 𝑥 𝑒2 + 𝑧 2𝜉 (3.16) 2 2 The derivative of 𝑉 with respect to time is then given by 𝑉¤ (𝑥 𝑒 , 𝑧 𝜉 ) = 𝑥 𝑒 (−𝑣 𝑟 cos(𝜓 𝑒 ) + 𝜙1 + 𝑦 𝑒 𝜙3 ) + 𝑧 𝜉 (𝜙3 𝑘 (𝑡, 𝒆) + 𝑝(𝑡, 𝒆, 𝑣 2 )) (3.17) By choosing −𝑝(𝑡, 𝒆, 𝑣 2 ) − 𝐾 𝑧 𝜉 𝑧 𝜉 𝜙3 (𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) = (3.18a) 𝑘 (𝑡, 𝒆) 𝜙1 (𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) =𝑣 𝑟 cos(𝜓 𝑒 ) − 𝑦 𝑒 𝜙3 − 𝐾𝑥 𝑒 𝑥 𝑒 (3.18b) one can get 𝑉¤ (𝑥 𝑒 , 𝑧 𝜉 ) = −𝐾𝑥 𝑒 𝑥 𝑒2 − 𝐾 𝑧 𝜉 𝑧2𝜉 (3.19) where 𝐾𝑥 𝑒 > 0 and 𝐾 𝑧 𝜉 > 0 are tuning parameters. From the above it can be concluded that 𝑧 𝜉 and 𝑥 𝑒 will asymptotically converge to the origin so long as 𝜼 is identical to 𝜙. However, given that there is no direct control over the virtual inputs, another ‘step’ in the backstepping design must be performed such that the actual input 𝑈 48 appears. By adding and subtracting 𝐿 (𝒆)𝜙(𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) on the right hand side of Eq. (3.15a) and 𝑘 (𝑡, 𝒆)𝜙3 (𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) on the right hand side of Eq. (3.15b), one can obtain the following:   𝒆¤ = 𝐹 (𝑡, 𝒆) + 𝐿 (𝒆)𝜙(𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) + 𝐿 (𝒆) 𝜼 − 𝜙(𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) (3.20a)   𝑧¤𝜉 = 𝑝(𝑡, 𝒆, 𝑣 2 ) + 𝑘 (𝑡, 𝒆)𝜙3 + 𝑘 (𝑡, 𝒆) 𝜔 − 𝜙3 (3.20b) 𝜼¤ = 𝐻 (𝜼) + 𝐵𝑈 (3.20c) Let the virtual input error be defined as   𝑣 − 𝜙 + 𝑦 (𝜔 − 𝜙 )  𝑒 𝜂1   1 1 𝑒 3     𝒆 𝜂 = 𝑒 𝜂2  =  0          𝑒 𝜂   𝑘 (𝑡, 𝒆) 𝜔 − 𝜙3  (3.21)  3    ¯ 𝒆)(𝜼 − 𝜙(𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 )) =𝑄(𝑡, where 1 0   𝑦𝑒    ¯ 𝒆) = 0 0 𝑄(𝑡, 0  (3.22)     0 0 𝑘 (𝑡, 𝒆)    The time derivative of 𝒆 𝜂 is given by 𝑒¤𝜂1        ¯ 𝒆) 𝐻¯ (𝜼) + 𝐵𝑈 𝒆¤𝜂 = 𝑒¤𝜂2  = 𝑄(𝑡, ¯ +   𝑒¤𝜂   3     ¤̄ 𝒆) 𝜼 − 𝜙(𝑡, 𝒆, 𝑣 , 𝑧 ) − 𝑄(𝑡, 𝑄(𝑡, ¯ 𝒆) 𝜙(𝑡, ¤ 𝒆, 𝑣 2 , 𝑧 𝜉 ) (3.23) 2 𝜉 | {z } 𝜓(𝑡,𝒆,𝑣 2 ,𝑧 𝜉 ) ¯ 𝒆) 𝐻¯ (𝜼) + 𝑄(𝑡, = 𝑄(𝑡, ¯ 𝒆) 𝐵𝑈¯ + 𝜓(𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) 49 where  ℎ1 (𝜼)      𝑏 1 𝑏 2      𝐻¯ (𝜼) =  0  , 𝐵¯ =  0 0       ℎ3 (𝜼)  𝑏 5 𝑏 6      " #  𝐹 (𝑡, 𝒆) + 𝐿(𝒆)𝜼   𝜕𝜙1 𝜕𝜙1 𝜕𝜙1   𝜕𝜙1 𝜙¤1 (𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) = ℎ (𝜼) + 𝑏 𝑢 + 𝑏 4 2  + 𝜕𝑡 𝑢  (3.24) 𝜕𝒆 𝜕𝑣 2 𝜕𝑧 𝜉  2 3 1   𝜔𝑘 (𝑡, 𝒆) + 𝑝(𝑡, 𝒆, 𝑣 2 )    " #  𝐹 (𝑡, 𝒆) + 𝐿(𝒆)𝜼   𝜕𝜙3 𝜕𝜙3 𝜕𝜙3   𝜕𝜙3 𝜙¤3 (𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) = ℎ2 (𝜼) + 𝑏 3 𝑢 1 + 𝑏 4 𝑢 2  + 𝜕𝒆 𝜕𝑣 2 𝜕𝑧 𝜉    𝜕𝑡  𝜔𝑘 (𝑡, 𝒆) + 𝑝(𝑡, 𝒆, 𝑣 2 )    Furthermore, since 𝑣 1 and 𝜔 are the only virtual inputs to be directly manipulated, the sway velocity 𝑣 2 is an uncontrolled state, so we let it signify a perturbation to the system and define 0  0          𝜼¯ = 𝜂¯ = 𝑣 2  (3.25)     0  0      To ease the control synthesis and the analysis of the closed-loop system, using Eqs. (3.20)-(3.23) and (3.25), we rewrite the error system as 𝒆¤ = 𝐹 (𝑡, 𝒆) + 𝐿 (𝒆)𝜙(𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) + 𝐿(𝑡, ¯ 𝒆)𝒆 𝜂 + 𝜼¯ (3.26a) 𝑧¤𝜉 = 𝑝(𝑡, 𝒆, 𝜂) ¯ + 𝑘 (𝑡, 𝒆)𝜙3 + 𝑒 𝜂3 (3.26b) 𝒆¤𝜂 = 𝑄(𝑡, ¯ 𝒆) 𝐻¯ (𝜼) + 𝑄(𝑡,¯ 𝒆) 𝐵𝑈 ¯ + 𝜓(𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) (3.26c) 𝜂¤̄ = ℎ2 (𝜼) + 𝑏 3 𝑢 1 + 𝑏 4 𝑢 2 (3.26d) where 1 0 0    0 0 − 𝑥 𝑒    ¯ 𝒆) =  𝐿(𝑡, 𝑘 (𝑡, 𝒆)  (3.27)   0 0 1     𝑘 (𝑡, 𝒆)  When 𝒆 𝜂 is zero (implying the virtual inputs 𝑣 1 and 𝜔 equal 𝜙1 and 𝜙3 , respectively), 𝑥 𝑒 and 𝑧 𝜉 approach zero asymptotically as previously shown with the Lyapunov function 𝑉 (𝑥 𝑒 , 𝑧 𝜉 ). 50 We thus would like to drive 𝒆 𝜂 asymptotically to the origin. To ease the control design, let ¯ = ( 𝝁 − 𝜓(𝑡, 𝒆, 𝑣 2 .𝑧 𝜉 )), where 𝝁 ∈ R3×1 is the nominal control input defined as 𝑄¯ 𝐵𝑈    𝜇1    𝝁 =  0  (3.28)    𝜇2    Note that since the second element of 𝝁 as well as the second row of of the right hand side of (3.26c) are zero, a unique expression for 𝝁 can be obtained using the definition above. With (3.28), the error system (3.26) then becomes 𝒆¤ = 𝐹 (𝑡, 𝒆) + 𝐿 (𝒆)𝜙(𝑡, 𝒆, 𝑣 2 , 𝑧 𝜉 ) + 𝐿(𝑡, ¯ 𝒆)𝒆 𝜂 + 𝜼¯ (3.29a) 𝑧¤𝜉 = 𝑝(𝑡, 𝒆, 𝜂) ¯ + 𝑘 (𝑡, 𝒆)𝜙3 + 𝑒 𝜂3 (3.29b) 𝒆¤𝜂 = 𝑄(𝑡,¯ 𝒆) 𝐻¯ (𝜼) + 𝝁 (3.29c) 𝜂¤̄ = ℎ2 (𝜼) + 𝑏 3 𝑢 1 + 𝑏 4 𝑢 2 (3.29d) Considering the system composed of 𝑥 𝑒 , 𝑧 𝜉 , 𝒆 𝜂 , we define a new Lyapunov function 𝑉2 as 1 𝑉2 (𝑥 𝑒 , 𝑧 𝜉 , 𝒆 𝜂 ) = 𝑉 (𝑥 𝑒 , 𝑧 𝜉 ) + 𝒆𝑇𝜂 𝒆 𝜂 (3.30) 2 with its time derivative given by " # 𝜕𝑉 (𝑥 𝑒 , 𝑧 𝜉 , 𝒆 𝜂 ) 𝜕𝑉 (𝑥 𝑒 , 𝑧 𝜉 , 𝒆 𝜂 )   −𝑣 𝑟 cos(𝜓 𝑒 ) + 𝜙1 + 𝜙3 𝑦 𝑒  𝑉¤2 =   𝜕𝑥 𝑒 𝜕𝑧 𝜉  𝑝(𝑡, 𝒆, 𝜂) ¯ + 𝑘 (𝑡, 𝒆)𝜙3      " #  𝜕𝑉 (𝑥 𝑒 , 𝑧 𝜉 , 𝒆 𝜂 ) 𝜕𝑉 (𝑥 𝑒 , 𝑧 𝜉 , 𝒆 𝜂 ) 𝑒 𝜂1  (3.31) +   + 𝒆𝑇 𝒆¤𝜂 𝜂 𝜕𝑥 𝑒 𝜕𝑧 𝜉   𝑒 𝜂3    = − 𝐾𝑥 𝑒 𝑥 𝑒2 − 𝐾 𝑧 𝜉 𝑧2𝜉 + 𝑥 𝑒 𝑒 𝜂1 + 𝑧 𝜉 𝑒 𝜂3 + 𝒆𝑇𝜂 𝑄(𝑡, ¯ 𝒆) 𝐻¯ (𝜼) + 𝝁  Let 𝝁 = −𝑄(𝑡, ¯ 𝒆) 𝐻¯ (𝜼) − 𝐾 𝒆 𝒆 𝜂 (3.32) 𝜂 where    𝐾𝑒𝜂 0 0   1 𝐾 𝒆𝜂 =  0 0 0       0 0 𝐾𝑒𝜂    3 51 which yields 𝑉¤2 = −𝐾𝑥 𝑒 𝑥 𝑒2 − 𝐾 𝑧 𝜉 𝑧2𝜉 + 𝑥 𝑒 𝑒 𝜂1 + 𝑧 𝜉 𝑒 𝜂3 − (𝐾𝑒𝜂 𝑒 2𝜂 + 𝐾𝑒𝜂 𝑒 2𝜂 ) (3.33) 1 1 3 3 𝑒2 𝜂 𝑒 2𝜂 After adding and subtracting 4𝐾 1 and 4𝐾 3 and completing the square, one can arrive at 𝑥𝑒 𝑧𝜉  𝑒 𝜂1  2  𝑒 𝜂3  2 𝑉¤2 ≤ −𝐾𝑥 𝑒 𝑥 𝑒 − − 𝐾𝑧𝜉 𝑧𝜉 − 2𝐾𝑥 𝑒 2𝐾 𝑧 𝜉 (3.34) 2  1  2  1  − 𝑒𝜂 𝐾𝑒𝜂 − − 𝑒𝜂 𝐾𝑒𝜂 − 1 1 4𝐾𝑥 𝑒 3 3 4𝐾 𝑧 𝜉 If 𝐾𝑥 𝑒 > 0, 𝐾 𝑧 𝜉 > 0, 𝐾𝑒𝜂 > 4𝐾1 and 𝐾𝑒𝜂 > 4𝐾1 , then 𝑉¤2 < 0 unless 𝑥 𝑒 = 𝑧 𝜉 = 𝑒 𝜂1 = 𝑒 𝜂3 = 0. 1 𝑥𝑒 3 𝑧𝜉 From Lyapunov theory ( [129]), one can conclude the convergence of (𝑥 𝑒 , 𝑧 𝜉 , 𝑒 𝜂1 , 𝑒 𝜂3 ) to the origin. Finally, let 𝜓 = [𝜓1 0 𝜓2 ] 𝑇 such that the input 𝑈 is given by    𝜇1 − 𝜓1  𝑈 = 𝐶 (𝑡, 𝒆)    (3.35)  𝜇2 − 𝜓2     where 𝐶 (𝑡, 𝒆) is given by  𝑏6 (𝑏 2 + 𝑏 6 𝑦 𝑒 )   −  − (𝑡, 𝒆)(𝑏 − )   𝑏 𝑏 𝑏 𝑏 𝑘 𝑏 𝑏 𝑏 𝐶 (𝑡, 𝒆) =  1 6 𝑏 2 5 1 6 2 5  (3.36)  − 5 (𝑏 1 + 𝑏 5 𝑦 𝑒 )    𝑏 𝑏 −𝑏 𝑏  1 6 2 5 𝑘 (𝑡, 𝒆)(𝑏 𝑏 1 6 − 𝑏 𝑏 ) 2 5     Remark 1 Note that Assumption 3 guarantees that 𝑏 1 𝑏 6 − 𝑏 2 𝑏 5 is nonzero. Theorem 3.2.1 Consider the error dynamics given by (3.11). If the system dynamics satisfy Assumptions 1 and 3 and the desired trajectory satisfies Assumption 4, then the backstepping control law (3.35) guarantees the convergence of the longitudinal tracking error 𝑥 𝑒 and the modified error 𝑧 𝜉 to the origin. Furthermore, if the system dynamics satisfies Assumptions 2 , then (𝑥 𝑒 , 𝑦 𝑒 , 𝜓 𝑒 ) from the error system (3.11) are stabilized at a neighborhood of the origin and are ultimately bounded as k𝒆k ≤ max{𝛽(k𝒆(𝑡0 )k, 𝑡 − 𝑡0 ), 𝜌(𝜉)}, ∀ 𝑡 ≥ 𝑡0 (3.37) where 𝛽(k𝒆(𝑡0 )k, 𝑡 − 𝑡 0 ) is a class KL function, 𝜌(𝜉) is a class K function and 𝜉 is an upper bound to be determined. 52 Remark 2 Note that typically for any set of inputs 𝑢 1 and 𝑢 2 , |𝑣 2 | ≤ |𝑣 1 | is satisfied in the absence of external disturbances. This assumption is often made in literature and it is reasonable in practice as many aquatic robotic systems satisfy this condition when no disturbances are present. Relevant examples include robotic fish [89] and under-actuated ships/surface vessels [130]. Therefore, the disturbance that arises from 𝑣 2 is expected to be relatively small. Note that the backstepping controller guarantees the convergence of 𝑥 𝑒 and 𝑧 𝜉 to the origin. The following analysis in Section 3.3 shows how the rest of the error states 𝑦 𝑒 and 𝜓 𝑒 are stabilized. 3.3 Stability Analysis of the Closed-Loop System We first show how the error system (3.11) is stable at the origin under the assumption that the perturbation arising from the uncontrolled state 𝑣 2 is identically zero, that is 𝜂(0) ¯ = 0 and 𝜂¤̄ = 0. The case of 𝑣 2 not being constantly zero will be considered in a later part of the stability analysis. With the control law 𝝁 substituted into Eq. (3.26), the closed-loop system is obtained as follows: 𝑥 𝑒 𝑒 𝜂3 𝑦¤ 𝑒 = 𝑣 𝑟 sin(𝜓 𝑒 ) − 𝑥 𝑒 𝜙3 − (3.38a) 𝑘 (𝑡, 𝑦 𝑒 , 𝑧 𝜉 , 𝑥 𝑒 ) 𝑒 𝜂3 𝜓¤ 𝑒 = −𝜔𝑟 + 𝜙3 + (3.38b) 𝑘 (𝑡, 𝑦 𝑒 , 𝑧 𝜉 , 𝑥 𝑒 ) 𝑥¤ 𝑒 = −𝐾𝑥 𝑒 𝑥 𝑒 + 𝑒 𝜂1 (3.38c) 𝑧¤𝜉 = −𝐾 𝑧 𝜉 𝑧 𝜉 + 𝑒 𝜂3 (3.38d) 𝒆¤𝜂 = −𝐾 𝒆𝜂 𝒆 𝜂 (3.38e) 𝜂¤̄ = 0 (3.38f) Eq. (3.38) allows one to see the time-scale structure of the closed-loop system as it is evident that 𝒆 𝜂 , 𝑧 𝜉 , and 𝑥 𝑒 are “fast” converging states if 𝐾 𝒆𝜂 , 𝐾 𝑧 𝜉 and 𝐾𝑥 𝑒 are chosen adequately large. Inspired by this, we utilize singular perturbation methods for nonautonomous systems [131] to analyze the stability properties of the closed-loop system. 53 3.3.1 Asymptotic Stability of Multi-Time-Scale Systems: The Setup In general, multi-time-scale analysis involves the decomposition of a system into stages dictated by the separation of different time scales. In particular, the system is decomposed into a “slow” reduced model that dominates the system behavior and a “fast” boundary layer which is composed of the states that evolve in faster time scales and represent deviations from the slow system. Following this 1 intuition, let 𝜒 = [ 𝜒1 𝜒2 𝜒3 𝜒4 ] 𝑇 = [𝑧 𝜉 𝑥 𝑒 𝑒 𝜂1 𝑒 𝜂3 ] 𝑇 and 𝜖 = max(𝐾 𝑧 𝜉 , 𝐾𝑥 𝑒 , 𝐾𝑒𝜂 , 𝐾𝑒𝜂 ) 1 3 such that the closed-loop system (3.38), excluding (3.38b), is transformed into a two-time scale singular perturbation model of the following form: 𝑦¤ 𝑒 = 𝑓 (𝑡, 𝑦 𝑒 , 𝜒) (3.39) 𝜖 𝜒¤ = 𝑔(𝑡, 𝑦 𝑒 , 𝜒, 𝜖) (3.40) 𝜂¤̄ = 0 (3.41) where 𝜒2 𝜒4 𝑓 (𝑡, 𝑦 𝑒 , 𝜒) =𝑽𝑟 sin(𝜓 𝑒 ) − 𝜒2 𝜙3 − (3.42a) 𝑘 (𝑡, 𝑦 𝑒 , 𝜒) −𝑎¯ 𝜒1 + 𝜖 𝜒4      −𝑏¯ 𝜒 + 𝜖 𝜒  2 3 𝑔(𝑡, 𝜒, 𝜖) =  (3.42b)    −𝑐¯ 𝜒3     −𝑑¯𝜒4      −𝑎¯ 0 0    𝜖   𝜒1      0 −𝑏¯ 𝜖 0  𝜒    2 = (3.42c)    0  0 −𝑐¯ 0   𝜒3  0 0 0 −𝑑¯  𝜒4        | {z } 𝐴 𝐾𝑧 𝐾 𝑒 𝐾 𝜂1 where 𝑎¯ = max(𝐾 ,𝐾 ,𝐾 ,𝐾 ) , 𝑏¯ = max(𝐾 ,𝐾 𝑥,𝐾 𝜉 , 𝑐¯ = max(𝐾 ,𝐾 ,𝐾 , 𝑧 𝜉 𝑥 𝑒 𝑒 𝜂1 𝑒 𝜂3 𝑧 𝜉 𝑥 𝑒 𝑒 𝜂1 ,𝐾𝑒 𝜂3 ) 𝑧 𝜉 𝑥 𝑒 𝑒 𝜂1 ,𝐾𝑒 𝜂3 ) 𝐾𝜂 3 𝑑¯ = max(𝐾 𝑧 𝜉 ,𝐾𝑥 𝑒 ,𝐾 𝑒 𝜂 ,𝐾 𝑒 𝜂 ) , 𝑦 𝑒 is the slow variable, and 𝜒 is the fast variable. Furthermore, 1 3 consider 𝜖  1. Eqs. (3.39)-(3.40) represent the singularly perturbed full system, which for simplicity is denoted as the Υ𝑆𝐹 system. Similarly, the different sub-index combinations in Υ∗ 54 Figure 3.2: The bottom-up approach to the analysis of two-time-scale system. denote the different subsystems. For example, Υ𝑆 denotes the slow subsystem. Note that the state 𝜓 𝑒 has not been included in the perturbation model, since we exploit its relationship with the error state 𝑦 𝑒 and the new error coordinate 𝑧 𝜉 , and instead analyze the evolution of 𝑧 𝜉 . In other words, we utilize the relationship 𝑧 𝜉 = 𝑞 1 (𝑡, 𝑦 𝑒 ) + 𝑞 2 (𝑡, 𝜓 𝑒 ), which is enforced via the backstepping feedback law. By considering the singular perturbation parameter 𝜖, the Υ𝑆𝐹 system can be broken into a pair of slow and fast subsystems, and its time-scale properties can be analyzed in an ascending manner. Figure 3.2 illustrates this methodology. The standard two-time scale stability analysis is based on the following result. Lemma 1 ( [131]) Consider the singularly perturbed system 𝜁¤ (𝑡) = 𝑓 (𝑡, 𝜁, 𝛾, 𝜖), 𝜁 ∈ R𝑛 (3.43) 𝜖 𝛾¤ = 𝑔(𝑡, 𝜁, 𝛾, 𝜖), 𝛾 ∈ R𝑚 Assume that the following assumptions are satisfied for all (𝑡, 𝜁, 𝛾, 𝜖) ∈ [0, ∞) × 𝐵 𝜁 × 𝐵𝛾 × [0, 𝜖0 ], 𝐵 𝜁 ⊂ R𝑛 , 𝐵𝛾 ⊂ R𝑚 , and 0 < 𝜖0 ∈ R 1. 𝑓 (𝑡, 0, 0, 𝜖) and 𝑔(𝑡, 0, 0, 𝜖) = 0. 2. The equation 𝑔(𝑡, 𝜁, 𝛾, 0) = 0 has an isolated root 𝛾 = 𝐺 (𝑡, 𝜁), and there exists a class K function 𝑝 1 such that k𝐺 (𝑡, 𝜁)k ≤ 𝑝 1 (k𝜁 k) 55 3. The functions 𝑓 , 𝑔, and 𝐺 are smooth and their partial derivatives up to the second order are bounded for 𝛾 − 𝐺 (𝑡, 𝜁) ∈ 𝐵𝛾 . 4. The origin of the reduced order system 𝜁¤ (𝑡) = 𝑓 (𝑡, 𝜁, 𝐺 (𝑡, 𝜁), 0) is asymptotically stable; there exists a positive-definite Lyapunov function candidate 𝑉𝑅 that satisfies 0 < 𝑠1 (k𝜁 k) ≤ 𝑉𝑅 (𝑡, 𝜁) ≤ 𝑠2 (k𝜁 k) for some class K functions 𝑠1 (·) and 𝑠2 (·), and 𝜕𝑉𝑅 (𝑡, 𝜁) 𝜕𝑉𝑅 (𝑡, 𝜁) + 𝑓 (𝑡, 𝜁, 𝐺 (𝑡, 𝜁), 0) ≤ −𝛼10 Ψ2 (𝜁) 𝜕𝑡 𝜕𝜁 where 𝛼10 > 0 and Ψ(·) is a continuous scalar function of 𝜁 that vanishes only at 𝜁 = 0. 5. The origin of the boundary-layer system 𝑑𝛽 = 𝑔(𝑡, 𝜁, 𝛽 + 𝐺 (𝑡, 𝜁), 0) 𝑑𝜏 (𝑡 − 𝑡0 ) where 𝛽 = 𝛾 − 𝐺 (𝑡, 𝜁) and 𝜏 = , is asymptotically stable at the origin, uniformly in 𝜖 (𝑡, 𝜁); there exists a Lyapunov function candidate 𝑊 (𝑡, 𝜁, 𝛾), that satisfies 0 < 𝑠3 (k𝛾 − 𝐺 (𝑡, 𝜁)k) ≤ 𝑊 (𝑡, 𝜁, 𝛾) ≤ 𝑠4 (k𝛾 − 𝐺 (𝑡, 𝜁)k) for some class K functions 𝑠3 (·) and 𝑠4 (·), and that satisfies the following: 𝜕𝑊 (𝑡, 𝜁, 𝛾) 𝑔(𝑡, 𝜁, 𝛾, 0) ≤ −𝛼20 Φ2 (𝛾 − 𝐺 (𝑡, 𝜁)), 𝜕𝛾 where 𝛼20 > 0 and Φ(·) is a continuous function of an R𝑚 vector 𝑧 𝑓 which vanishes only at 𝑧 𝑓 = 0. 6. 𝑉𝑅 (𝑡, 𝜁) satisfies the following interconnection condition: 𝜕𝑉𝑅 [ 𝑓 (𝑡, 𝜁, 𝛾, 𝜖) − 𝑓 (𝑡, 𝜁, 𝐺 (𝑡, 𝜁), 0)] ≤ 𝛽01 Ψ(𝜁)Φ(𝛾 − 𝐺 (𝑡, 𝜁)) + 𝜖𝜆01 Ψ2 (𝜁) 𝜕𝜁 where the constants 𝛽01 and 𝜆01 are nonnegative. 56 7. 𝑊 (𝑡, 𝜁, 𝛾) satisfies the second and third interconnection conditions: 𝜕𝑊 [𝑔(𝑡, 𝜁, 𝛾, 𝜖) − 𝑔(𝑡, 𝜁, 𝐺 (𝑡, 𝜁), 0)] ≤ 𝜖𝜆02 Φ(𝛾 − 𝐺 (𝑡, 𝜁)) + 𝜖 𝛽02 Ψ(𝜁) 0Φ(𝛾 − 𝐺 (𝑡, 𝜁)) 𝜕𝛾 𝜕𝑊 𝜕𝑊 + 𝑓 (𝑡, 𝜁, 𝛾, 𝜖) ≤ 𝜆03 Φ2 (𝛾 − 𝐺 (𝑡, 𝜁)) + 𝛽03 Ψ(𝜁)Φ(𝛾 − 𝐺 (𝑡, 𝜁)) 𝜕𝑡 𝜕𝜁 where the constants 𝜆02 , 𝜆03 ,𝛽02 , and 𝛽03 are nonnegative. Then there exists 𝜖 ∗ > 0 such that for all 𝜖 < 𝜖 ∗ , the origin of (3.43) is uniformly asymptotically stable. Moreover, there exists a composite Lyapunov function defined as 𝜈(𝑡, 𝜁, 𝛾) = (1 − 𝑑 0)𝑉𝑅 (𝑡, 𝜁) + 𝑑 0𝑊 (𝑡, 𝜁, 𝛾), 0 < 𝑑0 < 1 such that  𝑇    Ψ(𝜁)   Ψ(𝜁)  ¤ 𝜁, 𝛾) ≤ −  𝜈(𝑡,  Λ  Φ(𝛾 − 𝐺 (𝑡, 𝜁))  Φ(𝛾 − 𝐺 (𝑡, 𝜁))         where   (1 − 𝑑 0)(𝛼0 − 𝜖𝜆0 ) − 1 (1 − 𝑑 0) 𝛽0 − 1 𝑑 0 (𝛽0 + 𝛽0 )  1 1 2 0 1 2 2 3  Λ=  (3.44)  1 1  − (1 − 𝑑 0) 𝛽0 − 𝑑 0 (𝛽0 + 𝛽0 ) 𝑑 0 − 𝜖 (𝜆0 + 𝜆0 )    1 2 2 𝛼 2 3 𝜖 2 2 3     which is positive definite for 𝛼10 𝛼20 def 𝜖< = 𝜖𝑑 (3.45) 0 0 0 0 0 1 0 0 0 0 0 2 𝛼1 (𝜆 2 + 𝜆 3 ) + 𝛼2𝜆1 + [(1 − 𝑑 ) 𝛽1 + 𝑑 (𝛽2 + 𝛽3 )] 4(1 − 𝑑 0)𝑑 0 The maximum of 𝜖 𝑑 is given by 𝛼10 𝛼20 𝜖 𝑑 | 𝑑0=𝛽0 /(𝛽0 +(𝛽0 +𝛽0 )) = 𝜖∗ = 0 0 1 1 2 3 𝛼1 (𝜆 2 + 𝜆03 ) + 𝛼20 𝜆01 + 𝛽01 (𝛽02 + 𝛽03 ) The Υ𝑆𝐹 subsystem is formed by the reduced-order slow-subsystem Υ𝑆 and the four-dimensional fast Υ𝐹 boundary layer. Let 𝐺 (𝑡, 𝑦 𝑒 ) denote the unique equilibrium point of 𝑔(𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 ), 0) when 𝜖 = 0, such that 0 = 𝑔(𝑡, 𝑦 𝑒 , 𝜒, 𝜖) → 𝜒 = 𝐺 (𝑡, 𝑦 𝑒 ) = 0 (3.46) By considering the stretched time scale defined by 𝜏1 = (𝑡 − 𝑡0 )/(𝜖), the slow model is given by the Υ𝑆 𝑦¤ 𝑒 = 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 )) = 𝑣 𝑟 sin(𝜓 𝑒 ) (3.47) 57 Subsystem Condition Assumption 4 of Lemma 1: Reduced System 𝜕𝑉𝑆 (𝑡, 𝑦 𝑒 ) 𝜕𝑉 (𝑡, 𝑦 𝑒 ) Υ𝑆𝐹 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 )), 0) + 𝑆 ≤ −𝛼1 Ψ21 𝜕𝑦 𝑒 𝜕𝑡 Assumption 5 of Lemma 1: Boundary-Layer 𝜕𝑉𝐹 (𝑡, 𝑥) Υ𝑆𝐹 𝑔(𝑡, 𝑦 𝑒 , 𝜒, 0) ≤ −𝛼2 Φ21 𝜕𝑥 Assumption 6 of Lemma 1: Interconnection Condition 1 𝜕𝑉𝑆 (𝑡, 𝑦 𝑒 )  Υ𝑆𝐹 [ 𝑓 (𝑡, 𝑦 𝑒 , 𝜒, 𝜖) − 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 ), 0)] ≤ 𝛽1 Ψ1 Φ1 + 𝜖𝜆 1 Ψ21 𝜕𝑦 𝑒 Assumption 7.a of Lemma 1: Interconnection Condition 2 𝜕𝑉𝐹 (𝑡, 𝜒) Υ𝑆𝐹 [𝑔(𝑡, 𝑦 𝑒 , 𝜒, 𝜖) − 𝑔(𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 ), 0)] ≤ 𝜖𝜆2 Φ1 + 𝜖 𝛽2 Ψ1 Φ1 𝜕𝑦 𝑒 Assumption 7.b of Lemma 1: Interconnection Condition 3 𝜕𝑉𝐹 (𝑡, 𝜒) 𝜕𝑉𝐹 (𝑡, 𝑥) Υ𝑆𝐹 𝑓 (𝑡, 𝑦 𝑒 , 𝜒, 𝜖) + ≤ 𝜆3 Φ21 + 𝛽3 Ψ1 Φ1 𝜕𝑦 𝑒 𝜕𝑡 Table 3.1: Comparison functions and inequalities that guarantee the asymptotic stability require- ments for the two-time scale system. while the fast model or boundary layer for the Σ𝑆𝐹 -subsystem is given by the Σ𝐹 -subsystem as −𝑎¯ 𝜒1      𝑑𝜒  − ¯ 𝑏 𝜒  2 = 𝑔(𝑡, 𝑦 𝑒 , 𝜒(𝜏1 ), 0) =  (3.48)   𝑑𝜏1  −𝑐¯ 𝜒3     ¯  −𝑑 𝜒4    Recall that one of the fast variables, 𝑧 𝜉 , denotes the “modified error” defined as 𝑧 𝜉 = 𝑞 1 (𝑡, 𝑦 𝑒 ) + 𝑞 2 (𝑡, 𝜓 𝑒 ). At this time-scale stage, 𝑧 𝜉 reaches equilibrium faster than 𝑦 𝑒 , i.e, 𝑧 𝜉 → 0 which implies that 𝑞 2 (𝑡, 𝜓 𝑒 ) → −𝑞 1 (𝑡, 𝑦 𝑒 ). Since 𝑞 2 (𝑡, 𝜓 𝑒 ) satisfies Assumption 5, this suggests that 𝜓 𝑒 → −𝑞 −1 2 (𝑞 1 (𝑡, 𝑦 𝑒 )), and implies that once 𝑧 𝜉 has reached equilibrium, 𝜓 𝑒 is forced to move in a manifold defined by 𝜓 𝑒 = −𝑞 −1 2 (𝑞 1 (𝑡, 𝑦 𝑒 )) (enforced by the backstepping controller). By considering that 𝜓 𝑒 is constrained to evolve on this manifold, Eq. (3.47) can thus be rewritten as 𝑦¤ 𝑒 = 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 )) (3.49a) = −𝑣 𝑟 sin(𝑞 −1 2 (𝑞 1 (𝑡, 𝑦 𝑒 ))) (3.49b) 58 One can then carry out the stability analysis on the subsystem composed of the slow variable 𝑦 𝑒 and the fast variable 𝑧 𝜉 with 𝜓 𝑒 subject to evolving on the manifold defined by −𝑞 −1 2 (𝑞 1 (𝑡, 𝑦 𝑒 )). Note this means that the convergence of 𝜓 𝑒 is implied once 𝑦 𝑒 converges. 3.3.2 Lyapunov Function Candidates and Two-Time-Scale Analysis As seen from Lemma 1, the asymptotic stability analysis requires the existence of Lyapunov functions for each of the subsystems. These Lyapunov functions, in turn, will be utilized to form a composite Lyapunov function for the complete Υ𝑆𝐹 system. To choose the Lyapunov functions at each stage, we consider each of the subsystems in a descending manner. In particular, we first consider the reduced-order slow Υ𝑆 -subsystem given by Eq. (3.49), and let 𝑉𝑆 (𝑡, 𝑦 𝑒 ) represent its Lyapunov function, where the exact structure of 𝑉𝑆 (𝑡, 𝑦 𝑒 ) is dependent upon the choice of the 𝑞 1 and 𝑞 2 functions. Note that 𝑉𝑆 (𝑡, 𝑦 𝑒 ) is allowed to depend on time. An example of a candidate Lyapunov function will be given briefly. On the other hand, for the corresponding fast Υ𝐹 -subsystem (3.48), a natural choice is 𝑉𝐹 ( 𝜒) = 𝜒𝑇 𝑃 𝜒 (3.50) where 𝑃 is a real, symmetric positive-definite matrix and the solution to the Lyapunov equation 𝑃 𝐴 + 𝐴𝑇 𝑃 = −𝑆 where 𝑆 = 𝑆𝑇 > 0. Note that since A is Hurwitz, 𝑃 is the unique solution of the Lyapunov equation (Theorem 3.7 [129]). Considering the above Lyapunov functions, one can then establish asymptotic stability of the origin by satisfying assumptions (1)-(7) of Lemma 1. Table 3.1 summarizes the equivalent assumptions by substituting the corresponding system dynamics. It can be verified that (0, 0) is an equilibrium of the system given by Eqs. (3.39)-(3.40). The time derivative of 𝑉𝑆 (𝑡, 𝑦 𝑒 ) is given 𝜕𝑉 𝜕𝑉 𝑉¤𝑆 (𝑡, 𝑦 𝑒 ) = − 𝑆 𝑣 𝑟 sin(𝑞 −1 2 (𝑞 1 (𝑡, 𝑦 𝑒 ))) + 𝑆 (3.51) 𝜕𝑦 𝑒 𝜕𝑡 Depending on the choice of 𝑞 1 and 𝑞 2 , 𝑉𝑆 (𝑡, 𝑦 𝑒 ) can be determined such that 𝑉¤𝑆 (𝑡, 𝑦 𝑒 ) is shown to be negative definite. In general, with the appropriate choice of 𝛼1 and Ψ1 , Assumption 4 of 59 Lemma 1 can be satisfied. Take for example 𝑉𝑆 (𝑦 𝑒 ) = (1 − cos(𝑞 −1 2 (𝑞 1 (𝑦 𝑒 ))), where 𝑞 1 and 𝑞 2 are chosen to be independent of 𝑡, in which case 𝜕 (𝑞 −1 2 (𝑞 1 (𝑦 𝑒 ))) 𝑉¤𝑆 (𝑦 𝑒 ) = −𝑣 𝑟 sin2 (𝑞 −12 (𝑞 1 (𝑦 𝑒 ))) (3.52) 𝜕𝑦 𝑒 As a simple choice, let 𝑞 2 = 𝐾𝑒 𝜓 𝑒 , with 𝐾𝑒 > 0, such that 𝑞 (𝑦 𝑒 ) 𝜕 (𝑞 1 (𝑦 𝑒 )) 𝑉¤𝑆 (𝑦 𝑒 ) = −𝑣 𝑟 sin2 ( 1 ) (3.53) 𝐾𝑒 𝐾𝑒 𝜕𝑦 𝑒 𝜕 (𝑞 1 (𝑦 𝑒 )) By choosing 𝑞 1 such that > 0, 𝑉¤𝑆 (𝑦 𝑒 ) can be made negative definite. From Assumption 𝜕𝑦 𝑒 5, 𝑞 1 must be chosen such that it is continuously differentiable and well defined for all real numbers 𝑦 of 𝑦 𝑒 and has a bounded range. arcsin( q 𝑒 ) is one viable option that would satisfy all the 1+𝑦 2𝑒 aforementioned assumptions. Note that arcsin(·) has a limited domain; however, its argument is 𝑦 𝑦 restricted since −1 ≤ q 𝑒 ≤ 1. Let 𝑞 1 = 𝐾 𝑑 arcsin( q 𝑒 ) such that 1+𝑦 2𝑒 1+𝑦 2𝑒 𝐾 𝐾 𝑦𝑒 1  𝑉¤𝑆 (𝑦 𝑒 ) = −𝑣 𝑟 𝑑 sin2 ( 𝑑 arcsin( q )) (3.54a) 2 1 + 𝑦 2𝑒 1 + 𝑦 𝑒 𝐾𝑒 𝐾𝑒 𝐾 𝐾 𝑦𝑒 1  ≤ −𝑣 𝑟𝑚𝑖𝑛 𝑑 sin2 ( 𝑑 arcsin( q )) (3.54b) 2 1 + 𝑦 2𝑒 1 + 𝑦 𝑒 𝐾𝑒 𝐾𝑒 where 𝐾 𝑑 > 0 can be thought of as a scale to determine the effect of the “correction” term. 𝜕𝑞 2 (𝑡, 𝜓 𝑒 ) 𝜕𝑞 1 (𝑡, 𝑦 𝑒 ) 𝐾 𝑥𝑒 𝐾𝑒 𝑘 (𝑡, 𝒆) = − 𝑥 𝑒 = 𝐾𝑒 − 𝑑 satisfies 𝑘 (𝑡, 𝒆) ≠ 0 ∀ 𝑡 ≥ 0, ∀ |𝑥 𝑒 | < 𝜕𝜓 𝑒 𝜕𝑦 𝑒 1 + 𝑦𝑒 2 𝐾𝑑 𝐾 . In this example, Assumption 4 of Lemma 1 can be satisfied with 𝛼1 ≥ 𝑣 𝑟𝑚𝑖𝑛 𝐾𝑑𝑒 and Ψ1 = 𝐾 1  sin2 ( 𝐾𝑒𝑑 arcsin( q 𝑒 )) 𝑦 . For another example we refer the reader to Section 3.4.3. 1+𝑦 2𝑒 1 + 𝑦 2𝑒 Using Eq. (3.50), it can be shown that by letting 𝛼2 ≥ 𝜆¯ 𝑚𝑖𝑛 (𝑆), Φ1 = k 𝜒k, where 𝜆¯ 𝑚𝑖𝑛 (𝑆) is the smallest eigenvalue of 𝑆, Assumption 5 of Lemma 1 can be satisfied. Furthermore, with the appropriate choice 𝛽1 and 𝜆 1 = 0, Assumption 6 of Lemma 1 can also be satisfied. Finally, with 𝜆 2 = 0, 𝛽2 = 0, 𝜆 3 = 0, and 𝛽3 = 0, Assumption 7.a and Assumption 7.b of Lemma 1 can be satisfied. A composite Lyapunov function for the Υ𝑆𝐹 -subsystem is given by 𝑉1 (𝑡, 𝑦 𝑒 , 𝜒) = (1 − 𝑑1 )𝑉𝑆 (𝑡, 𝑦 𝑒 ) + 𝑑1𝑉𝐹 ( 𝜒) (3.55) 60 with  𝑇    ≤ −𝜆(Λ1 ) Ψ1 (𝑦 𝑒 ) Ψ1 (𝑦 𝑒 )  Ψ1 (𝑦 𝑒 )  2 𝑉¤1 (𝑡, 𝑦 𝑒 , 𝜒) = −   Λ1   Φ1 ( 𝜒)   Φ1 ( 𝜒)  Φ1 ( 𝜒)        | {z } 𝜎2 where Λ is given by   (1 − 𝑑1 )𝛼1 1  − (1 − 𝑑 1 ) 𝛽 1  Λ=  1  2  − (1 − 𝑑 ) 𝛽 𝛼2   1 1 𝑑1  2   𝜖  and 𝜆(Λ) is its smallest eigenvalue of Λ. The stability of the two time-scale Υ𝑆𝐹 -subsystem is then guaranteed for 4𝑑1 𝛼1 𝛼2 def 𝜖< = 𝜖𝑑 (1 − 𝑑1 ) 𝛽12 3.3.2.1 Asymptotic Stability of the Closed-Loop Perturbed System Throughout the stability analysis done so far, it has been assumed that the perturbation 𝜂¯ that results from the uncontrolled sway velocity 𝑣 2 is identically zero. However, to complete the proof of Theorem 3.2.1 and show how the error system (3.11) is ultimately bounded we incorporate 𝑣 2 as a disturbance and treat the resultant closed-loop system as a disturbed system. Using the perturbed system theory as presented in [129] (Lemma 4.4), stability analysis can be carried out. Let the perturbed Υ𝑆𝐹 system be expressed as  𝑓 (𝑡, 𝚺)    𝚺¤ = 𝑄(𝑡, 𝚺) + 𝑟 (𝑡, 𝚺) =  𝑔(𝑡, 𝚺)  + 𝑟 (𝑡, 𝚺)   (3.56)    0    ¯𝑇 where 𝚺 = [𝑦 𝑒 , 𝜒, 𝜂]   𝜒2 (− 𝑝(𝑡, ¯ 𝑦 𝑒 , 𝜒) − 𝑘 𝑧 𝜉 𝜒1 ) 𝜒2 𝜒3   𝑓 (𝑡, 𝚺) =  𝑟  𝑣 sin(𝜓 𝑒 ) − −  (3.57)  𝑘 (𝑡, 𝑦 𝑒 , 𝜒) 𝑘 (𝑡, 𝑦 𝑒 , 𝜒)     −𝑎¯ 𝜒1 + 𝜖 𝜒4      −𝑏¯ 𝜒 + 𝜖 𝜒  2 3 𝑔(𝑡, 𝚺) =  (3.58)    −𝑐¯ 𝜒3     −𝑑¯𝜒4      61  − 𝜕𝑞 1 (𝑡, 𝑦 𝑒 ) 𝜂¯ 𝜒     2  𝜕𝑦 𝑒  + 𝜂¯  𝑘 (𝑡, 𝑦 𝑒 , 𝜒)         0  𝑟 (𝑡, 𝚺) =  (3.59)   0        0       0    ℎ2 (𝑣 1 , 𝜂, ¯ 𝜔) + 𝑏 3 𝑢 1 + 𝑏 4 𝑢 2      𝜕𝑞 (𝑡, 𝑦 𝑒 , 𝜒) 𝜕𝑞 (𝑡, 𝑦 𝑒 , 𝜒) 𝜕𝑞(𝑡, 𝑦 𝑒 , 𝜒) ¯ 𝑦 𝑒 , 𝜒1 , 𝜒2 ) = 1 𝑝(𝑡, 𝑣 𝑟 sin(𝜓 𝑒 ) − 2 𝜔𝑟 + (3.60) 𝜕𝑦 𝑒 𝜕𝜓 𝑒 𝜕𝑡 Note that in this manner the nominal system is represented by 𝑄(𝑡, 𝚺) while the perturbation is denoted by 𝑟 (𝑡, 𝚺). In the previous section it was shown that the nominal system given by 𝚺¤ = 𝑄(𝚺) is asymptotically stable. Furthermore, a composite Lyapunov function 𝑉1 (𝑡, 𝚺) for the full nominal system is found, for which it can be shown that Γ1 (k𝚺k) ≤ 𝑉1 (𝑡, 𝚺) ≤ Γ2 (k𝚺k), (3.61) 𝜕𝑉1 𝜕𝑉 𝜕𝑉1 𝑄(𝑡, 𝚺) + 1 ≤ −Γ3 (k𝚺k), ≤𝜍 𝜕𝚺 𝜕𝑡 𝜕𝚺 for all 𝚺 ∈ 𝐵 𝑅 , where 𝐵 𝑅 ⊂ R𝑛 and Γ𝑖 , 𝑖 = 1, 2, 3 are class K functions. Considering the same Lyapunov function 𝑉1 for the pertubed system, we can express the derivative of 𝑉1 along the trajectories of (3.56) as 𝜕𝑉1 𝜕𝑉1 𝜕𝑉 𝑉¤1 (𝑡, 𝚺) = + 𝑄(𝚺) + 1 𝑟 (𝑡, 𝚺) (3.62a) 𝜕𝑡 𝜕𝚺 𝜕𝚺 𝜕𝑉1 ≤ −Γ3 (k𝚺k) + 𝑟 (𝑡, 𝚺) (3.62b) 𝜕𝚺 Suppose that the perturbation term satisfies the bound 𝜃Γ3 (Γ−1 2 (Γ1 (𝑅))) k𝑟 (𝚺, 𝑡)k ≤ 𝜑 < (3.63) 𝜍 for all 𝑡 ≥ 0 and 𝚺 ∈ 𝐵 𝑅 , with some positive constant 𝜃 < 1. The derivative of 𝑉1 along the 62 trajectories of (3.56) becomes 𝜕𝑉1 𝜕𝑉 𝑉¤1 (𝑡, 𝚺) = 𝑄(𝑡, 𝚺) + 1 𝑟 (𝑡, 𝚺) (3.64a) 𝜕𝚺 𝜕𝚺 𝜕𝑉1 ≤ − Γ3 (k𝚺k) + k𝑟 (𝑡, 𝚺) k (3.64b) 𝜕𝚺 ≤ − Γ3 (k𝚺k) + 𝜍 𝜑 (3.64c) ≤ − (1 − 𝜃)Γ3 (k𝚺k) − 𝜃Γ3 (k𝚺k) + 𝜍 𝜑 (3.64d)  𝜍𝜑  ≤ − (1 − 𝜃)Γ3 (k𝚺k), ∀ k𝚺k ≥ Γ−1 3 (3.64e) 𝜃 Then for all 𝚺(𝑡0 ) ∈ {𝑉1 (𝚺) ≤ Γ1 (𝑅)}, the solution 𝚺(𝑡) of the perturbed system satisfies k𝚺k ≤ max{𝛽(k𝚺(𝑡 0 )k, 𝑡 − 𝑡0 ), 𝜌(𝜑)}, ∀ 𝑡 ≥ 𝑡0 (3.65) for some class KL functions 𝛽, and a class K function 𝜌 given by 𝜌(𝜑) = Γ−1 (Γ (Γ −1 ( 𝜑𝜍 ))) 1 2 3 𝜃 3.4 Backstepping Control of Robotic Fish To demonstrate the effectiveness of the proposed approach, the backstepping control algorithm is implemented in a robotic fish, as this system has highly nonlinear dynamics with input coupling and is under-actuated. 3.4.1 Dynamic Model of Robotic Fish The tail-actuated robotic fish is modeled as a rigid body with a rigid tail that is actuated at its base, and is considered to undergo planar motion [89]. The system has three degrees of freedom, namely surge, sway, and yaw. The tail deflection angle is assumed to have the following periodic pattern: 𝛼(𝑡) = 𝛼0 + 𝛼𝑎 sin(𝜔𝛼 𝑡) (3.66) 63 where 𝛼0 , 𝛼𝑎 , and 𝜔𝛼 represent the bias, amplitude, and frequency of the tail beat, respectively. The dynamics of the system are described with an average model [89] and is given by 𝑣¤ 1 = ℎ1 (𝑣 1 , 𝑣 2 , 𝜔) + 𝐾 𝑓 𝑐 7 𝑓¯4 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) (3.67a) 𝑣¤ 2 = ℎ2 (𝑣 1 , 𝑣 2 , 𝜔) + 𝐾 𝑓 𝑐 8 𝑓¯5 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) (3.67b) 𝜔¤ = ℎ3 (𝑣 1 , 𝑣 2 , 𝜔) + 𝐾𝑚 𝑐 9 𝑓¯6 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) (3.67c) with q  𝑚2 𝑐1 𝑣 1 𝑣 21 + 𝑣 22     ℎ1 (𝑣 1 , 𝑣 2 , 𝜔) = 𝑣 2 𝜔 −    𝑚 1 𝑚 1  q 𝑐2 𝑣 𝑣 2 𝑣 21 + 𝑣 22 arctan( 2 )  +        𝑚1 𝑣1  q 𝑚 𝑐 ℎ2 (𝑣 1 , 𝑣 2 , 𝜔) = − 1 𝑣 1 𝜔 − 1 𝑣 2 𝑣 21 + 𝑣 22        𝑚2 𝑚2   q 𝑐 𝑣 − 2 𝑣 1 𝑣 21 + 𝑣 22 arctan( 2 )     𝑚2 𝑣1 (3.68)   ℎ3 (𝑣 1 , 𝑣 2 , 𝜔) =(𝑚 1 − 𝑚 2 )𝑣 1 𝑣 2 − 𝑐 4 𝜔2 sgn(𝜔)        3 3 𝑓¯4 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) =𝜔2𝛼 𝛼𝑎 (3 − 𝛼02 − 𝛼𝑎2 )        2 8  𝑓¯5 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) =𝜔2𝛼 𝛼𝑎2 𝛼0          𝑓¯6 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) =𝜔2𝛼 𝛼𝑎2 𝛼0     where 𝑚 1 = 𝑚 𝑏 − 𝑚 𝑎 𝑥 , 𝑚 2 = 𝑚 𝑏 − 𝑚 𝑎 𝑦 , 𝐽3 = 𝐽𝑏𝑧 − 𝐽𝑎 𝑧 , 𝑐 1 = 21 𝜌𝑆𝐶 𝐷 , 𝑐 2 = 12 𝜌𝑆𝐶 𝐿 , 𝑐 4 = 𝐾𝐷 2 2 2 𝑚𝐿 , 𝑐 = 𝑚𝐿 , and 𝑐 = − c𝑚𝐿 . Here 𝑚 is the mass of the body, 𝐽 is the 𝐽3 , 𝑐 7 = 12𝑚 8 4𝑚 2 9 4𝐽3 𝑏 𝑏𝑧 1 inertia of the body about the body-fixed 𝑧-axis, 𝑚 𝑎 𝑥 and 𝑚 𝑎 𝑦 are the hydrodynamic derivatives that represent the added masses of the robotic fish along the body-fixed 𝑥 and 𝑦 directions, respectively, and 𝐽𝑎 𝑧 represents the added inertia effect of the body about the body-fixed z direction. 𝑆 denotes the reference surface area for the robot body, 𝐶 𝐷 , 𝐶 𝐿 and 𝐾 𝐷 represent the drag force coefficient, lift coefficient, and drag moment coefficient, respectively, 𝜌 is the density of water, 𝐿 is the tail length, 𝑐 is the distance from the body center to the pivot point of the actuated tail and 𝑚 represents the mass of water displaced by the tail per unit length and is approximated by 𝜋4 𝜌𝑑 2 with 𝑑 denoting the tail depth. 𝐾 𝑓 is a scaling constant, and 𝐾𝑚 is a scaling function affine in 𝛼0 . As done in [92], to further facilitate control design, in this work 𝐾𝑚 is considered as a constant by taking the average 64 of 𝐾𝑚 for a given range of 𝛼0 . Furthermore, let (𝑥 𝑝 , 𝑦 𝑝 ) denote the robot’s center of mass and 𝜓 the heading angle, formed by the body fixed axis relative to the inertial axis. The system’s kinematics are thus given by 𝑥¤ 𝑝 = 𝑣 1 cos 𝜓 − 𝑣 2 sin 𝜓 (3.69a) 𝑦¤ 𝑝 = 𝑣 1 sin 𝜓 + 𝑣 2 cos 𝜓 (3.69b) 𝜓¤ = 𝜔 (3.69c) The robotic fish dynamics satisfy Assumption 1, and Assumption 2, the proof for which can be found in Appendix A. 3.4.2 Backstepping Control Design To design the controller, the robot’s simplified averaged model presented above is considered. Here the control inputs are functions of the actual physical variables, namely the tail-beat pattern parameters 𝛼0 , 𝛼𝑎 , and 𝜔𝛼 . To simplify discussion, a fixed tail-beat frequency 𝜔𝛼 is assumed. The control inputs are then chosen as 3 3 𝑢 1 = 𝛼𝑎 (3 − 𝛼02 − 𝛼𝑎2 ) (3.70) 2 8 𝑢 2 = 𝛼𝑎2 𝛼0 (3.71) which are present in functions 𝑓¯4 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) to 𝑓¯6 (𝛼0 , 𝛼𝑎 , 𝜔𝛼 ) in Eqs. (3.67a)-(3.67c). Note that the control inputs defined this way appear linearly in the dynamic equations. In this manner, (3.67a)-(3.67c) thus resembles (3.4) with 𝑏 1 = 𝐾 𝑓 𝑐 7 𝜔2𝛼 , 𝑏 2 = 0, 𝑏 3 = 0, 𝑏 4 = 𝐾 𝑓 𝑐 8 𝜔2𝛼 , 𝑏 5 = 0 and 𝑏 6 = 𝐾𝑚 𝑐 9 𝜔2𝛼 . The modified error function 𝑞 for the robotic fish should be chosen such that the heading angle of the robot can be used to minimize 𝑦 𝑒 , but still allow tracking of 𝜓𝑟 when 𝑦 𝑒 is small enough. For simplicity, we choose 𝑞 2 = 𝜓 𝑒 , and consider the Lyapunov function 𝑉𝑆 proposed in Section 3.3.2 to determine possible choices for 𝑞 1 . In particular the derivative of 𝑉𝑆 with respect to time is 65 given by 𝜕 (𝑞 1 (𝑡, 𝑦 𝑒 )) 𝜕𝑉𝑆 𝑉¤𝑆 (𝑦 𝑒 ) = −𝑣 𝑟 sin2 (𝑞 −1 2 (𝑞 1 (𝑦 𝑒 ))) + (3.72) 𝜕𝑦 𝑒 𝜕𝑡 From the above, 𝑞 1 need to be chosen such that 𝑉¤𝑆 is negative definite. For simplicity, we let 𝑞 1 be only a function of 𝑦 𝑒 . Furthermore, taking into consideration Assumption 5, 𝑞 1 needs to be a continuously differentiable function that is well defined for all real numbers of 𝑦 𝑒 and has a bounded range. The function arctan(𝑦 𝑒 ) is an attractive choice since it satisfies all of the aforementioned 𝜕 (𝑞 1 (𝑡, 𝑦 𝑒 )) 1 assumptions. Furthermore, its derivative with respect to 𝑦 𝑒 , = , is positive for 𝜕𝑦 𝑒 1 + 𝑦 2𝑒 all 𝑦 𝑒 . The modified error for the robotic fish system is thus chosen as 𝑧 𝜉 = 𝑞(𝑦 𝑒 , 𝜓 𝑒 ) = 𝜓 𝑒 + 𝑞 2 (𝑦 𝑒 ) (3.73) where 𝑞 2 is given by 𝑞 2 (𝑦 𝑒 ) = 𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ) (3.74) where 𝑘 𝛿1 and 𝑘 𝛿2 are positive constants. In particular, 𝑘 𝛿1 can be thought of as a scale for the “correction” angle within the total error 𝑧 𝜉 , while 𝑘 𝛿2 tunes the magnitude of the angle. Note that 𝜕𝑞 2 (𝑡, 𝜓 𝑒 ) 𝜕𝑞 1 (𝑡, 𝑦 𝑒 ) 𝑘 𝛿1 𝑘 𝛿2 𝑥 𝑒 with this choice we also satisfy 𝑘 (𝑡, 𝒆) = − 𝑥𝑒 = 1 − such that 𝜕𝜓 𝑒 𝜕𝑦 𝑒 1 + (𝑘 𝛿2 𝑦 𝑒 ) 2 1 𝑘 (𝑡, 𝒆) ≠ 0 ∀ 𝑡 ≥ 0, ∀ |𝑥 𝑒 | < . 𝑘 𝑧𝛿 𝑘 𝑧𝛿 1 2 3.4.2.1 Control Synthesis The backstepping controller for the robotic fish can be designed by following the procedure from Section 3.2.2. First, to stabilize the (𝑥 𝑒 , 𝑧 𝜉 ) subsystem, the following candidate Lyapunov function is chosen 1 1 𝑉 = 𝑥 𝑒2 + 𝑧 2𝜉 (3.75) 2 2 Let 𝑣 1 = 𝜙1 and 𝜔 = 𝜙3 represent the virtual inputs, which will be chosen shortly. The time derivative of Eq. (3.75) is given by 𝑉¤ =𝑥 𝑒 𝑥¤ 𝑒 + 𝑧 𝜉 𝑧¤𝜉 (3.76) =𝑥 𝑒 (𝜙1 − 𝑽𝑟 cos(𝜓 𝑒 ) + 𝜙3 𝑦 𝑒 ) + 𝑧 𝜉 (𝑘 (𝑡, 𝒆)𝜙3 + 𝑝(𝑡, 𝒆, 𝑣 2 )) 66 where (𝑽𝑟 sin(𝜓 𝑒 ) + 𝑣 2 ) 𝑝(𝑡, 𝒆, 𝑣 2 ) = 𝑘 𝛿1 𝑘 𝛿2 − 𝜔𝑟 (3.77) (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 𝑘 𝛿1 𝑘 𝛿2 𝑥 𝑒 𝑘 (𝑡, 𝒆) = 1 − (3.78) (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 Let 𝜙1 = 𝑽𝑟 cos(𝜓 𝑒 ) − 𝜙3 𝑦 𝑒 − 𝐾𝑥 𝑒 𝑥 𝑒 (3.79) −𝑝(𝑡, 𝒆, 𝑣 2 ) − 𝐾 𝑧 𝜉 𝑧 𝜉 𝜙3 = (3.80) 𝑘 (𝑡, 𝒆) so that 𝑉¤ = − 𝐾𝑥 𝑒 𝑥 𝑒2 + −𝐾 𝑧 𝜉 𝑧 2𝜉 (3.81) Let the virtual input error be given by 𝑒 𝜂  𝑣 1 − 𝜙1 + 𝑦 𝑒 (𝜔 − 𝜙3 )       1 =     (3.82)    𝑘 (𝑡, 𝒆) 𝜔 − 𝜙  𝑒 𝜂3   3      and define a new Lyapunov function 1 1 𝑉2 = 𝑉 + 𝑒 2𝜂 + 𝑒 2𝜂 (3.83) 2 1 2 3 The time derivative of Eq. (3.83) is given by 𝑉¤2 = 𝑉¤1 + 𝑒 𝜂1 𝑒¤𝜂1 + 𝑒 𝜂3 𝑒¤𝜂3 (3.84) With Eqs. (3.67a)-(3.67c) along with the input definition Eq. (3.70)- (3.71), one can expand the 𝑣¤ 1 , 𝑣¤ 2 and 𝜔¤ terms that appear in Eq. (3.84). To make 𝑉¤2 negative definite, let 𝑢 1 and 𝑢 2 be chosen as " # " # −1 " # 𝑢1 𝐾 𝑓 𝑐 7 𝐾𝑚 𝑐 9𝑌𝑒 𝑇1 = (3.85) 𝑢2 0 𝜂 𝑇2 67 with 𝑇1 = − ℎ1 (·) + 𝑽¤𝑟 cos(𝜓 𝑒 ) − 𝑽𝑟 sin(𝜓 𝑒 ) 𝜓¤ 𝑒 (3.86) − ℎ3 (·)𝑦 𝑒 − 𝜔 𝑦¤ 𝑒 − 𝐾𝑥 𝑒 𝑥¤ 𝑒 − 𝐾𝑒𝜂 𝑒 𝜂1 1 𝑇2 = − ℎ3 (·)𝑘 (·) − 𝑘¤ (·)(𝜔 − 𝜙3 ) − 𝑘 (·)Δ − 𝐾𝑒𝜂 𝑒 𝜂3 (3.87) 3 ¤ 𝑥 2𝑥 𝑒 𝑦 𝑒 𝑦¤ 𝑒 𝑘¤ (·) = − 𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 + 𝑘 𝑧 𝛿1 𝑘 2𝑧 𝑒 (3.88) (𝑘 𝑧 𝛿2 𝑦 𝑒 ) 2 + 1) 𝛿2 ((𝑘 2 𝑧 𝛿2 𝑦 𝑒 ) + 1) 2 − 𝑝¤̂ − 𝐾 𝑧 𝜉 𝑧¤𝜉 −𝑝 − 𝐾 𝑧 𝜉 𝑧 𝜉 Δ =𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 − 𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 𝑘¤ (·) (3.89) 𝑘 (·) 𝑘 (·) 2 ℎ (·) + 𝑽¤𝑟 sin(𝜓 𝑒 ) + 𝑽𝑟 cos(𝜓 𝑒 ) 𝜓¤ 𝑒 2𝑦 𝑒 𝑦¤𝑒 (𝑽𝑟 sin(𝜓 𝑒 ) + 𝑣 2 ) 𝑝¤̂ =𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 2 − 𝑘 𝑧 𝛿1 𝑘 2𝑧 (3.90) 2 (𝑘 𝑧 𝛿2 𝑦 𝑒 ) + 1 𝛿2 ((𝑘 𝑧 𝛿2 𝑦 𝑒 ) 2 + 1) 2 𝐾 𝑓 𝑐8 𝜂 =𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 + 𝐾𝑚 𝑐 9 𝑘 (𝑡, 𝒆) (3.91) (𝑘 𝑧 𝛿2 𝑦 𝑒 ) 2 + 1 3.4.2.2 Control Synthesis Incorporating Input Constraints Given that robot fish actuators have physical limitations, the backstepping-based controller design should accommodate these constraints so that the control scheme can be successfully implemented. In this section, we present the control synthesis incorporating the input constraints. However, note that the time-scale stability analysis assumes that the nominal inputs are achievable.// The latter can be true given that the desired trajectory is feasible. In other words, when the desired trajectory is such that the nominal values will lie within the actuator constraints, then the achievable input will converge to the nominal input. In order to address magnitude constraints on the control inputs, we employ the following scheme inspired by [132] and [133]. Let 𝑣 1 and 𝑣 2 represent the nominal backstepping control inputs, and let 𝑢 1 and 𝑢 2 be the inputs that can be practically implemented. To obtain the value for 𝑢 1 and 𝑢 2 , first the values given by 𝑣 1 and 𝑣 2 are used to solve for the tail-beat parameters 𝛼0 , 𝛼𝑎 using Eqs. (3.70)-(3.71), and then the tail-beat parameter values are saturated such that they lie within the range [𝛼0𝑚𝑖𝑛 , 𝛼0𝑚𝑎𝑥 ], and [𝛼𝑎 𝑚𝑎𝑥 , 𝛼𝑎 𝑚𝑖𝑛 ]. Finally 𝑢 1 and 𝑢 2 are obtained using Eqs. (3.70)-(3.71) 68 with the saturated values. To analyze the influence of the input constraints, the following auxiliary system is chosen,  𝜆¤ 1 = −𝜁1 𝜆 1 + 𝜆 2         𝜆¤ 2 = −𝜁2 𝜆 2 + 𝐾 𝑓 𝑐 7 (𝑢 1 − 𝑣 1 ) + 𝑌𝑒 𝐾𝑚 𝑐 9 (𝑢 2 − 𝑣 2 )     (3.92) 𝜆¤ 3 = −𝜁3 𝜆 3 + 𝜆 4         𝜆¤ 4 = −𝜁4 𝜆 4 − 𝜂(𝑢 2 − 𝑣 2 )     The variables 𝜆 1 -𝜆 4 defined above represent the filtered effect of the non-achievable portion of the virtual and control inputs. In particular, the additional tracking error that arises because of mismatch between the nominal and implementable inputs is represent by 𝜆 1 and 𝜆3 , while 𝜆 2 and 𝜆4 represent the error propagated to the virtual inputs. As a result, the modified tracking errors are defined as follows: 𝑥¯ 𝑒 = 𝑥 𝑒 − 𝜆 1 (3.93) 𝑧¯𝜉 = 𝑧 𝜉 − 𝜆 3 (3.94) Furthermore, let the modified virtual errors be given as 𝑒¯𝜂1 = 𝑣 1 − 𝜙1 + 𝑦 𝑒 (𝜔 − 𝜙3 ) − 𝜆 2 (3.95) 𝑒¯𝜂3 = 𝑘 (𝑡, 𝒆)(𝜔 − 𝜙3 ) − 𝜆 4 (3.96) To stabilize the (𝑥¯ 𝑒 , 𝑧¯𝜉 ) subsystem, the following Lyapunov function is chosen 1 1 𝑉¯ = 𝑥¯ 𝑒2 + 𝑧¯2𝜉 (3.97) 2 2 With these new definitions, similar stability analysis as done in Eq. (3.76)-(3.81) is carried out. Let the virtual inputs be defined as 𝜙1 =𝑽𝑟 cos(𝜓 𝑒 ) − 𝜔𝑦 𝑒 − 𝐾𝑥 𝑒 𝑥¯ 𝑒 − 𝜁1𝜆 1 (3.98) −𝑝(𝑡, 𝒆, 𝑣 2 ) − 𝐾 𝑧 𝜉 𝑧¯𝜉 − 𝜁3𝜆 3 𝜙3 = (3.99) 𝑘 (𝑡, 𝒆) 69 so that 𝑉¤̄ = 𝑥¯ 𝑒 ( 𝑒¯𝜂1 − 𝐾𝑥 𝑒 𝑥¯ 𝑒 ) + 𝑧¯𝑒 ( 𝑒¯𝜂3 − 𝐾 𝑧 𝜉 𝑧¯𝜉 ) (3.100) Then a new Lyapunov function is defined as 1 1 𝑉¯2 = 𝑉¯ + 𝑒¯2𝜂 + 𝑒¯2𝜂 (3.101) 2 1 2 3 The time derivative of Eq. (3.101) is given by 𝑉¤̄2 = 𝑉¤̄ + 𝑒¯𝜂1 𝑒¤̄𝜂1 + 𝑒¯𝜂3 𝑒¤̄𝜂3 (3.102) As previously done, the terms 𝑣¤ 1 , 𝑣¤ 2 and 𝜔¤ that appear in Eq. (3.102) can be further expanded. After simplifying the above, 𝑣 1 and 𝑣 2 can be chosen as 𝐾 𝑓 𝑐 7 𝑣 1 + 𝐾𝑚 𝑐 9 𝑦 𝑒 𝑣 2 = − ℎ1 (·) + 𝑽¤𝑟 cos(𝜓 𝑒 ) − 𝑽𝑟 sin(𝜓 𝑒 ) 𝜓¤ 𝑒 − ℎ3 (·)𝑦 𝑒 − 𝜔 𝑧 𝑦¤ 𝑒 − 𝐾𝑥 𝑒 𝑥¤̄ 𝑒 + 𝜁12𝜆 1 − (𝜁1 + 𝜁2 )𝜆 2 − 𝐾𝑒𝜂 𝑒¯𝜂1 1 (3.103) 𝜂𝑣 2 = − ℎ3 (·)𝑘 (·) − 𝑘¤ (·)(𝜔 − 𝜙3 ) − 𝑘 (·) Δ̄ + 𝜁32𝜆 3 − (𝜁3 + 𝜁4 )𝜆 4 − 𝐾𝑒𝜂 𝑒¯𝜂3 (3.104) 3 where − 𝑝¤̂ − 𝐾 𝑧 𝜉 𝑧¤̄𝜉 −𝑝 − 𝐾 𝑧 𝜉 𝑧¯𝜉 Δ̄ =𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 − 𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 𝑘¤ (·) (3.105) 𝑘 (·) 𝑘 (·) 2 ℎ (·) + 𝑽¤𝑟 sin(𝜓 𝑒 ) + 𝑽𝑟 cos(𝜓 𝑒 ) 𝜓¤ 𝑒 2𝑦 𝑒 𝑦¤𝑒 (𝑽𝑟 sin(𝜓 𝑒 ) + 𝑣 2 ) 𝑝¤̂ =𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 2 − 𝑘 𝑧 𝛿1 𝑘 𝑧 𝛿2 (3.106) (𝑘 𝑧 𝛿2 𝑦 𝑒 ) 2 + 1 ((𝑘 𝑧 𝛿2 𝑦 𝑒 ) 2 + 1) 2 By following similar stability analysis as previously done for the constraints-free case, one can arrive at the following 1 1 1 1 𝑉¤̄2 = − 𝐾𝑥 𝑒 ( 𝑥¯ 𝑒 − 𝑒¯𝜂1 ) 2 − 𝐾 𝑧 𝜉 ( 𝑧¯𝜉 − 𝑒¯𝜂3 ) 2 − 𝑒¯2𝜂 (𝐾𝑒 𝜂 − ) − 𝑒¯2𝜂 (𝐾𝑒 𝜂 − ) 2𝐾𝑥 𝑒 2𝐾 𝑧 𝜉 1 1 4𝐾𝑥 𝑒 3 3 4𝐾 𝑧 𝜉 (3.107) If 𝐾𝑥 𝑒 > 0, 𝐾 𝑧 𝜉 > 0, 𝐾𝑒𝜂 > 4𝐾1 and 𝑘 𝑒𝜂 > 4𝐾1 , then 𝑉¤̄2 < 0 unless when 𝑥¯ 𝑒 = 𝑧¯𝜉 = 1 𝑥𝑒 3 𝑧𝜉 𝑒¯𝜂1 = 𝑒¯𝜂3 = 0, implying the convergence of ( 𝑥¯ 𝑒 , 𝑧¯𝜉 , 𝑒¯𝜂1 , 𝑒¯𝜂3 ) to zero as time approaches infinity. 70 Furthermore, since 0 ≤ 𝑉¯2 (𝑡) ≤ 𝑉¯2 (0), one can conclude that ( 𝑥¯ 𝑒 , 𝑧¯𝜉 , 𝑒¯𝜂1 , 𝑒¯𝜂3 ) are each in L2 . This shows that even when physical limitations do not allow the the desired control signals to be implemented, the quantities 𝑥¯ 𝑒 and 𝑧¯𝜉 do not diverge. In other words, this guarantees convergence for the compensated tracking errors 𝑥¯ 𝑒 and 𝑧¯𝜉 but not the actual tracking errors 𝑥 𝑒 and 𝑧 𝜉 . The latter may actually increase during periods when input limitations are in effect given that the desired control signal is not being implemented (i.e. 𝑢 1 ≠ 𝑣 1 and/or 𝑢 2 ≠ 𝑣 2 ). However, when the control signal limitations are not in effect, (i.e. 𝑢 1 = 𝑣 1 and 𝑢 2 = 𝑣 2 ), 𝜆 1 -𝜆 4 approach zero, and (𝑥¯ 𝑒 , 𝑧¯𝜉 ) converges towards (𝑥 𝑒 , 𝑧 𝜉 ). Once the control input limitatiosn are not in the effect, the time-scale analysis previously presented will apply. Finally the nominal inputs 𝑣 1 and 𝑣 2 can be obtained from (3.105). In the following section we establish the stability analysis for the robotic fish under the assump- tion that the nominal input values are achieved. 3.4.3 Stability Analysis of Closed-Loop Robotic Fish System Following the time-scale analysis presented in Section 3.3, it can be shown that by choosing 𝑉𝑆 (𝑦 𝑒 ) = (1 − cos(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ))) (3.108a) 𝑉𝐹 ( 𝜒) = 𝜒𝑇 𝑃 𝜒 (3.108b) and a composite Lyapunov function 𝑉1 (𝑡, 𝑦 𝑒 , 𝜒) = (1 − 𝑑1 )𝑉𝑆 (𝑡, 𝑦 𝑒 ) + 𝑑1𝑉𝐹 ( 𝜒) (3.109) then for all 𝚺(𝑡0 ) ∈ {𝑉1 (𝑡, 𝚺) ≤ Γ1 (𝑅)}, the solution 𝚺(𝑡) of the robotic fish perturbed system satisfies k𝚺k ≤ max{𝛽(k𝚺(𝑡 0 )k, 𝑡 − 𝑡0 ), 𝜌(𝜑)}, ∀ 𝑡 ≥ 𝑡 0 (3.110) where 𝜌 is given by r 𝜑𝜍  𝜌(𝜑) = 𝜃𝜆 𝑚𝑖𝑛 (Λ) 71 and r 𝑐 1 (1 − 𝑑1 )𝑣 𝑟𝑚𝑎𝑥 𝑘 𝛿1 𝑘 𝑧 𝛿 2 + (2𝑑1𝜆¯ 𝑚𝑎𝑥 ( 𝑝)) 2 k𝑅k  𝜍= 2 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 𝜗¯ 1 2 𝜑= k𝑅k 𝐷𝑒 (3.111) with 𝐷 𝑒 defined such that |𝑥 𝑒 |(1 + 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 ) |𝑥 𝑒 | 1 2 ≤ (3.112) (𝑘 𝑧 𝛿 𝑦 𝑒 )2 + 1 − 𝑘 𝑧 𝑒1 𝑘 𝑧 𝛿 𝑥 𝑒 𝐷𝑒 2 2 1 for |𝑥 𝑒 | < . The reader is referred to Appendix B for a detailed proof of the time-scale 𝑘 𝑧𝛿 𝑘 𝑧𝛿 1 2 analysis without perturbation and Appendix C for the stability analysis of the perturbed robotic fish system. 3.5 Simulation Results To evaluate the effectiveness of the designed controller, simulations were carried out using MATLAB. Furthermore, a PI controller was implemented to provide performance comparison. The robotic fish parameters used for simulation are listed Table 3.2. The tunable backstepping and PI controller parameters for the line trajectory (defined briefly) in the simulation were chosen as follows: 𝐾𝑥 𝑒 =1.1 𝐾 𝑧 𝜉 =1.2 𝐾𝑒𝜂 =0.69 𝐾𝑒𝜂 =0.41 1 3 𝜁1 =0.4 𝜁2 =0.8 𝜁3 =0.9 𝜁4 =0.9 𝑘 𝑧 𝛿1 =5 𝑘 𝑧 𝛿2 =63.3 𝐾 𝑃1 =3.5 𝐾 𝑃2 =0.85 𝐾 𝐼1 =0.021 𝐾 𝐼2 =0.023 𝛼0min =-50◦ 𝛼0max =50◦ 𝛼𝑎 min =0◦ 𝛼𝑎 max =30◦ 𝑡 𝑠 =0.66 s 𝜔𝛼 = 3𝜋 where 𝛼0min , 𝛼0max , 𝛼𝑎 min and 𝛼𝑎 max are the physical limits on the tail-beat bias and amplitude respectively. Furthermore, 𝐾 𝑃1 , 𝐾 𝑃2 , 𝐾 𝐼1 , and 𝐾 𝐼2 are the PI controller tunable parameters. The variable 𝑡 𝑠 is the sampling interval which pertains to the amount of time between an update to the control inputs. In this design, we chose 𝑡 𝑠 = 0.66 seconds given that the tail-beat frequency is 72 1.5 Hz. The controller parameters were chosen such that under the right values the backstepping controller was able to regulate the error system to the origin. We found that varying 𝜁1 − 𝜁4 controlled the convergence rate of the errors 𝜆 1 − 𝜆4 , which are a consequence of the effect of the input constraints. For the PI controller, its gain parameters were tuned carefully in simulation using Ziegler-Nichols’ closed-loop method [134]. Note that although the backstepping controller was designed using the simplified averaged model, the simulations were performed on the original dynamic model. The following line and circular trajectories were considered    𝑥¤𝑟 = 𝒗 𝑟 cos(𝜓𝑟 ), 𝑦¤ 𝑟 = 0, 𝜓¤ 𝑟 = 𝜔𝑟 , with 𝒗 𝑟 = 0.02, 𝜔𝑟 = 0 and       𝑥¤𝑟 = 𝒗 𝑟 cos(𝜓𝑟 ), 𝑦¤ 𝑟 = 𝒗 𝑟 sin(𝜓𝑟 ), 𝜓¤ 𝑟 = 𝜔𝑟 , with   (3.113)   𝒗 𝑟 = 𝑅1 |𝜔𝑟 |, 𝜔𝑟 = −0.09,     𝑅1 = 0.2,     where 𝑥¤𝑟 and 𝑌¤𝑟 represent the velocity of the trajectory in the {I} frame. Figures 3.3a-3.3b depict the desired and the closed-loop trajectories of the robotic fish for the backstepping-based and PI controller in both the line and circular cases. Note that the diamonds represents the starting position of the robotic fish, while the green circle represents the starting point of the path. In particular, 10 simulations trials were run for each type of trajectory with different initial conditions. Figures 3.4-3.5 illustrate the averaged magnitude of the position errors over time along with the corresponding standard deviations for line and circular tracking, respectively, for both the proposed backstepping scheme and the PI controller. Similarly, Figures 3.6-3.7 illustrate the averaged magnitude of the angle errors over time along with the corresponding standard deviations for line and circular tracking, respectively, for both the proposed backstepping scheme and the PI controller. From simulation results, in particular from Figures 3.4-3.7, one can see that, with the proposed backstepping scheme, smaller position tracking error in the allotted time is obtained, as well as a faster convergence to the desired trajectory. 73 0.8 PI Trajectory Backstepping Trajectory 0.6 Desired Trajectory Y(m) 0.4 0.2 0 0 0.5 1 1.5 X(m) (a) Line-tracking trajectories. 0.8 PI Trajectory Backstepping Trajectory Desired Trajectory 0.6 Y(m) 0.4 0.2 0 0 0.5 1 1.5 X(m) (b) Circle-tracking trajectories. Figure 3.3: Simulation: line and circle-tracking trajectory results, respectively, for backstepping- based and PI control. Backstepping Control STD of Backstepping Control PI Control 0.3 Avg. Error (m) STD of PI Control 0.2 0.1 0 0 5 10 15 20 25 time(s) Figure 3.4: Simulation: line-tracking position error for backstepping-based and PI control. “STD” curves represent the standard deviation envelopes for the average error under each con- troller. 74 Backstepping Control STD of Backstepping Control 0.15 PI Control Avg. Error (m) STD of PI Control 0.1 0.05 0 0 10 20 30 40 50 time(s) Figure 3.5: Simulation: circle-tracking position error for backstepping-based and PI control. “STD” curves represent the standard deviation envelopes for the average error under each controller. Figure 3.6: Simulation: line-tracking angle error for backstepping-based and PI control. “STD” curves represent the standard deviation envelopes for the average error under each controller. Figure 3.7: Simulation: circle-tracking angle error for backstepping-based and PI control. “STD” curves represent the standard deviation envelopes for the average error under each controller. 75 Table 3.2: Parameters of the robotic fish. PARAMETER VALUE PARAMETER VALUE 𝑚𝑏 0.725 kg 𝑚 𝑎𝑥 -0.217 kg 𝑚 𝑎𝑦 -0.7888 kg 𝑐 0.105 m 𝐽𝑎𝑧 -7.93×10−4 kg · m2 𝐿 0.071 m 𝐽𝑏𝑧 2.66×10−3 kg · m2 𝑑 0.04 m 𝜌 1000 kg/m3 𝑆 0.03 m2 𝐶𝐷 0.97 𝐶𝐿 3.9047 𝐾𝐷 4.5 ×10−3 kg·m2 𝐾𝑓 0.7 𝐾𝑚 (averaged) 0.45 . 3.6 Experimental Results In order to evaluate the validity of the proposed scheme, experiments were carried out using the robotic fish presented in the previous Chapter (Figure 2.13). Furthermore, a similar experimental set up was utilized (Figure. 2.12). Finally, the parameters for the robotic fish are as shown in Table 3.2. To demonstrate the effectiveness of the proposed approach, experiments were carried out to compare the performance of the backstepping-based controller with that of a proportional-integral (PI) controller in terms of tracking performance. It is important to emphasize that the robotic fish has actuator constraints that must be considered when implementing the controller. To address this, the backstepping controller was modified as shown in Section 3.4.2.2. Furthermore, the inputs were saturated in a similar fashion as described in Section 3.5. In the case of the PI controller, the original calculated values for the tail-beat parameters were saturated to obtain the realizable values and thus the viable inputs. The tunable controller parameters were chosen as follows: 𝐾𝑥 𝑒 = 0.6 𝐾 𝑍𝜉 = 0.6 𝐾𝑒𝜂 = 1.02 𝐾𝑒𝜂 = 1.02 1 2 𝜁1 = 0.6 𝜁2 = 0.6 𝜁3 = 0.5 𝜁4 = 0.5 𝑘 𝛿1 = 1 𝑘 𝛿2 = 1.6 𝐾 𝑃1 =0.25 𝐾 𝑃2 = 0.31 𝐾 𝐼1 = 0.01 𝐾 𝐼2 = 0.0071 𝛼0min =-50◦ 𝛼0max = 50◦ 𝛼𝑎 min = 0◦ 𝛼𝑎 max = 30◦ 𝑡 𝑠 = 0.66 s 76 where 𝛼0min ,𝛼0max , 𝛼𝑎 min and 𝛼𝑎 max are the physical limits on the tail-beat bias and amplitude respectively. Furthermore, 𝐾 𝑃1 , 𝐾 𝑃2 , 𝐾 𝐼1 , and 𝐾 𝐼2 are the PI controller tunable parameters. 𝑡 𝑠 is the sampling interval that pertains to the amount of time between an update to the control inputs. In this design, we chose 𝑡 𝑠 = 0.66 seconds given that the tail-beat frequency is 1.5 Hz. Finally, 𝜁1 -𝜁4 are the controller parameters used to handle the input constraints. The PI gain parameters were tuned in experiments using Ziegler-Nichols’ closed-loop method [134]. The following line and circular trajectories were considered for the tracking tasks    𝑥¤𝑟 = 𝒗 1𝑟 cos(𝜓𝑟 ), 𝑦¤ 𝑟 = 0, 𝜓¤ 𝑟 = 𝜔𝑟 , with 𝒗 1𝑟 = 0.02, 𝜔𝑟 = 0 and       𝑥¤𝑟 = 𝒗 1𝑟 cos(𝜓𝑟 ), 𝑦¤ 𝑟 = 𝒗 1𝑟 sin(𝜓𝑟 ), 𝜓¤ 𝑟 = 𝜔𝑟 , with   (3.114)   𝒗 1𝑟 = 𝑅1 |𝜔𝑟 |, 𝜔𝑟 = −0.09,     𝑅1 = 0.2,     In particular, ten different trials with similar initial conditions were performed for both the backstepping and PI controllers. In Figure 3.8, the desired and the closed-loop robotic fish trajectories for all ten trials are depicted for both the line and circular trajectories, while in Figure 3.9 the tracking results for only one of the trials is depicted. In particular, Figure 3.9a illustrates the tracking results for backstepping-based control, and Figure 3.9b illustrates those obtained for PI control. In both figures the yellow diamond depicts the starting position of the robot while the green circle depicts the start of the trajectory. Furthermore, the blue solid line depicts the desired trajectory while the red solind like depicts that of the robotic fish. Finally, the arrowheads on the lines denote the direction of progression. In addition, Figure 3.10 depicts the averaged position tracking error along with its corresponding standard deviation for both control schemes in each trajectory case. In particular, the solid blue line with circular markers represents the averaged tracking error obtained for backstepping-based control, while the dotted blue lines represent its standard deviation. Similarly, the solid magenta line represents the averaged error obtained for PI control, while the dotted-dashed magenta lines represent its standard deviation. Similarly, Figure 3.11 depicts the averaged angle tracking error along with its corresponding standard deviation for both control schemes in each trajectory case. 77 0.8 PI Trajectory Backstepping Trajectory Desired Trajectory 0.6 Y(m) 0.4 0.2 0 0 0.5 1 1.5 X(m) (a) Line-tracking trajectories. 0.8 PI Trajectory Backstepping Trajectory Desired Trajectory 0.6 Y(m) 0.4 0.2 0 0 0.5 X(m) 1 1.5 (b) Circle-tracking trajectories. Figure 3.8: Experiments: line and circle-tracking trajectory results, respectively, for backstepping- based and PI control. From the average errors depicted in Figures 3.10-3.11 one can see that overall the proposed backstepping scheme resulted in faster convergence to the desired trajectory and achieves a smaller steady-state error for both types of trajectory. 3.7 Chapter Summary In this chapter, we proposed a backstepping-based scheme to achieve trajectory tracking of under-actuated planar robotic systems. In particular, we proposed a scheme that would guarantee the ultimate boundedness of the position and heading tracking errors to a neighborhood of the origin. This was achieved via defining a new error coordinate that coupled the lateral and heading errors. The controller was then synthesized such that it guaranteed the convergence of the longitudinal and new coordinate error. Via time-scale analysis of perturbed system, it was shown that the 78 (a) Backstepping-based control. (b) PI control. Figure 3.9: A snapshot of an experimental run for line and circle-tracking using backstepping-based (a) and PI control (b), respectively. convergence of these then guarantees the stabilization of the whole error system to a neighborhood around the origin. The proposed scheme was implemented on a tail-actuated robotic fish, where a high-fidelity averaged nonlinear dynamic model was used for controller design. The actuator constraints of the system were handled via an auxiliary system. Finally, real-time experimental results demonstrated the effectiveness of the proposed scheme and showed its value over standard PI control. While most of the discussions in this work were framed in the context of aquatic robots and vehicles, the approach applies to ground robots and vehicles with similar under-actuated constraints. 79 0.4 Backstepping Control STD of Backstepping Control PI Control STD of PI Control Avg. Error (m) 0.3 0.2 0.1 0 0 5 10 15 20 25 30 time(s) (a) Line-tracking averaged position error and its standard deviation. Backstepping Control STD of Backstepping Control 0.25 PI Control STD of PI Control 0.2 Avg. Error (m) 0.15 0.1 0.05 0 0 10 20 30 40 50 60 70 time(s) (b) Circle-tracking averaged position error and its standard deviation. Figure 3.10: Experiments: line-tracking (3.10a) and circle-tracking (3.10b) averaged position error for backstepping-based control and PI control. The solid blue line with circular markers represents the averaged position error obtained for backstepping-based control, while the dashed blue lines represent its standard deviation. Similarly, the solid magenta line represents the averaged position error obtained for PI control, while the dotted-dashed magenta lines represent its standard deviation. 80 (a) Line-tracking averaged angle error and its standard deviation. (b) Circle-tracking averaged angle error and its standard deviation. Figure 3.11: Experiments: line-tracking (3.11a) and circle-tracking (3.11b) averaged angle error for backstepping-based control and PI control. The solid blue line with circular markers represents the averaged angle error obtained for backstepping-based control, while the dashed blue lines represent its standard deviation. Similarly, the solid magenta line represents the averaged angle error obtained for PI control, while the dotted-dashed magenta lines represent its standard deviation. 81 CHAPTER 4 AVERAGED MODELING AND TRACKING CONTROL OF PECTORAL FIN-ACTUATED ROBOTIC FISH The approaches proposed in Chapter 2 and Chapter 3 focused on the control of tail fin-actuated robotic fish. While caudal fins have proven to be an efficient propulsion mode at higher speeds, pectoral fins play a vital role in maneuvering and stability while providing or assisting propulsion at lower speeds [25]. Pectoral fin motions can generally be classified into three modes based on the axis of rotation: rowing, feathering, and flapping. Feathering and flapping involve fin rotation about the transverse and longitudinal axis, respectively, while rowing motion involves fin rotation about the vertical axis. Rowing motion is classified as a “drag-based” swimming mechanism, where the drag element of fluid dynamics generates the thrust and can be utilized for a number of in-plane locomotion and maneuvering tasks, such as forward swimming, sideway swimming, and turning [66, 67]. The fin beat cycle in the rowing motion of pectoral fins comprises two sub-movements: a power stroke and a recovery stroke. During the power stroke, the pectoral fin rotates towards the back of the robot to produce thrust through induced drag on the pectoral fin surface, while during the recovery stroke, the fin moves toward the front of the body, ideally with minimal loading, to get ready for the next fin-beat cycle. Considering the cyclic nature of typical actuation modes, it is of interest to develop a dynamic average model that is amenable to controller design, where the control inputs are actuation pattern parameters. Furthermore, in practical applications it is more natural to control the parameters of periodic fin beats than to directly control the fin position at every moment, which makes an averaged model best suited for trajectory planning and tracking control. In this chapter, we present a nonlinear dynamic average model for robotic fish propelled by a pair of rigid pectoral fins undergoing rowing motion. In particular, we consider the robot undergoing planar motion, with its original dynamics incorporating pectoral fin-generated hydrodynamic forces evaluated via 82 the blade element theory. Inspired by the work in [89], which deals with averaged dynamics for tail-actuated robotic fish, we seek scaling factors, as functions of fin-beat parameters, for the original hydrodynamic forces and moment, such that when classical averaging is applied to the resulting modified dynamics the obtained average model produces locomotion behaviors close to those of the original dynamic model. One fundamental step in identifying the scaling functions is estimating the scaling values for a given fin-beat pattern. [89] used a trial-and-error approach for the tail-actuated robotic fish, which is time-consuming. We propose a novel systematic approach to finding optimal scaling values by formulating a nonlinear model-predictive control (NMPC) problem, which can be readily solved with NMPC packages. Once the scaling values are found for a set of fin-beat patterns, nonlinear regression is used to determine the scaling functions with minimal complexity. Simulation comparison between the averaged model and the original dynamic model, using fin-beat patterns not used in identifying the scaling functions, supports the efficacy of the developed averaged model. Furthermore, we conduct experiments on a pectoral fin-actuated robotic fish and compare the experimental results with simulation predictions when considering the forward swimming motion, where both fins are actuated symmetrically. Finally, to demonstrate the utility of the dynamic average model, we design a controller for trajectory tracking of a pectoral fin-actuated robotic fish. In particular, the proposed scheme uses a backstepping-based controller that finds the needed inputs for the robot to track the desired trajectory based upon the averaged model. In this design, the physical control inputs involve two of the fin-beat parameters, the bias, and the amplitude, while the other parameters (angular frequency and power/recover stroke ratio) are kept constant. We further use a multi-variable minimization solver to determine the optimal fin-beat parameters such that the achieved inputs are closed to the needed values. Simulation results are presented to demonstrate the effectiveness of the proposed model-based tracking control scheme. The rest of the chapter is organized as follows. We first review the dynamic model of the pectoral fin-actuated robotic fish in Section 4.1. In Section 4.2 we present the development of the proposed averaged model. In Section 4.3 we present the scheme to determine the model parameters 83 and the averaged scaling functions, as well as the experimental and simulation validation of the resulting average model. In Section 4.4 we detail the proposed backstepping-based tracking control approach, with simulation results in Section 4.5. Finally, we provide some concluding remarks in Section 4.6. 4.1 Dynamic Model For Pectoral Fin-actuated Robotic Fish 4.1.1 Rigid Body Dynamics We consider the robot to be a rigid body with rigid pectoral fins that are actuated at the base, and we assume that the robot operates in an inviscid, irrotational, and incompressible fluid within an infinite domain. Joint-base Y Vc point Sp R R MD FD A0 R ^y L 0L Right Dp Cp Pectoral ds FL Fin (b) v⃗p Robotic γR Fish Body R Z X R p (a) (c) Figure 4.1: (a) Top view of the pectoral fin-actuated robotic fish undergoing planar motion; (b) side view and blade element of the right pectoral fin with its parameters and variables; (c) top view of the pectoral fin with its associated forces and angles. As illustrated in Figure 4.1(a), we define [X,Y,Z]𝑇 and [𝑥, ˆ 𝑦ˆ , 𝑧ˆ] 𝑇 as the inertial coordinate system and the body-fixed coordinate system, respectively. The velocity of the center of mass in the body-fixed coordinates is expressed as 𝑽𝑐 = [𝑉𝑐𝑥 , 𝑉𝑐 𝑦 , 𝑉𝑐 𝑧 ], where 𝑉𝑐𝑥 , 𝑉𝑐 𝑦 , and 𝑉𝑐 𝑧 indicate surge, sway, and heave velocities, respectively. The angular velocity expressed in the body-fixed coordinate system is given by 𝜔 = [𝜔𝑥 , 𝜔 𝑦 , 𝜔 𝑧 ], which is composed of roll (𝜔𝑥 ), pitch (𝜔 𝑦 ), and 84 yaw (𝜔 𝑧 ). We let 𝛼 denote the angle of attack, formed by the direction of 𝑽𝑐 with respect to the 𝑉𝑐 𝑦 ˆ 𝑥-axis and is given by 𝛼 = arctan( 𝑉𝑐 ). Let 𝜓 denote the heading angle, formed by the 𝑥-axis ˆ 𝑥 relative to the X-axis, 𝐶 𝑝 be the distance between the pectoral fin base and the body’s center of mass, and 𝐴0𝐿 and 𝐴0𝑅 denote the pivot points for the left and right fins, respectively. Finally, 𝛾 𝐿 and 𝛾 𝑅 represent the angles between the left pectoral fin and the body-fixed 𝑥-axis ˆ and the right pectoral fin and the body-fixed 𝑥-axis, ˆ respectively. We only consider the robot’s planar motion, and further assume that the body is symmetric with respect to the 𝑥ˆ 𝑧ˆ-plane and that the pectoral fins move in the 𝑥ˆ 𝑦ˆ -plane, such that the system only has three degrees of freedom, surge (𝑉𝑐𝑥 ), sway (𝑉𝑐 𝑦 ), and yaw (𝜔 𝑧 ). Furthermore, we assume that we can neglect the inertial coupling between yaw, sway and surge motions [102], and arrive at the following equations of planar motion (𝑚 𝑏 − 𝑚 𝑎 𝑥 )𝑉¤𝑐𝑥 = (𝑚 𝑏 − 𝑚 𝑎 𝑦 )𝑉𝑐 𝑦 𝜔 𝑧 + 𝑓𝑥 (4.1) (𝑚 𝑏 − 𝑚 𝑎 𝑦 )𝑉¤𝑐 𝑦 = −(𝑚 𝑏 − 𝑚 𝑎 𝑥 )𝑉𝑐𝑥 𝜔 𝑧 + 𝑓 𝑦 (4.2) (𝐽𝑏𝑧 − 𝐽𝑎 𝑧 ) 𝜔¤ 𝑧 = (𝑚 𝑎 𝑦 − 𝑚 𝑎 𝑥 )𝑉𝑐𝑥 𝑉𝑐 𝑦 + 𝜏𝑧 (4.3) where 𝑚 𝑏 is the mass of the body, 𝐽𝑏𝑧 is the inertia of the body about the 𝑧ˆ-axis, 𝑚 𝑎 𝑥 and 𝑚 𝑎 𝑦 are the hydrodynamic derivatives that represent the added masses of the robotic fish along the 𝑥ˆ and 𝑦ˆ directions, respectively, and 𝐽𝑎 𝑧 represents the added inertia effect of the body about the 𝑧ˆ direction. Finally, the hydrodynamic forces and moment due to the pectoral fin actuation and the interaction of the body itself with the fluid are captured by 𝑓𝑥 , 𝑓 𝑦 , and 𝜏𝑧 and are given by 𝑓𝑥 = 𝑓 ℎ𝑥 − 𝐹𝐷 cos(𝛼) + 𝐹𝐿 sin(𝛼) (4.4) 𝑓 𝑦 = 𝑓 ℎ 𝑦 − 𝐹𝐷 sin(𝛼) − 𝐹𝐿 sin(𝛼) (4.5) 𝜏𝑧 =𝜏ℎ 𝑧 + 𝑀𝐷 (4.6) where 𝑓 ℎ𝑥 , 𝑓 ℎ 𝑦 , and 𝜏ℎ 𝑧 are the hydrodynamic forces and moment transmitted to the fish body by the right and left pectoral fins, while 𝐹𝐷 , 𝐹𝐿 , and 𝑀𝐷 are the body drag, lift, and moment, respectively. 85 4.1.2 Drag and Lift on the Robot Body The lift force 𝐹𝐿 , drag force 𝐹𝐷 , and drag moment 𝑀𝐷 acting on the robotic fish can be captured by ( [31, 135]) 1 𝐹𝐷 = 𝜌|𝑉𝑐 | 2 𝑆 𝐴 𝐶 𝐷 (4.7) 2 1 𝐹𝐿 = 𝜌|𝑉𝑐 | 2 𝑆 𝐴 𝐶 𝐿 𝛼 (4.8) 2 𝑀𝐷 = − 𝐶 𝑀 𝜔2𝑧 sgn(𝜔 𝑧 ) (4.9) where 𝜌 is the density of water, |𝑉𝐶 | is the linear velocity magnitude of the body in the body-fixed q frame and is defined as |𝑉𝑐 | = 𝑉𝑐2𝑥 + 𝑉𝑐2𝑦 , 𝑆 𝐴 is the wetted surface area for the robot, 𝐶 𝐷 is the drag force coefficient, 𝐶 𝐿 is the lift force coefficient, 𝐶 𝑀 is the drag moment coefficient, and sgn(·) is the signum function. 4.1.3 Hydrodynamic Forces from Rowing Pectoral Fins As shown in Figure 4.1(b), we consider the pectoral fins to be rectangular with span length 𝑆 𝑝 and chord length 𝐷 𝑝 , and assume that they perform pure rowing motion. To evaluate their hydrodynamic forces, we adopt the procedure proposed in [136]. Furthermore, we illustrate the force calculations using only the right pectoral fin, since they can be trivially extended to the left pectoral fin. We consider a coordinate system with unit vectors 𝑚ˆ 𝑅 and 𝑛ˆ 𝑅 that are attached to the pectoral fin. The relationship between these unit vectors and the robotic fish body-fixed coordinates is given by 𝑚ˆ 𝑅 = cos 𝛾 𝑅 𝑥ˆ − sin 𝛾 𝑅 𝑦ˆ (4.10) 𝑛ˆ 𝑅 = − sin 𝛾 𝑅 𝑥ˆ − cos 𝛾 𝑅 𝑦ˆ (4.11) In the following calculations we assume an anchored robotic fish body, often adopted in literature as it simplifies calculation without incurring significant error [25, 137]. The velocity and 86 acceleration at the point 𝑠 along the fin are then given by 𝑣 𝑝 𝑅 (𝑠, 𝑡) =𝑠 𝛾¤ 𝑅 𝑛ˆ 𝑅 (4.12) 𝑎 𝑅 (𝑠, 𝑡) =𝑠 𝛾¥ 𝑅 𝑛ˆ 𝑅 − 𝑠 𝛾¤ 2𝑅 𝑚ˆ 𝑅 (4.13) where 𝛾¤ 𝑅 and 𝛾¥ 𝑅 indicate the first and second time derivatives of 𝛾 𝑅 , respectively. The hydrodynamic forces on the pectoral fin have both span-wise and normal components. However, the fins are considered to have pure rowing motion which implies that the span-wise force that arises from friction is very small and can thus be neglected [138]. Using blade theory, we can then calculate the differential normal force d𝐹𝑛𝑅 (𝑠, 𝑡) on each blade element d𝑠 on the pectoral fin at time 𝑡 as 1 d𝐹𝑛𝑅 (𝑠, 𝑡) = − 𝐶𝑛 (𝜑 𝑅 (𝑠, 𝑡)) 𝜌𝐷 𝑝 |𝑣 𝑝 𝑅 (𝑠, 𝑡)| 2 d𝑠 𝑛ˆ 𝑅 (4.14) 2 where 𝐶𝑛 (𝜑 𝑅 (𝑠, 𝑡)) = 𝜆 sin 𝜑 𝑅 (𝑠, 𝑡) is the normal force coefficient, which depends on the angle of attack of each arbitrary blade, 𝜑 𝑅 (𝑠, 𝑡), and 𝜆 is a parameter that can be evaluated empirically through experiments. The angle of attack of the right pectoral fin at each point, 𝜑 𝑅 (𝑠, 𝑡), is defined as < 𝑣 𝑝 𝑅 (𝑠, 𝑡), 𝑛ˆ 𝑅 > tan 𝜑 𝑅 (𝑠, 𝑡) = (4.15) < 𝑣 𝑝 𝑅 (𝑠, 𝑡), 𝑚ˆ 𝑅 > where < ·, · > denotes the inner product. Note that in this work we assume < 𝑣 𝑝𝑅 (𝑠, 𝑡), 𝑚ˆ 𝑅 >= 0. The total hydrodynamic force acting on each pectoral fin is calculated by integrating the force density along the span length of the fin such that ∫ 𝑆𝑝 𝐹𝑛 𝑅 (𝑡) = − d𝐹𝑛𝑅 (𝑠, 𝑡) 0 ∫ 𝑆𝑝 1 =− 𝜆𝜌𝐷 𝑝 |𝑣 𝑝𝑅 (𝑠, 𝑡)| 2 d𝑠 𝑛ˆ 𝑅 0 2 (4.16) 1 = − 𝜆𝜌𝐷 𝑝 𝑆 3𝑝 𝛾¤ 2𝑅 sin 𝜑 𝑅 𝑛ˆ 𝑅 6 1 = − 𝜆𝜌𝐷 𝑝 𝑆 3𝑝 𝛾¤ 2𝑅 sgn( 𝛾¤ 𝑅 ) 𝑛ˆ 𝑅 6 87 𝑠 𝛾¤ 𝑠 𝛾¤ 𝑅 where we used sin 𝜑 𝑅 = sin(arctan( 0𝑅 )) = q = sgn( 𝛾¤ 𝑅 ). The total force acting on the (𝑠 𝛾¤ 𝑅 ) 2 right fin is determined by 𝐹®𝑅 = 𝐹𝑛 𝑅 𝑛ˆ 𝑅 − 𝐹®𝐴0 𝑅 = 𝑚 𝑝 𝑎 𝑅 (𝑠, 𝑡) 𝑆 𝑃 (4.17) 𝑠= 2 where 𝐹®𝐴0 represents the force applied by the rigid pectoral fin on the servo joint, and 𝑚 𝑝 is the effective mass of the rigid fin (the fin mass 𝑚 𝑝 𝑓 and the added mass, where the added mass is calculated based on a rigid plate moving in water [139]). The moment of the fin relative to its pivot point (𝐴0𝑅 ) is given by ∫ 𝑆𝑝 𝑀 ®𝑛 = 𝑠 𝑚ˆ 𝑅 × d𝐹𝑛 𝑅 (4.18) 𝑅 0 Finally, the force and moment exerted on the robotic fish body by the right pectoral fin is given by 𝑓 ℎ𝑥 𝑅 = < 𝐹®𝐴0 𝑅 , 𝑥ˆ > (4.19) 1 𝑆𝑝 𝑆𝑝 2 = 𝜆𝜌𝐷 𝑝 𝑆 3𝑝 𝛾¤ 2𝑅 sin 𝛾 𝑅 sgn( 𝛾¤ 𝑅 ) − 𝑚 𝑝 (− 𝛾¥ 𝑅 sin 𝛾 𝑅 − 𝛾¤ cos 𝛾 𝑅 ) 𝑥ˆ 6 2 2 𝑅 𝑓 ℎ 𝑦 𝑅 = < 𝐹®𝐴0 𝑅 , 𝑦ˆ > (4.20) 1 𝑆𝑝 𝑆𝑝 2 = 𝜆𝜌𝐷 𝑝 𝑆 3𝑝 𝛾¤ 2𝑅 cos 𝛾 𝑅 sgn( 𝛾¤ 𝑅 ) − 𝑚 𝑝 (− 𝛾¥ 𝑅 cos 𝛾 𝑅 + 𝛾¤ sin 𝛾 𝑅 ) 𝑦ˆ 6 2 2 𝑅 𝜏ℎ 𝑧 𝑅 =𝐶 𝑝 𝑦ˆ × 𝐹®𝐴0 (4.21) 1 𝑆𝑝 𝑆𝑝 2 = − 𝐶 𝑝 ( 𝜆𝜌𝐷 𝑝 𝑆 3𝑝 𝛾¤ 2𝑅 sin 𝛾 𝑅 sgn( 𝛾¤ 𝑅 ) − 𝑚 𝑝 (− 𝛾¥ 𝑅 sin 𝛾 𝑅 − 𝛾¤ cos 𝛾 𝑅 )) 𝑘ˆ 6 2 2 𝑅 For a more comprehensive derivation of the hydrodynamic forces, we refer the reader to [136]. By considering the kinematic equations of the robotic fish, the final dynamic model can be summarized as follows: 88    𝑣 𝑐 𝑥 cos 𝜓 − 𝑣 𝑐 𝑦 sin 𝜓   ¤    𝑋   𝑣 𝑐𝑥 sin 𝜓 + 𝑣 𝑐𝑦 cos 𝜓       𝑌¤        𝜔 𝑧   𝜓¤      𝑓 ℎ𝑥 𝑅 + 𝑓 ℎ𝑥 𝐿    =  𝑓1 (𝑣 𝑐 , 𝑣 𝑐 , 𝜔 𝑧 ) + (4.22) 𝑥 𝑦  𝑣¤ 𝑐𝑥   𝑚1        𝑓 ℎ 𝑦 𝑅 + 𝑓 ℎ 𝑦 𝐿  𝑣¤ 𝑐   𝑓 (𝑣 , 𝑣 , 𝜔 ) +  𝑦   2 𝑐𝑥 𝑐 𝑦 𝑧 𝑚2   𝜏ℎ 𝑅 + 𝜏ℎ 𝐿      𝜔¤ 𝑧      𝑓 (𝑣 𝑐 , 𝑣 𝑐 , 𝜔 𝑧 ) +  3 𝑥 𝑦  𝐽3    where q q 𝑣 𝑚2 𝑐1 2 + 𝑣2 + 𝑐2 𝑣 2 + 𝑉 2 arctan( 𝑐 𝑦 )  (𝑣 ) = −   𝑓 , 1 𝑐𝑥 𝑐 𝑦 𝑣 , 𝜔 𝑧 𝑣 𝑐 𝜔 𝑧 𝑣 𝑐 𝑣 𝑐 𝑣 𝑚1 𝑦 𝑚 1 𝑥 𝑐𝑥 𝑐𝑦 𝑚 1 𝑦 𝑐𝑥 𝑐𝑦     𝑣 𝑐𝑥   𝑚 𝑐 q 𝑐 q 𝑣𝑐𝑦 𝑓2 (𝑣 𝑐𝑥 , 𝑣 𝑐 𝑦 , 𝜔 𝑧 ) = − 1 𝑣 𝑐𝑥 𝜔 𝑧 − 1 𝑣 𝑐 𝑦 𝑣 2𝑐𝑥 + 𝑣 2𝑐 𝑦 − 2 𝑣 𝑐𝑥 𝑣 2𝑐𝑥 + 𝑣 2𝑐 𝑦 arctan(   ) (4.23)   𝑚2 𝑚2 𝑚2 𝑣 𝑐𝑥   𝑓3 (𝑣 𝑐 , 𝑣 𝑐 , 𝜔 𝑧 ) = (𝑚 1 − 𝑚 2 ) 𝑣 𝑐 𝑣 𝑐 − 𝑐 4 𝜔2 sgn(𝜔 𝑧 )     𝑥 𝑦 𝑥 𝑦 𝑧   𝐽3 with 𝑚 1 = 𝑚 𝑏 − 𝑚 𝑎 𝑥 , 𝑚 2 = 𝑚 𝑏 − 𝑚 𝑎 𝑦 , 𝐽3 = 𝐽𝑏𝑧 − 𝐽𝑎 𝑧 , 𝑐 1 = 12 𝜌𝑆𝐶 𝐷 , 𝑐 2 = 12 𝜌𝑆𝐶 𝐿 , 𝑐 4 = (𝐽1 ) 𝐶 𝑀 . 3 4.2 Averaging with Scaled Forcing In this section we present the scaled-averaging approach and the proposed scheme to estimate the scaling functions. Finally, we present experimental and simulation results to validate the proposed averaged model. 4.2.1 Averaged Model In order to generate a net thrust over each cycle, the pectoral fins need to be actuated differently in the power and recovery strokes. For example, to generate forward thrust the fin has to be actuated faster in the power stroke than in the recovery stroke. We specify the fin beat pattern as   (𝜁 + 1) 𝑇𝑝 𝛾0 − 𝛾 𝐴 cos[𝜋 0≤𝑡≤   𝑡], 𝜁 +1    𝑇 𝛾(𝑡) =  𝜁 + 1  𝑝 𝑇𝑝 𝑇𝑝 (4.24) 𝛾 + 𝛾 𝐴 cos[𝜋 (𝑡 − )], < 𝑡 ≤ 𝑇𝑝   0  𝜁 +1 𝜁 +1   𝜁𝑇𝑝  89 where 𝛾0 is the fin-beat bias, 𝛾 𝐴 is the fin-beat amplitude, 𝑇𝑃 is the fin-beat period, and 𝜁 is a parameter defining the ratio of angular velocities of the fin during the power and recovery strokes, respectively. Under this periodic fin movement, averaging can be a useful tool for gaining insight into the effect of the input parameters (such as the beat bias 𝛾0 , amplitude 𝛾 𝐴 , period 𝑇𝑝 , and ratio 𝜁) on the dynamics and for designing controllers. First-order averaging [96] tends to generate prohibitively complex model for control [89]. On the other hand, classical averaging (directly averaging the vector field over one period of the fin-beat) cannot be directly applied since the dynamics is not slow in typical scenarios as shown in [89]. Therefore, we first scale the original forcing terms with functions that are potentially dependent on the fin-beat parameters, and then apply classical averaging over the modified dynamics. Specifically, let the original system (4.22) be modified as 𝑣¤ 𝑐𝑥 = 𝑓1 (𝑣 𝑐𝑥 , 𝑣 𝑐 𝑦 , 𝜔 𝑧 ) + 𝐾 𝑓𝑥 𝑅 (𝛾0𝑅 , 𝛾 𝐴𝑅 , 𝑇𝑝𝑅 , 𝜁 𝑅 ) · 𝑓 ℎ𝑥 𝑅 (𝑡) (4.25) + 𝐾 𝑓𝑥 𝐿 (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 ) · 𝑓 ℎ𝑥 𝐿 (𝑡) 𝑣¤ 𝑐 𝑦 = 𝑓2 (𝑣 𝑐𝑥 , 𝑣 𝑐 𝑦 , 𝜔 𝑧 ) + 𝐾 𝑓 𝑦 𝑅 (𝛾0𝑅 , 𝛾 𝐴𝑅 , 𝑇𝑝𝑅 , 𝜁 𝑅 ) · 𝑓 ℎ 𝑦 𝑅 (𝑡) (4.26) + 𝐾 𝑓 𝑦 𝐿 (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 ) · 𝑓 ℎ 𝑦 𝐿 (𝑡) 𝜔¤ 𝑧 = 𝑓3 (𝑣 𝑐𝑥 , 𝑣 𝑐 𝑦 , 𝜔 𝑧 ) + 𝐾𝑚 𝑅 (𝛾0𝑅 , 𝛾 𝐴𝑅 , 𝑇𝑝𝑅 , 𝜁 𝑅 ) · 𝜏ℎ 𝑅 (𝑡) (4.27) + 𝐾𝑚 𝐿 (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 ) · 𝜏ℎ 𝐿 (𝑡) where 𝐾 𝑓𝑥 𝑅 (·), 𝐾 𝑓 𝑦 𝑅 (·), 𝐾𝑚 𝑅 (·), 𝐾 𝑓𝑥 𝐿 (·), 𝐾 𝑓 𝑦 𝐿 (·), 𝐾𝑚 𝐿 (·) are scaling functions to be determined later (Section 4.3.3), and are {𝛾0𝑅 , 𝛾 𝐴𝑅 , 𝑇𝑝𝑅 , 𝜁 𝑅 } and {𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 } are the fin-beat pa- rameters of the right fin and left fin, respectively. For brevity, the arguments of the functions 𝑓1 (·), 𝑓2 (·), 𝑓3 (·) and the scaling functions are omitted in the remainder of the chapter, and the calculations are illustrated using only the right fin since they can be extended for the left fin in a straightforward manner. To avoid the integration of nested sin functions and facilitate the computation of the averaging, we first use the second-order Taylor series expansion to approximate the cos(𝛾) and sin(𝛾) terms 90 that appear in the forcing terms in 𝑓 ℎ𝑥 and 𝑓 ℎ 𝑦 . After conducting classical averaging, the following averaged system is obtained  𝑣¤̄ 𝑐𝑥 = 𝑓1 + 𝐾 𝑓𝑥 𝑅 · 𝑓¯ℎ𝑥 𝑅 (𝛾0𝑅 , 𝛾 𝐴𝑅 , 𝑇𝑝𝑅 , 𝜁 𝑅 ) + 𝐾 𝑓𝑥 𝐿 · 𝑓¯ℎ𝑥 𝐿 (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 )          𝑣¤̄ 𝑐 𝑦 = 𝑓2 + 𝐾 𝑓 𝑦 𝑅 · 𝑓¯ℎ 𝑦 𝑅 (𝛾0𝑅 , 𝛾 𝐴𝑅 , 𝑇𝑝𝑅 , 𝜁 𝑅 ) + 𝐾 𝑓 𝑦 𝐿 · 𝑓¯ℎ 𝑦 𝐿 (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 ) (4.28)     𝜔¤̄ 𝑧 = 𝑓3 + 𝐾𝑚 𝑅 · 𝜏¯ℎ 𝑅 (𝛾0𝑅 , 𝛾 𝐴𝑅 , 𝑇𝑝𝑅 , 𝜁 𝑅 ) + 𝐾𝑚 𝐿 · 𝜏¯ℎ 𝐿 (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 )    where 𝐷 𝑝 𝜆𝑙 3𝑝 𝑆 3𝑝 𝜋 2 𝜌𝛾0 𝛾 2𝐴 (−4𝛾02 − 3(−8 + 𝛾 2𝐴 ))(−1 + 𝜁 2 ) 𝑓¯ℎ𝑥 𝑅 = (4.29) 288𝑚 1 𝜁𝑇𝑝2 −𝛾 2𝐴 𝑙 𝑝 𝜋 2  𝑓¯ℎ 𝑦 𝑅 = 4𝐷 𝑝 𝛾02𝜆𝑙 2𝑝 𝜌(−1 + 𝜁 2 )+ 96𝑚 2𝑇𝑝2 𝜁 (4.30)  𝐷 𝑝 (−8 + 𝛾 2𝐴 )𝜆𝑙 2𝑝 𝜌(−1 + 𝜁 2 ) − 4𝛾03 𝑚 𝑝 (1 + 𝜁) 2 − 3𝛾0 𝛾 2𝐴 𝑚 𝑝 (1 + 𝜁) 2 𝐶 𝑝 𝐷 𝑝 𝜆𝑙 3𝑝 𝜋 2 𝜌𝛾0 𝛾 2𝐴 (−4𝛾02 − 3(−8 + 𝛾 2𝐴 ))(−1 + 𝜁 2 ) 𝜏¯ℎ 𝑅 = (4.31) 288𝐽3 𝜁𝑇𝑝2 Note that the model (4.28) can be expressed in a control-affine form if one defines the control inputs as 𝛼1 = 𝐾 𝑓𝑥 𝑅 𝑓¯ℎ𝑥 𝑅 , 𝛼2 = 𝐾 𝑓𝑥 𝐿 𝑓¯ℎ𝑥 𝐿 , 𝛼3 = 𝐾 𝑓 𝑦 𝑅 𝑓¯ℎ 𝑦 𝑅 and 𝛼4 = 𝐾 𝑓 𝑦 𝐿 𝑓¯ℎ 𝑦 𝐿 . As an example, we refer the reader to [92], where the authors show how one can express an averaged model for a trail-actuated robotic fish in a control-affine form and use it to design a model-predictive controller. 4.3 Simulation and Experimental Model Validation In order to validate the presented averaged model, we must first identify the hydrodynamic pa- rameters (𝐶 𝐷 , 𝐶 𝐿 , and 𝐶 𝑀 ), the fin parameter (𝜆), and the scaling functions (𝐾 𝑓𝑥 𝑅 , 𝐾 𝑓 𝑦 𝑅 , 𝐾𝑚 𝑅 , 𝐾 𝑓𝑥 𝐿 , 𝐾 𝑓 𝑦 𝐿 , 𝐾𝑚 𝐿 ) for the model. In this section we present the experimental setup and discuss the ex- perimental identification and validation of the hydrodynamic and fin model parameters and present an estimation scheme to obtain the scaling coefficients. Finally, we present simulation and experi- mental results to evaluate the effectiveness of the proposed averaged model. 91 4.3.1 Experimental Setup To validate the presented averaged model, we conduct experiments using the free-swimming robotic fish depicted in Figure 4.2. The robot consists of a rigid-shell body, a tail and two rigid pectoral fins, which were all 3D-printed. Although the robot also has a servo-actuated caudal fin, the tail-actuation is not included in this work. The body and fins dimensions are shown in Table 4.1. Two Tenergy Li-Ion rechargeable batteries (7.4V, 3350mAh) are used to power the robot, and two Hitec digital micro waterproof servos (HS-5086WP) are used to actuate the pectoral fins. Finally, a Microchip Digital Signal Processor and Controller (DSPIC30F6014) is used to realize the control of the servos and a Xbee module is used for wireless communication with a computer. The robotic fish is run in a 2.30 m by 1.2 m space enclosed within a tank equipped with an overhead Logitech C930E camera as seen in Figure 4.2. To obtain the robotic fish’s position and orientation in the tank, two markers were attached to the anterior and posterior of the robotic fish body. An overhead video of the robotic fish swimming in the tank is captured using the camera, and Visual C++ with the OpenCV library is used to implement an image processing algorithm. The algorithm detects the positions of the two markers and uses their average to obtain the center position of the robotic fish. In addition, the heading angle of the robot is estimated using the positions of the two markers, and a high gain observer is used to estimate the linear and angular velocities of the robot based on the measured position and heading. 4.3.2 Original Dynamics Parameter Identification Before identifying the scaling functions and validating the averaged model, the hydrodynamic parameters present in the dynamic and averaged model must first be identified. These parameters are either measured directly or calculated based on measurements and are summarize in Table 4.1. As typically done in literature [102, 140], the body inertia about 𝑧ˆ-axis is evaluated as 𝐽𝑏𝑧 = 1 2 Body lenght Body width 5 𝑚 𝑏 (𝑎 + 𝑐2 ), where 𝑎 = 2 and 𝑐 = 2 are the semiaxis lengths. Furthermore, the added masses, added inertia, and wetted surface are calculated based on a prolate spheroid approximation of the robotic fish body [102, 130]. 92 Figure 4.2: The experimental setup. During experiments the pectoral-fin actuated robotic fish swims within the enclosed area (denoted by the yellow lines) in the tank, and the overhead Logitech camera captures a video of the robot swimming. An image processing algorithm detects the red and blue markers placed on top of the robot to localize it and determine its heading. The fin normal force coefficient 𝜆, as well as the body drag and lift coefficients (𝐶 𝐷 , 𝐶 𝐿 , and 𝐶 𝑀 ) are identified empirically from data collected using the robotic fish described above. In particular, we consider only turning motions which are achieved by activating only one fin at a time according to (4.24). To determine the body drag, lift and moment coefficients (𝐶 𝐷 , 𝐶 𝐿 , and 𝐶 𝑀 ), we let the robotic fish swim for some time (approximately 35 s) to reach the steady-state motion, and then stop actuating the pectoral fin such that the robot slowly halts to a stop. In particular, considering (5.2), once the actuation has stopped, the dynamics equations become  𝑚2 𝑐1 q 𝑐2 q 2 2 2 2     𝑚 𝑣 𝑐 𝑦 𝜔 𝑧 − 𝑚 𝑣 𝑐𝑥 𝑣 𝑐𝑥 + 𝑣 𝑐 𝑦 + 𝑚 𝑣 𝑐 𝑦 𝑣 𝑐𝑥 + 𝑣 𝑐 𝑦  𝑣¤ 𝑐𝑥   1 1 q 1 q    𝑚1 𝑐1 2 2 𝑐2  𝑣¤ 𝑐  = − 𝑣 𝑐𝑥 𝜔 𝑧 − 𝑣 𝑐 𝑦 𝑣 𝑐𝑥 + 𝑣 𝑐 𝑦 − 𝑣 𝑐𝑥 𝑣 2𝑐𝑥 + 𝑣 2𝑐 𝑦  (4.32)  𝑦   𝑚2 𝑚2 𝑚2      𝜔¤ 𝑧   (𝑚 1 − 𝑚 2 ) 𝑣 𝑐𝑥 𝑣 𝑐 𝑦 − 𝑐 4 𝜔2𝑧 sgn(𝜔 𝑧 )       𝐽3   where 𝑚 1 = 𝑚 𝑏 − 𝑚 𝑎 𝑥 , 𝑚 2 = 𝑚 𝑏 − 𝑚 𝑎 𝑦 , 𝐽3 = 𝐽𝑏𝑧 − 𝐽𝑎 𝑧 , 𝑐 1 = 12 𝜌𝑆𝐶 𝐷 , 𝑐 2 = 12 𝜌𝑆𝐶 𝐿 , 𝑐 4 = 𝐽1 𝐶 𝑀 . 3 Let 𝜙 = [𝐶 𝐷 𝐶 𝐿 𝐶 𝑀 ] 𝑇 and rearrange (4.32) such that 93 𝑌 = 𝑋𝜙 (4.33) where 𝑚 𝑣¤ 𝑐𝑥 − 2 𝑣 𝑐 𝑦      𝑚1   𝑚1  𝑌 =  ¤ 𝑣 𝑐 𝑦 − 𝑣 𝑐 𝑥 𝜔 𝑧  (4.34)  𝑚 2    (𝑚 1 − 𝑚 2 )  𝜔¤ 𝑧 − 𝑣 𝑐𝑥 𝑣 𝑐 𝑦    𝐽3   𝑐¯1 q 𝑐¯2 q − 𝑣 𝑐𝑥 𝑣 2𝑐 + 𝑣 2𝑐 𝑣 2𝑐𝑥 + 𝑣 2𝑐 𝑦   𝑚1 𝑥 𝑦 𝑣 𝑐 𝑦 0   𝑚 1     𝑋 =  𝑐¯1 ¯ (4.35)  q 𝑐 q  2 − 𝑣 𝑐 𝑦 𝑣 2𝑐𝑥 + 𝑣 2𝑐 𝑦 − 𝑣 𝑐𝑥 𝑣 2𝑐𝑥 + 𝑣 2𝑐 𝑦 0    𝑚2 𝑚2    0 0 2  −𝑐¯4 𝜔 𝑧 sgn(𝜔 𝑧 )     where 𝑐¯1 = 12 𝜌𝑆, 𝑐¯2 = 12 𝜌𝑆, 𝑐¯4 = 𝐽1 . We use the captured video along with the image processing 3 algorithm to determine the body-fixed velocities, and a high gain observer to estimate the body-fixed acceleration for different sets of fin amplitudes (𝛾 𝐴𝑅 ): 15◦ , 18◦ , 20◦ , 25◦ , 30◦ ; biases (𝛾0𝑅 ): 80◦ , 90◦ , 100◦ , 110◦ ; periods (𝑇𝑝𝑅 ): 0.5, 0.66, 1 s; and power/ recovery stroke ratios (𝜁 ): 5, 6. Furthermore, to estimate the fin parameter 𝜆, we formulate another parameter estimation paradigm, where we consider constant actuation. In particular, using the hydrodynamic parame- ters estimated above along with the dynamic equations and Eqs. (4.19)-(4.21), we can obtain the following  1 𝜌𝐷 𝑆3 𝛾¤ 2 sin 𝛾 sgn(𝑠 𝛾¤ )  𝑉¤ − 𝑓 (𝑉 , 𝑉 , 𝜔 ) − 𝑓ˆℎ𝑥   6 𝑝 𝑝 𝑅   𝑅 𝑅   𝑐𝑥 1 𝑐𝑥 𝑐 𝑦 𝑧 𝑚1     𝑚 1    𝑓ˆℎ𝑦   1 𝜌𝐷 𝑝 𝑆3 𝛾¤ 2 cos 𝛾 sgn(𝑠 𝛾¤ )  𝑉¤𝑐 𝑦 − 𝑓2 (𝑉𝑐𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) −  = 𝜆  6 𝑝 𝑅 𝑅 (4.36)  𝑅   𝑚2  𝑚2   𝜔¤ 𝑧 − 𝑓3 (𝑉𝑐 , 𝑉𝑐 , 𝜔 𝑧 ) − 𝜏ˆℎ𝑧   −𝐶 𝑝 1 𝜌𝐷 𝑝 𝑆3𝑝 𝛾¤ 2 sin 𝛾 𝑅 sgn(𝑠 𝛾¤ 𝑅 )       𝑥 𝑦 𝐽3   6 𝑅     𝐽3    where 𝑆𝑝 𝑆𝑝 2 𝑓ˆℎ𝑥 𝑅 = − 𝑚 𝑝 (− 𝛾¥ 𝑅 sin 𝛾 𝑅 − 𝛾¤ cos 𝛾 𝑅 ) 2 2 𝑅 𝑆𝑝 𝑆𝑝 2 𝑓ˆℎ 𝑦 𝑅 = − 𝑚 𝑝 (− 𝛾¥ 𝑅 cos 𝛾 𝑅 + 𝛾¤ sin 𝛾 𝑅 ) (4.37) 2 2 𝑅 𝑆𝑝 𝑆𝑝 2 𝜏ˆℎ 𝑧 𝑅 = − 𝐶 𝑝 (−𝑚 𝑝 (− 𝛾¥ 𝑅 sin 𝛾 𝑅 − 𝛾¤ cos 𝛾 𝑅 )) 2 2 𝑅 94 We collect the robotic fish steady-state body-fixed velocities for another set of fin amplitudes (𝛾 𝐴𝑅 ): 8◦ , 10◦ , 13◦ , 15◦ , 18◦ ; biases (𝛾0𝑅 ): 85◦ , 93◦ , 98◦ , 100◦ , 103◦ ; periods (𝑇𝑝𝑅 ): 0.5, 0.66 s ; and power/ recovery stroke ratios (𝜁): 4, 5, and estimate the parameter 𝜆 using (4.36). The resulting coefficients are listed in Table 4.1. These parameters are then used in independent model validation for the dynamic model. Table 4.1: Identified model parameters for the robotic fish. Robot Body Parameter Value unit Body Length 0.198 m Body Height 0.1 m Body Width (𝐶𝑃 ) 0.03 m Mass (𝑚 𝑏 ) 0.795 kg Inertia (𝐽𝑏𝑧 ) 4.26×10−4 kg · m2 −𝑚 𝑎𝑥 0.095 kg −𝑚 𝑎𝑦 0.1794 kg −𝐽𝑎𝑧 2.7 × 10−5 kg/m2 Wet surface area (𝑆 𝐴 ) 0.325 m2 Drag coef. (𝐶 𝐷 ) 0.3870 - Lift coef. (𝐶 𝐿 ) 0.0808 - Moment coef. (𝐶 𝑀 ) 8.5 ×10−3 kg/m2 Pectoral Fin Parameter Value unit Fin Length (𝑆 𝑝 ) 0.061 m Fin Heigth (𝐷 𝑝 ) 0.041 m Fin Mass (𝑚 𝑝 𝑓 ) 0.008 kg Effective mass (𝑚 𝑝 ) 0.008 kg Water density (𝜌) 1000 kg/m3 𝜆 4.1464 - To validate the dynamic model, we conduct experiments based on different fin actuation pa- rameters in both forward swimming and turning. In the forward swimming case both left and right fins are actuated in sync with the same fin-beat patterns. We compare the steady state swimming speeds predicted by the model and those obtained from experiments in the forward swimming case, whilst in the turning cases we compare the turning radius and period. Each experiment is repeated four times to obtain the average and standard deviation. Table 4.2 and Table 4.3 list the percent errors between the values obtained from experiments and those obtained from simulation using the parameters estimated earlier. The comparison indicates that the dynamic model has acceptable accuracy. 95 Table 4.2: Model validation results: relative model prediction error for turning radius and turning period. (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 ) Turning Radius Error(%) Turning Period Error(%) (85◦ , 18◦ , 1 s, 4 ) 10.96 5.96 (85◦ , 18◦ , 1 s, 3 ) 4.66 17.92 (80◦ , 22◦ , 1 s, 4 ) 12.67 4.91 (80◦ , 22◦ , 1 s, 3 ) 16.15 2.12 (85◦ , 22◦ , 1 s, 4 ) 2.60 1.39 (85◦ , 22◦ , 1 s, 3 ) 13.61 4.82 Table 4.3: Model validation results: surge velocities predicted by the original model and measured from experiments, and their relative error. (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 ) Surge Velocity(Experiments) Surge Velocity(Model) Error (%) (95◦ , 12◦ , 1 s, 4 ) 0.0383 0.0397 3.60 (95◦ , 12◦ , 0.66 s, 5 ) 0.0423 0.0449 5.77 (100◦ , 12◦ , 0.8 s, 4 ) 0.0536 0.0494 8.52 (100◦ , 12◦ , 0.8 s, 5 ) 0.0614 0.0558 9.96 (100◦ , 12◦ , 0.66 s, 5 ) 0.0490 0.0447 9.74 4.3.3 Identification of Scaling Functions To identify the corresponding scaling functions for the averaged model, we conduct simulations using the original dynamic model (with the experimentally identified parameters presented in the previous subsection) considering only one fin (the right fin) actuated with a given fin-beat pattern, and seek the corresponding values of the scaling functions (i.e., 𝐾 𝑓𝑥 𝑅 , 𝐾 𝑓 𝑦 𝑅 , 𝐾𝑚 𝑅 ) such that the resulting average model (4.28) produces the best match in the turning radius and turning period with those of the original dynamics (4.22). Under a given fin actuation pattern, instead of conducting blanket-search of the scaling param- eters as done in [89], we propose a novel formulation that treats the scaling values for the given actuation pattern as constant control inputs to the averaged model and solve for these values through nonlinear model predictive control [100], such that the averaged system tracks the surge, sway, and angular velocities extracted from the simulated trajectory of the original dynamics (4.22) under the same fin-actuation pattern. We elaborate on this below. 96 Simulations are first conducted using the original dynamic model with different sets of fin-beat parameters (𝛾0 , 𝛾 𝐴 , 𝑇𝑃 , 𝜁), where we use body parameters identified in the previous subsection (Table 4.1). Only the right fin is actuated, since the scaling functions are considered left-right symmetric. In particular, 560 simulations are conducted with the combination of the following fin-beat patterns: 7 different amplitudes (𝛾 𝐴𝑅 ) : 10◦ , 15◦ , 20◦ , 22◦ , 25◦ , 28◦ , 30◦ ; 5 different biases (𝛾0𝑅 ): 50◦ , 60◦ , 70◦ , 80◦ , 90◦ ; 4 different periods (𝑇𝑝𝑅 ): 0.5, 0.66, 1, 2 s; and 4 different power/recovery stroke ratios (𝜁): 2, 3, 4, 5. For each simulation, we extract the turning radius 𝑅, turning period 𝑇𝑏 (time taken to complete one turn) and angle of attack 𝛼 of the robot at the steady state, and then use the following relationships to determine the corresponding steady-state body-fixed linear (𝑣 𝑐𝑥 , 𝑣 𝑐 𝑦 ) and angular (𝜔 𝑧 ) velocities for the original dynamics: q 2𝜋 𝑣 2𝑐𝑥 + 𝑣 2𝑐 𝑦 𝑣𝑐𝑦 𝑇𝑏 = , 𝑅= , 𝛼 = arctan (4.38) 𝜔𝑧 𝜔𝑧 𝑣 𝑐𝑥 These velocities (𝑣 𝑐𝑥 , 𝑣 𝑐 𝑦 , 𝜔 𝑧 ) are then considered as desired values to be tracked by the averaged model: 𝑣¯ 𝑥𝑟 = 𝑣 𝑐𝑥 , 𝑣¯ 𝑦𝑟 = 𝑣 𝑐 𝑦 , 𝜔¯ 𝑟 = 𝜔 𝑧 . We further let 𝑢 1 = 𝐾 𝑓𝑥 𝑅 , 𝑢 2 = 𝐾 𝑓 𝑦 𝑅 , 𝑢 3 = 𝐾𝑚 𝑅 such that (4.28) can be rewritten as 𝑣¤̄ 𝑐 𝑥 = 𝑓1 + 𝑢 1 𝑓¯ℎ𝑥 𝑅 (4.39) 𝑣¤̄ 𝑐 𝑦 = 𝑓2 + 𝑢 2 𝑓¯ℎ 𝑦 𝑅 (4.40) 𝜔¤̄ 𝑧 = 𝑓3 + 𝑢 3 𝜏¯ℎ 𝑅 (4.41) We construct the velocity tracking error as " 𝑣¯ 𝑐 𝑥 − 𝑣¯ 𝑥𝑟 # 𝒆 𝜼 = 𝑣¯ 𝑐 𝑦 − 𝑣¯ 𝑦𝑟 (4.42) 𝜔¯ 𝑧 − 𝜔¯ 𝑟 The objective is to determine 𝑢 1 , 𝑢 2 , 𝑢 3 such that the tracking error states of system (4.42) are driven to zero. To do so, we define an objective function with the following stage cost 𝐹 (·) and 97 Figure 4.3: Obtained scaling values 𝐾 𝑓𝑥 𝑅 , 𝐾 𝑓 𝑦 𝑅 , 𝐾𝑚𝑅 versus the amplitude 𝛾 𝐴 𝑅 and bias 𝛾0 𝑅 , for fixed actuation period 𝑇𝑝 𝑅 = 1 s and ratio 𝜁 = 2. terminal penalty 𝐸 (·) 𝐹 (𝒆 𝜼 , 𝒖) =𝒆 𝜼 (𝜏)𝑇 𝑄𝒆 𝜼 (𝜏) (4.43) 𝐸 (𝒆 𝜼 (𝑡 + 𝑇)) =(𝒆 𝜼 (𝑡 + 𝑇))𝑇 𝑄𝑇 (𝒆 𝜼 (𝑡 + 𝑇)) (4.44) where 𝑇 is the prediction horizon, 𝒖(𝜏) = [𝑢 1 𝑢 2 𝑢 3 ] 𝑇 , and 𝑄 and 𝑄𝑇 are positive-definite weighting matrices that penalize deviations from the desired values. By solving the nonlinear model predictive control (NMPC) problem (for example, using ACADO Model Predictive Control Toolkit [90]), we obtain the optimal inputs 𝑢 1 , 𝑢 2 , 𝑢 3 and thus the values for 𝐾 𝑓𝑥 𝑅 , 𝐾 𝑓 𝑦 𝑅 , 𝐾𝑚 𝑅 for a given fin-beat parameter combination. Specifically, the following NMPC parameters are used in solving for the scaling values: Length of optimization horizon : 𝑇 = 10 s Sampling interval : 𝑡 𝑠 = 1 s Weighting matrix : 𝑄 = 2000𝑰3 Terminal Penalty Weighting matrix : 𝑄𝑇 = 80𝑰3 where 𝑰3 is a 3 by 3 identity matrix. Figure 4.3 shows the resultant 3D surfaces of optimal coefficients obtained for different fin amplitudes and fin biases when the fin actuation period and 98 Table 4.4: Averaged model validation results: relative model prediction error between the original and averaged models for turning radius and turning period. (𝛾0𝐿 , 𝛾 𝐴𝐿 , 𝑇𝑝𝐿 , 𝜁 𝐿 ) Turning Radius Error(%) Turning Period Error(%) (90◦ , 22◦ , 1 s, 4 ) 3.51 0.21 (90◦ , 22◦ , 1 s, 3 ) 3.58 3.13 (90◦ , 20◦ , 1 s, 4 ) 3.45 1.34 (90◦ , 20◦ , 1 s, 3 ) 3.77 5.58 (80◦ , 22◦ , 1 s, 4 ) 2.93 1.84 (80◦ , 22◦ , 1 s, 3 ) 3.31 2.56 (80◦ , 20◦ , 1 s, 4 ) 2.86 0.73 (80◦ , 20◦ , 1 s, 3 ) 3.78 2.17 (60◦ , 22◦ , 1 s, 4 ) 2.14 4.06 (60◦ , 22◦ , 1 s, 3 ) 2.29 2.00 (60◦ , 20◦ , 1 s, 4 ) 1.97 0.19 (60◦ , 20◦ , 1 s, 3 ) 2.28 4.60 power/recover stroke ratio are fixed at some particular values. Results for other period and ratio combinations are similar and thus omitted in the interest of brevity. To obtain closed-form expressions for the relationships between each scaling coefficient and the fin-beat parameters, we implement a multivariate nonlinear regression scheme using the MATLAB command 𝑓 𝑖𝑡𝑛𝑙𝑚, where we seek the lowest-degree polynomials that provide adequate match with the computed scaling coefficients, and obtain the following: 𝐾 𝑓𝑥 𝑅 =0.9801 − 0.0653𝛾 𝐴 𝑅 + 0.0828𝛾0 𝑅 − 0.0007𝜁 𝑅 − 0.0069𝑇𝑝𝑅 (4.45) 𝐾 𝑓 𝑦 𝑅 = − 529.2 + 0.6𝛾 𝐴 𝑅 + 1893.4𝛾0 𝑅 − 3.8𝛾 2𝐴 − 2500.6𝛾02 − 0.0000276𝜁 𝑅2 𝑅 𝑅 (4.46) − 0.0001324𝑇𝑝𝑅 2 + 9.1𝛾 3𝐴 + 1446.6𝛾03 − 8.2𝛾 4𝐴 − 309.1𝛾04 𝑅 𝑅 𝑅 𝑅 𝐾𝑚 𝑅 =0.9869 − 0.4004𝛾 𝐴 𝑅 + 0.0661𝛾0 𝑅 − 0.0007𝜁 𝑅 − 0.0059𝑇𝑝𝑅 (4.47) 4.3.4 Validation of the Averaged Model We first compare the turning radius and period predictions between the averaged and the dynamic model for the turning case. Table 4.4 lists the errors between the predictions obtained from the averaged model and those obtained from the dynamic model for different sets of fin parameters. Furthermore, Figure 4.4 depicts circular trajectories obtained from experiments and simulation using the dynamic and averaged models. From the figure, we can see how the trajectory behavior 99 Experiments 0.8 y(m) 0.6 0.4 0.8 1 1.2 1.4 x(m) Averaged Dynamics Original Dynamics 0.8 y(m) 0.6 0.4 0.8 1 1.2 1.4 x(m) Measurement Experiments Original Model Averaged Model Turning Radius 0.169 m 0.17 m 0.18 m Turning Period 26.59 s 31.55 s 32.44 s Figure 4.4: Circular trajectories captured in experiments and predicted by the original and average dynamic models. In this case the right fin remains still and the left fin undergoes actuation with the following parameters fixed: 𝛾 𝐴𝐿 = 22◦ , 𝛾0𝐿 = 85◦ , 𝑇𝑃𝐿 = 1 and 𝜁 = 4. and steady-state radius of the dynamic and averaged model predictions match that of experiments, which suggest that the average model is able to capture well the behavior of the original dynamics and the dynamics of the robot under the new actuation patterns. To further validate the average model, we conduct simulations and experiments considering the forward swimming motion. The forward swimming case was not used in obtaining the scaling functions, and thus provides independent validation for the proposed average model. Figure 4.5 compares the simulated steady-state forward swimming speeds predicted with the original dynamic and the average models, and those obtained from the experiments. Note that the steady state is considered to be reached after the first 15 s of the robot swimming. In the experiments, for the given 100 0.16 Experiments 0.14 Original Dynamics Surge Velocity (m/s) Averaged Dynamics 0.12 0.1 0.08 0.06 0.04 0.02 0.5 1 1.5 2 2.5 Fin's Frequency(Hz) Figure 4.5: Comparison between experimental results and the predictions of the steady-state forward swimming speed based on the average model and the original dynamic model. In this study the left and right fins undergo symmetric actuation with the following parameters fixed: 𝛾 𝐴 = 15◦ , 𝛾0 = 95◦ and 𝜁 = 4. 𝜁 = 4, to prevent exceeding the speed limit of the servo motors, a maximum actuation frequency 1.75 Hz is used. We have extended the simulation results to fin-beat frequency up to 2.75 Hz in order to capture the performance trend of the robotic fish. 4.4 Average Dynamic Model-based Trajectory Tracking Control To illustrate the utility and further evaluate the validity of the proposed averaged model, we consider the trajectory tracking control problem as an example. 4.4.1 Trajectory Tracking Problem Let the vectors F̄(𝑡) = [𝑋 𝑌 𝜓] 𝑇 and T̄(𝑡) = [𝑋𝑟 𝑌𝑟 𝜓𝑟 ] 𝑇 denote the position (of the center) and orientation of the robotic fish and the desired position/orientation, respectively, with respect to the inertial frame {I} at a given time 𝑡. Let 𝒆 𝜒 = [𝑋𝑒 𝑌𝑒 𝜓 𝑒 ] 𝑇 denote the tracking error vector expressed in the robot’s body-fixed frame: 𝒆 𝜒 = 𝐼 𝑹 𝐵 ( F̄ − T̄) (4.48) 101 where 𝐼 𝑹 𝐵 is the rotation matrix from the inertial frame {I} to the body-fixed frame {B} and it is defined as where  cos 𝜓 sin 𝜓 0   𝐼𝑹 =    𝐵  − sin 𝜓 cos 𝜓 0   (4.49)    0 0 1  Following the procedure outlined in Chapter 3, the following error state model augmented with the averaged dynamics (Eq. (4.28)), is obtained  ¤    𝑋𝑒   𝑣¯ 𝑐𝑥 − 𝑽𝑟 cos(𝜓 𝑒 − 𝜓¯ 𝑟 ) + 𝜔¯ 𝑧𝑌𝑒       𝑌¤𝑒   𝑣¯ 𝑐 + 𝑽𝑟 sin(𝜓 𝑒 − 𝜓¯ 𝑟 ) − 𝜔¯ 𝑧 𝑋𝑒     𝑦   𝜓¤ 𝑒      𝜔¯ 𝑧 − 𝜔 𝑟   = (4.50)  𝑣 𝑐𝑥   𝑓1 + 𝐾 𝑓𝑥 𝑅 · 𝑓¯ℎ𝑥 𝑅 + 𝐾 𝑓𝑥 𝐿 · 𝑓¯ℎ𝑥 𝐿   ¤̄       𝑣¤̄ 𝑐   𝑓 + 𝐾 ¯ ¯  𝑓 𝑦 𝑅 · 𝑓 ℎ 𝑦 𝑅 + 𝐾 𝑓 𝑦 𝐿 · 𝑓 ℎ 𝑦 𝐿   𝑦  2  𝜔¤̄ 𝑧   𝑓3 + 𝐾𝑚 · 𝜏¯ℎ + 𝐾𝑚 · 𝜏¯ℎ        𝑅 𝑅 𝐿 𝐿  q where 𝑽𝑟 = 𝑢¤𝑟2 + 𝑣¤ 𝑟2 , 𝜓¯ 𝑟 = arctan( 𝑢𝑣𝑟𝑟 ), 𝜔𝑟 = 𝜓, ¤ and 𝑢𝑟 , 𝑣 𝑟 and 𝜔𝑟 are some surge, sway and angular velocities, respectively, used to generate the desired trajectory such that 𝑋¤ 𝑟 = 𝑢𝑟 cos 𝜓𝑟 − 𝑣 𝑟 sin 𝜓𝑟 and 𝑌¤𝑟 = 𝑢𝑟 sin 𝜓𝑟 + 𝑣 𝑟 cos 𝜓𝑟 . By formulating the tracking problem in terms of the error dynamics, the control objective has become a stabilization problem, where the task is to find a control law such that, for an arbitrary initial error, the position and orientation error states (𝑋𝑒 ,𝑌𝑒 ,𝜓 𝑒 ) of system (4.50) converge to the neighborhood of the origin. 4.4.2 Trajectory Tracking Control Algorithm To design the controller, let the control inputs be chosen as 𝑢 1 =𝐾 𝑓𝑥 𝑅 · 𝑓¯ℎ𝑥 𝑅 + 𝐾 𝑓𝑥 𝐿 · 𝑓¯ℎ𝑥 𝐿 (4.51) 𝑢 2 =𝐾 𝑓 𝑦 𝑅 · 𝑓¯ℎ 𝑦 𝑅 + 𝐾 𝑓 𝑦 𝐿 · 𝑓¯ℎ 𝑦 𝐿 (4.52) 𝑢 3 =𝐾𝑚 𝑅 · 𝜏¯ℎ 𝑅 + 𝐾𝑚 𝐿 · 𝜏¯ℎ 𝐿 (4.53) such that the averaged dynamic model (4.28) is expressed in the control-affine form as 102 Trajectory tracking Fin Control [] v1 Robotic Fish Dynamics Controller v2 γL γ ALγ 0 L ζ L T pL ˙ (t ) T̄ (t ) , T̄˙ (t ) Backstepping v3 γ ARγ 0 R ζ R T pR F̄ (t ), F̄ [] Controller u1 γR u2 u3 [] T̄ (t ) ψr λ1 λ2 Inner Loop λ3 Auxiliary System Desired λ4 [][] λ˙ 1 λ˙ 4 Trajectory λ5 λ˙ 2 , λ˙ 5 λ6 λ˙ 3 λ˙ 6 Outer Loop Figure 4.6: Illustration of the proposed dual-loop pectoral fin-actuated robotic fish control scheme. The green dashed line encompasses the outer loop trajectory tracking controller, while the inner red dotted and dashed line encompasses the fin parameter optimization algorithm.  ¤̄    𝑣 𝑐 𝑥   𝑓1 + 𝑢 1       𝑣¤̄ 𝑐  =  𝑓 + 𝑢  (4.54)  𝑦  2 2 ¤̄      𝜔 𝑧   𝑓3 + 𝑢 3      where the inputs appearing linearly. To achieve trajectory tracking, we use a backstepping controller that determines the inputs 𝑢 1 , 𝑢 2 and 𝑢 3 needed such that (𝑋𝑒 , 𝑌𝑒 , 𝜓 𝑒 ) → 0. We then determine the fin-beat parameters, considering their practical constraints, such that the generated inputs are close to the desired values. The difference between the generated and desired input values is captured by the states of an auxiliary system, which is used along with the backstepping controller to guarantee closed-loop stability, as is similarly done in [94, 132]. Figure 4.6 illustrates the control scheme. We elaborate the controller design below. Let 𝑣 1 , 𝑣 2 and 𝑣 3 represent the nominal inputs from the backstepping design, and let 𝑢 1 , 𝑢 2 and 𝑢 3 be the inputs that can be practically implemented. To analyze the influence of the mismatch 103 between the nominal and actual inputs, the following auxiliary system is chosen  𝜆¤ 1 = −𝜁1𝜆 1 + 𝜆 2         𝜆¤ = −𝜁2𝜆 2 + (𝑢 1 − 𝑣 1 ) + 𝑌𝑒 (𝑢 3 − 𝑣 3 )    2      𝜆¤ 3 = −𝜁3𝜆 3 + 𝜆 4     (4.55) 𝜆¤ 4 = −𝜁4𝜆 4 + (𝑢 2 − 𝑣 2 ) − 𝑋𝑒 (𝑢 3 − 𝑣 3 )         𝜆¤ 5 = −𝜁5𝜆 5 + 𝜆 6         𝜆¤ 6 = −𝜁6𝜆 6 + (𝑢 3 − 𝑣 3 )     where 𝜁1 , 𝜁2 , 𝜁3 , 𝜁4 , 𝜁5 , 𝜁6 are positive tuning constants. The variables 𝜆 1 , 𝜆 3 and 𝜆5 represent the filtered effect of the non-achievable portion of the inputs as well as the additional tracking error that arises because of the mismatch between the nominal and implementable inputs. To stabilize the 𝒆 𝜒 error while also considering the difference between nominal and achievable inputs, we define the following Lyapunov function 1 1 1 𝑉1 ( 𝑋¯ 𝑒 , 𝑌¯𝑒 , 𝜓¯ 𝑒 ) = 𝑋¯ 𝑒2 + 𝑌¯𝑒2 + 𝜓¯ 𝑒2 2 2 2 (4.56) 1 1 1 = (𝑋𝑒 − 𝜆 1 ) 2 + (𝑌𝑒 − 𝜆 3 ) 2 + (𝜓 𝑒 − 𝜆 5 ) 2 2 2 2 where 𝑋¯ 𝑒 , 𝑌¯𝑒 and 𝜓¯ 𝑒 are the modified tracking errors. The time derivative of Eq. (4.56) is given by 𝑉¤1 = 𝑋¯ 𝑒 ( 𝑣¯ 𝑐𝑥 − 𝑽𝑟 cos(𝜓 𝑒 − 𝜓¯ 𝑟 ) + 𝜔¯ 𝑧𝑌𝑒 + 𝜁1𝜆 1 − 𝜆 2 ) + 𝑌¯𝑒 ( 𝑣¯ 𝑐 𝑦 + 𝑽𝑟 sin(𝜓 𝑒 − 𝜓¯ 𝑟 ) − 𝜔¯ 𝑧 𝑋𝑒 + 𝜁3𝜆 3 − 𝜆 4 ) (4.57) + 𝜓¯ 𝑒 ( 𝜔¯ 𝑧 − 𝜔𝑟 + 𝜁5𝜆 5 − 𝜆 6 ) Let the 𝛼1 = 𝑣¯ 𝑐𝑥 , 𝛼2 = 𝑣¯ 𝑐 𝑦 and 𝛼3 = 𝜔¯ 𝑧 denote the virtual inputs with 𝛼𝑑1 , 𝛼𝑑2 and 𝛼𝑑3 as the corresponding desired virtual inputs. The modified virtual inputs errors are then defined as 𝑍¯ 1 = 𝛼1 − 𝛼𝑑1 − 𝜆 2 (4.58) 𝑍¯ 2 = 𝛼2 − 𝛼𝑑2 − 𝜆 4 (4.59) 𝑍¯ 3 = 𝛼3 − 𝛼𝑑3 − 𝜆 6 (4.60) 104 Let the desired virtual inputs be given by 𝛼𝑑1 = 𝑽𝑟 cos(𝜓 𝑒 − 𝜓¯ 𝑟 ) − 𝜔¯ 𝑧𝑌𝑒 − 𝜁1𝜆1 − 𝐾 𝑋𝑒 ¯ 𝑋¯ 𝑒 (4.61) 𝛼𝑑2 = − 𝑽𝑟 sin(𝜓 𝑒 − 𝜓¯ 𝑟 ) + 𝜔¯ 𝑧 𝑋𝑒 − 𝜁3𝜆 3 − 𝐾𝑌¯𝑒 𝑌¯𝑒 (4.62) 𝛼𝑑3 =𝜔𝑟 − 𝜁5𝜆 5 − 𝐾𝜓¯ 𝑒 𝜓¯ 𝑒 (4.63) such that (4.57) becomes 𝑉¤1 = 𝑋¯ 𝑒 ( 𝑍¯ 1 − 𝐾 𝑋𝑒¯ 𝑋¯ 𝑒 ) + 𝑌¯𝑒 ( 𝑍¯ 2 − 𝐾𝑌¯𝑒 𝑌¯𝑒 ) + 𝜓¯ 𝑒 ( 𝑍¯ 3 − 𝐾𝜓¯ 𝑒 𝜓¯ 𝑒 ) (4.64) where 𝐾 𝑋𝑒¯ , 𝐾𝑌¯ 𝑒 and 𝐾𝜓 𝑒 are positive tuning constants. To account for the virtual input errors, we then define a new Lyapunov function 1 1 1 𝑉¯2 = 𝑉¯1 + 𝑍¯ 12 + 𝑍¯ 22 + 𝑍¯ 32 (4.65) 2 2 2 with its time derivative is given by 𝑉¤̄2 = 𝑉¤̄1 + 𝑍¤̄ 1 𝑍¯ 1 + 𝑍¤̄ 2 𝑍¯ 2 + 𝑍¤̄ 3 𝑍¯ 3 (4.66) Eq. (4.66) can be further expanded using Eq. (4.28) along with the input definition Eq. (4.51). After simplification, 𝑣 1 , 𝑣 2 and 𝑣 3 can be obtain from −1   1 0 𝑌𝑒    𝑣  1      Γ1  𝑣  = 0 1 −𝑋𝑒  Γ   2  (4.67)      2  𝑣 3  0 0 1  Γ3        where Γ1 = − 𝑓1 − 𝑓3𝑌𝑒 + 𝑽¤𝑟 cos(𝜓 𝑒 − 𝜓¯ 𝑟 ) − 𝑽𝑟 sin(𝜓 𝑒 − 𝜓¯ 𝑟 )( 𝜓¤ 𝑒 − 𝜓¤̄ 𝑟 ) − 𝜔¯ 𝑧𝑌¤𝑒 (4.68a) ¤̄ − 𝐾 𝑋𝑒 ¯ 𝑋𝑒 − 𝜁1 (−𝜁1 𝜆 1 + 𝜆 2 ) − 𝜁2 𝜆 2 − 𝐾1 𝑍¯ 1 Γ2 = − 𝑓2 + 𝑓3 𝑋𝑒 − 𝑽¤𝑟 sin(𝜓 𝑒 − 𝜓¯ 𝑟 ) + 𝜔¯ 𝑧 𝑋¤ 𝑒 − 𝑽𝑟 cos(𝜓 𝑒 − 𝜓¯ 𝑟 )( 𝜓¤ 𝑒 − 𝜓¤̄ 𝑟 ) (4.68b) − 𝐾𝑌¯ 𝑒𝑌¤̄𝑒 − 𝜁3 (−𝜁3𝜆 3 + 𝜆 4 ) − 𝜁4𝜆 4 − 𝐾2 𝑍¯ 2 Γ3 = − 𝑓3 + 𝜔¤ 𝑟 − 𝜁5 (−𝜁5𝜆 5 + 𝜆 6 ) − 𝜁6𝜆 6 − 𝐾𝜓¯ 𝑒 𝜓¤̄ 𝑒 − 𝐾3 𝑍¯ 3 (4.68c) 105 By adding and subtracting 4𝐾1 𝑍¯ 12 , 4𝐾1 𝑍¯ 22 and 4𝐾1 𝑍¯ 32 to Eq. (4.66), and after completing the ¯ 𝑋𝑒 𝑌¯ 𝑒 ¯ 𝜓𝑒 square one can arrive at 1 ¯ 2 1 ¯ 2 1 ¯ 2 ¯2 1 𝑉¤̄2 = − 𝐾 𝑋𝑒 ¯ ( 𝑋¯ 𝑒 − 𝑍1 ) − 𝐾𝑌¯𝑒 (𝑌¯𝑒 − 𝑍2 ) − 𝐾𝜓𝑒 ( 𝜓¯ 𝑒 − 𝑍3 ) − 𝑍1 (𝐾1 − ) 2𝐾 𝑋¯ 𝑒 2𝐾𝑌¯𝑒 2𝐾𝜓¯ 𝑒 4𝐾 𝑋¯ 𝑒 1 1 − 𝑍¯ 22 (𝐾2 − ) − 𝑍¯ 32 (𝐾3 − ) 4𝐾𝑌¯𝑒 4𝐾𝜓¯ 𝑒 (4.69) 1 1 1 If 𝐾 𝑋¯ 𝑒 > 0, 𝐾𝑌¯𝑒 > 0,𝐾𝜓¯ 𝑒 > 0, 𝐾1 > , 𝐾2 > and 𝐾3 > , then 𝑉¤̄2 < 0 4𝐾 𝑋¯ 𝑒 4𝐾𝑌¯𝑒 4𝐾𝜓¯ 𝑒 unless when 𝑋¯ 𝑒 = 𝑌¯𝑒 = 𝜓¯ 𝑒 = 𝑍¯ 1 = 𝑍¯ 2 = 𝑍¯ 3 = 0, implying the convergence of ( 𝑋¯ 𝑒 , 𝑌¯𝑒 , 𝜓¯ 𝑒 ) to zero as time approaches infinity. Given that 0 ≤ 𝑉¯1 (𝑡) ≤ 𝑉¯1 (0), one can conclude that ( 𝑋¯ 𝑒 , 𝑌¯𝑒 , 𝜓¯ 𝑒 ) belongs to L2 , which implies that even when the desired force and moment are not implemented, the quantities 𝑋¯ 𝑒 , 𝑌¯𝑒 and 𝜓¯ 𝑒 do not diverge. Note that while the convergence for the modified tracking errors 𝑋¯ 𝑒 , 𝑌¯𝑒 and 𝜓¯ 𝑒 is guaranteed, that of the actual tracking error 𝒆 𝜒 is not. The latter may actually increase during periods when input limitations are in effect and the desired values cannot be implemented (i.e. 𝑢 1 ≠ 𝑣 1 , 𝑢 2 ≠ 𝑣 2 and/or 𝑢 3 ≠ 𝑣 3 ). However, when the control signal limitations are not in effect, and 𝜆 1 , 𝜆 2 and 𝜆3 approach zero, ( 𝑋¯ 𝑒 , 𝑌¯𝑒 , 𝜓¯ 𝑒 ) converges towards (𝑋𝑒 , 𝑌𝑒 , 𝜓 𝑒 ) and the tracking errors can be stabilized. Let 𝑣 1 , 𝑣 2 and 𝑣 3 represent the nominal inputs obtained from the backstepping design. One needs to determine the (feasible) fin-beat parameters such that the resultant inputs 𝑢 1 , 𝑢 2 , and 𝑢 3 are close to 𝑣 1 , 𝑣 2 , and 𝑣 3 , respectively. Note that to ease the discussion, in this work we fix 𝑇𝑝𝐿 , 𝜁 𝐿 , 𝑇𝑝𝑅 and 𝜁 𝑅 , leaving (𝛾0𝑅 , 𝛾 𝐴𝑅 ) and (𝛾0𝐿 , 𝛾 𝐴𝐿 ) as the fin parameters to be found. We use a constrained multi-variable minimization solver, in particular, a controlled, elitist genetic algorithm available in MATLAB (a variant of NSGA-II [141]), to find the best fin parameters. Given 𝑣 1 , 𝑣 2 106 and 𝑣 3 , the constrained multi-variable optimization problem at time 𝑡 is posed as  (𝐾 𝑓𝑥 𝑅 · 𝑓¯ℎ𝑥 𝑅 + 𝐾 𝑓𝑥 𝐿 · 𝑓¯ℎ𝑥 𝐿 − 𝑣 1 ) 2          argmin (𝐾 𝑓 𝑦 𝑅 · 𝑓¯ℎ 𝑦 𝑅 + 𝐾 𝑓 𝑦 𝐿 · 𝑓¯ℎ 𝑦 𝐿 − 𝑣 2 ) 2 𝛾0𝑅 ,𝛾 𝐴𝑅 ,𝛾0𝐿 ,𝛾 𝐴𝐿      (𝐾𝑚 𝑅 · 𝜏¯ℎ 𝑅 + 𝐾𝑚 𝐿 · 𝜏¯ℎ 𝐿 − 𝑣 3 ) 2     subject to: 𝛾 𝐴𝑅 , 𝛾 𝐴𝐿 ∈ [𝛾 𝐴 min , 𝛾 𝐴 max ] 𝛾0𝑅 , 𝛾0𝐿 ∈ [𝛾0 min , 𝛾0 max ] (4.70) where [𝛾 𝐴 min , 𝛾 𝐴 max ] and [𝛾0 min , 𝛾0 max ] represent the ranges of the fin-beat amplitude and bias, respectively. 4.5 Trajectory Tracking Simulation Results Simulation is conducted to evaluate the effectiveness of the designed controller, where the following parameters for the simulation are used: ¯ =0.3 𝐾 𝑋𝑒 𝐾𝑌¯ 𝑒 =0.2 𝐾𝜓𝑒 ¯ =0.3 𝐾1 =1.03 𝐾2 = 1.45 𝐾3 =1.03 𝜁1 =0.1 𝜁2 =0.1 𝜁3 =0.1 𝜁4 =0.2 𝜁5 =0.2 𝜁6 =0.2 𝛾 𝐴min = 0◦ 𝛾 𝐴max = 50◦ 𝛾0min = 40◦ 𝛾0max = 100◦ 𝑇𝑝 =1 s 𝜁 =3 𝑡 𝑠 =1 s where 𝐾 𝑋𝑒 ¯ , 𝐾1 , 𝐾2 , 𝐾3 , 𝜁1 , 𝜁2 , 𝜁3 , 𝜁4 , 𝜁5 , 𝜁6 are tuning parameters for the backstepping ¯ , 𝐾𝑌¯ 𝑒 , 𝐾𝜓𝑒 controller an auxiliary system. Note that although the controller is designed using the averaged model, inputs from the controller are applied to the original dynamics described by Eq. (4.22). The following line and circular trajectories are considered  𝑋¤ 𝑟 𝑌¤𝑟 = 𝑣 𝑟 , 𝜓¤ 𝑟 = 𝜔𝑟 , with      =𝑢𝑟 ,     𝑢𝑟   =0.03, 𝑣 𝑟 = 0, 𝜔𝑟 = 0 and (4.71) 𝑋¤ 𝑟 =𝑅1 𝜔𝑟 cos(𝜔𝑟 𝑡), 𝑌¤𝑟 = −𝑅1 𝜔𝑟 sin(𝜔𝑟 𝑡),        𝜓¤ 𝑟    =𝜔𝑟 , with 𝑅1 = 0.25, 𝜔𝑟 = 0.09,  107 Robotic Fish State Figure 4.7: Simulation: position, heading angle and velocity tracking for the line-tracking case. Figure 4.8: Simulation: position, heading angle and velocity tracking for the circle-tracking case. where 𝑋¤ 𝑟 and 𝑌¤𝑟 represent the velocity of the trajectory in the {I} frame. In Figures 4.7-4.8 the desired and the closed-loop trajectories of the robotic fish are compared for both the line and circle-tracking cases. Furthermore, the surge, sway and angular velocities for the desired trajectory and robotic fish are presented. Finally, Figure 4.9 depicts the desired versus the achievable inputs 108 Figure 4.9: Simulation: desired input versus achievable inputs for the line tracking case. for the line tracking case. From the results, we can see that the robot is able to track the desired position and heading angle trajectories. 4.6 Conclusion In this work, we presented a nonlinear dynamic average model for a pectoral fin-actuated robotic fish. In particular, we proposed a scaling averaging scheme, where the pectoral fin-generated hydrodynamic forces and moment are first scaled using functions of the fin-beat parameters, and classical averaging is then conducted over the resulting dynamics. Furthermore, we proposed a novel estimation scheme employing a nonlinear model predictive controller and a multivariate nonlinear regression scheme to determine the scaling functions. To evaluate the averaged model, simulation and experimental results comparing the predictions from the original and average models were presented. Furthermore, the utility of the proposed modeling scheme was demonstrated via the design of a backstepping-based trajectory tracking controller. 109 CHAPTER 5 RAPID MANEUVERING CONTROL OF PECTORAL FIN-ACTUATED ROBOTIC FISH Although useful in practical applications, cyclic fin movement limits precise manipulation of the fin movements and the thrust profile that can be generated, which impedes full exploitation of the maneuverability of pectoral fin-actuated robotic fish. On the other hand, acyclic thrust or moments could be more instrumental for generating a quick maneuvering response, which can be valuable in scenarios like counteracting disturbances or avoiding fast obstacles. While rowing motion is beneficial in maneuvering, the actuation constraints (i.e., angular position, velocity, and acceleration limitations) and the mechanism in which the “drag-based” swimming method is used to generate thrust give rise to challenges for maneuvering control of robotic fish. In particular, the range constraint of the fin movement can often inhibit the robot from generating thrust in a direction required for maneuvering. The latter could necessitate the fin moving first in a direction opposite to the desired one (which in turn generates unwanted drag) in order to “back up” and create enough room for accelerating. While seeming natural for fish or humans, such fin maneuvers are difficult to engineer with existing control design methods. To overcome these challenges and achieve quick maneuvering control, in this work, we propose a dual-loop control approach composed of a backstepping-based controller in the outer loop and a fin movement-planning algorithm in the inner loop. In particular, for the inner loop, we propose a model-predictive planning scheme based on a randomized sampling algorithm that accommodates the fins’ constraints and “intelligently” determines the necessary fins’ movements to produce a desired thrust despite the fins’ current configuration. Simulation results are presented to demonstrate the performance of the proposed scheme via comparison with a nonlinear model predictive controller (NMPC) in rapid velocity maneuvering. The rest of this chapter is organized as follows. We first review the dynamic model of the pectoral fin-actuated robotic fish in Section 5.1. In Section 5.2, we present the proposed control approach in detail. In Section 5.3, simulation results are discussed. Finally, we provide some 110 concluding remarks in Section 5.4. 5.1 Dynamic Model of Pectoral Fin-actuated Robotic Fish The dynamic model of the robotic fish is presented in Section 4.1 of Chapter 4. By letting 𝛾¥ 𝑅 = 𝜔¤ 𝑅 , 𝛾¥ 𝐿 = 𝜔¤ 𝐿 , 𝑢 1 = 𝜔¤ 𝑅 and 𝑢 2 = 𝜔¤ 𝐿 , and by considering the kinematic equations of the robotic fish, the dynamic model can be summarized as follows:  ¤   𝑉𝑐 cos 𝜓 − 𝑉𝑐 sin 𝜓   𝑋   𝑥 𝑦      𝑌¤   𝑉𝑐𝑥 sin 𝜓 + 𝑉𝑐 𝑦 cos 𝜓       𝜓¤      𝜔𝑧    ¤   𝑓 (𝑉𝑐 , 𝑉𝑐 , 𝜔 𝑧 ) + 𝑓 ℎ𝑥     𝑉𝑐𝑥   1 𝑥 𝑦 𝑚1  𝑉¤   𝑓 (𝑉 , 𝑉 , 𝜔 ) + 𝑓 ℎ𝑦      𝑐 𝑦   2 𝑐𝑥 𝑐 𝑦 𝑧 𝑚2   = (5.1)  𝜔¤ 𝑧   𝑓 (𝑉 , 𝑉 , 𝜔 ) + 𝜏ℎ𝑧     3 𝑐𝑥 𝑐 𝑦 𝑧 𝐽3   𝛾¤ 𝐿          𝜔𝐿   𝛾¤     𝑅  𝜔     𝑅   𝜔¤ 𝐿       𝑢1    𝜔¤ 𝑅          𝑢 2   with  𝑚2 𝑐1 q 2 2     𝑓 (𝑉 , 1 𝑐𝑥 𝑐 𝑦𝑉 , 𝜔 𝑧 ) = 𝑉 𝜔 𝑐𝑦 𝑧 − 𝑉 𝑐 𝑥 𝑉𝑐 𝑥 + 𝑉𝑐 𝑦 +    𝑚1 𝑚1   q 𝑉 𝑐2 2 + 𝑉 2 arctan( 𝑐 𝑦 )    𝑉 𝑐 𝑉𝑐 𝑐 𝑚1 𝑦  𝑥 𝑦    𝑉𝑐𝑥   q 𝑚 𝑐 𝑓2 (𝑉𝑐𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) = − 1 𝑉𝑐𝑥 𝜔 𝑧 − 1 𝑉𝑐 𝑦 𝑉𝑐2𝑥 + 𝑉𝑐2𝑦   (5.2)   𝑚2 𝑚2   q 𝑉 𝑐2 2 + 𝑉 2 arctan( 𝑐 𝑦 )  −   𝑉𝑐 𝑉 𝑚 2 𝑥 𝑐𝑥 𝑐𝑦      𝑉𝑐𝑥 (𝑚 − 𝑚 2 )  𝑓3 (𝑉𝑐𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) = 1  𝑉𝑐𝑥 𝑉𝑐 𝑦 − 𝑐 4 𝜔2𝑧 sgn(𝜔 𝑧 )       𝐽 3 where 𝑚 1 = 𝑚 𝑏 −𝑚 𝑎 𝑥 , 𝑚 2 = 𝑚 𝑏 −𝑚 𝑎 𝑦 , 𝐽3 = 𝐽𝑏𝑧 −𝐽𝑎 𝑧 , 𝑐 1 = 12 𝜌𝑆𝐶 𝐷 , 𝑐 2 = 12 𝜌𝑆𝐶 𝐿 , 𝑐 4 = (𝐽1 ) 𝐶 𝑀 . 3 Note that 𝑓 ℎ𝑥 = 𝑓 ℎ𝑥 𝑅 + 𝑓 ℎ𝑥 𝐿 , 𝑓 ℎ 𝑦 = 𝑓 ℎ 𝑦 𝑅 + 𝑓 ℎ 𝑦 𝐿 and 𝜏ℎ𝑧 = 𝜏ℎ𝑧𝑅 + 𝜏ℎ𝑧𝐿 . 111 Velocity Tracking [ ] Vc ωr xr e η Controller F h Backstepping [ ] Mh xd zd Fin Control γL Robotic Fish Dynamics γ¨L γ˙L γ L γ¨R γ˙R γ R [Vω ] cx z Controller γR [λλ ] 1 Auxiliary System [ ] Fh Inner Loop [ ] x 2 Mh Fh − Fh λ˙ 1 =−ζ λ 1 + x xd z m1 Mh − Mh λ˙ 2 =−ζ λ 2 + x xd J3 Outer Loop Figure 5.1: Illustration of the proposed dual-loop pectoral fin-actuated robotic fish control scheme. The green dashed line encompasses the outer loop velocity tracking controller, while the inner red dotted and dashed line encompasses the fin movement planning and control algorithm. 5.2 Dual-Loop Fin Control Scheme Using the validated model proposed in Section 4.1, in this section we propose a dual-loop fin control scheme to achieve velocity tracking for a pectoral fin-actuated robotic fish. 5.2.1 Velocity Tracking Problem The velocity tracking problem involves controlling the robot to track desired body-fixed velocity trajectories that are parameterized in time t. Given the underactuated nature and input coupling of the robotic fish, tracking three velocities is challenging [94]; since the main focus of this work is in fin control we only consider tracking of the surge and angular velocity and leave the former for future work. Let the velocity tracking error at time 𝑡 be given by " # " # 𝑒𝑣 𝑉𝑐𝑥 − 𝑉𝑐𝑥𝑟 (𝑡) 𝒆𝜼 = = (5.3) 𝑒𝜔 𝜔 𝑧 − 𝜔𝑟 (𝑡) For brevity we drop the time-dependence for the remainder of the chapter. By formulating the tracking problem in terms of the error state, the control objective becomes a stabilization problem, where the task is to find suitable control laws for 𝑢 1 , 𝑢 2 such that for an arbitrary initial error, the states (𝑒 𝑣 , 𝑒 𝜔 ) of system (5.3) can be held near the origin (0,0). 112 5.2.2 Velocity Tracking Control Algorithm To achieve velocity tracking, a dual loop control structure is proposed. The outer loop is composed of a backstepping controller that determines the thrust and moment needed such that 𝒆 𝜼 → 0. In particular, with sampling time 𝑡 𝑠 , the robotic fish and auxiliary system states are sampled. The robot’s velocities are compared to the reference velocities 𝑉𝑐𝑥𝑟 and 𝜔𝑟 and the tracking error 𝒆 𝜼 is then calculated. The error 𝒆 𝜼 is used by the outer loop controller (i.e., the backstepping controller) to determine the thrust 𝐹ℎ𝑥 𝑑 and moment 𝑀ℎ 𝑧 𝑑 needed to drive 𝒆 𝜼 to the origin. On the other hand, the inner loop is composed of a pectoral fin controller that determines the inputs 𝑢 1 and 𝑢 2 such that the pectoral fins generate a thrust 𝐹ℎ𝑥 and moment 𝑀ℎ 𝑧 that are close to the desired values 𝐹ℎ𝑥 𝑑 and 𝑀ℎ 𝑧 𝑑 . The robotic fish dynamics are then propagated using 𝑢 1 and 𝑢 2 , and the difference between the generated and desired thrust is captured by the states of an auxiliary system which is used along with the backstepping controller to guarantee closed-loop stability. At time 𝑡 + 𝑡 𝑠 the process repeats. Figure 5.1 illustrates the proposed method. A detailed overview of proposed control approach will be provided in the following sub-sections. 5.2.3 Outer-loop Tracking Control Synthesis Backstepping control is a practical and systematic approach that provides stability guarantees, which makes it an attractive choice for velocity tracking. In this work, a backstepping-based controller is proposed to determine the thrust and moments necessary to drive the states (𝑒 𝑣 , 𝑒 𝜔 ) of the error system (5.3) to a neighborhood of the origin. In order to successfully stabilize the error states to the origin and guarantee closed-loop sta- bility, the backstepping controller must accommodate magnitude constraints on the thrust 𝑓 ℎ𝑥 and moments 𝜏ℎ 𝑧 that can be generated by the pectoral fins at a given time. In order to address this limitations, we adopt a similar scheme to that proposed in [94, 132]. Let 𝐹ℎ𝑥 𝑑 and 𝑀ℎ 𝑧 𝑑 represent the nominal backstepping control inputs, and let 𝐹ℎ𝑥 and 𝑀ℎ 𝑧 be the forces and moments that that can be practically implemented by the fins. Note that the 𝐹ℎ𝑥 and 𝑀ℎ 𝑧 will be determined by the inner loop fin controller. To analyze the influence of the input 113 difference, the following auxiliary system is chosen 𝐹ℎ − 𝐹ℎ𝑥 𝑑 𝜆¤ 1 = − 𝜁1𝜆1 + 𝑥 (5.4a) 𝑚1 𝑀ℎ 𝑧 − 𝑀ℎ 𝑧 𝑑 𝜆¤ 2 = − 𝜁2𝜆2 + (5.4b) 𝐽3 The auxiliary system composed of the variables 𝜆 1 and 𝜆 2 defined above represents the filtered effect of the non-achievable portion of the control inputs. In other words, they represent the addi- tional tracking error that arises because of the mismatch between the nominal and implementable forces and moments. To stabilize the (𝑒 𝑣 , 𝑒 𝜔 ) subsystem while also considering the difference in inputs, the following candidate Lyapunov function is proposed 1 1 1 1 𝑉1 = 𝑒¯2𝑣 + 𝑒¯2𝜔 = (𝑒 𝑣 − 𝜆 1 ) 2 + (𝑒 𝜔 − 𝜆 2 ) 2 (5.5) 2 2 2 2 where 𝑒¯𝑣 and 𝑒¯𝜔 are the modified tracking errors. The time derivative of Eq. (5.5) is given by 𝑉¤1 =𝑒¯𝑣 𝑒¤̄𝑣 + 𝑒¯𝜔 𝑒¤̄𝜔 𝐹ℎ 𝑑 𝑀ℎ 𝑧 𝑑 (5.6) =𝑒¯𝑣 ( 𝑓1 (𝑉𝑐 𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) − 𝑉¤𝑐 𝑥𝑟 + 𝑥 + 𝜁1 𝜆1 ) + 𝑒¯𝜔 ( 𝑓3 (𝑉𝑐 𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) − 𝜔¤ 𝑟 + + 𝜁2 𝜆 2 ) 𝑚1 𝐽3 Let 𝐹ℎ𝑥𝑑 and 𝑀ℎ 𝑧𝑑 be given by 𝐹ℎ =𝑚 1 (− 𝑓1 (𝑉𝑐 𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) + 𝑉¤𝑐 𝑥𝑟 − 𝜁1 𝜆1 − 𝐾𝑒¯𝑣 𝑒¯𝑣 ) (5.7a) 𝑥𝑑 𝑀ℎ 𝑧 𝑑 =𝐽3 (− 𝑓3 (𝑉𝑐 𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) + 𝜔¤ 𝑟 − 𝜁2 𝜆2 − 𝐾𝑒¯𝜔 𝑒¯𝜔 ) (5.7b) such that (5.6) becomes 𝑉¤1 = − 𝐾𝑒¯𝑣 𝑒¯2𝑣 − 𝐾𝑒¯𝜔 𝑒¯2𝜔 (5.8) If 𝐾𝑒¯𝑣 > 0 and 𝐾𝑒¯𝜔 > 0, then 𝑉¤̄1 < 0 except when 𝑒¯𝑣 = 𝑒¯𝜔 = 0 implying the convergence of ( 𝑒¯𝑣 , 𝑒¯𝜔 ) to zero as time approaches infinity. Given that 0 ≤ 𝑉¯1 (𝑡) ≤ 𝑉¯1 (0), one can conclude that ( 𝑒¯𝑣 , 𝑒¯𝜔 ) belongs to L2 , which implies that even when the desired force and moment are not implemented, the quantities 𝑒¯𝑣 and 𝑒¯𝜔 do not diverge. While the convergence for the modified tracking errors 𝑒¯𝑣 and 𝑒¯𝜔 is guaranteed, that of the actual velocity tracking errors 𝑒 𝑣 and 𝑒 𝜔 is not, 114 as the latter may actually increase during periods when the force and moment limitations are in effect and the desired values cannot be implemented (i.e. 𝐹ℎ𝑥 ≠ 𝐹ℎ𝑥 𝑑 or 𝑀ℎ 𝑧 ≠ 𝑀ℎ 𝑧 𝑑 ). On the other hand, when the control signal limitations are not in effect (i.e., 𝐹ℎ𝑥 = 𝐹ℎ𝑥 𝑑 and 𝑀ℎ 𝑧 = 𝑀ℎ 𝑧 𝑑 ), 𝜆1 and 𝜆 2 approach zero, and (𝑒¯𝑣 , 𝑒¯𝜔 ) converges towards (𝑒 𝑣 , 𝑒 𝜔 ) and thus the velocity error can be stabilized. From Eq. (4.21) and Eqs. (5.7a)-(5.7b), the desired left and right pectoral fin forces can be determined as 𝐽 𝑓 ℎ𝑥 𝑑𝐿 = 3 (− 𝑓3 (𝑉𝑐 𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) + 𝜔¤ 𝑟 + 𝜁2 𝜆2 − 𝐾𝑒¯𝜔 𝑒¯𝜔 ) 2𝐶 𝑝 (5.9) 𝑚 − 1 (− 𝑓1 (𝑉𝑐 𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) + 𝑉¤𝑐 𝑥𝑟 − 𝜁1 𝜆1 − 𝐾𝑒¯𝑣 𝑒¯𝑣 ) 2 𝐽3 𝑓 ℎ𝑥 𝑑𝑅 = (− 𝑓3 (𝑉𝑐 𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) + 𝜔¤ 𝑟 + 𝜁2 𝜆2 − 𝐾𝑒¯𝜔 𝑒¯𝜔 ) 2𝐶 𝑝 (5.10) 𝑚 + 1 (− 𝑓1 (𝑉𝑐 𝑥 , 𝑉𝑐 𝑦 , 𝜔 𝑧 ) + 𝑉¤𝑐 𝑥𝑟 − 𝜁1 𝜆1 − 𝐾𝑒¯𝑣 𝑒¯𝑣 ) 2 5.2.4 Pectoral Fin Control Algorithm The goal of the pectoral fin control algorithm is to determine the inputs 𝑢 1 and 𝑢 2 such that pectoral fin movement generates forces that track the desired forces 𝑓 ℎ𝑥 𝑑𝐿 and 𝑓 ℎ𝑥 𝑑𝑅 . To achieve this goal, we propose a model-predictive planning algorithm to determine an angular acceleration trajectory for each fin that accommodates the feasibility of the fin movement while producing a thrust that is close to the desired value for a given interval of time 𝑇𝑝 , where 𝑇𝑝 = 𝑡 𝑠 . The general procedure to determine the angular trajectory is as follows: 𝑇𝑃 is discretized into multiple evenly spaced sub-intervals. For the first sub-interval, a fixed number of 𝐵 different possible (constant) fin acceleration choices are generated by randomly sampling a distribution (the design of which will be discussed later), and their corresponding angular velocities, positions and force trajectories within the said sub-interval are calculated based on a constant-acceleration model. For the next sub-interval, for each choice previously generated, a new set of 𝐵 choices for the acceleration are generated and once again the fins’angular velocities, positions and force trajectories within the sub-interval are calculated for all choices. The process repeats until the total 115 number of sub-intervals is reached. In this manner the number of choices per sub-interval increases exponentially with the sub-interval stage. Figure 5.2 depicts an example of the general idea, where different possible acceleration, velocity and position trajectories generated for a planning interval 𝑇𝑃 are shown. The assemblage of individual angular accelerations values (and corresponding position and velocities) from each interval is considered a plausible angular trajectory for the period 𝑇𝑝 . Each possible angular trajectory is assigned a cost that is dependent on the difference between its corresponding generated force trajectory and the desired value. The trajectory that yields the lowest cost is selected as the solution. To elaborate on the control algorithm in detail, we utilize the right fin as an example; however, the same approach can be trivially extended to the left pectoral fin. Figure 5.2: Example of different possible fin acceleration, velocity and position trajectories generated within a planning interval 𝑇𝑃 that is divided into three sub-intervals. The outer loop control inputs are updated every 𝑡 𝑠 seconds and new 𝑓 ℎ𝑥 𝑑𝐿 and 𝑓 ℎ𝑥 𝑑𝑅 forces are calculated. For the duration of 𝑡 𝑠 , the desired forces remain constant and become the tracking reference for the fin control algorithm. Note that 𝑇𝑝 = 𝑡 𝑠 . Let 𝑇𝑃 be discretized into 𝑛0 evenly spaced sub-intervals of length Δ𝑡 and let 𝛾¥ 𝑅𝑖 be constant throughout the 𝑖th sub-interval, where 𝑖 = 1, · · · , 𝑛0 . At a given 𝑖th sub-interval there will be 𝐵𝑖 different choices for 𝛾¥ 𝑅 , such that for a given period 𝑇𝑃 there area a total of 𝐵𝑛0 possible choices of (piecewise constant) acceleration 116 trajectories. For a given choice of angular accelerations up to the beginning of the 𝑖th sub-interval, the following elaborates on the procedure for the related computation: 1. Determine allowed range for 𝜸¥ 𝑹 : Given that the pectoral fins position 𝛾 𝑅 is physically limited, the allowed angular velocities 𝛾¤ 𝑅 and angular acceleration 𝛾¥ 𝑅 are constrained to lie within a range dependent on the current fin’s position and velocity. Since 𝛾¥ 𝑅 is constant throughout each sub-interval, using the standard constant acceleration model the allowed 𝛾¥ 𝑅 within Δ𝑡 can be determined as follows (𝑖) (𝑖) 2(𝛾 𝑅𝑚𝑎𝑥 − 𝛾 𝑅0 − 𝛾¤ 𝑅0 Δ𝑡) 𝛾¥ 𝑅(𝑚𝑎𝑥)𝑖 = (5.11a) (Δ𝑡) 2 (𝑖) (𝑖) 2(𝛾 𝑅𝑚𝑖𝑛 − 𝛾 𝑅0 − 𝛾¤ 𝑅0 Δ𝑡) 𝛾¥ 𝑅(𝑚𝑖𝑛)𝑖 = (5.11b) (Δ𝑡) 2 where 𝛾 𝑅𝑚𝑎𝑥 and 𝛾 𝑅𝑚𝑖𝑛 denote the maximum and minimum allowed fin position, respectively, while (𝑖) (𝑖) 𝛾 𝑅0 and 𝛾¤ 𝑅0 denote the value of 𝛾 𝑅 and 𝛾¤ 𝑅 at the beginning of the 𝑖th sub-interval, respectively. 2. Calculate desired 𝜸¥ 𝑹𝒅 : Given 𝑓 ℎ𝑥 𝑑𝑅 , a desired 𝛾¥ 𝑅𝑑 is calculated from Eq. (4.19) as follows (𝑖) (𝑖) 𝜆𝜌𝐷 𝑝 𝑆 2𝑝 ( 𝛾¤ 𝑅0 ) 2 sgn( 𝛾¤ 𝑅0 ) 𝛾¥ 𝑅𝑑𝑖 = − 3𝑚 𝑝 (𝑖) 2 (𝑖) (5.12) ( 𝛾¤ 𝑅0 ) cos 𝛾 𝑅0 2 𝑓 ℎ𝑥 𝑅𝑑 − + (𝑖) (𝑖) sin 𝛾 𝑅0 𝑆 𝑝 𝑚 𝑝 sin 𝛾 𝑅0 3. Generate the sampling distribution and sample 𝑩 values : The 𝐵 different 𝛾¥ 𝑅 values are sampled from a distribution which is generated by dividing the allowable 𝛾¥ 𝑅𝑖 range into 𝑁 evenly spaced discrete values. Each possible 𝑛th value is assigned a normalized weight 𝑊𝑛 determined by the following: 𝑒 −𝑑𝑛 𝑊𝑛 = Í (5.13) 𝑁 𝑒 −𝑑𝑛 𝑛=1 q (𝑛) where 𝑛 = 1, · · · 𝑁, and 𝑑𝑛 = ( 𝛾¥ 𝑅𝑖 − 𝛾¥ 𝑅𝑑 ) 2 is the Euclidean distance between the 𝑛th 𝛾¥ 𝑅𝑖 value 𝑁 𝑊 = 1. and the desired 𝛾¥ 𝑅𝑑𝑖 . Note Σ𝑛=1 𝑛 117 Figure 5.3: Example of weights for each value of 𝛾¥ 𝑅𝑖 within a given allowed range. The vertical dotted red lines indicate the maximum and minimum values allowed for 𝛾¥ 𝑅𝑖 in the 𝑖th interval, while the green solid line represents the desired value. To sample from this distribution, generate 𝑃 “particles", where each “particle" is representative of each discrete value of 𝛾¥ 𝑅𝑖 . The number of particles generated for a particular discrete value of 𝛾¥ 𝑅𝑖 is given by 𝑃 𝑛 = 𝑊𝑛 𝑃 (5.14) Í𝑁 Note that 𝑃𝑛 must be rounded to the nearest integer and that 𝑛=1 𝑃𝑛 = 𝑃. Figure 5.3 illustrates an example of the weights calculated for a given range of 𝛾¥ 𝑅𝑖 . Finally, 𝐵 “particles" are uniformly sampled from the whole set of 𝑃 particles, which results in random choices of 𝛾¥ 𝑅𝑖 with preference towards values closer to 𝛾¥ 𝑅𝑑 . (𝑖) (𝑖) 4. Calculate 𝜸 𝑹𝒊 and 𝜸¤ 𝑹𝒊 : For a given 𝛾¥ 𝑅𝑖 , the resultant 𝛾¤ 𝑅 and 𝛾 𝑅 trajectories can be obtained from the constant acceleration model as follows (𝑖) (𝑖) 1 𝛾 𝑅 (𝑡𝑖 , 𝑡𝑖+1 ) =𝛾 𝑅0 + 𝛾¤ 𝑅0 𝑡 + 𝛾¥ 𝑅𝑖 𝑡 2 (5.15a) 2 (𝑖) (𝑖) 𝛾¤ 𝑅 (𝑡𝑖 , 𝑡𝑖+1 ) =𝛾¤ 𝑅0 + 𝛾¥ 𝑅𝑖 𝑡 (5.15b) where 𝑡 ∈ [𝑡𝑖 , 𝑡𝑖+1 ]. (𝒊) 5. Determine the force 𝒇 𝒉 𝑹 for a given 𝜸¥ 𝑹𝒊 : From Eq. (4.19), the force generated by the 𝒙 118 fin in the 𝑖th interval is calculated as (𝑖) 1 (𝑖) (𝑖) (𝑖) 𝑓 ℎ 𝑅 (𝑡𝑖 , 𝑡𝑖+1 ) = 𝜆𝜌𝐷 𝑝 𝑆 3𝑝 ( 𝛾¤ 𝑅 ) 2 sin 𝛾 𝑅 sgn( 𝛾¤ 𝑅 ) 𝑥 6 (5.16) 𝑆 𝑝 (𝑖) (𝑖) 𝑆 𝑝 (𝑖) 2 (𝑖) − 𝑚 𝑝 (− 𝛾¥ 𝑅 sin 𝛾 𝑅 − ( 𝛾¤ ) cos 𝛾 𝑅 ) 2 2 𝑅 6. Calculate cost for each 𝜸¥ 𝑹𝒊 : A cost is assigned to each generated acceleration choice within the 𝑖th sub-interval. The cost function is given by  ∫ 𝑡𝑖+1 2 (𝑖) 𝐾𝑖 = 𝑓 ℎ 𝑅 (𝜏) − 𝑓 ℎ𝑥 𝑅𝑑 𝑑𝜏 (5.17) 𝑡𝑖 𝑥 7. Determine the best 𝜸¥ 𝑹 trajectory among the 𝑩 𝒏0 candidates : 𝑛0 Õ min 𝐾𝑖 (5.18) 𝛾¥ 𝑅 (·) 𝑝=1 Note that in steps 1-5 sets of possible accelerations for each interval are generated, while in the steps 6-7 the best candidate is determined. Furthermore, by sampling from the skewed distribution presented in step 3, we take into consideration the ideal 𝛾¥ 𝑅𝑑 , which allows us to make an educated guess as to what possible 𝛾¥ 𝑅 should be generated in order to find a good solution. Finally, since the algorithm considers the angular position constraints, a plausible 𝛾¥ 𝑅 trajectory is always generated. Once the best 𝛾¥ 𝑅 and 𝛾¥ 𝐿 are selected, the total hydrodynamic force 𝐹ℎ𝑥 and moment 𝑀ℎ 𝑧 that will be exerted by the robotic fish can be calculated, and the auxiliary system states can be updated. 5.3 Simulation Results To evaluate the effectiveness of the designed controller, simulations were carried out using MATLAB. The robotic fish parameters used for simulation are listed in Table 4.1. Furthermore, the backstepping controller and planning algorithm parameters were chosen as follows: 𝐾𝑒¯𝑣 = 1.5 𝐾𝑒¯𝜔 = 2 𝜁1 = 0.1 𝜁2 = 0.2 𝛾 𝑅𝑚𝑖𝑛 = 𝛾 𝐿𝑚𝑖𝑛 = 45◦ 𝛾 𝑅𝑚𝑎𝑥 = 𝛾 𝐿𝑚𝑎𝑥 = 120◦ 𝑡 𝑠 = 0.2 s 𝑇𝑃 = 0.2 s 𝑛0 = 3 𝐵=5 𝑁 = 20 𝑃 = 100 119 where the variable 𝑡 𝑠 is the sampling interval which pertains to the amount of time between an update to the desired force and moment values, 𝐹ℎ𝑥 𝑑 and 𝑀ℎ 𝑧 𝑑 . The backstepping controller parameters were chosen such that under the right values the velocity error system was stabilized to a neighborhood of the origin. We found that 𝐾𝑒¯𝑣 and 𝐾𝑒¯𝜔 regulate the balance between the convergence rate of the 𝑒 𝑣 and 𝑒 𝜔 error, respectively, while varying 𝜁1 and 𝜁2 regulates the convergence rate of the control-deviation errors 𝜆 1 and 𝜆 2 , respectively. The randomized model- predictive planning scheme parameters, 𝑛0 , 𝐵, 𝑁 and 𝑃, were chosen as to balance the trade-off between computational effort and performance. To demonstrate the effectiveness of the proposed approach, simulations were carried out to compare its performance with an alternative approach, where NMPC is used as the inner loop fin controller. The cost function Eq. (5.17) was adopted to the NMPC algorithm, and the NMPC parameters were chosen as follows: Prediction Horizon= 0.2 s Control Intervals = 30 𝑄=5000 𝑡 𝑠 =0.2 s where the weighting matrix 𝑄 was chosen to heavily penalize the deviation from the desired force and the force being generated , and the prediction horizon was chosen as the length of the sampling time 𝑡 𝑠 . The following reference velocity trajectory, with abrupt changes, was considered in simulations   = 0.01 m/s, 𝜔𝑟 = −0.15 rad/s 𝑡 < 0.8     𝑉𝑐 𝑥𝑟     = 0.04 m/s, 𝜔𝑟 = 0.2 rad/s 0.8 ≤ 𝑡 < 1.6     𝑉𝑐 𝑥𝑟         𝑉𝑐 𝑥𝑟 = −0.01 m/s, 𝜔𝑟 = 0.4 rad/s 1.6 ≤ 𝑡 < 2.2          𝑉𝑐 𝑥𝑟 = 0.01 m/s, 𝜔𝑟 = −0.1 rad/s 2.2 ≤ 𝑡 < 2.4      𝑉𝑐 𝑥𝑟 = 0.01 m/s, 𝜔𝑟 = 0 rad/s 2.4 ≤ 𝑡 < 3     = 0.01 m/s, 𝜔𝑟 = −0.04 rad/s 3 ≤ 𝑡 < 3.4  𝑉𝑐 𝑥𝑟       3.4 ≤ 𝑡 < 4     𝑉𝑐 𝑥𝑟 = 0.04 m/s, 𝜔𝑟 = 0.1 rad/s          𝑉𝑐 𝑥𝑟 = −0.05 m/s, 𝜔𝑟 = 0.1 rad/s 4 ≤ 𝑡 < 4.4      𝑉𝑐 𝑥𝑟  = 0 m/s, 𝜔𝑟 = 0 rad/s 4.4 ≤ 𝑡 ≤ 5  120 Surge Velocity 0.05 Robotic Fish Reference m/s 0 -0.05 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 t(s) Angular Velocity 0.5 Robotic Fish Reference rad/s 0 -0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 t(s) (a) Proposed approach. Surge Velocity 0.05 Robotic Fish Reference m/s 0 -0.05 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 t(s) Robotic Fish Angular Velocity Reference 0.5 rad/s 0 -0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 t(s) (b) NMPC approach. Figure 5.4: Simulation: velocity-tracking trajectories results for the proposed scheme (a) and the alternative scheme with NMPC for the inner loop controller (b). Note that the above desired velocities include scenarios like backwards swimming ( 1.6 ≤ 𝑡 < 2.2 ), quick left and right turning, and stopping( 4.4 ≤ 𝑡 ≤ 5). In Figure 5.4 the desired and the closed-loop velocity trajectories of the robotic fish are depicted for the proposed method (Figure 5.4a) and the alternative NMPC scheme (Figure 5.4b) , while Figure 5.5 illustrates the fin positions over time for each scheme. From Figure 5.4 one can see that 121 (a) Proposed approach. (b) NMPC approach. Figure 5.5: Simulation: fin position trajectories resulting from the proposed scheme (a) and the alternative scheme with NMPC for the inner loop controller (b). The dashed blue lines depict the fin’s position constraints. the proposed approach allows the robot to respond to the sudden changes of the desired velocities. In particular, although there are slight overshoot the robot was able to change its surge and angular velocity velocities to keep up with the quickly varying reference. In contrast, although the NMPC approach is able to handle the fin range constraints, its tracking performance is inferior when 122 compared to the proposed approach. Finally, from Figure 5.5, one can note that for both approaches the fin’s positions constraints are not violated. 5.4 Chapter Summary In this work, a systematic approach for maneuvering control of a pectoral-fin actuated robotic fish was proposed. Specifically, we proposed a dual loop control scheme consisting of an outer- loop backstepping controller and an inner loop fin movement-planning algorithm. In particular, the outer loop backstepping-controller finds the thrust and moment required to stabilize the velocity tracking error, while the inner loop plans the motion of the fin for a given time-interval to produce a thrust and a moment close to their desired values by utilizing a randomized sampling algorithm. A parameter estimation scheme was employed to empirically identify the hydrodynamic parameters of the model. To illustrate the challenges in control, the velocity tracking problem with abrupt velocity changes for a robotic fish was considered. Simulation results showed the effectiveness of the proposed scheme and and its superiority over an alternative employing an NMPC in the inner loop. 123 CHAPTER 6 SUMMARY AND FUTURE WORK 6.1 Summary This dissertation presented several systematic model-based control approaches for tail-actuated and pectoral fin-actuated robotic fish that guarantee closed-loop system stability, accommodate input constraints, and are computationally viable for these robots. We first proposed and implemented in real-time a path-following NMPC scheme for a tail- actuated robotic fish, where a high-fidelity averaged nonlinear dynamic model was used for con- troller design. A novel parameter estimation scheme was employed to empirically identify the averaged model’s hydrodynamic parameters and scaling coefficients. Furthermore, given that the control inputs were functions of two of the tail-beat parameters, specifically the tail bias and tail am- plitude, a control projection strategy was implemented to handle these nonlinear input constraints and maximize the use of the admissible control region in a computationally efficient manner. Finally, simulation and experimental results demonstrated the effectiveness of the proposed scheme. Despite being a promising tracking control approach, NMPC’s computational complexity posed challenges in implementation in resource-constrained robots. We thus proposed a backstepping- based control that is practical, systematic, and computationally inexpensive, especially when com- pared to NMPC. We first considered the trajectory tracking of under-actuated planar robotic systems and demonstrated how the controller guarantees the ultimate boundedness of the position and head- ing tracking errors to a neighborhood of the origin. In particular, the controller was synthesized to guarantee the convergence of the longitudinal error and a new coordinate error that coupled the lateral error and the heading error. Via time-scale analysis of perturbed systems, it was shown that the convergence of these errors guarantees the stabilization of the whole error system to a neigh- borhood around the origin. Furthermore, the proposed scheme was implemented on a tail-actuated robotic fish, where a high-fidelity averaged nonlinear dynamic model was used for controller design. 124 Finally, real-time experimental results demonstrated the effectiveness of the proposed scheme and showed its value over standard PI control. Moving forward, we then turned our attention to pectoral fin-actuated robotic fish. In practical applications like trajectory tracking, it is more natural to control the parameters of periodic fin beats. To this end, we presented a nonlinear dynamic average model for a pectoral fin-actuated robotic fish that is amenable to controller design, where the control inputs are actuation pattern parameters. In particular, we proposed a scaling averaging scheme, where the pectoral fin-generated hydrodynamic forces and moment are first scaled using functions of the fin-beat parameters, and classical averaging is then conducted over the resulting dynamics. Furthermore, we proposed a novel estimation scheme employing a nonlinear model predictive controller and a multivariate nonlinear regression scheme to determine the scaling functions. To validate the averaged model simulation comparing the predictions from the original and average models were presented. Furthermore, both models were validated with experimental results. Finally, the utility of the proposed modeling scheme was demonstrated via the design of a dual-loop backstepping-based trajectory tracking controller. Simulation results demonstrate the effectiveness of the proposed control approach. Acyclic actuation could be more instrumental than cyclic actuation in tasks such as rapid maneuvering. To overcome the challenges pectoral fins pose in control, a systematic approach for maneuvering control of a pectoral-fin actuated robotic fish was proposed. Specifically, we proposed a dual-loop control scheme consisting of an outer-loop backstepping controller and an inner-loop fin movement-planning algorithm. The outer-loop backstepping-controller finds the thrust and moment required to stabilize the velocity tracking error, while the inner-loop plans the motion of the fin for a given time interval to produce a thrust and a moment close to their desired values by utilizing a randomized sampling algorithm. To illustrate the challenges in control, the velocity tracking problem with abrupt velocity changes for a robotic fish was considered. Simulation results showed the effectiveness of the proposed scheme and its superiority over an alternative employing an NMPC in the inner loop. 125 6.2 Future Work For future work, experimental trials that validate the proposed trajectory tracking control scheme for a pectoral fin-actuated robotic fish will be implemented. Furthermore, the proposed pectoral fin-actuated robotic fish maneuvering scheme will be optimized to accommodate the trade-off between performance and computational efficiency. In addition, experiments will be implemented on a robotic fish prototype to verify the effectiveness of the proposed approach. The work presented in this dissertation can also be expanded in different directions. First, it would be interesting to compare the trajectory tracking performance of controllers using the original dynamics versus the averaged dynamics when considering trajectories with sharp turns or sudden changes. Furthermore, since the original dynamic model captures transient behavior while the averaged dynamic model captures the steady state behavior, it would be of interest to develop frameworks that allow the coordination of two separate model-based controllers that are designed to implement pectoral fin-based locomotion. Finally, since the tail fin tends to be most beneficial at higher speeds, while pectoral fins tend to be more effective for maneuvering, it would be interesting to develop control algorithms that allow the coordination of the two forms of locomotion. In addition, it also would be interesting to investigate the use of Linear Parameter Varying (LPV) models for control of robotic fish [142, 143]. For example, one could explore the use of model predictive control or linear quadratic regulator control approaches along with the LPV model to implement trajectory tracking control. In another direction, it would be interesting to utilize the trajectory tracking control algorithms in an environmental sensing application, where an upper-level path planning scheme is integrated with one of the proposed trajectory tracking schemes for active sensing. We have done some preliminary work along this direction, where we used an ergodic exploration algorithm for trajectory planning along with an NMPC trajectory tracking controller [144]. Finally, while model-based control tends to be effective, it is challenging to obtain high-fidelity and accurate models that are amenable to control synthesis especially for dynamic, uncertain environmental conditions. Furthermore, it can be difficult to estimate parameters, either offline 126 and online, accurately. Data-driven methods such as Koopman operators are promising approaches that can facilitate learning control-affine models. In our recent work, we have explored the use of Koopman operators for developing linear models and implementing real-time velocity control of robotic fish [145, 146]. Exploring these data-driven techniques can be useful for robotic fish due to the highly nonlinear dynamics and the need for controllers that use limited computation. 127 APPENDICES 128 APPENDIX A PROOF OF THE ROBOTIC FISH’S INPUT-TO-STATE STABILITY Consider the robotic fish dynamics given by Eq. (3.67a)-(3.67c), where 𝑓 is locally Lipschitz in 𝑥 and 𝑢, and 𝑢(𝑡) is a piece-wise continuous, bounded function of 𝑡 for all 𝑡 ≥ 0. To determine if the system is input-to-state stable we apply the result from [129](Lemma 4.6). Unforced System Let 𝑥¯ = [ 𝑥¯1 𝑥¯2 𝑥¯3 ] 𝑇 = [𝑣 1 𝑣 2 𝜔]. The unforced system for the robotic fish is then given by 𝑥¤̄ = 𝑓 ( 𝑥, ¯ 0) (A.1) and in particular 𝑚2 𝑐 q 𝑐 q 𝑥¯ 𝑥¤̄1 = 𝑥¯2 𝑥¯3 − 1 𝑥 1 𝑥¯12 + 𝑥¯22 + 2 𝑥¯2 𝑥¯12 + 𝑥¯22 arctan 2 (A.2a) 𝑚1 𝑚1 𝑚1 𝑥¯1 𝑚 𝑐 q 𝑐 q 𝑥¯ 𝑥¤2 = − 1 𝑥¯1 𝑥¯3 − 1 𝑥¯2 𝑥¯12 + 𝑥¯22 − 2 𝑥¯1 𝑥¯12 + 𝑥¯22 arctan 2 (A.2b) 𝑚2 𝑚2 𝑚2 𝑥¯1 (𝑚 1 − 𝑚 2 ) 𝑥¤3 = 𝑥¯1 𝑥¯2 − 𝑐 4 𝑥¯32 sgn( 𝑥¯3 ) (A.2c) 𝐽3 To prove that this system has a global asymptotically stable equilibrium point at the origin, consider the following Lyapunov function 𝑚1 2 𝑚2 2 1 2 𝑉 (𝑥) = 𝑥¯ + 𝑥¯ + 𝑥¯ (A.3) 2𝐽3 1 2𝐽3 2 2 3 such that 𝑚 𝑚 𝑉¤ (𝑥) = 1 𝑥 1 𝑥¤̄1 + 2 𝑥¯2 𝑥¤̄2 + 𝑥¯3 𝑥¤̄3 𝐽3 𝐽3 𝑚 𝑚 = 2 − 1 𝐽3 𝐽3 (𝑚 1 − 𝑚 2 )  𝑥¯ 𝑐 q 𝑐  + 𝑥¯1 𝑥¯2 𝑥¯3 + 2 − 2 𝑥¯1 𝑥¯2 𝑥¯12 + 𝑥¯22 arctan 2 (A.4) 𝐽3 𝐽3 𝐽3 𝑥¯1 q q 𝑐 𝑐 − 1 𝑥¯12 𝑥¯12 + 𝑥¯22 − 1 𝑥¯22 𝑥¯12 + 𝑥¯22 − 𝑐 4 𝑥 33 sgn( 𝑥¯3 ) 𝐽3 𝐽3 q q 𝑐 𝑐 = − 1 𝑥¯12 𝑥¯12 + 𝑥¯22 − 1 𝑥¯22 𝑥¯12 + 𝑥¯22 − 𝑐 4 𝑥 33 sgn( 𝑥¯3 ) 𝐽3 𝐽3 129 𝑉 (𝑥) is thus negative definite globally and it can be concluded that the origin is globally asymptot- ically stable (G.A.S). State Boundedness Let 𝑥¤̄ = 𝑓 ( 𝑥, ¯ 𝑢) be a perturbation of the unforced system 𝑥¤ = 𝑓 ( 𝑥, ¯ 0). The function 𝑓 ( 𝑥, ¯ 𝑢) satisfies the following k 𝑓 ( 𝑥, ¯ 𝑢) − 𝑓 ( 𝑥, ¯ 0)k ≤ 𝐿 k𝑢k 2 𝑚𝐿 𝜔2 𝑢 𝐾 𝑓 12𝑚 1 1 𝛼 (A.5) 2 2 𝐾 𝑓 𝑚𝐿 4𝑚 2 𝜔 𝛼 𝑢 2 ≤ 𝐿 k𝑢k 2 −𝐾𝑚 c𝑚𝐿 2 4𝐽3 𝜔 𝛼 𝑢 2 where 𝐿 is a Lipschitz constant. From Definition 4.7 and Theorem 4.19 in [129], the ultimate bound of the states can be determined. Considering the derivative of Eq.(A.3) along 𝑓 ( 𝑥, ¯ 𝑢) 130 𝜕𝑉 𝑚 𝑚 ¯ 𝑢) = 1 𝑥¯1 𝑥¤̄1 + 2 𝑥¯2 𝑥¤̄2 + 𝑥¯3 𝑥¤̄3 𝑓 ( 𝑥, 𝜕 𝑥¯ 𝐽3 𝐽3 q q 𝑐1 2 2 𝑐1 2 2 = − 𝑥¯1 𝑥¯1 + 𝑥¯2 − 𝑥¯2 𝑥¯1 + 𝑥¯22 2 𝐽3 𝐽3 𝑚 𝑚𝐿 2 2 − 𝑐 4 𝑥33 sgn( 𝑥¯3 ) + 𝑥¯1 1 𝐾 𝑓 𝜔 𝑢 𝐽3 12𝑚 1 𝛼 1 𝑚 𝑚𝐿 2 2 c𝑚𝐿 2 2 + 𝑥¯2 2 𝐾 𝑓 𝜔𝛼 𝑢 2 − 𝑥 3 𝐾𝑚 𝜔 𝑢 𝐽3 4𝑚 2 4𝐽3 𝛼 2 q q 𝑐1 2 2 𝑐1 2 2 ≤ − 𝑥¯1 𝑥¯1 + 𝑥¯2 − 𝑥¯2 𝑥¯1 + 𝑥¯22 − 𝑐 4 𝑥¯33 sgn( 𝑥¯3 ) 2 𝐽3 𝐽3 𝐾 𝑓 𝑚𝐿 2 2 𝐾 𝑓 𝑚𝐿 2 2 + 𝜔 | 𝑥¯ ||𝑢 | + 𝜔𝛼 | 𝑥¯2 ||𝑢 2 | 12𝐽3 𝛼 1 1 4𝐽3 c𝑚𝐿 2 2 + 𝐾𝑚 𝜔 | 𝑥¯ ||𝑢 | 4𝐽3 𝛼 3 2 𝑐 𝑐 ≤ − (1 − 𝜃 1 ) 1 𝑥¯12 − (1 − 𝜃 2 ) 1 𝑥¯22 (A.6) 𝐽3 𝐽3 𝑐 − (1 − 𝜃 3 )𝑐 4 𝑥¯33 sgn( 𝑥¯3 ) − 𝜃 1 1 𝑥¯12 𝐽3 𝑐 − 𝜃 2 1 𝑥¯22 − 𝜃 3 𝑐 4 𝑥¯33 sgn( 𝑥¯3 ) 𝐽3 𝐾 𝑓 𝑚𝐿 2 2 𝐾 𝑓 𝑚𝐿 2 2 + 𝜔 | 𝑥¯ ||𝑢 | + 𝜔𝛼 | 𝑥¯2 ||𝑢 2 | 12𝐽3 𝛼 1 1 4𝐽3 c𝑚𝐿 2 2 + 𝐾𝑚 𝜔 | 𝑥¯ ||𝑢 | 4𝐽3 𝛼 3 2 𝑐 𝑐 ≤ − (1 − 𝜃 1 ) 1 𝑥¯12 − (1 − 𝜃 2 ) 1 𝑥¯22 𝐽3 𝐽3 𝑐 𝑐 − (1 − 𝜃 3 )𝑐 4 𝑥¯33 sgn( 𝑥¯3 ) − 𝜃 1 1 𝑥¯12 − 𝜃 2 1 𝑥¯22 𝐽3 𝐽3 − 𝜃 3 𝑐 4 𝑥¯33 sgn( 𝑥¯3 ) ∀ | 𝑥¯1 | ≥ 𝜇1 , | 𝑥¯2 | ≥ 𝜇2 , | 𝑥¯3 | ≥ 𝜇3 𝐾 𝑓 𝑚𝐿 2 2 𝐾 𝑓 𝑚𝐿 2 2 𝐾𝑚 𝑐𝑚𝐿 2 2 where when 𝜇1 = 𝜔𝛼 |𝑢 1 |, 𝜇2 = 𝜔𝛼 |𝑢 2 | and 𝜇3 = 𝜔 |𝑢 |. 12𝜃 1 𝑐 1 4𝜃 2 𝑐 1 4𝐾 𝐷 𝜃 3 𝛼 2 From Definition 4.4 of [129], for any initial state 𝑥(𝑡0 ) and a bounded input 𝑢(𝑡), the solution 𝑥(𝑡) exists for all 𝑡 ≥ 𝑡0 and satisfies n o k𝑥k ≤ max 𝛽(k𝑥(𝑡0 )k, 𝑡 − 𝑡0 ), 𝛼1−1 (𝛼2 (𝜇)) ∀ 𝑡 ≥ 𝑡 0 (A.7) 131 where 𝛼1 (𝑟) = 𝜆 𝑚𝑖𝑛 (𝑃)𝑟 2 , 𝛼2 (𝑟) = 𝜆 max (𝑃)𝑟 2 . In particular, for 𝑥¯1 , 𝑥¯2 , and 𝑥¯3 , respectively, the ultimate bounds 𝑏 1 − 𝑏 3 are given by 𝐾 𝑓 𝑚𝐿 2 2 𝑏1 = 𝜔 |𝑢 | (A.8a) 12𝜃 1 𝑐 1 𝛼 1 𝐾 𝑓 𝑚𝐿 2 2 𝑏2 = 𝜔 |𝑢 | (A.8b) 4𝜃 2 𝑐 1 𝛼 2 𝐾𝑚 𝑐𝑚𝐿 2 2 𝑏3 = 𝜔 |𝑢 | (A.8c) 4𝐾 𝐷 𝜃 3 𝛼 2 132 APPENDIX B PROOF OF STABILITY OF THE PERTURBATION-FREE ROBOTIC FISH CLOSED-LOOP SYSTEM The asymptotic stability of the origin for the robotic fish system can be demonstrated by following the procedure from Section 3.3. Using the following definitions 𝑣 1 = 𝜒3 + 𝜙1 − 𝑦 𝑒 (𝜔 − 𝜙3 ) (B.1) 𝜔 = 𝜒4 + 𝜚𝜙3 (B.2) we can write the closed-loop system as    𝑣 + 𝑣 sin(𝜓 ) − 𝜒2 𝜒4 − 𝜒 𝜙   𝑦¤ 𝑒   2 𝑟 𝑒 𝑘 (𝑡,𝒆) 2 3     𝜒¤   −𝐾 𝑧 𝜉 𝜒1 + 𝜒4   1        𝜒¤ 2   −𝐾𝑥 𝑒 𝜒2 + 𝜒3   = (B.3)    𝜒¤ 3   −𝐾 𝑧1 𝜒3          𝜒¤   −𝐾 𝑧2 𝜒4   4     𝜂¤̄   ℎ2 (𝑣 1 , 𝜂, ¯    ¯ + (𝛼 )     𝜔) 𝑏 𝑓 4 5 0 𝑎 𝛼  , 𝛼 , 𝜔  where 𝜓 𝑒 =𝜒1 − 𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ) (B.4) 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 𝑘 (𝑡, 𝒆) =1 − (B.5) (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 The reduced Υ𝑆 -subsystem for the robotic fish is defined as 𝑦¤𝑒 = 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 ), 0) = −𝑣 𝑟 sin(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 )) Its corresponding boundary layer is given by −𝑎¯ 𝜒1      𝑑𝜒 −𝑏¯ 𝜒  2 = 𝑔(𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝜒(𝜏1 )), 0) =    𝑑𝜏1  −𝑐¯ 𝜒3     ¯  −𝑑 𝜒4    133 Assumption 4 of Lemma 1: 𝜕𝑉𝑆 𝜕𝑉 𝑉¤𝑆 = 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 )) + 𝑆 𝜕𝑦 𝑒 𝜕𝑡 𝑣 𝑟 𝑘 𝛿1 𝑘 𝛿2 sin(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 )) (B.6) =− (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 Let 𝑦 𝑒 lie within compact set such that 𝐾¯ 𝑦 𝑒 = max 𝑦 𝑒 ∈Ω |𝑦 𝑒 |. One can then find 𝑐 0 such that in an interval of 𝑦 𝑒 sin(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 )) 2 ≥ 𝑐 0 𝑦 2𝑒 (B.7) (𝑘 𝛿2 𝑦 𝑒 )2 +1 such that 𝑉¤𝑆 ≤ −𝑣 𝑟𝑚𝑖𝑛 𝑘 𝛿1 𝑘 𝛿2 𝑐 0 𝑦 2𝑒 (B.8) Since 𝑣 𝑟𝑚𝑖𝑛 , 𝑘 𝛿1 , 𝑘 𝛿2 > 0, Assumption 4 of Lemma 1 can be satisfied with 𝛼1 ≥ 𝑣 𝑟𝑚𝑖𝑛 𝑘 𝛿1 𝑘 𝛿2 𝑐 0 and Ψ1 (𝑦 𝑒 ) = |𝑦 𝑒 | Assumption 5 of Lemma 1: 𝜕𝑉𝐹 𝑔(𝑡, 𝑦 𝑒 , 𝜒, 0) = −𝜒𝑇 𝑆 𝜒 ≤ −𝜆¯ 𝑚𝑖𝑛 (𝑆)k 𝜒k 2 (B.9) 𝜕𝑥 where 𝜆¯ 𝑚𝑖𝑛 (𝑆) is the smallest eigenvalue of 𝑆. Let 𝛼2 ≥ 𝜆¯ 𝑚𝑖𝑛 (𝑆) and Φ1 = k 𝜒k Assumption 6 of Lemma 1: Let 𝑞 = 𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ), and (𝑣 𝑟 sin(𝜓 𝑒 ) + 𝑣 2 ) 𝜏 = 𝑘 𝛿1 𝑘 𝛿2 (B.10) (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 134 𝜕𝑉𝑆 [ 𝑓 (𝑡, 𝑦 𝑒 , 𝜒) − 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 ))] 𝜕𝑦 𝑒 " # 𝑘 𝛿1 𝑘 𝛿2 sin(𝑞)  𝜒4 + 𝜔𝑟 − 𝜏 − 𝐾 𝑧 𝜉 𝜒1  = 𝑣 𝑟 sin(𝜓 𝑒 ) − 𝜒2 + 𝑣 𝑟 sin(𝑞) (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 𝑘 (𝑡, 𝒆) 𝜒4 ((𝑘 𝛿2 𝑦 𝑒 ) 2 + 1) " 𝑘 𝛿1 𝑘 𝛿2 sin(𝑞)   = 𝑣 𝑟 sin(𝜓 𝑒 ) + sin(𝑞) − 𝜒2 (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 𝜔𝑟 ((𝑘 𝛿2 𝑦 𝑒 )2 + 1) 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 𝑣 𝑟 sin(𝜓 𝑒 ) 1 2 − − − (𝑘 𝛿2 𝑦 𝑒 )2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 (𝑘 𝛿2 𝑦 𝑒 )2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 1 2 𝐾 𝑧 𝜉 𝜒1 ((𝑘 𝛿2 𝑦 𝑒 ) + 1) !# (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1  𝑣 𝑟 sin( 𝜒1 ) cos(𝑞) − cos( 𝜒1 ) sin(𝑞) + sin(𝑞) " = 𝑘 𝛿1 𝑘 𝛿2 sin(𝑞) (B.11) (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 𝜒2 𝜔𝑟 − (𝑘 𝛿2 𝑦 𝑒 )2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 𝑣 𝑟 sin(𝜓 𝑒 )  # 𝜒2  1 2 − 𝜒4 − 𝐾 𝑧 𝜉 𝜒1 − (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1  1  𝑣 𝑟 𝑘 𝛿1 𝑘 𝛿2 sin(𝑞) sin( 𝜒1 ) cos(𝑞) + (1 − cos( 𝜒1 )) sin(𝑞) = (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 𝜒2 𝑘 𝛿1 𝑘 𝛿2 sin(𝑞)𝜔𝑟 𝑘 𝛿1 𝑘 𝛿2 sin(𝑞) 𝜒2  − − 𝜒4 − 𝐾 𝑧 𝜉 𝜒1 (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 1 (𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 )𝑣 𝑟 sin(𝜓 𝑒 )  1 2 − (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 Since 𝐾¯ 𝑦 𝑒 = max 𝑦 𝑒 ∈Ω |𝑦 𝑒 |, one can always find an interval for all 𝑦 𝑒 within the compact set | sin(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ))| where the term is increasing and is bounded such that (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 | sin(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ))| 𝑐 0 |𝑦 𝑒 | ≤ (B.12) (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 Furthermore, one can fin 𝑐 1 such that | sin(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ))| ≤ 𝑐 1 |𝑦 𝑒 | (B.13) (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 135 | sin(𝑞)| so that ≤ 𝑐 1 |𝑦 𝑒 |, and | sin(𝑞)| ≤ 𝑘 1 |𝑦 𝑒 |. Similarly, let 𝐾¯ 𝜒1 = max 𝜒∈Ω | 𝜒1 | such that (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 𝑑0 | 𝜒1 | ≤ (1 − cos( 𝜒1 )) ≤ 𝑑1 | 𝜒1 | (B.14) 𝑏 0 | 𝜒1 | ≤ sin( 𝜒1 ) ≤ 𝑏 1 | 𝜒1 | (B.15) Furthermore, given | sin(𝜓 𝑒 | ≤ 1, | cos( 𝜒)| ≤ 1, and | cos(𝑞)| ≤ 1, (B.11) can be rewritten as   𝜕𝑉𝑆 𝑣 𝑟 𝑘 𝛿 𝑘 1 2 𝛿 sin(𝑞) 𝑏 1 | 𝜒1 | cos(𝑞) + 𝑑 1 | 𝜒1 | sin(𝑞) [ 𝑓 (𝑡, 𝑦 𝑒 , 𝜒) − 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 ))] ≤ + 𝜕𝑦 𝑒 (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 | 𝜒2 |𝑘 𝛿1 𝑘 𝛿2 | sin(𝑞)||𝜔𝑟 | (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 𝑘 𝛿1 𝑘 𝛿2 | sin(𝑞)|| 𝜒2 |  + | 𝜒4 | + 𝐾 𝑧 𝜉 | 𝜒1 |+ (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 𝑣 𝑟 | sin(𝜓 𝑒 )|  1 2 (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 ≤ 𝑣 𝑟max (𝑏 1 + 𝑑1 )𝑐 1 𝑘 𝛿1 𝑘 𝛿2 |𝑦 𝑒 || 𝜒1 | 𝑘 1 𝑘 𝛿1 𝑘 𝛿2 |𝜔 || 𝜒2 ||𝑦 𝑒 | 𝑟 + (𝑘 𝛿2 𝑦 𝑒 )2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 𝑘 1 𝑘 𝛿1 𝑘 𝛿2 |𝑦 𝑒 || 𝜒2 |   + | 𝜒4 | + 𝐾 𝑧 𝜉 | 𝜒1 | (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 𝑣 𝑟max 𝑘 1 (𝑘 𝛿1 𝑘 𝛿2 ) 2 | 𝜒2 ||𝑦 𝑒 | + (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿2 𝜒2 1 (B.16) Assume that 𝜒2 is restricted to a compact set such that 𝐾¯ 𝜒2 = max 𝝌∈Ω | 𝜒2 |, and let 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 | 𝜒2 | | 𝜒2 | 1 2 < (B.17) (𝑘 𝑧 𝛿 𝑦 𝑒 )2 + 1 − 𝑘 𝑧 𝑒1 𝑘 𝑧 𝛿 𝜒2 𝐷𝑒 2 2 1 for | 𝜒2 | ≤ such that (B.16) becomes 𝑘 𝑧𝛿 𝑘 𝑧𝛿 1 2 136 𝑐 1 𝑘 𝛿1 𝑘 𝛿2 |𝜔𝑟 || 𝜒2 ||𝑦 𝑒 | ≤ 2𝑐 1 𝑣 𝑟max (𝑏 1 + 𝑑1 )𝑘 𝛿1 𝑘 𝛿2 |𝑦 𝑒 || 𝜒1 | + 𝐷𝑒 𝑐 1 𝑘 𝛿1 𝑘 𝛿2 |𝑦 𝑒 |𝐾 𝜒2 |   𝑣 𝑟 𝑐 1 (𝑘 𝛿1 𝑘 𝛿2 ) 2 | 𝜒2 ||𝑦 𝑒 | (B.18) + | 𝜒4 | + 𝐾 𝑧 𝜉 | 𝜒1 | + 𝐷𝑒 𝐷𝑒 ≤ 𝐾𝛿 |𝑦 𝑒 |k 𝜒k where 𝐾𝛿 =𝑣 𝑟max (𝑏 1 + 𝑑1 )𝑐 1 𝑘 𝛿1 𝑘 𝛿2 𝑘 1 𝑘 𝛿1 𝑘 𝛿2   (B.19) + |𝜔𝑟 | + 𝐾 𝜒2 + 𝐾 𝜒2 𝐾 𝑧 𝜉 + 𝑣 𝑟max (𝑘 𝛿1 𝑘 𝛿2 ) 𝐷𝑒 Let 𝛽1 ≥ 𝐾𝛿 and 𝜆1 = 0 to satisfy Assumption 6 of Lemma 1. Assumption 7.a of Lemma 1 𝜕𝑉𝐹 [𝑔(𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 , ), 𝜖) − 𝑔(𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 ), 0)] = 0 (B.20) 𝜕𝑦 𝑒 Let 𝜆 2 = 𝛽2 = 0 to satisfy the second interconnection condition from Assumption 7 of Lemma 1. Assumption 7.b of Lemma 1 𝜕𝑉𝐹 𝑓 (𝑡, 𝑦 𝑒 , 𝐺 (𝑡, 𝑦 𝑒 )) = 0 (B.21) 𝜕𝑦 𝑒 Let 𝜆3 = 𝛽3 = 0 to satisfy third interconnection condition from Assumption 7 of Lemma 1. The composite Lyapunov function for the Υ𝑆𝐹 -subsystem is given by 𝑉1 (𝑦 𝑒 , 𝑥, 𝑡) = (1 − 𝑑1 )𝑉𝑆 (𝑦 𝑒 ) + 𝑑1𝑉𝐹 (𝑥), 𝑑1 ∈ (0, 1) (B.22) = (1 − 𝑑1 )(1 − cos(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ))) + 𝑑1 ( 𝜒𝑇 𝑃 𝜒) and its derivative is given by, p  𝑇 p  |𝑦 𝑒 | 2   |𝑦 𝑒 | 2   ¤ 𝑉1 ≤ −   Λ      k 𝜒k   k 𝜒k    q   (B.23)  2 ≤ −𝜆 𝑚𝑖𝑛 (Λ) 𝑦 𝑒 + k 𝜒k 2 2 137 APPENDIX C PROOF OF STABILITY OF THE ROBOTIC FISH’S PERTURBED CLOSED-LOOP SYSTEM Let the perturbed Σ𝑆𝐹 system be expressed as  𝑓 (𝑡, 𝚺)    𝚺¤ = 𝑄(𝑡, 𝚺) + 𝑟 (𝑡, 𝚺) =  𝑔(𝑡, 𝚺)  + 𝑟 (𝑡, 𝚺)   (C.1)    0    where 𝚺 =[𝑦 𝑒 , 𝜒, 𝜂] ¯𝑇 (C.2) " # 𝜒4 +𝜙¯3 𝑓 (𝑡, 𝚺) = 𝑣 𝑟 sin(𝜓 𝑒 ) − ( 𝜚 ) 𝜒2 (C.3) −𝑎¯ 𝜒1 + 𝜖1 𝜒4        −𝑏¯ 𝜒 + 𝜖 𝜒  𝑔(𝑡, 𝚺) =  2 1 3  (C.4)    −𝑐¯ 𝜒3    −𝑑¯𝜒4       −𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 𝜂¯ 𝜒2   1 2    (𝑘 𝐶 𝑦 ) 2 + 1 − 𝑘 𝑘 𝜒 + ¯ 𝜂    𝑧𝛿 𝑒 𝑧𝛿 𝑧𝛿 2   2 1 2    0       𝑟 (𝑡, 𝚺) =   (C.5)  0       0      0      ℎ (𝑣 2 1 , ¯ 𝜂, 𝜔) + 𝑏 𝑢 4 2     (𝑣 𝑟 sin(𝜓 𝑒 )) 𝜙¯3 =𝜔𝑟 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 − 𝐾 𝑍𝜉 𝜒1 (C.6) 1 2 (𝑘 𝑧 𝑦 𝑒 ) 2 + 1 𝛿 2 From Eq. (B.22) " # 𝜕𝑉1 𝑣 𝑟 𝑘 𝛿1 𝑘 𝛿2 sin(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 )) = (1 − 𝑑1 ) 𝑑1 2𝜒𝑇 𝑃 (C.7) 𝜕𝚺 (𝑘 𝛿2 𝑦 𝑒 ) 2 + 1 138 Since it is assumed that 𝑦 𝑒 lies within a compact set such that 𝐾¯ 𝑦 𝑒 = max 𝑦 𝑒 ∈Ω |𝑦 𝑒 |, one can bound the sin(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 )) term in (C.7) by (B.13), such that 𝜕𝑉1 h i ≤ 𝑐 1 (1 − 𝑑1 )𝑣 𝑟max 𝑘 𝛿 𝑘 𝛿 |𝑦 𝑒 | 𝑑1 2k𝑃kk 𝜒k (C.8a) 𝜕𝚺 1 2 h i max ¯ ≤ 𝑐 1 (1 − 𝑑1 )𝑣 𝑟 𝑘 𝛿 𝑘 𝛿 |𝑦 𝑒 | 𝑑1 2𝜆 max (𝑃)k 𝜒k (C.8b) 1 2 ≤𝑁 k𝚺k (C.8c) where we write (C.8b) as 𝑦𝑇 𝑤 and use the inequality |𝑦𝑇 𝑤| ≤ k𝑤kk𝑦k, 𝜆¯ max (𝑃) is the maximum q 𝑐 1 (1 − 𝑑1 )𝑣 𝑟max 𝑘 𝛿1 𝑘 𝛿2 2 + (2𝑑1𝜆¯ max ( 𝑝)) 2 . Furthermore, recall  eigenvalue of P and 𝑁 = 𝜕𝑉1 q 2 2 𝑄(𝑡, 𝚺) ≤ −𝜆 min (Λ) (𝑦 𝑒 + k 𝜒k ) 2 (C.9) 𝜕𝚺 Since 𝑦 𝑒 lies within a compact set such that 𝐾¯ 𝑦 𝑒 = max 𝑦 𝑒 ∈Ω |𝑦 𝑒 |, one could find 𝑞 0 and 𝑞 1 such that 𝑉1 (𝚺) =(1 − 𝑑1 )(1 − cos(𝑘 𝛿1 arctan(𝑘 𝛿2 𝑦 𝑒 ))) + 𝑑1 𝜒𝑇 𝑃 𝜒 ≤(1 − 𝑑1 )𝑞 1 𝑦 2𝑒 + 𝑑1𝜆¯ max (𝑃) 𝜒2 (C.10) q ≤ ((1 − 𝑑1 )𝑞 1 ) 2 + (𝑑1𝜆¯ max (𝑃)) 2 ||𝚺|| 2 and 𝑉1 (𝚺) ≥(1 − 𝑑1 )𝑞 0 𝑦 2𝑒 + 𝑑1𝜆¯ min (𝑃) 𝜒2 q (C.11) ≥ ((1 − 𝑑1 )𝑞 0 ) 2 + (𝑑1𝜆¯ min (𝑃)) 2 ||𝚺|| 2 Therefore, for 𝚺 ∈ 𝐵 𝑅 , the Lyapunov function 𝑉1 satisfies inequalities (3.61) with 139 q Γ1 = ((1 − 𝑑1 )𝑞 0 ) 2 + (𝑑1𝜆¯ min (𝑃)) 2 k𝚺k 2 | {z } 𝑎¯ q Γ2 = ((1 − 𝑑1 )𝑞 1 ) 2 + (𝑑1𝜆¯ max (𝑃)) 2 k𝚺k 2 | {z } 𝑏¯ Γ3 = 𝜆 min (Λ)k𝚺k 2 (C.12) 𝜍 = 𝑁 k𝚺k Recall 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 | 𝜒2 | | 𝜒2 | 1 2 < (C.13) (𝑘 𝑧 𝛿 𝑦 𝑒 )2 + 1 − 𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 𝜒2 𝐷𝑒 2 1 2 1 for | 𝜒2 | ≤ . Eq. (C.5) can thus be rewritten as 𝑘 𝑧𝛿 𝑘 𝑧𝛿 1 2  𝑘 𝑧 𝑘 𝑧 | 𝜂||  𝛿1 𝛿2 ¯ 𝜒2 |  + | 𝜂|   ¯   𝐷𝑒       0   k𝑟 (𝑡, 𝚺)k ≤     0   0         0      ℎ2 (𝑣 1 , 𝜂, ¯ 𝜔) + 𝑏 4 𝑢 2   𝑘 𝑧 𝛿 𝑘 𝑧 𝛿 𝜗¯ ≤ ( 1 2 + 1)k𝚺k 𝐷𝑒 The right-hand side of (3.63) is given by 𝜃Γ3 (Γ−1 (Γ1 (𝑅))) 𝜆min (Λ) 𝑎¯ r 2 = 𝜃𝑅 (C.14) 𝜍 𝑁 𝑏¯ The derivative of 𝑉1 along the trajectories of (C.1) satisfies 𝜕𝑉 𝜕𝑉 𝑉¤1 (𝑡, 𝚺) = 1 𝑄(𝚺) + 1 𝑟 (𝚺, 𝑡) 𝜕𝚺 𝜕𝚺 ≤ − 𝜆 min (Λ)k𝚺k 2 + 𝜑𝑁 k𝚺k (C.15) ≤ − (1 − 𝜃)𝜆 min (Λ)k𝚺k 2 − 𝜃𝜆min (Λ)k𝚺k 2 + 𝜑𝑁 k𝚺k 𝜑𝑁 ≤ − (1 − 𝜃)𝜆 min (Λ)k𝚺k 2 ∀ k𝚺k ≥ 𝜃𝜆min (Λ) 140 The solution 𝚺 of the perturbed system will be ultimately bounded with the bound given by (3.110). 141 BIBLIOGRAPHY 142 BIBLIOGRAPHY [1] Naomi Kato, Bugi Wiku Wicaksono, and Yoshihiro Suzuki. Development of biology-inspired autonomous underwater vehicle “BASS III” with high maneuverability. In Proceedings of the 2000 International Symposium on Underwater Technology (Cat. No. 00EX418), pages 84–89. IEEE, 2000. [2] Parasar Kodati, Jonathan Hinkle, Aaron Winn, and Xinyan Deng. Microautonomous robotic ostraciiform (MARCO): Hydrodynamics, design, and fabrication. IEEE Transactions on Robotics, 24(1):105–117, 2008. [3] George Lauder, Peter GA Madden, Rajat Mittal, Haibo Dong, and Meliha Bozkurttas. Locomotion with flexible propulsors: I. Experimental analysis of pectoral fin swimming in sunfish. Bioinspiration & Biomimetics, 1(4):S25, 2006. [4] Kristi A Morgansen, Timothy M La Fond, and Jennifer X Zhang. Agile maneuvering for fin- actuated underwater vehicles. In Proceedings of the 2006 Second International Symposium on Communications, Control and Signal Processing. Citeseer, 2006. [5] Kin H Low and A. Renandya Willy. Biomimetic motion planning of an undulating robotic fish fin. Journal of Vibration and Control, 12(12):1337–1359, 2006. [6] Xiaobo Tan. Autonomous robotic fish as mobile sensor platforms: Challenges and potential solutions. Marine Technology Society Journal, 45(4):31–40, 2011. [7] Feitian Zhang, Osama Ennasr, Elena Litchman, and Xiaobo Tan. Autonomous sampling of water columns using gliding robotic fish: Algorithms and harmful-algae-sampling experi- ments. IEEE Systems Journal, 10(3):1271–1281, 2016. [8] Stefano Marras and Maurizio Porfiri. Fish and robots swimming together: attraction to- wards the robot demands biomimetic locomotion. Journal of The Royal Society Interface, 9(73):1856–1868, 2012. [9] Giovanni Polverino, Mert Karakaya, Chiara Spinello, Vrishin R Soman, and Maurizio Porfiri. Behavioural and life-history responses of mosquitofish to biologically inspired and interactive robotic predators. Journal of the Royal Society Interface, 16(158):20190359, 2019. [10] Michael S. Triantafyllou and George S. Triantafyllou. An efficient swimming machine. Scientific American, 273(3):64–70, 1995. [11] Michael Sfakiotakis, David M Lane, and J Bruce C Davies. Review of fish swimming modes for aquatic locomotion. IEEE Journal of Oceanic Engineering, 24(2):237–252, 1999. [12] Michael S Triantafyllou, George S Triantafyllou, and Dick K. P. Yue. Hydrodynamics of fishlike swimming. Annual review of fluid mechanics, 32(1):33–53, 2000. 143 [13] Michael Rufo. Ghostswimmer: Tactically relevant, biomimetically inspired, silent, highly ef- ficient and maneuverable autonomous underwater vehicle. Boston Engineering Corporation, 411, 2012. [14] Sander C. van den Berg, Rob B. N. Scharff, Zoltán Rusák, and Jun Wu. Biomimetic design of a soft robotic fish for high speed locomotion. In Vasiliki Vouloutsi, Anna Mura, Falk Tauber, Thomas Speck, Tony J. Prescott, and Paul F. M. J. Verschure, editors, Biomimetic and Biohybrid Systems, pages 366–377, Cham, 2020. Springer International Publishing. [15] Shuxiang Guo, Toshio Fukuda, and Kinji Asaka. A new type of fish-like underwater microbot. IEEE/ASME Transactions on Mechatronics, 8(1):136–141, 2003. [16] Huosheng Hu, Jindong Liu, Ian Dukes, and George Francis. Design of 3D swim patterns for autonomous robotic fish. In Proceeedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 2406–2411, 2006. [17] Daniele Costa, Matteo Franciolini, Giacomo Palmieri, Andrea Crivellini, and David Scaradozzi. Computational fluid dynamics analysis and design of an ostraciiform swim- ming robot. In Robotics and Biomimetics (ROBIO), 2017 IEEE International Conference on, pages 135–140. IEEE, 2017. [18] Xingyu Chen, Junzhi Yu, Zhengxing Wu, Yan Meng, and Shihan Kong. Toward a ma- neuverable miniature robotic fish equipped with a novel magnetic actuator system. IEEE Transactions on Systems, Man, and Cybernetics: Systems, 13(3):1–11, 2018. [19] Pan Liao, Shiwu Zhang, and Dong Sun. A dual caudal-fin miniature robotic fish with an integrated oscillation and jet propulsive mechanism. Bioinspiration & Biomimetics, 13(3):036007, 2018. [20] Alessio Alessi, Angelo Sudano, Dino Accoto, and Eugenio Guglielmelli. Development of an autonomous robotic fish. In Biomedical Robotics and Biomechatronics (BioRob), 2012 4th IEEE RAS & EMBS International Conference on, pages 1032–1037. IEEE, 2012. [21] Tetsuo Ichikizaki and Ikuo Yamamoto. Development of robotic fish with various swimming functions. In Underwater Technology and Workshop on Scientific Use of Submarine Cables and Related Technologies, 2007. Symposium on, pages 378–383. IEEE, 2007. [22] Kin Huat Low and Willy A. Renandya. Development and initial investigation of NTU robotic fish with modular flexible fins. In Mechatronics and Automation, 2005 IEEE International Conference, volume 2, pages 958–963. IEEE, 2005. [23] Xiaobo Tan, Drew Kim, Nathan Usher, Dan Laboy, Joel Jackson, Azra Kapetanovic, Jason Rapai, Beniamin Sabadus, and Xin Zhou. An autonomous robotic fish for mobile sensing. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, pages 5424– 5429. IEEE, 2006. [24] Palmani Duraisamy, Rakesh Kumar Sidharthan, and Manigandan Nagarajan Santhanakrish- nan. Design, modeling, and control of biomimetic fish robot: A review. Journal of Bionic Engineering, 16(6):967–993, 2019. 144 [25] Sanaz Bazaz Behbahani. Role of Flexibility in Robotic Fish. PhD thesis, Michigan State University, 2016. [26] Patar Ebenezer Sitorus, Yul Yunazwin Nazaruddin, Edi Leksono, and Agus Budiyono. Design and implementation of paired pectoral fins locomotion of labriform fish applied to a fish robot. Journal of Bionic Engineering, 6(1):37–45, 2009. [27] Zhihang Ye, Piqi Hou, and Zheng Chen. 2D maneuverable robotic fish propelled by multiple ionic polymer–metal composite artificial fins. International Journal of Intelligent Robotics and Applications, 1(2):195–208, 2017. [28] Naomi Kato. Locomotion by mechanical pectoral fins. Journal of Marine Science and Technology, 3(3):113–121, 1998. [29] Daisy Lachat, Alessandro Crespi, and Auke Jan Ijspeert. Boxybot: a swimming and crawling fish robot controlled by a central pattern generator. In The First IEEE/RAS-EMBS Interna- tional Conference on Biomedical Robotics and Biomechatronics, 2006. BioRob 2006., pages 643–648. IEEE, 2006. [30] Xinyan Deng and Srinath Avadhanula. Biomimetic micro underwater vehicle with oscillating fin propulsion: System design and force measurement. In Proceedings of the 2005 IEEE International Conference on Robotics and Automation, pages 3312–3317. IEEE, 2005. [31] Kristi A Morgansen, Benjamin I Triplett, and Daniel J Klein. Geometric methods for modeling and control of free-swimming fin-actuated underwater vehicles. IEEE Transactions on Robotics, 23(6):1184–1199, 2007. [32] Kristi A Morgansen, Benjamin I Triplett, and Daniel J Klein. Geometric methods for mod- eling and control of free-swimming underwater vehicles. IEEE Transactions on Robotics, 23(6):1184–1199, 2007. [33] Zheng Chen, Stephan Shatara, and Xiaobo Tan. Modeling of biomimetic robotic fish propelled by an ionic polymer-metal composite caudal fin. IEEE/ASME Transactions on Mechatronics, 15(3):448–459, 2010. [34] Scott D Kelly and Richard M Murray. Modeling efficient pisciform swimming for control. International Journal of Robust and Nonlinear Control, 10:217–241, 2000. [35] Jianxun Wang, Freddie Alequin-Ramos, and Xiaobo Tan. Dynamic modeling of robotic fish and its experimental validation. In Proceeedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 588–594, San Fracisco, California, 2011. [36] Jianxun Wang, Philip K McKinley, and Xiaobo Tan. Dynamic modeling of robotic fish with a flexible caudal fin. In ASME 2012 Dynamic Systems and Control Conference, pages 203–212, Fort Lauderale, Fl, 2012. [37] Sanaz Bazaz Behbahani and Xiaobo Tan. Bio-inspired flexible joints with passive feathering for robotic fish pectoral fins. Bioinspiration & Biomimetics, 11(3):036009, 2016. 145 [38] Vladislav Kopman, Jeffrey Laut, Francesco Acquaviva, Alessandro Rizzo, and Maurizio Porfiri. Dynamic modeling of a robotic fish propelled by a compliant tail. IEEE Journal of Oceanic Engineering, 40(1):209–221, 2015. [39] David Barrett, Mark Grosenbaugh, and Michael Triantafyllou. The optimal control of a flexible hull robotic undersea vehicle propelled by an oscillating foil. In Autonomous Underwater Vehicle Technology, 1996. AUV’96., Proceedings of the 1996 Symposium on, pages 1–9. IEEE, 1996. [40] Jamie M Anderson and Narender K Chhabra. Maneuvering and stability performance of a robotic tuna. Integrative and Comparative Biology, 42(1):118–126, 2002. [41] Shuo Wang, Zhigang Zhang, and Haiquan Sang. Analysis of velocity control algorithms for biomimetic robot fish. In Robotics and Biomimetics, 2004. ROBIO 2004. IEEE International Conference on, pages 972–976. IEEE, 2004. [42] Chao Zhou, Zhiqiang Cao, Shuo Wang, and Min Tan. The posture control and 3D locomotion implementation of biomimetic robot fish. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, pages 5406–5411. IEEE, 2006. [43] Koichi Hirata, Tadanori Takimoto, and Kenkichi Tamura. Study on turning performance of a fish robot. In First International Symposium on Aqua Bio-Mechanisms, pages 287–292, 2000. [44] Kristi A Morgansen, V Duidam, Richard J Mason, Joel W Burdick, and Richard M Murray. Nonlinear control methods for planar carangiform robot fish locomotion. In Robotics and Automation, 2001. Proceedings 2001 ICRA. IEEE International Conference on, volume 1, pages 427–434. IEEE, 2001. [45] Junzhi Yu, Long Wang, and Min Tan. A framework for biomimetic robot fish’s design and its realization. In American Control Conference, 2005. Proceedings of the 2005, pages 1593–1598. IEEE, 2005. [46] Feitian Zhang, Fumin Zhang, and Xiaobo Tan. Tail-enabled spiraling maneuver for gliding robotic fish. Journal of Dynamic Systems, Measurement, and Control, 136(4):041028, 2014. [47] Michail Makrodimitris, Kostas Nanos, and Evangelos Papadopoulos. A novel trajectory plan- ning method for a robotic fish. In Control and Automation (MED), 2017 25th Mediterranean Conference on, pages 1119–1124. IEEE, 2017. [48] Long Wang, Shuo Wang, Zhiqiang Cao, Min Tan, Chao Zhou, Haiquan Sang, and Zhizhong Shen. Motion control of a robot fish based on CPG. In Industrial Technology, 2005. ICIT 2005. IEEE International Conference on, pages 1263–1268. IEEE, 2005. [49] Wei Zhao, Yonghui Hu, L Zhang, and Long Wang. Design and CPG-based control of biomimetic robotic fish. IET Control Theory & Applications, 3(3):281–293, 2009. [50] Wei Wang, Jiajie Guo, Zijian Wang, and Guangming Xie. Neural controller for swimming modes and gait transition on an ostraciiform fish robot. In Advanced Intelligent Mechatronics (AIM), 2013 IEEE/ASME International Conference on, pages 1564–1569. IEEE, 2013. 146 [51] Yonghui Hu, Weicheng Tian, Jianhong Liang, and Tianmiao Wang. Learning fish-like swimming with a CPG-based locomotion controller. In Intelligent Robots and Systems (IROS), 2011 IEEE/RSJ International Conference on, pages 1863–1868. IEEE, 2011. [52] Qinyuan Ren, Jianxin Xu, and Xuefang Li. A data-driven motion control approach for a robotic fish. Journal of Bionic Engineering, 12(3):382–394, 2015. [53] Wei Wang, Dongbing Gu, and Guangming Xie. Autonomous optimization of swimming gait in a fish robot with multiple onboard sensors. IEEE Transactions on Systems, Man, and Cybernetics: Systems, 49(99):1–13, 2017. [54] Kristi A Morgansen, Patricio A Vela, and Joel W Burdick. Trajectory stabilization for a planar carangiform robot fish. In Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No. 02CH37292), volume 1, pages 756–762. IEEE, 2002. [55] Jian-Xin Xu, Xue-Lei Niu, and Zhao-Qin Guo. Sliding mode control design for a carangiform robotic fish. In 2012 12th International Workshop on Variable Structure Systems, pages 308– 313. IEEE, 2012. [56] Junzhi Yu, Min Tan, Shuo Wang, and Erkui Chen. Development of a biomimetic robotic fish and its control algorithm. IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), 34(4):1798–1810, 2006. [57] Saroj Saimek and Perry Y Li. Motion planning and control of a swimming machine. The International Journal of Robotics Research, 23(1):27–53, 2004. [58] Yonghui Hu, Wei Zhao, and Long Wang. Vision-based target tracking and collision avoidance for two autonomous robotic fish. IEEE Transactions on Industrial Electronics, 56(5):1401– 1410, 2009. [59] Pichet Suebsaiprom and Chun-Liang Lin. Maneuverability modeling and trajectory tracking for fish robot. Control Engineering Practice, 45:22–36, 2015. [60] Kexu Zou, Chen Wang, Guangming Xie, Tianguang Chu, Long Wang, and Yingmin Jia. Cooperative control for trajectory tracking of robotic fish. In American Control Conference, 2009. ACC’09., pages 5504–5509. IEEE, 2009. [61] Pichet Suebsaiprom and Chun-Liang Lin. Sliding mode path tracking control for fish-robot under ocean current perturbation. In Control and Automation (ICCA), 2016 12th IEEE International Conference on, pages 972–977. IEEE, 2016. [62] Songlin Chen, Jianxun Wang, and Xiaobo Tan. Backstepping-based hybrid target tracking control for a carangiform robotic fish. In ASME 2013 Dynamic Systems and Control Con- ference, pages V002T32A005–V002T32A005. American Society of Mechanical Engineers, 2013. [63] Erfu Yang, Takayuki Ikeda, and Tsutomu Mita. Nonlinear tracking control of a nonholonomic fish robot in chained form. In SICE 2003 Annual Conference (IEEE Cat. No. 03TH8734), volume 2, pages 1260–1265. IEEE, 2003. 147 [64] Janet E Fontanella, Frank E Fish, Elizabeth I Barchi, Regina Campbell-Malone, Rachel H Nichols, Nicole K DiNenno, and John T Beneski. Two-and three-dimensional geometries of batoids in relation to locomotor mode. Journal of experimental marine biology and ecology, 446:273–281, 2013. [65] Lisa J Rosenberger. Pectoral fin locomotion in batoid fishes: undulation versus oscillation. Journal of Experimental Biology, 204(2):379–394, 2001. [66] Hongan Wang. Design and Implementation of Biomimetic Robotic Fish. Citeseer, 2009. [67] Steven Vogel. Life in moving fluids: the physical biology of flow. Princeton, N.J. : Princeton University Press, 1996. [68] John Palmisano, Ravi Ramamurti, Kerr-Jia Lu, Jonah Cohen, William Sandberg, and Bana- halli Ratna. Design of a biomimetic controlled-curvature robotic pectoral fin. In Proceedings 2007 IEEE International Conference on Robotics and Automation, pages 966–973. IEEE, 2007. [69] Rajat Mittal, Haibo Dong, Meliha Bozkurttas, GeorgeV Lauder, and Peter Madden. Loco- motion with flexible propulsors: II. computational modeling of pectoral fin swimming in sunfish. Bioinspiration & Biomimetics, 1(4):S35, 2006. [70] Yong Zhong, Zheng Li, and Ruxu Du. Robot fish with two-DOF pectoral fins and a wire- driven caudal fin. Advanced Robotics, 32(1):25–36, 2018. [71] Navinder Singh, Ankur Gupta, and Sujoy Mukherjee. A dynamic model for underwater robotic fish with a servo actuated pectoral fin. SN Applied Sciences, 1(7):659, 2019. [72] Shusheng Bi, Hongwei Ma, Yueri Cai, Chuanmeng Niu, and Yuliang Wang. Dynamic mod- eling of a flexible oscillating pectoral fin for robotic fish. Industrial Robot: An International Journal, 2014. [73] Xinmin Liu, Tetsuya Iwasaki, and Frank Fish. Dynamic modeling and gait analysis of batoid swimming. In 2013 American Control Conference, pages 566–571. IEEE, 2013. [74] Naomi Kato and Tadahiko Inaba. Guidance and control of fish robot with apparatus of pectoral fin motion. In Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No. 98CH36146), volume 1, pages 446–451. IEEE, 1998. [75] Naomi Kato and Motonori Furushima. Pectoral fin model for maneuver of underwater vehicles. In Proceedings of Symposium on Autonomous Underwater Vehicle Technology, pages 49–56. IEEE, 1996. [76] Ming Wang, Junzhi Yu, and Min Tan. Modeling neural control of robotic fish with pectoral fins using a CPG-based network. In Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference, pages 6502–6507. IEEE, 2009. 148 [77] Ming Wang, Junzhi Yu, Min Tan, and Jianwei Zhang. Design and implementation of a novel cpg-based locomotion controller for robotic dolphin. In 2010 8th World Congress on Intelligent Control and Automation, pages 1611–1616. IEEE, 2010. [78] Ming Wang, JunZhi Yu, Min Tan, and JianWei Zhang. Multimodal swimming control of a robotic fish with pectoral fins using a CPG network. Chinese Science Bulletin, 57(10):1209– 1216, 2012. [79] Junzhi Yu, Kai Wang, Min Tan, and Jianwei Zhang. Design and control of an embedded vision guided robotic fish with multiple control surfaces. The Scientific World Journal, 2014, 2014. [80] Shiwu Zhang, Yun Qian, Pan Liao, Fenghua Qin, and Jiming Yang. Design and control of an agile robotic fish with integrative biomimetic mechanisms. IEEE/ASME Transactions on Mechatronics, 21(4):1846–1857, 2016. [81] Ming Wang, Junzhi Yu, and Min Tan. CPG-based sensory feedback control for bio-inspired multimodal swimming. International Journal of Advanced Robotic Systems, 11(10):170, 2014. [82] Naomi Kato. Control performance in the horizontal plane of a fish robot with mechanical pectoral fins. IEEE journal of oceanic engineering, 25(1):121–129, 2000. [83] Fu-dong Gao, Cun-yun Pan, Yan-yan Han, and Xiang Zhang. Nonlinear trajectory tracking control of a new autonomous underwater vehicle in complex sea conditions. Journal of Central South University, 19(7):1859–1868, 2012. [84] Xianbo Xiang, Lionel Lapierre, Chao Liu, and Bruno Jouvencel. Path tracking: Combined path following and trajectory tracking for autonomous underwater vehicles. In In Proceedings of 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 3558–3563, 2011. [85] Gu Dongbing and Hu Huosheng. A stabilizing receding horizon regulator for nonholonomic mobile robots. IEEE Transactions on Robotics, 21(5):1022–1028, 2005. [86] Kiattisin Kanjanawanishkul and Andreas Zell. Path following for an omnidirectional mobile robot based on model predictive control. In In Proceedings of 2009 IEEE International Conference on Robotics and Automation, pages 3341–3346, 2009. [87] Xiang Li, Kiattisin Kanjanawanishkul, and Andreas Zell. Nonlinear model predictive control of an omnidirectional mobile robot. In In Proceedings of the 10th International Conference on Intelligent Autonomous Systems (IAS 2008), pages 92–99, 2008. [88] Kiattisin Kanjanawanishkul. MPC-based path following control of an omnidirectional mo- bile robot with consideration of robot constraints. Advances in Electrical and Electronic Engineering, 13(1):54–63, 2015. [89] Jianxun Wang and Xiaobo Tan. Averaging tail-actuated robotic fish dynamics through force and moment scaling. IEEE Transactions on Robotics, 31(4):906–917, 2015. 149 [90] Boris Houska, Hans Joachim Ferreau, and Moritz Diehl. ACADO Toolkit – An Open Source Framework for Automatic Control and Dynamic Optimization. Optimal Control Applications and Methods, 32(3):298–312, 2011. [91] Maria L Castaño and Xiaobo Tan. Model predictive control of a tail-actuated robotic fish. In Dynamic Systems and Control Conference, volume 50695, page V001T03A006. American Society of Mechanical Engineers, 2016. [92] Maria L Castaño and Xiaobo Tan. Model predictive control-based path-following for tail- actuated robotic fish. Journal of Dynamic Systems, Measurement, and Control, 141(7), 2019. [93] Khac Duc Do and Jie Pan. Control of Ships and Underwater Vehicles: Design for Underac- tuated and Nonlinear Marine Systems. Springer Science & Business Media, 2009. [94] Maria L Castaño and Xiaobo Tan. Backstepping control-based trajectory tracking for tail- actuated robotic fish. In 2019 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), pages 839–844. IEEE, 2019. [95] Jan A Sanders, Ferdinand Verhulst, and James Murdock. Averaging Methods in Nonlinear Dynamical Systems, volume 59. Springer, 2007. [96] Francesco Bullo and Andrew D Lewis. Geometric Control of Mechanical Systems: Modeling, Analysis, and Design for Simple Mechanical Control systems, volume 49. Springer, 2019. [97] Maria L Castaño and Xiaobo Tan. Averaged modeling of pectoral fin-actuated robotic fish. In The 2021 Inaugural Modeling, Estimation and Control Conference (MECC), page In press. AACC, 2021. [98] Maria L Castaño and Xiaobo Tan. Rapid maneuvering control of pectoral fin-actuated robotic fish. In 2021 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), pages 705–712. IEEE, 2021. [99] Carlos E Garcia, David M Prett, and Manfred Morari. Model predictive control: Theory and practice - a survey. Automatica, 25(3):335–348, 1989. [100] Frank. Allgöwer, Rolf Findeisen, and Zoltan K Nagy. Nonlinear model predictive control: From theory to application. J. Chin. Inst. Chem. Engrs, 35(3):299–315, 2004. [101] Shuyou Yu, Xiang Li, Hong Chen, and Frank Allgower. Nonlinear model predictive control for path following problems. IFAC Proceedings Volumes, 45(17):145 – 150, 2012. 4th IFAC Conference on Nonlinear Model Predictive Control. [102] Matteo Aureli, Vladislav Kopman, and Maurizio Porfiri. Free-locomotion of underwater vehicles actuated by ionic polymer metal composites. IEEE/ASME Transactions on Mecha- tronics, 15(4):603–614, 2009. [103] Filoktimon Repoulias and Evangelos Papadopoulos. Planar trajectory planning and tracking control design for underactuated AUVs. Ocean Engineering, 34(11-12):1650–1667, 2007. 150 [104] Filoktimon Repoulias and Evangelos Papadopoulos. Trajectory planning and tracking control of underactuated AUVs. In Proceedings of the 2005 IEEE International Conference on Robotics and Automation, pages 1610–1615. IEEE, 2005. [105] Jawhar Ghommam, Faical Mnif, and N Derbel. Global stabilisation and tracking control of underactuated surface vessels. IET Control Theory & Applications, 4(1):71–88, 2010. [106] Adriaan A J Lefeber. Tracking Control of Nonlinear Mechanical Systems. Universiteit Twente Eindhove, The Netherlands, 2000. [107] Kristin Y Pettersen and Henk Nijmeijer. Tracking control of an underactuated surface vessel. In Proceedings of the 37th IEEE Conference on Decision and Control (Cat. No. 98CH36171), volume 4, pages 4561–4566. IEEE, 1998. [108] Zhong-Ping Jiang. Global tracking control of underactuated ships by Lyapunov’s direct method. Automatica, 38(2):301–309, 2002. [109] Kristin Y Pettersen and Henk Nijmeijer. Underactuated ship tracking control: Theory and experiments. International Journal of Control, 74(14):1435–1446, 2001. [110] Ruiting Yu, Qidan Zhu, Guihua Xia, and Zhe Liu. Sliding mode tracking control of an underactuated surface vessel. IET Control Theory & Applications, 6(3):461, 2012. [111] Erjen Lefeber, Kristin Ytterstad Pettersen, and Henk Nijmeijer. Tracking control of an underactuated ship. IEEE Transactions on Control Systems Technology, 11(1):52–61, Jan 2003. [112] Elena Panteley and Antonio Lorıa. Growth rate conditions for uniform asymptotic stability of cascaded time-varying systems. Automatica, 37(3):453–460, 2001. [113] Jian Xu, Man Wang, and Lei Qiao. Dynamical sliding mode control for the trajectory tracking of underactuated unmanned underwater vehicles. Ocean Engineering, 105:54–63, Sep 2015. [114] John-Morten Godhavn. Nonlinear tracking of underactuated surface vessels. In Proceedings of 35th IEEE Conference on Decision and Control, volume 1, pages 975–980. IEEE, 1996. [115] John-Morten Godhavn, Thor I Fossen, and Svein P Berge. Non-linear and adaptive back- stepping designs for tracking control of ships. International Journal of Adaptive Control and Signal Processing, 12(8):649–670, 1998. [116] Jia-Zhong Zhang, Yingjie Wei, F.Y. Bi, and Weijin Cao. Position-tracking control of underactuated autonomous underwater vehicles in the presence of unknown ocean currents. IET Control Theory & Applications, 4(11):2369–2380, Nov 2010. [117] Hashem Ashrafiuon, Kenneth R Muske, Lucas C McNinch, and Reza A Soltan. Sliding- mode tracking control of surface vessels. IEEE Transactions on Industrial Electronics, 55(11):4004–4012, 2008. 151 [118] Taha Elmokadem, Mohamed Zribi, and Kamal Youcef-Toumi. Trajectory tracking sliding mode control of underactuated AUVs. Nonlinear Dynamics, 84(2):1079–1091, Apr 2016. [119] H Sira-Ramfrez. On the control of the underactuated ship: A trajectory planning approach. In Proceedings of the 38th IEEE Conference on Decision and Control (Cat. No. 99CH36304), volume 3, pages 2192–2197. IEEE, 1999. [120] Pedro A Aguiar and Joao P Hespanha. Trajectory-tracking and path-following of underac- tuated autonomous vehicles with parametric modeling uncertainty. IEEE Transactions on Automatic Control, 52(8):1362–1379, 2007. [121] Aman Behal, Darren M. Dawson, Bin Xian, and Pradeep Setlur. Adaptive tracking control of underactuated surface vessels. In Proceedings of the 2001 IEEE International Conference on Control Applications (CCA’01) (Cat. No.01CH37204), page 645–650. IEEE, 2001. [122] Khac Duc Do, Zhong-Ping Jiang, and Jie Pan. Underactuated ship global tracking under relaxed conditions. IEEE Transactions on Automatic Control, 47(9):1529–1536, Sep 2002. [123] Khac D. Do, Zhong-Ping. Jiang, and Jie Pan. Universal controllers for stabilization and tracking of underactuated ships. Systems & Control Letters, 47(4):299–317, Nov 2002. [124] Khac D Do, Zhong-Ping Jiang, and Jie Pan. Global partial-state feedback and output- feedback tracking controllers for underactuated ships. Systems & Control Letters, 54(10):1015–1036, 2005. [125] Zhang Wei, Xu Da, Tan Meiling, Wang Conglin, and Yan Zheping. Trajectory tracking con- trol of underactuated uuv for underwater recovery. In 2012 Second International Conference on Instrumentation, Measurement, Computer, Communication and Control, pages 386–391. IEEE, 2012. [126] Dongkyoung Chwa. Global tracking control of underactuated ships with input and velocity constraints using dynamic surface control method. IEEE Transactions on Control Systems Technology, 19(6):1357–1370, Nov 2011. [127] Reza Olfati-Saber. Nonlinear Control of Underactuated Mechanical Systems with Applica- tion to Robotics and Aerospace Vehicles. PhD thesis, Massachusetts Institute of Technology, 2001. [128] Jianxun Wang and Xiaobo Tan. A dynamic model for tail-actuated robotic fish with drag coefficient adaptation. Mechatronics, 23(6):659–668, 2013. [129] Hassan Khalil. Nonlinear Control. Pearson New York, 2014. [130] Thor I Fossen. Guidance and Control of Ocean Vehicles. PhD thesis, University of Trond- heim, Norway, 1994. [131] Petar Kokotovic, Hassan K Khali, and John O’reilly. Singular Perturbation Methods in Control: Analysis and Design, volume 25. Siam, 1999. 152 [132] Jay Farrell, Marios Polycarpou, and Manu Sharma. Adaptive backstepping with magnitude, rate, and bandwidth constraints: Aircraft longitude control. In Proceedings of the 2003 American Control Conference, volume 5, pages 3898–3904. IEEE, 2003. [133] Rong Mei and Qingliang Cui. Backstepping control for a 3DOF model helicopter with input and output constraints. International Journal of Advanced Robotic Systems, 14(1):1729881416687133, 2016. [134] Finn Haugen. PID Control, chapter 4, pages 94–98. Tapir Academic Press, 2004. [135] Jianxun Wang, Philip K McKinley, and Xiaobo Tan. Dynamic modeling of robotic fish with a base-actuated flexible tail. Journal of Dynamic Systems, Measurement, and Control, 137(1):11004(1)– 11004(11), 2015. [136] Sanaz Bazaz Behbahani and Xiaobo Tan. Role of pectoral fin flexibility in robotic fish performance. Journal of Nonlinear Science, 27(4):1155–1181, 2017. [137] Pablo Valdivia y Alvarado and Kamal Youcef-Toumi. Design of machines with compliant bodies for biomimetic locomotion in liquid environments. Journal of Dynamic Systems, Measurement, and Control, 128(1):3–13, 09 2005. [138] Eliot G Drucker, Jeffrey A Walker, and Mark W Westneat. Mechanics of pectoral fin swimming in fishes. Fish Physiology, 23:369–423, 2005. [139] R.G Dong. Effective mass and damping of submerged structures. Technical report, California Univ., Livermore (USA). Lawrence Livermore Lab., 1978. [140] Sanaz Bazaz Behbahani and Xiaobo Tan. Design and modeling of flexible passive rowing joint for robotic fish pectoral fins. IEEE Transactions on Robotics, 32(5):1119–1132, 2016. [141] Kalyanmoy Deb. Multi-objective Optimization Using Evolutionary Algorithms, volume 16. John Wiley & Sons, 2001. [142] Francesco Casella and Marco Lovera. LPV/LFT modelling and identification: overview, synergies and a case study. In 2008 IEEE International Conference on Computer-Aided Control Systems, pages 852–857. IEEE, 2008. [143] Yucai Zhu and Zuhua Xu. A method of LPV model identification for control. IFAC Proceedings Volumes, 41(2):5018–5023, 2008. [144] Maria L Castaño, Anastasia Mavrommati, Todd D Murphey, and Xiaobo Tan. Trajectory planning and tracking of robotic fish using ergodic exploration. In 2017 American Control Conference (ACC), pages 5476–5481. IEEE, 2017. [145] Giorgos Mamakoukas, Maria Castano, Xiaobo Tan, and Todd Murphey. Local koopman operators for data-driven control of robotic systems. In Robotics: science and systems, 2019. [146] Giorgos Mamakoukas, Maria L Castano, Xiaobo Tan, and Todd D Murphey. Derivative- based koopman operators for real-time control of robotic systems. IEEE Transactions on Robotics, 2021. 153