Asignacin 3: Algoritmos de dinmica directa e inversa, seleccin de motores y control desacoplado

Escuela Tcnica Superior de Ingeniera Industrial Universidad Politcnica de Madrid 2010-2011

Juan Carlos Cambera Juan David Hernndez

El siguiente informe resume los pasos seguidos para la obtencin de la dinmica directa, inversa y el control del robot manipulador PELICAN. El orden en que se presentan los puntos son los siguientes. Primero se comenzar con una explicacin breve de los algoritmos de dinmica directa e inversa, Newton Euler y Walker Orin, y los implementados mediante el toolbox de Robtica de Peter Corke. A continuacin, se realizar la seleccin de dos motores elctricos DC sin escobillas que sean capaces de aportar el torque y la velocidad necesaria para dos perfiles de velocidad propuestos. Por ltimo se pondrn a prueba las metodologas de control desacoplado para este robot manipulador.

1. Algoritmos de dinmica directa e inversa

En este apartado se muestran algunas pruebas realizadas para la solucin del problema dinmico inverso y directo para el caso particular del robot Pelican de 2 GDL (Kelly, Santibez Davila, & Lora, 2005). Para las respectivas pruebas se utilizaron diferentes implementaciones como las expuestas en la clase de Dinmica y Control de Robots (Saltarn, Almonacid, & Sabater), las presentadas en el toolbox de robtica para Matlab (Corke, 1996), y otras que son resultado del trabajo de grado de unos de los autores (Hernndez Vega, Magdalena, & Jaramillo-Botero, 2009). El sistema de prueba, como se mencion previamente, es el brazo robtico Pelican. En la Figura 1 se presenta una representacin del mismo. Adicionalmente en la Tabla 1 renen los parmetros fsicos del robot necesarios para resolver su dinmica.

L1

L2

Figura 1: Brazo Robtico

Descripcin

Pelican

Notaci

Valor

Unidad

n Longitud del link 1 L1 Longitud del link 2 L2 Distancia al Centro de Masa Lc1 link 1 Distancia al Centro de Masa Lc2 link 2 Masa del link 1 Masa del link 2 m1 m2

es 0.26 m 0.26 m 0.098 m 3 0.022 m 9 6.522 Kg 5 2.045 Kg 8 0.121 Kg 3 m2 0.011 Kg 6 9.81 m2 m/s2 * *

Inercia Relativa al Centro de I1 Masa Link 1 Inercia Relativa al Centro de I2 Masa Link 2 Aceleracin de la gravedad g

Tabla 1: Parmetros fsicos del brazo robtico Pelican (Kelly, Santibez Davila, & Lora, 2005)

Dado que la solucin de la dinmica del sistema se ha realizado mediante los algoritmos recursivos en la formulacin Newton-Euler expresada en coordenadas generalizadas, es necesario describir espacialmente cada uno de los cuerpos del sistema mediante marcos de referencia; esta caracterizacin se puede obtener de manera sistemtica por la denominada convencin DH (Denavit & Hartenberg, 1955), o la modificacin de la misma conocida como MDH -Modified Denavit Hartenberg- (Craig, 1986).

Y0 X0 Z0 Z1 L1

Y1 X1 Z2 L2

Y2 X2

Figura 2: Sistemas coordenados para el brazo robtico Pelican segn la convencin DH

i Link 1 2 0 0

ai L1 L2

i 0 0

Di 0 0

Tabla 2: Parmetros DH del brazo robtico Pelican

Y0

Y1 X0 X1

Y2 X2 Z2 L1

Z0 Z1

Figura 3: Sistemas coordenados para el brazo robtico Pelican segn la convencin MDH

Lin k 1

i1 0

ai1 0

i i 0 0

L1

0 0

Tabla 3: Parmetros MDH del brazo robtico Pelican

