# Sensor Fusion and Control Applied to Industrial Manipulators Patrik Axelsson

Linköping studies in science and technology. Dissertations. No. 1585 Sensor Fusion and Control Applied to Industrial Manipulators Patrik Axelsson Department of Electrical Engineering Linköping University, SE–581 83 Linköping, Sweden Linköping 2014 Cover illustration: Filled contour plot for the upper bound of the sample time given by (20) in Paper C. The upper bound is derived such that a stable continuoustime linear system remains stable after discretisation using Euler sampling. The sample time decreases when going from dark to light blue. Linköping studies in science and technology. Dissertations. No. 1585 Sensor Fusion and Control Applied to Industrial Manipulators Patrik Axelsson [email protected] www.control.isy.liu.se Division of Automatic Control Department of Electrical Engineering Linköping University SE–581 83 Linköping Sweden ISBN 978-91-7519-368-7 ISSN 0345-7524 Copyright © 2014 Patrik Axelsson Printed by LiU-Tryck, Linköping, Sweden 2014 Till min familj Abstract One of the main tasks for an industrial robot is to move the end-eﬀector in a predeﬁned path with a speciﬁed velocity and acceleration. Diﬀerent applications have diﬀerent requirements of the performance. For some applications it is essential that the tracking error is extremely small, whereas other applications require a time optimal tracking. Independent of the application, the controller is a crucial part of the robot system. The most common controller conﬁguration uses only measurements of the motor angular positions and velocities, instead of the position and velocity of the end-eﬀector. The development of new cost optimised robots has introduced unwanted ﬂexibilities in the joints and the links. The consequence is that it is no longer possible to get the desired performance and robustness by only measuring the motor angular positions. This thesis investigates if it is possible to estimate the end-eﬀector position using Bayesian estimation methods for state estimation, here represented by the extended Kalman ﬁlter and the particle ﬁlter. The arm-side information is provided by an accelerometer mounted at the end-eﬀector. The measurements consist of the motor angular positions and the acceleration of the end-eﬀector. In a simulation study on a realistic ﬂexible industrial robot, the angular position performance is shown to be close to the fundamental Cramér-Rao lower bound. The methods are also veriﬁed in experiments on an abb irb4600 robot, where the dynamic performance of the position for the end-eﬀector is signiﬁcantly improved. There is no signiﬁcant diﬀerence in performance between the diﬀerent methods. Instead, execution time, model complexities and implementation issues have to be considered when choosing the method. The estimation performance depends strongly on the tuning of the ﬁlters and the accuracy of the models that are used. Therefore, a method for estimating the process noise covariance matrix is proposed. Moreover, sampling methods are analysed and a low-complexity analytical solution for the continuous-time update in the Kalman ﬁlter, that does not involve oversampling, is proposed. The thesis also investigates two types of control problems. First, the norm-optimal iterative learning control (ilc) algorithm for linear systems is extended to an estimation-based norm-optimal ilc algorithm where the controlled variables are not directly available as measurements. The algorithm can also be applied to non-linear systems. The objective function in the optimisation problem is modiﬁed to incorporate not only the mean value of the estimated variable, but also information about the uncertainty of the estimate. Second, H∞ controllers are designed and analysed on a linear four-mass ﬂexible joint model. It is shown that the control performance can be increased, without adding new measurements, compared to previous controllers. Measuring the end-eﬀector acceleration increases the control performance even more. A non-linear model has to be used to describe the behaviour of a real ﬂexible joint. An H∞ -synthesis method for control of a ﬂexible joint, with non-linear spring characteristic, is therefore proposed. v Populärvetenskaplig sammanfattning En av de viktigaste uppgifterna för en industrirobot är att förﬂytta verktyget i en fördeﬁnierad bana med en speciﬁcerad hastighet och acceleration. Exempel på användningsområden för en industrirobot är bland annat bågsvetsning eller limning. För dessa typer av applikationer är det viktigt att banföljningsfelet är extremt litet, men även hastighetsproﬁlen måste följas så att det till exempel inte appliceras för mycket eller för lite lim. Andra användningsområden kan vara punktsvetsning av bilkarosser och paketering av olika varor. För dess applikationer är banföljningen inte det viktiga, istället kan till exempel en tidsoptimal banföljning krävas eller att svängningarna vid en inbromsning minimeras. Oberoende av applikationen är regulatorn en avgörande del av robotsystemet. Den vanligaste regulatorkonﬁgurationen använder bara mätningar av motorernas vinkelpositioner och -hastigheter, istället för positionen och hastigheten för verktyget, som är det man egentligen vill styra. En del av utvecklingsarbetet för nya generationers robotar är att reducera kostnaden men samtidigt förbättra prestandan. Ett sätt att minska kostnaden kan till exempel vara att minska dimensionerna på länkarna eller köpa in billigare växellådor. Den här utvecklingen av kostnadsoptimerade robotar har infört oönskade vekheter i leder och länkar. Det är därför inte längre möjligt att få den önskade prestandan och robustheten genom att bara mäta motorernas vinkelpositioner och -hastigheter. Istället krävs det omfattande matematiska modeller som beskriver dessa oönskade vekheter. Dessa modeller kräver mycket arbete att dels ta fram men även för att identiﬁera parametrarna. Det ﬁnns automatiska metoder för att beräkna modellparametrarna men oftast krävs det en manuell justering för att få bra prestanda. Den här avhandlingen undersöker möjligheterna att beräkna verktygspositionen med hjälp av bayesianska metoder för tillståndsskattning. De bayesianska skattningsmetoderna beräknar tillstånden för ett system iterativt. Med hjälp av en matematisk modell över systemet predikteras vad tillståndet ska vara vid nästa tidpunkt. Efter att mätningar av systemet vid den nya tidpunkten har genomförts justeras skattningen med hjälp av dessa mätningar. De metoder som har använts i avhandlingen är det så kallade extended Kalman ﬁltret samt partikelﬁltret. Informationen på armsidan av växellådan ges av en accelerometer som är monterad på verktyget. Med hjälp av accelerationen för verktyget och motorernas vinkelpositioner kan en skattning av verktygspositionen beräknas. I en simuleringsstudie för en realistisk vek robot har det visats att skattningsprestandan ligger nära den teoretiska undre gränsen, känd som Cramér-Raos undre gräns, samt att införandet av en accelerometer förbättrar prestandan avsevärt. Metoderna har även utvärderats på experimentella mätningar för en abb irb4600 robot. Ett av resultaten i avhandlingen visar att prestandan mellan olika metoder inte skiljer sig markant. Istället måste exekveringstid, modellkomplexitet och implementeringskostnader tas med i valet av metod. Vidare beror skattningsprestandan till stor del på hur ﬁltren har trimmats. Trimningsparametrarna är kopplade till processvii viii Populärvetenskaplig sammanfattning och mätstörningar som påverkar roboten. För att underlätta trimningen så har en metod för att skatta processbrusets kovariansmatris föreslagits. En annan viktig del som påverkar prestandan är modellerna som används i ﬁltren. Modellerna för en industrirobot är vanligtvis framtagna i kontinuerlig tid medan ﬁltren använder modeller i diskret tid. För att minska felen som uppkommer då de tidskontinuerliga modellerna överförs till diskret tid har olika samplingsmetoder studerats. Vanligtvis används enkla metoder för att diskretisera vilket innebär problem med prestanda och stabilitet. För att hantera dessa problem införs översampling vilket innebär att tidsuppdateringen sker med en mycket kortare sampeltid än vad mätuppdateringen gör. För att undvika översampling kan det motsvarande tidskontinuerliga ﬁltret användas för att prediktera tillstånden vid nästa diskreta tidpunkt. En analytisk lösning med låg beräkningskomplexitet till detta problem har föreslagits. Vidare innehåller avhandlingen två typer av reglerproblem relaterade till industrirobotar. För det första har den så kallade norm-optimala iterative learning control styrlagen utökats till att hantera fallet då en skattning av den önskade reglerstorheten används istället för en mätning. Med hjälp av skattningen av systemets tillståndsvektor kan metoden nu även användas till olinjära system vilket inte är fallet med standardformuleringen. Den föreslagna metoden utökar målfunktionen i optimeringsproblemet till att innehålla inte bara väntevärdet av den skattade reglerstorheten utan även skattningsfelets kovariansmatris. Det innebär att om skattningsfelet är stort vid en viss tidpunkt ska den skattade reglerstorheten vid den tidpunkten inte påverka resultatet mycket eftersom det ﬁnns en stor osäkerhet i var den sanna reglerstorheten beﬁnner sig. För det andra har design och analys av H∞ -regulatorer för en linjär modell av en vek robotled, som beskrivs med fyra massor, genomförts. Det visar sig att reglerprestandan kan förbättras, utan att lägga till ﬂer mätningar än motorns vinkelposition, jämfört med tidigare utvärderade regulatorer. Genom att mäta verktygets acceleration kan prestandan förbättras ännu mer. Modellen över leden är i själva verket olinjär. För att hantera detta har en H∞ -syntesmetod föreslagits som kan hantera olinjäriteten i modellen. Acknowledgments First I would like to thank my supervisors Professor Mikael Norrlöf and Professor Fredrik Gustafsson for their help and guidance with my research. I am also very thankful to Professor Lennart Ljung, our former head, for letting me join the Automatic Control group at Linköping University, to our current head, Professor Svante Gunnarsson, and our administrator Ninna Stengård for making the administrative work easier. This work has been supported by the Vinnova Industry Excellence Center LINKSIC and in particularly the partner abb Robotics. The thesis would not be possible without their ﬁnancial support, thanks a lot. Many thanks to Dr. Stig Moberg at abb Robotics for providing me with the robot models, both the mathematical equations and the models implemented in Matlab and Simulink. My gratitudes also goes to my co-authors, Docent Rickard Karlsson for helping me with anything concerning probability, and to Dr. Anders Helmersson for helping me endure the tough world of tuning H∞ controllers. The thesis layout had not been this good if not the previous LATEX gurus Dr. Henrik Tidefelt and Dr. Gustaf Hendeby (who will soon rule the world again as the LATEX guru) had spent a lot of their time creating the thesis template, many thanks to you. Also many thanks to Dr. Daniel Petersson and Dr. Christian Lyzell for all the help when Tik Z doesn’t produce the ﬁgures I want it to produce. Daniel also deserves many thanks for being my own computer support and answering all my easy/hard questions about matrix algebra. The content of the thesis had been much harder to read without all the good comments from Lic. André Carvalho Bittencourt, Lic. Daniel Simon, Lic. Ylva Jung, M.Sc. Clas Veibäck and my supervisors Professor Mikael Norrlöf and Professor Fredrik Gustafsson. Thanks a lot to all of you. The time from I started at the Automatic Control group until now has been full of nice co-workers and activities, thank you all for all the fun we have had. Everything from just taking a beer at the Friday-pub or at some nice place down town, to having a winter barbecue, or sharing an “Il Kraken”. Thanks also to all my friends outside the RT-corridor, I hope you know who you are. Finally, many thanks to my family for their support and to Louise for her patience and love. Louise, now it is ﬁnally time for me to decide what to do after this journey. I know where you want me, but also that everything can change if the price is right. Anyway, I hope that the future will be great for both of us. Linköping, March 2014 Patrik Axelsson ix Contents Notation I xvii Background 1 Introduction 1.1 Background and Motivation . . 1.2 Contributions . . . . . . . . . . 1.2.1 Included Publications . 1.2.2 Additional Publications 1.3 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 3 7 7 10 13 2 Industrial Robots 2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . 2.1.1 The Complete Robot System – An Overview 2.2 Modelling . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 Kinematic Models . . . . . . . . . . . . . . . 2.2.2 Dynamic Models . . . . . . . . . . . . . . . 2.3 Motion Control . . . . . . . . . . . . . . . . . . . . 2.3.1 Independent Joint Control . . . . . . . . . . 2.3.2 Feed-forward Control . . . . . . . . . . . . . 2.3.3 Feedback Linearisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 17 18 21 21 24 26 27 30 30 3 Estimation Theory 3.1 The Filtering Problem . . . . . . . . . . . . 3.1.1 The Extended Kalman Filter . . . . 3.1.2 The Particle Filter . . . . . . . . . . 3.2 The Smoothing Problem . . . . . . . . . . 3.3 The Expectation Maximisation Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 34 34 35 37 37 4 Control Theory 4.1 H∞ Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.1 Mixed-H∞ Control . . . . . . . . . . . . . . . . . . . . . . . 41 41 42 xi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xii Contents 4.2 Loop Shaping . . . . . . . . . . . . . . . . . 4.3 Iterative Learning Control . . . . . . . . . . 4.3.1 System Description . . . . . . . . . . 4.3.2 ILC Algorithms . . . . . . . . . . . . 4.3.3 Convergence and Stability Properties . . . . . 43 46 47 49 52 5 Concluding Remarks 5.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55 55 57 Bibliography 59 II . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Publications A Bayesian State Estimation of a Flexible Industrial Robot 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . 2 Bayesian Estimation . . . . . . . . . . . . . . . . . . . . . 2.1 The Extended Kalman Filter (EKF) . . . . . . . . 2.2 The Particle Filter (PF) . . . . . . . . . . . . . . . 2.3 Cramér-Rao Lower Bound . . . . . . . . . . . . . 3 Dynamic Models . . . . . . . . . . . . . . . . . . . . . . . 3.1 Robot Model . . . . . . . . . . . . . . . . . . . . . 3.2 Estimation Model . . . . . . . . . . . . . . . . . . 3.3 Sensor Model . . . . . . . . . . . . . . . . . . . . 4 Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Simulation Model . . . . . . . . . . . . . . . . . . 4.2 Cramér-Rao Lower Bound Analysis of the Robot 4.3 Estimation Performance . . . . . . . . . . . . . . 5 Experiments on an ABB IRB4600 Robot . . . . . . . . . 5.1 Experimental Setup . . . . . . . . . . . . . . . . . 5.2 Experimental Results . . . . . . . . . . . . . . . . 6 Conclusions and Future Work . . . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 75 77 78 79 80 80 80 81 82 84 84 84 85 86 86 88 93 95 B Evaluation of Six Diﬀerent Sensor Fusion Methods for an Industrial Robot using Experimental Data 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 State Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 Dynamic Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Robot Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Accelerometer Model . . . . . . . . . . . . . . . . . . . . . . 3.3 Modelling of Bias . . . . . . . . . . . . . . . . . . . . . . . . 4 Estimation Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Non-linear Estimation Model . . . . . . . . . . . . . . . . . 4.2 Estimation Model with Linear Dynamic . . . . . . . . . . . 4.3 Linear Estimation Model with Acceleration as Input . . . . 99 101 102 104 104 104 105 106 106 107 108 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii Contents 4.4 Non-linear Estimation Model with Acceleration as Input Experiments on an ABB IRB4600 Robot . . . . . . . . . . . . . . 5.1 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . 5.2 Experimental Results . . . . . . . . . . . . . . . . . . . . . 6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 109 109 110 115 117 C Discrete-time Solutions to the Continuous-time Diﬀerential Lyapunov Equation With Applications to Kalman Filtering 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Mathematical Framework and Background . . . . . . . . . . . . . . 2.1 Linear Stochastic Diﬀerential Equations . . . . . . . . . . . 2.2 Matrix Fraction Decomposition . . . . . . . . . . . . . . . . 2.3 Vectorisation Method . . . . . . . . . . . . . . . . . . . . . . 2.4 Discrete-time Recursion of the CDLE . . . . . . . . . . . . . 2.5 The Matrix Exponential . . . . . . . . . . . . . . . . . . . . 2.6 Solution using Approximate Discretisation . . . . . . . . . 2.7 Summary of Contributions . . . . . . . . . . . . . . . . . . . 3 Stability Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Stability for the Vectorisation Method using Euler Sampling 4 Reformulation of the Vectorised Solution for the CDLE . . . . . . . 5 Comparison of Solutions for the CDLE . . . . . . . . . . . . . . . . 5.1 Computational Complexity . . . . . . . . . . . . . . . . . . 5.2 Numerical Properties . . . . . . . . . . . . . . . . . . . . . . 6 Linear Spring-Damper Example . . . . . . . . . . . . . . . . . . . . 6.1 Stability Bound on the Sample Time . . . . . . . . . . . . . 6.2 Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . 7 Extensions to Non-linear Systems . . . . . . . . . . . . . . . . . . . 7.1 EKF Time Update . . . . . . . . . . . . . . . . . . . . . . . . 7.2 Simulations of a Flexible Joint . . . . . . . . . . . . . . . . . 8 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A Vectorisation of the CDLE . . . . . . . . . . . . . . . . . . . . . . . B Eigenvalues of the Approximated Exponential Function . . . . . . C Rules for Vectorisation and the Kronecker Product . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 121 123 123 124 124 125 125 126 127 128 129 131 134 134 136 136 137 138 140 140 142 144 146 147 148 149 D ML Estimation of Process Noise Variance in Dynamic Systems 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 The Expectation Maximisation Algorithm . . . . . . . . . . 3 EM for Covariance Estimation . . . . . . . . . . . . . . . . . . . . . 3.1 Expectation step . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Maximisation step . . . . . . . . . . . . . . . . . . . . . . . . 3.3 Stop Criterion . . . . . . . . . . . . . . . . . . . . . . . . . . 4 Alternative Ways to Find the Covariance Matrix of the Process Noise 5 Application to Industrial Robots . . . . . . . . . . . . . . . . . . . . 151 153 154 155 156 157 160 160 161 162 5 xiv Contents 6 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163 7 Conclusions and Future Work . . . . . . . . . . . . . . . . . . . . . 165 Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 166 E H∞ -Controller Design Methods Applied to One Industrial Manipulator 1 Introduction . . . . . . . . . . . . . . . . . . . 2 Controller Design Methods . . . . . . . . . . . 2.1 Mixed-H∞ Controller Design . . . . . 2.2 Loop Shaping using H∞ Synthesis . . 3 Flexible Joint Model . . . . . . . . . . . . . . . 4 Design of Controllers . . . . . . . . . . . . . . 4.1 Requirements . . . . . . . . . . . . . . 4.2 Choice of Weights . . . . . . . . . . . . 4.3 Controller Characteristics . . . . . . . 5 Simulation Results . . . . . . . . . . . . . . . . 6 Low Order Synthesis . . . . . . . . . . . . . . 7 Conclusions and Future Work . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . Joint of a Flexible . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169 171 173 173 174 175 176 176 177 180 180 184 185 187 F H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 3 Proposed H∞ -Synthesis Method . . . . . . . . . . . . . . . . . . . . 3.1 Uncertainty Description . . . . . . . . . . . . . . . . . . . . 3.2 Nominal Stability and Performance . . . . . . . . . . . . . . 3.3 Robust Stability . . . . . . . . . . . . . . . . . . . . . . . . . 3.4 Controller Synthesis . . . . . . . . . . . . . . . . . . . . . . 4 Non-linear Flexible Joint Model . . . . . . . . . . . . . . . . . . . . 5 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189 191 192 193 193 194 195 196 197 198 200 204 205 G Estimation-based Norm-optimal Iterative Learning Control 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Iterative Learning Control (ILC) . . . . . . . . . . . . . . . . . 3 Estimation-based ILC for Linear Systems . . . . . . . . . . . . 3.1 Estimation-based Norm-optimal ILC . . . . . . . . . . 3.2 Utilising the Complete PDF for the ILC Error . . . . . 3.3 Structure of the Systems used for ILC and Estimation 4 Estimation-based ILC for Non-linear Systems . . . . . . . . . 4.1 Solution using Linearised Model . . . . . . . . . . . . 4.2 Stability Analysis for the Linearised Solution . . . . . 5 Conclusions and Future Work . . . . . . . . . . . . . . . . . . A State Space Model and Kalman Filter in Batch Form . . . . . 207 209 210 211 211 213 215 216 216 217 218 218 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xv Contents Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222 H Controllability Aspects for Iterative Learning Control 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 2 State Space Model in the Iteration Domain . . . . . . . . . 3 Controllability . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 State Controllability . . . . . . . . . . . . . . . . . 3.2 Output Controllability . . . . . . . . . . . . . . . . 3.3 Stabilisability . . . . . . . . . . . . . . . . . . . . . 4 Interpretation of the Controllability Requirements . . . . 5 Target Path Controllability . . . . . . . . . . . . . . . . . . 6 Concept of Lead-in . . . . . . . . . . . . . . . . . . . . . . 7 Observability . . . . . . . . . . . . . . . . . . . . . . . . . . 8 Output Controllable System for Design of ILC Algorithms 9 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . A State Space Model in Batch Form . . . . . . . . . . . . . . Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225 227 229 229 230 231 232 232 234 236 236 237 239 240 242 Notation Estimation Notation Meaning xk State vector at time k Input vector at time k Process noise vector at time k Measurement vector at time k Measurement noise vector at time k Conditional density function for x given y Sequence of measurements from time 1 to time k Estimated state vector at time k given measurements up to and including time k Covariance of the estimated state vector at time k given measurements up to and including time k Smoothed state vector at time k given measurements up to time N ≥ k Covariance of the smoothed stated vector at time k given measurements up to time N ≥ k Particle i at time k wk ( N · ; μ, Σ) Q/R θ Weight for particle i at time k Gaussian distribution with mean μ and covariance Σ Covariance matrix for the process/measurement noise Unknown parameter vector xk uk wk yk vk p(x|y) y1:k x̂k|k Pk|k x̂sk|N Psk|N (i) (i) xvii xviii Notation Robotics Notation Meaning Qj/i , Rj/i rj/i Hj/i Rotation matrix for system j with respect to system i Vector from origin in frame i to origin in frame j Homogeneous transformation matrix for system j with respect to system i Homogeneous coordinate Position and orientation of the end-eﬀector Linear and angular velocity/acceleration of the endeﬀector Arm angular positions/velocities/accelerations Motor angular positions/velocities/accelerations Motor angular positions/velocities/accelerations expressed on the arm side of the gearbox Motor torque (expressed on the arm side of the gearbox) Forward kinematic model Jacobian matrix of the forward kinematic model Inertia matrix Coriolis- and centrifugal terms Gravitational torque Friction torque Stiﬀness torque Damping torque Gear ratio rhi Ξ Ξ̇/Ξ̈ qa /q̇a /q̈a qm /q̇m /q̈m qam /q̇am /q̈am τ m (τ am ) Υ( · ) J( · ) M( · ) C( · ) G( · ) F( · ) T(·) D( · ) η Control Notation P(s) K(s) Fl ( · , · ) KP , KD · ∞ Q, L Wu , WS , WP , MS , WT , W1 , W2 x (z, y, e, r, u) C (C o ) O P Meaning System from exogenous input signals and control signals to exogenous output signals and measurement signals Controller Linear fractional transformation Parameters for the pd controller Inﬁnity-norm ilc algorithm matrices Weighting functions for H∞ -control synthesis Weighting functions for loop shaping synthesis The vector x(t) (z(t), y(t), e(t), r(t), u(t)) stacked for t = 0, . . . , N (Output) Controllability matrix Observability matrix Lyapunov matrix xix Notation Miscellaneous Notation I 0 † T Ts , h tr rank E[·] Cov ( · ) g x, z (x̂, ẑ) O xi yi zi ρ̈ s b R/R+ /R++ Rn Rn×m n S++ (S+n ) ρ( · ) σ̄( · ) ⊗ vec (vech ) uk (t) ep,m ( · ) dβ(t) Meaning Identity matrix Null matrix Pseudo inverse Transpose Sample time Trace operator Rank of a matrix Expectation value Covariance Gravity constant (Estimated) Cartesian coordinates Cartesian coordinate frame named i Acceleration due to the motion in the accelerometer frame Bias vector Real/Non-negative/Positive numbers n-dimensional Euclidian space Space of real n × m matrices Set of symmetric positive deﬁnite (semi-deﬁnite) n × n matrices Spectral radius Maximal singular value Kronecker product (Half) Vectorisation Vector u at time t and ilc iteration k pth order Taylor expansion of the matrix exponential with scaling of the argument with a factor m Vector of Wiener processes xx Notation Abbreviations Abbreviation cdle crlb ddle dh dof em ekf eks ilc imm imu kf kkf kkt ml mc ode pf pdf rga rmse sde snr ssv tcp tpc zoh Meaning Continuous-time diﬀerential Lyapunov equation Cramér-Rao lower bound Discrete-time diﬀerence Lyapunov equation Denavit-Hartenberg Degrees of freedom Expectation maximisation Extended Kalman ﬁlter Extended Kalman smoother Iterative learning control interacting multiple model Inertial measurement unit Kalman ﬁlter Kinematic Kalman ﬁlter Karush-Kuhn-Tucker Maximum likelihood Monte Carlo Ordinary diﬀerential equation Particle ﬁlter Probability density function Relative gain array Root mean square error Stochastic diﬀerential equation Signal to noise ratio Structured singular value Tool centre point Target path controllability Zero order hold Part I Background 1 Introduction n this thesis, state estimation and control for industrial manipulators are studied. First, estimation of the unknown joint angles of the robot using motor angular position measurements together with acceleration measurements of the tool, is considered. Second, control of the manipulator using the estimated states, and the use of the acceleration measurement of the tool directly in the feedback loop, are investigated. I The background and motivation of the work are presented in Section 1.1. The contributions of the thesis are listed in Section 1.2 and the outline of the thesis is presented in Section 1.3. 1.1 Background and Motivation The ﬁrst industrial robots were big and heavy with rigid links and joints. The development of new robot models has been focused on increasing the performance along with cost reduction, safety improvement and introduction of new functionalities as described in Brogårdh [2007]. One way to reduce the cost is to lower the weight of the robot which leads to lower mechanical stiﬀness in the links. Also, the components of the robot are changed such that the cost is reduced, which can infer larger individual variations and unwanted non-linearities. The most crucial component, when it comes to ﬂexibilities, is the gearbox. The gearbox has changed more and more to a ﬂexible component described by non-linear relations, which cannot be neglected in the motion control loop. The friction in the gearbox is also an increasing problem that is described by non-linear relations. The available measurements for control are the motor angular positions, but since the end-eﬀector, which is the desired control object, is on the other 3 4 1 Introduction side of the gearbox it cannot be controlled in a satisfactory way. Instead, extensive use of mathematical models describing the non-linear ﬂexibilities are needed in order to control the weight optimised robot [Moberg, 2010]. In practice, the static volumetric accuracy is approximately 2–15 mm due to the gravity deﬂection which is caused by the ﬂexibilities. One solution to reduce the error is to create an extended kinematic model and an elasto-static model by conducting an oﬀ-line identiﬁcation procedure. The static error can, in this way, be reduced to 0.5 mm. For the dynamic accuracy a new model-based motion control scheme is presented in Björkman et al. [2008], where the maximum path error is one-ﬁfth of the maximum path error from a typical controller. However, reducing the material cost leads to more complex mathematical models that can explain the behaviour of the manipulator. Therefore, there is a demand for new approaches of motion control schemes, where new types of measurements are introduced and less complex models can be suﬃcient. One solution can be to estimate the position and orientation of the end-eﬀector along the path and then use the estimated position and orientation in the feedback loop of the motion controller. The simplest observer is to use the measured motor angular positions in the forward kinematic model to get the position and orientation of the end-eﬀector. In Figure 1.1a it is shown that the estimated position of the end-eﬀector does not track the true measured position very well. The reason for the poor result is that the oscillations on the arm side do not inﬂuence the motor side of the gearbox due to the ﬂexibilities. The ﬂexibilities can also distort the oscillations on the arm side, which means that the estimated path oscillates in a diﬀerent way than the true path. The kinematic model can consequently not track the true position and another observer is therefore needed. The observer requires a dynamic model of the robot in order to capture the oscillations on the arm side of the gearbox and possibly also more measurements than only the motor angular positions. Figure 1.1b shows one of the experimental results in this thesis, presented in Papers A and B, where a particle ﬁlter has been used. The true position of the end-eﬀector can, for evaluation purposes, be measured by an external laser tracking system from Leica Geosystems [2014], which tracks a crystal attached to the robot. The tracking system is very expensive and it requires line of sight and is therefore not an option to use for feedback control in practice. Note that measurements of the orientation of the end-eﬀector cannot be obtained using this type of tracking system. Diﬀerent types of observers for ﬂexible joint robots have been proposed in the literature. In Jankovic [1995] a high gain observer is proposed using only the motor angular positions and velocities as measurements. In Nicosia et al. [1988]; Tomei [1990], and Nicosia and Tomei [1992] diﬀerent observers are proposed where it is assumed that the arm angular positions and/or the arm angular velocities are measured. The drawback is that this is not the case for a commercial robot. The solution is obviously to install rotational encoders on the arm side of the gearbox and use them in the forward kinematic model. However, perfect measurements from the encoders on the arm side and the desired angles will diﬀer. Take the ﬁrst joint of the robot in Figure 2.2 as an example. The system from the motor 1.1 5 Background and Motivation 0.804 z [m] 0.802 0.8 0.798 0.796 1.15 1.2 1.25 1.3 1.35 x [m] (a) Estimated position using the forward kinematic model with the measured motor angular positions. 0.804 z [m] 0.802 0.8 0.798 0.796 1.15 1.2 1.25 1.3 1.35 x [m] (b) Estimated position using a particle ﬁlter. Figure 1.1: True path (grey) and estimated path (black) of the end-eﬀector using (a) the forward kinematic model with the measured motor angular positions, and (b) a particle ﬁlter using the motor angular positions and the acceleration of the end-eﬀector as measurements. The dynamical performance of the estimated path in (a) is insuﬃcient due to the ﬂexible gearboxes between the measured motor angular positions and the end-eﬀector. The estimated path from a particle ﬁlter in (b) is much better. 6 1 Introduction side of the gearbox to the end-eﬀector can be seen as a three-mass system, or more, and not a two-mass system. The motor encoder measures the position of the ﬁrst mass and the arm encoder measures the position of the second mass. The ﬂexibility between the second and third mass is due to ﬂexibilities in joints two and three. These ﬂexibilities are in the same direction as joint one and cannot be measured with encoders in joints two and three. Hence, there is still a need for estimating the end-eﬀector path. One way to obtain information about the oscillations on the arm side can be to attach an accelerometer to the robot, e.g. at the end-eﬀector, which is the approach used in this thesis. The accelerometer that has been used is a triaxial accelerometer from Crossbow Technology [Crossbow Technology, 2004]. A natural question is, how to estimate the arm angular positions from the measured acceleration as well as the measured motor angular positions. A common solution for this kind of problems is to apply sensor fusion methods for state estimation. The acceleration of the end-eﬀector as well as the measured motor angular positions can be used as measurements in e.g. an extended Kalman ﬁlter (ekf) or particle ﬁlter (pf). In Karlsson and Norrlöf [2004, 2005], and Rigatos [2009] the ekf and pf are evaluated on a ﬂexible joint model using simulated data only. The estimates from the ekf and pf are also compared with the theoretical Cramér-Rao lower bound in Karlsson and Norrlöf [2005] to see how good the ﬁlters are. An evaluation of the ekf using experimental data is presented in Henriksson et al. [2009], and in Jassemi-Zargani and Necsulescu [2002] with diﬀerent types of estimation models. A method using the measured acceleration of the end-eﬀector as input instead of using it as measurements is described in De Luca et al. [2007]. The observer in this case is a linear dynamic observer using pole placement, which has been evaluated on experimental data. Estimating the joint angles using a combination of measurements from accelerometers, gyroscopes and vision is presented in Jeon et al. [2009]. The so called kinematic Kalman ﬁlter (kkf), which basically is a Kalman ﬁlter applied to a kinematic model, is used for estimation and the results are veriﬁed on a planar two-link robot. In Chen and Tomizuka [2014] the estimates are obtained using a two-step procedure. First, rough estimates of the joint angles are obtained using the dynamical model and numerical diﬀerentiation. Second, the rough estimates are used to decouple the complete system into smaller systems where the kkf is applied to improve the estimates. The method is veriﬁed experimentally on a six degrees-of-freedom (dof) manipulator. In Lertpiriyasuwat et al. [2000], and Li and Chen [2001] the case with ﬂexible link models, where the acceleration or the position of the end-eﬀector are measured, is presented. From an on-line control perspective, it is important that the estimation method performs in real-time. The sample time for industrial manipulators is usually on the time scale of milliseconds, hence the real time requirements are very high, and e.g. the pf can have diﬃculties to achieve real-time performance. However, the estimated position of the end-eﬀector can still be used in an oﬀ-line application, such as iterative learning control (ilc), if the estimation method performs slower than real-time. In Wallén et al. [2008] it is shown that motor side learn- 1.2 Contributions 7 ing is insuﬃcient if the mechanical resonances are excited by the robot trajectory. Instead estimation-based ilc has to be considered, as presented in Wallén et al. [2009], to improve the performance even more. Other applications that can improve if the estimated position of the end-eﬀector is available, not necessarily online, are system identiﬁcation, supervision, diagnosis, and automatic controller tuning. Although the estimation method performs slower than real-time, it is interesting to investigate the direct use of an accelerometer, attached to the end-eﬀector, in the feedback loop together with more accurate models. Model-based controllers for ﬂexible joints, modelled as a two-mass model with linear ﬂexibility, have been considered for many years [Sage et al., 1999; De Luca and Book, 2008]. Section 2.3 presents an overview of common controllers for industrial manipulators. As previously stated, a two mass model with linear ﬂexibility does not represent the actual robot structure suﬃciently well, hence more complex models are needed for control. Moberg et al. [2009] presents a benchmark model for a single joint intended to be used for evaluation of new controllers. The joint is modelled as a four mass model, where the ﬁrst ﬂexibility is given by a non-linear function, and the only available measurement is the motor position. Four model based solutions are presented. An interesting thing from Moberg et al. [2009] is that one of the best controllers can be realised as a parallel pid controller. To improve the control performance signiﬁcantly, more measurements must be included. An encoder measuring the arm angular position will of course improve the performance. However, the position of all masses cannot be measured, hence all oscillations cannot be taken care of. Instead sensors attached to the end-eﬀector, such as an accelerometer, must be used to give more information. Feedback of the end-eﬀector acceleration has been considered in e.g. Kosuge et al. [1989] and Xu and Han [2000], where a rigid joint model has been used. In Kosuge et al. [1989] the non-linear robot model is linearised using feedback linearisation. For controller synthesis the H∞ methodology can be used. Song et al. [1992] and Stout and Sawan [1992] uses a rigid joint model which is ﬁrst linearised using feedback linearisation. Second, the linearised model is used for design of an H∞ controller. A similar approach is used in Sage et al. [1997] where the model is a linear ﬂexible joint model and the motor positions are measured. Non-linear H∞ methods applied to rigid joint manipulators are considered in Yim and Park [1999]; Taveira et al. [2006]; Miyasato [2008] and Miyasato [2009]. For ﬂexible joint models the non-linear H∞ approach is presented in Yeon and Park [2008] and Lee et al. [2007]. 1.2 Contributions The main contributions in this thesis are i) how to estimate the position of the end-eﬀector using an accelerometer, and ii) control strategies using either the estimated position or the accelerometer measurement directly. 8 1 1.2.1 Introduction Included Publications Paper A: Bayesian State Estimation of a Flexible Industrial Robot A sensor fusion method for state estimation of a ﬂexible industrial robot is presented in Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Bayesian state estimation of a ﬂexible industrial robot. Control Engineering Practice, 20(11):1220–1228, November 2012b. By measuring the acceleration at the end-eﬀector, the accuracy of the estimated arm angular position, as well as the estimated position of the end-eﬀector are improved using the extended Kalman ﬁlter and the particle ﬁlter. In a simulation study the inﬂuence of the accelerometer is investigated and the two ﬁlters are compared to each other. The angular position performance is increased when the accelerometer is used and the performance for the two ﬁlters is shown to be close to the fundamental Cramér-Rao lower bound. The technique is also veriﬁed in experiments on an abb irb4600 robot. The experimental results have also been published in Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Tool position estimation of a ﬂexible industrial robot using recursive Bayesian methods. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 5234–5239, St. Paul, MN, USA, May 2012a. Paper B: Evaluation of Six Different Sensor Fusion Methods for an Industrial Robot Using Experimental Data Experimental evaluations for path estimation on an abb irb4600 robot are presented in Patrik Axelsson. Evaluation of six diﬀerent sensor fusion methods for an industrial robot using experimental data. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 126– 132, Dubrovnik, Croatia, September 2012. Diﬀerent observers using Bayesian techniques with diﬀerent estimation models are investigated. It is shown that there is no signiﬁcant diﬀerence in performance between the best observers. Instead, execution time, model complexities and implementation issues have to be considered when choosing the method. Paper C: Discrete-time Solutions to the Continuous-time Differential Lyapunov Equation with Applications to Kalman Filtering Over-sampling strategies for ﬁltering of continuous-time stochastic processes are analysed in Patrik Axelsson and Fredrik Gustafsson. Discrete-time solutions to the continuous-time diﬀerential Lyapunov equation with applications to Kalman ﬁltering. Submitted to IEEE Transactions on Automatic Control, 2012, 1.2 Contributions 9 where a novel low-complexity analytical solution to the continuous-time diﬀerential Lyapunov equation (cdle), that does not involve oversampling, is proposed. The results are illustrated on Kalman ﬁltering problems in both linear and nonlinear systems. Another approach to the discretisation problem is presented in the related publication Niklas Wahlström, Patrik Axelsson, and Fredrik Gustafsson. Discretizing stochastic dynamical systems using Lyapunov equations. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014. A method to calculate the covariance matrix for the discretised noise is proposed, instead of solving the cdle. The covariance matrix is given as the solution to an algebraic Lyapunov equation. The same Lyapunov equation is a part of the solution to the cdle, which is presented in this thesis. Paper D: ML Estimation of Process Noise Variance in Dynamic Systems Parameter identiﬁcation using the expectation maximisation algorithm, which iteratively estimates the unobserved state sequence and the process noise covariance matrix based on the observations of the process, is presented in Patrik Axelsson, Umut Orguner, Fredrik Gustafsson, and Mikael Norrlöf. ml estimation of process noise variance in dynamic systems. In Proceedings of the 18th IFAC World Congress, pages 5609–5614, Milano, Italy, August/September 2011. The extended Kalman smoother is the instrument to ﬁnd the unobserved state sequence, and the proposed method is compared to two alternative methods on a simulated robot model. Paper E: H∞ -Controller Design Methods Applied to one Joint of a Flexible Industrial Manipulator Control of a ﬂexible joint of an industrial manipulator using H∞ -design methods is presented in Patrik Axelsson, Anders Helmersson, and Mikael Norrlöf. H∞ -controller design methods applied to one joint of a ﬂexible industrial manipulator. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014b. The considered design methods are i) mixed-H∞ design, and ii) H∞ loop shaping design. Two diﬀerent controller conﬁgurations are examined: one uses only the actuator position, while the other uses the actuator position and the acceleration of the end-eﬀector. The four resulting controllers are compared to a standard pid controller where only the actuator position is measured. Model order reduction of the controllers is also brieﬂy discussed. Paper F: H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models An H∞ -synthesis method for control of a ﬂexible joint, with non-linear spring characteristic, is proposed in 10 1 Introduction Patrik Axelsson, Goele Pipeleers, Anders Helmersson, and Mikael Norrlöf. H∞ -synthesis method for control of non-linear ﬂexible joint models. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014d. The method is motivated by the assumption that the joint operates in a speciﬁc stiﬀness region of the non-linear spring most of the time, hence, the performance requirements are only valid in that region. However, the controller must stabilise the system in all stiﬀness regions. The method is validated in simulations on a two mass non-linear ﬂexible joint model. Paper G: Estimation-based Norm-optimal Iterative Learning Control The norm-optimal iterative learning control (ilc) algorithm for linear systems is in Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Estimationbased norm-optimal iterative learning control. Submitted to Systems & Control Letters, 2014c extended to an estimation-based norm-optimal ilc algorithm, where the controlled variables are not directly available as measurements. For linear timeinvariant systems with a stationary Kalman ﬁlter it is shown that the ilc design is independent of the design of the Kalman ﬁlter. The algorithm is also extended to non-linear state space models using linearisation techniques. Finally, stability and convergence properties are derived. Paper H: Controllability Aspects for Iterative Learning Control The aspects of controllability in the iteration domain, for systems that are controlled using ilc, are discussed in Patrik Axelsson, Daniel Axehill, Torkel Glad, and Mikael Norrlöf. Controllability aspects for iterative learning control. Submitted to International Journal of Control, 2014a. A state space model in the iteration domain is proposed to support the discussion. It is shown that it is more suitable to investigate if a system can follow a trajectory instead of the ability to control the system to an arbitrary point in the state space. This is known as target path controllability. A simulation study is also performed to show how the ilc algorithm can be designed using the lq-method. It is shown that the control error can be reduced signiﬁcantly using the lq-method compared to the norm-optimal approach. 1.2.2 Additional Publications Related articles, which are not included in this thesis, are presented here with a short description of the contributions. Simulation results for the estimation problem are presented in the following two publications 1.2 Contributions 11 Patrik Axelsson, Mikael Norrlöf, Erik Wernholt, and Fredrik Gustafsson. Extended Kalman ﬁlter applied to industrial manipulators. In Proceedings of Reglermötet, Lund, Sweden, June 2010, Patrik Axelsson. A simulation study on the arm estimation of a joint ﬂexible 2 dof robot arm. Technical Report LiTH-ISY-R-2926, Department of Electrical Enginering, Linköping University, SE-581 83 Linköping, Sweden, December 2009, where performance in case of uncertain dynamical model parameters as well as uncertainties in the position and orientation of the accelerometer are studied. A detailed description of the simulation model that has been used is given in Patrik Axelsson. Simulation model of a 2 degrees of freedom industrial manipulator. Technical Report LiTH-ISY-R-3020, Department of Electrical Enginering, Linköping University, SE-581 83 Linköping, Sweden, June 2011a. Parts of the material in this thesis have previously been published in Patrik Axelsson. On Sensor Fusion Applied to Industrial Manipulators. Linköping Studies in Science and Technology. Licentiate Thesis No. 1511, Linköping University, SE-581 83 Linköping, Sweden, December 2011b. Method to Estimate the Position and Orientation of a Triaxial Accelerometer Mounted to an Industrial Robot A method, used in Papers A and B, to ﬁnd the orientation and position of a triaxial accelerometer mounted on a six dof industrial robot is proposed in Patrik Axelsson and Mikael Norrlöf. Method to estimate the position and orientation of a triaxial accelerometer mounted to an industrial manipulator. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 283–288, Dubrovnik, Croatia, September 2012. Assume that the accelerometer is mounted on the robot according to Figure 1.2a. The problem is to ﬁnd: (i) The internal sensor parameters and the orientation of the sensor. (ii) The position of the accelerometer with respect to the robot end-eﬀector coordinate system. The method consists of two consecutive steps, where the ﬁrst is to estimate the orientation of the accelerometer from static experiments. In the second step the accelerometer position relative to the robot base is identiﬁed using accelerometer readings. Identiﬁcation of the orientation can be seen as ﬁnding a transformation from the actual coordinate system Oxa ya za to a desired coordinate system Oxs ys zs , which can be seen in Figure 1.2b. The relation between O xa ya za and 12 1 zs ya × xa za zb yb × ys × xs zb yb × xb (a) The accelerometer and its actual coordinate system Oxa ya za . Introduction xb (b) The accelerometer and the desired coordinate system O xs ys zs . Figure 1.2: The accelerometer mounted on the robot. The yellow rectangle represents the tool or a weight and the black square on the yellow rectangle is the accelerometer. The base coordinate system Oxb yb zb of the robot is also shown. Oxs ys zs is given by, ρ s = κRa/s ρ a + ρ 0 , (1.1) where ρ a is a vector in O xa ya za , ρ s is a vector in O xs ys zs , Ra/s is the rotation matrix from Oxa ya za to O xs ys zs , κ is the accelerometer sensitivity and ρ 0 the bias. When the unknown parameters in (1.1) have been found the position of the accelerometer, expressed relative to the end-eﬀector coordinate system, can be identiﬁed. The position is identiﬁed using accelerometer readings when the accelerometer moves in a circular path and where the accelerometer orientation is kept constant in a path ﬁxed coordinate system. Estimation-based ILC using Particle Filter with Application to Industrial Manipulators In Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Estimationbased ILC using particle ﬁlter with application to industrial manipulators. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 1740–1745, Tokyo, Japan, November 2013, an estimation-based ilc algorithm is applied to a realistic industrial manipulator model. By measuring the acceleration of the end-eﬀector, the arm angular position accuracy is improved when the measurements are fused with the motor angular position observations using the extended Kalman ﬁlter, the unscented Kalman ﬁlter, and the particle ﬁlter. In an extensive Monte Carlo simulation study it is shown that the particle ﬁlter outperforms the other methods, see Fig- 1.2 13 Contributions 102 ρk [%] rmse(qa ) 10−1 10−2 10−3 101 100 10−1 0 0.2 0.4 0.6 0.8 1 1.2 Time [s] (a) Root mean square error for the ekf and ukf (dashed) and the pf (dash-dotted) compared to the Cramér-Rao lower bound (solid). 0 20 40 60 80 100 ilc iteration (b) Normalised control error when true arm position is used (solid), the ekf and ukf (dashed) and the pf (dash-dotted). Figure 1.3: Results for estimation-based ilc using Bayesian state estimation. ure 1.3a, and that the control error is substantially improved using the particle ﬁlter estimate in the ilc algorithm, see Figure 1.3b. Identiﬁcation of Wear in a Robot Joint The eﬀects of wear to friction based on constant-speed friction data are studied in the two papers André Carvalho Bittencourt and Patrik Axelsson. Modeling and experiment design for identiﬁcation of wear in a robot joint under load and temperature uncertainties based on friction data. IEEE/ASME Transactions on Mechatronics, 2013. DOI: 10.1109/TMECH.2013.2293001, André Carvalho Bittencourt, Patrik Axelsson, Ylva Jung, and Torgny Brogårdh. Modeling and identiﬁcation of wear in a robot joint under temperature uncertainties. In Proceedings of the 18th IFAC World Congress, pages 10293–10299, Milano, Italy, August/September 2011. It is shown how the eﬀects of temperature and load uncertainties, see Figure 1.4a, produce larger changes to friction than those caused by wear. Based on empirical observations, an extended friction model is proposed to describe the eﬀects of speed, load, temperature and wear. A maximum likelihood wear estimator is proposed, where it is assumed that an extended friction model and constant-speed friction data are available. The distribution of the load uncertainties can be estimated, hence the load can be marginalised away. The wear and temperature can then be jointly estimated. The proposed method is evaluated in both extensive simulations, see Figure 1.4b, and on experimental data. It is shown that reliable wear estimates can be achieved even under load and temperature uncertainties. Also, experiment design is considered in terms of an optimal selection of speed points, where the friction data should be collected. 14 1 0.2 T = 33 ◦ C, τl = 0.7 T = 33 ◦ C, τl = 0.01 τf 0.15 Introduction T = 80 ◦ C, τl = 0.7 T = 80 ◦ C, τl = 0.01 0.1 0.05 0 0 50 100 150 200 250 300 q̇ [rad/s] (a) Observed friction curves (markers) and model-based predictions (lines) for low and high values of the temperature T and load τl . N =2 40 Variance N =3 2 Bias 50 N =1 4 N =4 0 −2 30 20 −4 30 35 40 T [◦ C] 45 50 10 30 35 40 45 50 T [◦ C] (b) Monte Carlo based estimates of bias and variance for diﬀerent temperatures T , and N diﬀerent number of speed points. Figure 1.4: Friction model dependent of temperature and load used for identiﬁcation of wear in (a), and the results of the identiﬁcation method based on Monte Carlo simulations in (b). 1.3 1.3 Thesis Outline 15 Thesis Outline The thesis is divided into two parts. The ﬁrst part contains background and motivation of the thesis as well as a theoretical background. Chapter 1 presents the problem and lists all included publications. Chapter 2 contains background material for industrial manipulators, both modelling and control. The general non-linear estimation problem is presented in Chapter 3 and several algorithms for state and parameter estimation are presented. Chapter 4 introduces diﬀerent control strategies that have been used in the thesis. Finally, Chapter 5 concludes the work in the thesis and gives suggestions for future work. The second part of the thesis contains eight edited versions of selected papers. 2 Industrial Robots ndustrial manipulators are used in tasks where high precision and/or high speed are needed, such as spot welding, arc welding and laser cutting. Industrial manipulators are also important for tasks where the environment is harmful for humans, e.g. painting of car bodies. The robot needs therefore to be serviceable, have high precision, operate at high speeds and be robust to disturbances. Good models and controllers are necessary for all of these requirements. There are three types of robot structures for industrial robots. The most common is the serial arm robot in Figure 2.1a, whereas the other two robot structures have parallel arms, see Figure 2.1b and parallel links, see Figure 2.1c. In this thesis, the focus is on serial arm robots. I The chapter starts with an introduction to the concept of industrial robots in Section 2.1. Section 2.2 presents a short overview of the kinematic and dynamic models needed for control, and Section 2.3 discusses the control problem and what types of control structures that are commonly used. 2.1 Introduction In 1954, the American inventor George C. Devol applied for the ﬁrst patents for industrial robots, called the Programmed Article Transfer. Seven years later, in 1961, the patents were granted. Devol and Joseph Engelberger started the ﬁrst robot manufacturing company Unimation Inc. in 1956. The ﬁrst operating industrial robot Unimate was launched in 1959 and the ﬁrst robot installation was performed in 1961 at General Motors plant in Trenton, New Jersey. Europe had to wait until 1967 to get the ﬁrst robot installation, which was carried out in Sweden. In 1973, the Swedish company asea (current abb) launched the ﬁrst 17 18 2 (a) The serial arm robot abb irb4600. (b) The parallel arm robot abb irb360. Industrial Robots (c) The parallel linkage robot abb irb660. Figure 2.1: Three types of robots from abb [abb Robotics, 2014]. micro-processor controlled electrical robot called irb6. [Nof, 1999; Westerlund, 2000] Since then, abb has evolved to one of the largest manufactures of industrial robots and robot systems. Abb has over 200,000 robots installed world wide and the company was the ﬁrst with over 100,000 sold robots. Abb introduced the ﬁrst paint robot in 1969 and in 1998 the fastest pick and place robot FlexPicker, irb360 in Figure 2.1b, was launched [abb Robotics, 2014]. Other big manufactures are the German company kuka, fanuc Robotics with over 200,000 installed robots [fanuc Robotics, 2014], and Motoman owned by the Japanese company Yaskawa. kuka built the ﬁrst welding line with robots for DaimlerBenz in 1971 and launched the ﬁrst industrial robot with six electro mechanically driven axes in 1973 [kuka, 2014]. Motoman launched the ﬁrst robot controller where it was possible to control two robots in 1994, and a 13 axis dual arm robot in 2006. Today Motoman has over 270,000 installed robots world wide and produces 20,000 robots per year [Motoman, 2014]. 2.1.1 The Complete Robot System – An Overview A general robot system includes the manipulator, computers and control electronics. The desired motion of the robot is given in the user program. The program is composed by motion commands, such as a linear or circular trajectory between two points for the end-eﬀector1 in three dimensional space. Also the three dimensional orientation of the end-eﬀector can be aﬀected. In particular, the tool centre point (tcp), deﬁned somewhere on the end-eﬀector, is of interest. For example, in arc welding applications the tcp is deﬁned as the tip of the welding gun. The position and orientation, also known as the pose, are thus described in a six dimensional space. The robot needs therefore at least six degrees of freedom (dof) to be able to manoeuvre the end-eﬀector to a desired position and orientation. The pose is said to be deﬁned in the task space whereas the joint 1 The end-eﬀector is an equipment mounted at the end of the robot arm to interact with the environment. 2.1 19 Introduction Joint 4 Joint 6 Joint 5 Joint 3 Joint 2 Joint 1 Figure 2.2: The six dof serial arm robot abb irb6600 where the arrows indicate the joints [abb Robotics, 2014]. angles are said to be in the joint space. The total volume the robot end-eﬀector can reach is called the workspace. The workspace is divided in the reachable and the dexterous workspace. The reachable workspace includes all points the end-eﬀector can reach with some orientation. Whereas the dexterous workspace includes all points the end-eﬀector can reach with an arbitrary orientation. The dexterous workspace is of course a subset of the reachable workspace. The motion can also be deﬁned in the joint space, where each joint corresponds to one dof. That means that the robot moves between two sets of joint angles where the path of the end-eﬀector is implicit, meanwhile the velocity and acceleration are considered. A serial robot is said to have n dof if it has n joints. Figure 2.2 shows how the six joints for a six dof robot can be deﬁned. A desired velocity and acceleration of the end-eﬀector or the joints can also be speciﬁed in the user program. It is also possible to manoeuvre the robot using a joystick, either the position or orientation of the end-eﬀector are controlled or the joint angles. The control system, depicted in Figure 2.3, can be divided up into three main parts; path planning, trajectory generation, and motion control. The three parts will be discussed brieﬂy. A more thorough description of the motion controller will be addressed in Section 2.3. The path planner deﬁnes, based on the user speciﬁcations, a geometrical path in the task space, which is then converted to a geometrical path in the joint space using the inverse kinematic model discussed in Section 2.2.1. If the user speciﬁcations are expressed for the joints then a geometrical path is directly deﬁned in the joint space. Obstacle avoidance is also taken care of in the path planner. Note that the path only includes geometrical properties and nothing about time 20 2 User speciﬁcations Path planner Industrial Robots Models Trajectory generator Motion control Robot Sensors Figure 2.3: Block diagram of the robot control system. dependencies such as velocity and acceleration. The trajectory generator takes the geometrical path, which is deﬁned in the path planner, and the time dependencies. The time dependencies are deﬁned by the velocities and accelerations, which are speciﬁed by the user, but also limitations on the actuators, such as the joint torques, for the particular robot are used. To get a time dependent trajectory, that does not run into the physical limitations of the robot, requires a dynamical model of the manipulator. The dynamical model is discussed in Section 2.2.2. The problem to get the trajectory over time given the limitations can be formulated as an optimisation problem, see e.g. Verscheure et al. [2009] and Ardeshiri et al. [2011]. The output from the trajectory generator is position, velocity and acceleration of the robot joints as a function of time, which corresponds to the desired motion of the end-eﬀector in the task space. The motion controller uses the trajectories generated in the trajectory generator for control of the actuators, which often are electrical motors. Diﬀerent types of control structures have been proposed in the literature. The most common ones are independent joint control, feed-forward control, and feedback linearisation. These control methods will be discussed in Section 2.3. The output from the controller is the desired actuator torque for each actuator. Actually, the control signals to the actuators are electrical currents. A torque controller is therefore needed which takes the actuator torque from the controller and calculates the corresponding current. The torque controller is usually assumed to be signiﬁcantly faster than the robot dynamic and can therefore be omitted without inﬂuencing the control performance. The path planner, trajectory generator, and motion controller make an extensive use of models. The models can be divided into kinematic and dynamic models, where the kinematic models describe the relation between the pose of the endeﬀector and the joint angles, see Section 2.2.1 for details. The dynamic models describe the motion of the robot given the forces and torques acting on the robot, see Section 2.2.2. The remaining of this chapter presents modelling and control for industrial manipulators and the main references are Spong et al. [2006]; Sciavicco and Siciliano [2000]; Siciliano and Khatib [2008]; Craig [1989], and Kozłowski [1998]. 2.2 21 Modelling 2.2 2.2.1 Modelling Kinematic Models The kinematics describe the motion of bodies relative to each other. The position, velocity, acceleration and higher order time derivatives of the pose are studied regardless the forces and torques acting upon the bodies. The kinematic relations contain therefore only the geometrical properties of the motion over time. The kinematic relations can be derived from simple coordinate transformations between diﬀerent coordinate systems. The kinematics for an industrial robot can be divided into two diﬀerent parts. The ﬁrst consists of the relations between the known joint positions and the unknown pose of the end-eﬀector. The second is the opposite, consisting of the relations between the known pose of the end-eﬀector and unknown joint positions. These are called forward and inverse kinematics, respectively. Coordinate Transformation Let the vector rj be ﬁxed in frame j. The transformation to frame i can be written as ri = rj/i + Qj/i rj , (2.1) where rj/i is the vector from the origin in frame i to the origin in frame j and Qj/i is the rotation matrix representing the orientation of frame j with respect to frame i. The rotation can be described using the so called Euler angles, which are intuitive but can cause singularities. Instead, unit quaternions, which do not suﬀer from singularities, can be used. They are however not as intuitive as Euler angles. Another representation of a rotation is the axis/angle representation. A serial industrial robot with n dof consists of n + 1 rigid links (bodies) attached to each other in series. Let the links be numbered 0 to n, where link n is the end-eﬀector and link 0 is the world, and let coordinate frame i be ﬁxed in link i − 1. The pose of frame i relative to frame i − 1 is assumed to be known, i.e., ri/i−1 and Qi/i−1 are known. The transformation between two connected links can therefore be written as ri−1 = ri/i−1 + Qi/i−1 ri , (2.2) using (2.1). Iterating (2.2) over all links will give a relation of the pose of link n expressed in frame 0, which can be seen as the pose of the end-eﬀector expressed in the world frame, for a robot application. Equation (2.2) is described by a sum and a matrix multiplication which can be rewritten as one matrix multiplication if homogeneous coordinates are introduced according to r h ri = i . (2.3) 1 22 2 Equation (2.2) can now be reformulated as Qi/i−1 ri/i−1 h rhi−1 = ri = Hi/i−1 rhi . 0 1 Industrial Robots (2.4) The advantage with this representation is that the relation of the pose of link n expressed in link 0 can be written as only matrix multiplications according to ⎞ ⎛ n ⎟⎟ ⎜⎜ Q r h h h n/0 n/0 ⎟ ⎜ Hi/i−1 ⎟⎟⎠ rn = (2.5) rhn = Hn/0 rhn , r0 = H1/0 · . . . · Hn/n−1 rn = ⎜⎜⎝ 0 1 i=1 where rn/0 represents the position and Qn/0 the orientation of the end-eﬀector frame with respect to frame 0, rhn is an arbitrary vector expressed in the endeﬀector frame, and Hi/i−1 is given by (2.4). Forward Kinematics The forward kinematics for a n dof industrial robot is the problem of determining the position and orientation of the end-eﬀector frame relative to the world T frame given the joint angles qa = qa1 . . . qan . Here the subscript a is used to emphasis that the joint angles are described at the arm side of the gearbox, which will be convenient when ﬂexible models are introduced later in this chapter. The world frame is a user deﬁned frame where the robot is located, e.g. the industrial ﬂoor. The position p ∈ R3 is given in Cartesian coordinates and the orientation φ ∈ R3 is given in Euler angles, or quaternions if desirable. The forward kinematics has a unique solution for a serial robot, e.g. abb irb4600 in Figure 2.1a, whereas there exist several solutions for a parallel arm robot such as abb irb360 in Figure 2.1b. The kinematic relations can be written as a non-linear mapping according to p Ξ= = Υ(qa ), (2.6) φ where Υ(qa ) : Rn → R6 is a non-linear function given by the homogeneous transformation matrix Qn/0 rn/0 (2.7) Hn/0 = 0 1 in (2.5), where the dependency of qa has been omitted. The position of the endeﬀector frame, i.e., the ﬁrst three rows in Υ(qa ), is given by rn/0 and the orientation of the end-eﬀector frame, i.e., the last three rows in Υ(qa ), is given by Qn/0 . The construction of Hn/0 , i.e., determination of all the Hi/i−1 matrices in (2.4), can be diﬃcult for complex robot structures. A systematic way to assign coordinate frames to simplify the derivation of Hn/0 is the so-called Denavit-Hartenberg (dh) convention [Denavit and Hartenberg, 1955]. Taking the derivative of (2.6) with respect to time gives a relation between the joint velocities q̇a , the linear velocity v, and angular velocity ω of the end-eﬀector 2.2 23 Modelling according to ∂Υ(qa ) ṗ v = = q̇a = J(qa )q̇a , Ξ̇ = ω φ̇ ∂qa (2.8) where J(qa ) is the Jacobian matrix of Υ(qa ). The linear acceleration a and angular acceleration ψ of the end-eﬀector are given by the second time derivative of (2.6) according to d v̇ a Ξ̈ = J(qa ) q̇a , (2.9) = = J(qa )q̈a + ω̇ ψ dt where q̇a and q̈a are the joint velocities and accelerations, respectively. The time derivative of the Jacobian can be written as ∂J(q ) d a q̇ . J(qa ) = dt ∂qai ai n (2.10) i=1 The Jacobian matrix is fundamental in robotics. Besides being useful for the calculation of velocities and accelerations, it can also be used for • identiﬁcation of singular conﬁgurations, • trajectory planning, • transformation of forces and torques acting on the end-eﬀector to the corresponding joint torques. The Jacobian in (2.8) is known as the analytical Jacobian. Another Jacobian is the geometrical Jacobian. The diﬀerence between the analytical and geometrical Jacobian aﬀects only the angular velocity and acceleration. The geometrical Jacobian is not considered in this work. Inverse Kinematics In practice, often the position p and orientation φ of the end-eﬀector are given by the operating program and the corresponding joint angles qa are required for the control loop. An inverse kinematic model is needed in order to get the corresponding joint angles. For a serial robot, the inverse kinematics is a substantially harder problem which can have several solutions or no solutions at all, as opposed to the forward kinematics. For a parallel arm robot the inverse kinematics is much easier and gives a unique solution. In principle, the non-linear system of equations in (2.6) must be inverted, i.e., qa = Υ−1 (Ξ). (2.11) If an analytical solution does not exist, a numerical solver must be used in every time step. Given the joint angles qa , the linear velocity v and angular velocity ω, i.e., Ξ̇, then the angular velocities q̇a can be calculated from (2.8) according to q̇a = J−1 (qa )Ξ̇, (2.12) 24 2 Industrial Robots if the Jacobian is square and non-singular. The Jacobian is a square matrix when n = 6 since Υ(qa ) has six rows, three for the position and three for the orientation. The singularity depends on the joint angles qa . The Jacobian is singular if the robot is at the boundary of the work space, i.e., outstretched or retracted, or if one or more axes are aligned. The intuitive explanation is that the robot has lost one or more degrees of freedom when the Jacobian is singular. It is then not possible to move the end-eﬀector in all directions regardless of how large torques the controller applies. If the robot has less than six joints, i.e., the Jacobian has less than six columns, then the inverse velocity kinematics has a solution if and only if rank J(qa ) = rank J(qa ) Ξ̇ , (2.13) that is, Ξ̇ lies in the range space of the Jacobian. If instead the robot has more than six joints, then the inverse velocity kinematics is given by q̇a = J† (qa )Ξ̇ + I − J† (qa )J(qa ) b, (2.14) where J† (qa ) is the pseudo inverse [Mitra and Rao, 1971] of J(qa ) and b ∈ Rn is an arbitrary vector. See Spong et al. [2006] for more details. The angular acceleration q̈a can be calculated in a similar way from (2.9), when Ξ̈, qa , and q̇a are known and if the Jacobian is invertible, according to d −1 (J(q (2.15) q̈a = J (qa ) Ξ̈ − a )) q̇a . dt 2.2.2 Dynamic Models The dynamics describes the motion of a body considering the forces and torques causing the motion. The dynamic equations can be derived from the NewtonEuler formulation or Lagrange’s equation, see e.g. Goldstein et al. [2002]. The methods may diﬀer in computational eﬃciency and structure but the outcome is the same. Here, only Lagrange’s equation will be covered. Rigid Link Model For Lagrange’s equation the Lagrangian L(q, q̇) is deﬁned as the diﬀerence between the kinetic and potential energies. The argument q to the Lagrangian is a set of generalised coordinates. A system with n dof can be described by n gen T eralised coordinates q = q1 . . . qn , e.g. position, velocity, angle or angular velocity that describe the system. The dynamic equations are given by Lagrange’s equation d ∂L(q, q̇) ∂L(q, q̇) − = τi , dt ∂q̇i ∂qi (2.16) where the Lagrangian L(q, q̇) = K(q, q̇) − P(q) is the diﬀerence between the kinetic and potential energies, and τi is the generalised force associated with qi . For an industrial robot, the generalised coordinates are the joint angles qa , and the gen- 2.2 25 Modelling eralised forces are the corresponding motor torques. The dynamical equations for an industrial manipulator, given by Lagrange’s equation, can be summarised by M(qa )q̈a + C(qa , q̇a ) + G(qa ) = τ am , (2.17) where M(qa ) is the inertia matrix, C(qa , q̇a ) is the Coriolis- and centrifugal terms, a a T . Note that the equa. . . τmn G(qa ) is the gravitational torque and τ am = τm1 tion is expressed on the arm side of the gearbox, that is, the applied motor torque τmi must be converted from the motor side to the arm side of the gearbox. This is done by multiplication by the gear ratio ηi > 1, according to a τmi = τmi ηi . (2.18) Each link in the rigid body dynamics in (2.17) is described by a mass, three lengths describing the geometry, three lengths describing the centre of mass and six inertia parameters. The centre of gravity and the inertia are described in the local coordinate system. Each link is thus described by 13 parameters that have to be determined, see e.g. Kozłowski [1998]. The model can also be extended with a friction torque F(q̇i ) for each joint. A classical model is F(q̇i ) = f vi q̇i + f ci sign(q̇i ), (2.19) where f vi is the viscous friction and f ci is the Coulomb friction for joint i. More advanced models are the LuGre model [Åström and Canudas de Wit, 2008] and the Dahl model, see Dupont et al. [2002] for an overview. In this work, a smooth static friction model, suggested in Feeny and Moon [1994], given by F(q̇i ) = f vi q̇i + f ci μki + (1 − μki ) cosh−1 (β q̇i ) tanh(αi q̇i ), (2.20) is used. Here, the friction is only dependent on the velocity of the generalised coordinates. In practice, the measured friction curve on a real robot shows a dependency on the temperature and the dynamical load of the end-eﬀector, as described in Carvalho Bittencourt et al. [2010]; Carvalho Bittencourt and Gunnarsson [2012]. Flexible Joint Model In practice, the joints, specially the gearboxes, are ﬂexible. These ﬂexibilities are distributed in nature but this can be simpliﬁed by considering a ﬁnite number of ﬂexible modes. With a reasonable accuracy, it is possible to model each joint as a torsional spring and damper pair between the motor and arm side of the gearbox, see Figure 2.4. The system now has 2n dof and can be described by the simpliﬁed ﬂexible joint model Ma (qa )q̈a + C(qa , q̇a ) + G(qa ) = T (qam − qa ) + D(q̇am − q̇a ), Mm q̈am + F(q̇am ) = τ am − T (qam − qa ) − D(q̇am − q̇a ), (2.21a) (2.21b) where qa ∈ Rn are the arm angles, qam ∈ Rn are the motor angles. The superscript a indicates that the motor angles are expressed on the arm side of the gearbox, 26 2 Industrial Robots η T ( · ), D( · ) τm , qm qa Ma Mm F( · ) Figure 2.4: Flexible joint model where the arm angular position qa is related to the motor angular position qm and motor torque τm via the gear ratio η and the spring-damper pair modelled by T ( · ) and D( · ). The motor friction is modelled by F( · ). a i.e., qmi = qmi /ηi where qmi is the motor angle on the motor side of the gearbox for joint i. The same applies for the motor torque τ am according to (2.18). Furthermore, Ma (qa ) is the inertia matrix for the arms, Mm is the inertia for the motors, C(qa , q̇a ) is the Coriolis- and centrifugal terms, G(qa ) is the gravitational torque and F(q̇am ) is the friction torque. Moreover, T (qam − qa ) is the stiﬀness torque and D(q̇am − q̇a ) is the damping torque. Both the stiﬀness and damping torque can be modelled as linear or non-linear. The simpliﬁed ﬂexible joint model assumes that the couplings between the arms and motors are neglected, which is valid if the gear ratio is high [Spong, 1987]. In the complete ﬂexible joint model the term S(qa )q̈am is added to (2.21a) and the term S T (qa )q̈a as well as a Coriolisand centrifugal term are added to (2.21b), where S(qa ) is a strictly upper triangular matrix. A complete description of the simpliﬁed and complete ﬂexible joint model can be found in De Luca and Book [2008]. The ﬂexible joint model described above assumes that the spring and damper pairs are in the rotational direction. Another extension is to introduce multidimensional spring and damper pairs in the joints to deal with ﬂexibilities in other directions than the rotational direction, where each dimension of the spring and damper pairs corresponds to two dof. If the links are ﬂexible, then it can be modelled by dividing each ﬂexible link into several parts connected by multidimensional spring and damper pairs. This leads to extra non-actuated joints, hence more dofs. This is known as the extended ﬂexible joint model and a thorough description can be found in Moberg et al. [2014]. 2.3 Motion Control Control of industrial manipulators is a challenging task. The robot is a strongly coupled multivariate ﬂexible system with non-linear dynamics which changes all over the work space. In addition, other non-linearities such as hysteresis, backlash, friction, and joint ﬂexibilities have to be taken care of. Furthermore, the controlled variable is not measured directly and available sensors only provide parts of the information important for control, e.g. the measured motor angu- 2.3 Motion Control 27 Table 2.1: Performance for an abb irb6640 with a payload of 235 kg and a reach of 2.55 m at 1.6 m/s according to ISO 9283 [abb Robotics, 2013]. Pose accuracy 0.15 mm Pose repeatability 0.05 mm Settling time (within 0.4 mm) 0.19 s Path accuracy 2.17 mm Path repeatability 0.66 mm lar positions do not contain information about the oscillation of the end-eﬀector, hence good control of the tcp is diﬃcult. The available measurements are also aﬀected by uncertainties such as quantisation errors and measurement noise, as well as deterministic disturbances such as resolver ripple. The requirements for controllers in modern industrial manipulators are that they should provide high performance, despite the ﬂexible and non-linear mechanical structure, and at the same time, robustness to model uncertainties. Typical requirements for the motion controller are presented in Table 2.1. In the typical standard control conﬁguration the actuator positions are the only measurements used in the higher level control loop. At a lower level, in the drive system, the currents and voltages in the motors are measured to provide torque control for the motors. In this thesis it is assumed that the lower level control loop is ideal, i.e., the control loop is signiﬁcantly faster than the remaining system and the control performance is suﬃciently good not to aﬀect the performance of the higher level control loop. Adding extra sensors such as encoders measuring the joint angles and/or accelerometers measuring the acceleration of the end-eﬀector are possible extensions to the control problem. The use of an accelerometer for a single ﬂexible joint model, using H∞ -control methods, is investigated in Paper E. In the literature, the three control structures i) independent joint control, ii) feedforward control, and iii) feedback linearisation are common and they will be summarised in this section. Also, how the model complexity aﬀects the control structure will be discussed. The survey Sage et al. [1999] and the references therein describe how various models and control structures are combined for robust control. The survey includes both rigid and ﬂexible joint models with or without the actuator dynamics. The control structures are, among others, feedback linearisation, pid controllers, linear and non-linear H∞ methods, passivity based controllers, and sliding mode controllers. 2.3.1 Independent Joint Control In this control structure, each joint is, as the name suggests, controlled independently from the other joints using pid controllers. The inﬂuence from the other joints can be seen as disturbances. Usually the motor positions are measured and used in the feedback loop to calculate the motor torque for the corresponding 28 2 Industrial Robots joint. A common controller presented in the literature is the pd controller u = KP · (qd,m − qm ) + KD · (q̇d,m − q̇m ), (2.22) where KP and KD are diagonal matrices. Due to ﬂexible joints, presented in Section 2.2.2, other sensors such as encoders, measuring the arm angles, can be used to improve the performance of the robot. However, using measurements from the arm side of the gearbox to control the motor torque on the motor side result in so called non-collocated control [Franklin et al., 2002]. Non-collocated control problems are diﬃcult to stabilise as Example 2.1 shows. Example 2.1: Non-collocated control For a single ﬂexible joint in a horizontal plane, i.e., no gravity, it is fairly simple to show that the motor velocity has to be present in the feedback loop in order to stabilise the system [De Luca and Book, 2008]. For simplicity, no damping and friction are present, which corresponds to the worst case. Let the dynamical model be described by Ma q̈a − K · (qm − qa ) = 0, Mm q̈m + K · (qm − qa ) = τ. (2.23a) (2.23b) Using Laplace transformation, gives the two transfer functions Qa (s) = K T (s), Ma Mm s 4 + (Ma + Mm )K s 2 (2.24a) Ma s2 + K Qa (s). (2.24b) K Four feedback loops will be analysed using diﬀerent combinations of the arm position and velocity and the motor position and velocity as measurements. It will be assumed, without loss of generality, that both the arm and motor velocity references are equal to zero. The controllers are: Qm (s) = 1. feedback from arm position and arm velocity, τ = KP · (qa,d − qa ) − KD q̇a (2.25a) 2. feedback from arm position and motor velocity, τ = KP · (qa,d − qa ) − KD q̇m (2.25b) 3. feedback from motor position and arm velocity, τ = KP · (qm,d − qm ) − KD q̇a (2.25c) 4. feedback from motor position and motor velocity, τ = KP · (qm,d − qm ) − KD q̇m (2.25d) The corresponding closed-loop transfer functions from Qa,d (s) to Qa (s) become 2.3 29 Motion Control 1. Ma Mm s4 K KP + (Ma + Mm )K s2 + K KD s + K KP (2.26a) 2. K KP + (Ma + Mm )K s2 + K KD s + K KP (2.26b) K KP Ma Mm s 4 + ((Ma + Mm )K + KP Ma )s 2 + K KD s + K KP (2.26c) Ma Mm s4 + KD Ma s3 3. 4. Ma Mm s4 + KD Ma s3 K KP + ((Ma + Mm )K + KP Ma )s 2 + K KD s + K KP (2.26d) Using Routh’s algorithm [Franklin et al., 2002] makes it possible to analyse the stability conditions for the four controllers. Routh’s algorithm says that a system is stable if and only if all the elements in the ﬁrst column of the Routh array are positive, see Franklin et al. [2002] for deﬁnition of the Routh array. The Routh arrays for the four controllers show that 1. feedback from arm position and arm velocity is unstable independent of the parameters KP and KD . 2. feedback from arm position and motor velocity is stable if KD > 0 and 0 < KP < K. 3. feedback from motor position and arm velocity is unstable independent of the parameters KP and KD . 4. feedback from motor position and motor velocity is stable for all KP , KD > 0. It means that if sensors on the arm side of the gearbox are introduced, it is still necessary to have the motor velocity included in the feedback loop in order to get a stable system. If damping and friction are introduced, then the system can be stabilised without need of the motor velocity but the stability margin can be very low. For a robot with more than one joint it is more complicated to prove stability. To do so, Lyapunov stability and LaSalle’s theorem [Khalil, 2002] have to be used. It can be shown that a rigid joint manipulator aﬀected by gravity is stabilised by a pd controller. However, in order to have the joint angles to converge to the desired joint angles a gravity compensating term must be included in the controller, see Chung et al. [2008] for details. Similar proofs for ﬂexible joint manipulators exist in e.g. De Luca and Book [2008]. 30 2 Industrial Robots In Tomei [1991] it is shown that a ﬂexible joint robot can be robustly stabilised by a decentralised pd controller with motor positions as measurement. Robustness with respect to model parameters is also discussed. Moreover, Rocco [1996] presents stability results, including explicit stability regions, for a rigid joint manipulator using pid controllers. 2.3.2 Feed-forward Control Independent joint control is insuﬃcient for trajectory tracking. When the robot should follow a desired trajectory, restrictions on the path are required not to excite the ﬂexibilities and a model-based controller is necessary. Introducing a feed-forward controller that takes the dynamics of the robot into account makes it possible to follow the desired trajectory without exciting the ﬂexibilities. The feed-forward controller, for a rigid joint manipulator, takes the form d , q̇d ) + G(q d ), d )q̈d + C(q uf f = M(q (2.27) and G are the models used in the control system. Note that the feed C, where M, forward controller requires that the reference signal qd is twice diﬀerentiable with respect to time. If the model is exact and no disturbances are present, then the feed-forward controller achieves perfect trajectory tracking. Model errors and disturbances require that independent joint control also has to be present, giving the total control signal as d , q̇d ) + G(q d ) + KP · (qd − q) + KD · (q̇d − q̇) . d )q̈d + C(q u = M(q feed-forward (2.28) feedback The feed-forward controller design is a more diﬃcult problem for ﬂexible joint models. It depends on the complexity of the model, if for example the extended ﬂexible joint model is used, then a high-index diﬀerential algebraic equation (dae) has to be solved [Moberg and Hanssen, 2009]. 2.3.3 Feedback Linearisation Feedback linearisation [Khalil, 2002], also known as exact linearisation in the control literature, is a control method similar to feed-forward. Instead of having the dynamical model in feed-forward it is used in a static feedback loop to cancel the non-linear terms. For a rigid joint manipulator, the feedback is given by q̇) + G(q) u = M(q)v + C(q, (2.29) where q and q̇ are the measured joint positions and velocities, and v is a signal to chose. In case of no model mismatch, the feedback signal (2.29) gives the decoupled system q̈ = v. The signal v can be obtained as the output from e.g. a pd controller, i.e., independent joint control, according to v = q̈d + KP · (qd − q) + KD · (q̇d − q̇). (2.30) The drawback is that it is computational demanding to calculate the inverse dynamical model in the feedback loop as well as the need of the full state trajec- 2.3 Motion Control 31 tory. Robustness properties for the controller are discussed in Bascetta and Rocco [2010]. For ﬂexible joint models, the problem gets more involved, as was the case for the feed-forward controller. In Spong [1987] it is shown that a static feedback loop can linearise and decouple the model in (2.21) when the friction and damping terms are excluded. The complete ﬂexible joint model, i.e., when the matrix S(qa ) is included in (2.21), with the friction and damping terms excluded is proved to be linearisable using a dynamic non-linear feedback law in De Luca and Lanari [1995]. So far the damping and friction terms have been neglected. In De Luca et al. [2005] it is shown that it is possible to achieve input-output linearisation with a static or dynamic non-linear feedback loop. Input-output linearisation does not eliminate all the non-linearities, instead the so called zero-dynamics remains. However, the ﬂexible joint model gives stable zero-dynamics, hence input-output linearisation can be used for the model. 3 Estimation Theory ifferent techniques for non-linear estimation are presented in this chapter. The estimation problem for the discrete time non-linear state space model D xk+1 = f (xk , uk , wk ; θ), yk = h(xk , uk , vk ; θ), (3.1a) (3.1b) is to ﬁnd the state vector xk ∈ Rnx at time k and the unknown model parameters θ given the measurements yi ∈ Rny for i = 1, . . . , l. Here, l can be smaller, larger or equal to k depending on the method that is used. In this work the focus is on non-linear models with additive process noise wk and measurement noise vk given by xk+1 = f (xk , uk ; θ) + g(xk ; θ)wk , yk = h(xk , uk ; θ) + vk , (3.2a) (3.2b) where the probability density functions (pdfs) for the process noise pw (w; θ), and measurement noise pv (v; θ), are known except for some unknown parameters. The estimation problem can be divided into the ﬁltering problem where only previous measurements up to the present time are available, i.e., l = k, see Section 3.1 and the smoothing problem where both previous and future measurements are available, i.e., l > k, see Section 3.2. For the case with estimation of the unknown parameters θ, the expectation maximisation algorithm can be used as described in Section 3.3. 33 34 3.1 3 Estimation Theory The Filtering Problem The ﬁltering problem can be seen as calculation/approximation of the posterior density p(xk |y1:k ) using all measurements up to time k, where y1:k = {y1 , . . . , yk }, (3.3) and the known conditional densities for the state transition and measurements, i.e., xk+1 ∼ p(xk+1 |xk ), yk ∼ p(yk |xk ), (3.4a) (3.4b) which are given by the model (3.2). Using Bayes’ law, p(x|y) = p(y|x)p(x) , p(y) (3.5) and the Markov property for the state space model, p(xn |x1 , . . . xn−1 ) = p(xn |xn−1 ), (3.6) repeatedly, the optimal solution for the Bayesian inference [Jazwinski, 1970] can be obtained according to p(yk |xk )p(xk |y1:k−1 ) , p(yk |y1:k−1 ) p(xk+1 |y1:k ) = p(xk+1 |xk )p(xk |y1:k ) dxk , p(xk |y1:k ) = (3.7a) (3.7b) Rn x where k = 1, 2, . . . , N and p(yk |y1:k−1 ) = p(yk |xk )p(xk |y1:k−1 ) dxk . (3.7c) Rn x The solution to (3.7) can in most cases not be given by an analytical expression. For the special case of linear dynamics, linear measurements and additive Gaussian noise the Bayesian recursions in (3.7) have an analytical solution, which is known as the Kalman ﬁlter (kf) [Kalman, 1960]. For non-linear and non-Gaussian systems, the posterior density cannot in general be expressed with a ﬁnite number of parameters. Instead approximative methods must be used. Here, two approximative solutions are considered; the extended Kalman ﬁlter and the particle ﬁlter. Another approximative solution not considered here is the unscented Kalman ﬁlter (ukf) [Julier et al., 1995]. 3.1.1 The Extended Kalman Filter The extended Kalman ﬁlter (ekf) [Anderson and Moore, 1979; Kailath et al., 2000] solves the Bayesian recursions in (3.7) using a ﬁrst order Taylor expansion of the non-linear system equations around the previous estimate. The approximation is acceptable if the non-linearity is almost linear or if the signal to noise 3.1 35 The Filtering Problem ratio (snr) is high. The Taylor expansion requires derivatives of the non-linear system equations, which can be obtained by symbolic or numeric diﬀerentiation. The process noise wk and measurement noise vk are assumed to be Gaussian with zero means and covariance matrices Qk and Rk , respectively. The time update, x̂k|k−1 and Pk|k−1 , and the measurement update, x̂k|k and Pk|k , for the ekf with the non-linear model (3.2) can be derived relatively easy using the ﬁrst order Taylor approximation and the kf equations. The time and measurement updates are presented in Algorithm 1, where the notation x̂k|k , Pk|k , x̂k|k−1 and Pk|k−1 denotes estimates of the state vector x and covariance matrix P at time k using measurements up to time k and k − 1, respectively. It is also possible to use a second order Taylor approximation when the ekf equations are derived [Gustafsson, 2010]. Algorithm 1 The Extended Kalman Filter (ekf) Initialisation x̂0|0 = x0 P0|0 = P0 (3.8a) (3.8b) Time update x̂k|k−1 = f (x̂k−1|k−1 , uk−1 ) (3.9a) Pk|k−1 = Fk−1 Pk−1|k−1 FTk−1 + Gk−1 Qk−1 GTk−1 ∂f (x, uk−1 ) Fk−1 = ∂x (3.9b) (3.9c) x=x̂k−1|k−1 Gk−1 = g(x̂k−1|k−1 ) Measurement update (3.9d) −1 Kk = Pk|k−1 HTk Hk Pk|k−1 HTk + Rk x̂k|k = x̂k|k−1 + Kk yk − h(x̂k|k−1 , uk ) Pk|k = (I − Kk Hk ) Pk|k−1 ∂h(x, uk ) Hk = ∂x (3.10a) (3.10b) (3.10c) (3.10d) x=x̂k|k−1 3.1.2 The Particle Filter The particle ﬁlter (pf) [Doucet et al., 2001; Gordon et al., 1993; Arulampalam et al., 2002] solves the Bayesian recursions using stochastic integration. The pf (i) approximates the posterior density p(xk |y1:k ) by a large set of N particles {xk }N i=1 , N (i) (i) where each particle has a relative weight wk , chosen such that i=1 wk = 1. The position and weight of each particle approximate the posterior density in such a way that a high weight corresponds to a high probability at the point given by the particle. The pf updates the particle location and the corresponding weights recursively with each new observed measurement. The particle method for solv- 36 3 Estimation Theory ing the Bayesian recursion in (3.7) has been known for long but the particle ﬁlter has in practice been inapplicable due to degeneracy of the particles. The problem was solved in Gordon et al. [1993] by introducing a resampling step. Compared to the ekf, the pf does not suﬀer from linearisation errors and can handle non-Gaussian noise models. Hard constraints on the state variables can also be incorporated into the estimation problem. Theoretical results show that the approximated posterior density converges to the true density when the number of particles tends to inﬁnity, see e.g. Doucet et al. [2001]. The pf is summarised (i) (i) in Algorithm 2, where the proposal density p prop (xk+1 |xk , yk+1 ) can be chosen arbitrary as long as it is possible to draw samples from it. For small snr the (i) (i) conditional prior of the state vector, i.e., p(xk+1 |xk ), is a good choice [Gordon et al., 1993]. Using the conditional prior, the weight update can be written as (i) (i) (i) wk = wk−1 p(yk |xk ). The optimal proposal should be to use the conditional den(i) sity p(xk |xk−1 , yk ) [Doucet et al., 2000]. The problem is that it is diﬃcult to sample from it and also to calculate the weights. In Paper A the optimal proposal density, approximated by an ekf [Doucet et al., 2000; Gustafsson, 2010], is used for experimental data. The approximated proposal density can be written as † (i) (i) (i) (i) (i),T † (i) prop † p (xk |xk−1 , yk ) ≈ N xk ; f (xk−1 ) + Kk (yk − ŷk ), Hk Rk Hk + Qk−1 , where † denotes the pseudo-inverse, and where −1 (i) (i),T (i) (i),T Kk = Qk−1 Hk , Hk Qk−1 Hk + Rk ∂h(xk ) (i) Hk = , ∂xk xk =f (x(i) ) (3.11a) (3.11b) k−1 (i) (i) ŷk = h(f (xk−1 )). (3.11c) The matrices in (3.11) are evaluated for each particle. The approximated optimal proposal density gives a weight update according to (i) (i) (i) wk = wk−1 p(yk |xk−1 ), (i) (i) (i) (i),T p(yk |xk−1 ) = N yk ; ŷk , Hk Qk−1 Hk + Rk . (3.12a) (3.12b) The state estimate for each sample k is often chosen as the minimum mean square estimate x̂k|k = arg min E (x̂k − xk )T (x̂k − xk ) |y1:k , (3.13) x̂k which has the solution x̂k|k = E [xk |y1:k ] = xk p (xk |y1:k ) dxk ≈ Rn x N i=1 (i) (i) wk x k . (3.14) 3.2 37 The Smoothing Problem Algorithm 2 The Particle Filter (pf) (i) Generate N samples {x0 }N i=1 from p(x0 ). 2: Compute (i) (i) (i) (i) (i) p(yk |xk )p(xk |xk−1 ) wk = wk−1 (i) (i) p prop (xk |xk−1 , yk ) (j) (i) (i) and normalise, i.e., w̄k = wk / N j=1 wk , i = 1, . . . , N . 1: 3: (i ) [Optional]. Generate a new set {xk }N i=1 by resampling with replacement N (i) (i) (i ) (i) (i) times from {xk }N i=1 , with probability w̄k = Pr{xk = xk } and let wk = 1/N , i = 1, . . . , N . 4: Generate predictions from the proposal density (i) (i ) xk+1 ∼ p prop (xk+1 |xk , yk+1 ), i = 1, . . . , N . 5: Increase k and continue to step 2. 3.2 The Smoothing Problem The smoothing problem is essentially the same as the ﬁltering problem except that future measurements are used instead of only measurements up to present time k. In other words the smoothing problem can be seen as computation/approximation of the density p(xk |y1:l ), where l > k. The smoothing problem solves the same equations as the ﬁlter problem except that future measurements are available. Approximative solutions must be used here as well when the problem is non-linear and non-Gaussian. Diﬀerent types of smoothing problems are possible, e.g. ﬁxed-lag, ﬁxed-point and ﬁxed-interval smoothing [Gustafsson, 2010]. In this thesis, the ﬁxed-interval smoothing problem is considered and the extended Kalman smoother (eks) [Yu et al., 2004] is used. The ﬁxed-interval smoothing problem is an oﬀ-line method that uses all available measurements y1:N . The eks, using the Rauch-Tung-Striebel formulas, is presented in Algorithm 3. 3.3 The Expectation Maximisation Algorithm The maximum likelihood (ml) method [Fisher, 1912, 1922] is a well known tool for estimating unknown model parameters. The idea with the ml method is to ﬁnd the unknown parameters θ such that the measurements y1:N become as likely as possible. In other words, θ̂ ml = arg max p θ (y1:N ), θ∈Θ (3.17) where p θ (y1:N ) is the pdf of the observations, i.e., the likelihood, parametrised with the parameter θ. Usually, the logarithm of the likelihood pdf, Lθ (y1:N ) = log p θ (y1:N ), (3.18) 38 3 Estimation Theory Algorithm 3 The Extended Kalman Smoother (eks) Run the ekf and store the time and measurement updates, x̂k|k−1 , x̂k|k , Pk|k−1 and Pk|k . 2: Initiate the backward time recursion, x̂sN |N = x̂N |N , (3.15a) 1: PsN |N = PN |N . 3: Apply the backward time recursion for k = N − 1, . . . , 1, s T −1 x̂k|N = x̂k|k + Pk|k Fk Pk+1|k x̂sk+1|N − x̂k+1|k , s − P P P−1 Psk|N = Pk|k + Pk|k FTk P−1 k+1|k k+1|N k+1|k k+1|k Fk Pk|k , ∂f (x, uk ) Fk = . ∂x (3.15b) (3.16a) (3.16b) (3.16c) x=x̂k|k is used and then the problem is to ﬁnd the parameter θ that maximises (3.18). These two problems are equivalent since the logarithm is a monotonic function. The solution can still be hard to ﬁnd which has lead to the development of the expectation maximisation (em) algorithm [Dempster et al., 1977]. The em algorithm is an ml estimator for models with latent variables. Let all of the the latent variables be denoted by Z. Take now the joint log-likelihood function Lθ (y1:N , Z) = log p θ (y1:N , Z) (3.19) of the observed variables y1:N and the latent variables Z. The latent variables are unknown, hence the joint log-likelihood function cannot be calculated. Instead, the expected value of (3.19) given y1:N has to be derived. The expected value is given by Γ(θ; θ l ) = Eθ l [log p θ (y1:N , Z)|y1:N ] , (3.20) where Eθ l [ · | · ] is the conditional mean with respect to a pdf deﬁned by the parameter θ l , and p θ ( · ) means that the pdf is parametrised by θ. The sought parameter θ is now given iteratively by θ l+1 = arg max Γ(θ; θ l ). θ∈Θ The connection to the likelihood estimate θ̂ et al., 1977] any θ, such that ml (3.21) comes from the fact that [Dempster Γ(θ; θ l ) > Γ(θ l ; θ l ), (3.22) Lθ (y1:N ) > Lθ l (y1:N ). (3.23) implies that Hence, maximising Γ(θ; θ l ) provides a sequence θ l , l = 1, 2, . . ., which approxi- 3.3 39 The Expectation Maximisation Algorithm ml mates θ̂ better and better for every iteration. in Algorithm 4 and it can be stopped when L (y ) − L (y The em algorithm is summarised ≤ L , 1:N ) (3.24) θ l+1 − θ l ≤ θ . (3.25) θ l+1 1:N θl or when Example 3.1 shows how the em algorithm can be used for identiﬁcation of parameters in state space models. Algorithm 4 The Expectation Maximisation (em) Algorithm 1: 2: Select an initial value θ0 and set l = 0. Expectation Step (E-step): Calculate Γ (θ; θ l ) = Eθ l [log p θ (y1:N , Z)|y1:N ] . 3: Maximisation Step (M-step): Compute θ l+1 = arg max Γ (θ; θ l ) . θ∈Θ 4: If converged, stop. If not, set l = l + 1 and go to step 2. Example 3.1: The em Algorithm for System Identiﬁcation System identiﬁcation is about to determine the unknown model parameters θ given observations y1:N . Here the em algorithm will be used, where all details can be found in Gibson and Ninness [2005]. Consider the discrete-time state space model xk+1 = axk + wk , yk = cxk + vk , (3.26a) (3.26b) where the process noise wk ∼ N (0, 0.1), the measurement noise wk ∼ N (0, 0.1) T and the unknown parameters are θ = a c . Following Algorithm 4 the ﬁrst is to choose the latent variables Z and then derive p θ (y1:N , Z). The latent variables are simply chosen as Z = x1:N +1 , and Bayes’ rule together with the Markov property of the state space model (3.26) give p θ (y1:N , x1:N +1 ) = p θ (x1 ) N p θ (xk+1 , yk |xk ), (3.27) k=1 where p θ (ξ k |xk ) ∼ N (θxk , Π) , xk+1 ξk = , yk 0.1 Π= 0 0 . 0.1 Using the deﬁnition of the normal distribution gives ⎛N ⎞ ⎞ ⎛N ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ 1 ξ k xk ⎟⎟⎟⎠ θT − tr Π−1 θ ⎜⎜⎜⎝ xk2 ⎟⎟⎟⎠ θT , log p θ (y1:N , x1:N +1 ) ∝ tr Π−1 ⎜⎜⎜⎝ 2 k=1 k=1 (3.28) (3.29) 40 3 Estimation Theory Table 3.1: Estimated model parameters â and ĉ in Example 3.1 averaged over 1000 mc simulations for diﬀerent number of data points. 50 100 500 1000 1500 5000 10000 N â 0.2190 0.2479 0.2923 0.2874 0.2954 0.3002 0.3004 ĉ 0.5608 0.5788 0.5963 0.5965 0.5990 0.5980 0.6002 where all terms independent of θ are omitted and tr is the trace operator. To obtain Γ(θ; θ l ) the expected value of log p θ (y1:N , x1:N +1 ) is calculated giving ⎞ ⎛N ⎛N ⎞ ⎟⎟ ⎟⎟ ⎜ ⎜⎜ 1 −1 ⎜ −1 T 2 ⎟ ⎜ ⎜ Γ(θ; θ l ) ∝ tr Π ⎜⎜⎝ Eθ l [ξ k xk |y1:N ]⎟⎟⎠ θ − tr Π θ ⎜⎜⎝ Eθ l xk |y1:N ⎟⎟⎟⎠ θT . 2 k=1 k=1 Here, the terms involving the expected values can be calculated using a Kalman smoother, see Gibson and Ninness [2005] for details. Next step is to maximise Γ(θ; θ l ). If N Eθ l xk2 |y1:N > 0, (3.30) k=1 then the Hessian of Γ(θ; θ l ) is negative deﬁnite and the maximising argument can be obtained by taking the derivative of Γ(θ; θ l ), with respect to θ, equal to zero. Derivative rules for the trace operator can be found in Lütkepohl [1996]. The maximising argument is ﬁnally given by ⎡N ⎤ ⎡N ⎤ ⎢⎢ ⎥⎥ ⎢⎢ ⎥⎥ 2 ⎢ ⎥ ⎢ (3.31) θ l+1 = Eθ l ⎢⎢⎣ ξ k xk y1:N ⎥⎥⎦ Eθ l ⎢⎢⎣ xk y1:N ⎥⎥⎥⎦ . k=1 k=1 Identiﬁcation of a and c has been performed for a simulated system, with the true values a∗ = 0.3 and c∗ = 0.6, for diﬀerent values of the number of samples N T and θ0 = 0.25 0.5 . Table 3.1 shows the estimates â and ĉ averaged over 1000 mc simulations, where it can be seen that the estimates approach the true values when the number of samples increases. Figure 3.1 shows how the estimates converge for 10 mc simulations using N = 5000. 0.7 c ∗ = 0.6 0.5 0.4 a∗ = 0.3 0.2 0 20 40 60 80 100 em iteration Figure 3.1: The estimate θ̂ during 100 iterations for 10 mc simulations. 4 Control Theory hree different types of control methods, which are used in this thesis, will be introduced in this chapter. First, the general H∞ -control method and the loop shaping method, which are feedback controllers, are presented in Sections 4.1 and 4.2, respectively. Second, the idea of iterative learning control is explained in Section 4.3, with focus on the norm-optimal design method. T 4.1 H∞ Control For design of H∞ controllers the system z P11 (s) P12 (s) w w = = P(s) y P21 (s) P22 (s) u u (4.1) is considered, where w are the exogenous input signals (disturbances and references), u is the control signal, y are the measurements and z are the exogenous output signals. Using a controller u = K(s)y, see Figure 4.1, the system from w to z can be written as z = P11 (s) + P12 (s)K(s)(I − P22 (s)K(s))−1 P21 (s) w = Fl (P, K)w, (4.2) where Fl (P, K) denotes the lower linear fractional transformation (lft) [Skogestad and Postletwaite, 2005]. The H∞ controller is the controller that minimises Fl (P, K)∞ = max σ̄ (Fl (P, K)(iω)) , ω (4.3) where σ̄( · ) denotes the maximal singular value. It is not always necessary and sometimes not even possible to ﬁnd the optimal H∞ controller. Instead, a subop41 42 4 w u P(s) Control Theory z y K(s) Figure 4.1: The system P(s) in connection with the controller K(s) symbolising the lft Fl (P, K). timal controller is derived such that Fl (P, K)∞ < γ, (4.4) where γ can be reduced iteratively until a satisfactory controller is obtained. The aim is to get γ ≈ 1, meaning that the disturbances are not magniﬁed. A stabilising proper controller exists if a number of assumptions are fulﬁlled as discussed in Skogestad and Postletwaite [2005]. Furthermore, eﬃcient iterative algorithms to ﬁnd K(s), such that (4.4) is fulﬁlled, exist [Skogestad and Postletwaite, 2005; Zhou et al., 1996]. Note that the resulting H∞ controller has the same state dimension as P. A common design method is to construct P(s) by augmenting the original system y = G(s)u with the weighting functions Wu (s), WS (s), and WT (s), see Figure 4.2, such that the system Fl (P, K) can be written as ⎛ ⎞ ⎜⎜Wu (s)Gwu (s)⎟⎟ ⎜⎜ ⎟ (4.5) Fl (P, K) = ⎜⎜ −WT (s)T (s) ⎟⎟⎟ , ⎝ ⎠ WS (s)S(s) where S(s) = (I + G(s)K(s))−1 is the sensitivity function, T (s) = I − S(s) is the complementary sensitivity function, and Gwu (s) = −K(s)(I + G(s)K(s))−1 is the transfer function from w to u. Equations (4.4) and (4.5) give σ̄ (Wu (iω)Gwu (iω)) < γ, ∀ω, σ̄ (WT (iω)T (iω)) < γ, ∀ω, σ̄ (WS (iω)S(iω)) < γ, ∀ω. (4.6a) (4.6b) (4.6c) The transfers functions Gwu (s), S(s), and T (s) can now be shaped to satisfy the requirements by choosing the weighting functions Wu (s), WS (s), and WT (s). The aim is to get a value of γ close to 1, which in general is hard to achieve and it requires good insight in the design method as well as the system dynamics. Note that the design process is a trade-oﬀ between the requirements of S(s) and T (s) since S(s) + T (s) = I. For example, both S(s) and T (s) cannot be small at the same frequency range. For more details about the design method, see e.g. Skogestad and Postletwaite [2005]; Zhou et al. [1996]. 4.1.1 Mixed-H∞ Control The mixed-H∞ controller design [Pipeleers and Swevers, 2013; Zavari et al., 2012] is a modiﬁcation of the standard H∞ -design method. Instead of choosing the 4.2 43 Loop Shaping u G(s) + w K(s) Wu (s) z1 WT (s) z2 WS (s) z3 y P(s) Figure 4.2: The original system G(s) augmented with the weighting functions Wu (s), WS (s), and WT (s), giving the system P(s). Moreover, the system P(s) is in connection with the controller K(s). weighting functions in (4.5) such that the H∞ -norm of all weighted transfer functions satisﬁes (4.6), the modiﬁed method divides the problem into design constraints and design objectives. The controller can now be found as the solution to (4.7a) minimise γ K(s) subject to WP (s)S(s)∞ < γ MS (s)S(s)∞ < 1 Wu (s)Gwu (s)∞ < 1 (4.7d) WT (s)T (s)∞ < 1 (4.7e) (4.7b) (4.7c) where γ not necessarily has to be close to 1. Here, the weighting function WS (s) has been replaced with MS (s) and WP (s) to separate the design constraints and the design objectives. The method can be generalised to other control structures and in its general form formulated as a multi-objective optimisation problem. More details about the general form and how to solve the optimisation problem are presented in Pipeleers and Swevers [2013]; Zavari et al. [2012]. 4.2 Loop Shaping The loop shaping method was ﬁrst presented in McFarlane and Glover [1992] and is based on robust stabilisation of a normalised coprime factorisation of the system as described in Glover and McFarlane [1989]. Robust stabilisation of a normalised coprime factorisation proceeds as follows. Let the system G(s) be described by its left coprime factorisation G(s) = M(s)−1 N (s), where M(s) and N (s) are stable transfer functions. The set of perturbed plants ! # "" "" Gp (s) = (M(s) + ΔM (s))−1 (N (s) + ΔN (s)) : "" ΔN (s) ΔM (s) "" < , (4.8) ∞ where ΔM (s), and ΔN (s) are stable unknown transfer functions representing the uncertainties and is the stability margin, is robustly stabilised by the controller 44 4 K(s) if and only if the nominal feedback system is stable and "" "" "" K(s) 1 −1 −1 " "" I (I − G(s)K(s)) M(s) """ ≤ . ∞ Control Theory (4.9) Given the state space model ẋ(t) = Ax(t) + Bu(t), y(t) = Cx(t) + Du(t), (4.10a) (4.10b) then the maximal stability margin is given by $ 1 = 1 + ρ(XY) = γmin , max (4.11) where ρ( · ) is the spectral radius. For a strictly proper model, i.e., D = 0, the two matrices X and Y are the unique and positive deﬁnite solutions to the algebraic Riccati equations AY + YAT − YCT CY + BBT = 0, T T (4.12a) T XA + A X − XBB X + C C = 0. (4.12b) The two Riccati equations have a unique solution if the state space realisation in (4.10) is minimal. A non-proper model, i.e., D 0, also has an explicit solution but the corresponding Riccati equations are more extensive [Skogestad and Postletwaite, 2005]. Given γ > γmin , then a controller in closed form is given by ⎛ ⎜⎜ A − BBT X + γ 2 L−T YCT C γ 2 L−T YCT K(s) = ⎜⎜⎝ BT X 0 L = (1 − γ 2 )I + XY. ⎞ ⎟⎟ ⎟⎟ , ⎠ (4.13a) (4.13b) From the deﬁnition of γmin in (4.11) it holds that L becomes singular if γ = γmin , hence the controller K(s) cannot be obtained. Implementing the solution in software is not diﬃcult, however the Matlab function ncfsyn, included in the Robust Control Toolbox , is a good alternative to use. In practice a value γ < 4 is to aim for, otherwise the robustness properties become too poor. Having γ < 4 corresponds to > 0.25 which means that a coprime uncertainty of at least 25 % is achieved. For a siso system, the stability margin = 0.25 corresponds to 4.4 dB gain margin and 29◦ phase margin. For loop shaping [McFarlane and Glover, 1992], the system G(s) is ﬁrst pre- and post-multiplied with weighting functions W1 (s) and W2 (s), see Figure 4.3, such that the shaped system Gs (s) = W2 (s)G(s)W1 (s) has desired properties. The desired properties are usually requirements on the cut-oﬀ frequency, the low frequency gain, and the phase margin. The controller Ks (s) is then obtained using (4.13) applied on the system Gs (s). The ﬁnal controller K(s) is given by K(s) = W1 (s)Ks (s)W2 (s). (4.14) 4.2 45 Loop Shaping Gs (s) W1 (s) G(s) W2 (s) Ks (s) Figure 4.3: Block diagram of the loop shaping procedure. The system G(s) is pre- and post-multiplied with the weighting functions W1 (s) and W2 (s). The controller Ks (s) is obtained from (4.13) using the shaped system Gs (s). Note that the structure in Figure 4.3 for loop shaping can be rewritten as a standard H∞ problem according to Figure 4.1, see Zhou et al. [1996] for details, and the methods in Section 4.1 can be used for controller synthesis. Loop shaping is a simple to use method which does not require any problem dependent uncertainty model. Instead, the normalised coprime factorisation and the perturbed plant in (4.8) give a quite general uncertainty description. Compared to the H∞ method described in Section 4.1, the solution does not require any γ iterations. The choice of the weighting functions W1 (s) and W2 (s) requires good understanding of the system to be controlled. The following working progress can be useful 1. Scale G(s) in order to improve conditioning of the system. Also, reordering the inputs and outputs such that the system is as diagonal as possible is to prefer. How to do the reordering can be obtained using the relative gain array (rga) [Skogestad and Postletwaite, 2005]. 2. Choose W1 (s) and W2 (s). In Skogestad and Postletwaite [2005] a detailed description of how to chose the weighting functions is presented where W1 (s) is composed as the product of several matrices which does diﬀerent work, such as shape of the singular values, alignment of the singular values at the desired bandwidth, and control over the actuator usage. 3. Calculate Ks (s) using (4.13). Modify the weighting functions W1 (s) and W2 (s) until γ < 4. 4. Calculate the controller K(s) using (4.14), and test the performance. Modify the weighting functions until the desired performance is achieved. More details about the robust stabilisation of the normalised coprime factorisation, as well as the loop shaping synthesis can be found in e.g. Skogestad and Postletwaite [2005]; Zhou et al. [1996]. Example 4.1: Loop shaping for a siso system Let the nominal siso system be given by G(s) = 100 . s 3 + 12s 2 + 30s + 100 (4.15) 46 4 Control Theory Magnitude [dB] 40 0 −40 Loop gain Shaped system Original system −80 10−1 100 101 102 Frequency [rad/s] (a) The Bode diagrams for the loop gain K(s)G(s), the shaped system Gs (s), and the original system G(s). 1.5 1 0.5 0 Closed loop system Open loop system 0 1 2 3 4 5 6 Time [s] (b) Step response for the open loop system and the closed loop system. Figure 4.4: Results for Example 4.1. Design a controller using loop shaping that attenuates the oscillations without changing the cut-oﬀ frequency. G(s) is a siso system, hence it is possible to let W2 (s) = 1 and only focus on W1 (s). Integral action, i.e., a pole in the origin for W1 (s), is added to get an improvement of the low frequency performance. Moreover, to improve the phase margin, a zero in s = −5 is added to achieve a slope of -1, instead of -2, at the cut-oﬀ frequency for the loop gain. The desired cut-oﬀ frequency is then achieved by multiplying with 0.8. The weighting functions are ﬁnally given as s+5 (4.16) , W2 (s) = 1. s The resulting controller, using ncfsyn, gives γ = 2.38, hence a stability margin of 42 % is achieved. This corresponds to 7.8 dB gain margin and 50◦ phase margin. The Bode diagrams of the shaped system Gs (s), the loop gain G(s)K(s), and the original system G(s) are shown in Figure 4.4a. Step responses of the open loop system and the closed loop system are presented in Figure 4.4b. It can be seen that the controller attenuates the oscillations and keeps the time constant of the open loop system. W1 (s) = 0.8 4.3 4.3 Iterative Learning Control 47 Iterative Learning Control Iterative learning control (ilc) is a way to improve the performance of systems that perform the same task repeatedly. An industrial manipulator performing arc welding or laser cutting is a good example of such a system. The performance of the system is improved by adding a correction signal, calculated oﬀ-line, to the system. At each iteration the correction signal is updated using the correction signal and the control error, both from previous iteration. The ilc concept has been used, to some extent, from the beginning of the 1970s. However, it was not until 1984, when the three articles Arimoto et al. [1984a], Casalino and Bartolini [1984] and Craig [1984] were published, independently from each other, that ilc became a widespread idea. The name iterative learning control originates from the article Arimoto et al. [1984b]. The following six assumptions are essential for the concept of ilc [Arimoto, 1990] 1. Every iteration consists of a ﬁxed number of samples. 2. A desired output r(t) is given a priori. 3. The system is initialised at the beginning of each iteration with the same settings, i.e., xk (0) = x0 for all k. 4. The system dynamics are invariant in the iteration domain. 5. The controlled variable zk (t) can be measured and used in the ilc algorithm. 6. The system dynamics are invertible, i.e., the desired output r(t) corresponds to a unique input ud (t). In practice, it is not possible to initialise the system at the beginning of each iteration with exact same settings. The system is, most probably, also aﬀected with input disturbances, induced during the iterations, and measurement noise. Therefore, assumptions 3, 4, and 5 are relaxed and replaced with [Arimoto, 1998] 3’. The system is initialised at the beginning of each iteration in a neighbourhood of x0 , i.e., xk (0) − x0 ≤ 1 for all k. 4’. The norm of the input disturbances, induced during the iterations, is bounded. 5’. The controlled variable zk (t) can be measured with bounded noise and used in the ilc algorithm. Assumption 5 can be relaxed even more to the case where the controlled variable zk (t) is not directly measured. Instead, the measurements yk (t) are used in an estimation algorithm in order to estimate the controlled variable, which can be used in the ilc algorithm. The idea of estimation-based ilc has been investigated in Wallén et al. [2009], Wallén et al. [2011a], and Wallén et al. [2011b]. Paper G deals with estimation-based ilc in combination with norm-optimal ilc. Over the years, several surveys of the ﬁeld of ilc have been published, e.g. Moore [1998]; Ahn et al. [2007]; Bristow et al. [2006]. Moore [1998] covers the published 48 4 Control Theory literature up to 1998 and Ahn et al. [2007] continues from 1998 to 2004, with more than 500 references. 4.3.1 System Description Before discussing diﬀerent ilc algorithms, as well as convergence and stability properties, it is of interest to introduce a description of the system. The batch formulations of the system dynamics and the ilc algorithm are very useful for the ilc concept. Especially, the stability and convergence properties are simple to show using the batch formulation. Design of ilc algorithms, such as the normoptimal ilc method, can also be performed using the batch formulation. Here, only the time domain description, including the batch formulation, is presented but frequency domain descriptions are sometimes used as well [Norrlöf and Gunnarsson, 2002]. A general system is shown in Figure 4.5 with reference r(t), ilc signal uk (t), process disturbance wk (t), and measurement disturbance vk (t) as inputs. There are two types of output signals from the system, zk (t) denotes the controlled variable and yk (t) the measured variable. All signals are at ilc iteration k and time t. The system is not restricted to open loop systems. It is possible to design an ilc algorithm for systems with an existing feedback loop. For example, this is important for industrial manipulators, since it is not desirable to remove the feedback loops which are stabilising the manipulator and have good disturbance rejection. Instead, the ilc signal should only catch the small variations in the error to improve the performance. The system is commonly described in discrete time by zk (t) = Sr (q)r(t) + Su (q)uk (t) + Sw (q)wk (t), yk (t) = zk (t) + Sv (q)vk (t), (4.17a) (4.17b) where Sr (q), Su (q), Sv (q), and Sw (q) are discrete-time transfer functions relating the input signals to the output signals. The assumption here is that the controlled signal is measured with noise. An extension to this assumption would be to have dynamical couplings between zk (t) and yk (t), which is a more realistic model. For the batch formulation, also known as lifted system representation, let ⎞ ⎞ ⎛ ⎛ ⎜⎜ r(0) ⎟⎟ ⎜⎜ uk (0) ⎟⎟ ⎟⎟ ⎟⎟ ⎜⎜ ⎜ .. .. ⎟⎟ ∈ RN nz , uk = ⎜⎜⎜ ⎟⎟ ∈ RN nu , r = ⎜⎜⎜ (4.18a) ⎟⎟ ⎟⎟ ⎜⎜ . . ⎜⎝ ⎠ ⎠ ⎝ r(N − 1) uk (N − 1) ⎞ ⎞ ⎛ ⎛ ⎜⎜ vk (0) ⎟⎟ ⎜⎜ wk (0) ⎟⎟ ⎟⎟⎟ ⎟⎟⎟ ⎜⎜ ⎜⎜ .. .. vk = ⎜⎜⎜ (4.18b) ⎟⎟ ∈ RN nv , wk = ⎜⎜⎜ ⎟⎟ ∈ RN nw , . . ⎟⎠ ⎟⎠ ⎜⎝ ⎜⎝ vk (N − 1) wk (N − 1) ⎞ ⎞ ⎛ ⎛ ⎜⎜ zk (0) ⎟⎟ ⎜⎜ yk (0) ⎟⎟ ⎟ ⎟⎟ ⎜⎜ ⎜ ⎟⎟ ⎜ .. .. ⎟⎟ ∈ RN ny , zk = ⎜⎜⎜ (4.18c) ⎟⎟ ∈ RN nz , yk = ⎜⎜⎜ ⎟⎟ . . ⎠⎟ ⎠ ⎝⎜ ⎝⎜ zk (N − 1) yk (N − 1) 4.3 49 Iterative Learning Control r(t) uk (t) vk (t) yk (t) S zk (t) wk (t) Figure 4.5: System S with reference r(t), ilc input uk (t), process disturbance wk (t), measurement disturbance vk (t), measured variable yk (t) and controlled variable zk (t) at ilc iteration k and time t. which are known as super-vectors. The system (4.17) can then be written as zk = Sr r + Su uk + Sw wk , (4.19a) y k = z k + Sv v k , (4.19b) where the matrices Sr , Su , Sv , and Sw are Toeplitz matrices formed from the impulse response coeﬃcients for the corresponding transfer functions. 4.3.2 ILC Algorithms A ﬁrst order ilc algorithm can be formulated as −1 N −1 uk+1 (t) = F {uk (i)}N , {e (i)} k i=0 i=0 , t = 0, . . . , N − 1, (4.20) where F ( · ) is a function that can be either linear or non-linear, and ek (t) = r(t) − yk (t) is the tracking error, where r(t) is the reference and yk (t) is the measured signal. The main objective of ilc is to determine the function F ( · ) such that the tracking error converges to zero, i.e., lim ek (t)2 = 0, k→∞ t = 0, . . . , N − 1, (4.21) in as few iterations as possible. Since information from previous iterations, at all time instances, is used, it is possible to use non-causal ﬁltering. In practice, it is not possible to make the error vanish completely for some systems. In literature, the reasons are mostly said to be due to disturbances, and if a model based ilc algorithm has been used, also model errors aﬀect the result. However, it can also be a fundamental problem with the ilc concept that the error cannot vanish completely. In Paper H the aspect of controllability, along the iterations, is considered and analysed. Convergence and stability of the ilc algorithm (4.20) are important properties to be able to guarantee that the sequence of signals uk (t), k = 0, 1, . . . converges to a bounded correction signal, giving as low control error as possible. Convergence and stability properties for the update of the correction signal are presented in Section 4.3.3. An update of the correction signal uk (t) according to (4.20) is known as a ﬁrst order ilc algorithm. Higher orders of ilc algorithms in the form −1 N −1 N −1 N −1 uk+1 (t) = F {uk (i)}N (4.22) i=0 , {uk−1 (i)}i=0 , . . . , {ek (i)}i=0 , {ek−1 (i)}i=0 , . . . , for t = 0, . . . , N − 1 can also be possible to use. See for example Bien and Huh 50 4 Control Theory [1989]; Moore and YangQuan [2002]; Gunnarsson and Norrlöf [2006]. A widely used ilc algorithm, here shown for siso systems, is uk+1 (t) = Q(q)(uk (t) + L(q)ek (t)), (4.23) where q is the time-shift operator, t is time and k is the ilc iteration index. The ﬁlters Q(q) and L(q) are linear, possible non-causal, ﬁlters. Letting Q(q) = 1 and taking L(q) as the inverse of the system, makes the error converge to zero in one iteration. Inverting a system can, if possible, be very complicated. Even for linear systems there exist diﬃculties. There are in principal three cases for linear systems where an inverse does not exist or it has undesirable properties. First, if the system has more poles than zeros, i.e., the system is strictly proper, then the inverse cannot be realised. Second, a non-minimum phase system results in an unstable inverse. Finally, systems with time delays are not possible to invert. Instead of inverting the system it is common to choose L(q) = γ q δ , where 0 < γ < 1 and δ a positive integer, are the design variables. The parameters γ and δ are chosen such that the stability criteria, see Section 4.3.3, is satisﬁed. Usually, δ is chosen as the time delay of the system. The ﬁlter Q(q) is introduced in order to restrict the high frequency inﬂuence from the error. However, including Q(q) makes the ilc algorithm converging slower and to a non zero error. It is usually enough to choose Q(q) as a simple low-pass ﬁlter of low order. The cut-oﬀ frequency is chosen such that the bandwidth of the ilc algorithm is large enough without letting through too much noise. In the design of the ﬁlters, there is a trade-oﬀ between convergence speed, error reduction and plant knowledge used in the design process. Using the ilc algorithm (4.23) together with the system (4.17), where the disturbances are omitted, gives the ilc system uk+1 (t) = Q(q) 1 − L(q)Su (q) uk (t) + Q(q)L(q) 1 − Sr (q) r(t). (4.24) The batch form of the ilc algorithm (4.23) is given by uk+1 = Q(uk + Lek ), where ⎛ ⎜⎜ ⎜⎜ ek = r − yk = ⎜⎜⎜ ⎝⎜ ek (0) .. . (4.25) ⎞ ⎟⎟ ⎟⎟ ⎟⎟ . ⎟⎟ ⎠ (4.26) ek (N − 1) The matrices Q and L in (4.25) can be formed from the impulse response coeﬃcients of the ﬁlters in (4.23), or be the design variables directly. Together with the system (4.19), where the disturbances are omitted, gives the ilc system in batch form as uk+1 = Q(I − LSu )uk + QL(I − Sr )r. (4.27) Other choices of ilc algorithms, such as the pd- and pi-type, among others, can be found in e.g. Arimoto [1990]; Moore [1993]; Bien and Xu [1998]; Bristow et al. 4.3 51 Iterative Learning Control [2006]. In this thesis, the norm-optimal ilc algorithm [Amann et al., 1996a,b; Gunnarsson and Norrlöf, 2001] is considered in Paper G. A short introduction to norm-optimal ilc is given below. Norm-optimal ILC In this section, the norm-optimal ilc algorithm [Amann et al., 1996a,b; Gunnarsson and Norrlöf, 2001] for the case yk (t) = zk (t), i.e., with the noise term in (4.17) omitted, will be explained. Norm-optimal ilc using an estimate of the controlled variable is considered in Paper G. The method solves minimise uk+1 ( · ) subject to N −1 1 ek+1 (t)2We + uk+1 (t)2Wu 2 1 2 t=0 N −1 (4.28) 2 uk+1 (t) − uk (t) ≤ δ, t=0 n n where ek+1 (t) = r(t) − zk+1 (t) is the error, We ∈ S++z , and Wu ∈ S++u are weight matrices for the error and the ilc control signal, respectively. Let the Lagrangian function [Nocedal and Wright, 2006] be deﬁned by L(uk+1 (t), λ) = N −1 1 ek+1 (t)2We + uk+1 (t)2Wu + λ uk+1 (t) − uk (t)2 − λδ, 2 t=0 where λ ∈ R+ is the Lagrange multiplier. The ﬁrst-order suﬃcient conditions for optimality are given by the Karush-Kuhn-Tucker (kkt) conditions [Nocedal and Wright, 2006]. The solution can now be found using the kkt conditions for a ﬁxed value of δ giving u∗k+1 (t) and λ∗ . However, there is no predetermined value of δ, hence δ will be a tuning parameter. The solution procedure can be simpliﬁed if it is assumed that λ is a tuning parameter instead of δ. The solution becomes simpler to obtain since there is no need to calculate an optimal value for λ. The kkt conditions implies now that the constraint in (4.28) will always be satisﬁed with equality and where the value of δ depends indirectly of the value of λ. Moreover, the value of δ will decrease when number of iterations increase. Finally, the optimum is obtained where the gradient of the Lagrangian function with respect to uk+1 (t) equals zero. The second-order suﬃcient condition for optimality is that the Hessian of the Lagrangian function with respect to uk+1 (t) is greater than zero. Using the super-vectors uk , zk , and r from (4.18) gives 1 T L= ek+1 W e ek+1 + uTk+1 W u uk+1 + λ(uk+1 − uk )T (uk+1 − uk ) , 2 where ek+1 = r − zk+1 and Nn (4.29) W e = IN ⊗ We ∈ S++ z , (4.30a) Nn S++ u . (4.30b) W u = I N ⊗ Wu ∈ Here, IN is the N × N identity matrix and ⊗ denotes the Kronecker product. 52 4 Control Theory In (4.29), the term including λδ is omitted since it does not depend on uk+1 . The objective function (4.29) using the batch model in (4.19), without the noise terms, and ek+1 = r − zk+1 becomes 1 L = uTk+1 STu W e Su + W u + λI uk+1 − ((I − Sr ) r)T W e Su + λuTk uk+1 , (4.31) 2 where all terms independent of uk+1 are omitted. As mentioned before, the minimum is obtained when the gradient of L equals zero. The second-order suﬃcient condition is fulﬁlled, since the Hessian matrix STu W e Su + W u + λI is positive deﬁnite when W e ∈ S++ , W u ∈ S++ , and λ ∈ R+ . By solving the gradient with respect to uk+1 equal to zero, and using (4.19) together with ek = r − zk to eliminate the terms involving r and x0 gives uk+1 =Q(uk + Lek ), Q L =(STu W e Su + W u + λI)−1 (λI + STu W e Su ), =(λI + STu W e Su )−1 STu W e , (4.32a) (4.32b) (4.32c) which is in the same form as (4.25). The matrices Q and L should not be confused with the ﬁlters Q(q) and L(q) in (4.23). 4.3.3 Convergence and Stability Properties An important property for an ilc algorithm is that (4.20) is stable, i.e., uk , given by (4.24) or (4.27), should converge to a bounded signal. The convergence and stability properties are, for simplicity, analysed using the batch formulation in (4.27) of the ilc system. The ilc system is in fact a discrete-time linear system with the iterations index as the time. It is therefore possible to use standard stability results from linear system theory, see e.g. Rugh [1996]. Two main results about stability and convergence from Norrlöf and Gunnarsson [2002] are stated in Theorems 4.1 and 4.2. The reader is referred to Norrlöf and Gunnarsson [2002] for the details and more results, e.g. for the frequency domain. Theorem 4.1 (Stability [Norrlöf and Gunnarsson, 2002, Corollary 3]). The system (4.27) is stable if and only if ρ(Q(I − LSu )) < 1. (4.33) Theorem 4.2 (Monotone convergence [Norrlöf and Gunnarsson, 2002, Theorem 9]). If the system (4.27) satisﬁes σ̄(Q(I − LSu )) < 1, (4.34) u∞ − uk 2 ≤ ζ k u∞ − u0 2 , (4.35) u∞ = (I − Q(I − LSu ))−1 QL(I − Sr )r. (4.36) then the system is stable and with 0 ≤ ζ < 1 and 4.3 Iterative Learning Control 53 Nothing can unfortunately be said about monotonicity for the control error. However, it is still possible to calculate what the stationary error becomes as stated in Corollary 4.1. Corollary 4.1. The stationary control error e∞ = r − y∞ is given by e∞ = I − Su (I − Q(I − LSu ))−1 QL (I − Sr )r. (4.37) Proof: The result follows from (4.36) and (4.19), without the noise terms. It should actually be the true systems S0u (q) and S0r (q) that are used in the ilc algorithm (4.24) and (4.27) instead of Su (q) and Sr (q) when convergence and stability are investigated. However, the models must of course be used for obvious reasons. Norm-optimal ILC For norm-optimal ilc the ilc system (4.27) is stable independent of the system used. This special result is given in Theorem 4.3. Theorem 4.3. (Stability and monotonic convergence for norm-optimal ilc): The ilc system (4.27) is stable and monotonically convergent for all system descriptions in (4.19) using Q and L from (4.32). Proof: From Theorem 4.2 it holds that the iterative system (4.27) is stable and converges monotonically if σ̄ (Q(I − LSu )) < 1. Using Q and L from (4.32) gives −1 σ̄ (Q(I − LSu )) = σ̄ STu W e Su + W u + λI λ <1 (4.38) independent of the system description Su since W e ∈ S++ , W u ∈ S++ , and λ ∈ R+ . From Theorem 4.2 and Corollary 4.1 it holds that the asymptotic control signal and error becomes −1 u∞ = STu W e Su + W u STu W e (I − Sr )r, (4.39) −1 e∞ = I − Su STu W e Su + W u STu W e (I − Sr )r. (4.40) If W u = 0 and Su invertible then −1 u∞ = STu W e Su STu W e (I − Sr )r = S−1 u (I − Sr )r, −1 e∞ = I − Su STu W e Su STu W e (I − Sr )r = 0. (4.41) (4.42) It means that the norm-optimal ilc algorithm coincide with inversion of the system in (4.19). The assumption about Su invertible is for most cases not applicable, e.g. the number of inputs and outputs to Su must be equal. If Su is not invertible but still W u = 0, then the norm-optimal ilc algorithm gives a weighted least square solution of the inverse of the system with the weighting matrix W e . However, in general W u 0 is used in order to handle model errors. Example 4.2 shows how the performance changes for diﬀerent values of the weight matrices. 54 4 Control Theory Example 4.2: Norm-optimal ilc The performance of the norm-optimal ilc algorithm for diﬀerent values of the tuning parameters is analysed on a ﬂexible joint model. The model in continuous a a T , is given by q̇m time, using the state vector x = qa q̇a qm ⎞ ⎛ ⎛ ⎞ 1 0 0 ⎟⎟ ⎜⎜ 0 ⎜⎜ 0 ⎟⎟ ⎜⎜ k d k d ⎟ ⎜ ⎟ ⎜⎜ 0 ⎟⎟⎟ −M ⎜⎜− ⎟⎟ Ma Ma ⎟ ⎜⎜ ⎟⎟ a x + (4.43) ẋ = ⎜⎜⎜ Ma ⎟ ⎜⎜⎜ 0 ⎟⎟⎟ τ, 0 0 1 ⎟⎟⎟ ⎜⎜⎜ 0 ⎜ ⎟ ⎟ ⎝ ⎠ k ⎝ k f +d ⎠ τ d − Mk −M Mm M M m m m m with k = 8, d = 0.0924, Ma = 0.0997, Mm = 0.0525, f = 1.7825 kτ = 0.61. A discrete-time model is obtained using zero order hold sampling with a sample time Ts = 0.01 s. The model is stabilised with a p-controller with gain 1, and the output is chosen as qa . No process noise and measurement noise are present. The reference signal is a step ﬁltered four times through an fir ﬁlter of length n = 100 with all coeﬃcients equal to 1/n. Five diﬀerent conﬁgurations of the norm-optimal ilc algorithm, shown in Table 4.1, are used with We = 104 for all ﬁve tests. The performance is evaluated using the relative reduction of the rmse in percent with respect to the error when no ilc signal is applied, i.e., % % & & ' ' N N 1 1 ρk = 100 ek (t)2 e0 (t)2 . (4.44) N N t=1 t=1 The relative reduction of the rmse is shown in Figure 4.6. It can be seen that the convergence speed depends on λ and that the absolute error depends on Wu . Table 4.1: Parameters for the norm-optimal ilc algorithm in Example 4.2. Test 1 2 3 4 5 Wu 0.1 0.01 1 0.1 0.1 λ 1 1 1 0.1 10 ρk [%] 102 Test 1 Test 3 Test 5 Test 2 Test 4 101 100 0 20 40 60 80 100 ilc iteration k Figure 4.6: Result for Example 4.2. Performance for norm-optimal ilc for diﬀerent settings of the parameters Wu and λ. 5 Concluding Remarks his chapter concludes the work in the thesis and gives a brief summary of the included publications in Part II. Possible directions for future work are also discussed. T 5.1 Summary New lightweight robot structures require more advanced controllers than before. Nowadays, the controllers are divided into a feedback controller and a feed-forward controller. The feedback controller usually consists of a single pid controller for each joint, and the feed-forward controller requires complex models of the robot structure. The more complex models, the more diﬃcult will it be to derive and succeed in implementing the controller in an industrial environment, since a high-index dae may have to be solved in real time. Instead, other control structures are needed. This thesis presents some ideas in this topic, based on sensor fusion methods for estimating the end-eﬀector position. The thesis also presents more advanced control methods to be used in the feedback loop. The idea is not to remove the feed-forward controller completely since it is needed for trajectory tracking. Instead, the intention is to improve the feedback controller such that less complex models may be used in the feed-forward controller. The estimation of the end-eﬀector position is performed by combining a triaxial accelerometer at the end-eﬀector and the motor angular positions. The estimation problem is formulated in a Bayesian setting, where the extended Kalman ﬁlter (ekf) and the particle ﬁlter (pf) have been used (Papers A and B). Experimental data for a two degrees-of-freedom (dof) manipulator has been used 55 56 5 Concluding Remarks to evaluate the estimation performance. Diﬀerent types of estimators are used, where both the estimation model and the ﬁlter diﬀer. The three observers with the best performance are a) an ekf using a non-linear dynamic model, b) a particle ﬁlter using a linear dynamic model, c) an ekf with a non-linear model, where the acceleration of the end-eﬀector is used as an input instead of a measurement. The performance of these three observers is very similar when considering the path error. The execution time for a) was just above the real-time limit, for c) just below the limit, and for b) in the order of hours. The time required for modelling and implementation is also diﬀerent for the three diﬀerent observers. For b), most of the time was spent on implementing the ﬁlter and get it to work, whereas most of the time for a) was spent on modelling the dynamics. The estimation methods in this thesis are general and can be extended to higher degrees of freedom robots and additional sensors, such as gyroscopes and camera systems. The main eﬀect is more complex state space descriptions, a more problematic ﬁlter tuning, and longer computation time. In order to have good performance it is essential to have good models and good knowledge of the measurement and process noise. The models are often given in continuous time and the ﬁlters operate in discrete time. The problem with discretisation of the continuous-time models has been investigated (Paper C). Moreover, a method to estimate the process noise covariance matrix has been proposed using the em algorithm (Paper D). A great advantage with the em method, compared to other methods that have been suggested in the literature, is that the true position of the end-eﬀector is not needed. Although most of the observers in this thesis, which have been implemented in Matlab, are not running in real-time it is possible to use the estimates in oﬀ-line methods such as iterative learning control (ilc), system identiﬁcation, and diagnosis. Clearly, the computation time can be decreased by optimising the Matlab code or by using another programming language, e.g. C++. The estimationbased ilc framework, in particular for the norm-optimal ilc algorithm, has been considered in the thesis (Paper G). The algorithm has been extended to incorporate the full probability density function of the estimated control quantity. The estimation-based ilc has also made it possible to directly extend the normoptimal ilc algorithm to non-linear systems using linearisation. Moreover, output controllability in the iteration domain, or target path controllability in the time domain, for systems that are controlled using ilc, are important to investigate (Paper H), to be able to draw conclusions about how the control error will converge. The direct use of the accelerometer measurements in the feedback loop has also been considered with H∞ methods (Paper E). It is shown that the performance can be signiﬁcantly increased using H∞ controllers without the extra accelerome- 5.2 Future Work 57 ter measurements. However, the manipulator is usually described by non-linear models, which makes it diﬃcult to achieve a robust controller in the whole range of operation. A method to handle the non-linear ﬂexibility is proposed, where robust stability is achieved for the whole range of operation, whereas robust performance is only ensured for a speciﬁc linearisation point (Paper F). Adding more sensors such as accelerometers at the end-eﬀector increases the performance even more, though the tuning of the controllers becomes more diﬃcult. 5.2 Future Work A natural continuation is to extend the estimation problem to cover the complete six dof robot. The sensor system could be extended with a gyroscope to get measurements of the rotation of the end-eﬀector and not only the translation. It may not be possible to achieve good estimation performance if only one inertial measurement unit (imu) is mounted at the end-eﬀector. Instead, several imus should be mounted on well-chosen positions of the manipulator. Positioning of the imus is a complicated problem in itself. The ideal solution would be to formulate an optimisation problem to ﬁnd the most informative positions of the imus to be used for state estimation. Another measurement to consider is the arm angular position, i.e., a measurement on the arm side of the gearbox. A sensitivity analysis should also be considered to be able to ﬁnd out how the observers behave. It is interesting to see if the parameters that are crucial for the performance can be adapted at the same time as the states are estimated. One way could be to use the em algorithm to estimate both the parameters and the states. The em algorithm is in its general form an oﬀ-line method, however online solutions exist for special model structures. It is also interesting to investigate the tuning of the noise covariance matrices in more details, for example, by having time varying matrices that increase when the path changes drastically. This can be done in several ways, e.g. ﬁnd out when the path changes using the measured data, or using the programmed path. Another solution could be to use the interacting multiple model (imm) ﬁlter. The H∞ -control methods should of course also be extended to multi-dof manipulators and experimental evaluations should be performed. Except for handling non-linearities it becomes even more diﬃcult for the user to choose the weights compared to the single joint system considered in this thesis. To manage this increased complexity for the user it can be interesting to formulate an optimisation problem giving, in some sense, optimal weights that are used in the H∞ -synthesis method. Bibliography abb Robotics. Product speciﬁcation – abb irb4600. Document ID: 3HAC028284001. Revision: N, 2013. abb Robotics. Company homepage. URL: http://www.abb.com, accessed February 2014. Hyo-Sung Ahn, YangQuan Chen, and Kevin L. Moore. Iterative learning control: Brief survey and categorization. IEEE Transactions on Systems, Man, and Cybernetics—Part C: Applications and Reviews, 37(6):1099–1121, November 2007. Notker Amann, David H. Owens, and Eric Rogers. Iterative learning control using optimal feedback and feedforward actions. International Journal of Control, 65(2):277–293, 1996a. Notker Amann, David H. Owens, and Eric Rogers. Iterative learning control for discrete-time systems with exponential rate of convergence. IEE Proceedings Control Theory and Applications, 143(2):217–224, March 1996b. Brian D. O. Anderson and John B. Moore. Optimal Filtering. Information and System Sciences Series. Prentice Hall Inc., Englewood Cliﬀs, NJ, USA, 1979. Tohid Ardeshiri, Mikael Norrlöf, Johan Löfberg, and Anders Hansson. Convex optimization approach for time-optimal path tracking of robots with speed dependent constraints. In Proceedings of the 18th IFAC World Congress, pages 14648–14653, Milano, Italy, August/September 2011. Suguru Arimoto. Learning control theory for robotic motion. International Journal of Adaptive Control and Signal Processing, 4(6):543–564, 1990. Suguru Arimoto. Iterative Learning Control: Analysis, Design, Integration and Application, chapter A Brief History of Iterative Learning Control, pages 3–7. Kluwer Academic Publishers, Boston, MA, USA, 1998. Suguru Arimoto, Sadao Kawamura, and Fumio Miyazaki. Bettering operation of robots by learning. Journal of Robotic Systems, 1(2):123–140, 1984a. 59 60 Bibliography Suguru Arimoto, Sadao Kawamura, and Fumio Miyazaki. Iterative learning control for robot systems. In Proceedings of the IEEE International Conference on Industrial Electronics, Control, and Instrumentation, Tokyo, Japan, October 1984b. M. Sanjeev Arulampalam, Simon Maskell, Neil Gordon, and Tim Clapp. A tutorial on particle ﬁlters for online nonlinear/non-Gaussian Bayesian tracking. IEEE Transactions on Signal Processing, 50(2):174–188, February 2002. Karl-Johan Åström and Carlos Canudas de Wit. Revisiting the LuGre friction model. IEEE Control Systems Magazine, 28(6):101–114, December 2008. Patrik Axelsson. A simulation study on the arm estimation of a joint ﬂexible 2 dof robot arm. Technical Report LiTH-ISY-R-2926, Department of Electrical Enginering, Linköping University, SE-581 83 Linköping, Sweden, December 2009. Patrik Axelsson. Simulation model of a 2 degrees of freedom industrial manipulator. Technical Report LiTH-ISY-R-3020, Department of Electrical Enginering, Linköping University, SE-581 83 Linköping, Sweden, June 2011a. Patrik Axelsson. On Sensor Fusion Applied to Industrial Manipulators. Linköping Studies in Science and Technology. Licentiate Thesis No. 1511, Linköping University, SE-581 83 Linköping, Sweden, December 2011b. Patrik Axelsson. Evaluation of six diﬀerent sensor fusion methods for an industrial robot using experimental data. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 126–132, Dubrovnik, Croatia, September 2012. Patrik Axelsson and Fredrik Gustafsson. Discrete-time solutions to the continuous-time diﬀerential Lyapunov equation with applications to Kalman ﬁltering. Submitted to IEEE Transactions on Automatic Control, 2012. Patrik Axelsson and Mikael Norrlöf. Method to estimate the position and orientation of a triaxial accelerometer mounted to an industrial manipulator. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 283–288, Dubrovnik, Croatia, September 2012. Patrik Axelsson, Mikael Norrlöf, Erik Wernholt, and Fredrik Gustafsson. Extended Kalman ﬁlter applied to industrial manipulators. In Proceedings of Reglermötet, Lund, Sweden, June 2010. Patrik Axelsson, Umut Orguner, Fredrik Gustafsson, and Mikael Norrlöf. ml estimation of process noise variance in dynamic systems. In Proceedings of the 18th IFAC World Congress, pages 5609–5614, Milano, Italy, August/September 2011. Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Tool position estimation of a ﬂexible industrial robot using recursive Bayesian methods. In Proceedings Bibliography 61 of the IEEE International Conference on Robotics and Automation, pages 5234– 5239, St. Paul, MN, USA, May 2012a. Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Bayesian state estimation of a ﬂexible industrial robot. Control Engineering Practice, 20(11):1220–1228, November 2012b. Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Estimation-based ILC using particle ﬁlter with application to industrial manipulators. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 1740–1745, Tokyo, Japan, November 2013. Patrik Axelsson, Daniel Axehill, Torkel Glad, and Mikael Norrlöf. Controllability aspects for iterative learning control. Submitted to International Journal of Control, 2014a. Patrik Axelsson, Anders Helmersson, and Mikael Norrlöf. H∞ -controller design methods applied to one joint of a ﬂexible industrial manipulator. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014b. Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Estimation-based normoptimal iterative learning control. Submitted to Systems & Control Letters, 2014c. Patrik Axelsson, Goele Pipeleers, Anders Helmersson, and Mikael Norrlöf. H∞ synthesis method for control of non-linear ﬂexible joint models. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014d. Luca Bascetta and Paolo Rocco. Revising the robust-control design for rigid robot manipulators. IEEE Transactions on Robotics, 26(1):180–187, February 2010. Zeungnam Bien and Kyung M. Huh. Higher-order iterative learning control algorithm. IEE Proceedings, Part D, Control Theory and Applications, 136(3): 105–112, May 1989. Zeungnam Bien and Jian-Xin Xu, editors. Iterative Learning Control: Analysis, Design, Integration and Application. Kluwer Academic Publishers, Boston, MA, USA, 1998. Mattias Björkman, Torgny Brogårdh, Sven Hanssen, Sven-Erik Lindström, Stig Moberg, and Mikael Norrlöf. A new concept for motion control of industrial robots. In Proceedings of the 17th IFAC World Congress, pages 15714–15715, Seoul, Korea, July 2008. Douglas A. Bristow, Marina Tharayil, and Andrew G. Alleyne. A survey of iterative learning control — A learning-based method for high-performance tracking control. IEEE Control Systems Magazine, 26(3):96–114, June 2006. Torgny Brogårdh. Present and future robot control development—an industrial perspective. Annual Reviews in Control, 31(1):69–79, 2007. 62 Bibliography André Carvalho Bittencourt and Patrik Axelsson. Modeling and experiment design for identiﬁcation of wear in a robot joint under load and temperature uncertainties based on friction data. IEEE/ASME Transactions on Mechatronics, 2013. DOI: 10.1109/TMECH.2013.2293001. André Carvalho Bittencourt and Svante Gunnarsson. Static friction in a robot joint – modeling and identiﬁcation of load and temperature eﬀects. Journal of Dynamic Systems, Measurement, and Control, 134(5), September 2012. André Carvalho Bittencourt, Erik Wernholt, Shiva Sander-Tavallaey, and Torgny Brogårdh. An extended friction model to capture load and temperature eﬀects in robot joints. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 6161–6167, Taipei, Taiwan, October 2010. André Carvalho Bittencourt, Patrik Axelsson, Ylva Jung, and Torgny Brogårdh. Modeling and identiﬁcation of wear in a robot joint under temperature uncertainties. In Proceedings of the 18th IFAC World Congress, pages 10293–10299, Milano, Italy, August/September 2011. Giuseppe Casalino and Giorgio Bartolini. A learning procedure for the control of movements of robotic manipulators. In Proceedings of the IASTED Symposium on Robotics and Automation, pages 108–111, New Orleans, LA, USA, November 1984. Wenjie Chen and Masayoshi Tomizuka. Direct joint space state estimation in robots with multiple elastic joints. IEEE/ASME Transactions on Mechatronics, 19(2):697–706, April 2014. doi: 10.1109/TMECH.2013.2255308. Wankyun Chung, Li-Chen Fu, and Su-Hau Hsu. Springer Handbook of Robotics, chapter Motion Control, pages 133–159. Springer-Verlag, Berlin, Heidelberg, Germany, 2008. John J. Craig. Adaptive control of manipulators through repeated trials. In Proceedings of the American Control Conference, pages 1566–1572, San Diego, CA, USA, June 1984. John J. Craig. Introduction to Robotics Mechanics and Control. Addison Wesley, Menlo Park, CA, USA, second edition, 1989. Crossbow Technology. Accelerometers, High Sensitivity, LF Series, CXL02LF3, January 2004. Available at http://www.xbow.com. Alessandro De Luca and Wayne Book. Springer Handbook of Robotics, chapter Robots with Flexible Elements, pages 287–319. Springer-Verlag, Berlin, Heidelberg, Germany, 2008. Alessandro De Luca and Leonardo Lanari. Robots with elastic joints are linearizable via dynamic feedback. In Proceedings of the 34th IEEE Conference on Decision and Control, pages 3895–3897, New Orleans, LA, USA, December 1995. Bibliography 63 Alessandro De Luca, Riccardo Farina, and Pasquale Lucibello. On the control of robots with visco-elastic joints. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 4297–4302, Barcelona, Spain, April 2005. Alessandro De Luca, Dierk Schröder, and Michael Thümmel. An accelerationbased state observer for robot manipulators with elastic joints. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 3817– 3823, Roma, Italy, April 2007. Arthur Dempster, Nan Laird, and Donald Rubin. Maximum likelihood from incomplete data via the em algorithm. Journal of the Royal Statistical Society, Series B, 39(1):1–38, 1977. Jaques Denavit and Richard S. Hartenberg. A kinematic notation for lower-pair mechanisms based on matrices. Journal of Applied Mechanics, 22:215–221, 1955. Arnaud Doucet, Simon Godsill, and Christophe Andrieu. On sequential Monte Carlo sampling methods for Bayesian ﬁltering. Statistics and Computing, 10 (3):197–208, 2000. Arnaud Doucet, Nando de Freitas, and Neil Gordon, editors. Sequential Monte Carlo Methods in Practice. Statistics for Engineering and Information Science. Springer, New York, NY, USA, 2001. Pierre Dupont, Vincent Hayward, Brian Armstrong, and Friedhelm Altpeter. Single state elastoplastic friction models. IEEE Transactions on Automatic Control, 47(5):787–792, May 2002. fanuc Robotics. Company homepage. URL: http://www.fanucrobotics. com, accessed February 2014. B. Feeny and F. C. Moon. Chaos in a forced dry-friction oscillator: Experiments and numerical modelling. Journal of Sound and Vibration, 170(3):303–323, 1994. Ronald A. Fisher. On an absolute criterion for ﬁtting frequency curves. Messenger of Mathematics, 41:155–160, 1912. Ronald A. Fisher. On the mathematical foundations of theoretical statistics. Philosophical Transactions of the Royal Society, Series A, 222:309–368, 1922. Gene F. Franklin, J. David Powell, and Abbas Emami-Naeini. Feedback Control of Dynamic Systems. Prentice Hall Inc., Upper Saddle River, NJ, USA, fourth edition, 2002. Stuart Gibson and Brett Ninness. Robust maximum-likelihood estimation of multivariable dynamic systems. Automatica, 41(10):1667–1682, October 2005. Keith Glover and Duncan McFarlane. Robust stabilization of normalized coprime 64 Bibliography factor plant descriptions with H∞ -bounded uncertainty. IEEE Transactions on Automatic Control, 34(8):821–830, August 1989. Herbert Goldstein, Charles Poole, and John Safko. Classical Mechanics. Addison Wesley, San Francisco, CA, USA, third edition, 2002. Neil J. Gordon, David J. Salmond, and Adrian F. M. Smith. Novel approach to nonlinear/non-Gaussian Bayesian state estimation. IEE Proceedings on Radar and Signal Processing, 140(2):107–113, April 1993. Svante Gunnarsson and Mikael Norrlöf. On the design of ilc algorithms using optimization. Automatica, 37(12):2011–2016, December 2001. Svante Gunnarsson and Mikael Norrlöf. On the disturbance properties of high order iterative learning control algorithms. Automatica, 42(11):2031–2034, November 2006. Fredrik Gustafsson. Statistical Sensor Fusion. Studentlitteratur, Lund, Sweden, ﬁrst edition, 2010. Robert Henriksson, Mikael Norrlöf, Stig Moberg, Erik Wernholt, and Thomas B. Schön. Experimental comparison of observers for tool position estimation of industrial robots. In Proceedings of the 48th IEEE Conference on Decision and Control, pages 8065–8070, Shanghai, China, December 2009. Mrdjan Jankovic. Observer based control for elastic joint robots. IEEE Transactions on Robotics and Automation, 11(4):618–623, August 1995. Rahim Jassemi-Zargani and Dan Necsulescu. Extended Kalman ﬁlter-based sensor fusion for operational space control of a robot arm. IEEE Transactions on Instrumentation and Measurement, 51(6):1279–1282, December 2002. Andrew H. Jazwinski. Stochastic Processes and Filtering Theory, volume 64. Academic Press, New York, NY, USA, 1970. Soo Jeon, Masayoshi Tomizuka, and Tetsuaki Katou. Kinematic Kalman ﬁlter (KKF) for robot end-eﬀector sensing. Journal of Dynamic Systems, Measurement, and Control, 131(2), March 2009. Simon J. Julier, Jeﬀrey K. Uhlmann, and Hugh F. Durrant-Whyte. A new approach for ﬁltering nonlinear systems. In Proceedings of the American Control Conference, volume 3, pages 1628–1632, Seattle, WA, USA, June 1995. Thomas Kailath, Ali H. Sayed, and Babak Hassibi. Linear Estimation. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, 2000. Rudolf E. Kalman. A new approach to linear ﬁltering and prediction problems. Transactions of the AMSE–Journal of Basic Engineering, 82(Series D):35–45, 1960. Bibliography 65 Rickard Karlsson and Mikael Norrlöf. Bayesian position estimation of an industrial robot using multiple sensors. In Proceedings of the IEEE Conference on Control Applications, pages 303–308, Taipei, Taiwan, September 2004. Rickard Karlsson and Mikael Norrlöf. Position estimation and modeling of a ﬂexible industrial robot. In Proceedings of the 16th IFAC World Congress, Prague, Czech Republic, July 2005. Hassan K. Khalil. Nonlinear Systems. Prentice Hall Inc., Upper Saddle River, NJ, USA, third edition, 2002. K. Kosuge, M. Umetsu, and K. Furuta. Robust linearization and control of robot arm using acceleration feedback. In Proceedings of the IEEE International Conference on Control and Applications, pages 161–165, Jerusalem, Israel, April 1989. Krzysztof Kozłowski. Modelling and Identiﬁcation in Robotics. Advances in Industrial Control. Springer, London, UK, 1998. kuka. Company homepage. URL: http://www.kuka-ag.de/en/, accessed February 2014. Jae Young Lee, Je Sung Yeon, and Jong Hyeon Park. Robust nonlinear control for ﬂexible joint robot manipulators. In Proceedings of the SICE Annual Conference, pages 440–445, Takamatsu, Japan, September 2007. Leica Geosystems. Company homepage. URL: http://metrology. leica-geosystems.com/en/index.htm, accessed February 2014. Vatchara Lertpiriyasuwat, Martin C. Berg, and Keith W. Buﬃnton. Extended Kalman ﬁltering applied to a two-axis robotic arm with ﬂexible links. The International Journal of Robotics Research, 19(3):254–270, March 2000. Y. F. Li and X. B. Chen. End-point sensing and state observation of a ﬂexible-link robot. IEEE/ASME Transactions on Mechatronics, 6(3):351–356, September 2001. Helmut Lütkepohl. Handbook of Matrices. John Wiley & Sons, Chichester, West Sussex, England, 1996. Duncan McFarlane and Keith Glover. A loop shaping design procedure using H∞ synthesis. IEEE Transactions on Automatic Control, 37(6):759–769, June 1992. Sujit Kumar Mitra and C. Radhakrishna Rao. Generalized Inverse of Matrices and its Applications. Wiley Series in Probability and Mathematical Statistics. John Wiley & Sons, 1971. Yoshihiko Miyasato. Nonlinear adaptive H∞ control of robotic manipulators under constraint. In Proceedings of the 17th IFAC World Congress, pages 4090– 4095, Seoul, Korea, July 2008. 66 Bibliography Yoshihiko Miyasato. Nonlinear adaptive H∞ control of constrained robotic manipulators with input nonlinearity. In Proceedings of the American Control Conference, pages 2000–2005, St. Louis, MO, USA, July 2009. Stig Moberg. Modeling and Control of Flexible Manipulators. Linköping Studies in Science and Technology. Dissertations No. 1349, Linköping University, SE581 83 Linköping, Sweden, December 2010. Stig Moberg and Sven Hanssen. Inverse dynamics of ﬂexible manipulators. In Proceedings of the Conference on Multibody Dynamics, Warsaw, Poland, June 2009. Stig Moberg, Jonas Öhr, and Svante Gunnarsson. A benchmark problem for robust feedback control of a ﬂexible manipulator. IEEE Transactions on Control Systems Technology, 17(6):1398–1405, November 2009. Stig Moberg, Erik Wernholt, Sven Hanssen, and Torgny Brogårdh. Modeling and parameter estimation of robot manipulators using extended ﬂexible joint models. Journal of Dynamic Systems, Measurement, and Control, 136(3), May 2014. DOI: doi:10.1115/1.4026300. Kevin L. Moore. Iterative Learning Control for Deterministic Systems. Advances in Industrial Control. Springer-Verlag, London, UK, 1993. Kevin L. Moore. Iterative learning control: An expository overview. Applied and Computational Control, Signals, and Circuits, 1(1):151–214, 1998. Kevin L. Moore and Chen YangQuan. On monotonic convergence of high order iterative learning update laws. In Proceedings of the 15th IFAC World Congress, pages 852–857, Barcelona, Spain, July 2002. Motoman. Company homepage. URL: http://www.motoman.eu/, accessed February 2014. Salvatore Nicosia and Patrizio Tomei. State observers for rigid and elastic joint robots. Robotics and Computer-Integrated Manufacturing, 9(2):113–120, 1992. Salvatore Nicosia, Patrizio Tomei, and Antonio Tornambé. A nonlinear observer for elastic robots. IEEE Journal of Robotics and Automation, 4(1):45–52, February 1988. Jorge Nocedal and Stephen J. Wright. Numerical Optimization. Springer Series in Operations Research. Springer, New York, NY, USA, second edition, 2006. Shimon Y. Nof, editor. Handbook of Industrial Robotics. John Wiley & Sons, Hoboken, NJ, USA, second edition, 1999. Mikael Norrlöf and Svante Gunnarsson. Time and frequency domain convergence properties in iterative learning control. International Journal of Control, 75(14):1114–1126, 2002. Bibliography 67 Goele Pipeleers and Jan Swevers. Matlab-software mixedHinfsyn, 2013. Available at http://set.kuleuven.be/optec/Software/mixedhinfsyn. Gerasimos G. Rigatos. Particle ﬁltering for state estimation in nonlinear industrial systems. IEEE Transactions on Instrumentation and Measurement, 58(11): 3885–3900, November 2009. Paolo Rocco. Stability of PID control for industrial robot arms. IEEE Transactions on Robotics and Automation, 12(4):606–614, August 1996. Wilson J. Rugh. Linear System Theory. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, second edition, 1996. Hansjörg G. Sage, Michel F. de Mathelin, Gabriel Abba, Jacques A. Gangloﬀ, and Eric Ostertag. Nonlinear optimization of robust H∞ controllers for industrial robot manipulators. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 2352–2357, Albuquerque, NM, USA, April 1997. Hansjörg G. Sage, Michel F. de Mathelin, and Eric Ostertag. Robust control of robot manipulators: A survey. International Journal of Control, 72(16):1498– 1522, 1999. Lorenzo Sciavicco and Bruno Siciliano. Modelling and Control of Robot Manipulators. Springer, London, UK, second edition, 2000. Bruno Siciliano and Oussama Khatib, editors. Springer Handbook of Robotics. Springer-Verlag, Berlin, Heidelberg, Germany, 2008. Sigurd Skogestad and Ian Postletwaite. Multivariable Feedback Control, Analysis and Design. John Wiley & Sons, Chichester, West Sussex, England, second edition, 2005. Y. D. Song, A. T. Alouani, and J. N. Anderson. Robust path tracking control of industrial robots: An H∞ approach. In Proceedings of the IEEE Conference on Control Applications, pages 25–30, Dayton, OH, USA, September 1992. Mark W. Spong. Modeling and control of elastic joint robots. Journal of Dynamic Systems, Measurement, and Control, 109:310–319, December 1987. Mark W. Spong, Seth Hutchinson, and Mathukumalli Vidyasagar. Robot Modeling and Control. John Wiley & Sons, Hoboken, NJ, USA, 2006. Wayne L. Stout and M. Edwin Sawan. Application of H-inﬁnity theory to robot manipulator control. In Proceedings of the IEEE Conference on Control Applications, pages 148–153, Dayton, OH, USA, September 1992. Tatiana Taveira, Adriano Siqueira, and Marco Terra. Adaptive nonlinear H∞ controllers applied to a free-ﬂoating space manipulator. In Proceedings of the IEEE Conference on Control Applications, pages 1476–1481, Munich, Germany, October 2006. 68 Bibliography Patrizio Tomei. An observer for ﬂexible joint robots. IEEE Transactions on Automatic Control, 35(6):739–743, June 1990. Patrizio Tomei. A simple PD controller for robots with elastic joints. IEEE Transactions on Automatic Control, 36(10):1208–1213, October 1991. Diederik Verscheure, Bram Demeulenaere, Jan Swevers, Joris De Schutter, and Moritz Diehl. Time-optimal path tracking for robots: A convex optimization approach. IEEE Transactions on Automatic Control, 54(10):2318–2327, October 2009. Niklas Wahlström, Patrik Axelsson, and Fredrik Gustafsson. Discretizing stochastic dynamical systems using Lyapunov equations. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014. Johanna Wallén, Mikael Norrlöf, and Svante Gunnarsson. Arm-side evaluation of ilc applied to a six-degrees-of-freedom industrial robot. In Proceedings of the 17th IFAC World Congress, pages 13450–13455, Seoul, Korea, July 2008. Johanna Wallén, Svante Gunnarsson, Robert Henriksson, Stig Moberg, and Mikael Norrlöf. ilc applied to a ﬂexible two-link robot model using sensorfusion-based estimates. In Proceedings of the 48th IEEE Conference on Decision and Control, pages 458–463, Shanghai, China, December 2009. Johanna Wallén, Isolde Dressler, Anders Robertsson, Mikael Norrlöf, and Svante Gunnarsson. Observer-based ilc applied to the Gantry-Tau parallel kinematic robot. In Proceedings of the 18th IFAC World Congress, pages 992–998, Milano, Italy, August/September 2011a. Johanna Wallén, Mikael Norrlöf, and Svante Gunnarsson. A framework for analysis of observer-based ilc. Asian Journal of Control, 13(1):3–14, January 2011b. Lars Westerlund. The Extended Arm of Man. A History of the Industrial Robot. Informationsförlaget, Stockholm, Sweden, 2000. W. L. Xu and J. D. Han. Joint acceleration feedback control for robots: Analysis, sensing and experiments. Robotics and Computer-Integrated Manufacturing, 16(5):307–320, October 2000. Je Sung Yeon and Jong Hyeon Park. Practical robust control for ﬂexible joint robot manipulators. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 3377–3382, Pasadena, CA, USA, May 2008. Jongguk Yim and Jong Hyeon Park. Nonlinear H∞ control of robotic manipulator. In Proceedings of the IEEE International Conference on Systems, Man and Cybernetics, pages 866–871, Tokyo, Japan, October 1999. Byron M. Yu, Krishna V. Shenoy, and Maneesh Sahani. Derivation of extended Kalman ﬁltering and smoothing equations. URL: http://www-npl. stanford.edu/~byronyu/papers/derive_eks.pdf, 19 October 2004. Bibliography 69 Keivan Zavari, Goele Pipeleers, and Jan Swevers. Multi-H∞ controller design and illustration on an overhead crane. In Proceedings of the IEEE Conference on Control Applications. Part of the IEEE Multi-Conference on Systems and Control, pages 670–674, Dubrovnik, Croatia, October 2012. Kemin Zhou, John C. Doyle, and Keith Glover. Robust and Optimal Control. Prentice Hall Inc., Upper Saddle River, NJ, USA, 1996. Part II Publications Paper A Bayesian State Estimation of a Flexible Industrial Robot Authors: Patrik Axelsson, Rickard Karlsson and Mikael Norrlöf Edited version of the paper: Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Bayesian state estimation of a ﬂexible industrial robot. Control Engineering Practice, 20(11):1220–1228, November 2012b. A Bayesian State Estimation of a Flexible Industrial Robot Patrik Axelsson∗ , Rickard Karlsson∗∗ and Mikael Norrlöf∗ ∗ Dept. ∗∗ Nira of Electrical Engineering, Linköping University, SE–581 83 Linköping, Sweden [email protected], [email protected] Dynamics Teknikringen 6 SE-583 30 Linköping, Sweden rickard.karlsson @niradynamics.se Abstract A sensor fusion method for state estimation of a ﬂexible industrial robot is developed. By measuring the acceleration at the end-eﬀector, the accuracy of the arm angular position, as well as the estimated position of the end-eﬀector are improved. The problem is formulated in a Bayesian estimation framework and two solutions are proposed; the extended Kalman ﬁlter and the particle ﬁlter. In a simulation study on a realistic ﬂexible industrial robot, the angular position performance is shown to be close to the fundamental Cramér-Rao lower bound. The technique is also veriﬁed in experiments on an abb robot, where the dynamic performance of the position for the end-eﬀector is signiﬁcantly improved. 1 Introduction Modern industrial robot control is usually based only on measurements from the motor angles of the manipulator. However, the ultimate goal is to move the tool according to a predeﬁned path. In Gunnarsson et al. [2001] a method for improving the absolute accuracy of a standard industrial manipulator is described, where improved accuracy is achieved through identiﬁcation of unknown or uncertain parameters in the robot system, and applying the iterative learning control (ilc) method [Arimoto et al., 1984; Moore, 1993], using additional sensors to measure the actual tool position. The aim of this paper is to evaluate Bayesian estimation techniques for sensor fusion and to improve the estimate of the tool position from measurements of the acceleration at the end-eﬀector. The improved accuracy at the end-eﬀector is needed in demanding applications such as laser cutting, where low cost sensors such as accelerometers are a feasible choice. Two Bayesian state estimation techniques, the extended Kalman ﬁlter (ekf) and the particle ﬁlter (pf), are applied to a standard industrial manipulator and the result is evaluated with respect to the tracking performance in terms of position 75 76 Paper A Bayesian State Estimation of a Flexible Industrial Robot zs ys × xs zb yb × xb Figure 1: The abb irb4600 robot with the accelerometer. The base coordinate system, (xb , yb , zb ), and the coordinate system for the sensor (accelerometer), (xs , ys , zs ), are also shown. accuracy of the tool. The main contribution in this paper compared to previous papers in the ﬁeld is the combination of: i) the evaluation of estimation results in relation to the Cramér-Rao lower bound (crlb); ii) the utilisation of motor angle measurement and accelerometer measurement in the ﬁlters; iii) the experimental evaluation on a commercial industrial robot, see Figure 1; iv) the extensive comparison of ekf and pf, and ﬁnally; v) the use of a manipulator model including a complete model of the manipulator’s ﬂexible modes. In addition, the utilisation of the calibration of the accelerometer sensor from Axelsson and Norrlöf [2012] and the proposal density for the pf using an ekf [Doucet et al., 2000; Gustafsson, 2010] are non standard. Traditionally, many non-linear Bayesian estimation problems are solved using the ekf [Anderson and Moore, 1979; Kailath et al., 2000]. When the dynamic models and measurements are highly non-linear and the measurement noise is not Gaussian, linearised methods may not always be a good approach. The pf [Gordon et al., 1993; Doucet et al., 2001] provides a general solution to many problems where linearisation and Gaussian approximations are intractable or would yield too low performance. Bayesian techniques have traditionally been applied in mobile robot applications, see e.g. Kwok et al. [2004]; Jensfelt [2001], and Doucet et al. [2001, Ch. 19]. In the industrial robotics research area one example is Jassemi-Zargani and Necsulescu [2002] where an ekf is used to improve the trajectory tracking for a rigid 2-degrees-of-freedom (dof) robot using arm angle measurements and tool acceleration measurements. The extension to several dof is presented in Quigley et al. [2010], where the ekf is used on a robot manipulator with seven dof and three accelerometers. A method for accelerometer calibration with respect to orientation is also presented. The idea of combining a vision sensor, accelerometers, 2 77 Bayesian Estimation and gyros when estimating the tool position is explored in Jeon et al. [2009] for a 2-dof manipulator, using a kinematic Kalman ﬁlter. Another way is to use the acceleration of the tool as an input instead of a measurement as described in De Luca et al. [2007], where it is assumed that the friction is neglected, the damping and spring are assumed linear. As a result, the estimation can be done using a linear time invariant observer with dynamics based upon pole placement. For ﬂexible link robots the Kalman ﬁlter has been investigated in Li and Chen [2001] for a single link, where the joint angle and the acceleration of the tool are used as measurements. Moreover, in Lertpiriyasuwat et al. [2000] the extended Kalman ﬁlter has been used for a two link manipulator using the joint angles and the tool position as measurements. In both cases, the manipulator is operating in a plane perpendicular to the gravity ﬁeld. Sensor fusion techniques using particle ﬁlters have so far been applied to very few industrial robotic applications [Rigatos, 2009; Karlsson and Norrlöf, 2004, 2005], and only using simulated data. The pf method is also motivated since it provides the possibility to design control laws and perform diagnosis in a much more advanced way, making use of the full posterior probability density function (pdf). The pf also enables incorporation of hard constraints on the system parameters, and it provides a benchmark for simpler solutions, such as given by the ekf. This paper extends the simulation studies introduced in Karlsson and Norrlöf [2004, 2005] with experimental results. A performance evaluation in a realistic simulation environment for both the ekf and the pf is presented and it is analysed using the Cramér-Rao lower bound (crlb) [Bergman, 1999; Kay, 1993]. In addition to Karlsson and Norrlöf [2004, 2005], experimental data, from a state of the art industrial robot, is used for evaluation of the proposed methods. A detailed description of the experimental setup is given and also modiﬁcations of the pf for experimental data are presented. The paper is organised as follows. In Section 2, the Bayesian theory estimation is introduced and the concept of the crlb is presented. The robot, estimation, and sensor models, are presented in Section 3. The performance of the ekf and of the pf are compared to the Cramér-Rao lower bound limit for simulated data in Section 4. In Section 5 the experimental setup and performance are presented. Finally, Section 6 contains conclusive remarks and future work. 2 Bayesian Estimation Consider the discrete state-space model xk+1 = f (xk , uk , wk ), yk = h(xk ) + vk , (1a) (1b) with state variables xk ∈ Rn , input signal uk and measurements y1:k = {yi }ki=1 , with known probability density functions (pdfs) for the process noise, pw (w), and measurement noise pv (v). The non-linear posterior prediction density p(xk+1 | y1:k ) and ﬁltering density p(xk |y1:k ) for the Bayesian inference [Jazwinski, 1970] 78 Paper A Bayesian State Estimation of a Flexible Industrial Robot are given by p(xk+1 |y1:k ) = p(xk+1 |xk )p(xk |y1:k ) dxk , (2a) Rn x p(xk |y1:k ) = p(yk |xk )p(xk |y1:k−1 ) . p(yk |y1:k−1 ) (2b) For the important special case of linear-Gaussian dynamics and linear-Gaussian observations, the Kalman ﬁlter [Kalman, 1960] provides the solution. For nonlinear and non-Gaussian systems, the pdf cannot, in general, be expressed with a ﬁnite number of parameters. Instead approximative methods are used. This is usually done in two ways; either by approximating the system or by approximating the posterior pdf, see for instance, Sorenson [1988]; Arulampalam et al. [2002]. Here, two diﬀerent approaches of solving the Bayesian equations are considered; extended Kalman ﬁlter (ekf) , and particle ﬁlter (pf). The ekf will solve the problem using a linearisation of the system and assuming Gaussian noise. The pf on the other hand will approximately solve the Bayesian equations by stochastic integration. Hence, no linearisation errors occur. The pf can also handle non-Gaussian noise models where the pdfs are known only up to a normalisation constant. Also, hard constraints on the state variables can easily be incorporated in the estimation problem. 2.1 The Extended Kalman Filter (EKF) For the special case of linear dynamics, linear measurements and additive Gaussian noise, the Bayesian recursions in (2) have an analytical solution given by the Kalman ﬁlter. For many non-linear problems, the noise assumptions and the non-linearity are such that a linearised solution will be a good approximation. This is the idea behind the ekf [Anderson and Moore, 1979; Kailath et al., 2000] where the model is linearised around the previous estimate. The time update and measurement update for the ekf are ⎧ ⎪ ⎪ ⎨x̂k|k−1 = f (x̂k−1|k−1 , uk−1 , 0), tu: ⎪ (3a) ⎪ ⎩Pk|k−1 = Fk−1 Pk−1|k−1 FTk−1 + Gk−1 Qk−1 GTk−1 , ⎧ −1 ⎪ ⎪ Kk = Pk|k−1 HTk Hk Pk|k−1 HTk + Rk , ⎪ ⎪ ⎪ ⎨ mu: ⎪ (3b) x̂k|k = x̂k|k−1 + Kk yk − h(x̂k|k−1 ) , ⎪ ⎪ ⎪ ⎪ ⎩Pk|k = (I − Kk Hk ) Pk|k−1 , where the linearised matrices are given as Fk−1 = ∇x f (x, uk−1 , 0)|x=x̂k−1|k−1 , (4a) Gk−1 = ∇w f (x, uk−1 , w)|x=x̂k−1|k−1 , (4b) Hk = ∇x h(x)|x=x̂k|k−1 . (4c) In (3), Pk|k and Pk|k−1 denote the covariance matrices for the estimation errors at 2 79 Bayesian Estimation time k given measurements up to time k and k − 1, and the noise covariances are given as Qk = Cov (wk ) , Rk = Cov (vk ) , (5) where the process noise and measurement noise are assumed zero mean processes. 2.2 The Particle Filter (PF) The pf from Doucet et al. [2001]; Gordon et al. [1993]; Ristic et al. [2004] provides an approximate solution to the discrete time Bayesian estimation problem formulated in (2), by updating an approximate description of the posterior ﬁltering density. Let xk denote the state of the observed system and y1:k = {yi }ki=1 be the set of observed measurements until present time. The pf approximates the den(i) sity p(xk |y1:k ) by a large set of N samples (particles), {xk }N i=1 , where each particle (i) has an assigned relative weight, wk , chosen such that all weights sum to unity. The location and weight of each particle reﬂect the value of the density in the region of the state space. The pf updates the particle location in the state space and the corresponding weights recursively with each new observed measurement. Using the samples (particles) and the corresponding weights, the Bayesian equations can be approximately solved. To avoid divergence, a resampling step is introduced [Gordon et al., 1993]. The pf is summarised in Algorithm 1, where (i) (i) the proposal distribution p prop (xk+1 |xk , yk+1 ) can be chosen arbitrary as long as it is possible to draw samples from it. The estimate for each time, k, is often chosen as the minimum mean square estimate, i.e., N (i) (i) x̂k|k = E [xk ] = xk p(xk |y1:k ) dxk ≈ wk x k , (6) Rn x i=1 but other choices, such as the ML-estimate, might be of interest. There exist theoretical limits [Doucet et al., 2001] that the approximated pdf converges to the true as the number of particles tends to inﬁnity. 2.3 Cramér-Rao Lower Bound When diﬀerent estimators are used, it is fundamental to know the best possible achievable performance. As mentioned previously, the pf will approach the true pdf asymptotically. In practice only approximations are possible since the number of particles are ﬁnite. For other estimators, such as the ekf, it is important to know how much the linearisation or model structure used, will aﬀect the performance. The Cramér-Rao lower bound (crlb) is such a characteristic for the second order moment [Kay, 1993; Cramér, 1946]. Here, only state-space models with additive Gaussian noise are considered. The theoretical posterior crlb for a general dynamic system was derived in Van Trees [1968]; Tichavský et al. [1998]; Bergman [1999]; Doucet et al. [2001]. Here a continuous-time system is consid- 80 Paper A Bayesian State Estimation of a Flexible Industrial Robot Algorithm 1 The Particle Filter (pf) (i) Generate N samples {x0 }N i=1 from p(x0 ). 2: Compute (i) (i) (i) (i) (i) p(yk |xk )p(xk |xk−1 ) wk = wk−1 (i) (i) p prop (xk |xk−1 , yk ) (j) (i) (i) and normalise, i.e., w̄k = wk / N j=1 wk , i = 1, . . . , N . 1: 3: (i ) [Optional]. Generate a new set {xk }N i=1 by resampling with replacement N (i) (i) (i ) (i) (i) times from {xk }N i=1 , with probability w̄k = Pr{xk = xk } and let wk = 1/N , i = 1, . . . , N . 4: Generate predictions from the proposal density (i) (i ) xk+1 ∼ p prop (xk+1 |xk , yk+1 ), i = 1, . . . , N . 5: Increase k and continue to step 2. ered. By ﬁrst linearising and then discretising the system, the fundamental limit ), can in practice be calculated as the stationary solution for every k, P̄ = P̄(xtrue k of the Riccati recursions in the ekf, where the linearisations are around the true state trajectory, xtrue . Note that the approximation is fairly accurate for the ink dustrial robot application due to a high sample rate and a small process noise. The predicted value of the stationary covariance for each time t, i.e., for each , is denoted P̄p and given as the solution to point in the state-space, xtrue k P̄p = F̄(P̄p − K̄H̄P̄p )F̄T + ḠQḠT . (7) where the linearised matrices F̄, Ḡ and H̄ are evaluated around the true trajectory, xtrue , and k K̄ = P̄p H̄T (H̄P̄p H̄T + R)−1 . (8) The crlb limit can now be calculated as P̄ = P̄p − K̄H̄P̄p , (9) for each point along the true state-trajectory. 3 Dynamic Models In this section a continuous-time 2-dof robot model is discussed. The model is simpliﬁed and transformed into discrete time, to be used by the ekf and pf. The measurements are in both cases angle measurements from the motors, with or without acceleration information from the end-eﬀector. 3 81 Dynamic Models qa2 zb qa1 zs ρb xb P xs Figure 2: A 2-dof robot model. The links are assumed to be rigid and the joints are described by a two mass system connected by a spring damping pair. 3.1 Robot Model The robot model used in this work is a joint ﬂexible two-axes model, see Figure 2. The model corresponds to axes two and three of a serial 6-dof industrial robot like the one in Figure 1. A common assumption of the dynamics of the robot is that the transmission can be approximated by two or three masses connected by springs and dampers. The coeﬃcients in the resulting model can be estimated from an identiﬁcation experiment, see for instance Kozłowski [1998]. Here, it will be assumed that the transmission can be modelled as a two mass system and that the links are rigid. The dynamic model can be described by a torque balance for the motors and the arms. A common way to obtain the dynamic model in industrial robotics is to use Lagrange’s equation as described in Sciavicco and Siciliano [2000]. The equation describing the torque balance for the motor becomes Mm q̈am = −Fm q̇am − K · (qam − qa ) − D · (q̇am − q̇a ) + τ am , (10) T 1 /η 2 where Mm is the motor inertia matrix, qam = qm the motor angles 1 q m /η2 T 1 2 on the arm side of the gear box, qa = qa qa the arm angles, ηi the gear ratio, Fm the viscous friction at the motor, K the spring constant and D the damping coeﬃcient. No couplings between motor one and two implies that Mm is a diagonal matrix. The parameters Fm , K, and D are two by two diagonal matrices, where the diagonal element ii corresponds to joint i. The inputs to the system T 1η 2 are the motor torques, τ am = τm 1 τm η1 . The corresponding relation for the arm becomes a non-linear equation Ma (qa )q̈a + C(qa , q̇a )q̇a + G(qa ) = K · (qam − qa ) + D · (q̇am − q̇a ), (11) where Ma ( · ) is the arm inertia matrix, C( · ) the Coriolis- and centrifugal terms and G( · ) the gravitational torque. Here, it is assumed that there are no couplings between the arms and motors, which is valid if the gear ratio is high [Spong, 1987]. A more detailed model of the robot should include non-linear friction 82 Paper A Bayesian State Estimation of a Flexible Industrial Robot such as Coulomb friction. An important extension would also be to model the non-linear spring characteristics in the gearboxes. In general, the gearbox is less stiﬀ for torques close to zero and more stiﬀ when high torques are applied. The extended ﬂexible joint model proposed in Moberg [2010, Paper A], which improves the control accuracy, can also be used. 3.2 Estimation Model The estimation model has to reﬂect the dynamics in the true system. A straight forward choice of estimation model is the state space equivalent of (10) and (11), which gives a non-linear dynamic model with eight states (motor and arm angular positions and velocities). This gives both a non-linear state space model and a non-linear measurement model. Instead, a linear state space model is suggested with arm angles, velocities and accelerations as state variables, in order to simplify the time update for the pf. Note that the measurement model is still non-linear in this case. Bias states compensating for measurement and model errors have shown to improve the accuracy and are therefore also included. The state vector is now given as T xk = qTa,k q̇Ta,k q̈Ta,k bTm,k bTρ̈,k , (12) 1 2 T qa,k contains the arm angles from joint two and three in where qa,k = qa,k Figure 1, q̇a,k is the angular velocity, q̈a,k is the angular acceleration, bm,k = 1 T 2 T 1 2 bρ̈,k bm,k bm,k contains the bias terms for the motor angles, and bρ̈,k = bρ̈,k contains the bias terms for the acceleration at time k. The bias states are used to handle model errors in the measurement equation but also to handle drifts in the measured signals, especially in the acceleration signals. The ﬁrst three states are given by a constant acceleration model discretised with zero order hold, and the bias states are modelled as random walk. This yields the following state space model in discrete time xk+1 = Fxk + Gu uk + Gw wk , yk = h(xk ) + vk , where ⎛ ⎜⎜ ⎜⎜ ⎜⎜ ⎜ F = ⎜⎜⎜⎜ ⎜⎜ ⎜⎜ ⎝ I 0 0 0 0 Ts I I 0 0 0 Ts2 /2I Ts I I 0 0 0 0 0 I 0 0 0 0 0 I ⎛ T3 ⎞ ⎜⎜ 6s I ⎟⎟ ⎜⎜ 2 ⎟⎟ ⎜⎜ Ts ⎟⎟ ⎜⎜ I ⎟⎟ ⎟⎟ , Gw = ⎜⎜⎜ T2 I ⎟⎟ ⎜⎜⎜ s ⎟⎟ ⎜⎜ 0 ⎟⎠ ⎜⎝ 0 0 0 0 I 0 ⎛ T3 ⎞ ⎜⎜ 6s I 0 ⎟⎟⎟ ⎜⎜ 2 ⎟⎟ ⎜⎜ Ts ⎜⎜ 2 I 0 ⎟⎟⎟⎟ ⎜ ⎟ 0 ⎟⎟⎟ , Gu = ⎜⎜⎜⎜ Ts I ⎜⎜ ⎟⎟ ⎜⎜ 0 0 ⎟⎟ ⎝ ⎠ I 0 (13a) (13b) ⎞ ⎟⎟ ⎟⎟ ⎟⎟ ⎟⎟ ⎟⎟ . ⎟⎟ ⎟⎟ ⎟⎟ ⎟⎠ (14) The input, uk , is the arm jerk reference, i.e., the diﬀerentiated arm angular acceleration reference. The process noise, wk and measurement noise vk are considered Gaussian with zero mean and covariances, Qk and Rk respectively. The sample time is denoted Ts and I and 0 are two by two identity and null matrices. The sensor model (13b) is described in full detail in the next section. 3 83 Dynamic Models 3.3 Sensor Model The available measurements are motor angular positions from resolvers and the acceleration of the end-eﬀector from the accelerometer. The sensor model is thus given by a q + bm,k h(xk ) = m,k , (15) ρ̈ k + bρ̈,k T T 1 2 /η1 qm,k /η2 are the motor angles and ρ̈ k = ρkx ρkz is where qam,k = qm,k the Cartesian acceleration vector in the accelerometer frame Oxs zs , see Figure 2. With the simpliﬁed model described in Section 3.1, the motor angles qm,k are computed from (11) according to qam,k = qa,k + K−1 · Ma (qa,k )q̈a,k + G(qa,k ) + C(qa,k , q̇a,k )q̇a,k − D · (q̇am,k − q̇a,k ) . (16) Here, the motor angular velocity q̇am can be seen as an input signal to the sensor model. The damping term D · (q̇am − q̇a ) is small compared to the other terms and is therefore neglected. The approach is similar to the one suggested in Gunnarsson and Norrlöf [2004], which uses the relation given by (11) in the case when the system is scalar and linear. The results presented here are more general, since a multi-variable nonlinear system is considered. The acceleration in frame Oxs zs , in Figure 2, measured by the accelerometer, can be expressed as (17) ρ̈ s,k = Rs/b (qa,k ) ρ̈ b,k (qa,k ) + gb , T where Rs/b (qa,k ) is the rotation matrix from O xb zb to O xs zs , gb = 0 g is the gravity vector and ρ̈ b,k (qa,k ) is the second time derivative of the vector ρ b,k (qa,k ), see Figure 2. The vector ρ b,k (qa,k ) is described by the forward kinematics [Sciavicco and Siciliano, 2000] which is a non-linear mapping from joint angles to Cartesian coordinates, i.e., acc x (18) ρ b,k (qa,k ) = acc = Υacc (qa,k ), z where xacc and zacc are the position of the accelerometer expressed in frame Oxb zb . Diﬀerentiation of ρ b,k twice, with respect to time, gives ⎞ ⎛ 2 ⎜⎜ ∂Jacc (qa,k ) (i) ⎟⎟⎟⎟ ⎜⎜ q̇a,k ⎟⎟ q̇a,k , (19) ρ̈ b,k (qa,k ) = Jacc (qa,k )q̈a,k + ⎜⎜ (i) ⎠ ⎝ ∂q i=1 where i.e., (i) qa,k a,k is the ith element of qa,k and Jacc (qa,k ) is the Jacobian of Υacc (qa,k ), Jacc (qa ) = ∇qa Υacc (qa ). (20) 84 Paper A Bayesian State Estimation of a Flexible Industrial Robot Both the position model (16) for the motors and the acceleration model (19) are now a function of the state variables qa,k , q̇a,k , and q̈a,k . Remark 1. If the non-linear dynamics (10) and (11), are used, see Section 3.1, the relation in (16) becomes linear since qam,k is a state variable. However, the relation in (19) becomes more complex since q̈a,k is no longer a state, but has to be computed using (11). 4 4.1 Analysis Simulation Model In order to perform Cramér-Rao lower bound (crlb) analysis, the true robot trajectory must be known. Hence, in practice this must be conducted in a simulation environment since not all state variables are available as measurements. In the sequel, the simulation model described in Karlsson and Norrlöf [2005] is used, where the crlb analysis is compared to Monte Carlo simulations of the ekf and pf. 4.2 Cramér-Rao Lower Bound Analysis of the Robot In Section 2.3, the posterior Cramér-Rao lower bound (crlb) was deﬁned for a general non-linear system with additive Gaussian noise. In this section the focus is on the crlb expression for the industrial robot presented in Section 3.1. Solving for the acceleration in (11) yields κ(qa , q̇a ) = q̈a = Ma (qa )−1 K · (qam − qa ) + D · (q̇am − q̇a ) − G(qa ) − C(qa , q̇a )q̇a . Here, the motor angular velocity, q̇m , is considered as an input signal, hence not T part of the state-vector, x(t) = qTa q̇Ta q̈Ta . The system can be written in state space form as ⎛ ⎞ ⎛ ⎞ q q̇a ⎜⎜ ⎟⎟ d ⎜⎜⎜⎜ a ⎟⎟⎟⎟ ⎜ ⎟⎟⎟ , q̈a ẋ(t) = (21a) ⎜⎜q̇a ⎟⎟ = f c (x(t)) = ⎜⎜⎜ ⎟⎠ ⎝ dt ⎝ ⎠ q̈a Λ(qa , q̇a , q̈a ) d (21b) Λ(qa , q̇a , q̈a ) = κ(qa , q̇q ). dt The diﬀerentiation of κ is performed symbolically, using the Matlab symbolic toolbox. According to Section 2.3 the crlb is deﬁned as the stationary Riccati . This is formulated for a solution of the ekf around the true trajectory, xtrue k discrete-time system. Hence, the continuous-time robot model from (21) must be discretised. This can be done by ﬁrst linearising the system and then discretising it [Gustafsson, 2010]. The diﬀerentiation is done numerically around the true trajectory, to avoid the very complex symbolic gradient, and the result becomes, ⎞ ⎛ 0 I 0 ⎟⎟ ⎜⎜ ⎜ ⎟⎟⎟ ⎜ c c 0 0 I ⎜ (22) A = ∇x f (x)|x=xtrue = ⎜⎜ ⎟. k ⎜⎝ ∂Λ(q,q̇,q̈) ∂Λ(q,q̇,q̈) ∂Λ(q,q̇,q̈) ⎟⎟⎠ ∂q ∂q̇ ∂q̈ 4 85 Analysis The desired discrete-time system matrix is now given as c F̄ = eA · Ts , (23) where Ts is the sample time. The crlb is presented in Figure 3. 4.3 Estimation Performance The performance of the ekf and of the pf are compared against the Cramér-Rao lower bound (crlb) calculated in Section 4.2, using simulated data. The model is implemented and simulated using the Robotics Toolbox [Corke, 1996] in Matlab Simulink. The robot is stabilised using a single pid-controller. The estimation model and sensor model will not use the bias states described in Section 3.2 because no model errors or drift are included in the simulation. This means that only the upper left corner of the matrices in (14) are used. The simulation study is based mainly around the ekf approach, since it is a fast method well suited for large Monte Carlo simulations. The pf is much slower, therefore, a smaller Monte Carlo study is performed. The Monte Carlo simulations use the following covariance matrices for the process and measurement noise −6 10 · I 0 −6 . (24) Q = 4 · 10 · I, R = 0 10−4 · I The measurement covariance is basically given by the motor angle and accelerometer uncertainty, and the process noise covariance is considered as a ﬁlter design parameter. The system is simulated around the nominal trajectory and produces diﬀerent independent noise realisations for the measurement noise in each simulation. The continuous-time Simulink model of the robot is sampled in 1 kHz. The data is then decimated to 100 Hz before any estimation method is applied. The estimation performance is evaluated using the root mean square error (rmse) which is deﬁned as ⎞1/2 ⎛ NMC ⎟ ⎜⎜ 1 (j) 2 ⎟ ⎟ ⎜⎜ true xk − x̂k 2 ⎟⎟⎟ , (25) rmse(k) = ⎜⎜ ⎠ ⎝ NMC j=1 is the true state vecwhere NMC is the number of Monte Carlo simulations, xtrue k (j) tor and x̂k is the estimated state vector in Monte Carlo simulation j. Here, the state vector is divided up into states corresponding to angular position, angular velocity, and angular acceleration, before (25) is used. EKF In Figure 3 the root mean square error (rmse) from 500 Monte Carlo simulations is compared to the crlb limit, both with and without acceleration measurements. The crlb is computed as the square root of the trace for the covariance matrix part corresponding to the angular states. As seen, the rmse is close the fundamental limit. The discrepancy is due to model errors, i.e., neglected damping term and the fact that the estimator uses a simpliﬁed system matrix consisting of integrators only. Also note that the accelerometer measurements reduce 86 Paper A Bayesian State Estimation of a Flexible Industrial Robot Table 1: The rmse for arm-side angular position (qa ), angular velocity (q̇a ) and angular acceleration (q̈a ), with and without the accelerometer, using 500 Monte Carlo simulations. Accelerometer No accelerometer rmse qa 1.25 · 10−5 2.18 · 10−5 −5 rmse q̇a 7.57 · 10 4.08 · 10−4 −3 rmse q̈a 1.23 · 10 3.91 · 10−3 the estimation uncertainty. The results in Figure 3 are of course for the chosen trajectory, but the acceleration values are not that large, so greater diﬀerences will occur for larger accelerations. The rmse, ignoring the initial transient is given in Table 1 for both angular position, velocity and acceleration. The improvements are substantial in angular position, but for control, the improvements in angular velocity and acceleration are important. (i) (i) PF The proposal density p prop (xk+1 |xk , yk+1 ) in Algorithm 1 is chosen as the (i) (i) conditional prior of the state vector, i.e., p(xk+1 |xk ), and resampling is selected every time, which gives (i) (i) wk = p(yk |xk ), i = 1, . . . , N . (26) The particle ﬁlter is rather slow compared to the ekf for this model structure. Hence, the given Matlab implementation of the system is not well suited for large Monte Carlo simulations. Instead, a small Monte Carlo study over a short part of the trajectory used for the ekf case is considered. The pf and the ekf are compared, and a small improvement in performance is noted. The result is given in Figure 4. One explanation for the similar results between the ekf and pf is that the non-linearities may not give a multi modal distribution, hence the point estimates are quite similar. The advantage with the pf is that it can utilise hard constraints on the state variables and it can also be used for control and diagnosis where the full posterior pdf is available. Even though the pf is slow, it gives more insight in the selection of simulation parameters than the ekf, where the ﬁlter performance is more dependent on the ratio between the process and measurement noise. 5 Experiments on an ABB IRB4600 Robot The experiments were performed on an abb irb4600 industrial robot, like the one seen in Figure 1. To illuminate the tracking capacity of the ﬁlters, the servo tuning of the robot was not optimal, which introduces more oscillations in the path. The accelerometer used in the experiments is the triaxial accelerometer CXL02LF3 from Crossbow Technology, which has a range of ±2 g, and a sensitivity of 1 V/g [Crossbow Technology, 2004]. In the next sections the experimental setup and results are given. 87 Experiments on an ABB IRB4600 Robot 50 rmse ekf (no acc) rmse ekf (acc) crlb (no acc) crlb (acc) rmse [μrad] 40 30 20 10 0 1 2 3 4 5 6 7 Time [s] Figure 3: Angular position rmse from 500 Monte Carlo simulations using the ekf with and without accelerometer sensor are compared to the crlb limit for every time, i.e., the square root of the trace of the angular position from the time-varying crlb covariance. 50 ekf pf crlb 40 rmse [μrad] 5 30 20 10 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 Time [s] Figure 4: ekf and pf angular position rmse with external accelerometer signal from 20 Monte Carlo simulations. 88 Paper A Bayesian State Estimation of a Flexible Industrial Robot 1.05 1 z [m] 0.95 0.9 0.85 0.8 0.75 1.1 1.15 1.2 1.25 1.3 1.35 1.4 x [m] Figure 5: The path start at the lower left corner and is counter-clockwise. A laser tracking system from Leica Geosystems has been used to measure the true tool position (solid). The estimated tool position using the ekf (dashed) and Υtcp (qam,k ) (dash-dot) are also shown. 5.1 Experimental Setup The orientation and position of the accelerometer were estimated using the method described in Axelsson and Norrlöf [2012]. All measured signals, i.e., acceleration, motor angles and arm angular acceleration reference, are synchronous and sampled with a rate of 2 kHz. The accelerometer measurements are ﬁltered with a low pass ﬁlter before any estimation method is applied to better reﬂect the tool movement. The path used in the evaluation is illustrated in Figure 5, and it is programmed such that only joints two and three are moved. Moreover, the wrist is conﬁgured such that the couplings to joint two and three are minimised. It is not possible to get measurements of the true state variables xtrue , as k is the case for the simulation, instead, the true trajectory of the end-eﬀector, more and ztcp precisely the tool centre point (tcp), xtcp k k , is used for evaluation. The true trajectory is measured using a laser tracking system from Leica Geosystems. The tracking system has an accuracy of 0.01 mm/m and a sample rate of 1 kHz [Leica Geosystems, 2008]. The measured tool position is however not synchronised with the other measured signals, i.e., a manual synchronisation is therefore needed, which can introduce small errors. Another source of error is the accuracy of the programmed tcp in the control system of the robot. The estimated data is therefore aligned with the measured position to avoid any static errors. The alignment is performed using a least square ﬁt between the estimated position and the measured position. 5 Experiments on an ABB IRB4600 Robot 5.2 89 Experimental Results The only measured quantity to compare the estimates with is the measured tool position, as was mentioned in Section 5.1. Therefore, the estimated arm angles are used to compute an estimate of the tcp using the kinematic relation, i.e., tcp x̂ (27) = Υtcp (q̂a,k ), ẑtcp where q̂a,k is the result from the ekf or the pf. Another simple way to estimate the tool position is to use the forward kinematic applied to the motor angles 1 , i.e., Υtcp (qam,k ). In the evaluation study the estimates from the ekf, pf, and Υtcp (qam,k ) are compared to measurements from the Leica system. When computing the 2norm of the rmse the ﬁrst 0.125 seconds are disregarded in order to evaluate the tracking performance only, and not include ﬁlter transients. In the evaluation of the experiment, the focus is on position error only since the Leica laser reference system measures position only. However, the estimation technique presented is general, so the velocity estimates will be improved as well, which is important for many control applications. In simulations this has been veriﬁed, see Section 4.3 and Table 1. Since the position is based on integrating the velocity model, this will in general be true when applied to experimental data as well. However, the current measurement system cannot be used to verify this. EKF Figure 5 shows that the estimated paths follow the true path. The performance of the estimates is better shown in Figures 6 and 7, where the four sides are magniﬁed. At ﬁrst, it can be noticed that Υtcp (qam,k ) cannot estimate the oscillations of the true path. This is not a surprise since the oscillations originates from the ﬂexibilities in the gear boxes which are not taken care of in this straightforward way to estimate the tcp. However, as seen the accelerometer based sensor fusion method performs very well. It can also be noticed that the ekf estimate goes somewhat past the corners before it changes direction. An explanation to this phenomena can be that the jerk reference is used as an input to the estimation model. The jerk reference does not coincide with the actual jerk as a result of model errors and control performance. The initial transient for the ekf, due to incorrect initialisation of the ﬁlter, rapidly approaches the true path. In this case Υtcp (qam,k ) starts near the true path, but Υtcp (qam,k ) can start further away for another path. The position rmse is presented in Figure 8, where the ekf with acceleration measurements shows a signiﬁcantly improve in the performance. The 2-norm of the rmse2 for the ekf is reduced by 25 % compared to Υtcp (qam,k ). This is based on the single experimental trajectory, but the result is in accordance with the simulation result and the theoretical calculations. Figure 8 also shows that the ekf converges fast. The Matlab implementation of the ekf is almost real-time, and without losing performance the measurements can be slightly decimated (to approximately 200 Hz), yielding faster than real-time calculations. 1 The motor angles are ﬁrst transformed to the arm side of the gear box via the gear ratio. 2 The rmse is computed without considering the ﬁrst 0.125 seconds where the ekf has a transient behavior. 90 Paper A Bayesian State Estimation of a Flexible Industrial Robot 1.006 z [m] 1.004 1.002 1 0.998 0.996 0.804 z [m] 0.802 0.8 0.798 0.796 0.794 1.15 1.2 1.25 1.3 1.35 x [m] Figure 6: The top side (upper diagram) and bottom side (lower diagram) of the square path in Figure 5 for the true tool position (solid) and tool position estimates using the ekf (dashed) and Υtcp (qam,k ) (dash-dot). 1 0.98 0.96 0.94 z [m] 0.92 0.9 0.88 0.86 0.84 0.82 0.8 1.146 1.15 x [m] 1.154 1.348 1.352 1.356 x [m] Figure 7: The left side (left diagram) and right side (right diagram) of the square path in Figure 5 for the true tool position (solid) and tool position estimates using the ekf (dashed) and Υtcp (qam,k ) (dash-dot). 5 91 Experiments on an ABB IRB4600 Robot 6 ekf Υtcp (qm ) 5 rmse [mm] 4 3 2 1 0 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time [s] Figure 8: Tool position rmse for the ekf (dashed) and Υtcp (qam,k ) (dash-dot). The 2-norm of the rmse-signals, without the ﬁrst 0.125 seconds, are 0.1246 and 0.1655 for the ekf and Υtcp (qam,k ), respectively. PF The proposal density used during the simulation did not work properly for the experimental data due to a high signal to noise ratio (snr) and also model errors. One could use an optimal proposal density [Doucet et al., 2000; Gustafsson, 2010] but the problem is that it is diﬃcult to sample from that. Instead, the proposal density is approximated using an ekf, [Doucet et al., 2000; Gustafsson, 2010] (i) (i) (i) (i) (i) (i) (28) p prop (xk |xk−1 , yk ) = N (f (xk−1 ) + Kk (yk − ŷk ), (Hk R†k Hk + Q†k−1 )† ), where † denotes the pseudo-inverse, and where the matrices are assumed to be evaluated for each particle state. The result of the pf compared to the ekf can be found in Figure 9 and Figure 10. The pf performs better in the corners, i.e., the estimated path does not go past the corners before it changes. The motive that the pf can handle the problem with the jerk input better than the ekf can be that the particle cloud covers a larger area of the state space. The pf estimate is also closer to the true path, at least at the vertical sides. Figure 11 shows the rmse for the pf which is below the rmse for the ekf most of the time. The resulting 2-norm of the rmse for the pf is 0.0818, which is approximately 66 % of the ekf and 49 % of Υtcp (qam,k ). Note that the transients are not included, i.e., the ﬁrst 0.125 seconds are removed. The pf converges much faster than the ekf as can be seen clearly in Figure 11. The pf in the proposed implementation is far from real-time and the bias states are needed to control the model errors. 92 Paper A Bayesian State Estimation of a Flexible Industrial Robot 1.006 z [m] 1.004 1.002 1 0.998 0.996 0.804 z [m] 0.802 0.8 0.798 0.796 0.794 1.15 1.2 1.25 1.3 1.35 x [m] Figure 9: The top side (upper diagram) and bottom side (lower diagram) of the square path in Figure 5 for the true tool position (solid) and tool position estimates using the ekf (dashed) and the pf (dash-dot). 1 0.98 0.96 0.94 z [m] 0.92 0.9 0.88 0.86 0.84 0.82 0.8 1.146 1.15 x [m] 1.154 1.348 1.352 1.356 x [m] Figure 10: The left side (left diagram) and right side (right diagram) of the square path in Figure 5 for the true tool position (solid) and tool position estimates using the ekf (dashed) and the pf (dash-dot). 6 93 Conclusions and Future Work 6 ekf pf 5 rmse [mm] 4 3 2 1 0 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time [s] Figure 11: Tool position rmse for the ekf (dashed) and the pf (dash-dot). The 2-norm of the rmse-signals, without the ﬁrst 0.125 seconds, are 0.1246 and 0.0818 for the ekf and the pf, respectively. 6 Conclusions and Future Work A sensor fusion approach to ﬁnd estimates of the tool position, velocity, and acceleration by combining a triaxial accelerometer at the end-eﬀector and the measurements from the motor angles of an industrial robot is presented. The estimation is formulated as a Bayesian problem and two solutions are proposed; the extended Kalman ﬁlter and the particle ﬁlter. The algorithms were tested on simulated data from a realistic robot model as well as on experimental data. Suﬃciently accurate estimates are produced for simulated data, where the performance both with and without accelerometer measurements are close to the fundamental Cramér-Rao lower bound limit in Monte Carlo simulations. The dynamic performance for experimental data is also signiﬁcantly better using the accelerometer method. The velocity estimates are also proven to be much more accurate when the ﬁlter uses information from the accelerometer. This is important for control design in order to give a well damped response at the robot arm. Since the intended use of the estimates is to improve position control using an oﬀ-line method, like iterative learning control, there are no real-time issues using the computational demanding particle ﬁlter algorithm, however the extended Kalman ﬁlter runs in real-time in Matlab. The estimation methods presented in this paper are general and can be extended to higher degrees of freedom robots and additional sensors, such as gyros and camera systems. The main eﬀect is 94 Paper A Bayesian State Estimation of a Flexible Industrial Robot a larger state space model giving more time-consuming calculations and also a more complex measurement equation. The most time-consuming step in the ekf is the matrix multiplications Fk Pk|k FTk . The two matrix multiplications require in total 4n3 ﬂops3 . For example, going from two to six dof increases the computational cost with a factor of 27. For the pf it is not as easy to give a description of the increased computational complexity. Acknowledgement This work was supported by the Vinnova Excellence Center LINK-SIC and the SSF project Collaborative Localization. 3 A ﬂop is one of the elementary scalar operations +, −, ∗, /. Bibliography 95 Bibliography Brian D. O. Anderson and John B. Moore. Optimal Filtering. Information and System Sciences Series. Prentice Hall Inc., Englewood Cliﬀs, NJ, USA, 1979. Suguru Arimoto, Sadao Kawamura, and Fumio Miyazaki. Bettering operation of robots by learning. Journal of Robotic Systems, 1(2):123–140, 1984. M. Sanjeev Arulampalam, Simon Maskell, Neil Gordon, and Tim Clapp. A tutorial on particle ﬁlters for online nonlinear/non-Gaussian Bayesian tracking. IEEE Transactions on Signal Processing, 50(2):174–188, February 2002. Patrik Axelsson and Mikael Norrlöf. Method to estimate the position and orientation of a triaxial accelerometer mounted to an industrial manipulator. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 283–288, Dubrovnik, Croatia, September 2012. Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Bayesian state estimation of a ﬂexible industrial robot. Control Engineering Practice, 20(11):1220–1228, November 2012. Niclas Bergman. Recursive Bayesian Estimation: Navigation and Tracking Applications. Linköping Studies in Science and Technology. Dissertations No. 579, Linköping University, SE-581 83 Linköping, Sweden, May 1999. http://www.control.isy.liu.se/publications/. Peter I. Corke. A robotics toolbox for Matlab. IEEE Robotics and Automation Magazine, 3(1):24–32, March 1996. Harald Cramér. Mathematical Methods of Statistics. Princeton University Press, 1946. Crossbow Technology. Accelerometers, High Sensitivity, LF Series, CXL02LF3, January 2004. Available at http://www.xbow.com. Alessandro De Luca, Dierk Schröder, and Michael Thümmel. An accelerationbased state observer for robot manipulators with elastic joints. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 3817– 3823, Roma, Italy, April 2007. Arnaud Doucet, Simon Godsill, and Christophe Andrieu. On sequential Monte Carlo sampling methods for Bayesian ﬁltering. Statistics and Computing, 10 (3):197–208, 2000. Arnaud Doucet, Nando de Freitas, and Neil Gordon, editors. Sequential Monte Carlo Methods in Practice. Statistics for Engineering and Information Science. Springer, New York, NY, USA, 2001. Neil J. Gordon, David J. Salmond, and Adrian F. M. Smith. Novel approach to nonlinear/non-Gaussian Bayesian state estimation. IEE Proceedings on Radar and Signal Processing, 140(2):107–113, April 1993. 96 Paper A Bayesian State Estimation of a Flexible Industrial Robot Svante Gunnarsson and Mikael Norrlöf. Iterative learning control of a ﬂexible robot arm using accelerometers. In Proceedings of the IEEE Conference on Control Applications, pages 1012–1016, Taipei, Taiwan, September 2004. Svante Gunnarsson, Mikael Norrlöf, Geir Hovland, Ulf Carlsson, Torgny Brogårdh, Tommy Svensson, and Stig Moberg. Pathcorrection for an industrial robot. European Patent Application No. EP1274546, April 2001. Fredrik Gustafsson. Statistical Sensor Fusion. Studentlitteratur, Lund, Sweden, ﬁrst edition, 2010. Rahim Jassemi-Zargani and Dan Necsulescu. Extended Kalman ﬁlter-based sensor fusion for operational space control of a robot arm. IEEE Transactions on Instrumentation and Measurement, 51(6):1279–1282, December 2002. Andrew H. Jazwinski. Stochastic Processes and Filtering Theory, volume 64. Academic Press, New York, NY, USA, 1970. Patric Jensfelt. Approaches to Mobile Robot Localization in Indoor Environments. PhD thesis, Royal Institute of Technology, Sweden, 2001. ISBN 91-7283-135-9. Soo Jeon, Masayoshi Tomizuka, and Tetsuaki Katou. Kinematic Kalman ﬁlter (KKF) for robot end-eﬀector sensing. Journal of Dynamic Systems, Measurement, and Control, 131(2), March 2009. Thomas Kailath, Ali H. Sayed, and Babak Hassibi. Linear Estimation. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, 2000. Rudolf E. Kalman. A new approach to linear ﬁltering and prediction problems. Transactions of the AMSE–Journal of Basic Engineering, 82(Series D):35–45, 1960. Rickard Karlsson and Mikael Norrlöf. Bayesian position estimation of an industrial robot using multiple sensors. In Proceedings of the IEEE Conference on Control Applications, pages 303–308, Taipei, Taiwan, September 2004. Rickard Karlsson and Mikael Norrlöf. Position estimation and modeling of a ﬂexible industrial robot. In Proceedings of the 16th IFAC World Congress, Prague, Czech Republic, July 2005. Steven M. Kay. Fundamentals of Statistical Signal Processing: Estimation Theory. Signal Processing Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, 1993. Krzysztof Kozłowski. Modelling and Identiﬁcation in Robotics. Advances in Industrial Control. Springer, London, UK, 1998. Cody Kwok, Dieter Fox, and Marina Meilă. Real-time particle ﬁlters. Proceedings of the IEEE, 92(3):469–484, March 2004. Leica Geosystems. Case study abb robotics - Västerås, 2008. Available at http: //metrology.leica-geosystems.com/en/index.htm. Bibliography 97 Vatchara Lertpiriyasuwat, Martin C. Berg, and Keith W. Buﬃnton. Extended Kalman ﬁltering applied to a two-axis robotic arm with ﬂexible links. The International Journal of Robotics Research, 19(3):254–270, March 2000. Y. F. Li and X. B. Chen. End-point sensing and state observation of a ﬂexible-link robot. IEEE/ASME Transactions on Mechatronics, 6(3):351–356, September 2001. Stig Moberg. Modeling and Control of Flexible Manipulators. Linköping Studies in Science and Technology. Dissertations No. 1349, Linköping University, SE581 83 Linköping, Sweden, December 2010. Kevin L. Moore. Iterative Learning Control for Deterministic Systems. Advances in Industrial Control. Springer-Verlag, London, UK, 1993. Morgan Quigley, Reuben Brewer, Sai P. Soundararaj, Vijay Pradeep, Quoc Le, and Andrew Y. Ng. Low-cost accelerometers for robotic manipulator perception. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 6168–6174, Taipei, Taiwan, October 2010. Gerasimos G. Rigatos. Particle ﬁltering for state estimation in nonlinear industrial systems. IEEE Transactions on Instrumentation and Measurement, 58(11): 3885–3900, November 2009. Branko Ristic, Sanjeev Arulampalam, and Neil Gordon. Beyond the Kalman Filter: Particle Filters for Tracking Applications. Artech House, Norwood, MA, USA, 2004. Lorenzo Sciavicco and Bruno Siciliano. Modelling and Control of Robot Manipulators. Springer, London, UK, second edition, 2000. Harold W. Sorenson. Bayesian Analysis of Time Series and Dynamic Models, chapter Recursive Estimation for Nonlinear Dynamic Systems, pages 126–165. Marcel Dekker Inc., 1988. Mark W. Spong. Modeling and control of elastic joint robots. Journal of Dynamic Systems, Measurement, and Control, 109:310–319, December 1987. Petr Tichavský, Carlos H. Muravchik, and Arye Nehorai. Posterior Cramér-Rao bounds for discrete-time nonlinear ﬁltering. IEEE Transactions on Signal Processing, 46(5):1386–1396, May 1998. Harry L. Van Trees. Detection, Estimation and Modulation Theory, Part 1. John Wiley & Sons, Hoboken, NJ, USA, 1968. Paper B Evaluation of Six Different Sensor Fusion Methods for an Industrial Robot using Experimental Data Authors: Patrik Axelsson Edited version of the paper: Patrik Axelsson. Evaluation of six diﬀerent sensor fusion methods for an industrial robot using experimental data. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 126–132, Dubrovnik, Croatia, September 2012. B Evaluation of Six Diﬀerent Sensor Fusion Methods for an Industrial Robot using Experimental Data Patrik Axelsson Dept. of Electrical Engineering, Linköping University, SE–581 83 Linköping, Sweden [email protected] Abstract Experimental evaluations for path estimation are performed on an abb irb4600 robot. Diﬀerent observers using Bayesian techniques with diﬀerent estimation models are proposed. The estimated paths are compared to the true path measured by a laser tracking system. There is no signiﬁcant diﬀerence in performance between the six observers. Instead, execution time, model complexities and implementation issues have to be considered when choosing the method. 1 Introduction The ﬁrst industrial robots were big and heavy with rigid links and joints. The development of new robot models has been focused on increasing the performance along with cost reduction, safety improvement and introduction of new functionalities as described in Brogårdh [2007]. One way to reduce the cost is to lower the weight of the robot which conduces to lower mechanical stiﬀness in the links. Also, the components of the robot are changed such that the cost is reduced, which can infer larger individual variations and unwanted non-linearities. The most crucial component, when it comes to ﬂexibilities, is the gearbox. The gearbox has changed more and more to a ﬂexible component, where the ﬂexibilities have to be described by non-linear relations in the models in order to have good control performance. There is therefore a demand of new approaches for motion control where less accurate models can be suﬃcient. One solution can be to estimate the position and orientation of the end-eﬀector along the path and then use the estimated position and orientation in the feedback loop of the motion controller. The most simple observer is to use the measured motor angular positions in the forward kinematic model to get the position and orientation of the end-eﬀector. The performance is insuﬃcient and the reason is that the oscillations on the arm side do not inﬂuence the motor side of the gearbox that much 101 102 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods due to the ﬂexibilities. The ﬂexibilities can also distort the oscillations of the arm side. The observer can consequently not track the true position and another observer is therefore needed. The observer requires a dynamic model of the robot in order to capture the oscillations on the arm side of the gearbox as well as more measurements than only the motor angular positions. One way to obtain information about the oscillations on the arm side can be to attach an accelerometer on the robot, e.g. at the end-eﬀector. A natural question is, how to estimate the arm angular positions from the measured acceleration as well as the measured motor angular positions. A common solution for this kind of problems is to apply sensor fusion methods for state estimation. The acceleration of the end-eﬀector as well as the measured motor angular positions can be used as measurements in e.g. an extended Kalman ﬁlter (ekf) or particle ﬁlter (pf). In Karlsson and Norrlöf [2004, 2005], and Rigatos [2009] the ekf and pf are evaluated on a ﬂexible joint model using simulated data only. The estimates from the ekf and pf are also compared with the theoretical Cramér-Rao lower bound in Karlsson and Norrlöf [2005] to see how good the ﬁlters are. An evaluation of the ekf using experimental data is presented in Henriksson et al. [2009], and in Jassemi-Zargani and Necsulescu [2002] with diﬀerent types of estimation models. A method using the measured acceleration of the end-eﬀector as input instead of using it as measurements is described in De Luca et al. [2007]. The observer, in this case, is a linear dynamic observer using pole placement, which has been evaluated on experimental data. 2 State Estimation The estimation problem for the discrete time non-linear state space model xk+1 = f (xk , uk ) + g(xk )wk , yk = h(xk , uk ) + vk , (1a) (1b) at time k given the measurements yk ∈ Rny is to ﬁnd the state vector xk ∈ k = 1, . . . , N . The estimation problem can be seen as calculation/approximation of the posterior density p(xk |y1:l ) using all measurements up to time l, where y1:l = {y1 , . . . , yl }. There are two types of problems, ﬁltering and smoothing. Filtering uses only measurements up to present time and smoothing uses future measurements also, i.e., l = k for ﬁltering and l > k for smoothing. Using Bayes’ law, and the Markov property for the state space model, repeatedly, the optimal solution for the Bayesian inference can be obtained. See Jazwinski [1970] for details. The solution to the Bayesian inference can in most cases not be given by an analytical expression. For the special case of linear dynamics, linear measurements and additive Gaussian noise the Bayesian recursions have an analytical solution, which is known as the Kalman ﬁlter (kf). Approximative methods must be used for non-linear and non-Gaussian systems. Here three approximative solutions are considered; the extended Kalman ﬁlter (ekf), the extended Kalman smoother (eks) and the particle ﬁlter (pf). Rnx 2 103 State Estimation EKF The ekf [Anderson and Moore, 1979] solves the Bayesian recursions using a ﬁrst order Taylor expansion of the non-linear system equations around the previous estimate. The process noise wk and measurement noise vk are assumed to be Gaussian with zero mean and covariance matrices Qk and Rk , respectively. The time and measurement updates are ⎧ ⎪ ⎪ ⎨x̂k|k−1 = f (x̂k−1|k−1 , uk−1 ), tu: ⎪ (2a) ⎪ ⎩Pk|k−1 = Fk−1 Pk−1|k−1 FTk−1 + Gk−1 Qk−1 GTk−1 , ⎧ −1 ⎪ ⎪ Kk = Pk|k−1 HTk Hk Pk|k−1 HTk + Rk , ⎪ ⎪ ⎪ ⎨ mu: ⎪ (2b) x̂k|k = x̂k|k−1 + Kk yk − h(x̂k|k−1 , uk ) , ⎪ ⎪ ⎪ ⎪ ⎩Pk|k = (I − Kk Hk ) Pk|k−1 , where Fk−1 = ∂f (x, uk−1 ) , ∂x x=x̂k−1|k−1 Gk−1 = g(x̂k−1|k−1 ), Hk = ∂h(x, uk ) . (3) ∂x x=x̂k|k−1 The notation x̂k|k , Pk|k , x̂k|k−1 and Pk|k−1 means estimates of the state vector x and covariance matrix P at time k using measurements up to time k and k − 1, respectively. EKS The eks [Yu et al., 2004] solves the Bayesian recursions in the same way as the ekf. The diﬀerence is that future measurements are available. First, the ekf equations are used forward in time, then the backward time recursion s x̂sk|N =x̂k|k + Pk|k FTk P−1 (4a) k+1|k x̂k+1|N − x̂k+1|k , s −1 (4b) Psk|N =Pk|k + Pk|k FTk P−1 k+1|k Pk+1|N − Pk+1|k Pk+1|k Fk Pk|k , is used, where Fk is given above. PF The pf [Doucet et al., 2001; Gordon et al., 1993] solves the Bayesian recursions using stochastic integration. The pf approximates the posterior density (i) p(xk |y1:k ) by a large set of N particles {xk }N i=1 , where each particle has an as (i) (i) signed relative weight wk , chosen such that N i=1 wk = 1. The pf updates the particle location and the corresponding weights recursively with each new observed measurement. Theoretical results show that the approximated posterior density converges to the true density when the number of particles tends to inﬁnity, see e.g. Doucet et al. [2001]. The pf is summarised in Algorithm 1, where (i) (i) the proposal density p prop (xk+1 |xk , yk+1 ) can be chosen arbitrary as long as it is possible to draw samples from it. In this work the optimal proposal density, approximated by an ekf, is used. See Doucet et al. [2000], and Gustafsson [2010] for details. The state estimate for each sample k is often chosen as the minimum mean square estimate N (i) (i) xk p(xk |y1:k ) dxk ≈ wk x k . (5) x̂k|k = E [xk ] = Rn x i=1 104 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods Algorithm 1 The Particle Filter (pf) (i) Generate N samples {x0 }N i=1 from p(x0 ). 2: Compute (i) (i) (i) (i) (i) p(yk |xk )p(xk |xk−1 ) wk = wk−1 (i) (i) p prop (xk |xk−1 , yk ) (j) (i) (i) and normalise, i.e., w̄k = wk / N j=1 wk , i = 1, . . . , N . 1: 3: (i ) [Optional]. Generate a new set {xk }N i=1 by resampling with replacement N (i) (i) (i ) (i) (i) times from {xk }N i=1 , with probability w̄k = Pr{xk = xk } and let wk = 1/N , i = 1, . . . , N . 4: Generate predictions from the proposal density (i) (i ) xk+1 ∼ p prop (xk+1 |xk , yk+1 ), i = 1, . . . , N . 5: 3 3.1 Increase k and continue to step 2. Dynamic Models Robot Model This section describes a two-link non-linear ﬂexible robot model, corresponding to joint two and three for a serial six dof industrial robot. The dynamic robot model is a joint ﬂexible two-axes model from Moberg et al. [2008], see Figure 1. Each link is modelled as a rigid-body and the joints are modelled as a spring damping pair with non-linear spring torque and linear damping. The deﬂection in each joint is given by the arm angle qai and the motor angle qmi . Let T T T qa = qa1 qa2 , qam = qm1 /η1 qm2 /η2 , τ am = τm1 η1 τm2 η2 , where τmi is the motor torque and ηi = qmi /qai > 1 is the gear ratio. A dynamic model can be derived as Ma (qa )q̈a + C(qa , q̇a ) + G(qa ) + N (q) = 0, a Mm q̈am + F(q̇am ) − N (q) = τm , (6a) (6b) using Lagrange’s equation, where N (q) = T (qa − qam ) + D(q̇a − q̇am ), Ma ( · ) and Mm are the inertia matrices for the arms and motors, respectively, C( · ) is the Coriolisand centrifugal terms, G( · ) is the gravity torque, F( · ) is the friction torque, T ( · ) is the spring stiﬀness torque and D( · ) is the damping torque. See Moberg et al. [2008] for a full description of the model. 3.2 Accelerometer Model The accelerometer attached to the robot measures the acceleration due to the motion the robot performs, the gravity component and in addition some measurement noise is introduced. When modelling the accelerometer it is also important 3 105 Dynamic Models qa2 zb qa1 zs ρb xb P xs Figure 1: A two degrees-of-freedom robot model. The links are assumed to be rigid and the joints are described by a two mass system connected by a spring damper pair. The accelerometer is attached in the point P . to include a bias term. The acceleration is measured in a frame O xs zs ﬁxed to the accelerometer relative an inertial frame, which is chosen to be the world ﬁxed base frame Oxb zb , see Figure 1. The acceleration in O xs zs can thus be expressed as (7) ρ̈ s (qa ) = Rb/s (qa ) ρ̈ b (qa ) + gb + bacc , T where ρ̈ b (qa ) is the acceleration due to the motion and gb = 0 g models the gravitation, both expressed in the base frame Oxb zb . The bias term is denoted by bacc and is expressed in O xs zs . Rb/s (qa ) is a rotation matrix that represents the rotation from frame Oxb zb to frame O xs zs . The vector ρ̈ b (qa ) can be calculated as the second derivative of ρ b (qa ) which is shown in Figure 1. Using the forward kinematic relation, the vector ρ b can be written as ρ b (qa ) = Υacc (qa ), (8) where Υacc is a non-linear function. Taking the derivative of (8) with respect to time twice gives d2 d (9) J (q ) q̇a , ρ̈ b (qa ) = 2 Υacc (qa ) = Jacc (qa )q̈a + dt acc a dt acc where Jacc (qa ) = ∂Υ is the Jacobian matrix. The ﬁnal expression for the accel∂qa eration measured by the accelerometer is given by (7) and (9). 3.3 Modelling of Bias In (7) a bias component is included in the accelerometer model, which is un n T known and may vary over time. The bias component bk = bk1 . . . bk b can be modelled as a random walk, i.e., bk+1 = bk + wbk , (10) 106 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods T where wbk = wkb,1 . . . wkb,nb is process noise and nb is the number of bias terms. The random walk model is then included in the estimation problem and the bias terms are estimated simultaneously as the other states. Let a state space model without any bias states be given by (1). The augmented model with the bias states can then be written as xk+1 f (xk , uk ) g(xk ) 0 wk = + , (11) bk+1 bk 0 I wbk yk = h(xk , uk ) + Cbk + vk , (12) where I and 0 are the identity and null matrices, respectively, and C ∈ Rny ×nb is a constant matrix 4 Estimation Models Four diﬀerent estimation models are presented using the robot and acceleration models described in Section 3. The process noise wk and measurement noise vk are in all four estimation models assumed to be Gaussian with zero mean and covariance matrices Q and R, respectively. 4.1 Non-linear Estimation Model Let the state vector be T T x = xT1 xT2 xT3 xT4 = qTa qa,T , (13) q̇Ta q̇a,T m m T a a T qm2 are the mowhere qa = qa1 qa2 are the arm positions and qam = qm1 tor positions on the arm side of the gearbox. Let also the input vector u = τ am . Taking the derivative of x with respect to time and using (6) give ⎛ ⎞ x3 ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ x4 ⎜⎜ ⎟⎟ ẋ = ⎜⎜ −1 (14) ⎟. ⎜⎜Ma (x1 ) (−C(x1 , x3 ) − G(x1 ) − N (x))⎟⎟⎟ ⎝ ⎠ −1 Mm (u − F(x4 ) + N (x)) In order to use the estimation methods described in Section 2 the continuous state space model (14) has to be discretised. The time derivative of the state vector can be approximated using Euler forward according to x − xk ẋ = k+1 , (15) Ts where Ts is the sample time. Taking the right hand side in (14) and (15) equal to each other give the non-linear discrete state space model ⎛ ⎞ x1,k + Ts x3,k ⎜⎜ ⎟⎟ ⎜⎜⎜ ⎟⎟ x + T x 2,k s 4,k , -⎟⎟⎟⎟ . xk+1 = ⎜⎜⎜ (16) −1 ⎜⎜x3,k + Ts Ma (x1,k ) −C(x1,k , x3,k ) − G(x1,k ) − N (xk ) ⎟⎟ ⎝ ⎠ , x4,k + Ts M−1 m uk − F(x4,k ) + N (xk ) 4 107 Estimation Models The noise is modelled as a torque disturbance on the arms and motors, giving a model according to (1a) where f (xk , uk ) is given by the right hand side in (16) and ⎞ ⎛ 0 0 ⎟⎟ ⎜⎜ ⎜⎜ 0 0 ⎟⎟⎟⎟ ⎜ g(xk ) = ⎜⎜⎜ (17) ⎟, −1 ⎜⎜Ts Ma (x1,k ) 0 ⎟⎟⎟ ⎠ ⎝ 0 Ts M−1 m where 0 is a two by two null matrix and the noise wk ∈ R4 . The measurements are the motor positions qam and the end-eﬀector acceleration ρ̈ M s (qa ). The measurement model (1b) can therefore be written as x2,k (18) + vk , yk = Rb/s (x1,k ) ρ̈ b (xk ) + gb where ρ̈ b (xk ) is given by (9) and vk ∈ R4 . In (9) are qa and q̇a given as states, = whereas q̈a is given by the third row in (14). The accelerometer bias bacc k acc,x acc,z T is modelled as it is described in Section 3.3 with bk bk 0 C= , (19) I where I and 0 are two by two identity and null matrices, respectively. 4.2 Estimation Model with Linear Dynamic A linear dynamic model with arm positions, velocities and accelerations as state variables is suggested. Let the state vector be T x = xT1 xT2 xT3 = qTa q̇Ta q̈Ta , (20) T where qa = qa1 qa2 are the arm positions. This yields the following state space model in discrete time xk+1 = Fxk + Gu uk + Gw wk , (21) where uk is the input vector and the process noise wk ∈ R2 . The constant matrices are given by ⎛ 3 ⎞ ⎛ 2 ⎞ ⎜⎜ Ts I⎟⎟ ⎜⎜ I Ts I Ts I⎟⎟ ⎜⎜ 62 ⎟⎟ 2 ⎟ ⎜⎜ ⎟ ⎜ ⎟ F = ⎜⎜⎜0 I (22) Ts I ⎟⎟⎟ , Gu = Gw = ⎜⎜⎜ Ts I⎟⎟⎟ ⎠ ⎜⎝ 2 ⎟⎠ ⎝ 0 0 I Ts I where I and 0 are two by two identity and null matrices, respectively. The input, uk , is the arm jerk reference, i.e., the diﬀerentiated arm angular acceleration reference. 108 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods The motor positions are calculated from (6a) where the spring is linear, i.e., k 0 T (qam − qa ) = 1 (23) (qam − qa ) = K · (qam − qa ). 0 k2 The damping term is small compared to the other terms [Karlsson and Norrlöf, 2005] and is therefore neglected for simplicity. The measurement model for the accelerometer is the same as in (18) where q̈a,k is a state in this case. The measurement model can now be written as ⎞ ⎛ qam,k ⎜⎜ ⎟⎟⎟⎟ + v . (24) yk = ⎜⎜⎝ k Rb/s (x1,k ) ρ̈ b (xk ) + gb ⎠ where vk ∈ R4 and , qam,k =x1,k + K−1 Ma (x1,k )x3,k + C(x1,k , x2,k ) + G(x1,k ) , d J (x ) x2,k . ρ̈ b (xk ) =Jacc (x1,k )x3,k + dt acc 1,k (25a) (25b) is modelled according to Section 3.3. Once again, the accelerometer bias bacc k However, the estimation result is improved if bias components for the motor measurements also are included. The explanation is model errors in the mea T surement equation. The total bias component is bk = bqk m ,T bacc,T , where k q acc,x qm qm2 T acc,z T m1 acc bk = b k and bk = bk . The matrix C in (12) is for this bk bk model a four by four identity matrix. 4.3 Linear Estimation Model with Acceleration as Input In De Luca et al. [2007] a model using the arm angular acceleration as input is presented. Identifying the third row in (14) as the arm angular acceleration and use that as an input signal with (13) as state vector give the following model, ⎛ ⎞ x3 ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ x4 ⎜⎜ ⎟⎟ ẋ = ⎜⎜ (26) ⎟⎟ , in ⎜⎜ ⎟⎟ q̈a ⎝ −1 ⎠ Mm (u − F(x4 ) + N (x)) where q̈in a is the new input signal. If the friction, spring stiﬀness and damping are modelled with linear relations, then ⎞ ⎛ ⎞ ⎛ 0 I 0 0 ⎟⎟ ⎜⎜ 0 ⎟⎟ ⎜⎜0 ⎜⎜ 0 ⎟⎟ ⎜⎜0 0 0 I 0 ⎟⎟⎟⎟ q̈in ⎜ ⎟⎟ ⎜ ẋ = ⎜⎜⎜ (27) ⎟ a , ⎟⎟ x + ⎜⎜⎜ ⎟⎟ ⎜⎜ I 0 0 0 0 ⎟⎟⎟ u ⎜⎜⎝ 0 ⎠ ⎠ ⎝ −1 −1 −1 M−1 0 M−1 m K −Mm K Mm D −Mm (D + Fd ) m where k K= 1 0 0 , k2 d D= 1 0 0 , d2 η2f Fd = 1 d1 0 0 . η12 f d2 5 Experiments on an ABB IRB4600 Robot 109 The linear state space model is discretised using zero order hold (zoh). zoh is used instead of Euler forward since it gives a better result and an explicit solution exist when the model is linear. The only remaining measurements are the motor positions, which give a linear measurement model according to yk = 0 I 0 0 xk + vk , (28) where vk ∈ R2 . Note that the arm angular acceleration q̈in a is not measured direct, instead it has to be calculated from the accelerometer signal using (7) and (9), which is possible as long as the Jacobian Jacc (x1,k ) has full rank. 4.4 Non-linear Estimation Model with Acceleration as Input The linear model presented in Section 4.3 is here reformulated as a non-linear model. Given the model in (26) and using Euler forward for discretisation give ⎞ ⎛ x1,k + Ts x3,k ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜ x2,k + Ts x4,k ⎟⎟ ⎜⎜ ⎟⎟ . (29) xk+1 = ⎜⎜⎜ in ⎟⎟ q̈ x + T ⎜⎜ 3,k s a,k ⎟⎠ , ⎝ x4,k + Ts M−1 m uk − F(x4,k ) + N (xk ) The noise model is assumed to be the same as in Section 4.1, and the measurement is the same as in (28). 5 5.1 Experiments on an ABB IRB4600 Robot Experimental Setup The accelerometer used in the experiments is a triaxial accelerometer from Crossbow Technology, with a range of ±2 g, and a sensitivity of 1 V/g [Crossbow Technology, 2004]. The orientation and position of the accelerometer are estimated using the method described in Axelsson and Norrlöf [2012]. All measured signals are synchronous and sampled with a rate of 2 kHz. The accelerometer measurements are ﬁltered with an lp-ﬁlter before any estimation method is applied to better reﬂect the tool movement. The path used in the evaluation is illustrated in Figure 2 and it is programmed such that only joint two and three are moved. Moreover, the wrist is conﬁgured such that the couplings to joint two and three are minimised. The dynamic model parameters are obtained using a grey-box identiﬁcation method described in Wernholt and Moberg [2011]. It is not possible to get measurements of the true state variables, as is the case for simulation, instead, only the true trajectory of the tool, more precise the tcp, x and z coordinates, is used for evaluation. The true trajectory is measured using a laser tracking system from Leica Geosystems. The tracking system has an accuracy of 0.01 mm per meter and a sample rate of 1 kHz [Leica Geosystems, 2008]. However, the measured tool position is not synchronised with the other measured signals. Resampling of the measured signal and a manual synchronisation are therefore needed, which can introduce small errors. Another source of 110 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods 1.05 1 z [m] 0.95 0.9 0.85 0.8 0.75 1.1 1.15 1.2 1.25 1.3 1.35 1.4 x [m] Figure 2: Measured path for the end-eﬀector used for experimental evaluations. error is the accuracy of the programmed tcp in the control system of the robot. The estimated data is therefore aligned with the measured position to avoid any static errors. The alignment is performed using a least square ﬁt between the estimated position and the measured position. 5.2 Experimental Results Six observers using the four diﬀerent estimation models described in Section 4 are evaluated. The observers are based on the ekf, eks, pf or a linear dynamic observer using pole placement [Franklin et al., 2002]. OBS1: ekf with the non-linear model in Section 4.1. OBS2: eks with the non-linear model in Section 4.1. OBS3: ekf with the linear state model and non-linear measurement model in Section 4.2. OBS4: pf with the linear state model and non-linear measurement model in Section 4.2. OBS5: ekf with the non-linear model where the acceleration of the end-eﬀector is input, see Section 4.4 . OBS6: Linear dynamic observer using pole placement with the linear model where the acceleration of the end-eﬀector is input, see Section 4.3. [De Luca et al., 2007] The only measured quantity, to compare the estimates with, is the measured tool position, as was mentioned in Section 5.1. Therefore, the estimated arm angles 5 111 Experiments on an ABB IRB4600 Robot are used to compute an estimate of the tcp using the kinematic relation, i.e., x̂k (30) = Υtcp (q̂a,k ), ẑk where q̂a,k is the result from one of the six observers at time k. The result is presented with diagrams of the true and estimated paths for the horizontal parts of the path in Figure 2. The path error $ (31) ek = (xk − x̂k )2 + (zk − ẑk )2 , where xk , x̂k , zk and ẑk are the true and estimated position for the tool in the xand z-direction at time k, as well as the root mean square error (rmse) % & ' N 1 2 ek , (32) = N k=1 where N is the number of samples, are also used for evaluation. Moreover, the ﬁrst 250 samples are always removed because of transients. The execution time for the observers is also examined. Note that the execution times are with respect to the current Matlab implementation. The execution time may be faster after some optimisation of the Matlab code or by using another programming language, e.g. C++. The observers are ﬁrst paired such that the same estimation model is used, hence obs1–obs2, obs3–obs4, and obs5–obs6 are compared. After that, the best observers from each pair are compared to each other. OBS1 and OBS2. It is expected that obs2 (eks with non-linear model) will give a better result than obs1 (ekf with non-linear model) since the eks uses both previous and future measurements. This is not the case as can be seen in Figure 3. The reason for this can be model errors and in particular the non-linearities in the joint stiﬀness. One interesting observation is the higher orders of oscillations in the estimated paths. The oscillations can be reduced if the covariance matrix Q for the process noise is decreased. However, this leads to a larger path error. The rmse values can be found in Table 1. The table shows that obs2 is slightly better than obs1. With the current Matlab implementation the execution times are around ﬁve and seven seconds, respectively, and the total length of the measured path is four seconds, hence none of the observers are real-time. Most of the time is spent in evaluating the Jacobian Hk in the ekf and it is probably possible to decrease that time with a more eﬃcient implementation. Another possibility can be to run the ﬁlter with a lower sample rate. obs2 is slower since an ekf is used ﬁrst and then the backward time recursion in (4). However, most of the time in the eks is spent in the ekf. As a matter of fact, the execution time is irrelevant for obs2 since the eks uses future measurements and has to be implemented oﬀ-line. None of the two observers can be said to be better than the other in terms of estimation performance and execution time. The decision is whether future mea- 112 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods 1.006 z [m] 1.004 1.002 1 0.998 0.996 0.804 z [m] 0.802 0.8 0.798 0.796 0.794 1.15 1.2 1.25 1.3 1.35 x [m] Figure 3: The two horizontal sides of the true path (solid), and the estimated path using obs1 (dashed), and obs2 (dash-dot). Table 1: rmse values of the path error e for the end-eﬀector position given in mm for the six observers. obs1 obs2 obs3 obs4 obs5 obs6 1.5704 1.5664 2.3752 1.5606 1.6973 1.7624 surement can be used or not. obs1 is chosen as the one that will be compared with the other observers. OBS3 and OBS4. Figure 4 shows that the estimated paths follow the true path for both observers. It can be noticed that the estimate for obs3 (ekf with linear dynamic model) goes somewhat past the corners before it changes direction and that obs4 (pf with linear dynamic model) performs better in the corners. The estimate for obs4 is also closer to the true path, at least at the vertical sides. The rmse values of the path error for obs3 and obs4 are presented in Table 1. The rmse for obs4 is approximately two-thirds of the rmse for obs3. The Matlab implementation of obs3 is almost real-time, just above four seconds, and the execution time for obs4 is about ten hours. The execution time for obs3 can be reduced to real-time without losing performance if the measurements are decimated to approximately 200 Hz. The best observer in terms of the path error is obviously obs4 but if the execution time is of importance, obs3 is preferable. obs4 will be compared with the other observers since the path error is of more interest in this paper. 5 113 Experiments on an ABB IRB4600 Robot 1.006 z [m] 1.004 1.002 1 0.998 0.996 0.804 z [m] 0.802 0.8 0.798 0.796 0.794 1.15 1.2 1.25 1.3 1.35 x [m] Figure 4: The two horizontal sides of the true path (solid), and the estimated path using obs3 (dashed), and obs4 (dash-dot). OBS5 and OBS6. obs6 (linear model with acceleration as input using pole placement) performs surprisingly good although a linear time invariant model is used, see Figure 5. It can also be seen that obs5 (linear model with acceleration as input using ekf) performs a bit better. obs5 also has a higher order oscillation as was the case with obs1 and obs2. This is a matter of tuning where less oscillations induce higher path error. The rmse values of the path error are showed in Table 1. Both observers execute in real-time. The execution times are just below one second and around one-ﬁfth of a second, respectively. obs6 is clearly the fastest one of the six proposed observers. obs5 is the one that will be compared to the other observers. Summary The three observers obs1, obs4, and obs5 are the best ones from each pair. From Table 1 it can be seen that obs1 and obs4 have the same performance and that obs5 is a bit worse, see also the path errors in Figure 6. The diﬀerences are small so it is diﬃcult to say which one that is the best. Instead of ﬁlter performance, other things have to be considered, such as complexity, computation time, and robustness. Complexity. The complexity of the ﬁlters can be divided into model complexity and implementation complexity. The implementation of obs1 is straightforward and no particular tuning has to be performed in order to get an estimate. The 114 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods 1.006 z [m] 1.004 1.002 1 0.998 0.996 0.804 z [m] 0.802 0.8 0.798 0.796 0.794 1.15 1.2 1.25 1.3 1.35 x [m] Figure 5: The two horizontal sides of the true path (solid), and the estimated path using obs5 (dashed), and obs6 (dash-dot). 3.5 3 e [mm] 2.5 2 1.5 1 0.5 0 0 500 1000 1500 2000 2500 Sample Figure 6: The path error for the estimated path using obs1 (red), obs4 (green), and obs5 (blue). 6 Conclusions 115 tuning is of course important to get a good estimate. Instead, most of the time has to be spent on a rigorous modelling work and identiﬁcation of the parameters to minimise model errors. For obs4 the opposite is true. The model is simple and requires not that much work. Most of the time has to be spent on implementing the pf. The standard choices of a proposal distribution did not work due to high snr and noninvertible measurement model. Instead, an approximation of the optimal proposal, using an ekf, was required. The consequence is more equations to implement and more tuning knobs to adjust. The model complexity for obs5 is in between obs1 and obs2. No model for the rigid body motion on the arm side is needed which is a diﬀerence from the other two. However, a non-linear model for the ﬂexibilities and friction is still required, which is not the case for obs4. Computation time. The computation time diﬀers a lot for the three observers. obs5 is in real-time with the current Matlab implementation and obs1 can probably be executed in real-time after some optimisation of the Matlab code or with another programming language. The computation time for obs4 is in the order of hours and is therefore far from real-time. Robustness. An advantage with obs5, compared to the other two, is that the equations describing the arm dynamics are removed, hence no robustness issues concerning the model parameters describing the arm, such as inertia, masses, centre of gravity, etcetera. However, the model parameters describing the ﬂexibilities remains. Other advantages. An advantage with obs4 is that the pf provides the entire distribution of the states, which is approximated as a Gaussian distribution in the ekf. The information about the distribution can be used in e.g. control and diagnosis. 6 Conclusions A sensor fusion approach to estimate the end-eﬀector position by combining a triaxial accelerometer at the end-eﬀector and the motor angular positions of an industrial robot is presented. The estimation is formulated as a Bayesian estimation problem and has been evaluated on experimental data from a state of the art industrial robot. Diﬀerent types of observers where both the estimation model and the ﬁlter were changed, have been used. The three observers with the best performance were a) an ekf using a non-linear dynamic model, b) a particle ﬁlter using a linear dynamic model, and c) an ekf with a non-linear model, where the acceleration of the end-eﬀector is used as an input instead of a measurement. 116 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods The performances of these three observers were very similar when considering the path error. The execution time for a) was just above the real-time limit, for c) just below the limit, and for b) in the order of hours. The time required for modelling and implementation is also diﬀerent for the three diﬀerent observers. For b), most of the time was spent to implement the ﬁlter and get it to work, whereas most of the time for a) was spent on modelling the dynamics. Acknowledgement This work was supported by the Vinnova Excellence Center LINK-SIC. Bibliography 117 Bibliography Brian D. O. Anderson and John B. Moore. Optimal Filtering. Information and System Sciences Series. Prentice Hall Inc., Englewood Cliﬀs, NJ, USA, 1979. Patrik Axelsson. Evaluation of six diﬀerent sensor fusion methods for an industrial robot using experimental data. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 126–132, Dubrovnik, Croatia, September 2012. Patrik Axelsson and Mikael Norrlöf. Method to estimate the position and orientation of a triaxial accelerometer mounted to an industrial manipulator. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 283–288, Dubrovnik, Croatia, September 2012. Torgny Brogårdh. Present and future robot control development—an industrial perspective. Annual Reviews in Control, 31(1):69–79, 2007. Crossbow Technology. Accelerometers, High Sensitivity, LF Series, CXL02LF3, January 2004. Available at http://www.xbow.com. Alessandro De Luca, Dierk Schröder, and Michael Thümmel. An accelerationbased state observer for robot manipulators with elastic joints. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 3817– 3823, Roma, Italy, April 2007. Arnaud Doucet, Simon Godsill, and Christophe Andrieu. On sequential Monte Carlo sampling methods for Bayesian ﬁltering. Statistics and Computing, 10 (3):197–208, 2000. Arnaud Doucet, Nando de Freitas, and Neil Gordon, editors. Sequential Monte Carlo Methods in Practice. Statistics for Engineering and Information Science. Springer, New York, NY, USA, 2001. Gene F. Franklin, J. David Powell, and Abbas Emami-Naeini. Feedback Control of Dynamic Systems. Prentice Hall Inc., Upper Saddle River, NJ, USA, fourth edition, 2002. Neil J. Gordon, David J. Salmond, and Adrian F. M. Smith. Novel approach to nonlinear/non-Gaussian Bayesian state estimation. IEE Proceedings on Radar and Signal Processing, 140(2):107–113, April 1993. Fredrik Gustafsson. Statistical Sensor Fusion. Studentlitteratur, Lund, Sweden, ﬁrst edition, 2010. Robert Henriksson, Mikael Norrlöf, Stig Moberg, Erik Wernholt, and Thomas B. Schön. Experimental comparison of observers for tool position estimation of industrial robots. In Proceedings of the 48th IEEE Conference on Decision and Control, pages 8065–8070, Shanghai, China, December 2009. 118 Paper B Evaluation of Six Diﬀerent Sensor Fusion Methods Rahim Jassemi-Zargani and Dan Necsulescu. Extended Kalman ﬁlter-based sensor fusion for operational space control of a robot arm. IEEE Transactions on Instrumentation and Measurement, 51(6):1279–1282, December 2002. Andrew H. Jazwinski. Stochastic Processes and Filtering Theory, volume 64. Academic Press, New York, NY, USA, 1970. Rickard Karlsson and Mikael Norrlöf. Bayesian position estimation of an industrial robot using multiple sensors. In Proceedings of the IEEE Conference on Control Applications, pages 303–308, Taipei, Taiwan, September 2004. Rickard Karlsson and Mikael Norrlöf. Position estimation and modeling of a ﬂexible industrial robot. In Proceedings of the 16th IFAC World Congress, Prague, Czech Republic, July 2005. Leica Geosystems. Case study abb robotics - Västerås, 2008. Available at http: //metrology.leica-geosystems.com/en/index.htm. Stig Moberg, Jonas Öhr, and Svante Gunnarsson. A benchmark problem for robust control of a multivariable nonlinear ﬂexible manipulator. In Proceedings of the 17th IFAC World Congress, pages 1206–1211, Seoul, Korea, July 2008. Gerasimos G. Rigatos. Particle ﬁltering for state estimation in nonlinear industrial systems. IEEE Transactions on Instrumentation and Measurement, 58(11): 3885–3900, November 2009. Erik Wernholt and Stig Moberg. Nonlinear gray-box identiﬁcation using local models applied to industrial robots. Automatica, 47(4):650–660, April 2011. Byron M. Yu, Krishna V. Shenoy, and Maneesh Sahani. Derivation of extended Kalman ﬁltering and smoothing equations. URL: http://www-npl. stanford.edu/~byronyu/papers/derive_eks.pdf, 19 October 2004. Paper C Discrete-time Solutions to the Continuous-time Differential Lyapunov Equation With Applications to Kalman Filtering Authors: Patrik Axelsson and Fredrik Gustafsson Edited version of the paper: Patrik Axelsson and Fredrik Gustafsson. Discrete-time solutions to the continuous-time diﬀerential Lyapunov equation with applications to Kalman ﬁltering. Submitted to IEEE Transactions on Automatic Control, 2012. C Discrete-time Solutions to the Continuous-time Diﬀerential Lyapunov Equation With Applications to Kalman Filtering Patrik Axelsson and Fredrik Gustafsson Dept. of Electrical Engineering, Linköping University, SE–581 83 Linköping, Sweden [email protected], [email protected] Abstract Prediction and ﬁltering of continuous-time stochastic processes often require a solver of a continuous-time diﬀerential Lyapunov equation (cdle), for example the time update in the Kalman ﬁlter. Even though this can be recast into an ordinary diﬀerential equation (ode), where standard solvers can be applied, the dominating approach in Kalman ﬁlter applications is to discretise the system and then apply the discrete-time diﬀerence Lyapunov equation (ddle). To avoid problems with stability and poor accuracy, oversampling is often used. This contribution analyses over-sampling strategies, and proposes a novel low-complexity analytical solution that does not involve oversampling. The results are illustrated on Kalman ﬁltering problems in both linear and non-linear systems. 1 Introduction Numerical solvers for ordinary diﬀerential equations (ode) is a well studied area [Hairer et al., 1987]. The related area of Kalman ﬁltering (state prediction and state estimation) in continuous-time models was also well studied during the ﬁrst two decades of the Kalman ﬁlter, see for instance Jazwinski [1970], while the more recent literature such as the standard reference Kailath et al. [2000] focuses on discrete time ﬁltering only. A speciﬁc example, with many applications in practice, is Kalman ﬁltering based on a continuous-time state space model with discrete-time measurements, known as continuous-discrete ﬁltering. The Kalman ﬁlter (kf) here involves a time update that integrates the ﬁrst and second order moments from one sample time to the next one. The second order moment is a covariance matrix, and it governs a continuous-time diﬀerential Lyapunov 121 122 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation equation (cdle). The problem can easily be recast into a vectorised ode problem and standard solvers can be applied. For linear ode’s, the time update of the linear kf can thus be solved analytically, and for non-linear ode’s, the time update of the extended kf has a natural approximation in continuous-time. One problem is the large dimension of the resulting ode. Another possible explanation why the continuous-time update is not used is the common use of discrete-time models in Kalman ﬁlter applications, so practitioners often tend to discretise the state space model ﬁrst to ﬁt the discrete-time Kalman ﬁlter time update. Despite a closed form solution exists, this involves approximations that lead to well known problems with accuracy and stability. The ad-hoc remedy is to oversample the system, so a large number of small time updates are taken between the sampling times of the observations. In literature, diﬀerent methods are proposed to solve the continuous-discrete non-linear ﬁltering problem using extended Kalman ﬁlters (ekf). A common way is to use a ﬁrst or second order Taylor approximation as well as a RungeKutta method in order to integrate the ﬁrst order moments, see e.g. LaViola [2003]; Rao et al. [2011]; Mallick et al. [2012]. They all have in common that the cdle is replaced by the discrete-time diﬀerence Lyapunov equation (ddle), used in discrete-time Kalman ﬁlters. A more realistic way is to solve the cdle as is presented in Bagterp Jörgensen et al. [2007]; Mazzoni [2008], where the ﬁrst and second order moments are integrated numerically. A comparison between diﬀerent solutions is presented in Frogerais et al. [2012], where the method proposed by the authors discretises the stochastic diﬀerential equation (sde) using a Runge-Kutta solver. The other methods in Frogerais et al. [2012] have been proposed in the literature before, e.g. LaViola [2003]; Mazzoni [2008]. Related work using diﬀerent approximations to continuous integration problems in non-linear ﬁltering also appears in Särkkä [2007]; Zhang et al. [2005] for unscented Kalman ﬁlters and Arasaratnam and Haykin [2009] for cubature Kalman ﬁlters. This contribution takes a new look at this fundamental problem. First, we review diﬀerent approaches for solving the cdle in a coherent mathematical framework. Second, we analyse in detail the stability conditions for oversampling, and based on this we can explain why even simple linear models need a large rate of oversampling. Third, we make a new straightforward derivation of a low-complexity algorithm to compute the solution with arbitrary accuracy. Numerical stability and computational complexity are analysed for the diﬀerent approaches. It turns out that the low-complexity algorithm has better numerical properties compared to the other methods, and at the same time a computational complexity in the same order. Fourth, the methods are extended to non-linear system where the extended Kalman ﬁlter (ekf) is used. We illustrate the results on both a simple second order linear spring-damper system, and a non-linear spring-damper system relevant for mechanical systems, in particular robotics. 2 123 Mathematical Framework and Background 2 2.1 Mathematical Framework and Background Linear Stochastic Differential Equations Consider the linear stochastic diﬀerential equation (sde) dx(t) = Ax(t)dt + Gdβ(t), (1) nβ is the for t ≥ 0, where x(t) ∈ state vector and β(t) ∈ R is a vector of Wiener processes with E dβ(t)dβ(t)T = Qdt. The matrices A ∈ Rnx ×nx and G ∈ Rnx ×nβ are here assumed to be constants, but they can also be time varying. It is also possible to include a control signal u(t) in (1) but that is omitted here for brevity. Rnx Given an initial state x̂(0) with covariance P(0), we want to solve the sde to get x̂(t) and P(t) at an arbitrary time instance. By multiplying both sides with the integrating factor e−As and integrating over the time interval gives t At e A(t−s) Gdβ(s) ds . x(t) = e x(0) + (2) 0 =vd (t) The goal is to get a discrete-time update of the mean and covariance, from x̂(kh) and P(kh) to x̂((k + 1)h) and P((k + 1)h), respectively. The time interval h may correspond to one sample interval, or be a fraction of it in the case of oversampling. The latter case will be discussed in detail later on. For simplicity, the time interval [kh, (k + 1)h] will be denoted as [0, t] below. The discrete-time equivalent noise vd (t) has covariance given by t e Qd (t) = A(t−s) T AT (t−s) GQG e t ds = 0 Tτ eAτ GQGT eA dτ, (3) 0 We immediately get an expression for the ﬁrst and second order moments of the sde solution over one time interval as x̂(t) = eAt x̂(0), At P(t) = e P(0)e (4a) AT t + Qd (t). (4b) From (2) and (3), we can also recover the continuous-time update formulas ˙ = Ax̂(t), x̂(t) (5a) Ṗ(t) = AP(t) + P(t)AT + GQGT , (5b) by, a bit informally, taking the expectation of (2) and then dividing both sides with t and letting t → 0. Here, (5a) is an ordinary ode and (5b) is the continuoustime diﬀerential Lyapunov equation (cdle). Thus, there are two conceptually diﬀerent alternatives. Either, solve the inte- 124 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation gral (3) deﬁning Qd (t) and use (4), or solve the ode and cdle in (5). These two well-known alternatives are outlined below. 2.2 Matrix Fraction Decomposition There are two ways to compute the integral (3) described in literature. Both are based on computing the matrix exponential of the matrix A GQGT H= . (6) 0 −AT The result is a block matrix in the form M1 (t) Ht e = 0 M2 (t) , M3 (t) (7) T where the structure implies that M1 (t) = eAt and M3 (t) = e−A t . As shown in Van Loan [1978], the solution to (3) can be computed as Qd (t) = M2 (t)M1 (t)T (8) This is immediately veriﬁed by taking the time derivative of the deﬁnition (3) and the matrix exponential (7), and verifying that the involved Taylor expansions are equal. Another alternative known as matrix fraction decomposition, which solves a matrix valued ode, given in Grewal and Andrews [2008]; Särkkä [2006], is to com T pute P(t) directly. Using the initial conditions P(0) I for the ode gives P(t) = (M1 (t)P(0) + M2 (t)) M3 (t)−1 . (9) =C(t) The two alternatives in (8) and (9) are apparently algebraically the same. There are also other alternatives described in literature. First, the integral in (3) can of course be solved with numerical methods such as the trapezoidal method or the rectangle method. In Rome [1969] the integral is solved analytically in the case that A is diagonalisable. However, not all matrices are diagonalisable, and even in such cases, this method is not numerically stable [Higham, 2008]. 2.3 Vectorisation Method The odes for the ﬁrst and second order moments in (5) can be solved using a method based on vectorisation. The vectorisation approach for matrix equations is well known and especially for the cdle, see e.g. Davison [1975]; Gajić and Qureshi [1995]. The method uses the fact that (5) can be converted to one single 2 125 Mathematical Framework and Background ode by introducing an extended state vector z (t) x(t) z(t) = x = , zP (t) vech P(t) 0 A 0 z(t) + = Az z(t) + Bz , ż(t) = 0 AP vech GQGT (10) (11) where AP = D† (I ⊗ A + A ⊗ I) D. Here, vech denotes the half-vectorisation operator, ⊗ is the Kronecker product and D is a duplication matrix, see Appendix A for details. The solution of the ode (11) is given by [Rugh, 1996] t z(t) = e Az t eAz (t−s) dsBz . z(0) + (12) 0 One potentially prohibitive drawback with the solution in (12) is its computational complexity, in particular for the matrix exponential. The dimension of the extended state z is nz = nx + nx (nx + 1) /2, giving a computational complexity of O(n6x ). Section 4 presents a way to rewrite (12) to give a complexity of O(n3x ) instead of O(n6x ). 2.4 Discrete-time Recursion of the CDLE The solution in (4b) using (8), the matrix fraction decomposition in (9), and the ode (12) evaluated at the discrete-time instances t = kh and t = (k + 1)h give the following recursive update formulas T P((k + 1)h) = e Ah P(kh)eA h + M2 (h)M1 (h)T P((k + 1)h) = M1 (h)P(kh) + M2 (h) M3 (h)−1 (13) (14) h z((k + 1)h) = e Az h e Az s dsBz , z(kh) + (15) 0 which can be used in the Kalman ﬁlter time update. 2.5 The Matrix Exponential Sections 2.1 to 2.3 shows that the matrix exponential function is a working horse to solve the linear sde. At this stage, numerical routines for the matrix exponential are important to understand. One key approach is based on the following identity and Taylor expansion [Moler and Van Loan, 2003] p m 1 Ah Ah Ah Ah/m m = ep,m (Ah). (16) e = e + ··· + ≈ I+ m p! m In fact, the Taylor expansion is a special case of a more general Padé approximation of eAh/m [Moler and Van Loan, 2003], but this does not aﬀect the discussion 126 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation here. The eigenvalues of Ah/m are the eigenvalues of A scaled with h/m, and thus they can be arbitrarily small if m is chosen large enough for any given h. Further, the p’th order Taylor expansion converges faster for smaller eigenvalues of Ah/m. Finally, the power function Mm is eﬃciently implemented by squaring the matrix M in total log2 (m) times, assuming that m is chosen to be a power of 2. We will denote this approximation with ep,m (Ah). A good approximation ep,m (Ah) is characterised by the following properties: • Stability. If A has all its eigenvalues in the left half plane, then ep,m (Ah) should have all its eigenvalues inside the unit circle. " " • Accuracy. If p and m are chosen large enough, the error ""e Ah − ep,m (Ah)"" should be small. Since the Taylor expansion converges, we have trivially that m lim ep,m (Ah) = e Ah/m = eAh . p→∞ (17a) From the property limx→∞ (1 + a/x)x = e a , we also have lim ep,m (Ah) = e Ah . m→∞ (17b) Finally, from Higham [2008] we have that " Ap+1 hp+1 "" "eAh − ep,m (Ah)"" ≤ e Ah . mp (p + 1)! (17c) However, for any ﬁnite p and m > 1, then all terms in the binomial expansion of ep,m (Ah) are diﬀerent from the Taylor expansion of eAh , except for the ﬁrst two terms which are always I + Ah. The complexity of the approximation ep,m (Ah), where A ∈ Rnx ×nx , is in the order , of log2 (m) + p n3x , where pn3x multiplications are required to compute Ap and log2 (m)n3x multiplications are needed for squaring the Taylor expansion log2 (m) times. Standard numerical integration routines can be recast into this framework as well. For instance, a standard tuning of the fourth order Runge-Kutta method for a linear ode results in e4,1 (Ah). 2.6 Solution using Approximate Discretisation We have now outlined three methods to compute the exact time update in the discrete-time Kalman ﬁlter. These should be equivalent up to numerical issues, and will be treated as one approach in the sequel. Another common approach in practice, in particular in Kalman ﬁlter applications, is to assume that the noise is piece-wise constant giving the discrete-time 2 Mathematical Framework and Background 127 system x(k + 1) = Fh x(k) + Gh vh (k), (18a) Cov (vh (k)) = Qh , (18b) .h where Fh = ep,m (Ah), Gh = 0 e At dtG, and Qh = hQ. The discrete-time Kalman ﬁlter update equations x̂(k + 1) = Fh x̂(k), (19a) P(k + 1) = Fh P(k)FTh + Gh Qh GTh , (19b) are then used, where (19a) is a diﬀerence equation and (19b) is the discrete-time diﬀerence Lyapunov equation (ddle). The update equations (19) are exact for the discrete-time model (18). However, there are several approximations involved in the discretisation step: • First, Fh = ep,m (Ah) is an approximation of the exact solution given by Fh = eAh . It is quite common in practice to use Euler sampling deﬁned by Fh = I + Ah = e1,1 (Ah). • Even without process noise, the update formula for P in (19b) is not equivalent to (5b). • The discrete-time noise vh (t) is an aggregation of the total eﬀect of the Wiener process dβ(t) during the interval [t, t + h], as given in (3). The conceptual drawback is that the Wiener process dβ(t) is not aware of the sampling time chosen by the user. One common remedy is to introduce oversampling. This means that (19) is iterated m times using the sampling time h/m. When oversampling is used, the covariance matrix for the discrete-time noise vh (k) should be scaled as Qh = hQ/m. In this way, the problems listed above will asymptotically vanish as m increases. However, as we will demonstrate, quite large an m can be needed even for some quite simple systems. 2.7 Summary of Contributions • Section 3 gives explicit conditions for an upper bound of the sample time h such that a stable continuous-time model remains stable after discretisation. The analysis treats stability of both x and P, for the case of Euler sampling e1,m (A), for the solution of the sde given by the ode (11). Results for p > 1 are also brieﬂy discussed. See Table 1 for a summary when the vectorised solution is used. • Section 4 presents a reformulation of the solution to the ode (11), where the 3 - , computational complexity has been decreased from log2 (m) + p n2x /2 to , log2 (m) + p + 43 n3x . • Section 5 shows how the computational complexity and the numerical properties diﬀers between the diﬀerent methods. 128 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation Table 1: Summary of approximations ep,m (Ah) of e Ah . The stability region (h < hmax ) is parametrised in λi which are the eigenvalues to A. In the case of Runge-Kutta, only real eigenvalues are considered. Approach p m Stability region (hmax ) Euler sampling 1 1 Oversampled Euler 1 m>1 Runge-Kutta 4 1 Oversampled Runge-Kutta 4 m>1 − 2Re{λ2 i } |λi | 2mRe{λi } − |λi |2 2.7852 − λ , λi i ∈R − 2.7852m , λi ∈ R λ i • Section 6 presents a second order spring-damper example to demonstrate the advantages using a continuous-time update. • Section 7 discusses implications for non-linear systems, and investigates a non-linear system inspired by applications in robotics. 3 Stability Analysis It is known that the cdle in (5b) / has a unique positive solution P(t) if A is Hur1 T witz , GQG 0, the pair (A, GQGT ) is controllable, and P(0) 0 [Gajić and Qureshi, 1995]. We want to show that a stable continuous-time system results in a stable discrete-time recursion. We therefore assume that the continuoustime ode describing the state vector x(t) is stable, hence the eigenvalues λi , i = 1, . . . , nx to A are assumed to be in the left half plane, i.e., Re {λi } < 0, i = 1, . . . , nx . It will also be assumed that the remaining requirements are fulﬁlled. For the methods described in Section 2.2 we have that H has the eigenvalues ±λi , i = 1, . . . , nx , where λi are the eigenvalues of A. This follows from the structure of H. Hence, the matrix exponential eHt will have terms that tend to inﬁnity and zero with the same exponential rate when t increases. However, the case t = h is of most interest, where h is ﬁnite. Note that a small/large sample time depends strongly on the system dynamics. Even if the matrix eHt is ill-conditioned, the product (8) and the ratio (9) can be limited under the assumptions above, for not too large values of t. Note that the solution in (9) is, as a matter of fact, based on the solution of an unstable ode, see Grewal and Andrews [2008]; Särkkä [2006], but the ratio C(t)M3 (t)−1 can still be bounded. Both of these methods can have numerical problems which will be discussed in Section 5.2. 1 All eigenvalues are in the left half plane. 3 129 Stability Analysis 3.1 Stability for the Vectorisation Method using Euler Sampling The stability analysis in this section is standard and a similar analysis has been performed in Hinrichsen and Pritchard [2005]. The diﬀerence is that the analysis in Hinrichsen and Pritchard [2005] investigates which discretisation methods that are stable for suﬃciently small sample times. The analysis here is about to ﬁnd an upper bound of the sample time such that a stable continuous-time model remains stable after discretisation. The recursive solution (15) is stable for all h according to Lemma 3 in Appendix B, if the matrix exponential can be calculated exactly. Stability issues arise when eAz h has to be approximated by ep,m (Az h). In this section we derive an upper bound on h that gives a stable solution for e1,m (Az h), i.e., Euler sampling. The Taylor expansion and in particular Euler sampling is chosen due to its simplicity, the same approach is applicable to the Padé approximation as well. Higher orders of approximations using the Taylor expansion will be treated brieﬂy at the end of this section. From Section 2.3 we have that the matrix Az is diagonal, which means that calculation of the matrix exponential e Az h can be separated into e Ah and eAP h . From Lütkepohl [1996] it is known that the eigenvalues of AP are given by λi + λj , 1 ≤ i ≤ j ≤ nx , hence the ode describing the cdle is stable if A is Hurwitz. In order to keep the discrete-time system stable, the eigenvalues of both e1,m (Ah) and e1,m (AP h) need to be inside the unit circle. In Theorem 1 an explicit upper bound on the sample time h is given that makes the recursive solution to the continuoustime sde stable. Theorem 1. The recursive solution to the sde (1), in the form of (15), where the matrix exponential eAz h is approximated by e1,m (Az h), is stable if 1 0 ⎫ ⎧ ⎪ ⎪ ⎪ ⎪ ⎬ ⎨ 2mRe λi + λj , 1 ≤ i ≤ j ≤ n , (20) h < min ⎪ − 2 x⎪ ⎪ ⎪ ⎭ ⎩ λ + λ i j where λi , i = 1, . . . , nx , are the eigenvalues to A. Corollary 1. The bound in Theorem 1 becomes h<− 2m , λi λi ∈ R, (21) for real eigenvalues. Proof: Start with the ode describing the state vector x(t). The eigenvalues to e1,m (Ah) = (I + Ah/m)m are, according to Lemma 2 in Appendix B, given by (1 + λi h/m)m . The eigenvalues are inside the unit circle if |(1 + λi h/m)m | < 1, where m m $ λi h 1 2 2 2 2 m + 2ai hm + (ai + bi )h . (22) 1 + = m m In (22), the parametrisation λi = ai + ibi has been used. Solving |(1 + λi h/m)m | < 1 130 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation 3 4 3.5 2 0 1 Im λi + λj 3 1 2.5 0 2 -1 1.5 1 -2 0.5 -3 -6 -5 -4 0 -3 -2 Re λi + λj 1 -1 0 Figure 1: Level curves of (20), where the colours indicate the values on h. for h and using the fact |λi |2 = a2i + bi2 give h<− 2mai |λi |2 . (23) Similar calculations for the ode describing vech P(t) give 2m(ai + aj ) , h<− λ + λ 2 i j 1 ≤ i ≤ j ≤ nx . (24) 2m(ai + aj ) 4mai mai − =− =− , 2 2 λ + λ 2 |2λ | |λ i i| i j (25) Using λi = λj in (24) gives which is half as much as the bound in (23), hence the upper bound for h is given by (24). Theorem 1 shows that the sample time can be decreased if the absolute value of the eigenvalues are increased, but also if the real part approaches zero. The level curves of (20) for h = c = constant in the complex plane are given by 2aij m m 2 m2 2 − 2 = c ⇔ a + + b = (26) ij ij 2 c c2 aij + bij 0 0 1 1 where aij = Re λi + λj and bij = Im λi + λj . Equation (26) is the description of a circle with radius m/c centred in the point (−m/c, 0). The level curves are shown in Figure 1, where it can be seen how the maximal sample time depends on the magnitude and direction of the eigenvalues. 4 131 Reformulation of the Vectorised Solution for the CDLE Stability conditions of ep,m (Az h) for p > 1 can be carried out in the same way as for p = 1. For p = 2, the calculations can be done analytically and this results again in (20), 1 0 ⎫ ⎧ ⎪ ⎪ + λ 2mRe λ ⎪ ⎪ i j ⎬ ⎨ , 1 ≤ i ≤ j ≤ n . (27) h < min ⎪ − x⎪ ⎪ ⎪ 2 ⎭ ⎩ λ + λ i j It means that though the accuracy has increased, recall (17c), the stability condition remains the same. Increasing the order of approximation even more, results in a higher order polynomial inequality that has to be solved. A numerical solution is therefore preferred. The stability bound for h/m will actually increase when p > 2 increases. For example, e4,m (Ah), which corresponds to the Runge-Kutta solution for a linear ode gives, for real eigenvalues, h<− 2.7852m , λi λi ∈ R. (28) This is less conservative than the corresponding bound in (21). 4 Reformulation of the Vectorised Solution for the CDLE The solution to the ode describing the second order moment given by (12) can be computed eﬃciently using the following lemma. Lemma 1. The solution to the vectorised cdle vech Ṗ(t) = AP vech P(t) + vech GQGT , (29) vech P(t) = FP (t)vech P(0) + GP (t)vech GQGT . (30) is given by where FP (t) and GP (t) are given by 6 5 F (t) AP I t = P exp 0 0 0 GP (t) 0 (31) Proof: The derivation in Appendix A gives that FP (t) = eAP t , GP (t) = AP t A−1 P (e (32) − I). (33) It is now easily veriﬁed that the Taylor expansion of (31) and (32) as AP t − I) = It + AP A−1 P (e are the same. t2 t3 t4 + A2P + A3P + . . . , 2! 3! 4! (34) 132 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation The last result is important of two reasons. First, we avoid inversion of the large matrix AP by solving a matrix exponential instead. Second, the condition that AP has to be non-singular is relaxed. The new solution based on Lemma 1 is presented in Theorem 2. The solution requires the matrix exponential and the solution of an algebraic Lyapunov equation for which eﬃcient numerical solvers exist. Remark 1. In contrast to Lemma 1, the solution to the cdle in (5b) presented in Theorem 2 actually requires AP to be non-singular. The eigenvalues to AP are given by λi + λj , 1 ≤ i ≤ j ≤ nx [Lütkepohl, 1996], so we have that AP is non-singular when the eigenvalues of A are not mirrored in the imaginary axis. Eigenvalues in the origin is a special case of this. Theorem 2. Let Q be positive deﬁnite and assume that the eigenvalues of A are not mirrored in the imaginary axis. Then the solution of the cdle (5b) is given by T P(t) = e At P(0)eA t + Qd (t), (35a) AQd (t) + Qd (t)AT + GQGT − e At GQGT e AT t = 0, (35b) where Qd (t) is a unique and positive deﬁnite solution to (3). Proof: Taylor expansion of the matrix exponential gives A2P t 2 A3P t 3 + + ... (36) 2! 3! Using (73) in Appendix C, each term in the Taylor expansion can be rewritten according to e AP t = I + A P t + APk t k = D† (I ⊗ A + A ⊗ I)k t k D, (37) hence e AP t = D† e(I⊗A+A⊗I)t D (71),(72) = D† (eAt ⊗ e At )D. (38) The ﬁrst term in (30) can now be written as e AP t vech P(0) = D† (eAt ⊗ e At )Dvech P(0) = D† (eAt ⊗ e At )vec P(0) (70) = D† vec eAt P(0)eA Tt T = vech eAt P(0)eA t . (39) Similar calculations give e AP t vech GQGT = D† vec eAt GQGT e A Tt = vech Q(t). (40) The last term in (30) can then be rewritten according to AP t T − I)vech GQGT = A−1 A−1 P (e P vech (Q(t) − GQG ) = vech Q d (t), (41) where it is assumed that AP is invertible. Equation (41) can be seen as the solution − GQGT ). Using the of the linear system of equations AP vech Qd (t) = vech (Q(t) 4 133 Reformulation of the Vectorised Solution for the CDLE derivation in (65) in Appendix A backwards gives that Qd (t) is the solution to the algebraic Lyapunov equation = 0. AQd (t) + Qd (t)AT + GQGT − Q(t) (42) Combining (39) and (41) gives that (30) can be written as T P(t) = e At P(0)eA t + Qd (t), (43) where Qd (t) is the solution to (42). It is easily veriﬁed that Qd (t) in (3) satisﬁes (42) and it is well known that the Lyapunov equation has a unique solution iﬀ the eigenvalues of A are not mirrored in the imaginary axis [Gajić and Qureshi, 1995]. Moreover, the assumption that Q is positive deﬁnite gives from (3) that Qd (t) is positive deﬁnite, hence the solution to (42) is unique and guaranteed to be positive deﬁnite under the assumptions on A. If Lemma 1 is used directly, a matrix exponential of a matrix of dimension nx (nx + 1) × nx (nx + 1) is required. Now, only the Lyapunov equation (35b) has to be solved, where the dimensions of the matrices are nx × nx . The computational complexity for solving the Lyapunov equation is 35n3x [Higham, 2008]. The total computational complexity for computing the solution of (5b) using Theorem 2 , , is log2 (m) + p + 43 n3x , where log2 (m) + p n3x comes from the matrix exponen3 tial, and 43nx comes from solving the Lyapunov equation (35n3x ) and taking four matrix products giving 2n3x each time. The algebraic Lyapunov function (35b) has a unique solution only if the eigenvalues of A are not mirrored in the imaginary axis [Gajić and Qureshi, 1995], as a result of the assumption that AP is non-singular, and this is the main drawback with using Theorem 2 rather than using Lemma 1. In the case of integrators, the method presented in Wahlström et al. [2014] can be used. To be able to calculate Qd (t), the method transforms the system such that the Lyapunov equation (35b) is used for the subspace without the integrators, and the integral in (3) is used for the subspace containing the integrators. Discrete-time Recursion The recursive solution to the diﬀerential equations in (5) describing the ﬁrst and second order moments of the sde (1) can now be written as x̂((k + 1)h) = eAh x̂(kh), P((k + 1)h) = e Ah P(kh)e (44a) AT h + Qd (h), AQd (h) + Qd (h)AT + GQGT − e Ah GQGT e AT h (44b) = 0, (44c) Equations (44b) to (44c) are derived using t = kh and t = (k + 1)h in (35). The method presented in Theorem 2 is derived straightforwardly from Lemma 1. A similar solution that also solves an algebraic Lyapunov function is presented in Davison [1975]. The main diﬀerence is that Theorem 2 gives a value of the covariance matrix Qd (t) for the discrete-time noise explicitly, as opposed to the 134 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation Table 2: Six variants to calculate P(t). The last column shows the computap tional complexity p in O(nx ) which is described in detail in Section 5.1. Method I II III IV V VI Description P(t) from Lemma 1. P(t) from Theorem 2. P(t) from (4b) with Qd calculated using equation (8). P(t) from (4b) with Qd calculated using an eigenvalue decomposition for diagonalising the integrand in (3). P(t) from (4b) with Qd calculated using numerical integration of the integral in (3) using quadv in Matlab. P(t) from the matrix fraction decomposition in (9). p p in O(nx ) 6 3 3 3 3 3 solution in Davison [1975]. Moreover, the algebraic Lyapunov function in DaviT son [1975] is independent of time, which is not the case here since eAt GQGT e A t changes with time. This is not an issue for the recursive time update due to the T fact that eAh GQGT e A h is only dependent on h, hence the algebraic Lyapunov equation (44c) has to be solved only once. 5 Comparison of Solutions for the CDLE This section will provide rough estimates of the computational complexity of the diﬀerent approaches to compute the cdle, by counting the number of ﬂops. Numerical properties are also discussed. Table 2 summarises six variants of the methods presented in Section 2 of how to calculate P(t). 5.1 Computational Complexity Rough estimates of the computational complexity can be given by counting the number of operations that is required. From Section 4 it is given that the computational complexity for Method I is O(n6x ) and for Method II it is (log2 (m) + p + 43)n3x . The total computational complexity of Method III is roughly (8(log2 (m) + p) + 6)n3x , where (log2 (m) + p)(2nx )3 comes from e Ht and 6n3x from the remaining three matrix products. Using e.g. an eigenvalue decomposition to calculate the integral, i.e., Method IV, gives a computational complexity of O(n3x ). For numerical integration, i.e., Method V, the computational complexity will be O(n3x ) due to the matrix exponential and matrix products. The constant in front of n3x will be larger than for Method III and Method IV because of element-wise integration of the nx × nx symmetric matrix integrand, which requires nx (nx + 1)/2 number of integrations. For the last method Method VI, the same matrix expo- 5 135 Comparison of Solutions for the CDLE 102 101 Time [s] 100 10−1 10−2 I III V 10−3 10−4 101 102 II IV VI 103 Order nx Figure 2: Mean execution time for calculating P(t) for randomly generated state matrices with order nx × nx over 1000 mc simulations. nential as in Method III is calculated which gives (log2 (m) + p)8n3x operations. In addition, 2n3x operations for the matrix inverse and 4n3x operations for the two remaining matrix products are required. In total, the computational complexity is (8(log2 (m) + p) + 6)n3x . The product C(t)M3 (t)−1 can of course be calculated without ﬁrst calculate the inverse and then perform the multiplication, but it is a rough estimate presented here. The computational complexity is also analysed by performing Monte Carlo simulations over 1000 randomly chosen stable systems. The order of the systems are nx = 10, 50, 100, 500, 1000. As expected, the solution using Method I takes very long time as can be seen in Figure 2. For Method I the size has been restricted to nx ≤ 100 since AP grows to much for larger values. However, the computational time using Method I compared to the other methods is clear. The computational time for the other methods are in the same order, which is also expected. As discussed previously, the numerical integration will give a computational complexity that has the same slope but with a higher oﬀset than Method II–IV, and Method VI, which is seen in Figure 2. It can also be noted that the numerical integration for nx = 10 is slower than the Method I. Note that not only the number of operations of performing e.g. the matrix exponential and the matrix multiplications aﬀect the total time. Also, time for memory management is included. However, the slope of the lines for large nx is approximately six for Method I and three for the other methods, which agree 136 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation Table 3: Standard deviation of the execution time for calculating P(t). 10 50 100 500 1000 −5 −2 I 9.63 · 10 7.85 · 10 1.38 – – II 1.88 · 10−5 1.46 · 10−4 8.20 · 10−4 4.37 · 10−2 1.03 · 10−1 III 6.09 · 10−6 8.84 · 10−5 3.71 · 10−4 3.89 · 10−2 2.41 · 10−1 IV 7.67 · 10−5 1.72 · 10−4 7.42 · 10−4 5.14 · 10−2 2.15 · 10−1 V 1.26 · 10−4 4.96 · 10−4 1.68 · 10−3 2.12 · 10−1 1.39 VI 1.95 · 10−5 5.35 · 10−5 3.89 · 10−4 3.69 · 10−2 2.06 · 10−1 with the computational complexity discussed above. The standard deviation for the computation time for the diﬀerent methods is at least one order of magnitude less than the mean value, see Table 3. 5.2 Numerical Properties Here, the numerical properties will be analysed. First, the solution P(t) should hold for any value of t. It means that a large enough value of t should give that P(t) equals the stationary solution given from the stationary Lyapunov equation APstat + Pstat AT + GQGT = 0 (45) Second, the recursive updates should approach Pstat and then stay there when k → ∞. Randomly generated stable system matrices, over 100 mc simulations2 , of order nx = 2 will be used with GQGT = I to show how the methods perform. For the ﬁrst case the value t = 100 has been used and for the second case the sample time h = 0.01 s has been used and a total of 10,000 samples. The stationary matrix Pstat is not obtained for Method III–IV, and Method VI for all mc simulations. However, methods Method I–II and Method V gives Pstat as the solution. The reason that Method III and Method VI cannot give the correct stationary matrix is that they have to calculate the ill-conditioned matrix eHt . For "" the second " case, where the recursive update is used, the norm of diﬀerence "P(t) − Pstat "" for the methods are shown in Figure 3 for the ﬁrst 1000 samples. It can be seen that Method I–V converge to the stationary solution. Method VI is not able to converge to the stationary solution when the time goes by, instead numerical problems occur, giving Inf or NaN (Not-a-Number) as solution. 6 Linear Spring-Damper Example The diﬀerent solutions and approximations described above will be investigated for a linear model of a mass M hanging in a spring and damper, see Figure 4. The 2 Here, only 100 mc simulations are used to be able to visualise the result, otherwise too many samples with Inf or NaN as solution, which cannot be displayed in the ﬁgure. 6 137 Linear Spring-Damper Example 100 "" " "P(t) − Pstat "" Method I–V Method VI 10−1 0 200 600 400 800 1000 Sample Figure 3: The mean norm over 100 mc simulations of the error P(t) − Pstat for the recursive updates. Method I–V gives the same behaviour whereas Method VI diverges equation of motion is M q̈ + d q̇ + kq − M g = 0 (46) where q is the distance from where the spring/damper is unstretched and g = 9.81 is the gravity constant. A linear state space model, using M = 1, with x = T q q̇ is given by 0 0 1 x(t) + . (47) ẋ(t) = g −k −d A 6.1 B Stability Bound on the Sample Time The bound on the sample time that makes the solution to (47) stable, when Euler sampling e1,m (Ah) is used, can be calculated using Theorem 1. The eigenvalues for A are d 1√ 2 λ1,2 = − ± d − 4k. (48) 2 2 138 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation k d g q M Figure 4: A mass hanging in a spring and damper. If d 2 − 4k ≥ 0 the system is well damped and the eigenvalues are real, hence 7 8 2m 2m 2m 2m h < min , , , (49) = √ √ √ d + d 2 − 4k d − d 2 − 4k d d + d 2 − 4k If instead d 2 − 4k < 0, the system is oscillating and the eigenvalues are complex, giving 8 7 dm dm 2m dm , , = , (50) h < min 2k d 2k 2k where we have used the fact that d 2 − 4k < 0 to get the minimum value. The values on the parameters have been chosen as d = 2 and k = 10 giving an oscillating system. The stability bound is therefore h < 0.1m s. 6.2 Kalman Filtering We will now focus on Kalman ﬁltering of the spring-damper example. The continuous-time model (47) is augmented with process noise giving the model dx(t) = Ax(t)dt + B + Gdβ(t), (51) T where A and B are given by (47), G = 0 1 and dβ(t) is a scalar Wiener process with E dβ(t)dβ(t)T = Qdt. Here it is used that Q = 5 · 10−3 . It is assumed that the velocity q̇ is measured with a sample rate Ts . The measurement equation can be written as (52) yk = 0 1 xk + ek = Cxk + ek , where ek ∈ R is discrete-time normal distributed white noise with zero mean and a standard deviation of σ = 0.05. Here, yk = y(kTs ) has been used for notational convenience. It is easy to show that the system is observable with this measurement. The stability condition for the ﬁrst order approximation e1,m (Ah) was calculated to be h < 0.1m seconds in Section 6.1. We chose therefore Ts = h = 0.09 s. T The simulation represents free motion of the mass when starting at x0 = 0 0 . The ground truth data is obtained by simulating the continuous-time sde over 6 139 Linear Spring-Damper Example tmax = 20 s with a sample time hS that is 100 times shorter than Ts . In that case, the Wiener process dβ(t) can at each sample instance be approximated by a normal distributed zero mean white noise process with covariance matrix QhS . Four Kalman ﬁlters are compared where eAh is approximated either by e1,m (Ah) or by the Matlab-function expm. The function expm uses scaling and squaring techniques with a Padé approximation to compute the matrix exponential, see Moler and Van Loan [2003]; Higham [2005]. Moreover, the update of the covariance matrix P(t) is according to the discrete ﬁlter (19b) or according to one of the solutions presented in Sections 2.1 to 2.3. Here, the solution to the cdle given by Theorem 2 has been used, but the other methods would give the same results. Remember though that the matrix fraction method can have numerical problems. In summary, the Kalman ﬁlters are: 1. Fh = e1,m (Ah) and P(k + 1) = Fh P(k)FTh + Gh Qh GTh , 2. Fh is given by the Matlab-function expm and P(k +1) = Fh P(k)FTh +Gh Qh GTh , 3. Fh = e1,m (Ah) and P(k + 1) = Fh P(k)FTh + Qd (h), 4. Fh is given by the Matlab-function expm and P(k + 1) = Fh P(k)FTh + Qd (h), where Qd (h) is the solution to the Lyapunov equation in (44c). The Kalman ﬁlters are initialised with the true x0 , used for ground truth data, plus a normal distributed random term with zero mean and standard deviation 0.1. The state covariance is initialised by P(0) = I. The covariance matrix for the measurement noise is the true one, i.e., R = σ 2 . The covariance matrix for the process noise is diﬀerent for the ﬁlters. For ﬁlter 1 and 2 the covariance matrix Qh/m is used whereas for ﬁlter 3 and 4 the true covariance matrix Q is used. The ﬁlters are compared to each other using NMC = 1000 Monte Carlo simulations for diﬀerent values of m. The oversampling constant m takes the following values: {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 20, 30, 40, 50} Figure 5 shows the root mean square error (rmse) deﬁned according to % & ' tmax 1 MC 2 ρi = ρ i (t) N t=t (53) (54a) 0 where t0 = tmax /2 in order to remove the transients, N is the number of samples in [t0 , tmax ], and % & & ' NMC 2 1 (j) (j) MC ρ i (t) = (54b) xi (t) − x̂i (t) , NMC j=1 (j) xi (t) (j) is the true ith state and x̂i (t) is the estimated ith state for Monte where Carlo simulation number j. The two ﬁlters 1 and 3 give almost identical results 140 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation for the rmse, therefore only ﬁlter 1 is shown in Figure 5, see the solid line. The dashed lines are the rmse for ﬁlter 4 (ﬁlter 2 gives the same result). We can see that a factor of m = 20 or higher is required to get the same result for Euler sampling as for the continuous-time solution3 . The execution time is similar for all four ﬁlters and increases with the same amount when m increases, hence a large enough oversampling can be diﬃcult to achieve for systems with hard realtime requirements. In that case, the continuous-time solution is to prefer. Remark 2. The maximum sample time, derived according to Theorem 1, is restricted by the cdle as is described in the proof. It means that we can use a larger sample time for the ode describing the states, in this particular case a twice as large sample time. Based on this, we already have oversampling by a factor of at least two, for the ode describing the states, when the sample time is chosen according to Theorem 1. In Figure 6 we can see how the norm of the stationary covariance matrix4 for the estimation error changes when oversampling is used. The four ﬁlters converge to the same value when m increases. For the discrete-time update in (19b), i.e., ﬁlter 1 and 2, the stationary value is too large for small values of m. For the continuous-time update in Theorem 2, it can be seen that a ﬁrst order Taylor approximation of the exponential function, i.e., ﬁlter 3, gives a too small covariance matrix which increases when m increases. A too small or too large covariance matrix for the estimation error can be crucial for diﬀerent applications, such as target tracking, where the covariance matrix is used for data association. 7 Extensions to Non-linear Systems We will in this section adapt the results for linear systems to non-linear systems. Inevitably, some approximations have to be done, and the most fundamental one is to assume that the state is constant during the small time steps h/m. This approximation becomes better the larger oversampling factor m is chosen. 7.1 EKF Time Update Let the dynamics be given by the non-linear sde dx(t) = f (x(t))dt + G(x(t))dβ(t), (55) nx ×nβ for t ≥ 0, where x(t) ∈ Rnx , f (x(t)) : Rnx → Rnx , G(x(t)) : Rnx → , and R nβ T dβ(t) ∈ R is a vector of Wiener processes with E dβ(t)dβ(t) = Qdt. For simplicity, it is assumed that G(x(t)) = G. The propagation of the ﬁrst and second order moments for the extended Kalman ﬁlter (ekf) can, as in the linear case, be 3 It is wise to choose m to be a power of 2, as explained in Section 2.5 4 The covariance matrix at time t max is used as the stationary covariance matrix, i.e., P(tmax ). 141 Extensions to Non-linear Systems 0.15 ρ1 0.10 0.05 0 0.6 ρ2 0.4 0.2 0 0 10 20 30 50 40 m Figure 5: rmse according to (54) as a function of the degree of oversampling, where the solid line is ﬁlter 1 (ﬁlter 3 gives the same) and the dashed line is ﬁlter 4 (ﬁlter 2 gives the same). 1.1 Filter 1 Filter 2 Filter 3 Filter 4 0.9 ×10−3 1.0 P(tmax ), 7 0.8 0.7 0.6 0 10 20 30 40 50 m Figure 6: The norm of the stationary covariance matrix for the estimation error for the four ﬁlters, as a function of the degree of oversampling. 142 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation g qm qa ξ Figure 7: A single ﬂexible joint. written as [Jazwinski, 1970] ˙ = f (x̂(t)), x̂(t) (56a) T T Ṗ(t) = F(x̂(t))P(t) + P(t)F(x̂(t)) + GQG , (56b) where F(x̂(t)) is the Jacobian of f (x(t)) evaluated at x̂(t). The main diﬀerences to (5) are that a linear approximation of f (x) is used in the cdle as well as the cdle is dependent on the state vector x. Without any assumptions, the two equations in (56) have to be solved simultaneously. The easiest way is to vectorise (56b) similar to what is described in Appendix A and then solve the nonlinear ode d f (x̂(t)), x̂(t) , (57) = AP (x̂(t))vech P + vech GQGT dt vech P(t) where AP (x̂(t)) = D† (I ⊗ F(x̂(t)) + F(x̂(t)) ⊗ I)D. The non-linear ode can be solved using a numerical solver such as Runge-Kutta methods [Hairer et al., 1987]. If it is assumed that x̂(t) is constant over an interval of length h/m, then the two odes describing x̂(t) and vech P(t) can be solved separately. The ode for x̂(t) is solved using a numerical solver and the ode for vech P(t) becomes a linear ode which can be solved using Theorem 2, where A = F(x̂(t)). Remark 3. When m increases, the length of the interval, where x̂(t) has to be constant, decreases. In that case, the assumption of constant x̂(t) is more valid, hence the two odes can be solved separately without introducing too much errors. Similar extensions for the method using matrix fraction are straightforward to derive. The advantage with the vectorised solution is that it is easy to solve the combined ode for x(t) and vech P(t) using a Runge-Kutta solver. This can be compared to the method using matrix fraction, which becomes a coupled diﬀerential equation with both vector and matrix variables. 7.2 Simulations of a Flexible Joint A non-linear model for a single ﬂexible joint is investigated in this section, see Figure 7. The equations of motion are given by Ja q̈a + G(qa ) + D(q̇a , q̇m ) + T (qa , qm ) =0, (58a) Jm q̈m + F(q̇m ) − D(q̇a , q̇m ) − T (qa , qm ) =u, (58b) 7 143 Extensions to Non-linear Systems Table 4: Model parameters for the non-linear model. k2 fd g J a J m M ξ d k1 1 1 1 1 1 10 100 1 9.81 where the gravity, damping, spring, and friction torques are modelled as G(qa ) = −g M ξ sin(qa ), (59a) D(q̇a , q̇m ) = d(q̇a − q̇m ), (59b) 3 T (qa , qm ) = k2 (qa − qm ) + k1 (qa − qm ) , F(q̇m ) = f d q̇m , (59c) (59d) Numerical values of the parameters, used for simulation, are given in Table 4. The parameters are chosen to get a good system without unnecessary large os T cillations. With the state vector x = qa qm q̇a q̇m a non-linear system of continuous-time odes can be written as ⎛ ⎞ x3 ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ x4 ⎜⎜ ⎟⎟ ⎜ (60) ẋ = ⎜⎜ 1 g M ξ sin(x ) − dΔ − k Δ − k Δ3 ⎟⎟⎟, ⎜⎜ Ja 1 34 2 12 1 12 ⎟⎟⎟⎠ ⎜⎝ 1 3 Jm dΔ34 + k2 Δ12 + k1 Δ12 − f d x4 + u f (x,u) where Δij = xi − xj . The state space model (60) is also augmented with a noise model according to (55) with ⎛ ⎞ 0 ⎟⎟ ⎜⎜ 0 ⎜⎜ 0 0 ⎟⎟⎟⎟ ⎜ G = ⎜⎜⎜ −1 (61) ⎟. ⎜⎜Ja 0 ⎟⎟⎟ ⎝ ⎠ −1 0 Jm For the simulation, the arm is released from rest in the position qa = qm = π/2 T and moves freely, i.e., u(t) = 0, to the stationary point x = π π 0 0 . The ground truth data is obtained using a standard fourth order Runge-Kutta method with a sample time hS = 1 · 10−6 s, which is much smaller than the sample time Ts for the measurements. In the same way as for the linear example in Section 6, the Wiener process dβ(t) can be approximated at each discrete-time instant by a zero mean white noise process with a covariance matrix QhS , where Q = 1 · 10−3 I2 . It is assumed that the motor position qm and velocity q̇m are measured, with additive zero mean Gaussian measurement noise e(kTs ) ∈ R2 with a standard deviation σ = 0.05I2 . The sample time for the measurements is chosen to be Ts = h = 0.1 s. Two extended Kalman ﬁlters (ekf) are compared. The ﬁrst ﬁlter uses the discrete- 144 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation time update (19) where Euler sampling x((k + 1)h) = x(kh) + hf (x(kh), u(kh)) = F(x(kh)) (62) has been used for discretisation. The second ﬁlter solves the continuous-time ode (57) using a standard fourth order Runge-Kutta method. The ﬁlters are initialised with the true x(0) used for simulating ground truth data plus a random term with zero mean and standard deviation 0.1. The covariance matrix for the estimation error is initialised by P(0) = 1 · 10−4 I4 . The results are evaluated over 1000 Monte Carlo simulations using the diﬀerent values of m listed in (53). Figure 8 shows the rmse, deﬁned in (54), for the four states. The discrete-time ﬁlter using Euler sampling requires an oversampling of approximately m = 10 in order to get the same performance as the continuous-time ﬁlter, which is not affected by m that much. In Figure 9, the norm of the stationary covariance matrix of the estimation error, i.e., P(tmax ), is shown. Increasing m, the value P(tmax ) decreases and approaches the corresponding value for the continuous-time ﬁlter. The result is in accordance with the linear model described in Section 6.2. The standard deviation for P(tmax ) is several orders of magnitude less than the mean value and decreases as m increases with a similar rate as the mean value in Figure 9. The execution time for the two ﬁlters diﬀers a lot. They both increase linearly with m and the continuous-time ﬁlter is approximately 4-5 times slower than the discrete-time ﬁlter. This is because of that the Runge-Kutta solver evaluates the function f (x(t)) four times for each time instant whereas the discrete-time ﬁlter evaluates the function F(x(kh)) only once. However, the time it takes for the discrete-time ﬁlter using m = 10 is approximately 1.6 times slower than using m = 1 for the continuous-time ﬁlter. 8 Conclusions This paper investigates the continuous-discrete ﬁltering problem for Kalman ﬁlters and extended Kalman ﬁlters. The critical time update consists of solving one ode and one continuous-time diﬀerential Lyapunov equation (cdle). The problem can be rewritten as one ode by vectorisation of the cdle. The main contributions of the paper are: 1. A survey of diﬀerent ways to solve the linear sde is presented. The diﬀerent methods, presented in Table 2, are compared to each other with respect to stability, computational complexity and numerical properties. 2. Stability condition for Euler sampling of the linear ode which describes the ﬁrst and second order moments of the sde. An explicit upper bound on the sample time is derived such that a stable continuous-time system remains stable after discretisation. The stability condition for higher order of approximations, such as the Runga-Kutta method, is also brieﬂy investigated. 3. A numerical stable and time eﬃcient solution to the cdle that does not 145 Conclusions 10 8 8 ×10−3 6 6 ρ2 , 4 4 2 0 0 8 8 6 6 ×10−2 2 4 ρ4 , ρ3 , ×10−2 ρ1 , ×10−3 10 2 0 0 10 20 30 40 50 4 2 0 0 10 m 20 30 40 50 m Figure 8: rmse according to (54), where the solid line is the discrete-time ﬁlter using Euler sampling and the dashed line is the continuous-time ﬁlter using a Runge-Kutta solver. Euler sampling Runge-Kutta 10−2 P(tmax ) 8 10−3 0 10 20 30 40 50 m Figure 9: The norm of the stationary covariance matrix for the estimation error for the ekf using Euler sampling (solid) and a fourth order RungeKutta method (dashed). 146 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation require any vectorisation. The computational complexity for the straightforward solution, using vectorisation, of the cdle is O(n6x ), whereas the proposed solution, and the methods proposed in the literature, have a complexity of only O(n3x ). The continuous-discrete ﬁltering problem, using the proposed methods, is evaluated on a linear model describing a mass hanging in a spring-damper pair. It is shown that the standard use of the discrete-time Kalman ﬁlter requires a much higher sample rate in order to achieve the same performance as the proposed solution. The continuous-discrete ﬁltering problem is also extended to non-linear systems and evaluated on a non-linear model describing a single ﬂexible joint of an industrial manipulator. The proposed solution requires the solution from a RungeKutta method and without any assumptions, vectorisation has to be used for the cdle. Simulations of the non-linear joint model show also that a much higher sample time is required for the standard discrete-time Kalman ﬁlter to be comparable to the proposed solution. Appendix A Vectorisation of the CDLE The matrix valued cdle Ṗ(t) = AP(t) + P(t)AT + GQGT , (63) can be converted to a vector valued ode using vectorisation of the matrix P(t). P(t) ∈ Rnx ×nx is symmetric so the half-vectorisation is used. The relationship between vectorisation, denoted by vec, and half-vectorisation, denoted by vech, is vec P(t) = Dvech P(t), (64) where D is a n2x × nx (nx + 1)/2 duplication matrix. Let nP = nx (nx + 1)/2 and 9 = GQGT . Vectorisation of (63) gives Q 9 vech Ṗ(t) = vech (AP(t) + P(t)AT + Q) 9 = vech AP(t) + vech P(t)AT + vech Q 9 = D† (vec AP(t) + vec P(t)AT ) + vech Q (69) 9 = D† [(I ⊗ A) + (A ⊗ I)]Dvech P(t) + vech Q 9 = AP vech P(t) + vech Q (65) where ⊗ is the Kronecker product and D† = (DT D)−1 DT is the pseudo inverse of D. Note that D† D = I and DD† I. The solution of the ode (65) is given B Eigenvalues of the Approximated Exponential Function 147 by [Rugh, 1996] t vech P(t) = e AP t vech P(0) + 9 eAP (t−s) ds vech Q. (66) 0 Assume that AP is invertible, then the integral can be computed as t t e 0 AP (t−s) : : d −1 −AP s −AP e = e−AP s ds 0 −AP t AP t −I . = A−1 = eAP t A−1 P I−e P e ds = e AP t e −AP s ds = (67) B Eigenvalues of the Approximated Exponential Function The eigenvalues of ep,m (Ah) as a function of the eigenvalues of A are given in Lemma 2 and Lemma 3 presents the result when p → ∞ if A is Hurwitz. Lemma 2. Let λi and vi be the eigenvalues and eigenvectors, respectively, of A ∈ Rn×n . Then the p’th order Taylor expansion ep,m (Ah) of e Ah is given by p m 1 Ah Ah + ... + ep,m (Ah) = I + m p! m which has the eigenvectors vi and the eigenvalues ⎛ p ⎞m ⎜⎜ h3 λ3i hp λi ⎟⎟ h2 λ2i ⎟⎟ ⎜⎜1 + hλi + + + ... + ⎝ m p!mp ⎠ 2!m2 3!m3 (68) for i = 1, . . . , n. Proof: The result follows from an eigenvalue decomposition of the matrix A. Lemma 3. In the limit p → ∞, the eigenvalues of ep,m (Ah) converge to e hλi , i = 1, . . . , n. If A is Hurwitz (Re {λi } < 0), then the eigenvalues are inside the unit circle. Proof: When p → ∞ the sum in (68) converges to the exponential function e hλi , i = 1, . . . , n. The exponential function can be written as e λi h = e Re{λi }h (cos Im {λi } + i sin Im {λi }) which for Re {λi } < 0 has an absolute value less than 1, hence e λi h is inside the unit circle. 148 C Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation Rules for Vectorisation and the Kronecker Product The rules for vectorisation and the Kronecker product are from Higham [2008] and Lütkepohl [1996]. vec AB = (I ⊗ A)vec B = (BT ⊗ I)vec A T (C ⊗ A)vec B = vec ABC I⊗A+B⊗I=A⊕B e A⊕B = eA ⊗ e B DD† (I ⊗ A + A ⊗ I)D = (I ⊗ A + A ⊗ I)D (69) (70) (71) (72) (73) Acknowledgement This work was supported by the Vinnova Excellence Center LINK-SIC. The authors would like to thank Dr. Daniel Petersson, Linköping University, Sweden, for valuable comments regarding matrix algebra. Bibliography 149 Bibliography I. Arasaratnam and S. Haykin. Cubature Kalman ﬁltering: A powerful tool for aerospace applications. In Proceedings of the International Radar Conference, Bordeaux, France, October 2009. Patrik Axelsson and Fredrik Gustafsson. Discrete-time solutions to the continuous-time diﬀerential Lyapunov equation with applications to Kalman ﬁltering. Submitted to IEEE Transactions on Automatic Control, 2012. Johan Bagterp Jörgensen, Per Grove Thomsen, Henrik Madsen, and Morten Rode Kristensen. A computational eﬃcient and robust implementation of the continuous-discrete extended Kalman ﬁlter. In Proceedings of the American Control Conference, pages 3706–3712, New York City, USA, July 2007. E. J. Davison. The numerical solution of Ẋ = A1 X + XA2 + D, X(0) = C. IEEE Transactions on Automatic Control, 20(4):566–567, August 1975. Paul Frogerais, Jean-Jacques Bellanger, and Lofti Senhadji. Various ways to compute the continuous-discrete extended Kalman ﬁlter. IEEE Transactions on Automatic Control, 57(4):1000–1004, April 2012. Zoran Gajić and Muhammad Tahir Javed Qureshi. Lyapunov Matrix Equation in System Stability and Control, volume 195 of Mathematics in Science and Engineering. Academic Press, San Diego, CA, USA, 1995. Mohinder S. Grewal and Angus P. Andrews. Kalman Filtering. Theory and Practice Using Matlab. John Wiley & Sons, Hoboken, NJ, USA, third edition, 2008. Ernst Hairer, Syvert Paul Nørsett, and Gerhard Wanner. Solving Ordinary Differential Equations I – Nonstiﬀ Problems. Springer Series in Computational Mathematics. Springer-Verlag, Berlin, Heidelberg, Germany, 1987. Nicholas J. Higham. The scaling and squaring method for the matrix exponential revisited. SIAM Journal on Matrix Analysis and Applications, 26(4):1179–1193, June 2005. Nicholas J. Higham. Functions of Matrices – Theory and Computation. SIAM, Philadelphia, PA, USA, 2008. Diederich Hinrichsen and Anthony J. Pritchard. Mathematical Systems Theory I – Modelling, State Space Analysis, Stability and Robustness. Texts in Applied Mathematics. Springer-Verlag, Berlin, Heidelberg, Germany, 2005. Andrew H. Jazwinski. Stochastic Processes and Filtering Theory, volume 64. Academic Press, New York, NY, USA, 1970. Thomas Kailath, Ali H. Sayed, and Babak Hassibi. Linear Estimation. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, 2000. 150 Paper C Solutions to the Continuous-time Diﬀerential Lyapunov Equation J.J. LaViola. A comparison of unscented and extended Kalman ﬁltering for estimating quaternion motion. In Proceedings of the American Control Conference, pages 2435–2440, Denver, CO, USA, June 2003. Helmut Lütkepohl. Handbook of Matrices. John Wiley & Sons, Chichester, West Sussex, England, 1996. Mahendra Mallick, Mark Morelande, and Lyudmila Mihaylova. Continuousdiscrete ﬁltering using ekf, ukf, and pf. In Proceedings of the 15th International Conference on Information Fusion, pages 1087–1094, Singapore, July 2012. Thomas Mazzoni. Computational aspects of continuous-discrete extended Kalman-ﬁltering. Computational Statistics, 23(4):519–539, 2008. Cleve Moler and Charles Van Loan. Nineteen dubious ways to compute the exponential of a matrix, twenty-ﬁve years later. SIAM Review, 45(1):1–46, February 2003. Bin Rao, Shunping Xiao, Xuesong Wang, and Yongzhen Li. Nonlinear Kalman ﬁltering with numerical integration. Chinese Journal of Electronics, 20(3):452– 456, July 2011. H. J. Rome. A direct solution to the linear variance equation of a time-invariant linear system. IEEE Transactions on Automatic Control, 14(5):592–593, October 1969. Wilson J. Rugh. Linear System Theory. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, second edition, 1996. Simo Särkkä. Recursive Bayesian Inference on Stochastic Diﬀerential Equations. PhD thesis, Helsinki University of Technology, Finland, April 2006. ISBN 9512-28127-9. Simo Särkkä. On unscented Kalman ﬁltering for state estimation of continuoustime nonlinear systems. IEEE Transactions on Automatic Control, 52(9):1631– 1641, September 2007. Charles F. Van Loan. Computing integrals involving the matrix exponential. IEEE Transactions on Automatic Control, 23(3):395–404, June 1978. Niklas Wahlström, Patrik Axelsson, and Fredrik Gustafsson. Discretizing stochastic dynamical systems using lyapunov equations. arXiv:1402.1358., 2014. P. Zhang, J. Gu, E.E. Milios, and P. Huynh. Navigation with IMU/ GPS/digital compass with unscented Kalman ﬁlter. In Proceedings of the IEEE International Conference on Mechatronics and Automation, pages 1497–1502, Niagara Falls, Ontario, Canada, July–August 2005. Paper D ML Estimation of Process Noise Variance in Dynamic Systems D Authors: Norrlöf Patrik Axelsson, Umut Orguner, Fredrik Gustafsson, and Mikael Edited version of the paper: Patrik Axelsson, Umut Orguner, Fredrik Gustafsson, and Mikael Norrlöf. ml estimation of process noise variance in dynamic systems. In Proceedings of the 18th IFAC World Congress, pages 5609–5614, Milano, Italy, August/September 2011. ML Estimation of Process Noise Variance in Dynamic Systems Patrik Axelsson∗ , Umut Orguner∗∗ , Fredrik Gustafsson∗ , and Mikael Norrlöf∗ ∗ Dept. ∗∗ Dept. of Electrical & Electronics Engineering, Middle East Technical University, 06531 Ankara, Turkey [email protected] of Electrical Engineering, Linköping University, SE–581 83 Linköping, Sweden [email protected], [email protected], [email protected] Abstract The performance of a non-linear ﬁlter hinges in the end on the accuracy of the assumed non-linear model of the process. In particular, the process noise covariance Q is hard to get by physical modelling and dedicated system identiﬁcation experiments. We propose a variant of the expectation maximisation (em) algorithm which iteratively estimates the unobserved state sequence and Q based on the observations of the process. The extended Kalman smoother (eks) is the instrument to ﬁnd the unobserved state sequence. Our contribution ﬁlls a gap in literature, where previously only the linear Kalman smoother and particle smoother have been applied. The algorithm will be important for future industrial robots with more ﬂexible structures, where the particle smoother cannot be applied due to the high state dimension. The proposed method is compared to two alternative methods on a simulated robot. 1 Introduction Joint parameter identiﬁcation and state estimation in state space model is an important branch of system identiﬁcation [Ljung, 1999]. During the last decade, subspace based approaches for estimating fully parametrised linear state space models (so called black box models) have been well explored [Ljung, 1999]. At the same time, the theory of grey-box identiﬁcation of uncertain parameters in physical models has been developed [Bohlin, 2006]. The model is here a nonlinear state space model without process noise. The basic idea is that the system can be simulated for each value of the parameter vector, and the simulated output can be compared to the observed measurements, where for instance the maximum likelihood estimate (mle) is computed. The situation with process noise is considerably harder, since the simulated output has to be replaced with a smoothing ﬁlter for the state sequence. A further problem is that the likeli153 154 Paper D ML Estimation of Process Noise Variance in Dynamic Systems hood function becomes rather complicated. The em algorithm in Dempster et al. [1977] provides a method to compute the mle by separating the smoothing and parameter estimation problems. It has been explored for linear Gaussian models, where the system matrices (A, C, Q, R) are estimated using the Kalman smoother as the state estimator [Cappé et al., 2005]. For non-linear models, there is ongoing research on using the particle smoother to estimate the parameters in a non-linear dynamic model [Schön et al., 2011]. However, the particle smoother is not applicable for models with high state dimension. Here we propose to use a linearised model for state estimation, leading to an extended Kalman smoother (eks). The em algorithm will thus be approximate in the same way as the eks. We focus on the process noise covariance matrix, which is the hardest one to assess in the modelling phase. Our application in mind is industrial robots, where inertia, ﬂexibilities and friction parameters in each joint are all rather straightforwardly identiﬁed by dedicated experiments, see Wernholt [2007] and Carvalho Bittencourt et al. [2010]. The sensor noise covariance is also quite easy to get. Process noise, on the other hand, models quite complex phenomena as well as model uncertainties. The motivation to do this for industrial robots is the development of new robots with increased elasticity and larger individual variations, such as variation of gearbox stiﬀness or in the parameters describing the mechanical arm. To maintain or improve the robot performance, the motion control must be improved for this new generation of robots. For robots with traditional measurement systems, where only the motor angular position is measured, this can be obtained by improving the model-based control as described in Björkman et al. [2008]. Another option is to use inertial sensors to improve the estimation of the robot tool position and velocity, which requires good knowledge about the noise. The estimated state trajectories can be used for on-line feedback control as a mean of increasing both the robust and the nominal performance of the robot. Another possible use of tool estimation is iterative learning control (ilc) [Wallén et al., 2009]. Section 2 gives a short introduction to the problem and the em algorithm. The calculations for the em algorithm are then given in Section 3. Two alternative methods are presented in Section 4. Section 5 describes a robot model which is used in Section 6 to compare the three methods. Finally, Section 7 concludes the paper. 2 Problem Formulation This paper is focused on a model structure given by xk+1 = F1 (xk , uk ) + F2 (xk )wk , yk = h(xk , uk ) + vk , (1a) (1b) 2 155 Problem Formulation where xk ∈ Rnx , yk ∈ Rny , wk ∼ N (0, Q) and vk ∼ N (0, R). All model parameters ny n are assumed to be known except for Q ∈ S+v and R ∈ S++ 1 . Assume also that F2 (xk ) has the following structure 0 . (2) F2 (xk ) = 9 F2 (xk ) This type of model structure is common for mechanical systems derived by Newton’s law or Lagrange’s equation. One approach to ﬁnd the covariance matrices R and Q is to use the well known Maximum likelihood (ml) method. The idea with the ml method is to ﬁnd the unknown parameters θ such that the measurements y1:N = {y1 , . . . , yN } become as likely as possible. In other words, θ̂ ml = arg max p θ (y1:N ), θ∈Θ (3) where p θ (y1:N ) is the probability density function (pdf) of the observations parametrised with the parameter θ. It is common to take the logarithm of the pdf, Lθ (y1:N ) = log p θ (y1:N ), (4) and ﬁnd the parameter θ that maximises (4), i.e., θ̂ ml = arg max Lθ (y1:N ). θ∈Θ (5) These two problems are equivalent since the logarithm is a monotonic function. The ml problem can be solved using a standard optimisation method, see e.g. Boyd and Vandenberghe [2009]. The solution can however be hard to ﬁnd which has lead to the development of the expectation maximisation (em) algorithm. The em algorithm was originally invented by Dempster et al. [1977]. Theory and examples for the em algorithm can be found in McLachlan and Krishnan [2008], see also Gibson and Ninness [2005] for robust estimation of lti state space models. In Schön et al. [2011], a solution of the more complicated problem to estimate non-linear state space models is given, using a particle smoother. As mentioned before, the particle smoother cannot be applied if the state dimension is too high. 2.1 The Expectation Maximisation Algorithm The principal idea with the em algorithm is to introduce variables x1:N which are not observed directly. The variables x1:N can instead be observed indirectly from y1:N . Take now the joint log-likelihood function Lθ (y1:N , x1:N ) = log p θ (y1:N , x1:N ) p 1 Sp ++ S+ is the set of all symmetric positive deﬁnite (semi-deﬁnite) p × p matrices (6) 156 Paper D ML Estimation of Process Noise Variance in Dynamic Systems of the observed variables y1:N and the variables x1:N . Equation (6) cannot be used directly since x1:N are unknown. Instead, calculate Γ(θ; θ l ) = Eθ l [log p θ (y1:N , x1:N )|y1:N ] , (7) where Eθ l [ · | · ] is the conditional mean with respect to a pdf deﬁned by the parameter θ l . It can now be shown, see e.g. Schön et al. [2011], that any θ, such that Γ(θ; θ l ) > Γ(θ l ; θ l ), (8) Lθ (y1:N ) > Lθ l (y1:N ). (9) implies that Hence, the em algorithm provides a sequence θ l , l = 1, 2, . . ., which approximates ml θ̂ better and better for every iteration. The em algorithm is summarised in Algorithm 1. A possible stop criterion for the em algorithm could be when the change in θ, between two iterations, is small enough. Algorithm 1 The Expectation Maximisation (em) Algorithm 1: 2: Select an initial value θ0 and set l = 0. Expectation Step (E-step): Calculate Γ (θ; θ l ) = Eθ l [log p θ (y1:N , x1:N )|y1:N ] . 3: Maximisation Step (M-step): Compute θ l+1 = arg max Γ (θ; θ l ) . 4: If converged, stop. If not, set l = l + 1 and go to step 2. θ∈Θ 3 EM for Covariance Estimation This section describes how the covariance matrices for the process and measurement noise in (1) can be estimated using the em algorithm. It is not possible to simultaneously estimate both the covariance matrix Q for the process noise and the covariance matrix R for the measurement noise, since it is the scaling between them that aﬀects the estimate when an extended Kalman ﬁlter (ekf) [Anderson and Moore, 1979] is used. Therefore, estimate ﬁrst R with dedicated experiments and then Q with the em algorithm. The matrix R can be estimated according to the following steps. 1. Perform experiments and select a constant segment 9 y of the measured signal y. 2. Calculate vk = 9 yk − ȳ, where ȳ is the mean of 9 y. 3. Finally, R= N 1 vk vTk . N k=1 (10) 3 157 EM for Covariance Estimation The matrix R can now be used in the em algorithm to estimate Q. Equation (1) can also be expressed in the more general conditional densities as T xk+1 ∼ p(xk+1 |xk ) = N xk+1 ; F1,k , F2,k QF2,k , (11a) yk ∼ p(yk |xk ) = N (yk ; hk , R) , (11b) where N ( · ) is the multivariate normal distribution function. The multivariate normal distribution for the n-dimensional variable μ with mean μ̄ and covariance Σ is deﬁned according to N (μ; μ̄, Σ) = 1 T −1 1 e − 2 (μ−μ̄) Σ (μ−μ̄) . (2π)n/2 |Σ|1/2 (12) The short notation F1,k = F1 (xk , uk ), F2,k = F2 (xk ), hk = h(xk , uk ), is sometimes used for simplicity in the sequel. Proceed now with the derivation of the expectation and maximisation steps in Algorithm 1, where θ = Q is the sought parameter. 3.1 Expectation step The joint likelihood can easily be written as pQ (y1:N , x1:N ) = pQ (x1 , y1 ) N pQ (yi |xi )pQ (xi |xi−1 ), (13) i=2 where p(xk , yk |x1:k−1 , y1,k−1 ) = p(xk , yk |xk−1 ) = p(yk |xk )p(xk |xk−1 ) (14) has been used repeatedly. Taking the logarithm of (13) and using (11) together with (12) give ⎛ ⎞ N ⎜⎜ † ⎟⎟⎟ 1 ⎜ T ⎟⎟ L+ log ⎜⎜⎜⎜ λj F2,i−1 QF2,i−1 LQ (y1:N , x1:N ) = log pQ (y1:N , x1:N ) = 9 ⎟⎟ 2 ⎝ ⎠ i=2 † 1 9T T 91,i , F1,i F2,i−1 QF2,i−1 F 2 λj 0 N − (15) i=2 where 9 L is an expression independent of Q, † is the Moore-Penrose inverse [Mitra and Rao, 1971], ; 91,i = xi − F1,i−1 , F (16) and λj 0 λj ( · ) means to take the product of all non-zero eigenvalues. The structure of F2 in (2) gives † † 0 0 T , (17) F2,i−1 QF2,i−1 = 92,i−1 Q F 9T 0 F 2,i−1 158 Paper D which leads to λj ML Estimation of Process Noise Variance in Dynamic Systems T F2,i−1 QF2,i−1 † λj 0 † 9T 92,i−1 Q F = F 2,i−1 . (18) The joint log-likelihood function (15) can now be written as N † 1 92,i−1 Q F 9T LQ (y1:N , x1:N ) =9 L+ log F 2,i−1 2 i=2 − N 1 2 † 9T F2,i−1 QF T 9 F 1,i 2,i−1 F1,i . (19) i=2 Next step is to calculate the expectation of LQ (y1:N , x1:N ) to obtain Γ(Q; Ql ). N < = † 1 9T 92,i−1 Q F EQl log F Γ(Q; Ql ) = EQl LQ (y1:N , x1:N )|y1:N = L̄ + y 2,i−1 1:N 2 − 1 tr 2 N i=2 EQl < i=2 T F2,i−1 QF2,i−1 † = 91,i F 9T y1:N F 1,i where L̄ is independent of Q. The trace operator comes from the fact that † † 91,i = tr F 91,i . 9T F2,i−1 QF T 9T F2,i−1 QF T F F F 1,i 2,i−1 1,i 2,i−1 Start with the calculations of the ﬁrst expectation in (20). < = † 92,i−1 Q F 9T y EQl log F 2,i−1 1:N 9T (xi−1 ) † pQ (xi−1 |y1:N ) dxi−1 . 92 (xi−1 )QF = log F 2 l (20) (21) (22) The integral cannot be solved analytically. Instead, an approximation has to be made. The smoothed density of xi−1 has a high peak around the smoothed estimate if the sampling frequency and the snr are high. Here, we use the extended Kalman smoother (eks), see Yu et al. [2004]. We can therefore approximate xi−1 with the smoothed states x̂si−1|N , in other words, < = † † F s T s 9T 9 92,i−1 Q F 9 . ) (x̂ )Q F (x̂ y (23) EQl log F ≈ log 2 i−1|N 2,i−1 1:N 2 i−1|N The second expectation in (20) can be written as < = † T T 9 9 EQl F2,i−1 QF2,i−1 F1,i F1,i y1:N † 91,i F 9T pQ (xi , xi−1 |y1:N ) dxi dxi−1 . = F2 (xi−1 )QF2T (xi−1 ) F 1,i l (24) 3 159 EM for Covariance Estimation Now use the smoothed density again and let † † F2 (xi−1 )QF2T (xi−1 ) ≈ F2 (x̂si−1|N )QF2T (x̂si−1|N ) . (25) We have now < = † T T 9 9 EQl F2,i−1 QF2,i−1 F1,i F1,i y1:N † 9T pQ (xi , xi−1 |y1:N ) dxi dxi−1 , 91,i F F ≈ F2 (x̂si−1|N )QF2T (x̂si−1|N ) 1,i l (26) where pQl (xi , xi−1 |y1:N ) can be seen as the smoothed density of the augmented T state vector ξ i = xTi−1 xTi , i.e., s ξ,s p Ql (xi , xi−1 |y1:N ) = pQl (ξ i |y1:N ) = N ξ i ; ξ̂ i|N , Pi|N . (27) The ﬁrst and second order moments of the smoothed ξ i can be expressed as ⎞ ⎛ s Psi−1,i|N ⎟⎟ ⎜⎜ Psi−1|N x̂i−1|N s ξ,s ⎟⎟ , T ξ̂ i|N = , Pi|N = ⎜⎜⎜⎝ s (28) ⎟ x̂si|N Psi|N ⎠ Pi−1,i|N where x̂si−1|N , x̂si|N , Psi−1|N and Psi|N are the ﬁrst and second order moments of the smoothed x̂i−1 and x̂i respectively. These are obtained if the augmented model xk ξ k+1 = (29) F1 (xk , uk ) is used in the eks. The integral in (26) cannot be solved analytically. Instead, a ﬁrst order Taylor expansion of F1 (xi−1 ) around x̂si−1|N is used. The expression 91,i F 9T in (26) can now be written as F 1,i 91,i F 9T = xi − F1 (xi−1 ) xi − F1 (xi−1 ) T F 1,i ≈ xi − F1 (x̂si−1|N ) − J1,i−1 · (xi−1 − x̂si−1|N ) T × xi − F1 (x̂si−1|N ) − J1,i−1 · (xi−1 − x̂si−1|N ) T s s T −J1,i−1 I = −J1,i−1 I ξ i − ξ̂ i|N ξ i − ξ̂ i|N T s + −J1,i−1 I ξ i − ξ̂ i|N x̂si|N − F1 (x̂si−1|N ) T s T −J1,i−1 I + x̂si|N − F1 (x̂si−1|N ) ξ i − ξ̂ i|N T + x̂si|N − F1 (x̂si−1|N ) x̂si|N − F1 (x̂si−1|N ) , (30) where the Taylor expansion F1 (xi−1 ) ≈ F1 (x̂si−1|N ) + J1,i−1 · (xi−1 − x̂si−1|N ), ∂F1 (x) J1,i−1 = , ∂x s x=x̂i−1|N (31a) (31b) 160 Paper D ML Estimation of Process Noise Variance in Dynamic Systems has been used. The integral in (26) now becomes 9T pQ (xi , xi−1 |y1:N ) dxi dxi−1 = −J1,i−1 91,i F Mi = F 1,i l I Pξ,s −J1,i−1 i|N T + x̂si|N − F1 (x̂si−1|N ) x̂si|N − F1 (x̂si−1|N ) . T I (32) It is thus possible to calculate Γ(Q; Ql ) according to N T 1 † F + 9† (x̂s 9† (x̂s Q ) + log F ) log 2 i−1|N 2 i−1|N 2 1 log 2 N Γ(Q; Ql ) =L̄ + i=2 − 1 tr Q† 2 i=2 N T F2† (x̂si−1|N )Mi F2† (x̂si−1|N ) , (33) i=2 where we have used the fact that † T † 9† 9T 9† 92,i−1 Q F = F F 2,i−1 2,i−1 Q F2,i−1 . (34) 9 9 9T In (34) it is used that F 2,i−1 and Q have full row rank, and F2,i−1 and F2,i−1 Q have full column rank, see Mitra and Rao [1971]. 3.2 Maximisation step Maximisation with respect to Q is the same as maximisation with respect to Q† = Q−1 . Take the derivative of (33) with respect to Q−1 and let the result be equal to zero gives T ∂Γ(Q; Ql ) N − 1 1 † s = F2 (x̂i−1|N )Mi F2† (x̂si−1|N ) = 0. Q − −1 2 2 ∂Q N (35) i=2 The solution of the maximisation step is now obtained as T 1 † s F2 (x̂i−1|N )Mi F2† (x̂si−1|N ) . N −1 N Ql+1 = (36) i=2 3.3 Stop Criterion The stop criterion can be chosen in diﬀerent ways. Section 2.1 suggests that the em algorithm stops when the diﬀerence in the new and previous estimate is less than a threshold. Another way is to use LQ (y1:N ) in (4). The main problem is to maximise LQ (y1:N ), therefore stop the algorithm when no increase in LQ (y1:N ) can be observed. Equation (4) can be written as ⎛ ⎞ N −1 ⎜⎜ ⎟⎟ LQ (y1:N ) = log pQ (y1:N ) = log ⎜⎜⎜⎝p(y1 ) pQ (yi+1 |y1:i )⎟⎟⎟⎠ i=1 4 Alternative Ways to Find the Covariance Matrix of the Process Noise = log p(y1 ) + N −1 log pQ (yi+1 |y1:i ), 161 (37) i=1 where Bayes’ rule has been used repeatedly. Here, log p(y1 ) is a constant and can be omitted in the sequel for simplicity. The pdf pQ (yi+1 |y1:i ) is identiﬁed as the pdf for the innovations which can be calculated as pQ (yi+1 |y1:i ) = N yi+1 ; h(x̂i+1|i ), Hi+1 Pi+1|i HTi+1 + R , (38) ∂h(x) Hi+1 = , (39) ∂x x=x̂i+1|i where x̂i+1|i and Pi+1|i are calculated in the ekf during the measurement update. The algorithm can now be stopped when L (y ) − L (y ) ≤ γ, (40) Ql 1:N Ql−m 1:N where m and γ are parameters to choose. 4 Alternative Ways to Find the Covariance Matrix of the Process Noise There are many alternative ways of estimating the covariance matrix for the process noise and here two examples will be described. These two alternatives, which are less complicated than the em algorithm, will be compared to the result of the em algorithm in Section 6. The ﬁrst method, presented in Algorithm 2, minimises the path error $ ek = |xk − x̂k |2 + |yk − ŷk |2 , (41) where xk and yk are the true coordinates for the tool, and x̂k and ŷk are the estimated coordinates for the tool. To simplify the problem, the covariance matrix is parametrised as a diagonal matrix. The notation (x̂, ŷ) = ekf(Q) in Algorithm 2 denotes that the estimated position is a function of Q. A standard optimisation method can be used to solve step 2 in Algorithm 2, see e.g. Boyd and Vandenberghe [2009]. The problem is not convex, i.e., the solution is not guaranteed to give a global optimum. However, the method is straightforward and has been used before, see Henriksson et al. [2009] and Axelsson [2009]. One disadvantage with the method is that the true tool position is required. The second method starts with an initial guess Q0 . The smoothed states are then calculated using Q0 . After that, equation (1a) and the smoothed states are used in order to derive the noise wk , k = 1, . . . , N . The covariance matrix is ﬁnally obtained from the vector w1:N = {w1 , . . . , wN }. The method is repeated with the new Q-matrix until convergence is obtained. The method is summarised Algorithm 3. 162 Paper D ML Estimation of Process Noise Variance in Dynamic Systems Algorithm 2 Minimisation of the path error Select an initial diagonal matrix Q0 ∈ R4×4 . $ N 2 2: Minimise k=1 |ek | subject to λ j > 0, j = 1, . . . , 4, (x̂, ŷ) = ekf(Q), and Q = diag (λ1 , λ2 , λ3 , λ4 ) Q0 . 3: The sought covariance matrix Q is obtained as Q = diag λ∗1 , λ∗2 , λ∗3 , λ∗4 Q0 where λ∗j , j = 1, . . . , 4, are the optimal values from step 2. 1: Algorithm 3 Iterative covariance estimation with eks 1: 2: 3: Select an initial value Q0 and set l = 0. Use the eks with Ql . Calculate the noise according to wk = F2† (x̂sk|N ) x̂sk+1|N − F1 (x̂sk|N , uk ) . 4: Let Ql+1 be the covariance matrix for wk according to N 1 Ql+1 = wk wTk . N 5: If converged, stop, If not, set l = l + 1 and go to step 2. k=1 5 Application to Industrial Robots The robot model is a joint ﬂexible two axes model from Moberg et al. [2008]. The model assumes rigid links and ﬂexible joints. Each joint is a two mass system consisting of two angles, the arm angle qai , and the motor angle qmi . Let the state vector be given by T T (42) x = xT1 xT2 xT3 xT4 = qTa qTm q̇Ta q̇Tm , T T where qa = qa1 qa2 , qm = qm1 qm2 , contain the arm angles qa and the motor angles qm of both joints. The model accounts for ﬂexibilities in the joints via non-linear stiﬀness and linear viscous damping. The model also includes nonlinear friction. A continuous-time state space model of the system is given by, ⎞ ⎛ x3 ⎟⎟ ⎜⎜ ⎟⎟ ⎜⎜⎜ x 4 ⎟⎟ ⎜ (43) ẋ = ⎜⎜⎜M −1 (x ) − C(x , x ) − G(x ) − N (x) + w ⎟⎟⎟ , ⎜⎜ a 1 1 a ⎟ ⎟⎟ 1 3 ⎜⎝ ⎠ M−1 m N (x) + F(x4 ) + u + wm where N (x) = D · (x3 − x4 ) + T (x1 , x2 ). N (x) accounts for the ﬂexibilities in the joints, via the linear viscous damping D · (x3 − x4 ) and the non-linear stiﬀness T (x1 , x2 ). In other words, if we dispense with N (x), we are back at a standard rigid robot model. Furthermore, Ma (x1 ) and Mm are the mass matrices for the arm and motor, C(x1 , x3 ) accounts for the centrifugal and centripetal torques, 6 Simulation Results 163 and G(x1 ) accounts for the eﬀect of gravity on the links. The non-linear friction is described by F(x3 ), u represents the motor torque applied to the robot and T w = wTa wTm is the process noise. An Euler forward approximation of (43) gives a discretised model according to (1) and (2). The rank conditions in order to use (34) are also satisﬁed. 6 Simulation Results The method in Section 3 is evaluated and compared to the two alternative methods described in Section 4. The model given in Section 2 is ﬁrst simulated, according to Axelsson [2009], to get all the required quantities, i.e., uk , yk , xk and yk . In system identiﬁcation, it is common to estimate a certain parameter or parameters starting at diﬀerent initial values and see if the true one is obtained. Here, on the other hand, there is no information about the true covariance matrix, even for simulated data. Instead, the estimated covariance matrices, for diﬀerent initial values, are used to calculate the path error according to (41). When the path error diﬀers a lot with diﬀerent initial values it means that the method converges to diﬀerent local optima. There is however no guarantee that a solution is in a global optimum although the path errors do not diﬀer. Here, the maximum and minimum of the 2-norm of the path error are used to see how much the solutions diﬀer with diﬀerent initial values. It is preferred to have a method that results in small and similar path errors for diﬀerent initial values. Table 1 shows that the maximal and minimal path errors for the em algorithm are more or less the same. The same concerns Algorithm 3. The em algorithm gives however a lower path error. Algorithm 2 gives, on the other hand, path errors that diﬀers considerably. This can also be seen in Figure 1. This means that Algorithm 2 gets stuck in diﬀerent local optima. A comparison between the path errors for the em algorithm, Algorithm 3 and the best solution of Algorithm 2 is shown in Figure 2. The em algorithm is clearly much better than the two alternatives. It is also interesting to see how (37) looks like for Ql , l = 0, . . ., both for the em algorithm and Algorithm 3. The em algorithm and Algorithm 3 were therefore forced to take more iterations than necessary. The log-likelihood function (37) can be seen in Figure 3 for 100 iterations. We see that the curve for the em algorithm ﬂattens out somewhere around 50 iterations and stays constant after that. It means that it is unnecessary to continue to more than about 60 iterations. One Table 1: Max and min of the 2-norm of the path error in mm for the three diﬀerent methods. Max Min em 0.2999 0.2996 Alg. 2 3.3769 1.5867 Alg. 3 2.6814 2.6814 164 Paper D ML Estimation of Process Noise Variance in Dynamic Systems 0.18 0.16 Path error [mm] 0.14 0.12 0.1 0.08 0.06 0.04 0.02 0 0 0.2 0.6 0.4 0.8 1 Time [s] Figure 1: The path error for 10 Monte Carlo simulations of Algorithm 2. 0.16 0.14 Path error [mm] 0.12 0.1 0.08 0.06 0.04 0.02 0 0 0.2 0.6 0.4 0.8 1 Time [s] Figure 2: The best path error for the em algorithm (solid), Algorithm 2 (dashed) and Algorithm 3 (dash-dot). 7 165 Conclusions and Future Work 11.26 11.24 LQ (y1:N ) 11.22 11.20 11.18 11.16 11.14 0 20 40 60 80 100 em iteration Figure 3: The log-likelihood function for the ﬁrst 100 iterations in the em algorithm (solid) and Algorithm 3 (dash-dot). thing to comment is the peak around 10 iterations in the curve. This contradicts the property of the em algorithm that the sequence Ql , l = 0, . . ., approximates Q̂ml better and better. This can be explained by the approximations that have been made during the expectation step and that the calculation of (37) in the ekf is approximately. The curve for Algorithm 3 ﬂattens out after 10 iterations and stays constant after that. Algorithm 3 is also without any peak and the stationary value is lower than for the em algorithm. 7 Conclusions and Future Work Three diﬀerent methods to estimate the covariance matrices have been compared. The em algorithm derived in Section 3 gives a lower path error, considering the true path and the estimated path from an ekf. The em algorithm is also robust to changes in the initial value. One advantage with the em algorithm is that no true tool position is needed, which is the case for Algorithm 2. A next step is to use the em algorithm on experimental data to estimate the covariance matrices. Acknowledgement This work was supported by the Swedish Research Council under the Linnaeus Center CADICS and by the Vinnova Excellence Center LINK-SIC. 166 Paper D ML Estimation of Process Noise Variance in Dynamic Systems Bibliography Brian D. O. Anderson and John B. Moore. Optimal Filtering. Information and System Sciences Series. Prentice Hall Inc., Englewood Cliﬀs, NJ, USA, 1979. Patrik Axelsson. A simulation study on the arm estimation of a joint ﬂexible 2 dof robot arm. Technical Report LiTH-ISY-R-2926, Department of Electrical Enginering, Linköping University, SE-581 83 Linköping, Sweden, December 2009. Patrik Axelsson, Umut Orguner, Fredrik Gustafsson, and Mikael Norrlöf. ml estimation of process noise variance in dynamic systems. In Proceedings of the 18th IFAC World Congress, pages 5609–5614, Milano, Italy, August/September 2011. Mattias Björkman, Torgny Brogårdh, Sven Hanssen, Sven-Erik Lindström, Stig Moberg, and Mikael Norrlöf. A new concept for motion control of industrial robots. In Proceedings of the 17th IFAC World Congress, pages 15714–15715, Seoul, Korea, July 2008. Torsten Bohlin. Practical Grey-box Process Identiﬁcation, Theory and Applications. Advances in Industrial Control. Springer, London, UK, 2006. Stephen Boyd and Lieven Vandenberghe. Convex Optimization. Cambridge University Press, Cambridge, United Kingdom, ﬁrst edition, 2009. Olivier Cappé, Eric Moulines, and Tobias Rydén. Inference in Hidden Markov Models. Springer Series in Statistics. Springer, New York, NY, USA, 2005. André Carvalho Bittencourt, Erik Wernholt, Shiva Sander-Tavallaey, and Torgny Brogårdh. An extended friction model to capture load and temperature eﬀects in robot joints. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 6161–6167, Taipei, Taiwan, October 2010. Arthur Dempster, Nan Laird, and Donald Rubin. Maximum likelihood from incomplete data via the em algorithm. Journal of the Royal Statistical Society, Series B, 39(1):1–38, 1977. Stuart Gibson and Brett Ninness. Robust maximum-likelihood estimation of multivariable dynamic systems. Automatica, 41(10):1667–1682, October 2005. Robert Henriksson, Mikael Norrlöf, Stig Moberg, Erik Wernholt, and Thomas B. Schön. Experimental comparison of observers for tool position estimation of industrial robots. In Proceedings of the 48th IEEE Conference on Decision and Control, pages 8065–8070, Shanghai, China, December 2009. Lennart Ljung. System Identiﬁcation, Theory for the User. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, second edition, 1999. Bibliography 167 Geoﬀrey J. McLachlan and Thriyambakam Krishnan. The em Algorithm and Extensions. Wiley Series in Probability and Statistics. John Wiley & Sons, Hoboken, NJ, USA, second edition, 2008. Sujit Kumar Mitra and C. Radhakrishna Rao. Generalized Inverse of Matrices and its Applications. Wiley Series in Probability and Mathematical Statistics. John Wiley & Sons, 1971. Stig Moberg, Jonas Öhr, and Svante Gunnarsson. A benchmark problem for robust control of a multivariable nonlinear ﬂexible manipulator. In Proceedings of the 17th IFAC World Congress, pages 1206–1211, Seoul, Korea, July 2008. Thomas B. Schön, Adrian Wills, and Brett Ninness. System identiﬁcation of nonlinear state-space models. Automatica, 47(1):39–49, January 2011. Johanna Wallén, Svante Gunnarsson, Robert Henriksson, Stig Moberg, and Mikael Norrlöf. ilc applied to a ﬂexible two-link robot model using sensorfusion-based estimates. In Proceedings of the 48th IEEE Conference on Decision and Control, pages 458–463, Shanghai, China, December 2009. Erik Wernholt. Multivariable Frequency-Domain Identiﬁcation of Industrial Robots. Linköping Studies in Science and Technology. Dissertations No. 1138, Linköping University, SE-581 83 Linköping, Sweden, November 2007. http://www.control.isy.liu.se/publications/. Byron M. Yu, Krishna V. Shenoy, and Maneesh Sahani. Derivation of extended Kalman ﬁltering and smoothing equations. URL: http://www-npl. stanford.edu/~byronyu/papers/derive_eks.pdf, 19 October 2004. Paper E H∞-Controller Design Methods Applied to One Joint of a Flexible Industrial Manipulator E Authors: Patrik Axelsson, Anders Helmersson and Mikael Norrlöf Edited version of the paper: Patrik Axelsson, Anders Helmersson, and Mikael Norrlöf. H∞ -controller design methods applied to one joint of a ﬂexible industrial manipulator. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014b. H∞-Controller Design Methods Applied to One Joint of a Flexible Industrial Manipulator Patrik Axelsson, Anders Helmersson and Mikael Norrlöf Dept. of Electrical Engineering, Linköping University, SE–581 83 Linköping, Sweden [email protected], [email protected], [email protected] Abstract Control of a ﬂexible joint of an industrial manipulator using H∞ design methods is presented. The considered design methods are i) mixed-H∞ design, and ii) H∞ loop shaping design. Two diﬀerent controller conﬁgurations are examined: one uses only the actuator position, while the other uses the actuator position and the acceleration of the end-eﬀector. The four resulting controllers are compared to a standard pid controller where only the actuator position is measured. The choices of the weighting functions are discussed in details. For the loop shaping design method, the acceleration measurement is required to improve the performance compared to the pid controller. For the mixed-H∞ method it is enough to have only the actuator position to get an improved performance. Model order reduction of the controllers is brieﬂy discussed, which is important for implementation of the controllers in the robot control system. 1 Introduction The requirements for controllers in modern industrial manipulators are that they should provide high performance, at the same time, robustness to model uncertainty. In the typical standard control conﬁguration the actuator positions are the only measurements used in the higher level control loop. At a lower level, in the drive system, the currents and voltages in the motor are measured to provide torque control for the motors. In this contribution diﬀerent H∞ -controller design schemes are compared when using two diﬀerent sensor conﬁgurations. First, the standard case where only the position of the actuator rotation is used, and second a conﬁguration where, in addition, the acceleration of the tool tip is mea171 172 Paper E H∞ -Controller Design Methods sured. Two diﬀerent H∞ methods are investigated: i) loop shaping [McFarlane and Glover, 1992], and ii) multi-H∞ design [Pipeleers and Swevers, 2013; Zavari et al., 2012]. Motivated by the conclusions from Sage et al. [1999] regarding the area of robust control applied to industrial manipulators, this contribution includes: • results presented using realistic models, • a comparison with a standard pid control structure, • model reduction of the controllers to get a result that more easily can be implemented in practice. The model used in this contribution represents one joint of a typical modern industrial robot [Moberg et al., 2009]. It is a physical model consisting of four masses, which should be compared to the typical two-mass model used in many previous contributions, see Sage et al. [1999] and the references therein. The joint model represents the ﬁrst joint of a serial 6-dof industrial manipulator, where the remaining ﬁve axes have been conﬁgured to minimise the couplings to the ﬁrst axis. To handle changes in the conﬁguration of the remaining axes, gain scheduling techniques can be used based on the results in this paper. An important part of the design is the choice of the weighting functions, which is an essential task to get a satisfactory performance. The work of choosing the weights is diﬃcult, tedious and time consuming. This can be be the reasons for why H∞ methods are not used that often in practice even though the performance and robustness can be increased. In particular, the use of two measurements for control of one variable requires special treatment. The development of the weighting functions for the four controllers is discussed in details, and provides a signiﬁcant part of the contributions in the paper. Controller synthesis using H∞ methods has been proposed in Song et al. [1992]; Stout and Sawan [1992], where the complete non-linear robot model ﬁrst is linearised using exact linearisation, second an H∞ controller is designed using the linearised model. The remaining non-linearities due to model errors are seen as uncertainties and/or disturbances. In both papers, the model is rigid and the H∞ controller, using only joint positions, is designed using the mixed-sensitivity method. In Sage et al. [1997] H∞ loop shaping with measurements of the actuator positions is applied to a robot. The authors use a ﬂexible joint model which has been linearised. The linearised model makes it possible to use decentralised control, hence H∞ loop shaping is applied to n siso-systems instead of the complete mimo-system. Explicit use of acceleration measurements for control in robotic applications has been reported in, for example, de Jager [1993]; Dumetz et al. [2006]; Kosuge et al. [1989]; Readman and Bélanger [1991] and Xu and Han [2000]. In Dumetz et al. [2006], a control law using motor position and acceleration of the load in the feedback loop is proposed for a Cartesian robot1 . The robot is assumed to be 1 For a Cartesian robot the joint acceleration is measured directly by an accelerometer, while for a serial type robot there is a non-linear mapping depending on the states. 2 173 Controller Design Methods Gs (s) w u P(s) z y W1 (s) K(s) (a) G(s) W2 (s) Ks (s) (b) Figure 1: System description for general H∞ synthesis (a) and loop shaping (b). ﬂexible and modelled as a two-mass system, where the masses are connected by a linear spring-damper pair. Another control law of a Cartesian robot using acceleration measurements is presented in de Jager [1993]. The model is a rigid joint model and the evaluation is made both in simulation and experiments. In Kosuge et al. [1989] a 2-degrees-of-freedom (dof) manipulator is controlled using acceleration measurements of the end-eﬀector. The model is assumed to be rigid and it is exactly linearised. The joint angular acceleration used in the nonlinear feedback loop is calculated using the inverse kinematic acceleration model and the measured acceleration. The use of direct measurements of the angular acceleration in the feedback loop is presented in Readman and Bélanger [1991] for both rigid and ﬂexible joint models. A more recent work is presented in Xu and Han [2000], where a 3-dof manipulator is controlled using only measurements of the end-eﬀector acceleration. The theory for synthesis of H∞ controllers is presented in Section 2. The model describing the robot joint is explained in Section 3. In Section 4, the requirements of the system as well as the design of four controllers are described, and in Section 5 the simulation results are shown. Finally, Section 6 discuss low order controller synthesis and Section 7 concludes the work. 2 Controller Design Methods In this section, a brief introduction to mixed-H∞ design [Pipeleers and Swevers, 2013; Zavari et al., 2012] and H∞ loop shaping [McFarlane and Glover, 1992] will be presented. 2.1 Mixed-H∞ Controller Design A common design method is to construct the system P(s) in w z P11 (s) P12 (s) w = P(s) = u P21 (s) P22 (s) u y (1) by augmenting the original system y = G(s)u with the weights Wu (s), WS (s), and WT (s), such that the system z = Fl (P, K)w, depicted in Figure 1a, can be written 174 as Paper E H∞ -Controller Design Methods ⎛ ⎞ ⎜⎜Wu (s)Gwu (s)⎟⎟ ⎜⎜ ⎟ Fl (P, K) = ⎜⎜ −WT (s)T (s) ⎟⎟⎟ , ⎝ ⎠ WS (s)S(s) (2) where S(s) = (I + G(s)K(s))−1 is the sensitivity function, T (s) = I − S(s) is the complementary sensitivity function, and Gwu (s) = −K(s)(I + G(s)K(s))−1 is the transfer function from w to u. The H∞ controller is then obtained by minimising the H∞ -norm of the system Fl (P, K), i.e., minimise γ such that Fl (P, K)∞ < γ. Using (2) gives σ̄(Wu (iω)Gwu (iω)) < γ, ∀ω, σ̄(WT (iω)T (iω)) < γ, ∀ω, (3b) σ̄(WS (iω)S(iω)) < γ, ∀ω. (3c) (3a) The transfer functions Gwu (s), S(s), and T (s) can now be shaped to satisfy the requirements by choosing the weights Wu (s), WS (s), and WT (s). The aim is to get a value of γ close to 1, which in general is hard to achieve and it requires insight in the deign method as well as the system dynamics. For more details about the design method, see e.g. Skogestad and Postletwaite [2005]; Zhou et al. [1996]. The mixed-H∞ controller design [Pipeleers and Swevers, 2013; Zavari et al., 2012] is a modiﬁcation of the standard H∞ -design method. Instead of choosing the weights in (2) such that the norm of all weighted transfer functions satisﬁes (3), the modiﬁed method divides the problem into design constraints and design objectives. The controller can now be found as the solution to minimise γ (4a) subject to WP (s)S(s)∞ < γ MS (s)S(s)∞ < 1 Wu (s)Gwu (s)∞ < 1 (4b) (4d) WT (s)T (s)∞ < 1 (4e) K(s) (4c) where γ not necessarily has to be close to 1. Here, the weight WS (s) has been replaced by the weights MS (s) and WP (s). The method can be generalised to other control structures and in its general form it is formulated as a multi-objective optimisation problem. More details about the general form and how to solve the optimisation problem are presented in Pipeleers and Swevers [2013]; Zavari et al. [2012]. 2.2 Loop Shaping using H∞ Synthesis For loop shaping [McFarlane and Glover, 1992], the system G(s) is pre- and postmultiplied with weights W1 (s) and W2 (s), see Figure 1b, such that the shaped system Gs (s) = W2 (s)G(s)W1 (s) has the desired properties. The controller Ks (s) is then obtained using the method described in Glover and McFarlane [1989] applied on the system Gs (s), giving the controller Ks (s). Finally, the controller 3 175 Flexible Joint Model qm wm qa1 k1 u qa2 k2 Ja1 Jm d1 fm Ja2 d2 f a1 qa3 k3 Ja3 wP d3 f a2 f a3 Figure 2: A four-mass ﬂexible joint model, where Jm is the motor inertia and Ja1 , Ja2 , and Ja3 are the distributed arm inertias. K(s) is given by K(s) = W1 (s)Ks (s)W2 (s). (5) Note that the structure in Figure 1b for loop shaping can be rewritten as a standard H∞ problem according to Figure 1a, see Zhou et al. [1996] for details. It will be used in Section 6 for synthesis of low order controllers. The Matlab function ncfsyn, included in the Robust Control Toolbox, is used in this paper for synthesis of H∞ controllers using loop shaping. 3 Flexible Joint Model The model considered in this paper is a four-mass benchmark model of a single ﬂexible joint, see Figure 2, presented in Moberg et al. [2009]. The model corresponds to joint 1 of a serial 6-dof industrial manipulator, where the ﬁve remaining axes are conﬁgured such that the couplings to joint 1 are minimised, see Moberg et al. [2009] for more details about the operating point where the model has been linearised. Input to the system is the motor torque u, the motor disturbance wm and the end-eﬀector disturbance wP . The four masses are connected by spring-damper pairs, where the ﬁrst mass corresponds to the motor. The other masses represents distributed masses placed along the arm. The ﬁrst spring-damper pair is modelled by a linear damper and non-linear spring, whereas the other springdamper pairs are modelled as linear springs and dampers. The non-linear spring is characterised by a low stiﬀness for low deﬂections and a high stiﬀness for high deﬂections. This behaviour is typical for compact gear boxes, such as harmonic drive [Ruderman and Bertram, 2012]. For design of the H∞ controllers, the nonlinear model is linearised in one operating point in the high stiﬀness region, motivated by that a constant torque, e.g. gravity, is acting on the joint. Moreover, the friction torques are assumed to be linear and the input torque u is limited to ±20 Nm. The outputs of the system are the motor position qm and the endeﬀector acceleration P̈ , where P = l1 qa1 + l2 qa2 + l3 qa3 . η (6) 176 Paper E H∞ -Controller Design Methods In (6), η is the gear ratio and l1 , l2 , and l3 are the respective link lengths. Using Lagrange’s equation, the linearised ﬂexible joint model can be described by a set of four odes, which can be reformulated as a linear state space model according to ẋ = Ax + Bu u + Bw u, (7a) y = Cx + Du u + Dw w. (7b) where the state vector and disturbance vector are given by T x = qm qa1 qa2 qa3 q̇m q̇a1 q̇a2 q̇a3 , T w = wm wP . (8a) (8b) The linear state space model is used for design of the H∞ controllers. Note that the matrices C, Du , and Dw are diﬀerent depending on which sensor conﬁguration that is used, whereas the matrices A, Bu , and Bw stay the same. 4 Design of Controllers In this section, four controllers based on the methods in Sections 2.1 and 2.2 are considered, using only the motor angle qm or both qm and the acceleration of the end-eﬀector P̈ . The controllers are ls (q ): Loop shaping controller using q . 1. H∞ m m ls (q , P̈ ): Loop shaping controller using q and P̈ . 2. H∞ m m m (q ): Mixed-H controller using q . 3. H∞ m ∞ m m (q , P̈ ): Mixed-H controller using q and P̈ . 4. H∞ m ∞ m The four controllers are compared to a pid controller where only qm is used. The pid controller is tuned to give the same performance as the best controller presented in Moberg et al. [2009]. To get high enough gain for low frequencies, without having the pole exactly in 0, the break-point of the magnitude function has to be very small, around 10−5 rad/s. From Figure 3 it can be seen that the main dynamics of the system is present in the frequency interval 30-110 rad/s. The large frequency span from 10−5 rad/s to 110 rad/s makes it numerically diﬃcult to solve for the controller using the standard iterative methods described in Skogestad and Postletwaite [2005]; Zhou et al. [1996]. The mixed-H∞ method does not suﬀer from this, since the design objectives (choice of WP ) for low frequencies is separated from the design constraints (choice of MS ). 4.1 Requirements The controllers using H∞ methods are designed to give better performance than the pid controller. In practice it means that the H∞ controllers should attenuate 177 Design of Controllers Magnitude [dB] Magnitude [dB] 4 50 0 −50 50 0 −50 10−1 100 101 102 103 Frequency [rad/s] Figure 3: Singular values for the system from u to y (top) and w to y (bottom), for y = qm (dashed) and y = (qm P̈ )T (solid). the disturbances at least as much as the pid controller and the cut-oﬀ frequency should be approximately the same. In Figure 3, the singular values of the systems from w to y = qm and w to y = T qm P̈ show that an integrator is present. It means that in order to attenuate piecewise constant disturbances, it is required to have at least two integrators in the open loop GK. Since G already has one integrator, the other integrators have to be included in the controller K. For controllers 2 and 4, an integrator will be present if W1 or W2 include an integrator, recall (5). The requirements for controllers 1 and 3 become that |S(iω)| → 0 for ω → 0. Note that it is not possible to stabilise the plant P(s) with marginally stable weights. Instead the pole has to be moved into the left half plan a small distance. 4.2 Choice of Weights ls (q ): Using only q as a measurement gives a siso-system, hence W and H∞ m m 1 W2 are scalar transfer functions. For a linear siso-system it is possible to use one of W1 and W2 since the transfer functions commute with the system G(s). Therefore, W1 (s) = 1 and W2 (s) is chosen such that the desired loop shape is obtained. First of all, it is necessary to have an integrator as discussed above. Having a pure integrator will lead to that the phase margin will be decreased, a zero in s = −10 is therefore added in order not to change the loop gain for frequencies above 10 rad/s. Next, the gain is increased to get the desired cut-oﬀ frequency. The result using the weight is that the loop shape has peaks above 178 Paper E H∞ -Controller Design Methods 30 rad/s. To reduce the magnitude of the peaks a modiﬁed elliptic ﬁlter2 0.5227s2 + 3.266s + 1406 s 2 + 5.808s + 2324 is introduced in W2 . The weights are ﬁnally given as H(s) = s + 10 H(s). s Using ncfsyn a controller of order 13 is obtained. W1 (s) = 1, W2 (s) = 100 (9) (10) ls (q , P̈): Adding an extra measurement signal in terms of the acceleration H∞ m of the end-eﬀector gives a system with one input and two outputs. For stability reasons, it is not possible to have an integrator in both control channels. Therefore, the integrator is placed in the channel for qm since the accelerometer measurement has low frequency noise, such as drift. For the same reason as for the other controller, a zero in s = −3 is introduced. The transfer function from input torque to acceleration of the end-eﬀector has a high gain in the frequency range of interest. To decrease the gain such that it is comparable with the motor angle measurement, a low pass ﬁlter is added in the acceleration channel. The ﬁnal weights are 0.2 s+3 , (11) , W1 (s) = 50, W2 (s) = diag s (s + 5)2 giving a controller of order 13. Introducing an elliptic ﬁlter to attenuate the ls (q )-controller. peaks in the open loop did not give the same results as for the H∞ m Instead of improving the loop gain, the elliptic ﬁlter made it worse. m (q ): For this controller, four diﬀerent weights have to be chosen, recall (4). H∞ m The weight MS should limit the peak of S and is therefore chosen to be a constant3 . The peak of Gwu is also important to reduce in order to keep the control signal bounded, especially for high frequencies. A constant value of Wu is therefore also chosen. In the spirit of try simplest thing ﬁrst, the weight WT is also chosen to be a constant In order to attenuate the disturbances it is, as was mentioned above, necessary to have at least one integrator in the controller. Forcing S to 0 is the same as letting WP approach ∞ when ω → 0. To get a proper inverse, a zero is also included in the weight. Since a pure integrator is not used, the slope of the weight has to be higher than 20 dB per decade frequency, in order to force S to be low enough. This was accomplished by taking the weight to the power of 3 (2 was not enough). The numerical values of the weights are chosen as Wu = 10−50/20 , MS = 10−10/20 , WT = 10−10/20 , s + 100.1 3 WP = . s + 0.1 (12a) (12b) 2 The ﬁlter is designed to have a magnitude of 0 dB up to 50 rad/s, after that -10 dB, but due to ripple, the real magnitude will diﬀer from that. 3 More complicated weights can be used but here we try simple things ﬁrst. 4 179 Design of Controllers The constant weights in the form 10−α/20 can be interpret as a maximum value, for the corresponding transfer function, of α dB. The resulting controller is of order 10. m (q , P̈): Like for the controller H ls (q , P̈ ), designing the weights for the H∞ m ∞ m mixed-H∞ method becomes somewhat more involved with two measurements and one control signal. The aim is to attenuate the disturbances inﬂuence on the end-eﬀector position. A variant is to ﬁnd a rough estimate of the end-eﬀector position and then choosing the weights from that. A straightforward estimate of P using P̈ is 1 (13) P̂ = 2 P̈ . s Due to low frequency drift and bias in an accelerometer, this estimate is only useful for high frequencies. A high pass ﬁlter is therefore used according to P̂high = c2 1 s2 1 P̈ = c2 P̈ (p + s)2 s 2 (p + s)2 (14) where c2 and p are constants to choose. Another straightforward estimate of P is to use the motor angle qm according to P̂ = lqm where l is the length of the arm. Compared to the estimated position using the acceleration, this new estimate is only valid for low frequencies. Using a low pass ﬁlter gives an estimate for low frequencies. It is important that the two estimates do not overlap each other, hence the low pass ﬁlter is chosen as the complementarity to the previous used high pass ﬁlter. The low frequency estimate is now given by 2s + p s2 P̂low = c1 1 − plqm (15) lqm = c1 (p + s)2 (p + s)2 where c1 is a design variable. The ﬁnal estimate of P is the sum of the two estimates above, hence 2s+p q 1 m P̂ = c1 (p+s)2 pl c2 (p+s)2 (16) P̈ W Using the weights MS = M̃S W , WP = W̃P W , WT = W̃T W , (17) where M̃S , W̃P , and W̃T can be designed in a similar way as in Section 4.2, makes it possible to use more than one output together with one input. The last weight Wu can be chosen similar as in Section 4.2. The numerical values of the weights are 30s+75 0.1 Wu = 10−40/20 , W = (s+5)2 (s+5)2 , (18a) 3 s + 80 M̃S = 10−2/20 , W̃P = , (18b) s + 0.15 180 Paper E H∞ -Controller Design Methods and it turns out that WT is not needed for the performance. Using these weights results in a controller of order 13. 4.3 Controller Characteristics The resulting loop gains for the ﬁve controllers are shown in Figure 4. The four controllers using H∞ methods do not give as high peaks as the pid-controller around 100 rad/s. It can also be seen that introducing P̈ as a measurement eliminates the notch at 30 rad/s. In Figure 4, the magnitudes of the ﬁve controllers are presented. The pid controller is smoother than the other controllers. The reason is that a part of the system dynamics is included in the H∞ controllers. As a result, they try to remove the resonance peaks from the system, which can be seen in Figure 3, hence the peaks in the amplitude function of the H∞ controllers. The weights Wu for m (q ) and H m (q , P̈ ) are diﬀerent which can be seen in Figure 4 the controllers H∞ m ∞ m ls (q ) and H ls (q , P̈ ) for for high frequencies. Comparing the two controllers H∞ m ∞ m high frequencies it can be seen that they behave similar. The pid-controller has the highest magnitude for high frequencies, which implies that the measurement noise will be ampliﬁed more than for the H∞ controllers. 5 Simulation Results The ﬁve controllers are evaluated using a simulation model. The simulation model consists of the ﬂexible joint model described in Section 3, a measurement system, and a controller. The robot joint model is implemented in continuous time whereas the controllers operate in discrete time. The continuous-time controllers developed in Section 4, are therefore discretised using Tustin’s formula. The measurements are aﬀected by a time delay of one sample as well as zero mean normal distributed measurement noise. The sample time is Ts = 0.5 ms. The system is excited by a disturbance signal w containing steps and chirp signals on both the motor and end-eﬀector. The performance is evaluated using a performance index, which is a weighted sum of peak-to-peak errors and settling times in the simulated end-eﬀector position and the maximum torque and the torque noise in the simulated motor torque. The reader is referred to Moberg et al. [2009] for complete details about the disturbance signals and the performance index. Figure 5 shows how the motor torque diﬀers between the ﬁve controllers. In the ls (q ) gives higher torques than the pid and upper diagram it can be see that H∞ m m the H∞ (qm ) controllers. The pid gives higher torque noise during steady state due to the gain of the controller for high frequencies, recall Figure 4. In the lower ls (q , P̈ ) and H m (q , P̈ ) give diagram in Figure 5 it is shown that the controllers H∞ m ∞ m similar torque signals, and lower compared to the pid controller. A low torque signal is preferred to reduce the energy consumption and to decrease the wear in the motor and gear. The end-eﬀector position is presented in Figure 6. In the top graph it is seen that 181 Simulation Results Loop gain |K G| pid ls (q ) H∞ m 150 Magnitude [dB] ls (q , P̈ ) H∞ m m (q ) H∞ m m (q , P̈ ) H∞ m 100 50 0 −50 Controller gain |K| 120 pid ls (q ) H∞ m ls (q , P̈ ) H∞ m m (q ) H∞ m 100 Magnitude [dB] 5 m (q , P̈ ) H∞ m 75 50 25 10 10−1 100 101 102 103 Frequency [rad/s] Figure 4: Loop gain |K G| and controller gain |K| for the ﬁve controllers. 182 Paper E H∞ -Controller Design Methods Controllers using qm Torque [Nm] 5 0 −5 ls (q ) H∞ m pid m (q ) H∞ m −10 Controllers using qm and P̈ Torque [Nm] 5 0 −5 pid ls (q , P̈ ) H∞ m −10 m (q , P̈ ) H∞ m 0 10 20 30 40 50 Time [s] Figure 5: Applied motor torque from the ﬁve controllers. The top graph shows the controllers using only qm . The bottom graph shows the pid and the controllers using qm and P̈ . 183 Simulation Results Controllers using qm End-eﬀector Position [mm] 4 ls (q ) H∞ m pid m (q ) H∞ m 2 0 −2 −4 Controllers using qm and P̈ 4 pid ls (q , P̈ ) H∞ m End-eﬀector Position [mm] 5 m (q , P̈ ) H∞ m 2 0 −2 −4 0 10 20 30 40 50 Time [s] Figure 6: Simulated end-eﬀector position for the ﬁve controllers. The top graph shows the controllers using only qm . The bottom graph shows the pid and the controllers using qm and P̈ . 184 Paper E H∞ -Controller Design Methods Table 1: Performance index for the ﬁve controllers, where lower value is better. ls (q ) ls (q , P̈ ) m (q ) m (q , P̈ ) H∞ H∞ H∞ pid H∞ m m m m 55.7 55.4 45.8 42.4 28.8 ls (q ) gives, compared to the pid, higher oscillations during the time intervals H∞ m 10-15 s and 37-42 s, which corresponds to a chirp disturbance at the end-eﬀector. For step disturbances and chirp disturbances on the motor (time intervals 16-21 s ls (q ) and the pid are more similar. The controller H m (q ) is betand 43-58 s) H∞ m ∞ m ter than the other two controllers in the simulation. The bottom graph shows that m (q , P̈ ) can handle the chirp disturbances on the motor (time intervals 16-21 s H∞ m and 43-58 s) and step disturbances very good. For a chirp disturbance on the endeﬀector, the two H∞ controllers give similar results. For step disturbances, the ls (q , P̈ ) gives lower peaks than the pid controller, however the setcontroller H∞ m tling time is longer. The steady state error of approximately 2 mm after 25 s is a result of a constant torque disturbance on the end-eﬀector. The size of the error will depend on the size of the disturbance and the stiﬀness of the joint. The motor position, which is measured, is controlled to zero for all ﬁve controllers. The performance index for the ﬁve controllers is presented in Table 1. It shows, ls (q ) and the pid controller behave similar and that as discussed above, that H∞ m ls (q , P̈ ) and H m (q ) give similar behaviour. The H m (q , P̈ )-controller gives H∞ m ∞ m ∞ m the best result. 6 Low Order Synthesis For implementation of the controller in the robot control system it is important to have a low order controller. A controller in state space form requires O(n2x ) operations to calculate the control signal, where nx is the dimension of the state vector in the controller. The low order controllers are here obtained using the Matlab-function hinfstruct, which is included in Robust Control Toolbox and it is based on techniques from Apkarian and Noll [2006]. To ﬁnd controllers with low orders using hinfstruct requires a model description, including the weights, in the form of (1). This structure is already used for m (q ) and H m (q , P̈ ), hence it is straightforward to synthesis the controllers H∞ m ∞ m low order controllers using the weights presented in Sections 4.2 and 4.2. For the loop shaping design method, the structure in Figure 1b can be rewritten in the form of (1) including the weights W1 (s) and W2 (s), explained in e.g. Zhou et al. ls (q ) [1996]. Using the rewritten structure, the low order controllers based on H∞ m ls and H∞ (qm , P̈ ) can be obtained using the weights from Sections 4.2 and 4.2. Table 2 shows the lowest order for the respective controllers, that can be achieved without changing the closed-loop performance too much. The table also shows 7 Conclusions and Future Work 185 Table 2: Lowest order of the controllers obtained using hinfstruct, with the order before reduction in brackets. The corresponding performance index from simulations is also shown. ls (q ) ls (q , P̈ ) m (q ) m (q , P̈ ) H∞ H∞ H∞ H∞ m m m m Order 5 (13) 4 (13) 5 (10) 7 (13) Perf. ind. 60.8 49.4 48.3 40.8 the performance index obtained when the controllers are used in the simulation environment. The orders can be reduced by a factor of two to three but the performance of the reduced order controllers is worse than the full order controllers. Since the controller based on loop shaping with only qm as measurement has the same performance for the full order controller as the pid controller, the low order controller gives a worse performance than the pid controller. The other full order controllers are much better than the pid controller and aﬀord to get a reduced performance for the low order controllers without getting worse than the pid controller. Finally, note that the controllers only represents local minima solutions, hence rerunning hinfstruct with other initial values can give a better, or worse, controller. To handle this, several initial values have been used in hinfstruct. 7 Conclusions and Future Work Four diﬀerent H∞ controllers for a ﬂexible joint of an industrial manipulator are designed using mixed-H∞ controller design and the loop shaping method. The model, on which the controllers are based, is a four-mass model. As input, the controllers use either the motor angle only or both the motor angle and the acceleration of the end-eﬀector. Tuning of the controllers requires understanding of both the synthesis method and how the system behaves. For example, the measurements for the mixed-H∞ controller are ﬁrst pre-ﬁltered to give an estimate of the tool position. The weighting functions for the resulting siso system, from input torque to the estimated tool position, are then chosen similar to the case where only the motor position is used. The controllers are compared to a pid controller and it is shown that if only the motor angle is measured it is much better to use the mixed-H∞ design method compared to loop shaping. If instead the end-eﬀector acceleration is added then the performance is improved signiﬁcantly for both methods. The steady state error for the end-eﬀector position is unaﬀected since the accelerometer does not provide low frequency measurements. Using a low order controller synthesis method, it is possible to reduce the order of the controllers by a factor of two to three but at the same time a decrease in the performance index of 10–30 % can be observed. Investigation of robustness for stability with respect to model errors is one of 186 Paper E H∞ -Controller Design Methods several future directions of research. The mixed-H∞ method has an advantage compared to the loop shaping method since a model of the error is possible to incorporate in the augmented plant P(s). Another continuation is to investigate the improvement for other types of sensors. One possibility is to have an encoder measuring the position directly after the gearbox, i.e., qa1 . This will improve the stiﬀness of the system, although it will not eliminate the stationary error for the end-eﬀector position. The ultimate solution is to measure the end-eﬀector position, but for practical reasons this is in general not possible, instead the end-eﬀector position can be estimated, as described in Axelsson [2012]; Axelsson et al. [2012]; Chen and Tomizuka [2014], and used in the feedback loop. Extending the system to several joints giving a non-linear model, which has to be linearised in several points, is also a future problem to investigate. A controller, using the results from this paper, is designed in each point and for example gain scheduling can be used when the robot moves between diﬀerent points. Acknowledgement This work was supported by the Vinnova Excellence Center LINK-SIC and by the Excellence Center at Linköping-Lund in Information Technology, ELLIIT. Bibliography 187 Bibliography Pierre Apkarian and Dominikus Noll. Nonsmooth H∞ synthesis. IEEE Transactions on Automatic Control, 51(1):71–86, January 2006. Patrik Axelsson. Evaluation of six diﬀerent sensor fusion methods for an industrial robot using experimental data. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 126–132, Dubrovnik, Croatia, September 2012. Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Bayesian state estimation of a ﬂexible industrial robot. Control Engineering Practice, 20(11):1220–1228, November 2012. Patrik Axelsson, Anders Helmersson, and Mikael Norrlöf. H∞ -controller design methods applied to one joint of a ﬂexible industrial manipulator. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014. Wenjie Chen and Masayoshi Tomizuka. Direct joint space state estimation in robots with multiple elastic joints. IEEE/ASME Transactions on Mechatronics, 19(2):697–706, April 2014. doi: 10.1109/TMECH.2013.2255308. Bram de Jager. The use of acceleration measurements to improve the tracking control of robots. In Proceedings of the IEEE International Conference on Systems, Man and Cybernetics, pages 647–652, Le Touquet, France, October 1993. Eric Dumetz, Jean-Yves Dieulot, Pierre-Jean Barre, Frédéric Colas, and Thomas Delplace. Control of an industrial robot using acceleration feedback. Journal of Intelligent & Robotic Systems, 46(2):111–128, 2006. Keith Glover and Duncan McFarlane. Robust stabilization of normalized coprime factor plant descriptions with H∞ -bounded uncertainty. IEEE Transactions on Automatic Control, 34(8):821–830, August 1989. K. Kosuge, M. Umetsu, and K. Furuta. Robust linearization and control of robot arm using acceleration feedback. In Proceedings of the IEEE International Conference on Control and Applications, pages 161–165, Jerusalem, Israel, April 1989. Duncan McFarlane and Keith Glover. A loop shaping design procedure using H∞ synthesis. IEEE Transactions on Automatic Control, 37(6):759–769, June 1992. Stig Moberg, Jonas Öhr, and Svante Gunnarsson. A benchmark problem for robust feedback control of a ﬂexible manipulator. IEEE Transactions on Control Systems Technology, 17(6):1398–1405, November 2009. Goele Pipeleers and Jan Swevers. Matlab-software mixedHinfsyn, 2013. Available at http://set.kuleuven.be/optec/Software/mixedhinfsyn. Mark Readman and Pierre Bélanger. Acceleration feedback for ﬂexible joint robots. In Proceedings of the 30th IEEE Conference on Decision and Control, pages 1385–1390, Brighton, England, December 1991. 188 Paper E H∞ -Controller Design Methods Michael Ruderman and Torsten Bertram. Modeling and observation of hysteresis lost motion in elastic robot joints. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 13–18, Dubrovnik, Croatia, September 2012. Hansjörg G. Sage, Michel F. de Mathelin, Gabriel Abba, Jacques A. Gangloﬀ, and Eric Ostertag. Nonlinear optimization of robust H∞ controllers for industrial robot manipulators. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 2352–2357, Albuquerque, NM, USA, April 1997. Hansjörg G. Sage, Michel F. de Mathelin, and Eric Ostertag. Robust control of robot manipulators: A survey. International Journal of Control, 72(16):1498– 1522, 1999. Sigurd Skogestad and Ian Postletwaite. Multivariable Feedback Control, Analysis and Design. John Wiley & Sons, Chichester, West Sussex, England, second edition, 2005. Y. D. Song, A. T. Alouani, and J. N. Anderson. Robust path tracking control of industrial robots: An H∞ approach. In Proceedings of the IEEE Conference on Control Applications, pages 25–30, Dayton, OH, USA, September 1992. Wayne L. Stout and M. Edwin Sawan. Application of H-inﬁnity theory to robot manipulator control. In Proceedings of the IEEE Conference on Control Applications, pages 148–153, Dayton, OH, USA, September 1992. W. L. Xu and J. D. Han. Joint acceleration feedback control for robots: Analysis, sensing and experiments. Robotics and Computer-Integrated Manufacturing, 16(5):307–320, October 2000. Keivan Zavari, Goele Pipeleers, and Jan Swevers. Multi-H∞ controller design and illustration on an overhead crane. In Proceedings of the IEEE Conference on Control Applications. Part of the IEEE Multi-Conference on Systems and Control, pages 670–674, Dubrovnik, Croatia, October 2012. Kemin Zhou, John C. Doyle, and Keith Glover. Robust and Optimal Control. Prentice Hall Inc., Upper Saddle River, NJ, USA, 1996. Paper F H∞-Synthesis Method for Control of Non-linear Flexible Joint Models Authors: Norrlöf Patrik Axelsson, Goele Pipeleers, Anders Helmersson and Mikael Edited version of the paper: Patrik Axelsson, Goele Pipeleers, Anders Helmersson, and Mikael Norrlöf. H∞ -synthesis method for control of non-linear ﬂexible joint models. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014d. F H∞-Synthesis Method for Control of Non-linear Flexible Joint Models Patrik Axelsson∗ , Goele Pipeleers∗∗ , Anders Helmersson∗ and Mikael Norrlöf∗ ∗ Dept. of Electrical Engineering, Linköping University, SE–581 83 Linköping, Sweden [email protected], [email protected], [email protected] ∗∗ Department of Mechanical Engineering, Katholieke Universiteit Leuven, Celestijnenlaan 300B, B-3001 Heverlee, Belgium goele.pipeleers @mech.kuleuven.be Abstract An H∞ -synthesis method for control of a ﬂexible joint, with non-linear spring characteristic, is proposed. The ﬁrst step of the synthesis method is to extend the joint model with an uncertainty description of the stiﬀness parameter. In the second step, a non-linear optimisation problem, based on nominal performance and robust stability requirements, has to be solved. Using the Lyapunov shaping paradigm and a change of variables, the non-linear optimisation problem can be rewritten as a convex, yet conservative, lmi problem. The method is motivated by the assumption that the joint operates in a speciﬁc stiﬀness region of the non-linear spring most of the time, hence the performance requirements are only valid in that region. However, the controller must stabilise the system in all stiﬀness regions. The method is validated in simulations on a non-linear ﬂexible joint model originating from an industrial robot. 1 Introduction The demand and the requirements for high precision control in electro mechanical systems have been increasing over time. At the same time cost reduction and more developed mechanical design criteria, with lower margins in the design, reduces the size of the components involved. One such example is the speed reducers used in many electro mechanical systems where the size and cost have become increasingly important. The harmonic drive, sometimes referred to as “strain-wave gearing”, is a very common example of a gear type that can deliver high gear reduction ratio in a small device [Tuttle and Seering, 1996]. Characteristic to compact gear boxes, such as harmonic drives, are that they have a relatively small backlash, a highly non-linear friction behaviour, and in addition a very non-linear stiﬀness [Tjahjowidodo et al., 2006]. One typical application of electromechanical systems, where harmonic drive gearboxes are used, is in industrial 191 192 Paper F H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models robots where the motivation for the work presented in this paper also comes from. In this paper the control design for the electromechanical system, motor-gearbox joint, hereafter referred to as the ﬂexible joint system, is considered. In general, robots are strongly coupled multivariate systems with non-linear dynamics and in previous research on control of robots linear spring stiﬀness has been considered, see e.g. Sage et al. [1999] and the references therein. When the speed reducers are of harmonic drive type, linear models are however not suﬃcient for the control as will be shown in the paper. Several non-linear models of the gearbox have been presented in the literature, see Tuttle and Seering [1996]; Tjahjowidodo et al. [2006]; Ruderman and Bertram [2012] among others. What characterises the H∞ -controller synthesis method presented in this work is that it can facilitate in designing a controller which gives performance in one region of parameter values, while for another region the performance requirement is lower and only stability is suﬃcient. The proposed method is motivated by the fact that the ﬂexible joint operates in speciﬁc regions most of the time. For example, a joint which is aﬀected by gravity operates most of the time in the high stiﬀness region, hence it is more important to have a controller with good performance in the high stiﬀness region. However, the controller must stabilise the system in all stiﬀness regions. This paper is organised as follows. Section 2 presents the problem and how it will be solved and the proposed method is outlined in Section 3. In Section 4, the non-linear joint model used to test the proposed method is presented. The design of the controller and the results are given in Sections 5 and 6. Finally, Section 7 concludes the paper. 2 Problem Formulation The problem is to design a linear H∞ controller that can stabilise a non-linear ﬂexible joint model, for example a motor-harmonic drive-joint system, using only the primary position, the motor position qm . There are a number of non-linearities that characterise the gearbox in the ﬂexible joint. Here, the spring stiﬀness of the joint is considered and it is described by the function τs (Δq ), where Δq = qm − qa is the deﬂection between the motor position qm and the secondary position, the arm position qa . The non-linear spring is characterised by a low stiﬀness for small deﬂections and a high stiﬀness for large deﬂections, which is typical for compact gear boxes, such as harmonic drive [Tuttle and Seering, 1996; Ruderman and Bertram, 2012]. Linearising the stiﬀness function would give a linear expression k · (qm − qa ), where the gain k depends on the deﬂection qm − qa of the joint. The lowest and maximal values of k are k low and k high , respectively. The complete model and an explicit expression for τs (Δq ) are presented in Section 4. Control of non-linear systems using linear H∞ methods is usually done by ﬁrst linearising the model in several operating points, e.g. gain scheduling techniques, or using exact linearisation. Gain scheduling requires to know the operating point of the spring and exact linearisation requires the full state vector, hence 3 Proposed H∞ -Synthesis Method 193 only the motor position is not enough to measure. A common solution is to introduce an observer for estimating the state vector. However, it is not certain that the estimated state vector is accurate enough to use due to model errors and disturbances. The estimation problem has been investigated in e.g. Axelsson et al. [2012]; Chen and Tomizuka [2014]. Instead, the problem considered in this paper is managed using an uncertainty description of the stiﬀness parameter k to obtain a controller over the whole interval for k. In general, the interval has to be relative short in order to obtain a controller using regular methods. The reason for this is that the methods try to be both robustly stable and have robust performance over the whole interval. The uncertainty description of the linearised spring stiﬀness can give a long interval of the parameter k that has to be covered. Instead of having a controller that satisﬁes the requirements of both robust stability and performance over the whole interval, the aim is to ﬁnd a controller that is stable for all values of k but only satisﬁes the performance requirements in the high stiﬀness region. The reason for this is that in practice, the joint operates only in the high stiﬀness region most of the time, e.g. an industrial manipulator aﬀected by the gravity force. In reality as low as zero stiﬀness must be handled due to backlash, but that is omitted here. 3 Proposed H∞ -Synthesis Method This section presents the proposed H∞ -synthesis method. First, the uncertainty description is given. After that, the requirement for nominal stability and performance together with robust stability is discussed, and the ﬁnal optimisation problem is presented. 3.1 Uncertainty Description Let k be modelled as an uncertainty according to k(δ) = k + k̄δ, k = αk high k̄ = αk high (1a) + βk low , (1b) − βk low , (1c) where α, β are scaling parameters such that β ≤ α. The uncertain parameter δ is contained in δ = −1 1 ⊂ R and may change arbitrarily fast. For δ = ±1 it holds that the stiﬀness parameter k(δ) ∈ 2βk low 2αk high . (2) Since the aim is to have a controller that has good performance in the high stiﬀness region, but only stable in the low stiﬀness region, it is desirably to have k close to k high and the lower bound of k(δ) not larger than k low . The stiﬀness parameter enters only in the A-matrix of the linearised system and 194 Paper F H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models Δ w u P(s) z y wΔ w u P̄(s) K(s) K(s) (a) (b) zΔ z y Figure 1: Closed-loop system from w to z, without and with an uncertainty description in (a) and (b) respectively. assume that the part containing δ is of rank one, then A(δ) = A + LδR Rnx ×nx , (3) with A ∈ L ∈ and R ∈ For the forthcoming calculations, it is important to have L and R as a column and row matrix, respectively. The augmented system in Figure 1b can now be constructed according to ⎛ ⎞ Bu ⎟ ⎜⎜ A L Bw ⎟ ⎜⎜⎜ R 0 0 0 ⎟⎟⎟⎟ P̄ = ⎜⎜⎜ (4) ⎟, ⎜⎜ Cz 0 Dzw Dzu ⎟⎟⎟ ⎝ ⎠ Cy 0 Dyw 0 Rnx ,1 R1,nx . with Δ = δ. 3.2 Nominal Stability and Performance Let P represent an lti system, see Figure 1a, ẋ = Ax + Bw w + Bu u, (5a) z = Cz x + Dzw w + Dzu u, y = Cy x + Dyw w, (5b) (5c) where x ∈ Rnx is the state vector, w ∈ Rnw is the disturbance vector, u ∈ Rnu is the control signal, y ∈ Rny is the measurement signal, and z ∈ Rnz is the output signal that reﬂects our speciﬁcations. The matrices in (5) have dimensions corresponding to the vectors x, w, u, y, and z. Note that it is the nominal system, i.e., δ = 0 that is used here. Let the controller K in Figure 1a be given by ẋK = AK xK + BK y, (6a) u = CK xK + DK y. (6b) 3 Proposed H∞ -Synthesis Method 195 Using the lower fractional transformation Fl (P, K) gives the closed loop system from w to z according to ACL BCL Fl (P, K) = CCL DCL ⎞ ⎛ Bu CK Bw + Bu DK Dyw ⎟⎟ ⎜⎜ A + Bu DK Cy ⎟⎟ ⎜⎜ ⎟⎟ BK Cy AK BK Dyw (7) = ⎜⎜⎜ ⎟⎠ ⎝ Cz + Dzu DK Cy Dzu CK Dzw + Dzu DK Dyw From Gahinet and Apkarian [1994] it holds that the H∞ -norm of Fl (P, K) is less than γ, i.e., Fl (P, K)∞ < γ, and the closed loop system is stable, i.e., ACL has all eigenvalues in the left half plane, if and only if there exists a positive deﬁnite matrix P such that ⎞ ⎛ T ⎜⎜ACL P + PACL PBCL CTCL ⎟⎟ ⎟ ⎜⎜ ⎜⎜ (8) BTCL P −γI DTCL ⎟⎟⎟⎟ ≺ 0. ⎜⎝ ⎠ CCL DCL −γI 3.3 Robust Stability To guarantee robust stability of the uncertain system for arbitrarily fast changes in δ, quadratic stability is enforced which is given by ∃P ∈ Snx : P 0 and A(δ)T P + PA(δ) ≺ 0, ∀δ ∈ δ. (9a) (9b) From Scherer [2006], the robust lmi (9b) holds if and only if there exist p, q ∈ R with p > 0 such that T T I 0 0 P I 0 0 I −p iq 0 I + ≺0 (10) A L P 0 A L R 0 −iq p R 0 Note that p, q ∈ R with p > 0 parametrise all multipliers that satisfy T δ −p iq δ ≥ 0, ∀δ ∈ δ. 1 −iq p 1 (11) Since the negative deﬁniteness of a Hermitian matrix implies that its real part is negative deﬁnite, q = 0 can be enforced in (10) without loss of generality. It follows from the fact that L and R are rank one matrices1 . In addition, p = 1 can be enforced since the lmi is homogeneous in P and p. By elaborating the left-hand side of (10) gives that (9) is equivalent to T A P + PA + RT R PL nx ∃P ∈ S : P 0 and ≺ 0. (12) LT P −1 1 If rank L = rank R = r, than q should be a r × r skew symmetric matrix and the involved computations are much more complex. 196 Paper F H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models The last lmi in (12) can be rewritten, similar to what is given by (8), using the Schur complement, according to ⎛ T T⎞ ⎜⎜A P + PA PL R ⎟⎟ ⎜⎜ ⎟ (13) ⎜⎜ LT P −1 0 ⎟⎟⎟ ≺ 0. ⎝ ⎠ R 0 −1 The lmi in (12) can also be obtained using iqc-based robust stability analysis with frequency independent multipliers [Megretski and Rantzer, 1997], which guarantees stability for arbitrarily fast changes in δ. 3.4 Controller Synthesis The controller is now obtained from the following optimisation problem minimise AK ,BK ,CK ,DK γ subject to P 0 ⎛ T ⎞ ⎜⎜ACL P + PACL PBCL CTCL ⎟⎟ ⎟ ⎜⎜⎜ BTCL P −γI DTCL ⎟⎟⎟⎟ ≺ 0 ⎜⎜ ⎝ ⎠ CCL DCL −γI ⎞ ⎛ T ⎜⎜ACL P + PACL PLCL RTCL ⎟⎟ ⎟ ⎜⎜ ⎜⎜ LTCL P −1 0 ⎟⎟⎟⎟ ≺ 0 ⎜⎝ ⎠ 0 −1 RCL (14a) (14b) (14c) (14d) where LCL and RCL are the matrices L and R augmented with zeros in order for T the dimensions to satisfy the closed-loop state vector xCL = xT xTK . The minimisation problem in (14) gives a conservative solution because of the same Lyapunov matrix P is used in both (8) and (13). For the approach not to be conservative, diﬀerent Lyapunov matrices should be used in (8) and (13). However, this multi-objective controller design is non-convex. To obtain a convex, yet conservative, approximation, the Lyapunov shaping paradigm, as introduced by Scherer et al. [1997], is used. Moreover, the minimisation problem in (14) is non-linear due to products of P and the controller parameters AK , BK , CK , and DK . However, a change of variables [Scherer et al., 1997] makes the constraints linear and the resulting minimisation problem can be solved using lmi optimisation, e.g. using Yalmip [Löfberg, 2004]. The optimisation problem (14) will be easier to solve the smaller the perturbation is. It can therefore be useful to introduce a scaling parameter 0 < κ ≤ 1 such that δ ∈ [−κ κ]. Decreasing δ to [−κ κ] is equivalent to preserving δ = [−1 1] but rescaling L, according to L → κL. The good thing is that it can still be possible to stabilise the system for δ ∈ [−1 1] due to the conservatism of the proposed method. Note that κ is a tuning parameter that aﬀect the solution to (14). A too large value can make the problem impossible to solve whereas a too small value gives a controller that is not able to stabilise the non-linear system. 4 197 Non-linear Flexible Joint Model qm u qa τs (Δq ) Jm wm Ja wa d fm Figure 2: A two-mass ﬂexible joint model, where Jm is the motor and Ja the arm. 4 Non-linear Flexible Joint Model The ﬂexible joint model considered in this paper is of two-mass model type, see Figure 2, where qm is the motor position and qa the arm position. Here, both qm and qa are described on the motor side of the gearbox. Input to the system is the motor torque u, the motor disturbance wm and the arm disturbance wa . The two masses are connected by a spring-damper pair, where the ﬁrst mass corresponds to the motor and the second mass corresponds to the arm. The spring-damper pair is modelled by a linear damper, described by the parameter d, and the nonlinear spring is described by the function τs (Δq ) which is a piecewise aﬃne function with ﬁve segments, i.e., ⎧ ⎪ Δq ≤ Ψ klow Δq , ⎪ 4 ⎪ ⎪ ⎪ mk Ψ ⎪ Ψ Ψ ⎪ Δ (k ) Δ ≤ + m − sign(Δ ) , < ⎪ low k q q q 4 4 2 ⎪ ⎪ ⎪ 3mk Ψ ⎨ (k Ψ 3Ψ Δ ) Δ ≤ + 2m − sign(Δ ) , < τs (Δq ) = ⎪ low k q q q ⎪ 4 2 4 ⎪ ⎪ 3mk Ψ 3Ψ ⎪ ⎪ Δ (k ) ≤ Ψ + 3m − sign(Δ ) , < Δ ⎪ low k q q q 2 4 ⎪ ⎪ ⎪ 5mk Ψ ⎪ ⎩ Δ Δ − sign(Δ ) , Ψ< k high q q 2 q where mk = (khigh − klow )/4, and Ψ a model parameter describing the transition to the high stiﬀness region. Moreover, the friction torque is assumed to be linear, described by the parameter f m , and the input torque u is limited to ±20 Nm. The measurement of the system is the motor position qm , and qa is the variable that is to be controlled. The model is a simpliﬁcation of the experimental results achieved in, e.g. Tjahjowidodo et al. [2006], where the non-linear torsional stiﬀness also shows hysteresis behaviour. The dynamical model of the ﬂexible joint is given by Ja q̈a − τs (Δq ) − d(q̇m − q̇a ) = wa , Jm q̈m + τs (Δq ) + d(q̇m − q̇a ) + f m q̇m = u + wm , (15a) (15b) where the model parameters are presented in Table 1. Using a state vector x according to T x = qa qm q̇a q̇m , (16) 198 Paper F H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models Table 1: Numerical values of the model parameters. Jm Ψ khigh klow d fm Ja 0.042 0.005 220π/60/180 100 100/6 0.08 0.006 gives the non-linear state space model ⎛ ⎞ q̇a ⎜⎜ ⎟⎟ ⎜⎜ ⎟⎟ q̇m ⎜⎜ ⎟⎟ ⎟⎟ ẋ = ⎜⎜⎜ 1 ⎟ (Δ ) + d( q̇ − q̇ ) + w τ ⎜⎜ Ja s q m a a ⎟⎟⎟⎠ ⎜⎝ 1 u − τs (Δq ) − d(q̇m − q̇a ) − f m q̇m + wm J (17) m Linearising the non-linear ﬂexible joint model (17) gives a linear state space T 9x + 9 model 9̇ x = A9 Bu u + 9 Bw w, where w = wa wm and ⎛ ⎜⎜ 0 ⎜⎜ 0 ⎜ 9 = ⎜⎜⎜ k A ⎜⎜− J ⎜⎜ a ⎝ k 0 0 Jm ⎛ ⎞ ⎜⎜ 0 ⎟⎟ ⎜⎜⎜ 0 ⎟⎟⎟ 9 Bu = ⎜⎜⎜⎜ 0 ⎟⎟⎟⎟ , ⎜⎜ ⎟⎟ ⎝1⎠ Jm 9= 0 C 1 k Ja − Jk m 1 0 − Jd a d Jm ⎛ ⎜⎜ 0 ⎜⎜ ⎜⎜ 0 9 Bw = ⎜⎜⎜ 1 ⎜⎜ Ja ⎜⎝ 0 0 0 . ⎞ ⎟⎟ ⎟⎟ ⎟⎟ ⎟⎟ , d ⎟ Ja ⎟ ⎟ ⎠ d+f m ⎟ 0 1 − (18a) Jm ⎞ 0 ⎟⎟ ⎟ 0 ⎟⎟⎟ ⎟ 0 ⎟⎟⎟⎟ ⎠ 1 ⎟ (18b) Jm (18c) Here, k is the stiﬀness parameter given by (1b). The uncertainty description (1) gives 9 9 +9 9 A(δ) =A Lδ R, T 9 L = 0 0 Jk̄ − Jk̄ , a m 9 = −1 1 0 0 . R (19a) (19b) (19c) The notation 9 indicates that the weighting functions in the system P(s) are not included here. Section 5 presents the weighting functions and how they are included in the state space model to give the system P(s) in (5). 5 Controller Design A common design method is to construct the system P in (5) by augmenting the original system y = G(s)u with the weights Wu (s), WS (s), and WT (s), such that 5 199 Controller Design the system z = Fl (P, K)w, depicted in Figure 1a, can be written as ⎞ ⎛ ⎜⎜Wu (s)Gwu (s)⎟⎟ ⎟ ⎜⎜ Fl (P, K) = ⎜⎜ −WT (s)T (s) ⎟⎟⎟ , ⎠ ⎝ WS (s)S(s) (20) where S(s) = (I + G(s)K(s))−1 is the sensitivity function, T (s) = I − S(s) is the complementary sensitivity function, and Gwu (s) = −K(s)(I + G(s)K(s))−1 is the transfer function from w to u. The H∞ controller is obtained by minimising the H∞ -norm of the system Fl (P, K), i.e., minimise γ such that Fl (P, K)∞ < γ. Using (20) gives σ̄(Wu (iω)Gwu (iω)) < γ, ∀ω, σ̄(WT (iω)T (iω)) < γ, ∀ω, (21b) σ̄(WS (iω)S(iω)) < γ, ∀ω. (21c) (21a) The transfer functions Gwu (s), S(s), and T (s) can now be shaped to satisfy the requirements by choosing the weights Wu (s), WS (s), and WT (s). In general this is a quite diﬃcult task. See e.g. Skogestad and Postletwaite [2005]; Zhou et al. [1996] for details. The mixed-H∞ controller design [Pipeleers and Swevers, 2013; Zavari et al., 2012] is a modiﬁcation of the standard H∞ -design method. Instead of choosing the weights in (20) such that the norm of all weighted transfer functions satisﬁes (21), the modiﬁed method divides the problem into design constraints and design objectives. The controller can now be found as the solution to minimise γ (22a) subject to WP S∞ < γ MS S∞ < 1 Wu Gwu ∞ < 1 (22b) (22d) WT T ∞ < 1 (22e) K(s) (22c) where γ not necessarily has to be close to 1. The lmi in (8) can be modiﬁed to ﬁt into the optimisation problem (22), see Zavari et al. [2012]. The weight MS should limit the peak of S and is therefore chosen to be a constant. The peak of Gwu is also important to reduce in order to keep the control signal bounded, especially for high frequencies. A constant value of Wu is therefore also chosen. Finally, the weight WT is also chosen to be a constant for simplicity. The system from w to the output includes an integrator, hence it is necessary to have at least two integrators in the open loop GK in order to attenuate piecewise constant disturbances. The system G has one integrator hence at least one integrator must be included in the controller. Including an integrator in the controller is the same as letting |S(iω)| → 0, for ω → 0. Forcing S to 0 is the same as letting WP approach ∞ when ω → 0. However, it is not possible to force pure integrators in the design since the generalised plant P(s) is not possible to stabilise with 200 H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models Paper F Magnitude [dB] 60 K K̂ 40 20 0 10−3 10−2 10−1 100 101 102 103 Frequency [rad/s] Figure 3: Controller gains |K| and |K̂|. marginally stable weights. Instead the pole is placed in the left half plane close to the origin. Zeros must be included in the design as well to get a proper inverse. The following weights have been proven to work Wu = 10−50/20 , MS = 10−8/20 , WT = 10−8/20 , (s + 50)(s + 15)(s + 5) WP = . 500(s + 0.2)(s + 0.001)2 (23a) (23b) The constant weights in the form 10−λ/20 can be interpret as a maximum value, for the corresponding transfer function, of λ dB. The augmented system P is obtained using the command augw(G,[Wp;MS], 9 9 9 in (18). WU,WT) in Matlab, where G is the system described by A, Bu , and C The uncertainty description of k(δ) in (1) is used with α = 0.9167, and β = 0.5, i.e., the nominal value is k = k high , k̄ = 83.33, and k(δ) ∈ [16.67 183.33]. The scaling parameter is chosen as κ = 0.75. Finally, the uncertainty model is updated according to T 9 L= 0 9 (24) LT , R = 0 R where 0 is a zero matrix with suitable dimensions. 6 Results The optimisation problem in (14) is solved using Yalmip [Löfberg, 2004] and a controller K of order nK = 6 is obtained. A controller K̂, with the same weights as for the robust stabilising controller K, using the optimisation problem in (14) without the lmi (14d) is derived to show the importance of the extra lmi for robust stability and also what is lost in terms of performance. The controller gains |K| and |K̂| are shown in Figure 3, and they have a constant gain before 10−3 rad/s due to the pole at s = −0.001. The major diﬀerence is the notch for K̂ around 100 rad/s and the high gain for K for high frequencies. The robust stability can be analysed using the structured singular value (ssv). 6 201 Results 20 K K̂ [dB] 10 0 −10 −20 10−2 10−1 100 101 102 103 Frequency [rad/s] Figure 4: Structured singular values for robust stability using the controllers K and K̂. The system is robustly stable if the ssv for the closed loop system from wΔ to zΔ with respect to the uncertainty Δ is less than one for all frequencies. A thorough description of the ssv can be found in Skogestad and Postletwaite [2005]. Figure 4 shows the ssv for the closed loop system using K and K̂ and it can be seen that the ssv using K is less than one (0 dB) for all frequencies whereas the ssv using K̂ has a peak of approximately 15 dB. As a result K̂ cannot stabilise the system for all perturbations, as expected. The step responses for the controller K̂ using the linearised system in k = k high and the controller K using the linearised systems in k = k high and k = k low are shown in Figure 5. It can be seen that K̂ is better than K for the linearised system in the high stiﬀness region. It means that in order to get a controller that is robustly stable for k(δ), the performance has been impaired. It can also be seen that the performance for K is better in the high stiﬀness region than in the low stiﬀness region, since the nominal value k = k high . The ssv can also be used for analysing nominal performance and robust performance, see Skogestad and Postletwaite [2005]. The requirement is that the ssv should be less than one for diﬀerent systems with respect to some perturbations. Figure 6 shows the ssv for robust stability, nominal performance, and robust performance using K. It can be seen that the requirements for robust stability and nominal performance are satisﬁed. However, the requirement for robust performance is not satisﬁed. The reason is that the optimisation problem (14) does not take robust performance into account. Finally, simulation of the non-linear model using K is performed to show that the controller can handle dynamic changes in the stiﬀness parameter and not only stabilising the system for ﬁxed values of the parameter. The non-linear model is simulated in Simulink using the disturbance signal in Figure 7a, which is composed by steps and chirp signals. Figures 7b and 7c show the arm angle qa (t) on the arm side of the gearbox and the motor torque τ(t). From 0 s up to 25 s the system operates in the region 0 < Δq < Ψ most of the time except for short periods of time when the step disturbances occur. The arm disturbance after 25 s keeps 202 Paper F H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models 1.25 1 0.75 0.5 K, k high 0.25 K, k low K̂, k high 0 0 0.5 1 1.5 2 2.5 3 3.5 Time [s] Figure 5: Step response of the controllers K and K̂ using system linearised in k low and k high . The ﬁrst 0.15 s are magniﬁed to show the initial transients. 2 0 [dB] −5 −10 −15 −20 10−2 Robust stability Nominal performance Robust performance 10−1 100 101 102 103 Frequency [rad/s] Figure 6: Structured singular values for robust stability, nominal performance, and robust performance for the controller K. 203 Results wm wa w(t) 10 0 −10 0 10 20 30 40 50 Time [s] (a) Disturbance signal w(t) composed by steps and chirps. qa [mrad] 10 5 0 −5 −10 0 10 20 30 40 50 Time [s] (b) Arm angle qa (t) expressed on the arm side of the gearbox. 0 τ(t) [Nm] 6 −2 −4 −6 0 10 20 30 40 50 Time [s] (c) Motor torque τ(t). Figure 7: Disturbance signal for the simulation experiment and the obtained arm angle and motor torque. 204 Paper F H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models the system in the high stiﬀness region except for a few seconds in connection with the step disturbances. The stationary error for qa (t) at the end is due to the fact that the controller only uses qm (t) and the constant wa (t) is not observable in qm (t) hence qa (t) cannot be controlled to zero. The primary position qm (t) does not have any stationary error. 7 Conclusions A method to synthesise and design H∞ controllers for ﬂexible joints, with nonlinear spring characteristics, is presented. The non-linear model is linearised in a speciﬁc operating point, where the performance requirements should be full ﬁlled. Moreover, an uncertainty description of the stiﬀness parameter is utilised to get robust stability for the non-linear system in all operating points. The resulting non-linear and non-convex optimisation problem can be rewritten as a convex, yet conservative, lmi problem using the Lyapunov shaping paradigm and a change of variables, and eﬃcient solutions can be obtained using standard solvers. Using the proposed synthesis method an H∞ controller is computed for a speciﬁc model, where good performance can be achieved for high stiﬀness values while stability is achieved in the complete range of the stiﬀness parameter. A controller derived with the same performance requirement but without the additional stability constraint is included for comparison. By analysing the structured singular values for robust stability for the two controllers it becomes clear that the controller without the extra stability constraint will not be stable for the parameter variations introduced by the non-linear stiﬀness parameter. In the synthesis method it is assumed that the parameter δ changes arbitrarily fast, which is a conservative assumption for real systems. It would therefore be a good idea to have a limited change in δ in the future development of the method. Moreover, the use of a common Lyapunov matrix P must be relaxed to reduce the conservatism further. Here, the path following method from Hassibi et al. [1999]; Ostertag [2008] can be further investigated. Acknowledgement This work was supported by the Vinnova Excellence Center LINK-SIC and by the Excellence Center at Linköping-Lund in Information Technology, ELLIIT. Bibliography 205 Bibliography Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Bayesian state estimation of a ﬂexible industrial robot. Control Engineering Practice, 20(11):1220–1228, November 2012. Patrik Axelsson, Goele Pipeleers, Anders Helmersson, and Mikael Norrlöf. H∞ synthesis method for control of non-linear ﬂexible joint models. Accepted to the 19th IFAC World Congress, Cape Town, South Africa, 2014. Wenjie Chen and Masayoshi Tomizuka. Direct joint space state estimation in robots with multiple elastic joints. IEEE/ASME Transactions on Mechatronics, 19(2):697–706, April 2014. doi: 10.1109/TMECH.2013.2255308. Pascal Gahinet and Pierre Apkarian. A linear matrix inequality approach to H∞ control. International Journal of Robust and Nonlinear Control, 4(1):421–448, 1994. Arash Hassibi, Jonathan How, and Stephen Boyd. A path-following method for solving bmi problems in control. In Proceedings of the American Control Conference, pages 1385–1389, San Diego, CA, USA, June 1999. Johan Löfberg. YALMIP: A toolbox for modeling and optimization in Matlab. In Proceedings of the IEEE International Symposium on Computer Aided Control Systems Design, pages 248–289, Taipei, Taiwan, September 2004. URL http: //users.isy.liu.se/johanl/yalmip. Alexandre Megretski and Anders Rantzer. System analysis via integral quadratic constraints. IEEE Transactions on Automatic Control, 42(6):819–830, June 1997. Eric Ostertag. An improved path-following method for mixed H2 /H∞ controller design. IEEE Transactions on Automatic Control, 53(8):1967–1971, September 2008. Goele Pipeleers and Jan Swevers. Matlab-software mixedHinfsyn, 2013. Available at http://set.kuleuven.be/optec/Software/mixedhinfsyn. Michael Ruderman and Torsten Bertram. Modeling and observation of hysteresis lost motion in elastic robot joints. In Proceedings of the 10th International IFAC Symposium on Robot Control, pages 13–18, Dubrovnik, Croatia, September 2012. Hansjörg G. Sage, Michel F. de Mathelin, and Eric Ostertag. Robust control of robot manipulators: A survey. International Journal of Control, 72(16):1498– 1522, 1999. Carsten Scherer. LMI relaxations in robust control. European Journal of Control, 12(1):3–29, 2006. 206 Paper F H∞ -Synthesis Method for Control of Non-linear Flexible Joint Models Carsten Scherer, Pascal Gahinet, and Mahmoud Chilali. Multiobjective outputfeedback control via LMI optimization. IEEE Transactions on Automatic Control, 42(7):896–911, July 1997. Sigurd Skogestad and Ian Postletwaite. Multivariable Feedback Control, Analysis and Design. John Wiley & Sons, Chichester, West Sussex, England, second edition, 2005. Tegoeh Tjahjowidodo, Farid Al-Bender, and Hendrik Van Brussel. Nonlinear modelling and identiﬁcation of torsional behaviour in harmonic drives. In Proceedings of the International Conference on Noise and Vibration Engineering, pages 2785–2796, Leuven, Belgium, September 2006. Timothy D. Tuttle and Warren P. Seering. A nonlinear model of a harmonic drive gear transmission. IEEE Transactions on Robotics and Automation, 12(3):368– 374, June 1996. Keivan Zavari, Goele Pipeleers, and Jan Swevers. Multi-H∞ controller design and illustration on an overhead crane. In Proceedings of the IEEE Conference on Control Applications. Part of the IEEE Multi-Conference on Systems and Control, pages 670–674, Dubrovnik, Croatia, October 2012. Kemin Zhou, John C. Doyle, and Keith Glover. Robust and Optimal Control. Prentice Hall Inc., Upper Saddle River, NJ, USA, 1996. Paper G Estimation-based Norm-optimal Iterative Learning Control Authors: Patrik Axelsson, Rickard Karlsson and Mikael Norrlöf Edited version of the paper: Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Estimationbased norm-optimal iterative learning control. Submitted to Systems & Control Letters, 2014c. G Estimation-based Norm-optimal Iterative Learning Control Patrik Axelsson∗ , Rickard Karlsson∗∗ and Mikael Norrlöf∗ ∗ Dept. ∗∗ Nira of Electrical Engineering, Linköping University, SE–581 83 Linköping, Sweden [email protected], [email protected] Dynamics Teknikringen 6 SE-583 30 Linköping, Sweden rickard.karlsson @niradynamics.se Abstract In this paper the norm-optimal iterative learning control (ilc) algorithm for linear systems is extended to an estimation-based normoptimal ilc algorithm where the controlled variables are not directly available as measurements. The objective function in the optimisation problem is modiﬁed to incorporate not only the mean value of the estimated variable, but also information about the uncertainty of the estimate. It is further shown that if a stationary Kalman ﬁlter is used for linear time-invariant systems the ilc design is independent of the estimation method. Finally, the concept is extended to nonlinear state space models using linearisation techniques, where it is assumed that the full state vector is estimated and used in the ilc algorithm. Stability and convergence properties are also derived. 1 Introduction The iterative learning control (ilc) method [Arimoto et al., 1984; Moore, 1993] improves performance, for instance trajectory tracking accuracy, for systems that repeat the same task several times. Traditionally, a successful solution to such problems base the ilc control law on direct measurements of the control quantity. When this quantity is not directly available as a measurement, the controller must rely on measurements that indirectly relate to the control quantity, or the control quantity has to be estimated from other measurements. In Ahn et al. [2006]; Lee and Lee [1998] a state space model in the iteration domain is formulated for the error signal, and a kf is used for estimation. The difference to this paper is that in Ahn et al. [2006]; Lee and Lee [1998] it is assumed that the control error is measured directly, hence the kf is merely a low-pass ﬁlter, with smoothing properties, for reducing the measurement noise. In Wallén et al. [2009] it is shown that the performance of an industrial robot is signiﬁcantly increased if an estimate of the control quantity is used instead of measurements of 209 210 Paper G Estimation-based Norm-optimal Iterative Learning Control a related quantity. Still, the concept of ilc in combination with estimation of the control quantity, has not been given much attention in the literature. In this paper the estimation-based ilc framework, where the control quantity has to be estimated, is combined with an ilc design based on an optimisation approach, referred to as norm-optimal ilc [Amann et al., 1996]. The estimation problem is here formulated using recursive Bayesian methods. Extensions to non-linear systems, utilising linearisation techniques, are also presented. The contributions are summarised as 1. Extension of the objective function to include the full probability density function (pdf) of the estimated control quantity, utilising the KullbackLeibler divergence. 2. A separation lemma, stating that the extra dynamics introduced by the stationary kf is not necessary to include in the design procedure of the ilc algorithm. 3. Extensions to non-linear systems, including stability and convergence properties as well as additional appropriate approximations to simplify the ilc algorithm. 2 Iterative Learning Control (ILC) The ilc-method improves the performance of systems that repeat the same task multiple times. The systems can be open loop as well as closed loop, with internal feedback. The ilc control signal uk+1 (t) ∈ Rnu for the next iteration k + 1 at discrete time t is calculated using the error signal and the ilc control signal, both from the current iteration k. Diﬀerent types of update laws can be found in e.g. Moore [1993]. One design method is the norm-optimal ilc algorithm [Amann et al., 1996; Gunnarsson and Norrlöf, 2001]. The method solves minimise uk+1 ( · ) subject to N −1 t=0 N −1 ek+1 (t)2We + uk+1 (t)2Wu (1) 2 uk+1 (t) − uk (t) ≤ δ, t=0 where ek+1 (t) = r(t) − zk+1 (t) is the error, r(t) the reference signal, and zk+1 (t) n n the variable to be controlled. The matrices We ∈ S++z , and Wu ∈ S++u are weight matrices, used as design parameters, for the error and the ilc control signal, respectively1 . Using a Lagrange multiplier and a batch formulation (see Appendix A) of the 1 Sn denotes a n × n positive deﬁnite matrix. ++ 3 211 Estimation-based ILC for Linear Systems Estimation r(t) S uk (t) ẑk (t) yk (t) zk (t) Figure 1: System S with reference r(t), ilc input uk (t), measured variable yk (t) and controlled variable zk (t) at ilc iteration k and time t. system from uk+1 (t) and r(t) to zk+1 (t) gives the solution uk+1 =Q · (uk + L · ek ) Q L (2a) =(STzu W e Szu + W u + λI)−1 (λI + STzu W e Szu ) =(λI + STzu W e Szu )−1 STzu W e , (2b) (2c) where λ is a design parameter and Nn W e = IN ⊗ We ∈ S++ z , Nn W u = IN ⊗ Wu ∈ S++ u , (3) IN is the N × N identity matrix, ⊗ denotes the Kronecker product, Szu is the batch model from u to z, and ek = r − zk . The reader is referred to Amann et al. [1996]; Gunnarsson and Norrlöf [2001] for details of the derivation. The update equation (2a) is stable and monotone for all system descriptions Szu , i.e., the batch signal u converges to a constant value monotonically, see e.g. Barton et al. [2008]; Gunnarsson and Norrlöf [2001]. 3 Estimation-based ILC for Linear Systems The error ek (t) used in the ilc algorithm should be the diﬀerence between the reference r(t) and the controlled variable zk (t) at iteration k. In general the controlled variable zk (t) is not directly measurable, therefore an estimation-based ilc algorithm must be used, i.e., the ilc algorithm has to rely on estimates based on measurements of related quantities. The situation is schematically described in Figure 1. 3.1 Estimation-based Norm-optimal ILC A straightforward extension to the standard norm-optimal ilc method is to use the error êk (t) = r(t) − ẑk (t) in the equations from Section 2, where ẑk (t) is an estimate of the controlled variable. The estimate ẑk (t) can be obtained using e.g., a Kalman ﬁlter (kf) for the linear case, or an extended Kalman ﬁlter (ekf) for the non-linear case [Kailath et al., 2000]. Linear systems are covered in this section while Section 4 extends the ideas to non-linear systems. In both cases it must be 212 Paper G Estimation-based Norm-optimal Iterative Learning Control assumed that i) the system is observable, and ii) the ﬁlter, used for estimation, converges. The solution to the optimisation problem can be obtained in a similar way as for the standard norm-optimal ilc problem in Section 2, where the detailed derivation is presented in Amann et al. [1996]; Gunnarsson and Norrlöf [2001]. An important distinction, compared to standard norm-optimal ilc, relates to what models are used in the design. In the estimation-based approach, the model from uk+1 (t) and r(t) to ẑk+1 (t) is used, i.e., the dynamics from the kf must be included in the design, while in the standard norm-optimal design, the model from uk+1 (t) and r(t) to zk+1 (t) is used. Let the discrete-time state space model be given by xk (t + 1) = A(t)xk (t) + Bu (t)uk (t) + Br (t)r(t) + G(t)wk (t), (4a) yk (t) = Cy (t)xk (t) + Dyu (t)uk (t) + Dyr (t)r(t) + vk (t), (4b) zk (t) = Cz (t)xk (t) + Dzu (t)uk (t) + Dzr (t)r(t), (4c) where the process noise wt ∼ N (0, Qt ), and the measurement noise vt ∼ N (0, Rt ). A batch model (see Appendix A for deﬁnitions) for the output yk and the estimate ẑk can be written as yk = C y Φx0 + Syu uk + Syr r, 9 x̂0 + Sẑu uk + Sẑr r + Sẑy y , ẑk = C z Φ k (5a) (5b) where w(t) and v(t) have been replaced by their expected values, which are both equal to zero, in the output model (5a). The kf batch formulation has been used in the model of the estimate in (5b). The optimal solution is now given by uk+1 =Q · (uk + L · êk ) Q L =(STu W e Su + W u + λI)−1 (λI + STu W e Su ) =(λI + STu W e Su )−1 STu W e , (6a) (6b) (6c) where Su = Sẑu +Sẑy Syu (see (26), (30) for details), and êk = r− ẑk . The expressions for Q and L in (6) are similar to (2). The only diﬀerence is that Su is used instead of Szu . Theorem 1 presents a result for the special case of lti-systems using the stationary kf2 . Theorem 1. (Separation lemma for estimation-based ilc): Given an lti-system and the stationary kf with constant gain matrix K, then the matrices Su and Szu are equal, hence the ilc algorithm (2) can be used for the estimation-based normoptimal ilc. Proof: The result follows from the fact that it can be shown algebraically that Su and Szu are equal. The proof involves several algebraical manipulations, and 2 The stationary Kalman ﬁlter uses a constant gain K which is the solution to an algebraic Riccati equation, see Kailath et al. [2000]. 3 Estimation-based ILC for Linear Systems 213 only the ﬁrst steps of the calculations, in the case of Dyu = 0 and Dzu = 0, are pre9u + 9B sented here. From Appendix A it follows that Szu = C z ΨBu and Su = C z Ψ 9 u gives Su = C z ΨΓ 9 2 KC y ΨBu . The structure of B 9 +Ψ 9 2 KC y Ψ Bu , where C zΨ Γ = diag I − KCy , . . . , I − KCy , 0 . After some manipulations it can be shown 9 +Ψ 9 2 KC y Ψ = Ψ, hence Szu = Su . The case Dyu 0 and Dzu 0 gives that ΨΓ similar, but more involved, calculations. The result from Theorem 1 makes it computationally more eﬃcient to compute the matrices Q and L, since the matrix Szu requires fewer calculations to obtain, compared to the matrix Su . Even if the iterative kf update is used, the Kalman gain K converges eventually to the stationary value for lti systems. If this is done reasonably fast, then Su ≈ Szu can be a good enough approximation to use. The stability result for the ilc algorithm in (6) is given in Theorem 2. Theorem 2. (Stability and monotonic convergence): The estimation-based ilc algorithm (6) is stable and monotonically convergent for all systems given by (5). Proof: Assuming that the kf is initialised with the same covariance matrix P0 at each iteration, makes the sequence of Kalman gains K(t), t = 0, . . . , N − 1 the same for diﬀerent ilc iterations since P and K are independent of the control signal u. The system matrices Sẑu and Sẑy are therefore constant over the ilc iterations, hence the same analysis for stability and monotone convergence as for the standard norm-optimal ilc, presented in Barton et al. [2008]; Gunnarsson and Norrlöf [2001], can be used. 3.2 Utilising the Complete PDF for the ILC Error Usually, only the expected value of the probability density function (pdf) p(z(t)) of the estimated control quantity, which can be obtained by the kf or the ekf, is used. However, since the kf and ekf provides both the mean estimate and the covariance matrix for the estimation error, it is possible to use the full pdf in the design. The norm-optimal ilc is therefore extended to handle both the expected value and the covariance matrix of the estimated control error. The objective of ilc is to achieve an error close to zero. Only considering the mean value is insuﬃcient since a large variance means that there is a high probability that the actual error is not close to zero. Hence, the mean of the distribution should be close to zero and at the same time the variance should be small. To achieve this the pdf of the error is compared with a desired pdf with zero mean and small variance, using the Kullback-Leibler (kl) divergence [Kullback and Leibler, 1951]. The objective function (1) is modiﬁed by replacing the term ek+1 (t)2We with the kl-divergence Dkl (q(ek+1 (t))||p(ek+1 (t))), where p(ek+1 (t)) is the actual distribution of the error given by the estimator, and q(ek+1 (t)) is the desired distribution for the error, which becomes a design parameter. 214 Paper G Estimation-based Norm-optimal Iterative Learning Control Using a kf to estimate the state vector and calculating the control error according to ê(t) = r(t) − ẑ(t) gives that ê(t) has a Gaussian distribution [Kailath et al., 2000] x̂(t) ∼ N (x; x̂t|t , Pt|t ), (7) where x̂t|t denotes the mean value and Pt|t denotes the covariance matrix for the estimation error. In both cases the value is valid at time t given measurements up to time t. Moreover, let the estimated control quantity be given by ẑ(t) = Cz (t)x̂(t) + Dzu (t)u(t) + Dzr (t)r(t). (8) Since (8) is an aﬃne transformation of a Gaussian distribution ẑ(t) ∼ N (z; ẑ(t|t), Σ z (t|t)) , ẑ(t|t) = Cz (t)x̂t|t + Dzu (t)u(t) + Dzr (t)r(t), Σ z (t|t) = Cz (t)Pt|t Cz (t)T . (9a) (9b) (9c) Finally, the error ê(t) = r(t) − ẑ(t) has the distribution ê(t) ∼ p(e(t)) = N (e; ê(t|t), Σ e (t|t)) , ê(t|t) = r(t) − ẑ(t|t), Σ e (t|t) = Cz (t)Pt|t Cz (t)T . (10a) (10b) (10c) For the linear case where êk (t) is Gaussian distributed according to (10) it is assumed that the desired distribution should resemble q(e(t)) = N (e; 0, Σ), where Σ is a design parameter. Straightforward calculations utilising two Gaussian distributions give the kl-divergence [Arndt, 2001] ⎛ 1 ⎜⎜⎜ Dkl (N (x; μ0 , Σ 0 )||N (x; μ1 , Σ 1 )) = ⎝⎜ tr Σ −1 2 Σ1 2 ⎞ ⎟⎟ |Σ 2 | T −1 (11) + (μ1 − μ2 ) Σ 2 (μ1 − μ2 ) + log − nx ⎟⎟⎠, |Σ 1 | where tr is the trace operator and | · | the determinant of a matrix. Therefore, the kl-divergence using p(e(t)) from (10) and q(e(t)) = N (e; 0, Σ) gives 1 ê(t|t)T Σ −1 (12) e (t|t)ê(t|t) + ξ, 2 where ξ is a term which does not depend on x, u, and y. Here we used that the covariance matrix for the estimation error is independent of the observed data, as a consequence of the kf update equations. It only depends on the model parameters and the initial value P0 . The objective function is ﬁnally modiﬁed by replacing the term ek+1 (t)2We in (1) with Dkl (q(e(t))||p(e(t))) = êk+1 (t|t)2Σ −1 (t|t) . e (13) The interpretation of the result is that, if the estimated quantity is certain it will aﬀect the objective function more than if it is less certain, i.e., an estimated er- 3 Estimation-based ILC for Linear Systems 215 ror signal with large uncertainty has a low weight. The optimisation problem is solved in the same way as in Section 3.1. The only diﬀerence is that the weight matrix for the error in batch form, now is given by N nz −1 W e = diag Σ −1 e (0|0), . . . , Σ e (N − 1|N − 1) ∈ S++ . Remark 1. The separation lemma in Theorem 1 and the stability result in Theorem 2 are not aﬀected when the full pdf is included in the objective function. Remark 2. By interchanging the distributions p( · ) and q( · ) the result will be the norm of the mean error but now weighted with Σ −1 , which is a tuning parameter, hence no information of the covariance matrix of the control error is used in the design. 3.3 Structure of the Systems used for ILC and Estimation In the derivation of the estimation-based norm-optimal ilc algorithm in Section 3.1 it is assumed that the kf takes the signals r(t) and uk (t) as inputs. However, in general the estimation algorithm does not always use r(t) and uk (t) as input. As an example, a system with a feedback loop usually uses the input to the controlled system for estimation, not the input to the controller. More general, the estimation algorithm only uses a part of the full system for estimation, whereas the other part is completely known or not interesting to use for estimation. Nevertheless, the system (4) is a valid description of the estimation-based ilc since the internal signal used for estimation can be written as an output from a system with r(t), uk (t), and yk (t) as inputs. Let τ k (t) be the known signal used for estimation, hence the estimated variable can be written as ẑk (t) = Fẑτ (q)τ k (t) + Fẑy (q)yk (t). (14) Moreover, the signal τ k (t) can be seen as an output from a system with r(t), uk (t), and yk (t) as inputs, hence τ k (t) = Fτr (q)r(t) + Fτu (q)uk (t) + Fτy (q)yk (t). (15) Combining (14) and (15) gives ẑk (t) = Sẑr (q)r(t) + Sẑu (q)uk (t) + Sẑy (q)yk (t), (16) where Sẑr (q) = Fẑτ (q)Fτr (q), Sẑu (q) = Fẑτ (q)Fτu (q), Sẑy (q) = Fẑτ (q)Fτy (q) + Fẑy (q), which is in the form described in Figure 1. It is straightforward to take this into consideration when deriving the ilc update control algorithm since it only changes the deﬁnitions of Sẑu , Sẑr , and Sẑy . Note that the dimension of the state vector describing (5a) and (5b) can diﬀer since (5b) is constructed using only a part of the complete system. 216 4 Paper G Estimation-based Norm-optimal Iterative Learning Control Estimation-based ILC for Non-linear Systems Iterative learning control for non-linear systems has been considered in e.g. Avrachenkov [1998]; Lin et al. [2006]; Xiong and Zhang [2004]. Using a batch formulation of a non-linear state space model gives the following set of non-linear equations yk = F (uk ). The goal for the control is to solve for u such that r = F (u), which has an iterative solution according to uk+1 = uk + Lk ek . The proposed solutions in Avrachenkov [1998]; Lin et al. [2006]; Xiong and Zhang [2004] all have in common that they calculate the gain Lk using diﬀerent approximations of the Newton method, which is a common method for solving non-linear equations. In Lin et al. [2006] the solution is rewritten, giving ﬁrst a linearised system, where standard linear ilc methods can be applied, and then a ﬁnal update of the ilc signal for the non-linear system. Nothing is stated about how to generally obtain the state trajectory for the linearisation step. The method proposed here is to directly transform the non-linear system to a linear time-varying system, and then use the standard norm-optimal method. The linear state space model is obtained by linearising around an estimate of the complete state trajectory obtained from the previous ilc iteration. A necessary assumption is to have uk+1 close to uk , in order to get accurate models for the optimisation. It is possible to achieve uk+1 close to uk by assigning λ a large enough value. The drawback is that the convergence rate can become slow. 4.1 Solution using Linearised Model The non-linear model for iteration k and discrete time t xk (t + 1) = f (xk (t), uk (t), r(t)) yk (t) = hy (xk (t), uk (t), r(t)) (17b) zk (t) = hz (xk (t), uk (t), r(t)) (17c) (17a) is linearised around x̂k−1 (t|t), uk−1 (t), and r(t), yielding the following linear timevarying model xk (t + 1) = Ak−1 (t)xk (t) + Bk−1 (t)uk (t) + sx,k−1 (t) yk (t) = Cy,k−1 xk (t) + Dy,k−1 (t)uk (t) + sy,k−1 (t) zk (t) = Cz,k−1 xk (t) + Dz,k−1 (t)uk (t) + sz,k−1 (t) where ∂f , Bk−1 (t) = ∂x ∂hz Cz,k−1 (t) = , Dy,k−1 (t) = ∂x Ak−1 (t) = ∂hy ∂f , Cy,k−1 (t) = , ∂u ∂x ∂hy ∂hz , Dz,k−1 (t) = , ∂u ∂u 4 217 Estimation-based ILC for Non-linear Systems all evaluated at the point x = x̂k−1 (t|t), u = uk−1 (t), and r = r(t), with sx,k−1 (t) =f (x̂k−1 (t|t), uk−1 (t), r(t)) − Ak−1 (t)x̂k−1 (t|t) − Bk−1 (t)uk−1 (t) sy,k−1 (t) =hy (x̂k−1 (t|t), uk−1 (t), r(t)) − Cy,k−1 (t)x̂k−1 (t|t) − Dy,k−1 (t)uk−1 (t) sz,k−1 (t) =hz (x̂k−1 (t|t), uk−1 (t), r(t)) − Cz,k−1 (t)x̂k−1 (t|t) − Dz,k−1 (t)uk−1 (t). The linearised quantities only depend on the previous ilc iteration, hence they are known at the current iteration. Using the linearised state space model gives, as before, the estimate ẑk (t) and the output yk (t) in batch form as yk =C y,k−1 Φ k−1 x0 + Syu,k−1 uk + Sysx ,k−1 sx,k−1 + sy,k−1 , 9 k−1 x̂0 + Sẑu,k−1 uk + Sẑy,k−1 y ẑk =C z,k−1 Φ k + Sẑsx ,k−1 sx,k−1 + Sẑsy ,k−1 sy,k−1 + sz,k−1 , where all the matrices in the right hand side are dependent of k, and the vectors sx,k , sy,k , and sz,k are stacked versions of sx,k (t), sy,k (t), and sz,k (t). The norm-optimal ilc problem can be formulated and solved in the same way as before. Unfortunately, since the batch form matrices are dependent of k, the terms including r, x̂0 , x0 , sx,k , sy,k , and sz,k cannot be removed similar to what has been done before. Note that the weight matrix for the error is also dependent of k, since it consists of the covariance matrices for the control error. The solution is therefore given by −1 < uk+1 = STu,k W e,k Su,k + W u + λI λuk + STu,k W e,k 9 k x̂0 − Sẑs ,k sx,k − Sẑs ,k sy,k × r − C z,k Φ x y = − sz,k − Sẑy,k (C y,k Φ k x0 + Sysx ,k sx,k + sy,k ) , (18) where Su,k = Sẑu,k + Sẑy,k Syu,k (see (26), (30) for details). The initial condition x0 is of course not known, hence x̂0 must be used instead. Remark 3. If the change in uk+1 − uk is suﬃciently small, then the approximation Syu,k−1 ≈ Syu,k and similar for the rest of the quantities is appropriate. Given the approximation the ilc algorithm (18) is simpliﬁed to uk+1 =Qk · (uk + Lk · êk ) Qk Lk =(STu,k W e,k Su,k + W u + λI)−1 (λI + STu,k W e,k Su,k ) =(λI + STu,k W e,k Su,k )−1 STu,k W e,k . (19a) (19b) (19c) Note that the change in u depends strongly on the choice of λ. 4.2 Stability Analysis for the Linearised Solution To analyse the stability of non-linear systems, utilising the above described linearisation technique, it is necessary to use the convergence results from Norrlöf and Gunnarsson [2002], which are based on theory for discrete time-variant systems. The stability property for the iteration-variant ilc system (18) is presented 218 Paper G Estimation-based Norm-optimal Iterative Learning Control in Theorem 3. Theorem 3. (Iteration-variant stability): If f ( · ), hy ( · ) and hz ( · ) are diﬀerentiable, then the system using the ilc algorithm (18) is stable. Proof: From [Norrlöf and Gunnarsson, 2002, Corollary 3] it follows that (18) is stable if and only if −1 σ̄ STu,k W e,k Su,k + W u + λI λ < 1, (20) for all k. The construction of W e,k from the covariance matrices, gives that W e,k ∈ S++ for all k. This fact, together with the fact that W u ∈ S++ , and λ ∈ R+ guarantee that (20) is fulﬁlled for all k, hence the system with the ilc algorithm is stable. 5 Conclusions and Future Work An estimation-based norm-optimal ilc control algorithm was derived where the regular optimisation criteria for norm-optimal ilc was extended to an optimisation criteria including the ﬁrst and second order moments of the posterior pdf, enabling better performance. For lti-systems it was shown that the control law can be separated from the estimator dynamics. Extensions of the results to non-linear systems using linearisation were also presented together with stability and convergence results. The estimation-based norm-optimal ilc framework enables a systematic model-based design of ilc algorithms for linear as well as non-linear systems, where the controlled variables are not directly available as measurements. Future work includes smoothing instead of ﬁltering, to obtain the estimates, and to include the smoother dynamics in the ilc design. Also, investigating the use of the kl-divergence when the estimates are obtained using a particle ﬁlter or particle smoother is a possible extension. Appendix A State Space Model and Kalman Filter in Batch Form For the norm-optimal ilc algorithm it is convenient to describe the state space model over the whole time horizon in batch form. The discrete-time state space model x(t + 1) = A(t)x(t) + Bu (t)u(t), y(t) = Cy (t)x(t) + Dyu (t)u(t), (21a) (21b) A 219 State Space Model and Kalman Filter in Batch Form has the following update formula for the state vector [Rugh, 1996] x(t) = φ(t, t0 )x(t0 ) + t−1 φ(t, j + 1)Bu (j)u(j), (22) j=t0 for t ≥ t0 + 1, where the discrete-time transition matrix φ(t, j) is 7 A(t − 1) · . . . · A(j), t ≥ j + 1 φ(t, j) = . I, t=j (23) Using (21b) and (22), the output is given by y(t) = Cy (t)φ(t, t0 )x(t0 ) + Dyu (t)u(t) + t−1 Cy (t)φ(t, j + 1)Bu (j)u(j). (24) j=t0 Introducing the vectors T x = x(t0 )T . . . x(t0 + N )T T u = u(t0 )T . . . u(t0 + N )T T y = y(t0 )T . . . y(t0 + N )T (25a) (25b) (25c) gives the solution from t = t0 to t = t0 + N as x = Φx(t0 ) + ΨBu u and for the output as y = C y Φx(t0 ) + (C y ΨBu + D yu ) u. (26) =Syu Here x(t0 ) is the initial value, and Bu = diag Bu (t0 ), . . . , Bu (t0 + N − 1), 0 C y = diag Cy (t0 ), . . . , Cy (t0 + N ) D yu = diag Dyu (t0 ), . . . , Dyu (t0 + N ) ⎞ ⎛ I ⎟ ⎜⎜ ⎜⎜ φ(t + 1, t ) ⎟⎟⎟ ⎜⎜ 0 0 ⎟ ⎟⎟ ⎜⎜ ⎟ Φ = ⎜⎜⎜⎜ φ(t0 + 2, t0 ) ⎟⎟⎟⎟ .. ⎟⎟⎟ ⎜⎜⎜ . ⎟⎟ ⎜⎜ ⎠ ⎝ φ(t0 + N , t0 ) ⎛ 0 0 0 ⎜⎜ ⎜⎜ I 0 0 ⎜⎜ ⎜⎜ ⎜ φ(t + 2, t + 1) I 0 0 0 ⎜ Ψ = ⎜⎜ ⎜⎜ .. .. .. ⎜⎜ . . . ⎜⎝ φ(t0 + N , t0 + 1) φ(t0 + N , t0 + 2) · · · (27a) (27b) (27c) (27d) ⎞ · · · 0⎟⎟ · · · 0⎟⎟⎟⎟ ⎟ · · · 0⎟⎟⎟ ⎟ . ⎟⎟ .. . .. ⎟⎟⎟⎟ ⎠ I 0 (27e) 220 Paper G Estimation-based Norm-optimal Iterative Learning Control The Kalman ﬁlter can be written in a similar batch form as described above. Let the state space model be given by x(t + 1) = A(t)x(t) + Bu (t)u(t) + G(t)w(t), (28) y(t) = Cy (t)x(t) + Dyu (t)u(t) + v(t), (29) where w(t) ∼ N (0, Q(t)), and v(t) ∼ N (0, R(t)). From the ﬁlter recursions [Kailath et al., 2000] we get x̂(t + 1|t + 1) = I − K(t + 1)Cy (t + 1) A(t)x̂(t|t) + I − K(t + 1)Cy (t + 1) Bu (t)u(t) − K(t + 1)Dyu (t + 1)u(t + 1) + K(t + 1)y(t + 1) 9 yu (t + 1)u(t + 1) + K(t + 1)y(t + 1), 9 =A(t)x̂(t|t) +9 Bu (t)u(t) − D where, K(t) is the Kalman gain given by the recursion P(t|t − 1) =A(t − 1)P(t − 1|t − 1)A(t − 1)T + G(t − 1)Q(t − 1)G(t − 1)T −1 K(t) =P(t|t − 1)Cy (t)T Cy (t)P(t|t − 1)Cy (t)T + R(t) P(t|t) = I − K(t)Cy (t) P(t|t − 1). The update formula for the estimated state vector is ﬁnally given by 9 t0 )x̂(t0 |t0 ) + x̂(t|t) = φ(t, t−1 9 j + 1)9 φ(t, Bu (j)u(j) j=t0 − t j=t0 +1 9 j)D 9 yu (j)u(j) + φ(t, t 9 j)K(j)y(j), φ(t, j=t0 +1 9 is deﬁned as in (23), with A(t) 9 instead of A(t). The kf recursion in batch where φ form becomes 9u − Ψ 9 yu )u + Ψ 9 x̂(t0 |t0 ) + (Ψ 9B 9 2D 9 2 Ky, x̂ = Φ 9 u are given in (27) with A(t) 9 Ψ, 9 and B 9 and 9 Bu (t) instead of A(t) and where Φ, Bu (t). The remaining matrices are deﬁned as 9 IN nx 9 2 = 0(N +1)n ×n Ψ Ψ x x 0nx ×N nx 9 yu = diag 0, D 9 yu (t0 + 1), . . . , D 9 yu (t0 + N ) D K = diag 0, K(t0 + 1), . . . , K(t0 + N ) . Finally, the batch formulation for the variable z(t) = Cz (t)xt + Dzu (t)ut , A 221 State Space Model and Kalman Filter in Batch Form is given by 9 yu ) + D zu u + C z Ψ 9u − Ψ 9B 9 2D 9 x̂(t0 |t0 ) + C z (Ψ 9 2 K y, ẑ = C z Φ =Sẑu (30) =Sẑy where C z and D zu are given in (27) using Cz (t) and Dzu (t). Acknowledgement This work was supported by the Vinnova Excellence Center LINK-SIC, by the Excellence Center at Linköping-Lund in Information Technology, ELLIIT, and by the SSF project Collaborative Localization. 222 Paper G Estimation-based Norm-optimal Iterative Learning Control Bibliography Hyo-Sung Ahn, Kevin L. Moore, and YangQuan Chen. Kalman ﬁlter-augmented iterative learning control on the iteration domain. In Proceedings of the American Control Conference, pages 250–255, Minneapolis, MN, USA, June 2006. Notker Amann, David H. Owens, and Eric Rogers. Iterative learning control for discrete-time systems with exponential rate of convergence. IEE Proceedings Control Theory and Applications, 143(2):217–224, March 1996. Suguru Arimoto, Sadao Kawamura, and Fumio Miyazaki. Bettering operation of robots by learning. Journal of Robotic Systems, 1(2):123–140, 1984. Christoph Arndt. Information Measures. Springer-Verlag, Berlin, Heidelberg, Germany, 2001. Konstantin E. Avrachenkov. Iterative learning control based on quasi-Newton methods. In Proceedings of the 37th IEEE Conference on Decision and Control, pages 170–174, Tampa, FL, USA, December 1998. Patrik Axelsson, Rickard Karlsson, and Mikael Norrlöf. Estimation-based normoptimal iterative learning control. Submitted to Systems & Control Letters, 2014. Kira Barton, Jeroen van de Wijdeven, Andrew Alleyne, Okko Bosgra, and Maarten Steinbuch. Norm optimal cross-coupled iterative learning control. In Proceedings of the 47th IEEE Conference on Decision and Control, pages 3020–3025, Cancun, Mexico, December 2008. Svante Gunnarsson and Mikael Norrlöf. On the design of ilc algorithms using optimization. Automatica, 37(12):2011–2016, December 2001. Thomas Kailath, Ali H. Sayed, and Babak Hassibi. Linear Estimation. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, 2000. S. Kullback and R. A. Leibler. On information and suﬃciency. Annals of Mathematical Statistics, 22(1):79–86, March 1951. Kwang Soon Lee and Jay H. Lee. Iterative Learning Control: Analysis, Design, Integration and Application, chapter Design of Quadratic Criterion-based Iterative Learning Control, pages 165–192. Kluwer Academic Publishers, Boston, MA, USA, 1998. T. Lin, David H. Owens, and Jari Hätönen. Newton method based iterative learning control for discrete non-linear systems. International Journal of Control, 79(10):1263–1276, 2006. Kevin L. Moore. Iterative Learning Control for Deterministic Systems. Advances in Industrial Control. Springer-Verlag, London, UK, 1993. Bibliography 223 Mikael Norrlöf and Svante Gunnarsson. Time and frequency domain convergence properties in iterative learning control. International Journal of Control, 75(14):1114–1126, 2002. Wilson J. Rugh. Linear System Theory. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, second edition, 1996. Johanna Wallén, Svante Gunnarsson, Robert Henriksson, Stig Moberg, and Mikael Norrlöf. ilc applied to a ﬂexible two-link robot model using sensorfusion-based estimates. In Proceedings of the 48th IEEE Conference on Decision and Control, pages 458–463, Shanghai, China, December 2009. Z. Xiong and J. Zhang. Batch-to-batch optimal control of nonlinear batch processes based on incrementally updated models. IEE Proceedings Control Theory and Applications, 151(2):158–165, March 2004. Paper H Controllability Aspects for Iterative Learning Control Authors: Patrik Axelsson, Daniel Axehill, Torkel Glad and Mikael Norrlöf Edited version of the paper: Patrik Axelsson, Daniel Axehill, Torkel Glad, and Mikael Norrlöf. Controllability aspects for iterative learning control. Submitted to International Journal of Control, 2014a. H Controllability Aspects for Iterative Learning Control Patrik Axelsson, Daniel Axehill, Torkel Glad and Mikael Norrlöf Dept. of Electrical Engineering, Linköping University, SE–581 83 Linköping, Sweden [email protected], [email protected], [email protected], [email protected] Abstract This paper considers the aspects of controllability in the iteration domain for systems that are controlled using iterative learning control (ilc). The focus is on controllability for a proposed state space model in the iteration domain and it relates to an assumption often used to prove convergence of ilc algorithms. It is shown that instead of investigating controllability it is more suitable to use the concept of target path controllability (tpc), where it is investigated if the output of a system can follow a trajectory instead of the ability to control the system to an arbitrary point in the state space. Finally, a simulation study is performed to show how the ilc algorithm can be designed using the lq-method, if the state space model in the iteration domain is output controllable. The lq-method is compared to the standard norm-optimal ilc algorithm, where it is shown that the control error can be reduced signiﬁcantly using the lq-method compared to the norm-optimal approach. 1 Introduction Iterative learning control (ilc) is a method to improve the control of processes that perform the same task repeatedly [Arimoto et al., 1984; Moore, 1993]. A good example of such a process is an industrial robot performing arc welding or laser cutting in a general production situation. The system used for ilc can be both an open loop system as well as a closed loop system, therefore let the system be described by Figure 1. Usually, the ilc control signal uk (t) ∈ Rnu is updated according to −1 N −1 uk+1 (t) = F {uk (i)}N , {e (i)} t = 0, . . . , N − 1, k i=0 i=0 , 227 228 Paper H Controllability Aspects for Iterative Learning Control r(t) uk (t) S yk (t) Figure 1: System used for ilc. It can be either an open or closed loop system. The reference signal is denoted by r(t), the ilc control signal by uk (t) and the output signal by yk (t), where k is the ilc iteration index and t the time index. where ek (t) = r(t)−yk (t) is the control error, r(t) ∈ Rny the reference signal, yk (t) ∈ Rny the measurement signal, k the iteration index, t the time index and F ( · ) is an update function. The main task is to ﬁnd an update function that is able to drive the error to zero as the number of iterations tends to inﬁnity, i.e., ek (t) → 0, k → ∞, ∀t. (1) For the convergence proof it is usually suitable to use a batch description of the system, yk = Su uk + Sr r. (2) Here, the vectors yk , uk , and r are composed by the vectors yk (t), uk (t), and r(t) stacked on each other for t = 0, . . . , N − 1, and the matrices Su and Sr are lower triangular Toeplitz matrices containing the Markov parameters for the system, see Appendix A for details. In Lee and Lee [1998, 2000]; Lee et al. [2000] it is proven that (1) holds under the assumption that Su has full row rank. Moreover, in Amann et al. [1996] it is assumed that ker STu = ∅ which is equivalent to Su having full row rank. An important implication from this assumption is that it is necessary to have at least as many control signals as measurement signals. Even if the numbers of measurement signals and control signals are the same it can not be guaranteed that the full rank requirement is fulﬁlled. This paper investigates what it means that Su has full row rank, based on a state space model in the iteration domain for which diﬀerent types of controllability properties are considered. The result shows that the requirement of full row rank of Su is equivalent to the proposed state space model being output controllable. Using the deﬁnition of controllability, the interpretation of having a rank deﬁcient matrix Su , for a general system, is discussed. The aspects of controllability are then extended to target path controllability (tpc) [Engwerda, 1988] which is shown to be a more suitable requirement for ilc. Tpc naturally leads to the concept of “lead-in”, which is about extending the trajectory with a part in the beginning, where it is not important to have perfect trajectory tracking, see e.g. Wallén et al. [2011]. It is also important to realise that tpc can help in the design of the reference trajectory to achieve tracking with as short lead-in as possible. In this paper the focus is on time-invariant systems. However, extensions to timevarying systems are straightforward using standard results for time-varying systems. In general, non-linear phenomena, such as control signal limitations, must also be considered, but that has been omitted in this work. 2 2 State Space Model in the Iteration Domain 229 State Space Model in the Iteration Domain The state space model in the iteration domain uses the batch formulation of the system dynamics. The batch formulation is known as the lifted system representation in the ilc community and is used both for design of ilc update laws, e.g. norm-optimal ilc [Amann et al., 1996; Gunnarsson and Norrlöf, 2001], as well as for analysis of stability and convergence. In this work, the following linear time-invariant state space model in discrete-time x(t + 1) = Ax(t) + Bu u(t) + Br r(t) + Bw w(t), y(t) = Cx(t) + v(t), (3a) (3b) where w(t) ∼ N (0, Q) is the process noise and v(t) ∼ N (0, R) is the measurement noise, is considered. It is henceforth assumed that the system in (3) is both controllable and observable. The following batch formulation of the system x =Φx(0) + Sxu u + Sxr r, (4a) y =Cx, (4b) where w(t) and v(t) have been replaced with their expected values which are equal to zero, can now be obtained according to Appendix A. At ilc iteration k and k + 1 it holds that xk = Φx(0) + Sxu uk + Sxr r, (5) xk+1 = Φx(0) + Sxu uk+1 + Sxr r, (6) where the initial state x(0) and the reference r repeats for all iterations, which are two common assumptions when applying ilc [Arimoto, 1990]. Subtracting (5) from (6) gives the following expression xk+1 = xk + Sxu (uk+1 − uk ) = xk + Sxu Δuk , (7) where Δuk = uk+1 − uk is considered as a new control signal. The state space model in the iteration domain is therefore given by xk+1 = xk + Sxu Δuk , yk = Cxk . 3 (8a) (8b) Controllability An important property for state space models is controllability, which considers the ability to control the system to a predeﬁned state or output. This section considers necessary and suﬃcient conditions for the system in (8) to be controllable. First, state controllability is investigated and second, output controllability is investigated. 230 Paper H 3.1 State Controllability Controllability Aspects for Iterative Learning Control Controllability can formally be stated according to Deﬁnition 1. Deﬁnition 1 (Controllability [Rugh, 1996]). A linear time-invariant state space model is called controllable if given any state xf there is a positive integer tf and an input signal u(t) such that the corresponding response of the system, beginning at x(0) = 0, satisﬁes x(tf ) = xf . Theorem 1 (Controllability [Rugh, 1996]). An lti system is controllable if and only if the rank of the controllability matrix C is equal to the state dimension. From Theorem 1 it follows that the system in (8) is controllable if and only if rank C = N nx . Since the dynamics for (8) is an integrator, the controllability matrix C is given by Sxu repeated N times, i.e., C = Sxu · · · Sxu (9) and the rank is simply given by rank C = rank Sxu . (10) The system is therefore controllable if and only if rank Sxu = N nx . A necessary and suﬃcient condition for controllability of (8) is presented in Theorem 2. Theorem 2. System (8) in the iteration domain is controllable according to Definition 1 if and only if rank Bu = nx . Proof: Exploiting the structure of Sxu gives ⎞ ⎛ ⎞⎛ 0 · · · 0⎟⎟ ⎜⎜Bu 0 · · · 0 ⎟⎟ ⎜⎜ I ⎟ ⎟⎟ ⎜⎜ ⎜⎜ I · · · 0⎟⎟ ⎜⎜ 0 Bu · · · 0 ⎟⎟⎟ ⎜⎜ A ⎜ ⎟ ⎟ ⎜ ⎜ ⎟ ⎜ Sxu = ⎜⎜ . .. .. ⎟⎟ ⎜⎜ .. .. .. ⎟⎟⎟, .. .. ⎟⎟ ⎟⎟ ⎜⎜ . ⎜⎜ .. . . . . . . ⎟⎠ ⎟⎠ ⎜⎝ ⎜⎝ N −1 N −2 0 · · · Bu A ··· I 0 A =Ψ (11) =B where it can be noted that the matrix Ψ is square and triangular with all diagonal elements equal to 1. The determinant of a square triangular matrix is equal to the product of the diagonal elements [Lütkepohl, 1996], hence det Ψ = 1 giving that Ψ is non-singular. It now follows that rank Sxu = rank ΨB = rank B = N rank Bu . (12) The system is therefore controllable if and only if rank Bu = nx . Corollary 1. A necessary condition for system (8) to be controllable is that nu ≥ nx . 3 231 Controllability Proof: It is given that Bu ∈ Rnx ×nu , hence rank Bu ≤ min {nx , nu }. It is therefore necessary to have nu ≥ nx to be able to obtain rank Bu = nx . Remark 1. Controllability of the time domain system (3), i.e., rank Bu ABu · · · Anx −1 Bu = nx , (13) does not imply that the iteration domain system in (8) is controllable. 3.2 Output Controllability The system in (8) is, as shown above, not necessarily controllable. However, the requirement of controllability is very strict. Often, it is not of interest in ilc to control all the states but only the output. Therefore, it is more relevant to consider output controllability of the system. A formal deﬁnition of output controllability follows from Deﬁnition 1 with x replaced by y. The requirement for output controllability is that the output controllability matrix, denoted by C o , has full rank [Ogata, 2002], where C o = CBu CABu · · · CAnx −1 Bu D (14) for a general state space model parametrised by (A, B, C, D). A condition for output controllability for the system in (8) is presented in Theorem 3. Theorem 3. The system in (8) is output controllable if and only if rank CSxu = N ny . (15) Proof: From (9) and (14) it can be concluded that C o = CC for the system in (8). Hence, the system is output controllable if and only if C o = CSxu · · · CSxu (16) has full rank, i.e., rank C o = N ny . The result follows directly from the fact that rank C o = rank CSxu . Note that a general controllable lti system is not necessarily output controllable, and a general output controllable system is not necessarily controllable. It follows from (15) that if the system is output controllable the matrix CSxu must have N ny independent rows. Hence, the measurements in the time domain model (3) must be independent which holds if rank C = ny . Theorem 4 presents a necessary, but not suﬃcient, condition for output controllability. Theorem 4. Assume rank C = ny . A necessary condition for system (8) to be output controllable is that rank Bu ≥ ny . Proof: The rank of a product of two matrices is less than or equal to the minimum of the rank of each matrix [Lütkepohl, 1996], hence rank CSxu ≤ min {rank C, rank Sxu } = min {N rank C, N rank Bu } (17) 232 Paper H Controllability Aspects for Iterative Learning Control From the assumption rank C = ny it follows that a necessary condition to have rank CSxu = N ny is to have rank Bu ≥ ny In Section 1 it was mentioned that in Lee and Lee [1998, 2000]; Lee et al. [2000]; Amann et al. [1996] Su was assumed to have full row rank. In this work, Su = CSxu , hence the property (1) holds if the state space model (8) is output controllable. Section 4 will discuss the diﬃculties of having the system (8) controllable and output controllable. 3.3 Stabilisability When a system is uncontrollable it is important to make sure that the uncontrollable modes are asymptotically stable, referred to as stabilisability, see Deﬁnition 2. Deﬁnition 2 (Stabilisability [Rugh, 1996]). A linear system is stabilisable if there exists a state feedback gain L such that the closed-loop system x(t + 1) = (A − BL)x(t) (18) is exponentially stable. It follows from Deﬁnition 2 that a linear system is stabilisable if the uncontrollable modes are asymptomatically stable, i.e., the eigenvalues of A, that are associated with the uncontrollable modes, lie inside the unit circle. The uncontrollable modes can be obtained using a variable transformation, see e.g. Rugh [1996]. For a general state space system, the transformed system can be cast in the form 1 1 911 A 912 9 9 9 xk+1 xk A B (19) = + 1 uk 922 9 9 0 x2k+1 x2k 0 A 911 , 9 where 9 x1 is the controllable part, i.e., (A B1 ) is controllable, and 9 x2 is the uncontrollable part. The eigenvalues of the uncontrollable part are the eigenvalues 922 which must be inside the unit circle in order for the total system to be of A stabilisable. Considering the system in (8), it holds that all eigenvalues are equal to 1. Since a similarity transformation does not change the eigenvalues, the uncontrollable modes of (8) are only marginally stable, hence the system is not stabilisable. 4 Interpretation of the Controllability Requirements A discussion about the possibilities for the system in (8) to be (output) controllable is given in this section. It is shown that (output) controllability of system (8) is diﬃcult to achieve, both from theoretical and practical perspectives. In Rugh [1996] it is stated that controllability of the system (3) does not consider what happens after time tf . It only considers if it is possible to reach the desired state xf within the desired time interval. Moreover, a single input system with 4 Interpretation of the Controllability Requirements 233 state dimension nx can require nx time steps to be able to reach the desired state xf or the desired output yf . It means that it can take up to nx time steps before the state space model (3) reaches the reference trajectory. This practically means that the ﬁrst part of x, and the corresponding part of y, cannot be deﬁned by an arbitrary reference. Another reason for the system not being (output) controllable is the construction of the vectors x and y. It will be physically impossible to achieve any given xf or yf . A simple example with nx = 2, where the states are position and velocity, and the input is the acceleration, will be used to illustrate this. Let p(t) be the position, v(t) the velocity, and let the state vector be T x(t) = p(t) v(t) , then the continuous-time model becomes 0 1 0 ẋ(t) = x(t) + u(t). 0 0 1 Discretisation of (20) using zero order hold gives the discrete-time model 1 Ts T 2 /2 x(t + 1) = x(t) + s u(t), 0 1 Ts where Ts is the sample time. The batch vector x becomes T x = p(1) v(1) p(2) v(2) · · · p(N ) v(N ) . (20) (21) (22) From Theorem 2 it follows that the system in (8) is controllable if and only rank Bu = nx = 2. Here, rank Bu = 1 hence the system is not controllable. To explain this, consider the dynamics and assume that at time t the position p(t) = a and the velocity v(t) = b for some constants a and b. It should be possible to choose the position and velocity at the next time step t + 1 arbitrarily to have controllability of the state space model in the iteration domain, according to Definition 1. It can be noticed from (21) that it is impossible to go from p(t) = a and v(t) = b to an arbitrary point at time t + 1. It is therefore not possible to have any value of xf as Deﬁnition 1 requires. If the position or the velocity is considered as the output, i.e., ny = 1, then the necessary condition for output controllability from Theorem 4 is satisﬁed, hence the system can be output controllable. In order to establish if the system is output controllable it is necessary to check the rank of the matrix CSxu . It turns out that the system is output controllable in both cases. If instead Euler sampling is used to discretise (20), then the discrete-time model becomes 0 1 Ts x(t) + u(t). (23) x(t + 1) = 0 1 Ts If the position or the velocity is measured, then the necessary condition for out- 234 Paper H Controllability Aspects for Iterative Learning Control put controllability from Theorem 4 is still satisﬁed. However, considering the position as output gives that the ﬁrst row in CSxu is equal to zero because of the zero element in Bu , hence the rank condition for CSxu is not satisﬁed. It means that the control signal does not aﬀect the position directly. Instead the position is aﬀected indirectly via the velocity. Hence, from the explanation above, it follows that the state space model in the time domain can require up to nx time steps before it can reach the reference trajectory, which is the reason for the system in the iteration domain not being output controllable. For many practical applications, such as an industrial robot, it is not only the position that is of importance but also the velocity needs to follow a predeﬁned trajectory in order to achieve a satisfactory result. From this discussion, it follows that the assumption of Su having full row rank is too restrictive to be of practical value. Instead of requiring standard controllability it is necessary to check if the system can follow a trajectory, which is discussed in Section 5. 5 Target Path Controllability Output controllability concerns the possibility to reach a desired output at a speciﬁc time. For ilc it is of interest to reach a desired trajectory, in as few steps as possible, and then be able to follow that trajectory, hence it is more interesting to use the concept of target path controllability (tpc) [Engwerda, 1988]. Target path controllability is used to investigate if it is possible to track any given reference trajectory over some time interval for any initial state. A formal deﬁnition can be found in Deﬁnition 3, where r(l, m) symbolises the vectors r(t) for t = l, . . . , m stacked on top of each other. The same notation holds for u(l, m). Deﬁnition 3 (Target path controllability [Engwerda, 1988]). Let p and q be positive integers. Then a linear time-varying system is said to be target path controllable at t0 with lead p and lag q, if for any initial state x(t0 ) = x0 and for any reference output trajectory r(t0 + p, t0 + p + q − 1), there exists a control sequence u(t0 , t0 + p + q − 2) such that y(t) = r(t) for all t0 + p ≤ t ≤ t0 + p + q − 1. The target path controllability will be abbreviated as tpc(t0 ; p, q). For q = ∞ the system is said to be globally tpc at t0 with lead p. In this paper only lti systems are considered, therefore the starting time t0 = 0 is used without loss of generality, and tpc(p, q) = tpc(0; p, q). In Engwerda [1988] several results are presented to guarantee a system to be tpc. These results are based on diﬀerent subspaces deﬁned using the system matrices and will not be presented here. However, a rank condition of a certain matrix will be presented. The condition is similar to what is used for standard output controllability. 5 235 Target Path Controllability Theorem 5. A linear time-invariant system is tpc(p, q) if and only if rank Syu (p, q) = qny , where ⎛ ⎜⎜ CAp−1 Bu ⎜⎜ .. Syu (p, q) = ⎜⎜⎜⎜ . ⎜⎝ p+q−2 CA Bu ··· .. . CBu ··· ··· .. . 0 .. . CBu ⎞ ⎟⎟ ⎟⎟ ⎟⎟ . ⎟⎟ ⎟⎠ Proof: The result follows directly from [Engwerda, 1988, Lemma 8] using the lti model in (3). Remark 2. Let p = nx and q = 1, then Syu (nx , 1) = C o for the system in (3), i.e., the standard output controllability matrix is obtained. The connection between tpc and output controllability is presented in Theorem 6. Theorem 6. Output controllability of the system in (8) is equivalent to the system in (3) being tpc(1, N ). Proof: From Theorem 3 it holds that the system in (8) is output controllable if and only if rank CSxu = N ny . Using p = 1 and q = N gives Syu (1, N ) = CSxu and from Theorem 5 it follows that the system is tpc(1, N ) if and only if rank Syu (1, N ) = N ny . Hence, the two properties are equivalent. Return to the example in Section 4 for the case where Euler sampling has been used and the position is the output. It was shown that CSxu did not have full row rank due to the zero element in Bu . However, removing the ﬁrst row with only zeros in CSxu gives the matrix Syu (2, N − 1). The conditions in Theorem 5 are now satisﬁed, hence the system is tpc with lead 2. Theorem 5 states a requirement for the system to be tpc. Another important question, is it possible to track a given predeﬁned reference trajectory? Even if the system is tpc it can exist reference trajectories that cannot be tracked. Theorem 7 presents a necessary and suﬃcient condition for reference trajectories that are possible to track, which basically states that the reference should lie in the range space of Syu (p, q). Theorem 7 (Strongly admissible reference trajectory [Engwerda, 1988, Theorem 8]). A reference trajectory r(p, p + q − 1) is strongly admissible if and only if (24) rank Syu (p, q) z(p, p + q − 1) = rank Syu (p, q) where z(i) = r(i) − CAi x(0) for i = p, . . . , p + q − 1. 236 Paper H Controllability Aspects for Iterative Learning Control r(t) 9 r(t) y(t) y0 τ t Figure 2: Augmentation of the reference trajectory to include lead-in. The original reference is denoted by r(t) and 9 r(t) is the appended trajectory. The transient of the output y(t) is also shown. The reader is referred to Engwerda [1988] for a thorough description of admissible references and how to generate them. 6 Concept of Lead-in Target path controllability can now be used to investigate after how many samples it is possible to track the reference, and during how many samples the reference can be tracked. It comes now naturally to deﬁne the concept of lead-in. Lead-in means that the starting point of the original reference trajectory r(t) is moved τ samples forward in time by appending the reference with a new initial part 9 r(t), see Figure 2. Note that τ ≥ p in order to fulﬁl the requirements for tpc. The output now follows the new reference signal, see Figure 2. The assumption of the system being tpc with lead p ≤ τ means that the system should be able to follow the original reference r(t). The error in the beginning, i.e., 9 r(t) − y(t) for t ≤ τ, does not matter since the aim is to follow r(t). The new initial part 9 r(t) of the reference trajectory must of course be chosen carefully, e.g. using Theorem 7, to be able to track the remaining trajectory r(t). Lead-in may not always be possible to use in practice. If the application and the trajectory do not permit to append 9 r(t), then lead-in cannot be used. In that case, an update of the initial state must be used to get closer to the reference signal. Being able to change the initial state while applying the philosophy of ilc can help to decrease the initial error in an iterative manner. 7 Observability Given a state space description of the system, such as (8), it is natural to consider estimation of the state, using for example a Kalman ﬁlter. Intuitively, this would enable smoothing in the time domain, within each batch, for a ﬁxed iteration. Dual to the controllability property, the observability becomes important when considering state observer design. 8 237 Output Controllable System for Design of ILC Algorithms An lti system is observable if and only if the rank of the observability matrix O is equal to the state dimension [Rugh, 1996]. A condition for observability of the system in (8) is presented in Theorem 8. Theorem 8. System (8) in the iteration domain is observable if and only if rank C = nx . Proof: The observability matrix for the system in (8) is given by C repeated N times, i.e., T (25) O = CT · · · CT . The system is therefore observable if and only if rank O = rank C = N nx . (26) From the structure of C it follows that rank C = N rank C. Hence, the system is observable if and only if rank C = nx . Corollary 2. A necessary condition for system (8) to be observable is that ny ≥ nx . 0 1 Proof: It is given that C ∈ Rny ×nx , hence rank C ≤ min ny , nx . It is therefore necessary to have ny ≥ nx to be able to obtain rank C = nx . In practice, the number of measurements is usually less than the number of states. Therefore, the system in (8) is in practice often not observable. Remark 3. Observability of the time domain system (3), i.e., rank CT (CA)T ··· (CAnx −1 )T T = nx , (27) does not imply that system (8) is observable. 8 Output Controllable System for Design of ILC Algorithms If system (8) is output controllable, then the variable substitution z = y = Cx gives the new state space model zk+1 = zk + Su Δuk , yk = zk , (28a) (28b) which is controllable by design. The ilc control law can now be found using standard discrete-time control methods such as linear quadratic (lq) control with inﬁnite horizon, H∞ control, model predictive control, etcetera. For simplicity, no noise terms are included in (28), hence, there is no need for a state observer since 238 Paper H Controllability Aspects for Iterative Learning Control all states are directly measured. A small simulation study of a ﬂexible joint model with linear spring characteristic will show the advantage of using lq-design of the ilc control law compared to the standard norm-optimal ilc method [Amann et al., 1996]. The ﬂexible joint model in continuous-time with the state vector T x = qa q̇a qm q̇m is given by ⎛ ⎜⎜ 0 ⎜⎜ k ⎜⎜− ẋ = ⎜⎜⎜ Ma ⎜⎜ 0 ⎜⎝ k Mm 1 0 − Md a 0 k Ma d Mm − Mk 0 m ⎞ ⎛ ⎞ 0 ⎟⎟ ⎜⎜ 0 ⎟⎟ ⎟ d ⎟ ⎜⎜ 0 ⎟⎟ ⎜ ⎟ ⎟⎟ Ma ⎟ x + ⎜⎜⎜⎜ 0 ⎟⎟⎟⎟ u, ⎟ ⎟ 1 ⎟⎟ ⎜ ⎜⎝ kτ ⎟⎟⎠ ⎠ f +d ⎟ −M Mm (29) m with k = 8, d = 0.0924, Ma = 0.0997, Mm = 0.0525, f = 1.7825 kτ = 0.61. A discrete-time model is obtained using zero order hold sampling with a sample time Ts = 0.1 s. The system in the iteration domain is clearly not controllable since the state dimension is larger than the number of inputs. To satisfy the requirement for output controllability it is necessary to have at most one output. The output is chosen as qm which gives that CSxu has full row rank. The lq-problem for the model in (28) can be formulated as minimise Δu subject to ∞ eTi W e ei + ΔTui W Δ Δui i=1 ei = r − yi , (28), (30) which can be solved using standard methods [Franklin et al., 1998]. Remark 4. Since only the output is considered in the lq-formulation it is important to know that the remaining original states do not cause any problems. It can be concluded that the remaining states will behave properly due to observability of the original state space model in the time domain. The norm-optimal ilc from Amann et al. [1996]; Gunnarsson and Norrlöf [2001] can be formulated using the batch vectors as minimise uk+1 1 T e W e + uTk+1 W u uk+1 + λ(uk+1 − uk )T (uk+1 − uk ) 2 k+1 e k+1 (31) and the solution can be found in Gunnarsson and Norrlöf [2001]. To obtain a fair comparison the tuning of the two design methods must be similar. Comparing (31) and (30) gives that the control weights should be chosen as W u = 0 and W Δ = λI. Numerical values of the weight matrices are W e = I and λ = 1. The reference signal is a unit step ﬁltered four times through an fir ﬁlter of length n = 100 with all coeﬃcients equal to 1/n. The performance of the two ilc algorithms is evaluated using the relative reduction of the rmse in percent with 9 239 Conclusions 102 Norm-optimal ilc lq-ilc 101 100 ρk [%] 10−1 10−2 10−3 10−4 10−5 10−6 0 20 40 60 80 100 120 140 160 180 200 ilc iteration Figure 3: Relative reduction of the rmse for the norm-optimal ilc algorithm and the lq-ilc algorithm. respect to the error when no ilc signal is applied, i.e., % % & & ' ' N N 1 1 2 ρk = 100 ek (t) e0 (t)2 . N N t=1 (32) t=1 The relative reduction of the rmse is shown in Figure 3 for the two ilc algorithms. It can be seen that the error for the norm-optimal ilc levels out whereas the error for the lq-ilc continues to decrease. The convergence speed is also faster for the lq-ilc. The simulation study shows that it can be worth to check output controllability of the batch system and then use lq-design instead of the standard norm-optimal ilc algorithm. However, if the system is not output controllable then the norm-optimal ilc control law must be used. Remark 5. By introducing a terminal cost term ek+2 P in the objective function for the norm-optimal ilc, and letting P be the solution to a suitable Riccati equation, the normoptimal ilc turns out to be equivalent to the lq-controller. This follows from the principal of optimality and dynamic programming. 9 Conclusions This paper introduces a state space model in the iteration domain and discusses what it means that the state space model is (output) controllable. It is shown 240 Paper H Controllability Aspects for Iterative Learning Control that the condition to guarantee output controllability is equivalent to a condition used in the literature to prove that the error tends to zero for the complete batch. Furthermore, it is discussed what it means to have a general system (output) controllable. A system with two states, position and velocity, is used to exemplify this. For systems that are not controllable it is more suitable to use target path controllability. This leads to the concept of lead-in where the ﬁrst part of the reference trajectory is not considered for perfect tracking performance. Finally, lq design of the ilc law for an output controllable system is compared to the standard norm-optimal ilc law. It is shown that the lq design outperforms the norm-optimal ilc law. Appendix A State Space Model in Batch Form The discrete-time state space model x(t + 1) = Ax(t) + Bu u(t) + Br r(t) + Bw w(t), y(t) = Cx(t) + v(t), (33a) (33b) where w(t) ∼ N (0, Q) is the process noise and v(t) ∼ N (0, R) is the measurement noise, has the following update formula for the state vector [Rugh, 1996] x(t) = At x(0) + t−1 At−j−1 Bu u(j) + j=0 t−1 j=0 At−j−1 Br r(j) + t−1 At−j−1 Bw w(j), (34) j=0 for t ≥ 1. After introducing the vectors T x = x(1)T . . . x(N )T ∈ RN nx , T u = u(0)T . . . u(N − 1)T ∈ RN nu , T y = y(1)T . . . y(N )T ∈ RN ny , T r = r(0)T . . . r(N − 1)T ∈ RN ny , T w = w(0)T . . . w(N − 1)T ∈ RN nw , T v = v(1)T . . . v(N )T ∈ RN nv , the model in (33) can be written more compactly for a batch of length N as x =Φx(0) + Sxu u + Sxr r + Sxw w (35a) y =Cx + v (35b) A 241 State Space Model in Batch Form where x(0) is the initial value, C = IN ⊗ C, and ⎛ ⎛ ⎞ 0 ⎜⎜ B∗ ⎜⎜ A ⎟⎟ ⎜⎜ ⎜⎜ 2 ⎟⎟ B∗ ⎜⎜ AB∗ ⎜⎜ A ⎟⎟ Φ = ⎜⎜⎜⎜ . ⎟⎟⎟⎟ , Sx∗ = ⎜⎜⎜⎜ .. .. ⎜⎜ ⎜⎜ .. ⎟⎟ . . ⎜⎝ ⎜⎝ ⎟⎠ AN −1 B∗ AN −2 B∗ AN ··· ··· .. . ··· ⎞ 0 ⎟⎟ ⎟ 0 ⎟⎟⎟ .. ⎟⎟⎟⎟ , . ⎟⎟⎟ ⎠ B∗ (36) where ∗ = {u, r, w}. The process and measurement noise for (35) are w ∼ N (0, Q) and v ∼ N (0, R), where Q = IN ⊗ Q and R = IN ⊗ R. Acknowledgement This work was supported by the Vinnova Excellence Center LINK-SIC and by the Excellence Center at Linköping-Lund in Information Technology, ELLIIT. 242 Paper H Controllability Aspects for Iterative Learning Control Bibliography Notker Amann, David H. Owens, and Eric Rogers. Iterative learning control for discrete-time systems with exponential rate of convergence. IEE Proceedings Control Theory and Applications, 143(2):217–224, March 1996. Suguru Arimoto. Learning control theory for robotic motion. International Journal of Adaptive Control and Signal Processing, 4(6):543–564, 1990. Suguru Arimoto, Sadao Kawamura, and Fumio Miyazaki. Bettering operation of robots by learning. Journal of Robotic Systems, 1(2):123–140, 1984. Patrik Axelsson, Daniel Axehill, Torkel Glad, and Mikael Norrlöf. Controllability aspects for iterative learning control. Submitted to International Journal of Control, 2014. Jacob C. Engwerda. Control aspects of linear discrete time-varying systems. International Journal of Control, 48(4):1631–1658, 1988. Gene F. Franklin, J. David Powell, and Michael L. Workman. Digital Control of Dynamic Systems. Addison Wesley, Menlo Park, CA, USA, third edition, 1998. Svante Gunnarsson and Mikael Norrlöf. On the design of ilc algorithms using optimization. Automatica, 37(12):2011–2016, December 2001. Jay H. Lee, Kwang S. Lee, and Won C. Kim. Model-based iterative learning control with a quadratic criterion for time-varying linear systems. Automatica, 36(5): 641–657, May 2000. Kwang S. Lee and Jay H. Lee. Convergence of constrained model-based predictive control for batch processes. IEEE Transactions on Automatic Control, 45(10): 1928–1932, October 2000. Kwang Soon Lee and Jay H. Lee. Iterative Learning Control: Analysis, Design, Integration and Application, chapter Design of Quadratic Criterion-based Iterative Learning Control, pages 165–192. Kluwer Academic Publishers, Boston, MA, USA, 1998. Helmut Lütkepohl. Handbook of Matrices. John Wiley & Sons, Chichester, West Sussex, England, 1996. Kevin L. Moore. Iterative Learning Control for Deterministic Systems. Advances in Industrial Control. Springer-Verlag, London, UK, 1993. Katsuhiko Ogata. Modern Control Engineering. Prentice Hall Inc., Upper Saddle River, NJ, USA, fourth edition, 2002. Wilson J. Rugh. Linear System Theory. Information and System Sciences Series. Prentice Hall Inc., Upper Saddle River, NJ, USA, second edition, 1996. Johanna Wallén, Isolde Dressler, Anders Robertsson, Mikael Norrlöf, and Svante Gunnarsson. Observer-based ilc applied to the Gantry-Tau parallel kinematic robot. In Proceedings of the 18th IFAC World Congress, pages 992–998, Milano, Italy, August/September 2011. PhD Dissertations Division of Automatic Control Linköping University M. Millnert: Identiﬁcation and control of systems subject to abrupt changes. Thesis No. 82, 1982. ISBN 91-7372-542-0. A. J. M. van Overbeek: On-line structure selection for the identiﬁcation of multivariable systems. Thesis No. 86, 1982. ISBN 91-7372-586-2. B. Bengtsson: On some control problems for queues. Thesis No. 87, 1982. ISBN 91-7372593-5. S. Ljung: Fast algorithms for integral equations and least squares identiﬁcation problems. Thesis No. 93, 1983. ISBN 91-7372-641-9. H. Jonson: A Newton method for solving non-linear optimal control problems with general constraints. Thesis No. 104, 1983. ISBN 91-7372-718-0. E. Trulsson: Adaptive control based on explicit criterion minimization. Thesis No. 106, 1983. ISBN 91-7372-728-8. K. Nordström: Uncertainty, robustness and sensitivity reduction in the design of single input control systems. Thesis No. 162, 1987. ISBN 91-7870-170-8. B. Wahlberg: On the identiﬁcation and approximation of linear systems. Thesis No. 163, 1987. ISBN 91-7870-175-9. S. Gunnarsson: Frequency domain aspects of modeling and control in adaptive systems. Thesis No. 194, 1988. ISBN 91-7870-380-8. A. Isaksson: On system identiﬁcation in one and two dimensions with signal processing applications. Thesis No. 196, 1988. ISBN 91-7870-383-2. M. Viberg: Subspace ﬁtting concepts in sensor array processing. Thesis No. 217, 1989. ISBN 91-7870-529-0. K. Forsman: Constructive commutative algebra in nonlinear control theory. Thesis No. 261, 1991. ISBN 91-7870-827-3. F. Gustafsson: Estimation of discrete parameters in linear systems. Thesis No. 271, 1992. ISBN 91-7870-876-1. P. Nagy: Tools for knowledge-based signal processing with applications to system identiﬁcation. Thesis No. 280, 1992. ISBN 91-7870-962-8. T. Svensson: Mathematical tools and software for analysis and design of nonlinear control systems. Thesis No. 285, 1992. ISBN 91-7870-989-X. S. Andersson: On dimension reduction in sensor array signal processing. Thesis No. 290, 1992. ISBN 91-7871-015-4. H. Hjalmarsson: Aspects on incomplete modeling in system identiﬁcation. Thesis No. 298, 1993. ISBN 91-7871-070-7. I. Klein: Automatic synthesis of sequential control schemes. Thesis No. 305, 1993. ISBN 91-7871-090-1. J.-E. Strömberg: A mode switching modelling philosophy. Thesis No. 353, 1994. ISBN 917871-430-3. K. Wang Chen: Transformation and symbolic calculations in ﬁltering and control. Thesis No. 361, 1994. ISBN 91-7871-467-2. T. McKelvey: Identiﬁcation of state-space models from time and frequency data. Thesis No. 380, 1995. ISBN 91-7871-531-8. J. Sjöberg: Non-linear system identiﬁcation with neural networks. Thesis No. 381, 1995. ISBN 91-7871-534-2. R. Germundsson: Symbolic systems – theory, computation and applications. Thesis No. 389, 1995. ISBN 91-7871-578-4. P. Pucar: Modeling and segmentation using multiple models. Thesis No. 405, 1995. ISBN 91-7871-627-6. H. Fortell: Algebraic approaches to normal forms and zero dynamics. Thesis No. 407, 1995. ISBN 91-7871-629-2. A. Helmersson: Methods for robust gain scheduling. Thesis No. 406, 1995. ISBN 91-7871628-4. P. Lindskog: Methods, algorithms and tools for system identiﬁcation based on prior knowledge. Thesis No. 436, 1996. ISBN 91-7871-424-8. J. Gunnarsson: Symbolic methods and tools for discrete event dynamic systems. Thesis No. 477, 1997. ISBN 91-7871-917-8. M. Jirstrand: Constructive methods for inequality constraints in control. Thesis No. 527, 1998. ISBN 91-7219-187-2. U. Forssell: Closed-loop identiﬁcation: Methods, theory, and applications. Thesis No. 566, 1999. ISBN 91-7219-432-4. A. Stenman: Model on demand: Algorithms, analysis and applications. Thesis No. 571, 1999. ISBN 91-7219-450-2. N. Bergman: Recursive Bayesian estimation: Navigation and tracking applications. Thesis No. 579, 1999. ISBN 91-7219-473-1. K. Edström: Switched bond graphs: Simulation and analysis. Thesis No. 586, 1999. ISBN 91-7219-493-6. M. Larsson: Behavioral and structural model based approaches to discrete diagnosis. Thesis No. 608, 1999. ISBN 91-7219-615-5. F. Gunnarsson: Power control in cellular radio systems: Analysis, design and estimation. Thesis No. 623, 2000. ISBN 91-7219-689-0. V. Einarsson: Model checking methods for mode switching systems. Thesis No. 652, 2000. ISBN 91-7219-836-2. M. Norrlöf: Iterative learning control: Analysis, design, and experiments. Thesis No. 653, 2000. ISBN 91-7219-837-0. F. Tjärnström: Variance expressions and model reduction in system identiﬁcation. Thesis No. 730, 2002. ISBN 91-7373-253-2. J. Löfberg: Minimax approaches to robust model predictive control. Thesis No. 812, 2003. ISBN 91-7373-622-8. J. Roll: Local and piecewise aﬃne approaches to system identiﬁcation. Thesis No. 802, 2003. ISBN 91-7373-608-2. J. Elbornsson: Analysis, estimation and compensation of mismatch eﬀects in A/D converters. Thesis No. 811, 2003. ISBN 91-7373-621-X. O. Härkegård: Backstepping and control allocation with applications to ﬂight control. Thesis No. 820, 2003. ISBN 91-7373-647-3. R. Wallin: Optimization algorithms for system analysis and identiﬁcation. Thesis No. 919, 2004. ISBN 91-85297-19-4. D. Lindgren: Projection methods for classiﬁcation and identiﬁcation. Thesis No. 915, 2005. ISBN 91-85297-06-2. R. Karlsson: Particle Filtering for Positioning and Tracking Applications. Thesis No. 924, 2005. ISBN 91-85297-34-8. J. Jansson: Collision Avoidance Theory with Applications to Automotive Collision Mitigation. Thesis No. 950, 2005. ISBN 91-85299-45-6. E. Geijer Lundin: Uplink Load in CDMA Cellular Radio Systems. Thesis No. 977, 2005. ISBN 91-85457-49-3. M. Enqvist: Linear Models of Nonlinear Systems. Thesis No. 985, 2005. ISBN 91-8545764-7. T. B. Schön: Estimation of Nonlinear Dynamic Systems — Theory and Applications. Thesis No. 998, 2006. ISBN 91-85497-03-7. I. Lind: Regressor and Structure Selection — Uses of ANOVA in System Identiﬁcation. Thesis No. 1012, 2006. ISBN 91-85523-98-4. J. Gillberg: Frequency Domain Identiﬁcation of Continuous-Time Systems Reconstruction and Robustness. Thesis No. 1031, 2006. ISBN 91-85523-34-8. M. Gerdin: Identiﬁcation and Estimation for Models Described by Diﬀerential-Algebraic Equations. Thesis No. 1046, 2006. ISBN 91-85643-87-4. C. Grönwall: Ground Object Recognition using Laser Radar Data – Geometric Fitting, Performance Analysis, and Applications. Thesis No. 1055, 2006. ISBN 91-85643-53-X. A. Eidehall: Tracking and threat assessment for automotive collision avoidance. Thesis No. 1066, 2007. ISBN 91-85643-10-6. F. Eng: Non-Uniform Sampling in Statistical Signal Processing. Thesis No. 1082, 2007. ISBN 978-91-85715-49-7. E. Wernholt: Multivariable Frequency-Domain Identiﬁcation of Industrial Robots. Thesis No. 1138, 2007. ISBN 978-91-85895-72-4. D. Axehill: Integer Quadratic Programming for Control and Communication. Thesis No. 1158, 2008. ISBN 978-91-85523-03-0. G. Hendeby: Performance and Implementation Aspects of Nonlinear Filtering. Thesis No. 1161, 2008. ISBN 978-91-7393-979-9. J. Sjöberg: Optimal Control and Model Reduction of Nonlinear DAE Models. Thesis No. 1166, 2008. ISBN 978-91-7393-964-5. D. Törnqvist: Estimation and Detection with Applications to Navigation. Thesis No. 1216, 2008. ISBN 978-91-7393-785-6. P-J. Nordlund: Eﬃcient Estimation and Detection Methods for Airborne Applications. Thesis No. 1231, 2008. ISBN 978-91-7393-720-7. H. Tidefelt: Diﬀerential-algebraic equations and matrix-valued singular perturbation. Thesis No. 1292, 2009. ISBN 978-91-7393-479-4. H. Ohlsson: Regularization for Sparseness and Smoothness — Applications in System Identiﬁcation and Signal Processing. Thesis No. 1351, 2010. ISBN 978-91-7393-287-5. S. Moberg: Modeling and Control of Flexible Manipulators. Thesis No. 1349, 2010. ISBN 978-91-7393-289-9. J. Wallén: Estimation-based iterative learning control. Thesis No. 1358, 2011. ISBN 97891-7393-255-4. J. Hol: Sensor Fusion and Calibration of Inertial Sensors, Vision, Ultra-Wideband and GPS. Thesis No. 1368, 2011. ISBN 978-91-7393-197-7. D. Ankelhed: On the Design of Low Order H-inﬁnity Controllers. Thesis No. 1371, 2011. ISBN 978-91-7393-157-1. C. Lundquist: Sensor Fusion for Automotive Applications. Thesis No. 1409, 2011. ISBN 978-91-7393-023-9. P. Skoglar: Tracking and Planning for Surveillance Applications. Thesis No. 1432, 2012. ISBN 978-91-7519-941-2. K. Granström: Extended target tracking using PHD ﬁlters. Thesis No. 1476, 2012. ISBN 978-91-7519-796-8. C. Lyzell: Structural Reformulations in System Identiﬁcation. Thesis No. 1475, 2012. ISBN 978-91-7519-800-2. J. Callmer: Autonomous Localization in Unknown Environments. Thesis No. 1520, 2013. ISBN 978-91-7519-620-6. D. Petersson: A Nonlinear Optimization Approach to H2-Optimal Modeling and Control. Thesis No. 1528, 2013. ISBN 978-91-7519-567-4. Z. Sjanic: Navigation and Mapping for Aerial Vehicles Based on Inertial and Imaging Sensors. Thesis No. 1533, 2013. ISBN 978-91-7519-553-7. F. Lindsten: Particle Filters and Markov Chains for Learning of Dynamical Systems. Thesis No. 1530, 2013. ISBN 978-91-7519-559-9.

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

### Related manuals

Download PDF

advertisement