Laborator MATLAB Generarea spaiului de lucru al unei structuri robotizate

Generarea spaiului de lucru al unei structuri paralele cu 3 grade de libertate la care se

cunosc parametrii geometrici i modelul geometric direct.
Consideraii teoretice

Pagina 1 din 11

Fig. 1. Schema cinematic a structurii paralele PARAMIS pentru CMI

n figura 1 se prezint schema cinematic a structurii paralele noi, PARAMIS, care a fost
conceput pentru poziionarea laparoscopului n procedurile minim invazive. Structura are trei
grade de libertate n spaiu, i, prin analogie, 3 cuple active (actuatori) dintre care dou sunt
prismatice i una de rotaie. De asemenea, structura prezint mai multe cuple pasive, dintre care
dou sunt cilindrice, una este prismatic iar una este cardanic.
Robotul trebuie s poziioneze vrful laparoscopului, de coordonate E X E ,YE , Z E (extremitatea
endoscopului aflat n cavitatea abdominal a pacientului n care se afl poziionat camera de
luat vederi) n orice punct al cmpului operator pentru a oferi chirurgului cele mai bune informaii
vizuale. Particularitatea acestei micri o reprezint faptul c laparoscopul se va mica
ntotdeauna n jurul unui punct fix n spaiu de coordonate B X B ,YB , Z B , care reprezint punctul
de intrare al trocarului n peretele abdominal al pacientului. Prezena unei cuple cardanice naintea
punctului de fixare al laparoscopului de braul robotului, de coordonate A X A ,Y A , Z A , permite
micarea laparoscopului n jurul unui punct fix. Trocarul, punctul de intrare n peretele abdominal i
laparoscopul pot fi vzute ca i o cupl virtual cu 4 (patru) grade de libertate care permit camerei
rotaia jurul celor trei axe i translaia paralel cu axa trocarului. Astfel, laparoscopul poate fi
poziionat n orice punct al cmpului operator utiliznd acest robot paralel cu 3 (trei) grade de
libertate. Legat de comportamentul robotului, trebuie subliniat un aspect foarte important. Datorit
cuplei cardanice pasive, robotul nu poate controla laparoscopul dac acesta nu are un punct fix
de-a lungul su. De aceea, n cazul unei proceduri robotizate, laparoscopul va fi introdus manual,
prin trocar, n cavitatea abdominal a pacientului i apoi va fi conectat la robot, care va prelua apoi
controlul poziionrii laparoscopului, dup cerinele chirurgului.
Modelarea geometric a structurii paralele PARMIS
Pentru modelarea geometric este nevoie de un al doilea sistem de coordonate, mobil, ( x y z )
cu originea n punctul A, punctul de fixare al laparoscopului n cupla cardanic pasiv a robotului,
prezentat n figura 2.

Pagina 2 din 11

