Dissertation: a work Piece bases Approach for Programming Cooperating Industrial Robots

T
F
(5.20)
5.2.4.2 Analysis for cooperative manipulators
The dynamic equations for a set of m cooperative manipulators in the operational space
for i
= 1, ..., m could be written as follows
M
a1
(x
1
) ¨x
1
+ C
a1
(x
1
, ˙x
1
) ˙x
1
+ G
a1
(x
1
)
= τ
a1
− F
1
M
a2
(x
2
) ¨x
2
+ C
a2
(x
2
, ˙x
2
) ˙x
2
+ G
a2
(x
2
)
= τ
a2
− F
2
..
.
M
ai
(x
i
) ¨x
i
+ C
ai
(x
i
, ˙x
i
) ˙x
i
+ G
ai
(x
i
)
= τ
ai
− F
i
..
.
M
am
(x
m
) ¨x
m
+ C
am
(x
m
, ˙x
m
) ˙x
m
+ G
am
(x
m
)
= τ
am
− F
m
(5.21)
In a matrix form, the latter equations could be compounded to
M
A
¨x
A
+ C
A
˙x
A
+ G
A
= τ
A
− F
A
(5.22)
where:
x
A
= [ x
1
, . . . , x
m
]
T
∈ R
6m
M
A
= blockdiag(M
a1
. . . M
am
)
∈ R
6m
×6m
C
A
= blockdiag(C
a1
. . . C
am
)
∈ R
6m
×6m
G
A
= blockdiag(G
a1
. . . G
am
)
∈ R
6m
×6m
τ
A
= [ τ
1
, . . . , τ
m
]
T
∈ R
6m
F
A
= [ F
1
, . . . , F
m
]
T
∈ R
6m
63

---

5 Control Architecture
Equation (5.22) summarizes the collective dynamics of the manipulators but stops short
of describing the cooperative action. However, this could be completed by considering
the dynamics of the work-piece. A quick dynamic analysis thereof reveals the following
equation
M
o
(x
o
) ¨x
o
+ G
o
(x
o
)
= F
o
(5.23)
where:
M
o
(x
o
)
∈ R
6
×6
is the symmetric positive definite inertia matrix of the work-piece
G
o
(x
o
)
∈ R
6
represents the vector of gravitational forces acting on the work-piece
F
o
∈ R
6
represents the sum of all forces
/torques acting on the work-piece’s center of
gravity
O
 
F
i
r
(
O-T
1 )
F
1
F
2
W 
F
o
F
env
Figure 5.4: Force analysis on the work-piece
When more than one manipulator interact through a common object under the assumptions
mentioned in section 5.2.3.2, the external force vector on the work-piece from the i
th
manipulator are
F
o
−i
=
⎡
⎢⎢⎢⎢⎢
⎢⎢⎢⎢⎢
⎢⎢⎢⎢⎢
⎢⎢⎢⎢⎢
⎢⎢⎢⎣
I
0
S(
r
(
O−T
i
)
)
I
⎤
⎥⎥⎥⎥⎥
⎥⎥⎥⎥⎥
⎥⎥⎥⎥⎥
⎥⎥⎥⎥⎥
⎥⎥⎥⎦
F
i
(5.24)
64

---

5.3 Interaction control
and hence the resultant forces
/torques acting on the center of gravity of the work-piece
from the manipulators are equal to
F
o
=
m

i
=1
F
o
−i
(5.25)
which connects the dynamics of the manipulator (Equation (5.21)) to the dynamics of
the object (Equation (5.23)). Thereby transferring the focus of the analysis from the
operational space of the robots to the work-piece space as envisioned in Figure 3.1. It
is important to remember that all forces arising on the work-piece can not be solely
attributed to the interaction between the work-piece and the manipulators. To obtain a
comprehensive expression of the forces on the work-piece during interaction with the
environment, Equation (5.25) has to be augmented with the extra term F
env
∈ R
6
, which
represents the component of the total forces on the center of gravity of the work-piece
generated by the interaction of the work-piece with the environment as illustrated in Figure
5.4.
F
o
=
m

i
=1
F
o
−i
+ F
env
(5.26)
5.3 Interaction control
As already discussed in section 2.3 and section 2.4 di
fferent control algorithms, laws and
structures have been heavily investigated in the literature. In this section, the overarching
interaction method or control law implemented in this work will be introduced. This
method is based on force augmented impedance control similar to what was investigated
by Lopes & Almeida (2006). Initially, a fully fledged impedance controller is developed.
The most popular form for such a controller though remains the mass-damper-spring
system (MDSS) which is well studied and understood (Surdilovic & Vukobratovi´c 2002,
P. 23.14). During runtime the controller tries to achieve the target impedance by enforcing
the relationship between the manipulator and its immediate environment. This target
impedance is defined as (Kamnik et al. 1998)
f
= M
t
( ¨x
− ¨x
0
)
+ B
t
( ˙x
− ˙x
0
)
+ K
t
(x
− x
0
)
(5.27)
where:
x
0
∈ R
6
is the reference posture signal
M
t
∈ R
6
×6
is the symmetric positive definite matrix defining the target inertia
65

---

5 Control Architecture