Caracterización Biomecánica Del Movimiento de Un Brazo Humano Asociado A Un Brazo Robótico de 3GDL

UNIVERSIDAD NACIONAL DE TRUJILLO
Facultad de Ingeniería
Escuela Profesional de Ingeniería Mecatrónica
CARACTERIZACION BIOMECANICA DEL

MOVIMIENTO DE UN BRAZO HUMANO ASOCIADO A
UN BRAZO ROBOTICO DE 3GDL
Proyecto Final
BIOMECANICA
AUTOR(es):
AGUILAR CONTRERAS; Jose
CHAVEZ GERONIMO, Heber
GALVEZ INFANTE, Victor
MIRANDA CABANILLAS; Jherson
MENDEZ POLO; Jack
VIERA ZEGARRA, Diego
DOCENTE:
Ing. ALVA ALCANTARA; Josmell Henry
CICLO:
X CICLO
Trujillo, Perú
2019
CONTENIDO
CAPÍTULO 1: INTRODUCCION ............................................................................................ 3
1. REALIDAD PROBLEMÁTICA .................................................................................... 3
2. ANTECEDENTES .......................................................................................................... 4
3. OBJETIVOS.................................................................................................................... 6
3.1. OBJETIVO GENERAL .............................................................................................. 6
4. MARCO TEORICO ........................................................................................................ 6
4.1. ROBOTICA ................................................................................................................. 6
4.1.1. CINEMATICA DIRECTA .......................................................................................... 7
4.1.2. CINEMATICA INVERSA .......................................................................................... 9
4.2. PROTOCOLO I2C .................................................................................................... 10
4.3. FILTRO DE KALMAN ............................................................................................ 12
4.4. MULTIPLEXOR TCA9548A ................................................................................... 12
4.5. MPU 6050 ................................................................................................................. 13
4.5.1. SENSOR GIROSCOPIO ........................................................................................... 13
4.5.2. SENSOR ACELEROMETRO .................................................................................. 13
4.5.3. ESPECIFICACIONES .............................................................................................. 14
5. DESARROLLO DEL PROYECTO.............................................................................. 14
5.1 MODELADO CAD .................................................................................................... 14
5.2 ADQUISICIÓN DE DATOS ...................................................................................... 16
5.3 FILTRADO DE DATOS ....................................................................................... 17
5.4 OBTENCIÓN DE VALORES DE PARÁMETROS ARTICULARES ................ 19
5.5 SIMULACIÓN EN V-REP......................................................................................... 21
CAPÍTULO 2 ........................................................................................................................... 22
RESULTADOS Y DISCUCIONES ..................................................................................... 22
CAPÍTULO 3 ........................................................................................................................... 26
CONCLUSIONES ................................................................................................................ 26
REFERENCIAS ....................................................................................................................... 27
ANEXOS .................................................................................................................................. 28
NOVIEMBRE DE 2019 1
Lista de figuras
Figura 1: Esquema que relaciona la cinemática directa e inversa (Lee et Al, 1988) ................ 7
Figura 2:Ejemplificacion de la relacion Maestro-Esclavo entre dos equipos (TeslaBEM,
2017)......................................................................................................................................... 11
Figura 3: Estructura del mensaje en Protocolo I2C (TeslaBEM, 2017). ................................ 11
Figura 4:MPU6050 (hetpro-store, 2014). ............................................................................... 14
Figura 5: Diseño Cad del brazo de 3GDL en el software SolidWorks ................................... 15
Figura 6: Diseño Cad de los brazaletes que contienen los sensores en el software
SolidWorks. .............................................................................................................................. 15
Figura 7: Impresión 3D del brazo de 3GL. ............................................................................. 16
Figura 8: Brazo de 3GDL importado al software V-REP. ...................................................... 16
Figura 9: Tarjeta Arduino Mega conectada a un multiplexor y sensores. .............................. 17
Figura 10: Posicionamiento de sensores a lo largo del brazo. ................................................ 17
Figura 11: Movimiento de flexión para el 3er GDL del brazo con los 2DGL del hombro. ... 22
Figura 12: Movimiento de flexión hacia adelante para los 2DGL del hombro. ..................... 23
Figura 13: Movimiento de Abducción y Aducción horizontal para los 2GDL asociados al
hombro. .................................................................................................................................... 23
Figura 14: Movimiento de flexión del 3er GDL del brazo obtenidos en el software V-REP. 24
Figura 15: Movimiento de flexión para el brazo de 3GDL. ................................................... 25
Figura 16: Movimiento de aducción-abducción para el brazo de 3DGL................................ 25
NOVIEMBRE DE 2019 2
CAPÍTULO 1: INTRODUCCION
1. REALIDAD PROBLEMÁTICA
El ser humano ha realizado desde sus inicios en este planeta, diversas herramientas que le
han permitido adaptar el entorno a sus necesidades. Descubrimientos realizados en la
década del 90, permitieron darnos cuenta que ya nuestros antecesores evolutivos las usaban,
quizás hasta medio millón de años antes (Nobel, 1995). Las primeras herramientas no eran
más que cuchillos de roca primariamente tallados, que, sin embargo, le aportaban un
beneficio en cuanto a su capacidad de obtener alimento. Arqueólogos encabezados
por Zeresenay Alemseged de la California Academy of Sciences, también afirman que
tienen pruebas indirectas de uso de herramientas por parte de los homininos africanos,
concretamente del género Australophitecus, especie afarensis, la misma a la que pertenece
la famosa Lucy encontrada en Afar (Etiopia), en 1974, por el equipo de Donald Johanson
(Carbonell, 2010).
Estas primitivas herramientas han avanzado hasta conseguir las modernas naves espaciales
como la estación espacial internacional o el robot no tripulado Curiosity, el cual viaja por
la superficie de Marte desde 2012 en busca de depósitos de agua que nos den algún indicio
de vida extraterrestre (La República, 2019).
Dada esta enorme ambición de transformarlo todo, el propio cuerpo del ser humano no
podía quedar fuera de la investigación, no solo por los avances en la medicina y
biotecnología, sino también por los continuos avances en las prótesis humanas que cada vez
responden con mayor eficiencia a los movimientos del usuario. El material comúnmente
más usado es el polipropileno, que, a pesar de su cómodo precio, es difícil de adquirir. Ante
este problema de acceso a la materia prima la implementación de impresoras 3D promete
ser una solución viable, debido principalmente a que el material utilizado es de fácil acceso
y bajo costo.
Con el uso de impresiones 3D, las prótesis pueden modificarse fácilmente en el software
asociado a la impresora. Con esto se logra personalizar las prótesis a las necesidades de
cada paciente y además una rápida obtención del producto. Por ejemplo, los elementos de
una prótesis de mano demoran solo dos días en imprimirse y un poco más de tres horas en
ensamblarse (Borjas, R. 2015).
NOVIEMBRE DE 2019 3
Hay que tener en cuenta que para realizar una prótesis adecuada esta debe cumplir con ser
capaz de captar las características sensoriales de un miembro humano real como lo son la
fuerza, el tacto, el contacto, el deslizamiento y la posición; las cuales pueden ser captadas
por los sensores como celdas de carga, acelerómetros, rodadores elementos piezoeléctricos
(Vivas, 2019).
Una variante de esta prótesis es aquella que es usada para repetir el movimiento de un ser
humano y replicarlo en un objeto biomecánico que está situado a larga distancia. Esto es la
llamada tele operación, la cual resulta incompatible con una autonomía robótica, ya que el
robot no es quien debe decidir a donde moverse sino el ser humano que lo controla.
Los estudios realizados en esta materia demuestran que una limitación presente en la tele
operación es la capacidad de procesamiento de señales y con ello la coordinación hombre-
robot. Siempre será un factor crítico la interface hombre-máquina, haciendo que el operador
de este sistema debe ser una persona entrenada. El sistema teleoperador consta de una
estación de tele operación, un sistema de comunicación maestro-esclavo que puede ser un
manipulador o un robot equipado con un manipulador situado en un ambiente lejano (Cerón,
2015).
La importancia de la tele operación radica en que existen muchas circunstancias en las
cuales no es seguro emplear personas para realización de algunas tareas debido al riesgo
elevado al que se exponen como pueden ser químicos tóxicos o explosivos. Es por esto que
se pretende aportar un avance en cuanto a la tele operación, haciendo que un brazo robótico
de 3 grados de libertad pueda caracterizar el movimiento del brazo de un ser humano.
2. ANTECEDENTES
A nivel local, existen estudios relacionado al ámbito del presente proyecto, sobretodo, en
la Pontificia Universidad Católica del Perú, donde existen todo tipo de proyectos de
investigación sobre estudio y elaboración de prótesis.
Siendo así el estudio realizado por Salas (2014), cuyo proyecto denominado “Diseño de una
Prótesis Mioeléctrica para desarticulación de muñeca”; el cual, como su propio nombre
dice, se pretende realizar el diseño de una prótesis de una mano para la desarticulación de
una muñeca con el fin de sujetar objetos mayores a 3 cm3, además de regular la fuerza que
se requiere para manipular dichos objetos.
NOVIEMBRE DE 2019 4
Es así como se llegó a realizar el proyecto de forma efectiva, y que se puede ver plasmado
en simulaciones realizadas en el software SolidWorks. Sin embargo, existen ciertas
limitaciones a la hora de a la hora de sujetar objetos pequeños debido a que el área de
contacto de las falanges resulta relativamente grande a comparación de ciertos objetos.
Además, cabe mencionar que dicha prótesis es capaz de soportar objetos con peso mayor a
2 kg.
Además, la investigación realizada por Sullcahuaman (2013) denominada “Diseño
mecánico de un prototipo de prótesis mioeléctrica transradial”, cuyo fin es el de elaborar un
prototipo de prótesis mioelectrica orientada a pacientes que sufrieron amputaciones por
debajo del codo. El proyecto consiste en diseñar e implementar un mecanismo de un grado
de libertad el cual simule el movimiento de los dedos índice y pulgar de la mano humana
con la finalidad de realizar la sujeción de objetos de 0.5 kg de masa.
Dicha investigación se llevó a realizar con total conformidad el principal objetivo del
proyecto, el cual es el de diseñar y elaborar un prototipo de prótesis para personas que
sufrieron amputaciones por debajo del codo y además de tener la capacidad de poder
sostener objetos con un peso mayor a 500 gramos. Algo que tener en consideración es que
los materiales a partir de los cuales se realizó dicho proyecto fueron rentables en el aspecto
económico. Además, cabe mencionar, que se hizo uso de material rugo en la punta de los
dedos para así aumentar la capacidad de carga del prototipo.
Finalmente, tenemos el trabajo realizado por Bernal (2016), el cual tiene por nombres
“Modelación y simulación dinámica de un mecanismo de 4 GDL para desarrollar una
prótesis para personas con desarticulación humeral”. Este proyecto logra facilitar el análisis
de la biomecánica del movimiento en el miembro superior con el fin de obtener parámetros
dinámicos para iniciar un posterior diseño de la prótesis.
Este modelo logro obtener parámetros de selección de actuadores mediante la simulación
numérica, acorde a los requerimientos de carga de la prótesis. Además, el modelo permite
variaciones en las características físicas del mecanismo como peso, longitud e inercia de
los eslabones y actuadores, los cuales son los parámetros de entrada para la rutina del
cálculo.
NOVIEMBRE DE 2019 5
3. OBJETIVOS
3.1. OBJETIVO GENERAL
Caracterizar el movimiento biomecánico convencional que posee un ser humano y
asociarlo al movimiento de un brazo robótico articular de 3GDL.
3.2. OBJETIVOS ESPECIFICOS
- Diseño e implementación de un brazo robótico articular de 3GDL y de una