Fig. 2. Sistemul mobil de coordonate Ax`y`z` i unghiurile i

Parametrii geometrici ai robotului paralel sunt reprezentai de b, d, h, coordonatele punctului

B X B ,YB , Z B parametrii considerai cunoscui. Se noteaz cu h1 poriunea endoscopului aflat n
abdomenul bolnavului, i cu e h h1 poriunea endoscopului aflat n exterior.
Pentru modelarea geometric, se pornete de la urmtorul sistem de ecuaii, care stabilete o
relaie de legtur, ntre coordonatele spaiale ale punctului A X A ,Y A , Z A i coordonatele
motoare ale robotului q1, q 2 , q 3 :
r A b d 2 q 2 q1 2

(1)

X A r A cos q 3

Y A r A sin q 3

(2)

Z A q1

1. Modelul geometric direct

n cazul modelului geometric direct sunt date coordonatele motoare ale robotului, q1, q 2 , q 3 ,
alturi de care se cunosc parametrii geometrici ai structurii i trebuie determinate coordonatele
punctului E X E ,YE , Z E al efectorului final.
Pentru a putea rezolva sistemul de ecuaii, se disting dou cazuri:
Cazul 1.
Dac

( X A X B ) i (Y A YB )

Rezult:

Pagina 3 din 11

XE XB
YE YB

(3)

ZE Z A h
0, 0

Cazul 2.
Dac

( X A X B ) sau (Y A YB )

Din figura 2 rezult sistemul de ecuaii:

X B X A e sin cos

YB Y A e sin sin

(4)

Z B Z A e cos

n care:
e

X B X A 2 YB Y A 2 Z B Z A 2

(5)

Din ecuaiile (4) se determin unghiurile de rotaie i :

cos

Z A ZB
Z ZB
, sin 1 A

e
e

(6)

Constructiv, Z A Z B , adic 0 90 .
Valoarea unghiului se va exprima cu formula:

a tan 2

Z A ZB

Z A ZB

(7)

Din figura 2, pe baza relaiei 4 rezult expresiile unghiului :

Y YA
XB X A
sin B
respectiv cos
e sin
e sin

(8)

Valoarea unghiului se va exprima cu formula:

a tan 2YB Y A , X B X A

(9)

Pentru determinarea coordonatelor punctului E pe baza figurilor 1 i 2 se pot scrie relaiile:

BE X E X B i YE YB j Z E Z B k

Coordonatele punctului E se pot exprima cu relaia:

Pagina 4 din 11

(10)

 X E X B h1 sin cos

YE YB h1 sin sin
Z Z h cos
B
1
E

(11)

Din relaiile (2, 4 i 11) rezult ecuaiile finale ale modelului geometric direct:

X E X A h sin cos

YE Y A h sin sin
Z Z h cos
A
E

(12)

Modelul geometric direct a fost astfel rezolvat printr-o soluie analitic, ceea ce reprezint un
avantaj n realizarea comenzii robotului.
2. Modelul geometric invers
Modelul geometric direct urmrete determinarea coordonatelor motoare, q1, q 2 , q 3 dac se

cunosc coordonatele efectorului, final, i anume ale punctului E X E ,YE , Z E . Sunt considerai
cunoscui parametrii geometrici ai structurii i coordonatele punctului B X B ,YB , Z B .
Cunoscnd coordonatele punctului E, ntre acesta i punctul B se poate scrie relaia:
h1

XE

X B 2 YE YB 2 Z E Z B 2

(13)

Pentru rezolvarea problemei propuse, se disting, din nou, dou cazuri:

Cazul 1.
Dac
( X E X B ) i (YE YB )
Rezult:
X A XE
Y A YE
Z A ZE h
0

Cazul 2.
Dac

(14)

( X E X B ) sau (YE YB )

Din figurile (1 i 2) rezult:

X E X B h1 sin cos

YE YB h1 sin sin
Z Z h cos
B
1
E

Din relaiile (15) se determin unghiurile de rotaie i :

Pagina 5 din 11

(15)

 Z ZE
Z ZE
cos B
, sin 1 B
h1
h1

a tan 2

Z ZE
1 B
h1

ZB ZE

h1

(17)

Y YB
XE XB
sin E
respectiv cos
h1 sin
h1 sin
innd cont c

0 90 ,

(16)

(18)

din (18) rezult:

a tan 2YE YB , X E X B

(19)

X A X E h sin cos

Y A YE h sin sin

(20)

i apoi

Z A Z E h cos

Pe baza expresiilor pentru coordonatele punctului A X A ,Y A , Z A se pot determina expresiile

pentru coordonatele motoare q1, q 2 , q 3 ale robotului PARAMIS:
q1 Z A
rA

2
XA
Y A2

(21)
(22)

i pe baza relaiei (1) va rezulta

r A b 2

d 2 q 2 q1 2

q 2 q1 d 2 r A b 2

(23)

Expresia pentru q 3 fiind:

q 3 a tan 2(Y A , X A )

(24)

Modelul geometric direct a fost astfel rezolvat printr-o soluie analitic, ceea ce reprezint un
avantaj n realizarea comenzii robotului
Generarea analitic a spaiului de lucru al robotului PARAMIS
Generarea analitic a spaiului de lucru presupune gsirea unor relaii de legtur ntre valorile pe
care se pot obine n cuplele motoare i coordonatele efectorului final, astfel nct acesta s
rmn n spaiul de lucru.
Pentru a se gsi punctele spaiului de lucru al robotului, se pleac de la un set de parametrii
geometrici, care presupun cunoaterea, n afar de parametrii dimensionali ai robotului, i valorile
coordonatelor punctului B, X B ,YB , Z B .
Pe baza acestora, se determin o poziie extrem pentru elementul motor q1 , urmnd ca apoi,
ntr-o structur repetitiv de 3 cicluri imbricate s se verifice toate combinaiile de valori pentru
q1, q 2 , q3 dac valorile obinute (prin intermediul modelului geometric direct) verific sau nu
condiiile de apartenen la spaiul de lucru.

Pagina 6 din 11

Aceste condiii sunt:

Unghiul pe care l face laparoscopul cu verticala s nu depeasc 60;
Distana ntre punctul de fixare al laparoscopului n braul robotului i punctul de inserie n
abdomen s fie de minimum 50 mm; ( AB 50 mm );
Distana ntre cuplele motoare de translaie s fie de minimum 50 mm (pentru a evita
coliziunea ntre ele);
Laparoscopul trebuie s rmn introdus n cavitatea abdominal pe o lungime cel puin
egal cu cea a trocarului (80 mm) condiie impus doar pentru generarea spaiului de
lucru cu vizibilitate.
Se prezint n continuare schema logic a generrii spaiului de lucru, reprezentat n figura 3.
Pe baza schemei logice din figur, s-a realizat un program n Matlab, prin intermediul cruia se
obine un nor de puncte reprezentnd spaiul de lucru al robotului PARAMIS. Asupra acestui spaiu
de lucru trebuie fcut un amplu studiu, ntruct modificarea unor parametri afecteaz considerabil
spaiul de lucru, aa cum se va arta n exemplele urmtoare. Problema major este volumul de
calcul deoarece numrul de puncte din spaiul de lucru este: nr _ int_ q1 nr _ int_ q 2 nr _ int_ q3 la
peste 1.000.000 de puncte existnd dificulti n reprezentarea spaiului de lucru. Dei pare mare,
acest numr de puncte se poate echivala cu cate 100 de valori pe care le pot lua cuplele motoare,
ceea ce la baza inferioar a spaiului de lucru genereaz nite goluri n volumul acestuia.

Fig. 3. Schema logic de generare analitic a spaiului de lucru

Un spaiu de lucru cu vizibilitate, la un nivel de detaliu acceptabil se prezint n figura 4 n care

variaiile pentru cuplele de translaie au fost de 0.001 m (1 mm) i 0.05 grade pentru cupla de
rotaie. Cu toate c variaiile par mari numrul de puncte din care este format spaiul de lucru este
de peste 800000. Se observ ns o asemnare foarte mare ntre spaiul analitic i cel geometric,

Pagina 7 din 11

acestea validndu-se unul pe altul, demonstrnd att corectitudinea generrii spaiului geometric
ct i a ecuaiilor i condiiilor matematice impuse n spaiul generat analitic.

Fig. 4. Spaiu de lucru cu vizibilitate determinat analitic pentru robotul PARAMIS

Din pcate, aa cum se evideniaz n figura 5, n zona inferioar a spaiului de lucru apar puncte
singulare, datorit creterii distanei fa de punctul fix (B).

Fig. 5 Spaiu de lucru cu evidenierea unor puncte singulare n zona inferioar

Figurile de mai sus reflect faptul c printr-o reprezentare prin puncte, nu se poate obine un volum
unitar, cu aspectul unui corp solid, volumul de calcul pentru un numr aa de mare de puncte fiind
mult prea mare. Pentru a elimina acest neajuns au fost testate mai multe funcii de reprezentare
care s creeze o nvelitoare pentru mulimea de puncte. Acest lucru este posibil ntruct s-a artat
n acest capitol c nu exist puncte de singularitate n spaiul de lucru al robotului, deci o astfel de
nvelitoare nu ar cuprinde puncte n care robotul s fie necontrolabil. Cea mai apropiat
reprezentare a fost obinut prin utilizarea funciei matematice de nvelitoare convex.
Dezavantajul acestei funcii, dup cum sugereaz i numele ei este c nu poate reprezenta
concavitile unui volum. De aceea, cu aceast reprezentare a putut fi descris numai spaiul de
lucru efectiv al robotului, cel cu vizibilitate avnd n zona trocarului o concavitate (determinat de
trocar) ceea ce nu se poate reprezenta cu nvelitoarea convex. Figurile 6.26 i 6.27 prezint
spaiul de lucru efectiv obinut prin nvelitoarea convex, i o reprezentare a spaiului efectiv din
puncte.

Pagina 8 din 11

Fig. 6 Spaiu de lucru efectiv realizat cu ajutorul

funciei nvelitoare convex

Fig. 7 Spaiu de lucru efectiv obinut din puncte

Programul n MATLAB
%Determinarea spatiului de lucru al robotului PARAMIS utilizand modelul
%geometric direct
function w=workspace_PARAMIS()
b=0.385;
h=0.263;
d=0.68492;
XB=1.001;
YB=0;
ZB=0.281;
hilf=1;
for i=0:0.005:0.300
q(1)=0.340+i
for j=0:0.005:0.600
q(2)=0.345+j;
for k=-10:1:10
q(3)=k*pi/180;
rA=b+sqrt(d^2-(q(2)-q(1))^2);
XA=rA*cos(q(3));
YA=rA*sin(q(3));
ZA=q(1);
if (XA==XB) && (YA==YB)
XE=XB;
YE=YB;
ZE=ZA-h;
else
ee=sqrt((XA-XB)^2+(YA-YB)^2+(ZA-ZB)^2);
fi=atan2(sqrt(1-((ZA-ZB)/ee)^2),(ZA-ZB)/ee);
teta=atan2((YA-YB),(XA-XB));
XE=XA-h*sin(fi)*cos(teta);
YE=YA-h*sin(fi)*sin(teta);
ZE=ZA-h*cos(fi);
end
h1=sqrt((XB-XE)^2+(YB-YE)^2+(ZB-ZE)^2);

Pagina 9 din 11

if (acos((ZB-ZE)/h1)<60*pi/180)&&(ZB-ZE>0.050)&&(q(2)q(1)>0.020)&&(ZA-ZB>0.050)
X(hilf)=XE;
Y(hilf)=YE;
Z(hilf)=ZE;
hilf=hilf+1;
end;

end;
end;

end;

X(hilf-1)=XB;
Y(hilf-1)=YB;
Z(hilf-1)=ZB;

i=1:hilf-1;
scatter3(X(i),Y(i),Z(i),'filled');
colormap hot
grid on
axis square
view(-45,45)

% box on
% Determine the minimum and the maximum x and y values:
i=1:hilf-1;
x=X(i);
y=Y(i);
z=Z(i);
xmin = min(x); ymin = min(y);
xmax = max(x); ymax = max(y);
% Define the resolution of the grid:
xres=200;
yres=200;
% Define the range and spacing of the x- and y-coordinates,
% and then fit them into X and Y
xv = linspace(xmin, xmax, xres);
yv = linspace(ymin, ymax, yres);
[Xinterp,Yinterp] = meshgrid(xv,yv);
% Calculate Z in the X-Y interpolation space, which is an
% evenly spaced grid:
Zinterp = griddata(x,y,z,Xinterp,Yinterp);
% Generate the mesh plot (CONTOUR can also be used):
figure
meshc(Xinterp,Yinterp,Zinterp);
colormap hot
xlabel X; ylabel Y; zlabel Z;
figure
surf(Xinterp,Yinterp,Zinterp);
colormap hot
xlabel X; ylabel Y; zlabel Z;
hilf
figure
i=1:hilf-1;
x=X(i);

Pagina 10 din 11

y=Y(i);
z=Z(i);
XX=[x; y; z]';
[K,v]=convhulln(XX);
trisurf(K,XX(:,1),XX(:,2),XX(:,3))

Exerciiu suplimentar
Dac se pornete de la modelul geometric invers, ce soluie vedei pentru generarea
spaiului de lucru?
A se vedea celalalt program scris in Matlabworkspace_PARAMIS2.m

Pagina 11 din 11