- Email: [email protected]
Model based real time monitoring for collision detection of an industrial robot
Mechatronics 19 (2009) 695–704
Contents lists available at ScienceDirect
Mechatronics journal homepage: www.elsevier.com/locate/mechatronics
Model based real time monitoring for collision detection of an industrial robot Khaled Fawaz *, Rochdi Merzouki, Belkacem Ould-Bouamama LAGIS, UMR CNRS 8146, Ecole Polytechnique de Lille, Avenue Paul Langevin, 59655 Villeneuve d’Ascq, France
a r t i c l e
i n f o
Article history: Received 16 July 2008 Accepted 4 February 2009
Keywords: Fault Detection and Isolation (FDI) Industrial robot Dynamic modeling Co-simulation Bond graph
a b s t r a c t This paper deals with a model based real-time virtual simulator of industrial robot in order to detect eventual external collision. The implemented method concerns a model based Fault Detection and Isolation used to determine any lock of motion from an actuated robot joint after contact with static obstacles. Online implementation has been done in order to validate the proposed approach. Ó 2009 Elsevier Ltd. All rights reserved.
1. Introduction The fast modernization of the production equipments makes that the industrial systems become increasingly complex and very sophisticated. In particular, the use of robots in modern manufacturing industries was intensively increased during these last decades. Robot technology has been applied in many aspects such as industry, agriculture, family service and medical etc. These robots carry out without slackening repetitive tasks. In the assembly lines of auto industry for example, they replace the workmen in the painful and dangerous tasks like (painting, welding, stamping, etc.). Massive use of the robots requires monitoring in continuous time to avoid any kind of collisions or abnormal operations which are likely cause considerable losses. Simulation software (RobotStudio [20], SimPro [21]) proposed by the manufacturers of the robots in order to help users to validate their programs offline without interruption are often very expensive, moreover, these tools does not make it possible to have a real-time monitoring on the operation of the real robots. Many applications were developed in robotics during this last years in order to detect or to avoid collisions with an obstacle in the workspace of robot. Most of these applications are based on a virtual simulation. In [4] a robot simulation and monitoring system are described in order to establish a virtual environment using Java3D. It introduces the working principle and systematic architecture of a robot-oriented simulation and monitoring system based on Java3D and closed-loop feedback. In [7], a method simulating a redundant manipulator and the existing obstacle in their workspace with convex volumes, in order to avoid a collision is * Corresponding author. Tel.: +33 320436684; fax: +33 320337189. E-mail address: [email protected] (K. Fawaz). 0957-4158/$ - see front matter Ó 2009 Elsevier Ltd. All rights reserved. doi:10.1016/j.mechatronics.2009.02.005
presented, while in [8] the inverse kinematic problem of a spatial redundant or non-redundant manipulator considering the criteria of collision avoidance and the joint functionality limits is solved. In this paper, a 3D virtual simulator of an industrial robot with six degrees of freedom is developed. This simulator is based on the geometrical model of the manipulator robot and dynamical models of the six actuators which compose the joints of the robot. The main interest of this simulator is the real time monitoring of the real robot in real operation, allowing the detection of collisions. For modeling the joint actuator system, one needs a unified approach as bond graph tool [3], to represent the multiphysical aspect of the electromechanical system and to exploit the structural and causality properties for generating the diagnostic algorithm [1,9]. The innovative contribution through this work concerns the use of the model based Fault Detection and Isolation (FDI) theory [5,10,12], in detecting and isolating collisions on industrial robot manipulator. This is done by using an online virtual simulator of the robot based on the robot and the joint actuators models, by informing the supervision system of any external collisions through the evaluation in time domain of the residuals. This paper is organized as follows: after this section, the industrial robot system and online virtual simulator are presented in Section 2. Then, Section 3 regroups all the modeling of the studied system. The FDI algorithm is presented in Section 4 while in Section 5 the experimental results demonstrate the aim of the monitoring approach for industrial context.
2. System description and simulator architecture The industrial studied system is a manipulator robot IRB140 of Fig. 1. This industrial robot is not accessible to the control
696
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
(embedded control loop). This robot offers a single combination combining fast acceleration, important working area and high load capacity. It carries out six DOF (degrees of freedom) characterized by rotation motions. The first three articulations of this manipulator characterize for the first a rotation around a vertical axis, the second and the third following two horizontal axes whose motions are identified by the variables hs1, hs2 and hs3, while the three last are characterized by the articular variables hs4, hs5 and hs6. 2.1. Communication with the Robot To communicate with the robot, one used the WebWare SDK [17]: software development kit, used to create PC-operator interfaces that communicate and interact with the ABB-robot controller through an Ethernet network. It allows:
Transferring program between the PC and robot, Reading and writing robot I/O and RAPID variables, Starting and stopping robot execution, Receiving messages and events from the robot, Reading current robot position.
Fig. 2. Communication architecture.
The WebWare SDK supports windows applications created with Microsoft Visual Basic. His interface consists of a set of OLE Custom Controls known as ActiveX controls. It includes four robot specified ActiveXs; one of them is the Helper control that provides methods, properties, and events to expose the entire S4 communication interface. The WebWare SDK is based on the ABB Interlink communication module, a core component for communication with the ABB-robot controller over the TCP/IP network protocol. The WebWare SDK supports windows applications created with Microsoft Visual Basic, and the virtual 3D simulator is created with DevC++. To communicate with the two applications, we used the pipe which is a section of shared memory that processes use for communication. The process that creates a pipe is the pipe server. A process that connects to a pipe is a pipe client. One process writes information to the pipe, and then the other process reads the information from the pipe. In our case, application developed with visual basic is considered as the pipe server, while the application developed with DevC++ is the pipe client. Communication between the two applications is showed in Fig. 2.
dows (XP) operating systems (Fig. 3). Through the local area network connected with the robot, simulation system can receive the real-time data from the real system and dynamically display those data in a 3D graphics mode or reserve them for analysis. The virtual simulator proposed in this paper, is an assistant and supplementary tool to real robot system, helping operators to observe working condition from every view angles while being distant.
2.2. 3D virtual simulator
3.1. Direct geometrical model
The Virtual simulator was developed in C++ and includes a graphical representation based on OpenGL under Microsoft Win-
Direct geometrical model is the whole relations which allow to calculate the position of the end-effectors, i.e. the operational coor-
3. System modeling In this section, a description of the studied system parts models is introduced with the following scheduling: 1. Direct geometric model of the robot; 2. Inverse geometric model of the robot according to planned trajectory; 3. Direct dynamic model of the robot; 4. Dynamic models of joint actuators.
Fig. 1. Robot ABB-IRB 140.
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
697
Fig. 3. Virtual simulator.
dinates of the robot according to the articular coordinates (see Fig. 1). In our application, we used the Denavit and Hartenberg convention [11] to find the direct geometrical model of the manipulator robot, by considering the bodies of the robot perfectly rigid. The geometric coordinates of the end-effector (centre of tool) is given in Eq. (1).
Raghavan and Roth method [19]: giving the solution of the robots with six articulations according to a 16 degrees polynomial. In the presented work, a planned trajectory is considered (Fig. 4). It consists to move the robot from position (a) to position (b) by varying the articular variables hs1 and hs2 while the other variables remain known. Expressions of hs1 and hs2 are given in Eq. (2).
3.2. Inverse geometric model Inversely to direct geometric model which allows calculating the coordinates of the end-effectors in the base frame, the inverse geometrical model allows to calculate the articular coordinates according to a known geometric position of the centre of the tool. Numbers of methods were proposed in the literature in order to calculate the inverse geometric model such as: Paul method [15]: using for the most industrial robots, it consist to isolate and identify the articular variables in a successive way by multiplying the corresponding transformation matrix at each stage. Pieper method [18]: using for the robots with six degrees of freedom and having three rotations (rotoidal) actuated joint or three linear actuated joint.
X ¼ ð0:317 ððcosðhs1 Þ cosðhs2 Þ sinðhs3 Þ cosðhs1 Þ sinðhs2 Þ cosðhs3 ÞÞ cosðhs4 Þ þ sinðhs1 Þ sinðhs4 ÞÞÞ cosðhs5 Þ þ ð0:317 ðcosðhs1 Þ cosðhs2 Þ cosðhs3 Þ þ cosðhs1 Þ sinðhs2 Þ sinðhs3 ÞÞÞ sinðhs5 Þ þ 0:380 cosðhs1 Þ cosðhs2 Þ cosðhs3 Þ 0:380 cosðhs1 Þ sinðhs2 Þ sinðhs3 Þ þ 0:36 cosðhs1 Þ cosðhs2 Þ þ 0:07 cosðhs1 Þ; Y ¼ ð0:317 ððsinðhs1 Þ cosðhs2 Þ sinðhs3 Þ sinðhs1 Þ sinðhs2 Þ cosðhs3 ÞÞ cosðhs4 Þ cosðhs1 Þ sinðhs4 ÞÞÞ cosðhs5 Þ þ ð0:317 ðsinðhs1 Þ cosðhs2 Þ cosðhs3 Þ þ sinðhs1 Þ sinðhs2 Þ sinðhs3 ÞÞÞ sinðhs5 Þ þ 0:380 sinðhs1 Þ cosðhs2 Þ cosðhs3 Þ 0:380 sinðhs1 Þ sinðhs2 Þ sinðhs3 Þ þ 0:36:sinðhs1 Þ cosðhs2 Þ þ 0:07 sinðhs1 Þ; Z ¼ 0:352 þ ð0:317 ðsinðhs2 Þ sinðhs3 Þ þ cosðhs2 Þ cosðhs3 ÞÞÞ cosðhs4 Þ cosðhs5 Þ þ ð0:317 ðsinðhs2 Þ cosðhs3 Þ cosðhs2 Þ sinðhs3 ÞÞÞ sinðhs5 Þ þ 0:380 sinðhs2 Þ cosðhs3 Þ þ 0:380 cosðhs2 Þ sinðhs3 Þ þ 0:36 sinðhs2 Þ;
ð1Þ
Y ; X qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi1 0 0:313 þ 0:889:Z þ 0:345 ð3:54 þ 17:6:Z 25:Z 2 Þ B C hs2 ¼ arctan B qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi C @ A: 2 0:607 1:725:Z þ 0:1778: ð3:54 þ 17:6:Z 25:Z Þ hs1 ¼ arctan
ð2Þ Fig. 4. Trajectory of the robot.
698
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
3.3. Direct dynamic model of the robot The direct dynamic model is that which expresses the articular accelerations according to articular positions, velocities and torques of articulations. It is then represented by the relation (3):
_ i Þ þ Cðhsi Þ ¼ si þ di : Mðhsi ; €hsi Þ þ Nðhsi ; hs
ð3Þ
_ i and € The signals hsi, hs hsi are the joint position, joint velocity and joint acceleration vectors respectively of the ith body of the ro_ i Þ 2 Rn is a vechsi Þ 2 Rnn is the inertia matrix, Nðhsi ; hs bot. Mðhsi ; € tor resulting from Coriolis and centrifugal forces. Cðhsi Þ 2 Rn is the vector resulting from the gravitational forces. si 2 Rn is the control input vector containing the torques and forces to be applied at each joint. di 2 Rn is the vector containing the unmodeled dynamics and other unknown external disturbances. Several formalisms were used to obtain the dynamic model of the robots [2,6,16] formalisms most often used are: Lagrange formalism, Newton–Euler formalism. In this work, we used the Lagrangian formalism in order to obtain the dynamic model of the six DOF robot. The Lagrangian formulations describe the motion equations in terms of work and energy of the system, which results by
d @L @L dP mot dP ext ¼ þ ; _ i @hsi _ i _ i dt @ hs dhs dhs
i 2 ½1; 6;
ð4Þ
where L is the Lagrangian which defined as the kinetic energy of the system minus its potential energy Pmot is the effort power of actuators Pext and is the external power effort. 3.4. Joint actuator modeling There are six electromechanical actuators located inside the six DOF robot and used for the traction motion. They are constituted by three principal components (Fig. 5): the DC motor part, which is the combination of an electrical, and mechanical parts, the gears system part and the load part. In this subsection, dynamic bond graph models of all of these components are graphically synthesized then expressed by differential equations. The use of bond
Fig. 6. Electrical RL circuit of the jth DC motor.
graph allows dealing with one tool from modeling to ARR generation. 3.4.1. Electrical part of the DC motor This part corresponds to RL electrical circuit of the jth DC motor (Fig. 6), composed by: input voltage source U 0j , electrical resistance Rej , inductance Lj and back electromotive force EMF, which is linear _ j and equal to K e h_ e , with K e to the angular velocity of the rotor he j j j is the EMF constant. The index j 2 ½1; 2 corresponds to the jth motor of the robot. The corresponding RL circuit bond graph’ model is given in integral causality by Fig. 7. Let us note e1j ; P 1j ; M 1j , effort, momentum and algebraic value of element I of the jth motor (Fig. 8). The gyrator element GY describes the power transfer from the electric to mechanic by a flow variable f4j of the link 4 and Se0j is the input voltage source, then the following state equation is obtained (5):
e1j ¼ Se0j Rej
With :
P 1j kej f4j M 1j
8 R P1 j 1 > > < f0j ¼ f1j ¼ f2j ¼ f3j ¼ Lj e1j dt ¼ M1j ¼ ij ; Se0j ¼ U 0j ; > > : f4j ¼ hej
ð5Þ
:
Thus, the corresponding dynamic equation of circuit of Fig. 8 is obtained (6)
Lj
dij ¼ U 0j Rej ij kej h_ ej : dt
ð6Þ
3.4.2. Mechanical part of the DC motor This part represents the mechanical part of the jth DC motor, characterizing by its rotor inertia Jes, viscous friction parameter fej , transmission axis rigidity Kj and a motorized torque Uj. In this
Fig. 5. jth DOF of the manipulator robot.
Fig. 7. Bond graph model of the electrical part of the jth DC motor.
699
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
e5j ¼ fej
P 5j þ e4j þ e6j K j M 5j
Z
f9j dt;
ð8Þ
8 R P5 j 1 _ > > > f4j ¼ f5j ¼ f6j ¼ f7j ¼ f8j ¼ Jej e5j dt ¼ M5j ¼ hej ; > > > R > > < e8j ¼ e9j ¼ K j f9j dt; With : _ j Nj hs _ j; f9j ¼ f8j f10j ¼ he > > > > > e4j ¼ U j ¼ kej ij ; > > > : e6j ¼ wj Thus, the corresponding dynamic equation of mechanical part is given as follows (9):
J ej
part, the influence of backlash phenomenon expressed by a disturbing torque wj is represented by a modulated effort source for the bond graph model [14]. The corresponding bond graph model in integral causality is given by Fig. 8. The backlash mechanism considered in this model is represented by a disturbing torque, hampering the smooth functioning of the system, and caused by simultaneous and evaluative reactions of the shock between the two sides of the gears system (Fig. 9). This torque is chosen continuous, nonlinear and differentiable, compared to the size of the gears system and its effect on the global system. So, a smooth and continuous model of transmitted torque wj which is developed in [14] and illustrated in Fig. 9 is taken as a modulated effort source ðMSe : wj Þ (Fig. 8). This torque is expressed as follows (7):
1 ecj Dhj ; 1 þ ecj Dhj
ð7Þ
where wj is the disturbing and nonlinear transmitted torque, Dhj ¼ hej Nj hsj defines the difference between input hej and output hsj motor positions, Nj a reducer constant, Aj a graphical parameter which is taken as a negative integer to describe the reaction effect of the disturbing torque, Kj the rigidity constant of the transmission system, s0j is the dead zone amplitude and cj ¼ 1=2 s0j the sigmoid function slope of the jth DC motor [14]. Let’s note e5j ; P5j ; M 5j effort, momentum and algebraic value of element I of Fig. 8, f4j is the flow variable of link 4, f9j ; 1=K j flow and algebraic value of element C, then the following state equation (8) is obtained:
Fig. 9. Backlash mechanism for the jth DC motor.
ð9Þ
With Uj is the input motor torque of the jth DC motor, given in function of the current ij and the torque constant Kcj.
Fig. 8. Bond graph model of the mechanical part of the jth DC motor.
wj ¼ Aj K j s0j
dh_ ej ¼ fej h_ ej þ U j wj K j ðhej Nj hsj Þ: dt
3.4.3. Gears part This part concerns the mechanical gears which link between the mechanical and the load parts with a reduction constant Nj (Fig. 5). Bond graph model of this part is given by Fig. 10 and represents a transformer element TF between the jth velocities of the motor axis h_ ej and the wheel h_ sj . According to Fig. 10, let us choose f10j and f11j as the corresponding flows variables of links 10 and 11:
f10 ¼ N j f11 :
ð10Þ
This corresponds to the mechanical equation (11):
h_ ej ¼ Nj h_ sj :
ð11Þ
3.4.4. Load part This part represents the load part of the jth electromechanical system, characterizing by its inertia J sj , viscous friction parameter fsj and backlash disturbing torque N j wj . The bond graph model of this part in integral causality is given by Fig. 11. Let’s note e14j ; P 14j ; M 14j effort, momentum and algebraic value of element I of Fig. 11. f9j is the flow and of element C (Fig. 8), then the following state equation (12) is obtained:
e14j ¼ fsj
P14j þ e13j þ e11j þ e27j ; M 14j
8 R P14 > f11j ¼ f12j ¼ f13j ¼ f14j ¼ Js1j e14j dt ¼ M14j ¼ h_ sj ; > > > j > > R > > < e11j ¼ Nj e9j ¼ Nj K j f9j dt; With : > f9j ¼ f8j f10j ¼ h_ ej Nj h_ sj ; > > > > e13j ¼ Nj wj ; > > > : e27j ¼ uj ;
Fig. 10. Bond graph model of jth gears part.
ð12Þ
700
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
Thus, the dynamic nonlinear model of the jth electromechanical system of the mobile robot is the following (14):
8 d Lj ðij Þ ¼ U 0j Rej ij kej h_ ej ; > > < dt J ej dtd ðh_ ej Þ ¼ fej h_ ej þ U j wj K j ðhej Nj hsj Þ; > > : J sj dtd ðh_ sj Þ ¼ fsj h_ sj þ Nj wj þ Nj K j ðhej Nj hsj Þ uj
ð14Þ
4. Fault Detection and Isolation algorithm
Fig. 11. jth load part bond graph model.
Thus, the corresponding dynamic equation of mechanical part is given as follows (13):
J sj
dh_ sj ¼ fsj h_ sj þ Nj wj þ Nj K j ðhej Nj hsj Þ uj ; dt
ð13Þ
/j are the nonlinear impact efforts, function of h_ ej and h_ sj and regroups Coriolis, centrifugal and gravitational forces applied at each joint. It includes also the non modeled dynamic parts and unknown external disturbances. After a concatenation of the different bond graph models, the global model of the jth electromechanical system is deduced in Fig. 12. The measured and estimated states are shown by detectors element Df : h_ sj and Df : h_ ej deduced from derivation of the articular joint positions.
In this section, a model based Fault Detection and Isolation algorithm (FDI) [12,10] is build, in order to detect and to localize the presence of static obstacles on the jth joint of the manipulator robot. By using the FDI algorithms generated directly from the bond graph model [1], a list of analytical redundancy relation (ARR) along with the corresponding fault signature matrix (FSM) can be deduced. The principal of the presented approach is the use of (ARR) theory, not to detect a jth faulty actuator, but to detect an external joint obstacle which locks the normal operating of the jth actuator. The FDI proposed approach is based on the calculation of the residuals issued from the analytical redundancy relation (ARR) and it makes the difference between the dynamic system in normal and faulty situations. Note that for an observable system, with none an unresolved algebraic loop, the number of ARR generated is equal to the number of the measured states [13]. Knowing that the parameters uncertainties are not considered in this study, so, the used parameters are supposed perfectly known and given in Fig. 15. The main steps to generate the list of ARR and the FSM by using are summarized below [1]: Build the bond graph model in preferred integral causality; Put the bond graph model in preferred derivative causality after dualization of the sensors;
Fig. 12. jth global bond graph model of the electromechanical system.
Fig. 13. jth global bond graph model of the electromechanical system in derivative causality.
701
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
Write the constitutive relation for each junction; Eliminate the unknown variables from each constitutive relation by covering the causal paths in the bond graph model; Generate the list of ARR and the corresponding FSM. Note that for an observable system, the number of ARR generated is equal to the number of detectors on the bond graph model [1]. 4.1. ARR1j generation The first ARRj corresponds to those generated from the jth electromechanical system where its bond graph model in derivative causality (Fig. 13). In this model, the C element is already in integral causality, because the systems subdetermined with the actual configuration. So, the dualization of the sensors is possible in this case because the initial conditions of this electromechanical system are supposed known, and then the C element can be presented in integral causality.
Fig. 15. Control input of actuators 1–6.
The constitutive relations of the junctions 11, 12, 01 and 13 are given by the following equations:
Fig. 14. Residuals generation from virtual real-time simulator.
Table 1 Fault signature matrix of the manipulator robot system.
Actuator 1 Actuator 2 Actuator 3 Actuator 4 Actuator 5 Actuator 6
h_ e1 h_ s1 h_ e2 h_ s2 h_ e3 h_ s3 h_ e4 h_ s4 h_ e5 h_ s5 h_ e6 h_ s 6
Db
Ib
r11
r21
r12
r22
r13
r23
r14
r24
r15
r25
r16
r26
1 1 1 1 1 1 1 1 1 1 1 1
0 0 0 0 0 0 0 0 0 0 0 0
1 1 0 0 0 0 0 0 0 0 0 0
1 1 0 0 0 0 0 0 0 0 0 0
0 0 1 1 0 0 0 0 0 0 0 0
0 0 1 1 0 0 0 0 0 0 0 0
0 0 0 0 1 1 0 0 0 0 0 0
0 0 0 0 1 1 0 0 0 0 0 0
0 0 0 0 0 0 1 1 0 0 0 0
0 0 0 0 0 0 1 1 0 0 0 0
0 0 0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 0 0 0 0 1 1
0 0 0 0 0 0 0 0 0 0 1 1
702
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
Table 2 Numerical parameters values of the actuators. Parameters Actuators 1 and 2 Je1,2 Js1,2 L1,2 Ke1,2 Actuator 3 Je3 Js3 L3 Ke3 Actuators 4 and 5 Je4,5 Js4,5 L4,5 Ke4,5 Actuator 6 Je6 Js6 L6 Ke6
Value
Parameters
Value
1.14 (kg m2) 0.1 (kg m2) 0.0037 (H) 0.142 (N m/A)
N1,2 fe1,2 R1,2 K1,2
62 0.1 (N m s/rad) 0.54 (X) 0.1 (N m/rad)
4 (kg m2) 0.05 (kg m2) 0.005 (H) 0.2 (N m/A)
N3 fe3 R3 K3
53 0.1 (N m s/rad) 0.8 (X) 0.1 (N m/rad)
0.09 (kg m2) 0.008 (kg m2) 0.01 (H) 0.6 (N m/A)
N4,5 fe4,5 R4,5 K4,5
76 0.1 (N m s/rad) 1 (X) 0.01 (N m/rad)
0.0023 (kg m2) 0.002 (kg m2) 0.15 (H) 0.13 (N m/A)
N6 fe6 R6 K6
76 0.1 (N m s/rad) 1 (X) 0.01 (N m/rad)
e2 ¼ e0 e1 e3 ;
ð15Þ
e15 ¼ e4 þ e5 e6 þ e7 þ e8 ;
ð16Þ
f10 ¼ f8 f9 ;
ð17Þ
e11 ¼ e12 þ e14 e16 e13 e27 :
ð18Þ
Two structurally independent ARRj of the jth electromechanical system can be generated from Eqs. (16) and (17) after eliminating the unknown variables. This elimination process is achieved by following the causal paths from unknown to known variables on the bond graph model. From Eq. (16), the unknown variables are given by the following relations:
8 e15 ¼ 0; > > > > d d _ > > < e5 ¼ J ej dt ðf5 Þ ¼ J ej dt ðhej Þ; e7 ¼ fej f7 ¼ fej h_ ej ; > > > > dh_ s > > : e8 ¼ e10 ¼ eN11 ¼ N1 fsj h_ sj Nj wj þ J sj dtj þ uj j
Fig. 16. Articular variables in normal operation.
Fig. 17. Residuals in normal operation.
ð19Þ
j
From the junction 11 the following equations are deduced:
8 < e3 ¼ U 0j e2 e1 ¼ U 0j Rej f3 Lj df3 ; dt : f3 ¼ k1e e4
ð20Þ
j
Then,
e3 ¼ U 0j
R ej Lj de4 e4 ¼ kej f4 ¼ kej h_ ej : kej kej dt
ð21Þ
Fig. 18. Residuals in collision situation.
From Eqs. (19) and (21) the following ARR1j is deduced:
R ej d J ej ðh_ ej Þ wj þ fej h_ ej dt kej 1 d fsj h_ sj Nj wj þ J sj ðh_ sj Þ þ Nj dt Lj d d _ 1 þ fs h_ sj Nj wj J ej ðhej Þ wj þ fej h_ ej þ dt Nj j kej dt d _ þ J sj ¼0 hsj þ uj dt
ARR1j : K ej h_ ej U 0j þ
ð22Þ
4.2. ARR2j generation The second ARRj can be deduced from Eq. (13), where the following system is obtained:
Fig. 19. Articular positions in collision situation.
703
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
8 f10 ¼ Nj f11 ¼ Nj h_ sj ; > > < f8 ¼ f15 ¼ h_ ej ; > > : f ¼ 1 de9 ¼ 1 N d ðe 9
Kj
Kj
dt
j
dt
ð23Þ 12
8 e13 ¼ Nj wj > > > > > < e12 ¼ fsj f12 ¼ fsj h_ sj ;
ð24Þ
dh_ s > > e14 ¼ J sj dfdt14 ¼ J sj dtj e16 ¼ 0 > > > : e ¼ u : 27 j
The second ARRj is then deduced by replacing the unknown variables of Eqs. (23) and (17) by their expressions in (24):
dðhsj Þ d J sj þ fsj h_ sj Nj wj þ uj þ K j ðN j h_ sj h_ ej Þ ¼ 0: dt dt ð25Þ
Once the list of possible ARRj is generated, the Fault Signature Matrix (FSM) can be built by deducing the signature of the system components on each ARRj. Note that each component in the physical system can be represented by one or more variables in the ARRsj. In addition, in the residuals’ FSM are represented instead of ARR, where each residual rkj is a numerical evaluation of an ARRkj. The corresponding residuals r 1j and r2j of ARR1j and ARR2j j 2 ½1; 2 are given by the following relations:
r 1j ¼
2 Lj fej Rej J ej Lj J sj d _ 2 ðhsj Þ þ þ kej dt kej Nj dt k ej k ej ! ! Rej J sj Lj fsj R e fe d ðh_ sj Þ þ kej þ j j h_ ej þ þ dt Nj kej Nj kej k ej
Lj J ej
þ
2
d
ðh_ ej Þ þ 2
dðh_ sj Þ d J þ fsj h_ sj Nj wj uj dt sj dt
þ K j ðN j h_ sj h_ ej Þ
þ e14 þ e16 e13 Þ;
where
ARR2j : Nj
r 2j ¼ N j
!
!
d _ ðhe Þ dt j
Rej fsj Re Rej Lej _j h_ sj U 0j 2 j wj uj u N j k ej kej kej Nj k ej N j ð26Þ
The corresponding FSM of the system under study is given in (Table 1). In this table (Table 1), the rows represent the components signatures and the columns are respectively the fault detectability Db, the fault isolability Ib, and the twelve residuals r1j and r2j . A ‘1’ value on respectively Db and Ib columns means that faults on the corresponding components are detectable and isolable. The presence of ‘1’ values on the residual columns shows the influence of the corresponding component on the residual dynamics. In this application, the FSM helps to detect and to distinguish the presence of eventual obstacles, which lock the normal operating of the actuator. 5. Experimental results In this section, experimental results have been done on the IRB140 robot while following the planned trajectory of Fig. 4. The principal of the developed co-simulation architecture is given by Fig. 14 and numerical values of the actuators parameters used for the online simulation are given in (Table 2). Fig. 15 shows the identified controls inputs of actuators models during normal operating. In this case the time domain variation of the angular joint positions and the whole residuals are represented successively by Figs. 16 and 17. It is noticed that the residuals still equal to 0 while there is no collision detection with the robot. By repeating the experiment after introducing static obstacle on the first and the second DOF, the residuals r21 and r22 derivate from zeros (Fig. 18) because it locks the normal motion of the appropriate actuated axis. Then the articular positions are given by Fig. 19. The virtual simulator (Fig. 20) shows the variation state of the residuals two residuals in collision situation.
Fig. 20. Online collision detection on the real robot from a virtual simulator.
704
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
6. Conclusion In this work, a virtual simulator of an industrial robot is developed in order to make online supervision for possible collisions detection. The real and virtual systems are operating in parallel, while a model based FDI algorithm is running in real-time. This practical approach can alert the operator to predict dangerous and accidental situations without been closer to the industrial robot and without using visions feedback surveillance. The validation of the method is done after system modeling validation and online experiments. For future work, a stochastic model of the used network interlink channel will be considered in the monitoring approach, in order to consider the influence of the information delay transmission between the real robot and the simulator and to distinguish the physical faulty situation from delay information situation. References [1] Ould Bouamama B, Samantaray AK, Staroswiecki M, Dauphin-Tanguy G. Derivation of constraint relations from bond graph models for fault detection and isolation. In: International conference on bond graph modeling and simulation (ICBGM’03), Simulation series, vol. 35(2); 2003. p. 104–9. [2] Coiffet P. Modeling and control of robots. Hermès, Paris, 156 p., ISBN:978-286601-079-9 (09/1986). [3] Karnopp DC. An approach to derivative causality in bond graph models of mechanical systems. J Franklin Inst 1992;329(1):65–75. [4] Zhijiang D, Donghai Y, Lining S, Lixin F. Virtual robot simulation and monitoring system based on Java3D. In: Proceedings of the 2004 IEEE international conference on robotics and biomimetics, Shenyang, China; August 2004. [5] Magni JF, Mouyon P. On residual generation by observer and parity space approaches. IEEE Trans Autom Control 1994;39(2):441–7.
[6] Khalil W, Dombre E. Modeling, identification and control of robots. Translation from a French editor book, Hermès, Paris, ISBN:2-7462-0003-1; 1999. [7] Mitsi S, Bouzakis KD. Simulation of redundant manipulators for collision avoidance in manufacturing and assembly environments. Mech Mach Theory 1993;28(1):13–21. [8] Mitsi S, Bouzakis KD, Mansour G. Optimization of robot links motion in inverse kinematics solution considering collision avoidance and joint limits. Mech Mach Theory 1995;30(5):653–63. [9] Djeziri MA, Merzouki R, Ould-Bouamama B, Dauphin-Tanguy G. Bond graph model based for robust fault diagnosis ACC’2007, American Control Conference, New York, USA; 2007. p. 3017–22. [10] Staroswiecki M, Comtet-Varga G. Analytical redundancy relations for fault detection and isolation in algebraic dynamic systems. Automatica 2001;37:687–99. [11] Hartenberg RS, Denavit JA. A kinematic notation for loxer pair mechanisms based on matrices. J Appl Mech 1995;77:215–21. [12] Isermann R. Supervision, fault detection and fault diagnosis methods – an introduction. Control Eng Pract 1997;5(5):639–52. [13] Merzouki R, Medjaher K, Djeziri MA, Ould-Bouamama B. Backlash fault detection in mechatronics system. Mech Ifac J 2007;17:299–310. [14] Merzouki R, Cadiou JC. Estimation of backlash phenomenon in the electromechanical actuator. J Cont Eng Pract 2005;13/8:973–83. [15] Paul RCP. Robot manipulators: mathematics, programming and control. Cambridge: MIT Press; 1981. [16] Renaud M. Quasi-minimal computation of the dynamic model of a robot manipulator utilizing the Newton–Euler formalism and the notion of augmented body. In: Proceedings of the IEEE international conference on robotics and automation, Raleigh, mars-avril; 1987. p. 1677–82. [17] WebWar SDK, ABB Flexible Automation Inc., Reference User Manual. [18] Pieper DL. The kinematics of manipulators under computer control. PhD thesis, Stanford University; 1968. [19] Raghavan M, Roth B. Inverse kinematics of the general 6R manipulator and related linkages. Trans ASME J Mech Des 1990;115:502–8. [20] http://www.abb.com/product/seitp327/c1c4814d2c350882c125715d003f80e 5.aspx. [21] http://www.simpro.it/home.php.
Contents lists available at ScienceDirect
Mechatronics journal homepage: www.elsevier.com/locate/mechatronics
Model based real time monitoring for collision detection of an industrial robot Khaled Fawaz *, Rochdi Merzouki, Belkacem Ould-Bouamama LAGIS, UMR CNRS 8146, Ecole Polytechnique de Lille, Avenue Paul Langevin, 59655 Villeneuve d’Ascq, France
a r t i c l e
i n f o
Article history: Received 16 July 2008 Accepted 4 February 2009
Keywords: Fault Detection and Isolation (FDI) Industrial robot Dynamic modeling Co-simulation Bond graph
a b s t r a c t This paper deals with a model based real-time virtual simulator of industrial robot in order to detect eventual external collision. The implemented method concerns a model based Fault Detection and Isolation used to determine any lock of motion from an actuated robot joint after contact with static obstacles. Online implementation has been done in order to validate the proposed approach. Ó 2009 Elsevier Ltd. All rights reserved.
1. Introduction The fast modernization of the production equipments makes that the industrial systems become increasingly complex and very sophisticated. In particular, the use of robots in modern manufacturing industries was intensively increased during these last decades. Robot technology has been applied in many aspects such as industry, agriculture, family service and medical etc. These robots carry out without slackening repetitive tasks. In the assembly lines of auto industry for example, they replace the workmen in the painful and dangerous tasks like (painting, welding, stamping, etc.). Massive use of the robots requires monitoring in continuous time to avoid any kind of collisions or abnormal operations which are likely cause considerable losses. Simulation software (RobotStudio [20], SimPro [21]) proposed by the manufacturers of the robots in order to help users to validate their programs offline without interruption are often very expensive, moreover, these tools does not make it possible to have a real-time monitoring on the operation of the real robots. Many applications were developed in robotics during this last years in order to detect or to avoid collisions with an obstacle in the workspace of robot. Most of these applications are based on a virtual simulation. In [4] a robot simulation and monitoring system are described in order to establish a virtual environment using Java3D. It introduces the working principle and systematic architecture of a robot-oriented simulation and monitoring system based on Java3D and closed-loop feedback. In [7], a method simulating a redundant manipulator and the existing obstacle in their workspace with convex volumes, in order to avoid a collision is * Corresponding author. Tel.: +33 320436684; fax: +33 320337189. E-mail address: [email protected] (K. Fawaz). 0957-4158/$ - see front matter Ó 2009 Elsevier Ltd. All rights reserved. doi:10.1016/j.mechatronics.2009.02.005
presented, while in [8] the inverse kinematic problem of a spatial redundant or non-redundant manipulator considering the criteria of collision avoidance and the joint functionality limits is solved. In this paper, a 3D virtual simulator of an industrial robot with six degrees of freedom is developed. This simulator is based on the geometrical model of the manipulator robot and dynamical models of the six actuators which compose the joints of the robot. The main interest of this simulator is the real time monitoring of the real robot in real operation, allowing the detection of collisions. For modeling the joint actuator system, one needs a unified approach as bond graph tool [3], to represent the multiphysical aspect of the electromechanical system and to exploit the structural and causality properties for generating the diagnostic algorithm [1,9]. The innovative contribution through this work concerns the use of the model based Fault Detection and Isolation (FDI) theory [5,10,12], in detecting and isolating collisions on industrial robot manipulator. This is done by using an online virtual simulator of the robot based on the robot and the joint actuators models, by informing the supervision system of any external collisions through the evaluation in time domain of the residuals. This paper is organized as follows: after this section, the industrial robot system and online virtual simulator are presented in Section 2. Then, Section 3 regroups all the modeling of the studied system. The FDI algorithm is presented in Section 4 while in Section 5 the experimental results demonstrate the aim of the monitoring approach for industrial context.
2. System description and simulator architecture The industrial studied system is a manipulator robot IRB140 of Fig. 1. This industrial robot is not accessible to the control
696
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
(embedded control loop). This robot offers a single combination combining fast acceleration, important working area and high load capacity. It carries out six DOF (degrees of freedom) characterized by rotation motions. The first three articulations of this manipulator characterize for the first a rotation around a vertical axis, the second and the third following two horizontal axes whose motions are identified by the variables hs1, hs2 and hs3, while the three last are characterized by the articular variables hs4, hs5 and hs6. 2.1. Communication with the Robot To communicate with the robot, one used the WebWare SDK [17]: software development kit, used to create PC-operator interfaces that communicate and interact with the ABB-robot controller through an Ethernet network. It allows:
Transferring program between the PC and robot, Reading and writing robot I/O and RAPID variables, Starting and stopping robot execution, Receiving messages and events from the robot, Reading current robot position.
Fig. 2. Communication architecture.
The WebWare SDK supports windows applications created with Microsoft Visual Basic. His interface consists of a set of OLE Custom Controls known as ActiveX controls. It includes four robot specified ActiveXs; one of them is the Helper control that provides methods, properties, and events to expose the entire S4 communication interface. The WebWare SDK is based on the ABB Interlink communication module, a core component for communication with the ABB-robot controller over the TCP/IP network protocol. The WebWare SDK supports windows applications created with Microsoft Visual Basic, and the virtual 3D simulator is created with DevC++. To communicate with the two applications, we used the pipe which is a section of shared memory that processes use for communication. The process that creates a pipe is the pipe server. A process that connects to a pipe is a pipe client. One process writes information to the pipe, and then the other process reads the information from the pipe. In our case, application developed with visual basic is considered as the pipe server, while the application developed with DevC++ is the pipe client. Communication between the two applications is showed in Fig. 2.
dows (XP) operating systems (Fig. 3). Through the local area network connected with the robot, simulation system can receive the real-time data from the real system and dynamically display those data in a 3D graphics mode or reserve them for analysis. The virtual simulator proposed in this paper, is an assistant and supplementary tool to real robot system, helping operators to observe working condition from every view angles while being distant.
2.2. 3D virtual simulator
3.1. Direct geometrical model
The Virtual simulator was developed in C++ and includes a graphical representation based on OpenGL under Microsoft Win-
Direct geometrical model is the whole relations which allow to calculate the position of the end-effectors, i.e. the operational coor-
3. System modeling In this section, a description of the studied system parts models is introduced with the following scheduling: 1. Direct geometric model of the robot; 2. Inverse geometric model of the robot according to planned trajectory; 3. Direct dynamic model of the robot; 4. Dynamic models of joint actuators.
Fig. 1. Robot ABB-IRB 140.
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
697
Fig. 3. Virtual simulator.
dinates of the robot according to the articular coordinates (see Fig. 1). In our application, we used the Denavit and Hartenberg convention [11] to find the direct geometrical model of the manipulator robot, by considering the bodies of the robot perfectly rigid. The geometric coordinates of the end-effector (centre of tool) is given in Eq. (1).
Raghavan and Roth method [19]: giving the solution of the robots with six articulations according to a 16 degrees polynomial. In the presented work, a planned trajectory is considered (Fig. 4). It consists to move the robot from position (a) to position (b) by varying the articular variables hs1 and hs2 while the other variables remain known. Expressions of hs1 and hs2 are given in Eq. (2).
3.2. Inverse geometric model Inversely to direct geometric model which allows calculating the coordinates of the end-effectors in the base frame, the inverse geometrical model allows to calculate the articular coordinates according to a known geometric position of the centre of the tool. Numbers of methods were proposed in the literature in order to calculate the inverse geometric model such as: Paul method [15]: using for the most industrial robots, it consist to isolate and identify the articular variables in a successive way by multiplying the corresponding transformation matrix at each stage. Pieper method [18]: using for the robots with six degrees of freedom and having three rotations (rotoidal) actuated joint or three linear actuated joint.
X ¼ ð0:317 ððcosðhs1 Þ cosðhs2 Þ sinðhs3 Þ cosðhs1 Þ sinðhs2 Þ cosðhs3 ÞÞ cosðhs4 Þ þ sinðhs1 Þ sinðhs4 ÞÞÞ cosðhs5 Þ þ ð0:317 ðcosðhs1 Þ cosðhs2 Þ cosðhs3 Þ þ cosðhs1 Þ sinðhs2 Þ sinðhs3 ÞÞÞ sinðhs5 Þ þ 0:380 cosðhs1 Þ cosðhs2 Þ cosðhs3 Þ 0:380 cosðhs1 Þ sinðhs2 Þ sinðhs3 Þ þ 0:36 cosðhs1 Þ cosðhs2 Þ þ 0:07 cosðhs1 Þ; Y ¼ ð0:317 ððsinðhs1 Þ cosðhs2 Þ sinðhs3 Þ sinðhs1 Þ sinðhs2 Þ cosðhs3 ÞÞ cosðhs4 Þ cosðhs1 Þ sinðhs4 ÞÞÞ cosðhs5 Þ þ ð0:317 ðsinðhs1 Þ cosðhs2 Þ cosðhs3 Þ þ sinðhs1 Þ sinðhs2 Þ sinðhs3 ÞÞÞ sinðhs5 Þ þ 0:380 sinðhs1 Þ cosðhs2 Þ cosðhs3 Þ 0:380 sinðhs1 Þ sinðhs2 Þ sinðhs3 Þ þ 0:36:sinðhs1 Þ cosðhs2 Þ þ 0:07 sinðhs1 Þ; Z ¼ 0:352 þ ð0:317 ðsinðhs2 Þ sinðhs3 Þ þ cosðhs2 Þ cosðhs3 ÞÞÞ cosðhs4 Þ cosðhs5 Þ þ ð0:317 ðsinðhs2 Þ cosðhs3 Þ cosðhs2 Þ sinðhs3 ÞÞÞ sinðhs5 Þ þ 0:380 sinðhs2 Þ cosðhs3 Þ þ 0:380 cosðhs2 Þ sinðhs3 Þ þ 0:36 sinðhs2 Þ;
ð1Þ
Y ; X qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi1 0 0:313 þ 0:889:Z þ 0:345 ð3:54 þ 17:6:Z 25:Z 2 Þ B C hs2 ¼ arctan B qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi C @ A: 2 0:607 1:725:Z þ 0:1778: ð3:54 þ 17:6:Z 25:Z Þ hs1 ¼ arctan
ð2Þ Fig. 4. Trajectory of the robot.
698
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
3.3. Direct dynamic model of the robot The direct dynamic model is that which expresses the articular accelerations according to articular positions, velocities and torques of articulations. It is then represented by the relation (3):
_ i Þ þ Cðhsi Þ ¼ si þ di : Mðhsi ; €hsi Þ þ Nðhsi ; hs
ð3Þ
_ i and € The signals hsi, hs hsi are the joint position, joint velocity and joint acceleration vectors respectively of the ith body of the ro_ i Þ 2 Rn is a vechsi Þ 2 Rnn is the inertia matrix, Nðhsi ; hs bot. Mðhsi ; € tor resulting from Coriolis and centrifugal forces. Cðhsi Þ 2 Rn is the vector resulting from the gravitational forces. si 2 Rn is the control input vector containing the torques and forces to be applied at each joint. di 2 Rn is the vector containing the unmodeled dynamics and other unknown external disturbances. Several formalisms were used to obtain the dynamic model of the robots [2,6,16] formalisms most often used are: Lagrange formalism, Newton–Euler formalism. In this work, we used the Lagrangian formalism in order to obtain the dynamic model of the six DOF robot. The Lagrangian formulations describe the motion equations in terms of work and energy of the system, which results by
d @L @L dP mot dP ext ¼ þ ; _ i @hsi _ i _ i dt @ hs dhs dhs
i 2 ½1; 6;
ð4Þ
where L is the Lagrangian which defined as the kinetic energy of the system minus its potential energy Pmot is the effort power of actuators Pext and is the external power effort. 3.4. Joint actuator modeling There are six electromechanical actuators located inside the six DOF robot and used for the traction motion. They are constituted by three principal components (Fig. 5): the DC motor part, which is the combination of an electrical, and mechanical parts, the gears system part and the load part. In this subsection, dynamic bond graph models of all of these components are graphically synthesized then expressed by differential equations. The use of bond
Fig. 6. Electrical RL circuit of the jth DC motor.
graph allows dealing with one tool from modeling to ARR generation. 3.4.1. Electrical part of the DC motor This part corresponds to RL electrical circuit of the jth DC motor (Fig. 6), composed by: input voltage source U 0j , electrical resistance Rej , inductance Lj and back electromotive force EMF, which is linear _ j and equal to K e h_ e , with K e to the angular velocity of the rotor he j j j is the EMF constant. The index j 2 ½1; 2 corresponds to the jth motor of the robot. The corresponding RL circuit bond graph’ model is given in integral causality by Fig. 7. Let us note e1j ; P 1j ; M 1j , effort, momentum and algebraic value of element I of the jth motor (Fig. 8). The gyrator element GY describes the power transfer from the electric to mechanic by a flow variable f4j of the link 4 and Se0j is the input voltage source, then the following state equation is obtained (5):
e1j ¼ Se0j Rej
With :
P 1j kej f4j M 1j
8 R P1 j 1 > > < f0j ¼ f1j ¼ f2j ¼ f3j ¼ Lj e1j dt ¼ M1j ¼ ij ; Se0j ¼ U 0j ; > > : f4j ¼ hej
ð5Þ
:
Thus, the corresponding dynamic equation of circuit of Fig. 8 is obtained (6)
Lj
dij ¼ U 0j Rej ij kej h_ ej : dt
ð6Þ
3.4.2. Mechanical part of the DC motor This part represents the mechanical part of the jth DC motor, characterizing by its rotor inertia Jes, viscous friction parameter fej , transmission axis rigidity Kj and a motorized torque Uj. In this
Fig. 5. jth DOF of the manipulator robot.
Fig. 7. Bond graph model of the electrical part of the jth DC motor.
699
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
e5j ¼ fej
P 5j þ e4j þ e6j K j M 5j
Z
f9j dt;
ð8Þ
8 R P5 j 1 _ > > > f4j ¼ f5j ¼ f6j ¼ f7j ¼ f8j ¼ Jej e5j dt ¼ M5j ¼ hej ; > > > R > > < e8j ¼ e9j ¼ K j f9j dt; With : _ j Nj hs _ j; f9j ¼ f8j f10j ¼ he > > > > > e4j ¼ U j ¼ kej ij ; > > > : e6j ¼ wj Thus, the corresponding dynamic equation of mechanical part is given as follows (9):
J ej
part, the influence of backlash phenomenon expressed by a disturbing torque wj is represented by a modulated effort source for the bond graph model [14]. The corresponding bond graph model in integral causality is given by Fig. 8. The backlash mechanism considered in this model is represented by a disturbing torque, hampering the smooth functioning of the system, and caused by simultaneous and evaluative reactions of the shock between the two sides of the gears system (Fig. 9). This torque is chosen continuous, nonlinear and differentiable, compared to the size of the gears system and its effect on the global system. So, a smooth and continuous model of transmitted torque wj which is developed in [14] and illustrated in Fig. 9 is taken as a modulated effort source ðMSe : wj Þ (Fig. 8). This torque is expressed as follows (7):
1 ecj Dhj ; 1 þ ecj Dhj
ð7Þ
where wj is the disturbing and nonlinear transmitted torque, Dhj ¼ hej Nj hsj defines the difference between input hej and output hsj motor positions, Nj a reducer constant, Aj a graphical parameter which is taken as a negative integer to describe the reaction effect of the disturbing torque, Kj the rigidity constant of the transmission system, s0j is the dead zone amplitude and cj ¼ 1=2 s0j the sigmoid function slope of the jth DC motor [14]. Let’s note e5j ; P5j ; M 5j effort, momentum and algebraic value of element I of Fig. 8, f4j is the flow variable of link 4, f9j ; 1=K j flow and algebraic value of element C, then the following state equation (8) is obtained:
Fig. 9. Backlash mechanism for the jth DC motor.
ð9Þ
With Uj is the input motor torque of the jth DC motor, given in function of the current ij and the torque constant Kcj.
Fig. 8. Bond graph model of the mechanical part of the jth DC motor.
wj ¼ Aj K j s0j
dh_ ej ¼ fej h_ ej þ U j wj K j ðhej Nj hsj Þ: dt
3.4.3. Gears part This part concerns the mechanical gears which link between the mechanical and the load parts with a reduction constant Nj (Fig. 5). Bond graph model of this part is given by Fig. 10 and represents a transformer element TF between the jth velocities of the motor axis h_ ej and the wheel h_ sj . According to Fig. 10, let us choose f10j and f11j as the corresponding flows variables of links 10 and 11:
f10 ¼ N j f11 :
ð10Þ
This corresponds to the mechanical equation (11):
h_ ej ¼ Nj h_ sj :
ð11Þ
3.4.4. Load part This part represents the load part of the jth electromechanical system, characterizing by its inertia J sj , viscous friction parameter fsj and backlash disturbing torque N j wj . The bond graph model of this part in integral causality is given by Fig. 11. Let’s note e14j ; P 14j ; M 14j effort, momentum and algebraic value of element I of Fig. 11. f9j is the flow and of element C (Fig. 8), then the following state equation (12) is obtained:
e14j ¼ fsj
P14j þ e13j þ e11j þ e27j ; M 14j
8 R P14 > f11j ¼ f12j ¼ f13j ¼ f14j ¼ Js1j e14j dt ¼ M14j ¼ h_ sj ; > > > j > > R > > < e11j ¼ Nj e9j ¼ Nj K j f9j dt; With : > f9j ¼ f8j f10j ¼ h_ ej Nj h_ sj ; > > > > e13j ¼ Nj wj ; > > > : e27j ¼ uj ;
Fig. 10. Bond graph model of jth gears part.
ð12Þ
700
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
Thus, the dynamic nonlinear model of the jth electromechanical system of the mobile robot is the following (14):
8 d Lj ðij Þ ¼ U 0j Rej ij kej h_ ej ; > > < dt J ej dtd ðh_ ej Þ ¼ fej h_ ej þ U j wj K j ðhej Nj hsj Þ; > > : J sj dtd ðh_ sj Þ ¼ fsj h_ sj þ Nj wj þ Nj K j ðhej Nj hsj Þ uj
ð14Þ
4. Fault Detection and Isolation algorithm
Fig. 11. jth load part bond graph model.
Thus, the corresponding dynamic equation of mechanical part is given as follows (13):
J sj
dh_ sj ¼ fsj h_ sj þ Nj wj þ Nj K j ðhej Nj hsj Þ uj ; dt
ð13Þ
/j are the nonlinear impact efforts, function of h_ ej and h_ sj and regroups Coriolis, centrifugal and gravitational forces applied at each joint. It includes also the non modeled dynamic parts and unknown external disturbances. After a concatenation of the different bond graph models, the global model of the jth electromechanical system is deduced in Fig. 12. The measured and estimated states are shown by detectors element Df : h_ sj and Df : h_ ej deduced from derivation of the articular joint positions.
In this section, a model based Fault Detection and Isolation algorithm (FDI) [12,10] is build, in order to detect and to localize the presence of static obstacles on the jth joint of the manipulator robot. By using the FDI algorithms generated directly from the bond graph model [1], a list of analytical redundancy relation (ARR) along with the corresponding fault signature matrix (FSM) can be deduced. The principal of the presented approach is the use of (ARR) theory, not to detect a jth faulty actuator, but to detect an external joint obstacle which locks the normal operating of the jth actuator. The FDI proposed approach is based on the calculation of the residuals issued from the analytical redundancy relation (ARR) and it makes the difference between the dynamic system in normal and faulty situations. Note that for an observable system, with none an unresolved algebraic loop, the number of ARR generated is equal to the number of the measured states [13]. Knowing that the parameters uncertainties are not considered in this study, so, the used parameters are supposed perfectly known and given in Fig. 15. The main steps to generate the list of ARR and the FSM by using are summarized below [1]: Build the bond graph model in preferred integral causality; Put the bond graph model in preferred derivative causality after dualization of the sensors;
Fig. 12. jth global bond graph model of the electromechanical system.
Fig. 13. jth global bond graph model of the electromechanical system in derivative causality.
701
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
Write the constitutive relation for each junction; Eliminate the unknown variables from each constitutive relation by covering the causal paths in the bond graph model; Generate the list of ARR and the corresponding FSM. Note that for an observable system, the number of ARR generated is equal to the number of detectors on the bond graph model [1]. 4.1. ARR1j generation The first ARRj corresponds to those generated from the jth electromechanical system where its bond graph model in derivative causality (Fig. 13). In this model, the C element is already in integral causality, because the systems subdetermined with the actual configuration. So, the dualization of the sensors is possible in this case because the initial conditions of this electromechanical system are supposed known, and then the C element can be presented in integral causality.
Fig. 15. Control input of actuators 1–6.
The constitutive relations of the junctions 11, 12, 01 and 13 are given by the following equations:
Fig. 14. Residuals generation from virtual real-time simulator.
Table 1 Fault signature matrix of the manipulator robot system.
Actuator 1 Actuator 2 Actuator 3 Actuator 4 Actuator 5 Actuator 6
h_ e1 h_ s1 h_ e2 h_ s2 h_ e3 h_ s3 h_ e4 h_ s4 h_ e5 h_ s5 h_ e6 h_ s 6
Db
Ib
r11
r21
r12
r22
r13
r23
r14
r24
r15
r25
r16
r26
1 1 1 1 1 1 1 1 1 1 1 1
0 0 0 0 0 0 0 0 0 0 0 0
1 1 0 0 0 0 0 0 0 0 0 0
1 1 0 0 0 0 0 0 0 0 0 0
0 0 1 1 0 0 0 0 0 0 0 0
0 0 1 1 0 0 0 0 0 0 0 0
0 0 0 0 1 1 0 0 0 0 0 0
0 0 0 0 1 1 0 0 0 0 0 0
0 0 0 0 0 0 1 1 0 0 0 0
0 0 0 0 0 0 1 1 0 0 0 0
0 0 0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 0 0 1 1 0 0
0 0 0 0 0 0 0 0 0 0 1 1
0 0 0 0 0 0 0 0 0 0 1 1
702
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
Table 2 Numerical parameters values of the actuators. Parameters Actuators 1 and 2 Je1,2 Js1,2 L1,2 Ke1,2 Actuator 3 Je3 Js3 L3 Ke3 Actuators 4 and 5 Je4,5 Js4,5 L4,5 Ke4,5 Actuator 6 Je6 Js6 L6 Ke6
Value
Parameters
Value
1.14 (kg m2) 0.1 (kg m2) 0.0037 (H) 0.142 (N m/A)
N1,2 fe1,2 R1,2 K1,2
62 0.1 (N m s/rad) 0.54 (X) 0.1 (N m/rad)
4 (kg m2) 0.05 (kg m2) 0.005 (H) 0.2 (N m/A)
N3 fe3 R3 K3
53 0.1 (N m s/rad) 0.8 (X) 0.1 (N m/rad)
0.09 (kg m2) 0.008 (kg m2) 0.01 (H) 0.6 (N m/A)
N4,5 fe4,5 R4,5 K4,5
76 0.1 (N m s/rad) 1 (X) 0.01 (N m/rad)
0.0023 (kg m2) 0.002 (kg m2) 0.15 (H) 0.13 (N m/A)
N6 fe6 R6 K6
76 0.1 (N m s/rad) 1 (X) 0.01 (N m/rad)
e2 ¼ e0 e1 e3 ;
ð15Þ
e15 ¼ e4 þ e5 e6 þ e7 þ e8 ;
ð16Þ
f10 ¼ f8 f9 ;
ð17Þ
e11 ¼ e12 þ e14 e16 e13 e27 :
ð18Þ
Two structurally independent ARRj of the jth electromechanical system can be generated from Eqs. (16) and (17) after eliminating the unknown variables. This elimination process is achieved by following the causal paths from unknown to known variables on the bond graph model. From Eq. (16), the unknown variables are given by the following relations:
8 e15 ¼ 0; > > > > d d _ > > < e5 ¼ J ej dt ðf5 Þ ¼ J ej dt ðhej Þ; e7 ¼ fej f7 ¼ fej h_ ej ; > > > > dh_ s > > : e8 ¼ e10 ¼ eN11 ¼ N1 fsj h_ sj Nj wj þ J sj dtj þ uj j
Fig. 16. Articular variables in normal operation.
Fig. 17. Residuals in normal operation.
ð19Þ
j
From the junction 11 the following equations are deduced:
8 < e3 ¼ U 0j e2 e1 ¼ U 0j Rej f3 Lj df3 ; dt : f3 ¼ k1e e4
ð20Þ
j
Then,
e3 ¼ U 0j
R ej Lj de4 e4 ¼ kej f4 ¼ kej h_ ej : kej kej dt
ð21Þ
Fig. 18. Residuals in collision situation.
From Eqs. (19) and (21) the following ARR1j is deduced:
R ej d J ej ðh_ ej Þ wj þ fej h_ ej dt kej 1 d fsj h_ sj Nj wj þ J sj ðh_ sj Þ þ Nj dt Lj d d _ 1 þ fs h_ sj Nj wj J ej ðhej Þ wj þ fej h_ ej þ dt Nj j kej dt d _ þ J sj ¼0 hsj þ uj dt
ARR1j : K ej h_ ej U 0j þ
ð22Þ
4.2. ARR2j generation The second ARRj can be deduced from Eq. (13), where the following system is obtained:
Fig. 19. Articular positions in collision situation.
703
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
8 f10 ¼ Nj f11 ¼ Nj h_ sj ; > > < f8 ¼ f15 ¼ h_ ej ; > > : f ¼ 1 de9 ¼ 1 N d ðe 9
Kj
Kj
dt
j
dt
ð23Þ 12
8 e13 ¼ Nj wj > > > > > < e12 ¼ fsj f12 ¼ fsj h_ sj ;
ð24Þ
dh_ s > > e14 ¼ J sj dfdt14 ¼ J sj dtj e16 ¼ 0 > > > : e ¼ u : 27 j
The second ARRj is then deduced by replacing the unknown variables of Eqs. (23) and (17) by their expressions in (24):
dðhsj Þ d J sj þ fsj h_ sj Nj wj þ uj þ K j ðN j h_ sj h_ ej Þ ¼ 0: dt dt ð25Þ
Once the list of possible ARRj is generated, the Fault Signature Matrix (FSM) can be built by deducing the signature of the system components on each ARRj. Note that each component in the physical system can be represented by one or more variables in the ARRsj. In addition, in the residuals’ FSM are represented instead of ARR, where each residual rkj is a numerical evaluation of an ARRkj. The corresponding residuals r 1j and r2j of ARR1j and ARR2j j 2 ½1; 2 are given by the following relations:
r 1j ¼
2 Lj fej Rej J ej Lj J sj d _ 2 ðhsj Þ þ þ kej dt kej Nj dt k ej k ej ! ! Rej J sj Lj fsj R e fe d ðh_ sj Þ þ kej þ j j h_ ej þ þ dt Nj kej Nj kej k ej
Lj J ej
þ
2
d
ðh_ ej Þ þ 2
dðh_ sj Þ d J þ fsj h_ sj Nj wj uj dt sj dt
þ K j ðN j h_ sj h_ ej Þ
þ e14 þ e16 e13 Þ;
where
ARR2j : Nj
r 2j ¼ N j
!
!
d _ ðhe Þ dt j
Rej fsj Re Rej Lej _j h_ sj U 0j 2 j wj uj u N j k ej kej kej Nj k ej N j ð26Þ
The corresponding FSM of the system under study is given in (Table 1). In this table (Table 1), the rows represent the components signatures and the columns are respectively the fault detectability Db, the fault isolability Ib, and the twelve residuals r1j and r2j . A ‘1’ value on respectively Db and Ib columns means that faults on the corresponding components are detectable and isolable. The presence of ‘1’ values on the residual columns shows the influence of the corresponding component on the residual dynamics. In this application, the FSM helps to detect and to distinguish the presence of eventual obstacles, which lock the normal operating of the actuator. 5. Experimental results In this section, experimental results have been done on the IRB140 robot while following the planned trajectory of Fig. 4. The principal of the developed co-simulation architecture is given by Fig. 14 and numerical values of the actuators parameters used for the online simulation are given in (Table 2). Fig. 15 shows the identified controls inputs of actuators models during normal operating. In this case the time domain variation of the angular joint positions and the whole residuals are represented successively by Figs. 16 and 17. It is noticed that the residuals still equal to 0 while there is no collision detection with the robot. By repeating the experiment after introducing static obstacle on the first and the second DOF, the residuals r21 and r22 derivate from zeros (Fig. 18) because it locks the normal motion of the appropriate actuated axis. Then the articular positions are given by Fig. 19. The virtual simulator (Fig. 20) shows the variation state of the residuals two residuals in collision situation.
Fig. 20. Online collision detection on the real robot from a virtual simulator.
704
K. Fawaz et al. / Mechatronics 19 (2009) 695–704
6. Conclusion In this work, a virtual simulator of an industrial robot is developed in order to make online supervision for possible collisions detection. The real and virtual systems are operating in parallel, while a model based FDI algorithm is running in real-time. This practical approach can alert the operator to predict dangerous and accidental situations without been closer to the industrial robot and without using visions feedback surveillance. The validation of the method is done after system modeling validation and online experiments. For future work, a stochastic model of the used network interlink channel will be considered in the monitoring approach, in order to consider the influence of the information delay transmission between the real robot and the simulator and to distinguish the physical faulty situation from delay information situation. References [1] Ould Bouamama B, Samantaray AK, Staroswiecki M, Dauphin-Tanguy G. Derivation of constraint relations from bond graph models for fault detection and isolation. In: International conference on bond graph modeling and simulation (ICBGM’03), Simulation series, vol. 35(2); 2003. p. 104–9. [2] Coiffet P. Modeling and control of robots. Hermès, Paris, 156 p., ISBN:978-286601-079-9 (09/1986). [3] Karnopp DC. An approach to derivative causality in bond graph models of mechanical systems. J Franklin Inst 1992;329(1):65–75. [4] Zhijiang D, Donghai Y, Lining S, Lixin F. Virtual robot simulation and monitoring system based on Java3D. In: Proceedings of the 2004 IEEE international conference on robotics and biomimetics, Shenyang, China; August 2004. [5] Magni JF, Mouyon P. On residual generation by observer and parity space approaches. IEEE Trans Autom Control 1994;39(2):441–7.
[6] Khalil W, Dombre E. Modeling, identification and control of robots. Translation from a French editor book, Hermès, Paris, ISBN:2-7462-0003-1; 1999. [7] Mitsi S, Bouzakis KD. Simulation of redundant manipulators for collision avoidance in manufacturing and assembly environments. Mech Mach Theory 1993;28(1):13–21. [8] Mitsi S, Bouzakis KD, Mansour G. Optimization of robot links motion in inverse kinematics solution considering collision avoidance and joint limits. Mech Mach Theory 1995;30(5):653–63. [9] Djeziri MA, Merzouki R, Ould-Bouamama B, Dauphin-Tanguy G. Bond graph model based for robust fault diagnosis ACC’2007, American Control Conference, New York, USA; 2007. p. 3017–22. [10] Staroswiecki M, Comtet-Varga G. Analytical redundancy relations for fault detection and isolation in algebraic dynamic systems. Automatica 2001;37:687–99. [11] Hartenberg RS, Denavit JA. A kinematic notation for loxer pair mechanisms based on matrices. J Appl Mech 1995;77:215–21. [12] Isermann R. Supervision, fault detection and fault diagnosis methods – an introduction. Control Eng Pract 1997;5(5):639–52. [13] Merzouki R, Medjaher K, Djeziri MA, Ould-Bouamama B. Backlash fault detection in mechatronics system. Mech Ifac J 2007;17:299–310. [14] Merzouki R, Cadiou JC. Estimation of backlash phenomenon in the electromechanical actuator. J Cont Eng Pract 2005;13/8:973–83. [15] Paul RCP. Robot manipulators: mathematics, programming and control. Cambridge: MIT Press; 1981. [16] Renaud M. Quasi-minimal computation of the dynamic model of a robot manipulator utilizing the Newton–Euler formalism and the notion of augmented body. In: Proceedings of the IEEE international conference on robotics and automation, Raleigh, mars-avril; 1987. p. 1677–82. [17] WebWar SDK, ABB Flexible Automation Inc., Reference User Manual. [18] Pieper DL. The kinematics of manipulators under computer control. PhD thesis, Stanford University; 1968. [19] Raghavan M, Roth B. Inverse kinematics of the general 6R manipulator and related linkages. Trans ASME J Mech Des 1990;115:502–8. [20] http://www.abb.com/product/seitp327/c1c4814d2c350882c125715d003f80e 5.aspx. [21] http://www.simpro.it/home.php.