armadura para el brazo humano
- Estudio de movimiento del brazo de un ser humano
- Realizar el estudio cinemático del brazo robótico mediante el software V-
REP
- Hacer uso del sensor MPU6050 para el movimiento del brazo robótico.
- Realizar un filtro para la señal del sensor.
- Enlazar el movimiento del brazo humano al brazo robótico.
4. MARCO TEORICO
4.1. ROBOTICA
Para dar un alcance inicial, partimos de lo que es robótica en sí. La palabra robótica es
un derivado de la palabra robot, la cual posee varios significados acordes al investigador
del caso. Es tal el caso de Webster, quien define a un robot como un dispositivo
automático que efectúa funciones ordinariamente asignadas a los seres humanos. Pero,
la definición más usual es la que propone el Robot Institute of América, donde se indica
que es un manipulador reprogramable multifuncional diseñado para mover materiales,
piezas o dispositivos especializados a través de movimientos programados variables
para la realización de una diversidad de tareas. (Craig, 2006)
Teniendo una idea ya clara de lo que es robótica, es que se puede dar paso al estudio de
esta. Para nuestro caso de estudio, se analizará solo el aspecto cinemático del sistema,
el cual comprende a la cinemática directa y la cinemática inversa, y cuya explicación
viene a continuación.
NOVIEMBRE DE 2019 6
4.1.1. CINEMATICA DIRECTA

Hace uso principalmente del algebra vectorial y matricial para elaborar un método
general y sistemático, el cual nos permita obtener la ubicación de los elementos de
un brazo robótico respecto a un punto de referencia fijo. Por lo tanto, esto permite
que el problema de la cinemática directa se reduzca a determinar una matriz de
transformación homogénea (T), tal que relacione tanto la orientación como la
ubicación del efector final respecto a la base del robot. (Barrientos, 2007)
La función de la matriz de transformación homogénea (2-1), la cual es de 4x4, es la
de reorientar una vector de posición cualquiera desde un sistema de referencia fijo
(OXYZ) hacia otro sistema (OUVW) (2-2). Esta matriz de transformación nace a
partir de la conformación de una matriz general que englobe a una matriz de rotación
de 3x3 y un vector de posición de 3x1, además de su complemento, donde podemos
apreciar a continuación. (Lee, Gonzales & Fu, 1998). (Lee et Al, 1988)
Figura 1: Esquema que relaciona la cinemática directa e inversa (Lee et Al, 1988)
𝑀𝑎𝑡𝑟𝑖𝑧 𝑑𝑒 𝑅𝑜𝑡𝑎𝑐𝑖𝑜𝑛 (3𝑥3) 𝑉𝑒𝑐𝑡𝑜𝑟 𝑑𝑒 𝑃𝑜𝑠𝑖𝑐𝑖𝑜𝑛 (3𝑥1)
𝑇=[ ] (4-1)
𝑇𝑟𝑎𝑛𝑠𝑓𝑜𝑟𝑚𝑎𝑐𝑖𝑜𝑛 𝑑𝑒 𝑃𝑒𝑟𝑠𝑝𝑒𝑐𝑡𝑖𝑣𝑎 𝐸𝑠𝑐𝑎𝑙𝑎𝑑𝑜
𝑃𝑈𝑉𝑊 = 𝑇 ∗ 𝑃𝑋𝑌𝑍 (4-2)
Para el caso de robots manipuladores de “n” grados de libertad, las matrices de

