ROBÓTICOS, SU CINEMÁTICA Y DINÁMICA

Transcripción

1 Insttuto Poltécnco Naconal ESCUELA SUPERIOR DE INGENIERÍA MECÁNICA Y ELÉCTRICA SECCIÓN DE ESTUDIOS DE POSGRADO E INVESTIGACIÓN METODOLOGÍA PARA GENERACIÓN DE TRAYECTORIAS DE MANIPULADORES ROBÓTICOS, SU CINEMÁTICA Y DINÁMICA T E S I S QUE PARA OBTENER EL GRADO DE MAESTRO EN CIENCIAS CON ESPECIALIDAD EN INGENIERÍA MECÁNICA P R E S E N T A : ING. EMMANUEL ALEJANDRO MERCHÁN CRUZ DIRIGIDA POR: DR. GUILLERMO URRIOLAGOITIA CALDERÓN MÉXICO, D.F.

2 La magnacón es más mportante que el conocmento Albert Ensten

3 a ms padres

4 Índce Índce... Índce de Tablas y Fguras... v Smbología....v Resumen.... x Abstract.... x Justfcacón...x Objetvo....x Introduccón...x. Estado del Arte.... Antecedentes Generales.... Antecedentes Específcos Fundamentos Teórcos Cnemátca de Manpuladores Robótcos Representacón de la Orentacón entre Sstemas de Coordenadas Matrces de Rotacón Otros Métodos para Representar la Orentacón de Sstemas de Coordenadas Matrces de Transformacón Homogénea Asgnacón de Sstemas de Coordenadas a Manpuladores Robótcos (Parametrzacón de Manpuladores Robótcos) Representacón de Denavt y Hartenberg Dnámca de Manpuladores Robótcos Formulacón de Básca Formulacón de Lagrange Dnámca Lagrangana de un Manpulador de dos grados de Lbertad Control de Manpuladores Robótcos... 4

5 .3. Control de la velocdad de motores de C.D Ley de control en lazo aberto de manpuladores robótcos Ley de control del Torque Calculado El robot de 5 Grados de Lbertad Mtsubsh Movemaster como Caso de Estudo Descrpcón del Manpulador Robótco Mtsubsh Sstemas de Coordenadas sobre los que se desplaza el robot Mtsubsh Movemaster Planteamento del problema cnemátco Obtencón de las ecuacones de dseño del Manpulador Planteamento de la Solucón a la Cnemátca Drecta e Inversa del Manpulador Cnemátca Drecta Cnemátca Inversa Planteamento del Problema Dnámco Obtencón del Modelo Dnámco del Manpulador Planteamento de la solucón a la Dnámca del Manpulador Generacón de Trayectoras Metodología para la generacón de trayectoras Parametrzacón de Trayectoras Cnemátca de la generacón de trayectoras Dnámca en la generacón de trayectoras Desarrollo Expermental Dervado de este Trabajo Proyecto de Implementacón de las Técncas de Control PID en un Manpulador Robótco de Tres Grados de Lbertad Descrpcón físca... 8

6 5.. Modelado Matemátco y Solucón a la Cnemátca Drecta e Inversa Implementacón de Técncas de Control para el robot R3GL Accón de control Proporconal Accón de control Integral Accón de control Proporconal-Integral Accón de control Proporconal-Dervatvo Accón de control Proporconal Integral Dervatvo Funcón de Transferenca de un Controlador PID Dgtal Descrpcón del Sstema de Control del Manpulador Robótco R3GL Estado actual del Proyecto de Investgacón... 7 CONCLUSIONES... 9 REFERENCIAS... 3 ANEXOS...A ANEXO. Programas en Matlab...A ANEXO. Lstado para el Modelado en Realdad Vrtual del Robot Mtsubsh Movemaster....A4 ANEXO 3. Gráfcas correspondentes a las poscones que conforman la trayectora abc...a4 ANEXO 4. Programas del proyecto de mplementacón....a3 ANEXO 5. Dagramas de conexón del manpulador robótco R3GL....A39 ANEXO 6. Publcacón generada de ésta tess...a4

7 Índce de Tablas y Fguras FIGURA. TELAR AUTOMÁTICO DE JACUARD... FIGURA. CONFIGURACIÓN CARTESIANA O PRISMÁTICA... 5 FIGURA.3 CONFIGURACIÓN CILÍNDRICA FIGURA.4 CONFIGURACIÓN ESFÉRICA O POLAR... 6 FIGURA.5 CONFIGURACIÓN DE BRAZO ARTICULADO FIGURA.6 CONFIGURACIÓN SCARA... 7 FIGURA.7 POSICIONAMIENTO Y ORIENTACIÓN DEL EFECTOR FINAL FIGURA.8 ORIENTACIÓN DEL EFECTOR FINAL... 8 FIGURA..- REPRESENTACIÓN DE SISTEMAS DE COORDENADAS... 7 FIGURA..- (A) ROTACIÓN ALREDEDOR DEL EJE X, GRADOS; (B) ROTACIÓN ALREDEDOR DEL EJE Y, ϕ GRADOS; (C) ROTACIÓN ALREDEDOR DEL EJE Z GRADOS FIGURA.3.- OBTENCIÓN DE LA MATRIZ DE ROTACIÓN RESULTANTE... FIGURA.4.- MATRIZ DE TRANSFORMACIÓN HOMOGÉNEA... 4 FIGURA.5.- MATRICES DE BÁSICAS DE ROTACIÓN HOMOGÉNEAS... 5 FIGURA.6.- MATRIZ DE TRASLACIÓN HOMOGÉNEA BÁSICA... 5 FIGURA.7 DIFERENTES CONVENCIONES PARA LA ASIGNACIÓN... 7 DE SISTEMAS DE COORDENADAS... 7 FIGURA.8 PARÁMETROS DE D-H FIGURA.9 MANIPULADOR DE DOS GRADOS DE LIBERTAD FIGURA. CAMBIOS EN LA ENTRADA DE UN SISTEMA DE CONTROL... 4 FIGURA. ERROR DE SEGUIMIENTO DEBIDO AL EFECTO DINÁMICO... 43

8 FIGURA. SISTEMA MASA-RESORTE-AMORTIGUADOR FIGURA.3 DIAGRAMA POLOS-CEROS A)POLOS SIMPLES; B) POLOS COMPLEJOS FIGURA.4 ESTRUCTURA BÁSICA DE CONTROL DE POSICIÓN... 5 FIGURA 3. ROBOT MITSUBISHI MOVEMASTER FIGURA 3. DIMENSIONES (EN MM.) DEL ROBOT MOVEMASTER FIGURA 3.3 SISTEMA MOVEMASTER FIGURA 3.4 GRADOS DE LIBERTAD DEL MOVEMASTER FIGURA 3.5 POSICIÓN INICIAL PARA LA ASIGNACIÓN DE SISTEMAS DE COORDENADAS FIGURA 3.6 SISTEMAS DE COORDENADAS PARA EL ROBOT TABLA 3. PARÁMETROS DE D-H FIGURA 3.7 DIAGRAMA DE FLUJO DE LA FUNCIÓN CDMOVER... 6 FIGURA 3.8 DIAGRAMA DE FLUJO DE LA FUNCIÓN NEWTON FIGURA 3.9 PUNTO r EN EL SISTEMA DE COORDENADAS DE LA ARTICULACIÓN FIGURA 3. DIAGRAMA DE FLUJO PARA LA FUNCIÓN NUM FIGURA 4. TRAYECTORIA DEL EFECTOR FINAL ENTRE DOS PUNTOS FIGURA 4. SISTEMAS DE COORDENADAS DEL ROBOT Y DE OBJETOS EN SU ENTORNO FIGURA 4.3 RUTA A SEGUIR POR EL MANIPULADOR FIGURA 4.4 DEFINICIÓN DE LAS LIGADURAS DE CAMINO FIGURA 4.5 ESTABLECIMIENTO DE LA ORIENTACIÓN DESEADA DURANTE LA TRAYECTORIA FIGURA 4.6 IDENTIFICACIÓN DE LAS ORIENTACIONES DESEADAS FIGURA 4.7 ANÁLISIS DE LA TRAYECTORIA PARCIAL BC... 84

9 FIGURA 4.8 ANÁLISIS DE VARIACIÓN ANGULAR EN LA ORIENTACIÓN FIGURA 4.9 ANÁLISIS CINEMÁTICO TRAYECTORIA ABC FIGURA 4. CONFIGURACIÓN RESULTANTE CORRESPONDIENTE AL PUNTO A FIGURA 4. CONFIGURACIÓN RESULTANTE CORRESPONDIENTE AL PUNTO A... 9 FIGURA 4. CONFIGURACIÓN RESULTANTE CORRESPONDIENTE AL PUNTO B FIGURA 4.3 CONFIGURACIÓN RESULTANTE CORRESPONDIENTE AL PUNTO B TABLA 4. VELOCIDADES ANGULARES (RAD/SEG) TABLA 4. ACELERACIONES ANGULARES (RAD/SEG^) FIGURA 4.4 VELOCIDADES ANGULARES EN LAS ARTICULACIONES (RAD/S) FIGURA 4.5 ACELERACIONES EN LAS ARTICULACIONES (RAD/S^) TABLA 4.3 TORQUES APLICADOS EN LAS ARTICULACIONES PARA GENERAR LA TRAYECTORIA ABC (N-M) FIGURA 4.6 TORQUE ARTICULACIÓN FIGURA 4.7 TORQUE ARTICULACIÓN FIGURA 4.8 TORQUE ARTICULACIÓN FIGURA 4.9 TORQUE ARTICULACIÓN FIGURA 4. TORQUE ARTICULACIÓN 5... TABLA TORQUES CONSIDERANDO UNA CARGA DE.5 K.G EN EL EFECTOR FINAL (N-M)... FIGURA 4. COMPARACIÓN T VS T... FIGURA 4. COMPARACIÓN T VS T... FIGURA 4.3 COMPARACIÓN T3 VS T FIGURA 4.4 COMPARACIÓN DE T4 VS T FIGURA 4.5 COMPARACIÓN T5 VS T5... 4

10 FIGURA 5. DIMENSIONES GENERALES (EN MM) DEL MANIPULADOR R3GL, VISTA LATERAL... 9 FIGURA 5. DIMENSIONES GENERALES (EN MM) DEL MANIPULADOR R3GL, VISTA SUPERIOR... FIGURA 5.3 DIMENSIONES GENERALES (EN MM) DEL MANIPULADOR R3GL, VISTA FRONTAL... FIGURA 5.4 POSICIÓN DE SISTEMA DE COORDENADAS CERO.... FIGURA 5.5 ASIGNACIÓN DE SISTEMAS DE COORDENADAS.... TABLA 5. PARAMETROS DE D-H PARA EL ROBOT R3GL... FIGURA 5.6 POSICIÓN INICIAL ( 9 )... 3 FIGURA 5.7 POSICIÓN (9 9 )... 3 FIGURA 5.8 POSICIÓN ( )... 4 FIGURA 5.9 POSICIÓN 3 ( )... 5 FIGURA 5. POSICIÓN 4 ( )... 6 FIGURA 5. POSICIÓN 5 ( )... 7 FIGURA 5. ANÁLISIS DEL COMPORTAMIENTO DEL ERROR E(T), Y COMPORTAMIENTO DE LA SEÑAL MANIPULADA U(T) PARA UN CONTROL PI... 9 FIGURA 5.3 ANÁLISIS DEL COMPORTAMIENTO DEL ERROR E(T), Y COMPORTAMIENTO DE LA SEÑAL MANIPULADA U(T) PARA UN CONTROL PD... 9 FIGURA 5.4 ANÁLISIS DEL COMPORTAMIENTO DEL ERROR E(T), Y COMPORTAMIENTO DE LA SEÑAL MANIPULADA U(T) PARA UN CONTROL PID... FIGURA 5.5 DIAGRAMA DE FLUJO PARA LA IMPLEMENTACIÓN DEL PID DIGITAL... FIGURA 5.6 DIAGRAMA DE FLUJO DE LAS SUBRUTINAS RECTSU Y RECTBA... 3 FIGURA 5.7 DIAGRAMA DE FLUJO DE LA SUBRUTINA PID... 4

11 FIGURA 5.8 DIAGRAMA ESQUEMÁTICO DEL SISTEMA DE CONTROL DEL MANIPULADOR ROBÓTICO R3GL... 6

12 Smbología. x y z x y z x n y n z n - sstema de coordenadas fjo - sstema de coordenadas de la base del manpulador - sstema de coordenadas de la base del manpulador u v w - sstema de coordenadas fjo R, [ R] - matrz de rotacón 3 x 3 T, [ T ] - matrz de transformacón homogénea 4 x 4 I 3 - matrz dentdad 3 x 3 I 4 - matrz dentdad 4 x 4 ˆ ˆj kˆ - vectores untaros α φ - desplazamentos angulares - ángulo entre eslabones a, d,α, - parámetros de Denavt y Hartenberg a xyz a uvw - vector de poscón respecto al sstema de coordenadas xyz - vector de poscón respecto al sstema de coordenadas uvw [ ] T A - matrz transpuesta Q, ( q, q ˆ) - cuaternon A - matrz de transformacón homogénea para los sstemas de coordenadas e. T n n F m r - matrz del robot - grados de lbertad - fuerza - masa - vector de poscón r&, v - vector de velocdad lneal r&, a - vector de aceleracón lneal

13 g j q τ L K P ϖ α J V I E d & d & d vv vc - restrccones del sstema - coordenada generalzada - fuerza generalzada - Lagrangano del manpulador - energía cnétca - energía potencal - velocdad angular - aceleracón angular - matrz Jacobana - voltaje - corrente eléctrca - fuerza electromotrz - poscón angular deseada - velocdad angular deseada - aceleracón angular deseada - vector varable - vector constante

14 Resumen. En este trabajo se presenta una metodología que permte obtener los parámetros necesaros para la generacón de trayectoras de manpuladores robótcos fuera de línea, como son: los vectores de poscón angular que resulten en la confguracón espacal del manpulador que se requere durante la trayectora deseada, y los vectores de fuerza o torque necesaros para que se cumplan las condcones dnámcas de movmento. Partendo de un análss cnemátco y dnámco del manpulador y de la tarea a desempeñar por este. Desarrollando tambén, las herramentas de cómputo necesaras para este fn. Se presenta tambén el trabajo expermental desarrollado, que contempla el dseño y construccón de un manpulador de 3 grados de lbertad y la mplementacón de técncas de control para este. En la versón en dsco compacto (en formato HTML), se ncluye la anmacón del manpulador generando la trayectora deseada, los lstados de los programas necesaros para la mplementacón del control PID (Proporconal Integral Dervatvo), el lstado para el modelado vrtual del manpulador y los lstados necesaros para la solucón a la Cnemátca y Dnámca en MATLab, para su uso posteror. Este trabajo esta dsponble en Internet en las dreccones electróncas y

15 Abstract. A methodology to obtan, offlne, the parameters for the trajectory generaton of robotc manpulators s presented n ths work. Parameters such as the angular poston vectors, that conform the desred trajectory n the Cartesan space, and the force vectors whch are requred to fulfll the dynamc condtons of movement are obtaned from knematc and dynamc analyss of the robot manpulator and, the task to be carred out by t. The computer tools for ths purpose are developed as well. It s presented, as well, the expermental work done so far, whch ncludes the desgn and constructon of a 3 dof robot manpulator, and the mplementaton of control technques for ts operaton. The CD verson (n HTML format) ncludes: the anmaton of the desred trajectory followed by the robot manpulator, the programs of mplementaton for the PID control technques for the robot, the program for vrtual modelng of the robot manpulator, and the programs to solve the Knematcs and Dynamcs wth MATlab, for ts further use. Ths work s also avalable n Internet at and

16 Justfcacón. Actualmente, uno de los prncpales problemas que presentan las ndustras que tenen mplementadas en su línea de produccón células de manufactura robótcas, son los denomnados tempos muertos dervados de la reprogramacón de un manpulador robótco. Esta, en el contexto de la ndustra naconal, es en su mayoría gestual y textual, lo que oblga a detener la línea de produccón por espaco de muchas horas, perodo en el cual, los técncos dentfcan en un modelo físco, los puntos que habrán de defnr la trayectora a segur por el robot, para posterormente programar la secuenca de movmentos del msmo. El estudo de la generacón de trayectoras de un manpulador robótco, es de gran utldad para la determnacón de los parámetros necesaros que defnan la trayectora a segur por el robot, sn necesdad de detener la línea de produccón, permtendo así realzar una programacón fuera de línea, reducendo así los tempos de reprogramacón de manera sgnfcatva. Objetvo. El objetvo del presente trabajo, es plantear una metodología de análss que permta la determnacón de los parámetros cnemátcos y dnámcos de control necesaros para la generacón de trayectoras en un manpulador robótco.

17 Introduccón. El estudo de la robótca comprende muchas áreas, por lo que se consdera completamente nterdscplnara, un profesonal que quera desarrollarse dentro de esta debe tener conocmentos en las áreas de ngenería eléctrca, ngenería mecánca, ngenería electrónca, ngenería de manufactura, y cencas de la computacón, con el fn de poder ver los problemas que representan el estudo de la robótca desde un punto de vsta ntegral. Hoy en día, la ndustra naconal cuenta con una planta de aproxmadamente, robots nstalados, de los cuales un 6% tenen que ver con la ndustra automotrz [5]. Se estma que, debdo a la demanda en los mercados, la planta de robots nstalados crezca con un rtmo del % anual [53]. Por lo anteror es necesaro contar con el recurso humano que posea el nvel técnco necesaro para poder llevar a cabo la ntegracón e mplementacón de esta tecnología en la planta manufacturera naconal. Un aspecto de gran nterés es el que representa la generacón de trayectoras de manpuladores robótcos para el desarrollo de una tarea, la cual comprende la defncón de los puntos que habrán de conformar dcha trayectora. La manera menos efcente de defnr estos puntos y que es como actualmente se hace en la ndustra naconal es manpulando el robot para llevarlo manualmente a la poscón y orentacón deseada para su efector fnal, ya que esto representa el tener que detener la línea de produccón, de la cual forma parte el robot, para que un grupo de técncos realce esta nueva defncón de puntos de una trayectora, lo que se traduce en tempo en el que el robot no es productvo, además de que la defncón de los puntos de esta manera no son del todo precsos.

18 Un segundo método para defnr los puntos que conforman a una trayectora es en el que, para defnrlos, solo es necesaro contar con nformacón básca del manpulador robótco, como lo son sus dmensones y grados de lbertad. Obtenendo así la nformacón necesara para generar una trayectora fuera de línea, es decr, sn nterrumpr la línea de produccón para poder hacerlo. La generacón de trayectoras es, sn duda, uno de los prncpales problemas de análss en el área de la robótca. Esta ncluye el estudo de la cnemátca y dnámca del manpulador, el control del sstema y la parametrzacón de la tarea a desempeñar para defnr la trayectora a segur por el manpulador. El objetvo de este trabajo es el de plantear una metodología de análss para la generacón de trayectoras de manpuladores robótcos, tomando en cuenta la cnemátca y dnámca que tene que ver con el manpulador y la tarea a desempeñar. Para lo cual esta tess se dvde de la sguente manera: En el prmer capítulo, Estado del Arte, se exponen los antecedentes generales de la robótca, desde su prmera concepcón hasta la descrpcón de los manpuladores robótcos actuales. Así como tambén los antecedentes específcos que tenen que ver con el estudo de la robótca en sus dversos aspectos, como lo son la cnemátca, la dnámca, el control, entre otros. En el segundo capítulo, Fundamentos Teórcos, se presentan los fundamentos necesaros para realzar un estudo cnemátco y dnámco de un manpulador robótco. En este capítulo tambén se ncluyen aspectos báscos de control de manpuladores robótcos, pues es, en la opnón del que escrbe, necesaro establecer desde un prncpo, la relacón de un estudo dnámco de un sstema físco con su modelo de control correspondente y no tratarlo como dos temas ndependentes.

19 En el tercer capítulo, El manpulador robótco de 5 grados de Lbertad Mtsubsh Movemaster como Caso de Estudo, se toma el manpulador menconado para ejemplfcar el establecmento de los parámetros necesaros para realzar un estudo cnemátco y dnámco de un robot. En este capítulo, se plantean los problemas cnemátco y dnámco con el objeto de desarrollar las herramentas de cómputo necesaras para su solucón y posteror aplcacón en el estudo de trayectoras. Es mportante menconar que se utlzó el robot Mtsubsh Movemaster como espécmen de estudo, ya que el IPN cuenta con tres manpuladores de este tpo en las escuelas de nvel superor ESIME Azcapotzalco, ESIME Culhuacán, y UPIICSA, por lo que el acceso a estos equpos es relatvamente sencllo. En el cuarto capítulo, Generacón de Trayectoras, se presenta la metodología para el estudo de la generacón de trayectoras. Para la lustracón de esta metodología se propone una tarea a desempeñar por el manpulador, para la cual se llevan a cabo los sguentes pasos: parametrzacón de la trayectora y análss cnemátco y dnámco de la trayectora. En este capítulo se obtenen los vectores de poscones, velocdades y aceleracones angulares que corresponden a la trayectora deseada a partr del análss cnemátco y los vectores de torque necesaros en las artculacones, para satsfacer las condcones dnámcas de la trayectora a partr del análss dnámco. En el qunto capítulo, Desarrollo Expermental Dervado de este Trabajo, se presentan los dversos proyectos que se han realzado a lo largo del desarrollo de esta tess. De los cuales, se descrbe de manera general el proyecto que más ha avanzado y que lleva como título Proyecto de mplementacón de técncas de control PID en un manpulador robótco de 3 grados de lbertad y que comprende el dseño y construccón del manpulador, el modelado matemátco y solucón a la cnemátca drecta e nversa para este manpulador, sguendo la metodología que se presenta en esta tess y con las herramentas que se han desarrollado en Matlab. Por últmo, la mplementacón de técncas de control PID (Proporconal

20 Integral Dervatvo) en el manpulador robótco utlzando mcro controladores MC68HC de Motorola Por últmo se presentan las conclusones a las que se han llegado como resultado del desarrollo de este trabajo. Además de los anexos en los que se ncluyen los lstados de los programas en Matlab, el lstado para el modelado en realdad vrtual del manpulador, la representacón gráfca de poscones que conforman la trayectora de estudo, los programas de mplementacón del algortmo de control PID en un manpulador de 3 grados de lbertad, los dagramas de conexón del manpulador R3GL y las publcacones generadas de este trabajo. Es de gran mportanca el agradecer los apoyos recbdos por CONACYT y por el IPN a través del Programa Insttuconal de Formacón de Investgadores, pues sn este, hubese sdo muy dfícl el desarrollo de este trabajo, así como la construccón del prototpo de manpulador robótco, cuyos costos se han cuberto con los recursos generados por las becas de ambas nsttucones.

21 Estado del Arte E n é s t e c a p

22 . Estado del Arte. Antecedentes Generales Cuando hablamos de robótca, nevtablemente pensamos en un proceso automátco, y como la robótca es hoy en día snónmo de automatzacón, nos referremos a Joseph Jacquard como el prmer ponero de la automatzacón ndustral que, en el año de 8, nventó una máquna textl operada por tarjetas perforadas conocda como telar programable, dando lugar a la produccón en masa [,3]. Fgura. Telar automátco de Jacuard. No es sno hasta más de un sglo después, en el año de 9, que el térmno robot, dervado del vocablo checo robota, cuyo sgnfcado es srvente o esclavo, es utlzado por prmera vez por el Doctor en flosofía Karel Capek en una obra de teatro en la que descrbe srventes mecáncos llamados robots []. Para el año de 938, los amercanos Wllard Pollard y Harold Roselund dseñan un mecansmo programable para el pntado por aspersón para la compañía DeVlbss [3].

