Sie sind auf Seite 1von 66

Dinmica del Robot

Electiva I Robtica Industrial

Modelo dinmico de un robot


Objetivo: conocer la relacin entre el movimiento del
robot y las fuerzas implicadas en el mismo
Relacin matemtica entre:
La localizacin del robot definida por sus variables articulares o
por las coordenadas de localizacin de su extremo, y sus
derivadas: velocidad y aceleracin.
Las fuerzas y pares aplicados en las articulaciones (o en el
extremo del robot).
Los parmetros dimensionales del robot, como longitud, masas e
inercias de sus elementos.

Qu es un modelo dinmico? ejemplo


Ecuacin del modelo dinmico:

F = M&x& + Bx& + Kx Mg

Permite relacionar la evolucin de


x(t) con los cambios de F(t)
K
F

Step Response
1.4

1.2

B
x

Amplitude

0.8

0.6

0.4

0.2

6
Time (sec)

10

12

Complejidad del modelo dinmico de un


robot
Crece con el numero de GDL del robot
Interrelacin entre los movimientos de las articulaciones
(fuerzas de Coriolis)
Relaciones no lineales
No siempre es posible su obtencin en forma cerrada
(ecuaciones diferenciales de orden 2 acopladas a integrar)
En ocasiones se debe recurrir a procedimientos numricos
iterativos
Frecuentemente se realizan simplificaciones
Necesidad de incluir los actuadores y su dinmica

Utilidad del modelo


dinmico de un robot
Simulacin del movimiento del robot
Diseo y evaluacin de la estructura mecnica
del robot
Dimensionamiento de los actuadores
Diseo y evaluacin del control dinmico del
robot
Formar parte del propio algoritmo de control (en
lnea)

Modelos dinmicos directo e inverso


de un robot
Modelo dinmico directo
Expresa la evolucin temporal de las coordenadas articulares
del robot en funcin de las fuerzas y pares que intervienen
q(t)=f((t))

Modelo dinmico inverso


Expresa las fuerzas y pares que intervienen en funcin de la
evolucin de las coordenadas articulares y sus derivadas

(t)=f(q(t))

El modelo directo se resuelve por integracin del inverso

Formulaciones del modelo dinmico de


un robot
Formulacin de Newton-Euler
Basado en la segunda ley de Newton

d
F = dt (mv)
d
T = dt (I ) = I & + ( I )

L = Ec Ep
Formulacin de Lagrange
Basada en el balance energtico

d L L
i =

dt q&i qi

Formulacin de Newton. Ejemplo robot


1 gdl
Ley de Newton-Ley de Euler:
d
F = dt (mv)
d
T
=
dt (I ) = I & + ( I )

Equilibrio fuerzas/pares:

d 2
2 &&
MgLcos = I 2 =ML +MgLcos
dt

Formulacin de Lagrange. Ejemplo robot 1 gdl


L = Ec E p
d L L
i =

dt q&i qi

L : Funcin Lagrangiana.
Ec: energa cintica.
Ep: energa potencial.
i: fuerza o pares aplicado sobre qi.
qi: coordenadas generalizadas (articulares).

1 &2
Ec = I con I = ML2
2

E p = Mgh = MgLsen

1
L = Ec Ep = ML2& 2 MgLsen
2

L
= MgLcos

L
2&
=

ML
&
d L
= ML2&&

&
dt

d L L

=
dt &

ML2&&
+ MgLcos =

Formulacin de Newton. Ejemplo robot 2 gdl (1)


2

Robot Polar en disposicin


tumbada (con m1=0)

2
2
1

1
1

Expresiones de Newton-Euler:
d
F = dt (mv)
d
T
=
dt (I )

T1 =

d
( I &1 ) + m 2 gd 2 C1
dt

F2 = m2 d&&2 + m2 gS1 m 2 d 2&12

Formulacin de Newton. Ejemplo robot 2 gdl (2)


Momento de Inercia I de la masa m2 :

I = m2 d

2
2

m2

d2

d & d
(I1 )= (m2 d 22&1 ) = 2m2 d 2&1d&2 + m2 d 22&&1
dt
dt

T1
1
y0

F2
x0

T1 =

d &
( I1 ) + m2 gd2C1
dt

F2 = m2 d&&2 + m2 gS1 m2 d2&12

2 &&
&
&
T1 = 2m2 d 21d 2 + m2 d 2 1 + m2 gd 2C1
F = m d&& + m gS m d & 2
2

2 1

En forma matricial:

T1 m2 d 22
F =
2 0