transformación que enlacen la posición y orientación de un eslabón con su consecuente
se representan así: 𝑖−1 Ai. Por lo tanto, para expresar una matriz de transformación
NOVIEMBRE DE 2019 7
homogénea que englobe todos los grados de libertad del robot viene dado de la siguiente
forma (Barrientos, 2007):
T= 0An = 0A1* 1A2* 2A3*, …,n−1An (4-3)
 Representación Denavit-Hartenberg
Para explicar la relación de traslación y orientación entre elementos
consecuentes, Denavit y Hartenberg, allá por los 1955, elaboraron un método
matricial el cual es capaz de establecer un sistema de coordenadas para cada
eslabón que compone al manipulador (Lee et Al, 1988).
Acorde a la representación de Denavit-Hartenberg, si se realiza una correcta
selección de los ejes coordenados correspondientes a cada elemento, es factible
pasar al siguiente a través de 4 transformaciones sencillas, las cuales dependen
de las características geométricas del elemento. Dichas transformaciones
consisten en una serie de rotaciones y traslaciones, las cuales permiten relacionar
el sistema coordenado i-1 con el sistema i (Barrientos, 2007). Dichas
transformaciones se detallan a continuación:
1. Rotación alrededor del eje Zi-1 un ángulo θi
2. Traslación a lo largo de Zi-1 una distancia di
3. Traslación a lo largo del eje Xi una distancia ai
4. Rotación alrededor del eje Xi un ángulo αi
Estas transformaciones mantienen una dependencia directa de 4 parámetros, los
cuales tienen relación a cada eslabón del manipulador, y que, por medio de estos,
se pueden describir las articulaciones prismáticas o de revoluta. Dichos
parámetros se definen de la siguiente forma:
- θi: Es el ángulo de la articulación del eje Xi-1 al eje Xi respecto del eje Zi-1. Se
trata de un parámetro variable en articulaciones giratorias.
- di: Es la distancia desde el origen del sistema de coordenadas (i-1)-ésimo hasta
la intersección del eje Zi-1 con el eje Xi a lo largo del eje Zi-1. Se trata de un
parámetro variable en articulaciones prismáticas.
- ai: Es la distancia de separación desde la intersección del eje Zi-1 con el eje Xi
hasta el origen del sistema i-ésimo, en el caso de articulaciones giratorias. En el
NOVIEMBRE DE 2019 8
caso de que sean prismáticas, se calcula como la distancia más corta entre Zi-1 y
Zi.
- αi: Es el ángulo de separación del eje Zi-1 al eje Zi medido en un plano
perpendicular al eje Xi. (Lee, Gonzales & Fu, 1998)
4.1.2. CINEMATICA INVERSA

La cinemática inversa tiene como objetivo hallar los valores que adoptan los ángulos
de las articulaciones (q1(t), q2(t), …, qn(t)) con respecto al posicionamiento y
orientación que tiene el efector final (Figura 4). A diferencia de lo que sucede con
la cinemática directa, la cinemática inversa resulta más dificultosa a la hora de
encontrar una solución, ya que, a medida que mayor sea el número de grados de
libertad, más compleja se hace su solución. (Barrientos, 2007)
A lo largo de los años, se han ido desarrollando técnicas las cuales permitan obtener
una solución a la problemática que genera la cinemática inversa, como lo es la
técnica de transformación inversa, el álgebra de tornillo, matrices duales,
cuaterniones duales, métodos geométricos, etc. (Lee et Al, 1988)
Sin embargo, a continuación, se detallará acerca de las técnicas más usuales para la
solución de esta problemática:
 Métodos Geométricos
Este tipo de solución es adecuado cuando se trata de robots que poseen pocos
grados de libertad (máximo 3). La solución por este método se basa en determinar
el suficiente número de relaciones geométricas entre el efector final, los valores
articulares y las dimensiones de los eslabones. (Barrientos, 2007)
 Matriz de transformación homogénea

Este método se basa en que, así como se puede obtener el modelo cinemático
directo de un robot, es posible obtener su modelo cinemático inverso. Es así que,
si se asumen conocidas los valores de posición y orientación del efector final
respecto a sus valores articulares, es posible realizar una manipulación, de tal
forma, que se obtengan de estas sus relaciones inversas. (Barrientos, 2007)
NOVIEMBRE DE 2019 9
 Desacoplo cinemático
Las técnicas mencionadas anteriormente, suelen ser útiles cuando se trata
solamente de obtener los valores de las 3 primeras variables articulares, las
cuales son las que posicionan al efector final; aunque también pueden ser
aplicados a robots con más grados de libertad, pero es ahí donde la complejidad
aumenta. Sin embargo, no basta con solo posicionar el efector final, ya que, en
la mayoría de los casos, es necesario orientarlo de una forma específica.
Por ello es que la mayoría de los robots existentes poseen 3GDL, los cuales
continúan con la cadena de eslabones y que reciben el nombre de “muñeca del
robot”, cuyo objetivo primordial es la de reorientar la herramienta que posee el
efector final libremente en el espacio.
Es aquí donde la técnica del desacoplo cinemático toma importancia, ya que su
base está en separar el problema del posicionamiento con el de orientación. El
desarrollo de esto se basa en que, dada una posición y orientación del efector
final deseados, desliga inicialmente a la muñeca del robot de los tres primeros
elementos que componen al robot, y, de los cuales, obtendremos nuestras tres
primeras variables articulares. Una vez conseguido esto, y con los datos de
orientación del robot, es que se inicia el análisis en la muñeca del robot para
obtener el resto de variables articulares. (Barrientos, 2007)
4.2. PROTOCOLO I2C
I2C significa Circuito Interintegrado y es un protocolo de comunicación serial

desarrollado por Phillips Semiconductors allá por la década de los 80s. Básicamente se
creó para poder comunicar varios chips al mismo tiempo dentro de los televisores.
El protocolo I2C toma e integra lo mejor de los protocolos SPI y UART. Con el
protocolo I2C podemos tener a varios maestros controlando uno o múltiples esclavos.
Esto puede ser de gran ayuda cuando se van a utilizar varios microcontroladores para
almacenar un registro de datos hacia una sola memoria o cuando se va a mostrar
información en una sola pantalla.
El protocolo I2C utiliza sólo dos vías o cables de comunicación, así como también lo
hace el protocolo UART. (TeslaBEM, 2017)
NOVIEMBRE DE 2019 10
Figura 2:Ejemplificacion de la relacion Maestro-Esclavo entre dos equipos

(TeslaBEM, 2017)
Con el protocolo I2C la información viaja en mensajes. Los mensajes van divididos
en tramas de datos. Cada mensaje lleva una trama con una dirección la cuál transporta
la dirección binaria del esclavo al que va dirigido el mensaje, y una o más tramas que
llevan la información del mensaje. También el mensaje contiene condiciones de inicio
y paro, lectura y escritura de bits, y los bits ACK y NACK. Todo esto va entre cada
sección de datos. (TeslaBEM, 2017)
Figura 3: Estructura del mensaje en Protocolo I2C (TeslaBEM, 2017).

Condición de Inicio – Start: La vía SDA cambia de un nivel de voltaje Alto a un nivel de
voltaje Bajo, antes de que el canal SCL cambie de Alto a nivel Bajo.
Condición de Paro – Stop: La vía SDA ahora cambia de un nivel de voltaje Bajo a Alto,
después de que la vía SCL cambia de Bajo a Alto.
Trama de Dirección – Address Frame: Es una secuencia única que va de los 7 a los 10
bits. Esta sección (Frame) se envía a cada Esclavo, y va a identificar al Esclavo con el
que el Maestro se quiere comunicar.
Bit para Lectura/Escritura A – Read/Write Bit A: Es un bit de información enviado a
los Esclavos. Por medio de este bit el Maestro indica si le va enviar información al
Esclavo (Nivel Bajo de voltaje = Escritura), o si el Maestro quiere