23 George Devol, en el año de 946, dseñó una máquna programable capaz de almacenar y reprogramar nformacón [,4]. En el msmo año, los centífcos amercanos J. Presper Eckert y John Mauchly construyen la prmera computadora dgtal llamada ENIAC (Electronc Numercal Integrator and Computer) en la Unversdad de Penslvana [,5]. En el M.I.T., una segunda computadora llamada UNIVAC I (Unversal Automatc Computer) resuelve por prmera vez un problema matemátco [5,6]. Norbert Wener, profesor del M.I.T., en 948 publca el lbro Cbernétca, en el cual descrbe conceptos de comuncacones y control en sstemas electróncos, mecáncos y bológcos [,5]. En el año de 95, Raymond Goertz dseña un brazo artculado el cual es teleoperado para la manpulacón de materales radoactvos, a petcón de la Comsón de Energía Atómca [,5]. George Devol, en el año de 954, regstra bajo la patente número,988,37 de EUA al prmer robot []. Este estaba dseñado para la transferenca de materales en una fabrca [5]. Más adelante, en el año 956, Joseph Engelberger y Devol conforman la que a la postre sería una de las más extosas compañías de robots, Unmaton, Inc. [,4,5], construyendo el prmer robot Unmaton en 958. Pero no fue sno hasta el año de 96 cuando se nstaló por prmera vez un robot ndustral en una línea de produccón en una armadora propedad de General Motors [], cuya tarea era el tomar pezas calentes de una forja y empaquetarlas [5]. En Japón, Kawasak Heavy Industres mportó su prmer robot en el año de 967 [5], obtenendo la lcenca para la manufactura de robots Unmaton en 968 [,4]. En el año de 97, la Unversdad de Stanford dseña un brazo robot que se converte en el estándar en proyectos de nvestgacón []. Este brazo utlza

24 motores eléctrcos como actuadores y se le conoce como robot Stanford [5]. Al año sguente, 97, en Japón se conforma la prmera asocacón de robótca en el mundo, la Asocacón de Robots Industrales de Japón (JIRA) []. En 973, se utlza por prmera vez una mcrocomputadora para controlar un brazo robótco, desarrollado por Rchard Horn para Cncnat Mlacron Corporaton [,5]. En el año de 974, el creador del robot Stanford, Profesor Shenman, funda Vcarm Inc., con el propósto de comercalzar una versón para aplcacones ndustrales del robot Stanford orgnal []. La Asocacón Industras Robótcas es establecda en 975 [], dando, por prmera vez, una defncón para robot ndustral: Un robot ndustral es un manpulador multfunconal programable, dseñado para mover, materales, herramentas o dspostvos especalzados; por medo de movmentos programados para la realzacón de varas tareas [,7]. En 977, ASEA (ahora ABB), compañía europea, ofrece comercalmente dos modelos de robots ndustrales con actuadores eléctrcos conocdos como Irb6. Ambos utlzan mcrocomputadoras como controlador para su programacón y operacón [,8]. Durante el msmo año, Vcarm Inc. es adqurdo por Unmaton Co. []. Como resultado de esta fusón, uno de los mayores éxtos de Unmaton se obtene el robot PUMA (Programable Unversal Machne for Assembly). Por otra parte, el ngenero mecánco estadoundense Vctor Schenman, cuando estudaba la carrera en la Unversdad de Stanford, en Calforna, desarrolló este manpulador polvalente realmente flexble conocdo. El PUMA era capaz de mover un objeto y colocarlo con cualquer orentacón en un lugar deseado que estuvera a su alcance []. El concepto básco multartculado del PUMA es la base de la mayoría de los robots actuales, es dseñado utlzando las técncas de control de Vcarm y bajo patrocno de General Motors [,5].

25 FANUC Robotcs, compañía japonesa, es ntroducda al mercado amercano en 98, sendo General Motors el prncpal acconsta []. En este msmo año, un nuevo concepto en robots de ensamble es trado desde Japón, conocdo como robot SCARA (Selectve Complance Assembly Robot Arm) [8]. Para 984, se desarrollan robots con actuadores acoplados de manera drecta en las artculacones (drect-drve) por Adept Corporaton, elmnando el uso de engranajes o cadenas ntermedas []. En el msmo año, ASEA presenta una nueva confguracón de robot con 6 grados de lbertad tpo péndulo, para tareas de ensamble [8]. En 986, la nstalacón y aplcacón de robots contnua crecendo pero desde un punto de vsta ntegral, aumentando el nterés en Celdas de Manufactura, Sstemas Flexbles de Manufactura y sstemas CIM []. Durante este tempo, los tpos de confguracones báscas de robots que se han establecdo son las sguentes [9]: - - Cartesano: con capacdad de movmento a lo largo del los ejes X, Y y Z. Fgura. Confguracón Cartesana o prsmátca

26 - - Clíndrco: Con movmento a lo largo de Z y Y, alrededor de Z. Fgura.3 Confguracón Clíndrca. - - Esférco o polar: con dos artculacones de revolucón y una prsmátca. Fgura.4 Confguracón Esférca o Polar.

27 - - Brazo artculado o de revolucón: úncamente conformado por artculacones de revolucón. Fgura.5 Confguracón de Brazo Artculado. - - SCARA: Con dos pares de revolucón y uno prsmátco, pero dspuestos en una confguracón dferente a la de la confguracón esférca. Fgura.6 Confguracón SCARA Todas estas confguracones, en un prncpo, consderan úncamente tres grados de lbertad en su representacón, que permten que el manpulador poscone su efector fnal en cualquer punto dentro de su espaco de trabajo, en tanto que grados de lbertad adconales se utlzan para darle la orentacón deseada o

28 necesara, de acuerdo con la tarea a desempeñar, como se muestra en la fgura.7. Fgura.7 Posconamento y orentacón del efector fnal. Fgura.8 Orentacón del efector fnal.

29 . Antecedentes Específcos. Debdo a que la robótca es nterdscplnara, en la que ntervenen la ngenería mecánca, eléctrca, electrónca, cencas de la computacón y manufactura, se han conformado dversas líneas de nvestgacón bajo esta área. En un panorama global, esta líneas comprenden: el dseño de los mecansmos que conforman el manpulador, consderacón de eslabones rígdos y flexbles, confguracones de manpuladores de cadena aberta y manpuladores paralelos; desarrollo de nuevas técncas de control de manpuladores robótcos, control adaptvo, dfuso, etc; desarrollo de dversos algortmos y técncas de programacón en línea y fuera de línea, dentro de este panorama global pueden dentfcarse los algortmos de vsón artfcal y algortmos para robots cooperatvos. Como ejemplo de lo anteror podemos ctar los sguentes casos: En el área de ngenería mecánca hoy en día no solo se analzan robots de cadena aberta, sno tambén robots paralelos, en los cuales el prncpal problema es determnar el modelo cnemátco que rge el movmento del sstema, concentrándose el análss en la determnacón de este modelo. En la Unversdad de Vctora, Carretero, Nahon y otros trabajan, en 997, en el análss cnemátco de un mecansmo de manpulador paralelo de 3 grados de lbertad, para su uso en foco de un lente de telescopo. Proponen la solucón drecta e nversa de la cnemátca del manpulador []. Para 998, Podohorosdesk presenta el estudo de optmzacón en la arqutectura de este mecansmo []. En tanto que en la Unversdad de Queen s, actualmente se trabaja en el dseño de nuevas confguracones de robots paralelos bajo la supervsón de la Profesora Lela Notash [3]. En lo que respecta a los manpuladores artculados de cadena aberta, actualmente se desarrollan dversas metodologías para la solucón de la

30 cnemátca de dchos robots, o ben el planteamento del modelo cnemátco de este, sendo común encontrar trabajos como el desarrollado por Doty, 99, para la NASA, en el cual se establece la solucón a la cnemátca drecta e nversa de un manpulador desarrollado para esta agenca [4]. Para la solucón a la cnemátca drecta de manpuladores robótcos, se emplea un método sstemátco y generalzado que utlza el álgebra matrcal como herramenta, este método descrbe la geometría espacal de los elementos de un brazo manpulador con respecto a un sstema de referenca fjo, método propuesto para el análss de mecansmos por Denavt y Hartenberg en 955 [5] que se aplca a la robótca. En lo referente a la solucón de la cnemátca nversa (CI), se reconocen, en dversos textos, dos estrategas para la solucón de esta, las solucones de forma cerrada y las solucones numércas. En un prncpo se optaba por las solucones cerradas dervadas de un análss del problema algebraco o geométrco [5,6], ya que las herramentas de cómputo dsponbles no contaban con la velocdad sufcente para resolverlos en un tempo convenente, con esto, se buscaba smplfcar la solucón del problema, pues las solucones obtendas, por la aplcacón de este método no eran sempre la mejor de las solucones, y además, consderando la naturaleza del problema que se derva del análss de un mecansmo eslabonado de cadena aberta, es evdente que pueden exstr múltples solucones para una condcón deseada. Parkn [], señala que la solucón a la CI no se puede obtener de manera algebraca, lo que sgnfca que una solucón de forma cerrada no exste y que es necesara una solucón teratva que, debdo a la naturaleza del problema mplca que pueden haber varas solucones para un msmo punto.

31 Al respecto, se han desarrollado varos trabajos, por ejemplo, Doty y Shwartz [36], señalan que un par de métodos para la solucón a la CI conssten en utlzar métodos numércos para resolverla calculando el error cuadrátco mínmo, o ben, agregar restrccones algebracas en la solucón a la CI para manpuladores redundantes. Posterormente demuestran la aplcacón del método de Moore- Penrose en la solucón a esta y proponen un método llamado Teoría Inversa Generalzada, que se basa en la generalzacón del método antes menconado [37] y más adelante presentan la aplcacón de la metodología propuesta para la solucón a la CI en velocdades [38]. Trabajos más avanzados en cuanto a propuestas de solucón a la CI, tenen que ver con la aplcacón de la lógca dfusa para la solucón a esta. Tal como lo presentan Xu y Nechyba en [39], bajo el argumento de que la solucón a la CI requere de muchos recursos computaconales, lo que puede resultar en retrasos sgnfcatvos en el control en tempo real y, basándose en el hecho de que los humanos no calculan de manera exacta la CI para realzar sus movmentos, sn embargo, podemos, en todo momento, posconar en el espaco cualquera de nuestras extremdades u objeto manpulado, de manera precsa a partr de un razonamento dfuso. En el área de los mecansmos, actualmente se emplean los números duales para el análss y síntess de mecansmos, tal como lo descrbe Cheng en [4] y [43]; llevando este análss hasta los mecansmos de cadena aberta de manpuladores robótcos, a partr de los parámetros de Denavt y Hartenberg [4]. En lo que se refere a la dnámca de manpuladores robótcos, tambén se plantea el uso de números duales para descrbr la dnámca del cuerpo rígdo [45]. O ben, el uso de multplcadores de Lagrange en sstemas de smulacón de la dnámca de manpuladores, utlzando estos para establecer las restrccones en el modelo a smular [8]. Algunos análss de la dnámca nversa de manpuladores

32 comercales de 6 grados de lbertad, con el fn de comparar dversos recursos computaconales se presentan en [46] por Cheng y Gupta En el Insttuto de Robótca de Carnege-Mellon se desarrollan técncas de control adaptvo de manpuladores, así como el uso de b-cuaternones para establecer los algortmos de control de robots cooperatvos [8], además de algortmos de control para la determnacón de la fuerza de aprete óptma en mecansmos de aprensón de manpuladores robótcos [48]. En el Insttuto Tecnológco de Calforna trabajan en el dseño de un control adaptvo para sstemas físcos con dnámca cambante [49] y en análss de la dnámca de robots móvles para el establecmento de un control óptmo [5]. En la Unversdad de Ámsterdam, trabajan en aspectos de control como el dseño de un modelo de control para el uso de sstemas de vsón artfcal, como guía para el movmento de un brazo robot usando redes neuronales. En el ámbto naconal, la robótca no ha crecdo aún de manera notable, ya que son pocos los centros educatvos en donde se aborda este tema con fnes de nvestgacón y desarrollo, destacándose el CINVESTAV, el IPN, la UNAM y el ITESM. Es mportante hacer notar que, debdo a la mportanca de este tema, en el año de 99, en ESIME Azcapotzalco, se crea la carrera de Ingenería en Robótca Industral, cuyo enfoque, lamentablemente, es una automatzacón empírca de procesos ndustrales, sn conocer los fundamentos báscos de la manpulacón robótca, n los aspectos báscos de control de procesos. Dentro de la currícula de dversas carreras en ngenería del IPN, se mparten materas relaconadas con la robótca, tal es el caso de la carrera de Ingenería en Control y Automatzacón de la ESIME Zacatenco y de Ingenería en Mecatrónca de la UPITA.

33 En estos dversos centros, se llevan a cabo actvdades de nvestgacón y desarrollo sobre estas tecnologías a nvel posgrado, prncpalmente en el CINVESTAV, ITESM y SEPI-ESIME, no sempre llegando a resultados satsfactoros, tal es el caso del proyecto MIRH de la SEPI-ESIME-IPN que, a cerca de tres años de ncado, no ha llegado a resultados tangbles. En el CINVESTAV han dseñado un robot tpo SCARA, proyecto que ha evoluconado de manera notable. Dentro de esta Seccón de Estudos de Posgrado e Investgacón, se han desarrollado pocos trabajos referentes al tema, como la propuesta de una Metodología para el Dseño de un Robot Manpulador Industral en 993 por Díaz de León [54]. En el msmo año, López [55] presenta el Análss Cnemátco y Dnámco de un Manpulador Robótco con tres Grados de Lbertad. Después de este año, es hasta 997 cuando Rojas Garnca [56] presenta el Dseño de un Mecansmo Atornllador para un Brazo Manpulador Robótco, trabajo que no aporta conocmento a la línea de nvestgacón de robótca. Los dos trabajos más recentes sobre el tema son, en 997, el Desarrollo de un Smulador para el Control de un Brazo Robótco, por González Sánchez [57], en donde se plantea el desarrollo de un smulador para el control, sn defnr los conceptos de dnámca drecta y dnámca nversa, aun cuando la dnámca drecta es la que tene que ver con el dseño de smuladores dnámcos, tal como lo señalan Crag [7], Baraff [8] y Murray [9]. Además, se mencona como objetvo el control de un brazo robótco, cuando, dentro del texto no se hace un solo planteamento n consderacón de las técncas n modelos de control que se emplean en manpuladores robótcos, lo cual es señalado amplamente en dversos textos como [9], [5], [7], [9] y []. El menconado trabajo presenta tambén errores en la obtencón del modelo matemátco del manpulador analzado. Por lo que no se recomenda la lectura de este trabajo.

34 El segundo trabajo más recente dentro de la seccón, es la Modelacón y Smulacón de Sstemas Electromecáncos Bajo Control, por Domínguez Agurre en 998 [], en donde se hacen consderacones mportantes en cuanto al modelado de sstemas físcos para su análss. Como se puede observar, en 7 años, en el departamento de Ingenería Mecánca de la SEPI-ESIME se han desarrollado un total de 6 trabajos, ncluyendo el presente, relaconados con la robótca. Lo que no corresponde con la demanda de recursos humanos, con la preparacón necesara, que demanda la ndustra naconal.

35 Fundamentos Teórcos E n e s t e c a p ít

36 . Fundamentos Teórcos.. Cnemátca de Manpuladores Robótcos. La cnemátca es el estudo del movmento sn tomar en cuenta las fuerzas que lo producen; esta estuda la poscón, la velocdad y la aceleracón del sstema []. De esta forma, la cnemátca de un manpulador robótco descrbe la relacón entre el movmento de las artculacones del manpulador y el movmento resultante de los cuerpos rígdos que conforman al robot [9]. Esta comprende dos problemas: El prmero consste en determnar la poscón y orentacón del efector fnal del manpulador, dervada de un cambo en la confguracón del sstema de eslabones que conforman la cadena cnemátca de este (Cnemátca Drecta) y el segundo problema consste en defnr los valores de las poscones de las artculacones, para que la confguracón del sstema resulte en una orentacón y poscón deseadas del efector fnal, dentro de su espaco de trabajo (Cnemátca Inversa). Para plantear y dar solucón al modelo cnemátco del sstema, es necesaro establecer una relacón geométrca entre los elementos que conforman la cadena cnemátca. Para lo cual, se propone la asgnacón de sstemas de coordenadas que srvan como referenca para poder establecer dcha relacón. Es entonces necesaro representar, medante el uso de expresones matemátcas, la orentacón y poscón relatva entre sstemas de coordenadas asgnados a los elementos del manpulador... Representacón de la Orentacón entre Sstemas de Coordenadas. Para poder representar la orentacón de un sstema de coordenadas móvl, con respecto a otro sstema de referenca, se emplean matrces de rotacón.

37 Báscamente, estas representan las proyeccones de los vectores untaros de un sstema móvl, en un sstema fjo.... Matrces de Rotacón. La matrz de rotacón en su forma general es la sguente: w z v z u z w y v y u y w x v x u x k k j k k k j j j j k j R ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ Entonces, s tenemos dos sstemas de coordenadas que concden en su orgen y su orentacón, tal como se ve en la fgura., la matrz de rotacón correspondente que represente la orentacón del sstema de coordenadas OUVW, respecto del sstema de coordenadas OXYZ, es: 3 R I O X,U Y, V Z,W u x ˆ, ˆ j v y j ˆ, ˆ z k w k ˆ, ˆ...(.)...(.)

38 Fgura..- Representacón de sstemas de coordenadas. Lo cual es fácl de comprobar, ya que debdo a que ambos sstemas son ortonormales y concden en su orentacón, los vectores untaros î x e î u son vectores paralelos, para los que, por defncón, el producto punto entre ellos es ; mentras que î u es normal a ĵy lo anteror para las demás relacones. y kˆ z, cuyo producto punto es cero. Cumpléndose En segunda nstanca tenemos el sstema de coordenadas móvl OUVW, que puede ser rotado alrededor de cualquera de los ejes del sstema de coordenadas OXYZ, como se ve en la fgura.. w k w ˆ x, u ˆ Z O j v v Y w Z O ˆj y, ˆj Y, V kˆ Z,W z, kˆ w O v Y X,U X u u (a) (b) (c) X Fgura..- (a) Rotacón alrededor del eje X, grados; (b) rotacón alrededor del eje Y, ϕ grados; (c) rotacón alrededor del eje Z grados. Entonces, para determnar la matrz de rotacón que represente la orentacón para el caso que lustra la fgura.(a), partremos del hecho que î u es paralelo a î x y tenen el msmo sentdo, por lo que, sendo sstemas ortonormales, î u no tene proyeccón en los vectores untaros normales a î x, pero ambos tenen proyeccón en ĵy y kˆ z ; en tanto que, ĵ v y kˆ w ĵy sguen sendo y kˆ z. La matrz de rotacón

39 R 3, que representa la orentacón resultante de una rotacón respecto al eje X, α grados es: R x, α cosα senα senα cosα... (.3) hacendo ahora un análss smlar para los casos de las fguras.(b) y (c), obtenemos: R y, ϕ cosϕ senϕ senϕ cosϕ... (.4) R z, cos sen sen cos... (.5) Supóngase ahora que es necesaro conocer el nuevo vector de poscón de un punto cuyas coordenadas están descrtas en el sstema de coordenadas móvl OUVW y el punto es a uvw, con respecto al sstema de coordenadas de referenca OXYZ. Una vez que el sstema móvl ha sdo afectado por una rotacón alrededor del eje Y, por ejemplo 5. El nuevo vector pertenecente al punto a xyz, estaría dado por: s a uvw [3,,5] T a xyz estará dado por: a xyz [ R ] [a uvw ]

40 a xyz cos5 sen5 sen cos En un sstema real, la orentacón fnal de un sstema de coordenadas móvl no estará dada por una smple rotacón alrededor de alguno de los ejes del sstema de referenca, s no que será defnda como resultado de una secuenca de rotacones no solo respecto de los ejes XYZ del sstema de referenca. Tambén estará en funcón de rotacones alrededor de los ejes UVW del sstema móvl, de manera tal que se obtene una matrz resultante. La cual estará dada de acuerdo a la sguente convencón, descrta en [9],[5],[9]: Partendo del hecho que, en un prncpo ambos sstemas de coordenadas concden en el orgen y en su orentacón, condcón que está descrta por una matrz I 3, a partr de esta y de acuerdo a la secuenca de rotacones que afecten al sstema, habrán de ordenarse de manera tal que: las rotacones respecto a XYZ se escrbrán a la zquerda de la matrz I 3, en tanto que las rotacones respecto a UVW, se escrben a la derecha de esta, obtenéndose así la secuenca correcta de multplcacón para la obtencón de la matrz resultante, tal como se lustra en la fgura.3. R Rotacones y/o traslacones respecto a XYZ I 3 Rotacones y/o traslacones respecto a UVW Fgura.3.- Obtencón de la matrz de rotacón resultante. Para lustrar el procedmento anteror, habrá de obtenerse enseguda la matrz de rotacón resultante para la sguente secuenca de rotacones: R x, 3 ; R y, 45 ; R w, ; R u,- 35 ; estaría dada por:

41 R R y, 45 R x, 3 I 3 R w, R v,- 35 c45 s45 s45 c45 c 3 s3 s3 c3 c s s c c( 35) s( 35) s( 35) c( 35) como multplcar por I3 es multplcar por, entonces: c45 s45 s45 c45 c3 s3 s3 c3 c s s c c( 35) s( 35) s( 35) c( 35) R Puede ahora calcularse tambén el nuevo punto a xyz para el punto dado a uvw, de la manera en que se explcó anterormente: a xyz Ahora ben, s es necesaro calcular el vector de poscón de un punto dado en coordenadas para XYZ con respecto al sstema móvl UVW a partr de la solucón encontrada, puede hacerse de la sguente manera: b uvw T [ R] [ b ] xyz

42 b s xyz es [5,3,] T, el nuevo punto buvw estará dado por: b uvw T Otros Métodos para Representar la Orentacón de Sstemas de Coordenadas. Otra forma popular de especfcar la orentacón entre sstemas de coordenadas, es hacendo uso de los ángulos de Euler. Estos descrben cualquer posble orentacón en térmnos de una rotacón con respecto del eje Z, seguda por una rotacón respecto al nuevo eje rotado Y y por últmo, seguda por una rotacón respecto al nuevo eje rotado Z [9]. Representando lo anteror como: Euler(φ,,ψ) R z,φ R y, R z,ψ La orentacón entre sstemas de coordenadas puede defnrse tambén hacendo uso de cuaternones. Los cuaternones generalzan a los números complejos y pueden ser utlzados para representar rotacones, de la msma manera en que los números complejos, en el crculo untaro, se usan para representar rotacones planas [9], [9] y [3]. Formalmente un cuaternon es un número hpercomplejo de la sguente forma [4], [8], [9], [3]: Q q q ˆ q ˆj q kˆ (.6)

43 donde: q componente escalar de Q, y qˆ ( q, q, q3 ) componente vectoral por lo que es común que Q se escrba tambén como: ( q q) Q, ˆ...(.7) De aquí podemos antcpar que, al momento de realzar operacones con cuaternones, podemos estar manpulando operadores de traslacón y de rotacón que pudesen afectar al sstema móvl. Para mayor nformacón al respecto del tema, se recomenda la lectura de [4],[5],[6] y [7]... Matrces de Transformacón Homogénea. Para poder descrbr completamente la relacón espacal entre sstemas de coordenadas, es necesaro nclur tambén dentro de la representacón, un componente que relacone el vector de poscón entre los orígenes de los sstemas de coordenadas, ya que es común que el sstema móvl se encuentre descentrado respecto al sstema de referenca. Como una matrz de rotacón 3 x 3 no nos da nnguna posbldad para nclur la traslacón y el escalado, se ntroduce un cuarto componente al vector de poscón ( p, p p ) T P, x y ( wp, wp, wp w) T P ˆ, x y z en un espaco trdmensonal, que lo transforma en z, entonces decmos que el vector Pˆ está representado en

44 coordenadas homogéneas, pues se le ha ncludo un componente de escala w [9],[9]. El concepto de una representacón homogénea en un espaco eucldano trdmensonal, es útl para desarrollar transformacones matrcales que ncluyen: rotacón, traslacón, escalado y perspectva [5]. Entonces, una matrz de transformacón homogénea, puede consderarse como que está conformada por cuatro submatrces, como se lustra el la fgura.4: T R P 3x3 x3 P E 3x x Transformacón Matrz de Rotacón de Perspectva Vector de Poscón Escala Fgura.4.- Matrz de transformacón homogénea. en donde: La matrz de rotacón representa la orentacón del sstema de coordenadas del sstema móvl, con respecto del sstema de referenca. El vector de poscón representa el descentramento entre los orígenes de los sstemas de coordenadas. La matrz de perspectva se refere a la perspectva de análss, que para el caso de la robótca es sempre [ ]. El factor de escala, que nos ndca el escalado de la magntud del vector de poscón, que para el caso de la robótca es [].

