. ‘slab Corey fe uyDESCRIPCION DEL ROBOT Se describe al robot con el nivel de detalle suficiente para poder crear posteriormente el modelo para la simulacion. Se presta especial atencion a la Colinas use Mateo [a aL Xes-) modos de movimiento, para lel Taco Me lol ane Cate ruedas en funcion de la velocidad requerida al robot.El Robucar oo con aM Seen tect ee eee entrar Pence El robot sobre el que trata este proyecto es un oem eae CCR CRC is aot RY ieee aa eau eciea) Cee OR An Terme ier SRC Piet NC et Caan eur eee ue ce is Tere ate Sau rien ae eR ete on Mn ta ena I Cie cies cate Pee Cee eR eR CUE uci Pree es ura ais ae cia et teas) erie tra toa + Geometria: dimensiones del robot y de las Ce Seer vila SP Tnr wits foLeee + Limites de velocidades y aceleraciones. : modos de traccién o movimiento. friccién, masa total, distribucién deReem eM MG) lr versién FAST mientras que la (b) muestra la configuracién modificada del robot sobre el que trata el presente proyecto. ‘slab Corey fe uyGeometria ea eeCunC MSc ari esquema de una aproximacion para el disefio de una geometria de Celie Terel lola ewe Uae imagen (b) muestra el giro de orwell anita (@) GeometriaGeometria Medida Resultado (m) Longitud del robot 2,90 Anchura del robot 1,30 Altura hasta la tabla oR} Altura hasta el punto mas alto Vad Separacion entre ejes 1,20 Longitud de los ejes sive) Anchura de las ruedas 0,10 Radio tedrico de las ruedas leRcie) Es utilizado por el simulador para los calculos de velocidades necesarias para las ruedas, imitando el comportamiento del robot real, aunque en realidad esos radios varien segun la rueda.. ‘slab Corey fe uyCinematica rr ee Ne ee CR Geter ue RU a ee Cee ee ac ay Pree CCR Me cea Cole en eRe eee POC COM Ue tC eee Cony rece ur eee ce ee Dae eel ica eee CaN Neto Muec ees ace tee ene eR Mn ecu tee ee ue RRs) Peter aie wie ecu Re ae Re LN ie era a aca ee Ren Cee eee ie Ret ce eC Para realizar los clculos de cinematica tanto el robot como Prete eu ege eee eke ec ee eae) Pea Cees Deg eer tar sicaieeu Masami cure ria deformable. Sin embargo, el objetivo es que el simulador imite al robot real, y éste las trata como sélidos rigidos Cuando las ruedas estan giradas a la izquierda se consideraran los angulos como positivos, y viceversa.. ‘slab Corey fe uyModo coche tan(@ Pree ec eae eer ete UU ee ie one e ene weeeeiemecu ie Pe Nea ne tiee oa Ger nom Cnt ne ieee Cn See ee eT Pence ura cuuC as CROMER Cie CR Ret ate 1a Renan eae etude ee ey Dee mat Rtn a Caer Bir ue C Race Ecce estos dos datos y de la geometria del robot es Detect teker onl ia een Crete CER CU Peruse Pa ceRet rane En primer lugar se calcula el radio de giro del robot, ®, para el punto central del eje trasero. oem Raum ric) CU eae eee ears eerer Ne eset Rut kecie i Cicerone CieloModo coche Hay que tener en cuenta el caso particular de que el radio de giro sea infinito, es decir, que el robot no gire. Es el caso mas sencillo, puesto que implica que el angulo de todas las ruedas es cero y que la velocidad lineal CPmcets ee eee Vinicoee eae Meni atorai el que se encuentra la solucién al problema. La velocidad angular seria rirenModo coche Esquema del robot en modo coche, mostrando los angulos de las ruedas frontales i yld, el de la rueda virtual central y el centro de rotacién CR. Para el resto de los casos, se puede calcular la velocidad angular del robot, w = @, a partir de la velocidad lineal del punto central trasero y del radio de giro en ese mismo punto, utilizando:Modo coche Ahora se puede pasar a calcular los angulos de las ruedas. El de las traseras es cero, por estar en modo coche. Para las ruedas frontales se utilizan los triangulos rectangulos formados por el centro de la rueda, el centro de la rueda trasera del mismo lado y el centro de rotacién. Se conocen las longitudes de ambos catetos, siendo uno de ellos la separacién entre ejes S y el otro es el radio de giro del punto central del eje trasero, sumando o restando la mitad de la longitud del eje L. Las siguientes ecuaciones indican el angulo de giro de la rueda delantera izquierda, Wi, y Col petear- 0.0Modo coche Poe ee Eon ee ee Ce eed Seana velocidad angular del sélido. En este contexto un radio de giro negativo indica un giro a la derecha. Hay que recordar que el radio de giro R sigue siendo el del centro del eje trasero del robot. Conocidos los angulos de las ruedas, Pee ie tens tele etc sem ore onel tC sabemos la velocidad lineal de un punto y la velocidad angular del robot, se puede calcular la velocidad de cualquier punto, al ser un sdlido rigido. MSI Ta lem LeU Velo 1k relacion:donde v es la velocidad de un punto, © es la velocidad de otro punto, « es el vector que une ambos puntos y [es la velocidad angular del solido. tras [Oxe}a geysut sab eleia se ft (10-1Modo coche Para saber la velocidad lineal a la que debe avanzar cada rueda se calculara el mddulo de los vectores velocidad obtenidos, puesto que ya estan orientadas en la direccion del movimiento: donde wf .i es la velocidad lineal de la rueda frontal izquierda, wf ,d la de la frontal derecha, vt, la de la trasera izquierda y vt.d la de la trasera derecha. Hay que destacar que estas ecuaciones dan velocidades positivas, el signo debe ser invertido para movimientos hacia atras.Modo coche Por ultimo, es necesario saber a qué velocidad angular deben girar cada rueda, conocido su radio r. La siguiente ecuacion muestra esa velocidad para una rueda cualquiera, wr. La velocidad lineal requerida a esa rueda es wr:Particularizando para cada rueda se Modo coche obtiene:. ‘slab Corey fe uyfunction (qipp, 2ppl=modelo2gai(t201, 202,91, q1P, 92,929) {iParanatres de) robot hise.as; Ieleo.091; 12+0.45; 1¢2-0,048; fmi-23,902; 23,88; T1-1.266; 12-0,0535, (te2, 2885 b2=0.175} fFete7.17; Fe2e1.7345 g=9.81 Zimatri2 de inencta hide mivded*Led + m2e12*L2402"1e2*1e2 +2%n2e12*1C2*c05(@2)$126125 izona"]e2*le24aa"11"1e2*cos(q2) 4125, atonalea*lezena"11"1e2"cos(q2)4125 Na2en2*1e2"1e2012; Jimatriz de Coriolis Caa-sme"ii"iea"sin(a2)*a2p) caze-me*1i“le2*sin(a2)*(atpra2p) Catena*iite2*sin(a2) ain: 2005 71ar.gravitactonal fle(mrletenae1i)-g*sin(qd) © m2tgtie2*sin(qtea2)s fg2en2tgric2"sin(qata2); Wear de Friecten Upfeicd-Fer*sign(atp) + bt*atps Upéricanfearsign(aap) + 62°¢2p5 ‘grieasbitalp; Frbca=ba"ae {1kcuaciones diferenciales laz0 cerrado etnsnities vara; vat-teol-cliqip-cl2éqzp-g1-Frict; Votetae2-C2irgip-C22"q2p-g2-Frie2} Vets(Waa-val - mid*vba)/deth, vate(om2ivan + MA1"Yb1)/4e0hs ‘atppever; ‘pp=ve; Soafunction. ‘slab Corey fe uyMAURO GILBERTO LOPEZ RODRIGUEZ SUNIVERSIDAD NACIONAL AUTONGMA DE MEXICO SDINAMICA ROBOT GDL Function d2q = DINAMIC_OMNI_PHANTOM_SIMPLIFICADO(g,dq,tau) XPARAMETROS DINAMICOS DEL ROBOT OMNI PHANTOM XSIMPLIFICACION DE FUNCIONES TRIGONOMETRICAS % ctecos(a(1)); % stesin(q(1)); % c2ee0s(q(2)); % s2esin(a(2)); x x % cecos(q(3)) 5 s3esin(q(3)) 5 €23=cos(q(2)+q(3))5.eaecosta(a))s stesin(q(t))5 e2ecos(a(2) i aa(3)5 dateda(): siqa-dg(2)5 4g3-dq¢3); EALCULO DE LOS MOMENTOS DE INERCTA ESLAGON 2 pa=(a/32y"m*d1"23; raae(4/12)m*61"2 ESLABON 2 Ty2=(4/32) 2°22 22-(4/12)"m2*92"2 ESLABON 2 Ty30(1/12)"3*23°25 e3e(2/12)m3"23°2} % LA MATRIZ DE INERCIA # ES: Hel (n3*0c3"2+Iy3)*c0s(q3)"2+2*a2"nB*Oc3*cos(Q2)*cos(q3)+(m2¥OC2*2+22°2"HB+Ty2)*C05(42)"2 oe °2¢a2*m3*0c3* sin (q2)*sin(q3)+2*a2*m3*0c3* cos (q2)*cos(q3)+m3*OC3*2+m2*0C2*2+a2%2*NI+TZ3+12 2 a2*n3*0c3*5in(q2)*sin(q3)+a2*m3*0c3*cos(q2)*cos (q3)#m3°0C324123; @ a2*n3*0c3*sin(q2)*sin(q3)+a2"m3*0c3*cos (q2)*cos(q3)+m3*OC3*24123 m3*0C3*24123]; % LA MATRIZ CON LOS SINBOLOS DE CRISTOFFEL C ES: C11=dg3* ( -2* (m3*0c3*2+Iy3) *cos(q3)*sin(q3)-2*%a2*m3*0c3*cos(q2)*sin(q3) )+dq2*(-2*a2*m3*0c 3*sim(q2)*cos(q3)-2*(m2*0c2*2+a2"2*m3+Iy2)*cos(q2)*sin(q2)); Cadedqi*(-2*a2"m3+0c3*sin(q2)*cos (q3)-2*(n2*0c2"24a2*2*m3+T)2)*c05(q2)*84n(42) 5 (ca3edqa*(-2*(n3*0c3*24Iy3)"cos (q3)"sin(q3)-2*2*m3#0c3*cos(q2)*sin(q3)); igi* (2*a2*m3*Oc3*sin(q2)*cos(q3)+2*(m2"0c2*2+a2*2*m3+1y2)*cos(q2)*sin(q2)); *dq2* (2*a2*m3*0c3*cos(q2)*sin(q3)-2*a2*m3*0c3*sin(q2)*cos(q3))+dq3*(a2*m3*0c3*cos(q 2)*sin(q3)-a2*m3*0c3*sin(q2)*cos(q3) )+dq3*(a2*m3*0c3*sin(q2)*cos(q3)-a2*m3*0c3*cos(q2)*s_ in(q3))+dq3*(2*a2*m3*0c3*sin(q2) *cos(q3)-2*a2*m3*0c3*cos(q2)*sin(q3) )+dq2*(2*a2*m3*0c3*s: in(q2)*cos(q3)-2*a2*m3*0c3*cos(q2)*sin(q3)); iq2* (a2*m3*0c3*cos(q2)*sin(q3) -a2*m3*0c3*sin(q2)*cos(q3))+2*dq3*(a2*m3*Oc3*sin(q2)*c 105(q3) -22*n3“0c3*cos (q2)*sin(q3))+dq2* (a2*n3*0c3*sin(q2)*cos(q3)-a2*n3*0c3*cos (@2)*sin(q 3))#dqa* (2*a2"m3*0c3*sin(q2)*cos(Q3) -2*a2*n3*0c3*cos(q2)*sin(Q3)); C31=dq1* (2*(m3*0c3*2+Iy3) *cos(q3)*sin(q3)+2*a2*m3*0c3*cos(q2)*sin(q3)); C32=dq2*(2*a2*m3*0c3*cos(q2)*sin(q3)-2*a2*m3*0c3*sin(q2)*cos(q3))+dq3*(a2*m3*0c3*cos(q2) *s4n(q3)-a2*m3*0c3*sin(q2) *cos (q3) )+2*dq2* (a2*m3*0c3*cos q2)*sin(q3) -a2*n3*0c3*sin(q2)*¢ (05(q3)) #dq3*(a2*n3*0c3"sin(q2)*cos(q3)-a2*"3*0c3*cos(q2)*sin(43))3 C33edq2* (a2*m3*0c3*cos(q2)*sin(q3)-a2*m3*0c3*sin(q2)*cos(q3))+dq2*(a2*m3*0c3*sin(q2)*cos (@3)-22¢n3*0c3*cos(q2)*Sin(q3)) 5 c [cra c12 13; C21 C22 C23;c=.S*[c12 42 C13; 21 (22 C23; 31 C32 C33); EL VECTOR DE GRAVEDAD G E: G=[05g¥m2*0c2*cos (q2)+g*m3* (Oc3*cos (q2+q3)+a2*cos(q2)) ;g*M3¥0c3*cos(q3+q2) 15 %LA MATRIZ CON LOS COEFICIENTES DE FRICCION ES: Ds[cfl 8 0:0 cf2 0; @ 8 cF3]; d2q=inv(H)*(tau-(C*dq)-(D*€q)-6) ; end. ‘slab Corey fe uy