Cinemática de Robots. Práctica Introducción

Transcripción

1 Práctca de Robótca utlzando Matlab Práctca 2 Cnemátca de Robot 2..- Introduccón Lo robot cláco preentan una arqutectura antropomórfca eral, emejante al brazo humano. Conten de una ere de barra rígda unda entre í a travé de artculacone de un grado de lbertad del tpo rotaconal o prmátca. En general cada artculacón logra u movmento a travé de un acconamento de potenca e ncluye otro dpotvo como reductore de velocdad, freno y enore de pocón o velocdad. Aunque al defnr la relacone cnemátca de un robot no e uelen conderan lo apecto dnámco, nada má alejado de la realdad cuando e quere deñar un robot ya que exte una nevtable relacón caua-efecto entre la cnemátca y la dnámca. Nada má claro reulta que al penar en la dmenone de un robot, la longtud de un brazo afecta al cuadrado la nerca de lo elabone y por lo tanto el peo del robot y la potenca requerda en lo actuadore. La arqutectura de lo robot cláco preentan una ere de propedade dnámca y etructurale caracterzada por una gran rgdez etructural, repetbldad y elevado peo propo. El elevado peo propo de lo robot cláco lmta la capacdad carga útl y la velocdade de trabajo, la cuale uualmente etán en torno a lo 6 grado/eg. para la prmera tre artculacone de lo robot ndutrale (robot de oldadura) y 25 grado/eg. para lo robot pequeño de alta pretacone como el STÄUBLI RX9. Práctca 2.- Pág.

2 Práctca de Robótca utlzando Matlab Un apecto mportante que refleja la relacone dnámca del robot repecto de la carga útl que pueden manpular, puede etudare en la guente tabla. Robot Peo Carga útl Repetbldad Carga útl/peo ABB IRB ABB IRB ABB IRB 64/ STÄUBLI RX GMF S Htach M Puma SCARA Adept SCARA GMF A Al calcular la cnemátca de lo robot cláco debe conderare que dependendo de la dmenone de u prmera artculacone, el peo de lo robot de tpo ndutral ocla en torno a valore que tenen una relacón en el mejor de lo cao de.5 (Carga útl/peo). Por lo cual, por ejemplo un robot ndutral con un alcance de 3. metro con capacdad para mover carga de 75 kg puede tener un peo de 45 kg (ABB IRB 64). La guente on alguna recomendacone que deben tenere en cuenta al defnr la cnemátca de un robot, la cual debe hacere en conderacón de la dnámca que mponen la dmenone de la barra que lo forman: El epaco de trabajo del robot debe er cudadoamente etudado para defnr el volumen juto de trabajo del robot En un robot de e grado de lbertad rotaconal, la prmera tre barra on la que aportan la mayor dnámca debdo a u peo. A menudo e poble localzar lo prmero tre acconamento de potenca en la bae del robot, pero para lograr eto e debe er cudadoo en el uo de mecanmo de cuatro barra que mueven el brazo ma alejado (robot ABB IRB24). En un robot de e grado de lbertad, la tre prmera artculacone del robot deben dar la condcone de pocón y la tre últma artculacone del extremo del robot deben concentrar en un punto de la mano, lo tre grado de lbertad de orentacón. En eta práctca e van a preentar la herramenta neceara para reolver lo do problema fundamentale en el etudo de la cnemátca del robot. El prmero de ello, conte en determnar la pocón y orentacón del extremo fnal de la cadena cnemátca conocdo lo valore de la coordenada artculare y la caracterítca geométrca del robot, y e conocdo como problema cnemátco drecto. La olucón del problema nvero permte hallar la varable artculare conocda la pocón y orentacón del extremo de la cadena cnemátca. Para la reolucón de eto problema e utlza la repreentacón de Denavt-Hartenberg y la matrce de tranformacón homogénea. Práctca 2.- Pág. 2

3 Práctca de Robótca utlzando Matlab De acuerdo con la etructura del lbro, todo lo apartado preentan herramenta dearrollada en MatLab para la olucón de lo problema planteado Cnemátca drecta del brazo de un robot manpulador La técnca que e etudan aquí, e aplcan a un manpulador mecánco de cadena aberta y tratan el etudo analítco y el modelado en MatLab de la geometría del movmento de un robot con repecto a un tema de referenca fjo como una funcón del tempo n conderar la dnámca El problema cnemátco drecto El problema cnemátco drecto e plantea en térmno de encontrar una matrz de tranformacón que relacona el tema de coordenada lgado al cuerpo en movmento repecto a un tema de coordenada que e toma como referenca. Para lograr eta repreentacón e uan la matrce de tranformacón homogénea 4x4, la cual ncluye la operacone de tralacón y la orentacón. La matrz de tranformacón homogénea e una matrz de 4x4 que tranforma un vector expreado en coordenada homogénea dede un tema de coordenada hata otro tema de coordenada. Para una decrpcón má ampla acerca de la bae algebraca de la tranformacone homogénea e recomenda etudar la referenca: [] y [2]. La matrz de tranformacón homogénea tene la guente etructura: n x matrz de rotacón vector de pocón = = ny T f x3 ecalado nz n a p T = px p y p z z z donde lo vectore n,, a, on vectore ortogonale untaro y p e un vector que decrbe la pocón x, y, z del orgen del tema actual repecto del tema de referenca. Para entender la propedade de la matrz de tranformacón homogénea no fjamo en el guente gráfco. x y a a a x y Práctca 2.- Pág. 3