45 Por lo que podemos escrbr ahora, la matrz de transformacón homogénea en su forma general: (.8) ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ Λ Λ Λ Λ Λ z w z v z u z y w y v y u y x w x v x u x p k k j k k p k j j j j p k j T de donde se pueden dervar fáclmente las matrces báscas de rotacón homogénea que se lustran en la fgura.5., α α α α α c s s c T x, φ φ φ φ φ c s s c T y, φ φ φ c s s c T z Fgura.5.- Matrces de Báscas de Rotacón Homogéneas Así como tambén la matrz de traslacón homogénea básca, la cual tene el efecto de trasladar el sstema de coordenadas del sstema OUVW, a lo largo de los ejes del sstema de coordenadas OXYZ, a ben a lo largo de sus propos ejes, según sea el caso. ó w v u z y x T tr Fgura.6.- Matrz de Traslacón Homogénea Básca. Generalzando ahora lo antes dcho para el caso de las matrces de rotacón, una matrz de transformacón homogénea resultante no estará dada úncamente por operacones untaras de rotacón y/o traslacón, sno por una secuenca de estas, por lo que, para obtener la matrz de transformacón homogénea resultante,

46 aplcaremos la msma metodología que para el caso de matrces de rotacón R 3. De manera tal que: T Rotacones y/o traslacones respecto a XYZ I 4 Rotacones y/o traslacones respecto a UVW Tomando ahora como base a I4, ya que representa la condcón ncal en la que ambos sstemas de coordenadas, móvl y referenca, concden en su poscón y orentacón...3 Asgnacón de Sstemas de Coordenadas a Manpuladores Robótcos (Parametrzacón de Manpuladores Robótcos) Para poder aplcar las herramentas antes descrtas en el estudo de manpuladores robótcos, es necesaro asgnar sstemas de coordenadas a los componentes báscos de un robot,.e. eslabones y artculacones. Esto es con la fnaldad de descrbr la relacón espacal entre estos elementos, por medo de la obtencón de una matrz de transformacón homogénea que relacone al sstema de coordenadas asgnado al efector fnal, con el sstema de coordenadas de referenca fjo en la base del manpulador; matrz comúnmente conocda como, matrz de robot. La localzacón de los sstemas de coordenadas que permtan la obtencón de parámetros cnemátcos de un manpulador serán en: la base del robot, las artculacones y el efector fnal. Con el propósto de que se establecera una metodología estándar para la asgnacón de sstemas de coordenadas, y por consguente, en la obtencón de las matrces de transformacón homogénea que descrben la cnemátca del manpulador, se ha aceptado la representacón de Denavt y Hartenberg como lo establecen [9], [4], [5] y [9].

47 ..4 Representacón de Denavt y Hartenberg. Para poder descrbr la relacón traslaconal y rotaconal entre los elementos de una cadena cnemátca, Denavt y Hartenberg propuseron, en 955, un método matrcal para establecer de forma sstemátca un sstema de coordenadas lgado al cuerpo para cada elemento de una cadena artculada [5]. La representacón de Denavt y Hartenberg ha dado lugar a dos metodologías para la asgnacón de los sstemas de coordenadas; cada una de ellas permte certas lbertades en el establecmento de estos sstemas de coordenadas [6], []. Estándar Modfcada Fgura.7 Dferentes convencones para la asgnacón de sstemas de coordenadas En la prmera, el sstema de coordenadas tene su orgen a lo largo del eje de la artculacón +, como lo descrbe Lee [5]. En la segunda, el sstema de coordenadas tene su orgen a lo largo de la artculacón. Esta segunda

48 convencón es frecuentemente referda como convencón modfcada de Denavt y Hartenberg, como lo descrbe Crag [7]. La fgura.7 muestra la dferenca entre ambas convencones. La convencón que a lo largo de este trabajo se utlza corresponde a aquella descrta por Lee [5], que es la que se emplea de manera común [9], [4], [9], []. Y que llevada al área de manpuladores robótcos es como sgue [6]:.. Se lleva al manpulador a una poscón ncal, que servrá de referenca para medr los desplazamentos del sstema... Se numeran los eslabones del sstema, comenzando por para la base del robot, hasta n para el efector fnal Se numeran las artculacones del sstema, comenzando con para la prmer artculacón y n para la últma; donde n número de grados de lbertad Los sstemas de coordenadas se asgnarán en donde se ntersecan el eslabón con la artculacón con base en lo sguente: a. a. Los ejes Z estarán ubcados a lo largo del eje de movmento de la artculacón. Para el caso de artculacones de revolucón será a lo largo del eje de rotacón; en tanto que para las artculacones prsmátcas, será a lo largo del eje de movmento de esta. b. b. Se asgna el prmer sstema de coordenadas completa a la base del manpulador, procurando que uno de los ejes de este sstema este en línea con la orentacón del brazo de robot; numerando a este sstema de coordenadas como sstema cero.

49 c. c.los ejes x se asgnarán de manera tal que estos sean normales a los ejes z. d. d. Los ejes y complementarán los sstemas de coordenadas para formar sstemas dextrógros. e. e. Como los sstemas van numerados desde hasta n, un últmo sstemas de coordenadas se asgna al efector fnal, o ben al plato de herramentas, del robot. Este, habrá de conservar la orentacón del sstema de coordenadas n. f. f. Se dentfca el sentdo postvo en el desplazamento de las artculacones, de acuerdo a la regla de la mano derecha. Un eslabón puede ser consderado como un cuerpo rígdo, el cual puede ser descrto por dos parámetros, la longtud del eslabón y el gro del eslabón. Estos parámetros defnen la localzacón relatva de los ejes de artculacones vecnas en el espaco. Asmsmo, las artculacones pueden ser descrtas tambén por dos parámetros, el descentramento del eslabón, que es la dstanca de un eslabón a otro próxmo, a lo largo del eje de la artculacón; y el ángulo de la artculacón, que es la rotacón de un eslabón con respecto al próxmo, alrededor del eje de la artculacón. La representacón de Denavt y Hartenberg de un cuerpo rígdo depende entonces, de cuatro parámetros geométrcos asocados a cada elemento, estos descrben completamente la relacón espacal entre sstemas de coordenadas. Estos parámetros pueden resumrse como:

50 Longtud del eslabón a : dstanca desde el orgen del sstema de coordenadas hasta la nterseccón de los ejes x y z, a lo largo del eje x ; Gro del eslabón α : ángulo formado entre el eje z al eje z alrededor del eje x ; Descentramento del eslabón d : dstanca desde el orgen del sstema de coordenadas hasta la nterseccón de los ejes x y z, a lo largo del eje z. Ángulo de la artculacón : ángulo de la artculacón del eje x al eje x respecto al eje z. De esta forma, para una artculacón de revolucón, d, a y α son los parámetros de la artculacón y permanecen constantes, mentras que es la varable de la artculacón. Para una artculacón prsmátca, la varable será d. y z a d z z z x x z x α x x y y z Fgura.8 Parámetros de D-H.

51 De acuerdo con [5], [7] y [], la representacón de D-H resulta en una matrz de transformacón homogénea 4 x 4, - A : A cos sen sen cosα cos cosα senα sen senα cos senα cosα a cos a sen d...(.9) que descrbe la relacón entre el sstema de coordenadas de cada eslabón con respecto al sstema de coordenadas prevo [], T T T donde es la transformacón homogénea que descrbe la poscón del sstema de coordenadas con respecto al sstema de coordenadas de la base. A De manera tal que s tenemos un robot con 3 grados de lbertad, la matrz T 3 estará dada por: T 3 A A A 3 La matrz de transformacón homogénea [T], que especfca la poscón y orentacón del efector fnal del manpulador con respecto al sstema de coordenadas de la base, es tan frecuentemente utlzada en la cnemátca de los manpuladores robótcos que se conoce como matrz del robot, y es común escrbr la sguente forma: Tn...(.)

52 T n x n y nz s s s x y z a a a x y z p p p x y z donde [n,s,a] nos representa la orentacón del sstema de coordenadas del efector fnal con respecto al sstema de coordenadas de la base y [p] la poscón de este con respecto a la base. De esta matrz se obtenen las ecuacones de dseño del manpulador, las cuales srven como base para la solucón a la cnemátca nversa.. Dnámca de Manpuladores Robótcos. La dnámca es la rama de la mecánca que trata el movmento de los cuerpos bajo la accón de fuerzas. Esta ncluye a la cnemátca, que es el estudo del movmento sn tomar en cuenta las causas que lo producen, y la cnétca, que relacona estas fuerzas con el movmento resultante. Cuando una fuerza es aplcada a un cuerpo, este tende a acelerarse de acuerdo con las leyes de Newton. Por tanto, sobre la base de esta defncón, el comportamento dnámco de un manpulador robótco es descrto en térmnos de la relacón de tempo con el de cambo de la confguracón del brazo y con el torque dado por sus actuadores. La dnámca del robot trata con las formulacones matemátcas de las ecuacones de movmento del brazo, establecendo un conjunto de ecuacones que descrben su comportamento dnámco. Estas ecuacones son útles para la smulacón en computadora del movmento del robot, el dseño de leyes de control adecuadas y para la evaluacón del dseño y estructura del brazo [5].

53 La mayoría de los manpuladores robótcos son anmados por actuadores eléctrcos, pneumátcos o hdráulcos, los cuales aplcan torques (o fuerzas, en el caso de actuadores lneales) a las artculacones del robot. En donde las ecuacones de movmento del manpulador determnan la respuesta de este a la aplcacón de dchas fuerzas. El modelo dnámco del robot se puede obtener a partr de leyes físcas conocdas, tales como la mecánca newtonana y lagrangana, estas conducen al desarrollo de ecuacones de movmento dnámco para las dversas artculacones del manpulador, en térmnos de los parámetros geométrcos e nercales de los elementos. De manera smlar al caso de la cnemátca, la dnámca de un manpulador comprende el análss de dos problemas: el problema de la dnámca nversa, en el que dado un vector de poscones, velocdades y aceleracones para el efector fnal, se calcula el vector requerdo de torques en las artculacones, que satsfaga las condcones de movmento deseadas para un nstante (t). S la dnámca nversa es modelada de manera exacta, el controlador podrá predecr el torque adconal requerdo para segur una trayectora durante la aceleracón y/o desaceleracón. Con dcho controlador, el robot puede ncrementar la velocdad en el efector fnal y, por tanto, reducr el tempo para la ejecucón de una tarea. Un segundo problema, de mportanca en estudos de smulacón, es el problema de la dnámca drecta, en el que dado un vector de torques aplcados en las artculacones, se calcula la respuesta del sstema en base del movmento resultante del manpulador [6]. Un actuador tene que balancear los torques de cuatro fuentes: el torque dnámco, dervado del movmento; torque estátco, debdo a la frccón en el mecansmo, los torques por gravedad, dervados por la accón de la gravedad sobre cada eslabón, y fuerzas y torques externos que actúan en el efector fnal. Tres tpos de torques

54 dnámcos se dervan del movmento del manpulador: nercal, centrípeto y de Corols [9]. Torque nercal que, de acuerdo a la segunda ley de Newton, es proporconal a la aceleracón de la artculacón, en donde la nerca es la tendenca de un cuerpo a mantener el estado de movmento unforme o reposo en el que se encuentra. Torque centrípeto, se derva de las fuerzas centrípetas que restrngen a un cuerpo a rotar respecto de un punto, se drge haca el centro del movmento unforme crcular, y es proporconal al cuadrado de la velocdad angular de la artculacón. Torque de Corols dervado de las fuerzas de vórtce generadas por la nteraccón de dos eslabones rotantes, sendo estas proporconales al producto de las velocdades angulares de dchos eslabones, debdo al movmento relatvo entre estos. Las leyes báscas de la dnámca del manpulador pueden obtenerse a partr de las leyes de Newton en conjunto con conceptos como el prncpo del trabajo vrtual de D Alembert, ecuacones de Lagrange, y ecuacones de Hamlton [5], [7]. En la robótca, las dos formulacones que comúnmente se tratan son: la formulacón de Newton-Euler, y la formulacón de Lagrange [9]. La dnámca de un manpulador robótco se descrbe por medo de un conjunto de ecuacones dferencales ordnaras no lneales de segundo orden, que dependen de las propedades cnemátcas e nercales del robot. Para la solucón a la dnámca del manpulador habremos de basarnos en la dervacón Lagrangana de la dnámca. Esta técnca tene la ventaja de requerr úncamente de las energías cnétcas y potencales del sstema para su cálculo.

55 .. Formulacón de Básca. S se consdera un sstema de n partículas que obedecen la segunda ley de Newton, en la que la razón de cambo en el momento de una partícula es gual a la fuerza aplcada a esta. Sendo F la fuerza aplcada en la -ésma partícula, m la masa de esta y r su vector de poscón, entonces la segunda ley de Newton se escrbe como: F m& r,,..., n (.) pero, ya que el nterés de este estudo no es sobre una partícula ndependente, sno sobre un conjunto de partículas que están fjas a otras y tenen grados de lbertad lmtados [9]. Es entonces, necesaro descrbr estas nterconexones ntroducendo restrccones entre las poscones de las partículas. Cada restrccón es representada por una funcón g j : g ( r, Κ, r ) j,..., k j n k no. de restrccones... (.) Una restrccón que puede ser escrta en esta forma, como una relacón algebraca entre las poscones de las partículas, es conocda como restrccón holonómca [9], []. Una restrccón actúa en un sstema por la accón de una fuerza de restrccón, dcha fuerza es determnada de manera tal que la ecuacón de restrccón (.) es sempre satsfecha. S vemos a las restrccones como una superfce plana, las fuerzas de restrccón son sempre normales a esta superfce, y restrngen la velocdad del sstema a ser tangente a esta en todo momento [9]. Por lo que el modelo dnámco dado por (.) puede ser reescrto cómo una ecuacón vectoral de la sguente manera:

56 mi F Ο & r Μ + m ni r n k j Γ λ j j (.3) donde los vectores Γ,..., Γn son las bases para las fuerzas de restrccón, los cuales no se requere que sean ortonormales; y λ j es el factor de escala para el j - ésmo elemento. Para restrccones de la forma en que aparecen en la ecuacón (.3), Γ j puede tomarse como el gradente de g j, que es perpendcular al nvel establecdo por g j ( r). Los escalares λ,..., λn son conocdos como los multplcadores de Lagrange, cuyos valores dan úncamente magntudes relatvas de las fuerzas de restrccón, dado que los vectores Γ j no son necesaramente ortonormales [8], [9]. Este acercamento para tratar con restrccones es ntutvamente sencllo, pero complejo desde el punto de vsta de cómputo, ya que es necesaro mantener la atencón en el estado de todas las partículas en el sstema, aún cuando estas no sean capaces de movmento ndependente [9]. Un acercamento más atractvo consste en descrbr el movmento del sstema en térmnos de un conjunto más pequeño de varables, que descrban completamente la confguracón del sstema []. Para un sstema de n partículas con k restrccones, se busca obtener un conjunto de m 3 n k varables, q,...,qn, y funcones de gualdad f,..., f n, de manera que: ( r,..., r ) r f ( q,..., qn ) g j n,..., n j,..., k

57 Sendo las q ' s el conjunto de coordenadas generalzadas para el sstema. En un manpulador robótco compuesto por eslabones rígdos, estas coordenadas generalzadas corresponden a las varables de las artculacones, sendo: q d para artculacones de revolucón para artculacones prsmátcas Dado que los valores de las coordenadas generalzadas son sufcentes para especfcar la poscón de las partículas que conforman al robot, podemos reescrbr las ecuacones de movmento del sstema en térmnos de éstas, para componentes en coordenadas generalzadas se hace referenca a estas fuerzas como fuerzas generalzadas [6]... Formulacón de Lagrange. La dnámca lagrangana se basa en la determnacón de la energía contenda en el sstema [], defnendo al Lagrangano L como la dferenca entre la energía cnétca y la energía potencal, de manera tal, que las ecuacones para un sstema mecánco expresado en coordenadas generalzadas q, están dadas por: d dt L L,,..., n q τ & q...(.4) donde: L Funcón Lagrangana energía cnétca K energía potencal P, q Coordenada generalzada del brazo τ Fuerza o par generalzado aplcado al sstema en la artculacón.

58 Por lo que es necesara la comprensón de la matrz de transformacón de coordenadas homogéneas A, que descrbe la relacón espacal entre los sstemas de coordenadas del -ésmo elemento con el ( ) -ésmo elemento. Relaconando un punto fjado en el elemento, expresado en coordenadas homogéneas, con respecto al sstema de coordenadas -ésmo en el sstema de coordenadas (-)-ésmo [5]. Nótese que s no hay restrccones en el sstema, entonces se puede hacer a q componente de rˆ, resultando en K m r&, así la ecuacón (.4) se reduce a la ecuacón (.). De hecho, reordenando la ecuacón (.4), se puede representar como: d dt L L +τ q& q (.5) Que es un replanteamento de la ley de Newton en coordenadas generalzadas [9].... Dnámca Lagrangana de un Manpulador de dos grados de Lbertad. Para un manpulador de dos grados de lbertad, artculado en la base y el hombro, como se muestra en la fgura (.9), la base esta fja, y como consecuenca, el centro de masa de esta no se mueve, lo que hace que el térmno de velocdad lneal en la ecuacón de la energía cnétca sea cero, en tanto que la energía potencal del eslabón permanece constante.

59 Fgura.9 Manpulador de dos grados de lbertad. Par efectos de lustrar la dervacón de las ecuacones dnámcas de Lagrange, se modelarán a los eslabones como elementos unflares con concentracón de masa (m) en el extremo dstal de estos. Ya que la energía cnétca es ndependente del sstema de coordenadas en el cual se evalúa, el cálculo de la energía cnétca, debda a la velocdad angular, se hace en el sstema de coordenadas del eslabón, de esta manera, el tensor de nerca no tene que ser transformado al sstema de coordenadas de la base. Partendo de la matrz T que corresponde a la solucón a la cnemátca drecta de este manpulador, se obtenen las expresones para determnar las velocdades del sstema, como se muestra a contnuacón: + S l d C S C S l C S S C S C C l S C S C C T y como V, se puede dervar fáclmente el vector de poscón en la matrz anteror para obtener l C S S C C C S C S V & & & & &

60 las velocdades angulares ω & y + ω & & & & & S C C S S C se calcula ahora la energía cnétca para cada eslabón medante: & m K ya que el eje de rotacón pasa a través del centro de masa, entonces ω ω ω ω & & & & & C S m V m K T como el eje de rotacón pasa a través del centro de masa (sendo la velocdad angular expresada en el sstema de coordenadas de la artculacón), la nerca no tene contrbucón en la energía cnétca de sstema (cuando este no es el caso, el segundo térmno exste y es de la forma + T J ω ω ). ( ) ( ) [ ] [ ] [ ] l C l C S S S C l C C C S l C S S C C C S C S l V V V T & & & & & entonces: ( ) & & + C m l K y las energías potencales para cada eslabón están dadas como: ( ) S l l g m P m gl P +

61 por lo que el Lagrangano del manpulador es: ( ) ( ) ( ) ( ) S l l g m gl m C m l P P K K L & & de donde se obtenen los sguentes componentes correspondentes a la formulacón dnámca de Lagrange. ( ) && & & & & & & S C C m l L dt d L C m l C m l L y & & & & & & & & m l L dt d C gl m C S m l S gl m C m l L m l m l L lo que determna las ecuacones de los torques a partr de la formulacón de Lagrange, como: τ & & & S C m l C m l L L dt d C gl m C S m l m l L L dt d + + τ & & que son las ecuacones dferencales que gobernan el movmento del manpulador, expresando lo anteror en notacón matrcal: + + C m l S C m l C S m l C l m τ τ && & & &

62 que de forma general puede escrbrse como [5], [7]: ( )& + C(, && ) N(, & ) τ M (.6).3 Control de Manpuladores Robótcos. El prmer paso cuando se dseña un sstema de control es el determnar la funcón de transferenca, o modelo de la planta del proceso que se trata de controlar. Un modelo estátco de la planta descrbe la salda como funcón de la entrada, excluyendo los efectos dnámcos. En un sstema lneal, un modelo estátco es la relacón de la salda con respecto de la entrada. Por ejemplo, la relacón de revolucones a la salda de una caja de engranes que es constante. La mayoría de los sstemas físcos son lneales úncamente dentro de un rango de las varables [3], razón por la cual se dseñan los sstemas para que trabajen en el rango lneal. Cuando lo anteror no puede ser establecdo, se trata de controlar un sstema no lneal, en el que el modelo estátco es una funcón no lneal para una o más varables [3]. Por otra parte, las funcones de transferenca son modelos de planta que ncluyen tanto los efectos dnámcos del sstema, como la relacón estátca entre la entrada y la salda. Dchos efectos hacen que la relacón entre la entrada y la salda varíen durante la transcón de un estado a otro. El prncpal efecto dnámco, en lo que corresponde al control, es el retraso en el tempo desde que se vara la entrada hasta que la salda responde [9]. Fgura. Cambos en la entrada de un sstema de control

63 La entrada de un sstema puede ser manpulada de varas maneras (Fgura.). Una entrada del tpo mpulso, representa de manera general una perturbacón no deseada. Una entrada que vara de manera contnua, como la senodal, es usada para medr la funcón de transferenca. La mayoría de las entradas a un sstema son del tpo rampa, las cuales varían en un rango fjo [33]. Los efectos dnámcos dependen de la frecuenca y no solo retrazan el cambo en la salda en respuesta de la exctacón presente en la entrada, sno tambén, lmtan el rango en el cual la salda puede responder. Este límte es conocdo como relacón de desvacón (fgura.) del sstema. S la rampa es más rápda que la relacón de desvacón, la salda no puede gualar a la entrada, y un error de segumento es generado. Este últmo es la dferenca entre la entrada y la salda en cualquer nstante en el tempo. Uno de los usos del control predctvo es el antcpar la respuesta del sstema de manera tal que el error de segumento sea mínmo. Fgura. Error de segumento debdo al efecto dnámco Las funcones de transferenca para un sstema lneal son expresadas en el domno del tempo como ecuacones dferencales, y en el domno de Laplace como transformadas de Laplace. Al consderar un bloque de masa (m) que es jalado a lo largo de una superfce con frccón (f) por un cable de constante elástca (k) (Fgura.), el modelo dnámco para este sstema puede dervarse de la segunda ley de Newton. Cuando un vector F(t) es aplcado, una fuerza de reaccón, compuesta por una fuerza nercal proporconal a la aceleracón de la

64 masa, una fuerza de frccón proporconal a la velocdad de la masa y una fuerza elástca proporconal a la tensón del cable, deben ser consderadas. Fgura. Sstema masa-resorte-amortguador. m & x + fx& + kx& F(t) (.7) La funcón de transferenca es la relacón entre la transformada de Laplace de la salda y la transformada de Laplace de la entrada. salda entrada G( s) ms + fs + k ( S + a)( S + b) (.8) El polnomo en el denomnador, cuando este se guala a cero, se conoce como ecuacón característca, dado que las raíces de esta ecuacón caracterzan el tempo de respuesta del sstema. Las races de esta ecuacón son llamadas polos o sngulardades del sstema son llamadas ceros del sstema. Estos son dbujados en un dagrama de polos y ceros (fgura.3).

65 Fgura.3 Dagrama polos-ceros a)polos smples; b) polos complejos En robótca, funcones de transferenca son requerdas para los eslabones y los actuadores. Por ejemplo, un eslabón puede ser modelado como un péndulo con la masa concentrada en el extremo dstal de este. El torque aplcado a la flecha de la artculacón es entonces, funcón de la nerca, la frccón en la artculacón y la gravedad. τ m & + f & + mgl (.9).3. Control de la velocdad de motores de C.D. Debdo a que las varables a controlar (, &, & ) dependen drectamente del desplazamento, velocdad y aceleracón del actuador, que en este caso es un motor de c.d., se presentan las técncas y consderacones para su control. La velocdad de salda de un motor de corrente drecta, puede ser controlada ya sea, varando el voltaje en la armadura, o varando la corrente en el campo. Para un motor controlado por el voltaje en la armadura, el aspecto dnámco tene que consderar el efecto de la nductanca en la armadura y la carga por nerca (ncluyendo la nerca de la armadura, y la frccón). Como la corrente en la armadura es constante, no exste efecto dnámco en el campo. La corrente en la armadura (I ) es funcón del voltaje aplcado (V ), la resstenca de la armadura ( R a ), la nductanca de la armadura (L) y la fuerza electromotrz en retorno (E). ( R + LS ) I( s) E( ) V ( s) a + s (.) El torque ( τ ) a, producdo por el motor, es funcón de la fuerza del campo ( φ ) y de la corrente de la armadura.