solicitarle información al Esclavo (Nivel Alto de Voltaje = Lectura). (TeslaBEM, 2017)
Bit ACK/NACK – : Después de cada sección (Frame) de información enviada en un
mensaje, podemos notar que lleva un bit acknowledge/no-acknowledge (reconocido/no-
reconocido). Esto ayuda a identificar si la información fue enviada correctamente. En
seguida de que se envía un Frame, si este fue recibido con éxito, se retorna un bit ACK
al remitente. Si la información no fue recibida con éxito, se retorna un bit NACK.
(TeslaBEM, 2017)
4.3.FILTRO DE KALMAN
El filtro Kalman es un filtro predictivo recursivo que se basa en el uso de técnicas de
espacio de estado y algoritmos recursivos. Estima los estados de un sistema dinámico
que pueden estar perturbado por ruido blanco, dependiendo del retraso de las muestras
que se le ingresan puede cumplir la función de estimador de parámetros o únicamente
de filtro. El filtro de Kalman consiste en dos pasos fundamentales; el primer paso
consiste en predecir el estado con el modelo dinámico y el segundo paso en corregir con
el modelo de observación, de manera que se minimiza la covarianza de errores del
estimador. Para obtener una ecuación que calcule la estimación a posteriori a partir de
la estimación a priori y el error en la predicción de la observación.
4.4.MULTIPLEXOR TCA9548A
El multiplexor I2C TCA9548A permite manejar hasta 8 dispositivos I2C haciendo uso
de un solo puerto I2C en nuestro microcontrolador. Si necesitamos conectar dos o más
dispositivos I2C que tienen la misma dirección (I2C Address) a un microcontrolador,
pero este solo posee 1 puerto I2C por hardware, esto no podría realizarse debido al
conflicto de direcciones I2C en un mismo bus I2C. Es en este caso donde el multiplexor
I2C TCA9548A nos permite trabajar con hasta 8 dispositivos con la misma dirección
I2C, ya que el microcontrolador solo podrá "ver" un dispositivo I2C a la vez, sin crear
conflicto. El chip TCA9548A además permite modificar su dirección I2C entre 0x70 a
077 por lo que podríamos conectar hasta 8 multiplexores, que a su vez permitirían
manejar hasta 64 dispositivos I2C.
4.5.MPU 6050
El MPU6050 es un sensor y acelerómetro para Arduino con 6 grados de libertad. El
propósito de este tutorial consiste de dos partes, la primera será mostrar lo que es un
acelerómetro y giroscopio y la lectura de valores del MPU-6050 utilizando un Arduino
Uno. También se mostrarán las mediciones en el monitor serial. La segunda parte del
tutorial será de la implementación de una Unidad de Medición Inercial – IMU por sus
siglas en ingles. (Inertial Measurement Unit). (hetpro-store, 2014)
4.5.1. SENSOR GIROSCOPIO
Un giroscopio es un dispositivo que funciona para miden velocidades angulares
basándose en el mantenimiento del impulso de rotación. Si intentamos hacer girar
un objeto que está girando sobre un eje que no es el eje sobre el que está rotando, el
objeto ejercerá un momento contrario al movimiento con el fin de preservar el
impulso de rotación total. (hetpro-store, 2014).
El giroscopio muestra el cambio de rango en rotación en sus ejes X, Y y Z.
4.5.2. SENSOR ACELEROMETRO

Mide la aceleración, inclinación o vibración y transforma la magnitud física de
aceleración en otra magnitud eléctrica que será la que emplearemos en los equipos
de adquisición estándar. Los rangos de medida van desde las décimas de g, hasta los
miles de g. (hetpro-store, 2014).
El circuito integrado MPU-6050 contiene un acelerómetro y giroscopio MEMS en
un solo empaque. Cuenta con una resolución de 16-bits, lo cual significa que divide
el rango dinámico en 65536 fracciones, estos aplican para cada eje X, Y y Z al igual
que en la velocidad angular. El sensor es ideal para diseñar control de robótica,
medición de vibración, sistemas de medición inercial (IMU), detector de caídas,
sensor de distancia y velocidad, y muchas cosas más. El MPU-6050 contiene un
giroscópico, un acelerómetro, además de un sensor de temperatura, mediante
I2C regresa unos valores conocidos como raw o “crudos” según el registro
seleccionado. (hetpro-store, 2014)
4.5.3. ESPECIFICACIONES
 Salida digital de 6 ejes.

 Giroscopio con sensibilidad de ±250, ±500, ±1000, y ±2000dps
 Acelerómetro con sensibilidad de ±2g, ±4g, ±8g y ±16g
 Algoritmos embebidos para calibración
 Sensor de temperatura digital
 Entrada digital de video FSYNC
 Interrupciones programables
 Voltaje de alimentación: 2.37 a 3.46V
 Voltaje lógico: 1.8V±5% o VDD
 10000g tolerancia de aceleración máxima
Figura 4:MPU6050 (hetpro-store, 2014).
5. DESARROLLO DEL PROYECTO

5.1 MODELADO CAD
Para el desarrollo del proyecto se realizó un diseño de un brazo robótico de 3 grados de
libertad en el software SolidWorks Figura 5, dicho brazo guarda relación con las
longitudes reales del brazo y antebrazo de un ser humano, las longitudes del brazo y
antebrazo tomadas fueron de 0.158 m y 0.1325 m respectivamente. Los planos del brazo
diseñado son mostrados con detalle en el Anexo 1.
Figura 5: Diseño Cad del brazo de 3GDL en el software SolidWorks
Para poder calcular los ángulos de rotación del brazo se hizo uso de 3 sensores
MPU6050, estos fueron colocados a lo largo del brazo de la persona a la cual se desea
imitar el movimiento, con este fin se diseñó unos brazaletes ajustables Figura 6 que
contienen en su interior los sensores, haciendo así que los movimientos puedan ser
recolectados de cualquier persona con estos brazaletes ajustables. Los planos del
brazalete son mostrados en el Anexo2.
Figura 6: Diseño Cad de los brazaletes que contienen los sensores en el software
SolidWorks.
Los diseños modelados fueron confeccionados en impresión 3D Figura 7 para luego
ser ensamblados.
Figura 7: Impresión 3D del brazo de 3GL.

El modelo del brazo de 3GDL diseñado en SolidWorks se exportó como un archivo
URDF seleccionando los eslabones y luego importando este archivo en V-REP Figura8.
Figura 8: Brazo de 3GDL importado al software V-REP.
5.2 ADQUISICIÓN DE DATOS

Se usó una tarjeta Arduino con un multiplexor para recolectar y procesar los datos
obtenidos de los sensores Figura 9, la comunicación usada fue la I2C, para ello se utilizó
las librerías I2Cdev.h, MPU6050.h y Wire.h. El uso del multiplexor fue necesario ya
que nuestra tarjeta Arduino solo tiene un puerto en hardware para la comunicación I2C
y nuestros sensores tienen máximo hasta 2 direcciones 0x78 y 0x79, lo que significa
que el controlador podría comunicarse máximo con hasta 2 sensores sin tener conflicto,
utilizando un sensor la dirección 0x78 y el otro sensor la 0x79, sin embargo, se necesita
la comunicación con 3 sensores por lo que se utiliza el multiplexor.
Figura 9: Tarjeta Arduino Mega conectada a un multiplexor y sensores.

