MEMORIA DESCRIPTIVA

Vista previa del material en texto

Universidad de Sevilla 
Escuela Politécnica Superior de Sevilla 
 
Trabajo de Fin de Grado en Ingeniería Electrónica Industrial 
 
 
 
 
 
 
PROGRAMACIÓN E IMPLEMENTACIÓN DE UN BRAZO 
ROBÓTICO HACIENDO USO DE LAS COMUNICACIONES 
INDUSTRIALES PARA SU CONTROL 
 
 
 
 
 
 
 
Autor: Adrián Díaz Campano 
Tutor: Luis Muñoz Saavedra 
Departamento de Arquitectura y Tecnología de Computadores 
 
2 
 
 
Universidad de Sevilla 
Escuela Politécnica Superior de Sevilla 
 
 
PROGRAMACIÓN E IMPLEMENTACIÓN DE UN BRAZO 
ROBÓTICO HACIENDO USO DE LAS COMUNICACIONES 
INDUSTRIALES PARA SU CONTROL 
 
 
INDICE GENERAL 
 
Documento 1: Memoria Descriptiva. 
Documento 2: Anexo I. Código del microcontrolador 1. 
Documento 3: Anexo II. Código del microcontrolador 2. 
Documento 4: Anexo III. Librería para el bus CAN. 
Documento 5: Anexo IV. Código de la GUI. 
Documento 6: Plano I. Esquema del Circuito 1. 
Documento 7: Plano II. Esquema del Circuito 2. 
Documento 8: Presupuestos. 
 
 
 
 
 
 
3 
 
 
Universidad de Sevilla 
Escuela Politécnica Superior de Sevilla 
 
 
 
 
 
PROGRAMACIÓN E IMPLEMENTACIÓN DE UN BRAZO 
ROBÓTICO HACIENDO USO DE LAS COMUNICACIONES 
INDUSTRIALES PARA SU CONTROL 
 
 
MEMORIA DESCRIPTIVA 
 
 
 
 
 
 
 
Autor: Adrián Díaz Campano 
Tutor: Luis Muñoz Saavedra 
Departamento de Arquitectura y Tecnología de Computadores 
 
4 
 
Índice 
1. INTRODUCCIÓN Y OBJETIVOS ............................................................................................. 10 
2. Marco Normativo Legal y recursos ..................................................................................... 12 
2.1 Disposiciones legales y normas aplicadas. .................................................................. 12 
2.2 Programas utilizados. .................................................................................................. 13 
3. DEFINICIONES Y ABREVIATURAS ......................................................................................... 14 
4. ANÁLISIS DE SOLUCIONES ................................................................................................... 15 
4.1 Alternativas de Brazos robóticos para la docencia. .................................................... 15 
4.2 Alternativas de tecnologías de comunicaciones industriales. .................................... 21 
4.3 Alternativas de microcontroladores. .......................................................................... 23 
4.4 Alternativas de placas de desarrollo. .......................................................................... 25 
4.5 Alternativas para la programación de los microcontroladores................................... 28 
5. MODELO CINEMÁTICO ........................................................................................................ 30 
5.1 Problema cinemático directo. ..................................................................................... 31 
5.2 Problema cinemático inverso ...................................................................................... 39 
6. DISEÑO SOFTWARE ............................................................................................................. 48 
6.1 Programación del STM32 ............................................................................................ 48 
6.1.1 Introducción ........................................................................................................ 48 
6.1.2 Microcontrolador conectado al ordenador. ....................................................... 50 
6.1.3 Microcontrolador conectado al brazo robótico. ................................................. 54 
6.1.4 Librería para el uso del bus CAN. ........................................................................ 57 
6.2 Interfaz. ....................................................................................................................... 61 
7. DISEÑO HARDWARE ............................................................................................................ 66 
7.1 Circuito 1 ..................................................................................................................... 66 
7.1.1 Componentes utilizados. ..................................................................................... 66 
7.1.2 Diseño. ................................................................................................................. 68 
7.1.3 Resultado. ............................................................................................................ 71 
7.2 Circuito 2 ..................................................................................................................... 72 
7.2.1 Componentes utilizados. ..................................................................................... 72 
7.2.2 Diseño. ................................................................................................................. 73 
7.2.3 Resultado. ............................................................................................................ 79 
8. BANCO DE PRUEBAS ............................................................................................................ 80 
9. CONCLUSIONES ................................................................................................................... 84 
10. BIBLIOGRAFÍA .................................................................................................................. 86 
 
 
5 
 
Índice de figuras 
Figura 1.1. Diagrama del entorno de desarrollo. ........................................................................ 10 
Figura 4.1. Brazo robótico Thor. ................................................................................................ 16 
Figura 4.2. Brazo robótico EEZYBOTARM MK2. ......................................................................... 16 
Figura 4.3. Brazo robótico EEZYBOTARM MK2. ......................................................................... 17 
Figura 4.4. Sistema de Referencia para el robot KUKA KR6. ...................................................... 19 
Figura 4.5. Diseño de una mano de 28 GDL. .............................................................................. 19 
Figura 4.6. Estructura cinemática de la pierna y el robot. ......................................................... 19 
Figura 4.7. Pieza modificada 1. .................................................................................................. 20 
Figura 4.8. Pieza modificada 2. .................................................................................................. 20 
Figura 4.9. Pieza modificada 3. .................................................................................................. 21 
Figura 4.10. Placa NUCLEO-F103RB. ........................................................................................... 27 
Figura 4.11. Placa Blue Pill. ......................................................................................................... 27 
Figura 4.12. Programador ST-LINK V2. ........................................................................................ 28 
Figura 5.1. Diagrama de relación entre cinemática directa e inversa. “Fundamentos de 
Robótica”- Página 94. .................................................................................................................. 30 
Figura 5.2. Brazo robótico en posición inicial. ............................................................................ 33 
Figura 5.3. Diagrama con parámetros DH del brazo robótico. .................................................. 33 
Figura 5.5. Simulación de la posición inicial del robot. ..............................................................35 
Figura 5.6. Diagrama de los 3 primeros GDL del robot. ............................................................. 40 
Figura 5.7. Diagrama de las articulaciones 2 y 3 en disposición de codo abajo. ....................... 43 
Figura 5.8. Diagrama de las articulaciones 3 ,4 y 5. ................................................................... 44 
Figura 6.1. Ficheros de la plantilla básica para STM32. ............................................................. 49 
Figura 6.2. Propiedades del proyecto PlantillaBasica. ............................................................... 49 
Figura 6.3. Librerías del μC 1. ..................................................................................................... 51 
Figura 6.4. Función main del μC 1. ............................................................................................. 51 
Figura 6.5. Procesado de Comandos del μC 1. ........................................................................... 53 
Figura 6.6. Librerías del μC 2. ..................................................................................................... 54 
Figura 6.7. Función main del μC 2. ............................................................................................. 55 
Figura 6.8. Función main del μC 2. ............................................................................................. 55 
Figura 6.9. Procesado de Mensajes CAN del μC 2. .................................................................... 56 
Figura 6.10. Baud rates para el bus CAN. .................................................................................. 58 
Figura 6.11. Parámetros del bus CAN. ...................................................................................... 58 
Figura 6.12. Configuración de filtros del bus CAN. ................................................................... 59 
Figura 6.13. Envío de mensajes por bus CAN. ........................................................................... 60 
Figura 6.14. Recepción de mensajes por bus CAN. ................................................................... 60 
 
6 
 
Figura 6.15. GUI para el control del robot. ............................................................................... 62 
Figura 6.16. Esquina superior izquierda de la GUI. ................................................................... 62 
Figura 6.17. Esquina superior izquierda de la GUI. ................................................................... 64 
Figura 6.18. Parte derecha de la GUI. ....................................................................................... 65 
Figura 7.1. Disposición de pines del Blue pill. ............................................................................ 67 
Figura 7.2. Disposición de pines del MCP2551. ........................................................................ 67 
Figura 7.3. Disposición de pines del FT232RL FTDI. .................................................................. 67 
Figura 7.4. MB102. .................................................................................................................... 68 
Figura 7.5. Protoboard. ............................................................................................................. 68 
Figura 7.6. CAN network topology. “RM0008 Reference manual”- Página 665. ....................... 69 
Figura 7.7. CANopen architecture. “CANopen Hardware Setup Manual”- Página 12. .............. 69 
Figura 7.8. Circuito 1. ................................................................................................................ 71 
Figura 7.9. Disposición de pines del LM317T. “LM317 datasheet”- Página 1. .......................... 72 
Figura 7.10. Current Limit. “LM317”- Página 5. ........................................................................ 75 
Figura 7.11. Esquema de adaptador de nivel de tensión. ......................................................... 77 
Figura 7.12. Esquema del consumo del JI1. .............................................................................. 77 
Figura 7.13. Circuito 2. .............................................................................................................. 79 
Figura 8.1. Programa de prueba 1-1. ........................................................................................ 80 
Figura 8.2. Programa de prueba 1-2. ........................................................................................ 81 
Figura 8.3. Modo loop-back del bus CAN.................................................................................... 82 
 
 
 
 
 
 
 
 
 
 
 
 
 
7 
 
Índice de tablas 
Tabla 4.1. Alternativas de Brazos robóticos. .............................................................................. 18 
Tabla 4.2. Velocidad y distancia de Ethernet según el cable. .................................................... 22 
Tabla 4.3. Alternativas de Comunicaciones industriales. .......................................................... 23 
Tabla 4.4. Alternativas de microcontroladores.......................................................................... 25 
Tabla 4.5. STM32F103xx device features. ................................................................................. 26 
Tabla 5.1. Diagrama con parámetros DH del brazo robótico. ................................................... 34 
Tabla 6.1. Comandos entre el PC y el μC 1 ................................................................................ 52 
Tabla 6.2. Comandos entre el μC 1 y el μC 2. ............................................................................ 54 
Tabla 6.2. Recommended Bit Timing Settings. “CIA 301, CANopen application layer and 
communication profile”- Página 21. ........................................................................................... 57 
Tabla 6.3. Baud rate – distancia. Bus CAN ................................................................................. 61 
Tabla 7.1. Medidas de Intensidad del Servo HDKJ D3615. ........................................................ 74 
Tabla 7.2. Medidas de Intensidad del Servo HDKJ D3615. ........................................................ 76 
Tabla 7.3. Medidas de Intensidad del Servo HDKJ D3615. ........................................................ 76 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
8 
 
 
 
