R 23432

Dinmica del robot
Modelo dinmico de un eslabn rgido

Formulaciones del modelo dinmico
Algoritmos computacionales
Modelo dinmico en el espacio de la tarea
Robotica Industrial- Dinmica 1
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.
Complejidad del modelo
dinmico de un robot
? Crece con el numero de GDL del robot
? Interactividad entre movimientos (fuerzas de Coriolis)
? 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
? Caso especial: robot flexibles
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
?Aplicaciones en que se debe controlar la fuerza
ejercida por el extremo del robot
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
? Modelo dinmico inverso
expresa las fuerzas y pares que intervienen en funcin de la
evolucin de las coordenadas articulares y sus derivadas
Formulaciones del modelo
dinmico de un robot
? Formulacin de Lagrange
Basada en el balance energtico
?Funcin Lagrangiana: L =? -?
?Ecuacin de Lagrange: d/dt(?L/?q
i
)-?L/?q
i
=?
? Formulacin de Newton-Euler
Basado en la segunda ley de Newton
?? F=mv
?T=I? +? x (I? )
? Otras formulaciones
Formulacin de Newton. Ejemplo
robot rgido monoarticular
? ? F v T I I ? ? ? ? ? ? m? ? ? ? ?
Ley de Newton-Ley de Euler:
Equilibrio fuerzas/pares: ?
?
? ? ? ? ? I
d
dt
MgLcos = ML + MgLcos
2
2
2
??
Formulacin de Lagrange. Ejemplo
robot rgido monoarticular
d
dt
-
i i
?
?
?
?
?
_
_ ?
_ L L
L k
q q
? ?
? u
q
i
: coordenadas generalizadas (en este caso las articulares).
?: vector de fuerzas y pares aplicados en las q
i
.
L : Funcin Lagrangiana.
k: energa cintica.
u: energa potencial
k
?
?
1
2
2
I ?
donde I = ML
2
u ? ? Mgh MgLsen?
L = k - = u
?
1
2
ML MgLsen
2
? ?
2
?
?
??
?
?
??
?
?
??
?
L
L
L
? ?
?
?
MgLcos
ML
d
dt
ML
2
2
?
?
?
??
ML MgLcos =
2??
? ? ? ?
Formulacin de Lagrange. Ejemplo
robot rgido con 2 articulaciones
F
P
?
?
x
L/2
Mg
? ?
) ( ) , ( ) (
: matricial cin representa una Admite
cos ) ( ) ( 2 ) (
sen
)
2
1
( 2 )
2
1
(
12
1
Siendo
cos )
2
1
(
sen
tiempo el con vara J variar x, Al
) ( cos
) ( sen
2
1
2
1 2
2
1 2
12
1
2 2
q g q q h q q D T
L Mg L M L M ML P
Mg M F
L M J L M ML J
J J L Mg P
M Mg F
J
dt
d
Mgx P
M
dt
d
Mg F
?
?
?
? ?
?
?
? ?
? ?
?
?
? ? ? ?
? ?
?
?
? ? ?
?
?
?
? ? ? ? ? ? ?
? ?
? ? ? ? ? ?
?
?
?
?
?
? ? ? ?
? ?
?
?
?
?
?
?
?
? ?
? ?
? ? ? ? ? ? ?
? ?
? ? ?
? ? ? ?
? ?
? ?
? ?
Ecuacin dinmica de un robot
multiarticular
?(t) = D[q(t)] q(t) + h[q(t),q(t)] + c[q(t)]
? ?(t) = vector (n)de par generalizado aplicado a las ecuaciones
? q(t)= vector (n)de las variables de las articulaciones del brazo
? q(t) y q(t) = vectores (n) de la velocidad y aceleracin del
brazo
? D(q) = matriz (nxn) simtrica inercial denominada matriz de
inercia
? h(q,q) = vector (n)de fuerzas de Coriolis y centrfuga
? c(q) = vector (n) de las fuerzas de carga gravitatoria
Resolucin del modelo dinmico
para un robot de n gdl
? La obtencin del modelo analtico no es factible para
robots de 3 o ms gdl
? Hay que recurrir a algoritmos computacionales que
obtienen la solucin de manera numrica sin necesidad de
obtener una expresin explcita del modelo.
Estos algoritmos, para una trayectoria determinada (discretizada en pasos),
van evaluando los pares a desarrollar por cada articulacin paso a paso
? Esta resolucin puede ser necesaria para:
Realizar un control del robot con tcnicas avanzadas como
inversin del modelo, control adaptativo u otras
Realizar una simulacin para el dimensionamiento del robot
(estructura mecnica y actuadores) en la fase constructiva
Algoritmos computacionales
? Lagrange
Basado en la representacin de D-H
Poca eficiencia computacional: O(n
4
) (n=n GDL)
Ecuaciones finales bien estructuradas
? Newton-Euler
Basado en operaciones vectoriales
Mayor eficiencia computacional: O(n)
Ecuaciones poco estructuradas
Algoritmo computacional L-E (1)
1. Asignar a cada barra un sistema de referencia de acuerdo D-H.
2. Obtener las matrices de transformacin
0
A
i
para cada barra i.
3. Obtener las matrices U
ij
definidas por
U
ij
=?
0
A
i
/ ?q
j
=
0
A
j-1
Q
j
j-1
A
i
si j < i
[0] si j>i
4. Obtener las matrices U
ijk
definidas por
U
ijk
=? U
ij
/ ?q
k
=
0
A
j-1
Q
j
j- 1
A
k- 1
Q
k
k-1
A
i
si i > k > j
0
A
k-1
Q
k
k-1
A
j- 1
Q
j
j-1
A
i
si i > j > k
[0] si i < j o i < k
5. Obtener las matrices de PseudoInercias Ji para cada barra i.
? ?x
ij
2
m
j
? ?x
ij
y
ij
m
j
? ?x
ij
z
ij
m
j
? ?x
ij
m
j
? ?y
ij
x
ij
m
j
? ?y
ij
2
m
j
? ?y
ij
z
ij
m
j
? ?y
ij
m
j
J
i
= ? ?z
ij
x
ij
m
j
? ?z
ij
y
ij
m
j
? ?z
ij
2
m
j
? ?z
ij
m
j
? ?x
ij
m
j
? ?y
ij
m
j
? ?z
ij
m
j
? ?m
j
Donde los ? suponen los productos de cada masa mj de la barra por su
distancia a los eje 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
d
ij
=? Tr (U
kj
J
k
U
T
ki
) i,j=1,2...n
k=max (i,j)
7. Obtener los trmino hikm definidos por.
h
ikm
=? Tr (U
jkm
J
j
U
T
j i
) i,k,m=1,2...n
k=max (i,k,m)
8. Obtener el vector columna H de fuerzas de Coriolisy
Centrifugas, cuyos elementos son:
n n
h
i
=??? h
ikm
q
k
q
m
i=1,2,...n
k=1 m=1
9. Obtener el vector columna C de Fuerzas de Gravedad, cuyos
elementos son:
n
c
i
=? (-mj g Uji
i
r
j
) i=1,2,...n
j=1
10. La ecuacin del modelo Dinmico es:
?=Dq+H+C
Notas:
i
r
j
es el vector del centro de masas de la barra j expresado en
el sistema {i}
g es el vector gravedad expresado en el sistema {0}
??es el vector de fuerzas y pares motores efectivos aplicados
sobre cada coordenada generalizada
las matrices Ji y D son simtricas y semidefinidas positivas
Utiliza el lgebra matricial, implicando un elevado
nmero de operaciones O(n
4
)
En la solucin parecen explcitamente los trminos de
Inercia, Coriolis y Gravedad
En ocasiones (velocidades lentas) se puede
prescindir del trmino H (fuerzas de coriolis y
centrifugas)
Se ha desarrollado un mtodo computacional ms
eficiente a partir del planteamiento L-E
Caractersticas del mtodo
computacional L-E
Ejemplo programacin del
mtodo L-E para un robot 2 gdl
% Algoritmo de Lagrange Euler
% 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');
% 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;
% 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);
% PASO 1-2 Obtenci n de las matrices de transformacin Aij
%---------------------------------------------------------
% Matrices A00 y A11 son la identidad
A00=eye(4);
A11=eye(4);
%Evalualas 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;0 1 0 0;0 0 1 d2;0 0 0 1];
%Evaluala matriz A02=A011*A2
A02=A01*A12;
% PASO 3 Evaluacionmatrices 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;
% 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');
% 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;
% 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 Evaluaci n de los pares
%----------------------------------
PARES(:,t k)=D*[at1 ad2]'+H+C;
end % fin del bucle de paso de tiempo
% 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')
text(70,2750,'LE')
0 20 40 60
-4
-2
0
2
4
Q1
0 20 40 60
-1
0
1
2
x 10
4
PAR Q1
0 20 40 60
0
2
4
6
8
Q2
0 20 40 60
-1000
-500
0
500
PAR Q2
LE
Modelado dinmico por el
mtodo 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 articulares: qi)
wi (Aceleraciones articulares qi)
vi,ai (Aceleraciones 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 ejecido 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})
C
in
e
m
tic
a
D
in
m
ic
a
1. Asignar a cada barra un sistema de referencia segn las normas de D-H
2. Obtener las submatrices de rotacin
i-1
R
i
y sus inversas(
i-1
R
i
)
T
3. Establecer las condiciones iniciales:
-Sistema de la base S0
w
0
: Vel.ang.: (0,0,0)
T
w
0
: Ac.ang: (0,0,0)
T
v
0
: Vel. lineal: (0,0,0)
T
v
0
: Ac. lineal:(gx,gy,gz)
T
-Extremo del robot: F
n+1
, N
n+1
-Otros:
z
0
:(0,0,1)
T i
R
0
.p
i
*=(a
i
,d
i
S
i
, d
i
C
i
)
T
i
R
0
.si: Vector(O
i
,CDG
i
) expresado en S
i
i
R
0
.Ii.
0
R
i
.: Matriz de inercia del elemento i respecto a su CDG expresada
en Si
Algoritmo computacional N-E (1)
Para i=1 hasta n hacer:
4. Obtener la velocidad angular del sistema Si
i
R
i-1
(
i-1
R
0
w
i-1
+z
0
q
i
) Si i es de Rotacin
i
R
0
w
i
=
i
R
i-1
i-1
R
0
w
i-1
Si i es de Traslacin
5. Obtener la aceleracin angular del sistema Si
i
R
i-1
(
i-1
R
0
w
i-1
+z
0
q
i
) Si i es de Rotacin
i
R
0
w
i
=
i
R
i-1
i-1
R
0
w
i-1
6. Obtener la aceleracin lineal del sistema Si
i
R
0
w
i ^
i
R
0
p
i
* +
i
R
0
w
i ^
[
i
R
0
w
i ^
i
R
0
p
i
* ] +
i
R
i-1
i-1
R
0
v
i-1
i deRotacin
i
R
0
v
i
=
i
R
i-1
(z
0
q
i
+
i-1
R
0
v
i-1
) + [
i
R
0
w
i ^
i
R
0
p
i
* ] +
+ 2
i
R
0
w
i ^
i
R
i-1
z
0
q
i
+
i
R
0
w
i ^
[
i
R
0
w
i ^
i
R
0
p
i
* ]
i de Traslacin
7. Obtener la aceleracin lineal del centro de graved del sistema Si
i
R
0
a
i
=
i
R
0
w
i ^
i
R
0
s
i
+
i
R
0
w
i ^
[
i
R
0
w
i ^
i
R
0
s
i
] +
i
R
0
v
i-1
Para i=n hasta 1 hacer:
8. Obtener la fuerza ejercida sobre la barra i
i
R
0
f
i
=
i
R
i+1
i+1
R
0
f
i+1
+ m
i
i
R
0
a
i
9. Obtener el par ejercido sobre la barra i
i
R
0
n
i
=
i
R
i+1
[
i
R
0
n
i +1
+
i +1
R
0
p
i
*
^
i+1
R
0
f
i+1
] +
+ [
i
R
0
p*
i
+
i
R
0
s
i
]
^
( m
i
i
R
0
a
i
) +
+ (
i
R
0
.Ii.
0
R
i
)(
i
R
0
w
i
) + (
i
R
0
w
i
)
^
(
i
R
0
.Ii.
0
R
i
)(
i
R
0
w
i
)
10. Obtener la fuerza o el par aplicado a la articulacin i
(
i
R
0
n
i
)
T
(
i
R
i-1
z
0
) + b
i
q Si i es de Rotacin
?
i
=
(
i
R
0
f
i
)
T
(
i
R
i-1
z
0
) + b
i
q
i
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)
Caractersticas del mtodo de N-E
mtodo N-E para un robot 2 gdl
% Algoritmo de Newton Euler Para la solucin del
modelo
% dinmico inverso de un robot
%----------------------------------------------------------
% Robot cil ndrico 2 GDL (RD)
%(ver ej. 5.2 libro "Fundamentos de Robtica")
% Definici n de la trayectoria y fuerzas en TCP
%-----------------------------------------------
% Definicin simblica de la posici n
st1='sin(t)/100+pi/2';
sd2='sin(5*t)+1';
% Obtencin simblica de la velocidad y aceleraci n
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]';
% 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]';
% Matriz de rotaci n entre {S2} y TCP ({S3})
%---------------------------------------------
R23=eye(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);
%Evaluacin numrica de las fuerzas y pares en el TCP
%-----------------------------------------------
n3=eval(sn3)' ;
f3=eval(sf3)' ;
% 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};
%PASO 4 Evaluacin de las velocidades angulares de {Si}
%------------------------------------------------------
w1=R01'*(w0+z0*vt1);
w2=R12'*w1;
% PASO 5 Evaluaci n de las aceleraciones angulares de {Si}
%-----------------------------------------------------------
dw1=R01'*(dw0+z0*at1)+cross(w0,z0*vt1);
dw2=R12'*dw1;
% PASO 6 Evaluaci n 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 Evaluaci n de la aceleraci n angular de los cdg
%-------------------------------------------------------
a1=cross(dw1,s1)+cross(w1,cross(w1,s1))+dv1;
a2=cross(dw2,s2)+cross(w2,cross(w2,s2))+dv2;
% PASO 8 Evaluaci n de las fuerzas sobre los eslabones
%-----------------------------------------------------
f2=R23*f3+m2*a2;
f1=R12*f2+m1*a1;
% PASO 9 Evaluaci n 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));
% PASO 10 Evaluaci n 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
% Presentacin Grfica de los resultados
figure(1);
clf
text(70,2750,'NE')
Comparacin de mtodos
Modelo dinmico en el espacio
de la tarea
C J C
q J DJ H J H
DJ J D
C H j D T
T J J T q q J T
q j T velocidad Par Potencia
q J J j J q q J q J j q J j
T
j
T
j
T
j
j j j
T T T T T
T T
1
1 1
1 1
1 1
) (
) ( ) (
) (
dinmico modelo del ecuacin la en y q do Sustituyen
.
?
? ?
? ?
? ?
?
? ?
?
? ? ?
? ? ? ? ?
? ? ?
? ? ? ? ? ? ?
?
?
? ?
? ?
? ?
?
?
?
? ? ?
? ? ? ? ?
? ? ?
?
?
?
? ? ?
?
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 pueden ser considerables
las no linealidades tipo saturacin, zona
muerta, etc.
Modelado de los actuadores y
sistemas de trasmisin

R 23432

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

R 23432

Uploaded by

Copyright:

Available Formats

Dinmica del robot

Modelo dinmico de un eslabn rgido

You might also like