66 τ a ( s) K aφi ( s) K mi( s) (.) Este torque hace que el motor gre la carga con una velocdad angular ω. El torque del actuador es contrarrestado por el torque debdo a la nerca y la frccón. τ L JSω( s) + fϖ ( s) (.) La rotacón de la armadura produce una fuerza electromotrz de retorno que es proporconal a la velocdad de esta (ω ). E( s) K aφϖ ( s) K mϖ ( s) (.3) Entre más grande sea la carga, más lento rotará la armadura, reducendo con esto la fem de retorno e ncrementando la corrente en la armadura para mover la carga. Al combnar estas expresones para formar el dagrama a bloques de la fgura.9 y obtener la funcón de transferenca del motor, se obtene: K V S + m [( R + LS )( JS + F ) ( K ) ] a m (.4) S el eje del motor se une a una caja de engranajes, la velocdad de salda de la esta ( ϖ ) es el producto de la velocdad del motor. Fnalmente, s se quere controlar el torque aplcado por el eje del motor, en lugar de la velocdad o poscón de este, se utlza una funcón de transferenca de corrente a torque. τ I K K m m ( R a + ( R + LS)( JS + F) a + LS)( JS + F) (.5)

67 La técnca que se emplea para varar la velocdad de un motor de c.d. consste en hacer uso de un amplfcador, este controla el voltaje aplcado en las termnales del motor, y por tanto, la velocdad de este. Hoy en día, dos tpos de amplfcadores son utlzados, el amplfcador de potenca de c.d. y amplfcadores con modulacón por ancho de pulso (PWM por sus sglas en nglés). El amplfcador de potenca de c.d. magnfca un pequeño voltaje a la entrada de este, a un voltaje capaz de mover un motor. Tanto el valor de entrada, como el de salda del amplfcador son analógcos. La gananca en voltaje es normalmente pequeña, pero la gananca en potenca puede ser muy grande. De esta forma, la corrente de salda es funcón de la resstenca de la carga. Estos amplfcadores usualmente ncluyen una retroalmentacón nterna, de manera tal que poseen una funcón de transferenca lneal en voltaje, así que estos pueden ser modelados como una gananca constante. V V K a gananca del amplfcador El segundo tpo de amplfcador es el que utlza la modulacón por ancho de pulso. Este elemento proporcona un tren de pulsos al motor. El voltaje aplcado al motor es un valor promedo de c.d. t V promedo V t + t (.6) Este tpo de amplfcadores presentan dos ventajas: control electrónco sencllo y buen desempeño en operacón a bajo voltaje. El control electrónco puede ser smplfcado medante nterruptores de energía (transstores) con la temporzacón

68 hecha por programacón. A bajas velocdades, el voltaje aplcado por un amplfcador de c.d. puede que no sea sufcente para romper la resstenca de contacto entre las escobllas y el conmutador del motor, o para vencer la frccón estátca. Como la modulacón por ancho de pulso sempre aplca el msmo voltaje, este problema no se presenta, pero s la frecuenca de los pulsos es muy baja, la respuesta del motor puede ser muy burda, para mnmzar este efecto, la frecuenca de modulacón suele ser muy alta, de esta manera, la armadura se comporta como un fltro R-L, y promeda los pulsos de entrada. Como las ganancas de voltaje y corrente son smlares a aquellas del amplfcador de c.d., este amplfcador tambén se modela como uno con gananca constante. No obstante, s un control electrónco smple es usado, una retroalmentacón de voltaje puede ser requerda para hacer lneal el control en voltaje. El objetvo del control de un manpulador basado en computadora es el mantener la respuesta dnámca del sstema de acuerdo con objetvos deseados. Por lo que, para controlar el movmento de una sere de eslabones, de manera tal que el efector fnal trace una trayectora deseada, el sstema de control tene que proveer los torques en las artculacones para balancear las fuerzas en los eslabones. Cuando un eslabón es acelerado, el actuador habrá de proveer el torque necesaro para contrarrestar la nerca. S el robot utlza úncamente control de retroalmentacón de poscón o velocdad, la dnámca del sstema provocará que el efector fnal se atrase con relacón a la trayectora deseada durante la aceleracón y posblemente perda el punto fnal de la trayectora. De manera general, son tres los tpos de respuestas que se pueden observar en un sstema bajo control, estas son: sub-amortguado, respuesta que se obtene cuando el valor del amortguamento es <; crítcamente amortguado, cuando el valor del amortguamento es ; y sobre-amortguado, cuando el valor del amortguamento es >.

69 En el espaco cartesano se hace uso de la solucón a la cnemátca nversa para calcular los ángulos que habrán de posconar al efector fnal en la poscón y orentacón deseada en el espaco. Las artculacones se desplazan a esas poscones angulares y se asume que el manpulador se encuentra en la poscón cartesana correcta, s no exste algún elemento que permta cuantfcar el desplazamento en las artculacones, no habrá certeza de s el manpulador se encuentra en la poscón correcta. Lo anteror es conocdo como control de lazo aberto. Este modelo de control es tan exacto como lo sea el modelo de la planta, asegurando que no hay perturbacones que causen error, ya que las varacones mecáncas y/o del entorno, pueden causar errores en la poscón del efector fnal, pues el modelo de control no es capaz de detectar n corregr dcho error. Cuando el resultado de una accón preva es usado para corregr el error en la planta, recbe el nombre de control adaptvo. Este tpo de control corrge errores en la planta debdos a varacones a largo plazo, pero no corrge errores en la dnámca del sstema. Por ejemplo, cuando un lanzador suelta la pelota, este ya no tene más control sobre ésta, s el resultado no es el deseado, es decr s falla el lanzamento, el lanzador corregrá o adaptará, en otro lanzamento, la fuerza aplcada en el lanzamento para consegur que la pelota sgua la trayectora deseada. El uso de un modelo para predecr que accón tomar sobre la cantdad de energía en uso, se conoce como control prealmentado o control predctvo. Este tpo de control se utlza para realzar ajustes en el sstema s la señal de retroalmentacón esta retrasada y es necesaro tomar una accón para mantener la respuesta del sstema dentro de los límtes deseados.

70 Fgura.4 Estructura básca de control de poscón Para obtener un modelo de control precso de la planta durante la ejecucón de una accón, se utlza el control con retroalmentacón. Bajo este modelo de control, el parámetro a ser controlado es contnuamente meddo (retroalmentacón), y comparado con una referenca (cálculo del error). Cuando se controla un manpulador robótco, el control predctvo se usa para calcular las referencas de las artculacones a partr del espaco cartesano, utlzando la solucón a la cnemátca nversa. Por otra parte, para controlar una artculacón ndvdual (Fg..4), el control de retroalmentacón es utlzado para comparar el ángulo meddo con el ángulo de referenca. Cualquer error entre la señal de retroalmentacón y la de referenca es amplfcado para proveer la energía necesara al actuador..3. Ley de control en lazo aberto de manpuladores robótcos. Como se acordó anterormente, la dnámca de un manpulador está descrta por la ecuacón (.6), en donde [ ] es el conjunto de varables que defnen la confguracón del manpulador y τ representa el torque aplcado en las artculacones. Se denota aquí, por medo de d, la trayectora o poscón deseada

71 de la artculacón, la cual se asume que está defnda para cualquer nstante en el tempo y que es al menos dferencable en dos ocasones [9]. S se tene un modelo perfecto del robot y ( ) ( ), & ( ) ( ) d & d podemos modelar al torque necesaro como: τ ( )& + C(, & ) & + N(, & ), entonces, M d d d d d d d (.7) Dado que tanto como d satsfacen la msma ecuacón dferencal y tenen las msmas condcones ncales, se puede tomar que ( t) d ( t) para toda t. Esta estratega de control no es muy robusta, ya que s ( ) d ( ), entonces la ley de control en lazo aberto nunca habrá de corregr el error. Esto claramente no es deseable, pues nunca se sabría con certeza la poscón real del robot. Por esta razón, se ntroduce la retroalmentacón en la ley de control. Esta retro almentacón debe establecerse de manera tal que la trayectora real del robot converja en la trayectora deseada..3.3 Ley de control del Torque Calculado. La ley de control de lazo aberto puede refnarse dando la poscón y velocdad real del manpulador, cancelando todas las no lnealdades y aplcando exactamente el torque necesaro para contrarrestar la nerca del actuador. La ley de control puede expresarse como: ( )& + C(, && ) N(, & ) τ M d (.8) Al susttur esta ley de control en la ecuacón dnámca del manpulador, se observa que:

72 M ( ) M ( )& d & (.9) y dado que M ( ) está completamente defnda en ( ), tenemos: & & d (.3) De aquí, que s la poscón y velocdad ncales del manpulador concden con aquellas deseadas, el manpulador habrá de segur la trayectora deseada. Pero tambén, esta ley de control no corrge nnguna condcón de error que se encuentre presente, pues aún es una ley de control en lazo aberto. Las propedades de esta ley de control pueden mejorarse al nclur un térmno de realmentacón en ésta, la condcón de lnealdad (.3) sugere la sguente ley de control: τ M ( )(& K e K e) + C(, && ) N(, & ) d v & p (.3) K p donde e d, y K v y son las constantes de gananca. Al susttur en la ecuacón (.8), el error dnámco puede ser escrto como: M ( )( e + K e + K e) & v & p (.3) como M ( ) es sempre postvo y defndo, se tene: e& + K v e& + K pe (.33) Esta es la ecuacón dferencal lneal que goberna el error entre las trayectoras real y deseada. La ecuacón (.3) es conocda como la ley de control del torque calculado [9], que puede ser reescrta como:

73 ( )& + C & + N + M ( )( K e& K e) τ M d 4 44 τ ff τ fb p v (.34) El térmno τ ff es el componente de prealmentacón o predctvo, el cual provee la cantdad de torque necesaro para conducr al sstema a lo largo de su trayectora deseada. En tanto que el térmno τ fb es el parámetro de retroalmentacón, que provee el torque de correccón para reducr los errores en la trayectora del manpulador. De manera general, en éste capítulo se han presentado los fundamentos báscos para el modelado matemátco de manpuladores robótcos, en lo que corresponde a la cnemátca, dnámca y control de estos. En el sguente capítulo, se obtene el modelo matemátco que goberna el movmento del manpulador de análss.

74 El Manpulador Robótco de 5 Grados de Lbertad Mtsubsh Movemaster RV-M como Caso de Estudo E n e s t e c a

75 3. El robot de 5 Grados de Lbertad Mtsubsh Movemaster como Caso de Estudo. 3. Descrpcón del Manpulador Robótco Mtsubsh. El Mtsubsh Movemaster EX es un robot artculado con cnco grados de lbertad, fgura 3., con una capacdad de carga máxma de.5 Kg. Para que el robot Mtsubsh realce la secuenca de movmentos es ndspensable que estén actvos en la memora, tanto el archvo de puntos, como el programa, el cual descrbrá la secuenca de puntos y la nteraccón con otros elementos. Fgura 3. Robot Mtsubsh Movemaster Las dmensones físcas del manpulador se muestran en la fgura 3. Fgura 3. Dmensones (en mm.) del Robot Movemaster

76 Los elementos que consttuyen al sstema del robot Mtsubsh son: el brazo de robot, el controlador prncpal y el control manual; los cuales se presentan a contnuacón en la fgura 3.3. Fgura 3.3 Sstema Movemaster La capacdad de movmento de este manpulador de 5 grados de lbertad se lustra en la fgura 3.4 Fgura 3.4 Grados de Lbertad del Movemaster.

77 3.. Sstemas de Coordenadas sobre los que se desplaza el robot Mtsubsh Movemaster A contnuacón se descrben los dversos modos en los que se pueden realzar los movmentos del robot MITSUBISHI, para facltar la localzacón de un punto en especal. Modo PTP: este modo permte mover, artculacón por artculacón, los elementos del robot para modfcar su confguracón en el espaco. Modo XYZ: en este modo, el brazo realza movmentos en los planos X, Y y Z, tomando como base el sstema cartesano del robot, localzado en la base de este. Modo Herramental: en este modo, el robot se mueve en base al sstema de coordenadas que se localza en el plato de herramentas, que por la confguracón del brazo, el eje Z, corresponde a un acercamento o dstancamento del objetvo. 3. Planteamento del problema cnemátco. El problema cnemátco consste en obtener las ecuacones que descrben el movmento del robot, sn tomar en cuenta las fuerzas que lo producen, estas ecuacones se conocen como ecuacones de dseño del manpulador Obtencón de las ecuacones de dseño del Manpulador. Para poder dar solucón tanto a la cnemátca drecta como a la nversa, es necesaro establecer un conjunto de ecuacones que descrban el movmento del efector fnal en el espaco cartesano, en funcón de los desplazamentos angulares o lneales de los eslabones que lo componen. Estas ecuacones se obtenen de la

78 convencón establecda por Denavt y Hartenberg, de manera tal que, para un manpulador de n grados de lbertad, se obtendrá un sstema de ecuacones con n ncógntas, en donde por lo general, n<. Estas ecuacones representan la poscón y orentacón del efector fnal en coordenadas cartesanas, en funcón de los desplazamentos de las artculacones. Para el manpulador de 5 grados de lbertad, sguendo la metodología descrta en el capítulo, se tene: a) a) El manpulador en una poscón ncal convenente para asgnar los sstemas de coordenadas (Fgura 3.5). Fgura 3.5 Poscón ncal para la asgnacón de sstemas de coordenadas b) b) Asgnacón de los sstemas de coordenadas (Fgura 3.6), de donde se derva la matrz de parámetros de Denavt-Hartenberg (Tabla 3.): Fgura 3.6 Sstemas de coordenadas para el robot

79 Tabla 3. Parámetros de D-H Art culacón α a d De la cual se obtenen las matrces de transformacón homogénea A : A 3 C S S C A 5 5 S C S C S C A S C S C S C 3 A C S S C 4 A C S S C y la matrz de transformacón correspondente al efector fnal con relacón a la base 5 T. T S S C C S S C S C S C S S S S S C C S C S C S C C S C C C C C S C S C S S C C S S C C C donde ) cos( C y ) ( sen S

80 gualando membro a membro con la ecuacón (.), se tene: n n n s s s a a a p p p x y z x y z x y z x y z cos cos sen cos sen 34 cos cos cos sen sen C sen cos sen sen cos + cos sen sen sen cos sen + cos cos (3.) cossen 34 sensen 34 cos 34 7cossen cos cos cos cos 7sensen sen cos sen cos 7cos sen 3 + 5sen + 3 Al conjunto de ecuacones conformado por (3.) se le conoce como las ecuacones de dseño del manpulador [8]. En general, se trata de un conjunto de ecuacones no lneales sobredetermnado, de ecuacones con n ncógntas, que descrben el movmento del efector fnal en el espaco cartesano, en funcón de la poscón angular, o desplazamento angular de las artculacones. Para este caso se trata de un sstema de ecuacones con 5 ncógntas, ya que es un sstema de 5 grados de lbertad. Como la cnemátca no solo es el estudo de la poscón, sno tambén de la velocdad y aceleracón, las expresones que defnen dchas relacones de cambo se obtenen fáclmente al dervar las ecuacones de dseño con respecto al tempo, asumendo que es funcón de t y que esta puede dervarse.

81 3... Planteamento de la Solucón a la Cnemátca Drecta e Inversa del Manpulador Cnemátca Drecta. La solucón a la cnemátca drecta a partr de las ecuacones de dseño del manpulador es relatvamente senclla, ya que estas expresan la relacón espacal entre los elementos en funcón de los valores de entrada en las artculacones del manpulador. Para conocer la poscón y orentacón del efector fnal del manpulador para un vector de poscones dado para las artculacones, solo basta susttur los valores para las ' s en las ecuacones de dseño, en la matrz del robot, obtenendo como resultado el vector de poscón de la base al efector fnal y la matrz de orentacón [n, s, a] de este con respecto al sstema de coordenadas de la base. Y para conocer la velocdad y aceleracón lneal del efector fnal como resultado de velocdades y aceleracones angulares en las artculacones, se susttuyen estos últmos en las expresones correspondentes para velocdades y aceleracones de las ecuacones de dseño. Resolvendo con esto, de manera completa, lo referente a cnemátca drecta. Para la solucón numérca a la Cnemátca Drecta se desarrolló la funcón Cdmover (Anexo) bajo el programa Matlab, los elementos de la matrz de transformacón homogénea para el robot Movemaster que se utlzan en esta funcón para determnar la poscón y orentacón del efector fnal son los que se obtuveron en (3.). La funcón Cdmover resuelve la cnemátca drecta a partr de dos vectores de entrada, el vector constante [VC], que contene las longtudes de los eslabones del manpulador, y el vector varable [VV], que contene los valores de los ángulos de las artculacones, obtenendo la matrz correspondente para la poscón y orentacón del efector fnal especfcada por los ángulos de entrada. En la fgura 3.7 se muestra el dagrama de flujo de la funcón.

82 Fgura 3.7 Dagrama de Flujo de la Funcón Cdmover. Como ejemplo, la matrz [M] que se obtene para ángulos de entrada... 5 [π / π / ], que corresponden a la poscón de orgen selecconada y con longtudes de los eslabones L...L 4 [ ] es:» cdmover(vv,vc) ans

83 Como podemos observar, la poscón y orentacón del efector fnal obtendas corresponden a aquellas que concden con la poscón ncal consderada para establecer los sstemas de coordenadas del manpulador Cnemátca Inversa. Como se explcó anterormente, exsten dversos métodos para la solucón a la cnemátca nversa. El que aquí se plantea se basa en la comprensón de que hoy en día las herramentas de computo con las que se cuenta, poseen una velocdad de procesamento que permte realzar el cálculo de la solucón al problema planteado en poco tempo, permtendo así su mplementacón. La cnemátca nversa consste en determnar los valores de las artculacones que satsfagan condcones deseadas de poscón, velocdad o aceleracón en el espaco cartesano. De lo anteror, podemos dentfcar tres problemas a resolver, cnemátca nversa para poscones, cnemátca nversa para velocdades y cnemátca nversa para aceleracones. Para el prmer problema, se parte de las ecuacones de dseño, las cuales representan un sstema de ecuacones no lneales sobredetermnado, de ecuacones por n ncógntas. Aquí, se especfcan los valores deseados para [ n s, a, p], para los que se resolverán los valores de ' s. Como el sstema de ecuacones es no lneal, el prmer paso es lnealzar el sstema aplcando algún método teratvo como es el método de Newton, que hace uso de la expansón de Taylor[34]. El modelo matemátco del método de Newton es el sguente: f ( x, x,..., xn ),,,..., n...(3.)

84 donde f es una funcón no lneal de las xj. S tenemos una estmacón ncal de la solucón, esta puede escrbrse como sgue: xj xj ˆ + xj...(3.3) donde xj es la estmacón ncal y xj es una correccón desconocda. S expandmos la ecuacón 3. para obtener un polnomo de Taylor truncado de prmer orden alrededor de xˆ j obtenemos: j f xj f( xˆ, xˆ,..., xn ˆ ) xj...(3.4) donde las dervadas parcales se evalúan con las condcones ncales. La ecuacón 3.4 puede escrbrse en forma de matrz como: J x -f...(3.5) Donde J es la matrz Jacobana dada por f J xj...(3.6) y, x x x Μ xn...(3.7)

85 f f( xˆ, xˆ, Κ, xn ˆ ) f ( xˆ, xˆ, Κ, xn ˆ ) Μ fn( xˆ, xˆ, Κ, xn ˆ )...(3.8) Las dervadas parcales pueden evaluarse con una aproxmacón de dferenca, de la forma sguente: f f( xˆ, Κ, xj ˆ + σ xj, Κ, xn ˆ ) f(ˆ, x Κ, xj ˆ, Κxn ˆ ) xj σxj...(3.9) donde σxj es un valor pequeño elegdo arbtraramente. Una vez lnealzado el sstema de ecuacones, queda por resolver el sstema de ecuacones sobredetermnado, para esto se utlza algún método teratvo como es el método de Gauss, de Jacob o de Moore-Penrose. Este últmo, hace uso de la matrz nversa generalzada, algunas veces llamada matrz pseudonversa. La ventaja de utlzar el método de Moore-Penrose sobre otros, es que este dervará en la solucón que sgnfque la menor de la norma de las posbles solucones [37], [4]. La solucón que se obtene depende de la correcta mplementacón de ambos métodos y de la aproxmacón ncal que se determne para su solucón, esto para la cnemátca nversa de poscón. Para el caso del manpulador robótco Movemaster, las ecuacones de dseño a lnealzar se plantean de la sguente manera:

86 F cos cos 34 cos5 sensen5 nx F sen cos34 cos5 + cossen5 ny F 3 sen 34 cos5 nz F4 cos cos34sen5 sen cos5 sx F + 5 senc34sen5 cos cos5 sy F sen sen s z F7 cossen 34 ax F sen sen a 8 34 y F9 cos 34 az F 7cos sen + 6cos cos + 5cos cos p x F 7sen sen + 6sen cos + 5sen cos p y F 7cos34 + 6sen 3 + 5sen + 3 pz... (3.) Para las dervadas parcales que se ndcan en la ecuacón 3.9, el valor de σ xj es un pequeño ncremento elegdo arbtraramente, que en la funcón Newton (Anexo ) es llamada damp. Una vez lnealzado el sstema de ecuacones, se resuelve el sstema de ecuacones sobredetermnado utlzando el comando pnv, que solucona el problema sobredetermnado utlzando el método de Moore-Penrose. Consderando lo anteror, el lstado del programa para resolver el problema cnemátco nverso, hacendo uso del método de Newton, queda como se muestra en la funcón Newton en el Anexo, dcha funcón resuelve la cnemátca nversa a partr de una matrz de entrada [M], que contene la poscón y orentacón deseados del efector fnal y de valores ncales para las ncógntas... 5, de donde comenzará la funcón en el método teratvo de Newton a lnealzar el sstema de ecuacones no lneales (3.), en el cual habrá convergenca cuando las funcones F a F lleguen a un valor que pueda consderarse como cero. Una vez lnealzado el sstema de ecuacones, numércamente resolvemos el sstema

87 sobredetermnado obtenendo los valores de los ángulos de las artculacones para la poscón deseada en el vector de salda [t]. El dagrama de flujo de la funcón Newton se muestra en la Fgura 3.8. Fgura 3.8 Dagrama de Flujo de la Funcón Newton. 3.3 Planteamento del Problema Dnámco. La obtencón del modelo dnámco de un manpulador robótco proveerá de las expresones sufcentes para establecer las leyes de control del sstema, así como para el cálculo del torque requerdo para que el manpulador cumpla con las condcones de movmento deseadas. A partr de la solucón a la cnemátca nversa para poscones, velocdades y aceleracones, se obtenen los parámetros necesaros para evaluar el comportamento dnámco de este.

88 3.3. Obtencón del Modelo Dnámco del Manpulador. Para la obtencón del modelo dnámco del manpulador se utlza la formulacón de Lagrange. La aplcacón de esta, en conjunto con la representacón de coordenadas de elementos de Denavt y Hartenberg, resulta en una descrpcón algorítmca convenente y compacta de las ecuacones de movmento del manpulador [5]. Como se establecó prevamente, la formulacón de Lagrange requere del conocmento de la energía cnétca del sstema físco, lo que a su vez requere del conocmento de la velocdad de cada artculacón con respecto a la base del manpulador. Para manpuladores de más de dos grados de lbertad, resulta un tanto dfícl establecer el modelo dnámco como se planteó en el capítulo anteror. Se recomenda aquí el utlzar la dervacón de la formulacón de Lagrange como lo sugeren (Fu et al.) en [5], que se presenta a contnuacón. Ya que la mplementacón de esta en Matlab resulta de manera natural. Fgura 3.9 Punto r en el sstema de coordenadas de la artculacón.

89 S establecemos un punto cuyo vector de poscón, expresado en el sstema de coordenadas de la artculacón r (como se muestra en la fgura 3.9) y como el nterés se concentra en el sstema de coordenadas de la base, se establece que: ( ) ( ) o r A dt d r dt d v v r A A r A A r A A A r A A A & & & & ya que j j j r r q q A & &...(3.) La dervada parcal de A con respecto a j q se calcula fáclmente con la ayuda de la matrz Q, que para una artculacón de revolucón se defne como: Q...(3.) y para una artculacón prsmátca, como: Q...(3.3) de manera que: A Q q A...(3.4)

90 de aquí que, para n...,,,. > j para j para j j j j j j A A Q A A A q A..... (3.5) La ecuacón (3.5) se puede nterpretar como el efecto del movmento de la artculacón j, sobre todos los puntos del elemento. Con el fn de establecer una notacón smplfcada y compacta se defne, > j para j para... j j j j j A Q A q A U...(3.6) Entonces, la expresón correspondente para la velocdad en el elemento, se puede expresar como: j j r Ujq v &...(3.7) Lo anteror es váldo úncamente para un elemento, pero debdo a que se trata de una cadena eslabonada, es necesaro determnar el efecto en la velocdad, dervado de la nteraccón de las artculacones, de la sguente manera: < < k j k j A Q A Q A j k A Q A Q A U q U j j j k k k k k k j j j jk k j ó...(3.8) Una vez planteada la formulacón para velocdades en el sstema, es necesaro calcular la energía cnétca total del manpulador, la cual esta dada por la sguente expresón:

91 K n K Traza p r n n T [ Traza( U p J ju q& pq& r )] r p r U p J j U T r q& pq& r...(3.9) en donde J j es la matrz correspondente a los momentos de nerca de las seccones de los eslabones del manpulador. La energía potencal total del manpulador estará dada por: P n n P [ m g( A )] rˆ...(3.) en donde g es un vector fla de gravedad expresado en el sstema de coordenadas de la base. Para un sstema de nvel, g (,, - g, ). Una vez determnadas las energías cnétca y potencal del sstema, se pueden establecer las ecuacones de movmento del manpulador. Ya que la funcón Lagrangana del sstema está dada por L K P, tenemos L n n T [ Traza( U p J ju r q& pqr )] + [ m g( A rˆ )] & p r...(3.) de manera tal que el par generalzado τ dado por la formulacón de Lagrange es: τ d dt n j k L L q & q j Tr n j j ( U J U ) q& + Tr( U J U ) jk j T j k n T jkm j j q& kq& m j k m j m jq U j j rˆ j...(3.)

92 para,,..., n. La Ecuacón anteror se puede expresar en notacón matrcal como: n n n j τ Dk q& k + hkmq& k q& m + c,,..., n k k m m...(3.3) o de forma matrcal como: ( ( t) ) q( t) + h( q( t), q( t) ) c( q( )) τ ( t ) D q & & + t...(3.4) 3.3. Planteamento de la solucón a la Dnámca del Manpulador. Una vez conocdos los valores de poscón, velocdad y aceleracón del sstema, estos se susttuyen en la formulacón dnámca del manpulador. Para esto se desarrollaron los lstados num, num y num3 (ver Anexo ) que emplean los datos dervados de la solucón a la cnemátca nversa, para la solucón numérca de la dnámca del robot, al conjunto de lstados les corresponde el dagrama de flujo que se muestra en la fgura 3.. Fgura 3. Dagrama de Flujo para la Funcón num.

93 Han sdo defndos, hasta este punto, los modelos matemátcos que gobernan el movmento del manpulador Mtsubsh Movemaster y se han desarrollado las herramentas de cómputo necesaras para la solucón de estas en MATlab. En el sguente capítulo se plantea el uso de estos modelos matemátcos en la generacón de trayectoras de manpuladores robótcos, con el objeto de poder obtener la nformacón necesara que permta realzar una programacón fuera de línea.

94 Generacón de Trayectoras. E n é s t e c a p

95 4. Generacón de Trayectoras. De manera general, el objetvo de un manpulador robótco es que sga una trayectora deseada, con la fnaldad de realzar una tarea determnada. Para esto se habrán de defnr los puntos que conforman dcha trayectora. Hoy en día, la mayoría de las ndustras naconales que cuentan con robots nstalados, tenen el problema de que, para ndcar una nueva trayectora a segur por el manpulador, es necesaro detener la línea de produccón, para que un grupo de técncos localcen los puntos de manera gestual sobre una plantlla de tamaño real, o sobre un prototpo. Lo anteror se puede aceptar y no sería un problema s: a) a) La trayectora pudera defnrse con pocos puntos, y b) b) S no se requere de una gran precsón para defnr dchos puntos. Ya que las condcones planteadas en a) y b), por lo general no se dan, es necesaro, para localzar y grabar los puntos de una nueva trayectora, hacerlo de manera gestual, para lo cual se requere un perodo mportante de tempo para poder defnr los puntos y afnar la localzacón de estos. Lo que se propone en este trabajo, es un método para poder obtener los parámetros necesaros que permtan realzar una programacón fuera de línea. A partr de la solucón a la cnemátca nversa (para un robot comercal) y de la solucón a la dnámca (c.. y dnámca para prototpos de manpuladores), se podrá contar con la nformacón necesara para defnr la orentacón y poscón en cada uno de los puntos que conforman la trayectora deseada. Esto es, s el robot habrá de segur una trayectora deseada durante un perodo de tempo T, esta estará defnda, para un nstante (t), por los sguentes parámetros: poscón, orentacón, velocdad y aceleracón del efector fnal.