4 Práctca de Robótca utlzando Matlab T j z Stema-j z y y p=(, 4, 3) x Stema- x Fgura-2.. Interpretacón geométrca de la matrz de tranformacón homogénea T j = 4 3 Al analzar la columna de la ubmatrz de rotacón de la matrz de tranformacón homogénea T j, un obervador localzado en el orgen de tema-, puede ver como etán orentado lo eje x, y, z del tema-j, ademá tambén oberva como e ha deplazado en coordenada carteana el orgen del tema-j repecto del orgen del tema de referenca con la nformacón del vector de pocón La repreentacón de Denavt-Hartenberg La repreentacón de D-H, e aplca a robot de cadena cnemátca aberta y conte en una ere de regla para colocar lo tema de referenca de cada elabón del robot. Ante de aplcar el método de D-H e mportante tener en cuenta lo guente comentaro: Se parte de una confguracón cualequera del robot, ben e aconejable colocarlo en una pocón enclla de analzar. Se numeran lo elabone, agnando el para la bae y n- para el últmo elabón, endo n el número de grado de lbertad del robot. El tema de coordenada ortonormal dextrógro de la bae (x, y, z ) e etablece con el eje z localzado a lo largo del eje de movmento de la artculacón y apuntando haca fuera del hombro del brazo del robot. El tema de referenca de cada elabón e coloca al fnal del mmo, en el extremo de la artculacón a la cual eta conectado el elabón guente. El ángulo ó deplazamento de cada elabón empre e mde tomando como bae el tema de referenca del elabón anteror. Práctca 2.- Pág. 4

5 Elabón - + Artculacón Práctca Artculacón Elabón de Robótca + utlzando Matlab Al colocar el tema a de referenca del zelabón-, e deben egur la guente regla: - El eje z del tema de referenca debe x quedar alneado a lo largo de la artculacón - d El eje x debe colocare con orentacón normal al plano formado por lo eje z - y z z - Al etablecer lo tema de coordenada de la mano e debe tener en cuenta el prncpo de Peper en el cual e etablece que lo tre últmo tema de referenca e ntercepten en un punto, lo cual permte obtener una olucón para el problema cnemátco nvero de forma cerrada para eta artculacone. x - α Elabón + Ademá de la regla anterore la convencón de D-H etablece la guente condcone para lo demá parámetro geométrco, de acuerdo a la fgura-2.2. Fgura-2.2. Stema de coordenada para la convencón de D-H. Cada tema de coordenada e etablece obre la guente regla. : E el ángulo de la artculacón dede el eje x - hata el eje x, meddo repecto del eje z -, uando la regla de la mano derecha. d : E la dtanca medda dede el orgen del tema -, a lo largo del eje z - hata la ntereccón del eje z - con el eje x. a : E la dtanca de eparacón entre lo orígene de lo tema de referenca - e, medda a lo largo del eje x hata la ntereccón con el eje z -. (o la dtanca má corta entre lo eje z - y z, cuando eto no e nterceptan) α : E el ángulo que epara lo eje z y z -, meddo repecto del eje x Práctca 2.- Pág. 5

6 Práctca de Robótca utlzando Matlab Práctca 2.- Pág. 6 Con bae en la fgura-2.2 y de acuerdo a la regla de D-H, e determna la guente matrz de tranformacón homogénea: = c c a c c d A α α α α = d c a c c c c a c c A α α α α α α Códgo en Matlab. La funcón DENAVIT realza lo cálculo anterore devolvendo la matrz de tranformacón homogénea % DENAVIT Matrz de tranformacón homogénea. % DH = DENAVIT(TETA, D, A, ALFA) devuelve la matrz de tranformacon % homogénea 4 x 4 a partr de lo parametro de Denavt-Hartenberg % D, ALFA, A y TETA. % % See alo DIRECTKINEMATIC. functon dh=denavt(teta, d, a, alfa) dh=[co(teta) -co(alfa)*n(teta) n(alfa)*n(teta) a*co(teta); n(teta) co(alfa)*co(teta) -n(alfa)*co(teta) a*n(teta); n(alfa) co(alfa) d; ];

7 d 2 Práctca de Robótca utlzando Matlab Repreentacón de la cnemátca drecta de robot manpuladore En eta eccón e explcan alguna arqutectura de robot y como contrur la tabla de lo parámetro de D-H. Para una nformacón z 3 má detallada obre ete tema, e recomenda etudar la referenca [] y [2]. l 2 z Ejemplo 2. z x 2 y 2 x y z 2 3 a 2 x 3 d 3 y l 4 x 4 z 4 y y 4 x Fgura-2.3 Parámetro de D-H para un robot clíndrco Elabón d a α l 2 d 2 - a 2 -π/2 3 d l 4 Tabla 2. Parámetro de D-H para el robot clíndrco de la fgura-2.3 Note el lector el gno negatvo del parámetro a 2 aí como la localzacón del orgen del tema de coordenada (x 2, y 2, z 2 ) La varable artculare on en ete cao, d 2, d 3, 4 Práctca 2.- Pág. 7

8 y 3 Práctca de Robótca utlzando Matlab Ejemplo 2.2 x 2 3 y 2 a 2 z z 2 x 3 l z 3 x z 5 6 y 5 y 4 z 4 x 4 6 l 6 a n 2 y z x l x y Fgura-2.4 Parámetro de D-H para un robot rotaconal Tabla 2.2 Parámetro de D-H para el robot clíndrco de la fgura-2.4 Elabón d a α +π/2 l -π/ π/2 a π π/2 4 4 l 4 π/2 5 5 π/2 6 6 l 6 Note el lector que en ete cao toda la varable artculare correponden a lo gro en la artculacone. Obérvee la rotacón en 4 entre lo elabone 3 y 4. Obérvee que el tema de referenca (x 3, y 3, z 3 ) e ha traladado al prncpo del elabón 3 cumplendo con la repreentacón de D-H. Práctca 2.- Pág. 8