Los brazaletes que llevan consigo los sensores fueron dispuestos a lo largo del brazo al
cual se le realizaría el estudio de movimiento Figura 10.
Figura 10: Posicionamiento de sensores a lo largo del brazo.
5.3 FILTRADO DE DATOS

Debido a que las señales obtenidas presentan ruido, el cual provoca vibraciones en el
prototipo del brazo de 3GDL y también las simulaciones, se recurre a utilizar un filtro
para estabilizar la señal. El filtro seleccionado fue el filtro de Kalman debido a que
este no necesita una frecuencia de corte como los filtros tradicionales, se basa en
estimaciones de estados pasados y ecuaciones recursivas, que fueron implementadas
en Arduino para su utilización en tiempo real, estas fueron:
Predicción
- Predicción del estado.
𝑥𝑝 (𝑘) = 𝐴 − 𝑥𝑝 (𝑘 − 1) + 𝐵𝑢(𝑘) (5_1)
- Predicción covarianza del error.
𝑃𝑝 (𝑘) = 𝐴𝑃(𝑘 − 1)𝐴𝑇 + 𝑄 (5_2)
Donde:
𝑥𝑝 : Estado predicho.
𝑃𝑝 : Covarianza del error predicha.
𝐴: Matriz de coeficientes de términos que depende de las variables de estado.
B: Matriz de coeficientes de términos que no dependen de las variables de estado.
𝑢: Vector de variables de control.
Q: Matriz de ruido de la covarianza del proceso.
Corrección
- Calculo de la ganancia de Kalman
𝐾(𝑘) = 𝑃(𝑘)𝐻 𝑇 (𝐻𝑃(𝑘)𝐻 𝑇 + 𝑅)−1 (5_3)
- Actualización de la estimación con las medidas 𝑧(𝑘).
𝑥𝑐 (𝑘) = 𝑥𝑝 (𝑘) + 𝐾(𝑘)(𝑧(𝑘) − 𝐻𝑥𝑝 (𝑘)) (5_4)
- Actualización de la covarianza del error.
𝑃𝑐 (𝑘) = (𝐼 − 𝐾(𝑘)𝐻)𝑃𝑝 (𝑘) (5_5)
Donde:
𝑥𝑐 : Estado corregido.
𝑃𝑐 : Covarianza del error corregido.
𝑧: Muestra tomada en el instante k.
K: Ganancia de Kalman.
𝑅: Matriz de covarianza de la medida.
𝐻: Matriz que adapta la estructura de la matriz de covarianza del proceso para que
coincida con la matriz de covarianza de la medida.
En el caso de la primera iteración, al no existir una salida del filtro, se utilizan valores
iniciales previamente establecidos.
- 𝑋(0): Vector inicial de estado.

- 𝑃(0): Matriz inicial de la covarianza del proceso.
Estas estimaciones iniciales no son demasiadas críticas para el funcionamiento del filtro,
ya que la estimación va a converger. Las matrices Q y R si son críticas para el
funcionamiento de filtro.
Los valores que mejor repuesta tuvieron son mostrados en el Anexo 3 junto al código.
5.4 OBTENCIÓN DE VALORES DE PARÁMETROS ARTICULARES
Primero se realizó la calibración de los sensores MPU6050 esto es necesario ya que el
sensor MPU6050 probablemente no se encuentre 100% en una posición horizontal, esto
debido a que el sensor al ser soldado en el módulo puede estar desnivelado agregando
un error en cada componente. Se realizó la calibración con el programa en el anexo 4.
El programa básicamente está modificando constantemente los offset intentando
eliminar el error con la medida real que deseamos, en este caso ax=0, ay=0, az=1g y
gx=0, gy=0, gz=0.
Inicialmente leemos los offsets actuales y esperamos que el usuario envía un carácter
por el puerto serie. Antes de enviar el carácter es necesario ubicar el sensor en posición
horizontal y evitar moverlo durante la calibración, dicha posición será nuestro nivel para
futuras mediciones. Después de enviar el carácter el programa realiza las lecturas tanto
del acelerómetro como del giroscopio, se usó un filtro para estabilizar un poco las
lecturas y cada 100 lecturas se comprueba si los valores son cercanos a los valores que
deseamos leer, dependiendo de esto se aumenta o disminuye los offsets. Esto hará que
las lecturas filtradas converjan a:
Aceleración: p_ax=0 , p_ay=0 , p_az=+16384
Velocidad angular: p_gx=0 , p_gy=0 , p_gz=0
Cuando en el monitor serial se observó valores cercanos a los anteriores desconectamos
nuestro Arduino por lo que el MPU6050 quedó configurado con el último offset
calculado en el programa de calibración.
- Una vez calibrado los sensores se utilizó el algoritmo mostrado en el Anexo 3 para
obtener las aceleraciones en los ejes y relacionar con el ángulo de inclinación del sensor
en ese instante.
Al inicializar el sensor, los rangos por defecto serán:

- acelerómetro -2g a +2g
- giroscopio: -250°/sec a +250°/sec
Teniendo en cuenta que la resolución de las lecturas es de 16 bits por lo que el rango de
lectura es de -32768 a 32767.
En el void loop() realizamos las lecturas y las guardamos en sus variables respectivas
para cada uno de los sensores, esto se hace con la siguiente funcion:
sensor. getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
Esta lee la aceleración de los componentes x-y-z como parámetro, es necesario dar la
dirección de las variables como argumento, para lo que se usa: &variable. También
realiza lectura de la velocidad angular y guarda las lecturas en sus respectivas variables.
Para obtener las lecturas de cada sensor, se utiliza la función “tcaselect()” la que indica
en cual puerto del multiplexor se ha conectado, una vez seleccionado el puerto se puede
hacer uso de la comunicación entre el sensor y el controlador.
Para relacionar las aceleraciones medidas por el sensor en el Angulo de inclinación se
tiene en cuenta que la única fuerza que actúa sobre el sensor es la fuerza de la gravedad.
Entonces los valores que obtenemos en las componentes del acelerómetro corresponden
a la gravedad y los ángulos de la resultante serán la inclinación del plano del sensor,
puesto que la gravedad siempre es vertical. Entonces los ángulos de inclinación en un
espacio 3D tanto en X como en Y usamos las siguientes formulas:
𝑎𝑥
𝜃𝑥 = tan−1 ( )
√𝑎𝑦 + 𝑎𝑧 2
2
𝑎𝑦
𝜃𝑦 = tan−1 ( )
√𝑎𝑥 2 + 𝑎𝑧 2
Al realizar las pruebas se determinó que el sensor presentaba una mejor respuesta con
el ángulo de inclinación en el eje Y por lo que se utilizó este.
Luego de obtener el ángulo de inclinación en radianes se utilizó 3 filtros de Kalman para
los 3 angulos de inclinación hallados, uno por cada sensor, posteriormente se escaló
para el rango de trabajo de cada servomotor, ya que se estableció ángulos finales e
iniciales en los servomotores, estos son mostrados en el anexo 3 junto al algoritmo

desarrollado.
Los ángulos obtenidos en radianes fueron mandados por comunicación serial mediante
la función “Serial.print()”, estos fueron recibido por Matlab, abriendo el puerto serial y
mediante la función “fscanf()”, la velocidad a la cual se trabajo fue 115200 baudios.
Los ángulos escalados se enviaron a los servomotores mediante la función
myservo.write().
5.5 SIMULACIÓN EN V-REP

