Fault Tolerance in Cooperative Manipulators Renato Tin´os∗ , Marco H. Terra∗ , and Marcel Bergerman∗∗ ∗
Electrical Eng. Department, EESC, University of S˜ ao Paulo C.P.359, S˜ ao Carlos, SP, 13560-970, Brazil ∗∗
Genius Institute of Technology
Av. A¸ca´ı, 875, Bloco E, Manaus, AM, 69075-904, Brazil
[email protected] ,
[email protected] ,
[email protected]
Abstract The problem of fault tolerance in cooperative manipulators rigidly connected to a solid object is addressed in this paper. Four faults are considered: free-swinging joint faults, locked joint faults, incorrect measured joint position, and incorrect measured joint velocity. The faults are first detected by a fault detection and isolation system. Freeswinging and locked joint faults are isolated using artificial neural networks. The other faults are isolated based on the kinematic constraints imposed on the cooperative system. After the isolation of the faults, the control system is reconfigured. Control laws for the system with passive or locked joints are developed. Results of the fault tolerance system applied in simulations and in a real cooperative system are presented.
1 Introduction Nowadays, robots are each time more used in medicine, outer space, deep sea, homes, and other unstructured or hazardous environments. Robots are used in such environments to avoid the exposition of human beings to danger or because of the reliability of robots in executing repetitive tasks. However, faults can put in risk the robot, its task, and the working environment. Faults in robots have been usual due to the complexity of such systems. There are several sources of faults in robots, such as electrical, mechanical, hydraulic, etc. [12]. Some research indicate that the mean-time-to-failure in industrial robots are between 500 and 2500 hours [3]. If this number is small in structured environments, it is probably smaller in unstructured or hazardous environments due to external factors, as extreme temperatures, obstacles, radiation, etc. So, there are good reasons to research fault detection and isolation (FDI) systems for robots. In some environments, human beings cannot be sent to make the necessary repairs in faulty robots because of the hazardous or distance. In these cases, it is necessary to provide fault tolerance to the robot. Fault tolerance is also necessary when a fast response is required after a fault, like in robots used to disarm explosives.
Robotic systems with kinematic or actuation redundancy are interesting in applications where fault tolerance is needed because the number of degrees of freedom (dof) in these systems is greater than the dof required to execute the task. Actuation redundancy can be found in closedlink mechanisms like cooperative systems formed by two or more manipulators. As in the human case, where the use of two arms presents an advantage over the use of only one arm in several cases, two or more robots can execute tasks that are difficult or even impossible for only one robot [13]. For example, cooperative robots can be used in the manipulation of heavy, large or flexible loads, assembly of structures, and manipulation of objects that can slide from only one robot. Actuation redundancy makes the use of cooperative robots in unstructured or hazardous environments very appealing. However, as cited before, fault tolerance is crucial in these environments. In this work, a fault tolerance scheme for cooperative systems is presented. Section 2 describes the kinematics and the dynamics of cooperative manipulators, and Section 3 describes the fault tolerance scheme. Four faults are considered: free-swinging joint faults (FSJF’s), locked joint faults (LJF’s), incorrect measured joint position faults (JPF’s), and incorrect measured joint velocity faults (JVF’s). First, the faults are detected by an FDI system based on the kinematic constraints imposed on the cooperative system and by neural networks (Section 4). Then, the control system is reconfigured according to the isolated fault (Section 5). Section 6 presents the results of the fault tolerance system in simulations and in a real system. Finally, the conclusions are in Section 7.
2 Cooperative Manipulators The eq. of motion for the i-th arm in a fault-free system with m arms rigidly connected to the load is q ¨i = Mi (qi )−1 [τi + Ji (qi )T hi − gi (qi ) − Ci (qi )q˙ i ] (1) where qi is the vector of joint angles of arm i, i = 1, . . . , m, τi is the vector of the torques at the joints of arm i, Mi is the inertia matrix, Ci is the matrix of the centrifugal
and Coriolis terms, gi is the gravitational vector, Ji is the Jacobian (from joint velocity to end-effector velocity) of arm i, and hi is the force vector at the end-effector of arm i. The dynamics of all arms can be written as q ¨ = M(q)−1 [τ + J(q)T h − g(q) − C(q)q] ˙
(2)
T T ] , h = where q = [qT1 qT2 . . . qTm ]T , τ = [τ1T τ2T . . . τm T T T T [h1 h2 . . . hm ] , M is formed by the individual inertia matrices of the arms, C is formed by the individual centrifugal and Coriolis matrices of the arms, g is formed by the individual gravitational vectors of the arms, and J is formed by the terms Ji for i = 1, . . . , m. The equation of motion for the load is
x ¨o = Mo −1 [−Jo (xo )T h − bo (xo , x˙ o )]
(3)
where xo is the k-dimensional vector of load position and orientation at the center of gravity (CG), bo is the vector of centrifugal, Coriolis, and gravitational terms, Mo is the object’s inertia matrix, and Jo (xo ) = £ ¤T Jo1 (xo )T . . . Jom (xo )T , where Joi converts velocities at the CG into velocities at the end-effector of arm i. As it is possible to compute the positions and orientations of the object using the positions of the joints of any arm of the cooperative system, the following kinematic constraint appears xo = ϕ1 (q1 ) = ϕ2 (q2 ) = . . . = ϕm (qm )
(4)
where ϕi (qi ) is the vector of the position and orientation of the object computed via the joint positions of arm i, i.e., the direct kinematics of arm i. Differentiating eq. (4) x˙ o = D1 (q1 )q˙ 1 = D2 (q2 )q˙ 2 = . . . = Dm (qm )q˙ m
(5)
where Di is the Jacobian relating joint velocities of arm i and load velocities. The control of cooperative manipulators is a complex task due to the interaction among the arms caused by the forces and the kinematic constraints. The control should be coordinated and the squeeze forces at the object should be minimized to avoid damage to the load. The squeeze forces are the components of the forces and torques in the object that do not contribute to the motion. The forces and torques that contribute to the motion are called motion forces and are orthogonal to the squeeze forces [14]. The squeeze forces are given by hos = Ps (xo )h
(6)
where the matrix Ps transforms the forces and torques at the end-effectors in the squeeze forces. Several solutions appeared to deal with the control problem in fault-free cooperative manipulators rigidly connected to a solid object, as the master/slave strategy [5], the optimal division of the load control [2], the definition of new task objectives or variables [1], and the hybrid control of motion and squeeze in the object [14]. The hybrid
control developed in [14] is particularly interesting because the motion and the squeeze control in the load are decomposed, and because it does not utilize the detailed model of the system. In the cooperative system, joint torques in the form DT hos , where D is formed by the Jacobian matrices Di , do not affect the motion if there are no singular configuration of the arms. However, the motion of the arms affects the squeeze forces due to the squeeze components of the d’Alembert (inertial) forces. Thus, in [14], a stable motion control with compensation of the gravitational torques is projected ignoring the squeeze terms. Then, the squeeze control is projected considering the inertial forces as perturbation. In this work, the control system proposed in [14] is employed for the fault-free system, and it is used to develop controllers for the cooperative system with FSJF and LJF.
3 Fault Tolerance System The fault tolerance system is proposed based on the following faults: FSJF’s, where an actuation lost occurs in a joint of one arm; LJF’s, where a joint of one arm is locked; JPF’s, where the readings of one joint positions are not correct; and JVF’s, where the readings of one joint velocities are not correct. Because of the dynamic coupling of the joints, the inertia, and the gravitational forces, the faulty arms can quickly accelerate into wild motions that can cause serious damage [12]. As the cooperative system’s controller is not projected to operate with faults, the squeeze force can increase and cause damage to the load or instability to the system. The fault tolerance scheme proposed here is displayed at figure 1. The faults are first detected and isolated by an FDI system. When the faults are detected, the arms may be locked by brakes and the trajectory planning reconfigured. Other option is to apply the brakes and to reconfigure the trajectory planning starting with the current load velocity. The choice of one or another scheme should be done considering the parameters of the cooperative system, joint configurations, and joint velocities. If an FSJF or an LJF occurs, the control system is reconfigured (Section 5). If a JPF or a JVF occurs, the positions or velocities of the faulty joint are estimated using the positions and velocities of the other joints. This is possible because the cooperative system is kinematically closed.
4 FDI System A three step FDI system applied in each sample time is proposed here. First, JPF’s are detected by analyzing the position constraints (eq. 4). Then, JVF’s are detected by analysing the velocity constraints (eq. 5). The last step is the detection of FSJF’s and JLF’s via artificial neural
effects. Robust technique, fuzzy logic [6], ANN [7], [11] have been used to avoid these problems.
Figure 1: Fault tolerance system.
networks (ANN). By simplicity, the occurrence of only one fault each time is considered. 4.1 Joint Position and Joint Velocity Faults As xo can be calculated using the joint positions of any arm (eq. 4), it is possible to identify the arm f with the wrong joint position reading if m > 2. If the arm f is know, the vector xo can be estimated based on the Forward Kinematics of the other arms. The faulty joint can be identified by estimating the position of all joints of arm f . For each joint i, the vector xo is estimated by substituting the joint reading of the joint i for its estimate and using the readings of the other joints. The fault is isolated for that joint that gives the closest estimate of the vector xo [8]. If m = 2, the arm f cannot be identified just looking at the estimated xo for each arm. In this case, the estimation of the joint positions should be done for the two arms. The estimate of the vector xo for each estimated joint position should be then compared with the estimate of the vector xo for the other arm. As it is possible to calculate the velocity of the load by using the joint velocities of any arm, JVF’s can be detected in a similar way of JPF’s. The estimated velocities of the joints can be calculated using eq. 5, and can be used to isolate de joint with the JVF’s. 4.2 Free-Swinging and Locked Joint Faults As FSJF’s and LJF’s present effects in the dynamics of the cooperative system, the residual generation scheme is used to detect these faults. Generally, FDI system for individual manipulators employs the mathematical model of the arm in the residual generation scheme [12]. The residual vector is generated by comparison of the measured states of the arm with estimates of them. However, modelling errors may appear, generating false alarms or hiding the fault
To the best of the author’s knowledge, only in [10] and [8] FDI for cooperative manipulators was studied. In [10], only one multilayer perceptron (MLP) was trained to reproduce the dynamic of all arms (eq. 2). As the endeffector forces are function of the joint variables, the inputs of the MLP are the joint positions, velocities and torques in the arms at instant t. The outputs of the MLP are compared with the joint velocities at instant t + ∆t in order to generate the residual vector. The residual vector is then classified by a radial basis function network (RBFN) that gives the fault information. The use of only one MLP is a interesting approach when the end-effector forces are not measured. However, most of the controllers for cooperative manipulators use force sensors to minimize the squeeze forces on the load, and these variables can be very useful to map the system’s dynamics. Furthermore, the mapping of the MLP in [10] is dependent on the load parameters as the object’s mass. If the system has to manipulate a different object, the ANN have to be trained again. In [8], the dynamics of each manipulator (eq. 1) is mapped by a different MLP. This scheme is interesting because the mapping is not dependent on the load parameters and it is used here. The inputs of the MLP i are the joint positions, velocities, torques, and end-effector forces of the arm i at instant t. The outputs of this MLP are compared with the joint velocities of the arm i at instant t + ∆t in order to generate the residual vector of arm i (ˆ ri ). The residual vector from all arms (ˆ r) are then classified by an RBFN that gives the fault information. As the residual vector of FSJF and LJF occurring at the same joint can occupy the same region in the input space of the RBFN [7], an auxiliary input ζ that gives information about the velocity of the joints is used. The use of ζ is motivated by the fact that in LJF’s the velocity of the faulty joint is zero. As there are noise in the measurement of the joint velocity, the i = 1, . . . , n (n is the sum of the number of joints of all arms) component of ζ is defined as ½ 1 if |q˙i (t)| < δi ζi (t) = 0 otherwise where the threshold δi is a small number. Here, a value of δ proportional to the variance of the joint velocity measurement noise is used. In this work, the RBFN is trained by the Kohonen’s Self Organizing Map [10].
5 Control Reconfiguration 5.1 Joint Velocity and Position Faults When a JPF or JVF is isolated, the sensor readings of the faulty joint is ignored and the joint position or velocity is estimated based on the kinematic constraints. As the joint positions of the faulty arm f was already estimated by the
FDI system (Section 4), the component j = 1, . . . , n of the new joint position vector is defined as ½ qˆj if a JPF is isolated at joint j q ˆ[j] = θ[j] otherwise
manipulator with passive joints [4]. When m is odd
where qˆj is the estimate of the joint j position based on the other joint readings, and θ[j] is the measured position of joint j. In a similar way, the component j of the new joint velocity vector is defined as ½ ˆq˙j if a JVF is isolated at joint j ˆ˙ = q[j] ˙ θ[j] otherwise
Differentiating and partitioning eq. (11)
where ˆq˙j is the estimate of the joint j velocity based on ˙ is the measured velocity the other joint readings, and θ[j] of joint j.
where
5.2 Free-Swinging Joint Faults In [4], two arms with a number of passive joints np ≤k, where k is the dof of the load, is controlled by a modified computed torque controller. A Jacobian matrix Q that transforms the velocities of the actuated joints in the velocities of the load is obtained for m = 2, and makes possible the control of the system. If na > k, where na = n − np is the number of actuated joints, a method to control na − k components of the end-effector forces is proposed. Observe that the end-effector forces are controlled instead the squeeze forces. However, the end-effector forces are used to control the motion too, and the motion control affects the squeeze (Section 2). In [9], a new controller for the system with passive joints based on the decomposed control of the motion and squeeze [14] is proposed, and a new method to calculate the Jacobian matrix Q is proposed for the general case m > 1. The control system proposed in [9] is used here.
mxo = ϕ1 (q1 ) + ϕ2 (q2 ) + . . . + ϕm (qm ).
(7)
Differentiating and partitioning eq. (7) m X ∂ϕi (qi ) i=1
(−1)i+1 ϕi (qi ) = xo .
∂qa
q˙ a +
m X ∂ϕi (qi ) i=1
∂qp
Ra q˙ a + Rp q˙ p = x˙ o
(−1)
i+1
(12)
that relates actuated and passive joints velocities when m is odd. Using eq. (8), (10), and (12), x˙ o = Qq˙ a Q=
(13)
¢ 1 ¡ Da − D p R# p Ra m
(14)
if m is even, and ¡ ¢ ¢−1 ¡ Q = mI − Dp R# Da − D p R# p p Ra
(15)
if m is odd, where I is the identity matrix.
The motion control problem can be developed now. Partitioning eq. (2) in passive and actuated joints · ¸· ¸ · T ¸ ¸ · ¸ · ¸ · Maa
Map
q ¨a
Mpa
Mpp
q ¨p
+
Ca q ˙a Cp q ˙p
+
ga gp
=
τa 0
+
Ja
JT p
h.
(16) Consider now the Lyapunov function V =
1 T ¯ 1 1 T x˙ Mo x˙ o + q ¯˙ Mq ¯˙ + ∆xTo Kp ∆xo 2 o 2 2
(17)
where the two first terms are the kinetic energy of the system, ∆xo = (xo d − xo ) is the load position error, the ¤T £ , and diagonal matrix Kp is positive, q ¯˙ = q˙ Ta q˙ Tp · ¸ Maa
Map
Mpa
Mpp
.
˙ − 2C) is Differentiating eq. (17) and considering that (M antisymmetric, then V˙ = −x˙ To bo − q˙ Ta ga − q˙ Tp gp + q˙ Ta τa + ∆xTo Kp ∆x˙ o . (18)
q˙ p = Da q˙ a + Dp q˙ p
(8) where a refers to the actuated joints and p to the passive joints, qa is the vector of actuated joint positions, and qp is the vector of passive joint positions. From eq. (4), two cases can be considered. When m is even m X
(11)
i=1
¯ M=
From eq. (4),
mx˙ o =
m X
ϕi (qi ) = 0.
(9)
i=1
Differentiating and partitioning eq. (9) Ra q˙ a + Rp q˙ p = 0
(10)
that relates the actuated and passive joint velocities when m is even. Such relation cannot be found in an individual
Based on eq. (18), the following motion control law is proposed τm = QT (Kv ∆x˙ 0 + Kp ∆x0 ) (19) where Kv is a positive diagonal matrix, with a term to compensate the gravitational torques: T T τg = ga − (R# p R a ) gp + Q bo
(20)
if m is even, and ¢T ¡ gp + Q T bo τg = g a − R # p (Q − Ra )
(21)
V˙ = −x˙ To Kv x˙ o ≤ 0.
(22)
if m is odd. Substituting eq. (19)-(21) in eq. (18) and considering a step set-point, then
Now, the problem of the control of na −k components of the squeeze forces is considered. If we apply torques proportional to the end-effector forces, the stability is not guaranteed because the inertial component affects the squeeze. As the problem is caused by the feedback of the endeffector forces, [14] proposes a pre-processing of them by a strictly proper linear filter, as an integrator, for the faultfree system. Thus, the following squeeze control law for the system with (na > k) is used here τs = −Ea (q(t))T γs (t)
(23)
where the matrix Ea is formed by the Jacobian matrices Dai of the individual arms, and γs is the squeeze force that should be applied at the load by the squeeze force control when there are passive joints in the arms. The vector γs , which is in the squeeze subspace, must be calculated based on the constraints imposed by the passive joint and based on the integrator of the squeeze force error (hosd (t)− hos (t)) [9]. In summary, the control law is τa = τ m + τ g + τ s
(24)
where the terms in the right side are defined by eq. (19), (20), (21), and (23). 5.3 Locked Joint Faults A similar control system can be used to control the system with locked joints. However, the problem of the locked joints must be considered in the trajectory planning. Here, this problem will not be consider. The control problem for the system with locked joints performing a feasible trajectory will be considered now. As in the case of the system with passive joints, a Jacobian matrix relating the actuated joints, which are the joints that are not locked, and the load velocities can be defined. As the velocities of the locked joints are equal to zero, then x˙ o = Ql q˙ a
bases of the two arms). More details of the simulated system can be found in [8]. The FDI system was tested considering four trajectory sets, each of them with 480 trajectories with faults and 20 without faults. The first and the second sets have the same trajectories but with the faults starting at 0.15 s. and 0.3 s. respectively. The same occurs for the third and fourth sets. The four faults previously presented were simulated. In JPF’s and JVF’s, the correct sensor reading where changed by random numbers. The results of the FDI system can be viewed at table 1. The second and third columns present the number of detected faults and the number of correctly isolated faults respectively. The fourth column presents the number of false alarms in fault-free trajectories. The last column presents the Mean-Time-to-Detection (MTD): the mean time that the FDI system takes to isolate a fault after its occurrence. The tolerance system was still applied in a real cooperaTable 1: Set 1 2 3 4
where Ql = Using the same procedure described in the last section, the control law given by eq. (24) is utilized, where τm = QTl (Kv ∆x˙ 0 + Kp ∆x0 ),
(26)
τg = ga + QTl bo
(27)
and τs is given by eq. (23).
6 Results The fault tolerance system was applied in simulations of two three-dof planar cooperative arms manipulating an object with mass equal to 2.5 kg in a x-y plane. The gravity was parallel to the y-axis (the x-axis passes through the
Is. Faults 469 (97.71%) 461 (96.04%) 469 (97.71%) 459 (95.63%)
False Alarms 0 (0.00%) 0 (0.00%) 0 (0.00%) 0 (0.00%)
MTD(s) 0.016 0.017 0.017 0.019
tive system with two three-dof planar arms (figure 2). The two arms are equal and the axis of each joint is parallel to the gravity force (see [8] for more details of the real arms). The FDI system was tested considering three trajectory sets, each of them with 360 trajectories with faults and 15 without faults. The second and third sets have the same trajectories but an object of mass of 0.025 kg was manipulated in the second set and an object of 0.45 kg in the third set. The first set has different trajectories with the mass of load equal to 0.45 kg. The results of the FDI system considering the four faults described here occurring in each joint can be viewed at table 2. Figures 3 and 4 show a trajectory of the real system where an FSJF was isolated and the trajectory and control laws were reconfigured.
(25)
1 m Da .
Results of the FDI system: simulation.
Det. Faults 479 (99.79%) 480 (100.0%) 479 (99.79%) 480 (100.0%)
Table 2: Set 1 2 3
Results of the FDI system: real system.
Det. Faults 337 (93.61%) 333 (92.50%) 325 (90.28%)
Is. Faults 260 (72.22%) 247 (68.61%) 268 (74.44%)
False Alarms 1 (6.67%) 0 (0.00%) 0 (0.00%)
MTD(s) 0.469 0.419 0.458
7 Conclusions This work presents a fault tolerance system for cooperative manipulators. The faults are first detected and isolated and, then, the control system and the desired trajectory are reconfigured. Tests of the fault tolerance system applied to simulated cooperative arms and to real robots were presented. The number of correctly isolated faults was smaller in the real system mainly because FSJF’s were sometimes confused with LJF’s. This occurs because sometimes the velocities of the faulty vectors
Figure 2: Real system. Figure 4: Squeeze force in a trajectory showed at fig. 3.
[4] Y. H. Liu, Y. Xu, and M. Bergerman. Cooperation control of multiple manipulators with passive joints. IEEE Trans. on Robotics and Aut., 15(2):258–267, 1999. [5] J. Y. S. Luh and Y. F. Zheng. Constrained relations between two coordinated industrial robots for robot motion control. Int. J. of Robotics Res., 6(3):60–70, 1987. [6] H. Schneider and P. M. Frank. Observer-based supervision and fault-detection in robots using nonlinear and fuzzy logic residual evaluation. IEEE Trans. on Control Syst. Tech., 4(3):274–282, 1996. Figure 3: Position and orientation of the object in a trajectory with FSJF at joint 1 (arm 2). The FSJF started at t=1s.
were small due to the small gravitational torques at the joints. However, in this cases, even with FSJF’s, the load converged to the desired positions and the fault does not present significant effects in the system. This can occur, for example, if it is not necessary to apply high torques at the faulty joint in a given trajectory. Acknowledgments: This work is supported by FAPESP under grants 97/13384-7, 98/15732-5, and 99/10031-1.
References [1] F. Caccavale. Task-space regulation of cooperative manipulators. Prisma lab. tech. report 97-04, Universit`a degli Studi di Napoli Federico II, Napoli, Italy, 1997.
[7] M. H. Terra and R. Tin´os. Fault detection and isolation in robotic manipulators via neural networks - a comparison among three architectures for residual analysis. J. of Robotic Syst., 18(7):357–374, 2001. [8] R. Tin´os and M. H. Terra. Control of cooperative manipulators with passive joints. In Proc. of 15th IFAC World Cong. on Aut. Control, 2002. [9] R. Tin´os and M. H. Terra. Fault detection and isolation for multiple robotic manipulators. In Proc. of 2002 American Control Conf., 2002. [10] R. Tin´os, M. H. Terra, and M. Bergerman. Fault detection and isolation in cooperative manipulators via artificial neural networks. In Proc. of IEEE Conf. on Control App., pages 988–1003, 2001. [11] A. T. Vemuri and M. M. Polycarpou. Neuralnetwork-based robust fault diagnosis in robotic systems. IEEE Trans. on Neural Networks, 8(6):1410–1420, 1997. [12] M. L. Visinsky, J. R Cavallaro, and I. D. Walker. Robotic fault detection and fault tolerance: a survey. Reliability Eng. and Syst. Safety, 46:139–158, 1994.
[2] C. R. Carignan and D. L. Akin. Cooperative control of two arms in the transport of an inertial load in zero gravity. IEEE J. of Robotic and Aut., 4(4):414–419, 1988.
[13] M. Vukobratovic and A. Tuneski. Mathematical model of multiple manipulators: cooperative compliant manipulation on dynamical environments. Mech. and Machine Theory, 33:1211–1239, 1998.
[3] B. S. Dhillon and A. R. M. Fashandi. Robotic systems probabilistic analysis. Microel. and Reliability, 37(2):211–224, 1997.
[14] T. Wen and K. Kreutz-Delgado. Motion and force control for multiple robotics manipulators. Automatica, 28(4):729–743, 1992.