Índice de ecuaciones 
[Ecuación 5.1] .............................................................................................................................. 31 
[Ecuación 5.2] .............................................................................................................................. 31 
[Ecuación 5.3] .............................................................................................................................. 35 
[Ecuación 5.3.1] ........................................................................................................................... 36 
[Ecuación 5.3.2] ........................................................................................................................... 36 
[Ecuación 5.3.3] ........................................................................................................................... 36 
[Ecuación 5.3.4] ........................................................................................................................... 36 
[Ecuación 5.3.5] ........................................................................................................................... 37 
[Ecuación 5.3.6] ........................................................................................................................... 37 
[Ecuación 5.3.7] ...........................................................................................................................37 
[Ecuación 5.3.8] ........................................................................................................................... 37 
[Ecuación 5.3.9] ........................................................................................................................... 37 
[Ecuación 5.3.10] ......................................................................................................................... 37 
[Ecuación 5.3.11] ......................................................................................................................... 37 
[Ecuación 5.3.12] ......................................................................................................................... 38 
[Ecuación 5.4] .............................................................................................................................. 38 
[Ecuación 5.5] .............................................................................................................................. 38 
[Ecuación 5.6] .............................................................................................................................. 38 
[Ecuación 5.7] .............................................................................................................................. 38 
[Ecuación 5.8] .............................................................................................................................. 39 
[Ecuación 5.9] .............................................................................................................................. 39 
[Ecuación 5.10] ............................................................................................................................ 39 
[Ecuación 5.11] ............................................................................................................................ 39 
[Ecuación 5.12] ............................................................................................................................ 39 
[Ecuación 5.13] ............................................................................................................................ 40 
[Ecuación 5.14] ............................................................................................................................ 40 
[Ecuación 5.15] ............................................................................................................................ 41 
[Ecuación 5.16] ............................................................................................................................ 41 
[Ecuación 5.17] ............................................................................................................................ 41 
[Ecuación 5.18] ............................................................................................................................ 41 
[Ecuación 5.19] ............................................................................................................................ 41 
[Ecuación 5.20] ............................................................................................................................ 41 
 
9 
 
[Ecuación 5.21] ............................................................................................................................ 41 
[Ecuación 5.22] ............................................................................................................................ 41 
[Ecuación 5.23] ............................................................................................................................ 42 
[Ecuación 5.24] ............................................................................................................................ 42 
[Ecuación 5.25] ............................................................................................................................ 42 
[Ecuación 5.26] ............................................................................................................................ 43 
[Ecuación 5.27] ............................................................................................................................ 43 
[Ecuación 5.28] ............................................................................................................................ 43 
[Ecuación 5.29] ............................................................................................................................ 44 
[Ecuación 5.30] ............................................................................................................................ 44 
[Ecuación 5.31] ............................................................................................................................ 45 
[Ecuación 5.32] ............................................................................................................................ 45 
[Ecuación 5.33] ............................................................................................................................ 45 
[Ecuación 5.34] ............................................................................................................................ 45 
[Ecuación 5.35] ............................................................................................................................ 46 
[Ecuación 5.36] ............................................................................................................................ 46 
[Ecuación 5.37] ............................................................................................................................ 46 
[Ecuación 5.38] ............................................................................................................................ 47 
[Ecuación 5.39] ............................................................................................................................ 47 
[Ecuación 5.40] ............................................................................................................................ 47 
[Ecuación 5.41] ............................................................................................................................ 47 
[Ecuación 5.42] ............................................................................................................................ 47 
[Ecuación 5.43] ............................................................................................................................ 47 
[Ecuación 5.44] ............................................................................................................................ 47 
[Ecuación 7.1] .............................................................................................................................. 70 
[Ecuación 7.2] .............................................................................................................................. 70 
[Ecuación 7.3] .............................................................................................................................. 71 
[Ecuación 7.4] .............................................................................................................................. 71 
[Ecuación 7.5] .............................................................................................................................. 71 
[Ecuación 7.6] .............................................................................................................................. 71 
[Ecuación 7.7] .............................................................................................................................. 74 
[Ecuación 7.8] .............................................................................................................................. 76 
[Ecuación 7.9] ..............................................................................................................................76 
[Ecuación 7.10] ............................................................................................................................ 77 
[Ecuación 7.11] ............................................................................................................................ 78 
[Ecuación 7.12] ............................................................................................................................ 78 
 
10 
 
 
1. INTRODUCCIÓN Y OBJETIVOS 
 
El objetivo de este proyecto ha sido crear un entorno de desarrollo sencillo y 
económico para un mejor entendimiento de la robótica industrial en el ámbito 
docente, detallándose los aspectos necesarios para el control y el 
funcionamiento de este, así como las comunicaciones industriales utilizadas 
entre el robot y el ordenador. 
Es por ello que se ha desarrollado desde el firmware y el hardware para el 
control del robot, hasta la interfaz usada para manejar el mismo y las 
comunicaciones industriales que enlazan el PC con el robot, quedando 
detallados todos los procedimientos necesarios en esta memoria. 
De esta forma queda reflejada en el proyecto la unión de las diferentes 
materias necesarias para la implementación de un brazo robótico de uso 
docente, centrándose la memoria en las partes que, al parecer del proyectista, 
son las de mayor relevancia. 
El entorno de desarrollo (Figura 1.1) consta de un brazo robótico docente 
impreso en plástico mediante una impresora 3D de bajo coste, dos 
microcontroladores encargados del control del robot y de las comunicaciones, 
el hardware necesario para el funcionamiento del brazo y, por último, un 
programa creado para manipular el robot. 
 
 
 
Figura 1.1. Diagrama del entorno de desarrollo. 
 
Las cuatro grandes tareas llevadas a cabo para la realización de este proyecto 
son las siguientes: 
1- Estudio del modelo cinemático del brazo robótico. 
2- Programación software de sistemas embebidos. 
 
11 
 
3- Creación de un sistema GUI (Graphical User Interface) para el manejo 
del brazo. 
4- Implementación hardware del sistema ACS (Acondicionamiento y 
Conversión de Señales) de los actuadores del brazo y de las 
comunicaciones industriales. 
 
 
 
 
 
 
 
 
 
 
 
 
 
12 
 
2. Marco Normativo Legal y recursos 
 
2.1 Disposiciones legales y normas aplicadas. 
 
Directivas 
 
▪ Directiva 2012/19/UE del Parlamento Europeo y del Consejo, de 4 de 
julio de 2012, sobre residuos de aparatos eléctricos y electrónicos. 
▪ Directiva 2008/98/CE del Parlamento Europeo y del Consejo, de 19 de 
noviembre de 2008, sobre los residuos y por la que se derogan 
determinadas Directivas (Directiva marco de residuos). 
▪ Directiva 2004/108/CE del Parlamento Europeo y del Consejo, de 15 
de diciembre de 2004 relativa a la aproximación de las legislaciones 
de los Estados miembros en materia de compatibilidad 
electromagnética y por la que se deroga la Directiva 89/336/CE. 
 
Normas 
▪ UNE-EN ISO 10218-1:2011. Robots y dispositivos robóticos. 
Requisitos de seguridad para robots industriales -- Parte 1: Robots. 
▪ UNE-EN ISO 10218-2:2011. Robots y dispositivos robóticos. 
Requisitos de seguridad para robots industriales -- Parte 2: Sistemas 
robot e integración. 
▪ UNE-EN 6095-22:2017. Equipos de tecnología de la información. 
Seguridad. 
▪ UNE-EN 6100-4-14:2001/A1. Compatibilidad electromagnética. 
▪ UNE-EN 55034. Equipos de tecnología de la información. 
Características de inmunidad. Límites y métodos de medida. 
▪ ISO 11898-1:2015. 
 
Reglamentos 
▪ Reglamento Electrotécnico de Baja Tensión, aprobado por el Real 
Decreto 842/2002 del 2 de agosto, así como sus Instrucciones 
Técnicas complementarias. 
Leyes 
▪ Ley 21/1992 de 16 de Julio, de Industria. B.O.E. N.º 176 publicado el 
23/07/1992. 
 
 
13 
 
2.2 Programas utilizados. 
 
▪ STM32CubeIDE 1.2.0. 
▪ PuTTY 0.71. 
▪ MATLAB R2018b. 
▪ FreeCAD 0.18. 
▪ Meshmixer 3.5. 
▪ Ultimaker Cura 4.1.0. 
▪ PSpice 9.2. 
▪ Schmeme-it. 
▪ Herramienta Recortes de Windows 10. 
▪ Office 365: Microsoft Office Word. 
▪ Office 365: Microsoft Office Exel. 
▪ Office 365: Microsfot Office Power Point. 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
14 
 
3. DEFINICIONES Y ABREVIATURAS 
 
▪ GUI: Graphical User Interface. 
▪ ACS: Acondicionamiento y Conversión de Señales. 
▪ μC: Microcontrolador. 
▪ PC: Personal Computer. 
▪ CAN: Controller Area Network. 
▪ ID: Número de Identificación. 
▪ ADC: Analog Digital Converter. 
▪ tq: tiempo de quantum. 
▪ USB: Universal Serial Bus. 
▪ UART: Universal Asynchronous Receiver Transmitter. 
▪ USART: Universal Synchronous/Asynchronous Receiver Transmitter. 
▪ PWM: Pulse-Width Modulation. 
▪ GDL: Grados De Libertad. 
▪ FIFO: First In, First Out. 
▪ SRAM: Static Random Acess Memory. 
▪ ISR: Interrupt Service Routines. 
▪ PLL: Phase Locked Loop. 
▪ CCRx: Capture/Compare Register x. 
▪ IDE: Entorno de desarrollo Integrado. 
▪ LL: Low-Layers APIs. 
▪ HAL: Hardware Abstraction Layer. 
▪ DH: Denavit-Hartenberg. 
▪ TFE: Trabajo de Fin de Estudios. 
 
 
 
 
 
15 
 
4. ANÁLISIS DE SOLUCIONES 
 
En este capítulo se verán las distintas alternativas estudiadas para llevar a cabo 
cada uno de los apartados técnicos de este proyecto. 
Los criterios básicos que se han priorizado para la búsqueda de soluciones en 
todas las secciones de este apartado son los siguientes: 
1- Facilidad de réplica para su uso extendido en el entorno docente. 
2- Costo mínimo de los materiales y componentes. 
3- Viabilidad de su uso didáctico. 
Aparte, se pueden haber seguido otros criterios específicos para cada sección, 
en cuyo caso se detallará en el lugar oportuno. 
 
4.1 Alternativas de Brazos robóticos para la docencia. 
 