96 S se desea que el manpulador pase de un punto A a un punto B dentro de una trayectora, no exste nngún elemento que asegure que la trayectora descrta por el efector fnal sea una línea recta, ya que por la confguracón de cadena aberta del manpulador y por la naturaleza del control, las dferentes artculacones de este, se moverán de manera tal que estas converjan en la poscón angular deseada, lo que da como resultado la poscón y orentacón angular deseadas en el espaco cartesano. Por lo que el efector fnal descrbrá una trayectora en base a los desplazamentos de las artculacones, lo que no necesaramente es una línea recta (Fgura 4.). Fgura 4. Trayectora del Efector fnal entre dos puntos En algunos casos, la trayectora que sga el efector fnal entre los puntos A y B, puede no ser de gran nterés, pero s exste algún objeto u obstáculo con el que el manpulador pudese colsonar, o ben, el proceso que realza este, exge que la trayectora a segur sea una línea determnada, entonces se defnrán tantos puntos como se consdere necesaro para que efectvamente el efector fnal del manpulador descrba la trayectora deseada entre los dos puntos. S deseamos que el efector fnal descrba una trayectora crcular, entonces tambén tendremos que especfcar un número sufcente de puntos que, al momento de unrlos por la trayectora que descrba el efector fnal, correspondan a la crcunferenca deseada. 4. Metodología para la generacón de trayectoras. Los pasos a segur para la generacón de trayectoras son los sguentes:

97 .. Parametrzar la trayectora. Identfcar los puntos donde exsta un cambo de dreccón en la trayectora a segur (lgaduras de camno) y establecer las orentacones del efector fnal que se desean durante esta... Realzar el análss cnemátco de la trayectora para obtener los vectores de poscones angulares que satsfacen las condcones establecdas por las lgaduras de camno Realzar el análss dnámco, que ncluye el determnar los vectores de velocdades y aceleracones angulares correspondentes a las condcones de movmento deseadas para la trayectora, con el propósto de determnar los vectores de torque en las artculacones, que satsfacen la dnámca de la trayectora. Para lograr lo anteror, en el tercer capítulo, ya fueron defndos los modelos cnemátcos que gobernan el movmento del manpulador, así, habrán de defnrse las orentacones y poscones que conformarán la trayectora a descrbr y por medo de la solucón a la cnemátca nversa del manpulador, se calculan los vectores de poscones angulares para las artculacones que habrán de satsfacerlos. Por lo cual se requere realzar una parametrzacón de trayectora. 4. Parametrzacón de Trayectoras. El establecmento de parámetros que defnen la trayectora a segur por un manpulador, contempla: () () La dentfcacón de aquellos puntos en donde exste un cambo de dreccón en la trayectora a segur por el manpulador, referdas como lgaduras de camno [9], [5], [35]; y

98 () () La orentacón del efector fnal, que se desea mantener a lo largo de la trayectora. Para lo anteror, es necesaro conocer con precsón la ubcacón de los elementos que consttuyen el entorno de trabajo del robot, con el fn de poder relaconar drectamente, la tarea a desempeñar con el sstema de coordenadas de la base del manpulador (Fgura 4.). Fgura 4. Sstemas de coordenadas del robot y de objetos en su entorno La nformacón que se derve de la defncón de los puntos y orentacones deseadas para el efector fnal que conformarán la trayectora, será utlzada posterormente, como valores de entrada a los programas de solucón de la Cnemátca Inversa y Dnámca del manpulador, para determnar así las confguracones espacales del manpulador que satsfagan las poscones y orentacones que, a su vez defnan la trayectora deseada, defnendo asmsmo los vectores de fuerzas o torques a aplcar, para que se cumplan las condcones dnámcas de movmento de la trayectora. Con el objeto de lustrar lo anteror, supóngase que la tarea a desempeñar por el manpulador robótco consste en colocar un cordón de soldadura sobre el contorno marcado en la fgura 4.3 por la línea punteada.

99 Fgura 4.3 Ruta a segur por el manpulador. El prmer paso es determnar las lgaduras de camno que habrán de defnr.,de manera general, los puntos que conforman a la trayectora a segur (Fgura 4.4). Fgura 4.4 Defncón de las lgaduras de camno.

100 El segundo paso es relaconar estas lgaduras de camno (puntos a...h), con el sstema de coordenadas de la base, partendo de la suposcón que el vector de poscón de a, respecto al sstema de coordenadas de la base del manpulador, es a[,, 5, ] T. Así, que las demás lgaduras de camno estarán defndas por: b, c, d, 5-5 e, 5 f 3, g, h 5 Una vez determnados los vectores de poscón de las lgaduras de camno, el tercer paso consste en establecer la orentacón deseada del efector fnal durante la trayectora a segur, Fgura 4.5. Fgura 4.5 Establecmento de la orentacón deseada durante la trayectora.

101 Como al efector fnal le corresponde un sstema de coordenadas que determna su orentacón con respecto a la base del robot, se defnen las matrces de transformacón homogénea que corresponden a la trayectora (poscón de las lgaduras de camno y orentacón del efector fnal en estas), Fgura 4.6. Fgura 4.6 Identfcacón de las orentacones deseadas., 5 3 f, 5 3 e, d, 5 5 5, 5, 5 c b a h, g

102 Analzando la fgura 4.4, se observa que la trayectora a segur se compone por un conjunto de trayectoras parcales, que se defnen de la sguente manera:.. Trayectora ab recta.. Trayectora bc arco ¼ de crcunferenca Trayectora cd recta Trayectora de arco ¼ de crcunferenca Trayectora ef recta Trayectora fg arco ¼ de crcunferenca Trayectora gh recta Trayectora ha arco ¼ de crcunferenca De lo anteror se puede tomar, como representatvo para el análss, al segmento de trayectora abc. Para que la trayectora parcal de ab sea efectvamente una línea recta, es necesaro defnr un numero sufcente de puntos ntermedos (el número de puntos depende de la precsón que se requera dependendo de la tarea a desempeñar), para que la trayectora parcal ab del efector fnal descrba la línea deseada. De esta manera, en el momento en que el efector fnal del manpulador satsfaga los puntos (A, A, A,..., A n, B), la trayectora descrta por este, corresponderá con lo esperado. Así que, para el ejemplo, se defnen puntos en esta trayectora parcal, estos estarían defndos por: A, 5 A 6, 5 A, 5

103 , 5 B, 5 6 A, 5 A, 5 8 A, 5 4, 5 A, 5 4 A, A A Como se observa, la orentacón del efector se mantene en todo momento, en tanto que, la magntud en x se ncrementa, satsfacendo la ecuacón de la recta. Para el caso de la trayectora parcal bc que se desea descrba un arco de rado 5mm, es necesaro defnr de gual manera, las lgaduras de camno parcales, necesaras para que el efector fnal sga un arco de crcunferenca. De manera tal, se establecen puntos para defnr esta trayectora parcal (en general se habrán de defnr un mayor número de puntos para descrbr una trayectora crcular), se hace el sguente análss: Se determna los desplazamentos X y y, ya que en z el valor se mantene constante, a partr del punto b hasta el punto c. Como se muestra en la fgura 4.7.

104 Fgura 4.7 Análss de la Trayectora Parcal bc. En donde: y X Y r sen r r cos Se determna la varacón angular en la orentacón, para el sstema de coordenadas de efector fnal durante la trayectora parcal, hacendo el análss que se muestra en la fgura 4.8. Fgura 4.8 Análss de varacón angular en la orentacón.

105 De donde se obtene la matrz de varacón en coordenadas homogéneas: b n sen cos cos sen r sen + r - r cos 5 Con esta relacón se pueden determnar tantos puntos como se desee para defnr la trayectora parcal. Para este caso, se establecen puntos para la defncón de la trayectora, los cuales se generan de manera automátca con la funcón traycrc (Anexo) desarrollada en Matlab, de donde se obtenen los sguentes puntos: b 5, b b b b b b b b b

106 c Como se observa, los desplazamentos del vector de poscón satsfacen la ecuacón de una crcunferenca, en tanto que en el componente de orentacón, esta se defne para cada una de las poscón, de manera tal que el efector realza una trayectora suave a lo largo de un cuarto de crcunferenca. 4.3 Cnemátca de la generacón de trayectoras. Una vez obtenda la nformacón necesara que defne la trayectora, se realza el análss cnemátco de la trayectora, en este, se calculan los vectores de poscones angulares que satsfagan las condcones determnadas por las matrces de transformacón homogéneas, que corresponden a las poscones que defnen la trayectora. Estas matrces, se almentan como valor de entrada al programa Newton para resolver la cnemátca nversa para cada una de las poscones. Para la trayectora abc de la fgura 4.4, que ha sdo defnda por matrces de transformacón homogénea, se obtenen los sguentes vectores de poscón angular en grados a partr de la solucón a la cnemátca nversa del manpulador:

107 b b b b b b a a a a a a a a a a

108 b b b b c Estos vectores representan las poscones angulares que satsfacen las condcones de poscón y orentacón del efector fnal para la trayectora abc. Fgura 4.9 Análss cnemátco trayectora abc

109 Con el propósto de comprobar los resultados dervados del análss cnemátco, se modeló el manpulador robótco Mtsubsh Movemaster, hacendo uso de herramentas de modelado en realdad vrtual, cuyo lstado se ncluye en el Anexo. Dervado de este modelado, se obtenen los gráfcos correspondentes a las confguracones del brazo, como resultado de las poscones angulares que satsfacen la poscón y orentacón deseadas para el efector fnal. En la fguras 4. a 4.3 se muestran las confguracones resultantes para los puntos a, a, b 9 y c, desde tres dferentes perspectvas, frontal, lateral derecha y superor. (Los gráfcos correspondentes para el resto de las poscones se ncluyen en el Anexo 3). Otra forma de comprobar la veracdad de los resultados es la sguente: se sabe que en todo momento el ángulo que exste entre z y z 5 es de 8º, y que este está dado por la suma de los ángulos theta, theta3 y theta4, lo cual corresponde en todo momento al realzar la suma de estos ángulos. Fgura 4. Confguracón resultante correspondente al punto a.

110 Fgura 4. Confguracón resultante correspondente al punto a. Fgura 4. Confguracón resultante correspondente al punto b 9.

111 Fgura 4.3 Confguracón resultante correspondente al punto b. Para las demás trayectoras parcales, se hace un análss smlar con el fn de determnar las lgaduras de camno necesaras, para que el efector fnal del manpulador descrba de manera fel la trayectora prncpal deseada. 4.4 Dnámca en la generacón de trayectoras. Cuando un manpulador descrbe una trayectora, es mportante el consderar el comportamento dnámco que se desea presente el manpulador. Este comportamento estará defndo por los vectores de velocdad y aceleracón que afectarán el movmento del efector fnal a lo largo de su trayectora, además de las característcas de masas y momentos de nerca del propo manpulador. Los vectores de velocdades y aceleracones angulares que afectan al sstema para el ejemplo que se plantea, se obtenen tomando en cuenta las sguentes consderacones:

112 La longtud total consderada para el análss es la que comprende la tayectora abc, que es de mm. El tempo en el que se desea recorrer esta trayectora es de 3 mn. (que es el tempo que un soldador calfcado le tomaría realzar la tarea). Tanto la velocdad ncal, como la fnal del sstema es de m/s. Ya que la trayectora abc (fgura 4.4) está compuesta por dos trayectoras parcales, ab y bc, se determna el tempo en el que se habrán de cubrr cada una de estas trayectoras parcales de la sguente manera: la trayectora abc está defnda por un total de puntos, de los cuales, los prmeros puntos defnen la trayectora parcal ab, con una longtud total de 4mm; y los sguentes puntos, defnen la trayectora parcal bc con una longtud total de 96.35mm. Con esta nformacón es posble establecer una relacón que permta determnar el tempo que corresponde para descrbr cada una de las trayectoras parcales. De manera tal que, para la trayectora parcal ab tenemos: mm 3mn 4.mm x x.mn.7345seg como esta trayectora está defnda por puntos, el ntervalo de tempo entre poscones es de.7345seg. Consderando lo anteror, el tempo correspondente para descrbr la trayectora bc es de.9878mn, 59.68seg. Como esta trayectora esta defnda por puntos, el ntervalo de tempo entre puntos es de 5.388seg. Como la velocdad es la razón de cambo de poscón en el tempo, se obtene el gradente entre poscones

113 angulares y se dvde entre el ntervalo de tempo, obtenendo así las velocdades angulares para el sstema que se muestran en la tabla 4.. Para las aceleracones, tomando en cuenta que esta es la razón de cambo de la velocdad en el tempo, se obtenen los gradentes de velocdad y se dvde nuevamente entre el ntervalo de tempo, obtenendo así, las aceleracones angulares del sstema para las condcones de movmento deseadas (Tabla 4.). En las fguras 4.4 y 4.5, se observa gráfcamente el comportamento de estos parámetros para cada una de las artculacones. Tabla 4. Velocdades angulares (rad/seg) omega omega omega3 omega4 omega E

114 Tabla 4. Aceleracones angulares (rad/seg^) alfa alfa alfa3 alfa4 alfa E E E E E E E E E E E E E E-5 6.9E E E E E E E E E E-5.556E E E E E

115 Fgura 4.4 Velocdades angulares en las artculacones (rad/s)

116 Fgura 4.5 Aceleracones en las artculacones (rad/s^)

117 Con la nformacón de velocdades y aceleracones se pueden obtener las magntudes de los torques a aplcar en las artculacones para las condcones de movmento especfcadas. Para la solucón a la dnámca del manpulador se utlzan los lstados num, num y num3 descrtos en el capítulo 3. Para la solucón numérca empleando estos lstados, es necesaro ndcar como valores de entrada los datos correspondentes para las poscones, velocdades y aceleracones angulares para el nstante de nterés. Los torques necesaros para que el manpulador descrba la trayectora abc se tabulan en la tabla 4.3 y se muestran gráfcamente en las fguras 4.6, 4.7, 4.8, 4.9 y 4.. Tabla 4.3 Torques aplcados en las artculacones para generar la trayectora abc (N-m) Torque Torque Torque 3 Torque 4 Torque

118 Fgura 4.6 Torque artculacón Fgura 4.7 Torque artculacón

119 Fgura 4.8 Torque artculacón 3 Fgura 4.9 Torque artculacón 4

120 Fgura 4. Torque artculacón 5 Con este análss se puede determnar el valor nomnal máxmo del torque necesaro en las artculacones, para que el manpulador sga la trayectora deseada. Sobre la base de este análss y modfcando las condcones de movmento, es posble determnar el valor de torque máxmo para la seleccón de los elementos actuadores, que habrán de anmar al manpulador. Dervado del análss anteror, se puede observar que los valores para los torques y 3 son sgnfcatvamente mayores en su magntud, que aquellos para el resto de las artculacones. Esto obedece a que las artculacones y 3 expermentan mayores desplazamentos, además de que por la confguracón espacal del mecansmo del manpulador, son los nodos en donde exste una mayor reaccón como resultado de las cargas en los elementos. En tanto que en el resto de las artculacones, los valores de pares a aplcar son menores, en comparacón con los dos menconados. Es tambén de nterés los pcos que se observan en la demanda de torque, los cuales aparecen cuando las artculacones del manpulador tenen que cubrr un mayor desplazamento para llegar a la sguente poscón, por lo que se observa

121 una correspondenca con los pcos que se observan en las gráfcas de velocdades. S se consdera ahora una carga a manpular en el efector fnal de.5kg, y se analza la msma trayectora bajo el msmo patrón de velocdades y aceleracones se obtenen los sguentes valores de torques a ser aplcados por los elementos actuadores en las artculacones del robot, Tabla 4.4: Tabla Torques consderando una carga de.5 k.g en el efector fnal (N-m) Torque '' Torque '' Torque 3'' Torque 4'' Torque 5'' Para consderar esta carga adconal en la formulacón de Lagrange solamente se modfca la masa y el centro de gravedad que se especfca para el últmo sstema de coordenadas, el cual corresponde al efector fnal. En el lstado num se modfca el valor de masa, en tanto que en el lstado num3 se modfca el valor del vector de poscón del centro de masa de este.

122 A contnuacón se realza un análss comparatvo entre los torques obtendos para ambas condcones de carga, como se muestra en las grafcas de las fguras 4. a 4.5. Fgura 4. Comparacón t vs t Fgura 4. Comparacón t vs t

123 Fgura 4.3 Comparacón t3 vs t3 Fgura 4.4 Comparacón de t4 vs t4

124 Fgura 4.5 Comparacón t5 vs t5 De las gráfcas anterores se observa que entre los torques calculados ncalmente y aquellos obtendos consderando una carga en el efector fnal de.5kg, exste una correspondenca cualtatva en cuanto al comportamento de los torques a aplcar en las artculacones, en tanto que no exste una correspondenca cuanttatva, pues es necesara una mayor demanda de torque en los actuadores del robot para manpular una carga mayor. De manera general, se puede decr que al segur la metodología presentada en este capítulo, se obtene claramente la nformacón necesara para la generacón de trayectoras de manpuladores robótcos y, es posble pensar en la mplementacón de ésta en un prototpo real, desde el punto de vsta expermental, como se presenta en el sguente capítulo.

125 Desarrollo expermental dervado de este trabajo. En éste capít ulo se pres enta el traba

126 5. Desarrollo Expermental Dervado de este Trabajo. Dervado de este trabajo se han desarrollado un total de proyectos de dseño de prototpos de manpuladores robótcos para dversas aplcacones, buscando con esto alentar la nvestgacón sobre este tema en jóvenes ngeneros y como parte del trabajo desarrollado como profesor de las materas Sstemas Dgtales II (Mcroprocesadores) y Sstemas Dgtales III (Robótca) a nvel lcencatura en la carrera de Ingenería en Control y Automatzacón de la ESIME - Zacatenco. De estos, el que más avance presenta es el proyecto de Implementacón de las Técncas de Control PID en un Manpulador Robótco de Tres Grados de Lbertad, que a contnuacón se descrbe de manera más ampla. Es mportante menconar que los costos dervados de este proyecto se han fnancado hacendo uso de las becas de CONACYT y PIFI. El costo total aproxmado hasta el momento es de $9,.. 5. Proyecto de Implementacón de las Técncas de Control PID en un Manpulador Robótco de Tres Grados de Lbertad Los objetvos de este proyecto son: Dseño y construccón de un manpulador robótco de 3 grados de lbertad Solucón al modelo matemátco del manpulador Implementacón de la técnca de control PID para comprobar el modelo matemátco del manpulador Identfcacón y análss de las dferentes curvas de respuesta que presenta el control P, PI, PD Y PID. Para lo cual, este se ha dvddo en 5 etapas: Dseño y desarrollo mecánco.

127 Modelado matemátco y solucón de las ecuacones. Programacón fuera de línea. Crcuto de potenca. Control. El manpulador robótco consta de tres movmentos (grados de lbertad) en una confguracón denomna Brazo Artculado, puesto que las artculacones son gratoras. Cada una de las artculacones cuenta con un sensor (encoder) de tpo absoluto, que representa el ángulo de poscón de un eslabón con respecto al eslabón anteror. La funcón a desempeñar del manpulador, es la de generar una trayectora en el espaco, para llevar un objeto de un extremo de la base a otro, tenendo pleno control sobre las artculacones por medo de la técnca PID e dentfcando las dferentes respuestas que presenta esta técnca. En tanto que los parámetros para la generacón de trayectoras se obtenen de la solucón a la cnemátca nversa, comprobando dchos parámetros en un prncpo medante el modelado vrtual del manpulador, para verfcar que los resultados son los esperados y posterormente almentando los datos de la trayectora a la base de datos del control del manpulador, para generar así la trayectora deseada por el manpulador real. En un prncpo se determna el modelo matemátco correspondente al manpulador, partendo de la obtencón de los parámetros de Denavt y Hartenberg necesaros para defnr la poscón y orentacón del efector fnal del manpulador con respecto a la base. Ya que se tene el modelo matemátco del manpulador, se resuelven las ecuacones de dseño del manpulador para dar solucón a la cnemátca drecta.