9 Práctca de Robótca utlzando Matlab Códgo en Matlab. La funcón DIRECTKINEMATIC reuelve la cnemátca drecta de robot de cadena aberta. Para utlzarla debe nvocare junto con el vector de coordenada artculare Q de n componente endo n el número de elabone del robot. A contnuacón e muetra el códgo fuente utlzado para cada uno de lo robot que e etán utlzando como ejemplo. El lector debe notar la modfcacone realzada en el códgo, debdo al dferente número de elabone de cada robot y a lo parámetro D-H de cada robot. Se recomenda que e realcen eto ejemplo como práctca de lo conocmento adqurdo. Ejemplo 2.3 Códgo fuente de la funcón DIRECTKINEMATIC para el robot clíndrco de 4 grado de lbertad del ejemplo 2. Debe notare que el vector de coordenada artculare Q repreenta lo 4 grado de lbertad del robot, 2 rotaconale y 2 prmátca y e ntroduce en lo parámetro D- H como varable. La dmenone del robot, ntroducda en lo parámetro de D-H, on la ndcada en el capítulo. % DIRECTKINEMATIC4 Drect Knematc. % A4 = DIRECTKINEMATIC4(Q) devuelve la matrz de tranformacón del % prmer tema de coordenada al últmo en funcón del vector Q % de varable artculare. % % See alo DENAVIT. functon A4 = drectknematc4(q) % Parámetro Denavt-Hartenberg del robot teta = [q() q(4)]; d = [.4 q(2) q(3).2 ]; a = [ -. ]; alfa = [ -p/2 ]; % Matrce de tranformacón homogénea entre tema de coordenada conecutvo A = denavt(teta(), d(), a(), alfa()); A2 = denavt(teta(2), d(2), a(2), alfa(2)); A23 = denavt(teta(3), d(3), a(3), alfa(3)); A34 = denavt(teta(4), d(4), a(4), alfa(4)); % Matrz de tranformacón del prmer al últmo tema de coordenada A4 = A * A2 * A23 * A34; Práctca 2.- Pág. 9

10 Práctca de Robótca utlzando Matlab A contnuacón e preenta la matrz obtenda para una confguracón en la que toda la coordenada artculare tenen valor nulo:» q=zero(4,) q =» T=drectknematc4(q) T =» Se recomenda analzar la matrz homogénea obtenda tal y como e ndcó en el apartado 2.2., comprobando que tanto la pocón como la orentacón del efector fnal concden con lo eperado cuando e ntroducen la coordenada artculare del ejemplo. Ejemplo 2.4 Códgo fuente de la funcón DIRECTKINEMATIC para el robot rotaconal de 6 grado de lbertad del ejemplo 2.2 Debe notare que en el cao partcular de un robot enteramente rotaconal, la varable artculare repreentan lo parámetro teta de D-H, como e etudó en el apartado La dmenone del robot, ntroducda en lo parámetro de D-H, on la ndcada en la práctca. Práctca 2.- Pág.

11 Práctca de Robótca utlzando Matlab % DIRECTKINEMATIC6 Drect Knematc. % A6 = DIRECTKINEMATIC6(Q) devuelve la matrz de tranformacón del % prmer tema de coordenada al últmo en funcón del vector Q % de varable artculare. % % See alo DENAVIT. functon A6 = drectknematc6(q) % Parámetro Denavt-Hartenberg del robot teta = q; d = [ ]; a = [.45 ]; alfa = [-p/2 p/2 -p/2 p/2 ]; % Matrce de tranformacón homogénea entre tema de coordenada conecutvo A = denavt(teta(), d(), a(), alfa()); A2 = denavt(teta(2), d(2), a(2), alfa(2)); A23 = denavt(teta(3), d(3), a(3), alfa(3)); A34 = denavt(teta(4), d(4), a(4), alfa(4)); A45 = denavt(teta(5), d(5), a(5), alfa(5)); A56 = denavt(teta(6), d(6), a(6), alfa(6)); % Matrz de tranformacón del prmer al últmo tema de coordenada A6 = A * A2 * A23 * A34 * A45 * A56; A contnuacón e preenta la matrz obtenda para una confguracón en la que toda la coordenada artculare tenen valor nulo:» q=zero(6,) q =» T=drectknematc6(q) T = Práctca 2.- Pág.

12 Práctca de Robótca utlzando Matlab Cnemátca nvera del brazo de un robot manpulador La cnemátca nvera conte en hallar lo valore de la coordenada artculare del robot q = [ q, q,..., q ] T 2 n conocda la pocón y orentacón del extremo del robot. A pear de que en la lteratura [] y [2] e pueden encontrar dvero método genérco para la reolucón de la cnemátca nvera que pueden er mplementado en computadora, uele er habtual la reolucón por medo de método geométrco. La mayor parte de lo robot uelen tener cadena cnemátca relatvamente enclla, que facltan la utlzacón de lo método geométrco. Para mucho robot, e conderan ólo lo tre prmero grado de lbertad, e tene una etructura planar. Ete hecho faclta la reolucón del problema. Ammo lo últmo tre grado de lbertad uelen uare para la orentacón de la herramenta, lo cual permte una reolucón geométrca deacoplada de la pocón de la muñeca del robot y de la orentacón de la herramenta. En eta eccón e va a reolver el problema cnemátco nvero para lo do robot anterore, utlzando el método geométrco e mplementándolo en Matlab. Ejemplo 2.5 Solucón del robot clíndrco de 4 grado de lbertad. En ete cao partcular, la olucón geométrca e nmedata. Se parte de que la pocón del extremo del robot e conocda (p x, p y, p z ) y e va a calcular lo valore de la coordenada artculare. Artculacón Para obtener el valor de (TETA en el códgo de Matlab.) e proyecta el punto del extremo del robot (p x, p y, p z ) obre el plano (x, y, z ) obtenendo una enclla relacón angular. Sabendo que e el ángulo entre x y x, e obtenen la guente gráfca. Práctca 2.- Pág. 2