Para la elección del modelo físico del brazo se han tenido en cuenta varias 
opciones, entre las cuales está el diseño del mismo mediante Catia, pero debido 
a lo extenso de las materias tocadas en el TFG se ha tomado la decisión de 
obtener los ficheros de impresión de uno que fuese de código abierto e imprimirlo 
mediante impresión 3D. 
Debido al auge de la impresión 3D cada vez se encuentran más modelos de 
brazos robóticos; la mayoría de ellos son de código abierto, por lo que todos sus 
archivos están a libre disposición en internet. Se pueden encontrar brazos de 
todo tipo y con diferentes herramientas para utilizar. Debido a la gran cantidad 
de opciones, en este apartado solo se van a mostrar los candidatos finales que 
se han tenido en cuenta. 
Para la selección del robot se han seguido los siguientes criterios: 
- Que sea fácilmente replicable. 
- Que sea lo más económico posible. 
- Que tenga un mínimo de 4GDL (Grados De Libertad). 
Los brazos estudiados son los siguientes: 
 
 
 
 
 
 
 
16 
 
Thor 
 
 
Figura 4.1. Brazo robótico Thor. 
 
Este es un brazo de 6GDL, cuya configuración le permite una gran flexibilidad en 
cuanto a los movimientos que puede realizar. La disposición que implementa es 
con frecuencia utilizada en robots industriales existentes actualmente en el 
mercado. 
Mide aproximadamente 62,5cm y puede levantar objetos de unos 750g de peso. 
Otra de sus ventajas es que se le pueden acoplar diferentes entre las disponibles 
en cualquier brazo robótico industrial. 
La principal desventaja de este robot es el precio. Debido a que utiliza motores 
paso a paso de gran tamaño en lugar de servomotores, este se encarece 
considerablemente, costando el total de sus materiales 87€. También requiere 
una mayor cantidad de plástico para imprimirlo que las otras opciones estudiadas 
(800g aproximadamente), lo que se refleja en la cantidadde horas necesarias 
para imprimirlo y la complejidad de las piezas que lo componen. 
 
EEZYBOTARM MK2 
 
 
Figura 4.2. Brazo robótico EEZYBOTARM MK2. 
 
17 
 
Esta solución es la más simple de las analizadas. Consta de 4GDL y la cantidad 
de plástico gastada es mínima. El brazo imita la configuración del ABB IRB460 
(ampliamente utilizado en la industria) con una ratio de 1:7. 
Su ventaja más destacable es el bajo coste que tiene, encontrándose los 
materiales para montarlo por 20€. Su mayor desventaja a tener en cuenta es que 
los servomotores utilizados en el robot no son demasiado precisos y la cantidad 
de peso que puede levantar es muy pequeña en comparación con las otras 
soluciones. 
 
DIY Arduino Robot Arm 
 
 
Figura 4.3. Brazo robótico EEZYBOTARM MK2. 
 
Aquí se muestra una solución intermedia entre las dos propuestas en los puntos 
anteriores. Este brazo consta de 5GDL. Los tres primeros GDL realizan el 
movimiento del hombro, mientras que los dos últimos corresponden a los de la 
muñeca del robot, que se encargan de la orientación de la herramienta. El coste 
de los materiales necesarios para montar el brazo es de 31€ (detallado en los 
presupuestos). 
Es un brazo diseñado para una rápida impresión, haciendo uso solo de 150g de 
plástico. El tamaño es de unos 44cm cuando está extendido y los servomotores 
que utiliza en las articulaciones llevan todos engranajes de metal, lo que mejora 
la precisión de sus movimientos. El peso máximo que puede levantar son 200g. 
El precio del brazo es muy económico, ya que los motores utilizados son de bajo 
coste. 
 
18 
 
Los archivos para imprimirlo se pueden obtener en la web: 
https://howtomechatronics.com/tutorials/arduino/diy-arduino-robot-arm-with-
smartphone-control/. 
 
La siguiente tabla muestra un resumen de las alternativas: 
Modelo 
Complejidad del 
modelo Precio(€) GNL precisión 
Thor Muy alta 87 6 Alta 
EEZYBOTARM MK2 Baja 20 4 Baja 
DIY Arduino Robot 
Arm Media 31 5 Media 
 
Tabla 4.1. Alternativas de Brazos robóticos. 
 
Solución escogida 
Entre los candidatos se ha elegido el modelo de 5GDL, mostrado en el apartado 
anterior, ya que es el que muestra un mayor equilibrio en cuanto a precio y 
características técnicas. 
Además, se le han realizado algunas modificaciones para mejorar ciertos 
aspectos del robot. 
La primera ha sido sustituir servo B (Figura 4.3) por uno de mayor potencia. 
El servo que tenía era un MG996R y se ha remplazado por el HDKJ D3615 con 
un torque de 14kgcm en lugar de 11kgcm. 
La segunda modificación ha consistido en sustituir los servos D y E (Figura 4.3) 
por unos de igual potencia, pero con los engranajes de metal, en lugar de 
plástico, para así conseguir una notable mejora en la precisión del brazo. 
Los servos utilizados son: 
- 1x HDKJ D3615. Para la Articulación B (Figura 4.3). 
- 2x MG996R-Tower pro. Para las Articulaciones A y C (Figura 4.3) 
- 2x MG90S-Tower pro. Para la articulación D y E (Figura 4.3). 
- 1x SG90S. Para las pinzas (F en la Figura 4.3). 
La tercera modificación realizada ha consistido en rediseñar tres piezas del brazo 
robótico para poder simplificar el modelo matemático, ya que este brazo no fue 
diseñado para obtener su modelo cinemático y este modelo es incompatible con 
alguno de los procedimientos que se van a seguir para obtener las ecuaciones 
de movimiento. Aparte de esto, los brazos industriales también utilizan modelos 
matemáticos lo más simples posibles para, así, simplificar un problema que 
puede volverse muy complejo si se modela de forma poco eficiente. 
https://howtomechatronics.com/tutorials/arduino/diy-arduino-robot-arm-with-smartphone-control/
https://howtomechatronics.com/tutorials/arduino/diy-arduino-robot-arm-with-smartphone-control/
 
19 
 
Para diseñar las nuevas piezas se ha hecho que todos los ejes de las diferentes 
articulaciones puedan ser recogidas en un mismo plano. Este requerimiento en 
el diseño es habitual en la mayoría de los brazos industriales e incluso en otros 
tipos de robots o modelos cinemáticos, como se muestra en las siguientes 
figuras: 
 
Figura 4.4. Sistema de Referencia para el robot KUKA KR6. 
 “Study and modeling of KR 6KUKA robot”- Página 135. 
 
Figura 4.5. Diseño de una mano de 28 GDL. 
 “Modelo CAD de mano humana de 28 grados de libertad para el estudio 
cinemático”- Página 3. 
 
Figura 4.6. Estructura cinemática de la pierna y el robot. 
 “Modelado y simulación de un Robot Terapéutico para la Rehabilitación de 
Miembros Inferiores”- Página 3. 
 
20 
 
En las tres figuras anteriores se puede observar que todos los grados de libertad 
pueden ser contenidos en un solo plano. 
Por ello, se han desplazado algunos ejes de las articulaciones del brazo robótico 
para que así quedasen en el mismo plano, como se ha comentado 
anteriormente. Las piezas modificadas se muestran en las siguientes figuras: 
 
 
Figura 4.7. Pieza modificada 1. 
 
 
Figura 4.8. Pieza modificada 2. 
 
 
 
21 
 
 
Figura 4.9. Pieza modificada 3. 
 
 
A la izquierda de cada una de ellas se muestra la pieza original en blanco y a la 
derecha la modificada en gris. 
 
4.2 Alternativas de tecnologías de comunicaciones 
industriales. 
 
En este apartado se analizan las diferentes tecnologías industriales tenidas en 
cuenta para decidir cuál de ellas es la que mejor se adapta a este proyecto. 
 
ZigBee 
Es una tecnología de comunicación inalámbrica ampliamente utilizada en la 
industria y en otras aplicaciones, tales como la domótica. Está tecnología se 
encuentra en la norma IEEE 802.15.4. 
Se basa en la creación de una red en malla compuesta por nodos, de los 
cuales uno es el coordinador; es decir, es el encargado de crear la red. Otros 
hacen de routers para encaminar la comunicación y otros son los dispositivos 
de terminal. 
- Distancia: 90m en interiores, 1800m a rango de vista directa. 
- Velocidad: Entre 2,4 GHz y 868 MHz. 
Se trata de una tecnología muy potente para la realización de nuestro proyecto; 
su mayor impedimento es la complejidad de la comunicación, ya que hablamos 
de una red en malla en la que los routers han de decidir cuál es el camino 
óptimo que ha de seguir la información, además de seguir el estándar IEEE 
802.15.4, lo que hace que no se encuentren módulos que la implementen a un 
 
22 
 
precio bajo. Uno de los más utilizados es el XBee SC2, que no se encuentra 
por menos de 20 €. 
 
Ethernet 
Esta tecnología se utiliza cada vez más en la industria para sustituir los buses 
de datos. Igualmente, es una tecnología cableada que aporta una gran 
velocidad en el envío de datos. Esta comunicación está detallada en la norma 
IEEE 802.3. 
 
 
Tabla 4.2. Velocidad y distancia de Ethernet según el cable. 
 
- Distancia: 100m en interiores. 
- Velocidad: 2,4 GB/s. 
La implementación de este protocolo en un microcontrolador tiene cierta 
complejidad; por lo general, se suele colocar un módulo que adapte la 
comunicación ethernet a una comunicación serie para el enviar los datos al 
microcontrolador. Para este proyecto, en el que se van a implementar 
comandos cortos para controlar el brazo robótico, es una opción que cumple 
sobradamente con los requisitos. 
 
Bus CAN 
El bus CAN (Controller Area Network) es una de las tecnologías de comunicación 
más utilizadas en la industria. Se trata de una opción cableada. Una de las 
características más importantes de este tipo de comunicación es lo seguro del 
protocolo en cuanto a envío de datos, ya que dispone de una alta inmunidad a 
las interferencias y puede autodiagnosticar y reparar errores en los datos 
recibidos. 
- Distancia: De 20m a 5000m. 
- Velocidad: De 1000kbit/s a 10kbit/s.23 
 
Como se observa, las distancias a las que se puede llegar con este bus son muy 
grandes, pero es a costa de reducir bastante la velocidad de transmisión de 
datos. Para el caso que se está estudiando, no se requiere de una gran distancia, 
por lo que las velocidades son más que aceptables. 
Otra de las ventajas de esta comunicación es su bajo coste. No necesita de 
dispositivos adicionales, como routers que manejen el envío de datos. 
Simplemente se puede implementar mediante un microcontrolador y algo de 
hardware para adaptar las comunicaciones. 
Las soluciones estudiadas se recogen en la siguiente tabla: 
Tipo 
Complejidad 
para 
implementarla 
Precio de 
la solución 
Distancia 
máxima(m) Velocidad 
ZigBee Muy alta Alto 90 Muy Alta 
Ethernet Media Medio 100 Muy Alta 
Bus CAN Baja Bajo 5000 Aceptable 
 