0 &&1 0
2m2 d 2&1 &1 m2 gd 2C1
& +
&& +

&
m2 d 2 m2 d 21
0 d 2 m2 gS1

Formulacin de Lagrange.
Ejemplo robot polar 2 gdl (1)
Robot Polar en disposicin
tumbada (con m1=0)

2
2

2
1

1
1

Coordenadas y velocidades de la masa m2:


x& 2 = ( S 1 d& 2 + C1 d 2&1 )
x 2 = d 2 S1

&
&
y 2 = d 2 C1
y& 2 = C1 d 2 S1 d 2 1
v 22 = x& 22 + y& 22 = ( S1 d& 2 + C1 d 2&1 ) 2 + ( C1 d& 2 S 1 d 2&1 ) 2 =
= S 12 d& 22 + C 12 d 22&12 + 2 S1 d& 2 C1 d 2&1 + C 12 d& 22 + S12 d 22&12 2 C1 d& 2 S1 d 2&1
= d& 22 + d 22&12

Formulacin de Lagrange.
Ejemplo robot polar 2 gdl (2)
2
2 &2
&
v = d2 + d 2 1
2
2

Energa cintica:

Energa potencial:

Lagrangiana:

1
1
2
Ec = m2v2 = m2 (d&22 + d 22&12 )
2
2

E p = gm2 h2 = gm2 y2 = gm2 d 2 S1

1
L = Ec E p = m2 (d&22 + d 22&12 ) gm2 d 2 S1
2

Formulacin de Lagrange.
Ejemplo robot polar 2 gdl (3)
1
L = m2 (d&22 + d 22&12 ) gm2 d 2 S1
2
&i y sus derivadas respecto del tiempo:
Derivadas respecto de q
L
L
2 &
&
=
m
d
= m2 d 2 1
2 2
d&2
&1
d L
d L
2 &&
&
&
= m2 d&&2
= m2 d 2 1 + 2m2 d 21d 2
dt q&2
dt q&1
Derivadas respecto de qi y sus derivadas respecto del tiempo:

L
= gm2 d 2C1
1

L
= m2 d 2& 2 gm2 S1
d2
1

Formulacin de Lagrange.
Ejemplo robot polar 2 gdl (4)
d L L
i =

dt q&i qi
T1 = m2 d 22&&1 + 2m2 d 2&1d&2 + gm2 d 2C1

&& m d & 2 + gm S
F
=
m
d
2 2
2 2 1
2 1
2
En forma matricial:

T1 m2 d 22
F =
2 0

0 &&1 0
2m2 d 2&1 &1 m2 gd 2C1
& +
&& +

&
m2 d 2 m2 d 21
0 d 2 m2 gS1

Formulacin de Lagrange.
Ejemplo robot articular 2 gdl (1)
Coordenadas y velocidades de los centros de masas:

y2

Masa elemento 1:

x1 = d1C1 x&1 = d1S1q&1


2
2
2
2 2
&
&
&

v
=
x
+
y
=
d

1
1
1
1 q1
y1 = d1S1 y&1 = d1C1q&1

x2
m2

y0
q2
1

q1

m1

Masa elemento 2:

x2 = l1C1 + d 2C12
x&1 = (l1S1 + d 2 S12 )q&1 d 2 S12 q&2

y2 = l1S1 + d 2 S12
y&1 = (l1C1 + d 2C12 )q&1 + d 2C12 q&2
v22 = x&22 + y& 22 = (l12 + d 22 + 2l1d 2C2 )q&12 + d 22 q&22 + 2d 2 (l1C2 + d 2 )q&1q&2

x0

Formulacin de Lagrange.
Ejemplo robot articular 2 gdl (2)
v22 = (l12 + d 22 + 2l1d 2C2 )q&12 + d 22 q&22 + 2d 2 (l1C2 + d 2 ) q&1q&2

v12 = x&12 + y&12 = d12 q&12


Ec =
Energa cintica:

Energa potencial:

1
( m1v12 + m2 v22 ) =
2

1
1
1
m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) q&12 + m2 d 22 q&22 + [ m2 2d 2 (l1C2 + d 2 ) ] q&1q&2
2
2
2

E p = g (m1h1 + m2 h2 ) = g (m1 y1 + m2 y2 ) =
= g (m1d1S1 + m2l1S1 + m2 d 2 S12 )

L = Ec E p =
Lagrangiana:

1
1
m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) q&12 + m2 d 22 q&22 +
2
2

