Intelligent Control of Robots with Mismatched Dynamics and Mismatched Sensing. Wenjie Chen

Transcription

1 Intelligent Control of Robots with Mismatched Dynamics and Mismatched Sensing by Wenjie Chen A dissertation submitted in partial satisfaction of the requirements for the degree of Doctor of Philosophy in Engineering - Mechanical Engineering in the Graduate Division of the University of California, Berkeley Committee in charge: Professor Masayoshi Tomizuka, Chair Professor J. Karl Hedrick Professor Pieter Abbeel Fall 212

2 Intelligent Control of Robots with Mismatched Dynamics and Mismatched Sensing Copyright 212 by Wenjie Chen

3 1 Abstract Intelligent Control of Robots with Mismatched Dynamics and Mismatched Sensing by Wenjie Chen Doctor of Philosophy in Engineering - Mechanical Engineering University of California, Berkeley Professor Masayoshi Tomizuka, Chair Interest in industrial automation by robots has been continuously growing ever since the first industrial robot was installed in This creates an increasing demand for robot performance enhancement by means of intelligent control. Difficulties in meeting stringent performance requirements, however, arise by some inherent characteristics of the robots, which are: 1) mismatched dynamics (i.e., the control input and the system uncertainty/disturbance are in different channels), and 2) mismatched sensing (i.e., the system lacks of direct sensing of the desired output). A typical example of mismatched systems is an industrial robot with joint elasticity, the control objective of which is to obtain desired performances of the end-effector such as accurate positioning and tracking. This kind of robot can be characterized as a two-mass system with one mass being the motor side mass with control input and sensing and the other the load side/end-effector mass without control input or sensing, respectively. In this dissertation, several key technologies on intelligent control for this type of robot are introduced, including 1) handling mismatched sensing by sensor fusion, 2) handling mismatched dynamics by real-time control, and 3) handling mismatched dynamics by iterative learning control. This dissertation develops several sensor fusion algorithms to estimate both the load side and the end-effector states from limited sensing. The dynamic and kinematic models are utilized with consideration to problems such as fictitious noises and computation requirement. The well-developed sensor fusion schemes provide the essential desired information of the system for control purposes. With the mismatched sensing problem solved by sensor fusion, the control approaches to handle the mismatched dynamics are investigated in both the realtime domain and the iteration domain. Considering the nature of the mismatched system as a two-mass system, the real-time and iterative approaches are utilized in a hybrid two-stage manner to deal with the mismatched dynamics. The developed algorithms for sensor fusion and control provide not only the theoretical insights, but also the capability to address the real-world problems especially in industrial automation. Experimental studies on a single-joint research testbed and a commercial 6-DOF industrial robot manipulator have demonstrated the superior performance of the proposed algorithms.

4 To my family i

5 ii Contents Contents List of Figures List of Tables List of Common Symbols ii vii x xi 1 Introduction Background Motivation and Contribution Modeling Mismatched System Handling Mismatched Sensing: Sensor Fusion Approaches Handling Mismatched Dynamics: Real-time Control Approaches Handling Mismatched Dynamics: Iterative Learning Control Approaches Appearance of Research Contributions in Publications Dissertation Outline Robots with Mismatched Dynamics and Mismatched Sensing Introduction Dynamic Modeling of Mismatched Robots Mismatched System Model Single-Joint Robot Model Multi-Joint Robot Model Experimental Setup Single-Joint Robot Experimental Setup Multi-Joint Robot Experimental Setup Chapter Summary

6 iii I Handling Mismatched Sensing: Sensor Fusion Approaches 28 3 Load Side State Estimation in Single-Joint Robot Introduction Estimation Algorithms Model for Measurement Dynamics System Dynamic Model System Kinematic Model Kalman Filtering Estimation of Noise Covariance Experimental Study Experimental Setup Load Side Estimation Experiments Discussion Chapter Summary End-effector State Estimation in Multi-Joint Robot Introduction MD Kinematic Model MD Rigid Body Motion Sensor Measurement Dynamics State-Space Model Representation MD Kinematic Kalman Filter Prediction by Inertia Sensor Measurement Propagation of Estimation Errors Correction by Vision Sensor Measurement Experimental Study Sensor Fusion Using IMU & CompuGauge 3D Sensor Fusion Using IMU & PSD Camera Chapter Summary Load Side Joint State Estimation in Multi-Joint Robot Introduction Robot Inverse Differential Kinematics Dynamic Model for Rough Estimates Basic Differential Kinematics Optimization Based Inverse Differential Kinematics Final Optimization Solution Practical Implementation Issues Kinematic Kalman Filter Decoupled Kinematic Kalman Filter

7 iv Parameter Estimation Discussion of the Approach Computation Load Extensions to Other Sensor Configurations Simulation & Experimental Study Test Setup Algorithm Settings Simulation Results Experimental Results Chapter Summary II Handling Mismatched Dynamics: Real-time Control Approaches 8 6 Hybrid Adaptive Friction Compensation in Single-Joint Robot Introduction System Overview Single-Joint Indirect Drive Model Modified Friction Model Controller Structure Friction Compensation Motor Side Friction Compensation Load Side Friction Compensation Hybrid Compensator Structure Experimental Results Experimental Setup Motor Side Friction Compensation Load Side Friction Compensation Chapter Summary Motor Side Disturbance Rejection in Multi-Joint Robot Introduction Robot Model Study Transfer Function Analysis Inertia Variation and Model Simplification Controller Design Baseline Controller Design Feedforward Controller Design PID Controller Design Disturbance Observer (DOB) Based Disturbance Rejection Adaptive Robust Controller (ARC) Based Disturbance Rejection

8 v Transfer Function Analysis Experimental Study Experimental Setup Experimental Results Chapter Summary III Handling Mismatched Dynamics: Iterative Learning Control Approaches Hybrid Two-Stage Model Based Iterative Learning Control Scheme for MIMO Mismatched Linear Systems Introduction System Overview System Model Basic Controller Structure Two-Stage ILC Scheme ILC Basics ILC With Reference Update ILC With Torque Update Hybrid Scheme With Two-Stage ILC Experimental Study Experimental Setup & Dynamic Model System Disturbance Characteristics Algorithm Setup Experimental Results Chapter Summary Iterative Learning Control with Sensor Fusion: Application to Multi-Joint Robot Introduction System Overview Robot Dynamic Model Robot Controller Structure ILC Scheme with Sensor Fusion Two ILC Stages Hybrid Scheme For Two-Stage ILC Robot Load Side State Estimation Experimental Study Test Setup Algorithm Setup Experimental Results Chapter Summary

9 vi 1 Concluding Remarks and Open Issues Concluding Remarks Future Avenues of Research Bibliography 148 IV Appendix 156 A Robot System Setup 157 A.1 Robot Controller Hardware Configuration A.2 Real-Time System A.3 Payload Design A.4 Sensor Configurations A.4.1 Accelerometer A.4.2 Inertia Sensor Setup A.4.3 PSD Camera A.4.4 CompuGauge 3D A.5 Robot Simulator & Experimentor A.5.1 Robot Simulator A.5.2 Robot Experimentor B System Identification 171 B.1 Linear Dynamics Identification of Multi-Joint Robot B.1.1 Posture Selection B.1.2 Identification Procedure B.1.3 Closed Loop Identification B.2 Friction Identification B.2.1 Friction Identification for Multi-Joint Robot B.2.2 Friction Identification for Single-Joint Setup C Covariance Estimation 182 C.1 Covariance Estimation in Chapter C.2 Parameter Estimation in Chapter C.2.1 Maximizing Expected Likelihood C.2.2 Off-line Solution: EM Algorithm C.2.3 Online Solution

10 vii List of Figures 1.1 The Structure Overview of the Dissertation Single-Joint Indirect-Drive Mechanism Definition of the Transmission Error Single-Joint Indirect Drive System Setup Accelerometer Installation FANUC M-16iB Robot System FANUC M-16iB Robot System Setup Scheme Reference Configuration for FANUC M-16iB Robot Angle Conventions (left) and Attached Frames (right) Home Position for FANUC M-16iB Robot θ l (s) 3.1 Bode Plot of for the System Setup in Fig λ θ m (s) Structure of Dynamic Model Based Kalman Filter (DKF) Structure of Kinematic Model Based Kalman Filter (KKF) Chirp Input Signal Frequency Estimation Errors of Load Side Position in Chirp Experiment (Without Covariance Adaptation or Parametric Uncertainty) Estimation Errors of Load Side Position in Chirp Experiment (With Covariance Adaptation and Parametric Uncertainty) Covariance Estimation in Chirp Experiment Bias Estimation in Chirp Experiment Load Side Desired Trajectory Estimation Errors of Load Side Position in Trajectory Experiment Load Side Actual and Estimated Position Tracking Error Sensor Configuration and Coordinate System of FANUC M-16iB Robot End-effector Basic Structure of MD-KKF Scheme TCP Estimation for P2P Scanning Trajectory (with IMU & CompuGauge) Measurement Comparison Between PSD Camera and CompuGauge TCP Estimation for Square Trajectory without Orientation Change TCP Estimation for Circle Trajectory without Orientation Change

11 viii 4.7 End-effector Path with Orientation Change Locations of Two PSD Markers and the TCP TCP Estimation for Curve Path with Orientation Change Adaptive Kinematic Kalman Filter Process The Structure of Load Side State Estimation Approach Y-Z Plane TCP Position Estimation (Experiment) Load Side Joint Position Estimation Absolute Error (Simulation) Load Side Joint Velocity Estimation Absolute Error (Simulation) Load Side Joint Acceleration Estimation Absolute Error (Simulation) TCP Estimation on Y & Z Axes When Coming to a Stop (Experiment) TCP Estimation Error When Coming to a Stop (Experiment) Block Diagram of the Overall Control System Hybrid Compensator Structure Desired Load Side Trajectory Performance of the Motor Side Compensators Friction Estimates by FB-A Observer Friction Parameter Estimations (SI Units) Performance of the Hybrid Compensator Load Side Friction Estimates Frequency Responses of s 2 P i and s 2 P i for Each Robot Joint ld l ld 7.2 Variations of M n2 of the FANUC M-16iB Robot in the Work Space Frequency Responses of Joint Models of the FANUC M-16iB Robot (Solid Color Lines: Indirect Drive Model sp i ; Black Marker Line: Simplified Model sp i ) mu musim 7.4 Controller Structure of the Multi-Joint Robot PID Controller Structure of the Multi-Joint Robot DOB Controller Structure of the Multi-Joint Robot ARC Controller Structure of the Multi-Joint Robot TCP Cartesian Space Trajectory Reference TCP Cartesian Space Position Tracking Performance Motor Side Position Tracking Error (in Load Side Scale) Motor Position Error Spectrum Torque Profile Comparison Control Structure with Reference & Torque Update Load Side Disturbance Setup for Single-Joint System Load Side Desired Trajectory Disturbance Effects on Load Side Position Tracking Error Frequency Responses ofβ r andβ u with±5% Parametric Uncertainties Performance Comparisons using Accurate Nominal Model (After 1 Iterations)

12 ix 8.7 Error Convergence Comparisons using Nominal Model with 15% Parametric Uncertainties (1 Iterations) Performance Comparisons using Nominal Model with 15% Parametric Uncertainties (After 1 Iterations) Robot Control Structure with Reference & Torque Update TCP Position RMS Error Comparisons in Iteration Domain IMU Acceleration RMS Error Comparisons in Iteration Domain TCP Position Error Comparisons in Time Domain IMU Acceleration Error Comparisons in Time Domain A.1 FANUC M-16iB Robot System Setup Scheme A.2 Payload Mounted on Joint A.3 Payload Surface Straightness A.4 Inertia Sensor Setup Configuration A.5 Structure of the PSD Camera Prototype A.6 Precision Levels for Measurement Planes at Different Distances A.7 M-16iB Robot Simulator & Experimentor A.8 Testing Position Trajectory Comparison between Simulation and Experiment (Load Side Scale) A.9 Motor Torque Comparison between Simulation and Experiment A.1 Virtual Reality in Simulator vs. Actual Experiment (FANUC M-16iB) A.11 Virtual Reality in Simulator vs. Actual Experiment (Single-Joint Setup) B.1 FANUC M-16iB Robot Postures for System Identification B.2 System Identification Flow Chart for FANUC M-16iB Robot B.3 Static Friction Identification Result for FANUC M-16iB Robot with Payload B.4 Static Friction Identification Result for FANUC M-16iB Robot without Payload B.5 Static Friction Identification Result for Single-Joint Testbed

13 x List of Tables 2.1 Identified Parameters for the Single-Joint Robot DH Parameters for FANUC M-16iB Robot The Noise Variance Used in Chirp Experiment (SI Units) Identified Variances for the IMU Measurements for Sensor Fusion Using IMU & CompuGauge 3D Identified Variances for the IMU and PSD Camera Measurements for Sensor Fusion using IMU & PSD Camera TCP Estimation Errors When Coming to a Stop (Experiment) Improvements Compared to the No-Comp. Case The Comparison Between DOB and ARC A.1 Specification of the Payload A.2 Covariance of the Inertia Sensor Signals (in IMU Coordinates) B.1 Identified Friction Parameters for FANUC M-16iB Robot (SI Units) B.2 Identified Friction Parameters for Single-Joint Testbed (SI Units)

14 xi List of Common Symbols N P ld P lu P md P mu d d l d m θ l θ ld θ m θ md θ J l J m d fl d f m d j d l d m f h f l Reduction ratio of the gear reducer (single-joint/multi-joint robot) Transfer function from the disturbance d to the load side positionθ l (or q l for multijoint robot) Transfer function from the torque u (orτ m for multi-joint robot) to the load side positionθ l (or q l for multi-joint robot) Transfer function from the disturbance d to the motor side positionθ m (or q m for multi-joint robot) Transfer function from the torque u (orτ m for multi-joint robot) to the motor side positionθ m (or q m for multi-joint robot) Lumped (fictitious) disturbance (single-joint/multi-joint robot) Load side lumped (fictitious) disturbance (single-joint/multi-joint robot) Motor side lumped (fictitious) disturbance (single-joint/multi-joint robot) Load side position (single-joint robot) Load side desired position reference (single-joint robot) Motor side position (single-joint robot) Motor side desired position reference (single-joint robot) Reducer transmission error (single-joint robot) Load side inertia (single-joint robot) Motor side inertia (single-joint robot) Load side external disturbance (single-joint robot) Motor side external disturbance (single-joint robot) Joint reducer damping (single-joint robot) Load side damping (single-joint robot) Motor side damping (single-joint robot) Friction in the gear reducer (single-joint robot) Load side friction (including Coulomb friction and viscous damping) (single-joint robot)

15 xii f lc f m f mc k j u q q l q ld q m q md q Load side Coulomb friction (single-joint robot) Motor side friction (including Coulomb friction and viscous damping) (single-joint robot) Motor side Coulomb friction (single-joint robot) Joint reducer stiffness (single-joint robot) Motor side torque (single-joint robot) State vector for multi-joint robot model Load side position (multi-joint robot) Load side desired position reference (multi-joint robot) Motor side position (multi-joint robot) Motor side desired position reference (multi-joint robot) Reducer transmission error (multi-joint robot) C(q l, q l ) Coriolis and centrifugal force matrix (multi-joint robot) G(q l ) Gravity torque vector (multi-joint robot) J(q l ) Jacobian matrix mapping from the load side joint space to the end-effector Cartesian space (multi-joint robot) J(q l, q l ) Time derivative of the Jacobian matrix J(q l ) (multi-joint robot) M l (q l ) Load side inertia matrix (multi-joint robot) M m τ m D j D l D m f ex t F lc F mc K j I n Motor side inertia matrix (multi-joint robot) Motor side torque vector (multi-joint robot) Joint reducer damping matrix (multi-joint robot) Load side damping matrix (multi-joint robot) Motor side damping matrix (multi-joint robot) End-effector external contact force/torque vector (multi-joint robot) Load side Coulomb friction matrix (multi-joint robot) Motor side Coulomb friction matrix (multi-joint robot) Joint reducer stiffness matrix (multi-joint robot) An n n identity matrix

16 xiii Acknowledgments My five years in Berkeley have been the greatest part in my life so far. I know that I could not have reached to this stage without the precious help and support from many people. I hope to recognize the wonderful moments with all these people as much as possible even though I realize that it is not an easy job within this limited space. First of all, I want to express my earnest gratitude and sincere respect to my research advisor Professor Masayoshi Tomizuka. Professor Tomizuka has been a remarkably amazing mentor not only in academic research but also in life experience throughout my Ph.D. student career. The persistent encouragement, the profound knowledge, and the valuable suggestions that he has provided to me have greatly shaped my Ph.D. study and beyond. His huge enthusiasm on the work, insightful vision for the student, and also enormous courage to face the reality have set up a great example for me to follow. I would also like to thank Mrs. Miwako Tomizuka for her kindness and care to my life. I am also very grateful to Professor J. Karl Hedrick and Professor Pieter Abbeel for their willingness to serve as my dissertation committee. Their kind support has motivated me to further improve the quality of this dissertation. I am also thankful to my undergraduate s advisors, Professor Bin Yao, Professor Qingfeng Wang, and Professor Linyi Gu, for their valuable guidance and precious opportunities that led me to this academic area. Special thanks go to FANUC Ltd, the sponsor for the work in this dissertation. I would like to thank Dr. Seiuemon Inaba and Dr. Yoshiharu Inaba sincerely for their continuing interest in this research and their encouragement. I also appreciate the valuable technical discussions/communications with Dr. Kiyonori Inaba and Dr. Shinsuke Sakakibara that have helped to enhance this dissertation with industrial perspective. I also appreciate Chiang Chen Overseas Graduate Fellowship for its generous support during my first year Ph.D. study. I have also greatly benefited from being a member of the Mechanical Systems Control (MSC) Laboratory. I am very fortunate to interact with all the MSC talents in the last five years. Special thanks go to Evan Chang-Siu (also Kaori Noguchi and cutie baby Kenta Siu). I will not forget all those wonderful moments we shared together in study, research, and life. They have truly helped me to adapt and shape my multi-cultural perspectives of the life. Also I am greatly thankful to Kyoungchul Kong, who is like my big brother and has provided so many precious experiences and suggestions to my research and life. I really appreciate his encouragement and support for the great time and also difficult time I have gone through. I hope we can both succeed in our dreams. I would also like to thank Sumio Sugita who always knows how to make practical things to work and helped me patiently with his expertise. I also appreciate the inspiring technical discussions/collaborations on the FANUC project with Cheng-Huei (Nora) Han, Kiyonori Inaba, Haifei Cheng, Chun-Chih Wang, Soo Jeon, Pedro Mora-Reynoso, Mike Chan, Cong Wang, and Chung-Yen Lin. The collaborations with Jonathan Asensio on the neural network learning and with Miguel Lagullon Garcia on the 3D animation were also enjoyable. I will not forget the collaborations with Chi-Shen Tsai and Dae-Kyu Yun on the Hyundai project. The ongoing

17 collaborations with Joonbum Bae and Junkai Lu on the BMI project have also been delightful. I would also like to thank all the other current and past colleagues at the MSC Lab: Shuwen (Emma) Yu, Nancy Feng Dan Dong, Hoday Stearns, Qixing Zheng, Xuan Fan, Guoyuan Wu, Sandipan Mishra, Takashi Nagata, Sanggyum Kim, Xu Chen, Yizhou Wang, Wenlong Zhang, Kanjanapas (Oak) Kan, Raechel Tan, Pey Yuen Tao, Benjamin Fine, Yasuyuki Matzuda, Kiyotaka Kawashima, Atsushi Oshima, Atsuki Naka, Ahmed Hamdy El-shaer, Minghui Zheng, Chen-Yu Chan, Omar Abdul-hadi, Robert Matthew, Hiroshi Niki, Ric Jacobs, and all others. Also, I cannot forget all the other friends in Berkeley. Especially, I am thankful to Heng Pan, Weiya Fang, Liang Pan, Ran Liu, Yudong Ma, and Zhichao Song for their warm-hearted friendship. I will also always remember all the friends back in China, both from my hometown and from my undergraduate college, with whom I always had a great time whenever I was visiting. Last and foremost, I would like to express my deepest love to my parents, Naihui Chen and Ailing Chen, and my sister, Wenting Chen, for their unconditioned love, support, and encouragement throughout my whole life. I am, and will also be greatly indebted to my fiancee, Su Chen, for her enormous love, understanding, and dedication. You all are the reasons of my life that have made me achieve so far, and I love you all. xiv

18 1 Chapter 1 Introduction 1.1 Background The robot, as implied by its other name "automata", is a self-operating machine meant to automate human s work for the purpose of freeing human from monotonous or dangerous work and lowering the production cost. Since the first industrial robot was installed in 1961 [96], robots have continuously shaped working environment for more than 5 years in the area of industrial automation and beyond. The ever growing interest in robot utilization has also imposed an increasing demand for intelligent control techniques to enhance robot performance, in particular, the motion control performance. Difficulties in meeting stringent performance requirements, however, are directly caused by some inherent characteristics of the robots, which are: 1) mismatched dynamics (i.e., the control input and system uncertainty/disturbance are in different channels), and 2) mismatched sensing (i.e., the system lacks of direct sensing of the desired output). In robot motion control, these mismatched problems arise in the motion and torque transmission mechanisms. There are two types of transmission mechanisms between the actuator and the load: direct drive and indirect drive. In direct drive, the actuator and the load are rigidly connected. In indirect drive, the actuator and the load are connected via gears and/or belts for torque amplification and speed reduction. This allows the use of high speed and low torque actuators which result in various advantages such as cost and weight reduction. Thus, the indirect drive mechanism is the most popular means of motion/torque transmission in industrial applications[74]. In some specific applications such as the robot manipulator, the robot joint with an indirect drive mechanism is also referred to as an elastic joint, or a flexible joint, to indicate that there exists some elasticities/flexibilities/discrepancies between the actuator and the output load. In contrast, the joint with a direct drive mechanism is thus termed as a rigid joint indicating that the actuator and the output load are rigidly connected without any discrepancies in between. In this dissertation, the electric motor, which is most commonly used in drive trains, is used as the modeling example to illustrate the mismatched system dynamics. The two parts separated

19 CHAPTER 1. INTRODUCTION 2 by the indirect drive mechanism are termed as the motor side and the load side, respectively, with the working tool (i.e., end-effector) rigidly attached to the last joint of the robot. Therefore, in industrial robot applications, the ultimate objective is to achieve the desired performance at the end-effector. For simplicity and efficiency in industrial control, this is normally done by decentralized joint space control to achieve equivalent desired performance on the load side[58, 74]. Load side sensing at the joints, however, is usually not available due to cost and assembly issues. This leads to the mismatched sensing problem. On the other hand, the control input is supplied only at the motor side where the actuator is located, while the uncertainties and/or disturbances normally appear from the load side/end-effector. Thus, the desired end-effector performance cannot be guaranteed with only good motor side performance[19, 91]. This results in the mismatched dynamics problem. The fundamental problem associated with these mismatches is that the desired variable cannot be directly sensed or controlled. This sets severe challenges in meeting stringent performance requirements. To solve this problem in a cost-effective and timely manner, a mechatronic approach, which gives considerations to both mechanical hardware and servo software, should be pursued[81]. Therefore, in this dissertation, the mismatched problems are properly tackled from the perspectives of both estimation and control, with contributions to hardware (sensor) configuration, dynamic modeling, algorithm developments, and real-world experimentation. 1.2 Motivation and Contribution Modeling Mismatched System Mismatched systems take various forms in the physical realization. The actuator may be an electric motor, a pneumatic actuator, a hydraulic piston, and so on. The transmission mechanism can be realized as various kinds of gear boxes, belt-pulley sets, and so on. In terms of motion and torque transmissions, however, they all share the same characteristic, and hence the indirect drive train/elastic joint is usually modeled as a system of two masses connected by spring (and damper)[92, 97, 72]. Based on this unifying model, the dynamics of robots with mismatched dynamics and mismatched sensing is studied in Chapter 2. Different model representations such as equations of motion, state space model, and transfer function model are formulated. The nonlinear dynamics such as friction effects, transmission error, and external disturbances are also considered. This unified model formulation provides the foundation for the synthesis of model based sensor fusion and control algorithms. In addition, a novel approach is proposed to decouple the highly nonlinear coupling dynamics for multi-joint robots. The decoupled linear form is essential for decentralized estimation and control using linear system theory/techniques. In addition to the theoretical work for dynamic modeling, an original multi-functional Robot Simulator/Experimentor with high fidelity physical modeling is also developed. This creates a practical and safe environment for both simulation and experimentation. To obtain

20 CHAPTER 1. INTRODUCTION 3 the high fidelity model, extensive system identification has been conducted for both a singlejoint research testbed and a commercial 6 degrees-of-freedom (DOF) industrial robot. The results of system identification are also essential for the success of the sensor fusion and control study in this dissertation Handling Mismatched Sensing: Sensor Fusion Approaches As discussed above, precise load side measurements at the joints are usually not available due to cost and assembly issues. Motor side encoders, in most cases, have been the only sensors used for feedback control. Due to the joint flexibilities from gear mechanisms, kinematic errors of the links, and so on, the information from motor side encoders does not precisely characterize the load side/end-effector performance, which is of ultimate interest. To tackle this mismatched sensing problem, several novel sensor fusion approaches are developed to provide the desired estimates for the information of interest. More specifically, the sensor fusion schemes are investigated in the following three major cases: Load Side State Estimation in Single-Joint Robot In Chapter 3, as an initial effort to tackle the complex mismatched sensing problem, the load side state estimation for the single-joint indirect drive robot is investigated. To obtain the desired sensing in a cost-effective manner, economic MEMS inertia sensors (e.g, gyroscopes and accelerometers) instead of expensive encoders 1, are utilized on the load side. Measurement dynamics is incorporated into the model to deal with the sensor noises and biases. Kalman filtering method is designed based on the extended dynamic/kinematic model for the fusion of multiple sensor signals. The covariance adaptation for fictitious noises is also studied using the maximum likelihood principle. The effectiveness of the proposed scheme is experimentally demonstrated on a single-joint research testbed. End-effector State Estimation in Multi-Joint Robot The extension of the sensor fusion scheme from the single-joint case to the multi-joint case is considerably challenging, due to the highly nonlinear dynamics/kinematics induced by the multi-joint configuration. In Chapter 4, instead of the load side sensing on each joint, the inexpensive MEMS inertia measurement unit (IMU) is adopted only on the end-effector, which is more realistic for industrial applications. Also, in industrial operational space control tasks, vision systems are commonly utilized. To mimic industrial vision system, a three-dimensional position measurement system, CompuGauge 3D, is utilized to provide down-sampled measurements for the tool center point (TCP) position. As a vision system alternative, an optical sensor (PSD camera) based on a position sensitive detector (PSD) is developed 2 to measure 1 A representative cost comparison between the encoders and the MEMS inertia sensors can be found in[7]. 2 The complete details of PSD camera development can be found in[94]. Some key features/specifications of the setup are listed in Appendix A.4.3.

21 CHAPTER 1. INTRODUCTION 4 the end-effector position directly. The developed PSD camera features high precision and fast response speed for position sensing at a low cost. The velocity and orientation information inferred indirectly from the mimicked vision system/psd camera measurements, however, is still not satisfactory. To better estimate the TCP position, velocity, and orientation in Cartesian space, the singlejoint sensor fusion scheme is extended to the multi-dimensional kinematic Kalman filter (MD- KKF) for the multi-joint robot. The end-effector kinematic model is formulated based on multidimensional rigid body motion and sensor measurement dynamics. The MD-KKF approach proposed in[53] is revisited and extended in Chapter 4 with a newly phrased multi-rate processing procedure and extensive experimental validation. Experimentation on a 6-DOF industrial robot demonstrates the superior performance of the proposed method, especially for the TCP velocity/orientation estimation. Two sensor configurations (i.e., IMU plus mimicked vision camera by CompuGauge 3D, and IMU plus PSD camera) are experimentally tested. Load Side Joint State Estimation in Multi-Joint Robot End-effector state estimation of the multi-joint robot is not sufficient for industrial robot control, especially when decentralized joint space control is preferred. Hence, it is desired to directly estimate the load side joint states with limited available sensing. The resulting method should be cost-effective and computationally light for industrial applications. In Chapter 5, an innovative load side state estimation scheme is developed for the multijoint robot with joint elasticity. The low-cost sensor configuration is emphasized, i.e., motor encoders and an economic end-effector MEMS sensor such as a 3-axial accelerometer. An optimization based inverse differential kinematics algorithm is designed to obtain the load side joint acceleration estimate. The joint position/velocity estimation problem is then conveniently decoupled into a simple second order kinematic Kalman filter (KKF) for each joint. Maximum likelihood principle is utilized to adapt the covariances for the fictitious noises in KKF. Both off-line and online solutions are derived. The light computation load of the scheme is discussed. The scheme is also extended to other sensor configurations to show its wide application potential. The effectiveness of the developed method is demonstrated through both simulation and experimental study on a commercial 6-DOF industrial robot Handling Mismatched Dynamics: Real-time Control Approaches The above sensor fusion schemes conveniently provide desired information for further actions on the mismatched dynamics issue. The mismatched dynamics problem arises naturally in robots with joint elasticity, where the control input and the uncertainties/disturbances are unfortunately separated by the indirect drive mechanism. Thus, direct compensation for the mismatched uncertainties/disturbances is extremely challenging. This dissertation develops the following real-time feedback control approaches to handle this problem for the singlejoint robot as well as the multi-joint robot.

22 CHAPTER 1. INTRODUCTION 5 Hybrid Adaptive Friction Compensation in Single-Joint Robot In Chapter 6, friction, one of the main mismatched disturbance factors that diminish control performance, is investigated and compensated. The friction effects are properly compensated by manipulating both the reference trajectory and the torque input. In the motor side torque compensation, a novel adaptive friction observer is designed based on the modified LuGre model to estimate the entire friction force reflected to the motor side. It is theoretically proved that with additional assumptions, the designed motor side control law with adaptive observer achieves perfect motor side tracking performance. To further improve the load side performance, the motor side reference is modified by injecting the adaptive estimate of the load side friction. A hybrid controller structure is developed to properly engage or disengage the load side compensator based on the motor side performance. The effectiveness of the proposed scheme is validated through experimentation on a single-joint research testbed. Motor Side Disturbance Rejection in Multi-Joint Robot Similar to sensor fusion, the extension of friction compensation from the single-joint case to the multi-joint case is nontrivial due to the complex nonlinear coupling dynamics. Since friction effects are usually highly coupled with other forms of disturbances, it makes more sense to address the problem of estimating and rejecting disturbances rather than friction compensation only. As a first step for this study, Chapter 7 presents a decentralized motor side controller for the multi-joint robot with joint elasticity. The transfer function analysis of the decoupled model shows that, under ideal conditions in the low frequency range, the perfect disturbance rejection on the motor side also results in the effective disturbance rejection on the load side. Thus, the aim for this study becomes to improve the load side/end-effector performance by rejecting the low frequency disturbance effects from the motor side. With that idea in mind, motor side disturbance rejection is achieved by making the inner plant of the robot behave as the simplified nominal model. A disturbance observer (DOB) and an adaptive robust controller (ARC) are designed separately to accomplish this objective. A comparative study between the DOB and ARC approaches is conducted through both the closed loop dynamics analysis and the experimentation on a 6-DOF industrial robot. This study shows the significant contributions that the two schemes bring to the control system. To further enhance high frequency load side performance, direct load side disturbance rejection becomes necessary, which is a pending future work Handling Mismatched Dynamics: Iterative Learning Control Approaches In industrial applications, robots are often repeatedly performing a single task under the same operating conditions. Thus, in addition to the real-time approaches, it would be most bene-

23 CHAPTER 1. INTRODUCTION 6 ficial to further utilize this repeatable feature for better handling the mismatched dynamics. This leads to the following iterative learning control (ILC) approach. Hybrid Two-Stage Model Based Iterative Learning Control Scheme for MIMO Mismatched Linear Systems Chapter 8 presents a hybrid two-stage model based ILC scheme for a broad class of multiinput-multi-output (MIMO) mismatched linear systems. Two model based ILC algorithms, namely reference ILC and torque ILC, are designed for different injection locations in the closed loop system. With the derivation of the tracking error/model following error dynamics, the plant inversion learning filter is formulated with guaranteed error convergence for each standalone ILC stage. Similar to the friction compensation in Chapter 6, an original ad hoc hybrid scheme is proposed to enable the two ILC stages to work together properly. In this way, higher bandwidth learning could be safely achieved by inner model matching in the presence of mismatched dynamics. Besides the theoretical framework, the superior performance of the proposed hybrid scheme over other benchmark controllers is experimentally demonstrated on a single-joint research testbed with inherent and intentionally designed uncertainties and disturbances. Iterative Learning Control with Sensor Fusion: Application to Multi-Joint Robot Chapter 9 presents the cumulative work of applying the well-developed ILC scheme and senor fusion scheme to the multi-joint robot with mismatched dynamics and mismatched sensing. The novel decoupled model formulation for the multi-joint robot dynamics enables the application of the decentralized ILC scheme for MIMO mismatched linear systems. The minor modification of the nominal model for learning process is discussed, which is essential for successful practical implementation. New iteration varying gains for hybrid scheme are also proposed to address the different behaviors in the multi-joint robot. The modifications are well matched with the low-cost sensor configuration (i.e., motor encoders and end-effector accelerometer) and the load side joint state estimation method. The exceptional performance of the proposed algorithm is experimentally demonstrated in end-effector position tracking and vibration reduction of a commercial 6-DOF industrial robot Appearance of Research Contributions in Publications The research contributions described above have appeared in various publications as follows: 3 As the partial extension of the ILC work, a robot learning control based on neural network prediction has been studied in[9] to generate the feedforward torque for a set of general motions. This provides significant flexibilities to the real-world automation process, where with the help of neural network prediction, the production line does not need to be stopped for learning a new trajectory. Thus it greatly improves the productivity.

24 CHAPTER 1. INTRODUCTION 7 Handling mismatched sensing: sensor fusion approaches The sensor fusion study for the single-joint robot (Chapter 3) was reported in[24], while the multi-joint case was partially presented in[94] (Chapter 4, end-effector state estimation) and[26] (Chapter 5, load side joint state estimation). Handling mismatched dynamics: real-time control approaches The real-time friction compensation algorithm for the single-joint robot (Chapter 6) was published in[19], while the publication for the disturbance rejection scheme for the multi-joint robot (Chapter 7) is still in preparation. Handling mismatched dynamics: iterative learning control approaches The ILC scheme for MIMO mismatched linear systems (Chapter 8) was published in [23]. This ILC scheme together with the sensor fusion method applied to the multi-joint robot (Chapter 9) will appear in[25]. Furthermore, the extension of this ILC work to other general motions using neural network prediction will be presented in[9]. Moreover, most of the research studies performed in this dissertation were also documented in the research project reports[44, 43, 21, 22, 2] to FANUC Ltd. Some of the proposed approaches (i.e., dynamic modeling, DOB, and ILC) were also applied to the Hyundai Heavy Industry LCD substrate transfer robot in the simulation study, and the results were reported in[13]. 1.4 Dissertation Outline The structure of this dissertation is depicted in Fig More specifically, the remainder of this dissertation is organized as follows: Chapter 2 presents the dynamic modeling for robots with mismatched dynamics and mismatched sensing, as well as the experimental setups (i.e., the single-joint research testbed and the commercial FANUC M-16iB robot system) used for algorithm validation. The major contributions of the dissertation are then presented in three parts. In Part I, the sensor fusion approaches for handling mismatched sensing are presented with three chapters (i.e., Chapter 3 for sensor fusion in the single-joint robot, Chapter 4 for sensor fusion in the multi-joint robot to estimate end-effector states, and Chapter 5 for sensor fusion in the multijoint robot to estimate load side joint states). The benefits of sensor fusion for load side/endeffector state estimation are demonstrated. Part II discusses the real-time control approaches for handling mismatched dynamics. Chapter 6 proposes a hybrid adaptive friction compensation method for the single-joint robot, and Chapter 7 presents the motor side disturbance rejection scheme for the multi-joint robot. The load side/end-effector performance is significantly enhanced with the developed real-time feedback approaches. The iterative learning control approach for handling mismatched dynamics is given in Part III. In Chapter 8, a hybrid two-stage model based ILC scheme is proposed for MIMO