Tabla 4.3. Alternativas de Comunicaciones industriales. 
 
Solución escogida 
Dado que los datos a enviar en el proyecto van a ser cortas tramas de Bytes 
con los comandos necesarios para manejar el robot y que se está intentando 
priorizar el bajo costo económico en materiales, se escogerá el bus CAN como 
método de comunicación industrial. Este ofrece una buena velocidad en el 
envío de datos y un bajo coste, aparte de ser una de las opciones más 
utilizadas en la industria. 
Debido a que este protocolo ha sido previamente estudiado por el proyectista y 
dispone de un conocimiento previo de él, ha decidió crear la librería para el 
código que lleve a cabo todo lo referente a su manejo, para que quede 
reflejado de forma más didáctica cómo funcionan las comunicaciones. 
 
 
4.3 Alternativas de microcontroladores. 
 
Una vez elegido el bus CAN como método de comunicación y el modelo de brazo 
robótico, se ha seleccionado un microcontrolador para llevar a cabo tanto el 
manejo de los actuadores como las comunicaciones industriales. Para ello, se 
han tenido en cuenta los criterios anteriormente mencionados y además se ha 
buscado un microcontrolador que sea de uso extendido para que tenga una gran 
comunidad de la que se puedan nutrir los estudiantes. También se ha valorado 
 
24 
 
la potencia de cálculo del procesador y la calidad de los periféricos integrados 
en el microcontrolador. Por eso mismo, se han estudiado 3 posibilidades: 
1- Arduino. 
2- MSP430. 
3- STM32. 
 
Arduino 
Dentro de la familia de Arduino se encuentran una gran cantidad de dispositivos 
de diferentes precios y características. Esta es la opción que tiene una mayor 
comunidad y es, probablemente, la más extendida hoy en día para el prototipado 
rápido. Su mayor inconveniente es que los dispositivos más usados, como son 
el Arduino uno, el Arduino mega o el Arduino nano están basados en 
microcontroladores ATmega de 8 bits, lo que limita mucho su potencia de cálculo. 
Otro inconveniente es que los periféricos como timers y los ADCs no tienen una 
resolución tan buena como las otras alternativas. El precio de estos dispositivos 
varía en función de sus especificaciones. Desde 5€ se encuentra el Arduino 
nano, pero el precio aumenta a medida que se buscan modelos con más pines 
disponibles, como el Arduino Mega, disponible desde 14€. 
 
MSP430 
En la gran familia de los MSP430 se encuentran diversos dispositivos con los 
periféricos necesarios para el proyecto. Estos microcontroladores están 
enfocados al manejo de gran cantidad de periféricos, manteniendo un bajo 
consumo. La comunidad no es tan grande como la de Arduino, pero el fabricante 
pone a disposición de todo el mundo una gran cantidad de ejemplos y muchas 
de las librerías de Arduino adaptadas para el msp430. En cuanto a procesador, 
dispone de algo más de potencia que Arduino (procesador de 16 bits) y los 
periféricos tienen, en general, mayor resolución. El inconveniente es que los 
entornos de desarrollo que ofrece el fabricante son más caros y las opciones, 
más limitadas. El modelo MSP-EXP40FR6989, el cual cumple con los 
requerimientos para el proyecto se encuentra a 20€. 
 
STM32 
Por último, se han estudiado las diferentes familias del STM32. Estos dispositivos 
son los que tienen una mayor potencia de procesado entre los estudiados, 
debido a que están basados en las series M del procesador Cortex, 
procesadores de 32 bits que implementan tecnologías con mejores prestaciones 
a medida que se aumenta en la gama elegida. Dispone de una muy amplia 
comunidad también, y el fabricante ha creado una gran cantidad de librerías de 
libre acceso, como pueden ser las LL o las HAL. Analizando esta gama de 
microcontroladores, se encuentran opciones muy interesantes para el proyecto; 
entre ellas está la familia STM32F103xx, cuyos microcontroladores tienen un 
 
25 
 
cortex-M1 y que, además de tener todos los timers y UARTs necesarios, poseen 
como periférico integrado el bus CAN, con lo que el ahorro de tener que disponer 
aparte de un controlador para el bus es importante y simplifica la programación 
de las comunicaciones en gran medida. Hay varias placas de desarrollo que 
integran esta familia de microcontroladores y tienen una mayor potencia que 
Arduino y MSP430 además de cumplir con todas las especificaciones 
propuestas. El precio de estos chips es bajo, se encuentran placas de desarrollo 
con la familia STM32F103xx desde 3€. 
 
Tipo 
Potencia 
de cálculo Precio 
Documentación 
docente 
disponible 
Resolución 
de los 
timers 
Arduino Nano Baja Medio Muy Alta 8 bits 
MSP430FR6989 Media Alto Media 16 bits 
STM32F103xx Alta Bajo Alta 16 bits 
 
Tabla 4.4. Alternativas de microcontroladores. 
 
Solución escogida 
Teniendo en cuenta los criterios económicos y didácticos anteriormente 
mencionados, se ha concluido que la elección más optima es el STM32 y, en 
concreto, la familia STM32F103xx, pues al incluir el bus CAN, marca un ahorro 
significativo, ya que no tiene que añadir módulos adicionales al hardware para 
realizar esta tarea. Además, este chip es el más potente a nivel de cálculo que 
los mencionados anteriormente y el que tiene mayor versatilidad en cuanto a 
periféricos, lo que lo convierte en una opción todavía más adecuada. 
 
4.4 Alternativas de placas de desarrollo. 
 
En esta sección se han estudiado dos alternativas como entorno de desarrollo, 
que utilizan un microcontrolador de la familia STM32F103xx. 
 
26 
 
 
Tabla 4.5. STM32F103xx device features. 
 “STM32F103xx datasheet”- Página 10. 
 
Las opciones estudiadas llevan los microcontroladores marcados en la Tabla 4.2. 
Como se puede observar, las diferencias entre ambos chips son pequeñas. Solo 
varía el número de entradas y salidas digitales y el de canales del convertidor 
analógico/digital, lo que no es relevante para este proyecto. 
 
NUCLEO-F103RB 
Esta placa de desarrollo es ensamblada por el fabricante. Viene en una placa 
NUCLEO, que está dotada de tecnologías muy interesantes para la depuración 
de código y se puede programar directamente sin hacer uso de un dispositivo 
aparte para llevar a cabo dicha tarea. 
La placa se puede encontrar por 10 €. Dispone de bastantes pines accesibles 
para su uso, entre ellos los necesarios para el proyecto. Además, está diseñada 
pensando en ser compatible con módulos para Arduino. Como desventaja, hay 
que señalar que su tamaño es algo grande para conseguir una solución 
compacta del hardware en los circuitos que se van a realizar. 
 
27 
 
 
Figura 4.10. Placa NUCLEO-F103RB. 
 
 
Blue Pill 
Es una placa de desarrollo bastante simple (Figura 4.11). Lo más destacable es 
su precio, ya que se puede encontrar desde 3€ en diferentes proveedores de 
internet. Esta incluye lo necesario para usar el microcontrolador y gran parte de 
sus periféricos, además integra un cristal de cuarzo para poder trabajarcon 
velocidades de 72 MHz. También es destacable lo compacta que es, pues tiene 
un tamaño un poco mayor que el de un Arduino Nano, lo que es una ventaja a la 
hora de montarla en el hardware. La mayor pega que tiene es que se necesita 
un dispositivo para programarla mediante comunicación serie. El programador 
que se suele utilizar es el ST-LINK V2 (Figura 4.12). 
 
 
Figura 4.11. Placa Blue Pill. 
 
28 
 
 
Figura 4.12. Programador ST-LINK V2. 
 
Solución escogida 
Finalmente, basándose principalmente en los criterios económicos y en lo 
comentado en el párrafo anterior, la placa elegida es la Blue Pill. 
 
 
4.5 Alternativas para la programación de los 
microcontroladores. 
 
El STM32 se puede programar usando diversas formas, En esta sección se van 
a detallar las que han sido analizadas: 
 
Mediante librerías oficiales 
Una de las opciones más utilizadas para programar este microcontrolador es el 
uso de las bibliotecas que el fabricante pone a disposición de la comunidad. 
Estas son las LL (Low-Layers APIs) y, las más actuales, las librerías HAL 
(Hardware Abstraction Layer). Ambas son buenas opciones para programar este 
micro. Las LL proporcionan un mayor control al programador del código que está 
realizando, ya que estas librerías tienen funciones más simples y rápidas de 
ejecutar, lo que a su vez las hace más fáciles de interpretar. Por el contrario, 
estas son menos genéricas que las HAL, es decir, habrá que realizar más 
modificaciones si queremos utilizar el mismo código en dos dispositivos distintos 
dentro de la familia del STM32. Además, HAL permite una mayor velocidad a la 
hora de programar y la utilización de CUBEMX, el cual dota al programador de 
un entorno gráfico para configurar todas las características internas del micro, 
como las velocidades de reloj, los ADC (Analog Digital Converter), timers, etc. 
Todo esto quita una gran carga al programador a la hora de crear el código, a 
costa de utilizar unas librerías más difíciles de analizar y menos eficientes en 
cuanto a tiempo de cómputo. 
 
 
29 
 
STM32duino 
El STM32F103 también puede ser programado mediante librerías de Arduino 
adaptadas para que funcionen en este micro. Hay una gran cantidad de 
librerías, aunque no tantas como para dispositivos como el Arduino Uno. 
Cuando se programa de esta forma se habla del STM32duino entre la 
comunidad. Es una forma interesante ya que permite trabajar en el IDE de 
Arduino y además usar las librerías HAL. El lado negativo es que la 
optimización a nivel computacional también es baja. Las librerías de Arduino, 
en general, están pensadas para ser sencillas de usar, pero no rápidas. 
 
Programación mediante registros 
La última forma analizada para programar el STM32 es la programación 
mediante el uso directo de los registros del microcontrolador. Esta tiene la 
ventaja de que se posee un mayor control sobre el código, debido a que las 
librerías y funciones usadas son las creadas por el programador. También es la 
que permite una mayor optimización del código, pudiéndose usar los recursos 
necesarios del micro sin tener que lidiar con librerías que comprueben si es 
compatible con el dispositivo o lleven a cabo otro tipo de interacciones no 
deseadas en el programa. Su mayor inconveniente es que hay que crear todas 
las librerías y programar “desde cero”. El tiempo inicial invertido en crear las 
librerías básicas es importante, pero se compensa con el entendimiento que se 
adquiere sobre el funcionamiento del STM32. Desde el punto de vista del 
proyectista esta es la opción más didáctica. 
 