La Figura 2 presenta la distribucin de los sistemas de referencia segn la convencin DH, mientras que la Figura 3 muestra su equivalente en la convencin MDH. La diferencia entre stas se da en la ubicacin del sistema cartesiano que caracteriza a cada uno de los cuerpos. Para el primer caso, DH, el sistema cartesiano Oi se ubica en la articulacin
i+1

, y en el caso del

ltimo cuerpo de la cadena del sistema se ubica en la parte posterior del mismo. Por otra parte, en el caso de MDH, el marco de referencia Oi se ubica coincidente con la articulacin i. Esta coincidencia numrica de la ltima convencin mencionada, trae diferentes implicaciones a nivel de descripcin geomtrica (incluyendo los operadores espaciales que se usan), as como computacionales cuando se realizan las propagaciones cinemticas y dinmicas al momento de resolver las ecuaciones de movimiento de manera recursiva. En primer lugar, comparando la Tabla 2, donde los parmetros DH incluyen las longitudes L1 y L2, y la Tabla 3, donde solo dependen de L1, se puede observar como la descripcin espacial del sistema no depende de la geometra del ltimo cuerpo; lo anterior ocurre como consecuencia que los sistemas de referencia de cada cuerpo i se ubican en la parte anterior de los cuerpos y no posterior como en el caso de la DH estndar. Esto a su vez implica menor cantidad de operaciones de transformacin de las cantidades involucradas en los diferentes algoritmos (e.g. las cantidades de velocidad en las articulaciones se encuentran expresadas en un sistema de referencia local). Finalmente la utilizacin de la convencin MDH facilita la utilizacin de la teora de tornillos (screws) como operadores espaciales, ya que la transformacin espacial de cuerpo a cuerpo puede ser expresada como (Craig, 1986).

Para realizar las pruebas de dinmica y dinmica inversa primeramente se parti de los algoritmos de de Newton Euler y Walker-Orin presentados en el libro de Robotica: Modelado, Simulacin y Diseo con Matlab (Saltaren,2000) los cuales fueron modificados y adaptados al robot en cuestin.Como parte de las pruebas y comparaciones que se buscan realizar en este ejercicio se adjuntan implementaciones para solucionar los problemas de dinmica inversa y directa utilizando operadores espaciales, as como la solucin dinmica directa de complejidad lineal (Featherstone, The calculation of robot dynamics using articulated body inertias, 1983). Todas las pruebas de dinmica fueron finalmente comparadas con las expresiones tericas presentadas en el libro Control of Robot Manipulators in Joint Space (Santibez, 2005). Las configuraciones seleccionadas para para realizar las pruebas de la dinmica del sistema se muestran en la Figura 4.

a)

b)

c) Figura 4: Configuraciones de prueba del robot Pelican. Enlaces en direccin ortogonal a la gravedad (caso a). Enlaces en direccin paralela a la gravedad (caso b). Enlaces en posicin arbitraria (caso c)

Al momento de ejecutar las pruebas se definen inicialmente unas condiciones en posicin, velocidad y aceleracin. De estas configuraciones y luego de describir el sistemas robtico, se procede a calcular los pares en los actuadores (dinmica inversa) y luego, mediante el proceso inverso, teniendo los pares se puede evaluar la respuesta del sistema bajo stos (dinmica directa). A continuacin se presentan los resultados de las pruebas para la configuracin c).
Posiciones articulares -1.0472 0.6283

Velocidades articulares 0.1000 0.2000

Aceleraciones articulares 0.3000 0.4000

Dinamica inversa implementacion Roque et al. modificada para el robot Pelican 6.2887 0.4318

Dinamica inversa para convencion DH implementacion Robotics Toolbox, Peter Corke 6.2887 Corke 6.2887 0.4318 0.4318 Dinamica inversa para convencion MDH implementacion Robotics Toolbox, Peter

Dinamica inversa para convencion MDH implementacion Juan D. Hernandez-Vega, et al. (uso de algebra espacial) 6.2887 0.4318