25 CHAPTER 1. INTRODUCTION 8 Chapter 1 Chapter 2 Intelligent Control of Robots with Mismatched Dynamics and Mismatched Sensing Robot Modeling and Experimental Setups Appendix A Robot Setup Appendix B System Identification Handling Mismatched Sensing Handling Mismatched Dynamics Part I Part II Part III Sensor Fusion Real-time Control Iterative Learning Control Load Side State Estimation in Single-Joint Robot End-effector State Estimation in Multi-Joint Robot Load Side State Estimation in Multi-Joint Robot Friction Compensation in Single-Joint Robot Disturbance Rejection in Multi- Joint Robot ILC for MIMO Mismatched Systems ILC with Sensor Fusion Applied to Multi-Joint Robot Chapter 3 Chapter 4 Chapter 5 Chapter 6 Chapter 7 Chapter 8 Chapter 9 Covariance Estimation Appendix C Figure 1.1: The Structure Overview of the Dissertation mismatched linear systems. The proposed ILC scheme together with the sensor fusion algorithm applied to the multi-joint robot is presented in Chapter 9. Experimentation on a 6-DOF industrial robot has demonstrated its superior performance in position tracking and vibration reduction. The concluding remarks and future work are discussed in Chapter 1. More details about the robot experimental setup, system identification, and covariance estimation are documented in Appendix A, B, and C, respectively.

26 9 Chapter 2 Robots with Mismatched Dynamics and Mismatched Sensing 2.1 Introduction As discussed in Chapter 1, the mismatched dynamics and mismatched sensing are common inherent issues in robots with indirect drive mechanisms. Despite various physical realizations of the mismatched systems, they all share the same characteristic in terms of motion and torque transmissions, and hence the indirect drive train is usually modeled as a system of two masses connected by spring (and damper)[92, 97, 72]. To address the mismatched problems, good dynamic modeling of such mismatched systems is required. The rest of this chapter introduces the dynamic models and experimental setups of the mismatched systems that will be used throughout this dissertation. More specifically, Section first presents the general form of the mismatched dynamical system. Section formulates the mathematical model of the single-joint indirect drive robot, which is expanded with the consideration of friction effects and transmission error. Section presents the mathematical model of the multi-joint robot with joint flexibilities. A single-joint research testbed and a commercial 6-DOF industrial robot (i.e., FANUC M-16iB robot) in the Mechanical Systems Control Laboratory at the University of California, Berkeley are then described in Section Dynamic Modeling of Mismatched Robots Mismatched System Model Consider a multi-input-multi-output (MIMO) mismatched system in the following continuous time state space form

27 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 1 ẋ(t)=ax(t)+ B u u(t)+ B d d(t) qm (t) Cm y(t)= = x(t)+ q l (t) C l Dmu D lu u(t)+ Dmd D ld d(t) (2.1a) (2.1b) where x R n x is the system state, u R n u is the control input, d R n d is the lumped disturbance, q m R n m and q l R n l are the two outputs of the plant. d is regarded as the mismatched uncertainty/disturbance if it (or part of it) is applied to different channels from the control input u (i.e., B u αb d, α R). Another mismatched assumption is that, only part of the outputs (i.e., q m ) is measured for real-time feedback, even if the output of interest may be q l. Furthermore, besides the unknown mismatched dynamics, it is assumed that parametric uncertainties exist in the available nominal model. This system can be reformulated into the following transfer function form q m (s)= P mu (s)u(s)+ P md (s)d(s) q l (s)= P lu (s)u(s)+ P ld (s)d(s) (2.2a) (2.2b) where s is the complex variable in the Laplace domain. P mu, P md, P lu, and P ld are the transfer functions from u or d to the corresponding output as follows P mu (s)=c m (si nx A) 1 B u + D mu P lu (s)=c l (si nx A) 1 B u + D lu P md (s)=c m (si nx A) 1 B d + D md P ld (s)=c l (si nx A) 1 B d + D ld (2.3a) (2.3b) (2.3c) (2.3d) where I nx is an n x n x identity matrix. The above formulations (2.1) and (2.2) define the general form of the mismatched system that is addressed in this dissertation. In the following sections, more detailed dynamic modeling will be studied for the specific examples, i.e., robots with indirect drive trains/joint flexibilities Single-Joint Robot Model Dynamic Modeling of Indirect Drive Train Figure 2.1 shows the schematic of a single-joint indirect drive mechanism. The subscripts m andldenote the motor side and the load side quantities 1, respectively. θ represents the angular position and J is the moment of inertia. u is the motor torque input. d m and d l 1 Recall that as discussed in Chapter 1, the electric motor is used as the modeling example for the actuator. Thus, the two different parts separated by the indirect drive mechanism are termed as the motor side and the load side, respectively.

28 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 11 u θ m Motor J m Reducer N, f h, θ Load Inertia d fm, f m (f mc, d m ) θ o k j, d j d fl, f l (f lc, d l ) J l θ l Figure 2.1: Single-Joint Indirect-Drive Mechanism represent the viscous damping coefficients at the motor side and the load side, respectively. k j and d j are the stiffness and the damping coefficients of the reducer. The gear ratio of the (speed) reducer is denoted by N. f m, f l, and f h represent the nonlinear friction forces at the motor side, the load side, and the reducer, respectively. d f m and d fl are the additional disturbances at the motor side and the load side. f mc and f lc represent the nonlinear Coulomb frictions at the motor side and the load side, respectively. θ is the transmission error of the reducer, which is defined as the deviation between the expected reducer output position θ m and N the actual reducer output positionθ o. The friction forces (f m, f l, and f h ) and the transmission error θ will be further discussed later. Note that f m, f h, and f l include the viscous damping effects, d m and d l, and the nonlinear Coulomb frictions, f mc and f lc, respectively. The model of the single-joint indirect drive robot in Fig.2.1 can be written as J m θm = u+ d f m 1 θm f m + f h k j N N θ l θ θm + d j N θ l θ (2.4a) θm J l θl = d fl f l + k j N θ l θ + d j θm N θ l θ (2.4b) Remark 2.1. Note that, this indirect drive train model includes only one transmission mechanism, while in reality, multiple transmission mechanisms connected in series may be necessary to achieve the desired torque amplification/speed reduction. In this case, it is possible to model this multi-level transmission mechanism into an equivalent single-level transmission mechanism. The equivalent parameters for this single-level mechanism, such as gear ratio N eq, reducer stiffness k jeq, reducer damping d jeq, motor side inertia J meq, motor side damping d meq, and reducer friction

29 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 12 force f heq, can be computed as follows[13] N eq = n i=1 N i n 1 1 k jeq = k n i=1 ji N 2 l=i+1 l n 1 1 d jeq = d n i=1 ji N 2 l=i+1 l n 1 1 J meq = J m + i i=1 N J i 2 l=1 l n 1 d meq = d m + f heq = n i=1 i=1 1 i N 2 l=1 l 1 i 1 N f hi l=1 l d i 1 1 (2.5a) (2.5b) (2.5c) (2.5d) (2.5e) (2.5f) where n is the number of levels of transmission mechanism. N i, k ji, d ji, J i, d i, and f hi represent the gear ratio, reducer stiffness, reducer damping, reducer rotor inertia, reducer rotor damping, and reducer friction force for the i-th level of transmission mechanism, respectively. This equivalent transmission mechanism can greatly simply the dynamic modeling for the whole indirect drive train. In the case where friction forces (f m, f h, and f l ) are simplified to the viscous damping and Coulomb frictions, i.e., f m + f h = f mc sgn( θ m )+ d m θm, and f l = f lc sgn( θ l )+ d l θl, the dynamic model for this mechanism can be reformulated as J m θm + d m θm = u+ d m 1 θm θm k j N N θ l + d j N θ l (2.6a) J l θl + d l θl = k j θm N θ l + d j θm N θ l + d l (2.6b) where d m = d f m f mc sgn( θ m )+ 1 k j θ+ dj θ N d l = d fl f lc sgn( θ l ) k j θ+ dj θ (2.7a) (2.7b) Therefore, the above indirect drive model can be considered as a mismatched system described in (2.2) with the disturbance d= d m d l T. The two outputs qm and q l are the motor side positionθ m and the load side positionθ l, respectively.

30 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 13 Transfer Function Realizations With the above modeling, the continuous time transfer functions from the inputs to the outputs in (2.2) are derived as P mu (s)= J l s 2 +(d j + d l )s+k j J m J l s 4 + J d s 3 + J k s 2 + k j (d m + d l /N 2 )s (2.8a) P lu (s)= d j s+k j N (2.8b) J m J l s 4 + J d s 3 + J k s 2 + k j (d m + d l /N 2 )s d j s+k j P md m(s)= N (2.8c) J m J l s 4 + J d s 3 + J k s 2 + k j (d m + d l /N 2 )s J mi s 2 +(d j /N 2 + d m )s+k j /N 2 P ld l(s)= (2.8d) J m J l s 4 + J d s 3 + J k s 2 + k j (d m + d l /N 2 )s P md (s)= P mu (s) P md m(s), P ld (s)= P lu (s) P ld l(s) (2.8e) where J d = J m (d j + d l )+ J l dj N 2+ d m (2.9a) J k = J m k j + J lk j N 2 +(d j+ d l )d m + d jd l N 2 (2.9b) Furthermore, the transfer functions from the torque input u to the motor side velocity θ m and the load side acceleration θ l can be found as G m (s)= G l (s)= θ m (s) u(s) = P mu(s)s= θ l (s) u(s) = P lu(s)s 2 = J l s 2 + d j + d l s+kj (2.1a) J m J l s 3 + J d s 2 + J k s+k j dm + d l N 2 d j s 2 + k j s N (2.1b) J m J l s 3 + J d s 2 + J k s+k j dm + d l N 2 In practice, the damping parameters d m, d l, and d j are often relatively small and neglected. This simplifies the estimation of the plant poles and zeros. Based on this simplification, the anti-resonant modeω ar and the resonant modeω r for this open loop system are approximately[86] k j ω ar = (rad/sec) (2.11a) J l k j ω r = + k j (rad/sec) (2.11b) 2 J l J m N

31 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 14 State-Space Formulation The continuous time state-space formulation of (2.6) can be written as where ẋ(t)=a c x(t)+ B uc u(t)+ B dc d(t) (2.12) x=[θ m θm θ l θl ] T d m d f m f mc sgn( θ m )+ 1 k d= d l N j θ+ dj θ = d fl f lc sgn( θ l ) k j θ+ dj θ 1 k j d 2 m+d j /N k j d j A c = N 2 J m J m N J m N J m 1 B uc = B dc = k j N J l d j 1 J m N J l T 1 J m 1 J l T k j J l d j+d l J l Linear Approximation Note that all the nonlinear parts are grouped into the disturbance term d. While the actual robot joint is inherently nonlinear, a good linear approximation can still preserve satisfactory performance if either the nonlinear elements are negligible or if the linear parts are of most interest. Additionally, the linear model allows the use of various linear analysis and control synthesis methods. Thus, if the external disturbances (d f m and d fl, which are usually negligible under normal operation condition), the transmission error θ, and nonlinear friction effects are ignored, (2.6) can be rewritten as J m θm + d m θm = 1 θm θm k j N N θ l + d j N θ l + u (2.13a) J l θl + d l θl = k j θm N θ l and the state-space realization is accordingly + d j θm N θ l where the variables follow the same definition as in (2.12). (2.13b) ẋ(t)=a c x(t)+ B uc u(t) (2.14)

32 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 15 Friction Modeling For the purpose of friction identification and compensation, the following nonlinear model will be adopted without the consideration of the external disturbances d f m and d fl. J m θm = 1 N θm k j N θ l θ J l θl = k j θm N θ l θ θm + d j N θ l θ + u f m + f h f l + d j θm N θ l θ (2.15a) (2.15b) In this system, the energy is dissipated mainly by three friction forces: the motor bearing friction, f m, the load output bearing friction, f l, and the reducer gear meshing friction, f h. Although the gear meshing friction f h is dominant in a free load condition[76], the load side friction f l can become more significant as the load side inertia increases. The combination of system model with friction effects (2.15) gives J m θm + 1 N J l θ l = u F (2.16a) F= f m + f h + f l N (2.16b) where F can be explained as the entire friction force imposed on the whole system when reflected to the motor side. There are several reasons to model the entire friction force F in the viewpoint of the motor side. First of all, it is easier to identify the friction on the motor side for industrial indirect drives. Secondly, from the torque compensation point of view, the friction force is usually compensated at the motor side where the actuator locates. Also it is not necessary to model these friction forces separately, especially when they exhibit similar behaviors. To describe the friction effects imposed on the motor side, this dissertation (especially Chapter 6) employs the Lund-Grenoble (LuGre) model[15]. The LuGre model uses an internal friction state, z, governed by ż=v σ v g(v) z g(v)= F c + F s F c e ( v 2 /v 2 s) F=σ z+σ 1 ż+σ 2 v (2.17a) (2.17b) (2.17c) where v is the relative velocity between two contacting surfaces on the motor side, i.e., v= θ m. σ,σ 1, andσ 2 represent the micro-stiffness, micro-damping of the internal state z, and the macro-damping of the velocity v, respectively. The function g(v) is chosen to capture the Stribeck effect, where F C and F S are the levels of Coulomb friction and stiction force. v s is the Stribeck velocity. The internal friction state z can be regarded as the deflection of bristles, which represents the asperities between two contacting surfaces.

33 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 16 From (2.17), the entire friction force, F, can be written as: F= K T Φ (2.18) where K= T,Φ= T. σ 1 +σ 2 σ σ σ 1 v z v z g(v) If only static friction is considered, i.e., ż=, the LuGre model is simplified to F(v)= F c + F s F c e ( v 2 /vs) 2 sgn(v)+σ 2 v (2.19) If further omitting the Stribeck effect, i.e., F s = F c, this model reduces to F(v)= F c sgn(v)+σ 2 v (2.2) which is more commonly used in many practical applications due to its simplicity. If the load side friction effects are not negligible, it is necessary to consider the load side friction separately. Assuming that the load side friction shares the same characteristics as the entire friction force F in (2.16b), a ratio r l can be introduced to derive the load side friction f l from F, i.e. f l = r l F (2.21) This assumption makes sense in most cases, especially when the motor is spinning in one direction. The transient behavior during the velocity reversal might not strictly follow this assumption. This velocity reversal case, however, is not of the interest in most applications since the transient time is usually sufficiently short to be negligible. Transmission Error Modeling As stated above, there are various forms of transmission mechanisms in indirect drive trains for speed reduction and torque amplification. In industrial robots, rotary vector (RV) reducers and harmonic gears are the most popular means for this purpose due to their compact design and large torque capacity. However, they possess a periodic position error known as the transmission error. The transmission error θ is defined as the deviation between the theoretical output position and the actual output position. It is given by the following equation (see Fig. 2.2) θ= θ m N θ o (2.22) whereθ m andθ o are the motor position and reducer output position, respectively. Due to mechanical imperfections such as gear assembly misalignments and dimensional inaccuracies of the gear itself, the output oscillations may vary with different drives and operating conditions. It has been shown in the literature, however, that the dominant component of the experimental transmission error waveforms is repeated (such as for every half turn of the input shaft in harmonic drive)[39]. In other words, the ripple is periodic in nature

34 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 17 θ m θ o θ l 1 N + _ θ = θ m N θ o Figure 2.2: Definition of the Transmission Error and its fundamental frequency component corresponds to twice the rotation frequency of the input shaft (in the case of harmonic drive). In addition to the fundamental frequency, highfrequency error components are also observed but with relatively small amplitudes. Hence, θ can be approximated with a simple sinusoid as follows θ θ dominant = A te sin(ω o t+φ) (2.23) where A te is the amplitude of the transmission error which can be found in the reducer catalogue. φ is the phase of the error andω o is the frequency of the error (e.g.,ω o = 2 θ m in harmonic drive). The higher frequency components in the error are neglected here. Note that the error exhibits in the system as a position error at the output of the motor shaft. The dynamic model with this transmission error is formulated as in (2.4) Multi-Joint Robot Model Lagrangian Dynamics The multi-joint robot with indirect drive trains considered in this dissertation is assumed to be connected in serial. A typical example of this kind of system is the industrial serial robot manipulator with revolute/prismatic joints. The dynamics of such an n-joint robot manipulator with gear compliance can be expressed as[92, 97, 72] M l (q l ) q l + C(q l, q l ) q l + G(q l )+ D l q l + F lc sgn( q l )+ J(q l ) T f ex t = K J N 1 q m q l q + D J N 1 q m q l q (2.24a) M m q m + D m q m + F mc sgn( q m )=τ m N 1 K J N 1 q m q l q + D J N 1 q m q l q (2.24b) where q l R n and q m R n are the load side and the motor side position vectors, respectively. q R n is the transmission error vector resulted from the joint reducers.τ m R n is the motor torque input vector. M l (q l ) R n n is the load side inertia matrix, C(q l, q l ) R n n is the Coriolis and centrifugal force matrix, and G(q l ) R n is the gravity torque vector. M m, K J, D J,

35 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 18 D l, D m, F lc, F mc, and N R n n are all diagonal matrices. The(i, i)-th entries of these matrices, M mi, K Ji, D Ji, D li, D mi, F lci, F mci, and N i, represent the motor side inertia, joint stiffness, joint damping, load side damping, motor side damping, load side Coulomb friction, motor side Coulomb friction, and gear ratio of the i-th joint, respectively. f ex t R 6 denotes the external force/torque acting on the robot end-effector due to the contact with the environment. The matrix J(q l ) R 6 n is the Jacobian matrix mapping from the load side joint space to the end-effector Cartesian space. If the joints are rigid joints instead of flexible joints (i.e., K J and D J become infinity, and q m = Nq l ), then the model (2.24) reduces to[64, 74]: M(q) q+c(q, q) q+g(q)+ D q+f c sgn( q)+ J(q) T f ex t =τ (2.25) where q=q l andτ= Nτ m are the vectors of load side joint angles and applied load side joint torques, respectively. M(q)= M l (q l )+N 2 M m is the lumped positive-definite inertia matrix. C(q, q)=c(q l, q l ) accounts for Coriolis and centrifugal effects. F c = F lc + N F mc represents the Coulomb friction matrix, D= D l + N 2 D m represents the viscous damping matrix, and G(q)= G(q l ) represents the torque vector due to gravity. Decoupling Model The multi-joint robot model (2.24) is highly coupled due to the contact force, inertia, gravity, Coriolis and centrifugal effects. This introduces significant difficulty/complexity to the dynamic analysis and controller design. To solve this problem, a novel approach is proposed here to properly decouple the dynamic model in the way suitable for decentralized analysis and control for each joint. Define the nominal load side inertia matrix as M n = diag(m n1, M n2,, M nn ) R n n, where M ni = M l,ii (q l ) is the(i, i)-th entry of the inertia matrix M l (q l ) at the home (or nominal) position q l. M n can be used to approximate the inertia matrix M l (q l ). The off-diagonal entries of M l (q l ) represent the coupling inertia between the joints. Then, the robot dynamic model is reformulated as M m q m + D m q m =τ m + d m N 1 K J N 1 q m q l + DJ N 1 q m q l (2.26a) M n q l + D l q l = d l + K J N 1 q m q l + DJ N 1 q m q l (2.26b) where all the coupling and nonlinear terms, such as Coriolis/centrifugal force, gravity, Coulomb friction, transmission error, and external forces, are grouped into the fictitious disturbance torques d m (q), d l (q) as d m (q)= F mc sgn( q m )+ N 1 K J q+ D J q d m(q) d m(q) d m(q)) T 1 2 n R n (2.27a) d l (q)= M n M 1 (q l l ) I n KJ N 1 q m q l + DJ N 1 q m q l Dl q l M n M 1 (q l l ) C(q l, q l ) q l + G(q l )+ F lc sgn( q l )+ J T (q l )f ex t + K J q+ D J q d l (q) 1 dl(q) 2 dl(q)) T n R n (2.27b)

36 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 19 where I n is an n n identity matrix and q is defined as q= q T 1 q T 2 q T n T R 4n (2.28a) q i = q mi q mi q li q li T R 4, i= 1, 2,, n (2.28b) Thus, the robot can be considered as a MIMO system with 2n inputs and 2n outputs as q m (s)= P mu (s)τ m (s)+ P md (s)d(s) q l (s)= P lu (s)τ m (s)+ P ld (s)d(s) (2.29a) (2.29b) where the fictitious disturbance input d(t) is defined as d(t)=d(q(t))= d 1 (q(t)) T d 2 (q(t)) T d n (q(t)) T T R 2n d i (t)=d i (q(t))= d m i(q(t)) d l i (q(t)) T R 2, i= 1, 2,, n (2.3a) (2.3b) Similar to the single-joint case in Section 2.2.2, the continuous time transfer functions from the inputs to the outputs for the i-th joint are derived from (2.26) as P i (s)= M ni s 2 +(D Ji + D li )s+k Ji mu M mi M ni s 4 + J di s 3 + J ki s 2 + K Ji (D mi + D li /N 2 i )s (2.31a) P i (s)= D Ji s+k Ji (2.31b) lu N i Mmi M ni s 4 + J di s 3 + J ki s 2 + K Ji (D mi + D li /N 2 i )s D P i Ji s+k Ji (s)= (2.31c) md l N i Mmi M ni s 4 + J di s 3 + J ki s 2 + K Ji (D mi + D li /N 2 i )s M P i mi s 2 +(D Ji /N 2 + D i mi )s+k Ji /N 2 i (s)= ld l M mi M ni s 4 + J di s 3 + J ki s 2 + K Ji (D mi + D li /N 2 i )s (2.31d) P i (s)= P i (s) P i (s) md mu, P i (s)= P i(s) P i (s) md l ld lu (2.31e) ld l where the subscript/superscript i denotes the joint index, and J di =M mi (D Ji + D li )+ M ni ( D Ji N 2 i J ki =M mi K Ji + M nik Ji N 2 i + D mi ) (2.32a) +(D Ji + D li )D mi + D JiD li N 2 i (2.32b) Remark 2.2. Note that the robot dynamic model (2.26) is in a decoupled form since all the variables are expressed in the diagonal matrix form or vector form. This model formulation enables the way to design and analyze the robot controller in a decentralized fashion for each individual joint.

37 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 2 Remark 2.3. It is seen that the component d l of the fictitious disturbance d in (2.3a) influences the output in a different form from the motor torque inputτ m. Also, in practice, it is common that only motor side encoders are equipped in industrial robots even though the load side/end-effector performance is of the ultimate interest. Thus, with these mismatched dynamics and sensing issues, this robot system is regarded as a MIMO mismatched system described in Section State-Space Formulation Similar as the single-joint case (2.12), the state space form of the above indirect drive model for the i-th joint can be written as where Ā ni = d dt q i= Ā ni q i + B ui τ mi + B di d i (q) (2.33) 1 K Ji N 2 i M mi D mi+d Ji /N 2 i M mi K Ji N i M mi D Ji N i M mi 1 K Ji N i M ni D Ji N i M ni T 1 B ui = M mi B di = 1 M mi 1 M ni T K Ji M ni D Ji+D li M ni By stacking the state space model of each joint together, the state space form for the whole n-joint robot with joint flexibilities can be written as q 1 Ā n1 q 1 B u1 d q 2 dt. = Ā n2 q B u q n Ā nn q n B un B d1 d 1 (q) B d2 d 2 (q) B dn d n (q) τ m1 τ m2. τ mn (2.34) or simply d dt q=ā n q+ B u τ m + B d d(q) (2.35)

38 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 21 Figure 2.3: Single-Joint Indirect Drive System Setup 2.3 Experimental Setup Single-Joint Robot Experimental Setup The single-joint indirect drive research testbed available in the Mechanical Systems Control Laboratory at the University of California, Berkeley is shown in Fig This setup is used for initial testing on which most research algorithms are first experimentally verified before being extended to the multi-joint robot system. This single-joint testbed consists of: 1) a servo motor with a 2, counts/revolution encoder, 2) a harmonic drive with a 8:1 gear ratio, 3) a loadside 144, counts/revolution encoder, 4) and a payload. The anti-resonant and resonant frequencies of the setup are approximately 11Hz and 19Hz, respectively. System identification (see Appendix B) has been conducted for this setup to identify system parameters which are shown in Table 2.1. The load side encoder is normally used only for performance evaluation. Besides the encoder, there are two other sensors available on the load side. A MEMS gyroscope (Analog, Type: ADXRS15[4]) is installed at one end of the payload to measure the load side angular velocity. Two accelerometers (Kistler, Type: 833A3[1]) are installed at both ends of the

39 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 22 acc 2 θ l θ l acc 1 R a g Figure 2.4: Accelerometer Installation Table 2.1: Identified Parameters for the Single-Joint Robot Variable Symbol Value Units Gear ratio N 8 - Motor side inertia J m Nms 2 /rad Load side inertia J l 6.8 Nms 2 /rad Joint stiffness k j 31 Nm/rad Joint damping d j 47 Nms/rad Motor side damping d m Nms/rad Load side damping d l Nms/rad Coulomb friction F c.14 Nm Stiction friction F s.175 Nm Stribeck velocity v s rad/s Transmission error amplitude A te rad payload symmetrically as shown in Fig The accelerometers are arranged to compensate for the gravity effects on the accelerometer measurements. This configuration has the following relationship from the translational acceleration measurements, a 1 and a 2, to the load side

40 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 23 angular acceleration, θ l, and the gravity, g a 1 = R a θl + g cos θ l a 2 = R a θl g cos θ l (2.36a) (2.36b) where R a is the distance from the rotation axis to the accelerometer measurement point, and θ l is the angle of the payload from the horizontal position. It is not necessary to measure the angle θ l, since the angular acceleration θ l can be obtained as θ l = a 1+ a 2 2R a (2.37) which is independent of the gravity effect. Finally, the controller and sensor fusion algorithms are implemented in a LabVIEW realtime target installed with LabVIEW Real-Time and FPGA modules. The sampling rate is selected as 1kHz for all the research implementation on this setup Multi-Joint Robot Experimental Setup The multi-joint robot experimental setup used at the University of California, Berkeley, courtesy of FANUC, is the M-16iB/2 industrial robot as shown in Fig It is a six-axis, medium size robot and can carry objects up to 2 kg at a maximum speed of 2 mm/sec. It is mainly used for the high-speed applications such as spot welding, material handling, sealing, and water-jet cutting. The specification of this robot can be found in[59]. The hardware connection diagram of the experimental setup is illustrated in Fig The M-16iB robot is equipped with built-in motor encoder for each joint. An "L" shape steel payload of the weight of 18.37kg is attached at the last joint as the end-effector. An inertia measurement unit (IMU) (Analog Devices, ADIS164[3]) consisting of a 3-axial accelerometer and a 3-axial gyroscope is attached to the end-effector. The three-dimensional position measurement system, CompuGauge 3D (repeatability of.2mm, accuracy of.15mm, resolution of.1mm[28]), is utilized to measure the end-effector tool center point (TCP) position. Furthermore, a PSD camera of high precision and fast response has been developed to directly sense the two-dimensional positions of the infrared markers attached on the robot end-effector [94]. These sensors allow to obtain the end-effector information along with time information for the purpose of performance improvement and evaluation. The sampling rates of all the sensor signals as well as the real-time controller implemented through MATLAB xpc Target are set to 1kHz. System identifications are conducted for each individual joint at several different postures to obtain the nominal dynamic parameters in the dynamic model (2.26). The details of the system identifications can be found in Appendix B. The development of PSD camera is detailed in[94, 22]. All other parts (e.g., FANUC robot controller, real-time system, payload design, inertia sensor setup, and Robot Simulation/Experimentor) of the hardware setup are described in Appendix A and also reported in [44, 43, 21, 5].

41 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 24 Figure 2.5: FANUC M-16iB Robot System Robot Kinematic Configuration For the forward and inverse kinematics, a suitable kinematic parametrization, which is used extensively for industrial robots, is given by the so-called Denavit-Hartenberg (DH) parametrization[74]. This convention uses four parameters to completely parameterize the relative position and orientation of adjacent links. For this particular robot, FANUC M-16iB, to obtain a DH parametrization, a reference configuration with all exact dimensions needs to be established first. Figure 2.7 shows key lengths for establishment of the DH parameters, where J io represent the absolute Cartesian coordinates of the i-th joint origin relative to the world reference frame denoted byψ. Now, each of the reference frames can be attached to the corresponding links starting from the base link. Also, positive directions of each angle can be specified together with their respective axis of rotations. Both procedures are illustrated in Fig Once these frames have been specified, the remaining part of kinematic modeling is straightforward. Based upon Fig. 2.8, the DH parameters is obtained as shown in Table 2.2. Then using this convention, the joint angles (relative to the moving zero reference frame of each joint) for the robot home position shown in Fig. 2.9 are obtained as q h = q h1 q h2 q h6 T= π 2 π 2 π T (2.38)

42 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 25 Figure 2.6: FANUC M-16iB Robot System Setup Scheme Figure 2.7: Reference Configuration for FANUC M-16iB Robot

43 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING 26 θ 4 x 3z3 x 4, x 5 x 6 θ 3 θ 5 θ 6 z 2 x 2 z 4 z 5 Ψ 6 z 6 θ 1 θ 2 z x 1 Ψ z 1 x Figure 2.8: Angle Conventions (left) and Attached Frames (right) Table 2.2: DH Parameters for FANUC M-16iB Robot i α i a i θ i d i 1 π/ π/2 3 π/2.1 4 π/ π/2 6.1 Figure 2.9: Home Position for FANUC M-16iB Robot

44 CHAPTER 2. ROBOTS WITH MISMATCHED DYNAMICS AND MISMATCHED SENSING Chapter Summary This chapter introduced the dynamic modeling and experimental setups for the robots with mismatched dynamics and mismatched sensing. The unifying characteristic of such mismatched systems was described first. Then the dynamic modeling for the single-joint mismatched robot was studied with the consideration of friction and transmission error effects. The multi-joint mismatched robot dynamics was subsequently investigated, and a novel approach was presented to decouple the multi-joint model and to formulate it into the standard MIMO mismatched model form. The experimental setups (including single-joint and multijoint setups) that are used in this dissertation research were then introduced. The configuration and implementation of the experimental setups are detailed in Appendix A.

45 28 Part I Handling Mismatched Sensing: Sensor Fusion Approaches

46 29 Chapter 3 Load Side State Estimation in Single-Joint Robot 3.1 Introduction As discussed in previous chapters, in robot applications, discrepancies between the available measurements and the required information set difficulties in achieving good control performance. Such phenomena are resulted from sensor dynamics as well as robot dynamics. Particularly, in robots with joint flexibilities, good motor side performance based on motor side measurements does not guarantee good load side performance, which is a critical issue in practical applications. Precise load side position measurements (e.g., load side encoder), however, are usually not available in industrial robots due to cost and assembly issues. To overcome this problem, inexpensive MEMS sensors 1 that are easy to mount may be considered. Consideration should be given, however, to problems such as non-negligible biases, limited bandwidth, and noises from low-cost sensors, which set restrictions on the direct utilization of sensor signals. These problems may be circumvented by the proper fusion of multiple sensor signals. In recent years, estimation algorithms using multi-sensor configurations have been studied. Two robust estimation schemes incorporating the Kalman filter and disturbance observer for robot dynamic models have been reported in[49, 71]. These methods require accurate system parameters and thus are not reliable when subjected to model uncertainties. In[56, 51], it has been suggested that a kinematic model based Kalman filter (KKF) can be formulated by making use of accelerometers and position encoders. The model uncertainties are avoided by using the kinematic relation between the position and the acceleration. These methods, however, were developed mainly for one-mass systems or direct drives (i.e., without indirect drive joint compliance), and the main purpose is velocity estimation while assuming the position information is measured. Also, noise covariance in these methods were used as design parameters, which are not easy to select when multiple measurements are utilized. In this chapter, this load side state estimation problem in the single-joint indirect drive 1 A representative cost comparison between the encoders and the MEMS inertia sensors can be found in[7].

47 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 3 robot is investigated. Measurement dynamics are introduced into the dynamic/kinematic model to deal with sensor noises and biases. The estimation algorithm is formulated in Section 3.2 based on the extended model to fuse the measurements from both the motor side and the load side. Noise covariance adaptation issue is also addressed. The schemes are tested by experiments in Section 3.3 and will be utilized in the friction compensation in Chapter Estimation Algorithms Model for Measurement Dynamics The bias and the noise in the sensor output can be described as ḃ(t)= n b (t) x s (t)= x(t)+ b(t)+ n s (t) (3.1a) (3.1b) where b is the bias of the sensor output varying with the fictitious noise n b. x s is the sensor measurement for the actual physical quantity x with the measurement noise n s System Dynamic Model Consider the linear model representation (2.14) of an indirect drive robot joint restated as ẋ(t)=a c x(t)+ B uc u(t) (3.2) where x=[θ m θm θ l θl ] T 1 k j d 2 m+d j /N k j d j A c = N 2 J m J m N J m 1 B uc = k j N J l d j 1 J m N J l T N J m k j J l d j+d l J l and all the variable definitions are the same as in Chapter 2. If the load side is equipped with the gyroscope and the accelerometer, the system model (3.2) with measurement dynamics is extended to ẋ m (t)=a m x m (t)+b m,u u s (t)+b m,w w m (t) y m (t)=c m x m (t)+ v m (t) (3.3a) (3.3b)

48 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 31 where x m = x T b a b v T R 6 1, y m = θ m,s θl,s θl,s T R 3 1 w m = T R n u n ba n 3 1 bv, v m = T R 3 1 n sm n vl n al Ac A m = R 6 6, B m,u = B uc T R Buc B m,w = R 6 3, C I m = 1 1 R A c4 1 where A c4 is the fourth row of the matrix A c, and I n is an n n identity matrix. s denotes the sensor measurement of the actual physical quantity. b a and b v represent the biases of accelerometer and gyroscope outputs, respectively. n u, n ba, and n bv are the noises of motor torque output, u, accelerometer bias, b a, and gyroscope bias, b v, respectively. n sm, n vl, and n al are the measurement noises of motor side position,θ m, load side velocity, θ l, and load side acceleration, θ l, respectively System Kinematic Model The kinematic model from the acceleration to the position on the load side can be written as d θl (t) 1 θl (t) = + θ dt θ l (t) θ l (t) 1 l (t) (3.4) The Kalman filter based on the kinematic model (3.4) is called the kinematic Kalman filter (KKF)[56]. In KKF, the acceleration is used as an input to the filter, and the position information is used to correct the estimation output. In the indirect drive robot joint, however, precise load side position measurement (e.g., load side encoder) is usually not available, which makes it difficult to directly utilize the KKF algorithm. Here, a novel but intuitive method is proposed to approximate the load side position measurement. Let G m2l (s) be the transfer function from the motor side positionθ m to the load side positionθ l, i.e., G m2l (s) θ l(s) θ m (s) = d j s+k j N J l s 2 +(d j + d l )s+k j (3.5) by the inherent system dynamics ((2.13) in Chapter 2). Thus, G m2l (s) is approximately zerophase static gain at the low frequency region (Fig. 3.1), since the dynamic chain fromθ m toθ l is modeled by mass, gear, spring, and damper. This indicates that the low frequency component ofθ l can be approximated by that ofθ m. Passθ m andθ l through a first order low-pass filter G f (s)= α, whereα=β f s+α b, f b is the bandwidth of G m2l (s), andβ (, ) is a design parameter for the low-pass filter. Denote the

49 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 32 2 Bode Diagram Magnitude (db) 2 4 System: untitled1 Frequency (Hz): 16.5 Magnitude (db): Phase (deg) Figure 3.1: Bode Plot of Frequency (Hz) θ l (s) for the System Setup in Fig. 2.3 λ θ m (s) filter outputs asθ mf andθ lf, respectively. It follows that θ mf = αθ mf +αθ m θ lf = αθ lf +αθ l (3.6a) (3.6b) The above analysis shows that, ifβ is chosen properly, the filter outputs will have the following relation θ lf λ θ mf (3.7) whereλ is the DC gain of G m2l (s) (e.g.,λ = 1 for the system in Fig. 2.1). Generally, it is N desired thatβ 1 and thusα f b. If the low frequency components ofθ m andθ l are highly dominant, however,β can be chosen to be larger than 1 to increase the filter convergence rate. The filter dynamics (3.6) and the measurement dynamics (3.1) can be added into the system kinematic model (3.4), giving ẋ k (t)=a k x k (t)+b k,u θl,s (t)+b k,w w k (t) y k (t)=c k x k (t)+ v k (t) (3.8a) (3.8b)