1
[ m2 2d 2 (l1C2 + d 2 )] q&1q&2 g (m1d1S1 + m2l1S1 + m2 d 2 S12 )
2

Formulacin de Lagrange.
Ejemplo robot articular 2 gdl (3)
L = Ec E p =

Lagrangiana:
+

1
1
m 1 d 12 + m 2 ( l12 + d 22 + 2 l1 d 2 C 2 ) q&12 + m 2 d 22 q& 22 +
2
2

1
[ m 2 2 d 2 ( l1C 2 + d 2 ) ] q&1 q& 2 g ( m1 d 1 S 1 + m 2 l1 S 1 + m 2 d 2 S 12 )
2

&i y sus derivadas respecto del tiempo:


Derivadas respecto de q

L
= m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) q&1 + [ m2 d 2 (l1C2 + d 2 )] q&2
q&1
d L
= m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) q&&1 + [ m2 (2l1d 2 S2 q&2 )] q&1 + [ m2 d 2 (l1C2 + d 2 )] q&&2 + [ m2 d 2 (l1S2 q&2 )] q&2 =
dt q&1
= m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) q&&1 [ m2 d 2 (l1C2 + d 2 ) ] q&&2 [ 2m2l1d 2 S2 ] q&1q&2 [ m2 d 2l1S2 ] q&22

L
= m2 d 22 q&2 + [ m2 d 2 (l1C2 + d 2 ) ] q&1
q&2
d L
= m2 d 22 q&&2 + [ m2 d 2 (l1C2 + d 2 ) ] q&&1 + [ m2 d 2 (l1S 2 q&2 )] q&1 =
dt q&2
= m2 d 22 q&&2 + [ m2 d 2 (l1C2 + d 2 ) ] q&&1 [ m2 d 2l1S2 ] q&1q&2

Formulacin de Lagrange.
Ejemplo robot articular 2 gdl (4)

Lagrangiana:

1
2
2
2
2 1
2
2
&
&

L = Ec Ep = md
+
m
(
l
+
d
+
2
l
d
C
)
q
+
md
q
1 1
2 1
2
1 2 2 1
2 2 2 +

2
2
1
&1 &2 g(md
+ [ m2 2d2(lC
1 2 + d2 )] qq
1 1S1 + ml
2 1S1 + md
2 2S12 )
2

Derivadas respecto de qi

L
= g [ (m1d1 + m2l1 )C1 + m2 d 2C12 ]
q1
L
= [ m2l1d 2 S 2 ] q&12 [ m2 d 2l1S 2 ] q&1q&2 gm2 d 2C12
q2

Formulacin de Lagrange.
Ejemplo robot articular 2 gdl (5)
Expresin de Lagrange

i =

d L L

dt q&i qi

T1 = m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) q&&1 + [ m2 d 2 (l1C2 + d 2 ) ] q&&2 [ 2m2l1d 2 S 2 ] q&1q&2 [ m2 d 2l1S 2 ] q&22 +

+ g [ (m1d1 + m2l1 )C1 + m2 d 2C12 ]

2
2
T2 = [ m2 d 2 (l1C2 + d 2 )] q&&1 + m2 d 2 q&&2 [ m2 d 2l1S2 ] q&1q&2 + [ m2l1d 2 S 2 ] q&1 + [ m2 d 2l1S 2 ] q&1q&2 + gm2 d 2C12

En forma matricial:

T1 m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) m2 d 2 (l1C2 + d 2 ) q&&1


&& +
T =
2
m2 d 2 (l1C2 + d 2 )
m2 d 2
2
q2
2m2l1d 2 S2 q&2
+
m2l1d 2 S2 q&1

m2 d 2l1S 2 q&2 q&1


q& +
0
2

g [ (m1d1 + m2l1 )C1 + m2 d 2C12 ]


+

gm2 d 2C12

Ecuacin dinmica de un robot


multiarticular
Expresin general del modelo dinmico de un robot:

&& + H (q, q& ) + C(q )


= D ( q )q
Con:
q : vector de coordenadas articulares.
: vector de fuerzas o pares que se aplica a cada articulacin.
D (q ) : la matriz de inercias, de dimensin (nxn), cuyos elementos son funcin de q.
&
H (q, q& ) : matriz (nx1) de fuerzas de Coriolis, dependiente de q y q.
C (q ) : matriz (nx1) de fuerzas de gravedad, dependiente de q.
n : nmero de grados de libertad del robot.
Nota: El trmino de fuerzas de Coriolis puede expresarse alternativamente como Vector (nx1)
o como producto de una Matriz (nxn) por el Vector de velocidades (nx1)