Dinamica inversa por medio de las ecuaciones de movimiento de Lagrange 6.2887 0.4318

Dinamica directa implementacion Roque et al. modificada para el robot Pelican 0.3000 0.4000

10

Dinamica directa para convencion MDH implementacion Robotics Toolbox, Peter Corke 0.3000 0.4000

Dinamica directa implementacion Juan D. Hernandez-Vega et al. (uso de algebra espacial complejidad O(n) ) 0.3000 0.4000

Se

puede

apreciar

entonces

la

equivalencia

entre

diferentes

implementaciones, as como la posibilidad de obtener mediante un proceso inverso (dinmica directa) los movimientos inducidos en un sistema dado un conjunto de pares. En importante considerar la dinmica directa del sistema como una herramienta idnea para la simulacin de la respuesta del sistema, es decir, esta puede ser utilizada para predecir el comportamiento conociendo las fuerzas tanto internas como externas que afectan el sistema. La importancia de la complejidad temporal radica en las restricciones de tiempo real que se puedan tener.

2. Seleccin de motores
Para la seleccin de motores es preciso calcular los pares mximo pico y nominales a los cuales puede estar sometido cada motor del robot ante las condiciones de posicin, velocidad y aceleracin de la tarea ms exigente para la que se le disea. Para realizar esta estimacin, se parti del archivo selmor4.mdl , y se hicieron las modificaciones convenientes para adaptarle al robot Pelican. El esquema del sistema se presenta a continuacin:

11

Figura 5: Esquema para el clculo de pares para un robot de 2GDL con reductores.

El robot se diseo para que ambos motores cumplieran un perfil LPBS de velocidad mxima pi/3 rad/seg y aceleracin 0.1 rad/seg^2. Los perfiles de velocidades y aceleracin se muestran a continuacin:

a)

b)

Figura 6 : Perfiles de velocidad y aceleracin de diseo. En rojo se seala el punto de mayor demanda de torque

12