50 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 33 where x k = θ lf θ l θl b a b v T R 5 1, y k = θ lf,s θl,s T R 2 1 w k = T R n al n ba n 3 1 bv, v k = T R 2 1 n lf n vl α α 1 A k = 1 R5 5, B k,u = 1 T R 5 1 B k,w = T R 1 I 5 3 3, C k = R where n lf is the fictitious noise of filtered load side position,θ lf. By the approximation in (3.7),λ θ mf,s, whereθ mf,s (s)=g f (s)θ m,s (s), can be used as the fictitious measurement for the model outputθ lf,s. The noise covariance ofθ lf,s can be approximated by the noise covariance ofλ θ mf,s, or could be a design parameter for the following Kalman filter Kalman Filtering The discrete time form of the extended system model (3.3) or (3.8) can be obtained as x d (k+ 1)=A d x d (k)+b d,u u d (k)+b d,w w d (k) y d (k)=c d x d (k)+ v d (k) (3.9a) (3.9b) where x d (k), y d (k), u d (k), w d (k), and v d (k) are the k-th time step sampled values of x m (t), y m (t), u s (t), w m (t), and v m (t) for the system dynamic model (3.3), or x k (t), y k (k), θl,s (t), w k (t), and v k (t) for the system kinematic model (3.8), respectively. A d, B d,u, B d,w, and C d are derived from (3.3) or (3.8) by the zero-order-hold (ZOH) method. In practice, B d,w w d could be generalized to include the disturbance and/or mismatched model dynamics. For the extended system model (3.9), a Kalman filter to estimate the system states is given by ˆx d (k)=ˆx o d (k)+ L d(k)ỹ o d (k) ˆx o d (k+1)=a dˆx d (k)+b d,u u d (k) ỹ o d (k)= y d(k) C dˆx o d (k) (3.1a) (3.1b) (3.1c) where the Kalman filter gain L d is calculated as L d (k)= M d (k)c T d [C d M d (k)c T d +ˆR d (k)] 1 M d (k+1)=a d Z d (k)a T + B d d,wˆq d (k)b T d,w Z d (k)= M d (k) L d (k)c d M d (k) (3.11a) (3.11b) (3.11c)

51 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 34 n sm + + θ m,s θ m e m + θ l,s, θ l,s L d e l + b a + n al b v + n vl + θl, θ l + u + + n u u s DKF ˆθ m,s ˆθ m ˆθ l ˆθ l,s ˆθl,s Figure 3.2: Structure of Dynamic Model Based Kalman Filter (DKF) ˆQ d (k) andˆr d (k) are the covariance estimates of w d (k) and v d (k), respectively. M d (k) and Z d (k) are the one-step a-priori and a-posteriori covariances of the state estimation error, respectively. Note that A d,c d is observable and Ad,B d,w is controllable. Thus, if wd (k) and v d (k) are stationary zero-mean Gaussian white noises, the Kalman filter gain L d, the covariance matrices M d and Z d will converge to stationary matrices. However, w d (k) and v d (k) are often interpreted as fictitious noise terms and their covariances are adjusted to assign a reasonable set of closed loop eigenvalues to the estimator. In practice, they are chosen such that the estimator dynamics is five to ten times faster than that of the controller or fast enough to suppress the effect of perturbations[51]. Another way to deal with these covariances is to use the adaptive scheme in the next section. Remark 3.1. The Kalman filter structure employing the dynamic or kinematic model is illustrated in Fig. 3.2 or Fig. 3.3, respectively. If only the accelerometer measurement is available on the load side for the estimation purpose, i.e., the gyroscope measurement is not used in the system output (3.3) or (3.8), the proposed method can still be formulated in a similar way. However, it is better to include the gyroscope measurement, sinceθ lf,s uses a fictitious measurementλ θ mf,s to formulate KKF, and θ l,s is the only load side real measurement in the KKF model output in (3.8). The effects of the additional gyroscope will be shown in the experimental study in Section 3.3. Remark 3.2. The KKF has several advantages compared with the dynamic model based Kalman filter (DKF)[51]. Firstly, system representation using kinematic model is simpler than the one using dynamic model. Secondly, the kinematic model is an exact representation of the system states. It involves neither physical parameters nor external disturbances. Thus no model uncertainties need to be considered in the KKF.

52 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 35 θ mf,s + α s+α n ms + θ m,s + θm b v + n vl L d e mf e l θ l,s θl n al + b a + + ˆθ lf,s KKF ˆθl θ l θls ˆθ l,s Figure 3.3: Structure of Kinematic Model Based Kalman Filter (KKF) Estimation of Noise Covariance As an optimal stochastic estimator, the Kalman filter assumes linear model and Gaussian additive noises with known covariances. In reality, however, it is difficult to get the accurate covariances of the process and/or measurement noises. Thus, particular interest has been focused on the estimation of the noise covariances, i.e.,ˆq d (k) and/orˆr d (k). In the proposed Kalman filter, the bias noises (n ba and n bv ), and the outputθ lf,s, are fictitious and their covariances cannot be physically identified. Practically, if the measurement bias is time invariant or slowly varying, it is desired that the fictitious bias covariance should be large enough at the beginning to steer the bias estimate quickly to its actual value. Then the fictitious noise covariance should be decreased to reduce the sensitivity of the bias estimation. To handle uncertainties in these covariances, an adaptive approach utilizing the residual information to estimate the noise covariances is applied. The routines presented here are modified from the algorithms summarized in[62, 61, 2], which were developed based on the principle of maximum likelihood estimate[34, 14]. More details of this presented method can be found in Appendix C. By comparing the a-priori and a-posteriori state estimates, the process noise covariance estimateˆq d (k) is updated by ˆQ d (k)=b 1 d,w xd (k) x T d (k)+ Z d(k) A d Z d (k 1)A T d x d (k)=ˆx d (k) ˆx o d (k) ˆQ d (k+ 1)=ˆQ d (k)+ ˆQ d (k) ˆQ d (k) /N Q (B 1 d,w )T (3.12a) (3.12b) (3.12c) where the adaptive estimate ˆQ d (k) is shown to be the residual pseudo-covariance plus the change of the a-posteriori covariance between the two consecutive time steps. The current step

53 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 36 covariance estimate ˆQ d (k) is then updated to this estimate ˆQ (k) in an exponential moving d average manner with N Q as the window size. Note that, if B d,w is not invertible, B 1 in (3.12a) d,w can be replaced by the following pseudo-inverse B T B = B 1 d,w d,w B T, B d,w d,w is thin; (3.13a) d,w Bd,w T Bd,w Bd,w T 1, Bd,w is fat. (3.13b) by Similarly, the measurement noise covariance estimate,ˆr d (k), can be adaptively updated ˆR (k)= y d d(k) y T (k)+c d d Z d (k)c T d y d (k)= y d (k) C dˆx d (k) ˆR d (k+ 1)=ˆR d (k)+ ˆR d (k) ˆR d (k) /N R (3.14a) (3.14b) (3.14c) where y d (k) is not the innovation but the a-posteriori estimation error. The physical interpretation of this solution is that the theoretical value of measurement noise covariance should match with the estimation residual covariance (see Appendix C). Remark 3.3. Note that, the above adaptation for the process (or measurement) noise covariance estimate,ˆq d (orˆr d ), is the sub-optimal solution developed separately assuming that the actual covariance R d (or Q d ) is known[61]. The two solutions can be used, with caution, to estimate both covariances simultaneously, or can be implemented one after another in the serial way, or can be confined to adaptˆq d (orˆr d ) only. Remark 3.4. The adaptive performance is sensitive to the window sizes of the moving average filters. Thus, they should be carefully selected for each application. Sometimes it may be preferred to use small window sizes at the beginning to speed up the covariance estimation process. After the covariances are converged, the window sizes can be changed to a larger value to maintain smooth estimation performance. 3.3 Experimental Study Experimental Setup The proposed method is implemented on a single-joint indirect drive robot setup shown in Fig. 2.3 in Chapter 2. The load side encoder is used only for performance evaluation, while the gyroscope and the accelerometers are used for the estimation algorithm. The algorithm is implemented with the sampling rate of 1kHz. The noise ofθ m,s is bounded by the encoder resolutionδθ m,s. This gives the approximate output noise variance var(n sm )=δθ 2 m,s /12[51]. The noise variances for θ l,s and θ l,s can be

54 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 37 Table 3.1: The Noise Variance Used in Chirp Experiment (SI Units) n u n sm n lf,s n vl n al n bv n ba obtained by zero-acceleration and zero-velocity experiments. The fictitious noise variances of θ lf,s, u s and the two measurement biases, i.e., var(n lf,s ), var(n u ), var(n ba ), and var(n bv ), cannot be determined experimentally (no torque measurement is available in this experimental setup), and thus could be used as design parameters or with covariance adaptation for the estimation performance Load Side Estimation Experiments The performance validation is conducted for the following five estimation configurations: DKF-AG: Dynamic model based Kalman filter with accelerometers and gyroscope DKF-A: Dynamic model based Kalman filter with accelerometers only KKF-AG: Kinematic model based Kalman filter with accelerometers and gyroscope KKF-A: Kinematic model based Kalman filter with accelerometers only Torsion: Use motor encoder outputθ m,s as the load side position directly. In this case the load side position estimation error is given by the joint torsionδ l,s = θ m,s N θ l,s. Estimation without Covariance Adaption or Parametric Uncertainty: Open Loop Frequency Rich Response This experiment is to test the effectiveness of the above algorithm configurations in the open loop frequency rich response. The motor torque command is a quadratic chirp signal with the magnitude of.2nm. The frequency range varies from.5hz to 5Hz quadratically in 5sec as shown in Fig In this experiment, the algorithms are tested without covariance adaptation or parametric uncertainty to see the general benefits of the load side sensors. The fixed noise variances used in the algorithms are listed in Table 3.1. These variances are mostly the nominal variances identified experimentally as described in Section 3.3.1, except for the ones of n lf,s, n u, n ba, and n bv. Besides,β in the low-pass filter in KKF algorithms is selected to be 1, which gives α= f b. The nominal dynamic parameter values from Table 2.1 in Chapter 2 are used in the DKF-AG and DKF-A methods. The estimation errors of the load side position by five estimation configurations are compared in Fig In Fig. 3.5, the rms value on the right top of each sub-figure denotes the

55 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 38 6 Chirp Input Signal Frequency 5 Frequency (Hz) Time (sec) Figure 3.4: Chirp Input Signal Frequency Torsion DKF AG DKF A KKF AG KKF A Unit: rad 2 x 1 3 rms=.327 x1 3 rad x 1 3 rms=.116 x1 3 rad x 1 3 rms=.116 x1 3 rad x 1 3 rms=.15 x1 3 rad x 1 3 rms=.181 x1 3 rad Time (sec) Figure 3.5: Estimation Errors of Load Side Position in Chirp Experiment (Without Covariance Adaptation or Parametric Uncertainty)

56 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 39 Unit: rad Torsion DKF AG KKF AG 2 x 1 3 rms=.327 x1 3 rad x 1 3 rms=.199 x1 3 rad x 1 3 rms=.99 x1 3 rad Time (sec) Figure 3.6: Estimation Errors of Load Side Position in Chirp Experiment (With Covariance Adaptation and Parametric Uncertainty) root-mean-square value of the estimation error. It shows that the KKF-AG method achieves the best estimation performance, the rms estimation error of which is less than 1 of the one 3 by directly usingθ m. The utilization of the gyroscope does not benefit the dynamic model based algorithms, but gives much improvement to the kinematic model based algorithms. This makes sense since the dynamic model (3.3) usesθ m,s, θ l,s, and θ l,s as the model output, all of which are available measurements, while the kinematic model (3.8) only usesθ lf,s and θ l,s as the model output, whereθ lf,s is actually using θ mf,s as the approximated measurement. N Thus, in the following testing, performance comparisons will be conducted only for the case that both accelerometers and gyroscope are available, i.e., DKF-AG and KKF-AG. Estimation with Covariance Adaption and Parametric Uncertainty: Open Loop Frequency Rich Response In this experiment, the same open loop frequency rich response is utilized. The covariance estimation algorithm is tested with the same initial noise variances listed in Table 3.1 except that the variances of n ba and n bv are reset to 1 4 and.1 (different from the tuned suboptimal values). Also, J l and k j are added with 2% error to check the performance of DKF when subjected to the parameter uncertainty. Note thatβ in the KKF method is selected as.5, i.e., α=.5f b. The adaptive covariance estimation scheme is confined to adaptˆv d only.

57 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 4 DKF AG R(1,1) DKF AG R(2,2) DKF AG R(3,3) KKF AG R(1,1) KKF AG R(2,2) 5 x x x x Time (sec) Figure 3.7: Covariance Estimation in Chirp Experiment Figure 3.6 shows that the KKF-AG method still achieves the best estimation performance, where the estimation error is greatly reduced upon all the testing frequency range. The DKF- AG method does not perform as good as KKF-AG, and it amplifies the estimation error at some frequency range due to the parameter uncertainty. The adapted ˆV d in both methods is shown in Fig As expected, to match the covariance with the estimation results,ˆv d tends to increase when the estimation error is increasing (especially around the resonant frequency 19Hz at about 3sec). Figure 3.8 shows that both KKF-AG and DKF-AG are effective to estimate the biases of gyroscope and accelerometer outputs. Thus, the bias effects in the utilization of sensor signals are attenuated. Estimation with Covariance Adaption and Parametric Uncertainty: Closed Loop Trajectory Tracking This experiment is to verify the effectiveness of the methods for specific tracking trajectory. The desired load side trajectory is shown in Fig. 3.9, which is designed based on the fourthorder smooth time optimal trajectory suggested in[55]. The feedback controller used here is illustrated in Fig. 6.1 in Chapter 6 with basic PID and feedforward controller. All the covariances and dynamic parameter values remain the same as those in the second chirp experiment (Fig. 3.6) except for the window size N R used in the covariance adaptation scheme.

58 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 41 Bias (rad/sec) 15 x 1 3 Bias Estimation of Load Side Gyroscope DKF AG KKF AG Identified Bias Time (sec) Bias Estimation of Load Side Accelerometer Bias (rad/sec 2 ).4.2 DKF AG KKF AG Identified Bias Time (sec) Figure 3.8: Bias Estimation in Chirp Experiment 1 Position (rad) Velocity (rad/sec) Acceleration (rad/sec 2 ) Time (sec) Time (sec) Time (sec) Figure 3.9: Load Side Desired Trajectory

59 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 42 Unit: rad Torsion DKF AG KKF AG 1 x 1 3 rms=.384 x1 3 rad x 1 3 rms=.215 x1 3 rad x 1 3 rms=.166 x1 3 rad Time (sec) Figure 3.1: Estimation Errors of Load Side Position in Trajectory Experiment Load Side Position Tracking Error Estimation (rad) x 1 4 Load Side Position Tracking Error Estimation Using θ m,s Actual Error KKF AG Time (sec) Figure 3.11: Load Side Actual and Estimated Position Tracking Error

60 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 43 The result of this experiment is shown in Fig. 3.1, where KKF-AG still achieves the best estimation performance. It is seen that the remaining estimation error by KKF-AG is oscillatory. This is mainly due to the transmission error effect, since this effect is not considered in the estimation algorithm. Figure 3.11 illustrates the actual and estimated tracking errors of the load side position in this experiment. It shows that the tracking error estimated by KKF-AG captures most trends of the actual tracking error. Therefore, the KKF-AG method is utilized in the load side position tracking application, e.g., friction compensation in Chapter Discussion Note that, the DKF and KKF methods are individually formulated based on separated models. However, it may be beneficial to combine both the dynamic model and the kinematic model for Kalman filter formulation. The following two ways may be used to achieve this objective. First, in KKF, instead of simple first order low-pass filter G f (s)= α, the nominal dynamic transfer function θ l (s) θ m (s) ˆdj s+ˆk j N Ĵ l s 2 +(ˆd j +ˆd l )s+ˆk j can be used to obtain the load side position approximation from the motor side encoder measurement, where ˆ denotes the nominal value of. The problem with this approach is that it results in a higher order model formulation which introduces more complexity and computation load. Actually, it may not be favorable to use high order model since its high frequency response will always be subject to large error due the model uncertainties. Furthermore, one can note that nonlinear dynamic terms such as Coulomb friction and transmission error are still not considered in this method due to the utilization of the linear transfer function. The second way to approximate the load side position measurement is to use the dynamic model (2.6a), which yields θ l,s = 1 ˆd j s+ˆk j ˆkj N θ m,s+ s+α ˆd j N θ m,s N u s +ˆd m Ĵ mˆ θm,s ˆd m θm,s (3.15) whereθ m,s and θ m,s are obtained from motor encoder measurements. u s can be either motor torque command or measured by motor current. The desired trajectory θ md can be used instead of ˆ θm,s in (3.15) as approximation. By this approach, the nonlinear dynamic terms have been taken into account asˆd m if good model knowledge of these terms is available. This first order system formulation is relatively easy and efficient. Thus it will be adopted in the load side state estimation method for multi-joint robot discussed in Chapter Chapter Summary This chapter presented the load side state estimation algorithm for the single-joint indirect drive robot based on a multi-sensor configuration. Kalman filtering method was formulated

61 CHAPTER 3. LOAD SIDE STATE ESTIMATION IN SINGLE-JOINT ROBOT 44 using the extended dynamic/kinematic model taking measurement dynamics (bias and noise) into consideration. Noise covariance adaptation was studied. The effectiveness of the method was verified experimentally on a single-joint indirect drive robot setup. The developed KKF method will be applied to the friction compensation application in Chapter 6.

62 45 Chapter 4 End-effector State Estimation in Multi-Joint Robot 4.1 Introduction In Chapter 3 on load side state estimation, the kinematic Kalman filter (KKF) algorithm was formulated for the single joint robot by fusing the measurements of load side inertial sensors (accelerometer and gyroscope) with the measurement of the motor side encoder. In this chapter, this KKF algorithm is extended to the multi-joint robot to estimate the end-effector information. In robot applications, the end-effector performance enhancement is the ultimate objective. In general, position, velocity, and orientation information of the end-effector is all important for motion control. Without consideration of cost and assembly issues, many different kinds of end-effector senors could be utilized for such purpose, such as vision camera for position/pattern sensing, inertia sensor for acceleration and angular velocity sensing, and laser tracker for position sensing. However, none of these sensors alone would be sufficient for obtaining all the desired end-effector information. For example, the PSD camera (see Appendix A.4.3) developed in[94] has good capability on position sensing. Its measurement alone, however, is not sufficient to support fine velocity or orientation estimation. Also, almost all sensors exhibit some kind of limitations, such as slow sampling rate for vision camera, noneligible bias and noise in low-cost MEMS sensors. To address these issues, it is favorable to investigate the multi-sensor fusion approach to complement each individual sensor s limitations. Some previous work on the multi-sensor fusion for the multi-joint robot has been reported in[53]. In[53], the idea of multi-dimensional kinematic Kalman filter (MD-KKF) algorithm using the measurements of the vision camera and inertia sensors was proposed. The effectiveness of the method was validated through experiments on a two-link direct-drive robot. However, the technical potential of this approach cannot be fully validated on this simple 2-DOF planar robot.

63 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 46 In this chapter, the idea of MD-KKF method is revisited and implemented on the 6-DOF FANUC M-16iB robot system. In order to follow the idea of fusing vision sensor signals with inertia sensor signals, the CompuGauge 3D measurement output is resampled and adjusted to typical vision sensor specifications to simulate the vision sensor measurements. Furthermore, another experimental study is conducted with the inertia senor and the newly developed PSD camera, which is capable of end-effector position sensing with fast sampling rate and high precision. Promising results on the tool center point (TCP) velocity/orientation estimation have been demonstrated. This chapter is organized as follows. A multi-dimensional (MD) kinematic model of the robot end-effector is introduced first in Section 4.2. Then the idea of MD-KKF method is revisited in Section 4.3. Finally, the experimental study of the method on the FANUC M-16iB robot is presented in Section 4.4. The conclusion of this work is given in Section 4.5 at last. 4.2 MD Kinematic Model MD Rigid Body Motion For the MD-KKF scheme on the FANUC M-16iB robot system, consider the sensor configuration and coordinate system shown in Fig The CompuGauge 3D measurement point is used to simulate the virtual TCP, the absolute position of which is assumed to be measured by 3D vision sensors. There are two infrared markers attached on the end-effector for the PSD camera sensing (see[94] for more details). The inertia measurement unit (IMU) (3-axial gyroscope and 3-axial accelerometer) is attached to a fixed point on the end-effector of the robot. For convenience, denote the inertia sensor measurement point as the wrist. The coordinate frame of the inertia sensor is aligned with the body coordinate frame of the end-effector, denoted by (x b, y b, z b ). The spatial (world) coordinate frame is represented as (x s, y s, z s ). The desired and the actual paths that TCP is following are shown as dash line and solid line in the figure, respectively. Based on the above coordinate system definition, the MD rigid body motion on the endeffector can be described as ṗ s T C P = vs wr + R ω b r b T C P/wr v s = wr Rfb + wr gs Ṙ=R ω b (4.1a) (4.1b) (4.1c) where p s and T C P vs are the position of the TCP and the velocity of the wrist in world coordinates, (x s, y s, z s ), respectively. R is the rotational matrix of the end-effector mapping from wr the body coordinate frame to the world coordinate frame.ω b and f b are the angular velocity wr and the linear acceleration of the wrist in the body frame, (x b, y b, z b ), respectively. Note that the accelerometer includes the gravity effect g s = T g z in the actual measurement. Finally, r b denotes the position vector of the TCP relative to the wrist in the body frame. T C P/wr

64 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 47 z s Wrist O s y s x s Inertia Sensors O b y b Infrared Markers x b z b TCP Desired Path Actual Path Figure 4.1: Sensor Configuration and Coordinate System of FANUC M-16iB Robot End-effector Note that,[a ] R 3 3 is the skew-symmetric matrix, equivalent to the cross product operation of the corresponding vector, i.e., for a,b R 3,[a ]b a b. More specifically, for a= T, a 1 a 2 a 3 a 3 a 2 [a ]= a 3 a 1 (4.2) a 2 a Sensor Measurement Dynamics The inertia measurement unit (IMU), which consists of a set of MEMS sensors, has nonnegligible sensor biases and noises that need to be accounted for when utilizing the measurement outputs. Also the vision sensors accuracy is also be subject to certain biases and noises. This sensor measurement dynamics can be modeled as follows ḃ ω = n bω ω b =ω b + b ω + n ω ḃ f = n bf fb = wr fb + b wr f+ n f ḃ p = n bp p s T C P = ps T C P + b p+ n p (4.3a) (4.3b) (4.3c) (4.3d) (4.3e) (4.3f) where is the real quantity to be measured and is the corresponding measurement output. b is the bias of the measurement output, which is modeled to be updated by the fictitious noise n b. n is the noise of the measurement.

65 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT State-Space Model Representation Equations (4.1) and (4.3) can be simply rewritten in the state-space model as where ẋ e = Ax e + B u u e + B w w e y e = Cx e + n p x e = (p s T C P )T (v s wr )T b T ω bt f b T p T (4.4a) (4.4b) y e = p s T C P u e = (ω b ) T ( f b wr )T (g s ) T T w e = n T ω nt n T f b ω n T b f n T T b p I 3 R r bt C P/wr R A= R r bt C P/wr R I 3 B u = R r bt C P/wr R B w = I 3 I 3 I 3 C= I 3 I 3 and I 3 is a 3 3 identity matrix. Based on the nonlinear time-varying model (4.4), the nonlinear Kalman filter such as extended Kalman filter or unscented Kalman filter can be formulated. Prior to doing so, however, several problems need to be addressed. First, the unknown rotation matrix R appears in the system matrices, A, B u, and B w. Secondly, the differential equation (4.1c) of the rotation matrix R cannot be formulated in a linear ODE form in this state-space model. Additionaly, there are too many fictitious noise terms in this model. These issues make implementation of the Kalman filter impractical. Thus, a different approach to formulate the Kalman filter needs to be pursued to estimate the interested end-effector information. The alternative approach is presented in the following section.

66 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 49 Camera IMU End-effector Position Measurements Wrist Gyro. & Accl. Measurements MD-KKF based on MD Kinematic Model TCP Estimation Position Velocity Orientation Figure 4.2: Basic Structure of MD-KKF Scheme 4.3 MD Kinematic Kalman Filter The MD-KKF approach proposed in[53] is revisited and extended in this section. The basic structure of this scheme is shown in Fig This scheme fuses the end-effector position measurement from the camera device with the acceleration and angular velocity information from the inertia sensors. The Kalman filter is formulated based on the MD kinematic model (4.1) to estimate the TCP information. To investigate the feasibility of the MD-KKF scheme, an assumption is made to simplify the design process. Assume that the biases in the sensor measurements are time-invariant (or very slowly time-varying), and they have been accurately identified and removed in the utilization of the measurement outputs. As a result, the sensor bias terms (b ω, b f, b p ) are neglected. Then the system model is simplified to focus on the basic MD rigid body motion in (4.1) Prediction by Inertia Sensor Measurement From (4.1), the open loop estimation of the continuous time states can be written as ˆp so T C P =ˆvso +ˆR o ω b r b wr T C P/wr ˆv so =ˆR o fb + wr wr gs ˆR o =ˆR o ω b whereˆ o denotes the open loop estimate of the corresponding variable. The discrete time form of the open loop estimator (4.6) is then given by ˆR o (k+ 1)=ˆR o (k) I 3 + T s [ ω b (k+ 1) ]+ T 2 s 2 [ ωb (k+ 1) ] 2 ˆp so so (k+ 1) I3 T T C P ˆv so = s I 3 ˆp (k) T C P (k+ 1) I wr 3 ˆv so(k) wr ˆR o (k+ 1)[r b + T ] T s T C P/wr s 2ˆR o (k+ 1) ˆR o (k+1) ω b (k+ 1) Ts + T I 2 3 fb (k+ 1) s g s wr I 3 (4.6a) (4.6b) (4.6c) (4.7a) (4.7b)

67 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 5 whereˆ o (k) denotes the a-priori estimate of the corresponding variable at the k-th time step. And T s is the sampling time for the estimation algorithm. This discrete time form is obtained using second order Taylor approximation. Note that for simplicity, the measurements, ω b and f b, are assumed to be invariant within one sampling period. Also, the rotation matrix wr estimateˆr o is calculated first and then assumed to be invariant within one sampling period in the estimation of p s and T C P vs. wr Propagation of Estimation Errors As stated in Section 4.2, direct Kalman filtering for estimation of the original states, p s, T C P vs, wr and R in (4.1), is not easy to implement. Thus, a different approach is utilized by investigation of the propagation of the open loop estimation errors. The a-priori estimation errors of the original states, p s, T C P vs, and R in (4.1), are defined wr asδp,δv, andψ, respectively ˆp so T C P = ps T C P +δp ˆv so = wr vs +δv wr ˆR so =(I 3 Ψ)R (4.8a) (4.8b) (4.8c) where the use ofψwould help linearize the differential update equation of R. Ψ assumes a skew-symmetric form given by ψ 3 ψ 2 Ψ= ψ 3 ψ 1 (4.9) ψ 2 ψ 1 whereψ 1,ψ 2, andψ 3 can be interpreted as the small misalignment errors for the roll, pitch, and yaw angles. Following the derivation stated in[53], the propagation equations of the estimation errors can be obtained as δṗ= δv R r ω bt b ψ ˆR r o bt n C P/wr C P/wr ω (4.1a) δ v= Rf b wr ψ+ˆr o n f (4.1b) ψ= ˆR o n ω (4.1c) whereψ= ψ 1 ψ 2 ψ 3 T. Note that (4.1c) achieves the way of using a linear differential equation to update the rotation matrix estimateˆr indirectly. Define the new state variables for estimation errors asδx= δp T δv T ψ T T R 9, the new output asδy=( p s T C P ˆpso T C P ) R3, and the new noise variable vector as w= T n T ω f nt R 6. Then the propagation equations of the estimation errors can be written into the following

68 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 51 state-space form δẋ = F(t)δx + G(t)w δy= p s T C P ˆpso = p s + n T C P T C P p p s +δp = Cδx+n T C P p where C= I 3 3 6, and I 3 F(t)= R r ω bt b C P/wr Rf b wr,g(t)= ˆR o r b T C P/wr ˆR o ˆR o (4.11a) (4.11b) (4.12) The discrete time form of the above state-space error propagation model (4.11) can be obtained by using the ZOH (zero-order-hold) equivalents as δx(k + 1) = Φ(k)δx(k) + Γ(k)w(k) δy(k)=cδx(k)+n p (k) (4.13a) (4.13b) whereφ(k)=e F(t k)t s,γ(k)= T s ef(t k)τ G(t k )dτ, and t k = kt s. Remark 4.1. Note that (4.13) is a time-varying linear model, and the system matrices, Φ(k) and Γ(k), are parameterized byω b, f b wr, R, andˆr o. Besides the difficulty of obtaining the unknown true quantity R, it will also require a lot of computational effort to do real-time estimation based on this model. In the applications such as trajectory tracking, the desired trajectory is known beforehand. Thus, if the robot controller is good enough to track the desired trajectory sufficiently close, the reference trajectory can be used instead of the real one to calculate these system matrices, which can be done off-line to reduce the real-time computation load Correction by Vision Sensor Measurement Suppose the vision sensors provide the signals at the sampling time of N v T s with the measurement delay of one sampling period, N v T s, while the inertia measurement unit (i.e., gyroscope and accelerometer) is running at the sampling time of T s with no time delay. This means the multi-rate Kalman filtering is needed here. Following the standard Kalman filtering process, the a-priori estimationδˆx o (k) can be obtained by the open loop estimation from the inertia sensor measurement and/or reference trajectory at the sampling time of T s. Then the a-posteriori estimation δˆx(k) is obtained by correction from the vision sensor measurement at the sampling time of N v T s. The details are given below δˆx o (k+ 1)=Φ(k)δˆx(k), k=, 1, 2,... (4.14a) δˆx o (k)+ L(k)δỹ o (k), k=, N v, 2N v,... δˆx(k) = δˆx o (4.14b) (k), Otherwise δỹ o (k)=δy(k) Cδˆx o (k), k=, N v, 2N v,... (4.14c)

69 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 52 where the Kalman filter gain L(k) is calculated by L(k)= M(k)C T [CM(k)C T +ˆV(k)] 1, k=, N v, 2N v,... (4.15a) M(k+ 1)=Φ(k)Z(k)Φ T (k)+γ(k)ŵ(k)γ T (k), k=, 1, 2,... M(k) L(k)CM(k), k=, Nv, 2N v,... Z(k)= M(k), Otherwise (4.15b) (4.15c) where Ŵ(k) and ˆV(k) are the covariance estimates of w(k) and n p (k), respectively. M(k) and Z(k) are the one-step a-priori and a-posteriori covariances of the state estimation error, respectively. Note that due to the slow sampling rate of the vision sensor measurements, the a-posteriori correctionδỹ o (k) from the vision measurement and the Kalman filter gain L(k) are calculated at the sampling time of N v T s. Furthermore, with the measurement delay, the error propagation output δy(k) is actually based on the vision measurement available at the (k+n v )-th time step. More specifically, when calculating the error propagation outputδy(k), (4.11b) is modified to δy(k)= p s T C P (k+n v) ˆp so T C P (k)=cδx(k)+n p(k), k=, N v, 2N v,... (4.16) Thus, an N v -time-step delay is required for the a-priori estimate to be corrected by the vision sensor measurement, which doubles the computation load for the prediction step. For example, to obtain the estimation from the N v -th to the (2N v 1)-th time step, the following procedure needs to be carried out: 1. For k=n v,...,2n v 1, the open-loop estimation is done using (4.14a) and the second case of (4.14b). 2. At the 2N v -th time step, the vision measurement for the N v -th time step becomes available. 3. Equation (4.14c) and the first case of (4.14b) are used to recalculate the a-posteriori estimateδˆx(k) for k=n v using the newly obtained 2N v -th time step vision measurement. 4. Equation (4.14a) and the second case of (4.14b) are used again to recalculate the a- posteriori estimateδˆx(k) for k=n v + 1,...,2N v 1. Finally, denotingδˆx= δˆp T δˆv T ˆψT T R 9, the a-posteriori estimations of the position, the velocity, and the rotation matrix are updated as follows ˆp s T C P (k)=ˆpso (k) δˆp(k) T C P ˆv s wr (k)=ˆvso (k) δˆv(k) wr ˆR(k)= I 3 ˆψ(k) 1ˆR o (k) (4.17a) (4.17b) (4.17c)

70 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 53 Table 4.1: Identified Variances for the IMU Measurements for Sensor Fusion Using IMU & CompuGauge 3D Sensor Variable Value Units Gyroscope Accelerometer σ x x σ (rad/sec) 2 y y σ zz σ 2.75 x x σ [m/(sec 2 )] 2 y y σ zz Remark 4.2. Note that, the recalculation of the a-posteriori estimate δˆx(k) in Step 4 is necessary for better open loop prediction in the following sampling period for k=2n v,..., 3N v 1. However, this recalculation result is actually not available in real-time. Thus, the original a-posteriori estimateδˆx(k) for k=n v + 1,..., 2N v 1 are still used in (4.17) for real-time estimation. In contrast, if off-line estimation is desired, this recalculation resultδˆx(k) for k=n v +1,..., 2N v 1 should be used in (4.17). 4.4 Experimental Study The proposed method is implemented on the FANUC M-16iB robot system (Fig. 2.5) equipped with the inertia measurement unit (IMU) (Analog Devices, ADIS164), the CompuGauge 3D measurement system (Version 4.3), and the newly developed PSD camera. The details of this setup are introduced in Section in Chapter 2 and also in Appendix A. The sensor configuration and the coordinate system of the end-effector are illustrated in Fig Sensor Fusion Using IMU & CompuGauge 3D The absolute position measured by CompuGauge 3D measurement system (at the sampling time of 1ms) is used as the true position data to evaluate the estimation performance. This measurement is also used to simulate the measurements from a stereo vision camera set, which is to measure the relative position from the predefined reference trajectory. To do this, the CompuGauge 3D measurement output is resampled using 1ms sampling time with 1ms latency time and is adjusted to have the precision of.15mm (assumed specifications for the vision sensor). The covariances of the inertia sensors for this experiment are pre-identified and listed in Table 4.1. The testing trajectory is shown in Fig. 4.3, which is a point-to-point (P2P) scanning trajectory with a 4-th order smooth derivative as suggested in[55]. The tracking controller is a simple decentralized PID feedback controller with feedforward torque, which will be detailed

71 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 54 Position X (m) Position Y (m) Position Z (m) CG (Real) MD KKF "Vision" Time (sec) (a) TCP Position Estimation Velocity X (m/sec) Velocity Y (m/sec) Velocity Z (m/sec).5.5 CG (Real).1 MD KKF "Vision" Time (sec) (b) TCP Velocity Estimation 3.4 Roll X (rad) Pitch Y (rad) 3.2 Encoder MD KKF Yaw Z (rad) Time (sec) (c) TCP Orientation Estimation Figure 4.3: TCP Estimation for P2P Scanning Trajectory (with IMU & CompuGauge)

