Underwater Biomimetic Vehicle-Manipulator System: Control and Planning 9819906547, 9789819906543

Table of contents :
Preface
Brief Introduction
Contents
1 Introduction
1.1 Underwater Vehicle-Manipulator System
1.2 Underwater Biomimetic Vehicle Propelled by Undulating Fins
1.2.1 Theoretical Analysis of Undulatory Fin
1.2.2 Experimental Observation and Analysis of Undulating Fin
1.2.3 Numerical Analysis of Undulating Fin
1.3 Autonomous Operation of the Underwater Vehicle-Manipulator System
1.4 Trajectory Planning of the Underwater Vehicle-Manipulator System
References
2 Design of the Underwater Biomimetic Vehicle-Manipulator System
2.1 Mechanical Design of the UBVMS
2.1.1 Mechanical Design of the Bionic Propulsor
2.1.2 Mechanism Design of the Underwater Manipulator
2.2 Control System Design of the UBVMS
2.2.1 Underlying Driving System of Undulatory Fins
2.2.2 Underlying Driving Remote Update System
References
3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin
3.1 Kinematics Modeling of the Undulatory Fin
3.1.1 Kinematics Modeling
3.1.2 Effects on the Movement of Undulating Fin
3.2 Dynamic Modeling of Undulatory Fins
3.2.1 Basal Principle
3.2.2 Dynamic Modeling
References
4 Motion Control of the Underwater Biomimetic Robot
4.1 Three-Dimensional Motion Control
4.2 Depth Control and Yaw Angle Control
4.2.1 System Modeling
4.2.2 Depth Control
4.2.3 Yaw Angle Control
References
5 Path Planning of Underwater Biomimetic Robot
5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning
5.1.1 Problem Statement
5.1.2 RDDH Trajectory Smoothing
5.1.3 Simulation and Analysis
5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving Target
5.2.1 Problem Statement
5.2.2 Real-Time Trajectory Planning for Approaching a Moving Target
5.2.3 Model Description for Simultaneous Arrival of the Underwater Biomimetic Robot and the Target
5.2.4 Real-Time Trajectory Planning Algorithm for Approaching a Moving Target
5.2.5 Simulation and Analysis
References
6 Path Tracking Control of the Underwater Biomimetic Robot
6.1 Waypoint Tracking Control
6.1.1 Problem Statement
6.1.2 The Controller Design of Waypoint Tracking
6.1.3 Experiments and Analysis
6.2 Path Tracking Control
6.2.1 Reference Coordinate System
6.2.2 Problem Statement
6.2.3 The Controller Design
6.2.4 Experiment and Analysis
References
7 Autonomous Grasping Control of the Underwater Biomimetic Vehicle-Manipulator System
7.1 Hand–Eye Coordination Control of the Underwater Manipulator
7.1.1 Autonomous Grasping Control of Manipulator
7.1.2 Experiment and Results
7.2 Coordination Planning and Control of the Underwater Biomimetic Vehicle-Manipulator System
7.2.1 Coordination Planning and Control Based on Task-Priority Algorithm
7.2.2 Experiment and Results
References

Citation preview

Unmanned System Technologies

Shuo Wang · Yu Wang · Min Tan · Rui Wang · Xiang Dong · Qingping Wei · Liuji Shang

Underwater Biomimetic Vehicle-Manipulator System Control and Planning

Unmanned System Technologies

Springer’s Unmanned System Technologies (UST) book series publishes the latest developments in unmanned vehicles and platforms in a timely manner, with the highest of quality, and written and edited by leaders in the field. The aim is to provide an effective platform to global researchers in the field to exchange their research findings and ideas. The series covers all the main branches of unmanned systems and technologies, both theoretical and applied, including but not limited to: . Unmanned aerial vehicles, unmanned ground vehicles and unmanned ships, and all unmanned systems related research in: . Robotics Design . Artificial Intelligence . Guidance, Navigation and Control . Signal Processing . Circuit and Systems . Mechatronics . Big Data . Intelligent Computing and Communication . Advanced Materials and Engineering The publication types of the series are monographs, professional books, graduate textbooks, and edited volumes.

Shuo Wang · Yu Wang · Min Tan · Rui Wang · Xiang Dong · Qingping Wei · Liuji Shang

Underwater Biomimetic Vehicle-Manipulator System Control and Planning

Shuo Wang Institute of Automation Chinese Academy of Sciences Beijing, China University of Chinese Academy of Sciences Beijing, China Min Tan Institute of Automation Chinese Academy of Sciences Beijing, China Xiang Dong School of Electrical Engineering and Automation Anhui University Hefei, Anhui, China

Yu Wang Institute of Automation Chinese Academy of Sciences Beijing, China Rui Wang Institute of Automation Chinese Academy of Sciences Beijing, China Qingping Wei DEC Academy of Science and Technology Co., Ltd. Chengdu, Sichuan, China

Liuji Shang Shenzhen Sibo Zhilian Technology Co., Ltd. Shenzhen, Guangdong, China

ISSN 2523-3734 ISSN 2523-3742 (electronic) Unmanned System Technologies ISBN 978-981-99-0654-3 ISBN 978-981-99-0655-0 (eBook) https://doi.org/10.1007/978-981-99-0655-0 Jointly published with National Defense Industry Press The print edition is not for sale in China (Mainland). Customers from China (Mainland) please order the print book from: National Defense Industry Press. © National Defense Industry Press 2023 This work is subject to copyright. All rights are solely and exclusively licensed by the Publisher, whether the whole or part of the material is concerned, specifically the rights of reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way, and transmission or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar methodology now known or hereafter developed. The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication does not imply, even in the absence of a specific statement, that such names are exempt from the relevant protective laws and regulations and therefore free for general use. The publishers, the authors, and the editors are safe to assume that the advice and information in this book are believed to be true and accurate at the date of publication. Neither the publishers nor the authors or the editors give a warranty, expressed or implied, with respect to the material contained herein or for any errors or omissions that may have been made. The publishers remain neutral with regard to jurisdictional claims in published maps and institutional affiliations. This Springer imprint is published by the registered company Springer Nature Singapore Pte Ltd. The registered company address is: 152 Beach Road, #21-01/04 Gateway East, Singapore 189721, Singapore

Preface

The vast ocean provides abundant resources for mankind. The efficient use and sustainable development of marine resources are of great significance to mankind. As a kind of equipment that can move on the bottom of the sea or on the sea surface, underwater vehicles can replace or assist humans in completing underwater exploration or operations. In order to meet some specific requirements, such as underwater equipment maintenance, underwater vehicles are often equipped with manipulator to form an underwater vehicle-manipulator system (UVMS). At present, the propulsion devices for most of the UVMSs are propellers, combined with a steerable rudder surface to generate a maneuvering control force to realize the movement of the underwater vehicle. On the other hand, inspired by the high-efficiency and highmobility movement of underwater creatures such as fish, research work on underwater biomimetic propulsion has been carried out for many years. According to the existing research results, the biomimetic propulsion is highly efficient, stable, and highly mobile and could adapt to complex turbulence. In particular, some fish that use flexible long undulatory fins to swim, such as Arcfinidae and Nakedback Anguillidae, have flexible control capabilities in turbulent environments and low-speed conditions. Imitating these fishes and using undulatory propulsion systems, we develop a new type of underwater biomimetic vehicle-manipulator system (UBVMS), which has the characteristics of high efficiency, high mobility, stability, and environmental friendliness. The authors of this book have been carrying out scientific research work for more than ten years in the field of UBVMSs. This book is written based on long-term scientific research and practical experience about UBVMSs. This book is divided into seven chapters. Chapter 1 briefly introduces the basic concept of the UVMS and summarizes the research status of the UVMS. Chapter 2 mainly introduces the mechanism design scheme and control system design scheme of the UBVMS. Chapter 3 discusses the kinematics and dynamics modeling of undulatory fins in detail. Chapter 4 focuses on the three-dimensional motion control of the UBVMS and proposes the depth control and heading control based on the active disturbance rejection control technique. Chapter 5 discusses the tracking planning method, mainly studying the real-time dynamic Dubins-Helix tracking planning and vii

viii

Preface

smoothing method and the real-time tracking planning method of autonomously approaching the moving target. Chapter 6 focuses on the path following control of the UBVMS. Chapter 7 addresses autonomous operation problems of the UBVMS. The research work in this book has been strongly supported by the National Natural Science Foundation of China (U1713222, 61703401, 61421004, 51175496, 6005026) and the 863 Program (2006AA11Z225). The authors would like to express our sincere thanks. Special thanks to Xiang Dong, Liuji Shang, Qingping Wei, Rui Wang, Chong Tang who completed the above research work with the authors. Special thanks to Mingxue Cai, Xuejian Bai, et al. for participating in part of material collation work. The publication of this book has received strong support and help from the National Defense Industry Press, and the author would like to express his heartfelt thanks. Due to the limited knowledge and the rush of time, errors and improprieties in the book are inevitable. Suggestions from readers and experts would be appreciated. We express our sincere thanks. Beijing, China Beijing, China Beijing, China Beijing, China Hefei, China Chengdu, China Shenzhen, China

Shuo Wang Yu Wang Min Tan Rui Wang Xiang Dong Qingping Wei Liuji Shang

Brief Introduction

An underwater vehicle-manipulator system has a brilliant perspective in such applications as underwater rescue, equipment assembly, and environment protection. Its research is significant in academic and engineering research. In this book, the research status of the underwater vehicle-manipulator system is briefly described. The design and realization of the biomimetic vehicle-manipulator system are introduced. The kinematics and dynamics modeling of underwater undulating fins are deduced. The motion control, trajectory planning, path tracking of underwater biomimetic vehicle, and autonomous grasping control of the underwater biomimetic vehicle-manipulator system are systematically introduced. The simulations and experiments of underwater biomimetic vehicle-manipulator system are carried out, and the simulation and experiment results are analyzed and discussed. Moreover, this book provides detailed theoretical methods and key techniques for the design, modeling, control, and planning of the underwater biomimetic vehicle-manipulator system. This book is used for the scientific and technical personnel engaged in the research and application of the underwater vehicle-manipulator system and focuses on the research frontier of this field and the feasibility of the methods. It can be used as a reference book for researchers and managers in the field of underwater robots, as well as a reference book for graduate students and senior undergraduates of control theory and engineering. It is helpful for readers to understand the underwater vehiclemanipulator system.

ix

Contents

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1 Underwater Vehicle-Manipulator System . . . . . . . . . . . . . . . . . . . . . . 1.2 Underwater Biomimetic Vehicle Propelled by Undulating Fins . . . . 1.2.1 Theoretical Analysis of Undulatory Fin . . . . . . . . . . . . . . . . . 1.2.2 Experimental Observation and Analysis of Undulating Fin . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.3 Numerical Analysis of Undulating Fin . . . . . . . . . . . . . . . . . . 1.3 Autonomous Operation of the Underwater Vehicle-Manipulator System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4 Trajectory Planning of the Underwater Vehicle-Manipulator System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1 2 5 11 12 13 14 16 19

2 Design of the Underwater Biomimetic Vehicle-Manipulator System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Mechanical Design of the UBVMS . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.1 Mechanical Design of the Bionic Propulsor . . . . . . . . . . . . . . 2.1.2 Mechanism Design of the Underwater Manipulator . . . . . . . 2.2 Control System Design of the UBVMS . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 Underlying Driving System of Undulatory Fins . . . . . . . . . . . 2.2.2 Underlying Driving Remote Update System . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

27 28 30 30 36 39 40 42

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Kinematics Modeling of the Undulatory Fin . . . . . . . . . . . . . . . . . . . . 3.1.1 Kinematics Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1.2 Effects on the Movement of Undulating Fin . . . . . . . . . . . . . . 3.2 Dynamic Modeling of Undulatory Fins . . . . . . . . . . . . . . . . . . . . . . . .

43 44 44 46 52

xi

xii

Contents

3.2.1 Basal Principle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.2 Dynamic Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

53 54 63

4 Motion Control of the Underwater Biomimetic Robot . . . . . . . . . . . . . . 4.1 Three-Dimensional Motion Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Depth Control and Yaw Angle Control . . . . . . . . . . . . . . . . . . . . . . . . 4.2.1 System Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.2 Depth Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.3 Yaw Angle Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

65 65 69 69 72 80 87

5 Path Planning of Underwater Biomimetic Robot . . . . . . . . . . . . . . . . . . 5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning . . . . . . . . . . 5.1.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1.2 RDDH Trajectory Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1.3 Simulation and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2.2 Real-Time Trajectory Planning for Approaching a Moving Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2.3 Model Description for Simultaneous Arrival of the Underwater Biomimetic Robot and the Target . . . . . . 5.2.4 Real-Time Trajectory Planning Algorithm for Approaching a Moving Target . . . . . . . . . . . . . . . . . . . . . . 5.2.5 Simulation and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

89 89 90 91 102

6 Path Tracking Control of the Underwater Biomimetic Robot . . . . . . . 6.1 Waypoint Tracking Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1.2 The Controller Design of Waypoint Tracking . . . . . . . . . . . . . 6.1.3 Experiments and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Path Tracking Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2.1 Reference Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2.2 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2.3 The Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2.4 Experiment and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

127 127 128 129 136 138 138 140 142 144 149

7 Autonomous Grasping Control of the Underwater Biomimetic Vehicle-Manipulator System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1 Hand–Eye Coordination Control of the Underwater Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1.1 Autonomous Grasping Control of Manipulator . . . . . . . . . . . 7.1.2 Experiment and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

106 106 107 113 118 119 124

151 151 151 156

Contents

7.2 Coordination Planning and Control of the Underwater Biomimetic Vehicle-Manipulator System . . . . . . . . . . . . . . . . . . . . . . 7.2.1 Coordination Planning and Control Based on Task-Priority Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2.2 Experiment and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

xiii

157 158 165 172

Chapter 1

Introduction

The ocean is a treasure house of resources and the source of life. With the decrease of land resources, the ocean has become one of the important sources of human resources in the future. Back in 1986, the USA took the lead in establishing the Global Ocean Science Program, emphasizing that the countries that exploit the oceans first and best will benefit the most. Since the beginning of the twenty-first century, the USA, the UK, and Japan have all formulated medium- and long-term marine science and technology development plans to promote the development of marine science and technology and marine development technologies. The USA has issued the “An Ocean Blueprint for the 21st Century”, “Charting the course for ocean science in the United States for the next decade: an ocean research priorities plan and implementation strategy”, and formulated medium- and long-term marine science and technology action plans to promote ocean development and utilization. The UK released the “Ocean 2025” to comprehensively promote Marine science and technology. Japan issued the “Ocean and Japan’s Ocean Policy Proposal for the 21st Century”, which aims to build a nation by ocean, emphasizes the sustainable development of ocean, and promotes the development and utilization of Marine resources. In China’s medium- and longterm development plan, the state also attaches great importance to the development and utilization of Marine resources from the strategic level. Underwater vehicle-manipulator system (UVMS) is designed to perform a variety of military and civilian operations in complex water environments, including marine exploration, resource development, subsea equipment maintenance, and underwater rescue. It involves automatic control, robotics, bionic technology, machinery, materials, sensing, control, artificial intelligence and other disciplines, and its development has been concerned and valued by the world’s maritime powers. Generally, the UVMS realizes underwater intervention by manual remote control and has shown good application value in deep-water equipment installation and maintenance, rescue, deep-water survey, and other aspects. Future autonomy and intelligence is one of the inevitable trends of UVMS. However, due to the redundant degree of freedom, nonlinear, strong coupling, time-varying, high dimension, low bandwidth of sensor data acquisition and hydrodynamic interference, the system design, autonomous © National Defense Industry Press 2023 S. Wang et al., Underwater Biomimetic Vehicle-Manipulator System, Unmanned System Technologies,https://doi.org/10.1007/978-981-99-0655-0_1

1

2

1 Introduction

control and operation planning of UVMS have become a very challenging topic in the field of underwater robots at home and abroad. In this chapter, four aspects of the development of UVMS, the underwater biomimetic vehicle propelled by unfurling fin, the autonomous intervention and trajectory planning of UVMS are reviewed successively.

1.1 Underwater Vehicle-Manipulator System UVMS typically consists of a vehicle body and a manipulator that is designed, built, and assembled together to support underwater intervention through a highly integrated sensing, planning, and control system. In terms of underwater vehicles, traditional UVMS mostly adopt mature frame structure based on propeller propulsion. By controlling multiple propellers in the forward, vertical and lateral direction and cooperating with the rudder surface, underwater vehicles can achieve all-round underwater movement. On the other hand, inspired by the efficient and highly maneuverable movement modes of underwater creatures such as fish, underwater vehicles based on bionic propulsion technology have also been studied a lot. Professor Triantafyllou from MIT first developed RoboTuna [1] and carried out research on the theory and method of efficient caudal fin propulsion. Later, University of Essex [2–4], University of Washington [5], Michigan State University [7, 8], and others carried out a lot of work on underwater vehicle with oscillating caudal fin. Osaka University in Japan [8–10], Delft University of Technology in the Netherlands [11], Northwestern University in the USA [12– 14], and Nanyang Technological University in Singapore [15–17] have developed theoretical prototypes in the aspect of pectoral fin and undulating fin propellant robots. Beijing University of Aeronautics and Astronautics [18–20] and Institute of Automation, Chinese Academy of Sciences [21–25] developed a variety of prototype systems for pectoral fin, undulatory fin, and caudal fin propulsion underwater vehicle. National University of Defense Technology [26, 27], Harbin Engineering University [28, 29], Harbin Institute of Technology [30, 31], University of Science and Technology of China [32, 33], Peking University [34, 35], etc., have also carried out a lot of work in bionic propulsion system. In terms of underwater manipulators, most of the current underwater manipulators are designed and developed for the needs of ROV, manned deep-sea submersible, and deep-sea workstations. Mature multi-joint industrial manipulator structures and technologies are generally adopted and can be mounted on the carrier of underwater robots, such as ORION4R, CONAN7P, TITAN4, RigMaster, ATALAS7R of Schilling, Gips of Kraft TeleRobotics, MARIS7080 of Ansaldo, and Hlk-mb-4, HLKRHD5W, HLK-CRA60 of Hydro-lek. In “Octopus Integrating Project”, the seventh framework plan with the participation of many EU countries, the bionic octopus arm developed by The Laboratory of Advanced Robot Technology and Systems in Santa Ana, Pisa, Italy, also completed the underwater grabbing experiment [36]. A lot of research achievements have also been made on underwater manipulator in China.

1.1 Underwater Vehicle-Manipulator System

3

The manipulator of “Osprey” and “Blue Whale” developed by Huazhong University of Science and Technology and the hydraulic manipulator of “8A4” ROV have been put into practical use [37]. Recon-iv-300-sia series underwater vehicles developed by Shenyang Institute of Automation, Chinese Academy of Sciences are equipped with master–slave working manipulators that can wash, grind, cut, and blast [38]. In addition, Harbin Engineering University [39–41] and Zhejiang University [42] also carried out in-depth research on the design and control of underwater manipulator. In addition, Harbin Engineering University [39–41] and Zhejiang University [42] also carried out in-depth research on the design and control of underwater manipulator. Several classic UVMS are given below. In the TRIDENT project supported by the SEVENTH Framework of the European Union, many countries have participated in the development of the GIRONA500 UVMS, which is specially designed for autonomous intervention, as shown in Fig. 1.1. The total weight of the UVMS is about 169 kg and can carry either an ARM5E or GraalTeck manipulator. The absolute position of the system is measured and determined by the water GPS system and the underwater short baseline network. The autonomous navigation is realized by the data fusion of DVL sensor and AHRS sensor, and the autonomous obstacle avoidance is realized by the sonar sensor carried by the system. At present, the system can successfully realize the autonomous search and salvage of black boxes in rivers [43, 44]. Tokai University in Japan developed a dual-arm underwater vehicle-manipulator system, as shown in Fig. 1.2. The weight of the body of the underwater vehicle is 40.93 kg, and the weight of the manipulator is 11.2 kg. The UVMS system can change the position and attitude of the buoyancy module to control its attitude such as pitch and roll, so as to complete different tasks. This attitude control method has been verified by experiments and achieved good control effects [45].

Fig. 1.1 GIRONA500 underwater vehicle-manipulator system

4

1 Introduction

Fig. 1.2 UVMS developed by Tokai University in Japan

Figure 1.3 shows the UVMS system developed by Pohang University of Technology in South Korea. With six propellers, the vehicle can move in all directions and has a manipulator of 5 degrees of freedom. A system control method to minimize the recovery motion is proposed for the system, and the effectiveness of the proposed method is verified by simulation [46].

Fig. 1.3 UVMS developed by Pohang University of Technology in South Korea

1.2 Underwater Biomimetic Vehicle Propelled by Undulating Fins

5

1.2 Underwater Biomimetic Vehicle Propelled by Undulating Fins Since the propulsion system of the biomimetic fish motion mode has little disturbance and is not easy to lose power due to the entangling of water plants and other plants, we introduce the mode of wave propulsion into the design of the underwater vehicle-manipulator system to improve its underwater operating performance. The following is a brief introduction to the research status of underwater biomimetic vehicle propelled by undulatory fins. Compared with the biomimetic underwater propulsion with body and/or caudal fin propulsion (BCF) mode, the research on the propulsion mechanism of the media and/or paired fin propulsion (MPF) mode started late due to the restriction of biological experimental technology and unsteady fluid dynamics theory. However, in recent years, with the rapid development of fluid visualization technology and biological experimental measurement technology, the research on the propulsion mechanism of MPF propulsion mode and the development of underwater biomimetic vehicle based on MPF propulsion mode have made significant progress. In order to better study the undulating fin propulsion principle of knifefish and discuss the feasibility of using undulating fin propeller as the propulsion system of underwater robot, Northwestern University developed a ribbonlike fin propeller prototype in 2004 by imitating the long-fin undulating propulsion mode of “Nile Black Devil” knifefish, as shown in Fig. 1.4a. The propeller prototype is composed of 13 sets of driving units and adopts a rigid structure with a total length of 53 cm. Each drive unit is driven by an electromagnet, which controls the current of the electromagnetic coil to achieve the swing control of the fin bar and then controls the propeller to generate forward or reverse traveling waves with an amplitude of 30z and a frequency of 1–3 Hz [47]. In 2005, Northwestern University designed an improved undulating fin propulsion experimental device based on the first-generation undulating fin propeller, as shown in Fig. 1.4b. The undulating fin propulsion experiment consists of 16 identical drive units and latex film. Each drive unit consists of a digital servomotor, a 1:1 gear set, a fin strip, and a load-bearing frame. The propulsive performance of the experimental device is significantly improved over that of the first generation, thanks to the use of a servo motor with greater torque and easier control. In addition, the experimental device adopts the modular design idea, which is more conducive to study the influence of different fin film materials, fin strip materials, and fin strip size on the propulsion performance [48]. In 2009, in order to further study the propulsion mechanism of undulating fin and the relationship between propulsion performance and wave parameters, Northwestern University in the USA developed an experimental device for undulating fin propulsion driven by DC motor, as shown in Fig. 1.4c. The length of the flexible long fin of the new experimental device is 32.6 cm and the width is 3.37 cm, and the aspect ratio is roughly the same as that of a typical adult living black devil knife fish. The experimental device consists of 32 fin strips, each of which is driven by a 10 mm DC

6

1 Introduction

(a) Experimental device developed in 2004

(b) Experimental device developed in 2005

(c) Experimental device developed in 2005

(d) Fin drive unit developed in 2009

Fig. 1.4 Undulating fin propulsion experimental device developed by Northwestern University

motor with a reduction ratio of 64:1 and an encoder. In order to reduce the length of the entire experimental device, the driving motor is mounted inside the experimental device in a spiral overlap mode, as shown in Fig. 1.4d. Compared with the first and second generation of experimental devices, the propulsion characteristics of the new experimental device are closer to the swimming characteristics of real knifefish due to the DC motor with higher torque and the optimized design of the fin drive unit, which greatly promotes the research on the propulsion mechanism of undulating fin [49]. Since 2002, Osaka University in Japan has started to study the imitation squid underwater vehicle with two sets of flexible unfurling fins. Five experimental prototypes have been developed successively to improve the propulsion performance of the prototype [8]. Figure 1.5a shows its first prototype model-1 developed in 2002. The Model-1’s long fins are attached to the sides of the body, which needs to be mounted on brackets, resulting in a weight of 43.4 kg. Each long fin of Model-1 consists of 15 fins and a rubber film covering the fins. Each long fin is driven by an electric motor, and a Scottish yoke mechanism is used to coordinate and control each fin to generate a push wave on the fin. The prototype can move forward and turn freely in water, which verifies the feasibility of applying the biomimetic undulating fin propeller to underwater vehicle [50].

1.2 Underwater Biomimetic Vehicle Propelled by Undulating Fins

(a) Model-1

(b) Model-2

(d) Model-4

7

(c) Model-2

(e) Model-5

Fig. 1.5 Undulating fin propulsion experimental devices developed by Osaka University

In 2004, Osaka University in Japan developed the second generation of squid undulating fin propulsion underwater vehicle model-2, as shown in Fig. 1.5b. Each long fin of the Model-2 is driven by 16 steering gears, each of which can be individually controlled by a remote computer, allowing the Model-2 to generate a variety of long-fin motion patterns. Compared with Model-1, despite the lack of supports, Model-2 still needs many cables to connect with the outside world, which reduces the flexibility of swimming [50]. In 2006, Osaka University of Japan developed Model-3, as shown in Fig. 1.5c, on the basis of Model-1 and Model-2. The support and connecting cables were removed in Model-3, and the steering gear driver and controller were installed inside. Each long fin of the Model-3 consists of 17 fins, each driven by an independent steering gear. In addition to long fins on both sides, the Model-3 is also equipped with dorsal and caudal fins that control yaw angle and depth, respectively, giving the Model-3 threedimensional motion. In addition, the Model-3 also has a vertical center of gravity adjustment device, which further increases controllability. However, as the center of gravity adjustment device is installed on the body, the resistance increases, and asymmetric hydrodynamic force is produced, which affects the propulsion performance [8]. In 2009, Osaka University of Japan developed an improved prototype model-4 with better performance on the basis of the first three generations of prototypes, as shown in Fig. 1.5d. Like the Model-3, the two long fins of the Model-4 are made up

8

1 Introduction

of 17 fins, each independently driven by a steering gear, allowing the Model-4’s long fins to generate any type of undulating motion. The swimming experiment shows that Model-4 can move in any direction along the three-dimensional underwater space and its attitude angle can be adjusted. The Model-4 has a longer tail fin than the Model-3, which is used to adjust the pitch angle and thus change the depth. Model-4 also has a center of gravity adjustment device, which is installed inside the prototype and can be used to adjust the horizontal and vertical positions of the center of gravity. Experiments with real fish show that model-4 is environmentally friendly [8, 10]. In 2012, Osaka University in Japan developed model-5, a new prototype of squid undulating fin propulsion underwater vehicle that is simpler than all previous prototypes, as shown in Fig. 1.5e. The prototype has 7 fins on the left and right sides, which are driven by a DC motor. The Scottish yoke mechanism is used to convert the motor’s rotating motion into the swinging motion of each fin. The Model-5 also has two tail fins, each driven by a motor, to adjust depth. By properly controlling the movement of the left and right fins, the Model-5 can swim straight or turn or even rotate in any direction in the water [8]. Nanyang Technological University in Singapore has designed a single-fin thruster propelled by undulatory NKF-I that mimics the swimming mode of black devil saury, as shown in Fig. 1.6a. The single-fin thruster has the same undulating fin driving mode as the squid propeller. The body of NKF-I also has a suction and drainage function that regulates gravity for snorkeling control [51]. Experimental results show that NKF-I can move flexibly and freely in water [52, 53]. Subsequently, Nanyang Technological University in Singapore developed two single-fin undulating propulsion experimental devices NKF-II (shown as Fig. 1.6b) and Machine eel (shown as Fig. 1.6c) to improve the shortcomings of the above thrusters, such as limited fin swing and insufficient compact structure. Robotic eels have controllable tail fins to control the direction of cruising and can achieve a horizontal undulating motion similar to that of an eel or a vertical undulating motion similar to that of a dolphin. The device was used to study and analyze the propulsion force and velocity of the undulating fin [16, 17, 54]. National University of Defense Technology took the lead in developing undulating fin propulsion underwater vehicle in China. In 2003, they successfully developed a prototype system of biomimetic underwater robot based on undulating fin propulsion, as shown in Fig. 1.7a. The prototype system consists of 13 fins and a rubber fin surface. It is driven by a single motor, and the motion of the motor is converted into the motion of the fin by a transmission mechanism. The 2-degree-of-freedom tail rudder at the tail of the system can assist it to complete pitching, turning, and other maneuvers. The experiments show that the prototype system can move forward or backward flexibly. However, because the system is only driven by a motor, its control flexibility is not high [27, 55, 56]. Aiming at the shortcomings of the above model, National University of Defense Technology developed a set of undulating fin propulsion device in 2005, which was driven by the steering gear and could be independently controlled by each fin bar, as shown in Fig. 1.7b. The device has a total of 17 fins, each of which has an independent drive and control system. The swinging mechanism of the fins adopts the “steering

1.2 Underwater Biomimetic Vehicle Propelled by Undulating Fins

(a) NKF-I

9

(b) NKF-II

(c) Machine eel Fig. 1.6 Undulating fin propulsion experimental device developed by Nanyang Technological University, Singapore

(a) Year 2003

(c) Year 2007

(b) Year 2005

(d) Year 2009

Fig. 1.7 Undulating fin propulsion experimental device developed by National University of Defense Technology

10

1 Introduction

drive–gear drive” structure design. The fins are made of spring steel and embedded in the flexible fin surface made of elastic rubber material. The undulating fin biomimetic device can realize a variety of unfurled motion modes of biomimetic objects and meet the needs of fluid dynamics testing corresponding to various unfurled parameters in a given propulsion wave mode, becoming an important experimental system for the study of unfurled fin dynamics model in National University of Defense Technology [57]. In 2007, in order to reduce the size of the experimental device and enhance the propulsion capability, National University of Defense Technology developed a set of biomimetic undulating fin propeller experimental device, as shown in Fig. 1.7c. The experimental device has 9 fin bars, which can present two complete sine waves at most. Each fin bar is actuated by a permanent magnet DC servo motor, whose rotational motion is directly converted into the swinging motion of the fin bar by a set of gears [58–60]. In 2009, National University of Defense Technology developed a squid undulating fin propulsion underwater vehicle, as shown in Fig. 1.7d. The vehicle consists of a barrel-shaped body, two long undulating fins, two tail fins, and other electronics. Two long undulating fins mounted symmetrically on either side of the body act as thrusters for the vehicle. University of Science and Technology of China successfully developed a biomimetic bluespotted ribbontail ray prototype in 2008 on the basis of in-depth research on the biological morphology of bluespotted ribbontail ray and the characteristics of pectoral fin undulation and movement, as shown in Fig. 1.8. The prototype is composed of three parts: a pair of pectoral fins, the main body, and the tail. Each pectoral fin consists of seven fins and corresponding fin surfaces, each of which is driven by an independent steering gear to increase the controllability of the pectoral fin. In addition, the prototype also has a center of gravity adjustment mechanism for snorkeling control. On the basis of the prototype, the Chinese university of science and technology of the thorough analysis of the kinematics and dynamics characteristic of the bluespotted ribbontail ray fins, and fluctuations fin propulsion of kinematics and dynamics model is established, the fin propulsion and advancing speed fluctuations has carried on the theoretical derivation, through active regulation and the design parameters of multiple biomimetic shark prototype of the basic movements such as cruise, turns, ups, and downs, combined with underwater infrared sensor array, the obstacle avoidance function is realized [60]. Institute of Automation, Chinese Academy of Sciences has carried out a large number of studies on underwater vehicle propelled by undulating fins, and successively developed two generations of squid underwater vehicle propelled by undulating fins, as shown in Fig. 1.9 [61, 62]. The second generation of underwater vehicle propelled by undulating fins is composed of 6 fins on each side, and each fin is driven by a steering gear alone. Various forms of wave motion can be realized on the surface of the long fin, and parameters such as wave frequency, wave amplitude and wavelength can be adjusted in real time. The three-dimensional motion control of the undulating fin propulsion underwater vehicle, such as advance and retreat, steering, and snorkeling, as well as the closed-loop motion control of the fixed depth directional motion control and the path point tracking control, are realized.

1.2 Underwater Biomimetic Vehicle Propelled by Undulating Fins

(a) Undulating fin

11

(b) Vehicle prototype

Fig. 1.8 Undulating fin propulsion experimental device developed by University of Science and Technology of China

(a) First generation prototype

(b) Second generation prototype

Fig. 1.9 Undulating fin propulsion underwater vehicle developed by institute of Automation, Chinese Academy of Sciences

At present, there are three main methods to study the propulsion mechanism of unfurled fin: theoretical analysis using hydrodynamic theory, experimental observation, and numerical calculation.

1.2.1 Theoretical Analysis of Undulatory Fin The theoretical research on the propulsion mechanism of undulatory fin is mainly based on “Elongated Body Theory” [1, 64], “Waving Plate Theory” [65, 66], and “Actuator Disk Theory” [67]. In 1960, Lighthill introduced the “Elongated Body Theory” in aerodynamics to the fluid analysis of fish swimming, and for the first time proposed the “Unsteady Elongated Body Theory” applicable to lateral body fluctuations, and established the first mathematical model used to analyze the propulsion mode of Carangidae fish [68]. Later, Lighthill proposed the “Elongated Body Theory” [1] and the “Large

12

1 Introduction

Swing Elongated Body Theory” [64] in 1970 and 1971, respectively, which were applied to analyze the propulsion methods of Carangidae and Anguillidae. Due to the similarity between the wave mode of MPF and BCF propulsion mode, Lighthill and Blake used the large swing elongated body theory in 1990 to analyze the wave propulsion performance of MPF propulsion mode [69–72]. Wu Yaozu regarded the fish body as a 2-D elastic thin plate and applied the potential flow theory and boundary layer conditions to study its propulsion performance. In 1961, he proposed the “2-D Waving Plate Theory” [65]. In 1971, Wu Yaozu further proposed the “Unsteady 2-D Waving Plate Theory,” which analyzed the swimming of flat crescent tail fish and its optimization method [73–75]. In 1991, Cheng Jianyu and others extended the 2-D waving plate model to the 3-D situation. Based on the unsteady potential flow theory of the small wave surface, they studied the motion of the wave plate with any plane shape and any aspect ratio. Using the semi-analytic– semi-numerical approach, they established the “3-D Waving Plate Theory” [66, 76], used it to analyze three swimming modes, gave a quantitative 3-D unsteady theoretical description, and revealed the morphological adaptation relationship between fish shape evolution and swimming mode 错误!未 未找到引用源。. Actuator disk theory is a special application of momentum principle in fluid dynamics. This theory simplifies the undulatory fin to an ideal device (actuator disk). When the fluid flows through the actuator disk, the pressure around it increases. By integrating the pressure on the entire actuator disk, the propulsion force generated by it can be obtained. The main advantage of this method is that it regards the undulatory fin as a “Black Box” without knowing its detailed kinematics model. The disadvantage is that when the energy of the vortex shedding in the wake is not negligible, the theoretical calculation result will exist a larger error [77].

1.2.2 Experimental Observation and Analysis of Undulating Fin In experimental observation, high-speed cameras, photoelectric measuring instrument, biological myoelectric signal measuring instrument, digital particle image velocimetry (DPIV), and other technical means are mainly used to observe the physiological information, morphological structure, etc., of living fish. By processing experimental data to reveal the swimming mechanism of fish. Rosenberger used a high-speed camera and EMG signal measuring instrument to conduct a series of experimental observations on the bluespotted ray and eight species of the ray, which not only revealed the pectoral fin wave motion pattern of the bluespotted ray and the muscles during the pectoral fin wave motion. The physiological changes [79] and the transition process of ray propulsion from wave propulsion to swing propulsion are also analyzed by comparison [80]. MacIver et al. used infrared camera technology to conduct a large number of experimental studies on the predation behavior of naked-back eel pattern saury,

1.2 Underwater Biomimetic Vehicle Propelled by Undulating Fins

13

revealing that saury can use its own electrical sensing organs to sense and locate the environment in a complex environment, and it can also be used in the process of predation. Continuously adjust its strategy of tracking food to ensure the success rate of predation [81]. Hu Tianjiang et al. conducted experimental observational research on the “Black Nile Devil” saury and revealed the relationship between the morphological characteristics, motion characteristics, and motion parameters of the flexible waving long fin, and based on this, established a simple kinematic model of flexible waving long fin [82]. Curet et al. observed the characteristics of the fin surface of the “Black Nile Devil” saury while staying in the fish tank and found that the “Black Nile Devil” saury achieved stable residence by generating two traveling waves that propagate back to the center of the long fin [83].

1.2.3 Numerical Analysis of Undulating Fin In the research on the propulsion mechanism of the undulatory fin, because the experimental observation method can only obtain qualitative results or relatively rough quantitative results, there is an urgent need for a more accurate method to quantitatively analyze the propulsion mechanism of the undulatory fin. In recent years, with the development of computer science, complex fluid calculation methods, and fluid simulation technology, some researchers have applied computational fluid dynamics (CFD) methods to the study of undulatory fin propulsion mechanism and achieved a series of meaningful results [84–86]. Northwestern University in the USA has long been committed to studying the swimming mechanism of the bareback eel saury and the principle of weak electric field perception. In 2008, the CFD method was used to study the hydrodynamics of the flexible long fin, and the movement of the long fin was idealized as sine traveling wave, through numerical calculations, the relationship between the propulsion force and the wavelength, frequency, amplitude and the width of the long fin is obtained, and it is revealed that the primary mechanism for generating the propulsion force is the central jet and the corresponding vortex ring generated when the long fin moves [87]. In 2011, they used the CFD method to conduct an in-depth study on the new motion pattern generated by the traveling wave backpropagation, revealing that there are two inward and backward traveling waves on a single flexible long fin that will produce a mushroom cloud-like flow field, and jet flow and pointed out that the vertical thrust generated by this swimming mode is stronger than other swimming modes [83]. The National University of Defense Technology began to use the CFD method to study the propulsion mechanism of the undulatory fin earlier in China. In 2006, they used the CFD method to conduct an in-depth study on the relationship between the position of the dorsal fin and the pressure and drag coefficient of the “Nile Black Devil” saury, which proved that the natural evolution of the saury was the optimal

14

1 Introduction