Se realizó una comunicación entre el software Matlab y V-REP mediante una API
remota, para ello se hizo uso de una configuración inicial en el script de Matlab
designando la dirección de conexión y el puerto de conexión, luego se definió los objetos
que representan los parámetros articulares del robot, luego se hizo una comunicación
serial con Arduino mediante la cual se obtuvo los valores articulares dados por los
sensores para luego almacenarlos en un vector.
Se realizó un escalamiento para cada uno de los parámetros angulares para luego limitar
el espacio de trabaja en un solo cuadrante. Los parámetros articulares obtenidos fueron
enviados a V-REP mediante la función vrep.simxSetJointTargetPosition de modo que
el brazo de 3DGL en el simulador siga las posiciones articulares indicadas
reconfigurándose de manera constante. El código en detalle es mostrado en el Anexo 4.
CAPÍTULO 2
RESULTADOS Y DISCUCIONES
Los movimientos analizados fueron los de flexión, extensión, abducción y aducción.
Para la flexión del 3er grado de libertad del brazo se obtuvieron las señales mostradas en la
Figura 11, se muestran las señales originales captadas y además las que se obtiene luego de
aplicar el filtro de Kalman. Las señales obtenidas mediante los sensores MPU6050 estan en
radianes.
Figura 11: Movimiento de flexión para el 3er GDL del brazo con los 2DGL del hombro.
En la Figura 12 se muestra el movimiento de flexión hacia adelante que realiza el brazo en el
cual están involucrados principalmente los 2GDL asociados al hombro. Se aprecia la señal
original recogida por los sensores y además las señales ya luego de aplicarles el filtro de
Kalman.
Figura 12: Movimiento de flexión hacia adelante para los 2DGL del hombro.
En la Figura 13 se muestran los movimientos de abducción y aducción horizontal del brazo, en
este movimiento están involucrados principalmente los 2GDL del hombro, por lo cual en la
imagen se muestran las señales obtenidas de los sensores originalmente y las que se obtienen
luego de aplicar el filtro de Kalman.
Figura 13: Movimiento de Abducción y Aducción horizontal para los 2GDL asociados al
hombro.
En las gráficas mostradas anteriormente se pueden notar la gran cantidad de ruidos obtenidos
al usar los sensores por lo que es una potente solución el uso del filtro de Kalman.
Adicionalmente de obtener los datos mediante los sensores también se obtuvieron los dados en
el simulador V-REP que son los siguientes.
Por la configuración inicial del brazo en el software V-REP los parámetros articulares se
muestran en negativo.
En la Figura 14 se muestra el movimiento de flexión del 3GDL del brazo en grados
sexagesimales.
Figura 14: Movimiento de flexión del 3er GDL del brazo obtenidos en el software V-REP.
En la Figura 15 se muestran los valores de los parámetros articulares en grados sexagesimales
a lo largo del movimiento de flexión para todo el brazo.
Figura 15: Movimiento de flexión para el brazo de 3GDL.

En la Figura 16 se muestra el valor de los parámetros articulares en el movimiento de abducción
y aducción para el brazo de 3GDL obtenidos en el software V-REP.
Figura 16: Movimiento de aducción-abducción para el brazo de 3DGL.
CAPÍTULO 3
CONCLUSIONES
El diseño del modelo Cad del brazo de 3GDL sigue las longitudes de las extremidades de un
ser humano en una escala de 1:2.
El uso de los sensores MPU6050 para determinar los parámetros articulares del brazo genera
una señal con presencia de ruido por ello se hace necesario la utilización de un filtro de Kalman
para cada señal, obteniendo una señal más estable y evitando así las vibraciones en la estructura
del brazo robótico.
Los movimientos generados son de flexión, abducción y aducción, para estos se obtuvo las
señales asociados a los parámetros articulares en radianes, tanto las señales originales, así como
las obtenidas después de aplicar el filtro de Kalman, logrando el seguimiento del brazo humano.
Los valores obtenidos con los sensores después de aplicarles el filtro son pasados al software
V-REP en el cual se simula el movimiento del brazo en tiempo real, dichos parámetros
articulares resultaron negativos por la propia configuración inicial del robot al momento de
exportarlos a URDF.
Los parámetros que mejor respondieron para el filtro de Kalman son R=10 y Q=0.1.
La simulación en Vrep, se puede observar un pequeño retraso comparado con el movimiento
real de brazo humano y el movimiento de prototipo impreso en 3D, esto es debido al tiempo
que se toma para la comunicación serial y al procesamiento de obtener los datos en Matlab
escalarlos y luego enviarlos a Vrep.
Debido a las posiciones de los sensores y que se está utilizando un solo ángulo de inclinación
de cada uno, nos restringe movimientos a un solo cuadrante y a movimientos básico como
extensión, aducción y abducción.
REFERENCIAS
Barrientos, A. (2007). Fundamentos de Robotica. Madrid: McGrawHill.
Bernal. (2016). MODELACIÓN Y SIMULACIÓN DINÁMICA DE UN MECANISMO DE 4
GDL PARA DESARROLLAR UNA PRÓTESIS PARA PERSONAS CON
DESARTICULACIÓN HUMERAL. Lima.
Craig, J. (2006). Robotica. Mexico: Pearson.
hetpro-store. (5 de Abril de 2014). hetpro-store. Obtenido de https://hetpro-
store.com/TUTORIALES/
Lee et Al. (1988). Robotica: control, deteccion, vision e inteligencia. España: McGrawHill.
Salas. (2014). Diseño de una Prótesis Mioeléctrica para desarticulación de muñeca. Lima.
Sullcahuaman. (2013). DISEÑO MECÁNICO DE UN PROTOTIPO DE PRÓTESIS
MIOELÉCTRICA TRANSRADIAL . Lima.
TeslaBEM. (4 de Abril de 2017). TeslaBEM. Obtenido de http://www.teslabem.com
Borjas, R. (2015). Fabricación de una Prótesis Humana utilizando una impresora 3D

en Honduras. Trigésima Quinta Convención de Centroamérica y Panamá. Convención
llevada a cabo en Honduras.
Carbonell, E. (21 de agosto de 2010). Las primeras herramientas, hace 3,3 millones de
años. El Mundo.
Cerón, A. (2015). Sistemas robóticos teleoperados. Ciencia E Ingeniería
Neogranadina, 15(1), 62-69.
La República. (04 de diciembre 2019). NASA capta amanecer en Marte gracias al
Curiosity.
Nobel, J. (10 de Julio de 1995). PRIMERAS HERRAMIENTAS HUMANAS. El
Tiempo.
Vivas, O. (2019). DISEÑO Y CONSTRUCCION DE UNA PROTESIS ROBOTICA DE
MANO FUNCIONAL ADAPTADA A VARIOS AGARRES (Tesis de Maestría). Universidad
del Cauca.
ANEXOS
Anexo 1: Planos de Brazo de 3GDL
Anexo 2: Planos de brazaletes
Anexo 3: Código I de Arduino.
/* UNIVERSIDAD NACIONAL DE TRUJILLO
* CURSO BIOMECANICA
* CALIBRACIÓN DE SENSORES MPU6050
*/
// Librerias I2C para controlar el mpu6050
// la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
// La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo

// del estado de AD0. Si no se especifica, 0x68 estará implicito
MPU6050 sensor;
// Valores RAW (sin procesar) del acelerometro y giroscopio en los ejes x,y,z
int ax, ay, az;
int gx, gy, gz;
//Variables usadas por el filtro pasa bajos

long f_ax,f_ay, f_az;
int p_ax, p_ay, p_az;
long f_gx,f_gy, f_gz;
int p_gx, p_gy, p_gz;
int counter=0;
//Valor de los offsets