72 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 55 in Chapter 7. The controller and the MD-KKF method are running at the sampling time of 1ms. The fictitious vision sensor provides the position measurement at the sampling time of 1ms with 1ms time delay, i.e., N v = 1. The Kalman filter gain for each correction step is calculated beforehand using the desired trajectory. Figure 4.3a shows the TCP position estimation result. It can be seen that the estimated TCP position (MD-KKF) captures the true position (CG(Real)) smoothly and closely, while the fictitious vision measurement ("Vision") shows the time delay and slow sampling rate effects. The TCP velocity and orientation estimation results are shown in Fig. 4.3b and Fig. 4.3c respectively. The benefits of the MD-KKF method can be appreciated clearly from these two figures. The velocity data (CG(Real)), generated by backward numerical differentiation of the original CompuGauge measurement, is used as the true velocity data to evaluate the performance. The velocity data generated similarly from the fictitious vision measurements ("Vision"), however, appears to be very noisy compared to the clean velocity estimation (MD-KKF). The orientation estimation is illustrated by Roll-Pitch-Yaw Euler angle set[74]. Figure 4.3c shows that the orientation estimation (MD-KKF) also closely follows the orientation data calculated from the forward kinematics of the motor encoder data (Encoder), while the vision sensor or CompuGauge 3D does not provide any orientation measurement Sensor Fusion Using IMU & PSD Camera The PSD camera developed in[94] provides another mean of end-effector position sensing with fast sampling rate and high precision. With this new TCP position sensing device, the MD- KKF scheme is applied again to further validate the sensor fusion method for TCP information estimation. Note that, since the PSD camera achieves a high sampling rate, the drawbacks of the low sampling rate and measurement delay in the normal vision camera system are no longer an issue. Actually, the original PSD camera measurement is filtered and downsampled to get the same sampling rate (i.e., 1kHz) as that of the IMU signals and the estimation algorithm. Thus, in the MD-KKF algorithm, it is set that N v = 1 without measurement delay. Again, the absolute position measured by CompuGauge system is used as the true value for performance evaluation. Note that, the developed PSD camera has good capability on the position sensing (see Fig. 4.4). Thus, the main objective here is to obtain better TCP velocity/orientation estimate rather than the TCP position estimate. The covariances of the inertia sensor and PSD camera measurements are pre-identified for each experiment and are listed in Table 4.2. The slight difference of the variances between different experiments are due to the time-varying property of the MEMS sensor for environmental changes, and also the PSD calibration process for each experiment. The larger covariances of PSD camera measurement for curve trajectory are due to the combination of measurements for two markers (see second set of experiment later). Note that the current setting (i.e., one camera and one/two markers) of PSD camera is only capable of two-dimensional position sensing (at X-Z plane). Thus, in order to implement the MD-KKF algorithm, a fictitious Y-axis position measurement is added into the final PSD

73 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 56 End effector Position Z (mm) CompuGauge PSD camera End effector Position X (mm) End effector Position Z (mm) (a) Square Path End effector Position X (mm) End effector Position Z (mm) End effector Position X (mm) End effector Position Z (mm) End effector Position X (mm) (b) Circle Path Figure 4.4: Measurement Comparison Between PSD Camera and CompuGauge Table 4.2: Identified Variances for the IMU and PSD Camera Measurements for Sensor Fusion using IMU & PSD Camera Sensor Variable Trajectory Square Circle Curve Units Gyroscope Accelerometer PSD σ x x σ (rad/sec) 2 y y σ zz σ x x σ [m/(sec 2 )] 2 y y σ zz σ x x m 2 σ zz

74 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 57 Position X (m) Position Z (m) PSD.9 CG (Real) MD KKF Time (sec) (a) TCP Position Estimation Velocity X (m/sec) Velocity Z (m/sec).5 PSD CG (Real) 3.5 MD KKF Time (sec) (b) TCP Velocity Estimation Figure 4.5: TCP Estimation for Square Trajectory without Orientation Change camera measurement. Without loss of generality, this Y-axis position measurement is set to be simply the same constant value as in the robot home position. And the variance of this fictitious measurement is set to be the same as X-axis measurement. Two sets of experiments are conducted as follows. Using IMU & PSD Camera (One Marker) In the first set, one marker is installed on the end-effector, providing two-dimensional position sensing without rotation measurement. The testing TCP trajectories are designed to be only within the X-Z plane without orientation changes. Two testing TCP trajectories are shown in Fig The first one is a 16mm 16mm square with a peak Cartesian speed of 3mm/sec.

75 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 58 Position X (m) Position Z (m) PSD CG (Real).65 MD KKF Time (sec) (a) TCP Position Estimation Velocity X (m/sec) Velocity Z (m/sec) PSD.4 CG (Real) MD KKF Time (sec) (b) TCP Velocity Estimation Figure 4.6: TCP Estimation for Circle Trajectory without Orientation Change The second one is a circle of radius of 8mm with a peak Cartesian speed of 2mm/sec. Both trajectories have same fixed orientation as the robot home position. Figure 4.5a and Fig. 4.6a show the TCP position estimation results of MD-KKF method for two trajectories, respectively. This again verifies that the PSD position measurement is already satisfactory. Thus sensor fusion for position estimation is not necessary. In contrast, the benefits of the MD-KKF method are evident from the TCP velocity estimation results, which are shown in Fig. 4.5b and Fig. 4.6b for the two trajectories, respectively. The velocity data (CG(Real)), generated by differentiation of the CompuGauge measurements, is used as the true velocity data to evaluate the performance. The velocity data (PSD) generated similarly from the PSD measurement, however, is very noisy compared to the clean velocity estimation (MD-KKF).

76 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 59 Figure 4.7: End-effector Path with Orientation Change Infrared Marker TCP Figure 4.8: Locations of Two PSD Markers and the TCP Using IMU & PSD Camera (Two Markers) In the second set of experiments, two markers are installed on the end-effector (Fig. 4.7), with a 12mm distance between them. The motion of the end-effector is still constrained in the X-Z plane, with orientation change allowed only along Y axis. With two markers, the TCP position can be determined by triangulation based on the measured positions of the markers and their known locations relative to the TCP (Fig. 4.8). The markers are controlled by a micro-processor (Atmel, Mega328) to flash alternately, and thus sensed by the PSD camera alternately. The end-effector path is illustrated in Fig Figure 4.9 shows the estimation result, where the noisy velocity data obtained by differentiating PSD measurements is unacceptable and the PSD measurement alone does not provide orientation information. Thus, the result from MD-KKF shows significant superiority again, especially for velocity and orientation estimation. However, compared with the first set of experiments where no orientation change is involved, the velocity estimation along Z axis shows some drifting phenomena. This may

77 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 6 be due to the calibration inaccuracy of the gyroscope. To address this, a more complex sensor fusion algorithm with the consideration of sensor self-calibration may be necessary. 4.5 Chapter Summary This chapter presented the study on the sensor fusion work for the purpose of end-effector state estimation. An MD-KKF scheme for the multi-joint robot based on the work in[53] was re-investigated. Some minor extensions were presented and the method was then implemented on the 6-DOF FANUC M-16iB robot system with equipped inertia measurement unit, CompuGauge 3D, and also newly developed PSD camera. The experimental results showed that the proposed method was effective to estimate the TCP information using the fusion of the sensor signals at two different sampling rates (e.g., inertia sensor signals and fictitious vision sensor signals), and was especially beneficial for TCP velocity/orientation estimation. However, this algorithm was formulated for the simplified case without consideration of sensor biases. Thus, self-calibration technique of sensor signals to incorporate with the MD-KKF scheme may be a key aspect if the estimation performance is to be further improved.

78 CHAPTER 4. END-EFFECTOR STATE ESTIMATION IN MULTI-JOINT ROBOT 61 Position X (m) Position Z (m) PSD CG (Real).7 MD KKF Time (sec) (a) TCP Position Estimation.5 Velocity X (m/sec) Velocity Z (m/sec).5 PSD 1 CG (Real) MD KKF Time (sec) (b) TCP Velocity Estimation 3.5 Roll X (rad) 3 Pitch Y (rad) Yaw Z (rad) Encoder MD KKF Time (sec) (c) TCP Orientation Estimation Figure 4.9: TCP Estimation for Curve Path with Orientation Change

79 62 Chapter 5 Load Side Joint State Estimation in Multi-Joint Robot 5.1 Introduction As discussed in previous chapters, in robot applications, discrepancies between the available and the desired measurements make it difficult to achieve good control performance. These discrepancies are caused by both sensor and robot dynamics. Particularly, in robots with complex joint dynamics (e.g., flexibilities, friction, etc.), end-effector performance cannot be guaranteed with motor side information alone. This mismatched sensing problem is a critical issue for most practical robot applications where only motor side information is available. In previous chapters, it has been proposed that this problem can be tackled by adopting low-cost MEMS sensors, such as an accelerometer, for robot end-effector sensing. Since decoupled joint space position/velocity control is usually preferred in industrial robot control, end-effector sensing with end-effector state estimation only is not ideal/sufficient. Therefore, it would be most beneficial to be able to estimate the load side joint states from the endeffector measurements. In Chapter 3, a Kalman filter scheme using either dynamic model or kinematic model was investigated for a single-joint robot with joint elasticity. The scheme fused the motor encoder measurements with the load side inertia sensor signals to estimate the load side position. The multi-dimensional kinematic Kalman filter (MD-KKF) proposed in[53] was reinvestigated in Chapter 4 for multi-joint robot end-effector sensing. Several end-effector sensors (i.e., camera, gyroscope, and accelerometer) were required. The approach, however, was not able to directly achieve joint space estimation nor utilize available motor encoder information. In[69, 27], joint angle estimation was achieved utilizing an accelerometer (and a gyroscope) for each joint without the use of motor encoders. The achieved accuracy was only good for service robots where millimeter-order errors are acceptable. In[48, 1, 9], the load side state estimation problem was formulated as an extended Kalman filter (EKF) or particle filter (PF) problem. Both filters utilized motor encoders and end-effector accelerometer. The computation load,

80 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 63 however, was nontrivial due to the complex dynamic model and the EKF/PF algorithms. Thus, the methods were only intended for the applications such as iterative learning control, where off-line computing is feasible. In this chapter, a sensor fusion scheme, which is computationally light and suitable for various applications, is proposed for the multi-joint robot with joint elasticity equipped with an end-effector accelerometer. In Section 5.2, an optimization based inverse differential kinematics approach is designed to obtain the joint acceleration estimates. Section 5.3 continues with the estimation scheme as a decoupled kinematic Kalman filter for each joint to estimate the load side joint position and velocity. Expectation maximization (EM) ([73, 32, 38]) is utilized for determining the unknown parameters off-line. An online solution is also proposed. The computation load and extensions to other sensor configurations are discussed in Section 5.4. Section 5.5 presents the simulation and experimental study on the 6-DOF FANUC M-16iB robot. The conclusion is given in Section 5.6 at last. 5.2 Robot Inverse Differential Kinematics Dynamic Model for Rough Estimates Consider a 6-DOF robot manipulator with n elastic joints (n 6). The dynamic model (2.24) of this robot with gear compliance is restated as M l (q l ) q l + C(q l, q l ) q l + G(q l )+ D l q l + F lc sgn( q l )+ J(q l ) T f ex t = K J N 1 q m q l q + D J N 1 q m q l q (5.1) M m q m + D m q m + F mc sgn( q m )=τ m N 1 K J N 1 q m q l q + D J N 1 q m q l q (5.2) where all the variable definitions are the same as in Chapter 2. Due to the joint compliance dynamics, discrepancies exist between the motor side joint position q m and the load side joint position q l (i.e., q m q l in general). Normally, q m can be measured by motor side encoders, while q l, which is of ultimate interest, is not measurable due to the lack of load side sensor. With (5.2), the load side joint position q l can be roughly estimated as ˆq o =(ˆD l J s+ˆk J ) ˆK 1 J N 1 q m ˆ q +ˆD J N 1 q m ˆ q N τ m ˆM mˆ q m ˆD m q m ˆF mc sgn( q m ) (5.3) whereˆ denotes the nominal value of the dynamic parameter. q m and q m are obtained from motor encoder measurements, andτ m can be either motor torque command or measured by motor current. The desired trajectory q md can be used instead ofˆ q m in (5.3) as approximation. The transmission error effects can be omitted for simplicity due to the lack of measurement,

81 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 64 i.e.,ˆ q= andˆ q=. Furthermore, with Euler differentiation ofˆq o, the rough estimate of the l load side joint velocity,ˆ q o, is obtained. l Note that the rough estimateˆ q o could be acceptable but noisy. So the load side joint l acceleration estimateˆ q l cannot be obtained by direct differentiation ofˆ q o. Also, this rough l estimate by (5.3) is subject to the model uncertainty. Thus, it is necessary to adopt an endeffector sensor such as accelerometer to supplement the lacking information Basic Differential Kinematics Let v e = ṗ T e ω T e T R 6 denotes the end-effector Cartesian velocity vector composing of the translational velocity ṗ e and the angular velocityω e at the accelerometer mounting point. The kinematic relation between the joint space and the Cartesian space can be described as[74] v e = J(q l ) q l (5.4) And the acceleration relationship is obtained accordingly by taking the time derivative of both sides of (5.4), which gives v e = J(q l ) q l + J(q l, q l ) q l (5.5) In practice, even if equipped with an additional gyroscope, the measured angular velocity will be contaminated with additive sensor noise. Thus, it is not a good idea to obtain the angular acceleration (last three rows of v e ) by simply differentiating the gyroscope measurements. So it is impractical to simply use (5.5) in the actual implementation. Here, to set the problem further restrictive, assume that only three-dimensional translational acceleration measured by the end-effector accelerometer is available for control/sensor fusion. Let J(q l ) R 3 n and J(q l, q l ) R 3 n denote the first three rows of the Jacobian matrix, J(q l ), and its time derivative, J(q l, q l ), respectively. Then (5.5) reduces to p e = J(q l ) q l + J(q l, q l ) q l (5.6) This provides the base to fully retrieve the load side joint acceleration information from the limited-dimensional end-effector measurements Optimization Based Inverse Differential Kinematics Define the pseudo-inverse of J(q l ) as J (q l )= J(q l ) T J(q l ) J(q l ) T 1 (5.7) Then from (5.6), the load side joint acceleration estimate can be solved as the following general solutions ˆ q l = J (q l ) p e J(q l, q l ) q l + I n J (q l ) J(q l ) ϕ (5.8)

82 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 65 where I n is an n n identity matrix andϕ R n is an arbitrary vector. The first term on the right hand side, J (q l ) p e J(q l, q l ) q l Null J(q l ) Row J(q l ) T, is the particular solution which minimizes the norm of the solution ˆ q l. The second term, I n J (q l ) J(q l ) ϕ, is the projection ofϕ into Null J(q l ) and is termed as homogeneous solution. The choice ofϕ is thus important for selecting an appropriate estimate for the load side joint acceleration. The redundancy of these infinite solutions makes it possible to enforce some practical constraints. Rewrite (5.6) as J(q l ) q l = p e J(q l, q l ) q l Āˆ q l = b (5.9) which becomes a constraint for the satisfactory load side acceleration estimateˆ q l. Therefore, the inverse differential kinematics problem can be reformulated as the following standard optimization problem min f(ˆ q l ) s.t. Āˆ q l = b (5.1) ˆ q l where the imposed physical constraint is expressed as to minimize f(ˆ q l ) Final Optimization Solution With the rough estimateˆ q o ˆ q o dt from the Euler differentiation l l ofˆqo in (5.3), a leastsquares optimization problem can be formulated as l min f(ˆ q l )= 1 2 ˆ q l 2 ˆ q l dt ˆ q o dt s.t. Āˆ q l l = b (5.11) This optimization problem over the whole time series would be impractical to solve especially within the real-time environment. Instead, a point-wise optimization can be performed for each time step to generate the sub-optimal solution. For each time step t k, let 2 tk ˆ q o = ˆ q o (t)dt, ˆ q l,k l l,k = k ˆ q l,i t (5.12) i= where the subscript k denotes the time index and t is the sampling time. Then (5.11) is relaxed to a convex optimization problem for each time step t k as min f(ˆ q l,k )= 1 ˆ q l,k ˆ q 2 l,k (5.13) 2 ˆ q l,k 2 s.t. A kˆ q l,k = b k where A k = J(q l,k ), b k = p e,k J(q l,k, q l,k ) q l,k (5.14)

83 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 66 and ˆ q l,k = ˆ q o l,k ˆ q l,k 1 also includes the accumulated acceleration estimation error not compensated from previous steps. The resulting problem (5.13) has the global optimal closed form t solution (i.e., load side joint acceleration estimate) as ˆ q l,k = A T Ak A T 1 bk + I k k n Ak T Ak A T 1 Ak ˆ q k l,k (5.15) which is in the form of the general solution (5.8) Practical Implementation Issues In practice, the acceleration measurement f a provided by the end-effector accelerometer is the translational acceleration with additional gravity effect expressed in the accelerometer coordinate frame. Thus, the end-effector translational acceleration p e in the world coordinates is obtained as p e = R a (q l )f a + g s (5.16) where R a (q l ) is the rotation matrix of the accelerometer coordinate frame with respect to the world coordinate frame and g s = 9.8 T m/sec 2 is the gravity vector expressed in the world coordinate frame. Furthermore, since the measurements of q l and q l are generally not available, the rough estimatesˆq o andˆ q o are used instead in (5.14) and (5.16) to calculate J(q l l l ), J(q l, q l ) q l, and R a (q l ). 1 These adjustments for the calculation are reasonable under the fact that the tiny discrepancies between the actual motion and the rough estimates normally do not make much difference in the Jacobian matrices and the orientation matrix. 5.3 Kinematic Kalman Filter Decoupled Kinematic Kalman Filter As shown above, the load side rough approximations have been obtained asˆq o l in (5.3) andˆ q l in (5.15) for each joint. Thus, the estimation problem for the whole robot can be decoupled into n kinematic Kalman filters (KKF) running in parallel, which are computationally efficient, to refine the estimates for the load side joint position and velocity. The discrete time kinematic 1 The calculations of the Jacobian J(q l ) and its time derivative J(q l, q l ) are detailed in[74, 12, 22]. The calculation of the rotation matrix R a (q l ) can be found in[74].

84 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 67 model for the Kalman filter is written as ql,k+1 In ti 1 = n ql,k + 2 t2 I n ˆ q q l,k+1 I n q l,k ti l,k +w }{{}}{{}}{{} n }{{} k (5.17a) }{{} u k x k+1 A x k B In ql,k +v k (5.17b) ˆq o = l,k }{{} y k }{{} C which is in the following standard form q l,k }{{} x k x k+1 = Ax k + Bu k + w k y k = C x k + v k (5.18a) (5.18b) with the assumption that x 1 X 1 =(ˆx 1, P 1 ), w k W k =(,Q), v k V k =(, R), 1 k T, where T is the total number of time steps for the executing trajectory. Then in the off-line case, the Kalman smoother using the following forward recursion and backward recursion procedures[32] can be applied to estimate the state: Forward recursion ˆx k k 1 = Aˆx k 1 k 1 + Bu k 1 P k k 1 = AP k 1 k 1 A T +ˆQ K k = P k k 1 C T (C P k k 1 C T +ˆR) 1 ˆx k k =ˆx k k 1 + K k (y k Cˆx k k 1 ) P k k = P k k 1 K k C P k k 1 P (k,k 1) k =(I n K k C)AP k 1 k 1 (5.19a) (5.19b) (5.19c) (5.19d) (5.19e) (5.19f) Backward recursion L k 1 = P k 1 k 1 A T P 1 k k 1 ˆx k 1 T =ˆx k 1 k 1 + L k 1 (ˆx k T ˆx k k 1 ) P k T = P k k + L k (P k+1 T P k+1 k )L T k P (k,k 1) T = P k T P 1 P k k (k,k 1) k (5.2a) (5.2b) (5.2c) (5.2d) whereˆ k j represents the conditional expectation of k given the information up to the j-th time step.ˆq andˆr are the estimates of Q and R. The online version of the Kalman filter is the causal forward recursion part (5.19) only. Note that, P k j denotes the error covariance ofˆx k j, while P (k,k 1) j denotes the cross error covariance ofˆx k j andˆx k 1 j, i.e. T P k j = E xk ˆx k j xk ˆx k j (5.21) T P (k,k 1) j = E xk ˆx k j xk 1 ˆx k 1 j (5.22)

85 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 68 Process for whole time series ˆx 1,ˆP 1,ˆQ,ˆR Kalman Smoother (E-Step) Maximization (M-Step) Iterates (a) Off-line Estimation Structure ˆQ(k), ˆR(k) One-step Kalman Filter One-step Maximization k k+ 1 (b) Online Estimation Structure Figure 5.1: Adaptive Kinematic Kalman Filter Process Recall thatˆq o l,k andˆ q l,k are only approximations instead of direct measurements. Thus, to implement this KKF, it is critical to determine the appropriate covariances for the fictitious noises w k and v k. This meansˆx 1, P 1,Q, and R are the unknowns to be estimated first, which is detailed in the following section Parameter Estimation These parameters can be adapted based on the maximum likelihood principle[34, 14, 73, 32, 38]. The derivation of the following estimation solutions for this specific problem is detailed in Appendix C. The adaptive KKF procedure is illustrated in Fig Off-line Estimation (Fig. 5.1a) If off-line processing is available, which is applicable in iterative applications, the whole time series data can be accessed. In this case, expectation maximization (EM) algorithm ([73, 32, 38]) can be applied as follows E-step: run Kalman smoother (5.19)-(5.2) with current best estimates ofˆx 1, P 1,Q, and R. M-step: updateˆx 1, P 1,Q, and R as in (5.23) using the acausal estimates from Kalman

86 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 69 smoother (5.19)-(5.2). ˆx 1 =ˆx 1 T ˆP 1 = P 1 T (5.23a) ˆQ= 1 T T ˆxk T Aˆx k 1 T Bu k 1 ˆxk T Aˆx k 1 T Bu k 1 T 1 k=2 + P k T AP T P (k,k 1) T (k,k 1) TA T + AP k 1 T A T (5.23b) ˆR= 1 T T T+C yk Cˆx k T yk Cˆx k T Pk T C T k=1 (5.23c) Return to E-step until the increment of the expected likelihood is within chosen threshold. Online Estimation (Fig. 5.1b) If real-time computing is desired, only causal estimates from forward recursion (5.19) (i.e., Kalman filter) can be used. Furthermore, instead of estimating using the whole time series as in (5.23), the covariances Q and R are adapted for each time step as ˆQ o k+1 =ˆx k k Aˆx k 1 k 1 Bu k 1 ˆxk k Aˆx k 1 k 1 Bu k 1 T + P k k AP T (k,k 1) k P (k,k 1) ka T + AP k 1 k 1 A T (5.24a) ˆR o k+1 = y k Cˆx k k yk Cˆx k k T+C Pk k C T (5.24b) In practice, to avoid drastic change to the covariances, exponential moving average can be applied to control the adaptive rate for smooth estimation. This is done as 1 1NQ ˆQ k+1 = ˆQ k + 1 N QˆQ o k+1 ˆR k+1 = 1 1NR ˆR k + 1 N RˆR o k+1 (5.25a) (5.25b) where N Q and N R are the window sizes for the moving average filters. ˆQ k andˆr k are the estimated covariance matrices actually utilized in the online Kalman filter. Also, note that, the initial conditionsˆx 1 and P 1 is not adapted in this real-time case. 5.4 Discussion of the Approach Computation Load It should be noted that one of the great advantages of the proposed method over others is its light computation load. As shown in Fig. 5.2, this approach mainly consists of two stages,

87 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 7 To Decouple Problem Motor Encoder End-effector Accelerometer Optimization Based Inverse Kinematics Joint (Pos/Vel/Acc) Rough Estimates Decoupled Adaptive KKF TCP (Pos/Vel/Acc) Estimates Forward Kinematics Joint (Pos/Vel/Acc) Estimates Figure 5.2: The Structure of Load Side State Estimation Approach optimization based inverse differential kinematics and KKF with parameter estimation. The forward kinematics stage is additional if the tool center point (TCP) information is desired. At the inverse differential kinematics stage, the main computation lies in (5.3), which is diagonal matrix calculation due to the property of motor side model, and (5.15), where a 3 3 matrix inversion requires the most effort. The calculations of (5.14) and (5.16) are basically 3 1 vector operations and 3 3 matrix multiplications. After this stage, the problem becomes decoupled Kalman filter (or smoother) for each joint with the kinematic model of only 2 states, 1 input, and 1 output. The matrix inversion becomes scalar inversion for Kalman filter case and 2 2 matrix inversion for smoother case, both of which are computationally simple. In the parameter estimation, only 2 2 matrix multiplications and scalar operations are required. The optional forward kinematics stage, which consists of n 4 4 matrix multiplications, can also be efficiently processed. Therefore, the overall computation load for the proposed approach is sufficiently light for both online computing and off-line processing with limited onboard industrial computation power Extensions to Other Sensor Configurations The developed approach is designed for the case where motor encoders and the end-effector accelerometer are available. It should be noted, however, that the extensions to other sensor configurations can also be easily derived. If motor encoders and the end-effector gyroscope are available, the optimization problem (5.11) in the inverse differential kinematics stage can be modified to obtain the load side joint velocity estimate as min f(ˆ q l )= 1 ˆ q l ˆ q o 2 s.t. J(ˆq o l 2 l ˆ q l 2 )ˆ q l =ω e (5.26) where J(ˆq o l ) R3 n denotes the last three rows of J(ˆq o ). The Kalman filter (smoother) is then l

88 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 71 simplified to be the one with a first order kinematic model q l,k+1 = q l,k + tˆ q l,k + w k, ˆq o l,k = q l,k+v k (5.27) Note that only q l and q l can be estimated due to the lack of accelerometer. However, if the rotational vibration is the motion of interest to observe, this approach would be suitable. Similarly, if the robot is only equipped with motor encoders and the end-effector position sensor such as a vision camera, the inverse differential kinematics stage (5.15) becomes the case to obtain the load side joint position estimate as min f(ˆq l )= 1 ˆql ˆq o 2 s.t. J(ˆq o ˆq l 2 l l 2 )ˆq l=p e (5.28) This stage needs to be iterated with newly updatedˆq o l ˆq l until the solution converges. The Kalman filter (smoother) stage becomes unnecessary and the final approach is only suitable for estimating the load side joint position q l. 5.5 Simulation & Experimental Study Test Setup The proposed method is implemented on the 6-joint FANUC M-16iB robot as shown in Fig The motor encoders and the 3-axial end-effector accelerometer (Analog Devices, ADIS164) are utilized for the load side state estimation algorithm. The CompuGauge 3D system is utilized to measure the end-effector tool center point (TCP) position as a ground truth for performance validation. The sampling rates of all the sensor signals as well as the real-time controller implemented through MATLAB xpc Target are set to 1kHz. Furthermore, a robot simulator (see Appendix A.5) has been designed using MATLAB Simulink & SimMechanics Toolbox based on robot dynamic and kinematic parameters as well as the calibrated sensor parameters. Therefore, robot dynamic simulation is available with the access of load side joint information for performance evaluation. The experimental validation, however, can be conducted only in Cartesian space to compare with CompuGauge 3D and accelerometer measurements Algorithm Settings The testing TCP trajectory (Fig. 5.3) is a 1cm 1cm square path on the Y-Z plane with fixed orientation and maximum speed of 1m/sec. For this motion, all joints except Joint 4 need to be moved. The estimation algorithm settings for comparisons are listed as follows: 1. KKF-Offline: Off-line estimation with EM and Kalman smoother. A 3Hz zero phase lowpass filter is applied to the raw accelerometer measurements f a and the rough estimate ˆ q o l.

89 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 72 Z Axis (m) Z Axis s (m) Y Axis (m) Y-Z Plane Position Trajectory KKF-Online KKF-OnlineFix KKF-Offline CG3D Motor Y Axis (m) Z Axis (m) Y Axis (m) Figure 5.3: Y-Z Plane TCP Position Estimation (Experiment) 2. KKF-Online: Online estimation with Kalman filter forward recursion only and online covariance updating using window sizes N Q = N R = 5. A 1Hz causal low-pass filter is applied to the raw signals. Note that this low-pass filter will introduce some phase delay to the signals and thus the bandwidth is chosen as 1Hz as the trade off between the phase delay effect and high frequency noise filtering. 3. KKF-OnlineFix: The same as KKF-Online except that the covariances Q and R are pretuned in advance and fixed in the online KKF process. 4. CG3D: Real measurement and its differentiation from CompuGauge 3D system. 5. Motor: Using motor side information (i.e., simulated motor side position, velocity, and acceleration in the simulation, or motor encoder signals in the experiment) directly as load side information to estimate Simulation Results The load side joint estimation errors calculated from the simulated load side information are plotted using absolute values in Fig. 5.4, Fig. 5.5, and Fig It is shown that the proposed

90 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 73 Absolute Error (rad) 6 x Joint 1 Position Motor KKF-OnlineFix KKF-Online Motor KKF-Offline KKF-Offline / KKF-Online / KKF-OnlineFix Absolute Error (rad) 8 x KKF-Online / KKF-OnlineFix Joint 2 Position Motor KKF-Offline Time (sec) 2 x 1-3 Joint 3 Position Time (sec) 1.2 x 1-4 Joint 4 Position Absolute Error (rad) KKF-Offline KKF-Online / KKF-OnlineFix Motor Absolute Error (rad) Motor KKF-Offline / KKF-Online / KKF-OnlineFix Time (sec) 6 x 1-4 Joint 5 Position Time (sec) 2 x 1-3 Joint 6 Position Absolute Error (rad) 5 4 KKF-Offline / KKF-Online / 3KKF-OnlineFix 2 1 Motor Absolute Error (rad) Motor KKF-Offline / KKF-Online / KKF-OnlineFix Time (sec) Time (sec) Figure 5.4: Load Side Joint Position Estimation Absolute Error (Simulation)

91 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 74 Absolute Error (rad/sec) Ab bsolute Error (rad/sec) Joint 1 Velocity Motor KKF-Offline KKF-Online / KKF-OnlineFix Time (sec) KKF-Online / KKF-OnlineFix Joint 3 Velocity Motor KKF-Offline locity Absolute Error (rad/sec) KKF-OnlineFix KKF-Online Motor KKF-Offline.2 Ab bsolute Error (rad/sec) KKF-Offline Joint 2 Velocity KKF-Online / KKF-OnlineFix Time (sec) KKF-Offline / Motor Joint 4 Velocity KKF-OnlineFix KKF-Online Motor Time (sec) Joint 5 Velocity Time (sec) Joint 6 Velocity Absolute Error (rad/sec) KKF-Offline / Motor KKF-Online / KKF-OnlineFix Absolute Error (rad/sec) Motor KKF-Offline / KKF-Online / KKF-OnlineFix Time (sec) Time (sec) Figure 5.5: Load Side Joint Velocity Estimation Absolute Error (Simulation)

92 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 75 2 Joint 1 Acceleration 1.4 Joint 2 Acceleration Absolute Error (rad/sec 2 ) Abs solute Error (rad/sec 2 ) Absolute Error (rad/sec 2 ) Motor KKF-Offline KKF-Online / KKF-OnlineFix Time (sec) 1.5KKF-Online / KKF-OnlineFix Joint 3 Acceleration Motor KKF-Offline locity Time (sec) KKF-Online / KKF-OnlineFix Joint 5 Acceleration KKF-Offline Time (sec) Motor Absolute Error (rad/sec 2 ) Abs solute Error (rad/sec 2 ) KKF-OnlineFix KKF-Online Motor KKF-Offline Absolute Error (rad/sec 2 ) KKF-Offline KKF-Online / KKF-OnlineFix Time (sec) Joint 4 Acceleration Motor KKF-Offline Motor KKF-Online / KKF-OnlineFix Time (sec) Joint 6 Acceleration KKF-Offline Motor KKF-Online / KKF-OnlineFix Time (sec) Figure 5.6: Load Side Joint Acceleration Estimation Absolute Error (Simulation)

93 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 76 KKF schemes outperform the Motor setting significantly. Particularly in Fig. 5.4, for Joint 2, 3, and 5, where gravity effects are evident, the position estimation by Motor suffers from noticeable offset error, while the KKF schemes successfully account for the gravity effects. Among the KKF schemes, the online velocity and acceleration estimates (Fig. 5.5 and Fig. 5.6) from KKF-Online and KKF-OnlineFix are not clean due to the high-bandwidth low-pass filter applied to the raw signals, while KKF-Offline provides the best estimates among the all. Even though for some particular instances, KKF-Online and KKF-OnlineFix may perform noisier than Motor in simulation, it is still beneficial to use KKF-Online and KKF-OnlineFix since in reality Motor does not provide velocity/acceleration measurements Experimental Results The methods are also implemented on the actual experimental setup for Cartesian space comparisons. The load side joint state estimates are used in the forward kinematics to obtain the Cartesian space estimates for comparisons. Figure 5.3 shows the estimated position trajectory on the Y-Z plane. Again, it is clearly seen that all the KKF settings perform much better than the Motor setting by capturing closer transient motion on Y Axis and with much less offset on Z Axis. The superior performance of the proposed schemes can be better appreciated for the residual vibration sensing when the robot comes to a stop as shown in Fig. 5.7 and Fig In general, the Motor setting cannot capture the vibratory motion at the end-effector and the resulting TCP acceleration estimation is very noisy, while the proposed KKF schemes are able to do both very well with the fusion of end-effector accelerometer measurements. It is shown in Fig. 5.7 that the proposed KKF estimates closely follow the CG3D measurements and the Accelerometer measurements. In particular, during the stopping period (i.e., after 3.85sec for Y Axis and after 3.5sec for Z Axis), the Motor estimates look static. The actual residual motion, however, is vibratory and can be successfully captured by the KKF estimates. Recall that, the CompuGauge measurement and its differentiation (CG3D) are used as the ground truth for position and velocity estimation performance evaluation. In the acceleration comparison, however, the real accelerometer measurement (Accelerometer) is used instead of the noisy CG3D data differentiation as the ground truth. Another benefit of the proposed KKF schemes is that the velocity/acceleration estimates are even cleaner than the CG3D setting which is obtained by direct differentiation from the position measurement, even though there are also some minor noises present in the KKF- Online and KKF-OnlineFix estimates. The estimation errors 2 for end-effector TCP position/velocity/acceleration when the robot is coming to a stop are illustrated in Fig The above conclusions are confirmed again in this figure. Among all the KKF settings, KKF-Offline performs the best due to its acausal processing availability. KKF-Online and KKF-OnlineFix perform similarly, which indicates that 2 This Cartesian space error is defined as the Euclidean distance between the estimated position/velocity/acceleration and the actual ones.

94 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 77 Position (m) Y Axis Position Motor CG3D KKF-Online / KKF-OnlineFix KKF-Offline Time (sec) Y Axis Velocity Position (m) Z Axis Position KKF-Online Motor CG3D KKF-OnlineFix KKF-Offline Time (sec) Z Axis Velocity Velocity (m/sec) CG3D Motor KKF-Offline / KKF-Online / KKF-OnlineFix Time (sec) Y Axis Acceleration Velocity (m/sec) KKF-Offline KKF-Online / KKF-OnlineFix Motor -.2 CG3D CG3D Motor KKF-OnlineFix Time (sec) KKF-Online KKF-Offline Z Axis Acceleration Accelerometer 3 Acceleration (m/sec 2 ) Accelerometer Motor KKF-Offline / KKF-Online / KKF-Online Fix CG3D Time (sec) Acceleration (m/sec 2 ) CG3D Accelerometer / KKF-Offline / KKF-Online / KKF-OnlineFix Motor Time (sec) Figure 5.7: TCP Estimation on Y & Z Axes When Coming to a Stop (Experiment)

95 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 78 Pos Error (mm) Cartesian Position Error Comparisons Motor KKF Offline / KKF Online / KKF OnlineFix Motor KKF OnlineFix KKF Online KKF Offline Vel Error (m/sec) Time (sec) (a) TCP Position Estimation Error Cartesian Velocity Error Comparisons Motor Motor KKF OnlineFix KKF Online KKF Offline KKF Offline / KKF Online / KKF OnlineFix Time (sec) (b) TCP Velocity Estimation Error Acc Error (m/sec 2 ) Cartesian Acceleration Error Comparisons Motor KKF Online / KKF OnlineFix KKF Offline CG3D Motor KKF OnlineFix KKF Online KKF Offline CG3D Time (sec) (c) TCP Acceleration Estimation Error Figure 5.8: TCP Estimation Error When Coming to a Stop (Experiment)

96 CHAPTER 5. LOAD SIDE JOINT STATE ESTIMATION IN MULTI-JOINT ROBOT 79 Table 5.1: TCP Estimation Errors When Coming to a Stop (Experiment) RMS Errors Pos. (mm) Vel. (mm/sec) Acc. (mm/sec 2 ) KKF-Offline KKF-Online KKF-OnlineFix Motor with properly pre-tuned covariances, the online computing can be further simplified without sacrificing much performance. These conclusions can also be drawn from the root-meansquare (RMS) estimation errors for position/velocity/acceleration as listed in Table 5.1, which shows that the RMS estimation errors can be reduced by the proposed KKF schemes to about half or even less of that of the Motor estimates. 5.6 Chapter Summary This paper investigated the load side state estimation problem for the robots with mismatched sensing, i.e., robots with joint elasticity. The problem was tackled by using a combination of motor encoders and a low-cost end-effector sensor (e.g., an end-effector accelerometer). The direct joint space estimation was achieved for the purpose of decentralized joint control. With the equipped end-effector accelerometer, the load side joint acceleration was estimated through an optimization based inverse differential kinematics algorithm. The problem was then decoupled into n simple kinematic Kalman filters to estimate the load side joint position and velocity. Both off-line and online solutions were presented for determining the fictitious noise covariance. The proposed approach is computationally efficient for both off-line applications and online computing. Simulation and experimental study on a 6-DOF industrial robot demonstrated the superior performance of the developed method and the necessity of the end-effector sensing.

97 8 Part II Handling Mismatched Dynamics: Real-time Control Approaches

98 81 Chapter 6 Hybrid Adaptive Friction Compensation in Single-Joint Robot 6.1 Introduction As pointed out in Chapter 2, the mismatched dynamics and sensing problem is common among the robots with indirect drive trains. These indirect drive trains, e.g., robotic joints that utilize gear transmissions, are commonly used in industrial applications due to their high torque capacity. The indirect drive transmissions, however, set challenges in high precision motion control because of gear compliances as well as nonlinearities such as friction and torque hysteresis. Particularly, the nonlinear friction effects on the load side are difficult to compensate for because: 1) the load side position measurement is often unavailable (i.e., mismatched sensing), and 2) it is indirectly compensated for through the gear transmission (i.e., mismatched dynamics). Since the load side performance is critical in practical applications, a method that compensates for the load side friction as well as the motor side friction is necessary. Over the past several decades, modeling and compensation of the friction effects have been intensively studied from various viewpoints, including static/dynamic characterizations and feedback/feedforward compensation schemes[8, 15, 82, 5]. For example, many modelbased friction compensation techniques have been developed to reject the friction effects by adaptively[13, 33] or robustly[98] estimating the real friction. Also, typical approaches such as the disturbance observer[66] or repetitive control[83, 85] have been used to compensate for friction. Most of these studies, however, validated their performance based on simple one-mass systems or direct drives only. When a two-mass system or an indirect drive is considered, the friction effects introduced from the load side remain as a challenge. For this reason, friction characteristics in harmonic drive systems were studied in[76, 35]. The performance and stability issues due to friction forces (e.g., limit cycles) were addressed in[52]. Some compensation schemes were proposed to overcome the friction effects in harmonic drives. Most of them, however, were still focused on the motor side compensation only[35, 31, 46]