position of the dorsal fin [88]. In 2009, they conducted a numerical simulation study on the hydrodynamic model of the flexible long fin for 2-D wave motion and obtained the propulsion force of the undulatory fin under different wave amplitudes, different wave frequencies, and different wave numbers. Using statistical analysis method, they obtained a dimensionless two-parameter model of the undulatory fin [89]. In 2010, they used the CFD method to study the hydrodynamic performance of the bionic undulatory fin propulsion underwater robot, and through the comparative analysis of CFD calculation results and experimental results revealed a variety of specific phenomena in the swimming process of the bionic undulatory fin propulsion underwater robot [85]. The University of Science and Technology of China used the CFD method to carry out a lot of numerical research work on the bionic waving thruster and used the unstructured, unstable Navier–Stokes solver with automatic adaptive regrid to analyze the unsteady flow field around the undulatory fin. A simulation calculation was carried out, revealing the relationship between the propulsion force and different kinematic parameters, and pointed out that the wavelength has the greatest influence on the propulsion force [90, 91]. In addition, they also used the CFD method to analyze the propulsion force and propulsion efficiency of the undulatory fin in different wave modes and pointed out that the propulsion force and the propulsion efficiency are the largest when the traveling wave amplitude envelope increases from the head to the tail [92]. Zhejiang University took the saury fish of the bowfin order as the research object and used the CFD method to carry out the 3-D numerical simulation calculation of the flexible long-fin wave propulsion movement, and revealed the pressure field and velocity field distribution characteristics of the surrounding fluid during the wave movement of the flexible long fin. And it is concluded that the fluctuation period of the flexible fin’s waving propulsion is half of the long-fin wave period [86, 93]. After decades of research on fish swimming methods, the research on fish propulsion mechanism has made great progress. Whether it is experimental observation research or numerical calculation research, it has greatly contributed to our understanding of fish swimming mechanism. But the current research results are far from our complete understanding of fish swimming mechanism.

1.3 Autonomous Operation of the Underwater Vehicle-Manipulator System In order to realize the autonomous operation of the underwater vehicle-manipulator system, a number of technical problems need to be solved. At present, much work has been carried out on the UVMS motion redundancy, system dynamics modeling, control methods, and other aspects. As an over-redundant system, the UVMS has two main methods to solve the problem of robot motion redundancy. One is to plan the path of the underwater

1.3 Autonomous Operation of the Underwater Vehicle-Manipulator System

15

robot and the manipulator; the other is to solve the problem by using the pseudoinverse matrix. Using the first method, Santos et al. proposed a feedback neural network control method to solve the problem of motion redundancy of the UVMS, mainly by controlling the speed of the underwater robot to avoid the appearance of the system’s singular posture [94]. Combining the second method, Antonelli et al. proposed a pseudo-inverse kinematics method based on task priority to solve the motion redundancy problem of the UVMS and combined the fuzzy strategy to achieve the coordinated control of the robot and the manipulator [96, 97]. Similarly, Zhang Qifeng et al. studied the problem of system redundancy and action coordination based on the pseudo-inverse matrix and penalty factor method. The dynamic model of the robot body and the manipulator of UVMS is mainly established through theories and methods such as Kane equation, Newton–Euler equation, Lagrange equation, and its change equations. The dynamic model generally focuses on the influence of hydrodynamics and the factors such as coupling between robot body and the manipulator. On the basis of analyzing the kinematics of UVMS, Xu Changmi considered the influence of hydrodynamics and used the Kane method to establish the kinetic model of UVMS [98]. Guo Ying considered various hydrodynamic influences, used Quasi-Lagrange equations to establish the dynamic model of UVMS, and then decomposed the model into a series of interrelated subsystems, so as to use PID controller and sliding mode controller to realize the coordinated control of UVMS [99]. Sarkar et al. used the quasi-Lagrange method to establish the state space equation of the underwater robot system and planned the operation trajectory with the minimum water resistance of the system as the index [100]. Many scholars have also proposed many methods for the control of UVMS. Han et al. considered the influence of external disturbances and proposed a nonlinear H∞ optimal control with disturbance observer to implement the tracking control of UVMS [102]. Mohan et al. proposed an indirect adaptive control method based on Kalman filter for the autonomous operation of UVMS. This method overcomes the shortcomings of the existing disturbance observer and direct adaptive control, and the method can be used for consideration of load compensation, underwater ocean currents or external disturbance compensation, the compensation of the reaction force between the robot and the manipulator, etc. [103]. Xu et al. proposed an intelligent control algorithm based on neuro-fuzzy to realize the job control of UVMS. The proposed decentralized neural network compensator is used to estimate the dynamics of UVMS. This method can better cope with changes in load and water dynamic disturbance [104]. Olguin-Diaz et al. proposed a force motion-based control framework to implement UVMS job control. The framework does not need to establish a UVMS dynamic model in advance and applies a second-order modelfree sliding mode control method to calculate the control rate of force motion in control framework [105]. Lynch et. al combined feedback and feedforward methods to achieve the stability of the robot’s posture and position, thereby achieving effective manipulator control. The feedforward compensation considers the kinematics model and dynamics model of UVMS. In dynamic model, the effect of hydrodynamics takes into account the additional mass compensation, buoyancy, and the reaction force of

16

1 Introduction

the manipulator to the robot. This realization can reduce the energy consumption of the entire system [106]. Although the job control method of UVMS has made great progress, most studies mainly use simulation results to show the effectiveness of the proposed method, and lack of platform experiment verification. At present, only a few UVMS can realize the autonomous operation and grasping of the target. At present, there are two international engineering research projects that have greatly promoted the development of UVMS autonomous operations. One is the SAUVIM project, which developed a semi-autonomous UVMS platform with a 7-degree-of-freedom underwater manipulator and realized autonomous operation [107, 108]. The other is the TRIDENT project, which developed the GIRONA500 UVMS, which successfully achieved underwater autonomous operations using algorithms such as coordinated control based on task priority assignment and manipulator control based on vision sensors [109–112].

1.4 Trajectory Planning of the Underwater Vehicle-Manipulator System In order to improve the autonomy and intelligence of UVMS, UVMS should autonomously move from the current initial point to the target operating point to realize the operation. Therefore, for UVMS, path planning is one of its key technologies. Its basic idea is based on a certain performance index (such as the shortest time, the least energy, and the shortest path), to find an optimal or approximately optimal path from the initial point to the final point. At present, it mainly includes the following algorithms. (1) Road Map Method The road map method is mainly divided into the view method and the Voronoi diagram method. The visual view method was first proposed by Nilsson of Stanford University [113]. Its main idea is: In the space of a robot, obstacles of any shape can be approximated by polygons, and the robot itself is regarded as a mass point. The idea that the robot can wipe the vertices of a polygonal obstacle, regard the robot as a point, connect the starting point, the target point and all the vertices of the polygonal obstacle with line segments, and delete the line passing through the obstacle to form a visible view, and then search the shortest path [114]. This method can be applied to 2D path planning problems. Kuwata et al. improved the visual method and applied it to 3-D trajectory planning. Obstacles are represented by polyhedrons, but the obtained path is not necessarily optimal [115]. The Voronoi diagram method is an evolution and improvement of the visualization method, which makes up for the inability of the visualization method to be suitable for circular obstacles, and improves the problem that the visualization method does not consider the size of the robot itself. The basic idea of the Voronoi diagram method is to generate Voronoi edges equidistant from all

1.4 Trajectory Planning of the Underwater Vehicle-Manipulator System

17

edges of the obstacle polygon and compose the intersection of all Voronoi edges into a vertex set, and then plan the path by searching for the shortest path edge similar to the visible view. The Voronoi diagram method can ensure that the robot reaches the target with the greatest degree of safety, but it leads to the increase of the robot’s path to the target position, which is not the optimal path [116, 117]. (2) Artificial Potential Field Method The basic idea of the artificial potential field method is to use a potential function to describe the robot environment space. As a particle, the robot is subject to force. The target point has the lowest potential energy to generate attraction to the robot, and obstacles generate repulsion to the robot. The path follows the attraction and repulsion. The direction planning of the resultant force formed by the force. As early as 1990, Warren et al. applied the artificial potential field method to generate a safe and feasible global path for underwater autonomous robots in a two-dimensional or three-dimensional environment with obstacles [96]. Similarly, in 1993, Sun Mao et al. applied artificial potential field theory to realize the optimal obstacle avoidance path planning of underwater autonomous robots in complex and changing ocean conditions [118]. Witt et al. used the artificial potential field method to generate an optimized path for the underwater robot with the least resistance and the shortest time as the index [119]. The artificial potential field method has a simple structure and is convenient for real-time control of the bottom layer. It has been widely used in real-time obstacle avoidance and smooth trajectory control. (3) Heuristic Algorithm The heuristic algorithm is to evaluate each searched position in the state space to get the best position, and then search from this position to the target, including genetic algorithm, neural network method, etc. • Genetic Algorithm Genetic algorithm is a kind of random optimization search algorithm based on Darwin’s biological evolution theory of natural selection and genetic mechanism of biological evolution theory. It realizes the search for optimal solution by simulating the selection, crossover, and mutation mechanism in the biological evolution process. Genetic algorithm is a multi-point search algorithm, which does not rely on gradient information and is suitable for searching the global optimal solution. This method is very suitable for solving the problem of robot trajectory planning [120–123]. Alvarez et al. proposed a genetic algorithm to search for a safe path from the initial point of the underwater robot to the specified operation target point with the energy cost as the optimization index. For the sea areas with variable time and space and strong ocean currents, special genetic operations can be used to ensure that the overall energy is searched for the optimal path [124]. Based on the realtime information of forward-looking sonar detection, Chang Zonghu et al. proposed to use genetic algorithm to generate a path for underwater robots to autonomously

18

1 Introduction

avoid moving obstacles [125]. Cheng et al. combined genetic algorithm and dynamic programming algorithm to navigate the underwater autonomous robot and generate a path with minimum path length, minimum turning angle, and maximum pitch rate for the robot [126]. • Neural Network Method In recent years, the neural network method has received more and more attention in the path planning of robots [127–130]. The algorithm defines the total energy function of the entire path, including the energy of the path length part and the energy corresponding to the collision function part. Since the entire energy is a function of each path point, by moving each path point toward the direction of energy reduction, the path with the smallest total energy can be obtained in the end [131]. Neural network method adopts parallel distributed processing, fast speed, self-learning, strong adaptive nonlinear approximation ability, and strong search path ability. Based on the created spatial grid map, Zhu Daqi et al. proposed a biologically inspired neural network model and associated it with the spatial grid map. By matching the neurons with the grid of the grid map, the underwater autonomous robot is attracted to move toward the activated neurons, while the inhibited neurons repel the underwater autonomous robot to deviate from its movement, and the maximum activity value grid is used as the navigation direction of the underwater autonomous robot. This method can adaptively generate a collision avoidance safety path [132, 133]. (4) Rapid Expansion Random Search Tree Method The rapid expansion random search tree method (RRT) is a sampling-based single query path planning method, which was proposed by LaValle in 1998 [134]. In the robot motion space, the current initial position of the robot is taken as the root node, and the space is expanded and constructed by random sampling and gradually expanded into a random tree under constraints. The path connecting the root node and the final target point is the planned path. The RRT algorithm is suitable to be applied to path planning problems with kinematics and dynamics constraints [135–137]. Tan et al. used the RRT algorithm to realize the path planning of the underwater robot under turbulence. The random search tree used is extended in the direction of ocean currents. This method generates collision avoidance paths faster in a highdimensional configuration space [138]. Heo et al. considered the kinematics model of the underwater autonomous robot and used the RRT algorithm to effectively solve the robot’s local trajectory planning problem, and then used the A∗ algorithm to search for the shortest path to avoid collision from the tree obtained by the RRT algorithm [139]. Hernandez et al. proposed the HRRT algorithm to realize the 2-D trajectory planning of underwater autonomous robots. The Jenkins method is used to generate topological information in the robot workspace, and the path search is guided based on the topological information [140, 141].

References

19

(5) Curve Planning Method The curve planning method is a path planning method that uses a commonly used smooth curve to achieve a smooth connection between the initial point and the target point. This method can consider the posture information of the robot. Mahmoudian et al. used the Dubins curve to generate a path with the least energy consumption for the glider [142]. The Dubins curve is the shortest path in the plane that satisfies the turning radius constraint and connects the initial pose and the target pose [143]. B-spline curve is widely used in trajectory planning. In 2008, Zhang et al. proposed a B-spline curve model with two-dimensional position and time changes to generate a trajectory with the best energy consumption for the glider in an ocean current environment [144]. Similarly, in 2012, Zeng et al. used the B-spline curve to plan a feasible optimized and effective trajectory for underwater robots in uncertain and unknown ocean waters [145]. In 2015, Li Ye et al. combined the cubic Bspline curve and genetic algorithm to generate a recovery path for the under-actuated autonomous underwater vehicle, which took into account the task terminal constraints and motion constraints brought by the under-actuated characteristics [146]. In addition, the Bezier curve is often applied to the trajectory planning of underwater robots. Filaretov et al. used the Bezier curve to generate a path for the MARC underwater robot [147].

References 1. Triantafyllou MS, Triantafyllou GS (1995) An efficient swimming machine. Sci Am 272(3):64–71 2. Liu J, Hu H (2005) Mimicry of sharp turning behaviours in a robotic fish. In: Proceedings of the 2005 IEEE international conference on robotics and automation, Barcelona, Spain, pp 3318–3323 3. Hu H, Liu J, Dukes I, Francis G (2006) Design of 3-D swim patterns for autonomous robotic fish. In: Proceedings of 2006 IEEE/RSJ international conference on intelligent robots and systems, Beijing, China, pp 2406–2411 4. Liu J, Hu H (2010) Biological inspiration: from carangiform fish to multi-joint roboticfish. J Bionic Eng 7(1):35–48 5. Morgansen KA, Triplett BI, Klein DJ (2007) Geometric methods for modeling and control of free-swimming fin-actuated underwater vehicles. IEEE Trans Robot 23(6):1184–1199 6. Zhang F, Zhang F, Tan X (2012) Steady spiraling motion of gliding robotic fish. In: Proceedings of IEEE/RSJ international conference on intelligent robots and systems, Algarve, Portugal, pp 1754–1759 7. Zhang F, Thon J, Thon C, Tan X (2014) Miniature underwater glider: design and experimental results. IEEE/ASME Trans Mechatron 19(1):394–399 8. Rahman MM (2013) Study on biomimetic squid-like underwater robots with two undulating side fins. Ph.D. thesis, Osaka University, Osaka 9. Toda Y, Suzuki T, Uto S, Tanaka N (2004) Fundamental study on propulsion of a fish-like body with two undulating finns, book section 8. Springer, Japan, pp 93–110 10. Rahman MM, Sugimori S, Miki H, Yamamoto R, Sanada Y, Toda Y (2013) Braking performance of a biomimetic squid-like underwater robot. J Bionic Eng 10(3):265–273

20

1 Introduction

11. Simons DG, Bergers MMC, Henrion S, Hulzenga JIJ, Jutte RW, Pas WMG, van Schravendijk M, Vercruyssen TGA, Wilken AP (2009) A highly versatile autonomous underwater vehicle with biomechanical propulsion. In: OCEANS 2009—EUROPE, pp 1–6 12. Epstein M, Colgate JE, MacIver MA (2006) Generating thrust with a biologically inspired robotic ribbon fin. In: Proceedings of 2006 IEEE/RSJ international conference on intelligent robots and systems, Beijing, China, pp 2412–2417 13. Curet OM, Patankar NA, Lauder GV, MacIver MA (2011) Mechanical properties of a bioinspired robotic knifefish with an undulatory propulsor. Bioinspir Biomim 6(2):1–9 14. Bale R, Shirgaonkar AA, Neveln ID, Bhalla APS, MacIver MA, Patankar NA (2014) Separability of drag and thrust in undulatory animals and machines. Sci Rep 4 15. Low KH, Willy A (2005) Development and initial investigation of NTU robotic fish with modular flexible fins. In: Proceedings of 2005 IEEE international conference on mechatronics and automation, pp 958–963 16. Low KH (2009) Modelling and parametric study of modular undulating fin rays for fish robots. Mech Mach Theory 44(3):615–632 17. Low KH, Zhou C, Zhong Y (2009) Gait planning for steady swimming control of biomimetic fish robots. Adv Robot 23(7–8):805–829 18. Liang J, Zheng W, Wen L, Wang T, Liu Y (2010) Propulsion and mobility of two joint bionic underwater vehicle SPC–III. Robot 32(6):726–731 19. Liang J, Wang T, Wen L (2011) Development of a two-joint robotic fish for real world exploration. J Field Robot 28(1):70–79 20. Wen L (2010) Propulsion mechanism experiment and control of bionic robotic fish. Doctoral dissertation, Beijing University of Aeronautics and Astronautics, Beijing 21. Shang L, Wang S, Tan M, Dong X (2009) Motion control for an underwater roboticfish with two undulating long-fins. In: Proceedings of the 48th IEEE conference on decision and control, Shanghai, China, pp 6478–6483 22. Wei Q (2015) Design and control method of modulatory fin propulsion underwater vehicle. Doctoral dissertation, Graduate School of Chinese Academy of Sciences, Beijing 23. Wang M (2010) CPG-based motion modeling and control of biomimetic robotic fish. Ph.D. dissertation, Graduate School of Chinese Academy of Sciences, Beijing 24. Su Z (2012) High maneuvering motion control of bionic robot fish. Doctoral Dissertation, Graduate School of Chinese Academy of Sciences, Beijing 25. Wu Z (2015) 3D maneuver and gliding motion control of bionic robot fish. Doctoral Dissertation, Graduate School of Chinese Academy of Sciences, Beijing 26. Xie H, Zhang D, Shen L (2006) Design and Implementation of a bionic underwater robot based on flexible long fin wave propulsion. Robot 28(5):526–529 27. Xie H (2006) Design, modeling and control of a bionic underwater robot based on multi-wave fin propulsion. Ph.D. dissertation, National University of Defense Technology, 2006. 28. Yu K (2007) Theoretical prediction and experimental research on hydrodynamic performance of bionic propeller. Doctoral Dissertation, Harbin Engineering University, Harbin 29. Su Y (2012) Research on bionic micro robot fish driven by ICPF smart materials. Doctoral Dissertation, Harbin Engineering University, Harbin 30. Wu Z (2015) Numerical simulation and experimental research of wave propulsion based on crystal kiss ray. Doctoral Dissertation, Harbin: Harbin Institute of Technology 31. Gu X (2015) The mechanism and experimental research of air pressure regulating variable stiffness flexible bionic robotic fish. Master’s degree thesis, Harbin Institute of Technology, Harbin 32. Chen H (2006) Research on the movement mechanism of bionic robot fish cruise and maneuver. Doctoral Dissertation, University of Science and Technology of China, Hefei 33. Wu W (2010) Experimental study on the propulsion performance of imitating carpidae robotic fish. Master’s degree thesis, University of Science and Technology of China, Hefei 34. Hu Y, Wang L, Yu J, Huo J, Jia Y (2008) Development and control of dolphin-like underwater vehicle. In: Proceedings of the American control conference, Washington, USA, pp 2858– 2863

References

21

35. Yu J, Li Y, Hu Y, Wang L (2009) Towards development of link-based robotic dolphin: experiences and lessons. In: Proceedings of IEEE international conference on robotics and biomimetics, Bangkok, Thailand, pp 240–245 36. Laschi C, Cianchetti M, Mazzolai B, Margheri L, Follador M, Dario P (2012) Soft robot arm inspired by the octopus. Adv Robot 26(7):709–727 37. Xiao Z (2011) Research on dynamic characteristics and autonomous operation of deep-water manipulators. Doctoral dissertation, Huazhong University of Science and Technology, Wuhan 38. Jiang X, Feng X, Wang D (2000) Underwater robot. Liaoning Science and Technology Press, Shenyang 39. Meng Q, Du W, Wang C, Wang L (2005) Study on automatic docking of SIWR-II underwater operating manipulators and tools. Ship Eng 27(6):1–4 40. Wang C (2009) Seven-function underwater manipulator body design and adaptive control research. Master’s degree thesis, Harbin Engineering University, Harbin 41. Zhang L (2008) Research on the body structure and impedance control of a three-degreeof-freedom underwater manipulator. Master’s degree thesis, Harbin Engineering University, Harbin 42. Qiu H (2006) Research on profiling handle control technology of manipulators for underwater operations in deep sea. Master’s degree thesis, Zhejiang University, Hangzhou 43. Ribas D, Palomeras N, Ridao P, Carreras M, Mallios A (2012) Girona 500 AUV: from survey to intervention. IEEE/ASME Trans Mechatron 17(1):46–53 44. Fernandez JJ, Prats M, Sanz PJ, Garcia JC, Marin R, Robinson M, Ribas D, Ridao P (2013) Grasping for the seabed: developing a new underwater robot arm for shallow-water intervention. IEEE Robot Autom Mag 20(4):121–130 45. Sakagami N, Ueda T, Shibata M, Kawamura S (2011) Pitch and roll control using independent movable floats for small underwater robots. In: 2011 IEEE/RSJ international conference on intelligent robots and systems, pp 25–30 46. Han J, Chung WK (2014) Active use of restoring moments for motion control of an underwater vehicle-manipulator system. IEEE J Oceanic Eng 39(1):100–109 47. MacIver MA, Fontaine E, Burdick JW (2004) Designing future underwater vehicles: principles and mechanisms of the weakly electric fish. IEEE J Oceanic Eng 29(3):651–659 48. Epstein M, Colgate JE, MacIver MA (2006) Generating thrust with a biologically-inspired robotic ribbon fin. In: 2006 IEEE/RSJ international conference on intelligent robots and systems, pp 2412–2417 49. Curet OM, Patankar NA, Lauder GV, MacIver MA (2011) Mechanical proper-ties of a bioinspired robotic knifefish with an undulatory propulsor. Bioinspir Biomim 6(2):1–9 50. Toda Y, Suzuki T, Uto S, Tanaka N (2004) Fundamental study on propulsion of a fish-like body with two undulating fins, book section 8. Springer, Japan, pp 93–110 51. Low KH (2006) Locomotion and depth control of robotic fish with modular undulating fins. Int J Autom Comput 3(4):348–357 52. Low KH, Willy A (2005) Development and initial investigation of NTU robotic fish with modular flexible fins. In: 2005 IEEE international conference on mechatronics and automation, pp 958–963 53. Willy A, Low KH (2005) Initial experimental investigation of undulating fin. In: 2005 IEEE/RSJ international conference on intelligent robots and systems, pp 1600–1605 54. Low KH, Seet GLG, Chunlin Z (2007) Biomimetic design and workspace study of compact and modular undulating fin body segments. In: 2007 IEEE international conference on mechatronics and automation, pp 129–134 55. Xie H, Zhang D, Shen L (2006) Design and implementation of bionic underwater robot based on flexible long fin wave propulsion. Robot 28(5):526–529 56. Xie HB, Shen LC (2007) Dynamic analysis on the bionic propulsor imitating undulating fin of aquatic animals. In: 2007 IEEE international conference on robotics and biomimetics, pp 268–273 57. Zhang D (2007) Research on wave fin bionic underwater propeller and its control method. Doctoral Dissertation, National University of Defense Technology, Changsha

22

1 Introduction

58. Hu TJ, Lin LX, Zhang DB, Wang DW, Shen LC (2009) Effective motion control of the biomimetic undulating fin via iterative learning. In: 2009 IEEE international conference on robotics and biomimetics, pp 627–632 59. Hu TJ, Shen LC, Low KH (2009) Bionic asymmetry: from amiiform fish to undulating robotic fins. Chin Sci Bull 54(4):562–568 60. Wu WG, Yan Q, Zhang SW, Zhang YH, Yang J (2009) Design and experimental research on an undulatory robotic fin. In: 2009 IEEE international conference on robotics and biomimetics, pp 2440–2444 61. Dong X (2009) Research on the design and control of the wave fin propelling bionic robotic fish. Ph.D. dissertation, Graduate School of Chinese Academy of Sciences, Beijing 62. Shang L (2011) Research on modeling and intelligent control method of bionic swimming machine. Doctoral Dissertation, Graduate School of Chinese Academy of Sciences, Beijing 63. Lighthill MJ (1970) Aquatic animal propulsion of high hydromechanical efficiency. J Fluid Mech 44(11):265–301 64. Lighthill MJ (1971) Large-amplitude elongated-body theory of fish locomotion. Proc R Soc Lond Ser B Biol Sci 179(1055):125–138 65. Wu TYT (1961) Swimming of a waving plate. J Fluid Mech 10(3):321–344 66. Cheng JY, Zhuang LX, Tong BG (1991) Analysis of swimming three-dimensional waving plates. J Fluid Mech 232:341–355 67. Blake RW (1980) Undulatory median fin propulsion of two teleosts with different modes of life. Can J Zool 58:2116–2119 68. Lighthill MJ (1960) Note on the swimming of slender fish. J Fluid Mech 9(2):305–317 69. Lighthill J, Blake R (1990) Biofluiddynamics of balistiform and gymnotiform locomotion. Part 1. Biological background, and analysis by elongated-body theory. J Fluid Mech 212:183–207 70. Lighthill J (1990) Biofluiddynamics of balistiform and gymnotiform locomotion. Part 2. The pressure distribution arising in two-dimensional irrotational flow from a general symmetrical motion of a flexible flat plate normal to itself. J Fluid Mech 213:1–10 71. Lighthill J (1990) Biofluiddynamics of balistiform and gymnotiform locomotion. Part 3. Momentum enhancement in the presence of a body of elliptic cross-section. J Fluid Mech 213:11–20 72. Lighthill J (1990) Biofluiddynamics of balistiform and gymnotiform locomotion. Part 4. Short-wavelength limitations on momentum enhancement. J Fluid Mech 213:21–28 73. Wu TYT (1971) Hydromechanics of swimming propulsion. Part 1. Swimming of a two dimensional flexible plate at variable forward speeds in an inviscid fluid. J Fluid Mech 46:337–355 74. Wu TYT (1971) Hydromechanics of swimming propulsion. Part 2. Some optimum shape problems. J Fluid Mech 46:521–544 75. Wu TYT (1971) Hydromechanics of swimming propulsion. Part 3. Swimming and optimum movements of slender fish with wide fins. J Fluid Mech 46:545–568 76. Cheng J, Zhuang L, Tong B (1991) Swimming of three dimensional variable amplitude wave plate. Hydrodyn Res Progr 6:1–11 77. Zhang Y (2008) Theoretical and experimental research on flexible bionic undulatory fin propulsion. Doctoral dissertation, University of Science and Technology of China, Hefei 78. Sfakiotakis M, Lane DM, Davies JBC (1999) Review of fish swimming modes for aquatic locomotion. IEEE J Oceanic Eng 24(2):237–252 79. Rosenberger LJ, Westneat MW (1999) Functional morphology of undulatory pectoral fin locomotion in the stingray taeniura lymma (chondrichthyes: Dasyatidae). J Exp Biol 202(24):3523–3539 80. Rosenberger LJ (2001) Pectoral fin locomotion in batoid fishes: Undulation versus oscillation. J Exp Biol 204:379–394 81. MacIver MA, Sharabash NM, Nelson ME (2001) Prey-capture behavior in gymnotid electric fish: motion analysis and effects of water conductivity. J Exp Biol 204(3):543–557 82. Hu TJ, Shen LC, Lin LX, Xu HJ (2009) Biological inspirations, kinematics modeling, mechanism design and experiments on an undulating robotic fin inspired by gymnarchus niloticus. Mech Mach Theory 44(3):633–645

References

23

83. Curet OM, Patankar NA, Lauder GV, MacIver MA (2011) Aquatic manoeuvering with counter-propagating waves: a novel locomotive strategy. J R Soc Interface 8(60):1041–1050 84. Kowalczyk W, Delgado A (2007) Simulation of fluid flow in a channel induced by three types of fin-like motion. J Bionic Eng 4(3):165–176 85. Zhou H, Hu TJ, Xie HB, Zhang DB, Shen LC (2010) Computational and experimental study on dynamic behavior of underwater robots propelled by bionic undulating fins. Sci China Technol Sci 53(11):2966–2971 86. Liu FF, Yang CJ, Lee KM (2010) Hydrodynamic modeling of an undulating fin for robotic fish design. In: 2010 IEEE/ASME international conference on advanced intelligent mechatronics, pp 55–60 87. Shirgaonkar AA, Curet OM, Patankar NA, MacIver MA (2008) The hydrodynamics of ribbonfin propulsion during impulsive motion. J Exp Biol 211(21):3490–3503 88. Hu TJ, Cheng SL, Gong PL (2006) CFD validation of the optimal arrangement of the propulsive dorsal fin of gymnarchus niloticus. J Bionic Eng 3(3):139–146 89. Zhou H, Zhang D, Hu T, Xie H, Shen L (2009) Statistical hydrodynamics modeling of two-dimensional undulating fins for robotic fish. In: 2009 IEEE international conference on robotics and biomimetics, pp 1663–1668 90. Zhang YH, Jia LB, He JH, Yang J, Zhang SW, Low KH (2006) A numerical analysis of an undulatory mechanical fin driven by shape memory alloy. In: 2006 IEEE international conference on robotics and biomimetics, pp 73–78 91. Zhang YH, He JH, Yang J, Zhang SW, Low KH (2006) A computational fluid dynamics (CFD) analysis of an undulatory mechanical fin driven by shape memory alloy. Int J Autom Comput 3(4):374–381 92. Zhang YH, Jia LB, Zhang SW, Yang J, Low KH (2007) Computational research on modular undulating fin for biorobotic underwater propulsor. J Bionic Eng 4(1):25–32 93. Liu FF, Lee KM, Yang CJ (2012) Hydrodynamics of an undulating fin for a wave-like locomotion system design. IEEE-ASME Trans Mechatron 17(3):554–562 94. Liu F (2012) Research on propulsion performance of underwater bionic system based on flexible fin fluctuation. Doctoral dissertation, Zhejiang University, Hangzhou 95. Santos CH, Cuenther R, Pieri ED (2006) A reactive neural network architecture to redundancy resolution for underwater vehicle-manipulator systems. In: Proceedings of IEEE international conference on robotics and automation, Orlando, FL, USA, pp 3238–3243 96. Antonelli G, Chiaverini S (2003) Fuzzy redundancy resolution and motion coordination for underwater vehicle-manipulator systems. IEEE Trans Fuzzy Syst 11(1):109–120 97. Antonelli G, Chiaverini S (2003) A fuzzy approach to redundancy resolution for underwater vehicle-manipulator systems. Control Eng Pract 11(4):445–452 98. Qifeng Z, Aiqun Z (2006) Research on coordinated motion of autonomous underwater robot manipulator system. Ocean Eng 4(3):79–84 99. Xu C (2010) Research on dynamic modeling and motion control of underwater robot manipulator system. Master’s thesis, Ocean University of China, Qingdao 100. Guo Y (2008) Coordinated control technology of underwater autonomous operating system. Doctoral dissertation, Huazhong University of Science and Technology, Wuhan 101. Sarkar N, Podder TK (2001) Coordinated motion planning and control of autonomous underwater vehicle-manipulator systems subject to drag optimization. IEEE J Ocean Eng 26(2):228–239 102. Han J, Park J, Chung WK (2011) Robust coordinated motion control of an underwater vehiclemanipulator system with minimizing restoring moments. Ocean Eng 38(10):1197–1206 103. Mohan S, Kim J (2012) Indirect adaptive control of an autonomous underwater vehiclemanipulator system for underwater manipulation tasks. Ocean Eng 54(22):233–243 104. Xu B, Pandian SR, Sakagami N, Petry F (2012) Neuro-fuzzy control of underwater vehiclemanipulator systems. J Franklin Inst 349(3):1125–1138 105. Olguin-Diaz E, Arechavaleta G, Jarquin G, Parra-Vega V (2013) A passivity-based modelfree force-motion control of underwater vehicle-manipulator systems. IEEE Trans Rob 29(6):1469–1484

24

1 Introduction

106. Lynch B, Ellery A (2014) Efficient control of an AUV-manipulator system: an application for the exploration of Europa. IEEE J Ocean Eng 39(3):552–570 107. Kim TW, Yuh J (2004) Development of a real-time control architecture for a semi-autonomous underwater vehicle for intervention missions. Control Eng Pract 12(12):1521–1530 108. Marani G, Choi SK, Yuh J (2009) Underwater autonomous manipulation for intervention missions AUVs. Ocean Eng 36(1):15–23 109. Kermorgant O, Petillot Y, Dunnigan M (2013) A global control scheme for free floating vehicle-manipulators. In: Proceeding of IEEE/RSJ international conference on intelligent robots and systems, Tokyo, Japan, pp 5015–5020 110. Simetti E, Casalino G, Torelli S, Sperinde A, Turetta A (2014) Floating underwater manipulation: developed control methodology and experimental validation within the TRIDENT project. J Field Robot 31(3):364–385 111. Bemfica JR, Melchiorri C, Moriello L, Palli G, Scarcia U (2014) A three-fingered cabledriven gripper for underwater applications. In: Proceeding of IEEE international conference on robotics and automation, Hong Kong, China, pp 2469–2474 112. Ribas D, Ridao P, Turetta A, Melchiorri C, Palli G, Fernandez JJ, Sanz PJ (2015) I-AUV mechatronics integration for the TRIDENT FP7 project. IEEE/ASME Trans Mechatron 20(5):2583–2592 113. Nilsson NJ (1969) A mobile automation: an application of artificial intelligence techniques. In: Proceedings of 1st international joint conference on artificial intelligence, Washington D.C, USA, pp 509–520 114. Cao J (2011) AUV path planning method in complex environment. Master’s thesis, Ocean University of China, Qingdao 115. Kuwata Y, How J (2004) Three dimensional receding horizon control for UAVs. In: Proceedings of AIAA guidance, navigation, and control conference and exhibit, Washington D.C, USA, pp 2100–2113 116. Takahashi O, Schilling RJ (1985) Motion planning in a place using generalized Voronoi diagram. IEEE Trans Robot Autom 5(2):143–150 117. Choset H, Burdick J (1995) Sensor based planning. I. The generalized Voronoi graph. In: Proceedings of IEEE international conference on robotics and automation, Nagoya, Japan, pp 1649–1655 118. Sun M, Wang Y, Wu X (1993) Path planning of underwater vehicle with dynamic compensation. Robot 15(2):8–12 119. Witt J, Dunbabin M (2008) Go with the flow: optimal AUV path planning in coastal environments. In: Proceedings of Australian conference on robotics and automation, Sydney, Australia, pp 1–16955 120. Tsai CC, Huang HC, Chan CK (2011) Parallel elite genetic algorithm and its application to global path planning for autonomous robot navigation. IEEE Trans Industr Electron 58(10):4813–4821 121. Roberge V, Tarbouchi M, Labonté G (2013) Comparison of parallel genetic algorithm and particle swarm optimization for real-time UAV path planning. IEEE Trans Industr Inf 9(1):132–141 122. Raja R, Dutta A, Venkatesh KS (2015) New potential field method for rough terrain path planning using genetic algorithm for a 6-wheel rover. Robot Auton Syst 72:295–306 123. Cakir M (2015) 2D path planning of UAVs with genetic algorithm in a constrained environment. In: Proceedings of IEEE international conference on modeling, simulation, and applied optimization, Istanbul, Turkey, pp 1–5 124. Alvarez A, Caiti A, Onken R (2004) Evolutionary path planning for autonomous underwater vehicles in a variable ocean. IEEE J Oceanic Eng 29(2):418–429 125. Chang Z, Tang Z, Cai H, Shi X, Bian X (2005) GA path planning for AUV to avoid moving obstacles based on forward looking sonar. In: Proceedings of IEEE international conference on machine learning and cybernetics, Guangzhou, China, pp 1498–1502 126. Cheng CT, Fallahi K, Leung H, Tse CK (2012) A genetic algorithm-inspired UUV path planner based on dynamic programming. IEEE Trans Syst Man Cybern Part C Appl Rev 42(6):1128–1134

References

25

127. Yang SX, Luo C (2004) A neural network approach to complete coverage path planning. IEEE Trans Syst Man Cybern B Cybern 34(1):718–724 128. Qu H, Yang SX, Willms AR, Zhang Y (2009) Real-time robot path planning based on a modified pulse-coupled neural network model. IEEE Trans Neural Netw 20(11):1724–1739 129. Syed UA, Kunwar F, Iqbal M (2014) Guided autowave pulse coupled neural network (GAPCNN) based real time path planning and an obstacle avoidance scheme for mobile robots. Robot Auton Syst 62(4):474–486 130. Duan H, Huang L (2014) Imperialist competitive algorithm optimized artificial neural networks for UCAV global path planning. Neurocomputing 125:166–171 131. Hu F (2011) Multi robot formation reconstruction method based on path planning. Master’s thesis, Graduate School of Chinese Academy of Sciences, Beijing 132. Zhu D, Sun Bing L (2015) AUV 3D autonomous path planning and safe obstacle avoidance algorithm based on biologically inspired model. Control Decis 30(5):798–806; Yan M, Zhu D, Yang SX (2013) A novel 3-D bio-inspired neural network model for the path planning of an AUV in underwater environments. Intell Autom Soft Comput 19(4):555–566 133. Yan M, Zhu D, Yang SX (2013) A novel 3-D bio-inspired neural network model for the path planning of an AUV in underwater environments. Intell Autom Soft Comput 19(4):555–566 134. LaValle SM (1998) Rapidly-exploring random trees: a new tool for path planning. Technical Report TR, 98-11 135. Jaillet L, Porta JM (2013) Path planning under kinematic constraints by rapidly exploring manifolds. IEEE Trans Rob 29(1):105–117 136. Lee K, Koo JC, Choi HR, Moon H (2015) An RRT* path planning for kinematically constrained hyper-redundant inpipe robot. In: Proceedings of IEEE international conference on ubiquitous robots and ambient intelligence, Goyang, Korea, pp 121–128 137. Webb DJ, Berg J (2013) Kinodynamic RRT*: asymptotically optimal motion planning for robots with linear dynamics. In: Proceedings of IEEE international conference on robotics and automation, Karlsruhe, Germany, pp 5054–5061 138. Tan CS, Sutton R, Chudley J (2005) Quasi-random manoeuvre based motion planning algorithm for autonomous underwater vehicles. In: Proceedings of the 16th world congress of international federation of automatic control, Praha, Czech Republic 139. Heo YJ, Chung WK (2013) RRT-based path planning with kinematic constraints of AUV in underwater structured environment. In: Proceedings of IEEE international conference on ubiquitous robots and ambient intelligence, Jeju, Korea, pp 523–525 140. Hernandez E, Carreras M, Antich J, Ridao P, Ortiz A (2011) A topologically guided path planner for an AUV using homotopy classes. In: Proceedings of IEEE international conference on robotics and automation, Shanghai, China, pp 2337–2343 141. Hernandez E, Carreras M, Ridao P (2015) A comparison of homotopic path planning algorithms for robotic applications. Robot Auton Syst 64:44–58 142. Mahmoudian N, Woolsey C (2008) Underwater glider motion control. In: Proceedings of IEEE international conference on decision and control, Cancun, Mexico, pp 552–557 143. Dubins LE (1957) On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. Am J Math 79(3):497–516 144. Zhang W, Inanc T, Ober-Blobaum S, Marsden JE (2008) Optimal trajectory generation for a glider in time-varying 2D ocean flows B-spline model. In: Proceedings of IEEE international conference on robotics and automation, Pasadena, CA, USA, pp 1083–1088 145. Zeng Z, Sammut K, He F, Lammas A (2012) Efficient path evaluation for AUVs using adaptive B-spline approximation. In: Proceedings of 2005 IEEE/MTS oceans conference and exhibition, Hampton Roads, VA, USA, pp 1–8 146. Li Y, Jiang Y, Zhang G, Li Y, Chen P (2015) AUV recovery path planning considering geometric constraints. Robot 37(4):478–485 147. Filaretov VF, Yukhimets DA, Mursalimov ES, Scherbatyuk AF, Tuphanov IE (2014) Some marine trial results of a new method for AUV trajectory motion control. In: Proceedings of 2014 IEEE/MTS oceans conference and exhibition, St. John’s, NL, pp 1–6