13 p x,p y,p z orentacón y a 2 d 3 +l 4 R r Práctca de Robótca utlzando Matlab β φ x z x y Fgura-2.5 Cnemátca nvera del robot de 4 gdl. De la que e deducen la guente relacone: R = p x p y 2 2 r = R a2 = d3 + p y nφ = R a2 nβ = R l 4 co φ = co β = utlzando la funcón atan2 de Matlab e calculan lo valore de lo ángulo: que permten el cálculo de como φ-β. Artculacón 2 φ = atan2( nφ,coφ) β = atan2( nβ,co β ) De la fgura 2.3 e obtene la guente fórmula: p x R r R Artculacón 3 De la fgura 2.5: l + d2 = pz d2 = pz l 2 2 r = R a2 = d3 + l 4 Práctca 2.- Pág. 3

14 y 3 x 4 donde n y on vectore de orentacón del extremo del robot. Práctca de Robótca utlzando Matlab 4 x 3 Artculacón 4 Para calcular la últma artculacón e neceta el cálculo prevo del tema de referenca (x 3, y 3, z 3 ), que e reolverá medante la cnemátca drecta explcada en el ejemplo 2.3. Dado que el vector a de aproxmacón e necearamente paralelo a z 4 e deben cumplr la guente relacone: n 4 = n y 3 co 4 = y 3 4 = atan2( n 4,co 4) Códgo en Matlab. La funcón INVERSEKINEMATIC4 reuelve la cnemátca nvera del robot clíndrco de 4 gdl. Para ello toma como parámetro la matrz homogénea T, que repreenta la orentacón y pocón del extremo del robot y devuelve el vector de coordenada artculare. % Q = INVERSEKINEMATIC4(T) devuelve el vector de coordenada % artculare correpondente a la olucón cnemátca nvera de % la mano del manpulador en la pocón y orentacón expreada % en la matrz T. % % See alo DIRECTKINEMATIC4, DENAVIT. functon q = nvereknematc4(t) p = T(:3,4); % Pocón de la mano del manpulador % Incalzacón de la varable artculare a calcular q = ; q2 = ; q3 = ; q4 = ; % Parámetro Denavt-Hartenberg del robot teta = [q q4 ]; d = [.4 q2 q3.2]; a = [ -. ]; alfa = [ -p/2 ]; % Solucón de la prmera artculacón: q R = qrt(p()^2+p(2)^2); r = qrt(r^2-a(2)^2); ph = -p()/r; cph = p(2)/r; ph = atan2(ph, cph); Práctca 2.- Pág. 4

15 Práctca de Robótca utlzando Matlab beta = -a(2)/r; cbeta = r/r; beta = atan2(beta, cbeta); q = ph - beta; % Solucón de la egunda artculacón: q2 q2 = p(3) - d(); % Solucón de la tercera artculacón: q3 q3 = r - d(4); % Solucón de la cuarta artculacón: q4 % Cálculo de la matrz de tranformacón A3 A = denavt(q, d(), a(), alfa()); A2 = denavt(teta(2), q2, a(2), alfa(2)); A23 = denavt(teta(3), q3, a(3), alfa(3)); A3 = A * A2 * A23; y3 = A3(:3,2); q4 = dot(t(:3,), y3); cq4 = dot(t(:3,2), y3); q4 = atan2(q4, cq4); % Vector orentacón n: T(:3,) % Vector orentacón : T(:3,2) % Vector de varable artculare q = [q q2 q3 q4]'; Se oberva como la cnemátca drecta etá ncluda en lo cálculo necearo para obtener la matrz A 3. En el ejemplo motrado a contnuacón e puede comprobar como depué de agnar un vector de coordenada artculare aleatoro, y obtener la matrz homogénea del extremo de robot correpondente a ete vector, obre eta matrz e aplca la funcón INVERSEKINEMATIC4 e obtene el vector q orgnal. Práctca 2.- Pág. 5

16 Práctca de Robótca utlzando Matlab» q=rand(4,) q = » T=drectknematc4(q) T = » nvereknematc4(t) an = Ejemplo 2.6 Solucón del robot rotaconal de 6 grado de lbertad. En contrate con el ejemplo anteror, la olucón de la cnemátca nvera de un robot de 6 grado de lbertad requere del lector un mayor efuerzo de comprenón. En ete ejemplo e va a reolver medante el método geométrco. Para ello, en vrtud del prncpo de Peper, e necearo eparar el cálculo de la pocón y orentacón del extremo del robot. En prmer lugar e calcula la pocón del punto de ntereccón de lo 3 últmo grado de lbertad, que e conoce como MUÑECA del robot. Con la coordenada (p x, p y, p z ) de la muñeca e reuelve el problema de la pocón, de manera mlar al ejemplo anteror. Lo 3 últmo grado de lbertad e utlzan para orentar la herramenta colocada en el extremo del robot, y e reuelven en un pao poteror. Como e oberva en la fgura 2.6, la artculacón 3 ( 3 ), en adelante CODO del robot, puede tener do valore dtnto, conocdo como confguracón CODO ARRIBA y CODO ABAJO, y que repreentan lo cao en que la artculacón 3 etá tuada por Práctca 2.- Pág. 6