Solución escogida 
La programación de los microcontroladores en este proyecto se hará mediante 
registros, según lo expuesto en el párrafo anterior. Se pretende que esta sea lo 
más didáctica posible. Otra motivación para programar de esta manera ha sido 
la falta de ejemplos y la poca información que hay para trabajar con el STM32 
mediante sus registros, pudiendo ser el código creado para este proyecto una 
ayuda para entender mejor el funcionamiento del microcontrolador. 
 
 
 
 
 
 
 
30 
 
5. MODELO CINEMÁTICO 
 
En este apartado se va a realizar un estudio del movimiento del brazo robótico 
con respecto a un sistema de referencia. La cinemática trata la descripción 
analítica del movimiento espacial del robot, mostrando la relación existente entre 
la posición y orientación del extremo final del robot con cada grado de libertad 
asociado a una articulación del brazo. 
Hay dos problemas básicos a resolver en esta sección (Figura 4.1); el problema 
cinemático directo, que consiste en determinar la posición y orientación del 
extremo final del robot, conocidos los movimientos relativos entre cada una de 
las articulaciones y las medidas del robot. El segundo es el problema cinemático 
inverso, que trata de encontrar los valores de cada coordenada articular, 
conocidas la posición y la orientación del extremo final del robot. 
 
 
 Figura 5.1. Diagrama de relación entre cinemática directa e inversa. 
“Fundamentos de Robótica”- Página 94. 
 
Una vez se ha llegado a este punto del proyecto, se ha constatado un grave 
problema en el diseño del brazo para hallar estas soluciones. Esto es debido a 
que el brazo robótico elegido no fue diseñado para realizar estos modelos, sino 
que, probablemente, se diseñó pensando en que fuese compacto y tuviese un 
diseño atractivo y asequible desde el punto de vista económico. 
El problema radica en que los ejes de las articulaciones no están alineados en el 
mismo plano y que los ejes perpendiculares entre sí no coinciden en un punto 
con los ejes de las articulaciones contiguas. Esto complica enormemente el 
modelo matemático tanto a nivel analítico como computacional. Para que se vea 
la gravedad del asunto, la mayoría de los brazos robóticos industriales u otros 
tipos de robot, como las aproximaciones del brazo humano, se hacen teniendo 
en cuenta estos criterios. Es debido a esto que se han tenido que modificar tres 
piezas (Figura 4.7, Figura 4.8, Figura 4.9) del brazo robótico para que todos los 
ejes de las articulaciones queden en el mismo plano y también para simplificar 
las posiciones relativas entre sus ejes. 
 
 
31 
 
5.1 Problema cinemático directo. 
 
Para resolverlo se usará el método matricial DH (Denavit-Hartenberg), que 
establece un sistema de coordenadas 𝑆𝑖 ligado a un eslabón i de una cadena 
articulada, para determinar a continuación las ecuaciones cinemáticas de la 
cadena completa. Para ello, hay que escoger los sistemas de coordenadas 
asociados a cada eslabón del robot haciendo uso únicamente de las siguientes 
cuatro transformaciones básicas para pasar del sistema de un eslabón al 
siguiente: 
1- Rotación alrededor del eje 𝑧𝑖−1 un ángulo θ𝑖. 
2- Traslación a lo largo del eje 𝑧𝑖−1 una distancia d𝑖. 
3- Traslación a lo largo del eje 𝑥𝑖 una distancia a𝑖. 
4- Rotación alrededor del eje 𝑥𝑖 un ángulo α𝑖. 
Por lo tanto, cada matriz de transformación asociada al sistema 𝑆𝑖 de cada 
eslabón queda definida de la siguiente forma (Ecuación 4.2): 
𝐴𝑖−1
𝑖 = 𝑇(𝑧, θ𝑖)𝑇(0,0, 𝑑𝑖)𝑇(𝑎𝑖, 0,0)𝑇(𝑥, α𝑖) 
[Ecuación 5.1] 
 
𝐴𝑖−1
𝑖 = (
𝐶θ𝑖 −𝑆θ𝑖
𝑆θ𝑖 𝐶θ𝑖
0 0
0 0
0 0
0 0
1 0
0 1
)(
1 0
0 1
0 0
0 0
0 0
0 0
1 𝑑𝑖
0 1
)(
1 0
0 1
0 𝑎𝑖
0 0
0 0
0 0
1 0
0 1
)(
1 0
0 𝐶α𝑖
0 0
−𝑆α𝑖 0
0 𝑆α𝑖
0 0
𝐶α𝑖 0
0 1
) = 
= (
𝐶θ𝑖 −𝐶α𝑖𝑆θ𝑖
𝑆θ𝑖 𝐶α𝑖𝐶θ𝑖
𝑆α𝑖𝑆θ𝑖 𝑎𝑖𝐶θ𝑖
−𝑆α𝑖𝐶θ𝑖 𝑎𝑖Sθ𝑖
0 0
0 0
1 𝑑𝑖
0 1
) 
[Ecuación 5.2] 
 
Para obtener cada uno de los Sistemas de coordenadas de cada eslabón se 
debe seguir el siguiente algoritmo: 
1- Numerar los eslabones empezando por0 (eslabón fijo) y siguiendo por 
todos los eslabones móviles hasta el eslabón n. 
2- Numerar las articulaciones comenzando por 1 para el primer GDL 
hasta el n. 
3- Localizar el eje de cada articulación. Para los pares de revolución 
coincidirá con el eje de giro y para prismáticos con el eje en el cual se 
produce el desplazamiento. 
4- Colocamos las direcciones 𝑧𝑖−1 en los ejes de las articulaciones i para 
i = 1, … , n. 
5- Se coloca el sistema 𝑆0 en cualquier punto del eje z. Los ejes 𝑥0 e 𝑦0 
se situarán formando un sistema dextrógiro con 𝑧0 y de forma que se 
 
32 
 
puedan seleccionar las coordenadas del sistema 𝑆1 aplicando 
únicamente las transformaciones básicas de la Ecuación 5.2. 
6- Para los demás sistemas 𝑆𝑖, siendo i = 1,… , n − 1, colocar 𝑧𝑖 en el eje 
de la articulación i+1. Si el eje de la articulación corta i se cruza en un 
punto con el de la articulación i+1 se colocarla en ese punto. 
7- Situar 𝑥𝑖 en dirección normal común a 𝑧𝑖−1 y 𝑧𝑖. 
8- Situar 𝑦𝑖 de forma que el sistema sea dextrógiro con 𝑥𝑖 y 𝑧𝑖 además 
de tener en cuenta que el sistema 𝑆𝑖+1 pueda seleccionarse aplicando 
únicamente las transformadas de la Ecuación 5.2. 
9- Colocar el sistema 𝑆𝑛 en el extremo del robot de forma que 𝑧𝑛 sea 
paralelo con 𝑧𝑛−1 . Los ejes 𝑥𝑛 e 𝑦𝑛 deben colocarse en direcciones 
válidas. 
10- Obtener θ𝑖 como el ángulo que va desde 𝑥𝑖−1 hasta 𝑥𝑖 girando 
alrededor del eje 𝑧𝑖−1. 
11- Obtener d𝑖 como la distancia en el eje 𝑧𝑖−1 que va desde el sistema 
𝑆𝑖−1 hasta 𝑆𝑖. 
12- Obtener a𝑖 como la distancia en el eje 𝑥𝑖 que va desde el sistema 𝑆𝑖−1 
hasta 𝑆𝑖. 
13- Obtener α𝑖 como el ángulo que va desde 𝑧𝑖−1 hasta 𝑧𝑖 girando 
alrededor del eje 𝑥𝑖. 
14- Obtener las matrices de transformación individuales 𝐴𝑖−1
𝑖 de toda la 
cadena. 
15- Obtener la matriz de transformación que relaciona la base del robot 
con el extremo 𝑇 = 𝐴0 1 𝐴1 2… 𝐴𝑛−1
𝑛 . 
 
Este algoritmo da múltiples soluciones, con lo que la experiencia y la metodología 
usada para aplicarlo es importante para obtener una solución más eficiente del 
problema. 
Aplicando este algoritmo al brazo robótico (Figura 5.2) se obtienen los siguientes 
parámetros (Figura 5.3): 
 
33 
 
 
Figura 5.2. Brazo robótico en posición inicial. 
 
Figura 5.3. Diagrama con parámetros DH del brazo robótico. 
 
 
34 
 
Se recogen los parámetros DH en la siguiente tabla: 
Articulación θ d(mm) a(mm) α 
1 q1 L1 = 96.3 L2 = 11 90 
2 q2 + 90 0 L3 = 120 0 
3 q3 0 L4 = -6 90 
4 q4 L5 = 120.5 0 90 
5 q5 + 90 0 L6 = 123 0 
 
Tabla 5.1. Diagrama con parámetros DH del brazo robótico. 
 
A continuación, se realiza una simulación en MATLAB haciendo uso del toolbox 
“robotics“ de Peter Corke, y se comprueba que con los parámetros Denavit-
Hartenberg escogidos se consigue el modelo esperado (Figura 5.4). 
El código usado es el siguiente: 
 
startup_rvc % Inicia toolbox "robotics". 
L1 = 11; L2 = 96.3; L3 = 120; L4 = -6; L5 = 120.5; L6 = 123; % Medidas 
del brazo. 
 
% Parámetros DH. 
% Theta d a alfa tipo offset 
L(1)= Link ([ 0 L2 L1 pi/2 0 0 ]); 
L(2)= Link ([ 0 0 L3 0 0 pi/2 ]); 
L(3)= Link ([ 0 0 L4 pi/2 0 0 ]); 
L(4)= Link ([ 0 L5 0 pi/2 0 0 ]); 
L(5)= Link ([ 0 0 L6 0 0 pi/2 ]); 
 
 
% Unión de eslabones. 
Rob = SerialLink(L) 
% Posición inicial de los ángulos de las articulaciones. 
q1 = 0; q2 = 0; q3= 0; q4 = 0; q5 = 0; 
 
Rob.plot([q1,q2,q3,q4,q5]) % Gráfica. 
Rob.fkine([q1,q2,q3,q4,q5]) % Cálculo de la matriz T. 
 
Este código muestra la posición del brazo robótico (Figura 5.4) y obtiene la matriz 
T, resultante de los parámetros q1,q2,q3,q4 y q5 en sus estados iniciales que 
sitúan el extremo del brazo en el punto [254.5 , 0 , 210.3] correspondiente al 
calculado teóricamente ([L1+L5+L6 , 0 , L2+L3-L4]), así como en la misma 
orientación. 
De esta manera, quedan definidos todos los valores DH para resolver el 
problema cinemático directo, ya que sustituyendo los valores deseados en 
q1,…,q5 se obtiene la posición del extremo del brazo y su orientación. 
 
 
35 
 
 
 Figura 5.5. Simulación de la posición inicial del robot. 
 
