Applied Science, Faculty of
Mechanical Engineering, Department of
DSpace
UBCV
Tufail, Muhammad
Doctor of Philosophy - PhD
University of British Columbia
Bilateral teleoperation allows a human operator to interact with a remote environment using the superior actuation and sensing skills of a robot and the unmatched cognitive skills of a human operator. It has shown promising results in applications such as telemedicine, telesurgery, and access to hazardous or remote environments. In all of these applications, the robot has to co-exist with humans and other delicate objects in the environment and therefore has to behave in a compliant (“soft”) manner. Moreover, in order to improve the task performance, the interaction force must be fed back to the operator to feel. In this backdrop, the present thesis focuses on the application of bilateral teleoperation in a homecare environment. In view of the underlying challenges involved with bilateral teleoperation, this dissertation focuses on the development of a complete teleoperation system that can effectively perform in real-time. A primary objective here is to use the impedance control approach to design local controllers for master and slave manipulators where the dynamic relationship between the applied forces and the resulting positions of the manipulators during interaction, is controlled. Impedance control requires the identification of the robot inverse dynamic model that can be computed in real-time and can adapt to changes in the actual dynamics of the robot. A complete data-driven learning-based technique called Locally Weighted Projection Regression (LWPR) is therefore used, which does not assume any a-priori knowledge of the inertial parameters of the robot. Performance of the system is improved by using online estimation of impedance of the unknown environment with which the slave manipulator interacts. A method of admittance control is designed. This method overcomes the shortcomings of the standard impedance control, as observed during experimentation. In the end, a method is developed to improve the transparency and position synchronization of the popular approach of wave-variables, which ensures stability under time delay that is induced by the communication channel during the exchange of information between the master and the slave ends. The effectiveness of the present developments is validated in an environment of homecare robotics, through simulation and experimentation, and the results are discussed.
Haptic Teleoperation with Impedance Control based on Learned Inverse Dynamics with Application in Homecare Robotics by Muhammad Tufail M.A.Sc., Asian Institute of Technology, Thailand, 2007 B.Sc., University of Engineering and Technology, Pakistan, 2004 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY in THE FACULTY OF GRADUATE AND POSTDOCTORAL STUDIES (Mechanical Engineering) THE UNIVERSITY OF BRITISH COLUMBIA (Vancouver) December 2015 © Muhammad Tufail, 2015ii Abstract Bilateral teleoperation allows a human operator to interact with a remote environment using the superior actuation and sensing skills of a robot and the unmatched cognitive skills of a human operator. It has shown promising results in applications such as telemedicine, telesurgery, and access to hazardous or remote environments. In all of these applications, the robot has to co-exist with humans and other delicate objects in the environment and therefore has to behave in a compliant (“soft”) manner. Moreover, in order to improve the task performance, the interaction force must be fed back to the operator to feel. In this backdrop, the present thesis focuses on the application of bilateral teleoperation in a homecare environment. In view of the underlying challenges involved with bilateral teleoperation, this dissertation focuses on the development of a complete teleoperation system that can effectively perform in real-time. A primary objective here is to use the impedance control approach to design local controllers for master and slave manipulators where the dynamic relationship between the applied forces and the resulting positions of the manipulators during interaction, is controlled. Impedance control requires the identification of the robot inverse dynamic model that can be computed in real-time and can adapt to changes in the actual dynamics of the robot. A complete data-driven learning-based technique called Locally Weighted Projection Regression (LWPR) is therefore used, which does not assume any a-priori knowledge of the inertial parameters of the robot. Performance of the system is improved by using online estimation of impedance of the unknown environment with which the slave manipulator interacts. A method of admittance control is designed. This method overcomes the shortcomings of the standard impedance control, as observed during experimentation. In the end, a method is developed to improve the transparency and position synchronization of the popular approach of wave-iii variables, which ensures stability under time delay that is induced by the communication channel during the exchange of information between the master and the slave ends. The effectiveness of the present developments is validated in an environment of homecare robotics, through simulation and experimentation, and the results are discussed. iv Preface The work presented in this thesis was conducted at the Industrial Automation Laboratory (IAL), Department of Mechanical Engineering, The University of British Columbia (UBC), Vancouver and was supervised by Prof. Clarence W. de Silva. The thesis topic, objectives, and the application were suggested by Prof. de Silva. My responsibility was to develop the system models and algorithms as well as experimentation with guidance and help from my supervisor, Prof. de Silva. Part of this work has been published as: 1. M. Tufail, C.W. de Silva, “Haptic Teleoperation using Impedance Control with Application in Homecare Robotics,” Control and Intelligent Systems, vol. 41, no. 3, 2013. 2. M. Tufail, C. W. de Silva, “Impedance Control Schemes for Bilateral Teleoperation”, in 10th IEEE International Conference on Computer Science & Education (ICCSE), Vancouver, Canada, August 22-24 2014. 3. M. Tufail, C.W. de Silva, “Teleoperation using Impedance Control for Homecare Robotics,” The IASTED International Conference on Engineering and Applied Science (EAS 2012), Colombo, Sri Lanka, December 27-29, 2012. In all the listed published work which is based on Chapter 3 of the thesis, I developed the control schemes and performed numerical simulation and experimentation. The use of impedance control in haptic teleoperation and their application in homecare were suggested by Prof. de Silva. He provided guidance and feedback in carrying out the activities. I wrote the manuscripts, which were edited and refined by Prof. de Silva. v Table of Contents Abstract .......................................................................................................................................... ii Preface ........................................................................................................................................... iv Table of Contents .......................................................................................................................... v List of Tables .............................................................................................................................. viii List of Figures ............................................................................................................................... ix List of Symbols ........................................................................................................................... xiv List of Abbreviations ................................................................................................................ xvii Acknowledgments .................................................................................................................... xviii Chapter 1: Introduction ............................................................................................................... 1 1.1 Motivation ................................................................................................................... 1 1.2 Problem Formulation .................................................................................................. 2 1.3 Goals of the Research ................................................................................................. 4 1.4 Related Work .............................................................................................................. 5 1.5 Contributions and Organization of the Thesis .......................................................... 16 Chapter 2: Derivation and Identification of Kinematic and Dynamic Models ..................... 18 2.1 Introduction ............................................................................................................... 18 2.2 Development of Experimental Setup ........................................................................ 18 2.3 Derivation of Kinematic Models .............................................................................. 23 2.4 Derivation of Dynamic Models ................................................................................ 40 2.5 Identification of Dynamic Model .............................................................................. 51 2.6 Results and Discussion ............................................................................................. 61 2.7 Summary ................................................................................................................... 69 vi Chapter 3: Interaction Control in Homecare Robotics ........................................................... 70 3.1 Introduction ............................................................................................................... 70 3.2 Standard Impedance Control..................................................................................... 71 3.3 First-order Impedance Control .................................................................................. 74 3.4 First-order Impedance Control based on Learned Inverse Dynamics ...................... 77 3.5 Admittance Control ................................................................................................... 78 3.6 Simulation Study: Interaction with Soft Environment .............................................. 81 3.7 Experiment 1: Control of PHANToM Interacting with a Hard Environment ......... 89 3.8 Experiment 2: First-order Impedance Control of WAM Arm .................................. 92 3.9 Estimation of the Environment Dynamic Model ...................................................... 96 3.10 Selection of Desired Impedance Model .................................................................. 103 3.11 Summary ................................................................................................................. 105 Chapter 4: Haptic Teleoperation with Time Delay ............................................................... 106 4.1 Haptics .................................................................................................................... 106 4.2 Haptic Teleoperation with Time Delay................................................................... 108 4.3 Experimental Results .............................................................................................. 127 4.4 Summary ................................................................................................................. 132 Chapter 5: Conclusion and Future Work ............................................................................... 133 5.1 Conclusions ............................................................................................................. 134 5.2 Contributions........................................................................................................... 137 5.3 Significance of the Work ........................................................................................ 138 5.4 Limitations and Suggested Future Work ................................................................ 139 REFERENCES .......................................................................................................................... 141 vii Appendix A ................................................................................................................................ 149 A.1 Specifications of the PHANToM Premium Device ................................................ 149 A.2 Homogeneous Transformations and Jacobian of the PHANToM Device .............. 149 Appendix B ................................................................................................................................ 152 B.1 Specifications of the WAM Arm ............................................................................ 152 B.2 Homogeneous Transformations and Jacobian of the WAM Arm .......................... 153 B.3 WAM Inertial Specifications and Code in the MATLAB Robotics Toolbox ........ 155 viii List of Tables Table 2.1 Specifications of external WAM PC. ........................................................................... 20 Table 2.2 Specifications of PHANToM PC .................................................................................. 21 Table 2.3 The D-H parameters of the WAM arm ......................................................................... 35 Table 2.4 The D-H parameters of the PHANToM haptic interface. ............................................. 36 Table 2.5 LWPR parameters for tuning. ....................................................................................... 58 Table 4.1 Description of the hybrid matrix, H(s)........................................................................ 109 ix List of Figures Figure 1.1 Bilateral teleoperation system. ...................................................................................... 2 Figure 1.2 Scattering transformation in a bilateral teleoperation setup. ....................................... 14 Figure 2.1 WAM arm by Barrett Technology. ............................................................................. 19 Figure 2.2 PHANToM haptic interface by Sensable Technologies. ............................................. 21 Figure 2.3 Experimental teleoperation testbed. ............................................................................ 23 Figure 2.4 Resolved-motion rate control (based on the Jacobian-inverse). .................................. 29 Figure 2.5 Resolved-motion rate control (based on the Jacobian-transpose). .............................. 30 Figure 2.6 Kinematic chain representation of the WAM arm. (a) rear view, (b) side view. ........ 35 Figure 2.7 Kinematic chain representation of the PHANToM haptic interface. (a) Frames assigned, (b) Joint angles. ............................................................................................................. 36 Figure 2.8 Workspace mapping for haptic teleoperation. ............................................................. 37 Figure 2.9 Master-slave teleoperation: x-Position tracking. ......................................................... 38 Figure 2.10 Master-slave teleoperation: y-Position tracking. ....................................................... 39 Figure 2.11 Master-slave teleoperation: z-Position tracking. ....................................................... 39 Figure 2.12 Master-slave teleoperation: tracking in 3D space. .................................................... 40 Figure 2.13 Robot dynamic models. (a) Direct, (b) Inverse. ........................................................ 41 Figure 2.14 Cartesian PD control of WAM arm with inverse dynamic compensation. ............... 45 Figure 2.15 Tracking of the desired Cartesian trajectory and interaction with the environment. 47 Figure 2.16 Animated WAM arm while tracking the given Cartesian trajectory. (a) WAM in initial configuration, (b) WAM frames of references, (from [51]). .............................................. 47 Figure 2.17 Tracking of the given Cartesian trajectory, x-direction. ............................................ 48 Figure 2.18 Tracking of the given Cartesian trajectory, y-direction. ............................................ 48 x Figure 2.19 Joint angles during tracking....................................................................................... 49 Figure 2.20 Manipulability index during tracking. ....................................................................... 49 Figure 2.21 Comparison between LWPR and ANN. (a) ANN, (b) LWPR, adapted from [60] ... 54 Figure 2.22 Estimating nonlinear function with local linear models. ........................................... 56 Figure 2.23 Example of nonlinear function approximation using LWPR .................................... 61 Figure 2.24 Taught Cartesian trajectory to the WAM arm (Experiments I & II). ........................ 62 Figure 2.25 Joint positions for learning the inverse dynamic model (Experiments I & II). ......... 62 Figure 2.26 Joint velocities for learning the inverse dynamic model (Experiment I). ................. 64 Figure 2.27 Joint accelerations for learning the inverse dynamic model (Experiment I). ............ 64 Figure 2.28 Joint torques for learning the inverse dynamic model (Experiment I). ..................... 65 Figure 2.29 Joint velocities for learning the inverse dynamic model (Experiment II). ................ 66 Figure 2.30 Joint accelerations for learning the inverse dynamic model (Experiment II). .......... 66 Figure 2.31 Joint torques for learning the inverse dynamic model (Experiment II). .................... 67 Figure 2.32 Error convergence in learning the inverse dynamic model (Experiment I). ............. 68 Figure 2.33 Evolution of number receptive fields in learning the inverse dynamic model (Experiment I). .............................................................................................................................. 68 Figure 2.34 Error convergence in learning the inverse dynamic model (Experiment II). ............ 69 Figure 3.1 Standard impedance control. ....................................................................................... 72 Figure 3.2 Impedance controlled manipulator – a spring-mass-damper system. ......................... 72 Figure 3.3 Standard impedance control. ....................................................................................... 74 Figure 3.4 First-order impedance control. .................................................................................... 75 Figure 3.5 Impedance-controlled manipulator (a spring-mass-damper system) interacting with an environment (a spring-damper system). ....................................................................................... 76 xi Figure 3.6 First-order impedance control based on learned inverse dynamics. ............................ 78 Figure 3.7 Admittance control. ..................................................................................................... 80 Figure 3.8 Experimental setup (haptic teleoperation with time delay). ........................................ 82 Figure 3.9 Position tracking (Standard impedance control). ........................................................ 83 Figure 3.10 Velocity tracking (Standard impedance control). ...................................................... 83 Figure 3.11 Interaction force (Standard impedance control). ....................................................... 84 Figure 3.12 Position tracking (First-order impedance control). ................................................... 84 Figure 3.13 Velocity tracking (First-order impedance control). ................................................... 85 Figure 3.14 Interaction force (First-order impedance control). .................................................... 85 Figure 3.15 Poor position tracking (Admittance control). ............................................................ 86 Figure 3.16 Velocity tracking in case of poor position tracking (Admittance control). ............... 87 Figure 3.17 Interaction force in case of poor position tracking (Admittance control). ................ 87 Figure 3.18 Position tracking (Admittance control). .................................................................... 88 Figure 3.19 Velocity tracking (Admittance control). .................................................................... 88 Figure 3.20 Interaction force (Admittance control). ..................................................................... 89 Figure 3.21 PHANToM devices as the master and slave manipulators with their corresponding controllers (PCs). .......................................................................................................................... 90 Figure 3.22 First-order impedance control (Interaction with hard environment). ........................ 91 Figure 3.23 Admittance control (Interaction with hard environment). ......................................... 92 Figure 3.24 Output of the inverse dynamic model (tracking based on LWPR model). ............... 94 Figure 3.25 Controller output (tracking based on LWPR model). Gains = 2000, = 20p dk b . ..... 94 Figure 3.26 Tracking of a desired Cartesian trajectory based on libbarrett’s inverse dynamics model............................................................................................................................................. 95 xii Figure 3.27 Output of the inverse dynamic model (tracking based on libbarrett’s inverse dynamics model). .......................................................................................................................... 95 Figure 3.28 Controller output (tracking based on libbarrett’s inverse dynamics model). Gains = 2000, = 20p dk b . ....................................................................................................................... 96 Figure 3.29 RLSR algorithm for environment model identification. ........................................... 97 Figure 3.30 Simulation setup for example simulation of online identification of the environment model........................................................................................................................................... 101 Figure 3.31 Input signal (example simulation of online identification of the environment model)..................................................................................................................................................... 101 Figure 3.32 Stiffness estimate (example simulation of online identification of the environment model) ......................................................................................................................................... 102 Figure 3.33 Damping estimate (example simulation of online identification of the environment model). ........................................................................................................................................ 102 Figure 3.34 Bode plot of actual and estimated systems (example simulation of online identification of the environment model). .................................................................................. 103 Figure 4.1 PHANToM Premium haptic interface and controller. .............................................. 107 Figure 4.2 Bilateral teleoperation system. .................................................................................. 108 Figure 4.3 Bilateral teleoperation system. .................................................................................. 109 Figure 4.4 Teleoperation system as an interconnection of 2-port network elements. ................ 112 Figure 4.5 Communication (a) before wave-variables transformation, (b) after transformation...................................................................................................................................................... 114 Figure 4.6 Communication channel with wave variables. .......................................................... 119 Figure 4.7 Wave-based teleoperation architecture. ..................................................................... 119 xiii Figure 4.8: Wave reflections in wave-based teleoperation system. ............................................ 122 Figure 4.9 Cancellation of wave reflections in wave-based teleoperation system. .................... 122 Figure 4.10 Modified wave-based teleoperation system. (Adapted from [42]). ......................... 124 Figure 4.11 Delay emulation in haptic teleoperation through WANem. .................................... 128 Figure 4.12 Position tracking under a time delay of 200ms. ...................................................... 129 Figure 4.13 Velocity tracking under a time delay of 200ms. ..................................................... 130 Figure 4.14 Force tracking under a time delay of 200ms. .......................................................... 130 Figure 4.15 Position tracking under a time delay of 400ms. ...................................................... 131 Figure 4.16 Velocity tracking under a time delay of 400ms. ...................................................... 131 Figure 4.17 Force tracking under a time delay of 400ms. .......................................................... 132 xiv List of Symbols a Link Length A Homogeneous Transformation Matrix b Wave Impedance eb Damping of the Environment Surface B Joint Space Friction Matrix DB Desired Damping Matrix C Joint Space Coriolis/Centrifugal Matrix xC Cartesian Space Coriolis/Centrifugal Matrix d Link Offset D Distance Metric of Gaussian Kernel dmf Desired Force for the Master Manipulator extF External Force G Joint Space Gravity Matrix xG Cartesian Space Gravity Matrix H Hybrid Matrix iI Inertia Tensor of Link i J Manipulator Jacobian Matrix iJ Jacobian Matrix of Link i J  Jacobian Pseudo-Inverse ek Stiffness of the Environment Surface K Joint Stiffness Matrix pK Proportional Gain in Cartesian PD control DK Desired Stiffness Matrix M Joint Space Inertia Matrix DM Desired Inertia Matrix xv xM Cartesian Space Inertia Matrix 1iio  Origin of Frame i-1 Expressed in Frame i P Error Covariance Matrix q Joint Position R Rotation Matrix S Scattering Operator 1iiT  Composite Transformation Matrix Representing Frame i-1 in Frame i  Joint Torque u Forward Wave Variable v Backward Wave Variable w Weight of Gaussian Kernel x Cartesian x-Position dx Desired Cartesian x-Position ex Environment Location (x-Position) dsx Desired Velocity for the Slave Manipulator x Cartesian x-Velocity X Cartesian Position Vector aX Adjusted Position Vector Y Regressor Matrix y Prediction of output y toZ Impedance Presented to the Operator  Twist Angle  Joint variable  Cartesian Angular Velocity i Dynamic Coefficients Vector of Link i  Gaussian Noise k Centre of Gaussian Kernel  Regressor Coefficients xvi  Regressor Matrix (in Parameter Estimation)  Forgetting Factor xvii List of Abbreviations ANN Artificial Neural Networks API Application Programming Interface ARMAX Autoregressive Moving Average eXogeneous BCKDF British Columbia Knowledge Development Fund CFI Canada Foundation for Innovation CoM Center of Mass DOF Degrees of Freedom EoM Equation of Motion FoGS The Faculty of Graduate Studies GPR Gaussian Process Regression HEC Higher Education Commission IAL Industrial Automation Laboratory ICT Information and Communications Technology LWPR Locally Weighted Projection Regression MSN Master-Slave-Network nMSE Normalized Mean Squared Error NSERC Natural Sciences and Engineering Research Council PLSR Partial Least Squares Regression RF Receptive Field RLSR Recursive Least Squares Regression ROS Robot Operating System SVR Support Vector Regression TCP Tool Centre Point TDPC Time-Domain Passivity Control UBC University of British Columbia UDP User Datagram Protocol WAM Whole Arm Manipulator xviii Acknowledgments I wish to acknowledge and thank my supervisor, Prof. Clarence W. de Silva for supervising my thesis research. I feel honored to have remained under the supervision and in company of such a learned, knowledgeable, kind, and supportive personality. Throughout the course of my Ph.D. Research, I have learned extensively from him and by reading his well-written and comprehensive textbooks, research publications, talks, discussions at group meeting, and even his inspirational speeches that he has made on various occasions. His presence in my life has always encouraged me to work harder as I feel there is always someone who can steer me through difficult situations, if they arise. I thank the members of my research committee, Dr. Mu Chiao, Dr. Mohammad Gadala, and Dr. Ryozo Nagamune, whose thoughtful feedback has led to improvement of my thesis. The research reported in this thesis has been supported by research grants to Prof. Clarence W. de Silva from the Natural Sciences and Engineering Research Council (NSERC), the Canada Foundation for Innovation (CFI), British Columbia Knowledge Development Fund (BCKDF), the Tier 1 Canada Research Chair in Mechatronics and Industrial Automation, and the scholarship to me from Higher Education Commission (HEC) of the Government of Pakistan through my home university, University of Engineering & Technology, Peshawar. I wholeheartedly thank them for their support. Special thanks to my colleagues at the Industrial Automation Laboratory (IAL), Dr. Mohammad Tahir Khan, Dr. Rolang Lang, Dr. Edward (Yanjun) Wang, Mr. Yunfei Zhang and all others. It was due to their continuous support and friendship that I have enjoyed my stay at UBC. Special thanks to Dr. Khan for introducing me to Dr. de Silva. This was the best gift a senior could ever give to his colleague. xix A very special thanks to my parents for their love and encouragement, especially my mom who has always pushed me to work hard. Finally, and most importantly, I would like to express my love and appreciation to my wife, Summiya, and son, Hayyan, who have been very supportive and patient throughout. I know at times it became unbearable for them to accept my busy schedule. Without your support this thesis would not have been possible. Thank you! 1 Chapter 1: Introduction 1.1 Motivation The rapidly increasing elderly population in our society is a challenging trend which is expected to accelerate in the coming years. Between 2006 and 2011 the population of Canadian seniors aged 65 saw a record high increase of 14.1% and reached nearly 5 million [1]. As of July 1, 2015, seniors accounted for 16.1% of the national population of Canada [2]. For the first time in the history of Canadian census, there were more persons aged 65 years and older than children aged 0 to 14 years. This number is projected to reach 20.1% by 2024, which means one in five Canadians will be aged 65 or over at that time. For the Canadian workforce, it will have several negative consequences. For example, it will be hard to provide adequate healthcare services to meet the growing demand for the care of the elderly. According to the Canadian Federation of Nurses Unions, a shortfall of 60,000 nurses is expected by 2022 [3]. Due to the recent attitudes towards service robots and the increasing value of the global market for technology products related to the care of older persons (anticipated to grow to $4 billion by 2015 [4]) there is a great need for developing innovative technologies that can provide homecare including healthcare to the elderly and the disabled. Reference [5] highlights the successful projects as well as the shortcomings of Information and Communications Technology (ICT) services for digital homecare. Our focus here is to develop haptic assisted, semi-autonomous robots in a home setting where they can carry out basic service and survival tasks such as serving food and medicine, cleaning, bathing, and providing assistance for mobility. The development of a teleoperation system with multimodal feedback (e.g., haptic, verbal and visual) of these service robots from a hospital control room will further improve the efficacy of the solution. Together, the two modes 2 of operations of the robot: autonomous and semi-autonomous with teleoperation, can enhance an older person’s ability to function as independently as possible in a natural (home) environment and call for emergency help as and when needed. One of the ways that can reduce the spending on healthcare of older people is to exploit the recent technological advancements in robotics and information and communication technologies (ICT) for providing high-quality supportive environments for older people in their homes. As a practical direction to the proposed research, a robotic homecare environment that will have autonomous robots can be augmented by the haptic teleoperation capability comprising a haptic-assisted remotely controlled robot to monitor and assist individuals within the home environment. The design challenges for such sophisticated application of robots include dealing with the instability due to communication constraints such as time delay and ensuring transparent manipulation of the remote environment through force feedback. A focus of this thesis is to formulate a bilateral control architecture that addresses these challenges. 1.2 Problem Formulation Figure 1.1 Bilateral teleoperation system. 3 In a bilateral teleoperation system, as shown in Figure 1.1, a human operator with the help of a master manipulator (a haptic interface) interacts remotely with an environment (a human in a homecare environment) through a slave manipulator that is usually located at distance. The human operator inputs motion (such as position and velocity) and force into the system, which is sent over a communication channel to the slave manipulator. The communication channel incurs time delay in any information that is sent through it. The slave manipulator tracks the motion received from the master and interacts with the environment accordingly. The slave manipulator also measures or estimates the interaction force between its end effector and the environment. It then sends the force estimate back to the master side where the haptic interface allows the human operator to feel the interaction force and adjusts his hand motion accordingly. This also closes the control loop at the master side. The feedback of force as well as any other information such as visual, tactile etc. is intended to improve the system performance and enhance transparency. In haptic teleoperation the emphasis is more on the force feedback as the term haptic refers to the sense of touch. A teleoperation system is called transparent if the human operator receives an authentic feel of the remote interaction. In particular, under transparent conditions, the operator will not feel any unwanted dynamics added to the interaction force. The teleoperator, a subsystem that comprises master/slave manipulators and the communication channel, must not contribute to modifying the feel of the interaction force negatively and therefore deteriorating the transparency. This means the teleoperator must act like a massless and infinitely stiff connection between the human operator and the environment. In addition to the proper force feedback (transparency) the overall system must be stable. Stability and transparency together define key characteristics of system performance. The control design stage 4 must ensure improved performance of the teleoperator. This is usually not easy to achieve as ensuring enhanced transparency comes at the cost of degraded stability and vice versa; the two are conflicting goals. A perfectly stable teleoperation system may still not be ideal due to compromised transparency. Some technical factors that may adversely affect the system performance include:  Presence of the human operator in the closed-loop system  Kinematics and dynamics of the master and slave manipulators  Computational and communication time delays and other communication constraints such as noise and packet loss/drop-out  Strict loop-rate requirement of the controllers  Unavailability of adequate sensor types due to cost constraints  Strict safety regulations such as those in homecare and medical robotics Particularly because of these challenges haptic teleoperation has been an active area of research. 1.3 Goals of the Research The main objectives of this thesis are as follows:  To develop a bilateral haptic teleoperation scheme for a homecare environment that is safe and reliable.  To design an advanced interaction controller such as impedance control that can achieve a desired level of compliant behavior in the manipulator.  To analyze performance of the haptic system with the developed teleoperation scheme subject to uncertainties in the robot dynamic model and other undesirable effects. 5  To evaluate and validate the performance of the developed system through computer simulation and laboratory experimentation with a prototype haptic teleoperation system. 1.4 Related Work 1.4.1 Real-time Online Model Learning in Robotics Model-based control such as computed-torque control and impedance control requires an accurate dynamic model of the robot that is amenable to computation within a fast control loop (usually running at 500Hz to 1000Hz). As an alternative to deriving equations of motion for the robot by traditional approaches such as the Lagrangian and Newton-Euler formulations, learning of these equations using sensory data has recently drawn the attention of researchers. The Lagrangian formulation is not only complex to derive but calculating the kinetic energy of a robot is computationally expensive and therefore cannot be used in real-time control. The Newton-Euler algorithm, due to its recursive nature, has facilitated computation of forward and inverse dynamics in real-time. Both these analytical models are parametric in nature and rely on the inertial parameters (such as link mass, center of mass, moments of inertia, and friction parameters) provided by the manufacturer, which are not usually accurate and may change over time due to wear and tear. In some situations they may be completely unavailable for an added extra part (such as a tool, sensor, or gripper) or even for the entire robot. Techniques based on machine learning, on the other hand, are non-parametric and data-driven and also fast, adaptive and efficient (if properly trained). Moreover, machine learning techniques can be used not only for learning the inverse dynamic model, but also to learn the forward kinematic model [6], inverse kinematic model [7] and task-dependent impedance models [8][9] for the robot. Another 6 reason which have made them popular in the last three decades is the advancement and cost-effectiveness of computer memory and processing hardware. Machine learning techniques can be categorized as supervised or unsupervised. In supervised learning, the model is trained on both the input and the correct desired outputs (targets). The model then performs generalization in which it has to produce, based on an inferred approximated function, reasonable outputs for the input data that is unseen and not encountered during the training. In robotics, supervised learning is mostly used for learning the inverse kinematic/dynamics model, which is then used in computed-torque control, impedance control, model predictive control, or operational space control of the robot. In learning kinematic or dynamic model of a robot, the function that is to be approximated is highly nonlinear and the input data is high-dimensional. The function approximation methods used and found in the literature can be classified as either global or local learning methods. In global methods, distributed weight representations are used by the learning algorithm, which attempts to fit the input-output data with a single global function. Every time a new input data are incorporated into model training and development, the weights are updated globally. In local methods, weighting functions (usually Gaussian) are local and therefore update of only a small subset of weights is required. Here, the given nonlinear function is fitted with simple local functions. Local methods are well-suited for real-time control in robotics. With the availability of modern software packages, which use incremental learning where local functions and weights are updated according to the incremental addition of new input data over time. Unlike traditional machine learning, the learning process does not assume the availability of sufficient training data. They also require less computer memory as all the raw data is not saved 7 for training. This is the reason why machine learning has received increasingly more attention of researchers in modern robotic control these days. Global learning frameworks include Gaussian Process Regression (GPR) [10] and Support Vector Machines (SVM) [11]. LWPR [12] and local GPR [13] are examples of local learning systems. ANN has been used as both global and local function approximation method. In [14], [15] and [16], the authors have presented a survey of model learning for robot control. Salaun et al. in [17] used incremental model learning method to learn the forward model of a redundant robot. The intended use was to achieve motion tracking by the manipulator in task-space which had been overlooked by the researchers earlier. Unlike model learning in joint-space, learning in task-space is harder due to the fact that the mapping between two spaces (joint- and task-spaces) is ill-posed, especially for redundant robots in which infinite number of inverse mappings exist. The LWPR algorithm was used for model learning. The proposed method was tested on a simulated 3-DOF planar robot. Nguyen-Tuong and Peters 2011 [14] have categorized the ways a learned model is used by a learning control framework into three categories: direct modeling, indirect modeling, and distal teacher approach. Each category uses a different approach to learn and update the model while they are in use. The distal learning approach is particularly useful for ill-posed mapping problem where the input-output relationship is ill-posed. In this approach, a direct dynamic model is used to guide learning of the inverse model. The direct dynamic model may be computed by using the known or partially known dynamic parameters provided by the manufacturer. Sun de la Cruz et al. in [18], [19], have used LWPR to learn the manipulator inverse dynamics. The authors have proposed a learning algorithm that can incorporate prior knowledge 8 about the model being learned. An approximation of the prior information is used to initialize the local linear models of the LWPR framework. The proposed approach was evaluated on a simulated 6-DOF PUMA 560 robot. The same authors, in [20], have incorporated a-priori knowledge of the system model in the learning algorithm. More specifically, the authors have used the first order approximation of the available dynamic model to initialize the learning algorithm LWPR (explained next). The proposed approach allows the system to generalize outside of the regions of training and also operate even if not enough measurements are available for initial training. The validity of the proposed approach has been shown experimentally on a 6-DOF arm where the tracking performance has significantly been approved. Nguyen-Tuong and Peters in [21] proposed a model parameterization approach for the local model to achieve operational space control of redundant robots based on the online learning model. The proposed approach was evaluated on 3-DOF and 7-DOF versions of a simulated WAM arm. The same authors along with other colleagues in [22] have used GPR, SVR, and LWPR methods to learn inverse dynamics of both simulated and real versions of the SARCOS arm to compare performance of the three algorithms. It was found that the LWPR algorithm outperformed the other two in terms of online execution time, is relatively faster, and therefore best for time critical control of the robot. On the other hand GPR and SVR were found to be more accurate. The latest work in learning inverse dynamics came from the authors in [9] where the learning process also incorporates contact with the environment. The focus is on the whole-body control where the contacts may occur anywhere along the robot body. The authors have proposed a data-driven mixture-of-experts learning approach using Gaussian processes. The learned model predicts joint torques generated through contact with the arm body from the raw data obtained 9 with tactile and force/torque sensors. Evaluation of the proposed method is done on the iCub humanoid robot, a popular open source cognitive robotic platform. 1.4.2 Interaction Control In many robotic applications in the industry and healthcare the robot end-effector comes in contact with the environment. There is an exchange of forces (action/reaction) between the robot and the environment. During interaction, control of both motion and force (directly or indirectly) becomes crucial. Interaction control algorithms can be classified depending on whether the control law uses the relation between position and force (stiffness control), between velocity and force (impedance/admittance control), or is directly based on explicit position and/or force feedback [23]. Volpe and Khosla in [24] experimentally evaluated both force and impedance control strategies. The paper focused on the importance of considering the implementation details that are required for the force and impedance control of robotic manipulators. Ueberle and Buss in [25] analyzed and evaluated using a set of hardware experiments the performance of haptic control architectures including the standard impedance and admittance control schemes. In [59] the authors have experimentally evaluated position-, force-, and impedance control for their applicability in minimally invasive surgical applications. Stanzyk et al. in [26] has developed an experimental teleoperation system with an admittance controller on the master side and a position-based impedance controller on the slave side. They showed the effectiveness of the implemented control schemes in a series of experiments such as tracking of free space in motion, haptic exploration of different materials, and driving a screw with an aluminum tool. The choice of appropriate control scheme (admittance control on master side and impedance control on the 10 slave side) helps them successfully achieve the requirements such as accurate position/force tracking, singularity-robust kinematic transformations, and stable environment interaction. Recently, Kikuuwe in 2014 [27] has enhanced the inner motion tracking loop in admittance control by proposing a new position controller which behaves as a proportional-integral-derivative (PID) controller with acceleration feedforward in normal situations and as a sliding mode controller when the actuator force is saturated. The proposed controller, which is validated through experiments on a 6-DOF industrial manipulator, achieves better stability and smoothness and also quickly responds to changes in the applied force. 1.4.3 Impedance Control for Haptic Teleoperation Bilateral teleoperation allows a human operator to interact with a remote environment using the enhanced skills of actuation and sensing of a robot and the superior cognitive skills of a human. It has shown promising results in applications such as telemedicine, telesurgery, micro- and macro-telemanipulation, and remote access to hazardous and difficult environments. In most such applications, the human operator has to rely only on visual feedback for transparency in carrying out the task properly. It is believed that along with visual feedback, the introduction of force (haptic) feedback in the teleoperation system can result in significantly improved performance. In a bilateral teleoperation system, a human operator with the help of a master manipulator (a haptic interface) interacts remotely with an environment (in the present application, a human in a homecare environment) through a slave manipulator (teleoperator). The interaction force (sensor information) between the slave manipulator and the environment is 11 reflected back to the human operator giving him the feeling of interaction in a transparent manner, resulting in improved performance [28]. The choice of the control scheme for teleoperation depends on such considerations as the nature of the application and the task that is being carried out. Different control techniques have been considered by researchers and are found in the literature. References [29] and [30] provide a comprehensive survey of the literature on bilateral teleoperation. In a position-position control scheme for teleoperation, a bilateral proportional-derivative (PD) controller connects the master and slave manipulators via a virtual spring and damper between the two end effectors. The work reported in [31] uses a local contact force controller to control the slave manipulator during contact. The scheme is a modification of the well-known virtual coupling scheme and uses a virtual spring to connect the end effectors of the master and slave robots. The desired contact force, which the slave controller is expected to track, is computed at the master side and is proportional to the error signal, which is the relative position difference between the master and the slave manipulators. A force representing this error is fed back to the master and felt at the haptic interface of the human operator. This approach is claimed to have enhanced stability because the contact force measurement is not used in the control loop to avoid the effect of associated noise. However, this enhancement comes at the expense of degraded transparency because the impedance felt by the human operator is now a function of the virtual (coupling) impedance and the environmental impedance. An alternative scheme of position-force control enhances the haptic feedback by explicitly measuring the contact force between the slave and the environment and sending it to the master manipulator, to be presented to the human operator. If the contact instability is 12 properly dealt with, it will provide enhanced transparency by hiding unwanted dynamics of the slave manipulator from the user and therefore resulting in more realistic haptic feedback. In [32], it is shown that the direct display of measured force to the human operator is of limited benefit. Consequently a four-channel architecture may be implemented where information on both motion and interaction force is exchanged between the master and the slave manipulators [33]. The objective is to match the impedance felt by the human operator with that of the remote environment. Therefore, instead of directly feeding the environmental force to the human operator, impedance control of master and slave manipulators may be used to ensure reflection of the desired impedance characteristics to the operator at the master manipulator. The concept of impedance control in robotics was first introduced in [34]. Instead of directly controlling the robot’s motion, the mechanical impedance (velocity/force ratio) was controlled such that a desired impedance model is enforced. In [35], impedance control is used in bilateral teleoperation. They have used sliding mode control on the slave side to enforce the desired impedance and achieve perfect motion tracking in the presence of time delay in the communication channel. Recently, in [36] the authors have used impedance control in their proposed four-channel force-reflecting teleoperation algorithm. In [37], a teleoperation architecture has been proposed and is termed position-based admittance control with force–force exchange. There the focus is on the robust stability analysis and the effect of variable human movement on task performance and the feeling of Telepresence. 1.4.4 Time Delayed Teleoperation Several methods exist in the literature that address the problem of instability due to the time delay in a teleoperation system. Often the goals are to: (1) stabilize the teleoperation 13 operation system under constant or variable time delay, (2) achieve or improve transparency of the teleoperation by faithfully displaying the interaction force and impedance to the human operator at the master side, and (3) achieve bilateral motion synchronization between the master and slave ends. The two popular methods that have been used to achieve these goals are: (1) scattering or wave variables, and (2) Time Domain Passivity Control (TDPC). Both are based on the passivity theory. Pioneering work that deals with bilateral teleoperation that includes time delay came from Anderson and Spong [38] and Niemeyer and Slotine [39]. They used passivity theory, which deals with the exchange of energy between interconnected systems. In order to make use of passivity theory for bilateral teleoperation with constant time delay, the teleoperation system, as shown in Figure 1.2, is treated as an interconnection of 1- or 2-port networks where energy at each port is exchanged in the form of an effort-flow pair of variables. For a mechanical system the effort-flow pair corresponds to the force-velocity pair, which for a robot manipulator may also be interpreted as the torque-angular velocity pair at each revolute joint. These effort ( ( )f t ) and flow ( ( )x t ) variables at each port are related by a scattering operator S as follows: ( ) = ( ( ) ( )) / 2 , ( ) = ( ( ) ( )) / 2( ) = ( ( ) ( )) / 2 , ( ) = ( ( ) ( )) / 2d dm m m m m md ds e s s e su t f t bx t b v t f t bx t bv t f t bx t b u t f t bx t b     (1.1)    =Tm s s mv u S v u (1.2) 14 Figure 1.2 Scattering transformation in a bilateral teleoperation setup. In order for an n -port network in the teleoperation system to be passive, the required bound condition on the norm of the scattering operator is given by ( ) = ( ( ) ( )) 1TS j S j S j    (1.3) Therefore, passivity becomes a condition on the system gain, which is unaffected by the time delay. If a human operator and the environment are represented by a 1-port network and the master/slave manipulators and the communication channel are represented by a 2-port network, then the aforementioned scattering formulation can be used to guarantee stability of the closed-loop teleoperation system by developing appropriate control laws for the master and slave manipulators. Chopra and Spong [40] and Lee and Spong [41] has extended and improved over this previous work. In particular, Chopra and Spong discuss time-varying delays and position synchronization. Passivity-based approaches assume perfect transmission of data between the master and the slave sides with no loss of information and errors in the communication channel. The stability conditions are also overly conservative in some cases. Passive controllers cannot hide the dynamics of the slave robot from the human operator and therefore transparency will be compromised. Scattering or wave-variables-based approach has been actively investigated and extended as can be found in the recent work in [42], [43], and [44]. In [42], the author has extended the 15 wave-variables scheme to the case where the slave manipulator transitions between two different environments. The work addresses the problem of tuning the wave impedance which is an important design parameter in the wave-based approach. They suggest that instead of having a constant wave impedance, its value should be varied smoothly between a certain interval. This will help adjust and improve the level of transparency of the teleoperation system. The wave impedance is kept small when the slave robot is in free motion, and increased when the slave robot is in contact with a hard environment. It was observed that faster variations in the value of the wave impedance resulted in adding unwanted oscillations to the transient response. In [43], the authors have proposed a new four-channel (4-CH) architecture to deal with the negative effect of wave-based reflections. They propose two modifications to the wave-based scheme to produce improved transparency and stability under large time delays. In their modifications, the outgoing wave variables on the master and slave ends are computed such that they no longer retain information from the incoming wave variable. Experimental results demonstrate the effectiveness of the proposed 4-CH system. In [44], the authors have investigated the effects of communication delays on the backdrivability of a teleoperation system. The wave-variable-based control techniques have been used. They suggest that position compensation, force compensation, or decoupling of the slave velocity can achieve better backdrivability in wave-based controllers and result in reduced reflections for unknown environments. The other popular approach for dealing with time delay in teleoperation is the TDPC which was originally presented in [45][46]. Currently, this is an active research topic with more recent articles appearing as in [47], [48], and [49]. Rebelo et al. in [48] has extended the TDPC approach to a 4-CH bilateral controller. This new architecture provides perfect transparency 16 unlike the previous position-force and position-position architectures. In [49], the authors address the problem of position synchronization in the TPDC approach. 1.5 Contributions and Organization of the Thesis Unlike traditional teleoperation, haptic teleoperation in the context of homecare involves some challenges that need to be carefully addressed. The master and slave manipulators must behave in as much compliant manner as possible. This should not be achieved at the expense of loosing motion accuracy. In this thesis the problem of interaction control is addressed. Following are the key contributions made in the thesis: 1. The inverse dynamic model of a four-degree-of-freedom (4-DOF) commercial robot manipulator is identified by a complete data-driven learning-based technique, which does not assume any a-priori knowledge of the dynamic parameters. For comparing the performance, the dynamic model is also analytically derived and simulated. The developed model can be used and updated online within the real-time control loop of the manipulator and therefore solves a major problem in the implementation of impedance control in haptic teleoperation. 2. Three interaction control techniques: stiffness control, standard impedance control, and admittance control, are designed and experimentally evaluated for their performance and applicability in homecare robotics. Shortcomings of stiffness control and impedance control are highlighted with the help of experimentation and are then improved by admittance control. Criteria for selecting the desired impedance/admittance model are proposed. 3. Position synchronization and transparency of the popular wave-variables based approach for time-delay compensation in haptic teleoperation is improved. The results of the 17 proposed approach are presented for an experimental 1-DOF test bed which demonstrate the effectiveness of the proposed improvement. 4. A complete experimental testbed for haptic teleoperation with special focus on safety and reliability is developed in the laboratory. This thesis is divided into the following five chapters:  Chapter 1 presents the motivation behind the thesis and a review of the relevant literature on the subject. This provides a context where this work fits in and also identifies the gaps in the literature that need to be closed.  In Chapter 2 a detailed dynamic model of the robot is derived and experimentally identified. A new learning-based technique is used, which has advantages over traditional approaches.  Chapter 3 presents interaction control algorithms based on the inverse dynamic model. It is found experimentally that the admittance control outperforms the rest of the techniques.  Chapter 4 develops and implements a haptic teleoperation architecture with time delay. The time delay in the communication channel, which poses a significant threat to the stability and the transparency of the teleoperation system, is addressed. The popular wave-variable-based approach is used and ways are presented in which position tracking and transparency can be improved further.  Chapter 5 provides a summary of the overall thesis. Possible directions for future work are presented as well. 18 Chapter 2: Derivation and Identification of Kinematic and Dynamic Models 2.1 Introduction This chapter first addresses the development of an experimental testbed for the present research. Next, kinematic and dynamic model are analytically derived. Some simulation studies are carried out. To come up with a robust and realistic inverse dynamic model of the robot, a data-driven learning-based technique is used. Procedures for identification of the model parameters are presented. 2.2 Development of Experimental Setup This section provides an overview of the hardware and software systems that are used for experimental validation of the algorithms developed in this work. Barrett WAM Arm The Whole Arm Manipulator (WAM) developed by Barrett Technology, shown in Figure 2.1, is a backdrivable (with backdrivability greater than 95%) serial-link manipulator with human-like kinematics. Each axis of the arm is driven by a brushless motor that has a digital motor controller, called Puck, directly mounted on it. Puck also has sensors for position, temperature, and current, and therefore plays a crucial role in monitoring the system health for safety purposes. The arm can be controlled by an internal or external computer connected through its CAN bus interface. The latter provides more memory and processing power which is required for advanced control algorithms as developed in this work. An external PC is therefore configured. Communication between the Pucks and control PC is achieved through a built-in CAN bus at the rate of 1Mbps. The control loop, which runs at 500Hz (but scalable up to 19 1000Hz) involves reading joint positions from Pucks, calculating new torque signal by the control algorithm based on the sensed information and presenting the result to the Puck which then applies it to the actuator. WAM is fundamentally a torque-controlled robot, but allows for direct position control (independent joint proportional-integral-derivative or PID control) too. Figure 2.1 WAM arm by Barrett Technology. Application development for the WAM arm is done in the open-source C++ library, called libbarrett [50], which is provided and supported by the manufacturer. The library communicates with the arm and lets the developer adjust the control loop sample time, log data in real-time, which can be used later for plotting and analysis purposes (in MATLAB/SIMULINK), and provides the control loop callbacks that can allow calculations based on WAM data in real-time [51]. In order to make the real-time control of the Barrett products (WAM arm) possible, the libbarrett library must be compiled on a customized PC with real-time Operating System. While Barrett provides an internal PC as part of the WAM arm, a customized PC with advanced hardware and software configuration has been built at the IAL, the details of which are presented in Table 2.1. 20 Table 2.1 Specifications of external WAM PC. PHANToM Haptic Interface The PHANToM haptic interface (shown in Figure 2.2) is a popular commercially available haptic system. The interface was originally developed by Sensable Technologies and is currently supported by 3D Systems Geomagic Solutions, SC, USA. The device, which actually is a 6DOF serial manipulator, is manipulated by holding a haptic stylus (also called gimbal). The stylus is actuated by three small motors in case of force only feedback and six motors in case of both force and torque feedback. Apart from its use in haptic teleoperation for application in homecare, hazardous material handling, mining, and so on the device has also been used in medical and flight simulators, video-gaming, mobile phones, and robot-assisted surgery. Haptic devices in general and PHANToM haptic device in particular, are designed as low-impedance hardware. Like other industrial robots, they are a kinematic chain of rigid links, but with insignificant dynamics which are often neglected in controller design. Friction is low due to the cable-driven mechanism. Gravity is compensated for by using the counterweights that are physically mounted on the mechanism. This is the reason why the device is known in haptic research community for open-loop impedance control. 21 Application development for the PHANToM device is done in the open-source C++ library, called CHAI 3D [52], which is a multiplatform haptic development framework for computer haptics, visualization, and interactive real-time simulation. The library supports many of the commercially available haptic devices. Therefore an application developed on one haptic device can be run on different hardware without any changes. The complexities of underlying device hardware are abstracted away from the application developer. CHAI 3D runs on PHANToM PC (see specifications in Table 2.2) that is used as a controller of the PHANToM device. Although application development is done in Microsoft Windows which is a non-real-time operating system, CHAI 3D strives to achieve a strict haptic rendering update rate of 1kHz. Figure 2.2 PHANToM haptic interface by Sensable Technologies. Table 2.2 Specifications of PHANToM PC 22 Complete Experimental Testbed A complete experimental setup is shown in Figure 2.3, which highlights the hardware/software systems and control loop rates. The communication between the master and slave ends is carried out in the form of User Datagram Protocol (UDP) packets, which is an ideal protocol for real-time communication due to low traffic-overhead involved as compared to other protocols in the Internet protocol suite. Figure 2.3 also highlights the control loop bandwidths. The high control loop rate on PHANToM PC (master side) is guaranteed by CHAI 3D only for the high priority thread which does the haptic force computation. Due to the non-real-time nature of the underlying system, variation in update rate is observed to be in the range 900Hz to 1kHz, which is acceptable. The haptic rendering rate puts a limit on the bandwidth of force displayed to the human operator. With the given 1kHz limit on the sampling rate, the CHAI 3D haptic loop can reconstruct and display forces having bandwidth less than or equal to 500Hz (following the Shannon sampling theorem). In the context of teleoperation, it means that an interaction force, simulated or received from the actual remote environment, can only be displayed with a reasonable amount of stability and smoothness if it lies below the aforementioned limit (500Hz). Extremely stiff objects, therefore, may cause instability of the haptic rendering algorithm. The slave side runs even slower (at a default rate of 500Hz). The reason is that the intensive computations such as computing the inverse dynamic model cannot be achieved at a rate higher than 500Hz. The effective bandwidth of the system is therefore 500Hz although some parts such as haptic system and force/torque sensor may have higher bandwidths. This works well in the context of haptic teleoperation in a home environment. 23 Kinematic and dynamic specifications of PHANToM and WAM as provided by the manufacturers are given in Appendix A and Appendix B, respectively. Figure 2.3 Experimental teleoperation testbed. 2.3 Derivation of Kinematic Models 2.3.1 Kinematic Modeling In forward kinematics a set of kinematic equations is obtained, which can be used to compute the position and the orientation of the end-effector for a given set of values of joint variables. The kinematic equations are obtained using homogeneous transformation matrices, which characterize the relative movements of the coordinate frames assigned to each link. As a first step towards determining the kinematic equations, coordinate frames are assigned to each 24 link using the Denavit-Hartenberg (D-H) convention. According to this convention, the frames are assigned such that the axis ix (i.e. the x-axis of the thi frame) is perpendicular to, and also intersects, the 1iz  axis (i.e. z-axis of the previous frame). Once the coordinate frames are assigned following the D-H convention, a set of four D-H parameters is obtained for each link, which are defined as follows:  Link length ia : Distance between 1iz  and iz . It is measured along ix and is always constant.  Link twist i : Angle between 1iz  and iz . It is measured around ix and is always constant.  Link offset id : Distance between 1io  and intersection of 1iz  and ix . It is measured along 1iz  is variable if the joint is prismatic.  Joint angle i : Angle between 1ix  and ix . It is measured around 1iz  and is variable if the joint is revolute. These parameters are summarized in a table, called D-H Table, where row i consists of the D-H parameters of link i . The number of rows n corresponds to the number of DOF of the manipulator (i.e. =1,2, ,i n ). Corresponding to each row of the D-H table, is a unique homogeneous transformation matrix Ai computed as ([53]), 25 , , , ,Rot Trans Trans Rot0 0 1 0 0 01 0 0 0 1 0 00 00 0 0 1 0 0 0 1 0 00 0 1 0 0 1 0 0 00 0 1 00 0 0 1 0 0 0 10 0 0 1 0 0 0 1i i i ii ii ii ii ii i i i i ii ii z z d x a xiiiAc s ac ss cd s cc s c s s a cs c                                                   00 0 0 1i i i ii iiic c s a ss c d            (2.1) where • ,Rot zi : counterclockwise rotation by i about the iz -axis. • ,Transz di: translation by id along the iz -axis. • ,Transx ai: translation by ia along the ix -axis. • ,Rot xi : counterclockwise rotation by i about the ix -axis. The transformation matrix iA represents the position and orientation of frame i i i io x y z with respect to the previous frame (i.e. 1 1 1 1i i i io x y z    ). The transformation matrix 0nT that relates the end-effector frame n n n no x y z to the base frame 0 0 0 0o x y z is given as,         0 001 1 2 2= =0 1n nn n nR q o qT A q A q A q     (2.2) 2.3.2 Differential Kinematics The forward or direct kinematics model that was developed in the previous section is used to convert the specified values of joint variables into the end-effector position and orientation. The differential kinematic model relates incremental joint movements to incremental 26 end-effector movements (both linear and angular). Hence it provides a relationship between the joint velocities and the end-effector linear and angular velocities. Essential to this modeling is finding the Jacobian matrix, 6 nJ  , which gives the instantaneous mapping between a vector of joint velocities nq and the end-effector linear and angular velocities 6( ) = [ ( ), ( ), ( ), ( ), ( ), ( )]Tx y z x y zx t v t v t v t t t t    . ( ) = ( ) ( )x t J q q t (2.3) An important use of the Jacobian is to find singular configurations of the robot. Singularities play an important role in the design and control of robot manipulators. At singular configuration or the singularity, the null space of the Jacobian, ( )TN J , has a non-zero dimension and the external forces have to be borne by the manipulator structure of manipulator. Moreover, bounded end-effector velocities (force and torques) may result in unbounded joint velocities (joint torques). Dynamic modeling and control of manipulators also make extensive use of the Jacobian matrix which will be explained later (in Section 2.4). In order to compute the manipulator Jacobian, it is partitioned into two sub-parts, ( )vJ q and ( )J q , as follows, ( )( ) =( )vJ qJ qJ q    (2.4) Here ( )vJ q corresponds to the linear velocities of the end-effector and relates them to joint velocities q while ( )J q corresponds to angular velocities of end-effector about an arbitrary 27 direction. The number of columns in the Jacobian matrix corresponds to the number of DOFs of the manipulator. Column i of ( )vJ q matrix is computed as, 0 ( )( ) = nviio qJ qq (2.5) where 0 ( )no q is the origin of the end-effector frame (and it moves as the configuration of the manipulator changes) and is extracted from Equation 2.2. Column i of ( )J q matrix is computed as ([53]), 01 1ˆ( ) = =i i i i iJ q z R k    (2.6) where =1i if joint i is revolute, and is 0 if it is prismatic. Matrix 01iR  represents the rotation of frame 1i  relative to the base frame. Joint i rotates about a unit vector kˆ about its z -axis (by the DH convention). A special case of the manipulator Jacobian is the link Jacobian ( )ciJ q ( =1, ,i n ) which relates the linear and angular velocities of the center of mass ic of the link to the angular velocities of all the joints that contribute to it. = ( )cicicivJ q q    (2.7) If ic represent the center of mass of link i relative to base coordinates, then 1 21 0 2 1( )0 0( ) = =0 0( )vii i iiii iiJ qc c cq q qJ qz z zJ q                        (2.8) 28 Inverse Differential Kinematics Control of robotic manipulators in Cartesian space involves inverse of the Jacobian matrix, 1J  . For this purpose, the inverse solution of Equation 2.3 is obtained which only exists if J is a square matrix ( n n ). In the case of under-actuated robots ( <n m ) ( m being the number of Cartesian DOFs) or redundant robots ( <m n ), 1 ( )m nJ q does not exist. In these cases pseudoinverse or generalized inverse of the Jacobian, ( )J q , is computed which always exists and is unique. For under-actuated robots having <n m it is can be computed as [53]: 1= ( )T Tm nJ J J J  (2.9) while, for kinematically redundant robots ( < , m nm n J R  ), it is computed as: 1= ( )T Tn mJ J JJ  (2.10) where 1( )TJJ  exists if ( ) =rank J m ; i.e. J is full (row) rank. If the full (row) rank requirement is not met, the pseudoinverse J  is computed numerically using the singular value decomposition (SVD) as: 1= TJ V U  (2.11) where  is a diagonal matrix of the singular values i of J i.e., =1= =nT Ti i iiJ U V u v  . The commanded joint-space trajectory ( )q t can now be calculated from the reference Cartesian trajectory x as: = ( )q J x I J J b   (2.12) where n nI R  is the identity matrix and nb R is an arbitrary vector. Equation 2.12 is a nonlinear differential equation whose solution can be obtained using least square techniques which aim to minimize 2( )q t subject to the constraint = ( )x J q q [54]. Equation 2.12 along 29 with the initial condition 1(0) = (0)q f x (where 1f  is the inverse kinematics function) is commonly referred to as resolved-motion rate control [54] and is depicted in Figure 2.4. Figure 2.4 Resolved-motion rate control (based on the Jacobian-inverse). The disadvantage of Cartesian control using the Jacobian-inverse method (Figure 2.4) is its non-robustness to joint-space singularities. Although in case of redundant robotic arms, the extra joints can be used to avoid singularities, the Jacobian-inverse-based rate control is still not suited for real-time robotic applications such as homecare. It is due to the fact that their numerical implementation is computationally intensive and therefore online and real-time computation (within control loops of speed of the order 500Hz) is impractical. Conventional industrial robots, when controlled in the Cartesian space to carry out tasks such as spray-painting, drilling, cutting, and welding have to compute the Jacobian-inverse off-line on powerful processors [55]. In order to overcome the above shortcoming of Jacobian-inverse-based motion control, modern robotic arms and haptic devices use the Jacobian-transpose-based control which is shown in Figure 2.5. Robotic arms such as WAM arm and PHANToM haptic interface that are 30 highly backdriveable, having inherent backdrivability > 95% , make use of the Jacobian-transpose for Cartesian control of forces (force control), motion trajectories (position control) and impedance control where the force-motion relationship of the arm is controlled. The Jacobian-transpose, ( )TJ q , also plays an important role in torque-control of robotic arms (presented in Chapter 3) and also in the transformation of external forces to joint torques. Figure 2.5 Resolved-motion rate control (based on the Jacobian-transpose). 2.3.3 Force/Torque Relationship As noted before, the manipulator’s Jacobian matrix can also be used to relate small changes in angular displacement, q , at the joints to small changes in position and orientation of the end-effector, X , as, = ( )X J q q  (2.13) where T 6= [ , , , , , ]X x y z     is a vector comprising of P , which are small changes in end-effector’s , ,x y and z positions, and  , which are small changes in end-effector’s orientation about , ,x y and z axes. If an external force = [ , , , , , ]Text x y z x y zF f f f n n n (a vector of static or 31 dynamic forces and moments) is applied at the end of the arm, which causes an infinitesimal displacement X then the net work done, W , at the end point will be, = = Text extW F X F X   (2.14) According to the principle of virtual work [54], there will be a corresponding net work done by the joints, = = TW q q    (2.15) From Equations 2.13, 2.14, and 2.15, the following relationship between external forces (and moments) and joint torques can be derived, = T extJ F (2.16) If 1 2= diag{ , , , }nK k k k    is a matrix of joints stiffness, then under static conditions the application of external force will be corrected by a restoring torque at the joints as, = K q  (2.17) Using equations 2.13, 2.16, and 2.17, the following expression can be derived, = ( )x extX G q F (2.18) where 1( ) = ( ) ( )TxG q J q K J q is the end-of-arm compliance and it measures the sensitivity of the arm to deflections. For a constant external force, increased compliance would mean more deflection of the end effector of the manipulator. The reciprocal of compliance is the end-of-arm stiffness, ( )xK q , and is given as,    1( ) = TxK q J q K J q  (2.19) The Cartesian stiffness, ( )xK q , is a positive-definite and symmetric matrix of size 6 6 and represents the apparent stiffness of a manipulator at its end-effector. The properties of positive-32 definiteness and symmetry are preserved as long as the Jacobian matrix, ( )J q , is non-singular (i.e., 1( )J q exists). At static singularities (i.e., when det( ) 0TJ  ), the apparent stiffness of the manipulator gets extremely large resulting in an unstable and unsafe behavior, which may ultimately damage the environment and/or the robot itself. One of the practical solution in the context of the current work may be to not let the robot operate in configurations where singularities may occur. Proper analysis of the robot workspace and kinematic behavior needs to be performed before deploying the robot in a homecare environment, which usually involves human robot interaction (such as care-giving to elderly). One such measure of kinematic optimization of a manipulator is manipulability, which is discussed next. 2.3.4 Manipulability Measure Manipulability of a robotic system is derived from a manipulator’s kinematic properties and was first defined by Yoshikawa [56]. It is the product of the singular values of the Jacobian matrix. Later, the concept was extended to include dynamic characteristics of the robot as well. Manipulability can help find out regions of the robot workspace that are safe and where the arm’s configurations are away from singularities. These singularities are particularly detrimental to haptic interfaces because the end-point of a haptic interface becomes extremely stiff and it poses resistance to motion by the human operator. Similarly, for the slave robot, a singularity would result in extremely large velocities of the end effector for finite joint velocities. Safety is therefore compromised if proper analysis of both the master and slave robot’s workspace is not carried out. The worst directions of the end effector motion in the workspace as pointed to by the singular values of the Jacobian must be avoided, and the master and slave 33 robots must be put in safe home positions. Manipulability therefore can be treated as a dexterity measure for teleoperation. Manipulability is best described and represented by a manipulability ellipsoid corresponding to a given pose (position and orientation). This ellipsoid describes the motion in Cartesian space as generated by unit joint velocities. Chiu in [57] used manipulability to describe the arm’s force transmission capabilities. A unit sphere in joints velocity nq R is defined as 2 2 2 21 2= , , 1nq q q q  (2.20) which using Equation 2.12 can be projected into Cartesian space as 1( ) 1T Tx JJ x  (2.21) This refers to an ellipse in the Cartesian space, called velocity ellipsoid, and it characterizes the achievable velocities in Cartesian space, mx R , subject to the constraint 2 1q  . As Jacobian is a function of joint angles q , it means there is one ellipsoid for each joint configuration q and its corresponding Cartesian position = ( )x f q . The lengths and the directions of the principal axes of this ellipsoid represent feasible velocities in the Cartesian space. Mathematically, the magnitudes are the square roots of the eigenvalues min max,  of the term TJJ in the ellipsoid equation (2.21). The axes of the ellipsoid are given by the eigenvectors min max,v v corresponding to the eigenvalues. Yoshikawa [56] considered the volume of this ellipsoid, given by ( ) det( )Tq JJ  , as a measure of manipulability and singularity avoidance and proposed that this measure can be maximized by exploiting the kinematic redundancy of a manipulator. 34 Similarly a force manipulability ellipsoid is an ellipsoid that is defined as the projection of a unit sphere in joint torques 2 1  . ( ) 1T TF JJ F  (2.22) The force ellipsoid represents the directions of feasible forces in the Cartesian space. The principal axes are now obtained from eigen-decomposition of 1( )TJJ  . This is in contrast to the eigen-decomposition of TJJ as in the case of the velocity ellipsoid. It is because of this reciprocity that the eigenvectors (directions of the ellipsoids) of both force and velocity ellipsoids are the same but their eigenvalues are the reciprocal of each other. The principal axes of one ellipsoid coincide with those of the other, but their lengths are in inverse proportions to each other. This is known as force/velocity duality [57]. This is in line with the conservation of energy principle, if the manipulator is viewed as a mechanical power transformer. 2.3.5 Kinematic Models of WAM Arm and Haptic Interface Figure 2.6 shows the kinematic chain of the Barrett WAM arm. The D-H parameters are given in Table 2.3. 35 Figure 2.6 Kinematic chain representation of the WAM arm. (a) rear view, (b) side view. Table 2.3 The D-H parameters of the WAM arm Figure 2.7 shows the kinematic chain representation of the PHANToM haptic interface. The D-H parameters are given in Table 2.4. 36 Figure 2.7 Kinematic chain representation of the PHANToM haptic interface. (a) Frames assigned, (b) Joint angles. Table 2.4 The D-H parameters of the PHANToM haptic interface. Detailed derivations of the homogeneous transformations and the Jacobian matrices for both manipulators are given in Appendices A and B. Workspace Mapping for Teleoperation The tool (end effector) Cartesian coordinate frame mTool yTool zToolx y z of the PHANToM interface moves relative to its fixed frame m m mx y z . Similarly the WAM tool moves with respect 37 to its own fixed frame 0 0 0x y z . In order to relate the motions of both tools, their motion must be relative to a common frame of reference. Choosing the WAM’s frame 0 0 0x y z as a reference (see Figure 2.8), the following homogeneous transformation relates the two motions: 0 0= sTool mToolm sTool mTool mT T T T (2.23) where sToolmToolT is a constant transformation, which is chosen according to the task. It maps the coordinates of the PHANToM tool to those of the WAM tool. One choice of sToolmToolT is: (2.24) which states that the , ,and x y z -axes of the WAM arm are mapped onto the , ,and y x z -axes of the PHANToM interface, respectively. Figure 2.8 Workspace mapping for haptic teleoperation. 38 2.3.6 Experimental Results Experimental results of the developed physical setup at IAL are shown in Figure 2.9 to Figure 2.12. These show perfect tracking by the slave of the trajectory as commanded by the master in all Cartesian directions i.e., x, y, and z. Keeping track of WAM’s manipulability during teleoperation helped avoid the singular configurations and resulted in safe and reliable teleoperation. This feature is even more helpful in later chapters when WAM will be impedance controlled unlike here, where it is position controlled. Figure 2.9 Master-slave teleoperation: x-Position tracking. 39 Figure 2.10 Master-slave teleoperation: y-Position tracking. Figure 2.11 Master-slave teleoperation: z-Position tracking. 40 Figure 2.12 Master-slave teleoperation: tracking in 3D space. 2.4 Derivation of Dynamic Models The Manipulator dynamic model provides a mathematical description of the manipulator dynamics in the form of differential equations. The model accounts for time-dependent changes in the state of the system. It comprises n coupled second-order nonlinear differential equations (one for each joint). These equations, also called Euler-Lagrange equations, are found from the total energy of the system and they describe the relationship between the internal or external forces that act on the arm (input torque, gravity, centrifugal, Coriolis, and friction) and the resulting motion (displacement, velocity, and acceleration). A dynamic model is useful in design, simulation, and control of the manipulator. The following sub-sections provide formulation and identification of the dynamic model which will be used for control design purposes later, in Chapter 3. 41 2.4.1 Lagrangian Dynamic Formulation In this section, the formulation of manipulator dynamics is developed using the energy-based (Euler-Lagrangian) approach. Two versions of the robot dynamic problem will be discussed: direct dynamic problem and inverse dynamic problem. They are shown in Figure 2.13. The direct dynamic model, using a given trajectory, , ,q q q , finds the required vector of joint torques,  . This model is used to simulate the manipulator dynamics in Matlab/Simulink for analysis purposes. The second version, inverse dynamic model, calculates, for a given torque vector  , the resulting motion of the manipulator, , ,q q q . This model is used in model-based control design in subsequent chapters. Figure 2.13 Robot dynamic models. (a) Direct, (b) Inverse. Let ( )q t be the set of n generalized coordinates (joint variables), then define the Lagrangian L as, ( , ) = ( , ) ( )L q q K q q P q (2.25) where K and P are the total kinetic and potential energies of the system. Using the calculus of variations it can be shown that [54], ( , ) ( , )= , 1kk kd L q q L q qk ndt q q     (2.26) 42 where k is the input torque acting on joint k . Equation 2.26 is also known as the Euler-Lagrange equation. The kinetic energy of an n -link manipulator can be computed as, 0=1( ) ( )( , ) =2 2T Tni i i i i iiv m v IK q q  (2.27) where im is the mass of link i at its Centre of Mass (CoM). iv is the linear velocity of the CoM, i is angular velocity about the CoM, and 0iI is the 3 3 inertia tensor, all expressed in base frame 0 0 0 0O X Y Z . Using the kinematic model of the manipulator, Equation 2.27 can be expanded into,              =11( , ) =21=2nT T TTi v v i i ii i i iiTK q q q m J q J q J q R q I R q J q qq M q q     (2.28) where iI represents inertia tensor in coordinate frame attached to the CoM of link i and is transformed into the base frame as 0 = ( ) ( )Ti i i iI R q I R q . viJ and iJ represent the link Jacobian (of link i ) as given by Equation 2.8. Matrix ( )M q in Equation 2.28 is the manipulator inertia matrix of size n n and is always symmetric and positive-definite. As can be seen, the inertia matrix is configuration-dependent. For a rigid-link manipulator, the potential energy P can be computed as, =1 =1( ) = =n nTi i cii iP q P m g r  (2.29) 43 where cir is the position of the CoM of link i and g is the gravity vector, both expressed in the inertial frame of reference. The substitution of Equations 2.28 and 2.29 into Equation 2.25 results in the following expression for the Lagrangian,       =1 =1( , ) = ( , ) ( )1=21=2Tn nij i jj iL q q K q q P qq M q q P qm q q q P q (2.30) where ( )ijm q refers to thij element of the inertia matrix ( )M q . The Euler-Lagrangian equation (Equation 2.27) now becomes, =1 =1 =11=2n n nkj ijkikj j i j kj j i i j k km mm Pm q q qq q q q               (2.31) By letting 12kj ijkiijki j km mmcq q q          (2.32) and =kkPgq (2.33) Equation 2.31 can be re-written as,      =1 =1 =1= , 1n n nkj j ijk i j k kj j im q q c q q q g q k n     (2.34) which is the dynamic Equation of Motion (EoM) of an n -link manipulator and is commonly written in a concise (vector) form as, 44      , =M q q C q q G q   (2.35) If the effects of friction are modeled as ( )B q and external forces/moments extF (Section 2.3.3) are also considered, then equation 2.35 can be re-written as,      , ( ) = ( )T extM q q C q q B q G q J q F    (2.36) here ( ) : the inertia matrix,found using the link Jacobian ( )( ) : the velocity cross coupling matrix, found using the inertia matrix( ) : the gravity loading vector, found using the link Jacobian ( )(ccM q J qC qG q J qB) : the friction vector that comprises both viscous and Colulomb frictions: the input torque vector from actuatorsq For simplicity, Equation 2.36 can be re-written as   ( , ) = ( )T extM q q n q q J q F  (2.37) where the Coriolis, centripetal, friction, and gravity torques are grouped as    ( , ) = , ( )n q q C q q B q G q  . Example Simulation: Cartesian PD Control of WAM Arm with Dynamic Compensation In this example simulation, a Cartesian PD controller of the 4-DOF WAM arm is designed with compensation for nonlinear dynamics in the joint-space. The purpose of this simulation is to evaluate the accuracy of the kinematic and dynamic models that have been derived. The dynamic parameters of the WAM arm as provided by the manufacturer are used (see Appendix B) to compute the model. The Robotic Toolbox in [58] is used to simulate the arm in MATLAB/SIMULINK (see Appendix B.3 for the code). Block diagram of the implemented controller is shown in Figure 2.14. The desired Cartesian trajectory is a circle of radius r in the 45 XY plane centered at point ( , )x yc c . If the circle needs to be traced out in time T , then the trajectory can be expressed as, 2 2( ) = [ cos , sin ,0] ,0Td x yt tx t c r c r t TT T     (2.39) The arm is initially at configuration = [-0.0048 -1.9912 -0.0112 3.1227]Tinitq (rad). It is not shown in the block diagram (Figure 2.14), but the arm interacts with a simulated environment that is modeled as a linear spring i.e., ( )e e eF k x x  where 10000ek  N/m. The environment is located in the same XY-plane where the desired circular trajectory is located. Nominal (uncompressed) position of environment is 0.6ex   m Contact is made as soon as the x Cartesian position of end effector gets smaller than the nominal position i.e., when ex x . Figure 2.14 Cartesian PD control of WAM arm with inverse dynamic compensation. The control law is computed as, = ( )( ) ( , )T p DJ q K e K e n q q   (2.40) with the controller gains selected as 1000pK  N/m and 100DK  Ns/m. Closed-loop dynamics (by substituting Equation (2.40) into (2.38)) are obtained as, 46 ( ) ( , ) = ( )( ) ( , )T Tp D extM q q n q q J q K e K e n q q J F    (2.41) ( ) = ( )( )T Tp D extM q q J q K e K e J F  (2.42) 1= ( ) ( )( )T p D extq M q J q K e K e F     (2.43) As can be seen from Equation 2.43, the impedance controlled arm operates with its natural inertia and the desired impedance. This will be explained in detail later in Chapter 3. Figures 2.15-2.20 show the resulting plots. As shown in Figure 2.15, 2.17, and 2.18, the given Cartesian trajectory (a circle in Cartesian space) is perfectly tracked. As the arm is not stiffer than the environment, it does not penetrate into the environment. Joints path is shown in Figure 2.19 with the corresponding manipulability index in Figure 2.20. From the manipulability index, it can be noticed that the distance to singularity is very small (less than 0.02) when the arm is making contact with the environment. It means the configuration of the arm was not optimized for interaction and therefore the manipulability is limited. The dynamic model simulated in this example does not include the dynamics due to friction. In reality, such a perfect tracking and ideal cancellation of the manipulator nonlinear dynamics is almost impossible to achieve due to unmodeled dynamics, errors in dynamic parameters, and variations in the load. A more sophisticated approach is therefore needed that can experimentally identify the dynamic model to overcome the aforementioned issues. This is a first step toward any model-based control, including impedance control and is covered in the next section (Section 2.5). 47 Figure 2.15 Tracking of the desired Cartesian trajectory and interaction with the environment. Figure 2.16 Animated WAM arm while tracking the given Cartesian trajectory. (a) WAM in initial configuration, (b) WAM frames of references, (from [51]). 48 Figure 2.17 Tracking of the given Cartesian trajectory, x-direction. 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-0.2-0.15-0.1-0.0500.050.10.15Time (sec)Cartesian position (m) yyd Figure 2.18 Tracking of the given Cartesian trajectory, y-direction. 49 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-2-101234timeJoint Angles [rad] q1q2q3q4 Figure 2.19 Joint angles during tracking. Figure 2.20 Manipulability index during tracking. 2.4.2 Operational Space Formulation Cartesian control techniques require the dynamic model (Equation 2.36) to be described in Cartesian space instead of the joint space. The operational or Cartesian space formulation of the dynamic model relates the acceleration of the end-effector to the forces and moments acting on the end-effector, all represented in the Cartesian or tool space. This also forms a basis for 50 impedance control of the manipulator, which is the subject of Chapter 3. The general form of the dynamic model in Cartesian space is given as [59], ( ) ( , ) ( ) =x x xM q X C q q G q F  (2.44) where = ( )TF J q  1( ) = ( ) ( ) ( )TxM q J q M q J q  (2.45) ( , ) = ( ) ( , ) ( ) ( , )Tx xC q q J q C q q M q J q q q  (2.46) ( ) = ( )Tx iG q J G q (2.47) Given that, T 6= [ , , , , , ]X x y z     is a vector of the position and orientation of the end-effector that is related to the manipulator’s joint angles T1 2= [ , ,..., ]nq q q q through the forward kinematic model as ( )X f q , the Cartesian acceleration, X , can be derived through differentiation as, ( )X f q (2.48) = ( )X J q q (2.49) = ( ) ( , )X J q q J q q q (2.50) It is this transformation of motion from joint coordinates to tool coordinates that has resulted in the operational-space formulation (Equation 2.44) of the dynamic model. 2.4.3 Linear Parameterization The dynamic model of a robot can be factored into a vector of dynamic parameters of the robot,  , and a matrix that consists of joint position, velocities, and accelerations, ( , , )Y q q q as 51 ( , , )Y q q q  . Written in this form, the dynamic model is linear in  . This form of formulation is usually required for experimental identification of the dynamic model. Defining the dynamic coefficients vector for the WAM arm as ( i being the joint number and 1, ,i n ), = , , , , , , , , ,Ti ixx ixy ixz iyy iyz izz i i ix i iy i izI I I I I I m m r m r m r    (2.51) The Lagrangian in Equation 2.30 is factored into i (a vector comprising of all constants) and ( , )y   as: =1= ( , )nTi iiL y q q  (2.52) The new dynamic equation for joint k now becomes: =1( , ) ( , )=nT Ti ii i ki k ky q q y q qddt q q     =1( , , ) =nTki i kiy q q q   ( , , )Y q q q   (2.53) Equation 2.53 is commonly used for model identification based on least-square techniques, which estimate the unknown parameters in  given the regressor, which consists of all measurements , ( , , )Y q q q . 2.5 Identification of Dynamic Model System identification aims at building a mathematical model of a dynamic system based on given measurements of the system inputs and outputs. It can be categorized as 1). Identification based on assumed model structure (as given by Equation 2.53) and experimental 52 data, 2). Identification completely based on experimental data assuming no prior knowledge of the system. In the first category, which can be referred to as classical identification methods, a rigid body model such as in Equation 2.53 is derived. The unknown parameters in  are then estimated a statistical technique using given measurements of the joint position, velocities, and accelerations corresponding to known input torques  . While this method has been traditionally used in robotics, it has some drawbacks. First, as the method does not have any learning capability associated with it, the quality of approximation remains poor if the parameters are poorly estimated or the unmodeled dynamics in the system are significant. The models obtained through this method cannot adapt to unknown situations e.g., if a robot picks up an unknown payload or interacts with an unknown environments. Second, this method becomes tedious and difficult to use for large and complex systems (e.g., a 30-DOF robot). The parameterization is achieved once all the dynamic terms have been derived analytically. Last, the rigid body models in robotics usually require intensive numerical computations and are therefore not well suited for real-time applications like in haptic teleoperation. The second category usually referred to as data-driven or intelligent system identification techniques are lately becoming popular due to the advances in the field of machine learning. These techniques can be run both online or offline, and are well-suited for real-time implementation due to the availability of sophisticated software libraries. It can be categorized into several types such as least-squares identification, Locally Weighted Projection Regression (LWPR), Gaussian-processes, artificial neural-networks, and so on. Sigaud et al. 2011 [16] and Nguyen-Tuong and Peters 2011 [14] present a comprehensive survey of model learning for robot control. 53 In robotic teleoperation applications, the problem of system identification is encountered at various stages and in various depths. Mostly these estimated models are highly nonlinear. In this thesis, two types of models are estimated: (1) model identification of the master and/or slave robots, (2) model estimation of the environment, which the master or the slave robot is interacting with. The challenging part here is building an accurate model and the ability to update it online. 2.5.1 Learning-based Identification In this sub-section a complete data-driven, machine learning approach for model identification is discussed, which is used later in this thesis for learning the inverse dynamic model of the robot. Statistical multiple regression methods have recently become popular in the field of robotics. Furthermore, the availability of open-source, highly efficient and fast software packages such as LWPR [6] which can be run online and in real-time, have drawn attention of the researchers to develop practical robotic applications where model-based control is needed. These methods and algorithms have proved to be highly effective in learning the forward and/or inverse kinematic and dynamic models of a robot that has a complex mechanical design and has to interact with a complex physical environment. Even for common industrial robots, statistical learning offers a clear advantage over analytical methods of solving the inverse dynamic problem, which is a challenging task for the control designer and are also computationally heavy even for advanced hardware. Modern haptic applications are strict about the control loop timing and do not allow rates lower than 500Hz. Real-time impedance control can particularly benefit from such techniques. 54 LWPR belongs to a more general category of locally weighted learning techniques that learn a variable (and optimized) set of weighted local linear models to estimate nonlinear models. They have the capability to handle high-dimensional input data spaces in which some of the dimensions may be irrelevant or even redundant. They also do not assume any model structure like in the case of parameter identification method described in the previous sub-section. In [60], the authors have compared the LWPR with ANN (more specifically the Radial Basis Function Network). They have found that while the structure of LWPR is similar to that of a basis function network, the flexibility that the weights, widths, and the number of receptive fields (nodes) can be incrementally updated during adaptation, gives them an added advantage in applications where rapid changes in observed data exist. Their work focuses on the use of LWPR for the human motor adaptation to novel tasks and environments. Figure 2.21 Comparison between LWPR and ANN. (a) ANN, (b) LWPR, adapted from [60] In this thesis, the choice of using LWPR for online model learning can be justified due to the following reasons: 55 1. The inverse dynamic model to be learned is highly nonlinear. The algorithm is scalable to any extra degrees of freedom (an extra tool or gimbal) that are added to the arm. 2. The model must be able to adapt to changes over time (due to wear and tear of the robot), independently and without any interference. 3. Large amounts of experimental data is available for offline training. Once the application has been deployed in a homecare environment, model training and estimation can still be performed during the online operation of the arm in real time. 4. LWPR is fast and reasonably accurate. Due to the incremental learning approach, the algorithm can be implemented and updated online within the strict timing requirement of the control loop (which executes at frequencies between 500Hz to 1kHz). In addition to the above-listed features, LWPR addresses the problem of nonlinear function approximation in high dimensional spaces with redundant input and irrelevant dimensions [61]. This is particularly useful in situations where the measurements (training data) collected from different sensors carry redundant information about the actual model. 2.5.2 Locally Weighted Regression Suppose that the following nonlinear function needs to be approximated: = ( )y f x (2.54) where y , mx , and  is the white Gaussian noise with variance 2 i.e., 2(0, )G  . LWPR then uses a set of spatially localized linear models that span the input space to predict 56 estimate of the given function. These localized linear models are weighted using Gaussian distributions. As illustrated in Figure 2.22, for a given new input x , the weight kw corresponding to the thk local linear model, also called a receptive field, is computed using the Gaussian kernel as 1( ) ( )2( ) =Tk k kkw e   x D xx (2.55) where kD represents the distance metric and k is the center of the thk receptive field. The weight ( )kw x defines the region of validity of the thk local model and therefore is also called the activation function of input x . During training, a new receptive field is created only if all the existing receptive fields do not yield activation above a certain threshold genw [6]. Figure 2.22 Estimating nonlinear function with local linear models. The output ky of receptive field k is then calculated using univariate regression as = [( ) ,1]T T Tk kx x (2.56) = Tk k ky x (2.57) 57 where k is the vector of estimated parameters for the thk model. The final predicted output value, = ( )iy f x , which is an approximation of the thi element of the output y in Equation 2.54, is then a weighted combination of K locally linear models and is given by: =11= ( ) ( )( )Ki k kky w yW x xx (2.58) where =1( ) = ( )KkkW wx x . The number of receptive fields, K , the coefficients of regression,  , and weights w are learned and adjusted incrementally using a stochastic gradient descent algorithm where the cost function to be minimized is, 2 2cost ,=1 , =1=11=M Ni i i i ijM i i jiiJ w y y DNw   (2.59) Here P is the inverted weighted covariance matrix of the input data and M is the number of data points seen during training. In order to adjust a receptive field, the corresponding distance metric D is updated as 1 =n n costJM MM (2.60) = TD M M (2.61) where M is an upper triangular matrix obtained from the Cholesky decomposition of D and  is the learning rate. Equation 2.61 ensures positive definiteness of D . To reduce the dimensionality of the problem, the projected inputs z are obtained by removing the redundant and irrelevant dimensions from the input data. This subspace of lower dimensionality is used by the LWPR for learning instead of the original high-dimensional space. 58 Complete details of the algorithm are given in References [62] and [6]. Tuning the LWPR Parameters There are several important parameters of the LWPR algorithm that need to be tuned. Some of the important parameters are listed in Table 2.5. Table 2.5 LWPR parameters for tuning. Parameter Description  Learning rate of the gradient descent algorithm  Multiplication factor in the cost function D Initial distance metric for newly created receptive fields NormIn Input normalization factor The most important parameter to tune is D as it decides the shape and the size of the receptive field. The convergence properties and the speed of the algorithm strongly depend on how this parameter is being initialized. Too large a value of D means that the size of a receptive field is small and therefore the number of total receptive fields is very large. This can result in over-fitting of the data, thereby resulting in poor predictive performance. On the contrary, a small value means larger receptive fields and therefore smoother function, but it may cause slow convergence speed and can also have the local minima problem. The other two elements,  and  , are also related to D. Too large a value of  can result in instability in convergence. Its value therefore may only be increased in situations where the MSE is decreasing, but the 59 convergence is slow. Large values of  will result in wider receptive fields (smaller distance metrics). The last parameter to tune is NormIn. Normalization of the input data makes it dimensionless by dividing each dimension by its variance. Once the input data is normalized, the distances between the receptive fields and the input data is calculated. Performance Measure To evaluate the performance of the LWPR algorithm during training and validation, the approximation error can be used as a performance measure. It is the normalized mean squared error calculated as 2=MSEnMSE where MSE is the mean-squared error (between the actual and approximation output) and is calculated as =11= ( )NiMSE y yN . Also, 2=11= ( )Niy yN  is the variance of the outputs of the test set. During evaluation of the regression model by leave-one-cut cross validation, = 0nMSE means perfect prediction. The nMSE error is especially useful for query points (points that are not seen by the algorithm during training) as it shows confidence in the prediction. The algorithm can output the trace of the nMSE error, the number of receptive fields added and pruned, and the amount of data processed during each cycle of execution. This information can later be used for purposes of plotting and analysis. The LWPR algorithm can be executed in BATCH or ONLINE modes. In the BATCH learning mode, fixed training and testing data sets (recorded as ASCII files) are used for training and validation of the model that is being developed. During execution, the algorithm in the 60 BATCH mode still works incrementally and outputs the trace of the learning progress continuously. The ONLINE mode is particularly useful when a model, originally learnt in the BATCH mode, is to be updated during the real-time operation of the robot. Example Simulation: Nonlinear Function Approximation with LWPR In this example simulation, the two-dimensional nonlinear function  2 2 2 2max exp( 10 ),exp( 50 ),1.25exp( 5( ))z x y x y     is learned from a set of noisy data samples using the LWPR algorithm. The resulting plots are shown in Figure 2.23 for two different initial values of the distance metric D. These two values are selected such that the problem of underfitting and overfitting can be demonstrated. As can be seen, in the case of a low value of D (i.e., when D = 10), the resulting model (labeled as fitted function) does not generalize correctly from the training data. In other words, underfitting is observed due to the wider receptive fields. On the other hand, a relatively large value of D (i.e., when D = 70) results in a model that overfits the training data. Initialization of D, therefore, plays an important role in the performance of the LWPR algorithm. 61 Figure 2.23 Example of nonlinear function approximation using LWPR 2.6 Results and Discussion 2.6.1 Experiments Two sets of experiments are performed. In both these experiments, the “Teach and Play" feature of the WAM software library (the libbarrett library [50]) is utilized. Necessary modifications are made to the teach and play example that comes as part of the libbarrett library. During the TEACH mode, the WAM arm is taught a path by physically moving it in a random fashion with variable speed in all possible directions in the robot’s workspace as shown in Figure 2.24. The corresponding motion of all joints is shown in Figure 2.25. This is possible due to the high backdrivability feature offered by the WAM arm. Once the taught path is recorded, a spline is built between the recorded points in order to connect them smoothly. Then it is played back by the WAM arm. During the PLAY mode, the trajectory (Cartesian positions x and velocities x , 62 joint positions q, velocities q , and accelerations q ) along with the joint torques  applied by the actuators are recorded from within the real-time control loop which runs at the rate of 500Hz. Figure 2.24 Taught Cartesian trajectory to the WAM arm (Experiments I & II). Figure 2.25 Joint positions for learning the inverse dynamic model (Experiments I & II). 63 Experiment I: Model Learning using Fast Trajectory Figures 2.25 to 2.28 show the recorded joint positions, velocities, accelerations, and torques respectively. As can be seen from Figure 2.26, some of the joint velocities (e.g., joint 3) are as high as about 2 rads-1. The WAM arm issues a fault signal (by turning on the red LED on its pendant) when the velocity of any joint exceeds the threshold value of 2 rads-1. The corresponding joint accelerations and therefore joint torques are also significant (see Figures 2.27 and 2.28). While practically, any robotic arm would rarely execute such a large-amplitude and high frequency motion, the purpose of using a fast trajectory for learning the inverse dynamic model is to capture the high-frequency dynamics of the arm and also to analyze the performance of the learning algorithm in a worst case scenario. Moreover, assuming constant angular speed is not feasible in the context of haptic teleoperation where the velocities in the system actually originate from the hand of the human operator who is manipulating the haptic interface. The slave manipulator must have to track velocities received from the master side, even if they are high (but still under the safety threshold). 64 Figure 2.26 Joint velocities for learning the inverse dynamic model (Experiment I). Figure 2.27 Joint accelerations for learning the inverse dynamic model (Experiment I). 65 Figure 2.28 Joint torques for learning the inverse dynamic model (Experiment I). Experiment II: Model Learning using Slow Trajectory In this experiment, the same joint displacements as in Experiment I are used, but the desired trajectory is smoothen using a trapezoidal velocity profile between consecutive positions. The trapezoidal velocity profile ensures smooth and continuous motion with slower joints velocities and accelerations for the WAM to track. The resulting slow trajectory (velocities and accelerations) and the corresponding actuator torques are re-recorded and are shown in Figures 2.29, 2.30 and 2.31 respectively. 66 Figure 2.29 Joint velocities for learning the inverse dynamic model (Experiment II). Figure 2.30 Joint accelerations for learning the inverse dynamic model (Experiment II). 67 Figure 2.31 Joint torques for learning the inverse dynamic model (Experiment II). 2.6.2 Results and Discussion In the fast trajectory case, model learning was difficult due to the corresponding large dynamic effects of inertia, Coriolis/centripetal and friction. The error convergence is shown in Figure 2.32 where the learned model still converges to an acceptable error level for all the joints. The corresponding number of receptive fields (RFs) during training is shown in Figure 2.33. As can be noticed, joint 2 had a complex model to approximate. The number of RFs therefore grows as the training continues. For the case of slow trajectory, the error convergence, as shown in Figure 2.34, and the number of RFs convergence was relatively smooth. For practical reasons and in the given context of homecare robotics, a fast trajectory as used in this experiment is never used due to safety reasons, but it has served the purpose of exposing the strengths and weaknesses of the learning algorithm. The successful convergence for both fast and slow trajectories show that the model learned with LWPR has a significant advantage over the analytical model and models identified with traditional techniques. 68 Figure 2.32 Error convergence in learning the inverse dynamic model (Experiment I). Figure 2.33 Evolution of number receptive fields in learning the inverse dynamic model (Experiment I). 69 Figure 2.34 Error convergence in learning the inverse dynamic model (Experiment II). 2.7 Summary This chapter first presented development of an experimental testbed for haptic teleoperation. This was followed by kinematic and dynamic modeling of the involved manipulators. A learning-based dynamic model identification technique was presented and its effectiveness compared to traditional techniques was studied and validated experimentally. The identified model will be used in impedance control in the next chapter. 70 Chapter 3: Interaction Control in Homecare Robotics 3.1 Introduction Instead of the traditional control schemes such as motion and force control, for robot manipulators and haptic devices, active interaction control is used in cases where interaction with a dynamic environment is involved. Active interaction control can be categorized into indirect and direct force control. The difference between these two categories depends on whether there exists a direct force feedback loop or whether the force control is achieved via motion control. The first category (indirect force control) includes impedance control. There, using a desired impedance model, the contact force due to interaction with an environment is related to the deviation of the end-effector trajectory from the desired trajectory. Further categorization of impedance control is possible based on the order of the desired impedance model, which is a virtual mass-spring-damper system with adjustable parameters. In standard impedance control the desired model is second order and includes all components of impedance, desired inertia, damping, and stiffness. In case of a first-order model, only stiffness and damping are controlled. This will be subsequently called first-order impedance control. If the target behavior is provided in the form of an admittance model, the corresponding control method is called admittance control. This chapter focuses on the design and evaluation of these control schemes in the context of haptic teleoperation. An impedance controller based on learned inverse dynamics is also proposed. 71 3.2 Standard Impedance Control Impedance control of robotic manipulators was originally introduced by Hogan in [34]. As defined in that work, an impedance controller regulates the dynamic behavior between the robot manipulator motion and the interaction force, which is the force exerted on the environment. An impedance-controlled manipulator can mimic the behavior of a human arm during interaction with an unknown environment. The block diagram of standard impedance control is given in Figure 3.1. As shown, the desired impedance block produces a dynamic force cF corresponding to the desired relationship between the desired trajectory ,d dX X and the contact force eF . For impedance control, the Cartesian-space formulation of the manipulator dynamic model (Equation 2.46) is used. An additional term, eF , is added to the right side to represent the external force exerted by the environment on the end effector of the manipulator. Then the dynamic model of the manipulator becomes, ( ) ( , ) ( ) =x x x eM q X C q q G q F F   (3.1) The model given by Equation 3.1 would make the formulation of the impedance control laws (as derived next) consistent for manipulators with different DOFs and dissimilar kinematics. This is helpful for robot teleoperation where the master and the slave manipulators are usually different in workspace size, number of DOFs, and dynamic manipulability. Impedance control requires a desired target dynamic behavior specified in the form of a given impedance function, usually called the desired impedance model, to be imposed on the manipulator during its interaction with the environment. The desired impedance model is given as 72 ( ) ( ) ( ) =d d d d d d eM X X B X X K X X F     (3.2) 1= ( ) ( )d d e d d d dX X M F B X X K X X        (3.3) where dM , dB , and dK are positive-definite matrices (usually diagonal) of size N N representing the desired (virtual) inertia, damping, and stiffness, respectively; and dX is the desired trajectory which the manipulator has to track while maintaining the contact with the environment in a desired fashion. Figure 3.1 Standard impedance control. Figure 3.2 Impedance controlled manipulator – a spring-mass-damper system. The control law in the standard impedance control scheme is based on computed-torque control and is given as 73 = ( ) ( ) ( , ) ( )T x x x eJ q M q X C q q G q F      (3.4) Computed-torque control is a special application of feedback linearization of nonlinear systems. Substituting Equation 3.3 into 3.4 results in the following control law: 1= ( ) ( ) ( ) ( )T x d d e d d d dJ q M q X M F B X X K X X         ( ) ( , ) ( ) ( ) ( )T T Tx x eJ q C q q J q G q J q F   (3.5) which can be simplified further using Equations 2.48 and 2.49 into 1= ( ) ( ) ( ) ( ) ( )T x d d e d d d dJ q M q X Jq M F B X X K X X          ( , ) ( ) ( )T eC q q G q J q F   (3.6) This is shown in the block diagram form in Figure 3.3. If the estimate of the nonlinear dynamic model is perfect, then the impedance-controlled manipulator behaves like a linear mass-spring-damper system as shown in Figure 3.2. Note that the only term of the robot’s dynamic model in the Cartesian space is the inertia term ( )xM q . The rest of the dynamic terms ( ( , )C q q q and ( )G q ) are in the joint-space. This would ease the computation burden and therefore make real-time implementation of the control law possible. This will be discussed further in the section on experimental results. 74 Figure 3.3 Standard impedance control. 3.3 First-order Impedance Control If the focus in impedance control is only on the stiffness and damping behavior of the manipulator, then the standard impedance control simplifies to first-order impedance control. Here, the desired inertia is the same as the natural inertia of the manipulator. The standard impedance control law in Equation 3.5 can be re-arranged as 1= ( ) ( ) ( ) ( ) ( ) ( )T Tx d x d d d d dJ q M q X Jq J q M q M B X X K X X           1( , ) ( ) ( ) ( )T x d eC q q G q J q M q M I F      (3.7) If the desired inertia is chosen equal to the natural Cartesian inertia of the manipulator, (i.e., = ( )d xM M q ), the impedance control law in (3.7) can be simplified as, = ( ) ( ) ( ) ( ) ( )( , ) ( )T Tx d d d d dJ q M q X Jq J q B X X K X XC q q G q             (3.8) Assuming that = 0dX and also 0J  , the control law becomes = ( ) ( ) ( ) ( , ) ( )T d d d dJ q B X X K X X C q q G q        (3.9) 75 This is known as first-order impedance control or inverse dynamics based Cartesian PD (Proportional-Derivative) control and is shown in Figure 3.4. The resulting closed-loop system becomes, 1= ( ) ( ) ( , ) ( )T eq M q J q F C q q G q      (3.10)  1= ( )x d x d x eX M q K e B e F    (3.11) Figure 3.4 First-order impedance control. An impedance-controlled manipulator effectively behaves as a linear mass-spring-damper system with the given (desired) parameters. This is shown in Figure 3.5 where the environment is modeled as a linear system of impedance ( )eZ s having the equation ( ) = ( ) ( )e eF s Z s X s (3.12) where ( ) = /e e eZ s b k s is the environment impedance model, ( )eF s is the force exerted on the environment, and ( )X s is the velocity of manipulator at the contact point, all expressed in the Laplace (or frequency) domain. The environment undergoes a small deformation during contact. 76 Figure 3.5 Impedance-controlled manipulator (a spring-mass-damper system) interacting with an environment (a spring-damper system). At the equilibrium point (considering only 1-DOF for analysis purposes) we get 0 = ( )d d ek x x f   (3.13) For an environment located at ex and modeled with lumped stiffness ek , we can express = ( ),e e e ef k x x x x  (3.14) This leads to the following expression for describing the coupled dynamic behavior between the robot and the environment: 1= ( ) ( )d e d d e ex k k k x k x  (3.15) = d eeqe dx xx kk k    (3.16) The equivalent stiffness is = / ( )eq d e d ek k k k k (two springs in series). From (3.15) it is clear that in cases where >d ek k (i.e., the manipulator’s tool is stiffer than the environment), the tool 77 will penetrate into the environment and will be able to reach its desired position dx x (see Figure 3.5(a)). Similarly, we have ex x in the case of a stiffer environment (i.e., >e dk k ). 3.4 First-order Impedance Control based on Learned Inverse Dynamics A major challenge faced by impedance control is that it is based on the computed-torque method. Computed-torque control makes use of the rigid-body dynamic model of the manipulator to cancel out the effects of the nonlinear inertial, Coriolis, centripetal and gravitational forces resulting from the motion of the manipulator. In practice, this computed torque (which is eventually a control action) includes two terms: one from the control law (such as the first- or second-order impedance controller), and the other from computation of the inverse dynamic model. The latter is aimed at canceling out the nonlinear dynamics of manipulator so that the desired level of compliance and impedance behavior is achieved. It is the computation of this second torque term that presents a bottleneck in the implementation of any impedance control scheme. First, it requires accurate knowledge of the robot dynamic model, which is usually partly or completely unknown. Secondly, its online computation is an extremely computationally intensive task, which cannot be accommodated online within the control loop of the manipulator that runs in hard real-time. This problem can be solved by combining the strengths of feed forward control with incremental online learning of the inverse dynamic model. No a-priori knowledge about the system dynamics is assumed. For this purpose, a fast algorithm, LWPR is used. The torque that is computed based on the learned inverse-dynamic model is fed forward to the inner torque loop of the impedance controller. The proposed impedance control system that is based on learned inverse dynamics is shown in Figure 3.6. 78 Figure 3.6 First-order impedance control based on learned inverse dynamics. 3.5 Admittance Control Admittance control in the literature is also called position-based impedance control. Here the desired model is of admittance type and outputs motion to the inner computed-torque controller based on the input force and the desired motion. In this section, the admittance control law will be derived. The desired impedance model (Equation 3.2) is now re-written as =d a d a d a eM X B X K X F  (3.17) 1= (a d e d a d aX M F B X K X     (3.18) where aX is the adjustment in position that must be added to the desired (input) position dX to obtain a new position rX . This adjusted position is used as a reference position for the manipulator to track. 79 In case an inverse-dynamic-based PD control is used for the motion control, then the modified form of Equation 3.8 becomes = ( ) ( ) ( ) ( ) ( ) ( , )T Tx r r rJ q M q X Jq J q B X X K X X n q q             (3.19) This is shown in the block diagram form in Figure 3.7. A simplified version of the admittance model (Equation 3.17) is obtained when only the desired damping dB is specified as: =d a eB X F (3.20) 1=a d eX B F (3.21) Assuming that a desired velocity dx is directly provided, then aX can be used as an adjustment to obtain a reference velocity for the manipulator to track; i.e., =r d aX X X (3.22) where the subscripts ,r d and a stand for reference, desired, and actual (end effector velocity). The reference velocity rX can then be integrated to get the reference position rX as = ( )r d rX X X d   (3.23) The derived reference position and velocity can then be used in the control law in Equation 3.19 which after the assumptions = 0rX and 0Jq  reduces to = ( ) ( ) ( ) ( , )T r rJ q B X X K X X n q q       (3.24) The 1=a dK B in Equation 3.21 is actually a mechanical admittance (velocity/force) which is why this scheme is called admittance control. The desired admittance aK is chosen according to the dynamics of the task that is being carried out. The motion control (inner loop) 80 gains B and K in Equation 3.24 can now be chosen independent of the outer admittance control loop to overcome the system’s unwanted nonlinear dynamics. This was not possible with the other two approaches (given in Sections 3.2 and 3.3). As can be noticed, the control law (Equation 3.24) is similar to that in Equation 3.9 except that in the former case the reference Cartesian trajectory is obtained from the admittance model in Equation 3.18. In the absence of interaction, when = 0eF , admittance control reduces to pure motion control and the control law (3.19) is used with the desired motion as a reference without making any adjustment to it. Figure 3.7 Admittance control. Admittance control is also referred to as position-based impedance control in the literature because of the cascaded inner feedback loop (loop 2 in Figure 3.7) for position control of the manipulator. The main advantage of admittance control as compared to standard impedance control is that the inner and outer sub-systems can be designed independently. This makes the control design process easy. The inner loop is designed such that it can compensate for nonlinear dynamics of the robot (based on its inverse dynamic model) and also attenuate the 81 effect of disturbances (unmodeled dynamics) in the output. The outer loop is designed such that the desired dynamic behavior is met without having to deal with the nonlinearities. Standard impedance control will have to achieve all these goals with a single set of control gains, which is not always possible. When the arm is controlled such that it achieves the desired level of compliance, motion tracking is compromised and vice versa. Admittance control solves this problem. A requirement of implementing admittance control is that the inner loop (loop 2) must run faster than the outer loop (loop 3). This poses a constraint on the dynamics of the system. As the interaction force is fed back into the outer loop, a slower loop rate would mean that the force bandwidth is limited. Interaction with hard and stiff environments, which may contain frequencies up to 1000Hz or higher, therefore, would not be possible and can result in instability. In the context of haptic teleoperation this would mean that the stiff spring-like environment on the slave side may destabilize the control loop. On the master side, the human operator must not tense his arm while manipulating the haptic interface. Assumptions are therefore put on the environment and the human operator to be passive in order for the complete system to be stable. This is a common assumption in the haptic teleoperation literature. 3.6 Simulation Study: Interaction with Soft Environment Simulation setup for demonstrating the effectiveness of all the impedance control schemes is shown in Figure 3.8. To make the simulation more realistic, actual haptic device (PHANToM) is used as the master manipulator. The slave manipulator and the environment are simulated as 1-DOF systems. The manipulator is simulated as a mass-damper system with parameters = 0.3kg, =1.5Ns/mms bs . The environment is simulated as a virtual spring for 82 which the interaction force is calculated as = ( )e e eF k x x  where =100ek N/m and = 0.075ex m (nominal location of the environment, which lies along the x-direction of the master’s end effector). Cartesian positions of the master and slave systems are graphically displayed on computer screens. Figure 3.8 Experimental setup (haptic teleoperation with time delay). Standard Impedance Control The desired impedance model has the parameter values = 0.003kg, = 0.5Ns/md dM B and =100dK N/m. The results are shown in Figures 3.9 to 3.11. Figure 3.9 shows the tracking of the desired position (as dotted red) as received from the master side. Figure 3.10 shows velocity tracking and Figure 3.11 shows the interaction force. 83 Figure 3.9 Position tracking (Standard impedance control). Figure 3.10 Velocity tracking (Standard impedance control). 84 Figure 3.11 Interaction force (Standard impedance control). First-order Impedance Control The desired impedance model has the parameters values =100N/mdK and =10Ns/mdB . Results are shown in Figures 3.12 to 3.14. Figure 3.12 Position tracking (First-order impedance control). 85 Figure 3.13 Velocity tracking (First-order impedance control). Figure 3.14 Interaction force (First-order impedance control). Admittance Control The desired admittance has been set to = 0.1ms/NaK . The motion control parameter values are =100N/mK and =10Ns/mB . Results are shown in Figures 3.15 to 3.17 for a case where position-error correction is not made (see Equation 3.23). As can be seen in Figure 3.15, 86 position tracking is poor corresponding to the velocity tracking shown in Figure 3.16, which is rather accurate. As a result, interaction with the environment is limited (small magnitude forces in Figure 3.17). The drift in position is due to the fact that it has been obtained by integrating velocity and the initial condition is not set properly. Increasing the admittance gain ( setting it equal to unity) further aggravates the problem by increasing the position drift. This was solved by also adjusting the reference position rX by adding the desired position dX to it (as in Equation 3.23). This adjustment of position actually matches the initial conditions of the Integrator to the actual desired position. This means that in the case of teleoperation, the desired positions must also be exchanged along with velocities between the master and slave manipulators. This will be discussed in detail in Chapter 4. Improved results are shown in Figures 3.18 to 3.20. Figure 3.15 Poor position tracking (Admittance control). 87 Figure 3.16 Velocity tracking in case of poor position tracking (Admittance control). Figure 3.17 Interaction force in case of poor position tracking (Admittance control). 88 Figure 3.18 Position tracking (Admittance control). Figure 3.19 Velocity tracking (Admittance control). 89 Figure 3.20 Interaction force (Admittance control). 3.7 Experiment 1: Control of PHANToM Interacting with a Hard Environment This experiment is designed to demonstrate the strengths of each control scheme presented in this chapter while interacting with a stiff environment. This is important in order to assess the safety and stability of each control scheme for practical application in a homecare environment. Experimental setup is shown in Figure 3.21 where the actual master and slave manipulators are used (both PHANToM devices). The environment is simulated as in the previous simulation, but with larger stiffness value 800N/meK  located at 0.065ex  m. The haptic controllers are Windows-based Intel x86 PCs with real-time scheduling ensured by the CHAI 3D library with a servo rate of 1,000 Hz. Communication between the master and slave systems occurs on a Local Area Network (LAN) network. For the current results, the round-trip time delay is less than 1ms and is therefore neglected. The desired apparent inertia of the slave PHANToM is set equal to the natural inertia. The nonlinear dynamics were low and therefore neglected. The results of the first-order impedance control are shown in Figure 3.23. The desired stiffness and damping were selected as 200Pk  N/m, and 1Db  Ns/m. The desired stiffness is 90 selected such that the manipulator should feel compliant (soft) while interacting with a hard environment. But, as indicated from the results, this extra level of compliance comes at the cost of loosing the position accuracy. This sluggish behavior is inherent in first-order impedance control and can be considered as its weakness. This problem is solved by admittance control as can be seen in Figure 3.24 which shows a stable interaction. As seen in the velocity plot, the end effector slows down while making contact with the hard environment. As position tracking is achieved by the inner loop, the impedance parameters in the outer loop can be independently selected. For robotic surgery and homecare applications, the desired damping term must be selected high in order to dampen out fast oscillatory motions that may arise during making contact with a stiff environment. This work is published in [63]. Figure 3.21 PHANToM devices as the master and slave manipulators with their corresponding controllers (PCs). 91 Figure 3.22 First-order impedance control (Interaction with hard environment). 92 Figure 3.23 Admittance control (Interaction with hard environment). 3.8 Experiment 2: First-order Impedance Control of WAM Arm In this final experiment, the first-order impedance control was implemented on the WAM arm whose nonlinear dynamics such as friction and Coriolis cannot be ignored. The WAM arm is instructed to follow the same circular trajectory as in the example simulation in Chapter 2 of this thesis. Interaction with any environment could not be performed due to the unavailability for force/torque sensor. So the first-order impedance controller is practically equivalent to Cartesian PD controller with inverse dynamic compensation. The experiment still served to assess the performance of the learned inverse dynamic model based on the LWPR algorithm. For 93 comparison the same trajectory was tracked using the libbarrett’s inverse dynamic system (systems::InverseDynamics) which uses the Newton-Euler formulation to compute the inverse dynamic model using the dynamic parameters provided by the manufacturer. Results are shown in Figures 3.24 to 3.29. For the case of LWPR, Figure 3.24 shows the end effector position in x-y plane during tracking. Figure 3.25 shows the feed forward torques as computed by the LWPR model based on the desired trajectory (in the joint-space which was computed from the desired Cartesian trajectory using inverse kinematics; see Figure 3.6). Figure 3.26 shows the total torque sent to the actuator. This is the sum of the feed forward torque and the torque from the controller. Similar plots for the libbarrett inverse dynamic model are shown in Figures 3.27 to 3.29. Results show that the tracking performance of the LWPR model is comparable to that of the analytical model. Figure 3.24 Tracking of a desired Cartesian trajectory based on LWPR-generated model. 94 Figure 3.24 Output of the inverse dynamic model (tracking based on LWPR model). Figure 3.25 Controller output (tracking based on LWPR model). Gains = 2000, = 20p dk b . 95 Figure 3.26 Tracking of a desired Cartesian trajectory based on libbarrett’s inverse dynamics model. Figure 3.27 Output of the inverse dynamic model (tracking based on libbarrett’s inverse dynamics model). 96 Figure 3.28 Controller output (tracking based on libbarrett’s inverse dynamics model). Gains = 2000, = 20p dk b . 3.9 Estimation of the Environment Dynamic Model During interaction control, dynamics of the environment (the contact object) couple with the dynamics of the manipulator. The environment’s behavior affects the control of the manipulator. In order for the impedance control to be accurate, knowledge about the dynamic models of the manipulator and the environment both are required. In Section 2.4 enhanced method for computing the inverse dynamic model of the manipulator using a data-driven approach has been presented. The dynamics of the environment, however, are usually completely or partly unknown. The estimation technique of the environmental dynamic model depends on the type of environment that the manipulator is interacting with. The focus of the current work is on the type of environment that is stationary and has a rigid or compliant surface. Several methods have been investigated to determine the unknown impedance model of the environment (e.g., see in [64], [65], and [66]) and selecting the corresponding controller 97 gains according to the estimated model parameters. A practical and recommended method uses the online identification of the environmental model and adaptive tuning of the controller impedance gains. In most cases the environmental dynamic model is assumed as a first- or second-order linear system. Once the model structure is assumed, the model parameters can be estimated using least-square recursive regression techniques. These techniques have the capability to deal with time-varying systems and therefore form the basis of many of the adaptive control strategies. They also can be initially trained offline (in an ideal laboratory environment) and then iteratively updated online. Lastly, they are computationally efficient, less memory intensive, and therefore can be implemented in real time. Generally, the dynamic behavior of the environment can be approximated by a lumped stiffness ek and a lumped damping coefficient eb . Using measurement data such as information from the force, position and velocity sensors, the unknown environmental parameters: the coefficients eb and ek can be estimated online. Once the experimental data are available, a recursive least-squares estimator is used to estimate the impedance parameters. This is illustrated in Figure 3.30. The estimation process is executed at discrete samples in real-time where the current state at time instant i is predicted from the previous state (at 1i  ). The predicted state is then combined with the current measurements to estimate the current state. Details of the RLSR Algorithm are presented next. Figure 3.29 RLSR algorithm for environment model identification. 98 The environment can be modeled as a mass-spring-damper system with lumped damping coefficient eb , and lumped stiffness constant ek : =e e ef b x k x  (3.25) Here = ex x x  , and ex is the location of the environment. In the discrete (z-transform) domain, the system (Equation 3.25) can be represented as 112 1=1e e eszf b k xT z          (3.26) where sT is the sampling period and 1z is the time-shift operator (or, the z-transform variable). The resulting difference equation is: 1 2( ) ( 1) = ( ) ( 1)e ef i f i a x i a x i     (3.27) where i represents the time-step index. The coefficients 1a and 2a are the model parameters that need to be estimated. The system constants eb and ek are then computed from these parameters. The position ( )x t is treated as the input to the system and the force ( )ef t as the output. This means the environment is treated as an impedance-type, although the algorithm presented here is applicable to admittance-type models as well. The system in Equation 3.27 can be represented as a linear regression ( ) = ( )Ty i i  (3.28) where 1 2= [ , ]Ta a is parameter estimate vector and  ( ) = ( ), ( 1)Tt x i x i    is the regression vector. Variable ( )y i is the system output, which in the current case equals ( ) ( 1)e ef i f i  . The model in (3.28) belongs to a family of more general time-domain input-output models referred to 99 as ARMAX which are usually used for system identification purposes. Their application is not limited to robotics only. Considering that the regression matrix ( )n is formed from the regression vector ( )i as ( ) = (1), (2), , ( )TT T Tn n      and the output vector as  ( ) = (1), (2), , ( )Ty n y y y n , the normal least-squares estimate of Equation 3.28 can be found as: 1ˆ( ) = ( ) ( ) ( ) ( )T Tn n n n y n     (3.29) where ˆ( )n represents an estimate of the output parameters at time =t n . As the ARMAX system has its inputs and outputs that rely on their current and previous values, it lends itself to recursion and therefore identification in real time. A complete RLSR algorithm can be obtained in two steps as outlined below [67]: Prediction step: ˆ ˆ ˆ( ) = ( 1) ( ) ( ) ( ) ( 1)Ti i K i y i i i         (3.30) Update step: 1 ( 1) ( ) ( ) ( 1)( ) = ( 1)( ) ( 1) ( )TTP i i i P iP i P ii P i i           (3.31) ( 1) ( )( ) =( ) ( 1) ( )P i iK ii P i i    (3.32) where P is the error covariance matrix ( P being a large positive number) and <1 is the forgetting factor, which determines that the measurements that are  samples old will be forgotten and not used in the current computation. 1=1 is called the memory horizon of the 100 algorithm. The gain, ( )K i , determines the effect of the current prediction error ( ˆ( ) ( ) ( 1)Ty i i i   ) on the parameter estimate. The RLSR algorithm with a forgetting factor can estimate the time-varying environmental parameters in real time provided that the system dynamics change slowly with time. 3.9.1 Simulation Study An example simulation is presented here to assess the strengths and shortcomings of the system identification algorithm that has been presented. The assumed model structure whose parameters are to be identified is a second-order linear system (a mass-spring-damper) having natural frequency = 37.4n rads-1 and damping ratio = 0.53 . This corresponds to actual stiffness =1400ek Nm-1 and damping coefficient = 40eb Nsm-1, which are the target values of the parameters for the identification process. It is assumed here that the available measurements (sensor identification data) include the exogenous input to the system position x , and the output interaction force ef . An input signal consisting of two sinusoidal functions of different frequencies, as shown in Figure 3.31, is used: 1 2( ) = sin(2 ) 0.5sin(2 )x t f t f t  (3.33) The input signal has sufficiently rich frequency content (in the given context of teleoperation) to serve as a persistent excitation. The frequencies considered here are 10Hz and 20 Hz. From a practical viewpoint, these frequencies are of the order in magnitude as the human arm movement (the source of input motion) while he/she is manipulating the master robot in a real-time teleoperation application. The sampling time is taken as = 0.001sT s. The white noise of standard 101 deviation 0.01 is also added to the input. The results of the simulation are presented in Figures 3.32 to 3.34 which show the convergence of the algorithm to the target values. Figure 3.30 Simulation setup for example simulation of online identification of the environment model. Figure 3.31 Input signal (example simulation of online identification of the environment model) 102 Figure 3.32 Stiffness estimate (example simulation of online identification of the environment model) Figure 3.33 Damping estimate (example simulation of online identification of the environment model). 103 Figure 3.34 Bode plot of actual and estimated systems (example simulation of online identification of the environment model). 3.10 Selection of Desired Impedance Model Once the environmental impedance is estimated online, the desired impedance parameters for impedance control of the slave manipulator can be selected according to the identified parameters. This section provides some guidelines that can help tune the desired parameters. For an impedance-controlled robot (where { , , }d d dm b k are the desired impedance parameters) that is in contact with an environment (modeled as a linear spring with stiffness ek and damping eb ), the desired natural frequency and the damping ratio along each Cartesian axis are given by 104 = d eddk km (3.34) =2 ( )ddd d ebm k k (3.35) To overcome the contact instability problem, an over-damped behavior ( >1ds ) is stipulated. The manipulator end effector may exhibit chattering behavior, especially in the case of contact with a very stiff environment. The contact instability problem can be resolved by selecting the damping term to be larger than the corresponding mass and stiffness terms. A large damping ratio also means avoiding large impact forces, which may arise during contact with an environment having uncertain geometry. This necessitates a small value for dk (high compliance) in directions where contact with a stiffer environment ( >>e dk k ) is foreseen. Thus the desired stiffness of the slave manipulator matches the environmental stiffness in a complementary way. Once the desired stiffness term is selected based on the estimated environmental stiffness, the desired damping term db is used to shape the closed-loop transient behavior. The desired stiffness of the master manipulator should be set equal or proportional to the estimated environmental stiffness to achieve enhanced transparency. One way to select the desired impedance characteristics is based on qualitative analysis. Criterion formulated as a result of detailed stability analysis does not usually suffice due to the fact that stable impedance control gains may still be practically unsafe. Following are some general guidelines for the selection of desired impedance parameters [68]:  Desired inertia should be selected relatively high in directions where a contact is expected. This will limit the dynamics of the robot and result in limited contact forces. 105  Desired damping is selected to be relatively high in directions where a (critically- or over-) damped response is required. This will lead to dissipating more energy along those directions.  Desired stiffness is selected to be relatively high in a direction where position control is required and relatively low where contact is expected. High stiffness value in position-controlled directions will also lead to enhanced accuracy in motion tracking by the end effector. 3.11 Summary This chapter presented three types of impedance controllers. It provided an experimental evaluation and showed the superiority of admittance control over the other two. Impedance control based on learned inverse dynamic model was also proposed. Algorithm and guidelines for selecting parameters of the desired impedance model were given. 106 Chapter 4: Haptic Teleoperation with Time Delay 4.1 Haptics The word haptic is from the Greek “haptikos" and it means “pertaining to the sense of touch." It is related to the perception of objects through touch. A haptic system (which comprises software and hardware) allows humans to interact with real or virtual environments. Haptic feedback is the haptic information that a haptic interface provides to a human operator. Haptic feedback can be categorized into two different classes: tactile and kinesthetic. The kinesthetic haptic feedback is felt by the sensory receptors (known more formally as proprioceptors) in our muscles, joints and tendons. They excite the human locomotive system, which includes joint positions, limb alignment, body orientation, and muscle tension [69]. We can feel the weight, mass, stiffness, orientation etc. of an object from the sensation of movement or stress in our muscles, tendons, and joints. On the other hand, tactile information is felt by receptors embedded in our skin. It enables us to feel vibration, pressure, pain, touch, texture, temperature, and so on. Haptic teleoperation using a haptic interface (normally a 6DOF robotic manipulator) helps reproduce to the operator, contact forces that are actually generated at a remote location or in a virtual world (such as within a computer simulation). In the context of haptic teleoperation, the term force feedback usually describes the kinesthetic feedback, which is normally provided using physically grounded haptic interfaces. Availability of force feedback to the human operator has resulted in improved task performance. As an example, in [70], the authors have experimentally demonstrated that kinesthetic force feedback would improve the task performance in carrying out a ground-space bilateral teleoperation of a robot arm. Similarly, in [71], the authors have assessed the benefits of force feedback in robotic surgery. They found force feedback helpful in blunt dissection as it helped constrain the surgeon’s hand from 107 commanding inappropriate motions that could generate large forces and damage the surrounding tissues. Reference [69] also shows that the force feedback helps lower the error rate during telemanipulation. The PHANToM Premium 1.5 model provides force feedback around roll, pitch, and yaw axes in addition to three translations. In this thesis, the haptic feedback is limited to kinesthetic feedback in the three spatial directions (3-DOF). Structurally, it is an impedance-type haptic interface because the human operator can mechanically input motion by holding and moving the position of the Tool Centre Point (TCP) into the system. The mechanical output is in the form of a force or torque that the device (with the help of actuators) displays at the stylus location (a 3D point in the workspace) for the operator to feel (see Figure 4.1). As the position is measured and the force is generated, and hence the name “impedance-type." The impedance - type would also mean that the device can be naturally impedance controlled both in open- and closed-loop fashions. Figure 4.1 PHANToM Premium haptic interface and controller. 108 4.2 Haptic Teleoperation with Time Delay Figure 4.2 shows a network representation of a teleoperation system. This way of modeling and representing a robotic teleoperation system was originally inspired by network theory from the field of electrical power systems. The human operator, the Master-Slave-Network (MSN) subsystem, and the environment are modeled as a 1-port, 2-port, and 1-port network, respectively. The operator’s hand velocity, hV , is provided to the MSN system, which returns to him an output force feedback hF . On the right side of MSN is an environment, which applies velocity eV as a result of its interaction with the end effector of the slave arm. The direction of eV is the reverse of hV . Also, eF represents the environmental force, which is to be fed back to the human operator. The MSN subsystems comprise the master and slave manipulators along with their controllers. Later, the communication channel between the master and slave ends will also be included. Figure 4.2 Bilateral teleoperation system. In order to study and analyze the stability and performance of the system, the MSN subsystem is represented by a set of four transfer functions between various inputs and outputs at the interface of MSN with the external world. These transfer functions are arranged in a 2 2 matrix ( )H s called hybrid matrix [72] and is illustrated in Figure 4.3. As shown, the human 109 operator and the environment are modeled as the impedances hZ and eZ , respectively. The negative sign associated with eV indicates that the actual direction is opposite to what is considered here. Figure 4.3 Bilateral teleoperation system. The hybrid matrix ( )H s is defined as 11 1221 22( ) ( )( ) ( )=( ) ( )( ) ( )h he eF s V sh s h sV s F sh s h s              (4.1) where s is the Laplace variable. The description of all four transfer functions in ( )H s is given in Table 4.1. Table 4.1 Description of the hybrid matrix, H(s). ( )ijh s Description 11=0( )( ) =( )hh FeF sh sV s Impedance felt when slave arm is in free motion 12=0( )( ) =( )he VhF sh sF s Force feedback gain when master arm is not moving 21=0( )( ) =( )eh FeV sh sV s Forward velocity gain when slave is in free motion 110 22=0( )( ) =( )ee VhV sh sF s Impedance felt by environment when master is not moving The goal of any teleoperation system design is to achieve an ideal hybrid matrix, idealH , defined as ideal0 1=1 0H    (4.2) This states that if the slave arm is not interacting with the environment, there should not be any impedance felt by the operator; hence, 11( ) = 0h s . This element characterizes the impedance transmitted to the operator by the MSN when the slave is in free motion. Also, 12 ( ) =1h s states that the desired force must be displayed transparently to the operator; =h eF F , while he is interacting with the environment and holding the master arm firmly ( = 0hV ). This element characterizes the force scaling during interaction. Furthermore, 21( ) = 1h s  states that =e hV V ; hence, the slave must perfectly track the desired trajectory that is received from the master side. This element characterizes the slave’s motion tracking capability while its not in interaction with the environment. Similarly, the last element 22 ( ) = 0h s states that there shouldn’t exist any activity (interaction) between the slave arm and the environment if there is no such command received from the master side ( = 0hV ). This element characterizes the admittance transmitted by the MSN to the environment when the master is clamped. Various measures of system stability and transparency are then derived based on the hybrid matrix formulation. Other matrix representations of MSN, such as impedance, admittance, and inverse hybrid, also exist in the literature, which are different only with regard to 111 the choice of input-output variables on the left and right hand sides of Equation 4.1. The essence, however remains the same. Reference [73] presents and extends Llwellyn’s absolute stability criterion, which renders the two-port network absolutely stable. The criterion is given in terms of the two-port network impedance parameters. In [74], the authors have extended the criterion to a non-passive operator. Apart from stability, an important design goal for teleoperation systems is transparency, which is defined as a match between the positions and forces of the master and the slave manipulators. The two elements, 11( )h s and 21( )h s , combined, can be used as a measure of the system’s transparency. In addition to the position and force correspondence, a transparent teleoperation system also aims at ensuring that the impedance match. According to this, the impedance presented to the operator by the MSN during interaction, toZ , is the same as that of the environment, to = eZ Z . It means that the operator feels the same (and undistorted) environmental impedance without feeling any unwanted dynamics of the master and slave manipulators. The transmitted impedance toZ can be computed as [75],  11 11 22 12 21to22( ) ( ) ( ) ( ) ( )( )= =( ) 1 ( )ehh eh s h s h s h s h s ZF sZV s h s Z  (4.3) For the extreme conditions when = 0eZ , the slave moves freely without touching the environment, and when =eZ  , the slave interacts with an extremely stiff environment. The corresponding expressions for toZ are, 11 22 12 21to,stiff22( ) ( ) ( ) ( )=( )h s h s h s h sZh s (4.4) 112 to,free 11= ( )Z h s (4.5) which are the maximum and minimum values of transmitted impedance toZ . From these values another important performance measures, widthZ , can be derived as width to,stiff to,free=Z Z Z (4.6) As can be noticed, widthZ is the range of impedances that the operator can experience during interaction. Two other transparency measures that have been derived in [75] from the to,stiffZ and to,freeZ are the apparent stiffness and damping of the teleoperation system. to,stiff to,stiff0= limsk sZ (4.7) to,free to,free0= limsb Z (4.8) 4.2.1 Delay Effect on Stability Figure 4.4 Teleoperation system as an interconnection of 2-port network elements. Figure 4.4 shows an expanded version of Figure 4.2 where the communication channel has now been considered. The major impact that the communication channel has on the 113 performance and stability of the teleoperation system is that it induces a time delay T in the information that is being sent through it. If a symmetric time delay T is assumed, then = ( )h eF F t T (4.9) = ( )e hV V t T (4.10) Using the hybrid matrix notation for the communication channel that is modeled as a 2-port network, ( ) ( )0=( ) ( )0sTh hsTe eF s V seV s F se              (4.11) Scattering theory has been widely used for the stability analysis of force reflecting teleoperators with time delay. According to the scattering theory, a scattering operator, S , is defined as a mapping between the difference and the sum of force and velocity: = ( )f v S f v  (4.12) They the stability criterion of a 2-port network is given by  *1/2( ) = ( ) ( ) 1supS s S j S j   (4.13) where  is the eigenvalue of ( )S s . The “ * ” here represents the transpose conjugate of ( )S s . Equation 4.13 is also a definition of a passive system. The criterion states that in order for a system to be passive and therefore stable, the norm of its scattering matrix, which is the maximum eigenvalue of *( ) ( )S j S j  , must not exceed unity. This, for example, is not true for the communication channel that is represented using the hybrid notation in Equation 4.11 for which the scattering representation and the scattering matrix are given as ( ) ( ) ( ) ( )= ( )( ) ( ) ( ) ( )h h h he e e eF s V s F s V sS sF s V s F s V s            (4.14) 114 tanh( ) sech( )( ) =sech( ) tanh( )sT sTS ssT sT    (4.15) and its norm is given as  ( ) = tan( ) sec( ) =supS s T T   (4.16) Time delay, therefore renders the teleoperation system unstable because of its non-passivity. For any 2-port network, the scattering matrix can be derived from a given hybrid matrix ( )H s as,   11 0( ) = ( ) ( )0 1S s H s I H s I     (4.17) Niemeyer and Slotine in [76][39] introduced the concept of wave variables to guarantee stability of force reflecting systems under unknown but constant transmission delays. In wave-based teleoperation, wave variables mu and sv are sent over the communication channel instead of the original variables hV and eF , as illustrated in Figure 4.5. The details of “Wave Variables Transformation" block will be explained in the next section (Section 4.2.2). Figure 4.5 Communication (a) before wave-variables transformation, (b) after transformation. 115 Due to the introduction of wave variables, the resulting scattering matrix ( )S s comprises only the delay exponentials: 0( ) =0sTsTeS se    (4.18) which has the norm equal to one and therefore the resulting system is passive and stable despite the time delay during transmission. 4.2.2 Wave-based Haptic Teleoperation Passivity Theory For the last two decades, passivity-based control has provided has been a strong approach towards delay-independent stabilization of bilateral teleoperators. Pioneer work in this area came from Anderson and Spong [38] and Niemeyer and Slotine [77]. The underlying key concept is to represent a master-slave teleoperator system as an interconnection of two-port networks and then encode the velocity and force signals as so-called scattering variables before transmitting them over the network. This approach renders the communication channel passive and the entire system stable, independent of the time delay. Scattering theory uses the notion of scattering transformations which relate the sum of velocity and force to their difference, so that passivity becomes a condition on the scattering matrix (system gain), which is unaffected by the delay. The other popular approach for dealing with time delay in teleoperation, i.e., TDPC [45] [46], is also based on the passivity theory. Passivity theory deals with the exchange of energy between interconnected systems and can be applied to both linear and nonlinear systems. In order to make use of passivity theory for 116 bilateral teleoperation with time delay, the teleoperation system (see Figure 4.4) is modeled as an interconnection of one- or two-port networks where energy at each port is exchanged in the form of an effort-flow pair of (power) variables. For a mechanical system the effort-flow (or through-across) pair corresponds to the force-velocity pair, which for a robot manipulator may also be interpreted as the torque-angular velocity pair at each revolute joint or the force-linear velocity pair at each Cartesian axis that is assigned to the end effector. If all the systems (one- or two-port networks) in Figure 4.4 (human operator, master/slave manipulators, and the environment) are passive, their interconnection (whether in parallel or feedback) is still passive and overall stability of the complete teleoperation system is therefore guaranteed. In the network representation of the teleoperation system (Figure 4.4), the human operator and the environment are modeled as one-port systems and can be treated as mechanical admittance and mechanical impedance, respectively. According to passivity theory, a one-port system is passive if the following condition holds: ( ) = ( ) ( ) 0tE t f x d t    (4.19) Or, 0( ) = ( ) ( ) (0) 0 > 0tE t f x d E t      (4.20) Here 0(0) = ( ) ( )E f x d   (4.21) 117 is the initial energy stored in the system. For a two-port network having an input conjugate pair of power variables 1 1( ), ( )f t x t and an output pair 2 2( ), ( )f t x t , the condition of passivity is given as  2 2 1 10( ) = ( ) ( ) ( ) ( ) (0) 0 > 0tE t f x f x d E t        (4.22) The above two conditions state that there is no internal power generated by the system. A bounded input energy supplied to the system results in bounded output energy. The system is therefore stable in the input-output sense. These definitions actually originated from electrical networks, but can be analogously applied to mechanical systems. It is a reasonable assumption to consider the human operator and the environment (which in the case of homecare may also be a human) as passive systems. Practically, this means that the humans, being part of the closed-loop, do not input random and out-of-bound motions and forces to the system. The human operator firmly holds the end effector of the haptic interface (to avoid contact instability) at all times during teleoperation and does not exceed the magnitude and frequency of arm motion beyond a certain threshold. The human’s passivity assumption is valid as long as active movements of the human arm (which is the source of input motion to the teleoperation system) remain under 10Hz [78]. The master and slave manipulators, which are nonlinear systems, can be controlled in such a way that their passivity is guaranteed. Passivity-based analysis and design of nonlinear control systems have been successfully used and have demonstrated satisfactory results as can be found in the literature. A strong relationship exists between passivity and Lyapunov stability, which forms the basis for the design of robot motion control where the goal is to achieve a globally asymptotically stable system. 118 By treating the communication channel as a two-port network, energy stored at time t can be computed as  0( ) = ( ) ( ) ( ) ( ) (0) 0 > 0td dm m s sE t f x f x d E t        (4.23) This condition on passivity of the communication channel does not hold true in the presence of time delay. 119 Wave Variables Figure 4.6 Communication channel with wave variables. Figure 4.7 Wave-based teleoperation architecture. Introduction of wave-variables transformation, as shown in Figure 4.6, renders the communication channel passive, even in the presence of time delay. A complete teleoperation system with wave variables is shown in Figure 4.7. It is seen that the wave variables mu and sv are exchanged between the master and the slave sides, instead of the actual power variables mx (the velocity of the master as measured directly or derived from the measured position) and dmf 120 (the interaction force). The transformation between the wave variables and power variables is given by  1( ) = ( ) ( )2bdm m mu t bx t f t (4.24)  1( ) = ( ) ( )2bds s sv t bx t f t (4.25) where b is a positive constant referred to as the characteristic wave impedance, which plays an important role in determining the system response. The variable mu is called the forward or right-moving wave as it is being sent by the master side to the slave. Similarly sv is called the backward or left-moving wave. These (wave) variables when received by the respective ends are represented as 1( ) = ( )s mu t u t T (4.26) 2( ) = ( )m sv t v t T (4.27) where 1 2 and T T are the time delays induced by the communication channel during transmission. On receiving the transmitted wave variables, the master and the slave ends have to decode the wave variables in order to derive the corresponding actual power variables as ( ) = ( ) 2b ( )dm m mf t bx t v t (4.28)  1( ) = 2 ( ) ( )ds s sx t bu t f tb (4.29) From Equations 4.24 to 4.29, the power variables can be written in terms of the wave variables as  ( ) = ( ) ( )2dm m mbf t u t v t (4.30) 121  1( ) = ( ) ( )2m m mx t u t v tb (4.31)  ( ) = ( ) ( )2s s sbf t u t v t (4.32)  1( ) = ( ) ( )2ds s sx t u t v tb (4.33) It is possible now to transform the expression for energy stored in the communication channel (Equation 4.23) into the wave-domain as (assuming that 1 2= =T T T for the sake of simplicity)  2 2 2 201( ) = ( ) ( ) ( ) ( )2tm m s sE t u v u v d       (4.34) 2 21 1( ) = ( ) ( ) 02 2t tm st T t TE t u d v d       (4.35) This states that the passivity condition has been met and the communication channel is stable for any arbitrary time delay T . From Figure 4.7 and also from Equations 4.24 to 4.25, it is noticed that a sending wave variable is computed partly based on the incoming signal (force on the master side and position on the slave slide). This forms an algebraic loop where part of the received information is reflected back to the sender. This phenomenon is known as wave reflection and is illustrated in Figure 4.8. 122 Figure 4.8: Wave reflections in wave-based teleoperation system. Wave reflections can disorient the human operator by causing oscillatory behavior and poor transient response. In order to cancel out the reflected wave information, Niemeyer and Slotine [39] suggested that the impedances on the two ends must be matched properly. Their solution is shown in Figure 4.9 where two blocks “b” and “1b” are appended at the right and left ends, respectively. Figure 4.9 Cancellation of wave reflections in wave-based teleoperation system. This resulted in the following re-computation of forward and backward wave-variables which does not involve a component of the reflected signal anymore: ( ) = ( )2m mbu t x t (4.38) 123 ( )( ) =2ssf tv tb (4.39) The corresponding actual power variables are derived from the received wave variables as 2 2( )2 2dm m msmb bf x vf t Tbx   (4.40) 1 12 21 1( )2 2ds s sm sx f ub bx t T fb     (4.41) This solves the wave reflection problem, but as can be noticed from Equations (4.40) and (4.41), transparency still remains compromised. Ideally the desired signals received at the master and the slave ends should be the delayed version of what was sent by the corresponding end; i.e., ( )dm sf f t T  and ( )ds mx x t T  . But this is not the case in Equations (4.40) and (4.41). The master has to feel an unwanted damping (as shown in Figure 4.9) and therefore the displayed force is distorted. Similarly, for the slave side the desired velocity is also not the same as sent by the master side. In summary, force and velocity tracking is not ideal. To solve this problem and improve position and force tracking in wave-variables based teleoperation, Li and Kawashima in a recent work [79] have proposed a modification to the impedance-matched wave-variable scheme. According to their solution (shown in Figure 4.10), before retrieving the actual variables, the wave variables at the receiving end need to be modified as [79]: ( )( ) = 2b ( )2bss mf tu t x t T  (4.42) 124 2( ) = ( ) ( )2m m sbv t x t f t Tb  (4.43) or alternatively, ( ) = 2 ( ) ( )s m su t u t T v t  (4.42) ( ) = 2 ( ) ( )m s mv t v t T u t  (4.43) Substitution of these new variables into Equations (4.40) and (4.41) results in the following, ( )dm sf f t T  (4.44) ( )ds mx x t T  (4.45) These are ideal command for the master and slave controllers to track. Transparency has been improved since the unwanted terms have been removed. The new system with modified wave variables is shown in Figure 4.10. Figure 4.10 Modified wave-based teleoperation system. (Adapted from [42]). Position-error Correction in Wave-based Teleoperation Poor position tracking results in sluggish behavior of the teleoperation system and it becomes worse with the increase in time delay. This problem, known as position drift in bilateral teleoperation, has been widely studied by researchers. 125 Passivity-based control schemes such as wave-based teleoperation provide stability against time delays and guarantee perfect velocity tracking. However, these approaches fail to guarantee perfect position and force tracking in steady state. This can be attributed to the fact that only power variables (velocity and force) are exchanged between the master and the slave sides, and the position information is not explicitly encoded. It is obtained by numerical integration of velocity, which is prone to error. There may also exist an offset in the initial conditions between the two sides. Data loss during transmission further aggravates the problem. Furthermore, it is typically hard to achieve perfect impedance matching between the master and the slave manipulators, which have dissimilar kinematics and dynamics. For example, in the homecare system that is considered in this thesis, a small and lightweight haptic interface communicates with a heavy and larger industrial manipulator. Even if the nonlinearities are perfectly dealt with (removed) through feedback linearization, the impedances on the two sides cannot be perfectly matched no matter what the value of the wave impedance b is. Even if the master and the slave manipulators are similar, the commonly used control methods such as impedance control and admittance control require exclusive position information for perfect motion tracking. This was observed in Chapter 3 where the position tracking capability of an admittance controller has been improved by incorporating the desired position information into the controller along with the desired velocity. For the Homecare system developed in the thesis, the proposed solution takes into account the exchange of position information along with the wave variables. Implementation of the wave-variable algorithm is difficult due to the inherent algebraic loops. A complete wave-variable algorithm is presented next to avoid these loops. It is assumed here that a position-force architecture is used for teleoperation. The desired force as received 126 from the slave is directly displayed to the human operator via the haptic interface. It may be modified depending on the control scheme (such as impedance control of the master). Similarly, the slave controller is a first-order impedance controller. The Complete Wave Variables Algorithm At the Master Side: 1. Receive mv from the slave 2. Modify mv as in Equation 4.43 3. Read measured position mx and velocity mx from the haptic device 4. Compute dmf using Equation 4.40 and input it to the haptic device 5. Compute mu using Equation 4.38 and send it via UDP to the slave side over the communication channel At the Slave Side: 1. Receive su from the master 2. Modify su as in Equation 4.42 3. Read measured position sx and velocity sx from slave manipulator 4. Compute dsx (assuming a PD control law is used as in Equation 4.47)  1 1(x ) ( )2 2( ) 22d d ds ps s s ds s s sdps s s ds s sdsdsx k x b x x ub bk x x b x buxb k        (4.46) 127 5. Compute dx by integrating dsx 6. Compute the PD control force term sf (and output it to the device)    = d ds ps s s ds s sf k x x b x x   (4.47) 7. Measure the interaction force ef 8. Compute sv using Equation (4.39) and send it via UDP to the master side 4.3 Experimental Results The same experimental setup as used in Chapter 3 is employed here, with the time delays emulated. Emulating Time Delay Network emulation tools emulate Wide Area Network (WAN) characteristics using a Local Area Network (LAN). They can be used to emulate time delay (both constant and random) and other communication constraints such as packet loss, packet corruption, packet reordering, congestion, and bandwidth limitation for application development and testing in areas such as teleoperation, haptics, online video gaming, and networked control systems. Although a variety of tools exist (such as NetEm, NISTNet, WANsim, and WANEm), in this thesis WANem [80] is used because of three main reasons: (1) it is freely available, (2) it allows setting rules for both incoming and outgoing packets on the WAMem router (PC), (3) it is easily configurable and is more realistic. WANem is based on the underlying netem tool of the Linux kernel, which is provided by Linux for testing protocols by emulating the network properties of wide area networks. The netem tool models the network delay as a sum of fixed delay, called latency, and 128 random delay, called jitter. The latter is a variation around the fixed delay and is modelled by probability density functions such as Normal or Pareto. A customized non-uniform distribution based on experimental data can also be specified. Figure 4.11 shows the experimental setup for teleoperation with time delay where a WAN Emulator PC, with WANem installed and configured, induces controlled delays into traffic between the Master and Slave Controllers. As shown, data between the master and slave ends is transmitted in the form of UDP/IP packets over a 100Mbps Ethernet via the WAN Emulator PC. In this thesis, only a constant time delay is emulated. No jitter, packet loss, duplication, and re-ordering is considered. Extensions of the wave variables approach to incorporate the effects of these properties exist in the literature. Figure 4.11 Delay emulation in haptic teleoperation through WANem. Experiments Two sets of experiments are performed, one for the average time delay of 200ms and the other for 400 ms. These are the magnitudes of worst-case delays for traffic over the Internet. According to the live Internet Weather Map™ [81] that displays and refreshes latency on the 129 Internet every 30 seconds, warnings are issued when the latency gets over 300 ms. The wave variable approach, however, can handle any arbitrary amount of delay. Results of the experiments are given in Figures 4.12 to 4.17, which show perfect position tracking, velocity tracking, and force feedback between the master and the slave ends. Figure 4.12 and Figure 4.13 show plots of the actual and the desired positions and velocities of the slave manipulator during tracking. The slave manipulator interacts with a virtual environment and sends the resulting interaction force back to the master, which is shown in Figure 4.14. Figure 4.12 Position tracking under a time delay of 200ms. 130 Figure 4.13 Velocity tracking under a time delay of 200ms. Figure 4.14 Force tracking under a time delay of 200ms. 131 Figure 4.15 Position tracking under a time delay of 400ms. Figure 4.16 Velocity tracking under a time delay of 400ms. 132 Figure 4.17 Force tracking under a time delay of 400ms. 4.4 Summary In this chapter, wave variable-based algorithm was used to stabilize a haptic teleoperation system with time delay. Results that produced stable position and force tracking were presented. It was shown that sending the desired position along with the wave variables can solve the position drift problem. This would also allow for integration of all the control schemes that were presented in the previous chapter. The issues of wave reflection and transparency have been solved, and the controller design of both master and slave manipulators can be carried out independent of the dynamics of the communication channel and by tuning the wave impedance parameter b. 133 Chapter 5: Conclusion and Future Work This thesis focused primarily on the use of an impedance control approach to design controllers for the master and the slave manipulators in a teleoperation system for elderly care in a home environment. An inverse dynamic model of the robot was identified using a learning-based approach that did not require the availability of knowledge about the existing model. The resulting impedance controller, which used the learned model in a feed forward manner, was implemented in real-time and was found to be capable of adapting to changes in the arm’s real dynamics. The desired impedance model was adaptively selected based on the estimated impedance of the unknown environment with which the slave manipulator was in contact. A method of admittance control was designed and its merits investigated compared to the standard impedance control based on experimental results. Lastly, the transparency and position synchronization of the popular wave variable approach was improved. This allowed stable position and force tracking in teleoperation under time delay that was induced by the communication channel during the exchange of information between the master and the slave ends. All the algorithms developed were validated in the context of homecare robotics through simulation and experimentation, and the results were presented and discussed. This chapter concludes the thesis with a description of the overall significance of the work and the key research contributions to the broader discipline of haptic teleoperation in the context of homecare robotics. Limitations and strengths of the work are discussed. Finally, possible directions for future research are reported, which may further improve the performance and effectiveness of a homecare robotic system. 134 5.1 Conclusions In this thesis, interaction control algorithms were investigated, designed, analyzed, and evaluated, which would ensure safe and compliant behavior of a robotic system in a home environment in giving routine care to the elderly or the disabled and assisting in daily tasks. Pure position-, force-, or hybrid control techniques cannot achieve the simultaneous conflicting goals of controlling the position and force, and are therefore not suitable for dealing with contact situations. Stiffness control has long been tried, but it is unable to cope with the nonlinear and coupled robot dynamics. Impedance control, therefore, has been a preferred method of choice for robotic control, especially in the homecare and service industry where the robot has to coexist with humans and delicate objects in its surroundings. Due to its known advantages over other approaches, practical application of impedance control in real-time is still a challenging task and therefore many applications in industry have still not benefited from it. This thesis has attempted to address these challenges. Chapter 2 of this thesis investigated the kinematic considerations that could ensure a successful implementation of impedance control. As the impedance control formulates the robot dynamic model in the operational space, the presence of singular configurations in a manipulator’s workspace can adversely affect the performance and stability of impedance control. The operational space formulation of dynamic model makes extensive use of the manipulator’s Jacobian matrix, which relates the joint velocities to the Cartesian velocities of the end-effector. At the singularity, the Jacobian matrix loses its rank which results in extremely large forces and torques, loss of stiffness, and ultimately instability of the control algorithm. Manipulability measures such as velocity and force ellipsoids were therefore used in the kinematic transformation of the master and slave motion in the teleoperation system. This helped 135 in finding optimal configurations (a subset of the complete workspace) of the slave manipulator that are away from singularities and therefore safe. The slave manipulator, prior to making contact with the environment, was put in one of these safe regions before tracking the motion commands through the haptic interface. Complete kinematic models of an industrial manipulator (WAM) and haptic interface (PHANToM) were also derived in Chapter 2, and an experimental testbed for haptic teleoperation was developed. A realistic implementation of impedance control is hard to achieve on manipulators that have significant dynamics. Traditionally this problem has been solved by feeding forward the inverse dynamic model, which is computed ahead of time based on the desired motion trajectory (positions, velocities, and accelerations). This results in reduced computational burden as the dynamic terms, which are now computed offline and stored in memory, just need to be fetched. However, like any feedforward control scheme, there is the inherent problem of lack of robustness with respect to parametric and structural uncertainties in the manipulator dynamics. If the inverse dynamic model is computed offline and never updated, the robustness property will not be achieved. In Chapter 3, a new impedance control algorithm was proposed which combined feedforward control with incremental online learning of the inverse dynamic model using machine learning techniques. No a-priori knowledge about the system dynamics was assumed. The learned model can not only be trained offline, but also updated in real-time and is therefore robust to any changes in system dynamics. For this purpose, in Chapter 2 of this thesis, a fast algorithm, LWPR, was used which is a statistical regression technique with learning capability to estimate the nonlinear inverse dynamic function. The learned inverse-dynamic feed-forward torque was provided to the inner torque loop of the impedance controller. The impedance control gains were then tuned to achieve the desired dynamic behavior. The resulting 136 arm had a “soft" feel and behaved in a compliant way as demonstrated by the experimental results on a commercial 4-DOF WAM arm and a PHANToM device (a commercial haptic interface). As WAM has significant dynamics compared to the haptic interface, its dynamic model was analytically derived and results of impedance control based on both models (analytical and experimental) were presented. Once the impedance control implementation in real-time was achieved, the next objective was to experimentally evaluate different types of impedance control that exist in the literature. In Chapter 3, three impedance control schemes were assessed in the laboratory on a commercially available industrial manipulator and a haptic interface. The evaluated methodologies were standard (second-order) impedance control, first-order impedance control, and admittance control. While first-order impedance control enjoyed easy implementation in real-time (based on feed-forward learned inverse dynamic model), it had a major drawback as observed from experimental results. The stiffness and damping parameters in the desired impedance model are normally selected in order to achieve a desired level of compliance or stiffness. In applications such as homecare robotics where a robot may encounter an environment of unknown stiffness, these parameters must be kept small in order for the robot to have an overall “soft" and safe feel in a desired Cartesian direction. However, this softness comes at the price of loosing the force- and position-tracking accuracy in that direction. On the other hand, if these controller gains are selected high in order to suppress the effect of nonlinear dynamics (which may still exist due to inaccuracy of the estimated model), the robot will feel stiff, which may be unsafe in a caregiving situation. This trade-off between compliance and position/force tracking accuracy makes the selection of the desired impedance model in standard and first-order impedance control extremely difficult. This problem was solved by using admittance control, which uses a separate 137 inner loop for achieving motion tracking and a different outer loop for shaping the dynamic behavior of the manipulator. Experiments and numerical simulations were carried out to verify the effectiveness of admittance control as compared with the other two schemes. Algorithm and guidelines for selecting the desired impedance parameters were also given. Finally, Chapter 4 presented an algorithm for improving transparency and tracking capabilities of the popular wave-variable approach that deals with time delay in haptic teleoperation. The wave-variable approach makes the teleoperation stable and insensitive to time delays by transmitting wave variables instead of velocities and forces through the communication channel. However, this stability comes at the price of degraded transparency where the force feedback that is displayed to the human operator on the master side is not exactly the same as intended. It was shown that the transmission of position information along with the wave variables can result in stable position and force tracking and therefore enhance transparency. This was verified through experimentation. 5.2 Contributions This thesis aimed at solving some of the challenges in successful implementation of robotic homecare systems. Specifically, it sought to ensure that the master and the slave manipulators behaved in as much compliant manner as possible. At the same time, the accuracy of motion tracking during interaction must not be compromised. Towards these goals, this thesis has made the following four key contributions: 1. The inverse dynamic model of a robot manipulator with significant dynamics was identified using a complete data-driven learning-based technique which did not assume any a-prior knowledge of the dynamic parameters. The developed model can be used and 138 updated online within the real-time control loop of the manipulator and therefore solves a major problem in the implementation of impedance control in haptic teleoperation. 2. An impedance control scheme based on learned-inverse dynamic model was proposed which could achieve the desired compliant behavior despite the parametric and structural uncertainties in the robot dynamic model. Experimental validation of the proposed controller was conducted on a 4-DOF WAM arm. In addition to this, three interaction control techniques: standard impedance control, first-order impedance control and admittance control, were designed and experimentally evaluated for their performance and applicability in homecare robotics. 3. Position synchronization and transparency of the popular wave variable-based approach for time-delay compensation in haptic teleoperation was improved. 4. A complete experimental testbed for haptic teleoperation with special focus on safety and reliability was developed. 5.3 Significance of the Work The algorithms proposed and developed in the current work are primarily intended for application in a robotic homecare system, which will become more common in the next two decades. However, the same algorithms can find applications in other fields as well. There is a shift in research and technology towards the development of more complex robotic systems such as humanoid robots or 2-armed robots that can have up to 40 degrees of freedom or more. Deriving kinematic and dynamic models for such complex robots analytically will not be possible. The complete data-driven learning framework developed in this thesis can therefore be a preferred solution for model-based control of such complex robotic systems. 139 Cooperative manipulation where several robots jointly manipulate an object is used in such applications as robotic surgical systems where humans and robots work cooperatively perform a useful task. Admittance control, which results in safe interaction, even in a stiff environment is desirable for interaction control. The desired admittance model can incorporate behavior for guided motion of cooperative manipulators. Other application areas where the developed interaction control algorithms can be useful include space robotics, telesurgery, and remote access to hazardous or difficult environments. 5.4 Limitations and Suggested Future Work There are several factors that limit the effectiveness of the algorithms and techniques developed in this thesis. In Chapters 2 and 3, the learned-dynamic model is expressed in the joint-space. Therefore it is not well-suited for use in an impedance control architecture. The reliance on inverse kinematics could not be eliminated. For redundant robots, the inverse kinematics is not only a challenging task, but is an ill-posed problem. A possible future topic of research would be to represent the learned model in the Cartesian space. The online identification of the environmental model in Chapter 3 may also be achieved using a learning framework. Many tasks in a home environment are repetitive and involve interaction with the same environment (for example, opening a door, or cleaning a floor). A set of models may be developed off line, one for each environment, and stored in memory. The online execution of a task could benefit from such a model base, which could be updated at the same time. This could be a direction of future work. Furthermore, task geometry could also be taken into consideration and a sophisticated task planner module could be added to the impedance control framework. Vision sensors may be incorporated for this purpose. 140 In impedance control, the desired dynamic behavior has be provided in the task space. Robot manipulators, on the other hand, find it hard to ensure smooth tracking of the desired trajectory in the Cartesian space. All industrial robots achieve perfect position tracking in the joint-space. This is further improved by interpolation and blending of the desired joint motion. In a haptic teleoperation setup, where the desired Cartesian trajectory for the slave manipulator is received from the master manipulator, trajectory optimization must be done before being tracked by the slave. Finally, in order to ensure stable and transparent force display at the haptic interface, model-mediated teleoperation [82] needs to be investigated further. Rather than directly sending the slave motion and force information to the master, a model of the environment may be transmitted to the master where it is displayed to the human operator. The slave then can receive commands from the master consistent with the model. Alternatively, the module of environmental model estimation on the slave side may be transferred to the master side, which can be used to provide the human operator with a predicted environmental force [83]. The environmental model can then be updated as and when the information resulting from interaction with the actual environment is received from the slave end. This would allow a stable and transparent teleoperation in the presence of substantial communication delays. 141 REFERENCES [1] Statistics Canada, The Canadian Population in 2011: Age and Sex, 2011 Census. The Ministry of Industry, Canada, 2012. [2] Statistics Canada, “Table 051-0001 - Estimates of population, by age group and sex for July 1, Canada, provinces and territories.” The Ministry of Industry, Canada, 2015. [3] Canadian Federation of Nurses Unions, “The Nursing Workforce,” 2013. [Online]. Available: https://nursesunions.ca/sites/default/files/2013.backgrounder.nursing_workforce.e.pdf. [Accessed: 10-Feb-2015]. [4] International Federation on Ageing, “Long Term Care and Technology,” 2012. [Online]. Available: http://www.ifa-fiv.org/wp-content/uploads/2012/11/som-2012-ltc-and-technology-final-report.pdf. [Accessed: 12-Mar-2015]. [5] L. Bos, A. Dumay, L. Goldschmidt, G. Verhenneman, and K. Yogesan, Eds., Handbook of Digital Homecare: Successes and Failures. Springer Berlin Heidelberg, Berlin, 2011. [6] S. Klanke, S. Vijayakumar, and S. Schaal, “A library for locally weighted projection regression,” J. Mach. Learn. Res., vol. 9, pp. 623–626, 2008. [7] S. Schaal and C. G. Atkeson, “Learning Control in Robotics,” IEEE Robot. Autom. Mag., vol. 17, no. 2, pp. 20–29, Jun. 2010. [8] N. Aghasadeghi, H. Zhao, L. J. Hargrove, A. D. Ames, E. J. Perreault, and T. Bretl, “Learning impedance controller parameters for lower-limb prostheses,” in 2013 IEEE/RSJ international conference on Intelligent robots and systems (IROS), 2013, pp. 4268–4274. [9] M. D. E. R. Roberto Calandra Serena Ivaldi and J. Peters, “Learning Inverse Dynamics Models with Contacts,” in 2015 IEEE International Conference on Robotics and Automation, 2015. [10] C. K. I. Williams and C. E. Rasmussen, “Gaussian processes for regression,” in Advances in Neural Information Processing Systems, D. S. Touretzky, M. C. Mozer, and M. E. Hasselmo, Eds. MIT Press, 1996, pp. 514–520. 142 [11] A. J. Smola and B. Schölkopf, “A tutorial on support vector regression,” Stat. Comput., vol. 14, no. 3, pp. 199–222, 2004. [12] S. Vijayakumar and S. Schaal, “Locally weighted projection regression: Incremental real time learning in high dimensional space,” in Proceedings of the Seventeenth International Conference on Machine Learning, 2000, pp. 1079–1086. [13] D. Nguyen-Tuong, M. Seeger, and J. Peters, “Model learning with local gaussian process regression,” Adv. Robot., vol. 23, no. 15, pp. 2015–2034, 2009. [14] D. Nguyen-Tuong and J. Peters, “Model learning for robot control: a survey,” Cogn. Process., vol. 12, no. 4, pp. 319–340, 2011. [15] B. D. Argall, S. Chernova, M. Veloso, and B. Browning, “A survey of robot learning from demonstration,” Rob. Auton. Syst., vol. 57, no. 5, pp. 469–483, 2009. [16] O. Sigaud, C. Salaün, and V. Padois, “On-line regression algorithms for learning mechanical models of robots: A survey,” Rob. Auton. Syst., vol. 59, no. 12, pp. 1115–1129, 2011. [17] C. Salaun, V. Padois, and O. Sigaud, “Learning forward models for the operational space control of redundant robots,” in From Motor Learning to Interaction Learning in Robots, Springer, 2010, pp. 169–192. [18] J. de la Cruz, D. Kulic, and W. Owen, “Learning inverse dynamics for redundant manipulator control,” in 2010 International Conference on Autonomous and Intelligent Systems (AIS), 2010, pp. 1–6. [19] J. S. De La Cruz, D. Kulić, and W. Owen, “Online incremental learning of inverse dynamics incorporating prior knowledge,” in Autonomous and Intelligent Systems, Springer, 2011, pp. 167–176. [20] J. de la Cruz, D. Kulić, and W. Owen, “Online Incremental Learning of Inverse Dynamics Incorporating Prior Knowledge,” in Autonomous and Intelligent Systems, vol. 6752, M. Kamel, F. Karray, W. Gueaieb, and A. Khamis, Eds. Springer Berlin Heidelberg, 2011, pp. 167–176. 143 [21] D. Nguyen-Tuong and J. Peters, “Online Kernel-Based Learning for Task-Space Tracking Robot Control,” Neural Networks Learn. Syst. IEEE Trans., vol. 23, no. 9, pp. 1417–1425, Sep. 2012. [22] D. Nguyen-Tuong, J. Peters, M. Seeger, and B. Schölkopf, “Learning inverse dynamics: a comparison,” in European Symposium on Artificial Neural Networks, 2008, no. EPFL-CONF-175477. [23] W. Khalil and E. Dombre, Modeling, identification & control of robots. Sterling, VA; London: Kogan Page Science, 2004. [24] R. Volpe and P. Khosla, “Computational considerations in the implementation of force control strategies,” J. Intell. Robot. Syst., vol. 9, no. 1–2, pp. 121–148, 1994. [25] M. Ueberle and M. Buss, “Control of kinesthetic haptic interfaces,” in IEEE/RSJ Int. Conf. on Intell. Robots and Syst., Workshop on Touch and Haptics, 2004. [26] B. Stanczyk, A. Peer, and M. Buss, “Development of a high-performance haptic telemanipulation system with dissimilar kinematics,” Adv. Robot., vol. 20, no. 11, pp. 1303–1320, 2006. [27] R. Kikuuwe, “A Sliding-Mode-Like Position Controller for Admittance Control With Bounded Actuator Force,” IEEE/ASME Trans. Mechatronics, vol. 19, no. 5, pp. 1489–1500, 2014. [28] D. Feth, A. Peer, and M. Buss, “Incorporating human haptic interaction models into teleoperation systems,” in 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2010, pp. 4257–4262. [29] P. Hokayem and M. Spong, “Bilateral teleoperation: An historical survey,” Automatica, vol. 42, no. 12, pp. 2035–2057, Dec. 2006. [30] P. Arcara, “Control schemes for teleoperation with time delay: A comparative study,” Rob. Auton. Syst., vol. 38, no. 1, pp. 49–64, Jan. 2002. [31] J. Park and O. Khatib, “A Haptic Teleoperation Approach Based on Contact Force Control,” Int. J. Rob. Res., vol. 25, no. 5–6, pp. 575–591, 2006. 144 [32] D. R. Madill, “Modelling and Control of a Haptic Interface : A Mechatronics Approach,” University of Wateloo, 1998. [33] K. Hashtrudi-Zaad and S. E. Salcudean, “Transparency in time-delayed systems and the effect of local force feedback for transparent teleoperation,” IEEE Trans. Robot. Autom., vol. 18, no. 1, pp. 108–114, 2002. [34] N. Hogan, “Impedance control-An approach to manipulation. I-Theory. II-Implementation. III-Applications,” ASME Trans. J. Dyn. Syst. Meas. Control, vol. 107, pp. 1–24, 1985. [35] J. H. Park and H. C. Cho, “Sliding-mode controller for bilateral teleoperation with varying time delay,” in 1999 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, 1999, pp. 311–316. [36] N. D. Do and T. Namerikawa, “Four-channel force-reflecting teleoperation with impedance control,” Int. J. Adv. Mechatron. Syst., vol. 2, no. 5–6, pp. 318–329, 2010. [37] A. Peer, “Design and Control of Admittance-Type Telemanipulation Systems,” Technical University Munich, Germany, 2008. [38] R. J. Anderson and M. W. Spong, “Bilateral control of teleoperators with time delay,” IEEE Trans. Automat. Contr., vol. 34, no. 5, pp. 494–501, 1989. [39] G. Niemeyer, “Telemanipulation with Time Delays,” Int. J. Rob. Res., vol. 23, no. 9, pp. 873–890, Sep. 2004. [40] N. Chopra and M. W. Spong, “Adaptive coordination control of bilateral teleoperators with time delay,” in 43rd IEEE Conference on Decision and Control (CDC), 2004, vol. 5, pp. 4540–4547. [41] D. Lee and M. W. Spong, “Passive Bilateral Teleoperation With Constant Time Delay,” IEEE Trans. Robot., vol. 22, no. 2, pp. 269–281, Apr. 2006. [42] E. J. Rodríguez-Seda, “Passive Transparency Compensation for Bilateral Teleoperators with Communication Delays,” J. Robot., vol. 2015, pp. 1–13, 2015. [43] D. Sun, F. Naghdy, and H. Du, “Transparent four-channel bilateral control architecture 145 using modified wave variable controllers under time delays,” Robotica, no. July 2014, pp. 1–17, 2014. [44] H. Li and K. Kawashima, “Experimental comparison of backdrivability for time-delayed telerobotics,” Control Eng. Pract., vol. 28, pp. 90–96, 2014. [45] B. Hannaford and J.-H. Ryu, “Time-domain passivity control of haptic interfaces,” IEEE Trans. Robot. Autom., vol. 18, no. 1, pp. 1–10, 2002. [46] J.-H. Ryu, D.-S. Kwon, and B. Hannaford, “Stable Teleoperation With Time-Domain Passivity Control,” IEEE Trans. Robot. Autom., vol. 20, no. 2, pp. 365–373, Apr. 2004. [47] H. Li and K. Kawashima, “Bilateral teleoperation with delayed force feedback using time domain passivity controller,” Robot. Comput. Integr. Manuf., 2015. [48] T. B. Teleoperation and S. Member, “Time Domain Passivity Controller for,” IEEE Trans. Haptics, vol. 8, no. 1, pp. 79–89, 2015. [49] V. Chawda and M. K. O’Malley, “Position Synchronization in Bilateral Teleoperation Under Time-Varying Communication Delays,” IEEE/ASME Trans. Mechatronics, vol. 20, no. 1, pp. 245–253, 2015. [50] Barrett Technology Inc., “Libbarrett C++ library.” [Online]. Available: http://support.barrett.com/wiki/Libbarrett. [Accessed: 09-Sep-2014]. [51] Barrett Technology Inc., “Whole Arm Manipulator (WAM) User Manual,” 2009. [Online]. Available: http://support.barrett.com/wiki/WAM. [Accessed: 15-Mar-2015]. [52] “CHAI 3D: An Open-Source C++ Library for Haptics.” [Online]. Available: http://www.chai3d.org. [Accessed: 01-Jun-2013]. [53] M. W. Spong, S. Hutchinson, and M. Vidyasagar, Robot modeling and control, vol. 3. Wiley New York, 2006. [54] R. J. Schilling, Fundamentals of Robotics: Analysis and Control. New York: Prentice Hall, 1992. 146 [55] “Barrett Technology Inc. Glossary.” [Online]. Available: http://www.barrett.com/glossary.htm. [Accessed: 09-Sep-2015]. [56] T. Yoshikawa, “Manipulability of robotic mechanisms,” Int. J. Rob. Res., vol. 4, no. 2, pp. 3–9, 1985. [57] S. L. Chiu, “Task compatibility of manipulator postures,” Int. J. Rob. Res., vol. 7, no. 5, pp. 13–21, 1988. [58] P. Corke, “Robotics Toolbox for MATLAB, release 9 [Software].” 2011. [59] J. J. Craig, Introduction To Robotics, Mechanics And Control, 3rd ed. Pearson Education, 2008. [60] E. N. Marongelli and K. A. Thoroughman, “The advantage of flexible neuronal tunings in neural network models for motor learning,” Front. Comput. Neurosci., vol. 7, no. 100, 2013. [61] S. Vijayakumar, A. D’souza, and S. Schaal, “Incremental online learning in high dimensions,” Neural Comput., vol. 17, no. 12, pp. 2602–2634, 2005. [62] S. Vijayakumar, “Locally Weighted Projection Regressin (LWPR) - a users manual.” 2001. [63] M. Tufail and C. W. de Silva, “Haptic Teleoperation using Impedance Control with Application in Homecare Robotics,” Control Intell. Syst., vol. 41, no. 3, 2013. [64] D. Erickson, M. Weber, and I. Sharf, “Contact Stiffness and Damping Estimation for Robotic Systems,” Int. J. Rob. Res., vol. 22, no. 1, pp. 41–57, 2003. [65] H. Il Son, T. Bhattacharjee, and D. Y. Lee, “Estimation of environmental force for the haptic interface of robotic surgery,” Int. J. Med. Robot. Comput. Assist. Surg., vol. 6, no. 2, pp. 221–230, 2010. [66] F. Mobasser and K. Hashtrudi-Zaad, “Transparent Rate Mode Bilateral Teleoperation Control,” Int. J. Rob. Res., vol. 27, no. 1, pp. 57–72, 2008. 147 [67] P. Ioannou and B. Fidan, Adaptive Control Tutorial. Cambridge University Press, Cambridge UK, 2007. [68] W. Khalil and E. Dombre, Modeling, identification and control of robots. Butterworth-Heinemann, 2004. [69] C. Hatzfeld and T. A. Kern, Engineering Haptic Devices: A Beginner’s Guide, 2nd ed. London: Springer London, 2014. [70] T. Imaida, Y. Yokokohji, M. Oda, T. Yoshikawa, and others, “Ground-space bilateral teleoperation of ETS-VII robot arm by direct bilateral coupling under 7-s time delay condition,” IEEE Trans. Robot. Autom., vol. 20, no. 3, pp. 499–511, 2004. [71] C. R. Wagner, N. Stylopoulos, P. G. Jackson, and R. D. Howe, “The benefit of force feedback in surgery: Examination of blunt dissection,” Presence teleoperators virtual Environ., vol. 16, no. 3, pp. 252–262, 2007. [72] B. Hannaford, “Stability and performance tradeoffs in bi-lateral telemanipulation,” in Proceedings IEEE International Conference on, Robotics and Automation, 1989, pp. 1764–1767. [73] F. B. Llewellyn, “Some fundamental properties of transmission systems,” Proc. IRE, vol. 40, no. 3, pp. 271–283, 1952. [74] A. Jazayeri and M. Tavakoli, “Revisiting Llewellyn’s absolute stability criterion for bilateral teleoperation systems under non-passive operator or environment,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2012, pp. 70–75. [75] G. A. V Christiansson and F. C. T. Van Der Helm, “The low-stiffness teleoperator slave - a trade-off between stability and performance,” Int. J. Rob. Res., vol. 26, no. 3, pp. 287–299, 2007. [76] G. Niemeyer and J.-J. E. Slotine, “Stable adaptive teleoperation,” IEEE J. Ocean. Eng., vol. 16, no. 1, pp. 152–162, Jan. 1991. [77] N. Diolaiti, G. Niemeyer, F. Barbagli, and J. K. Salisbury, “Stability of Haptic Rendering: Discretization, Quantization, Time Delay, and Coulomb Effects,” IEEE Trans. Robot., 148 vol. 22, no. 2, pp. 256–268, Apr. 2006. [78] G. Burdea, Force and touch feedback for virtual reality. New York: John Wiley and Sons, 1996. [79] H. Li and K. Kawashima, “Achieving Stable Tracking in Wave-Variable-Based Teleoperation,” IEEE/ASME Trans. Mechatronics, vol. 19, no. 5, pp. 1574–1582, 2014. [80] H. K. Kalitay and M. K. Nambiar, “Designing WANem : A Wide Area Network emulator tool,” in Third International Conference on Communication Systems and Networks (COMSNETS), 2011, pp. 1–4. [81] “Internet Weather Map v2.8.” [Online]. Available: https://www.internetweathermap.com/. [Accessed: 10-Sep-2015]. [82] P. Mitra and G. Niemeyer, “Model-mediated Telemanipulation,” Int. J. Rob. Res., vol. 27, no. 2, pp. 253–262, 2008. [83] A. Iqbal, “Stabilization Of Delayed Teleoperation Systems Using Time Domain Passivity Control,” University of Siegen, Germany, 2007. 149 Appendix A A.1 Specifications of the PHANToM Premium Device Workspace 381 W x 267 H x 191 D mm Footprint 330 W x 254 D mm Range of motion Lower arm movement pivoting at elbow Nominal position resolution 0.03 mm Backdrive friction 0.04 N Maximum exertable force (nominal position) 8.5 N Continuous exertable force (nominal position) 1.4 N Stiffness 3.5 N mm-1 Inertia (apparent mass at tip) - without encoder gimbal < 75 g Force feedback x, y, z Position sensing x, y, z (roll, pitch, yaw optional) Interface Parallel port Supported platforms Intel-based PCs A.2 Homogeneous Transformations and Jacobian of the PHANToM Device Based on the DH parameters of the PHANToM device, the following homogeneous transformation matrices can be achieved 150 1 1 2 2 1 2 2 3 2 3 2 2 31 1 2 2 2 2 2 3 2 3 2 2 31 2 320 0 0 00 0 0 0= , = , =0 1 0 0 0 1 0 0 0 1 00 0 0 1 0 0 0 1 0 0 0 1c s c s l c s c l ss c s c l s c s l cA A Al                                        3010 1 0 0 0 1 0 00 0 1 0 0 0 1 0= , =1 0 0 1 0 0 00 0 0 1 0 0 0 1mmToolT Tl                    1 1 3 3 1 13 3 230 1 2 31 1 3 1 3 30= =0 0 0 1mmm mmTool mToolmc s s c s xc s xT T A A A Ts c s c c x        where 1 1 1 2 2 3= ( )mx s l c l s 2 1 2 2 3= ( 1)mx l s l c  3 1 1 2 1 2 1 3=mx l c c l l c s  The Jacobian matrix mJ of PHANToM haptic interface is given below. 11 12 1321 22 2331 32 33( ) =mj j jJ j j jj j j      where 151 11 1 1 2 2 312 1 1 213 2 3 12122 1 223 2 331 1 1 2 2 332 1 1 233 2 1 3= ( )=== 0=== ( )==j c l c l sj l s sj l c sjj l cj l sj s l c l sj l c sj l c c  152 Appendix B B.1 Specifications of the WAM Arm Workspace 3.5 m2 Maximum Payload 4 kg Endtip velocity 1 m/s Mass of robot 25.6 kg Repeatability 50 μm Total joint friction 3 Nm Mechanical Stiffness 1.5 x 106 N/m Control Stiffness 5000 N/m Percent Backdrivability > 95% Motor Type Neodymium Iron Boron, brushless, DC servo motors 153 B.2 Homogeneous Transformations and Jacobian of the WAM Arm Based on the DH parameters of the WAM arm, the following homogeneous transformation matrices can be achieved 1 1 2 2 3 3 3 31 1 2 2 3 3 3 31 2 330 0 0 0 00 0 0 0 0= , = , =0 1 0 0 0 1 0 0 0 1 00 0 0 1 0 0 0 1 0 0 0 1c s c s c s a cs c s c s c a sA A Ad                                 4 4 3 44 4 3 4 440 1 0 0 00 0 1 0 0= , =0 1 0 0 0 0 10 0 0 1 0 0 0 1Tc s a cs c a sA TL                    11 12 13 121 22 23 20 41 2 3 431 32 33 3= =0 0 0 1sssTool sToolsr r r xr r r xT A A A A Tr r r x       where 11 4 1 3 1 2 3 1 2 412 3 1 1 2 313 1 4 2 4 1 3 1 2 321 4 1 3 2 3 1 1 2 422 1 3 2 1 323 4 1 3 2 3 1 4 1 231 2 4 3 4 232 2 333 2 4 3 2 41 3 1 2 4 1 3 1 2= ( )== ( )= ( )== ( )==== ( (sr c s s c c c c s sr c s c c sr c c s s s s c c cr c c s c c s s s sr c c c s sr s c s c c s c s sr c s c c sr s sr c c c s sx d c s L s s s c c         3 1 4 2 3 1 3 3 4 1 3 1 2 3 3 1 2 3 3 1 2 42 4 1 3 2 3 1 4 1 2 3 1 3 3 1 2 3 4 1 3 2 3 1 3 2 3 1 3 1 2 43 3 2 4 3 2 3 3 2 3 2 4 3 3 4 2) ) ( )= ( ( ) ) ( )= )ssc c c s a s s a c s s c c c a c c c a c s sx L s c s c c s c s s a c s d s s a c c s c c s a c c s a s s sx c s s d c a c s a c s a c c s                154 The Jacobian matrix sJ of WAM arm is given below where ikj represents the thjk element of matrix sJ . For clarity, the short notations = sin( )i is  and = cos( )i ic  ( =1, ,4i ) are used. 11 12 13 1421 22 23 2431 32 33 34( ) =sj j j jJ j j j jj j j j      where 11 3 4 1 3 2 3 1 3 1 3 3 1 2 4 1 3 2 3 1 4 1 2 3 2 3 1 3 1 2 412 1 3 2 2 4 3 3 2 3 2 4 3 2 4 3 3 4 213 3 1 1 2 3 3 4 3 414 3 1 4 2 3 4 1 3 1 2 3 4 1 3= ( ) ( ( ) )= ( )= ( )( )= ( ) ( (j a c c s c c s a c s d s s L s c s c c s c s s a c c s a s s sj c d c Lc c a c s a c s Lc s s a c c sj c s c c s a Ls a cj a c c s a s s s c c c L c s s                  1 2 3 1 2 421 3 1 2 4 1 3 1 2 3 1 4 2 3 1 3 3 4 1 3 1 2 3 3 1 2 3 3 1 2 422 1 3 2 2 4 3 3 2 3 2 4 3 2 4 3 3 4 223 1 3 2 1 3 3 4 3 424 4 1 3 2 3 1 1 2) )= ( ( ) ) ( )= ( )= ( )( )= ( ( )c c c c s sj d c s L s s s c c c c c s a s s a c s s c c c a c c c a c s sj s d c Lc c a c s a c s Lc s s a c c sj c c c s s a Ls a cj L c c s c c s s s s               4 3 4 1 3 2 3 1 3 4 1 23132 3 2 3 4 3 2 3 2 3 3 2 4 4 2 2 3 4233 2 3 3 4 434 3 2 4 2 4 3 4 2 3 3 2 4) ( )= 0= ( )= (2 sin( / 2) )= ( )a s c s c c s a c s sjj a c c c d s a c c a s s L c s c c sj s s a Lsj a c c L c s c c s a c s s         155 B.3 WAM Inertial Specifications and Code in the MATLAB Robotics Toolbox % MDL_WAM Creates model of Barrett WAM 4DOF manipulator % % mdl_wam % % Script creates the workspace variable wam which describes the % kinematic and dynamic characteristics of a 4-DOF WAM manipulator % using standard DH conventions. % % Also define the workspace vectors: % qz: zero joint angle configuration, arm straight up % qSample: arm is at a nominal non-singular configuration % % Author: Muhammad Tufail, IAL, UBC Canada % % The following license statement is for using the Robotic Toolbox. % Copyright (C) 1993-2014, by Peter I. Corke % % This file is part of The Robotics Toolbox for MATLAB (RTB). % % RTB is free software: you can redistribute it and/or modify % it under the terms of the GNU Lesser General Public License as published by % the Free Software Foundation, either version 3 of the License, or % (at your option) any later version. % % RTB is distributed in the hope that it will be useful, % but WITHOUT ANY WARRANTY; without even the implied warranty of % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % GNU Lesser General Public License for more details. % % You should have received a copy of the GNU Leser General Public License % along with RTB. If not, see <http://www.gnu.org/licenses/>. % % http://www.petercorke.com clear L deg = pi/180; % From document "WAM_InertialSpecifications_AC-02.pdf" % Link 1 L(1) = Revolute('d', 0, 'a', 0, 'alpha', -pi/2, ... % I is the inertia tensor = [Ixx Iyy Izz Ixy Iyz Ixz] 'I', [134880.33e-6, 113283.69e-6, 90463.30e-6, -2130.41e-6, 685.55e-6, -124.85e-6], ... % r is the center of mass = [rx ry rz] 'r', [-4.43e-3, 121.89e-3, -0.66e-3], ... % m is mass of the link 'm', 10.7677, ... 'Jm', 0, ... 'G', 0, ... 'B', 0, ... 'Tc', [0 0], ... % qlim are joint limits 'qlim', [-150 150]*deg ); % joint limits 156 % Link 2 L(2) = Revolute('d', 0, 'a', 0, 'alpha', pi/2, ... 'I', [21409.58e-6, 13778.75e-6, 15589.06e-6, 271.72e-6, -1819.20e-6, 24.61e-6], ... 'r', [-2.37e-3, 31.06e-3, 15.42e-3], ... 'm', 3.8749, ... 'Jm', 0, ... 'G', 0, ... 'B', 0, ... 'Tc', [0 0], ... 'qlim', [-113 113]*deg ); % Link 3 L(3) = Revolute('d', 0.5500, 'a', 0.045, 'alpha', -pi/2, ... 'I', [59110.77e-6, 3245.50e-6, 59270.43e-6, -2496.12e-6, -17.67e-6, 7.38e-6], ... 'r', [-38.26e-3, 207.51e-3, 0.03e-3], ... 'm', 1.8023, ... 'Jm', 0, ... 'G', 0, ... 'B', 0, ... 'Tc', [0 0], ... 'qlim', [-157 157]*deg ); % Link 4 L(4) = Revolute('d', 0, 'a', -0.045, 'alpha', pi/2, ... 'I', [18485.77e-6, 18916.58e-6, 1975.17e-6, 2.19e-6, 5.15e-6, -1608.68e-6], ... 'r', [10.95e-3, -0.03e-3, 140.54e-3], ... 'm', 1.0651, ... 'Jm', 0, ... 'G', 0, ... 'B', 0, ... 'Tc', [0 0], ... 'qlim', [-50 180]*deg); % Link 5 (Toolplate) L(5) = Revolute('d', 0.35, 'a', 0, 'alpha', 0, ... 'I', [0, 0, 0, 0, 0, 0], ... 'r', [0, 0, 0], ... 'm', 0, ... 'Jm', 0, ... 'G', 0, ... 'B', 0, ... 'Tc', [0 0], ... 'qlim', [0 0]*deg); % Define the SerialLink object wam = SerialLink(L, 'name', 'WAM 4DOF', ... 'manufacturer', 'Barret', 'comment', 'WAM 95, Document: D1005, 2008'); % some useful poses % qZero = [0 0 0 0 0]; % zero angles, L shaped pose 157 qs = [0 0 -pi/2 0 0]; qn = [0 pi/4 pi 0 0]; qSample = [-0.0160, 1.0300, -0.0310, 1.5850 0]; clear L
Thesis/Dissertation
eng
Mechanical Engineering
Vancouver : University of British Columbia Library
Attribution-NonCommercial-NoDerivs 2.5 Canada
Graduate
Haptic teleoperation with impedance control based on learned inverse dynamics with application in homecare robotics
Text