int ax_o,ay_o,az_o;
int gx_o,gy_o,gz_o;
void setup() {
Serial.begin(57600); //Iniciando puerto serial
Wire.begin(); //Iniciando I2C
sensor.initialize(); //Iniciando el sensor
if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente");
// Leer los offset los offsets anteriores

ax_o=sensor.getXAccelOffset();
ay_o=sensor.getYAccelOffset();
az_o=sensor.getZAccelOffset();
gx_o=sensor.getXGyroOffset();
gy_o=sensor.getYGyroOffset();
gz_o=sensor.getZGyroOffset();
Serial.println("Offsets:");
Serial.print(ax_o); Serial.print("\t");
Serial.print(ay_o); Serial.print("\t");
Serial.print(az_o); Serial.print("\t");
Serial.print(gx_o); Serial.print("\t");
Serial.print(gy_o); Serial.print("\t");
Serial.print(gz_o); Serial.print("\t");
Serial.println("nnEnvie cualquier caracter para empezar la calibracionnn");
// Espera un caracter para empezar a calibrar
while (true){if (Serial.available()) break;}
Serial.println("Calibrando, no mover IMU");
void loop() {
// Leer las aceleraciones y velocidades angulares
sensor.getAcceleration(&ax, &ay, &az);
sensor.getRotation(&gx, &gy, &gz);
// Filtrar las lecturas

f_ax = f_ax-(f_ax>>5)+ax;
p_ax = f_ax>>5;
f_ay = f_ay-(f_ay>>5)+ay;
p_ay = f_ay>>5;
f_az = f_az-(f_az>>5)+az;
p_az = f_az>>5;
f_gx = f_gx-(f_gx>>3)+gx;
p_gx = f_gx>>3;
f_gy = f_gy-(f_gy>>3)+gy;
p_gy = f_gy>>3;
f_gz = f_gz-(f_gz>>3)+gz;
p_gz = f_gz>>3;
//Cada 100 lecturas corregir el offset

if (counter==100){
//Mostrar las lecturas separadas por un [tab]
Serial.print("promedio:"); Serial.print("t");
Serial.print(p_ax); Serial.print("\t");
Serial.print(p_ay); Serial.print("\t");
Serial.print(p_az); Serial.print("\t");
Serial.print(p_gx); Serial.print("\t");
Serial.print(p_gy); Serial.print("\t");
Serial.println(p_gz);
//Calibrar el acelerometro a 1g en el eje z (ajustar el offset)

if (p_ax>0) ax_o--;
else {ax_o++;}
if (p_ay>0) ay_o--;
else {ay_o++;}
if (p_az-16384>0) az_o--;
else {az_o++;}
sensor.setXAccelOffset(ax_o);
sensor.setYAccelOffset(ay_o);
sensor.setZAccelOffset(az_o);
//Calibrar el giroscopio a 0º/s en todos los ejes (ajustar el offset)

if (p_gx>0) gx_o--;
else {gx_o++;}
if (p_gy>0) gy_o--;
else {gy_o++;}
if (p_gz>0) gz_o--;
else {gz_o++;}
sensor.setXGyroOffset(gx_o);
sensor.setYGyroOffset(gy_o);
sensor.setZGyroOffset(gz_o);
counter=0;
}
counter++;
}
Anexo 5: Código II Arduino

/* UNIVERSIDAD NACIONAL DE TRUJILLO
* CURSO BIOMECANICA
* CARACTERIZACION DE MOVIMIENTO DE UN BRAZO DE 3GDL
*/
// Librerias I2C para controlar el mpu6050

// la libreria MPU6050.h necesita I2Cdev.h, I2Cdev.h necesita Wire.h
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
#include "Adafruit_Sensor.h"
#include <Servo.h>
Servo myservo; // create servo object to control a servo
Servo myservo2; // create servo object to control a servo
Servo myservo3;
volatile unsigned int cuenta = 0;

#define TCAADDR 0x70
// La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo

// del estado de AD0. Si no se especifica, 0x68 estará implicito
MPU6050 sensor1 (0x68);
// Valores RAW (sin procesar) del acelerometro y giroscopio en los ejes x,y,z
int ax, ay, az;
int gx, gy, gz;
int ax2, ay2, az2;

int gx2, gy2, gz2;
int ax3, ay3, az3;

int gx3, gy3, gz3;
float ang_x, ang_y, ang_z;

float ang_x_prev, ang_y_prev;
float ang_x2, ang_y2, ang_z2;

float ang_x_prev2, ang_y_prev2;
float ang_x3, ang_y3, ang_z3;

float ang_x_prev3, ang_y_prev3;
float VarSensor = 10;//varianza del sensor a determinar

float VarProcess = 0.1;// Varianza del proceso pequeña
float P = 1.0;
float Pc = 0.0;
float G = 0.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;
float VarSensor2 = 10;//varianza del sensor a determinar

float VarProcess2 = 0.1;// Varianza del proceso
float P2 = 1.0;
float Pc2 = 0.0;
float G2 = 0.0;
float Xp2 = 0.0;
float Zp2 = 0.0;
float Xe2 = 0.0;
float VarSensor3 = 10; // varianza del sensor a determinar

float VarProcess3 = 0.1;// Varianza del proceso
float Pc3 = 0.0;
float G3 = 0.0;
float Xp3 = 0.0;
float Zp3 = 0.0;
float Xe3 = 0.0;
void setup() {
// Se declaran la
myservo.attach(8);
myservo2.attach(9);
myservo3.attach(10);
myservo.write(120);
myservo2.write(65);
myservo3.write(30);
Serial.begin(115200); //Iniciando puerto serial

Wire.begin(); //Iniciando I2C
tcaselect(1);
sensor1.initialize(); //Iniciando el sensor
tcaselect(2);
tcaselect(3);
/*
if (sensor1.testConnection()) Serial.println("Sensor iniciado correctamente");
else Serial.println("Error al iniciar el sensor 1");
if (sensor2.testConnection()) Serial.println("Sensor iniciado correctamente");
else Serial.println("Error al iniciar el sensor 1");
delay(2000);
*/
}
void tcaselect(uint8_t i) {
if (i > 7) return;
Wire.beginTransmission(TCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}
float kalman(float dato){

// Filtro de kalman
Pc = P + VarProcess;
G = Pc/(Pc + VarSensor); // Ganancia de kalman
P = (1-G)*Pc;
Xp = Xe;
Zp = Xp;
Xe = G*(dato-Zp)+Xp; // Estimacion del estado sensado
return(Xe);
}
float kalman2(float dato2){

// Filtro de kalman
Pc2 = P2 + VarProcess2;
G2 = Pc2/(Pc2 + VarSensor2); // Ganancia de kalman
P2 = (1-G2)*Pc2;
Xp2 = Xe2;
Zp2 = Xp2;
Xe2 = G2*(dato2-Zp2)+Xp2; // Estimacion del estado sensado
return(Xe2);
}
float kalman3(float dato3){

// Filtro de kalman
Pc3 = P3 + VarProcess3;
G3 = Pc3/(Pc3 + VarSensor3); // Ganancia de kalman
P3 = (1-G3)*Pc3;
Xp3 = Xe3;
Zp3 = Xp3;
Xe3 = G3*(dato3-Zp3)+Xp3; // Estimacion del estado sensado
return(Xe3);
}
void loop(){
// Leer las aceleraciones angulares
tcaselect(1);
sensor1.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
float accel_ang_y=atan(ay/sqrt(pow(ax,2) + pow(az,2)));
ang_y=kalman(accel_ang_y);
int ang1=((-70/1.2)*ang_y)+120;
if(ang1>120){
ang1=120;
}
if(ang1<50){
ang1=50;
}
tcaselect(2);
sensor2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);
float accel_ang_y2=atan(ay2/sqrt(pow(ax2,2) + pow(az2,2)));
ang_y2=kalman2(accel_ang_y2);
int ang2=(-65/0.44)*(ang_y2-1.3);
if(ang2>65){
ang2=65;
}
if(ang2<0){
ang2=0;
}
tcaselect(3);
sensor3.getMotion6(&ax3, &ay3, &az3, &gx3, &gy3, &gz3);
float accel_ang_y3=atan(ay3/sqrt(pow(ax3,2) + pow(az3,2)));
ang_y3=kalman3(accel_ang_y3);
int ang3=((60/1.03)*(ang_y3-0.3))+30;
if(ang3>90){
ang3=90;
}
if(ang3<30 or ang1<100){
ang3=30;
}
Serial.print(ang_y);
Serial.print(",");
Serial.print(ang_y2);
Serial.print(",");
Serial.println(ang_y3);
/* Serial.print(accel_ang_y);
Serial.print(",");
Serial.print(ang_y);
Serial.print(",");
Serial.print(accel_ang_y2);
Serial.print(",");
Serial.print(ang_y2);
Serial.print(",");
Serial.print(accel_ang_y3);
Serial.print(",");
Serial.println(ang_y3);
*/
myservo.write(ang1);
myservo2.write(ang2);
myservo3.write(ang3);
Anexo 5: Script de Matlab
%% UNIVERSIDAD NACIONAL DE TRUJILO

% ING. MECATRONICA
% CURSO BIOMECANICA
% CARATERIZACIÓN DE MOVIMIENTO PARA UN BRAZO DE 3GDL
%Parámetos de configuración inicial para comunicación con VREP.

vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
vrep.simxFinish(-1); % just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);
a=zeros(3,1)
%************************************************************************
**
if (clientID>-1)
disp('Connected to remote API server');
% Se crea algunas posiciones de las articulaciones:
h=[0,0,0];
[r,h(1)]=vrep.simxGetObjectHandle(clientID,'Hombro',vrep.simx_opmode_bloc
king );
[r,h(2)]=vrep.simxGetObjectHandle(clientID,'Antebrazo',vrep.simx_opmode_b
locking );
[r,h(3)]=vrep.simxGetObjectHandle(clientID,'Brazo',vrep.simx_opmode_block
ing );
%Inicializar puerto serial a usar
delete(instrfind({'Port'},{'COM5'}));
puerto_s=serial('COM5','BaudRate',115200,'Terminator','CR/LF');
warning('off','MATLAB:serial:fscanf:unsuccessfulRead');
%Apertura de puerto serial
fopen(puerto_s);
j=1;
while true
a = fscanf(puerto_s,'%e,%e,%e',21)
ang1=((pi/2)*a(1)/1.2)-(pi/2);
if ang1>0
ang1=0;
end
if ang1<-pi/2
ang1=-pi/2;
end
ang2=(-(pi/2)/(1.38-0.86))*(a(2)-0.86);
if ang2>0
ang2=0;
end
if ang2<-pi/2
ang2=-pi/2;
end
ang3=((pi/2)/1.03)*(a(3)-0.3);
if ang3>pi/2
ang3=pi/2;
end
if ang3<0 || ang1>-(80*(pi/180))
ang3=0;
end
a1(j,1)=ang1*180/pi;
a2(j,1)=ang2*180/pi;
a3(j,1)=-ang3*180/pi;
joint_pos1=[ang1,ang2,-ang3]
for i=1:3
vrep.simxSetJointTargetPosition(clientID,h(i),joint_pos1(i),vrep.simx_opm
ode_streaming);
end
j=j+1;
end
fclose(puerto_s);
delete(puerto_s);
clear puerto_s;
else
disp('Failed connecting to remote API server');
end
vrep.delete(); % call the destructor!
disp('Program ended');

Caracterización Biomecánica Del Movimiento de Un Brazo Humano Asociado A Un Brazo Robótico de 3GDL

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Caracterización Biomecánica Del Movimiento de Un Brazo Humano Asociado A Un Brazo Robótico de 3GDL

Cargado por

Copyright:

Formatos disponibles

UNIVERSIDAD NACIONAL DE TRUJILLO

CARACTERIZACION BIOMECANICA DEL

- Diseño e implementación de un brazo robótico articular de 3GDL y de una

4.1.1. CINEMATICA DIRECTA

𝑃𝑈𝑉𝑊 = 𝑇 ∗ 𝑃𝑋𝑌𝑍 (4-2)

Para el caso de robots manipuladores de “n” grados de libertad, las matrices de

4.1.2. CINEMATICA INVERSA

 Matriz de transformación homogénea

I2C significa Circuito Interintegrado y es un protocolo de comunicación serial

Figura 2:Ejemplificacion de la relacion Maestro-Esclavo entre dos equipos

Figura 3: Estructura del mensaje en Protocolo I2C (TeslaBEM, 2017).

Esclavo (Nivel Bajo de voltaje = Escritura), o si el Maestro quiere

4.5.2. SENSOR ACELEROMETRO

 Salida digital de 6 ejes.

Figura 4:MPU6050 (hetpro-store, 2014).

5. DESARROLLO DEL PROYECTO

Figura 5: Diseño Cad del brazo de 3GDL en el software SolidWorks

Figura 7: Impresión 3D del brazo de 3GL.

Figura 8: Brazo de 3GDL importado al software V-REP.

5.2 ADQUISICIÓN DE DATOS

Figura 9: Tarjeta Arduino Mega conectada a un multiplexor y sensores.

Figura 10: Posicionamiento de sensores a lo largo del brazo.

5.3 FILTRADO DE DATOS

- 𝑋(0): Vector inicial de estado.

Al inicializar el sensor, los rangos por defecto serán:

iniciales en los servomotores, estos son mostrados en el anexo 3 junto al algoritmo

5.5 SIMULACIÓN EN V-REP

Figura 15: Movimiento de flexión para el brazo de 3GDL.

Figura 16: Movimiento de aducción-abducción para el brazo de 3DGL.

Borjas, R. (2015). Fabricación de una Prótesis Humana utilizando una impresora 3D

// La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo

//Variables usadas por el filtro pasa bajos

//Valor de los offsets

if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente");

// Leer los offset los offsets anteriores

// Filtrar las lecturas

//Cada 100 lecturas corregir el offset

//Calibrar el acelerometro a 1g en el eje z (ajustar el offset)

//Calibrar el giroscopio a 0º/s en todos los ejes (ajustar el offset)

Anexo 5: Código II Arduino

// Librerias I2C para controlar el mpu6050

volatile unsigned int cuenta = 0;

// La dirección del MPU6050 puede ser 0x68 o 0x69, dependiendo

int ax2, ay2, az2;

int ax3, ay3, az3;

float ang_x, ang_y, ang_z;

float ang_x2, ang_y2, ang_z2;

float ang_x3, ang_y3, ang_z3;

float VarSensor = 10;//varianza del sensor a determinar

float VarSensor2 = 10;//varianza del sensor a determinar

float VarSensor3 = 10; // varianza del sensor a determinar

Serial.begin(115200); //Iniciando puerto serial

float kalman(float dato){

float kalman2(float dato2){

float kalman3(float dato3){

Anexo 5: Script de Matlab

%% UNIVERSIDAD NACIONAL DE TRUJILO

%Parámetos de configuración inicial para comunicación con VREP.

También podría gustarte