99 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 82 or torque tracking control[77]. To improve the load side position tracking performance, the position information of both the motor side and the load side was utilized in the controller design[95, 79]. Such controllers, however, are difficult to implement in industrial robots, which are usually not equipped with precise load side position sensors. In this chapter, a hybrid adaptive two-stage friction compensation scheme for indirect drive trains is developed in the absence of precise load side position measurements. The load side state estimation method proposed in Chapter 3 is utilized in the compensation algorithm. The proposed friction compensation method manipulates both the torque input and the reference trajectory, which is controlled by a hybrid system scheme based on the tracking performance. The proposed method is verified by the experimentation on a single-joint indirect drive setup. 6.2 System Overview Single-Joint Indirect Drive Model The schematic of a single-joint indirect drive mechanism was illustrated in Fig. 2.1 in Chapter 2. The equations of motion (2.15) for this system are restated as J m θm = u 1 θm f m + f h k j N N θ l θ θm + d j N θ l θ (6.1a) θm J l θl = f l + k j N θ l θ θm + d j N θ l θ (6.1b) where all the variables are as defined in Section Note that the external disturbances d f m and d fl are omitted here since this chapter will focus on friction compensation rather than compensation of external disturbances Modified Friction Model As described in Section 2.2.2, the friction effect in the indirect drive train is characterized by three friction forces, i.e., the motor side bearing friction, f m, the load side bearing friction, f l, and the reducer gear meshing friction, f h. The combination of system model (6.1) with friction effect gives J m θm + 1 N J l θ l = u F (6.2a) F= f m + f h + f l (6.2b) N where F represents the entire friction force imposed on the whole system when reflected to the motor side. As suggested in Section 2.2.2, the load side friction f l could be roughly modeled as a scaled quantity from the entire friction force F with r l as a scaling factor, i.e. f l = r l F (6.3)

100 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 83 To facilitate the development of the adaptive friction compensation algorithm, the LuGre model (2.17), which was used in Section to describe F, is further modified as ż=v β v g(v) z g(v)= F c + F s F c e ( v 2 /v 2 s) F=σ z+σ 1 ż+σ 2 v (6.4a) (6.4b) (6.4c) where all the variables exceptβ in this modified model are as defined in Section Here in (6.4a),β is used to denote the nominal micro-stiffnessˆσ of the internal state z (i.e., the deflection of bristles between the two contacting surfaces). This differs from the standard LuGre model (2.17), and the modification will be shown to simplify the friction compensation algorithm. From (6.4), the friction force, F, can be written as: F= K T Φ (6.5) where K= σ 1 +σ 2 σ β σ 1 T,Φ= v z v g(v) z T. Notice that the modified LuGre model still preserves the following property of standard LuGre model[15]: Property 6.1. F c g(v) F s. If z() F s β, then z(t) F s β, t. In reality, it can be assumed that the deflection of bristles starts at rest, i.e., z()=, without loss of generality. Therefore, z(t) F s β, t. This property will be used in the subsequent controller design to ensure the bounded stability of the friction observer. Remark 6.1. In practice, friction characteristics may vary due to the variations of the normal contact forces, lubricant condition, temperature, material wear, and so on[13]. Variations in the normal forces usually cause an impact on the values of only static parameters, i.e., F C, F S, v s, and σ 2, while the changes in lubricant condition, temperature and/or materials may affect both static and dynamic parameters. Remark 6.2. If only static friction is considered, i.e., ż=, the modified LuGre model reduces to F(v)= σ β F c + F s F c e ( v 2 /v 2 s) sgn(v)+σ 2 v (6.6) This indicates that, by fixing the nominal micro-stiffness,β, the adaptation of real micro-stiffness, σ, effectively changes the level of static parameters, e.g., F C and F S in g(v). Thus, the adaptation ofσ,σ 1, andσ 2 can account for most parameter variations in the modified friction model regardless of the dynamics for internal state z. Based on this, K in (6.5), which consists ofσ, σ 1, andσ 2, may be adapted in real-time withφas the regressor.

101 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 84 ˆF F O θ m, ˆθ l KKF θ ld ˆf l F 1 F 2 θ md + C fb u fb + + u ff u P θ l θ l θ m Figure 6.1: Block Diagram of the Overall Control System Controller Structure Figure 6.1 shows the overall control structure of the single-joint indirect drive system, where P denotes the system plant with the measurable outputs as the motor side positionθ m, load side velocity θ l, and load side acceleration θ l. The controller structure consists of two feedforward controllers, F 1 and F 2, one feedback controller, C f b, the state estimator, KKF, and the friction observer, F O. Here the state estimator K K F utilizes the kinematic Kalman filter (KKF-AG) method developed in Chapter 3 to estimate the load side positionθ l using gyroscope and accelerometer measurements. The friction observer F O detailed in the following sections estimates the entire friction F and the load side friction f l to be used in the feedforward controllers F 1 and F 2. The feedforward controller, F 2, is designed from (6.2a) as u f f (t)= J m θmd (t)+ 1 N J l θ ld (t)+ˆf(t) (6.7) whereˆf is the estimated entire friction force. θ ld is the desired load side acceleration. In the case where the acceleration is measured by sensors (e.g., load side accelerometers), θ ld can be replaced with θ l.θ md is the motor side position reference generated by F 1 from the desired load side position,θ ld, i.e. θ md (t)= N k j Jl θld (t)+ˆf l (t) + Nθ ld (t) (6.8) whereˆf l is the estimated load side friction. Equation (6.8) is derived from (6.1b) by neglecting the transmission error θ θm and the joint damping d j, since k θ θm j N l dj θ N l usually holds within the system bandwidth. The feedback controller, C f b, which consists of a proportional position control loop and a proportional-integral (PI) velocity control loop, can be described as C f b (s)= k v + k s+kp i (6.9) s

102 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 85 where k p is the gain of the position loop, k v and k i are the gains of the velocity loop. The above controller is discretized by the Euler method for implementation. 6.3 Friction Compensation As shown in the controller structure (6.7)-(6.8), the proposed friction compensator consists of two parts. The motor side friction compensation is designed in the feedforward controller F 2 by injecting into the torque input u, and the load side friction effects are further compensated for in the feedforward controller F 1 by manipulating the motor side reference,θ md Motor Side Friction Compensation Feedback Friction Compensation Let e m =θ m θ md be the motor side position tracking error. Define a first order sliding surface, p m, as p m = ė m + k s e m (6.1) where k s is a positive constant. Thus e m is small or converges to zero if p m is small or converges to zero, since e m(s) = 1 is asymptotically stable. p m (s) s+k s From (6.2a) and (6.1), the motor side error dynamics is derived as J m ṗ m = u F 1 N J l θ l J m θmd + J m k s ė m (6.11) The controller structure (Fig. 6.1) yields the following control law u(t)=u f f (t)+u f b (t) = J m θmd (t)+ 1 N J θ l ld (t)+ˆf(t) k v s m (t)+ k i s m (τ)dτ t (6.12) where s m (t)= ė m (t)+ k p e m (t) is the error term in the velocity loop. By applying the adaptive control technique[75], an adaptive friction observer is designed to obtainˆf(t), i.e. ˆF=ˆK TˆΦ (6.13) whereˆφ= θ m ẑ θ T m.ˆk, ẑ g( θ, and ẑ 1 are the estimates obtained from the following m)ẑ1

103 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 86 update laws ˆK= ΓˆΦp m ẑ = θ m β θm g ẑ γ 3 p m θm ẑ 1 = θ m β θm θm g ẑ 1 +γ 4 θm g p m θm (6.14a) (6.14b) (6.14c) whereγ=diag γ,γ 1,γ 2,γ3, andγ 4 are positive adaptation gains. Notice that the unmeasurable internal friction state z is estimated by a dual-observer structure[78] with the two state estimates, ẑ and ẑ 1. Equations (6.14b) and (6.14c) result in the following observer error dynamics z = β θm g z +γ 3 p m (6.15a) θm z 1 = β θm θm g z 1 γ 4 θm g p m (6.15b) θm where z = z ẑ and z 1 = z ẑ 1 are the estimation errors with regard to the same internal state z. Note that the nominal static parameters (e.g., F C, F S, and v s ) in g θm are to be identified and the actual variations of these parameters can be effectively accounted for by the adaptation ofσ as mentioned above. Stability Analysis The following theorem proves the stability of the closed-loop system under additional assumptions as stated therein. Theorem 6.1. For the system model described by (6.1)-(6.4), global asymptotic tracking performance on the motor side can be achieved by the proposed controller in (6.7)-(6.9) and adaptive friction observer in (6.13)-(6.14), if the integrator term of the PID controller C f b is suppressed, i.e., k i =, and the real load side acceleration measurement, θ l, is used instead of θ ld in (6.7). Proof. Suppose that the PID controller gains are k p = hk s J m k s + h, k v= J m k s + h (6.16) where h is a positive constant. By the assumption in the theorem, k i =. Then the control law in (6.12) can be rewritten as u= J m θmd + 1 N J θ l ld +ˆF J m k s ė m + hp m (6.17)

104 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 87 From (6.11) and (6.17), the control law results in the following closed loop dynamics J m ṗ m = hp m J l θl N θ ld F ˆF = hp m J l N ël K TˆΦ K T Φ (6.18) where ë l = θ l θ ld is the load side acceleration tracking error, K= K ˆK and Φ=Φ ˆΦ are the estimation errors. Let a Lyapunov function candidate for the system be V(p m, K, z, z 1 )= 1 2 J mp 2 m K T Γ 1 K+ 1 2γ 3 k 1 z γ 4 k 2 z 2 1 (6.19) where k 1 =σ and k 2 =β σ 1 are the last two entries in K. In most applications, it is reasonable to assume that the actual friction parameters, K = T, σ1 +σ 2 σ β σ 1 are unknown but constant. It follows that K = and K = ˆK. Therefore, by differentiating V in (6.19) and substituting the update laws from (6.14), the derivative of V is obtained as V(p m, K, z, z 1 )= hp 2 m J l N p më l k 1 β θm γ 3 g z 2 k 2β θm θm γ 4 g z 2 (6.2) 1 θm Recall that h is a positive constant. Thus, every term on the right hand side of (6.2), except for the second term, is negative semi-definite. Moreover, if the load side acceleration is measured by sensors (i.e., θ ld is replaced with θ l to calculate u f f in (6.7)), the second term becomes zero. It follows that V(p m, K, z, z 1 )= hp 2 k 1β θm m γ 3 g z 2 k 2β θm γ 4 θm g θm z 2 1 hp2 m (6.21) From Lyapunov stability theory[75], it is concluded that the motor side tracking error, p m, the friction parameter estimation error, K, and the internal friction state estimation errors, z and z 1, are globally uniformly bounded. Since the actual friction parameters K are assumed to be constant during one trajectory task, and actual internal friction state, z, is bounded as shown in Property 6.1, it follows that the estimates, ˆK, ẑ, and ẑ 1, are globally uniformly bounded as well. In addition, the actual motor side position and velocity,θ m and θ m, are globally uniformly bounded, since p m is globally uniformly bounded, and the generated motor side referenceθ md is bounded by the characteristics of the feedforward controller, F 1. With all these bounded variables, it can be shown that V is also bounded. Then by Barbalat s lemma, (6.19) and (6.21) imply that V (i.e., p m ) as t. Therefore, the motor side tracking error e m = 1 s+k s p m asymptotically converges to zero.

105 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 88 Remark 6.3. The above theorem provides a sufficient condition to achieve global asymptotic tracking performance on the motor side. Two assumptions were introduced to theoretically guarantee the stability of the proposed method. These assumptions, however, may be relaxed without jeopardizing the closed loop stability of the system in practice. The relaxation is demonstrated in the experiments with preserved integrator in C f b to maintain the basic feedback controller structure (i.e., k i ). Also, the desired reference θ ld is used in (6.7) instead of the real measurement θ l to decrease the dependence on the load side sensors. Feedforward Friction Compensation The above friction compensation scheme is in the feedback form, since the actual physical measurements are used in the regressor estimate, ˆΦ. When doing tracking control, feedforward control often improves the performance in the sense that it allows anti-causal terms in the controller. It recovers the limitation of feedback controller due to the dynamic lag of the plant[82]. Recall that the feedback controller must see an error first before it can take any corrective action. In particular, this limitation adversely affects the performance by the quick direction reversal of Coulomb friction force. By replacing the measurements in the friction observer regressor ˆΦ with corresponding trajectory references, and excluding the motor side error term, p m, a feedforward version of friction observer regressor,ˆφ d, is obtained as ˆΦ d = θ md ẑ d θ T md, ẑ g( θ d = θ β θmd md)ẑd md g ẑ d (6.22) θmd Thus, the dual estimations of internal friction state z are replaced by single desired (nominal) internal state ẑ d as in (6.22). In this feedforward form, the friction observer structure is simplified by reducing the number of adaptive variables from 5 to 3, and the noises and disturbances from feedback measurements are also avoided Load Side Friction Compensation Due to the characteristics of indirect drives, perfect tracking performance on the motor side does not guarantee perfect load side tracking performance. Since the load side performance is often of the major concern in practical applications, friction compensation should also be considered on the load side. Recall that, in (6.3), the load side friction, f l, was modeled as a scaled quantity of the entire friction force, F. Thus, once friction parameters are adapted successfully on the motor side, the load side friction f l can be roughly estimated as ˆf l =ˆr lˆf (6.23) whereˆf is the estimated entire friction force from the motor side compensator, andˆr l is the estimated scaling factor obtained by the following update laws ˆr l = γ rlˆfê l, ê l =ˆθ l θ ld (6.24)

106 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 89 whereγ rl > is the adaptation gain, ˆθ l is the load side position estimated by the estimation algorithm such as the kinematic Kalman filter with accelerometers and gyroscope (KKF-AG) described in Chapter Hybrid Compensator Structure Note that in (6.8), the load side friction estimate,ˆf l, is injected into the motor side reference, θ md. It is desired that the compensated reference is still smooth and continuously differentiable, since the motor side velocity reference, θ md, is needed in the feedback controller, C f b. To ensure this, the load side compensation should be activated only when the friction parameters in the motor side compensator have converged to some sub-optimal values and set fixed (i.e.,γ=). This leads to the following hybrid compensator scheme. Figure 6.2 shows the hybrid compensator structure. q and q 1 are the two discrete controller states. In industrial applications, the same task is usually executed repeatedly. Define Θ m,k as the two-norm of the motor side position tracking error in the k-th execution of the 2 same task, i.e. n Θ = m,k θm,k (i) θ 2 md (i) (6.25) i=1 where i is the time index and n defines the time duration of each execution. The controller starts from the state q, where the adaptive motor side compensator is activated and the load side compensator is disabled (i.e.,ˆf l = ). If Θ m,k+1 2 δ, δ, where Θ m,k 2 δ, δ is the convergence boundary (e.g.,δand δ are positive numbers very close to 1), the compensator scheme is switched to the state q 1. This means that the friction adaptation in the motor side compensator has reached its sub-optimal stage, and cannot significantly improve the motor side tracking performance. At this stage q 1, the load side compensator is enabled while the friction parameters in the motor side compensator are set fixed. If Θ m,k+1 2 Θ m,k 2 / δ, δ, this indicates that the motor side tracking performance can be further improved by adapting the parameters in the motor side compensator. Thus, the compensator scheme is switched back to the initial state q. 6.4 Experimental Results Experimental Setup The proposed friction compensation method is applied to a single-joint indirect drive setup shown in Fig. 2.3 in Chapter 2. The motor side encoder measurement is used in the feedback controller, while the load side encoder is only for performance evaluation. Also, the accelerometers and gyroscope are only for load side state estimation rather than for direct control use; e.g., θld is used in (6.7) (one relaxation of Theorem 6.1). It is shown in the experiments

107 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 9 q ˆF ˆf l = Θ m,k+1 2 Θ m,k 2 (δ, δ) q 1 Θ m,k+1 2 Θ m,k 2 ˆF ˆf l = ˆr l ˆF / (δ, δ) Figure 6.2: Hybrid Compensator Structure that this does not practically influence the stability. Finally, the controller is implemented in a LabVIEW real-time target with the sampling rate of 1kHz. Friction identification (see Appendix B for details) is conducted for this experimental setup to set initial values in the adaptive friction observer. The identified nominal values of fiction parameters are shown in Table B.2 in Appendix B Motor Side Friction Compensation Figure 6.3 shows the desired load side trajectory in this experiment, which is designed as a fourth-order smooth time optimal trajectory suggested in[55]. Feedback Friction Compensation To show the effectiveness of the proposed adaptive algorithm, the estimates of all the friction parameters are initialized to one half of the identified values. The gains of the feedback controller are set as k p = 3, k v =.3, and k i = 1. (one relaxation of Theorem 6.1). The adaptation gains in the feedback adaptive friction compensator (FB-A) are selected by trial and error as γ,γ 1,γ 2,γ 3,γ 4 =[.1, 5,,.1,.1]. 1 For comparison, a feedforward Coulomb friction compensator (FF-C) is designed, i.e.,ˆf=ˆf c sgn θmd, using the same initial estimate,ˆf C. Figure 6.4 shows the motor side and the load side tracking performances of these compensators. It is clearly seen that, motor side tracking performance is significantly improved by the 1 Due to the lack of a high resolution encoder, the identification ofσ is very rough and the identification of σ 1 is not even available (i.e., the identifiedˆσ 1 is set to ). This low resolution encoder also limits the benefits of adaptation forβ σ 1. Thus, this adaptation is turned off withγ 2 =.

108 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 91 Position (rad) Velocity (rad/sec) Acceleration (rad/sec 2 ) Time (sec) Time (sec) Time (sec) Figure 6.3: Desired Load Side Trajectory 4 x 1 4 Load Side Position Error 3 (No Comp.) (FF C).2.15 Motor Side Position Error (No Comp.) (FF C) e l = θ l θ ld (rad) e m = θ m θ md (rad) (FB A) Load Side Friction Effect (FB A) Time (sec) NO Comp..15 FF C FB A Time (sec) Figure 6.4: Performance of the Motor Side Compensators a a (No-Comp.) [Green-Solid]: Without Friction Compensator; (FF-C)[Grey-Dash]: Feedforward Coulomb Friction Compensator; (FB-A)[Blue-Solid]: Feedback Adaptive Friction Compensator.

109 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 92 Table 6.1: Improvements Compared to the No-Comp. Case FF-C FB-A FF-A Hybrid Θ l 18.3% 35.26% 37.61% 66.5% 2 Θ l 12.69% 32.81% 35.23% 65.77% Θ m 45.66% 74.41% 78.53% 75.74% 2 Θ m 41.74% 68.3% 73.31% 74.78% Estimate of z, z1 (rad) x 1 3 Internal Friction State Estimate Time (sec) z z1 Estimate of F (Nm) Total Friction Estimate Time (sec) Figure 6.5: Friction Estimates by FB-A Observer FB-A method, converging to a sub-optimal stage in about 3 executions. The load side performance is slightly improved with reduced peak error. Let Θ l 2, Θ l, Θ m 2, and Θ m denote the two-norms and -norms of the load side and the motor side position tracking errors in the last repeated execution, respectively. Table 6.1 shows the performance indices improved by these algorithms compared to the case without compensation (No-Comp.). Figure 6.5 shows the friction estimates by FB-A, whereˆf is fully adapted in about 3 executions. Figure 6.6 illustrates the friction parameter adaptation process. Note that, the converged values are not exactly twice the initial values. This is because the identified values may not be exactly the actual values. Also the friction model structure is modified to adapt three dynamic parameters only. Besides,ˆk 2 =β ˆσ 1 is kept zero sinceˆσ 1 = andγ 2 =.

110 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT x 1 4 Friction Parameter Estimations (SI) 11 k = σ 1 +σ Time (sec) 4 35 k 1 = σ Time (sec) Figure 6.6: Friction Parameter Estimations (SI Units) Feedforward Friction Compensation The modification of the proposed adaptive friction observer from the feedback form to the feedforward form is also tested in the experiment. The same controller gains, k p, k v, and k i, and adaptation gains,γ,γ 1, andγ 2, from FB-A, are used in the feedforward compensator (FF-A). Table 6.1 shows that FF-A slightly improves the performance of FB-A Load Side Friction Compensation The motor side compensation result (e.g., Fig. 6.4) shows that, even if the motor side performance is significantly improved, little improvement can be observed at the load side. Actually, in Fig. 6.4, the oscillation in the load side position error is mainly due to the transmission error θ, while the offset in the tracking error is due to the load side friction effects, which is explained by (6.8). This suggests the necessity to implement the load side friction compensation. In Chapter 3, it was shown that the tracking error offset rather than the transmission error can be successfully estimated by the KKF-AG method. Therefore, this method is employed here to estimate the load side position. To show the effectiveness of the proposed load side compensation algorithm,ˆr l was initialized to 1.N where N is the reducer gear ratio. Figure 6.7 illustrates the performance of friction compensator enabled on both the motor side and the load side (Hybrid), i.e., the state

111 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 94 Load Side Position Error 4 x 1 4 (FF A) (Hybrid) Motor Side Position Error (FF A) (Hybrid) e l = θ l θ ld (rad) 1 1 e m = θ m θ md (rad) (No. Comp.) Time (sec).1 NO Comp. FF A.15 Hybrid (No. Comp.) Time (sec) Figure 6.7: Performance of the Hybrid Compensator b b (No-Comp.) [Green-Solid]: Without Friction Compensator; (FF-A)[Grey-Dash]: Feedforward Adaptive Friction Compensator at Motor Side Only; (Hybrid)[Blue-Solid]: Hybrid Compensator at the State q 1. q 1 in Fig It can be seen that the load side position error offset is significantly reduced by the hybrid compensator scheme in less than 1 execution, while the motor side tracking performance is maintained at about the same level. This is also confirmed by the performance indices in Table 6.1. The convergence of the estimated friction ratioˆr l and the estimated load side frictionˆf l is illustrated in Fig Chapter Summary This chapter investigated the friction compensation in the single-joint indirect drive mechanism by separating the problem into two parts, i.e., motor side and load side. The motor side compensator employed the idea of an adaptive friction observer based on the modified LuGre model, while the load side compensator was implemented by injecting the load side friction estimate into the generated motor side reference. A hybrid compensator scheme was proposed to deal with the switching between the two compensator algorithms. It has demonstrated the necessity of load side friction compensation for enhancing load side performance. Experimental results validated the effectiveness of the proposed scheme.

112 CHAPTER 6. HYBRID ADAPTIVE FRICTION COMPENSATION IN SINGLE-JOINT ROBOT 95 Estimated r l /N Estimated Friction (Nm) Estimation of Friction Scaling Factor r l Time (sec) Total Friction F Friction Estimations Load Side Friction f /N l Time (sec) Figure 6.8: Load Side Friction Estimates

113 96 Chapter 7 Motor Side Disturbance Rejection in Multi-Joint Robot 7.1 Introduction In motion control applications, various disturbances, such as friction force, contact force, transmission error, or model uncertainties, can severely deteriorate performance. Thus, effective disturbance rejection is essential for the feedback controller to achieve high precision position/force tracking. In indirect drive robot joints, however, disturbances operate in combination with compliant gears and also nonlinear robot dynamics, which makes it difficult to reject them directly. Thus, it is desired to study the disturbance estimation and rejection taking the indirect drive mechanism into consideration. Chapter 6 presented the friction force compensation in the single-joint indirect drive robot. A hybrid adaptive friction compensator was proposed and experimentally validated. In extending the single-joint results to the multi-joint case, one can note that friction compensation is one form of disturbance rejection. Since friction effects are usually highly coupled with other disturbances, it makes more sense to estimate and reject the lumped disturbance rather than solely focus on friction compensation. Therefore, in this chapter, the disturbance rejection schemes in the multi-joint indirect drive robot are investigated. The ultimate objective is to improve the load side (or endeffector) performance. The robot dynamics is studied in Section 7.2, which indicates that low frequency motor side disturbance rejection can also result in effective low frequency disturbance rejection on the load side. This leads to the general idea of low frequency motor side disturbance rejection. With this idea in mind, a decentralized motor side controller for the indirect drive robot manipulator is presented in Section 7.3, where two disturbance cancellation schemes, disturbance observer (DOB) and adaptive robust controller (ARC), are developed separately to reject the low frequency disturbance from the motor side. The proposed schemes are validated through the experimental study on the FANUC M-16iB robot system in Section 7.4.

114 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT Robot Model Study Transfer Function Analysis As shown in Section 2.2.3, the decoupled indirect drive model for the i-th joint of the robot can be described as follows M mi q mi + D mi q mi =τ mi + di(q) m N 1 KJi i N 1 q i mi q li + DJi N 1 q i mi q li (7.1a) M ni q li + D li q li =K Ji N 1 q i mi q li + DJi N 1 q i mi q li + d l (q) i i=1, 2,, n (7.1b) where all the coupling and nonlinear terms, such as Coriolis/centrifugal force, gravity force, friction force, and external disturbances, are grouped into the fictitious disturbance torques d m (q), d l (q) R n as d m (q)= F mc sgn( q m )+ N 1 K J q+ D J q (7.2a) d m(q) d m(q) d m(q)) T 1 2 n R n d l (q)= M n M 1 (q l l ) I n KJ N 1 q m q l + DJ N 1 q m q l Dl q l M n M 1 (q l l ) C(q l, q l ) q l + G(q l )+ F lc sgn( q l )+ J T (q l )f ex t + K J q+ D J q (7.2b) d l(q) 1 dl(q) 2 dl(q)) T n R n and the overall fictitious disturbance input d i (t) for the i-th joint is defined as d i (t)=d i (q(t))= d m i(q(t)) d l i (q(t)) T (7.3) Note that di(q) m is in the channel of motor torqueτ mi, and thus can be directly compensated byτ mi. The fictitious disturbance torque d l (q), however, is in the different channel from i τ mi. The compensation of this mismatched dynamics will be the focus of the following study. As in (2.29), the transfer function representation of the i-th joint is restated as q mi (s)= P i (s)τ mu mi(s)+ P i (s)d md i(s)= P i (s)[τ mu mi(s)+di(s)]+ m P i (s)d l (s) (7.4a) md l i q li (s)= P i (s)τ lu mi(s)+ P i (s)d ld i(s)= P i (s)[τ lu mi(s)+di(s)]+ m P i (s)d l (s) (7.4b) ld l i Ideally, suppose one could find a compensation torque τ mi to cancel out the disturbance effects on the motor side, i.e. So P i mu τ mi= P i md l d l i P i mu d m i. It follows that τ mi = (P i mu ) 1 P i md l d l i d m i (7.5) q mi (s)= P i τmi + d m mu i q li (s)= P i τmi + d m lu i + τ mi + P i d md i= l P i τ l mu mi + τ mi + P i d ld i= l P i τ l lu mi+ P i P i (P i ld l lu mu ) 1 P i md d l l i (7.6a) (7.6b)

115 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 98 Denote P i ld l = P i lu (P i mu ) 1 P i md l M ni (D Ji s+k Ji ) 2 = N 2 i s M mi M ni s 3 + J di s 2 + J ki s+k Ji (D mi + D li /N 2 i ) Mni s 2 +(D Ji + D li )s+k Ji which has the following property M ni P i, s, (7.8a) = s(n ld l 2 i D mi + D li ), s. (7.8b) From (2.31d), one can show that the transfer function P i from the fictitious disturbance ld l input d l to the load side position output q i li has the similar property as (7.8a) when s. Figure 7.1 plots the frequency responses of the two transfer functions (i.e., s 2 P i and s 2 P i ) ld l ld l for each joint of the FANUC M-16iB robot. It shows that the frequency responses of the two transfer functions match each other very well up to the anti-resonant frequencies. This means that, under ideal conditions, the perfect disturbance rejection on the motor side also results in perfect disturbance rejection on the load side (i.e., P i P i(p i ld l lu mu ) 1 P i in (7.6b) vanishes) in md l the low frequency range. Thus, it is possible to improve the load side performance by adding motor side disturbance rejection if the focus is in the low frequency region Inertia Variation and Model Simplification Recall that M n represents the diagonal terms of the load side inertia matrix M l (q l ), i.e., M n = diag(m n1, M n2,, M nn ). From the simulation study of the FANUC M-16iB robot, it can be shown that the load side inertia M l (q l ) varies significantly in the work space. For example, Fig. 7.2 shows the variations of the second diagonal term M n2 of the FANUC M-16iB robot inertia matrix M l (q l ) in the work space, which ranges from about 3 kg m 2 to 9 kg m 2. It is important to see how this inertia variation affects the frequency response of the indirect drive model. Consider the simplification of the indirect drive model, e.g., assuming K Ji =, D Ji = (i.e., N i q li = q mi ). The transfer function P i mu in (2.31a) from the motor torqueτ mi to the motor side position q mi is thus simplified as P i musim = q mi(s) τ mi (s) = 1 s(j i s+ D i ) (7.9) where J i = M ni N 2 i + M mi, D i = D li + D N 2 mi (7.1) i Figure 7.3 shows the frequency responses of the indirect drive model sp i (2.31a) and the mu simplified model sp i (7.9) together. The frequency response of the indirect drive model is musim

116 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 99 J1 J2 Phase (deg) Magnitude (db) Frequency (rad/sec) Phase (deg) Magnitude (db) Frequency (rad/sec) J3 J4 Phase (deg) Magnitude (db) Frequency (rad/sec) Phase (deg) Magnitude (db) Frequency (rad/sec) J5 J6 Magnitude (db) Phase (deg) Frequency (rad/sec) Phase (deg) Magnitude (db) P ld s 2 P ld l Frequency (rad/sec) P ld s 2 P ld l Figure 7.1: Frequency Responses of s 2 P i and s 2 P i for Each Robot Joint ld l ld

117 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 1 M(2,2) in the Work Space z (m) y (m) x (m) Figure 7.2: Variations of M n2 of the FANUC M-16iB Robot in the Work Space computed using the load side inertia variation spanning in the range of the work space. The frequency response of the simplified model (7.9) is computed using the nominal inertia value calculated at the robot home position. Figure 7.3 shows that the load side inertia variation itself does not change the frequency response much due to the constant inertia on the motor side and the fact that the load side inertia is scaled by 1 N 2 i when reflected to the motor side. The simplified model preserves the low frequency response property up to the anti-resonant frequency of the fourth order indirect drive model. Note that, if the Coulomb friction, F lc and F mc, and the external contact torque, f ex t, are ignored, the fictitious disturbance torque d i (q) mainly consists of M l (q l ), C(q l, q l ) q l, and G(q l ), which are mainly the algebraic combinations of trigonometric functions such as sin(q) and cos(q). Thus, d i (q) is typically at the low frequency range for low-to-mid speed motions. Also, recall that from the transfer function analysis, the ideal case for motor side disturbance rejection to improve load side performance is valid at the low frequency region up to the antiresonant frequency. Therefore, it is reasonable to investigate the low frequency disturbance rejection on the motor side. Furthermore, from the comparison between the simplified model and the indirect drive model, it makes sense to use the simplified model as the candidate model in the controller design for low frequency motor side disturbance rejection.

118 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 11 1 Joint 1 1 Joint 2 Phase (deg) Magnitude (db) Phase (deg) Magnitude (db) Magnitude (db) Phase (deg) Phase (deg) Magnitude (db) Frequency (rad/sec) Frequency (rad/sec) Joint 3 Magnitude (db) Magnitude (db) Magnitude (db) Phase (deg) Phase (deg) Phase (deg) Magnitude (db) Phase (deg) Frequency (rad/sec) Frequency (rad/sec) Joint Frequency (rad/sec) Frequency (rad/sec) Frequency (rad/sec) Frequency (rad/sec) 8 Joint 5 8 Joint 6 Magnitude (db) Magnitude (db) Magnitude (db) Magnitude (db) Phase (deg) Phase (deg) Frequency (rad/sec) Frequency (rad/sec) Phase (deg) Phase (deg) Frequency (rad/sec) Frequency (rad/sec) Figure 7.3: Frequency Responses of Joint Models of the FANUC M-16iB Robot (Solid Color Lines: Indirect Drive Model sp i mu ; Black Marker Line: Simplified Model sp i musim )

119 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 12 F 2 d τ ff q q ld F md + + q l 1 C pid + τ P a m q m τ pid + τ int Cint Figure 7.4: Controller Structure of the Multi-Joint Robot 7.3 Controller Design Baseline Controller Design A basic decentralized controller is designed utilizing only motor side encoder signals for realtime feedback. This is common practice in industrial robot control due to its simple structure and efficient computation. The basic controller structure for the multi-joint robot is illustrated in Fig. 7.4, which is similar to the single-joint case shown in Fig In Fig. 7.4, P a represents the actual multi-joint robot, which produces the load side position output q l and the motor side position output q m. d is the fictitious disturbance torque defined in (7.3). The overall controller structure consists of two feedforward controllers, F 1 and F 2, and two feedback controllers, C pid and C int. When performing a trajectory tracking task, the robot is typically given the tool center point (TCP) trajectory in the Cartesian space, which is converted to the equivalent load side desired trajectory by inverse kinematics. Then the motor side position reference q md is generated by the feedforward controller F 1 from the load side position reference q ld. The feedfoward controller F 2 generates the feedforward torqueτ f f based on the available desired references, q ld and q md. C pid is a PID controller designed in the decentralized manner. C int is the internal feedback controller (e.g., disturbance observer (DOB)[66, 87] or adaptive robust controller (ARC)[12, 99, 11, 1]) to reject the low frequency disturbances from the motor side. In this controller structure, C pid and C int can be treated as the motor side controller, while F 1 (also part of F 2 ) could be regarded as the load side controller to address the joint compliance effects resulted from the indirect drive mechanism. The design for each controller part is detailed in the following sections Feedforward Controller Design In most cases, the feedback controllers are designed based on some simplified dynamic model, i.e., the effects of some nonlinearities such as transmission elasticity, joint compliances, and amplifier dynamics, are neglected. This implies that imposing large feedback gains to ob-