17 R c CODO p x,p y MUÑECA p x, p y, p z Práctca de Robótca utlzando Matlab debajo o por encma de la artculacón 2, para lograr una mma pocón de la muñeca. a Ammo, la MUÑECA puede tomar tambén do confguracone dtnta, MUÑECA ARRIBA y MUÑECA ABAJO, para obtener n una mma orentacón del efector fnal. Debdo a la anterore conderacone, e necearo ntroducr en la reolucón del problema cnemátco nvero do parámetro, llamado por mltud CODO y MUÑECA, que z repreenten la cuatro poble confguracone que puede adoptar el robot cuando e le olcta alcanzar una pocón y orentacón determnada. Para una decrpcón má ampla acerca de la reolucón del problema cnemátco y nvero e x recomenda etudar la referenca: [] y [2]. Fgura-2.6 Cnemátca nvera del robot de 6 gdl. A contnuacón e muetra la olucón geométrca del robot etudado y u mplementacón en Matlab. Se recomenda egur la explcacone con el códgo fuente de la funcón INVERSEKINEMATIC6. Problema de pocón. Cálculo de la tre prmera artculacone. Artculacón R = p x p y n = p y R co = p x R = atan2( n,co ) Práctca 2.- Pág. 7

18 x 2 l 3 p x, p y, p z r l 3 p x, p y, p z l 2 2 β Práctca de Robótca utlzando Matlab r α β l 2 2 α x 2 l x Artculacón 2 l x Para la reolucón de la egunda artculacón, e ha de tener en cuenta la pobldad de CODO ARRIBA y CODO ABAJO. En cada cao el ángulo 2 e calculará como la uma o la reta de lo ángulo α y β calculado. Confguracón CODO ARRIBA Confguracón CODO ABAJO Identfcando la relacone geométrca preente en lo anterore equema: = α + β r = l p 2 x + p 2 y + ( p l = r + l2 2rl l3 r + l2 = 2rl 2 n β = ± nα z ) co β co β co l pz r R r = co α = 2 2 = α β 2 β 2 Confguracón CODO ARRIBA Confguracón CODO ABAJO Práctca 2.- Pág. 8

19 x 2 l 3 l 2 x 3 3 β l 3 Artculacón 3 l l 2 β Práctca de Robótca utlzando Matlab Al gual que en la artculacón 2, e debe tener en cuenta la do poble confguracone del codo. 3 x 3 x 2 3 3π = β 2 3 π = β 2 Problema de orentacón. Cálculo de la tre últma artculacone. Conocdo lo tre prmero ángulo, 2 y 3 e reuelve la cnemátca drecta para lo tre prmero elabone, obtenéndoe la matrz A 3 neceara para la reolucón de la últma tre artculacone. Sguendo la referenca [], para reolver el problema de la orentacón e han de conegur que la tre últma artculacone cumplan lo crtero guente:. Etablecer la artculacón 4 de forma tal que una rotacón repecto de la rotacón 5 alneará el eje de movmento de la artculacón 6 con el vector de aproxmacón dado (a). 2. La artculacón 5 alneará el eje de movmento de la artculacón 6 con el vector de aproxmacón. 3. Fjar la artculacón 6 para alnear el vector de orentacón dado () (o el de delzamento (n)) y el vector normal. Matemátcamente, eto crtero gnfcan:. z 4 ± = z 2. a = z 5 3. = y 6 ( z xa) 3 3 xa Amba orentacone de la muñeca (ARRIBA y ABAJO) e defnen obervando la orentacón del tema de coordenada de la mano (n,,a) con repecto al tema de coordenada (x 5, y 5, z 5 ). Para analzar la confguracone de MUÑECA ARRIBA y MUÑECA ABAJO, e utlza un parámetro de orentacón Ω (omega en el códgo de Matlab ) que hace referenca a la orentacón del vector untaro n (o ) con repecto al vector untaro x 5 (o y 5 ) y que vene defndo en la referenca Fu [] como: e etá en el cao degenerado Ω= y 5 y 5 n y 5 y 5 = Práctca 2.- Pág. 9

20 z 4 y 4 a Práctca n de Robótca utlzando Matlab y 5 y x 5 x 3 x 4 x 4 n Artculacón 4 Para conocer el gno de la ecuacón z 4 ( z xa) ± 3 = e determna conocendo la z xa orentacón Ω y la confguracón de MUÑECA. Ω e calcula egún u defncón, y MUÑECA e un parámetro del robot. Se parte de la upocón de que el gno e potvo, y eta hpóte e corrge con el parámetro M que e calcula como combnacón de la confguracón de la muñeca y la orentacón requerda. Utlzando lo parámetro Ω y MUÑECA e contruye la guente tabla: Confguracón MUÑECA Ω M=MUÑECA*gn(Ω) ABAJO + + ABAJO + < - ARRIBA - - ARRIBA - < + que permte obtener M como parámetro para calcular la rotacón 4 en cualquera de la confguracone poble. De la fgura 2.4 e puede demotrar la guente relacone: 3 n = M ( z ) x co 4 = M ( z4 y3) 4 = atan2( n 4,co 4) Artculacón 5 De la fgura 2.4 e obtene: n = a 5 x 4 co 5 = ( a y4 ) 5 = atan2( n 5,co 5 ) Artculacón 6 n = n 6 y 5 co 6 = y 5 6 = atan2( n 6,co 6 ) Práctca 2.- Pág. 2