Las condiciones de mayor demanda de torque son aquella para las cuales las velocidades y aceleraciones de las articulaciones son mximas, y el robot se encuentra pasando justo por la posicin donde la fuerza de gravedad produce su torque ms importante. Para nuestro caso, esta ltima condicin ocurre cuando el robot de encuentra en la posicin cero, es decir, cuando se encuentra completamente estirado y paralelo con el eje X. Las condiciones de velocidad mxima y aceleracin mxima se encuentran debidamente sealadas en la Figura XX. Realizando la simulacin para la aplicacin del perfil simultneamente a las dos articulaciones se obtienen los siguientes pares mximos y nominales: Articulacin Tpico (N.m) 1 0.412 2 0.18 0 Tnominal (N.m) 0.251 0.10 51 Tpico (onz in) 58.3 25.4 61 Tnominal in) (onz 35.5 14.8 88

Tabla 4. Torques requeridos para el perfil LPBS

Dada las condiciones anteriores se seleccionaron los siguientes motores para la articulacin 1 y 2. Articulaci n 1 Nombre Motor DA23DBBM400 2 DA23GBB1.5 2.35 0.095 0.095 11.1 R( ) 21 L(m H) 25 Kt (Nm/A) 0.21 Kv(V/rad/s eg) 0.218 Imx(A) 2.6

13

M300

Tabla 5: Tabla de caracterstica de los motores seleccionados

Los puntos de operacin nominales y picos del motor 1 y 2 se indican, respectivamente, en las curvas de torques del motor se presentan a continuacin:

Cond.Nomina les Cond.Pico

Figura 7: Curva caracterstica de los motores de la serie DA23DBB

Cond. Nominales Cond.Pico

Figura 8: Curva caracterstica de los motores de la serie DA23GBB

3. Control desacoplado
Se puede demostrar matemticamente que en presencia de reductoras con altas relaciones, el control de cada uno de los motores que realizan el movimiento del manipulador se puede desacoplar. La ventaja de este

14

mtodo es que el diseo del regulador ms adecuado para cada articulacin puede realizarse con las tcnicas de control clsico. Tomando lo anterior en cuenta, el problema de control se plantea mediante el siguiente diagrama de Simulink.

Figura 9: Diagrama en Simulink para la sintonizacin de motores por control desacoplado.

Una vez configurado todos parmetros del motor segn el apartado anterior, se ajustaron los PID manualmente partiendo del conocimiento de los efectos de la ganancia proporcional, derivativa e integral en la respuesta transitoria y de estado estacionario del sistema. Esta sintonizacin se realiz considerando que los pares de los otros motores diferentes al sintonizado eran iguales a cero. Segn los experimentos no hizo falta la incorporacin de ninguna ganancia derivativa ni integral para obtener una respuesta rpida, sin sobrepicos, y con error de estado estacionario despreciable esto se poda intuir porque la funcin de transferencia era tipo 1. La tabla de ganancias se muestra a continuacin:

15

Ganancia Proporcional Derivativa Integral

Valor 400 0 0

Ganancia Proporcional Derivativa Integral

Valor 300 0 0

Tabla 06: Parmetros de regulador PID para el motor 1

Tabla 07: Parmetros de regulador PID para el motor 2

A continuacin se presentan las grficas de la respuesta a lazo cerrado ante un escaln de amplitud pi/16.

Figura 10: Respuesta a lazo cerrado del sistema de posicin de la 1era articulacin

16

Figura 11: Respuesta a lazo cerrado del sistema de posicin de la 2da articulacin

Para comprobar el funcionamiento del diseo se realizaron pruebas del sistema controlado cuando recibe una referencia del perfil para el cual fue diseado. A continuacin se muestran las salidas de los sistemas ante la aplicacin consecutiva de dos veces cada perfil.

17

Figura 12: Seguimiento del perfil de posicin por la articulacin 1. Perfil de posicin de referencia (rojo) , salida del sistema controlado (verde)

Figura 13: Seguimiento del perfil de posicin por la articulacin 2. Perfil de posicin de referencia (rojo), salida del sistema controlado (verde)

Como se puede ver en las Figuras 12 y Figura 13, el sistema controlado es capaz de seguir el perfil de posicin de velocidad de referencia sin ningn sobrepico ni retardo importante.

4. Bibliografa
Corke, P. (1996). A Robotics Toolbox for Matlab. IEEE Robotics and Automation Magazine , 3 (1), 24-32. Craig, J. (1986). Introduction to Robotics. Addison Wesley. Denavit, J., & Hartenberg, R. (1955). A kinematic notation for lower-pair mechanisms based on matrices. Trans ASME J. Appl. Mech, (pgs. 215-221). Featherstone, R. (2007). Rigid Body Dynamics Algorithms. New York: Springer.

18

Featherstone, R. (1983). The calculation of robot dynamics using articulated body inertias. Proc. of The Int. Journal of Robotics Research, 2, pgs. 13-30. Hernndez Vega, J. D., Magdalena, X., & Jaramillo-Botero, A. (2009). Modelo Cinemtico y Dinmico de un Caminante Robtico. Cali, Colombia: Javeriana Cali. Kelly, R., Santibez Davila, V., & Lora, A. (2005). Control of Robot Manipulators in Joint Space (Advanced Textbooks in Control and Signal Processing). London: Springer. Saltarn, R., Almonacid, M., & Sabater(2000), J. Robotica: Modelado, Simulacin y Diseo con Matlab. Madrid.

19

5. Apndices A. Hoja de caractersticas de los motores

20

21

B.

Cdigo de Cinemtica Inversa

Este cdigo est basado en una modificacin del algoritmo de Newton-Euler implementado en Robotica: Modelado, Simulacin y Diseo con Matlab (Saltarn,2000).
% % % % % % % % % % % NEWTONEULER2 PELICAN Din?mica inversa utilizando el m?todo de Newton-Euler. TAU = NEWTONEULER4(Q, QP, QPP, G, M5, IEXTER) calcula el vector 4x1 de pares/fuerzas de entrada a las articulaciones. Q el vector 4x1 de coordenadas articulares. QP es el vector 4x1 que representa la velocidad de cada articulaci?n. QPP es el vector 4x1 que indica la aceleraci?n de cada articulaci?n. G es el valor de la gravedad (m/s^2). M5 es la masa de la carga externa(Kg) que transporta el brazo robot. IEXTER es la matriz 3x3 de inercia de la carga exterior(Kg-m^2). See also DH, RI0PI, RI0SI, RI0WI, RI0WIP, RI0VPI_R, RI0AI, RI0FI, RI0NI, RI0FIA, RI0NIA, T_R, F_P.

function tau = newtoneulerPELICAN(q,qp,qpp,m3,Iexter) L1=0.26; L2=0.26; Lc1=0.0983; Lc2=0.0229; Ic1=0.1213; Ic2=0.0116; g=9.81; % -----------------------------------------------------------% Par?metros Denavit-Hartenberg del robot % -----------------------------------------------------------teta = [q(1),q(2)]; d = [0 ,0 ]; a = [L1,L2]; alfa = [0,0 ]; % -----------------------------------------------------------% Factores de posicionamiento de los centros de gravedad % -----------------------------------------------------------factor1 = -(1-Lc1/L1); factor2 = -(1-Lc2/L2); % -----------------------------------------------------------% Masa de cada elemento (Kg) % -----------------------------------------------------------m1=6.5225; m2=2.0458; % -----------------------------------------------------------% Coeficiente de rozamiento viscoso de cada articulacion % -----------------------------------------------------------b1 = 0; b2 = 0; % -----------------------------------------------------------% Matrices de Inercia (Kg-m^2) % -----------------------------------------------------------r10I_r01=Ic1*eye(3); r20I_r02=Ic2*eye(3); %-------------------------------------------------------------%------------ Cinem?tica Directa------------------------------%-------------------------------------------------------------% -----------------------------------------------------------% Vectores ri0pi, ri0si. % -----------------------------------------------------------%Componentes de posici?n de la matrices homogeneas para cada caso r10p1 = ri0pi(a(1), d(1), alfa(1)); r20p2 = ri0pi(a(2), d(2), alfa(2));

22

r30p3 = zeros(3,1); r10s1 = ri0si(a(1), d(1), alfa(1), factor1); r20s2 = ri0si(a(2), d(2), alfa(2), factor2); r30s3 = zeros(3,1); % -----------------------------------------------------------% Matrices de transformacion % -----------------------------------------------------------r01 = dh(teta(1), alfa(1));r10 = r01'; r12 = dh(teta(2), alfa(2));r21 = r12'; r23 = eye(3); r32 = r23'; %-------------------------------------------------------------%------------ Din?mica Inversa ------------------------------%-------------------------------------------------------------%-------------------------------------------------------------%------------------------------------------------------------%Iteraciones hacia afuera (velocidades y aceleraciones) %------------------------------------------------------------%------------------------------------------------------------% -----------------------------------------------------------% Velocidad angular de las articulaciones % -----------------------------------------------------------r00w0 = zeros(3,1); r10w1 = ri0wi(r10, r00w0, qp(1)); r20w2 = ri0wi(r21, r10w1, qp(2)); r30w3 = ri0wi(r32, r20w2, 0); %%Link para agregar la fuerza externa al robot % -----------------------------------------------------------% Aceleracion angular de las articulaciones % -----------------------------------------------------------%r10 representa Matriz de Rotacion que tranforma vectores del sistema 0 al 1 %r00wp0 vector de aceleracion angular del frame 0 (inercial) %r00wp0 vector de velocidad angular del frame 0 (inercial) %qp(1) es la m?gnitud de la velocidad del sistema i respecto al i-1 (del 1 respecto al 0 %qpp(1) es la magnitud de la aceleraci?n angular del sistema 1 respecto al 0 %r10wp1 = aceleracion angular del link 1 expresada en 1 r00wp0 = zeros(3,1); %%Base est?tica r10wp1 = ri0wpi(r10, r00wp0, r00w0, qp(1), qpp(1)); %%Articulacion rotacional r20wp2 = ri0wpi(r21, r10wp1, r10w1, qp(2), qpp(2)); %%Articulacion rotacional r30wp3 = ri0wpi(r32, r20wp2, r20w2, 0, 0); %%Link para agregar la fuerza externa al robot % -----------------------------------------------------------% Aceleracion lineal articular % -----------------------------------------------------------%r00vp0 Aceleracion lineal del frame cero vista desde cero (en sistema inercial) %r10vp1 Aceleracion lineal del frame uno sobre el sistema inercial y vista desde el sistema 1 %ri0vpi_r Funci?n para el calculo de las aceleraciones lineales para articulaciones rotacionales %ri0vpi_p Funci?n para el caluclo de las aceleraciones rotacionales para articulaciones prism? ticas %r21 matriz de rotaci?n que transforma vectores del frame 1 al 2 %r10vp1 Aceleraci?n lineal del sistema 1 respecto al sistema universal visto desde 1 %r20wp2 Aceleraci?n angular del sistema 2 respecto al sistema universal visto desde 2 %r20w2 Velocidad angular del sistema 2 respecto al sistema universal visto desde 2 %r20p2 Distancia entre el sistema p2 y p1 visto desde 2 %qp(2) %qpp(2) r00vp0 = [0; g; 0]; %%Con esto se a?aden las condiciones de gravedad sobre todos los centros de masa r10vp1 = ri0vpi_r(r10, r00vp0, r10wp1, r10w1, r10p1); r20vp2 = ri0vpi_r(r21, r10vp1, r20wp2, r20w2, r20p2); r30vp3 = ri0vpi_r(r32, r20vp2, r30wp3, r30w3, r30p3); % -----------------------------------------------------------% Aceleracion del centro de masa de cada elemento % ------------------------------------------------------------

23

%r10vp1 %r10wp1 %r10w1 %r10s1 r10a1 = r20a2 = r30a3 =

aceleracion lineal de 1 respecto al sistema inercial vista en 1 aceleraci?n angular de 1 respecto al sistema inercial vista en 1 velocidad angular de 1 respecto al sistema de inercia visto respecto a 1 Posici?n del centro de masa del link 1 vista desde el sistema 1 ri0ai(r10vp1, r10wp1, r10w1, r10s1); ri0ai(r20vp2, r20wp2, r20w2, r20s2); ri0ai(r30vp3, r30wp3, r30w3, r30s3);

% -----------------------------------------------------------% Fuerza en el centro de masa de cada elemento % -----------------------------------------------------------% Simplemente es masa por aceleraci?n de cada link % r50a5 aceleracion del centro de masa 5 respecto al sistema inercial vista desde el sistema 5 % m5 masa del eslab?n 5 r30f3 = ri0fi(r30a3, m3); % Esta es un link extra que se a?adi? para incluir una fuerza externa r20f2 = ri0fi(r20a2, m2); r10f1 = ri0fi(r10a1, m1); % -----------------------------------------------------------% Par en el centro de masa de cada elemento % -----------------------------------------------------------% r50wp5 aceleracion angular del sistema 5 respecto al sistema inercial y visto desde el sistema 5 % r50w5 velocidad angular del sistema 5 respecto al sistema inecial y visto desde el sistema 5 % r40I_r04 Momento de inercia 4 visto desde el centro de masa r30n3 = ri0ni(r30wp3, r30w3, Iexter); r20n2 = ri0ni(r20wp2, r20w2, r20I_r02); r10n1 = ri0ni(r10wp1, r10w1, r10I_r01); %-------------------------------------------------------------%------------------------------------------------------------%Iteraciones hacia dentro (fuerzas y torques) %------------------------------------------------------------%------------------------------------------------------------% -----------------------------------------------------------% Fuerzas articulares % -----------------------------------------------------------% % % % % r50f5 fuerza en el centro de masa 5, vista desde el sistema 5 r50f5a fuerza en el sistema 5 debido al link 5, vista desde el sistema 5 r40f4 fuerza en el centro de masa 4, vista desde el sistema 4 r40f4a fuerza en la posici?n del sistema 4 vista desde 4 r45 Transformada que lleva de 5 a 4

r30f3a = r30f3; r20f2a = ri0fia(r23, r30f3a, r20f2); r10f1a = ri0fia(r12, r20f2a, r10f1); % -----------------------------------------------------------% Pares articulares % -----------------------------------------------------------% % r20p1 posici?n entre 0 y 1 vista desde 2 % r45 Matriz que lleva desde 5 a 4 % r50n5a Torque en el extremo inferior del link 5 ,sobre el sistema 5, visto desde 5 % r50f5a Fuerza en el extremo inferior del link 5, sobre el sistema 5,visto %desde 5 % r40n4a Torque en el extremo inferior del link 4, sobre el sistema 4, visto desde el sistema 4 % r40f4 Torque en el centro de masa del sistema 4, visto desde 4 % r50p4 Distancia entre el sistema % r40p4 Distancia entre sistema 4 y 3 visto desde 4 % r50p4 Distancia entre 4 y 3 vista desde el sistema 5 % r40s4 Posici?n del centro de masa del sistema 4 vista desde el sistema 4 r20p1 = r21*(r10p1); r30p2 = r32*(r20p2); %%Para ver las fuerzas externas que se le aplican

24

r30n3a = r30n3; r20n2a = ri0nia(r23, r30n3a, r30f3a, r20n2, r20f2, r30p2, r20p2, r20s2); r10n1a = ri0nia(r12, r20n2a, r20f2a, r10n1, r10f1, r20p1, r10p1, r10s1); % -----------------------------------------------------------% Fuerzas y pares de accionamientos % -----------------------------------------------------------%r10 matriz que transforma desde el sistema 0 al sistema 1 %r10n1a Torque en el extremo inferior del link 1 expresado respecto a 1 %qp velocidad de las articulaciones %b(1) articulacion de la primera articulacion t_1 = t_r(r10, r10n1a, qp(1), b1); t_2 = t_r(r21, r20n2a, qp(2), b2); tau = [t_1; t_2];

C.

Cdigo de Cinemtica Directa

Este cdigo est basado en una modificacin del algoritmo de Walker-Orin implementado en Robotica: Modelado, Simulacin y Diseo con Matlab (Saltarn,2000).
function qpp = WalkerOrinPelican(q,qp,tau,masaext,inerciaext) % Se calcula el vector b. b = newtoneulerPELICAN(q,qp,zeros(2,1),masaext,inerciaext); % Se calcula la matriz de momentos de inercia H. H = H2_Matrix(q,masaext,inerciaext); % Se calcula el vector de aceleraci?n de cada articulaci?n. qpp = inv(H)*(tau-b); end

% H2_Matrix Matriz de momentos de inercia. % H = H2_Matrix(Q, MASAEXT, INERCIAEXT) calcula la matriz de momentos de % inercia H 4x4 utilizando el tercer mtodo de Walker y Orin. Q es % el vector 4x1 de variables articulares. MASAEXT es la masa de la % carga externa. INERCIAEXT es la inercia de la carga externa. % function h = H2_Matrix(q, masaext, inerciaext)

% -----------------------------------------------------------% Par?metros Bsicos % -----------------------------------------------------------L1=0.26; L2=0.26; % Lc1=0.0983; % Lc2=0.0229; Ic1=0.1213; Ic2=0.0116; % g=9.81;

25

% -----------------------------------------------------------% Par?metros Denavit-Hartenberg del robot % -----------------------------------------------------------teta = [q(1),q(2)]; d = [0 ,0 ]; a = [L1,L2]; alfa = [0,0 ]; % -----------------------------------------------------------% Masa de cada elemento (Kg) % -----------------------------------------------------------m = [6.5225; 2.0458]; %m1=6.5225; m2=2.0458; % Matrices de Inercias Centroidales. (Kg-m^2.) % J = [ 0.0191 0 0; 0 0.0191 0; % 0.0031 0 0; 0 0.0484 0; % 0.0606 0 0; 0 0.0053 0; % 0.0022 0 0; 0 0.0022 0; J = [Ic1*eye(3);Ic2*eye(3)]; % La tercera inercia es la de la carga externa. J(7:9,1:3) = inerciaext; % Vector Z0. z0 = [0; 0; 1]; % Condiciones de Carga Externa. M(3) = masaext; cj_1j = zeros(3,1); Ej_1j = J(7:9,1:3); for j = 2:-1:1 % Constante para sacar la Inercia J. k = (j-1)*3 + 1; % Vectores p y s. p = [a(j); d(j)*sin(alfa(j)); d(j)*cos(alfa(j))]; s = -0.5*p; % Matrices de transformacion. aj_1j = dh(teta(j),alfa(j)); ajj_1 = aj_1j'; % Centroide e Inercia de los elementos anteriores. cjjM1 = cj_1j; EjjM1 = Ej_1j; % Masa de todos los elementos anteriores. M(j) = M(j+1) + m(j); % Nuevo centroide. cjj = ((s + p)*m(j) + M(j+1)*(cjjM1 + p))/M(j); cj_1j = aj_1j*cjj; % Distancia de traslado de las inercias. p1 = (cjjM1 + p - cjj); d1 = dot(p1,p1)*eye(3) - (p1*p1'); p2 = (s + p - cjj); d2 = dot(p2,p2)*eye(3) - (p2*p2'); % Nueva Inercia. Ej_1j = aj_1j*(EjjM1 + M(j+1)*d1 + J(k:k+2,1:3) + m(j)*d2)*ajj_1; % % % % % Fuerza y if (j == Fj_1j = Nj_1j = else % Fj_1j Nj_1j par de los elementos j hasta N. 1) | (j == 2) % Articulaciones rotacionales cross(z0,M(j)*cj_1j); Ej_1j*z0; Articulaciones prismticas = M(j)*z0; = zeros(3,1); 0 0 0 0 0 0 0 0 0.0068; 0.0484; 0.0606; 0.0014];

26

end % Elemento j. Componente H(j,j). fi_1i = Fj_1j; ni_1i = Nj_1j + cross(cj_1j, Fj_1j); if (j == 1) | (j == 4) % Articulaciones rotacionales h(j,j) = ni_1i(3,1); else % Articulaciones prismticas h(j,j) = fi_1i(3,1); end % Elementos j-1 hasta 1. Componentes H(1:j-1,j) i = j - 1; while i >= 1 % Vector p. p = [a(i); d(i)*sin(alfa(i)); d(i)*cos(alfa(i))]; % Matrices de rotacion. ai_1i = dh(teta(i),alfa(i)); aii_1 = ai_1i'; % Fuerza y par del anterior elemento. fiiM1 = fi_1i; niiM1 = ni_1i; % Fuerza y par de este elemento. fi_1i = ai_1i*fiiM1; ni_1i = ai_1i*(niiM1 + cross(p, fiiM1));

% % % %

% % % %

% Componente H(i,j). if (i == 1) | (i == 4) % Articulaciones rotacionales h(i,j) = ni_1i(3,1); else % Articulaciones prismticas h(i,j) = fi_1i(3,1); end % H es simetrica. h(j,i)= h(i,j); i = i - 1; end

end

27