120 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 13 tain satisfactory performance may be impractical, since the existence of unmodeled dynamics might degrade system performance and may eventually drive it to instability[74]. In other words, the gains of the feedback controller should not be aggressively chosen because of the unmodeled dynamics, and a well-tuned feedforward controller becomes essential for achieving good performance. Good knowledge of the dynamic model is important in the feedforward controller design. In this study, the Lagrangian model (2.24) is utilized, which leads to the following control law for F 1 and F 2 ˆτ ld = ˆM l (q ld ) q ld + Ĉ(q ld, q ld ) q ld + Ĝ(q ld )+ J T (q ld )f ex t,d q md = N q ld +ˆK 1 ˆτ J ld ˆτ md = ˆM m q md +ˆD q md +ˆF c sgn( q md ) τ f f =ˆτ md + N 1ˆτ ld (7.11a) (7.11b) (7.11c) (7.11d) whereˆ is the nominal value of the matrix. The actual computation of (7.11a) is performed using recursive Newton-Euler method, which is computationally efficient. Remark 7.1. Note that, due to the unmeasurable load side information, the friction parameter identification is performed only on the motor side (see Appendix B for details). The identified Coulomb friction, F c = F mc +N 1 F lc, and viscous damping coefficient, D= D m +N 2 D l, represent the combined frictional effects of the motor side and the load side, and the feedforward friction compensation is implemented only through the motor side. Remark 7.2. To avoid the singularity of the signum function, an approximation for sgn( q md ) is implemented as follows to have a smooth and continuous derivative sgn( q ˆ md ) π 1 atan(α qmd ) atan( α q md ) (7.12) whereαis the parameter to adjust the sharpness of the function shape around q md =, i.e., sgn( q ˆ md ) sgn( q md ) asα. The feedforward torqueτ f f in (7.11d) can be divided into two parts, the linear partˆτ ln corresponding to the simplified nominal modelˆp musim, and the remaining nonlinear partˆτ nl, i.e. ˆτ ln (q md )=J n q md + D n q md ˆτ nl =τ f f ˆτ ln (7.13a) (7.13b) where the diagonal matrices J n = ˆM m + N 2ˆM n R n n and D n =ˆD R n n are the nominal linear inertia and nominal linear damping referencing to the motor side, respectively.

121 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 14 q md q md + K p + + K v + + τ pid K i s C pid q m q m Figure 7.5: PID Controller Structure of the Multi-Joint Robot PID Controller Design Similar to the single-joint case in Section 6.2.3, Fig. 7.5 illustrates the decoupled PID controller structure of the multi-joint robot, which is described as C pid (s)= K v + K Kp i + s (7.14a) s = K i + K v K p + K i K p s + K v s (7.14b) where the diagonal matrix K p R n n is the proportional gain of the position loop, the diagonal matrices K v R n n and K i R n n are the proportional and integral gains of the velocity loop, respectively. Remark 7.3. As analyzed in previous sections, to compensate for the fictitious disturbance d(q), the feedforward controller and the PID controller require a good knowledge of the robot dynamic model to achieve high performance. In reality, however, it is normally impossible to obtain a perfect and complete robot dynamic model. Hence, d(q) cannot be completely compensated for and the resulting model discrepancy can be treated as a form of fictitious disturbance in the actual plant. To handle this problem, several approaches can be utilized, such as disturbance observer (DOB)[66, 87] and adaptive robust controller (ARC)[12, 99, 11, 1], to reject the low frequency disturbance from the motor side. By doing disturbance rejection, these two approaches aim to make the shaded part (i.e., the inner plant) in Fig. 7.4 behave as the simplified nominal model. Details of the two schemes are covered separately in the following sections Disturbance Observer (DOB) Based Disturbance Rejection The disturbance observer regards any difference between the actual plant output and the nominal model output as being caused by an equivalent disturbance applied to the nominal model

122 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 15 ˆτ ln (q md ) ˆτ nl (q md, q ld ) d q md + C pid + + µ τ pid + u + τ m 1 P a s + + q m q m + P 1 n ˆd Q DOB Figure 7.6: DOB Controller Structure of the Multi-Joint Robot [8]. It estimates the equivalent disturbance, and utilizes the estimate ˆd as a cancellation signal. In motion control systems, the disturbance observer can be utilized in the velocity loop or position loop. The implementation of DOB in the velocity loop, however, is preferred due to the nominal model of lower relative order. Such a DOB can reject the disturbances within the inner velocity loop, and the outer position loop can be designed based on the nominal model using linear feedback control theory. The structure of the disturbance observer designed in the multi-joint robot system is shown in Fig This DOB design follows the standard process in[8]. The simplified model is used as the nominal model in the velocity loop, i.e. P n = J n s+ D n 1 (7.15) The nonlinear partˆτ nl of the feedforward torqueτ f f is injected inside the DOB loop to cancel the nominal nonlinear and coupling dynamics in the fictitious disturbance d(q). This would greatly relieve the burden for DOB and linear controller (i.e., PID and feedforward torque ˆτ ln ). Let d l denote the remaining disturbance effect uncompensated by ˆτ nl due to model mismatched uncertainty. Thus, the DOB objective is to obtain the estimate ˆd to cancel this d l within the frequency region specified by the Q-filter. The Q-filter here is designed as the following low-pass filter proposed in[87] Q(s)=(3τ s s+ I n )(τ 3 s s3 + 3τ 2 s s2 + 3τ s s+ I n ) 1 (7.16) whereτ s R n n is the diagonal matrix of the time constants to specify the frequency region for disturbance rejection. More design details can be referred to[8] and thus are not repeated here Adaptive Robust Controller (ARC) Based Disturbance Rejection The adaptive robust controller (ARC) deals with the disturbance rejection in a different way such that it is easier to relate with time domain specification in addition to the frequency

123 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 16 domain requirement which is usually used in the DOB design. The details of the ARC design is as follows. ARC Objective Recall that the motor side dynamics for the multi-joint robot is described as q m (s)= P mu (s)τ m (s)+ P md (s)d(s) (7.17a) τ m (s)+ P 1 mu (s)p md(s)d(s)= P 1 mu (s)q m(s) P 1 musim (s)q m(s)=(js 2 + Ds)q m (s) (7.17b) This leads to the simplified nominal model dynamics as J n q m + D n q m =τ m + d (7.18) where J n and D n are the nominal model matrices defined in (7.13). d R n denotes the lumped disturbance torque on the motor side to produce equivalent motor side effects from the fictitious disturbance d and the model discrepancy due to the use of the nominal model, i.e. d=(p mu ) 1 P md d+(ˆp 1 musim P 1)q mu m (7.19) With this model dynamics (7.18), the remaining disturbance effect uncompensated byˆτ nl is thus d l = d+ˆτ nl. Similar to the DOB design (Fig. 7.6), the objective of the ARC is to synthesize a control law such that the inner plant fromµto q m behaves like the nominal simplified model P n, i.e. J n q m + D n q m =τ m + d=µ (7.2) whereµ R n is the torque from the linear PID controller and linear feedforward torque, i.e., µ=τ pid +ˆτ ln. Define an intermediate switching surface p R n as p= q m + J 1 D n nq m J 1 n and combine (7.21) with (7.18), which yields t µ(τ)dτ (7.21) J n ṗ=τ m + d µ (7.22) Thus, the ARC objective (7.2) is achieved as ṗ. Since the relationship between ṗ andτ m is static, it is difficult to achieve ṗ= directly due to the causality problem. If all the signals in the definition of p (7.21) are uniformly continuous, then p means ṗ. Therefore, τ m can be synthesized such that p and the objective (7.2) is then attained.

124 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 17 ARC Control Law The proposed ARC control law is as follows τ m = u s + u f, u s = K s p, u f =µ+ˆτ nl ˆd l (7.23) where K s R n n is a positive definite diagonal gain matrix. ˆdl is the estimate of the uncompensated fictitious disturbance d l. Furthermore, it is assumed that this uncompensated disturbance d l is bounded, i.e., d l [d m, d M ]. Comparing with the basic controller without ARC, the additional control action introduced here is K s p ˆd l. Substituting the control law (7.23) into the system dynamics (7.22), one can get J n ṗ= K s p+µ+ˆτ nl ˆd l + d µ = K s p+d l ˆd l J n ṗ+k s p=d l ˆd l (7.24a) (7.24b) (7.24c) which is a first order stable system yielding p( ) K 1 s d l ( ) ˆd l ( ) (7.25) This indicates that p can be decreased by imposing a larger K s. However, K s is limited by the system bandwidth and thus cannot be arbitrarily large. The other way to decrease p is by decreasing d l ˆd l, which can be done by adaptively estimating d l. The adaptation law to obtain the estimateˆd l is designed as ˆdli = d Mi and p i >, if ˆd li = Pro j(γ i p i )= ˆd li = d mi and p i < (7.26) Γ i p i, otherwise whereγ=diag(γ 1,Γ 2,,Γ n ) R n n is a positive definite diagonal matrix, and i denotes the i-th entry of the vector. It follows that ˆd li [d mi, d Mi ], dli ( ˆdli Γ i p i ), i= 1, 2,, n (7.27) Remark 7.4. With this adaptation law, ifˆd l is not saturated, the dynamic equation (7.24c) with regard to p becomes J n ṗ+k s p+γ t pdt= d l (7.28) and thus p(s) = s(j n s 2 + K s s+γ) 1 (7.29) d l (s) PI D+ARC where denotes the element-wise division of the numerator and denominator vectors and results in a diagonal matrix. Note that (7.29) is a second order system. Therefore, an appropriate

125 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 18 ˆτ ln (q md ) ˆτ nl (q md, q ld ) d q md + C pid + + µ τ pid 1 s + + τ m q m P a ˆd l + 1 s q m P roj(γp) p (J n s) 1 K s + + Jn 1 D n ARC Figure 7.7: ARC Controller Structure of the Multi-Joint Robot synthesis process would be to increase K s to minimize p, and then designγbased on K s and the chosen damping factor for this second order system. This design enables achieving appropriate bandwidth for rejecting low frequency disturbance as well as meeting the time domain specifications for p. Remark 7.5. In[99], it was shown that with the ARC law (7.23) and (7.26), the tracking error p can be arbitrarily reduced by increasing the feedback gain K s. Furthermore, if the lumped disturbance d l is an unknown constant within the defined bounds, the tracking error p will converge to zero as the adaptation ofˆd l converges to the actual d l. This conclusion cannot be applied here due to the complex dynamics of d l (q), which may not be bounded in reality. In this case, the ARC adaptation law (7.26) provides the built-in antiwindup mechanism for control saturation. As seen from (7.26)-(7.27),ˆd l is always within the defined bound[d m, d M ]. In the case that the actual d l is outside the preset region, the system dynamics is still described by (7.24c), which is a first-order stable system with regard to p. Once the actual d l returns to the preset region, the adaptation law and the error convergence to zero may be recovered. Remark 7.6. The overall ARC structure can be compactly integrated and is illustrated in Fig It shows that the overall ARC control law can be accomplished with only 2 integrators, 4 additions, and 4 multiplications, which are computationally simple as the standard DOB Transfer Function Analysis It is of interests to do transfer function analysis for the closed loop system to see what differences the disturbance cancellation schemes (i.e., DOB and ARC) bring to the closed loop

126 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 19 system dynamics. Feedback Controller of PID Only Consider the original closed loop system with PID only first. It has the following dynamics Thus, J n q m + D n q m d=τ m =µ+ˆτ nl (7.3) J n q m + D n q m d l =µ=τ pid +ˆτ ln = K 1 e+k 2 edt+ K 3 ė+j n q md + D n q md (7.31) where K 1 = K i + K v K p, K 2 = K i K p, and K 3 = K v are the proportional, integral, and derivative gains for the standard PID controller (7.14), and e= q md q m R n is the motor side position tracking error. It follows that d l = K 1 e+k 2 edt+ K 3 ė+j n ë+d n ė (7.32) Then the transfer function from the lumped disturbance d l (s) to the tracking error e(s) is e(s) = s J n s 3 +(K 3 + D n )s K 1 s+k 2 (7.33) d l (s) PI D Feedback Controller of PID plus DOB Now consider the closed loop dynamics with DOB controller activated. From Fig. 7.6, one can obtain the closed loop dynamics as ˆd= Q(P 1 q n m u)=q(j n q m + D n q m µ+ˆd) (I n Q)ˆd= Q(J n q m + D n q m µ) J n q m + D n q m (K 1 e+k 2 ˆd=(I n Q) 1 Q ˆd= (I n Q) 1 Q K 1 e+k 2 edt+(k 3 + D n )ė+j n ë edt+ K 3 ė+j n q md + D n q md ) On the other hand, the relationship betweenˆd and d l is obtained as follows (7.34) ˆd=Q(P 1 n q m u)=q(j n q m + D n q m u) =Q(τ m + d u)=q(ˆτ nl + d)=qd l (7.35) Thus, with (7.34) and (7.35), it follows that e(s) = (I n Q)s J n s 3 +(K 3 + D n )s K 1 s+k 2 (7.36) d l (s) PI D+DOB

127 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 11 Feedback Controller of PID plus ARC Now consider the following closed loop system dynamics with ARC activated J n q m + D n q m d=τ m =µ+ˆτ nl K s p ˆd l (7.37) From (7.21), one can get p= q m + J 1 D n nq m J 1 n = q m + J 1 D n nq m J 1 n = µ(t)d t K 1 e+k 2 ė+j 1 (K n 3+D n )e+j 1 K n 1 edτ+k 3 ė+j n q md + D n q md dt edt+ J 1 n K 2 edτdt (7.38) Thus, e(s) p(s) PI D+ARC = J n s 2 J n s 3 +(K 3 + D n )s 2 + K 1 s+k 2 1 (7.39) With (7.29) and (7.39), it follows that e(s) = s J n s 3 +(K 3 + D n )s 2 + K 1 s+k 2 1 Jn s 2 J n s 2 + K s s+γ 1 d l (s) PI D+ARC (7.4) Comparing (7.33) with (7.36) and (7.4), one can see that the DOB contributes to the closed loop system with an additional filter I n Q, while the ARC contributes with an additional high pass filter J n s 2 J n s 2 + K s s+γ 1. The disturbance rejecting capability in the frequency domain can be shaped with these additional filters. In DOB, based on the chosen Q-filter (e.g, low-pass or band-pass filter), the additional filter I n Q could behave as a high-pass or band-stop filter for disturbance rejection. In ARC, the additional high pass filter is equivalent to increasing the low frequency gains by 2 integrators and thus rejecting the low frequency disturbance. However, these additional filters cannot be arbitrarily assigned (e.g., the corner frequency of the high pass filter or the rejecting frequency bandwidth cannot be arbitrarily high) due to the well-known waterbed effect in the loop shaping theory. DOB versus ARC Note that both DOB and ARC are designed for the same purpose of rejecting low frequency disturbances from the motor side (i.e., making the inner plant behave as the chosen nominal model). Thus, it is of interests to compare between these two methods to see their strengths and weaknesses. Table 7.1 lists some of the differences between DOB and ARC.

128 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 111 Table 7.1: The Comparison Between DOB and ARC Aspects DOB ARC Disturbance Rejection Frequency Range Design with Performance Specification Controller Order Plant Uncertainty Frequency range specified by Q-filter, e.g., low-pass or band-pass filters Normally design in the frequency domain rather than the time domain Depends on the order of the nominal inversed model and Q-filter Q-filter bandwidth needs to be conservative due to plant uncertainty Low frequency disturbance rejection Could relate the design with both time domain and frequency domain requirements Usually simple and low order Needs to make appropriate assumption for plant uncertainty bounds Control Saturation Usually not considered Built-in anti-windup mechanism for saturation 7.4 Experimental Study Experimental Setup The above disturbance rejection algorithms are validated on the FANUC M-16iB industrial robot system shown in Section In spite of the many sensors available on this experimental setup, only motor side encoders are used for feedback control, while the CompuGauge 3D measurements are utilized only for performance evaluation. The real-time controllers are implemented on the UCB-FANUC M-16iB Robot Experimentor through the xpc-target system with the sampling rate as 1kHz. In the DOB and ARC implementations, the nominal linear inertia J n and linear damping D n are chosen as the nominal values at the robot home position. The bandwidth of the Q-filter in the DOB design is set to 1.5 times higher than the position loop bandwidth. In the ARC design, K s andγare designed such that the second order system (7.29) has a damping factor of 1. The lumped fictitious disturbance bounds, d m and d M, are selected based on the motor specifications and torque profiles from the feedforward torque calculation Experimental Results The tool center point (TCP) testing trajectory (Fig. 7.8), which is a 4-th order smooth time optimal trajectory[55], is designed to be a point-to-point scanning trajectory with a maximum

129 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT TCP Cartesian Space Position Trajectory Position (m) x y z Time (sec) (a) 3.5 TCP Orientation Trajectory (Euler Angle ZYX/RPY) Angle (rad) Yaw (X) Pitch (Y) Roll (Z) Time (sec) (b) Figure 7.8: TCP Cartesian Space Trajectory Reference

130 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 113 Cartesian velocity of 8mm/sec, and a maximum Cartesian acceleration of 3 mm/sec 2. Figure 7.9 and Fig. 7.1 illustrate the TCP Cartesian space position trajectory and the motor side position errors, respectively. The performances of three controller designs are compared: the feedback controller with PID only (PID) the feedback controller with PID plus DOB (PID+DOB) the feedback controller with PID plus ARC (PID+ARC) Figure 7.1 shows that both DOB and ARC can greatly improve the motor side tracking performance. This improvement on the motor side also results in the significant improvement of the load side/end-effector performance (Fig. 7.9), which is expected from the transfer function analysis in Section The frequency spectrum of the motor side position errors is illustrated in Fig It shows that the low frequency disturbance effects on the motor side has been substantially reduced by the designed schemes. Figure 7.12 compares the joint torque profiles for the three controller designs: the PID feedback torque (i.e.,τ pid in Fig. 7.4) in the setting PID the estimated disturbance torque (i.e., ˆd in Fig. 7.6) in the setting PID+DOB the ARC compensation torque (i.e., K s p ˆd l in Fig. 7.7) in the setting PID+ARC Note that the compensation torques produced by the disturbance rejection methods (i.e., DOB or ARC) match quite well with the PID torque in the setting PID. The large differences around the middle and end points of the trajectory are due to the implementation setting that DOB and ARC are basically disabled at the near-zero speed region for anti-windup. Overall, it implies that the disturbance rejection schemes are essentially relieving the burden on the PID controller. This means that even with conservative PID controller gains, good performance can still be achieved with the help of disturbance rejection schemes. 7.5 Chapter Summary This chapter presented the motor side disturbance rejection algorithms for the multi-joint robot. The ultimate objective is to improve the load side (or end-effector) performance through the disturbance rejection. As a prerequisite to achieving this goal, however, this chapter focused on the design of motor side disturbance rejection controller. Two motor side disturbance rejection schemes (i.e., DOB and ARC) were designed and compared. From the robot model analysis and the experimental results, it demonstrated that the effects of the low frequency motor side disturbance were greatly suppressed, and in addition, the load side/endeffector performance was also significantly improved. In order to further achieve high bandwidth load side performance, disturbance rejection scheme on the load side other than the motor side is one of the future works.

131 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT Cartesian Space Position Reference Actual z (m) y (m) x (m) Cartesian Space Position Reference Actual z (m) y (m) x (m) Cartesian Space Position Reference Actual z (m) y (m).85 x (m) Figure 7.9: TCP Cartesian Space Position Tracking Performance

132 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT Motor Side Position Error (PID) Position Error (rad).1.1 Position Error (rad) Time (sec) Motor Side Position Error (PID+DOB) J1 J2 J3 J4 J5 J Time (sec).2 Motor Side Position Error (PID+ARC) Position Error (rad) Time (sec) Figure 7.1: Motor Side Position Tracking Error (in Load Side Scale)

133 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT 116 x 1 3 Single Sided Amplitude Spectrum of Motor Position Error PID x 1 3 PID+DOB x 1 3 PID+ARC J1 J2 J3 J4 J5 J6 MotorPosErrFFT 5 4 MotorPosErrFFT 5 4 MotorPosErrFFT Frequency (Hz) Frequency (Hz) Frequency (Hz) Figure 7.11: Motor Position Error Spectrum

134 CHAPTER 7. MOTOR SIDE DISTURBANCE REJECTION IN MULTI-JOINT ROBOT J1 Torque Profile Comparison 1 J2 Torque (Nm).2.2 Torque (Nm) Time (sec) Time (sec).4 J3.2 J4 Torque (Nm).2.2 Torque (Nm) Time (sec) Time (sec).1 J5 J6 Torque (Nm).5.5 Torque (Nm) Time (sec) PID Trq w.o. DOB & ARC DOB Trq ARC Trq Time (sec) Figure 7.12: Torque Profile Comparison

135 118 Part III Handling Mismatched Dynamics: Iterative Learning Control Approaches

136 119 Chapter 8 Hybrid Two-Stage Model Based Iterative Learning Control Scheme for MIMO Mismatched Linear Systems 8.1 Introduction Automated systems (e.g., robotic manipulators) used in industrial applications are often required to repeatedly perform a single task under the same operating conditions. If the system repeatability is good, the trajectory tracking error will become repetitive from one run to another. In this case, the iterative learning control (ILC) scheme is well suited to compensate for the repeatable tracking error[11, 7]. ILC is a data-driven methodology which iteratively utilizes the data (e.g., error profile) from previous trials to update the system inputs for the next iteration. Many versions of ILC have been developed for various applications[11]. Most of them, however, are developed for the fundamental case where the system has direct measurement of the interested output for real-time feedback and does not have disturbances in the channels different from the control input. Therefore, in mismatched systems where the above scenario does not hold, the performance of the standard ILC is limited and in some cases may not be guaranteed to converge. As discussed in Chapter 2, mismatched systems are common in practical applications, e.g., industrial robots with indirect-drive joint mechanisms (joints with elasticity). Several feedback control approaches, such as integrator backstepping[45], dynamic surface control[47], and adaptive robust control[1], have been developed specifically to handle mismatched system behavior. Some efforts have been devoted to migrating these ideas to the field of ILC to deal with the mismatched uncertainty iteratively while exploiting the advantage of noncasual offline processing. Among those early works, a two-stage ILC approach was proposed in[63] for robots with joint elasticity. Similar to backstepping, the real-time measured output (i.e., motor side state) is utilized in[63] as a hypothetical input to control the output of interest (i.e., load side state).

137 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 12 As shown later in this chapter, the convergence rate of this learning process may be adversely affected and thus the use of a high bandwidth Q filter to learn high frequency error may compromise stability. Other studies such as[57, 89] also reported the compromise on the tracking performance they had to make for a better learning convergence. This is especially the case when the system exhibits mismatched uncertainties. Regarding this stability issue under uncertainty, various robust approaches have been proposed[3, 67]. The resulting algorithms are usually nontrivial and the performance is normally compromised for a conservative robust controller. The scheme in[67] requires the plant resonances to be suppressed by feedback compensation in order for the proposed method to improve the robustness to high-frequency modeling errors. In[41], a model-based ILC approach was developed for robots with elastic joints to learn the error component beyond the first resonant frequency. This approach, however, requires an accurate piecewise-linear model to be identified for each trajectory in advance, which limits its application. This chapter proposes a hybrid two-stage model-based ILC approach for a class of MIMO mismatched linear systems. The two-stage ILC is aimed to push the learning algorithm to a higher bandwidth while maintaining the fast model-based convergence rate. The chapter is organized as follows. The system model and the basic controller structure are introduced first. Then two ILC schemes are designed independently, and are followed by an ad hoc hybrid scheme to enable the two ILC schemes to execute simultaneously. The experimental study on a single-joint indirect drive system is presented to validate the effectiveness of the proposed scheme. The parametric uncertainty and mismatched dynamics such as various disturbances at different locations of the system will be addressed. Finally, the conclusions of this work will be given. 8.2 System Overview System Model The MIMO mismatched linear system (2.1) studied in this dissertation is restated here as ẋ(t)=ax(t)+ B u u(t)+ B d d(t) (8.1a) y(t)= q T m (j), qt l (j) T = C x(t)+ Du u(t)+ D d d(t) (8.1b) where x R n x is the system state, u R n u is the control input, d R n d is the lumped disturbance, q m R n m and q l R n l are the two outputs of the plant. d is regarded as the mismatched uncertainty/disturbance if it (or part of it) is applied to different channels from the control input u (i.e., B u αb d, α R). Another mismatched assumption is that, only part of the outputs (i.e., q m ) is measured for real-time feedback, even if the output of interest may be q l. However, the measurement of q l may be available for iteration based off-line use. Furthermore, besides the unknown mismatched dynamics, it is assumed that parametric uncertainties exist in the available nominal model.

138 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 121 q ld,k ˆP muˆp 1 lu F 1 q d,k + r q,k q md,k F 2 ˆP mu 1 τ ln,k Inner Plant d k τ nl,k P ld + + P md + C + + µ k + + u k P mu P lu q md,k τ fb,k e k q l,k q m,k q k ˆP u q p,k + e p,k Figure 8.1: Control Structure with Reference & Torque Update Now consider the discrete-time model representation of this system in the following form q m (j)= P mu (z)u(j)+ P md (z)d(j) q l (j)= P lu (z)u(j)+ P ld (z)d(j) (8.2a) (8.2b) where j is the time step index, and z is the one step time advance operator in the discrete time domain. P mu, P md, P lu, and P ld are the transfer functions from u or d to the corresponding output. For simplicity, the following control scheme is formulated for the case where P mu and P lu are diagonal matrices. However, it may be possible to extend this work to a more general case by carefully considering the plant inversion and commutative multiplication for the non-diagonal matrices Basic Controller Structure Figure 8.1 illustrates the control structure for this mismatched system, where the subscript k is the iteration index. It consists of two feedforward controllers, F 1 and F 2, and one feedback controller, C. Here, C can be any linear feedback controller such as a decoupled PID controller to stabilize the system. The feedforward controllers, F 1 and F 2, are designed using the nominal inverse model as q md,k (j)=ˆp mu (z)ˆp 1 lu (z)q ld,k(j) F 1 (z)q ld,k (j) τ ln,k (j)=ˆp 1 mu (z) q md,k (j)+ r q,k (j) F 2 (z) q md,k (j) (8.3a) (8.3b) whereˆ is the nominal model representation of. q ld,k is the desired plant output for q l,k. r q,k andτ nl,k are used as the additional reference and feedforward torque updates generated iteratively by the two-stage ILC algorithm designed later.

139 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS Two-Stage ILC Scheme ILC Basics Here some basics of the general ILC scheme are reviewed first, which will be utilized in the subsequent ILC scheme design. Consider the MIMO linear system with the error dynamics and the ILC law in the following form ē k (j)= P eu (z)ū k (j)+ r(j) ū k+1 (j)=q(z) ū k (j)+ L(z)ē k (j) (8.4a) (8.4b) where ē is the error that the ILC scheme aims to reduce, r is the lumped repetitive reference and/or disturbance input to the system, and ū is the control input updated iteratively by the ILC scheme using the filters L(z) and Q(z). Similar to[68, 11], the following convergence property holds: Theorem 8.1. The ILC system (8.4) is monotonically and exponentially convergent in the sense that ūk ū and ēk ē p as k, if p β= Q(z) I L(z)Peu (z) p < 1 (8.5) whereβ is the rate of convergence, I is the identity matrix with appropriate dimension, the p- norm p = i i p 1/p, and ū (j)= I Q(z)+Q(z)L(z)P eu (z) 1 Q(z)L(z) r(j) ē (j)= I Q(z)+Q(z)L(z)P eu (z) 1 [I Q(z)] r(j) (8.6a) (8.6b) Proof. First, with (8.4), it is easy to see that ū k+1 (j)=q(z) I L(z)P eu (z) ū k (j)+ Q(z)L(z) r(j) (8.7) which yields ūk+1 ū p = Q(z) I L(z)Peu (z) (ū k ū ) p Q(z) I L(z)Peu (z) ūk ū p (8.8) p Therefore, if Q(z) I L(z)Peu (z) < 1, ūk ū p as k. With (8.4a), similar p conclusion can be drawn for the convergence of ē k. The converged steady states of ū and ē are given in (8.6). Note that the inversion I Q(z)+Q(z)L(z)P eu (z) 1 exists because Q(z)[I L(z)Peu (z)] p < 1. Equation (8.6b) shows that the steady state error ē vanishes with complete learning (i.e., Q(z)= I), which means the effects on ē from the repetitive input r will be fully compensated.

140 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 123 The ILC law (8.4b) implies the Q filter can also be used to shape the learning ability in the frequency domain. In order to achieve better performance, it is desired to push the bandwidth of Q(z) to be as high as possible. Equation (8.5), however, indicates that the bandwidth of Q(z) may have to be compromised to ensure monotonic convergence and to avoid poor transients in the learning process. In practice, a low-pass filter Q(z) is typically employed to prevent the effects of high frequency model uncertainties from entering the learning process[11]. Also, Q(z) should be unity gain at low frequencies where complete learning is preferred to achieve zero steady state error. Since the ILC scheme is an off-line iteration based method, acausal filtering can be utilized to obtain a zero-phase learning response. 1 Given a fixed Q filter, the optimal learning filter to achieve the fastest convergence is obtained as L (z)=argmin Q(z) I L(z)Peu (z) (8.9) L(z) p This leads to the plant inversion choice, i.e., L (z)= P 1 (z). This model matching problem eu can be solved with many optimization techniques, such as the H synthesis[3], if the model uncertainty bound is known. The designed Q(z) and L(z) need to be validated using (8.5) with the knowledge of system model uncertainty. Without loss of generality, the optimal learning filter in this chapter is simply chosen as L (z)=ˆp 1(z). eu ILC With Reference Update Let S p (z)= I+ C(z)P mu (z) 1 denote the sensitivity function of the closed loop system in Fig From (8.3a), q md,k is related to q d,k (i.e., q ld,k or q md,k ) as follows q md,k =ˆP muˆp 1 u q d,k (8.1) where P u can be either P mu or P lu depending on the choice of q d,k. The time index j for all signals and the discrete time operator z for all transfer functions are omitted hereafter for simplicity. To reduce the tracking error e k = q d,k q k, an ILC scheme with reference update can be applied according to the following lemma. Lemma 8.1. If the desired trajectory q d,k, the feedforward torque updateτ nl,k, and the disturbance d k are repetitive for each iteration, the tracking error e k will be monotonically converging 1 A typical way of designing a zero-phase low pass filter is Q(z)=Q 1 (z 1 )Q 1 (z) where Q 1 (z) is a causal lowpass filter with chosen cut-off frequency. In digital implementation (e.g., MATLAB "filtfilt" function), it means the data series is filtered using Q 1 (z) in the forward direction and then filtered again in the backward direction. The phase delay of the filtered data is canceled and the magnitude amplification is the square of the Q 1 (z) magnitude. This Q filter is also usually used to make the non-proper plant inversion learning filter realizable. If Q 1 (z) or the non-proper learning filter is an IIR filter, the initial condition of the filtered data needs to be taken care of as suggested in[4].

141 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 124 in the iteration domain by the following ILC scheme as long as the convergence rateβ r satisfies r q,k+1 = Q r (r q,k + L e r k) (8.11) L 1 =ˆP ˆP r u mu (8.12) β = Qr (I ˆP 1 P r u u S p Ŝ 1 ) 1 (8.13) p < Proof. The system output q k (i.e., q m,k or q l,k ) can be derived as q k = P u u k + P d d k = P u S p (C+ˆP 1 mu )(q md,k+r q,k )+τ nl,k C P md d k + Pd d k =ˆP 1 P mu us p Ŝ 1 r p q,k+p u S p ˆP 1 Ŝ 1 q u p d,k+τ nl,k C P md d k + P d d k (8.14) where it has been assumed that P mu, P lu, and C are diagonal matrices. Hence commutative multiplication law applies hereafter. The corresponding tracking error e k is e k = q d,k q k = ˆP 1 P mu us p Ŝ 1 p r q,k+(i P u S pˆp 1 u Ŝ 1 )q p d,k P u S p τ nl,k +(P u S p C P md P d )d k P eu,r r q,k + r r,k (8.15) Therefore, the tracking performance of the next iteration can be improved by the ILC scheme with reference update (8.11) (namely reference ILC (L) or reference ILC (M) depending on the choice of e k ), if the desired trajectory q d,k, the feedforward torque updateτ nl,k, and the disturbance d k are repetitive for each iteration. From Theorem 8.1, the monotonic convergence of this ILC scheme (8.11) can be guaranteed ifβ r = Q r (1 L rˆp 1P mu us p Ŝ 1) 1. With the p < nominal model inversionˆp 1 1 =ˆP ˆP eu,r u mu, the optimal learning filter and the convergence rate are obtained as (8.12) and (8.13), respectively. Remark 8.1. With complete learning (i.e., Q r = I), the tracking error e vanishes when the convergent condition (8.13) is met. Remark 8.2. It is seen in (8.13) that, the model uncertainty will greatly affect the convergence rate. In order to achieve fast convergence rate without compromising the bandwidth of Q r, it is desired to reduce the model uncertainties. This can be done by obtaining a nominal modelˆp u more accurately representing the actual physical plant, which requires significant system identification effort and the resulting controller is generally too complex (highly nonlinear or high order). In contrast, the same objective can be achieved by making the inner plant (blue shaded area in Fig. 8.1) behave as the chosen nominal modelˆp u. In the next section, an ILC scheme with torque update is introduced to accomplish this, i.e., making q k ˆP u µ k, whereµ k =τ ln,k +τ f b,k is the torque input to the inner plant.

142 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS ILC With Torque Update Define e p,k as the model following error between the nominal plant output (i.e., q p,k ˆP u µ k ) and the actual plant output q k (i.e., q l,k or q m,k ). The ILC scheme to reduce this model following error e p,k = q p,k q k can be written as τ nl,k+1 = Q u τnl,k + L u e p,k (8.16) whereˆp u isˆp lu orˆp mu to match with the choice of q k. The corresponding ILC is termed as torque ILC (L) or torque ILC (M). Remark 8.3. It shows thatτ nl,k is used to cancel out the effects on q k from model uncertainty PP u ˆP u and mismatched disturbance d k. The idealτ to achieve this objective can be nl,k derived as ˆP u µ k = P u (µ k +τ nl,k )+ P dd k τ nl,k = P 1 u ( Pµ k+p d d k ) (8.17) In mismatched systems, the two objectives, followingˆp mu (i.e., torque ILC (M)) and followingˆp lu (i.e., torque ILC (L)), cannot be attained simultaneously (i.e.,τ ), since nl,mk τ P 1 P nl,lk mu md P 1 P lu ld and P 1 P mu m P 1 P lu l in general. This means that at this stage it is desired to select the nominal model to match with the chosen one in the reference ILC stage. Convergence of Model Following Error Lemma 8.2. If the desired trajectory q md,k, the reference update r q,k, and the disturbance d k remain the same for each iteration, the model following error e p,k will be monotonically converging by the torque ILC scheme (8.16) if L u = L 1 =ˆP (8.18) u u β = Qu u I PuˆP 1 Sp < 1 (8.19) u Proof. The input-output equation of the nominal plant can be derived as q p,k ˆP u µ k =ˆP u S p (C+ˆP 1 )(q mu md,k+r q,k ) C P mu τ nl,k C P md d k (8.2) Then from (8.14) and (8.2), the model following error e p,k becomes e p,k = q p,k q k = T u S p τ nl,k PS p (C+ˆP 1 mu )(q md,k+r q,k ) +( PS p C P md P d )d k P eu,u τ nl,k + r u,k (8.21) where T u =ˆP u C P mu + P u. Therefore, if the desired trajectory q md,k, the reference update r q,k, and the disturbance d k remain the same for each iteration, by Theorem 8.1, the torque ILC scheme (8.16) will be

