Arhitectura Sistemelor Robotizate Avansate L5

Lucrare de laborator nr. 5

ANALIZA CINEMATICĂ INVERSĂ A POZIȚIILOR A UNUI
MECANISM DE POZIȚIONARE

1. SCOPUL LUCRĂRII
Scopul lucrări este de a determina coordonatele cuplelor conducătoare care determină
coordonatele carteziene ale punctului caracteristic şi orientarea obiectului manipulat. Deci funcţie
de coordonatele operaţionale ale punctului caracteristic se vor determina coordonatele cuplelor con-
ducătoare ale unui mecanism de poziționare.
În acest sens se va stabili un algoritm pentru determinarea relațiilor de legătură dintre
coordonatele carteziene ale punctului caracteristic și coordonatele cuplelor conducătoare. Aceste
relații se vor utiliza pentru stabilirea coordonatelor cuplelor conducătoare.

2. PROBLEMA CINEMATICII INVERSE

Cinematica unui robot tip manipulator poate fi împărțită în cinematica directă și cinematica
inversă a pozițiilor. Relaţia între cinematica directă şi inversă a pozițiilor este ilustrată în fig. 1

1
2 Cinematica directă
Spațiul Spațiul
0
coordonatelor nT coordonatelor
cuplelor Cinematica inversă carteziene
n

Fig. 1 Reprezentarea schematică a cinematicii directe și inverse

Cinematica inversă a pozițiilor este o problemă mult mai dificilă decât cinematica directă.
Soluția problemei cinematicii inverse se determină într-un interval de timp mai mare decât timpul
necesar controlului în timp real al manipulatorilor.
Două tehnici principale de determinare a soluției pentru problema cinematicii inverse sunt
metode analitice și numerice. In primul tip, variabilele comune sunt rezolvate analitic conform
datelor de configurare cunoscute pentru manipulatori. In al doilea tip de soluție, variabilele comune
sunt obținute pe baza tehnicilor numerice de rezolvare.
Există două abordări în metodele analitice: soluții geometrice și algebrice. Abordarea
geometrică este aplicată structurilor simple ale robotului, cum ar fi manipulatorul plan cu 2 grade de
1
Arhitectura Sistemelor Robotizate Avansate L5

libertate (DOF) sau manipulatorului cu mai puține grade de libertate și cu axele cuplelor paralele.
Pentru manipulatori cu mai multe elemente și ale căror brațe se mișcă în spațiul tridimensional
geometria devine mult mai dificilă. În acest caz, abordare algebrică este mai benefică pentru soluția
cinematicii inverse.
Există unele dificultăți pentru a rezolva problema cinematicii inverse când ecuațiile cinematice
au mai mult de o variabilă în relațiile dintre ele, și există soluții multiple și singularități. Soluțiile
matematice pentru problema cinematicii inverse pot să nu corespundă întotdeauna cu soluțiile fizice
și metoda soluției sale depinde de structura robotului.
Abordarea soluției algebrice. Pentru manipulatori cu mai multe elemente cinematice și pentru
care efectorul final acționează în spațiul tridimensional, abordarea geometrică devine mult mai
dificilă. Prin urmare, în acest caz, abordarea algebrică este aleasă pentru soluționarea problemei
cinematicii inverse.
Pentru a găsi soluția cinematicii inverse pentru un manipulator cu șase grade de libertate, vom
considera relația de la cinematica directă a poziției.
 r11 r12 r13 px 
 
0  r 21 r 22 r 23 py 0
6T    1 T(q 1 )21 T(q 2 )32 T(q 3 )43 T (q 4 )54 T(q 5 )65 T(q 6 ). (1)
r r r33 pz 
 31 32 
 0 0 0 1 

Pentru a găsi soluția cinematică inversă pentru prima cuplă ( q 1 ) ca o funcție de elementele

cunoscute ale matricei de transformare omogenă 06 T , de la bază la efector final, matricele de

transformare omogene intermediare sunt multiplicate după cum urmează.

01 T(q1)1 60T  01 T(q1)101 T(q1)21 T(q 2 )32 T(q 3 )43 T(q 4 )54 T(q 5 )65 T(q 6 ).
unde 01 T(q 1 ) 01 T(q 1 )  I , I este matricea identitate. În acest caz, ecuația de mai sus este dată
1

de relația:

 T (q ) 
0
1 1
1 0 1 2 3 4 5
6 T  2 T (q 2 )3 T (q 3 ) 4 T (q 4 )5 T (q 5 )6 T (q 6 ). (2)

Pentru a găsi alte variabile, următoarele ecuații se obțin într-un mod asemănător.

 T(q ) T(q ) T T(q ) T(q ) T(q ) T(q

0
1
1
1 2 2
1 0
6
2
3
3
3 4
4
4 5
5
5 6 6 ). (3)

 T(q ) T(q ) T(q ) T T(q ) T(q ) T(q

0
1
1
1 2
2
2 3 3
1 0
6
3
4
4
4 5
5
5 6 6 ). (4)

 T(q ) T(q ) T(q ) T(q ) T T(q ) T(q

0
1
1
1 2
2
2 3
3
3 4 4
1 0
6
4
5
5
5 6 6 ). (5)

 T(q ) T(q ) T(q ) T(q ) T(q ) T T(q

0
1
1
1 2
2
2 3
3
3 4
4
4 5 5
1 0
6
5
6 6 ). (6)

