1 1. INTRODUCERE 1.1 Stadiul actual al roboticii 1921 Karel Capek „Roboţii industriali ai lui Rossum“ 1943 Isaac Asimov
„I am a Robot“
1954 Kernward – UK
Brevet – Manipulator cu două braţe
1956 George C. Devol – USA 1959 Joseph Engelberger
Brevet – Dispozitiv de transfer automat Cumpără brevetul de la Devol şi realizează în
anul 1960 robotul Unimate la firma Unimation Inc., firm ă ce va lucra în profit de abia din anul 1973. 1968 38 roboţi Unimate la linia de sudare a caroseriilor din Lordstown 1968 Kawasaki Heawy Industries a cumpărat licenţa Unimate 1971 Primul robot japonez la Nissan Motors 1971 Fiat – Italia / Torino 1971 ASEA – Suedia
Linia de sudat în puncte caroserii
Primul robot cu acţionare electrică
1974 RV – 70 Regie Renault / Franţa robot cu acţionare electrică 1976 Prof. Chr. Pelecudi – UP Bucureşti / „Introducere în robotică” primul articol de robotică apărut în România – MTM 76 Timişoara / Reşiţa 1979 „Colectiv Multidisciplinar de Roboţi Industriali” la Institutul Politehnic „Traian Vuia” Timişoara. 1982, 8 februarie REMT – 1 Primul robot industrial românesc, Institutul Politehnic „Traian Vuia” Timi şoara şi Electromotor Timişoara Distins cu Premiul „TRAIAN VUIA“ al Academiei Române
2 1982 Kovacs Fr. Cojocaru G – “Manipulatoare, Roboţi şi aplicaţiile lor prima carte de robotică din România.
industriale”
1983 RIP 6,3 – Automatica Bucureşti 1985 RIC – 25,
RIS – 63
Implementat la Autobuzul Bucureşti
ICSIT TITAN Bucureşti în cadrul Programului
Naţional de Roboţi Industriali 1986 Institutul Politehnic „Traian Vuia” Timi şoara şi Electro-Timiş Timişoara Familie de Roboţi cu: Acţionare electrică
mR 2,5
MRE 12,5
Acţionare pneumatică
MRP 12,5
MAP 100
Acţionare hidraulică
RH 1kN
MAE 100
1987 Institutul Politehnic „Traian Vuia” Timişoara şi IMMUM Baia Mare Manipulator sincron – MASINh 500
Expus la TIB´87 şi distins
cu Medalia de aur 1988 Institutul Politehnic „Traian Vuia” Timişoara şi IMMUM Baia Mare Robotul Portal – ROPOS 50 Medalia de aur
Expus la TIB´88 şi distins cu
3 1.2 Tendinţe de dezvoltare ale roboticii -
creşterea gradului de autonomie ale roboţilor
-
scăderea gradului de structurare al mediului
a) ROBOŢI INDUSTRIALI
1960 –
-
mediu structurat
-
obiecte situate în puncte de precizie ale mediului
-
sarcini predefinite prin program
-
executarea automată de programe explicite
b) ROBOŢI de PRESTĂRI SERVICII
1984 –
-
model al universului bazat pe prelevate din mediu
-
percepţie artificială
-
procesare informaţională multisenzorială
-
programe implicite
-
planificarea traiectoriei (traiectorie dinamic ă)
b) ROBOŢI PERSONALI
2003 –
-
capacităţi de comunicare cu mediul
-
utilizare de modele pentru înţelegerea mediului
-
planificarea automat ă a întregului program
-
funcţii de supraveghere a întregii activităţi Robot staţionar – Manipulare Vehicul ghidat automat (VGA) – Deplasare prin rulare Maşini păşitoare – Deplasare prin păşire
4 Manipulare Robot prestări servicii staţionar
Robot industrial avansat
Robot Industrial
Interacţiune om maşină
Senzori
Roboţi de Servicii
Deplasare pe roţi Vehicule Ghidate Automat
Roboţi de Serviciu Mobili
Vehicule Ghidate Automat Avansate
Telerobot
Reţea Internet Senzori
Stabilitate
Deplasare prin păşire Mecanism de păşire
Maşină
Maşină
Păşire multiped
P ăşire biped
Robot Humanoid
Fig. 1.1 Dezvoltarea Roboticii 1. ROBOŢI INDUSTRIALI: •
Robot Industrial (Manipulare) + Senzori = Asamblare automată;
•
Roboţi inteligenţi staţionari; Pe viitor Robo ţi de dezasamblare
2. VEHICULE GHIDATE AUTOMAT: •
Fără elemente de inteligenţă artificială = Roboţi Mobili pentru Transportul
ă în mediu structurat; Materialelor pe o traiectorie ghidat •
cu Senzori (Video, de Proximitate)şiî aleg traiectoria într-un mediu nestructurat.
3. MAŞINI PĂŞITOARE multi-pedşi biped
5
ELECTRONICĂ
MECANICĂ
CALCULATOARE
MECATRONICĂ şi ROBOTICĂ
Fig. 1.2 Mecatronica şi Robotica
6 2. NOŢIUNI DE TEORIA SISTEMELOR 2.1 Clasificarea sistemelor
Sistem: ansamblu de elemente şi relaţiile (legăturile) dintre ele.
Mediu: ansamblul elementelor care nu fac parte din sistem.
Legături orientate: Intrări (INPUT) sau Ieşiri (OUTPUT)
Legături neorientate: perturbaţii („zgomote”).
Sistem închis: nu are legături cu mediul său.
Sistem deschis:are legături orientate sau neorientate cu mediul ău.s
Sistem simplu: are puţine elemente şi legături între acestea.
Sistem complex: are multe elemente şi legături între ele.
Sistem concret: are elemente materiale.
Sistem abstract: nu are elemente materiale.
Sistem natural: există fără intervenţia omului.
Sistem artificial: este creat de om.
Sistem determinist: are legături orientate.
Sistem stohastic: are legături neorientate. z1 Perturbaţii („zgomote“)
z2
zk
.
x1 Intrări Input
x2 xm
.
MEDIU
.
y1
(S) . . .
T
Fig. 2.1 Sistem deschis
. . .
y2 yn
Ieşiri Output
7 2.2 Funcţia de transfer T
Funcţia de transfer: asigură transformarea mărimii de intrare X = X (t) în mărimi de ieşire Y = Y (t) conform relaţiei: .
Y=T X
(2.1)
Vectorul intrărilor
X = X (x1, x2, x3, . . . , xm)
Vectorul ieşirilor
Y = Y (y1, y2, y3, . . . , yn)
Matricea de transfer
T
T
T
Vectorul perturbaţiilor
T
Z = Z (z1, z2, z3, . . . , zk)
MEDIU
Z
(S)
T
X
Y
TR ΔY
ΔY (SR)
TR
Fig. 2.2 Sistem cu buclă de reacţie
Z – caracter aleatoriu Y’ – Vectorul ieşirilor perturbate Y’ = T ( X + Z )
(2.2)
8 Bucla de reacţie (SR) asigură eliminarea perturbaţiilor. Vectorul erorilor = intrarea în bucla de reacţie ΔY = Y’ – Y
(2.3)
.
TR ΔY – ieşirea din bucla de reacţie TR – funcţia de transfer a buclei de reacţie .
Y = T ( X + Z + TR ΔY )
(2.4)
Ierarhizarea sistemelor Sistemulde rang „R“ conţine unul sau mai multe sisteme de rang „R-1“ . S
R-1
∈
S
R
S
R
∈
S
R+1
(2.5)
Sistemul de rang „R“ este un element al unui sistem de rang „R+1“ . Ierarhizarea sistemelor = cunoaşterea structurii unui sistem şi a legăturilor dintre subsistemele acestuia. 2.3 Cuplarea a două sisteme
Două sisteme sunt cuplate dacă cel puţin o ieşire a unuia este o intrare în cel ălalt. (S1)
(S2) Y
(1)
X
(2)
Fig. 2.3 Două sisteme cuplate X
(2)
T
= [x21, x22, x23, . . . , x2m]
(1)
Y
(2.6)
T
= [y11, y12, y13, . . . , y1n]
x2p = y1q
p = 1...m q = 1...n
(2.7)
Matricea de cuplare= Matrice „m x n“ cu „m“ intr ări in S2 şi „n“ ieşiri din S1. K 12 = [ e pq ]
p = 1...m q = 1...n
(2.8)
9
⎡ e11 e12 ⎢e e 22 ⎢ 21 ⎢ . . ⎢ . . K12 = ⎢ ⎢ e p1 e p2 ⎢ . ⎢ . ⎢ . . ⎢ ⎣⎢e m1 e m2
...
e1q
...
...
e 2q
...
...
e1n ⎤
e 2n ⎥
⎥
.
. ⎥
.
. ⎥
e pq
⎥
...
⎥
.
. ⎥ . ⎥
. ...
e mq
(2.9)
e pn ⎥
...
e mn
⎥ ⎦⎥
⎧⎪1 ∀ x 2 p = y1q p = 1, m e pq = ⎨ ⎪⎩0 ∀ x 2 p ≠ y1q q = 1, n epq = 1
există o legătură între S1 şi S2
epq = 0
nu există o legătură între S1 şi S2
(2.10)
Modelul matematic al cuplării sistemelor S1 şi S2 X
(2)
.
= K12 Y
(2.11)
m=5
EXEMPLUL 2.1 x11
x12
n=3 K 12 = [ e pq ]5 x 3
(S1) y11
(1)
X Y
(1)
(2)
= [1 0 1 0 0]
= [1 1 0]
y13
x24 x22 x21
x23
y21
(S2)
y22
Fig. 2.4 Cuplarea a două sisteme
T
⇒
e11 = 1
x23 = y13 ⇒
e32 = 1
x21 = y11
y12
x25
T
⎡1 ⎢0 ⎢ K12 = ⎢0 ⎢ ⎢0 ⎢⎣0
0
0⎤
0
0⎥
1
0⎥
0 0
0⎥ 0⎥⎦
⎥ ⎥
10 Matricea de structură = matricea unui sistem de rang „R“ care conţine "Si" subsistem i = 1...n.
⎡ 0 ⎢K ⎢ 21 S = ⎢ K 31 ⎢ ⎢ .
K13
.
0
K 23
.
K 2n ⎥
K 32
0
.
K 3n ⎥
.
. .
0 ⎦⎥
⎣⎢K n1 K n 2 K ij – matricele de cuplare ale sistemelor "S i" Umărul maxim de cuplări
K1n ⎤
K12
K n3
⎥ (2.12)
⎥ . ⎥
K ii = 0
Nmax = n (n-1)
(2.13) .
EXEMPLUL 2.2
n = 3 Nmax = 3 2 = 6
x11
x12
(S1)
y11
y12
⎡ 0 K 12 S = ⎢ K 21 0 ⎢ ⎢⎣ 0 0
K 23 ⎥
K 31 = 0
K 32 = 0
x21
⎥
0 ⎥⎦
x31
(S2)
y21
K13 ⎤
y22
Fig. 2.5 Matricea de structură
(S3)
x32
y31
11 3. MODULUL MATEMATIC AL MANIPULĂRII
Celulă de fabricaţie = Mediu Industrial (MI) + Robot Industrial (RI) + Dispozitiv de Alimentare / Evacuare + Ma şină - Unealtă + Dispozitiv de lucru (DL) + Obiect (OB) + Sculă (SC) + Efector Final (EF)
EF
OB
SC
RI MI
DL
Fig. 3.1 Modelul matematic al manipulării
Manipulare = Corelarea elementelor lanţului tehnologic de către sistemul de comandă prin programe elaborate pe baza algoritmelor de calcul‚ transformări succesive de coordonate între diferite sisteme de referin ţă.
12 Zj Yj Oj j
Sj
i
rj
Xj
( Xi, Yi, Zi ) ( Xj, Yj, Zj )
P
P ∈ (S j )
Zi i
rP
rP
Si Oi Yi
Xi
Fig. 3.2 Transformări de coordonate între dou ă sisteme de referinţă i
i
j
rp = r j + rp
ir
p
= i r j + Bij ⋅ j r p
(3.1)
Vectorul de poziţie al punctului P în sistemul solidului „Si“ Vectorul de poziţie al punctului P în sistemul solidului „Sj“
i
T
r p = (x i y i z i ) j
i
Vectorul de poziţie al srcinii O j in raport cu Oi
⎧⎪x i ⎫⎪ ⎧⎪r0 x ⎫⎪ ⎡⎢ e11 e12 ⎨ y i ⎬ = ⎨r0 y ⎬ + ⎢e 21 e 22 ⎪ z ⎪ ⎪ r ⎪ ⎢e ⎩ i ⎭ ⎩ 0 z ⎭ ⎣ 31 e 32 i
T
i
j
rp = T j ⋅ rp
j e13 ⎤ ⎧ ⎪x ⎫⎪ e 23 ⎥ ⋅ ⎨ y j ⎬
⎥ ⎪ e 33 ⎥⎦ ⎪ ⎩z j ⎭
(3.2)
r j = (r0 x r0 y r0 z )
Bij = e ij
Matricea cosinusurilor directoare
Cu quadri-vector
r p = (x j y j z j )
T
i = j = 1,3
(3.1’)
(3.3)
13 i
j
T
r p = (x i y i z i 1)
r p = (x j y j z j 1)
T
⎧x i ⎫ ⎡ a 11 a 12 ⎪ y ⎪ ⎢a ⎪ i ⎪ ⎢ 21 a 22 ⎨ ⎬= ⎪ z i ⎪ ⎢a 31 a 32 ⎪⎩ 1 ⎪⎭ ⎢⎣ 0 0
T ij = [a kl ] k = l = 1,3
(3.4)
a 14 ⎤ ⎧x j ⎫
a 13
a 24 ⎥ ⎪ y ⎪ ⎥ ⋅ ⎪⎨ j ⎪⎬ a 34 ⎥ ⎪ z j ⎪
a 23 a 33
(3. 3’)
⎥ 1 ⎦ ⎪ ⎩ 1 ⎪⎭
0
3.1 Matricea de transformare omogenă Exprimă situarea relativă a solidului Sj în raport cu solidul Si. i
i
R j = Matricea de rotaţie r j = Matricea de translaţie
⎡ e11 e12 ⎢e e 22 i 21 Tj = ⎢ ⎢e 31 e 32 ⎢ 0 ⎣0
r0 x ⎤
e13
r0 y ⎥
⎡i ⎥ =⎢ Rj r0 z ⎥ ⎢ O T ⎥ ⎣ 1⎦
e 23 e 33 0
Zj
0
Oi
i
ri
rP
jr
j
P
P
Xj
rj
0
Z0
(3. 5)
i
r
0
⎥
1 ⎥⎦
Oj
Yi
Xi
r j⎤
Sj
Zi
Si
i
rP
S0 O0
Y0
X0
Fig. 3.3 Transformări de coordonate între trei sisteme de referin ţă
14 0
0
0
Relaţia de recurenţă
MI
MI
i
j
i
j
0
r p = T i ⋅ i Tj ⋅ r p
T SC =
T SC =
T RI ⋅
(3.6)
rp = T j ⋅ rp
0
MI
T SC =
0
r p = Ti ⋅ r p
T j = T i ⋅ i Tj
MI
MI
j
rp = T j ⋅ rp
0
i
0
MI
MI
RI
(3.8)
i=j-1 j=1..6
T RI ⋅
RI
T DL ⋅
DL
TEF ⋅
(3.7)
EF
TEF ⋅
EF
TOB ⋅
TSC =
MI
TSC
OB
(3.9)
TSC
T DL ⋅
DL
TOB ⋅
OB
TSC
(3.10)
T RI = depinde de amplasarea robotului în hală
RI
TEF = funcţie de coordonatele generalizate EF
M
TSC = depinde de construcţia efectorului final
T DL = depinde de amplasarea DL (MU) în hală
DL
TOB = depinde de situarea OB în DL
OB
TSC = se citeşte din desenul de execuţie al OB RI
TEF =
MI
T RI
−1 MI
⋅ T DL ⋅
DL
TOB ⋅
OB
TSC ⋅
EF
TSC
−1
(3.11)
Conform cerinţelor tehnologice = situarea sculei în raport cu obiectul ⇒ Situarea efectorului final în raport cu robotul
OB
TSC ⇒
R
TEF
prin modificarea
coordonatelor generalizate, qi , i=1...6; Pentru o cuplă de rotaţie qi = θi;
Pentru o cuplă de translaţie qi = di.
15 3.1.1 Matricele de transformare omogene elementare
Z0 Zi
θi
ki i0
θi
k0
Yi ji
θi
j0 ii O0 = Oi
Y0
X0 = Xi
Fig. 3.4 Cuplă de rotaţie
0 ⎡1 ⎢0 cos θ i Rot ( x i , θ i ) = ⎢ ⎢0 sin θi ⎢ 0 ⎣0
⎡ cos θi ⎢ 0 Rot ( y i , θ i ) = ⎢ ⎢− sin θi ⎢ ⎣ 0
0⎤
0
− sin θ i cos θ i 0
0⎥ ⎥ 0⎥ ⎥ 1⎦
⎡cos θi ⎢ sin θ i Rot ( z i , θi ) = ⎢ ⎢ 0 ⎢ ⎣ 0
− sin θi cos θ i
0 0
0⎥
0
1
0⎥
0
0
1⎦
⎥ ⎥
Zi
Oi
ii
ji
c Xi k0
O0 a
j0
sin θ i
1
0
0⎥
0
cos θ i
0⎥
0
0
1⎦
0⎤
⎥ ⎥
0⎤
Z0 ki
0
Yi
b
i0
X0
Fig. 3.5 Cuplă de translaţie
Y0
(3.12)
16
⎡1 ⎢0 Trans( x i , a ) = ⎢ ⎢0 ⎢ ⎣0 ⎡1 ⎢0 Trans( y i , b) = ⎢ ⎢0 ⎢⎣0
0
0
1
0
0⎤
a⎤
0
0
1
0
0⎥
0
1
0⎥
0
0
1⎦
⎥
(3.13)
⎥
⎡1 ⎢0 Trans(z i , c) = ⎢ ⎢0 ⎢⎣0
b⎥
⎥ 0 1 0⎥ 0 0 1 ⎥⎦
0⎤
0
0
1
0
0⎥
0
1
c⎥
0
0
1 ⎥⎦
⎥
3.2 Matricea de situare
EF
n
δ'
6 z0
1
x0
O0
M
p
RI
o
a
OB
δ
y0
Fig. 3.6 Matricea de situare
n = {n x
ny
nz}
Versor de orientare
o = {o x
oy
oz }
Versor de apropiere
a = {a x
ay
az}
Vectorul de poziţie
p = {p x
py
pz }
Versor normal
T
T
T
T
(3.14)
17 0
0
0
0
T 6 = T 5 ⋅ 5 T6 0
0
T j = T i ⋅ Tj i
0
T 5 = T 4 ⋅ 4 T5
0
i=j-1 j=1..6 0
(3.15) 0
0
T 4 = T 3 ⋅ 3 T4 . . . T 2 = T1 ⋅ 1T2 6
T 6 = T1 ⋅ 1T2 ⋅ 2 T3 ⋅ 3 T4 ⋅ 4 T5 ⋅ 5 T6 = ∏ 0
i −1
Ti
(3.16)
1
⎡n o a p ⎤
R
Matricea de situare Ecuaţia matriceală a manipulării
T EF
= ⎢⎣ 0 0 0 1 ⎥⎦ R
0
T EF = T 6
(3.17) (3.18)
18 4. MODELUL GEOMETRIC AL SITUĂRII OBIECTULUI MANIPULAT 4.1 Modelul geometric direct al situării obiectului manipulat 4.1.1 Modelul Denavit - Hartenberg
zi-1
zi
i zi-2
i+1
i
i-1
i+1
i-1 ai i-2
αi
O’i-1 xi-1
di Oi-1
Oi
xi
θi
Oi-2 Fig. 4.1 Notaţiile utilizate de modelul Denavit - Hartenberg
Metoda Denavit - Hartenberg s-a introdus în 1955. Este cea mai răspândită metoda de modelare geometrică a roboţilor. Avantajul metodei constă în numărul redus de parametri necesari trecerii de la un sistem de referinţă la altul (6
4).
Algoritmul este următorul: 1) Se notează elementele pornind de la baza robotului (elementul 0) şi terminând cu efectorul final. Numărul cuplei cinematice este dat elementul cu cifră mai mare din componenţa ei. 2) Axa z i-1 este axa cuplei cinematice„i”, care leagă elementul„i-1” de elementul„i”. 3) Axa x i-1 este perpendiculara comună a axelor z i-1 şi z i-2 şi este orientată de la Oi-2 la Oi-1. Originea sistemului de referinţă se alege în punctul de intersecţie al
perpendicularei comune cu axele cuplelor cinematice.
19 Sensul pozitiv fiind de la indicele mai mic la cel cu indice mai mare. 4) Axele Y sunt definite de produsul vectorial Y = Z × X . 5) Se construieşte un tabel cu parametri: θi , di , ai , αi Trecerea de la sistemul de referinţă Oi-1 la cel cu srcinea în Oi se face prin 4 mişcări elementare:
⇒
o rotaţie cu unghiul θi în jurul axei z i-1 , de suprapunere a axeixi-1 pe
direcţia axei xi, sensul pozitiv fiind de la xi-1 la x i.
⇒
o translaţie cu di de-a lungul axei z i-1 ce
aduce srcinea Oi-1 în O’i-1,
sensul pozitiv de la Oi-1 în O’i-1.
⇒
o translaţie cu ai de-a lungul axei xi suprapunând srcinea O’i-1cu Oi,
sensul pozitiv de la O’i-1 la Oi.
⇒
o rotaţie cu unghiul αi în jurul axei xi , de suprapunere a axeizi-1 pe
axa zi, pozitiv în sensul trigonometric. Matricea de transformare de la elementul „i” la elementul „i-1” este de forma: i-1
Ti = Rot (zi-1 , θi) . Trans (zi-1 , di) . Trans (xi , ai) . Rot (xi , αi)
⎡cos θi − sin θi ⎢ sin θ cos θ i −1 i i Ti = ⎢ ⎢ 0 0 ⎢ 0 ⎣ 0
(4.1)
0 0⎤ ⎡1 0 0 0 ⎤ ⎡1 0 0 a i ⎤ ⎡1 0 0 0⎤ 0 0⎥⎥ ⎢⎢0 1 0 0 ⎥⎥ ⎢⎢0 1 0 0 ⎥⎥ ⎢⎢0 cos α i − sin α i 0⎥⎥ (4.2) ⋅ ⋅ ⋅ 1 0⎥ ⎢0 0 1 d i ⎥ ⎢0 0 1 0 ⎥ ⎢0 sin α i cos α i 0⎥ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ 0 1 ⎦ ⎣0 0 0 1 ⎦ ⎣0 0 0 1 ⎦ ⎣0
0
⎡cos θi − sin θi ⋅ cos α i sin θi ⋅ sin α i a i ⋅ cos θi ⎤ ⎢ sin θ cos θ ⋅ cos α − cos θ ⋅ sin α a ⋅ sin θ ⎥ i −1 i i i i i i i⎥ Ti = ⎢ ⎢ 0 sin α i cos α i di ⎥ ⎢ 0 ⎥ 0 0 1 ⎦ ⎣
0
1⎦
(4.2’)
20 Deci rezolvarea modelului geometric direct necesită cunoaşterea unor dimensiuni (liniare şi unghiulare):
Pentru o cuplă de rotaţie:
di , ai , αi sunt constante şi θi = θi (t)
Pentru o cuplă de translaţie:
θi , ai , αi sunt constante şi di = di (t)
Tabelul Denavit-Hartenberg
θi
di
ai
αi
0-1
θ1 θ2
2-3
θ3
d1 d2 d3
a1 a2 a3
α1
1-2
α2 α3
Revenind la ecuaţia matriceală: R
0
0
T EF = T 6
unde
6
T6 = ∏ 1
i −1
Ti
(4.3)
⎡n x o x a x p x ⎤ ⎡ f11 f12 f13 f14 ⎤ ⎢ n o a p ⎥ ⎢f ⎥ ⎢ y y y y ⎥ = ⎢ 21 f 22 f 23 f 24 ⎥ ⎢ n z o z a z p z ⎥ ⎢f 31 f 32 f 33 f 34 ⎥ ⎢0 0 0 1⎥ ⎢0 0 0 1⎥ ⎣ ⎦ ⎣ ⎦
(4.4)
f ij = f ij (θi d i a i α i )
(4.5)
Pentru convenţia Denavit - Hartenberg, soluţia problemei geometrice directe constă în egalarea elementelor matricelor din relaţia 4.4: n x = f11 o x = f12 a x = f13 p x = f14 n y = f 21 o y = f 22 a y = f 23 p y = f 24 n z = f 31 o z = f 32 a z = f 33 p z = f 34
(4.6)
21 EXEMPLUL 4.1
d3
α2
2
O2
z0
M=O3
1
d2
x3
3
M
1
d3
z2
2
O’1
3
d2
a2
z1
C
B
A
x2
z3
θ1
x1
0
b)
a)
θ1
O0=O1
x0 Fig. 4.2 Modelul geometric direct - Structura RTT: a) schema cinematică b) schema asociată
θi θ1 0 0
0–1 1–2 2–3
di
ai
0
0
d2
a2
d3
0
αi 0 +900 0
⎡cos θi − sin θi ⋅ cos α i sin θi ⋅ sin α i a i ⋅ cos θi ⎤ ⎢ sin θ cos θ ⋅ cos α − cos θ ⋅ sin α a ⋅ sin θ ⎥ i −1 i i i i i i i⎥ Ti = ⎢ ⎢ 0 sin α i cos α i di ⎥ ⎢ 0 0 0 1 ⎥⎦ ⎣ ⎡cos θ1 − sin θ1 ⎢ sin θ cos θ 0 1 1 T1 = ⎢ ⎢ 0 0 ⎢ 0 0 ⎣
0 0 1 0
0⎤ 0⎥⎥ 0⎥ 1⎥⎦
⎡1 ⎢0 1 T2 = ⎢ ⎢0 ⎢0 ⎣
0 0 a2 ⎤ 0 − 1 0 ⎥⎥ 1 1 d2 ⎥ 0 0 1 ⎥⎦
⎡1 ⎢0 2 T3 = ⎢ ⎢0 ⎢0 ⎣
0 T3 0 T1 1 T2 2 T3
=
⎡cos θ1 ⎢ sin θ 0 1 T3 = ⎢ ⎢ 0 ⎢ 0 ⎣
⋅
⋅
0 sin θ1 a 2 ⋅ cos θ1 + d 3 ⋅ sin θ1 ⎤ 0 − cos θ1 a 2 ⋅ sin θ1 − d 3 ⋅ cos θ1 ⎥⎥ ⎥ 1 0 d2 ⎥ 0 0 1 ⎦
(4.2’)
0 1 0 0
0 0⎤ 0 0 ⎥⎥ 1 d3 ⎥ 0 1 ⎥⎦
22 4.1.2 Modelul Craig
zi i
i-1
zi-1 i-1
i
zR
zP
αi-1
i-2
Oi-1=R
xi
zQ
xR
ai-1
di
xi-1
Oi
Q=P
xP θi
xQ
Fig. 4.3 Notaţiile utilizate de modelul Craig
Particularitatea modelului Craig constă în alegerea axei z i în axa cuplei cinematice „i”, care leagă elementul„i-1” de elementul„i”. Transformarea omogen ă este o
funcţie de 4 parametri dintre care una este variabilă iar celelaltetrei sunt constante.De fapt putem afirma că fiecare parametru exprim ă o matrice elementar ă motiv pentru care se mai ataşează 3 sisteme de referin ţă intermediareR, Q şi P. Strategia de trecere de la sistemul legat de elementul„i-1” la elementul„i” constă în:
⇒ Trecerea de la sistemul de referinţă
„i-1” la sistemul de referinţă „R”, care
are loc printr-o rotaţie cu unghiul αi-1 în jurul axei xi-1 de suprapunere a axei zi-1 pe axa zR, pozitiv în sensul trigonometric;
⇒ Trecerea de la sistemul de referinţă „R” la sistemul de referinţă „Q” care are loc printr-o translaţie cu ai-1 de-a lungul axei xi-1 suprapunând srcinea „R” cu „Q”, sensul pozitiv de la „R” la „Q”.
23 ⇒ Trecerea de la sistemul de referinţă „Q” la sistemul de referinţă „P” care are loc printr-o rotaţie cu unghiul θi în jurul axei zi de suprapunere a axei xQ pe axa direcţia axei xP, pozitiv în sensul trigonometric;
⇒ Trecerea de la sistemul de referinţă
„P” la sistemul de referinţă „i” care are
loc printr-o translaţie cu di de-a lungul axei zi ce aduce srcinea „P” în „Oi”, sensul pozitiv de la „P” în „Oi”.
Conform relaţiei de recurenţă: i −1
T i = i −1 T R ⋅ R TQ ⋅ Q TP ⋅ P Ti
(4.7)
sau matricea de transformare omogenă de la elementul „i-1” la elementul„i” este: i-1
Ti = Rot (xi-1 , αi-1) . Trans (xi-1 , ai-1) . Rot (zi , θi) . Trans (zi , di)
respectiv
i-1
Ti = Elice (xi-1 , αi-1 , ai-1) . Elice (zi , θi , di)
(4.8)
Utilizând matricele elementare se obţine: 0 0 ⎡1 ⎢0 cos α − α sin i −1T = ⎢ i ⎢0 sin α i −1 cos α i −1 i −1 i −1 ⎢ 0 0 ⎣0
0⎤ ⎡1 0⎥⎥ ⎢⎢0 ⋅ 0 ⎥ ⎢0 ⎥ ⎢ 1 ⎦ ⎣0
0 1 0 0
0 a i −1 ⎤ ⎡cos θi − sin θi 0 0 ⎥⎥ ⎢⎢ sin θi cos θi ⋅ 1 0 ⎥ ⎢ 0 0 ⎥ ⎢ 0 1 ⎦ ⎣ 0 0
0 0 1 0
0⎤ ⎡1 0⎥⎥ ⎢⎢0 ⋅ 0 ⎥ ⎢0 ⎥ ⎢ 1 ⎦ ⎣0
0 1 0 0
0 0⎤ 0 0 ⎥⎥ (4.9) 1 di ⎥ ⎥ 0 1⎦
cos θ i sin θi 0 a i −1 ⎡ ⎤ ⎢sin θ ⋅ cos α ⎥ cos θ ⋅ cos α − sin α − d ⋅ sin α i −1 i i −1 i i −1 i −1 i i −1 ⎥ Ti = ⎢ ⎢ sin θi ⋅ sin α i −1 cos θi ⋅ sin α i −1 cos α i −1 d i ⋅ cos α i −1 ⎥ ⎢ ⎥ 0 0 0 1 ⎣ ⎦
(4.10)
Modelul Craig elimină confuzia datorată utilizării indicelui „i” pentru axa „i-1” ca în cazul convenţiei Denavit – Hartenberg, dar păstrează problema singularităţii de model datorat cazului axelor paralele. Transformarea omogenă după modelul Craig poate fi privită ca o compunere de două mişcări elicoidale.
24 4.1.3 Modelul Hayati
zi
z'i-1 βi
zi-1
i+1
yi
i θi
i
zi-1
αi
y'i-1
βi
i+1
xi
αi
y'i-1
i-1
αi
yi-1 Oi-1
βi
ai
x'i-1
Oi
θi
xi-1 Fig. 4.4 Notaţiile utilizate de modelul Hayati
Pe baza modelului Denavit – Hartenberg, modelul Hayati încearcă eliminarea singularităţii de model pentru cazul axelor paralele succesive, prin înlocuirea distanţei „di”, dintre srcinile sistemelor „i” şi „i-1” (modelele Denavit–Hartenberg şi Craig),
cu unghiul de aliniere „βi”, al axei „yi” cu axa „yi-1”. Utilizând acest model singularitatea de axe paralele este înlocuită cu singularitatea de axe perpendiculare, problemele menţinându-se. i-1
Ti = Rot (zi-1 , θi) . Trans (xi-1 , ai) . Rot (xi -1, αi) . Rot (yi , βi)
(4.11)
25 4.1.4 Modelul Hsu - Everett
zi
z'i-1 βi
αi
i+1
zi-1
yi
i
y'i-1
βi
θi
i
zi-1
i+1
xi
αi βi
i-1
ai O'i-1
y'i-1
di
αi
x'i-1
Oi
yi-1
Oi-1
x'i-1 θi
xi-1 Fig. 4.5 Notaţiile utilizate de modelul Hsu - Everett
Pentru a elimina problemele apărute în urma modificării făcute de Hayati, Hsu şi Everett menţin această modificare
(unghiul de aliniere „βi”), dar reintroduc
parametrul „di”, i-1
Ti = Rot (zi-1 , θi) . Trans (zi-1 , di) . Trans (xi-1 , ai) . Rot (xi-1, αi) . Rot (yi , βi) (4.12)
Calibrarea utilizând acest model, precum şi studiul sensibilităţii de model referitor la liniaritatea operatorului au condus la rezultate remarcabile privind insensibilitatea datorată erorilor de execuţie şi de montaj. Numărul parametrilor variabili rămâne acelaşi, fiind vorba doar de unul singur pentru o cuplă cinematică conducătoare. Chiar dacă iniţial volumul de calcul este mai mare, simplificările de model pentru studiul dinamicii pot justifica folosirea a 5 parametrii.
26 4.1.5 Modelul Kovács - Stone
z2’
B
2
O2’= O2’’
x2
O2
d3
x3
z2’’
M
1
x2’
z0
3
d2
a2’
z1
C
x2’’
α2’’
z2
d3
M=O3 z3 d2
θ1
x1
A 0
b)
a) O0=O1
θ1
x0
Fig. 4.6 Notaţiile utilizate de modelul Kovács pentru o structura de robot RTT a) schema cinematică b) schema asociată
Modelul Kovács constă în ataşarea unui contur poligonal deschis dispozitivului de ghidare, ataşându-se câte un sistem de referinţă fiecărui laturi a conturului şi elementelor în mişcare relativă. Convenţia este următoarea: ⇒ Axele z se aleg ca axe ale cuplelor cinematice de rotaţie sau de translaţie;
⇒ Offseturile se aleg ca axex; ⇒ Axele x se aleg paralele între ele. 0
0
T 3 = 0 T1 ⋅ 1T2 ⋅ 2 T2' ⋅ 2' T2'' ⋅ 2'' T3
(4.13)
T 3 = Rot (z0 , θ1) . Trans (z1 , d2) . Trans (x2’ , a2’) . Rot (x2’’ , α2’’) . Trans (z2’’ , d3) (4.14)
⎡cos θ1 − sin θ1 ⎢ sin θ cos θ 0 1 1 T3 = ⎢ 0 ⎢ 0 0 ⎢⎣ 0
0 0 1 0
0 ⎤ ⎡1 0 ⎥ ⎢0 ⎥⋅⎢ 0 ⎥ ⎢0 1 ⎥⎦ ⎢⎣0
0 1 0 0
0 0 ⎤ ⎡1 0 0 ⎥ ⎢0 ⎥⋅⎢ 1 d 2 ⎥ ⎢0 0 1 ⎥⎦ ⎢⎣0
0 1 0 0
0 a 2 ⎤ ⎡1 0 0 0 0 ⎥ ⎢0 0 − 1 ⎥⋅⎢ 1 0 ⎥ ⎢0 1 0 0 1 ⎥⎦ ⎢⎣0 0 0
0 ⎤ ⎡1 0 ⎥ ⎢0 ⎥⋅⎢ 0 ⎥ ⎢0 1 ⎥⎦ ⎢⎣0
0 1 0 0
0 0⎤ 0 0⎥ ⎥ 1 d3 ⎥ 0 1 ⎥⎦
(4.15)
27
⎡cos θ1 ⎢ sin θ 0 1 T3 = ⎢ ⎢ 0 ⎢ 0 ⎣
0 sin θ1 d 3 sin θ1 + a 2 cos θ1 ⎤ 0 − cos θ1 − d 3 cos θ1 + a 2 sin θ1 ⎥⎥ ⎥ 1 0 d2 ⎥ 0 0 1 ⎦
(4.16)
Modelul STONE (Arm Signature Model, S – Model) este analog cu cel prezentat anterior, utilizând însă alte notaţii. Situarea (orientarea + poziţionarea) sunt calculate asemănător modelului Denavit – Hartenberg. Sn = B1 . B2 .
. . .
.
Bn
(4.17)
Matricele Bi descriu transformările elementare relative dintre sistemele Si-1 şi Si având ca referinţă Si-1, fiind definită prin: i-1
B i = Rot (z, γi) . Trans (0, 0, d i) . Trans (ai, 0, 0) . Rot (x, αi) . Trans (0, 0, b i) (4.18)
28 4.1.6 Modelul Kovács utilizând noţiunea PeSiR
SS
PeSiR = Perechi de Sisteme de Referinţă
M’ M
SM
N'
rM N'
M
p
N’ N r N'
p P
Np
N
SN
O’
SO
O
Fig. 4.7 Notaţiile utilizate de modelul PeSiR
Elementele N şi N’ în contact materializează o cuplă cinematică generalizată. Sistemele de referinţă N şi N’ ataşate constituie Perechea de Sisteme de Referinţă, PeSiR N - N’. Situarea relativă a două sisteme de referinţă N şi N’, se exprimă prin matricea de situare:
⎡1 0 0 0 ⎤ ⎢r n o a ⎥ N x x x⎥ (4.19) SN' = ⎢ x ⎢ry n y o y a y ⎥ ⎢ ⎥ z z z z ⎣r n o a ⎦ Situarea relativă a sistemului de referinţă N’ faţă de sistemul de referinţă N se poate exprima şi prin intermediul matricei de trecere de la N’ la N: N
S N’ = N T N’
(4.20)
29 unde: N
T N’ = Trans (x, q1) . Trans (y, q2) . Trans (z, q3) . Rot (x, q4) . Rot (y, q5) . Rot (z, q6) (4.21)
0 0 o ⎡1 ⎤ ⎢q c ⋅ c s ⋅ s ⋅ c − c s c ⋅ s ⋅ c + s s ⎥ N T N' = ⎢ 1 5 6 4 5 6 4 6 4 5 6 4 6 ⎥ ⎢ q 2 c5 ⋅ s 6 s 4 ⋅ s 5 ⋅ s 6 + c 4 c 6 c 4 ⋅ s 5 ⋅ s 6 − s 4c 6 ⎥ ⎢⎣q 3 − s5 ⎥⎦ s 4 ⋅ c5 c 4 ⋅ c5 unde s4 = sin q4 N
(4.22)
c4 = cos q4
T N’ se poate exprimaşi ca o matrice în formă vectorială (6 x 1) şi anume: N
T vN ' = {q1 q 2 q 3 q 4 q 5 q 6 }T
(4.23)
Ceea ce conduce la exprimarea situării sistemului N’ în raport cu N cu expresia: N v SN'
= N T vN '
(4.24)
unde matricea de situare N S vN ' are expresia: N v SN'
⇒
N
= {rx ry rz o rx o ry o rz }T
(4.25)
r N ' = {rx ry rz }T este vectorul de poziţie al srcinii sistemului de referin ţă
N’ în raport cu sistemul de referinţă N;
⇒
N
o r N ' = {o rx o ry o rz }T este vectorul de orientare al sistemului de referinţă
N’ în raport cu sistemul de referinţă N.
Modelul matricial de ordinul zero al cuplei generalizate este definit de expresia: N
T vN ' = {q i }T
i = 1,6
având în componenţă elemente constante şi elemente variabile:
(4.26)
30 N
T vN '
=
N
T vN ' ct.
N
+
T vN ' var.
(4.27)
i = 1,6
(4.28)
sau
{q}i T {= }q i Tct{. }+ q i
T var .
Modelul cinematic de ordinul 1 al cuplei generalizate are expresia: N
∂ N v v T& N ' = ∂t ( T N ' ) =
N
(
v
T& N '
T
)
var .
= {q& i }var.
∂ N v ∂t ( T N ' )ct. = 0
(4.29)
Modelul cinematic de ordinul 2 al cuplei generalizate are expresia: N
&& N ' = T v
∂2 N v ( T N' ) = ∂t 2
(
N
&& N ' T v
)
var .
= {&q&i }Tvar.
(4.30)
∂{q i }ct. =0 ∂t
unde: qi ct. este partea constantă qi var. este partea variabilă
qi var.= qi var.(t)
ă cu relaţia: Numărul gradelor de libertate al cuplei generalizate se calculeaz 6
∑ (q var. ) j L= Clasa cuplei generalizate este:
j=1
(q var. ) j
q var. ≠ 0
(4.31)
i=6-L
Elementul N’M formează un „offset” generalizat (Fig. 4.7). O pereche de sisteme de referinţă N’ şi M se solidarizează cu acest element. Modelul matematic de ordinul zero al „offset”-ului generalizat are expresia: N'
v TM = {q c1 q c 2 q c3 q c 4 q c5 q c 6 }T
(4.32)
în care elementul N’M este nedeformabil (rigid). Întrucât q& ci = &q& ci = 0 modelele cinematice de ordinul 1 şi 2 al „offset”-ului generalizat nu mai au sens.
31 Alegând un reper P∈M, vectorul de poziţie al punctului P (Fig. 4.7), în raport cu sistemul de referinţă N se exprimă prin: N
M
N'
p = N T M ⋅ p= N T N '⋅ p
(4.33)
iar în raport cu sistemul de referinţă N’ cu relaţia: N'
M
N'
TM
(4.34)
⎧⎪ N p= N T N ' ⋅ N ' T M ⋅M p ⎨N N M ⎪⎩ p= T M ⋅ p
(4.35)
p= ⋅ p Din sistemul format de relaţiile (4.33) şi (4.34):
se obţine: N
N
T M = N T N' ⋅N' T M = ∏ T
(4.36)
M
Astfel în raport cu un reper exterior, „offset”-ul generalizat are ca expresie a modelelor cinematice de ordinul 1 şi 2: N
N T& M = T& N ' ⋅ N ' T M
N'
T& M = 0
(4.37)
respectiv: N &&
N && N' TM = T N' ⋅ T M
(4.38)
Considerând lanţul cinematic deschis (Fig. 4.8 a) format din două elemente N şi M legate printr-o cuplă cinematică NN’ şi având un „offset” N’M, expresia matricei de situare a elementului M în raport cu elementul N este: N
N
SM = N T M = ∏ T M
respectiv pentru un reper exterior:
(4.39)
32 N
N
M
p = ∏T ⋅ p
(4.40)
M
Viteza şi acceleraţia de schimbare a situării elementului M în raport cu elementul N este dată de expresia: N
S& M =
∂ N ∏T ∂t M
N
2 N &S& M = ∂ ∏ T 2
(4.41)
″ ∂2 ⎛ N ⎞ M p = 2 ⎜ ∏ T ⎟⋅ p ∂t ⎝ M ⎠
(4.42)
∂t
M
respectiv pentru un reper exterior: N
′ ∂ ⎛ N ⎞M p = ⎜ ∏ T ⎟⋅ p ∂t ⎝ M ⎠
deoarece pentru P∈M rezultă:
M
N
′ M ″ p = p =0
C C2
3 2 M
B
C1
C3 2’
B1
1
A
0
2’’
B3
I A1 a
1’ 0
2’’’
B2 1’’
II
A2
1’’’ A3
III
b
Fig. 4.8 Mecanism generator de traiectorie: a) robot serial; b) robot paralel
Contururile închise ale mecanismului (Fig. 4.8 se formeaz b) ă astfel încât fiecare conexiune care leagă elementul„m” de elementul„0” să se includă în maximum dou ă contururi închise. Conturul I:
0 – A1 – B1 – C1 – C2 – B2 – A2 – 0
Conturul II:
0 – A2 – B2 – C2 – C3 – B3 – A3 – 0
Conturul III:
0 – A1 – B1 – C1 – C3 – B3 – A3 – 0
33 0
⎛ ⎞ ⎛ ⎞ ⎛ ⎞ Sm = ⎜ ∏ T ⎟ = ⎜ ∏ T ⎟ = ⎜ ∏ T ⎟ = ⋅ ⋅ ⋅ ⎝ m ⎠1 ⎝ m ⎠ 2 ⎝ m ⎠ 3 0
N1'
0
0
T N 3' = N1' T N 2 ' = N 2 ' T N 3'
(4.43)
(4.44)
0
unde
∏ T corespunde conexiunii NN’, iar numărul de egalităţi matriceale din m
sistemul 4.44 este de :
(4.45)
l = ∑ (ord ε − 2)
unde s-a notat cu ordε, ordinul elementului ε, dacă ordε ≥ 3 4.1.7 Modelul geometric invers Denavit – Hartenberg
Pornind de la ecuaţia matriceală prezentată în relaţia 4.3 R
6
T EF = 0 T 6 = ∏ i −1 T i 1
Pe baza matricei coordonatelor generalizate: T
q = {q1 q 2 q 3 q 4 q 5 q 6 } şi înmulţind la stânga se separă succesiv variabilele „qi”: 0 1 2 3 4
(4.46)
T1−1 ⋅R T EF =1 T 2 ⋅2 T 3 ⋅3 T 4 ⋅4 T 5 ⋅5 T 6 =1 T 6
T −21 ⋅0 T1−1 ⋅R T EF = 2 T 3 ⋅3 T 4 ⋅4 T 5 ⋅5 T 6 = 2 T 6
T 3−1 ⋅1 T −2 1 ⋅0 T1−1 ⋅R T EF = 3 T 4 ⋅4 T 5 ⋅5 T 6 = 3 T 6
(4.47)
T −41 ⋅2 T 3−1 ⋅1 T −21 ⋅0 T1−1 ⋅R T EF = 4 T 5 ⋅5 T 6 = 4 T 6
T 5−1 ⋅3 T −4 1 ⋅2 T 3−1 ⋅1 T −21 ⋅0 T1−1 ⋅R T EF = 5 T 6
Produsele matriceale conduc la următoarea egalitate:
⎡ f1 (n ) f1 (o) f1 (a ) f1 (p) ⎤ ⎡ h11 h12 h13 h14 ⎤ ⎢f ( n ) f ( o ) f ( a ) f ( p ) ⎥ ⎢ h ⎥ 2 2 2 ⎢2 ⎥ = ⎢ 21 h 22 h 23 h 24 ⎥ ⎢f 3 (n ) f 3 (o) f 3 (a ) f 3 (p) ⎥ ⎢h 31 h 32 h 33 h 34 ⎥ ⎢ 0 0 0 1 ⎥⎦ ⎢⎣ 0 0 0 1 ⎥⎦ ⎣
(4.48)
34 respectiv următoarele relaţii între elementele matricelor:
⎧ f1 (n ) = h11 f1 (o) = h12 f1 (a ) = h13 f1 (p) = h14 ⎪ ⎨f 2 (n ) = h 21 f 2 (o) = h 22 f 2 (a ) = h 23 f 2 ( p) = h 24 ⎪f (n ) = h f 3 (a ) = h 33 f 3 (p) = h 34 31 f 3 (o) = h 32 ⎩3
(4.49)
Dintre aceste relaţii se selectează relaţia care este dependentă numai de variabila „qi” unde i=1...6. Soluţionând pe rând relaţiile 4.47 se obţin variabilele „qi”. EXEMPLUL 4.2
Aferent structurii robotului prezentat în Exemplul 4.1. Pornind de la expresia matricei de transformare omogenă aferentă mecanismului generator de traiectorie de structura RzTzTx:
⎡cos θ1 0 sin θ1 a 2 ⋅ cos θ1 + d 3 ⋅ sin θ1 ⎤ ⎢ sin θ 0 − cos θ a ⋅ sin θ − d ⋅ cos θ ⎥ 0 1 1 2 1 3 1⎥ T3 = ⎢ ⎢ 0 ⎥ 1 0 d2 ⎣⎢ 0
0
0
1
⎦⎥
⎧p x = a 2 cos θ1 + d 3 sin θ1 ⎪ ⎨p y = a 2 sin θ1 − d 3 cos θ1 ⎪p = d ⎩ z 2
(4.50)
Prima soluţie este d2 = pz. Primele două ecuaţii ale sistemului 4.50 conduc la obţinerea lui d3.
⎧⎪p 2x = (a 2 cos θ1 + d 3 sin θ1 )2 ⎨ 2 2 ⎪⎩p y = (a 2 sin θ1 − d 3 cos θ1 ) ⎧⎪p 2x = a 22 cos 2 θ1 + 2 a 2 d 3 sin θ1 cos θ1 + d 32 cos 2 θ1 ⎨ 2 2 2 ⎪⎩p y = a 2 sin θ1 − 2 a 2 d 3 sin θ1 cos θ1 + d 32 sin 2 θ1 d 32 (cos 2 θ1 + sin () 2 θ1 + a 22) cos 2 θ1 + sin 2 θ1 = p 2x + p 2y
(4.51)
35 d 32
+
a 22
p 2x
=
+
p 2y
pt. a2 = const.
y
A doua soluţie este d 3 = p 2x + p 2y − a 22
r
B
ϕ
Din prima relaţie a sistemului 4.50 şi anume
A
d 3 sin θ1 + a 2 cos θ1 = p x
x
Fig. 4.9
se admite o soluţie de forma: A sin θ1 + B cos θ1 = D
(4.52)
Utilizând substituţia (conform Fig. 4.9): A = r cos ϕ si B = r sin ϕ unde ϕ = arctg
B iar r = A 2 + B 2 A
(4.53)
r cos ϕ sin θ1 + r sin ϕ cos θ1 = D D cos ϕ sin θ1 + sin ϕ cos θ1 = r sin (ϕ + θ1 ) = Dr cos(ϕ + θ1 ) = 1 −
D2 r2
sin (ϕ + θ1 ) tg(ϕ + θ1 ) = = cos(ϕ + θ1 )
D r
± 1−
2
D r2
=
D ± r − D2 2
D D = arctg 2 2 ± r −D ± A + B2 − D 2 D B θ1 = arctg − arctg 2 2 2 A ± A +B −D
ϕ + θ1 = arctg
Pentru A = d3
B = a2
şi
θ1 = arctg
2
D = px d 32
se obţine a treia soluţie θ1:
± px a − arctg 2 2 2 d3 + a 2 − px
(4.54)
36 5 MODELUL CINEMATIC DIRECT ŞI INVERS AL ROBOTULUI
In cazul rezolv ării problemei cinematice directe a vitezelor se cunosc lungimile elementelorşi vitezele lor relative şi se calculeaz ă viteza rezultant ă a punctului caracteristic M. In cazul rezolvării problemei cinematice inverse a vitezelor se cunosc lungimile elementelor şi viteza rezultantă a punctului caracteristic M (Fig. 5.1) şi se calculează vitezele relative ale cuplelor conducătoare. 5.1 Modelul cinematic direct al robotului
N EF
z0
P
RI O0
x0
M=Oi
O yi
xi
A zi
y0
Fig. 5.1 Model de calcul pentru analiza cinematică directă a vitezelor
Se consideră un sistem de referinţă fix ataşat bazei robotului - RI cu srcinea în punctul O0 şi un sistem de referinţă mobil ataşat efectorului final – EF sau obiectului manipulat cu srcinea în punctul caracteristic M = O i. Starea de viteză a EF se defineşte prin matricea: r
⎧⎪ P& ⎫⎪ X = ⎨ r& ⎬ ⎪ ⎪ ⎩Φ ⎭ r&
r
T
unde: P& = {P& x P& y P& z } este vectorul viteză liniară al srcinii O i; r
& & Φ = {Φ x
& Φ y
T
& } este vectorul viteză unghiulară rezultant al EF; Φ z
(5.1)
37 Se defineşte matricea vitezelor generalizate: r
Q& = {q& 1 q& 2 q& 3 q& 4 q& 5 q& 6 }T unde: q& i = q& i =
(5.2)
∂ (θi ) pentru o cuplă conducătoare de rotaţie; ∂t ∂ (d i ) pentru o cuplă de translaţie; ∂
t r r Legătura dintre cele două relaţii X& şi Q& este dată de matricea Jacobiană: r
r
X& = J ⋅ Q&
(5.3)
sau ⎧ P& x ⎫ ⎡ J11 J12 ⎪ & ⎪ ⎢J ⎪ Py ⎪ ⎢ 21 J 22 ⎪⎪ P& z ⎪⎪ ⎢ J 31 J 32 ⎨& ⎬=⎢ ⎪Φ x ⎪ ⎢ J 41 J 42 & y ⎪ ⎢ J 51 ⎪⎪Φ ⎪ ⎢ & ⎪ ⎩⎪Φ z ⎭ ⎣ J 61
J13 J 23 J 33 J 43
J14 J 24 J 34 J 44
J15 J 25 J 35 J 45
J16 ⎤ ⎧ q& 1 ⎫ J 26 ⎥⎥ ⎪⎪q& 2 ⎪⎪ J 36 ⎥ ⎪q& 3 ⎪ ⋅⎨ ⎬ J 46 ⎥⎥ ⎪q& 4 ⎪
(5.4)
J 52 J 53 J 54 J 55 J 56 ⎥ ⎪⎪q& 5 ⎪⎪ ⎥ J 62 J 63 J 64 J 65 J 66 ⎦ ⎩q& 6 ⎭
Soluţia problemei constă în determinarea elementelor matricei Jacobiene. Explicitând prima linie: P& x = J11q& 1 + J12 q& 2 + J13q& 3 + J14 q& 4 + J15 q& 5 + J16 q& 6 pentru: se obţine
P& x =
∂Px ∂t
q& i =
∂q i ∂t
∂Px = J11∂q1 + J12 ∂q 2 + J13∂q 3 + J14 ∂q 4 + J15 ∂q 5 + J16 ∂q 6 / ∂q1
(5.5)
(5.6)
(5.7)
respectiv: J11 =
∂q ∂q ∂q ∂Px ∂q ∂q − J12 2 − J13 3 − J14 4 − J15 5 − J16 6 ∂q1 ∂q1 ∂q1 ∂q1 ∂q1 ∂q1
(5.8)
38 şi având în vedere că în fiecare cuplă conducătoare parametrii cinematici sunt
independenţi între ei, adică: ∂q 2 ∂q 3 ∂q 4 ∂q 5 ∂q 6 = = = = =0 ∂q1 ∂q1 ∂q1 ∂q1 ∂q1
J11 =
∂Px ∂q1
(5.9)
În mod similar se deduc şi celelalte elemente ale matricei Jacobiene, soluţionând linie cu linie relaţia 5.4. Sistemul de 36 de relaţii rezultat este de forma: J1i =
∂Px ∂q i
J 2i =
∂Py ∂q i
J 3i =
∂Pz ∂q i
J 4i =
∂Φ x ∂q i
J 5i =
∂Φ y ∂q i
J 6i =
∂Φ z ∂q i
(5.10)
5.2 Modelul cinematic invers al robotului r
Cunoscând starea de viteză a EF prin matricea X& , se determină starea de viteză a cuplelor conducătoare cu relaţia: r
r
Q& = J −1 ⋅ X&
(5.11)
Din expresiile elementelor matricei Jacobiene, se constată că elementele primei coloane depind numai de q1, elementele celei de-a doua coloane depind numai de q2, ş.a.m.d., adică:
Ji1 = f(q1)
Ji2 = f(q2)
...
Ji6 = f(q6)
(5.12)
Astfel primele trei linii ale matricei Jacobiene se obţin cu ajutorul submatricei de translaţie, iar următoarele trei linii cu ajutorul submatricei de rotaţie din cadrul matricei de transformare omogenă. 5.3 Metoda P. G. Ránky de determinare a elementelor matricei Jacobiene r
Considerând un punct „P”, definit de vectorul de poziţie P , faţă de un sistem de referinţă mobil cu srcinea O M, poziţionat în raport cu un sistem de referinţă fix cu r
srcinea în O F, prin vectorul X 0 .
39 xM OM
zF
x0 OF
xF
yM
P P
x
yF
zM
Fig. 5.2 Model de calcul pentru viteza punctului P
Ecuaţia vectorială a punctului P în sistemul de referinţă fix: r
r
r
F
X= F X 0 + F P
F
V = F V0 + F P′
(5.13)
derivată în raport cu timpul: r
r
r
(5.14)
Considerând submatricea de orientare definită prin relaţia_ ⎡cos θ − sin θ cos α sin θ sin α ⎤ R = ⎢⎢ sin θ cos θ cos α − cos θ sin α ⎥⎥ sin α cos α ⎦⎥ ⎣⎢ 0
(5.15)
Vectorul de poziţie al punctului P în raport cu sistemul de referinţă fix se exprimă prin relaţia: F
r
r
P = F R M ⋅M P
(5.16)
sau derivata ei: r F P′=
r
F
R ′ M ⋅M P +
r
F
R M ⋅M P′
(5.17)
Întrucât matricea R este ortonormală, având proprietatea: R −1 = R T şi cum:
M
r
P = cons tan t ⇒
R −1 ⋅ R = I M
r
P′ = 0
(5.18)
40 se obţine: F
r
r F
F
r
r
P′= (R ′ ⋅ R T )⋅ R (⋅M P=) R ′ ⋅ R T ⋅F R ⋅M P = F Ω⋅F R⋅M P
(5.19)
Derivând relaţia 5.15 se obţine: ⎡− sin θ − cos θ cos α cos θ sin α ⎤ ′ R = ⎢⎢ cos θ − sin θ cos α sin θ sin α ⎥⎥ ⎣⎢
0
0
0
(5.20)
⎦⎥
Transpusa matricei din relaţia 5.15 este: sin θ 0 ⎤ ⎡ cos θ ⎢ R = ⎢− sin θ cos α cos θ cos α sin α ⎥⎥ ⎣⎢ sin θ sin α − cos θ sin α cos α ⎥⎦ T
(5.21)
iar produsul matriceal: ⎡0 − 1 0 ⎤ R ′ ⋅ R = ⎢⎢1 0 0⎥⎥ ⋅ θ& = Ω ⎣⎢0 0 0⎦⎥ T
(5.22)
Revenind la relaţia 5.14: F
r
r
r
V = F V0 + F Ω⋅F R ⋅M P
(5.23)
Submatricea de translaţie are expresia: F
⎧a ⋅ cos θ⎫ ⎪ ⎪ X 0 = ⎨ a ⋅ sin θ ⎬ ⎪ d ⎪ ⎩ ⎭ r
(5.24)
care prin derivare: ⎧− θ& ⋅ a ⋅ sin θ⎫ ⎪ ⎪ F V0 = ⎨ θ& ⋅ a ⋅ cos θ ⎬ ⎪ ⎪ d& ⎩ ⎭ r
(5.25)
41 Relaţia finală a vitezei liniare este ⎧− θ& ⋅ a ⋅ sin θ⎫ ⎡0 − 1 0⎤ ⎡cos θ − sin θ cos α sin θ sin α ⎤ r ⎪ ⎪ V = ⎨ θ& ⋅ a ⋅ cos θ ⎬ + ⎢1 0 0⎥ ⋅ θ& ⋅ ⎢ sin θ cos θ cos α − cos θ sin α ⎥⋅M P ⎥ ⎪ ⎪ ⎢⎣⎢0 0 0⎥⎥⎦ ⎢⎣⎢ 0 d& sin α cos α ⎦⎥ ⎩ ⎭
Fr
(5.26)
5.4 Cinematica unei cuple conducătoare de rotaţie
XF XM
θi
ZF = ZM
OF = OM 1
YF
θi
θi
YM
0
Fig. 5.3 Schema unei cuple conducătoare de rotaţie
Pentru cupla conducătoare de rotaţie conform Fig. 5.3 viteza liniară a srcinii F r
V = F Ω⋅F R ⋅M Pr
deci din relaţia 5.23 se obţine:
zi
(5.27)
xi
zn-1
r
Pn
On-1
yn-1 xn-1
zi-1 yi-1
r
V0 ,
yi
Oi i
F
xi-1 Oi-1
i −1
r
Pn yn
xn On=M zn
Fig. 5.4 Calculul vectorilor de poziţie al punctului P
Pentru două elemente succesive „i-1” şi „i” legate între ele printr-o cuplă conducătoare de rotaţie i-1Ci relaţia 5.27 devine: i −1
r
r
Vi = i −1 Ω i ⋅i −1 R i ⋅i Pn
(5.28)
42
r
i
Considerând vectorul Pn ca unind punctul caracteristic M, aparţinând efectorului final cu srcinea O i, vectorul
i −1
r
Pn se obţine din relaţia: i −1
r
r
Pn = i −1 R i ⋅i Pn
(5.29)
Expresie utilizată în relaţia 5.28 conduce la: i −1
r
i −1
Vi i −1 i −1
sau:
r
Vi =
i −1
r
= Ω ⋅ Pn i
i −1
⎡0 − 1 0 ⎤ ⎢1 0 0 ⎥ ⋅ θ& ⋅ ⎢ ⎥ i ⎢⎣0 0 0 ⎥⎦ i
⎧Px ⎫ ⎪ ⎪ ⎨ Py ⎬ = ⎪P ⎪ ⎩ z ⎭i
(5.30) i −1
⎧− Py ⎫ ⎪ ⎪ ⎨ Px ⎬ ⋅ θ& i ⎪ 0 ⎪ ⎩ ⎭n
(5.31)
Pentru a exprima viteza elementului „i” în sistemul de referinţă fix este necesar să apelăm la submatriacea de rotaţie R a matricei de transformare omogenă 0 T i-1 şi anume 0 R i-1 şi obţinem: i −1
0
⎧− Py ⎫ ⎪ ⎪ Vi = R i−1 ⋅ ⎨ Px ⎬ ⋅ θ& i = ⎪ 0 ⎪ ⎩ ⎭n r
0
0
O x A x ⎤ i−1 ⎧− Py ⎫ ⎪ ⎪ O y A y ⎥⎥ ⋅ ⎨ Px ⎬ ⋅ θ& i = O z A z ⎥⎦ i−1 ⎪⎩ 0 ⎪⎭ n
⎡N x ⎢N ⎢ y ⎢⎣ N z
0 ⎡ 0 ⎧N x ⎫ ⎤ ⎧O x ⎫ ⎢ ⎪ ⎪ i−1 ⎥ ⎪ ⎪ i−1 (Py )n + ⎨O y ⎬ (Px )n ⎥ ⋅ θ& i = ⎢− ⎨ N y ⎬ ⎪O ⎪ ⎢ ⎪N ⎪ ⎥ ⎩ z ⎭i−1 ⎣ ⎩ z ⎭i−1 ⎦
(5.32)
Mişcarea de rotaţie finită în jurul axei zi: ⎧0 ⎫ ⎪ ⎪ i−1 Φ i = 0 ii + 0 ji + θk i = ⎨0⎬θ i ⎪1⎪ ⎩ ⎭ r
iar derivata:
r
r
r
⎧0⎫ ⎪ ⎪ Φ i = ⎨0⎬θ& i ⎪1⎪ ⎩ ⎭
r i−1 &
(5.33)
(5.34)
43 În mod similar pentru a exprima viteza unghiulară a elementului „i” în sistemul de referinţă fix, apelăm la submatricea de rotaţie 0 R i-1: 0
⎡N x r 0 & Φ i = ⎢⎢ N y ⎢⎣ N z
Ox Ax ⎤ r O y A y ⎥⎥ ⋅i−1 Φ& i = O z A z ⎥⎦ i−1
0
⎡N x ⎢N ⎢ y ⎢⎣ N z
Ox Ax ⎤ ⎧0⎫ r ⎪ ⎪ ⎥ i−1 & O y A y ⎥ ⋅ Φ i = ⎨0⎬θ& i = ⎪1⎪ O z A z ⎥⎦ i−1 ⎩ ⎭
0
⎧A x ⎫ ⎪ ⎪ & ⎨A y ⎬ θ i (5.35) ⎪A ⎪ ⎩ z ⎭i−1
Grupând viteza liniară şi viteza unghiulară relativă şi absolută conform relaţiei 5.1: i −1
i −1
r
⎧V ⎫ X i = ⎨ r& ⎬ ⎩Φ ⎭i
r i −1 &
r i −1 &
Xi =
⎧− Py ⎫ ⎪P ⎪ ⎪ x ⎪ ⎪ 0 ⎪ & ⎨ 0 ⎬ ⋅ θi ⎪ ⎪ ⎪ 0 ⎪ ⎪ ⎪ ⎩ 1 ⎭n
0
& 0r
0
r
X i = ⎧⎨V r& ⎫ ⎬
⎩Φ ⎭ i
(5.36a)
0 0 ⎫ ⎧ P& x ⎫ ⎧ ⎧ N x ⎫ ⎧O x ⎫ ⎪ ⎪ & ⎪ ⎪ ⎪ ⎪ i −1 ( ) ⎪ ⎪ i −1 Py n + ⎨O y ⎬ (Px )n ⎪ ⎪ Py ⎪ ⎪− ⎨ N y ⎬ & 0r ⎪⎩O z ⎪⎭i −1 ⎪⎬ θ& (5.36b) X& i = ⎪⎨ Pz ⎪⎬ = ⎪⎨ ⎪⎩ N z ⎪⎭i −1 0 i & Φ ⎧A x ⎫ ⎪ x⎪ ⎪ ⎪ ⎪ ⎪ & ⎪ ⎪Φ ⎪ ⎪ ⎨A y ⎬ ⎪ y⎪ ⎪ ⎪ ⎪ ⎪ & ⎪⎩Φ z ⎪⎭i ⎪ ⎪⎭ ⎩ A z ⎭i −1 ⎩
5.5 Cinematica unei cuple conducătoare de translaţie
XF
di
1
XM
ZF
ZM
OF
OM
0 YF
YM
Fig. 5.5 Schema unei cuple conducătoare de translaţie
θ = cons tan t ⇒
Se caracterizează prin lipsa mişcării de rotaţie:
F
rămâne numai componenta specifică translaţiei:
r
θ& =
44 0 deci
r
V = F V0
(5.37)
Considerând sistemele fix şi mobil ca fiind două sisteme succesive ataşate elementelor „i-1” şi ”i” legate între ele printr-o cuplă de translaţie i-1Ci: i −1
⎧ ⎫ i −1 r Vi = ⎪⎨ 00 ⎪⎬ = ⎪d& ⎪ ⎩ i ⎭n
i −1
⎧ ⎫ ⎪00⎪ ⋅ d& ⎨ ⎬ i ⎪1⎪ ⎩ ⎭n
(5.38)
Pentru a exprima viteza elementului „i” în sistemului de referinţă fix se apelează la submatricea de orientare 0 R i-1 a matricei de transformare omogenă 0 T i-1: 0
⎡N x 0 0 i −1 Vi = R i−1 ⋅ Vi = ⎢⎢ N y ⎢⎣ N z r
r
i −1
Ox Ax ⎤ ⎧0⎫ ⎧A x ⎫ ⎪ ⎪ & ⎪ ⎪ ⎥ O y A y ⎥ ⋅ ⎨0⎬ ⋅ d i = ⎨A y ⎬ ⋅ d& i ⎪A ⎪ O z A z ⎥⎦ i−1 ⎪⎩1⎪⎭ n ⎩ z ⎭i−1
Ţinând cont că vectorul viteză unghiulară relativă
i −1 r & Φ
i
(5.39)
r & = 0, = 0 şi absolută 0 Φ i
alături de viteza liniară relativă şi absolută se obţine: i −1
i −1
r
⎧V ⎫ X i = ⎨ r& ⎬
r i −1 &
r i −1 &
Xi =
⎩Φ ⎭ i
0
0
r
⎧V ⎫ X i = ⎨ r& ⎬ ⎩Φ ⎭i
r 0 &
⎧ P& ⎫ ⎪ P& x ⎪ ⎪ y⎪ r& ⎪⎪ P& ⎪⎪ 0 Xi = ⎨ z ⎬ = & ⎪Φ x ⎪ & ⎪ ⎪Φ ⎪ y⎪ & ⎪ ⎩⎪Φ z ⎭i
0
⎧0 ⎫ ⎪0 ⎪ ⎪ ⎪ ⎪1⎪ & ⎨0 ⎬ ⋅ d i ⎪ ⎪ ⎪0 ⎪ ⎪ ⎪ ⎩0 ⎭ n
⎧A ⎫ ⎪A xy ⎪ ⎪ ⎪ ⎪A z ⎪ ⎨ ⎬ ⋅ d& i ⎪0⎪ ⎪0⎪ ⎪ ⎪ ⎩ 0 ⎭i −1
(5.40a)
(5.40b)
45 6. MODELUL CINETOSTATIC AL DISPOZITIVULUI DE GHIDARE AL ROBOŢILOR
Se consider ă dispozitivul de ghidare al unui robot industrial avândă la lanbaz ţul cinematic deschis prezentat în Fig. 6.1. Pentru determinarea ţiunilor reac dinamice în cuplele cinematice şi a forţelor /momentelorde echilibrare în cuplele cinematice conduc ătoare se impuneăs cunoaştem mişcarea mecanismului şi rezultanta for ţelor / momentelor exterioare (gravita ţionale, r
r
de frecareşi tehnologice)Fn şi M n ce acţionează asupra elementului „n”. RA MA n-1
Mi
n A
Mn
RD i
Fi
D
Fn MD i-1
2 M 1
N 0
Fig. 6.1 Schema de calcul cinetostatic al dispozitivului de ghidare al roboţilor r
r
Se va determina torsorul reac ţiunilor dinamiceR A şi M A din cupla cinematic ă de rotaţie A pentru a atinge starea deşcare mi dorită. Ecuaţiile de echilibru sunt:
46
⎧⎪R A + Fn = 0 ⎨ ⎪⎩M A + M n = 0 r
r
r
(6.1)
r
Admiţând că, cupla cinematic ă D este o cuplă de translaţie, al cărui torsor al for ţelor exterioare r
r
include for ţele/momenteleFi şi M i de echilibrare ale cuplei cinematice analizate anterior, atunci ecua ţia de echilibru este:
⎧⎪R D + Fi = 0 ⎨ ⎪⎩M D + M i = 0 r
r
r
(6.2)
r
Torsorul forţelor generalizate din cupla cinematică de rotaţie A este:
⎧R A = {R Ax R Ay R Az }T ⎪ 3 x1 unde ⎨ T { M M M M = ⎪⎩ A Ax Ay Az }3 x1 r
⎧⎪R A ⎫⎪ ℜA = ⎨ ⎬ ⎪⎩M A ⎪⎭6 x1 r
r
(6.3)
iar torsorul forţelor generalizate exterioare: r
⎧⎪F n ⎫⎪
ℑ= ⎨ ⎬ ⎪⎩M n ⎪⎭ 6 x1 r
unde
⎧⎪Fn = {Fnx
⎨ ⎪⎩M n = {M nx
Fny
Fnz }3Tx1
r
M ny
M nz }
T
(6.4)
3 x1
Utilizând matricea Jacobiană:
ℜA = J ⋅ ℑ r
⎧ R Ax ⎫ ⎡ J11 ⎪ R ⎪ ⎢J ⎪ Ay ⎪ ⎢ 21 ⎪ R Az ⎪ ⎢ J 31 ⎨ ⎬=⎢ ⎪M Ax ⎪ ⎢J 41 ⎪M Ay ⎪ ⎢ J 51 ⎪ ⎪ ⎢ ⎩ M Az ⎭ ⎣J 61
r
(6.5) J16 ⎤ ⎧ Fnx ⎫
J12
J13
J14
J15
J 22
J 23
J 24
J 25
J 26 ⎥ ⎪ Fny ⎪
J 32
J 33
J 34
J 35
J 36 ⎥ ⎪ Fnz ⎪
J 42
J 43
J 44
J 45
⎥ ⎪
⎪
⎥⋅⎨
⎬
⎥ ⎪
⎪
J 52
J 53
J 54
J 55
J 46 ⎥ ⎪M nx ⎪ J 56 ⎥ ⎪M ny ⎪
J 62
J 63
J 64
J 65
J 66 ⎦ ⎩M nz ⎭
Explicitând prima linie: R Ax = J11 ⋅ Fnx + J12 ⋅ Fny + J13 ⋅ Fnz + J14 ⋅ M nx + J15 ⋅ M ny + J16 ⋅ M nz
(6.6)
47 Se determină elementele matricei Jacobiene:
∂R Ax ⎧ ⎪ J11 = ∂F nx ⎪ ∂R Ay ⎪J ⎪ 21 = ∂F nx ⎪ ⎪ J = ∂R Az ⎪⎪ 31 ∂Fnx ⎨ ⎪J 41 = ∂M Ax ∂Fnx ⎪ ⎪ ∂M Ay ⎪J 51 = ∂Fnx ⎪ ⎪ ∂M Az ⎪ J 61 = ∂Fnx ⎩⎪
J12 = J 22 =
∂R Ax ∂Fny ∂R Ay
∂Fny ∂R Az J 32 = ∂Fny
J 42 = ∂M Ax ∂Fny ∂M Ay J 52 = ∂Fny ∂M Az J 62 = ∂Fny
J13 = J 23 =
∂R Ax ∂Fnz ∂R Ay
∂Fnz ∂R Az J 33 = ∂Fnz
J 43 = ∂M Ax ∂Fnz ∂M Ay J 53 = ∂Fnz ∂M Az J 63 = ∂Fnz
J14 = J 24 =
∂R Ax ∂M nx ∂R Ay
J15 = J 25 =
∂M nx ∂R Az J 34 = ∂M nx
∂R Ax ∂M ny ∂R Ay
∂M ny ∂R Az J 35 = ∂M ny
J 44 = ∂M Ax ∂M nx ∂M Ay J 54 = ∂M nx ∂M Az J 64 = ∂M nx
J 45 = ∂M Ax ∂M ny ∂M Ay J 55 = ∂M ny ∂M Az J 65 = ∂M ny
J16 = J 26 =
∂R Ax ∂M nz ∂R Ay
∂M nz ∂R Az J 36 = ∂M nz
J 46 = ∂M Ax (6.7) ∂M nz ∂M Ay J 56 = ∂M nz ∂M Az J 66 = ∂M nz
6.1 Cinetostatica unei cuple conducătoare de rotaţie
M eA
θ
MA
n
Fn
n-1
RA
Mn
hA
i On xn
j
k
zn
dn
yn
Fig. 6.2 Model de calcul al unei cuple conducătoare de rotaţie
48 Ecuaţia de echilibru în cupla cinematic ă A este:
⎧⎪R A + Fn = 0 ⎨ ⎪⎩M A + M n + h A × R A + d n × Fn + ΣM fA = 0 r
r
r
r
r
r
r
r
(6.8)
r
Neglijând efectul momentelor forţelor de frecare: ΣM fA = 0 şi: r
⎧
Fn = {Fnx r
r
ΣM fA r
{ M ny} M nz { 3Tx1 } d nz 3Tx1{ d ny Fnz
T
3 x1
R }A = R Ax r
R Ay
R Az
T
3 x1 T M Ay M Az 3 x1 T h Ay h Az 3 x1
⎪⎨ M n = {M nx M A} = M Ax (6.9) ⎪ d = {d } h h = ⎪⎩ n nx A Ax ≠ 0 vezi Roboţi Industriali – Kovacs F.şi Rădulescu C.: Litografia Institutul Politehnic ”Traian Vuia” Timi şoara, 1992 – pag. 116. r
Pentru
F}ny
r
r
r
unde:
Fn - rezultanta forţelor exterioare; r
M n - momentul rezultant al forţelor exterioare; r
r
d n - perpendiculara din On pe direcţia vectorului Fn ; r
R A - reacţiunea din cupla cinematică A; r
M A - momentul reactiv din cupla cinematic ă A; r
r
h A - perpendiculara din On pe direcţia vectorului R A . Din prima ecuaţie a relaţiei 6.8 rezultă:
⎧R Ax ⎫ ⎧Fnx ⎫ ⎪ ⎪ ⎪ ⎪ ⎨R Ay ⎬ = −⎨Fny ⎬ ⎪R ⎪ ⎪F ⎪ ⎩ Az ⎭ ⎩ nz ⎭
R Ax = − Fnx R Ay = − Fny
(6.10)
R Az = − Fnz
Ceea ce permite, ca din relaţia 6.7, să se determine elementele matricei Jacobiene din primele trei linii:
∂R Ax = −1 J12 =J13 =J14 = J15 = J16 = 0 ∂ nx F ∂R Ay J 22 = = −1 J 21=J 23 = J 24 =J 25 = J 26 = 0 ∂Fny J11=
J 33 =
(6.11)
∂R Az = −1 J 31=J 32 =J 34 = J 35 =J 36 = 0 ∂Fnz
Neglijând suma momentelor forţelor de frecare ΣM fA = 0 şi efectuând produsele: r
49
⎡ i ⎢ h A × R A = ⎢ h Ax ⎢R Ax ⎣ r
k ⎤
j
⎥ (h R − h AzR Ay )i − (h Ax R Az − h AzR Ax ) j + h Az ⎥ = Ay Az (6.12) + (h Ax R Ay − h AyR Ax )k ⎥ R Az ⎦
r
h Ay
r
r
R Ay
⎡ i ⎢ d n × Fn = ⎢d nx ⎢ Fnx ⎣ r
r
k ⎤
j
⎥ (d ny Fnz − d nz Fny )i − (d nx Fnz − d nz Fnx ) j + d nz ⎥ = + (d nx Fny − d ny Fnx )k Fnz ⎥⎦
r
d ny
r
r
r
Fny
(6.13)
Din a doua ecuaţie a relaţiei 6.8 rezultă:
⎧M Ax ⎫ ⎧M nx ⎫ ⎧ h AyR Az − h AzR Ay ⎫ ⎧ d ny Fnz − d nz Fny ⎫ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨M Ay ⎬ = −⎨M ny ⎬ − ⎨ h AzR Ax − h Ax R Az ⎬ − ⎨d nz Fnx − d nx Fnz ⎬ ⎪M ⎪ ⎪M ⎪ ⎪h R − h R ⎪ ⎪d F − d F ⎪ Ay Ax ⎭ ny nx ⎭ ⎩ Az ⎭ ⎩ nz ⎭ ⎩ Ax Ay ⎩ nx ny
(6.14)
Ceea ce permite să se determine elementele matricei Jacobiene din ultimele trei linii: M Ax = −M nx − h AyR Az + h AzR Ay − d ny Fnz + d nz Fny
= −M nx + h AyFnz − d nyFnz − h AzFny + d nz Fny =−
− M nx
−
(h Az
+ d nz )Fny
J 41 =
∂M Ax =0 ∂Fnx
J 44 =
∂M Ax ∂M Ax = −1 J 45 = =0 ∂M nx ∂M ny
J 42 =
(6.15)
−
(h Ay
d ny )Fnz
∂M Ax ∂M Ax = −(h Az − d nz ) J 43 = = + (h Ay − d ny ) ∂Fny ∂Fnz J 46 =
∂M Ax =0 ∂M nz
(6.16)
M Ay = −M ny − h Az R Ax + h Ax R Az − d nz Fnx + d nx Fnz
= −M ny + h Az Fnx − d nz Fnx − h Ax Fnz + d nx Fnz
(6.17)
= −M ny + (h Az − )d nz( Fnx −) h Ax − d nx Fnz J 51 = ∂M Ay = +(h) Az − d nz ∂Fnx J 54 =
∂M Ay ∂M nx
=0
J 52 = ∂M Ay = 0 ( ) J 53 = ∂M Ay = − h Ax − d nx ∂Fny ∂Fnz J 55 =
∂M Ay ∂M ny
= −1
J 56 =
∂M Ay ∂M nz
=0
(6.18)
50 M Az = −M nz − h Ax R Ay + h Ay R Ax − d nx Fny + d ny Fnx
= −M nz − h Ay Fnx + d ny Fnx + h Ax Fny − d nx Fny
(6.19)
= −M nz − (h Ay − d ny )Fnx + (h Ax − d nx )Fny J 61 =
∂M Az = −(h Ay − d ny ) ∂Fnx
J 62 =
∂M Az = +(h Ax − d nx ) ∂Fny
J 63 =
∂M Az =0 ∂Fnz
J 64 =
∂M Az =0 ∂M
J 65 =
∂M Az =0 ∂M
J 66 =
∂M Az = −1 ∂M
nx
ny
(6.20)
nz
Elementele matricei Jacobiene sunt: J11 =J 22 =J 33 = J 44 =J 55 =J 66 = −1 J 42 = −(h Az − d nz ) = − J 51
(6.21)
J 43 = +(h Ay − d ny ) = −J 61 J 53 = −(h Ax − d nx ) = −J 62
Matrcea Jacobiană pentru o cuplă conducătoare de rotaţie are expresia:
J rotatie
⎡ J11 ⎢ ⎢ 0 ⎢ 0 =⎢ ⎢ 0 ⎢− J 42 ⎢ ⎣ − J 43
0
0
0
0
0 ⎤
J 22 0
0 J 33
0 0
0 0
0 ⎥ 0 ⎥
J 42
J 43
J 44
0
0
J 53
0
J 55
0 ⎥ 0 ⎥
− J 53
0
0
0
J 66 ⎦
⎥ ⎥
(6.22)
⎥
Momentului de echilibrare MeA din cupla cinematică de rotaţie A, ce acţionează pe direcţia axei z şi pune în mişcare elementul „n” în raport cu „n-1”, se obţine din relaţia: MeA = MAz
(6.23)
51 6.2 Cinetostatica unei cuple conducătoare de translaţie
F eD
d
MD
Fn
n-1 n
RD
Mn
hD
i
On
xn
j
zn
k
dn
yn
Fig. 6.3 Model de calcul al unei cuple conducătoare de translaţie
Ecuaţia de echilibru în cupla cinematic ă D este:
⎧⎪R D + Fn + ΣFfD = 0 ⎨ ⎪⎩M D + M n + h D × R D + d n × Fn = 0 r
r
r
r
r
r
r
r
(6.24)
r
Pentru : ΣFfD = 0 şi: r
T ⎧ Fn = {Fnx F}ny Fnz T { R }D = R Dx R Dy R Dz 3x1 3 x1 ⎪⎪ { 3Tx1 M D} = M Dx M Dy M Dz 3Tx1 (6.25) ⎨ M n = {M nx M ny} M nz T T ⎪ d = {d d ny d nz } h D = {0 0 h Dz }3x1 ⎪⎩ n nx 3 x1 Pentru ΣFfD ≠ 0 vezi Roboţi Industriali – Kovacs F.şi Rădulescu C.: Litografia Institutul Politehnic ”Traian Vuia” Timi şoara, 1992 – pag. 126. r
r
r
r
r
r
r
r
unde:
Fn - rezultanta forţelor exterioare; r
M n - momentul rezultant al forţelor exterioare; r
r
d n - perpendiculara din On pe direcţia vectorului Fn ; r
R D - reacţiunea din cupla cinematică D;
52 r
M D - momentul reactiv din cupla cinematic ă D; r
h D - paralela cu direcţia de mişcare. Din prima ecuaţie a relaţiei 6.24 rezultă:
⎧R Dx ⎫ ⎧Fnx ⎫ ⎪ ⎪ ⎪ ⎪ ⎨R Dy ⎬ = −⎨Fny ⎬ ⎪R ⎪ ⎪F ⎪ ⎩ Dz ⎭ ⎩ nz ⎭
R Dx = − Fnx R Dy = − Fny
(6.26)
R Dz = − Fnz
Ceea ce permite, ca din relaţia 6.7, să se determine elementele matricei Jacobiene din primele trei linii:
∂R Dx = −1 J12 =J13 = J14 = J15 = J16 = 0 ∂Fnx ∂R Dy J 22 = = −1 J 21 =J 23 = J 24 = J 25 = J 26 = 0 ∂Fny J11 =
J 33 =
(6.27)
∂R Dz = −1 J 31 =J 32 =J 34 =J 35 = J 36 = 0 ∂Fnz r
Neglijând suma forţelor de frecare ΣFfD = 0 şi efectuând produsele:
⎡ i ⎢ hD × RD = ⎢ 0 ⎢R Dx ⎣ r
⎡ i ⎢ d n × Fn = ⎢d nx ⎢ Fnx ⎣ r
j
j
r
d ny Fny
k ⎤
⎥
r
r
h Dz ⎥ = − h Dz R Dy i + h Dz R Dx j
(6.28)
⎥ (d ny Fnz − d nz Fny )i − (d nx Fnz − d nz Fnx ) j + d nz ⎥ = + (d nx Fny − d ny Fnx )k Fnz ⎥⎦
(6.13)
r
k ⎤
0 R Dy
R Dz ⎥⎦
r
r
r
Din a doua ecuaţie a relaţiei 6.24 rezultă:
⎧M Dx ⎫ ⎧M nx ⎫ ⎧− h Dz R Dy ⎫ ⎧ d ny Fnz − d nz Fny ⎫ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨M Dy ⎬ = − ⎨M ny ⎬ − ⎨ h Dz R Dx ⎬ − ⎨d nz Fnx − d nx Fnz ⎬ ⎪M ⎪ ⎪M ⎪ ⎪ ⎪ ⎪d F − d F ⎪ 0 ⎩ Dz ⎭ ⎩ nz ⎭ ⎩ ⎭ ⎩ nx ny ny nx ⎭
(6.29)
Ceea ce permite să se determine elementele matricei Jacobiene din ultimele trei linii:
53 M Dx = −M nx + h Dz R Dy − d ny Fnz + d nz Fny
= −M nx − h Dz Fny − d ny Fnz + d nz Fny
(6.30)
= −M nx − (h Dz − d nz )Fny − d ny Fnz J 41 =
∂M Dx =0 ∂Fnx
J 44 =
∂M Dx ∂M Dx = −1 J 45 = =0 ∂ ∂
J 42 =
M nx
∂M Dx ∂M Dx = −(h Dz − d nz ) J 43 = = −d ny ∂Fny ∂Fnz J 46 =
∂M Dx =0 ∂
M ny
(6.31)
M nz
M Dy = − M ny − h Dz R Dx − d nz Fnx + d nx Fnz
= −M ny + h Dz Fnx − d nz Fnx + d nx Fnz
(6.32)
= −M ny + (h Dz − d nz )Fnx + d nx Fnz J 51 = J 54 =
∂M Dy ∂Fnx ∂M Dy ∂M nx
= +(h Dz − d nz )
J 52 =
=0
J 55 =
∂M Dy ∂Fny ∂M Dy ∂M ny
=0
J 53 =
= −1
J 56 =
∂M Dy ∂Fnz ∂M Dy ∂M nz
= d nx (6.33)
=0
M Dz = − M nz − d nx Fny + d ny Fnx = −M nz − +d ny Fnx − d nx Fny J 61 =
∂M Dz = d ny ∂Fnx
J 62 =
∂M Dz = −d nx ∂Fny
J 63 =
∂M Dz =0 ∂Fnz
J 64 =
∂M Dz =0 ∂M nx
J 65 =
∂M Dz =0 ∂M ny
J 66 =
∂M Dz = −1 ∂M nz
(6.34)
(6.35)
Elementele matricei Jacobiene sunt: J11 =J 22 =J 33 = J 44 =J 55 =J 66 = −1 J 42 = −(h Dz − d nz ) = −J 51 J 43 = − d ny = −J 61 J 53 = +d nx = −J 62
Matrcea Jacobiană pentru o cuplă conducătoare de translaţie are expresia:
(6.36)
54
⎡ J11 ⎢ 0 ⎢ ⎢ 0 J translatie = ⎢ ⎢ 0 ⎢ − J 42 ⎢ ⎣ − J 43
0 ⎤
0
0
0
0
J 22
0
0
0
0 ⎥
0
J 33
0
0
0 ⎥
J 42
J 43
J 44
0
0
J 53
0
J 55
0 ⎥ 0 ⎥
− J 53
0
0
0
J 66 ⎦
⎥ ⎥
(6.37)
⎥
For a de echilibrare F din cupla cinematic de transla ie D, ce ac ioneaz pe direc ia eA ţ ă ţ ţ ă ţ axei z şi pune în mişcare elementul „n” în raport cu „n-1”, se obţine din relaţia: FeD = RDz
(6.38)
Analiza cinetostatică se efectuează pentru fiecare cuplă conducătoare în parte , pornind de la efectorul final. Torsorul forţelor exterioare (forţa gravitaţională, forţele tehnologice şi momentul rezultant al forţelor exterioare) ce încarcă elementul „n” şi cupla cinematică n-1Cn, permit determinarea reacţiunilor dinamice şi a forţei, respectiv momentului de echilibrare cu ajutorul c ăreia se poate alege motorul de antrenare liniar, respectiv rotativ. Forţa, respectiv momentul de echilibrare astfel determinat se va îngloba în torsorul forţelor exterioare (reacţiunea şi momentul reactiv ce încarcă elementul „n-1”), ceea ce permite determinarea reacţiunii şi forţei, respectiv momentului de echilibrare din cupla conducătoare
n-2
Cn-1 ş.a.m.d.
55 7.
MODELUL DINAMIC AL DISPOZITIVULUI DE GHIDARE AL ROBOŢILOR
În capitolele precedente am studiat modelul matematic al manipulării într-o celulă de fabricaţie flexibilă, modelele geometrice ale situării obiectului manipulat, modelul cinematic şi modelul cinetostatic al dispozitivului de ghidare al roboţilor, dar nu am considerat niciodată forţa ca o cauză a mişcării. În acest capitol vom considera ecuaţiile de mişcare ale roboţilor pe baza forţei, respectiv momentului de acţionare al motorului. Dinamica roboţilor va fi studiată pe structuri de roboţi având la bază lanţuri cinematice deschise. Dinamica roboţilor comportă două probleme:
⇒ prima problemă o reprezintă cazul când se impune un punct pe traiectorie, q, q& , &q& şi dorim să aflăm momentul motor Mm, cazul conducerea robotului;
⇒ a doua problemă este cum să calculăm mişcarea mecanismului la aplicarea unor forţe, respectiv momente motoare în cuple conducătoare. Deci se cunoaşte momentul de acţionare Mm şi se calculează q, q& , &q& , cazul simularea mişcării robotului. 7.1 Modelul Lagrange – Euler
Modelul dinamic al unui robot utilizând metoda Lagrange – Euler este simplă şi sistematică. Ecuaţiile mişcării ce se obţin sunt de ordinul doi, ecuaţii diferenţiale neliniare specifice mişcărilor cuplate. De precizat că pentru a simplifica aceste ecuaţii s-au neglijat dinamica conducerii şi forţele de frecare din angrenaje. Formalismul lagrangean se bazează pe funcţia lui Lagrange definită prin: L = Ec – Ep
(7.1)
56 Ecuaţiile dinamice clasice exprimate în termenii funcţiei lagrangene sunt: d ⎛ ∂L ⎞ ∂L ⎜ ⎟− = Qi dt ⎜⎝ ∂q& i ⎟⎠ ∂q i
i = 1, n
(7.2)
unde n este numărul de grade de mobillitate al robotului, qi – coordonata generalizată, q& – viteza generalizată, iar Qi – forţa generalizată. Ecuaţiile dinamice pentru un robot se obţin in 5 paşi: a. se calculează viteza liniară a tuturor punctelor materiale ale elementelor; b. se calculează energia cinetică Eci = 0,5 m . v2; c. se calculează energia potenţială Ep = m g h; d. se formează funcţia lui Lagrange L = Ec – Ep; e. se determină ecuaţia dinamică Q i =
d ⎛ ∂L ⎞ ∂L ⎜ ⎟− dt ⎜⎝ ∂q& i ⎟⎠ ∂q i
i = 1, n
a. Calculul vitezei unui punct al robotului Un punct material aparţinând elementului „i” este definit în raport cu sistemul de referinţă propriu prin vectorul de poziţie: ir
r = {x i y i z i 1}T
(7.3)
iar în raport cu sistemul de referinţă fix prin vectorul de poziţie: 0r
r = {x 0 y 0 z 0 1}T
(7.4)
Relaţia de legătură dintre cei doi vectori de poziţie: 0r 0
r
r = T i ⋅i r
iar viteza:
0
T i = 0 T i (q j ) q j = q j ( t ) j = 1, i
⎛ i ∂ 0 T i ⎞ i r ∂ i r& ( r)= 0 r = ⎜⎜ ∑ q& j ⎟⎟⋅ r ∂t ⎝ j=1 ∂q j ⎠
0r &
ir
r = cons tan t
(7.5) (7.6)
57 Pătratul vitezei se exprimă prin produsul scalar:
(0 rr& )2 =0 rr&⋅0 rr&
(7.7)
sau prin urma matricei produs (la o matrice pătratică suma pătratelor elementelor de pe diagonala principală): 2 0r &
r
r
r = TR 0 r& ⋅0 r& T
()
care conduce la:
(
(7.8)
)
T r 2 0 0 ⎛ d r ⎞ = TR ⎡⎢ i ∂ T i q& ⋅i rr ⋅ ⎛⎜ i ∂ T i q& ⋅i rr ⋅ ⎞⎟ ⎤⎥ = ⎜ ⎟ ∑ j ⎜ k∑=1 ∂q k k ⎟ ⎥ ⎢⎣ j=1 ∂q j ⎝ dt ⎠ ⎝ ⎠ ⎦
⎡ i i ∂ 0 T r r T ⎛ ∂ 0 T ⎞T ⎤ i i i⎟ & &k⎥ = TR ⎢∑∑ ⋅ r ⋅ (i r ) ⋅ ⎜⎜ q ⋅ q j ⎟ ⎢⎣ j=1 k =1 ∂q j ⎥⎦ ⎝ ∂q k ⎠
(7.9)
b. Calculul energiei cinetice Energia cinetică a unei particule de masă „dm” localizătă în elementul „i” prin r
vectorul de poziţie i r este: T r 2 ⎡ i i ∂0 T r r T ⎤ ⎛ ∂ 0 Ti ⎞ 1 dr i i ⎟ q& j ⋅ q& k ⎥ dE ci = ⎛⎜ ⎞⎟ ⋅ dm = TR ⎢∑ ∑ ⋅ r ⋅ (i r ) ⋅ dm⎜⎜ ⎟ 2 ⎝ dt ⎠ ⎢⎣ j=1 k =1 ∂q j ⎥⎦ ⎝ ∂q k ⎠
(7.10)
Energia cinetică a elementului „i” este suma energiilor cinetice ale tuturor punctelor materiale ale elementului „i”: T ⎡i i 0 ⎤ ⎛ 0 ⎞ r r T i i i i & & c ci j k E = ∫ dE = 12 TR ⎢⎢∑ ∑ ∂∂qT ⋅ ∫ r ⋅ ( r ) ⋅ dm⎜⎝ ∂∂qTk ⎟⎠ q ⋅ q ⎥⎥⎦ Mi ⎣ j=1 k =1 j Mi
(7.11)
Matricea de inerţie: Ji =
r
r T
∫ i r ⋅ (i r )
Mi
⋅ dm
(7.12)
58 sau:
⎧x i ⎫ ⎪y ⎪ ⎪ i⎪ ∫ ⎨ z ⎬ ⋅ {x i yi z i Mi ⎪ i ⎪ ⎪⎩ 1 ⎪⎭
⎡ ∫ x i2 dm ⎢ Mi ⎢ ∫ y i x i dm ⎢ 1}dm = ⎢Mi ⎢ Mi∫ z i x i dm ⎢ ⎢ ∫ x i dm ⎣ Mi
∫ x i y i dm ∫ x i z i dm ∫ x i dm⎤⎥ Mi Mi ∫ y i2 dm ∫ y i z i dm ∫ y i dm⎥⎥ Mi Mi Mi ⎥ ∫ z i y i dm ∫ z i2 dm ∫ z i dm ⎥ Mi Mi Mi ⎥ y dm z dm ∫ i ∫ i ∫ dm ⎥
Mi
Mi
Mi
Mi
(7.13)
⎦
Ţinând cont de momentele de inerţie axiale şi a centrifugale, în raport cu sistemul de
referinţă propriu elementului „i”: Ii xx =
∫
yi2 + z i2 dm
Mi
Ii yy =
Mi
∫ ( + )dm ∫ (x i2 + yi2 )dm z i2
x i2
Mi
Ii zz =
Ii xy = ∫ x i y i dm
Mi
Ii xz = ∫ x i z i dm
(7.14)
Mi
Ii yz = ∫ y i z i dm Mi
şi a momentelor statice mecanice:
∫ x i dm = M i x i
Mi
∫ y i dm = M i y i
Mi
∫ z i dm = M i z i
(7.15)
Mi
unde x i , y i , z i sunt coordonatele centrului de greutate al elementului „i”, în care este concentrată masa Mi. Întrucât: 1 1 1 ∫ x i2dm = − 2 ∫ (y i2 +) z i2 dm( + 2) ∫ z i2 (+ x i2 )dm + 2 ∫ x i2 + y i2 dm =
Mi
Mi
Mi
Mi
= 12 (− Ii xx + Ii yy + Ii zz ) 1 1 1 ∫ y i2 dm = − 2 ∫ (z i2 +) x i2 dm( + 2) ∫ x i2 (+ y i2 )dm + 2 ∫ y i2 + z i2 dm = Mi Mi Mi Mi 1 = (Ii xx − Ii yy + Ii zz ) 2
(7.16 a)
59 1 1 1 ∫ z i2 dm = − 2 ∫ (x i2 +) y i2 dm( + 2) ∫ y i2 (+ z i2 )dm + 2 ∫ z i2 + x i2 dm = Mi Mi Mi Mi 1 = (Ii xx + Ii yy − Ii zz ) 2
(7.16 b)
rezultă:
⎡ − Ii xx + Ii yy + Iizz ⎢ 2 ⎢ ⎢ Ii yx Ji = ⎢ ⎢ Ii zx ⎢ ⎢ Mi xi ⎣⎢
Ii xy
Ii xz
Ii xx − Ii yy + Ii zz 2
Ii yz Ii xx + Ii yy − Ii zz 2 Mi zi
Ii zy M i yi
⎤ Mi x i ⎥ ⎥ Mi yi ⎥ ⎥ ⎥ Mi zi ⎥ ⎥ M i ⎦⎥
(7.17)
ceea ce permite reformularea relaţiei: T ⎡∂0 T ⎤ ⎛ ∂ 0 Ti ⎞ 1 i i i ⎟ q& j ⋅ q& k ⎥ E c i = ∑∑ TR ⎢ ⋅ J i ⋅ ⎜⎜ ⎟ 2 j=1 k =1 ⎢ ∂q j ⎥⎦ ⎝ ∂q k ⎠ ⎣
(7.18)
Energia cinetică totală ale elementelor componente ale dispozitivului de ghidare: T ⎡∂0 T ⎛ ∂0 Ti ⎞ ⎤ 1 6 i i i ⎜ ⎟ ⎢ ⎥ ⋅ q& ⋅ q& E c = ∑ E c i = ∑∑∑ TR ⋅J ⋅ 2 i=1 j=1 k =1 ⎢ ∂q j i ⎜⎝ ∂q k ⎟⎠ ⎥ j k i=1 ⎣ ⎦ 6
(7.19)
c. Calculul energiei potenţiale Energia potenţială a elementului „i” de masă Mi, admiţând că nivelul de potenţial zero este planul xiOiyi al sistemului de referinţă propriu elementului „i”, este exprimată de relaţia: r
r
E p i = − M i ⋅ g T ⋅ 0 T i i rci unde vectorul acceleraţie gravitaţională este:
(7.20)
ir
g = {0 0 g i 1}T
(7.21)
60 iar vectorul de poziţie al centrului de greutate Ci al elementului „i”, în care este ir rci
concentrată masa Mi a acestui element este:
T
= {x i y i z i 1}
(7.22)
Energia potenţială totală a elementelor dispozitivului de ghidare este: 6
6
i =1
i =1
r
r
E p = ∑ E p i = −∑ M i ⋅ g T ⋅ (0 T i i rci )
(7.23)
d. Funcţia Lagrange L = Ec – Ep L=
(7.1)
T ⎡∂0 T 6 ⎛ ∂0 Ti ⎞ ⎤ 1 6 i i i ⎜ ⎟ ⎢ ⎥ ⋅ q& j ⋅ q& k + ∑ M i ⋅ gr T ⋅ (0 T i i rrci ) (7.24) TR ⋅ J ⋅ ∑∑∑ i ⎜ ⎟ 2 i=1 j=1 k =1 ⎢ ∂q j i =1 ⎝ ∂q k ⎠ ⎥⎦ ⎣
f. Ecuaţiile dinamice d ⎛⎜ ∂L ⎞⎟ ∂L − = Qi dt ⎜⎝ ∂q& p ⎟⎠ ∂q p
p = max i, j, k
T ⎡∂0 T ⎛ ∂0 Ti ⎞ ⎤ ∂L 6 i i ⎟ ⎥ ⋅ q& = ∑ ∑ TR ⎢ ⋅J ⋅⎜ ∂q& p i = p k =1 ⎢ ∂q p i ⎜⎝ ∂q k ⎟⎠ ⎥ k ⎣ ⎦ T ⎡∂0 T ⎛ ∂0 Ti ⎞ ⎤ d ⎛⎜ ∂L ⎞⎟ 6 i i ⎜ ⎟ ⎢ ⎥ ⋅ &q& + = ∑∑ TR ⋅J ⋅ dt ⎜⎝ ∂q& p ⎟⎠ i=p k =1 ⎢ ∂q p i ⎜⎝ ∂q k ⎟⎠ ⎥ k ⎣ ⎦ T ⎡ ∂2 0 T 6 i i ⎛ ∂ 0 Ti ⎞ ⎤ i ⎟ ⎥ ⋅ q& k ⋅ q& m + + ∑∑ ∑ TR ⎢ ⋅ J i ⋅ ⎜⎜ ⎟ ⎢⎣ ∂q p ∂q m i = p k =1 m =1 ⎝ ∂q k ⎠ ⎥⎦ T⎤ ⎡ 0 20 6 i i + ∑∑ ∑ TR ⎢⎢ ∂∂qT i ⋅ J i ⋅ ⎛⎜ ∂∂q ∂Tq i ⎞⎟ ⎥⎥ ⋅ q& k ⋅ q& m i = p k =1 m =1 ⎝ k m⎠ ⎦ ⎣ p
(7.25)
(7.26)
(7.27)
T ⎡ ∂2 0 T 6 ⎛ 0 ⎞ ⎛ ∂ 0 Ti ⎞ ⎤ ∂L 6 i i i ⎜ ⎟ ⎢ ⎥ ⋅ q& j ⋅ q& p + ∑ M i ⋅ gr T ⋅ ⎜ ∂ T i i rrci ⎟ (7.28) = ∑ ∑ ∑ TR ⋅ Ji ⋅ ⎜ ⎟ ⎜ ⎟ ∂q p i = p j=1 k =1 ⎢ ∂q j∂q p i=p ⎝ ∂q k ⎠ ⎥⎦ ⎝ ∂q p ⎠ ⎣
61 Având în vedere că ecuaţiile dinamice sunt independente de ordinea de sumare se evidenţiază următoarea formă: i
i
i
∑ Dij ⋅ &q&i + ∑∑ Dijk ⋅ q& j ⋅ q& k + D i = Q i j=1
i = 1,6
(7.29)
j=1 k =1
r& &q
r
unde &q&i - acceleraţia generalizată qr& j - viteza generalizată
i
= {&q&1 &q& 2 ... &q& n }T
qr& jqr& k = {q& 1q& 2 q& 1q& 3 ... q& n −1q& n }T r
T
q& 2k = {q& 12 q& 22 ... q& 2n }
(7.30)
D ij - este termenul inerţial: T ⎡∂0 T ⎛ ∂0 Tp ⎞ ⎤ p ⎟ ⎥ D ij = ∑ TR ⎢ ⋅ J ⋅⎜ ⎢ ∂q j p ⎜⎝ ∂q i ⎟⎠ ⎥ p = max i , j ⎣ ⎦ 6
(7.31)
pentru: i = j D ii este termenul inerţial al cuplei conducătoare „i”; pentru: i ≠ D ijj este cuplarea efectului inerţial între cuplele conducătoare „i” şi „j”; Termenul cinetic:
T ⎡ ∂2 0 T ⎛ ∂0 Tp ⎞ ⎤ p ⎜ ⎟ ⎢ ⎥ D ijk = ∑ TR ⋅J ⋅ ⎢ ∂q j∂q k p ⎜⎝ ∂q i ⎟⎠ ⎥ p = max i , j, k ⎣ ⎦ 6
(7.32)
pentru: j = k D ijj este termenul forţei centrifugale al cuplei conducătoare „i”, dependent de acceleraţia cuplei „j”; pentru j≠
D k ijk este termenul forţei Corriolis al cuplei conducătoare „i”, dependent de acceleraţia cuplelor „j” şi „k”;
Termenul gravitaţional D i ce încarcă cupla conducătoare „i”: 6 ⎛ ∂0 Tp p r ⎞ r Di = − ∑ M i ⋅ g T ⋅ ⎜ r ⎟ ⎜ ∂q i ci ⎟ p =1 ⎝ ⎠
i = 1,6
(7.33)
62 Matricele D depind de dimensiunea elementelor, de masele lor şi de masa manipulată, de poziţie centrelor de greutate, momentul de inerţie masic şi de poziţia momentană. Interdependenţa dintre parametri cinematici generalizaţi duce la o cuplare a mişcărilor. Termenii inerţiali şi gravitaţionali sunt importanţi în conducerea robotului, deoarece afectează stabilitatea şi precizia de situare. Termenii centrifugal şi Corriolis sunt importanţi dacă mişcarea are loc cu o viteză mare. 7.2 Modelul Newton – Euler
Se va considera fiecare element al dispozitivlui de ghidare ca un solid rigid. Dacă cunoaştem centrul de masă şi tensorul de inerţie al elementului, atunci distribuţia masei este complet caracterizată. Pentru a mişca elementul se impune accelerarea lui. Forţa de inerţie necesară este în funcţie de masa şi acceleraţia dorită a elementului. Ecuaţia lui Newton pentru mişcarea de translaţie şi ecuaţia lui Euler pentru mişcarea de rotaţie fac legătura dintre forţa de inerţie, masă şi acceleraţie. r
VCi
r
V& Ci Ci i
r
Fi Fig. 7.1 Acţiunea unei forţe în centrul de masă
În Fig. 7.1 un element avînd centrul de masă Ci va cauza accelerarea conform ecuaţiei lui Newton: unde Mi este masa elementului „i”.
r
r
Fi = M i ⋅ V& Ci
(7.34)
63 r
ω Ci
r
ω& Ci Ci i
r
Ni Fig. 7.2 Acţiunea unui moment în centrul de masă r
În Fig. 7.2 se arată un element rotindu-se cu viteza unghiulară ωi şi acceleraţia r
ω& i . În această situaţie, momentul Ni cu care va fii acţionat elementul, va cauza această mişcare conform ecuaţiei lui Euler: r
r
r
r
N i = Ci Ii ⋅ ω& i + ωi ×Ci I i ⋅ ωi
unde
(7.35)
Ci
Ii – tensorul inerţial, relativ la un sistem de axe cu centrul în Ci; Fi – rezultanta forţelor exterioare ce acţionează în centrul de masă; Ni – momentul rezultant al forţelor exterioare în raport cu centrul de masă Ci;
7.2.1 Metoda iterativă Newton – Euler
Stabilirea ecuaţiilor dinamice prin metoda iterativă, care pune în evidenţă variabilele generalizate, forţele generalizate motoare şi forţele de legătură dintre elementele componente ale robotului. a) Metoda iteraţiei de la baza robotului spr e efectorul final.Pentru fiecare element se determină viteza şi acceleraţia liniareşi unghiulare, respectiv for ţele şi momentele forţelor exterioare conform ecua ţiilor dinamice Newton – Euler. b) Metoda iteraţiei de la efectorul final spre baza robotului. Pentru fiecare element se determină forţele şi momentele forţelor de legătură dintre
64 elementele „i-1 – i” şi „i – i+1”, respectiv forţele generalizate motoare din cuplele cinematice conducătoare ale robotului. a) Metoda iteraţiei de la baza robotului spre efectorul final (i = 1…n) În ordine pentru calculul forţei de inerţie ce acţionează asupra elementelor, se calculează viteza de rotaţie, acceleraţia liniară şi unghiulară a centrului de masă al fiecărui element. Iteraţia porneşte cu elementul 1 şi continuă element cu element spre efectorul final „n”. Propagarea vitezei unghiulare de la element la element:
⎧1 daca i = rotatie r i = 1, n ωi = i R i −1 ⋅i −1 ωi −1 + Δ i q& i i k i unde Δ i = ⎨ ⎩0 daca i = translatie
ir
(7.36)
Viteza liniară a srcinii fiec ărui sistem al elementului „i”: i
r
V =i R i
i −1 i −1
r
r
r
V + i−1 ω ×i−1 r + Δ q& i k unde Δ =
[
i −1
i −1
i
]
i i
i
⎧ 1 daca i = rotatie
(7.37)
⎩⎨0 daca i = translatie
i
Acceleraţia unghiulare de la un element la altul: ir &
r
r
ωi = i R i −1 ⋅i −1 ω& i −1 + Δ i i R i −1⋅i −1 ω& i −1 × q& i i k i + &q&i i k i
(7.38)
Acceleraţia liniară a srcinii fiec ărui sistem al elementului „i”: r i &
Vi = i R i−1
r i −1 &
r
r
r
r
r
r
Vi−1 + i−1ω& i−1×i−1 ri + i−1 ωi−1 × (i−1 ωi−1×i−1 r )i + Δ i [2i ω& i × q& i i k i + &q&i i k i ] (7.39)
Acceleraţia centrului de masă: r iV & Ci
=
r iV &i
r
r
r
r
r
+ i ω& i ×i rCi + i ωi × (i ωi ×i rCi ) r
(7.40)
r
De notat că pentru elementul 0 se obţine 0 ω0 = 0 ω& 0 = 0 . Având calculate acceleraţiile liniare şi unghiulare ale centrului de masă al fiecărui element se aplică ecuaţiile lui
65 Newton – Euler pentru a calcula forţa de inerţie şi momentul necesar acţionării centrului de masă al fiecărui element. i
i
r
r
r
Fi = M i ⋅i V& Ci r
(7.41)
r
r
N i = Ci I i ⋅i ω& Ci + i ωi ×Ci Ii ⋅i ω& i
(7.42)
b) Metoda iteraţiei de la efectorul final spre baza robotului (i = n…1) Având calculate forţa şi momentul ce acţionează asupra fiecărui element, ne rămâne să calculăm forţa / momentul ce rezultă în fiecare articulaţie. Acestea se obţin prin scrierea unei ecuaţii de echilibru a fiecărui element. Asupra fiecărui element acţionează forţe şi momente exterioare, care sunt echilibrate de forţele şi momentele r
r
de legătură f i şi n i dintre elementele „i – 1, i” respectiv forţele şi momentele de r
r
legătură f i+1 şi n i+1 dintre elementele „i, i + 1” zi+1
i+1 zi
i
yi ir rCi
i +1 r
n i +1
ir ri+1
Ci (Mi, iIi)
yi+1 Oi+1 i +1
ir
ni i
r
Fi
Oi r
fi
i
xi i
r
Ni
i-1 Fig. 7.3 Starea de echilibru al elementului „i” r
f i – forţa exercitată de elementul ”i” asupra elementului „i-1”; r
n i – momentul exercitat de elementul ”i” asupra elementului „i-1”.
xi+1
r
f i +1
66 Sumând forţele ce acţionează asupra elementului ”i” scriem de fapt ecuaţia de echilibru a elementului ”i”: i
r
r
r
r
r
Fi = i f i − i f i +1 =i f i − i R i +1 ⋅i +1 f i +1
(7.43)
Sumând momentele ce acţionează în raport cu centrul de masă se ob ţine ecuaţia de echilibru a momentelor: i
r
r
r
r
r
r
r
r
r
r
r
r
N i = i n i − i n i+1 − i rCi ×i f i − (i ri+1 − i rCi )×i f i+1 = r
r
r
r
= i n i − i n i+1 − i rCi ×i f i − i ri+1×i f i+1 + i rCi ×i f i+1 = r r r r r r r = i n i − i n i+1 − i rCi × (i f i − i f i+1 )− i ri+1×i f i+1 = r
r
r
r
(7.44)
r
r
= i n i − i R i+1 ⋅i+1 n i+1 + i rCi ×i Fi − i ri+1×i R i+1 ⋅i+1 f i+1 În final se pot rearanja relaţiile de echilibru ale forţei şi momentelor astfel ca ele să devină relaţii iterative, de la elementul „n la 1”. i
r
r
r
f i = i Fi + i R i +1 ⋅i +1 f i +1
in ri
=
r i Ni
+
r i R i + 1 i +1 n r i +1 i r rCi i Fi
⋅
+
× +
r ir ri +1 i R i +1 i +1 f i +1
×
(7.45)
⋅
Forţa generalizată motoare iQm din cupla cinematică conducătoare este: i
unde pentru: i
r
r
r
r
r
r
Q m = Δ i ⋅i n iT ⋅i k i + (1 − Δ i )⋅i f iT ⋅i k i + i Q f i
Δ i = 1 daca i = rotatie Δ i = 0 daca i = translatie
(7.46)
r
r
r
r
r
r
r
r
Q m = i n iT ⋅i k i + i Q f i
Q m = i f iT ⋅i k i + i Q f
r
Q f - este forţa generalizată datorită frecărilor vâscoase, având expresia: i
r
i
r
Q f = b i ⋅ q& i ⋅ Q fc bi - coeficient de frecare vâscoasă; i
(7.47)
r
Q fc - este forţa generalizată datorită frecărilor uscate (coulombiene), având
expresia:
(7.48)
67 r r d r r i Q fc = Δ i ⋅ ci ⋅ i ⋅ i k i ×i f i ⋅ sign (q& i ) + (1 − Δ i ) ⋅ ci ⋅ i k i ×i f i ⋅ sign (q& i ) 2 r d r r i Δ i = 1 daca i = rotatie Q fc = ci ⋅ i ⋅ i k i ×i f i ⋅ sign (q& i ) 2 unde pentru: r r r i Δ i = 0 daca i = translatie Qfc = ci ⋅ i k i ×i f i ⋅ sign (q& ) ci - coeficient de frecare uscată; di – diametrul fusului din cupla de rotaţie. r
(7.49)
(7.50)
7.3 Modelul Apple
Ecuaţiile lui Apple permit exprimarea dinamicii sistemelor mecanice. Ecuaţiile lui Apple au fost introduse pentru a descrie dinamica sistemelor non - holonomice. In continuare se tratează utilizarea ecuaţiilor lui Apple pentru sisteme de rigide cuplate. Se consideră un sistem de N particule notate cu l = 1, 2, …, N. Fie ca particula l să r
aibă masa ml şi vectorul de poziţie rl . Atunci poziţia sistemul este definită prin r r
r
vectorii: r1 , r2 , L, rN . Fiecare vector de poziţie se poate proiecta într-un sistem de referinţă cartezian având proiecţiile:
x1, x2, x3,…..x3N-2, x3N-1, x3N.
Deci poziţie sistemului de particule (solidul rigid) este definită printr-un vector de dimensiunea 3N
r
x = (x1 , x 2 , x 3 , ………., x 3N ) T .
Să luăm în considerare constrângerile impuse sistemului: f(x, t) = 0
sau
f(x) = 0
(7.51)
numiteconstrângeri geometricesau constrângeri holonomice . Dacă un sistem este în mişcare şi este supus numai la astfel de constrângeri atunci vorbimsisteme de holonomice. Dacă constrângerile au o formă neintegrabilă: 3N
∑ A ν (x, t ) ⋅ dx ν + A ⋅ dt = 0
ν =1
sau
3N
∑ A ν ( x ) ⋅ dx ν = 0
ν =1
(7.52)
68 atunci acestea se numesc constrângeri non – holonomice. Dacă ele sunt impuse unui sistem atunci vorbim de sisteme non – holonomice. Constrângerile la care timpul nu este exprimat în mod explicit se cheamă scleronome sau constrângeri statice. Dacă este explicitată dependenţa de timp, aceste constrângeri sunt reonome sau constrângeri dinamice. Considerăm un sistem de particule în mişcare supus la k constrângeri holonomice (geometrice): f μ ( x, t ) = 0
μ = 1,2,L, k
(7.53)
Conform principiului lui d’Alembert, dinamica unui sistem holonomic se poate descrie prin ecuaţiile diferenţiale: r
r
r
Fl − m l&r&l + R l = 0
(7.54)
r
unde: Fl - rezultanta forţelor active ce acţionează asupra particulelor ml; r
R l - rezultanta forţelor reactive pe care le impun constrângerile. Numărul de ecuaţii necesare pentru a rezolva dinamica unui asemenea sistem este mai mare decât numărul de grade de liberatate ale sistemului. Sistemul are n = 3N + k grade de libertate, iar aici avem nevoie de 3N+k ecuaţii scalare (3N ecuaţii r
diferenţiale de tipul (7.54), pentru că sunt rN vectori de poziţie cu câte 3 proiecţii şi k ecuaţii de legătură de tipul (7.53) aferente fiecărei constrângeri în parte). Se pune întrebarea care este setul de ecuaţii minimal pentru a descrie dinamica sistemului. În acest scop pentru a deriva ecuaţiile lui Apple se utilizează metoda deplasărilor virtuale: r
r
(Fl − m l&rr&l + R l )⋅ δrrl = 0 r
unde δ rl - este deplasarea virtuală a particulei „l”.
l = 1, N
(7.55)
69 Dacă sumăm pentru toate particulele l = 1, N : N r
r
r
r
∑ (Fl − m l&r&l + R l )⋅ δrl = 0
(7.56)
l =1
Vom lua în considerare numai constrângerile geometrice ideale pentru care r
r
r
r
R l ⋅ δ rl = 0 , adică R l este perpendicular pe δ rl , atunci: N r
r
r
∑ (Fl − m l&r&l )⋅ δrl = 0
(7.57)
l =1
Această ecuaţie uneşte principiul lui d’Alembert şi principiul deplasărilor virtuale al lui Lagrange. De aceea de multe ori se cheamă ecuaţiile lui d’Alembert - Lagrange. Introducem proiecţiile carteziene cu următoarele notaţii: r r1 r
r
r
= (x1 , x )2 ,( x 3 , r)2 = x( 4 , x 5 , x 6 ,L), rN = x 3 N − 2 , x 3N −1 , x 3 N r r F1 = (P1 , P)2 , (P3 , F)2 = P(4 , P5 , P6 , L ,)FN = P3 N − 2 , P3N −1 , P3N
(7.58)
şi o renumerotare a maselor:
m1 → m1 , m 2 , m 3 m 2 → m 4 , m 5 , m 6
L
m N → m 3 N − 2 , m 3 N −1 , m 3N
(7.59)
Cu aceste notaţii ecuaţia (7.57) ia forma: 3N
∑ (Pν − mν &x& ν ) ⋅ δx ν = 0
(7.60)
ν =1
Sistemul de N particule supus la k constrângeri are n = 3N – k grade de libertate, iar coordonatele x1, x2, …, x3N nu sunt independente datorită constrângerilor. Se definesc n parametri independenţi q1, q2, …, qn, care determină poziţia sistemului şi care se cheamă coordonate generalizate. Deci fiecare coordonată carteziană x ν se poate exprima în funcţie de aceste coordonate generalizate şi timp: x ν = x ν (q1 q 2
L
q)n )t ( q i = q i t
i = 1, n
(7.61)
70 Pentru un sistem staţionar timpul t nu este explicit exprimat. Deplasarea virtuală δx ν se exprimă prin relaţia: n ∂x ν ⋅ δq i =∑ a νi ⋅ δq i i =1 ∂q i i =1 n
δx ν = ∑
a νi =
∂x ν ∂q i
(7.62)
Înlocuind relaţia (7.62) în relaţia (7.60) 3N
n
∑ (Pν − mν &x& ν ) ⋅ ∑ a νi ⋅ δq i = 0
ν =1 3N
i =1
n
3N
n
∑ Pν ⋅ ∑ a νi ⋅ δq i − ∑ m ν &x& ν ⋅ ∑ a νi ⋅ δq i = 0
ν =1 i =1 n 3N
ν =1
(7.63)
i =1 n 3N
∑ ∑ m ν ⋅ &x& ν ⋅ a νi ⋅ δq i = ∑ ∑ Pν ⋅ a νi ⋅ δq i i =1 ν =1
i =1 ν =1
Termenul din membrul drept reprezintă lucrul mecanic virtual al tuturor forţelor active ce acţionează asupra sistemului. Introducând notaţia: 3N
ν νi Q i = ν∑ =1 P ⋅ a
i = 1, n
(7.64)
Lucrul mecanic virtual primeşte forma: n
dA = ∑ Q i ⋅ δq i
(7.65)
i =1
unde Qi este forţa generalizată corespunzătoare coordonatelor generalizate. Pentru a transforma membrul stâng al ecuaţiei (7.63 – ultima linie), se derivă expresia (7.61): n
x& ν = ∑
∂x ν
⋅ q& i +
∂x ν
n
=∑ a νi ⋅ q& i +a νi
aν =
∂x ν
∂q i ∂t i =1 ∂t n n ∂a n ∂a n ∂a ∂a ν i ν i &x& ν = ∑ a νi ⋅ &q&i + ∑ ∑ ⋅ q& i ⋅ q& j + ∑ ⋅ q& j + ∑ νi ⋅ q& i + ν ∂t i =1 i =1 j =1 ∂q i j =1 ∂q j i =1 ∂t i =1 n
De unde se observă că a νi se poate exprima şi sub forma:
(7.66)
71
δ&x& a νi = ν δ&q&i
(7.67)
Revenind la membrul stâng al relaţiei (7.63 – ultima linie) se obţine: n 3N
n 3N
i =1 ν =1
i =1 ν =1
δ&x&
∑ ∑ m ν ⋅ &x& ν ⋅ a νi ⋅ δq i = ∑ ∑ m ν ⋅ &x& ν ⋅ δ&q&ν ⋅ δq i
(7.68)
i
Introducând funcţia S: 1 3N S = ⋅ ∑ m ν ⋅ &x& ν2 2 ν =1
(7.69)
numită energia de accelerare. Este evidentă relaţia: 3N
∂&x&
∂S
∑ m ν ⋅ &x& ν2 ⋅ ∂&q&ν = ∂&q&
ν =1
i
i = 1, n
(7.70)
i
Deci membrul stâng se poate scrie şi sub forma: n ∂S i i i sau i =1 ∂&q&i δq = ∑ i =1 Q ⋅ δq ∑ n
⎛ ∂S ⎞ i & & q Q i =1 ⎜ ⎝ ∂ i − ⎟⎠δq i = 0 ∑ n
(7.71)
Deoarece deplasare virtuală δq i este o variabilă independentă, rezultă drept consecinţă că forma finală a ecuaţiilor lui Apple:
∂S = Qi ∂&q&i
i = 1, n
(7.72)
Deci problema modelului dinamic se reduce la exprimarea energiei de acceleraţie S în funcţie de coordonatele generalizate. Aplicând ecuaţiile lui Apple la un robot, rezultă că este suficient să calculăm energia de acceleraţie a unui element (corp rigid) şi să sumăm toate elementele lanţului cinematic pentru a determina funcţia S.
72 De exemplu funcţia S este exprimată în funcţie de coordonatele generalizate qi, utilizând o expresie de recurenţă a acceleraţiei. Pornind de la expresia care consideră
⎛ d 2 rr ⎞ 1 N S = ⋅ ∑ m l ⋅ ⎜⎜ 2l ⎟⎟ 2 l=1 ⎝ dt ⎠
corpul rigid ca un sistem de particule:
2
(7.73)
r
Dacă introducem centrul de masă cu vectorul său de poziţie rc , atunci: r rl
r
r
= rc + rl'
(7.74)
r
unde rl' este vectorul de poziţie al particulei ml, raportat la centrul de masă Cl. 2 2 r r r 2 ⎛ d 2 rr d 2 rr ⎞ 1 N ⎡⎛ d 2 rr ⎞ d2 r d2 r ⎛ d2 r ⎞ ⎤ 1 N S = ⋅ ∑ m l ⋅ ⎜⎜ 2c + 2l' ⎟⎟ = ⋅ ∑ m l ⎢⎜⎜ 2c ⎟⎟ + 2 2c ⋅ 2l' + ⎜⎜ 2l' ⎟⎟ ⎥ = 2 l=1 dt ⎠ 2 l=1 ⎢⎣⎝ dt ⎠ dt dt ⎝ dt ⎠ ⎥⎦ ⎝ dt (7.75) r 2 r r r 2 d 2 rl' 1 N ⎛⎜ d 2 rl ' ⎞⎟ 1 ⎛⎜ d 2 rc ⎞⎟ d 2 rc N = ⋅ M⎜ 2 ⎟ + 2 ∑ m l ⋅ 2 + ⋅ ∑ m l ⎜ 2 ⎟ 2 ⎝ dt ⎠ 2 l=1 ⎝ dt ⎠ dt l=1 dt N l ă a corpului rigid. unde M = ∑ l=1 m estemasatotal N
∑ ml ⋅
Expresia
l=1
conduce la:
N
r
d 2 rl' d 2 ⎛ N r ⎞ = ⎜ ∑ m l ⋅ rl ' ⎟ dt 2 dt 2 ⎝ l=1 ⎠
r
∑ m l ⋅ rl' = 0
în raport cu centrul de masă Cl.
(7.76) (7.77)
l=1
Utilizând relaţiile (7.76) şi (7.77) în relaţia (7.75) obţinem: r r 1 1 N S = ⋅ M ⋅ a c 2 + ⋅ ∑ m l ⋅ a l2' 2 2 l=1
(7.78)
r
unde: a c este acceleraţia centrului de masă; r
a l' este acceleraţia centrului de masă ml în mişcarea s-a relativă raportată la centrul de masă.
73 7.4 Modelul dinamic simplificat
Acest model decuplează axele robotului, luând în considerare numai o singură axă şi neglijând influenţele reciproce ale axelor dispozitivului de ghidare al robotului. Fiecare axă, fiecare modul, fiecare cuplă cinematică conducătoare se tratează separat ca şi un sistem mecanic izolat compus dintr-un motor, o transmisie mecanic ă şi un consumator. Mred M
TM
C
Mm Fig. 7.4 Componenţa unei axe a robotului
Calculele se efectuează pentru cupla cinematică conducătoare „i, i+1”. Se iau în considerare ca elemente mobile, elementele „i+1 .. n”. Lanţul cinematic „i+1 .. n”, conţinând şi efectorul final cu obiectul manipulat, se vor considera în poziţia cea mai dezavantajoasă, când momentul rezistent redus Mred este maxim. Toate reducerile se fac la arborele motorului electric de acţionare rotativ, sau la tija pistonului motorului pneumatic / hidraulic liniar. a) Cupla cinematică conducătoare de rotaţie „i, i+1”.
Se impune ca la arborele motorului electric rotativ ă se dezvolte s o putere motoare capabil ă să învingă puterea momentelor rezistente (gravita ţionale, de frecare, de iner ţie şi tehnologice). Pm 〉 Pred ωm ⋅ M m 〉 ωm ⋅ M red ∀ ωm ≠ 0 ⇒ M m 〉 M red M red = M red + M red + M red + M red g
fr
in
(7.79)
tehn
unde: M red g - momentul rezistent redus al forţelor gravitaţionale; M red fr - momentul rezistent redus al forţelor de frecare; M red in - momentul rezistent redus al forţelor de inerţie; M red tehn - momentul rezistent redus al forţelor tehnologice, care se calculează numai dacă robotul este purtător al unei scule sau al unui cap de forţă.
74 Momentul rezistent redus al forţelor gravitaţionale al elementelor lanţului cinematic „i+1 .. n” se determină din condiţia echilibrării puterilor consumate în sistem de forţele de greutate: n
∑ G j ⋅ VC j ⋅ cos α j
Pred g = M red g ⋅ ωm = M red g
1 = ωm
j=i +1
(7.80)
n j=i +1 G
∑
j
⋅ VC j ⋅ cos α j
unde: Gj – forţa de greutate a elementului j; VCj – viteza centrului de greutate al elementului j; αj – unghiul de presiune.
j
r
VCj
Cj
mj
αj r
G Cj Fig. 7.5 Schema de calcul al
M red g
Momentul rezistent redus al forţelor de frecare se determină din condiţia compensării puterilor disipate în sistem de for ţele de frecare, care acţionează în fiecare cuplă cinematică a lanţului cinematic „i+1 .. n” (inclusiv în cuplele cinematice ale transmisiei mecanice şi ale motorului rotativ). Pred fr = M red fr ⋅ ωm = M red fr
m
n
j=i +1
k = m+1
∑ Ffr j ⋅ Vj + ∑
M fr k ⋅ ωk
n ⎞ 1 ⎛⎜ m = F ⋅ V + M fr k ⋅ ωk ⎟⎟ ∑ ∑ fr j ⎜ j ωm1 ⎝ j=i+1 k = m +1 ⎠
(7.81)
75 Momentul rezistent redus al forţelor de inerţie al lanţului cinematic „i+1 .. n” se determină din relaţia: n
2 ∑ E Cj M red in = J red ⋅ ε m
J red =
j=i +1 2 m
(7.82)
ω
unde: Jred - momentul de inerţie masic redus se determină din condiţia ca energia cinetică înmagazinată de întregul sistem să fie înmagazinată şi de arborele motor (arborele de reducere). n 1 1 m j ⋅ VCj2 + ∑ J Ck ⋅ ω2k j= i +1 2 k = m +1 2
n
m
∑ E Cj = ∑
j= i +1
(7.83)
unde: mj – masa elementului j concentrată în centru de greutate Cj aflat în mişcare de translaţie cu viteza VCj; JCk – momentul de iner ţie masic al elementului k în raport cu axa de rotaţie (luându-se în calcul şi elementele transmisiei mecanice, inclusiv rotorul motorului electric) în mişcare de rotaţie cu viteza unghiulară ωk. J red =
n ⎞ 1 ⎛⎜ m m ⋅ VCj2 + ∑ J Ck ⋅ ω2k ⎟⎟ 2 ⎜ ∑ j ωm ⎝ j=i+1 k = m +1 ⎠
(7.84)
ω ωreg
tacc
tfr
t
T Fig. 7.6 Variaţia vitezei unghiulare
76 Acceleraţia unghiulară εm a arborelui motor pentru un profil trapezoidal al vitezei unghiulare ωm a arborelui motor se exprimă prin relaţia:
εm =
ωm kT
(7.85)
unde k < 1 exprimă ponderea tacc, respectiv tfr, în timpul total, respectiv: t acc = t fr = kT T=
Ψj (1 − 2k ) ⋅ ωk reg
(7.86) (7.87)
unde: Ψ j – cursa unghiulară a mişcării relative a elementului în mişcare de rotaţie. b) Cupla cinematică conducătoare de translaţie „i, i+1”.
Se impune la nivelul tijei motorului liniar ă se s dezvolte o putere motoare capabil ă să învingă puterea forţelor rezistente (gravita ţionale, de frecare, de iner ţie şi tehnologice). Pm 〉 Pred ωm ⋅ Fm 〉 ωm ⋅ Fred ∀ ωm ≠ 0 ⇒ Fm 〉 Fred Fred = Fred g + Fred fr + Fred in + Fred tehn
(7.88)
unde: Fred g - forţa rezistentă redusă a forţelor gravitaţionale; Fred fr - forţa rezistentă redusă a forţelor de frecare; Fred in - forţa rezistentă redusă a forţelor de inerţie; Fred tehn - forţa rezistentă redusă a forţelor tehnologice, care se calculează numai dacă robotul este purtător al unui cap de forţă. Forţa rezistentă redusă a forţelor gravitaţionaleal elementelor lanţului cinematic„i+1 .. n” se ţele de greutate: determină din condiţia echilibrării puterilor consumate în sistem de for
Pred g = Fred g ⋅ Vm = Fred g
1 = Vm
n
∑ G j ⋅ VC j ⋅ cos α j
j=i+1 n
∑ G j ⋅ VC j ⋅ cos α j
j=i +1
(7.89)
77 unde: Gj – forţa de greutate a elementului j; VCj – viteza centrului de greutate al elementului j; αj – unghiul de presiune.
j
r
VCj
Cj
mj
αj r
G Cj Fig. 7.7 Schema de calcul al
Fred g
Forţa rezistentă redusă a forţelor de frecare se determină din condiţia învingerii puterilor disipate în sistem de for ţele de frecare, care acţionează în fiecare cuplă cinematică a lanţului cinematic „i+1 .. n” (inclusiv în cuplele cinematice ale transmisiei mecanice şi ale motorului liniar). Pred fr = Fred fr ⋅ Vm = Fred fr
m
n
j=i +1
k = m +1
∑ Ffr j ⋅ Vj + ∑
M fr k ⋅ ωk
n ⎞ 1 ⎛⎜ m = F ⋅ V + M fr k ⋅ ωk ⎟⎟ ∑ ∑ fr j ⎜ j Vm1 ⎝ j=i+1 k = m +1 ⎠
(7.90)
Forţa rezistentă redusă a forţelor de inerţie al lanţului cinematic „i+1 .. n” se determină din relaţia: n
Fred in = m red ⋅ a m
m red =
2 j=∑i+1E Cj Vm2
(7.91)
unde: mred - masa redusă se determină din condiţia ca energia cinetică înmagazinată de întregul sistem să fie înmagazinată şi de tija motoare (arborele de reducere).
78 n
m
n
1 1 m j ⋅ VCj2 + ∑ J Ck ⋅ ω2k 2 j= i +1 k = m +1 2
∑ E Cj = ∑
j= i +1
(7.92)
unde: mj – masa elementului j concentrată în centru de greutate Cj aflat în mişcare de translaţie cu viteza VCj; JCk – momentul de inerţie masic al elementului k în raport cu axa de rotaţie, (luându-se în calcul şi elementele transmisiei mecanice, inclusiv rotorul motorului electric) în mişcare de rotaţie cu viteza unghiulară ωk. n ⎞ 1 ⎛ m m red = 2 ⎜⎜ ∑ m j ⋅ VCj2 + ∑ J Ck ⋅ ω2k ⎟⎟ Vm ⎝ j=i+1 k = m +1 ⎠
(7.93)
V Vreg
tacc
tfr
t
T Fig. 7.8 Variaţia vitezei liniare
Acceleraţia liniară am a tijei motoare pentru un profil trapezoidal al vitezei liniare Vm a tijei motoare se exprimă prin relaţia: am =
Vm kT
unde k < 1 exprimă ponderea tacc, respectiv tfr, în timpul total, respectiv: t acc = t fr = kT T=
hj (1 − 2k ) ⋅ Vj reg
unde: h – cursa liniară a mişcării relative a elementului în mişcare de translaţie.
(7.94)
(7.95) (7.96)
79 c) Determinare momentului de frânare ω ωm
t12
t
t2
Fig. 7.9 Variaţia vitezei unghiulare la frânare
Pentru o cuplă cinematică conducătoare „i, i+1” acţionată de un motor rotativ, se pune condiţia ca pe durata frânării viteza unghiulară să scadă la zero în momentul t2. Energia cinetică în momentul începerii frânării, t12 este: 1 E C = J red ⋅ ω2m 2 2
2
t 12
t 12
E C = t∫ M red ⋅ dϕ = M red t∫ dϕ
sau unde:
(7.97) (7.98)
M red = M red g + M red fr + M red tehn + M fr red t2
t2
t 12
t 12
∫ dϕ =
1
dϕ = ωm dt
1
∫ ωmdt = 2 ωm (t 2 − t12 ) = 2 ωm ⋅ k ⋅ T
1 2E C E C = M red ⋅ ωm ⋅ k ⋅ T M red = 2 ωm ⋅ k ⋅ T M fr red = M red − M red g − M red fr − M red tehn
(7.99) (7.100) (7.101) (7.102)
2E C − M red g − M red fr − M red tehn (7.103) ωm ⋅ k ⋅ T Pentru alegerea frânei se calculează momentul sau forţa de frânare care trebuie M fr red =
dezvoltată. Pentru o cuplă cinematică conducătoare acţionată de un motor rotativ: M fr = M fr red
ωm ωfr
Ffr = M fr red
ωm Vfr
(7.104)
80 d) Determinare forţei de frânare
V Vm
t12
t2
t
Fig. 7.10 Variaţia vitezei liniare la frânare
Pentru o cuplă cinematică conducătoare „i, i+1” acţionată de un motor liniar, se impune aceeaşi condiţie ca pe durata frânării viteza motorului să scadă la zero în momentul t2. Energia cinetică în momentul începerii frânării, t12 este: 1 E C = m red ⋅ Vm2 2 2
2
t 12
t 12
E C = t∫ Fred ⋅ ds = Fred t∫ ds
sau unde:
(7.105)
Fred = Fred g + Fred fr + Fred tehn + Ffr red t2
t2
t 12
t 12
∫ ds =
1
(7.106) ds = Vm dt
1
∫ Vmdt = 2 Vm (t 2 − t12 ) = 2 Vm ⋅ k ⋅ T
2E C 1 E C = Fred ⋅ Vm ⋅ k ⋅ T Fred = 2 Vm ⋅ k ⋅ T Ffr red = Fred − Fred g − Fred fr − Fred tehn
(7.107) (7.108) (7.109) (7.110)
2E C −F −F −F (7.111) Vm ⋅ k ⋅ T red g red fr red tehn Pentru alegerea frânei se calculează momentul sau forţa de frânare care trebuie Ffr red =
dezvoltată. Pentru o cuplă cinematică conducătoare acţionată de un motor liniar: M fr = Ffr red
Vm ωfr
Ffr = Ffr red
Vm Vfr
(7.112)
82 8. COMANDA ŞI CONTROLUL ROBOŢILOR 8.1. Sarcina şi funcţiile sistemului de comandă 8.1.1. Structura robotului
Robotul se consideră ca un sistem de rangul R, compus din subsisteme de rang R-1. Structura unui robot este prezentată în figura 8.1. SISTEM DE COMANDĂ
SISTEM DE ACŢIONARE
SENZORI TRADUCTOARE APARATE DE MASURĂ
SISTEM DE ENERGIZARE A FLUIDULUI
SISTEM MECANIC
M
EFECTOR FINAL
E
PLATFORMĂ MOBILĂ
D I U
SISTEM DE CONDUCERE
Fig. 8.1 Schema structurală a robotului Sistemul mecanic este compus din mecanisme, care asigură mişcarea efectorului
final în situările sale pe traiectoria programată. Platforma mobilă se regăseşte numai în structura roboţilor mobili. Sistemul mecanic preia forţele generalizate ce acţionează din mediu asupra robotului. Sistemul de acţionare impune mişcarea relativă a elementelor mecanismelor
sistemului mecanic. Sistemul de acţionare are ca şi componente câte un element de acţionare pentru fiecare cuplă cinematică conducătoare a mecanismelor sistemului mecanic.
83 Sistemul de energizare a fluidului se utilizează la roboţi în cazul sistemelor de
acţionare hidraulice, respectiv pneumatice cu compresor propriu, deci a celora care folosesc ca purtător de energie un fluid. Sistemul de comandă prelucrează informaţiile despre starea mediului (externo -
percepţie) şi despre starea interioară a robotului (proprio - percepţie) şi emite , în conformitate cu aceste informaţii şi propriul program, comenzi către sistemul de acţionare. Senzorii şi traductoarele sunt elemente de inteligenţă artificială ce culeg informaţii (senzori culeg informaţii care nu sunt legate de deplasări – provenite din mediu –, iar traductoarele culeg informaţii legate de deplasări ale subsistemelor robotului. Sistemul de conducere al robotului, reuneşte sistemul de comandă, sistemul de acţionare şi elementele de inteligentă artificială. Sarcina principală a sistemului de comandă este de a transpune programele de aplicaţie în mişcări relative al elementelor cuplelor cinematice conducătoare ale dispozitivului de ghidare. Comanda mişcării este prima sarcina a sistemului de comandă şi constă în: a) coordonarea şi supravegherea (controlul) mişcării efectorului final prin situările programate, sau a punctului caracteristic M pe traiectoriile programate; b) sincronizarea desfăşurării mişcărilor robotului ce evenimentele care au loc la periferia robotului prin prelucrarea semnalelor externe preluate din procesul de producţie (maşini unelte, dispozitive de lucru etc.)
84 c) prelucrarea datelor de la senzorii externi (sisteme de prelucrarea imaginilor) şi transpunerea rezultatelor prelucrate în reacţiile dorite. Programele de aplicaţie arată într-o formă interpretabilă sarcinile pe care le are de executat robotul. Aceste programe au fost concepute „on-line” de către un programator, care de cele mai multe ori a folosit dispozitivul de ghidare şi sistemul de comandă al robotului, în regim de instalaţie de teleoperare. De aceea se impune a doua sarcină a sistemului de comandă şi anume programarea robotului. Prin programarea robotului se realizează în primul rând dialogul cu operatorul uman, căruia i se pun la dispoziţie comenzile şi funcţiile pentru conceperea, corectarea şi testarea programului de mişcare, precum şi dispozitivele de: introducere, scoatere şi arhivare a programului. Sarcinile sistemului de comandă sunt realizate de cele două componente ale sale: a) subsistemul hardware bazat pe microprocesor; b) subsistemul software. În cazul sistemelor de comandă moderne are loc o conlucrare a celor două componente permiţând: a) comanda manuală a robotului; b) comanda automată a robotului; c) comanda adaptivă a robotului; d) iniţializarea robotului (aducerea lui în poziţia de referinţă); e) programarea robotului, testarea şi corectarea programelor.
85 8.2. Comanda mişcării robotului
Principala sarcină a unui robot este aceea de execuţie automată a mişcărilor programate. Comanda mişcării robotului se realizează pe nivele ierarhice.
PROGRAM
Senzor de temperatură
Traductoare Poziţie Viteză
Senzor audio Senzor video
Nivel înalt
Planificarea mişcării
Nivel mediu
Generarea traiectoriei
Nivel de bază
Execuţia mişcării
Interfaţa pentru senzori şi traductoare
Senzor tactil
Senzor de forţă / moment
1 2
6
Fig. 8.2 Comanda mişcării robotului
Pe nivelul înalt se planifică mişcarea sistemului mecanic pe baza programului de aplicaţie şi în acord cu informaţia senzorială şi a traductoarelor. Pentru obţinerea informaţiilor din mediul de operare al robotului (externo – percepţie) se folosesc senzori: tactili, de forţă /moment, vizuali (camere video – CCD), auditivi (microfoane), de temperatură etc. Prin senzor se înţelege un dispozitiv care preia o anumită informaţie prin intermediul unui receptor, îl transformă într-un semnal , de
86 obicei electric, prin intermediul unui convertor şi îl transmite mai departe sistemului de comandă, eventual amplificat şi prelucrat. Pentru obţinerea informaţiilor despre stare internă a robotului (proprio – percepţie) se folosesc traductoare care măsoară poziţia şi viteza generalizată, relativă a elementelor cuplelor cinematice conducătoare. La nivelul mediu are loc generarea traiectoriei, adică se determină punctele de precizie prin care trece traiectoria punctului caracteristic între punctul START şi punctul ŢINTĂ. Se realizează de asemenea o coordonare a mişcării axelor robotului (viteze şi acceleraţii generalizată) astfel că aceste puncte să poată fi atinse. La nivelul de bază, execuţia mişcării, se controlează mişcarea relativă a elementelor fiecărei cuple cinematice conducătoare. Axele energetice aferente acestuia sunt controlate digital. 8.2.1. Transformări de coordonate
Descrierea unei mişcări complexe se poate realiza direct în coordonate generalizate (coordonate specifice robotului – joint coordinate), adică în variabilele cuplelor q q
q
3
q
2
q
z
Z
5
c
C 4
q
T
T
x
6
a
B
1
A a)
X
O b)
Y
c)
Fig. 8.3 Sisteme de coordonate a) coordonate generalizate (ROBOT); b) coordonate operaţionale(carteziene ataşate bazei robotului); c) coordonate operaţionale (carteziene ataşate efectorului final)
b T y
87 cinematice conducătoare. Acest lucru este însă dificil de sesizat de către operatorul uman. De aceea situarea efectorului final în sistemul de coordonate operaţionale ataşat bazei robotului sau a efectorului final este mai uşor de sesizat de către operatorul uman, deoarece ambele sisteme sunt carteziene. În sistemul de coordonate ataşat efectorului final, mişcările sunt relative deoarece srcinea T – tool point a sistemului de coordonate se modifică. Acest sistem este preferat în cazul operaţiilor de montaj. Sistemele de comandă avansate dau posibilitatea utilizatorului, ca în faza de programare să poată alege între coordonatele generalizate şi cele operaţionale (având srcinea la baza robotului sau la efectorul final). Acest lucru simplific ă conducerea manuală a robotului, operatorul putând utiliza taste de direcţionare sau joystick – ul. Pentru a exprima aceleaşi mişcări în diferite sisteme de coordonate sunt necesare transformări de coordonate. Transformarea directă de coordonate Când punctele unei traiectorii sunt deja specificate în sistemul de coordonate generalizat (de exp. la programarea prin instruire), atunci se poate determina în mod univoc situarea efectorului final al robotului în sistemul de coordonate generalizat ataşat bazei robotului. Aceasta se realizează prin matricea de transformare omogenă. T
r
q = (q1 q 2
L
q6 )
⇒
R
0
6
r
i −1
T EF = T 6 = ∏ T i i =1
R
T EF
r
r
Transformarea inversă de coordonate Asigură trecerea de la sistemul de coordonate operaţional cu srcinea la baza robotului, la cel de coordonate generalizate.
r
⎡n o a p⎤ =⎢ ⎥ (8.1) ⎣0 0 0 1⎦
88 R R 0 1
T
r
T EF ⇒ q = (q1 q 2 0
1
T EF = T1 ⋅ T 2
5 L
−1 R
2
5 L
−1 0
T EF = T 6
T 6 ⇒ q1
3
T 2 ⋅ T1 ⋅ T EF T 3 ⋅ T 4 −1 1
q6 )
0
T6
T1−1 ⋅R T EF =1 T 2 ⋅2 T 3 −1 0
L
R
−1 R
5 L
(8.2)
T 3 ⋅ T 2 ⋅ T1 ⋅ T EF = T 4 ⋅ T 5 ⋅ T 6 ⇒ q 3
3
T −41 ⋅2 T 3−1 ⋅1 T −21 ⋅0 T1−1 ⋅R T EF = 4 T 5 ⋅5 T 6
4
−1 3
−1 2
3
T6 ⇒ q2
2
−1 1
−1 0
4
5
−1 R
⇒ q4
5
T 5 ⋅ T 4 ⋅ T 3 ⋅ T 2 ⋅ T1 ⋅ T EF = T 6
⇒ q5 , q6
8.2.2. Comanda automată a roboţilor
Comanda automată trebuie să suplinească operatorul uman în prescrierea activităţii robotului. În acest scop se necesită: preluarea informaţiilor privind sarcinile de executat de către robot de la operatorul uman sub formă de program, memorarea acestor sarcini, definirea mişcărilor pe care trebuie să le execute efectorul final, definirea mişcărilor relative ale cuplelor cinematice conducătoare ale dispozitivului de ghidare, recepţionarea şi interpretarea informaţiilor primite privind mişcarea realizată. Z M12
M13 M2
M11 M1
C
ΔX
ΔZ
M1k M
M1’
ΔY
M1’’
B Y X A
Fig. 8.4 Comanda automată a mişcării robotului
89 Funcţia de comandă automată a robotului este realizată de sistemul de comandă automată a robotului compusă din: unitatea centrală de comandă şi de procesare a informaţiilor, dispozitivul de programare, dispozitivul de memorare a mişcărilor axelor, dispozitivul de intrare / ieşire. Comanda în regim automat a mişcării robotului, între punctele STARTşi ŢINTĂ poate fi: a) Comanda secvenţială când mişcarea efectorului final se compune din atâtea
secvenţe câte cuple cinematice conducătoare contribuie la realizarea ei. Se acţionează succesiv fiecare cuplă cinematică conducătoare. Traiectoria punctului caracteristic M este compusă din segmentele M1M1’, M1’M1’’, M1’’ M2. b) Comanda punct cu punct PTP –Point To Point,asincronă, sincronă şi
multipunct.
b1) comanda punct cu punct asincronă – PTP asincronă, este cel mai vechi şi cel mai simplu mod de comandă. El este folosit în aplicaţii de manipulare, paletizare şi sudare în puncte. În acest caz se specifică doar distanţa până la punctul ţintă şi
profilul de vitez ă. Mişcările axelor nu sunt corelate, ele încep simultan şi se termină pe rând. Traiectoria punctului caracteristic nu este definită. b2) comanda punct cu punct sincronă – PTP sincronă, îi este caracteristică faptul că toate axele îşi încep mişcarea şi o termină în acelaşi timp. În acest caz se impune corelarea vitezelor elementelor cuplelor cinematice conducătoare. Şi în acest caz traiectoria punctului caracteristic nu este definită. b3) comanda multipunct – MP, este tot o comandă de tip PTP, însă numărul punctelor impuse pe traiectorie este mult mai mare. La comanda MP, pe lângă condiţia ca mişcarea relativă a celor 3 cuple cinematice conducătoare să înceapă
90 şi să se termine în acelaşi moment, se impune şi condiţia ca în intervalul de timp
ti+1 – ti să se parcurgă cursele xi+1 – xi, yi+1 – yi şi zi+1 – zi, între punctele de precizie Mi(xi, yi, zi) şi Mi+1 (xi+1, yi+1, zi+1). Aceste puncte de precizie sunt memorate automat, în procesul de instruire, la intervale de timp fixe, stabilite de sistemul de comandă. Şi în acest caz traiectoria punctului caracteristic dintre două puncte de precizie nu este definită. Această comandă este folosită pentru roboţi utilizaţi la sudarea în puncte sau la vopsire. c) Comanda de traiectorie continuă CP – Continous Parth, realizează deplasarea
punctului caracteristic al efectorului final pe o traiectorie continu ă impusă. Pentru aceasta toate axele trebuie comandate simultan. Semnalele de comand ă pentru aceste axe sunt calculate de interpolatorul de mişcare, care este o componentă sofware a generatorului de traiectorie. Pentru generarea acestora se parcurg ţional următoarele etape: interpolarea traiectoriei în sistemul de coordonate opera
ataşat bazei robotului, sau al efectorului final, transformarea invers ă de coordonate, interpolarea fină la nivelul axei de mişcare şi reglarea situării elementelor fiecărei cuple cinematice conducătoare. Comanda pe traiectorie continuă se impune în cazul unor aplicaţii de sudare cu arc electric, tăiere cu jet de apă sau de laser, polizare şi montaj. La comandaCP traiectoria punctului caracteristic între punctele START şi ŢINTĂ, este aproximată prin intermediul unor curbe de interpolare (segmente de dreaptă, arce de cerc sau arce de parabolă), având ecuaţia: ⎧F( x , y, z) = 0 ⎨ ⎩Φ ( x , y, z) = 0
(8.3)
91 ceea ce impune corelarea mişcărilor relative ale cuplelor cinematice conducătoare. Dacă la momentul tj punctul caracteristic trebuie să fie în poziţia Mj(xj, yj, zj), deplasările relative ale cuplelor cinematice conducătoare vor fi: ⎧Δx1 j = x j − x1 ⎪ ⎨Δy1 j = y j − y1 ⎪ ⎩Δz1 j = z j − z1
(8.4)
Pentru deplasarea Δx1j a cuplei cinematice conducătoare A rezultă: ⎧⎪F( x1 + Δx1 j , y1 + Δy1 j , z1 + Δz1 j ) = 0 ⎨ ⎪⎩Φ ( x1 + Δx1 j , y1 + Δy1 j , z1 + Δz1 j ) = 0
(8.5)
sistem din care se obţin Δy1j şi Δz1j. 8.2.3. Comanda manuală a roboţilor
Se realizează de către operatorul uman, rolul sistemului de comand ă fiind preluat de către acesta, el fiind sau nu asistat de senzori / traductoare. Dac ă operatorul uman se găseşte la distanţă de sistemul mecanicşi de cel de acţionare al robotului, comanda manuală este de teleoperare / telecomand ă, iar roboţii se numesc instalaţii de teleoperare. Comanda manuală se poate realiza fiedirect fie prin metodamaster – slave. a) Comanda manuală directă. Este asemănătoare cu programarea „on line” prin instruire a unui robot comandat automat. Ea se realizează prin acţionarea de către operatorul uman a unor butoane, întrerupătoare sau manete (joy – stick), care comandă direct motoarele de acţionare ale cuple lor cinematice conducătoare. Toate butoanele, întrerupătoarele sau manetele se montează pe un panou, care constituie interfaţa om – maşină. Transmiterea la distanţă a comenzilor manuale se poate realiza prin: ⇒ cabluri / conducte de alimentare cu energie a motoarelor sistemului deţionare; ac
92 ⇒ unde radio prin emiţătoare / receptoare radio;
ţe fizice în cazul protezelor ⇒ biocurenţi prelevaţi de la operatorul uman cu deficien
sau ortezelor. b) Comanda manuală copiere. Comanda manuală directă este dificilă, deoarece supune operatorul uman la controlul simultan aşi coordonat a unui număr mare de semnaleşi informaţii. Comanda manuală prin copiere constă în comandarea de către operatorul uman a unui lanţ cinematic identic structuralşi proporţional geometric (coeficientul de propor ţionalitate fiind denumit „coeficient de sclav”). Lanţul cinematic comandat direct poart ă denumirea de „lanţ master”, iar robotul este un „ lan ţ slave”. Instalaţiile master-slave se mai numescşi „ manipulatoare sincrone” deoarece lan ţul master şi slave se mişcă sincron. c) Comanda mixtă a roboţilor. Se realizează prin conlucrarea operatorului uman cu sistemul de comandă automat al robotului. De regulă operatorul uman apelează manual secvenţele de program pe care sistemul de comandă automată al robotului o execută automat. 8.3. Programarea roboţilor
Flexibilitateaşi productivitatea unui robot industrial este legat ă de faptul că sistemul său de comandă poate fi programat pentru realizareaoptimă a unor sarcini diversificate. ă a unui robot presupune: programarea mi şcărilor şi Realizarea programului de comand
programarea derulării comenzilor. Programarea mişcărilor se referă la axele energetice componente. În cadrul ei se determină punctele traiectoriei de mişcare a punctului caracteristic, orientările aferente efectorului final şi respectiv intervalele de mişcare.
93 Programarea derulării comenzilorse referă la stabilirea legăturii dintre intervalele de şi acceleraţii generalizate, mişcare, determinarea parametrilor de proces ca: timpi, viteze
prelucrarea datelor de proces, precumşi comunicaţia cu periferia sistemului robot. Programarea robotului poate fi„on – line” (programare apropiată de robot) şi „off – line” (programare îndepărtată de robot). Programarea „on – line” este cea mai răspândită. Programarea mişcării se face prin instruire, programatorul realizând mişcările prin conducerea manuală a robotului, sau a unui model în regim de instalaţie de teleoperare, în timp ce sistemul de comand ă al robotului înregistrează în memorie datele de mişcare. În continuare operatorul uman prin comenzi alfanumerice sau taste func ţionale programează derularea programului. Programarea „off – line”– programatorul prestabileşte valorile numerice ale parametrilor mişcării pe un calculator aflat la distan ţă faţă de robot. Modul de realizare a program ării mişcării: Playbackşi Teach-in. ăbate întreaga succesiune de situ ări programate, Programarea Playback– Efectorul final str
iar înregistrarea situ ărilor de pe traiectorie este independent ă de programator, fiind realizat ă ă cu un tact prestabilit. Comanda în regim automatunui de sistemul de comand a robot la
care s-a folosit programarea Playback se face în modul multipunct MP. Dezavantajul constă în capacitatea de memorie mare pentru a stoca informa ţiile privind succesiunea de situ ări. ările marcate ale Programarea Teach – in– programatorul conduce efectorul final spre situ ă în memoria calculatorului. succesiunii de situări programate, care apoi este înregistrat
Parametrii de mişcare ca: viteza, accelera ţia, precizia de atingerea a unui punct şi tipul şi de programator înfaza de editare a interpolării între două situări memorate, sunt introdu
94 programului. După editare urmează faza de compilare,în care programul editat este ă. Tot în această fază se transformat într-un cod binar, perceput de sistemul de comand
corectează eventualele erori din faza de editare. Înregistrarea situărilor succesive ale efectorului final se poate face prin: a) conducerea directă a robotului, când după decuplarea frânelor, operatorul uman acţionează manual robotul cu ajutorul unui mâner plasat pe efectorul final. Aplicaţia este frecventă la operaţiile de vopsire. b) conducerea unui model cinematic, când programatorul se foloseşte de un lanţ cinematic asemănător din punct de vedere cinematic cu dispozitivul de ghidare al robotului. c) conducerea master - slave, când se manipulează sarcini foarte mari, iar mişcarea robotului se realizează cu propriul sistem de acţionare. Pentru aceasta se utilizează un lanţ cinematic de conducere a robotului. Mişcările robotului se realizează sincron cu ale lanţului cinematic, manevrat de operatorul uman, având o instalaţie master – slave. d) conducerea prin panou de învăţare, comanda mişcării robotului şi înregistrarea în memorie a situărilor succesive se face cu panouri mobile cu taste de direcţie şi joystick-uri. Acţionarea unei taste de direcţie comandă robotul să se deplaseze în concordanţă cu semnificaţia tastei şi a sistemului de coordonate ales. Sistemele de comandă moderne permit transformări de coordonate (directe sau inverse) în timp real, permiţând alegerea de către operator a sistemului de coordonate.
95 9. SINTEZA FUNCŢIILOR DE CONDUCERE A ROBOŢILOR
Mişcarea efectorului final se realizează în spaţiul cartezian 3D. Poziţiile punctului caracteristic respectiv orientarea dreptei caracteristice şi dreptei auxiliare se definesc în conformitate cu matricea de situare în raport cu sistemul de referinţă fix ataşat bazei robotului. Definirea mişcării relative a elementelor cuplelor cinematice conducătoare se realizează prin calcul, de către sistemul de comandă al robotului. Legile de variaţie în funcţie de timp al deplas ărilor şi vitezele generalizate relative ale elementelor cuplelor cinematice conducătoare se numesc legi de mişcare. Cele mai des utilizate legi de mi şcare sunt cele cu „profil de viteză”, „polinomiale” şi „polinomiale cu racordări”. Sistemul de comandă calculează la intervale de timp prescrise, în funcţie de situările efectorului final (coordonatele sale în spaţiul cartezian), şi legea de mişcare adoptată, în funcţie de poziţia şi viteza relativă generalizată ale elementelor cuplelor cinematice conducătoare (coordonatele sale în spaţiul cuplelor cinematice conducătoare), operaţia numindu-se transformări de coordonate. În cazul comenzii pe traiectorie continuă, sistemul de comandă calculează vitezele generalizate ale mişcărilor relative ale elementelor cuplelor cinematice conducătoare în momentele aferente timpului de eşantionare prin intermediul modelului cinematic invers al dispozitivului de ghidare al robotului. 9.1.
Funcţia de conducere
Funcţia de conducere a axelor de robot este o îmbinare a funcţiei de comandă şi funcţia de acţionare a axelor robotului. Conţinutul funcţiei de conducere este:
96 recepţionarea semnalelor emise de sistemul de comand ă privind variaţia în timp a parametrilor mişcărilor relative ale elementelor cuplelor cinematice conducătoare, comanda funcţionării motoarelor de acţionare pentru realizarea parametrilor amintiţi, controlul realizării lor şi corelarea funcţionării motoarelor. 9.2. Legi de mişcare ale cuplelor cinematice conducătoare 9.2.1. Legi de mişcare parabolică (cu profil de viteză trapezoidal)
x x2 x22
x11 x1
t1 t11
t22 t2
t
V V12
t a
t Fig. 9.1 Legea de mişcare cu profil de viteză trapezoidal
Legile de mişcare cu profil de viteză trapezoidal, v = ct., au dezavantajul unor şocuri puternice în funcţionare, ca efect al forţei de inerţie rezultante.
97 Pentru condiţiile iniţiale:
a1 = a 2 = a
t 11 − t 1 = t 2 − t 22 = kt 2
(9.1)
1 1 x 11 = a 1 ⋅ ( t 11 − t 1 ) 2 = x 11 = a ⋅ k 2 t 22 2 2 x 22 = V12 ( t 22 − t 11 ) = V12 (1 − 2 k ) t 2 ( t 22 − t 11 ) = t 2 − ( t 11 − t 1 ) − ( t 2 − t 22 ) = (1 − 2 k ) t 2 1 1 2 2 2 2 12 2 22 2 2 22 12 2 (9.2) x = V ( t − t ) − 2 a ⋅ ( t − t ) = V kt − 2 a ⋅ k t 1 1 x 12 = x 11 + x 22 + x 2 = a ⋅ k 2 t 2 + V12 (1 − 2 k ) t 2 + V12 kt 2 − a ⋅ k 2 t 2 = 2 2 = V12 (1 − k ) t 2 x x 12 k opt = 1 − 12 = 1 − V12 t 2 Vmax t 2 Se obţine amax din expresia: 2x11 x − x 22 x12 − Vmax (1 − 2k opt ) t 2 x11 = x 2 = 12 = 2 2 2 2 k opt t 2 x12 − Vmax (1 − 2k opt ) t 2 2 2 = k opt t2
a max = a max
(9.3)
9.2.2. Principiile legilor de mişcare polinomiale
Un polinom de rangul n şi primele doua derivate ale sale au expresiile: F( t ) = C 0 + C1t + C 2 t 2 + L + C n −1t n −1 + C n t n F′( t ) = C1 + 2C 2 t + 3C3 t 2 L + (n − 1)C n −1t n −2 + nC n t n −1 F′′( t ) = 2C 2 + 6C3 t
2
L + (n
− 2)(n − 1)C n −1t
n −3
+ n (n − 1)C n t
(9.4) n −2
Pentru trasarea legii de mişcare de ordinul zero se impun N perechi de valori, unde N – numărul coeficienţilor polinomiale are expresia: N=n+1. Pentru a se evita şocurile în funcţionare se impune ca la capetele de cursă să avem V = 0 şi a = 0 adică: ∀ t = t 1 = 0 F( t 1 ) = C 0 = 0 F′( t 1 ) = C1 = 0 F′′( t 1 ) = 2C 2 = 0 ∀ t = t2 F( t 2 ) = x12 F′( t 2 ) = 0 F′′( t 2 ) = 0
(9.5)
98 Pentru N = 6 condiţii iniţiale impuse rezulta n = 5 gradul poligonului, adică: F( t ) = C 3 t 3 + C 4 t 4 + C5 t 5
(9.6)
Din a doua condiţie pusă în relaţiile (9.6) se obţine sistemul de 3 ecua ţii cu 3 necunoscute: ⎧ 3 4 5 ⎪F( t 2 ) = C 3 t 2 + C 4 t 2 + C 5 t 2 = x12 ⎪
C 3 = 10
⎪⎨F′( t 2 ) = 3C 3 t 22 + 4C 4 t 32 + 5C 5 t 42 = 0 ⇒ ⎪ ⎪ 2 3 ⎪F′′( t 2 ) = 6C 3 t 2 + 12C 4 t 2 + 20C 5 t 2 = 0 ⎪⎩
C 4 = −15 x12 t 42 x C5 = 6 12 t 52
x12 t 32 (9.7)
Rezolvând sistemul (9.7) expresia generală a legii de mişcare polinomiale ce ăr spunde la condiţiile iniţiale impuse este: 3
4
⎛ t ⎞ ⎛ t ⎞ ⎛ t ⎞ ⎟⎟ − 15x12 ⎜⎜ ⎟⎟ + 6x12 ⎜⎜ ⎟⎟ t t ⎝ 2⎠ ⎝ 2⎠ ⎝ t2 ⎠
5
F( t ) = −10 x12 ⎜⎜ x
(9.8)
x xj x j’ Mi
Mj
Δ xj
Mj’
Mi’ x i’ Δ x i Mi xi
Mj t
V
ti
tj
t
V Vij
ti
tj
a)
t
b)
Fig. 9.2 Legea de mişcare cu profil de viteză o funcţie polinomială – a) şi aproximarea cu segmente de dreaptă – b)
t
99 În cazul în care profilul vitezei nu este trapezoidal, ci o func ţie polinomială sau o funcţie trigonometrică, ea se poate aproxima cu segmente de dreapt ă. Cea mai bună aproximare se obţine dacă: Δx i = Δx j = Δx x i' = x i + Δx x 'j = x j − Δx x i' − x i = x j − x 'j = Δx
F( t i ) = C 0 + C1t i + C 2 t i2 + L + C n −1 t in −1 + C n t in = x i F( t j ) = C 0 + C1t j + Vij =
2 C2t j
+L+
n −1 C n −1t j
+
n Cn t j
(9.9a)
= xj
x j '− x i ' x − x − 2Δx x j '− x i ' = x j − x i − 2Δx ⇒ Vij = j i t j − ti t j − ti
t 3j − t 3i t nj − t in − 2Δx Vij = + C1 + C 2 ( t j + t i ) + C 3 + ⋅ ⋅ ⋅ + Cn t j − ti t j − ti t j − ti
(9.9b)
9.2.3. Legi de mişcare polinomiale cu racordări
Pentru a exprima o traiectorie descrisă în coordonate carteziene sunt necesareM funcţii ţii de aproximare, câte una pentru fiecare grad de mobilitate al robotului, iar aceste func
se calculează pentru fiecare punct de precizie impus pe traiectorie.Ţinând cont că fiecare punct de precizie reclamă trei condiţii iniţiale (poziţie, viteză şi acceleraţie) gradul funcţiei polinomiale ce trece prink puncte de precizie esten = 3k-1. Metoda Ho şi Cook utilizează aproximarea traiectoriei polinomiale prin segmente de curbă, utilizând funcţii polinomiale de gradul 3 pentru segmentele intermediare şi de gradul 4 pentru segmentele de începutşi de sfârşit. Pentru a trasa o traiectorie prinn puncte sunt necesaren-1 funcţii polinomiale de aproximare:2 funcţii de gradul 4, pentru începutul şi sfârşitul traiectoriei şi n-3 funcţii de gradul 3 pentru segmentele intermediare.
100 a) Pentru două puncte de precizie intermediare ale traiectoriei,Mk şi Mk+1
( 2 ≤ k ≤ n − 1 ), funcţia polinomială de gradul trei pe segmentul M k M k +1 scris ă pentru o singură cuplă cinematică conducătoare: F( t ) = C 0 + C1t + C 2 t 2 + C3 t 3
F”(tk+1)
t ∈ [t k = 0, t k +1 ]
(9.10)
F”(tk+2) Mk+2
F(t) Mk+1 Mk
0 ≤ t ≤ t k +1 0 ≤ t ≤ t k + 2 tk
tk+1
tk+2
Fig. 9.3 Aproximarea între două puncte de precizie ale traiectoriei
Pentru intervalul de timp t ∈ [t k = 0, t k +1 ] se impun următoarele condiţii iniţiale: F(0) = Fk
F( t k +1 ) = Fk +1
F′(0) = Fk′
F′( t k +1 ) = Fk′ +1
(9.11)
unde Fk şi Fk+1 reprezintă poziţia în planul mişcării, al punctelor Mk şi Mk+1 la momentele tk=0 şi tk+1, iar F’k şi F’k+1 reprezintă vitezele cuplei cinematice conducătoare în aceleaşi puncte şi aceleaşi momente. Derivând relaţia (9.10) se obţine expresia legii de mişcare de ordinul unu. F′( t ) = C1 + 2C 2 t + 3C3 t 2
(9.12)
Înlocuind condiţiile iniţiale din relaţia (9.11) în relaţiile (9.10) şi (9.12) se obţine: F(0) = Fk = C 0
F′(0) = Fk′ = C1
⎧⎪F( t k +1 ) = Fk +1 = Fk + Fk′ t k +1 + C 2 t 2k +1 + C 3 t 3k +1 ⎨ ⎪⎩F′( t k +1 ) = Fk′ +1 = Fk′ + 2C 2 t k +1 + 3C 3 t 2k +1
(9.13) (9.14)
101 Rezolvând sistemul de ecuaţii (9.14) se obţine pe intervalul de timp t ∈ [t k , t k +1 ] : C2 =
⎤ 1 ⎡ 3(Fk +1 − Fk ) − 2Fk′ − Fk′ +1 ⎥ ⎢ t k +1 ⎣ t k +1 ⎦
⎤ 1 ⎡ 2(F − F ) C 3 = 2 ⎢ k k +1 + Fk′ + Fk′ +1 ⎥ t k +1 ⎣ t k +1 ⎦
(9.15)
Pentru valori particulare 2 ≤ k ≤ n − 1 relaţiile (9.13) şi (9.15) se pot grupa matriceal: ⎡ 1 ⎧C 0 ⎫ ⎢ 0 ⎪C ⎪ ⎢ − 3 ⎪ 1⎪ ⎢ ⎨ ⎬ = t2 ⎪C 2 ⎪ ⎢ k +1 ⎪⎩C 3 ⎪⎭ ⎢ 2 ⎢ 3 ⎣ t k +1
0 0 3 t 2k +1 −2 t 3k +1
0 1 −2 t k +1 1 t 2k +1
0 ⎤ 0 ⎥⎥ ⎧⎪ Fk ⎫⎪ − 1 ⎪Fk +1 ⎪ ⎥⋅ t k +1 ⎥ ⎨⎪ Fk' ⎬⎪ 1 ⎥ ⎪F ' ⎪ 2 ⎥ ⎩ k +1 ⎭ t k +1 ⎦
(9.16)
Se vor selecta valorile lui tk+1 pentru care punctul Mk+1 asigura o eroare de traiectorie minimă: M t k +1 ∀ Φ = ⎡⎢∑ q i ,k +1 − q i ,k 2 ⎤⎥ = min im ⎣ i=1 ⎦
(9.17)
qi,k+1 este coordonata generalizată obţinută prin acţionarea cuplei cinematice conducătoare pentru a atinge punctul Mk+1. Vitezele Fk′ şi Fk′ +1 se determină din condiţiile de continuitate a legii de mişcare de ordinul doi (a acceleraţiilor) în punctele de precizie. Prin derivarea expresiei vitezei din relaţia (9.11) se obţine acceleraţia la sfârşitul primului segment în punctul de precizie Mk+1:
F′′(t k +1 ) = 2C 2 + 6C3 t k +1
(9.18)
Pentru începutul celui de-al doilea segment expresia acceleraţiei devine: F′′(t k +2 ) = 2C 2 + 6C 3 t k +2 Particularizând la t=0
⇒
F”(0) = 2C2
(9.19) (9.20)
102 Continuitatea legii de mişcare de gradul doi (în acceleraţie constantă)în punctul Mk+1: F′′(t k +)1 =( F) ′′ t k +2
(9.21)
Utilizând expresiile (9.16), (9.18) şi (9.20) în relaţia (9.21) se obţine: t k +2 Fk′ + 2( t k +1 + t k +2 )Fk′ +1 + t k +1Fk′ +2 =
3 [t 2 (F − F) ( + t 2k)+2 Fk+1 − Fk ](9.22) t k +1t k +2 k +1 k +2 k +1
Pentru valori particulare 2 ≤ k ≤ n − 1 relaţia (9.22) se poate grupa matriceal: ⎡t 4 ⎢0 ⎢ ⎢0 ⎢ ⎢M ⎢⎣ 0
2( t 3 + t 4 ) t3 0 0 t5 2( t 4 + t 5 ) t4 0 0 t6 2( t 5 + t 6 ) t 5
0 0 0
0 0 0
0 0 0
⎤ ⎥ ⎥ ⎥⋅ L ⎥ M M M M M M M ⎥ 0 0 0 0 L t n −1 2( t n −2 + t n −1 ) t n −2 ⎥⎦ 3 2 ⎧ ⎫ [ t 3 (F4 −)F3( + t 24) F3 − F2 ] ⎪ ⎪ t 3t 4 ⎪ ⎧ F2' ⎫ ⎪ 3 2 2 ⎪ ⎪ ' ⎪ ⎪ [ t ( F − ) F ( + t ) F − F ] 5 4 3 ⎪ ⎪⎪ F3 ⎪⎪ ⎪⎪ t 4t5 4 5 4 ⎪ ' 2 2 4 F 3 ⋅ ⎨⎪ ⎬⎪ = ⎨⎪ ⎬⎪ [t (F −)F ( + t 6) F5 − F4 ] t5t 6 5 6 5 ⎪ M ⎪ ⎪ ⎪ ' M ⎪ ⎩⎪Fn −1 ⎭⎪ ⎪ 3 ⎪ [ t 2n −2 (Fn −1 − F)n −24 ( + t 2n −)1 Fn −2 − Fn −3 ]⎪ ⎩⎪ t n −2 t n −1 ⎭⎪ (9.23) L L
Rezolvând sistemul matriceal (9.23) se obţin valorile matricei vitezelor F′ şi înlocuind în matricea (9.16) rezultă o soluţie unică a coeficienţilor polinomiali pentru fiecare funcţie polinomială de ordinul trei. Pe măsură ce se obţin soluţiile funcţiilor polinomiale - relaţia (9.10) - se impune verificarea respectării condiţiei de eroare minimă faţă de traiectoria programată. b) Pentru primul şi ultimul segment al traiectorieise impune condiţia iniţială de
funcţionare fără şocuri:
F1′ = F1′′ = Fn′ = Fn′′ = 0
(9.24)
103 utilizând funcţia polinomială de gradul patru: F( t ) = C 0 + C1t + C 2 t 2 + C3 t 3 + C 4 t 4 F”(t2)
(9.25)
M3
F”(t0) F2(t) M2
M1
F1(t)
0 ≤ t ≤ t2 t1
0 ≤ t ≤ t k +1 t2
t3
Fig. 9.4 Aproximarea primului segment al traiectoriei
⇒
pentru segmentul iniţial M1M 2 pe intervalul de timp t ∈ [t1 = 0, t 2 ] se
impun următoarele condiţii iniţiale: F(0) = C 0 = F1
F′(0) = C1 = 0
F′′(0) = C 2 = 0
⎧F( t 2 ) = F1 + C 3 t 32 + C 4 t 42 = F2 ⎪⎪ 2 3 ⎨F′( t 2 ) = 3C 3 t 2 + 4C 4 t 2 = F2′ ⎪ ′′ 2 ⎪⎩F ( t 2 ) = 6C 3 t 2 + 12C 4 t 2 = F2′′
(9.26)
(9.27)
Rezolvând primele 2 ecuaţii din sistemul de ecuaţii (9.27) se obţine: 4 1 (F2 − F1 ) − 2 F2′ 3 t2 t2 −3 1 C 4 = 4 (F2 − F1 ) + 3 F2′ t2 t2 C3 =
⇒
(9.28)
pentru segmentul final M n-1M n pe intervalul de timp t ∈ [t n −1 = 0, t n ] se
impun următoarele condiţii iniţiale: F(0) = C 0 = Fn −1
F′(0) = C1 = Fn′ −1
(9.29)
104 F”(tn-1) F”(tn) Fn-1(t)
Mn Mn-1
Fn(t)
Mn-2
0 ≤ t ≤ t n −1 0 ≤ t ≤ t n tn-2
tn-1
tn
Fig. 9.5 Aproximarea ultimului segment al traiectoriei
⎧F( t n ) = Fn −1 + Fn′ −1t n + C 2 t 2n + C 3 t 3n + C 4 t 4n = Fn ⎪⎪ 2 3 ⎨F′( t n ) = Fn′ −1 + 2C 2 t n + 3C 3 t n + 4C 4 t n = 0 ⎪ ′′ 2 ⎪⎩F ( t n ) = 2C 2 + 6C 3 t n + 12C 4 t n = 0
(9.30)
Rezolvând sistemul de ecuaţii (9.30) se obţine: 1
(6Fn − 6 Fn −1 − 3Fn′ −1t n ) t 2n 1 C 3 = 3 (− 8Fn + 8 Fn −1 + 3Fn′ −1 t n ) tn 1 C 4 = 4 (3Fn − 3 Fn −1 − Fn′ −1t n )2 tn C2 =
(9.31)
Pentru determinarea matricei vitezelor F′ se impune condiţia de continuitate a legii de mişcare de ordinul doi (a acceleraţiilor) în punctele de precizie M2 pentru primul segment al traiectoriei şi Mn-1 pentru ultimul al traiectoriei. Acceleraţia la sfârşitul primului segment de curbă, în punctul de precizieM2 se exprimă ţia (9.25): prin derivata a doua a funcţiei polinomiale de gradul patru - vezi rela
F′′(t 2 ) = 6C3 t 2 + 12C 4 t 22 care cu ajutorul relaţiilor (9.31) ia forma:
(9.32)
105 − 12
F′′()t 2 = (
t 22
6 )F2 − F1 + F2′ t2
(9.33)
La începutul primului segment de curbă intermediară, derivata de ordinul doi a funcţiei polinomiale de ordinul trei se obţine cu ajutorul relaţiei (9.12): F′′(t ) = 2C 2 + 6C3 t
(9.34)
Particularizând la t=0, corespunzător punctului de precizie M2 de început al primului segment de curba intermediar:
⇒
F”(0) = 2C2
(9.35)
unde coeficientul C2 este definit de relaţia (9.15) pentru k = 2: F”(0) =
⎤ 2 ⎡ 3(F3 − F2 ) − 2F2′ − F3′ ⎥ ⎢ t3 ⎣ t3 ⎦
(9.36)
Continuitatea legii de mişcare de gradul doi (în acceleraţie constantă)în punctul M2, se exprimă prin egalarea relaţiilor (9.33) şi (9.36): F′′(t)2 =( F′′ t)k = 0 ⎛2 3⎞ 1 6 3 ⎜⎜ + ⎟⎟F2′ + F3′ = 2 (F2 −)F1 ( + 2) F3 − F2 t t t t t ⎝ 3 2⎠ 3 2 3
(9.37) (9.38)
ă intermediară, în În mod similar acceleraţia la sfârşitul ultimului segment de curb
punctul de precizieMn-1, este obţinută din relaţiei (9.34) pentrut = tn-1: F′′(t n −1 ) = 2C 2 + 6C 3 t n −1
(9.39)
unde coeficienţii C2 şi C3, definiţi de relaţia (9.15), pe intervalul de timp t ∈ [t k +1 , t k +2 ] au expresia:
106 C2 =
⎤ 1 ⎡ 3(Fk +2 − Fk +1 ) − 2Fk′ +1 − Fk′ + 2 ⎥ t k +2 ⎢⎣ t k +2 ⎦
⎤ 1 ⎡ 2(F − F ) C 3 = 2 ⎢ k +1 k +2 + Fk′ +1 + Fk′ +2 ⎥ t k+2 t k +2 ⎣ ⎦
(9.15a)
ce se particularizează pentru k = n – 1:
C2
⎤ 1 ⎡ 3(Fn −1 − Fn −2 ) n 2 n 1 − − t n −1 = t n −1 ⎢⎣ − 2F′ − F′ ⎥⎦
⎤ 1 ⎡ 2(F − F ) C 3 = 2 ⎢ n −2 n −1 + Fn′ −2 + Fn′ −1 ⎥ t n −1 t n −1 ⎣ ⎦
(9.40)
Cu aceste precizări relaţia (9.38) devine: F′′(t n −1 ) =
6 6 2 4 Fn −2 − 2 Fn −1 + Fn′ −2 + Fn′ −1 t 2n −1 t n −1 t n −1 t n −1
(9.41)
La începutul segmentului final, pentru tn = 0, acceleraţia este definită de derivata a doua a funcţiei polinomiale de ordinul patru, prin a treia ecuaţie din relaţia (9.30): F′′(0) = 2C 2 (9.42) unde coeficientul C2 este definit de relaţia (9.31): F) ′′((0 =
2 6Fn − 6 F)n −1 − 3Fn′ −1t n t 2n
(9.43)
Egalând relaţiile (9.40) cu (9.41) rezultă: F′′(t n)−1 =( F′′ )t n = 0
(9.44)
se obţine: ⎛ 2 1 3⎞ 3 6 F′ + ⎜ + ⎟F′ = (F − F) ( + ) F − F t n −1 n −2 ⎜⎝ t n −1 t n ⎟⎠ n −1 t 2n −1 n −1 n −2 t 2n n n −1
(9.45)
107 Combinând relaţiile (9.38) cu (9.45) cu relaţia (9.23), prin completarea primei linii cu relaţia (9.38) şi a ultimei linii cu relaţia (9.45) se obţine: ⎡2 3 ⎢t + t 2 ⎢ 3 t 4 ⎢ ⎢ 0 ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣
0M 0 0
1 t3
0
2(t 3 + t 4 ) t3 t5 2( t 4 + t 5 )
0
0
L
0
0
0
0 t4
0 0
L
0 0
0 0
0 0
0M
0M
0M
+
0M
tM6
2(t 5 M t 6 ) tM5
L L
⎤ ⎥ ⎥ ⎥ ⎥ ⎥
⎥ ⎥ 0 0 0 0 L t n −1 2(t n −2 + t n −1 ) t n −2 ⎥ 1 2 3⎥ 0 0 0 0 L 0 + ⎥ t n −1 t n −1 t n ⎦ 3 6 ⎫ ⎧ (F − )F2 ( + 2) F2 − F1 2 3 ⎪ ⎪ t3 t2 ⎪ ⎪ 3 2 ⎪ ⎧ F2' ⎫ ⎪ [ t 3 (F4 −)F3( + t 24) F3 − F2 ] ⎪ t 3t 4 ⎪ ' ⎪ ⎪ ⎪ ⎪ F3 ⎪ ⎪ 3 2 [ t 4 (F5 − )F4 ( + t 52) F4 − F3 ] ⎪ ⎪ F4' ⎪ ⎪ t 4t5 ⎪⎪ ⎪ ' ⎪ ⎪⎪ ⋅ ⎨ F5 ⎬ = ⎨ 3 2 ⎬ 2 5 6 5 6 5 4 t t t F F t F F M ( ) ( ) ⎪⎪ ⎪⎪ ⎪⎪ − + − ] ⎪⎪ 5 6[ M ⎪Fn' −2 ⎪ ⎪ ⎪ 2 ⎪ ' ⎪ ⎪ 3 [t 2n −2 (Fn −1 − F)n −24 ⎪ ( + t ) F − F ] n −1 n − 2 n −3 ⎩ Fn −1 ⎭ ⎪ t n −2 t n −1 ⎪ ⎪ ⎪ 3 6 (Fn −1 − F)n −(2 + )2 Fn − Fn −1 ⎪ ⎪ 2 ⎪⎩ ⎪⎭ t n −1 tn
(9.46) Rezolvând sistemul matriceal al relaţiei (9.46)se obţin toate valorile matricei vitezei F′ , definind astfel toate funcţiile polinomiale de gradul trei şi patru utilizate pentru aproximare traiectoriei prescrise în planul mişcării. Extinzând metoda prezentată pentru toate cele M cuple cinematice conducătoare ale unui robot industrial, ceea ce conduce la un maxim de (n-1).M funcţii polinomiale pentru o traiectorie aproximată prin n puncte de precizie.
108 Valorile extreme ale vitezelor se pot exprima printr-un factor: ⎡ max i ( Fij′ (t ) ): 0 ≤ t ≤ t i+1 ⎤ ⎥ Vij ⎣⎢ ⎦⎥
S v = max j ⎢
unde Vij – viteza de regim a unei cuple cinematice conducătoare j,
(9.47) pentru j = 1,
; M
F’ij – viteza pe segmentul de curba i, prin acţionarea cuplei cinematice conducătoare j. Valorile extreme ale acceleraţiilor se pot exprima de asemenea printr-un factor: ⎡ max i ( Fij′′(t ) ): 0 ≤ t ≤ t i+1 ⎤ ⎥ a ij ⎢⎣ ⎥⎦
Sa = max j ⎢
unde aij – acceleraţia de regim a cuplei cinematice conducătoare j,
(9.48) pentru j = 1,
; M
F”ij – acceleraţia pe segmentul de curba i, prin acţionarea cuplei cinematice conducătoare j. Valoarea optimă a factorului S este: S = max. (Sv , Sa)
(9.48)