Ejemplo: Modelo dinmico de un robot de


2 grados de libertad
T1 m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) m2 d 2 (l1C2 + d 2 ) q&&1
&& +
T =
2
m2 d 2 (l1C2 + d 2 )
m2 d 2
2
q2

&& + H + C
= Dq
y2

x2
m2

d2

y0

q2

2m2l1d 2 S2 q&1q&2 m2 d 2l1S 2 q& 22


+

2
&
m
l
d
S
q

21 2 2 1

g [ (m1d1 + m2l1 )C1 + m2 d 2C12 ]


+

gm
d
C

2 2 12

l
d

q1

m1
x0

O con el trmino de Corilis


expresado matricialmente:

T1 m1d12 + m2 (l12 + d 22 + 2l1d 2C2 ) m2 d 2 (l1C2 + d 2 ) q&&1


&& +
T =
m2 d 2 (l1C2 + d 2 )
m2 d 22
2
q2
2m2l1d 2 S2 q&2
+
m2l1d 2 S 2 q&1

m2 d 2l1S 2 q&2 q&1


q& +
0
2

g [ (m1d1 + m2l1 )C1 + m2 d 2C12 ]


+

gm2 d 2C12

Algoritmos computacionales
Para robots con ms de 3 gdl la deduccin analtica se hace
excesivamente compleja.
Alternativamente se han desarrollado algoritmos que permiten
obtener el valor del par a partir de q(t) en pasos incrementales

Lagrange
Basado en la representacin de D-H
Poca eficiencia computacional: O(n4) (n=n GDL)
Ecuaciones finales bien estructuradas (D,H,C por separado)

Newton-Euler
Basado en operaciones vectoriales
Mayor eficiencia computacional: O(n)
Ecuaciones finales no estructuradas (D,H,C sumados)

Algoritmo computacional de Lagrange (1)


1. Asignar a cada barra un sistema de referencia de acuerdo D-H.
2. Obtener las matrices de transformacin 0Ai para cada barra i.
3. Obtener las matrices Uij definidas por
Uij= 0Ai /qj = 0Aj-1 Qj j-1Ai
[0]

si j < i
si j>i

4. Obtener las matrices Uijk definidas por


Uijk=Uij/qk =

0A Q j-1A
k-1A
j-1 j
k-1 Qk
i
0A
k-1A
j-1A
k-1 Qk
j-1 Qj
i

[0]
0 1
1 0
Qi =
0 0

0 0

0 0
0 0
0 0

0 0

0
0
si la articulacin i es de rotacin Qi =
0

si i > k > j
si i > j > k
si i < j o i < k

0 0 0
0 0 0
0 0 1

0 0 0

si la articulacin i es de traslacin

Algoritmo computacional de Lagrange (2)


5. Obtener las matrices de PseudoInercias Ji para cada barra i.
xij2mj xijyijmj xijzijmj xijmj
yijxijmj yij2mj yijzijmj yijmj

Ji =

zijxijmj zijyijmj zij2mj zijmj


xijmj

yijmj

zijmj

mj

Donde los suponen los productos de cada masa mj de la barra por su


distancia a los ejes Xi, Yi o Zi de esa barra. (Si la distribucin de masa
no es discreta se transformarn en Integrales)

6. Obtener la matriz de Inercia D definida por


dij= Tr (Ukj Jk UTki)
k=max (i,j)

i,j=1,2...n

Algoritmo computacional de Lagrange (3)


7. Obtener los trmino hikm definidos por
Con i,k,m = 1,2,...,n .

h ikm =

j=max(i,k,m)

Traza ( U jkm J jUTji )

8. Obtener el vector columna H de


fuerzas de Coriolis y Centrifugas,
cuyos elementos son

h i = h ikm q&k q&m

9. Obtener el vector columna C de


Fuerzas de Gravedad, cuyos
elementos son:

ci = ( m j gU ji j r j )

k=1 m=1

j =1

g: es el vector de gravedad expresado en el sistema de la base {S0}y viene expresado por


(gx0 , gy0 , gz0 , 0)
ir : es el vector de coordenadas homogneas del centro de masas del elemento j expresado
j
en el sistema de referencia del elemento i.

Algoritmo computacional de Lagrange (4)


10. La ecuacin del modelo Dinmico es:

=Dq+H+C
Notas:
1.

g es el vector gravedad expresado en el sistema {0}

2.

es el vector de fuerzas y pares motores efectivos aplicados sobre


cada coordenada generalizadas

3.

