Internal model based fault tolerant control of a robot manipulator

Transcription

1 Internal model based fault tolerant control of a robot manipulator Claudio Bonivento, Luca Gentili and Andrea Paoli Abstract In this paper an implicit fault tolerant control scheme is specialized for an n-dof fully actuated mechanical manipulator subject to various sinusoidal torque disturbances acting on joints. More in detail we show how a standard tracking controller can be augmented with an internal model unit designed so as to compensate the unknown spurious torque harmonics. In this way the controller is proved to be global implicitly fault tolerant to all the faults belonging to the model embedded in the regulator. Moreover simply testing the state of the internal model we will show how to perform fault detection and isolation. I. INTRODUCTION Fault tolerant control (FTC) systems are control systems able on one hand to detect incipient faults in sensors and/or actuators and, on the other, to promptly adapt the control law in such a way as to preserve pre-specified performances in terms of quality of the production, safety, etc. The common approach in dealing with such a problem (see [4] and the reference therein) is to split the overall design in two distinct phases. In the first phase the Fault Detection and Isolation (FDI) problem is addressed; this part consists in designing a dynamical system (filter) which, by processing input/output data, is able to detect the presence of an incipient fault and to isolate it from other faults and/or disturbances. The second phase usually consists in the design of a supervisory unit which, on the basis of the information provided by the FDI filter, reconfigures the control so as to compensate for the effect of the fault and to fulfill performances constraint; i.e. by means of parameterized controller which is suitably updated. In [1] a different approach to fault tolerance control has been discussed. Specifically, it has been addressed the case in which the faults affecting the controlled system can be modelled as functions (of time) within a finitelyparameterized family of such functions. Then a controller which embeds an internal model of this family is designed in order to generate a supplementary control action which compensate for the presence of any of such faults, regardless their entity. The idea is pursued using the theoretical machinery of the (nonlinear) output regulation theory (see [3]) under the assumption that the side-effects generated by the occurrence of the fault can be modelled as an exogenous signal generated by an autonomous neutrally stable system (the so-called exosystem ). In this framework, the This work was supported by MIUR and EC-Project IFATIS partly funded by the European Commission in the IST programme 21 of the 5th EC framework programme (IST ). Authors are with CASY-DEIS-University of Bologna. Corresponding author: Dr. Luca Gentili, CASY-DEIS-University of Bologna, Via Risorgimento 2, 4136 Bologna-Italy. Tel , Fax , lgentili@deis.unibo.it Fault Detection and Isolation phase is postponed to that of control reconfiguration since it can be carried out by testing the state of the internal model unit which automatically activates to offset the presence of the fault. In this paper the approach outlined above is specialized to the design of a fault tolerant control system for n-dof fully actuated mechanical robot subject to various sinusoidal torque disturbances acting on joints (see [7]). We will show how this framework can be casted as an output regulation problem. More in detail we show how a standard tracking robot control (see [11], [12], [5]), can be augmented with an internal model unit designed so as to compensate the unknown spurious torque harmonic. In this way the controller is proved to be global implicitly fault tolerant to all the faults belonging to the model embedded in the regulator. In section II the problem is introduced and some preliminary positions are given in order to show how the problem can be casted in the framework illustrated in [1]. In section III the control design of a canonical internal model unit able to achieve the implicit fault tolerance of the robot controlled is presented; in section IV, moreover, the main results regarding the implicit fault tolerance of the robot controlled with the proposed algorithm, augmented with an adaptive mechanism, is given: this results is proved to be robust with respect to the not perfect knowledge of the characteristic frequencies of the sinusoidal disturbances. In section V some simulation results are provided in order to show the effectiveness of the proposed controller, while section VI concludes the work with some final remarks. II. PROBLEM STATEMENT AND PRELIMINARY POSITIONS In this section we are going to introduce the model of a n-degree of freedom fully actuated robot manipulator and state the FTC-FDI problem. Usually the joint actuators are modelled as pure torque sources; however they can be subject to some asymmetries (e.g. due to some electrical or mechanical faults) that comport the arise of spurious harmonics in the electrical variables and then in the generated torques. Hence, in the following, we will model these effects as sinusoidal signals superimposed to the controlled torque signals. We will then show how it is possible to cast this problem in the framework illustrated in [1]: in order to point out that a pre-existing control can be augmented without modification with the FTC-FDI module designed (internal model unit) able to overcome the disturbance and, moreover, to isolate it, in this section a simple tracking controller is also considered. The regulation scheme developed is depicted in figure 1. Consider an n degree of freedom fully-actuated robot ma-