21 Práctca de Robótca utlzando Matlab Códgo en Matlab.La funcón INVERSEKINEMATIC6 reuelve la cnemátca nvera del robot rotaconal de 6 gdl. Para ello toma como parámetro la matrz homogénea T, y lo parámetro CODO y MUÑECA para defnr la poble confguracone. Ha de tenere en conderacón que determnado punto y orentacone que pertenezcan al epaco de trabajo del robot, no podrán alcanzare con alguna confguracone. Por ello e recomenda al lector que trate de expermentar con eto punto extremo con la herramenta que a contnuacón e preenta, y analce lo reultado. % INVERSEKINEMATIC6 Invere Knematc % Q = INVERSEKINEMATIC6(T, CODO, MUNECA) devuelve el vector de coordenada % artculare correpondente a la olucón cnemátca nvera de la mano % del manpulador en la pocón y orentacón expreada en la matrz T. % CODO = ndca codo del robot arrba, e decr, que la artculacón 3 e % túa por encma de la artculacón 2, mentra que CODO = - ndca codo % abajo, e decr que la artculacón 2 e túa por encma de la 3. % MUNECA = ndca que la muñeca del robot e túa por debajo de la coordenada % expreada en T, mentra que MUNECA = - gnfca que la muñeca e túa % por arrba. % % See alo DIRECTKINEMATIC6, DENAVIT. functon q = nvereknematc6(t,codo,muneca) % Parámetro Denavt-Hartenberg del robot d = [ ]; a = [.45 ]; alfa = [-p/2 p/2 -p/2 p/2 ]; % Pocón de la mano del manpulador p = T(:3,4)-d(6)*T(:3,3); % Solucón de la prmera artculacón: q R = qrt(p()^2+p(2)^2); q=p(2)/r; cq=p()/r; q = atan2(q,cq); % Solucón de la egunda artculacón: q2 r = qrt(r^2+(p(3)-d())^2); alfa = (d()-p(3))/r; calfa = R/r; cbeta = (r^2+a(2)^2-d(4)^2)/(2*r*a(2)); beta = qrt(-cbeta^2); f codo == - % Codo abajo q2 = alfa*cbeta+beta*calfa; cq2 = calfa*cbeta-alfa*beta; ele % Codo arrba q2 = alfa*cbeta-beta*calfa; cq2 = calfa*cbeta+alfa*beta; end q2 = atan2(q2,cq2); % Solucón de la tercera artculacón: q3 cbeta=(a(2)^2+d(4)^2-r^2)/(2*a(2)*d(4)); beta=qrt(-cbeta^2); beta=atan2(beta,cbeta); Práctca 2.- Pág. 2

22 Práctca de Robótca utlzando Matlab f codo == % Codo arrba q3 = 3*p/2-beta; ele % Codo abajo q3 = beta - p/2; end % Solucón de la cuarta artculacón: q4 % Cálculo de la matrz de tranformacón A3 A = denavt(q, d(), a(), alfa()); A2 = denavt(q2, d(2), a(2), alfa(2)); A23 = denavt(q3, d(3), a(3), alfa(3)); A3 = A * A2 * A23; x3 = A3(:3,); y3 = A3(:3,2); z3 = A3(:3,3); z4 = cro(z3,t(:3,3)); % Vector orentacón a: T(:3,3) % Determnacón del ndcador de orentacón omega aux = dot(t(:3,2),z4); % Vector orentacón : T(:3,2) f aux ~= omega = aux; ele aux=dot(t(:3,),z4); % Vector orentacón n: T(:3,) f aux ~= omega=aux; ele omega=; end end M = muneca*gn(omega); q4 = -M*dot(z4,x3); cq4 = M*dot(z4,y3); q4 = atan2(q4,cq4); % Solucón de la qunta artculacón: q5 z5 = T(:3,3); % Vector de orentacón a: T(:3,3) A34 = denavt(q4, d(4), a(4), alfa(4)); A4 = A3 * A34; x4 = A4(:3,); y4 = A4(:3,2); q5 = dot(t(:3,3),x4); % Vector de orentacón a: T(:3,3) cq5 = -dot(t(:3,3),y4); % Vector de orentacón a: T(:3,3) q5 = atan2(q5,cq5); % Solucón de la exta artculacón: q6 y6 = T(:3,2); % Vector de orentacón : T(:3,2) A45 = denavt(q5, d(5), a(5), alfa(5)); A5 = A4 * A45; y5 = A5(:3,2); q6 = dot(t(:3,),y5); % Vector de orentacón n: T(:3,) cq6 = dot(t(:3,2),y5); % Vector de orentacón : T(:3,2) q6 = atan2(q6,cq6); % Vector de coordenada artculare q = [q q2 q3 q4 q5 q6]'; Práctca 2.- Pág. 22

23 Práctca de Robótca utlzando Matlab En el ejemplo motrado a contnuacón e puede comprobar como depué de agnar un vector de coordenada artculare aleatoro, lo cual ya ncluye una determnada confguracón de CODO y MUÑECA y obtener la matrz homogénea del extremo de robot correpondente a ete vector, obre eta matrz e aplca la funcón INVERSEKINEMATIC6 con lo valore correcto de CODO y MUÑECA para el vector de coordenada artculare, e obtene el vector q orgnal. Se recomenda expermentar la funcón INVERSEKINEMATIC6 con vectore de coordenada artculare encllo de analzar.» q=rand(6,) q = » T=drectknematc6(q) T = » nvereknematc6(t,-,) an = Repreentacón gráfca en MatLab uando alambre. En ete apartado e van a preentar una enclla herramenta gráfca válda para comprobar lo reultado anterore utlzando la funcone ya etudada. Se recomenda al lector que realce eto ejemplo modfcando el códgo de Matlab. Para comprobar lo reultado e ha utlzado una nueva funcón nombrada como DRAWROBOT3D, que permte una enclla repreentacón 3D utlzando línea de la confguracón del manpulador. Ha de conderare que eta funcón realza el trazado del robot conforme a u repreentacón D-H, por lo que lo tema de referenca que han do deplazado dan lugar a elabone de longtud nula, dando la mpreón óptca de que han deaparecdo. Para comprobar que el efecto de eta rotacone e tomado en conderacón e ugere al lector que coloque un pequeño tema de referenca en el extremo del robot. Práctca 2.- Pág. 23