Chapter 2

Design of the Underwater Biomimetic Vehicle-Manipulator System

After a long period of natural evolution, fishes possess superior performance in perception, mobility, hovering, and stability. The research work on underwater biomimetic propulsion methods has been carried out for many years. Judging from the existing research results, both system analysis, design and control methods have good reference significance for the research of underwater biomimetic vehicle-manipulator system (UBVMS). The pectoral fin and undulatory fin propulsion system have the characteristics of low speed, high stability, and high maneuverability. Compared with the underwater vehicle-manipulator system (UVMS) propelled by axial propellers, undulatory propulsion pattern will provide new research ideas about solving problems such as autonomous hovering and hydrodynamic interference of underwater robots. During the operation of the UVMS, axial propellers are usually used to realize the compensation of the reaction force of the manipulator movement. However, this method is sometimes not particularly effective due to the difficulty of hovering control of the robot body [1]. Therefore, a simple and direct method to reduce the motion of the UVMS caused by the manipulator’s movement is to make the vehicle be far larger than the manipulator. This vehicle with large size can be applied in spacious environments, but is not practical in relatively narrow space. Remarkably, many companies have developed various types of underwater manipulators. In [2], the specific performance parameters of different types of underwater manipulators have been summarized. As Fernandez et al. described, these underwater manipulators are generally used for remote operation because of large mass and volume. Once the vehicle is equipped with one of these manipulators, it needs to overcome the large reaction of the manipulator. Hence, a novel mechanical structure of an underwater manipulator is designed which can reduce the coupling between the manipulator and the vehicle drastically. Furthermore, this manipulator can be applied to perform autonomous manipulation in a short time. This chapter introduces the overall design ideas of the UBVMS and gives detailed analysis from the aspects of mechanism and control system. In the mechanism, the underwater creature in undulatory propulsion mode is used as the bionic object, and © National Defense Industry Press 2023 S. Wang et al., Underwater Biomimetic Vehicle-Manipulator System, Unmanned System Technologies,https://doi.org/10.1007/978-981-99-0655-0_2

27

28

2 Design of the Underwater Biomimetic Vehicle-Manipulator System

the modular undulatory propulsion device is designed; according to the requirements of lightweight and low resistance, a new lightweight manipulator is designed, and finally through the integration of software and hardware, a new type of UBVMS is developed.

2.1 Mechanical Design of the UBVMS The cuttlefish is a shallow-water organism widely distributed along the coasts of all continents, with a roughly ellipsoidal body shape as shown in Fig. 2.1. The propulsion mode of this organism is the fluctuating fin propulsion mode, which means that the fluctuation of the long flexible fins on both sides of the body can be used to achieve underwater movement. This type of creature with fluctuating fin propulsion mode is more capable of handling complex turbulence and stable hovering and has higher underwater maneuverability and attitude stability. Therefore, the introduction of fluctuating fin propulsion mode to build a new UBVMS will provide a new idea for its stable autonomous operation underwater. The mechanical structure of UBVMS is shown in Fig. 2.2. UBVMS consists of two underwater bionic propulsor modules and a lightweight underwater manipulator module, and the specific technical parameters of the robot are shown in Table 2.1. The mechanism design of the underwater bionic propulsor and the manipulator are described below.

Fig. 2.1 Cuttlefish with fluctuating fin propulsion

2.1 Mechanical Design of the UBVMS

29

(a) Prototype

(b) 3-D model Fig. 2.2 Mechanical design of the UBVMS Table 2.1 Structure parameters of the UBVMS Parameters

Value

Parameters

Value

Length

1230 mm

Sensor

Pressure sensor, binocular vision system

Width

970 mm

Driving pattern

DC motor, digital motor

Height

390 mm

Power supply

7.4 V, 22.2 V lithium battery

Mass

52 kg

Endurance time

3h

30

2 Design of the Underwater Biomimetic Vehicle-Manipulator System

2.1.1 Mechanical Design of the Bionic Propulsor The underwater fluctuating fin bionic propulsor is shown in Fig. 2.3 and consists of a cylindrical cavity and a long fin [3]. The cavity is made of aluminum and houses the batteries, driving PCB, and servo motors. The servo motor is placed at one end of the cavity, and its output shaft is connected to a screw on which a counterweight block is mounted. The servo drives the screw and adjusts the position of the counterweight block to adjust the center of gravity of the underwater bionic propulsor for easy underwater leveling. The long fin has 12 fin rays, each equally spaced and connected together by a flexible membrane fin surface. Each fin ray is individually driven by a motor, so that the long fin itself can achieve multiple sinusoidal or more complex waveforms.

2.1.2 Mechanism Design of the Underwater Manipulator The underwater operation of UVMS has been a challenging and topical problem due to the coupling between the manipulator and the robot. References [4, 5] analyzed the effect of the mass and volume ratio between the manipulator and the robot on the position and attitude of the manipulator action to the robot body and concluded that the lower the mass and volume ratio between the manipulator and the robot, the more stable the robot itself. Typically, in conventional underwater manipulators, the motors are mounted near the drive joints and this mechanical structure results in a high weight of the manipulator. Engineering usually reduces the coupling between the robot and the manipulator by increasing the mass and volume of the robot body, which makes the robot system large and difficult to meet the demands of operating in special environments such as cramped conditions. Therefore, a lightweight multilinked 4-degree of freedom underwater manipulator was designed and the structure is illustrated in Fig. 2.4 [6]. The manipulator consists of a waterproof base, a lightweight multi-link structure, a waist joint motor, and a gripper, where the servo motor driving the waist joint is placed inside the robot float and is used to rotate the watertight base. The remaining four servo motors and the corresponding actuators are placed uniformly inside the waterproof base. The lightweight and small volume of the two manipulator rods and the gripper form a multi-link structure that serves as the manipulator. The mass of the waterproof base is therefore 11.04 kg, and the mass of the multi-link structure is 0.889 kg, so that the center of gravity of the entire manipulator will be in the waterproof base unit. The initial position of the manipulator is closely related to the establishment of the coordinate system, so the initial position is defined first to facilitate subsequent analysis. The position of the multi-link structure in a certain orientation is taken as the initial position of the wrist joint. Link 1 and link 2 are both parallel to the horizontal plane, which are taken as the initial positions of the shoulder and elbow joints, respectively. The plane formed by the gripper opening is also parallel to the horizontal

2.1 Mechanical Design of the UBVMS

31

(a) Prototype

(b) 3-D model Fig. 2.3 Schematic diagram of the bionic propulsor

plane and is used as the initial position of the waist joint. When the gripper is closed, it is used as the initial position of the gripper. Therefore, θ1 , θ2 , θ3 , and θ4 are defined as the rotation angles of the waist, shoulder, elbow, and wrist joints, respectively, and θ5 as the opening angle of the gripper. All angles are 0° when the manipulator is in the initial position. The specific details of the manipulator mechanism are described

32

2 Design of the Underwater Biomimetic Vehicle-Manipulator System

Fig. 2.4 Mechanism design of the manipulator

below, which includes the mechanism design for the shoulder, elbow, wrist, gripper, and waist joints.

2.1.2.1

Design of the Shoulder and Elbow Joint

As shown in Fig. 2.4, the shoulder joint is driven by a servo motor 2 and the two are driven by reduction gears 1. The elbow joint is driven by a servo motor 3, which is driven by a two-stage reduction gear, comprising reduction gears 2 and a timing pulley set. The servo motor 3 drives the reduction gear set, which drives the timing pulley in synchronous rotation, thereby driving the rotation of the elbow joint. It is worth noting that the passive pulley of the reduction gear set 2 is fixed to the shoulder joint, so that the rotation angle of the elbow joint is determined by both the servo motor 2 and the servo motor 3. The angle of rotation of the shoulder joint θ2 and the angle of rotation of the elbow joint θ3 can be determined by the following equation. θ2 = n 1 · θMotor2 θ3 = −n 1 · n 3 · θMotor2 + n 2 · n 3 · θMotor3

(2.1)

where θMotor2 and θMotor3 indicate the rotation angles of servo motor 2 and servo motor 3, respectively, and n1 , n2, and n3 are the reduction ratios of reduction gear set 1, reduction gear set 2, and timing belt pulley set, respectively.

2.1 Mechanical Design of the UBVMS

2.1.2.2

33

Design of the Wrist Joint and Gripper