2 Nominal controller Exosystem v(t) ν(t) τ(t) FDI ξ(t) Logic Internal Model Unit Fault Estimation Fig. 1. n-dof Robot FTC controller scheme Trajectory Generator q (t) + - p (t) q(t), p(t) q(t), p(t) nipulator with generalized coordinates q = (q 1,,q n ) T. If p = M(q) q = (p 1,,p n ) T are the generalized momenta, with M(q) the inertia matrix, symmetric and positive definite for all q, an explicit port-hamiltonian representation of this system can be obtained defining the whole state x := ( q p ) T and the Hamiltonian function as the total energy of the system (sum of kinetic energy and potential energy) and, finally, ( ) In J = I n H(q,p) := 1 2 pt M 1 (q)p + P(q) R = ( ) D(q) ( ) G = with D(q) = D T (q) taking into account the dissipation effects. The input is an effort representing the input torques and the output is a flow representing the joint velocities. These considerations lead to the following model H [ ] q = [J R] ṗ H + Gν p H (1) y = G T H p This system will be affected by an external torque ripple v(t) acting through the control input channel (i.e. actually the torque applied to the system will be the sum of the control torque and the external disturbance ν + v(t)) and the problem addressed in this paper is to compensate this disturbance, detecting and isolating in the meanwhile the entity of this (unknown) disturbance. It is worth to point out again that the design of the internal model unit doesn t affect a previous regulator, designed in order to carry out a particular task. To remark this feature, in the following we will introduce a control scheme whose aim is to make the manipulator track a known trajectory. This I n tracking control is developed following [5], but the same results can be obtained using also a simpler controller. A. Tracking control Firstly a preliminary torque input able to compensate potential energies (as gravity) is designed: ν = P(q) + ν (2) Let define the desired trajectory for the generalized coordinates and the generalized momenta as (q (t), p (t)); this trajectory, to be realizable, has to satisfy p (t) = M(q ) q (t). To define new error variables, let consider the following change of coordinates q = q q (t) p = p M(q) q (t) Deriving the new error coordinates we obtain: q = M 1 (q) p p = H D(q) H p + ν d dt (M(q) q (t)) = = 1 M 1 (q) 2 pt p D(q)M 1 (q) p + ν + Π(q, q (t), q (t)) Defining a new Hamiltonian function as H = 1 2 pt M 1 (q) p it is possible to write again (4) as a port-hamiltonian system: q = H p = H q D(q) H + ν Π(q, q (t), q (t)) (5) It is now possible to obtain a perfect asymptotic tracking designing the control torque in order to delete the bad term Π( ), to shape the energy of the error system in order to have a minimum in the origin 1 and to add some damping in order to have this minimum globally attractive: ν = Π(q, q (t), q (t)) + DM 1 (q) p q M 1 (q) p + τ (6) where τ is an additional control torque that will be used in the following section in order to compensate the presence of additional torque disturbances. The whole error system (5) with the controller (6) writes as q = H + τ 1 note that q = means that the tracking is achieved as q q (t). (3) (4) (7)