2
Arhitectura Sistemelor Robotizate Avansate L5
Există un set de 12 ecuații neliniare care trebuiesc rezolvate. Cele 12 elemente din partea
dreaptă ale matricei neliniare sunt fie zero, constante sau funcție de q2 până la q6. Dacă elementele
de pe partea stângă, care sunt funcție de q1 sunt egale cu elementele de pe partea dreaptă, atunci
variabila q1 poate fi rezolvată ca funcții de r11, r12, ... r33, px, py, pz și parametrii elementului fix.
Odată ce q1 este găsit, atunci celelalte variabile comune sunt rezolvate prin același mod ca și mai
înainte. Nu există nici o obligație ca prima ecuație să aibă ca soluție q1, iar a doua ecuație să aibă ca
soluție q2 , etc. Pentru a găsi ecuația potrivită pentru soluționarea problemei cinematicii inverse,
orice ecuație definită mai sus poate fi folosită în mod arbitrar.
Problema cinematicii inverse permite calculul coordonatelor, care aduc efectorul final în
poziţia si orientarea dorită, date fiind coordonatele carteziene (operaționale).
După Roth, roboţii cu mai puţin de şase grade de libertate au întotdeauna soluţie. Roboţii cu
şase grade de libertate au soluţie, dacă prezintă una dintre următoarele caracteristici:
 posedă trei cuple de translaţie;
 posedă trei cuple de rotaţie cu axe concurente;
 posedă o cuplă de rotaţie şi una de translaţie coaxiale;
 posedă două perechi de cuple de rotaţie cu axe concurente.
Aproape toate structurile de roboţi industriali utilizate în industrie prezintă o soluţie a
problemei cinematice inverse şi de aceea au structuri asemănătoare celor descrise anterior.
Din punct de vedere al numărului de soluţii există trei cazuri:
I. Problema cinematică inversă nu are soluţii, ca în cazul când ţinta se află în afara spaţiului de
lucru al robotului.
II. Problema cinematică inversă are o infinitate de soluţii atunci când :
 robotul este redundant vis a vis de misiunea încredinţată;
 robotul se află într-o configuraţie singulară. Robotul nu-şi poate roti efectorul final în jurul
anumitor axe. Această situaţie nu se datorează structurii robotului ci valorilor numerice ale
unor parametri ce descriu situaţiile impuse.
III. Problema cinematică inversă are un număr finit de soluţii şi toate pot fi calculate fără
ambiguitate. Numărul de soluţii depinde de arhitectura robotului.
Pentru clasa roboţilor cu şase grade de libertate posedând trei cuple cinematice de rotaţie cu
axe concurente numărul maxim de soluţii este de 32.
Acest număr, obţinut atunci când nici un parametru geometric nu este nul, descreşte atunci
când aceştia iau anumite valori particulare.

3
Arhitectura Sistemelor Robotizate Avansate L5

3. EXEMPLUL NUMERIC
Pentru exemplul de calcul, se consideră manipulatorului în coordonate sferice, care are 3 grade
de libertate. Cunoscând parametrii Denavit – Hartenberg este necesar să se determine coordonatele
carteziene ale punctului caracteristic M. Determinarea acestor coordonate se va face folosind
calculul simbolic din mediul de programare Matlab.
În Fig. 1 este reprezentat manipulatorul în coordonate sferice şi sistemele de axe ataşate
fiecărui element.
Parametrii Denavit – Hartenberg pentru convenţia standard, sunt prezentaţi în Tabelul 1. Pe
baza acestor parametrii se determină coordonatele carteziene ale punctului caracteristic M, în raport
cu originea sistemului de referinţă.

Fig. 2 Manipulator în coordonate sferice

Tabelul 1 Parametrii Denavit –Hartenberg standard

ai i di i
1 0  2 L1 1
2 0 2 L2 2
3 0 0 d3 0

Matricele de transformare omogene de trecere de la un sistem de axe la altul sunt prezentate

mai jos:

4
Arhitectura Sistemelor Robotizate Avansate L5

 cos(1 ) 0 - sin(1 ) 0   cos(2 ) 0 sin(2 ) 0  1 0 0 0

     
0  sin(1 ) 0 cos(1 ) 0  1  sin( 2 ) 0  cos( 2 ) 0  2 0 1 0 0
1T   ; T  ; T  .(1)
L1   0 L2  0 0 1 d3 
2 3
0 1 0 1 0
     
 0 0 0 1   0 0 0 1  0 0 0 1 
   

În final, se determină matricea de transformare omogenă pentru întregul manipulator:

c 1  c 2  s 1 c 1  s 2 d 3  c 1  s 2  L 2  s 1 
s  c c 1 s 1  s 2 L 2  c 1  d 3  s 1  s 2 
0
 0 1 2
 