The wrist joint is driven by a servo motor 4, as shown in Fig. 2.4, and is driven by a wire rope 1. Figure 2.5 gives the operating principle of the wrist joint and gripper from position 1 to position 2. Position 1 indicates that the wrist joint is in the initial position (θ4 = 0) and the gripper is closed. Position 2 indicates that the wrist joint is rotated to a certain angle (θ4 /= 0) and the gripper is open (θ5 /= 0). As shown in Fig. 2.5, the wire rope 1 is fixed to the draw-bar 1 and moves along a spiral trajectory line by means of a pin 1 on the draw-bar 1. The projection of the spiral trajectory in the xd yd plane is a circle of radius r. The equation of the helix trajectory in the set reference coordinate system od xd yd z d (as shown in Fig. 2.5) can be expressed as {

x 2 + y2 = r 2 , θ4 z = 2π L

(2.2)

where L is the distance traveled along the z d axis. The stick is fixed to the gripper and the wire rope 1 pulls the draw-bar 1, then the movement of the pin 1 in a straight line pushes the stick and the gripper to rotate simultaneously. At this point, the movement of the draw-bar 1 causes the spring 1 to compress. Thus, once the servo motor 4 is reversed, the spring 1 pushes the draw-bar 1 causing the gripper to rotate in reverse. The angle of rotation of the carriage joint θ4 and the angle of rotation of the servo motor 4 θMotor4 can be obtained from the following equation. θ4 =

2π R1 θMotor4 , L

(2.3)

where R1 is the radius of the wire pulley 1, where the wire pulley 1 is fixed directly to the motor shaft of the servo motor by means of a key. Similarly, the gripper is driven by a servo motor 5 and then transmitted by a wire rope 2. The wire rope 2 pulls the draw-bar 2, the pin 2 moves along the slots and the gripper opens. A spring 2 is fitted inside the gripper and the gripper closes when the servo motor 5 is rotated in the opposite direction. Figure 2.6 gives the exact working principle of the gripper from the opening to the closing action, and the solid line represents the open state of the gripper and the dashed line the closed state of the gripper. Since the mechanical parameters l1 ∼ l4 (marked in Fig. 2.6) are constant values and θg is a variable, the opening and closing action of the gripper can be achieved by the linear movement of the pin 2. Then we have (l4 − l2 cos θg − l g )2 + (l2 sin θg + l3 )2 = l12 ,

(2.4)

where l g represents the distance traveled by pin 2 and l1 ∼ l4 is the fixed mechanical length. When the gripper is closed, these four mechanical parameters l1 ∼ l4 form a right-angle trapezoid. The wire pulley 2 is fixed to the motor shaft of the servo motor 2 by means of a key (as shown in Fig. 2.4). R2 indicates the radius of the wire pulley 2, θ5 and θMotor 5 indicate the angle of opening of the gripper and the angle

34

2 Design of the Underwater Biomimetic Vehicle-Manipulator System

Fig. 2.5 Implementation of wrist and gripper from State 1 to State 2. a State 1. b Specific implementation of wrist and gripper in State 1. c State 2. d Specific implementation of wrist and gripper in State 2

2.1 Mechanical Design of the UBVMS

35

Fig. 2.6 Implementation of gripper. a Mechanical structure of gripper. b Simplified schematic diagram for the realization of the action from closed gripper to open gripper

of rotation of the servo motor 5, respectively. By bringing l g = R2 − θMotor 5 and θg = l2 θ5 into Eq. (2.4), we obtain the relationship between the two angles θ5 and θMotor 5 as follows: 1 1 λ1 · cos θ5 + λ2 · sin θ5 = λ3 2 2

(2.5)

where the parameters λ1 , λ2 , and λ3 can be calculated from the following equation. λ1 = 2l2 R2 θMotor 5 − 2l2 l4 λ2 = 2l2 l3 2 2 2 λ3 = l12 − l42 − R22 θMotor 5 − l2 − l3 + 2l4 R2 θMotor 5

(2.6)

36

2 Design of the Underwater Biomimetic Vehicle-Manipulator System

Table 2.2 Main components’ weight

Components

Weight in air (kg)

Weight in water (kg)

Waterproof base

11.04

6.65

Link 1

0.44

0.281

Link 2

0.311

0.205

Gripper

0.138

0.095

11.929

7.231

Total structure

Table 2.3 Mechanical parameters of the underwater manipulator

2.1.2.3

Parameters

Value (mm)

Parameters

Value

l1

25

R1

35 mm

l2

24

R2

35 mm

l3

15

n1

20/44

l4

44

n2

20/37

L

40

n3

32/40

Design of the Waist Joint

In order to extend the effective working space of the manipulator, the waist joint is designed to drive the waterproof base and the manipulator together in rotation. The waist joint is driven by a servo motor 1, which is positioned inside the robot body. By adjusting the position of the servo motor inside the waterproof base, the center of mass and the floating center of the manipulator fall on the axis of the waist joint. Such a setup reduces the impact on the underwater robot body caused by the rotational movement of the waist joint. In other words, the design ensures that the center of mass and floating center of the manipulator can change position to a lesser extent during the rotational movement of the waist joint. In accordance with the above mechanism design, the underwater manipulator shown in Fig. 2.4 was developed. Table 2.2 lists the gravitational and buoyancy forces of the main units of the boom, g in the table is the acceleration of gravity, and Table 2.3 lists the values of the mechanical parameters of the boom mentioned above. As can be seen from Table 2.2, the mass of the waterproof base accounts for a very large proportion of the mass of the underwater manipulator. The mass of the waterproof base is 11.04 kg, and the mass of the multi-link structure is 0.889 kg.

2.2 Control System Design of the UBVMS Figures 2.7 and 2.8, respectively, show the hardware layout and control system block diagram of the UBVMS. Because the power supply of the UBVMS is a built-in battery pack, so the trailer cable is a communication cable (network cable). The

2.2 Control System Design of the UBVMS

37

Fig. 2.7 Hardware configuration

remote console communicates with the main control unit inside the robot through Ethernet. The UBVMS is equipped with battery pack, PC104 industrial computer, vision system, servo motor, counterweight, etc. PC104 is mainly used to realize the upperlevel algorithms of motion control, motion planning, target recognition and positioning, and autonomous operation of the manipulator of the UBVMS. The servo motors of the manipulator are Maxon motors. The binocular vision system is used to identify and locate the target and is connected to the PC104 through the USB interface. The driver corresponding to the servo motor 1 is connected to the PC104 through the serial port, and the drivers corresponding to the remaining servo motors are connected by CAN bus. The servo motor system that drives the joints and grippers of manipulator includes an encoder, a Maxon DC brushed motor, and a gearbox. The detailed parameters are shown in Table 2.4. The battery pack of the robot cavity is 24 V, which is used to power the PC104 (PCM3362) and the servo motors of each joint of the manipulator, as shown in Fig. 2.9. Modularized bionic propulsors are installed on both sides of the robot cavity. Each propulsor is equipped with a battery pack, FPGA control board, and a servo motor to drive the fins. The battery pack is used to supply power to the FPGA control board and the servo motors that drive the fins. Among them, the power supply of the servo motor is 7.4 V, and the power supply of FPGA control board is 3.3 V.

38

2 Design of the Underwater Biomimetic Vehicle-Manipulator System

Fig. 2.8 Control system configuration

Table 2.4 Parameter table of servo motor for each joint of manipulator Joint

Waist joint

Shoulder joint

Elbow joint

Wrist joint

Gripper

Motor model

RE-35

RE-40

RE-35

RE-35

RE-35

Motor power (W)

90

120

90

90

90

Rated voltage (V)

24

24

24

24

24

Rated speed (rpm)

6910

6930

6910

6910

6910

Rated torque (mNm)

93.3

170

93.3

93.3

93.3

Reduction ratio

86:1

74:1

86:1

86:1

86:1

The battery voltage is mainly converted from 7.4 to 3.3 V through DC-DC chip, as shown in Fig. 2.9. FPGA control board, as the underlying driving unit of the bionic propulsor, is mainly used to receive the control commands of the PC104 industrial computer and then coordinate and control the movement of each servo motor of the bionic propulsor to realize the corresponding waveform. Among them, the command sent by the PC104 to the FPGA control board is the traveling wave parameters of undulatory fin, including wave propagation direction, wave frequency, wave amplitude, undulatory fin offset angle, and phase difference between adjacent fins. Servo motors that drive the fin are Borch CDS5516 digital servo motors, which

2.2 Control System Design of the UBVMS

39

Fig. 2.9 Power supply frame diagram

use asynchronous serial bus to communicate. In theory, up to 254 servo motors can be connected to a bus for unified control.

2.2.1 Underlying Driving System of Undulatory Fins The underlying driving system of undulatory fins is implemented by custom logic on the FPGA, and its system block diagram is shown in Fig. 2.10. The UART receiving logic circuit is responsible for receiving the undulatory fin control commands from the remote console, and the PCI receiving logic circuit is responsible for receiving the undulatory fin control commands from the upper control unit based on a custom communication protocol. After receiving the control command, the receiving logic circuit will check it first. If the check fails, the control command will be discarded. If the check succeeds, the command parsing module will be triggered to analyze the command. The command analysis module uses the finite-state machine method to analyze the undulatory fin traveling wave parameters. After the command parsing module completes the command parsing, it triggers the left fin control command generator and the right fin control command generator at the same time. The control command generators of the left and right fins have the same structure and are driven by the same 50 Hz clock source, which can ensure that the control commands of the left and right fins are generated synchronously. Take the left fin control command generator as an example. The control command generator adopts the finite-state

40

2 Design of the Underwater Biomimetic Vehicle-Manipulator System Cyclone III FPGA Custom logic Left fin Command generator

UART

Left fin UART

Left fin

Right fin UART

Right fin

Remote console Command parsing

sinusoidal wave data

RS232 PCI PC/104

Right fin Command generator

Fig. 2.10 Block diagram of undulating fin underlying driving system

machine method. According to the traveling wave parameters, the position data of the 6 servo motors is sequentially read from the sine wave waveform data table, and then the servo motor control command package is generated according to the communication protocol of digital servo motor. The control command generator is cyclically executed every 20 ms under the drive of a 50 Hz clock source. After the control command generator generates a new servo motor control command packet, it stores the servo motor control command packet into the FIFO buffer unit. As long as there is data in the FIFO buffer unit, the UART will send the servo control commands to the servo motors and control the fin to produce corresponding undulatory motion.

2.2.2 Underlying Driving Remote Update System As mentioned in the previous section, the underlying driving system of the UBVMS is realized by custom logic on the FPGA. In the process of developing the system, it is necessary to modify the debugging logic circuit repeatedly to obtain a steady system. The traditional method to update the system by using config file requires a direct connection between a dedicated emulator and the underlying driver board. The disadvantage of this update method is that the robot needs to be disassembled, which not only wastes time, reduces debugging efficiency, but also increases the probability of damage to the waterproof mechanism. Therefore, this section presents a scheme for remotely updating the underlying driving system through wireless communication [7]. As shown in Fig. 2.11, the underlying driving remote update system includes two parts: the upper computer system (program development side) and the lower computer system (underlying driving system). The upper computer system includes FPGA development software and customized underlying driver system update file sending client, which is responsible for editing, compiling, generating and sending

2.2 Control System Design of the UBVMS

41

Fig. 2.11 Block diagram of FPGA remote update system

underlying driving system update files. The hardware of lower computer system consists of the Qsys system with Nios II/f as the soft core, EPCS64 memory used to store FPGA config files and Qsys system program files, SDRAM memory used as Qsys system operating RAM. The Qsys system is a system-on-chip solution provided by Altera for its FPGAs, which can freely select or customize system peripherals for specific applications. The Qsys system we designed here is based on Nios II/f soft core, equipped with serial port, remote system update controller, SDRAM controller, EPCS Flash controller, timer, and other peripherals. The serial port is used to receive FPGA update files, the remote system update controller is used to operate and monitor the remote update circuit on the FPGA, the SDRAM controller is used to read and write SDRAM memory, and the EPCS Flash controller is used to erase, read, and write EPCS64 memory. The timing device is adopted to reset the watchdog timer regularly, and other peripherals are necessary to ensure stable and reliable operation of the Qsys system. The underlying driving system update files generated by the host computer system include FPGA custom logic config files (hardware data) and Qsys program files (software data). In the underlying driving remote update system, the update files are divided into factory image and application image, both of which contain corresponding software and hardware data. Factory image is a backup file, when the underlying driving system is powered on or detects any error, the system will load the factory image automatically to achieve basic functions. After the factory image is written into the lowest address segment of EPCS64 memory, it can no longer be changed in the process of remote update, to ensure that the underlying driving system works normally in the factory default state. The application image has the latest system config file and can be changed during remote update. As long as the EPCS64 memory is able to store application images, multiple different application images would be stored in EPCS64 at the same time, so that the underlying driving system could load different application images as needed. The remote update process of the underlying driving system includes the following four steps: (1) The program is started in the upper computer system to generate the updated file, which is the updated application image of the underlying driving system;

42

2 Design of the Underwater Biomimetic Vehicle-Manipulator System

(2) the upper computer system sends the application image to the lower computer system through wireless communication; (3) the lower computer system receives the updated application image and stores it in dedicated address unit in the EPCS64 memory; (4) the lower computer system triggers the FPGA to load the updated application image. After the underlying driving system is powered on, the FPGA first loads the factory-mirrored hardware configuration file from the 0 × 0 address of the EPCS64 memory to complete the FPGA logic circuit configuration. Then the Qsys program file is copied from the factory-mirrored hardware config file to the SDRAM, and a jump command is executed to jump to the first instruction of the Qsys program. In the Qsys program of the factory image, the hardware config file of the application image is loaded from the specified address of the EPCS64 memory as needed, and then the Qsys program of the application image is copied to SDRAM and starts to be executed, enabling the underlying driving system run with updated software and hardware configuration.

References 1. Yoerger DR, Cooke JG, Slotine JJE (1990) The influence of propulsor dynamics on underwater vehicle behavior and their incorporation into control system design. IEEE J Ocean Eng 20(3):167–178 2. Fernandez J, Prats M, Sanz PJ, Garcia JC, Marin R, Robinson M, Ribas D, Ridao P (2013) Grasping for the seabed: developing a new underwater robot arm for shallow-water intervention. IEEE Robot Autom Mag 20(4):121–130 3. Wang R, Wang S (2015) Design and implementation of a biomimetic underwater propeller with an undulating long fin. J Huazhong Univ Sci Technol (Nat Sci Edn) 43(s1):408–411 4. Zhang W, Xu H, Ding X (2015) Design and dynamic analysis of an underwater manipulator. In: Intelligent technology and systems. Springer, pp 399–409 5. Barbalata C, Dunnigan MW, Petillot Y (2014) Dynamic coupling and control issues for a lightweight underwater vehicle manipulator system. In: Proceedings of 2014 IEEE/MTS oceans conference and exhibition, St. John’s, NL, pp 1–6 6. Wang Y, Wang S, Wei Q, Tan M, Zhou C, Yu J (2016) Development of an underwater manipulator and its free-floating autonomous operation. IEEE/ASME Trans Mechatron 21(2):815–824 7. Wei Q, Wang S, Tan M (2013) An approach to remote update driver system of a robotic fish with two long fins. In: Proceedings of the 32nd Chinese control conference, pp 5705–5709

Chapter 3

Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

The underwater biomimetic robot is propelled by the undulatory motion of its left and right undulatory fins, and the motion shapes and characteristics of the undulatory fins determine the motion states of the underwater biomimetic robot. The purpose of studying the dynamic model of the biomimetic undulatory fin is to analyze the mechanism of the propulsive force generated by the undulatory motion of the undulatory fin on the left and right sides of the underwater biomimetic robot, and to establish the relationships between motion characteristics of the undulatory fin, such as wave frequency, wave amplitude, and wavelength and the propulsion force and torque generated by the undulatory fin, which provides a theoretical basis for motion control of underwater biomimetic robots. The hydrodynamic modeling of fish swimming has always been a difficult problem in the study of fish propulsion mechanism. Scholars at home and abroad have proposed a variety of theories for this problem, such as Lighthill’s “large swing amplitude slender body theory” [1], Wu’s “two-dimensional wave plate theory” [2], Blake’s “blade element theory” [3], academician Tong Binggang’s “three-dimensional wave plate theory” [4], but most of these theories focus on abstract analysis, the modeling and analysis processes are complex and the results obtained are not very intuitive. This paper adopts a simplified method, which mainly considers the interaction force between the fin surface element of the undulatory fin and the fluid. The relationship between the generated propulsion force and torque and the undulatory motion is analyzed, and the influence of the motion characteristics of the undulatory fin on the propulsion force and torque is analyzed. The establishment of the dynamic model needs to calculate the sum of the forces performing on all the fin surface elements of the entire fin surface according to the motion law of the undulatory fin, so the kinematic model of the undulatory fin needs to be established. The movement of the undulatory fin is realized by the coordinated swing of multiple fin rays to drive the fin membrane to perform undulatory motion. The characteristics of the swing movement of the fin rays directly affect the movement characteristics of the undulatory fin. Therefore, we first establish the kinematic model

© National Defense Industry Press 2023 S. Wang et al., Underwater Biomimetic Vehicle-Manipulator System, Unmanned System Technologies,https://doi.org/10.1007/978-981-99-0655-0_3

43

44

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

of undulatory fin according to the movement law of the fin rays and then analyze the influence of the motion characteristics of the fin rays on the motion of the undulatory fin.

3.1 Kinematics Modeling of the Undulatory Fin The thickness of the surface of the biomimetic undulatory fin is negligible compared with the overall dimension of the fin surface. Therefore, the motion of the undulatory fin in space can be abstracted as surface motion in space without thickness, and the swinging motion of the fin rays can be regarded as a space-dependent sampling of the motion surface. The motion law of the particle on the surface is similar to that on the fin ray, this section starts from this similarity and establishes the kinematic model of the undulatory fin through the swing motion relationship of the fin ray.

3.1.1 Kinematics Modeling The movement of the undulatory fin is realized by the coordinated swing of multiple fin rays to drive the movement of the fin membrane. The movement form of the fin rays at each moment can be regarded as the sampling of the space surface at this moment. Only considering the deformation of the undulatory fin membrane driven by the swing of the fin ray, without considering the deformation caused by the pressure of the water when the fin membrane moves in the fluid, and the motion law of any particle on the fin membrane and the particle on the fin ray are similar. According to this similarity, we deduce the motion law of the undulatory fin surface from the motion law of the undulatory fin rays to establish the kinematic model of the undulatory fin. For the fin ray whose position is sn on the baseline of fin surface, the swing angle law of its two-dimensional periodic swing in the motion plane can be expressed as: θ (t) = θm f (t),

(3.1)

where θm is the maximum swing angle amplitude, f (t) is a periodic function of amplitude normalization. In the fin ray coordinate system Or X r Y r Z r (sn ), the swing motion of the fin ray can be expressed as a motion curve in three-dimensional space, and its parameter equation is: ⎧ ⎨ xr (h, t) = 0 y (h, t) = h cos[θ (t)] , ⎩ r zr (h, t) = h sin[θ (t)]

(3.2)

3.1 Kinematics Modeling of the Undulatory Fin

45

where h represents the distance from the particle on the fin ray to the baseline of the fin surface, the value range is [0, H], H is the length of the fin ray, which is also equal to the width of the undulatory fin surface, t is the time variable. In order to perform a steady swing motion, the swing law and frequency of each fin ray should be the same, while the swing amplitude and phase of the fin ray at different positions can be different. In the fin surface coordinate system Oq X q Y q Z q , the swing amplitude and the phase of the fin ray is a function of the position sn , and the swing angle of the fin ray at the position sn can be expressed as: θ (sn , t) = θm (sn ) f (sn , t),

(3.3)

where sn is a discrete variable, and the value range is the set of fin ray base points along the Oq X q axis, t is a time variable. The relative relationship between the fin ray coordinate system and the fin surface coordinate system is shown in Fig. 3.1. The motion curve of the fin ray can be expressed as a parametric equation about (sn , h, t) in the fin surface coordinate system: ⎧ ⎨ xq (sn , h, t) = sn y (s , h, t) = h cos[θ (sn , t)] , ⎩ q n z q (sn , h, t) = h sin[θ (sn , t)]

(3.4)

where sn is a discrete variable, and its value range is the set of fin ray base points along the Oq X q axis; h is the distance from the particle on the fin ray to the fin surface baseline, t is a time variable.

Fig. 3.1 Schematic diagram of the relative relationship between the fin ray coordinate system and the fin surface coordinate system

46

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

In Eq. (3.4), sn represents a series of discrete points on the baseline of the fin surface, From the similarity of the motion law of the particle on the fin surface and the motion law of the particle on the fin ray, Eq. (3.4) is extended to the entire motion surface, let s be a variable along the Oq X q axis in the fin surface coordinate system, and then the surface equation of the fin surface motion can be expressed as a parametric equation about (s, h, t) as follows: ⎧ ⎨ x(s, h, t) = s y(s, h, t) = h cos[θ (s, t)] , ⎩ z(s, h, t) = h sin[θ (s, t)]

(3.5)

where s is the position variable along the Oq X q axis, the value range is [0, S], S is the length of the baseline of the fin surface, which is also equal to the chord length of the undulatory fin, h is the distance from the particle of the fin surface to the baseline of the fin surface, the value range is [0, H], t is the time variable.

3.1.2 Effects on the Movement of Undulating Fin In order to perform a steady swing motion, the swing frequency of each fin ray should be the same, and other motion characteristics of fin ray also affect the movement shape of the undulatory fin. The influence of the motion characteristics of the fin ray on the movement shape of the undulatory fin was studied by simulation from four aspects: the swing law of the fin ray, the swing amplitude of the fin ray, the swing phase of the fin ray, and the offset of the swing center of the fin ray.

3.1.2.1

The Swing Law of the Fin Ray

The swing law of the undulatory fin rays depends on the rotation law of the driving steering engine. When the driving steering engine rotates in different rules, the undulatory fin surface will show different movement patterns. When each driving steering engine rotates with cosine law, linear law, and bell-shaped law, respectively, the swing law of the fin rays is shown in Fig. 3.2. Under these three swing laws, the motion form simulation of the undulatory fin surface is shown in Fig. 3.3. In Fig. 3.3, the view of the left side is at the upper left of the moving fin surface, and the view of the right side is inward along the outer central axis of the moving fin surface. Among the three motion forms of the undulatory fin, only the swing law of the fin rays is different, while the swing frequency, the swing amplitude and the phase difference among the fin rays are all the same. The wavelength of the undulatory fin motion surface is taken as the half length of static fin surface, the undulatory fins exhibit characteristics of two wavelengths during movement. From the simulation results in Fig. 3.3, it can be seen that the spatial shape of the moving surface of the

3.1 Kinematics Modeling of the Undulatory Fin

(a) Swing by cosine law

(b) Swing by linear law

47

(c) Swing by bell-shaped law

Fig. 3.2 Diagram of typical swing law of fin ray

undulatory fin at a certain moment has similar characteristics with the specific shape of the fin ray within one swing period, which is in line with the movement law of the particles on the fin surface and the particles on the fin rays. The motion shapes of the undulatory fin propelled fish are closer to the surface shape in Fig. 3.3a, c, while the surface shape in Fig. 3.3b is not smooth enough to be the propulsion of the undulatory fin underwater biomimetic robot. In the following kinematics simulation and dynamic modeling, the discussion is based on the cosine law swing of the fin rays. When the fin rays swing by cosine law, the motion shape of the undulatory fin surface conforms to the shape of most fish propulsion by undulatory fins.

3.1.2.2

The Swing Amplitude of Fin Ray

When the swing amplitudes of each fin ray are different, the surface of the undulatory fin will show different motion shapes. When the swing amplitude of each fin ray takes the equal amplitude, linear increase in amplitude, and nonlinear change in amplitude, the simulation of the motion shape of the undulatory fin surface is shown in Fig. 3.4. Among the three motion shapes of the undulatory fin shown in Fig. 3.4, only the swing amplitude of the fin rays is different, while the swing frequency of the fin rays and the swing phase difference between the fin rays are the same. From the simulation results in Fig. 3.4, it can be seen that when the swing amplitude of each fin ray is the same, the motion surface of the undulatory fin is also in the form of equal amplitude, and the equal amplitude form of the undulatory fin is more common in the periodic motion of the undulatory fin propelling the fish. When the swing amplitude of the fin rays increases linearly, the motion surface of the undulatory fin also presents the form of increasing amplitude, and the amplitude increasing or decreasing of the undulatory fin is more common in the transient motion such as the rapid start and braking of the undulatory fin propelling fish. When the swing amplitude multiple fin rays have a nonlinear relationship of small on both sides and large in the middle, the amplitude of the moving surface of the undulatory fin is in a fusiform shape. The amplitude of the fusiform shape is often used to maintain the posture of the fish when the undulatory fins propel the fish to move. From the simulation results of the influence of different swing amplitudes of fin rays on the motion shape of the

48

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

(a) The fin ray swing by cosine law

(b) The fin ray swing by linear pattern

(c) The fins swing by a bell-shaped pattern Fig. 3.3 Simulation of undulatory fin motion with different swinging rules of fin ray

undulatory fin surface, it can be seen that the swing amplitude of the fin rays is an important parameter to change the motion shape of the undulatory fin.

3.1.2.3

The Swing Phase of the Fin Ray

When the swing phase of the fin rays is different, the motion shapes of the undulatory fin surface are also different. When the swing phase of each fin ray is taken as the linear function, the downward and upward convex power function, of the fin ray position,

3.1 Kinematics Modeling of the Undulatory Fin

49

(a) Swing in equal amplitude

(b) Swing in linear increased amplitude

(c) Swing in nonlinear change amplitude Fig. 3.4 Simulations of the motion shape of the undulatory fin when the fin rays swing with different amplitudes

the motion shape simulation of the undulatory fin surface is shown in Figs. 3.5 and 3.6. Among the three motion shapes of the undulatory fin shown in Fig. 3.6, only the swing phase of the fin rays is different, and the swing frequency and swing amplitude of the fin rays are the same. From the simulation results in Fig. 3.6, it can be seen that when the swing phase of each fin ray has a linear function relationship with the position of the fin ray, the motion surface of the undulatory fin shows an equalwavelength shape, and the equal-wavelength shape is the undulatory fin when the fish

50

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

(a) linear function

(b) the downward convex power function

(c) the upward convex power function

Fig. 3.5 Schematic diagram of the relationship between the swing phase of fin ray and fin ray position

swims in a steady state. When the swing phase of each fin ray has a downward convex power function relationship with the position of the fin ray, the moving surface of the undulatory fin shows a wavelength-reducing shape; that is, the wavelength of the surface wave decreases from front to back. When the swing phase of each fin ray has an upward convex power function relationship with the position of the fin ray, the moving surface of the undulatory fin presents a wavelength-increasing shape; that is, the wavelength of the surface wave increases from front to back. Some fishes will change the wavelength of the fins during accelerated swimming or decelerating swimming, so that the undulatory fins show the shape of decreasing or increasing. From the simulation results of the influence of different swing phases of the fin rays on the motion shape of the undulatory fin surface, it can be seen that the swing phase of the fin rays is also an important parameter to change the motion shape of the undulatory fin.

3.1.2.4

The Offset of the Swing Center of the Fin Ray

When the swing center of the fin ray is shifted, the motion shape of the undulatory fin surface is also different. When the swing center of each fin ray is taken as invariant, upward offset, and downward offset, the simulation of the motion shape of the undulatory fin surface is shown in Fig. 3.7. Among the three motion shapes of the undulatory fin shown in Fig. 3.7, only the swing center of the fin rays is different, while the swing frequency, swing amplitude, and swing phase of the fin rays are the same. It can be seen from the simulation results in Fig. 3.7b, c that when the swing center of the fin ray is offset, the center axis of the moving surface of the undulatory fin is also offset in the corresponding direction. The central axis offset phenomenon of the undulatory fin is common in the transient motion of some undulatory fin propelled fish, such as ups and downs, attitude maintenance, and so on. From the simulation results of the influence of the swing center offset of the fin ray on the motion shape of the undulatory fin surface, it can be seen that the swing center of the fin ray is also an important parameter to change the motion shape of the undulatory fin.

3.1 Kinematics Modeling of the Undulatory Fin

51

(a) linear function

(b) the downward convex power function

(c) the upward convex power function Fig. 3.6 Simulation of the motion shape of the undulatory fin with different swing phases of the fin rays

The above analysis is based on the change of a single motion feature of the fin ray to study the change of the motion shape of the undulatory fin. By changing several motion characteristics of the fin ray at the same time, the motion shape of the undulatory fin can produce more complex changes. The study of the relationship between the motion characteristics of the undulatory fin rays and the motion shape of the undulatory fin can lay a foundation for the research on the motion control of the underwater biomimetic robot.

52

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

(a) swing center of the fin ray is invariant

(b) swing center of the fin ray is upward offset

(c) swing center of the fin ray is downward offset Fig. 3.7 Simulation of the motion shapes of the undulatory fin with different swing center of fin rays

3.2 Dynamic Modeling of Undulatory Fins Biomimetic undulatory fins generate thrust by interacting with the fluid through undulatory motion. In this section, starting from the interaction forces between the microelements on the fin surface and the fluid during the undulatory fin motion, the force generated by the entire undulatory fin is obtained by calculating the sum of the forces on the microelements within the entire undulatory fin surface, and the dynamic model of the undulatory fin is established. In addition, the motion characteristics of

3.2 Dynamic Modeling of Undulatory Fins

53

the undulatory fin, such as wave frequency, wave amplitude, and wavelength are analyzed to influence the thrust and moment generated by the undulatory fin.

3.2.1 Basal Principle The surface of the undulatory fin can be considered to consist of multiple fin surface microelements, and when the undulatory fin moves in the fluid, the interaction force between any of the fin surface microelements on the undulatory fin and the fluid can be expressed as [5]: ⎧ ⇀ ⎪ ⎪ ⎪ ⎨f =

dF

⎪ ⎪ ⎪ ⎩f =

dF

n ⇀ τ

⇀

⇀

n

τ

⇀ n

dS ⇀ τ

dS

∥ ∥ ∥ ⇀∥ ⇀ ∥V = − 21 ρCn ∥ ∥V n ∥ n ∥ ∥ , ∥ ⇀∥ ⇀ ∥ = − 21 ρCτ ∥ ∥V ∥ V τ

(3.6)

τ

where d F and d F are the instantaneous normal force and the instantaneous tangential force on the fin surface microelement, dS is the area of the microelement, ρ is the fluid density, Cn and Cτ are the tangential resistance coefficient and the viscous ⇀

⇀

resistance coefficient of the microelement, V and V are the instantaneous normal n τ velocity and the instantaneous tangential velocity of the microelement relative to the fluid. Therefore, the instantaneous normal force and instantaneous tangential force on the undulatory fin can be obtained by integrating the force on the fin surface microelement over the entire fin surface: ⎧⇀ ˜ ⇀ ⎪ f dS ⎪ = ⎨F n Ω(t) n (3.7) ˜ ⇀ , ⇀ ⎪ ⎪ f dS ⎩F = τ

⇀

⇀

n

τ

Ω(t) τ

where F and F are the instantaneous normal force and the instantaneous tangential force on the undulatory fin, and Ω(t) is the spatial motion surface of the undulatory fin at time t. Since the Reynolds number Re = U L/v (where U is the velocity of the fin relative to the fluid, L is the scale of the fin, and v is the viscous coefficient of the fluid) of the undulatory fin moving in the fluid is above the order of 105 , the fluid can be regarded as an inviscid fluid, so the effect of the viscous resistance of the fluid is ignored in the force analysis; i.e., the tangential force of the undulatory fin is neglected in the analysis.

54

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

3.2.2 Dynamic Modeling Taking the analysis of the stable state motion of the underwater biomimetic robot as the starting point, in the process of deriving the dynamic model of the undulatory fin, the following two assumptions are made: (1) The underwater biomimetic robot moves linearly along the Oe X e axis in the inertial coordinate system Oe X e Y e Z e , with no displacement in the Oe Y e direction and Oe Z e direction, and no rotational motion. (2) The undulatory fins on the left and right sides of the underwater biomimetic robot undulate with equal amplitude and wavelength of the cosine law, and the central axis of the undulatory motion does not shift, and the traveling waves on the undulatory fins are characterized by the whole wavelength. ⇀

In the fin surface coordinate system Oq X q Y q Z q , any point P on the fin surface can be regarded as a two-dimensional periodic swing around the baseline of the fin surface with a cosine law, and its swing angle law can be expressed as: ) ( 2π s + φ0 , θ (s, t) = θm cos 2π f t − λ

(3.8)

where s is the position variable along the Oq X q axis, θm is the maximum swing angle ⇀

amplitude of the point P , f is the swing frequency of the point, λ is the wavelength ⇀ of the traveling wave of the undulatory fin, φ0 is the initial phase of the point P , and t is the time variable. ⇀ The parametric equation at point P is: ⎡

⎤ ⎡ ⎤ X q (s, l, t) s P (s, h, t) = ⎣ Yq (s, l, t) ⎦ = ⎣ h cos θ ⎦, q Z q (s, l, t) h sin θ ⇀

(3.9)

⇀

where θ represents θ (s, t), and h is the distance from point P to the baseline of the fin surface. ⇀ Therefore, the speed of point P in the fin surface coordinate system can be obtained ⇀

by the differentiation of the parametric equation of point P to time t: ⇀

⇀

V (s, h, t) =

qp

where

∂ P (s, h, t) q

∂t

⎡

⎤ 0 = ⎣ −hθt, sin θ ⎦, hθt, cos θ

(3.10)

3.2 Dynamic Modeling of Undulatory Fins

θt, =

55

) ( ∂θ (s, t) 2π = −2π f θm sin 2π f t − s + φ0 ∂t λ

(3.11)

Suppose the velocity of the origin of the fin surface coordinate system in the inertial ⇀

coordinate system is V = (U X , UY , U Z )T , and the angular velocity of rotation around eq

⇀

the origin of the inertial coordinate system is ω = (ω X , ωY , ω Z )T , then the velocity eq

⇀

⇀

of the point P in the inertial coordinate system is V : ep

⇀

⇀

⇀

⇀

⇀

ep

eq

eq

qp

qp

V = V +ω × r +V,

(3.12)

⇀

⇀

where r represents the position vector of the point P in the fin surface coordinate qp system. From the assumption (1), the underwater biomimetic robot moves in a straight line along the X e axis of the inertial coordinate system, and there is no rotational movement around the origin of the inertial coordinate system, then the origin of the fin surface coordinate system only has the speed in the X e direction, which is set ⇀

⇀

eq

eq

as the function U X of time t. Therefore, V = (U X , 0, 0)T , ω = (0, 0, 0)T , and the ⇀

⇀

velocity V of the point P in the inertial coordinate system can be obtained at this ep

time: ⎡

⎤ UX V = V + V = ⎣ −hθt, sin θ ⎦. ep eq qp hθt, cos θ ⇀

⇀

⇀

(3.13)

⇀

The instantaneous normal velocity of the fin surface microelement at the point P relative to the fluid is obtained by calculating the surface normal vector at the point ⇀

P in the inertial coordinate system. In the inertial coordinate system, the parametric equation of the fin surface can be expressed as: ⇀

⇀

⇀

e

q

eq

P (s, h, t) = P + r ,

(3.14)

⇀

where r is the position vector of the origin of the fin surface coordinate system in eq

the inertial coordinate system. From the linear motion of the underwater biomimetic ⇀ robot along the X e axis of the inertial coordinate system, it is known that r has eq

only the component r x in the X e direction, and r x is a function of time t, therefore,

56

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

⇀

⇀

eq

eq

r = (r x , 0, 0)T . From the independence of r with s, h, it follows that the normal ⇀

vector of the fin surface microelement at the point P in the inertial coordinate system is equal to the normal vector in the fin surface coordinate system:

n =

∂ P (s, h, t) e

∂s

| ⇀ | | i | = | ∂ Xqp | ∂ ∂s | Xqp ∂h

⇀

j

∂Yq p ∂s ∂Yq p ∂h

⇀

⇀

⇀

⇀

∂ P (s, h, t) e

×

∂h ⎡

=

q

| ⎤ | −hθs, k || ∂ Z q p | = ⎣ − sin θ ⎦, ∂s | ∂ Zqp | cos θ ⇀

⇀

∂ P (s, h, t) ∂s

∂ P (s, h, t) ×

q

∂h (3.15)

∂h

where θs,

) ( 2π 2π ∂θ (s, t) = θm sin 2π f t − s + φ0 . = ∂s λ λ ⇀

(3.16)

⇀

The unit normal vector n can be obtained from n : 0

⎡

⎤ −hθs, ⎣ − sin θ ⎦ n = ∥⇀∥ = √ ∥ ∥ , )2 + 1 0 (hθ n s ∥ ∥ cos θ ⇀

n

⇀

1

(3.17)

Therefore, the instantaneous normal velocity of the fin surface microelement at ⇀

⇀

the point P relative to the fluid is V : pn

⇀

⇀ ⇀ ⇀

pn

ep

V = (V · n ) n = 0

0

−U X hθs, + hθt, (−hθs, , − sin θ, cos θ )T (hθs, )2 + 1

(3.18)

⇀

Substituting V into Eq. (3.7), we can get: pn

⇀

¨

F= n

Ω

⇀

1 f dS = − ρCn 2 n

¨ ∥ ∥ ∥ ⇀∥ ⇀ ∥ V ∥ V dS, ∥pn ∥ pn

(3.19)

Ω

where Ω is the integral domain formed by the spatial motion surface of the undulatory fin. Convert the first type surface integral of Eq. (3.19) into a double integral, and from θt, = −λ f θs, , the following formula can be obtained:

3.2 Dynamic Modeling of Undulatory Fins ⇀

1 F = − ρCn n 2

57

¨ ∥ ∥ ¨ ∥ ⇀∥ ⇀ ∥ V ∥ V dS = − 1 ρCn ∥pn ∥ pn 2 Ω

1 = − ρCn (U X + λ f )2 2

¨

| ,| , |hθ |hθ s

D

∥ ∥ ∥ ∥ ∥ ⇀∥ ⇀∥⇀∥ ∥ V ∥ V ∥ n ∥dsdh ∥pn ∥ pn

D

s

(hθs, )2 + 1

(−hθs, , − sin θ, cos θ )T dsdh,

(3.20)

where D = {(s, h)|0 ≤ s ≤ S, 0 ≤ h ≤ H }. For Eq. (3.20), calculate the integral results of the components in the X, Y, and Z ⇀

directions of F , respectively. n For the component Fn X in the X direction: Fn X

1 = ρCn (U X + λ f )2 2

¨ || , || , 2 hθ (hθ ) s

s

(hθs, )2 + 1

D

dsdh,

(3.21)

where θs, , as shown in Eq. (3.16),| is| a function with period λ about s. By the trigonometric properties, (θs, )2 and |θs, | are functions with period λ2 about s. Let the integrand in Eq. (3.21) be: g(s, h, t) =

| ,| , 2 |hθ |(hθ ) s

s

(hθs, )2 + 1

.

(3.22)

Therefore, g(s, h, t) is also a function with period λ2 about s. For any periodic function f (x) defined in the interval (−∞, +∞) with a period of T, the following properties: (1) The definite integral value of a periodic function is equal in any interval of length T, i.e., ∀a ∈ R; there is: {T

a+T {

f (x)dx = 0

f (x)dx.

(3.23)

a

(2) The definite integral value of a periodic function in an integer number of cycles is equal to an integral multiple of the definite integral value in a single cycle, i.e., ∀n ∈ N ; there is: {nT

{T f (x)dx = n

0

f (x)dx.

(3.24)

0

From the whole wavelength characteristic of the traveling wave of the undulatory fin, which is the chord length of the undulatory fin S = nλ, the integral term in Eq. (3.21) can be simplified as follows:

58

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

¨

{H {S g(s, h, t)dsdh =

g(s, h, t)dsdh = 0

D

{H

0

⎡ ⎢ 2n ⎣

0

⎤

λ

{2

⎥ g(s, h, t)ds ⎦dh

(3.25)

0

According to the property (1) of periodic function, for any time t, the integral {λ values of 02 g(s, h, t)ds are equal. Therefore, t 0 satisfying the following equation can be obtained: 2π f t + φ0 = 2kπ, k ∈ Z

(3.26)

then: θs, (s, t0 )

) ( 2π 2π , s = θs0 = − θm sin . λ λ

, Substitute θs0 into Eq. (3.25):

¨

{H g(s, h, t0 )dsdh = 0

D

{H = 0

⎡ ⎢ 2n ⎣

⎤

λ

{2

⎥ g(s, h, t0 )ds ⎦dh

0

⎤ λ { 2 || , || , 2 hθs0 (hθs0 ) ⎥ ⎢ ds ⎦dh 2n ⎣ , 2 (hθs0 ) +1 ⎡

0

{H =− {H 0

⎢ 2n ⎣

⎤

λ

{2 0

0

=−

⎡

, 3 (hθs0 ) , 2 (hθs0 ) +

⎫ ⎧ λ ⎪ ] ⎪ ⎬ ⎨{ 2 [ , θs0 , ds dh θs0 − 2nh , ⎪ ⎪ (hθs0 )2 + 1 ⎭ ⎩ 0

{H [ =

1

⎥ ds ⎦dh

4nθm h +

√

nλ2

π 4π 2 θm2 h 2 + λ2 |] | √ | 4π 2 θ 2 h 2 + λ2 − 2π θ h | m | | m ln | √ | dh | 4π 2 θm2 h 2 + λ2 + 2π θm h | 0

nλ2 ln2 λ 2π 2 θm (/ ) 2 2 2 2 2 ln 4π θm H + λ − 2π θm H

= 2nθm H 2 + −

nλ2 4π 2 θm

(3.27)

3.2 Dynamic Modeling of Undulatory Fins

59

(/ ) nλ2 2 2 2 2 2 4π θm H + λ + 2π θm H . − ln 4π 2 θm0

(3.28)

From the result of Eq. (3.28), we can obtain: Fn X

1 = ρCn (U X + λ f )2 2

¨ || , || , 2 hθ (hθ ) s

D

s

(hθs, )2 + 1

dsdh

[ 1 nλ2 nλ2 = ρCn (U X + λ f )2 2nθm H 2 + ln2 λ − ln2 2 2 2π θm 4π 2 θm (/ ) nλ2 2 2 2 2 4π θm H + λ − 2π θm H − 4π 2 θm )] (/ 4π 2 θm2 H 2 + λ2 + 2π θm H ln2

(3.29)

For the component FnY in the Y direction and the component Fn Z in the Z direction ⇀

of F , the integral value can be proved to be zero by the properties of trigonometric n functions as follows: ¨ || , || , hθs hθs sin θ 1 2 FnY = ρCn (U X + λ f ) dsdh. (3.30) 2 (hθs, )2 + 1 D

In Eq. (3.30), θs, and sin θ are functions with period λ about s. Let the integrand in Eq. (3.30) be: ζ (s, h, t) =

| ,| , |hθ |hθ sin θ s

s

(hθs, )2 + 1

(3.31)

Therefore, ζ (s, h, t) is also a function with period λ about s. According to the properties of periodic function and S = nλ, the integral term in Eq. (3.30) can be simplified as follows: ¨ D

{H {λ ζ (s, h, t)dsdh = n ζ (s, h, t)dsdh. 0

(3.32)

0

According to the properties of periodic function, for any time t, the integral values {λ of 0 ζ (s, h, t)ds are equal. Therefore, the t 0 satisfying Eq. (3.26) can be taken into the integral equation by substituting, then: (

2π s θ (s, t0 ) = θm cos λ

) = θs0

(3.33)

60

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

) ( 2π 2π , s = θs0 θs (s, t0 ) = − θm sin λ λ ,

{λ ζ (s, h, t0 )ds = 0

{λ || , || , hθs0 hθs0 sin θs0 ,

(hθs0 )2 + 1

0

ds

λ

{2

, 2 (hθs0 ) sin θs0 ds + , 2 (hθs0 ) + 1

=− 0

(3.34)

{λ λ 2

, 2 (hθs0 ) sin θs0 ds. , 2 (hθs0 ) + 1

(3.35)

Substitute u = λ − s into the second term on the right side of Eq. (3.35), we get: {λ λ 2

, 2 (hθs0 ) sin θs0 ds = − , 2 (hθs0 ) +1

{0 λ 2

λ

, 2 (hθu0 ) sin θu0 du = , 2 (hθu0 ) +1

{2

, 2 (hθs0 ) sin θs0 ds , 2 (hθs0 ) +1

(3.36)

, 2 (hθs0 ) sin θs0 ds = 0. , 2 (hθs0 ) + 1

(3.37)

0

Therefore. λ

λ

{λ

{2 ζ (s, h, t0 )ds = −

0

0

, 2 (hθs0 ) sin θs0 ds + , 2 (hθs0 ) + 1

{2 0

Namely FnY = 0

(3.38)

The same can be proved: Fn Z = 0. From the above derivation process, it is clear that the thrust generated by the undulatory motion of the undulatory fins in the stable state motion state of the underwater biomimetic robot is along the direction of motion, and there is no other directional component of the thrust, and the directionality of the thrust is the reason why the underwater biomimetic robot can maintain a straight line motion. From Eq. (3.29), it can be seen that the thrust generated by the undulatory fins is related to the wave frequency, wave amplitude, wavelength, and wave number of the undulatory fins, etc. The following is a specific analysis of the effect of the motion characteristics of the undulatory fins on the thrust. The chord length S and length H of the undulatory fin are determined after the design of the underwater biomimetic robot is completed, and the chord length S is equal to the product of the wavelength and the wave number on the undulatory fin, so the wavelength and the wave number are related, and only the relationship between wave number and thrust is discussed in this chapter. The tangential resistance coefficient Cn in Eq. (3.29) is related to the properties of the undulatory fin membrane material and the fluid. In this chapter, Cn is taken as 10, and the fluid density ρ is

3.2 Dynamic Modeling of Undulatory Fins

61

taken as 1.00. The movement speed U X of the underwater biomimetic robot is a function of time t, which is related to the thrust generated by the undulatory fins on the left and right sides of the underwater biomimetic robot and the resistance of the underwater biomimetic robot body moving in the fluid, in order to avoid the complexity of the analysis, which is taken as 0 when the thrust of the undulatory fin is studied alone. When discussing the relationship between wave frequency, wave amplitude and wave number and thrust, the constants in Eq. (3.29) are shown in Table 3.1. The relationship between the thrust Fn X of the undulatory fin and the wave frequency f is shown in Fig. 3.8. The wave frequency of the undulatory fin gradually increases from 0 to 5 Hz. It can be seen from Fig. 3.8 that the value of the thrust Fn X increases nonlinearly with the increase of wave frequency f , and its value is always greater than or equal to 0. The relationship between the thrust Fn X of the undulatory fin and the wave amplitude θm is shown in Fig. 3.9. The wave frequency of the undulatory fin gradually Table 3.1 Constants taken for the analysis of the undulatory fin thrust equation Variable

Constant H

λ

UX

Cn

ρ

f

θm

n

f

0.12

0.50

0

10

1.00

–

0.2π

1

θm

0.12

0.50

0

10

1.00

2

–

1

n

0.12

–

0

10

1.00

2

0.2π

–

Fig. 3.8 Variation of thrust with wave frequency

62

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

Fig. 3.9 Variation of thrust with wave amplitude

increases from 0 to π/2. It can be seen from Fig. 3.9 that the value of the thrust Fn X also increases nonlinearly with the increase of the wave amplitude θm , and its value is always greater than or equal to 0. The product of the wave number n on the undulatory fin and the wavelength λ of the undulatory fin’s traveling wave is a constant, and the wave number n is taken as an integer. When the wave number changes, the wavelength of the undulatory fin’s traveling wave also changes. When the wave number is 1, 2, 3, 4, and 5, respectively, the corresponding value of the thrust of the undulatory fin is drawn in Fig. 3.10. It can be seen from Fig. 3.10 that when the wave number increases from 1 to 2, the thrust of the undulatory fin increases, and when the wave number increases again, the thrust of the undulatory fin shows a decreasing trend. Limited by the number of undulatory fin rays of the underwater biomimetic robot, the number of traveling waves on the undulatory fin cannot exceed two at most. In the above analysis, the thrust generated by the undulatory fins is all positive, i.e., in the inertial coordinate system Oe X e Y e Z e , the thrust is along the positive direction of the Oe X e axis. While by Eq. (3.8), the swing phase of the microelement of the undulatory fins lags from the front of the undulatory fins to the back in order; i.e., the propagation direction of the traveling wave of the undulatory fins is along the negative direction of the Oe X e axis, and changing the order of the swing phase of the microelement in Eq. (3.8); the propagation direction of the traveling wave is along the positive direction of the Oe X e axis, then it can be deduced that the propulsive force generated by the undulatory fin is all negative; i.e., the propulsive force is along the negative direction of the Oe X e axis; therefore, the direction of the thrust generated by the undulatory fin is opposite to the propagation direction of the traveling wave of the undulatory fin.

References

63

Fig. 3.10 Variation of thrust with wave number

From the above analysis, it can be seen that the direction of the thrust generated by the undulatory motion of the undulatory fin can be adjusted by the propagation direction of the traveling wave of the undulatory fin, and the magnitude of the thrust can be adjusted by the wave frequency, wave amplitude or the wave number of the undulatory fin. The magnitude and direction of the thrust generated by the undulatory fin can be controlled by adjusting these motion characteristics of the undulatory fin.

References 1. Lighthill J (1971) Large-amplitude elongated-body theory of fish locomotion. Proc R Soc Lond B179:125–138 2. Wu TY (1961) Swimming of a waving plate. J Fluid Mech 10:321–344 3. Blake R (1978) On balistiform locomotion. J Marine Biol 58:73–80 4. Cheng J, Zhuang L, Tong B (1991) Analysis of swimming three-dimentional waving plates. J Fluid Mech 232:341–355 5. Kobayashi S, Kameyama T, Morikawa H (2004) Generation of movement for multi-link prolusion mechanism in fluid. In: Proceedings of the fourteenth international offshore and polar engineering conference, Toulon, France, pp 290–294 6. Wei Q, Wang S, Dong X et al (2013) Design and kinetic analysis of a biomimetic underwater vehicle with two undulating long-fins. Acta Automatica Sinica 39(8):1330–1338

64

3 Kinematics and Dynamics Modeling of the Biomimetic Undulatory Fin

7. Dong X (2009) Research on the design and control of a biomimetic robot fish propelled by undulatory fins. Doctoral Dissertation, Institute of Automation, Chinese Academy of Sciences, Beijing 8. Wang R, Wang S, Wang Y (2016) A hybrid heading control scheme for a biomimetic underwater vehicle. In: International ocean and polar engineering conference, pp 619–625 9. Wang R (2018) Research on the locomotion control for an UVMS propelled by undulatory fins. Doctoral Dissertation, University of Chinese Academy of Sciences, Beijing

Chapter 4

Motion Control of the Underwater Biomimetic Robot

In this chapter, the basic motion control methods of underwater bio-inspired robot are mainly introduced. First, the adjustable parameters for controlling the motion of undulatory fin are determined based on kinematic model of the bio-inspired robot. Second, the control strategies of various three-dimensional motion modes are designed by coordinating the motion of the undulatory fins on both sides of the bioinspired robot. The three basic motions, forward/backward swimming, diving/rising motion, and turning, are implemented. Next, a control based on active disturbance rejection control (ADRC) is presented to achieve closed-loop depth and course control based on the system model of the bio-inspired robot. Then, in order to solve the problem that the control signal given by ADRC is inconsistent with the actual parameters of the propagating wave, two parameter mapping models based on fuzzy strategy are designed, which can map the control signals given by depth control algorithm and course control algorithm into the propagating wave parameters of each undulatory fin. Finally, experiments verify the effectiveness and feasibility of the depth control method and course control method proposed in this chapter.

4.1 Three-Dimensional Motion Control By coordinating the motion of the undulatory fins on both sides of the bio-inspired robot, different thrust force and moment can be generated, which can drive the underwater bio-inspired robot to realize a variety of motion modes, including forward/backward swimming, lateral swimming, diving/rising motion, turning, and other swimming modes. Forward swimming mode is the most commonly used propulsion mode of underwater bio-inspired robot. When the propagating waves generated by the left and right undulatory fins are transmitted from the head to the tail, and the offset angles of the undulatory fins are zero, the thrust FN perpendicular to the baseline generated by the left and right undulatory fins offset each other, and the thrust FP parallel to the © National Defense Industry Press 2023 S. Wang et al., Underwater Biomimetic Vehicle-Manipulator System, Unmanned System Technologies,https://doi.org/10.1007/978-981-99-0655-0_4

65

66

4 Motion Control of the Underwater Biomimetic Robot

baseline will push the bio-inspired robot forward. Therefore, the forward swimming mode is realized. Based on formula (3.8) in Chap. 3, the wave equation of undulatory fin can be described as θ (x, t) = θm cos(2π f t + kx + φ0 ) + θ B ,

(4.1)

where t represents time, f represents the wave frequency, θm represents the wave amplitude, k = ±2π/λ represents the wave number of the wave angle, λ represents the wave length of the generated wave, x represents the coordinate on the X axis, φ0 represents the initial phase of the wave, θ B is the deflection angle of the undulatory fin (the angle between fin rays and horizontal plane), and θ (x, t) is the angle between the fin ray at position x (the intersection line between the plane perpendicular to the baseline and the fin surface at position x) and horizontal plane. Note that the symbol of k determines the propagating direction of the wave. A plus sign indicates that the generated wave passes from the positive direction to the negative direction along the X axis, while the minus sign generates a wave in the opposite direction along the long fin. Because the underwater bio-inspired robot has forward and backward symmetry, and the initial phase only affects the initial state of the propagating wave, the following discussion is based on k = −2π/λ and φ0 = 0. The wave motion of the undulatory fins is considered to be generated by the coordinated swing of 12 fins, namely the swing motion of 12 fins should be the sampling of the desired propagating wave at each fin. Therefore, the swing motion of a single fin can be defined by   2π xi + θ B , (i = 1, 2, ..., 12), θ (si , t) = θm cos 2π f t − λ

(4.2)

where f represents the swing frequency of the fin rays, θm represents the swing amplitude of the fin rays, xi represents the position of the ith fin ray along the base line, and the first fin ray is set at the origin. Assuming that the spacing between adjacent fin rays is h, and the wave number on the undulatory fin is N , then xi = (i − 1)h, λ = 11h/N . Therefore, the swing equation of each fin can be described as   2 θ (i, t) = θm cos 2π f t − (i − 1)N π + θ B , (i = 1, 2, ..., 12). 11

(4.3)

Note that, when the wave number is zero (N = 0), each fin of the undulatory fin swings synchronously. The undulatory fin will no longer generate propagating waves and will flap. In formula (4.3), φi = 2(i − 1)N π/11, i = 1, 2, ..., 12 represents the swing phase difference of each fin. When actually controlling the motion of the undulatory fin, the phase difference φi is used as the control variable instead of the wave number N . Therefore, the adjustable parameters of undulatory fin include swing frequency, swing amplitude, swing phase difference of fin ray, undulatory fin offset angle, and propagating wave propagating direction.

4.1 Three-Dimensional Motion Control

67

In the forward swimming mode, the swinging motion equations of each fin of the left and right undulatory fins are, respectively, described as follows:  2 θ L (i, t) = θm cos 2π f t − (i − 1)N π , (i = 1, 2, ..., 12) 11   2 θ R ( j, t) = θm cos 2π f t − ( j − 1)N π , ( j = 1, 2, ..., 12). 11 

(4.4)

For real fish in nature, backward swimming is a transient swimming mode. However, the backward swimming of the underwater bio-inspired robot is easy to realize, because of its forward and backward symmetry. Thus, the backward swimming mode becomes a steady-state swimming mode. Compared with the forward swimming mode, the backward swimming mode of the underwater bio-inspired robot can be realized by changing the propagating direction of the propagating wave generated by the undulatory fins on both sides from the tail to the head. Based on formula (4.3), in the backward swimming mode, the swinging motion equations of each fin ray of the left and right undulatory fins are, respectively, described as follows:   2 θ L (i, t) = θm cos 2π f t + (i − 1)N π , (i = 1, 2, ..., 12) 11   2 θ R ( j, t) = θm cos 2π f t + ( j − 1)N π , ( j = 1, 2, ..., 12). 11

(4.5)

It is difficult for fish in nature to swim laterally, but it is an easy steady-state swimming mode for underwater bio-inspired robot. In the autonomous operation of underwater biomimetic vehicle-manipulator system, lateral swimming plays an important role in quickly adjusting its horizontal position. When the right undulatory fin of the underwater bio-inspired robot remains stationary and the wave number of the left undulatory fin is adjusted to zero for flapping motion, the left undulatory fin will produce thrust FN perpendicular to the baseline, which can push the underwater bio-inspired robot to swim laterally to the right. On the contrary, if the left undulatory fin is kept stationary and the right undulatory fin is controlled to flap, the underwater bio-inspired robot will swim laterally to the left. Based on formula (4.3), when the underwater bio-inspired robot swims laterally to the right, the swing equations of each fin ray of the left and right undulatory fins are, respectively, described as follows: θ L (i, t) = θm cos(2π f t), (i = 1, 2, ..., 12) θ R ( j, t) = 0, ( j = 1, 2, ..., 12).

(4.6)

Similarly, when the underwater bio-inspired robot swims laterally to the left, the swing equations of each fin ray of the left and right undulatory fins are, respectively, described as follows: θ L (i, t) = 0, (i = 1, 2, ..., 12)

68

4 Motion Control of the Underwater Biomimetic Robot

θ R ( j, t) = θm cos(2π f t), ( j = 1, 2, ..., 12)

(4.7)

Crash diving motion is of great significance to improve the three-dimensional maneuverability and hovering control ability of underwater bio-inspired robot. Since the left and right undulatory fins of the underwater bio-inspired robot are driven separately, the offset angles of the left and right undulatory fins can be easily adjusted. When the offset angles of the left and right undulatory fins are the same negative angle and the left and right undulatory fins flap at the same time, both the left and right undulatory fins will only produce thrust FN perpendicular to the baseline. The joint force will push the underwater bio-inspired robot to dive. Based on formula (4.3), when the underwater bio-inspired robot dives, the swing equations of each fin ray of the left and right undulatory fins are, respectively, described as follows: θ L (i, t) = θm cos(2π f t) + θ B , (i = 1, 2, ..., 12, θ B < 0) θ R ( j, t) = θm cos(2π f t) + θ B , ( j = 1, 2, ..., 12, θ B < 0).

(4.8)

Similar to the dive swimming mode, when the offset angles of the left and right undulatory fins are the same positive angle and the left and right undulatory fins flap at the same time, both the left and right undulatory fins will produce thrust to push the underwater bio-inspired robot to rise. The swing equations of each fin ray of the left and right undulatory fins are, respectively, described as follows: θ L (i, t) = θm cos(2π f t) + θ B , (i = 1, 2, ..., 12, θ B > 0) θ R ( j, t) = θm cos(2π f t) + θ B , ( j = 1, 2, ..., 12, θ B > 0).

(4.9)

Turning swimming mode is of great significance for the navigation, which is easy to achieve for the underwater bio-inspired robot. Based on the forward/backward swimming, a turning maneuver is achieved when the thrust forces generated by both side fins are different. The thrust difference depends on the wave amplitude and/or wave frequency and/or phase difference of both side fins. Then, the course of the underwater bio-inspired robot can be changed while swimming forward or backward. Another method to turn quickly can be implemented as well, where the two waves produced by the two side fins, respectively, propagate in different directions. In order to verify the effectiveness of the multi-modal swimming control method of underwater bio-inspired robot, multi-modal swimming experiments were carried out for various swimming modes of the underwater biomimetic vehicle-manipulator system in an indoor swimming pool. Its dimensions (L × W × H) are 5 × 4 × 1.1 m. Figure 4.1a shows the snapshot sequence of the forward swimming experiments of the underwater biomimetic vehicle-manipulator system. Figure 4.1b describes the snapshot sequence of the turning swimming experiments of the underwater biomimetic vehicle-manipulator system. Figure 4.1b describes the snapshot sequence of the diving swimming experiments of the underwater biomimetic vehicle-manipulator system. The experimental results indicate that the multi-modal swimming control method proposed in this section is effective.

4.2 Depth Control and Yaw Angle Control

69

Fig. 4.1 Snapshot sequence of the multi-modal swimming experiments

4.2 Depth Control and Yaw Angle Control 4.2.1 System Modeling The system model of the underwater bio-inspired robot cannot only provide a reference object for the design of intelligent control algorithm, but also provide a fast verification platform for the verification of intelligent control algorithm. An intelligent control algorithm often needs many parameter debugging from design to practical application in underwater robot. However, the parameter debugging directly on the underwater robot is usually subject to many limitations, such as the limited endurance of the underwater robot, the damage of the underwater robot caused by wrong controller parameters, etc., while the above limitations do not exist in the numerical simulation on the mathematical model, and the parameter debugging through simulation has higher efficiency. Thus, the optimized controller parameters can be obtained faster. Moreover, many parameter debugging experience can be accumulated in the simulation, which can guide the parameter debugging of the actual control system, so as to improve the parameter debugging efficiency of the actual control system and improve the safety of the actual system debugging. Therefore, this section first establishes the system model of the underwater bio-inspired robot based on the existing research results.

70

4 Motion Control of the Underwater Biomimetic Robot

In order to analyze and describe the position and attitude of the underwater bio-inspired robot, it is necessary to establish the corresponding coordinate system and symbol rules. This section adopts the system recommended in the technical and research bulletin of the Society of Naval Architects and Marine Engineers [1]. Figure 4.2 shows the established world coordinate system (fixed coordinate system) O E − X E Y E Z E and body coordinate system (moving coordinate system) O B − X B Y B Z B . Both of them follow the right-hand rule. The origin of the world coordinate system can be selected arbitrarily. However, for the convenience of discussion, this paper specifies that the origin of the world coordinate system is selected as the upper left vertex of the water surface of the experimental pool. The X E -axis and Y E -axis remain horizontal, and the Z E -axis is perpendicular to the water surface and points to the center of the earth. The origin of the body coordinate system is selected as the center of gravity of the underwater bio-inspired robot. The X B -axis points to the head of the underwater bio-inspired robot. The Y B -axis points to the right side of the underwater bio-inspired robot. The Z B -axis points to the bottom of the underwater bio-inspired robot. The position of the underwater bio-inspired robot is described by the coordinates η1 = [x, y, z]T in the world coordinate system. The attitude is described by the Euler angle η2 = [φ, θ, ψ]T of the body coordinate system relative to the world coordinate system. Among them, the course angle ψ is defined as the included angle between the projection of the X B -axis of the body coordinate system on the X E Y E plane of the world coordinate system and the X E -axis. The pitch angle θ is defined as the angle between the X B axis of the body coordinate system and the X E Y E plane of the world coordinate system. The roll angle φ is defined as the angle between the X B Z B plane of the body coordinate system and the plane passing through the X B -axis and parallel to the Z E axis. The linear velocities of the underwater bio-inspired robot along the X E -axis,

Fig. 4.2 World coordinate system and the body coordinate system

4.2 Depth Control and Yaw Angle Control

71

Y E -axis, and Z E -axis in the world coordinate system are, respectively, expressed ˙ y˙ , z˙ ]T . In the body coordinate system, the linear velocities along the as η˙ 1 = [x, X B -axis, Y B -axis, and Z B -axis are, respectively, expressed as ν1 = [u, v, w]T . In the body coordinate system, the angular velocities of rotation around the X B -axis, Y B -axis, and Z B -axis are, respectively, expressed as ν2 = [ p, q, r ]T . Considering that the underwater bio-inspired robot has good static stability, it ◦ produces very small pitch angle and roll angle in the process of motion (θ < 5 , φ < ◦ 5 ). Therefore, in the discussion of this section, the pitch motion and roll motion are ignored. The motion of underwater bio-inspired robot is regarded as rigid body motion, and the following assumptions are made. (1) High frequency movement of the underwater bio-inspired robot is ignored, and only low frequency motion is considered. (2) The lateral force generated by the undulatory fin is ignored, that is, it is assumed that the underwater bio-inspired robot cannot actively generate lateral propulsion. (3) The surge and heave motions are decoupled from the yaw motion. (4) The effect of nonlinear damping is neglected, while linear damping is considered due to low speed of the underwater bio-inspired robot. Based on these assumptions and the good bilateral symmetry and fore-and-aft symmetry of the underwater bio-inspired robot, the system model can then be simplified and described as follows: η˙ = J (ψ)ν, M ν˙ = −C(ν)ν − Dν + τ + τd , where ⎡

⎤ cos(ψ) − sin(ψ) 0 0 ⎢ sin(ψ) cos(ψ) 0 0 ⎥ ⎥, J (ψ) = ⎢ ⎣ 0 0 1 0⎦ 0 0 01 ⎤ ⎡ m 11 0 0 0 ⎢ 0 m 22 0 0 ⎥ T ⎥ M =⎢ ⎣ 0 0 m 33 0 ⎦, τ = [τu , 0, τw , τr ] , 0

0

0 m 44

⎤ 0 0 0 −m 22 v ⎢ 0 0 0 m 11 u ⎥ ⎥, C(ν) = ⎢ ⎣ 0 0 0 0 ⎦ m 22 v −m 11 u 0 0 ⎡ ⎤ d11 0 0 0 ⎢ 0 d22 0 0 ⎥ T ⎥ D=⎢ ⎣ 0 0 d33 0 ⎦, τd = [τdu , τdv , τdw , τdr ] 0 0 0 d44 ⎡

(4.10)

72

4 Motion Control of the Underwater Biomimetic Robot

where M denotes the generalized mass matrix, C represents the matrix of Coriolis and centripetal force, and D is the linear damping matrix. τ represents the vector of control input. τu , τw denote the surge and heave force, respectively, and τr denotes the yaw moment in body-fixed frame. τd is the disturbance force and moment caused by external environment. τdu , τdv , τdw are the disturbance forces acting on surge, sway, heave, respectively, and τdr is the disturbance moment acting on yaw.

4.2.2 Depth Control Based on the system model of the underwater bio-inspired robot given in formula (4.10), the dynamic model of the depth channel can be expressed as follows:

z˙ = w w˙ = − md3333 w +

1 τ m 33 dw

+

1 τ m 33 w

,

(4.11)

where z represents the depth of the underwater bio-inspired robot, m 33 is the generalized mass of the depth channel, and d33 is the damping factor of the depth channel. Based on formula (4.11), the depth channel of the underwater bio-inspired robot is a second-order system, which is affected by external interference. The system model of the underwater bio-inspired robot is given in the previous section, but the parameters of the system model are difficult to determine for the actual prototype, which brings great obstacles to many control methods based on the accurate system model. Therefore, when designing the control algorithm for the underwater bio-inspired robot, the control method which requires less accuracy of the system model is considered. Active disturbance rejection control (ADRC) proposed by Han [2] combining modern control theory with classical PID control method can better handle the above problems. Classical ADRC consists of the tracking differentiator, the extended state observer (ESO), and the nonlinear error feedback. The tracking differentiator transforms the set signal of step change into a smooth transition signal and gives the differential signal of the transition signal at the same time. Expanded state observer not only estimates the system state variables in real time, but also estimates the system disturbances (including system external disturbances and system model errors) in real time, and compensates the system disturbances in real time in the control signal, thus eliminating the influence of external disturbances and system model errors on the control system. Unlike the linear combination of error proportion, integral, and differential used in PID controllers, ADRC uses the nonlinear function of state error as its control variable, because the nonlinear error feedback has higher efficiency and better control performance. However, the parameters of the nonlinear ADRC are difficult to tune, and there is no mature parameter tuning method at present, which brings a lot of difficulties to the engineering application. For this reason, Professor Gao of Cleveland State University simplifies the nonlinear ADRC

4.2 Depth Control and Yaw Angle Control

73

technology to linear ADRC technology and gives a simple method to determine the parameters of linear extended state observer using the bandwidth concept [3], which makes the linear ADRC technology widely used in practical engineering. Considering that the underwater bio-inspired robot is disturbed by external water flow during its swimming, and the system model parameters are difficult to obtain, a depth controller based on ADRC is designed in this section. Moreover, the underwater bio-inspired robot is a multi-variable, nonlinear, and strong-coupling system, which is difficult to model accurately. In this section, we hence designed a hybrid control that combines ADRC with a fuzzy strategy to enable the robot with undulatory fins to achieve closed-loop depth, which determines the relationship between the heave force and the parameters of the propagating wave. Note that the linear ADRC is composed of tracking differentiator, extended state observer, state error feedback, and other modules. Its structural block diagram is shown in Fig. 4.3, where τdw denotes the disturbance forces acting on heave. z 0 denotes the desired depth as the input of the control, usually a step signal. The tracking differentiator transforms the step signal z 0 into a smooth transition signal z 1 , and outputs the differential signal z 2 of z 1 , that . is z 2 = z 1 . z denotes the actual depth of the robot. ζz1 and ζz2 are the estimated states of the dynamic heave equation, and ζz3 is the extended state estimating the general disturbance. ez1 and ez2 denote state errors of ESO. The primary controlling quantity τw0 is given by the state error feedback module through ez1 and ez2 . The disturbance of the system is compensated in real time in the controlling quantity τw given by the ADRC. In depth control, the controlling quantity given by ADRC is the thrust in heave direction, but the actual control parameters of the underwater bioinspired robot are the propagating wave parameters of the left and right undulatory fins. Therefore, the parameter mapping model based on fuzzy strategy is to map the heave force into the parameters of the propagating wave, that is, the wave frequency FL , FR of the left and right undulatory fins, the swing phase difference ϕ of the adjacent fins, and the offset angle θ B . Moreover, the amplitude of the wave motion of the undulatory fins remains unchanged. Another input signal τu of the parameter mapping model is the propulsion force in surge direction, which is given by the speed controller in surge direction. τu = 0 represents that the robot is required to quickly dive or rise in place. The ADRC is designed with the dynamic model described in formula (4.11) as the controlled object, which mainly includes tracking differentiator, extended state observer, and state error feedback. The components are described below. (a) Tracking Differentiator: For a given set value z 0 , the discrete form tracking differentiator used to track it can be described as

z 1 (k + 1) = z 1 (k) + h z z 2 (k) , (4.12) z 2 (k + 1) = z 2 (k) + h z fhan(z 1 (k) − z 0 , z 2 (k), δz , h z0 ) where h z denotes the control period of ADRC controller, k denotes the kth sample constant, z 1 is the tracking signal of z 0 , and z 2 is the differential signal of z 1 . δz is the time factor, determining the tracking speed. h z0 is the filtering factor,

74

4 Motion Control of the Underwater Biomimetic Robot

Fig. 4.3 Structural block diagram of the depth control

which determines the noise suppression ability. fhan(z 1 (k) − z 0 , z 2 (k), δz , h z0 ) is calculated as ⎧ d = h z0 δz2 ⎪ ⎪ ⎪ ⎪ a0 = h z0 z 2 ⎪ ⎪ ⎪ ⎪ ⎪ y = z1 + a + 0 ⎪ ⎪ √ ⎪ ⎪ a ⎨ 1 = d(d + 8|y|) , (4.13) a2 = a0 + sgn(y)(a1 − d)/2  ⎪ ⎪ ⎪ s y = sgn(y + d) − sgn(y − d) /2 ⎪ ⎪ ⎪ ⎪ a = (a ⎪ ⎪  0 + y − a2 )s y + a2  ⎪ ⎪ s = sgn(a+ d) − sgn(a − d) /2 ⎪ a ⎪  ⎩ a fhan = −δz d − sgn(a) sa − δz sgn(a) where sgn(·) denotes the symbolic function. δz and h z0 are adjustable parameters. Note that, in the process of parameter adjustment, increasing δz can speed up the tracking speed, but the required controlling quantity will increase accordingly. The larger h z0 , the more obvious the noise suppression effect of the tracking differentiator. (b) Extended State Observer: Formula (4.11) is rewritten as z¨ = f w (˙z , dw ) + bw τw ,

(4.14)

where f w (˙z , dw ) = − md3333 w + m133 τdw represents the disturbance in the heave direction (including the external disturbance to the system and the system model error, which is represented by f w below). bw = m133 is the input gain of the control.

4.2 Depth Control and Yaw Angle Control

75

For formula (4.14), let ζz1 = z, ζz2 = z˙ , ζz3 = f w , ζz = [ζz1 , ζz2 , ζz3 ]T and assume that f w is differentiable, then the state space expression of the system described in (4.14) can be obtained as

ζ˙z = Aζz + Bτw + Eg , z = Cζz

(4.15)

where ⎡

⎤ 010 . A = ⎣ 0 0 1 ⎦, B = [0, bw , 0]T , E = [0, 0, 1]T , C = [1, 0, 0], g = f w . 000

Formula (4.15) is easily proved to be observable according to the observability criterion of modern control theory. Therefore, the following form of linear extended state observer can be designed as

ζ˙ˆz = Aζˆz + Bτw + L z (z − zˆ ) , zˆ = C ζˆz

(4.16)

where ζˆz = [ζˆz1 , ζˆz2 , ζˆz3 ]T denotes the estimated state. L z = [l z1 , l z2 , l z3 ]T represents the ESO gain, which is an adjustable parameter. The observer gain vector must be selected so that all characteristic roots of the system described in formula (4.16) are located in the left half plane of the S plane. For simplicity, all characteristic roots of formula (4.16) are designed in −ωoz , that is λ(s) = s 3 + l z1 s 2 + l z2 s + l z3 = (s + ωoz )3 ,

(4.17)

where ωoz represents the bandwidth of the ESO, which is an adjustable parameter. Therefore, the ESO gain can be obtained as 2 3 T L z = [3ωoz , 3ωoz , ωoz ] .

(4.18)

(c) State Error Feedback: When the ESO gain is reasonably selected, the ESO described in formula (4.16) will give the estimated value of the state variable of formula (4.15), that is ζˆz1 ≈ ζz1 , ζˆz2 ≈ ζz2 , ζˆz3 ≈ ζz3 . The controlling quantity is designed as τw =

τw0 − ζˆz3 , bw

(4.19)

where τw0 is defined below. Substituting formula (4.19) into formula (4.14) can obtain

76

4 Motion Control of the Underwater Biomimetic Robot

z¨ = f w − ζˆz3 + τw0 ≈ τw0 .

(4.20)

Under the action of the controlling quantity given in formula (4.19), based on formula (4.20), the dynamic model of the depth channel has been simplified to a second-order series integral system. A PD controller is designed to control the second-order series integral system, which is designed as τw0 = k zp (z 1 − ζˆz1 ) + k zd (z 2 − ζˆz2 ),

(4.21)

where k zp and k zd represent proportional gain and differential gain, respectively. Therefore, the discrete time depth ADRC controller can be summarized as follows: ⎧ ⎪ z (k + 1) = z 1 (k) + h z z 2 (k) ⎪ ⎪ 1 ⎪ ⎪ ⎪ ⎪ z 2 (k + 1) = z 2 (k) + h z fhan(z 1 (k) − z 0 (k), z 2 (k), δz , h z0 ) ⎪ ⎪ ez = ζˆz1 (k) − z(k) ⎪ ⎪   ⎪ ⎪ ⎪ ζˆz1 (k + 1) = ζˆz1 (k) + h z ζˆz2 (k) − 3ωoz ez ⎪ ⎪ ⎪   ⎪ ⎨ˆ 2 ζz2 (k + 1) = ζˆz2 (k) + h z ζˆz3 (k) − 3ωoz ez + bw τw , ⎪ 3 ˆ ˆ ⎪ ζ (k + 1) = ζ (k) − h ω e z3 z3 z z ⎪ oz ⎪ ⎪ ⎪ ⎪ ez1 = z 1 (k + 1) − ζˆz1 (k + 1) ⎪ ⎪ ⎪ ⎪ ez2 = z 2 (k + 1) − ζˆz2 (k + 1) ⎪ ⎪ ⎪ ⎪ τw0 = k zp ez1 + k zd ez2 ⎪ ⎪ ⎪ ⎩ τ = τw0 −ζˆz3 (k+1) w bw

(4.22)

where the discretization period h z depends on the acquisition period of depth sensing information. The controlling quantity given by the above depth ADRC is the thrust in heave direction, and the controlling quantity given by the controller in the surge direction is the surge thrust. But the actual control parameters of the underwater bio-inspired robot are the propagating wave parameters of the left and right undulatory fins. Therefore, in the actual depth control of the robot, the propulsion force needs to be mapped into the propagating wave parameters of the undulatory fins. Based on the experience obtained from the above multi-modal three-dimensional motion control experiment, a mapping model of heave thrust to propagating wave parameters based on fuzzy strategy is constructed, which maps the thrust in heave direction and surge direction into the propagating wave parameters of undulatory fins. Its structure is shown in Fig. 4.4, which consists of fuzzification, an inference engine, and defuzzification. Fuzzification transforms the crisp controlling quantity provided by the ADRC into fuzzy sets. The rule base is set up according to the evaluation of the three-dimensional motion control. The inference engine computes the fuzzy output based on the fuzzy input and rule base, which is the core of parameter mapping model. Defuzzification retransforms the fuzzy outputs into crisp ones.

4.2 Depth Control and Yaw Angle Control

77

Fig. 4.4 Block diagram of the depth fuzzy inference system

(1) Fuzzification: Fuzzification comprises the determination for variable sets, the selection of linguistic values, and selection of membership functions. According to the evaluation of the three-dimensional motion control and the motion characteristics of the robot, the universes of discourse is determined τu ∈ [−7, 7], τw ∈ [−5, 5], FL ∈ [−40, 40], FR ∈ [−40, 40], ϕ ∈ [0, 40], and θ B ∈ [−170, 170], where the sign of the fin frequency represents the direction of the propagating wave. A positive wave frequency indicates that the propagating wave propagates from the head to the tail of the underwater bio-inspired robot; on the contrary, a negative wave frequency indicates that the propagating wave propagates from the tail to the head of the robot. The sets for τu , τw , FL , FR , ϕ, θB are represented as Tu , Tw , U FL , U FR , Uϕ , Uθ B , respectively. The sets with seven linguistic values for τu , τw , FL , FR are defined as Tu =Tw =U FL =U FR ={NB, NM, NS, Z , PS, PM, PB}. The set with four linguistic values for Uϕ is defined as Uϕ ={Z , PS, PM, PB}. The set with five linguistic values for Uθ B is defined as Uθ B ={NB, NS, Z , PS, PB}. Here, NB, NM, NS, Z , PS, PM, PB are the linguistic values “negative large”, “negative median”, “negative small”, “zero”, “positive small”, “positive median”, and “positive large”, respectively. To simplify the calculation, standard triangular membership functions are utilized, as shown in Fig. 4.5. (2) Rule Base: The rule base is generated directly from the evaluation of the threedimensional motion control. The fuzzy rule base is established as listed in Table 4.1. Four key principles need to be taken into account when generating the rule base, as follows. Heave force in depth control is closely related to the deflection angle of the long fin. Heave force increases as the deflection angle increases, and vice versa. When the forward force is small and heave force is large, the deflection angle of the long fin and wave frequency along the two fins should both increase, while the phase difference between the two adjacent fin rays should decrease.

78

4 Motion Control of the Underwater Biomimetic Robot

Fig. 4.5 Membership functions of the depth fuzzy inference system

When the forward force is large and heave force is small, the deflection angle of the long fin and wave frequency along the two fins should both decrease, while the phase difference between two adjacent fin rays should increase. The priority of the depth control is higher than the priority of the surge control. Namely, when the forward force and heave force are both large, a large heave force is preferred. (3) Fuzzy Inference and Defuzzification: The minimum method is used for fuzzy inference, and the weighted average method is used for defuzzification operation to obtain the propagating wave parameters of the undulatory fins. Next, taking the determination of the propagating frequency of the left undulatory as an example, the process of fuzzy inference and defuzzification is explained. The wave frequency of the left undulatory fin obtained by fuzzy inference and defuzzification can be expressed as

4.2 Depth Control and Yaw Angle Control

79

Table 4.1 Fuzzy inference rule base of the depth control U FL , U FR , U ϕ ,Uθ B

Tw NB

NM

NS

Z

PS

PM

PB

Tu

NB

NB, NB, PB, PB

NB, NB, PB, PB

NB, NB, PB, PS

NB, NB, PB, Z

NB, NB, PB, NS

NB, NB, PB, NB

NB, NB, PB, NB

NM

NB, NB, PS, PB

NM, NM, PB, PB

NM, NM, PB, PS

NM, NM, PB, Z

NM, NM, PB, NS

NM, NM, PM, NB

NB, NB, PS, NB

NS

NB, NB, Z, PB

NM, NM, Z, PB

NM, NM, PS, PS

NS, NS, PM, Z

NM, NM, PS, NS

NM, NM, Z, NB

NB, NB, Z, NB

Z

NB, PB, Z, PB

NB, PB, Z, PB

NM, PM, Z, Z, Z, Z Z, PB

NM, PM, NM, PM, Z, NS Z, NB

NB, PB, Z, NB

PS

PB, PB, Z, PB

PM, PM, Z, PB

PM, PM, PS, PS

PS, PS, PM, Z

PM, PM, PS, NS

PM, PM, Z, NB

PB, PB, Z, NB

PM

PB, PB, PS, PB

PM, PM, PM, PB

PM, PM, PB, PS

PM, PM, PB, Z

PM, PM, PB, NS

PM, PM, PM, NB

PB, PB, PS, NB

PB

PB, PB, PB, PB

PB, PB, PB, PB

PB, PB, PB, PS

PB, PB, PB, Z

PB, PB, PB, NS

PB, PB, PB, NB

PB, PB, PB, NB

m FL i=1 bi μi (τu , τw ) , FL =  m i=1 μi (τu , τw )

(4.23)

where FL is the clear value of wave frequency of the left undulatory fin, output by fuzzy inference system. m is the number of fuzzy rules activated by the current input (τu , τw ) in the rule base. biFL is the i-th fuzzy rule corresponds to the center of the membership function of the fuzzy output language value of FL . μi (τu , τw ) denotes the discourse of the i-th fuzzy rule. The specific form can be calculated as follows: μi (τu , τw ) = min{μTuj (τu ), μTwk (τw )},

(4.24)

where μTuj (τu ) denotes the membership of the j-th language value of fuzzy variable Tu when the input is τu . μTwk (τw ) represents the membership of the k-th language value of fuzzy variable Tw when the input is τw . In order to show the feasibility and effectiveness of the proposed depth control method in this section, the depth control experiment for the underwater biomimetic vehicle-manipulator system in an indoor water pool is presented. The initial depth of the robot was 0.187 m, and the desired depth was 0.7 m. The control period is 0.01 s, and the controller parameters are set as δz = 0.008, h z0 = 0.02, ωoz = 25, k zp = 16, k zd = 8.8. The data of the depth control experiment is illustrated in Fig. 4.6. Under the control of the depth controller given in this section, the depth of the underwater biomimetic vehicle-manipulator system almost reached the desired depth without overshoot and then entered the steady state with less steady-state error,

80

4 Motion Control of the Underwater Biomimetic Robot

Fig. 4.6 Result of the depth control experiment

Fig. 4.7 Snapshot sequence of the depth control experiment

which indicates that the depth controller given in this section is effective. The images of the depth control experiment are illustrated in Fig. 4.7.

4.2.3 Yaw Angle Control Based on the system model given in formula (4.10), the dynamic model of the course channel can be expressed as follows:

ψ˙ = r 22 ) uv − r˙ = (m 11m−m 44

d44 r m 44

+

1 τ m 44 dr

+

1 τ m 44 r

.

(4.25)

4.2 Depth Control and Yaw Angle Control

81

Fig. 4.8 Structural block diagram of the course control

Comparing formulas (4.11) and (4.25) shows that the system model of course channel is similar to that of depth channel. Therefore, the course controller is designed to be similar to the depth controller, as described in Fig. 4.8. The course control consists of an ADRC controller and a parameter mapping module based on fuzzy strategy. The tracking differentiator transforms the step signal into a smooth transition signal and outputs its differential signal. The ESO takes the control quantity τr and the real-time course angle of the robot as input and estimates the state of the course channel of the robot and the total disturbance to the course channel, including the external environment disturbance and the course channel model error. The state error feedback module gives the primary control quantity τr 0 by linearly combining the state error. The parameter mapping model maps the control quantity given by the course ADRC to the propagating wave parameters of the undulatory fins. Since there is no need for heave motion in course control, the offset angles of the undulatory fins are always kept at zero in course control. Formula (4.25) is rewritten as ˙ dr ) + br τr , ψ¨ = fr (ψ,

(4.26)

˙ dr ) = (m 11 −m 22 ) uv − d44 r + 1 represents the total disturbance in the where fr (ψ, m 44 m 44 m 44 yaw direction. br = m144 is the input gain of the control. Similar to the depth ADRC, the discrete time course ADRC controller can be designed as

82

4 Motion Control of the Underwater Biomimetic Robot

⎧ ⎪ ψ1 (k + 1) = ψ1 (k) + h ψ ψ2 (k) ⎪ ⎪ ⎪ ⎪ ψ2 (k + 1) = ψ2 (k) + h ψ fhan(ψ1 (k) − ψ0 (k), ψ2 (k), δψ , h ψ0 ) ⎪ ⎪ ⎪ ⎪ eψ = ζˆψ1 (k) − ψ(k) ⎪ ⎪   ⎪ ⎪ ⎪ ζˆψ1 (k + 1) = ζˆψ1 (k) + h ψ ζˆz2 (k) − 3ωoψ eψ ⎪ ⎪ ⎪   ⎪ ⎨ˆ 2 eψ + br τr ζψ2 (k + 1) = ζˆψ2 (k) + h ψ ζˆz3 (k) − 3ωoψ , 3 ⎪ ⎪ ζˆψ3 (k + 1) = ζˆψ3 (k) − h ψ ωoψ eψ ⎪ ⎪ ⎪ ⎪ eψ1 = ψ1 (k + 1) − ζˆψ1 (k + 1) ⎪ ⎪ ⎪ ⎪ ⎪ eψ2 = ψ2 (k + 1) − ζˆψ2 (k + 1) ⎪ ⎪ ⎪ ⎪ τr 0 = kψ p eψ1 + kψd eψ2 ⎪ ⎪ ⎪ ⎩ τ = τr 0 −ζˆψ3 (k+1) r

(4.27)

br

where h ψ is the discretization period, ψ0 denotes the desired course, ψ1 is the tracking . signal of ψ0 , ψ2 is the differential signal of ψ1 , that is ψ2 = ψ1 . δψ is the time factor of course tracking differentiator, determining the tracking speed. h ψ0 is the filtering factor. ζˆψ1 , ζˆψ2 are the estimated states of the dynamic yaw equation, and ζˆψ3 is the extended state estimating the general disturbance. ωoψ represents the bandwidth of the ESO. kψ p and kψd represent proportional gain and differential gain, respectively. The control quantity given by the course ADRC is the rotational moment around the Z B -axis, but the actual control parameters of the underwater bio-inspired robot are the propagating wave parameters of the left and right undulatory fins. Therefore, the rotational moment and propulsion force need to be mapped into the propagating wave parameters of the undulatory fins. Similar to the depth control system, this section constructs a parameter mapping model based on fuzzy strategy, which maps the yaw moment and propulsion thrust into the propagating wave parameters of undulatory fins, which is described in Fig. 4.9. The parameter mapping model consists of fuzzification, an inference engine, and defuzzification. Fuzzification transforms the crisp controlling quantity provided by the course ADRC into fuzzy sets. The rule base is set up according to the evaluation of the open-loop motion control. The inference engine computes the fuzzy output based on the fuzzy input and rule base, which is the core of parameter mapping model. Defuzzification retransforms the fuzzy outputs into crisp ones. (1) Fuzzification: Fuzzification comprises the determination for variable sets, the selection of linguistic values, and selection of membership functions. The universe of other parameters is consistent with the depth fuzzy inference system, except that the universe of yaw moment τr ∈ [−5, 5] and wave amplitude A ∈ [10, 40]. The sets for τu , τr , FL , FR , A, ϕ are represented as Tu , Tr , U FL , U FR , U A , Uϕ , respectively. The set with four linguistic values for U A is defined as U A ={PS, PM, PB, PL}. The set with seven linguistic values for Tr is defined as Tr ={NB, NM, NS, Z , PS, PM, PB}. The set of fuzzy language value elements of other fuzzy variables is the same as that of the depth fuzzy inference system, and the membership functions are the same.

4.2 Depth Control and Yaw Angle Control

83

Fig. 4.9 Block diagram of the course fuzzy inference system

(2) Rule Base: The rule base is generated directly from the evaluation of the threedimensional motion control. The fuzzy rule base is established as listed in Table 4.2. The key principles need to be taken into account as follows. The priority of the course control is higher than the priority of the surge control. Namely, when the rotational moment and forward force are both large, the differential motion of the left and right undulatory fins produces large rotational motion and small forward force. When the forward force is large and rotational moment is small, the propagating waves with different frequencies but the same direction are generated Table 4.2 Fuzzy inference rule base of the course control U FL , U FR , T r UA, Uϕ NB Tu

NM

NS

Z

PS

PM

PB

NB

NB, NB, PL, PB

NB, NS, PB, PB

NB, NM, NB, NB, PB, PB, PB PB

NM, NB, NS, NB, PB, PB PB, PB

PB, NB, PL, PB

NM

NB, NB, PB, PB

NB, NS, PB, PB

NM, NS, PB, PB

NM, NM, PB, PB

NS, NM, PB, PB

NS, NB, PB, PB

PB, NB, PB, NB

NS

NB, NB, PB, PB

NM, PM, NM, NS, PB, PB PM, PM

NS, NS, PS, PS

NS, NM, PM, PM

PM, PB, NB, NM, PB, PB, PB PB

Z

NB, PB, PB, PB

NM, PM, NS, PS, PM, PM PS, PS

Z, Z, Z, Z

PS, NS, PS, PS

PM, NM, PM, PM

PS

NB, PB, PB, PB

NM, PM, PS, PM, PB, PB PM, PM

PS, PS, PS, PS PM, PS, PM, PM

PM, PB, NB, NM, PB, PB, PB PB

PM

NB, PB, PB, PB

PS, PB, PB, PB

PS, PM, PB, PB

PM, PM, PB, PB

PM, PS, PB, PB

PB, PS, PB, PB

PB, NB, PB, PB

PB

NB, PB, PL, PB

PS, PB, PB, PB

PM, PB, PB, PB

PB, PB, PB, PB

PB, PM, PB, PB

PB, PS, PB, PB

PB, NB, PL, PB

PB, NB, PB, PB

84

4 Motion Control of the Underwater Biomimetic Robot

Fig. 4.10 Membership functions of the course fuzzy inference system

by the left and right undulatory fins, which can produce small rotational motion and large forward force. (3) Fuzzy Inference and Defuzzification: The method of fuzzy inference and defuzzification is consistent with that of the depth fuzzy inference system (Fig. 4.10). In order to show the feasibility and effectiveness of the proposed course control method in this section, the course keeping control experiment and course switching control experiment for the underwater biomimetic vehicle-manipulator system in an indoor water pool are presented. ◦ In the course keeping experiment, the initial course of the robot was 70 and the ◦ desired course was 210 . The control period is 0.05 s, and the controller parameters are set as δψ = 0.05, h ψ0 = 0.1, ωoψ = 5, kψ p = 9, kψd = 6.6. The data of the course keeping control experiment is illustrated in Fig. 4.11. The images of the course keeping control experiment are illustrated in Fig. 4.12. The pink curve in the figure shows the track of the robot. Under the control of the course controller given in this section, the robot adjusted its course angle to the desired course angle at a fast speed, and the whole adjustment process lasted for 12 s. Moreover, that the adjustment process had a small overshoot as shown in Fig. 4.13. When the course angle reached the desired course, the robot continued to swim forward at the desired course with less steady-state error until the end of the experiment. Similar to course keeping control, the purpose of course switching control is to keep the underwater biomimetic vehicle-manipulator system at the desired course angle during swimming. However, unlike course keeping control, course switching control should continuously change the desired course. It is necessary for the robot to continuously adjust its course angle in order to maintain the latest desired course ◦ angle. In the course switching experiment, the initial course of the robot was 7 , and ◦ ◦ ◦ ◦ the desired courses were 10 , 100 , 190 , and 280 , successively. The control period is 0.05 s, and the controller parameters are set as δψ = 0.07, h ψ0 = 0.1, ωoψ = 1.5, kψ p = 12, kψd = 7. The data of the course switching control experiment is illustrated in Fig. 4.13. The images of the course switching control experiment are illustrated in Fig. 4.14. Under the control of the course controller given in this section, the robot adjusted its course angle to the first desired course angle and keeps swimming forward at that desired course angle. At 43.3 s, the desired course angle is

4.2 Depth Control and Yaw Angle Control

Fig. 4.11 Result of the course keeping control experiment

Fig. 4.12 Snapshot sequence of the course keeping control experiment

85

86

4 Motion Control of the Underwater Biomimetic Robot

Fig. 4.13 Result of the course switching control experiment ◦

switched to 100 , and the robot adjusted its course angle as fast as possible to the second desired course angle and then keeps moving forward at that desired course angle. Thereafter, the robot completed two more desired course angle switching and moved forward with the new desired course angle, respectively, until the end of the course switching control experiment. The experimental results show that the course controller proposed in this section is also effective for continuous course switching control.

Fig. 4.14 Snapshot sequence of the course switching control experiment

References

87

Fig. 4.14 (continued)

References 1. SNAME (1950) Nomenclature for treating the motion of a submerged body through a fluid. Soc Naval Archit Marine Eng Tech Res Bull 1–5:1–15 2. Han J (1998) Auto-disturbances-rejection controller and its application. Control Decis 13(1):19–23 3. Gao Z (2003) Scaling and bandwidth-parameterization based controller tuning. In: American control conference, pp 4989–4996 4. Wei Q, Wang S, Tan M (2013) An approach to remote update driver system of a robotic fish with two long fins. In: Proceedings of the 32nd Chinese control conference, pp 5705–5709 5. Wei Q, Wang S, Tan M, Zhou C (2013) ADRC based yaw control of a robotic fish with two long fins. J Huazhong Univ Sci Technol (Nat Sci Edn) 41(s1):335–338 6. Wang R, Wang S (2015) Design and implementation of a biomimetic underwater propeller with an undulating long fin. J Huazhong Univ Sci Technol (Nat Sci Edn) 43(s1):408–411 7. Shang L, Wang S, Tan M, Cheng L (2012) Swimming locomotion modeling for biomimetic underwater vehicle with two undulating longfins. Robotica 30(6):913–923 8. Wei Q, Wang S, Zhou C, Wang Y, Tan M (2015) Course and depth control for a biomimetic underwater vehicle–RobCutt-I. Int J Offshore Polar Eng 25(2):81–87 9. Wei Q, Wang S, Dong X, Shang L, Tan M (2013) Design and kinetic analysis of a biomimetic underwater vehicle with two undulating long-fins. Acta Automatica Sinica 39(8):1330–1338 10. Wang R, Wang S, Wang Y (2016) A hybrid heading control scheme for a biomimetic underwater vehicle. In: International ocean and polar engineering conference, pp 619–625 11. Wang R, Wang S, Wang Y, Tang C (2016) Modular design and control of an underwater biomimetic vehicle-manipulator system. In: IEEE/RSJ international conference on intelligent robots and systems (IROS) workshop on robot modularity

Chapter 5

Path Planning of Underwater Biomimetic Robot

Path planning is one of its primary robot technologies. The objective of path generation is to identify a feasible route from an initial point to a final point in terms of path length, fuel consumption, obstacle avoidance, etc. [1]. This chapter focuses mostly on introducing the real-time dynamic Dubins-Helix trajectory planning technique and the real-time approach moving target trajectory planning method.

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning In the unknown marine environment, it is crucial that the underwater bionic robot determines a safe route from its starting point to its destination and execute autonomously. Path planning is the process of establishing a viable and optimal (or suboptimal) route between two specified places. Numerous conventional path planning techniques have been created as a result of their widespread usage in autonomous systems, including the A-star algorithm, graph theory [4], evolutionary algorithms (EAs), rapidly-exploring random trees (RRTs) [3], and mixed-integer linear programming (MILP) techniques. However, none of the approaches previously discussed specifically focused on trajectory smoothing; instead, they just attempted to identify an ideal or suboptimal path from the starting point to the ending one. The trajectory smoothing has been successfully resolved in 2-D space. It has been demonstrated that a Dubins curve [2] is the quickest route between the starting and ending positions with a minimal curvature radius restriction. This curve may take the form of a CSC or CCC route, where “C” denotes an arc and “S” denotes a straight line. In addition to Bezier curves and B-splines, other types of curves having a closed-form representation of position, such as B-splines and B-curves, were employed for 2-D route smoothing.

© National Defense Industry Press 2023 S. Wang et al., Underwater Biomimetic Vehicle-Manipulator System, Unmanned System Technologies,https://doi.org/10.1007/978-981-99-0655-0_5

89

90

5 Path Planning of Underwater Biomimetic Robot

But smoothing a 3-D trajectory is far more difficult. For the goal of determining the feasibility of 3-D trajectory smoothing, there are a number of issues to be taken into account. (1) The robots’ minimum curvature radius and pitch angle constraints must be satisfied. Because the vehicle’s curvature radius is closed tied to the kinematics and dynamics of the vehicle. For the sake of the vehicle’s safety in 3-D space, the pitch angle is often regulated to a given range, i.e., the pitch angle at all locations along the trajectory must be constrained between the specified lower and higher bounds. (2) The optimal path is essential for path planning. The optimal (i.e., shortest) path between the starting posture with the ending pose should be produced using trajectory smoothing. (3) Real-time performance of trajectory creation is needed in many practical applications. The 3-D trajectory smoothing RDDH technique is developed in this part.

5.1.1 Problem Statement The route separating the two postures in the Cartesian coordinate system (x, y, z) is supplied by trajectory smoothing, which can be written as ⎧ dx ⎪ ⎪ ds = cos θ (s) cos γ (s) ⎪ ⎪ dy ⎪ ⎨ ds = sin θ (s) cos γ (s) dz , = sin γ (s) ds ⎪ ⎪ ⎪ dθ = μ 1 ⎪ ds ⎪ ⎩ dγ = μ2 ds

(5.1)

where s represents the curvilinear abscissa along the path, μ1 and μ2 indicate the control inputs, and ϕ(s) and γ (s) denote the heading angle and the pitch angle, respectively. Given that there are certain connections between geometric constraints and the actual constraints of the underwater bionic robot, control inputs (μ1 , μ2 ) should be chosen to adhere to the following geometric constraints: (1) The curvature radius constraint: |R| ≥ Rmin . (2) The pitch angle constraint: γ min ≤ γ ≤ γ max . where γ min and γ max represent the maximum and minimum pitch angle, R denotes the curvature radius, and Rmin indicates the minimum curvature radius, respectively. The curvature radius R(s) is calculated as follows:

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning

1 . R(s) = / μ21 (s) cos2 γ (s) + μ22 (s)

91

(5.2)

Finding the shortest path is essentially an optimum control issue, described as {sg min

ds,

(5.3)

0

where sg represents the planned trajectory length. Numerical techniques can be used to tackle this problem. But in reality, trajectory smoothing must typically fulfill the requirements for real-time computation.

5.1.2 RDDH Trajectory Smoothing The overall framework is shown in Fig. 5.1. The initial and final poses of the robot are indicated by Ps (x s , ys , zs , ϕ s , γ s ) and Pg (x g , yg , zg , ϕ g , γ g ), respectively, where ϕ i , γ i (i = s, g) denote the initial heading and pitch angles, respectively. The curvature radius constraint and the pitch angle constraint are both taken into account while designing the RDDH trajectory smoothing. There are three subpaths that make up the whole trajectory, i.e., ς s , ς DH , and ς g , where ς s and ς g are the results of pitch angle smoothing, and ς DH is the generated Dubins-Helix trajectory. In the whole path of ς s , its pitch angle varies linearly while its heading angle stays constant, that is, the path of ς s is an arc of a vertical plane with a radius of Rmin ; in the entire path of ς g , its heading angle remains unchanged, and the pitch angle changes linearly, that is, the ς g path is also an arc of a vertical plane with a radius of Rmin . The Dubins-Helix trajectory and the pitch smoothing trajectory are each described below in turn.

5.1.2.1

The Dubins-Helix Trajectory

The Dubins curve meets the curvature radius constraint when it projects the DubinsHelix trajectory on the horizontal plane. The Dubins-Helix trajectory’s initial and final poses are indicated by P0 (x 0 , y0 , z0 , θ 0 , γ 0 ) and Pe (x e , ye , ze , θ e , γ e ), respectively, where θ 0 and θ e denote the initial and final heading angles of Dubins-Helix trajectory, respectively. As shown in Fig. 5.2, there are four subpaths in the Dubins-Helix trajectory, which are ς 1 –ς 4 . The pitch angle is taken to be a constant value in each subpath for the sake of simplicity. ς 1 , ς 2, and ς 4 are 3-D Helix curves, while ς 3 is a straight line. The parameters of each subpath are listed in Table 5.1. Note that (x ts , yts ) and (x tg , ytg ) indicate the tangent points C ts and C tg on the Dubins curve,

92

5 Path Planning of Underwater Biomimetic Robot

Fig. 5.1 Solution description. a Initial pose and the final pose. b Projection of the RDDH trajectory on the x–y plane. c RDDH trajectory. The entire trajectory is composed of three subpaths, i.e., ς s , ς DH , and ς g , where ς s and ς g are generated by pitch angle smoothing, and ς DH is the generated Dubins-Helix curve

respectively, γ 0 and γ e are the initial and final pitch angles, respectively. All the pitch angles of subpaths ς 1 ∼ ς 4 must conform to the pitch angle constraint.

Dubins Curve The Dubins curve [2] is a geometric exploration of the shortest path between the initial and target points of a connecting plane in 2-D space while taking into account the minimal turning radius factor. As shown in Fig. 5.3, the curve can be a CSC or CCC path. There are four CSC paths and two CCC paths in 2-D Dubins curves, where “C” stands for an arc and “S” stands for a straight line. The “CCC” path is entirely made up of three arcs, whereas the “CSC” path is made up of two arcs and the tangents between them, where the

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning

93

Fig. 5.2 Dubins-Helix trajectory. Subpaths ς 1 –ς 4 are sequentially marked in red, blue, green, and yellow, among which ς 1 ,ς 2, and ς 4 are 3-D Helix curves, and ς 3 is a straight line

Table 5.1 Parameters of each subpath

Subpath

Pitch angle

Initial position

Final position

ς1

γ1

(x 0 , y0 , z0 )

(x ts , yts , zts )

ς2

γ2

(x ts , yts , zts )

, ) (x ts , yts , z ts

ς3

γ3

(x ts , yts , z0 )

(x tg , ytg , ztg )

ς4

γ4

(x tg , ytg , ztg )

(x e , ye , ze )

arc is utilized to adjust the initial and final poses. The curve can be a CSC or CCC path, as shown in Fig. 5.3. In 2-D Dubins curves, there are four CSC paths and two CCC paths, where C stands for an arc and S for a straight line. The “CCC” path is entirely made up of three arcs, whereas the “CSC” path is made up of two arcs and the tangents between them. The arcs are utilized to change the initial and final poses. To explain the Dubins curve and the parameter derivation, using the CSC path depicted by the blue curve in Fig. 5.3a as an example. (1) Center of O0 and Oe The center Oo (x o0 , yo0 ) of the initial arc is calculated as follows: [

xo0 yo0

]

[ =

] [ ][ ] x0 cos π2 − sin π2 cos θ0 + Rh , sin θ0 y0 sin π2 cos π2

(5.4)

where Rh represents the radius of curvature of the 2-D Dubins curve. The center Oe (x oe , yoe ) of the final arc is calculated as follows: [

xoe yoe

]

[ =

] [ ][ ] xe cos π2 − sin π2 cos θe + Rh . sin θe ye sin π2 cos π2

(5.5)

94

5 Path Planning of Underwater Biomimetic Robot

( x0 , y0 ,θ 0 )

( x0 , y0 ,θ 0 )

α Cts

Rh

O

0

O

Rh

e

Ctg

β

( xe , ye ,θ e )

( xe , ye ,θ e ) (a)

( xDH ,s , yDH ,s ,ϕ DH ,s )

( xDH ,s , yDH ,s ,ϕ DH ,s ) ( xDH , g , yDH , g , ϕ DH , g )

( xDH , g , yDH , g , ϕ DH , g )

(b)

Fig. 5.3 Dubins curve. a “CSC” curve, where “C” stands for an arc and “S” stands for a straight line. b “CCC” curve, where the first “C” represents the initial arc, and the position and direction of the initial point of the arc are determined by (x 0 , y0 ) and θ 0, respectively; the second “C” represents the final arc, and the position and direction of the final point of the arc are determined by (x e , ye ) and θ e , respectively; C ts C tg is the tangent segment of two arcs, where point C ts is the tangent point of the initial arc to the straight line, and point C tg is the tangent point of the final arc to the straight line

(2) Tangent of C ts and C tg The tangent C ts (x ts , yts ) and C tg (x tg , ytg ) are calculated as follows: ( )] ] [ ( π) xo0 cos( − 2 ) − sin( − π2) − → qo , = + Rh yo0 sin − π2 cos − π2 [ ] [ ] [ ] xtg x cos(− π2 ) − sin(− π2 ) − → qo , = oe + Rh ytg yoe sin(− π2 ) cos(− π2 )

[

xts y ys

]

[

(5.6)

(5.7)

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning

95

−−−→ → q O is the normalized vector of O0 Oe . where − (3) Initial rotation angle α Definition α is the heading angle rotated along the arc from point (x 0 , y0 ) to tangent C ts in the initial circular arc of the Dubins curve. Point S 0 is the projection point of −−→ point (x 0 , y0 , z0 ) on the x–y plane, and α 0 is the angle between the vector O0 S0 and −−−→ O0 Cts . The initial rotation heading angle α is calculated as follows: { α=

sin θ0 [ yts − y0 + cot θ0 ( xts − x0 )] > 0 α0 . 2π − α0 sin θ0 [ yts − y0 + cot θ0 ( xts − x0 )] ≤ 0

(5.8)

(4) Final rotation angle β Definition β is the heading angle rotated along the arc from tangent C tg to point (x e , ye ) in the initial circular arc of the Dubins curve. Point Ge is the projection point −−−→ of point (x e , ye , ze ) on the x–y plane, and β 0 is the angle between vector Oe G e and −−−→ Oe Ctg . The final rotation heading angle β is calculated as follows: [

xo0 yo0

[

] = Rmin ·

( ) ( ) ][ ] [ ] cos( π2 · t) − sin( π2 · )t x cos θs + s . sin θs ys sin π2 · t cos π2 · t

(5.9)

(5) The length of the 2-D Dubins curve L 2D L 2D is calculated as follows: ∥ −−−−→∥ ∥ ∥ L 2D = α · Rh + ∥ Cts Ctg ∥ + β · Rh .

(5.10)

Helix Curve As shown in Fig. 5.4, the 3-D Helix curve’s projection on the x–y plane is an arc with a radius of Rh . The initial position is dedicated as S H (x H , yH , zH ), the constant pitch angle is represented as γ H , and the center of the arc on the x–y plane is denoted by OH (x H o , yo H ). By denoting P(x, y, z) as a point on the Helix curve, the 3-D Helix curve equation is given by [ ][ H ] ⎧[ H ] [ H ] qox ⎨ x P = xo + R cos θ P − sin θ P h H , qoy y PH yoH sin θ P cos θ P ⎩ H H H z P = z + |θ P | · Rh · tan γ

(5.11)

where (qH ox , qH oy )T is the normalized unit vector of (x H − x H o , yH − yH o ), θ p is the rotation angle from the initial position S H (x H , yH , zH ) to the position P(x H P , yH P ,

96

5 Path Planning of Underwater Biomimetic Robot

Fig. 5.4 Helix curve and the projection

zH P ) on the x–y plane. If the rotation is counterclockwise, θ P ≥ 0, otherwise, θ P < 0, we can judge by the followings: {

θ P ≥ 0, cos θ H [yoH − y H − tan θ H (xoH − x H )] > 0 . θ P < 0, cos θ H [yoH − y H − tan θ H (xoH − x H )] ≤ 0

(5.12)

The curvature radius on the horizontal plane and the one in 3-D space in the Helix curve are related as follows: Rh = R · cos2 γ H .

(5.13)

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning

97

In the Dubins-Helix trajectory, subpaths ς 1 , ς 2, and ς 4 are Helix curve, with the projection of subpaths ς 1 and ς 4 on the x–y plane representing the initial arc and the final with radius Rh in the Dubins curve, respectively. In subpath ς 1 , α is the heading angle rotated along the arc from point (x 0 , y0 ) to tangent C ts ; in subpath ς 2 , the heading angle rotated on the x–y plane is 2π n, where n is the number of the rotation of subpath ς 2 ; in subpath ς 4 , β is the heading angle rotated along the arc from tangent C tg to point (x e , ye ).

The Relationship of the Parameters and the Dubins-Helix Trajectory Length When a Dubins curve is chosen, the parameters of the Dubins-Helix trajectory and their relationship to the length of trajectory are analyzed. Since the analysis for the case of zts ≤ ztg can be derived similarly, only the case of zts > ztg is discussed in the following context. First, we will go through the notations that will be utilized in the subsequent context. The number of rotation laps in subpath ς 2 is n, the pitch angle of the underwater bionic robot γ is in the range of [γ min , γ max ], and the other parameters of the Dubins-Helix trajectory are denoted as previously described. On the x–y plane, L denotes the distance between (x ts , yts ) and (x tg , ytg ). The total height of subpaths ς 2 and ς 3 is represented by H ς 23 . In two cases as following, the association between the parameters and the length of the Dubins-Helix trajectory is discussed independently: Case 1: H ς 23 > L · tan|γ min |. Case 2: H ς 23 ≤ L · tan|γ min |. In Case 1, the relationship of the parameters n, γ 2 and γ 3 is 2π n · Rh tan γ2 + L · tan γ3 = − Hς23 .

(5.14)

The Dubins-Helix trajectory length S DH is calculated by SDH =

α · Rh 2π n · Rh L β · Rh + + + . cos γ0 cos γ2 cos γ3 cos γe

(5.15)

In (5.15), only when the initial and final poses are fixed, γ 2 , γ 3, and n are uncertain. In Case 1, because H ς 23 > L·tan|γ min |, we have n ∈ N ∗ . It is assumed that n is a constant value, then from (5.14) we can get γ 2 ∈ [γ 2min , γ 2max ] and γ 3 ∈ [γ 3min , γ 3max ], which are determined by  γmin ≤ γ2 ≤ − arctan  γmin ≤ γ3 ≤ − arctan

 Hς23 + L · tan γmin , 2π n · Rh

 Hς23 + 2π n · Rh tan γmin . L

(5.16) (5.17)

98

5 Path Planning of Underwater Biomimetic Robot

2π n ⋅ Rh

Fig. 5.5 Expanded plane chart of subpaths ς 2 and ς 3

γ2

γ3 L As illustrated in Fig. 5.5, for any γ 2 ∈ [γ 2min , γ 2max ], we can get S ς 23 , the total length of subpaths ς 2 and ς 3 , which is given by Sς23 =

2π n · Rh L + . cos γ2 cos γ3

(5.18)

When n is a fixed nonzero value, from Fig. 5.5, we can get Sς 23 ≥ Sς23 min ,

(5.19)

where S ς 23min , the length of the solid red line, is the total length minimizer of subpaths ς 2 and ς 3 . Consequently, if S ς 23 = S ς 23min , we can calculate the pitch angle as  Hς23 . γ2 = γ3 = − arctan 2π n · Rh + L 

(5.20)

By substituting (5.20) into (5.16) and (5.17), we can find that γ 2 and γ 3 in (5.20) satisfy (5.16) and (5.17). β·Rh α·Rh Since cos and cos are fixed, in the circumstances of (5.20), we can conclude γ0 γe that the Dubins-Helix trajectory is the minimizer under the assumption that n is also fixed. To fulfill the pitch angle constraint, we can get n ∈ [nmin , ∞), determined by | n≥

Hς23 + L · tan γmin 2π · Rh | tan γmin |

| + 1.

(5.21)

Figure 5.5 demonstrates that the length of the solid red line increases as n increases. Therefore, theDubins-Helix  trajectory length is minimized when n = nmin and γ2 = H γ3 = − arctan 2π n·Rς23h +L .

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning

99

The analysis of Case 2 is identical to the one of Case 1, except that n can be zero. As can be seen in Fig. 5.5, when n is zero, the solid red line is shorter than all others when n > 0. So in Case2, when n = 0 and γ3 = − arctan(H ς 23 /L), the minimizer is Dubins-Helix trajectory. Note that in this situation, subpath ς 2 disappears, but for the sake of uniformity, we still assume that γ 2 = γ 3 for this degenerated subpath. As a result, the subsequent conclusion is as follows. The Dubins-Helix  trajectory is at its shortest when n = nmin , γ2 = γ3 =  Hς23 − arctan 2π n·Rh +L , where nmin is given by  n min =

5.1.2.2

0|

Hς23 +L tan γmin 2π Rh | tan γmin |

|

Hς23 ≤ L · | tan γmin | + 1 Hς23 > L · | tan γmin |

.

(5.22)

Design of Pitch Angle Smoothing

To address the discontinuity of the pitch angle, the RDDH approach incorporates a smoothing procedure. The pitch angle smoothing approach employs the Dubins curve, as seen in Fig. 5.6. For pitch angle modification, subpaths ς s and ς g are inserted at the initial and final positions, respectively. The heading angle is fixed on subpath ς s , while the pitch angle is revolved from γ s to γ tran . Ps and P0 are the initial and final poses of ς s , respectively. Subpath ς g is similar, but the pitch angle is revolved from γ tran to γ g . The initial and final poses of ς g are Pe and Pg , respectively.

Fig. 5.6 Design of pitch angle smoothing

100

5 Path Planning of Underwater Biomimetic Robot

S DH , the length of the Dubins-Helix trajectory (marked as the red line in Fig. 5.6), is uncertain. Therefore, it is hard to precisely define the pitch angle γ DH . The binary search algorithm (see Algorithm 5.1) is utilized in engineering applications to compute the pitch angle γ DH . To limit the number of iterations and, consequently, the calculation time, it is essential to choose an adequate initial value for the pitch angle γ DH . If zs > zg , we utilize the initial pitch angle provided by (5.23). According to our experiments, γ DH can be calculate with this initial pitch angle γ trans0 in fewer than five repetitions. Note that the computation for zs ≤ zg is comparable.  z g − zs , = arctan 2π n 0 · Rmin + L 2D0 

γtran0

(5.23)

where L 2D0 is the length of the Dubins curve on the x–y plane between the initial pose and the final pose, and n0 can be obtained by  n0 =

0|

z s −z g +L 2D0 tan γmin 2π Rh | tan γmin |

|

z s − z g ≤ L 2D0 | tan γmin | + 1 z s − z g > L 2D0 | tan γmin |

.

(5.24)

As the case of zs ≤ zg is comparable to that of zs > zg , it will not be repeated.

5.1.2.3

Radius Selection

In order to satisfy the requirements of engineering applications, an efficient and workable path between the initial and final poses must to be generated in real-time. According to Sect. 5.1.2.1.3, once the Dubins curve has been selected, it is possible to get the shortest Dubins-Helix trajectory. Consequently, an effective path can be produced by choosing an appropriate horizontal curvature radius Rh . The procedure is discussed in two cases, i.e., n = 0 and n /= 0.

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning

101

(1) n = 0: If L 2D is the shortest, the path is an effective one. Therefore, we choose the minimum value of Rh to provide a 2-D Dubins path. The minimal value of Rh should meet the curvature radius constraint |R| ≥ Rmin . First, let Rh = Rmin , we can get a trajectory generated by path generation, and the pitch angle of the Dubins-Helix trajectory γ DH . Second, select a radius of curvature Rh on the x–y plane to satisfy δ 1 ≥ Rh , cos2γ tran − Rmin ≥ 0, where δ 1 is a small threshold. (2) n /= 0: n is a crucial element for determining the path length. Assume that H +L·tan γ σ = 2πς23Rh | tan γminmin| +1−n r e f (σ ∈ (0, 1)), where nref is derived from (5.15) when Rh = Rmin . On subpath ς 2 , the length of a Helix curve with the rotation angle of 2π is less effective, particularly when σ approaches 0. To meet the demand of getting an effective path, the horizontal curvature radius Rh is increased to make δ 2 < σ < 0, where δ 2 is a negative threshold. 5.1.2.4

Design of RDDH Trajectory and Smoothing Algorithm

The procedure of the RDDH method is as follows: (1) Choose an acceptable radius Rh of curvature for 2-D Dubins route planning. (2) The binary search algorithm is used to determine the parameters of subpath ς s and subpath ς g . (3) Determine the 2-D Dubins curve using the initial and final poses of the DubinsHelix trajectory as Let P0 and Pe , respectively. (4) Calculate the parameters of the shortest Dubins-Helix trajectory γ2 = γ3 =  Hς23 − arctan 2πn·Rh +L . (5) Acquire the equation of each subpath. Following the preceding steps, the RDDH method generates an effective and viable 3-D trajectory. The initial and final poses, the parameters of the Dubins curve, the parameters of the pitch angle smoothing approach, and the number of laps in subpath ς 2 (denoted by n) are the parameters that determine the RDDH trajectory. Tables 5.2 and 5.3 list the parameters of the second and third categories, respectively. Obviously, a path can be described by a limited number of parameters. Therefore, the RDDH trajectory only requires a tiny amount of storage and is computationally inexpensive. The RDDH method can provide an effective path for different initial and final poses configurations. CSC paths of Dubins curves are employed when the distance Table 5.2 Parameters of the Dubins curve on the x–y plane

Parameters of the Dubins curve on the x–y plane 1

The horizontal curvature radius Rh

2

The initial rotation angle α

3

The final rotation angle β

4

The center of initial circle Os

5

The center of final circle Og

102 Table 5.3 Parameters of the pitch angle smoothing method

5 Path Planning of Underwater Biomimetic Robot Parameters of pitch angle smoothing method 1

The curvature radius Rmin

2

The pitch angle γ tran

3

The center of initial circle Ozs

4

The center of final circle Ozg

between the initial and final poses onto the x–y plane exceeds 2Rmin . Otherwise, CCC curves are used instead of CSC curves, and the other steps are identical.

5.1.3 Simulation and Analysis In our simulations ran on a 2.2 GHz Pentium-class computer with 2G RAM, 3-D RDDH trajectory generation can accomplish real-time computation with an average execution time of 65 μs. Compared to the minute-class execution time of the method in [4], the RDDH method has a microsecond-class execution time. In this part, the simulations are run using the parameters of Rmin = 40 m and − 15z ≤ γ ≤ 20z . The RDDH trajectory which was planned based on the data in Table 5.4 is compared to the optimal trajectory that was generated by the numerical method in Fig. 5.7a. As Fig. 5.7b demonstrates, the projection of the Dubins-Helix trajectory on the x–y plane and the graph illustrating the link between z and the abscissa along the RDDH trajectory are both Dubins curves. The purpose of subpaths ς s and ς g is to make a smooth transition of the pitch angle. The results of pitch angle smoothing are represented by the blue parts at two ends of the curves in Fig. 5.7b, where the heading angle remains constant, while the pitch angle is continuously adjusted for attitude. The Dubins-Helix trajectory is denoted by the red curve, the pitch angle of the trajectory is a fixed value, and the projection on the x–y plane is a Dubins curve (Fig. 5.8). To assess the effectiveness of the RDDH method, we compare the length of the paths generated by three different methods, including the RDDH method, the approach of Ambrosino et al. [3], and the numerical method [4] named multiple shooting. When the distance between the two points onto the x–y plane is vast, 3-D Table 5.4 Initial and final poses of the underwater bionic robot

Pose

Initial pose

Final pose

x

500

− 100

y

100

400

z

300

0

Heading angle

240°

45°

Pitch angle

15°

15°

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning

103

Fig. 5.7 RDDH path and the projection in x–y plane. a RDDH path. b Projection of the RDDH trajectory on the x–y plane

path generation [3] in is effective in terms of path length (not far from optimal). Consequently, the issue is discussed individually in two cases based on this distance. First, five pairs of poses separated by significant distances are presented in Table 5.5, and then in Table 5.6, the lengths of trajectories generated by the three methods are listed. To compare the paths generated by the RDDH method and the approach of Ambrosino et al. with the ones produced by the numerical method, the length differences e(%) are provided in Table 5.6. Second, another five pairs of poses with small distances are provided in Table 5.7, while simulation results obtained by three methods are presented in Table 5.8. Under the identical conditions, the average execution time of the numerical method [4] is minute-class, whereas the average execution time of the RDDH method and the approach of Ambrosino et al. [3] are both microsecond-class, which is significantly less than the numerical method that can produce the optimal path.

104

5 Path Planning of Underwater Biomimetic Robot

Fig. 5.8 Description of the RDDH trajectory. a Pitch angle versus the abscissa along the RDDH trajectory. b Heading angle versus the abscissa along the RDDH trajectory Table 5.5 Initial and final poses of the underwater bionic robot I Initial pose

Final pose

Position

γ (°)

θ (°)

Position

γ (°)

θ (°)

1

(200, 500, 200)

−5

180

(500, 350, 100)

−5

2

(100, − 400, 100)

0

30

(500, − 700, 0)

0

150

3

(− 200, 200, 250)

15

240

(500, 800, 0)

15

45

4

(− 300, 1200, 350)

0

160

(1000, 200, 0)

0

30

5

(− 500, − 300, 600)

10

150

(1200, 900, 100)

10

300

0

5.1 Real-Time Dynamic Dubins-Helix Trajectory Planning

105

Table 5.6 Comparisons of the three methods I RDDH method Path length

e (%)

Ambrosino et al. [3]

Numerical method

Path length

Path length

e (%)

1

467.70

4.03

472.21

5.04

449.56

2

649.52

2.10

652.57

2.59

636.12

3

1088.10

2.32

1093.83

2.86

1063.41

4

1802.60

0.75

1806.70

0.98

1789.21

5

2245.14

1.30

2248.57

1.45

2216.40

Table 5.7 Initial and final poses of the vehicle II Initial pose

Final pose

Position

γ (°)

θ (°)

Position

γ (°)

θ (°)

1

(120, − 30, 250)

− 10

100

(220, 150, 100)

10

300

2

(380, 230, 200)

0

30

(280, 150, 30)

0

200

3

(− 80, 10, 250)

0

20

(50, 70, 0)

0

240

4

(400, − 250, 600)

0

350

(600, − 150, 300)

0

150

5

(− 200, − 200, 450)

0

340

(− 300, − 80, 100)

0

100

Table 5.8 Comparisons of the three methods II RDDH method Path length

Ambrosino et al. e (%)

Path length

Numerical method e (%)

Path length

1

588.60

0.88

770.23

32.00

583.47

2

667.71

1.39

879.82

33.60

658.53

3

979.34

1.14

1193.46

23.28

968.25

4

1169.73

0.70

1391.46

19.79

1161.55

5

1367.56

0.99

1689.12

24.74

1354.12

We provide a novel method for 3-D trajectory smoothing in this part, termed realtime dynamic Dubins-Helix (RDDH) method. Combining Dubins-Helix trajectory generation and pitch angle smoothing, this method satisfies both the radius of curvature and pitch angle constraints. The method for parameter selection and calculation is created to produce a shorter and smoother trajectory based on our research of the connection between the parameters and the length of the obtained trajectory. The numerical results indicate that the proposed method can provide an effective (not far from optimal) and feasible path that fulfills the geometric constraints and various initial conditions and can be implied in real-time engineering applications.

106

5 Path Planning of Underwater Biomimetic Robot

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving Target Following the discussion in Sect. 5.1, the real-time dynamic Dubins-Helix method can provide an effective and feasible path that satisfies the radius of curvature constraint and pitch angle constraint for underwater bionic robot. Nonetheless, this approach is mostly used for fixed target points in the operation area. The underwater biomimetic robot-operating arm system must occasionally work against specific moving targets, such as submarines, underwater equipment, and ships. This system must approach the moving target from the current initial position in order to complete such task. In this section, a method for autonomously approaching a moving target with the underwater bionic robot is demonstrated using real-time trajectory planning. According to the real-time dynamic Dubins-Helix trajectory planning method proposed in Sect. 5.1, this method discusses two cases in which the moving target path is either a straight line or a circular arc.

5.2.1 Problem Statement It is assumed that there are no environmental obstacles and disturbances under consideration, and the moving target moves exclusively in the plane. In order to navigate a UBVMS to a moving target autonomously, as seen in Fig. 5.9, the UBVMS’s main controller must autonomously generate a smooth trajectory approaching the moving target. Thus, the final goal is present: (1) The UBVMS and the target have the same attitudes; (2) The UBVMS is just above the target with a relative height distance;

z

Fig. 5.9 Autonomous navigation of the UBVMS

UBVMS

y UBVMS

x

Underwater Moving Target Underwater Moving Target

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

107

(3) The UBVMS and the target are sailing at the same speed. The initial and final positions of the vehicle are represented by Ps (x s , ys , zs , θ s , γ s ) and Pg (x g , yg , zg , θ g , γ g ), the velocity of the UBVMS is v, respectively. The initial heading and pitch angles of the vehicle are θ s , γ s , respectively, while the vehicle’s final heading and pitch angles are θ g , γ g , respectively. Assume that the target follows the desired trajectory on the horizontal plane. Ptargets (x targets , ytargets , 0, θ targets , 0) and Ptargetg (x targetg , ytargetg , 0, θ targetg , 0) represent the initial and final positions of moving target, while θ targets and θ targetg indicate the initial and final heading angles of the target. The target speed denoted by vtarget is constant. The final realization of autonomous approach is expressed by ⎧ Tv = Ts ⎪ ⎪ ⎪ ⎪ ⎪ x g = xtargetg ⎪ ⎪ ⎪ ⎪ ⎨ yg = ytargetg , zg = h ⎪ ⎪ ⎪ = θ θ targetg ⎪ g ⎪ ⎪ ⎪ ⎪ γg = 0 ⎪ ⎩ vg = vtarget

(5.25)

where T V and T S are the vehicle and moving target arrival times, h is the final relative height distance between the target and the UBVMS, and vg is the final speed of the UBVMS. Because the approaching trajectory needs to be generated online by the UBVMS, the method for approaching needs to be capable of satisfying real-time performance requirements. Additionally, in order to better track the trajectory, the planned trajectory still needs to meet formula (5.1)–(5.3).

5.2.2 Real-Time Trajectory Planning for Approaching a Moving Target The real-time dynamic Dubins-Helix (RDDH) trajectory described in Sect. 5.1 is adjusted and modified when the target trajectory is a straight line or an arc with a fixed radius in order to provide a smooth trajectory approaching the moving target. Then, we analyze the conditions for the underwater bionic robot and the moving target to achieve their respective target pose at the same time. Finally, the underwater bionic robot and the moving target both satisfy Eq. (5.25). The six subpaths ς s , ς 1–4, and ς g make up the smoothing trajectories. On subpaths ς s , ς 1 –ς 4 , the underwater bionic robots move at a constant speed (the initial speed v), however, on subpaths ς g , the speed is changed. When comparing Dubins-Helix curve (subpaths ς 1 –ς 4 ) and subpath ς g (ς s is still pitch angle smoothing) to RDDH trajectory, the differences are as follows:

108

5 Path Planning of Underwater Biomimetic Robot

(1) Currently, only those situations where the projection distance between the initial pose and the target pose on the X–Y plane is greater than 2Rmin are covered in trajectory planning of approaching moving targets. One of the four “CSC” paths in the Dubins curve is represented in this instance by the subpaths of the RDDH trajectories (ς 1 –ς 4 ) projected onto the X–Y plane. As illustrated in Fig. 5.10, the projection trajectories of the Dubins-Helix curve (subpaths ς 1 –ς 4 ) on this plane only use one of the tangents’ CSC paths in the approaching moving target trajectories (Fig. 5.11). Fig. 5.10 2-D external tangential CSC curves. The blue trajectory is the counterclockwise external tangential CSC curve. The yellow trajectory is the clockwise external tangential CSC curve

( x0 , y0 ,θ 0 )

C

Rh ts

α

O

C

0

RhO e tg

β

( xe , ye ,θ e )

ur P1

Fig. 5.11 Relative relationship between two unit vectors

uur P2

uur P2

uur P

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

109

(1) The 3-D Trajectory Based on CSC and Helix Curves: Consequently, when the external tangential CSC curve is chosen for 2-D trajectory, the path length L 2D is calculated as −−−→ −−−→ L 2D = α · Rh + ||Cts Ctg || + β · Rh = θr · Rh + || O0 Oe ||,

(5.26)

where θr is the rotation heading angle along the whole 2-D external tangential −−−→ CSC curve, which is the sum of α and β in Fig. 5.3, and || O0 Oe || is given by −−−→ || O0 Oe || =

/ (xe − t Rh sin θe − x0 + t Rh sin θ0 )2 + (ye + t Rh cos θe − y0 − t Rh cos θ0 )2 ,

(5.27) where t is the optional parameter of the external tangential CSC curve (the blue curve marked in Fig. 5.10) and the clockwise external tangential CSC curve (the yellow curve marked in Fig. 5.10), respectively. Assume that t determines which 2-D external tangential CSC curves will be chosen. For 3-D path generation, the counterclockwise external tangential CSC curve is selected when t = 1, and when t = −1, the clockwise external tangential CSC one is chosen. The 2-D external tangential CSC curves have a unique feature: the rotation heading angle θ r (θ r = α + β) has connection with θ sg , which is the angle between the unit vector (cosθ 0 , sinθ 0 ) and (cosθ e , sinθ e ). To explain this connection, firstly giving a definition as follows: − → − → Assuming that P1 (x1 , y1 ) and P2 (x2 , y2 ) are unit vectors, there are three − → − → cases for the relative connection between the two vectors P1 and P2 . As can be − → seen in Fig. 5.4, when P2 is the red and dotted vector, y1 x 2 − y2 x 1 < 0, we define − → − → − → P2 is in the counterclockwise direction of the vector P1 ; when P2 is the blue − → and solid vector, y1x2 – y2x1 > 0, we define P2 is in the clockwise direction of − → − → − → P1 ; when y1 x 2 − y2 x 1 = 0, P2 and P1 are the same or opposite vectors. According to definition 1, we discuss the relationship between θ r and θsg. As shown in Fig. 5.12, it is divided into four cases. The rotation heading angle is a function about θ sg, t, g, expressed as θr = f (θsg , t, g),

(5.28)

where g represents the internal parameter determined by the relative relationship → between the unit vector − qo (qox , qoy ) and the unit vector (cosθ e , sinθ e ) as { g=

1 sin θe qox − cos θe qoy ≥ 0 , −1 sin θe qox − cos θe qoy < 0

−−−→ → qo (qox , qoy ) is the unit vector of O0 Oe as illustrated in Fig. 5.5. where −

(5.29)

110

5 Path Planning of Underwater Biomimetic Robot

α R h

( x0 , y0 ,θ 0 )

O

0

OR

h

β

(a)

O

0

O

e

( xe , ye ,θ e )

α Rh ( x , y ,θ ) 0 0 0

β ( xe , ye ,θe )

g=1 t=1

(b)

α Rh O

O Rh e

β (c)

0

Rh

e

t=1

α Rh

( x0 , y0 ,θ 0 )

( xe , ye ,θ e ) g=1

g=-1

t=-1

( x0 , y0 ,θ 0 )

O

0

O Rh e

( xe , ye ,θ e ) (d)

β g=-1

t=-1

Fig. 5.12 Classification of the computation of θ r a s = 1, t = 1. b s = − 1, t = 1. c s = 1, t = − 1

The precise expression of the connection between θ r and θ sg is given, using the example of the case when t = 1, g = 1. When the vector (cosθs, sinθs) is in the counterclockwise direction of (cosθe, sinθ e) (see the purple circle), as illustrated in Fig. 5.5a, θr = 2π − θsg. When the vector (cosθ s, sinθs) is in the clockwise direction of (cosθe, sinθe) and in the clockwise direction of (qox, qoy) (see the red circle), θr = θsg. When the vector (cosθ s, sinθ s) is in the clockwise direction of (cosθe, sinθe) and the counterclockwise direction of (qox, qoy) (see the blue circle), θ r = 2π + θsg. As a result, according to the

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

111

four cases in Fig. 5.12, the connection between θ r and θsg can be given by (Fig. 5.13). ⎧ c0 ≤ 0 ⎨ 2π − g · t · θsg θr = 2π − g · t · (2π − θsg ) c0 > 0 and c1 ≥ 0 , ⎩ c0 > 0 and c1 < 0 2π + g · t · θsg

(5.30)

where c0 = s(sinθecosθ s − sinθscosθe) and c1 = s(qoycosθs − sinθsqox). In light of (5.30), we can summarize: The relationship is divided into three cases, and in each case, once the parameters t and g are determined, the rotation heading angle θ r is a fixed value related to θ sg. To satisfy the pitch angle constraint, the trajectory is divided into two cases according to the relationship between L 2D tanγm and |ze − z0 |, where γm is defined as { γmax , ze − z0 ≥ 0 γm = . (5.31) |γmin |, z e − z 0 < 0

Fig. 5.13 S-z diagram of subpath ς g

112

5 Path Planning of Underwater Biomimetic Robot

Then, n and γ tran can be obtained, respectively, as γtran = arctan  n=

0

|z e −z 0 |−L 2D tan γm 2π Rmin tan γm

ze − z0 , L 2D + 2π Rmin n

(5.32)

 L 2D · tan γm ≥ |z e − z 0 | . L 2D · tan γm < |z e − z 0 |

(5.33)

(2) The Curve for Pitch Angle and Speed Adjustment: Subpath ς g is the curve for pitch angle and speed adjustment. The projection of subpath ς g on the x–y plane, which is a straight line or a circle, is a part of the target path. According to Fig. 5.6, the red curve is the S-z diagram of subpath ς g , where S is the abscissa along with the projection of subpath ς g on the x–y plane (a straight line or a circle). Pe( x e , ye , ze ) is the initial position of ς g , which is also the final position of the 3-D trajectory based on CSC Helix curves. θ e is the initial heading angle of subpath ς g . Ptargets represents the target position when the underwater bionic robot reaches Pe . θ target represents the ship heading angle when the ship reaches Ptarget . Pm represents the robot final position of pitch angle adjustment, which is also the initial position of speed adjustment. Ptargetm represents the target position when the underwater bionic robot reaches Pm . The red curve between Pe and Pm is the process of pitch angle adjustment, where the robot speed is v, while the other red one between Pm and Pg is the process of speed adjustment from v to vtarget at a fixed acceleration av . S vp is the S value between Pe and Pm , S sp is the sailing distance of the target during pitch angle adjustment, S vv is the S value between Pm and Pg; Ssv is the sailing distance of the target during speed adjustment. The description of subpath ς g is shown in Fig. 5.6. χ 1 is a circle of radius Rmin and intersects Pe , and the tangent direction of χ 1 at Pe is determined by γ t . χ 2 is a circle of radius Rmin and intersects both χ 1 and z = h at exactly one point each. The tangent point between the circle χ 2 and the line z = h is Pm . Thus, we can get SL = Sv p + Svv − Ssp − Ssv ,

(5.34)

where ⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩

Sv p = Sk + R min sin γ tran Ssp = vtarget

v2 −v2

2 arccos

cos γtran +1 2

 +γtran Rmin

v v−v

, Ssv = vtarget · atarget Svv = 2atarget v v √ Sk = 4Rmin2 − (Rmin cos γtran + Rmin )2

.

(5.35)

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

113

5.2.3 Model Description for Simultaneous Arrival of the Underwater Biomimetic Robot and the Target Based on the generation of a 3-D path, the corresponding equations are created such that the robot and ship arrive simultaneously in their respective poses. When the target sails along a straight line, the equations are the constrained quadratic or linear ones, and the final pose can be obtained in an analytic form. The nonlinear equations are obtained when the target sails along a circle. Numerical methods can be used to solve them.

5.2.3.1

Approaching the Target Moving Along a Straight Line

Figure 5.15 depicts the analysis diagram of the simultaneous arrival of the underwater bionic robot and the target when the operation target moves along the straight track (Fig. 5.14). The underwater bionic robot pitch angle should be set to an initial pitch angle before approaching a moving target. As a result, the amount of time the robot spends sailing on subpath ς s , which is the result of the pitch angle smoothing process, can be negligible. So we can get P0 (x 0 , y0 , z0 ) = Ps (x s , ys , zs ). In the case of cosθ targets = 0, let T v = T s , we can get −−−→ xe + SL cos θtargets − xtargets θr Rmin + || O0 Oe || + 2π Rmin n = . v cos γtran vtarget cos θtargets

(5.36)

( xtargets , ytargets ,θ targets )

Fig. 5.14 S-z diagram of subpath ς g

( xs , y s , θ s )

P

β

e

SL

P

targete

α

O

e

O0

114

5 Path Planning of Underwater Biomimetic Robot

OB Rmin B

Os Rmin ( x0 , y0 ,θ s ) ( xoship , yoship ,θ ships ) ( xships , yships ,θ ships )

OB Rmin B ( x0 , y0 ,θ s )

Rmin Os ( xoship , yoship ,θ ships ) ( xships , yships ,θ ships ) (a)

g=1, t=1

B Rmin OB

(b) g=-1, t=1

B

Rmin O

B

( x0 , y0 ,θ s )

Rmin

Os Rmin ( x0 , y0 ,θ s )

Os

( xships , yships ,θ ships ) ( xoship , yoship ,θ ships ) ( xships , yships ,θ ships ) ( xoship , yoship ,θ ships ) (c)

g=1, t=-1

(d) g=-1, t=-1

Fig. 5.15 Classification of the computation of θ r when the ship sails along a straight line. a s = 1, t = 1. b s = − 1, t = 1. c s = 1, t = − 1. d s = − 1, t = − 1

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

115

By substituting (5.4) into (5.13), one linear equation can be obtained as 

(1 + p 2 − q 2 )xe2 + (2 A + 2 p B + 2qC)xe + A2 + B 2 − C 2 = 0 , xe ≥ Cq

(5.37)

where ⎧ ⎪ ⎪ p = tan θtargets ⎪ ⎪ ⎪ ⎨ q = v cos γtran /(vtarget · cos θtargets ) . A = −xs + t Rmin (sin θs − sin θtargets ) ⎪ ⎪ ⎪ B = ytargets − ys − pxtargets + t Rmin (cos θtargets − cos θs ) ⎪ ⎪ ⎩ C = qx targets + θr Rmin + 2π Rmin n − q S L cos θtargets

(5.38)

The derivation of the equation is similar to (5.37) in the other case of cosθ targets = 0, as a result, it can be omitted. The pitch angle γ tran , the number of rotational laps n, and the rotation heading angle θ r are three unknown variables that need to be determined in the equations due to the uncertainty of the final position. Using a simple search method can get γ tran . γ t can be calculated with less than ten iterations, based on experience. Because of (5.33), n is a decreasing function regarding the projection length L 2D of the trajectory on the x–y plane. Because of this, the maximum value nmax is attainable, and we are able to get the solution of (5.37) by employing a finite and small number of cycles meeting n ≤ nmax . Here, we only provide an overview of the calculations for θ r that make use of the unique characteristic of 2-D external tangential CSC curves. The computation of θ r is divided into four cases, as depicted in Fig. 5.15, based on the choice of the parameters t and s. Due to the fact that the ship sails along a straight line, the final attitudes of both the ship and robot are fixed. By (5.16), which is equivalent to { g= { g=

1, sin θtargets qox − cos θtargets qoy ≥ 0 −1, sin θtargets qox − cos θtargets qoy < 0 1 cos θtargets [yo0 − yotarget − tan θtargets (xo0 − xotarget )] ≥ 0 −1 cos θtargets [yo0 − yotarget − tan θtargets (xo0 − xotarget )] < 0

(5.39)

where 

   xotarget xtargets − t · Rmin · sin θtargets = yotarget ytargets + t · Rmin · cos θtargets    xo0 xs − t · Rmin · sin θs = ) yo0 ys + t · Rmin · cos θs

(5.40)

116

5 Path Planning of Underwater Biomimetic Robot

From (5.16), we can see that the parameter s is determined once 2-D external tangential CSC curve has been chosen (namely is determined). We utilize (5.7) to obtain the rotation heading angle θ r . As shown in Fig. 5.15, in the case of s(sinθ ships cosθ s − cosθ ships sinθ s ) ≤ 0, θ r = 2π − s·t·θ sg , else another two cases need to be divided. B(x B , yB ) in Fig. 5.15 is the boundary of the two cases, and the −−−→ vector Os O B and (cosθ s , sinθ s ) are in the same or opposite direction. Thus, in the other case of s(sinθ ships cosθ s − cosθ ships sinθ s ) > 0, if cosθ ships (x B − x ships ) ≤ 0, θ r = 2π − s·t·(2π − θ sg ), else we can obtain { θr =

2π − g · t · (2π − θsg ) c2 ≥ 0 , c2 < 0 and c3 ≥ 0 2π + g · t · θsg

(5.41)

where c2 = cosθ ships(xe − xB) and c3 = cosθ ships(xe − x ships ). Therefore, once solving the function in (5.14), the final poses the ship and vehicle reach simultaneously can be obtained.

5.2.3.2

Approaching the Ship Moving Along a Circle

To derive the equations in this section, we just take the case where the ship sails along a clockwise circle as an example. As seen in Fig. 5.16, the circle centers O0 (x o0 , yo0 ) and Oe (x oe , yoe ) can be obtained [

xo0 yo0

) ( ) ][ ( ] [ ] x cos θs cos( π2 · t) − sin( π2 · )t + s , sin θs ys sin π2 · t cos π2 · t [ ] ] [ xoe cos(−θ pe ) − sin(−θ pe ) = (Rtarget + t Rmin ) · yoe sin(−θ pe ) cos(−θ pe ) ] [ ] [ x − sin θtargets + otarget , cos θtargets yotarget

]

[

= Rmin ·

(5.42)

(5.43)

where Otarget (x otarget , yotarget ) and Rtarget are the center and the radius of the circle which the ship sails along, respectively, and θ pe is marked in Fig. 5.8. Let T v = T s , we can get −−−→ θ pe Rtarget + SL θr Rmin + || O0 Oe || + 2π Rmin n = . v cos γtran vtarget By substituting (5.18) and (5.19) into (5.20), we can obtain

(5.44)

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

117

Fig. 5.16 Analysis of the final goal when the ship sails along a clockwise circle. The circle which the ship sails along intersects the final arc of the 2-D external tangential CSC curve at exactly one point. Pe is the tangent point between the above two circles

√

D + E sin θ pe + F cos θ pe =

v cos γtran θ pe Rtarget vtarget

− θr Rmin − 2π Rmin n +

vSL cos γtran , vtarget

(5.45)

where ⎧ D = Rs2 + (xotarget − xo0 )2 + (yotarget − yo0 )2 ⎪ ⎪ ⎨ E = 2Rs · [(xotarget − xo0 ) · cos θtargets + (yotarget − yo0 ) · sin θtargets ] . (5.46) ⎪ F = 2Rs · [−(xotarget − xo0 ) · sin θtargets + (yotarget − yo0 ) · cos θtargets ] ⎪ ⎩ R_{s} = R_{target} + t R_{min} We can use numerical methods to solve the equations. In the equations, the pitch angle γ tran can be obtained by a search method, and the maximum of N can also be determined. Here, the calculation of θ r is described. We also employ the special characteristic of 2-D external tangential CSC curves to obtain θ r , which is discussed into two cases. When sinθ targets cosθ s − cosθ targets sinθ s < 0, θ r can be obtained as ⎧ ⎨ 2π − t · θsg or 2π + t · (2π − θsg ) θ pe ∈ [0, π − θ L ] θr = 2π − t · (2π − θsg ) or 2π + t · θsg θ pe ∈ (π − θ L , 2π − θ L ] , ⎩ 2π − t · θsg or 2π + t · (2π − θsg ) θ pe ∈ (2π − θ L , 2π ]

(5.47)

118

5 Path Planning of Underwater Biomimetic Robot

where θ sg is computed as θsg

⎧ θ pe ∈ [0, π − θ L ] ⎨ θ L + θ pe = 2π − θ pe − θ L θ pe ∈ (π − θ L , 2π − θ L ] , ⎩ θ pe + θ L − 2π θ pe ∈ (2π − θ L , 2π ]

(5.48)

where θ L is the angle between the vector (cosθ s , sinθ s ) and (cosθ targets , sinθ targets ). When sinθ targets cosθ s − cosθ targets sinθ s ≥ 0, θ r can be obtained as ⎧ ⎨ 2π + t · θsg or 2π − t · (2π − θsg ) θ pe ∈ [0, θ L ] θr = 2π − t · θsg or 2π + t · (2π − θsg ) θ pe ∈ (θ L , π + θ L ] , ⎩ 2π + t · θsg or 2π − t · (2π − θsg ) θ pe ∈ (π + θ L , 2π ]

(5.49)

where θ sg is computed as θsg

⎧ θ pe ∈ [0, θ L ] ⎨ θ L − θ pe = θ pe − θ L θ pe ∈ (θ L , π + θ L ] . ⎩ 2π − θ pe + θ L θ pe ∈ (π + θ L , 2π ]

(5.50)

Since the above process is identical to the equation’s calculation for the case when the ship sails along a counterclockwise circle, it is omitted. It is noted that θ r is different when θ pe is in different areas. The final pose must be determined in accordance with the three cases as θ r is computed from (5.23) and (5.25).

5.2.4 Real-Time Trajectory Planning Algorithm for Approaching a Moving Target According to the ship path, the autonomous navigation of the vehicle to a moving ship is divided into two cases. Algorithm 5.2 summarizes it. Using a simple search method can obtain γ t . In our experience, γ t can be determined for fewer than ten iterations. In Fig. 5.10, there are two kinds of trajectories that can be chosen for the projection of subpaths ς 2 –ς 4 on the x–y plane. Therefore, we choose the CSC trajectory that the vehicle and the ship can arrive at their respective poses simultaneously and use a less amount of time. In the case where the equations are unsolvable, the solutions are found by expanding the curvature radius.

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

119

5.2.5 Simulation and Analysis 5.2.5.1

Experimental Setup and Test of Real-Time Performance

Numerical simulations are used to evaluate the approaching algorithm. The initialization of the specific parameters for simulations are given: v = 15 cm/s vtarget = 10 cm/s h = 30 cm Ptargets = (0, 0, 0) ◦ θ targets = 0 av = 5 cm/s2

(5.51)

The geometric constraints are selected: − 15z ≤ γ ≤ 15z , R ≥ 60 m. Various initial conditions are used to implement our algorithm. The UBVMS initial configurations are listed in Table 5.1. The simulations are discussed in two cases, where the ship sails along a straight line and a clockwise circle with Rtarget =

120

5 Path Planning of Underwater Biomimetic Robot

Table 5.9 Initial configurations of the unmanned vehicle

θ s (°)

Ps (x s , ys , zs )

γ s (°)

1

(1000, 1000, 300)

45

− 7.5

2

(− 1000, 1000, 300)

225

− 7.5

3

(− 1000, − 1000, 300)

180

− 7.5

4

(1000, − 1000, 300)

300

− 7.5

800 m. The simulation of the approaching algorithm is done in 3.4 GHz Pentium-class computer with 4G RAM. The Eqs. (5.37) and (5.45) are established by analyzing the time of the simultaneous arrival, where only γ tran is determined by searching but the number of iterations is limited. Moreover, once all the parameters are known, the time required to solve the two equations is microsecond-class. Thus, the proposed algorithm satisfies high real-time demand. The test is implemented to show this performance. In the case where the target sails along a straight line, the run average execution time is 44.9 μs, whereas in the other case, where the target sails along a circle, the run average execution time is 194.1 μs. It is demonstrated that the approaching algorithm could be implemented in real time.

5.2.5.2

Estimation and Analysis of the Planning Trajectory

The proposed method is applied in the initial conditions of four different UBVMSs listed in Table 5.9 in order to illustrate its efficiency. Figure 5.19a–d gives the simulation results when the trajectory of the moving target is straight line; Fig. 5.19e–h gives the simulation results when the moving target track is a circular arc. Specific parameters are listed in Tables 5.10 and 5.11. Table 5.10 Parameters of positions and attitudes when the ship sails along a straight line a 1

Pe

θ e (°)

γ tran (°)

Ptargete

θ targete (°)

(882.6, 0, 30)

0

− 11.5

(897.5, 0, 0)

0

2

(2641.0, 0, 30)

0

− 3.9

(2651.7, 0, 0)

0

3

(2562.4, 0, 30)

0

− 4.0

(2573.1, 0, 0)

0

4

(867.3, 0, 30)

0

− 11.7

(880.0, 0, 0)

0

Pg

θ g (°)

Ptargetg

θ targetg (°)

b γ g (°)

1

(932.4, 0, 30)

0

0

(932.4, 0, 0)

0

2

(2677.9, 0, 30)

0

0

(2677.9, 0, 0)

0

3

(2599.7, 0, 30)

0

0

(2599.7, 0, 0)

0

4

(910.6, 0, 30)

0

0

(910.6, 0, 0)

0

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

121

Table 5.11 Parameters of positions and attitudes when the ship sails along a circle a Pe

θ e (°)

γ tran (°)

Ptargete

θ targete (°)

1

(784.6, − 956.4, 30)

258.7

− 7.3

(783.3, − 962.4, 0)

258.3

2

(362.7, − 1513.0, 30)

207.0

− 4.8

(347.0, − 1520.8, 0)

205.7

3

(794.2, − 896.2, 30)

263.1

− 7.6

(793.0, − 906.0, 0)

262.4

4

(647.4, − 329.9, 30)

306.0

− 13.2

(662.9, − 352.1, 0)

304.0

Pg

θ g (°)

γ g (°)

Ptargetg

θ targetg (°)

b 1

(779.7, − 979.3, 30)

257.1

0

(779.7, − 979.3, 0)

257.1

2

(313.5, − 1543.3, 30)

202.8

0

(313.5, − 1543.3, 0)

202.8

3

(789.2, − 930.8, 30)

260.6

0

(789.2, − 930.8, 0)

260.6

4

(694.4, − 402.7, 30)

299.8

0

(694.4, − 402.7, 0)

299.8

The 3-D trajectory is generated when the target sails along a straight line and choose the data in the first line of Table 5.9 as the initial configurations of the vehicle, this trajectory can guide the UBVMS to a moving target (see Fig. 5.19a). Figure 5.17a shows the vehicle and ship paths projected on the x–y plane, and Fig. 5.17b shows the vehicle and ship paths projected on the x–z plane, revealing that ς 1 –ς 4 (see the red curve) are produced to let the unmanned vehicle fall on the ship path while keeping a small distance S L = 14.9 m with the ship on the x–y plane and a height distance 30 m with the ship. Figure 5.17c magnifies the x–z diagram of subpath ς5 (see the black curve), which is used for realizing pitch angle adjustment and speed adjustment. It is noted that the sailing of the trajectory including ς 1 –ς 4 needs 89.7 s to complete, while the sailing of ς g for pitch angle adjustment and speed adjustment only needs 3.5 s. Subpath ς g is specially designed for the final purpose that the vehicle sails along with the ship at the same speed while keeping the same attitude. This can provide a good condition for automatic tracing of the moving target. To verify the feasibility of the trajectory, Fig. 5.18 shows the time history of the robot pitch angle and heading angle along the whole trajectory. It can be seen that both the heading angle and the pitch angle of the UBVMS trajectory is smooth. Furthermore, because of the design of the Helix curve, as shown in Fig. 5.18b, the generated trajectory can meet the pitch angle constraint.

122

5 Path Planning of Underwater Biomimetic Robot

Fig. 5.17 Description of the approaching trajectory. a Vehicle and the ship trajectories projected on the x–y plane. b Vehicle and the ship trajectories projected on the x–z plane. c x–z diagram of subpath ς 5

5.2 Real-Time Trajectory Planning for Autonomous Approaching a Moving …

123

Fig. 5.18 Description of trajectory parameters. a Time history of heading angle. b Time history of pitch angle

Fig. 5.19 Simulations under different initial conditions. a–d Test of autonomous approach to the ship along a straight line path. e–h Test of autonomous approach to the ship along a circle

124

5 Path Planning of Underwater Biomimetic Robot

Fig. 5.19 (continued)

References 1. Jiang X (1994) Introduction to robotics. Liaoning Science and Technology Press, Shenyang 2. Dubins LE (1957) On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. Am J Math 79(3):497–516 3. Ambrosino G, Ariola M, Ciniglio U, Corraro F, De Lellis E, Pironti A (2009) Path generation and tracking in 3-D for UAVs. IEEE Trans Control Syst Technol 17(4):980–988 4. Hota S, Ghose D (2010) Optimal path planning for an aerial vehicle in 3D space. In: Proceeding of IEEE internationla conference on decision and control, Atlanta, USA, pp 4902–4907 5. Yu W (2016) Autonomous operation and trajectory planning of underwater bionic submersible vehicle-manipulator system. Ph.D. dissertation, University of Chinese Academy of Sciences, Beijing 6. Wang Y, Wang S, Tan M, Wei Q (2015) Real-time dynamic Dubins-Helix method for 3-D trajectory smoothing. IEEE Trans Control Syst Technol 23(2):730–736

References

125

7. Wang Y, Wang S, Tan M, Yu J (2017) Simultaneous arrival planning for multiple unmanned vehicles. Int J Robot Autom 32(4):360–368 8. Wang Y, Wang S, Tan M (2015) Path generation of autonomous approach to a moving ship for unmanned vehicles. IEEE Trans Industr Electron 62(9):5619–5629

Chapter 6

Path Tracking Control of the Underwater Biomimetic Robot

Whether autonomous underwater vehicles (AUVs) can accurately move along the planned path is directly related to whether the AUV can reach the task area and return to the recovery point from the task area in underwater operation missions. In addition, when performing some special underwater operation missions, such as underwater pipeline repair, underwater topographic exploration, underwater search and rescue, AUVs need to move along a set path to complete a specific task. Therefore, the research on the path tracking control method of AUV is the key to complete the task successfully. Recently, the path tracking control methods of underwater vehicles mainly include the following categories: linear control methods [1], adaptive control methods [2], sliding mode control methods [3], backstepping methods [4], and intelligent control methods [5].

6.1 Waypoint Tracking Control In this section, a waypoint tracking control method based on line-of-sight (LOS) guidance system is proposed to solve the waypoint tracking control of underwater bio-inspired robot. Firstly, based on the principle of LOS guidance, the desired course angle and desired depth are given according to the current pose and current tracking path of the robot. Therefore, the three-dimensional waypoint tracking control problem is transformed into independent three-channel control problems such as speed control, yaw angle control, and depth control. Then, independent controllers are designed for these three-channel control problems. Through a parameter mapping model based on fuzzy strategy, the control quantity in the form of force or torque given by the three-channel controller is mapped to the propagating wave parameters of the undulatory fins. Finally, the effectiveness of the waypoint tracking control method proposed in this section is verified by experiments.

© National Defense Industry Press 2023 S. Wang et al., Underwater Biomimetic Vehicle-Manipulator System, Unmanned System Technologies,https://doi.org/10.1007/978-981-99-0655-0_6

127

128

6 Path Tracking Control of the Underwater Biomimetic Robot

6.1.1 Problem Statement As described in Fig. 6.1, the waypoint tracking problem that we consider in this chapter can be described as follows. Position p(x, y, z) denotes the position of the underwater bio-inspired robot in the world frame called O E X E Y E Z E , and ψ denotes the yaw angle of the robot in the O E X E Y E Z E frame. The desired path is composed of a series of given three-dimensional waypoints in the world frame: WP = { p1 , p2 , ..., pn }, pi = (xi , yi , z i ) ∈ R3 , i = 1, 2, ..., n. For each ball Bγi ( pi ) with center pi and waypoint pi (i = 1, 2, ..., n), there is a neighborhood  Bγi ( pi ) = p ∈ R3 : ∥ p − pi ∥ ≤ γi , γi > 0 , where γi represents the radius of the ball. The control objective is to force the robot’s center of mass p(x, y, z) to successively visit the neighborhoods of all waypoints pi (i = 1, 2, ..., n − 1) except the last and to ultimately converge to the last waypoint pn by controlling the motion of the left and right undulatory fins of the robot in the presence of unknown external environmental disturbances and model uncertainties. Note that for waypoint pi (i = 1, 2, ..., n − 1), it is more meaningful to minimize the cross-track error, defined as the distance from the robot’s center of mass to the corresponding path segment defined by two adjacent waypoints, rather than to minimize the distance error between the robot’s center of mass and the expected waypoint. Hence, the aim is to make the robot enter the neighborhoods of all the waypoints except for the final one. When the robot needs to reach the final waypoint, the distance error between the robot’s center of mass and the last waypoint should be minimized. Hence, the robot should be forced to eventually converge to the last

Fig. 6.1 Diagram of the waypoint tracking control

6.1 Waypoint Tracking Control

129

waypoint, namely the radius of the last waypoint’s neighborhood ball Bγ n ( pn ) should be sufficiently small.

6.1.2 The Controller Design of Waypoint Tracking The research on the waypoint tracking control algorithm is proposed to solve the above waypoint tracking control problem in this section. First, the structure of the waypoint tracking controller is introduced as a whole, and then each unit module of the controller is described in detail. Due to the structural characteristics and the way in which the thrust is generated, the thrust and moment generated by the undulatory fins are highly coupled. It is difficult for the two undulatory fins to generate the required thrust and moment at the same time. In addition, considering that the lateral thrust generated by the underwater bio-inspired robot is small, the system is simplified in this section by ignoring the lateral thrust generated when designing the waypoint tracking control algorithm, and only considers the control inputs such as surge thrust, heave thrust, and yaw moment. Underwater bionic robot is a nonlinear system. Although some researchers have established dynamic models of undulatory fins in different ways, these models are either not accurate enough or model parameters are difficult to determine, making it difficult to design control algorithms based on accurate system models. Therefore, the underwater bio-inspired robot is taken as a whole when designing the waypoint tracking control algorithm in this section. Assuming its control inputs are surge thrust, heave thrust, and yaw moment, the waypoint tracking control algorithm is designed with the simplified robot system model as described in formula (4.10). However, when actually controlling the motion of the underwater bio-inspired robot prototype, its control input should be wave parameters (including wave frequency, wave amplitude, phase difference between adjacent fins, direction of propagating wave, and offset angle of the undulatory fins), so the thrust and yaw moment given by the above control algorithm need to be mapped as wave parameters of the undulatory fins. Thus, the motion of the underwater bio-inspired robot prototype can be controlled. Moreover, in order to reduce the impact of model errors on the performance of waypoint tracking control for simplified underwater bio-inspired robot systems, the ADRC technology is still used in this section to design the controller. Based on the above considerations, a waypoint tracking controller which is not highly dependent on the model of the underwater bio-inspired robot system is presented in this section, and its structure block diagram is shown in Fig. 6.2. The waypoint tracking controller mainly consists of five parts, namely three-dimensional LOS guidance system, speed controller, course controller, depth controller, and parameter mapping model based on fuzzy inference. The goal of waypoint tracking control is to converge the position of the robot to the desired path, which is essentially a three-dimensional spatial location control problem. In this section, a threedimensional LOS guidance system is constructed by applying the principle of LOS guidance. Based on the current position p(x, y, z) and tracking path of the underwater

130

6 Path Tracking Control of the Underwater Biomimetic Robot

Fig. 6.2 Waypoint tracking control block diagram

bio-inspired robot, the desired course angle ψ D and depth z d are given. The threedimensional waypoint tracking control problem is converted to the yaw, depth, and speed control problem, and the waypoint tracking control is indirectly implemented. Since the desired course angle, desired depth and desired speed correspond to the yaw channel, heave channel, and surge channel of the robot, controllers can be designed for each channel to control the course angle, depth, and speed. The speed controller is designed with ADRC technology to adjust the surge thrust τu through feedback, which can control the robot speed u approach the desired speed u d . In order to reduce the distance error between the actual motion track and the desired path of the robot, the course angle of the robot is required to quickly track the desired course angle given by the three-dimensional LOS guidance system. Therefore, the course controller is designed with the course control algorithm given in Chap. 4 to adjust the yaw moment τr through feedback, so that the course angle ψ of the robot can quickly track the desired course angle ψ D given by the three-dimensional LOS guidance system. The depth controller is also designed with the depth control algorithm given in Chap. 4 to adjust the heave thrust through feedback, which can quickly control the depth of the robot to the desired depth given by the three-dimensional LOS guidance system. Finally, in the actual control system, a trust and yaw moment to propagating wave parameter mapping model based on fuzzy inference is constructed, using the three-dimensional motion control experience obtained in Chap. 4. The thrust and moment control given by the three-channel controller are mapped to the propagating wave parameters of the undulatory fins to complete the closed-loop control of the robot. The three-dimensional LOS guidance system is the core component of the waypoint tracking controller proposed in this section, which plays the role of planning the motion of robot. Based on the idea of finite-state machine (FSM), the LOS guidance system can give the desired course angle and desired depth of the underwater bio-inspired robot according to the current position and the waypoint library.

6.1 Waypoint Tracking Control

131

Fig. 6.3 Waypoint tracking FSM

The workflow of the three-dimensional LOS guidance system can be described as two steps: (1) Select the LOS point on the desired path as the target point for the next moment of the robot approaching. The three-dimensional LOS guidance system takes the form of the FSM. First, the waypoint tracking control process is divided into four phases: initial guidance, switching guidance, line-segment guidance, and final guidance phases. Then, the LOS point selection strategy is designed for each phase. The phases transition process is illustrated in Fig. 6.3. Figure 6.4a illustrates the robot’s progress from the initial position into the ball of the first waypoint, which is called the initial guidance phase. The first waypoint serves as the foresight point of the LOS during this process, namely plos (xd , yd , z d ) = p1 (x1 , y1 ,√ z 1 ). Once the BUV enters the ball of the first waypoint E p0 p1 < γ1 (where E p0 p1 = (x0 − x1 )2 + (y0 − y1 )2 + (z 0 − z 1 )2 , p0 (x0 , y0 , z 0 ) is current position of the robot), the initial guidance ends and the switching guidance phase begins. Figure 6.4b illustrates the switching guidance phase. Here, L pi−1 pi represents the current tracking path, and L pi pi+1 represents the next tracking path. The foresight point is selected as the point on the segment L pi pi+1 that is the distance R from the path point pi . The following equation is solved: 

(x − xi )2 + (y − yi )2 + (z − z i )2 = R 2 . y−yi z−z i x−xi = yi+1 = zi+1 xi+1 −xi −yi −z i

(6.1)

The foresight point of the switching guidance phase is obtained as follows: ⎧ R Λ= √ ⎪ ⎪ (xi+1 −xi )2 +(yi+1 −yi )2 +(z i+1 −z i )2 ⎪ ⎨ xd = xi + Λ(xi+1 − xi ) , ⎪ ⎪ yd = yi + Λ(yi+1 − yi ) ⎪ ⎩ z d = z i + Λ(z i+1 − z i )

(6.2)

where R > 0 is adjustable parameter.√ Once the robot enters the plos ball, namely E p0 plos < R (where E p0 plos = (x0 − xd )2 + (y0 − yd )2 + (z 0 − z d )2 ),

132

6 Path Tracking Control of the Underwater Biomimetic Robot

Fig. 6.4 Definitions of the waypoint tracking phases. a Initial guidance. b Switching guidance. c Line-segment guidance. d Final guidance

the switching guidance ends and the line-segment guidance begins, as described in Fig. 6.4c. In the line-segment guidance phase, the objective of this process is to reduce the distance error between the robot and the desired path. As shown in Fig. 6.5, the virtual neighborhood ball of radius R > 0, whose center is the robot’s current position p0 (x0 , y0 , z 0 ), is assumed. And the currently tracked path is the line-segment L pi pi+1 determined by path points pi and pi+1 . If the distance between the robot and the current tracking path segment is less than R, the virtual ball associated with the robot will intersect with the current tracking line-segment in the solution of the following equations: 

(x − x0 )2 + (y − y0 )2 + (z − z 0 )2 = R 2 . y−yi x−xi z−z i = yi+1 = zi+1 xi+1 −xi −yi −z i

(6.3)

6.1 Waypoint Tracking Control

133

Fig. 6.5 Line-segment guidance phase of the waypoint tracking

At this point, one of the two crossing points is selected as the LOS foresight point. The distance between this crossing point and pi+1 is shorter than the one between the other crossing point and pi+1 . The foresight point plos (xd , yd , z d ) is selected as follows: ⎧ A = (xi+1 − xi )2 + (yi+1 − yi )2 + (z i+1 − z i )2 ⎪ ⎪ ⎪ ⎪ ⎪ B = (xi − x0 )(xi+1 − xi ) + (yi − y0 )(yi+1 − yi ) + (z i − z 0 )(z i+1 − z i ) ⎪ ⎪ ⎪ 2 2 2 2 ⎪ C ⎨ = (xi −√ x0 ) + (yi − y0 ) + (z i − z 0 ) − R 2 . (6.4) Λ = −B+ AB −AC ⎪ ⎪ ⎪ xd = xi + Λ(xi+1 − xi ) ⎪ ⎪ ⎪ ⎪ y = yi + Λ(yi+1 − yi ) ⎪ ⎪ ⎩ d z d = z i + Λ(z i+1 − z i ) If the distance between the robot and the current tracking path segment is greater than or equal to R, the virtual ball associated with the robot does not intersect the line-segment L pi pi+1 or intersects line-segment L pi pi+1 only at one point. At this point, the point that is both on the current tracking line-segment and closest to the robot’s current location is selected as the LOS foresight point, namely the crossing point of the line passing through the robot’s current position and perpendicular to the current tracking line-segment and the current tracking line-segment, which is solved as follows:

134



6 Path Tracking Control of the Underwater Biomimetic Robot

(x − x0 )(xi+1 − xi ) + (y − y0 )(yi+1 − yi ) + (z − z 0 )(z i+1 − z i ) = 0 . y−yi z−z i x−xi = yi+1 = zi+1 xi+1 −xi −yi −z i

(6.5)

The current foresight point plos (xd , yd , z d ) is selected as follows: ⎧ Λ = (x0 − xi )(xi+1 − xi ) + (y0 − yi )(yi+1 − yi ) + (z 0 − z i )(z i+1 − z i ) ⎪ ⎪ ⎨ xd = xi + Λ(xi+1 − xi ) . (6.6) ⎪ yd = yi + Λ(yi+1 − yi ) ⎪ ⎩ z d = z i + Λ(z i+1 − z i ) Note that the foresight point is related to the current position of the robot during this process, namely the foresight point changes along with the progress of the robot. Once the robot enters the ball of pi+1 , namely E p0 pi+1 < γi+1 , line-segment guidance ends. At this time, if pi+1 is the final point pn , final guidance begins, as shown in Fig. 6.4d. Otherwise, switching guidance begins again. In the final guidance phase, the objective of this process is to control the robot reach the last path point as far as possible. Hence, the final waypoint pn is selected as the LOS foresight point, namely plos (xd , yd , z d ) = pn (xn , yn , z n ). (2) Determine the desired course angle and desired depth. In order to control the robot to swim toward the LOS foresight point, the desired course angle ψd of the robot can be defined as the direction angle of the projection of the vector pointing to the LOS point in the horizontal plane, as shown in Fig. 6.5. The desired course is given as  ψd =

xe = 0 π + 21 sgn(y  e ) · π, , ye 1 1 arctan xe + π + 2 sgn(ye ) · π − 2 sgn(xe )sgn(ye ) · π, xe /= 0 (6.7)

where xe = x0 − xd , ye = y0 − yd , sgn(·) denotes symbolic function, and sgn(0) = 1. In addition, when there is constant turbulence in the waypoint tracking control process, the use of the above desired course angle will lead to a horizontal distance static error between the actual motion trajectory of the robot and the desired path. In order to eliminate the horizontal distance static error, a compensation angle related to the horizontal distance error is proposed based on the desired course angle described in formula (6.7). Finally, the desired course is given as ψD = ψd + c0 tanh(c1 ex y ) · f lr ,

(6.8)

where ex y is the horizontal distance between the robot and the current tracking path, c0 and c1 are adjustable parameters. flr is symbolic function, defined as

6.1 Waypoint Tracking Control

135

⎧ ⎨ 1, The robot is on the left side of the current tracking path flr = 0, The robot is on the current tracking path . ⎩ −1, The robot is on the right side of the current tracking path

(6.9)

Finally, the desired depth is selected as the depth of the foresight point. The objective of speed control is to enable the underwater bio-inspired robot to complete the entire waypoint tracking control process at a set speed. In this section, an ADRC controller is used to provide a speed control method for the robot. Based on Chap. 4, the dynamic model of the robot in velocity channel can be obtained as

u˙ = f u (u, d(t), t) + bu τu , u = ud

(6.10)

22 where bu = m111 is proportion factor of input, f u (u, d(t), t) = − md1111 u + m vr + m 11 1 τ , u d denotes the desired velocity. Similar to the depth and course controller m 11 du proposed in Chap. 4, the discrete time depth ADRC controller can be summarized as follows: ⎧ u 1 (k + 1) = u 1 (k) + h u u 2 (k) ⎪ ⎪ ⎪ ⎪ ⎪ u (k + 1) = u 2 (k) + h u fhan(u 1 − u d , u 2 , δu , h u0 ) ⎪ ⎪ 2 ⎪ ⎪ ⎨ eu = ζu1 (k) − u(k) ζu1 (k + 1) = ζu1 (k) + h u [ζu2 (k) − 2ωou eu + bu0 τu ] , (6.11) ⎪ 2 ⎪ ⎪ (k + 1) = ζ (k) − h ω e ζ u2 u2 u u ⎪ ou ⎪ ⎪ ⎪ τu0 = ku [u 1 (k + 1) − ζu1 (k + 1)] ⎪ ⎪ ⎩ τu = τu0 −ζbu2u0(k+1)

where h u is the discretization period, u d denotes the desired velocity, u 1 is the tracking signal of u d , u 2 is the differential signal of u 1 , u is current velocity. δu is the time factor determining the tracking speed. h u0 is the filtering factor. ζu1 is the estimated value of robot velocity, ζu2 is the estimated value of general disturbance in velocity channel, ωou represents the bandwidth of the ESO. bu0 is the estimated value of bu , ku represents proportional gain. The objective of depth control is to enable the robot to quickly track the desired depth given by the three-dimensional LOS guidance systems. The depth controller used in this section has the same form as the depth controller given in formula (4.22). The objective of course control is to enable the robot to quickly track the desired course given by the three-dimensional LOS guidance systems. The course controller used in this section has the same form as the course controller given in formula (4.27). The control quantities given by the above three-channel controller are surge thrust τu , heave thrust τw and yaw moment τr , respectively. However, the actual control input of the underwater bio-inspired robot is the propagating wave parameters of the left and right undulatory fins. Through the trust and yaw moment to propagating wave parameter mapping model based on fuzzy inference, the trust and yaw moment

136

6 Path Tracking Control of the Underwater Biomimetic Robot

control quantities given by the above three-channel controller are mapped into the propagating wave parameters of the undulatory fins.

6.1.3 Experiments and Analysis In order to verify the feasibility and effectiveness of waypoint tracking, the waypoint tracking experiments were conducted in this section. In the three-dimensional waypoint tracking control simulation, the desired path is described by the following three-dimensional path points: p1 = (1.5, 1.0, 0.64), p2 = (2.7, 1.0, 0.54) p3 = (3.8, 1.5, 0.44), p4 = (3.8, 2.5, 0.34) . p5 = (2.7, 3.0, 0.44), p6 = (1.5, 3.0, 0.54) In the process of waypoint tracking, the desired velocity of the robot was set as u d = 0.1 m/s. The initial pose of the robot was (x0 , y0 , z 0 , ψ0 ) = (0.802 m, 1.802 m, 0.644 m, 5.472 rad), and the initial velocity was (u 0 , v0 , w0 , r0 ) = (−0.067 m/s, 0.060 m/s, −0.011 m/s, 0.098 rad/s). The control period of the waypoint tracking controller was 0.2 s. The parameters of the LOS guidance system were set as follows: The neighborhood ball radius of each path point except the last path point was 0.2 m, and the last one was 0.05 m, namely γi = 0.2(i = 1, 2, ..., 5), γ6 = 0.05. The neighborhood ball radius of the robot was R = 0.2 m. In the three-dimensional waypoint tracking control experiment, the depth information of the underwater bio-inspired robot is converted from the underwater pressure information, and the horizontal position and attitude information (in this case, only the heading angle) is obtained by collecting and processing the image information of the robot through the global visual system. Note that in the three-dimensional waypoint tracking control experiment, the robot is controlled to dive into the water along the desired path, resulting in the global vision system collecting the image that is the real image of the robot after refraction into the virtual image of the camera. Using this virtual image information directly to obtain the horizontal position information of the robot will result in a large horizontal position error, which will affect the control effect of path point tracking. Therefore, in the three-dimensional waypoint tracking control experiment, we eliminate the position error caused by refraction effect by combining visual information and depth information. As described in Fig. 6.6, the Z E -axis of the world coordinate system O E − X E Y E Z E is perpendicular to the water surface, and the plane X E Y E coincides with the water surface. The global camera is suspended at the top of the center of the pool, with the optical axis perpendicular to the water surface, the optical axis intersecting the image plane at the point Oc , and the optical axis intersects water surface at the point OW . The coordinates (x W , yW , z W ) of the point OW in the world coordinate

6.1 Waypoint Tracking Control

137

Fig. 6.6 Global visual system

system can be obtained by measuring. The distance z C between point OC and point OW can be obtained by camera calibration, namely |OC OW | = z C . p0 (x0 , y0 , z 0 ) denotes current position of the robot. The depth z 0 can be obtained by converting the pressure sensing information. Suppose the image of the robot enters the camera along the light p0 I and I OC with the crossover point I of light and water, incidence angle θW , and refraction angle θ A . The coordinate (x I , y I , z I ) of point I in the world coordinate system O E − X E Y E Z E can be calculated by image processing and using the internal and external parameter matrices of the global visual system. According to the law of refraction, √ |I OW | | p0 H |2 + |H I |2 nW sin(θ A ) √ = , = nA sin(θW ) | p0 H | |I OW |2 + |OC OW |2

(6.12)

where n W is the refractive index of light in water and n A is the refractive index of light in air. According to formula (6.12), the distance | p0 H | between the robot and the light incidence normal can be obtained as follows: 

√ |I OW | = (x I − x W )2 + (y I − yW )2 . | p0 H | = √ 2 n A2z0 |I OW |2 2 2

(6.13)

(n W −n A )|I OW | +n W z C

Then, according to the geometric relationship, 

x0 −x I x0 −x W y0 −y I y0 −yW

= =

| p0 H | | p0 H |+|I OW | | p0 H | | p0 H |+|I OW |

.

(6.14)

Therefore, the actual three-dimensional coordinate of the underwater bio-inspired robot is obtained as follows:

138

6 Path Tracking Control of the Underwater Biomimetic Robot

Fig. 6.7 Desired path and actual path in x–y–z space

⎧ ⎪ ⎨ x0 = x I + y0 = y I + ⎪ ⎩ z0 = z0

| p0 H | (x |I OW | I | p0 H | (y |I OW | I

− xW ) − yW ) .

(6.15)

Figure 6.7 shows the desired and actual paths of the robot. Figure 6.8 shows the time history of the cross-track error. Figure 6.9 shows the time history of the velocity. Figure 6.10 shows the time history of parameters of undulatory fins. The results show that the waypoint tracking control method given in this section can effectively control the robot to move along the desired three-dimensional path, indicating the effectiveness of the proposed waypoint tracking controller.

6.2 Path Tracking Control Waypoint tracking is to control the underwater bio-inspired robot to reach a specified point in space, and the objective of path tracking is to control the robot to follow a planned spatial track. This section discusses two-dimensional path tracking control for the underwater bio-inspired robot.

6.2.1 Reference Coordinate System In order to analyze and describe the position and orientation of the underwater bioinspired robot conveniently, the corresponding coordinate system and symbol rules are established firstly. In this chapter, the two-dimensional path tracking control of

6.2 Path Tracking Control

Fig. 6.8 Cross-track error

Fig. 6.9 Velocity of the robot

139

140

6 Path Tracking Control of the Underwater Biomimetic Robot

Fig. 6.10 Parameters of undulatory fins

the robot is proposed; hence, the motion of the underwater bio-inspired robot is limited to a horizontal plane. The origin of the world coordinate system can be chosen arbitrarily, but for discussion purposes, the origin of the world coordinate system in this chapter is selected as a vertical line on the top left vertex of the water surface of the pool, as shown in Fig. 6.11. The X E -axes and Y E -axes are parallel to the sides of the pool. Moreover, the origin of the body coordinate system is chosen as the center of gravity of the robot. The X B -axis points to the head of the robot and the Y B -axis points to the right side of robot. The position of the underwater bio-inspired robot is described by the coordinates [x, y, ψ]T in the world coordinate system. Among them, the course angle ψ is defined as the included angle between the X B -axis of the body coordinate system and X E -axis of the world coordinate system. The linear velocities of the underwater ˙ T . In the ˙ y˙ , ψ] bio-inspired robot in the world coordinate system are denoted as [x, body coordinate system, the linear velocities are expressed as ν = [u, v, r ]T .

6.2.2 Problem Statement In the path tracking task, the main objective is to control the underwater bio-inspired robot swim along the planned path accurately, as shown in Fig. 6.12. In this section, the path tracking control problem can be described as follows. p(x, y) denotes the position of the robot in the world coordinate system O E X E Y E . ψ is the course angle

6.2 Path Tracking Control

141

Fig. 6.11 Schematic diagram of the coordinate system

of the robot in the world coordinate system. The planned reference path is described by the parameter s. The goal of path tracking control is to design the control quantity τ = [τu 0 τr ]T , which can control the centroid position p(x, y) of the robot converge globally and progressively to the reference path, namely tracking error is globally terminated and bounded, and the velocity vector is tangent to the reference path. Moreover, the forward speed u d can be adjusted online, and the robot can reach the end of the reference path.

Fig. 6.12 Schematic of the path tracking

142

6 Path Tracking Control of the Underwater Biomimetic Robot

6.2.3 The Controller Design In order to solve the above problem, a path tracking control paradigm is proposed as illustrated in Fig. 6.13. The integrated control design mainly consists of three parts. The first part is a guidance system based on LOS principle, which is implemented to deduce the foresight point plos . The robot needs to track currently in real time based on reference path and position feedback p(x, y). The second part is a backstepping (BP) controller by using foresight point plos and position/orientation feedbacks. In particular, the BP controller outputs propulsive force τu and torque τr , which are further mapped to the parameter settings of propagating waves on bilateral fins to force the robot to converge to the current foresight point plos based on fuzzy inference. A detailed description about the controller design is presented as follows. An LOS guidance system is implemented in the path following control to accomplish motion planning of the robot. Specially, straight line and circular paths are considered. The notations of the LOS guidance system are depicted in Fig. 6.14. Assume that there is a virtual circle associated with the robot, whose center is located in the barycenter of the robot p(x, y) and the radius is γ > 0. If the distance between the robot and current reference path is less than γ , the virtual circle intersects the refer, , plos (xd , yd ). The intersection point plos (xd , yd ) further ence path at two points plos forward along the path is served as the foresight point. If the distance between the robot and current reference path is greater than or equal to γ , the virtual circle does not intersect or intersects only one of the current reference path. The point, which is both on the current tracking reference path and closest to the virtual circle, is chosen as the foresight point. After the LOS point is selected, the tracking error can be written as follows: ⎧ xe = x − xd ⎪ ⎪ ⎪ ⎪ ⎪ ye = y − yd ⎪ ⎪ / ⎪ ⎨ ex y = xe2 + ye2 , ⎪ / ⎪ ⎪ ⎪ ⎪ e = xe2 + ye2 + z e2 ⎪ ⎪ ⎪ ⎩ ψe = ψ − ψd

Fig. 6.13 Block diagram of the path tracking controller

(6.16)

6.2 Path Tracking Control

143

Fig. 6.14 Modeling of the LOS guidance system

where ex y denotes the planar distance error and ψe is course angle error. The kinematics equation of motion of the robot can be rewritten as follows: ⎧ ⎨ e˙x y = −u cos(ψe ) + vsin(ψe ) cos(ψe ) , sin(ψe ) ⎩ ψ˙ e = r + u+ v ex y ex y

(6.17)

Based on the formula (6.17), the objective is to control the above errors converge to zero gradually under the action of the controller, as follows: ψe → 0 ex y → γ (γ > 0).

(6.18)

With the formulation developed previously, kinematic control is first designed. Consider the nonlinear, invariant system in closed-loop with the control law as follows: ⎧ n ⎨ u = k1 (ex y − δ) cos (ψe ) (6.19) cos(ψe ) ⎩r = − v − k2 ψe ex y where u is surge velocity, r is yaw angular velocity, δ is an arbitrary small positive constant, n is a natural number, and k2 > k1 > 0. Therefore, the tracking error ψe can converge to zero, ex y can converge to δ, and z e can converge to zero. The objective of dynamic level control is to control the actual speed of the robot reach the desired speed by adjusting the surge thrust, heave thrust, and yaw moment, as follows: ⎧ n ⎨ α1 = k1 (ex y − γ ) cos (ψe ) . (6.20) cos(ψe ) ⎩ α2 = − v − k2 ψe ex y

144

6 Path Tracking Control of the Underwater Biomimetic Robot

According to the model of the underwater bio-inspired robot system described in formula (4.10), the differential velocity equation can be described as 

u˙ = r˙ =

1 (τ + m 22 vr − d11 u) m 11 u 1 (τ + (m 11 − m 22 )uv m 33 r

− d33 r )

.

(6.21)

The control objective is to converge β1 , β2 to zero, where β1 = u−α1 , β2 = r −α2 . Therefore, the control law is designed based on formula (6.23), as follows: ⎧ ⎨ τu = −m 22 vr +d11 u+m 11 α˙ 1 − m 11 sin(ψe ) ψe − k3 β1 , k3 > 0 ex y , ⎩ τr = (m 22 − m 11 )uv + d44 r +m 44 α˙ 3 − m 44 ψe − k4 β2 , k4 > 0

(6.22)

6.2.4 Experiment and Analysis In order to verify the feasibility and effectiveness of path tracking, the path tracking experiments were conducted in this section.

6.2.4.1

Straight Path Tracking

A straight-line path tracking experiment is first carried out. The slope and intercept of the straight line were 1 and − 0.5 m, respectively, while the initial states of the robot were (x0 , y0 , ψ0 ) = (0.72 m, 0.836 m, 0.25 rad) and (u 0 , v0 , r0 ) = (0.017 m/s, 0.037 m/2, −0.128 rad/s). The parameters of the path tracking controller were selected as given in Table 6.1. The control period was 0.05 s. The real-time status feedback of the position, course, and speed of the robot were obtained through the image processing of the global visual camera. Experimental results of the straight-line path tracking are shown in Figs. 6.15, 6.16, and 6.17. The clip of path tracking experiment is depicted in Fig. 6.15, where the red dotted line is the path to be followed and the pink curve indicates the trajectory of the robot. It is observed that the proposed path following control system is able to force the robot to catch up and land on the desired path smoothly with small overshoot. Then, the robot moves along the desired path and the cross-track error between the desired path and the actual trajectory is small. Figure 6.16 shows the time Table 6.1 Parameters of straight-line path tracking control k1

k2

k3

k4

γ

δ

n

c0

c1

0.12

1.8

50

7

0.6

0.04

1

1.5

1

6.2 Path Tracking Control

145

Fig. 6.15 Image sequence of the straight-line path following

evolution of the control parameters of propagating waves on bilateral fins. Notice that the amplitudes of the bilateral long fins maintain at 30° and the turning motion of the robot is realized by regulating different frequency of two long fins. The path tracking error is shown in Fig. 6.17. When the robot reaches the desired path, the maximum tracking error is only about 0.05 m.

6.2.4.2

Circular Path Tracking

Another experiment is to track a circular path. The initial position and course angle of the robot were (0.828 m, 2.080 m, 6.20 rad), respectively, while the initial velocity is (0.012 m/s, − 0.008 m/s, 0.113 rad/s). The parameters of the path tracking controller were selected as given in Table 6.2. The control period was still 0.05 s.

146

6 Path Tracking Control of the Underwater Biomimetic Robot

Fig. 6.16 Control parameters of propagating waves on bilateral fins

Fig. 6.17 Path tracking error Table 6.2 Parameters of circular path tracking control k1

k2

k3

k4

γ

δ

n

c0

c1

0.7

2.1

50

5

0.45

0.04

1

1

0.95

6.2 Path Tracking Control

147

Fig. 6.18 Image sequence of the circular path tracking experiment

Figure 6.18 gives the image sequence of the circular path following experiment. The robot converges to the circular path and moves along the desired path. Figure 6.19 shows the parameter settings of propagating waves on bilateral fins. Note that the frequency difference is always large to achieve stable turning maneuver. Furthermore, we evaluate the proposed method with the typical PID controller. Comparative experiment results are shown in Figs. 6.20 and 6.21. Figure 6.20 shows the desired path and actual path of the robot in the comparative experiment. Figure 6.21 shows the change curve of tracking error. The experimental results show that the path tracking error obtained by the BP method is smaller than that obtained by the PID method. Specifically, the maximum cross-track error of the proposed method after the robot converges to the desired path is 0.07 m, while the maximum tracking errors is 0.121 m using the PID method. It can be concluded that the proposed path following control paradigm is more accurate and effective.

148

6 Path Tracking Control of the Underwater Biomimetic Robot

Fig. 6.19 Control parameters of propagating waves on bilateral fins

Fig. 6.20 Desired path and actual path in comparative experiment results of circular path following

References

149

Fig. 6.21 Cross-track errors

References 1. Ang KH, Chong G, Li Y (2005) PID control system analysis, design, and technology. IEEE Tran Control Syst Technol 13(4):559–576 2. Antonelli G, Caccavale F, Chiaverini S (2004) Adaptive tracking control of underwater vehiclemanipulator systems based on the virtual decomposition approach. IEEE Trans Robot Autom 20(3):594–602 3. Healey AJ, Lienard D (1993) Multivariable sliding mode control for autonomous diving and steering of unmanned underwater vehicles. IEEE J Oceanic Eng 18(3):327–339 4. Sun B, Zhu D, Yang SX (2014) A bioinspired filtered backstepping tracking control of 7000-m manned submarine vehicle. IEEE Trans Industr Electron 61(7):3682–3693 5. Lian R-J (2011) Intelligent Controller for Robotic Motion Control. IEEE Trans Industr Electron 58(11):5220–5230 6. Wang R, Wang S, Wang Y, Tan M, Yu J (2019) A paradigm for path following control of a ribbon-fin propelled biomimetic underwater vehicle. IEEE Trans Syst Man Cybern Syst 49(3):482–493 7. Wang R, Wang Y, Wang S, Tang C, Tan M (2018) Switching control for 3-D way-point tracking of a biomimetic underwater vehicle. Int J Offshore Polar Eng 28(3):255–262 8. Wang R, Wang S, Wang Y, Tang C (2017) Path following for a biomimetic underwater vehicle based on ADRC. In: 2017 IEEE international conference on robotics and automation, pp 3519–3524 9. Wang R, Wang S, Wang Y, Wei Q (2016) Way-point tracking control for a biomimetic underwater vehicle based on backstepping. In: 35th Chinese control conference, pp 5970–5975 10. Wang R, Wang S, Wang Y, Tang C, Tan M (2018) Three-dimensional helical path following of an underwater biomimetic vehicle-manipulator system. IEEE J Oceanic Eng 43(2):391–401

Chapter 7

Autonomous Grasping Control of the Underwater Biomimetic Vehicle-Manipulator System

This chapter introduces the autonomous grasping control method of the underwater biomimetic vehicle-manipulator system (UBVMS) based on visual positioning. The method includes two parts: hand–eye coordination control of the underwater manipulator and coordination planning and control of the UBVMS.

7.1 Hand–Eye Coordination Control of the Underwater Manipulator 7.1.1 Autonomous Grasping Control of Manipulator The diagram of the vision-based control system is depicted in Fig. 7.1. The components of this control system include image processing, position filtering, inverse kinematics, angle conversion, and servo motion control. As an input to the control system, a vision system with two cameras is used to measure the position of the target. During arm motions, the coupling between the manipulator and the vehicle alters the UVMS’s orientation. Therefore, the goal position in the O0 X 0 Y0 Z 0 frame must be continuously updated. Image processing is used for target identification and location calculation. Location filtering is used to increase the accuracy of the visual system’s calculated target position. The conversion from θi to θMotori (i = 1, 2, 3, 4, 5), which is derived from the relationship between θi and θMotori , is accomplished using angle conversion. Since the joint positions of an underwater manipulator need high speed and absolute positional precision, two closed-loops of the motor, comprising the velocity loop and the position loop, are used to achieve dc servo motor control. In Fig. 7.3, these two closed-loops are indicated. Their respective feedback signals are derived from the optical encoder data difference. The details of inverse kinematics and image processing follow. © National Defense Industry Press 2023 S. Wang et al., Underwater Biomimetic Vehicle-Manipulator System, Unmanned System Technologies,https://doi.org/10.1007/978-981-99-0655-0_7

151

7 Autonomous Grasping Control of the Underwater Biomimetic …

Fig. 7.1 Block diagram of the vision-based control system

152

7.1 Hand–Eye Coordination Control of the Underwater Manipulator

7.1.1.1

153

Kinematics and Inverse Kinematics of the Manipulator

Since Denavit–Hartenberg (D–H) [3] reference frames are shown in Fig. 7.2, it is simple to determine the kinematics of the underwater manipulator using the D–H parameters model. Hence, the position and orientation of the end effector in the O0 X 0 Y0 Z 0 frame are obtained by

0

TE =

0

R E 0 PE 0 1

 = A1 (θ1 , a1 ) · A2 (θ2 , a2 ) · A3 (θ3 ) · A4 (θ4 , d4 ),

where 0 R E = [N E , O E , A E ] and 0 PE = [ px , p y , pz ]T are the orientation matrix and the position vector of the end effector in the O0 X 0 Y0 Z 0 frame, respectively; A1 , A2 , A3 , and A4 are the transformation matrices of the link frames. Here, N E = [n x , n y , n z ]T , O E = [ox , o y , oz ]T , and A E = [ax , a y , az ]T . Specific parameters of D–H model are listed in Table 7.1. Therefore, forward kinematic equations are obtained.

Fig. 7.2 D–H reference frames

Table 7.1 D–H parameters of underwater manipulator 连杆

◦

θ( )

d (mm)

◦

a (mm)

α( ) 90

1

θ1

0

67

2

θ2

0

189.95

3

θ3 + 90

0

0

90

4

θ4

323

0

0

0

154

7 Autonomous Grasping Control of the Underwater Biomimetic …

⎧ ⎨ px = cos θ1 (d4 cos(θ2 + θ3 ) + a1 + a2 cos θ2 ) p = sin θ1 (d4 cos(θ2 + θ3 ) + a1 + a2 cos θ2 ) . ⎩ y pz = a2 sin θ2 + d4 sin(θ2 + θ3 )

(7.1)

By the derivation of (7.1), we can get pz sin θ2 + D cos θ2 = E,

(7.2)

where D= E=

/

px2 + p 2y − a1 / ( px2 + p 2y − a1 )2 + a22 + pz2 − d42 2a2

.

(7.3)

Considering the field of view of the cameras, the ranges of θ1 , θ2 , and θ3 should be restricted to the respective values: ⎧ ◦ ◦ ⎨ θ1 ∈ (−30 , 30 ) ◦ . θ ∈ (0, 90 ) ⎩ 2 ◦ ◦ θ3 ∈ (−150 , 0 )

(7.4)

Therefore, θ2 can be determined by (7.2) and (7.4). Then, θ1 and θ3 are computed by substituting θ2 into (7.2). According to (7.2), we can get 0

TE · A−1 4 = A1 · A2 · A3 = l4×4 ,

(7.5)

where l4×4 is a 4 × 4 matrix related to the parameters θ1 , θ2 , θ3 , a1 , a2 which are known. Moreover, Eq. (7.6) as shown below: T A−1 = ⎡E 4 n x cos θ4 − ox sin θ4 ox cos θ4 + n x sin θ4 ⎢ n y cos θ4 − o y sin θ4 o y cos θ4 + n y sin θ4 ⎢ ⎣ n z cos θ4 − oz sin θ4 oz cos θ4 + n z sin θ4 0 0

0

⎤ ax px − ax d4 a y p y − a y d4 ⎥ ⎥ az pz − az d4 ⎦ 0 1

(7.6)

Therefore, θ4 could be calculated by (7.5) and (7.6).

7.1.1.2

Target Recognition and Localization Based on Binocular Vision

Once the positions of the target’s center in the images collected by the two cameras are known, the vision system is utilized. And the position of the target within the frame O0 X 0 Y0 Z 0 might be naturally calculated using a 3-D reconstruction method.

7.1 Hand–Eye Coordination Control of the Underwater Manipulator

155

Therefore, we briefly discuss the estimation of the center position of the target in the image, as depicted in Algorithm 7.1, where Tarea is the threshold value of the region of interest (ROI). As a matter of convenience, the red-colored object is chosen as the target. Applying threshold segmentation to acquire the target region, which is defined as S pic = S1 ∩ S2 S1 = ((i, j ) | Pi,Rj > T1 , Pi,Rj > krg Pi,Gj , Pi,Rj > kr b Pi,Bj ) , S2 = ((i, j) | Pi,Rj − Pi,Gj > T2 , Pi,Rj − Pi,Gj > T3 )

(7.7)

where Pi,Rj , Pi,Gj , and Pi,Bj are the values of R, G, B components of pixel point (i, j), respectively, krg is the threshold value of the ratio of R component to G component, kr b is the threshold value of the ratio of R component to B component, and T1 , T2 , T3 are the threshold values.

Algorithm 7-1 the determination of the position of the target center 1: while (1) do 2: Image capture. 3: get the ROI based on image segmentation. 4: if (the ROI is smaller than Tarea ) then 5: Break. 6: else 7: Canny edge detection algorithm for the ROI image. 8: get the binary image. 9: extract all the contours in the binary image. 10: compute the area of every contour. 11: find the contour with the maximum area as the target contour. 12: compute the center of the target contour. 13: end if 14: end while The target center in the images of the two cameras should be determined at first to get the target position in the O0 X 0 Y0 Z 0 frame. Because certain pixels with identical color characteristics in the image may incorrectly belong to the target, all of the image’s contours must be extracted first in the binary image, and the contour with the biggest area is selected as the target. As the point of the image’s target center, the center of all contour pixels is used. Thus, utilizing a visual positioning system, the location of the target’s center in O0 X 0 Y0 Z 0

156

7 Autonomous Grasping Control of the Underwater Biomimetic …

Fig. 7.3 Image processing. a Left camera. b Right camera

frame is determined. The determination of the underwater target in the pictures obtained by the two cameras is shown in Fig. 7.3. Thus, the location of the target’s center in O0 X 0 Y0 Z 0 frame may be determined using the 3-D reconstruction approach described in [4]. Note that the parameters of the cameras need to be calibrated in water.

7.1.2 Experiment and Results To demonstrate the validity of the mechanical structure, the experiment of autonomous free-floating manipulation is conducted in a swimming pool. The system processor is a 1.8 GHz Pentium-class PC104 controller with 1 GB of RAM, and the front of the vehicle is equipped with two cameras. The calibration of the intrinsic and external parameters of the vision system, as well as the hand-eye calibration, have been accomplished prematurely in water. In the experiment, we just do autonomous manipulation and disregard UVMS motion. Thus, a red, rope-bound target is placed in the vision system, the target’s position is computed, and autonomous free-floating manipulation is achieved. We arrange the target at random initial positions within the binocular system’s field of view, and then autonomous execution commences. Figure 7.4 illustrates the relationship between execution time and the distance between the beginning positions of the target and gripper. The average execution time for approaching the target and closing the gripper is 2.96 s. The autonomous operation is deduced when the initial position of target in the O0 X 0 Y0 Z 0 frame is (470.23, − 26.25, − 175.63). Consecutive snapshots of freefloating autonomous manipulation are given in Fig. 7.5, where Fig. 7.5a–f shows the process of approaching the target, Fig. 7.5g–i shows the process of closing the gripper, and Fig. 7.5j–l shows the process of returning to the initial position. The graph reflecting time history of the positions of target and end effector in the O0 X 0 Y0 Z 0 frame is depicted in Fig. 7.6, where D is defined as the distance between the target and end effector. The X value of the target in the O0 X 0 Y0 Z 0 frame is progressively

7.2 Coordination Planning and Control of the Underwater Biomimetic …

157

Fig. 7.4 Execution time versus the distance between the initial position of target and the initial position of gripper. Execution time versus the distance between the initial position of target and the initial position of gripper. T is the execution time, and D0 represents the distance between the initial location of the target and the initial position of the gripper

smaller see (Fig. 7.6a), which illustrates that forward movement of the vehicle with respect to the target is produced during arm maneuvers. In addition, the changing rules of the Z value of the target reveal that the UVMS head is pitched upward during manipulator motions and then returns to its previous angle after the approach is completed (see Fig. 7.6). We give experimental evidence that the innovative design results in a tiny dynamic coupling between the manipulator and the vehicle, even when the vehicle’s weight in air is not much more than that of the manipulator. Meanwhile, autonomous execution is accomplished rapidly.

7.2 Coordination Planning and Control of the Underwater Biomimetic Vehicle-Manipulator System In this section, the coordinated planning and control method of the underwater biomimetic vehicle-manipulator system based on multi-task priority allocation is presented, and autonomous grasping is realized in the swimming pool.

158

7 Autonomous Grasping Control of the Underwater Biomimetic …

(a)

(b)

t=0.000s (d)

(g)

(j)

t=1.000s

t=2.000s

t=3.000s

(e)

(c)

t=0.333s

t=0.667s (f)

t=1.333s (h)

(k)

(i)

t=2.333s

(l)

t=3.333s

t=1.667s

t=2.667s

t=3.667s

Fig. 7.5 Consecutive snapshots of free-floating autonomous manipulation

7.2.1 Coordination Planning and Control Based on Task-Priority Algorithm 7.2.1.1

Motion Planning

In an unstructured underwater environment, the UBVMS will inevitably wander. Traditional trajectory planning is ineffective when significant and frequent drifting motion is present. When the actual course deviates much from the planned path, trajectory planning must be performed regularly. This will impair the precision and effectiveness of control. To address these deficiencies, an online motion planning system based on tracking differentiator is created. Given the current and projected ultimate states of the UBVMS, the devised motion planning approach creates the next desirable state rather than the full trajectory, which greatly increases the processing efficiency and reduces the cost of computing. During the process of autonomous manipulation, many activities can be viewed as set point jump problems, such as waypoint monitoring during a change in heading direction. For a set point leap, the tracking differentiator might generate a smooth transient profile. The motion planner is meant to create the required trajectory and speed by virtue of the fast tracking differentiator introduced in [5]. The discrete form

7.2 Coordination Planning and Control of the Underwater Biomimetic …

(a) Time history of X values of the target and end-effector.

(c) Time history of Z values of the target and end-effector

159

(b) Time history of Y values of the target and end-effector.

(d) Time history of D

Fig. 7.6 Time history of positions of the target and end effector in the reference O0 X 0 Y0 Z 0 frame. a Time history of X values of the target and end effector. b Time history of Y values of the target and end effector. c Time history of Z values of the target and end effector. d Time history of D, which is defined as the distance between the manipulator end effector and the target

of the motion planner can be written as ⎧ ⎨ f i = f (z i − z 0 , vi , r0 , h 0 ) , = z i + h 0 vi z ⎩ i+1 vi+1 = vi + h 0 f i

(7.8)

where z 0 is the expected point, z i and vi are the generated trajectory and its derivative at time i, r0 and h 0 are the parameters relating with the steepness and smoothness of the trajectory, respectively, and they need to be adjusted accordingly for a specific set point jump problem. f (·) is the time-optimal solution deduced from the discrete double integral plant, and it is [6].

160

7 Autonomous Grasping Control of the Underwater Biomimetic …

⎧ d = r0 h 20 ⎪ ⎪ ⎪ ⎪ a0 = h 0 vi ⎪ ⎪ ⎪ ⎪ ⎪ y = z i + a0 ⎪ ⎪ ⎪ a = √d(d + 8|y|) ⎪ ⎨ 1 , a2 = a0 + sign(y)(a1 − d)/2 ⎪ ⎪ ⎪ = (sign(y + d) − sign(y − d))/2 s y ⎪ ⎪ ⎪ ⎪ a = (a0 + y − a2 )s y + a2 ⎪ ⎪ ⎪ ⎪ s = (sign(a + d) − sign(a − d))/2 ⎪ ⎪ ⎩ a f = −r0 (a/d − sign(a))sa − r0 sign(a)

(7.9)

where sign(·) is sign function. d, a0 , y, a1 , a2 , s y , a, sa are the symbols for denoting the calculation procedure of f (·). f (·) has been optimized by the saturation function to eliminate steady-state oscillations and ensures that the generated trajectory has no overshoot. Figure 7.7 shows the simulation results of the developed motion planning method under different situations. In the cases study, the initial position is 0, the terminal position is 2 and the maximum velocity is limited to 0.15 m/s. Figure 7.7a, b shows the trajectory and velocity under the ideal conditions, i.e., without any drifting in trajectory and velocity. Figure 7.7c, d shows the trajectory and velocity with some random drifting in trajectory. Figure 7.7e, f shows the planned trajectory and velocity. Figure 7.7 depicts the simulation results of the suggested approach for motion planning in various conditions. In the case study, the beginning position is 0 and the final position is 2, with a maximum velocity of 0.15 m/s. Figure 7.7a, b illustrates the trajectory and velocity under ideal conditions, i.e., with no drift in trajectory and velocity. The trajectory and velocity are depicted in Fig. 7.7c, d, with some random drift in trajectory (e) and (f) in Fig. 7.7 depict the projected trajectory and velocity.

7.2.1.2

Kinematic Control Based on Task-Priority Algorithm

The online motion planner calculates the intended trajectory and sends the required task velocity to the kinematic control. The Jacobian connection between the task velocity and the system velocity may be represented as σ˙ = J ζ,

(7.10)

where J ∈ R m×(n 1 +n 2 ) is the Jacobian matrix σˆ ∈ R m is the task velocity vector, and usually m < n 1 + n 2 . Here, n 1 , n 2 are the degree of freedom of the vehicle and manipulator, respectively, and ζ ∈ R n 1 +n 2 is the system velocity vector.

7.2 Coordination Planning and Control of the Underwater Biomimetic …

161

Fig. 7.7 Cases study of the online motion planning based on tracking differentiator

By implementation of the online motion planning, the task desired velocity σˆ d can be obtained. Then, the system reference velocity ζd is able to be found to realize σˆ d at best by solving following optimization problem: min ∥σ˙ d − J ζ ∥2W , ζ

(7.11)

where W is the weighted matrix. Thus, by generalized inverse of the Jacobian matrix, the solution is [7] ζ = J + σ˙ d + (I − J + J )z,

(7.12)

where J + is the generalized inverse of J and z is the arbitrary item. As for underdetermined system of equation such as (7.10), the generalized inverse J + is right inverse based on matrix theory, i.e., J + = W −1 J T (J W −1 J T )−1 . However, J W −1 J T is not invertible when J loses full row rank. Therefore, a singular value-oriented regulated

162

7 Autonomous Grasping Control of the Underwater Biomimetic …

kinematic singularity-robust form is   −1   J + = W −1 J T J W −1 J T + P Σ J W −1 J T , a, b, c ,

(7.13)

where P(Σ(J W −1 J T ), a, b, c)) is the bell-shaped and positive matrix function.     P Σ J W −1 J T , a, b, c =

1 . 1 + (abs(Σ − c)/a)2b

(7.14)

It is parameterized by eigenvalue matrix Σ of the singular value decomposition, i.e., J W −1 J T = U ΣV T , c is the center of the bell-shaped region, a and b determine the shape of the bell-shaped region. This function will prevent J + from growing to infinite and ensure the continuity of the algorithm. The second task can be represented by following Jacobian relationship: σ˙ 2 = J2 ζ,

(7.15)

with J2 ∈ R m 2 ×(n 1 +n 2 ) , σˆ 2 ∈ R m 2 . Analogously, the optimization problem is min ||σ˙ 2 − J2 ζ ||2W . ζ

(7.16)

Equations (7.11) and (7.16) are taken into consideration together, so optimization problem could be rewritten as ζ = ρ1 + Q 1 z 2 : z 2 = min ||σ˙ 2 − J2 ρ1 − J2 Q 1 z 2 ||2W , z2

(7.17)

where ρ1 = J + σ˙ d , Q 1 = I − J + J , Q 1 is null space operator of J . This means that σ˙ 2 = J2 ρ 1 + J2 Q 1 z 2 .

(7.18)

The algorithmic singularity occurs when J2 Q 1 loses full rank. In [8], algorithmic singularity can be avoided by solving following optimization problem instead of (7.17) ζ = ρ1 + Q 1 z 2 : z 2 = min ||J + σ˙ 2 − ρ1 − Q 1 z 2 ||2W . z2

(7.19)

Q 1 owns the properties of Hermitian and idempotent, so the corresponding solution is z 2 = Q 1 J2+ σ˙ 2 + (I − Q + 1 Q 1 )z 3 .

(7.20)

7.2 Coordination Planning and Control of the Underwater Biomimetic …

163

Substitute (7.20) into (7.12) and the singularity-robust iterative solution can be obtained. ζ = ρ1 + Q 1 J2+ σ˙ 2 .

(7.21)

Therefore, the singularity-robust task-priority iteration can be concluded as ρi = ρi−1 + Q i−1 Ji+ σ˙ i .

(7.22)

The above iteration is an open-loop task-priority algorithm. Here, the state observers are designed to estimate state and velocity of given task, providing necessary feedback information. The model of the state observer is expressed as ⎧ e = zˆ i − z i ⎪ ⎪ ⎨ zˆ i+1 = zˆ i + h 0 (ˆvi − β1 e) , ⎪ vˆ i+1 = vˆ i + h 0 (aˆ i − β2 e + J ζ ) ⎪ ⎩ aˆ i+1 = aˆ i + h 0 (−β3 e)

(7.23)

where z i is the actual state at the moment of i of given task, zˆ i , vˆ i , aˆ i are the estimations of state, velocity, acceleration of given task, β1 , β2 , β3 are the parameters ensuring the convergence of the control. Here, the task-priority projection can be abstracted into a virtual plant, and q˙ is the output of this virtual system, i.e., the system reference velocity. J is the corresponding Jacobian relationship matrix. The feedback law is represented as ⎧ ⎨ e1 = zˆ i − z d . e = vˆ i − σ˙ di ⎩ 2 u = h(σ˙ d , e1 , e2 )

(7.24)

One available choice of h(·) is linear weighted superposition u = σ˙ d + k1 e1 + k2 e2 .

(7.25)

Finally, the completed kinematical coordinated plan and control strategy based on task-priority redundancy resolution framework is described thoroughly in Algorithm 7.2.

164

7 Autonomous Grasping Control of the Underwater Biomimetic …

Algorithm 7-2 Coordinated plan and control algorithm. 1: Initialization 2: 0 0, Q0 I , i 1 ; 3: while (true) do 4: if (task is over) then 5: Qi z ; i 6: Break; 7: else 8: Compute Jacobian matrix J i ; 9: On-line motion planning to compute di ; 10: State observers to estimate zˆi , vˆi ; 11: Compute position error e1i zˆi zd ; ; 12: Compute velocity error e2i vˆi di 13: Compute feedback law ui k1e1i k2e2i ; di 14: Qi Qi 1 I J i J i ; 15: Qi 1 J i ui ; i i 1 i 16: ; 17: end if 18: end while Given set point z sp , the tracking differentiator generates both the trajectory point z di and speed vdi sequentially. In Algorithm 7.2, z d can be selected as z sp or z di . If z d = z di , the planned trajectory is achieved strictly, and this must be guaranteed by high motion precision at every moment. Apparently, this would increase control cost significantly, while z d = z sp , the trajectory may not be achieved strictly, but z sp can be reached at a higher speed.

7.2.1.3

Feedforward Compensation

The manipulator is modeled as multi-linkage mechanism, while the vehicle body is assumed to be link 0. Kinematic chain hence transfers the extra effect of the manipulator to the vehicle’s body. The UBVMS always maintains stability at the equilibrium positions in roll and pitch angle, or returns to equilibrium under the influence of restorative forces. Therefore, here define the system velocity vector as ξ = [vx , v y , vz , r ], where vx , v y , vz are the linear velocity of the UBVMS, and r is the yaw angle rate. Customarily, the dynamic model of the vehicle is written as the following compact form [11]: M ξ˙ + C(ξ )ξ + Dξ = τv − τa ,

(7.26)

7.2 Coordination Planning and Control of the Underwater Biomimetic …

165

where M(q), C(q, ζ ), D(q, ζ ) ∈ R n 1 ×n 1 are the inertia matrix, Coriolis and centripetal matrix, friction and hydrodynamic damping matrix, respectively. Here, all of them include the added mass effects, and τv ∈ R n 1 is the vector of force and moment provided by the biomimetic propulsors, and the τa represents the coupling effect caused by the presence of the manipulator. Therefore, the desired driving force can be written as τv = τcontrol + τ˜a ,

(7.27)

where τcontrol is the output of vehicle controller, while τ˜a is the estimation of coupling effect. The estimation of τ˜a based on the forward and inverse dynamics recursive equations is given in [12].

7.2.2 Experiment and Results To completely investigate the characteristics of the UBVMS under varying settings and disturbances, three sets of pool-based experiments are done. The red-labeled target object is approximatively relocated at [0.55, 1.8, 0.5]T m in earth-fixed frame, and the UBVMS departs from a remote location with any beginning posture.

7.2.2.1

Control Tasks

The UBVMS has the ability to spin on the spot, which will aid in energy conservation and visual scanning; thus, the heading direction is modified first. The first control task may therefore be represented as ∥ψ − ψd ∥ = 0,

(7.28)

where ψ is the yaw of the UBVMS and ψd is desired heading direction. The end gripper of the manipulator must finally reach the target item to prepare for the subsequent gripping phase, so the second task is ∥ ∥ ∥ηee1 − ηobj ∥ = 0,

(7.29)

where ηee1 is the position of the end gripper and ηobj is the position of the target object. In the preceding section, the optimal working field has been examined in full consideration of the performance of the binocular camera and the manipulator. When the pose of the manipulator is close to qd = [0, 45, −60, 0]T , above optimal working field can be satisfied without visual blocking. Thus, the third task is ∥q − qd ∥ = 0.

(7.30)

166

7 Autonomous Grasping Control of the Underwater Biomimetic …

These three tasks are performed in subsequent studies to accomplish autonomous navigation and autonomous grasping.

7.2.2.2

Experiment on Single Accuracy Grasping

The first experiment focuses on the implementation of the single precision grasping, i.e., performing the operation on the first try. Amplitude of the long fin’s undulations is a crucial factor in determining stability and speed. The actual adaptive undulatory amplitude weighted by the adaptive parameter modifier is depicted in Fig. 7.8. The value of amplitude is defined as integers; therefore, the curve has the appearance of a staircase. When the UBVMS is distant from the target, speed takes precedence over amplitude. Otherwise, tiny amplitudes are used to promote stability. During the transition from 2.2 to 0.4 m, the amplitude progressively changes from its highest of ◦ ◦ 29 to its lowest of 15 . Without this adaptive system, autonomous grasping likely to fail owing to inertial factors. As described before, the heading direction is modified first, and then the yaw angle of the UBVMS is adjusted to face the target. As seen in Fig. 7.8, the yaw angle is visibly altered throughout the 0–7-s initial phase. The yaw angle of the UBVMS is then delicately adjusted until it reaches a stable position. Figure 7.9 illustrates the UBVMS end effector and vehicle body trajectories. The solid lines reveal that the location of the vehicle body undergoes a dramatic change between 6 and 40 s, indicating that the UBVMS is rapidly approaching the region of interest. The autonomous grasping phase is then initiated, while the UBVMS remains steady, when the UBVMS reaches the area of interest and locks on to the target item. Before 7 s, however, there is an undesirable motion in the O I − X direction. This is due to the unavoidable spinning drifting of the UBVMS’s rotation motion, and the dashed lines indicate that the end gripper of the UBVMS reaches the object under the guidance of Fig. 7.8 Adaptive amplitude versus distance

7.2 Coordination Planning and Control of the Underwater Biomimetic …

167

the binocular camera. The feedback laws of the yaw of the UBVMS and position of the end effector are shown in Fig. 7.10. The yaw angle’s feedback law first converges to a relatively stable zero state. All feedback laws virtually converge to zero within 60 s, indicating that the control requirements have been achieved. The whole operation of autonomous grasping is depicted in Fig. 7.11, which comprises three phases: yaw angle adjustment, rapid approaching, and autonomous grabbing. Throughout this entire procedure, the UBVMS requires around 94 s and takes only a single opportunity to achieve autonomous grasping. Fig. 7.9 Trajectories of the UBVMS during a single accuracy grasping test. The blue and red dashed lines represent the trajectories of the end effector in the X and Y directions, while the blue and red solid lines represent the trajectories of the vehicle body in the X and Y directions. EE is the abbreviation for the term end effector

Fig. 7.10 Feedback laws u in single accuracy grasping experiment. The blue and red lines represent the end effector’s X , Y position feedback laws, while the green line represents the yaw angle feedback law

168

7 Autonomous Grasping Control of the Underwater Biomimetic …

Fig. 7.11 Images of the single autonomous grasping. (1)–(4) are the phase of heading direction adjustment, (5)–(8) are the phase of fast approaching to the target, and (9)–(12) are the phase of autonomous grasping

7.2.2.3

Experiment on Multi-Try Grasping

After performing the grabbing action, the UBVMS would swim away. If the distance between the target object and the end gripper of the UBVMS exceeds the threshold, the grasping mission will fail, and a subsequent grasping attempt will be attempted until the operation is successful or the job is manually terminated. If it is successful, the procedure will be automatically halted. In the multi-try grasping experiment seen in Figs. 7.12 and 7.13, the first attempt grasping is undertaken at around 70 s, and the object is purposefully pushed away to cause the grasping to fail. The target is then returned to its original location, and the UBVMS attempts grasping once more. In Fig. 7.12, noticeable alterations occur at around 70 s, and the second grasping occurs at approximately 104 s. As the grasping has been deemed effective, the procedure will shortly conclude. Based on the feedback law curves in Fig. 7.13, we can also conclude that feedback laws increase after the apparent failure of the first grasping and converge to zero in preparation for the second grasping. The multi-try autonomous grasping experiment is depicted in Fig. 7.14, and it takes the UBVMS approximately 109 s to successfully complete the multi-try autonomous grasping. This experiment demonstrates that the UBVMS could pursue the completion of autonomous manipulation missions at the expense of multiple opportunities and additional time.

7.2.2.4

Experiment on Grasping for Unfixed Object

The experiment of autonomous grasping for a moving object is conducted in order to assess the performance of the UBVMS’s autonomous operation for moving objects. After the target item is recognized by a binocular camera, it is continually moved inside the vision area of the UBVMS, as seen in Fig. 7.15. The UBVMS successfully

7.2 Coordination Planning and Control of the Underwater Biomimetic …

169

Fig. 7.12 Yaw angle and trajectories of the UBVMS in multi-try grasping experiment. The purple line represents the yaw angle of the UBVMS. The blue and red dashed lines represent the trajectories of the end effector in the X and Y directions, while the blue and red solid lines represent the trajectories of the vehicle body in the X and Y directions. The vertical dashed lines at 70 and 104 s represent the first and second gripping times, respectively

Fig. 7.13 Feedback law u in multi-try grasping experiment. Blue and red line denote the feedback law of position of the end effector in X , Y direction, respectively, and the green line denotes the feedback law of yaw angle

tracks the target by using visual guidance. Figure 7.16 depicts the error between the end effector and the target. This erroneous information is really employed by the feedback legislation. The yaw angle of the UBVMS is modified to point at the object, as shown in Fig. 7.17. Meanwhile, the end gripper progressively approaches the item. The control approach is effective while the item is in motion, as seen in Fig. 7.18, and the object is eventually caught successfully.

170

7 Autonomous Grasping Control of the Underwater Biomimetic …

Fig. 7.14 Images of the multi-try grasping. (1)–(4) are the phase of heading direction adjustment, (5)–(8) are the phase of fast approaching to the target, (9)–(12) are the phase of first grasping and failed, and (13)–(16) are the phase of second try of grasping and successful

Fig. 7.15 Images of autonomous grasping for unfixed object. Here, the phases of heading direction adjustment and fast approaching to the target are omitted. (1)–(8) are the phase of UBVMS tracking the moving object, and (9)–(12) are the phase of locking again and autonomous grasping

7.2.2.5

Discussion

Autonomous manipulation is successful due to the high stability of bio-inspired underwater robots. The amplitude of the undulating fin has a significant effect on the stability in this investigation. Consequently, the adaptive amplitude modification technique is intended to lessen the impacts of floating inertia. During trials involving pool grasping, this mechanism performs admirably in terms of stage transition and stability maintenance.

7.2 Coordination Planning and Control of the Underwater Biomimetic …

171

Fig. 7.16 Between the end effector and the target, there is an error. Prior information determines the position of the target prior to 37.5 s (the green dashed line) The UBVMS detects the target for the first time at 37.5 s. At about 46 s (the black dashed line), the end gripper reaches the target for the first time. At around 80 s, the end gripper re-approaches the target

Fig. 7.17 Yaw angle and trajectories of the UBVMS in autonomous grasping experiment for unfixed object experiment. The purple line represents the yaw angle. The blue and red dashed lines represent the trajectories of the end effector in the X and Y directions, while the blue and red solid lines represent the trajectories of the vehicle body in the X and Y directions. The vertical dashed line indicates when the object begins to move

172

7 Autonomous Grasping Control of the Underwater Biomimetic …

Fig. 7.18 Feedback law u in autonomous grasping experiment for unfixed object. Blue and red line denote the feedback law of position of the end effector in X ,Y directions, respectively, and the green line denotes the feedback law of yaw angle

Moreover, compared to the experiment findings in the current literature, our newly constructed UBVMS powered by the long-fin propulsors is capable of autonomous grasping in many circumstances, including the successful grasping of an unfixed object target. This shows that the coordinated plan and control technique presented in this study is very adaptable and adds to the future research and implementation of autonomous manipulation in contexts with greater complexity.

References 1. Xu D, Tan M, Li Y (2008) Visual measurement and control for robots. National Defense Industry Press 2. Zhang ZY (2000) A flexible new technique for camera calibration. IEEE Trans Pattern Anal Mach Intell 22(11):1330–1334 3. Tan M, Xu D, Hou ZG, Wang S, Cao ZQ (2007) Advanced robot control. Higher Education Press 4. Li H, Chen YL, Chang T, Wu X, Ou Y, Xu Y (2011) Binocular vision positioning for robot grasping. In: Proceeding of IEEE international conference robotics and biomimetics, Phuket, Thailand, pp 1522–1527 5. Han JQ (2009) From PID to active disturbance rejection control. IEEE Trans Industr Electron 56(3):900–906 6. Han JQ (2008) Active disturbance rejection control technique—the technique for estimating and compensating the uncertainties. National Defense Industry Presses, Beijing, China, pp 46–110 7. Moe S, Antonelli G, Teel AR, Pettersen KY, Schrimpf J (2016) Setbased tasks within the singularity-robust multiple task-priority inverse kinematics framework: general formulation, stability analysis, and experimental results. Front Robot AI 3(16):1–18 8. Chiaverini S (1997) Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators. IEEE Trans Robot Autom 13(3):398–410 9. Tang C, Wang Y, Wang S, Wang R, Tan M (2018) Floating autonomous manipulation of the underwater biomimetic vehicle-manipulator system: Methodology and verification. IEEE Trans Industr Electron 65(6):4861–4870

References

173

10. Wang Y, Wang S, Wei Q, Tan M, Zhou C, Yu J (2016) Development of an underwater manipulator and its free-floating autonomous operation. IEEE/ASME Trans Mechatron 21(2):815–824 11. Wang S, Wang Y, Wei Q, Tan M, Yu J (2017) A bio-inspired robot with undulatory fins and its control methods. IEEE/ASME Trans Mechatron 22(1):206–216 12. Antonelli G (2013) Underwater robots, 3rd edn, vol 96. Springer, New York, NY, USA