Con la función .fkine en MATLAB se ha calculado de forma automática la 
cinemática directa. Ahora se va a proceder a la obtención de las ecuaciones sin 
hacer uso de las funciones del toolbox. Para ello, se resuelve la siguiente 
ecuación, donde cada término procede de la Ecuación 5.2 y de la Tabla 5.1: 
 
𝑇 = [
𝑛𝑥
𝑛𝑦
𝑜𝑥
𝑜𝑦
𝑎𝑥
𝑎𝑦
𝑃𝑥
𝑃𝑦
𝑛𝑧
0
𝑜𝑧
0
𝑎𝑧
0
𝑃𝑧
1
] = 𝐴0 1 𝐴1 2 𝐴2 3 𝐴3 4 𝐴4 5 
[Ecuación 5.3] 
 
Esta se resuelve de forma sencilla con el siguiente código en MATLAB: 
% Se definen las variables simbólicas del sistema. 
syms q1 q2 q3 q4 q5 
syms L1 L2 L3 L4 L5 L6 
 
% Matrices de cada articulación. 
 
A01=[cos(q1) 0 sin(q1) L1*cos(q1); 
 sin(q1) 0 -cos(q1) L1*sin(q1); 
 0 1 0 L2 ; 
 0 0 0 1 ]; 
 
A12=[-sin(q2) -cos(q2) 0 -L3*sin(q2); 
 cos(q2) -sin(q2) 0 L3*cos(q2) ; 
 0 0 1 0 ; 
 0 0 0 1 ]; 
 
A23=[cos(q3) 0 sin(q3) L4*cos(q3); 
 
36 
 
 sin(q3) 0 -cos(q3) L4*sin(q3); 
 0 1 0 0 ; 
 0 0 0 1 ]; 
 
A34=[cos(q4) 0 sin(q4) 0 ; 
 sin(q4) 0 -cos(q4) 0 ; 
 0 1 0 L5 ; 
 0 0 0 1 ]; 
 
A45=[-sin(q5) -cos(q5) 0 -L6*sin(q5); 
 cos(q5) -sin(q5) 0 L6*cos(q5); 
 0 0 1 0 ; 
 0 0 0 1 ]; 
 
T05=A01*A12*A23*A34*A45 
 
 
De esta forma queda resuelto el problema cinemático directo, quedando 
definidos todos los términos de la matriz T de la Ecuación 5.3 en las siguientes 
ecuaciones: 
 
 
𝑛𝑥 = cos(𝑞5) ∗ (cos(𝑞1) ∗ cos(𝑞2) ∗ cos(𝑞3) − cos(𝑞1) ∗ sin(𝑞2) ∗ sin(𝑞3))
− sin(𝑞5)
∗ (𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞4) − 𝑐𝑜𝑠(𝑞4)
∗ (𝑐𝑜𝑠(𝑞1) ∗ 𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞1) ∗ 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2))) 
[Ecuación 5.3.1] 
 
 
𝑛𝑦 = sin(𝑞5) ∗ (cos(𝑞1) ∗ sin(𝑞4) + cos(𝑞4)
∗ (cos(𝑞2) ∗ sin(𝑞1) ∗ sin(𝑞3) + cos(𝑞3) ∗ sin(𝑞1) ∗ sin(𝑞2)))
− cos(𝑞5)
∗ (sin(𝑞1) ∗ sin(𝑞2) ∗ sin(𝑞3) − cos(𝑞2) ∗ cos(𝑞3) ∗ sin(𝑞1)) 
[Ecuación 5.3.2] 
 
𝑛𝑧 = 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)) − 𝑐𝑜𝑠(𝑞4) ∗ 𝑠𝑖𝑛(𝑞5)
∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) 
[Ecuación 5.3.3] 
 
𝑜𝑥 = − 𝑐𝑜𝑠(𝑞5) ∗ (𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞4) − 𝑐𝑜𝑠(𝑞4) ∗ (𝑐𝑜𝑠(𝑞1) ∗ 𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) 
+ 𝑐𝑜𝑠(𝑞1) ∗ 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2))) − 𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞1) ∗ 𝑐𝑜𝑠(𝑞2)
∗ 𝑐𝑜𝑠(𝑞3) − 𝑐𝑜𝑠(𝑞1) ∗ 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) 
[Ecuación 5.3.4] 
 
𝑜𝑦 = cos(𝑞5) ∗ (cos(𝑞1) ∗ sin(𝑞4) + cos(𝑞4)
∗ (cos(𝑞2) ∗ sin(𝑞1) ∗ sin(𝑞3) + cos(𝑞3) ∗ sin(𝑞1) ∗ sin(𝑞2)))
+ sin(𝑞5)
∗ (sin(𝑞1) ∗ sin(𝑞2) ∗ sin(𝑞3) − cos(𝑞2) ∗ cos(𝑞3) ∗ sin(𝑞1)) 
 
37 
 
[Ecuación 5.3.5] 
 
 
𝑜𝑧 = −sin(𝑞5) ∗ (cos(𝑞2) ∗ sin(𝑞3) + cos(𝑞3) ∗ sin(𝑞2)) − cos(𝑞4) ∗ cos(𝑞5)
∗ (cos(𝑞2) ∗ cos(𝑞3) − sin(𝑞2) ∗ sin(𝑞3)) 
[Ecuación 5.3.6] 
 
 
𝑎𝑥 = − 𝑐𝑜𝑠(𝑞4) ∗ 𝑠𝑖𝑛(𝑞1) − 𝑠𝑖𝑛(𝑞4) ∗ (𝑐𝑜𝑠(𝑞1) ∗ 𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞1)
∗ 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)) 
[Ecuación 5.3.7] 
 
 
𝑎𝑦 = 𝑐𝑜𝑠(𝑞1) ∗ 𝑐𝑜𝑠(𝑞4) − 𝑠𝑖𝑛(𝑞4) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3)
∗ 𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞2)) 
[Ecuación 5.3.8]𝑎𝑧 = 𝑠𝑖𝑛(𝑞4) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) 
[Ecuación 5.3.9] 
 
 
𝑃𝑥 = 𝐿1 ∗ cos(𝑞1) + 𝐿5
∗ (cos(𝑞1) ∗ cos(𝑞2) ∗ cos(𝑞3) − cos(𝑞1) ∗ sin(𝑞2) ∗ sin(𝑞3)) − 𝐿3
∗ cos(𝑞1) ∗ sin(𝑞2) − 𝐿6 ∗ sin(𝑞5)
∗ (sin(𝑞1) ∗ sin(𝑞4) − cos(𝑞4)
∗ (cos(𝑞1) ∗ cos(𝑞2) ∗ sin(𝑞3) + cos(𝑞1) ∗ cos(𝑞3) ∗ sin(𝑞2))) + 𝐿6
∗ cos(𝑞5)
∗ (cos(𝑞1) ∗ cos(𝑞2) ∗ cos(𝑞3) − cos(𝑞1) ∗ sin(𝑞2) ∗ sin(𝑞3)) − 𝐿4
∗ cos(𝑞1) ∗ cos(𝑞2) ∗ sin(𝑞3) − 𝐿4 ∗ cos(𝑞1) ∗ cos(𝑞3) ∗ sin(𝑞2) 
[Ecuación 5.3.10] 
 
 
𝑃𝑦 = 𝐿1 ∗ 𝑠𝑖𝑛(𝑞1) − 𝐿5 ∗ (𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) − 𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3)
∗ 𝑠𝑖𝑛(𝑞1)) − 𝐿3 ∗ 𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞2) + 𝐿6 ∗ 𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞1)
∗ 𝑠𝑖𝑛(𝑞4) + 𝑐𝑜𝑠(𝑞4) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3)
∗ 𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞2))) − 𝐿6 ∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞2)
∗ 𝑠𝑖𝑛(𝑞3) − 𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞1)) − 𝐿4 ∗ 𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞1)
∗ 𝑠𝑖𝑛(𝑞3) − 𝐿4 ∗ 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞1) ∗ 𝑠𝑖𝑛(𝑞2)] 
[Ecuación 5.3.11] 
 
 
 
 
38 
 
𝑃𝑧 = 𝐿2 + 𝐿5 ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)) + 𝐿3 ∗ 𝑐𝑜𝑠(𝑞2) + 𝐿6
∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)) + 𝐿4
∗ 𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝐿4 ∗ 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) − 𝐿6 ∗ 𝑐𝑜𝑠(𝑞4)
∗ 𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) 
[Ecuación 5.3.12] 
 
 
Estas ecuaciones corresponden a la posición (𝑃𝑥, 𝑃𝑦 y 𝑃𝑧) y la orientación 
(vectores 𝑛, 𝑠 y 𝑎) del extremo del robot. Se puede observar que la orientación 
queda en nueve ecuaciones, así que se va a pasar a expresar solo mediante los 
ángulos α, β y γ reduciendo el número de ecuaciones a solo tres. Estos ángulos 
corresponden a la rotación del sistema final de referencia respecto del sistema 
fijo, girándolo primero sobre el eje 𝑥0, luego sobre el eje 𝑦0 y por último sobre el 
eje 𝑧0: 
𝑅5 =
0 𝑅(𝑧0, γ)𝑅(𝑦0, β)𝑅(𝑥0, α) = 
= [
cos γ
sen γ
0
−sen γ
cos γ
0
0
0
1
] [
𝑐𝑜𝑠 β
0
−𝑠𝑒𝑛 β
0
1
0
𝑠𝑒𝑛 β
0
𝑐𝑜𝑠 β
] [
1
0
0
0
𝑐𝑜𝑠 α
𝑠𝑒𝑛 α
0
−𝑠𝑒𝑛 α
𝑐𝑜𝑠 α
] = 
= [
𝐶γ𝐶β
𝑆γ𝐶β
−𝑆β
−𝑆γ𝐶α + 𝐶γ𝑆β𝑆α
𝐶γ𝐶α + 𝑆γ𝑆β𝑆α
𝐶β𝑆α
𝑆γ𝑆α + 𝐶γ𝑆β𝐶α
−𝐶γ𝑆α + 𝑆γ𝑆β𝐶α
𝐶β𝐶α
] 
[Ecuación 5.4] 
 
Para simplificar la nomenclatura, 𝐶 y 𝑆 son el coseno y seno respectivamente. 
Se iguala esta matriz de rotación a la orientación final del robot, quedando: 
𝑅5 =
0 [
𝐶γ𝐶β
𝑆γ𝐶β
−𝑆β
−𝑆γ𝐶α + 𝐶γ𝑆β𝑆α
𝐶γ𝐶α + 𝑆γ𝑆β𝑆α
𝐶β𝑆α
𝑆γ𝑆α + 𝐶γ𝑆β𝐶α
−𝐶γ𝑆α + 𝑆γ𝑆β𝐶α
𝐶β𝐶α
] [
𝑛𝑥
𝑛𝑦
𝑛𝑧
𝑜𝑥
𝑜𝑦
𝑜𝑧
𝑎𝑥
𝑎𝑦
𝑎𝑧
] 
[Ecuación 5.5] 
 