24 Práctca de Robótca utlzando Matlab Ejemplo 2.7 Códgo en Matlab. La funcón DRAWROBOT3D realza una repreentacón 3D de un robot en funcón del vector de varable artculare Q Partcularzando DRAWROBOT3D para lo ejemplo anterore e obtene la funcone que e han llamado DRAWROBOT3D4 y DRAWROBOT3D6. % DRAWROBOT3D4 Repreentacón 3D de un robot. % DRAWROBOT3D4(Q) realza una repreentacón 3D de un robot % en funcón del vector de varable artculare Q. % % See alo DENAVIT, DIRECTKINEMATIC4. functon drawrobot3d4(q) % Parámetro Denavt-Hartenberg del robot teta = [q() q(4)]; d = [.4 q(2) q(3).2 ]; a = [ -. ]; alfa = [ -p/2 ]; % Matrce de tranformacón homogénea entre tema de coordenada conecutvo A = denavt(teta(), d(), a(), alfa()); A2 = denavt(teta(2), d(2), a(2), alfa(2)); A23 = denavt(teta(3), d(3), a(3), alfa(3)); A34 = denavt(teta(4), d(4), a(4), alfa(4)); % Matrce de tranformacón del prmer tema al correpondente A2 = A * A2; A3 = A2 * A23; A4 = A3 * A34; % Vector de pocon (x, y, z) de cada tema de coordenada x = ; y = ; z = ; x = A(,4); y = A(2,4); z = A(3,4); x = x; y = y; z = z + d(2); x2 = A2(,4); y2 = A2(2,4); z2 = A2(3,4); x3 = A3(,4); y3 = A3(2,4); z3 = A3(3,4); x4 = A4(,4); y4 = A4(2,4); z4 = A4(3,4); % Se dbuja el robot x = [x x x x2 x3 x4]; y = [y y y y2 y3 y4]; z = [z z z z2 z3 z4]; plot3(x,y,z); % Se coloca una rejlla a lo eje grd; % Se etablecen lo límte de lo eje ax([ ]); S e repreenta la confguracón en la que toda la coordenada artculare on nula, e obtene el guente dbujo: Práctca 2.- Pág. 24

25 Práctca de Robótca utlzando Matlab Ejemplo 2.8 Se preenta ahora el códgo y la repreentacón gráfca del robot rotaconal de 6 grado de lbertad. % DRAWROBOT3D6 Repreentacón 3D de un robot. % DRAWROBOT3D6(Q) realza una repreentacón 3D de un robot % en funcón del vector de varable artculare Q. % % See alo DENAVIT, DIRECTKINEMATIC6. functon drawrobot3d6(q) % Parámetro Denavt-Hartenberg del robot teta = q; d = [ ]; a = [.45 ]; alfa = [-p/2 p/2 -p/2 p/2 ]; % Matrce de tranformacón homogénea entre tema de coordenada conecutvo A = denavt(teta(), d(), a(), alfa()); A2 = denavt(teta(2), d(2), a(2), alfa(2)); A23 = denavt(teta(3), d(3), a(3), alfa(3)); A34 = denavt(teta(4), d(4), a(4), alfa(4)); A45 = denavt(teta(5), d(5), a(5), alfa(5)); A56 = denavt(teta(6), d(6), a(6), alfa(6)); Práctca 2.- Pág. 25

26 Práctca de Robótca utlzando Matlab % Matrce de tranformacón del prmer tema al correpondente A2 = A * A2; A3 = A2 * A23; A4 = A3 * A34; A5 = A4 * A45; A6 = A5 * A56; % Vector de pocon (x, y, z) de cada tema de coordenada x = ; y = ; z = ; x = A(,4); y = A(2,4); z = A(3,4); x2 = A2(,4); y2 = A2(2,4); z2 = A2(3,4); x3 = A3(,4); y3 = A3(2,4); z3 = A3(3,4); x4 = A4(,4); y4 = A4(2,4); z4 = A4(3,4); x5 = A5(,4); y5 = A5(2,4); z5 = A5(3,4); x6 = A6(,4); y6 = A6(2,4); z6 = A6(3,4); % Se dbuja el robot x = [x x x2 x3 x4 x5 x6]; y = [y y y2 y3 y4 y5 y6]; z = [z z z2 z3 z4 z5 z6]; plot3(x,y,z); % Se coloca una rejlla a lo eje grd; % Se etablecen lo límte de lo eje ax([- -.5]); S e repreenta la confguracón en la que toda la coordenada artculare on nula, e obtene el guente dbujo: Práctca 2.- Pág. 26