las matrices Ji y D son simtricas y semidefinidas positivas

Caractersticas del mtodo computacional de


Lagrange
Utiliza el lgebra matricial, implicando un elevado
nmero de operaciones O(n4)
En la solucin parecen explcitamente los trminos de
Inercia, Coriolis y Gravedad
Esto permite que si se desea se puede prescindir del
trmino H (fuerzas de coriolis y centrifugas) (por
ejemplo si las velocidades son lentas)
Se han desarrollado otros mtodos computacionales
ms eficientes a partir de la formulacin de Lagrange

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (1)
% Algoritmo de Lagrange
% Para la solucin del modelo dinmico inverso de un robot
%---------------------------------------------------------% Robot cilndrico 2 GDL (RD)
%(ver ej. 5.1 libro "Fundamentos de Robtica")
% Definicin de la trayectoria
%-----------------------------% Definicin simblica de la posicin
st1='sin(t)*t+pi/2';
sd2='sin(5*t)+t+1';
% Obtencin simblica de la velocidad y aceleracin
svt1=diff(st1,'t');
svd2=diff(sd2,'t');
sat1=diff(svt1,'t');
sad2=diff(svd2,'t');

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (2)
% Parmetros dimensionales
L1=10;
m1=20;
m2=10;
g=[0 0 -9.8 0];
r11=[0 0 L1 1]';
r22=[0 0 0 1]';
%Matrices de Derivacin
Qr=zeros(4);Qr(1,2)=-1;Qr(2,1)=1;
Qd=zeros(4);Qd(3,4)=1;

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (3)
% Bucle de paso de tiempo
%-------------------------for tk=1:1:50;
t=(tk-1)/10;
% Evaluacin numrica de posicin, velocidad y aceleracin
%-------------------------------------------------------t1=eval(st1);Q1(tk)=t1;
d2=eval(sd2);Q2(tk)=d2;
vt1=eval(svt1);
vd2=eval(svd2);
at1=eval(sat1);
ad2=eval(sad2);

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (4)
% PASO 1-2 Obtencin de las matrices de transformacin Aij
%--------------------------------------------------------% Matrices A00 y A11 son la identidad
A00=eye(4);
A11=eye(4);
%Evalua las matrices A01 y A02
A01 =[cos(t1) 0 -sin(t1) 0;
sin(t1) 0 cos(t1) 0;
0
-1 0
0;
0
0 0
1];
A12 =[1 0 0 0 ;
0100 ;
0 0 1 d2;
0 0 0 1 ];
%Evalua la matriz A02=A01*A2
A02=A01*A12;

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (5)
% PASO 3 Evaluacion matrices Uij
%-------------------------------U11=A00*Qr*A01;
U12=zeros(4);
U21=A00*Qr*A02;
U22=A01*Qd*A12;
%PASO 4 Evaluacin de las matrices Uijk
%-------------------------------------U111=A00*Qr*A00*Qr*A01;
U112=zeros(4);
U121=zeros(4);
U122=zeros(4);
U211=A00*Qr*A00*Qr*A02;
U212=A00*Qr*A01*Qd*A12;
U221=A00*Qr*A01*Qd*A12;
U222=A01*Qd*A11*Qd*A12;

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (6)
% PASO 5 Evaluacin matrices de pseudoinercia Ji
%----------------------------------------------J1=zeros(4);J1(3,3)=L1^2*m1;J1(3,4)=L1*m1;J1(4,3)=L1*m1;J1(4,4)=m1;
J2=zeros(4);J2(4,4)=m2;

% PASO 6 Evaluacin de la matriz de Inercias D