De la Ecuación 4.5 es de donde se sacan los ángulos α, β y γ: 
α = 𝑎𝑟𝑐𝑡𝑔 (
𝐶β𝑆α
𝐶β𝐶α
) = 𝑎𝑟𝑐𝑡𝑔(
𝑜𝑧
𝑎𝑧
) 
[Ecuación 5.6] 
γ = 𝑎𝑟𝑐𝑡𝑔 (
𝑆γ𝐶β
𝐶γ𝐶β
) = 𝑎𝑟𝑐𝑡𝑔(
𝑛𝑦
𝑛𝑥
) 
[Ecuación 5.7] 
 
 
39 
 
 
senβ = −𝑛𝑧 
[Ecuación 5.8] 
cosβ = ±√1 − 𝑠𝑒𝑛2β 
[Ecuación 5.9] 
β = arctg (±
senβ
√1 − 𝑠𝑒𝑛2β
) = arctg (±
−𝑛𝑧
√1 − 𝑛𝑧2
) 
[Ecuación 5.10] 
 
5.2 Problema cinemático inverso 
 
Resolver este problema consiste en calcular todas las variables articulares a 
partir de la posición (Px, Py y Pz) y orientación (α, β y γ) conocidas del extremo 
del brazo robótico. Para ello, se van a calcular las variables q1, q2 y q3 
correspondientes a los tres primeros GDL mediante métodos geométricos. De 
esta forma, se obtiene primero la posición de la muñeca del brazo robótico 
correspondiente al punto Pm (Figura 5.5) sin tener en cuenta la orientación. 
Lo primero es construir la matriz T (Ecuación 5.11) a partir de los datos de 
partida. De esta forma se tienen que transformar los ángulos a la matriz de 
rotación [𝑛 𝑜 𝑎]: 
𝑇 = [
𝑛 𝑜 𝑎 𝑝
0 0 0 1
] = [
𝑛𝑥
𝑛𝑦
𝑜𝑥
𝑜𝑦
𝑎𝑥
𝑎𝑦
𝑃𝑥
𝑃𝑦
𝑛𝑧
0
𝑜𝑧
0
𝑎𝑧
0
𝑃𝑧
1
] 
[Ecuación 5.11] 
Esta transformación es directa, igualando los vectores [𝑛 𝑜 𝑎] de la Ecuación 
5.11 a la Ecuación 5.4. Se obtiene lo siguiente: 
𝑇 = [
𝑛𝑥
𝑛𝑦
𝑜𝑥
𝑜𝑦
𝑎𝑥
𝑎𝑦
𝑃𝑥
𝑃𝑦
𝑛𝑧
0
𝑜𝑧
0
𝑎𝑧
0
𝑃𝑧
1
] = [
𝐶γ𝐶β
𝑆γ𝐶β
−𝑆γ𝐶α + 𝐶γ𝑆β𝑆α
𝐶γ𝐶α + 𝑆γ𝑆β𝑆α
𝑆γ𝑆α + 𝐶γ𝑆β𝐶α
−𝐶γ𝑆α + 𝑆γ𝑆β𝐶α
 
𝑃𝑥
𝑃𝑦
−𝑆β 𝐶β𝑆α
 0 0
 
𝐶β𝐶α
0
 
𝑃𝑧
1
] 
[Ecuación 5.12] 
 
 
40 
 
 
Figura 5.6. Diagrama de los 3 primeros GDL del robot. 
 
Las medidas L, h y el ángulo Ψ quedan definido por las siguientes ecuaciones: 
𝐿 = √𝐿4
2 + 𝐿5
2 
[Ecuación 5.13] 
ℎ = 𝑃𝑧
′ − 𝐿2 
[Ecuación 5.14] 
 
 
41 
 
Ψ = 𝑎𝑟𝑐𝑡𝑔 (
𝐿5
−𝐿4
) 
[Ecuación 5.15] 
 
Se puede observar que el ángulo 𝑞1 queda directamente definido mediante el 
punto de la muñeca: 
𝑞1 = 𝑎𝑟𝑐𝑡𝑔 (
𝑃𝑦′
𝑃𝑥′
) 
[Ecuación 5.16] 
 
Ahora se calcula 𝑞2 utilizando el teorema de Pitágoras (Ecuación 5.20 y 
Ecuación 5.21) y el teorema del coseno (Ecuación 5.22) sobre el ángulo Φ. 
Previamente se define r’ (Ecuación 5.19): 
𝑟 = [𝑃𝑚𝑥 𝑃𝑚𝑦 𝑃𝑚𝑧] 
[Ecuación 5.17] 
 
𝑟′ = [𝑟𝑥′ 𝑟𝑦′ 𝑟𝑧′] 
[Ecuación 5.18] 
 
𝑟′ = 𝑟 − [𝐿1𝑐𝑜𝑠𝑞1 𝐿1𝑠𝑖𝑛𝑞1 𝐿2] → 
→ 𝑟′ = [𝑃𝑚𝑥 − 𝐿1𝑐𝑜𝑠𝑞1 𝑃𝑚𝑦 − 𝐿1𝑠𝑒𝑛𝑞1 𝑃𝑚𝑧 − 𝐿2] 
[Ecuación 5.19] 
 
𝑟𝑥𝑦
′ = √(𝑃𝑚𝑥 − 𝐿1𝑐𝑜𝑠𝑞1)2 + (𝑃𝑚𝑦 − 𝐿1𝑠𝑒𝑛𝑞1)
2
 
[Ecuación 5.20] 
 
𝐷2 = (𝑟𝑥𝑦 
′)2 + ℎ2 
[Ecuación 5.21] 
 
𝐷2 = 𝐿3
2 + 𝐿2 − 2𝐿3𝐿𝑐𝑜𝑠(𝑞3 +Ψ) 
[Ecuación 5.22] 
 
 
42 
 
A continuación, se igualan la Ecuación 5.21 con la Ecuación 5.22: 
𝑟𝑥𝑦
′ 2 + ℎ2 = 𝐿3
2 + 𝐿2 − 2𝐿3𝐿𝑐𝑜𝑠(𝑞3 +Ψ) → 
→ 𝑐𝑜𝑠(𝑞3 +Ψ) =
−𝑟𝑥𝑦
′ 2 − ℎ2 + 𝐿3
2 + 𝐿2
2𝐿3𝐿
 
[Ecuación 5.23] 
 
Se calcula el ángulo 𝑞3 utilizando la arcotangente, en lugar del arcocoseno, ya 
que es más eficiente a nivel computacional: 
𝑠𝑒𝑛(𝑞3 +Ψ) = ± √1 − cos
2(𝑞3 +Ψ) 
[Ecuación 5.24] 
 
𝑞3 = 𝑎𝑟𝑐𝑡𝑔 (
± √1 − cos2(𝑞3 +Ψ)
𝑐𝑜𝑠(𝑞3 +Ψ)
) − Ψ 
[Ecuación 5.25] 
 
Como se observa en la Ecuación 5.25, hay dos posibles soluciones para 𝑞3. Esto 
se debe a que la barras 𝐿3 y 𝐿 pueden colocarse de dos formas distintas para 
llegar al punto (articulación con el codo arriba o el codo abajo). Se escoge una 
cualquiera como solución, ya que ambas son válidas. 
Ahora se puede calcular el ángulo 𝑞2 (Figura 5.7): 
 
43 
 
 
 
Figura 5.7. Diagrama de las articulaciones 2 y 3 en disposición de codo abajo. 
 
𝑞2 + 90 = β + α 
[Ecuación 5.26] 
 
β = 𝑎𝑟𝑐𝑡𝑔 (
ℎ
𝑟𝑥𝑦′
) = 𝑎𝑟𝑐𝑡𝑔
(
 
ℎ
±√(𝑟𝑥𝑦′ )
2
)
 
[Ecuación 5.27] 
 
α = arctg ( 
𝐿 𝑠𝑒𝑛(𝑞3 +Ψ)
𝐿3 − 𝐿 cos(𝑞3 +Ψ)
) 
[Ecuación 5.28] 
 
 
44 
 
A continuación, se sustituyen la Ecuación 5.27 y la Ecuación 5.28 en la Ecuación 
5.26: 
𝑞2 = 𝑎𝑟𝑐𝑡𝑔
(
 
ℎ
±√(𝑟𝑥𝑦′ )
2
)
 + arctg ( 
𝐿 𝑠𝑒𝑛(𝑞3 +Ψ)
𝐿3 − 𝐿 cos(𝑞3 +Ψ)
) − 90 
[Ecuación 5.29] 
 
Como se puede comprobar también, existen dos soluciones diferentes para 𝑞2, 
siendo ambas válidas. 
Llegados a este punto, ya se dispone de la posición del robot hasta la muñeca. 
Ahora, mediante el desacoplo cinemático, se van a calcular los siguientes 2GDL, 
𝑞4 y 𝑞5, encargados de la orientación del extremo del robot. Primero se halla la 
relación entre el punto de la muñeca del robot, 𝑃𝑚, y el punto del final del extremo 
del robot, 𝑃𝑓, por medio de la geometría (Figura 5.7): 
 
 Figura 5.8. Diagrama de las articulaciones 3 ,4 y 5. 
 
𝑃𝑚 = O0𝑂4 = [𝑃𝑚𝑥 𝑃𝑚𝑦 𝑃𝑚𝑧]𝑇 
[Ecuación 5.30] 
 
 
45 
 
𝑃𝑓 = O0𝑂5 = [𝑃𝑥 𝑃𝑦 𝑃𝑧]𝑇 
[Ecuación 5.31] 
 
𝑃𝑚 = 𝑃𝑓 − 𝐿6 𝑥5 → [
𝑃𝑚𝑥
𝑃𝑚𝑦
𝑃𝑚𝑧
] = [
𝑃𝑥
𝑃𝑦
𝑃𝑧
] − 𝐿6 [
𝑛𝑥
𝑛𝑦
𝑛𝑧
] 
[Ecuación 5.32] 
 
Parael cálculo de 𝑞4 y 𝑞5 se va a utilizar el método de matrices homogéneas. 
Ahora se define la matriz 𝑅5 
0 como la submatriz de rotación de 𝑇: 
𝑅5
0 = [𝑛 𝑜 𝑎] = [
𝑛𝑥
𝑛𝑦
𝑛𝑧
𝑜𝑥
𝑜𝑦
𝑜𝑧
𝑎𝑥
𝑎𝑦
𝑎𝑧
] 
[Ecuación 5.33] 
 