143 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 126 monotonically converging ifβ u = Q u I Lu T u S p 1. Using the nominal plant inversion < ˆP 1 = eu,u Ŝ 1 p ˆT 1 u and (8.19), respectively. =ˆP 1 u, the optimal choice of L u and the convergence rateβ u become (8.18) Remark 8.4. With complete learning (i.e., Q u = I), the inner plant behaves like the nominal model as the model following error e p, when the convergent condition (8.19) is met. Convergence of Tracking Error Lemma 8.3. If q d,k, r q,k, and d k do not vary from one iteration to another, then the tracking error e k will converge monotonically with the rate ofβ e P u T 1 u β u as long as the model following error e p,k converges and P u T 1 is BIBO stable. u Proof. Using (8.15) and (8.21), the tracking error e k can be derived as e k =P u T 1 u e p,k +ˆP mu C P 1 u Thus, the conclusion in the lemma is obtained. P muˆp uˆp 1 mu I q d,k 1 ˆP + CˆP mu u r q,k + CˆP u Pmd P d P mu P 1 u Remark 8.5. For torque ILC (M) (i.e., P u = P mu and P d = P md ), P 1 u P md P d P mu P 1 =, which further reduces (8.22) to u e k = P u T 1 ep,k (ˆP 1 + C)ˆP u mu u r q,k dk Thus if r q,k = (i.e., the reference ILC is not activated), e k as e p,k vanishes. (8.22) P muˆp uˆp 1 mu I = and (8.23) Remark 8.6. For torque ILC (L) (i.e., P u = P lu and P d = P ld ), even if r q,k = and e p,k vanishes, e k is not necessarily true due to the mismatched behavior. The remaining tracking error e is ˆP muc P 1 e = P u T 1 u u P muˆp uˆp 1 mu I q d, (ˆP 1 + C)ˆP mu u r q, + CˆP u (P md P d P mu P 1 )d u P eu,ur r q, + r ur, (8.24) This steady state tracking error e can be further eliminated through the reference ILC using ˆP mu, and this matches with (8.12). L r =ˆP 1 eu,ur =ˆP 1 u

144 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS Hybrid Scheme With Two-Stage ILC In general, for the closed loop system with a satisfactory feedback controller, the sensitivity function S p will behave as a high-pass filter to mitigate the low frequency error. Therefore, in the convergence condition (8.19), the low frequency model uncertainty is greatly suppressed by S p. This allows Q u to have higher bandwidth without worrying about the low frequency uncertainty. Then with the effects of the torque ILC, the inner plant will behave like the nominal model (i.e., q k ˆP u µ k ) up to the bandwidth of Q u. Within this frequency range, the convergence condition of the reference ILC (8.13) will be simplified to β r Q r I S p Ŝ 1 < 1 (8.25) p which allows to push Q r to a higher bandwidth for better tracking performance. Note that the repetitive assumption is used in the derivation of the aforementioned two ILC schemes. When these two ILC schemes are activated simultaneously, the repetitive assumption will be no longer valid (i.e., r q,k andτ nl,k are not repetitive from one iteration to another). Therefore, an ad hoc hybrid scheme is designed to reduce the adverse interference of the two ILC stages. Specifically, an iteration-varying gain is applied to each ILC stage as follows τ nl,k+1 = Q u τnl,k +γ u,k L u e p,k (8.26) r q,k+1 = Q r (r q,k +γ r,k L r e k ) (8.27) j where the two gainsγ u,k andγ r,k can be tuned by trial and error, e.g.,γ u,k = min(4 e p,k(j) 2, 1) j e p,1(j) 2 andγ r,k = γ u,k. The basic idea behind is that the torque ILC needs to take more effects j whenever the model following error becomes larger in the previous iteration (i.e., e p,k(j) 2 j e p,1(j) 2 increases). In order for the torque ILC to perform better, the effects of the reference ILC is accordingly attenuated with a decreasedγ r,k. In contrast, once the model following error is sufficiently small (i.e., the inner plant behaves as the nominal model), the torque ILC becomes not necessary and the reference ILC can be fully activated. Remark 8.7. As shown in (8.23), for the application of tracking q m, the reference ILC is not necessary and the torque ILC (M) withˆp u =ˆP mu will be sufficient. In order to track q l, however, the aforementioned hybrid two-stage ILC scheme withˆp u =ˆP lu will be necessary. And it is understood that the nominal models used in two ILC stages should match with each other due to the mismatched dynamics. In the experimental study, the algorithm validation will focus on the case of tracking q l to test the effectiveness of the hybrid two-stage ILC scheme.

145 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS Experimental Study Experimental Setup & Dynamic Model The proposed method is validated on a single-joint indirect drive robot shown in Fig. 2.3 in Chapter 2. It is assumed that the load side encoder measurement is only available for iteration based off-line use rather than for real-time feedback use. The gyroscope and accelerometer measurements are not used in this ILC study. Finally, the algorithms are implemented using a 1kHz sampling rate through a LabVIEW real-time target. Figure 2.1 in Chapter 2 illustrates the schematic of the single-joint indirect drive mechanism. And the dynamic model (2.6) for this setup is restated as J m θm + d m θm = u+ d m 1 θm θm k j N N θ l + d j N θ l (8.28a) J l θl + d l θl = k j θm N θ l + d j θm N θ l + d l (8.28b) where d m = d f m f mc sgn( θ m )+ 1 k j θ+ dj θ N d l = d fl f lc sgn( θ l ) k j θ+ dj θ (8.29a) (8.29b) and all the variables are as defined in Chapter 2. Therefore, the above indirect drive model can be considered as a mismatched system described in (8.2) with the disturbance d= d m d l T. The two outputs q m and q l are the motor side positionθ m and the load side positionθ l, respectively. The continuous time transfer functions (i.e., P mu, P md, P lu, and P ld ) from the inputs to the outputs were derived as (2.8) in Chapter System Disturbance Characteristics Note that the disturbance d includes the Coulomb frictions (i.e., f mc and f lc ), external disturbances (i.e., d f m and d fl ), and the transmission error θ. It can be seen that d is repetitive if q m and q l are repetitive. In this setup, the identified Coulomb friction combined at the motor side (i.e., f mc + f lc ) is N 2 about.14nm (see Appendix B for more details). A fictitious torque is added in the motor torque command to simulate the external disturbance d f m at the motor side. In the following experiments, d f m is set as a 1Hz sinusoid starting from 3sec with an amplitude of.2nm, i.e., d f m =.2sin(2π(t 3))Nm. The repetitive external disturbance d fl at the load side is generated using the setup shown in Fig It is designed to have the extension springs apply a maximum disturbance of approximately 2Nm at the load side when the payload hits the ball and continues rotating for about 14 degrees.

146 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 129 Load Side External Disturbance Setup.2m.28m 5N/m Figure 8.2: Load Side Disturbance Setup for Single-Joint System The motor side feedback controller C designed for this setup has a resonant frequency at about 11Hz for the velocity loop. This corresponds to about 1 rad/sec at the load side. Recall that the dominant component of the transmission error in the harmonic drive has the frequencyω o twice the motor side velocity θ m (i.e.,ω o = 2 θ m ). Therefore, in order to amplify the transmission error effects, the load side desired trajectory is designed to have a speed of.5 rad/sec for most time so that the resulting transmission error frequency will coincide with the resonant frequency of the velocity loop. The resulting trajectory is shown in Fig. 8.3, which was designed as a fourth-order smooth time optimal trajectory suggested in[55]. The effects of these different kinds of disturbances on the load side tracking performance with the basic controller structure (i.e., C, F 1, and F 2 in Fig. 8.1) are illustrated in Fig Algorithm Setup Design the zero-phase acausal low-pass filters Q r and Q u as Q r (z)=q u (z)=q 1 (z 1 )Q 1 (z), where Q 1 (z) is a low-pass filter with a cut-off frequency of 3Hz, which is beyond the system elastic anti-resonant and resonant frequencies. With this selection of Q r (z) and Q u (z), the frequency responses ofβ r in (8.13) andβ u in (8.19) with±5% parametric uncertainties are plotted in Fig. 8.5 to verify the monotonic stability condition. Figure 8.5 shows that, using either motor side model or load side model, the magnitudes ofβ r andβ u are always below db indicatingβ r < 1 andβ u < 1. Therefore, the monotonic stabilities (8.13) and (8.19) are ensured separately for both ILC schemes. Now consider the old two-stage approach proposed in[63] as a benchmark controller. It can be shown that, if using plant inversion learning filters, this old approach in[63] can be reformulated similarly as the reference ILC (L) withˆp u =ˆP lu plus the torque ILC (M) withˆp u = ˆP mu in this chapter. This means that the two-stage ILC scheme is performed with mismatched nominal models. As expected, this will not help to attenuate the model uncertainty but instead

147 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 13 Pos (rad) Vel (rad/sec) Acc (rad/sec) Load Side Desired Trajectory Time (sec) Figure 8.3: Load Side Desired Trajectory 1 x Load Side Position Error Model Uncertainty Load Side Disturbance Pos Error (rad) Time (sec) Coulomb Friction Transmission Error Motor Side Disturbance Figure 8.4: Disturbance Effects on Load Side Position Tracking Error

148 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 131 β r = Q r (1 Reference Side ILC - Using Motor Side Model 1 ˆP u P u S p Ŝp 1 ) < 1 Reference Side ILC - Using Load Side Model Magnitude (db) -4-5 Magnitude (db) Frequency (rad/sec) Torque Side ILC - Using Motor Side Model β u = Q u (1 ˆP 1 u P u)s p < Frequency 1(rad/sec) Torque Side ILC - Using Load Side Model Magnitu ude (db) Magnitu itude (db) Frequency (rad/sec) Frequency (rad/sec) Figure 8.5: Frequency Responses ofβ r andβ u with±5% Parametric Uncertainties may even deteriorate the ILC convergence performance. To see this, the tracking performances in the following experiments will be compared in three controller settings, i.e. reference ILC withˆp u =ˆP lu only (RefILC(L)) reference ILC withˆp u =ˆP lu plus torque ILC withˆp u =ˆP mu (RefILC(L)+TrqILC(M)) reference ILC withˆp u =ˆP lu plus torque ILC withˆp u =ˆP lu (RefILC(L)+TrqILC(L)) Experimental Results Using Accurate Nominal Model Each controller setting is implemented to track the load side desired trajectory in Fig. 8.3 for 1 iterations. First, the nominal model with accurately identified system dynamic parameters (Table 2.1) is used in the controller design. With an accurate nominal model, it is expected that the three controller settings will perform equally well sinceβ in (8.13). As shown r

149 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS x 1 4 Load Side Position Error Initial Run RefILC(L) RefILC(L)+TrqILC(M) RefILC(L)+TrqILC(L) Pos Error (rad) Time (sec) (a) Load Side Acceleration Error Initial Run RefILC(L) RefILC(L)+TrqILC(M) RefILC(L)+TrqILC(L) Acc Error (rad/sec 2 ) Time (sec) (b) Figure 8.6: Performance Comparisons using Accurate Nominal Model (After 1 Iterations)

150 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS 133 in Fig. 8.6, the load side tracking errors for these three settings are all significantly reduced (especially the position errors are reduced to almost the level of load side encoder resolution). Using Inaccurate Nominal Model Next the three controller settings are compared using the nominal model with 15% uncertainty in the dynamic parameters. Normally, with larger model uncertainties, the cut off frequencies of Q filters need to be reduced to ensure the convergence of the learning process. Here, the Q filters are kept the same as in the previous case in order to verify the benefits of the proposed scheme. The error ( -norm) convergence comparisons are illustrated in Fig Figure 8.7a shows that the torque ILC performs very well once it is activated. No matter which nominal model is chosen to follow, the model following errors are greatly reduced with fast convergence rate. As to the load side tracking performance, however, more differences are expected. It is shown in Fig. 8.7b and Fig. 8.7c that the setting RefILC(L) cannot achieve monotonic convergence due to the model uncertainty. The convergence by the setting RefILC(L)+TrqILC(L) is close to monotonic, since TrqILC(L) intends to make the inner plant behave as the load side nominal model and thus greatly releases the uncertainty burden on the reference ILC. In contrast, the setting RefILC(L)+TrqILC(M) actually performs the worst resulting unstable error trends over iterations, since TrqILC(M) intends to make the inner plant match with the motor side nominal model while the load side behavior may actually deviate further from its nominal behavior. The error profiles of the last iteration compared to the initial run are shown in Fig. 8.8, which confirms the above conclusion again. Due to the model uncertainty, the setting RefILC(L) does not perform as well as before. The setting RefILC(L)+TrqILC(M) actually deteriorates the performance due to the wrong model matching. Above all, the setting RefILC(L)+TrqILC(L) performs the best with the help of correct inner loop model matching. 8.5 Chapter Summary In this chapter, a model based two-stage ILC scheme was developed for a class of MIMO mismatched linear systems. To improve the performance of the ILC stage aimed at tracking error reduction, another ILC utilizing the idea of model following was designed to drive the inner plant to behave like the nominal model. The convergence property was investigated along with the design of Q filter and learning filter. In order for the two ILC stages to work together effectively, an ad hoc hybrid scheme was proposed to transition between the two ILC stages. A single-joint indirect drive system with several inherent as well as intentional external disturbances was utilized to experimentally validate the proposed ILC scheme. It was shown that the proposed hybrid two-stage ILC scheme using the load side nominal model outperformed the other two benchmark controller settings in the load side tracking application. The application extension of this work to a 6-joint robot manipulator will be discussed in Chapter 9.

151 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS Model Following Error (Max) Pos Error (rad) RefILC(L) RefILC(L)+TrqILC(M) RefILC(L)+TrqILC(L) Time (sec) (a) 1.2 x Load Side Position Error (Max) RefILC(L) RefILC(L)+TrqILC(M) RefILC(L)+TrqILC(L) Pos Error (rad) Time (sec) (b) Load Side Acceleration Error (Max) RefILC(L) RefILC(L)+TrqILC(M) RefILC(L)+TrqILC(L) Acc Error (rad/sec 2 ) Time (sec) (c) Figure 8.7: Error Convergence Comparisons using Nominal Model with 15% Parametric Uncertainties (1 Iterations)

152 CHAPTER 8. HYBRID TWO-STAGE MODEL BASED ITERATIVE LEARNING CONTROL SCHEME FOR MIMO MISMATCHED LINEAR SYSTEMS x Load Side Position Error Initial Run RefILC(L) RefILC(L)+TrqILC(M) RefILC(L)+TrqILC(L) Pos Error (rad) Time (sec) (a) Load Side Acceleration Error Initial Run RefILC(L) RefILC(L)+TrqILC(M) RefILC(L)+TrqILC(L) Acc Error (rad/sec 2 ) Time (sec) (b) Figure 8.8: Performance Comparisons using Nominal Model with 15% Parametric Uncertainties (After 1 Iterations)

153 136 Chapter 9 Iterative Learning Control with Sensor Fusion: Application to Multi-Joint Robot 9.1 Introduction As discussed in Chapter 2, the multi-joint robot with joint elasticity can be regarded as a typical mismatched dynamic system. Chapter 8 proposed a hybrid two-stage model based ILC scheme, where the reference trajectory and the feedforward torque input were both iteratively updated to achieve high bandwidth performance while maintaining convergence property. This scheme will be applied in this Chapter to deal with the mismatched system behaviors in the multi-joint robot with joint elasticity. While many literatures have suggested different kinds of iterative learning control (ILC) schemes[11, 7], one of the essential factors for the algorithms to work effectively is to choose and obtain the appropriate error information for the learning process. For robots with joint compliance, the load side (end-effector) performance is of ultimate interest, which cannot be guaranteed with motor side information alone[19, 91]. Chapter 8 and some early work[63, 57] on this topic considered only the single joint case or assumed availability of load side joint measurements. In industrial robots, however, the load side joint encoders are usually not available due to cost and assembly issue, thus creating a mismatched sensing problem. Chapter 5 has shown that this problem can be tackled by adopting a low-cost MEMS accelerometer to measure the end-effector vibration. The load side state estimation was done using an optimization based inverse differential kinematics algorithm and an adaptive kinematic Kalman filter (KKF) for each joint. The method is computationally light and will be utilized in this chapter to provide desired load side joint state estimates. Thus, in this chapter, the developed ILC scheme together with the sensor fusion method will be applied to the multi-joint robot with joint elasticity, to demonstrate its superior performance on the end-effector position tracking as well as the vibration reduction. This chapter is organized as follows. The robot modeling and control structure are reviewed first. These are followed by the application of the two-stage ILC scheme and the load side state estima-

154 CHAPTER 9. ITERATIVE LEARNING CONTROL WITH SENSOR FUSION: APPLICATION TO MULTI-JOINT ROBOT 137 tion method, which is to retrieve the load side joint information from the measured Cartesian space acceleration for implementation of the ILC scheme in the joint space. The experimental study on the 6-DOF FANUC robot is presented to validate the effectiveness of the proposed approach. Finally the conclusion is given. 9.2 System Overview Consider an n-joint robot with gear compliance. The robot is equipped with motor side encoders to provide direct measurements of motor side joint positions and velocities for real-time feedback. In addition, a three-dimensional accelerometer is mounted at the robot end-effector to measure the end-effector acceleration in Cartesian space. Note that, if the computing resource and the sensor configuration allow, the end-effector sensor can also be used online. This chapter, however, will address the conservative case where the end-effector sensor is for off-line and training use only, which is usually preferred in industry due to the cost saving and the limited real-time computational power Robot Dynamic Model Following the variable definitions in Chapter 2, the decoupled robot dynamic model ((2.26) in Chapter 2) can be restated as follows M m q m + D m q m =τ m + d m N 1 K J N 1 q m q l + DJ N 1 q m q l (9.1a) M n q l + D l q l = d l + K J N 1 q m q l + DJ N 1 q m q l (9.1b) where all the coupling and nonlinear terms, such as Coriolis/centrifugal force, gravity, Coulomb frictions, transmission error, and external forces, are grouped into the fictitious disturbance torques d m (q), d l (q) R n as d m (q)= F mc sgn( q m )+ N 1 K J q+ D J q (9.2a) d l (q)= M n M 1 (q l l ) I n KJ N 1 q m q l + DJ N 1 q m q l Dl q l M n M 1 (q l l ) C(q l, q l ) q l + G(q l )+ F lc sgn( q l )+ J T (q l )f ex t + K J q+ D J q (9.2b) Thus, the robot can be considered as a MIMO system with 2n inputs and 2n outputs in the following discrete-time form q m (j)= P mu (z)τ m (j)+ P md (z)d(j) q l (j)= P lu (z)τ m (j)+ P ld (z)d(j) (9.3a) (9.3b) where j is the time index, z is the one step time advance operator in the discrete time domain, and the fictitious disturbance input d(j) is defined as (2.3a). The transfer functions (P mu, P lu, P md, and P ld ) are derived as (2.31) in Chapter 2.

155 CHAPTER 9. ITERATIVE LEARNING CONTROL WITH SENSOR FUSION: APPLICATION TO MULTI-JOINT ROBOT 138 q ld,k ˆP muˆp 1 lu F 1 q d,k + r q,k q md,k F 2 ˆP mu 1 τ ln,k Inner Plant d k τ nl,k P ld + + P md + C + + µ k + + τ m,k P mu P lu q md,k τ fb,k e k q l,k q m,k q k ˆP u + q p,k e p,k Figure 9.1: Robot Control Structure with Reference & Torque Update Robot Controller Structure It is seen that the robot dynamic model (9.1) is in a decoupled manner since all the variables are expressed in the diagonal matrix form or vector form. Therefore, the robot controller can be implemented in a decentralized form for each individual joint. Note that the MIMO linear system representation (9.3) is obtained not through local linearization but by considering the fictitious disturbance d as an input which includes the model uncertainties and nonlinearities. The d l component of the fictitious disturbance d in (2.3a) influences the output in a different way from the motor torque inputτ m. Thus this robot system is regarded as a MIMO mismatched dynamic system. The compensation of this fictitious disturbance d should be considered in the controller design. To achieve this, the same controller structure (Fig. 9.1) developed in Chapter 8 can be applied to this system. The variables in this controller structure follow the definitions in Chapter 8. However, the initialization of two control updates (i.e., r q,k andτ nl,k ) for the first experiment iteration is designed as where r q, = NˆK 1 (ˆτ J ld ˆM n q ld D l q ld ) (9.4a) τ nl, =τ f f, ˆτ ln, (9.4b) ˆτ ld = ˆM l (q ld ) q ld + Ĉ(q ld, q ld ) q ld + Ĝ(q ld )+ D l q ld + F lc sgn( q ld )+ J T (q ld )f ex t,d (9.5a) ˆτ md, = ˆM m q md, +ˆD m q md, +ˆF mc sgn( q md, ) (9.5b) τ f f, =ˆτ md, + N 1ˆτ ld (9.5c)

156 CHAPTER 9. ITERATIVE LEARNING CONTROL WITH SENSOR FUSION: APPLICATION TO MULTI-JOINT ROBOT ILC Scheme with Sensor Fusion To deal with the MIMO mismatched dynamic system described above, the hybrid two-stage ILC scheme proposed in Chapter 8 is adopted here with specific consideration of the assumed sensor configuration Two ILC Stages For robot applications, the ultimate objective is to reduce the load side (end-effector) tracking error. Here, to make the algorithm more general, define the tracking error as e k q d,k q k. When load side learning is desired and feasible with available information, the tracking error is thus e k = e l,k q ld,k q l,k, with the corresponding transfer functions denoted as P u = P lu and P d = P ld. In contrast, if the load side information is not available for learning, only motor side learning can be conducted with e k = e m,k q md,k q m,k. Similarly, the corresponding transfer functions would be P u = P mu and P d = P md. As derived in Chapter 8, the tracking performance of the next iteration can be improved with the following reference update scheme using plant inversion learning filter r q,k+1 = Q r (r q,k + L r e k) (9.6) L r =ˆP 1 eu,r 1 =ˆP ˆP u mu (9.7) In order to achieve fast convergence rate without compromising the bandwidth of Q r, it is desired to reduce the model uncertainties. This can be done by making the inner plant (blue shaded area in Fig. 9.1) behave as the chosen nominal modelˆp u (e.g., making q k ˆP u µ k ). Thus, another ILC stage with torque update based on the idea of model following is formulated as τ nl,k+1 = Q u τnl,k + L e u p,k (9.8) L u =ˆP 1 = eu,u Ŝ 1 p ˆT 1 u =ˆP 1 u (9.9) where e p,k q p,k q k is the model following error between the nominal plant output (i.e., q p,k ˆP u µ k ) and the actual plant output q k. Similarly, depending on the available error information for learning process, this ILC stage can be either load side learning (i.e., q k = q l,k, P u = P lu, and P d = P ld ), or motor side learning (i.e., q k = q m,k, P u = P mu, and P d = P md ). In practice, the plant inversion in (9.9) usually encounters numerical difficulty since the relative order of P lu (s) or P mu (s) is 3 or 2. Thus it is more favorable to choose P u (s)=p lu (s)s 2 or P u (s)=p mu (s)s, both of which have lower relative orders. The corresponding desired learning information for this new choice of P u is the load side acceleration q l,k or the motor side velocity q m,k, which is available with the assumed sensor configuration (i.e., end-effector accelerometer and motor encoders). By similar derivation, the torque ILC stage with these changes can still be obtained exactly the same as (9.8)-(9.9), and achieves the same objective, i.e., making the inner plant behave as the chosen

157 CHAPTER 9. ITERATIVE LEARNING CONTROL WITH SENSOR FUSION: APPLICATION TO MULTI-JOINT ROBOT 14 nominal model. Another benefit of this modification is that the torque ILC will be effective for vibration suppression, while the reference ILC achieves the position error reduction. Thus, in the practical implementation such as the following experimental study, these changes will be applied to the torque ILC scheme to avoid numerical instability Hybrid Scheme For Two-Stage ILC Recall that in Chapter 8, the repetitive assumption is used in the derivation of the aforementioned two ILC schemes. Here, the disturbance d k depends on the actual robot state q, and is thus not repetitive. This can be relaxed in practice as follows: since the robot basic performance should be already close to satisfactory, the tiny changes of q around desired state q d in each iteration normally will not result in drastic change in d k. However, when these two ILC stages are activated simultaneously, the repetitive assumption will be no longer valid (i.e., r q,k andτ nl,k are not repetitive from one iteration to another). Therefore, an ad hoc hybrid scheme is designed to reduce the adverse interference of the two ILC stages. Specifically, an iteration-varying gain is applied to each ILC stage as follows τ nl,k+1 = Q u τnl,k +γ u,k L e u p,k (9.1) r q,k+1 = Q r (r q,k +γ r,k L r e k) (9.11) whereγ u,k = diag(γ 1, u,k,γn ) andγ u,k r,k= diag(γ 1, r,k,γn). The two r,k gainsγi u,k andγi for r,k the i-th joint can be tuned by trial and error, e.g. γ.2, i = max 6 e i min p,k 2 u,k γ i e i 1 u,k 1 p,k 1 2, 1 (9.12a) γ i = 1.8γ min 2 i ei k r,k u,k e i, 1 (9.12b) with the initialization asγ i u, = 1,γi r, following error is becoming stable (i.e., =.2. The basic idea behind is that once the model e i p,k 2 e i p,k 1 2 1, which means either the inner plant has behaved as the nominal model or the torque ILC cannot make further improvement), the torque ILC becomes less important and the reference ILC can be further activated with a decreasedγ i and an u,k increasedγi. In contrast, the torque ILC can take more effects and make r,k further improvement whenever the model following error is still drastically changing from the previous iteration. In order for the torque ILC to perform better, the effects of the reference ILC is accordingly attenuated with a decreasedγ i. Furthermore, if the maximum tracking r,k error is sufficiently small (i.e., ei k ), the necessity of the reference ILC diminishes. Thus, e i the gainγ i is accordingly decreased. However, to maintain the basic convergence rate, the r,k gainγ i for the torque ILC is constrained to be within[.2, 1] as indicated in (9.12). u,k

158 CHAPTER 9. ITERATIVE LEARNING CONTROL WITH SENSOR FUSION: APPLICATION TO MULTI-JOINT ROBOT 141 Remark 9.1. It is understood that the nominal models used in two ILC stages should match with each other for the hybrid scheme to perform well. This means these two stages need to be both load side learning or both motor side learning, but not learning on the two sides together, since the nominal behaviors of load side and motor side cannot be achieved simultaneously due to the mismatched dynamics Robot Load Side State Estimation Note that, in the above ILC scheme with load side learning, the required load side joint information (i.e., q l,k and q l,k ) cannot be measured directly. Therefore, it is desired to retrieve this information from the available sensing, i.e., by fusing the measured end-effector acceleration with the motor encoder measurements. The estimation scheme developed in Chapter 5 for off-line applications is thus utilized in this study to obtain the load side state estimates. After the EM procedure,ˆ q l in (5.15) andˆq l,j T from the last Kalman smoother iteration are used as the required load side state information in the ILC scheme for load side learning. 9.4 Experimental Study Test Setup The proposed method is implemented on the 6-joint FANUC M-16iB robot shown in Fig. 2.5 in Chapter 2. The built-in motor encoders and the 3-axial end-effector accelerometer are utilized for ILC and sensor fusion algorithms. The CompuGauge 3D system is utilized to measure the end-effector tool center point (TCP) position as a ground truth for performance validation. The sampling rates of all the sensor signals as well as the real-time controller implemented through MATLAB xpc Target are set to 1kHz. System identifications are conducted for each individual joint at several different postures to obtain the nominal dynamic parameters in the dynamic model (9.1) (see Appendix B for more details) Algorithm Setup Design the zero-phase acausal low-pass filters Q r and Q u as Q r (z)=q u (z)=q 1 (z 1 )Q 1 (z), where Q 1 (z) is a diagonal matrix of low-pass filters with cut-off frequencies beyond or around the identified first elastic anti-resonant frequency of the corresponding joint in order to deal with the joint elasticity. With this selection of Q r (z) and Q u (z), the frequency responses ofβ r in (8.13) andβ u in (8.19) using nominal values and load side inertia variations among the workspace are checked for each joint to verify the monotonic stability conditions. To see the superiority of proposed methods (i.e., hybrid two-stage scheme versus singlestage scheme, load side learning versus motor side learning), the tracking performances in the experiments will be compared in the following four controller settings implemented for 1 iterations each

159 CHAPTER 9. ITERATIVE LEARNING CONTROL WITH SENSOR FUSION: APPLICATION TO MULTI-JOINT ROBOT 142 RMS Error (m) x 1 3 TCP Position RMS Error RefILC(L) TrqILC(L) RefILC(M)+TrqILC(M) RefILC(L)+TrqILC(L) Iteration Figure 9.2: TCP Position RMS Error Comparisons in Iteration Domain 1. RefILC(L): Reference ILC only using load side learning, i.e.,ˆp u (s)=ˆp lu (s),γ r,k 1, and γ u,k. 2. TrqILC(L): Torque ILC only using load side learning, i.e.,ˆp u (s)=ˆp lu (s)s 2,γ r,k, and γ u,k RefILC(L)+TrqILC(L): Reference ILC plus torque ILC using load side learning, i.e.,ˆp u (s)= ˆP lu (s) for reference ILC andˆp u (s)=ˆp lu (s)s 2 for torque ILC.γ r,k andγ u,k are updated as in (9.12). 4. RefILC(M)+TrqILC(M): Reference ILC plus torque ILC using motor side learning, i.e., ˆP u (s)=ˆp mu (s) for reference ILC and ˆP u (s)= ˆP mu (s)s for torque ILC.γ r,k andγ u,k are updated as in (9.12) Experimental Results The testing TCP trajectory is the same as the one (Fig. 5.3) in Chapter 5 for sensor fusion method validation, which is a 1cm 1cm square path on the Y-Z plane with fixed orientation, maximum velocity of 1m/sec, and maximum acceleration of 12.5m/sec 2. Figure 9.2 and Fig. 9.3 show the iteration domain root-mean-square (RMS) tracking error 1 convergence profiles, while Fig. 9.4 and Fig. 9.5 show the time domain error profiles of the initial run and the last runs of these controller settings. It can be seen that the RefILC(L)+TrqILC(L) setting achieves the overall best performance in position tracking and vibration reduction, even though there is non-monotonic transient 1 The Cartesian space error here is defined as the Euclidean distance between the estimated position/acceleration and the actual measured position/acceleration.

160 CHAPTER 9. ITERATIVE LEARNING CONTROL WITH SENSOR FUSION: APPLICATION TO MULTI-JOINT ROBOT IMU Acceleration RMS Error RMS Error (m/sec 2 ) RefILC(L) TrqILC(L) RefILC(M)+TrqILC(M) RefILC(L)+TrqILC(L) Iteration Figure 9.3: IMU Acceleration RMS Error Comparisons in Iteration Domain 8 x 1 3 TCP Position Error 6 Error (m) Time (sec) 3 2 Initial Run RefILC(M)+TrqILC(M) RefILC(L) TrqILC(L) RefILC(L)+TrqILC(L) Figure 9.4: TCP Position Error Comparisons in Time Domain

161 CHAPTER 9. ITERATIVE LEARNING CONTROL WITH SENSOR FUSION: APPLICATION TO MULTI-JOINT ROBOT 144 IMU Acceleration Error 6 Error (m/sec 2 ) Time (sec) 3 2 RefILC(L) RefILC(M)+TrqILC(M) Initial Run TrqILC(L) RefILC(L)+TrqILC(L) Figure 9.5: IMU Acceleration Error Comparisons in Time Domain around the 5-th and 6-th iterations in the position error convergence due to the interference between the two ILC stages. The RefILC(L) setting turns out to be unstable in the iteration domain without the help of torque ILC to reduce model mismatched uncertainty. This implies that the Q filter bandwidth (i.e., learning ability) for the RefILC(L) setting needs to be further compromised. The TrqILC(L) setting looks monotonically convergent but with quite limited improvement in error reductions (especially for moving periods), since the torque ILC aims at modeling matching for the inner plant rather than directly addresses the load side tracking error. The RefILC(M)+TrqILC(M) setting does not perform well either, since motor side model can only be used for motor side learning, while the load side (end-effector) performance is not guaranteed and may be even degraded due to the mismatched dynamics. 9.5 Chapter Summary This chapter applied the model based ILC scheme developed in Chapter 8 to the multi-joint robot with joint compliance for end-effector performance enhancement. The scheme iteratively updated the reference trajectory and the feedforward torque input in a two-stage manner in order to achieve both high bandwidth performance and robust convergence. In order to implement the decentralized ILC scheme in joint space, the load side state estimation method developed in Chapter 5 was applied to retrieve the joint space information from the endeffector measurements. The resulting ILC scheme and the load side state estimation method were validated through the experimental study on a 6-DOF industrial robot. The study verified the benefits of sensor fusion utilizing end-effector measurements for learning process, and the advantage of the hybrid two-stage ILC scheme to deal with the mismatched dynamics, in end-effector position tracking and vibration mitigation.

162 145 Chapter 1 Concluding Remarks and Open Issues 1.1 Concluding Remarks In the motion control of robots with joint flexibilities, the key inherent challenge is that the robots exhibit mismatched dynamics and mismatched sensing. In other words, the desired variable cannot be directly controlled or sensed. This dissertation attempted to solve this fundamental problem through a mechatronic approach with contributions to hardware (sensor) configuration, dynamic modeling, algorithm developments, and real-world experimentation. More specifically, the two mismatched problems were tackled from the perspectives of both estimation and control as follows: Handling Mismatched Sensing: Sensor Fusion Approaches This dissertation explored various sensor fusion schemes for handling mismatched sensing in three fundamental cases: Load side joint state estimation in single-joint robot End-effector state estimation in multi-joint robot Load side joint state estimation in multi-joint robot The sensor fusion algorithms for these three cases can meet most real-world needs in robots with mismatched sensing. The sensor fusion schemes utilized dynamic and/or kinematic models. The study also examined various practical issues such as economic benefit, measurement dynamics, model uncertainty, fictitious noise, and computation effort in one or more algorithm designs. The designed algorithms and their validations through simulation and experimentation implicitly confirmed the following arguments It is undoubtedly beneficial to supplement robots with additional sensing on the load side (for single-joint robots) or on the end-effector side (for multi-joint robots).

163 CHAPTER 1. CONCLUDING REMARKS AND OPEN ISSUES 146 With proper sensor fusion, even low-cost and unprecise sensors can provide significant performance enhancement. With careful customization, it is possible for the computation load of sensor fusion to be considerably light. Especially, the developed load side state estimation scheme is suitable for both off-line and online processing even with limited industrial computation power. Handling Mismatched Dynamics: Control Approaches This dissertation examined various control approaches for handling mismatched dynamics in both the real-time domain (e.g., adaptive friction compensation, disturbance observer (DOB), and adaptive robust controller (ARC)) and the iteration domain (e.g., iterative learning control (ILC)). It was shown that compensation for the load side/end-effector uncertainty/disturbance is necessary in addition to that for the motor side. However, if the focus is in the low frequency range, then perfect motor side disturbance rejection also results in effective disturbance rejection on the load side/end-effector. The novel decoupled model formulation enabled the implementation of the proposed controllers in the decentralized manner for multi-joint robots. Considering the nature of the mismatched system as a two-mass system, the real-time and iterative approaches were utilized in a hybrid two-stage manner to deal with the mismatched dynamics. The algorithms manipulated both the motor side reference trajectory and the torque input, in real-time or iteratively, using hybrid transition mechanisms to enable the two stages to work together effectively. It was experimentally demonstrated that better load side/endeffector performance (or higher bandwidth learning for the iterative approach) could be safely achieved in this way in the presence of mismatched uncertainties/disturbances. 1.2 Future Avenues of Research The results in this dissertation raise several issues as further research topics. Sensor Fusion The load side joint state estimation method (Chapter 5) is designed to satisfy industrial demands for decentralized robot control. It can be, however, further improved by considering more realistic factors. These issues include the sensor measurement dynamics (e.g., timevarying biases and noises) and model uncertainty (e.g., dynamic and kinematic uncertainties). These issues were considered in the single-joint sensor fusion study. The merit of the approach may be properly extended to the multi-joint robot case. Furthermore, similar to the multi-rate MD-KKF (for end-effector state estimation in Chapter 4), the multi-rate extension can also be investigated for load side joint state estimation, when sensors with different sampling rates are used.