%---------------------------------------------D(1,1)=trace(U11*J1*U11')+trace(U21*J2*U21');
D(1,2)=trace(U22*J2*U21');
D(2,1)=D(1,2);
D(2,2)=trace(U22*J2*U22');

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (7)
% PASO 7-8 Evaluacin de la matriz de Coriolis H
%-----------------------------------------------h111=trace(U111*J1*U11')+trace(U211*J2*U21');
h112=trace(U212*J2*U21');
h121=trace(U221*J2*U21');
h122=trace(U222*J2*U21');
h211=trace(U211*J2*U22');
h212=trace(U212*J2*U22');
h221=trace(U221*J2*U22');
h222=trace(U222*J2*U22');
H(1,1)=h111*vt1*vt1 + h112*vt1*vd2 + h121*vd2*vt1 + h122*vd2*vd2;
H(2,1)=h211*vt1*vt1 + h212*vt1*vd2 + h221*vd2*vt1 + h222*vd2*vd2;

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (7)
% PASO 9 Evaluacin de la matriz de Gravedad C
%---------------------------------------------C(1,1)=-m1*g*U11*r11-m2*g*U21*r22;
C(2,1)=-m1*g*U12*r11-m2*g*U22*r22;

% PASO 10 Evaluacin de los pares


%---------------------------------PARES(:,tk)=D*[at1 ad2]'+H+C;
end % fin del bucle de paso de tiempo

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (8)
% Presentacin Grfica de los resultados
figure(2);
clf
subplot(2,2,1),plot(Q1),title('Q1')
subplot(2,2,2),plot(PARES(1,:)),title('PAR Q1')
subplot(2,2,3),plot(Q2),title('Q2')
subplot(2,2,4),plot(PARES(2,:)),title('PAR Q2')
text(70,2750,'LE')

Ejemplo programacin del mtodo de


Lagrange para un robot 2 gdl (9)

Algoritmo computacional de Newton Euler


Basado en la 2 ley de Newton F=ma
Es un procedimiento recursivo en el que para
i=1,..,n se calcula:

wi (Velocidades angular del sistema {Si}: qi)


wi (Aceleraciones angular del sistema {Si}: qi)
vi,ai (Aceleraciones lineales del sistema {Si} y del centro de masa de la barra i)
i=n,..1

se calcula:

Fi (Fuerza ejercida en el centro de masa de la barra i)


Ni (Par ejercido en el centro de masa de la barra i)
fi (Fuerza ejercida sobre la articulacin i-1, unin barra i-1 con barra i, expresada en {i-1})
ni (Par ejercido sobre la articulacin i-1, unin barra i-1 con barra i,expresada en {i-1})

Algoritmo computacional de Newton Euler


(1)

N-E 1. Asignar a cada eslabn un sistema de referencia


de acuerdo con las normas de D-H

Algoritmo computacional de Newton Euler


(2)

N-E 2. (1) Establecer las condiciones iniciales para la


base del robot
0
0 : velocidad angular=[0,0,0]T
0

& 0 : aceleracion angular=[0,0,0]T

v 0 : velocidad lineal= [0,0,0]T

v& 0 : aceleracion lineal= -[g x0 ,g y0 ,g z0 ]T

& 0 y 0 v 0 son tpicamente nulos salvo que la base del robot est
0 , 0
en movimiento

[ g x 0 , g y0 , g z 0 ]

es el vector de gravedad expresado en el sistema


{S0} (habitualmente toma el valor [0, 0, -9.8] pues z0
se sita vertical hacia arriba).

Algoritmo computacional de Newton Euler


(3)

N-E 2. (2) Establecer las condiciones iniciales para el


extremo del robot
Para el extremo del robot se conocer la fuerza y el par ejercidos
externamente n+1fn+1 y n+1nn+1

Otras condiciones iniciales:


z 0 = [ 0, 0,1]

pi = Vector que une el origen de {Si-1} con el de {Si } expresadas en {Si } = [ ai , di sen( i ), di cos( i ) ]

si = Coordenadas del centro de masas del eslabon i respecto del sistema {Si }

I i = Matriz de inercia del eslabn i expresado en un sistema paralelo al {Si }


y con el origen en el centro de masas del eslabn.

Algoritmo computacional de Newton Euler


(4)

N-E 3. Obtener las matrices de rotacin i-1Ri y sus inversas


i

R i 1 =

i 1

Ri

=(

i 1

Ri

siendo:
Ci
i 1
R i = Si
0

C i Si
C i Ci
S i

S i Si
S i Ci
C i

Algoritmo computacional de Newton Euler


(5)
Para i=1 hasta n hacer:
N-E 4. Obtener la velocidad angular del sistema {Si}

i R i 1 ( i 1i 1 + z 0 q& i )
i
i =
i
i 1
R
i 1 i 1

si el eslabn i es de rotacin
si el eslabn i es de traslacin

N-E 5. Obtener la aceleracin angular del sistema {Si}


i 1
i R i 1 ( i 1& i 1 + z 0 &&
q
i 1 z 0 q& i
+
i)
i
& i =
i
i 1
R
i 1 & i 1

si el eslabn i es de rotacin
si el eslabn i es de traslacin

Algoritmo computacional de Newton Euler


(6)
N-E 6. Obtener la aceleracin lineal del sistema {Si}
i& i i p i + ii ( ii i pi ) + i R i 1 i 1 v& i 1
si el eslabn i es de rotacin

i
v& i = i R i 1 ( z 0 &&
q i + i 1 v& i 1 ) + i& i i p i + 2 ii i R i 1z 0 q& i +

+ ii ( ii i p i )
si el eslabn i es de traslacin

N-E 7. Obtener la aceleracin lineal del centro de


gravedad del eslabn i
i

i
i
i
i
i
&
ai = i si + i ( i si ) + v& i
i

Algoritmo computacional de Newton Euler


(7)
Para i=n hasta 1 hacer :
N-E 8. Obtener la fuerza ejercida sobre el eslabn i
i

fi = i R i +1 i +1 fi +1 + mi i ai

N-E 9. Obtener el par ejercido sobre el eslabn i


i

ni = i R i +1 i +1 ni +1 + ( i +1 R i i pi ) i +1 fi +1 + ( i pi + i si ) mi i ai + i I i i& i + ii ( i I i ii )

Algoritmo computacional de Newton Euler


(8)
N-E 10. Obtener la fuerza o par aplicado sobre la
articulacin

i nTi i R i 1z 0
i = i T i
fi R i 1z 0

si el eslabn i es de rotacin
si el eslabn i es de traslacin

Caractersticas del mtodo computacional


de N-E
Es un mtodo recursivo: obtiene informacin de la
articulacin i a partir de la i-1 (o viceversa)
Utiliza el lgebra vectorial
Su orden de complejidad computacional es O(n)
Proporciona el valor del par total (sin separar las
componentes D, H y C

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (1)
% Algoritmo

de Newton Euler Para la solucin del

modelo
% dinmico inverso de un robot
%---------------------------------------------------------% Robot cilndrico 2 GDL (RD)
%(ver ej. 5.2 libro "Fundamentos de Robtica")
% Definicin de la trayectoria y fuerzas en TCP
%----------------------------------------------% Definicin simblica de la posicin
st1='sin(t)/100+pi/2';
sd2='sin(5*t)+1';

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (2)
% Obtencin simblica de la velocidad y aceleracin
svt1=diff(st1,'t');
svd2=diff(sd2,'t');
sat1=diff(svt1,'t');
sad2=diff(svd2,'t');
%Fuerzas en el extremo
sf3='[0 0 0]';
sn3='[0 -t 0]';

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (3)
% Parmetros iniciales
%----------------------------------% Velocidades y aceleraciones de la base del robot
w0=[0 0 0]';
dw0=[0 0 0]';
v0=[0 0 0]';
dv0=[0 0 9.8]';
% Parmetros dimensionales
L1=10;
m1=20;
m2=10;
z0=[0 0 1]';

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (4)
% Matriz de rotacin entre {S2} y TCP ({S3})
%--------------------------------------------R23=eye(3);
% Bucle de paso de tiempo
%-------------------------for tk=1:1:50;
t=(tk-1)/10;

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (5)
% Evaluacin numrica de posicin, velocidad y aceleracin
%-------------------------------------------------------t1=eval(st1);Q1(tk)=t1;
d2=eval(sd2);Q2(tk)=d2;
vt1=eval(svt1);
vd2=eval(svd2);
at1=eval(sat1);
ad2=eval(sad2);
%Evaluacin numrica de las fuerzas y pares en el TCP
%----------------------------------------------n3=eval(sn3)' ;
f3=eval(sf3)' ;

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (6)
% PASO 1-2 Obtencin de las matrices de rotacin R01 y R12
%--------------------------------------------------------R01 =[cos(t1) 0 -sin(t1);sin(t1) 0 cos(t1);0 -1 0];
R12 =[1 0 0;0 1 0;0 0 1];
% PASO 3 Evaluacin de los vectores p,s y de la matriz de inercias
%-----------------------------------------------------------------p1=[0 0 0]';
%Coordenadas de {S1} respecto {S0}
p2=[0 0 d2]';
%Coordenadas de {S2} respecto {S1}
s1=[0 0 L1]';
%Coordenadas del cdg de 1 respecto {S1}
s2=[0 0 0]';
%Coordenadas del cdg de 2 respecto {S2}
I1=zeros(3);
%Inercias de 1 respecto su cdg en la base {S1}
I2=zeros(3);
%Inercias de 2 respecto su cdg en la base {S2};

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (7)
%PASO 4 Evaluacin de las velocidades angulares de {Si}
%-----------------------------------------------------w1=R01'*(w0+z0*vt1);
w2=R12'*w1;
% PASO 5 Evaluacin de las aceleraciones angulares de {Si}
%----------------------------------------------------------dw1=R01'*(dw0+z0*at1)+cross(w0,z0*vt1);
dw2=R12'*dw1;

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (8)
% PASO 6 Evaluacin de las aceleraciones lineales de {Si}
%---------------------------------------------------------dv1=cross(dw1,p1)+cross(w1,cross(w1,p1))+R01'*dv0;
dv2=R12'*(z0*ad2+dv1)+cross(dw2,p2)+cross(2*w2,(R12'*z0*vd2))+
cross(w2,cross(w2,p2));
% PASO 7 Evaluacin de la aceleracin angular de los cdg
%------------------------------------------------------a1=cross(dw1,s1)+cross(w1,cross(w1,s1))+dv1;
a2=cross(dw2,s2)+cross(w2,cross(w2,s2))+dv2;

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (9)
% PASO 8 Evaluacin de las fuerzas sobre los eslabones
%----------------------------------------------------f2=R23*f3+m2*a2;
f1=R12*f2+m1*a1;
% PASO 9 Evaluacin de los pares sobre los eslabones
%----------------------------------------------------n2=R23*(n3+cross((R23*p2),f3))+cross((p2+s2),m2*a2)+I2*dw2+
cross(w2,(I2*w2));
n1=R12*(n2+cross((R12*p1),f2))+cross((p1+s1),m1*a1)+I1*dw1+
cross(w1,(I1*w1));

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (10)
% PASO 10 Evaluacin de los pares
%---------------------------------P2=f2'*R12'*z0;
P1=n1'*R01'*z0;
PARES(1,tk)=P1;
PARES(2,tk)=P2;
PARES2(1,tk)=T1;
PARES2(2,tk)=F2;
%-.-.-.-.--.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.
end % fin del bucle de paso de tiempo

Ejemplo programacin del mtodo N-E para


un robot 2 gdl (11)
% Presentacin Grfica de los resultados
figure(1);
clf
subplot(2,2,1),plot(Q1),title('Q1')
subplot(2,2,2),plot(PARES(1,:)),title('PAR Q1')
subplot(2,2,3),plot(Q2),title('Q2')
subplot(2,2,4),plot(PARES(2,:)),title('PAR Q2')
text(70,2750,'NE')

Comparacin de mtodos

Modelo dinmico en Variables de Estado


Permite resolver computacionalmente el modelo directo:
1
&
&
&
&
&
&
Dq + H + C = Dq + N = q = D [ N ]

d q q& 0
=
+ 1

1
dt q& D N D
d q 0 I q 0
1
=
+
u
con
u
=
D
( N)

dt q& 0 0 q& I
Partiendo de unas condiciones iniciales de q y dq/dt y de los valores de en cada
instante de tiempo, se evaluaran los valores del vector u y a partir de l la
derivada del vector de estado (q,dq/dt). A partir de ella se evala el estado en el
siguiente instante mediante:

q (k + 1) = q& (k )t + q (k )

Modelo dinmico en el espacio de la tarea


& = vx , v y , vz , wx , wy , wz

&& q
&& = J 1 && J 1J& q&
& = J q& && = J& q& + J q
Potencia = Par velocidad

T T & = T q&

TT & = T q& TT J q& = T q& TT J = T

= JT T

Sustituyendo en la ecuacin del modelo dinmico q& y


&& + H + C
= Dq
&& +H+C
J T T = DJ 1&& DJ 1Jq
& & + ( JT ) H + ( JT ) C
T = ( J T ) DJ 1&& ( J T ) DJ 1Jq
1

con

D = ( J T )1 DJ 1
j

T 1
1 &
&)
H
J
=
j ( ) ( H DJ Jq

1
C j = ( J T ) C

T = D j&& + H j + C j
&

Modelado de los actuadores y sistemas de


trasmisin
Adems de la dinmica de la estructura mecnica
del brazo, es necesario considerar la de los
sistemas actuadores (elctricos, hidralicos o
neumticos) y elementos de trasmisin (reductores
includos).
En ambos casos deben ser considerables las no
linealidades tipo saturacin, zona muerta, etc.

Motor de Corriente Continua

Motor CC. Diagrama de bloques y


funciones de transferencia

&( s )
u (s)

k p k1k2

( R + ki k2 )( Js + B ) + k p ( kb + kT k1k2 )

km
Tm s + 1

k p k1k2 ( Js + B )
Tm s + 1
T ( s)

=
= km
u ( s ) ( R + ki k2 )( Js + B ) + k p ( kb + kT k1k2 )
Tm s + 1

Preguntas