Se puede descomponer de la siguiente forma: 
𝑅5
0 = 𝑅3
0 𝑅5
3 
[Ecuación 5.34] 
 
A partir de aquí se ha usado MATLAB para el cálculo de las siguientes matrices 
debido a su complejidad: 
 
% Se definen las variables simbólicas del sistema. 
syms q1 q2 q3 q4 q5 
syms L1 L2 L3 L4 L5 L6 
syms n_x n_y n_z o_x o_y o_z a_x a_y a_z % Valores de la submatriz de rotación 
de T. 
syms Px Py Pz % Posición del extremo del robot. 
syms xm ym zm % Posición de la muñeca del robot . 
 
PI= sym(pi) 
% Posición de la muñeca. 
xm = Px -L6*n_x 
ym = Py -L6*n_y 
zm = Pz -L6*n_z 
 
% Submatrices de rotación. 
noa=[n_x o_x a_x; 
 n_y o_y a_y; 
 n_z o_z a_z]; 
 
R01=[cos(q1) 0 sin(q1) ; 
 sin(q1) 0 -cos(q1); 
 0 1 0 ]; 
R12=[-sin(q2) -cos(q2) 0; 
 cos(q2) -sin(q2) 0; 
 
46 
 
 0 0 1]; 
R23=[cos(q3) 0 sin(q3) ; 
 sin(q3) 0 -cos(q3); 
 0 1 0 ]; 
R34=[cos(q4) 0 sin(q4) ; 
 sin(q4) 0 -cos(q4); 
 0 1 0 ]; 
R45=[-sin(q5) -cos(q5) 0; 
 cos(q5) -sin(q5) 0; 
 0 0 1]; 
 
%R03 
R03= R01*R12*R23 
%R35. Lado izquierdo de la ecuación. 
R35= R34*R45 
%R05 
R05= R01*R12*R23*R34*R45 
% Lado derecho de la ecuación. 
ec_d= (R03.')*noa 
 
 
 La submatriz 𝑅3
0 se puede obtener, ya que se han calculado previamente los 
ángulos 𝑞1, 𝑞2 y 𝑞3. El resultado obtenido mediante el código anterior es: 
𝑅3
0 = [
−𝐶1𝐶2𝑆3 − 𝐶1𝐶3𝑆2
−𝐶2𝑆1𝑆3 − 𝐶3𝑆1𝑆2
𝐶2𝐶3 − 𝑆2𝑆3
𝑆1
−𝐶1
0
−
𝐶1𝐶2𝐶3 − 𝐶1𝑆2𝑆3
𝐶2𝐶3𝑆1 − 𝑆1𝑆2𝑆3
𝐶2𝑆3 − 𝐶3𝑆2
] 
[Ecuación 5.35] 
 
Para simplificar la nomenclatura: 𝐶𝑖 y 𝑆𝑖 son el cos(𝑞𝑖) 𝑦 𝑒𝑙 𝑠𝑒𝑛(𝑞𝑖) 
respectivamente. 
Para hallar 𝑅5
3 despejamos de la Ecuación 5.34: 
𝑅5
3 = ( 𝑅3)
0 −1 𝑅5
0 
[Ecuación 5.36] 
 
Al ser 𝑅3
0 una matriz ortogonal, su inversa es igual a su transpuesta. Solo se 
van a representar en la matriz los valores necesarios, ya que esta es muy 
extensa: 
𝑅5
3 = ( 𝑅3)
0 𝑇 𝑅5
0 = 
= [
−
−
𝑛𝑋(𝐶1𝐶2𝐶3 − 𝐶1𝑆2𝑆3) − 𝑛𝑦( 𝑆1𝑆2𝑆3 − 𝐶2𝐶3𝑆1) + 𝑛𝑧(𝐶2𝑆3 + 𝐶3𝑆2)
−
−
−
−
𝑎𝑥𝑆1 − 𝑎𝑦𝐶1
−
] 
[Ecuación 5.37] 
 
Para obtener 𝑅5
3 también se puede aplicar la siguiente ecuación: 
 
47 
 
𝑅5
3 = ( 𝑅4)
3 𝑇 𝑅5 →
4 𝑅5
3 = [
−𝐶4𝑆5
−𝑆4𝑆5
𝑪𝟓
−𝐶4𝐶5
−𝐶5𝑆4
−𝑆5
𝑆4
−𝑪𝟒
0
] 
[Ecuación 5.38] 
 
De la Ecuación 5.38 se utilizarán los valores resaltados en rojo para igualarlos a 
los de la matriz 𝑅5
3 de la Ecuación 5.37, quedando como resultado los ángulos 
buscados: 
cos(𝑞4) = −𝑎𝑥𝑆1 + 𝑎𝑦𝐶1 
[Ecuación 5.39] 
 
cos(𝑞5) = 𝑛𝑋(𝐶1𝐶2𝐶3 − 𝐶1𝑆2𝑆3) − 𝑛𝑦( 𝑆1𝑆2𝑆3 − 𝐶2𝐶3𝑆1) + 𝑛𝑧(𝐶2𝑆3 + 𝐶3𝑆2) 
[Ecuación 5.40] 
 
Se dejan los ángulos en función del arcotangente: 
𝑠𝑒𝑛(𝑞4) = ± √1 − cos2(𝑞4) 
[Ecuación 5.41] 
 
𝑞4 = 𝑎𝑟𝑐𝑡𝑔 (
± √1 − cos2(𝑞4)
𝑐𝑜𝑠(𝑞4)
) 
[Ecuación 5.42] 
 
𝑠𝑒𝑛(𝑞5) = ± √1 − cos2(𝑞5) 
[Ecuación 5.43] 
 
𝑞5 = 𝑎𝑟𝑐𝑡𝑔 (
± √1 − cos2(𝑞5)
𝑐𝑜𝑠(𝑞5)
) 
[Ecuación 5.44] 
 
Tanto 𝑞4 como 𝑞5 tienen dos soluciones válidas. Se escoge una de ellas y, con 
esto, ya queda resuelto el problema cinemático inverso. 
 
 
48 
 
6. DISEÑO SOFTWARE 
 
6.1 Programación del STM32 
 
6.1.1 Introducción 
 
Para programar mediante registros el STM32F103C8, primero se debe crear una 
plantilla desde la que partir. Esta plantilla va a ser lo más básica posible. Para 
ello, se va a partir de una en blanco, creada en el STM32CubeIDE, y se van a 
introducir únicamente los archivos indispensables para que el microcontrolador 
funcione correctamente tras su encendido. Estos archivos se muestran en la 
Figura 6.1 y realizan las siguientes funciones: 
1. El fichero startup_stm32f103c8tx.s contenido dentro de la carpeta “Startup” 
es el primero que se ejecuta después de un evento de reset. Una vez ocurre 
el reset, se ejecuta la función Reset_handler, que realiza las siguientes 
operaciones: 
- Copia los segmentos de datos con los valores iniciales de la memoria flash 
a la SRAM. 
- Llama a la función SystemInit(). 
- Llama a la función __libc_init_array(). 
- Llama a la función main(). 
En este fichero también se inicializan los registros SP (Stack Pointer) y PC 
(Program Counter) del Cortex-M1, además de configurarse la tabla de 
vectores de las interrupciones con las direcciones ISR (Interrupt Service 
Routines). 
2. El fichero system_stm32f1xx.h es el que contiene la función System_Init(). 
Esta función se encarga de configurar el sistema de reloj (system clock 
source, PLL Multiplier Factors, AHB/APBx prescalers, etc) con los valores 
por defecto. 
3. Los archivos stm32f1xx.h y el stm32f103xb.h contienen todas las 
definiciones de los registros del stm32 y, en particular, las del 
STM32F103C8. 
4. Los archivos incluidos dentro de la carpeta Drivers/CMSIS/include tienen 
varias definiciones y funciones necesarias para que funcione el Cortex-M1. 
 
49 
 
 
Figura 6.1. Ficheros de la plantilla básica para STM32. 
 
A continuación, se deben abrir las propiedades del proyecto PlantillaBasica y 
añadir las carpetas que contienen archivos .h tal como se muestra en la Figura 
6.2. 
 
Figura 6.2. Propiedades del proyecto PlantillaBasica. 
 
50 
 
Una vez hecho esto, ya se dispone de la plantilla sobre la que se va a trabajar 
en los dos microcontroladores del proyecto. 
Cabe destacar que los ficheros que aparecen en este apartado han sido 
tomados directamente del fabricante y no han sido creados por el proyectista. 
Todas las demás librerías para el STM32 que aparecen en el resto de la 
memoria sí han sido creadas por él. 
 
6.1.2 Microcontrolador conectado al ordenador. 
 
Este STM32 es el encargado de recibir los comandos del ordenador por uno de 
sus puertos serie y transmitirlo hacia el otro STM32 mediante el bus CAN (es el 
μC 1 de la Figura 1.1). 
El programa generado para este microcontrolador parte de la PlantillaBasica 
(Figura 6.1) y se le han creado las siguientes librerías (Figura 6.3): 
1. Archivos delays.h y delays.c: Contienen funciones básicas para hacer 
retrasos en milisengundos y microsegundos. 
2. Archivos lib_canbus.h y lib_canbus.c: Aquí se encuentran todas las 
funciones necesarias para configurar y usar el periférico del bus CAN que 
tiene el stm32f103. Esta librería será analizada más adelande. 
3. Archivos lib_clock_system.h y lib_clock_system.c: En estos se encuentra la 
función donde configurar todos los subsistemas del clock system. 
4. Archivo lib_pin_definitions.h: Incluye algunas definiciones para los 
diferentes pines de entrada y salida. 
5. Archivos lib_ringbuffer.h y lib_ringbuffer.c: En esta librería se ha creado un 
buffer de anillo para optimizar el envío de datos mediante la comunicación 
UART (Universal Asynchronous Receiver-Transmitter). 
6. Archivos lib_uart.h y lib_uart.c: Es la librería que contiene las funciones 
necesarias para configurar el periférico USART (Universal Synchronous 
/Asynchronous Receiver-Transmitter) usado para la comunicación con el 
PC. También se encarga de procesar los comandos recibidos desde el PC 
y mandarlos a través del bus CAN haciendo uso de lib_canbus.h. 
7. Archivos stm32f1xx_it.h y stm32f1xx_it.c: En el archivo stm32f1xx_it.h se 
encuentran todas las declaraciones de las funciones de excepción y de 
interrupción del microcontrolador. En el archivo stm32f1xx_it.c se definen 
todas las funciones de excepción y la de interrupción del SysTick, que es el 
único timer directamente integrado