3 where the new Hamiltonian is defined by H = 1 2 pt M 1 (q) p qt q (8) Remark. It is worth to remark that this kind of control strategy is very similar the classical tracking control made by inversion of the model and introducing simple proportional and derivative terms (see e.g. [11], [12], [9]) B. Problem statement It is now possible to state the input disturbance suppression problem, introducing in the model of a controlled n-degree of freedom robot manipulator like (7), (8) the exogenous torque disturbance v(t): q = H p = H + τ + v(t), (9) In (9), v(t) is a torque disturbance belonging to the class of signals generated by the linear, neutrally stable autonomous system (exosystem) { ż = Sz (1) v(t) = Γz with z IR 2k, Γ IR 2k m a known matrix and S is defined by S = diag{s 1,...,S k } (11) with S i = [ ] ωi ω i ω i > i = 1,...,k (12) and z() Z, with Z IR 2k bounded compact set. In this discussion the matrix S is firstly considered perfectly known, and then, in section IV, this hypothesis is removed (as in [1]): the dimension 2k of matrix S will be still known but all characteristic frequencies ω i will be unknown but ranging within known compact sets, i.e. ωi min ω i ωi max. In this set up the lack of knowledge of the exogenous disturbance reflects into the lack of knowledge of the initial state z() of the exosystem and, in section IV, also of the characteristic frequencies. For instance, in the next section, any v(t) obtained by linear combination of sinusoidal signals with known frequencies but unknown amplitudes and phases will be considered, while, in section IV, the frequencies will be unknown too. All those assumptions allow us to cast the problem of disturbance suppression as a problem of output regulation (see [2], [6]) that will be complicated by the lack of knowledge of the matrix S (see [1]), and suggests to look for a controller which embeds an internal model of the exogenous disturbances augmented by an adaptive part in order to estimate the characteristic frequencies of the disturbances. III. CANONICAL INTERNAL MODEL UNIT DESIGN In this section we are going to briefly design a canonical internal model unit able to overcome external torque disturbances (i.e. exogenous sinusoidal torque ripples). The main hypothesis here (that will be removed in the next section) is that the exogenous matrix S is perfectly known. As previously announced, the regulator to be designed will embed the internal model of the exogenous disturbance: this internal model unit is designed according to the procedure proposed in [8] (canonical internal model). Given any Hurwitz matrix F and any matrix G such that (F,G) is controllable, denote by Y the unique matrix solution of the Sylvester equation Y S FY = GΓ and define Ψ := ΓY 1. Let introduce the internal model unit as And set the control law as ξ = (F + GΨ)ξ + N( p, q) (13) τ = Ψξ + τ st (14) where N( q, p) and τ st are additional terms that will be designed later. Defining the changes of coordinate χ = ξ Y z G p (15) system (9), (15), becomes q = M(q) 1 p + Ψξ + τ st ΨY z χ = (F + GΨ)ξ + N( p, q) Y Sz G p (16) Choosing τ st = ΨG p, simple computation shows that the p-dynamic become + Ψχ (17) Concentrating on the χ-dynamic it is possible to design N( q, p) = Ψ T p ( H FG p G q + H ) + ΨG p and write the last equation of (16) as χ = Fχ Ψ T p (18) Consider now the first equation of (16) with (17) and (18). This new system identifies a port-hamiltonian system described by: ẋ = [J(x) R(x)] H x(x) x (19) with x = ( q p χ ) T, the Hamiltonian Hx (x) defined by H x (x) = 1 2 pt M(q) 1 p qt q χt χ

4 the skew-symmetric interconnection matrix J(x) and the positive-definite damping matrix R defined by: I J(x) = I Ψ, R = I Ψ T F Proposition 1: Consider the controlled n-degree of freedom robot manipulator (9) with Hamiltonian (8), affected by the torque disturbances generated by (1), (11), (12). The additional control law generated by the internal model unit: ξ = (F + GΨ)ξ Ψ T p FG p + G q+ +G p T M 1 (q) p + GM 1 (q) p GΨG p τ = Ψξ ΨG p. (2) assures asymptotically the input disturbance suppression (fault tolerance with respect to torque ripple, i.e. ( q, p) (,) as time t ) and the convergence of the state of the internal model to the fault signal (fault detection, i.e. ξ Y z). Proof: Considering H x (x) as a Lyapunov function the proof is immediate as (remembering that F is an arbitrary Hurwitz matrix) Ḣ x M 1 (q) p 2 + F χ 2 and for the Lasalle invariant principle the system will asymptotically converge to lim t ( p,χ) = (,). Moreover from the first and second equation of (2) it is possible to state that also lim t q(t) = and the proposition is proved. IV. ADAPTIVE INTERNAL MODEL UNIT DESIGN In this section we are going to introduce the main result of the paper, designing an adaptive internal model unit able to overcome external torque disturbances (i.e. exogenous sinusoidal torque ripples). As previously announced, in this section the perfect knowledge of the exogenous matrix S is not assumed, as only the dimension 2k of the matrix is assumed to be known; this means that, for instance, any v(t) obtained by linear combination of sinusoidal signals with unknown frequencies, amplitudes and phases can be modelled. For this reason it is now impossible to implement the classical internal model control introduced in (13) depending on Ψ; hence let design a canonical adaptive internal model ξ = (F + GˆΨ)ξ + N( p, q) ˆΨ T (21) i = ϕ i (ξ, p, q) calling ˆΨ T i with i = 1,,n every column of the matrix ˆΨ T IR 2k n. Moreover let set the control law as τ = ˆΨξ + τ st where N( q, p) and τ st are additional terms that will be designed later. The adaptation law ϕ(ξ, p, q) will be designed in order to assure that asymptotically the internal model unit will provide a torque able to overcome all disturbances. Defining the changes of coordinate χ = ξ Y z G p Ψ T i = ˆΨ T i ΨT i i = (1,,n) (22) where Ψ T i represent the i-th column of Ψ T, system (9), (21) becomes q = M(q) 1 p Note that + ˆΨξ + τ st ΨY z χ = (F + GˆΨ)ξ + N( p, q) Y Sz G p ˆΨ T i = ϕ i (ξ, p, q) i = (1,,n) + ˆΨ(ξ Y z G p) + ˆΨG p 1 + +τ st Ψ(ξ χ G p) choosing τ st = ˆΨG p + τ st it is possible to write + ˆΨχ + Ψξ Ψχ ΨG p + τ st (23) Choosing now τ st = ĀM 1 (q) p with Ā such that A = Ā I is Hurwitz we obtain q + A H + Ψχ + Ψ(ξ G p). (24) Considering every single element of vector p it is possible to write (from now on apex i means the i-th element of the vector considered) ( p i = H ) i H + A q + Ψχ + (ξ G p) T ΨT i (25) with i = 1,,n. Concentrate now on the χ-dynamic in order to design suitably the update term N( q, p): χ = Fχ + FG p + N( p, q) + G H q Choosing we obtain N( p, q) = FG p G H q + G H Gτ st G H + Gτ st (26) χ = Fχ = Fχ Ψ T p + Ψ T p. (27) As all dynamics of (23) have been investigated, it is now possible to design an adaptation law for ˆΨ T : assume then ϕ i (ξ, p, q) = (ξ G p) p i i = 1,,n.

5 With this in mind it is immediate to write the Ψ i T -dynamic as Ψ T i = ˆΨT i Ψ T i = (ξ G p) p i i = 1,,n. (28) Consider now the first equation of (23) with all (25), (27) and (28). This new system (with a small abuse of notation in order to obtain a more compact and readable formulation) identifies an interconnection described by: ẋ = [J(x) R(x)] H x(x) x + Λ(x) (29) with x = ( q p χ Ψ ) T T, the Hamiltonian Hx (x) defined by H x (x) = 1 2 pt M(q) 1 p qt q + 1 n 1 2 χt χ + 2 Ψ i ΨT i i=1 the skew-symmetric interconnection matrix J(x) defined by I J(x) = I Ψ (ξ G p) T Ψ T (ξ G p) the damping matrix R = diag (, A, F, ) and Λ(x) = ( Ψ T p ) T. Proposition 2: Consider the controlled n-degree of freedom robot manipulator (9) with Hamiltonian (8), affected by the torque disturbances generated by (1), (11), (12). The additional control law generated by the adaptive internal model unit: ξ = (F + GˆΨ)ξ FG p + G 1 1 M(q) pt p+ 2 +G q GˆΨG p + GAM 1 (q) p for a certain value of ε. Now choosing ε = η A /η Ψ, we obtain V η ( ) A 2 p 2 + η F η2 Ψ χ 2 2η A hence choosing matrix F such that η F < η2 Ψ 2η A we have that V. System asymptotic behavior for t and for any bounded torque disturbance generated by (1) with arbitrary bounded initial conditions z() Z, is then defined by Lasalle s invariant principle: V = implies that asymptotically, as time t, p and χ (and consequently lim t ξ = Y z). From the first equation of (23) it is possible to point out that q i.e. q tends to a constant vector q. Again from (28) it is possible to state that Ψ, hence Ψ tends to a constant matrix Ψ. The asymptotic behavior of the system (according to V = ) is then characterized by (24): as time t the following hold: = q + Ψ Y z (33) As Y is a full rank matrix (always invertible) and z is a vector whose elements are all sinusoidal signals, the only solution to (33) is q = and Ψ =. Hence we proved that lim x(t) =. t ˆΨ = p(ξ G p) T τ = ˆΨξ ˆΨG p + AM 1 (q) p. (3) assures asymptotically the input disturbance suppression (fault tolerance with respect to torque ripple, i.e. ( q, p) (,) as time t ) and the convergence of the state of the adaptive internal model to the fault signal (fault detection, i.e. ξ Y z). Proof: Consider for system (29) (obtained connecting (9) with (3)) the following Lyapunov function V = H x (x). Easy computations (remembering the skew-symmetry of interconnection matrix J(x)) show that there exists two real numbers η A IR, η F IR (depending on design matrices A and F ) and η Ψ IR, such that V η A p 2 + η F χ 2 + Ψ T pχ η A p 2 + η F χ 2 + η Ψ p χ. Using a Young s inequality argumentation we can write: (31) V η A p 2 + η F χ 2 + η Ψ 2 ε p 2 + η Ψ 2ε χ 2, (32) Remark. The fault detection and isolation phase can be performed by checking the state of the fault compensation unit which automatically offsets the fault effect. In this framework the detection phase can be easily carried out by comparing ξ(t) with a suitably tuned threshold; in fact, as proved in Proposition 2, ξ(t) asymptotically converge to Y z(t) which is zero in the un-faulty case and different from zero when a fault occurs. Also the isolation procedure is possible in this framework. According to the modelling presented, it is clear that fault acting on different actuators, are represented by different components of the exogenous signal Γz(t). In view of this, setting ẑ(t) := Ŷ 1 ξ(t), it is possible to define residual signals as r i (t) = { 1 if Γi ẑ(t) T i otherwise (i = 1...n) where T i, (i = 1...n) are n positive thresholds and Γ i is the i-th row of matrix Γ.

6 4 2 internal model on adaptation on internal model on internal model off adaptation off Fig. 2. Tracking error for the first and second joint angle positions (rad) θ 1 (upper plot) and θ 2 (lower plot) Fig. 3. From upper to lower plot: disturbance torque ripple acting on the first joint (Nm), controlled torque τ 1 on the first joint (Nm) and controlled torque τ 2 on the second joint (Nm) V. SIMULATION RESULTS In order to check the performances of the regulator above designed, a certain number of tests have been made, simulating the response of a 2-dof fully actuated mechanical manipulator without dissipation and inertia matrix defined by [ ] M =, subject to various sinusoidal torque disturbances acting on both joints. The results of these tests are shown in fig. 2 and fig. 3. In particular the disturbance ripple to be rejected was a sinusoidal signal δ(t) = V sin(ωt) with amplitude V = 1 Nm and (unknown) frequency Ω =.4rad/sec occurring at time t = 1 sec and affecting only the first joint torque. The internal model unit was connected at time t = 2 sec, but the frequency adaptation unit was connected only at time t = 3 sec; in fig. 2 it is possible to see clearly the action of the adaptation unit, avoiding completely the effect of the disturbance ripple on the tracking error for the angle position of the first joint; moreover it is worth to point out that the adaptation unit makes it possible to obtain a perfect detection of the fault occurring on the first joint: in fig. 3, while the upper plot represents the disturbance ripple, the last two plots show the behavior of the controlled torques and how the adaptation unit makes it possible to clearly see the fault affecting only the control torque acting on the first joint. The adaptation unit is then disconnected again at time t = 5 sec for 2 sec in order to point out that the effect of the exogenous ripple is always present and that a classical internal model designed for a wrong frequency is not able to overcome completely this disturbance. VI. CONCLUSIONS Main result of this paper is contained in section 4 where an adaptive internal model unit is designed in order to compensate unknown spurious torque harmonics that degrade performances of an n-dof fully actuated mechanical robot. We have shown how a standard tracking robot control, can be augmented with an internal model unit to achieve global implicit fault tolerance to all the faults belonging to the model embedded in the regulator. We also have shown how it is possible to perform fault detection and isolation simply testing the state of the internal model. REFERENCES [1] C. Bonivento, A. Isidori, L. Marconi, and A. Paoli. Implicit fault tolerant control: Application to induction motors. Automatica, 4(3): , 24. [2] C.I. Byrnes, F. Delli Priscoli, and A. Isidori. Output regulation of uncertain nonlinear systems. Birkhäuser, Boston, [3] C.I. Byrnes, F. Delli Priscoli, A. Isidori, and W. Kang. Structurally stable output regulation of nonlinear systems. Automatica, 33(2): , [4] P. M. Frank. Fault diagnosis in dynamic systems using analitical and knowledge based redundancy: a survey and some new results. Automatica, 26(3), 199. [5] K. Fujimoto, K. Sakurama, and T. Sugie. Trajectory tracking control of port-controlled hamiltonian systems via generalized canonical transformations. Automatica, 39(12): , 23. [6] L. Gentili and A. J. van der Schaft. Regulation and input disturbance suppression for port-controlled hamiltonian systems. 2nd IFAC Workshop LHMNLC, Seville, Spain, 23. [7] A. De Luca and R. Mattone. Actuator failure detection and isolation using generalized momenta. ICRA, Taipei, Taiwan, 23. [8] V.O. Nikiforov. Adaptive non-linear tracking with complete compensation of unknown disturbances. European Journal of Control, 4: , [9] R. Ortega. Some applications and recent results on passivity based control. 2nd IFAC Workshop on Lagrangian and Hamiltonian Methods for Nonlinear Control, Seville, Spain, 23. [1] A. Serrani, A. Isidori, and L.Marconi. Semiglobal output regulation with adaptive internal model. IEEE Transaction On Automatic Control, 46(8): , August 21. [11] G. Takegaki and S. Arimoto. A new feedback method for dynamic control of manipulators. ASME Journal of Dynamic Systems Measurement and Control, 12, [12] A.J. van der Schaft. L 2 -gain and Passivity Techniques in Nonlinear Control. Springer-Verlag, London, UK, 1999.