128 Posterormente, las ecuacones de dseño son lnealzadas por medo del método de Newton, para obtener la solucón a la cnemátca nversa de este manpulador, lo cual conduce a encontrar los ángulos que satsfacen una poscón y orentacón fnal deseada. La solucón a la Cnemátca Drecta e Inversa corresponde a la programacón fuera de línea, es decr, se realza con ayuda de una PC para resolver las ecuacones y obtener los ángulos de cada artculacón que descrben una trayectora deseada. Los ángulos obtendos en la programacón fuera de línea, son sumnstrados a un Mcro controlador para reproducr la trayectora; el Mcro Controlador cuenta con un algortmo de control PID el cual se encarga de evtar que las artculacones se salgan de la poscón deseada. El algortmo de control PID es parte del desarrollo de este proyecto, este control precsa de la nformacón que otorga la retro-almentacón, para poder determnar la poscón angular de cada artculacón. Una vez que el Mcro Controlador ha realzado los cálculos necesaros del control, este nos arroja señales (en los pnes del puerto del mcrocontrolador) que nos representan la velocdad, arranque, paro y sentdo de gro de un motor de corrente drecta. Estas señales deben de ser acondconadas de tal forma que tengan la potenca necesara para mover el motor de CD; esto se logra con la etapa de potenca en donde se emplean crcutos en confguracón tpo H para controlar el gro del motor, así como la velocdad del msmo. 5.. Descrpcón físca El materal que conforma la estructura del manpulador es perfl rectangular de alumno, las guardas o fundas están hechas con lamna de alumno de mm de

129 espesor; el manpulador cuenta con 4 moto reductores, los cuales le dan movmento a cada artculacón. Los motores empleados para proporconar el movmento a cada uno de los eslabones son de -4 V de C.D. con un corrente nomnal de.8 amp y una corrente máxma de 3 amp. El prmer movmento corresponde a la base, el cual tene un rango de trabajo de 34, el sguente movmento es el del brazo superor cuyo rango de trabajo es de 6 con respecto de la base. El tercer movmento es el del antebrazo, su área de trabajo es de 8 con respecto del prmer eslabón. El GRIPER o efector fnal tene una capacdad de movmento de 8º; este, no es gobernado por nngún motor, sno por medo de la gravedad, es decr, con su msmo peso se alnea vertcalmente. Las característcas y dmensones se presentan detalladamente en las fguras 5. a 5.3. Fgura 5. Dmensones generales (en mm) del manpulador R3GL, vsta lateral.

130 Fgura 5. Dmensones generales (en mm) del manpulador R3GL, vsta superor Fgura 5.3 Dmensones generales (en mm) del manpulador R3GL, vsta frontal

131 5.. Modelado Matemátco y Solucón a la Cnemátca Drecta e Inversa. Tomando en cuenta las característcas físcas del manpulador robótco, se establecen los sstemas de coordenadas correspondentes a cada artculacón, de la manera en que se explcó en el capítulo, defnendo para esto una poscón ncal para la asgnacón de los sstemas de coordenadas (Fgura 5.4). Fgura 5.4 Poscón de sstema de coordenadas cero. Fgura 5.5 Asgnacón de sstemas de coordenadas. Una vez que se han determnado los sstemas de coordenadas para cada artculacón (Fgura 5.5), se obtenen los parámetros de Denavt-Hartenberg (Tabla 5.). Obtenendo las matrces: Tabla 5. Parametros de D-H para el robot R3GL ARTICULACION d a

132 A B AS C S AC S C S A CS C S CC S C A DS C S DC S C 3 A C S S C 4 A E C S S C de donde se derva la matrz A 5, que corresponde a la matrz del manpulador. A 5 ( ) ( ) ( ) B CS DS EC C S S C S AS CC DC ES S S S C C S C S C S C C S AC CC DC ES C C S C S S C C S S C C C Una vez obtenda esta matrz, se puede dar solucón tanto a la cnemátca drecta como a la nversa. Ejemplos de solucón de la cnemátca drecta. Poscón ncal Valores del Vector Varable en grados: VV[ 9 ] Para el que le corresponde la matrz [M] de orentacón y poscón del efector fnal:

133 M Fgura 5.6 Poscón ncal ( 9 ) Prmera poscón. Valores del Vector Varable VV[9 9 ] Para el que le corresponde la matrz [M] de orentacón y poscón del efector fnal: M Fgura 5.7 Poscón (9 9 )

134 Segunda poscón. Valores del Vector Varable: VV[ ] Para el que le corresponde la matrz [M] de orentacón y poscón del efector fnal: M Fgura 5.8 Poscón ( ) Tercera poscón. Valores del Vector Varable: VV[ ] Para el que le corresponde la matrz [M] de orentacón y poscón del efector fnal:

135 M Fgura 5.9 Poscón 3 ( ) Cuarta Poscón. Valores del Vector Varable: VV[ ] Para el que le corresponde la matrz [M] de orentacón y poscón del efector fnal: M

136 Fgura 5. Poscón 4 ( ) Qunta poscón. Valores del Vector Varable: VV[ ] Para el que le corresponde la matrz [M] de orentacón y poscón del efector fnal: M

137 Fgura 5. Poscón 5 ( ) 5..3 Implementacón de Técncas de Control para el robot R3GL. A contnuacón se descrben las técncas de control que se mplementan en el manpulador robótco R3GL Accón de control Proporconal Para un controlador con accón de control proporconal, la relacón entre la salda del controlador y la señal del error es: Salda del controlador Kp * Error Nota: La salda del controlador y la señal del error están en funcón del tempo. Cualquera que sea el mecansmo real y la forma de la potenca de operacón, el controlador proporconal es, en esenca, un amplfcador con gananca ajustable.

138 5..3. Accón de control Integral En un controlador de accón de control Integral, el valor de la salda del controlador se camba a una razón proporconal a la señal de error. u(t) K t e(t)dt... (5.) S se duplca el valor del error, el valor de la salda del controlador vara dos veces más rápdo. Para un error de cero, el valor de la salda del controlador permanece estaconaro. En ocasones, la accón del control ntegral se denomna control de reajuste (reset) Accón de control Proporconal-Integral La accón de control de un controlador Proporconal-Integral (PI) se defne medante: u( t) K p e( t) + K T p t e( t) dt... (5.) En donde Kp es la gananca proporconal y T, se denomna tempo ntegral. Tanto Kp como T son ajustables. El tempo ntegral ajusta la accón de control ntegral, mentras que un cambo en el valor de Kp afecta las partes ntegral y proporconal de la accón de control. Al nverso del tempo ntegral T se le denomna velocdad de reajuste, la cual corresponde a la cantdad de veces por mnuto que se duplca la parte proporconal de la accón de control. La velocdad de reajuste se mde en térmnos de repetcones por mnuto.

139 Fgura 5. Análss del comportamento del error e(t), y comportamento de la señal manpulada u(t) para un control PI Accón de control Proporconal-Dervatvo La accón de control de un controlador Proporconal-Dervatvo (PD) se defne medante: d u( t) K pe( t) + K ptd e( t) dt... (5.3) Donde Td es una constante denomnada tempo dervatvo. Tanto Kp como Td son ajustables. Esta accón es denomnada control de velocdad, ocurre donde la magntud de la salda del controlador es proporconal a la velocdad de cambo de la señal de error. El tempo dervatvo Td es el ntervalo de tempo durante el cual la accón de la velocdad hace avanzar el efecto de la accón de control proporconal; Por lo que el control dervatvo tene un carácter de prevsón pero tene la desventaja de que amplfca la señales de rudo y puede provocar un efecto de saturacón en el actuador. Fgura 5.3 Análss del comportamento del error e(t), y comportamento de la señal manpulada u(t) para un control PD

140 Accón de control Proporconal Integral Dervatvo La combnacón de una actuacón de control Proporconal, una accón de control Integral, y una accón de control Dervatvo se denomna control Proporconal Integral Dervatvo (PID). Esta accón combnada tene las ventajas de cada una de las tres accones de control ndvduales. La ecuacón del controlador con esta accón combnada se obtene medante: K p u( t) K p e( t) + e( t) dt + K T t p T d d dt e( t)... (5.4) Fgura 5.4 Análss del comportamento del error e(t), y comportamento de la señal manpulada u(t) para un control PID 5..4 Funcón de Transferenca de un Controlador PID Dgtal El prncpo básco del esquema de control PID es que actúa sobre la varable manpulada a través de una apropada combnacón de las tres accones de control. En stuacones donde muchas plantas se controlan drectamente medante una sola computadora dgtal, la mayoría de los lazos de control se pueden manpular medante esquemas de control PID.

141 La accón del control PID en controladores analógcos esta dada por la ecuacón 5.4, en donde e(t) es la entrada al controlador (señal del error actuante), u(t)es la salda del controlador (señal manpulada), Kp es la gananca proporconal, T es el tempo ntegral (o tempo de reajuste) y Td es el tempo dervatvo (o tempo de adelanto). Para obtener la Funcón de Transferenca al pulso del controlador PID dgtal se aplca la transformada de Laplace a la ecuacón 5.4, obtenendo: U ( s) K p E() s + + Td E( s) T s E( s)... (5.5) Aplcando a su vez la transformada Z a U(s) tenemos U ( z) K p E( z) + + Td E( z) z E( z) T z ( )... (5.6) y consderando a Kp (cte proporconal), Kp(/T)K (cte ntegral) y Kp*TdKd (cte dervatva), se obtene: M(k)Kp*e(k)+K*W(k)+Kd*V(k)... (5.7) En donde: W(k)W(k-)+T/(e(k)+e(k-)) V(k)[e(k)-e(k-)]/T Tperodo de muestreo eseñal de referenca señal de salda

142 La ecuacón 5.7 se toma como base para el desarrollo de los programas de mplementacón del algortmo de control PID dgtal, con el mcrocontrolador MC68HC. A contnuacón se presentan los dagramas de flujo para la mplementacón del PID. Fgura 5.5 Dagrama de Flujo para la Implementacón del PID Dgtal.

143 Fgura 5.6 Dagrama de flujo de las subrutnas RECTSU y RECTBA

144 Fgura 5.7 Dagrama de Flujo de la Subrutna PID.

145 I Confguracon del puerto C como saldas Carga un dato de FFFF en la localdad PWM. (PWM) - TAlto STALTO (PWM) - TBajo #$FF - PC #$FE - PC PC PC STBAJO ST ALTO ST BAJO (T ALTO) IX (T BAJO) IX (IX)- (IX)-? NO? NO SI SI R R Fgura 5.7 Dagramas de Flujo para la Generacón del PWM

146 5..5 Descrpcón del Sstema de Control del Manpulador Robótco R3GL. Para controlar el movmento de las artculacones del manpulador y tomando en cuenta que se gobernan 3 artculacones, es necesaro manejar, en tres mcrocontroladores, el algortmo de control PID, uno por cada artculacón (los programas de mplementacón se ncluyen en el Anexo 4). El sstema de control está compuesto por 4 mcroprocesadores, los tres para las artculacones, que trabajan bajo la confguracón de esclavos y uno prncpal que trabaja en confguracón de Maestro, el cual habrá de ndcar a los esclavos las poscones deseadas para cada artculacón, de acuerdo al vector de poscón que corresponda a una orentacón y poscón deseadas del efector fnal. Una vez que las tres artculacones han llegado a la poscón deseada, estas mandan una señal de confrmacón al Maestro, para que se proceda con la carga de datos de la nueva poscón que se desea del manpulador. En la fgura 5.8 se muestra, de manera esquemátca, el sstema de control del manpulador robótco, en esta se ncluye una computadora que tene como funcón en el sstema el cálculo de parámetros fuera de línea (Los dagramas de conexón se ncluyen en el Anexo 5). GP MCS 68HC FA SP B U S BH M MCM 68HC NP B U S GP MCS 68HC SA SP B U S BH M GP MCS 68HC T A B U S BH M SP Fgura 5.8 Dagrama esquemátco del sstema de control del manpulador robótco R3GL

147 5..6 Estado actual del Proyecto de Investgacón. Actualmente, el manpulador robótco R3GL, puede generar trayectoras que han sdo calculadas fuera de línea, almentando al sstema de control úncamente la base de datos que conforma la trayectora deseada (vectores de poscón angular para la generacón de la trayectora). Hasta este punto, es posble pensar en nclur dentro del control del sstema, la técnca de control por par calculado, para poder comprobar los resultados dervados del análss dnámco de la trayectora (los torques calculados para condcones dnámcas de movmento). En tanto que lo referente a la solucón a la cnemátca drecta e nversa ha sdo plenamente resuelta y se puede comenzar a generar trayectoras especfcas para éste manpulador.

148 CONCLUSIONES

149 CONCLUSIONES. La metodología para la generacón de trayectoras que aquí se presenta, ha demostrado, hasta el momento, ser adecuada para el propósto que se persgue, que es su aplcacón en la programacón fuera de línea de manpuladores robótcos. Los resultados del análss cnemátco, que dervan en la determnacón de los vectores de poscón angular, a partr de la solucón a la cnemátca nversa, han demostrado ser correctos; toda vez que, en la smulacón vrtual del manpulador se observa que, efectvamente, éste genera la trayectora deseada. En cuanto a los resultados dervados del análss dnámco, estos muestran, para una msma trayectora de análss consderando dferentes condcones de carga, una correspondenca cualtatva, pues se conserva la tendenca de demanda de torque en las artculacones del manpulador. Mentras que en lo cuanttatvo, se observa una mayor demanda de torque en las artculacones, dervado de la consderacón de que la masa a manpular es mayor. Para un manpulador robótco comercal, basta con realzar un análss cnemátco de la trayectora, puesto que es muy dfícl que se pueda manpular el torque entregado a las artculacones por los motores, debdo a la confguracón del sstema de control del propo manpulador. Por lo que, solo basta el conocer las dmensones físcas de este, así como sus capacdades de movmento, para establecer el modelo matemátco que goberna el movmento del manpulador y, a partr de esto y de la tarea a desempeñar por este, obtener la nformacón necesara para la generacón de la trayectora deseada, sn necesdad de detener la línea de produccón dentro de la cual se encuentre nstalado el manpulador, reducendo con esto, los tempos muertos que se dervan de la programacón de un robot en línea.

150 En el área expermental, la metodología que aquí se presenta, puede aplcarse de manera completa, ya que la nformacón dervada del análss dnámco se emplea para el dseño y verfcacón de las técncas de control bajo las cuales habrá de regrse el comportamento dnámco del manpulador. El trabajo expermental que se ha desarrollado hasta el momento, ha permtdo verfcar hasta ahora, lo correspondente al análss cnemátco en un prototpo real. Pues las técncas de control utlzadas en este, contemplan solamente un control por poscón, en donde las varables velocdad, aceleracón y, por consguente, torque, tenen el carácter de varables secundaras. Ahora, una vez que ya se puede obtener la nformacón dervada del análss dnámco, que es el comportamento del torque aplcado en una artculacón a lo largo del tempo, es posble pensar en mgrar a esta varable, el torque aplcado, a la categoría de varable prncpal a controlar. Dado que la nformacón que se derva de un análss dnámco de un manpulador robótco, es de gran nterés para los dferentes actores que ntervenen en el dseño de un robot, como son: El ngenero mecánco, para el cual la magntud de los torques que actúan en las artculacones del robot, le permtrá evaluar el dseño estructural de este, así como de los elementos consderados para la transmsón de potenca mecánca, ya que estará en condcones de determnar los esfuerzos bajo los cuales se encuentran sometdos los elementos que consttuyen al manpulador. Y, para el ngenero electrónco, que dervado del análss dnámco, podrá mplementar técncas de control adecuadas para que el actuador pueda contrarrestar la nerca de los elementos en movmento para que se presente el comportamento dnámco del robot que se requere durante una tarea específca. Como trabajo futuro se recomenda realzar:

151 El modelado de un manpulador en ANSYS, con objeto de optmzar el dseño estructural de este, El modelado en ADAMS para comparar los resultados de una smulacón dnámca con aquellos que se obtenen a partr de los lstados que aquí se presentan para la solucón a la dnámca en MATlab, La generalzacón de la solucón que aquí se presenta para manpuladores de 6 grados de lbertad, la cual solamente consste en agregar algunos térmnos a las ecuacones que aquí se presentan, La optmzacón a los lstados de solucón a la Cnemátca y Dnámca para poder obtener resultados para dferentes manpuladores a partr de los parámetros de Denavt y Hartenberg, El mplementar un control por torque para manpuladores robótcos a partr de la nformacón que se obtene del análss dnámco para la generacón de trayectoras. Es mportante señalar que, el tema que aquí se aborda, ha demostrado ser de gran nterés, puesto que los avances de este trabajo se han estado publcando en la dreccón o ben, de la Internet; págna que ha sdo consultada hasta el momento, desde su fecha de publcacón en el mes de abrl de este año, en más de 3 ocasones.

152 REFERENCIAS [] Regh, J. Introducton to Robotcs n CIM Systems ; 3 rd Edton Prentce Hall, 997 [] Robot The Encyclopeda Brtannca, 6 th ed., 998 [3] Dunbar, R. Robotcs, past, present and future ; Aprl 6 th, [4] James, A.; Engelberger, J. Robot The Encyclopeda Amercana, 994 [5] Hstory of robotcs and robots [6] Automaton The Encyclopeda Brtannca: Macropeda 6 th ed., 998 [7] A short story of Robots [8] Development of the Industral Robot [9] McKerrow, P.J. Introducton to Robotcs Addson Wesley, 99 [] Robot Encyclopeda Encarta [] Carretero, J.A.; Nahon, M.; Bucklam, B.; Gosseln, C.M.: Knematc Analyss of a Three-DOF Mechansm for Telescope Applcatons Proceedngs of the 997 ASME Desgn Engneerng techncal Conference and Computers n Engneerng Conference September 4-7, 997, Sacramento, Calforna

153 [] Carretero, J.A.; Podorodesk, R.P.; Nahon, M.; Archtecture Optmzaton of a 3-DOF Parallel Mechansm Proceedngs of the 998 ASME Desgn Engneerng Techncal Conference 5 th Bennal Mechansm Conference September 3-6, 998, Atlanta, Georga [3] Notash, L.; Research Actvtes Report Queen s Unversty 999, Ontaro, Canada [4] Doty, K.L.; Knematc Analyss of the ARID Manpulator NASA, Summer faculty fellowshp program Kennedy Space Center Summer 99 [5] Fu, K.S.; Gonzalez, R.C.; Lee, C.S.G.; Robótca: Control, Deteccón, Vsón e Intelgenca McGraw Hll, 99 [6] Palacos Montufar, C. Merchán Cruz, E.A.; Notas de Clase [7] Crag, J.; Introducton to Robotcs: Mechancs and Control Addson Wesley, 989 [8] Baraff, D. Lnear Tme Dynamcs usng Lagrange Multplers Computer Graphcs Proceedngs, Annual Conference Seres, 996 Robotcs Insttute, Carnege Mellon Unversty August 4-9, 996, New Orleans [9] Murray, R.M.; L,Z.; Sastry, S.S.; A Mathematcal Introducton to Robotc Manpulaton CRC Press, Inc., 994 [] Agurre, H.; Modelacón y Smulacón de Sstemas Electromecáncos Bajo Control Tess de Maestra, SEPI ESIME 998 [] Parkng, R.E.; Appled Robotc Analyss Prentce Hall, 99 [] Corke, P.I.; Robotcs Toolbox Dvson of Manufacturng Technology Preston, Australa July 999

154 [3] Duffy, C.; Precsons on the Generalzed Inverses Theory Unversty of Florda 993 [4] Kupers, J.B.; Quaternons and Rotaton Sequences Prnceton Unversty Press Prnceton, New Jersey 999 [5] Wheeler, M.D.; Katsush, I. Iteratve Estmaton of Rotaton and Translaton usng the Quaternon School of Computer Scence Carnege Mellon Unversty Pttsburgh, PA December, 995 [6] Noyes, P. From Bt-Strng to Quaternons Stanford Lnear Accelerator Center Stanford Unversty Aprl, 99 [7] Joy, K. Quaternons Vsualzaton and Graphc Research Laboratory Department of Computer Scence Unversty of Calforna, Davs 994 [8] Paul, G.; Ikeuch, K. Representng the Moton of Objects usng Dual Quaternons and ts Applcatons Robotcs Insttute Carnege Mellon Unversty Pttsburgh, PA 997 [9] Goddard, J.S.; Abd, M.A. Pose and Moton Estmaton Usng Dual Quaternons Based Extended Kalman Flterng Oak Rdge Natonal Laboratory Unversty of Tennessee 996 [3] Shoham, M. A Note on Clfford s Dervaton of B-quaternons Tenth World Congress on the Theory of machne and Mechansms June, 999

155 [3] Ogata, K. Ingenería de Control Moderna Prentce-Hall 998 [3] Dorf, R. Modern Control Systems Unversty of Santa Clara 973 [33] Tou, J.T. Dgtal and Sampled-data Control Systems McGraw-Hll 96 [34] Nakamura, S. Análss Numérco y Vsualzacón Gráfca con Matlab Prentce-Hall 997 [35] Mason, M.T. Mechancs of Robotc Manpulaton M.I.T. 999 [36] Doty, K.L.; Schwartz, E.M. Spatal Freedom, Sngularty Surfaces and Workspaces of Seral Knematc Chans Machne Intellgence Laboratory Department of Electrcal Engneerng Unversty of Florda 99 [37] Doty, K.L. An Essay on the Applcaton of Weghted Generalzed Inverses n Robotcs Machne Intellgence Laboratory Department of Electrcal Engneerng Unversty of Florda 99 [38] Doty, K.L. Adjustments to the Theory on Sngularty Robust Inverses Machne Intellgence Laboratory Department of Electrcal Engneerng Unversty of Florda 99 [39] Xu, Y.; Nechyba, M. Fuzzy Inverse Knematc Mappng: Rule Generaton, Effcency, and Implementaton Carnege Mellon Unversty Pttsburgh, PA 998

156 [4] Wessten, E. Moore-Penrose Generalzed Matrx Inverse World of Mathematcs [4] Cheng, H.H.; Thompson, S. Programmng wth Dual Numbers and ts Applcatons n Mechansms Desgn Engneerng wth Computers Vol, No. 4, pp [4] Cheng, H.H.; Thompson, S. Dual Polynomals and Complex Numbers of Spatal Mechansms Proceedngs of the 996 ASME Desgn Engneerng Conference and Computers n Engneerng Conference. August 996 [43] Cheng, H.H.; Gonzalez, P. Development of a Software Lbrary Functons for Analyss of Spatal Mechansms Proceedngs of the 5 th Appled Mechansms and Robotcs Conference. Cncnnat, OH October, 997 [44] Hollerbach, J.M. Robotc Analyss Novembre, 999 [45] Brodsky, V.; Shoham, M. Dual Number Representaton of Rgd Body Dynamcs Department of Mechancal Engneerng Israel Insttute of Technology 996 [46] Cheng, H.; Gupta, K.C. Vectorzaton of Robot Inverse Dynamcs on a Ppelned Vector Processor IEEE Trans. on Robbotcs and Automaton Vol 9, No. 6, pp [47] Doty, K.; Englsh III, J.D. A three-pont velocty approach to robot moton control 4 th Internatonal Robot Board Capr, Italy 994 [48] Emrs, I. Force Closure Grasp of Hgh Qualty Advanced Robotcs Laboratory Robotcs Insttute Carnege Mellon Unversty 994

157 [49] Zefran, M.; Brdck, J. Desgn of Swtchng Controllers for Systems wth Changng Dynamcs IEEE Conference on Decson Control 998 [5] Ostrowsk, J.P.; The Mechancs and Control of UndulatoryRobotc Locomoton PhD.Thess Calforna Insttute of Technology 996 [5] Van der Smagt,P.; Vsual Robot Gudance usng Neural Networks PhD.Thess Unversty of Amsterdam 996 [5] Pertenece el IPN a la Red Mexcana de Robótca Gaceta Poltécnca IPN 5 de Marzo, [53] Secretara de Comerco y Fomento Industral [54] Díaz de León, V.M.; Metodología para el Dseño de un Robot Manpulador Industral Tess de Maestría SEPI-ESIME-IPN 993 [55] Ramro López, A.; Análss Cnemátco y Dnámco de un Manpulador Robótco con Tres Grados de Lbertad Tess de Maestría SEPI-ESIME-IPN 993 [56] Rojas Garnca, J.C.; Dseño de un Mecansmo Atornllador para un Brazo Manpulador Robótco Tess de Maestría SEPI-ESIME-IPN 997 [57] Gonzáles Sánchez, A. Desarrollo de un Smulador para el Control de un Brazo Robótco Tess de Maestría SEPI-ESIME-IPN 993