3 T 1T 2 T 3 T   1 2
. (2)
 s2 0 c2 L2  d3 c2 
 
 0 0 0 1 
Primele trei elemente ale coloanei a patra reprezintă coordonatele punctului caracteristic M al
obiectului manipulat, în raport cu originea O0 a sistemului de referinţă x0y0z0.
In vederea realizării unui program de calcul am apelat la mediul de programare Matlab, care
oferă importante facilități de calcul matricial și numeric.
Programul realizat în baza algoritmului de rezolvare a problemei cinematice directe descrise
este prezentată în continuare:

%Cinematica directa a poziției pentru manipulatorul in coordonate sferice calcul simbolic

clear;
clc;
rad=pi/180;
% Variabile simbolice considerate
syms L1 L2 d3 t1 t2 d3
%
T0_1=mdh_s(0,-pi/2,L1,t1);
T1_2=mdh_s(0,pi/2,L2,t2);
T2_3=mdh_s(0,0,d3,0);
T0_3=T0_1*T1_2*T2_3;
%
T0_3 = vpa(simple(T0_3),2)
xM=T0_3(1,4)
yM=T0_3(2,4)
zM=T0_3(3,4)

În urma rulării programului de mai sus se obţin relațiile pentru coordonatele carteziene ale
punctului caracteristic M. În acest sistem de ecuaţii se cunosc coordonatele carteziene, iar
necunoscutele sunt coordonatele cuplelor conducătoare.
x M   L 2  sin( t 1 )  d 3  cos( t 1 )  sin( t 2 ) (1)

 y M  L 2  cos( t 1 )  d 3  sin( t 1 )  sin( t 2 ) (2) (3)

z M  L 1  d 3  cos( t 2 ) (3)

5
Arhitectura Sistemelor Robotizate Avansate L5

După înmulțirea relaţiei (1) cu sin(t1) şi relația (2) cu cos(t1), se scad cele două relații obținute.
Relaţia obținută este utilizată la determinarea unghiului 1 , notat cu t 1 :

x M  sin( t 1 )  y M  cos( t 1 )  L 2  0 . (4)

Relaţiile (2) şi (3), după izolarea termenului care conține unghiul  2 , se împart și se obţine

relaţia de mai jos, utilizată pentru determinarea unghiului  2 , notat cu t 2 :

( y M  L 2  cos( t 1 )) sin( t 1 )
tg ( t 2 )  . (5)
z M  L1

În final, din relaţia (3) a sistemului (3) se determin parametrul de translaţie d 3 :

z M  L1
d3  . (6)
cos( t 2 )

Pentru determinarea coordonatelor cuplelor conducătoare s-a utilizat un program în mediul de

programare Matlab, care este prezentat mai jos.
%Cinematica inversa a poziției pentru manipulatorul in coordonate sferice calcul numeric
clear;
clc;
rad=pi/180;
%Dimensiuni geometrice
L1=250; % in [mm]
L2=160;
%
xM = -108.564065; % in [mm]
yM = 131.961524;
zM = 353.923048;
% Rezolvarea sistemului
% t1 se obține din ecuația xM*sin(t1) - yM*cos(t1) + L2 = 0
delta = xM^2 + yM^2 - L2^2;
if delta < 0
fprintf('Unghiul teta1 nu se poate calcula');
break;
end;
ta = (-xM + sqrt(delta))/(yM + L2);
t1 = 2*atan(ta);
if abs(t1)<1.e-10
t1 = 0;
end;
if (t1 ~= 0) & (t1 ~= pi)
a2 = (yM - L2*cos(t1))/sin(t1);
b2 = zM - L1;
else
a2 = (xM + L2*sin(t1))/cos(t1);
b2 = zM - L1;
end
t2 = atan2(a2,b2);

6
Arhitectura Sistemelor Robotizate Avansate L5

if (t2 ~= pi/2) & (t2 ~= -pi/2)

d3 = (zM - L1)/cos(t2);
else
if (t1 ~= 0) & (t1 ~= pi)
d3 = (yM - L2*cos(t1))/(sin(t1)*sin(t2));
else
d3 = (xM + L2*sin(t1))/(cos(t1)*sin(t2));
end;
end;
fprintf('teta1 = %6.2f \t teta2 = %6.2f \t d3 = %6.2f \n',t1/rad,t2/rad,d3);

4. MODUL DE LUCRU
1. În figura de mai jos se prezintă schema structurală a unui mecanism de poziționare din
structura unui manipulator. Pentru mecanismul precizat se vor determina relaţiile coordonatelor
carteziene ale punctului caracteristic M, folosind un program de calcul simbolic.

B
d2
2
C

1 3 3
M
zo yo
1

Oo
A xo

Fig. 3 Schema structurală a mecanismului de poziționare

2. Se va stabili algoritmul de calcul pentru coordonatele cuplelor conducătoare.

3. Se calculează coordonatele cuplelor conducătoare folosind un program de calcul numeric.

7
Arhitectura Sistemelor Robotizate Avansate L5