27 Práctca de Robótca utlzando Matlab PRACTICA. Anmacón de lo robot. En ete apartado e va a realzar un encllo ejemplo de utlzacón de la funcone etudada hata ahora. Se trata de colocar al robot en do pocone dtnta y anmar una trayectora recta entre ea do confguracone. Para ello e deberán generar vara pocone ntermeda. Se utlza la funcón planfca(p,p2,n) en la que e ntroducen la coordenada carteana de lo punto ncal y fnal y el número de punto ntermedo. Ejemplo 2.9 En ete ejemplo e utlza la cnemátca nvera del robot rotaconal de 6 gdl para trazar una línea recta entre un punto p ncal y un punto p2 fnal. El número de punto ntermedo e varable. Se ha utlzado la funcón PLANIFICA6(P,P2,N,S,A,CODO,MUÑECA,NPUNTOS) en el que e ntroduce la coordenada carteana de lo punto ncal y fnal, la orentacón (n,,a) del punto fnal, lo parámetro CODO y MUÑECA para elecconar la confguracón del robot y el número de punto ntermedo. Eta funcón proporcona una matrz de (npunto+2) columna por 6 fla que e utlzará por la funcón ANIMACION6(MAT_Q) para dbujar la trayectora entre lo do punto. Códgo en Matlab. La funcón PLANIFICA6(P,P2,N,S,A,CODO,MUÑECA, NPUNTOS) calcula la matrz de coordenada artculare utlzada para grafcar el movmento del robot. % PLANIFICA6 Planfcacón de trayectora % MAT_Q = PLANIFICA6(P, P2, N, S, A, CODO, MUNECA, NPUNTOS) realza % una planfcacón de trayectora en línea recta dede la coordenada % carteana P hata la P2, de manera que la mano del manpulador % poee la orentacón expreada por [N S A]. CODO = ndca codo del % robot arrba, e decr, que la artculacón 3 e túa por encma de % la artculacón 2, mentra que CODO = - ndca codo abajo, e decr % que la artculacón 2 e túa por encma de la 3. MUNECA = ndca que % la muñeca del robot e túa por debajo de la coordenada carteana, % mentra que MUNECA = - gnfca que la muñeca e túa por arrba. % NPUNTOS ndca el número de punto en lo que e dvde la trayectora. % En MAT_Q e devuelve la coordenada artculare, almacenada por % columna, correpondente a cada uno de lo punto carteano en lo % que e dvde la trayectora. MAT_Q e una matrz de NPUNTOS + 2 columna % y 6 fla. % % See alo INVERSEKINEMATIC6. functon mat_q = planfca6(p, p2, n,, a, codo, muneca, npunto) % Cálculo del vector untaro u = p2-p; mu = qrt(u()^2+u(2)^2+u(3)^2); u = (/mu)*u; % Cálculo de la dtanca entre punto d = mu/(npunto+); Práctca 2.- Pág. 27

28 Práctca de Robótca utlzando Matlab for =:(npunto+) % Cálculo de la pocón carteana actual de la mano del manpulador p = p+(*d)*u; T = [n a p]; % Cálculo de la coordenada artculare q = nvereknematc6(t,codo,muneca); mat_q(:,+) = q; end Utlzando ahora la funcón ANIMACION6(MAT_Q) e preenta el movmento del robot entre lo 2 punto epecfcado. % ANIMACION6 Anmacón de la trayectora de un robot % ANIMACION(MAT_Q) realza la anmacón de la trayectora, expreada % en la matrz MAT_Q, de un brazo robot de 6 GDL. MAT_Q contene 6 fla % y una columna para cada dpocón del robot. % % See alo PLANIFICA6, DRAWROBOT3D6. functon anmacon6(mat_q) % Parámetro Denavt-Hartenberg del robot d = [ ]; a = [.45 ]; alfa = [-p/2 p/2 -p/2 p/2 ]; % Vector de pocon (x, y, z) del tema de coordenada de referenca x = ; y = ; z = ; % Se dbuja el tema de coordenada de referenca. Se agna el modo XOR para borrar % ólo el robot dbujado anterormente. Se utlza un groor de línea de 2 undade p = plot3(x,y,z,'eraemode','xor','lnewdth',2); % Se agna una rejlla a lo eje grd; % Se etablecen lo límte de lo eje ax([- -.5]); % Mantene el gráfco actual hold on; % Número de columna de la matrz n = ze(mat_q,2); % Se dbuja la dpocón del robot correpondente a cada columna for =:n % Varable artculare del brazo robot teta = mat_q(,); teta2 = mat_q(2,); teta3 = mat_q(3,); teta4 = mat_q(4,); teta5 = mat_q(5,); teta6 = mat_q(6,); Práctca 2.- Pág. 28

29 Práctca de Robótca utlzando Matlab % Matrce de tranformacón homogénea entre tema de coordenada conecutvo A = denavt(teta, d(), a(), alfa()); A2 = denavt(teta2, d(2), a(2), alfa(2)); A23 = denavt(teta3, d(3), a(3), alfa(3)); A34 = denavt(teta4, d(4), a(4), alfa(4)); A45 = denavt(teta5, d(5), a(5), alfa(5)); A56 = denavt(teta6, d(6), a(6), alfa(6)); % Matrce de tranformacón del prmer tema al correpondente A2 = A * A2; A3 = A2 * A23; A4 = A3 * A34; A5 = A4 * A45; A6 = A5 * A56; % Vector de pocon (x, y, z) de cada tema de coordenada x = A(,4); y = A(2,4); z = A(3,4); x2 = A2(,4); y2 = A2(2,4); z2 = A2(3,4); x3 = A3(,4); y3 = A3(2,4); z3 = A3(3,4); x4 = A4(,4); y4 = A4(2,4); z4 = A4(3,4); x5 = A5(,4); y5 = A5(2,4); z5 = A5(3,4); x6 = A6(,4); y6 = A6(2,4); z6 = A6(3,4); % Se dbuja el robot x = [x x x2 x3 x4 x5 x6]; y = [y y y2 y3 y4 y5 y6]; z = [z z z2 z3 z4 z5 z6]; et(p,'xdata',x,'ydata',y,'zdata',z); % Se fuerza a MATLAB a actualzar la pantalla drawnow; end Notar que la funcón plot3 ha permtdo dbujar obre la anmacón la trayectora eguda por el extremo del robot. Práctca 2.- Pág. 29

30 Práctca de Robótca utlzando Matlab EJERCICIOS PROPUESTOS Se pde mplementar la funcone PLANIFICA4 y ANIMACIÓN4 para el ejemplo con el robot prmátco de 4 gdl y realzar una anmacón entre do punto del epaco de trabajo del robot. Práctca 2.- Pág. 3