158 ANEXOS

159 ANEXO. Programas en Matlab functon [M]CDmov(VV,VC) % funcón desarrollada por Emmanuel Merchán % construye la matrz de un robot MINIMOVER % a partr de dos vectores de entrada % VV vector varable [theta... theta6] % VC vector constante [L L L3 L6] VVVV*p/8 t[vv() ]; t[vv() ]; t3[vv(3) ]; t4[vv(4) ]; t5[vv(5) ]; L[VC() ]; L[VC() ]; L3[VC(3) ]; L4[VC(4) ]; M(,)(cos(t)*cos(t5)*cos(t+t3+t4)-sn(t)*sn(t5)); M(,)(-cos(t+t3+t4)*sn(t5)*cos(t)-sn(t)*cos(t5)); M(,3)-cos(t)*sn(t+t3+t4); M(,4)- L4*cos(t)*sn(t+t3+t4)+L3*cos(t)*cos(t+t3)+L*cos(t)*cos(t); M(,)cos(t5)*sn(t)*cos(t+t3+t4)+cos(t)*sn(t5); M(,)-sn(t5)*sn(t)*cos(t+t3+t4)+cos(t)*cos(t5); M(,3)-sn(t)*sn(t+t3+t4); M(,4)sn(t)*(-L4*sn(t+t3+t4)+L3*cos(t+t3)+L*cos(t)); M(3,)cos(t5)*sn(t+t3+t4); M(3,)sn(t5)*sn(t+t3+t4); M(3,3)cos(t+t3+t4); M(3,4)L4*cos(t+t3+t4)+L3*sn(t+t3)+L*sn(t)+L; M(4,); M(4,); M(4,3); M(4,4);

160 functon [kk]newton(m,vv) %Funcón desarrollada por Emmanuel Merchán %Resuelve el problema cnemátco nverso a partr de la matrz de entrada [M] %que contene la orentacón del efector fnal con repecto a la base nx...az %y la poscón deseada px,py y pz. damp.; VC[ ]; %tnput('valor ncal de theta '); %tnput('valor ncal de theta '); %t3nput('valor ncal de theta3 '); %t4nput('valor ncal de theta4 '); %t5nput('valor ncal de theta5 '); tvv()*p/8; tvv()*p/8; t3vv(3)*p/8; t4vv(4)*p/8; t5vv(5)*p/8; for n: tpt+damp; tpt+damp; t3pt3+damp; t4pt4+damp; t5pt5+damp; J(,)(f(tp,t,t3,t4,t5,M)-(f(t,t,t3,t4,t5,M)))/damp; J(,)(f(t,tp,t3,t4,t5,M)-(f(t,t,t3,t4,t5,M)))/damp; J(,3)(f(t,t,t3p,t4,t5,M)-(f(t,t,t3,t4,t5,M)))/damp; J(,4)(f(t,t,t3,t4p,t5,M)-(f(t,t,t3,t4,t5,M)))/damp; J(,5)(f(t,t,t3,t4,t5p,M)-(f(t,t,t3,t4,t5,M)))/damp; J(,)(f(tp,t,t3,t4,t5,M)-f(t,t,t3,t4,t5,M))/damp; J(,)(f(t,tp,t3,t4,t5,M)-f(t,t,t3,t4,t5,M))/damp; J(,3)(f(t,t,t3p,t4,t5,M)-f(t,t,t3,t4,t5,M))/damp; J(,4)(f(t,t,t3,t4p,t5,M)-f(t,t,t3,t4,t5,M))/damp; J(,5)(f(t,t,t3,t4,t5p,M)-f(t,t,t3,t4,t5,M))/damp; J(3,)(f3(tp,t,t3,t4,t5,M)-f3(t,t,t3,t4,t5,M))/damp; J(3,)(f3(t,tp,t3,t4,t5,M)-f3(t,t,t3,t4,t5,M))/damp; J(3,3)(f3(t,t,t3p,t4,t5,M)-f3(t,t,t3,t4,t5,M))/damp; J(3,4)(f3(t,t,t3,t4p,t5,M)-f3(t,t,t3,t4,t5,M))/damp; J(3,5)(f3(t,t,t3,t4,t5p,M)-f3(t,t,t3,t4,t5,M))/damp; J(4,)(f4(tp,t,t3,t4,t5,M)-f4(t,t,t3,t4,t5,M))/damp; J(4,)(f4(t,tp,t3,t4,t5,M)-f4(t,t,t3,t4,t5,M))/damp; J(4,3)(f4(t,t,t3p,t4,t5,M)-f4(t,t,t3,t4,t5,M))/damp; J(4,4)(f4(t,t,t3,t4p,t5,M)-f4(t,t,t3,t4,t5,M))/damp; J(4,5)(f4(t,t,t3,t4,t5p,M)-f4(t,t,t3,t4,t5,M))/damp; J(5,)(f5(tp,t,t3,t4,t5,M)-f5(t,t,t3,t4,t5,M))/damp; J(5,)(f5(t,tp,t3,t4,t5,M)-f5(t,t,t3,t4,t5,M))/damp; J(5,3)(f5(t,t,t3p,t4,t5,M)-f5(t,t,t3,t4,t5,M))/damp; J(5,4)(f5(t,t,t3,t4p,t5,M)-f5(t,t,t3,t4,t5,M))/damp; J(5,5)(f5(t,t,t3,t4,t5p,M)-f5(t,t,t3,t4,t5,M))/damp; J(6,)(f6(tp,t,t3,t4,t5,M)-f6(t,t,t3,t4,t5,M))/damp; J(6,)(f6(t,tp,t3,t4,t5,M)-f6(t,t,t3,t4,t5,M))/damp; J(6,3)(f6(t,t,t3p,t4,t5,M)-f6(t,t,t3,t4,t5,M))/damp; J(6,4)(f6(t,t,t3,t4p,t5,M)-f6(t,t,t3,t4,t5,M))/damp;

