Memora del Trabajo Fn de Máster realzado por Fdel Pérez Menéndez para la obtencón del título de Máster en Ingenería de Automatzacón e Informátca Industral Desarrollo de sstema de control para un manpulador de ses grados de lbertad Febrero, 214
2. Desarrollo de sstema de control para un manpulador de ses grados de lbertad Índce 1. INTRODUCCIÓN... 7 1.1. Estado actual de la robótca... 7 1.2. Característcas actuales de los robots.... 8 1.2.1. Causas de la falta de precsón y contnudad de trayectoras... 1 1.3. Objetvos del proyecto... 11 1.4. Descrpcón del manpulador... 12 1.4.1. Eslabones... 13 1.4.2. Artculacones... 17 1.4.3. Compensadores... 19 1.5. Estructura del Trabajo... 19 1.5.1. Dagrama de Gantt... 2 2. ESTUDIO CINEMÁTICO... 22 2.1. Parámetros Denavt-Hartenberg... 22 2.2. Matrces de Transformacón Homogéneas... 24 2.2.1. Fxed Angles X-Y-Z... 26 2.3. Cnemátca drecta... 27 2.4. Cnemátca nversa: método geométrco... 29 2.5. Cnemátca de la Velocdad: el Jacobano... 4 2.5.1. Cálculo del Jacobano... 42 2.5.2. Sngulardades... 44 3. ESTUDIO DINÁMICO... 46 3.1. Dstrbucón de la Masa del Manpulador... 46 3.1.1. Centros de Masas... 46 3.2. Pseudo-Matrz de Inerca... 47 3.3. Dnámca Inversa... 49 3.3.1. Formulacón Euler-Lagrange... 5 3.3.1.1. Velocdad de un punto del manpulador... 51 3.3.1.2. Energía Cnétca... 52 3.3.1.3. Energía Potencal... 53 3.3.1.4. Ecuacones del Movmento del Manpulador... 54 3.3.1.5. Introduccón de los efectos de frccón... 56 3.3.1.6. Dnámca de los actuadores... 57
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 3 3.4. Dnámca Drecta... 58 4. CONTROL... 6 4.1. Generador de trayectoras... 6 4.1.1. Modelado matemátco... 63 4.2. Sensores... 66 4.2.1. Sensores electromagnétcos rotatvos... 66 4.2.2. Resolvers... 67 4.2.3. Encóder Incremental... 69 4.3. Actuadores... 7 4.3.1. Servomotores... 71 4.3.1.1. Motores Brushless... 72 4.3.1.2. Etapas reductoras... 72 4.3.2. Drvers... 73 4.4. Modelado del sstema de control... 74 4.4.1. Lnealzacón... 75 4.4.2. Control por par calculado... 76 4.4.2.1. Controlador PD... 79 4.4.2.2. Espaco de estados (PD)... 8 4.4.2.3. Eleccón de las ganancas... 83 4.4.3. Control por dnámca nversa... 84 4.4.3.1. Controlador lneal PID... 86 4.4.3.2. Espaco de estados (PID)... 88 4.4.4. Control Dgtal... 91 4.4.4.1. Ecuacones en dferencas... 92 4.4.5. Control en el Espaco Cartesano... 96 4.4.5.1. Control con dnámca en el espaco artcular... 96 4.4.5.2. El jacobano en el domno de fuerzas... 11 4.4.5.3. Control con dnámca en el espaco cartesano... 12 4.4.6. Control de fuerza... 14 4.4.6.1. Control Híbrdo Poscón-Fuerza... 16 5. CÁLCULOS NUMÉRICOS... 11 5.1. Regulador PID en tempo contnuo... 11 5.2. Reguladores PID y PD en tempo dscreto... 112 6. SIMULACIÓN... 114 6.1. Smulacón dnámca... 114 6.2. Smulacón de trayectoras... 115 6.3. Control por par calculado... 117
4. Desarrollo de sstema de control para un manpulador de ses grados de lbertad 6.4. Control por dnámca nversa... 122 7. CONCLUSIONES Y TRABAJOS FUTUROS... 129 8. PRESUPUESTO... 131 8.1. Hardware... 131 8.2. Software... 132 8.3. Costes de personal... 132 8.4. Coste total del proyecto... 133 9. BIBLIOGRAFÍA... 134
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 5 MEMORIA
6. Desarrollo de sstema de control para un manpulador de ses grados de lbertad
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 7 1. Introduccón El presente Trabajo Fn de Máster de Ingenería de Automatzacón e Informátca Industral de la Unversdad de Ovedo se ha elaborado durante la realzacón de las Práctcas en Empresa en la Fundacón PRODINTEC. Este Trabajo forma parte del proyecto MILL2MATIC, actualmente en estado de desarrollo y enmarcado dentro del Programa Naconal de cooperacón Públco Prvada, dentro del subprograma INNPACTO 212. En este capítulo se muestra el sstema general en el que se engloba el proyecto MILL2MATIC, sus prncpales objetvos y la parte que este trabajo aporta a su desarrollo y consecucón. Fgura 1. Celda robótca de la FUNDACION PRODINTEC 1.1. Estado actual de la robótca Para comprender el objetvo que se plantea en este proyecto, es necesaro conocer prmero las posbldades actuales de la robótca, especalmente la parte referda a manpuladores de ses grados de lbertad, muy extenddos en la ndustra. El campo de la robótca ndustral se defne como el estudo, dseño y uso de robots para la ejecucón de procesos ndustrales. El estándar ISO 8373:1994 (Robots Industrales Manpuladores) defne un robot ndustral como un manpulador programable en tres o más ejes multpropósto, controlado automátcamente y reprogramable. En ndustras como la de la automocón, donde se realzan muchas tareas repettvas como operacones de pntado, la soldadura o la manpulacón, en las últmas décadas
8. Desarrollo de sstema de control para un manpulador de ses grados de lbertad se ha producdo la progresva mplantacón de sstemas robotzados para cumplr dchas tareas. Los robots desarrollados hasta el momento han mostrado ser capaces y efcentes, susttuyendo y mejorando los resultados de labores que anterormente se realzaban de manera manual o semautomátca. Sn embargo, en tareas en las que la repettvdad no es el factor determnante, por ejemplo, en las que se exgen otro tpo de cualdades como la precsón o la resolucón de tareas complejas, la robótca no ha penetrado a un nvel comparable. En tareas donde se fabrcan pezas úncas o seres muy cortas, con tolerancas estrechas (del orden de,1 mm) y trayectoras complejas, hasta el momento las aplcacones robotzadas no han pasado de nstalacones a nvel de laboratoro. La automatzacón de algunas de estas aplcacones se consgue hoy en día a través de máqunas herramenta, como pueden ser los centros de mecanzado de alta velocdad o las máqunas de encntado automátco (ATL), las cuales ofrecen solucones con una gran robustez y fabldad, consguéndose una gran precsón en el segumento de trayectoras. No obstante, las máqunas-herramenta traen consgo una sere de desventajas, como por ejemplo su elevado coste asocado, propósto específco, baja flexbldad y volumen elevado, por lo que suelen necestar de una gran superfce de planta para su nstalacón. La robótca, dada su flexbldad, menor coste, polvalenca y alta productvdad se presenta como una alternatva ventajosa de automatzacón para procesos como los anterormente menconados. De esta manera, el objetvo general del proyecto MILL2MATIC es avanzar en la mplementacón de la robótca en la ndustra, acercándola en mayor medda a procesos donde no se aplca actualmente, medante la superacón de las lmtacones comentadas. De manera resumda, las necesdades actuales, y por tanto, los retos a los que se enfrenta el proyecto global se pueden enuncar como: Obtencón de una mayor precsón de posconamento en los robots o manpuladores, tanto de forma puntual como a lo largo de una trayectora, a fn de garantzar la fabrcacón de pezas con reducdas tolerancas. Mejora en la contnudad en las trayectoras a elevadas velocdades, a fn de alcanzar productvdades y acabados smlares a las de una máquna herramenta. 1.2. Característcas actuales de los robots. A partr de la defncón y de las característcas menconadas en el apartado anteror, se pueden destacar como prncpales vrtudes de la robótca ndustral actual la polvalenca y la flexbldad. Hablamos de dspostvos polvalentes porque son máqunas de propósto general que deben ser programadas para una determnada tarea. Su funcón se determna, además de por la programacón, por la herramenta utlzada para llevar a cabo la tarea. Su flexbldad resde en que la programacón y el cambo de herramenta pueden cambar a lo largo de su vda útl con una smple
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 9 reconfguracón de sus parámetros. A estas vrtudes hay que sumar la repettvdad que presentan en la poscón alcanzada (en torno a.1 mm de margen). Por últmo cabe destacar la alta productvdad que consguen gracas a que la lgereza de la estructura posblta movmentos muy veloces. En cuanto a las lmtacones, las más mportantes se centran en la dfcultad de programacón, en la baja precsón de posconamento alcanzable y en la falta de contnudad en trayectoras de gran densdad de puntos. Respecto a la falta de precsón, cabe menconar que es debda prncpalmente a la elastcdad de su estructura, que produce deflexones mportantes dfícles de modelar de forma precsa. Otra causa es el control, que no utlza un sstema de lazo cerrado en el que se realmente con un sstema de medcón externo la poscón de la herramenta, sno que por el contraro son los resolver de los motores los que controlan la poscón de las artculacones, lo que por defncón no permte alcanzar altas precsones. En cuanto a la contnudad en las trayectoras, ésta resulta totalmente satsfactora en el caso de trabajos a baja velocdad. Sn embargo, cuando se trata de aumentar la velocdad de operacón, el control no es capaz de mantener la suavdad en el movmento debdo a que no tene un tempo de respuesta lo sufcentemente corto y, en menor medda, a problemas mecáncos como holguras, mprecsones en la fabrcacón, etc. El hecho de que se trate de un sstema de baja precsón pero alta repettvdad se debe a que cuando el robot es envado a un punto objetvo y a una orentacón dada, el control hace los cálculos necesaros para que cada eje se poscone hasta llegar a esa poscón y orentacón real calculada, que tene una desvacón respecto al objetvo. Habtualmente no exste una medda drecta de la poscón y orentacón del efector fnal: uno debe confar en la geometría asumda del manpulador y en su rgdez para calcular la poscón del efector fnal a partr de las poscones de las artculacones meddas. De este modo, la precsón se ve afectada por errores computaconales, errores de fabrcacón, flexón de los eslabones, backlash en las undades de reduccón, etc. Este error resulta ser aproxmadamente constante, por lo que cada vez que el robot va a un punto lo hará desvándose del punto objetvo una dstanca gual al valor de la precsón, pero sempre caerá dentro de una esfera de rado el valor de la repettvdad. Es decr, es probable que en x movmentos del robot a un punto objetvo determnado nunca estemos sobre dcho punto objetvo, pero sempre tendremos la certeza de que la poscón real del robot se encuentra dentro de una esfera de rado RP marcada por la repettvdad, sendo la precsón absoluta AP el valor de la dstanca entre el punto objetvo y el centro de esa esfera de rado RP. Todos estos conceptos pueden verse representados en la Fgura 2 a contnuacón:
1. Desarrollo de sstema de control para un manpulador de ses grados de lbertad Fgura 2. Concepto de precsón y repettvdad En cuanto a la falta de contnudad de las trayectoras, ésta está motvada por una actuacón de los motores más lenta que la que demanda para el movmento del manpulador. Este fenómeno se materalza en forma de mcroparadas en cada uno de los puntos que forman la trayectora, que generan vbracones y otras perturbacones que afectan al proceso de fabrcacón. Por ejemplo, en el caso del mecanzado, el acabado de las superfces teórcamente lsas resulta defcente debdo a la elevada rugosdad y a la presenca de muescas. A la vsta de estas lmtacones se puede entender por qué las aplcacones habtuales de los robots en la ndustra son aquellas que se caracterzan por ser sencllas, repettvas, y no requerr precsón o que debdo a la alta productvdad es asumble un tempo de programacón y puesta a punto elevado. Se justfca así la baja mplantacón en aplcacones con altos requstos de toleranca o con necesdad de trayectoras complejas. 1.2.1. Causas de la falta de precsón y contnudad de trayectoras En cuanto a la precsón y a la contnudad de trayectoras, se ahondará en este apartado en las causas por las que estos aspectos son lmtantes a la hora de utlzar manpuladores robótcos en tareas en las que se precsan dchas característcas. Las prncpales causas son: Sensórca: las desvacones atrbubles a la sensórca ntegrada en los sstemas robótcos actuales tene una mplcacón drecta en el correcto funconamento del msmo, ya que son los elementos que devuelven los valores utlzados para el posconamento del robot o manpulador. Este tpo de defectos se pueden corregr medante calbracones. Las desvacones dervadas de la sensórca pueden venr de: Precsón del sensor: capacdad que tene el sensor de tener una precsón nterna adecuada, es decr, de su repettvdad y de su capacdad de transformar la medcón de dchos parámetros en meddas dgtales. Resolucón: lmtacón del sensor, según su característcas mecáncas consttutvas. Excentrcdad: defectos asocada al montaje del sensor en el sstema. Influencas externas: errores que proceden de nfluencas externas, como pueden ser cargas adconales, temperaturas, nterferencas, etc. Control: es el responsable del correcto funconamento y movldad del sstema, y por esta razón, la mayor fuente de desvacones. Las desvacones de las que es fuente el control se pueden englobar en: Transformacones Matemátcas: Todo el funconamento del manpulador es controlado, a grandes rasgos, por algortmos matemátcos que representan su funconamento y que ante
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 11 determnados valores de entrada ofrecen los valores de salda programados. Estos algortmos, que representan no solo el funconamento cnemátco y dnámco, sno tambén la manera como el robot se mueve en espaco y controla sus actuadores, están dseñados en base a desarrollos matemátcos que tenen errores asocados y altos costes computaconales. Precsón computaconal: El hardware dseñado para poder albergar toda esta matemátca, trabaja con un determnado tpo de varable, lmtadas en espaco y tamaño. Capacdad computaconal: En funcón de la matemátca desarrollada y de la precsón requerda, se desarrolla el hardware que tenga la capacdad operatva de realzar las operacones para poder envar las señales a la actuacón. En realdad, el hardware tene retrasos y sumados a los ya menconados antes, se obtene el tempo total de refresco o la capacdad del sstema para actuar frente a determnados estímulos. Este tempo de refresco mpone el límte de actuacón del control y por consguente la capacdad del sstema para poder moverse con una precsón determnada. Mecánca: La consttucón mecánca de un robot y sus acconamentos son tambén una de las fuentes prncpales de pérdda de precsón. En este sentdo, las deformacones que sufren los eslabones del robot en su movmento provocan que exsta una dferenca entre la defncón matemátca teórca de funconamento y su comportamento real. Del msmo modo, los actuadores tenen un límte de operacón, véndose afectados por factores externos, por lo que perden resolucón en funcón de los msmos parámetros que la mecánca. Instalacón: La puesta en marcha de uno de estos equpos supone un reto desde el punto de vsta de la precsón del msmo, ya que muchos de los defectos asocados a un robot se crea en ese momento. Los errores de posconamento y la falta de una calbracón en la nstalacón fnal hacen de esta fase una fuente de pérdda de precsón en su funconamento. Se dentfcan por tanto el control del robot y la sensórca ntegrada como los factores lmtantes fundamentales de cara a mejorar tanto la precsón absoluta del robot (capacdad para stuarse en un punto determnado dentro del volumen de trabajo) como en la contnudad en trayectoras. 1.3. Objetvos del proyecto El objetvo general del proyecto MILL2MATIC consste en la mejora de las capacdades actuales de los manpuladores o robots ndustrales, de manera que puedan presentarse como una alternatva a las máqunas herramenta para determnados procesos donde las tolerancas estrechas y las trayectoras complejas hacen que la robótca actual no sea una alternatva factble.
12. Desarrollo de sstema de control para un manpulador de ses grados de lbertad La consecucón de este objetvo general mplca el cumplmento de los sguentes objetvos específcos, que representan una mejora con respecto a los sstemas actuales: Desarrollo de un sstema de control con nuevas capacdades, que tenga en cuenta el orgen de las desvacones (mecáncas, externas, matemátcas, etc.) que dan lugar a la falta de precsón actual de los robots, a través de su mplementacón en un hardware específco. Integracón de sstemas de medcón externos basados en nterferometría láser que no estén en contacto con la mecánca del robot y que permtan un mejor posconamento tanto estátco como dnámco del conjunto gracas al cerre del lazo de control de posconamento de la herramenta. Se busca consegur así una mayor precsón y una mejora en la contnudad de las trayectoras. El resultado fnal esperado es la obtencón de un sstema de control que mejore la precsón y exacttud del posconamento del efector fnal. Esa mejora vendrá dada por un menor perodo de funconamento del lazo de control, por el uso de modelos matemátcos más completos del manpulador, y por la adcón de sensórca que proporcone otro tpo de datos que permtan mejorar el sstema de control. El objetvo de este Trabajo Fnal de Máster, englobado dentro de los objetvos del proyecto Mll2Matc, será la propuesta y realzacón de lazos de control que permtan llevar a cabo las mejoras propuestas anterormente. 1.4. Descrpcón del manpulador Un manpulador robótco es un conjunto de cuerpos conectados en una cadena por medo de juntas o artculacones. Estos cuerpos se denomnan enlaces o eslabones. Cada una de las artculacones une a dos eslabones consecutvos. En los manpuladores pueden exstr dos tpos de artculacones: la prsmátca y la rotatora. Una artculacón prsmátca, tambén conocda como junta deslzante, posblta a un eslabón el movmento lneal sobre el otro eslabón que conecta la artculacón. Una artculacón rotatora permte el gro un eslabón con respecto al otro eslabón que conecta la artculacón. En el caso del manpulador KUKA KR5-2 todas las artculacones que lo conforman son de tpo rotatoro. Cada una de ellas a su vez establece cada uno de los grados de lbertad, por lo que el manpulador tene 6 artculacones. Este modelo de manpulador es un modelo destnado a cargas muy pesadas, lo que hace de él un sstema especalmente adecuado para su utlzacón en procesos de mecanzado. Además ofrece una capacdad de carga y un espaco de trabajo mucho mayor, y prolonga consderablemente las dstancas entre los puntos de gravedad de referenca. Además, posee una almentacón de energía estrechamente ceñda al cuerpo o drectamente ntegrada, lo cual le permte reducr los contornos de perturbacón de manera que la superfce de colocacón del robot resulta mucho más aprovechable.
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 13 1.4.1. Eslabones En las sguentes fguras se separa la estructura del manpulador para dferencar las partes fundamentales que lo componen: Fgura 3. Brazo del manpulador Fgura 4. Muñeca del manpulador La fgura 3 consttuye el brazo antropomórfco del manpulador y es la responsable de su posconamento en el espaco, mentras que la fgura 4 consttuye la muñeca del sstema y supondrá la orentacón fnal del sstema de coordenadas del extremo del robot (efector fnal). Realmente, la poscón y orentacón alcanzadas fnalmente no dependerán úncamente de las partes respectvas menconadas, sno que será un conjunto de ambas, ya que la muñeca poscona el efector fnal en funcón del posconamento prevo del brazo. Lo msmo sucede con el brazo respecto de la muñeca. Tanto el brazo como la muñeca forman un conjunto, que es el manpulador, y deben coordnarse conjuntamente para posconar y orentar el efector fnal. Cada una de las dos partes estará dvdda a su vez en eslabones conectados a través de artculacones rotatvas. Cada una de ellas se muestra a contnuacón de manera ndependente del resto del manpulador resaltada en color amarllo frente al resto de eslabones.
14. Desarrollo de sstema de control para un manpulador de ses grados de lbertad El prmer eslabón se mostrará en las dos partes que lo componen, ya que está compuesto por dos pezas que forman el conjunto que se estuda en relacón al movmento de la artculacón. La prmera de ellas es la base: Fgura 5. Base (prmer eslabón) En ella se encuentra el sstema de referenca del manpulador. Es una parte fja sobre la que se ancla el manpulador al suelo, por ello el sstema de referenca está stuado sobre ella, y es el punto al que se referen los datos de poscón y orentacón del efector fnal en lo que se denomnará espaco cartesano. La segunda peza que conforma el prmer eslabón se muestra en la sguente fgura: Fgura 6. Prmer eslabón
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 15 Esta peza es la que rota en torno a la base formando la prmera artculacón. El segundo eslabón es el sguente: Fgura 7. Segundo eslabón Este segundo eslabón está formado por una únca peza, al gual que el resto de los eslabones sguentes. El tercer eslabón se muestra a contnuacón: Fgura 8. Tercer eslabón
16. Desarrollo de sstema de control para un manpulador de ses grados de lbertad Este es el últmo eslabón que conforma el brazo. Los sguentes forman parte de la muñeca del manpulador. Todos ellos se muestran a contnuacón: Fgura 9. Cuarto eslabón Fgura 1. Qunto eslabón
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 17 Fgura 11. Sexto eslabón El últmo eslabón, el sexto, está formado por la peza resaltada de la fgura anteror. En ella se ncluye el efector fnal, que representa un offset que no será tendo en cuenta para los cálculos cnemátcos del manpulador, ya que el uso de una herramenta modfcaría ese offset, permtendo mantener los modelos matemátcos que se desarrollarán en el apartado 2. 1.4.2. Artculacones Además de los eslabones que contene el manpulador, otra parte fundamental para su estudo son las artculacones sobre las que se produce el movmento de los propos eslabones. El manpulador Kuka KR5-2 es un manpulador de ses grados de lbertad. Cada uno de ellos es aportado por cada una de sus ses artculacones, todas ellas rotaconales. En la sguente fgura se representa el manpulador con el eje de gro de cada una de ellas:
18. Desarrollo de sstema de control para un manpulador de ses grados de lbertad Fgura 12. Ejes de rotacón En rosa aparecen las artculacones del brazo, que posconan el efector fnal; y en verde se muestran las artculacones de la muñeca, que lo orentan. Las artculacones tenen unas lmtacones angulares y velocdades de funconamento, que defnen el dextrous space (puntos alcanzables con cualquer orentacón) y el espaco alcanzable del robot (puntos alcanzables): Fgura 13. Dextrous space y espaco alcanzable Las lmtacones de dchas artculacones, tanto de poscón como de velocdad, están defndas a contnuacón: Eje Lmtes Velocdad Máxma
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 19 A1 (θ1) ±185º 69 º/s A2 (θ2) +11º a -4º 69 º/s A3 (θ3) +6 a -184º 69 º/s A4 (θ4) ±35º 77 º/s A5 (θ5) ±118º 76 º/s A6 (θ6) ±35º 12 º/s Tabla 1 1.4.3. Compensadores Además de los eslabones y las artculacones, el manpulador dspone de un sstema de compensacón dnámca, conformado por un clndro hdráulco-neumátco. El objetvo de este sstema es compensar el peso de los eslabones posterores a la segunda artculacón y de la carga en el efector fnal o herramenta, que tene un efecto dnámco muy elevado sobre la segunda artculacón, al ser ésta la artculacón posteror a la base y sobre la que se artcula el resto del peso del manpulador. Además de este sstema, en el codo del robot se ncorpora un contrapeso cuyo objetvo es compensar el peso de la carga, así como de los eslabones cuarto, qunto y sexto, de manera que las poscones más extremas no tengan un efecto dnámco muy elevado que exgese una actuacón muy exgente para el actuador de la artculacón. 1.5. Estructura del Trabajo Para el desarrollo del trabajo se han hecho una sere de tareas que se descrbrán a contnuacón: Documentacón: recoplacón de nformacón sobre robótca e ntroduccón de los objetvos a realzar durante la elaboracón del proyecto. Desarrollo matemátco e mplementacón de la cnemátca drecta: a partr de la documentacón recoplada se afronta el problema cnemátco drecto del manpulador Kuka KR5-2. Desarrollo matemátco e mplementacón de la cnemátca nversa: con el desarrollo de la cnemátca drecta ya realzado, se procede entonces a resolver
2. Desarrollo de sstema de control para un manpulador de ses grados de lbertad el problema cnemátco nverso para el manpulador dado a través del método geométrco. Desarrollo matemátco e mplementacón de la cnemátca de la velocdad: para completar el estudo cnemátco del manpulador, se desarrollan e mplementan los desarrollos de la cnemátca de velocdad, cuyo prncpal objetvo es permtr conocer el Jacobano. Desarrollo matemátco e mplementacón de la dnámca: con los modelos cnemátcos ya realzados, se avanza un paso más consderando el movmento en relacón a las fuerzas que lo orgnan. Estudo de sstemas de control para manpuladores: estudo de leyes de control compatbles con los modelos desarrollados del manpulador. Aprendzaje de los métodos exstentes y adaptacón a las necesdades propas del proyecto. Desarrollo de la smulacón del manpulador: a través de toolboxes de Smulnk se realzan dferentes modelos que representan vrtualmente las característcas cnemátcas y dnámcas del manpulador. Implementacón de sstemas de control: realzacón de lazos de control de acuerdo con los modelos mplementados en Smulnk con el fn de comprobar la correccón de los msmos y realzar posterormente pruebas HIL en el hardware de control específco. Realzacón de la memora del Trabajo Fn de Máster: redaccón de la memora que recoge el trabajo realzado prevamente. 1.5.1. Dagrama de Gantt Cada una de las tareas descrtas es mostrada en un dagrama de Gantt, junto con la planfcacón temporal del desarrollo de cada una de ellas. Se ha utlzado para su realzacón el software Gantt Project:
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 21 sep.-13 sep.-13 oct.-13 nov.-13 nov.-13 dc.-13 dc.-13 ene.-14 feb.-14 Documentacón Desarrollo matemátco e mplementacón de la Cnemátca Drecta Desarrollo matemátco e mplementacón de la Cnemátca Inversa Desarrollo matemátco e mplementacón de la Cnemátca de Velocdad Desarrollo matemátco e mplementacón de la Dnámca Estudo de Sstemas de Control para manpuladores Desarrollo de la smulacón del manpulador Implementacón de sstemas de control Realzacón de la memora del trabajo Fgura 14. Dagrama de Gantt
22. Desarrollo de sstema de control para un manpulador de ses grados de lbertad 2. Estudo cnemátco Tras la descrpcón ncal del sstema se ha proceddo a su descrpcón matemátca, desarrollada a contnuacón para poder ser mplementada más adelante medante el uso del software de smulacón MATLAB. El uso de este software, nos permtrá tener un modelo sobre el que mplementar los conceptos de control propuestos y probar los resultados prevamente a su desarrollo sobre el hardware de control. El desarrollo de los modelos matemátcos que defnen las cnemátcas del manpulador es el prmer paso a realzar para el estudo mecánco que dará el conocmento del comportamento del sstema para poder controlarlo. A través de la cnemátca drecta se podrá conocer la poscón del extremo o punto fnal del manpulador, que será el punto de nterés dado que es donde se encontrará sujeta la herramenta que se utlzará para realzar el trabajo oportuno, a través de las poscones de cada una de sus artculacones. La cnemátca nversa, por el contraro, permte conocer las poscones artculares de los datos de poscón y orentacón del extremo del manpulador. La cnemátca es la cenca que trata el movmento sn tener en cuenta las fuerzas que lo causan. Dentro de la cnemátca, se estuda la poscón, la velocdad y la aceleracón. Por lo tanto, el estudo de la cnemátca de manpuladores se refere a todas las propedades geométrcas y basadas en el tempo de los movmentos. El estudo de la cnemátca del manpulador mplca, entre otras cosas, suponer cambos en la ubcacón de los sstemas de referenca de cada artculacón a medda que el mecansmo se mueve. La notacón de las artculacones se hará comenzando desde la base (prmera artculacón) hasta el efector fnal (sexta artculacón), sguendo la cadena mecánca que forma el manpulador. 2.1. Parámetros Denavt-Hartenberg Para el desarrollo de los modelos tanto de la cnemátca drecta como de la nversa, se utlzará la representacón de Denavt-Hartenberg. Se trata de un procedmento que asgna a cada una de las artculacones un sstema de referenca, estando el orgen sobre el eje de la artculacón de dcho sstema y dotándolo de tres ejes (X, Y, Z). Se establecen así coordenadas cartesanas o coordenadas rectangulares, que son un tpo de coordenadas ortogonales usadas en espacos euclídeos para la representacón del movmento o poscón en físca, caracterzadas porque usa como referenca ejes ortogonales entre sí que se cortan en un punto orgen. Las coordenadas cartesanas se defnen así como la dstanca al orgen de las proyeccones ortogonales de un punto dado sobre cada uno de los ejes. S el sstema en s es un sstema trdmensonal, se denomna espaco cartesano. Los sstemas de referenca se defnen de la sguente manera: El eje Z se coloca en la dreccón del eje de artculacón.
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 23 El eje X es paralelo a la normal común entre Z n-1, Z n. S no hay una únca normal común (ejes Z paralelos), entonces d (abajo) es un parámetro lbre. El eje Y es orto-normal a los dos anterores según la regla de la mano derecha. Los parámetros de Denavt-Hartenberg permten el paso de un eslabón al sguente, es decr, los eslabones undos por una artculacón, medante cuatro transformacones báscas. Las transformacones dependen exclusvamente de las característcas constructvas del robot. Las transformacones báscas son: Rotacón α alrededor del eje x es la ángulo entre z y z Rotacón θ alrededor del eje z es el ángulo entre x y x Traslacón d a lo largo del eje z entre x y x Traslacón a a lo largo del eje x entre z y z La defncón de estas característcas puede varar respecto del eje de referenca utlzado (el o el -1). Es mportante tener en cuenta la defncón establecda y no cambarla en nngún momento para que los parámetros utlzados conduzcan al resultado correcto de las cnemátcas drecta e nversa. En la sguente fgura se muestra un ejemplo de dos artculacones undas por un eslabón, en la que se dentfcan las transformacones báscas de Denavt-Hartenberg: Fgura 15 Conocendo cómo deben colocarse los ejes en cada artculacón y como se realzan las transformacones báscas, se procede ahora a obtener los parámetros de Denavt- Hartenberg para el manpulador Kuka KR5-2. Se colocarán en la sguente tabla, que recoge los parámetros de cada uno de los eslabones:
24. Desarrollo de sstema de control para un manpulador de ses grados de lbertad α-1 (rad) a-1 (m) d (m) θ (rad) 1 -p/2,5 1,45 θ 1 2-1,3 θ 2 3 -p/2 -,55 θ 3 4 p/2-1,25 θ 4 5 -p/2 θ 5 6,29 θ 6 Tabla 2 2.2. Matrces de Transformacón Homogéneas Contnuando con la notacón de Denavt-Hartenberg, se puede obtener una matrz de transformacón homogénea para cada grado de lbertad en la cual ya estarán ncludos todos los parámetros antes descrtos. Utlzando las transformacones homogéneas a través de las matrces homogéneas, es posble representar las rotacones y traslacones relatvas entre los dstntos eslabones que componen el manpulador. Las matrces de transformacón homogénea se representan con la letra T, precedda de un subíndce y un superíndce, que representan el sstema de referenca de la artculacón precedente (-1), y del posteror () respectvamente. S se desea descrbr la transformacón completa entre las dos artculacones, es decr, entre los sstemas de coordenadas {} e {-1}, se puede escrbr: P 1 1 R T R Q T Q P T P T P (2.1) Sendo P un punto defndo en el sstema de referenca {}, proyectado en el sstema de referenca {-1}. Operando sobre la expresón anteror se obtene:
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 25 P 1 1 T P (2.2) De donde se deduce que: R Q T RT QT PT 1 1 P T (2.3) S consderamos esta matrz de transformacón, formada por cuatro transformacones ndvduales, podemos ver que la expresón anteror puede nterpretarse como una sere de rotacones y desplazamentos alrededor de los ejes X y Z: 1 T R X ( 1 ) DX ( a 1) R Z ( ) D Z ( d ) (2.4) S se utlza la notacón screw, en el que un movmento es la combnacón de una rotacón y una traslacón alrededor de un eje, la expresón anteror puede representarse medante: 1 T Screw X ( 1, a 1) ScrewZ (, d ) (2.5) La expresón anteror puede ser expresada por sus térmnos matrcales de la sguente manera: T cosθ snθ cosα snθ snα a cosθ snθ = cosθ cosα cosθ snα a snθ (2.6) snα cosα d 1 Así se obtene la forma general de la matrz de transformacón homogénea, que expresa la transformacón entre los sstemas de coordenadas de dos eslabones consecutvos. Además, se puede expresar la relacón entre cualquer par de eslabones, aunque no sean consecutvos. Así se puede obtener una matrz de transformacón que será muy mportante para los desarrollos cnemátcos y dnámcos del manpulador: la matrz de transformacón entre la base y el efector fnal, que expresará la orentacón y poscón del efector fnal en las coordenadas cartesanas. Cada una de las columnas expresa en sus tres prmeras flas, comenzando por la zquerda y haca la derecha, el vector de orentacón en el eje X, el Y, el Z y la poscón en el msmo orden de
26. Desarrollo de sstema de control para un manpulador de ses grados de lbertad coordenadas. Por lo tanto las tres prmeras columnas se corresponden con la matrz de rotacón entre los eslabones tratados, y la últma será el vector de poscón. La últma fla se utlza para que la matrz sea cuadrada y poder realzar operacones matrcales de manera más senclla. En la práctca tene sgnfcado físco y expresa la relacón con transformacones de perspectva y escalado, que pueden tener aplcacón en otros campos como la vsón artfcal. 2.2.1. Fxed Angles X-Y-Z Para representar e nterpretar las orentacones se utlzará además de los vectores correspondentes de las matrces de transformacón homogéneas, otra notacón dferente llamada fxed angles X-Y-Z. Se utlzará por la mayor smplcdad a la hora de representar las orentacones, al tratarse de datos úncos y no de vectores, además de ser la manera más común utlzada en la ndustra para transmtr este tpo de nformacón, pudendo tratarse de la notacón aquí propuesta o a través de la notacón de Euler. En cualquer caso, la nformacón recogda en una matrz de rotacón puede ser representada por tres datos cuya nformacón es equvalente. En este proyecto su uso se lmta para representar la orentacón del efector fnal respecto de la base del manpulador, ya que el resto de relacones serán tratadas medante las matrces de transformacón homogéneas. Cada uno de los tres datos se llamará gro (α), nclnacón (β) y osclacón (γ). Para la ntroduccón de referencas, estos parámetros serán el formato utlzado, y a través de su conversón a vectores de orentacón, se trabajará con ellos en los modelos matemátcos. El orgen de la matrz de rotacón equvalente R (α, β, γ), es sencllo, porque todas las rotacones se producen alrededor de los ejes del sstema de referenca, es decr: R (α, β, γ) = R (α)r (β)r (γ) = cosα senα cosβ senβ 1 = senα cosα 1 cosγ senγ (2.7) 1 senβ cosβ senγ cosγ Es mportante tener en cuenta el orden de las rotacones utlzadas en la expresón anteror. Pensando en los térmnos de las rotacones como operadores, se han aplcado las rotacones (desde la derecha) de R (γ), despuésr (β), y fnalmente R (α). Multplcando la expresón, se obtene: R (α, β, γ) = cos α cosβ cos α sen β senγ senα cosγ cosα sen β cosγ + senα senγ = sen α cosβ sen α sen β sen γ + cosα cosγ sen α sen β cosγ cosα senγ (2.8) senβ cosβ senγ cosβ cosγ
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 27 El problema nverso, la de extraccón de ángulos equvalentes fjos X-Y-Z desde una matrz de rotacón, es tambén un proceso de nterés. La solucón depende de la resolucón de un conjunto de ecuacones trascendentales: hay nueve ecuacones y tres ncógntas s la ecuacón (2.8) es equparada a una matrz de rotacón dada. Entre las nueve ecuacones hay ses dependencas, por lo que, en esenca, tenemos tres ecuacones y tres ncógntas. Expresando: R (α, β, γ) r r r = r r r (2.9) r r r Cada uno de los parámetros de nclnacón, gro y osclacón (en este sentdo) equvalen entonces a: β = atan2( r, r + r (2.1) α = atan2( r cosβ, r cosβ (2.11) γ = atan2( r 32 cosβ, r 33 cosβ ) (2.12) Aunque exste una segunda solucón, medante el uso de la raíz cuadrada postva en la fórmula de β, calculamos sempre la solucón únca para la cual -9 β 9º. Esto es generalmente una buena práctca, porque entonces se puede defnr una relacón unoa-uno entre las funcones de orentacón. Sn embargo, en algunos casos, el cálculo de todas las solucones es mportante. S β = ± 9º (por lo que cosβ = ), la solucón degenera. En esos casos, sólo la suma o la dferenca de α e γ puede ser calculada. Una convencón posble es elegr α = en estos casos. 2.3. Cnemátca drecta El objetvo del desarrollo del modelo cnemátco drecto del manpulador es obtener, para unos valores de artculacones dados, tanto la poscón como la orentacón del efector fnal. De esta manera, medante la medcón de la poscón de las artculacones se puede conocer, para cualquer valor de éstas, donde se encuentra el extremo del robot. Para la resolucón de este modelo matemátco es necesaro utlzar las matrces de transformacón homogéneas descrtas en el apartado anteror. Como se vo, cada una de ellas representa el gro y la poscón relatva de cada uno de los eslabones,
28. Desarrollo de sstema de control para un manpulador de ses grados de lbertad proporconando la nformacón de la poscón y orentacón de la artculacón posteror al eslabón tratado. Se relacona así el sstema de coordenadas de un eslabón con el de su eslabón anteror. Después, se procede a encadenar las transformacones ndvduales, de forma que se obtene la poscón y orentacón de un lnk n respecto al eslabón (base). Una vez que se han defndo los parámetros de Denavt-Hartemberg y según se defnan las matrces de transformacón de toda la cadena cnemátca del manpulador, el planteamento de las ecuacones de la cnemátca drecta del manpulador se hace de forma drecta. El prmer paso consste entonces en determnar cada una de las matrces de transformacón sguentes: T, T, T, T, T, T (2.13) A partr de ellas, se calculará la matrz homogénea que relacone el efector fnal con la base, que será la matrz que permtrá obtener de forma drecta los datos en el espaco cartesano del efector fnal. Para ello se multplcarán de forma encadenada las matrces de las que se dspone, utlzando la sguente propedad: T = T T T.. T (2.14) Así, la matrz a calcular T 6 se obtendrá de la sguente expresón: T = T(θ ) T(θ ) T(θ ) T(θ ) T(θ ) T(θ ) (2.15) Donde cada una de las matrces se defne para el manpulador Kuka KR5-2: 1 T cos1 sn1 1 sn cos 1 1 a1 cos 1 a sn 1 1 d 1 1 (2.16) cos 2 sn 1 2 2T sn cos 2 2 1 a2 cos 2 a sn 2 2 1 (2.17)
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 29 (2.18) (2.19) (2.2) (2.21) 2.4. Cnemátca nversa: método geométrco Para el desarrollo de la cnemátca nversa exsten dferentes métodos. El método elegdo se basa en el estudo de la geometría del manpulador respecto de cada una de las artculacones rotatvas que lo componen. Otro tpo de métodos de resolucón suponen un desarrollo más largo y complejo, como es la resolucón algebraca. Dado que cualquer método conduce a la resolucón de los valores artculares para una poscón y orentacón dadas del efector, se elgó un método de senclla resolucón desde el punto de vsta computaconal, factor mportante para fnalmente consegur que los modelos se ejecuten rápdamente para consegur una alta frecuenca de funconamento del lazo de control a mplementar. La expresón arctan2 que aparece en este apartado equvale a la funcón arcotangente con dos argumentos. El propósto de utlzar dos argumentos en lugar de uno es reunr 1 1 sn cos sn cos sn cos 3 3 3 3 3 3 3 3 2 3 a a T 1 1 cos sn sn cos 4 4 4 4 4 3 4 d T 1 1 cos sn sn cos 5 5 5 5 4 5 T 1 1 cos sn sn cos 6 6 6 6 6 5 6 d T
3. Desarrollo de sstema de control para un manpulador de ses grados de lbertad nformacón sobre los sgnos de las entradas con el fn de devolver el cuadrante correspondente del ángulo calculado. El sstema de referenca del manpulador es el únco cuya artculacón rota (la dreccón Z) perpendcular a la base del robot (prmera artculacón). Por lo tanto, para poder calcular el ángulo grado basta con utlzar la poscón del plano perpendcular a la dreccón del gro (formado por X e Y) del efector fnal (parámetro conocdo). Para consegur que el modelo sea más flexble y poder utlzar herramentas de cualquer morfología, en lugar del punto marcado por el efector fnal (sn herramenta), se utlzará el sstema de referenca anteror, es decr, sn tener en cuenta la brda y la herramenta s la hubera. Ese punto será la ubcacón de la qunta artculacón por tanto, y se llamará P. Para calcularlo, a la poscón marcada por la poscón y orentacón del efector fnal, se deben restar las dmensones de la herramenta acoplada. En el caso actual, en el que se supone el manpulador sn nngún tpo de herramenta, el punto P se calcula: P = [x, y, z] d6 Z6 (2.22) d6 representa el parámetro de Denavt-Hartenberg que se corresponde con la longtud de la brda, que se resta a la poscón del efector fnal en la dreccón Z de la últma artculacón. A través de sus coordenadas X e Y se puede conocer por tanto el gro de la prmera artculacón. θ = arctan2(px, Py) (2.23) Para calcular el gro de la segunda artculacón es necesaro un planteamento geométrco más complejo que para la prmera.
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 31 Fgura 16 El prmer paso es el cálculo del segmento que une la tercera artculacón con el efector fnal, con el fn de tener una medda drecta evtando el offset que tene el manpulador en el codo: l = a + l (2.24) A partr de las coordenadas X e Y de la poscón del efector fnal respecto del sstema de referenca actual, se calculará la dstanca entre la segunda artculacón y el extremo del robot: r = P + P (2.25) El ángulo formado por el segmento que une ambos puntos con la dreccón X del sstema de referenca de la segunda artculacón será necesaro para el posteror cálculo del ángulo de gro de la artculacón: β = arctan2( Px, Py) (2.26) Otro ángulo que es necesaro calcular es el del msmo segmento r con respecto al segundo eslabón. Para la obtencón de este ángulo se recurre a la ley de los cosenos: α = arcos l + r + l 2 r l (2.27)
32. Desarrollo de sstema de control para un manpulador de ses grados de lbertad Con estos dos últmos valores ya es posble calcular el ángulo de gro de la segunda artculacón. Dependendo de la confguracón del manpulador puede tratarse de dos gros dferentes. En el caso de que la tercera artculacón stúe al efector fnal entre ella y la segunda artculacón en la dreccón Z del sstema de referenca orgnal (el de la prmera artculacón), la confguracón que se produce se llamará codo abajo y hace que el cálculo del gro de la segunda artculacón se realce con la sguente expresón: θ = π 2 + α β (2.28) Un ejemplo de confguracón de codo abajo sería el sguente: Fgura 17 Se observa que respecto de la vertcal, que stúa la poscón de referenca (cero grados), tenendo en cuenta el sentdo de los ángulos calculados, se debe segur la expresón anteror para poder calcular el ángulo θ. El caso contraro se llama codo arrba, e mplca para el cálculo de la segunda artculacón la utlzacón de la sguente expresón: θ = π 2 α β (2.29) Un ejemplo de esta confguracón aparece en la sguente magen:
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 33 Fgura 18 En este caso los sentdos de los gros hacen que sea necesaro la resta de los ángulos α y β. Ejemplos de estas confguracones sobre la representacón 3D del manpulador son mostrados en las sguentes mágenes, en las que se muestra como para una msma poscón y orentacón del efector fnal exsten al menos dos solucones váldas que mplcan el uso de una u otra confguracón: Fgura 19. Confguracón codo arrba Para el cálculo del gro de la tercera artculacón el proceso es smlar al de la segunda.
34. Desarrollo de sstema de control para un manpulador de ses grados de lbertad Fgura 2. Confguracón codo abajo Fgura 21 Incalmente se calcula la hpotenusa r, que une la segunda artculacón con el efector fnal, al gual que en el caso anteror: r = P + P (2.3) Los ángulos a calcular ahora son dstntos. Uno de ellos es el ángulo formado por el tercer eslabón y el segmento que une la tercera artculacón con el efector fnal:
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 35 φ = arcos a + l + l 2 a l (2.31) El otro es el ángulo que forma esa msma dstanca al efector fnal, pero en este caso con el segundo eslabón. Una vez más se utlza para despejar la ley de los cosenos: ε = arcos l + l r 2 l l (2.32) Fnalmente, para calcular la poscón angular se deben utlzar dchos ángulos en funcón de la confguracón que presente el manpulador. Para la confguracón codo abajo la expresón es: θ = π φ + ε (2.33) En el caso de la confguracón codo arrba: θ = π φ ε (2.34) Para las sguentes artculacones, las de la muñeca, el proceso es dferente. En funcón de la poscón de la muñeca se pueden dentfcar dos nuevas confguracones, que se llamarán muñeca arrba y muñeca abajo. Se suman por tanto a las dos anterores, lo que da lugar a que para una msma poscón y orentacón del efector fnal se puedan dar cuatro poscones artculares dferentes. Ambas confguracones de la muñeca se defnen observando la orentacón del sstema de coordenadas del efector fnal con respecto al sstema de referenca de la cuarta artculacón. Para analzar las confguracones se utlza un parámetro de orentacón que hace referenca a la orentacón del vector untaro Y6 (o X6) con respecto al vector untaro Y4 y que se defne como: = Y6 Y4 X6 Y4 sy6 Y4 = yx6 Y4 = sy6 Y3 sy6 Y3 = (2.35) Medante este parámetro se establecerá el gro de la cuarta artculacón, lo que posterormente nflurá para los gros de la qunta y la sexta artculacón se orenten de manera que se alcance la poscón y orentacón dadas. Dado que la cuarta y la sexta artculacón rotan en torno a un plano común, y sus límtes mecáncos son muy elevados (±35º en ambos casos) se pueden dar las dos confguracones de la
36. Desarrollo de sstema de control para un manpulador de ses grados de lbertad muñeca, en las que la cuarta y la sexta artculacón tengan ángulos complementaros y la qunta artculacón tenga un ángulo opuesto. En el caso de la cuarta artculacón, el prmer paso consste en hallar la dreccón del eje de gro de la qunta artculacón: Fgura 22 z = z z (2.36) A partr del plano formado por las dreccones de gro de la cuarta y la sexta artculacón, se puede hallar esa dreccón, ya que el plano es sempre perpendcular a ella, dado que los gros de la muñeca respecto de las artculacones cuarta y sexta no lo modfcan. El procedmento de cálculo consstrá en hallar el producto vectoral de ambos vectores. Una vez conocda la dreccón del eje de gro de la qunta artculacón, se procede a la proyeccón de los ejes z 5 y x 5 sobre el sstema de referenca de la cuarta artculacón:
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 37 Fgura 23 a = z x (2.37) b = z y (2.38) Para el cálculo del ángulo, es necesaro tener en cuenta la confguracón de la muñeca. Se creará un nuevo parámetro M, que establezca se sgno del gro, de manera que se pueden dar dos confguracones en las que la cuarta artculacón presente gros opuestos: Confguracón Muñeca M Muñeca abajo 1 1 Muñeca abajo 1 < -1 Muñeca arrba -1-1 Muñeca arrba -1 < 1 Tabla 3 Este nuevo parámetro se utlzará en la funcón arcotangente, establecendo en funcón de la orentacón fnal el ángulo que tene que grar la cuarta artculacón para obtener la confguracón deseada:
38. Desarrollo de sstema de control para un manpulador de ses grados de lbertad θ = arctan2( M a, M b) (2.39) El método de resolucón de las ecuacones posterores se basa de nuevo en el cálculo de productos vectorales cuyos resultados srven para obtener el ángulo de gro a través de la trgonometría. Una vez calculado θ 4, tenemos totalmente defndo el sstema de coordenadas de la qunta artculacón, de modo que el ángulo θ 5 puede calcularse a partr de los ángulos formados entre los vectores drectores de esta artculacón y el efector fnal, todos conocdos. Para ello procedemos de nuevo a la proyeccón de ambos sstemas de coordenadas sobre un msmo plano, en este caso, el plano x5y5: Fgura 24 El procedmento de obtencón del ángulo θ es análogo al de la artculacón anteror:
Desarrollo de sstema de control para manpulador de ses grados de lbertad. 39 Fgura 25 a = y z (2.4) b = x z (2.41) θ = arctan2(b, a) (2.42) Fnalmente, proyectamos de nuevo los sstemas de coordenadas de la qunta y sexta artculacón en el msmo plano, esta vez proyectando en la dreccón de gro de la sexta artculacón z6 en el plano x5z5. Una vez más, se sgue el msmo procedmento para la obtencón del gro de la artculacón:
4. Desarrollo de sstema de control para un manpulador de ses grados de lbertad Fgura 26 a = x z (2.43) b = y z (2.44) θ = arctan2( a, b) (2.45) 2.5. Cnemátca de la Velocdad: el Jacobano Medante los modelos de la cnemátca drecta e nversa, se consgue el control estátco del posconamento, stuando el efector fnal en una localzacón determnada. Sn embargo, la poscón y orentacón del efector fnal no es el únco elemento a consderar, sno que tambén lo serán las velocdades con las que este se moverá hasta alcanzar esta localzacón. Esta tarea requere la coordnacón de la velocdad nstantánea del efector fnal (lneal y angular) y la de las artculacones (angular). En este apartado se procede a la obtencón de las relacones de velocdad, es decr, la relacón exstente entre las velocdades lneal y angular del efector fnal y las artculacones. Matemátcamente, las relacones de velocdad entre ambos espacos son determnadas medante el Jacobano de la funcón de la cnemátca drecta. El Jacobano de esta funcón va a ser una matrz, la cual es uno de los elementos más mportantes en el análss y control del movmento en robótca, ya que juega un papel muy mportante en muchos aspectos: se utlza para el cálculo y ejecucón de