164 CHAPTER 1. CONCLUDING REMARKS AND OPEN ISSUES 147 Control The transfer function analysis and the experimentation in Chapter 7 demonstrated the significant improvement of the load side/end-effector performance by low frequency disturbance rejection from the motor side. However, to further enhance high frequency load side performance, the direct load side disturbance rejection becomes necessary. Disturbance rejections on both the motor side and the load side need to be active for optimal performance. This may be approached in a hybrid two-stage manner similar to the ILC scheme in Chapter 8-9. The stability assurance for the designed real-time feedback algorithms will also be among the future work. The ILC scheme in Chapter 9 demonstrated its superior performance. Some ad hoc settings, however, could be further investigated and replaced by a systematic control synthesis. These settings include the optimal tuning of the two iteration-varying learning gains in (9.12) for any given trajectory. The learning gain synthesis may be formulated as an optimization problem with the cost function to properly describe the trade off between the two stages. Nonlinear programming techniques may be employed to solve the optimization problem with experimental data. Other Applications The sensor fusion methods (especially the load side joint state estimation) can also be applied to applications such as the automatic controller tuning for load side/end-effector performance enhancement. For example, the estimated load side joint states can be incorporated in the cost functions in[93, 16] to address the load side/end-effector performance. The optimization methods stated thereafter in[93, 16] can be then used to find the optimal controller gains. Another important area that could be addressed is force/impedance sensing and estimation at the load side/end-effector. With estimated load side/end-effector states, the information of force/impedance may be properly observed. This extension would greatly broaden the application area from position control to force/impedance control, or even beyond industrial automation (such as service robots which require more human-robot interaction).

165 148 Bibliography [1] Kistler (833A3). Online. May 29. [2] Paul D. Abramson. Simultaneous estimation of the state and noise statistics in linear dynamical systems. PhD thesis. Massachusetts Institute of Technology, [3] Analog Devices (ADIS164). /products/product.html. Online. July 29. [4] Analog Devices (ADXRS15). Online. July 29. [5] Friedhelm Altpeter. Friction Modeling, Identification and Compensation. PhD thesis. Ecole Polytechnique Federale de Lausanne, [6] MATLAB Simulink 3D Animation. Online. May 212. [7] Suguru Arimoto, Sadao Kawamura, and Fumio Miyazaki. Bettering operation of Robots by learning. In: Journal of Robotic Systems 1.2 (1984), pp [8] B Armstronghelouvry, Pierre Dupont, and Carlos Canudas De Wit. A survey of models, analysis tools and compensation methods for the control of machines with friction. In: Automatica 3.7 (1994), pp [9] Jonathan Asensio, Wenjie Chen, and Masayoshi Tomizuka. Robot Learning Control Based on Neural Network Prediction. In: Proceedings of the ASME Dynamic Systems and Control Conference (DSCC) [1] Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Bayesian State Estimation of a Flexible Industrial Robot. Tech. rep. LiTH-ISY-R-327. SE Linkoping, Sweden: Department of Electrical Engineering, Linkoping University, Oct [11] Douglas A Bristow and Marina Tharayil. A survey of iterative learning control: A learning-based method for high-performance tracking control. In: IEEE Control Systems Magazine June (26), pp [12] H Bruyninckx and J Deschutter. Symbolic differentiation of the velocity mapping for a serial kinematic chain. In: Mechanism and Machine Theory 31.2 (Feb. 1996), pp

166 BIBLIOGRAPHY 149 [13] P Lischinsky C. Canudas de Wit. Adaptive friction compensation with partially known dynamic friction model. In: International Journal of Adaptive Control and Signal Processing 11.1 (1997), pp [14] L Le Cam. Maximum likelihood: an introduction. In: Statistical Review/Revue Internationale de Statistique 58.2 (199), pp [15] C. Canudas de Wit, H. Olsson, K.J. Astrom, and P. Lischinsky. A new model for control of systems with friction. In: IEEE Transactions on Automatic Control 4.3 (1995), pp [16] Michael Chan, Kyoungchul Kong, and Masayoshi Tomizuka. Automatic Controller Gain Tuning of a Multiple Joint Robot Based on Modified Extremum Seeking Control. In: Proceedings of the 18th IFAC World Congress. 211, pp [17] Wenjie Chen. UCB-FANUC M-16iB Robot Experimentor. Software. Version 4.2. University of California at Berkeley, 212. [18] Wenjie Chen. UCB-FANUC M-16iB Robot Simulator. Software. Version 4.2. University of California at Berkeley, 212. [19] Wenjie Chen, Kyoungchul Kong, and Masayoshi Tomizuka. Hybrid Adaptive Friction Compensation of Indirect Drive Trains. In: Proceedings of the 29 ASME Dynamic Systems and Control Conference (DSCC). 29, pp [2] Wenjie Chen, Pedro R. Mora, Mike Chan, Cong Wang, Chung-Yen Lin, and Masayoshi Tomizuka. UCB-FANUC Project, Activity Report June May 212. Tech. rep. Department of Mechanical Engineering, University of California, Berkeley, 212. [21] Wenjie Chen, Pedro Reynoso Mora, Mike Chan, Cheng-Huei Han, and Masayoshi Tomizuka. UCB-FANUC Project Activity Report June 29 - July 21. Tech. rep. Department of Mechanical Engineering, University of California at Berkeley, 21. [22] Wenjie Chen, Pedro Reynoso Mora, Mike Chan, Cong Wang, and Masayoshi Tomizuka. UCB-FANUC Project, Activity Report June 21 - May 211. Tech. rep. Department of Mechanical Engineering, University of California, Berkeley, 211. [23] Wenjie Chen and Masayoshi Tomizuka. A Two-Stage Model Based Iterative Learning Control Scheme for a Class of MIMO Mismatched Linear Systems. In: Proceedings of the ASME International Symposium on Flexible Automation (ISFA) [24] Wenjie Chen and Masayoshi Tomizuka. Estimation of load side position in indirect drive robots by sensor fusion and kalman filtering. In: Proceedings of American Control Conference (ACC). 21, pp [25] Wenjie Chen and Masayoshi Tomizuka. Iterative Learning Control with Sensor Fusion for Robots with Mismatched Dynamics and Mismatched Sensing. In: Proceedings of the ASME Dynamic Systems and Control Conference (DSCC). 212.

167 BIBLIOGRAPHY 15 [26] Wenjie Chen and Masayoshi Tomizuka. Load Side State Estimation in Robot with Joint Elasticity. In: Proceedings of the IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM) [27] Peng Cheng and Bengt Oelmann. Joint-Angle Measurement Using Accelerometers and Gyroscopes A Survey. In: IEEE Transactions on Instrumentation and Measurement 59.2 (21), pp [28] CompuGauge 3D. Online. 29. [29] P.I. Corke. A Robotics Toolbox for MATLAB. In: IEEE Robotics and Automation Magazine 3.1 (1996), pp [3] D. De Roover and O.H. Bosgra. Synthesis of robust multivariable iterative learning controllers with application to a wafer stage motion system. In: International Journal of Control 73.1 (2), pp [31] R Dhaouadi. Nonlinear friction compensation in harmonic drives with hysteresis. In: Proceedings of IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM). Vol , pp [32] V Digalakis, J.R. Rohlicek, and M Ostendorf. ML estimation of a stochastic linear system with the EM algorithm and its application to speech recognition. In: IEEE Transactions on Speech and Audio Processing 1.4 (1993), pp [33] A Dixit and S Suryanarayanan. Adaptive observers for servo systems with friction. In: Proceedings of IEEE International Conference on Control Applications (CCA). 28, pp [34] RA Fisher. Theory of statistical estimation. In: Mathematical Proceedings of the Cambridge 22 (1925), pp [35] P.S. Gandhi, F.H. Ghorbel, and J. Dabney. Modeling, identification, and compensation of friction in harmonic drives. In: Proceedings of the 41st IEEE Conference on Decision and Control. Vol , pp [36] Miguel Lagullón García. Robot Simulation based on the Integration of MatlabSimulink and UGS Platforms. MA thesis. University of California, Berkeley, and Polytechnic University of Valencia, 211. [37] GE. Online. May 29. [38] Zoubin Ghahramani and Geoffrey E. Hinton. Parameter estimation for linear dynamical systems. Tech. rep. CRG-TR University of Toronto, [39] Fathi H. Ghorbel, Prasanna S. Gandhi, and Friedhelm Alpeter. On the Kinematic Error in Harmonic Drive Gears. In: Transactions of the ASME, Journal of Mechanical Design (21), pp [4] F. Gustafsson. Determining the initial states in forward-backward filtering. In: IEEE Transactions on Signal Processing 44.4 (Apr. 1996), pp

168 BIBLIOGRAPHY 151 [41] W.B.J. Hakvoort, R.G.K.M. Aarts, J. van Dijk, and J.B. Jonker. Model-based iterative learning control applied to an industrial robot with elasticity. In: Proceedings of IEEE Conference on Decision and Control. 27, pp [42] Cheng-huei Han. High Precision Control of Indirect Drive Systems Based on Endeffector Sensor Information. PhD thesis. University of California, Berkeley, 29. [43] Cheng-huei Han, Wenjie Chen, Pedro R. Mora, Mike Chan, and Masayoshi Tomizuka. UCB-FANUC Project Activity Report June 28 - July 29. Tech. rep. Department of Mechanical Engineering, University of California at Berkeley, 29. [44] Cheng-Huei Han, Chun-Chih Wang, Wenjie Chen, and Masayoshi Tomizuka. UCB- FANUC Project Activity Report June 27 - July 28. Tech. rep. Department of Mechanical Engineering, University of California at Berkeley, 28. [45] Hassan K. Khalil. Nonlinear Systems. Prentice Hall, 22. [46] J P Hauschild, G. Heppler, and J. McPhee. Friction compensation of harmonic drive actuators. In: Proceedings of the 6th International Conference on Dynamics and Control of Systems and Structures in Space. v. 24, pp [47] J. K. Hedrick and P. P. Yip. Multiple Sliding Surface Control: Theory and Application. In: Journal of Dynamic Systems, Measurement, and Control (2), pp [48] Robert Henriksson, M. Norrlof, Stig Moberg, Erik Wernholt, and TB Schon. Experimental comparison of observers for tool position estimation of industrial robots. In: Proceedings of the 48th IEEE Conference on Decision and Control. 29, pp [49] Kim Heui-Wook and Sul Seung-Ki. A new motor speed estimator using Kalman filter in low-speed range. In: IEEE Transactions on Industrial Electronics 43.4 (1996), pp [5] Kiyonori Inaba. Iterative learning control for industrial robots with end effector sensing. PhD thesis. University of California at Berkeley, 29. [51] Soo Jeon and Masayoshi Tomizuka. Benefits of acceleration measurement in velocity estimation and motion control. In: Control Engineering Practice 15.3 (Mar. 27), pp [52] Soo Jeon and Masayoshi Tomizuka. Limit cycles due to friction forces in flexible joint mechanisms. In: Proceedings of IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM). 25, pp [53] Soo Jeon, Masayoshi Tomizuka, and Tetsuaki Katou. Kinematic Kalman Filter (KKF) for Robot End-Effector Sensing. In: Journal of Dynamic Systems, Measurement, and Control (29), pp [54] National Instruments LabVIEW. July 212. [55] Paul Lambrechts, Matthijs Boerlage, and Maarten Steinbuch. Trajectory planning and feedforward design for electromechanical motion systems. In: Control Engineering Practice 13.2 (25), pp

169 BIBLIOGRAPHY 152 [56] D J Lee and Masayoshi Tomizuka. State/Parameter/Disturbance Estimation with an Accelerometer in Precision Motion Control of a Linear Motor. In: Proceedings of the ASME International Mechanical Engineering Congress and Exposition (IMECE). 21. [57] Alessandro De Luca and Giovanni Ulivi. Iterative Learning Control of Robots with Elastic Joints. In: Proceedings of IEEE International Conference on Robotic and Automation (ICRA). Vol , pp [58] J. Y. S. Luh. Conventional controller design for industrial robots - A tutorial. In: IEEE Transactions on Systems, Man, and Cybernetics 13 (1983), pp [59] FANUC Ltd. (M-16iB). m_16ib_2_1l.pdf. Online. Apr [6] MATLAB. Online. May 212. [61] Peter S Maybeck. Stochastic Models, Estimation, and Control. Vol. 2. New York: Academic Press, Inc., [62] R. Mehra. Approaches to adaptive filtering. In: IEEE Transactions on Automatic Control 17.5 (1972), pp [63] F Miyazaki, S Kawamura, M. Matsumori, and S. Arimoto. Learning control scheme for a class of robot systems with elasticity. In: Proceedings of the 25th IEEE Conference on Decision and Control, vol , pp [64] R.M. Murray, Z. Li, and S.S. Sastry. A Mathematical Introduction to ROBOTIC MANIPU- LATION. 1st. CRC Press, [65] Unigraphics Nx7. Online. June 211. [66] K Ohnishi. A new servo method in mechatronics. In: Transactions of Japanese Society of Electrical Engineering 17-D (1987), pp [67] DH Owens, JJ Hatonen, and S. Daley. Robust monotone gradient based discrete time iterative learning control. In: International Journal of Robust and Nonlinear Control 19 (29), pp [68] Francois Padieu and Renjeng Su. An H approach to learning control systems. In: International Journal of Adaptive Control and Signal Processing 4.6 (Nov. 199), pp [69] Morgan Quigley, Reuben Brewer, S.P. Soundararaj, Vijay Pradeep, Quoc Le, and A.Y. Ng. Low-cost accelerometers for robotic manipulator perception. In: Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 21, pp [7] P. Roan, N. Deshpande, Y. Wang, and B. Pitzer. Manipulator state estimation with low-cost accelerometers and gyroscopes. In: Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 212.

170 BIBLIOGRAPHY 153 [71] Kwon SangJoo, Chung Wan Kyun, and Youm Youngil. A combined observer for robust state estimation and Kalman filtering. In: Proceedings of the American Control Conference (ACC). Vol , vol.3. [72] Scholarpedia. Online. June 29. [73] R. H. Shumway and D. S. Stoffer. An Approach To Time Series Smoothing and Forecasting Using the Em Algorithm. In: Journal of Time Series Analysis 3.4 (July 1982), pp [74] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo. Robotics: Modelling, Planning and Control. 1st. Advanced Textbooks in Control and Signal Processing. Springer-Verlag, 29. [75] Jean-Jacques E Slotine and Weiping Li. Applied nonlinear control. Upper Saddle River, NJ: Prentice-Hall, [76] H D Taghirad and P R Belanger. Modeling and Parameter Identification of Harmonic Drive Systems. In: Journal of Dynamic Systems, Measurement, and Control 12.4 (1998), pp [77] H D Taghirad and P R Belanger. Robust friction compensator for harmonic drive transmission. In: Proceedings of the IEEE International Conference on Control Applications. Vol , pp [78] Yaolong Tan and I. Kanellakopoulos. Adaptive nonlinear friction compensation with parametric uncertainties. In: Proceedings of the American Control Conference (ACC). Vol , pp [79] T Tjahjowidodo, F Al-Bender, Hendrik Van Brussel, and Wim Symens. Positioning controller for mechanical systems with a mini harmonic drive servo actuator. In: Proceedings of IEEE/ASME international conference on Advanced intelligent mechatronics (AIM). 27, pp [8] Masayoshi Tomizuka. Advanced Control Systems II. Class Notes for ME233, University of California at Berkeley, Spring 28. [81] Masayoshi Tomizuka. Mechatronics: from the 2th to 21st century. In: Control engineering practice 1 (22), pp [82] Masayoshi Tomizuka. On the compensation of friction forces in precision motion control. In: Proceedings of the Asia-Pacific Workshop on Advances in Motion Control. 1993, pp [83] Masayoshi Tomizuka, T.-C. Tsao, and K Chew. Discrete-time domain analysis and synthesis of repetitive controllers. In: ASME Journal of Dynamic Systems, Measurement, and Control 111 (1989), pp [84] MATLAB SimMechanics Toolbox. Online. May 212.

171 BIBLIOGRAPHY 154 [85] E D Tung, G Anwar, and Masayoshi Tomizuka. Low Velocity Friction Compensation and Feedforward Solution Based on Repetitive Control. In: Journal of Dynamic Systems, Measurement, and Control 115.2A (1993), pp [86] S. Tungpataratanawong, K Ohnishi, and T. Miyazaki. High performance robust motion control of industrial robot using parameter identification based on resonant frequency. In: Proceedings of the 3th Annual Conference of IEEE Industrial Electronics Society (IECON) (24), pp [87] T. Umeno and Y. Hori. Robust speed control of DC servomotors using modern two degrees-of-freedom controller design. In: IEEE Transactions on Industrial Electronics 38.5 (1991), pp [88] Virtual Reality Modeling Language (VRML). Online. May 212. [89] Masayoshi Wada, Takashi Tsukahara, Kiichiro Tsuda, Fuji Electric, and Electronics Development Division. Learning Control of Elastic Joint Robot and its Application to the Industrial Robot Manipulator. In: Proceedings of the IEEE International Conference on Robotics and Automation (1993), pp [9] J. Wallén, Svante Gunnarsson, Robert Henriksson, Stig Moberg, and M. Norrlof. ILC applied to a flexible two-link robot model using sensor-fusion-based estimates. In: Proceedings of the 48th IEEE Conference on Decision and Control (CDC) , pp [91] Johanna Wallén, Mikael Norrlöf, and Svante Gunnarsson. Arm-side evaluation of ILC applied to a six-degrees-of-freedom industrial robot. In: Proceedings of 17th IFAC World Congress. Seoul, Korea, July 28, pp [92] Chun-Chih Wang. Motion Control of Indirect-drive Robots: Model Based Controller Design and Performance Enhancement Based on Load-side Sensors. PhD thesis. University of California at Berkeley, 28. [93] Chun-Chih Wang and Masayoshi Tomizuka. Sensor-based controller tuning of indirect drive trains. In: Proceedings of the 1th IEEE International Workshop on Advanced Motion Control (AMC). 28, pp [94] Cong Wang, Wenjie Chen, and Masayoshi Tomizuka. Robot End-effector Sensing with Position Sensitive Detector and Inertial Sensors. In: Proceedings of the IEEE International Conference on Robot and Automation (ICRA). 212, pp [95] Zhu Wen-Hong and M. Doyon. Adaptive control of harmonic drives. In: Proceedings of the 43rd IEEE Conference on Decision and Control (CDC) 3 (24), pp [96] Wikipedia. July 212. [97] Carlos Canudas de Wit, Georges Bastin, and Bruno Siciliano. Theory of Robot Control. 1st. Secaucus, NJ: Springer-Verlag New York, Inc., Jan

172 BIBLIOGRAPHY 155 [98] Li Xu and Bin Yao. Adaptive Robust Control of Mechanical Systems with Nonlinear Dynamic Friction Compensation. In: Proceedings of the American Control Conference. Vol. 4. 2, pp [99] Bin Yao, M. Al-Majed, and Masayoshi Tomizuka. High-performance robust motion control of machine tools: an adaptive robust control approach and comparative experiments. In: IEEE/ASME Transactions on Mechatronics 2.2 (June 1997), pp [1] Bin Yao and Masayoshi Tomizuka. Adaptive robust control of MIMO nonlinear systems in semi-strict feedback forms. In: Automatica 37.9 (21), pp [11] Bin Yao and Masayoshi Tomizuka. Adaptive robust control of SISO nonlinear systems in a semi-strict feedback form. In: Automatica 33.5 (1997), pp [12] Bin Yao and Masayoshi Tomizuka. Smooth Robust Adaptive Sliding Mode Control of Manipulators With Guaranteed Transient Performance. In: Journal of Dynamic Systems, Measurement, and Control (Dec. 1996), pp [13] Daekyu Yun, Chi-Shen Tsai, Wenjie Chen, and Masayoshi Tomizuka. Development of DesignAnalysis and Evaluation Technologies for Vibration Reduction of LCD Substrate Transfer Robot. Tech. rep. University of California at Berkeley, Feb. 212.

173 156 Part IV Appendix

174 157 Appendix A Robot System Setup This appendix provides more details about the FANUC M-16iB robot system setup introduced in Chapter 2. The hardware connection diagram of this system setup is re-illustrated in Fig. A.1. A.1 Robot Controller Hardware Configuration The M-16iB robot controller by FANUC provides robot position and velocity feedback control. Once the desired trajectory is determined, it is programmed in the controller, and followed by the robot during operation. However, the structure of the commercial robot controller is fixed and does not offer flexibility in the customization of the control algorithm. It is desired to design and implement control algorithms using MATLAB[6] for the FANUC M-16iB robot for research purpose. Therefore, a DSA (Digital Servo Adapter) is utilized to connect the real-time target computer (see Section A.2) with the FANUC robot controller. The control algorithms can be designed by MATLAB on the host computer. By using DSA, the control signal (i.e., motor torque command) is determined by the control algorithm running on the target computer, and is converted to the current command for the FANUC robot controller to actuate the robot. Thus the feedback control algorithm in the FANUC robot controller is no longer in action. However, the FANUC robot controller still plays significant roles in current feedback control and brake control. The current feedback loop is active in the FANUC robot controller to generate the motor current as desired. The brake function is controlled with a DIO (Digital Input/Output) board installed on the target computer (see Brake box in Fig. A.1). The robot driver used in the target PC controller turns off the brake when the operation starts and turns on the brake after the operation ends. In case of emergency (e.g., robot motion speed becomes too high), the FANUC robot controller automatically activates the brake of the motors. The operator could also activate the brake with the emergency button on the robot teaching pendant.

175 APPENDIX A. ROBOT SYSTEM SETUP 158 Figure A.1: FANUC M-16iB Robot System Setup Scheme A.2 Real-Time System Commercialized programs, such as MATLAB[6] and LABVIEW[54] that include control toolboxes, provide engineers with various advantages. In the robot system setup, MATLAB installed on Windows platform together with xpc Target is utilized to design and implement real-time control algorithms. In real-time systems, the exact sampling period is required such that uncertainties in timing caused by control equipments are minimized. Windows systems, however, do not guarantee the exact sampling period due to unexpected interferences such as virus checking and event logging and are not appropriate for the real-time operation. To overcome these problems, two computers are used in the real-time control system setup. The host computer is operated on Windows platform, and therefore allows users to easily use MATLAB/Simulink to design control algorithms. In another computer called the target computer, the system is operated based on the xpc Target system such that the real-time

176 APPENDIX A. ROBOT SYSTEM SETUP 159 x xz y O z o y Figure A.2: Payload Mounted on Joint 6 processing is guaranteed. Notice that the target computer has access of the motor position, the motor velocity, and the motor current through HSSB (High-Speed Serial Bus). The host and target computers are connected via Ethernet. Once a control algorithm is designed on the host computer, it is compiled and downloaded to the target computer. When the control algorithm is running on the target computer, the connection between the two computers is automatically disconnected such that the real-time execution of the algorithm is not interfered by the host computer. A.3 Payload Design 1 A payload is designed to attach to the last joint of the robot as the end-effector. Sensor mounting issues such as the direction of measurement and the straightness of the mounting surface are considered in the design. As shown in Fig. A.2, the payload is made of a steel block and a steel plate, which weights about 18.37kg in total. This is close to the maximum load weight (i.e., 2kg) of this robot in order to test the robot under this extreme condition. It is mounted on the 6-th joint (J6) of the FANUC M-16iB/2 robot using pin and screw fixtures. The facade of the plate and the bottom surface of the block can be used for mounting the inertia measurement unit (IMU), vision sensor, the CompuGauge measuring tip, etc. Since the accuracy of the sensor measurements highly depends on the flatness of mounting surfaces, it is necessary to make the surfaces of the payload within certain straightness requirements. Figure A.3 shows the measured results of the payload after final machining. The final specification of the payload is listed in Table A.1. The center point of the tip of J6 is regarded 1 Payload design and machining was performed by Dr. Soo Jeon, a former Ph.D graduate, and Mr. Yasuyuki Matsuda, a Visiting Industrial Fellow from NSK Corporation. The final refinement of the payload with regard to the sensor mounting was performed by the author.

177 APPENDIX A. ROBOT SYSTEM SETUP 16 Figure A.3: Payload Surface Straightness Table A.1: Specification of the Payload Parameter Unit Value Mass kg Volume m Center of Mass xc, y c, z c mm (68.135,.9, ) Moments of Inertia I x x I x y I xz I y x I y y I yz I zx I z y I zz kgm as the origin of the payload coordinates (the coordinates direction is shown in Fig. A.2 which is the same as the world coordinates defined in Fig. 2.7). The moments of inertia are taken at the center of mass and aligned with the payload coordinates, where I jk denotes the moment of inertia along the k-axis when the object is rotated along the j-axis. A.4 Sensor Configurations A.4.1 Accelerometer For system identification (see Appendix B for details) of each individual joint, an accelerometer (Kistler, Type: 833A3[1]) is installed on the payload (Fig. A.2 - Fig. A.3) to measure the end-

178 APPENDIX A. ROBOT SYSTEM SETUP 161 effector state. It is mounted with a cube made of polypropylene to avoid the electrical noise transferred through robot frames. For details of this sensor mounting and configuration, please refer to the work in[44]. A.4.2 Inertia Sensor Setup The above accelerometer is of high-sensitivity with good dynamic range. However, it is only capable of single axis acceleration measurement. In this 6 degree-of-freedom (DOF) robot, it is desired to have the sensor to measure the multi-dimensional information of the end-effector for the general motion. Therefore, an inertia measurement unit (Analog Devices, ADIS164 [3]) is adopted on the end-effector (i.e., payload) of the FANUC M-16iB robot. This sensor functions as a 3-axial gyroscope, a 3-axial accelerometer, and a 3-axial magnetometer. The addition of this sensor allows to explore the methodologies utilizing more information from the end-effector. The specification of this sensor can be found in[3]. Data Acquisition The acquisition of the inertia sensor signals in real-time turns out to be a non-trivial task for the FANUC M-16iB system. The inertia sensor, ADIS164, outputs the sensor signals in Serial Peripheral Interface Bus (SPI) format, which cannot be directly read by the xpc target system. To solve this issue, a driver to decode the SPI signals was programmed in an NI FPGA board, which is set to auto load and auto run the driver when powered up. The inertia sensor SPI output is fed into the NI FPGA board through an NI CB-68LP connector block. The NI FPGA board decodes the SPI data input and then outputs all the sensor signals (i.e., 3-axial gyroscope, 3-axial accelerometer, 3-axial magnetometer, and temperature) in analog form through the NI CB-68LP connector block. These analog signals are directly read through an NI SCB-68 connector block into an NI PCI-623E DAQ board installed in the xpc Target system. This data acquisition procedure is illustrated in Fig. A.4. Noise Issue A Cat-6 shielded network cable is utilized to transfer the signals from the inertia sensor on the end-effector to the NI FPGA board. The cable is suitably attached to the robot and selected to be long enough to match the robot workspace. The long cable, however, has a large resistance and noise inductance, which might attenuate the sensor signals and also add noise into them. Moreover, it is known that the PWM (Pulse-Width Modulation) signal used to drive the motor causes large noises when the motor control is activated, which frequently changes the magnitude and the sign of the signal. Particularly, the noise induced by the PWM signal shows repetitive peaks due to rapid changes in magnetic flux. To reduce the huge noise influence on the sensor signals by PWM, a shielded cable and separated grounds have been utilized. Furthermore, several ferrite beads are employed on the shielded cable to suppress the high-frequency noise (especially the noise induced by PWM). The resulting sensor signals

179 APPENDIX A. ROBOT SYSTEM SETUP 162 Figure A.4: Inertia Sensor Setup Configuration Table A.2: Covariance of the Inertia Sensor Signals (in IMU Coordinates) a Sensor Value Gyroscope Unit: (rad/sec) 2 σ 2 σ 2 σ 2 x x x y xz σ 2 σ 2 σ 2 y x y y yz σ 2 σ 2 σ 2 zx z y zz Accelerometer Unit: (m/sec 2 ) 2 σ 2 x x σ 2 x y σ 2 xz σ 2 y x σ 2 y y σ 2 yz σ 2 zx σ 2 z y σ 2 zz a The covariance result may subject to change from one experiment to another experiment due to the timevarying property of the MEMS sensor for environmental changes. The values shown here are from one set of typical identification experiment and indicate the rough level of the covariance values for this IMU sensor. are as clean as indicated on the sensor specification in[3]. This can be seen through the covariance of the sensor signals (e.g., gyroscope and accelerometer measurements) identified in this system as shown in Table A.2.

180 APPENDIX A. ROBOT SYSTEM SETUP 163 Figure A.5: Structure of the PSD Camera Prototype Figure A.6: Precision Levels for Measurement Planes at Different Distances A.4.3 PSD Camera The position sensitive detector (PSD), unlike CCD/CMOS image sensors which sense input light in image form, senses only the position of a light spot. The PSD camera based on this principle was developed in[94] to directly sense the two-dimensional positions of infrared markers attached on the robot end-effector. The structure of the designed PSD camera prototype is illustrated in Fig. A.5. This PSD camera features high precision sensing as described in Fig. A.6. For example, at the distance of.5m, the PSD camera can achieve.15mm precision level for a measurement area of 45 2 mm 2, which is much better compared to the traditional vision camera. Furthermore, this PSD camera achieves a much faster response time at the level of microseconds and can be sampled easily with a much higher sampling rate (e.g., up to 1kHz in this real-time system setup). These advantages make it very suitable for the real-time feedback control in the robot end-effector tracking application. The details of this camera development are reported in[94].

181 APPENDIX A. ROBOT SYSTEM SETUP 164 A.4.4 CompuGauge 3D The three-dimensional position measurement system, CompuGauge 3D, is utilized to measure the end-effector tool center point (TCP) position as the ground truth for performance evaluation. This measurement setup features a repeatability of.2mm, accuracy of.15mm, resolution of.1mm, measurement space of 1.5m 1.5m 1.5m, tracking rate of up to 5m/s, and the sampling frequency of up to 1Hz. The details of this setup can be found in its operation manual[28]. A.5 Robot Simulator & Experimentor A.5.1 Robot Simulator In order to utilize the robot system more efficiently and confidently, a Robot Simulator (see Fig. A.7a)[18] is developed to simulate proposed algorithms prior to implementing them on the actual FANUC robot. Based on the parameters obtained from system identification (See Appendix B), and also from FANUC Ltd., a basic M-16iB Robot Simulator is set up in MATLAB using Simulink model with SimMechanics Toolbox[84] and Simulink 3D Animation Toolbox [6] complemented by MATLAB script files. This Simulator is capable of simulating real robot dynamic behaviors, performing trajectory planning tasks, and providing virtual reality robot animation as well. Convention Consistency Note that conventions for the coordinate system and robot angle definitions used in this dissertation are different from the ones used by FANUC engineers. Thus, special efforts have been made to convert these conventions properly in the Simulator. To make conventions consistent in the research study, all the conventions of the signals outside the robot model are restricted to follow the standard conventions used in the MATLAB Robotic Toolbox[29]. These conventions were illustrated in Section Controller Design The basic controller used in the Robot Simulator is a decentralized PID feedback controller with pre-calculated feedforward torques as described in Chapter 7. The decentralized PID feedback controller is proportional control in position loop and proportional and integral control in velocity loop for each joint. The feedforward torques are calculated off-line using the simplified multi-joint robot model (2.25) in Section with nominal parameter values. This calculation can be done efficiently using recursive Newton-Euler method such as the "rne" function in the MATLAB Robotic Toolbox. This controller structure can be further customized to add on more high level and/or more sophisticated controllers/estimators.

182 APPENDIX A. ROBOT SYSTEM SETUP 165 (a) M-16iB Robot Simulator (b) M-16iB Robot Experimentor Figure A.7: M-16iB Robot Simulator & Experimentor

183 APPENDIX A. ROBOT SYSTEM SETUP 166 Model Validation Figure A.8 shows a testing trajectory profile of each joint for comparison between the simulation and the experiment using the same controller. The corresponding torque profile comparisons are illustrated in Fig. A.9. Note that, the first 1.5sec of the trajectory is set as the robot setup initialization period and thus is not shown in the figure. The last 1sec period exhibits some mismatches in the torque due to the static friction effects. Other than this, the Simulator results closely match with those measured in actual experiments, especially during the dynamic periods. Thus, the Robot Simulator model using SimMechanics Toolbox can be regarded as an accurate representation of the actual FANUC robot. 3D Animation The Simulink 3D Animation toolbox further enables the Simulator with the virtual reality animation feature as shown in Fig. A.1 and Fig. A.11. This is done by building the CAD model in Unigraphics Nx7[65] and then exporting the VRML file[88] for the use in the MATLAB Simulink 3D Animation model. More details of this Simulator virtual reality design can be found in[36]. Trajectory Generation The Simulator has been designed with some basic trajectory generation functions. By specifying some key parameters (e.g., the positions and postures of the desired via points for the tool center point, and the velocity and acceleration limits in the Cartesian space), the trajectory generator is able to generate the joint space trajectory as the 4-th order smooth time optimal trajectory suggested in[55]. Some basic inverse kinematic functions are incorporated in this trajectory generator. A.5.2 Robot Experimentor Based on the Simulator structure (Fig. A.7a), a Robot Experimentor[17] (Fig. A.7b) is designed by replacing the robot simulation model with the real robot driver and the real measurement acquisition block. Some modifications are made in the supplementary script files to communicate with the xpc Target system for real-time implementation. This Experimentor design allows algorithms simulated on the Simulator to be easily performed on the actual FANUC robot, which can expedite the research process.

184 APPENDIX A. ROBOT SYSTEM SETUP Joint 1 Position Reference Experiment 1.75 Simulation Joint 2 Position Position (rad).1.2 Position (rad) Time (sec) Joint 3 Position Time (sec) Joint 4 Position Position (rad).5 Position (rad) Time (sec) Joint 5 Position Time (sec) Joint 6 Position Position (rad) Position (rad) Time (sec) Time (sec) Figure A.8: Testing Position Trajectory Comparison between Simulation and Experiment (Load Side Scale)

185 APPENDIX A. ROBOT SYSTEM SETUP Joint 1 Motor Torque 3 Joint 2 Motor Torque Torque (Nm) Torque (Nm) Time (sec) Joint 3 Motor Torque Torque (Nm) 2 1 Experiment 1 Feedforward Torque Simulation Time (sec) Torque (Nm) Joint 4 Motor Torque Time (sec) Joint 5 Motor Torque Time (sec) Joint 6 Motor Torque Torque (Nm) Torque (Nm) Time (sec) Time (sec) Figure A.9: Motor Torque Comparison between Simulation and Experiment

186 APPENDIX A. ROBOT SYSTEM SETUP 169 Simulator Virtual Reality Actual Experiment Environment Figure A.1: Virtual Reality in Simulator vs. Actual Experiment (FANUC M-16iB)

187 APPENDIX A. ROBOT SYSTEM SETUP 17 Simulator Virtual Reality Actual Experiment Environment Figure A.11: Virtual Reality in Simulator vs. Actual Experiment (Single-Joint Setup)

188 171 Appendix B System Identification 1 B.1 Linear Dynamics Identification of Multi-Joint Robot Extensive system identification has been conducted for the single-joint indirect drive robot (Fig. 2.3) as well as the multi-joint indirect drive robot (i.e., FANUC M-16iB robot shown in Fig. 2.5). The linear two-mass model (2.13) was utilized to identify each joint s linear dynamics. Key parameters (e.g., motor/load side inertia, joint stiffness, joint damping) in this linear model were identified for each joint at their corresponding maximum-inertia posture. The identification process was done through sine sweep and sine-by-sine excitation experiments. The optimization based transfer function fitting was performed to identify the system model. Furthermore, the multi-input-multi-output (MIMO) system identification considering the system coupling dynamics was also investigated. The identification approaches, experimental setup, data processing procedures, and results of this linear dynamics identification are extensively documented in[43, 44, 42] and will not be detailed here. However, as a glance of the identification procedure, some of the key points in the linear system identification for the multi-joint robot are briefly summarized as follows. B.1.1 Posture Selection To ensure that the two-inertia model (2.13) is valid, the robot joint motions must be decoupled from each other during the system identification process. As a result, the robot postures for the system identification process are selected to produce the largest inertia with respect to the moving joint while trying to reduce coupling motions among the joints. The robot postures for the identification experiment of each joint are visually shown in Fig. B.1. 1 The extensive experiments for the linear dynamics identification study (Section B.1) were performed as the work by a group, which includes the author, Ms. Cheng-Huei Han, Mr. Michael Chan, and Mr. Pedro Reynoso Mora, while the friction identification study in Section B.2 was conducted mainly by the author.

189 APPENDIX B. SYSTEM IDENTIFICATION 172 J 1 J 4 J 2 J 5 J 3 J 6 Figure B.1: FANUC M-16iB Robot Postures for System Identification