UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Integration of multirate heteroceptive sensor data in robotic system servo-loops Langlois, David 2002

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata

Download

Media
831-ubc_2002-0144.pdf [ 16.44MB ]
Metadata
JSON: 831-1.0099625.json
JSON-LD: 831-1.0099625-ld.json
RDF/XML (Pretty): 831-1.0099625-rdf.xml
RDF/JSON: 831-1.0099625-rdf.json
Turtle: 831-1.0099625-turtle.txt
N-Triples: 831-1.0099625-rdf-ntriples.txt
Original Record: 831-1.0099625-source.json
Full Text
831-1.0099625-fulltext.txt
Citation
831-1.0099625.ris

Full Text

Integration of Multirate Exteroceptive Sensor Data in Robotic System Servo-Loops by  David Langlois B.Sc.A., Universite Laval, Quebec City, 1999  A THESIS SUBMITTED IN PARTIAL F U L F I L M E N T OF THE REQUIREMENTS FOR THE D E G R E E OF  M A S T E R OF APPLIED SCIENCE in THE F A C U L T Y OF G R A D U A T E STUDIES (Department of Mechanical Engineering)  We accept this thesis as conforming to the required standard  THE UNIVERSITY OF BRITISH C O L U M B I A December 2001 ©David Langlois, 2001  In  presenting  degree freely  at  this  the  thesis  in  partial  fulfilment  of  University  of  British  Columbia,  I agree  available for  copying  of  department publication  this or of  reference  thesis by  this  for  his thesis  and study. scholarly  or for  her  Department The University of British Vancouver, Canada  DE-6 (2/88)  Columbia  purposes  gain  shall  requirements that  agree  may  representatives.  financial  permission.  I further  the  be  It not  is  that  the  permission  granted  allowed  an  advanced  Library shall  by  understood be  for  for  the that  without  make  it  extensive  head  of  my  copying  or  my  written  Abstract This work is concerned with the development of control policies in order to increase the robustness of robot control systems in the presence of sensor fault or failure. The policies are based on the integration of low sampling rate Heteroceptive Sensor feedback data (i.e., sensor data providing contextual and environmental feedback) in the robot system position servo-lbop where Proprioceptive  Sensor data (i.e., sensor data providing feedback about the internal state of  the robotic device) is already used to close the feedback loop. Increased robustness is achieved through the dynamic reconfiguration of the sensing subsystem on fault occurrence.  The smooth transition between the different sensing subsystem configuration is performed using Heuristic-based  Geometric Redundant Fusion (HGRF), which generates a fused feedback signal  based on the relative uncertainty of each feedback signal with respect to the overall sensing uncertainty. The uncertainty of each feedback signal is quantified though the use of an Uncertainty  Quantification  Metric,  which quantifies the signal uncertainty in terms its  discrepancy with the deterministic set-point signal of the robotic device.  The crux of the policies lies in defining a coherent and uniform way to optimize the use of the low-rate heteroceptive signal in the high-rate servo-loop. Two policies are proposed in order to compensate for the low sampling rate of the heteroceptive sensor and generate refereed feedback values for all controller signal update instant. The first policy uses Kalman filter-based predictors in order to both time-correlate the slow heteroceptive data, and propagate its information through the following controller update instants. The second policy uses an open-loop polynomial prediction scheme in order to generate heteroceptive measurements for all controller update: instants, without time-correlating the lagging heteroceptive signal.  Stability analysis, uncertainty quantification metric performance evaluation and simulation results are presented for both policies. Experimental results are provided for the polynomial policy, based on the implementation of the multirate control policy on a Cartesian manipulator.  ii  Table of Contents Abstract  ......ii  Table of Contents  Hi  List of Tables  viii  List of Figures  x  Acknowledgements  x  '  v  i  Chapter 1: Introduction  1  1.1 Background  1  1.2 Industrial Motivation  :  4  1.3 Research Objectives  6  1.4 Thesis Outline  .1  Chapter 2: Literature Review  ..............  9  2.1 Preamble 2.2 Sensor Integration Architectures  ...10  2.2.1 Logical Sensors Approaches  10  2.2.2 Other Approaches  14  2.3 Proprioceptive Sensor Integration  16  2.4 Heteroceptive Sensor Integration  17  2.5 Summary  —20  Chapter 3: Methodology  22  3.1 Preamble  :  22  3.2 The Basics of Robotic Manipulator Servoing  23  3.3 Basic Ideas  28  3.4 Multisensor Fusion  29  3.4.1 Review  29  3.4.2 Heuristic Geometric Redundant Fusion Process  iii  ,  31  Table of Contents  iv  3.5 Uncertainty Quantification  33  3.5. J Basic Concepts....  33  3.5.2 Formulation  35  of the Uncertainty Metric,  3.5.3 Simulation Testing.  36  3.6 Summary  39  Chapter 4: Model-based Policy  41  4.1 Preamble  ..41  4.2 General Formulation  :  42  4.3 KalmanFilter  45  4.4 Recursive Kalman Filter Transfer Function  50  4.5 Summary  ......52  Chapter 5: Polynomial Approach  55  5.1 Preamble  55  5.2 General Formulation  56  5.3 Newton-Gregory Backward Difference Interpolation  58  5.4 Formulation as a Transfer Function  60  5.5 Summary  61  Chapter 6: Implementation  62  6.1 Preamble.  6  6.2 Experimental Platform  63  6.3 Open-Architecture Control Platform.  .........69 ......7V-  6.4 Encapsulated Logical Devices Implementation 6.4.1 The ELD Object 6.4.2 Implementation of the ELD Architecture  2  J....71' ,  73;  6.5 X-Axis Dynamics Identification  76  6.6 Summary  82  Table of Contents  v  Chapter 7: Stability & Dynamic Simulations  .84  7.1 Preamble  .84  7.2 Model-based Policy  ;...85.  7.2.1 Development of the Closed-Loop Characteristic Equation,.  1.........'8.5.  7.2.2 Stability Analysis  92  7.2.3 Construction of the Dynamic Simulation  100  7.2.4 Simulation Results  103  7.2.5 Summary;  H5  7.3 Polynomial Policy  116  7.3.1 Construction of the Closed-Loop Characteristic Equation  116  7.3.2 Stability Analysis  U8  7.3.3 Construction of the Dynamic Simulation  125  7.3.4 Simulation Results  126  7.3.5 Summary  136  7.4 Summary  137  Chapter 8: Experimental Results  140  8.1 Preamble  ......  8.2 Experimental Parameters  141  8.2.1 Fault Tolerance, High Servo-Loop Sampling Rate 8.2.2 Fault Tolerance, Low Servo-Loop Sampling Rate 8.3 Experimental Results 8.3.1 Fault Tolerance, High Servo-Loop Sampling Rate 8.3.2 Fault Tolerance, Low Servo-Loop Sampling Rate 8.4 Discussion  .140 •  :  .„  ...141 ...146 147 147 ...J 56 161  Chapter 9: Conclusions & Recommendations 9.1 Conclusions  164 164  9.2 Recommendations  ...169  9.2.7 Uncertainty Quantification Metric  169  9.2.2 Development  170  of the Control Policy  Table of Contents  9.2.3 Implementation  vi & Experimental Testing  .  171  Bibliography  173  Appendix A : X Y Table Circuits  180  A . l Preamble  180  A.2 X-Axis Linear Push-Pull DC Amplifier  ...180  A.3 Unipolar Stepper Driver  183  A.4 Dual Limit Switches Driver  185  A. 5 Optical Encoder Switching Circuit  .187  Appendix B: Low-level ELD's Code  ......189  B. l Preamble  ;  .....  ;  B.2 E L D Functions  189 190  B.2.1 ELD Basic Parsing Functions_  „.•  190  B.2.2 ELD Structures  202  B.2.3 ELD Trajectory Generation Functions  204  B.3 Fusion Functions  218  B.3.1 Unidimensional HGRFfor  two Sensors  218  B.3.2 Uncertainty Quantification  222  B.3.3 Newton-Gregory  224  Interpolator  Appendix C : Miscellaneous O R T S Processes  C l Preamble  230  ...  230  C.2 F3 Indy DSP Board Digital I/O Processes C. 3 Miscellaneous Processes  Appendix D: O R T S Script Files  D. l Preamble D.2 E L D Implementation Script File D.3 Axis Dynamics Identification Script  230 ,  238  .243  243 ..243 246  Table of Contents  vii  D.4 Low-level Heteroceptive Feedback Integration Script  247  D. 5 Axes Controller Loops Scripts  249  D.5.1 X-Axis Controller Script D.5.2 Y-Axis Controller Script_  Appendix E : Kalman Filter Derivations for a Full State-Space Model  „  .249 ......250  ...253  E. l Preamble  253  E. 2 Derivations  254  Appendix F : Discrete-Time PID Derivations  256  F. l Preamble  256  F.2 Derivations  257  Appendix G : Low Servo-Loop Sampling Rate Fusion Gains Plots  260  G . l Preamble  260  G.2 Fusion Gain Plots for Low Servo-Loop Sampling Rate  261  List of Tables Table 3.1. Uncertainty Sources in Autonomous Control Table 5.1. Difference Table for Second Order Interpolation  33 __  59  :  Table 6.1. Summary of X-Axis Drive Parameters  .  Table 6.2. Summary of the Y-Axis Drive Parameters  65  :  .  66  Table 6.3. Summary of Camera and Lens Parameters  68  Table 6.4. MFIO-3B I/O Features  .70  Table 7.1. Servo-Loop Poles and Zeros Position in the Complex Plane  ...........93  Table 7.2. Major-Time Step Stability Analysis Sensing Subsystem Configurations.;. .j....;...; ;;; 95 :  Table 7.3. Minor Time-Step Stability Analysis Sensing Subsystem Configurations  :  :  .98  Table 7.4. Basic Uncertainty Levels for Model-Based Simulations Sensors  102  Table 7.5. Model-Based Simulations Parameters for Table 7.4 Uncertainty Levels  103  Table 7.6. Model-Based Simulations Parameters for Case #1 Controller Gains  108  Table 7.7. Model-Based Simulations Parameters for Case #1 Gains and Kalman Filters  111  Table 7.8. Sensing Subsystem Configurations for a Fault Tolerance Policy Implementation  122  Table 7.9. Heteroceptive Sampling Rates for Fault Rejection Implementation of the Policy  123  Table 7.10. Polynomial Policy Simulation Parameters for Fault Tolerance Implementation  127  Table 7.11. Simulation Parameters for the Fault Rejection Case of the Polynomial Policy  ..130  Table 8.1. Simulation and Experimental Controller Gains for a Fault Tolerance Case  142  Table 8.2. Basic Uncertainty Levels for the Experimental Manipulator Sensors  143  Table 8.3. High Servo-Loop Rate Experiments Parameters  145  Table 8.4. Reduced Servo-Loop Sampling Rate Experiments Parameters Table A . l . D C Amplifier Specifications  ...147 :.,  Table A.2. D C Amplifier Parts List  182 ...182  Table A.3. Stepper Motor Driver Specs  184  Table A.4. Stepper Motor Driver Parts List  185.  Table A.5. Limit Switches Driver Specifications.  185  Table A.6. Limit Switches Driver Parts List  186  Table A.7. Optical Encoder Switching Circuit Specifications  187  viii  List of Tables Table A.8. Optical Encoder Switching Circuit Parts List Table G. 1. Reduced Servo-Loop sampling Rate Experiment Parameters  List of Figures Figure 2.1. The Basic Logical Sensor with Control Object [33]  12  Figure 2.2. Logical Sensor/Actuator Object [16]  13  Figure 2.3. Visual Tracking Scheme Using Low-level Heteroceptive Sensor Integration (adapted from [61])  .........18  ;  Figure 2.4. Extended Robot Servo-Loop using Heteroceptive Measurement Loop (adapted from [30])  19  Figure 2.5. Indirect Visual Servoing Architecture (adapted from [44])  .......20  Figure 3.1. Basic Robotic System Control Block Diagram  ......u ...23  Figure 3.2. Basic Robotic System with Feedforward Trajectory Generation  .24  Figure 3.3. Teaching Tool Locations in a Typical Workcell  25  Figure 3.4. Pick-and-Place Operation using Heteroceptive Sensing for Target Generation  .26  Figure 3.5. Robotic Control Scheme with Heteroceptive Sensing for Target Generation  27  Figure 3.6. Robotic Control Scheme with Heteroceptive Sensing for Targets and End-Effector Tracking  27  Figure 3.7. Formulation of the Problem as a Feedback Control Problem  29  Figure 3.8. Uncertainty Quantification Metric Evaluation Block Diagram  37  Figure 3.9. Set-point and Output of Simulated System under Sensor Fault  38  Figure 3.10. Uncertainty Levels for Various Moving Window Sizes  38  Figure 4.1. Time-Correlation of the Heteroceptive Feedback Signal using Kalman Prediction .43 Figure 4.2. Model-Based Policy for Multirate Heteroceptive Sensor Integration  44  Figure 4.3. Recursive Kalman Filter Block Diagram [18]  46  Figure 4.4. Heteroceptive Sensor and Set-Point Signals Availability  47  Figure 4.5. Kalman Filter-based Predictor Block  51  Figure 5.1. Polynomial Approach block Diagram  57  Figure 5.2. Newton-Gregory Interpolator Block Definition  ;  60  Figure 6.1. Experimental Cartesian Manipulator  63  Figure 6.2. Camera Location in the Workcell  67  Figure 6.3. X-Axis Linear Push-Pull Amplifier Characteristic  68  List of Figures  xi  Figure 6.4. ORTS-PC7NT Scripting Environment  ,  70  Figure 6.5. The Encapsulated Logical Device (ELD) Object (adapted from [21])  ...72  Figure 6.6. Repartition of Processing Tasks for the X Y Table Implementation (adapted from [21])  ;  74  Figure 6.7. X Y Table Implementation of the E L D Architecture (adapted from [21]).  76  Figure 6.8. D C Motor System Block Diagram  77  Figure 6.9. Simplified Drive Dynamics Block Diagram  78  Figure 6.10. Excitation Waveform for Dynamics Identification  80  Figure 6.11. X-Axis Dynamics Identification Position Measurement  81  Figure 6.12. Simulated and Calculated from the First Position Derivative Velocities  82  Figure 7.1. Major Time-Step Equivalent Block Diagram  _  86  Figure 7.2. Minor Time-Step Equivalent Block Diagram  90  Figure 7.3. Basic System Position Servo-Loop Block Diagram  92  Figure 7.4. Robotic Manipulator Servo-Loop Poles and Zeros...  94  Figure 7.5. Robotic Manipulator Servo-Loop Bode Plot  94  Figure 7.6. Major Time-Step Pole-Zero Maps for Different Fusion Gains  96  Figure 7.7. Minor Time-Step Pole-Zero Maps for Various System Configurations  ...99  Figure 7.8. Model-Based Policy Simulations Set-Point Signal  102  Figure 7.9. Encoder Feedback Signal Under Hardware Fault Occurrence  103  Figure 7.10. Simulated Position Response and Fused Feedback Measurement for Case #1.;...J.105 Figure 7.11. Sensors Feedback for Simulation Case #1  105  Figure 7.12. System Responses for Simulation Cases #2 and #3  107  Figure 7.13. System Response and Fused Feedback Signal for Simulation Case #4  108  Figure 7.14. System Response and Fused Feedback Signal for Simulation Case #5  109  Figure 7.15. System Response and Fused Feedback Signal for Simulation Case #6  109  Figure 7.16. Sensor Uncertainty for Simulation Case #1  ...111  Figure 7.17. System Response and Fused Feedback Signal for Simulation Case #7  113  Figure 7.18. System Response and Fused Feedback Signal for Simulation Case #8  114  Figure 7.19. System Response and Fused Feedback Signal for Simulation Case #9  114  Figure 7.20. Polynomial Policy Equivalent Block Diagram  116  Figure 7.21. Simplified Closed-Loop Block Diagram for the Polynomial Policy  117  List of Figures  xii  Figure 7.22. Poles and Zeros Plot for n=80 and High Controller Gains  120  Figure 7.23. Closed-Loop Poles and Zeros for n=80, G|=0, and Low Controller Gains  121  Figure 7.24. Poles and Zeros Plots for Different Sensing Subsystem Configurations  122  Figure 7.25. Closed-Loop Poles for Different Heteroceptive Sampling Rates  124  Figure 7.26. Simulated Robotic Trajectory.  126  Figure 7.27. System Response Under Sensor Fault Condition for High Controller Gains...  128  Figure 7.28. System Response Under Sensor Fault Condition for Low Controller Gains  128  Figure 7.29. Evolution of the Fusion Gains for the Simulation Case #2  . . ..  ...130  Figure 7.30. System Response Under Sensor Fault Condition for n=3  ...131  Figure 7.31. System Response Under Sensor Fault Condition for n=4  .........132  Figure 7.32. System Response Under Sensor Fault Condition for n=5 and n=6.  ........132  Figure 7.33. Evolution of the Fusion Gains for the Simulation Case #3 Figure 7.34. Simulation Results for a Fault Tolerance Policy Failure Case  134 ...135  Figure 7.35. Fusion Gains Evolution for a Fault Tolerance Failure Case  136  Figure 8.1. Repetitive Ramp Signal Used as Experimental Set-Point  144  Figure 8.2. System Response for High (Case #1) and Low (Case #2) Controller Gains  148  Figure 8.3. Evolution of the Fusion Gains for High Controller Gains, Case#l (No Fault)  .....149  Figure 8.4. Evolution of the Fusion Gains for Low Controller Gains, Case #2 (No Fault)  150  Figure 8.5. System Response for Fault Occurrence Before Motion Starts, Case #3  150  Figure 8.6. Evolution of the Fusion Gains for Fault Occurrence Before Motion Starts  151  Figure 8.7. System Response for Fault Occurrence After Motion Starts, Case #4  152  Figure 8.8. Fusion Gains Evolution for Fault Occurrence After Motion Starts, Case #4;  153  Figure 8.9. System Response for Fault Occurrence in the Cyclic Motion Range, Case #5  154  Figure 8.10. Evolution of the Fusion Gains for Fault Occurrence in the Cyclic Motion Range, Case #5  .156  Figure 8.11. System Response for Reduced Servo-Loop Sampling Rate (No Fault), Case #6. .158 Figure 8.12. System Response for Fault Occurrence Before Motion Starts, Case #7 Figure 8.13. System Response for Fault Occurrence After Motion Starts, Case #8....  ...159 i..;.160  Figure 8.14. System Response for Fault Occurrence Within the Cyclic Motion Range, Case #9 ....... 161 Figure 9.1. Control System Using Fusion Process for Controller Gains Programming  171  List of Figures  xiii  Figure A . l . D C Amplifier Input/Output Characteristic Figure A.2. D C Amplifier Circuit Schematic  181 _  Figure A.3. Modified Stepper Driver Circuit Figure A.4. Dual Limit Switches Driver Circuit  183 ,  .184 186  Figure A.5. Optical Encoder Switching Circuit Schematic  „:.188  Figure F . l . Continuous-Time Domain P1D Controller Block  ...257  Figure G . L Evolution of the Fusion Gains for Reduced Servo-Loop Sampling Rate, Case #6.261 Figure G.2. Evolution of the Fusion Gains for Fault Occurrence Before Motion Starts, Case #7 262 Figure G.3. Evolution of the Fusion Gains for Fault Occurrence After Motion Starts, Case #8 262 Figure G.4. Evolution of the Fusion Gains for Fault Occurrence within the Cyclic Motion Range, Case #9  263  Acknowledgements I would first like to acknowledge the patience and support of my supervisor, Dr Elizabeth A . Croft. Her constant correction of my poor English grammar and syntax, as well as allowing me to transform her lab into a nice work environment are much appreciated. I would also like to acknowledge the financial support of the Natural Science and Engineering Research Council (NSERC), the British Columbia Advanced System Institute (ASI), and the University of British Columbia Top-Up Program.  This thesis would have never been completed without the constant support, and always constructive criticism, of my coworkers of the Industrial Automation Laboratory; Sonja Macfarlane for enduring my constant complaints and having lunch with me every working day for two years, Jason Elliott for spending so many nights in the lab with me and his commitment to the X Y table project, Damien Clapa for sailing with me and his intense passion for learning, Daniela Constantinescu for her always judicious advice, and, finally, the last but not the least, Greg Forrest for his patience debugging electronics and sharing so many thoughts about life with me.  Special thanks go to Dr Yusuf Altintas and the Manufacturing Automation Laboratory crew, for sharing far more than their hardware with me. Very special thanks to Kaan Erkorkmaz for his friendship and his infinite patience while teaching me controls. I also wish to acknowledge the support of Gordon Wright & Glenn Jolly, for finally teaching me electronics correctly, and Doug Yuen, for sharing his manufacturing and design knowledge with me.  I would also like to thank my parents, Michel & Claire, for their unconditional support and constant worrying. Not to forget, Blaise-Alexandre Malette & Gaston Fillion for helping me to keep it real and the most incredible ski trip ever put together.  Finally, I wish to thank the Whistler-Blackcomb lifties & trail groomers, the direction & personnel of the Dentry's Irish Grill, and Thomas Brakel, for making it all worth it. xiv  "The variability of the world makes it impractical to develop very detailed plans of actions before execution since the world might change before execution begins and thus invalidate the plan. " - Budenske and Gini  O Fortuna velut /una statu variabilis, semper crescis out deerescis; vita detestabilis nunc obdurat et tunc curat ludo mentis aciem, egestatem, potestatem dissolvit ut glaciem.  xv  Chapter 1 Introduction 1.1 Background For many centuries, humans have been concerned with the study and design of machines able to assist or replace people in performing both physical and intellectual tasks. This field of study is now  known under the general term of Robotics [68]. Industry has rapidly realized that many  advantages could be gained from using such mechanical devices in order to perform production related tasks. The use of robotic devices in industry became so widespread in the last few years that the original field of study has been subdivided in order to better fit the needs of the market. Industrial Robotics is the field of study concerned with the design, control and application of robots in industrial environment. The popularity of robotic devices in industrial environment can be easily explained by the many advantages that are gained from their use. Reduction of manufacturing costs, increase of productivity, improvement of product quality, improvement of manufacturing repeatability, and, finally, elimination of possibly harmful or alienating tasks for operators are all factors that justify the use of robotic devices on the shop floor.  Another field of study is concerned with the integration of robotic devices in industrial production environment, combined with the intelligent processing of infonnation related to the 1  1.1 Background  2  process status: Automation. Automation can be seen as the integration of industrial technologies, typical to manufacturing processes, with computer technology for information management.  It is generally accepted that three levels, or types, of automation can be found in the current industrial environment: Rigid Automation, Programmable Automation,  and Flexible  Automation.  Rigid Automation is concerned with the mass production of identical, or near identical products. The quantity of parts produced and the quality standards generally required in such applications justify, from both engineering and financial point of views, the use of dedicated machines to perform the operational sequence. On the other hand, Programmable Automation deals with the production of medium batches of products of a family of types. The sequence of operations necessary to manufacture the product from raw materials is modified to accommodate the requirements of the product range. The machines, processes, and products in this type of automation are versatile and are said to have a common "group technology".  Programmable  Automation represents the most typical automation technology used in the current consumer product manufacturing. Finally, Flexible Automation represents the next level of Programmable Automation. This automation level enables the production of variable size batches of different products by minimizing the set-up time necessary to reconfigure the sequence of operations and machines. This type of automation, sometimes referred to as Advanced Automation [70], is the most complex and requires a high-level of integration of computer technology with existing industrial technologies.  Many benefits can be gained from the use of robotic manipulators at all levels of automation. In particular, the benefits coming from the use of robotic devices in Programmable Automation  and Flexible  systems are summarized as: high versatility of the manipulator, high ratio of  workspace to manipulator size, and, finally, high level of programmability.  Traditional industrial robotic workcell design has been performed based on the assumption that each workcell would be used for a dedicated task, i.e., the vast majority of the robots used in industry are implemented under a Rigid Automation system, or in the best case, a Programmable Automation system [35]. That is, the devices in the workcell are pre-programmed to perform very specific and repetitive tasks. Under this design paradigm, the workcell requires  1.1 Background  3  reprogramming, and even redesign, for each new task or variations of the original task. More recent trends in workcell design involve a flexible approach that facilitates reconfiguration of the workcell when the task to be performed is changed or modified. Thus, recent designs are continually moving closer to the Flexible  Automation  system concept. The change in the  automation workcell design paradigms are driven by the demand for automation system platforms that reduce the production costs of small-batch/high-cost products, more frequently encountered in today's production [67].  Over time, many different approaches have been developed to reduce production costs of small batches through the use of robotic devices. With the development of such systems, and with the increasing level of complexity of task automation, it has become apparent that a higher level autonomy in the robotic device controller is necessary in order to move further toward fully Flexible Automation [35]. Achieving a higher level of autonomy in automation systems, as well as higher adaptability and flexibility is generally attained through various means [67], including: (1) - Off-line robot programming and virtual environment simulation (2) - Automated task planning and programming (similar to C N C pre-processing) (3) - Reduction of the environmental constraints in the workcell through the addition of an integrated set of sensor subsystem (i.e., sensor integration).  These three ways of increasing the flexibility of industrial workcells optimize the use of the workcell through a reduction of the workcell set-up time. Particularly, the first two approaches have been intensively used in the last years. For CNC machines, where the environment is static and the investment justified, automated task planning and some virtual process simulators are currently available under the form of commercial packages. However, for robotic workcells, the first two approaches present severe drawbacks related to the requirement of building a complete and exact model of the environment inside the workcell and correlating this model with the real environment [14].  .'  The limitations associated to these first two ways of increasing flexibility and adaptability of automation systems are inherently related to the lack of environmental sensory capabilities found in a typical Programmable  Automation workcell [35]. This lack of environmental sensing is  1.2 Industrial Motivation  4  addressed by the third proposed way to increase versatility, namely, sensor integration. It has long been recognized that the integration of environmental sensing, also known as heteroceptive sensing [68], with robotic manipulators would greatly increase the application domain of robots by enabling the automation of processes where the work environment and the object placement are neither accurately controlled or known [35]. Unlike typical manufacturing processes performed on CNC-controlled machine tools, these applications require the machine to operate in an uncertain environment which can not, or should not, due to economic or engineering constraints, be adapted to the rigid environment of the traditional CNC-machine tool. Therefore, in order to perform automated tasks in such an environment, it is necessary to sense the environment, integrate the environmental data with the system status, and generate a decision regarding the best way to achieve the prescribed goal. This type of process shows a level of complexity higher than what has been established as Robotics definition in the earliest days of the field. A more contemporary definition of the field is provided by Sciavicco [68], defining the Robotics field as the "science studying the intelligent connection of perception to action".  Heteroceptive sensing for control and high-level planning is well accepted in some fields (i.e., autonomous robotics, high-bandwidth navigation, 3D modeling, etc.), but has not been widely integrated in industrial applications. The use of such sensing, integrated in a standard industrial automation package, would provide many benefits, as underlined some 15 years ago in a workshop that initiated the field of sensor integration for manufacturing automation [31]. Since then, the field has matured and many tools have been developed to address problems that present increasingly higher levels of complexity as compared to early problems such as majority sensor voting or architecture development. The following section takes a closer look at the benefits of integrating heteroceptive sensing in industrial manufacturing and automation systems.  1.2 Industrial Motivation  ! u  ,  c  The use of heteroceptive sensing as a way to increase automation system autonomy arid flexibility has been studied under the generic name of sensor fusion and/or integration. A large number of definitions have been proposed for both concepts. A generally accepted definition is provided for each concept by Luo & Kay [47].  1.2 Industrial Motivation  5  Multisensor Integration : A concept referring to the synergistic use of the information provided by multiple sensory devices to assist in the accomplishment of a task by a system.  Multisensor Fusion : A concept referring to any stage in the integration process where there is an actual combination (or fusion) of different source of sensory information into one representational format.  From these definitions, it is clear that multisensor fusion is an actual subset of the more general, problem consisting in integrating multiple sensing sources. From an industrial standpoint, the core of the research lies in developing strategies to perform the coherent and efficient treatment of the information provided by multiple sensors, especially when the sensors are of various types and nature [31].  Research in the general area of multisensor integration has been motivated by the fact that i f one sensor can increase the capabilities of a system, the use of many sensors should increase it even further [47]. Over time, multisensor integration has been applied to numerous fields of work such as target tracking [11, 40, 51], automatic target recognition [33, 34, 66], fault-detection [17, 50, 78, 82], autonomous robotics [15, 16, 81], industrial robotics [44, 61], military applications [75, 80], etc.  In industry, it is necessary, in order to stay competitive, to maintain production costs at a minimum, while also maintaining a high level of quality and production rates, even when only small production series are considered [70]. Due to the fact that robot prices are continually dropping, such systems are no longer considered only for elimination of tedious, difficult or hazardous tasks currently done by humans, but are now considered for a wide variety of more complex tasks [30]. The increasing need for automation systems and/or robotic devices able to deal with non-traditional applications is driving the development of automation systems to fulfill this market niche. Particularly, as already established, the sensing abilities of robotic systems  1.3 Research Objectives  6  need to be further developed in order to increase the flexibility and autonomy of those systems [37, 35, 46, 77].  The increase of autonomy and flexibility of automation systems present numerous advantages, particularly when applied to robotic applications. Increased autonomy of automation systems enhances safety, reliability, productivity, and adaptability [I]. As argued earlier, increased autonomy can be attained through the use of multiple sensing sources, both heteroceptive and proprioceptive, [9, 35]. The advantages gained in using multiple sensors in an industrial automation system, such as a robotic workcell, are vast and some of them are outlined below. (1) - Many, less accurate, sensors may cost less than a few highly accurate sensors. (2) - Higher insensitivity of multiple sensors to sensor failure. (3) - Distributed sensing provide better world mapping in heavily constrained (populated) environments.  The use of multiple sensors in an industrially oriented system has two major goals [31]: to be able to perform more tasks and to perform tasks more reliably. Both of these goals are directly related to the fact that, in a multiple sensor system, it is possible to generate cooperation between different observation sources and, therefore, provide more information, or better information, that it would be available from a unique sensing source.  1.3 Research Objectives One of the challenges in flexible automation system design, as already introduced, involves the integration of heteroceptive sensing in existing systems. Furthermore, another challenge lies in defining control policies that optimize the use of the redundant information about the system state, as provided by the heteroceptive sensors. One particular way to optimize the use of the knowledge and data generated through these sensors consists in making it available to the lowlevel sensing and control systems already used in the application.  The information provided by the heteroceptive sensors has traditionally been used in high-level processes to generate target points or trajectories or, in more advanced applications, to directly  1.4 Thesis Outline  7  servo the position control loop [35, 61]. The heteroceptive data available in such an advanced automation workcell contains redundant and/or complementary information about the state of the system. The use of the knowledge extracted from this sensory data provides non-dedicated active sensing redundancy that can be used to increase the overall performance of the control system, as well as to increase the overall system robustness.  This thesis is concerned with the development of structured policies to integrate multirate heteroceptive sensing data directly into the low-level control loop in systems where heteroceptive sensing is already present and used as input for high-level processes (i.e., target point generation, manipulator calibration, etc.). The main objective of this work is to provide fault tolerance or fault accommodation capacities for robotic systems used to perform typical operations in a semi-constrained environment. In order to maintain system performance in the presence of a faulty sensor state, the corresponding policies must be shown to be sufficiently robust in order to overcome the fault with only minimal effects on the overall system performance.  Finally, the last objective of the work included in this thesis is to implement the proposed policy on a real system in order to demonstrate the increased performance.  1.4 Thesis Outline The first sections of this chapter have provided a review of the basic concepts related to industrial automation and robotics, and have introduced the relationship between sensor integration and the use of intelligent robotic devices in industiy. Also, in this chapter, the objectives of the work included in this thesis have been stated, in order to clearly situate the scope of the work presented here, and the context in which this work is relevant.  Chapter 2 presents a review of the relevant literature for the problem at hand and situates the current work with respect to previous work. Chapter 3 is concerned with the context in which this work is performed. Since the current work is to be integrated with already existing  1.4 Thesis Outline  8  automation and robotic systems, there is a need to clearly establish how robotic system control is currently done and how it could be improved through the integration of heteroceptive feedback.  Chapter 4 and Chapter 5 present two different policies based on the context established in Chapter 3. Particularly, Chapter 4 presents a model based approach to solve the problem of multirate heteroceptive sensing data integration in the low-level control loop while Chapter 5 presents a numerical approach to this problem.  Chapter 6 covers the implementation of the policies defined in the two previous chapters on an experimental set-up designed, manufactured, and implemented in the Industrial Automation Laboratory for testing purposes. In this chapter, the robotic system dynamics identification is also described and the results are presented.  Chapter 7 presents simulation results as well as stability analysis corresponding to the implementation of both policies on the system described in the previous chapter. This chapter also presents the basic assumptions underlying the simulation work for both proposed policies.  Chapter 8 presents the experimental results obtained from performing the same set of experiment on the physical set-up. The correlation between the simulation and the experimental results are established in this chapter.  Finally, Chapter 9 draws conclusions related to the results presented in the previous chapter and provide recommendations regarding the pursuit of this work, or closely related work.  Chapter 2 Literature Review 2.1 Preamble This chapter discusses relevant issues regarding the design of sensor integration architectures as well as the integration of proprioceptive and heteroceptive sensory data in industrial robotics and automation systems. Classical work related to the development of sensor integration architectures in a sensing and control context is examined first. The topic of sensor/actuator integration architecture design is outside the scope of this thesis but its presuppositions in the field make it fundamental to any implementation attempt. For sake of clarity, this topic is divided under two themes: Logical Sensors Approaches and Other  Approaches.  Next, the integration of proprioceptive sensor data and its applications are discussed. This type of sensor integration is the most prevalent and traditional approaches have been implemented in low-level control loops of a wide range of systems in order to increase the system robustness or to improve the level of performance of a given set of sensors.  Existing work regarding the integration of heteroceptive sensing with the robotics or automation control system is then examined. Since this particular type of sensor integration is now currently  2.2 Sensor Integration Architectures  1.0  used in many applications and under different types of implementation, most of the attention in this chapter is focused on this topic.  Finally, a summary of the major contributions to the field of sensor integration is provided. Furthermore, these contributions are compared with the current work in order to situate it within the rest of the field, as well as to clearly establish the contributions of this work to the field.  2.2 Sensor Integration Architectures 2.2.1 Logical Sensors Approaches One of the earliest problems arising in the field of sensor integration was concerned with the efficient and coherent treatment of information in systems where multiple sensing sources are present or where multiple hierarchical levels are necessary to perform complex tasks [31]. Fields of research like autonomous robotics, artificial intelligence, distributed control, teleoperation and manufacturing automation, to name a few, have looked at the question.  Initial attempts to integrate sensing in automation systems have been performed in an ad-hoc manner [4]. The need for an elaborated sensor integration paradigm or framework became obvious from the limited performance obtained from the development of task-oriented, or sensor-dedicated systems, that did not allow for much flexibility, adaptability or modularity [26, 41, 70]. The distinction between a sensor integration paradigm and a sensor integration framework may be semantic, but must be established since such distinctions generally lead to important differences in the system development.  Luo & Kay [47] describe a sensor integration paradigm as being the general concept or idea behind the development of a particular frameworks In a framework, the processing algorithms and procedures are specified in greater detail, allowing for a direct implementation. Since this work is mainly concerned with the implementation of a paradigm, and not the establishment of a new one, frameworks are therefore more interesting. In fact, in this section, most of the attention  2.2 Sensor Integration Architectures  11  is directed toward the variant frameworks developed from a common paradigm, the logical sensor.  Henderson [34] was the first to propose an integration paradigm and support it with a coherent framework for sensor integration. The Logical Sensor Specification  Language introduced a  means by which the user is insulated from the details of the input devices, generally physical sensors, while the Multisensor Kernel System (MKS) was designed to provide an efficient and uniform mechanism to deal with multisensor data [32, 34]. Both systems where designed to address two requirements for the configuration of multisensor systems. The major requirement was the development of a coherent and efficient infonnation management specification for systems where multiple sensors of various nature are used. The other requirement was the need for increased flexibility to allow for system reconfiguration, both as a means of increasing robustness towards sensor fault or failure, and to facilitate future modifications to the system (i.e., incorporation of new sensors, replacement of existing ones, etc.).  The original Logical  Sensor Specification  was mainly directed at data acquisition and  processing. Therefore, this paradigm did not account for the presence of control signals in the Logical  Sensor hierarchy. A later effort addressed that issue with the development of a  specification for distributed sensing and control [33]. The specification of control in Logical Sensors is build based on the original concept, in which, in addition to the stream of sensor data flowing up through the hierarchy of sensors, a stream of control commands flows downward in the same hierarchy. Nevertheless, the development of the control side of the logical objects did not reach the same level of development as the sensor side.  At about the same time, Ruokangas [67] developed the Automation Science Testbed (ASTB), a hierarchical control architecture for a robotic workcell based on a paradigm similar to the Logical Sensor Specification paradigm. The integration of vision, acoustic ranging, and forcetorque sensing within the robotic device controller was shown to provide failsafe capabilities for the system. Furthermore, the integration of different sensing modalities provided the basis for the implementation of variable control strategies based on a complete world model established with the various types of data available.  2.2 Sensor Integration Architectures  12  CHARACTERISTIC  CONTROL  OUTPUT VECTOR  COMMANDS  4  LOGICAL SENSOR NAME SELECTOR  CONTROL COMMAND INTERPRETER PROGRAM 1  PROGRAM n  LOGICAL S E N S O R  LOGICAL S E N S O R  C O M M A N D S TO  INPUTS  INPUTS  LOGICAL S E N S O R S  t - t I -I  Figure 2.1. The Basic Logical Sensor with Control Object [33] Allen [4] reorganized Henderson's concepts in an Object Oriented Programming  paradigm in  order to address sensors, and sensory related, tasks found in a robotic workstation. Under the object-oriented paradigm, generic logical sensors, having common sensing behavior but different physical implementation, can be reused only by redefining default parameters. Again, a hierarchical layout was adopted for the framework implementation. The highest levels of the architecture perform tasks related to more than one type of sensing object, while the lowest levels implement sensor, or actuator, specific processes.  Further work in the area has been presented under the concept of Logical Sensor/Actuator and the corresponding Sensor Explication paradigm [16]. Extending the concept of Logical Sensor to also include actuators, Budenske proposed an architecture based on a Logical  Sensor/Actuator  (LSA) reconfigurable object that contains domain knowledge and knowledge about the sensors and actuators available. In this architecture, the detailed control signals are generated from highlevel primitives, in a procedure called Sensor Explication. Implemented in autonomous robots, it is argued that the use of both an architecture of Logical Sensor/Actuator Explication  objects and Sensor  increase the robustness and the reliability of such a system, even in the presence of  noise, actuators and sensors faults and limited environmental knowledge.  2.2 Sensor Integration Architectures  13  CONTROL DETAILS IN  PRIMARY BEHAVIOR Procedural Knowledge on how to achieve the primary goal  RELEVANT INFORMATION OUT  SUFFICIENT DESCRIPTION Declarative knowledge on how to utilize and interact with this object  STRUCTURES AND MECHANISMS FOR S E N S O R  - i  EXPLICATION  DATAFLOW IN STRUCTURES AND MECHANISMS FOR INTERACTION  DATAFLOW OUT  WITH OTHER O B J E C T S  CONTROL DETAILS OUT  RELEVANT INFORMATION IN  Figure 2.2. Logical Sensor/Actuator Object [16] Another implementation of the Logical Sensor paradigm has been presented by Naish [53, 54]. The Extended Logical  Sensor Architecture  (ELSA) aims toward providing modularity and  scalability in industrial inspection and grading systems. The overall architecture is designed to be used in inspection and grading systems for non-uniform biological products (e.g., food products). A feature-based approach is adopted as guideline for the overall architectural organization. This effort focuses on data processing and does not include control functions necessary for application to robotic systems control.  A Logical Sensor implementation for robotic systems control has been presented by Zheng [83]. In this architecture, the use of logical sensors and their treatment as objects modules is aimed at making integration feasible. The integration of multisensor feedback into different hierarchical levels of the architecture enhances the capability and performance of the different manipulator activities without interfering with the original control mechanism of the robotic device. The enhancement of the capability and performance of the manipulator is clearly expected to lead to increased robot productivity. One drawback of the proposed architecture is related to the fact that the control and sensing are performed within independent  architectures, increasing the  communication bandwidth requirements between the parallel architectures.  2.2 Sensor Integration Architectures The Encapsulated  Logical Device Architecture  14 [19, 20, 21] addressed this last problem by  defining a Logical Sensor based framework in which both control and sensing are performed by the logical sensor objects. This particular implementation is oriented toward the development of industrial automation and robotic systems in an uniform and structured manner. A coherent and flexible framework to perform sensor data fusion and data uncertainty management is also provided. This framework has been selected as implementation architecture for the current work, due to its versatility and adaptability to various industrial context and tasks.  2.2.2 Other  Approaches  Another field of research that addresses the problems of multisensor integration is Artificial Intelligence (Al). The development of system architectures in which high-level reasoning and low-level real-time control can cohabit and benefit from the presence of each other has been studied by many A l researchers.  Brooks [12] was one of the first A l researchers to formulate a paradigm to integrate multisensor feedback and high-level reasoning in order to manage and control an autonomous robotic device. This architecture is based on a layered arrangement of functional behaviors, where increasingly complex behaviors are able to subsume less complex behaviors. While this framework is easily implemented at the low-level (i.e., sensor and actuator level), its implementation at higher-level (i.e., task level) rapidly becomes intractable and the hierarchical subsumption of one module by another generally fails [4].  A n alternative to Brooks subsumption approach is found in the Multi-Agent  paradigm. The  Multi-Agent paradigm has typically been applied to more conceptual applications than industrial automation systems, or industrial robotics, and has been generally shown to present severe drawbacks regarding the fulfillment of the real-time constraints found at the low-level of control systems. Nevertheless, some researchers have developed Multi-Agent  architectures providing  both the high-level reasoning capacities and the real-time low-level capacities to handle constraints found in robotic systems.  2.2 Sensor Integration Architectures Hayes-Roth [28] defined an Intelligent Agent Architecture  15  in which intelligent agents are  continuously perfonning three functions: perception of the dynamic environment, action to affect the environment, and reasoning to draw inferences and determine actions. The emphasis of this work is focused on the necessity for the agent to adapt its reasoning, perceptual strategies, and infonnation requirements to meet the dynamic goal-based constraints in which it is evolving. However, no particular framework or implementation based on this paradigm is provided.  Another Agents-based approach has been developed as a hybrid architecture in which complex plans are established by the high-level agents before being implemented in real-time by a subsystem of the architecture [52, 80]. This approach presents the advantage of guaranteeing the execution of the low-level tasks inside of the real-time constraint deadline, but still requires the design of two independent architectures to handle high-level reasoning and low-level control.  Agents-based architectures have been used for the control of autonomous robotic devices and have shown performance levels comparable to Brooks layered approach [4]. Nevertheless, the resulting architectures generally demonstrate a higher-level of complexity compared to what can be obtained using a hierarchical Logical Sensor based approach, while providing a lower-level of flexibility and manageability. In the current context, the examination of industrially-oriented implementations, where a hierarchical layout is most often used, indicates that the gains in flexibility and manageability justify the use of a Logical Sensor based approach. It is interesting to note at this point, as underlined in [4], that the use of the Logical Sensor approach is not restricted to the development of hierarchically controlled systems and can also be used in the development of both agent-based architectures or subsumption-based architectures.  Distributed  Control Systems is another area of research that includes the development of.  architectural arrangements required to facilitate efficient infonnation management  in a  multisensor system. The type of frameworks proposed are, in fact, similar in nature and function to what is required to perfonn multisensor integration. Albus introduced the Real-time  Control  System (RCS) [2] and the NASA/NBS Standard Reference Model ( N A S R E M ) [3] for intelligent  real-time control and telerobot control systems, respectively. Both architectures consist of a hierarchical arrangement of layers based on functional and timing constraints. This type of  2.3 Proprioceptive Sensor Integration  16  architectural design focuses on respecting the low-level timing constraints, while allowing the user to define specialized sensory processes. The major drawback related to such architectures is their lack of reconfigurabiliry. Each component is designed to occupy a particular location in the architecture and there is high interconnection between components, severely limiting the manageability and interchangeability of the components found in such a framework.  Finally, a more recent effort in the framework development area is provided by the Open Control Platform (OCP), the software technology component of the Software Enabled Control (SEC) research program [29, 62]. The OCP provides an open, object-oriented, and middleware-enabled software framework, as well as a development platform for controls technology simulation and experimental testing. The major components of the OCP software include the run-time framework and middleware, a simulation environment, and a tool integration module. The middleware is used to fuse embedded system application components together, managing their execution and communication. The simulation environment allows the embedded application to be tested in a complete simulation. Finally, the tool integration module provides linkages to useful design and analysis tools, such as MatLab and Simulink. The development of the overall software control system is oriented toward Uninhabited Aerial  Vehicles ( U A V ) embedded  system design and testing [62], although its completely open architecture  allows for  implementation in other type of applications, such as industrial robotics or automation.  2.3 Proprioceptive Sensor Integration The integration of multiple signals provided by proprioceptive sensors is the oldest form of multisensor integration. Increased reliability, accuracy, and fault tolerance are all factors that justify the use of multiple sensing sources to estimate the same variable in a given system. Optimal estimation and high-bandwidth navigation are two majors application examples of the use of proprioceptive multisensor integration.  Optimal estimation of position feedback signals through the use of Kalman filtering has been described by Erkorkmaz [23] for the case of machine tool feed drive control. Position and velocity are sensed on the system and both signals are integrated in the control system after  2.4 Heteroceptive Sensor Integration  17  being fused by a Kalman filter to improve the quality and accuracy of the position and velocity estimates, beyond the measured level of performance of both sensors.  High-bandwidth navigation also presents many interesting applications of proprioceptive multisensor integration. In [17], an integration scheme for missile guidance systems including fault detection and rejection is proposed. Through the integration of radar and infrared position feedback, the missile guidance performance and fault rejection capabilities are optimized. The use of different sensors, operating under different modalities, increases the system robustness since any given environmental disturbance typically will not affect both sensors. This approach is especially valid for military applications where environmental perturbations are purposefully generated by opposing elements in order to induce failures in each others systems.  Mirabadi [50] used multisensor integration to increase system reliability and accuracy for train navigation systems. Again, in this application, using multiple sensors to measure the same variable, where each sensor has its own accuracy, reliability, and limitations, and then merging the data together using data fusion in an integration scheme leads to improved sensing performance, generally at a lower cost. Rehbinder [65] and Brown [13] have also presented applications where multirate integration of data is used to increase the accuracy and eliminate drifts from high-bandwidth navigation measurements.  2.4 Heteroceptive Sensor Integration The use of heteroceptive multisensor integration in automation and robotic systems has generally been done at the high levels of the sensing and control architectures. Target location, obstacle avoidance, advanced trajectory generation are all examples of tasks that have been implemented using high-level multisensor feedback [4, 10, 14, 37, 49]. However, the work presented herein is mainly concerned with the integration of multirate heteroceptive sensor data in low-level systems, and particularly, position servo-loops.  The integration of heteroceptive multisensor feedback in low-level systems has generally been performed outside of the intelligent system development realm. Due to the nature of the problem  2.4 Heteroceptive Sensor Integration  18  at hand, this type of multisensor integration typically has been performed using a feedback control approach. The integration is performed by adding outer feedback loops to the original position feedback loop, c.f. Figure 2.3. Visual servoing of robotic manipulators is an example of such an application where this particular type of integration is commonly used in a control context.  OBJECT MOTION  T(k) DESIRED FEATURE POSITIONS  I COMPUTATION  +  •CK/-H  0FERR0R  |  VECTOR  V  e(k)  VISUAL CONTROLLER  T(K), R(k)  "o00  R'(k) v  TRANSFORMATION TO THE ENDEFFECTOR FRAME  ^ ™  /Ov  FC  ROBOT CONTROLLER  c< > k  +  CAMERA MOTION  u(k) v(k)  J  MEASURED FEATURE POSITIONS  COMPUTATIONAL DELAY  —  VISION SENSOR OPTICAL FLOW  Figure 2.3. Visual Tracking Scheme Using Low-level Heteroceptive Sensor Integration (adapted from [61]) Controlled Active Vision has been introduced by Papanikolopoulos [61] in order to optimize the tracking performance of a robotic manipulator by combining vision with traditional control theory. In this application, the sensing information is used at the low-level to derive the control signals, but is also used at the high-level to create models of the system and the environment. While most of the treatment presented in [61] addresses the visual flow processing and the control issues of such a system, this implementation still provides valuable insight into the possibilities of such a design. Furthermore, this approach is not limited to the use of visual feedback and can also be used for other types of sensors (i.e., position, velocity, force, etc.).  Weiss [77] also addressed the question of direct visual servoing of robotic manipulators as a way to compensate for inaccurate modeling of manipulator geometry. Traditional robotic systems utilize joint positions as control variables, leading to an open-loop control of the end-effector position. Therefore, slight modeling errors of the manipulator geometry will result in endeffector errors that can not be monitored or compensated by the control algorithm. Westmore  2.4 Heteroceptive Sensor Integration  19  [79] also presented a similar mechanism to adjust for positioning uncertainties encountered in an unstructured environment.  The major drawback of direct visual servoing is the need to redesign the robot controller in order to fully integrate the visual sensor data into the servo-loop at sufficiently high sampling frequencies (i.e., 200 Hz and more). In most of industrial robot cases, this is extremely expensive and difficult to achieve due to the proprietary nature of the robot control system. This problem is generally avoided through the use of indirect visual servoing. Approaches to perform indirect visual servoing have been introduced in [30] and [44], based on the Robot Sensor Interface (RSI), a semi-open architecture controller developed by K U K A Roboter GmbH [10, 43]. In this approach, again, a feedback loop is added to the basic position feedback signal. However, this time is used to correct the path generation, leaving the original control loop intact.  ROBOT  REAL ROBOT  MODEL  GEOMETRY  DESIRED  INVERSE  DRIVE  AXES  FORWARD  TRUE 3-D  3-D POSE  TRANSFORMATION  CONTROL  DRIVES  TRANSFORMATION  POSE  AXES POSITION MEASUREMENT  MEASUREMENT OF TRUE 3-D POSE  Figure 2.4. Extended Robot Servo-Loop using Heteroceptive Measurement Loop (adapted from [30]) The authors of these indirect approaches reported good performance in experimental testing. These last approaches are most likely to reach the industrial environment in the short term due to their relative simplicity and the fact that they make use of the control algorithm provided by the robot manufacturer. Nevertheless, these indirect approaches do not provide the same level of fault rejection capabilities as the direct approaches outlined earlier.  2.5 Summary  20  PATH MEMORY  PATH  FEEDFORWARD  MODIFICATION  CONTROLLER  ROBOT WITH FEEDBACK CONTROL  Ax., VISION SYSTEM  Figure 2.5. Indirect Visual Servoing Architecture (adapted from [44])  2.5 Summary This chapter covered previous work related to multisensor integration for industrial automation and robotic systems. The first section provided an overview of the architectural requirements for a systematic design approach to this problem. Many paradigms have been proposed to provide an efficient and coherent framework to perform multisensor fusion in a variety of applications. The most common paradigm used for architecture design is Henderson's Logical Sensors. A , framework based on this concept, the Encapsulated Logical Device (ELD) architecture, has been selected to develop multisensor industrial automation and robotics control systems related applications. This framework provides flexibility and adaptability in system development, while also providing a basic framework to quantify and manage uncertainty at all levels of the architecture.  A review of existing implementations and applications of proprioceptive multisensor feedback was provided. When multiple feedback sources are used to estimate the internal states of a system, it has been shown that it is possible to merge the knowledge and data represented by the multiple feedback signals to improve the quality and relevance of the system's state. Typically, this type of sensor integration has been performed through Kalman filter based optimal estimation in applications such machine tool drive control and high-bandwidth navigation.  Heteroceptive multisensor integration has been used both at the high-level and the low-level of, intelligent systems. Environmental information in high-level processes of intelligent systems is  2.5 Summary  21  used in order to generate world models and/or execution plans, mainly to achieve autonomous and intelligent behavior in unconstrained environment. Target location, obstacle avoidance, and world mapping are all high-level tasks that have been implemented utilizing heteroceptive feedback sources.  At the low-level, the environmental infonnation provided by the heteroceptive sensors is generally used to generate redundant feedback about the system's state. The rich sensory signal provided by most heteroceptive sensing sources generally contains not only information about the environment, but also about the system itself. Extracting the redundant information from the signal and integrating it directly into the system control loop can increase the robustness of the system and dynamically compensate for unmodeled change in environmental conditions.  :  Chapter 3 Methodology 3.1 Preamble In this chapter, a methodology to integrate multirate heteroceptive sensing capabilities into the low-level control loop of a robotic system is introduced. The methodology is based on a structured framework to process and manage information provided in a multisensor system and aims at increasing the system robustness in the presence of sensor fault or sensor failure.  This chapter starts with a review of traditional robotic control systems in order to establish the context in which this work is performed. The proposed policy is then formulated in general terms. A n overview of the Sensor Fusion concept, and common implementations, follows. The sensor integration framework described is based on a generally accepted robotic control system configuration and the basic requirements for industrial manipulators. Once the nature of the integration and its context have been established, the fusion process is then described in more detail. A brief review of geometric data fusion is also provided in order to clarify the advantages gained through its use in robotic system position servo-loops.  22  3.2 The Basics of Robotic Manipulator Servoing  23  Finally, a procedure to quantify, on-line, sensor uncertainty in deterministic feedback systems is established. This metric is necessaiy in order to optimize the performance of the fusion process, as well as to enable the dynamic reconfiguration of the sensing subsystem on fault or failure occurrence.  3.2 The Basics of Robotic Manipulator Servoing From their inception, the vast majority of robotic manipulator systems have been designed under the same control paradigm. Robotic manipulators are composed of a mechanical system (linkages, revolute arms, etc.), coupled to actuators that inject energy in the system, causing a system state transition. The system state transition is constrained by the mechanical system and its effect is measured through a sensor, or set of sensors. The actuators inject energy into the system following a set-point signal, expressed in the form of a control variable, generally defined and provided by the user. The control variable can take a range of forms, depending on the particular application and user requirements. Traditionally, speed (linear or revolute) and position have been used as control variables. Due. to the increased level of complexity of the tasks to be performed using robotic manipulators, various other control variables have recently been introduced. Force, stiffness, mechanical impedance, and acceleration are a few examples of control variables in modern robotic control systems.  C O N T R O L L E R  A C T U A T O R D Y N A M I C S  S Y S T E M  0(z)  D Y N A M I C S  S E N S O R D Y N A M I C S  Figure 3.1. Basic Robotic System Control Block Diagram Figure 3.1 represents a basic control system design for a robotic system where the set-point signal, U(z), is compared to the control variable, © ( z ) , as expressed by the sensor feedback. The result of this comparison is then fed into a control algorithm used to compensate for the dynamic characteristics of the coupled actuator and mechanical system. Robotic control is a  3.2 The Basics of Robotic Manipulator Servoing  24  servoing problem, therefore an input/output closed-loop transfer function as close to unity as possible, over the whole controllable bandwidth, is desirable. The control algorithm is therefore used to modify the system dynamics in order to suit the task requirements.  This approach represents the simplest way to servo a mechanical system. In order to facilitate the use of such systems, automatic set-point signal generation techniques, based on user provided reference points or way-points, have been developed. The Trajectory Generator block, Figure 3.2, enables the user to only specify the desired position, (Wi), without having to manually establish the intermediate positions, for all sampling instants, between the current position and the desired position. Performing a precise task using this approach is critically dependent on the , ;  accuracy of the given desired position, assuming sufficient performance from the controller algorithm.  TRAJECTORY GENERATOR  CONTROLLER  ACTUATOR  SYSTEM  DYNAMICS  DYNAMICS  0(z)  SENSOR DYNAMICS  Figure 3.2. Basic Robotic System with Feedforward Trajectory Generation The generation of reference points is based on a model of the robotic manipulator workcell environment and requires modeling of the interaction between the robot and the environment. A typical robotic task, based on user provided way-points, is a Pick-And-Place  (PAP) operation. To  successfully complete this type of operation, the robot needs to move to a particular point in the workspace, grab an object through the use of an end-effector tool, move to another workspace location and place the object in a particular location with a specific orientation. The accurate specification of the pick-up and placing points by the user, in the robot coordinate frame, is, critical to the success of the operation. This implies that the user is able to establish, with high accuracy and through a wide range of environmental operating conditions, the relation between the world frame (i.e., the coordinate frame in which the users can express their perception of the  3.2 The Basics of Robotic Manipulator Servoing  25  object position) and the robot base frame (i.e., the coordinate frame in which the end-effector position is generally expressed).  While not impossible to establish, the relation between the user reference frame, or world, and the robot end-effector frame is generally extremely complex to establish with a sufficient level of accuracy [14, 30, 35, 37]. In particular, errors in modeling the robot work environment, as well as its correlation to the manipulator reference frame, the imprecision of the mechanical system, and the variability of the kinematic chain of the manipulator are all factors that affect the accuracy of the overall coordinate transform. The problems related to off-line programming of high-precision pick-and-place operations have traditionally been avoided by teaching the points on the real system (i.e., the robot "world") instead of hard-coding them. This solution is timeconsuming for the operator, reduces the productivity of the workcell, and can become quite complex in a crammed workspace, for example see Figure 3.3. Furthermore, the reference points have to be periodically re-taught in order to compensate for factors such as wear in the manipulator mechanical system, environmental changes, retooling, task changes, etc.  Although the point-teaching solution to the off-line programming problem is quite simple to implement and currently intensively used in industry, more elegant solutions have been developed to overcome the drawbacks associated with this procedure.  Figure 3.3. Teaching Tool Locations in a Typical Workcell (Photo courtesy of the Manufacturing Processes Laboratory, Universite Laval, Quebec City)  3.2 The Basics of Robotic Manipulator Servoing  26  A more advanced approach to the problem of correlating the two world models, namely, the user model and the robot model, consists of using additional sensors to establish the position of the necessary target points. This way, the position of the target points can be continuously estimated to correct for variations, or measurement errors, and minimize the role of the user, who only has to specify the nature of the target. In particular, some heteroceptive sensors have the capacity to estimate the position of target points, and at the same time, the position of the robot's endeffector, minimizing the effects of the coordinate frame transformation uncertainties between the two models.  The first approach, using heteroceptive sensing to estimate the position of the target point, is illustrated by Figure 3.4 and Figure 3.5. The first one shows a pick-and-place type of operation in which the position of the object, as well as the position of the placement area, or target, are identified by the overhead camera and then transmitted to the trajectory generator. The trajectory generator then uses the given locations to generate a trajectory that will enable the effective pickup and placement of the object. Figure 3.5 represents the block diagram corresponding to this approach.  Figure 3.4. Pick-and-Place Operation using Heteroceptive Sensing for Target Generation In this type of automated operation, the heteroceptive sensor is only used once to initiate the operation. Therefore, in order for the feedforward trajectory generation process to create an accurate trajectory, that guarantee the completion of the task, an accurate model of the  3.2 The Basics of Robotic Manipulator Servoing  27  manipulator kinematics and a high-level of certainty for the camera to robot base-frame transformation are necessary.  HETEROCEPTIVE SENSOR  WJz)  TRAJECTORY GENERATOR  U(z) CONTROLLER  TARGET LOCATIONS  ACTUATOR  SYSTEM  DYNAMICS  DYNAMICS  SENSOR DYNAMICS  Figure 3.5. Robotic Control Scheme with Heteroceptive Sensing for Target Generation The other possible approach, as introduced earlier, includes the estimation of the location of the targets, as well as the position of the end-effector. This approach enables the on-line correction of the modeling errors since it is then possible to configure the control system to minimize the difference between the robot's end-effector position and the target positions, in the same reference frame. This last approach has been studied for some time [30, 35, 61, 77] and is represented by the block diagram of Figure 3.6.  HETEROCEPTIVE SENSOR  WJz)  TRAJECTORY GENERATOR  CONTROLLER  TARGET LOCATIONS  ACTUATOR  SYSTEM  DYNAMICS  DYNAMICS  SENSOR DYNAMICS  Figure 3.6. Robotic Control Scheme with Heteroceptive Sensing for Targets and End-Effector Tracking  3.3 Basic Ideas  28  This last approach to robotic system control is assumed to be a basic functional requirement of the work presented in this thesis. The nature of the heteroceptive sensing devices can vary with the application, as well as with the engineering constraints of the implementation. In order to provide a flexible methodology that can be applied to various types of heteroceptive sensors, it is also assumed that this type of sensing has a lower sampling rate than the control signal generation rate. This assumption is critical to the development of a realistic control policy since, in many cases, the rich signal provided by the heteroceptive sensor requires more processing than a simpler, low-level signal. The processing required for a heteroceptive sensor signal generally results in delays longer than the controller sampling period. The next section will cover in more detail this particular issue as it pertains to this work.  3.3 Basic Ideas As previously stated, this work aims at providing a coherent and efficient methodology to integrate heteroceptive sensing data in the low-level control loops to increase system tolerance and robustness toward sensor fault or failure. Based on the assumption that heteroceptive sensing is already available in the system, this work aims at optimizing the use of the data and knowledge provided by that sensing resource.  A general approach to this problem can be formulated based on the block diagram of Figure 3.6. Removing the high-level components present in that block diagram and reorganizing the feedback loops leads to Figure 3.7, which provides a feedback control formulation for the multirate heteroceptive sensing integration problem.  In Figure 3.7, a time delay has been added on the heteroceptive feedback loop to reflect the processing delay associated with the use of a higher-level sensor to generate environmental information. In the current work, the processing delay is assumed to consist in an integer number of controller sampling periods. The exact duration of the delay is dependent on the sensor nature, as well as the complexity of the processing necessary to extract relevant information from the • raw data stream. In order to improve tracking perfonnance under sensor fault condition, it is then necessary to generate a heteroceptive sensor signal for all the controller sampling instants. This  3.4 Multisensor Fusion  29  represents the first major issue related to the use of multirate heteroceptive sensing to increase tracking performance and provide sensor fault tolerance.  C O N T R O L L E R  S E N S O R F U S I O N  I  A C T U A T O R D Y N A M I C S  S Y S T E M D Y N A M I C S  S E N S O R D Y N A M I C S  H E T E R O C E P T I V E S E N S O R  Figure 3.7. Formulation of the Problem as a Feedback Control Problem The other major issue related to the integration of multirate heteroceptive sensor information directly in the manipulator servo loop is concerned with the nature of the process used to merge the knowledge and data coming from the two different sensors. It is desirable to use a process that supports the strong real-time constraint associated with the stability requirements of the servo-loop. This process should also enable the dynamic reconfiguration of the sensing subsystem in the presence of a sensor fault, or sensor failure, occurrence. Multisensor fusion is the tool that has been selected to merge the data stream together, due to its versatility and simplicity when used on low-level signals. The next section covers in more detail the issues related to the use of multisensor fusion in this context.  3.4 Multisensor Fusion 3.4.1 Review As already established, Multisensor Fusion is a concept referring to any stage in the integration process where an actual combination, or fusion, of different sources of sensory information into one representational format. Fusion can be performed on data presenting various levels of abstraction. Luo [47] identifies four different levels of data abstraction at which data fusion is  3.4 Multisensor Fusion  30  used. Signal, pixel, feature and symbolic levels are the architectural levels, or data representation fonnats, at which different modalities of multisensor fusion are performed. In particular, signallevel multisensor fusion refers to a type of fusion where signals coming from a group of sensors are combined together in order to generate a signal of the same form as the original signals, but of greater quality. The increase of quality of the resulting fused signal is generally quantified through a reduction of its variance, or the level of uncertainty associated with the signal. This type of fusion requires less computational effort, but also requires data streams to be correlated both in the time domain and in the measurement domain. Furthermore, Richardson [66] has shown that the inclusion of redundant sensory information almost always improves the performance of any signal-level fusion method that is based on optimal estimation.  Many different types of algorithms have been developed to perform data fusion at the signallevel. In his survey paper, Luo [47] identifies four different approaches to this problem. Weighted average methods, Kalman filter based methods, Bayesian estimation using sensor consensus theory, and statistical decision theory are the four majors paradigms utilized in signallevel multisensor fusion. The Kalman filter based, or optimal estimation based, approaches are particularly attractive for control-oriented applications due to their computationally efficient implementation and, consequently, their related real-time capacities.  A useful signal-level fusion technique has been developed by Nakamura [55] based on the minimization of the geometric volume of an uncertainty ellipsoid corresponding to all linear combinations of sensory information coming from multiple redundant sensors. This type of approach is based on previous work by Smith [71], and generally known as a Geometric Redundant Fusion (GRF) technique. The GRF methodology has been shown to be analytically equivalent to the corresponding Bayesian estimators and Kalman filters methodologies. One particular advantage of geometric data fusion processes comes from the strong physical meaning underlying the mathematical equations and, therefore, their ease of implementation. One of the limitations of Nakamura's approach comes from its inherent inability to correctly estimate the fused signal uncertainty when a large discrepancy is observed between the. different sensor measurements.  3.4 Multisensor Fusion  31  Elliott [19, 20, 21] extended Nakamura's work in order to provide realistic fused signal uncertainty levels when the sensors generate incoherent observations. This approach, known as a Heuristic Geometric Redundant Fusion (HGRF) technique, heuristically scale the fused signal variance, in ^-dimensions and for m sensors, based on the observed discrepancy between the sensors measurements. The HGRF approach is the one implemented in the current work and is briefly outlined, in the next subsection, for a unidimensional case where two sensors are providing feedback.  3.4.2 Heuristic Geometric Redundant Fusion Process From the general formulation [21], it is possible to develop the case corresponding to the redundant fusion of unidimensional data provided by two sensors. The fused signal mean,  x, N  corresponds to Nakamura's expression for this quantity, [55], and is given by the weighted average of the sensor measurements based on their relative uncertainty (i.e., variance) level, _X,<7 A  W  ~  2 2  +<Jl 2X2  2  2  W  '  -  U  (T| + <T  2  where <r, is the standard deviation of the sensor measurement x , and <7, is the corresponding t  variance. The expression for the fused signal uncertainty, or variance, is more complex and corresponds to a heuristic scaling of Nakamura's expression for this quantity. The scaling is based on the distance between the two measurements provided. First, the measurement spacing distances are evaluated, v,=fc^,  (3.2)  and  (*Z^1.  (3.3)  The separation factor is then established based on the largest measurement spacing distance, A: = max(v,,v ) 2  (3.4)  and the scaling factor is computed based on the separation factor. Two different relations are defined to compute the value of the scaling factor. The first scaling factor relation is defined for values of AT corresponding to the zone where the uncertainty regions of both sensors overlap (i.e.,  3.4 Multisensor Fusion  32  0 < K < 1). This is the case where the discrepancy between the sensor measurements is quite small. In this case, the scaling factor is defined as c = 1.0 + 0.273AT + 5.355/C -3.799AT 2  3  (3.5)  4  The other range of separation factor values (i.e., K > 1) corresponds to the situation where the distance between the measurement is greater than half the combined uncertainty of both sensors leading to no overlap of the measurement uncertainly ranges. This is the case where a high discrepancy between the sensor measurements is observed. Since both sensors disagree, the overall level of uncertainty is degraded proportionally to the separation factor. This is expressed mathematically by:  c = V2(K + l).  (3.6)  The justification of the numerical parameters used in equations (3.5) and (3.6) is provided in [21]. The scaling is applied to Nakamura's expression for the fused signal variance, given by: i  cr, <J~, 2  <y  =1  N  :  ;  2  (3-7)  \-  2 2  The HGRF fused uncertainty is then computed by applying the scaling to this last expression, c r / =c a . 2  (3.8)  2  N  Equations (3.1) to (3.8) provide a uniform and coherent way to optimally merge the data and knowledge provided by two sensors. This process is computationally inexpensive and only relies  2  2  on the sensors measurements (i.e., x and x ) and their associated variances (i.e., cr, and cr ). x  2  2  The fusion process itself acts as a weighted average based on the relative uncertainty of each sensor. Therefore, the fusion process eliminates faulty sensor measurements by assigning them a light weight. This leads to the dynamic reconfiguration of the sensing subsystem as long as it is possible to dynamically quantify the uncertainty of each sensor signal. It is therefore necessary to establish a robust uncertainty metric that is able to translate and quantify the relevance and accuracy of each sensor signal, at each sampling instant. The following section address the issues related to the development of such a metric.  3.5 Uncertainty Quantification  33  3.5 Uncertainty Quantification 3.5.1 Basic Concepts In order to use the data fusion process to dynamically reconfigure the sensing subsystem, it is necessary to quantify, on-line, the evolution of the sensor signal uncertainty. Quantifying the uncertainty of the sensor signal is equivalent to providing the fusion process with knowledge about the sensors state at each sampling instant. The dynamic reconfiguration of the system is directly dependent on the ability to generate an accurate and relevant observation of the level of data uncertainty associated with each sensor.  Uncertainty is found under various forms in control systems, and generates a variety of effects on the overall control system perfonnance. Pachter [60] provides an overview of the major sources of uncertainty affecting an autonomous system evolving in an unstructured environment, as well as the traditional solutions to address the problems caused by these uncertainties. Although the analysis is performed in a military autonomous vehicle context, it is still relevant to the applications discussed in this work. Table 3.1 provides a summary of uncertainty sources, as listed in [60].  #  Uncertainty Source  1  parametric uncertainty (unknown plant parameters)  2  unmodeled dynamics  3  stochastic disturbances  4  sensor/ measurement (random) noise  5  multiple control agents (decentralized control)  6  deterministic control signal manipulated by a non-cooperative agent  7  measurement noise controlled by a cooperative, or non-cooperative, agent  8  deterministic measurement error inserted by a non-cooperative agent Table 3.1. Uncertainty Sources in Autonomous Control  3.5 Uncertainty Quantification  34  From Table 3.1, one can see that the nature of the uncertainty sources is quite variable. In order to optimize the controller performance, it is necessary to include as many of these uncertainty sources as possible in the definition of the control law.  One can assume that the evaluation of the sensor performance under static conditions will provide a reasonable and simple quantification of the sensor uncertainty. In this case, the uncertainty associated with each sensor can be directly taken as the measurement variance, or error, associated with a given sensor. The variance can be quantified off-line, through repeated measurement and statistical analysis of the data, for the particular sensing conditions associated with the application. However, assuming static sensing condition is a fairly simplistic assumption and, in most of the cases, is not representative of realistic sensing conditions.  Sensing performance is affected by many variables. Errors in modeling, non-linearities, variation in environmental conditions (i.e., temperature, air density, relative humidity, lighting, etc.), decay of sensor sensitivity, and accumulated strain are all factors contributing to decreased sensor performance. Some of these effects are definitive and irreversible, while others are more variant in nature. Variant uncertainties have a particularly important effect since they cannot easily be modeled, even i f the presence of multiple sensing sources would generally enable their identification.  In this context, the overall absolute accuracy of a sensor is not as important as its instantaneous contextual accuracy. A measurement is only relevant in the context in which it has been taken. Especially when dealing with high-level sensor signals, from which lower-level information is extracted, based on feature identification (e.g., position from a vision sensor), the absolute accuracy of the sensing element is not as important as the capacity of the data processing system to correctly identify the feature and extract the corresponding information. This issue is particularly important since the control policies described herein, imply that: (a)- fault identification needs to be performed, and, (b)- faults, or failures, are more heavily related to the contextual accuracy of the sensor than to the individual absolute sensor accuracy.  3.5 Uncertainty Quantification  35  One determining factor in the establishment of an uncertainty metric comes from the nature of the system in which the metric is to be implemented. Robotic systems are deterministic, at least as far as the input/output relation is concerned. When the system is submitted to a given input signal (i.e., set-point), it is expected that the output will converge to set-point value in a limited amount of time. For this reason, it is possible, assuming an appropriate controller design, to use the set-point signal as referee to evaluate the signals provided by the multiple sensors. Furthermore, the set-point signal value is more conveniently available than the "true", or expected, value of the sensor signal that would be observed by a non-biased statistical estimator, and the use of the set-point signal enables the identification of a wider range of uncertainty sources.  It is possible to quantify the uncertainty of a sensor signal based on its expected value by computing the signal variance. This leads to a quantification of the dispersion of the measurements with respect to the absolute expected measurement. In the case where it is necessary to perform fault occurrence identification, it is of little use to measure the dispersion of the measurements with respect to the average value of the sensor signal. On fault occurrence, such an uncertainty metric is unable to identify the fault since the expected value of the signal follows the faulty sensor signal. This difficulty is overcome by referencing the variance of the sensor measurements to the set-point signal. As described above, this signal can be considered as the expected value of the sensor measurements due to the deterministic nature of the system.  3.5.2 Formulation of the Uncertainty Metric The variance of a sampled signal is given in [ 18] as:  ^ =^i(x -x).  (3.9)  2  k  Modifying the expression to account for a signal referenced to the input u  k  following expression:  leads to the  3.5 Uncertainty Quantification  36  In this last equation, the average input over the sampling window of length N is found though the usual expression of the mean: (3.11)  u  The uncertainty metric provided by equation (3.10) does not account for the static component of the uncertainty. This could be observed at the points where the measurement is exactly equal to the prescribed set-point signal. At these points, the metric provided by (3.10) leads to an exactly certain measurement (i.e., o~ =0), which is unrealistic since no sensors can generate such x  measurements. Therefore, it is necessary to modify equation (3.10) to include the basic sensor uncertainty, <r  min  . This basic sensor uncertainty is the conservative minimal uncertainty level  including the static sensor uncertainty, as well as the uncertainty brought in by the sensing context. The static sensor uncertainty is generally quantified by the sensor manufacturer for the best possible measurement context. Accuracy, resolution, signal-to-noise ratio, and hysteresis are all factors that describe the static sensor uncertainty. The inclusion of the basic sensor uncertainty leads to a modified expression for the uncertainty metric given by  ) +Kin) 2  2  (3.12)  3.5.3 Simulation Testing In order to demonstrate the efficiency of the metric as well as its ability to identify fault occurrence, a simple simulation is implemented in MatLab Simulink. Figure 3.8 illustrates the construction of the simulation. A closed-loop system is constructed and an input signal is applied to the system. The occurrence of a fault is simulated through the addition of a disturbance on the feedback path. Both the set-point signal and the measurement provided by the sensor are fed to the uncertainty quantification process.  For simulation purposes, an arbitrary plant model is selected and submitted to a sinusoidal setpoint signal. The plant transfer function for a sampling rate of 400 Hz is given by:  3.5 Uncertainty Quantification  37  4 \ X{z)  = — •  z-1  (3-13)  .  '  Assuming that the sensor originally has a unit transfer function, and that the applied perturbation reduces the sensor gain to 0.01 in 8 seconds, the system response is simulated. Figure 3.9 shows the response of the system, as well as the corresponding set-point.  U(z)  P L A N T  S E N S O R  U N C E R T A I N T Y Q U A N T I F I E R  Figure 3.8. Uncertainty Quantification Metric Evaluation Block Diagram From Figure 3.9, it can be seen that the system response amplitude increases linearly with the decreasing feedback gain. The uncertainty, as quantified by the metric defined in Subsection 3.5.2, is plotted in Figure 3.10 for various moving window sizes. A basic uncertainty level of 1 mm is assumed, and the uncertainty level immediately rises to this point at the beginning of the simulation. It can also be seen from Figure 3.10 that, the metric correctly identifies the discrepancies between the set-point signal and the measurement provided by the sensor.  3.5 Uncertainty Quantification  Set-point and System Output 1  i — •  Mi  •  :  I 1 I  >\  r  ' V / V  v  I I  "  ' : ' ' "  « »  '  1  l  '-A  E  W 0.  -20  -  -  :  :  I  • • • • \ \  i  I  •  ;  y/  — Set-point - - Output  i  ,  i  ,  V  Time [s]  Figure 3.9. Set-Point and Output of Simulated System under Sensor Fault  Figure 3.10. Uncertainty Levels for Various Moving Window Sizes  3.6 Summary  39  The effect of moving window size can also be identified from Figure 3.10. As the window size is increased, the uncertainty signal is smoothed. When the window size increases, the effect of measurement discrepancy with respect the set-point signal is reduced by the other points included in the window. This can be compared to a low-pass filter effect; it protects the system against the effect of highly dynamic measurements (i.e., noise-corrupted measurements). Therefore, using a large window size has the advantage of regularizing the uncertainty level signal and absorbing transient faults caused by noise spikes and dazzled sensors. On the other hand, a large window size increases the computational burden on the low-level control loop and generates a lag in fault occurrence identification. Both of these results are problematic for the implementation of this metric in high sampling rate servo-loop applications.  3.6 Summary In this chapter, a general formulation of the multirate heteroceptive sensor data integration policy for robotic servo-loops has been presented in order to increase the system tolerance to sensor failures or faults. The methodology is based on the traditional robotic system servo-loop design, where multirate heteroceptive data is merged with the high sampling rate feedback signal, and the requirements of advanced robotic automation systems.  The methodology uses the HGRF type of multisensor fusion in order to merge the data and knowledge provided by each sensor. The HGRF has been presented and the particular case of unidimensional data and two sensors has been developed, based on the general formulation of [21]. The use of multisensor fusion is justified by Richardson's work, [66], while the H G R F is selected over other techniques for its robustness toward incoherent measurement and its strong physical meaning.  An uncertainty quantification metric has been defined in order to monitor, on-line, sensor uncertainty. The on-line monitoring of each sensor uncertainty enable the use of the HGRF fusion process to dynamically reconfigure the sensing subsystem in the presence of fault occurrence. The uncertainty metric is based on the quantification of each signal variance with respect to the mean value of the deterministic system set-point signal, for a given moving  3.6 Summary  40  window size, to which a basic uncertainty level is added to account for the uncertainty not only related to the sensing process itself, but also to the sensing context. The capacity of the metric to identify the occurrence of a sensor fault has been examined in simulation for the case of a progressive sensor failure. The performance of the uncertainty quantification metric in presence of other types of sensor faults will be further examined in the next chapters.  The issues related to the multirate nature of the system still remain to be addressed. The particular approach utilized to address the multirate nature of the system will complete the definition of the methodology. Two different approaches are presented in the following chapters.  Chapter 4 Model-based Policy 4.1 Preamble This chapter presents the first of two policies for integrating multirate redundant heteroceptive data in the position servo-loop of a robotic manipulator in order to increase the system tolerance to the occurrence of sensor fault or failure. Since the heteroceptive measurements are only available at an integer fraction of the controller update rate, the policy provides a framework to regulate and propagate the effect of the heteroceptive sensor estimate of the system position on the proprioceptive measurements that track the system position.  This chapter begins with the general formulation of the integration policy in the context that has been defined in the previous chapters. Namely, in a system where heteroceptive sensing is already present and used in high-level tasks (i.e., set-point generation, off-line calibration, etc.), low-level sensor fault/ failure tolerance is increased through the integration of the redundant information provided by the heteroceptive sensor. The policy presented in this chapter is based on recursive Kalman filter prediction, to both predictively compensate the processing delay associated with the heteroceptive sensor feedback path, and propagate the effects of the heteroceptive measurement update forward in time when no updates are available. The 41  4.2 General Formulation  42  prediction of the redundant data at the controller update frequency optimizes the use of the information available in the system and aims to maintain sufficient tracking performance during the period when no heteroceptive measurement is directly available.  In the latter portion of this chapter, the recursive formulation of the Kalman filter predictor is closely examined. A review of the fundamental theoretical background behind the statistical procedure is provided, as well as the necessary assumptions for this particular application. The standard equations are established and the predictor case is derived. Finally, the recursive formulation of the Kalman filter predictor is formulated as a transfer function. This transfer function will be utilized in the policy stability evaluation given in Chapter 7.  4.2 General Formulation The use of a multisensor data fusion method such as HGRF requires time-correlation of the data streams to be fused. For the fused value to retain a relevant physical sense, it is necessary that the information provided by the various sensors translate the same physical reality. The use of highlevel heteroceptive sensors (e.g., digital cameras, area scans, etc.) to quantify simple features (e.g., position, speed, etc.) requires extensive processing of the raw signal. The processing delay related to the extraction of the features from the raw signal limits the sensor bandwidth to only a fraction of the controller update rate. Thus, it is necessary to predictively compensate the timedelay caused by the rich heteroceptive signal processing requirements to preserve the physical meaning of the signal through the fusion process.  The predictive time-correlation of the heteroceptive sensor signal fulfils the requirements of the multisensor data fusion procedure and preserves the physical nature of the fused signal. One must also be concerned with the effect of the prediction scheme on the heteroceptive sensor measurement quality, as quantified by the signal uncertainty. The propagation of the signal uncertainty through the prediction process must be minimized in order for that signal to maintain a sufficient weight in the fusion process, and to have a quantifiable effect on the quality of the system state estimate.  4.2 General Formulation  43  The recursive Kalman filter algorithm is well suited for this problem. This algorithm minimizes the variance of the prediction, even in an uncertain or noisy context. The use of such an optimal estimation, or prediction, algorithm minimizes the propagation of the heteroceptive feedback signal uncertainty through the time-correlation prediction process. Exponential propagation of the uncertainty due to the prediction process would eventually nullify the purpose of multisensor fusion. Once the predictively time-correlated measurement uncertainty level becomes higher than the one presented by the proprioceptive sensor, only a negligible reinforcement effect on the fused signal mean or its uncertainty level is obtained.  This policy proposes the use of a Kalman filter predictor to time-correlate the data streams provided by the various sensors, as shown by Figure 4.1. This approach provides an optimal procedure to time-correlate the data streams issued by both sensors. Nevertheless, this does not address the issues related to the multirate nature of the system. Adopting a two-step fusion scheme enables data fusion when heteroceptive feedback is available, but generates a discontinuous feedback signal that affects the tracking performance of the system. In this case, the periodic heteroceptive signal acts more as a disturbance than as a means to increase the quality of the feedback signal. Thus, it is then necessary to extend the policy to regulate the effects of the addition of redundant data in the system.  CONTROLLER  ACTUATOR  SYSTEM  DYNAMICS  DYNAMICS  G(z)  SENSOR DYNAMICS  HGRF UNCERTAINTY QUANTIFIER  U(z) HETEROCEPTIVE SENSOR  Z  UNCERTAINTY QUANTIFIER  KALMAN PREDICTOR  Figure 4.1. Time-Correlation of the Heteroceptive Feedback Signal using Kalman Prediction  4.2 General Formulation  44  It is possible to predict the time-correlated heteroceptive data at the controller update rate. However, it is again preferable to implement a closed-loop prediction scheme in order to ensure the optimality of the predicted measurements and minimize the propagation of the uncertainty through the feedback signal processing. The implementation of a closed-loop prediction algorithm at the controller update rate can only be performed based on the available high-rate proprioceptive feedback. Therefore, a Kalman filter based observer is added to the policy to generate high-rate feedback data. This data is used to referee the proprioceptive sensor data in the absence of heteroceptive measurements. Figure 4.2 presents the block diagram of the complete proposed policy, where one Kalman filter-based predictor is used to compensate the delay associated with the heteroceptive feedback data, herein referred to as the time-correlator, and another Kalman filter-based predictor is used to generate high-rate analytical feedback redundancy, herein referred to as the regulator.  CONTROLLER  ACTUATOR  SYSTEM  DYNAMICS  DYNAMICS  0(z)  SENSOR DYNAMICS KALMAN PREDICTOR  UNCERTAINTY QUANTIFIER  U(z)  HETEROCEPTIVE SENSOR  UNCERTAINTY  UNCERTAINTY  QUANTIFIER  QUANTIFIER  KALMAN  Z  PREDICTOR  Figure 4.2. Model-Based Policy for Multirate Heteroceptive Sensor Integration Based on the strategy discussed above, a multisensor fusion scheme is defined. The system presented in Figure 4.2 present two different redundant data sources that are used for different purposes by the HGRF fusion process. On the Major Time-Steps, herein defined as the timesteps where heteroceptive feedback is available, the time-correlated heteroceptive measurement is fused with the proprioceptive feedback measurement. On the Minor  Time-Steps, herein  defined as the time-steps corresponding to the controller update instants, no heteroceptive data is  4.3 Kalman Filter  45  available and the proprioceptive sensor measurement is fused with the analytical feedback measurement provided by the regulator. This two-step fusion scheme aims to provide measurements to referee all the proprioceptive measurements and to propagate the information injected by the heteroceptive sensor at the major time-step to the following minor time-steps.  The following section takes a closer look at the Kalman filter itself and its use for signal prediction. The necessary assumptions behind the use of such a procedure are also reviewed in this section.  4.3 Kalman Filter A Kalman filter is a procedure used to generate statistically optimal estimates of a system's internal states when the available outputs are corrupted by zero-mean noise (i.e., white noise). In the current work, the discrete-time recursive fonnulation of the filtering algorithm is adopted. This section provides the necessary fundamental background as well as the derivation for the current application. A more detailed development of the Kalman filter, as well as the underlying theory, is provided in [7, 13, 18, 38].  One can consider a noisy discrete-time system given by the state model, x =^x +Au +rw , k+l  k  k  (4.1)  k  and the measurement model, = k+\ + "k+\ + *+i> Cx  where x  k  D  (-)  v  4  2  is the system state vector, u is the input vector, and 0^ is the output vector, all for k  time-step k. O is the state transition matrix, A is the input matrix, C is the output matrix, and D is the feed-forward matrix. Finally, w is the system noise vector, v k  k  is the measurement  noise vector, both for the time-step k, and T is the noise coupling matrix.  The recursive formulation from [18], illustrated in Figure 4.3, is adapted to this particular application. For an arbitrary time-step k, the best estimate generated by (4.1), based on the initial state vector estimate, is given by  4.3 Kalman Filter  46 ^M\k=^ +Au . klk  (4.3)  k  From (4.2), a one step ahead prediction of the system output is given by, e =Cx +Du . k+]  k+]]k  (4.4)  k+]  At the following time step (i.e., k+J), the system output is measured and it is possible to compute the prediction error, 0  =O,  t + 1  + l  -0,  + 1  .  (4.5)  c  z  c  -^^^ K  1  A  z  *+l|*  A  1 z  Figure 4.3. Recursive Kalman Filter Block Diagram [18] Given the prediction error, it is possible to improve the state estimate provided by (4.3) and drive the prediction error to zero by adding a proportion of this error to the estimate of the state vector, through the correction equation,  The recursive computation of the Kalman Gain, K, enables the minimization of the estimation error covariance, assuming known noise statistics. The estimation error is defined as, k+l  x  =  k+\ ~ k+\\k+l •  x  x  (4-7)  The estimation error covariance, P , is the quantity to be minimized, k  P =E[x x \. r  k  k  k  (4.8)  4.3 Kalman Filter  47  A recursive scheme is then established to generate the minimum-variance estimate. The recursive scheme is given by (4.9)  P* =<&P <& +rQT , T  r  k  (4.10)  K =P*C [cP*C +RY, T  T  k  V  +  =[l-K C^k\k-\ k  (4.11)  ~K Du , k  k  (4.12)  P =[l-K C]P *, k+i  k  k  where Q is the system noise covariance matrix and R is the measurement noise covariance matrix. These matrices are describing the quality of the model defined earlier (i.e., equations (4.1) and (4.2)), and the quality of the measurement provided by the sensor, respectively. The establishment of these matrices is discussed in the latter portion of this section. Two major modifications are necessary to the development presented in [18] in order to implement the recursive formulation of the Kalman filter in the current context. First, the correction of the state vector estimate is delayed until the heteroceptive measurement is available. Furthermore, the derivation for a complete state-space system model is performed (i.e., in (4.2) the feed-forward matrix, D, is assumed to be non-null). The first modification is illustrated in Figure 4.4, which shows the availability of the data in a system where a processing delay is present. Assuming that the processing delay on the heteroceptive feedback path corresponds to the major time-step sampling period, the heteroceptive measurement corresponding to the state of the system at major time-step Mis only available at major time-step M+1.  m  1 1  1 1 1  M1  l l I I  m+4  l l 1 1 M +l M+\  U  l l  l l  m+8  l l l l M +2  M I N O R  T I M E - S T E P  M A J O R  T I M E - S T E P  M+2  U  Figure 4.4. Heteroceptive Sensor and Set-Point Signals Availability  4.3 Kalman Filter  48  Therefore, re-writing equation (4.11) for the major time-step M+J leads to an optimal estimation of the state of the system corresponding to the previous major time-step based on the current realization 0  M  and on the previous prediction x \ _ , M  M\M =[l- M ^M\M-\  X  K  M  {  M®M  C  ~ M M-  +K  K  Du  (4-13)  Based on this estimation of the system state at the previous major time-step, the current state of the system, as well as the output, are estimated using the general system model expressed by (4.1) and (4.2), in this case re-written as: M+\\M  =  X  (4-14)  ® XM\M  and =  M+\\M  Cx  +  D  u  M  +  \ -  (4-15)  Thus, the implementation of equations (4.9) to (4.12), (4.14), and (4.15) provide a way to predictively time-correlate sensing data delayed by its associated processing time.  The second modification to the procedure developed in [18] requires the recursive formulation for a complete state-space model, as shown by equations (4.9) to (4.12). Appendix E provides the necessary derivations. In a normal situation, the use of a full state-space model in a prediction context would lead to non-causal terms in the measurement model (i.e., in (4.15) the future value of the input signal is required but is not yet available). Due to the processing delay present on the feedback path, this situation does not occur. Instead, the measurement prediction is performed to instantaneously time-correlate a lagging signal and the necessary input signal is actually available, c.f. Figure 4.4.  The procedure described in this section is used for both Kalman filters illustrated iii Figure 4.2. Each implementation of the optimal prediction algorithm is done according to the characteristics of the signal to be predicted. The time-correlator Kalman filter uses a model of the system corresponding to the heteroceptive sensor sampling rate. Also, in order to compute (4.9) and (4.10), it is necessary to quantify the amount of noise, or uncertainty, associated with the establishment of the system model, Q, and the measurement of the system output, R. In a general situation, these covariance matrices would be established from measurements performed directly on the system or from conservative assumptions. The definition of Q and R greatly affect the  4.3 Kalman Filter  49  dynamics of the fdtering process. Particularly, the relative magnitude of these noise terms, with respect to one each other, leads to different filter behavior, as described below.  For increasing values of the noise matrices magnitude ratio, (i.e.,  ), the Kalman filter  increases the weighting of the realization sequence. This situation leads to a filter that follows the measured output more closely than the system model. On the other hand, a decreasing ratio, leads to an increase in the weight put on the system model and, therefore, a stronger filtering effect. In the current case, the desired behavior of each filter can be inferred from the two general requirements of the multirate heteroceptive sensor data integration policy. Thus, to implement the policy, a heuristic tuning rule for the noise characteristics, for both filters, is preferred to a more empirical tuning.  Again, the general objective of the policy is to provide fault tolerance capability in the presence of proprioceptive sensor fault, or failure, through the integration of heteroceptive sensor feedback in the servo loops of robotics manipulator. Fault tolerance is generated through the dynamic reconfiguration of the sensing subsystem on fault occurrence. As stated earlier, this implies that it is possible to identify the occurrence of the sensor fault and that the timecorrelating process used on the heteroceptive feedback path does not affect the original signal beyond the fault occurrence detection threshold.  The time-correlator filter used to predictively compensate the processing delay characteristic of the heteroceptive feedback path role must then be limited to time-correlation. In order for the uncertainty quantification process to identify the occurrence of the fault, it is required that the time-correlator filter generate the predicted system output based on the current heteroceptive measurement available, and not on its internal model. This filter behavior is caused by a stronger weight on the realization sequence, than on the filter internal system model. This particular tuning of the Kalman filter is achieved by selecting a large value for the noise matrices magnitude ratio, while maintaining the measurement noise magnitude to a level appropriate with the sensor hardware and signal processing used. This results in a good time-correlation behavior, while only presenting weak disturbance rejection characteristics.  4.4 Recursive Kalman Filter Transfer Function  50  On the other hand, the regulator filter, used to regulate and propagate, through time, the effect of the heteroceptive measurement update in the system, must present a stronger regulation behavior. Therefore, it is weighted more heavily on the system model than the time-correlator filter. In this case, a relatively low value of noise magnitude ratio is used while maintaining the measurement noise magnitude to a physically relevant level. This type of tuning leads to stronger filtering performance from the Kalman filter algorithm.  The selection of the actual noise matrices magnitude ratio values used in order to generate each type of behavior is not examined in more detail in this chapter. Chapter 7 presents values for both the system noise covariance matrix and the measurement covariance matrix magnitudes, as well as an analysis of the effect of the variation of these parameters on the performance of the model-based policy. This chapter also presents simulation results to illustrate the effect of the noise magnitude ratio.  The heuristic definition of the noise characteristics used in the Kalman filters completes the definition of the model-based low-level heteroceptive sensing integration policy to increase system tolerance toward proprioceptive sensor fault occurrence. The next section takes a closer look at the expression of the recursive algorithm as a transfer function for a complete state-space model.  4.4 Recursive Kalman Filter Transfer Function The integration of a Kalman filter-based predictor in the heteroceptive feedback loop of a robotic manipulator directly affects the dynamic characteristic of the system. In order to account for that change, it is necessary to include this process in the evaluation of the closed-loop stability of the system. The expression of the Kalman filter recursive fonnulation as a transfer function allows for a straightforward integration of this component with the others processes included in this multirate heteroceptive sensing integration policy. This section address the issues related to the fonnulation of the recursive algorithm as a discrete-time transfer function. The development  4.4 Recursive Kalman Filter Transfer Function  51  presented follows [13]; however some modifications are necessary in order to account for the particular context in which the Kalman filter is used. Figure 4.5 presents the general input/output relation carried for the Kalman filter transfer function. The Kalman filter is provided with a heteroceptive measurement, which is, with respect to the current state of the system, delayed by T as input signal, Q _ . A predictively timek  T  correlated measurement estimate, Q , is generated as output at a rate corresponding to the k  processing delay duration. Assuming that the heteroceptive sensing rate is set by the processing delay duration implies that the sensing hardware is actually faster than the execution of the processing algorithm. This is usually the case when visual heteroceptive sensing is used, as in the current application.  0, r  0  K A L M A N F I L T E R  Figure 4.5. Kalman Filter-based Predictor Block Assuming that the system represented in Figure 4.5 is represented by a state-space model where the state equation is given by, k\k-T = ® k-T\k-T  X  X  + k-T Au  >  (4- 1 6)  and the measurement equation is given by, ®k\k-T=Cx _ +Du , k]k  T  k  (4.17)  it is possible to suppress the recursive nature of the algorithm, based on the filter definition. As discussed earlier, the Kalman filter is a two step procedure in which a prediction is first computed, then compared with the realization when it becomes available, generating a corrected estimate of the current state vector. The correction part of the filter is expressed by, k-T\k-T ~ k-T\k-2T  X  X  (4-18)  4.5 Summary  52  and corresponds to the addition of the innovation term, scaled by the Kalman gain, to the previously predicted state vector to form the optimal state vector estimate. Substituting the innovation term for its definition (i.e., equation (4.5)) leads to, k-x\k-x k-T\k-2r  x  {®k-T  =x  +K  (4.19)  ~®k-T\k-2r)-  Then, the output measurement prediction can be written as a function of the previous state vector . prediction, X_ _ k  T]k  + K(e _ - (CVr|*-2r +  = X_ _  T  k  T]k  k  2T  T  )) •  k-r  Du  (4.20)  Simplifying this last equation and substituting back into (4.16) yields, k\k-r  x  = *I 7  ^k- k-2r  KC  A  + ®K®k-r + [A - O K D k - r •  (4.21)  Performing the z-transform at time step k and rewriting leads to, X  (  Z  [ A - O ^ ( Z - ' ) Z -  - )  '  V  [I-®[l-KC]z- \  [l-®[l-KC]z~ \  T  T  Performing z-transform on equation (4.17) and substituting in (4.22) leads to the expression of the estimated output of the block as a function of the system input signal, u(z~ ), and the l  delayed measurement, © ( z ) , -1  e( -.) z  V  '  =  C[A-0^]c/(z-'k-  C*KBb-*y [l-<&[l-KC}z- \ T  [l-<&[l-KCy \ T  ^_, V  1  )  }  •  This expression can be seen as a multiple inputs, single output, transfer function. Equation (4.23) will be used in Chapter 7 in order to build the closed-loop transfer function of the system and evaluate the system stability.  4.5 Summary This chapter presents a model-based policy to integrate redundant heteroceptive feedback in robotic manipulator low-level servo loops. This policy completes the general methodology that has been formulated in Chapter 3. The main objective of the policy presented in this chapter is to provide a coherent framework to address the multirate nature of the heteroceptive sensor data to be fused with the high-rate proprioceptive feedback.  4.5 Summary  53  The use of high-level heteroceptive feedback sources creates a time-delay on the feedback path and reduces the usable sensor bandwidth. The processing delays are caused by the amount of processing necessary to extract low-level features (i.e., position, speed, etc.) from the raw heteroceptive signal. This policy proposes the use of optimal prediction (i.e. Kalman filtering) to compensate the processing delay and enable the fusion of time-correlated data. Furthermore, in order to provide reference values that can be fused with the proprioceptive measurements between the heteroceptive measurement updates, a Kalman-based regulator is used. This regulator generates one step ahead predictions of the fused feedback measurements, at a rate corresponding to the controller update rate. This particular data propagation scheme for the heteroceptive feedback measurements enables the use of a closed-loop predictor and therefore minimizes the signal degradation due to prediction process itself.  The use of independent time-correlator and regulator processes generates two different types of reference measurements. Accordingly, a two step data fusion scheme is defined to optimize the information provided by the different sources. On the major time-steps, the proprioceptive measurements are fused with the available time-correlated heteroceptive measurement. On the minor time-steps contained between two consecutive heteroceptive measurements, computed measurements, generated by the regulator, are used to referee the proprioceptive measurements and reinforce the quality and relevance of the feedback signal used in the control law.  The recursive formulation of the Kalman filter-based predictor has been developed for a complete state-space model and for a system presenting delayed measurements. This formulation enable the use of the full state-space model in the generation of the predicted measurements, while still respecting the causality of the Kalman filter algorithm. The policy is completed by the definition of a heuristic rule for the tuning the noise characteristics used in each filter. The behavior of the Kalman filter is governed by the definition of the noise characteristics associated with the system in which the filter is used. The time-correlation of the heteroceptive measurement requires heavy weighting on the realization sequence to avoid corrupting the information contained in the heteroceptive feedback signal. This behavior is achieved by selecting a large value for the ratio of the system noise magnitude to the measurement noise:  4.5 Summary  54  magnitude, while keeping the measurement noise level to a physically meaningful level. On the other hand, the regulator must be weighted more heavily on the system model in order to provide a sufficient level of regulation. This filter behavior is achieved by using a smaller ratio of noise magnitudes when maintaining the measurement noise level to a physically coherent level. The Kalman-based regulator generates better noise rejection performance than the time-correlator one, but also presents less bandwidth.  Finally, the recursive Kalman filter has been formulated as a multiple-input, single-output, transfer function that can be integrated in the evaluation of the system stability, given in Chapter 7.  Chapter 5 Polynomial Policy 5.1 Preamble This chapter presents a numerically based approach to address the issues related to the multirate .;• nature of the system at hand. It is necessary to analytically expand the limited bandwidth of the heteroceptive sensor in order to optimize the use of the information it provides. The major difference between this approach and the one presented in Chapter 4 is that this type of approach does not necessitate the establishment of a highly developed and complete system model.  A general formulation of the proposed approach is provided first. This approach requires the relaxation of the time-correlation of the data stream constraint required by the use of the multisensor fusion process. In a control context, this constraint can be relaxed provided that the residual time delay is included in the closed-loop stability derivations and that stability can be achieved. The control policy is based on the use of a polynomial prediction algorithm to extrapolate the heteroceptive sensor measurements for the control signal update instants when no current heteroceptive data is available.  55  5.2 General Formulation  56  The polynomial predictive algorithm is based on a Newton-Gregory numerical method developed for a second order system. This approach does not require an elaborate model of the system to predictively estimate the heteroceptive sensor output, only a general assumption about the system order is necessary.  The derivation of the numerical process as a transfer function follows. The transformation of the difference equation to a transfer function, in a multirate context, is necessary for the stability analysis of the proposed policy, which will be presented in Section 7.3. Finally, a summary concludes this chapter.  5.2 General Formulation In the model-based approach, a closed-loop prediction scheme (i.e., Kalman prediction) was adopted in order to propagate and regulate the effects of the heteroceptive sensor update on the system state estimation. This time, an opposite approach is developed. A numerical open-loop prediction algorithm is utilized in order to extrapolate past heteroceptive measurements based on a polynomial established in real-time. This approach enables the generation of predicted heteroceptive measurements at a rate matching the control signal update rate, but without correcting the measurements for the processing delay associated with the use of a higher-level sensor. Figure 5.1 present the block diagram corresponding to the proposed policy.  The use of open-loop prediction to analytically increase the bandwidth of slow sensors and improve controller performance has been used in a number of different applications and under various implementations [27, 45, 64]. Particularly, Hara and Ramachandran [27, 64] present multirate control algorithms for servoing electro-mechanical devices. The basic objective in the use of multirate control strategies in servoing problems is to provide smoother control input and higher control bandwidth under lower sampling frequencies. This is generally achieved by updating the control signal faster that it is possible to measure the state of the system. A n openloop predictor is generally used to estimate the system output, or state, between two consecutive measurements. Traditionally, an open-loop estimator, or observer, is constructed following  5.2 General Formulation  57  observer design theory and extensive knowledge of the system dynamics is required for a successful implementation.  ()  U Z  ^+  C O N T R O L L E R  A C T U A T O R D Y N A M I C S  S Y S T E M D Y N A M I C S  0(z)  S E N S O R D Y N A M I C S U N C E R T A I N T Y Q U A N T I F I E R  U(z)  £ H E T E R O C E P T I V E S E N S O R  U N C E R T A I N T Y Q U A N T I F I E R  I N T E R P O L A T O R  Z  Figure 5.1. Polynomial Approach Block Diagram This type of approach strongly relies on the quality of the system model in order to provide a high-level of performance. Furthermore, in order to maintain a reasonable level of performance in the presence of unmodeled perturbations or sensor failures, the observer must be adaptive. That is, the system model must be updated when the observer performance starts to decrease, as it would in the presence of a sensor fault.  To handle modeling uncertainties, Ramachandran [64] stresses the advantages of using a numerical prediction scheme. Due to its numerical nature, this type of procedure is less sensitive to modeling uncertainty than model-based methods, but does not provide the same level of performance and noise rejection capabilities. However, in the case where modeling uncertainties are known to severely affect the prediction scheme, a numerical prediction algorithm provides a simpler and computationally cheaper solution, while still providing sufficient performance in most applications. Many types of numerical procedures based on polynomial predictive algorithms may be considered [64]. For equally spaced data, the use of the Newton-Gregory  5.3 Newton-Gregory Backward Difference Interpolation  58  method has been shown to provide very good performance, while being computationally inexpensive. The only assumption necessary to implement this type of open-loop prediction algorithm is related to the system response characteristic.  Most electro-mechanical systems, such as feed drives, are generally modeled with a second order Laplace-domain transfer function, which lead to an oscillatory time-response. It is then possible to locally fit to the oscillatory system response a second order polynomial based on the most recent measurement and the two previous ones. Once established, this polynomial can be used in order to predict the plant output when no measurements are available. The next section details the computation of the polynomial parameters in real-time.  Finally, this technique enables the simplification of the data fusion scheme. Since, in either predicted or measured format, the heteroceptive sensor data is available at every minor timestep, the same signals are available for fusion. This allows for more unifonn signal processing and minimizes the discontinuities imposed on the feedback signal by switching between two different type of signal processing, as it would be the case i f the heteroceptive sensor data was only available at the major time-steps.  5.3 Newton-Gregory Backward Difference Interpolation When processing uniformly time-spaced data, it is possible to use the Newton-Gregory backward difference interpolation method to predict future values, spaced over smaller time intervals. The general derivations of the numerical method are presented in [42], while the derivations for the particular case of data sampling in control systems are presented in [64]. This section presents a brief overview of the procedure, developed for a case where the heteroceptive feedback is provided at a lower rate than the control update rate.  Assuming that the heteroceptive sensor measurement update are provided every Major TimeStep (time index T, sampling  frequency—J—), the controller update is performed every  Minor  AT  Time-Step (time index a, sampling frequency—), and the ratio of the Major Time-Step period to At  5.3 Newton-Gregory Backward Difference Interpolation  59  the Minor Time-Step period (i.e., — ) , is a positive non-null integer (i.e., «), it is possible to At  write the Newton-Gregory interpolator as, Y „=Y +sAf +?^A f ,  (5.1).  2  T+  T  2  l  where,  • J  T  +  s  *<y  (5.2)  T  Ar is the non-dimensional interpolation independent variable. This variable represents the distance, or elapsed time, since the last measurement update. The coefficients of the polynomial presented in (5.1) are evaluated from a standard difference table, as shown by Table 5.1. More precisely, the difference terms shown in that last table are computed using the following relations: *f =Y _ .-Y _ , 2AT  (5.3)  Af =Y -Y _ ,  (5.4)  l  T  M  2  T  T  T  AT  and A j\=Af -Af\.  (5.5)  2  2  Time T-2AT  Y,  A  A  2  Y -2AT T  4/i T-AT  T  YT-AT  A /, 2  Y  T  Table 5.1. Difference Table for Second Order Interpolation Evaluating the difference table each time a new heteroceptive measurement is available (i.e., on every major time-step) enables the implementation of the interpolation procedure in real-time. At every minor time-step, equation (5.1) is calculated based on the new value of s and a predicted output is generated. At every major time-step, the difference table (i.e., (5.3) to (5.5)) is recomputed while the sensor measurement is directly written as the output.  Only equations (5.1) to (5.5) must be implemented in order to include the interpolation algorithm within the control loop. Nevertheless, in order to include the interpolator dynamics in the system  5.4 Formulation as a Transfer Function  60  stability evaluation, it is necessary to formulate equation (5.1) as a discrete-time transfer function. The next section addresses the issues related to that question.  5.4 Formulation as a Transfer Function Figure 5.2 defines the transfer function required in order to easily integrate the Newton-Gregory interpolator dynamics in the evaluation of the closed-loop stability. The input of the transfer function is the heteroceptive measurements at every major time-step (i.e., Y is a function of z" ), while the output of the transfer function is the predicted heteroceptive measurements, at the •,. controller update rate.  Figure 5.2. Newton-Gregory Interpolator Block Definition In order to evaluate the stability of the multirate closed-loop system, many different methods have been derived [7]. In the current work, the multirate characteristic of the system is removed by expressing both loops as a function of the minor time-step. Since it is assumed that the major time-step is an integer multiple of the minor time-step, it is possible to express the slow rate heteroceptive data as a function of the minor time-step. Therefore, all the following derivations are performed in the minor time-step framework. Rewriting equation (5.1) for an arbitrary time-step a following a heteroceptive measurement update AT and substituting equations (5.3) through (5.5) into (5.1) yields K+a  = Y +s{Y -Y _ ) K  K  K  ll  +  2  {YK-2Y _ +Y _ ) K  n  K  2n  (5.6)  where s is now expressed as a ratio of number of time-steps, _ (K + a)-K  n  and  _a n  (5.7)  5.5 Summary  61 eve [(),/?-1].  (5.8)  Performing the z transform for the current time-step leads to the interpolator transfer function of the output at a given minor time-step, a, following the latest heteroceptive measurement update,  r{z)  V  Y{z) v  s  ,  —+ - + 1 2 2  Z  -a  ~S  2  Z  -n-a  . +  -2n-a  (5.9)  5.5 Summary In this chapter, a polynomial based approach was defined to handle the multirate nature of the system and efficiently propagate the effect of the heteroceptive measurement update on the system position estimate inside on its sampling period. This approach allows for a unifonn data fusion scheme where the same processing algorithm is used at every minor time-step.  A general formulation of the proposed policy has been presented, in block diagram form, in Figure 5.1 and discussed in detail. The policy is based on the use of a Newton-Gregory polynomial algorithm to predict heteroceptive measurements at the control signal update rate.  1  The Newton-Gregory interpolator has been developed for control-oriented signal sampling and prediction where the heteroceptive measurements are updated at a frequency that is an integer fraction of the controller update frequency. Finally, the discrete-time transfer function corresponding to the interpolator polynomial has been derived. This transfer function expression for the interpolator will be used in Chapter 7 to integrate the interpolator dynamics in the closedloop stability evaluation of the overall polynomial policy for multirate heteroceptive sensor data integration in robotic systems position servo-loop.  Chapter 6 Implementation 6.1 Preamble In this chapter, a description of the experimental set-up is provided. This description is followed by an overview of the open-architecture control platform used in this application. The integration of the electromechanical experimental robot within the control system, as well as the implementation of the proposed policy, are then addressed.  The system is implemented in an Encapsulated Logical Device Architecture, which provides an object-oriented approach to efficient and coherent treatment of information and control signals for multisensor robotics control system development. Once the overall architecture of the system is established, the architecture is implemented in the control system.  In order to generate good simulation results, an adequate model of the system is necessary. In the current work, the system is empirically identified based on a parametric model. The identification procedure, as well as a brief overview of necessary theoretical background, are also provided.  62  6.2 Experimental Platform  63  6.2 Experimental Platform The proposed policies are aimed at implementation on industrial automation and robotic systems. In order to simplify the problem at hand and limit the number of variables, as well as their effects, a simple robotic device was designed and built in the Industrial Automation Laboratory for test purposes.  Figure 6.1. Experimental Cartesian Manipulator The experimental manipulator, see Figure 6.1, is a Cartesian manipulator where both axes are actuated using a different mechanism. The X-axis is actuated by a Pittman D C motor series 14204 connected to a double threaded lead screw of pitch 6.35 mm (0.250"). The X-axis has an active range of 180 mm and the position feedback is provided by an Agilent Technologies HEDS-6310 optical encoder presenting a resolution of 0.36° (1000 pulses per turn). Table 6.1 presents a summary of the motor parameters, as well as the mechanical drive and the sensor parameters for this axis.  The Y-axis is powered by a Superior Electric synchronous stepper motor through a timing belt and sprockets (35 mm pitch diameter). The active range of motion for the axis is 160 mm and the position feedback is provided through a Daytronic Ultrasonic Proximity Sensor model D4900. The ultrasonic sensor has a sensing range of 4 to 30 inches, adjusted for the current application.  6.2 Experimental Platform  64  According to the manufacturer specifications, this analog sensor provides a repeatability of ± 1 % of the full scale setting and approximately 5% hysteresis. This sensor measures distance via the elapsed time of flight of the ultrasonic wave and, therefore, requires a reflective surface perpendicular to its axis to function properly. This surface is provided by adding an aluminum reflector on the end-effector trolley, see Figure 6.1. A summary of the Y-axis mechanical drive, actuator and sensor parameters are provided by Table 6.2.  An overhead camera is also used in the system (Figure 6.2) in order to provide information about the environment in which the system is operating (i.e., heteroceptive sensing), but it can also be used to provide redundant data about the system state. In this case, redundant data is generated for the end-effector position. The position of the end-effector is identified by the presence of a colored marker on the trolley, Figure 6.1. The JVC C C D color digital camera is interfaced to a PC running Windows N T through a Imaging Technologies IC2-COMP framegrabber [36]. This combination of hardware generates a 640x480 pixel resolution color digital image in approximately 30 ms. The whole acquisition sequence, including grabbing the image, transferring it to the host memory, and processing the data, requires approximately 200 ms. A summary of the camera and lens parameters is provided in Table 6.3.  The workcell is also equipped with a lighting system to provide uniform lighting of the workspace and facilitate the location of the end-effector marker. In order to avoid lighting variation problems related to the use of commercial fluorescent light tubes, high-frequency electronic ballasts have been substituted to the original magnetic ballasts. The high frequency signal generated by the electronic ballasts (= 15 kHz) is sufficiently higher than the camera sampling frequency (30 Hz) such that the camera is unaffected by the variations in the light signal. Typical commercial magnetic ballasts running at A C power frequency (i.e., 60 Hz) are unsuitable for this purpose.  6.2 Experimental Platform  65  Motor Data - Pittman 14-204-B352 Parameter  Symbol  Units  Value  Continuous Motor Torque (Max.)  Tc  N m  183.6xl0"  Peak Torque (Stall)  TpK  N •m  1.44  Motor Constant  KM  No-Load Speed  SNL  rad 1 s  388  Friction Torque  1>  N •m  11.3xl0"  3  Rotor Inertia  JM  kg • m  2.61xl0"  5  Electrical Time Constant  TE  ms  1.58  Mechanical Time Constant  TM  ms  7.04  N •m  60.9xl0"  4w  1  Nm  Torque Constant  K  T  Back-EMF Constant  K  E  Reference Voltage  E  Resistance  R  Inductance  A v  76.5x10" :  rad/s  76.5xl0"  V  30.3  Q  1.57  L  mH  2.50  No-Load Current  INL  A  0.21  Peak Current (Stall)  Ip  A  19.2  r  Optical Encoder Data - Agilent Technologies HEDS-6310  Cycles per Revolution  CPR  CPR  1000  Drive Data - Lead Screw  Number of Threads  n  Thread Pitch  P  mm  6.35  Drive Range  X  mm  180  R  3  2  Table 6.1. Summary of X-Axis Drive Parameters  3  3  3  6.2 Experimental Platform  66  Motor Data - Superior Electric M 0 9 2 - F C - 3 1 1 C Parameter  Symbol  Number of Steps per Revolution  Srev  Peak Torque (Stall)  TPK  N  m  1.4123  Residual Torque  TR  N •m  2.82xl0~  Rotor Inertia  JM  Reference Voltage  E  V  22.5  Under-Load Current (Max.)  IMX  A  0.61  Units  Value  s  200  rev  kg  m  2  2  1.23x10"  4  Ultrasonic Proximity Sensor Data - Daytronic D4900  Sensing Range (Min.)  mm  101.6  Sensing Range (Max.)  mm  762  Accuracy (% range setting)  %  ±1  Hysteresis (% of set-point)  %  5  Response Speed  ms  50  mm  35  mm  160  Drive Data - Timing Belt  Sprockets Pitch Diameter Drive Range  YR  Table 6.2. Summary of the Y-Axis Drive Parameters  The manipulator actuators and sensors are powered by a 24 V D C , 12 A power supply. The power for the X-axis actuator is managed by a custom made linear push-pull amplifier. The amplifier design, (see appendix A), uses a positive input signal [0-10 V D C ] with an adjustable neutral point. Figure 6.3 provides both the exact and linearized characteristics of the amplifier. The major difference between the exact and the linearized characteristics is the omission of the saturation zones at low and high input voltage. The linear regression of the measured data omitting the saturation zones of the amplifier shows a perfect linear behavior (i.e., the regression coefficient R is unity). Therefore, avoiding the use of extreme amplifier input signal regions 2  justifies the assumption of a linear amplifier. Practically, the usable range of the amplifier can be  6.2 Experimental Platform  67  limited by imposing, in software, lower and upper limits on the signal generated by the control algorithm.  Figure 6.2. Camera Location in the Workcell  The Y-axis stepper motor is driven through an Allegro MicroSystems Unipolar Stepper-Motor Translator/Driver (#5804). This driver provides three excitation modes (i.e., one-phase drive, two-phase drive, and half-step drive) and requires as input enable and clock signals. The stepping mode can be modified on the fly, allowing for coarse/fine motion types. The clock signal frequency dictates the stepping speed and, therefore, provides an easy way to implement a PID-type control algorithm, while the three excitation modes enable the achievement of a sufficient level of positioning accuracy, or speed.  6.2 Experimental Platform  68  Camera Data - J V C TK-1070U Parameter  Units  Value  C C D Chip Size  in  2/3  Effective Pixels (Horizontal)  pixels  768  Effective Pixels (Vertical)  pixels  493  Scanned Pixels (Horizontal)  pixels  640  Scanned Pixels (Vertical)  pixels  480  Scanning Line  lines  525  Interlacing Ratio Acquisition Time  2 ms  30  Lens Data - Computar T V Zoom Lens 1:1.8/12,5-75  Aperture Ratio (Max.)  1:1.8  Focal Length  mm  12.5-75  Focus  m  1-00  Table 6.3. Summary of Camera and Lens Parameters  Figure 6.3. X-Axis Push-Pull Amplifier Characteristics  6.3 Open-Architecture Control Platform  69  6.3 Open-Architecture Control Platform In order to commission the Cartesian manipulator described in the previous section, a real-time control system is necessary. Furthermore, to servo the position of the end-effector and perform sensor integration in the lowest level of the control structure, an open-architecture control platform is necessary. A n open-architecture control platform provide the basic functions of a C N C system (i.e., preemptive tasking, kernel functions, timing, I/O functions, etc.) while still allowing the user to define the control loops and processing functions.  For the current application, the Open Real-Time CNC Operating System (ORTS) [5, 24, 25, 57,  58, 59], developed by the Manufacturing Automation Laboratory of the University of British Columbia, has been selected to implement the real-time functions of the overall architecture. ORTS has been developed for C N C control of machine tools, mechatronics devices control, and process-control system designers. Therefore, this package provides the flexibility and processing capacities required to implement the current application.  The real-time operating system is composed of two major components, a real-time microkernel (ORTS-DSP) and a PC-based scripting environment (ORTS-PC/NT). The real-time microkernel runs on one or more DSP boards plugged into a PC running an enhanced Windows N T operating system. The DSP handles all the real-time processing necessary to perform motion control, as well as I/O interfacing and communication with the PC through the ISA or PCI bus. The current application is developed on a Spectrum Signal Processing Indy F3 processor board [72, 73], equipped with the Texas Instrument TMS320C32 chip [76]. User defined processes are coded in ANSI C language [39], following ORTS specific guidelines and conventions, and registered in the kernel list of processes. The registration of the processes enable their compilation, their downloading on the DSP, and their use from the scripting interface.  I/O is handled by the DSP board through a DSP~Link2 or DSP~Link3 interface. Various I/O boards are available for these interfaces and compatible with ORTS. For this particular application, a motion control oriented I/O board is selected, the Precision MicroDynamics Inc. MFIO-3B [63]. A description of the ports provided by the multifunction I/O board is given by  6.3 Open-Architecture Control Platform  70  Table 6.4. Furthermore, to increase the flexibility of the hardware, screw terminal boards have been built to accommodate the MFIO-3B interfacing connectors.  Multifunction I/O board MFIO-3B Function  # of Channels  Quadrature Encoder Inputs  3  D/A(16bits)  3  A/D(16bits)  3  Digital I/O  24 bits  Table 6.4. MFIO-3B I/O Features The ORTS-PC/NT component of the system consist of the scripting environment in which the user can organize the process specific tasks that are to be implemented in real-time on the DSP platform. The ORTS scripting interface is shown in Figure 6.4. tOORTS  LLD_XaHis'J spl  Fie Ed* y«wtfmdowHelp •if?  B f LD_X«ii3 spi Link c a » _ y ( l ) ; //Link cst4(l); Link cam_x_fast(1); Link value_mean(1); Link value_var(l); / * Chock T i » i n g » / //F3_WRITE_DIG_COM5T(11.11.0); / » X axis Encoder Measurement"/ MFIO_ReadEncoder(1),output-(X_mm_value); /*X axis Feedback Gain to convert absolute position in counts to mm*/ / * —0.003175 mm/count, negative sign to make the counter sign to follow the axis d i r e c t i o n * / / * Gain added i n the MFI0-3B I n i t i a l i z e function, i n startup.spt*/ / * Read command s i g n a l , in mm, from somewhere*/ ELD_COMMAND_IHTERPRETER(). input-(x_com*and. X_»m _value). output-(x_set_point. DSPtoPC. trig,cam): //ELD_TRAJ_GENERATOR< 0.5,0),input -(x_set_point),output-(x_control): ELD CART_JRAJ1D(40,140). input-(x__set_point),output-(x_control); i  //E1D_CART_TRAJ2D(10,10,150,100,150.10,10,100), input-(x_set_point),output"(x_control,y_control / * test fusion process*/  iL * Messages :  J  Man Reading f3.ini file Given name: F3 Full board name: Indy F3 Processor Board Vendor name: Spectrum Signal Processing Inc. F3: CommandManager: Ready! T3: CommandManager: Inline functions disabled! startup.spt has 0 errorsl fa Help, press f i  Figure 6.4. ORTS-PC/NT Scripting Environment  6.4 Encapsulated Logical Devices Implementation  71  The ORTS-PC/NT and ORTS-DSP applications provide a transparent communication protocol inside of the real-time application itself, and also between the PC and DSP platforms. Transparent communication between the ORTS-PC/NT application and other processes running on the same PC, or on a networked PC, is also available through the Microsoft Foundation Classes Libraries pipe classes.  6.4 Encapsulated Logical Devices Implementation 6.4.1 The ELD Object As discussed earlier, in order to enable the development of structured, modular, and flexible industrial robotic applications, an efficient and coherent framework is necessary. The advantages gained through the use of such a framework have been extensively reviewed in an earlier chapter. For this implementation, the Encapsulated  Logical  Device  (ELD)[19, 20, 21]  architecture has been selected for its ability to manage both sensors and actuators in a control oriented fashion, as well as high-level and event driven functions.  The basic entity of the E L D architecture is the E L D object itself. A n E L D object is a highly functional driver that processes information or control signals according to its user defined nature. Each E L D object has a modular and self-contained common framework, as shown in Figure 6.5.  The three main components of the E L D object are the manager module, the sensor module, and the actuator module. Each module fulfills a particular set of tasks inside the E L D object and is discussed briefly below. More details on each of these modules, as well as their basic functions can be found in [21].  The sensor module of the E L D object contains knowledge and functions necessary to perform sensory data processing inside the object. Three types of functions are included in the sensing module: processing functions, data fusion functions, and input driver functions. The input driver functions are used to interface with physical sensors or other E L D objects in the hierarchical  6.4 Encapsulated Logical Devices Implementation  72  arrangement. Typically, these functions will translate the sensory data from its raw format to the format used in that particular object. Since an E L D object can have multiple inputs, many different input driver may be required in the same object.  COMMANDS AND REPLIES  RESPONSES AND REQUESTS  Encapsulated Logical Device Manager  REQUESTS  Knowledge Base  CONTROL S  L  G  N  A  L  SENSOR INPUT  COMMANDS  Figure 6.5. The Encapsulated Logical Device (ELD) Object (adapted from [21]) When multiple input sources are available for a particular E L D object, sensor fusion functions are used to merge the data and knowledge brought in by the different sensory sources. Again, different sensor fusion functions can be defined and used at the same time, depending on the particular application requirements. Processing functions are also present in the sensor module of the E L D object. These functions are independent of the fusion functions and can be invoked either before or after the fusion functions depending on the application. Typically, processing functions are used to perform processing on the sensory signal, modify its format to make it suitable to a higher-level E L D object, or make a decision based on the content of the signal.  The actuator module of the E L D object is directly responsible for controlling physically connected actuators. Each E L D directly connected to an actuator is used to implement a control loop. Therefore, two types of functions are included in that module, namely controller algorithms  6.4 Encapsulated Logical Devices Implementation  73  and output driver functions. The output drivers translate the command signal issued by the control algorithm into a format that can directly be written to an output port. This also implies that the output driver functions must address hardware communication issues.  Finally, the manager module of the E L D object is responsible for the implementation of the higher-level functions of the object. These functions include, but are not limited to, communication functions, local knowledge management,  message parsing, and message  interpretation. The communication functions are common to all the E L D objects in an architecture, while planning and interpreter functions are specified locally, even i f a common basis is also included.  6.4.2 Implementation of the ELD Architecture A n architecture based on E L D objects was designed and implemented by establishing hierarchical relations and communications channels in a group of task specific objects. The first step in the implementation of the experimental manipulator described in Section 6.2 involves establishing the processing and timing requirements of each particular type of object. Figure 6.6 illustrates all the sensors and actuators present in the system, as well as their functional arrangement based on their processing requirements and real-time behavior. The actuators must be controlled in real-time, at a high sampling rate. Therefore, all the low-level sensors and the actuators are assigned to the real-time control platform. The heteroceptive sensing source, the camera, is assigned to the high-level processing platform, mainly due to its high computational power requirements.  For the current application, the architecture is constructed from the bottom up. First, the lowlevel objects are defined in order to implement the low-level control loops and sensory processing functions. These E L D objects are represented in Figure 6.7 under the labels Encoder ELD, Ultrasound ELD, X-Axis ELD, and Y-Axis ELD.  74  6.4 Encapsulated Logical Devices Implementation  ENCODER SIGNAL  ULTRASOUND SIGNAL  X-AXIS MOTOR SIGNAL  Y - A X I S MOTOR SIGNAL  •r-r  CAMERA SIGNAL  High-Level PC Running NT  Low-Level PC Running NT  ! DSP to PC NT Pipe  MFIO [*A  DSP  !  Grabber  PC to DSP NT Pipe  Figure 6.6. Repartition of Processing Tasks for the X Y Table Implementation (adapted from [21]) The Encoder ELD and the Ultrasound ELD implement the sensor-related functions necessary to transform the raw data into a usable form for the controller algorithms. These E L D ' s are coded in the C programming language, following the ORTS-specified context. The scripts are provided in Appendix D, while the corresponding processes are provided in Appendix B . The manipulator drive control algorithms are implemented in the X-Axis ELD and the Y-Axis ELD. These two E L D ' s require a cross-platform implementation due to the nature of the system. The control algorithm, as well as a part of the sensing component algorithms, are implemented on the real-time platform, while the manager functions and the sensing functions dealing with the camera feedback are implemented under the high-level Windows N T platform. Again, the ELD functions implemented in the open-architecture control platfonn are written in C language and the corresponding code can be found in Appendix B. The components of these E L D objects implemented under the Windows NT platform are coded in object-oriented C  + +  and the  corresponding code can be found in [21].  The heteroceptive sensor feedback is generated through two different E L D ' s . In Figure 6.7, the first one is labeled Image ELD, while the second one is labeled Pointer Locator ELD. The Image ELD deals with issues related to the image acquisition by the hardware components of the vision  6.4 Encapsulated Logical Devices Implementation  75  system. The major functions of this E L D object include triggering of the camera on request by a higher-level E L D , and management of the data transfer from the framegrabber board to the host PC memory.  The Pointer  Locator  ELD presents a higher level of complexity, represented  on the  organizational graph of Figure 6.7 by being above the low-level sensing Image ELD. The main functions performed by this E L D object include the calibration of the camera image reference frame and the processing of the raw images to extract features of interest. In this case, the specific feature of interest is the position of the end effector in the Cartesian world frame.  Finally, at the highest level, the Planner ELD is responsible for the coordination of the overall architecture of ELDs and the generation of decisions regarding the nature of the motion to be performed by the X and Y-axes. Furthermore, in the current application, this E L D is responsible for interfacing with the Graphical  User Interface (GUI), which is used to configure E L D  specific parameters and generate motion commands.  Although the whole system has been designed, built, and implemented, in this work, only the X axis is used to evaluate the proposed methodology, and its associated control policy. The following section discuss in detail the establishment of the axis transfer function, in order to later pursue with the development of a control policy for that particular axis.  6.5 X-Axis Dynamics Identification  76  "Planner ELD  " H i g h Level Platform Implementation (NT) *Split Platform Implementation (NT and DSP)  'Pointer Locator ELD  "Encoder ELD  Ultrasound ELD  Image ELD  u  =1 Encoder  X Axis DC Motor  Y Axis Stepper Motor  Ultrasound Sensor  Camera  Figure 6.7. X Y Table Implementation of the E L D Architecture (adapted from [21])  6.5 X-Axis Dynamics Identification Simulation testing of the proposed integration and control policy requires the modeling of the system dynamics. In the current case, the system corresponds to the X-axis of the Cartesian manipulator described in the previous sections. The establishment of the manipulator model can be performed using different approaches. In this work, system dynamics (i.e., transfer function) identification using a least squares technique is selected as an appropriate and straightforward procedure.  6.5 X-Axis Dynamics Identification  77  The transfer function of a system can be identified using various means: impulse response techniques, step response techniques, frequency response techniques, either in continuous time domain or in discrete time domain [5, 22]. For computer-controlled system, the easiest way to identify the system transfer function parameters is to collect input and output data at a frequency equal to the computer control algorithm sampling period.  The general expression for the system transfer function can be found from a simple model. Spong [74] provides a simple representation for the drive dynamics where the actuator is a D C motor, see Figure 6.8.  Figure 6.8. DC Motor System Block Diagram Assuming that the electrical time constant of the motor is negligible with respect to the mechanical time constant of the motor,  R  B  , and incorporating the drive dynamics into m  J  the block diagram of Figure 6.8 enable a further simplification of the drive model, Figure 6.9.  6.5 X - A x i s Dynamics Identification  78  Figure 6.9. Simplified Drive Dynamics Block Diagram Since the system input and output are to be sampled using a computer-based acquisition system, it is necessary to convert the model to discrete-time domain. The conversion is made using the backward difference approximation, given by z-l  (6.1)  1T~  Neglecting the effects of the disturbance torque, r , d  on the system, the transfer function  associated with the block diagram of Figure 6.9 is given by W\z Wl  (6.2) ( e,f J  R  +  + T K,K )S  EMF  J Rz eff  One can note that, the transfer function is computed without the last integrator present on the block diagram o f Figure 6.9. The presence of the integrator tends to generate drift in the output measurements and affects the overall performance of the identification procedure. This problem is easily avoided by identifying the transfer function using the velocity as the output o f the system and adding the last integrator afterward. Nevertheless, since oj(z~*) is not directly available from a physical sensor in the system, the CT(Z~')signal is generated through the backward differentiation approximation of the position signal, given at the sampling instant k by  e(*)-eM)  CTW=  ( 6 3 )  6.5 X-Axis Dynamics Identification  79  The transfer function of equation (6.2) can be represented by a more general expression, referred to as an Autoregressive Integrated Moving Average (ARJMA) process in the literature, ^ o\ G  B(Z~ ) z - { b » + b ^ + b z - ^ r i = — : ij is 1  L  z  V  J  A\z  In (6.4), the z~  d  d  2  =  l + tf,z + a z  J  2 +  . . .+  b z-»i-) - „ — • ni  ( - ) 6  4  +... + a„ z "  2  term represents the time delays present in the system. The number of sample  periods corresponding to the duration of the delay can generally be measured directly from an input-output plot. In the current case, because of the nature of the sampling process, it is known that, at least, a delay of one sampling period affects the system. The order of both the numerator and denominator polynomials can be inferred from the simplified model established by equation (6.2). The identification problem can then be rewritten by the corresponding general expression,  V\z  )  \ + az {  and the corresponding difference equation, Gj(k) = b V(k-\)-a tu(k-l), 0  i  (6.6)  or  Gj(k)=[v(k-l)  Bj(k-l)\b  0  aj=(p{k)e.  (6.7)  Since tn{k) is a vector corresponding to the length of the sampled output data, the regression matrix (j)(k) is not square and the equation (6.7) cannot be solved directly. It is possible, however, to solve it using a least squares technique. Such a technique will provide the value of the transfer function parameters that minimize the sum of the square of the errors over the whole output vector. The use of the pseudo-inverse, instead of the inverse, provides an optimal solution under a least squares criteria. Rewriting the previous expression substituting in the pseudoinverse expression leads to: 0 = (<p <py<p aJ. r  r  (6.8)  When using an identification technique such as the one described above, one must be careful in the selection of the excitation signal. The excitation signal must inject enough energy in the  6.5 X-Axis Dynamics Identification  80  system in order to excite all the dominant modes of the system (i.e., poles), in order to make their identification possible. A step signal is used as excitation signal since its harmonic content is high and, therefore, will excite a wide range of system natural frequencies. To ensure a good sampling distribution, different step amplitudes are used as well as different directions of motion. Figure 6.10 provide a typical example of input signal used to excite the X-axis drive dynamics.  One can note that, the excitation amplitude is smaller then the saturation levels of the amplifier. Saturation of the amplifier would corrupt the measured data since the system could not be assumed to be of the Linear Time-Invariant (LTI) form. Also, from the amplifier characteristic presented in Figure 6.3, it can be seen that a 5 V D C offset must be added to the voltage command signal to respect the amplifier design. This offset is added by the  Digital-to-Analog  Converter (DAC) before the voltage command values are written to the amplifier.  Dynamics Identification Excitation Waveform I  i  I  I  Voltage Command [V]  i  !  U;  1  0  0.2  1  0.4  1  0.6  0.8  1  1.2  1.4  1  1.6  1  1.8  2  Time [s]  Figure 6.10. Excitation Waveform for Dynamics Identification Applying the voltage command represented in Figure 6.10 to the X-axis of the system and measuring the position of the end-effector of the manipulator along the X-axis generates the results presented in Figure 6.11. The data presented on this figure represents the average of a  6.5 X-Axis Dynamics Identification  81  series of ten consecutive repetitions of the experiment. Also, as specified earlier, the sampling rate is selected to be the X-axis drive position servo-loop update rate (i.e., 400 Hz). The plot shows the effect of the velocity to position integrator, causing the steady state position error observed at the end of the excitation sequence.  Figure 6.12 presents the velocity profile calculated from the first time derivative of the position signal when applying the voltage command of Figure 6.10 to the system. Also represented on this figure is the computed velocity obtained from applying the same voltage command signal to the identified transfer function model. The two signals demonstrate fairly good time correlation as well as approximately equivalent amplitudes.  Figure 6.11. X-Axis Dynamics Identification Position Measurement The transfer function simulated in Figure 6.12 is given by tn\z' V\z-')  41.322 lz" l-0.7288z"'  (6.9)  Adding the velocity to position integrator in discrete time domain for a sampling frequency of 400 Hz leads to the open-loop transfer function of the plant at hand,  6.6 Summary  82  0.1033z~ Viz' )  1-1.7288z~' + 0.7288z  1  (6.10)  -2  This last transfer function will be used in Chapter 7 to perform stability analysis on the closedloop system, including the dynamics of both multirate heteroceptive sensor integration policies. Also, this transfer function will be used in the construction of a dynamic simulation of the system behavior when submitted to a sensor fault or failure.  Simulated and Measured Velocity Responses 500  Measured Velocity Simulated Velocity  300  °  4  0  I X  -300 -400 _5QQI 0  I 0.2  I 0.4  I 0.6  I 0.8  I 1  Time [s]  I 1.2  I 1.4  I 1.6  L 1.1  Figure 6.12. Simulated and Calculated from the First Position Derivative Velocities  6.6 Summary In this chapter, an extensive description of the experimental manipulator has been provided. The Cartesian manipulator has been designed, built, and tested in the Industrial Automation Laboratory. In the current work, only the D C motor/lead screw driven X-axis is used. The electro-mechanical system is operated through an open-architecture C N C control system, the Open Real-Time CNC Operating System (ORTS).  6.6 Summary The system architecture is designed following the Encapsulated  83 Logical  Device (ELD)  framework, before being implemented across multiple computing platforms. The real-time lowlevel tasks are implemented on a Digital Signal Processor (DSP) under the ORTS operating system, while the event-driven high-level tasks are implemented under Windows N T operating system. This system configuration allows for an efficient and structured treatment of the information available in a multisensor system, as well as the management of control-oriented processes.  Finally, to complete the description of the experimental platform, a model for the X-axis drive dynamics has been established. The model is based on a least-square identification technique. A parametric model is first established and its parameters are identified from input/output data measured on the system itself. This model of the X-axis drive dynamics is used in Chapter 7 in order to perform simulation testing of the proposed methodology and their corresponding policies.  Chapter 7 Stability & Dynamic Simulations 7.1 Preamble This chapter addresses the issues related to the closed-loop stability of the low-level servo loop of robotic manipulators under the proposed policies. The model-based policy is considered first. An expression for the Closed-Loop Characteristic Equation (CLCE) is developed based on the methodology established in Chapter 3 and the recursive Kalman filter-based predictor transfer function developed in Chapter 4. This section is followed by a parametric stability analysis of the characteristic equation. The effects of different parameters on the position of the closed-loop poles are analyzed and suitable values for the parameters are determined.  In order to verify the validity of these parameters, a dynamic simulation of the complete system under the model-based policy is constructed and run in the MatLab Simulink [69] environment. The assumptions necessary to establish the simulation are described and simulation results are presented. The purpose of the simulation is to show the feasibility of the proposed policy and the validity of the proposed methodology as a means to minimize the effects of proprioceptive sensor faults, or failures, on the dynamic behavior of the system. In this work, the behavior of both multirate heteroceptive sensor data integration policies is examined in the presence of one 84  7.2 Model-based Policy  85  particular type of fault, namely, the complete hardware failure of the proprioceptive sensor. However, the objective of this work is to develop a policy able to handle any type of proprioceptive sensor fault, where the proprioceptive sensor becomes increasingly uncertain with respect to the uncertainty displayed by the heteroceptive sensor. As discussed in Section 7.2.4, the simulated performance of the model-based policy is observed to be marginal, mainly due to the slow response of the uncertainty quantification metric and the presence of unstable closedloop transfer function poles at the minor time-step. Instead of further work towards the enhancement of the uncertainty quantification metric, a second policy was developed to function with the current metric, as the focus of this work is aimed at the development of the sensor integration policies.  Section 7.3 addresses the stability and dynamic simulation issues related to the second proposed policy, namely the polynomial policy. Again, based on the multisensor integration methodology presented in Chapter 3 and the formulation of the Newton-Gregory polynomial interpolation algorithm as a transfer function presented in Chapter 5, the closed-loop transfer function of the system is established. Stability requirements are formulated for two different implementation cases, Fault Tolerance and Fault Rejection. For both cases, a parametric stability analysis is performed and the parameters are established in order to ensure the stability of the CLCE.  The dynamic simulation of the system under the polynomial policy is then constructed, following the results of the parametric stability analysis. Simulation results are provided for both implementation cases and for different types of proprioceptive sensor faults, or failures. The chapter concludes with a review of the major conclusions from the simulation work presented herein.  7.2 Model-based Policy 7.2.1 Development of the Closed-Loop Characteristic Equation As discussed in Chapter 4, the use of a Kalman filter-based policy leads to the establishment of a two-step fusion scheme, in which different feedback sources are used on the major and minor  7.2 Model-based Policy  86  time-steps. Due to the periodic nature of the resulting system, it is possible to study the stability for the major time-step closed-loop system independently of the minor time-step system.  It is possible to rearrange the block diagram of Figure 4.2 in order to independently address each of the two fusion schemes included in the model-based policy. Substituting the HGRF fusion process by the equivalent weighted average expression for the fused mean and removing the regulation Kalman filter loop leads to the major time-step block diagram of Figure 7.1.  -r)  PROCESSING  PREDICTOR  DELAY  I  D_ATA_FlJS10N_  D  >  D I G I T A L P I D  k+\  +  1 l  'k + \  A  A  PLANT  D A T A FUSION .  Figure 7.1. Major Time-Step Equivalent Block Diagram The fusion gains, G and G , represent the weight of each signal in the fusion process and are 2  3  computed accordingly to equation (3.1) in a normal on-line implementation. This particular representation provides a simple and convenient way to integrate the fusion process in the closed-loop stability evaluation. It is also important to note that, for stability analysis purposes, the fusion gains are imposed on the system and not established through the computation of the sensor signal uncertainties, as it would be done in an implementation. Finally, as mentioned earlier, these gains are mutually exclusive (i.e., G + G = 1). 2  In Figure 7.1, a digital Proportional-Integral-Derivative  3  (PID) controller is used in order to  correct the dynamics characteristics of the X-axis drive and improve tracking performance. The discretization of the continuous-time expression for the PID controller is provided in Appendix F and its transfer function is repeated below for convenience. The discrete-time implementation of  7.2 Model-based Policy  87  the PID controller includes a low-pass filter on the derivative term in order to minimize the effects of high-frequency noise on the system.  The controller transfer function is given by -K,dpole-2K )z~'  v(z~ ) _ JK +K,+ K )+{{-dpole-\)K x  P  D  P  D  I+ (-dpole -\)z~  e(z )  ]  _1  + {K dpole + K )z' P  D  +z~ dpole 2  -,(7.1)  where,  dpole •  (7.2)  1  is the discrete-time pole implementing a low-pass filter with cut-off frequency f  co  [Hz].'-K , P  K,, and K are the discrete-time controller gains evaluated from the continuous-time gains D  through the following relations, 7; K  P  ~  K  K  I  -  ~ h:  (7.3)  s '  (7.4)  K  P c  K  I c  T  and K  n  (7.5)  = ^  The construction of the closed-loop characteristic equation requires the definition of the Kalman predictor. The predictor is based on a system model established from the identified drive axis dynamics, to which a unitary feedback loop is added. Using the discrete-time transfer function given in equation (6.10), the following minimal realization closed-loop, unitary-feedback, statespace model is generated using MatLab for a 400 Hz sampling rate: " 2.0371 -0.7952 A=  1.9707 " -0.4116  0.4050 0.0321  C = [0.2436  0.1446],  (7.6)  (7.7) (7.8)  7.2 Model-based Policy  88  and D = [0].  (7.9)  Using this state-space model in the one step-ahead predictor transfer function of equation (4.23) defines the Kalman predictor. Nevertheless, in order to express all the components of the closedloop system in the same time frame, it is necessary to express the major time-step time-correlator transfer function in the controller time frame. Since it is assumed that the processing delay Can be represented by an integer number n of controller update periods, it is necessary to develop the one major time-step ahead Kalman predictor as a A? minor time-steps ahead predictor.  Using the same approach as presented in Section 4.4, the Kalman predictor of equation (4.23) becomes  (z-h * " f H f xA-**™w^+DU^). c  Q  V  '  \l-0 \l-KCh~ \ n  (7.10)  l-O \l-KC\z- \  n  n  n  Substituting equations (7.6) to (7.9) in (7.10) leads to scalar numerators and a denominator composed of a matrix of transfer functions. Following [13], taking the determinant of this matrix reduces the denominator. Furthermore, the stationary Kalman gain vector is computed using the MatLab Control Toolbox discrete-time system Kalman estimator design function, dlqe, for given noise characteristics. The estimation of the optimal stationary Kalman gains requires the definition of the noise coupling matrix, T, as well as the system noise covariance matrix, Q, and the measurement covariance matrix, R.  As established in Chapter 4, the noise covariance matrices are used in order to implement the heuristic tuning of the filters. From preliminary testing, it has been found that the noise characteristics 2500  0  0  2500  (7.11)  and /? = [O.OOl],  (7.12)  provide good time-correlation behavior, when the noise coupling matrix is assumed to be the identity matrix in continuous time domain. This noise coupling matrix generates an even  7.2 Model-based Policy  89  coupling of the noise on all the system states. The necessary discretization of this matrix is performed, following [18], using the MatLab continuous-time model to discrete time model conversion function, c2d, and leads for a controller sampling frequency of 400 Hz to:  r=  "0.6899  -0.1162"  0.7090  0.9296  (7.13).  As discussed in Chapter 6, due to the processing delay necessary to extract the end-effector position from the raw camera signal, the heteroceptive feedback rate is limited to rates under 5 Hz. Assuming that the major time-step period corresponds to the heteroceptive sensor processing delay, the time-correlator prediction horizon is found from the ratio of the sensors sampling rate (i.e., n=80). Thus, using the prediction horizon expressed in the minor time-step time-frame, it is possible to compute the values of the Kalman gain that produces an optimal state estimate. The use of the MatLab command dlqe{o" ,T,C,Q,R) K =  leads to the stationary Kalman gains "2.0640"  (7,14)  3.4390  and equation (7.10) is reduced to (7.15) where I.282xl0" z~ 6  M  • 7.464x10~V  80  80  (7.16)  -17 -.60  +4.831x10  z  and N  0.10332 1 - 7.464xl0 z -6  - 8 0  -80  + 4.83 l x l O  -17  z'  m  (7.17)  The algebraic formulation of the feedback equation is provided by,  V\z)  e\  (7.18)  and performing the necessary substitution leads to the closed-loop transfer function for the major time-step system, given by,  7.2 Model-based Policy  90  0z" 9z"  v  U[z~  \  ) f  z  (7.19)  =  The same procedure is applied to the construction of the minor time-step system closed-loop transfer function. Figure 7.2 illustrates the equivalent minor time-step block diagram, where the fusion process is again expressed as a weighted average. The fusion gains, G, and G , represent 2  the relative weight of each signal in the fusion process and, for simulation purposes, these gains are imposed in order to evaluate the stability of all the possible sensing subsystem configurations.  P -n+1  D I G I T A L P I D  0  1 z  0 fM  0.1033z  ^+.k  1  0 P R E D I C T O R  A  N  I  _1  l-1.729z +0.7288z~ -l  /(«+O  L  2  T I +1  1  3*1 f(n+\\n  ' H  G  1  H  D A T A  F U S I O N  Figure 7.2. Minor Time-Step Equivalent Block Diagram The Kalman predictor of Figure 7.2 is designed to act as a regulator, and to propagate the effect of the heteroceptive sensor measurement in the system. This is accomplished by predicting the major time-step fused feedback measurement one minor time-step ahead to referee the following proprioceptive measurements. The state-space model provided by equations (7.6) to (7.9) is used again for the minor time-step, but this time the noise characteristics are different to account for the regulator nature of this filter. From preliminary simulation testing, it was determined that the Kalman predictor based on the noise characteristics given by "40 Q  0  0" 40  (7.20)  7.2 Model-based Policy  91  and * = [10],  (7.21)  provided sufficient regulation. The same noise coupling matrix is assumed and the stationary Kalman gain vector is found again using the discrete-time systems Kalman estimator design tool provided by the MatLab control toolbox, dlqei<&,r,C,Q,R). The Kalman gain vector is found to be 3.1238  K =  (7.22)  -0.5938  The one step-ahead Kalman predictor transfer function is then given by equation (4.23), where the D matrix is set to zero according to the state-space model established earlier,  Performing the necessary substitutions and solving the denominator of equation (7.23) through the use of the determinant leads to the expression of the Kalman predictor as a transfer function, e (z- )=Me (z- )+Nu(z- ), ]  l  f  (7.24)  ]  f  where 0.9412z~' M - l. - 0 . 6 8 4 3 z , +0.2368z~ , _l  2  (7-25)  and N  01033^  m  l-0.6843z"' +0.2368z  2 -2  Solving algebraically the feedback loop leads to an expression of the form  e H ^ . ^ . ^ - e ^ - ' ) ) ,  (7.27);  where substituting the necessary terms and rearranging provides the closed-loop transfer function for the minor time-step system,  7.2 Model-based Policy  92  elzU(z'  e(z- ) ]  e(z- ) ]  {  (l-G.M)  1+  (7.28)  l-G,M  Equation (7.28), and equation (7.19) are used in the next subsection in order to evaluate the stability of the closed-loop system, for a given set of controller gains. In order to achieve stable behavior of the system response, it is necessary to achieve sufficient stability for both the major time-step and the minor time-step closed-loop systems.  7.2.2 Stability  Analysis  The stability analysis for the model-based policy for heteroceptive feedback integration in robotic servo-loops is performed based on the assumption that a controller providing sufficient tracking performance is used. It is desired that the policy be stable for all the sensing subsystem configurations for both the major and the minor time-step systems, but also that the policy maintain a certain level of tracking perfonnance. This is quantified by the control bandwidth of the system, which should remain fairly close to the original position servo-loop value. The base  ;  line stability of the original position servo-loop is evaluated, in order to be Used as a comparative value, or performance criteria, for the policy implementation. The basic position servo-loop of the experimental manipulator is represented by Figure 7.3.  DIGITAL  V(z)  PID  D R I V E D Y N A M I C S  e(z)  Figure 7.3. Basic System Position Servo-Loop Block Diagram The corresponding closed-loop transfer function is  7.2 Model-based Policy  93 Hz) 0(z)_  e(z)  e(z) V(z) e(z) V{z)  The digital PID controller transfer function is known from equation (7.1) and the plant dynamics transfer function is given by equation (6.10). Using the controller gains found from experimental tuning (K  Pc  = 0.4, K  Ic  - 0.3, and K  Dc  = 0.001), and a cut-off frequency of 40 Hz, the closed-  loop transfer function is solved for the poles and zeros. The gains are selected through iterative tuning in order to provide sufficient tracking performance and minimal steady-state error.  Figure 7.4 presents the pole-zero plot for the position servo-loop transfer function, assuming that the tuning parameters introduced above are used. All the poles and zeros appear to be contained by the unit circle boundary, indicating that the servo-loop is stable. Table 7.1 presents the position of the poles and zeros for the manipulator position servo-loop, confirming that all the poles are inside the region of the complex plane formed by the unit circle boundary. Finally, Figure 7.5 presents the Bode diagram of the servo-loop. The Bode diagrams of the position servo-loop clearly show the effect of the first order low-pass filter added on the derivative term of the digital PID control law. Shortly after the cut-off frequency used to generate these Bode diagrams (i.e., 40 Hz or 251.33 rad/sec), the system gain magnitude starts decreasing, attenuating the effect of the high-frequency components on the system response. Servo-Loop Zeros and Poles Zeros  Poles  0  0.8748  1.0000  l.0000±0.0000j  1.0000  0.6937 ± 0.1773j  0.9981  0.9981  0.7288  0.7288  0.6141  0.6141  0.8079  -  Table 7.1. Servo-Loop Poles and Zeros Position in the Complex Plane  7.2 Model-based Policy  94  Robotic Manipulator Position Servo-Loop Poles and Zeros i  r-  I  i  0.6h 0.4 [  .» 0.2  i: |  H ,  o|-  ®  ®  O x  0  CU  I -0.21-  -0.6 k  -0.8 r  -1  -0.8  -0.6  -0.4  -0.2  0 Real Axis  0.2  0.4  _l 0.6  L.  0.8  1  Figure 7.4. Robotic Manipulator Servo-Loop Poles and Zeros  Robotic Manipulator ServoLoop Bode Diagrams 10 .—. CQ  i  •  : i  I  •t  i  0  2, <B  10  £.  20  10  30 : :i  40 50  • • i  • >  0  d> w  (0  100  Q_  150  : : i 10°  200  10'  : i  1  10  10  !  Frequency [rad/sec]  Figure 7.5. Robotic Manipulator Servo-Loop Bode Plot  3  7.2 Model-based Policy  95  Table 7.1 and Figure 7.4 define the base-line stability of the robotic manipulator closed position loop, where no heteroceptive feedback is used. The closed-loop stability of the proposed modelbased policy can then be compared against this base-line case. The major time-step closed-loop system is analyzed first. Using the tracking controller described above and a range of fusion gains covering all of the different sensing-subsystem configurations, the poles and zeros of equation (7.19) are computed and plotted in the complex plane. Table 7.2 provides a summary of the different sensing subsystem configurations tested and the corresponding controller gain values.  Case  Controller Gains  n Pc  K  1  K  lc  Fusion Gains G  G  2  Figure  3  #1  80  0.4  0.3  0.001  0  1.0  7.6(a)  #2  80  0.4  0.3  0.001  0.25  0.75  7.6(b)  #3  80  0.4  0.3  0.001  0.50  0.50  7.6(c)  #4  80  0.4  0.3  0.001  0.75  0.25  7.6(d)  #5  80  0.4  0.3  0.001  1.0  0.0  7.6(e)  Table 7.2 Major-Time Step Stability Analysis Sensing Subsystem Configurations Figure 7.6 presents the pole-zero maps for the complete range of fusion gains corresponding to the major time-step sensing subsystem configurations. The representation of the single major time-step-ahead predictor as an n minor time-step-ahead predictor enables the solution of the closed-loop transfer function poles and zeros, but generates a very high order system. The use of a second order system model in the one-step-ahead time-correlator design leads to the addition of two poles in the system. Using the same model in an w-step ahead time-correlator leads to the addition of 2n poles in the system. In this case, the sampling rate ratio n is quite important (i.e., n=80), resulting in the high number of poles present on these maps. In a real implementation, the predictor is designed as a one major time-step ahead predictor using the corresponding sampling rate and the state-space model discretized accordingly.  7.2 Model-based Policy  96  Model-Based Policy Major Time-Slep Poles(+) and Zeros(o) [G =0.0]  Model-Based Policy Major Time-Slep Poles(+) and Zeros(o)[G =0.25)  2  „ rtO ° O O O OOr,  Real Axis  Real Axis  (a)  (b)  Model-Based Policy Major Time-Step Poles(+) and Zeros(o)[G =0.5j  Model-Based Policy Major Time-Step Poles(+) and ZerosfoHG^O./S]  2  0 +  o iScjw^r *^ o ?  -0.6  -0.-1  -0.2  0  -O.fl  OA  02  Pp.oooop.o?  -0,-1  Real Axis  Real Axis  (c)  (d)  0.2  04  0G  Model-Based Policy Major Time-Step Poles{+) and Zeros(o)[G,=1.0]  ;•••+•••  CO  o  j? -o.21  i -0.4  -0.2  0  02  0.4  Real Axis  (e) Figure 7.6. Major Time-Step Pole-Zero Maps for Different Fusion Gains  0.8  7.2 Model-based Policy  97  Nevertheless, for all the sensing subsystem configurations shown on the maps in Figure 7.6, the poles, as well as the zeros, are all inside of the unitary circle boundary, which ensures the stability of the system under the conditions previously specified. The case illustrated by Figure. 7.6(e) is particular, since only the proprioceptive sensing device is used to generate the feedback signal (i.e., G = 1.0) and, therefore, this case corresponds to the original position servo-loop. In 2  reality, due to the nature of the fusion gains, it is unlikely that the fusion gains will reach either of their limit values (i.e., 0 and 1) since this would imply that one sensor presents a null uncertainty (i.e., perfectly certain measurements). This would be inconsistent with the physical nature of the sensor itself or the sensing process. Thus, Figure 7.6(a) and (e) illustrate the limit cases of the sensing subsystem configurations.  It is also clear from the pole-zero maps presented in Figure 7.6 that the proposed policy does not provide the same level of stability as the original system servo-loop. For any sensing subsystem configuration, many poles are located outside of the region of the complex plane formed by the original system servo-loop poles. It can be shown that the reduction of the controller gains defined in Table 7.2 slightly increases the stability of the system. However, since the increase of the closed-loop stability caused by a reduction of the controller gains is marginal, it is reasonable to pursue the analysis using the gains defined in that Table 7.2.  It is also possible to increase the bandwidth of the time-correlator by increasing the systemnoise-magnitude to measurement-noise-magnitude ratio (i.e.,  mag{[R})  ). This forces the Kalman  filter algorithm to weight its estimate more strongly on the realization sequence than on the model-based estimation computed in the prediction algorithm. In this case, it is possible to demonstrate that the effect of increasing the noise magnitudes ratio is to move the poles of the inner ring, apparent on Figure 7.6(a) to (d), towards the origin of the complex plane, while the outer ring poles are not noticeably affected by this noise characteristics change. The effect of increasing the noise-magnitude ratio on the system stability will be studied in more detail in simulation, since the effects of moving the inner ring poles toward the origin of the complex plane should be easier to evaluate.  7.2 Model-based Policy  98  The same type of analysis is also performed on the minor time-step closed-loop system. It is desired that the minor time-step system not only be stable, but also be as stable as the major time-step system in order to limit the effects of transitioning from one system configuration to the other at every major time-step. The same controller gains used for the major time-step stability analysis are used here since both fusion schemes are implemented within the same system design. Again, the stability is analyzed for all possible sensing subsystem configurations, following Figure 7.2. A summary of the configurations analyzed is provided by Table 7.3.  Case  j  Controller Gains  n K  1  P c  K  Ic:  Fusion Gains K  D c  #6  80  0.4  0.3  #7  80  0.4  0.3  0.001  #8  80  0.4  0.3  #9  80  0.4  #10  80  0.4  0.001  Gi 0  |  G  Figure  2  1.0.  7.7(a)  0.25 ,  0.75  7.7(b)  0.001  0.50  0.50  7.7(c)  0.3  0.001  0.75  0.25  7.7(d)  0.3  0.001  1.0  0.0  7.7(e)  Table 7.3. Minor Time-Step Stability Analysis Sensing Subsystem Configurations Figure 7.7 presents pole-zero maps obtained for the stability cases described in the table above. Case #6, presented in Table 7.3, corresponds to a system where only the proprioceptive sensor is used. Therefore, Figure 7.7(a) represents the poles and zeros for the original position servo-loop.  It can be observed from Figure 7.7 that, as the proprioceptive sensor weight in the H G R F fusion (i.e., G ) process decreases, a pair of complex conjugates poles, as well as a zero, move outside 2  the unit circle boundary, generating an unstable system behavior for that particular sensing subsystem configuration. It is found that the last stable sensing subsystem configuration is generated for a fusion gain of G = 0.45. 2  7.2 Model-based Policy  99  Model-Based Policy Minor Time-Step Poles(+) and Zeros(o)[C^=1.0]  Model-Based Policy Minor Time-Step Poles(+) and Zeros(o)[G =0.75]  - i •  e:  to  1 "°  •  ;>+••>-  . . . + ...  3  a a  -0.2  0  •  + • :  -1  -0,8  -0 6  -OA -0.2  0  02  0,4  06  -0,6  Real Axis  5  +  0.2]  $  -0.4  -0.2  0  02  04  Ofi  Ofl  Real Axis  (a)  (b)  Model-Based Policy Minor Time-Step Poles(+) and Zeras(o)[C^=0.5]  Model-Based Policy Minor Time-Step Poles(+) and Zeros(o)[(^=0.25)  e + -:  03  $  0,  » a< O)  ro E -o.a  e  -0.5 -0.7  -0.6  -0.9  -o.e  -0.6  -0.4  -0.2  0.2  0  0.4  Real Axis  o.e  1.3 -1.1 -0.9 -0.7 -0.5 -0.3 -0,1  0.1  0.3  0.5  0.7  0.9  1.1  Real Axis  (c)  (d)  „  0.5k  I «| £• 0,1 ra c -0.1 & ro -03  ^  -0,  -1.7 -1.5 -1.3 -1.1 •  -0.7 -0.5 -0.3 -0.1  0.1 0.3 0.5 0.7 OB 1.1 1.3 1.5 1.7  Real Axis  (e) Figure 7.7 Minor Time-Step Pole-Zero Maps for Various System Configurations  1.3  7.2 Model-based Policy  100  Again, it is possible to vary the controller gains, as well as the noise characteristics, in order to verify i f the unstable poles and zeros generated for low proprioceptive sensor weights can be constrained inside of the unitary circle boundary. In this case, it is not possible to find a particular set of parameters leading to a stable closed-loop behavior for all the sensing subsystem configurations. This presents a major drawback in the use of such a technique in order to propagate the effect of the heteroceptive position update in the system, since it appears that in a proprioceptive sensor fault or failure case, it is most likely that the minor time-step fusion scheme will cause the system to become unstable. This last observation is confirmed by simulations in the following subsections.  At this point in the work, a question arises regarding the effect of the prediction horizon (i.e., n) on the stability of the closed-loop system. The time-correlator prediction horizon directly affects the number of poles of the closed-loop characteristic equation. This can be seen in equation 7.19 where the order of the M-term is dictated by the prediction horizon n. This affects the major time-step system poles and zeros position, has shown in Figure 7.6. For a given value of the encoder fusion gain, G2, it can be shown that the poles are drawn toward the origin of the complex plane as the sampling rate ratio is decreased. On the other hand, the minor time-step system stability is not affected by a reduction of the prediction horizon. In equation 7.28, the closed-loop characteristic equation is shown not to be a function of the prediction horizon. Therefore, it is not possible to ensure the stability of the minor time-step closed-loop system for all sensing subsystem configurations through a reduction of the sampling rate ratio.  7.2.3 Construction  of the Dynamic  Simulation  Subsection 7.2.2 presented the stability analysis of the closed-loop system corresponding to the implementation of the model-based policy to increase system tolerance, or robustness, towards proprioceptive sensor fault. In order to corroborate the results obtained from the stability analysis, a dynamic simulation was built in MatLab Simulink. In particular, a discrete-time dynamic simulation of the system response to a given input signal and under the influence of a proprioceptive sensor fault is examined.  7.2 Model-based Policy  101  First, the plant transfer function (i.e., equation (6.10)) was implemented in a discrete-time Simulink block and the other processes (e.g., Kalman predictors, HGRF fusion, uncertainty quantification, etc.) are implemented as S-Functions  [69]. As discussed earlier, two  implementations of Kalman predictor are necessary, a time-correlator and a regulator implementation. The Kalman filter S-functions were built following the derivations presented in Chapter 4 and Appendix E. The implementations only differ by their sampling rates, state-space models, and noise characteristics, which are left as parameters.  A particular implementation of the HGRF fusion process, as presented in Chapter 3, is necessary to account for the two-step fusion scheme adopted for the model-based policy. The fusion process is coded as an S-function using three input signals, each one composed of the sensor measurement and its uncertainty, as quantified by the uncertainty metric. A timing routine is included in this S-function in order to select the appropriate input signal to use for the generation of the fused feedback signal, based on the current time-step. Only two input signals are used to generate the fused feedback signal at any particular time-step. On the major time-steps, the timecorrelated heteroceptive measurement is fused with the proprioceptive sensor measurement. On the minor time-steps, the proprioceptive sensor measurement is fused with the one step ahead prediction of the previous fused measurement, as generated by the regulator Kalman filter.  The uncertainty of all three feedback signals used by the HGRF fusion process is quantified through the use of the uncertainty metric described previously. As explained previously, the uncertainty of each signal is taken as the variance of the signal with respect to the deterministic input of the system over a time window of finite length, to which the basic sensor uncertainty is added. The basic levels of sensor uncertainty used in these simulations are provided by Table 7.4, established from conservative estimates from physical observations.  For simulation purposes, a typical robot set-point signal is selected. This set-point signal is represented by Figure 7.8 and consists of a ramp signal in joint space coordinates. Since it is desired to evaluate the performance of the policy in the presence of a proprioceptive sensor fault, it is necessary to simulate the fault occurrence. The fault is generated through the addition of a  7.2 Model-based Policy  102  variable gain on the optical encoder feedback path and a step signal is used to toggle the value of the gain between one and zero at any given moment of the simulation.  Basic Sensor Uncertainty Sensor  Uncertainty [mm  2  Optical Encoder  0.001  Camera  0.125  \  0.2  Regulator  Table 7.4. Basic Uncertainty Level for Model-Based Simulations Sensors  Model-Based Policy Simulations Set-Point Signal c i  i  i  i  i  1.8  tion [mm]  1.6  '7k  A  o  1  Q_  w  £ X  0.8 0.6  0.4 0.2 03  05  t  1  i  1.5  Time [sec]  i  2  i  2.5  i  3  3.5  Figure 7.8. Model-Based Policy Simulations Set-Point Signal Figure 7.9 illustrates the effect of the fault occurrence on the encoder feedback signal. Before the fault occurrence, the feedback provided by the optical encoder follows the ramp pattern imposed by the system set-point. The fault occurs 2.5 seconds after the beginning of the simulation and the encoder feedback path gain is switched from one to zero. This results in the encoder signal being absorbed by the null gain, generating a null feedback value while the real position of the  7.2 Model-based Policy  103  system is not null. The following subsection presents the simulation results, as well as an evaluation of the uncertainty metric perfonnance for this particular implementation.  Encoder Feedback Signal Under Fault Occurence  1.4  •  i  i  i  3.5  4  1.2  1 E,  8  0.8  •a JD  O) OJ  u. ai 0.6 "D O O  c  UJ  0.4  0.2 )  i  0.5  1  i  1.5  0  2 2.5 Time [sec]  3  4.5  (  Figure 7.9. Encoder Feedback Signal Under Hardware Fault Occurrence  7.2.4 Simulation Results The first series of simulations is performed using the Kalman filter noise characteristics presented in subsection 7.2.2, as well as the sensor basic-uncertainty levels provided by Table 7.4. Table 7.5 presents the parameters used for this series of simulations.  Simulation  it  Controller Gains  Fault Occurrence Time [sec]  Figure K  D c  #1  80  1.4  0.4  0.3  0.001  7.10  #2  80  1.4  0.04  0.03  0.0  7.12  #3  80  1.4  0.004  0.003  0.0  7.12  Table 7.5. Model-Based Simulations Parameters for Table 7.4 Uncertainty Levels  7.2 Model-based Policy  104  Figure 7.10 presents the response of the simulation case #1 system to proprioceptive sensor fault occurrence. It is clear from the plot that the response is unstable. This plot also provides the fused feedback measurement, on which it is possible to see the effect of the heteroceptive data on the system. The fused feedback signal shows spikes at every major time-steps due to the injection of the heteroceptive data in the weighted average. It can also be inferred from this plot that the time-correlator takes three major time-step periods to correctly identify the new position of the system. This delay is due to the nature of the Kalman filter process itself, which actually retards the identification of the fault occurrence by the heteroceptive sensor.  Once the fault occurrence is finally identified by the heteroceptive sensor, 2 seconds after the beginning of the simulation, the recognition of the real system position by the heteroceptive sensor as being far from the set-point signal causes the rapid growth of the uncertainty associated with the camera measurement. The fast moving, unstable system response combined with the definition of the uncertainty metric results in the camera eliminating itself from the fusion process, even though it identifies the correct position of the system. This demonstrates a drawback related to the use of an uncertainty metric utilizing the set-point signal as reference signal in the quantification of the uncertainty. This type of metric can only allow for the identification of the fault occurrence before the system has moved off its prescribed path, when the faulty-sensor uncertainty grows faster than the system error. In this case, the lag associated with the time-correlator does not allow for the rapid identification of the fault occurrence by the uncertainty quantification process used on the heteroceptive feedback path, and the policy fails.  Another point that can be inferred from Figure 7.10 is that the time-correlated camera feedback is not propagated throughout the whole major time-step period by the regulator. The regulation effect is apparent from the shape of the spikes observed in this figure, namely, a rapid rise due to the injection of the heteroceptive data and a slower decay to return to the initial trend. However, the propagation is not strong enough to spread the decay of the heteroceptive data over the whole major time-step period.  7.2 Model-based Policy  105  Simulated Position Response and Fused Feedback Measurement for Case #1 Set-Point Response Fused Feedback :  I I  •S3  0  0.5  1  1.5  Time [sec]  2  2.5  3  Figure 7.10. Simulated Position Response and Fused Feedback Measurement for Case #1  Figure 7.11. Sensors Feedback for Simulation Case #1  7.2 Model-based Policy  106  Figure 7.11 presents the feedback signals generated by the different sensors present in the simulation. On this figure, the occurrence of the fault on the encoder signal can clearly be seen as the signal suddenly goes to zero at the time of the fault occurrence, while the camera signal indicates the real position of the system, once the camera measurement has propagated through the time-correlation algorithm.  It can be seen that the Kalman filter regulator is actually filtering out the heteroceptive sensor data, providing a feedback value corresponding to its internal model of the system. This is mainly caused by the fact that the model of the system used by the Kalman filter is not updated on fault occurrence, while the real system model is changing. In order to avoid the problems related to this situation, either an adaptive Kalman filter is needed or it is necessary to, instantaneously identify and correct for the sensor fault occurrence, keeping the filter model as accurate and relevant as possible. The latter approach is considered in this thesis.  On fault occurrence, a slower system drift would allow the recovery from the fault before the camera signal uncertainty becomes much greater than the encoder signal uncertainty. This would generate a fault rejection policy, which would also ensure high performance from the regulator. One way to slow the drift of the system on proprioceptive sensor fault is to reduce the controller gains. Simulation cases #2 and #3 examine this particular approach. Figure 7.12 presents the system responses for these simulations cases. From these response plots, it can be seen that decreasing the controller gains effectively slows down the drift of the system on fault occurrence, although the response is still unstable. Decreasing the controller gains to the level used in simulation case #3 generates a quasi open-loop system, where only a minimal part of the feedback signal is used by the controller. Since this policy is implemented on the feedback signal, reducing the controller gains to this level effectively nullifies the benefits of the use of such a policy.  As mentioned earlier, the Kalman filter used as a regulator provides a stronger filtering effect than a time propagation effect. Decreasing the noise rejection capacities of the regulation filter would provide the desired propagation effect. The second series of simulations examines the effects of a looser tuning of the regulator in order to trade filtering performance for better data  7.2 Model-based Policy  107  propagation. Table 7.6 provides the simulation parameters related to the study of the effects of the Kalman regulator tuning. In order to limit the filtering effect of the filter, it is necessary to decrease the level of confidence associated with the system model and increase the weight that the filter is places on the realization sequence.  System Response for Simulation Cases #2 and #3 1 — Set-Point - - Response Case #2 • • Response Case #3  i  i  > / /  6h  /  "  /  / / /  t / / / / / / /  /  *  2h / :  v  i  0  0.5  1  "  /  y•••  *  i  1.5  Time [sec]  2  2.5  3  Figure 7.12. System Responses for Simulation Cases #2 and #3 The three simulation cases given in Table 7.6 implement the same controller gains used in simulation case #1, generating a controller characterized by good tracking performance. These simulations also assume that the basic-levels of sensor uncertainty are those provided in Table 7.4. It is proposed to increase the level of system noise for simulation cases #4 and #5, while, in case #6, the system noise is maintained at its original level and the measurement noise is decreased.  7.2 Model-based Policy  108  K F Regulator  Simulation  n  Fault Time [sec]  mag[Q]  wag [ft]  Figure  #4  80  1.4  50  10  7.13  #5  80  1.4  60  10  7.14  #6  80  1.4  40  1  7.15  Table 7.6. Model-Based Simulation Parameters for Case #1 Controller Gains The simulation results corresponding to these three simulation cases are presented in Figures 7.13, 7.14, and 7.15, respectively. These three figures present similar results, in which the looser tuning of the filter leads to an increasingly noisy fused feedback signal, while no improvement is observed in the response of the system. That is, the response remains unstable while using the new noise characteristics in the filter definition. This results demonstrates that the noise characteristics previously selected are appropriate and that the data propagation performance of the regulation Kalman filter can not be increased beyond the level currently observed, in this context.  Figure 7.13. System Response and Fused Feedback Signal for Simulation Case #4  7.2 Model-based Policy  109  System Response and Fused Feedback Signal for Simulation Case #5  Figure 7.14. System Response and Fused Feedback Signal for Simulation Case #5  Figure 7.15. System Response and Fused Feedback Signal for Simulation Case #6  7.2 Model-based Policy  110  Additional simulation work has also shown that the increase of the magnitude of the system noise matrix for the time-correlator process beyond the level specified in subsection 7.2.2 does not increase its performance. Furthermore, since the time-correlator is already providing a sufficient level of performance, no further attention was directed towards that issue.  Returning to the first series of simulations (i.e., cases #1-5), the fact that the time-correlation process was adding a lag in the detection of the fault occurrence might be further considered. The uncertainty of each signal plotted against simulation time is given in Figure 7.16. It can be seen that the camera uncertainty grows faster than the encoder uncertainty after the occurrence of the fault, even though the camera signal actually represents the true position of the system. This shows that the uncertainty metric does not increase the uncertainty of the faulty signal sufficiently fast enough to provide a rapid and smooth sensing subsystem reconfiguration. As mentioned previously, it is important that the reconfiguration occurs shortly after the occurrence of the sensor fault in order to minimize the use of the faulty sensor signal.  The use of a more aggressive uncertainty metric is proposed in order to increase the performance level of the model-based implementation. To demonstrate this effect, the use of a more aggressive metric is simulated by increasing the basic uncertainty level associated with the encoder. This particular approach actually decreases the system tracking performance when no fault is present in the system, by voluntarily degrading the performance of the proprioceptive sensor. This technique is used only to demonstrate the perfonnance of the policy in the case where a more aggressive uncertainty metric would be available, since imposing a high basic uncertainty level would prevent the sensing subsystem from automatically reconfiguring on sensor fault occurrence.  7.2 Model-based Policy  111  Sensors Uncertainty for the Three Sensing Sources 1  1  1  • •  • ••• Encoder — — KF Regulator • - - Camera  • • • • i • "i • •  • • • i i • •  ,1.  - r-  0  0.5  -  - . - - . T - . - -  1  -  -  1  1  1.5  2  Time [sec]  i 1  H  2.5  = 1  3  Figure 7.16. Sensor Uncertainty for Simulation Case #1 Table 7.7 presents the simulation parameters used in order to demonstrate the effect that a more aggressive uncertainty metric would have on the policy performance. Again, the controller gains corresponding to the simulation case #1 are used, as well as the noise characteristics described in subsection 7.2.2.  Basic Sensor Uncertainty  Simulation  n  Fault Occurrence Time [sec]  #7  80  1.4  10  0.125  0.2  7.17  #8  80  1.4  50  0.125  0.2  7.18  #9  80  1.4  100  0.125  0.2  7.19  Encoder  Camera  KF Regulator  Figure  Table 7.7. Model-Based Policy Simulation Parameters for Case #1 Gains and Kalman Filters Simulation case #7 results are presented in Figure 7.17, where it can be seen that the fused signal has a stronger effect on the system response. The spikes correspond to the points where heteroceptive knowledge and data is injected into the system. Nevertheless, after some time, the uncertainty of the camera signal outgrows the encoder signal uncertainty and the camera feedback signal is slowly set aside by the fusion process in favor of the faulty encoder signal.  7.2 Model-based Policy  112  This is shown by the inversion of the major time-step spikes of the fused feedback signal, at around 4.2 seconds into the simulation.  A further increase of the basic uncertainty level of the encoder leads to the simulations results presented for case #8 in Figure 7.18. In this simulation case, the encoder feedback is now so uncertain that it does not have any influence in the fusion process (i.e., the regulator and the camera signal compose the majority of the fused feedback signal value). Due to the high uncertainty associated with the faulty proprioceptive sensor, the fused feedback values remain close to set-point signal, since both the camera and the regulator provide the feedback values. Therefore, the effect of the faulty sensor signal in the fusion process and on the system response is minimized, slowing down the system drift to a level where it can be observed by the timecorrelator early enough to keep the system under control. Once the time-correlator lag is overcome, the injection of heteroceptive measurements, providing the real (i.e., showing a quantifiable error with respect to the set-point) position of the system, stop the system drift and a dynamic equilibrium state is reached. The equilibrium state has a noticeable ripple at the major time-step frequency, as well as a substantial steady-state error. These two response characteristics observed on the plot of Figure 7.18 are caused by the influence of the faulty sensor on the system, which is still sufficient to contradict the infonnation provided by both the camera and the regulator.  Further increase of the basic uncertainty level associated with the optical encoder leads to the results presented in Figure 7.19, based on the specification for the simulation case #9. This figure presents a system response similar to the one presented for the simulation case #8, but the increased encoder uncertainty level now results in a smaller steady-state error and less ripple in the system position response. This corroborates the previous conclusion that the cause of the steady-state error and the ripple observed on Figure 7.18 and Figure 7.19, is the small, but not null, weight of the faulty sensor in the fusion process.  7.2 Model-based Policy  113  System Response and Fused Feedback Signal for Simulation Case #7 T  I T  > 1  7h  ./  6h  i  — -  1  Set-Point Response Fused Feedback  It Ii Jl  /» h i * :/• 'l  2h  '  °0  1  '  1 1 I :  2  l^.li,  11  1  3  Time [sec]  4  5  6  Figure 7.17. System Response and Fused Feedback Signal for Simulation Case #7 The last three simulation cases demonstrate that the model-based policy is able to partially, or completely, reject the effects of a proprioceptive sensor failure on the system response, assuming that an aggressive uncertainty metric is available. Since the development of such a metric is not within the scope of this work, and was not identified in the review of the literature, these results have been obtained by increasing the basic level of uncertainty of the encoder used as proprioceptive sensor in the experimental system. This approach has been used for demonstration purposes and should not be implemented as a solution since it imposes a weak weighting on the encoder signal in the fusion process, regardless of the sensor state (i.e., faulty or not). Nevertheless, these simulations provide a proof of concept. As discussed in the following section, the development of the polynomial policy was pursued as an approach that can provide better performance using the current metric.  7.2 Model-based Policy  114  System Response and Fused Feedback Signal for Simulation Case #8  2  3  Time [sec]  Figure 7.18. System Response and Fused Feedback Signal for Simulation Case #8  System Response and Fused Feedback Value for Simulation Case #9 - - Response — Fused Feedback SetPoint  j  4  E2.5  //' 1  E  »»  4  j t 1\* ' I r* 1I (V I I 1  Xmr  4/1/ 1.5  t  /  /  *  *: 1 L r  *  0.5  2  3  4  Time [sec]  Figure 7.19. System Response and Fused Feedback Signal for Simulation Case #9  7.2 Model-based Policy  7.2.5  115  Summary  This subsection presented the development of the closed loop transfer function for both the major time-step and the minor time-step heteroceptive fusion schemes. The stability of each . scheme has been evaluated for the complete range of sensing subsystem configurations. It has been shown that the major time-step closed-loop system poles and zeros are all inside the unit circle boundary for any values of the fusion gains, while the minor time-step closed-loop poles and zeros move outside the unit circle boundary for some sensing subsystem configurations.  A dynamic simulation was built in MatLab Simulink in order to further investigate the stability issues related to the implementation of the policy, as well as the performance of the policy itself. The effect of different parameters on the performance of the policy in the presence of a simulated complete failure of the proprioceptive sensor has been analyzed. A ramp signal has been used as set-point signal due to its common use for robotic joint trajectory generation. Further exploratory simulations have shown that the use of other types of set-point signal, such as sine waves or square waves, leads to the same level of performance.  The principal limiting factor for the policy performance is related to the slow uncertainty growth for the faulty sensor provided by the uncertainty quantification process. The presence of poles and zeros outside of the unitary circle found for certain minor time-step sensing subsystem configurations were not shown to be a limiting factor for the implementation of this policy. Although, the saw-tooth response obtained in the presence of a proprioceptive sensor fault can ',• be considered marginally stable. Finally, a proof of the viability of the model-based multirate heteroceptive feedback integration policy for robotic servo-loops has been provided through a proof of concept simulation of the effects of a more aggressive uncertainty metric on the faulty sensor.  7.3 Polynomial Policy  116  7.3 Polynomial Policy 7.3.1 Construction of the Closed-Loop Characteristic Equation In Chapter 5, the implementation of a polynomial prediction policy in order to analytically extend the bandwidth of the heteroceptive sensor was presented. This policy requires a relaxation of the time-correlation of the data streams constraint imposed by the geometric fusion algorithm and does not require the establishment of a detailed model of the plant dynamics. In this chapter, the stability issues related to the use of such a policy are addressed. Furthermore, a dynamic simulation of the system is built and submitted to proprioceptive sensor fault. Finally, stability requirements and dynamic behavior of the system under sensor fault are related to establish two types of performance requirements, namely, Fault Tolerance and Fault  Rejection.  In order to assess the closed-loop system stability, the closed-loop transfer function of the system at hand is established. Redrawing the block diagram of Figure 5.1 to represent the system described in Chapter 6 and substituting the HGRF fusion process with its corresponding gains leads to the block diagram of Figure 7.20.  DIGITAL  V(z)  PID  0(z)  D R I V E D Y N A M I C S  E N C O D E R  H  G,  I N T E R P O L A T O R  2 - '  C A M E R A  Figure 7.20. Polynomial Policy Equivalent Block Diagram This figure represents the robotic servo system with two loops: the inner proprioceptive sensing loop and the outer heteroceptive sensing loop. Each sensing loop is characterized by its sampling rate, as discussed  earlier, but also by its sensitivity  to disturbances  and its level of  accuracy/repeatability. The expression of the fusion process through the fusion gains Gj and Gf,  7.3 Polynomial Policy  117  corresponding to the relative uncertainty of each sensor signal with respect to the overall sensing uncertainty, enables its substitution by the corresponding weighted average. For stability evaluation purposes, the fusion gains are imposed on the system in order to evaluate the effects, of the sensing subsystem configurations on the closed-loop system stability.  Using the assumptions previously stated and assuming that the camera processing delay d is given by an integer number of controller sampling periods n, the block diagram of Figure 7.20 is simplified and the resulting block diagram is shown by Figure 7.21.  V(z)  D I G I T A L  0(Z)  D R I V E  D Y N A M I C S  PID  G, +G  2  Y ' z"  Figure 7.21. Simplified Closed-Loop Block Diagram for the Polynomial Policy The closed-loop transfer function is then given by r ( z ) Q(z) e(z) V(z)  0(z)_ u{ ) z  l  +  m . m . ( e(z) V(z)  G  ]  {  +  G  (7.30) . L . _ L V Y z")  2  ]  where the drive dynamics are provided by equation (6.10), 0z~  0.1033z"  V\z'  l-l.7288z"' +0.7288z""  (7.31)  2  Viz)  The digital PID controller transfer function, —^y, is provided by equation (7.1), while the e(z) Newton-Gregory interpolator transfer function for a given minor time-step a is repeated here from equation (5.9),  r(z) Y(z)~  s  f  2  s  ,  — + - + 1z 2 2  V  —a  2  —s z  —n-a  where the non-dimensional interpolation step is given by  ,  +  -2n—a  {zt-L K  2  2  (7.32)  7.3 Polynomial Policy  118  s—  a  (7.33)  and ae[0,»-l].  (7.34)  Finally, the fusion gains are related through  G,+G =l, 2  (7.35)  and are left as parameters in order to study all the possible sensing subsystem configurations and their effects on the stability of the system.  Two other parameters are left as variable in the evaluation of the closed-loop stability, the interpolation step a and the interpolation horizon, or length, n. As stated earlier, in the current. work, it is assumed that the processing delay is equal to the heteroceptive sampling period. Therefore, the interpolation horizon is computed as the ratio of the proprioceptive sensor sampling rate to the heteroceptive sensor sampling rate. The variation of n provides an easy way to study the effect of the heteroceptive sampling rate on the performance of the overall policy.  Implementing equation (7.30) in MatLab as a function of G,, a, n, and the continuous-time controller gains K , Pc  K, lc  and K , Dc  permits the evaluation of the position of the closed-loop  poles and zeros in the complex plane. The stability analysis of the proposed polynomial policy for multirate heteroceptive sensor feedback integration is presented in the following subsection.  7.3.2 Stability  Analysis  The closed-loop characteristic equation of the system, as formulated by (7.30), is a function of many variables. In order to clearly establish the effect of each of these variable, it is necessary to limit their number. In particular, the effect of the fusion gains (i.e., the sensing subsystem configuration) on the system is critical, while other parameters have a more predictable effect on the system and do not receive the same level of attention.  The effect of the interpolation step, a, on the system is as expected. Due to the open loop nature of the prediction process, it is clear that the quality of the prediction decreases as the  7.3 Polynomial Policy  119  interpolation step moves farther away in time from the last measurement update point. Furthermore, as the interpolation step moves further from the last measurement update and as the interpolation horizon, n, gets longer, the amount of time-delay present in the system increases. Therefore, the stability of the closed-loop system is evaluated for the last possible interpolation step (i.e., a = n -1),  which represents to the worst stability conditions for the closed-loop  system.  The stability of two particular implementation cases is examined in this section. The first implementation case corresponds to the case where both the proprioceptive and the heteroceptive sensor sampling rates are fixed by hardware constraints, while it is desired to increase the level of tolerance of the system to the occurrence of a proprioceptive sensor failure. This type of implementation has been described earlier as a Fault Tolerance implementation case.  The second implementation case examined for the polynomial policy corresponds to the case where a certain level of tracking performance, or bandwidth, is required from the system in all configurations. In that case, it is desired to establish the minimum bandwidth of the heteroceptive sensor necessary to maintain the required level of tracking performance. This implementation case has been described earlier as a Fault Rejection implementation of this policy.  First, the implementation case described in Chapter 6 is examined. This implementation is based on sampling rates permitted by the hardware and processing constraints, and is therefore classified as a Fault Tolerance implementation. The characteristic of the sensors described in that last chapter leads to a sampling rate ratio, n, of 80, due to the limited bandwidth of the heteroceptive feedback loop (i.e., 5 Hz), with respect to the servo-loop sampling rate (i.e., 400 Hz). Evaluating the stability for the last interpolation step and a 50% fusion ratio leads to a system with severe instability. The poles and zeros corresponding to this system are plotted on Figure 7.22, where it can be seen that many poles are outside of the unitary circle boundary on the right hand side of the map.  7.3 Polynomial Policy  120  Poles and Zeros Plot for n=80, a=n-1, and G^O.5 1  i  1  i  i  i  i  i  i  x x xx : jdfx  IJ^X X  x m , x X  JJPK '  ofx  '  I*-. X ? X XfL x  XX ' fx  *  it  x|  &*  A A WW  A  M* 5x  4 1  X jj x JS  Sx...; :\x .  ......  Xjf: xJ r  J»X .  xjr  « Jr  x  1 ^Sfcx  X  xjir  jjr  X j j r  • x -XXT x "J**^ -1 -1  -0.8  -0.6  i  i  i  i  -0.4  -0.2  0  0.2  I  0.4  I  0.6  Real Axis  i  i  0.8  1  Figure 7.22. Poles and Zeros Plot for n=80 and High Controller Gains Thus, it is necessary to reduce the controller gains in order to maintain system stability even when only the heteroceptive feedback is used (i.e., G, =0). The reduction of the controller gains has a direct effect on the tracking performance of the overall system. Since it is only possible, for fixed sampling rate, to accommodate one of the two requirements, high tracking accuracy or fault tolerance, it is assumed that, in this type of implementation, the tracking performance requirements of the system are relaxed.  Setting the fusion gain G, to a small value, the gain values are iteratively tuned in order to reach closed-loop stability. It is found that for controller gains values of K  Pc  K  Dc  = 0.018, K  lc  = 0.02, and  = 0, the stability of the closed-loop system is greatly improved and only one pole is outside  of the unit circle boundary. Figure 7.23 presents the plot of the poles and zeros in the complex plane.  • -' >  As mentioned, only one pole is outside of the unit circle (i.e., z = 1.0003), quite close to the boundary. The presence of a pole outside of the unit circle is not considered critical for the stability of the closed-loop system for two reasons. This pole is very close to the boundary, and,  7.3 Polynomial Policy  121  furthermore, this sensing subsystem configuration is a worst case scenario unlikely to occur in a ' real implementation.  Closed-loop Poles and Zeros for n=80, G^O, and Low Controller Gains  0.8 0.6 0.4  ra '6> ro  o  E -0.2 -0.4 -0.6 -0.8  -1  -0.8  -0.6  -0.4  0  -0.2  0.2  0.4  0.6  0.8  1  Real Axis  Figure 7.23. Closed Loop Poles and Zeros for n=80, Gi=0, and Low Controller Gains A fusion gain value of G, =0 would imply that the heteroceptive sensor has a null basic uncertainty level, and that the measurement provided by the sensor perfectly matches the system set-point signal. Since every sensor displays a non-null basic level of uncertainty, the fusion gain , may get close to the value assumed (i.e., G =0), but will never reach it. The unstable pole x  quickly moves inside of the unitary circle for a small increase of the proprioceptive feedback path fusion gain value.  These controller gain values are used to study the stability of different sensing subsystem configurations, as summarized by Table 7.8. The pole-zero maps corresponding to the sensing subsystem configurations presented in Table 7.8 are provided in Figure 7.24.  7.3 Polynomial Policy  Case  n  122 Controller Gains  a K  P c  Fusion Gains K  D c  G,  G  Figure  2  #1  80  n-1  0.018  0.02  0.0  0  1.0  7.23  #2  80  n-1  0.018  0.02  0.0  0.25  0.75  7.24 (a)  #3  80  n-1  0.018  0.02  0.0  0.50  0.50  7.24 (b)  #4  80  n-1  0.018  0.02  0.0  0.75  0.25  7.24 (c)  #5  80  n-1  0.018  0.02  0.0  1.0  0.0  7.24 (d)  Table 7.8. Sensing Subsystem Configurations for a Fault Tolerance Policy Implementation  (c)  (d)  Figure 7.24. Poles and Zeros Plots for Different Sensing Subsystem Configurations  7.3 Polynomial Policy  123  From the pole-zero maps presented in Figure 7.24, it is shown that the closed-loop stability of the system defined in Table 7.8 is maintained over the range of possible sensing subsystem configurations. However, the stability of the closed-loop system is attained through the reduction of the controller gains, which reduces the tracking performance of the system. The quantification of the loss of tracking performance is examined more closely in subsection 7.3.4, where dynamic simulations of the system under fault conditions are performed.  The second implementation case, Fault Rejection, implies that the tracking performance of the system must not decrease under proprioceptive sensor fault condition. In this case, the stability requirements of the closed-loop system are met under all possible sensing subsystem configurations by increasing the required bandwidth of the heteroceptive sensor. The required heteroceptive sampling rate is found by defining the controller gains first and then iteratively reducing the prediction horizon until all the system poles are more stable than the original system poles. This procedure is based on the assumption that the processing delay of the heteroceptive sensor is equal to its sampling period, and that this delay is expressed as an integer number of minor time-step. Furthermore, the analysis is performed for the worst stability case, given by the last interpolation step and a null fusion gain (i.e., a = n -1 and G, = 0.0).  The stability analysis is performed based on the controller gains established experimentally. Table 7.9 presents the summary of the different heteroceptive sampling rates for which the stability of the closed-loop system has been evaluated. The pole maps corresponding to the different systems definition established in Table 7.9 are presented in Figure 7.25.  Case  n  Controller Gains  a Pc  Fusion Gains Dc  K  K  G\  G  Figure  2  #6  2  n-1  0.4  0.3  0.001  0.0  1.0  7.25 (a)  #7  3  n-1  0.4  0.3  0.001  0.0  1.0  7.25 (b)  #8  4  n-1  0.4  0.3  0.001  0.0  1.0  7.25 (c)  #9  5  n-1  0.4  0.3  0.001  0.0  1.0  7.25 (d)  #10  6  n-1  0.4  0.3  0.001  0.0  1.0  7.25 (e)  Table 7.9. Heteroceptive Sampling Rates for Fault Rejection Implementation of the Policy  7.3 Polynomial Policy  124  Original System Poles(x) and Polynomial Policy Poles(+)[n=2 and G =0]  $  Original System Poles(x) and Polynomial Policy Poles(+) [n=3 and G^O)  0.2  -0 6  -04  -0.2  0  0.2  04  -0B  -0.6  -0.4  -0.2  0  0.2  0.4  Real Axis  Real Axis  (a)  (b)  Original System Poles{x) and Polynomial Policy Poles(+) [n=4 and G^OJ  Original System Poles(x) and Polynomial Policy Poles(+) [n=5 and G^O] 1  08 +  :  +  :  + :..+  0.6 +  0.4  +  •  „  "" + " :  Imagineiry Axis  +  m 0.2  Imagin;  £  .+ .  0.8  : +  + 06  +  +  +  : +  ;. +  • • •••••+•*••••• ..  +  :  ;+  +  -0.8  + •  -1 -1  -0.8  -0.6  -04  -0.2  0  O.'l  02  06  OB  -1  1  -OB  -06  -0.4  -0 2  0  02  Real Axis  Real Axis  (c)  (d)  OA  06  0.8  Original System Poles(x) and Polynomial Policy Poles(+)[n=6 and G^O]  CO  0  -0.8  -0.6  -0.4  -0.2  0  0.2  0.4  0.6  0,8  1  Real Axis  (e) Figure 7.25. Closed-Loop Poles for Different Heteroceptive Sampling Rates  1  7.3 Polynomial Policy  125  Figure 7.25 presents the closed-loop poles for different values of sampling rate ratio. On these maps, the original system poles (i.e., the closed-loop system where G, =1.0) are plotted on top of the polynomial policy closed-loop poles. This enables visual identification of the longest prediction horizon, which generates a system, at least, as stable as the original system. In the current case, n-2  (i.e., Figure 7.25(b)) or n = 3 (i.e., Figure 7.25(c)) seems to fulfill the stability  requirements, while « = 5(i.e., Figure 7.25(d)) and rc = 6(i.e., Figure 7.25(e)) presents, poles outside of the unitary circle boundary. The validity of these results is confirmed by the dynamic simulations performed in a following section.  7.3.3 Construction  of the Dynamic  Simulation  In order to corroborate the parameters values obtained from the stability analysis of the closedloop polynomial policy for multirate heteroceptive sensing integration and establish the basis of the implementation of this policy on a real system, a dynamic simulation of the overall system is built. Again, a discrete-time dynamic simulation of the response of the system to a given input signal and under the influence of a proprioceptive sensor fault is examined.  First, the system model is built using the discrete-time transfer function already available for the axis-drive dynamics. The other necessary discrete-time processes (i.e., digital PID controller, HGRF fusion process, Newton-Gregory interpolator, and the uncertainty quantification process) are coded as S-functions and included in the simulation loop. For simulation purposes, the uncertainty metric is used to identify sensor fault occurrence and quantify the deviation of each sensor signal with respect to the deterministic input of the system. This differs from the stability analysis where the fusion gains where imposed on the system. In this case, the fusion gains are computed on-line and it is necessary to artificially generate a fault on the proprioceptive sensor signal in order to test the performance of the policy. The basic uncertainty level assumed for each sensor signal is based on real values observed on the experimental manipulator, as given in Table 7.4.  As mentioned earlier, for simulation purposes, it is necessary to generate the sensor fault. A complete proprioceptive sensor hardware failure is examined. This fault is represented on Figure  7.3 Polynomial Policy  126  7.9, and corresponds to the same type of fault as used in the model-based policy simulations. The details have already been provided in subsection 7.2.3, and are not repeated here. The simulations are again performed using a typical robotic trajectory as set-point signal. Figure 7.26 presents the linear trajectory used as the set-point signal for simulation purposes. This type of trajectory is commonly used in robotic manipulators, especially for Cartesian manipulators. This particular trajectory presents a repetitive motion along the manipulator axis, which is typical of pick-and-place operations.  Simulated Robotic Trajectory 1  T  0  1  2  1  1  3  4  1  5  1  1  6  7  1  8  T  9  10  Time [sec]  Figure 7.26. Simulated Robotic Trajectory Finally, the performance of the uncertainty quantification metric is also examined in simulation. The following subsection presents simulation results for both implementation cases, Fault Tolerance and Fault Reject ion, under the polynomial policy.  7.3.4 Simulation Results The implementation case corresponding to Fault Tolerance is first simulated. Two different simulations are presented for that particular case. The first simulation shows the effect of the high controller gains on the system stability for a high sampling rate ratio. The second simulation  7.3 Polynomial Policy  127  presents the system position response once the controller gains have been brought down to levels where it is possible to achieve stability. Table 7.10 presents the parameters used to generate both simulations, while Figure 7.27 and Figure 7.28 are presenting the system responses for both simulation cases. On these figures, the occurrence of the proprioceptive sensor fault is also illustrated by plotting the encoder feedback signal, which goes to zero on fault occurrence.  Simulation  n  Controller Gains  Fault Occurrence Time [sec]  Figure Dc  K  #1  80  2.5  0.4  0.3  0.001  7.27  #2  80  2.5  0.018  0.02  0.0  7.28  Table 7.10. Polynomial Policy Simulation Parameters for Fault Tolerance Implementation From the stability evaluations presented in Section 7.3.2, it is apparent that increasing the prediction horizon n, or increasing the controller gains, pushes the closed-loop system poles outside of the stability region of the complex plane. This situation is observed for the simulation case #1 where the combination of high controller gains and a long prediction horizon pushes poles outside of the stability region and generates an unstable system response following the fault occurrence.  As proposed, the instability of the first simulation is remedied by reducing the controller gains. This solution is justified by the assumption that the prediction horizon, or sampling rate ratio, is fixed. Therefore, the closed-loop stability of the system must be enforced through the adjustment of the controller gains.  7.3 Polynomial Policy  128  -  S y s t e m R e s p o n s e Using High Controller Gains (n=80) I  1  1  1 1 1 1 1 1  • - Set-Point Encoder Msmt Response  -—-  1 1 1  \  1 1 1 1 1  1 /  o  0  1  1 2  1  3  4  1  1  5 T i m e [sec]  1 6  1  7  1  8  9  10  Figure 7.27. System Response Under Sensor Fault Condition for High Controller Gains  Figure 7.28. System Response Under Sensor Fault Condition for Low Controller Gains  7.3 Polynomial Policy  129  The second simulation of Table 7.10 implements the reduced controller gains in order to provide a sufficient level of closed-loop stability. Figure 7.28 presents the system response for this particular simulation case. The position response is obviously more stable than the one presented for the first simulation case, but also presents poor tracking. Nevertheless, the effect of the sensor failure on the system is minimized by the proposed policy, since the system remains under control. The tracking performance of the system is limited under fault condition due to the low bandwidth of the heteroceptive sensor. To achieve better tracking performance under sensor fault condition, it is necessary to provide more heteroceptive sensor bandwidth. This approach is addressed with the second implementation case for the polynomial policy, Fault Rejection.  It is also possible to look at the value of the fusion gains G, and G to verify that the sensing 2  subsystem reconfiguration was automatically performed on fault occurrence. Figure 7.29 presents the evolution of the fusion gains through the simulation. It can be seen from the plot that shortly after the sixth second, the encoder fusion gain starts increasing again, even though the fault is still affecting the encoder. This is due to the fact that the system is moving toward the point where the proprioceptive sensor has actually crashed, reducing the magnitude of the discrepancy between the system set-point and the faulty sensor signal, which is reporting an erroneous null output. A more detailed discussion of the issues related to the use of the uncertainty metric in such a context is provided at the end of this subsection.  7.3 Polynomial Policy  130  Fusion G a i n s G a n d G 1  2  Evolution  0.9 h  ro 0.6 . G Camera Fusion Gain , G. Encoder Fusion Gain 2  <q o.4h 0.3h * Iri i  X  \ i  \i  0.1  4  5 T i m e [sec]  6  Figure 7.29. Evolution of the Fusion Gains for the Simulation Case #2 The second implementation case for the polynomial policy is concerned with the establishment of the minimal heteroceptive sensor bandwidth in order to maintain a certain level of tracking performance or system bandwidth. A high gain controller is implemented in the simulation and the system responses to the set-point signal are plotted for various prediction horizons. Table 7.11 provides the parameters used for each simulation. According to the stability analysis performed in subsection 7.3.2, prediction horizons of « = 3or « = 4should result in good tracking performance and a stable response.  Simulation  n  Fault Occurrence Time [sec]  Controller Gains Pc  K  1  lc  K  Figure Dc  K  #3  3  1.5  0.4  0.3  0.001  7.30  #4  4  1.5  0.4  0.3  0.001  7.31  #5  5  1.5  0.4  0.3  0.001  7.32  #6  6  1.5  0.4  0.3  0.001  7.32  Table 7.11. Simulation Parameters for the Fault Rejection Case of the Polynomial Policy  7.3 Polynomial Policy  131  Figures 7.30, 7.31, and 7.32 present the system response for the three simulation cases described in Table 7.11.  System Response Using High Controller Gains (n=3)  100  1 1 " " Set-Point - - Encoder Msmt  90  i  i  i  i  i  i  i  80  1"  70  M.  60  c  o  |  50  <  40  I  X 30  20  H _  '•. _1 ft....  :/ / *  1  j ... » •- •  4  5  6  10  Time [sec]  Figure 7.30. System Response Under Sensor Fault Condition for n=3 Figure 7.30 presents a system response showing good tracking characteristics and the effect of the sensing subsystem reconfiguration is barely observable from the position response plot, fulfilling the requirements underlying this type of implementation of the polynomial policy. Figure 7.31 also presents good tracking characteristics and a quite smooth transition between the sensing subsystem configurations, but random noise spikes can be observed on the system response, which eventually become unstable. Figure 7.32 presents the system response for the last two cases defined in Table 7.11. The first one, n-5, generates an oscillatory response, which still follow the set-point signal as the system is moved back and forth, while the second case, n=6, is completely unstable.  7.3 Polynomial Policy  1601  132  System Response Using High Controller Gains (n=4) 1 1 " • • Set-Point - - Encoder Msmt — Response  1  1  1  1  1  r—  120 k-  Time [sec]  Figure 7.31. System Response Under Sensor Fault Condition for n=4  System Response Using High Controller Gains (n=5 and n=6) ••" •- • —— — -  Set-Point Encoder Msmt Response (n=5) Response (n=6)  Time [sec]  Figure 7.32. System Response Under Sensor Fault Condition for n=5 and n=6  1  7.3 Polynomial Policy  133  The disturbances observed in these two last figures are caused by the increase of the prediction horizon, which pushes the interpolator poles closer to the boundary of the unit circle. When poles are located close to the boundary, the system modeling errors, as well as the presence of unmodeled disturbances, generally pushes these poles outside of the stability region. As a result, erratic behavior of the system response is observed. If the disturbances cease after some short period of time, the system starts to behave correctly again, as seen in the first part of the Figure 7.31 response plot. When the poles are too close to the boundary, the destabilizing effect is more continuous, Figure 7.32, and can eventually lead to instability.  These last simulations have shown that it is possible to obtain good tracking performance under a proprioceptive sensor fault condition using the polynomial policy, in the case where sufficient heteroceptive sensor bandwidth is available. Since only the sampling rate ratio is actually important, reducing the servo-loop sampling rate would results in the same type of results, as long as it is possible to maintain the position servo-loop tracking performance at lower controller frequencies.  Again, it is possible to consider the evolution of the fusion gains values through the simulation. Particularly, the evolution of the fusion gains for simulation case #3 is of interest since this one represents the best implementation case demonstrated for the polynomial policy. Figure 7.33 presents the evolution of the fusion gains for that particular simulation. From the plot, it can be , seen that, this time, the encoder fusion gain does not start to increase as the system moves, toward the point corresponding to the erroneous encoder measurement. Instead, the tracking: performance of the system is sufficient to maintain the heteroceptive signal uncertainty to a minimum and the camera fusion gain at its maximum. From Figure 7.33 and the similar plot provided for the Fault Tolerance implementation case (i.e., Figure 7.29), it is apparent that the uncertainty metric defined in Chapter 3 provides maximum performance for the Fault Rejection implementation of the polynomial policy. In such cases, the system response does not diverge from the set-point signal and the faulty sensor is easily identified.  The major drawback in using this uncertainty metric in Fault Tolerance implementation cases is related to the reduced tracking performance of that type of implementation. Since the system  7.3 Polynomial Policy  134  response is always somewhat off of the set-point signal, all the sensors present a non-negligible level of uncertainty and the identification of the faulty sensor is not as clear as in the other implementation case. The limitations of the ability of the uncertainty metric to identify fault occurrence in a Fault Tolerance implementation are clearly seen for a proprioceptive sensor failure where the faulty output value is actually inside of the motion range of the manipulator. While it can easily be shown that the effect of such a fault in a Fault Rejection implementation is minimal, the occurrence of such a fault causes the failure of the polynomial policy to maintain the system under control in a Fault Tolerance implementation.  Fusion Gains G and G Evolution 1  1  1  1  1  2  1  _r- - —  K: I  •  G Camera Fusion Gain - - G Encoder Fusion Gain 2  1  0.2  • i i i  0 i  1  0  1  2  i 3  4  5  6  7  8  9  10  Time [sec]  Figure 7.33. Evolution of the Fusion Gains for the Simulation Case #3 A n example of the effect of the encoder crashing in the middle of the motion range of the manipulator is provided for the simulation case #2 of Table 7.10, where instead of driving the faulty encoder output to zero, it is driven to an output value of 50 mm. Figure 7.34 presents the results obtained for the simulation case mentioned above, where the fault occurs 1.5 seconds after the beginning of the simulation and follow the failure profile described above. As seen from the figure, and from the corresponding fusion gain plots of Figure 7.35, as the system moves toward the value where the proprioceptive sensor is crashed, the uncertainty associated with the encoder measurement starts to decrease and the smaller basic uncertainty level of this sensor  7.3 Polynomial Policy  135  results in the system being driven away from the set-point by the influence of the faulty data in the composition of the fused feedback signal.  Effect of an Encoder Failure in the Middle of the Motion Range on the System Response  150  1  i •••• Set-Point — - Response — Encoder Feedback  <  i  i  i  i >  „100 E E i  i .•" ».-• ••"  /  \  .-i  i  /  \  y  y 0  1  1•  /  2  3  4  5  6  Time [sec]  7  8  9  10  Figure 7.34. Simulation Results for a Fault Tolerance Policy Failure Case  This problem is related to the lack of memory of the uncertainty metric. Increasing the size of the window used to compute the variance increases its memory capacity, but also introduces a lag in the detection of the fault occurrence and increases the process computational requirements. In order to perform well in a dynamic system, it is required that the uncertainty metric be able to rapidly identify the fault occurrence and increase the uncertainty associated with the sensor signal, while taking more time to reset the uncertainty level when the sensor measurement appears to agree again with the system set-point signal.  7.3 Polynomial Policy  136  Fusion Gains  0.8 0.7  y. i. • i 11  \ ;  and G Evolution for a Fault Tolerance Policy Failure 2  ;  '  \;  ; G_ Camera Fusion Gain  8  0 5  4  5  Time [sec]  6  10  Figure 7.35. Fusion Gains Evolution for a Fault Tolerance Failure Case The addition of a heuristic rule to the uncertainty metric defined in Chapter 3 to avoid the quasiinstantaneous reset of the uncertainty level would generate the desired behavior. However, the design of a robust heuristic rule is a complex process, and it is difficult to demonstrate the relevance and correct behavior of the rule for all the situations. Nevertheless, it is also possible to integrate memory capacities in the uncertainty metric previously defined under the form of an integral term. Integrating the difference between the sensor signal uncertainty, as established by the current uncertainty metric, and using the value obtained to limit the decreasing rate of the uncertainty would generate the required behavior. The design of a more robust uncertainty metric is outside the scope of the current work and is left as future work.  7.3.5  Summary  In this section, the closed-loop stability of both cases of the polynomial-based policy for heteroceptive sensor feedback integration in robotic position servo-loop has been presented. The Fault Tolerance case has been shown to be stable for small controller gain values, even though some poles are located very close to the unit circle boundary. In order to maintain the system bandwidth under a proprioceptive sensor fault condition in a Fault Rejection implementation, it  7.4 Summary  137  has been shown that a sampling rate ratio as low as three or four is necessary to maintain tracking performance, as well as system bandwidth.  A dynamic simulation has been built for both implementation cases and the results obtained from the systems stability analysis have been corroborated in simulation. The Fault  Tolerance  stability issues requires the use of reduced controller gains for large values of the sampling rate ratio, reducing the tracking performance of the system but allowing the system to remain under control. The Fault Rejection case requires higher heteroceptive bandwidth, which allows for the use of high controller gains, and leads to good tracking performance. Generally, the selection of the implementation case is based on the available sensing hardware or the required system performance under a proprioceptive sensor fault condition.  Finally, the performance of the uncertainty metric in the polynomial policy context has been examined. It has been shown that the uncertainty metric defined in Chapter 3 performs better in a Fault Rejection implementation case, where the impact of the sensor fault on the system response is minimized. The major limitation related to the use of this metric in a Fault Tolerance context is related to the lack of memory of the metric, and its ability to instantaneously reset the faulty sensor uncertainty level when the set-point signal and the faulty sensor measurement converge. Two approaches have been proposed to address these issues and improve the performance of the metric in the Fault Tolerance implementation case.  7.4 Summary The first section of this chapter presented the construction of the closed-loop transfer function for the model-based policy for multirate integration of heteroceptive sensor integration in robotic position servo-loops. The stability of both fusion schemes of the model-based policy has been evaluated. It has been found that the major time-step system configuration is stable for all the sensor subsystem configurations, while the minor time-step system configuration presents poles and zeros outside of the unitary circle for some sensing subsystem configurations.  7.4 Summary  138  The implementation of this policy in a dynamic simulation has shown that the uncertainty metric defined in Chapter 3 does not propagate the faulty sensor uncertainty rapidly enough to keep the system under control. The effects of a more aggressive uncertainty metric on the policy have been simulated by increasing the proprioceptive sensor basic uncertainty level, and the validity of the proposed approach has been demonstrated. However, the Kalman regulator cannot propagate the heteroceptive measurement effect on the system through the complete major timestep period. This issue, which remains unresolved, represents a major drawback in the use of the proposed model-based policy.  The second section presented the construction of the closed-loop transfer function corresponding to the implementation of the polynomial-based policy on the experimental system introduced in ; Chapter 6. This closed-loop transfer function has then been used in order to analyze the stability of the of the two implementation cases of the polynomial policy, namely Fault Tolerance and Fault Rejection. It has been shown that the Fault Tolerance case requires the reduction of the controller gains to maintain the stability of the closed-loop system under all sensing subsystem configurations. This leads to a severe reduction of the tracking performance of the system; although the system remains under control at all times.  In order to maintain the tracking performance of the system to a level comparable to the original system position servo-loop level, it is necessary to increase the heteroceptive sensor bandwidth. Increasing the heteroceptive  sensor bandwidth moves the Newton-Gregory polynomial  prediction process poles towards the origin of the complex plane, increasing their stability and their bandwidth. The minimal sampling rate ratio is found for the prediction horizon generating poles that are more stable than the original servo-loop poles. For the implementation case described in Chapter 6, the minimal sampling rate ratio is found as being n = 3.  Both implementation cases for the polynomial policy are then implemented in a dynamic simulation and the results obtained from the stability analysis are corroborated. Particularly, the limitations related to the use of the uncertainty metric in a Fault Tolerance implementation are examined. In a system where poor tracking is observed, all the sensors provide observations that are quite different from the set-point signal. Thus, the metric is unable to rapidly identify the  7.4 Summary  139  occurrence of a sensor fault. Furthermore, in a case where the faulty sensor provides measurements close to the set-point values, the uncertainty of this sensor is instantaneously reduced, injecting corrupted information in the system and diminishing its overall performance level. The addition of a memory effect to the uncertainty metric defined in Chapter 3 would leads to the creation of a more robust metric and the performance optimization of the Fault, Tolerance implementation of the polynomial policy.  Finally, two approaches have been proposed for the implementation of the memory capacity in the uncertainty quantification process previously defined. However, the complete development of such an uncertainty quantification process is left as future work.  Chapter 8 Experimental Results 8.1 Preamble This chapter presents experimental results related to the implementation of the polynomial policy for multirate heteroceptive feedback integration in robotic system position servo-loops. The experiments are performed on the experimental platform described in Chapter 6 and aim at demonstrating the fault tolerance capacities of the policy presented in Chapter 5, and simulated in Chapter 7. The model-based policy presented in Chapter 4 is not implemented on the experimental set-up since the uncertainty quantification process cannot propagate the faulty sensor signal uncertainty rapidly enough, as discussed in Chapter 7. Since the uncertainty quantification metric cannot quickly, and with high confidence, reconfigure the sensing subsystem, the position response becomes unstable. The complete implementation of the modelbased policy is therefore impossible. This approach is set aside and the focus of attention is directed towards the implementation and experimental testing of the polynomial policy.  The description of the experimental procedure and parameters is provided first. This section is divided into two subsections, discussing two different experiments. The first series of; experiments implement the Fault Tolerance case simulated in Chapter 7 based on the system 140  8.2 Experimental Parameters  141  hardware constraints described in Chapter 6. The second series is also based on a Fault. Tolerance implementation of the polynomial policy, but where the servo-loop sampling rate is reduced to illustrate the effects of the sampling rate ratio on the performance level of this last policy. For both experiments, the necessary assumptions are described and the discrepancies ; between the simulation results obtained in the previous chapter and the corresponding experimental results are analyzed.  The experiments focus on minimizing the effect of a hardware proprioceptive fault on the tracking performance of the control system through the dynamic reconfiguration of the sensing subsystem. Corresponding experimental results are presented and aim at establishing the performance of the polynomial policy, as well as its limitations, for both cases where proprioceptive sensor fault is observed and cases where no fault is observed. This chapter concludes with a discussion of the results presented and a comparative analysis of the experimental results with respect to those obtained in simulation.  8.2 Experimental Parameters 8.2.1 Fault Tolerance, High Servo-Loop Sampling Rate Following the experimental robotic set-up description provided in Chapter 6, it is assumed that a 400 Hz sampling rate position servo-loop is used, and that heteroceptive sensing is available at 5 Hz. The position servo-loop is closed using an optical encoder (i.e., proprioceptive sensor) and a digital camera (i.e., heteroceptive sensor) is used to generate high-level feedback. It is desired to optimize the use of the information provided by that last sensor and extend its role in the system.  The processing delay associated with the extraction of the position of the end-effector from the image imposes an upper limit on the heteroceptive sensor bandwidth, as well as a delay, and therefore the camera sampling rate can not be increased any further. As a consequence, only the Fault Tolerance implementation case of the polynomial policy can be experimentally studied, the Fault Rejection implementation case leads to a severely unstable system response for such a high sampling rate ratio.  8.2 Experimental Parameters  142  As discussed earlier, and demonstrated by the simulations presented in the previous chapter, the implementation of the polynomial policy in a high sampling rate ratio context aims at providing a fail-safe operation mode in the case of proprioceptive sensor failure. Nevertheless, in order to maintain a stable system response, it is necessary to reduce the controller gains and, consequently, the tracking performance of the system. This approach is then more suited to be implemented in systems where tracking performance requirements are not that stringent (i.e., robotic painting, sandblasting, autonomous robots, etc.), but where proprioceptive sensor fault tolerance is a concern.  In Chapter 7, controller gains to ensure the stability of the system response in a Fault  Tolerance  implementation have been presented, as well as the corresponding simulated system response. From preliminary experiments, a slight discrepancy is observed between the value of the gains used in simulation and the one used on the real system. Table 8.1 presents the controller gains corresponding to both the simulation and the experimental implementation. Since the discrepancies between the two set of gains seems to be consistent, it is assumed that the discrepancies are caused by modeling errors and their exact nature is not investigated further.  Controller Gain K  P  fco  Units  Simulated  Experimental  -  0.018  0.035  -  0.020  0.025  -  0.0  0.0  Hz  40  40  Table 8.1. Simulation and Experimental Controller Gains for a Fault Tolerance Case It is also necessary to establish the basic uncertainty levels for each sensor in order to implement the uncertainty quantification process described in Chapter 3 and implemented in the dynamic simulations of Chapter 7. The digital camera basic uncertainty level is quantified through the repeated measurement of the Cartesian manipulator end-effector position, while it is maintaining a known static position. The variance associated with the stream of data provides a good estimate of the overall uncertainty associated with the acquisition of the image, the identification of the feature in the image, and the coordinate transform of the feature position from pixel space to  8.2 Experimental Parameters  143  Cartesian space. Repeating the procedure for multiple points over the X-axis range of motion and taking the worst measured variance as the basic uncertainty level provides a conservative estimate for this last quantity.  The basic uncertainty level of the optical encoder is established using a different approach. The position measurement error as provided by an optical encoder presents a uniform distribution of variance, (8.1)  12  where ^correspond to the Cartesian distance between two consecutive encoder counts, [22], This relation provides a basic uncertainty level for the sensor itself, but does not include the kinematics chain errors, drive mechanism imprecision, and other non-linear effects such as backlash in the lead screw nut and X-axis guiding rods flexibility. The flexibility of the X-axis guide rods particularly affects the accuracy of the end-effector positioning. Since the endeffector is not aligned with the X-axis lead screw, the bending of the guide rods in the horizontal plane is amplified by the Y-axis, further increasing the end-effector X-axis error. Elliott [21] provides a more thorough analysis of the experimental manipulator positioning uncertainty.  Considering the above sources of error, it is assumed that a scaling of the rounded value provided by equation (8.1) for the current proprioceptive sensor by a factor of 1000 provides a conservative, and much more realistic, estimate of its basic uncertainty level. Table 8.2 provides a summary of the basic uncertainty level assumed in the experimental work described in this chapter.  Sensor  Units  Basic Uncertainty  Optical Encoder  mm  0.001  Digital Camera  mm  2  0.125  The experiment itself involves generating the repetitive motion of the manipulator along its X axis and then interrupting the optical encoder signal. The interruption of the optical encoder  8.2 Experimental Parameters  144  signal latches the counter to its last value before the occurrence of the interruption, generating a proprioceptive sensor fault condition. The encoder signal is interrupted through the addition of a switching board between the encoder itself and the data acquisition board used to decode the signal quadrature and count the pulses. The schematics for the switching board are provided in Appendix A . Again, a typical robotic set-point signal, a joint-space ramp signal, is used to generate the motion of the end-effector. The repetitive pattern used for the set-point signal is represented by the plot of Figure 8.1.  Experimental S e t - P o i n t Signal 1501  0  ,  1  5  10  1  15  20  25  T i m e [sec]  Figure 8.1. Repetitive Ramp Signal Used as Experimental Set-Point For this implementation case, five different experiments are performed. In the first two experiments, the tracking performance of the system is established by submitting the manipulator to the set-point signal described above, but without generating a proprioceptive sensor fault. These experiments aim at quantifying the tracking performance obtained when implementing the Fault Tolerance policy for two different set of controller gains; namely, the original system high controller gains and the reduced gains following the Fault  Tolerance  variant of the polynomial policy. The three other experiments aim as examining the position /, response of the system when a fault is affecting the performance of the proprioceptive sensor. Particularly, three different cases, where the fault occurs in different part of the prescribed  8.2 Experimental Parameters  145  trajectory, are examined: (i) the occurrence of the fault before the manipulator motion is started, (ii) the occurrence of the fault after the beginning of the manipulator motion but outside of the cyclic motion range, and (iii) the case where the faulty sensor provides a measurement continuously inside of the manipulator cyclic motion range. Table 8.3 presents a summary of the experiments parameters for the Fault Tolerance implementation of the polynomial policy for the cases where a high servo-loop sampling rate is used.  Servo Rate [Hz]  Camera Rate [Hz]  #1  400  5  #2  400  #3  Case  Controller Gains  Fault Time [sec]  Perf. Measure [mm]  Figure  K  P  */  -  0.4  0.3  0.001  0.44  8.2  5  -  0.035  0.025  0.0  5.14  8.2  400  5  0  0.035  0.025  0.0  6.65  8.5  #4  400  5  -2  0.035  0.025  0.0  7.24  8.7  #5  400  5  « 15  0.035  0.025  0.0  6.88  8.9  Table 8.3. High Servo-Loop Rate Experiments Parameters In order to quantify the tracking performance of the policy for each experiment, a performance measure is established. Due to the deterministic nature of the system, the performance of the policy is established by measuring the mean absolute error for the whole experiment duration. The performance measure, p, is defined by: p = -t(abs(®A )- (k% k  u  (8-1)  r k=\  where r represents the number of sampled data points during the experiment. This performance measure provides a straightforward way to compare results obtained for different experiment cases. The effects of the reduction of the servo-loop sampling rate are examined in the following subsection in order to quantify any tracking performance improvement related to the shorter prediction horizon (i.e., lower sampling rate ratio n) used in such a case.  8.2 Experimental Parameters  146  8.2.2 Fault Tolerance, Low Servo-Loop Sampling Rate Due to the hardware limitations of the heteroceptive sensor used in the current experimental setup, it is not possible to implement the Fault Rejection case of the polynomial policy. As it has been shown in the simulation of Chapter 7, this implementation is interesting since it provides enough controller bandwidth to maintain the system tracking perfonnance, while still improving the system robustness towards proprioceptive sensor fault occurrence. Nevertheless, it is still possible to show the effects of the sampling rate ratio on the system tracking performance by reducing the servo-loop sampling rate, hence reducing the sampling rate ratio and, consequently, the open-loop prediction horizon.  In this particular application, it is possible to reduce the servo-loop sampling rate down to 100 Hz, without endangering the closed-loop system stability. Again, the experiments are performed using the assumptions stated in the previous subsection and aim at demonstrating the capacity of. the polynomial policy to dynamically reconfigure the sensing subsystem on fault occurrence and: minimize the effects of the fault on the system tracking performance.  The same four cases, using the low controller gains, are experimentally examined for this servoloop sampling rate: (i) no fault, (ii) fault occurrence before motion starts, (iii) fault occurrence after the beginning of the motion but outside of the cyclic motion range, and, (iv) fault occurrence  inside of the manipulator cyclic motion range. Table 8.4 summarizes  the  experimental cases performed for the implementation of the polynomial policy using a reduced position servo-loop sampling rate.  The next section presents the results obtained experimentally for the parameters described in Table 8.3 and Table 8.4. These results are presented as position response plots and fusion gains plots in order to verify that the dynamic reconfiguration of the sensing subsystem correctly occurs subsequent to a proprioceptive sensor fault.  8.3 Experimental Results  Case  Servo Rate [Hz]  147  Camera Rate [Hz]  Controller Gains  Fault Time [sec]  K  P  K,  K  Perf. Measure  Figure  D  #6  100  5  -  0.035  0.025  0.0  6.04  8.11  #7  100  5  0  0.035  0.025  0.0  6.47  8.12  #8  100  5  - 1.5  0.035  0.025  0.0  6.46  8.13  #9  100  5  - 15  0.035  0.025  0.0  9.02  8.14  Table 8.4. Reduced Servo-Loop Sampling Rate Experiments Parameters  8.3 Experimental Results 8.3.1 Fault Tolerance, High Servo-Loop Sampling Rate The first experiment performed for this particular case aims at establishing the tracking performance o f the control system as designed and implemented, as well as the behavior o f the policy in a normal situation (i.e., where no fault occurs). A s it has been mentioned earlier, it is necessary to decrease the value o f the controller gains in order maintain closed-loop stability in a proprioceptive sensor fault context. In order to compare the tracking performance o f both the original system (i.e., high controller gains) with the modified one (i.e., low controller gains), their position responses to the set-point signal o f Figure 8.1 are presented in Figure 8.2.  From Figure 8.2, it can be seen that the system tracking performance is severely affected by the reduction o f the controller gains. This reduction o f the tracking performance o f the system is also quantified by the mean absolute error, as presented in Table 8.3, which increases by an order o f magnitude from a value o f 0.44 mm for high controller gains, to a value o f 5.14 m m using the reduced controller gains. This clearly indicates the limited ability o f the control system to make the system response match the set-point signal. Nevertheless, the position response for the low controller gains case still follows the set-point signal in a stable manner. It is important to note that, the position responses for both controllers are plotted using the fused position measurement signal, since this is the feedback signal to the system and is, therefore, more relevant to the system response than the individual sensor signals.  8.3 Experimental Results  148  System Position Response for High and Low Controller Gains 160r  Time [sec]  Figure 8.2. System Response for High (Case #1) and Low(Case #2) Controller Gains^ (No Fault) The use of the fusion process in a case where a high performance tracking controller is used and no proprioceptive sensor fault occurs leads to a system where the sensor presenting the smallest basic uncertainty level is predominantly used to generate the fused measurement. In the case where high controller gains are used, the optical encoder predominates the fused measurement value, at the exception of the points where important tracking errors are observed. This situation is illustrated by Figure 8.3, where the fusion gains are plotted against time.  In Figure 8.3, one can observe that, the encoder fusion gain effectively predominates over the camera fusion gain for the majority of the experiment duration. Only the points characterized by a discontinuity of the prescribed set-point are shown to present a stronger weighting on the camera feedback measurements. This last observation is caused by the fact that, at these points, the discontinuity of the set-point signal creates a tracking error, correctly quantified by the encoder, but not by the camera signal, which has a processing delay. As the encoder measurements are slightly diverging from the set-point signal, while the delayed camera signal is still close to the set-point signal, the encoder uncertainty is increased and its fusion gain is accordingly decreased. This situation lasts until the system overshoot, or lag, is reduced by the  8.3 Experimental Results  149  controller action and the discrepancy between the encoder measurements and the prescribed setpoint signal is minimized.  Evolution of the Fusion Gains i  i  i  i 6  8  — Enco der Fusion Gain • • • • Camara Fusion Gain  55 0.4  0  2  i 4  i  10  12  14  Time [sec]  Figure 8.3. Evolution of the Fusion Gains for High Controller Gains, Case #1 (No Fault) The same situation is observed for the low controller gains case, but this time, due to the poor tracking of the controller generated, not only do the fusion gains spike at the set-point direction changes, but they also are more closer to a mid-point weighting for the major part of the experiment duration, as shown on Figure 8.4.  Using the system position response, presented by Figure 8.2, for the low controller gains case as a performance reference, the effect of an encoder fault on the system response is examined. In the first case examined, the fault actually occurs before the motion of the end-effector is started and the faulty encoder output is approximately, and constantly, null (i.e., =0 mm). The response obtained for this case is presented in Figure 8.5. As it can be observed from this figure, the system position response is jerky in the first part of the motion and demonstrates poor tracking in the second part. This is quantified by the increase of the mean absolute error through the experiment duration, which goes from a value of 5.14 mm for the non-faulty case to a value of 6.65 mm. Nevertheless, the system remains under control for the duration of the simulation.  8.3 Experimental Results  150  Evolution of the Fusion Gains  Figure 8.4. Evolution of the Fusion Gains for Low Controller Gains, Case #2 (No Fault)  Figure 8.5. System Response for Fault Occurrence Before Motion Starts, Case #3  8.3 Experimental Results  151  It is also possible to consider the sensing subsystem reconfiguration performed by the policy, as given by the fusion gains plot of Figure 8.6. It can be seen from that plot that the camera predominates the composition of the fused position measurement even before the system starts moving away from the faulty encoder measurement. This predominance of the camera measurements over the faulty encoder measurement ensures a good reconfiguration of the sensing subsystem. This implies that the faulty encoder data is sufficiently rejected in the H G R F fusion process to avoid corrupting the fused feedback signal.  Evolution of the Fusion Gains 1  1  1  ....  — " "  Encoder Fusion Gain Camera Fusion Gain  \ AtL0  1 2  1  4  6  8  10  1 12  1 14  Time [sec]  Figure 8.6. Evolution of the Fusion Gains for Fault Occurrence Before Motion Starts, Case #3 The response corresponding to the occurrence of the encoder fault shortly after the system starts the first leg of its motion, as described by the experiment case #4 of Table 8.3, is presented in Figure 8.7. This figure presents the system response and the faulty encoder measurement corresponding to this experiment.  As seen from the response plot and the fusion gain plot of Figure 8.8, the encoder has increased weight in the fusion process until the occurrence of the sensor fault, shortly before the 2 seconds mark. On fault occurrence, the sensing subsystem configuration rapidly changes in favor of the  8.3 Experimental Results  152  camera signal, which follow more closely the set-point trend, while the encoder is now continuously providing a faulty measurement close to 15 mm.  The position response observed in this experiment is similar to that presented for the previous experiment, in Figure 8.5. However, this time, instead of a jerky profile over the first segment of the trajectory, there is now a steady oscillation, decreasing the tracking performance of the control system. The system decreased tracking performance with respect to the non-faulty sensor case, and the previous experiment case, is again illustrated by the mean absolute error, quantified as 7.24 mm in Table 8.3. However, the system remains stable for the experiment.  The decreased performance is attributed to the fact that the uncertainty associated with the faulty encoder signal has decreased with respect to the previous experiment. Since the faulty encoder measurement is closer to the motion range of the manipulator, the magnitude of the discrepancy between the set-point signal and the faulty encoder measurements is less, and so is the uncertainty associated with the encoder measurement. This leads to a higher contribution of the encoder measurements in the fused position signal.  Position Response Under Encoder Failure (case #4)  Ohm  0  m lit*-• i  2  1  i  i  i  i  4  6  8  10  12  Time [sec]  I- •  14  Figure 8.7. System Response for Fault Occurrence After Motion Starts, Case #4  8.3 Experimental Results  153  The increase in the encoder measurement proportion in the composition of the fused position signal is seen from the fusion gains plot of Figure 8.8. Compared to Figure 8.6, the previously flat fusion gains are now rippled as both sensors disagree about the real position of the system.  Evolution of the Fusion Gains i  0.81  •55 0.4  :  0.2  J  0 0  — "  / L^w^i—  Encoder Fusion Gain Camera Fusion Gain  ,  1  1  1  2  4  6  8  Time [sec]  10  i  i  12  14  Figure 8.8. Fusion Gains Evolution for Fault Occurrence After Motion Starts, Case #4 Finally, the experimental analysis of the high servo-loop sampling rate implementation case of the Fault Tolerance variant of the polynomial policy is concluded with the analysis of the situation where the sensor fault occurs inside the motion range of the manipulator. This particular case has been shown in simulation to be one of the most problematic cases of use of the uncertainty quantification metric, as presented in Figure 7.34.  Figure 8.9 presents the fused position measurement obtained for this particular case. Latching the encoder measurement to a value close to 60 mm leads to a response where the end-effector is not able to follow the set-point below that position value. This is caused by the automatic sensor subsystem reconfiguration as the encoder signal uncertainty suddenly drops, due to the reduction of the discrepancy between the encoder measurements and the set-point signal.  8.3 Experimental Results  154  Nevertheless, the general tracking performance of the system for that type of encoder fault is comparable to the levels observed in the previous experiment cases. In this case, the mean absolute error value, given in Table 8.3, can mot be fairly compared with the previous results due to the fact that the fault does not occur in the first motion cycle. Since the first motion cycle is performed in a non-faulty context, the low mean absolute error value obtained for that part of the experiment decreases the actual average for the complete experiment duration. Computing the mean absolute error for the second motion cycle (i.e., from the 12 second mark to the end of the experiment) leads to a more realistic value of 8.5 mm.  Again, an oscillatory behavior can be observed during the first part of the motion following the occurrence of the fault, while the position response corresponding to the second part of the trajectory (i.e., the downward section) shows a lower frequency oscillation also characterized by a higher error magnitude.  S y s t e m R e s p o n s e Under E n c o d e r Failure 1  -  1  I  l  l  l  •\  ;  l  /  -  | 100  \  /  • " Set-Point - - Encoder Msmt — Fused Position Msmt  0  2  4  6  8  10  12  14 16 T i m e [sec]  18  20  22  24  26  28  30  Figure 8.9. System Response for Fault Occurrence in the Cyclic Motion Range, Case #5 The first motion cycle, shown on Figure 8.9, is performed in a non-faulty context and shows quite reasonable tracking performance. The tracking performance is actually only affected by a slow rise-time and a slight overshoot. As mentioned earlier, these two drawbacks are caused by  8.3 Experimental Results  155  the use of reduced controller gains to ensure closed-loop stability for all sensing subsystem configurations. The position response quickly converges to the set-point signal, as this last signal trend stabilizes. This increase in tracking performance as the set-point signal trend stabilizes can directly be related to the use of a polynomial predictive algorithm to analytically extend the heteroceptive sensor bandwidth.  This type of open-loop prediction algorithm provides better performance predicting signals following a linear trend than signals showing highly-dynamic behavior. In the case where a second polynomial prediction algorithm is used, the regression parameters established by the divided difference terms provide an exact fit. This limited ability of the polynomial algorithm to correctly predict heteroceptive measurements following a highly dynamic set-point, or system response, is translated in the closed-loop stability evaluation by the upper limit imposed on the system bandwidth by the polynomial policy poles and can be observed at the motion direction change points in the response plots, where the polynomial prediction algorithm fails to generate an accurate prediction.  Finally, the fusion gains plot corresponding to this case, shown by Figure 8.10, provides a better idea of the limitations associated with the use of the uncertainty quantification metric, as discussed in Chapter 7. Due to the lack of memory shown by the metric, latching the output of the optical encoder within the cyclic motion range of the manipulator causes an increase of the encoder contribution to the fused position signal, as the end-effector moves toward the faulty encoder measurement.  From Figure 8.10, the typical fusion gains profile for a non-faulty system is observed until the occurrence of the encoder fault, shortly after the 14 second mark. Following the fault occurrence, the camera measurement starts to dominate the composition of the fused position signal, until the set-point signal moves towards the faulty encoder measurement, shortly after the 18 second mark. This leads to a situation where the faulty encoder measurement dominates, and corrupts the fused position signal with faulty data. As the set-point starts to move away from the faulty encoder measurement, this situation is corrected. This particular limitation of the uncertainty quantification metric has been discussed earlier in this thesis and is shown, both in simulation  8.3 Experimental Results  156  and in experimentation, to be the major drawback in using the metric defined in Chapter 3 in order to generate the automatic reconfiguration of the sensing subsystem on sensor fault or failure occurrence. However, the ability of the uncertainty metric to correctly reconfigure the sensing subsystem in the presence of a fault inside the manipulator cyclic motion range is shown to be better in the experimental implementation than what has previously been observed in simulation. In simulation, the occurrence of the proprioceptive sensor fault inside of the cyclic motion range caused the system to go unstable, as shown by Figure 7.34, while the system response presented in Figure 8.9 remains stable in the presence of the sensor fault.  Evolution of the Fusion Gains  1.2  0.8 <D 3 CO >  E 0.6  co c o  O  'Ui  i f 0.4  0.2  0 0  2  4  6  8  10  12  14  16  18  20  22  Time [sec]  24  26  I  I  Figure 8.10. Evolution of the Fusion Gains for Fault Occurrence in the Cyclic Motion Range  8.3.2 Fault Tolerance, Low Servo-Loop Sampling Rate As previously discussed, due to hardware limitations, it is not possible to achieve a sufficiently high heteroceptive sensor sampling rate in order to implement the Fault Rejection variant of the polynomial policy. Since the performance of the polynomial policy for multirate sensor integration is affected more by the sensors sampling rate ratio than by the heteroceptive sensing  8.3 Experimental Results  157  rate, it is possible to comparatively examine the effects of a shorter prediction horizon on the system performance by reducing the position servo-loop sampling rate.  In this particular series of experiments, the servo-loop sampling rate is reduced to 100 Hz (i.e., dividing the open-loop prediction horizon by a factor of 4). The reduction of the position servoloop sampling frequency leads to a decrease of the tracking performance of the control system, since the system is actually only controlled at the sampling instants. Figure 8.11 presents the system position response using the reduced servo-loop frequency, corresponding to the case #6 of Table 8.4. From the figure, it can be seen that the tracking performance of the system is degraded with respect to the high servo-loop sampling rate presented in Figure 8.2. In particular, the position response has a slightly higher overshoot, while the response convergence to the setpoint seems to be improved. Computing the performance measure established in section 8.2 provides a quantification of the tracking performance of the control system using a reduced servo-loop sampling rate. Table 8.4 provides the mean absolute error for this particular experiment case, which is 6.04 mm. This value is slightly higher than the value found for the corresponding case using high servo-loop sampling rate (i.e., 5.14 mm). The discrepancy between the two system tracking performance is sufficient to limit direct comparisons between the results obtained in each case. However, it can be noted that the level of performance obtained in the low servo-loop sampling rate experiments follows the same trends as in the high servoloop sampling rate experiments. Furthermore, the deterioration of the tracking performance of the system due to the reduction of the servo-loop sampling rate appears to be somewhat ; ameliorated by the reduction of the prediction horizon. A relative comparison of the policy performance under a series of experiments, for the same servo-loop sampling rate, is again performed.  One can note that, the controller gains used for the current experiment series, as presented in Table 8.4, are the same as the ones used for the high servo-loop sampling rate. This is due to the fact that the controller gains, specified in Tables 8.3 and 8.4, are the continuous time gains for the digital PID controller used in this application. The discrete-time equivalent gain values are computed directly inside of the controller algorithm as a function of the current position servoloop sampling rate used. Therefore, the controller performance and tuning is quite similar for  8.3 Experimental Results  158  both experiment series. Appendix F provides further details about the controller gains discretization process.  System Response Using Reduced Servo-Loop Sampling Rate T'-'  1 1 • • " Set-Point - - Encoder Msmt — Fused Position Msmt  1  1  1  ~100  -  IE  .1  80  \ \ ^\  *55 O CL .52  6  /j  >k  0  0  •  i  2  4  6  1 8  10  12  1 14  Time [sec]  Figure 8.11. System Response for Reduced Servo-Loop Sampling Rate (No Fault), Case #6 One can also note that the set-point signal speed is increased for this second experiment series in order to compensate for the slower set-point generation rate. Since a constant step size interpolation process is used, a reduction of the servo-loop frequency would reduce the set-point signal speed. In order to maintain the experiment parameters to the value used for the high servoloop sampling rate experiments, the set-point interpolation step size is scaled up by the same factor as the servo-loop sampling rate is scaled down (i.e., 4). The resulting physical demanded endpoint speed remains constant.  The position response for the case where no proprioceptive sensor fault is imposed is presented by Figure 8.11. This figure represents the base-line performance level for this polynomial policy implementation and is used as referee for the other cases (i.e., cases #7, #8, and #9) described in Table 8.4. It is also possible to examine the behavior of the fusion gains corresponding to the basic performance level for this particular implementation. The fusion gain plot is similar to Figure 8.4, and is given in Appendix G for reference.  8.3 Experimental Results  159  The system response for case #7 is presented in Figure 8.12. For this case, the optical encoder fault is generated before the motion of the end-effector is started. This response plot presents similar characteristics as case #3 ( c f , Figure 8.5), the corresponding case using the high servoloop sampling rate, although the oscillations visible on the fused position signal are more pronounced than in case #3. The quantification of the mean absolute error leads to a value of 6.47 mm, as presented in Table 8.4. This value is higher than the baseline value found for experiment case #6 (i.e., 6.04 mm), mainly due to the presence of oscillations on the first part of the motion. Comparing the mean absolute error increase from case #2 to #3 and from case #6 to #7, leads to a significantly lower increase for the second case (i.e., 1.51 mm versus 0.43 mm), for the same fault occurrence case. This is mainly attributed to the loose tracking provided by the reduced servo-loop sampling rate controller, which is not as affected by the occurrence of the proprioceptive sensor fault, but may also be related to the reduction in the prediction horizon.  150r  System Response Under Encoder Failure Set-Point Encoder Msmt Fused Position Msmt  Time [sec]  Figure 8.12. System Response for the Fault Occurrence Before Motion Starts, Case #7 Figure 8.13 and Figure 8.14 presents the system responses for cases #8 and #9, corresponding to the occurrence of the fault after the motion is started, but outside of the cyclic motion range, and the occurrence of the fault inside of the manipulator cyclic motion range, respectively. Case #8  8.3 Experimental Results  160  provide a system response similar to the response corresponding to case #7, where the fault occurs before the motion of the manipulator starts. This is also illustrated by their almost identical mean absolute error level (i.e., 6.47 mm and 6.46 mm, respectively). The high level of correspondence between the two experiments is attributed to the faulty encoder measurement value, which is actually not far from the null one observed in case #7. The fusion gains plot corresponding to this experiment is presented in Appendix G.  System Response Under Encoder Failure  I  160  1  140  120  1100 c o | Q_ to  fX  80  60  40  20  0 0  2  4  6  8  Time [sec]  10  12  14  I I  Figure 8.13. System Response for Fault Occurrence After Motion Starts, Case #8 Figure 8.14 presents another example of the failure of the uncertainty quantification metric to correctly identify sensor fault. As in experiment case #5, the occurrence of the proprioceptive sensor fault within the cyclic motion range of the manipulator cause the uncertainty metric failure to maintain the sensor subsystem in its non-faulty configuration. Latching the faulty encoder measurement to a value close to the middle of the manipulator cyclic motion range (i.e., =80 mm) leads to a response where the system is unable to follow the set-point as it move through the faulty encoder measurement.  Again, computing the mean absolute error for the complete experiment duration produces a misleading value. The performance measure value presented in Table 8.4 is an optimistic  8.4 Discussion  161  estimate of the tracking performance of the system. The good tracking performance displayed by the policy for the first motion cycle brings down the mean absolute error for the complete experiment. Quantifying the performance measure for the second motion cycle (i.e., from the 12 second mark to the end of the experiment) leads to a more realistic value of 12.26 mm.  The fusion gains plot corresponding to this experiment is similar to the one presented for the high servo-loop sampling rate (c.f, Figure 8.10) and is presented in Appendix G for reference.  Figure 8.14. System Response for Fault Occurrence Within Cyclic Motion Range, Case #9  8.4 Discussion In this chapter, experimental results related to the implementation of the Fault Tolerance variant of the polynomial policy for multirate heteroceptive sensor data integration in robotic position servo-loops have been presented. The implementation of the policy in a robotic control system aims at providing an uniform and coherent procedure to increase the system tolerance to the occurrence of proprioceptive sensor fault through the dynamic reconfiguration of the sensing subsystem.  8.4 Discussion  162  Due to hardware limitations of the experimental set-up presented in Chapter 6, it impossible to achieve a sufficient heteroceptive sampling rate in order to implement the Fault  Rejection  variant of the polynomial policy, as presented in the previous chapter. Nevertheless, it is possible to implement the Fault Tolerance policy using the available heteroceptive sensing bandwidth. As discussed in Chapter 7, the implementation of the polynomial policy for systems characterized by a high sensors sampling rates ratio (i.e., n) requires the use of a loose tracking controller in order to ensure the stability of the system position response under all sensing subsystem configurations. The sensing subsystem is dynamically reconfigured by the HGRF fusion process, on the identification of sensor fault, or failure, by the uncertainty quantification process. The uncertainty quantification process identifies sensor fault occurrence by monitoring the sensor measurement deviation with respect to the system set-point signal. The use of reduced controller gains leads to reduced tracking performance of the control system. The use of a looser tracking controller may be desirable for some type of robotic applications, such as robotic painting, robotic sandblasting, or in the situation where high-inertia payloads are manipulated using an under-sized robotic manipulator.  The position response of the experimental manipulator X-axis has been presented for different cases of fault occurrence, based on a complete proprioceptive sensing hardware failure. It has been shown that the occurrence of the optical encoder fault outside of the cyclic motion position range defined by the set-point signal is tolerated by the system, at the expense of a reduced performance. Mainly, under a sensor fault condition, the system response displays a higher level of jerkiness and oscillation than the non-faulty system. For each case, the performance of the uncertainty quantification process at dynamically reconfiguring the sensing subsystem has been examined through the evolution of the fusion gains, as defined by Figure 7.20.  The uncertainty quantification process has been shown to provide sufficient performance as long as the faulty measurements provided by the proprioceptive sensor are not closer to the set-point signal than the heteroceptive measurements. The occurrence of the encoder failure inside of the cyclic motion range, or the convergence of the set-point signal to the position provided by the faulty sensor, leads to a failure of the uncertainty quantification process to identify the  8.4 Discussion  163  occurrence of a fault and the HGRF fusion to discriminate the corrupted measurements. The case of a failure of the encoder inside of the cyclic motion range defined by the set-point signal has been presented and shows that the reconfiguration of the sensing subsystem to a faulty state harms the performance of the system by keeping it from correctly following the prescribed setpoint signal. Nevertheless, the policy performance for this sensor fault case is shown to be superior to what was observed in simulation, where the system would go unstable. This limitation of the uncertainty quantification process is directly related to the lack of memory of the process defined in Chapter 3, and is not addressed in this work.  Another series of experiments has been presented in order to demonstrate the effect of a reduction of the sampling rate ratio, or the prediction horizon, on the overall performance of the policy. Since the digital camera sampling rate is fixed by hardware and processing delays, the sampling rate ratio has been reduced through a reduction of the servo-loop sampling rate. The experiments previously performed are repeated for a position servo-loop scaled down by a factor of 4. The reduction of the servo-loop sampling rate directly affects the controller performance and therefore it is difficult to insulate the effects of the reduced servo-loop sampling rate from those of the reduced sampling rate ratio.  Since the reduced servo-loop sampling rate system does not present the same tracking characteristics as the original system, and, furthermore, the fault occurrence scenarios are not identical, limited comparisons can be made from the results obtained in each case. In the second series, it is noted that the relative increase of the mean absolute error caused by fault occurrence is reduced. This points to the need for more directly comparative experiments with varying prediction horizons, utilizing a higher bandwidth heteroceptive sensor. Finally, this second series of experimental results provides a way to corroborate the reduction of perfonnance, observed in the first experiment series, associated with the faulty encoder measurement moving closer to the cyclic motion range. Both experiment series show, through the computation of the mean absolute error, that the tracking performance of the control system is decreased as the faulty encoder measurement is moved closer to the cyclic motion range. This last observation underlines the need for a more robust uncertainty quantification process in order to maintain the control system tracking performance under a sensor fault condition.  Chapter 9 Conclusions & Recommendations 9.1 Conclusions In this thesis, a methodology to increase the reliability and robustness of robotic control systems facing sensor faults or failures has been presented. This methodology is based on the integration of heteroceptive sensing in the manipulator position servo-loop. Fault Tolerance or Fault Rejection is obtained through the fusion of redundant end-effector position data. Redundant endeffector position data is extracted from the feedback signal provided by the Heteroceptive Sensor (i.e., a sensor providing contextual and environmental feedback), before being fused with the low-level feedback provided by the robotic device Proprioceptive  Sensor (i.e., a sensor  providing feedback about the internal state of the robotic device).  Multisensor Integration has been used in various systems and under various implementations in order to provide a more complete, or more accurate, representation of the state of a system. In this thesis, multisensor integration has been performed inside the Encapsulated Logical  Device  (ELD) framework. The ELD framework provides an efficient and coherent framework in order to process and manage sensory information provided in a multisensor system, while still  164  9.1 Conclusions  165  allowing sufficient flexibility to implement low-level control loops and high-level event based control functions.  The definition of the E L D architecture and the fundamentals of multisensor integration provide the foundations for the proposed methodology. However, an efficient and relatively simple way to merge the data and the knowledge provided by the various sensors is found in Multisensor Fusion. Multisensor fusion is generally seen as a subset of multisensor integration in which data streams are actually merged together in order to generate a more accurate, or more relevant, signal. The increase of quality, or relevance, of the fused signal is generally quantified through the reduction of its variance, or Uncertainty. The uncertainty associated with a measurement has multiple sources and provides a quantification of the quality and the relevance of the measurement for a given context. The quantification of the uncertainty is based not only on the sensor baseline uncertainty, as specified by the manufacturer, but also on the context in which the measurement has been performed.  A Heuristic-based  Geometric Redundant Fusion  (HGRF) method is adopted to fuse the  proprioceptive and heteroceptive position feedback signals. The HGRF method is a heuristically extended geometric fusion method, which provides optimal uncertainty estimation for coherent measurements and a conservative estimation of the signal uncertainty for non-coherent measurements. The use of this approach is advantageous over traditional geometric fusion methods, which are unable to handle non-coherent measurements.  The H G R F  is a  computationally efficient, physically interpretable, and flexible redundant fusion algorithm that can also be used to dynamically reconfigure the sensing subsystem on fault, or failure, occurrence at any level of this subsystem (i.e., hardware, wiring, sensing, processing, etc.).  In this thesis, the HGRF scheme is utilized to Dynamically Reconfigure the sensing subsystem. Both proprioceptive and heteroceptive sensors can be affected by a deterioration of the environmental conditions in which measurements are performed, leading to poor sensing accuracy or data relevance. When a sensor is identified as being permanently or temporarily faulty, it is necessary to minimize the effects of the measurement on the system to avoid corrupting better information, even i f this infonnation is deemed less accurate. The HGRF  9.1 Conclusions  166  generates a fused measurement based on a weighted average where the weights are evaluated by the relative uncertainty of each sensor with respect to the sum of their uncertainties. Hence, the dynamic quantification of the sensor uncertainty leads to the dynamic reconfiguration of the sensing subsystem, as a sensor weight in the fusion process converges to one or zero. This implies that it is necessary to identify the occurrence of a sensor fault through the measurement of the sensor uncertainty, or variance.  A simple metric to quantify the sensor signal uncertainty has been developed. The Uncertainty Quantification  Metric is based on the evaluation of the variance of the sensor signal taken with  respect to the deterministic system input. The ability of the metric to identify basic types of faults and failures (i.e., hardware failure, wiring failure, etc.) has been demonstrated. The use of the input signal as reference signal, instead of the sensor signal mean, enables the identification of sensors faults or failure, since in a robotic system, it is expected that the end-effector position will converge toward the desired position in a finite, and generally short, period of time.  The definition of the integration methodology to provide fault tolerance of fault rejection capacities to the system is completed by the definition of a procedure to handle the issues related to the multirate nature of the system. Heteroceptive sensors such as digital cameras, and area, or volumetric, scan devices provide a rich feedback signal that requires extensive processing in order for simple features, such as position and speed, to be extracted from the raw signal. It is generally infeasible to generate low-level information from a high-level signal at rates close to servo-loops rates. In order to account for the multirate nature of the system and optimize the use of the low bandwidth information available, two policies have been proposed.  •  In the first, model-based policy, Kalman filter optimal prediction is used both to correct the heteroceptive sensor processing delay and to propagate the effects of the information injected by this sensor to the following control sampling instants where no redundant information is available. Simulations performed using this approach have shown only limited performance, while the policy requires a complex implementation. The major drawback of this particular approach is that, under a sensor fault condition, the model assumed for the system is no longer  9.1 Conclusions  167  valid. To be efficient, this approach would require implementation in an adaptive context, where the system identification is performed on-line and the model adapted as fault, or failure, occurs.  Another solution to the problems related to the limited performance of the model-based policy for multirate heteroceptive sensor integration in robotic position servo-loop is found through the use of a more aggressive uncertainty quantification process. In the case where it would be possible to identify the occurrence of a sensor fault and completely eliminate the influence of this sensor on the composition of the fused feedback signal before the system has drifted away from the set-point signal, it would be possible to avoid the problems related to the deterioration of the system model. The development of another, simpler, approach to address the multirate nature of the system was proposed as an alternative to extending the already computationally expensive model-based policy to a fully adaptive solution, or to the development of a more aggressive uncertainty quantification process.  The second policy adopts a different approach. First, the requirement of the data fusion process for time-correlated measurement is relaxed. Then, a predictive polynomial approach is adopted to extrapolate the heteroceptive data for all the future controller update instants. In this way, it is possible to fuse the open-loop prediction of the heteroceptive feedback with the high-rate proprioceptive feedback signal. The predictive polynomial algorithm utilized is the NewtonGregory algorithm. This algorithm is based on the divided differences of the feedback signal, assuming a second order system.  Based on the bandwidth and stability requirements of the system, two types of implementation have been defined. The first implementation corresponds to a case where it is required to maintain the level of tracking performance of the system under a complete sensor failure. In this case, it is possible, through the position of the system's Closed-Loop  Characteristic  Equation  (CLCE) poles in the complex plane, to determine the minimum bandwidth of the heteroceptive sensor required to maintain tracking perfonnance under a complete failure of the proprioceptive sensor. This implementation case is characterized by the Fault Rejection capacities of the proposed approach. In this case, the effects of the fault occurrence on the system are hardly noticed and tracking performance is fully maintained.  9.1 Conclusions  168  The second implementation case is concerned with a system where the bandwidth of the heteroceptive sensor is limited by the available hardware. In this case, it is impossible to provide complete Fault Rejection capacities to the system, due to the limited bandwidth of the heteroceptive sensor. However, it is still possible to provide the system with Fault  Tolerance  capacities. In this case, the overall system stability and tracking performance are decreased, however it is still possible keep the robotic device under control.  Based on a model of an experimental Cartesian manipulator designed, built, and tested in the Industrial Automation Laboratory, simulation results are provided both for the model-based approach and the polynomial approach. In these simulations, a complete failure of the proprioceptive sensor is simulated. Marginal performance is observed with the model-based approach. Therefore, further development of this approach was curtailed. The polynomial approach demonstrated good performance for both implementation cases, Fault Rejection and Fault  Tolerance.  The polynomial approach was then implemented on the experimental platform and experimental results are provided for the implementation case of Fault Tolerance. It was not possible to implement a Fault Rejection case due to heteroceptive sensor hardware limitations. Experimental results concur with those obtained in simulation. The occurrence of a proprioceptive sensor failure is identified, leading to the sensing subsystem re-configuration, and the faulty sensor data rejection.  The major contributions of this work to the field of industrial automation and robotic control systems have been done following the research objectives stated at the beginning of this thesis. The principal contributions are: - development of a policy to integrate heteroceptive feedback in the position servo-loops of robotic manipulators where this type of feedback is already available, - development of a real-time uncertainty metric to enable the dynamic reconfiguration of the sensor subsystem on sensor fault or failure occurrence,  9.2 Recommendations  169  - establishment of heteroceptive sensor performance requirements in order to fulfil two different types of system performance requirements: namely, Fault Tolerance and Fault Rejection, - implementation of the multirate heteroceptive sensor integration policy for robotic manipulator servo-loop in an experimental manipulator and testing of the policy behavior in realistic conditions.  9.2 Recommendations 9.2.1 Uncertainty Quantification Metric A simple uncertainty metric has been defined for the current work based on the variance of each sensor signal with respect to the deterministic input of the system. This metric has been shown to provide good perfonnance for serious failure such as complete hardware failure, wiring mishaps, sensor dazzling, or counter lock-up for a digital sensor (i.e., optical encoder). Nevertheless, the perfonnance of this metric in the presence of less dynamic perturbations, such as sensor drift or calibration errors, is less effective. A metric that could robustly deal with the faults or failures type mentioned earlier would be necessary to efficiently implement such a policy in an industrial application.  Furthermore, one of the major drawbacks of the uncertainty metric is related to its lack of memory; that is, its ability to almost instantaneously reset as the system set-point is moved towards the point corresponding to the faulty sensor output. For cases where the set-point only transits through this position, the system will recover quickly as the set-point moves away. However, for cases where the set-point is set directly equal to the faulty sensor output, the true point will never be achieved. Such a problem could be easily avoided by treating the uncertainty metric as a dynamic process and adding an integrator type of function to the process.  Another approach to this problem would be the establishment of a heuristic function used to avoid the automatic reset of the uncertainty level of a faulty sensor as the system moves towards the faulty output. Such a heuristic could also be used to implement an integral type action. However, the establishment of a robust heuristic is generally a complex task due to the  9.2 Recommendations  170  difficulties of identifying all the possible situations. Therefore, a more physically oriented approach based on the system dynamics (i.e., the integral action suggested above) is more likely to provide a robust and straightforward solution.  9.2.2 Development of the Control Policy The work presented in this thesis is based on an existing control paradigm. It could be possible to further increase the performance of the integration scheme through the use of a better suited control policy. One particular problem that arises in the dynamic sensing subsystem reconfiguration is related to the fact that, in most of the cases, the heteroceptive sensor does not present the same control bandwidth as the proprioceptive sensor. In the case where it is possible to generate sufficient bandwidth from the heteroceptive sensor, in order not to harm the system stability, the transition between feedback configurations does not affect the system response or stability.  In the case where sensors with two different bandwidths are used to provide redundant information about the system, the transition between the system configurations is not generally smooth. This problem can be corrected through the use of different controller gains on each feedback path, enabling the use of different dynamic correction algorithms for each feedback path. Another way to compensate for the different dynamics characteristics generated by each feedback path would be to use a control algorithm allowing for gain programming.  Furthermore, developing control algorithms independently for each sensing system configuration would enable the use of the fusion process to generate the transitions between the two sensing subsystem configurations, and also between the two sets of controller gains. Figure 9.1 illustrates this particular approach. Using such an approach, it would be possible to minimize the feedback paths bandwidth disparities and optimize the overall control (i.e., tracking) performance.  9.2 Recommendations  171  PROPRIOCEPTIVE SENSOR DYNAMICS PROPRIOCEPTIVE FEEDBACK CONTROLLER  UNCERTAINTY QUANTIFICATION  1  1  1  U{z)  • •  HGRF  PLANT  FUSION  DYNAMICS  8{z)  UNCERTAINTY QUANTIFICATION  HETEROCEPTIVE FEEDBACK CONTROLLER  INTERPOLATOR DYNAMICS  PROCESSING DELAY  HETEROCEPTIVE SENSOR DYNAMICS  Figure 9.1. Control System Using Fusion Process for Controller Gains Programming  Finally, the control policies defined in this thesis do not make use of the fused signal uncertainty level. It would be interesting to use the fused uncertainty signal in order to tune, on-line, the tightness of the position tracking, as required by the user. In fact, the position information fed back to the robot control algorithm is only as good as its uncertainty level. Therefore, the setpoints provided by the user should not be pursued inside of the uncertainty region of the feedback sensors, thus limiting the energy spend chasing a set-point when this one can not even be accurately located by the proprioceptive and heteroceptive sensors.  9.2.3 Implementation & Experimental Testing In order to fully assess the capacities and limitation of the proposed methodology and policies, more extensive testing is necessary. The Fault Rejection policy based on the use of the predictive polynomial algorithm requires experimental testing. This requires the development of faster vision processing routines in order to increase the bandwidth currently available from the  9.2 Recommendations  172  heteroceptive sensor (currently 5 Hz). As mentioned earlier in this thesis, the processing delay is currently the major limiting factor for the high-level feedback. Even i f the bandwidth gained through the optimization of the vision processing algorithm is limited (i.e., the maximum bandwidth of the sensor is about 50 Hz), it would be valuable to quantify the effects of the increased heteroceptive sensor bandwidth on the performance of the polynomial policy.  Furthermore, it would be necessary, in order to demonstrate the flexibility of the policies, to perform some testing based on another type of heteroceptive sensor, such as a 3D laser scanner. It would also be of interest to increase the number of heteroceptive, or proprioceptive, sensing sources, again to verify the behavior of the integration and fusion schemes in such a context and the resulting overall robustness of the system in the presence of sensor fault or failure. Finally, the implementation of the policy in a real robotic workcell using a six degree of freedom robot, and the experimentation in real, or simulated, pick-and-place operations would confirm the capacities of such a system and its industrial relevance.  Bibliography [I]  Abidi, M . A . , Gonzalez, R . C , "The Use of Multisensor Data for Robotic Applications", IEEE Transactions on Robotics and Automation, Vol. RA6-(2), (1990): 159-177.  [2]  Albus, J.S., "RCS: A Reference Model Architecture for Intelligent Control", Computer Magazine, Vol.25, (1992):56-59.  [3]  Albus, J.S., Lumia, R., " N A S A / N B S reference model", Aerospace (1990):21-23.  [4]  Allen, P.K., " A Framework for Implementing Multi-Sensor Robotic Tasks", Proceedings of the Image Understanding  America,  IEEE  Vol.28,  Workshop, Vol. 1, (1987):392-398.  [5]  Altintas, Y . , Erol, N.A., "Open Architecture Modular Tool Kit for Motion and Machining Process Control", Annals of the CIRP, Vol.47, N o . l , (1998):295-300.  [6]  Altintas,  Y . , Manufacturing  Automation,  Metal  Cutting  Mechanics,  Machine  Tool  Vibrations, and CNC Design, Cambridge University Press, (2000). [7]  Astrom, K.J., Wittenmark, B., Computer-Controlled  Systems, Theory and Design, Prentice  Hall Inc., (1997). [8]  Astrom, K . J . , Hagglund, T., PID  Controllers:  Theory, Design,  and Tuning,  Second  Edition, Instrument Society of America, (1995). [9]  Balchen, J.G., Dessen, F., "Structural Solution of highly Redundant Sensing in Robotic Systems", Highly Redundant Sensing in Robotic Systems (Jou J.T. & Balchen, J.G., eds.) ,  Springer-Verlag, (1990): 264-275. [10] Born, H., Busendal, J., "Programmable Multi Sensor Interface for Industrial Applications", Proceedings  of the International  Conference on Multisensor  Fusion and Integration  for  Intelligent Systems, (2001): 189-194. [II] Broida, T.J., "Kinematic and Statistical Models for Data Fusion Using Kalman Filtering", Data Fusion in Robotics and Machine Intelligence  (Abidi, M.A. & Gonzalez, R.C, eds.),  Academic Press, (1992): 311-365. [12] Brooks, R.A., "Intelligence Without Reason", Massachusetts Artificial [13]  Institute of  Technology  Intelligence Laboratory, A.I. Memo No. 1293, (1991).  Brown, R.G., Hwang, P.Y.C., Introduction  Filtering, John Wiley & Sons, (1997). 173  to Random  Signals  and Applied  Kalman  Bibliography  174  [14] Brunner, B., Arbter, K . , Hirzinger, G., "Task Directed Programming of Sensor Based Robots", Intelligent Robots and Systems '94 (IROS '94), V o l . 2, (1994): 1080-1087.  [15] Budenske, J., Gini, M . , "Achieving Goals Through Interaction with Sensors and Actuators", Proceedings  of the 1992 IEEE/RSJ  International  Conference  on  Intelligent  Robots and Systems, (1992): 903-908. [16] Budenske, J., Gini, M . , "Sensor Explication: Knowledge-Based Robotic Plan Execution through Logical Objects", IEEE Transactions on Systems, Man, and Cybernetics - Part B:  Cybernetics, Vol. 27, (1997): 611-625. [17] Bullock, T.E., Sangsuk-iam, S., Pietsch, R., Boudreau, E.J., "Sensor Fusion Applied to System Performance under Sensor Failures", Proceedings of SPIE: Sensor Fusion, Vol.931, (1988),131-138. [18] Dutton, K . , Thompson, S., Barraclough, B., The Art of Control Engineering, AddisonWesley, (1997). [19] Elliott, J.D., Langlois, D., Croft, E.A., "Sensor Uncertainty Management for an Encapsulated Logical Device Architecture: Part 1- Fusion of Uncertain Sensor Data", Proceedings  of the American Control Conference, Vol.6, (2001):4282-4287.  [20] Elliott, J.D., Langlois, D., Croft, E.A., " A Systematic Approach to Automation Workcell Design: The Encapsulated Logical Device Architecture", Proceedings of the International Conference  on Multisensor  Fusion and Integration for Intelligent Systems, (200l):283-  288. [21]  Elliott, J.D., Multisensor  Fusion  within an Encapsulated  Logical  Device  Architecture,  M.A.Sc. Thesis, Department of Mechanical Engineering, University of British Columbia, (2001). [22] Erkorkmaz, K . , Altintas, Y . , "High speed C N C system design. Part II: modeling and • identification of feed drives", International  Journal of Machine  Tools and  Manufacture^  Vol.41, No.10, (2001):1487-1509. [23] Erkorkmaz, K., Altintas, Y . , "High speed C N C system design. Part III: high speed tracking and contouring control of feed drive", International Journal of Machine Tools and Manufacture, Vol.41, N o . l l , (2001):1637-1658. [24] Erol, N . A . , Altintas, Y . , "Open Architecture Modular Tool Kit for Motion and Machining Process Control", Proceedings  of the ASME  International  Mechanical  Engineering  Congress and Exposition, Vol.6, No. 1, (1997): 15-22. [25] Erol, N.A., Altintas, Y . , Ito, M.B., "Open System Architecture Modular Tool Kit for Motion and Machining Process Control", IEEE/ASME Transactions on Mechatronics, Vol.5, No.3,(2000):281-291.  Bibliography  175  [26] Groen, F . C . A . , Petriu, E . M . , Vreeburg, M . , Wannerdam, T., "Multi-sensor robot assembly station", Proceedings of the 5 International Conference on Robot Vision and Sensory Controls, (1985):439-448. th  [27] Hara, T., Tomizuka, M . , "Multi-rate Controller for Hard D i s k Drive with Redesign o f State Estimator", Proceedings of the IEEE American Control Conference, (1998):3033-3037. [28] Hayes-Roth, B . , " A n architecture for adaptive intelligent systems", Artificial V o l . 7 2 , N o . 1-2, (1995):329-365.  Intelligence,  [29] Heck, B.S., W i l l s , L . M . , Vachtsevanos, G . J . , "Software Enabled Control: Background and Motivation", Proceedings of the American Control Conference, V o l . 5 , (2001):3433-3438. [30] Hefele, J., Brenner, C , "Robot Pose Correction using Photogrammetric Tracking", Machine Vision and Three-Dimensional Imaging Systems for Inspection and Metrology, Proceedings ofSPIE, V o l . 4189, (2000). [31] Henderson, T . C . , A l l e n , P . K . , Mitiche, A . , Durrant-Whyte, H . , Snyder, W . , Workshop oh : Multisensor Integration in Manufacturing Automation, Technical Report U U C S - 8 7 - 0 0 6 , Department o f Computer Science, University o f Utah, (1987). [32] Henderson, T . C . , Fai, W . S . , Hansen, C , " M K S : A Multisensor Kernel System", IEEE Transactions on Systems, Man, and Cybernetics, V o l . S M C - 1 4 , No.5, (1984):784-791. [33] Henderson, T., Hansen, C , Bhanu, C , "The Specification o f Distributed Sensing and Control", Journal of Robotic Systems, V o l . 2 , No.4, (1985): 387-396. [34] Henderson, T., Shilcrat, E . , "Logical Sensor Systems", Journal of Robotic Systems, V o l . 1 , No.2, (1984): 169-193. [35] Hutchinson, S., Hager, G . D . , Corke, P.I., " A Tutorial on Visual Servo Control", IEEE Transactions on Robotics and Automation, V o l . 12, N o . 5, (1996): 651 -670. [36] ITEX&IFC  Software and Documentation,  Imaging Technology Inc., (1999).  [37] Jdrg, S., Langwald, J., Stelter, J., Hirzinger, G . , Natale, C , "Flexible Robot-Assembly using a Multi-Sensory Approach", Proceedings of the 2000 IEEE International Conference on Robotics and Automation, V o l . 4, (2000): 3687-3694. [38] Kalman, R . E . , " A N e w Approach to Linear Filtering and Prediction Problems", Transactions of the ASME Series D: Journal of Basic Engineering, V o l . 8 2 , N o . l , (1960):35-45. [39] Kernighan, B . W . , Ritchie, D . M . , The C Programming Language, Second Edition, Prentice H a l l Software Series, (1988).  Bibliography  176  [40] Kooyman, T., "Verifying Aircraft Assembly with Real-Time Digital Photogrammetry", Sensors Magazine, Vol.17, No.6, (2000): 71-73. [41] Kremers, J., Blahnik, C , Brain, A . , Cain, R., DeCurtins, J., Meseguer, J., Peppers, N . , "Development of a Machine-Vision-Based Robotic Arc-Welding System", Proceedings of the 13 International Symposium on Industrial Robots and Robots 7, (1983): 14.19-14.33. th  [42] Kreysig, E., Advanced Engineering Inc., (1993). [43]  Mathematics, Seventh Edition, John Wiley & Sons.  KR Cl, KR C2, Robot Sensor Interface (RSI) Release 1.0 Beta, K U K A Roboter GmbH,  (2001). [44] Lange, F., Wunsch, P., Hirzinger, G., "Predictive Vision Based Control of High Speed Industrial Robot Paths", Proceedings  of the 1998 International  Conference  on  Robotics  [45] Lu, W., Fisher, D.G., "Output estimation with multi-rate sampling", International of Control, Vol.48, N o . l , (1988): 149-160.  Journal  and Automation, (1998): 2646-2651.  [46] Luo, R . C , Lin, M . , "Multisensor Integrated Intelligent Robot for Automated Assembly", Proceedings  of the Workshop on Spatial Reasoning and Multisensor Fusion, (1987): 351-  360. [47] Luo, R.C., Kay, M.G., "Data Fusion and Sensor Integration: State-of-the-Art 1990s", Data Fusion  in Robotics  and Machine  Intelligence  (Abidi,  M.A. & Gonzalez,  R.C,  eds.),  Academic Press, (1992): 7-135. [48] MatLab's User Guide, The MathWorks, Natwick, Massachusetts, (1995). [49] McCarragher, B.J., Asada, H., "The Discrete Event Control of Robotic Assembly Tasks", Transactions  of the ASME - Journal of Dynamic Systems, Measurement, and  Control,  Vol.117, (September 1995):384-393. [50] Mirabadi, A . , Mort, N . , Schmid, F., "Fault Detection and Isolation in Multisensor Train Navigation Systems", Proceedings of the UKACC International  Conference on  CONTROL  '98, (1998): 969-974. [51] Mukai, T., Ishikawa, M . , " A n Active Sensing Method Using Estimated Errors for Multisensor Fusion System", IEEE Transactions on Industrial Electronics, Vol.43, No.3, (1996):380-386. [52] Musliner, D.J., Durfee, E.H., Shin, K . G . , "World modeling for dynamic construction of real-time control plans", Artificial Intelligence, Vol.74, N o . l , (1995):83-127.  Bibliography  177  [53] Naish, M . D . , Croft, E.A., "Data Representation and Organization for an Industrial Multisensor Integration Architecture", Proceedings of the International Conference on Computational Cybernetics and Simulation, Vol.1, (1997): 821-826. [54] Naish, M.D., Croft, E.A., " E L S A : a multisensor integration architecture for industrial grading tasks", Mechatronics, Vol. 10, No. 1-2, (2000): 19-51. [55] Nakamura, Y . , X u , Y . , "Geometrical Fusion Method for Multi-sensor Robotic Systems", Proceedings  of the IEEE International  Conference on Robotics and Automation,  Vol.2,  (1989):668-673. [56] Ogata, K., Discrete-Time Control Systems, Prentice-Hall Inc., (1987). [57]  ORTS 2.2 Real Time Signal Processing  Users Reference,  Manufacturing Automation  Laboratories Inc., (1999). [58]  ORTS  2.2  Real  Time Signal  Processing  Programmers  Reference,  Manufacturing  Automation Laboratories Inc., (1999). [59]  ORTS 2.2 Real  Time Signal Processing  Pirns Reference,  Manufacturing Automation  Laboratories Inc., (1999). [60] Pachter, M . , Chandler, P.R., "Challenges of Autonomous Control", IEEE Control Systems Magazine, Vol.18, No.4, (1998):92-97. [61] Papanikolopoulos, N.P., Khosla, P.K., Kanade, T., "Visual Tracking of a Moving Target by a Camera Mounted on a Robot: A Combination of Control and Vision", IEEE Transactions on'Robotics and Automation, Vol. 9, No. 1,(1993): 14-35.  [62] Paunicka, J.L., Mendel, B.R., Corman, D.E., "The OCP - A n Open Middleware Solution for Embedded Systems", Proceedings  of the American  Control  Conference,  Vol.5,  (2001):3445-3450. [63]  Model MFIO-3B  High-Speed Motion-Control  Interface Card for DSPLink™  User Manual  Version 1.1, Precision MicroDynamics Inc., (2000). [64] Ramachandran, P., Young, G.E., Misawa, E.A., "Intersample Output Estimation with Multirate Sampling", Proceedings  Applications,  of the 1996 IEEE International  Conference on Control  (1996):576-581.  [65] Rehbinder, H., Ghosh, B.K., "Multi-rate fusion of visual and inertial data", Proceedings of the International  Conference  on Multisensor  Fusion  and Integration  for  Intelligent  Systems, (2001):97-102. [66] Richardson, J.M., Marsh, K . A . , "Fusion of Multisensor Data", The International of Robotics Research, Vol.7, No.6, (1998): 78-96.  Journal  Bibliography  178  [67] Ruokangas, C C , Black, M.S., Martin, J.F., Schoenwald, J.S., "Integration of Multiple Sensors to Provide Flexible Control Strategies", Proceedings of IEEE International Conference on Robotics and Automation, (1986): 1947-1953. [68] Sciavicco, L . , Siciliano, B., Modeling and Control of Robot Manipulators,  McGraw-Hill  Companies, Inc. (1996). [69] Simulink User's Guide, The MathWorks, Natwick, Massachusetts, (1995). [70] Smith, R . C , Nitzan, D., " A Modular Programmable Assembly Station", Proceedings  of ;  the 13 International Symposium on Industrial Robots and Robots 7, (1983): 5.53-5.75. th  [71] Smith, R . C , Cheeseman, P., "On the Representation  and Estimation of Spatial  Uncertainty", International Journal of Robotic Research, Vol.5, No.4, (1986):56-68. [72] Indy F3 Processor Board Technical Reference, Revision 1.01, Spectrum Signal Processing  Inc., (1998). [73] Indy F3 Processor Board User Guide, Revision 2.02, Spectrum Signal Processing Inc.,  (1999). [74] Spong, M.W., Vidyasagar, M . , Robot Dynamics & Control, John Wiley & Sons, (1989). [75] Stover, J.A., Hall, D.L., Gibson, R.E., " A Fuzzy-Logic Architecture for Autonomous Multisensor Data Fusion", IEEE Transactions on Industrial Electronics, Vol.43, No.3, (1996): 403-410. [76] TMS320C3x/C4x  Optimizing C Compiler User's Guide, Texas Instrument Inc., (1998).  [77] Weiss, L.E., Sanderson, A . C , Neuman, C P . , "Dynamic Sensor-Based Control of Robots with Visual Feedback", IEEE Journal of Robotics and Automation,  V o l . R A - 3 , No. 5,  (1987): 404-417. [78] Weller, G.A., Groen, F.C.A., Hertzberger, L.O., " A Sensor Processing Model Incorporating Error Detection and Recovery", Traditional and Non-Traditional Robotic Sensors (Henderson, TC, ed.), Vol. F63, Springer-Verlag, (1990):351-363. [79] Westmore, D.B., Wilson, W.J., "Direct Dynamic Control of a Robot Using an End-Point Mounted Camera and Kalman Filter Position Estimation", Proceedings of the IEEE Conference on Robotic and Automation, (1991 ):2376-2384.  [80] Wilkins, D.E., Myers, K.L., Lowrance, J.D., Wesley, L.P., "Planning and reacting in uncertain and dynamic environments", Journal of Experimental  and Theoretical  Artificial  Intelligence, Vol.6, (1994): 197-227. [81] Williams, B.C., Nayak, P.P., " A Reactive Planner for a Model-based Executive", Proceedings of the International Joint Conference on Artificial Intelligence,  (1997).  Bibliography  179  [82] Williams, B.C., Nayak, P.P., " A Model-based Approach to Self-Configuring Systems", Proceedings of the National Conference on Artificial  Intelligence, (1996).  [83] Zheng, Y.F., "Integration of Multiple Sensors into a Robotic System and its Performance Evaluation", IEEE Transactions on Robotics and Automation,  669.  Vol.5, No.5, (1989):658-  Appendix A X Y Table Circuits A . l Preamble In this appendix, the custom circuits designed and built in order to commission the experimental Cartesian manipulator are presented. This section provides details of the circuits design themselves and their integration in the system. Their management through ORTS is described in Appendix B, C, and D.  The X-axis D C amplifier design is presented first, followed by the Y-axis stepper motor driver/indexer. A dual inductive limit switches driver is then presented. The limit switches driver circuit has been developed and tested, but has not been used for the current application. Finally, a switching circuit is presented. This particular circuit is used in order to generate hardware fault by interrupting the communication between the optical encoder and the I/O board.  A.2 X-Axis Linear Push-Pull DC Amplifier The X-axis is powered through a DC motor and a lead screw. In order to servo the speed and the position of the manipulator end-effector, an amplifier is necessary to manage the power 180  A.2 X-Axis Linear Push-Pull DC Amplifier  181  delivered to the motor and insulate the control side of the circuit, performed on a DSP board, of the power side of the circuit.  The combination amplifier/DC motor is configured to be used in voltage mode. Therefore the motor is submitted to a given voltage and draws the current necessary to follow the position setpoint. The voltage configuration requires less bandwidth than the current counterpart but leads to lower tracking performance  due to longer rise-time. Nevertheless, since the tracking  performance requirements of the system are not that stringent and the voltage mode leads to easier controller implementation, this approach is considered sufficient.  In voltage mode, the amplifier acts as a gain, as shown by its input/output characteristic, repeated here for convenience.  Linearized X-Axis Amplifier Characteristic  25 ..  1  1  1  1  1  1  1  Input Voltage [V]  Figure A. 1. D C Amplifier Input/Output Characteristic  Table A . l , Table A.2, and Figure A.2 present the amplifier specifications, the amplifier parts list, and the amplifier circuit schematic, in that respective order. The circuit requires the presence of heat sinks on the collector electrodes of the Darlington transistors to dissipate heat generated by  A.2 X-Axis Linear Push-Pull D C Amplifier  182  the current flowing through the device. It is also recommended not to use the full power output provided by the transistors to minimize the amount of heat generated and increase the robustness of the amplifier.  Linear Push-Pull D C Amplifier, Specs Designed by  Gordon Wright  Built by  David Langlois  Date  Fall 2000 / X Y Table Power Supply box  Location Input Voltage  0-10 V D C  Output Voltage  -17.47 to 18.89 V D C  Output Current (Max.) Balance Adjustment Range  12 A (NTE 2343) Oto 12 V D C 24 V D C  Supply  Table A . l . DC Amplifier Specifications  Linear Push-Pull D C Amplifier, Parts Lists Part#  Quantity  T-PNP Darlington Transistor [NTE 2344]  2.  T-NPN Darlington Transistor [NTE 2343]  2  Fast Recovery Rectifier [FR601CT]  4  Low Power Quad Operational Amplifier [ L M 324]  1  Resistor 100 Q - % W  2  Resistor 24.9 kQ - !/ W  3  Resistor 49.9 kQ - '/ W  1  Voltage Regulator [7812]  1  Capacitor 1.0 u F - 35 V  2  Potentiometer 10 kQ  1  4  4  Table A.2. D C Amplifier Parts List  A.3 Unipolar Stepper Driver  183  24 VDC  24 VDC  SF62 12 VDC  SF62  Adjust 5 VDC  24 VDC  LM 324  INPUT 0-10 VDC  Figure A.2. D C Amplifier Circuit Schematic  A.3 Unipolar Stepper Driver In order to power the stepper motor used on the Y-axis, a stepper motor driver/indexer is necessary. In a stepper motor, the rotor motion is generated by sequentially powering the windings located around the circumference of the rotor. A custom stepper motor driver is built around a commercially available chip, the Allegro MicroSystems Inc. UCN5804B. Using this chip, only a step signal needs to be generated in order to generate the rotation of the motor shaft. Additional bits are used to configure different parameters, such as rotation direction, drive format, and enable. A l l the configuration bits can be modified on the fly. More infonnation regarding the design of the chip itself and its specifications can be found on their web site [http ://www .allegromi cro. com/].  The original design from Allegro MicroSystems is modified to minimize the heat dissipated through the chip casing. Flyback diodes are added outside of the casing and pin #2 & #7 are left unconnected. Also, a heat sink is soldered between the ground tabs (i.e., pins #4,5,12, & 13) to maximize the heat sinking capability of these last ones. Since the driver sinks current from the motor windings, both these modification are greatly increasing the robustness of the driver. A voltage regulator is used to generate the 5 V D C supply of the chip from the 24 V D C feed. Finally, a CMOS buffer is added on the digital input lines to convert the TTL digital signal of the DSP board to the required CMOS signal.  A.3 Unipolar Stepper Driver  184  Stepper Motor Driver/Indexer, Specs Designed by  Allegro MicroSystems Inc.  Modified by  Gordon Wright  Built by  David Langlois  Date  Fall 2000  Location  X Y Table Power Supply box  Input Format  TTL or CMOS  Supply  24 V D C  Table A.3. Stepper Motor Driver Specs Table A.3 presents the specification of the stepper driver. Figure A.3 presents the modified version of the stepper driver. On this figure, complementary elements such as the C M O S buffer and the voltage regulator have been omitted for sake of clarity. When wiring the motor with the stepper driver, one needs to match the motor stepping sequence with the driver stepping sequence. As specified on the 5804 chip specs sheet, the driver sequence is pin #8 - pin #1 - pin #6 - pin #3. On the motor side, using the motor wiring harness wires colors, the sequence is yellow - blue - green - white. The four other wires (i.e., red, gray, orange, and purple) are the common (i.e., drain) conductor.  24 VDC  5 VDC  • OUTPUT ENABLE DIRECTION  STEP INPUT HALF STEP O N E PHASE  Figure A.3. Stepper Driver Modified Circuit  A.4 Dual Limit Switches Driver  185  Stepper Motor Driver/Indexer, Parts List Part #  Quantity  Stepper Motor Translator/Driver [UNC5804]  1  Diode Rectifier 1A- 50V [1N4001]  8  Resistor 6,8 Q -2,5 W  2  Voltage Regulator [7805]  1  Capacitor 1.0uF-35 V  2  CMOS Hex Buffer/ Converters [CD4050]  1  Table A.4. Stepper Motor Driver Parts List  A.4 Dual Limit Switches Driver The automation of the available heteroceptive sensor (i.e., camera) calibration can be performed by the addition of limit switches at both extremities of the Cartesian manipulator axes. The presence of the limit switches at the end of the axis enable to accurately and repeatedly reach two given positions, from which the calibration grid of the camera can be established. In order to do so, four SunX GL-6HB inductive proximity sensors were used. Two of these switches have been integrated to the structure of the X-axis and interfacing circuitry built.  Limit Switches Driver, Specs Designed by  Glenn Jolly  Built by  David Langlois  Date  Fall 2001  Location  X Y Table Manipulator  Output Format  TTL or CMOS  Supply  12 V D C & 5 V D C  Table A.5. Limit Switches Driver Specifications The role of the interfacing circuitry is two convert the 0-12 V D C output of the sensor to a TTL signal that can be interfaced directly to the F3 Indy DSP board digital ports. A transistor-based circuit to convert the output of two inductive limit switches has been designed and built.  A.4 Dual Limit Switches Driver  186  LSW1 Brown Black Blue GL-6HB  LSW2  5 VDC  Brown Black Blue GL-6HB  Figure A.4. Dual Limit Switches Driver Driver Table A.5 presents the technical specifications of the inductive limit switches driver circuit. The details of the circuitry can be found from Figure A.4 where the circuit schematic is provided. This circuit could be used to interface any type of sensor presenting a N P N transistor output driver to a TTL port.  Limit Switches Driver, Parts List Part#  Quantity  SunX Inductive Proximity Sensor [GL-6HB]  2  N P N transistor [2SD1990]  2  Resistor 200 Q - 7 W  2  Resistor 1 kQ - 7 W  2  4  4  Resistor 4,7 kQ - %W Table A.6. Limit Switches Driver Parts List  A.5 Optical Encoder Switching Circuit  187  A. 5 Optical Encoder Switching Circuit The experimental testing of the policies developed in this thesis implies that it is possible to generate proprioceptive faults in a simple and repeatable manner. Particularly, the complete failure of the optical encoder hardware has been used as typical sensor fault. This type of fault is generated in the experimental system by interrupting the communication between the optical encoder and the I/O board used to interface this last one. The interruption of the communication is achieved in hardware by adding CMOS switches on the signal lines (i.e., Channel A , Channel B, and Channel Z) of the encoder cable. In reality, this type of fault can be caused by the occurrence of faults at many levels, such as an encoder hardware failure, a cabling failure, or a counter lock-up.  Table A.7 presents the circuit specifications. The circuit is designed to be serially use on the cable between the I/O board and the optical encoder. Therefore, the circuit uses the same supply voltage as the sensing device itself. This is made possible due to the low power consumption of the switching circuitry, as well as the optical encoder.  Optical Encoder Switching Circuit, Specs Designed by  David Langlois  Built by  David Langlois  Date  Fall 2001  Location  X Y Table Manipulator  Supply  5 VDC  Table A.7. Optical Encoder Switching Circuit Specifications The circuit schematic is provided by Figure A.5. The simple switching circuit couples two • C M O S switches in order to generate a double-pole/ single-throw switch for each channel used by the encoder. The switching of the three signal lines is synchronized through the use of only one toggle switch to control all the CMOS switches. Also, to avoid leaving the state lines floating, the second pole of the CMOS switches is pulled down through a current limiter resistor.  A.5 Optical Encoder Switching Circuit  188  Figure A.5. Optical Encoder Swiching Circuit Schematic Finally, Table A.8 provides a list of the major parts needed in order to built this circuit. The circuit schematic presented in Figure A.5 is the simplest version of this design and only requires few components. It is also possible to add components to make the circuit easier to use (e.g., LED's to indicate the state of the board).  Optical Encoder Switching Circuit, Parts List Part#  Quantity  CMOS Quad Bilateral Switch [CD4066B]  2  Hex Inverter [SN74LS04]  1  Resistor 4.7 kQ - !/ W  4  Toggle Switch DPDT  1  4  Table A.8. Optical Encoder Switching Circuit Parts List  Appendix B Low-level ELD's Code B . l Preamble This appendix contains the basic functions of the E L D architecture as developed on the F3 Indy DSP Board. These functions are written as ORTS processes and follow the conventions associated with the use of the open-architecture C N C platform. The processes are grouped around two main themes, the E L D oriented processes and the Fusion oriented processes. The first section presents the E L D base processes regarding communication and parsing oh the DSP platform. The general communication definitions are also provided. That section concludes by presenting the E L D processes related to trajectory generation. The second section is concerned with the Fusion oriented processes. In this section, the code necessary to implement the HGRF type of fusion for a unidimensionnal case where two sensors are used is presented. This appendix concludes with the presentation of the code to implement the uncertainty quantification metric on the DSP platform, as well as the Newton-Gregory interpolator.  189  B.2 E L D Functions  B.2 E L D Functions B.2.1 ELD Basic Parsing Functions #include "gen-dsp.h" #include "kernel.h" #include <math.h> #include "ELD.h" #include "matrix.h" #ifdef_FPF_ELD_RECIPIENT INTERPRETER /* Fast DSP process to parse command coming from the high level ELD /* architecture through the pipes to the good DSP ELD. /*  /* by David Langlois, January 2001 /* Industrial Automation Laboratory /* University of British Columbia /* /* Interpret the recipients based on the structure prototype and on the ELD /* keywords defined in ELD.h. I*  I* The parameters and links are established in function of the architecture /* requirements and functionnalities I* I* ELD_RECIPIENT_INTERPRETER(),input=(ELD_PIPE_IN), output=(ELD#l,ELD#2,...); typedef struct { Link *inputlinkO; Link *outputlinkO; Link *outputlinkl; Link *outputlink2; } SEld_recipient_interpreter; #define inputlinkO #define outputlinkO #define outputlinkl #define outputlink2  */ */ */ */ */ */ */ */ */ */ */ */ */ */  /* process data structure */ /*variables*/ /*links*/ /* links */ /* type name */ (p->inputlinkO) (p->outputlinkO) (p->outputlinkl) (p->outputlink2)  rtncode FPF_Eld_recipient_interpreterInit  SEldrecipientinterpreter *p;  (int NumlnputLinks, Link **InpulLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void **Ptr) /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SEld_recipient_interpreter)))==NULL)  return R D S P O u t o f m e m o r y ;  /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */  B.2 E L D Functions  191  if(NumInputLinks!=l) if(NumOutputLinks!=3) if(NumParameters!=0)  return RRCPJnvalidnumberof inputlinks; return RRCPJnvalidnumberofoutputlinks; return R_RCP_Invalid_number_of_parameters;  if(InputLink[0]->ItemSize !=10) if(OutputLink[0]->ItemSize!=10) if(OutputLink[ 1 ]->ItemSize!= 10) if(OutputLink[2]->ItemSize!=l 0).  return return return return  RRCPInvalidinputlinksize; RRCPInvalidoutputlinksize; RRCPInvalidoutputlinksize; R_RCP_Invalid_output_link_size;  /* assign link pointers */ inputlinkO=InputLink[0]; outputlink0=OutputLink[0]; outputlink 1 =OutputLink[ 1 ]; outputlink2=OutputLink[2]; /* exit initialization function to start continuous function */ return RSuccess; i i  #undef #undef #undef #undef  inputlinkO outputlinkO outputlinkl outputlink2  #define inputlinkO #define outputlinkO #define outputlink 1 #define outputlink2  (((SEld_recipient_inteipretci*)Ptr)->inputlinkO) (((SEld_recipient_interpreter*)Ptr)->outputlinkO) (((SEld_recipient_interpretcr*)Ptr)->outputlinkl) (((SEld_recipient_interpretcr*)Ptr)->outputlink2)  void FPFELDRECIPIENT INTERPRETER {  SCommDSP SCommDSP int float  (void *Ptr)  ELDRecipientln; ELDError; test; time;  /*Check input link for a value to read*/ /*do not remove get rid of the pipe memory dump on first reading*/ test = LinkReadyToRead(inputlinkO); if (test !=0) { /* Read Pointer Coming From the Pipe*/ /* Gave the address of the variable with the good type*/ LinkRead(inputlinkO,&ELDRecipientIn); /*get time for pipe timing on Initialize*/ time = GefTime(); //Print("insertion time:%f',time);  addressed*/  /* Output command signal to prescribed recipient*/ switch ((int)(ELDRecipientIn.recipieht))  /* Determine to which ELD the message is  {  case X_AXIS: /* Recipient is X AXIS ELD*/ /* Print ELD name in message window*/ //Print("Xaxis");  B.2 E L D Functions  192 /* if command is initialize write insertion time in p2*/ if ((int)ELDRecipientln.command == SYNCHRONIZETIME) {'  ELDRecipientIn.p2 = time; }  /* Pass structure to corresponding recipient*/ if(LinkReadyToWrite(outputlinkO))LinkWrite(outputlinkO,&ELDRecipientIn); break; case Y_AX1S: /* Recipient is Y_AXIS ELD*/ /* Print ELD name in the message window*/ //Print("Yaxis"); /* Pass structure to corresponding recipient*/ if(LinkReadyToWrite(outputlinkl))LinkWrite(outputIinkl,&ELDRccipientln); break; caseXYAXIS: /* Recipient is both axis ELD*/ /* Print ELD name in message window*/ //PrintfXYaxis"); /* Pass structure to corresponding recipients*/ /•Xaxis*/ if(LinkReadyToWrite(outputlinkO)) LinkWrite(outputlinkO,&ELDRecipientIn); /*Yaxis*/ ifTLinkReadyTo Write(outputlink 1)) LinkWrite(outputlinkl ,&ELDRecipientIn); break; case EVERYONE: /* Recipient is all ELD*/ /* Print ELD name in message window*/ //Print("EVERYONE"); /* Pass structure to corresponding recipients*/ /*Xaxis*/ if(LinkReadyToWrite(outputlinkO))LinkWrite(outputlinkO,&ELDRecipientIn); /*Yaxis*/ if(LinkReadyToWrite(outputlinkl))LinkWrite(outputlinkl,&ELDRecipientIn); break; case ENCODER: /* Pass structure to corresponding recipients*/ test = LinkReadyToWrite(outputlinkO); if(tcst != 0) { //Print("before encoderwrite,%d",test); LinkWrite(outputlinkO,&ELDRecipientJn); /*Xaxis*/ /* Print ELD name in message window*/ //Print("ENCODER"); }  break; case ULTRASOUND: /* Print ELD name in message window*/  B.2 E L D Functions  193  //Print("ULTRASOUND"); /* Pass structure to corresponding recipients*/ /*Yaxis*/ if(LinkReadyToWrite(outputlinkO))LinkWrite(outputlinkl,&ELDRecipientIn); break; default:  /* Unknown Recipient*/ /* Print error message in message window*/ //Print("UNKNOWN_ELD_NAME"); /* Reply now*/ /* Package Reply*/ ELDError.sender = ELDRecipientln.recipient; ELDError.recipient = ELDRecipientln.sender; ELDError.command = U N K N O W N E L D N A M E ; ELDError.handShaking = NONE; /•Write Packaged Information to the DSPtoPC link*/ if(LinkReadyToWrite(outputlink2))LinkWrite(outputlink2,&ELDError); break;  }  1 else { ;  /* do nothing*/  } }  #undef #undef #undef #undef  inputlinkO outputlinkO outputlinkl outputlink2  #endif/*_FPF_ELD_RECIPlENTINTERPRETER*/ #ifdef F P F E L D C O M M A N D I N T E R P R E T E R /* Fast DSP process to interpret command coming from the high level ELD /* architecture through the pipes. Use the recipient processed command /* generated by the ELD RECIPIENTINTERPRETER process  */ */ */  /*  */  /* by David Langlois, January 2001 */ /* Industrial Automation Laboratory */ /* University of British Columbia • •*/ /* */ /* Interpret the commands based on the structure prototype and on the command */ /* keywords already defined in ELD.h. */ /* */ /* Links are established in function of the architecture */ /* requirements and functionnalities */ /* */ /* ELD_COMMAND_INTERPRETER(),input=(CommandIn,feedback), output=(setpoint,DSPtoPC);*/ /*********************************^ typedef struct /* process data structure */ {  B.2 E L D Functions int float float float int float  Monitor; MValue; MSender; MRecipient; killlatch; latch;  Link *inputlinkO; Link *inputlinkl; Link *outputlinkO; Link *outputlinkl; Link *outputlink2; Link *outputlink3; } SEld_command_interpreter; #define Monitor #define MValue #define MSender #define MRecipient #define killlatch #define latch #define inputlinkO #define inputlinkl #define outputlinkO #define outputlinkl #define outputlink2 #define outputlink3  /•"variables*/  /*links*/ /* links */  (p->Monitor) (p->MValue) (p->MSender) (p->MRecipient) (p->kill_latch) (p->latch) (p->inputlinkO) (p->inputlinkl) (p->outputlinkO) (p->outputlinkl) (p->outputlink2) (p->outputlink3)  rtncode FPF_Eld_command_interpreterInit  {  SEldcommandinterpreter *p;  /* type name */  (int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void **Ptr) /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SEld_command_interpreter)))=NULL)  return RDSPOutofmemory;  /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumlnputLinks!=2) return RRCPInvalidnumberofinputlinks; if(NumOutputLinks!=4) return R_RCP_Invalid numberofoutputlinks; if(NumParameters!=0) return R_RCP_Invalid_number_ofj>arameters; if(InputLink[0]->ItemSize!=10) if(InputLink[l]->ItemSize!=l) if(OutputLink[0]->ItemSize!=l) if(OutputLink[ 1 ]->ItemSize!=10) if(OutputLink[2]->ItemSize!=3) if(OutputLink[3]->ItemSize!=2) /•Initialize Variables*/ Monitor = 0; MValue = 0; MSender = 0;  return R R C P Invalid_input_link_size; return R R C P Invalid_input_linksize; return R R C P Invalidoutputjink size; return R R C P Invalidoutputjinksize; return R R C P Invalidoutputlinksize; return R R C P Invalidoutputjinksize;  B.2 E L D Functions MRecipient = 0; killlatch = 1; latch = 0; /* assign link pointers */ inputlinkO=InputLink[0]; inputlinkl =InputLink[ 1 ]; outputlink0=OutputLink[0]; outputl ink 1 =Output Link [ 1 ]; outputlink2=OutputLink[2]; outputlink3=OutputLink[3];  /* from recipient*/ /* feedback*/ /* to axis*/ /* back to pc*/ /* fusionjrig*/ /* slow sensor*/  /* Initialize link to zero*/ LinkWriteFloat(outputlinkO,0); LinkWriteFloat(outputlink3,0); /* exit initialization function to start continuous function */ return R Success; i f  #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef  Monitor MValue MSender MRecipient killlatch latch inputlinkO inputlinkl outputlinkO outputlinkl outputlink2 outputlink3  #define Monitor #define MValue #define MSender #define MRecipient #define killlatch #define latch #define inputlinkO #define inputlinkl #define outputlinkO #define outputlinkl #define outputlink2 #define outputlink3  (((SEld command (((SEld command (((SEld" command (((SEld command (((SEld command (((SEld command (((SEld command (((SEld command (((SEld command (((SEld command (((SEld command (((SEld command  void F P F E L D C O M M A N D I N T E R P R E T E R float SCommDSP SCommDSP int int float float float  interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr)interpreter*)Ptr>  (void *Ptr)  inputsignal; ELDCommandln; ELDReply; Flag; test; init_array[3] = {O.Of O.Of, 1 .Of}; cam_array[2] = {0,0}; reset_array[3]={0,0};  >Monitor) >MValue) >MSender) >MRecipient) >killjatch) >latch) >inputlinkO) >inputlinkl) >outputlinkO) >outputlinkl) >outputlink2) >outputlink3)  B.2 E L D Functions  196  /* kill latch if it is the case */ if (killlatch = 1) {  LinkWrite(outputlink2,init_array); killlatch = 0; }  else if (killjatch = 2)  1 LinkWrite(outputlink3,reset_array); killjatch = 0; }  /* Read feedback value in case it is necessary to send it up*/ test = LinkReadyToRead(inputlinkl); if(test!=0) {  input_signal=LinkReadFloat(inputlink 1); //Print("feedbackread"); }  /* Monitoring function used when handshaking = WHEN DONE and command = SET_POINT*/ /* Check for feedback to be almost equal to set-point and then reply*/ if (Monitor == 1) { if (fabs(MValue - inputsignal) <= 4) { /* Reply now*/ /* Package Reply*/ ELDReply.sender = MSender; ELDReply.recipient = MRecipient; ELDReply.command = D O N E N O E R R O R ; ELDReply.handShaking = NONE; /•Write Packaged Information to the DSPtoPC link*/ LinkWrite(outputlinkl,&ELDReply); /* Reset Monitoring flag*/ Monitor=0; //Print("handshaking_sent%f .inputsignal); } }  test=LinkReadyToRead(inputlinkO); //Print("test%d",test); if(test!=0) /*check input link*/ {  /* Read Pointer Coming From the Pipe*/ /* write to address of the variable with structure type*/ //Print("linkreadbefore,%d",test); LinkRead(inputlinkO,&ELDCommandIn); the ELD*/  switch ((int)(ELDCommandIn.command)) {  case SET_POINT:  /* Determine what action is required from •  B.2 E L D Functions  command*/  197  /* Generate handshaking acordingly to the handshaking request of the switch((int) ELDCommandJn.handshaking)  { case NONE: break;  /*Do Nothing*/  case WHEN_DONE: /* Enable Feedback monitoring function until task is done*/ Monitor = 1; MSender = ELDCommandln.recipient; MRecipient = ELDCommandln.sender; MValue = ELDCommandln.p 1; break; case WHEN STARTING: /* Reply now*/ /* Package Reply*/ ELDReply.sender = ELDCommandln.recipient; ELDReply.recipient = ELDCommandln.sender; ELDReply.command = D O N E N O ERROR; ELDReply.handShaking = NONE; /*Write Packaged Information to the DSPtoPC link*/ LinkWrite(outputlinkl,&ELDReply); break; default: /* error message*/ break; }  /* Confirm Command on Message Window*/ //Print("SET_P01NT"); /* Output Set point to trajectory generator*/ test = LinkReadyToWrite(outputlinkO); if (test !=0) LinkWriteFloat(outputlinkO,ELDCommandIn.p 1); /* enable latch*/ //latch = ELDCommandIn.pl; }  else Print("Can not output setpoint"); break; case SEND OUTPUT: /* Generate handshaking acordingly to the handshaking request of the command*/ switch((int) ELDCommandln.handShaking) {  case NONE: break; /*Do Nothing*/ case W H E N D O N E : Flag = 1;  B.2 E L D Functions  198 break; case WHENSTARTING: /* Reply now*/ /* Package Reply*/ ELDReply.sender = ELDCommandln.recipient; ELDReply.recipient = ELDCommandln.sender; ELDReply.command = DON E N O E R R O R; ELDReply.handShaking = NONE; /*Write Packaged Information to the DSPtoPC link*/ LinkWrite(outputlinkl,&ELDReply); break; default: /* error message*/ //Print("UNKNOWN_HANDSHAKING_REQUEST"); break; }  /* Confirm Command on Message Window*/ //Print("SENDING_OUTPUT"); /*Package feedback signal to be sent to PC ELD who made request*/ //This hardcoded example should be replaced later by the real code ELDReply.sender = ELDCommandln.recipient; ELDReply.recipient = ELDCommandln.sender; ELDReply.command = OUTPUT; /* need to be checked*/ ELDReply.handShaking = NONE; ELDReply.pl = inputsignal; ELDReply.p6 = 4.73; //Print("beforetest"); /* Write Packaged Information to the DSPtoPC link*/ test = LinkReadyToWrite(outputlinkl); if(test != 0) { //Print("beforeWrite,%d",test); LinkWrite(outputlinkl ,&ELDReply); //Print("SENT"); }  else  Print("didn't write");  if(Flag == 1)  /* W H E N D O N E handshaking*/  {  /* Reply now*/ /* Package Reply*/ ELDReply.sender = ELDCommandln.recipient; ELDReply.recipient = ELDCommandln.sender; ELDReply.command = DONE NOERROR; ELDReply.handShaking = NONE; /•Write Packaged Infonnation to the DSPtoPC link*/ LinkWrite(outputlink 1 ,&ELDReply); //Print("Handshaking_Sent"); Flag=0;  B.2 E L D Functions  199 }  break; case EMERGENCYSTOP: /* Confirm Command on Message Window*/ //Print("EMERGENCY_STOP"); /* Need to add here E-stop command*/ /* Generate handshaking acordingly to the handshaking request of the command*/ switch((int) ELDCommandln.handShaking) {  case NONE: break;  /*Do Nothing*/  case WHENDONE: /* Reply now*/ /* Package Reply*/ ELDReply.sender = ELDCommandln.recipient; ELDReply.recipient = ELDCommandln. sender; ELDReply.command = DONE NO ERROR; ELDReply.handShaking = NONE; /*Write Packaged Information to the DSPtoPC link*/ Link Write(outputlink 1 ,&ELDReply); break; case WHENSTARTING: /* Meaningless for this command*/ break; default: /* error message*/ //Print("UNK.NOWN_HANDSHAKlNG_REQUEST"); break; }  break; case TUNECONTROLLER: /* Confirm Command on Message Window*/ //Print("TUNING_CONTROLLER"); break; case CHANGECONTROLLER: /* Confirm Command on Message Window*/ //Print("CHANGING CONTROLLER"); break; case SYNCHR0N1ZETIME: /* Confirm Command on Message Window*/ //Print("SYNCHRONIZING"); /* package array to pass to fusion process*/ init_array[0] = 1;  /*hard coded initcount*/  B.2 E L D Functions  200 init_array[l] = ELDCommandIn.p2; init_array[2] = 1; //Print("%f',ELDCommandIn.p2);  /* insertiontime*/  /* Enable Initializing routine in the fusion process*/ /* by passing the number of samples in the init routine*/ if (LinkReadyToWrite(outputlink2)) » i  LinkWrite(ourputlink2,init_array); //Print("%f' ,init_array[ 1 ]);  }  else  { }  Print("unable to write");  /* Kill the intra group link latch*/ killjatch = 1; break; case CONTINUOUS_OUTPUT: /* Confirm Command on the Message Window*/ //Print("CONTINUOUS_OUTPUT"); /* Package slow sensor measurement to pass to fusion process*/ cam_array[0] = ELDCommandln.p 1; cam_array[l] = ELDCommandln.p2; /* y measurement*/ /* Enable Initializing routine in the fusion process*/ /* by passing the number of samples in the init routine*/ if (LinkReadyToWrite(outputlink3)) {  LinkWrite(outputl ink3 .camarray); //Print("%f',cam_array[0]); } else { Print("unable to write"); }  /* Kill the intra group link latch*/ //killjatch = 2; break; case STARTFUSION: /* Confirm Command on Message Window*/ //Print("START_FUSION"); /* package array to pass to fusion process*/ init_array[0] = 0; /*hard coded initcount*/ init_array[l] = 0; /* insertionJime*/  B.2 E L D Functions init_array[2] = 0; //Print("%f',ELDCommandIn.p2); /* Enable the fusion process*/ /* by passing the flag 0*/ if (LinkReadyToWrite(outputlink2)) {  LinkWrite(outputlink2,init_array); Print("%f',init_array[l]); }  else { Print("unable to write"); }  /* Kill the intra group link latch*/ //killjatch = 1; break; default:  /* Unknown Command*/ /* Confirm Command on Message Window*/ //Print("UNKNOWN_COMMAND"); /*Send error message back to sender*/ /* Package Information */ ELDReply.sender = ELDCommandln.recipient; ELDReply.recipient = ELDCommandln.sender; ELDReply.command = UNKNOWN JV1 ESS AGE; ELDReply.handShaking = NONE; /* Write to output link DSPtoPC*/ LinkWrite(outputlinkl, &ELDReply); break;  else {  //test = LinkReadyToWrite(outputlink2); //if (test != 0) //{  //LinkWriteFloat(outputlinkO,latch); //LinkWriteFloat(outputlink3,latch); //init_array[0] = 0; //init_array[ 1 ] = 0; //init_array[2] = 1; //LinkWrite(outputlink2, init_array); //}  //Print("else");  #undef Monitor #undef MValue #undef MSender  /*do nothing*/  /*hard coded init_count*/ /* insertiontime*/  B.2 E L D Functions #undef #undef #undef #undef #undef #undef #undef #undef #undef  202  MRecipient killlatch latch inputlinkO inputlinkl outputlinkO outputlinkl outputlink2 outputlink3  #endif/*_FPFELD_COMMAND  INTERPRETER*/  B.2.2 ELD Structures II ELD.h : main header file for the ELD application /* Modified Version of ELD.h for the DSP level Implementation /* by David Langlois & Jason Elliott  */ */  /*  */  /* Industrial Automation Laboratory /* University of British Columbia /* January 2001 /************************************************  */ */ */  //ft******************************************* // GENERAL DEFINITIONS //******************** ************^ enum ELDMessages { // Commands going down in the architecture SENSE = 0, CALIBRATE, THRESHOLD, SETPOINT, DISPLAY, SEND_OUTPUT, SEND_CONTINUOUS_OUTPUT, START_CONTINUOUS_INPUT, ENDCONTINUOUSJNPUT, SYNCHRONIZETIM E, EMERGENCYSTOP, TUNECONTROLLER, CHANGECONTROLLER, STARTFUSION, INITIALIZE, STATUS, // Commands going up in the architecture QUERY_MANIPULATOR_X_POSITION_GT, REQUESTMANIPULATORXMOTION, //Replies DONENOERROR, DONE ERROR,  B.2 E L D Functions  203  CONTINUOUS_OUTPUT, NOTDONE, OUTPUT, REPLYQUERY, TIME_OUT, IDLE, // Errors UNKNOWNELDNAME, UNKNOWN MESSAGE, MESSAGE IGNORED  // parameters depend on what output it is  // pi = unknown ELD name // pi = unknown message // pi = ignored message  enum ELDs { /* ELD names */ EVERYONE = 0, POINTER LOCATOR, X_AXIS, Y_AXIS, XY_AXIS, JVC,  HITACHI, PLANNER, ENCODER, ULTRASOUND, USERINTERFACE };  enum handShakingModes { /* Types of handshaking that can be requested by the high level*/ NONE = 0, WHENDONE, WHEN_START1NG };  typedef struct /* Communication structure type definition*/ { float sender; float recipient; float command; float handshaking; // This is for hand shaking requests float p i ; float p2; float p3; float p4; float p5; float p6; } SCommDSP; typedef struct {  float data; JSPipeTest;  /* Pipe debugging structure type definition*/  B.2 E L D Functions  204  B.2.3 ELD Trajectory Generation Functions #ifdef F P F E L D T R A J G E N E R A T O R /* Fast DSP process to generate a trajectory based on the next setpoint /* issued by the system. The main purpose is to facilitate the life /* of the controller and avoid jerk caused by large setpoint variation  */ */ */  /*  */  /* by David Langlois, January 2001 /* Industrial Automation Laboratory /* University of British Columbia  */ */ */  /*  */  /* Generate a trajectory (linear) based on the gain parameter. /* Input link provide setpoint  */ */  /*  */  /* Output link provide continuous setpoint signal /* Could be later modified to include many type of interpolation passed by /* the E L D C O M M A N D I N TERPR ETER through an input link  */ */ */  /*  */  /* ELD_TRA.I_GENERATOR(Gain,noise_gain),input=(LinkIn), output=(outputlinks); ^******************************************************************************/ typedef struct /* process data structure */  */  {  float float float long float  gain; prevvalue; increment; rand_par; noisegain;  /*parameter*/  rtncode FPF_Eld_traj_generatorlnit(int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void **Ptr ) {  SEldtrajgenerator *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SEld_traj_generator)))==NULL) return RDSPOutofmemory; /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumInputLinks!=l) return RRCPInvalidnumberofinputlinks;  B.2 E L D Functions if(NumOutputLinks!=l) ifINumParameters!=2)  205 return RRCPJnvalidnumberof outputlinks; return R_RCP_Invalid_number_of_parameters;  if(InputLink[0]->ItemSizc!= 1) if(OutputLink[0]->ItemSize!=l)  return RRCPInvalidinputlinksize; return RRCPInvalidoutputlinksize;  /•Initialize Variables*/ gain = ParameterfO]; prevvalue = 0; increment = gain*0.25; rand_par = -1; noise_gain = Parameter[l]; /* assign link pointers */ inputlinkO=InputLink[0]; outputlink0=OutputLink[0]; /* exit initialization function to start continuous function */ return RSuccess; #undef #undef #undef #undef #undef #undef #undef  gain prevvalue increment rand_par noisegain inputlinkO outputlinkO  #define gain #define prev_value #define increment #define rand_par #define noisegain #define inputlinkO #defme outputlinkO  (((SEld_traj_gcnerator*)Ptr)->gain) (((SEld_trajgenerator*)Ptr)->prev_valuc) (((SEld_traj_generator*)Ptr)->increment) (((SEld_traj_generator*)Ptr)->randpar) (((SEld_traj_generator*)Ptr)->noise_gain) (((SEld_traj_generator*)Ptr)->inputlinkO) (((SEld_traj_generator*)Ptr)->outputlinkO)  void FPF_ELDTRAJ_GEN ERATOR { float newvalue = 0; float noise = 0;  (void *Ptr)  /*Read input link to check if there is a new set point value*/ new_value=LinkReadFloat(inputlinkO); if ((new_value-prev_value)> (increment/2)) {  /* new setpoint going up*/  prevvalue = prev_value + increment;  }  else if ((new_value-prev_value)< -(increment/2)) {  /*new setpoint going down*/  prev_value = prevvalue - increment;  }  else {  /* no new setpoint or reached final value*/ prev_value = newvalue;  B.2 E L D Functions  /* adds white noise to the signal*/ ran2(&rand_par,&noise); prev_value +=((noise-0.5) *noise_gain); /*Write current trajectory point to output link*/ LinkWriteFloat(outputlinkO,prev_value); }  #undef #undef #undef #undef #undef #undef #undef  gain prev_value increment rand_par noisegain inputlinkO outputlinkO  #endif/* FPF ELD TRAJ GENERATOR*/  /* Fast DSP process to generate a trajectory based on the /* 2 way points specified. This version is made to generate sequential /* trajectories for 1 axis. /* /* by David Langlois, September 2001 /* Industrial Automation Laboratory /* University of British Columbia /* /* Generate a trajectory (linear) based with specified gain. /* Input link start and stop repetitive motion. /* /* Output link provide continuous setpoint signal /* /* E L D C A R T T R A J lD(pointl ,point2),input=(LinkIn), output=(outputlinks); y************************************** typedef struct /* process data structure */ float float float int float float  . start_pt; stop_pt; pos; phase; increment; override;  Link *inputlinkO; Link *outputlinkO; } SEldcarttrajld; #define start_pt #define stop_pt #define pos #define phase #define increment #define override #define inputlinkO #define outputlinkO  (p->start_pt) (p->stop_pt) (p->pos) (p->phase) (p->increment) (p->override) (p->inputlinkO) (p->outputlinkO)  /*parameter*/  /*links*/ /* links */ /* type name */  B.2 E L D Functions  207  rtncode FPFEldcarttraj 1 dlnit  (int NumlnputLinks, Link **JnputLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void **Ptr)  {  SEldcarttraj 1 d *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SEld_cart_traj 1 d)))=NULL) return R_DSP_Out_of_memory; /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumInputLinks!=l) return RRCPInvalidnumberofinputlinks; if(NumOutputLinks!= 1) return RRCPInvalidnumberofoutputlinks; if(NumParameters!=2) return R_RCP_Invalid_number_of_parameters; if(InputLink[0]->ItemSize!=l) if(OutputLink[0]->ItemSize!=l)  return RRCPInvalid inputjinksize; return RRCPInvalidoutputJink size;  /•Initialize Variables*/ start_pt = Parameter[0]; stop_pt = Parameter[l]; pos =0; phase = 0;  /* starts from home*/  increment = ((stop_pt-start_pt)/fabs(stop_pt-start_pt))*0.05; override = 2;  /* used to be 0.0125*/  /* used to speed up normal interpolation*/  /* assign link pointers */ inputlinkO=lnputLink[0]; outputlink0=OutputLink[0]; /* exit initialization function to start continuous function */ return RSuccess; }  #undef #undef #undef #undef #undef #undef #undef #undef  .  start_pt stop_pt pos phase increment override inputlinkO outputlinkO  #define start_pt #define stop_pt #definepos #define phase #define increment #define override #define inputlinkO  (((SEldcarttraj 1 d*)Ptr)->start_pt) (((SEld_cart_trajld*)Ptr)->stop_pt) (((SEld_cart_trajld*)Ptr)->pos) (((SEld_cart_trajld*)Ptr)->phase) (((SEld_cart_trajld*)Ptr)->increment) (((SEld_cart_trajld*)Ptr)->override) (((SEld_cart_traj 1 d*)Ptr)->inputlinkO)  .  .  .  .  .  .  B.2 E L D Functions #define outputlinkO  208 (((SEldcarttraj ld*)Ptr)->outputlinkO)  void F P F E L D CART TRAJ1D { float command = 0;  (void *Ptr)  /*Read input link to check what is the command*/ /* 1 start the motion patern*/ /* 0 stop the motion pattern*/ command = LinkReadFloat(inputlinkO); /* Start motion assuming starting point is home*/ if (command = 1 ) { /* first move to start point*/ if (phase == 0) {  /* this increment is always positive*/ pos = pos + fabs(increment); /* check for phase 1 completion*/ if (pos >= start_pt) { phase = 1; } }  /* Move to stop point*/ else if (phase == 1) {  /* positive increment*/ pos = pos + increment; if ((pos >=stop_pt) && (increment >= 0)) {  phase=2; } else if ((pos <= stop_pt)&&(increment<0)) {  }  phase=2;  }  /* Move back to start point*/ else if (phase=2) {  /•negative increment*/ pos = pos - increment; if ((pos <= start_pt)&&(increment >= 0)) {  phase = 1; }  else if ((pos >= start_pt)&&(increment<0)) {  phase=l;  B.2 E L D Functions  209 }  } }  else if (command—0)  { /*move back home*/ if(pos>0)  ! /* this increment is always negative*/ pos = pos - fabs(increment*override); }  else { }  pos =0;  phase =0;  /* reset phase*/  } else {  /* usual interpolation*/  if ((command-pos)> fabs(increment/2))  /* new setpoint going up*/  pos = pos + fabs(override*increment); else if ((command-pos)< -fabs(increment/2)) /*new setpoint going down*/ pos = pos - fabs(override*incremeni); else  /* no new setpoint or reachedfinalvalue*/ pos = command;  ) /*Write current trajectory point to output link*/ LinkWriteFloat(outputlinkO,pos); }  #undef #undef #undef #undef #undef #undef #undef #undef  start_pt stop_pt pos phase increment override inputlinkO outputlinkO  #endi f /*_FPF_ELD_CART_TRAJ ID*/ #ifdef _FPF_ELD_CART_TRAJ2D  /***********************************^ /* Fast DSP process to generate a trajectory based on the /* 4 way points specified. This version is made to generate sequential /* motion for 2 axis.  */ */ */  B.2 ELD Functions  210  /* /* by David Langlois, September 2001 /* Industrial Automation Laboratory /* University of British Columbia /* /* Generate a trajectory (linear) based with specified gain. /* Input link start and stop repetitive motion. /* 1 => start; 0 => stop and move home; /* anything else is considered as the new set-point /* /* Output link provide continuous setpoint signal for each axis /* /* E L D C A R T TRA.I1 D(point 1 ,point2,point3,point4),input=(LinkIn), output=(x,y); typedef struct /* process data structure */ { float pt lx; /•parameter*/ float pt_ly; float pt_2x; float pt_2y; float pt_3x; float pt_3y; float pt_4x; float pt_4y; float pos_x; float pos_y; int phase; float increment; float override; Link *inputlinkO; Link *outputlinkO; Link *outputlinkl; } SEld_cart_traj2d; #define p t l x #define p U y #define pt_2x #define Pt_2y #define pt_3x #define p t j y #define pt_4x #define pt_4y #define pos_x #define posy #define phase #define increment #define override #define inputlinkO #define outputlinkO #define outputlink 1  /•links*/ /* links */ /* type name */ ;p->pt_lx) p->pt_ly) p->pt_2x) p->pt_2y) >->pt_3x) >pt_3y) p->pt_4x) >pt_4y) p->pos_x) p->pos_y) >phase) p->increment) 'p->override) >inputlinkO) p->outputlinkO) p->outputlinkl)  rtneode FPF_Eld_cart_traj2dInit (int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void ••Ptr)  */ */ */ */ */ */ */ */ */ */ */ */ */  B.2 E L D Functions {  SEld_cart_traj2d *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SEld_cart_traj2d)))=NULL)  return RDSPOutofmemory;  /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumInputLinks!=l) return RRCPInvalidnumberofjnputlinks; if(NumOutputLinks!=2) return RRCPInvalidnumberofoutputlinks; if(NumParameters!=8) return R_RCP_Invalid_number_of_parameters; if(lnputLink[0]->ltemSize!=l) if(OutputLink[0]->ItemSize!=l)  return RRCPInvalidinputlinksize; return RRCPJnvalidoutputJinksize;  /•Initialize Variables*/ p t l x = Parameter[0]; pt_ 1 y = Parameter[ 1 ]; pt_2x = Parameter[2]; pt_2y = Parameter[3]; pt_3x = Parameter[4]; pt_3y = Parameter[5]; pt_4x = Parameter[6]; pt_4y = Parameter[7]; posx =0; pos y =0; phase = 0;  /* starts from home*/ /* first move to start point*/  increment = 0.1; override = 2;  /* used to speed up normal interpolation*/  /* assign link pointers */ inputlinkO=InputLink[0]; outputlink0=OutputLink|0]; outputlinkl=OutputLink[ 1 ]; /* exit initialization function to start continuous function */ return RSuccess; #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef  ptlx ptly pt_2x pt_2y pt_3x pt_3y pt_4x pt_4y posx posy phase increment override  B.2 E L D Functions  212  #undef inputlinkO #undef outputlinkO #undef outputlink 1 #define pt_lx #define p t l y #define pt_2x #define pt_2y #define pt_3x #define pt_3y #define pt_4x #define pt_4y #define pos_x #define pos_y #define phase #define increment #define override #define inputlinkO #define outputlinkO #define outputlink 1  (((SEld_cart_traj2d*)Ptr)->pt_lx) (((SEld_cart_traj2d*)Ptr)->pt_ 1 y) (((SEld_cart_traj2d*)Ptr)->pt_2x) (((SEld_cart_traj2d*)Ptr)->pt_2y) (((SEld_cart_traj2d*)Ptr)->pt_3x) (((SEld_cart_traj2d*)Ptr)->pt_3y) (((SEld_cart_traj2d*)Ptr)->pt_4x) (((SEld_cart_traj2d*)Ptr)->pt_4y) (((SEld_cart_traj2d*)Ptr)->pos_x) (((SEld_cart_traj2d*)Ptr)->pos_y) (((SEld_cart_traj2d*)Ptr)->phase) (((SEld_cart_traj2d*)Ptr)->incrcment) (((SEld_cart_traj2d*)Ptr)->ovenide) (((SEld_cart_traj2d*)Ptr)->inputlink0) (((SEld_cart_traj2d*)Ptr)->outputlinkO) (((SEld_cart_traj2d*)Ptr)->outputlink 1)  void F P F E L D C A R T T R A J 2 D r float command = 0; float signx; float sign_y;  (void *Ptr)  /*Read input link to check what is the command*/ /* 1 start the motion patern*/ /* 0 stop the motion pattern and move back home*/ /* anything else is considered as a new set-point*/ command = LinkReadFloat(inputlinkO); /* Start motion assuming starting point is home*/ if (command == 1) {  /* first move tofirstpoint*/ if (phase = 0) {  /* this increment is always positive*/ posx = pos_x + increment; posy = pos_y + increment; /* check for phase 0 completion*/ if ((pos_x >= pt_lx)&&(pos_y >= ptly)) {  phase = 1;  } else if (posx >= pt_lx) { /* hold back x position while y is still being incremented*/ pos_x = posx - increment; }  else if (posy >= pt_ 1 y) { /* hold back y position while x is still being incremented*/  B.2 E L D Functions  213 pos_y = posy - increment; }  else { }  }  /*do nothing, keep incrementing both of them*/  /* Move to second point*/ else if (phase = 1) {  /* find motion directions*/ if(floor(fabs(pt_2x-pt_lx)) !=0) { signx = (pt_2x-pt_lx)/fabs(pt_2x-pt_lx); }  else { }  signx = 1;  if(floor(fabs(pt_2y-pt_ly))!=0) { sign_y = (pt_2y-pt_l y)/fabs(pt_2y-pt_l y); }  else { }  sign_y = 1;  /* increment x and y*/ posx = posx + sign_x*increment; pos_y = pos_y + sign_y*increment; if ((signx >0)&&(pos_x >= pt_2x)) { posx = pt_2x; }  else if (sign_x <=0) { if (pos_x<=pt_2x) { posx = pt_2x; } }  if ((signy >0)&&(pos_y>=pt_2y)) { pos_y = pt_2y; }  else if (sign_y <=0) { if (posy <= pt_2y) { posy = pt_2y; } }  B.2 E L D Functions  214  if ((posx == pt_2x)&&(pos_y==pt_2y)) { phase=2; } }  /* Move to third point*/ else if (phase = 2) { /* find motion directions*/ if(floor(fabs(pt_3x-pt_2x)) !=0) { signx = (pt_3x-pt_2x)/fabs(pt_3x-pt_2x); i else { signx = 1; }  if(floor(fabs(pt_3y-pt_2y))!=0) sign_y = (pt_3y-pt_2y)/fabs(pt_3y-pt_2y); }  else { sign_y=l; }  /* increment x and y*/ posx = posx + sign_x*increment; posy = pos_y + sign_y*increment; if ((sign_x >0)&&(pos_x >= pt_3x)) { posx = pt_3x; }  else if (signx <=0) { if (pos_x<=pt_3x) { posx = pt_3x; }  }  if ((signy >0)&&(pos_y>=pt_3y)) { posy = pt_3y; } else if (signy <=0) {  if (pos_y <= pt_3y) { posy = pt_3y; }  }  B.2 E L D Functions if ((posx == pt_3x)&&(pos_y==pt_3y)) { phase=3; }  }  /* Move to fourth point*/ else if (phase = 3) {  /* find motion directions*/ if (floor(fabs(pt_4x-pt_3x)) != 0) {  signx = (pt_4x-pt_3x)/fabs(pt_4x-pt_3x);  }  else { signx = 1; } if(floor(fabs(pt_4y-pt_3y))!=0) { sign_y = (pt_4y-pt_3y)/fabs(pt_4y-pt_3y); }  else { sign_y = 1; }  /* increment x and y*/ posx = pos_x + sign_x*increment; pos_y = pos_y + sign_y*increment; if ((signx >0)&&(pos_x >= pt_4x)) { posx = pt_4x; }  else if (signx <=0) { if (pos_x<=pt_4x) { posx = pt_4x; }  }  if ((sign_y >0)&&(pos_y>=pt_4y)) { posy = pt_4y; }  else if (signy <=0) { if (pos_y <= pt_4y) { pos_y = pt_4y; } }  if ((posx == pt_4x)&&(pos_y==pt_4y))  B.2 E L D Functions  216 {  phase=4;  >  }  /* Move to first point*/ else if (phase = 4) {  /* find motion directions*/ if (floor(fabs(pt_lx-pt_4x)) != 0) {  signx = (pt_lx-pt_4x)/fabs(pt_lx-pt_4x);  }  else { signx = 1;  1 if (floor(fabs(pt_l y-pt_4y))!=0) { sign_y = (pt_ly-pt_4y)/fabs(pt_ly-pt_4y); }  else { }  signy = 1;  /* increment x and y*/ pos_x = pos_x + sign_x*increment; posy = pos_y + sign_y* increment; if ((signx >0)&&(pos_x >= ptlx)) { pos_x = pt_lx; } else if (signx <=0) { if (pos_x<=pt_ 1 x) { posx = pt_lx; } }  if ((signy >0)&&(pos_y>=pt_l y)) { pos_y = pt_ly; >  else if (signy <=0) { if (posy <= p t l y ) { pos_y = pt_ly; }  }  if ((pos_x == pt_lx)&&(pos_y=pt_ly)) {  B.2 E L D Functions  217 phase=l;  else if (command==0) /*move back home*/ if(pos_x>0) {  /* this increment is always negative*/ posx = pos_x - (override*increment); }  else {  posx =0;  }  if (pos_y > 0) { /* this increment is always negative*/ pos_y = posy - (override*increment); }  else { posy =0; }  phase =0; else  -  /* reset phase*/  /* usual interpolation*/  {  /* x axis*/ if ((command-pos_x)> fabs(increment/2))  /* new setpoint going up*/  {  posx = posx + (override*increment); else if ((command-pos_x)< -fabs(increment/2)) { posx = posx - (override*increment);  /*new setpoint going down*/  1  else { }  /* no new setpoint or reached final value*/ pos_x = command;  /* y axis*/ /* fixed set-point is assumed*/ command = 25; if ((command-pos_y)> fabs(increment/2))  /* new setpoint going up*/  {  posy = posy + fabs(override*increment); }  else if ((command-pos_y)< -fabs(increment/2))  /*new setpoint going down*/  B.3 Fusion Functions  218  { }  posy = posy - fabs(override*increment);  else  /* no new setpoint or reachedfinalvalue*/  {  pos_y = command; } //posy = 0; /* Y axis is kept fix*/ } /* debug call*/ //Print("%f\(float)phase); /*Write current trajectory point to output link*/ LinkWriteFloat(outputlinkO,pos_x); LinkWriteFloat(outputlinkl,pos_y); }  #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef  ptlx ptly pt_2x pt_2y pt_3x pt_3y pt_4x pt_4y posx posy phase increment override inputlinkO outputlinkO outputlinkl  #endif /*_FPF_ELD_CART TRAJ2D*/  B.3 Fusion Functions B.3.1  Unidimensional HGRF for two Sensors  #ifdef FPFDATAFUSION2 /***********************************^ /* Fast DSP process to perform data fusion in a multisensor system /* /* /* by David Langlois, September 2001 /* Industrial Automation Laboratory /* University of British Columbia /* /* Performs Elliott data fusion based on the mean and variance of the input /* signals. /*  */ */ */ */ */ */ */ */ */ . */  B.3 Fusion Functions  219  /* Input is composed of two links of size two (mean,var) and one of size 3 /* link #1: fast sensor measurement and variance /* link #2: interpolated slow sensor measurement and variance /* link #3: initialization trigger signal /* Output link #1 is a link of size two (fused_mean,fused_var) /* ParameterO is used to select fusion[0],bypassl[l],bypass2[2] /*  .  .  .  .  .  .  .  /* DATA_FUSION2(par),input=(mean 1 ,varl ;mean2,var2),output=(f_mean,f_var); typedef struct {  /* process data structure */ /* parameters */  int flag; Link *inputlinkO Link *inputlinkl Link *inputlink2 Link *outputlinkO; SData_fusion2;  #define flag #define inputlinkO #define inputlinkl #define inputlink2 #define outputlinkO  */ */ */ */ */ */ */ */  /*links*/ /* links */ /* type name */ (p->flag) (p->inputlinkO) (p->inputlinkl) (p->inputlink2) (p->outputlinkO)  rtneode FPF_Data_fusion2Init(int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void **Ptr) SData_fusion2 *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SData_fusion2)))==NULL)  return R D S P O u t o f m e m o r y ;  /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumInputLinks!=3) return R R C P Invalidnumberofjnputlinks; if(NumOutputLinks!=l) return RRCPInvalidnumberofoutputJinks; if(NumParameters!=l) return RRCPInvalidnumber_of_parameters; if(InputLink[0]->ItemSize!=2) if(InputLink[l]->ItemSize!=2) if(InputLink[2]->ItemSize!=3) if(OutputLink[0]->ItemSize!=2)  return return return return  R_RCP_Invalid_input_link_size; RRCPInvalidinputlinksize; RRCPInvalidinputlinksize; RRCPInvalidoutputlinksize;  /* Assign Parameters*/ flag = Parameter[0];  /* bypass selection*/  /* assign link pointers */ inputlinkO=InputLink[0]; inputlink 1 =InputLink[ 1 ]; inputlink2=InputLink[2]; outputlinkO=OutputLink[0];  /* fast sensor*/ /* slow sensor measurement from KF*/ /* initialization triggenlenght + insertion_time*/ /* fused measurement and variance*/  B.3 Fusion Functions  220  /* exit initialization function to start continuous function */ return RSuccess; }  #undef #undef #undef #undef #undef  flag inputlinkO inputlinkl inputlink2 outputlinkO  #define #define #define #define #define  flag inputlinkO inputlinkl inputlink2 outputlinkO  (((SData_fusion2*)Ptr)->flag) (((SData_fusion2*)Ptr)->inputlinkO) (((SData_fusion2*)Ptr)->inputlinkl) (((SData_fusion2*)Ptr)->inputlink2) (((SData_fusion2*)Ptr)->outputlinkO)  void F P F D A T A F U S 1 0 N 2 (void *Ptr) { float signal 1 [2]; float signal2[2]; float time; float out_vector[2]; float SD1,SD2; float vl,v2; float fusedmean,fusedVar,fusedSD,K,sigma_n,c; float trig_in[3]= {0,0,0}; /* Check trig signal to start fusion*/ if(LinkReadyToRead(inputlink2)) {  /* check for and read trig signal*/ LinkRead(inputlink2 ,&trig_in); flag = (int)trig_in[2]; } switch(flag) {  case 0:  /*Fuse signals*/ /*Readsignal#l*/ LinkRead(inputlink0,signal 1); LinkRead(inputlinkl,signal2); /* fuse signal#l and signal#2*/ SD1 = sqrt(signal 1 [1 ]); //SD1 =sqrt(2); //SD2 = sqrt(2); SD2 = sqrt(signal2[l]);  /* signal#l standard deviation*/ /* signal#2 standard deviation*/  /* Fusion version 2.0*/ /*Nakamura's mean*/ fusedmean = (signal 1 [0]*signal2[ 1 ]+signal 1 [ 1 ]*signal2[0])/(signal 1 [ 1 ]+signal2[ 1 ]); /* v parameters*/ vl = (fusedmean-signall[0])/SDl;  B.3 Fusion Functions  221 v2 = (fusedmean-signal2[0])/SD2; /* Separation factor K*/ if(fabs(vl)>=fabs(v2)) { K = fabs(vl); } else { K = fabs(v2); } /* Nakamura's standard deviation*/ sigma_n = (SD 1 *SD2)/sqrt(signal 1 [ 1 ]+signal2[ 1 ]); /* Scaling factor according to K*/ if((K>=0) & & ( K < 1)) {  c = 1 + 0.273*pow(K,2) + 5.355*pow(K.,3) - 3.799*pow(K,4); } else { }  c= 1.41421356*(K+1);  fusedVar = pow((sigma_n*c),2); /* generate output vector*/ out_vector[0] = fusedmean; out_vector[l] = fusedVar; /* Write result to output link*/ LinkWrite(outputlinkO,out_vector); break; easel:  /*Bypass signal #1*/ /* Read input signals*/ LinkRead(inputlinkO,signal 1); LinkRead(inputlinkl ,signal2); /* Output signal #1*/ LinkWrite(outputlinkO,signal 1); break;  case 2:  /*Bypass signal #2*/ /* Read input signals*/ LinkRead(inputlinkO,signal 1); LinkRead(inputlinkl ,signal2); /* Output signal #2*/ LinkWrite(outputlinkO,signal2); break;  }  }  B.3 Fusion Functions #undef #undef #undef #undef #undef  222  flag inputlinkO inputlinkl inputlink2 outputlinkO  #endif/*_FPF_DATA_FUSION2*/  B.3.2 Uncertainty Quantification #ifdef_FPF_VAR_QUANTIF!ER y******************************************************************************/ /* Fast DSP process to quantify variance of sensor signals with respect to /* the system set-point signal.  */ */  /* /*  */ */  /* by David Langlois, September 2001 */ /* Industrial Automation Laboratory */ /* University of British Columbia */ /* */ /* Implement a modified variance definition where the mean of the measurement */ /* is taken as themean of the input signal. Use a window offixedsize. */ /* The size of the window set up the dynamic behaviour of the */ /* process. */ /* • . */ /* Input is composed of two links of size 1 */ /* link #1: sensor measurement */ /* link #2: set-point signal */ /* Output link #1 is the quantified variance */ /* ParameterO is used to specify baseline sensor variance */ /* */ /* VAR_QUANTIFIER(par),input=(sensor_signal,set-point),output=(variance); */ ^******************************************************************************/ typedef struct /* process data structure */ float float float int  baselinevar; data[10]; input[10]; window;  /* parameters */ /* size of the array must match window*/ /* size of the array must match window*/  Link *inputlinkO; Link *inputlinkl; Link *outputlinkO; } SVarquantifier; #define baselinevar #define data #define input #define window #define inputlinkO #define inputlink 1 #define outputlinkO  /*links*/ /* links */ /* type name */ (p->baseline_var) (p->data) (p->input) (p->window) (p->inputlinkO) (p->inputlinkl) (p->outputlinkO)  rtncode FPF_Var_quantifierInit(int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter,  B.3 Fusion Functions  223 float Freq, void **Ptr)  SVar_quantifier *p; int i;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SVar_quantifier)))==NULL)  return R_DSP_Out_of_memory;  /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumlnputLinks!=2) return R_RCP_Invalid_number_of_input_links; if(NumOutputLinks!= 1) return RRCPInvalidnumberofoutputlinks; if(NumParameters!=l) return RRCPInvalidnumberofparameters; if(InputLink[0]->ItemSize!=l) if(InputLink[ 1 ]->ItemSize!=1) if(OutputLink[0]->ItemSize!=l)  return RRCPInvalidinputlinksize; return RRCPInvalidinputlinksize; return R_RCP_Invalid_outpiit_link_size;  /* Assign Parameters*/ baselinevar = Parameter[0]; window = 4;  /* baseline variance*/ /* moving window size*/  for (i=0;i<window;i++) { data[i] = 0; input[i] = 0;  /* measurment array*/ /* input array*/  }  /* assign link pointers */ inputlinkO=InputLink[0]; inputlink 1 =InputLink [ 1 ]; outputlink0=OutputLink[0];  /* sensor feedback*/ /* axis set-point*/ /* quantified variance*/  /* exit initialization function to start continuous function */ return RSuccess; }  baseline_var data  #undef #undef #undef #undef #undef #undef #undef  window inputlinkO inputlinkl outputlinkO  #define #define #define #define #define #define #define  baselinevar data input window inputlinkO inputlinkl outputlinkO  input  void F P F V A R Q U ANTIFIER  (((SVar quantifier*)Ptr)- >baseline_var) (((SVar_ quantifier*)Ptr)- >data) (((SVar quantifier*)Ptr)- >input) (((SVar quantifier*)Ptr)- >window) (((SVar quantifier*)Ptr)- >inputlinkO) (((SVar quantifier*)Ptr)- >inputlinkl) (((SVar quantifier*)Ptr)- >outputlinkO) (void *Ptr)  B.3 Fusion Functions  int float float float float  224  insum = 0; in_mean = 0; sum_sq = 0; var = 0;  /* update windows of points*/ for(i=( window- l);i>0;i~) { input[i] = input[i-l]; data[i] = data[i-l]; }  /* get new measurement and new input*/ input[0] = LinkReadFloat(inputlinkO); data[0] = LinkReadFloat(inputlinkl);  /*sensor*/ /* input signal*/  /* Calculate mean of input signal*/ for(i=0;i<window;i++) {  in_sum = insum + input[i];  }  inmean = insum/window; /* Calculate variance*/ for(i=0;i<window;i-H-) {  sumsq = sumsq + (data[i]-in_mean)*(data[i]-in_mean);  var = (sum_sq/(float)(window-l))+baseline_var; /* Output result*/ LinkWriteFloat(outputlinkO,var); }  #undef #undef #undef #undef #undef #undef #undef  baselinevar data window input inputlinkO inputlinkl outputlinkO  #endif /*_FPF_VAR_QU ANTIFIER*/  B.3.3 Newton-Gregory Interpolator #ifdef_FPF_NG_INTERPOLATOR2 /* Fast DSP process to perform Newton-Gregory Interpolation in a multirate /* system where slow sensor need to be interpolated in order to analyticaly /* increase its bandwitdh /* /*  */ */ */ */ */  B.3 Fusion Functions  225  /* by David Langlois, September 2001 /* Industrial Automation Laboratory /* University of British Columbia  */ */ */  /*  */  /* Performs Newton-Gregory interpolation based on a second order model of the /* system. See Kreysig for more details on the process or Ramachandran for /* details on the implementation.  */ */ */  /*  '  /* Input is the slow sensor measurement, at slow rate. /* Output link#l is the predicted sensor measurement, at fast rate /* Output link #2 is a link of size 10 used to send info to high level /* ParameterO is used to specify the slow rate in Hz /*  */ */ */ */ : */  /*NG_INTERPOLATOR2(slow_freq),input=(slow_z),output=(z_hat,dsp2pc); /*******************************^ typedef struct /* process data structure */ int int int float float float float float float int float int  countslow; rate ratio; t; prevz; prev2_z; z; , deltafl; delta f2; delta2_fl; synch; reply_freq; synchreply;  Link *inputlinkO; Link *inputlinkl; Link *outputlinkO; Link *outputlinkl;  /* variables*/ /* previous measurement*/ /*previous-prcvious measurement*/ /* current measurement*/ /* prev_z-prev2_z*/ /* z-prev z*/ /* deltaJ2-deita_fl*/  /*links*/ /* links */  } SNg_interpolator2; #define #defme #define #define #define #define #define #define #define #define #define #define #define #define #define #define  */  countslow rateratio t prevz prev2_z z deltafl delta f2 delta2_fl synch replyfreq synchreply inputlinkO inputlinkl outputlinkO outputlinkl  /* type name */ (p->count_slow) (p->rate ratio) (p->t) (p->prev_z) (p->prev2 z) (p->z) (p->delta_fl) (p->delta_f2) (p->delta2_fl) (p->synch) (p->reply_freq) (p->synch_reply) (p->inputlinkO) (p->inputlinkl) (p->outputlinkO) (p->outputlinkl)  rtncode FPF_Ng_interpolator2Init (int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink,  */  B.3 Fusion Functions  226 int NumParameters, float *Parameter, float Freq, void **Ptr)  {  SNg_interpolator2 *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SNg_interpolator2)))==NULL) return R_DSPOut_of memory; /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(Num!nputLinks!=2) return R_RCP_Invalid_number_of_input_links; if(NumOutputLinks!=2) return R_RCP_Invalid_nuinber_of_output_links; if(NumParameters!=l) return RRCPInvalidnumber ofparameters; if(InputLink[0]->ItemSize!=l) if(InputLink[l]->ItemSize!=3) if(OutputLink[0]->ItemSize!=l) if(OutputLink[ 1 ]->ItemSize!= 10)  return RRCPInvalid inputjinksize; return RRCPInvalidinputlinksize; return RRCPInvalidoutputlinksize; return RRCPInvalidoutputlinksize;  /* Assign Parameters*/ count_slow = 1; rateratio = (int)floor(Freq/Parameter[0]); t = 1; prevz =0; prev2_z = 0; z = 0; delta_fl = 0; delta_f2 = 0; delta2_fl = 0; synch = 0; replyfreq = Parameter[0]; synchreply = 0; /* assign link pointers */ inputlinkO=InputLink[0]; inputlink 1 =InputLink[ 1 ]; outputlink0=OutputLink[0]; outputlink 1 =OutputLink[ 1 ];  /* rate divider counter*/ /* rate divider counter limit*/ /* interpolation index*/  /* fast sensor*/ /* trig signal*/ /* fused measurement and variance*/ /* back to pc*/  /* exit initialization function to start continuous function */ return RSuccess; }  #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef  countslow rateratio t prevz prev2_z z delta_fl delta_f2 delta2_fl synch replyfreq synchreply  B.3 Fusion Functions #undef #undef #undef #undef  inputlinkO inputlinkl outputlinkO outputlinkl  #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define #define  countslow rateratio t prevz prev2_z z deltafl delta f2 delta2 fl synch replyfreq synchreply inputlinkO inputlinkl outputlinkO outputlinkl  227  (((SNg_ interpolator2*)Ptr ->count_slow) (((SNg_ interpolator2*)Ptr' ->rate ratio) (((SNg_ interpolator2*)Ptr' ->t) (((SNg_ interpolator2*)Ptr' ->prev_z) («SNg_ interpolator *)Ptr" ->prev2 z) (((SNg_ interpolator2*)Ptr) ->z) (((SNg_ interpolator2*)Ptr ->delta fl) (((SNg_ interpolator2*)Ptr ->delta f2) (((SNg_interpolator2*)Ptr' ->delta2_fl) (((SNg_interpol ator2 *) P tr ->synch) (((SNg interpolator *)Ptr ->reply_freq) (((SNg_ interpolator2*)Ptr ->synch_reply) (((SNg_ interpol ator2 *)Ptr ->inputlinkO) (((SNg_ interpolator2*)Ptr ->inputlinkl) (((SNg_ interpolator2*)Ptr' ->outputlinkO) (((SNg_ interpolator2*)Ptr) ->outputlinkl)  void FPFNGINTERPOLATOR2 (void *Ptr) { float Y = 0; /* Y is the output signal*/ float s = 0; float trig_in[3] = {0,0,0}; float time; SCommDSP InitReply; /* count_slow need to be reseted on trig from high-level*/ /* to ensure synch between platforms*/ if(synch==0) { /* check trig signal*/ if(LinkReadyToRead(inputlink 1)) {  /* check for and read trig signal*/ LinkRead(inputlinkl ,&trig_in); /* trigger flag on trig*/ if((int)trig_in[0]) { synchreply = 1; }  if((synch_reply = 1) && (count_slow == rateratio)) {  time = GefTime(); if( LinkReadyTo Write(outputlink 1)) { InitReply.sender = X A X I S ; InitReply.recipient = X A X I S ; InitReply.command = D O N E N O E R R O R ; InitReply.handShaking = NONE;  B.3 Fusion Functions  228  InitReply.pl = replyfreq; /* reply frequency*/ InitReply.p2 = triginfl]; /* insertion time*/ InitReply.p3 = time; /* reply time*/ InitReply.p6 = time; LinkWrite(outputlink 1 ,&InitReply); i r  else { Print("unable to echo"); }  /* reset frequency divider counter*/ countslow = 1; /* disable time synch routine*/ synch = 1; synchreply = 0; /* Confirm Synch*/ Print("Synch"); }  } i  i  if (countslow = rateratio) i  /* update measurement table */ prev2_z = prevz; prevz = z; /* get measurement*/ z = LinkReadFloat(inputlinkO); /* update difference table */ deltafl = prevz - prev2_z; delta_f2 = z - prevz; delta2_fl = delta_f2 - deltafl; /* Output measurement*/ Y = z; /* reset interpolator counter*/ t=l; /* reset slow rate counter*/ count_slow= 1; }  else { /* evaluate s*/ s =(float) t/rate_ratio; /interpolate */ Y = z + s*delta_f2 + (s*(s-1 )/2)*delta2_f I; /* increment interpolator counter*/ t++; /* increment slow rate counter*/ count_slow ++;  B.3 Fusion Functions }  /* Output result*/ LinkWriteFloat(outputlinkO,Y); }  #undef #undcf #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef #undef  countslow rateratio t prevz prev2z z deltafl delta_f2 delta2_fl synch replyfreq synchreply inputlinkO inputlinkl outputlinkO outputlinkl  #endif/* FPF NG INTERPOLATOR2*/  229  Appendix C Miscellaneous O R T S Processes C l Preamble This appendix presents code for various ORTS processes. The first section presents the processes necessary to address the digital ports of the F3 lndy DSP Board. These ports, as well as the driver processes have been shown to be more reliable than the corresponding ones on the MFIO3B. These processes enable the user to write to, or read from, the digital port on a bitwise basis. The processes are also providing latching capacity (i.e., the port is read first and only the specified bits are modified). Finally, the second section presents ORTS processes corresponding to various mathematical functions and signal generation processes.  C.2 F3 lndy DSP Board Digital I/O Processes #include "gen-dsp.h' #include "kernel.h" #include <math.h> #define DIGIT A L R E G A D D R (unsigned int *) #define I N P U T M A S K #define OUTPUT_MASK #defineMAX ELEMENTS  0x200000 OxFFFFOOOO OxOOOOFFFF 20  #ifdef FPF F3 READ DIG  230  /* pointer to memory mapped digital I/O*/ /*mask to get rid of the lower 16 bits word*/ /*mask to get rid of higher 16 bits word*/ /* maximum number of channels */  C.2 F3 Indy DSP Board Digital I/O Processes  231  /************************************* /* /* F3 R E A D D I G /* /* by David Langlois, September 2000 /* Industrial Automation Laboratory /* University of British Columbia /* /* Fast DSP process used to read the digital port of the F3 Indy board /* Read bits of data from the DIGITAL 10 terminal /* D15..DO are the latched output data [used for output] /* D31..D16 are the data bits read from the digital IO terminal /* [used for input] /* all the bits between the startbit and the lastbit will be returned /* under a float format  •  .  .  j* .  /* /*  */  Usage: F3_READ_DIG(first_bit, last_bit),output=(OutputLink)  /*  /*  */ */ */ */ */ */ */ */ */ */ */ */ */ */ */ */ */  Modified February 2001; Changed decimal to binary conversion routine  typedef struct {  #define startbit #define lastj>it #define outputlinkO  */  /* process data structure */  (p->startj>it) (p->lastj>it) (p->outputlinkO)  rtneode FPF_F3_READ_DIGInit  (int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void **Ptr)  {  SF3_read_dig *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SF3_read_dig)))==NULL) return RDSPOutofjnemory; /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumOutputLinks !=1) return RRCPInvalidnumberofoutput links; if(NumParameters!=2) return R_RCPNo_paramcter_expected; /* initialize variable from parameters*/ startJ>it = Parameter[0]; lastJ>it = Parameterf 1 ]; if(OutputLink[0]->ItemSize!=(last_bit - startbit + 1))  return R RCP Invalidoutputjinksize;  C.2 F3 Indy DSP Board Digital I/O Processes /* parameters call error handlers*/ if ((startbit > 15) || (start_bit<0)) {Printflnvalid Value for the Start Bit");rcturn R RCP Error;} if ((lastbit >15) || (last bit < 0)) {Printflnvalid Value for the Last Bit");rcturn RJRCP_Error;} /* assign link pointers */ outputlink0=OutputLink[0]; /* exit initialization function to start continuous function */ return RSuccess; }  #undef startbit #undef lastbit #undef outputlinkO #define startbit #define lastbit #defme outputlinkO  (((SF3_read_dig*)Ptr)->start_bit) (((SF3_read_dig*)Ptr)->last_bit) (((SF3_read_dig*)Ptr)->outputlink0)  void FPF F3 READ_DIG(void *Ptr) { register int i; register int k; register int j=0; unsigned long invalue; float in_value_array[16]; floatfinal_value_array[16]; /•reading the input terminal, masking lower word and removing the lower word*/ invalue = (*D1GITAL_REG_ADDR & INPUTJV1ASK)/ 0x10000; /* creation of the array */ for (i=0;i<16;i++) { invaluearrayfi] =(float) (in_value-(in_value &~ 1)); in_value»=l; }  /* read the specified part of the array*/ for (k=start_bit; k<=last_bit; k++) { final_value_array[j] = in_valuc_array[k]; }  /* writing the final value to the output link*/ /*if(LinkReadyToWrite(outputlinkO))*/ LinkWrite(outputlinkO,final_value_array); }  #undef startbit #undcf lastbit #undef outputlinkO #endif/* FPF F3 READ DIG*/  232  C.2 F3 lndy DSP Board Digital I/O Processes  233  #ifdcf _FPF_F3_WR1TE_DIG_C0NST /* /* /* /* /* /*  by David Langlois, September 2000 Industrial Automation Laboratory University of British Columbia  */ */ */ */ */ */  Fast DSP process used to write on the digital port of the F3 lndy board Latches 16 bits of data from the DIGITAL IO terminal on the low word of the terminal. DI 5..DO are the latched output data [used for output] The bits where to write must be specified by the starting bit of the sequence and the last bit to write and the value by the third parameter Only the specified bits are modified, the rest of the word stays the the same as it was before  */ */ */ */ */ */ */ */  Usage F3_WRITE_DIG_CONST(start_bit, lastbit, value);  */ */ */ */ */  F3_WRITE_DIG_CONST  /*  /* /* /* /* /* /* /* /*  */  /*  /* /* /* /* /*  */  Modified February 2001; changed decimal to binary conversion routine modified accordingly index and test conditions  typedef struct  /* process data structure */  {  #define startbit #define lastbit #define value  (p->start_bit) (p->last_bit) (p->value)  rtncode FPF_F3_WRITE_DIG_CONSTInit (int NumlnputLinks, Link **InpufLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void **Ptr )  { SF3_write_dig_const *p;  /* create pointer to data structure  /* allocate memory for local variables */ if((p=Malloc(sizeof(SF3_write_dig_const)))==NULL) return R D S P O u t o f m e m o r y ; if((p=Malloc(sizeof(SF3_write_dig_const)))==NULL) return R_DSP_ /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumInputLinks!=0) return R_RCP_No_input_link_expected; if(NumOutputLinks!=0) return RRCPNooutputJinkexpected; if(NumParameters!=3) return R_RCP_No_paramcter_expected; /* initialize variables from parameters*/ startbit = Parameter[0];  C.2 F3 Indy DSP Board Digital I/O Processes  234  last_bit = Parameter! 1]; value = Parameter[2]; /* parameters call error handlers*/ if ((startbit > 15) || (start_bit<0)) {Printflnvalid Value for the Start Bit");return R RCPError;} if ((last_bit >15) || (lastbit < 0)) {Print("Invalid Value for the Last Bit");rcturn'R_RCP_Error;} /* Reset latch of the 10 port low-word to low*/ *DIG1TAL_REG_ADDR= 0x0000; /* exit initialization function to start continuous function */ return R_Success; }  #undef startbit #undef lastbit #undef value  \  #defme startbit #define lastbit #define value  (((SF3_write_dig_const*)Ptr)->start_bit) (((SF3_write_dig_const*)Ptr)->last_bit) (((SF3_write_dig_const*)Ptr)->value)  void FPF_F3_WRJTE_DIG_CONST  (void *Ptr)  {  unsigned int new_value = value; /*get the value to write*/ register int i,k; register int p=0; int numbits = 0; unsigned long previousvalue; unsigned int final_num_value=0; int prev_value_array[16]; int new_value_array[16]; intfinal_array[ 16]; /•read the previous value and get rid of the high word*/ previousvalue = *DIGITAL_REG ADDR; previousvalue = (previousvalue & OUTPUTMASK); /* write the previous value to an array*/ for(i=0;i< 16;i++) /*divide the number in binary digits*/ {  prev_value_array[i]=(previous_value-(previous_value previous_value»=l;  &~1));  } /*write the value to write to an array*/ numbits = 0; for(i=0;i<16;i++) /*divide the number in binary digits*/ i i  if (newvalue != 0) num_bits++; /* count the number of bits required to write the new value*/ new_value_array[i]=(new_value-(new_value new_value»=l; }  &~1));  C.2 F3 lndy DSP Board Digital I/O Processes  235  /* check if the word will fit in the defined word length parameter*/ if (numbits > (lastbit - startbit +1)) {Print("Warning: Word to write longer than the allocated space");/*return RRCPError;*/} /*creation of the final array from parts of previous array and new value*/ /* location to write to is given by parameters*/ for(k=0;k< 16;k++) { if( (k >= startbit) && (k <= lastbit)) { final_array[k]= new_value_array[p]; P++; }  else { }  final_array[k] = prev_value_array[k];  } ^conversion of the final array to a numerical value*/ for (k=0;k<16;k++) {  finalnumvalue = finalnumvalue + (pow(2,(k))* finalarrayfk]); } /* writing the final value to the digital port terminal*/ *DIGITAL_REG_ADDR= final_num_value; #undef startbit #undef lastbit #undef value #endif/*_FPF_F3_WRITE_DIG_CONST*/ #ifdef _FPF_F3_WR1TE_D1G /******************************************************************************/ /*  /*  F3 WRITED1G  */  */  I*  *!  /* by David Langlois, September 2000 /* Industrial Automation Laboratory /* University of British Columbia /* /* Fast DSP process used to write on the digital port of the F3 lndy board /* Latches 16 bits of data from the DIGITAL IO terminal /* on the low word of the terminal. /* D15. .DO are the latched output data [used for output] /* The bits where to write must be specified by the starting bit of the /* sequence and the last bit to write /* The value to write is passed by the input link /* /* Usage /* F3_WRITE_DIG(start_bit, last_bif),input=(value);  */ */ */ */ */ */ */ */ */ */ */ */ */ */  /*  */  C.2 F3 Indy DSP Board Digital I/O Processes  typedef struct { int startbit; int lastbit;  /* process data structure */  Link *inputlinkO, *outputlinkO; } SF3_write_dig;  /* link */ /* type name */  #define startbit #define lastbit #define inputlinkO #define outputlinkO  (p->start_bit) (p->last_bit) (p->inputlinkO) (p->outputlinkO)  /* start bit */ /* last bit */  rtncode FPF_F3_WRITE_DIGInit (int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParameters,float*Parameter, float Freq, void **Ptr) {  register int i; SF3_write_dig *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SF3_write_dig)))==NULL) return RDSPOutofmemory; /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumInputLinks!=l) return RRCPInvalidnumberofjnputlinks; //if(NumOutputLinks!=0) return RRCPNooutputlinkexpccted; if(InputLink[0]->ItemSize!=l) if(NumParameters!=2)  return R_RCP_Invalid_inpuMink_sizc;  return RRCPInvalidnumberofjwameters;  /* acquire parameters and initialize startbit and lastbit */ startbit = Parameter[0]; lastbit = Parameterfl]; /* verification of the parameters */ if ((startbit > 15) || (start_bit<0)) {Printflnvalid Value for the Start Bit");return R_RCP_Error;} if ((lastbit > 15) || (last_bit < 0)) {Printflnvalid Value for the last of Bit");retum R_RCP_Error;} /* assign link pointers */ inputlinkO=InputLink[0]; outputlink0=OutputLink[0]; /* exit initialization function to start continuous function */ return R_Success; ! #undef #undef #undef #undef  startbit lastbit inputlinkO outputlinkO  236  C.2 F3 lndy DSP Board Digital I/O Processes #define startbit #define lastbit #define inputlinkO #define outputlinkO  (((SF3_write_dig*)Ptr)->start_bit) (((SF3_write_dig*)Ptr)->last_bit) (((SF3_write_dig*)Ptr)->inputlinkO) (((SF3_write_dig*)Ptr)->outputlinkO)  void FPF_F3_WRITE_DIG  (void *Ptr)  237  register int i; register int k; register int p=0; int num_bits=0; unsigned long previousvalue; unsigned int final_num_value=0; unsigned int new_value; int prev_value_array[16]; int new_value_array[16]; int final_array[16]; /* read the new values to write*/ /*if(LinkReadyToRead(inputlinkO))*/ newvalue =(unsigned int) LinkReadFloat(inputlinkO); /*read the previous value and get rid of the high word*/ previousvalue = *DIGITAL R E G A D D R ; previousvalue = (previousvalue & OUTPUTMASK); /* write the previous value to an array*/ for(i=0;i<l 6;i++) /*divide the number in binary digits*/ {  prev_value_array[i]=(previous_value-(previous_value &~1)); previous_value»= 1; }  /*write the value to write to an array*/ for(i=0;i<16;i++) /*divide the number in binary digits*/ { if ( new_value != 0) num_bits++; /* count the number of bits required to write the new value*/ new_value_array[i]=(new_value-(new_value &~ 1)); new_value»=l; /* check if the word will fit in the defined word length parameter*/ if (numbits > (last_bit - startbit +1)) Print("Warning: Word to write longer than the allocated space"); /•creation of the final array from parts of previous array and new value*/ /* location to write to is given by parameters*/ for(k=0;k< 16;k++) { if( (k >= start_bit) && (k <= last_bit)) {  finalarray [k]= new_value_array[p]; P++;  C.3 Miscellaneous Processes  238  }  else { final_array[k] = prevvaluearrayfk]; >  }  /•conversion of the final array to a numerical value*/ for(k=0;k<16;k++) {  finalnumvalue = final_num_value + ( pow(2,k)* finalarrayfk]);  }  /* writing the final value to the digital port terminal*/ *DIGITAL REG ADDR= finalnumvalue; } #undef #undef #undef #undef  startbit lastbit inputlinkO outputlinkO  #endif/* FPF WRITE DIG*/  C.3 Miscellaneous Processes #ifdef F P F D I S C R E T E R A M P /* Fast DSP process to generate a bounded discrete ramp signal with /* variable step size(slope)  */ */  /*  */  /* by David Langlois, November 2000 /* Industrial Automation Laboratory /* University of British Columbia /* /* Generate a discrete ramp with limits and step size defined by /* the parameters. /* White noise magnitude defined by the 4th parameter /* /* /* DISCRETE_RAMP(start_value, step_size,stop_value,mag_noise), output=(OutputLink); ^*************************************  */ */ */ */ */ */ */ */ */ */  typedef struct  /* process data structure */  {  float startvalue; float stepsize; float stopvalue; float noise_gain;  /* parameters */  int count; long rand_par;  /* Variables*/  Link *outputlink0; } SDiscrete_ramp;  /* links */ /* type name */  C.3 Miscellaneous Processes  239  #define start_value #define step_size #define stop_value #define noisegain #define rand_par #define count #define outputlinkO  (p->start_value) (p->step_size) (p->stop_value) (p->noise_gain) (p->rand_par) (p->count) (p->outputlinkO)  rtncode FPFDiscreteramplnit  (int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParamcters, float *Parameter, float Freq, void **Ptr)  SDiscrete_ramp *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SDiscrete_ramp)))==NULL)  return RDSPOutofmemory;  /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumInputLinks!=0) return RRCPNoinputlinkexpected; if(NumOutputLinks!= 1) return RRCPInvalidnumberof outputlinks; if(NumParameters!=4) return R_RCP_Invalid_numberof_parameters; if(OutputLink[0]->ItemSize!=l)  return R_RCP_lnvalid_output_link_size;  /* Assign Parameters*/ startvalue = Parameter[0]; stepsize = Parameter[l]; stopvalue = Parameter[2]; noisegain = Parameter[3]; /* Initialize Variables*/ count=0; rand_par = -1; /* assign link pointers */ outputlinkO=OutputLink[0]; /* exit initialization function to start continuous function */ return RSuccess; #undef #undef #undef #undef #undef #undef #undef  startvalue stepsize stopvalue noisegain count rand_par outputlinkO  #deftne start_value #defme step_size #define stop_value  (((SDiscrete_ramp*)Ptr)->start_value) (((SDiscrete_ramp*)Ptr)->step_size) (((SDiscrete_ramp*)Ptr)->stop_value)  C.3 Miscellaneous Processes  240  #define noise_gain #define count #define rand_par #define outputlinkO  (((SDiscrete_ramp*)Ptr)->noise_gain) (((SDiscrete_ramp*)Ptr)->count) (((SDiscrete_ramp*)Ptr)->rand_par) (((SDiscrete_ramp*)Ptr)->outputl i nkO)  void F P F D I S C R E T E R A M P { float rampout; float noise;  (void *Ptr)  /* Generation of the Ramp Signal */ rampout = count*step_size + startvalue; count++; if (fabs(ramp_out - startvalue) >= fabs(stop_value - startvalue)) { /* Saturate the Output to the Stop value*/ rampout = stopvalue; }  /* Adding the white noise term*/ ran2(&rand_par,&noise); rampout +=((noise-0.5)*noise_gain); /* write to output link */ /*if(LinkReadyToWrite(outputlinkO))*/  LinkWriteFloat(outputlinkO,ramp_out);  1 #undef #undef #undef #undef #undef #undef #undef  startvalue stepsize stopvalue noisegain count rand_par outputlinkO  #endif/* FPF DISCRETE RAMP*/  /* Fast DSP process to evaluate a linear equation type of gain /* y=m*x+b /*  /* by David Langlois, January 2001 /* Industrial Automation Laboratory /* University of British Columbia /* /* Evaluate an expression of the type y=m*x+b where m and b are passed as /* parameters /* /*  /* LlNEAR_GAlN(m,b),input=(x), output=(y); typedef struct { float m; float b;  /* process data structure */ /* parameters */  */ */ */ */ */ */ */ */ */ */.  */ */  C.3 Miscellaneous Processes  Link ""inputlinkO; Link *outputlinkO; } SLinear_gain; #define m #define b #define inputlinkO #define outputlinkO  /*links*/ /* links */ /* type name */  (p->m) (p->b) (p->inputlinkO) (p->outputlinkO)  rtncode FPF Linear Gainlnit  (int NumlnputLinks, Link **InputLink, int NumOutputLinks, Link **OutputLink, int NumParameters, float *Parameter, float Freq, void **Ptr)  SLineargain *p;  /* create pointer to data structure */  /* allocate memory for local variables */ if((p=Malloc(sizeof(SLinear_gain)))=NULL)  return R D S P  Outofjnemory;  /* assign process data structure address to data passing pointer */ *Ptr=p; /* process call error handlers */ if(NumlnputLinks!==l) return RRCPInvalidnumber ofinputlinks; if(NumOutputLinks!=l) return RRCPInvalidnumber ofoutputlinks; if(NumParameters!=2) return RRCPlnvalidnumberof parameters; if(OutputLink[0]->ltemSize!=l)  return RRCPJnvalidoutputJinksize;  /* Assign Parameters*/ m = Parameter[0]; b = Parameter! 1]; /* assign link pointers */ inputlinkO=InputLink[0]; outputlink0=OutputLink[0]; /* exit initialization function to start continuous function */ return R_Success; }  #undef #undef #undef #undef  m b inputlinkO outputlinkO  #define m #define b #define inputlinkO #define outputlinkO void F P F L I N E A R G AIN {  float y;  (((S Linearga in*)Ptr)->m) (((SLinear_gain*)Ptr)->b) (((SLinear_gain*)Ptr)->inputlinkO) (((SLinear_gain*)Ptr)->outputlinkO) (void *Ptr)  C.3 Miscellaneous Processes float x; /* Read x value on input link*/ x=LinkReadFloat(inputlinkO); /* Evaluation of the linear equation*/ y=m*x+b; /* write to output link */ LinkWriteFloat(outputlinkO,y); }  #undef #undef #undef #undef  m b inputlinkO outputlinkO  #endif/* FPF LINEAR GAIN*/  242  Appendix D O R T S Script Files D.l Preamble Script files are used by the ORTS-PC/NT interface as a convenient way to define and assemble the real-time code to be downloaded on the DSP platfonn. Mainly, the script file is used to call objects (i.e., ORTS processes) and define the relationships between the different processes. Communication between the different platforms is also established in the script file. More details on the script files conventions and the necessary background to write them can be found in [57]. This appendix presents the script files that have been used to implement the low levels of the E L D architecture, as well as to generate the experimental results presented in this thesis and in [21].  D.2 ELD Implementation Script File This script files implements ORTS processes provided in Appendix C and Appendix D. This script file represents the most complete E L D implementation and has been used to generate the experimental data presented in [21].  243  D.2 E L D Implementation Script File  /* /* /* /* /* /* /* /* /*  Script file used to generate Jason Elliott experimental data Implement the most complete ELD version build up yet by David Langlois Industrial Automation Laboratory University of British Columbia September 2001  244  */ */ */ */ */ '*/  */ */.  */  DSP F3: Link public DSPtoPC(dsp2pc, buffered, 10,10); Link public PCtoDSP(pc2dsp,buffered,10,10); Link x_command(local,buffered, 1,10); Link y_command(local,buffered, 1,10); Link y_control(local,buffered, 1,1); Link out(dsp2pc, buffered, 80,5); Link out I (dsp2pc, buffered, 80,3); Group ELDCommand, priority=0, freq=400: { ELD_RECIPIENT_INTERPRETER(),input=(PCtoDSP),output=(x_command,y_command,DSPtoPC); }  Group XControl, priority=0, freq=400: {  Link X_mm_value( 1); Link x_set_point( 1); Link x_control( 1); Link trig(3); Link cam(2); Link Xerror(l); Link X_control_signal( 1); Link Xb_control_signal( 1); Link camx(l); Link camy(l); Link camxfast(l); /* X axis Encoder Measurement*/ MFJO_ReadEncoder( 1 ),output=(X_mm_value); /*X axis Feedback Gain to convert absolute position in counts to mm*/ /* -0.003175 mm/count, negative sign to make the counter sign to follow the axis direction*/ /* Gain added in the MFIO-3B Initialize function, in startup.spt*/ /* Read command signal, in mm, from somewhere*/ ELD_COMMAND_INTERPRETER(),input=(x_command,Xjnm_value),output=(x_set_point,DSPtoPC,tr ig,cam); ELD_CART_TRAJ2D(  10,10,150,100,150,10,10,100),input=(x_set_point),output=(x_control,y_control);  D.2 E L D Implementation Script File  Sum( 1,-1 ),input=(x_control,X_mrn_value),output=(X_error); /* Controler Algorithm*/ PIDControl(0.4,0.3,0.001 40),input=(X_error),output=(X_control_signal); >  /* Saturation of the control signal*/ Bound(-4.8,4.8),input=(X_control_signal),output=(Xb_controlsignal); /* DC offset to match the amplifier design*/ /* is added directly on the MFIO-3B*/ /* Output Controlsignal to amplifier*/ MFIOWriteDAC(l), input=(Xb_control_signal); /* For debugging purposes*/ //log( 1 ),input=(cam,X_mm_value,x_control),output=(out); log(l),input=(x control,X_mm_value,cam_x,var_x),output=(out); }  Group YControl, priority=0, freq=400: { Link feedbackraw(l); Link feedback_rawmm(l); Linkfeedbackfilt(l); Link feedbackmm(l); Link ycontrol 1(1); Link count_direction(2); Link train_direction(2); Link step(l); Link direction(l); /*Read feedback value from Analog port of MFIO-3B*/ MFIO_ReadADC( 1 ),output=(feedback_raw); /* Filter feedback signal*/ ChangeThreshold(0.0156),input=(feedback_raw),output=(feedback_filt); /* Feedback Gain and fix boundaries to protect Set-up*/ LINEAR_GAIN(-39.07675,101.6686),inputKfeedback_filt),output=(fcedback_mm); LINEAR_GAIN(-39.07675,101.6686),input=(feedbackjaw),output=(feedback_rawrrun); /* Convert Absolute Displacement in Relative Counts and Directions*/ /* Includes Controller Code*/ gain( 1 ),input=(y_control),output=(y_control 1); CLOSE_LOOP_STEPPER(),input=(y_control 1 ,feedback_mm), output=(count_direction); /* Generate Pulse Train based on Number found Above*/ PULSE_GEN(400,50), input=(count_direction),output=(train_direction); /* Demux train_direction signal*/ Map( 1,2),input=(train_direction),output=(step,direction); /* 10 changed to F3 digital 10*/ F3_WRITE_DIG(0,0),input=(direction); F3_WRITE_DIG( 1,1 ),input=(step);  245;  D.3 Axis Dynamics Identification Script  246  /* for debugging purposes*/ log( 1 ),input=(feedback_rawmm,y_control 1 ),output=(out 1); } PC: PriorityClass=High; SavetoDisk("..Wlog_files\jason_data.log"), input=(out); SavetoDisk("..\\log_files\jasonl_data.log"), input=(outl);  D.3 Axis Dynamics Identification Script This script file generates the excitation waveform to perform drive dynamics identification and performs the corresponding data acquisition. This file is a slightly modified version of the version developed by Kaan Erkorkmaz of the Manufacturing Automation Laboratory for the same purpose.  /***********************************^  /*  Axis Identification Script  */  by David Langlois Industrial Automation Laboratory University of British Columbia June 2001  */ */ */ */  /*  /* /* /* /*  */  /*  /* /*  */  Adapted from the version designed by Kaan Erkorkmaz  DSPF3: Link out(dsp2pc, buffered, 150,3); Link signal(local,buffered, 1,1); IdentO 1 (400,0,1,0.5,0.1 ,-0.5,0.1,1,0.1,-1,0.1,1.5,0.1,-1.5,0.1,2,0.1 ,-2,0.1,2.5,0.1 ,-2.5,0.1,2.5,0.1, -2.5,0.1,2,0.1,-2,0.1,1.5,0.1,-1.5,0.1,1,0.1,-1,0.1,0.5,0.1,0.5,0.1,0,1 ),priority=0,freq=400,output=(signal); Group Output, priority=0,freq=400: { Link feedbackmm(l); /* Get Encoder Measurement*/ MFIO_ReadEncoder( 1 ),output=(feedback_mm); /* Output Controlsignal to amplifier*/ MFIOWriteDAC(l), input=(signal); /* Logging the data for postprocessing*/ Log(l),input=(signal,feedback_mm),output=(out);  */ */  D.4 Low-level Heteroceptive Feedback Integration Script  247  PC: PriorityClass=High; SavetoDisk("..Wlog_files\axis_ident_400_9.log"), input=(out),priority=TimeCriticaI;  D.4 Low-level Heteroceptive Feedback Integration Script This script file implements the low-level heteroceptive feedback integration polynomial policy in the experimental manipulator servo loop. This aims at providing fault tolerance capabilities to a system where heteroceptive sensor capabilities are present and used to provide redundant endeffector information. Using this script, the fault is generated directly in sensing hardware through a switching box designed for that particular purpose, see Appendix A .  /* /* Script file used to test low-level fusion scheme for fault recovery /* Use only the x axis /* /* by David Langlois /* Industrial Automation Laboratory /* University of British Columbia /* October 2001 /* /**************************************^  */ */ */• */ */• */ */ */ */•  DSP F3: Link public DSPtoPC(dsp2pc, buffered, 10,10); Link public PCtoDSP(pc2dsp,buffered,10,10); Link x_command(local,buffered, 1,10); Link y_command(local,buffered, 1,10); Link y_control(local,buffered, 1,1); Link out(dsp2pc, buffered, 60,8); Group ELDCommand, priority=0, freq=400: { ELD_RECIPIENT_INTEPvPPvETER(),input=(PCtoDSP),output=(x_command,y_command,DSPtoPC); } Group XControl, priority=0, freq=400: { Link X_mm value( 1);  D.4 Low-level Heteroceptive Feedback Integration Script  248  Link xsetpoint(l); Link xcontrol(l); Link trig(3); Link cam(2); Link value(2); Linkcstl(2); Link cst2(2); Link varcam(l); Link varenc(l); Link X error( 1); Link X_control_signal( 1); Link Xb_control_signal( 1); Link cam_x(l); Link camy(l); Link cam_x_fast( 1); Link valuemean(l); Link valuevar(l); /* X axis Encoder Measurement*/ MFIO_ReadEncoder( 1 ),output=(X_mm_value); /*X axis Feedback Gain to convert absolute position in counts to mm*/ /* -0.003175 mm/count, negative sign to make the counter sign to follow the axis direction*/ /* Gain added in the MFIO-3B Initialize function, in startup.spt*/ /* Read command signal, in mm, from somewhere*/ ELD_COMMAND_INTERPRETER(),input=(x_command,X_mm_valuc),output=(x_set_point,DSPtoPC ig,cam); ELD CARTTRAJ1 D(40,140),input=(x_set_point),output=(x_control); Map( 1,2),input=(cam),6utput=(cam_x,cam_y); /* Interpolator*/ NGJNTERPOLATOR2(5),input=(cam_x,trig),output=(cam_x_fast,DSPtoPC); /* Signal Variance Quantification*/ VAR_QUANTlFIER(0.125),input=(x_control,cam_x_fast),output=(var_cam); VAR_QUANTIFIER(0.003175),input=(x_control,X_mm_value),output=(var_enc); /* Assemble signal and their variance*/ Map( 1,2),input=(X_mm_value,var_enc),output=(cst 1); Map( 1,2),input=(cam_x_fast,var_cam),output=(cst2); /* Fuse feedback signal*/ DATA_FUS10N2(l),input=(cstl,cst2,trig),output=(value); Map( 1,2),input=(value),output=(value_mean,value_var); Sum(l,-l),input=(x_control,value_mean),output=(X_error); /* Controler Algorithm*/ PIDControl(0.035,0.01,0,40),input=(X_error),output=(X_control_signal); /* Saturation of the control signal*/ Bound(-4.8,4.8),input=(X_control_signal),output=(Xb_control_signal);  D.5 Axes Controller Loops Scripts  249  /* DC offset to match the amplifier design*/ /* is added directly on the MFIO-3B*/ /* Output Controlsignal to amplifier*/ MFIO_WritcDAC( 1), input=(Xb_control_signal); /* For debugging purposes*/ log(l),input=(x_control,cstl,cst2,value),output=(out); }  PC: PriorityClass=High; SavetoDisk("..\\log_files\fault_data.log"), input=(out);  D.5 Axes Controller Loops Scripts D. 5.1 X-Axis Controller  Script  This script file implements the X-axis position servo-loop. This file includes the sensor signal processing, the command signal generation, and the trajectory generation. The main purpose of this file is to manually jog the axis around the workspace, fine tune the controller gains; and experimentally evaluate different servo-loop configuration performance.  /**************************^ /* /* DC Motor Controller and Trajectory Generator I* I* by David Langlois /* Industrial Automation Laboratory /* University of British Columbia /* October 2000  */ */ *j */ */ */ */  /* .  */  /* Used to move the x-axis around the workspace andfinetune the controller gains /* y****************************************** DSP F3:  */ */  Link out(dsp2pc, buffered, 70,3); Link error(local,buffered, 1,1); Group Setpoint, priority=0,freq=400: { Link command( 1); Link feedback_mm( 1); /* Generate Set-Point in mm*/ //Constant( 10),output=(command);  D.5 Axes Controller Loops Scripts  250  //DlSCRETE_RAMP(0,-0.05,-80,0),output=(command); DISCRETE_RAMP(0,0.15,40,0),output=(command); /* Get Encoder Measurement*/ MFIO_ReadEncoder( 1 ),output=(feedback_mm); /•Feedback Gain to convert absolute position in counts to mm*/ /* -0.003175 mm/count, negative sign to make the counter sign to follow the axis direction*/ /* gain added directly in the MFIO-3B Initialize function, in startup.spt*/ /* Close Feedback loop by summing command and feedback*/ Sum(l,-l),input=(command, feedbackmm), output=(error); /* For debugging purposes*/ Log( 1 ),input=(command,feedback_mm),output=(out); }  Group Control, priority=0, freq=400: { Link control(l); Link controlsignal(l); Link b_control_signal(l); /* Controler Algorithm*/ PIDControl(0.4,0.3,0.001,40),input=(error),output=(control_signal); /* Saturation of the control signal*/ Bound(-4.8,4.8),input=(control_signal),output=(b controlsignal); /* DC offset to match the amplifier design*/ /* is added directly on the MFIO-3B*/ /* Output Controlsignal to amplifier*/ MFIO_WriteDAC( 1), input=(b_control_signal); /*For Debugging Purposes*/ Log( 1 ),input=(b_control_signal),output=(out); }  PC: PriorityClass=High; SavetoDisk("..Wlog_Files\encoder_data.log"), input=(out),priority=TimeCritical;  D.5.2  Y-Axis Controller Script  This script file corresponds to a simplified version of the position servo-loop designed for the Y axis. This script includes the sensing of the end-effector position, the processing of the feedback signal, the generation of the command signal based on the difference between the set-point and the feedback signal, and finally the trajectory generation itself. The pulse generation process implements a constant frequency excitation controller, which provides limited performance. This  D.5 Axes Controller Loops Scripts  251  process can easily be modified to use a variable excitation frequency and a standard controller algorithm (i.e., PID) to impose gains on the frequency command signal. /********************^ /*  /* /* /* /* /* /*  */  Stepper Motor Controller and Trajectory Generator by David Langlois Industrial Automation Laboratory University of British Columbia October 2000  */ */ */ */ */ */  Used to move the y-axis around the workspace and evaluate the performance of the design  */  /*  /*  */  /*  */  DSP F3: Link out(dsp2pc, buffered, 200,3); Group Setpoint, priority=0, freq=400: { Link command(l); Link feedbackraw(l); Linkfeedbackfilt(l); Link feedbackmm(l); Link count_direction(2); Link train_direction(2); Link step(l); Link direction(l); //Link configO); /* Read command signal, in mm, from somewhere*/ //Constant( 10),output=(command); //DISCRETE_RAMP(140,-0.125,0,0),output=(command); //DISCRETE_RAMP(0,0.125,140,0),output=(command); CARTTRAJ1 D(0,90),input=(),output=(command); /*Read feedback value from Analog port of MFIO-3B*/ MFIO_ReadADC( 1 ),output=(feedback_raw); /* Filter feedback signal*/ ChangeThreshold(0.0156),input=(feedback_raw),output=(feedback_filt); /* Feedback Gain and fix boundaries to protect Set-up*/ LlNEAR_GAlN(-39.07675,101.6686),input=(feedback filt),output=(feedback_mm); /* Convert Absolute Displacement in Relative Counts and Directions*/ /* Includes Controller Code*/ CLOSE_LOOP_STEPPER(),input=(command,feedback_mm), output=(count_direction); /* Generate Pulse Train based on Number found Above*/ PULSE_GEN(400,100), input=(count_direction),output=(train_direction); /* Write direction to the analog port channel #3*/  D.5 Axes Controller Loops Scripts /* Before writing the pulses train*/ /* Scaling necessary because using analog port to generate TTL value*/ /* Scaling done on MF10-3B*/ /* Write the pulse train to the analog port channel #2*/ /* Scaling necessary because using analog port to generate TTL value*/ /* Scaling done on MFIO-3B*/ //MFIO_WriteDAC(2,3),input=(train_direction); /* Demux traindirection signal*/ Map( 1,2),input=(train_direction),output=(step,direction); /* Config Stepper Driver through I/O port*/ //Constant(4),output=(config); //F3_WRITE_DIG(2,4),input=(config); /* 10 changed to F3 digital 10*/ F3_WRITE_DIG(0,0),input=(direction); F3_WRITE_DIG(1,1 ),input=(step); /* for debugging purposes*/ log( 1 ),input=(command,feedback_mm),output=(out); }  PC: PriorityClass=High; SavetoDisk(".A\Log_Files\stepper_data.log"), input=(out), priority=TimeCritical;  252  Appendix E K a l m a n Filter Derivations for a F u l l State-Space M o d e l E . l Preamble This appendix presents the derivation of the recursive Kalman filter algorithm for a system presenting a full state-space model. This derivation is performed based on the one presented in [18] for the case of a system where no input coupling matrix is present in the measurement model (i.e., the D matrix is null). It is generally possible to convert a full state-space model to one with no input coupling matrix since there is no unique model to describe the dynamic behavior of a dynamic system. The use of software as MatLab [48] to perform operations on complex system models often leads to the generation of a full state-space model and it is judged simpler to extend the Kalman filter algorithm presented in [18] instead of continuously having to remodel complex systems.  In most of the cases, the use of a full state-space model in Kalman filter based prediction problems leads to causality problems, which, as discussed in an earlier chapter, is not the case when implementing the prediction algorithm in a system where the measurements are delayed. 253  E.2 Derivations  254  The derivation of the recursive Kalman filter algorithm for a full state-space system model are straightforward and rapidly converge to the solution presented in [18]. Therefore, only the part that differs from this last derivation is presented here.  E.2 Derivations Assuming a system model by the full state-space model given by the state model, k+\=^x +Au +rw ,  (E.l)  x  k  k  k  and the measurement model, k \ = k+\ + k \ + V+i *  z  Cx  Du  (E-2)  v  +  +  it is possible to write the best estimate provided by (E. 1) as: k+\\k =® k\k + "k-  x  x  (E-3)  A  From the best estimate given by (E.3), it is possible to generate the best one step ahead prediction substituting the state vector estimate in the measurement model, which leads to: **+H* =  i® k\k  c  x  + "k J+ Du .  (E.4)  A  k+[  Defining the prediction error, also known as Innovations Sequence, k+\ ~ k+\ ~ Zk+\\k •>  z  and the Estimation  (E.5).  z  Error, k+\  x  =  k+\  x  +  k+\\k+\'  (E.6)  x  it is possible to define the strategy adopted by the Kalman filter algorithm. It is desired to minimize the prediction error by adding a proportion of this last one to the each term of the estimated state vector, which is expressed by: k+\\k+\  x  =  k+\\k  x  +  KZk+\ •  (E.7)  Substituting into this last equation (E.3), (E.4) and (E.5) leads to: = ® k\k + "k + * k + i - (® k\k x  A  c  + "k )~DUM J•  x  A  (E- ) 8  Rearranging the terms in (E.8) leads to: [/-KC)[<t>x  k{k  +Au \+ k  Kz  k+i  -KDu . k+x  (E.9)  E.2 Derivations  255  Establishing the performance criteria of the recursive algorithm as the covariance of the estimation error, given by: P =E[x x \, k  k  (E.10)  k  it is therefore necessary to express the estimation error as a function of the system parameters. Substituting (E.9) and (E.l) in the definition of the estimation error (i.e., equation (E.6)), it is possible to generate such an expression, = ® k + A«* + Tw - [[/ - KC][<S>x + Au J+ Kz  - KDu \  x  k  k]k  k  k+l  k+  J.  (E. 11)  Substituting (E.2) into that last equation and rearranging the terms leads to: = 3»k - k \ k \ + F w + KC[<t>x -Ox x  k  k{k  k  -Tw J-Kv , k  k+X  (E. 12)';  where equation (E.6) written for the time-step k is used to simplify this last equation, which now becomes: x  =Ox  k+[  +rw -KC[®x  k  k  +Fw ]-Kv .  k  k  (E.13)  k+i  A last rearrangement of the terms of (E.l 3) leads to x  k+i  = [l-KC\m  (E.14)  +rw ]-Kv ,  k  k  k+i  which is identical to equation (A6.1) in [18]. This implies that developing the Kalman filter recursive algorithm for a system with, or without, a D matrix leads to the same expression for the estimation error. The rest of the development provided in [18] addresses the establishment of the Kalman gains that effectively minimize the performance criteria, (i.e., equation (E.10)). It is apparent  that the calculation of the Kalman gains in this case is identical to the formulation  provided in [18]. The only difference in the recursive algorithm formulation coming from the addition of the input coupling matrix is found in the state estimate update equation. This equation is modified from: **+i|t i +  =  t " k+\ l°**l* 7  K  C  + A u  (E- 5)  k \ + k \ k \, K  1  z  +  +  for a system with a null input coupling matrix to the corresponding relation provided by equation (E.8),to:  •W+i [ - ^ i l ° ^ =  c  7  +  k\+ k \ k \  +Au  K  z  +  for a system with a non-null input coupling matrix D.  +  ~ k \ k \> K  Du  +  +  (E.16);  Appendix F Discrete-Time PID Derivations F.l Preamble This appendix presents the derivations of the discrete-time PID controller used in the current work. This particular implementation of the classic control law is chosen due to its availability as an ORTS process, and the derivative term, low-pass filter included in the algorithm. The discretization scheme is following the one provided in [56]. Astrom [7] also presents a similar development, but uses a different discretization scheme. [8] introduces the notion of limiting the high-frequency of the derivative term to avoid noise amplification problems. Generally, the highfrequency gain is limited through the addition of a cascaded first-order system on the derivative term of the control law. The first-order low-pass filter on the derivative term acts as a normal derivative for low-frequency signal components, while limiting the gain applied to the highfrequency component of the signal. High-frequency noise is commonly found in experimental set-ups and a robust controller implementation must provide an efficient way to deal with such disturbances.  256  F.2 Derivations  257  Finally, the discrete-time controller is developed as a function of the continuous-time gains, enabling the use of gains found in continuous-time simulation directly in discrete-time applications, regardless of the sampling rate at which the control law is implemented.  F.2 Derivations The continuous-time domain formulation of the PID controller is expressed by the block presented in Figure F . l . This block illustrates the input/output relationship of a continuous-time PID controller.  Figure F . l . Continuous-Time Domain PID Controller Block There are many expressions for the continuous-time PID controller. Each expression is oriented toward a particular controller behavior. A simple expression for the continuous-time PID controller, known as the parallel implementation, is given by the Laplace domain equation, —r^r  Pc  e[s)  where K , Pc  K, Ic  and K  Dc  +  K  Oc •> S  (F.l)  s  are the continuous-time proportional gain, integrator gain, and  derivative gain, respectively, and s is the Laplace variable. This last equation is discretized following the scheme provided in [56]. The proportional term is directly converted to discretetime, while approximations are used to transform the derivative and integral terms. Ogata proposes to convert the integral term through the use of the Tustin approximation, 2  z-1  r/z+r  (F.2)  and the backward difference for the derivative term, _ z-1 S  ~  zT.  (F.3)  F.2 Derivations  258  It is also desired to limit the bandwidth of the signal subject to the derivative gain, which is done by reducing the bandwidth of the corresponding pole. Substituting (F.2) and (F.3) in the continuous-time PID controller formulation given by (F.I) and adding the necessary pole to generate an upper limit on the bandwidth of the signal used to calculate the contribution of the derivative term leads to  Y ^ =  K  e(z- ) ]  p  c  +  K  P c  i  lc  . ^ ^ . J ^ A _  c  2(z-l)  T  .  v  (F 4)  {z-dpo/e)  s  The derivative term bandwidth is limited by the position of the pole. Erkorkmaz [59] proposes the following relation to determine the location of the derivative term pole, dpo!e=  ^  j  .  (F.5)  +—!— (2*)./,,,  *  In this last equation, the low-pass cut-off frequency applied on the derivative term is given in Hertz. The bandwidth of the derivative term of the controller is limited by moving the pole, originally at the origin in the backward difference approximation, to a location closer to the unit circle boundary. Equation (F.5) is established based on an experimental parametric tuning of the pole position in order to achieve sufficient noise rejection.  Through the simplification of the integral term, it is possible to rewrite equation (F.4) as viz'') /  K T,  KT  lc  , \f =  K  P c  +  /C  +  Tp)  2  (z-1)  K  S  ,  Dc  \  +  —  (z-1)  —  T,  • 7-*  — c  .  (F.6  (z-dpole)  It can be seen from this last equation that the continuous-time integral gain directly contributes to the proportional gain. To simplify the tuning of the controller, it is preferable that the controller gain to be independent. The coupling between these gains is compensated by removing the contribution of the integral gain from the proportional gain. This is expressed by defining the discrete-time proportional gain as K =K r  r  <  - ^ - .  Substituting this last definition back in (F.6) leads to  (F.7.)  F.2 Derivations  259  e[z- )  (z-1)  x  T  s  (F.8)  ( z - dpole)  Defining the discrete-time integral gain as Kj=K T , lc  (F.9)  s  and the discrete-time derivative gain as (F.10)  *o=^, s  enables the simplification o f (F.8) to —h-rt-Kp+K,-. e[z  ~^ + K  ——.  D  U U  J  (F.ll)  z-dpole  -  Rewriting that last equation using a common denominator leads to the formulation o f the control law as a function o f the discrete-time gains and the position o f the derivative pole, v(z~ ) _(K +K, l  P  e[z~ ) x  +K )+ D  ( ( - dpole -\)K  P  \+  - K,dpole - 2K  )z~ + JK dpole x  D  P  +K  )z"  2  D  (-dpole-\)z' +(dpole)z{  2  This last equation provides the transfer function o f the control law to be used in this thesis.  Appendix G L o w Servo-Loop Sampling Rate Fusion G a i n Plots G.l Preamble This appendix presents the fusion gain plots corresponding to the implementation of the polynomial policy under a reduced servo-loop sampling rate, for different case of proprioceptive sensor fault occurrence. The corresponding response plots are presented in Chapter 8 and are not repeated here. Table G. 1 provides the experiment parameters for all four experiments performed using a reduced servo-loop sampling rate. The purpose of these experiments was to examine the effect of a shorter prediction horizon on the performance of the policy.  260  G.2 Fusion Gain Plots for Low Servo-Loop Sampling Rate  261  G.2 Fusion Gain Plots for Low Servo-Loop Sampling Rate Controller Gains  Servo Rate [Hz]  Camera Rate [Hz]  Fault Time [sec]  Kp  #6  100  5  -  0.035  0.025  #7  100  5  0  0.035  #8  100  5  - 1.5  #9  100  5  « 15  Case  Perf. Measure  Figure  0.0  6.04  G.l  0.025  0.0  6.47  G.2  0.035  0.025  0.0  6.46  G.3  0.035  0.025  0.0  9.02  G.4  K  D  Table G . l . Reduced Servo-Loop Sampling Rate Experiment Parameters  0  2  4  6  8  10  12  14  Time [sec]  Figure G. 1. Evolution of the Fusion Gains for Reduced Servo-Loop Sampling Rate, Case #6  G.2 Fusion Gain Plots for Low Servo-Loop Sampling Rate  262  Evolution of the Fusion Gains for Experiment Case #7  0.8  ™  o S  — Encoder Fusion Gain G ,.'.. Camera Fusion Gain G,  1  0.6  0.4  0.2  h  _i  i_  6  8  Time [sec]  10  12  Figure G.2. Evolution of the Fusion Gains for Fault Occurence Before Motion Starts, Case #7  Evolution of the Fusion Gains  o.ahl  .§  0.6h  Encoder Fusion Gain Camera Fusion Gain  c o  '55 0 . 4 r  0.2 h  0  2  4  6  8  10  12  14  Time [sec]  Figure G.3. Evolution of the Fusion Gains for Fault Occurence After Motion Starts, Case #8  G.2 Fusion Gain Plots for Low Servo-Loop Sampling Rate  263  Figure G.4. Evolution of the Fusion Gains for Fault Occurence within the Cyclic Motion Range, Case #9  

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                        
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            src="{[{embed.src}]}"
                            data-item="{[{embed.item}]}"
                            data-collection="{[{embed.collection}]}"
                            data-metadata="{[{embed.showMetadata}]}"
                            data-width="{[{embed.width}]}"
                            async >
                            </script>
                            </div>
                        
                    
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0099625/manifest

Comment

Related Items