161 J(6,5)(f6(t,t,t3,t4,t5p,M)-f6(t,t,t3,t4,t5,M))/damp; J(7,)(f7(tp,t,t3,t4,t5,M)-f7(t,t,t3,t4,t5,M))/damp; J(7,)(f7(t,tp,t3,t4,t5,M)-f7(t,t,t3,t4,t5,M))/damp; J(7,3)(f7(t,t,t3p,t4,t5,M)-f7(t,t,t3,t4,t5,M))/damp; J(7,4)(f7(t,t,t3,t4p,t5,M)-f7(t,t,t3,t4,t5,M))/damp; J(7,5)(f7(t,t,t3,t4,t5p,M)-f7(t,t,t3,t4,t5,M))/damp; J(8,)(f8(tp,t,t3,t4,t5,M)-f8(t,t,t3,t4,t5,M))/damp; J(8,)(f8(t,tp,t3,t4,t5,M)-f8(t,t,t3,t4,t5,M))/damp; J(8,3)(f8(t,t,t3p,t4,t5,M)-f8(t,t,t3,t4,t5,M))/damp; J(8,4)(f8(t,t,t3,t4p,t5,M)-f8(t,t,t3,t4,t5,M))/damp; J(8,5)(f8(t,t,t3,t4,t5p,M)-f8(t,t,t3,t4,t5,M))/damp; J(9,)(f9(tp,t,t3,t4,t5,M)-f9(t,t,t3,t4,t5,M))/damp; J(9,)(f9(t,tp,t3,t4,t5,M)-f9(t,t,t3,t4,t5,M))/damp; J(9,3)(f9(t,t,t3p,t4,t5,M)-f9(t,t,t3,t4,t5,M))/damp; J(9,4)(f9(t,t,t3,t4p,t5,M)-f9(t,t,t3,t4,t5,M))/damp; J(9,5)(f9(t,t,t3,t4,t5p,M)-f9(t,t,t3,t4,t5,M))/damp; J(,)(f(tp,t,t3,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,)(f(t,tp,t3,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,3)(f(t,t,t3p,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,4)(f(t,t,t3,t4p,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,5)(f(t,t,t3,t4,t5p,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,)(f(tp,t,t3,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,)(f(t,tp,t3,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,3)(f(t,t,t3p,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,4)(f(t,t,t3,t4p,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,5)(f(t,t,t3,t4,t5p,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,)(f(tp,t,t3,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,)(f(t,tp,t3,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,3)(f(t,t,t3p,t4,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,4)(f(t,t,t3,t4p,t5,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; J(,5)(f(t,t,t3,t4,t5p,VC,M)-f(t,t,t3,t4,t5,VC,M))/damp; f()f(t,t,t3,t4,t5,m); f()f(t,t,t3,t4,t5,m); f(3)f3(t,t,t3,t4,t5,m); f(4)f4(t,t,t3,t4,t5,m); f(5)f5(t,t,t3,t4,t5,m); f(6)f6(t,t,t3,t4,t5,m); f(7)f7(t,t,t3,t4,t5,m); f(8)f8(t,t,t3,t4,t5,m); f(9)f9(t,t,t3,t4,t5,m); f()f(t,t,t3,t4,t5,vc,m); f()f(t,t,t3,t4,t5,vc,m); f()f(t,t,t3,t4,t5,vc,m); %ds -J\f'; dspnv(j)*(-f'); tt+ds(); tt+ds(); t3t3+ds(3); t4t4+ds(4); t5t5+ds(5); t[t t t3 t4 t5];

162 kkt*8/p; %fprntf('n%.f, t%.5e, t%.5e, t3%.5e, t4%.5e, t5%.5e,', n,kk(),kk(),kk(3),kk(4),kk(5)) %fprntf(' f()%.e, f()%.e, f(3)%.e, f(4)%.e, f(5)%.e, f(6)%.e, f(7)%.e, f(8)%.e, f(9)%.e, f()%.e, f()%.e, f()%.e\n', f(), f(), f(3), f(4), f(5), f(6), f(7), f(8), f(9), f(), f(), f()) f (abs(f())<.e-9 & abs (f())<.e-9 & abs (f(3))<.e-9 & abs (f(4))<.e-9 & abs (f(5))<.e-9 & abs (f(6))<.e-9 & abs (f(7))<.e-9 & abs (f(8))<.e-9 & abs (f(9))<.e-9 & abs (f())<.e-9 & abs (f())<.e-9 & abs (f())<.e-9), break;end end

163 %Funcón num.m desarrollada por Emmanuel Merchán %determnacón de los parámetros necesaros en la formulacón %de Lagrange-Euler, prmer lstado vvnput('vector de poscones angulares '); velocnput('vector de velocdades angulares '); accnput('vector de aceleracones angulares '); dtveloc(); dtveloc(); dt3veloc(3); dt4veloc(4); dt5veloc(5); accacc'; L.3; L.5; L3.6; L4.7; vvvv*p/8; tvv(); tvv(); t3vv(3); t4vv(4); t5vv(5); A[cos(t) sn(t) ;sn(t) -cos(t) ; L; ]; A[cos(t) -sn(t) L*cos(t);sn(t) cos(t) L*sn(t); ; ]; A3[cos(t3) -sn(t3) L3*cos(t3);sn(t3) cos(t3) L3*sn(t3); ; ]; A34[cos(t4) -sn(t4) ;sn(t4) cos(t4) ; - ; ]; A45[cos(t5) -sn(t5) ;sn(t5) cos(t5) ; L4; ]; A5A*A*A3*A34*A45; Q[ - ; ; ; ]; m5; m8; m36; m4; m5; J[(/3)*m*L^ -(/)*m*l; ; ;-(/)*m*l m]; J[(/3)*m*L^ -(/)*m*l; ; ;-(/)*m*l m]; J3[(/3)*m3*L3^ -(/)*m3*l3; ; ;-(/)*m3*l3 m3]; J4[(/3)*m4 -(/)*m4; ; ;-(/)*m4 m4]; J5[(/3)*m5*L4^ -(/)*m5*l4; ; ;-(/)*m5*l4 m5]; UQ*A; UQ*A*A; U3Q*A*A*A3; U4Q*A*A*A3*A34; U5Q*A*A*A3*A34*A45; UA*Q*A; U3A*Q*A*A3; U4A*Q*A*A3*A34; U5A*Q*A*A3*A34*A45;

164 U33A*A*Q*A3; U43A*A*Q*A3*A34; U53A*A*Q*A3*A34*A45; U44A*A*A3*Q*A34; U54A*A*A3*Q*A34*A45; U55A*A*A3*A34*Q*A45; UQ*U; UQ*U; U3Q*U3; U4Q*U4; U5Q*U5; UQ*U; U3Q*U3; U4Q*U4; U5Q*U5; U33Q*U33; U43Q*U43; U53Q*U53; U44Q*U44; U54Q*U54; U55Q*U55; UU; U3U3; U4U4; U5U5; UA*Q*Q*A; U3A*Q*Q*A*A3; U4A*Q*Q*A*A3*A34; U5A*Q*Q*A*A3*A34*A45; U33U*Q*A3; U43U*Q*A3*A34; U53U*Q*A3*A34*A45; U44U3*Q*A34; U54U3*Q*A34*A45; U55U4*Q*A45; U33U33; U43U43; U53U53; U33U33; U43U43; U53U53; U333A*A*Q*Q*A3; U433A*A*Q*Q*A3*A34; U533A*A*Q*Q*A3*A34*A45; U443U33*Q*A34; U543U33*Q*A34*A45; U553U43*Q*A45; U44U44; U54U54; U44U44;

165 U54U54; U434U443; U534U543; U444A*A*A3*Q*Q*A34; U544A*A*A3*Q*Q*A34*A45; U554U44*Q*A45; U55U55; U55U55; U535U553; U545U554; U555A*A*A3*A34*Q*Q*A45; htrace(u*j*u')+trace(u*j*u')+trace(u3*j3*u3')+trace(u4 *J4*U4')+trace(U5*J5*U5'); htrace(u*j*u')+trace(u3*j3*u3')+trace(u4*j4*u4')+trace(u5 *J5*U5'); h3trace(u33*j3*u3')+trace(u43*j4*u4')+trace(u53*j5*u5'); h4trace(u44*j4*u4')+trace(u54*j5*u5'); h5trace(u55*j5*u5'); htrace(u*j*u')+trace(u3*j3*u3')+trace(u4*j4*u4')+trace(u5 *J5*U5'); htrace(u*j*u')+trace(u3*j3*u3')+trace(u4*j4*u4')+trace(u5 *J5*U5'); h3trace(u33*j3*u3')+trace(u43*j4*u4')+trace(u53*j5*u5'); h4trace(u44*j4*u4')+trace(u54*j5*u5'); h5trace(u55*j5*u5'); h3trace(u33*j3*u3')+trace(u43*j4*u4')+trace(u53*j5*u5'); h3trace(u33*j3*u3')+trace(u43*j4*u4')+trace(u53*j5*u5'); h33trace(u333*j3*u3')+trace(u433*j4*u4')+trace(u533*j5*u5'); h34trace(u434*j4*u4')+trace(u534*j5*u5'); h35trace(u535*j5*u5'); h4trace(u44*j4*u4')+trace(u54*j5*u5'); h4trace(u44*j4*u4')+trace(u54*j5*u5'); h43trace(u443*j4*u4')+trace(u543*j5*u5'); h44trace(u444*j4*u4')+trace(u544*j5*u5'); h45trace(u545*j5*u5'); h5trace(u55*j5*u5'); h5trace(u55*j5*u5'); h53trace(u553*j5*u5'); h54trace(u554*j5*u5'); h55trace(u555*j5*u5'); htrace(u*j*u')+trace(u3*j3*u3')+trace(u4*j4*u4')+trace(u5 *J5*U5'); htrace(u*j*u')+trace(u3*j3*u3')+trace(u4*j4*u4')+trace(u5 *J5*U5'); h3trace(u33*j3*u3')+trace(u43*j4*u4')+trace(u53*j5*u5'); h4trace(u44*j4*u4')+trace(u54*j5*u5'); h5trace(u55*j5*u5'); htrace(u*j*u')+trace(u3*j3*u3')+trace(u4*j4*u4')+trace(u5 *J5*U5');

166 htrace(u*j*u')+trace(u3*j3*u3')+trace(u4*j4*u4')+trace(u5 *J5*U5'); h3trace(u33*j3*u3')+trace(u43*j4*u4')+trace(u53*j5*u5'); h4trace(u44*j4*u4')+trace(u54*j5*u5'); h5trace(u55*j5*u5'); h3trace(u33*j3*u3')+trace(u43*j4*u4')+trace(u53*j5*u5'); h3trace(u33*j3*u3')+trace(u43*j4*u4')+trace(u53*j5*u5'); h33trace(u333*j3*u3')+trace(u433*j4*u4')+trace(u533*j5*u5'); h34trace(u434*j4*u4')+trace(u534*j5*u5'); h35trace(u535*j5*u5'); h4trace(u44*j4*u4')+trace(u54*j5*u5'); h4trace(u44*j4*u4')+trace(u54*j5*u5'); h43trace(u443*j4*u4')+trace(u543*j5*u5'); h44trace(u444*j4*u4')+trace(u544*j5*u5'); h45trace(u545*j5*u5'); h5trace(u55*j5*u5'); h5trace(u55*j5*u5'); h53trace(u553*j5*u5'); h54trace(u554*j5*u5'); h55trace(u555*j5*u5'); h3trace(u3*j3*u33')+trace(u4*j4*u43')+trace(u5*j5*u53'); h3trace(u3*j3*u33')+trace(u4*j4*u43')+trace(u5*j5*u53'); h33trace(u33*j3*u33')+trace(u43*j4*u43')+trace(u53*j5*u53'); h34trace(u44*j4*u43')+trace(u54*j5*u53'); h35trace(u55*j5*u53'); h3trace(u3*j3*u33')+trace(u4*j4*u43')+trace(u5*j5*u53'); h3trace(u3*j3*u33')+trace(u4*j4*u43')+trace(u5*j5*u53'); h33trace(u33*j3*u33')+trace(u43*j4*u43')+trace(u53*j5*u53'); h34trace(u44*j4*u43')+trace(u54*j5*u53'); h35trace(u55*j5*u53'); h33trace(u33*j3*u33')+trace(u43*j4*u43')+trace(u53*j5*u53'); h33trace(u33*j3*u33')+trace(u43*j4*u43')+trace(u53*j5*u53'); h333trace(u333*j3*u33')+trace(u433*j4*u43')+trace(u533*j5*u53'); h334trace(u434*j4*u43')+trace(u534*j5*u53'); h335trace(u535*j5*u53'); h34trace(u44*j4*u43')+trace(u54*j5*u53'); h34trace(u44*j4*u43')+trace(u54*j5*u53'); h343trace(u443*j4*u43')+trace(u543*j5*u53'); h344trace(u444*j4*u43')+trace(u544*j5*u53'); h345trace(u545*j5*u53'); h35trace(u55*j5*u53'); h35trace(u55*j5*u53'); h353trace(u553*j5*u53'); h354trace(u554*j5*u53'); h355trace(u555*j5*u53'); h4trace(u4*j4*u44')+trace(u5*j5*u54'); h4trace(u4*j4*u44')+trace(u5*j5*u54'); h43trace(u43*j4*u44')+trace(u53*j5*u54');

167 h44trace(u44*j4*u44')+trace(u54*j5*u54'); h45trace(u55*j5*u54'); h4trace(u4*j4*u44')+trace(u5*j5*u54'); h4trace(u4*j4*u44')+trace(u5*j5*u54'); h43trace(u43*j4*u44')+trace(u53*j5*u54'); h44trace(u44*j4*u44')+trace(u54*j5*u54'); h45trace(u55*j5*u54'); h43trace(u43*j4*u44')+trace(u53*j5*u54'); h43trace(u43*j4*u44')+trace(u53*j5*u54'); h433trace(u433*j4*u44')+trace(u533*j5*u54'); h434trace(u434*j4*u44')+trace(u534*j5*u54'); h435trace(u535*j5*u54'); h44trace(u44*j4*u44')+trace(u54*j5*u54'); h44trace(u44*j4*u44')+trace(u54*j5*u54'); h443trace(u443*j4*u44')+trace(u543*j5*u54'); h444trace(u444*j4*u44')+trace(u544*j5*u54'); h445trace(u545*j5*u54'); h45trace(u55*j5*u54'); h45trace(u55*j5*u54'); h453trace(u553*j5*u54'); h454trace(u554*j5*u54'); h455trace(u555*j5*u54'); h5trace(u5*j5*u55'); h5trace(u5*j5*u55'); h53trace(u53*j5*u55'); h54trace(u54*j5*u55'); h55trace(u55*j5*u55'); h5trace(u5*j5*u55'); h5trace(u5*j5*u55'); h53trace(u53*j5*u55'); h54trace(u54*j5*u55'); h55trace(u55*j5*u55'); h53trace(u53*j5*u55'); h53trace(u53*j5*u55'); h533trace(u533*j5*u55'); h534trace(u534*j5*u55'); h535trace(u535*j5*u55'); h54trace(u54*j5*u55'); h54trace(u54*j5*u55'); h543trace(u543*j5*u55'); h544trace(u544*j5*u55'); h545trace(u545*j5*u55'); h55trace(u55*j5*u55'); h55trace(u55*j5*u55'); h553trace(u553*j5*u55'); h554trace(u554*j5*u55'); h555trace(u555*j5*u55'); num

168 %funcón num.m desarrollada por Emmanuel Merchán %para determnar los parámetros de la matrz [D] D(,)trace(U*J*U')+trace(U*J*U')+trace(U3*J3*U3')+trace(U4 *J4*U4')+trace(U5*J5*U5'); D(,)trace(U*J*U')+trace(U3*J3*U3')+trace(U4*J4*U4')+trace(U5 *J5*U5'); D(,)D(,); D(,3)trace(U33*J3*U3')+trace(U43*J4*U4')+trace(U53*J5*U5'); D(3,)D(,3); D(,4)trace(U44*J4*U4')+trace(U54*J5*U5'); D(4,)D(,4); D(,5)trace(U55*J5*U5'); D(5,)D(,5); D(,)trace(U*J*U')+trace(U3*J3*U3')+trace(U4*J4*U4')+trace(U5 *J5*U5'); D(,3)trace(U33*J3*U3')+trace(U43*J4*U4')+trace(U53*J5*U5'); D(,4)trace(U44*J4*U4')+trace(U54*J5*U5'); D(,5)trace(U55*J5*U5'); D(3,)D(,3); D(4,)D(,4); D(5,)D(,5); D(3,3)trace(U33*J3*U33')+trace(U43*J4*U43')+trace(U53*J5*U53'); D(3,4)trace(U44*J4*U43')+trace(U54*J5*U53'); D(3,5)trace(U55*J5*U53'); D(4,3)D(3,4); D(5,3)D(3,5); D(4,4)trace(U44*J4*U44')+trace(U54*J5*U54'); D(4,5)trace(U55*J5*U54'); D(5,4)D(4,5); D(5,5)trace(U55*J5*U55'); num3

169 %funcón num3.m desarrollada por Emmanuel Merchán %que determna la matrz [h] y la matrz [c] a partr de los %parámetros calculados por las funcones anterores %y resuelve de manera numérca la formulacón de Lagrange Euler %para obtener el valor de las torcas..5 r[; ; -.5; ]; r[-.5; ; ; ]; r33[-.8; ; ; ]; r44[; ; ; ]; r55[; ; -.36; ]; grav[ -9.8 ]; h(,)h*(dt^)+h*(dt)*(dt)+h3*(dt)*(dt3)+h4*(dt)*(dt4)+h 5*dt*dt5+h3*dt*dt3+h3*dt*dt3+h33*(dt3^)+h34*dt3*dt4+h35*dt3*d t5+h4*dt*dt4+h4*dt*dt4+h43*dt3*dt4+h44*(dt4^)+h45*dt4*dt5+h5* dt*dt5+h5*dt*dt5+h53*dt3*dt5+h54*dt4*dt5+h55*dt5^; h(,)h*(dt^)+h*(dt)*(dt)+h3*(dt)*(dt3)+h4*(dt)*(dt4)+h 5*dt*dt5+h3*dt*dt3+h3*dt*dt3+h33*(dt3^)+h34*dt3*dt4+h35*dt3*d t5+h4*dt*dt4+h4*dt*dt4+h43*dt3*dt4+h44*(dt4^)+h45*dt4*dt5+h5* dt*dt5+h5*dt*dt5+h53*dt3*dt5+h54*dt4*dt5+h55*dt5^; h(3,)h3*(dt^)+h3*(dt)*(dt)+h33*(dt)*(dt3)+h34*(dt)*(dt4)+h3 5*dt*dt5+h33*dt*dt3+h33*dt*dt3+h333*(dt3^)+h334*dt3*dt4+h335*dt3*d t5+h34*dt*dt4+h34*dt*dt4+h343*dt3*dt4+h344*(dt4^)+h345*dt4*dt5+h35* dt*dt5+h35*dt*dt5+h353*dt3*dt5+h354*dt4*dt5+h355*dt5^; h(4,)h4*(dt^)+h4*(dt)*(dt)+h43*(dt)*(dt3)+h44*(dt)*(dt4)+h4 5*dt*dt5+h43*dt*dt3+h43*dt*dt3+h433*(dt3^)+h434*dt3*dt4+h435*dt3*d t5+h44*dt*dt4+h44*dt*dt4+h443*dt3*dt4+h444*(dt4^)+h445*dt4*dt5+h45* dt*dt5+h45*dt*dt5+h453*dt3*dt5+h454*dt4*dt5+h455*dt5^; h(5,)h5*(dt^)+h5*(dt)*(dt)+h53*(dt)*(dt3)+h54*(dt)*(dt4)+h5 5*dt*dt5+h53*dt*dt3+h53*dt*dt3+h533*(dt3^)+h534*dt3*dt4+h535*dt3*d t5+h54*dt*dt4+h54*dt*dt4+h543*dt3*dt4+h544*(dt4^)+h545*dt4*dt5+h55* dt*dt5+h55*dt*dt5+h553*dt3*dt5+h554*dt4*dt5+h555*dt5^; cc(,)-(m*grav*u*r+m*grav*u*r+m3*grav*u3*r33+ m4*grav*u4*r44+m5*grav*u5*r55); cc(,)-(m*grav*u*r+m3*grav*u3*r33+m4*grav*u4*r44+ m5*grav*u5*r55); cc(3,)-(m3*grav*u33*r33+m4*grav*u43*r44+m5*grav*u53*r55); cc(4,)-(m4*grav*u44*r44+m5*grav*u54*r55); cc(5,)-(m5*grav*u55*r55); torqued*acc+h+cc; %esta es la ecuacón matrcal correspondente a la %formulacon de Lagrange-Euler fprntf('torques N-m\n') fprntf('%.e\n', torque()) fprntf('%.e\n', torque()) fprntf('%.e\n', torque(3)) fprntf('%.e\n', torque(4)) fprntf('%.e\n', torque(5))

170 %Funcon Traycrc %desarrollada por Emmanuel Merchán %para la generacon de las matrces de tranformacón %correspondentes a la trayectora crcular "bc" for n: tt+9; a[-sn(t*p/8) -cos(t*p/8) --(5*sn(t*p/8));- cos(t*p/8) sn(t*p/8) +5-5*cos(t*p/8); - 5] end

171 ANEXO. Lstado para el Modelado en Realdad Vrtual del Robot Mtsubsh Movemaster. #VRML V. utf8 # # # Este archvo construye un robot Mtsubsh RV-M Movemaster EX # en un modelo de realdad vrtual WorldInfo { ttle "Robot Mtsubsh" nfo "Emmanuel Merchán", NavgatonInfo { type "EXAMINE" headlght FALSE, Vewpont { poston orentaton -. descrpton "Frst vew - From top left", Vewpont { poston -8 4 orentaton descrpton "Second vew - From Front", Vewpont { poston -3 3 orentaton descrpton "Thrd vew - From left corner", Vewpont { poston -8 5 orentaton -.57 descrpton "Fourth vew - From top", Vewpont { poston orentaton 3.4 descrpton "Ffth vew - From Back", Vewpont { poston -5 orentaton -.57 descrpton "Sxth vew - From left sde", PontLght { locaton 5 4, PontLght { locaton -5, PontLght { locaton, PontLght { locaton -, PontLght { locaton - -4, PontLght { locaton -4, PontLght {

172 , locaton - 3 # Axes Shape { appearance Appearance { materal Materal { emssvecolor... geometry IndexedLneSet { coord Coordnate { pont [..., 35...,. 35..,.. 35., , , ] coordindex [,, -,,, -,, 3, -,, 4, -,, 5, -,, 6, - ], Group { chldren [ DEF Robot Transform { translaton chldren [ # # Robot Base # # Bottom Plate (/6) Group { chldren [ Transform { translaton.5.. chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Box { sze , # Front Base Cylnder (/6) Transform { translaton chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght 5.5 radus.5 bottom FALSE ],

173 # Rear Base Cylnder (3/6) Transform { translaton..875 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght 5.5 radus.5 bottom FALSE ], # Md Base Cube (4/6) Transform { translaton chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Box { sze ], # Chrome Top Rng (4/6) Transform { translaton chldren [ Shape { appearance Appearance { materal Materal { ambentintensty. dffusecolor geometry Cylnder {, ] heght.5 radus.75 bottom FALSE top FALSE # Top Rng (4/6) Transform { translaton chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor

174 ] geometry Cylnder { heght.5 radus.75 bottom FALSE top FALSE ] ] # # Moveable Sectons # # Rotatng secton DEF Wast Transform { translaton rotaton, chldren [ # Shoulder Cradle Transform { translaton chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry IndexedFaceSet { coord Coordnate { pont [.5, 4.5.5, , -.5,.75 3, , ,.75-3, 4.5 3, , , 4.5-3, ] coordindex [,,,3,-,,4,5,,-,,5,6,,-,,6,7,3,-, 3,7,4,,-, 4,8,9,5,-, 5,9,,6,-, 6,,,7,-, 7,,8,4,-, ] normalpervertex FALSE sold FALSE creaseangle.5

175 ], # Wde Shoulder Cylnder Transform { translaton.75 rotaton.57 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght 7 radus.5 ], # Narrow Shoulder Cylnder Transform { translaton.75 rotaton.57 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght 9 radus.875 ], # Large Rear Shoulder Quarter Transform { translaton 5.65 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Box { sze ], # Thn Rear Shoulder Quarter Transform { translaton chldren [ Shape { appearance Appearance {

176 , ] materal Materal { ambentintensty dffusecolor geometry Box { sze # Upper Arm from shoulder to elbow DEF Shoulder Transform { translaton rotaton, center -6 chldren [ # Cylndrcal Rear Secton (/3) Transform { translaton -6 rotaton.57 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght 4.5 radus.65 ], # Cylndrcal Front Secton (/3) Transform { translaton -6 rotaton.57 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght 4.5 radus.75 ], # Cube Md Secton (3/3) Transform { translaton - chldren [ Shape { appearance Appearance { materal Materal { ambentintensty

177 ] dffusecolor geometry Box { sze # Fore Arm from elbow to wrst DEF Elbow Transform { translaton rotaton, center -7 chldren [ # Cylndrcal Front Secton (/3) Transform { translaton -33 rotaton.57 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght.75 radus.65 ], # Cylndrcal Rear Secton (/3) Transform { translaton -7 rotaton.57 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght.75 radus.65 ], # Cube Md Secton (3/3) Transform { translaton -3 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor

178 ] geometry Box { sze # Arm from wrst to grpper DEF Wrst Transform { translaton -5 rotaton, center -8 chldren [ # Cylndrcal Hnge (/) Transform { translaton -8 rotaton,.57 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Cylnder { heght.5 radus.675 ], # Cube Extenson Secton (/) Transform { translaton -9.5 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty dffusecolor geometry Box { sze.5.5 ] # Grpper DEF Hand Transform { translaton rotaton,.5 center chldren [ # Cylndrcal Cuff (/4) Transform { translaton - rotaton.57 chldren [ Shape { appearance Appearance { materal Materal {

179 , ] ambentintensty.5 dffusecolor geometry Cylnder { heght.5 radus.65 # Cube Hand Secton (/4) Transform { translaton -.5 chldren [ Shape { appearance Appearance { materal Materal { ambentintensty.5 dffusecolor geometry Box { sze ] # Cube Top Grp Fnger (3/4) DEF Top_Fnger Transform { translaton chldren [ ] Transform { translaton chldren [ Shape { appearance Appearance { materal Materal { ambentintensty.5 dffusecolor geometry Box { sze.5.75 ] # Cube Bottom Grp Fnger (4/4) DEF Bottom_Fnger Transform { translaton chldren [ Transform { translaton chldren [ Shape { appearance Appearance { materal Materal { ambentintensty.5 dffusecolor.5.5.5

180 ] ] ] ] ] ] ] ] geometry Box { sze.5.75 # # Trgger thngs on touch # DEF Touch TouchSensor { ]

181 ANEXO 3. Gráfcas correspondentes a las poscones que conforman la trayectora abc. VISTA FRONTAL

182

183

184 VISTA LATERAL DERECHA

185

186 VISTA SUPERIOR

187

188

189 ANEXO 4. Programas del proyecto de mplementacón. ;************************************************************************ ;* PROGRAMA QUE IMPLEMENTA EL CONTROL P, PI, PD Y PID PARA EL MOVIMIENTO* ;* DE UN MOTOR DE CD. * ;************************************************************************ ; ;************************************************************************ ;* INSTITUTO POLITECNICO NACIONAL * ;* ESCUELA SUPERIOR DE INGENIERIA MECANICA Y ELECTRICA * ;* UNIDAD PROFESIONAL `ADOLFO LOPEZ MATEOS`(ZACATENCO) * ;* INGENIERIA EN CONTROL Y AUTOMATIZACION * ;* PROGRAMA DESARROLLADO POR: VELAZQUEZ SANCHEZ ALEJANDRO T. * ;* RAMIREZ GORDILLO JAVIER * ;************************************************************************ ;***************************************** ;MENSAJE AL LOCALIZAR LA POSICION DESEADA ;***************************************** MEN DB D,D,'THETA',4 ;******************************** ;MENSAJES DEL MOTOR ;******************************** MEN DB 'UP',4,D MEN DB 'DWN',4,D ;***************************** ;REGISTROS PARA LOS SENSORES ;***************************** ROP EQU $39 ;REGISTRO DE CONFIGURACION DE OPCIONES DEL SISTEMA RAD EQU $3 ;CONTROL DEL CONVERTIDOR A/D OSU EQU $FFC7 ;SALIDA DE UNA CADENA DE BYTES UTA EQU $FFB8 ;SALIDA DEL ACUMULADOR A COMO CARACTER ASCII ENT EQU $3 ;RESULTADOS DEL CANAL DEL CAD OUR EQU $FFB5 ;REALIZA UNA CONVERSION EN ASCII Y LA SACA POR EL PUERTO PID EQU $B73 ;SUB RUTINA PARA EL CALCULO DEL CONTROL PID ;***************************** ;REGISTROS PARA EL MOTOR ;***************************** RDDC EQU $7 ;REGISTRO DE DIRECCION DE DATOS DEL PUERTO C RPC EQU $3 ;REGISTRO DEL PUERTO C

190 POS EQU $F ;LOCALIDAD PARA ALMACENAR LA POSICION A BUSCAR PWM EQU $F4 ;LOCALIDAD QUE CONTIENE EL CICLO DE TRABAJO DEL PWM ERROR EQU $F6 ;LOCALIDAD QUE CONTIENE EL ERROR DEL SISTEMA MERROR EQU $F8 ;LOCALIDAD PARA ALMACENAR EL MARGEN DE ERROR LIINF EQU $FC ;LOCALIDAD QUE CONTIENE EL LIMITE INFERIOR LISUP EQU $FD ;LOCALIDAD QUE CONTIENE EL LIMITE SUPERIOR ;**************************** ;INICIO DEL PROGRAMA ;**************************** ORG $ ;LOCALIDAD INICIAL LDAA #$FF ;{ SE CONFIGURAN LOS PINES DEL STAA RDDC ;{ PUERTO C COMO SALIDAS GO LDAA #$6 ;@ SE CONFIGURA STAA RAD ;@ ACTIVA EL LDAA #$8 ;@ STAA ROP ;@ EN MODO JSR DELAY ;RETARDO PARA ESTABILIZAR LA CONVERSION LDAA POS ;{ SE CARGA LA POSICION A BUSCAR SUBA MERROR ;{ Y SE CALCULA EL MARGEN DE ERROR STAA LIINF ;{ PARA ESTABLECER EL LIMITE INFERIOR LDAA POS ;@ SE CARGA LA POSICION A ADDA MERROR ;@ Y SE CALCULA EL MARGEN DE STAA LISUP ;@ PARA ESTABLECER EL LIMITE LDAA LIINF ;{ SE DETERMINA SI LA POSICION ACTUAL CMPA ENT ;{ ESTA DENTRO DE LOS LIMITES SUPERIOR BHI RECTSU ;{ E INFERIOR, POR LO QUE DE NO SER ASI LDAA LISUP ;{ SE CANALIZA A SUB RUTINAS PARA HACER CMPA ENT ;{ LOS CALCULOS NECESARIOS Y CORREGIR BLO RECTBA ;{ INMEDIATAMENTE HASTA ENCONTRARLA; LDX #MEN ;{ SI LA POSICION SE ENCUENTRA JSR OSU ;{ DENTRO DEL MARGEN SE ENVIA LDAA ENT ;{ EN MENSAJE DEL VALOR DE JSR UTA ;{ LA POSICION PREDETERMINADA. BRA GO RECTSU LDAA POS ;{ AQUI SE HACE LA CORRECCION DE SUBA ENT ;{ LA POSICION DE ACUERDO AL VALOR DEL STAA ERROR ;{ ERROR GENERADO ENTRE LA POSICION JSR PID ;{ ACTUAL Y LA DE REFERENCIA, LDX #MEN ;{ ESTE ERROR ES EMPLEADO POR LA JSR OSU ;{ SUB RUTINA DEL PID PARA CALCULAR EL LDAA #$FB ;{ VALOR DEL PWM, NECESARIO PARA STAA RPC ;{ CORREGIR ESTE ERROR. JSR DELAY ;{ SE CARGA EL DATO QUE PROPORCIONA EL LDAA #$FF ;{ SENTIDO DE GIRO DEL MOTOR, STAA RPC ;{ EL CUAL ES ALMACENADO Y SACADO POR EL BRA GO ;{ PUERTO C. RECTBA LDAA ENT ;{ SE HACE EL CALCULO CORRESPONDIENTE

191 SUBA POS ;{ AL ERROR PARA PROPORCIONARLO A LA STAA ERROR ;{ SUB RUTINA PID JSR PID ;{ ESTA SUB RUTINA HACE EL CALCULO DEL LDX #MEN ;{ PWM CORRESPONDIENTE PARA CORREGIR EL JSR OSU ;{ ERROR. LDAA #$F7 ;{ CARGA EL DATO QUE PROPORCIONA EL STAA RPC ;{ SENTIDO DE GIRO DEL MOTOR, JSR DELAY ;{ EL CUAL ES ALMACENADO SACADO POR EL LDAA #$FF ;{ PUERTO C STAA RPC JMP GO DELAY LDX PWM ;{ A QUI CARGAMOS EL DATO DEL PWM QUE UNO DEX ;{ ARROJO EL CONTROL PID PARA GENERAR BNE UNO ;{ EL TIEMPO DE ACTIVO ALTO. RTS ;{ REGRESO DE SUB RUTINA DELAY LDY #$F ;{ RETARDO CORRESPONDIENTE A LA DOS DEY ;{ CONFIGURACION DEL DAC BNE DOS ;{ PARA ESTABILIZAR RTS ;{ LA CONVERSION FIN SWI ;{ FIN DE PROGRAMA

192 ;*********************************************************************** ;* SUB PROGRAMA CORRESPONDIENTE A EL CALCULO DEL CONTROL PID PARA * ;* GENERAR EL PWM NECESARIO PARA CORREGIR EL ERROR * ;*********************************************************************** ; ;*********************************************************************** ;* INSTITUTO POLITECNICO NACIONAL * ;* UNIDAD PROFESIONAL ADOLFO LOPEZ MATEOS * ;* ESCUELA SUPERIOR DE INGENIERIA MECANICA Y ELECTRICA * ;* CONTROL Y AUTOMATIZACION * ;* PROGRAMA DESARROLLADO POR: RAMIREZ GORDILLO JAVIER * ;* VELAZQUEZ SANCHEZ ALEJANDRO T * ;*********************************************************************** ;********************************************************* ; DECLARACION DE REGISTROS EMPLEADOS POR EL SUB PROGRAMA ;********************************************************* CP EQU $F ;CONSTANTE PROPORCIONAL CI EQU $F ;CONSTANTE INTEGRAL CD EQU $F3 ;CONSTANTE DIFERENCIAL PWM EQU $F4 ;VALOR DEL PWM ERROR EQU $F6 ;ERROR ACTUAL ERROR EQU $F7 ;ERROR ANTERIOR WK EQU $F9 ;RESULTADO DE LA PARTE INTEGRACION VK EQU $FA ;RESULTADO DE LA DIFERENCIACION RESERR EQU $FE ;LOCALIDAD RESERVADA PARA TRANSFERENCIA DE DATOS RETRO EQU $FF ;RESULTADO DE LA SECCION A RETRO ALIMENTAR PRO EQU $EA ;RESULTADO DEL CALCULO PROPORCIONAL INT EQU $EC :RESULTADO DEL CALCULO INTEGRAL DER EQU $EE ;RESULTADO DEL CALCULO DERIVATIVO ENT EQU $33 ;CANAL DEL CAD ORG $B73 ;LA SUB RUTINA SE ENCUENTRA EN LA LOCALIDAD $B73 PID LDAA CP ;CARGA LA CONSTANTE PROPORCIONAL Y AL MULTIPLICA LDAB ERROR ;POR EL ERROR DE LA SEÑAL Y EL RESULTADO LO MUL ;COLOCA EN LA LOCALIDAD PRO, PARA SER EMPLEADA STD PRO ;POSTERIORMENTE POR EL PROGRAMA. LDAA ERROR ;SE HACE UN PROMEDIO ENTRE LOS ERRORES ACTUAL ADDA ERROR ;Y ANTERIOR, SEGUN EL ALGORITMO DEL CONTROL. ADDA WK ;ESTE RESULTADO ES SUMADO AL CALCULO INTEGRAL STAA WK ;PREVIO A ESTE CICLO Y POSTERIORMENTE SE LDAB CI ;ACTUALIZA Y SE ALMACENA EN WK; ESTE RESULTADO MUL ;ES MULTIPLICADO POR LA CONSTANTE INTEGRAL STD INT ;Y ALMACENADO EN INT LDAA ERROR ;SE CARGA EL ERROR ACTUAL Y EL ANTERIOR Y SUBA ERROR ;SE RESTAN PARA OBTENER EL ERROR PROMEDIO STAA RESERR ;ESTA RESTA SE ALMACENA EN RESERR LDD RESERR ;EL VALOR DEL ERROR PROMEDIO ES EMPLEADO LDX #$ ;PARA CALCULAR EL TIEMPO DE MUESTREO DEL

193 IDIV ;ALGORITMO DE CONTROL. STD VK ;ESTE VALOR SE ALMACENA EN VK Y ES ;MULTIPLICADO LDAA CD ;POR LA CONSTANTE PROPORCIONAL Y COLOCANDO EL LDAB VK ;RESULTADO EN VK. MUL ; STD DER ; LDD PRO ;SE SUMAN LOS RESULTADOS CORRESPONDIENTES ADDD INT ;A LA PARTE PROPORCIONAL, INTEGRAL Y ;DERIVATIVA SUBD DER ;PARA QUE ESTE RESULTADO SE ALMACENE EN PWM STD PWM ; LDAA ERROR ;SE ACTUALIZA EL ERROR ANTERIOR Y EL ACTUAL. STAA ERROR ; LDAA ENT ; STAA RETRO ; RTS ;FIN DE LA SUB RUTINA SWI ;

194 PROGRAMA DEL PWM RDDC EQU $7 RPC EQU $3 DES EQU $FFC7 INP EQU $FFAC POS EQU $C NEG EQU $C MEN DB 'ROTAR PRESIONE (L/R)',4,D MEN DB 'SALIR (E)',4,D MEN3 DB 'FUERA',4,D ORG $ LDAA #$FF STAA RDDC CLR RPC LDAA #$FF STAA RPC LDX #MEN JSR DES LDX #MEN JSR DES LDAA #$7 STAA POS STAA NEG MAS JSR PWM JSR INP CMPA #$4C BNE MAS BRA LEFTH MAS JSR PWM JSR INP CMPA #$5 BNE MAS BRA RIGHT MAS JSR PWM JSR INP CMPA #$45 BNE BRA MAS EXIT PWM LDAA #$FE STAA RPC JSR DELAY LDAA #$FF STAA RPC JSR DELAY RTS

195 LEFTH INC POS DEC NEG BRA MAS RIGHT DEC POS INC NEG BRA MAS EXIT LDX #MEN3 JSR DES JSR DELAY3 SWI END DELAY LDY POS UNO DEY BNE UNO RTS DELAY LDY NEG UNO DEY BNE UNO RTS DELAY3 LDX #$5 DOS3 LDY #$FFFF UNO3 DEY BNE UNO3 DEX BNE DOS3 RTS

196 ANEXO 5. Dagramas de conexón del manpulador robótco R3GL. Puente H U? XT EX RESET IRQ XIRQ MODB PA PA PA PE PE PE PE3 PE4 PE5 PE6 PE7 VRH VRL MC68HCE9 3 PA3 3 PA4 9 PA5 8 PA6 7 PA7 4 PB 4 PB 4 PB 39 PB3 38 PB4 37 PB5 36 PB6 35 PB7 9 PC PC PC PC3 3 PC4 4 PC5 5 PC6 6 PC7 PD PD PD 3 PD3 4 PD4 5 PD5 MODA 3 E 5 AS 4 R/W U? A A A3 A4 A5 A6 A7 A8 5V CD G DIR 74LS45 8 B 7 B 6 B3 5 B4 4 B5 3 B6 B7 B8 Haca los Puentes H y H3 R? RESISTOR R? RESISTOR Q? TIP3 Q? TIP + A - V CD R? RESISTOR M? MOTOR SERVO R? RESISTOR Q? TIP3 Q? TIP GND Dagrama de potenca para gro del motor

197 5V 5V FLECHA FLECHA FLECHA BUFFER HACIA PUENTES "H" R K R K R K MC68HCE9 XT 8 EX 7 RESET 7 IRQ 9 XIRQ 8 MODB PA 34 PA 33 PA 3 PE 43 PE 45 PE 47 PE3 49 PE4 44 PE5 46 PE6 48 PE7 5 VRH 5 VRL 5 PA3 3 PA4 3 PA5 9 PA6 8 PA7 7 PB 4 PB 4 PB 4 PB3 39 PB4 38 PB5 37 PB6 36 PB7 35 PC 9 PC PC PC3 PC4 3 PC5 4 PC6 5 PC7 6 PD PD PD PD3 3 PD4 4 PD5 5 MODA 3 E 5 AS 4 R/W 6 74LS45 A A 3 A3 4 A4 5 A5 6 A6 7 A7 8 A8 9 G 9 DIR B 8 B 7 B3 6 B4 5 B5 4 B6 3 B7 B8 MORE ANTEBRAZO MORE BASE MORE BRAZO 5V 5V FLECHA FLECHA FLECHA BUFFER HACIA PUENTES "H" R K R K R K MC68HCE9 XT 8 EX 7 RESET 7 IRQ 9 XIRQ 8 MODB PA 34 PA 33 PA 3 PE 43 PE 45 PE 47 PE3 49 PE4 44 PE5 46 PE6 48 PE7 5 VRH 5 VRL 5 PA3 3 PA4 3 PA5 9 PA6 8 PA7 7 PB 4 PB 4 PB 4 PB3 39 PB4 38 PB5 37 PB6 36 PB7 35 PC 9 PC PC PC3 PC4 3 PC5 4 PC6 5 PC7 6 PD PD PD PD3 3 PD4 4 PD5 5 MODA 3 E 5 AS 4 R/W 6 74LS45 A A 3 A3 4 A4 5 A5 6 A6 7 A7 8 A8 9 G 9 DIR B 8 B 7 B3 6 B4 5 B5 4 B6 3 B7 B8 MORE ANTEBRAZO MORE BASE MORE BRAZO DIAGRAMA DE CONEXIÓN AL MICROPROCESADOR

198 ANEXO 6. Publcacón generada de ésta tess.

199