337 27 9MB
English Pages XXIII, 203 [222] Year 2021
Advances in Industrial Control
Tong Heng Lee Wenyu Liang Clarence W. de Silva Kok Kiong Tan
Force and Position Control of Mechatronic Systems Design and Applications in Medical Devices
Advances in Industrial Control Series Editors Michael J. Grimble, Industrial Control Centre, University of Strathclyde, Glasgow, UK Antonella Ferrara, Department of Electrical, Computer and Biomedical Engineering, University of Pavia, Pavia, Italy Editorial Board Graham Goodwin, School of Electrical Engineering and Computing, University of Newcastle, Callaghan, NSW, Australia Thomas J. Harris, Department of Chemical Engineering, Queen’s University, Kingston, ON, Canada Tong Heng Lee, Department of Electrical and Computer Engineering, National University of Singapore, Singapore, Singapore Om P. Malik, Schulich School of Engineering, University of Calgary, Calgary, AB, Canada KimFung Man, City University Hong Kong, Kowloon, Hong Kong Gustaf Olsson, Department of Industrial Electrical Engineering and Automation, Lund Institute of Technology, Lund, Sweden Asok Ray, Department of Mechanical Engineering, Pennsylvania State University, University Park, PA, USA Sebastian Engell, Lehrstuhl für Systemdynamik und Prozessführung, Technische Universität Dortmund, Dortmund, Germany Ikuo Yamamoto, Graduate School of Engineering, University of Nagasaki, Nagasaki, Japan
Advances in Industrial Control is a series of monographs and contributed titles focusing on the applications of advanced and novel control methods within applied settings. This series has worldwide distribution to engineers, researchers and libraries. The series promotes the exchange of information between academia and industry, to which end the books all demonstrate some theoretical aspect of an advanced or new control method and show how it can be applied either in a pilot plant or in some real industrial situation. The books are distinguished by the combination of the type of theory used and the type of application exempliﬁed. Note that “industrial” here has a very broad interpretation; it applies not merely to the processes employed in industrial plants but to systems such as avionics and automotive brakes and drivetrain. This series complements the theoretical and more mathematical approach of Communications and Control Engineering. Indexed by SCOPUS and Engineering Index. Proposals for this series, composed of a proposal form downloaded from this page, a draft Contents, at least two sample chapters and an author cv (with a synopsis of the whole project, if possible) can be submitted to either of the: Series Editors Professor Michael J. Grimble Department of Electronic and Electrical Engineering, Royal College Building, 204 George Street, Glasgow G1 1XW, United Kingdom email: [email protected] Professor Antonella Ferrara Department of Electrical, Computer and Biomedical Engineering, University of Pavia, Via Ferrata 1, 27100 Pavia, Italy email: [email protected] or the Inhouse Editor Mr. Oliver Jackson Springer London, 4 Crinan Street, London, N1 9XW, United Kingdom email: [email protected] Proposals are peerreviewed. Publishing Ethics Researchers should conduct their research from research proposal to publication in line with best practices and codes of conduct of relevant professional bodies and/or national and international regulatory bodies. For more details on individual ethics matters please see: https://www.springer.com/gp/authorseditors/journalauthor/journalauthorhelpdesk/ publishingethics/14214
More information about this series at http://www.springer.com/series/1412
Tong Heng Lee Wenyu Liang Clarence W. de Silva Kok Kiong Tan •
•
•
Force and Position Control of Mechatronic Systems Design and Applications in Medical Devices
123
Tong Heng Lee Department of Electrical and Computer Engineering National University of Singapore Singapore, Singapore
Wenyu Liang Department of Electrical and Computer Engineering National University of Singapore Singapore, Singapore
Clarence W. de Silva Department of Mechanical Engineering University of British Columbia Vancouver, BC, Canada
Kok Kiong Tan Department of Electrical and Computer Engineering National University of Singapore Singapore, Singapore
ISSN 14309491 ISSN 21931577 (electronic) Advances in Industrial Control ISBN 9783030526924 ISBN 9783030526931 (eBook) https://doi.org/10.1007/9783030526931 MATLAB and Simulink are registered trademarks of The MathWorks, Inc. See mathworks. com/trademarks for a list of additional trademarks. Mathematics Subject Classiﬁcation: 93C85, 93C95 © Springer Nature Switzerland AG 2021 This work is subject to copyright. All rights are reserved by the Publisher, whether the whole or part of the material is concerned, speciﬁcally the rights of translation, reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microﬁlms 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 speciﬁc statement, that such names are exempt from the relevant protective laws and regulations and therefore free for general use. The publisher, 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 publisher 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 publisher remains neutral with regard to jurisdictional claims in published maps and institutional afﬁliations. This Springer imprint is published by the registered company Springer Nature Switzerland AG The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland
To our families
Series Editor’s Foreword
Mechatronics is a discipline that includes elements of mechanics, electronics, computer science, and control. It is a complex discipline simply because its nature combines the essences of so many others but it is extremely fascinating and relevant for our daily life and for industrial production. We live in a world pervaded by mechatronic systems: our cars are an example, as are the robots that contribute to their assembly, the robotic systems that process the food we eat, the automated cells involved in the manufacture of fabrics and leather to create the clothes and the shoes we wear, the machines that make the work of farmers in agriculture less laborious, and many home automation systems and smart components associated with the Internet of Things; all these are mechatronic systems. Apart from the aforementioned applications, there is no doubt that one of the ﬁelds in which mechatronics has taken hold signiﬁcantly in recent decades is healthcare. Many fundamental medical devices are mechatronic systems. Think, for example, of prosthetic limbs that allow realistic movements, restoring to amputees a quality of life that would have been unbelievable decades ago. Think of robotic surgery devices, which support surgeons in surgical activities that are extremely complex to perform by laparoscopy, also allowing the surgeon to operate while sitting and with a perfect vision of the operating ﬁeld, which is often impossible with conventional surgery. In addition, since robotassisted surgery is minimally invasive, it enables quicker patient recovery. Among the possible medical applications of mechatronics, we cannot omit the use of machines for rehabilitation, including wearable robot systems such as exoskeletons and rehabilitation robots, which nowadays are increasingly combined with advanced communication systems, even allowing the patient to be rehabilitated remotely. It is no coincidence that, at present, the term biomechatronics is more and more often used in the related technical literature. In writing this Foreword, I cannot ignore the difﬁcult moment in which this book is being prepared. A historic moment marked by the heavy social and economic impact of COVID19. In this dramatic situation, medical mechatronics has played a
vii
viii
Series Editor’s Foreword
crucial role, supporting hospital workers in carrying out their duties despite difﬁcult and risky conditions. The pandemic has further motivated researchers and technologists from all over the world to investigate new possibilities in the medical mechatronics. Given that the pandemic caused by SARSCoV2 is still ongoing and that future disease outbreaks cannot be excluded, nascent ideas of new mechatronic medical applications deserve to be sustained and brought to realization whether these be directly related to patient care—in the automation of taking nasal swabs without undue patient discomfort, for example—or in more general preventative measures such as the automated sanitization of daily work spaces without damage to nearby equipment. In scenarios in which the risk of contagion is high, and effective personal protection devices are not always available, the role of machines equipped with a certain level of autonomy, reliability, and ﬁnesse becomes of utmost importance. The application of mechatronics to the healthcare sector and medicine, in general, brings formidable challenges, both from the point of view of the development of the underlying theory, and of the realization of the devices at affordable cost. To this end, the use of accurate kinematic and dynamic models is fundamental. The theoretical implications of controlling the interaction between the device and the environment, and of the hybrid force/position control of the parts of the machine which come in contact with the patient cannot be overlooked. Turning to economic and productivity concerns, the production volumes of these machines, especially of those which perform niche operations or veryhighprecision tasks such as ophthalmic surgical robots, may not be enormous. Therefore, the research and development phase must also address cost containment, in order to guarantee a competitive advantage for producers. Achieving a fair balance between costs and beneﬁts is also assisted by dedicating attention to the theoretical aspects and following a rigorous methodological approach in the design and development phase. From this perspective, this book plays two important roles. It provides the user with the methodological tools that can help understand the functioning and characteristics of a certain mechatronic device, speciﬁcally a device of medical type. It also reviews theoretical tools relevant to ensuring that mechanical and control design is reliable and efﬁcient. It is a very thoroughgoing book, covering a broad spectrum of topics, from force and position control to observerbased force estimation, encompassing supervisory and visionbased motion and interaction control strategies. If, at ﬁrst glance, the book may appear to be a classic robotics book, a more careful reading reveals that this is absolutely not true. All parts, even those that refer to very classic topics, are made modern and interesting thanks to their being customized for the speciﬁc case of mechatronic medical devices.
Series Editor’s Foreword
ix
With this volume, Advances in Industrial Control is enriched with a new monograph that contributes strongly to broadening the scope of the series, conﬁrming that the meaning of the adjective “industrial” that we agree on in this context is very broad and inclusive of the control of machines, devices, and systems that do not fall under the conventional classiﬁcation of industrial applications. Antonella Ferrara University of Pavia, Italy
Preface
Mechatronic systems have been increasingly used in many industrial and medical applications, where they are designed to work for various tasks in different environments. Signiﬁcantly, many applications are required to carry out the contact operation and handle the interaction between the mechatronic systems and the environments (contacting objects) in order to complete the speciﬁc task successfully, such as grasping, polishing, assembly, robotic surgery, injection, etc. During the contact operation, the interaction force needs to be regulated carefully to avoid the undesirable effects and ensure the success of the performed task. As a consequence, force control is needed and designed delicately to meet speciﬁc requirements and achieve desired performance. To achieve an appropriate or desired interaction, force feedback control is an effective way to regulate contact behavior. In recent years, huge numbers of research works report various force feedback schemes, which show good effectiveness of applying force controller in different applications. The explicit force controllers can achieve low force overshoot good force tracking performance, especially when the contact model is established accurately. However, it is noted that the motion/position of the actuation system is unconstrained or uncontrolled for pure force controllers (i.e., only the force is controlled directly). To deal with the applications where both force control and position control are required, force and position control is the major approach. In this book, we offer systematic coverage of theoretical and practical aspects in the area of force and position control, which gives the readers an overview on the concepts, design, and implementation approaches of such control system. This book totally consists of nine chapters. More speciﬁcally, the ﬁrst chapter of this book introduces the general concepts and technologies related to the force sensing, interaction modeling, and control strategy. In the following chapters from Chap. 2 to Chap. 8, the novel ideas and innovations related to the force estimation and the force and position control (includes direct force control, force–position control and impedance control) are reported in detail. Signiﬁcantly, Chaps. 3–8 are technical chapters that are presented along with speciﬁc applications in medical devices. These chapters not only offer the readers various general knowledge and new xi
xii
Preface
thinking to solve their application challenges or control problems, but also provide the readers detailed references and examples on the ways to integrate the suitable control approaches into the practices. Lastly, the ﬁnal chapter concludes this book. In summary, this book gives an overview of the force and position control techniques; shows the readers our several recent novel ideas and innovations on the design and implementation of the force control and the force and position control for mechatronics; and uses the practical applications as case studies where detailed experimental veriﬁcations and results are given. From this book, readers can expect to learn how to design and implement new techniques of force control or force and position control for mechatronic systems, especially, medical devices. In particular, applicationoriented readers can beneﬁt more from this book. Besides, we would like to take the opportunity to many thank Dr. Sunan Huang for his help and constructive suggestions in the writing of this book. Also, this book would not be possible without the generous assistance of the following colleagues and friends: Mr. Chee Siong Tan, Dr. Lynne Hsueh Yee Lim, Mr. Chee Wee Gan, Dr. Cailin Ng, Mr. Zhao Feng, Dr. Wenchao Gao, Dr. Jun Yik Lau, Dr. Jun Ma, and Dr. Silu Chen. Moreover, we are grateful for the help provided by the Editors. Finally, we thank our families for their love and support. May the force be with you! Singapore, Singapore Singapore, Singapore Vancouver, Canada Singapore, Singapore April 2020
Tong Heng Lee Wenyu Liang Clarence W. de Silva Kok Kiong Tan
Contents
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
1 1 3 4 4 11 12 13 16 16 17 17 17 18 20 21 22 24 25
2 Disturbance ObserverBased Force Estimation Without Force Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Disturbance ObserverBased Force Estimation Methods . . . . 2.1.1 Disturbance Observer . . . . . . . . . . . . . . . . . . . . . . . . 2.1.2 Nonlinear Disturbance Observer . . . . . . . . . . . . . . . . 2.1.3 Extended State Observer . . . . . . . . . . . . . . . . . . . . . 2.1.4 Discussions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Friction Compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 ModelBased Friction Compensation . . . . . . . . . . . . . 2.2.2 ModelFree Friction Compensation . . . . . . . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
29 29 30 32 35 40 41 42 50
1 Introduction . . . . . . . . . . . . . . . . . . . . . . . 1.1 Overview . . . . . . . . . . . . . . . . . . . . . . 1.2 Mechatronic Systems . . . . . . . . . . . . . 1.3 Force Sensing . . . . . . . . . . . . . . . . . . . 1.3.1 Strain Gauge and Load Cell . . . 1.3.2 Force Sensing Resistor . . . . . . . 1.3.3 Piezoelectric Force Sensor . . . . 1.3.4 Other Force Sensors . . . . . . . . . 1.4 Modeling of Contacting Object . . . . . . 1.4.1 Linear Model . . . . . . . . . . . . . 1.4.2 Nonlinear Model . . . . . . . . . . . 1.5 Force and Position Control . . . . . . . . . 1.5.1 Position Control . . . . . . . . . . . 1.5.2 Force Control . . . . . . . . . . . . . 1.5.3 Hybrid Force–Position Control . 1.5.4 Parallel Force–Position Control 1.5.5 Impedance Control . . . . . . . . . . 1.6 Organization of the Book . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
xiii
xiv
Contents
2.3 Gravity Compensation . . . . . 2.3.1 Mechanical Approach 2.3.2 Control Approach . . . 2.4 Chapter Summary . . . . . . . . . References . . . . . . . . . . . . . . . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
53 53 54 55 55
3 ForceBased Supervisory Control Assisted Surgery 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 System Description . . . . . . . . . . . . . . . . . . . . . . 3.2.1 Challenges . . . . . . . . . . . . . . . . . . . . . . 3.2.2 Surgical Device Design . . . . . . . . . . . . . 3.2.3 Force Sensing System . . . . . . . . . . . . . . 3.3 ForceBased Supervisory Controller Design . . . . 3.3.1 Position Controller . . . . . . . . . . . . . . . . . 3.3.2 Supervisory Controller . . . . . . . . . . . . . . 3.4 Case Study . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4.1 Prototype . . . . . . . . . . . . . . . . . . . . . . . 3.4.2 Experimental Results . . . . . . . . . . . . . . . 3.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
. . . . . . . . . . . . . .
61 61 63 64 66 67 69 70 71 80 80 81 82 83
4 Stabilization System Based on VisionAssisted Force Feedback 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.1 Surgical Device Design . . . . . . . . . . . . . . . . . . . . . . 4.2.2 Human Head Motion . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3.1 Force Feedback Controller . . . . . . . . . . . . . . . . . . . . 4.3.2 VisionBased Motion Compensator . . . . . . . . . . . . . . 4.3.3 VisionAssisted Force Feedback Controller . . . . . . . . 4.4 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4.1 Experimental System Setup . . . . . . . . . . . . . . . . . . . 4.4.2 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . 4.4.3 Discussions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . .
. . . . . . . . . . . . . . .
. 85 . 85 . 87 . 87 . 89 . 92 . 93 . 96 . 98 . 98 . 98 . 99 . 106 . 106 . 107
5 Optimal and Robust Contact Force Control on Soft Membrane . 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.1 Optimal Force Controller . . . . . . . . . . . . . . . . . . . . . . 5.3.2 Disturbance ObserverBased Motion Compensator . . . . 5.3.3 Stability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . .
. . . . . . . .
. . . . . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
109 109 111 111 112 121 122 122
Contents
5.4.1 Application Overview and Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . 5.4.2 Force Controller only . . . . . . . . . . . . . . 5.4.3 Force Controller with Compensation . . . 5.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
xv
System . . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
123 124 125 129 129
6 Force–Position Control for Fast Tube Insertion . . . . . . . . . 6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Tube Insertion Procedure . . . . . . . . . . . . . . . . . . . . . . . 6.2.1 System Architecture . . . . . . . . . . . . . . . . . . . . . . 6.2.2 Working Process . . . . . . . . . . . . . . . . . . . . . . . . 6.3 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.3.1 PositionBased Insertion Method . . . . . . . . . . . . 6.3.2 ForceBased Insertion Method . . . . . . . . . . . . . . 6.3.3 Force–Position Insertion Method . . . . . . . . . . . . 6.3.4 Discussions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.4 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . 6.4.1 Experiments and Results on Rigid Setup . . . . . . . 6.4.2 Experiments and Results on Stabilized Handheld Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
133 133 134 135 137 139 140 142 144 148 148 148
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . . . . 153 . . . . . . . 155 . . . . . . . 155
7 Robust Impedance Control of Constrained Piezoelectric ActuatorBased EndEffector . . . . . . . . . . . . . . . . . . . . . . . 7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2 Piezoelectric Ultrasonic Motor Modeling in Constrained Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.3 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.3.1 Impedance Control . . . . . . . . . . . . . . . . . . . . . . . 7.3.2 Robust Impedance Control Design . . . . . . . . . . . 7.4 Experimental Study . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.4.1 Experimental System Setup . . . . . . . . . . . . . . . . 7.4.2 Results and Discussions . . . . . . . . . . . . . . . . . . . 7.5 Chapter Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
160 161 162 165 170 171 172 176 177
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2 System Review and Problem Formulation . . . . . . . . . . 8.3 Controller and Observer Design . . . . . . . . . . . . . . . . . . 8.3.1 Modeling of Motor Stage . . . . . . . . . . . . . . . . . 8.3.2 Position Controller . . . . . . . . . . . . . . . . . . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
179 179 181 182 182 184
. . . . . .
. . . . . . . 157 . . . . . . . 157
xvi
Contents
8.3.3 Advanced Disturbance Observer . 8.3.4 Contact Estimator . . . . . . . . . . . 8.3.5 Failure Detector . . . . . . . . . . . . . 8.4 Numerical Study . . . . . . . . . . . . . . . . . . 8.5 Experimental Veriﬁcation . . . . . . . . . . . 8.5.1 Experimental System Setup . . . . 8.5.2 Experimental Results . . . . . . . . . 8.6 Chapter Summary . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
185 189 189 190 191 193 193 197 197
9 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 199 Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201
List of Figures
Fig. Fig. Fig. Fig. Fig. Fig.
1.1 1.2 1.3 1.4 1.5 1.6
Fig. 1.7 Fig. 1.8 Fig. 1.9 Fig. 1.10 Fig. 1.11 Fig. 1.12 Fig. 1.13 Fig. 1.14 Fig. 1.15 Fig. 1.16
Fig. Fig. Fig. Fig. Fig. Fig. Fig. Fig.
1.17 1.18 1.19 1.20 1.21 2.1 2.2 2.3
Feedback control system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . General concept of mechatronics . . . . . . . . . . . . . . . . . . . . . . Working concept of a typical strain gauge . . . . . . . . . . . . . . . An example of force measurement using strain gauges . . . . . . Wheatstone Bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Different Wheatstone Bridges: a quarter bridge; b half bridge; c full bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Temperature compensation for strain guage . . . . . . . . . . . . . . Instrumentation ampliﬁer . . . . . . . . . . . . . . . . . . . . . . . . . . . . Different kinds of load cells: a structure; b commercially available load cells . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Electrical system for strain gauge and piezoresistive force sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Force sensing resistors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Voltage divider . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Syringe detection using FSR in a medical device: a medical device prototype; b syringe detection . . . . . . . . . . . . . . . . . . . Working principle of piezoelectric force sensor . . . . . . . . . . . Working principle of optical reﬂectance sensor for force measurement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Linear contact models: a Maxwell; b Kelvin–Voigt; c Kelvin–Boltzmann (Reprinted from [65], Copyright (2019), with permission from Elsevier) . . . . . . . . . . . . . . . . . . . . . . . . Position control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Force control: a direct approach; b positionbased approach. . Hybrid force–position control . . . . . . . . . . . . . . . . . . . . . . . . . Parallel force–position control . . . . . . . . . . . . . . . . . . . . . . . . . Impedance control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Classical disturbance observer. . . . . . . . . . . . . . . . . . . . . . . . . Nonlinear disturbance observer . . . . . . . . . . . . . . . . . . . . . . . . Extended state observer with known model . . . . . . . . . . . . . .
. . . . .
3 4 5 6 6
.. .. ..
8 9 9
..
10
.. .. ..
11 11 12
.. ..
12 13
..
14
. . . . . . . . .
16 18 19 20 21 23 31 34 37
. . . . .
. . . . . . . . .
xvii
xviii
List of Figures
Fig. 2.4 Fig. 2.5 Fig. 2.6 Fig. 2.7 Fig. 2.8 Fig. 2.9 Fig. Fig. Fig. Fig.
3.1 3.2 3.3 3.4
Fig. Fig. Fig. Fig. Fig. Fig.
3.5 3.6 3.7 3.8 3.9 3.10
Fig. 3.11 Fig. 3.12 Fig. 3.13 Fig. 3.14 Fig. Fig. Fig. Fig. Fig. Fig.
3.15 3.16 3.17 3.18 4.1 4.2
Fig. 4.3 Fig. 4.4 Fig. 4.5
Friction Compensation (© 2019 IEEE. reprinted, with permission, from [57]) . . . . . . . . . . . . . . . . . . . . . . . . . . Adaptive learningbased friction compensation (© 2019 IEEE. reprinted, with permission, from [57]) . . . . . . The ﬂow chart of Genetic Algorithm (© 2019 IEEE. reprinted, with permission, from [57]) . . . . . . . . . . . . . . . . . . . . . . . . . . Artiﬁcial intelligence system (© 2019 IEEE. reprinted, with permission, from [57]) . . . . . . . . . . . . . . . . . . . . . . . . . . Neural network friction modeling and compensation scheme (© 2019 IEEE. reprinted, with permission, from [57]) . . . . . . Fuzzy logic estimatorbased controller (© 2019 IEEE. reprinted, with permission, from [57]) . . . . . . . . . . . . . . . . . . Prototype of the surgical device . . . . . . . . . . . . . . . . . . . . . . . Tiny Tytan grommet . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Surgical device for the treatment of OME . . . . . . . . . . . . . . . Installation and force analysis of the force sensor: a installation of the force sensor; b force analysis . . . . . . . . . Filtered output of the force sensor . . . . . . . . . . . . . . . . . . . . . Forcebased supervisory controller . . . . . . . . . . . . . . . . . . . . . Motion controller for USM stage . . . . . . . . . . . . . . . . . . . . . . Program ﬂow chart . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Force proﬁle during touch and incision sequences . . . . . . . . . Boxplot of the maximum incision force of the different types of mock membranes . . . . . . . . . . . . . . . . . . . . . . . . . . . Structure of the MLP neural network with one hidden layer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Confusion plot of MLP neural network after training stage . . Step response of a critical damped thirdorder system . . . . . . Modiﬁed Scurve motion proﬁle (Scurve ending with gentle straightline path) . . . . . . . . . . . . . . . . . . . . . . . . . Motion proﬁle for the four types of membranes . . . . . . . . . . . Velocity proﬁle for the four types of membranes . . . . . . . . . . Experimental system: a setup; and b system architecture . . . . Successful tube insertion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Surgical device for the treatment of OME . . . . . . . . . . . . . . . Top view of the head and the device (Reprinted from [10], Copyright (2016), with permission from Elsevier) . . . . . . . . . Head motions along Zaxis of three different persons . . . . . . . Spectrum of the head motion . . . . . . . . . . . . . . . . . . . . . . . . . Control scheme for the stabilization system (Reprinted from [10], Copyright (2016), with permission from Elsevier) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
..
42
..
44
..
48
..
49
..
51
. . . .
. . . .
52 63 64 66
. . . . . .
. . . . . .
68 69 70 71 73 75
..
75
.. .. ..
75 76 77
. . . . . .
. . . . . .
78 79 80 81 82 88
.. .. ..
90 91 91
..
92
List of Figures
Fig. 4.6 Fig. 4.7 Fig. 4.8 Fig. 4.9
Fig. 4.10 Fig. 4.11 Fig. 4.12 Fig. 4.13 Fig. 4.14 Fig. 4.15
Fig. 4.16 Fig. 4.17 Fig. 4.18
Fig. 5.1 Fig. 5.2 Fig. 5.3 Fig. Fig. Fig. Fig.
5.4 5.5 5.6 5.7
Fig. 5.8 Fig. 5.9 Fig. 5.10 Fig. 5.11 Fig. 5.12 Fig. 5.13 Fig. 5.14 Fig. 6.1
Displacement input signal and output response: a displacement input (deformation); b force sensor output . . . Model validation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bode plot of force feedback system . . . . . . . . . . . . . . . . . . . . Schematic diagram of the setup for the motion compensator (Reprinted from [10], Copyright (2016), with permission from Elsevier) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Flow chart of the motion measurement . . . . . . . . . . . . . . . . . . Experimental system setup (Reprinted from [10], Copyright (2016), with permission from Elsevier) . . . . . . . . . . . . . . . . . . Force output and error of the force control system: a force output; b force error . . . . . . . . . . . . . . . . . . . . . . . . . . Position outputs from the linear encoder and the image processing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Error of the visionbased motion measurement . . . . . . . . . . . . Force outputs of different control methods: a without control; b only under force feedback control; c only under motion compensation; d under visionassisted force control . . . . . . . . Errors of different control methods . . . . . . . . . . . . . . . . . . . . . Comparison among different control methods . . . . . . . . . . . . . Force outputs of different control methods for head motion: a without control; b only under force feedback control; c under visionassisted force control . . . . . . . . . . . . . . . . . . . . Control scheme for the contact force control . . . . . . . . . . . . . Kelvin–Boltzmann (SLS) model (© 2018 IEEE. reprinted, with permission, from [26]) . . . . . . . . . . . . . . . . . . . . . . . . . . Displacement input and force sensor output: a displacement input (deformation); b FFT of input; c force sensor output . . . Model veriﬁcation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Illustration of gradientbased optimization algorithm . . . . . . . Change of cost and norm of projection gradient matrix . . . . . Setup of the ear surgical device (© 2018 IEEE. reprinted, with permission, from [26]) . . . . . . . . . . . . . . . . . . . . . . . . . . Experimental system setup (© 2018 IEEE. reprinted, with permission, from [26]) . . . . . . . . . . . . . . . . . . . . . . . . . . Comparison of force controller (only) with different gains . . . Comparison of different control schemes . . . . . . . . . . . . . . . . Contact force control without and with DOB subject to sine wave disturbance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Estimated error of DOB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Comparison between visionbased motion compensation method and proposed method . . . . . . . . . . . . . . . . . . . . . . . . . Proposed control scheme subject to random motion . . . . . . . . System architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
xix
.. .. ..
94 95 96
.. ..
97 97
..
99
. . 100 . . 101 . . 101
. . 102 . . 103 . . 104
. . 105 . . 112 . . 114 . . . .
. . . .
115 116 120 120
. . 123 . . 124 . . 125 . . 126 . . 126 . . 126 . . 127 . . 128 . . 135
xx
List of Figures
Fig. 6.2 Fig. 6.3
Fig. 6.4 Fig. 6.5 Fig. 6.6 Fig. 6.7 Fig. 6.8 Fig. 6.9 Fig. Fig. Fig. Fig. Fig. Fig.
6.10 6.11 6.12 6.13 6.14 6.15
Fig. 6.16 Fig. 6.17 Fig. 6.18 Fig. 6.19 Fig. 7.1 Fig. 7.2 Fig. 7.3 Fig. 7.4 Fig. 7.5 Fig. 7.6 Fig. Fig. Fig. Fig. Fig. Fig.
7.7 7.8 7.9 7.10 7.11 7.12
Mechanical design of the surgical device . . . . . . . . . . . . . . . . Working process of the proposed device: a initialization; b touch detection; c myringotomy; d tube insertion; e tube release (© 2016 IEEE. reprinted, with permission, from [16]) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Force sensor output and its ﬁltered output during tube insertion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Differential of ﬁlter output . . . . . . . . . . . . . . . . . . . . . . . . . . . Motion controller for USM stage . . . . . . . . . . . . . . . . . . . . . . Positionbased insertion method (© 2016 IEEE. reprinted, with permission, from [16]) . . . . . . . . . . . . . . . . . . . . . . . . . . Forcebased insertion method (© 2016 IEEE. reprinted, with permission, from [16]) . . . . . . . . . . . . . . . . . . . . . . . . . . Force–position insertion method (© 2016 IEEE. reprinted, with permission, from [16]) . . . . . . . . . . . . . . . . . . . . . . . . . . Motion sequence for insertion . . . . . . . . . . . . . . . . . . . . . . . . . Designed weighting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Features by using different slopes . . . . . . . . . . . . . . . . . . . . . . Experimental system setup: rigid . . . . . . . . . . . . . . . . . . . . . . Force output of the force control system. . . . . . . . . . . . . . . . . Insertion time by using forcebased insertion method: a histogram; b boxplot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Insertion time by using force–position insertion method: a histogram; b boxplot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Comparison among the insertion methods on rigid setup . . . . Experimental system setup: stabilized handheld (© 2016 IEEE. reprinted, with permission, from [16]) . . . . . . . . . . . . . . . . . . Insertion time by using force–position insertion method on stabilized handheld setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . USMdriven (PAbased) endeffector with manipulator . . . . . . Model of USM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Device in contact with environment (Reprinted from [35], Copyright (2019), with permission from Elsevier) . . . . . . . . . Block diagram of robust impedance control scheme . . . . . . . . General structure of DOB . . . . . . . . . . . . . . . . . . . . . . . . . . . . Experimental system setup of the USMdriven surgical device (endeffector) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Experimental results with varying kf . . . . . . . . . . . . . . . . . . . . Tracking errors with varying kf . . . . . . . . . . . . . . . . . . . . . . . Experimental comparison results (0.25 Hz) . . . . . . . . . . . . . . . Experimental comparison results (0.5 Hz) . . . . . . . . . . . . . . . . Experimental comparison results (1 Hz) . . . . . . . . . . . . . . . . . Error comparison (between PIDIC and RIC) of MaxAE and RMSE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . 136
. . 137 . . 139 . . 139 . . 140 . . 142 . . 143 . . . . . .
. . . . . .
145 145 146 147 149 149
. . 150 . . 152 . . 153 . . 154 . . 154 . . 160 . . 161 . . 162 . . 166 . . 168 . . . . . .
. . . . . .
171 173 173 174 175 175
. . 176
List of Figures
Fig. 8.1 Fig. 8.2 Fig. Fig. Fig. Fig.
8.3 8.4 8.5 8.6
Fig. Fig. Fig. Fig.
8.7 8.8 8.9 8.10
Fig. 8.11 Fig. 8.12 Fig. 8.13
Medical devices with force sensing system: ear surgical device . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Medical devices with force sensing system: needle insertion device for epidural anesthesia . . . . . . . . . . . . . . . . . . . . . . . . . Control scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Position controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Advanced disturbance observer . . . . . . . . . . . . . . . . . . . . . . . . Advanced disturbance observer output in simulation: a overall view; b zoomed view. . . . . . . . . . . . . . . . . . . . . . . . Observer error in simulation . . . . . . . . . . . . . . . . . . . . . . . . . . Experimental system setup . . . . . . . . . . . . . . . . . . . . . . . . . . . Position controller performance. . . . . . . . . . . . . . . . . . . . . . . . Advanced disturbance observer output during touch detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Observer error during touch detection . . . . . . . . . . . . . . . . . . . Advanced observer and force sensor output . . . . . . . . . . . . . . Analysis of the difference between observer and sensor outputs: a actual difference; b ﬁltered difference; c output of decision mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
xxi
. . 181 . . . .
. . . .
182 183 184 188
. . . .
. . . .
192 192 193 194
. . 194 . . 195 . . 195
. . 196
List of Tables
Table Table Table Table Table Table Table Table Table Table Table Table
3.1 3.2 3.3 3.4 4.1 4.2 4.3 5.1 5.2 5.3 6.1 7.1
Conditions for instance identiﬁcation from force output . . . . . Types of mock membranes . . . . . . . . . . . . . . . . . . . . . . . . . . Parameters for different types of membranes . . . . . . . . . . . . . Experimental results for the four types of membranes . . . . . . Effects of the head motions . . . . . . . . . . . . . . . . . . . . . . . . . . Errors for sine wave motions . . . . . . . . . . . . . . . . . . . . . . . . . Errors for simulated head motion . . . . . . . . . . . . . . . . . . . . . . Comparison among different models . . . . . . . . . . . . . . . . . . . Errors of different sine wave motions . . . . . . . . . . . . . . . . . . Comparison to visionbased motion compensation method . . Comparisons among insertion methods on rigid setup . . . . . . Controller parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . .
. . . . . . . . . . . .
72 74 79 82 92 104 106 117 127 128 152 172
xxiii
Chapter 1
Introduction
Forces result from interactions between objects. Interactions with other objects play a significant role in the proper and successful completion of various specific tasks for mechatronic systems. To deal with the interaction problems, force control is an effective and good way to guarantee the acceptable or desired contact. In recent years, the force control plays a more and more important role in mechatronic systems as the tasks of these systems become more complex and have increasing needs for handling the interaction between their contacting objects (e.g., environment, human) and themselves.
1.1 Overview With the rapid development of mechatronics, mechatronic systems have been increasingly used in healthcare and medical applications due to their capabilities of automating processes with precise and fast motions, such as ear surgical device [1], surgical robot for laparoscopic surgery [2], robotassisted beating heart surgery [3, 4], robotassisted vitreoretinal surgery [5, 6], palpation probe for minimally invasive surgery (MIS) [7], handheld ultrasound probe [8], cell injection system [9–11], powered exoskeleton [12], and so on. Remarkably, advancements in mechatronic systems of the medical devices allow surgeons/doctors to conduct the surgical treatments in a more efficient way. Meanwhile, the tasks of medical devices become more complex, and higher dexterity as well as higher adaptability to different circumstances are needed with the increase of various applications. Therefore, they are not only required to generate precise motions to complete their tasks but also required to handle the interactions between the environment or human and themselves (e.g., robot–environment inter© Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931_1
1
2
1 Introduction
action and human–robot interaction) in the sophisticated tasks. To this end, the force control plays an important role in these mechatronic systems. Force control is essentially a kind of approach that controls the dynamic interaction between a mechatronic system and its contacting object. Its main objectives are to maintain the contact force within an acceptable range or control the applied force to follow a desired reference. There are enormous numbers of research works on force control. For example, in [3], a model reference adaptive force control was designed suitably for beating heart surgery. In [10], a force control strategy for cell injection system was proposed based on the cell model of polynomial function and feedback linearization technique, so that an explicit force tracking was achieved. Also of interest is the work in [13] where an inversionfree force tracking controller for a variable physical damping actuator was proposed without complicated modeling. Nevertheless, although suitable individual force controllers can achieve fast response and low force overshoot, the position of the actuated device is typically unconstrained and uncontrolled for pure force controllers; and such a situation (while obviously possibly posing certain dangers) is oftentimes part of the tradeoff for the actuated device to reach the desired location to complete the overall task [14]. Furthermore, many applications need to perform the position tracking in certain directions while the force control is required in other directions. To deal with such cases, the force and position control is the major approach, where both force and position are considered in the control system. In moving beyond pure force control, it can be noted that the regulation of both position and force can be realized through employing the hybrid force–position (force/position) control approach [15] or the parallel force–position (force/position) control approach [16], which includes a position controller and a force controller to track position and force, respectively. Such force–position control approaches are widely used in various mechatronic systems. In [17], a hybrid force/position control scheme is designed and implemented in a flexible parallel manipulator. In [18], through employing a suitably optimized algorithm, a selective force–position control approach was applied on an ear surgical device. In [19], a parallel force/position control approach was designed and used in a parallel wire robot for epicardial interventions. In [20], a proportional–integral–derivative (PID) force controller and an adaptive sliding mode position controller were combined to penetrate zebrafish embryos. Alternatively, the methodology of impedance control proposed by Hogan in [21] is an effective and practical approach to regulate the position and force simultaneously without direct force control. Through establishing a virtual mass–spring–damper system containing position error and contact force, a delicate and compliant interaction control is achieved. Large numbers of research works on the impedance control are reported in various publications, and some examples of which are listed as follows. In [22], a force tracking impedance control was designed for a robot manipulator contacting with a rigid environment. In [23], a robust impedance control was proposed to handle parametric uncertainties, unknown force conversion function and hysteresis nonlinearity for a piezoactuated flexurebased fourbar mechanism. Additionally too, discretetime sliding mode impedance controllers have also been designed to complete microassembly in [24, 25].
1.1 Overview
3
Controller
Actuator
Plant
Sensor Fig. 1.1 Feedback control system
In summary, there are mainly four types of control schemes that can be used in the force and position control: positionbased force control, hybrid force–position control, parallel force–position control and impedance control. It is noteworthy that the feedback control is the core for all of these control schemes. A block diagram showing a typical feedback control for mechatronic systems is depicted in Fig. 1.1. The feedback control system senses the output of a plant (i.e., system to be controlled), computes corrective actions by the comparison between the sensing output and desired behavior and then actuates the system to approach the desired behavior [26]. In particular, the sensor and the controller are two key components to construct the feedback control system. In the following of this chapter, a brief introduction of mechatronic systems is given at first, and then an overview of the general concepts and technologies on force sensing, interaction modeling, and control is presented.
1.2 Mechatronic Systems The term mechatronics was originated and created by an engineer of Yaskawa Electric Corporation in the late 1960s, which is a portmanteau of “mechanics and electronics”. An accepted definition of mechatronics is: Mechatronics is the synergistic application of mechanics, electronics, control engineering, and computer science in the development of electromechanical products and systems through integrated design [27]. Figure 1.2 illustrated the general concept of mechatronics. Mechatronics is a multidisciplinary engineering field which involves a synergistic integration of several areas, such as mechanical engineering, electrical and electronic engineering, control engineering, and computer engineering. A typical mechatronic system consists of a mechanical skeleton, actuators, sensors, controllers, signal conditioning/modification devices, computer/digital hardware and software, interface devices, and power sources [28]. Some examples of mechatronic systems are modern automobiles, aircraft, spacecraft, robots, medical devices, which are multidomain systems consisting of multiple subsystems: mechanical systems, electrical and electronic systems, and control systems.
4
1 Introduction
Information Technology (Control and Computer Engineering)
Mechatronics
Fig. 1.2 General concept of mechatronics
1.3 Force Sensing The sense of touch is one of the crucial sensory modalities to close the control loop with adequate feedback, so that the dexterous and dynamic interaction with the environment can be achieved. A force sensing system, consisting of a force sensor or a network of force sensors, is a system that provides the force information (i.e., the sense of touch). The force sensor is the key component in the force sensing system. Generally, a sensor is a device that detects or measures a physical property and records, indicates, or responds to it [29]. A force sensor is a device that converts an input mechanical force into an electrical output signal. In the following subsections, different types of force sensors are introduced and discussed.
1.3.1 Strain Gauge and Load Cell 1.3.1.1
Strain Gauge
When a force is applied on an object, a deformation will be generated on the object’s body. As the strain is proportional to the deformation that is related to the applied force, the applied force can be measured via the strain measurement in an indirect manner. A strain gauge, a device used to measure strain on an object [27], is a very widely used sensor for force measurement. Figure 1.3 shows the working concept of a typical strain gauge. The strain gauge is mostly made of metal foil as well as metal wire, which can be considered as an
1.3 Force Sensing
5
Fig. 1.3 Working concept of a typical strain gauge
electrical conductor. It will become narrower and longer when it is stretched, which leads to an increase in its electrical resistance mainly due to the physical property of electrical conductance. On the contrary, the strain gauge will become broader and shorter when it is compressed, which causes its electrical resistance to be decreased. Remarkably, all the deformations on the strain gauge should be within the limit of its elasticity. By measuring the electrical resistance of the strain gauge, the stress and strain on the strain gauge can be obtained. The ratio of relative change in electrical resistance to the mechanical strain is called “Gauge Factor (GF)”, which is mathematically written as ΔRs /Rs , (1.1) KG F = ε where Rs is the electrical resistance of the strain gauge, ε is the strain. By bonding the strain gauge to the surface of an object, the strain on the object can be determined through measurement of the resistance change and thus the force applied on the object can be calculated through analytical mechanics. Figure 1.4 shows an example of force measurement using strain gauges. In this setup, the applied force on the needle will cause the bending on the beam which can be detected by the attached strain gauges.
1.3.1.2
Wheatstone Bridge
The Wheatstone Bridge is an electrical circuit that transforms resistance change to voltage change. To capture the resistance changes from an electrical system, the Wheatstone Bridge is used which configuration is shown in Fig. 1.5. The voltage output Vout of the Wheatstone Bridge is given by
6
1 Introduction
Fig. 1.4 An example of force measurement using strain gauges
Fig. 1.5 Wheatstone Bridge
R1 R4 Vin = − R1 + R2 R3 + R4 R1 R3 − R2 R4 Vin . = (R1 + R2 )(R3 + R4 )

+
Vout
(1.2)
For a balance bridge (i.e., Vout /Vin = 0), the bridge meets the following condition: R4 R1 = . R2 R3
(1.3)
Consider there are changes on the electrical resistances from the balanced condition and the fact that ΔRi /Ri 1 (i = 1, 2, 3, 4), we can have the following approximation on the voltage output: Vout ≈
1 4
ΔR1 ΔR2 ΔR3 ΔR4 − + − R1 R2 R3 R4
.
(1.4)
Here, if we select R1 as a strain gauge while the other three as three constant resistors with the same resistance, (1.4) becomes Vout ≈
1 ΔR1 Vin . 4 R1
(1.5)
1.3 Force Sensing
7
Since ΔRr /R1 = K G F ε1 according to (1.1), the voltage output of the Wheatstone Bridge is approximately proportional to the strain, i.e., Vout ≈
1 K G F ε1 Vin . 4
(1.6)
In this case, the Wheatstone Bridge is a quarter bridge. Furthermore, there are different versions of the Wheatstone Bridge as shown in Fig. 1.6, which include quarter bridge, half bridge, and full bridge. For a half bridge, its voltage output is given by Vout ≈
1 K G F (ε1 − ε2 )Vin . 4
(1.7)
For a full bridge, its voltage output is given by Vout ≈
1 K G F (ε1 − ε2 + ε3 − ε4 )Vin . 4
(1.8)
Specially, with proper installations of strain gauges such that ε1 = −ε2 (i.e., the tension on the strain gauge R1 equals to the compression on the strain gauge R2 ), R3 = R4 in a half bridge, and ε1 = −ε2 =ε3 = −ε4 in a full bridge, we can have 1 K G F ε1 Vin 2 ≈ K G F ε1 Vin
Vout ≈
for a half bridge;
(1.9)
Vout
for a full bridge.
(1.10)
This shows that the half bridge and the full bridge can achieve two times and four times the sensitivity of the quarter bridge, respectively. Furthermore, variations in temperature will cause the change in the object’s size due to thermal expansion which will also lead to the strain change on the strain gauge. Also, the electrical resistances of the strain gauge and its connecting wires will be affected by the temperature change. Therefore, the variations in temperature can affect the measurement by the strain gauge. To compensate the temperature effects, one effective way is the use of a dummy strain gauge as shown in Fig. 1.7. As can be seen, two identical strain gauges are used in a half bridge: the active one is mounted on the object surface where the stress will be applied and the other (dummy) one is mounted on the same material but not subjected to the stress. Therefore, the resistance changes on both strain gauges due to the variations in temperature will be in the same amount. As a result, the temperature effects will be canceled according to (1.7) and (1.9).
1.3.1.3
Instrumentation Amplifier
To increase the voltage output signal from the Wheatstone Bridge to a suitable range such that it can be used in a sensing system or a control system, a special type
8
1 Introduction
Fig. 1.6 Different Wheatstone Bridges: a quarter bridge; b half bridge; c full bridge
+

(a)
+

(b)
+

(c)
1.3 Force Sensing Fig. 1.7 Temperature compensation for strain guage
9
(unstressed)
(stressed)
Fig. 1.8 Instrumentation amplifier

+
3
gain
3
of differential amplifier called “Instrumentation Amplifier” is commonly used. The electrical circuit is shown in Fig. 1.8. The instrumentation amplifier has the features of very low DC offset, low drift and noise, very high openloop gain, very high commonmode rejection ratio, and very high input impedances. The amplifier gain of instrumentation amplifier is shown in (1.11), which can be adjusted by only one resistor Rgain . Vout 2 R¯ 1 R¯ 3 Av = = 1+ . V2 − V1 Rgain R¯ 2 1.3.1.4
(1.11)
Load Cell
A load cell is a type of force sensor that returns an electrical signal proportional to the mechanical force applied to itself. Strain gauge load cells are one very common type of load cells, different kinds of which are shown in Fig. 1.9. Structurally, a load cell consists of a sturdy and minimally elastic body that is mounted with strain gauges and a housing. When a force is exerted on the load cell, there is a deformation on the elastic body and thus changes the shapes of the strain gauges. Therefore, there are changes in the electrical resistances of the strain gauges which are transformed
10
1 Introduction
Strain gauge Elastic body
Strain gauge Housing Strain gauge Housing Elastic body
Elastic body
(a)
(b)
Fig. 1.9 Different kinds of load cells: a structure; b commercially available load cells
to the voltage change via the electrical circuit in the load cell. Load cells are very widely used in many applications in both industrial and medical technologies. One common application in our daily life is electronic weigh scales.
1.3.1.5
Piezoresistive Force Sensor
Usually, the change in electrical resistance of a strain gauge is mainly due to the geometric deformation of the resistor. Besides this strain gauge effect, the piezoresistive effect of a material can also lead to a change in electrical resistance (resistivity change) when a semiconductor or metal is subjected to a mechanical strain. With the discovery of piezoresistive effect, numbers of piezoresistive force sensors have been developed [30, 31]. Due to the similarity between the strain gauge and the piezoresistive force sensor, a typical electrical system for both sensors can be summarized by the block diagram as shown in Fig. 1.10. It consists of a Wheatstone Bridge to transform the electrical resistance change into the voltage output, an amplifier to increase the voltage output signal, a filter to remove the noise in the signal and an analogtodigital converter (ADC) to convert the analog signal into digital signal so that the signal can be used as an input to a microcontroller or a computer.
1.3 Force Sensing
11
Amp
Filter
ADC
Fig. 1.10 Electrical system for strain gauge and piezoresistive force sensor
Fig. 1.11 Force sensing resistors
1.3.2 Force Sensing Resistor A force sensing resistor (FSR), consisting of a conductive polymer, is a device that exhibits electrical resistance changes when a force is applied to its surface. Most FSRs are operated on the basis of the quantum tunneling (QT). As the FSR is compressed, the conductive particles are brought closer, and thus the barrier widths are reduced while the probability of tunneling is increased at the same time. As a result, the electrical resistance of FSR is exponentially decreased. In this case, the electrical resistance of FSR decreases as the applied force on the FSR increases. Unlike a strain gauge, the electrical resistance of an FSR varies dramatically when a force is applied [32]. Figure 1.11 shows some commercially available FSRs in different sizes and shapes. FSRs are thinfilm devices and they can be made into different sizes and shapes. Moreover, due to the large electrical resistance change of FSR with applied force, a voltage divider (see Fig. 1.12) rather than a bridge circuit can be used to convert the electrical resistance change of FSR into the voltage change. Hence, the electrical interface of FSR is extremely simple. The voltage divider output is calculated by R Vin . (1.12) Vout = R + RF S R
12
1 Introduction
Fig. 1.12 Voltage divider
Fig. 1.13 Syringe detection using FSR in a medical device: a medical device prototype; b syringe detection
FSRs have the features of lightweight, lowcost, flexible, and sensitive, but the accuracy is limited. They are suitable for the use of press button. Figure 1.13 shows a medical device, where an FSR is used for syringe detection. The FSR is placed between the syringe’s finger grips and the syringe holder. When the syringe is placed inside the device correctly, a pusher will start to move toward the push button of the plunger. After the pusher touches the plunger, it will continue to move the whole syringe forwards until the FSR is pressed by the finger grips above a certain threshold. Hence, the syringe can be detected automatically by the FSR no matter how many fluids is inside the syringe. FSRs can be also used in wearable devices due to the compact size and flexibility, such as an integrated force sensing insole for gait feedback and training [33], a lowcost insole for kinetic gait analysis [34], a wearable lowcost device for single finger force detection [35], etc.
1.3.3 Piezoelectric Force Sensor A piezoelectric force sensor is a device that employs the piezoelectric effect for the measurement of pressure, acceleration, strain, or force by transforming them to an
1.3 Force Sensing
13
Fig. 1.14 Working principle of piezoelectric force sensor
electrical signal. The piezoelectric force sensor is made of piezoelectric materials which produce electric charge under mechanical stress as shown in Fig. 1.14. Once the piezoelectric materials are stretched or compressed, they generate electric charge which is picked up by the electrodes between the sensor. Different from the piezoresistive effect that causes a change in electrical resistance subjecting to an applied force, the change caused by piezoelectric effect is in electric potential. Piezoelectric force sensors have the features of relatively high resolution, compact size, large measurement range and high stiffness [36, 37]. However, piezoelectric force sensors are mainly used for transient or dynamic force measurements rather than steady or static force measurements [38]. This is because the charge produced by the steady or static force is a fixed amount which can be leak off over time and thus yields a decreasing signal. The applications of piezoelectric force sensors include gripping force sensing in microgripper [39], force and viscoelastic property measurement in minimally invasive surgery [40], stamping machinery analysis [41], cutting force measurement [42], etc.
1.3.4 Other Force Sensors Besides the aforementioned three commonly used types of force sensors, there are other types of force sensors that have been developed and used in industrial and medical applications. In the following, three other types of force sensors as well as some other thoughts in force sensing are presented.
1.3.4.1
Optical Force Sensor
An optical force sensor is a device that measures the force based on a change in light. Significantly, the property of reflected light (e.g., intensity) can be used to measure the force [43, 44]. Figure 1.15 shows the basic working principle of an optical reflectance sensor for force measurement.
14
1 Introduction Reflective plate
Emitter
Detector
Fig. 1.15 Working principle of optical reflectance sensor for force measurement
The optical force sensor mainly consists of an emitter (light source), a light detector, and a reflective plate. Once a force is applied on the reflective plate, there is a displacement or deformation on the plate and thus affects the reflected light intensity to the detector. In other words, as the reflective plate moves closer with applied force (downwards), the detector will detect increased reflected light from the emitter. Based on the measured values on the detector, the applied force can be reconstructed. Comparing to the mechanical force sensors, the optical force sensor can be more robust because the deforming surfaces and the sensing element are physically separated [45]. In addition, the optical force sensor has the features of high spatial resolution for static and dynamic measurements, good reliability, and high repeatability [31].
1.3.4.2
Tactile Sensor
Unlike most force/torque sensors which only measure a simple pointbased force, a tactile sensor is a device that provides rich environmental information from physical interaction with its environment. It can be used to detect not only contact force but also contact configuration, slippage, and object geometry [46]. Tactile sensors are differentiated by its fabrication material and sensing principles. In different tactile sensing systems, a variety of tactile sensors have been adopted to address specific problems. In [47], tactile sensor array was used in wearable devices to detect robotic hand grasping forces and slippage. In [48], a visionbased tactile sensor with great flexibility was integrated into a manipulator to control the insertion of objects in dense boxpacking. Furthermore, a fingerlike tactile sensor, consisting of 19 impedance sensing electrodes, one hydroacoustic pressure sensor and one thermistor, was used in robotic manipulations [49, 50] and robotassisted minimally invasive surgery [51]. Studies have also shown that the tactile sense of touch can be used to understand the properties of the interacted environment, such as surface textures and precise rims [52, 53].
1.3 Force Sensing
1.3.4.3
15
Microelectromechanical Systems Force Sensor
Microelectromechanical systems (MEMS) are microminiature devices consisting of microminiature components such as sensors, actuators, and signal processing integrated and embedded into a single chip while exploiting both electrical/electronic and mechanical features of them [54]. The benefits provided by MEMS mainly include: small size and lightweight (negligible loading errors), high speed (high bandwidth), low power consumption, and convenient massproduction (lowcost). The MEMS technology and devices have very wide applications in sensors, which can be also used for force sensors. The scientific principles of the MEMS sensors are often the same as those of their macro counterparts [54]. There are numerous kinds of MEMS force sensors were designed and reviewed in [55]. For example, in [56], a Silicon Nanowirebased MEMS triaxial force sensor utilizing piezoresistive principle was developed for roboticassisted MIS. In [57], a sixaxis MEMS forcetorque sensor was designed, in which sensing principle is based on measuring the deflection of the sensing element through multiple variable capacitors.
1.3.4.4
Some Other Thoughts
Actually, force sensing can be also indirectly achieved by other sensors in the mechatronic system based on some physics knowledge. For example, the Hooke’s Law states that the force F required to extend an elastic object (e.g., spring) is proportional to the extension x of the object (i.e., F = −kx, where k is a constant). Therefore, a force can be measured via measuring the extension of a spring with the Hooke’s Law. A spring scale is the simplest force gauge that leverages on the Hooke’s law. In [58], a linear potentiometer is used to measure the displacement of a linear spring in a rehabilitation robot with series elastic actuators so as to obtain the force of the actuator. Furthermore, a DC (electric) motor is a very commonly used actuator in a mechatronic system, which is working based on the Lorentz Force. As the Lorentz Force is related to the motor current, the external force applied to the actuation system can be obtained through the motor current reading. There are many research works and various applications on the force estimation based on motor current measurement. In [59], a cutting force prediction method based on current measurement was developed. In [60], a current sensor was used for the measurement of penetration force. In [61], a solution using the motor current to estimate the tool–tissue interaction force was proposed. Similar to the DC motor, the force of an AC motor can also be determined by measuring the motor current. In [62], an intelligent cutting force estimation method based on the feedmotor current measured by a PCBmounted Halleffect transducer was developed. Remarkably, such idea of force estimation via the motor control input inspires the force estimation methods (without force sensors) via disturbance observer, more details of which are presented in Chap. 2.
16
1 Introduction
Fig. 1.16 Linear contact models: a Maxwell; b Kelvin–Voigt; c Kelvin–Boltzmann (Reprinted from [65], Copyright (2019), with permission from Elsevier)
1.4 Modeling of Contacting Object Modeling of contacting object depends on the nature of bodies in contact, which includes their material properties, applied force, contact deformation, and elastic properties [63]. Human tissue exhibits complex mechanical properties, such as nonlinearity, viscoelasticity, anisotropy, etc. Several models have been proposed to describe the viscoelastic behavior of soft tissues. One simple way is to build the force–displacement relationship between the two interacting bodies. Analytical models are usually represented as a combination of springs and dampers [64] and are defined by several components such as: force exerted by the tissue, F(t), when a strain is applied; the deformation, x(t), which is the amount of displacement of the tissue from resting position; its velocity, x(t); ˙ and the elastic and damping coefficients, K and β, respectively. Several linear models are developed based on this modeling technique as shown in Fig. 1.16.
1.4.1 Linear Model (i) Maxwell Model The Maxwell (MW) model is represented by the series of a spring and a damper, as shown in Fig. 1.16a, and is expressed as ˙ − α F˙ M W (t), FM W (t) = β x(t)
(1.13)
where F˙ M W is the derivative of the exerted force and α = β/K . (ii) Kelvin–Voigt Model The Kelvin–Voigt (KV) model consists of a spring in parallel with a damper, as shown in Fig. 1.16b, and is expressed as ˙ FK V (t) = K x(t) + β x(t).
(1.14)
1.4 Modeling of Contacting Object
17
(iii) Kelvin–Boltzmann Model The Kelvin–Boltzmann (KB) model is another linear model which is obtained by adding a spring in series with the KV model, as shown in Fig. 1.16c, and is expressed as ˙ − ψ F˙ K B (t), (1.15) FK B (t) = K K B x(t) + ξ x(t) where K K B = K 1 K 2 /(K 1 + K 2 ), ξ = β K 2 /(K 1 + K 2 ), ψ = β/(K 1 + K 2 ) with K 1 , K 2 , and β denoting the elastic and damping coefficients, respectively.
1.4.2 Nonlinear Model The above viscoelasticity tissue models have the advantages of simplicity and linearity and hence present a simple mathematical model that can be identified in real time using estimation methods such as recursive least squares. However, the linear massdamperspring models, in general, are shown to have physical inconsistencies in terms of power exchange during contact. This is made even more apparent in dynamic models, resulting in negative contact forces during rebound. In 1975, Hunt and Crossley [66] proposed a nonlinear model, namely, the Hunt–Crossley (HC) model as given by ˙ (1.16) FH C = K x n (t) + λx n (t)x(t), where n is a positive scalar that is often close to 1 and depends on the surface geometry, K x n denotes the nonlinear elastic force, and λx n x˙ denotes the nonlinear viscous force due to damping. The main characteristics of the HC model are that the damping term is dependent on the deformation depth and have been shown to model the hysteresis and the power flow of the stress–strain curve much better than the linear models [67].
1.5 Force and Position Control AS mentioned previously, four main control approaches can be used to realize the force and position control. In the following subsections, the basic ideas and components of these control approaches are provided.
1.5.1 Position Control First of all, mechatronic systems are almost always required to provide desired motions for different tasks. Therefore, the position control (or motion control) is needed.
18
1 Introduction
Position Controller
Mechatronic System
Environment
Position Sensor Fig. 1.17 Position control
Figure 1.17 shows a basic position control. As can be seen, it is a closedloop (i.e., feedback) control scheme where the position output of the system is measured by position sensor (e.g., encoder) and compared with the desired position reference. Then, the difference, namely, position error is applied to a properly designed controller to compute a control signal that will bring the position output of the system to the desired value as close as possible. The position control can be represented by the following transfer function: U p (s) = C p (s)[X d (s) − X (s)] = C p (s)E p (s),
(1.17)
where U p (s) is the control output of the position controller, C p (s) is the position control law, X d (s) and X (s) are the desired position and measured position, respectively, E p (s) = X d (s) − X (s) is the position error. A PID controller is commonly used in the position control, which control law in transfer function form is given by C p (s) = K P + K I
1 + K D s, s
(1.18)
where K P is the proportional gain, K D is the derivative gain, and K I is the integral gain.
1.5.2 Force Control Similar to the position control, the force control can be achieved by a closedloop control scheme with force feedback. There are generally two types of force control approaches for mechatronic systems, which are shown in Fig. 1.18. The first type of force control shown in Fig. 1.18a directly output its control signal to the system, which can be described by the following transfer function: U f (s) = C f (s)[Fd (s) − F(s)] = C f (s)E f (s),
(1.19)
1.5 Force and Position Control
Position Controller
19
Mechatronic System
Environment
Position Sensor
Force Controller
Force Sensor
Fig. 1.18 Force control: a direct approach; b positionbased approach
where U f (s) is the control output of the force controller, C f (s) is the force control law, Fd (s) and F(s) are the desired force and measured force, respectively, E f (s) = Fd (s) − F(s) is the force error. The second type of force control shown in Fig. 1.18b is named as positionbased force control, where the force is controlled by adjusting the position output of the system during contact. In other words, this control scheme has two control loops, where the inner loop is position control and the external loop is explicit force control. It can be described by the following transfer function: U p f = C p (s)[U f (s) − X (s)] = C p (s)[C f (s)E f (s) − X (s)],
(1.20)
where U p f (s) is the control output of the positionbased force controller. Significantly, due to the principle of the positionbased force control, the position control can be used to handle the position constrain by limiting the control output of the external force loop. Another advantage of the positionbased force control is its implementation simplicity which can be implemented without modifying the
20
1 Introduction
Force Sensor IS
Force Controller Mechatronic System
S
Environment
Position Controller Posit on Sensor
Fig. 1.19 Hybrid force–position control
existing position control in the mechatronic system. However, the bandwidth of the force control would be limited by the position control in the inner loop.
1.5.3 Hybrid Force–Position Control For fast dynamics in both force and position control, the hybrid force–position (force/position) control can be a good solution. The hybrid approach for force/position control was formulated by Mason [68] and its present form as illustrated in Fig. 1.19 was firstly proposed by Raibert and Craig [15]. The idea of the hybrid force–position control is to divide the task space into two complementary orthogonal subspaces by a selection matrix S (S = diag(Si ) for i = 1, 2, . . . , n, where n is the number of degreeoffreedom (DOF) of the mechatronic system). Then, a force control and a position control that are placed in parallel are applied to the two different subspaces, respectively. Generally, the directions that are positionconstrained by the environment are forcecontrolled (i.e., si = 0) while the unconstrained directions are positioncontrolled (i.e., si = 1). The hybrid force– position control law can be described by the following transfer function: U H f p (s) = SC p (s)[Xd (s) − X(s)] + (I − S)C f (s)[Fd (s) − F(s)], = SC p (s)E p (s) + (I − S)C f (s)E f (s),
(1.21)
1.5 Force and Position Control
21
Force Sensor Force Controller Mechatronic System
Environment
Position Controller Position Sensor Fig. 1.20 Parallel force–position control
where U H f p is the control output vector of the hybrid force–position controller, I is identity matrix, C p (s) and C f (s) are the position controllers and the force controllers for n actuators, respectively, Xd (s) and X(s) are desired position vector and measured position vector, respectively, Fd (s) and F(s) are desired force vector and measured force vector, respectively, E p (s) and E f (s) are position error vector and force error vector, respectively. For the hybrid force–position control, the detailed environment model with little or no uncertainty is needed to be known for determining the subspaces and their corresponding constraint types [69]. Furthermore, a switch between position control and hybrid force–position control is required if the mechatronic system operates from a constraintfree space to a constrained space [70]. These two features (needs environment model, requires switch) make the hybrid approach does not suitably address the requirement of high robustness of the controller.
1.5.4 Parallel Force–Position Control When the accurate environment model is not available, one alternative solution of the hybrid approach is the parallel force–position (force/position) control. The parallel force/position control was first proposed by Chiaverini and Sciavicco [16] and further developed in [71–73]. Figure 1.20 shows the parallel force–position control. As can be observed from Figs. 1.19 and 1.20, the parallel force–position control has similar configuration to the hybrid force–position control where a force con
22
1 Introduction
trol and a position control are acting to the system in parallel. However, unlike the hybrid force–position control, the force control output and the position control output are superimposed (without any selection mechanism) in the parallel force–position control. The parallel force–position control law can be described by the following transfer function: U P f p (s) = C p (s)[X d (s) − X (s)] + C f (s)[Fd (s) − F(s)] = C p (s)E p (s) + C f (s)E f (s),
(1.22)
where U P f p is the control output of the parallel force–position controller. In this approach, both force control and position control are applied along the same directions. To make sure the force control along the constrained task directions, the force control is designed to prevail over or dominate the position control so that the position error would be tolerated along the constrained task directions [74]. Moreover, the parallel force–position control is robust to unpredicted task changes because this approach is implemented based on the force dominance rule without any selection mechanism. Compared to the hybrid approach, the knowledge about the task and environment is not required in the parallel force–position control [69], but the dynamic performance of the parallel approach is slower and its controller design is more complex [72].
1.5.5 Impedance Control All the three forcerelated control approaches presented previously are considered as direct force control which aims at tracking the desired force reference with force feedback directly. To realize the interaction control without direct force control, the impedance control is a good idea to be employed. The impedance control was originally proposed by Hogan [21] in 1985. In the impedance control, the interaction can be regulated by the target impedance model through establishing a virtual dynamics (e.g., massspringdamper system) with contact force and position errors. Instead of regulating or controlling the force and the position individually, the impedance control aims to regulate a desired relationship between the force and position [70, 75]. A simple idea of the impedance control will be presented as follows. Firstly, a mechatronic system with interaction to the environment can be described by X (s) = P(s)[U (s) + F(s)], (1.23) where P(s) represents the plant (i.e., the mechatronic system to be controlled), U (s) is the system input, F(s) is the environmental applied force to the system. Consider a target impedance model given by Z (s)E p (s) = E f (s),
(1.24)
1.5 Force and Position Control
System Model Inversion
23
Mechatronic System
Environment
Impedance Model Position Sensor Fig. 1.21 Impedance control
where Z (s) represents the target impedance model, it should be also noted that the target impedance model will become Z (s)E p (s) = −F(s) if the desired force Fd is zero (i.e., Fd (s) = 0). The target impedance can be simply achieved by the following impedance control law: (1.25) U I mp = −Z (s)E p (s) + Pˆ −1 (s)X (s) − Fd (s), and for Fd = 0,
U I mp = −Z (s)E p (s) + Pˆ −1 (s)X (s),
(1.26)
ˆ where U I mp is the control output of the impedance controller, P(s) ≈ P(s) is the system model. Significantly, Z (s)E p (s) ≈ E f (s) is obtained by substituting (1.25) into (1.23). As a result, the force and position can be controlled at the same time via selecting suitable impedance parameters. The idea of the aforementioned impedance control is illustrated by the block diagram in Fig. 1.21. Moreover, the applied force/external force information can be also used in the impedance control of the mechatronic system (which is usually modeled as a secondorder system), for example, to avoid the acceleration measurement [76–78]. Furthermore, a detailed list showing the variants of impedance control can be found in [69]. Remarkably, the selection of the desired impedance parameters decides the physical behavior of the mechatronic system being controlled as well as a tradeoff between the contact force control performance and the deviation from desired position reference [69, 70]. Specially, a pure position control is obtained with the selection of an infinite impedance while a pure force control is achieved by choosing a zero impedance. One way to select the desired impedance parameters is through qualitative analysis [76]. Some general guidelines for the selection of desired impedance parameters can be found in [76, 79]. Furthermore, the impedance control can achieve better robustness than the hybrid force–position control, but good target impedance model selection and enhanced system performance can be benefited by consid
24
1 Introduction
ering the knowledge of the environmental dynamic characteristics as well as the environment properties during the selection of the desired impedance parameters [70, 80]. Additionally, as accurate dynamics of the mechatronic system is usually needed in the impedance control (see (1.25)), the controller performance is sensitive to modeling error and it will be degraded by the disturbances and uncertainties in the system dynamic parameters. To address this issue, robust control scheme can be considered.
1.6 Organization of the Book In this chapter, the overview of the general concepts and technologies in the area of force and position control is introduced. Further on in this book, the novel ideas and innovations, as well as the results of our recent research works related to this area are presented and reported in detail with the applications in medical technology. The following chapters of this book are organized as follows. In Chap. 2, the force estimation methods without the use of a force sensor are presented. To realize this, the main idea is to estimate the contact force or applied force by leveraging on the disturbance observer (DOB), where the force is treated as an external disturbance. Hence, different kinds of DOBs that can be used in force estimation are presented in detail in that chapter. Moreover, because the friction force or the gravity force sometimes can be also considered as a part of the disturbance, they can affect the force estimation accuracy using DOB. To this end, a summary on the methods of friction compensation and gravity compensation is also given at the end of Chap. 2. In Chap. 3, a new control strategy named forcebased supervisory control is proposed and presented along with a case study. In the case study, the supervisory control is employed to realize the automation process for an ear surgical device, which guide and supervise the motion sequences to complete the surgical procedure based on the force information. From Chap. 4 to Chap. 8, various applicationoriented advanced works focused on force and position control in medical devices are presented in detail. All of these chapters are associated with specific applications. In Chap. 4, a novel control scheme based on force and vision feedback designed for motion stabilization is presented. In Chap. 5, an optimal and robust control scheme, consisting of an optimal PID force controller tuned by the linearquadratic (LQ) optimization algorithm and a DOBbased motion compensator, is designed for stable contact force control on a soft membrane. In Chap. 6, a new insertion method using selective force–position (force/position) control is proposed to achieve fast tube insertion on human tissue with a high success rate. In Chap. 7, an enhanced robust impedance control scheme is developed for the constrained piezoelectric actuatorbased manipulation based on the dynamic coordination of force and position control. In Chap. 8, a force sensor failure detection method based on an advanced disturbance observer is proposed to enhance the safety and reliability while keeping the hardware setup unchanged. At last, the final chapter concludes this book.
References
25
References 1. K.K. Tan, W. Liang, C. Ng, C.W. Gan, H.Y. Lim. Ventilation tube applicator: a revolutionary officebased solution for the treatment of otitis media with effusion, in Handbook of Robotic and ImageGuided Surgery (Elsevier, 2020), pp. 673–692 2. H. Choi, H.S. Kwak, Y.A. Lim, H.J. Kim, Surgical robot for singleincision laparoscopic surgery. IEEE Trans. Biomed. Eng. 61(9), 2458–2466 (2014) 3. Z. Zarrouk, A. Chemori, P. Poignet, Adaptive force feedback control for 3d compensation of physiological motion in beating heart surgery, in 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IEEE, 2010), pp. 1856–1861 4. S.G. Yuen, D.P. Perrin, N.V. Vasilyev, J. Pedro, R.D. Howe, Force tracking with feedforward motion estimation for beating heart surgery. IEEE Trans. Robot. 26(5), 888–896 (2010) 5. T. Ueta, Y. Yamaguchi, Y. Shirakawa, T. Nakano, R. Ideta, Y. Noda, A. Morita, R. Mochizuki, N. Sugita, M. Mitsuishi et al., Robotassisted vitreoretinal surgery: development of a prototype and feasibility studies in an animal model. Ophthalmology 116(8), 1538–1543 (2009) 6. A. Ebrahimi, C. He, M. Roizenblatt, N. Patel, S. Sefati, P. Gehlbach, I. Iordachita, Realtime sclera force feedback for enabling safe robotassisted vitreoretinal surgery, in 2018 40th Annual International Conference of the IEEE Engineering in Medicine and Biology Society (EMBC) (IEEE, 2018), pp. 3650–3655 7. U. Kim, Y.B. Kim, D.Y. Seok, J. So, H.R. Choi, A surgical palpation probe with 6axis force/torque sensing capability for minimally invasive surgery. IEEE Trans. Ind. Electron. 65(3), 2755–2765 (2017) 8. M.W. Gilbertson, B.W. Anthony, Force and position control system for freehand ultrasound. IEEE Trans. Robot. 31(4), 835–849 (2015) 9. H.B. Huang, D. Sun, J.K. Mills, S.H. Cheng, Robotic cell injection system with position and force control: toward automatic batch biomanipulation. IEEE Trans. Robot. 25(3), 727–737 (2009) 10. Y. Xie, D. Sun, C. Liu, H.Y. Tse, S.H. Cheng, A force control approach to a robotassisted cell microinjection system. Int. J. Robot. Res. 29(9), 1222–1232 (2010) 11. Y. Wei, Q. Xu, A survey of forceassisted robotic cell microinjection technologies. IEEE Trans. Autom. Sci. Eng. 16(2), 931–945 (2018) 12. H. Yu, S. Huang, G. Chen, Y. Pan, Z. Guo, Humanrobot interaction control of rehabilitation robots with series elastic actuators. IEEE Trans. Robot. 31(5), 1089–1100 (2015) 13. J. Lee, M. Jin, N. Kashiri, D.G. Caldwell, N.G. Tsagarakis, Inversionfree force tracking control of piezoelectric actuatorsusing fast finitetime integral terminal slidingmode. Mechatronics 57, 39–50 (2019) 14. K.K. Tan, W. Liang, L.P. Phuong, S. Huang, C.W. Gan, H.Y. Lim, Design of a surgical device for officebased myringotomy and grommet insertion for patients with otitis media with effusion. J. Med. Devices 8(3) (2014) 15. M. Raibert, Hybrid position/force control of manipulators. ASME J. Dyn. Syst. Meas. Control 102, 126–133 (1981) 16. S. Chiaverini, L. Sciavicco, Force/position control of manipulators in task space with dominance in force. IFAC Proc. Vol. 21(16), 137–143 (1988) 17. M. Madani, M. Moallem, Hybrid position/force control of a flexible parallel manipulator. J. Frankl. Inst. 348(6), 999–1012 (2011) 18. W. Liang, K.K. Tan, Force feedback control assisted tympanostomy tube insertion. IEEE Trans. Control Syst. Technol. 25(3), 1007–1018 (2016) 19. A.D. Costanza, M.S. Breault, N.A. Wood, M.J. Passineau, R.J. Moraca, C.N. Riviere, Parallel force/position control of an epicardial parallel wire robot. IEEE Robot. Autom. Lett. 1(2), 1186–1191 (2016) 20. G. Wang, Q. Xu, Design and precision position/force control of a piezodriven microinjection system. IEEE/ASME Trans. Mechatron. 22(4), 1744–1754 (2017) 21. N. Hogan, Impedance control: an approach to manipulation: part IIIapplications. J. Dyn. Syst. Meas. Control 107, 17 (1985)
26
1 Introduction
22. S. Jung, T.C. Hsia, R.G. Bonitz, Force tracking impedance control for robot manipulators with an unknown environment: theory, simulation, and experiment. Int. J. Robot. Res. 20(9), 765–774 (2001) 23. H.C. Liaw, B. Shirinzadeh, Robust generalised impedance control of piezoactuated flexurebased fourbar mechanisms for micro/nano manipulation. Sens. Actuators A: Phys. 148(2), 443–453 (2008) 24. Q. Xu, Adaptive discretetime sliding mode impedance control of a piezoelectric microgripper. IEEE Trans. Robot. 29(3), 663–673 (2013) 25. Q. Xu, Precision position/force interaction control of a piezoelectric multimorph microgripper for microassembly. IEEE Trans. Autom. Sci. Eng. 10(3), 503–514 (2013) 26. K.J. Åström, R.M. Murray, Feedback Systems: An Introduction for Scientists and Engineers (Princeton University Press, 2010) 27. C.W. De Silva, Mechatronics: An Integrated Approach (CRC Press, 2004) 28. C.W. De Silva, Mechatronic Systems: Devices Devices, Design, Control, Operation and Monitoring (CRC Press, 2007) 29. F. Balali, J. Nouri, A. Nasiri, T. Zhao, Data Intensive Industrial Asset Management IoTbased Algorithms and Implementation (Springer, 2020) 30. M. Bao, Chapter 6  Piezoresistive Sensing, in Analysis and Design Principles of MEMS Devices, ed. by M. Bao (Elsevier Science, Amsterdam, 2005), pp. 247–304 31. Y. Wei, Q. Xu, An overview of microforce sensing techniques. Sens. Actuators A: Phys. 234, 359–374 (2015) 32. S. Yaniger. Force sensing resistors: a review of the technology, in Electro International, 1991 (IEEE, 1991), pp. 666–668 33. C.B. Redd, S.J.M. Bamberg, A wireless sensory feedback device for realtime gait feedback and training. IEEE/ASME Trans. Mechatron. 17(3), 425–433 (2012) 34. A.M. Howell, T. Kobayashi, H.A. Hayes, K.B. Foreman, S.J.M. Bamberg, Kinetic gait analysis using a lowcost insole. IEEE Trans. Biomed. Eng. 60(12), 3284–3290 (2013) 35. C. Castellini, V. Ravindra, A wearable lowcost device based upon forcesensing resistors to detect singlefinger forces, in 5th IEEE RAS/EMBS International Conference on Biomedical Robotics and Biomechatronics (IEEE, 2014), pp. 199–203 36. Y. Jia, Q. Xu, MEMS microgripper actuators and sensors: the stateoftheart survey. Recent Pat. Mech. Eng. 6(2), 132–142 (2013) 37. D.M. Stef˘ ¸ anescu, M.A. Anghel, Electrical methods for force measurementa brief survey. Measurement 46(2), 949–959 (2013) 38. W. Bolton, Mechatronics: Electronic Control Systems in Mechanical and Electrical Engineering, 6th edn. (Pearson Education, 2015) 39. D.H. Kim, M.G. Lee, B. Kim, Y. Sun, A superelastic alloy microgripper with embedded electromagnetic actuators and piezoelectric force sensors: a numerical and experimental study. Smart Mater. Struct. 14(6), 1265 (2005) 40. N. Narayanan, A. Bonakdar, J. Dargahi, M. Packirisamy, R. Bhat, Design and analysis of a micromachined piezoelectric sensor for measuring the viscoelastic properties of tissues in minimally invasive surgery. Smart Mater. Struct. 15(6), 1684 (2006) 41. X. Li, A. Bassiuny, Transient dynamical analysis of strain signals in sheet metal stamping processes. Int. J. Mach. Tools Manuf. 48(5), 576–588 (2008) 42. G. Totis, M. Sortino, Development of a modular dynamometer for triaxial cutting force measurement in turning. Int. J. Mach. Tools Manuf. 51(1), 34–42 (2011) 43. A.M. Dollar, C.R. Wagner, R.D. Howe, Embedded sensors for biomimetic robotics via shape deposition manufacturing, in The First IEEE/RASEMBS International Conference on Biomedical Robotics and Biomechatronics, 2006. BioRob 2006 (IEEE, 2006), pp. 763–768 44. L.S. Lincoln, M. Quigley, B. Rohrer, C. Salisbury, J. Wheeler, An optical 3D force sensor for biomedical devices, in 2012 4th IEEE RAS & EMBS International Conference on Biomedical Robotics and Biomechatronics (BioRob) (IEEE, 2012), pp. 1500–1505 45. Q. Limited, OptoForce force sensing systems. http://www.quadratecltd.co.uk/
References
27
46. M. Prats, A.P. Del Pobil, P.J. Sanz, Robot Physical Interaction through the combination of Vision, Tactile and Force Feedback (Springer, Berlin, 2013) 47. Y. Wang, X. Wu, D. Mei, L. Zhu, J. Chen, Flexible tactile sensor array for distributed tactile sensing and slip detection in robotic hand grasping. Sens. Actuators A: Phys. 297, 111512 (2019) 48. S. Dong, A. Rodriguez, Tactilebased insertion for dense boxpacking, in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2019), pp. 7953–7960 49. Z. Su, K. Hausman, Y. Chebotar, A. Molchanov, G.E. Loeb, G.S. Sukhatme, S. Schaal, Force estimation and slip detection/classification for grip control using a biomimetic tactile sensor, in 2015 IEEERAS 15th International Conference on Humanoid Robots (Humanoids) (IEEE, 2015), pp. 297–303 50. Z. Deng, Y. Jonetzko, L. Zhang, J. Zhang, Grasping force control of multifingered robotic hands through tactile sensing for object stabilization. Sensors 20(4), 1050 (2020) 51. C. Pacchierotti, D. Prattichizzo, K.J. Kuchenbecker, Cutaneous feedback of fingertip deformation and vibration for palpation in robotic surgery. IEEE Trans. Biomed. Eng. 63(2), 278–287 (2015) 52. T. Taunyazov, H.F. Koh, Y. Wu, C. Cai, H. Soh, Towards effective tactile identification of textures using a hybrid touch approach, in 2019 International Conference on Robotics and Automation (ICRA) (IEEE, 2019), pp. 4269–4275 53. D. De Gregorio, R. Zanella, G. Palli, S. Pirozzi, C. Melchiorri, Integration of robotic vision and tactile sensing for wireterminal insertion tasks. IEEE Trans. Autom. Sci. Eng. 16(2), 585–598 (2018) 54. C.W. De Silva, Sensors and Actuators: Engineering System Instrumentation (CRC Press, 2015) 55. Q. Liang, D. Zhang, G. Coppola, Y. Wang, S. Wei, Y. Ge, Multidimensional mems/micro sensor for force and moment sensing: a review. IEEE Sens. J. 14(8), 2643–2657 (2014) 56. M. Hamidullah, A.T.H. Lin, B. Han, Y.J. Yoon, A sensorized surgical needle with miniaturized mems triaxial force sensor for robotic assisted minimally invasive surgery, in 2012 IEEE 14th Electronics Packaging Technology Conference (EPTC) (IEEE, 2012), pp. 57–60 57. F. Beyeler, S. Muntwyler, B.J. Nelson, A sixaxis mems forcetorque sensor with micronewton and nanonewtonmeter resolution. J. Microelectromech. Syst. 18(2), 433–441 (2009) 58. H. Yu, M.S. Cruz, G. Chen, S. Huang, C. Zhu, E. Chew, Y.S. Ng, N.V. Thakor, Mechanical design of a portable kneeanklefoot robot, in 2013 IEEE International Conference on Robotics and Automation (IEEE, 2013), pp. 2183–2188 59. Y. Altintas, Prediction of cutting forces and tool breakage in milling from feed drive current measurements. J. Eng. Ind. 114(4), 386–392 (1992) 60. Y. Sun, J. Lin, D. Ma, Q. Zeng, P.S. Lammers, Measurement of penetration force using a hallcurrentsensor. Soil Tillage Res. 92(1–2), 264–268 (2007) 61. B. Zhao, C.A. Nelson, Estimating tooltissue forces using a 3degreeoffreedom robotic surgical tool. J. Mech. Robot. 8(5) (2016) 62. X. Li, A. Djordjevich, P.K. Venuvinod, Currentsensorbased feed cutting force intelligent estimation and tool wear condition monitoring. IEEE Trans. Ind. Electron 47(3), 697–702 (2000) 63. I. Kao, K. Lynch, J.W. Burdick, Contact modeling and manipulation. Springer Handbook of Robotics (2008), pp. 647–669 64. Y.C. Fung, Biomechanics: Mechanical Properties of Living Tissues (Springer Science & Business Media, 2013) 65. C. Ng, W. Liang, C.W. Gan, H.Y. Lim, K.K. Tan, Optimization of the penetrative path during grommet insertion in a robotic ear surgery. Mechatronics 60, 1–14 (2019) 66. K. Hunt, Coefficient of restitution interpreted as damping in vibroimpact. J. Appl. Mech. 10, 1–3423596 (1975) 67. A. Pappalardo, A. Albakri, C. Liu, L. Bascetta, E. De Momi, P. Poignet, HuntCrossley model based force control for minimally invasive robotic surgery. Biomed. Signal Process. Control 29, 31–43 (2016)
28
1 Introduction
68. M.T. Mason, Compliance and force control for computer controlled manipulators. IEEE Trans. Syst. Man Cybern. 11(6), 418–432 (1981) 69. M. Schumacher, J. Wojtusch, P. Beckerle, O. von Stryk, An introductory review of active compliant control. Robot. Auton. Syst. 119, 185–200 (2019) 70. S.S. Xie, Advanced Robotics for Medical Rehabilitation: Current State of the Art and Recent Advances, vol. 108 (Springer, 2015) 71. S. Chiaverini, L. Sciavicco, Edgefollowing strategies using the parallel control formulation, in [Proceedings 1992] The First IEEE Conference on Control Applications (IEEE, 1992), pp. 31–36 72. S. Chiaverini, L. Sciavicco, The parallel approach to force/position control of robotic manipulators. IEEE Trans. Robot. Autom. 9(4), 361–373 (1993) 73. S. Chiaverini, B. Siciliano, L. Villani, Force/position regulation of compliant robot manipulators. IEEE Trans. Autom. Control 39(3), 647–652 (1994) 74. B. Siciliano, Parallel force/position control of robot manipulators, in Robotics research (Springer, 1996), pp. 78–89 75. S. Chiaverini, B. Siciliano, L. Villani, A survey of robot interaction control schemes with experimental comparison. IEEE/ASME Trans. Mech. 4(3), 273–285 (1999) 76. M. Tufail, C.W. de Silva, Impedance control schemes for bilateral teleoperation, in 2014 9th International Conference on Computer Science & Education (IEEE, 2014), pp. 44–49 77. C. Ott, R. Mukherjee, Y. Nakamura, A hybrid system framework for unified impedance and admittance control. J. Intell. Robot. Syst. 78(3–4), 359–375 (2015) 78. J.H. Park, Compliance/impedance control strategy for humanoids, in Humanoid Robotics: A Reference, ed. by A. Goswami, P. Vadakkepat (Springer, Netherlands, 2019), pp. 1009–1028 79. W. Khalil, E. Dombre, Modeling, Identification and Control of Robots (ButterworthHeinemann, 2004) 80. Q. Xu, Robust impedance control of a compliant microgripper for highspeed position/force regulation. IEEE Trans. Ind. Electron. 62(2), 1201–1209 (2014)
Chapter 2
Disturbance ObserverBased Force Estimation Without Force Sensors
In some cases, it is difficult to design and integrate a force sensing system (or force sensor) into a mechatronic system or a device which needs to be compact and lightweight. Furthermore, a redundant force sensing system can be helpful to improve the safety and reliability of a mechatronic system or a device, i.e., another force sensing system can be integrated into the device. However, one more force sensing system will increase the weight and size to the overall system which may affect the compact form and portability of the system. To solve this issue and detect/measure the force while using the same hardware setup, disturbance observers (DOBs) can be a good solution. In this chapter, the relevant techniques about the force estimation based on DOB without the use of force sensor are presented. To improve the accuracy on estimating the applied force, there is a need to compensate other forces that are also counted in the disturbance. Therefore, the methods of friction compensation and gravity compensation are introduced in this chapter.
2.1 Disturbance ObserverBased Force Estimation Methods Generally, a system can be represented by the following mathematical model.
x˙ = h(x) + bu, y = r (x),
(2.1)
where x ∈ R p represents the system state, u ∈ Rκ represents the system input, h(x) and r (x) are smooth functions related to x, b is the model parameter vector, and y ∈ Rγ represents the system output. © Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931_2
29
30
2 Disturbance ObserverBased Force Estimation Without Force Sensors
Specially, secondorder systems are frequently used in modeling mechatronic systems and highorder systems can be approximately described by secondorder systems because the system response is characterized by the dominant eigenvalues even for more complicated systems [1]. A singleinputsingleoutput (SISO) mechatronic system can be simply described by the following secondorder linear timeinvariant (LTI) system x¨ = h(x, x) ˙ + bu, (2.2) y = x, where x = [x, x] ˙ T is the system state, and b is the model parameter. Furthermore, the model of the mechatronic system in a secondorder form can be also given by m x¨ + kx + c x˙ = u, (2.3) where m, k, and c are the model parameters. In a springmassdamper system, m is the mass, k is the spring constant and c is the damping coefficient. In a motor system, x is the position, x˙ is the velocity, and x¨ is the acceleration. The transfer function form of the above secondorder system is written as 1 X (s) = . 2 U (s) ms + cs + k
(2.4)
When an external force is acting on the system, the system (2.2) can be represented by x¨ = h(x, x) ˙ + bu + F,
(2.5)
where F represents the external force. Actually, F can be treated as a kind of external disturbance, and thus F can be estimated by a DOB. In the following, three different types of disturbance observers are presented in detail.
2.1.1 Disturbance Observer DOB was first proposed by Ohishi et al. [2] in 1983, which is a kind of observer that observe or estimate the disturbance as well as one of the most widely used robust control tools [3]. The block diagram of a classical DOB is shown in Fig. 2.1. Consider the external force as the disturbance to the system, the system with disturbance can be simply depicted in frequency domain via the following equation: [U (s) + D(s)]H (s) = X (s),
(2.6)
2.1 Disturbance ObserverBased Force Estimation Methods
Position Controller
31
Environment
Fig. 2.1 Classical disturbance observer
where H (s) is the transfer function of the actual system, D(s) is the disturbance that is related to the external force (i.e., D(s) = F(s)/b). Assume that the nominal model is built accurately such that it is very close to the actual system (i.e., Hn (s) ≈ H (s)), then we can have the following observer for disturbance estimation: D(s) ≈ Hn−1 (s)X (s) − U (s),
(2.7)
where Hn (s) is the nominal model without the disturbance and Hn−1 (s) is the inverse of the nominal model. To make Hn−1 (s) proper, a lowpass filter Q(s) is frequently used, and thus (2.7) can be rewritten as a combination of the inverse nominal model and the lowpass filter which is = [Hn−1 (s)X (s) − U (s)]Q(s), D(s)
(2.8)
is the estimated disturbance. where D(s) ≈ D(s) = F(s)/b in the frequency range of As can be observed from (2.8), D(s) Q( jω) ≈ 1. The design of filter Q(s) plays a central role in DOB [4], and the filter Q(s) is needed to be designed carefully. The following two Remarks show some guidelines for the Qfilter design in DOB. Remark 2.1 The filter Q(s) should be designed to close to 1 in all of the frequency range [4], the bandwidth f Δ of the external disturbance must be included in the frequency range (i.e., f c > f Δ , where f c is the filter cutoff frequency). Remark 2.2 The relative degree of the filter Q(s), i.e., the difference between the order of the transfer function’s denominator (i.e., number of poles) and the order
32
2 Disturbance ObserverBased Force Estimation Without Force Sensors
of its numerator (i.e., number of zeros), must not be lower than that of the nominal model Hn (s) so that Q(s)Hn−1 (s) is proper and implementable. Case Study: Secondorder System For the case of a secondorder system shown in (2.4), a secondorder lowpass filter Q(s) as shown in (2.9) is chosen. Q(s) =
1 , s 2 /ωn2 + s/(qωn ) + 1
(2.9)
where ωn = 2π f n , f n is the natural frequency, and q is the quality factor. √ Remarkably, the cutoff frequency f c is defined by f n , where f c = f n when q=1/ 2 (i.e., secondorder lowpass Butterworth filter).
2.1.2 Nonlinear Disturbance Observer Generally, the classical DOB mentioned in the previous subsection is designed and implemented using linear system techniques or based on linearized models [5, 6]. To extend the use of DOB to the systems in the presence of highly nonlinear and coupled dynamics, nonlinear disturbance observer (NDOB) is being investigated in the past decades. NDOB was first proposed by Chen et al. [5] in 2000 where a technically sound NDOB is designed for nonlinear robotic manipulation. The NDOB can be not only designed for nonlinear systems but also used in linear systems, which application areas include independent joint control [5], interaction control [7], sensorless force/torque sensing [8, 9] and control [10], fault diagnosis in mechatronic and robotic systems [11, 12], etc. The main idea of NDOB is based on Lyapunov theory with an auxiliary variable. Consider an affine nonlinear system with external disturbance given by
¯ ¯ x˙ = h(x) + b(x)u + d, y = r (x),
(2.10)
¯ ¯ ¯ where h(x), b(x), r (x) are smooth functions related to x, h(x) consists both linear dynamics h(x) and nonlinear dynamics, and d ∈ Rn represents the disturbance. An auxiliary variable z is defined as
z = dˆ − p(x), ¯ ¯ z˙ = −l(x)z − l(x)[h(x) + b(x)u + p(x)],
(2.11)
where z ∈ Rn represents the internal state of the NDOB, dˆ ∈ Rn represents the estimated disturbance, l(x) is the observer gain of the NDOB, and p(x) is determined
2.1 Disturbance ObserverBased Force Estimation Methods
by l(x) =
∂ p(x) . ∂x
33
(2.12)
Substituting (2.10) and (2.12) into (2.11), we have z˙ = −l(x)z − l(x)[˙x − d + p(x)],
(2.13)
˙ ˙ z˙ = dˆ − p(x) ˙ ∂ p(x) ∂x = dˆ − ∂x ∂t ˙ˆ = d −l(x)˙x.
(2.14)
and
Then, the NDOB holds the following relation: ˙ dˆ − l(x)˙x = −l(x)[dˆ − p(x)] − l(x)[˙x − d + p(x)],
(2.15)
˙ dˆ = −l(x)(dˆ − d).
(2.16)
i.e.,
Define the observer error ed as ˆ ed = d − d,
(2.17)
where ed ∈ Rn , and thus (2.16) becomes
and we also have
˙ dˆ = l(x)ed ,
(2.18)
˙ ˙ e˙ d = d˙ − dˆ = −l(x)ed + d.
(2.19)
Remark 2.3 The observer error of the NDOB asymptotically converges to zero with a proper design of the observer gain l(x) while the disturbance is constant or very slowly varying relative to the observer dynamics (i.e., d˙ ≈ 0). In the case that the disturbance is constant (d˙ = 0), it is easy to have e˙ d = −l(x)ed ,
(2.20)
and therefore the asymptotically stability of the NDOB can be guarantee by a proper selection of the observer gain l(x).
34
2 Disturbance ObserverBased Force Estimation Without Force Sensors
Mechatronic System
Position Controller
Environment
Fig. 2.2 Nonlinear disturbance observer
Remark 2.4 The observer error of the NDOB is bounded with a proper design of the observer gain l(x) while the disturbance is slowly varying with bounded rate (i.e., ˙ < wm where wm is constant). d ˙ < wm ), the In the case that the disturbance is varying with bounded rate (d convergence analysis can be referred to [11, 13]. In summary, the block diagram of the NDOB is depicted by Fig. 2.2. Case Study: Secondorder System For the case of a secondorder system shown in (2.3), consider the system is with a disturbance F like (2.5), we have x¨ = −
c 1 k x − x˙ + u + F, m m m
(2.21)
then, the function p(x, x) ˙ and the observer gain can be designed as p(x, x) ˙ = L x, ˙
(2.22)
where L is a positive constant which is the observer gain. Hence, the disturbance F can be estimated via the following equations: z˙ = −Lz − L[−
c 1 k x − x˙ + u + L x], ˙ m m m
Fˆ = z + L x. ˙
(2.23) (2.24)
2.1 Disturbance ObserverBased Force Estimation Methods
35
More details on the design and analysis of the NDOB on the secondorder system and its application can be also found in Chap. 8.
2.1.3 Extended State Observer Extended state observer (ESO) was first proposed by Han in the 1990s in Chinese [14, 15], which is crucial in active disturbance rejection control (ADRC) [16, 17]. ESO allows the establishment of a new state variable on the controlled object disturbance with a special feedback mechanism. Unlike the classical state observer which only concerns system state, the ESO observes system state and external disturbance. It is a versatile and practical disturbance observer with simple structure. With the implementation and practical effectiveness, ESO has been successfully used in solving many engineering problems [18–20] because of its ability to estimate internal dynamics and external disturbances on the plant in real time. Furthermore, many research works on collision detection and force estimation using ESO have been reported in [21–23] recently.
2.1.3.1
ESO Design Concept
It should be noted that ESO is applicable to most nonlinear multiinputmultioutput (MIMO) systems and SISO systems [15]. However, for the sake of simplicity and clarity, the SISO system is used here to present and show the ESO design concept. Consider a SISO system with disturbance given by ˙ . . . , x ( p−1) (t), t) + bu(t) + d, x ( p) (t) = h(x(t), x(t),
(2.25)
where y = x is the system output, x (i) (t) denotes the ith time derivative of x, d represents the external disturbance, and d = F. Let a = h(x(t), x(t), ˙ . . . , x ( p−1) (t), t) + d and assuming that a is differentiable, the system (2.25) can be rewritten in an augmented state space form as ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨
x = x1 , x˙1 = x2 , .. . x˙ p−1 = x p , ⎪ ⎪ ⎪ ⎪ ⎪ x˙ p = x p+1 + bu ⎪ ⎪ ⎪ ⎪ ⎪ = h(x(t), x(t), ˙ . . . , x ( p−1) (t), t) + d + bu = a + bu, ⎪ ⎪ ⎩ x˙ p+1 = a. ˙
(2.26)
36
2 Disturbance ObserverBased Force Estimation Without Force Sensors
Then, the ESO can be designed as shown in the following equations to estimate the states and total disturbance a x˙ˆi = xˆi+1 + βi (x1 − xˆ1 ) for i = 1, 2, . . . , p, (2.27) x˙ˆ p+1 = β p+1 (x1 − xˆ1 ), where βi (i = 1, . . . , p + 1) are the observer gains to be designed. One effective design of the observer gains is given by
β1 β2 . . . β p β p+1 = ω0 α1 ω02 α2 . . . ω0p α p ω0p+1 α p+1 ,
(2.28)
i.e., βi = ω0i αi for i = 1, 2, . . . , p, p + 1,
(2.29)
where ω0 > 0 determines the observer bandwidth, and the term αi are selected such that the characteristic polynomial s p+1 + α1 s p + α2 s p−1 + · · · + α p s + α p+1 is Hurwitz. Remark 2.5 The unknown dynamics and disturbances can be estimated by the ESO (2.27) with the proper designed observer gains according to (2.29), and the estimation error upper bound of the ESO monotonously decreases with ω0 [24]. The proof of the above Remark can be found in [24]. Now, with the ESO (2.27), the disturbance d is able to be estimated if a is known or identified. Significantly, the ESO can take advantage of the model information while the system model or dynamics is known. In this case, the ESO can be modified in the following form: ⎧ ⎪ ⎪ ⎨
x˙ˆi = xˆi+1 + βi (x1 − xˆ1 ) for i = 1, 2, . . . , p − 1, x˙ˆ p = xˆ p+1 + β p (x1 − xˆ1 ) + bu, ⎪ ⎪ ⎩˙ ˙ˆ xˆ p+1 = β p+1 (x1 − xˆ1 ) + a.
(2.30)
Figure 2.3 illustrates the block diagram of the ESO with known model. Remark 2.6 The estimation error of the ESO (2.30) is asymptotically stable with a proper design of ω0 . The proof and convergence analysis of the ESO (2.30) are given through the case study on the secondorder system in the following. Case Study: Secondorder System For the case of a secondorder system shown in (2.3), consider the system is with a disturbance F like (2.5), by rewriting it into the augmented state space form, we have
2.1 Disturbance ObserverBased Force Estimation Methods
Position Controller
Mechatronic System
Model
ESO
37
Environment
Fig. 2.3 Extended state observer with known model
⎧ x ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ x˙1 x˙2 ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ x˙3
= x1 , = x2 , = x3 + bu
(2.31)
= a + bu, = a, ˙
where a = − mk x − mc x˙ + F in this case. Then, the ESO referring to (2.30) is designed as ⎧ ⎪ x˙ˆ = xˆ2 + β1 (x1 − xˆ1 ), ⎪ ⎨ 1 x˙ˆ2 = xˆ3 + β2 (x1 − xˆ1 ) + bu, ⎪ ⎪ ⎩˙ ˙ˆ xˆ3 = β3 (x1 − xˆ1 ) + a,
(2.32)
where the observer gains β1 , β2 , and β3 are selected as β1 β2 β3 = ω0 α1 ω02 α2 ω03 α3 ,
(2.33)
where ω0 > 0, and the term αi (i = 1, 2, 3) are selected such that the characteristic polynomial s 3 + α1 s 2 + α2 s + α3 is Hurwitz. Let s 3 + α1 s 2 + α2 s + α3 = (s + 1)3 , then the characteristic polynomial of (2.32) is written as follows: s 3 + ω0 α1 s 2 + ω02 α2 s + ω03 α3 = (s + ω0 )3 ,
(2.34)
where ω0 is considered as the observer bandwidth, and the term αi is given as follows:
38
2 Disturbance ObserverBased Force Estimation Without Force Sensors
αi =
( p + 1)! , i!( p + 1 − i)!
(2.35)
where i = 1, 2, 3 and the term p = 2. Hence, the term ω0 in (2.34) becomes the only tuning parameter of the ESO. Let x˜i = xi − xˆi for i = 1, 2, 3, the observer estimation error can be shown as follows: ⎧ ⎪ x˙˜ = x˜2 − 3ω0 x˜1 , ⎪ ⎨ 1 (2.36) x˙˜2 = x˜3 − 3ω02 x˜1 , ⎪ ⎪ ⎩ x˜˙ = a˙ − aˆ˙ − ω3 x˜ , 3 1 0
where x˜i (i = 1, 2, 3) are the estimation errors. ˜i for i = 1, 2, 3, then the observer estimation in (2.36) can be Now, let εi = ωxi−1 0 rewritten as a˙ − a˙ˆ (2.37) ε˙ = ω0 Aε + B p , ω0 T where B = 0 0 1 , and
⎡
⎤ −α1 1 0 A = ⎣−α2 0 1⎦ , −α3 0 0
with A is Hurwitz for the αi (i = 1, 2, 3). Lemma 2.1 There exists a constant ω0 > c such that lim x˜i (t) = 0 for i = 1, 2, 3 t→∞
with the assumption that a = − mk x − kc x˙ + F is globally Lipschitz, where c > 1 is a constant. Proof of Lemma 2.1 A Lyapunov candidate function V is proposed for the ESO as V = ε T Pε,
(2.38)
where P is a positive definite matrix and it is found by solving A T P + P A = −I . The time derivative of V is given by V˙1 = ε˙ T Pε + ε T P ε˙ .
(2.39)
Substituting (2.37) into (2.39), we have a˙ − a˙ˆ V˙ = ω0 ε T (A T P + P A)ε + 2ε T PB p ω0 ˙ a˙ − aˆ = −ω0 ε2 + 2ε T PB p . ω0
(2.40)
2.1 Disturbance ObserverBased Force Estimation Methods
39
˙ˆ ≤ cw X − There exists a constant cw such that a˙ − a X  because the function a T T is globally Lipschitz, where X = x1 x2 x3 and X = xˆ1 xˆ2 xˆ3 . Hence, when ω0 ≥ 1, we have X − X  V˙ ≤ −ω0 ε2 + 2ε T PBcw n ω0 ≤ −ω0 ε2 + ε2 + PBcw 2 ε2 ≤ −(ω0 − c)ε2 ,
(2.41)
where c = 1 + PBcw 2 . Since ω0 is chosen such that it fulfills ω0 − c ≥ 0, we have V˙ ≤ 0.
(2.42)
Lyapunov stability check implies that V˙ is always negative for ε = 0 as t → ∞ for
a stable system. Therefore, x˜i is bounded and lim x˜i = 0 for i = 1, 2, 3. t→∞ Therefore, the disturbance F can be estimated by the following equation with the known or identified system model: c c k k Fˆ = aˆ + xˆ + x˙ˆ = xˆ1 + xˆ2 + xˆ3 . m m m m 2.1.3.2
(2.43)
ModelBased ESO
With the known model information, another form of ESO named modelbased ESO (MESO) has been designed and reported recently in [25, 26], which can be used to estimate the disturbance directly. Here, a brief introduction on this modelbased ESO is given in the following. Consider a system with disturbance like (2.25), it can be represented by the following statespace form: ¯ x˙ = Λx + Γ (u + d), (2.44) y = x = x1 = Cx, where d¯ = d/b, Γ = 0 . . . 0 b , C = 1 0 . . . 0 , and ⎡
0 ⎢ .. ⎢ . ⎢ Λ=⎢ 0 ⎢ ⎣ 0 −a0
⎤ 0 ··· 0 ⎥ . 0 .. 0 0 ⎥ ⎥ . 0 ··· 1 0 ⎥ ⎥ ⎦ 0 ··· 0 1 −a1 · · · −a p−2 −a p−1 1
¯ we have Now, extend the system (2.44) with a new state x p+1 = d,
40
2 Disturbance ObserverBased Force Estimation Without Force Sensors
˙ˆ ¯ d, ¯ x + Γ¯ u + E x˙¯ = Λ¯ ¯ y = x1 = C x¯ ,
(2.45)
where Λ¯ =
Λ Γ Γ ¯ , Γ = , C¯ = C¯ 0 , E¯ = 0 . . . 0 1 . 01× p 0 0
Then, the MESO is designed as
¯ 1 − xˆ1 ), x˙ˆ¯ = Λ¯ xˆ¯ + Γ¯ u + β(x yˆ = xˆ1 ,
(2.46)
where β¯ is the observer gain vector. For simplicity of tuning, β¯ can be designed with the following characteristic equation φ(s) = (s + ω0 ) p+1 , where ω0 determines the observer bandwidth. The observer gain is calculated based on Ackermann’s Formula as shown below. T ⎤−1 ⎡ ⎤ C¯ 0 ⎢ ¯T ¯ ⎥ ⎢ ⎥ ⎢ C Λ ⎥ ⎢0⎥ ¯ ⎢ . ⎥ ⎢.⎥ . β¯ = φ(Λ) ⎢ . ⎥ ⎣.⎦ . ⎣ . ⎦ T p 1 ¯ ¯ C Λ
⎡
(2.47)
By implementing the MESO (2.46), all the states in (2.45) can be estimated. Significantly, the disturbance can be estimated by dˆ = b xˆ p+1 .
(2.48)
Remark 2.7 The estimation error of the MESO is bounded with the following conditions that d is bounded and Λ¯ − β¯ C¯ is Hurwitz. The proof of the above Remark as well as the convergence analysis of MESO can be found in [25].
2.1.4 Discussions In this section, three different types of disturbance observers are presented in detail and the case studies with secondorder systems are given to show how to design and implement these observers. All these aforementioned observers are able to estimate the external disturbance with a certain bandwidth which can be used for force estimation without a force sensor or sensing system. However, in some cases, the external
2.1 Disturbance ObserverBased Force Estimation Methods
41
disturbance is not only the applied force but also other forces such as frictional force, gravity force. For such cases, the compensation schemes are needed to compensate the effects of the frictional force or/and the gravity force. In the following two sections, the compensation schemes for frictional force and gravity force are introduced, respectively.
2.2 Friction Compensation Friction is the force resisting the relative motion of two solid objects [27], which can be observed in all mechanical systems with moving solid bodies. Today, a wide range of approaches for friction compensation have been reported in [28–38]. One way to reduce the effects of friction is to use a control force to cancel it. To this end, one method is to compensate the friction via the precise knowledge of the frictional force. Unfortunately, the physical nature of friction is such that it is very difficult to be determined a priori. Moreover, the amount of friction presenting in a process often varies with time, position, or velocity. Hence, a more practical approach of canceling or minimizing the friction in a motion control system is to design an online friction estimation scheme for estimating the frictional force at each point in time and space. All of these motivate the development of modelbased friction compensation schemes, which require indepth understanding of friction characteristics. The different types of friction models have been studied and they can be classified into two types: (1) static models, including Coulomb model [39], viscous model [40], Stribeck model [41], and timedelay model [42]; (2) dynamical models, including Dahl model [43], LuGre model [44], and Generalized MaxwellSlip (GMS) model [45, 46]. In general, modelbased friction compensation schemes assume that model parameters are constant or slowly varying in nature, and they make use of soft techniques to compensate the effects of friction [33, 47, 48]. However, in certain scenarios, the frictional force may be varying and uncertain, in which, the modelbased compensation schemes may not be valid and adequate. It is therefore necessary to develop intelligent methods to deal with such occurrences. Over the past decade, intelligent soft computing technologies have been widely researched and reported. Specifically, soft computing approaches such as adaptive technique [34, 35, 49–53], fuzzy logic [54], and neural network algorithms [55], have been used to formulate modelfree (nonmodelbased) friction compensation schemes. In summary, the existing friction compensation schemes can be classified into two categories: (1) modelbased friction compensation; (2) modelfree friction compensation.
42
2 Disturbance ObserverBased Force Estimation Without Force Sensors
Fig. 2.4 Friction Compensation (© 2019 IEEE. reprinted, with permission, from [57])
Significantly, a general form of friction compensation in a closedloop control system is shown in Fig. 2.4. Generally, this function is delivered via another loop in the control system which is parallel to the feedback or feedforward control loop [56]. The system output and the feedback error will typically serve as the inputs to the friction compensator. The compensator’s output is then directed as the input to the system compensating the frictional effects. In the following subsections, the key ideas of the commonly used intelligent friction compensation schemes, including both modelbased and modelfree schemes, are presented.
2.2.1 ModelBased Friction Compensation The modelbased friction compensation schemes mainly have two methods: one uses the friction estimation output based on designed friction model to compensate the frictional force directly, the other considers unknown or uncertain model parameters and it presents intelligent learning approaches to deal with the uncertainties.
2.2.1.1
Friction Compensation with Identified Model
A simple way to compensate the friction is to build and identify the friction model and then estimate and compensate the frictional force via the friction model. For example, a relay technique is employed to identify Coulomb, viscous and Stribeck frictional forces, and which identified output is used in a feedforward controller to
2.2 Friction Compensation
43
carry out the necessary compensation (refer to the compensation scheme shown in Fig. 12 of [47]). Consider a mechatronic (motion) system with friction f given by m x¨ = −kx − c x˙ + u − f + F,
(2.49)
where m, k, c are the model parameters, f is the frictional force which can be represented by a structured friction model f m , and F is the bounded disturbance (e.g., applied force from environment). Therefore, the friction compensation scheme u f c can be designed as follows: u f c = fˆ,
(2.50)
where fˆ is the estimated friction which can be obtained by fˆ = f m (x, x) ˙ or fˆ = ˙ ˆ x). ˆ f m (x, If the friction model perfectly matches the actual friction, i.e., we have fˆ = f , and thus the mechatronic system becomes a combination of a linear system and disturbance: m x¨ = −kx − c x˙ + u + F, which is similar to (2.5). Therefore, F can be estimated by a disturbance observer as presented in Sect. 2.1. The commonly used friction models are shown as follows: (1) Classical model: ˙ + ( f s − f c )exp(−x/ ˙ x˙v δ ) + f v x, ˙ f = f c sign(x)
(2.51)
where sign(·) is the sign function, exp(·) is the (natural) exponential function, f c > 0 is the Coulomb friction, x˙ is the velocity, f s is the stiction force, x˙v is the Stribeck velocity, f v is the viscous friction coefficient, and δ > 0 is the shaping factor of the Stribeck function. Remarkably, both f s and x˙v can be determined by empirical experiments. (2) Timedelay model: ˙ − h) + f = c0 x(t
c1 sign(x), ˙ 1 + c2 x˙ 2 (t − h)
(2.52)
where h represents the time delay which can be obtained via experiments, and c0 > 0, c1 > 0, c2 > 0 represent the constants that will be determined by users. (3) LuGre model: x ˙ z = x˙ − σ0 δ(x)z, ˙ g(x) ˙ ˙ f = σ0 z + σ1 z˙ + σ2 x, z˙ = x˙ − σ0
(2.53)
44
2 Disturbance ObserverBased Force Estimation Without Force Sensors
with ˙ x˙v )2 ), g(x) ˙ = α0 + α1 exp(−(x/
(2.54)
where z is the state variable (the deflection of the bristles), σ0 is an equivalent ˙ stiffness, σ1 is the viscous damping, σ2 is the viscous friction coefficient, g(x) models the Stribeck effect, α0 is Coulomb friction, and α0 + α1 is the stiction force.
2.2.1.2
Adaptive LearningBased Friction Compensation
When the model parameters are difficult to identify due to nonlinear or (slowly) timevarying factors, adaptive controller will be a good candidate to be considered for friction compensation. The adaptive scheme is based on a particular, static or dynamical friction model with parameters which are tuned online such that a satisfactory compensation performance is obtained. For example, in [33, 37, 51, 58], different adaptive learning algorithms were proposed to identify the Coulomb friction and which is then used to compensate the frictional force; in [48], an adaptive sliding mode tracking scheme, which can handle a bounded disturbance, was designed to compensate the stickslip friction in a servo system. These adaptive learningbased compensation schemes can be illustrated with a typical adaptive friction compensation example described below, which is also given in [59]. Figure 2.5 shows the block diagram of this method. (i) Static Friction Model Consider a mechatronic system with friction f given by
Fig. 2.5 Adaptive learningbased friction compensation (© 2019 IEEE. reprinted, with permission, from [57])
2.2 Friction Compensation
45
x¨ = (c x˙ + u − f − F)/m,
(2.55)
where c and m are the model parameters, f is the static friction model [41] given by (2.51), and F is the bounded disturbance. Furthermore, it is observed that 0 < exp(−(x/ ˙ x˙v )2 ) < 1 in (2.51) and thus the Stribeck effect can be regarded as a kind of bounded disturbance. Therefore, we have ˙ x˙v )2 ) ≤ f f M , where f f M is a constant. ( f s − f c )exp(−(x/ Define the tracking errors as e = xd − x, e˙ = x˙d − x, ˙
(2.56)
where xd and x˙d are the desired position and velocity, respectively. Then, the adaptive friction compensation takes the form of the following equation: ˙ + fˆv sign(x) ˙ x˙ + fˆsign(sΔ ), u = −cˆ x˙ − mu ˆ f b + fˆc sign(x)
(2.57)
where cˆ and mˆ are the estimates of c and m, respectively, fˆc represents the estimate of f c , fˆv represents the estimate of f v , fˆ is the estimate of f f M , u f b is an additional control input which is used to deal with other nonlinear terms, and sΔ is filtered error based on (2.56). The adaptive parameter learning laws are given by ˙ Δ, c˙ˆ = −kc xs ˙ mˆ = −km u c sΔ , ˙ Δ, f˙ˆc = k f c sign(x)s ˙ ˙ x)s ˙ Δ, fˆv = k f v xsign( ˙ fˆ = k s . f
Δ
This controller consists of a feedback part and an adaptive learning mechanism, where the feedback control term is used to control the dominant part of the system, and the adaptive learning part is used to estimate the parameters of the friction model online. Thus, the adaptive learning term provides an extra compensation for friction. It should also be noted that the proposed learning laws can achieve the stability. The detailed proof can be referred to [59]. In addition, the estimate fˆ can use the projection algorithm to limit its value such that the estimated value does not go to a very large one. (ii) Dynamical Friction Model While considering f as a dynamical friction model, several adaptive compensation schemes [50, 60–64] are available. The following representative example [62] gives a good illustration of these compensation schemes. Consider a mechatronic system given by
46
2 Disturbance ObserverBased Force Estimation Without Force Sensors
m x¨ = u − f + F,
(2.58)
where m denotes the unknown inertia of the system, f is the dynamical friction (LuGre model) shown in the following equations, and F represents the bounded disturbance. The control objective is to design a control signal u such that the actual position x can track the desired position xd (t) closely, even when friction and other uncertainties or disturbances exist, i.e., e = xd − x → 0 as t → ∞. The adaptive control signal u consists of a friction compensation term u f c and a robust feedback term u f b , which is shown in (2.59), u = u f b + u f c.
(2.59)
Here, the compensation term u f c is given by ˙ u f c = ρˆ1 x˙2eq + ρˆ2 zˆ 0 + ρˆ3 δ x˙ zˆ 1 + ρˆ4 x,
(2.60)
where x˙eq = x˙d + ke, the variables ρˆ1 , ρˆ2 , ρˆ3 , ρˆ4 represent the estimates of the unknown parameters m, σ0 , σ1 , σ1 + σ2 , respectively, and zˆ 0 combining with zˆ 1 is to estimate the state z. The observers of zˆ 0 , zˆ 1 are given by z˙ˆ 0 = x˙ − δ x˙ zˆ 0 + γ0 eeq , ˙ eq , z˙ˆ 1 = x˙ − δ x˙ zˆ 1 − γ1 δ xe
(2.61) (2.62)
where γ0 , γ1 are two positive design parameters, and eeq = e˙ + ke. The parameter learning law is given by ρ˙ˆ = −Γ φeeq ,
(2.63)
T T where ρ= ˆ ρˆ1 ρˆ2 ρˆ3 ρˆ4 , Γ is the learning rate matrix, and φ= x˙2eq zˆ 0 −δ x˙ zˆ 1 x˙ . The key feature of this controller is that the internal state z is estimated by the observers (2.61) and (2.62). Therefore, the term (2.60) uses the estimated state to compensate the friction. in the LuGre model is nonIt should be noted that the term δ(x) ˙ = g(x˙x) ˙ differentiable. This implies that it is difficult to design the friction compensation by a backstepping method which requires smooth functions. To address this issue, a smooth function tanh(·) is introduced to replace that term, and thus the backstepping algorithm can be successfully applied to enable friction compensation [65]. With the abovementioned adaptive control mechanisms, the static friction or the dynamical friction can be estimated and compensated. After the friction compensation, it is easy to employ the disturbance observers mentioned in Sect. 2.1 to estimate the external disturbance (i.e., the applied force) for the mechatronic systems.
2.2 Friction Compensation
47
Although the above method can be used to handle the static friction, dynamical friction as well as uncertainties, part of the model like the nonlinear part g(x) ˙ is still assumed to be known. In fact, this nonlinear function is difficult to be obtained a priori. Hence, artificial intelligence algorithms can serve as possible solutions to solve such issue.
2.2.1.3
Artificial Intelligence SystemBased Friction Compensation
Traditional adaptive compensation algorithms are well developed for accurate control of nonlinear systems in the face of unknown parameters in friction models. These techniques, however, require a priori, namely, the linear form representation of friction dynamics (linear parameterizations). The advances in artificial intelligence systems (AISs) render possible the extension to nonlinear parameterizations problem. Existing AIS technologies include genetic algorithms and machine learning. (i) Genetic Algorithmbased Friction Compensation Friction is a natural and highly nonlinear phenomenon. For precise compensation purposes, complex nonlinear models are preferred to be used in representing the friction behavior more accurately. Unfortunately, traditional adaptive learning algorithms, such as those discussed previously, are not suitable for handling nonlinear parameters. Genetic Algorithm is an adaptive heuristic search stemming from the evolutionary ideas of natural selection and genetic variations. It is one of the best ways to cope with the cases in which information is incomplete. Genetic Algorithms have been used to identify various nonlinear friction models as reported in [66–69]. Basically, Genetic Algorithm is an optimization algorithm involving objective functions, constraints, and chromosomes. In Genetic Algorithm, a friction model has to be first converted to a suitable fitness function. The system performance is positively related to the fitness value. This can be explained as follows: Consider a mechatronic system with LuGre model given by m x¨ = u − f.
(2.64)
To estimate the parameters α0 , α1 , x˙s ,σ0 , σ1 , and σ2 , the objective function J and the fitness function F(xi ) are defined as 1 [u i − f (xi , x˙i )]2 , 2 i=1 n
J =
F(xi ) = max(J (xi )) − J (xi ),
(2.65) (2.66)
where u i is the control of the ith chromosome, and xi and x˙i are the position and velocity of the ith chromosome, respectively. Genetic Algorithm covers a population of n chromosomes (solutions) with associated fitness values. It uses multiple concurrent search points (chromosomes), which perform three genetic operations, including reproduction, crossover, and mutation, to produce new search points called offsprings
48
2 Disturbance ObserverBased Force Estimation Without Force Sensors
Fig. 2.6 The flow chart of Genetic Algorithm (© 2019 IEEE. reprinted, with permission, from [57])
for the next iteration [68]. Figure 2.6 depicts the Genetic Algorithm evolution flow chart. The work in [66] used a similar Genetic Algorithm to identify all parameters in the dynamical model. It is observed that after friction compensation using Genetic Algorithm, the system behaves almost like an ideal linear system without friction. To prevent Genetic Algorithm getting stuck to a local optimal value, an improved Genetic Algorithm was developed, where the presented Genetic Algorithm is formulated as a chaos particle swarm algorithm in which the chaos operator is incorporated into the inertia weighted particle swarm algorithm [69, 70]. The result shows that the proposed Genetic Algorithm converges to a global optimum. (ii) Machine Learningbased Friction Compensation Genetic Algorithm is one choice to cope with nonlinear issues, but the whole operation in a Genetic Algorithm is an offline optimization process. It may consume an exceptionally long time with nontrivial problems. This situation necessitates the extension of the adaptive control and current Genetic Algorithm to the “machine learning” paradigms which are capable of obtaining much more sophisticated nonlinear friction models. It has been proven that the major AISs (e.g., neural network, fuzzy logic, etc.) have the capability of approximating any nonlinear continuous functions uniformly. Based on the data training and online learning, these systems can produce the friction model. The works in [55, 71–75] have provided some results in modeling friction and compensation using machine learning methods. For illustration purpose, a simple timedelay model [55] is used here to demonstrate the online
2.2 Friction Compensation
49
compensation scheme. In fact, principles of this method can be applied to more complicated systems. Consider the system (2.58) with the timedelay friction model (2.52) given by m x¨ = u − f + F ˙ − h) + = u − c0 x(t
c1 sign(x) ˙ + F. 1 + c2 x˙ 2 (t − h)
It should be noted that the nonlinear function the following neural network
c1 1+c2 x˙ 2 (t−h)
(2.67)
can be approximated by
c1 = W T Φ(x(t ˙ − h)) + , 1 + c2 x˙ 2 (t − h)
(2.68)
T where W = w1 w2 . . . wn is the constant weight vector of the neural network, T ˙ − h)) φ2 (x(t ˙ − h)) . . . φn (x(t ˙ − h)) is the basis function Φ(x(t ˙ − h)) = φ1 (x(t vector of the neural network, and is the neural network approximation error. The compensation control is given by ˆ T Φ(x(t ˙ − h) + W ˙ − h))sign(x), ˙ u f c = cˆ0 x(t
(2.69)
ˆ T Φ(x(t ˙ − h)) is the neural network representation. The online learning where W law is used to estimate the weight vector W. The resulting compensation diagram is shown in Fig. 2.7. This solution leads to the compensation action which contains the nonlinear function (neural network) and the linear part. It is observed that the
Fig. 2.7 Artificial intelligence system (© 2019 IEEE. reprinted, with permission, from [57])
50
2 Disturbance ObserverBased Force Estimation Without Force Sensors
proposed method is based on the known friction model structure and the neural network is only used for dealing with the nonlinear terms in the friction model.
2.2.2 ModelFree Friction Compensation When the friction model is not available/adequate in a specific application which violates the assumption of a modelbased method, modelfree friction compensation (i.e., compensation scheme without the use of an analytical mathematical model to represent friction) is an alternative. In modelfree friction compensation schemes, the intelligent learning methods and artificial intelligence tools such as neural networks and fuzzy logic are used to learn the friction characteristics and subsequently compensate it. The researches and studies in [76–80] have reported the advances in this area.
2.2.2.1
Neural NetworkBased Friction Compensation
This method employs multilayered feedforward neural network models to represent velocity dependent frictional uncertainties [76]. Consider the dynamics of a base link given by ˙ τ = (Im + Icg + Mr 2 )x¨ + f (x),
(2.70)
where x, x, ˙ and x¨ are the angular position, velocity and acceleration of the link, respectively, Im is the motor inertia, Icg is the link inertia, M is the link mass, r is the link’s radius of gyration, and f is the unmodeled friction. It can be rewritten as follows, which is similar to the mechanical system in (2.58). u = m x¨ + f (x), ˙
(2.71)
where m = (Im + Icg + Mr 2 ). For the compensation scheme, a neural networkbased predictor is proposed uˆ = mˆ x¨ + fˆ(x), ˙
(2.72)
where mˆ is the estimate of the inertia, and fˆ is the estimate of the unknown friction. Then the control with friction compensation is given by ˙ + fˆ(x), ˙ u = m( ˆ x¨d + K p e + K v e)
(2.73)
where e and e˙ are defined in (2.56), K p and K v are the proportional and derivative gains, respectively. K p e + K v e˙ is the feedback controller. The friction estimation fˆ
2.2 Friction Compensation
51
Fig. 2.8 Neural network friction modeling and compensation scheme (© 2019 IEEE. reprinted, with permission, from [57])
is thus obtained from the proposed predictor. Figure 2.8 shows the proposed scheme for the friction compensation. It should be noted that this method does not use any friction model or structure and the frictional force is predicted totally by the neural network. In addition, neural networks can be a basis for other functions or methods to develop the friction compensation scheme. For example, a support vector regression method uses the radial basis function neural network to approximate the unknown friction [79], and the regression estimate is given by solving the optimization problem. It can be also seen from [76, 78, 79] that the modelfree friction compensation scheme uses a neural network to approximate the unknown friction term. It does not depend on a particular friction model. This differentiates it from modelbased friction compensation as shown in [55, 71–73].
2.2.2.2
Fuzzy LogicBased Friction Compensation
Fuzzy logic describes an imprecise or uncertain variable. Fuzzy logic theory emerged from the development of fuzzy sets that were first proposed by Zadeh [81]. The fuzzy logic system can be used to approximate the unknown nonlinear friction and thus form a friction compensator, which overcomes difficulties in modeling the friction [82]. The patent presented in [77] gives a control design with a modelfree friction compensation scheme that incorporates fuzzy logic into the controller. Figure 2.9 illustrates a fuzzy logic estimatorbased position controller. It includes a feedback control loop and a fuzzy logic system which is used to estimate the friction
52
2 Disturbance ObserverBased Force Estimation Without Force Sensors
Fig. 2.9 Fuzzy logic estimatorbased controller (© 2019 IEEE. reprinted, with permission, from [57])
based on the velocity and position error measurements. In the application of the fuzzy logic method, the general operating procedures are (1) (2) (3) (4)
fuzzification of the input variables; building the fuzzy rule base; designing fuzzy reasoning mechanism; defuzzification of the output variable.
The identified friction fuzzy output serves as the input to a filter which produces the compensation force/torque. The advantage of fuzzy logic over the traditional methods is that its decisionmaking mimics those of humans. Moreover, fuzzy logic is very easy to be implemented in various microcontrollers. In general, fuzzy logic uses experience to define the fuzzy functions. It is usually assumed that fuzzy functions are known exactly. Since the realworld problems are very complex, the fuzzy functions may be varied. Therefore, many scientists have shown their interests in developing online estimation methods of the fuzzy functions, where the backpropagation learning algorithm is used to tune the fuzzy functions. For example, Wai and Lin et al. used this idea in [80] to design a robust fuzzy neural network controller to compensate the effects of the nonlinear friction behavior. There are various control schemes that can be applied to friction compensation. The friction compensation is helpful in force estimation as well as useful in precision motion control. More details about this topic can be found in [57].
2.3 Gravity Compensation
53
2.3 Gravity Compensation Gravity (here, it refers to the gravity of Earth) is the force by which the Earth attracts objects toward its center, which can be represented by the following equation. G 0 = mg,
(2.74)
where G represents the gravity, m is the object’s mass, and g denotes the gravitational acceleration (g ≈ 9.8 m2 /s). As the mechatronic systems generally work in gravitational fields, the gravity force will affect the performance and accuracy of both control system [83] and force sensing system once there exists an angle θ between the direction of the object motion and the horizontal. In mathematics, it can be represented by G(θ ) = mgsinθ.
(2.75)
Specially, if the mass m and the angle θ are fixed during the operation using the mechatronic system, the gravity force is a constant and thus is easy to be compensated by a predetermined offset or a highpass filter (with low stopband) on the force sensor output. Furthermore, to reduce or cancel the effect of gravity while the gravity force is not constant during the operation, there are numbers of approaches in terms of both hardware and software have been reported in [84, 85]. The gravity compensation schemes can be mainly summarized into two categories: (1) gravity compensation via mechanical approach; (2) gravity compensation via control approach.
2.3.1 Mechanical Approach The main idea to compensate the effect of gravity via mechanical approach is to compensate or balance the weight that contributes to the gravity, i.e., the mass m in (2.75). Actually, there are many methods to deal with the mass, which can be classified into two types [85]: (1) passive gravity compensation; (2) active gravity compensation. In passive gravity compensation, the balance mechanisms are designed to balance the effect of moving weight. There are two main approaches, one approach uses the counterweights and the other uses the springs [84–86]. For the use of counterweights, the usual way is to add counterweights into the system so that the center of mass of moving part or mechanism is kept stationary or located at the joint axis. The advantage of this approach is that the center of mass is fixed. However, the need for extra mass
54
2 Disturbance ObserverBased Force Estimation Without Force Sensors
increases the weight, the size, and the inertia of the system which can decrease the system dynamic performance. For the use of springs, the mechanism with spring is designed to store the potential energy that is used to counter the gravitational loading. In comparison to the approach using counterweights, the approach using springs needs smaller mass and hence the inertia of the system does not increase too much. However, the mechanical design of the approach using springs can be more complex. In active gravity compensation, auxiliary actuators are employed to drive the system weight so that the effect of gravity can be reduced. Some examples of the active gravity compensation can be found in [87–89]. It should be noted that the aforementioned mechanical approaches for gravity compensation are usually designed for improving the motion control performance of the mechatronic systems, but the ideas behind these approaches can be also used in the gravity compensation for force sensing systems.
2.3.2 Control Approach The mechanical approach presented previously has been proven in both theory and practice that it is effective in gravity compensation. However, extra mechanism or actuation system are normally required in such approach which can increase the weight and overall size of the system or even lead to mechanical interference and motion limitation. To address this issue, the gravity compensation via control approach can be considered. Similar to the friction compensation, the gravity compensation via control approach is to use a control force to cancel it. Consider a mechatronic system with gravity given by x¨ = h(x, x) ˙ + bu + F + G. (2.76) Therefore, if the dynamic model information (e.g., m, etc.) is known and the angle θ is measurable or observable, it is very easy to design a gravity compensation scheme as shown below to compensate the effect of gravity. 1 ˆ u g = − G, b
(2.77)
where Gˆ = mgsin( ˆ θˆ ) ≈ G is the estimated gravity, mˆ is the known mass, and θˆ is the measurement or estimation of the angle θ . To measure the angle θ , an angular sensor or an inertial measurement unit (IMU) can be used. Alternatively, the angle θ can be estimated by an observer via the position sensing information with the system model. Substituting (2.77) into (2.76), we can have x¨ ≈ h(x, x) ˙ + bu + F.
(2.78)
2.3 Gravity Compensation
55
Hence, the effect of gravity can be compensated by the control force u g , which makes the system to be similar to (2.5). Therefore, F can be estimated by a disturbance observer as presented in Sect. 2.1.
2.4 Chapter Summary In this chapter, the force estimation methods based on disturbance observer techniques when treating the force applied to the mechatronic system as a disturbance are presented and discussed in detail. All these force estimation methods are able to estimate the externally applied force, and which are suitable in the cases that the force sensor is not available or the redundant force sensing system for improving system reliability is needed. Furthermore, considering that the frictional force and the gravity force can be also part of the disturbance which affect the estimation on the applied force, the reviews on friction compensation and gravity compensation are presented. In summary, the disturbance observerbased force estimation method with the friction compensation or/and gravity compensation is a good alternative for the force sensing in mechatronic systems.
References 1. K.J. Aström, R.M. Murray, Feedback Systems: An Introduction for Scientists and Engineers (Princeton University Press, 2010) 2. K. Ohishi, Torquespeed regulation of dc motor based on load torque estimation, in IEEJ International Power Electronics Conference, IPECTOKYO, 19833, vol. 2 (1983), pp. 1209– 1216 3. E. Sariyildiz, R. Oboe, K. Ohnishi, Disturbance observerbased robust control and its applications: 35th anniversary overview, in IEEE Transactions on Industrial Electronics (2019) 4. W.H. Chen, J. Yang, L. Guo, S. Li, Disturbanceobserverbased control and related methodsan overview. IEEE Trans. Ind. Electron. 63(2), 1083–1095 (2015) 5. W.H. Chen, D.J. Ballance, P.J. Gawthrop, J. O’Reilly, A nonlinear disturbance observer for robotic manipulators. IEEE Trans. Ind. Electron. 47(4), 932–938 (2000) 6. A. Mohammadi, H.J. Marquez, M. Tavakoli, Nonlinear disturbance observers: design and applications to eulerlagrange systems. IEEE Control Syst. Mag. 37(4), 50–72 (2017) 7. H. Yu, S. Huang, G. Chen, Y. Pan, Z. Guo, Humanrobot interaction control of rehabilitation robots with series elastic actuators. IEEE Trans. Robot. 31(5), 1089–1100 (2015) 8. W. Liang, S. Huang, S. Chen, K.K. Tan, Disturbance observer based small force detection for an ultrasonic motor with application to a surgical device, in IECON 201541st Annual Conference of the IEEE Industrial Electronics Society (IEEE, 2015), pp. 002673–002678 9. M. Saadatzi, D.C. Long, O. Celik, Torque estimation in a wrist rehabilitation robot using a nonlinear disturbance observer, in ASME 2015 Dynamic Systems and Control Conference (Citeseer, 2015), pp. V001T18A001–V001T18A001 10. S. Ueki, T. Mouri, H. Kawasaki, Nonlinear disturbance observer for object grasping/manipulation by multifingered robot hand. IFACPapersOnLine 50(1), 12704–12709 (2017) 11. W. Liang, S. Huang, S. Chen, K.K. Tan, Force estimation and failure detection based on disturbance observer for an ear surgical device. ISA Trans. 66, 476–484 (2017)
56
2 Disturbance ObserverBased Force Estimation Without Force Sensors
12. J. Su, W.H. Chen, B. Li, Disturbance observer based fault diagnosis, in Proceedings of the 33rd Chinese control conference (IEEE, 2014), pp. 3024–3029 13. W.H. Chen, L. Guo, Analysis of disturbance observer based control for nonlinear systems under disturbances with bounded variation 14. J. Han, The “etxtended state observer” of a class of uncertain systems. Control Decis. (in Chinese) 10(1), 85–88 (1995) 15. J. Han, From PID to active disturbance rejection control. IEEE Trans. Ind. Electron. 56(3), 900–906 (2009) 16. B.Z. Guo, Z.L. Zhao, On the convergence of an extended state observer for nonlinear systems with uncertainty. Syst. Control Lett. 60(6), 420–430 (2011) 17. R. Mado´nski, P. Herman, Survey on methods of increasing the efficiency of extended state disturbance observers. ISA Trans. 56, 18–27 (2015) 18. X. Wang, C. Shi, S. Wang, Extended state observerbased motion synchronisation control for hybrid actuation system of large civil aircraft. Int. J. Syst. Sci. 48(10), 2212–2222 (2017) 19. X. Wang, R. Liao, C. Shi, S. Wang, Linear extended state observerbased motion synchronization control for hybrid actuation system of more electric aircraft. Sensors 17(11), 2444 (2017) 20. R. Cui, L. Chen, C. Yang, M. Chen, Extended state observerbased integral sliding mode control for an underwater robot with unknown disturbances and uncertain nonlinearities. IEEE Trans. Ind. Electron. 64(8), 6785–6795 (2017) 21. T. Ren, Y. Dong, D. Wu, K. Chen, Collision detection and identification for robot manipulators based on extended state observer. Control Eng. Pract. 79, 144–153 (2018) 22. B. Chen, Z. Huang, W. Liu, R. Zhang, F. Zhou, J. Peng, A novel adhesion force estimation for railway vehicles using an extended state observer, in IECON 201945th Annual Conference of the IEEE Industrial Electronics Society, vol. 1 (IEEE, 2019), pp. 225–230 23. G. Sebastian, Z. Li, V. Crocher, D. Kremers, Y. Tan, D. Oetomo, Interaction force estimation using extended state observers: an application to impedancebased assistive and rehabilitation robotics. IEEE Robot. Autom. Lett. 4(2), 1156–1161 (2019) 24. Q. Zheng, L.Q. Gao, Z. Gao, On validation of extended state observer through analysis and experimentation. J. Dyn. Syst. Meas. Control 134(2), 024505 (2012) 25. L. Sun, D. Li, Z. Gao, Z. Yang, S. Zhao, Combined feedforward and modelassisted active disturbance rejection control for nonminimum phase system. ISA Trans. 64, 24–33 (2016) 26. X. Geng, S. Hao, T. Liu, Q.G. Wang, Antiwindup design for discretetime systems with time delay via predictorbased extended state observer, in 2019 IEEE 15th International Conference on Control and Automation (ICCA) (IEEE, 2019), pp. 1161–1166 27. J. CourtneyPratt, E. Eisner, The effect of a tangential force on the contact of metallic bodies. Proc. R. Soc. Lond. Ser. A. Math. Phys. Sci. 238(1215), 529–550 (1957) 28. B. Bona, M. Indri, Friction compensation in robotics: an overview, in Proceedings of the 44th IEEE Conference on Decision and Control (2005), pp. 4360–4367 29. H. Olsson, K.J. Astrm, C.C. de Wit, M. Gfvert, P. Lischinsky, Friction models and friction compensation. Eur. J. Control 4(3), 176–195 (1998) 30. M. Kermani, M. Wong, R. Patel, M. Moallem, M. Ostojic, Friction compensation in low and highreversalvelocity manipulators. Proc. IEEE Int. Conf. Robot. Autom. 2004, 4320–4325 (2004) 31. P.E. Dupont, Avoiding stickslip through pd control. IEEE Trans. Autom. Control 39(5), 1094– 1097 (1994) 32. J. Amin, B. Friedland, A. Harnoy, Implementation of a friction estimation and compensation technique. IEEE Control Syst. Mag. 17(4), 71–76 (1997) 33. B. Friedland, Y.J. Park, On adaptive friction compensation. IEEE Trans. Autom. Control 37(10), 1609–1612 (1992) 34. K.M. Misovec, A.M. Annaswamy, Friction compensation using adaptive nonlinear control with persistent excitation. Int. J. Control 72(5), 457–479 (1999) 35. P. Vedagarbha, D.M. Dawson, M. Feemster, Tracking control of mechanical systems in the presence of nonlinear dynamic friction effects. IEEE Trans. Control Syst. Technol. 7(4), 446– 456 (1999)
References
57
36. B. ArmstrongHelouvry, Stickslip and control in lowspeed motion. IEEE Trans. Autom. Control 38(10), 1483–1496 (1993) 37. K.K. Tan, T.H. Lee, S.N. Huang, X. Jiang, Friction modeling adaptive compensation using a relay feedback approach. IEEE Trans. Ind. Electron. 48(1), 169–176 (2001) 38. C. AguilarAvelar, J. MorenoValenzuela, New feedback linearizationbased control for arm trajectory tracking of the furuta pendulum. IEEE/ASME Trans. Mech. 21(2), 638–648 (2016) 39. C.A. Coulomb, Theorie des machines simples, enayant egardau frottemen de leurs parties, et a la roideur dews cordages. Mem. Math Phys. x, 161–342 (1785) 40. O. Reynolds, On the theory of lubrication and its application to mr. beauchamp tower’s experiments, including an experimenal determination of the viscosity of olive oil. Philos. Trans. R. Soc. Lond. 177, 157–234 (1886) 41. B. ArmstrongHlouvry, Control of Machines with Friction (Kluwer Academic Publishers, Boston, 1991) 42. D.P. Hess, A. Soom, Friction at a lubricated line contact operating at oscillating sliding velocities. J. Tribol. 112(1), 147–152 (1990) 43. P. Dahl, A solid friction model. Technical report, TOR0158H310718I1, the Aerospace Corporation, E1 Segundo, CA (1968) 44. C.C.D. Wit, C.H. Olsson, K.J. Astrm, P. Lischinsky, A new model for control of systems with friction. IEEE Trans. Autom. Control 40(3), 419–425 (1995) 45. J. Swevers, F. AlBender, C. Ganseman, T. Prajogo, An integrated friction model structure with improved presliding behaviour for accurate friction compensation. IEEE Trans. Autom. Control 45(4), 675–686 (2000) 46. V. Lampaert, J. Swevers, F. AlBender, Modification of the leuven integrated friction model structure. IEEE Trans. Autom. Control 47(4), 683–687 (2002) 47. S.L. Chen, K.K. Tan, S. Huang, Friction modeling and compensation of servomechanical system with dualrelay feedback approach. IEEE Trans. Control Syst. Technol. 17(6), 1295– 1305 (2009) 48. S.W. Lee, J.H. Kim, Robust adaptive stickslip friction compensation. IEEE Trans. Ind. Electron. 42(5), 474–479 (1995) 49. J.C. MartinezRosas, L. AlvarezIcaza, D. NoriegaPineda, Dynamic friction compensation in velocity control of servoactuators, in Proceedings of 2009 IEEE Control Application, and Intelligent Control Conference (Russia, 2009), pp. 54–59 50. C.I. Huang, L.C. Fu, Adaptive approach to motion controller of linear induction motor with friction compensation. IEEE/ASME Trans. Mechatron. 12, 480–490 (2007) 51. D. Zheng, H. Xu, Adaptive backsteppingflatness control based on an adaptive state observer for a torque tracking electrohydraulic system. IEEE/ASME Trans. Mechatron. 21(5), 2440–2452 (2016) 52. E. Franco, M. Rea, W. Gedroyc, M. Ristic, Control of a masterslave pneumatic system for teleoperated needle insertion in mri. IEEE/ASME Trans. Mechatron. 21(5), 2595–2600 (2016) 53. D.P. Losey, A. Erwin, C.G. McDonald, F. Sergi, M.K. O’Malley, A timedomain approach to control of series elastic actuators: adaptive torque and passivitybased impedance control. IEEE/ASME Trans. Mechatron. 21(4), 2085–2096 (2016) 54. L. Mostefai, M. Denai, O. Sehoon, Y. Hori, Optimal control design for robust fuzzy friction compensation in a robot joint. IEEE Trans. Ind. Electron. 56(10), 3832–3839 (2009) 55. S.N. Huang, K.K. Tan, T.H. Lee, Adaptive friction compensation with a timedelay model. IME J. Syst. Control Eng. 218, 173–183 (2004) 56. V.V. Geffen, A study of friction models and friction compensation. Technical Report DCT 2009. 118, Technische Universiteit Eindhoven (2009) 57. S. Huang, W. Liang, K.K. Tan, Intelligent friction compensation: a review. IEEE/ASME Trans. Mechatron. 24(4), 1763–1774 (2019) 58. J.R. Garretson, W.T. Becker, S. Dubowsky, The design of a friction compensation control architecture for a heavy lift precision manipulator in contact with the environment, in Proceedings of 2006 IEEE International Conference on Robotics and Automation (Orlando, USA, 2006), pp. 31–36
58
2 Disturbance ObserverBased Force Estimation Without Force Sensors
59. K.K. Tan, S.N. Huang, T.H. Lee, Robust adaptive numberical compensation for ripple force and friction of permanent magnet linearmotor. IEEE Trans. Magn. 38(1), 221–228 (2002) 60. B. Bona, M. Indri, N. Smaldone, Rapid prototyping of a modelbased control with friction compensation for a directdrive robot mechatronics. IEEE/ASME Trans. Mechatron. 11(5), 576–584 (2006) 61. P. Lischinsky, C.C. de Wit, G. Morel, Friction compensation for an industrial hydraulic robot. IEEE Control Syst. Technol. Mag. 19(1), 25–33 (1999) 62. L. Xu, B. Yao, Adaptive robust control of mechanical systems with nonlinear dynamic friction compensation. Int. J. Control 81(2), 167–176 (2008) 63. M.R. Sobczyk, V.I. Gervini, E.A. Perondi, M.A.B. Cunha, A continuous version of the LuGre friction model applied to the adaptive control of a pneumatic servo system. J. Frankl. Inst. 353, 3021–3039 (2016) 64. J. Kabzinski, M. Jastrzebski, Practical implementation of adaptive friction compensation based on partially identified LuGre model, in Proceedings of the 19th International Conference on Methods and Models in Automation and Robotics (Miedzyzdroje, Poland, 2014), pp. 699–704 65. J. Yao, W. Deng, Z. Jiao, Adaptive control of hydraulic actuators with LuGre modelbased friction compensation. IEEE Trans. Ind. Electron. 62(10), 6469–6477 (2015) 66. M.A. Krzeminski, Modeling friction through the use of a genetic algorithm. Master’s thesis, University of Waterloo, Canada (2003) 67. L. Wu, Identification of friction parameters based on genetic algorithm in servo control system, in Artificial Intelligence and Computational Intelligence. Lecture Notes in Computer Science, vol. 7002 (Springer, Heidelberg, Germany, 2011), pp. 43–50 68. S.J. Huang, C.M. Chiu, Optimal LuGre friction model identification based on genetic algorithm and sliding mode control of a piezoelectricactuating table. Trans. Inst. Meas. Control 31(2), 181–203 (2009) 69. R. Lai, X. Liu, G. Wu, Z. Chen, Valves dynamic damping characteristics  measurement and identification, in Proceedings of the 50th IEEE Conference on Decision and Control (Orlando, USA, 2011), pp. 970–975 70. Y. Lu, a.D.L.D. Yan, Friction coefficient estimation in servo systems using neural dynamic programming inspired particle swarm search. Appl. Intell. 43(1), 1–14 (2015) 71. S.N. Huang, K.K. Tan, T.H. Lee, Adaptive friction compensation using neural network approximations. IEEE Trans. Syst. Man Cybern. PartC 30(4), 551–557 (2000) 72. S.S. Ge, T.H. Lee, J. Wang, Adaptive nn control of dynamic systems with unknown dynamic friction, in Proceedings of the 39th IEEE Conference on Decision and Control (Australia, Sydney, 2000), pp. 1760–1765 73. K. Sato, K. Tsuruta, A. Shoji, Adaptive h∞ control for linear slider with friction compensation using gradient algorithms with projection, in Proceedings of 2005 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (Monterey, USA, 2005), pp. 1611–1616 74. S.N. Huang, K.K. Tan, K.Z. Tang, Neural Network Control: Theory and Applications (Research Studies Press, Baldock, UK, 2004) 75. H.M. Kim, S.H. Park, S.I. Han, Precise friction control for the nonlinear friction system using the friction state observer and sliding mode control with recurrent fuzzy neural networks. Mechatronics 19(6), 805815 (2009) 76. M.K. Ciliza, M. Tomizukab, Friction modelling and compensation for motion control using hybrid neural network models. Eng. Appl. Artif. Intell. 20(7), 898–911 (2007) 77. Y. Yao, B. Ashrafi, A.H. Larsen, Friction compensation in a vehicle steering system. US Patent, US6901320B2 (2005) 78. Y. Pan, Y. Liu, B. Xu, H. Yu, Hybrid feedback feedforward: an efficient design of adaptive neural network control. Neural Netw. 76, 122–134 (2016) 79. I.B. Tijani, R. Akmeliawati, Support vector regression based friction modeling and compensation in motion control system. Eng. Appl. Artif. Intell. 25, 1043–1052 (2012) 80. R.J. Wai, F.J. Lin, R.Y. Duan, K.Y. Hsieh, J.D. Lee, Robust fuzzy neural network control for linear ceramic motor drive via backstepping design technique. IEEE Trans. Fuzzy Syst. 10, 102–112 (2002)
References
59
81. L.A. Zadeh, Fuzzy sets. Inf. Control 8(3), 338–353 (1965) 82. Y. Tang, D. VelezDiaz, Robust fuzzy control of mechanical systems. IEEE Trans. Fuzzy Syst. 11(3), 411–418 (2003) 83. T. Yamawaki, M. Yashima, Effect of gravity on manipulation performance of a robotic arm, in Proceedings 2007 IEEE International Conference on Robotics and Automation (IEEE, 2007), pp. 4407–4413 84. V. Arakelian, Gravity compensation in robotics. Adv. Robot. 30(2), 79–96 (2016) 85. Y.R. Chheta, R.M. Joshi, K.K. Gotewal, M. ManoahStephen, A review on passive gravity compensation, in 2017 International conference of Electronics, Communication and Aerospace Technology (ICECA), vol. 1 (IEEE, 2017), pp. 184–189 86. J. Boisclair, P.L. Richard, T. Laliberte, C. Gosselin, Gravity compensation of robotic manipulators using cylindrical halbach arrays. Ieee/asme Trans. Mechatron. 22(1), 457–464 (2016) 87. C. Baradat, V. Arakelian, S. Briot, S. Guegan, Design and prototyping of a new balancing mechanism for spatial parallel manipulators. J. Mech. Des. 130(7), 072305 (2008) 88. S. BrudniokGünter, Schreiber, J. Maischberger, Method and handling system for automatically moving a gravitycompensated load body, US Patent, US8812158B2 (2014) 89. B. Lian, T. Sun, Y. Song, X. Wang, Passive and active gravity compensation of horizontallymounted 3rps parallel kinematic machine. Mech. Mach. Theory 104, 190–201 (2016)
Chapter 3
ForceBased Supervisory Control Assisted Surgery
Nowadays, there is an increasing number of mechatronic and robotic systems that are used in robotic surgery. In this chapter, the main focus is on developing a novel “allinone” surgical device and its control system which carry out an officebased surgical treatment for Otitis media with effusion (OME) that overcomes the disadvantages and limitations of the current surgery. OME is a very common ear disease occurring in adults and children alike when the middle ear is infected or the eustachian tube becomes dysfunctional, resulting in accumulation of fluid in the middle ear space. When medication as the first treatment fails, a ventilation tube (grommet) is commonly surgically inserted on the tympanic membrane (TM) of the patient to discharge the fluid. The “allinone” surgical device utilizes a highly integrated structure encompassing key components of a mechanical system, a force sensing system and a control system, all are synergized to enable the surgery to be completed in a short time automatically, precisely, effectively and safely. Significantly, to realize the automatical operation with this surgical device, a welldesigned forcebased supervisory control is employed to assist the surgical procedure, the effectiveness of which is verified by a large number of experiments.
3.1 Introduction OME is a very common ear disease affecting people of all ages worldwide, though more commonly encountered in children. OME generally arises when the eustachian tube that normally drains the middle ear becomes dysfunctional. It leads to fluid accumulation in the middle ear space [1]. In chronic OME, the ear gets infected and conductive hearing loss may manifest. OME also causes body imbalance, discomfort and reduces the quality of one’s daily life [2]. In more serious cases, OME may even © Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931_3
61
62
3 ForceBased Supervisory Control Assisted Surgery
result in irreversible damage to the middle ear structure, further complicating the treatment regimen. When medication as a treatment for OME fails, a ventilation tube (also called “grommet”) is surgically inserted on the TM (i.e., eardrum) so that the accumulated fluid can be drained out [3] from the middle ear space. During this surgery, the patient is usually put under general anesthesia (GA) in an operating room so that the patient can be kept completely still. Specifically, this surgery can be performed under local anesthesia (LA) in adults if they could tolerate the discomfort, but it is still necessary to keep the children under GA. Moreover, some adults still choose to undergo GA if they are worried about the pain or are not able to fully cooperate during the surgery under LA. Thus, the GA is needed in most tube insertion surgery. After the GA, the patient’s head is positioned in the line of view with the surgical microscope, and the ear canal is cleaned with fine ear pick, curette, or suction. Then the surgeon carries out myringotomy (i.e., makes an incision onto the TM by a surgical knife). Finally, a tube is carefully inserted through the incision created on the TM using microforceps and a fine Rosen needle. During the insertion, the inner flange of the tube is gently inserted into the slit so that both outer and inner flanges can hold onto the eardrum in place. It is a small surgery which can be completed in about 15 mins by an experienced surgeon, yet it is enormous in terms of setup and other requirements. The conventional and still predominant method for this surgery has several limitations [4–6]: (1) mostly need for GA with associated risks; (2) highly dependent on surgeon’s skills; (3) costly operating room time (each surgery costs about USD 2,000 [7]); (4) patients in some areas with poor medical infrastructures are deprived of appropriate or prompt treatments; and (5) delayed treatment due to the waiting time for operating room and preparation. Over the past decades, plenty of officebased instruments and devices have been developed for assisting the surgeon to carry out myringotomy or/and tube insertion, some of which can be found in [4–6, 8–14]. In [8, 9], a laser device was used to incise the TM in patients without the need for GA. This treatment allows drainage of the fluid. However, to maintain the perforation which otherwise will close in a couple of weeks, the conventional method of inserting the tube is still needed. The circular shape of the hole also predisposes the tube to earlier extrusion compared to conventional incision pattern on the TM. The needs for very expensive laser sources and the strict credential of skilled surgeons that is needed for such a laser procedure have further resulted in relatively poor uptake and reduced costeffectiveness of using a CO2 laser just in myringotomy. In [10], an innovative surgical device was introduced, combining myringotomy, fluid suction, and tube insertion into one single device, which can perform the surgery under LA or topical anesthesia. This device eliminates the need for changing different surgical instruments during the operation so that the procedure is not interrupted unduly and the required time can be reduced. In [5], another recent tympanotomy tube delivery system was presented. By using this system, the eardrum is punctured and the grommet tube is applied onto the eardrum sequentially and automatically by a mechanism incorporating a special retractable cutter. In [12], an innovative “allinone” device with an embedded endoscope was developed. However, manual operations are still needed while using these devices,
3.1 Introduction
63
i.e., they require surgeon with special skills. Moreover, no sensing and control systems are developed and integrated in all these devices, leading to low precision and intelligence. In this chapter, to overcome the aforementioned disadvantages of the current art, the current approaches are revisited and the extensive setup and requirements are simplified to be a novel precision officebased surgical device involving sensing and control systems. The proposed device allows the officebased, automatic, precise and quick surgical treatment (myringotomy with tube insertion) for OME to be administered to a patient under LA. A simple, automated and quick “point, click and insert” device is developed to carry out both myringotomy and tube insertion in a single procedure, avoiding GA, costly expertise and equipment, treatment delays, and enhancing the precision and performance of the procedures.
3.2 System Description From concept to prototype, the design of the officebased surgical device is shown in Fig. 3.1, which requires to address four key challenges.
Fig. 3.1 Prototype of the surgical device
64
3 ForceBased Supervisory Control Assisted Surgery
3.2.1 Challenges The four key challenges shape the selection and design of the constituent components of the overall device, which are elaborated in the following.
3.2.1.1
Space and Accessibility
The subject of interest to the design of the device is the TM with a mean diameter of about 8 to 10 mm and a nonuniform spatial thickness distribution in the range of 30 to 120 µm [15, 16]. It is a delicate elastic membrane with a convex surface contour and it varies from one person to the next in these characteristics. To reach the eardrum, the device has to traverse the ear canal which is approximately 25 to 35 mm in length measuring from the ear hole (external auditory meatus) to the eardrum and 5 to 10 mm in diameter (adult), with a slight bend along the path. Furthermore, there are ear bones located at the upper portion of the middle ear space behind the TM. In particular, the malleus bone attaches to the inner surface of the TM at the upper part of the TM. This part of the TM is thus outofbounds to myringotomy so as not to hit and interfere with the vibration of this bone, leaving an even smaller area at the lower quadrant of the TM which the device can work on. There are two commercially available types of grommet tubes are commonly used, which are the Tiny Tytan grommet (made of titanium alloy) and the Mini Shah grommet (made of fluoroplastic). In this study, the device is designed primarily for the Tiny Tytan grommet. The shape of a Tiny Tytan grommet is shown in Fig. 3.2. As can be seen, the dimensions of the Tiny Tytan grommet are shown as follows, length: 1.6 mm; flange diameter: 1.5 to 1.6 mm; shaft diameter: 1.2 mm; and inner diameter: 0.76 mm. Therefore, an incision of 1.3 to 1.5 mm is required to be made in a small area of approximately 6.5 to 8 mm2 during myringotomy, and the tube insertion is to be accomplished within this small area. To carry out the full procedure of myringotomy with tube insertion at once, the required tools, including a surgical knife and a forcepslike tool to manipulate the tube, as well as the grommet tube are required to be collectively encased and brought toward the TM through the ear canal. Above all, proper synchronization of the tools and operational steps is required to successfully and safely execute the process with
Fig. 3.2 Tiny Tytan grommet
3.2 System Description
65
adequate sensing mechanisms. This is the main challenge behind the design of the device.
3.2.1.2
Operation Time
Throughout the procedure, the patient is proposed to be under LA instead of GA. The delicate operation has to be accomplished as instantaneously as possible to minimize the trauma on patient and to avoid agitating the patient, causing undue movements that will affect the outcome. Negatively, it should be further noted that many of these patients will be children, and thus an even more difficult task on hand to ensure that they are kept still. The time to complete the myringotomy with tube insertion is most important, which should be much shorter than conventional surgery, and short enough to overcome or alleviate the effects of the undue patient’s movements.
3.2.1.3
Precision and Repeatability
Only a quarter of the TM is the ideal site for the operation as highlighted above. A small incision of the correct size to weave/push in the tube has to be done accurately in this small area. A deformation during incision can lead to discomfort. The incision should not deform the TM unduly or damage it. Thus, the deformation on the TM during the process should be as small as possible. Following the precise incision, the tinysize tube has still to be manipulated to fit into the slit precisely, quickly and again without undue deformation and tearing of the TM. The manipulation of the small parts over a small area and that being further confined by the ear canal collectively require the device and the control actions to be highly precise and repeatable.
3.2.1.4
Diversity
No two TMs are identical. They differ in dimensions, anatomical orientation, flatness as well as mechanical characteristics, and the suitable area for incision also varies from one to another. Except the TM, the ear canal is also varying from patient to patient, it may not be a straight one, which is resulting in more constriction. Some patients with congenital or acquired ear disorders such as granulation tissues or lump on the skin of ear canal have relatively narrower canal and overall diameter. Exactly repeating a successful operation on the next TM may not work. A fair amount of feedback and intelligent adaptation is needed.
66
3 ForceBased Supervisory Control Assisted Surgery
Fig. 3.3 Surgical device for the treatment of OME
3.2.2 Surgical Device Design To realize the functions of the officebased myringotomy with tube insertion and address the aforementioned challenges, the “allinone” surgical device is well designed which mainly consists of a mechanical system, a force sensing system and a control system.
3.2.2.1
Mechanical System
The design of the surgical device for the treatment of OME is shown in Fig. 3.3. It mainly consists of the following components: (1) A linear ultrasonic motor (USM) stage for driving the tool set (and a grommet tube) along Zaxis for completing all the required surgical procedures. The USM is a kind of piezoelectric actuator/motor (PA/PM) generally designed and implemented based on the piezoelectric effect [17]. It offers the advantages of high accuracy, fast response, and fine displacement resolution of the medical device far beyond that is possible manually, which can ensure the precision and repeatability for the surgical procedure. The USM is embedded with a linear encoder with a resolution of 0.1 µm. The minimum incremental motion of the USM is 0.3 µm, the travel range is 19 mm and the maximum velocity achievable is 400 mm/s. (2) A tightly integrated tool set, leveraging on a telescopic structure, is designed to address the space and accessibility challenge allowing the required tools to be brought to the TM proximity at one go, and to allow each of them to carry out its function in the right order and at the right time. It is mainly comprised of
3.2 System Description
67
(2a) a cutter for making the incision on the TM and holding onto the grommet tube through its hollow; and (2b) a hollow holder, which allows the cutter to extend from or retract into, for pushing the tube onto the incision of the TM. (3) A cutter retraction (CR) mechanism for moving the cutter forward and backward inside the holder. (4) A force sensor for providing the contact force information between the tool set and TM. (5) A casing for protecting and supporting the whole device and being manipulated by the user. (6) A foot pedal (not shown in Fig. 3.3) for triggering the automated surgical procedure and facilitating the “pointandclick” concept of the device. The overall size of the device is designed to be fitted for holding by one hand and its weight is around 200 g. The compact and lightweight allows the device to be portable.
3.2.3 Force Sensing System The force sensing system built into the device is mainly to assist each step in the overall procedure to be synchronized in the right and efficient order. A highly sensitive lowcost force sensor with a sensitivity of 0.12 mV/g is utilized to detect different moments of the procedure and provide the required information to the device. Moreover, an amplifier is connected to the force sensor for amplifying the sensor output signal.
3.2.3.1
Force Sensor Installation
The direct force measurement method is employed so that the sensitivity of the sensor can be maximized. The installation of the force sensor is shown in Fig. 3.4a. The sensor is fixed on the fixed plate while the tool set is mounted on the movable base. A linear ball guide way links the base and the plate so that the friction between them can be minimized. The base is constrained by the slide locks so that it can be only moved on the guide way in the negative direction of the Zaxis. The sensor probe contacts to the movable base, so the sensor can measure the force applied on the base along the Zaxis. The force analysis schematic diagram is shown in Fig. 3.4b. As can be seen, because the tool set is fixed on the base, they can be considered as a whole rigid body. Due to the contact between the sensor and the base, any external force exerted on the tool set can be transmitted to the base and measured by the sensor. For the friction F f of the linear guide way, its frictional resistance is from 0.002 to 0.003 while the load on it is about 0.4 N, hence, the static friction is only
68
3 ForceBased Supervisory Control Assisted Surgery
Fig. 3.4 Installation and force analysis of the force sensor: a installation of the force sensor; b force analysis
0.0008 to 0.0012 N which can be ignored. Thus, the measured force Fm can be used to represent the applied force Fn (i.e., Fm ≈ Fn ).
3.2.3.2
Signal Processing of Force Measurement
The motions from the USM stage have an effect on the sensor output due to the inertia force. It acts like a noise on the sensor output. The noiseinfiltrated force measurements can be rather erratic and this phenomenon can severely affect the detection of the milestones, especially when this is done by a computer control system automatically. In addition, it is difficult to carry out the automated surgical procedure robustly with these measurements. To address this problem, a median filter shown in (3.1) is applied. (3.1) yi = median(X i ), where i = 0, 1, 2, . . . , n − 1, yi represents the filtered output which is the median value of the output sequence X i , X i = {xi− j , xi− j+1 , . . . , xi , xi+1 , . . . , xi+k−1 , xi+k } is a subset of the input sequence X centered about the ith element of X with a range from i − j to i + k. The filtered output of the force measurement during the myringotomy with tube insertion procedure on a mock membrane is shown in Fig. 3.5. Significantly, the key steps of the working process are depicted in the figure which include: (1) touch detection (detect whether the grommet tube touches the membrane); (2) myringotomy (push out the cutter from the hollow holder and the tube, and then make an incision on the membrane); (3) tube insertion (retract the cutter inside the tube and then push the tube into the incision); and (4) tube release and tool set withdrawn.
3.2 System Description
69
Fig. 3.5 Filtered output of the force sensor
Also, the sensor can detect whether the tool set hits the ear canal or TM due to the different outputs. Hence, the accidental hitting on the ear canal can be detected to minimize hits along there occurrences. It assists to keep the operations safe compared to a device without such feedback. Additionally, a Notch filter is also applied to eliminate the power source interference at the frequency of 50 Hz.
3.3 ForceBased Supervisory Controller Design The force sensing system is not only used for identifying and monitoring each instance but also for assisting the proposed device to carry out the surgical process automatically. To this end, a forcebased supervisory controller is designed as shown in Fig. 3.6. The key components of the supervisory controller for this surgical device are (1) An instance correlator for identifying each instance during the process according to the force sensing information, the order of the instance and the specified conditions listed in Table 3.1. (2) An action (or motion) selector for selecting the corresponding designed motion trajectory for the current instance based on the output of the correlator. (3) A position controller (in the inner loop) for executing the selected action by controlling the motors to follow the designed motion trajectory precisely. The supervisory controller is a kind of force feedback control system working in an outer loop which supervises the surgical device to carry out the required process automatically, systematically, and sequentially.
70
3 ForceBased Supervisory Control Assisted Surgery
Position Controller
Motion Trajectories
Surgical Device
Membrane
Linear Encoder
Force Sensor
Instance Correlator
Specified Conditions
Action (Motion Sequence) Selector Forcebased Supervisory Controller Fig. 3.6 Forcebased supervisory controller
3.3.1 Position Controller The USM is the main enabler for the motions of the tool set, which controlled by a motion control system to yield the necessary precise and customized motion trajectories to achieve incision and insertion meeting the requirements of an officebased procedure. A precise and highperformance feedback motion control system is designed and the block diagram is shown in Fig. 3.7. In the proposed control system, it mainly consists of two parts: (1) A 2DOF controller (propositionalintegralderivative (PID) feedback controller with feedforward controller) is employed as the main controller. (2) A nonlinear compensation controller, including an inversed friction model function and a sliding mode controller, is designed to compensate the nonlinear dynamic and eliminate the uncertainty. The details behind the design and implementation of the control algorithms can be found in [18–20]. In addition, by treating the applied force on the device as an external disturbance, a disturbance observer based on the Hunt–Crossley contact force model can be used to further reject the effects of the applied force to the motion control system. The idea of using the Hunt–Crossley model in precision motion control can be found in [21].
3.3 ForceBased Supervisory Controller Design
71
Fig. 3.7 Motion controller for USM stage
3.3.2 Supervisory Controller For the supervisory controller, its main design objective is to design the logic and mapping relation in the instance correlator according to the specified tasks. Actually, the supervisory controller is a higher level controller (on top of the position controller) that commands the position controller to take the desired action corresponding to the current instance. In the following, two supervisory controllers focus on different tasks are presented: one is designed for the overall procedure and the other is designed for the tube insertion with optimal penetrative path selection.
3.3.2.1
Supervisory Controller for Overall Procedure
Referring to Fig. 3.5, there are five different milestones during the surgical process: (1) tool set has just engaged the membrane; (2) membrane is just penetrated by the cutter; (3a) tube has contacted the membrane; (3b) tube is inserted onto the membrane (the inner flange of the tube is placed behind the membrane); (4) entire tool set is withdrawn. Based on the force sensor output and the order of these milestones, the time instances corresponding to these fiber different milestones can be identified and differentiated, which enables the synchronization of the various functions of the device to complete myringotomy with tube insertion automatically, minimize the process time and improve the success rate. The mathematic conditions for identifying the five main instances based on the filtered sensor output are designed in Table 3.1. S0 is the normal force value, which is a mean value when there is no external force being applied on the tool set and the
72
3 ForceBased Supervisory Control Assisted Surgery
Table 3.1 Conditions for instance identification from force output Instances Force value Membrane touched Membrane penetrated Tube touched Tube inserted Tool set withdrawn
Fully Under
>S0 + Δt ==S0 ± Δ p >S0 + Δi S0 + Δi ==S0 ± Δ p
Force change ↑ ↓ ↑ ↓ ↓
base. This value is set after the device is settled down in the place, fixed at a certain angle and before the tool set starts to be moved toward the TM. Δt , Δ p , and Δi are the designed threshold values for touch detection, incision detection (whether the incision is made), and insertion detection (whether the tube is inserted), respectively. Remarkably, there are two main situations of the tube insertion shown in Table 3.1: (1) successful insertion; and (2) under insertion, which can be identified via the force sensor. The force signal will remain relatively large if the tube is not inserted, unlike the case if the tube is successfully and fully inserted when the signal will drop to a low level close to an unobstructed movement. In fact, there is a special case of under insertion that can be detected by the force signal as well. This special case happens when the tube is only partially inserted where the force signal is smaller than the usual under insertion case but still larger than the successful insertion case (e.g., {< S0 + 2Δi } & {> S0 + Δi }). By programming the logics stated in the table inside the instance correlator, the surgical device is able to proceed the myringotomy with tube insertion via the sequential actions automatically. For example, when the surgical process is activated, the correlator will first identify whether the TM is touched. If it is not touched, then the selector will select the motion designed for touching as the reference signal to the motion controller (i.e., tool set will be moved forward along Zaxis). The correlator will continuously check if the sensor output fulfills the touching condition. Once the output is larger than the designed threshold, the correlator will identify that the TM is touched, then it will send out a signal to the selector and thus the selector will select the next action (stop the motion for touching and trigger the designed motion for incision), and thus the myringotomy is started. Once the motion for incision is done, the correlator will identify whether the TM is penetrated. If so, the selector will select the motion for the following instance, i.e., tube insertion. If not, the selector will select the motion for incision again until the TM is penetrated. After that, the surgical device will conduct the tube insertion and follow by the tool set withdrawn once the correlator detects that the tube is inserted successfully. Moreover, the system allows for contingency actions when a failure occurs. If the tube is not inserted successfully, the device will stop immediately and its manipulation will be switched to a manual control mode. The surgeon will be alerted and be able to carry out different corresponding operations, dealing with the different failure cases identified by the
3.3 ForceBased Supervisory Controller Design
73
Fig. 3.8 Program flow chart
force signal, to complete the procedure. Some failures can also be observed visually from the microscope or endoscope. Significantly, the contingency actions can be also designed as some specified automatic motion (e.g., push the tube further forward) driven by the surgical device automatically that can help to insert or reinsert the tube on the membrane. Figure 3.8 shows the program flow chart of the supervisory controller for the overall procedure. The key steps of the working process are programmed to be done automatically and sequentially with the force feedback.
3.3.2.2
Supervisory Controller for Tube Insertion
To account for patient variability, one solution is to identify the biomechanical properties of TM before determining the parameters for the tube insertion path. For example, the grommet tube might be inserted at a very short depth if the TM is thin, but might
74
3 ForceBased Supervisory Control Assisted Surgery
Table 3.2 Types of mock membranes Thin Low elastic modulus High elastic modulus
Membrane 1 Membrane 3
Thick Membrane 2 Membrane 4
require a longer distance for a thicker TM. With the hypothesis that the biomechanical property that affect tube insertion depth the most is the membrane’s thickness and elastic modulus, the neural networkbased pattern classification is used. Similar to how a surgeon might use his/her sense of touch to differentiate the different types of tissues, the force sensor output from the touch and incision sequences is used to differentiate the types of TMs. (i) TM Model The human TM is a thin membrane that separates the external ear from the middle ear and is made up of three layers of tissue: the outer cutaneous layer, the fibrous middle layer and a mucous inner membrane. The mechanical properties such as thickness, strength, and elastic modulus of the TM varies between individuals. To model this TM variability, four different classes of mock membranes are used to model thin/thick, low/high elastic modulus membranes (see Table 3.2). Membrane 1 is a soft polyvinyl chloride (PVC) film with elastic modulus of around 35 MPa and thickness around 13 to 20 µm; Membrane 2 is made up of the same PVC material but with thickness around 25 to 40 µm; Membrane 3 is a linear lowdensity polyethylene (PE) film with elastic modulus of around 225 MPa, and thickness around 12 to 15 µm; and Membrane 4 is made of the same PE film but with thickness around 36 to 55 µm. To identify the four membranes, the force sensor readings during the touch and incision sequences can be used. The mean force profile during the touch and incision sequences is shown in Fig. 3.9. From the figure, it may seem that the incision force is sufficient to identify the membranes. However, by doing that, the accuracy is only 75.6%. This is because the range of the incision force of one type of membrane tends to overlap with another, as shown in the boxplot in Fig. 3.10. (ii) Multilayer Perceptron with Backpropagation To increase the classification accuracy, an artificial neural network is designed and the touch force profile together with the maximum incision force are used as the inputs. The Multilayer Perceptron (MLP) is a singlehiddenlayer feedforward artificial neural network that is widely used for pattern classification. Each node in the hidden layer and the output layer is a neuron that uses a nonlinear activation function, hence it is able to distinguish data that is not linearly separable. The MLP uses backpropagation algorithm, a type of supervised learning technique, for training. The structure of the MLP is shown in Fig. 3.11. For this surgical device application, a simple singlehiddenlayer MLP with 50 inputs, 15 neurons in the hidden layer, and 4 outputs is used to achieve a high success rate. Sigmoid activation function is used in the hidden layer and softmax regression in
3.3 ForceBased Supervisory Controller Design Fig. 3.9 Force profile during touch and incision sequences
Fig. 3.10 Boxplot of the maximum incision force of the different types of mock membranes
Fig. 3.11 Structure of the MLP neural network with one hidden layer
75
76
3 ForceBased Supervisory Control Assisted Surgery
Fig. 3.12 Confusion plot of MLP neural network after training stage
the output layer. The first 49 inputs are the touch force data which tracks the reading from the force sensor every 0.01 s for 0.49 s, and the 50th input is the maximum incision force. A total of 500 experiments were performed, with equal number of datasets acquired for the four membranes. The Neural Pattern Recognition Toolbox (nprtool) in MATLAB is used to train the MLP using the recorded data. The datasets are divided into 70% training set, 15% validation set, and 15% testing set. After training, the MLP is able to achieve 97.4% success rate in identifying the correct membrane. The confusion plot of the MLP is shown in Fig. 3.12. (iii) Tube Insertion Path After the membrane is identified correctly, the correlator will select the tube insertion path corresponding to the class of the identified membrane. For this surgical device, operation time is an important factor as the patient is not under GA. Unfortunately, fast motion generates higher forces and also produces residual vibrations or jerkings which are undesirable. A possible solution is to use an Scurve motion path. The Scurve has the advantages of a smooth curve with no discontinuities that might result in temporary micropositional errors, as well as has less chatter and vibration leading to less wear on mechanical parts. It can be seen as a compromise between procedural time and insertion force, as the velocity reaches the maximum in the middle and has a mild slope at the end such that the tube can be inserted into the incision gently without overshoot. For the tube insertion path, a modified Scurve
3.3 ForceBased Supervisory Controller Design
77
Fig. 3.13 Step response of a critical damped thirdorder system
path with optimized parameters is proposed, where the Scurve to follow the step response of a critically damped system is designed. The step response of a third order critically damped system is a natural asymmetrical Scurve with fast rise time, no overshoot, and slow settling time. This is wellsuited for the tube insertion application which requires fast overall insertion time, yet slow speed at the point of insertion so that the tube is able to be pushed/weaved into the incision gently, without overshoot. It is also very easy to implement because only two parameters are needed. The step response of such a system is shown in (3.2). G(s) =
kω3 , s(s + ω)3
(3.2)
where k is the gain of the system, and ω is the natural frequency of the system. After taking the inverse Laplace transform, the time response of a step input is shown in (3.3) and in Fig. 3.13. g(t) = −ke−ωt − kω(te−ωt ) −
kω2 2 −ωt (t e ) − k. 2
(3.3)
The gain of the step response k can be set as the insertion depth, and its value can be determined empirically. As the size of the incision is slightly smaller than the diameter of the tube, the insertion depth is governed by the mechanical properties of the membrane. A thinner membrane or one with lower elastic modulus will have a lower insertion depth. For the Scurve shown in Fig. 3.13, the velocity at the endpoint is zero, hence the motor will stop once the desired distance is achieved.
78
3 ForceBased Supervisory Control Assisted Surgery
Fig. 3.14 Modified Scurve motion profile (Scurve ending with gentle straightline path)
There are two possible scenarios: (1) the tube is successfully inserted before or at the optimal insertion depth; (2) the tube fails to be inserted at the optimal insertion depth. In the former case, the adaptive insertion algorithm kicks in when the force sensor detects a successful insertion and the next phase (tool set withdrawn) begins. In the latter scenario, the motor stops, natural relaxation of the viscoelastic membrane occurs, causing an inverse backwards motion against the tube. Eventually, the tube can be successful inserted, however, this takes a very long time. The second scenario may occur if there is a localized thickness of the membrane or if the MLP neural network identified the membrane wrongly. Hence, there is a need to modify the Scurve such that the end velocity is at vα mm/s (instead of zero), as shown in Fig. 3.14. Adding to this, there are also a few constraints and parameters for optimality to be considered. Constraints: • Maximum insertion distance from the surface of TM cannot exceed 3 mm. This is because the tympanic cavity has a transverse diameter of 3 to 5 mm [22]. Beyond this, the tool set might hit the bone structure at the back of the middle ear space. • Maximum velocity is set. From experience, the success rate of the procedure drops when the velocity is too high. As the TM is oblique, when the speed of insertion is high, the grommet tube becomes unstable on an oblique surface and insertion fails. Pointer for optimality: • Different gain for the Scurve, k, is set based on the membrane type identified by MLP neural network, as the optimal insertion distance varies between different membranes. Hence, the optimal Scurve motion profile for each type of membrane will be slightly different, although the shape is similar.
3.3 ForceBased Supervisory Controller Design
79
Table 3.3 Parameters for different types of membranes Material ki (mm) ωi Membrane 1 Membrane 2 Membrane 3 Membrane 4
1.98 2.31 2.28 2.63
13.94 12.47 12.75 10.47
tα (s) 0.46 0.51 0.50 0.61
Fig. 3.15 Motion profile for the four types of membranes
Consider the above, the motion trajectory of the modified Scurve is designed as shown in the following equation. yi (t) =
−ki e−ωi t − ki ωi (te−ωi t ) − vα t,
ki ωi2 2 −ωi t (t e ) 2
− k i , t < tα , t ≥ tα
(3.4)
where ki is the optimal tube insertion depth for membrane i (i = 1, 2, 3, 4), ωi is a hypothetical variable that represents the natural frequency in the original step response, and vα is the final velocity. To find out the optimal parameters for the modified Scurve profile of different types of membranes, numbers of experiments have been done on the four membranes to determine their average insertion depth based on the previous insertion trajectory and algorithm. Fifty tests were performed for each type of membrane. The average insertion depth and the corresponding parameters for optimal modified Scurve profile based on a maximum velocity of 8 mm/s are shown in Table 3.3. With all the optimal parameters for different types of membranes, the optimal motion profiles and the velocity profile are shown in Figs. 3.15 and 3.16, respectively. By integrating the supervisory controller for tube insertion into the supervisory controller for overall procedure, we can have an integrated forcebased supervisory
80
3 ForceBased Supervisory Control Assisted Surgery
Fig. 3.16 Velocity profile for the four types of membranes
controller that is able to conduct the automated myringotomy with tube insertion via the optimal insertion path.
3.4 Case Study To verify the effectiveness of the integrated forcebased supervisory controller, a large number of experiments are conducted using the prototype of the surgical device on the mock membranes.
3.4.1 Prototype The system setup with the prototype and the system architecture are shown in Fig. 3.17. The system setup mainly comprises the surgical device, a computer embedded with dSPCA DS1104 control card and power supplies. The overall system is implemented on the control card and programmed via MATLAB/Simulink. The computer provides the graphical user interface (GUI) and manages the whole process with a sampling time of 1 ms.
3.4 Case Study
81
Fig. 3.17 Experimental system: a setup; and b system architecture
3.4.2 Experimental Results In the experiments, all the four types of mock membranes are used and 50 tests are carried out on each membrane. The type of grommet tube used in all the 200 tests is the Tiny Tytan grommet. The insertion peak force, deformation on the membrane during tube insertion, insertion time, maximum absolute tracking error (MaxAE), rootmeansquare tracking error (RMSE), and success rate are examined in the experiments. It should be also noted that a procedure is considered a success if the tube is inserted successful and the total travel distance (calculated from the surface of the TM/membrane to the final insertion depth) is below 3 mm according to constraints mentioned in the previous section. Table 3.4 shows the experimental results for the four types of membranes. As can be seen, the grommet tubes can be successfully inserted on all types of mock membranes with success rate above 98% and insertion time less than 0.45 s. Figure 3.18 shows a successful tube insertion on the mock membrane. With the designed supervisory controller, the surgical device is able to carry out the myringotomy with tube insertion automatically with a very short time. It can be
82
3 ForceBased Supervisory Control Assisted Surgery
Table 3.4 Experimental results for the four types of membranes Peak force Deformation Insertion MaxAE (N) (mm) time (s) (mm) 1 2 3 4
0.329 0.536 0.331 0.504
1.866 2.150 2.125 2.370
0.356 0.415 0.415 0.428
0.026 0.039 0.027 0.035
RMSE (mm)
Success rate (%)
0.019 0.022 0.019 0.025
100 100 100 98
Fig. 3.18 Successful tube insertion
also found from the experimental results that the success rates of inserting the tubes on different mock membranes are almost the same and close to 100%, which implies that the supervisory controller can help the device to achieve guaranteed robustness. Furthermore, one of the criteria for successful tube insertion is to keep the total travel distance beyond the surface of the TM to below 3 mm, where the total travel distance includes the touch distance and the insertion distance. It can be observed that all the deformations on the four types of mock membranes are less than 3 mm which means the safe tube insertion is achieved by using the modified Scurve motion trajectory. Besides that, the Mini Shah grommet is also able to be inserted on the mock membrane successfully by the device with a new design of the tool set patented in [23] and the supervisory controller for the overall procedure. The high success rate (more than 95%) and low insertion force (less than 0.35 N) can be achieved for inserting the Mini Shah grommet under different situations.
3.5 Chapter Summary In this chapter, a novel ear surgical device for officebased myringotomy with tube insertion is developed to address the disadvantages and limitations associated with the conventional surgery as well as the reported devices for similar purposes. The challenges faced in the development of such a device are highlighted and they are
3.5 Chapter Summary
83
addressed via a highly integrated mechatronic design of the key constituent components of the device. Significantly, a forcebased supervisory controller is designed for conducting the automated, fast, safe, and robust surgical procedure. The device has been put to numbers of tests and the experimental results show that the prototype is able to carry out the procedure with the success rate above 98% using Tiny Tytan grommet. The process time duration needed by the key process of tube insertion is less than 0.45 s, and the overall time for the whole procedure from the time of tube/membrane contact to tube release is kept around 1 s. Significantly, the smart and novel device is able to accomplish the required tasks in an office setting without the issues of the current art. The simplicity of the device also allows the access of patients from developing countries to an effective and affordable treatment of OME.
References 1. J.W. Seibert, C.J. Danner, Eustachian tube function and the middle ear. Otolaryngol. Clin. N. Am. 39(6), 1221–1235 (2006) 2. R. D’Eredit`a, R.R. Marsh, S. Lora, K. Kazahaya, A new absorbable pressureequalizing tube. Otolaryngol. Head Neck Surg. 127(1), 68–72 (2002) 3. S. McDonald, C.L. Hewer, D. Nunez, Grommets (ventilation tubes) for recurrent acute otitis media in children. Cochrane Database Syst. Rev. (4) (2008) 4. L. Brodsky, P. Brookhauser, D. Chait, J. Reilly, E. Deutsch, S. Cook, M. Waner, S. Shaha, E. Nauenberg, Officebased insertion of pressure equalization tubes: the role of laser  assisted tympanic membrane fenestration. Laryrigoscope 109(12), 2009–2014 (1999) 5. G. Liu, J.H. Morriss, J.D. Vrany, B. Knodel, J.A. Walker, T.D. Gross, M.D. Clopp, B.H. Andreas, Tympanic membrane pressure equalization tube delivery system, US Patent, US8864774B2 (2011) 6. E.J. Shahoian, System and method for the simultaneous automated bilateral delivery of pressure equalization tubes, US Patent, US8052693B2 (2008) 7. G. Gates, Costeffectiveness considerations in otitis media treatment. Otolaryngol. Head Neck Surg. 114(4), 525–530 (1996) 8. O. Friedman, E. Deutsch, J. Reilly, S. Cook, The feasibility of officebased laserassisted tympanic membrne fenestration with tympanpostomy tube insertion: the dupont hospital experience. Int. J. Pediatr. Otorhinolaryngol. 62(1), 33–35 (2002) 9. E. Hassmann, B. Skotnicka, M. Baczek, M. Piszcz, Laser myringotomy in otitis media with effusion: longterm followup. Eur. Arch. Otorhinolaryngol. 261(6), 316–320 (2004) 10. Myringo  By ENT Surgical, A Naiot Venture Accelerator portfolio company (2011). http:// vimeo.com/18496049 11. S.P. cottler, B.W. Kesser, Tube, stent and collar insertion device, US Patent, US200851804A1 (2008) 12. A.V. Kaplan, J. Tartaglia, R. Vaughan, C. Jones, Mechanically registered videoscopic myringotomy/tympanostomy tube placement system, US Patent, US7704259B2 (2010) 13. Y. Katz, R. Shabat, Myringotomy instrument, US Patent, US20090299379A1 (2009) 14. J.Z. Cinberg, P.J. Wilk, Tympanic ventilation tube, applicator, and related technique, US Patent, US5178623A (1993) 15. J. Aernouts, J. Soons, J. Dirckx, Quantification of tympanic membrane elasticity parameters from in situ point indentation measurements: validation and preliminary study. Hear. Res. 263(1–2), 177–182 (2010)
84
3 ForceBased Supervisory Control Assisted Surgery
16. L.C. Kuypers, W.F. Decraemer, J.J. Dirckx, Thickness distribution of fresh and preserved human ear membranes measured with confocal microscopy. Otol. Neurotol. 27(2), 256–264 (2006) 17. L. Liu, K.K. Tan, S.L. Chen, S. Huang, T.H. Lee, Svdbased preisach hysteresis identification and composite control of piezo actuators. ISA Trans. 51(3), 430–8 (2012) 18. K.K. Tan, W. Liang, S. Huang, L.P. Pham, S. Chen, C.W. Gan, H.Y. Lim, Precision control of piezoelectric ultrasonic motor for myringotomy with tube insertion. J. Dyn. Syst. Meas. Control 137(6), 064504 (2015) 19. W. Liang, K.K. Tan, S. Huang, L.P. Plam, H.Y. Lim, C.W. Gan, Control of a 2dof ultrasonic piezomotor stage for grommet insertion. Mechatronics 23(8), 1005–13 (2013) 20. C. Ng, W. Liang, C.W. Gan, H.Y. Lim, K.K. Tan, Optimization of the penetrative path during grommet insertion in a robotic ear surgery. Mechatronics 60(6), 1–14 (2019) 21. C. Ng, W. Liang, C.W. Gan, H.Y. Lim, K.K. Tan. Precision motion control using nonlinear contact force model in a surgical device, in 2019 41st Annual International Conference of the IEEE Engineering in Medicine and Biology Society (EMBC) (IEEE, 2019), pp. 5378–5381 22. C.D. Bluestone, J.O. Klein, Otitis media in infants and children. PMPHUSA (2007) 23. C. Ng, W. Liang, K.K. Tan, C.W. Gan, H.Y.L. Lim, Method and devices for incision and insertion of a ventilation tube, International Patent Application, WO2019054946A1 (2019)
Chapter 4
Stabilization System Based on VisionAssisted Force Feedback
In recent years, an increasing number of auto or semiauto surgical devices and robots are used to assist surgeons in carrying out surgeries. Many of them are designed to provide surgical treatments to patients automatically or semiautomatically in the doctor’s/surgeon’s office rather than the operating room. The officebased surgical device can potentially make the conventional surgical procedures more efficient and more affordable. However, due to the officebased design, it is no longer possible to subject the patient to general anesthesia (GA), i.e., the patient can be awake during the surgical treatment with the device. To ensure the safety and maintain a high success rate, it is very important that the relative motion and the contact force between the surgical device and the contacting object can be stabilized. For example, the officebased ear surgical device mentioned in Chap. 3, it requires constant contact between the tool set of the device and the tympanic membrane (TM). To this end, a control scheme using force and vision feedback is proposed in this chapter. The force feedback controller is a proportional–integral–derivativebased (PIDbased) controller, which is designed for force tracking. The vision feedback controller is a visionbased motion compensator, which is designed to measure and compensate the head motion that is equivalent to TM motion. In addition, the control scheme is implemented and tested in a mockup system and the experimental results will be shown and discussed at the end of this chapter.
4.1 Introduction An “officebased surgical device” is a kind of device which aims to shift the conventional surgical procedures from the operating room to the confines of the doctor’s/surgeon’s office as well as to assist the surgeons to carry out the surgeries on the © Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931_4
85
86
4 Stabilization System Based on VisionAssisted Force Feedback
patients automatically or semiautomatically. These devices offer many advantages such as • remove the need for the extensive and expensive resources of operating room settings including specialized equipments and surgical team; • simplify the surgical procedures and thus avoiding the high dependence on the surgeon’s skills; and • improve the precision, speed, and success rate for the surgery. Therefore, officebased surgical devices can greatly reduce the cost, waiting time as well as operating time and thus it can also increase the access to medical treatment for patients in areas with poor medical infrastructures. However, due to the officebased design, the officebased surgical device cannot subject a patient to GA as usual once the surgery is shifted to the office. There are challenges involved while carrying out an officebased procedure on an awake patient. One key factor to a high success rate is to minimize the relative motion between the device and the patient’s contacting part. This is because that motion will affect the contact force and therefore affect the success rate. More specifically, the procedures may not be successful if the contact forces are out of the allowable threshold. Take the ear surgical device introduced in Chap. 3 as an example, it is very important to maintain the relative motion and the contact force between the tool set of the device and the patient’s head or TM to be kept still after the tool set touches the TM. An excessive contact force may result in overinsertion while a zero or small contact force may lead to underinsertion. Significantly, an excessive contact force may also lead to large discomfort or even injury on the TM. Thus, an effective motion/force stabilization is required. An approach for stabilization is proposed in our previous work [1], combining mechanical restraints and physiological engagement, which can be leveraged on at different times with a strategy to encourage the patient to participate in the process while using the device. This approach helps to stabilize the device directly and reduce the patient’s head motion, but it is still not able to guarantee that the relative motion between the tool set and the TM is minimized and the contact force meets the requirements. Moreover, the mechanical restriction is not the best solution because the patient may be hurt or feel uncomfortable. Thus, a new approach for stabilization by forcing the tool set to follow the patient’s contacting part to sustain a desired contact force can be considered. To this end, a force feedback can be considered to address the stabilization challenge. Recently, several research works that relate to the force control system or stabilization system for surgical devices have been developed in [2–5]. In [3], a haptic feedback system in robotassisted minimally invasive surgery (MIS) was described. In [4], a handheld surgical device with a robotic force tracking system was developed for maintaining a desired contact force in beating heart. In [5], researchers proposed a force control system for maintaining a desired contact force between the target tissue and the probe of the probebased confocal laser endomicroscopy (pCLE). As evident, a force control system can benefit the surgery and improve the stabilization and the performance of the surgical device. Thus, a stabilization system based on force feedback is proposed in this chapter. Nevertheless, some researchers reported
4.1 Introduction
87
that the response of the force control system may not be fast enough for robotics and surgical applications [4, 6]. Noteworthily, some researchers suggested that the use of visual servoing can improve the force control system performance [7]. Thus, a vision feedback scheme is proposed and designed in this chapter for speeding up the response and achieving better performance of the force control. In this chapter, the ear surgical device presented in Chap. 3 will serve as the object of study. Then, a novel stabilization system using force and vision feedback is developed for such automated ear surgical operation so as to minimize the effects of the head motion and guarantee the performance and the safety of the surgical device. Since the force feedback served as the main controller while the vision feedback is designed to assist the force control to achieve better robustness, the force and vision feedback approach presented in this chapter is also named as “visionassisted force feedback”.
4.2 Problem Formulation To develop the stabilization system between the device and the patient, it needs to understand the design of the ear surgical device as well as the characteristics of the human head motion.
4.2.1 Surgical Device Design The preliminary version of the ear surgical device is shown in Fig. 4.1. The device is controlled by a computer and mainly consists of three systems which will be presented in the following subsections. The working prototype of the surgical device has been built and large numbers of tests with the prototype have been done on the mockup system (ear model with mock membrane). The test results show that a success rate close to 100% can be achieved by the device.
4.2.1.1
Mechanical System
The mechanical system consists of two parts: the main mechanical part for achieving the surgical operations in sequence and the supporting apparatus for the device stabilization. In the main mechanical part, the whole tool set is driven by a linear ultrasonic motor (USM) stage while the cuter is moved by a cutter retraction (CR) mechanism. Additionally, a force sensor is installed to measure all the forces that are applied on the tool set. For the supporting apparatus, a mechanical approach using a 7degreeoffreedom (7DOF) universal arm with a locking mechanism is employed to assist the surgeon
88
4 Stabilization System Based on VisionAssisted Force Feedback
Force sensor
Fig. 4.1 Surgical device for the treatment of OME
in stabilizing the device. Before the lock is activated, the device can be moved by the surgeon in all directions and orientations freely to any position. Once the lock is activated, all the joints of the arm are restricted and thus the device on the arm is fixed and stabilized. The universal arm can provide enough DOF and accuracy for the manipulation.
4.2.1.2
Motion Control System
The motion control system is designed to yield the necessary precise and customized motion profile, utilizing the USM stage, for the device. The USM stage controlled
4.2 Problem Formulation
89
by the motion control system drives the tool set to achieve the incision and insertion and meet the requirements of the officebased procedure.
4.2.1.3
Sensing System
The sensing system in the surgical device mainly refers to the force sensing system which is designed to provide the force information inside the ear canal to the surgeon and the device during the surgery. Significantly, the force sensing system is the key component for realizing the force feedbackbased stabilization. A highly sensitive lowcost force sensor with a sensitivity of 0.12 mV/g is chosen, which can directly measure the force applied on the tool set. Besides the force sensing system, a visionbased motion measurement system is used for head motion measurement, details of which will be presented in Sect. 4.3. All these three systems mentioned above construct the ear surgical device that is able to conduct the surgical procedure called “myringotomy with (grommet) tube insertion” efficiently and automatically at one go at about 1 s. To ensure a high success rate of the myringotomy with tube insertion, the relative motion and the contact force between the tube and the membrane after they are in contact are required to be within a proper threshold (where the threshold is determined by amounts of experimental tests which is a parameter that can obtain the highest success rate with the minimal contact force).
4.2.2 Human Head Motion As mentioned previously, the device is stabilized by an universal arm. Thus, only the TM’s movement will affect the relative motion between the tool set (with tube) and the TM. Generally, the TM is connected inside the human head. Furthermore, the TM vibration due to the sound is in submicron level ( 0
(5.5)
where pc (x) describes a relation between the displacement of the tool set x and the membrane deformation after the tool set touches the membrane. For pc (x), it can be found that the membrane deformation can follow the displacement of the tool set if the movement of the tool set is at low frequency, while the membrane deformation will response to the displacement of the tool set with a negative difference or a delay if the movement of the tool set is at high frequency. Therefore, the relation pc (x) can be approximated as a lowpass filter. In this work, a secondorder lowpass filter is used to describe the contact effect. Thus, (5.5) can be rewritten in the transfer function form as shown below: Pc (s) =
βc E (s) = 2 . X (s) s + αc s + βc
(5.6)
(iii) Combined Model Combining (5.4) and (5.6) together, we can have the displacement–force relationship between the rigid tool set and the soft membrane when they are in contact (i.e., x > 0). The relationship is give by
5.3 Controller Design 0.6
Deformation (mm)
Fig. 5.3 Displacement input and force sensor output: a displacement input (deformation); b FFT of input; c force sensor output
115
0.4 0.2 0 0.2 0
5
10
15
20
25
30
Time (s)
(a)
Amplitude
10 0
10
5
10 1
10 0
10 1
Frequency (Hz)
Force Sensor Output (V)
(b) 1.5 1 0.5 0 0.5 0
5
10
15
20
25
30
Time (s)
(c)
F (s) βc γv s + βv = Pc (s)Pv (s) = 2 · X (s) s + αc s + βc αv s + 1 b1 s + b0 , = 3 s + a2 s 2 + a1 s + a0
P f n (s) =
(5.7)
where b0 = βαc βv v , b1 = βαc γv v , a0 = αβvc , a1 = βc + ααvc , and a2 = αc + α1v . To find out the model parameters, a triangular wave motion shown in Fig. 5.3a is used. The fast Fourier transform (FFT) of the triangular wave motion is shown in Fig. 5.3b), which shows that the triangular wave motion is a relatively exciting signal that can be beneficial to system identification. This motion is applied to the artificial membrane, where the displacement is measured by the encoder embedded in the USM stage and the contact force is measured by a force sensor integrated in the device. The force output is shown in Fig. 5.3c. Based on these data obtained from the experiments and the model structure presented in (5.7), the interaction model parameters are estimated via the System Identification Toolbox (ident) in MATLAB. The displacement–force interaction model is shown in the following equation:
116
5 Optimal and Robust Contact Force Control on Soft Membrane
Fig. 5.4 Model verification Force Output (V)
1.5 Actual Simulated
1 0.5 0 0.5 0
5
10
15
20
25
30
Time (s)
Force Output (V)
1.5 Actual Simulated
1 0.5 0 0.5 0
5
10
15
20
25
30
Time (s)
P f n (s) =
s3
4101s + 4276 . + 34.52s 2 + 1520s + 2287
(5.8)
To verify the model accuracy, a numerical study using the experimental data is conducted. Figure 5.4a shows the simulation output from (5.8) and the actual contact force output while the system is subjected to the triangle wave motion. To analyze how good the model fits the actual system, a normalized rootmeansquare (NRMS) fitness value in percentage is used, which is defined by (5.9). As can be seen, the model output matches the actual system output very well, where the fitness value is close to 90%. The output difference may be attributed to the nonlinear dynamics of the soft membrane which can be treated as the uncertainty. f it = 1 −
y − yˆ y − mean(y)
× 100%.
(5.9)
Moreover, for a comparison purpose, the models based on pure viscoelastic behavior and pure contact effect (with a fixed DC gain K DC ) are also built and identified, respectively. Applying these models in the same simulation, the model errors (rootmeansquare error (RMSE) and maximum absolute error (MaxAE)) are shown in Table 5.1. As can be seen, all the three models can describe most part of the system dynamics and the combined model is the most accurate one. Significantly, the combined model has a 29.5% improvement in terms of the maximum error than the pure viscoelastic model although the improvement on the RMSE is only by 1.6%. This is because the MaxAE appears at the turning points of the displacement (i.e., the high speed/frequency points), where the combined model has a better representation. Hence, it can be concluded that the combined model is more suitable to represent the displacement–force relationship in this application.
5.3 Controller Design
117
Table 5.1 Comparison among different models Model Model error RMSE Pf n Pv K DC Pc
0.0441 0.0448 0.0944
MaxAE 0.1146 0.1625 0.2043
In addition, another kind of motion is also used for the model verification. Two trapezoidal wave motions with different rise times are applied to the system. The simulation output using the combined model and the actual force output is shown in Fig. 5.4b. As can be seen, the combined model is still able to match the actual system well with high fitness value (88.8%) which is similar to the result on the triangle wave motion. Thus, the combined model can well represent the displacement–force relationship.
5.3.1.2
Force Controller Design
To design a PID controller with optimal performance, the LQ optimization algorithm can be a good and effective solution. To apply the LQ optimization algorithm to design the PID controller, the error state model is required. According to (5.7), its observable canonical form in statespace model can be written as shown below: ⎡
⎡ ⎤ ⎤ 0 1 0 0 ⎦uf b1 X˙ f = ⎣ 0 0 1 ⎦ X f + ⎣ −a0 −a1 −a2 b0 − a2 b1
Y f = 1 0 0 X f = F,
(5.10)
T where X f = F F˙ F¨¯ is the state vector and F¨¯ = F¨ − b1 u f . In the case that f d is constant or slowly varying, the force error becomes e f = ¨ Hence, the error state model of (5.7) and (5.10) Fd − F, e˙ f = − F˙ and e¨ f = − F. can be given by (5.11) ⎡
0 ⎢0 ˙ f =⎢ E ⎣0 0
1 0 0 −a0
0 1 0 −a1
AE f + Bu f , where E f =
t 0
e f dτ e f e˙ f F¨¯
T .
⎤ ⎤ ⎡ 0 0 ⎥ ⎢ 0 ⎥ 0 ⎥E + ⎢ ⎥uf ⎦ 1 ⎦ f ⎣ −b1 −a2 −b0 + a2 b1 (5.11)
118
5 Optimal and Robust Contact Force Control on Soft Membrane
Define the cost function as
∞
Jf = 0
(E Tf Q f E f + u Tf r f u f )dτ
(5.12)
with Q f = diag{0, q f , 0, 0}. The control law is given by uf = KfEf,
(5.13)
where K f = K i f K p f K d f K 4 is the controller gain. Remark 5.1 K 4 is set to be zero (i.e., K 4 = 0) to constrain the force controller to be the PID controller form. The optimal solution of a classical infinite horizon linearquadratic regulator (LQR) problem can be obtained based on algebraic Riccati equation (ARE). However, there is no standard closedform solution when there are constraints in the gain matrix. In the literature, projectiongradientbased algorithms already demonstrated their effectiveness in solving the constrained optimization problems [27, 28]. The structural constraint (K 4 = 0) is expressed as a linear function of K f , i.e., C(K f ) = K f M = 0,
(5.14)
where M = diag{0, 0, 0, 1}. The original optimization problem to minimize J f (K f ) is converted to approximated equivalent counterpart, such that the Euclidean distance between the projection dJ gradient in the constrained hyperplane D f to the unconstrained counterpart d K ff is minimized. Denote · as the Euclidean norm of a matrix, the problem is expressed as 2 dJf 1 − Df (5.15) min , Df 2 dK f C(D f ) = 0.
(5.16)
Problems (5.15) and (5.16) are further converted to the equivalent dual problem as 2 d J 1 f T − Df max min + Tr Λ f C(D f ) , Λf Df 2dK f
(5.17)
where Λ f is the Lagrange multiplier associated with the constraint C(D f ) = 0. To execute the projectiongradientbased optimization algorithm, we need to calculate the projection gradient matrix D f . Thus, we have the following lemma: Lemma 5.1 The projection gradient matrix is given by Df =
dJf (I − M), dK f
(5.18)
5.3 Controller Design
where
119
dJf = 2 r f K f + B T P V, dK f
(5.19)
in which P and V are solutions of the Lyapunov equations (A + B K f )T P + P(A + B K f ) + Q f + K Tf r f K f = 0,
(5.20)
(A + B K f )V + V (A + B K f ) + X 0 = 0, X 0 =
(5.21)
T
x0 x0T ,
where initial state value x0 can be assumed to be uniformly distributed over the surface of a unit sphere. Proof of Lemma 5.1 The derivation of unconstrained gradient matrix d J f /d K f can be found in [27]. Then, we need to project d J f /d K f onto the constrained hyperplane to get D f . The necessary and sufficient optimal solution to the dual problem (5.17) is given by Df −
dJf ∂ T Tr Λ f C(D f ) = 0. + dK f ∂Df
(5.22)
dJf + Λ f M T = 0. dK f
(5.23)
dJf . dK f
(5.24)
Thus, we have Df − By (5.23) ×M, we have Λf =
Substituting (5.24) into (5.23) gives (5.18). To this point, the gradientbased optimization algorithm for constrained LQ problem is given in Algorithm 1. In Fig. 5.5, the procedures in the proposed algorithm are illustrated. Remarkably, the structural constraint in the gain matrix is preserved using the algorithm. Also, the closedloop stability of the system is ensured as long as the initial gain matrix K 0f stabilizes the closedloop system [29].
The initial gain is set as the gain given in [30], where K 0f = 6.72 0.58 0.00724 0 , and it defines a functional cost of J 0f = 4.8099. After 50 iterations, the gain is updated
50 as K 50 f = 6.64 1.01 0.14 0 , and the cost converges to J f = 2.9138. As compared to the initial cost, the optimized one has a 39.42% reduction. The cost and the norm of projection gradient matrix during the 50 iterations are plotted in Fig. 5.6. Significantly, the derivative action is used in this force control because this action can help the system to achieve fast response. However, the undesirable noise amplification can be brought by the pure derivative action. To reduce such amplification and avoid the phenomenon known as “derivative kick” [31], the derivative action is
120
5 Optimal and Robust Contact Force Control on Soft Membrane
Algorithm 1 Gradientbased Optimization Algorithm for Constrained LQ Problem • Step 1: Set i = 0 and define an initial stable gain such that C(K 0f ) = 0. • Step 2: Set i = i + 1 and determine the projection gradient matrix D if by Lemma 5.1. • Step 3: Optimize the step size α i to minimize the functional cost after the iteration, written as min J (K i−1 − α i D if ). f αi
• Step 4: Update the gain matrix as K if = K i−1 − α i D if . f • Step 5: Go back to Step 2 to continue the iterations until stopping criterion is met.
Fig. 5.5 Illustration of gradientbased optimization algorithm
Fig. 5.6 Change of cost and norm of projection gradient matrix
5
J
f
4
3
2 0
5
10
15
20
25
30
35
40
45
50
35
40
45
50
Iteration Number 60
f
D 
40
20
0 10
15
20
25
30
Iteration Number
combined with a firstorder lowpass filter and thus comes out the PIDF (PID with filter) controller as shown below: C f (s) =
U f (s) 1 s = K p f + Ki f + Kd f , E f (s) s Tf s + 1
where T f is derivation filter parameter and T f = 0.007 is used in this work.
(5.25)
5.3 Controller Design
121
5.3.2 Disturbance ObserverBased Motion Compensator For the lumped disturbance, it can be indicated by −1 Dl (s) = D(s) + [P −1 f (s) − P f n (s)]F (s),
(5.26)
where the disturbancelike head motion D(s) is the dominant part of Dl (s) in this application (i.e., Dl (s) ≈ D(s) while P f (s) ≈ P f n (s)), P f n (s) serves as the nominal model without the disturbance, P −1 f n (s) is the inverse of the nominal model, and −1 −1 [P f (s) − P f n (s)] represents the model uncertainty. For the disturbance observer, refer to Fig. 5.1, it is easy to find that the disturbance can be estimated by (5.27) because it holds F (s) = P f (s)[X (s) − D(s)]. D(s) = X (s) − P −1 f (s)F (s).
(5.27)
Substituting (5.26) into (5.27), we have −1 −1 Dl (s) = X (s) − P −1 f (s)F (s) + [P f (s) − P f n (s)]F (s)
= X (s) − P −1 f n (s)F (s).
(5.28)
As can be seen from (5.28), X (s) and F (s) are measurable and P f n (s) is defined and identified in (5.7) and (5.8), so the lumped disturbance can be obtained by (5.28). However, the relative degree of P −1 f n (s) in this application is less than zero, which (s) is not proper. To make the DOB proper, a filter Q(s) with its relative means P −1 fn degree being equal or higher than that of P f n is applied. Normally, the filter Q(s) is a lowpass filter. Then, the estimation of Dl (s) is given by (5.29), which will be fed back to the outer loop of the force and position controller for the external human motion compensation because the only way to change the relative position between tool set and soft membrane is to control the USM stage. l (s) = Q(s)X (s) − Q(s)P −1 D f n (s)F (s).
(5.29)
For this application, a secondorder lowpass filter shown in (5.30) is used, Q(s) =
1 , s 2 /ωn2 + s/(qωn ) + 1
(5.30)
where ωn = 2π f n , f n is the natural frequency, √ q is the quality factor and defines f c as the cutoff frequency ( f c = f n when q=1/ 2, i.e., secondorder Butterworth lowpass filter). At the frequency less than the cutoff frequency of the lowpass filter, Q(s) ≈ 1 l (s) ≈ D(s); otherwise, Q(s) ≈ 0 and then the filter cuts off the signal and then D (i.e., Dl (s) ≈ 0). It is important that the Qfilter is designed properly so that the
122
5 Optimal and Robust Contact Force Control on Soft Membrane
disturbance estimation is fast enough for the application. To design the Qfilter in this application, it can be guided by the two remarks presented below: Remark 5.2 The cutoff frequency f c of the filter Q(s) must be larger than the bandwidth f Δ of the external disturbance (i.e., f c > f Δ ). Remark 5.3 The cutoff frequency f c of the filter Q(s) should not be greater than the gain crossover frequency f gmc of the position closedloop control system G m (s) (i.e., f c < f gmc ). For Remark 5.2, it is to guarantee that the motion (in the direction of the contact force control) can be estimated correctly within its frequency range. For Remark 5.3, because the proposed DOBbased motion compensation is different from the conventional DOBbased compensation method in which the estimated disturbance is fed back to the inner loop (to the system input) directly, the proposed compensation method requires a condition that the output of G m (s) should be as close as possible to the input of G m (s) (i.e., G m (s) ≈ 1). To meet this condition, the input to G m (s) should be within the bandwidth which is below the gain crossover frequency of G m (s) (because G m ( jω) ≈ 1, ∀ω < 2π f gmc ).
5.3.3 Stability Analysis In this subsection, the stability analysis of the proposed control scheme is briefly discussed. For the position controller, its stability analysis can be found in [19], which indicates that the position error is bounded and able to converge to zero finally. For the force controller, the poles of the closedloop system, i.e., the eigenvalues of the matrix (A + B K f ), are {−606.59, −4.62 + 5.01i, −4.62 − 5.01i, −1.01}, which indicates that the force closedloop system is stable. Furthermore, after adding the filter into the derivative action of the designed optimal force controller, the real parts of all the poles of the closedloop system are still negative, which implies that the force closedloop system under the PIDF controller is also stable. For the DOB shown in (5.29), because the zero of the nominal model is negative, the position and the force are bounded and the lowpass filter is stable, the output of the DOB is bounded and stable. In summary, the overall control system is stable.
5.4 Experiments and Results To verify the performance of the proposed control scheme, an experimental system setup is built.
5.4 Experiments and Results
123
Surgical device
Support arm
Fig. 5.7 Setup of the ear surgical device (© 2018 IEEE. reprinted, with permission, from [26])
5.4.1 Application Overview and Experimental System Setup Figure 5.7 shows the setup of an ear surgical device, which mainly consists of two parts: the main device and the support arm. In the main device, a linear USM stage and a cutter retraction (CR) mechanism are employed to generate the tool set movements along Zaxis, where the tool set includes a cutter for incision making and a holder for (grommet) tube insertion. The maximum traveling range of the tool set driven by the USM stage is set to be 8 mm. A highly sensitive force sensor with a sensitivity of 0.12 mV/g is embedded in the device for the contact force measurement between the tool set and the environment (e.g., soft membrane). For the support arm, it is a 7degreeoffreedom (7DOF) universal arm with one lock for assisting the surgeon in stabilizing the device. Particularly, the surgeon can use the surgical device without this support arm if he/she can hold the device steady enough. During the operation, this device is required to have contact with the tympanic membrane (TM), which is a kind of soft membrane. Due to the usage of the support arm and the connection between the TM and the human head, the relative motion and the contact force are only affected by the human head motion. Furthermore, several tests have been conducted on artificial membranes to investigate the motion effects to the device. The results show that the motion along Zaxis affects the contact between the membrane and the device significantly. Therefore, it is important to control the contact force and compensate the head motion along Zaxis during the operation. The experimental system setup for the control scheme performance test is shown in Fig. 5.8. It consists of the designed surgical device, a sensor amplifier, a motor drive, power supplies, and a computer embedded with dSPACE DS1104 control card (some of them are not shown in the figure). The proposed control scheme is
124
5 Optimal and Robust Contact Force Control on Soft Membrane
Surgical device
Soft membrane USM stage
Fig. 5.8 Experimental system setup (© 2018 IEEE. reprinted, with permission, from [26])
implemented in the dSPACE control card with a sampling time of 1 ms. Moreover, an artificial membrane motion system for providing the simulated head motion is used in the experiments, which consists of an artificial (mock) membrane made of PE and another USM stage.
5.4.2 Force Controller only In this subsection, the performance comparison between the force controller with the initial gain and the optimized gain is investigated. For this comparison purpose, a square wave with frequency of 0.2 Hz and peaktopeak amplitude of 0.03 N is applied to the closedloop system. It is noticed that only the force controller is used and the disturbance is not applied in this experiment. Figure 5.9 shows the experimental results of both controller gains. As can be seen, the controller with both gains is able to follow the square wave correctly. However, the controller with the optimized gain can achieve a faster rise time and smaller error. Moreover, the MaxAEs and RMSEs of them are 0.0318 N, 0.00616 N for the initial gain and 0.0290 N, 0.00589 N for the optimized gain, respectively. The RMSE is reduced by 4.38% while the optimized gain is applied, which implies that the controller with the optimized gain can have a better regulation performance. In summary, the optimized gain is better than the initial gain which can enable the system to be faster and more precise.
5.4 Experiments and Results 0.05 0.04
Force (N)
Fig. 5.9 Comparison of force controller (only) with different gains
125
0.03 0.02 Reference with initial gains
0.01 0 0
5
10
15
Time (s) 0.05
Force (N)
0.04 0.03 0.02 Reference with optimized gains
0.01 0 0
5
10
15
Time (s)
5.4.3 Force Controller with Compensation In this subsection, the disturbance is added into the system and the proposed control scheme is examined. The disturbances used in the following experiments are selected similar or close to this kind of human head motion. According to the study reported in [14], the human head motion is with the bandwidth of mainly around 0.1 Hz and the maximum amplitude of ±1.0 mm while he/she is in a proper lying position. Based on Remarks 5.2 and 5.3, the filter parameters are chosen as f n = 4 Hz and q = 0.707. Furthermore, the desired force Fd has to be set to meet the following requirements: (1) below the minimal breakage force Fb = 0.2641 N, where it can be calculated by the minimal breakage pressure Pbl = 20.5 gf/mm2 reported in [32] and the contact area Ac (i.e., Fb = Pbl Ac ); (2) at the value that can achieve satisfying success rate of the surgery; and (3) as small as possible in order to minimize the harm and discomfort to the patient. Therefore, a desired force Fd = 0.04 N is determined through enormous number of experimental tests, which is the minimal force with the satisfying success rate.
5.4.3.1
Regular Disturbance
At first, a sine wave motion with a frequency of 0.1 Hz and an amplitude of ±1.0 mm is applied as the simulated head motion to the membrane motion system. The performance of different control schemes is shown in Fig. 5.10. As can be observed in the figure, the contact force cannot follow the set point (desired force) while no controller is applied, where the error is huge due to the disturbance. After the force controller is turned on at about 20 s, the contact force starts to follow the set point. Further on, the DOBbased motion compensator is activated at 40 s, the force error is further reduced, and the contact force follows the set point very well. A more
126
5 Optimal and Robust Contact Force Control on Soft Membrane
Fig. 5.10 Comparison of different control schemes
without control (openloop)
0.5
Force feedback control Force control with DOB
Force (N)
0.4
0.3 Reference Force output
0.2
0.1
0 0
10
20
30
40
50
60
Time (s)
0.04
Force Error (N)
Fig. 5.11 Contact force control without and with DOB subject to sine wave disturbance
Force control only Force control with DOB
0.02 0 0.02 0.04 0
5
10
15
10
15
Fig. 5.12 Estimated error of DOB
Estimated Error (mm)
Time (s)
0.05
0
0.05 0
5
Time (s)
detailed comparison between the controller without and with the DOBbased motion compensator is shown in Fig. 5.11. Moreover, Fig. 5.12 shows the comparison between the estimated disturbance and ˆ is within the the actual disturbance. As can be seen, the estimated error (d¯ = d − d) range of ±60 µm, and its RMSE is 28.54 µm, which can be concluded that the DOB can estimate the disturbed motion accurately. Furthermore, more sine wave motions with different frequencies and amplitudes are used. Table 5.2 shows the experimental results of various sine wave motions. As can be seen from the results, the contact force is still able to be maintained at the desired value using the proposed control scheme while the system is subjected to different simulated head motions. With the DOBbased motion compensator, the
5.4 Experiments and Results
127
Table 5.2 Errors of different sine wave motions Sine wave motion Force control only (N) Frequency Amplitude MaxAE RMSE (Hz) (mm) 0.5 1.0 0.5 1.0 0.5 1.0
Fig. 5.13 Comparison between visionbased motion compensation method and proposed method
0.007549 0.01518 0.01345 0.02438 0.02218 0.05105
0.005128 0.008412 0.008268 0.01485 0.01432 0.02681
0.006054 0.008701 0.009930 0.01009 0.01271 0.01307
0.001534 0.001816 0.002251 0.002469 0.003261 0.004092
MaxAE
0.03
Error (N)
0.05 0.05 0.10 0.10 0.20 0.20
Force control with DOB (N) MaxAE RMSE
0.02
0.01
0 0.1 Hz, 0.5 mm
0.1 Hz, 1.0 mm
0.2 Hz, 0.5 mm
RMSE
0.03
Error (N)
Force control + Vision [14] Force control + DOB
0.02
0.01
0 0.1 Hz, 0.5 mm
0.1 Hz, 1.0 mm
0.2 Hz, 0.5mm
Sine Wave Motion
contact force errors are reduced by 19.80% to 74.7% in terms of MaxAE and 70.09% to 84.74% in terms of RMSE. It can be concluded that the DOBbased motion compensation makes a great improvement on disturbance rejection as well as force control. Besides that, the contact force control performance using the visionbased motion compensation method presented in [14] is shown in Fig. 5.13 and Table 5.3. In comparison to those results, the proposed control scheme can offer a slightly better performance at 0.1 Hz but a significantly better performance at 0.2 Hz. This is because the visionbased motion compensation method is limited by the camera frame rate. Therefore, the proposed control scheme offers a guaranteed contact force control performance without any change on the system hardware (no extra equipments are needed).
128
5 Optimal and Robust Contact Force Control on Soft Membrane
Table 5.3 Comparison to visionbased motion compensation method Sine wave motion Error (N) Force control + vision [14] Amplitude
0.1 Hz
0.5 mm
0.1 Hz
1.0 mm
0.2 Hz
0.5 mm
Fig. 5.14 Proposed control scheme subject to random motion
MaxAE RMSE MaxAE RMSE MaxAE RMSE
0.01133 0.002443 0.01470 0.00300 0.02810 0.007244
−12.36% −7.86% −31.36% −17.70% −54.77% −54.98%
1
Displacement (mm)
Frequency
Improvement using force control + DOB
Simulated head motion DOB output
0.5 0 0.5 1 0
10
20
30
40
50
60
40
50
60
Time (s)
Force Error (N)
0.04 0.02 0 0.02 0.04 0
10
20
30
Time (s)
5.4.3.2
Random Disturbance
To have a better simulation on the head motion, a random motion with a maximum frequency of about 0.3 Hz, a dominant frequency around 0.1 Hz, and an amplitude within ±1.0 mm is applied as the simulated head motion (see Fig. 5.14a). The contact force control performance using the proposed control scheme is shown in Fig. 5.14b. As can be observed in Fig. 5.14a, the DOB can estimate the random motion correctly and precisely, which is very helpful in the motion compensation. In Fig. 5.14b, the result shows that the proposed control scheme can still offer a good performance in tracking the set point while the random motion is applied. The MaxAE is 0.008928 N and the RMSE is 0.003190 N which is within 10% of the desired set point. Therefore, the proposed control scheme is good and efficient at controlling the contact force on such a soft membrane.
5.5 Chapter Summary
129
5.5 Chapter Summary In this chapter, a control system using force feedback controller with DOBbased motion compensator for contact force control and motion compensation is proposed, developed, and verified. An improved model of the soft membrane (describing the displacement–force relationship) is built on basis of the viscoelastic behavior and the contact effect. Based on the improved model, an optimal PID controller for the contact force control is designed using a novel constrained LQ optimization algorithm. Furthermore, to compensate the head motion and the model uncertainty, a DOB is designed. With the proposed control scheme, the external human motion compensation can be achieved without adding any extra equipments. It can keep the overall system of the surgical device to be the simplest setup, which is good to the medical application. Finally, to verify the performance of the proposed control scheme, an artificial membrane motion system to simulate the soft membrane and the human motion is built and used in the experiments. From the experimental results on the force controller, it can be found that the optimized PID controller gain can perform an optimal tracking performance on the set point while there is no disturbance. Moreover, several regular sine wave motions and a random motion are applied as the simulated human motion to the system in the tests. According to the experimental results, it can be concluded that the DOB can estimate the simulated motion correctly and precisely, and thus the simulated motion can be greatly compensated by the DOBbased motion compensator. By combining the optimal force controller and the DOBbased motion compensator together, the contact force can be controlled accurately at a desired value, and the relative position between the tool set and the membrane can be maintained steadily. Hence, the performance of the surgical device can be further guaranteed by this optimal and robust control scheme.
References 1. K.K. Tan, W. Liang, L.P. Pham, S. Huang, C.W. Gan, H.Y. Lim, Design of a surgical device for officebased myringotomy and grommet insertion for patients with otitis media with effusion. J. Med. Devices 8(3), 031001–1–12 (2014) 2. K.K. Tan, S. Huang, M.H.T. Nguyen, W. Liang, S.C. Ng, An innovative design for invitro fertilization oocyte retrieval systems. IEEE Trans. Ind. Inform. 9(4), 1892–1899 (2013) 3. T. Ueta, Y. Yamaguchi, Y. Shirakawa, T. Nakano, R. Ideta, Y. Noda, A. Morita, R. Mochizuki, N. Sugita, M. Mitsuishi, Y. Tamaki, Robotassisted vitreoretinal surgery: development of a prototype and feasibility studies in an animal model. Ophthalmology 116(8), 1538–1543 (2009) 4. S.G. Yuen, D.P. Perrin, N.V. Vasilyev, J. Pedro, R.D. Howe, Force tracking with feedforward motion estimation for beating heart surgery. IEEE Trans. Robot. 26(5), 888–896 (2010) 5. H. Choi, H.S. Kwak, Y.A. Lim, H.J. Kim, Surgical robot for singleincision laparoscopic surgery. IEEE Trans. Biomed. Eng. 61(9), 2458–2466 (2014) 6. D. Pisla, B. Gherman, C. Vaida, M. Suciu, N. Plitea, An active hybrid parallel robot for minimally invasive surgery. Robot. Comput.Integr. Manuf. 29(4), 203–221 (2013)
130
5 Optimal and Robust Contact Force Control on Soft Membrane
7. F. Bechet, K. Ogawa, E. Sariyildiz, K. Ohnishi, Electrohydraulic transmission system for minimally invasive robotics. IEEE Trans. Ind. Electron. 62(12), 7643–7654 (2015) 8. U. Kim, Y.B. Kim, D.Y. Seok, J. So, H.R. Choi, A surgical palpation probe with 6axis force/torque sensing capability for minimally invasive surgery. IEEE Trans. Ind. Electron. 65(3), 2755–2765 (2018) 9. T. Osa, N. Sugita, M. Mitsuishi, Online trajectory planning and force control for automation of surgical tasks. IEEE Trans. Autom. Sci. Eng. 15(2), 675–691 (2017) 10. Z. Mo, W. Xu, N.G.R. Broderick, Capability characterization via exvivo experiments of a fiber optical tip force sensing needle for tissue identification. IEEE Sens. J. 18(3), 1195–1202 (2018) 11. W.T. Latt, R.C. Newton, M. VisentiniScarzanella, C.J. Payne, D.P. Noonan, J. Shang, G.Z. Yang, A handheld instrument to maintain steady tissue contact during probebased confocal laser endomicroscopy. IEEE Trans. Biomed. Eng. 58(9), 2694–2703 (2011) 12. S. Kyeong, D. Chang, Y. Kim, G. M. Gu, S. Lee, S. Jeong, J. Kim, A handheld micro surgical device for contact force regulation against involuntary movements, in the 37th Annual International Conference of the IEEE Engineering in Medicine and Biology Society (2015), pp. 869–872 13. J.M. Florez, J. Szewczyk, G. Morel, An impedance control strategy for a handheld instrument to compensate for physiological motion, in the 2012 IEEE International Conference on Robotics and Automation (2012), pp. 1952–1957 14. W. Liang, W. Gao, K.K. Tan, Stabilization system on an officebased ear surgical device by force and vision feedback. Mechatronics 42, 1–10 (2017) 15. P. Moreira, N. Zemiti, C. Liu, P. Poignet, Viscoelastic model based force control for soft tissue interaction and its application in physiological motion compensation. Comput. Methods Progr. Biomed. 116(2), 52–67 (2014) 16. M.H. Raibert, J.J. Craig, Hybrid position/force control of manipulators. J. Dyn. Syst. Meas. Control 103(2), 126–133 (1981) 17. J. Hewit, J. Burdess, Fast dynamic decoupled control for robotics, using active force control. Mech. Mach. Theory 16(5), 535–542 (1981) 18. K. Ohishi, K. Ohnishi, K. Miyachi, Adaptive dc servo drive control taking force disturbance suppression into account. IEEE Trans. Ind. Appl. 24(1), 171–176 (1988) 19. K.K. Tan, W. Liang, S. Huang, L.P. Pham, S. Chen, C.W. Gan, H.Y. Lim, Precision control of piezoelectric ultrasonic motor for myringotomy with tube insertion. J. Dyn. Syst. Meas. Control 137(6), 064504 (2015) 20. W.H. Chen, J. Yang, L. Guo, S. Li, Disturbanceobserverbased control and related methods: an overview. IEEE Trans. Ind. Electron. 63(2), 1083–1095 (2016) 21. Y.C. Fung, Biomechanics: Mechanical Properties of Living Tissues (Springer, New York, 1981) 22. N. Famaey, J.V. Sloten, Soft tissue modelling for applications in virtual surgery and surgical robotics. Comput. Methods Biomech. Biomed, Eng. 11(4), 351–366 (2008) 23. A. Asadian, M.R. Kermani, R.V. Patel, A compact dynamic force model for needletissue interaction, in The 32nd Annual International Conference of the IEEE Engineering in Medicine and Biology Society (2010), pp. 2292–2295 24. Á. Takács, P. Galambos, P. Pausits, I.J. Rudas, T. Haidegger, Nonlinear soft tissue models and force control for medical cyberphysical systems, in the 2015 IEEE International Conference on Systems, Man, and Cybernetics (2015), pp. 1520–1525 25. A. Biswas, M. Manivannan, M.A. Srinivasan, Multiscale layered biomechanical model of the pacinian corpuscle. IEEE Trans. Haptics 8(1), 31–42 (2015) 26. W. Liang, J. Ma, K.K. Tan, Contact force control on soft membrane for an ear surgical device. IEEE Trans. Ind. Electron. 65(12), 9593–9603 (2018) 27. J. Ma, S.L. Chen, N. Kamaldin, C.S. Teo, A. Tay, A.A. Mamun, K.K. Tan, Integrated mechatronic design in the flexurelinked dualdrive gantry by constrained linearquadratic optimization. IEEE Trans. Ind. Electron. 65(3), 2408–2418 (2018)
References
131
28. J. Ma, S.L. Chen, N. Kamaldin, C.S. Teo, A. Tay, A.A. Mamun, K.K. Tan, A novel constrained H2 optimization algorithm for mechatronics design in flexurelinked biaxial gantry. ISA Trans. 71, 467–479 (2017) 29. J. Ma, S.L. Chen, C.S. Teo, C.J. Kong, A. Tay, W. Lin, A.A. Mamun, A constrained linear quadratic optimization algorithm toward jerkdecoupling cartridge design. J. Frankl. Inst. 354(1), 479–500 (2017) 30. W. Liang, K.K. Tan, Force feedback control assisted tympanostomy tube insertion. IEEE Trans. Control Syst. Technol. 25(3), 1007–1018 (2017) 31. K.H. Ang, G. Chong, Y. Li, Pid control system analysis, design, and technology. IEEE Trans. Control Syst. Technol. 13(4), 559–576 (2005) 32. N. Yamamoto, T. Ishii, T. Machida, Measurement of the mechanical properties of the tympanic membrane with a microtension tester. Acta otolaryngologica 110(1–2), 85–91 (1990)
Chapter 6
Force–Position Control for Fast Tube Insertion
Insertion is one of the commonly used surgical procedures in the surgery. For example, in otolaryngology, the tympanostomy tube insertion is the frequently performed otologic surgical procedure to treat the otitis media with effusion (OME), a common ear disease affecting children and adults all over the world. An officebased surgical device for this surgical treatment of OME is developed to overcome the disadvantages of the current way of inserting the tube. Such tube insertion is one key to success in this surgery, it is necessary to ensure that the procedure is carried out in a quick and safe manner with a high success rate via the developed surgical device. Moreover, the insertion time should be as short as possible so that the discomfort on the patient can be minimized. To this end, a control scheme with a new insertion method utilizing the concept of force feedback is proposed, implemented, and tested in a mockup system in this chapter.
6.1 Introduction Tympanostomy tube insertion is a frequently performed otologic surgical procedure to treat OME after medication fails. OME is a common ear disease affecting children and adults all over the world, which occurs when fluid accumulates in the middle ear (behind the tympanic membrane, TM) [1]. Conventionally, the tympanostomy tube insertion is carried out in an operating room because most of these surgeries require the general anesthesia (GA). After the patient is put under GA, an incision is made on the TM (also known as “eardrum”) with a surgical knife (this procedure is also called “myringotomy”). Then, a tympanostomy tube (also known as “grommet”) is carefully inserted through the incision by forceps and the Rosen needle. Thus, the pressure between the middle ear and the atmosphere can be balanced and the fluid within the middle © Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931_6
133
134
6 Force–Position Control for Fast Tube Insertion
ear can be drained out from the tube. Actually, this conventional surgery for OME is a small but costly (approximately USD 2,000 per surgery [2]) surgery which can be done in 15 mins by an experienced surgeon. Over the last decade, a number of officebased approaches presented in [3–7] have been developed to assist the surgeon to carry out the surgery to overcome the major disadvantages of the conventional surgical treatment for OME. However, these devices are mostly operated by hand with special skills required of the surgeon as it is not automated. Moreover, no sensing and control systems are developed and involved in all these devices, which lead to low precision and intelligence. Therefore, the adaptation to different eardrums and the success rate of using this device may be limited. To this end, a novel precision officebased surgical device involving sensing and control systems has been developed by our team. The proposed device allows the officebased, automatic, precise, and quick surgical treatment (myringotomy with tube insertion) for OME to be accomplished in a patient without the use of GA. Moreover, to maximize the success of insertion, a new method on inserting the tympanostomy tube is required. In recent years, several research works on utilizing the force control system for surgical devices have been developed in [8–11]. In [8], the benefits of force feedback were examined. A force control system can benefit the surgery and improve the performance of the surgical device. The force feedback controlassisted tympanostomy tube insertion will be a good way to improve the insertion success rate. Furthermore, some current works on using the force/tactile sensing or force feedback in the field of the otology application have been reported in [12–14]. In [12, 13], a smart sensory guided surgical drill for cochleostomy which is able to discriminate the state of the drilling process was developed. In [14], researchers developed a tool pose estimation algorithm for an imageguided microsurgical robot during a drilling task (for direct cochlear access) based on the correlation of the observed axial drilling force and the heterogeneous bone density. However, all these systems are not designed for tube insertion or the similar procedure with the requirements of time, success rate, and safety. Hence, a new insertion method based on the concept of force feedback control is proposed and designed in this chapter to assist the device in inserting tympanostomy tube and achieve better performance. In this chapter, a force feedback control system for tympanostomy tube insertion using the developed device is designed. With the force feedback control system, a new method for reliable and fast tube insertion via a novel force–position (force/position) control approach is developed.
6.2 Tube Insertion Procedure To understand the tube insertion procedure conducted by the surgical device (an officebased auto device to carry out tympanostomy tube insertion), the system architecture of the surgical device is presented at first and followed by its working process.
6.2 Tube Insertion Procedure
135
Force Sensing System Filter
Force Sensor Amplifier
Motion Controller
Motor Drive Motion Control System
Fig. 6.1 System architecture
6.2.1 System Architecture The system architecture of the surgical device is shown in Fig. 6.1. It is controlled by a computer with an embedded control card and mainly consists of the following subsystems: (1) mechanical system (i.e., main body) designed to carry out the whole surgical operations on TM inside the ear; (2) motion control system designed to yield the necessary precise and customized motion profiles; (3) force sensing system designed carefully to precisely provide the force information between the tool set and the membrane to the device during the surgery. In the force sensing system, a highly sensitive lowcost force sensor with a sensitivity of 0.12 mV/g is chosen to be integrated into the device. It provides precise reliable force sensing performance in a compact commercial grade package [15] and measures the force applied on the tool set directly. The sensor uses a specialized piezoresistive micromachined silicon sensing element. It converts the concentrated force from the applications to the changes of the piezoresistor’s resistance [15]. This change then results in a corresponding voltage output change through the Wheatstone bridge circuit. Such voltage output change will be amplified by an instrumentation amplifier and then the amplifier will send the amplified output to the control card. Furthermore, the main body of the surgical device, as the core part, is designed and shown in Fig. 6.2. It mainly consists of the following components: (1a) a linear ultrasonic motor (USM) stage for driving the tool set (and the tube) along Zaxis to complete all the required surgical procedures; (1b) a hollow cutter for making the incision on the TM and holding the tube through the tube’s hole; (1c) a hollow holder allows the cutter to extend out of or retract into it (the holder) for pushing the tube forward to insert the tube onto the TM; (1d) a cover shields the tool set (the cutter and the holder) to prevent the tube falling from the cutter while the tool set is moving into the ear canal and minimize
136
6 Force–Position Control for Fast Tube Insertion
Cutter
Force sensor
Cover
Grommet
Z
Fig. 6.2 Mechanical design of the surgical device
the direct contact between the holder and the ear canal so as to avoid the false alarms from the force sensing; (1e) a cutter retraction (CR) mechanism for moving the cutter forward and backward inside the holder; (1f) a force sensor for providing the contact force information between the tool set and TM; and (1g) a handle for supporting the device as well as being manipulated by the surgeon. In particular, the cover and the tool set with the tube are integrated compactly into a telescopic structure design, where the cover is the outer core and followed by the holder and the cutter in the inner core. This design minimizes the size of the required tools and allows all of them to enter the limited space of the ear canal of a diameter about 5 to 10 mm. In addition, the human hand tremor could affect the success rate for any handheld device. Therefore, a mechanical approach to assist the surgeon in stabilizing the device is proposed (refer to Fig. 4.1).
6.2 Tube Insertion Procedure
137
Holder Cover Cutter
(a)
(b)
(c)
(d)
(e)
Fig. 6.3 Working process of the proposed device: a initialization; b touch detection; c myringotomy; d tube insertion; e tube release (© 2016 IEEE. reprinted, with permission, from [16])
6.2.2 Working Process The working process of the surgical device is shown in Fig. 6.3, which has five steps: (1) Initialization: The desired insertion location (spot) is determined by the surgeon and the cutter tip is pointed at the spot with the help of a microscope. Then the tool set is moved slowly close to the TM (but not touched) by the surgeon. (2) Touch detection: After the initialization, the surgeon is allowed to press the startup button. Upon activation of the button, the cutter is slightly retracted and withdrawn into the tube, and the tool set (with the tube) is driven by the USM stage to be moved toward the membrane until the tube touches the membrane at a certain contact force. The touch position is set as the start point for the rest of the procedures. (3) Myringotomy: The procedures of myringotomy are initiated to make an incision on the membrane. (4) Tube insertion: Immediately following the myringotomy, the cutter is retracted slightly by the CR mechanism to avoid hurting the inner bones inside middle/inner ear during the tube insertion, and the holder is moved forward to insert the tube through the incision on the membrane directly.
138
6 Force–Position Control for Fast Tube Insertion
(5) Tube release: The cutter is fully retracted by the CR mechanism and the entire tool set is withdrawn to release the tube on the membrane. In particular, step (2) to step (5) are to be efficiently, automatically, and sequentially accomplished with the guidance of the sensing system. Furthermore, step (4) is the key to success in tympanostomy tube insertion. The main requirements of this step can be summarized as follows: • minimize the pain to the patient (one solution is to minimize the insertion time) and • maximize the success rate of insertion. Therefore, the insertion time and the success rate are two critical factors to step (4), i.e., the insertion time needs to be as short as possible while the success rate can be as high as possible. For the purpose of designing a new insertion method, the force information during insertion is needed to be investigated.
6.2.2.1
Tympanostomy Tube Insertion Force
To investigate the tympanostomy tube insertion force, a mock membrane is used. Generally, human TM is with a mean diameter of about 8 to 10 mm and a nonuniform spatial thickness distribution in the range of 30 to 120 µm [17, 18]. Young’s modulus of TM is about 20 to 300 MPa [17, 19–21]. In 2005, Fay et al. estimated the elastic modulus of human tympanic membrane based on several approaches and their data suggested an elastic modulus in the range of 100 to 300 MPa [19]. According to the physical properties of the human TM, the mock membrane made of polyethylene (PE) film which has similar characteristics (Young’s modulus is about 170 to 280 MPa) and thickness (about 50 to 100 µm) to the human TM is used for investigation and experiment. During the experiment, an incision is made on the mock membrane at first. Then, a tympanostomy tube is pushed by the device toward the incision at a constant speed until the tube is inserted on the membrane successfully. The insertion force is measured by the force sensing system and recorded by a computer. The force sensor output and its filtered output are shown in Fig. 6.4. Significantly, the designed filter is a median filter which is used for eliminating the noises and retaining the useful force information. As can be seen in Fig. 6.4, it is easy to find out that the force is increasing as the tube is moved toward the membrane before it is inserted. Then the force drops a lot to zero once the tube is inserted on the membrane. This is because the resistance from the membrane is reduced after the front flange of the tube is pushed through the incision, i.e., the force applying on the tool set after the insertion is very small or zero. The differential of the filtered output is shown in Fig. 6.5. As observed, it is very clear that the differential output decreases suddenly to a large negative value
6.2 Tube Insertion Procedure
139
Fig. 6.4 Force sensor output and its filtered output during tube insertion
0.8 Sensor output Median filtered output
0.7 0.6
Force (N)
0.5 Tube is inserted
0.4 0.3 0.2 0.1 0 1.5
2
2.5
3
3.5
4
4.5
5
4
4.5
5
Time (s)
Fig. 6.5 Differential of filter output
20 10
Differential Output (N/s)
0 10 20 30 40 50 60 70
Tube is inserted
80 1.5
2
2.5
3
3.5
Time (s)
at the moment while the tube is inserted. According to this change, the force and its differential output are used to detect whether the tube is inserted.
6.3 Controller Design Generally, there are two conventional methods, position control and force control, which are commonly used in controlling or guiding a surgical device to carry out the insertion. In the following, these two conventional insertion methods designed for the tube insertion are introduced at first. Following that, a new insertion method which
140
6 Force–Position Control for Fast Tube Insertion
Position Controller Optimal PID Controller USM Nonlinear Compensator Linear Encoder Fig. 6.6 Motion controller for USM stage
combines the advantages of both conventional methods is proposed and presented in detail.
6.3.1 PositionBased Insertion Method To meet the time requirement, the simplest way to minimize the time is to design a motion trajectory for insertion to be a direct forward penetration with a certain insertion depth (the required travel distance of the tool set for tube insertion) and at a high speed. To achieve high precision and high speed, a precise and highperformance feedbackbased motion controller is designed for the USM stage and its block diagram is shown in Fig. 6.6. For the USM stage, it consists of nonlinear dynamics and its model can be considered as a combination of a linear term and a nonlinear term. The linear term can be represented by a secondorder system. On the other hand, the nonlinear term can be written as a combination of the nonsymmetric Coulomb friction (structured component) and the unknown but bounded uncertain component Δf . Therein, the nonsymmetric friction can be described by two sign functions. Hence, the model of USM stage can be given by m x¨ = −kx − c x˙ + bu − σ sign(x) ˙ + ηsign(x) ˙ + Δf,
(6.1)
where x is the position output of the USM stage, k, c, b are the dominant parameters of the USM stage, u m (t) is the input signal to the motor drive, σ and η are coefficients of the structured nonlinear component, and sign(·) is a sign function. In the proposed motion control system, a proportional–integral–derivative (PID) controller is applied as the main controller of the system (because PID controller has a simple structure which is easy to design and implement in a wide range of applications [22, 23]) while a nonlinear compensation including the sign functions
6.3 Controller Design
141
and a sliding mode controller is designed to compensate the nonlinear dynamic and eliminate the uncertainty. Furthermore, the PID controller parameters are derived optimally using a linearquadratic regulatorassisted (LQRassisted) tuning approach with the following index:
∞
J=
[E(t)T QE(t) + r u(t)T u(t)]dt,
(6.2)
0
where Q is the weighting matrix that is normally chosen as a diagonal matrix, i.e., Q = diag{q1 , q2 , q3 } and Q > 0, r is the weighting factor. To apply the LQR control strategy, the USM stage model is converted tothe error model in a statespace form t ]T , where e(t) = in which states are the error states: E(t) = [ 0 e(τ )dτ, e(t), de(t) dt xd (t) − x(t) is the position error and xd (t) is the desired position. Thus, the state matrix A and input matrix B of the error model are given by ⎡
⎤ ⎡ ⎤ 0 1 0 0 A = ⎣ 0 0 1 ⎦ and B = ⎣ 0 ⎦ . b 0 − mk − mc
(6.3)
Hence, we can have the LQRassisted PID controller shown as
t
K pm e(t) + K im
e(τ )dτ + K dm
0
de(t) , dt
(6.4)
where K pm , K im , K dm are controller parameters which can be calculated by [K i m, K p m, K d m]T = r −1 B T P while P is the solution of the algebraic Riccati equation (ARE): (6.5) A T P + P A − P Br −1 B T P + Q = 0. The overall controller is then given by (6.6)
u m (t) = K pm e(t) + K im
t 0
e(τ )dτ + K dm
de(t) + [σ sign(x) ˙ − δsign(x)] ˙ + kˆs sign(E(t)T P B), dt
(6.6)
where k˙ˆs = Proj(ρ1 ET P B, kˆs ), ρ1 is the adaptive gain and Proj(·) is the smooth projection algorithm. The controller parameters and the system parameters are derived as K pm = 40.5069, K im = 160, K dm = 0.0994, σ = 2.42, δ = 0.115, the adaptive gain is chosen as ρ1 = 0.0001, and the P and B are shown as follows: ⎡
⎤ ⎡ ⎤ 405.5714 1.4025 0.002 0 P = ⎣ 1.4025 0.3533 0.005 ⎦ and B = ⎣ 0 ⎦ . 0.002 0.005 0 4940
142
6 Force–Position Control for Fast Tube Insertion
Motion Trajectory
Position Controller
Surgical Device
Membrane
Linear Encoder Fig. 6.7 Positionbased insertion method (© 2016 IEEE. reprinted, with permission, from [16])
More details behind the design of the control algorithms for the motion controller and their implementation can be found in [24]. Based on the motion controller, a motion trajectory input for insertion is designed and thus the positionbased insertion method is formed as shown in Fig. 6.7. The designed motion trajectory xd is designed though amount of experimental tests with the success rate as the criterion. It is able to achieve the insertion time less than 0.15 s which is validated in the experiments. However, the fixed insertion depth limits the adaption of the device to the uncertainties of the membranes and the difference among them. In other words, the tube may be underinserted if the membrane is too thick/strong while the tube may be overinserted if the membrane is too thin/weak. As a result, the success rate by using this positionbased insertion method would be affected due to its poor robustness. Thus, it is necessary to explore an alternative insertion method which is more robust. As presented in Chap. 3, a forcebased supervisory controller for tube insertion is proposed which identifies four different types of membranes by neural network technique and selects welldesigned motion trajectory corresponding to the identified type of membrane. This forcebased supervisory controller can improve the robustness of the surgical device and guarantee the high success rate of tube insertion. However, the need for the preclassification on different types of membranes may be a challenge in the practical application.
6.3.2 ForceBased Insertion Method Alternatively, an insertion method utilizing the force feedback is designed to solve the robustness issue of using the positionbased insertion method. According to the insertion force information analyzed in Sect. 6.2.2, the tube insertion can be accomplished by controlling the insertion force to follow a desired force profile (such as the ramp trajectory) until the tube is inserted (which can be detected through the force measurement). Different from the positionbased insertion
6.3 Controller Design
143
Position Controller
Surgical Device
Membrane
Linear Encoder
Force Sensor
Force Controller
Fig. 6.8 Forcebased insertion method (© 2016 IEEE. reprinted, with permission, from [16])
method (fixed insertion depth), the insertion depth can be varied with regard to different membranes (with different thickness or strength) so that the success rate of tube insertion can be improved. Hence, a force feedback control system is designed to achieve this insertion method. The forcebased insertion method and its control scheme are illustrated in Fig. 6.8. It is a positionbased force controller, where the position controller is in the inner loop for the precision motion control of the USM stage and the force feedback controller is in the outer loop for the force control. For the force feedback controller, another PID controller is designed which is shown in (6.7). u f (t) = K p f e f (t) + K i f 0
t
e f (τ )dτ + K d f
de f (t) , dt
(6.7)
where u f (t) is the input to the motion controller (i.e., xd (t) = u f (t)); e f (t) = Fd (t) − F(t) is the force error; Fd (t) is the desired force; and K p f , K i f , and K d f are controller parameters. The transfer function of the above PID controller is given by U f (s) = K p f E f (s) + K i f
1 E f (s) + K d f s E f (s). s
(6.8)
In this application, because the pure derivative action in the controller may bring about the undesirable noise amplification to the system, the pure derivative action is replaced by a firstorder lowpass filter so as to reduce the noise and avoid the phenomenon known as “derivative kick” [25]. Thus, the PID controller can be modified to be a PIDF (proportional–integral–derivativefilter) controller [26], which is written as U f (s) = K p f E f (s) + K i f
1 s E f (s) + K d f E f (s), s Tf s + 1
(6.9)
144
6 Force–Position Control for Fast Tube Insertion
where T f is the time constant of the filter. With the help of Control System Toolbox (pidTuner) in MATLAB, the controller parameters are designed as K p f = 0.29, K i f = 3.36, K d f = 0.00362, T f = 0.0171. For the forcebased insertion method, it can help to increase the adaptation needed when inserting the tubes on different membranes. In other words, the forcebased insertion method can achieve a high and equal success rate with respect to different membranes with different parameters. However, because the force feedback controller is in the outer loop, the response of the force feedback controller is slower than the position/motion controller. Moreover, some researchers also concluded that the response of the force control system may not be fast enough for robotics and surgical applications [10, 27]. Thus, the forcebased insertion method will lengthen the insertion time comparing to the positionbased insertion method. The longer insertion time will bring additional pain and discomfort to the patient.
6.3.3 Force–Position Insertion Method Both insertion methods mentioned above have pros and cons. The positionbased insertion method is faster because only the position controller is needed, but its adaptive capacity is poorer than the forcebased insertion method due to its fixed insertion depth. On the other hand, the forcebased insertion method has a better robustness because it can have the sense on whether the tube is inserted, but its speed is limited and the insertion time is longer. There should be a tradeoff between the insertion time and the success rate. To achieve the tube insertion in a short time as well as to ensure a high success rate, a new insertion method called “force–position insertion method” is proposed in this chapter. In this insertion method, the concept of a novel force–position control, namely, selective force–position control is employed, where the positionbased insertion method and the forcebased insertion method are combined to a new form as shown in Fig. 6.9. The cascade control structure is kept but a designed motion trajectory input (for insertion) and an adjustable weighting (after the output of the force controller) are added. In particular, the motion trajectory input acts like a feedforward mechanism which is designed to speed up the process. The adjustable weighting is used to adjust the participating proportion of the force feedback control and thus it becomes a kind of force feedback ratio controller, i.e., the effect of the force feedback control can be changed by the weighting. Thus, the input to the motion controller can be rewritten as (6.10) u i (t) = r (t) + wm (t)u f (t),
(6.10)
where r (t) is the designed motion trajectory input and wm (t) is the adjustable weighting. The motion trajectory used in the force–position insertion method is the same rapid motion trajectory (within 0.15 s) for insertion in the positionbased insertion
6.3 Controller Design
145
Motion Trajectory
Position Controller
Weighting
Surgical Device
Membrane
Linear Encoder
Force Sensor
Force Controller
Fig. 6.10 Motion sequence for insertion
Position Reference (mm)
Fig. 6.9 Force–position insertion method (© 2016 IEEE. reprinted, with permission, from [16]) 2 1.5 1 0.5 0 0.5 0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
Time (s)
method which is shown in Fig. 6.10, so that the speed of the insertion procedure can be maximized. In particular, the first 0.05 s in Fig. 6.10 is reserved for the cutter retraction after myringotomy. Furthermore, the weighting wm is changed with time which is shown in Fig. 6.11. As can be seen, during the period of applying the designed motion trajectory, the weighting is set to be 0.5 so as to reduce the effect of the force feedback to the motion. After that, the weighting is increased from 0.5 to 1 within 0.15 s. Therefore, the effect of the force feedback is increased and thus the insertion procedure can be faster and more precise. The weighting is given by the following equation: ⎧ ⎪ ⎨0.5, if t ≤ 0.15 wm (t) = 10t/3, if 0.15 < t ≤ 0.3 ⎪ ⎩ 1, otherwise
.
(6.11)
146
6 Force–Position Control for Fast Tube Insertion
Fig. 6.11 Designed weighting Weighting
1.2 1 0.8 0.6 0.4 0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
Time (s)
6.3.3.1
Desired Force Profile Selection
Besides that, the desired force profile to the force feedback controller is a ramp trajectory. In this application, it requires the insertion to be completed precisely in a short time and with a small force. In other words, if this is done more aggressively with a shorter time and steeper slope, the membrane can be punctured. If a more gentle slope is used, it may lead to more deformation and a longer time. Meanwhile, the force tracking performance is also important because it may affect the insertion success rate. Thus, an optimal slope for the desired force profile is needed. To find out an appropriate slope of the desired force profile, experiments using different slopes were carried out. Five slopes from gradual to steep were used in the experiments, which are 0.25, 0.5, 0.75, 1, and 1.25 N/s. In order to compare the performance when using different slopes, the insertion time, the insertion peak force, and the rootmeansquare error (RMSE) of force controller are selected and used for the comparison. In particular, the time and the peak force directly relate to the pain level. A shorter time and a smaller force will result in less discomfort and pain to the patient. The RMSE represents the performance of the force controller. The smaller error will lead to more accurate force tracking. The average insertion time, the peak force, and the RMSE incurred with different slopes on the forcebased insertion method are shown in Fig. 6.12. As can be seen, it is obvious that the insertion peak force does not change too much but the force with a gentler slope is slightly smaller. Moreover, as the slope of the desired force increases, the insertion time reduces while the error increases. This is reasonable because a steeper slope results in a faster insertion speed. To determine the optimal slope, the following data processing is carried out. Firstly, curve fitting for each feature via the collected data is applied, respectively. The fitted curves of the different features are described in (6.12) to (6.14). f t (m) = 16.3exp(−13.75m) + 0.8167exp(−0.7035m), f f (m) = 0.0217m −0.7725 + 0.5059,
(6.12) (6.13)
f e (m) = 0.1389m + 0.05874,
(6.14)
where exp(·) is the natural exponential function, m is the slope, f t (m) describes insertion time function of the slope which is an exponential function, f f (m) describes
6.3 Controller Design
147
Fig. 6.12 Features by using different slopes
1.4 Insertion time (s) Insertion force (N) RMSE (N)
1.2
Parameters
1
0.8
0.6
0.4
0.2
0 0.25
0.50
0.75
1.00
1.25
Slope (N/s)
the insertion peak force function of the slope which is a power function, and f e (m) describes the RMSE function of the slope which is a linear function. Next, combining all the three fitted functions that represent the relations between the slope and each feature with different weightings, a cost function is obtained as shown below: (6.15) L(m) = w1t f t (m) + w2 f f f (m) + w3e f e (m), where w1t , w2 f , and w3e are the weightings for the features, each of which consists of two parts as shown in the below equations: w1t = w1 wt
(6.16)
w2 f = w2 w f w3e = w3 we
(6.17) (6.18)
wi f i∗ (m)
= 1/ f i∗ (m) = min( f i (m))
(6.19) (6.20)
(i = 1, 2, 3), where wi is used to make all the features into a same importance and wt , w f , and we represent the importance of the features, respectively. In this application, the insertion time is the most important and the force experienced is the second while the error is the least important. Finally, it can be calculated by numerical computation that the slope (from 0.1 to 1.5 N/s) that minimizes the cost function is the optimal one and the optimal slope for this application is 0.7827 N/s. For the purpose of an efficient implementation, the slope of the desired force profile is chosen as 0.8 N/s.
148
6 Force–Position Control for Fast Tube Insertion
6.3.4 Discussions In summary, two conventional insertion methods (positionbased and forcebased) are introduced and discussed. Then, a force–position insertion method which aims to combine the advantages of both conventional insertion methods (short insertion time, good robustness, and high success rate) is proposed and designed. Furthermore, an optimal desired force profile is determined for the tube insertion force tracking control to make sure that the insertion time and the insertion force can be minimized while the force feedback controller can offer a good tracking performance.
6.4 Experiments and Results To evaluate the proposed control scheme and the force–position insertion method, plenty of experiments are carried out in different setups.
6.4.1 Experiments and Results on Rigid Setup The experimental system setup is shown in Fig. 6.13. It consists of the surgical device mounted on an adjustable rigid support, a sensor amplifier, a motor drive, power supplies, and a computer embedded with dSPACE DS1104 control card (some of them are not shown in the figure). The motion controller and the force controller as well as the overall working process are implemented in the dSPACE control card with a sampling time of 1 ms. Furthermore, a mockup system (see Fig. 6.13) attached with the mock membrane is used in the experiments. Two different kinds of mock membranes are used in the following experiments so as to test the robustness of the proposed control scheme. Both mock membranes are made of PE films, but one is the normal mock membrane with characteristics closer to the TM and the other is a stronger membrane with higher Young’s modulus and greater thickness.
6.4.1.1
Experimental Results on ForceBased Insertion Method
At first, the forcebased insertion method is examined. One hundred tube insertion tests are carried out by using the forcebased insertion method. Fifty membranes of each kind are used in the tests. For the forcebased insertion method, all the tubes are fully inserted on the membranes automatically and successfully. Therefore, the success rate from the use of forcebased insertion method is 100% no matter what kind of membrane is concerned. The desired insertion force and the filtered force sensor output are shown in
6.4 Experiments and Results
149
Surgical device
Mock membrane
Fig. 6.13 Experimental system setup: rigid 1 Desired force input Filtered force sensor output
0.8
Force (N)
0.6
0.4
0.2
0
0.2 1.2
1.4
1.6
1.8
2
2.2
Time (s)
Fig. 6.14 Force output of the force control system
Fig. 6.14. As can be seen, the actual force is able to follow the desired force quite well to insert the tube. It can be concluded that the tracking performance of the force feedback controller is acceptable for this application. For a comparison, 100 tube insertion tests of the same conditions are also carried out by using the positionbased insertion method. In the tests, 1 out of 50 tubes fails to be inserted on the normal membranes while 7 out of 50 tubes fail on the strong membranes. Therefore, the total success rate using positionbased insertion method
150
6 Force–Position Control for Fast Tube Insertion 20 Normal Strong
Number
15 10 5 0 0.4
0.45
0.5
0.55
0.6
0.65
Time (s)
(a) 0.65
Time (s)
0.6
0.55
0.5
0.45
0.4 Normal
Strong
Type of Membrane
(b)
Fig. 6.15 Insertion time by using forcebased insertion method: a histogram; b boxplot
is 92%, therein the success rates are 98% and 86% on normal membrane and strong membrane, respectively. By comparing the success rate of both methods, it can be found that the forcebased insertion method has a better performance than the positionbased one. The success rate is improved obviously while the forcebased insertion method is applied on the strong membranes. This is because the forcebased insertion method allows a varied insertion depth on different membranes based on the force sensing information while the positionbased insertion method fixes the insertion depth. The adaptive varied insertion depth helps the device to adapt different membranes. However, the insertion time of the forcebased insertion method is also varied. The insertion time statistics and boxplot while applying the forcebased insertion method are shown in Fig. 6.15. The average insertion time of the forcebased insertion method is 0.4904 s for the normal membranes and 0.5470 s for the stronger membranes while the median insertion time of the forcebased insertion method is 0.4890 s for the normal membranes and 0.5490 s for the stronger membranes. This is reasonable because the resistance of the stronger membrane is bigger than the other one, i.e., larger force is required for inserting the tube on strong membranes. Moreover, the
6.4 Experiments and Results
151
minimum insertion time is 0.413 s and the maximum insertion time is 0.644 s. On the other hand, because of the fixed insertion depth, the insertion time by using the positionbased insertion method is a constant which is 0.138 s. As can be observed, the insertion time of the forcebased method is about 3 to 4.7 times longer than the positionbased method. It is due to the fact that the response of the force feedback controller is slow which slow down the procedure. This is also validated in the analysis in the previous section.
6.4.1.2
Experimental Results on Force–Position Insertion Method
Next, the force–position insertion method is applied. One hundred tube insertion tests are carried out where 50 normal membranes and 50 strong membranes are used. For the force–position insertion method, 49 out of 50 tubes are successfully inserted on the normal membranes and 49 out of 50 tubes are also inserted on the strong membranes successfully. Therefore, the success rates for both kinds of membranes are the same which is 98%, and the overall success rate is 98% as well. These results show that the force–position insertion method is robust and reliable which has better performance than the postilionbased insertion method and has similar performance to the forcebased insertion method. As similar to the forcebased insertion method, the insertion depth is varied by using the force–position insertion method and thus the insertion time is varied as well. Its insertion time statistics and boxplot are shown in Fig. 6.16. The average insertion time (success cases) of the force–position insertion method is 0.1720 s for the normal membranes and 0.1879 s for the strong membranes while the median insertion time of the forcebased insertion method is 0.1630 s for the normal membranes and 0.1720 s for the stronger membranes. The minimum insertion time is 0.1470 s while the maximum time is 1.0480 s, the second longest time is 0.6600 s (not shown in the boxplot), and all the other insertion times are less than 0.19 s. In addition, the myringotomy time by the proposed surgical device is fixed which is 0.35 s, and thus the whole surgical time from myringotomy to tube release by using the force–position insertion method can be mostly within 0.54 s. This time is also one crucial parameter as it involves intrusion on the TM. In comparison to conventional surgery mentioned in Sect. 6.1, both the insertion time and the surgical time using the force–position insertion method are much shorter. Table 6.1 and Fig. 6.17 show the comparisons among the force–position insertion method and the two other insertion methods. As observed, the force–position insertion method is capable of remaining a high and reliable success rate close to the forcebased insertion method as well as achieving a short insertion time close to the positionbased insertion method. Specifically, the force–position insertion method improves 6% in success rate over the positionbased insertion method, and its insertion time reduced to about 35% of the time incurred using the forcebased insertion method. In fact, this method integrates the advantages from the positionbased and forcebased insertion methods. The motion trajectory input is the factor in speeding
152
6 Force–Position Control for Fast Tube Insertion 30
Number
Normal Strong
20
10
0 0.145
0.15
0.155
0.16
0.165
0.17
0.175
0.18
0.185
Time (s)
(a) 0.19 0.185 0.18
Time (s)
0.175 0.17 0.165 0.16 0.155 0.15 0.145 0.14 Normal
Strong
Type of Membrane
(b)
Fig. 6.16 Insertion time by using force–position insertion method: a histogram; b boxplot Table 6.1 Comparisons among insertion methods on rigid setup Membrane Positionbased Forcebased Normal
Strong
Total
Success rate Mean time Median time Success rate Mean time Median time Success rate Mean time Median time
98% 0.138 s 0.138 s 86% 0.138 s 0.138 s 92% 0.138 s 0.138 s
100% 0.4904 s 0.4890 s 100% 0.5470 s 0.5490 s 100% 0.5187 s 0.5215 s
Force–position 98% 0.1720 s 0.1630 s 98% 0.1879 s 0.1720 s 98% 0.1799 s 0.1650 s
up the procedure while the success rate is guaranteed by the force feedback ratio controller.
6.4 Experiments and Results
153
Fig. 6.17 Comparison among the insertion methods on rigid setup
100
0.7 Positionbased Forcebased ForcePosition
90 0.6 80 0.5
Mean Time (s)
Success Rate (%)
70 60 50 40 30
0.4
0.3
0.2
20 0.1 10 0
0 Normal
Strong
Type of Membrane
Normal
Strong
Type of Membrane
6.4.2 Experiments and Results on Stabilized Handheld Setup Actually, the human hand tremor could affect the success rate of the device no matter which insertion method is applied. To address this issue, the mechanical approach for stabilizing the handheld device is used. The experimental tests while the device is held by hand with the assistance of the stabilization approach have been carried out on a mock ear model by using the force–position insertion method, the setup of which is shown in Fig. 6.18. One hundred tube insertion tests are carried out where 50 normal membranes and 50 strong membranes are used. Totally, 97 out of 100 tubes were success in being inserted on the membranes and thus the overall success rate is 97%. The boxplot of insertion time is shown in Fig. 6.19. As can be seen, most insertion times are also less than 0.19 s. Moreover, the average insertion time is 0.1755 s and the median insertion time is 0.1740 s. By comparing these results to the previous results shown in Sect. 6.4.1.2, the success rate and the insertion time are very similar. Therefore, the force–position insertion method is also very efficient on the stabilized handheld setup. In summary, the proposed force–position insertion method is an efficient compromise of all which is able to implement the tympanostomy tube insertion steadily with a high success rate and a relatively short time (much shorter than the insertion time in the conventional surgery).
154
6 Force–Position Control for Fast Tube Insertion Support Arm
Ear Model with Mock Membrane Surgical Device
Fig. 6.18 Experimental system setup: stabilized handheld (© 2016 IEEE. reprinted, with permission, from [16]) 0.24 0.23 0.22
Time (s)
0.21 0.2 0.19 0.18 0.17 0.16 0.15 Total
Type of Membrane
Fig. 6.19 Insertion time by using force–position insertion method on stabilized handheld setup
6.5 Chapter Summary
155
6.5 Chapter Summary In this chapter, the procedure of tympanostomy tube insertion using a novel officebased surgical device is investigated. The tube insertion procedure is one key step to success in the surgery. To insert the tube by the developed surgical device successfully and fast, a new insertion method using the concept of selective force–position control is proposed and developed. A positionbased force control (in cascade control structure) is first designed as the basic control scheme, where a position controller for the USM stage is in the inner loop and a force feedback controller is in the outer loop. After that, a motion trajectory input serves like feedforward and an adjustable weighting between both loops are added into the basic control scheme to construct the modified control scheme, which is a kind of force–position control scheme. The feedforward mechanism helps to speed up the insertion procedure, and the force feedback ratio controller guarantees the high success rate in different membranes. The experimental results show that the proposed force–position control scheme has a good performance with respect to tube insertion. By using the rigid or the stabilized handheld setup, its success rate is at least 97%, which is close to the forcebased insertion method (100%). Meanwhile, its insertion time is less than 0.19 s, which is much shorter than the forcebased insertion method and close to the positionbased insertion method. The proposed insertion method, integrating the positionbased and the forcebased insertion methods, benefits the surgical device in tympanostomy tube insertion. The proposed force–position control scheme can be also applied to other applications where fast and robust insertion operations are needed. Moreover, further research on the optimization of the motion control and the force control aiming to achieve better insertion accuracy and faster response (which could help to improve the performance) can be one future work.
References 1. J.W. Seibert, C.J. Danner, Eustachian tube function and the middle ear. Otolaryngol. Clin. North Am. 39(6), 1221–1235 (2006) 2. G. Gates, Costeffectiveness considerations in otitis media treatment. Otolaryngol. Head Neck Surg. 114(4), 525–530 (1996) 3. Myringo  By ENT Surgical, A Naiot Venture Accelerator portfolio company. http://vimeo. com/18496049 (2011) 4. A.V. Kaplan, J. Tartaglia, R. Vaughan, C. Jones, Mechanically registered videoscopic myringotomy/tympanostomy tube placement system. US Patent, US7704259B2 (2010) 5. G. Liu, J.H. Morriss, J.D. Vrany, B. Knodel, J.A. Walker, T.D. Gross, M.D. Clopp, B.H. Andreas, Tympanic membrane pressure equalization tube delivery system. US Patent, US8864774B2 (2011) 6. E.J. Shahoian, System and method for the simultaneous automated bilateral delivery of pressure equalization tubes. US Patent, US8052693B2 (2008) 7. S.P. cottler, B.W. Kesser, Tube, stent and collar insertion device. US Patent, US20080051804A1 (2008) 8. C.R. Wagner, N. Stylopoulos, P.G. Jackson, et al., The benefit of force feedback in surgery: examination of blunt dissection. Presence Teleoperators Virtual Environ. 16(3), 252–262 (2007)
156
6 Force–Position Control for Fast Tube Insertion
9. A.M. Okamura, Haptic feedback in robotassisted minimally invasive surgery. Curr. Opin. Urol. 19(1), 102 (2009) 10. S.G. Yuen, D.P. Perrin, N.V. Vasilyev, P.J. del Nido, R.D. Howe, Force tracking with feedforward motion estimation for beating heart surgery. IEEE Trans. Robot. 26(5), 888–896 (2010) 11. W.T. Latt, R.C. Newton, M. VisentiniScarzanella, C.J. Payne, D.P. Noonan, J. Shang, G.Z. Yang, A handheld instrument to maintain steady tissue contact during probebased confocal laser endomicroscopy. IEEE Trans. Biomed. Eng. 58(9), 2694–2703 (2011) 12. R. Taylor, X. Du, D. Proops, A. Reid, C. Coulson, P.N. Brett, A sensoryguided surgical microdrill. Proc. Inst. Mech. Eng. Part C J. Mech. Eng. Sci. 224(7), 1531–1537 (2010) 13. X. Du, M.Z. Assadi, F. Jowitt, P.N. Brett, S. Henshaw, J. Dalton, D.W. Proops, C.J. Coulson, A.P. Reid, Robustness analysis of a smart surgical drill for cochleostomy. Int. J. Med. Robot. Comput. Assist. Surg. 9(1), 119–126 (2013) 14. T.M. Williamson, B.J. Bell, N. Gerber, L. Salas, P. Zysset, M. Caversaccio, S. Weber, Estimation of tool pose based on force–density correlation during robotic drilling. IEEE Trans. Biomed. Eng. 60(4), 969–976 (2013) 15. H.I. Inc. FSS low profile force sensors, http://www.honeywell.com/ 16. W. Liang, K.K. Tan, Force feedback control assisted tympanostomy tube insertion. IEEE Trans. Control Syst. Technol. 25(3), 1007–1018 (2017) 17. J. Aernouts, J. Soons, J. Dirckx, Quantification of tympanic membrane elasticity parameters from in situ point indentation measurements: validation and preliminary study. Hear. Res. 263(1–2), 177–182 (2010) 18. L.C. Kuypers, W.F. Decraemer, J.J. Dirckx, Thickness distribution of fresh and preserved human ear membranes measured with confocal microscopy. Otol. Neurotol. 27(2), 256–264 (2006) 19. T. Koike, H. Wada, T. Kobayashi, Modeling of the human middle ear using the finiteelement method. J. Acoust. Soc. Am. 111(3), 1306–1317 (2002) 20. R.Z. Gan, Q. Sun, Finite element modeling of human ear with external ear canal and middle ear cavity, in Proceedings of the Second Joint 24th Annual Conference and the Annual Fall Meeting of the Biomedical Engineering Society][Engineering in Medicine and Biology, vol. 1 (2002), pp. 264–265 21. J. Fay, S. Puria, W. Decraemer, C. Steele, Three approaches for estimating the elastic modulus of the tympanic membrane. J. Biomech. 38, 1807–1815 (2005) 22. K.K. Tan, S. Huang, W. Liang, A.A. Mamun, E.K. Koh, H. Zhou, Development of a spherical air bearing positioning system. IEEE Trans. Ind. Electron. 59(9), 3501–3509 (2012) 23. Q. Ren, J. Xu, X. Li, A datadriven motion control approach for a robotic fish. J. Bionic Eng. 12(3), 382–394 (2015) 24. K.K. Tan, W. Liang, S. Huang, L.P. Pham, S. Chen, C.W. Gan, H.Y. Lim, Precision control of piezoelectric ultrasonic motor for myringotomy with tube insertion. J. Dyn. Syst. Meas. Control 137(6), 064504 (2015) 25. K.H. Ang, G. Chong, Y. Li, Pid control system analysis, design, and technology. IEEE Trans. Control Syst. Technol. 13(4), 559–576 (2005) 26. K.J. Åström, T. Hägglund, Advanced PID Control. ISAThe Instrumentation, Systems, and Automation Society (2006) 27. S.D. Eppinger, W.P. Seering, Understanding bandwidth limitations in robot force control, in 1987 IEEE ICRA, vol. 4, pp. 904–909 (1987)
Chapter 7
Robust Impedance Control of Constrained Piezoelectric ActuatorBased EndEffector
Impedance control is one of the common force and position control methods. It regulates the relationship between motion and force which is able to deal with the tasks where the control subject has certain level of uncertainty. It is often used in the applications of robot–environment interaction or human–robot interaction where the force–position relation is of concern. In this chapter, a robust impedance control is designed for a semiautomated surgical device based on the dynamic coordination of force and position control. The control scheme is proposed for compliant manipulation in which motion and force trajectories are controlled to achieve position and force regulation. Moreover, the control scheme is formulated to accommodate parametric uncertainties, nonlinearities, and external disturbances in the motion system. The proposed scheme employs only a single controller to control both position and contact force of the surgical device. The stability of the control scheme is analyzed and proven theoretically. Desirable control performances in following the desired motion and force trajectories is demonstrated through experimental studies on the surgical device. An important advantage of the robust impedance control scheme is that it does not require the exact system parameters in the physical realization. The proposed control scheme is useful for the implementation of applications demanding both sensing and control of motion and force trajectories.
7.1 Introduction Various medical application tasks inevitably involve direct interaction with the environment (e.g., human body). As the advancement in medical technology field continues to offer surprising advances in medical applications, one of the most prominent developments is in piezoelectric actuator (PA) technology. Even though the tech© Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931_7
157
158
7 Robust Impedance Control of Constrained Piezoelectric …
nology is still in its relatively early stages of application development, it is already demonstrating the tremendous value and functionality it offers to medical devices necessitating some forms of position and force control. PAs are often preferred because of certain distinct advantages in terms of hightorque operations without the need for gears, fast response, compact, lightweight, and high holding torque in the absence of supplied power [1]. Recent design innovations have created PAs with dramatically smaller size, greater robustness, and improved manufacturability. With these improvements, PAs are now expanding into applications such as piezoelectric micromotor technology (e.g., squiggle motor from New Scale Technologies Inc. [2]), handheld clinical diagnostics system [3], medical device [4], micro and nanofluidic [5], implantable device [6], micromanipulation [7, 8], etc. One of the major concerns in the setting up of effective control methodology is the existence of nonlinearities in the PAs. Apart from that, they need to perform robustly against the variations of parametric uncertainties and other modeling uncertainties [9]. The piezoelectric nonlinearities and uncertainties prevent the actuator from realizing an extremely high precise motion accuracy and resolution. Examples of modeling techniques for the PAs include using an inverse hysteresis model feedforward control [10, 11], voltage actuation [12], and current or charge actuation [13]. In most of the studies, a complex disturbance model has been adopted to compensate the disturbances. The modeling of disturbance is timeconsuming and prone to modeling error at the time of identification. Hence, a disturbance observer (DOB) is often used in the motion control system due to the advantages of efficiency and performance [14, 15]. By treating the nonlinearities and uncertainties as a lumped disturbance between an actual plant and a nominal model, DOB can be used to compensate and guarantee the system dynamics to be approximately as the nominal one. The robustness and performance of motion control system can be improved with an appropriate DOBbased control method. Recent examples include estimation of higher order disturbances [16], fine motion control with sudden disturbance observer [17], and disturbance estimation in the plant [18]. However, DOB alone cannot compensate nonlinear disturbances and unmodeled dynamics sufficiently, especially the highfrequency component. The variable structure control has been demonstrated as an attractive nonlinear control approach against nonlinearities, uncertainties, and bounded disturbances [19, 20]. By combining DOB and sliding mode control (SMC), position tracking control with disturbance compensation can be managed simultaneously and extended to impedance control. Examples of accurate position tracking using SMC and DOB include quadrotor vehicle [21], optical disk drives [22], autonomous mobile robots [23], and so on. However, studies relating to medical devices are limited. Another major challenge in the application of medical devices is the fusion of position and force control. Extensive study of the position and force regulation strategies has led to two important approaches, which are hybrid positionforce control [24–26] and impedance control [27–30]. For the hybrid positionforce control, the critical issue is to achieve smooth transition between the switching of both controls. The control method decomposes the mechanism into position subspace and force
7.1 Introduction
159
subspace. During the implementation, switching between different control laws is needed because position and force are controlled separately. It is prone to a robustness problem when frequent transition is incurred. Meanwhile, impedance control [31] has a unified framework to control both position and force. A generalized impedance control is adopted to perform compliant manipulation. Compliant manipulation [32] is a control strategy that allows the manipulator to follow a motion trajectory and also exert a force trajectory while contacting with dynamic environment. In this chapter, the ear surgical device presented in Chap. 3 is used as a case study. In the surgical device, the precise motion generated by the PA is needed for the surgical application so that the success rate of the operation can be guaranteed. On the other hand, it is necessary to include an interaction force control method that reliably adapts the force exerted on the tympanic membrane (TM) in order to minimize serious deformation or discomfort. Thus, it should be good that both the position control and force control are considered, especially during the touch detection (by the tool set or the tube). Furthermore, the nonlinearities include hysteresis effect and creep effect on the PA as well as the instrument mechanism mounted on the actuator that acts as an external disturbance may degrade the device performance, and hence good robustness is also needed for this surgical device. Motivated by aforementioned important issues, a robust impedance control for the piezoelectric ultrasonic motordriven (USMdriven) medical device (i.e., PAbased endeffector) [33] is proposed in this chapter to address these challenges. The control methodology is formulated without using any form of feedforward compensation for the system uncertainties or nonlinearities. The main objective is to figure out a control scheme that is able to track and regulate both desired motion and force trajectories so as to achieve a desired dynamic performance through specifying the impedance parameters in the proposed mechanism. In addition, the proposed impedance control scheme is designed to accommodate system parametric uncertainties, nonlinearities, and disturbances. This chapter mainly focuses on the development of the robust impedance control scheme and its implementation on the novel surgical device. Significantly, the impedance control is integrated with a SMC and a DOB in order to achieve better robustness. The SMC and the DOB can be complemented to each other. The DOB helps the SMC to decrease the switching gain and attenuate the chattering effect, while the SMC provides assistance for the DOB in compensating the nonlinear disturbance in high frequency. Furthermore, the proposed control scheme is implemented to the surgical device, which extends such kind of control scheme into a medical application. Finally, the relationship between the controller performance and the impedance parameter is investigated via the experimental study, which may provide a guidance on the parameter selection.
160
7 Robust Impedance Control of Constrained Piezoelectric …
7.2 Piezoelectric Ultrasonic Motor Modeling in Constrained Environment Figure 7.1 depicts the USMdriven (PAbased) surgical device as an endeffector equipped on to a manipulator (LBR iiwa 14, KUKA). The endeffector driven by USM is envisioned as a medical device which is mainly designed for carrying out precise surgical operations. Especially, this device is feasible in assisting the surgical treatment for otitis media with effusion (OME). It will ease up the myringotomy with tube insertion procedure in treating OME after medication as the first treatment failed. Therefore, precision motion tracking performance on desired fine motion trajectories is needed in completing the surgical procedure smoothly and efficiently. After the touch detection, it is required to maintain a steady contact force between the tool set or the tube and the TM so as to guarantee the high success rate. Moreover, during the tube insertion, the insertion force should be minimal in order to avoid overdeformation on the TM which might cause discomfort or possible deaf to patients. Hence, a robust impedance control is necessary for such surgical device to regulate both position and force trajectories during the surgical procedure. Before embarking on the design of the proposed control scheme, an electromechanical model of USM is needed and has been identified based on previous studies [34]. The USM is formulated based on a voltagedriven system that is making contact with the environment by its endeffector. The whole manipulation mechanism is driven by the USM with the output displacement and the interaction force mea
Manipulator
Piezoelectric actuatorbased endeffector
Fig. 7.1 USMdriven (PAbased) endeffector with manipulator
7.2 Piezoelectric Ultrasonic Motor Modeling in Constrained Environment
161
Fig. 7.2 Model of USM
sured by a linear encoder and a force transducer/sensor, respectively. For compliant manipulation control purposes, the dynamic equation of the USM can be described by the following set of equations: m a x¨ + ba x˙ + ka x + F = Ta u,
(7.1)
where m a is the mass on the USM, ba is the viscous damping coefficient, ka is the stiffness coefficient, and f is the interaction force exerted on the environment which is the tympanic membrane. The terms x, ¨ x, ˙ and x are the acceleration, velocity, and position, respectively, of the USM slider. The variable Ta is the electromechanical ratio and u is the control input to the system. In practice, there are other nonlinear effects present in the USM. Furthermore, there are always external disturbances in the practical motion system. For this reason, the model of USM (7.1) is rewritten as (7.2) and shown in Fig. 7.2 in order to include these effects, m a x¨ + ba x˙ + ka x + F + Fn + Fed = Ta u,
(7.2)
where Fn is the normalized lumped effect of uncertain nonlinearities such as friction and Fed is the unknown disturbance. The terms Fn and Fed are bounded with Fn  ≤ δn and Fed  ≤ δed , where both δn and δed are positive constant numbers. The dynamic model of the USM is obtained by rearranging (7.2) into m x¨ + b x˙ + kx + kta (Fn + Fed ) + Fe = u,
(7.3)
where m = m a /Ta , b = ba /Ta , k = ka /Ta , kta = 1/Ta , and Fe = F/Ta . With the given model (7.3), a robust impedance control scheme can be established to effectively control the USM manipulation system.
7.3 Controller Design For a better understanding on the design of the proposed control scheme, the basic idea of the impedance control is firstly introduced and followed by the presentation of the robust impedance control design.
162
7 Robust Impedance Control of Constrained Piezoelectric …
Fig. 7.3 Device in contact with environment (Reprinted from [35], Copyright (2019), with permission from Elsevier)
7.3.1 Impedance Control In the formulation of an impedance control scheme to account for the situation where possible external forces are acting on the device that would change the dynamic behavior of the device, a model of the environment will be included in the system where the interaction force will act on the surgical device. The environment is modeled as m e (x¨ − x¨e ) + be (x˙ − x˙e ) + ke (x − xe ) = Fe ,
(7.4)
where m e is the equivalent mass, be is the equivalent damping coefficient, ke is the equivalent stiffness, x is the position at the contact point, and xe is the equilibrium position of the environment in the absence of interaction force. The terms m e , be , and ke are positive parameters representing the environment. Figure 7.3 depicts such a concept, where a device of mass m is trying to reach the desired position xd and contacts with the environment at position xe . The impedance control method focuses on the design to achieve desired dynamic relation experienced by the device and its environment which is the TM. The target impedance is specified as in [31], which is m i e¨ p + bi e˙ p + ki e p = −Fe ,
(7.5)
where m i , bi , and ki are the target impedance parameters representing inertia, damping, and stiffness, respectively, which are usually specified by the user to be positive in order to obtain a stable response. They cause the device and environment to exhibit the dynamics of a massspringdamper system. The term e p = x − xd is the position tracking error, where xd is the desired position. The original definition of impedance requires accurate control on the interaction of the manipulator and environment to achieve a desired force response [36]. Therefore, by employing the extended and generalized impedance control scheme [27], it is possible to apply a reference interaction force trajectory. The generalized impedance equation, which combines both position and force errors, is chosen as a secondorder function and given by m i e¨ p + bi e˙ p + ki e p = −k f e f ,
(7.6)
7.3 Controller Design
163
where m i , bi , ki , and k f are the target impedance parameters of the generalized impedance. The term e f = Fe − Fd is the force error and Fd is the desired force. Assuming k f = 1 and Fd = 0, the original impedance in (7.5) is obtained from the generalized impedance equation (7.6). With the generalized impedance, the interaction force can be directly controlled for systems that require explicit control on the contact force during the compliant manipulation. The impedance parameters should be specified such that the system stability is preserved when it contacts the environment. The generalized impedance parameters described in (7.6) are chosen to be positive to ensure the stability of the closedloop system. Combining the dynamics of the contact environment and the impedance model yields m i x¨d + bi x˙d + ki xd + k f (m e x¨e + be x˙e + ke xe + Fd ) = m i x¨ + bi x˙ + ki x + k f (m e x¨ + be x˙ + ke x).
(7.7)
Based on the overall dynamics described by (7.7), a steadystate response analysis is conducted in the following. For the steadystate response, we have x¨d = 0, x˙d = 0, x¨e = 0, and x˙e = 0. Therefore, the above equation becomes ki xd + k f (ke xe + Fd ) = ki x + k f ke x, ki (xd − x) = k f (ke x − ke xe − Fd ).
(7.8)
Furthermore, at steady state, assuming trajectory x is reachable, the environment described by (7.4) is alternatively expressed as ke (x − xe ) = Fe .
(7.9)
Substituting (7.9) into (7.8) gives the following equation: ki (xd − x) = k f (Fe − Fd ), ki e p = −k f e f .
(7.10)
From (7.8) to (7.10), we can obtain the following equations: ki e p = −(k f ke x − k f de xe − k f Fd ) = −(k f ke x − k f de xe − k f Fd − k f ke xd + k f ke xd ) = −[k f ke (x − xd ) − k f ke xe − k f Fd + k f ke xd ] = −k f ke e p + k f ke xe + k f Fd − k f ke xd . Hence, we have
(7.11)
164
7 Robust Impedance Control of Constrained Piezoelectric …
(ki + k f ke )e p = k f (ke xe − ke xd + Fd ) = k f [ke (xe − xd ) + Fd ].
(7.12)
Therefore, the steadystate position error and force error of the system are given as kf [ke (xe − xd ) + Fd ], ki + ke k f ki = − e pss , kf
e pss =
(7.13)
e f ss
(7.14)
where e pss and e f ss are the steadystate position error and steadystate force error, respectively. The analytical results in (7.13) and (7.14) can be used to evaluate the tracking performance. If ki k f is designated, the position control is emphasized. kf [ke (xe − xd ) + Fd ], ki ≈ −ke (xe − xd ) − Fd .
e pss ≈
(7.15)
e f ss
(7.16)
Under this condition, the steadystate position error e pss is relatively small, whereas the steadystate force error e f ss is comparatively large. If ki k f is designated, force control is emphasized. e pss ≈ (xe − xd ) + e f ss ≈ −
Fd , ke
ki Fd [(xe − xd ) + ]. kf ke
(7.17)
(7.18)
Under this condition, the steadystate position error, e pss , is relatively large, whereas the steadystate force error, e f ss , is comparatively small. Therefore, a compromise between position and force control is needed depending on the task requirement through adjustment of the target impedance parameters in consideration of the environment property. To realize a satisfactory operation with desired interaction behavior using generalized impedance control methodology, desired motion trajectory, desired interaction force trajectory, desired dynamic relationship between position and force errors, as well as the selection of suitable target impedance parameters should be determined [37]. Remark 7.1 In unconstrained free space, both the desired force and measured force are zero, i.e., Fd = Fe = 0. The generalized impedance described by (7.6) becomes a motion tracking because the righthand side of the equation is zero.
7.3 Controller Design
165
Remark 7.2 In constrained space, both of the desired force and measured force are nonzero in compliant manipulation as shown in (7.6), i.e., Fd = 0 and Fe = 0. Therefore, a balance between the motion control and force control takes place to ensure a desired transient response of the closedloop system (7.7) with the assignment of impedance parameters [38].
7.3.2 Robust Impedance Control Design Apart from this, the actual parameters of the system are difficult to be obtained. Consequently, the nominal or identified model is employed to design the controller so that the model uncertainties should be taken into consideration. Since the electrical time constant is typically much smaller than the mechanical time constant, the delay due to electrical transient response can be ignored. For these reasons, a model with uncertainties is introduced. Firstly, the parametric uncertainties are modeled as Δm = m − m n  ≤ δm , Δb = b − bn  ≤ δb ,
(7.19)
Δk = k − kn  ≤ δk , where Δm, Δb, and Δk represent the parametric model uncertainties; m n , bn , and kn represent the nominal parameters; and the positive values δm , δb , and δk denote the bound of the parameters. Secondly, there exists an upper bound δned of the nonlinear effects and external disturbances such that Fn + Fed  ≤ δn + δed ≤ δned .
(7.20)
It should be noted that the exact values of m, b, k, kta , Fn , and Fed in (7.3) are assumed to be unknown or uncertain but bounded in the proposed model of uncertainties. However, the estimated values and their corresponding bounds of the system parameters, the bound of nonlinear effects and external disturbances δned and δ˙ned are available. With this assumption, the robust impedance control can be realized. In summary, the model with uncertainties can be written as m n x¨ + bn x˙ + kn x + Δm x¨ + Δb x˙ + Δkx + kta (Fn + Fed ) = u − Fe .
(7.21)
For the USMbased ear surgical device described in (7.3), a robust impedance control scheme is proposed and formulated to achieve desired generalized impedance given by (7.6). The block diagram of the proposed robust impedance control scheme is shown in Fig. 7.4. Remarkably, the SMC with DOB is designed to improve the
166
7 Robust Impedance Control of Constrained Piezoelectric …
Sliding Mode Controller
Endeffector
System Model
Disturbance Observer
Contact Environment
Linear Encoder
Force Sensor
Impedance Model
Fig. 7.4 Block diagram of robust impedance control scheme
robustness of the basic impedance controller. The detailed design of the control scheme is given in the following.
7.3.2.1
Sliding Mode Controller
For the SMC, the sliding surface is defined as σ = e˙ p + ξ,
(7.22)
where ξ is the state of a dynamic compensator used to shape the motion and interaction force errors. The dynamic compensator can be formulated as ξ˙ = −αξ + k p e p + kv e˙ p + k f e f ,
(7.23)
where α ≥ 0 is a constant scalar. k p , kv , k f are the control gains which are related to the specified impedance. Differentiating the sliding function with respect to time yields (7.24) σ˙ = e¨ p + ξ˙ . To establish the closedloop dynamics of the system under the robust impedance control, the dynamic compensator (7.23) is taken into (7.24) to yield the following relationship: (7.25) σ˙ = e¨ p − αξ + k p e p + kv e˙ p + k f e f , e¨ p + k p e p + kv e˙ p + k f e f = σ˙ + αξ = σ˙ + α(σ − e˙ p ),
(7.26)
7.3 Controller Design
167
e¨ p + k p e p + (kv + α)e˙ p + k f e f = σ˙ + ασ.
(7.27)
By choosing kp =
kfd kd bd , kv = − α, k f = , md md md
(7.28)
the closedloop dynamics (7.24) becomes m d e¨ p + bd e˙ p + kd e p = −k f d e f + m d (σ˙ + ασ ).
(7.29)
During sliding motion where σ = 0 and σ˙ = 0, the closedloop dynamics (7.29) reaches the target impedance (7.4). Based on (7.24), we can obtain that σ˙ = e¨ p + ξ˙ = x¨ − x¨d + ξ˙ .
(7.30)
According to the model (7.3), then we have x¨ =
1 (u − bn x˙ − kn x − Fe + d), mn
(7.31)
where d is the lump disturbance, − d = kta (Fn + Fed ) + Δm x¨ + Δb x˙ + Δkx.
(7.32)
Take it into (7.30), it can be obtained that σ˙ =
1 (u − bn x˙ − kn x − Fe + d) − x¨d + ξ˙ , mn
u = bn x˙ + kn x + Fe − ks σ − λsign(σ ) + m n (x¨d − ξ˙ ),
(7.33)
(7.34)
where sign(·) is the sign function and λ is the sliding mode controller gain.
7.3.2.2
Disturbance Observer
To increase the robustness, a DOB is designed, the output of which is used in the robust impedance control scheme. By assembling the parametric uncertainties, unknown dynamics, and unknown disturbance into the lump disturbance, d, the actual motion control system described in (7.3) can be rewritten as follows: m n x¨ + bn x˙ + kn x = u − Fe + d.
(7.35)
168
7 Robust Impedance Control of Constrained Piezoelectric …
Position Controller
Environment
DOB Fig. 7.5 General structure of DOB
An overall control block diagram of DOB is shown in Fig. 7.5. From Fig. 7.5, we have (7.36) gn ∗ (u − Fe + d) = x, where gn represents the plant model of the system in time domain and ∗ denotes the convolution operator. If Q(s) = 1, then we have d = gn−1 ∗ x − (u − Fe ),
(7.37)
where gn−1 represents the inverse plant model. As can be seen, the disturbance estimation can be obtained from the difference between the actual control input to the plant and also the estimate of control input obtained from the inverse plant model. If the lump disturbance is compensated completely, the plant in (7.3) will perform as a nominal model. The lump disturbance, d, can be estimated by using DOB with a lowpass filter, where the filter is to ensure that the inverse plant model has proper causality. The DOB then can be described by dˆ = q ∗ [gn−1 ∗ x − (u − Fe )] = q ∗ d,
(7.38)
where dˆ is the estimated lump disturbance and q represents the lowpass filter to be designed. As suggested in [39], the relative degree of the filter should be greater than or equal to the transfer function of the nominal plant to satisfy the causality. For the USM, it is a secondorder system and thus a secondorder lowpass Butterworth filter is designed as shown in the following equation:
7.3 Controller Design
169
Q(s) =
s2 +
√
wc2 2wc s + wc2
,
(7.39)
where wc is the cutoff frequency of the secondorder lowpass filter in the DOB. It defines the bandwidth over which the DOB is effective (i.e., Q(s) ≈ 1 while its input is within the cutoff frequency). Detailed analysis on disturbance observer controlled systems can be found in [40, 41]. Hence, the final control law for the robust control scheme is given by u = bn x˙ + kn x + Fe − ks σ − λsign(σ ) − dˆ + m n (x¨d − ξ˙ ).
7.3.2.3
(7.40)
Stability Analysis
To perform the stability analysis for the control system, there is a theorem shown below with an assumption that the DOB works and thus the disturbance observer error between the estimated disturbance and lump disturbance is bounded (i.e., dˆ − d ≤ Δ), where Δ > 0 is the upper bound of the disturbance observer error. Theorem 7.1 For the USM system described in (7.3) under the modeled uncertainties (7.19) and (7.20), the target generalized impedance can be achieved when the control law (7.40) is applied and the condition λ ≥ Δ is satisfied. Proof of Theorem 7.1 The Lyapunov function is defined as V =
1 mn σ 2, 2
(7.41)
which is continuous and nonnegative. The time derivative of V is calculated as V˙ = m n σ σ˙ 1 (u − bn x˙ − kn x − Fe + d) − x¨d + ξ˙ ] mn = σ (u − bn x˙ − kn x − Fe + d − m n x¨d + m n ξ˙ ).
= mn σ [
(7.42)
Substituting (7.34) into the above equation yields ˆ V˙ = σ (−ks σ − λsign(σ ) + d − d) ˆ = −ks σ 2 − λσ  + (d − d)σ ≤ −ks σ 2 − λσ  + dˆ − dσ . If the λ is chosen properly such that it holds λ > Δ, then we have
(7.43)
170
7 Robust Impedance Control of Constrained Piezoelectric …
V˙ ≤ −ks σ 2 − λσ  + Δσ  ≤ −ks σ 2 .
(7.44)
From the time derivative of the Lyapunov function, the Lyapunov stability check using (7.44) implies that V˙ is always negative for σ = 0 as t → ∞ for a stable system. According to the definition of sliding surface (7.22), the target generalized impedance described by (7.6) for compliant manipulation is guaranteed. Both control system stability and impedance control convergence are guaranteed by the proposed control law (7.40). Remark 7.3 The high gain of the sign function sign(σ ) in the robust impedance control law (7.40) can lead to the control chattering because it is a discontinuous function. The concept of boundary layer technique [42] is used to smoothen the control signal. The smoothed control signal avoids the excitation of highfrequency dynamics. With that, the discontinuous function is replaced by a smooth saturation function which is defined as follows: ⎧ −1, for σ < −φ ⎪ ⎪ ⎨σ σ , for − φ ≤ σ ≤ φ , = (7.45) sat φ ⎪ φ ⎪ ⎩ +1, for σ > φ where φ is a small constant which denotes the width of the boundary layer near the sliding mode surface. Then, the control law (7.40) becomes u = bn x˙ + kn x + Fe − ks σ − λsat
σ − dˆ + m n (x¨d − ξ˙ ). φ
(7.46)
Now, the control law (7.46) is a smooth varying function and the sliding condition holds. The control law can reduce the chattering effect and achieve fast sliding trajectory reaching action within the boundary layer.
7.4 Experimental Study To evaluate physically the proposed robust impedance control scheme for the USMdriven surgical device, a prototype for the experimental research has been established and the experimental study on mockup system is carried out.
7.4 Experimental Study
171
Contact environment (Membrane)
Endeffector
Fig. 7.6 Experimental system setup of the USMdriven surgical device (endeffector)
7.4.1 Experimental System Setup The device and the experimental system setup is shown in Fig. 7.6. The system comprises the prototype of the surgical device, a motor drive, a force sensor, an interface box with sensor amplifier module, and a computer embedded with dSPACE DS1104 control card which comprises analogtodigital converters (ADCs), digitaltoanalog converters (DACs), and quadrature encoder pulse (QEP) interfaces. The USM stage used in the endeffector is manufactured by Physik Instrumente (PI) GmbH & Co. KG., in which model number is M663. It has a maximum velocity of 400 mm/s and a maximum travel range of 19 mm. The minimum incremental displacement of the USM stage is 0.3 µm and the resolution of its integrated linear encoder is 0.1 µm for position measurement. A force sensor with a sensitivity of 0.12 mv/g manufactured by Honeywell International Inc. is utilized to measure the interaction force exerted on the environment (i.e., TM or mock membrane). The sampling time of the control loop is set at 1 ms. With fast sampling rate, the time derivative of position is assumed to approach the velocity and hence the time derivatives of x and e p are derived from the linear encoder measurement output. The dSPACE control card is a development and rapid prototyping system integrates the whole development cycle into a single environment where MATLAB/Simulink blocks can be used directly for graphical input–output configuration. This realtime interface enables Simulink program to be compiled, downloaded, and run in real time automatically. For the application in this work, the environment is constituted with a thin film, a mock membrane made of polyethylene (PE), where the stiffness depends on the
172
7 Robust Impedance Control of Constrained Piezoelectric …
Table 7.1 Controller parameters Parameters mi bi Values
1
2×1×ω
ki
ω
ω2
α
2 × π × 5 20
λ
ks
ωc
0.01
0.001
100 Hz
contact point. The controller parameters of the proposed robust impedance control scheme are finetuned and shown in Table 7.1.
7.4.2 Results and Discussions Two kinds of experiments have been carried out in this study to verify the effectiveness and evaluate the performance of the proposed robust impedance control scheme.
7.4.2.1
Varying Impedance Experiment
To verify the effectiveness of the proposed robust impedance control scheme, several experiments are conducted with different target impedances by varying k f while keeping other parameters the same. Each experiment is to simulate the process of contacting the mock membrane by the tool set or the tube. During the experiment, the tool set with the tube is placed in a certain distance from the mock membrane at first, and then the goal of the control scheme is to control both the motion and the contact force so that the tool set or the tube can be moved forward to be in contact with the mock membrane at a certain level of force. Two Scurves are used as the desired motion trajectory and the force trajectory, respectively. Figure 7.7 shows the tracking performance on both the motion and force trajectories with the varying k f . As can be seen, both the motion trajectory and the force trajectory can be tracked correctly using the proposed robust impedance control scheme. Significantly, it should be noted that the steady contact force control can be achieved while the position control performance does not significantly degrade with k f = 100000. This is very helpful for the surgical device application when it conducts the membrane touch detection process, where the success rate can be guaranteed by the steady and precise contact force control and the safety can be ensured by the gentle position control. Furthermore, it is very clear that the position/motion tracking performance decreases but the force tracking performance increases as k f increases. Figure 7.8 shows the steadystate position error and steadystate force error with different k f . The force error strictly decreases as k f increases while the position error has the increasing trend as k f decreases. This phenomenon matches the analysis of the impedance control shown in (7.15) to (7.18). Therefore, it can be concluded that the position tracking performance and
7.4 Experimental Study
173
0.6
Position (mm)
0.5 0.4 0.3 0.2
Reference k f=0.01
0.1
k f=100 k f=10000
0 0
1
2
3
4
5
6
7
4
5
6
7
Time (s) 0.12 Reference k f=0.01
Force (N)
0.1 0.08
k f=100
0.06
k f=10000
0.04 0.02 0 0
1
2
3
Time (s)
Fig. 7.7 Experimental results with varying k f 0.06 Position error (mm) Force error (N)
0.05
RMSE
0.04
0.03
0.02
0.01
0 0.01
1
100
k
1000
100000
f
Fig. 7.8 Tracking errors with varying k f
the force track performance can be adjusted by choosing different k f . In other words, the system impedance can be adjusted by the impedance control scheme. Hence, different applications or functions can be achieved by adjusting k f in the impedance control scheme according to their requirements and the tradeoff between position control and force control.
174
7.4.2.2
7 Robust Impedance Control of Constrained Piezoelectric …
Comparison Experiment
To evaluate the performance of the proposed robust impedance control scheme, several experiments were conducted on tracking different force trajectories with different frequencies. For the purpose of comparison, a commonly used impedance control scheme called “proportional–integral–derivative (PID)based impedance control (PIDIC)” is used in the experiments (as a benchmark) where the PID position controller is designed to track the generated virtual reference based on (7.6), calculated by x¨r = x¨d −
1 (bi e˙ p + ki e p − k f e f ), mi
(7.47)
where xr is the commanded position to the position control loop generated by impedance model which is corrected depending on the value of the contact force Fe . To guarantee the position tracking performance, the optimal controller gains of the PID controller are calculated by the linearquadratic regulator (LQR) technique. The detailed implementation of this method can refer to the control scheme presented in [35]. Under the precision motion control, if a very good tracking performance can be achieved then x can be very close to xr (i.e., x ≈ xr and e p ≈ xr − xd ) and hence the impedance control can be achieved. Figures 7.9, 7.10, and 7.11 show the experimental results and the force tracking performance of both impedance control schemes mentioned in this chapter, where RIC stands for robust impedance control. The frequencies of the desired force trajectories are 0.25 Hz, 0.5 Hz, and 1 Hz, respectively, while their amplitudes are the same at 0 to 0.1 N.
0.1
Reference PIDIC RIC
Force (N)
0.08 0.06 0.04 0.02 0 0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Time (s)
Force Error (N)
0.02
PIDIC RIC
0.01 0 0.01 0.02 0
0.5
1
1.5
2
2.5
Time (s)
Fig. 7.9 Experimental comparison results (0.25 Hz)
3
3.5
4
4.5
5
7.4 Experimental Study
175
0.1
PIDIC RIC
Force (N)
0.08 0.06 0.04 0.02 0 0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Time (s)
Force Error (N)
0.02
PIDIC RIC
0.01 0 0.01 0.02 0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Time (s)
Fig. 7.10 Experimental comparison results (0.5 Hz) 0.1
Reference PIDIC RIC
Force (N)
0.08 0.06 0.04 0.02 0 0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Time (s)
Force Error (N)
0.02
PIDIC RIC
0.01 0 0.01 0.02 0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
Time (s)
Fig. 7.11 Experimental comparison results (1 Hz)
As can be observed from all the figures, both control schemes can track the force trajectories well. However, the proposed RIC can achieve better force trajectory tracking performance than the PIDIC at all frequencies, especially at 1 Hz. Figure 7.12 shows the error comparison results between the proposed RIC and the PIDIC. Two types of errors are used in the comparison, which are the maximum absolute error (MaxAE, emax ) and the rootmeansquare error (RMSE, er ms ). As can be seen, it is very clear that both the MaxAEs and the RMSEs while using the proposed RIC are smaller than the ones using PIDIC. Remarkably, the MaxAEs are improved by 17.29% to 35.03% and the RMSEs are improved by at least 48% comparing the proposed RIC with PIDIC. This is because the proposed RIC has the
176
MaxAE
0.03
Force Error (N)
Fig. 7.12 Error comparison (between PIDIC and RIC) of MaxAE and RMSE
7 Robust Impedance Control of Constrained Piezoelectric …
0.02
0.01
0 0.25
Force Error (N)
0.5
1
RMSE
0.03
PIDIC RIC
0.02
0.01
0 0.25
0.5
1
Frequency (Hz)
ability to take care of the model uncertainties, unknown disturbances, etc., which also implies that the proposed RIC has strong robustness to the uncertainties and disturbance.
7.5 Chapter Summary In this chapter, a robust impedance control scheme is proposed and investigated, dedicated to an USMdriven (PAbased) endeffector (surgical device). This control scheme tracks and regulates both motion and force trajectories to achieve desired position control and force control simultaneously. The SMC and DOB techniques are integrated into the impedance control scheme, which makes the control scheme to be capable of accommodating parametric uncertainties, nonlinearities, as well as unknown disturbances in the system. Theoretical analysis of the steadystate errors and stability are performed, which proves that the convergence of the robust impedance control scheme is guaranteed. Finally, an experimental study has been conducted and the experimental results show a promising result for both position control and force control. It also shows that a compromise between force control and position control can be achieved through the tuning of the impedance parameter k f . Furthermore, the comparison experimental results show that the proposed robust impedance control scheme can achieve better force tracking performance with different conditions than a commonly used impedance control scheme, PIDIC. This proposed control scheme can be extended to other similar systems where the position control and force control are required at the same time. In future, the optimization of the control parameters will be one research topic to be investigated. Besides that, to further improve the proposed control scheme, designing the con
7.5 Chapter Summary
177
troller using the higher order sliding mode control techniques [19] could be also one potential future work for this study.
References 1. K. Uchino, Piezoelectric Actuators and Ultrasonic Motors, vol. 1 (Springer, Science & Business Media, 1997) 2. D. Henderson. Mechanism comprised of ultrasonic lead screw motor. US Patent, US7309943B2 (2008) 3. C.H. Huang, J. Zou, A novel twoaxis micromechanical scanning transducer using waterimmersible electromagnetic actuators for handheld 3d ultrasound imaging. Sens. Actuators A: Phys. 236, 281–288 (2015) 4. C.J. Payne, G.Z. Yang, Handheld medical robots. Ann. Biomed. Eng. 42(8), 1594–1605 (2014) 5. S. Catarino, L.R. Silva, P. Mendes, J. Miranda, S. LancerosMendez, G. Minas, Piezoelectric actuators for acoustic mixing in microfluidic devicesnumerical prediction and experimental validation of heat and mass transport. Sens. Actuators B Chem. 205, 206–214 (2014) 6. J. Zhang, J. Tian, N. Ta, X. Huang, Z. Rao, Numerical evaluation of implantable hearing devices using a finite element model of human ear considering viscoelastic properties, in Proceedings of the Institution of Mechanical Engineers, Part H: Journal of Engineering in Medicine (2016), p. 0954411916652923 7. S. Yang, R.A. MacLachlan, C.N. Riviere, Manipulator design and operation of a sixdegreeoffreedom handheld tremorcanceling microsurgical instrument. IEEE/ASME Trans. Mechatron. 20(2), 761–772 (2015) 8. Q. Xu, Micromachines for Biological Micromanipulation (Springer, 2018) 9. C. Zhao, Ultrasonic Motors: Technologies and Applications (Springer, Science & Business Media, 2011) 10. G.Y. Gu, L.M. Zhu, C.Y. Su, H. Ding, Motion control of piezoelectric positioning stages: modeling, controller design, and experimental evaluation. IEEE/ASME Trans. Mechatron. 18(5), 1459–1471 (2013) 11. H. Ghafarirad, S.M. Rezaei, M. Zareinejad, A.A. Sarhan, Disturbance rejectionbased robust control for micropositioning of piezoelectric actuators. Comptes Rendus Mécanique 342(1), 32–45 (2014) 12. C.M. Wen, M.Y. Cheng, Development of a recurrent fuzzy CMAC with adjustable input space quantization and selftuning learning rate for control of a dualaxis piezoelectric actuated micromotion stage. IEEE Trans. Ind. Electron. 60(11), 5105–5115 (2013) 13. J. Minase, T.F. Lu, B. Cazzolato, S. Grainger, A review, supported by experimental results, of voltage, charge and capacitor insertion method for driving piezoelectric actuators. Precis. Eng. 34(4), 692–700 (2010) 14. K. Ohnishi, M. Shibata, T. Murakami, Motion control for advanced mechatronics. IEEE/ASME Trans. Mechatron. 1(1), 56–67 (1996) 15. E. Sariyildiz, K. Ohnishi, Stability and robustness of disturbanceobserverbased motion control systems. IEEE Trans. Ind. Electron. 62(1), 414–422 (2015) 16. K.S. Kim, K.H. Rew, S. Kim, Disturbance observer for estimating higher order disturbances in time series expansion. IEEE Trans. Autom. Control 55(8), 1905–1911 (2010) 17. K. Ohishi, Realization of fine motion control based on disturbance observer, in 10th IEEE International Workshop on Advanced Motion Control (AMC’08) (2008), pp. 1–8 18. K. Yamada, I. Murakami, Y. Ando, T. Hagiwara, Y. Imai, G.D. Zhi, M. Kobayashi, The parametrization of all disturbance observers for plants with input disturbance, in 4th IEEE Conference on Industrial Electronics and Applications (ICIEA 2009) (2009), pp. 41–46
178
7 Robust Impedance Control of Constrained Piezoelectric …
19. Q. Xu, Precision motion control of piezoelectric nanopositioning stage with chatteringfree adaptive sliding mode control. IEEE Trans. Autom. Sci. Eng. 14(1), 238–248 (2017) 20. Y. Shtessel, C. Edwards, L. Fridman, A. Levant, Sliding Mode Control and Observation (Springer, 2014) 21. L. Besnard, Y.B. Shtessel, B. Landrum, Quadrotor vehicle control via sliding mode controller driven by sliding mode disturbance observer. J. Frankl. Inst. 349(2), 658–684 (2012) 22. Y.S. Lu, Slidingmode disturbance observer with switchinggain adaptation and its application to optical disk drives. IEEE Trans. Ind. Electron. 56(9), 3743–3750 (2009) 23. X. Kun, C. Mou, Terminal sliding mode control with disturbance observer for autonomous mobile robots, in 2015 IEEE 34th Chinese Control Conference (CCC2015) (2015), pp. 765– 770 24. S. Sakaino, T. Sato, K. Ohnishi, Precise position/force hybrid control with modal mass decoupling and bilateral communication between different structures. IEEE Trans. Ind. Inf. 7(2), 266–276 (2011) 25. P. Gierlak, Hybrid position/force control in robotised machining. Solid State Phenom. 210, 192–199 (2014). Trans Tech Publ 26. Q. Xu, Precision position/force interaction control of a piezoelectric multimorph microgripper for microassembly. IEEE Trans. Autom. Sci. Eng. 10(3), 503–514 (2013) 27. Q. Xu, Robust impedance control of a compliant microgripper for highspeed position/force regulation. IEEE Trans. Ind. Electron. 62(2), 1201–1209 (2015) 28. S. Oh, H. Woo, K. Kong, Frequencyshaped impedance control for safe humanrobot interaction in reference tracking application. IEEE/ASME Trans. Mechatron. 19(6), 1907–1916 (2014) 29. Q. Xu, Adaptive discretetime sliding mode impedance control of a piezoelectric microgripper. IEEE Trans. Robot. 29(3), 663–673 (2013) 30. H.C. Liaw, B. Shirinzadeh, Robust generalised impedance control of piezoactuated flexurebased fourbar mechanisms for micro/nano manipulation. Sens. Actuators A Phys. 148(2), 443–453 (2008) 31. N. Hogan, Impedance control: an approach to manipulation: Part iiimplementation. J. Dyn. Syst. Meas. Control 107(1), 8–16 (1985) 32. T. Kröger, D. Kubus, F.M. Wahl, Force and acceleration sensor fusion for compliant manipulation control in 6 degrees of freedom. Adv. Robot. 21(14), 1603–1616 (2007) 33. K.K. Tan, W. Liang, L.P. Pham, S. Huang, C.W. Gan, H.Y. Lim, Design of a surgical device for officebased myringotomy and grommet insertion for patients with otitis media with effusion. J. Med. Devices 8(3), 031001–1–12 (2014) 34. J. Shan, L. Yang, Z. Li, Output feedback integral control for nanopositioning using piezoelectric actuators. Smart Mater. Struct. 24(4), 045001 (2015) 35. J.Y. Lau, W. Liang, K.K. Tan, Enhanced robust impedance control of a constrained piezoelectric actuatorbased surgical device. Sens. Actuators A Phys. 290(97–106) (2019) 36. S. Chiaverini, B. Siciliano, L. Villani, A survey of robot interaction control schemes with experimental comparison. IEEE/ASME Trans. Mechatron. 4(3), 273–285 (1999) 37. D.A. Lawrence, Impedance control stability properties in common implementations, in 1988 IEEE International Conference on Robotics and Automation (1988), pp. 1185–1190 38. J.C. Arevalo, E. Garcia, Impedance control for legged robots: an insight into the concepts involved. IEEE Trans. Syst. Man Cybern. Part C Appl. Rev. 42(6), 1400–1411 (2012) 39. H.S. Lee, Robust Digital Tracking Controllers for Highspeed/highaccuracy Positioning Systems (University of California, Berkeley, 1994) 40. H. Shim, Y.J. Joo, State space analysis of disturbance observer and a robust stability condition, in 2007 46th IEEE Conference on Decision and Control (2007), pp. 2193–2198 41. Y. Joo, G. Park, Reduced order typek disturbance observer based on a generalized Qfilter design scheme, in 2014 14th IEEE International Conference on Control, Automation and Systems (ICCAS2014) (2014), pp. 1211–1216 42. J.J.E. Slotine, W. Li, Applied Nonlinear Control (PrenticeHall Englewood Cliffs, NJ, 1991)
Chapter 8
Advanced Disturbance ObserverBased Failure Detection for Force Sensor
Disturbance observer (DOB) is one of the useful tools to estimate the contact force between the environment and the subject body in mechatronic and robotic systems as mentioned in Chap. 2. In Chap. 3, a novel automatic officebased ear surgical device for the treatment of otitis media with effusion (OME) under the guidance of force sensing information is introduced. To ensure the safety of such device, it is very important that the force sensing information must be reliable. Generally, a redundant sensing system and a failure detection system can help to improve the reliability and safety of a mechatronic and robotic system. To this end, a contact force estimation method based on DOB is proposed in this chapter especially for the aforementioned ear surgical device. The mathematic model and the position control scheme of the device’s actuation system are reviewed, and then the detailed design and the stability analysis of an advanced disturbance observer are given. Furthermore, a contact estimator and a failure detector, aiming to enhance the safety and reliability, are designed. Both numerical simulation and practical experiment are conducted to study the proposed control scheme.
8.1 Introduction One important application of the mechatronic systems to the medical devices is the semiautomatic/automatic surgical device, which aims to assist the surgeon to carry out the surgery more efficiently. However, in case a fault (even though it is a small fault) occurs in the device, it will potentially cause an injury to the patient. Hence, safety is the most important requirement to such kind of device. In this chapter, its concern is with the safety issue on the automatic officebased surgical device. With the mechatronic design in such surgical device, the sensing and control system have been developed and involved in the device, which makes © Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931_8
179
180
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor
the device to be of a higher precision and intelligence. Significantly, a force sensing system (using a force sensor) has been designed to measure the contact force between the tool set mounted on the device and the human tissues, e.g., tympanic membrane (TM). The contact force information will be provided to the surgeon and the device during the surgery. Moreover, this information is used to assist the control of the whole surgical procedure by the device (such as forcebased supervisory control). Therefore, the dependency on surgeon’s skills can be much reduced and the cost to access this treatment can be also reduced. Nevertheless, the safety and reliability is one of the key issues needed to be addressed in such device. In other words, if there is a fault occurring in the force sensing system during the surgery, this device may become malfunctional, possibly leading to an injury to the patient. Therefore, it is very important to make sure that the force sensing information is reliable or the force sensing system fault is detectable. In this case, a redundant force sensing system can be considered to improve the safety and reliability of the ear surgical device, i.e., another force sensing system can be integrated into the device. However, an additional force sensing system will increase the weight and size to the overall system which may affect its compact form and portability. To detect the force while using the same hardware setup, a disturbance observer (DOB) can be a good solution. In recent years, plenty of research works on utilizing the state observer and the disturbance observer in mechatronic and robotic systems can be found in [1–14]. In [2–4], different observers were designed for disturbance rejection in the control system. Furthermore, some research works on observerbased fault diagnosis and force sensing were proposed in [5–14]. In [5], a sliding mode observer was designed for current sensor fault diagnosis. In [6], the fault diagnosis for a motor system based on a disturbance observer was investigated. In [7], a fault detection system was designed for a process experimental system by using a combined observer using an unknown parameter identification mechanism combined with a disturbance observer. In [8], a robust adaptive fault diagnosis approach was proposed on the basis of disturbance observer. In [9], the fault detection and isolation technique in sensors applied to a concentricpipe heat exchanger was designed based on highgain observers. In [10], a force estimation method based on disturbance observer was proposed for force control of a robot manipulator without force sensor. In [11], a robust nonlinear observerbased approach for distributed fault detection was presented. In [12], researchers implemented another force sensing approach using a disturbance observer instead of the force sensor for force control. In [13], two neural networkbased contact force observers for robotic manipulators and haptic devices were developed. In [14], a Kalman filterbased disturbance observer using multisensor signals was designed for force sensing. As can be seen, the disturbance observer is not only useful in fault diagnosis but also efficient in force sensing. In the surgical device, the contact force can be considered as a kind of disturbance presenting in the motion control system and a welldesigned DOB is capable of observing and estimating this disturbance. The force information can be obtained by the DOB without the force sensor, which can be served as the redundant force information to the device. Thus, the safety and reliability can be increased and the fault of the force sensing system can be detected.
8.1 Introduction
181
In this chapter, a method using the disturbance observer is developed to carry out the force and contact estimation without force sensor and detect the sensor failure for the officebased surgical device. Based on this method, a redundant force sensing system can be built and the safety and reliability of the device can be enhanced while keeping the same hardware setup.
8.2 System Review and Problem Formulation Two examples of the medical devices that employ the force sensing system are shown in Figs. 8.1 and 8.2. In this chapter, the similar surgical device presented in Chap. 3 but with a different handle shown in Fig. 8.1 is used as the main case to investigate. Significantly, the force sensing system is designed to provide the contact force information between the tool set and the membrane during the surgery. For the force sensing system, a highly sensitive lowcost force sensor with a sensitivity of 0.12 mV/g is chosen to be integrated into the device. It provides precise force sensing performance in a compact commercialgrade package [15]. Furthermore, the force sensing information is very important to one operation step called “touch detection”. In this step, the ultrasonic motor (USM) stage will not stop unless the contact force reaches a designed threshold. It implies that the patient may be in danger if the force sensing system fails during this step. The tool set may penetrate the eardrum or even hurt the inner ear if such malfunction occurs, which may cause the hearing loss. It is necessary to avoid this situation for this ear surgical device as the safety is the most important consideration in a medical device. Therefore, a redundant sensing system is required in this device.
Force sensor
Tool set
USM
Fig. 8.1 Medical devices with force sensing system: ear surgical device
182
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor
Force sensor
Fig. 8.2 Medical devices with force sensing system: needle insertion device for epidural anesthesia
8.3 Controller and Observer Design In this study, an advanced DOBbased force estimation method is proposed to work as the redundant sensing system. The control scheme for the surgical device is shown in Fig. 8.3. It mainly consists of a position controller, a disturbance observer, a contact estimator, and a failure detector. To design both the controller and the observer, the mathematic model of the USM stage is required. In this section, the system model is built first. Following that, a position controller is presented. Then, the advanced disturbance observer for force estimation is proposed. Finally, a contact estimator and a failure detector based on the advanced disturbance observer are designed.
8.3.1 Modeling of Motor Stage The movement of the USM stage is consequent of the friction generated between the piezoceramic plate mounted in the stator and the friction bar attached to the mover [16]. Therefore, the model of the USM stage can be described in (8.1) which consists
8.3 Controller and Observer Design
183
Position Controller
Device
Contact Environment
Linear Encoder Disturbance Observer Threshold
Contact Estimator
Threshold
Contact Estimator
Force Sensor
Fig. 8.3 Control scheme
of two terms: a linear term and a nonlinear term mainly due to the friction [17]. m x(t) ¨ = Flinear (t) + Fnonlinear (t),
(8.1)
where x(t) is the position of the USM stage and m is the mass of the device’s moving part. The linear term behaves like a secondorder system which can be described similarly as ˙ + bu(t), (8.2) Flinear (t) = −kx(t) − c x(t) where k, c, b are the dominant parameters of the USM stage and u(t) is the input signal to the motor drive. In the nonlinear term, the Coulomb friction is the major nonlinear component. Moreover, the Coulomb friction in this system is not symmetric along the forward and backward directions. Therefore, the nonlinear term can be written as a nonsymmetric ˙ Hence, we have Coulomb friction f c (x). ˙ Fnonlinear (t) = − f c (x(t)) = −σ sign(x(t)) ˙ + ηsign(x(t)), ˙
(8.3)
where σ and η are coefficients, and sign(·) is the sign function. Thus, the full USM model is shown in (8.4). m x¨ = −kx − c x˙ + bu − σ sign(x) ˙ + ηsign(x). ˙
(8.4)
184
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor
Position Controller Optimal PID Controller USM
Contact Environment
Nonlinear Compensator Linear Encoder Fig. 8.4 Position controller
Furthermore, considering the contact force between the tool set and the TM as the disturbance d(t) applying to the system, the model can be rewritten as m x¨ = −kx − c x˙ + bu − σ sign(x) ˙ + ηsign(x) ˙ + d,
(8.5)
and the above model can be also represented by Eq. (8.6): −σ sign(x) ˙ + ηsign(x) ˙ +δ m = − G(x, x) ˙ + βu − f c (x)/m ˙ + δ,
x¨ = − λx − γ x˙ + βu +
(8.6)
where λ = k/m, γ = c/m, β = b/m, G(x, x) ˙ = λx(t) + γ x(t), ˙ and δ(t) = d(t)/m represents the disturbance term.
8.3.2 Position Controller To achieve highprecision and highspeed motions for the proposed surgical device, a precise and highperformance position feedback controller for the USM stage is designed and its block diagram is shown in Fig. 8.4. In the proposed position control system, a proportional–integral–derivative (PID) controller is applied as the main controller of the system to deal with the linear term while a nonlinear compensator including a nonsymmetric sign function is designed to compensate the nonlinear term mainly due to the friction. The PID controller parameters are derived optimally using an LQRassisted tuning approach (see Chap. 3) with the following index:
∞
J= 0
[E(t)T QE(t) + r u(tu)T u(t)]dtu,
(8.7)
8.3 Controller and Observer Design
185
t where E(t) = [ 0 e(τ )dτ, e(t), de(t) ]T represents the error states, e(t) = xd (t) − dt x(t) is the position error, xd (t) is the desired position, Q is the weighting matrix which is normally chosen as a diagonal matrix, i.e., Q = diag{q1 , q2 , q3 } and Q > 0, and r is the weighting factor. Hence, we can have the LQRassisted PID controller which is an optimal PID controller shown as t de(t) , (8.8) e(τ )dτ + K d u linear (t) = K p e(t) + K i dt 0 where K p , K i , K d are controller parameters calculated via infinite horizon LQR control strategy. The nonlinear compensator is designed as u nonlinear (t) =
σ sign(x) ˙ − ηsign(x) ˙ ˙ f c (x) = . mβ mβ
(8.9)
In summary, combine (8.8) and (8.9) together, the overall position controller is given by (8.10) u m (t) =u linear (t) + u nonlinear (t) t ˙ − ηsign(x) ˙ de(t) σ sign(x) + , e(τ )dτ + K d =K p e(t) + K i dt mβ 0
(8.10)
where u m (t) is the control input. Substituting the overall position controller (8.10) into the system (8.6), we have x¨ = −λx(t) − γ x(t) ˙ + βu linear (t) + δ(t).
(8.11)
˙ in this application Significantly, the disturbance term δ(t) and its changing rate δ(t) are bonded because the contact force is a finite value and it is slowly varying, i.e., ˙ < Δm , where δm and Δm are constants. δ(t) < δm and δ(t)
8.3.3 Advanced Disturbance Observer Generally, the acceleration information is required in a classical disturbance observer. However, the acceleration information x¨ is not available in this ear surgical device because no accelerometer is integrated in the device, and it is also difficult to obtain x¨ accurately from the position information by differentiation (second derivative) due to the measurement noise. Installing an accelerometer in the device can be a simple way to obtain the acceleration information, but it will increase the weight and size of the overall system which may affect the compact form and the portability.
186
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor
To overcome the disadvantages of the classical disturbance observer, an advanced disturbance observer referring to the nonlinear disturbance observer (NDOB) [18] which can have a good performance with a simple structure is designed. The design of the advanced disturbance observer is shown as follows. Firstly, an auxiliary variable z is defined as ⎧ ⎨ z = δˆ − p(y), z˙ = −l(y)z + l(y)[G(x, x) ˙ − βu linear − p(y)], ⎩ y = C x, ˙
(8.12)
where z ∈ R 2 is the auxiliary variable, p(y) is the function to be designed, l(y) is the observer gain function which is determined by l(y) =
∂ p(y) , ∂y
(8.13)
and C is a parameter which is selected to be 1 here (i.e., C = 1 and y = x). ˙ Choose the design function p(y) as p(y) = L y = L x, ˙
(8.14)
where L > 0 is a constant and the time derivative of p(y) is p(y) ˙ = L x. ¨ Therefore, the observer gain function becomes l(y) =
∂ (L y) = L , ∂y
(8.15)
where L becomes the observer gain to be designed. Hence, the auxiliary variable is able to be obtained from the following equation: ˙ z˙ = − Lz + L[G(x, x) ˙ − βu linear − p(x)] = − Lz + L[λx + γ x˙ − βu linear − p].
(8.16)
Following that, define the observer error as ˆ δ˜ = δ − δ,
(8.17)
and one of the observer goals is to minimize the observer error. Theorem 8.1 The estimation δˆ of the advanced disturbance observer approaches to the disturbance δ if the disturbance δ is constant (i.e., δ˙ = 0). ˜ is bounded within a certain value Theorem 8.2 The value of the observer error δ ˜δm determined by δ˜m = Δm /L if the disturbance δ is varying (i.e., δ˙ = 0). Proof of Theorem 8.1 and Theorem 8.2
8.3 Controller and Observer Design
187
Consider a Lyapunov function as V =
1 2 δ˜ . 2
(8.18)
Then, the time derivative of the Lyapunov function is given by ˙˜ V˙ = δ˜δ.
(8.19)
According to (8.12), it gives that −δˆ = −z − p, then − δ˙ˆ = −˙z − p. ˙
(8.20)
From (8.11), (8.12) to (8.16) and (8.20), the time derivative of δ˜ is given by δ˙˜ =δ˙ − δˆ˙ = δ˙ − z˙ − p˙ =δ˙ + Lz − L(λx + γ x˙ − βu linear − p) − L x¨ =δ˙ + Lz − L(δ − x) ¨ − L p − L x¨ =δ˙ + Lz − Lδ + L p =δ˙ − L(δ − z − p) ˆ = δ˙ − L δ. ˜ =δ˙ − L(δ − δ)
(8.21)
Substituting (8.21) into (8.19), the time derivative of the Lyapunov function becomes ˙ ˜ δ˙ − L δ) ˜ = −L δ˜2 + δ˜δ. (8.22) V˙ = δ( (i) Constant Case If the disturbance is constant, (8.22) becomes V˙ = −L δ˜2 ≤ 0.
(8.23)
Hence, the observer error δ˜ is bounded. From (8.21), it can be found that δ˜˙ = ˜ Therefore, the observer error can be represented by 0 − δ˙ˆ = −L δ. ˜ = Γ e−Lt , δ(t)
(8.24)
where Γ is a constant related to the initial condition. From the above equation, ˜ = 0, i.e., the observer error δ˜ can converge to zero and the it implies that lim δ t→∞
estimated disturbance δˆ can approach to the constant disturbance δ. In other words, the advanced disturbance observer is asymptotically stable under this condition. (ii) Varying Case If the disturbance is slowly varying, then (8.22) becomes
188
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor
Position Controller
Device
Environment
Linear Encoder
Advanced Disturbance Observer Fig. 8.5 Advanced disturbance observer
V˙ = − L δ˜2 + δ˜δ˙ ˙ = −Lδ ˜ 2 + δ ˜ δ ˙ ≤ − L δ˜2 + δ˜δ ˜ m ˜ 2 + δΔ ≤ − Lδ ˜ ˜ − Δm ). = − δ(L δ
(8.25)
As can be seen in (8.25), the time derivative of the Lyapunov function V˙ is negative ˜ − Δm ) > 0 is met. Therefore, it can be concluded that as long as the inequality (Lδ ˜ < 0, ∀δ ˜ > Δm /L . V˙ < −δ
(8.26)
Hence, the observer error δ˜ is uniformly bounded. In other words, all the solutions ˜ ≤ Δm /L} (i.e., V˙ < 0) will enter that start outside of the compact set BΔ = {δ this set in a finite time and remain inside it in the future for all time. For the force sensing application using the advanced disturbance observer, the error bound δ˜m can be considered as the sensing resolution. Furthermore, in order to achieve a small observer error such that δˆ ≈ δ, it can choose a large observer gain L for the advanced disturbance observer. Based on (8.12) to (8.16), the advanced disturbance observer is designed and shown in Fig. 8.5. As can be seen, both acceleration information are not required in the advanced disturbance observer.
8.3 Controller and Observer Design
189
Remark 8.1 The observer error approaches to zero if the disturbance changing rate ˙ = 0), where td < ∞. δ˙ goes to zero in a finite time td (i.e., lim δ t→td
In this case, considering that δ˙ is bounded and according to Theorem 8.1 and ˜ ˜ − Δm ) while t < td , and Theorem 8.2, it can be concluded that V˙ (t) = −δ(L δ 2 ˙ ˜ V (t) = −L δ ≤ 0 while t ≥ td . Therefore, the disturbance observer is asymptotically stable after t = td . This implies that the estimated disturbance δˆ can approach to the disturbance δ eventually (i.e., lim δˆ = δ). t→∞
8.3.4 Contact Estimator During the touch detection, it can be found that the contact force and the time derivative of the contact force are zero (i.e., δ = 0 and δ˙ = 0) before the tube touches the TM. According to Theorem 8.1, we can have V˙ = −L δ 2 0 and it implies that the disturbance observer is asymptotically stable before the contact. Therefore, it can be concluded that there exists a constant ε > 0 such that ˆ < ε. δ
(8.27)
Based on (8.27), a contact estimator can be designed as shown below: Sc =
ˆ 0 is the designed threshold and S f represents the working status of the force sensor, S f = 0 indicates that the sensor works normally while S f = 1 indicates that the sensor is malfunctional. ˆ is within a designed threshold, it indicates Significantly, if the difference (δ¯ − δ) that the force sensing system is operating appropriately. Conversely, if the difference is out of the threshold, it indicates that the force sensing system may be malfunctional and the surgical operation by using the device should be stopped immediately.
8.4 Numerical Study In this section, a simulation is carried out to verify and evaluate the effectiveness of the advanced disturbance observer.
8.4 Numerical Study
191
The parameters of the USM stage are obtained by the databased system identification method, which are shown as follows: λ = 248.4, γ = 202, β = 4940, σ/m = 11954.8, η/m = 568.1. Hence, the full model in (8.6) can be written as x(t) ¨ = −248.4x(t) − 202 x(t) ˙ + 4940u(t)−11954.8sign(˙x) + 568.1sign(x)+δ. ˙ (8.33) For the position controller, referring to Bryson’s rule [19], the weighting matrix is selected as Q=diag{1600, 100, 10−4 } and r = 0.0625. Then, the PID gain can be obtained by the calculation, which is K = [160, 40.5069, 0.0994], i.e., K p = 40.5069, K i = 160, K d = 0.0994. Moreover, the nonlinear compensator can be derived according to (8.9). For the advanced disturbance observer, three different observer gains L (10, 100, and 1000) are chosen in the simulation to study the effects of the observer gain. The simulation is implemented in MATLAB/Simulink. Specifically, the disturbance δs used in the simulation is assumed as a spring model by (8.34). The disturbance behaves like a spring when the tool set and the membrane are contacted (x > 0), and it is zero when the tool set and the membrane are not in contact (x ≤ 0). δs =
ks x , x > 0 , 0, x ≤0
(8.34)
where ks is a constant and it is chosen as ks = 0.5 in this simulation. The simulation results are shown in Fig. 8.6 and the differences between the advanced disturbance observer output and the simulated contact force are shown in Fig. 8.7. Significantly in Fig. 8.6, there is no contact before 0.5 s and then the contact force (includes constant and varying contacts) is applied to the membrane after 0.5 s. As can be seen, all the observers can estimate the disturbance exactly and the observer error is able to converge to zero while the disturbance is constant from 1.5 to 2.5 s. It verifies that Theorem 8.1 is correct. For the varying disturbance from 0.5 to 1.5 s and 2.5 to 4.0 s, all the observers are still able to estimate the disturbance. However, nonzero but bounded observer errors exist during these periods. It can be also observed that the observer error reduces as the observer gain increases. Therefore, a larger observer gain can result in a smaller observer error. Furthermore, the error becomes larger as the disturbance changing rate δ˙ increases. In summary, the observer error is bounded and the error bound relates to L and δ˙ for the varying disturbance, which implies that Theorem 8.2 is correct.
8.5 Experimental Verification In this section, the experimental system setup is built and then the experiments for verifying the proposed control scheme are given and discussed in detail.
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor Disturbance and Observer Output (N)
192
0.5 0.4 0.3 Disturbance L=10 L=100 L=1000
0.2 0.1 0 0
0.5
1
1.5
2
2.5
3
3.5
4
Time (s)
Zoomed Output (N)
(a) 0.5 0.48 0.46 Disturbance L=10 L=100 L=1000
0.44 0.42 1.45
1.46
1.47
1.48
1.49
1.5
1.51
1.52
1.53
1.54
1.55
Time (s)
(b)
Observer Error (N)
Fig. 8.6 Advanced disturbance observer output in simulation: a overall view; b zoomed view L=10
0.04 0.02 0 0.02 0
0.5
1
1.5
2
2.5
3
3.5
4
Observer Error (N)
Time (s)
L=100
0.04 0.02 0 0.02 0
0.5
1
1.5
2
2.5
3
3.5
4
Observer Error (N)
Time (s)
L=1000
0.04 0.02 0 0.02 0
0.5
1
1.5
2
Time (s)
Fig. 8.7 Observer error in simulation
2.5
3
3.5
4
8.5 Experimental Verification
193
Mock membrane
Ear surgical device
Fig. 8.8 Experimental system setup
8.5.1 Experimental System Setup Figure 8.8 shows the experimental system setup. It consists of the ear surgical device, a sensor amplifier, a motor drive, power supplies, and a computer with dSPACE DS1104 controller board. The control scheme, consisting of the position controller and the advanced disturbance observer, is implemented in the dSPACE control card with a sampling time of 1 ms. Furthermore, a mock membrane setup for simulating the TM is placed next to the tool set (see Fig. 8.8). Significantly, the mock membrane is made of polyethylene (PE) films which has similar characteristics to the human TM. In the experiment, the procedure of touch detection is carried out on the mock membrane setup. The contact force can be considered as a varying disturbance while the tool set is being pushed toward the membrane.
8.5.2 Experimental Results In this application, a slow ramp signal with the velocity of 1 mm/s is chosen as the motion sequences for touch detection. Figure 8.9 shows the position reference and the output of the USM stage during touch detection. It can be observed that the position control system offers precise motions for the device and achieves a high level of tracking performance. During the experiment, the tool set is driven by the USM stage to follow the motion sequences as shown in Fig. 8.9 toward the membrane. The USM stage will stop until the tool set is in contact with the membrane at a certain contact force.
194
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor
Position (mm)
2 1.8 1.6 1.4 Reference Actual position output
1.2 1 0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2
0.8
0.9
1
1.1
1.2
Time (s) 0
Error (mm)
0.002 0.004 0.006 0.008 0.01 0.2
0.3
0.4
0.5
0.6
0.7
Time (s)
Fig. 8.9 Position controller performance 0.25 0.2 0.15
Force (N)
0.1 0.05 0 0.05 0.1
Sensor output Observer output
0.15 0
0.2
0.4
0.6
0.8
1
1.2
Time (s)
Fig. 8.10 Advanced disturbance observer output during touch detection
The observer gain is chosen as L = 1000 in the experiment. The advanced disturbance observer output δˆ and the force sensor output δ¯ ≈ δ are shown in Fig. 8.10. As can be seen, the advanced disturbance observer can estimate the contact force precisely and Fig. 8.11 shows the observer error. The observer error is roughly bounded within ±0.05 N. Moreover, the average of the absolute observer error is 0.0142 N while the rootmeansquare (RMS) of observer error is 0.0182 N, which implies that the advanced disturbance observer is effective in the contact force estimation. Furthermore, it should be noted that the “spikes” in the figure are due to the sensor noise but not the observer output. Hence, the advanced disturbance observer
8.5 Experimental Verification
195
0.15
Observer Error (N)
0.1
0.05
0
0.05
0.1
0.15 0
0.2
0.4
0.6
0.8
1
1.2
Time (s)
Fig. 8.11 Observer error during touch detection 0.25 Sensor output Observer output
0.2 0.15
Force (N)
0.1 0.05 0 0.05 0.1 0.15 0
0.2
0.4
0.6
0.8
1
1.2
Time (s)
Fig. 8.12 Advanced observer and force sensor output
is capable of force estimation and fault diagnosis on the force sensing system, i.e., the disturbance observerbased force estimation is feasible. Remark 8.2 The redundant force sensing system can be built by using the advanced disturbance observer, and thus the safety and reliability of the surgical device can be enhanced without changing the hardware setup. Figure 8.12 shows the advanced disturbance observer output and the force sensor output with a sensor failure after 0.93 s caused by a broken connection on the signal
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor
Fig. 8.13 Analysis of the difference between observer and sensor outputs: a actual difference; b filtered difference; c output of decision mechanism
0.3 Oringinal
Difference (N)
196
0.2 0.1 0 0.1 0
0.2
0.4
0.6
0.8
1
0.8
1
1.2
Time (s)
(a) Difference (N)
0.3 0.2 0.1 0 Filtered
0.1 0
0.2
0.4
0.6
1.2
Time (s)
(b) Decision mechanism output
S
1
0
0
0.2
0.4
0.6
0.8
1
1.2
Time (s)
(c)
channel. As can be seen in Fig. 8.12, the tool set and the membrane are in contact after 0.5 s, which can be detected by both the observer and the sensor. However, the force sensor fails to detect the contact force due to the fault after 0.93 s. In the other hand, the observer is still able to estimate the contact force. The difference between the sensor and the observer is shown in Fig. 8.13a. Apply a median filter on their difference, it can be observed more clearly from the filtered output shown in Fig. 8.13b that the difference becomes large and it is out of the bound once the fault occurs in the sensor. Apply the decision mechanism presented in Sect. 8.3.5 to the filtered signal shown in Fig. 8.13b, its output is shown in Fig. 8.13c. It can be found clearly form Fig. 8.13c that the sensor fails at 0.933 s. Hence, it implies that the output of the decision mechanism is working properly and able to send out the correct failure information. Thus, the advanced disturbance observer is capable of realizing the fault diagnosis for the force sensor in the device. Remark 8.3 The fault diagnosis can be achieved by the comparison between the force sensor output and the estimated disturbance from the observer, and thus the system safety can be further enhanced.
8.6 Chapter Summary
197
8.6 Chapter Summary In this chapter, a disturbance observerbased force estimation method is employed and developed to enhance the safety and reliability for an ear surgical device while keeping the hardware setup unchanged. In order to avoid the disadvantages of the classical disturbance observer such as the need of acceleration information, an advanced disturbance observer referring to NDOB is designed. Furthermore, the system model of the USM stage is built and the modelbased control scheme for the force and contact estimation and the sensor failure detection is developed. The stability analysis on the advanced disturbance observer is given in detail as well. To verify the proposed control scheme, both numerical study and experiment are carried out. The experimental results show that the advanced disturbance observer offers a good estimation performance on the disturbance. Hence, the force sensing and fault diagnosis can be achieved by virtue of the advanced disturbance observer. Besides that, the advanced disturbance observer can be also used in contact detection, force sensorless force control as well as robust motion control.
References 1. S. Haddadin, A.D. Luca, A. AlbuSchäffer, Robot collisions: a survey on detection, isolation, and identification. IEEE Trans. Robot. 33(6), 1292–1312 (2017) 2. X. Shao, H. Wang, Active disturbance rejection based trajectory linearization control for hypersonic reentry vehicle with bounded uncertainties. ISA Trans. 54(1), 27–38 (2015) 3. X. Shao, H. Wang, Sliding mode based trajectory linearization control for hypersonic reentry vehicle via extended disturbance observer. ISA Trans. 53(6), 1771–1786 (2014) 4. X. Shao, H. Wang, Backstepping active disturbance rejection control design for integrated missile guidance and control system via reducedorder ESO. ISA Trans. 57(7), 10–22 (2015) 5. G. Huang, Y.P. Luo, C.F. Zhang, Y.S. Huang, K.H. Zhao, Current sensor fault diagnosis based on a sliding mode observer for PMSM driven systems. Sensors 15(5), 11027–11049 (2015) 6. J. Su, W.H. Chen, B. Li, Disturbance observer based fault diagnosis, in 2014 33rd Chinese Control Conference (CCC2014) (2014), pp. 3024–3029 7. M. Deng, T. Maekawa, N. Bu, A. Inoue, Actuator fault detection on a process control experimental system using combined observers. Proc. Inst. Mech. Eng. Part I: J. Syst. Control Eng. 225, 807–813 (2011) 8. S. Cao, L. Guo, Y. Yi, Robust adaptive fault diagnosis approach based on disturbance observer, in 2011 30th Chinese Control Conference (CCC2011) (2011), pp. 4340–4345 9. R.F. Escobar, C.M. AstorgaZaragoza, A.C. TellezAnguiano, ´ D. JuarezRomero, ´ J.A. ´ Hernandez, ´ G.V. GuerreroRamirez, Sensor fault detection and isolation via highgain observers: application to a doublepipe heat exchanger. ISA Trans. 50(3), 480–486 (2011) 10. K.S. Eom, I.H. Suh, W.K. Chung, S.R. Oh, Disturbance observer based force control of robot manipulator without force sensor, in 1998 IEEE International Conference on Robotics and Automation (1998), pp. 3012–3017 11. C. Keliris, M.M. Polycarpou, T. Parisini, A robust nonlinear observerbased approach for distributed fault detection of inputoutput interconnected systems. Automatica 53, 408–415 (2015) 12. S. Katsura, Y. Matsumoto, K. Ohnishi, Modeling of force sensing and validation of disturbance observer for force control. IEEE Trans. Ind. Electron. 54(1), 530–538 (2007)
198
8 Advanced Disturbance ObserverBased Failure Detection for Force Sensor
13. A.C. Smith, F. Mobasser, K. HashtrudiZaad, Neuralnetworkbased contact force observers for haptic applications. IEEE Trans. Robot. 22(6), 1163–1175 (2006) 14. T.T. Phuong, C. Mitsantisuk, K. Ohishi, M. Sazawa, FPGAbased wideband force sensing with kalmanfilterbased disturbance observer, in 36th Annual Conference on IEEE Industrial Electronics Society (IECON2010) (2010), pp. 1269–1274 15. H.I. Inc. Fss low profile force sensors, http://www.honeywell.com/ 16. P.I. (pi) GmbH and C. KG. M663 pline® linear motor stage, http://www.pi.ws 17. K.K. Tan, W. Liang, S. Huang, L.P. Pham, S. Chen, C.W. Gan, H.Y. Lim, Precision control of piezoelectric ultrasonic motor for myringotomy with tube insertion. J. Dyn. Syst. Meas. Control 137(6), 4–7 (2015) 18. W.H. Chen, D.J. Ballance, P.J. Gawthrop, A nonlinear disturbance observer for robotic manipulators. IEEE Trans. Ind. Electron. 47(4), 932–938 (2000) 19. G.F. Franklin, J.D. Powell, A. EmamiNaeini, Feedback Control of Dynamic Systems, 6th edn. (Prentice Hall Inc., Upper Saddle River, 2001), Chap. 7
Chapter 9
Conclusion
This book has focused on force and position control and its medical applications. This book contains the necessary aspects for the force and position control of mechatronic systems: force sensing, interaction modeling, force estimation, controller design, and applications. Furthermore, this book systematically covers all aspects both in theory and in practice from general concepts to novel ideas with the applications in medical technology. Besides that, this book provides an overview as well as a sequential view on the design and implementation of the force and position control system. In Chap. 1, the general concepts and technologies in the area of force and position control, including mechatronic systems, force sensors (strain gauge and load cell, force sensing resistor, piezoelectric force sensor, etc.), contact models (linear models and nonlinear Hunt–Crossley model), force and position controllers (positionbased approach, hybrid approach, parallel approach and impedance approach), are overviewed. In Chap. 2, disturbance observerbased (DOBbased) force estimation methods are introduced, where the force sensor is not needed. The reviews on disturbance observer, nonlinear disturbance observer, extended state observer, friction compensation, and gravity compensation are presented in detail. In Chap. 3, a forcebased supervisory control scheme is proposed for a novel ear surgical device. Two supervisory controllers for the overall procedure as well as the tube insertion with optimal penetrative path selection are presented, which help the surgical device to meet the design requirements and achieve high success rate. In Chap. 4, a patient motion stabilization system based on force and vision feedback is designed. With the help of the visionbased motion compensation scheme, the force control system achieves a high level of force tracking performance and the relative motion between a medical device and the patient can be maintained. In Chap. 5, an optimal and robust control scheme for stable contact force control on a soft membrane is proposed, which consists of a constrained linearquadratic (LQ) optimal proportional–integral–derivative (PID) force controller and a DOB© Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931_9
199
200
9 Conclusion
based motion compensator. The control scheme can be extended to the contact force control on soft environments (e.g., human tissues, elastomer materials). Furthermore, the proposed control scheme in Chap. 5 can be used to the application mentioned in Chap. 4, where the proposed control scheme in Chap. 5 does not need a camera system and offers much better performance against highfrequency motions comparing with the control system designed in Chap. 4. In Chap. 6, a selective force–position (force/position) control approach is proposed and employed for an application of fast tube insertion on human tissue. This new insertion method based on force–position control integrates the positionbased insertion method and the forcebased insertion method, which combines the advantages of both methods. As a result, the fast and robust tympanostomy tube insertion can be achieved. In Chap. 7, an enhanced robust impedance control scheme is developed for the constrained piezoelectric actuatorbased (PAbased) endeffector. Like a conventional impedance controller, this control scheme is able to realize both the desired position control and the force control simultaneously. Furthermore, the integration of the sliding mode control (SMC) and DOB techniques into the impedance controller makes the control system have good robustness. In Chap. 8, an advanced DOB on the basis of nonlinear disturbance observer (NDOB) is designed for the force sensor failure detection. Remarkably, the idea of DOBbased force estimation method is employed in the design. With such DOBbased failure detection method, the safety and reliability can be greatly enhanced without changing the hardware setup. In summary, the knowledge and ideas presented in this book provide effective ways on force and position control of mechatronic systems, which can be not only applied in medical applications but also extended to various applications where the mechatronic system is required to be in contact with the environment, such as soft grasping, humanrobot interaction, humanrobot collaboration, etc.
Index
A Adaptive control, 44 Algebraic Riccati equation (ARE), 141 Analogtodigital converter (ADC), 10
B Bode plot, 95
C Chattering, 170 Constrained LQ problem, 119 Contact effect, 114 Contact estimator, 189 Contact force control, 111
D Data processing, 146 DC motor, 15 Derivative kick, 93 Disturbance, 30 Disturbance observer (DOB), 30, 121, 167, 179 DOBbased force estimation, 182 DOBbased motion compensator, 112
E Endeffector, 160 Extended state observer (ESO), 35 modelbased ESO (MESO), 39
F Failure detection, 190 Feedback control, 3 Force and position control, 2 Forcebased insertion method, 143 Force control, 18 direct force control, 18 positionbased force control, 19 Force estimation, 29 Force–position insertion method, 144 Force sensing resistor (FSR), 11 Friction, 41 Friction compensation, 41 modelbased friction compensation, 42 modelfree friction compensation, 50 Friction models, 41, 43 classical model, 43 LuGre model, 43 timedelay model, 43 Fuzzy logic, 51
G Gauge Factor (GF), 5 Generalized impedance, 162 Genetic Algorithm, 47 Gradientbased optimization algorithm, 119 Gravity, 53 Gravity compensation, 53
H Hooke’s Law, 15 Human head motion, 89
© Springer Nature Switzerland AG 2021 T. H. Lee et al., Force and Position Control of Mechatronic Systems, Advances in Industrial Control, https://doi.org/10.1007/9783030526931
201
202 Hunt–Crossley (HC) model, 17 Hybrid force–position (force/position) control, 20
I Image processing, 98 Impedance control, 22, 162 Insertion force, 138 Instance correlator, 69 Instrumentation amplifier, 9 Interaction model, 113
K Kelvin–Boltzmann (KB) model, 17 Kelvin–Voigt (KV) model, 16
L Linearquadratic (LQ) optimization algorithm, 117 Linearquadratic regulator (LQR), 141 Load cell, 9 Lorentz Force, 15 Lowpass Butterworth filter, 32, 168
M Maxwell (MW) model, 16 Mechatronics, 3 Median filter, 68 Medical devices, 181 Microelectromechanical systems (MEMS), 15 Motor current, 15 Multilayer Perceptron (MLP), 74 Myringotomy, 62 Myringotomy with tube insertion, 89
N Neural networks, 51 Nonlinear disturbance observer (NDOB), 32, 186 Nonsymmetric Coulomb friction, 140, 183 Normalized rootmeansquare (NRMS), 116
O Officebased surgical device, 85 Optical force sensor, 13 Optimal PID controller, 111, 185 Otitis media with effusion (OME), 61, 133
Index P Parallel force–position (force/position) control, 21 PIDbased impedance control (PIDIC), 174 Piezoelectric actuator, 157 Piezoelectric effect, 13 Piezoelectric force sensor, 12 Piezoresistive effect, 10 Piezoresistive force sensor, 10 Positionbased insertion method, 142 Position control, 17 Proportional–integral–derivative (PID) controller, 18, 117, 140, 184 Proportional–integral–derivativefilter (PIDF) controller, 93, 120, 143
Q Quantum tunneling (QT), 11
R Redundant force sensing system, 29, 180 Robust impedance control, 165
S Scurve, 76 Secondorder system, 30 Sensor, 4 force sensor, 4 Sliding mode controller (SMC), 166 Soft environment, 111 Stabilization system, 92 Steadystate response, 163 Strain, 4 Strain gauge, 4 Supervisory controller, 69 Surgical devices/robots, 109 System identification, 93
T Tactile sensor, 14 Temperature compensation, 7 Tube insertion, 133
U Ultrasonic motor (USM), 66, 182 Uncertainties, 165
Index V Viscoelastic behavior, 113 Visionassisted force controller, 98 Visionbased motion compensator, 96 Voltage divider, 11
203 W Wheatstone Bridge, 5 full bridge, 7 half bridge, 7 quarter bridge, 7