TESIS:DISEÑO DE ROBOT BIPEDO CON PLANIFICACIÓN DE TRAYECTORIAS Y ZMP PARA SU ESTABILIDAD
1. UNIVERSIDAD NACIONAL DE TRUJILLO
UNIVERSIDAD NACIONAL DE TRUJILLO
Facultad de Ingeniería
Escuela Profesional de Ingeniería Mecatrónica
DISEÑO DE UN PROTOTIPO DE ROBOT BÍPEDO CON PLANIFICACIÓN DE
TRAYECTORIAS CON CRITERIO ZMP PARA SU ESTABILIDAD AL CAMINAR
SOBRE SUPERFICIE PLANA
TESIS
Para optar el Título Profesional de
INGENIERO MECATRÓNICO
AUTOR: Br. Josmell Henry Alva Alcántara.
ASESOR: Mg. Ing. Edward Javier León Lescano.
Trujillo, Perú
2017
2. UNIVERSIDAD NACIONAL DE TRUJILLO
I
«No hay azar, destino, ni suerte que pueda con la firme resolución de un alma decidida.»
— Ella Wheeler Wilcox.
3. UNIVERSIDAD NACIONAL DE TRUJILLO
II
DEDICATORIA
Dedico el presente trabajo a mis padres Francisco, Mirian y a mis hermanos,
por su compresión y ayuda en cada momento difícil. A mis familiares, amigos, y docentes
que tuvieron palabras de apoyo en todo momento desde el inicio de esta tesis.
4. UNIVERSIDAD NACIONAL DE TRUJILLO
III
AGRADECIMIENTO.
Quiero agradecer a aquellas personas que compartieron sus conocimientos
conmigo para hacer posible la conclusión de esta tesis. Especialmente agradezco a mi
asesor el Ing. Javier León Lescano, que mostró su apoyo como docente y asesor en el
desarrollo de este trabajo. Agradecer a todos mis compañeros las cuales compartimos
experiencias e investigación en esta etapa importante de mi vida.
5. UNIVERSIDAD NACIONAL DE TRUJILLO
IV
INDICE
I.- CAPITULO 1: INTRODUCCIÓN
1.1 Realidad Problemática…………………………………………………………….…1
1.2 Trabajos Previos………………………………………………………………………2
1.2.1 Robot Asimov……………………………………………………………….2
1.2.2 Robot Hubo…………………………………………………………………2
1.2.3 Robot Sdr-4x: Qrio ………………………………………………………...3
1.2.4 Robot Nao…………………………………………………………………..3
1.2.5 Tesis: Diseño y Control de Kokone, un pequeño robot humanoide….4
1.2.6 Tesis “Construcción de un robot Bípedo, Basado en Caminado
Dinámico”…………………………………………………………………4
1.2.7 Tesis “Mecatrónica Bio Inspirada de Robots humanoides de
Tamaño Natural………………………………………………………….5
1.28. Robots humanoides en Perú……………………………………………..5
1.3. Marco Teórico………………………………………………………………………..6
1.3.1 Cinemática del Robot Bípedo. …………………………………………..6
1.3.1.1 Cinemática Directa del Robot Bípedo………………………..6
1.3.1.2 Cinemática inversa del Robot bípedo………………………..8
1.3.2 Análisis Dinámico…………………………………………………………9
1.3.2.1 Dinámica Newton-Euler……………………………………… 10
1.3.2.2 Sistemas de Coordenadas rotantes…………………………10
1.3.2.3 Cinemática de los elementos del robot……………………...11
1.3.2.4 Ecuaciones de movimiento recursivas para
Manipuladores………………………………………………….13
1.3.3 Planificación de trayectorias…………………………………………….14
1.3.3.1 Locomoción Humana………………………………………….15
1.3.3.2 Ciclo de Marcha………………………………………………..15
1.3.2.3 Fases de la Marcha……………………………………………16
1.3.2.4. Locomoción Pasiva…………………………………………...17
1.3.2.5 Locomoción Activa…………………………………………….17
1.3.4 Punto de Momento Cero, ZMP………………………………………….18
1.3.5.- Arquitectura de la Pierna Humana……………………………………19
1.4. Formulación del Problema………………………………………………………...20
1.5 Justificación del Estudio……………………………………………………………20
1.6 Hipótesis……………………………………………………………………………..21
1.7 Objetivos……………………………………………………………………………..21
1.7.1 Objetivo General………………………………………………………….21
1.7.2 Objetivos específicos…………………………………………………….21
II CAPITULO: MÉTODO
2.1 Diseño de investigación……………………………………………………………23
2.2. Variables.
2.2.1 Variables Independiente……………………………………………......23
2.2.2 Variables dependientes………………………………………………....23
2.3 Población y muestra………………………………………………………………..23
2.4 Técnicas e Instrumentos…………………………………………………………..23.
6. UNIVERSIDAD NACIONAL DE TRUJILLO
V
III CAPITULO: RESULTADOS.
3.1 Cinemática directa del robot bípedo 10GDL…………………………………… 24
3.1.1 Cinemática directa de la cadera con respecto al pie de apoyo……..26
3.1.2 Cinemática directa del pie flotante con respecto a la cadera………. 31
3.2 Cinemática inversa del robot bípedo 10GDL…………………………………... 34
3.2.1 Paso a seguir en el algoritmo implementado………………………….34
3.2.2 Cinemática inversa de la cadera del robot bípedo 10GDL…………..38
3.2.3 Cinemática inversa del pie flotante del robot bípedo 10GL………….42
3.3 Análisis Dinámico…………………………………………………………………...45
3.3.1 Algoritmo computacional……………………………………………….. 45
3.3.2 Dinámica Newton Euler del robot bípedo 10 GDL………………….. 47
3.3.3 Dinámica del pie flotante respecto a la cadera……………………… 47
3.3.3 Dinámica de la cadera respecto al pie de apoyo……………………..52
3.4 Arquitectura de robot bípedo 10GDL……………………………………………. 57
3.4.1. Modelo CAD 3D del robot bípedo 10GDL…………………………... 57
3.4.2. Modelo 3D Simplificado del robot bípedo de 10GDL……………… 58
3.5 Planificación de la caminata de robot bípedo 10GDL…………………………..59
3.5.1 Control cinemático……………………………………………………….59
3.5.2 Trayectorias robot bípedo 10GDL…………………………………….. 59
3.5.3 Interpolación de trayectorias……………………………………………59
3.5.4 Planificación de la cadera respecto al pie de apoyo………………... 61
3.5.4.1 Plano sagital…………………………………………………... 61
3.5.4.2 Plano frontal…………………………………………………... 64
3.5.5 Planificación del pierna flotante……………………………………….. 66
3.5.6 Inicio y parada del robot bípedo 10GDL…………………………. … 69
3.5.6.1 Paso de inicio……………………………………………… ..70
3.5.6.2 Paso de fin…………………………………………………….. 71
3.6.- Criterio de estabilidad ZMP……………………………………………………… 76
3.6.1 ZMP, robot bípedo de 10GDL………………………………………… .78
3.7 Simulación de la caminata del robot Bípedo 10GDL………………………… ...84
3.7.1 Simulación del Robot Bípedo 10GDL…………………………………..85
3.7.2 Movimiento simulado de la pierna de apoyo…………………………..91
3.7.3 Movimiento simulado de la pierna flotante…………………………….92
IV CAPITULO: DISCUSIÓN DE RESULTADOS.
4.1 Cinemática directa…………………………………………………………………. 97
4.2 Cinemática inversa………………………………………………………………….97
4.3 Dinámica del robot bípedo………………………………………………………... 97
4.4 Diseño del Robot bípedo…………………………………………………………...97
4.5 Planificación de trayectorias……………………………………………………….97
4.6 Criterio de estabilidad de caminata ZMP…………………………………………97
V CAPITULO: CONCLUSIONES Y RECOMENDACIONES.
5.1 Conclusiones……………………………………………………………………….98
5.2 Recomendaciones…………………………………………………………………99
VI CAPITULO: REFERENCIAS BIBLIOGRÁFICAS
6.1 Bibliografía………………………………………………………………………….100
7. UNIVERSIDAD NACIONAL DE TRUJILLO
VI
ANEXOS:
ANEXOS A. RESULTADOS DE ECUACIONES OBTENIDAS…………………..102
A.1 Resultado de Cinemática Directa de la “cadera respecto al pie de apoyo”…102
A.2 Resultado de Cinemática Directa de la “pie flotante respecto a la cadera”…103
A.3. Resultados de Cinemática Directa de la pierna flotante respecto al pie
de apoyo…………………………………………………………………………….104
A.4 Resultados de la dinámica de la Pierna Flotante………………………………109
A.5 Resultados de la dinámica de la Pierna de apoyo…………………………….124
A.6 Resultados de las ecuaciones ZMP, Velocidades y
Aceleraciones del COG. ………………………………………………………….133
A.6.1 Velocidades del COG del Robot Bípedo. ……………………………133
A.6.2 Aceleraciones del COG del Robot Bípedo…………………………..138
ANEXO B: ALGORITMOS DE MALTAB IMPLEMENTADOS:…………………..144
B.1 Cinemática Directa………………………………………………………………...144
B.1.1. Cinemática Directa: archivo Script……………………………………………144
B.1.2.Función para el cálculo de la cinemática directa- Cadera respecto al
pie de apoyo……………………………………………………………………145
B.1.3 Función para el cálculo de la cinemática directa- Pie flotante respecto
a la cadera……………………………………………………………………..146
B.1.4 Cinemática Directa de la Pie flotante respecto a pie de apoyo……..……..147
B.2. Cinemática Inversa……………………………………………………………….148
B.2.1 Cinemática inversa de la cadera respecto al pie de apoyo………………...148
B.2.2 Cinemática inversa del pie flotante respecto al pie de apoyo…………......152
B.2.3 Algoritmo para generar los ángulos de la Cinemática inversa en
función a las trayectorias propuestas…………………………………………162
B.3 Cálculo de la Dinámica……………………………………………………………163
B.3.1 Se presenta el archivo Script de cálculo de la dinámica……………………163
B.3.2 La dinámica del robot bípedo 10GDL del pie flotante respecto
a la cadera……………………………………………………………………….166
B.3.3 La Dinámica para el robot de la cadera respecto al pie de Apoyo………...170
B.3.4 Script para el cálculo de los toques, velocidades y aceleraciones
angulares………………………………………………………………………...174
B.3.5 5 Script para las gráficas de torques de pie flotante, velocidad
y aceleraciones articulares del robot bípedo………………………………175
B.4 Algoritmos para la Planificación de Trayectorias………………………………178
B.4.1 Algoritmo que describe la generación de trayectorias del robot bípedo…..178
B.4.2 Algoritmo para la generación de ángulos para aplicar la
cinemática inversa……………………………………………………………….184
B-4.3 Script que permite graficar los ángulos dados para la cinemática inversa.185
B.5 Algoritmos para el Cálculo del ZMP……………………………………………..190
B.5.1 Función que permite calcular la velocidad y aceleración del COG………..190
B.5.2 Función que permite calcular la velocidad y aceleración del COG. ………191
ANEXO C PLANOS DEL ROBOT BÍPEDO………………………………………..193
C.1 Masa de los eslabones del Robot……………………………………………….201
8. UNIVERSIDAD NACIONAL DE TRUJILLO
VII
RESUMEN
La presente tesis busca diseñar un robot bípedo de 10GDL en total 5GDL por cada
pierna, con planificación de trayectorias con criterio de ZMP para su estabilidad al caminar
sobre superficie plana. Se inicia presentando el desarrollo del modelo cinemático directo,
mediante el método de Denavit-Hartenberg, de matrices de transformación homogénea;
luego se desarrolló la cinemática inversa, para esto se decidió utilizar un método numérico
para su solución, aplicando el criterio de “Damped Least Square”, esto nos permitirá
encontrar las posiciones, velocidades y aceleraciones de cada uno de los GDL de robot.
El modeló dinámico de este proyecto de investigación se realizó mediante la
formulación de Newton - Euler, para calcular los torques que se producen en cada
articulación, debido a que es más rápido computablemente respecto al de Lagrange -Euler,
esto se debe a su naturaleza recursiva y vectorial; además asumiendo que no existe
deslizamiento entre el suelo y el piso.
Una vez tenido el modelo cinemático, y la dinámica se procede a hacer el modelo
CAD del robot bípedo en Solidworks.
Se planificó las trayectorias de movimiento del robot bípedo 10GDL en los planos
sagital y frontal, para esto se utilizó las “Spline Cúbic” asegurando la existencia de sus
derivadas (velocidad y aceleración) permitiendo el movimiento del robot se asemejara a la
caminata humana.
Se evaluó la estabilidad de la caminata del robot bípedo mediante el criterio de ZMP,
para esto se modeló el robot bípedo mediante un péndulo invertido en 3D, basado en las
trayectorias generadas, además para esto fue necesario calcular la velocidad y aceleración
del COG.
Se logró generar la simulación del robot bípedo en la plataforma de Simulink
utilizando la librería de SimMechanics, siguiendo las trayectorias propuestas para la cadera
y el pie flotante, esperando fundamentalmente que tenga un impacto investigativo,
científico - tecnológico en la Universidad Nacional de Trujillo, y en el país que permita abrir
nuevos campos de estudio en base a este proyecto de diseño de robot Bípedo.
9. UNIVERSIDAD NACIONAL DE TRUJILLO
VIII
ABSTRACT
The present thesis aims to design a biped robot of 10GDL in total 5GDL per leg, with
planning of trajectories with criterion of ZMP for its stability when walking on flat surface. It
begins by presenting the development of the direct kinematic model, by the Denavit-
Hartenberg method, of homogeneous transformation matrices; Then the inverse kinematics
developed, so it was decided to use a numerical method for its solution, applying the
criterion of "Damped Least Square", this will allow us to find the positions, velocities and
accelerations of each of the robot GDL.
The dynamic modeling of this research project was done by the formulation of
Newton - Euler, to calculate the torques that occur in each joint, because it is faster
computablely than the Lagrange - Euler, this is due to its recursive nature And vectorial;
Also assuming that there is no slip between the floor and the floor.
Once the kinematic model has been created, the dynamics will proceed to the CAD model
of the biped robot in Solidworks.
The movement paths of the 10GDL biped robot were planned in the sagittal and
frontal planes. The "Spline Cúbic" was used to ensure the existence of its derivatives
(velocity and acceleration), allowing the movement of the robot to resemble the human walk.
The stability of the walk of the biped robot was evaluated by means of the ZMP criterion,
for this the biped robot was modeled by a pendulum inverted in 3D, based on the trajectories
generated, in addition to this it was necessary to calculate the speed and acceleration of
the COG.
It was possible to generate the simulation of the biped robot in the Simulink platform
using the SimMechanics library, following the proposed paths for the hip and the floating
foot, expecting fundamentally that it has an investigative, scientific - technological impact in
the National University of Trujillo, and In the country that allows to open new fields of study
based on this design of Bipedal robot design.
10. UNIVERSIDAD NACIONAL DE TRUJILLO
1
CAPITULO I: INTRODUCCIÓN.
1.1 Realidad Problemática:
La robótica es una rama de la tecnología integrada por un conjunto de conocimientos
teóricos y prácticos que permiten el diseño, construcción, operación y aplicación de los
robots; la robótica además combina diversas disciplinas como la física, mecánica,
electrónica, informática, ingeniería de control e inteligencia artificial.
La robótica está tomando un papel muy importante para el futuro de la sociedad, ya
que cada vez hay más robots que desempeñan tareas del ser humano. Por ejemplo los
robots manipuladores que son utilizados en procesos industriales sirven para trabajar con
materiales peligrosos, también sirven para desempeñar procesos en serie.
Por otro lado, existen también la robótica móvil con ‘patas’, los cuales tienen dos
extremidades inferiores para realizar su desplazamiento, conocidos como bípedos, o robots
humanoides, al querer imitar el comportamiento del caminado, acciones efectuadas y
apariencia del ser humano que por excelencia es el ser vivo que mejor aprovecha la
locomoción bípeda,.
La caminata bípeda al momento de imitar es una de las más complicadas en
comparación con cuadrúpedos o hexápodos debido a la complejidad de mantener el
equilibrio, por eso, es está la limitación de los proyectos de investigación científica en esta
área de la robótica, muchas empresas ya consolidadas tienen grandes resultados pero esto
debido a años de investigación, muchos ingenieros atrás del proyecto y presupuestos muy
altos como es el caso de Honda, Sony,etc. también se dan grandes competencias
internaciones de robot humanoide como es el caso de “Darpa Robotics Challenger”,etc.
Dada la importancia de desarrollo de tecnologías de esta tesis que se encuentran en
la línea de investigación de robots androides, rama que está en evolución en nuestro país,
como lo muestra los últimos papers presentados en los últimos (CONEIMERA), por lo que,
mediante la realización de un robot humanoide se desea contribuir con un escalón más
sobre el cual se levante proyectos de una mayor complejidad y aplicaciones en este campo,
como también aportar al desarrollo en proyectos que tengan impacto en la industria minera,
metalúrgica, metal mecánica, médica, para rehabilitación, con desarrollo de exoesqueletos
o robots bípedos –humanoide independientes que puedan solución problemas que es
inaccesible o peligroso para las personas.
Por esas y otras razones la robótica es imprescindible para el desarrollo y competitividad
de un país. Debe quedar claro que no se está hablando de importación de tecnología de
otros países sino más bien de la comprensión de la ciencia que hay detrás y el desarrollo
de propia tecnología.
11. UNIVERSIDAD NACIONAL DE TRUJILLO
2
1.2 .-Trabajos Previos:
Desde la década de 1980's se han construido maquinas bípedas de tamaño similar a
las personas. Después de años de investigación y desarrollo, los primeros robots
humanoides hicieron su aparición en los años 1990's. La construcción de robots
humanoides de tamaño real es un gran reto tecnológico.
Para el desarrollo de un solo prototipo se necesitan fuertes recursos económicos y
sobre todo, un respaldo en tecnología. De tal manera, únicamente grandes empresas e
instituciones académicas de prestigio son los principales fabricantes de este tipo de robots.
1.2.1.- Robot Asimo:
El robot ASIMO De la compañía japonesa Honda es uno de los ejemplos más
completos en robots humanoides bípedos hoy en día. ASIMO (Advanced Step in
Innovative Mobility, Paso Avanzado en Movilidad Innovadora) fue concluido el 20 de
noviembre del año 2000. La versión más reciente mide 130 cm y pesa 48 kg. Está
provisto de 57 GDL distribuidos de la siguiente manera: 3 en la cabeza, 7 en cada
brazo, 2 en cada mano, 13 en la cadera y 6 en cada pierna. ASIMO implementa la
tecnología llamada i-WALK, (Intelligent Real-Time Flexible Walking, Caminado Flexible
e Inteligente en Tiempo Real), que le permite girar cuando camina sin pausas en su
movimiento.
ASIMO posee también una gran capacidad de interacción con las personas gracias
a su tecnología de reconocimiento visual. Es capaz de interpretar los movimientos de
una persona e interactuar con esta Corre a una velocidad de 9 km/h en línea recta y a
5 km/h de manera circular.
Figura 1.2.1 Robot Asimo
(Fuente: http://science.howstuffworks.com/asimo.htm)
1.2.2.- Robot Hubo:
La construcción del robot HUBO fue llevada a cabo por el KAIST (Korea
Advanced Instituto of Science and Technology, Instituto Avanzado de Ciencia y
Tecnología de Korea). Este robot es la continuación de los trabajos realizados en
robótica humanoide en este instituto. HUBO tiene como predecesores al robot KHR-
1 y KHR-2, además de su análogo Albert HUBO (en honor a Albert Einstein, un robot
12. UNIVERSIDAD NACIONAL DE TRUJILLO
3
humanoide con características similares a HUBO, pero con la peculiaridad de que su
rostro es una recreación del rostro del famoso científico).
Un HUBO Introducido por TEAM KAIST fue el ganador de las DARPA Robotics reto
final de 6 de junio del 2015. La máquina llamada RDC – Hubo fue una adaptación del
dispositivo multifuncional.
Figura 1.2.2. Robot Hubo
(Fuente: http://www.koreaittimes.com/image/hubo-robot)
Los robots de tamaño mediano cuentan con características parecidas a los robots
humanoides de tamaño real y a los de tamaño pequeño. Son construidos por instituciones
educativas y por empresas, con un mercado de consumo creciente para el entretenimiento.
1.2.3.- Robot Sdr-4x: Qrio:
El robot SDR-4X, también conocido como Qrio, es la culminación de la investigación
en robots humanoides por parte de SONY. Tiene como predecesor al robot SDR-3X. Las
siglas SDR derivan de SONY Dream Robot o en español Robot Sonado por Sony, y sus
orígenes se remontan al año de 1997 cuando la compañía empezó a desarrollar sus
primeros prototipos. Qrio (como también es conocido) mide 58 cm y tiene un peso
aproximado de 7 kg.
1.2.4.- Robot Nao:
Es un robot humanoide programable y autónomo, desarrollado por Aldebaran
Robotics, una compañía de robótica francesa con sede en París. El desarrollo del robot
comenzó con el lanzamiento del Proyecto Nao en 2004.
En el 15 de agosto de 2007, Nao sustituye al perro robot Aibo de Sony como la
plataforma estándar para la Robocup ("Robot Soccer World Cup"), un concurso
internacional de robótica. y el NaoV3R fue elegido como la plataforma para el SPL en
RoboCup 2010, actualmente es uno de los robot más modernos.
13. UNIVERSIDAD NACIONAL DE TRUJILLO
4
Figura 1.2.3. Robot Nao
(Fuente: https://www.aldebaran.com/en/humanoid-robot/nao-robot)
1.2.5.- Tesis “Diseño y Control de Kokone, un pequeño robot humanoide”, - Ing.
Víctor Enrique González Hernández, Centro de Investigación y de Estudios
Avanzados del Instituto Politécnico Nacional. México 2009.
En este proyecto de tesis, proponemos el diseño y control de Kokone (niños en
náhuatl), un pequeño robot humanoide. Kokone tiene como objetivos asistir en el desarrollo
de estrategias de control y fomentar el uso de métodos computacionales en la solución de
problemas de robótica humanoide. El esquema de la plataforma robótica facilita las pruebas
de laboratorio, pues se aprovecha la flexibilidad del uso de una PC como unidad de control.
La distribución de sus 22 grados de libertad, permiten realizar la mayoría de los movimientos
del cuerpo humano, y su diseño mecánico, realizado en Solid Edge, requiere un mínimo de
piezas y un fácil ensamblaje. Con base en el diseño final, se resolvió la cinemática directa
mediante el método de Denavit-Hartenberg, y se propuso un algoritmo genético para
resolver la cinemática inversa. Como primera aplicación, se propuso una estrategia de
caminado basado en los conceptos de la caminata bípeda estática. Los desplazamientos
obtenidos se verificaron en Matlab, para después ser implementados satisfactoriamente en
el robot Kokone.
1.2.6.- Tesis “Construcción de un robot Bípedo, Basado en Caminado Dinámico”.
Ing. Cesar Humberto Guzmán Valdivia, Centro Nacional de Investigación y Desarrollo
Tecnológico, México 2010.
Se llevó a cabo un análisis de la etapa de avance de la pierna, el cual cuenta con
ecuaciones cinemáticas y dinámicas. El resultado de las ecuaciones cinemáticas es para
conocer la posición y orientación del extremo de la pierna. Se utilizó el método del
lagrangiano para describir el comportamiento dinámico de la pierna cuando se encuentra
en la fase de avance. También se analizó la cadera del robot para conocer el peso máximo
que puede soportar la estructura. Se simuló el robot virtualmente para comprobar que la
estructura mecánica no presentara problemas y ver la estabilidad que tiene al dar un paso.
El robot se construyó bajo un enfoque mecatrónica; cuenta con parte mecánica, electrónica
14. UNIVERSIDAD NACIONAL DE TRUJILLO
5
y programación. El prototipo que se realizó cuenta con cinco grados de libertad, uno en la
cadera, dos en las rodillas y dos en los tobillos, los brazos están mecánicamente ligados a
los muslos por medio de bandas y poleas, las rodillas cuentan con un sistema interno de
enclavamiento, la cadera mantiene el robot erguido mediante un sistema de engranes y
cadenas, los músculos neumáticos proporcionan el par necesario para mover el robot. El
prototipo caminó de forma análoga a la locomoción, Humana.
1.2.7.- Tesis “Mecatrónica BioInspirada de Robots humanoides de Tamaño Natura”-
Ing. Luis María Cabás Ormaechea, Universidad Carlos III de Madrid, España 2009.
Esta tesis se enmarca dentro del estudio de robots humanoides y se centra
específicamente en el estudio de mecatrónica de los mismos. El problema de resolver es
como diseñar de forma eficiente esta clase de robots para que puedan desplazarse de
forma automáticamente y realizar una gran variedad de tareas. Por tal motivo, en esta tesis
se desarrolla y aplica unas metodologías de diseño para u robot humanoide de tamaño
natural tomando como modelo algunas características del ser humano. Esta metodología
pretende resolver de forma progresiva el problema del diseño de un robot humanoide
utilizando aquellas analogías físicas y mecánicas que tienen este tipo de ingenieros
mecánicos con el cuerpo humanos y a partir de eso, utilizar como piedra angular para
obtener los modelos que nos ayudaran a logran nuestro propósito, de diseñar un humanoide
poli funcional.
1.2.8.- Robots humanoides en Perú:
En nuestro país se han desarrollado trabajos muy interesantes en el campo de la
robótica bípeda – humanoide. Estudiantes de ingeniería Mecatrónica de la Universidad
Nacional de Ingeniería UNI, presentaron un robot Bípedo en el CONEIMERA- 2010
realizado en la Ciudad de Trujillo, la cual se llevó el 1° lugar en la competencia de más de
200 proyectos a nivel nacional, este robot era una máquina que tenía específicamente 2
piernas, y se caracterizaba por su movimiento, el cual se asemejaba a la caminata humana.
Otros robots bípedos construidos en nuestro País se desarrolló en la Universidad
Peruana de Ciencias Aplicadas, la cual se Tituló “Diseño y Construcción de un Robot
Bípedo caminante”, El proyecto consistía en implementar un bípedo capaz de caminar y de
captar imágenes a través de una cámara inalámbricamente en la parte superior del cuerpo,
actuando como si fuera la cabeza del robot. Contaba con 5 GDL piernas y 2 GDL en la
cabeza la cual suma un total de 12 GDL.
En el INTERCON 2011, estudiantes de la Universidad Nacional de Ingeniería
presentan un proyecto Walking Simulation and Controllers for a 18DOF small humanoid
robot, quedando en el 1° puesto.
Con la literatura revisada, se recabo la información necesaria para definir puntos
importantes del proyecto, de los cuales podemos mencionar el esquema general del
15. UNIVERSIDAD NACIONAL DE TRUJILLO
6
sistema, diseño del robot, software de diseño, elección de los materiales para la
construcción y programas de control del robot.
1.3.- Marco Teórico.
En este capítulo se presentan los conceptos necesarios para el desarrollo de la
tesis.
1.3.1.- Cinemática del robot bípedo:
El análisis Cinemático de un robot, comprende el estudio de su movimiento con respecto
a un sistema de referencia.
- Cinemática Directa: Determinar cuál es la posición y orientación del extremo final
del robot, con respecto a un sistema de coordenadas que se toma como referencia,
conocido los valores de las articulaciones y los parámetros geométricos de los
elementos del robot.
- Cinemática inversa: Resuelve la configuración (valores de las articulaciones) que
debe adoptar el robot para una posición y orientación del extremo conocidas.
Cinemática Directa
Cinemática inversa
Figura 1.3.1 Diagrama de Relación entre Cinemática Directa e Inversa
(Fuente: [1])
1.3.1.1 Cinemática directa:
El problema cinemático directo se plantea en términos de encontrar una matriz de
transformación que relaciona el sistema de coordenadas ligado al cuerpo en movimiento
respecto a un sistema de coordenadas que se toma como referencia. Para lograr esta
representación se usan las matrices de transformación homogénea 4x4, la cual incluye
las operaciones de traslación y la orientación.
La matriz de transformación homogénea es una matriz 4x4 que transforma un vector
de posición expresado en coordenadas homogéneas desde un sistema de coordenadas
hasta otro sistema de coordenadas. Una matriz de transformación homogénea se puede
considerar que consiste en sub matrices:
T= [
𝑅3𝑥3 𝑃3𝑥1
𝑓1𝑥3 1𝑥1
] = [
𝑀𝑎𝑡𝑟𝑖𝑧 𝑑𝑒 𝑅𝑜𝑡𝑎𝑐𝑖𝑜𝑛 𝑉𝑒𝑐𝑡𝑜𝑟 𝑑𝑒 𝑃𝑜𝑠𝑖𝑐𝑖𝑜𝑛
𝑇𝑟𝑎𝑛𝑠𝑓. 𝑃𝑒𝑟𝑠𝑝𝑒𝑐𝑡𝑖𝑣𝑎 𝐸𝑠𝑐𝑎𝑙𝑎𝑑𝑜
]
La Submatriz 3x3 superior izquierda representa la matriz de rotación; la Submatriz
superior derecho 3x1 representa el vector de posición del origen del Sistema de
coordenadas rotado con respecto al Sistema de referencia; la Submatriz inferior izquierda
Valor de las
coordenadas articulares
(q1, q2,…, qn)
Posición y orientación
del Extremo del Robot.
(X, y, z, 𝛼, 𝛽, 𝛾)
16. UNIVERSIDAD NACIONAL DE TRUJILLO
7
1x3 representa la transformación de perspectiva. Y el cuarto elemento diagonal es el factor
de escala global.
La matriz de transformación homogénea se puede utilizar para explicar la relación
geométrica entre el sistema ligado al cuerpo, es decir transformar un vector expresado en
coordenadas homogéneas O’UVW en el sistema de coordenadas OXYZ.
T= [
𝑛 𝑥 𝑠 𝑥 𝑎 𝑥
𝑛 𝑦 𝑠 𝑦 𝑎 𝑦
𝑛 𝑧 𝑠 𝑧 𝑎 𝑧
𝑝 𝑥
𝑝 𝑦
𝑝𝑧
0 0 0 1
]
𝑃𝑥𝑦𝑧 = 𝑇 ∗ 𝑃̃𝑢𝑣𝑤 (1.3.1)
Normalmente la matriz de transformación homogénea que representa la posición y
orientación relativa entre los sistemas asociados a dos eslabones consecutivos del robot
se suele denominar matriz : 𝐴𝑖−1
𝑖. Así pues °𝐴1 Describe la posicion y orientación del
sistema de referencia solidario al 1° eslabón con respecto al sistema de referencia solidario
a la base.
Cuando se consideran todos los grados de libertad, la matriz °An se le suele
denominar T. Así dado un robot de “n” grados de libertad, Se tiene que la posición y
orientación del eslabón final vendrá dada por la matriz T.
T= 𝐴 𝑛
0
= 𝐴1
0
𝐴2
1
𝐴3
2
𝐴4
3
𝐴 𝑛
4
(1.3.2)
a) Denavit – hartenberg (D - H) , propusieron en 1955 un método matricial que permita
establecer de manera sistemática un Sistema de coordenadas {Si} ligado a cada eslabón i,
de una cadena articulada, pudiéndose determinar a continuación la ecuaciones cinemáticas
de la cadena completa [1].
Según la representación de D-H, escogido adecuadamente los sistemas de
coordenadas asociados a cada eslabón, será posible pasar de uno al siguiente mediante 4
trasformaciones básicas que dependen exclusivamente de las características geométricas
del eslabón.
Estas transformaciones básicas consisten en una sucesión de rotaciones y
traslaciones que permitan relacionar el sistema de referencia del elemento i con el sistema
del elemento i-1. Las trasformaciones en cuestión son las siguientes:
1.- Rotación alrededor del eje 𝑧𝑖−1 un Angulo 𝜃𝑖.
2.- Traslación a lo largo de 𝑧𝑖−1 una distancia 𝑑𝑖.
3.-Traslacion a lo largo de 𝑥𝑖 una distancia 𝑎𝑖
4.- Rotación alrededor del eje 𝑥𝑖 un angulo 𝛼𝑖
Dado que el producto de matrices no es conmutativo, las transformaciones se han
de realizar en el orden indicado. De este modo se tiene que:
𝐴𝑖−1
𝑖 = 𝑇(𝑧, 𝜃𝑖)𝑇(0,0, 𝑑𝑖)(𝑇𝑎𝑖, 0,0) 𝑇(𝑥, 𝛼𝑖) (1.3.3)
17. UNIVERSIDAD NACIONAL DE TRUJILLO
8
𝐴𝑖−1
𝑖 = [
𝐶𝜃𝑖 −𝐶𝑎𝑖 𝑆𝜃𝑖 𝑆𝑎𝑖 𝑆𝜃𝑖
𝑆𝜃𝑖 𝐶𝑎𝑖 𝐶𝜃𝑖 −𝑆𝑎𝑖 𝐶𝜃𝑖
0 𝑆𝑎𝑖 𝐶𝑎𝑖
𝐶𝜃𝑖
𝑆𝜃𝑖
𝑑𝑖
0 0 0 1
] (1.3.4)
Donde 𝜃𝑖 , 𝑑𝑖 , 𝛼𝑖 , 𝑎𝑖 Son los parámetros D-H de eslabón i . De esta modo basta con
identificar los parámetros para obtener las matrices de transformación homogénea A y
relacionar así todos y cada uno de los eslabones del robot.
Hay que tener encuentra los parámetros de trabajo de DH.
Figura 1.3.2. Parámetros de Denavit Hartenberg para un eslabón giratorio
(Fuente: [1])
𝜃𝑖 ∶ Es el ángulo de la articulación desde el eje xi-1 hasta el eje xi, medido respecto del eje
zi-1, usando la regla de la mano derecha.
di: Es la distancia medida desde el origen del sistema i-1, a lo largo del eje zi-1 hasta la
intersección del eje zi-1 con el eje xi.
ai: Es la distancia de separación entre los orígenes de los sistemas de referencia i-1 e i,
medida a lo largo del eje xi hasta la intersección con el eje zi-1. (o la distancia más corta
entre los ejes zi-1 y zi, cuando estos no se interceptan)
αi: Es el ángulo que separa los ejes zi y zi-1, medido respecto del eje xi
1.3.1.2 Cinemática inversa del robot bípedo:
El objetivo del problema cinemático inverso consiste encontrar los valores que
deben adoptar las coordenadas articulares del robot q= [q1, q2,…qn]’ para que su extremo
se posicione y oriente según una determinada localización espacial [2]
Con la Cinemática Directa se obtiene la matriz de transformación homogénea que
relación el efector final con la base, la misma que es aplicable para robots simples, es decir
a los que poseen pocos grados de Libertad. Dentro de este campo se Puede resolver la
cinemática inversa por algunos métodos: método Geométrico, Matrices Transformación -
Algebraico, desacoplo cinemático. La principal ventaja de estos métodos es que se pueden
obtener una solución cerrada (exacta), sin embargo, para un robot mayor a 6 GDL ya no
se puede aplicar estos métodos.
Al Investigar en los métodos o técnicas para la resolución de cinemática inversa para
este tipo de Robot (fuente: [3]), se logró identificar las siguientes: resolución de la cinemática
18. UNIVERSIDAD NACIONAL DE TRUJILLO
9
inversa mediante redes neuronales, mediante algoritmos genéticos, y mediante desarrollo
de método numérico (linealización de funciones) .
Para esta tesis se trabajó mediante el desarrollo de método numérico (linealización
de funciones) en este robot bípedo 10GDL se tendrá que resolver la cinemática inversa de
la cadera al pie de apoyo y la del pie flotante respecto al pie de apoyo.
Como el sistema de ecuaciones no es lineal, se utiliza el método de Newton –
Rhapson para linealizar las siguientes funciones:
𝐹𝑖 = (𝑥1, 𝑥2, 𝑥3, … , 𝑥 𝑛); 𝑖 = 1,2,3, … , 𝑛 (1.3.5)
Se utiliza Taylor para linealizar las funciones:
𝑓(𝑥) = 𝑓(𝑥̌) + Δ𝑓(𝑥̌) (1.3.6)
Dónde f(x) es la estimación inicial, 𝑓(𝑥̌) es la corrección no conocida. Si expandimos la
ecuación (1.3.6) se tiene un polinomio de Taylor truncado de primer orden alrededor de
𝑥̌:
f(x) = f(𝑥̌) + ∑
𝜕f(𝑥̌)
𝜕𝜃𝑖
𝑖
𝑖=0 (1.3.7)
El Segundo término de las ecuación (1.3.2.3) se le denomina Jacobiano, “J”.
J=[
𝜕𝑓𝑖
𝜕𝑥𝑖
] (1.3.8)
𝑓 = [
𝑓1(𝑥̌1, 𝑥̌2, 𝑥̌3, … , 𝑥̌ 𝑛)
𝑓1(𝑥̌1, 𝑥̌2, 𝑥̌3, … , 𝑥̌ 𝑛)
⋮
𝑓1(𝑥̌1, 𝑥̌2, 𝑥̌3, … , 𝑥̌ 𝑛)
] (1.3.9)
De estas ecuaciones se obtiene el Jacobiano, y esto nos sirve para encontrar los
ángulos deseados.
∇𝑓 = 𝐽 ∗ ∇θ (1.3.10)
Se aplica el Criterio de “Damped Least Squares” [Fuente: [3]], para resolver la
transpuesta de la matriz Jacobiana.
1.3.2.- Análisis Dinámico:
Las ecuaciones de movimiento de un manipulador son un conjunto de ecuaciones
matemáticas que describen su conducta dinámica.
19. UNIVERSIDAD NACIONAL DE TRUJILLO
10
El modelo dinámico establece relaciones entre las coordenadas articulares (o las
coordenadas del extremo del robot), sus derivadas (velocidad y aceleración), las fuerzas y
pares aplicados en las articulaciones (o en el extremo) y los parámetros del robot (masas
de los eslabones, inercias, etc).
Para ello se realizara el estudio de la formulación de Newton – Euler, con el fin de
conocer las torques necesarias de los motores para poder mover el Bípedo de 5GDL y a la
vez te sirve para definir el tipo de estructura.
1.3.2.1.- Dinámica Newton – Euler.
Esta formulación cuando se aplica a un robot resulta en un conjunto de ecuaciones
recursivas hacia adelante y hacia atrás con términos del tipo de producto vectorial, que
relaciones las velocidades de cada elemento articular desde la base se propagan hacia el
extremo, (recursivas hacia adelante) y los torques, momentos de inercia, fuerzas externas,
hacia atrás desde el efector final hasta la base [Fuente: [2]]
1.3.2.2.- Sistemas de Coordenadas rotantes:
Figura 1.3.2.1 Sistemas de coordenadas en movimiento
(Fuente: [2] pág. 111)
Con respecto a la figura 1.3.2.1 se tiene que el sistema de coordenadas 0* está
girando y trasladándose con respecto al sistema de coordenadas sin asterisco 0, que es un
Sistema inercial. Una partícula P con masa m se localiza mediante vectores r* y r con
respecto al sistema coordenadas O* y O respectivamente. El origen O* se localiza mediante
un vector h con respecto al origen O. La relación entre los vectores de posición r y r* viene
dada por [Fuente: [2]]
𝑟 = 𝑟∗
+ ℎ (1.3.11)
𝑑𝑟
𝑑𝑡
=
𝑑𝑟∗
𝑑𝑡
+
𝑑ℎ
𝑑𝑡
= 𝑣∗
+ 𝑣ℎ (1.3.12)
Donde 𝑣∗
es la velocidad del punto P relativa al sistema de coordenadas 0* en
movimiento y 𝑣ℎ es la velocidad del origen del sistema 0* respecto de la base.
𝑣 =
𝑑𝑟∗
𝑑𝑡
+
𝑑ℎ
𝑑𝑡
= (
𝑑∗ 𝑟∗
𝑑𝑡
+ 𝑤 𝑥 𝑟∗
) +
𝑑ℎ
𝑑𝑡
(1.3.13)
20. UNIVERSIDAD NACIONAL DE TRUJILLO
11
Donde:
𝑑∗ 𝑟∗
𝑑𝑡
es la velocidad lineal del punto P respecto al origen 0* y (𝑤 𝑥 𝑟∗
) es la velocidad angular
del punto P respecto al Origen 0*
Análogamente, la aceleración de la partícula P con respecto al Sistema de coordenadas sin
asterisco.
𝑎 =
𝑑𝑣
𝑑𝑡
=
𝑑2 𝑟∗
𝑑𝑡2 +
𝑑2ℎ
𝑑𝑡2 = 𝑎∗
+ 𝑎ℎ (1.3.14)
Con la ecuación 1.3.13 y 1.3.14 se obtienen:
𝑎 =
𝑑2 𝑟∗
𝑑𝑡2 + 2𝑤𝑥
𝑑∗ 𝑟∗
𝑑𝑡
+ 𝑤𝑥(𝑤𝑥𝑟) +
𝑑𝑤
𝑑𝑡
𝑥𝑟∗
+
𝑑2ℎ
𝑑𝑡2 (1.3.15)
Con estas ecuaciones de desarrollo de coordenadas móviles, se podrá aplicar este
concepto a los sistemas de coordenadas de los elementos que establecimos para un robot.
[Fuente: 2]
1.3.2.3.- Cinemática de los elementos del robot:
Se desarrolla un conjunto de ecuaciones matemáticas que, describirán las
relaciones cinemáticas de los elementos en movimiento de un robot con respecto al sistema
de coordenadas de la base.
A partir de las ecuaciones (1.3.11) a (1.3.15) Se tiene:
Figura 1.3.2.2 Relación entre los sistemas 0, 0* y O’
(Fuente: [2] pág. 113)
𝑣𝑖 =
𝑑∗ 𝑃𝑖
∗
𝑑𝑡
+ 𝑤𝑖−1 𝑥𝑃𝑖
∗
+ 𝑣𝑖−1 (1.3.16)
𝑤𝑖=𝑤𝑖−1 + 𝑤𝑖
∗
La aceleración lineal 𝑣𝑖̇ y la aceleración angular 𝑤̇ 𝑖 del sistema de coordenadas de
la articulación i son:
21. UNIVERSIDAD NACIONAL DE TRUJILLO
12
𝑣𝑖̇ =
𝑑∗2 𝑃𝑖
∗
𝑑𝑡2 + 𝑤̇ 𝑖−1 𝑋𝑃𝑖
∗
+ 2𝑤𝑖−1 𝑋
𝑑∗ 𝑃𝑖
∗
𝑑𝑡
+ 𝑤𝑖−1 𝑥(𝑤𝑖−1 𝑥 𝑃∗
𝑖) + 𝑣̇ 𝑖−1 (1.3.17)
𝑤̇ 𝑖 = 𝑤̇ 𝑖−1 + 𝑤̇ ∗
𝑖 (1.3.18)
La aceleración angular del sistema de referencia i (xi, yi, zi), respecto al sistema de
referencia (𝑥𝑖−1, 𝑦𝑖−1, 𝑧𝑖−1)
𝑤̇ ∗
𝑖 =
𝑑𝑤 𝑖
∗
𝑑𝑡
+ 𝑤𝑖−1 𝑥𝑤∗
𝑖 (1.3.19)
La ecuación 1.3.19 queda definido:
𝑤̇ 𝑖 = 𝑤̇ 𝑖−1 +
𝑑𝑤 𝑖
∗
𝑑𝑡
+ 𝑤𝑖−1 𝑥𝑤∗
𝑖 (1.3.20)
Los sistemas de coordenadas (𝑥𝑖−1, 𝑦𝑖−1, 𝑧𝑖−1) y (𝑥𝑖, 𝑦𝑖, 𝑧𝑖) Están unidos a los
eslabones i-1 e i. La velocidad del eslabón i con respecto al sistema de coordenadas i-1 es
𝑞̇ 𝑖 . Si el eslabón es prismático, la velocidad será una velocidad de traslación relativa
respecto del sistema (𝑥𝑖−1, 𝑦𝑖−1, 𝑧𝑖−1) y si es rotacional le corresponderá una velocidad
rotacional relativa de eslabón i respecto del sistema (𝑥𝑖−1, 𝑦𝑖−1, 𝑧𝑖−1), (Fuente: [2]) en mi
caso del robot bípedo 10GDL todos sus eslabones son rotacionales por lo que las
ecuaciones quedaran definidas de la siguiente forma:
𝑤𝑖
∗
= 𝑍𝑖−1 𝑞̇ 𝑖 (1.3.21)
Donde 𝑞̇ 𝑖 es la magnitud de la velocidad angular del eslabón i con respecto al
sistema de coordenadas (𝑥𝑖−1, 𝑦𝑖−1, 𝑧𝑖−1) De manera similar:
𝑑∗ 𝑤∗
𝑑𝑡
= 𝑍𝑖−1 𝑞̈ 𝑖 (1.3.22)
Debe notarse que el vector de Zi-1 es igual (0,0,1) 𝑇
.
Las velocidades y aceleraciones de los sistemas de coordenadas ligados a cada eslabón
son absolutas y se calcula como:
𝑤𝑖 = 𝑤𝑖−1 + 𝑍𝑖−1 𝑞̇ 𝑖 (1.3.23)
𝑤𝑖 = 𝑤𝑖−1 + 𝑍𝑖−1 𝑞̈ 𝑖 + 𝑤𝑖−1x (𝑍𝑖−1 𝑞̇ 𝑖) (1.3.24)
Las velocidades lineales de los sistemas de referencia de cada eslabón se calcula como:
𝑑∗ 𝑃𝑖
𝑑𝑡
= 𝑤𝑖 𝑥𝑃𝑖
∗
(1.3.25)
𝑑∗2 𝑃𝑖
∗
𝑑𝑡2 =
𝑑∗ 𝑤∗
𝑖
𝑑𝑡
𝑥𝑃∗
𝑖 + 𝑤𝑖
∗
𝑥(𝑤𝑖
∗
𝑥 𝑃∗
𝑖) (1.3.26)
La velocidad lineal absoluta del sistema de coordenadas ligado a cada eslabón se calcula
como:
𝑣𝑖 = 𝑤𝑖 𝑥𝑃∗
𝑖 + 𝑣̇ 𝑖−1 (1.3.27)
22. UNIVERSIDAD NACIONAL DE TRUJILLO
13
𝑣̇ 𝑖 = 𝑤̇ 𝑖 𝑥𝑃∗
𝑖 + 𝑤𝑖 𝑥(𝑤𝑖 𝑥𝑃∗
𝑖) + 𝑣̇ 𝑖−1 (1.3.28)
1.3.2.4.- Ecuaciones de movimiento recursivas para manipuladores:
A partir de las ecuaciones cinemáticas del desarrollo anterior y aplicando el principio
de D’ Alembert del equilibrio estático para todos los instantes de tiempo, se obtienen las
ecuaciones recursivas de Newton – Euler.
Figura 1.3.2.3 Fuerzas y momentos sobre el elemento i
(fuente: [2] Pág 116)
Donde:
𝑚𝑖 : Masa total del eslabón i.
𝑟̅𝑖 : Posición del centro de masa del elemento i desde el origen del sistema de
referencia de la base.
𝑠̅𝑖 : Posición del centro de masas del elemento i desde el origen del sistema
de coordenadas (xi,yi,zi).
𝑃𝑖
∗
: Posición del origen de coordenadas i – esimo con respecto al sistema de
coordenadas (i -1) – esimo.
𝑣̅𝑖 =
𝑑𝑟̅ 𝑖
𝑑𝑡
: Velocidad lineal del centro de masa del elemento i.
𝑎̅𝑖 =
𝑑𝑣̅ 𝑖
𝑑𝑡
: Aceleración lineal del centro de masa del elemento i.
𝐹𝑖 : Fuerza total externa ejercida sobre el elemento i en el centro de masa.
𝑁𝑖 : Momento total externo ejercido sobre ele elemento i en el centro de masa.
𝐼𝑖 :Matriz de Inercia del elemento i respecto de su centro de masa con
respecto al sistema de coordenadas (xo,yo,zo)
𝑓𝑖 : Fuerza ejercida sobre el elemento i por el elemento i-1 en el sistema de
Coordenadas (𝑥𝑖−1, 𝑦𝑖−1, 𝑧𝑖−1) para soportar al elemento i los elemento por
encima de él.
𝑛𝑖 : Momento ejercido sobre el elemento i por el elemento i – 1 en el sistema de
Coordenadas (𝑥𝑖−1, 𝑦𝑖−1, 𝑧𝑖−1).
23. UNIVERSIDAD NACIONAL DE TRUJILLO
14
Si se omiten los efectos del rozamiento viscoso en las articulaciones, y se aplica el
principio de D’ Alembert, se obtienen para cada eslabón:
𝐹𝑖 =
𝑑(𝑚 𝑖 𝑣̅ 𝑖)
𝑑𝑡
= 𝑚𝑖 𝑎̅𝑖 (1.3.29)
𝑁𝑖 =
𝑑(𝐼𝑖 𝑤 𝑖)
𝑑𝑡
= 𝐼𝑖𝑤̇ 𝑖 + 𝑤̇ 𝑖 𝑥(𝐼𝑖 𝑤𝑖) (1.3.30)
Realizando el balance de pares y fuerzas:
𝐹𝑖 = 𝑓𝑖 − 𝑓𝑖+1 (1.3.31)
𝑁𝑖 = 𝑛𝑖 − 𝑛𝑖−1 + (𝑃𝑖−1 − 𝑟̅𝑖)𝑥𝑓𝑖 − (𝑃𝑖 − 𝑟̅𝑖)𝑥𝑓𝑖−1 (1.3.32)
Que utilizando la relación geométrica:
𝑟̅𝑖 − 𝑃𝑖−1 = 𝑃∗
𝑖 + 𝑠̅𝑖 (1.3.33)
Se obtienen las ecuaciones recursivas:
𝑓𝑖 = 𝐹𝑖 + 𝑓𝑖+1 = 𝑚𝑖 𝑎̅ 𝑖 + 𝑓𝑖+1 (1.3.34)
𝑛𝑖 = 𝑛𝑖+1 + (𝑃𝑖
∗
)𝑥𝑓𝑖+1 + (𝑃∗
𝑖 − 𝑠̅𝑖)𝑥𝐹𝑖 + 𝑁𝑖 (1.3.35)
Se observa que estas ecuaciones son recursivas y permiten obtener las fuerzas y
momentos en los elementos i=1,2,3…,n para un robot de n elementos
𝑓𝑖+1 𝑦 𝑛𝑖+1 representan las fuerzas y momentos ejercidos por el efector final del robot sobre
un objeto externo. Por lo tanto el par de fuerzas para cada articulación se expresa como:
𝜏𝑖 = 𝑛𝑖
𝑇
𝑍𝑖−1 + 𝑏𝑖 𝑞̇ 𝑖 (1.3.36)
Donde bi _ coeficiente viscoso de la articulación.
1.3.3.- Planificación de Trayectorias:
Una vez obtenido los modelos cinemáticos y dinámicos del robot se pueden abordar
el problema de control de los mismos. Definir el movimiento de un robot, implica controlar
dicho robot de manera que siga un camino pre planificado. EL objetivo es por tanto
establecer cuáles son las trayectorias que deben seguir cada articulación del robot a lo largo
del tiempo para conseguir los objetivos fijados.
24. UNIVERSIDAD NACIONAL DE TRUJILLO
15
1.3.3.1 .-Locomoción Humana:
El conocimiento de la locomoción humana es la base del tratamiento sistemático y
del manejo de la marcha patológica, especialmente cuando se usan prótesis. La locomoción
humana se ha descrito como una serie de movimientos alternantes, rítmicos, de las
extremidades y del tronco que determinan un desplazamiento con respecto al centro de
gravedad. Aunque existen pequeñas diferencias en la forma de la marcha de un individuo
a otro, estas diferencias caen dentro de pequeños límites.
1.3.3.2.- El ciclo de la marcha
El ciclo de la marcha comienza cuando el pie contacta con el suelo y termina con el
siguiente contacto con el suelo del mismo pie. Los dos mayores componentes del ciclo de
la marcha son: la fase de apoyo y la fase de balanceo (ver Figura 1.3.3.1). Una pierna está
en fase de apoyo cuando está en contacto con el suelo y está en fase de balanceo cuando
no contacta con el suelo.
Figura 1.3.3.1. Ciclo de caminado del ser humano
(Fuente: [11].])
La longitud del paso completo es la distancia lineal entre los sucesivos puntos de
contacto del talón del mismo pie. Longitud del paso es la distancia lineal en el plano de
progresión entre los puntos de contacto de un pie y el otro pie (ver Figura 1.3.3.2).
Figura 1.3.3.2. Longitud del Paso.
(Fuente:[11])
25. UNIVERSIDAD NACIONAL DE TRUJILLO
16
Para que un individuo pueda andar, el sistema locomotor debe cumplir los siguientes
requisitos. Cada pierna debe ser capaz de soportar el peso del cuerpo sin colapsar.
El equilibrio debe ser mantenido tanto de forma estática como dinámica en apoyo
simple. e
La pierna oscilante debe ser capaz de avanzar hasta la posición en que se pueda
convertir en la pierna de soporte.
Debe poder proporcionarse la fuerza suficiente para realizar el movimiento de las
extremidades y avanzar el tronco.
Hay que tomar en cuenta la orientación del cuerpo humano, en la cual se consideran
tres planos de corte, estos planos se denominan planos anatómicos (Cabás, 2009, p. 81).
a) Plano sagital: Pasa desde la parte anterior del cuerpo (o segmento de éste) hasta
la posterior, dividiendo a éste en dos mitades, izquierda y derecha.
b) Plano frontal: Pasa desde un extremo lateral del cuerpo (o segmento de éste) hasta
el otro, dividiendo a este en dos mitades, anterior y posterior.
c) Plano transversal: Pasa horizontalmente el cuerpo (o un segmento de éste),
dividiéndolo en mitades superior e inferior.
Figura 1.3.3.3 Planos de Corte del cuerpo Humano
(Fuente: Blog Biomecánica: José A. Acero Jáuregui)
1.3.3.3.- Fases de la marcha.
El caminado es un proceso cíclico, cada ciclo comienza cuando un pie tiene contacto
con el suelo y termina en el siguiente contacto con el suelo del mismo pie, a la distancia
entre estos dos pasos se le llama paso completo, el ciclo está dividido en dos fases:
26. UNIVERSIDAD NACIONAL DE TRUJILLO
17
Fase de Apoyo: Cuando una pierna está en contacto con el suelo, también se utiliza
el término apoyo doble que es cuando las dos piernas están en contacto con el
suelo.
Fase de Balanceo: Cuando una pierna no contacta con el suelo.
En la Figura 8 se observa que la cantidad de tiempo ocupada por la fase de apoyo es 40%
del ciclo, la fase de doble apoyo es del 20% y la fase de balanceo es de 40% del ciclo
(Hernández, 2008, p. 39).
Figura 1.3.3.4 Componentes de la Marcha.
Fuente: Francisco Hernández (2008). Diseño y construcción de prototipo neumático de
prótesis de pierna humana. Capítulo 3. P.38.
La locomoción bípeda en una persona consiste en una concatenación de
movimientos controlados en respuesta a un sistema retroalimentado en tiempo real, en el
caso de robots bípedos es algo similar.
1.3.3.4.- Locomoción pasiva
Esta caminata hace referencia a sistemas robóticos que caminan sin necesidad de
control ni de actuadores, es así que su movimiento se genera únicamente por acción de la
gravedad y por una pendiente en el piso. La principal ventaja de la caminata pasiva es que
no se requiere de energía externa para caminar, pero la desventaja está en la
direccionalidad limitada y la imposibilidad para subir pendientes (Santana, 2013, p. 3).
1.3.3.5.- Locomoción Activa:
La cual se relaciona directamente con robots que poseen un sistema de control
complejo, utilizan actuadores y sensores, así el robot desarrolla tareas complejas como
caminar en diferentes direcciones, subir escalones y permanecer en una pierna (Santana,
2013, p. 3).
Se utilizan criterios para lograr su estabilidad al momento de caminar, uno es el
punto de momento cero (ZMP), que se basa en mantener el centro de presión dentro del
contacto del pie (Guzmán, 2010, p. 9).
27. UNIVERSIDAD NACIONAL DE TRUJILLO
18
1.3.4.- Punto de Momento Cero ZMP:
El ZMP es el punto sobre la superficie de desplazamiento donde la suma de todos
los momentos, ocasionados por la gravedad y las fuerzas inerciales, es igual a cero.
Cuando el ZMP se ubica dentro del área convexa formada por los puntos de
contacto entre el pie de soporte y el suelo, el robot puede desarrollar una marcha
dinámicamente estable
i) Se comenzara el análisis de todas las fuerzas y Momentos que actúan sobre el Pie
de Apoyo.
Figura 1.3.4.1 Fuerzas y Momentos Aplicados sobre el pie de Apoyo
(Fuente: [8] Pág. 31)
Respecto a la figura 6.1 MA y FA Son el Momento y la Fuerza Resultante generado
por el cuerpo en movimiento del Robot Bípedo 10GDL y “M” y “R” son el Momento y la
fuerza de Reacción del Piso. Para que se encuentre en equilibrio Estático se tiene:
∑ 𝐹̅ = 0 (1.3.37)
𝑅̅ + 𝐹𝐴
̅̅̅ = 0 (1.3.38)
∑ 𝑀 𝑜
̅̅̅̅ = 0 (1.3.39)
(𝑂𝑃̅̅̅̅ 𝑥𝑅̅ + 𝑂𝐴̅̅̅̅ 𝑥𝐹𝐴
̅̅̅ + 𝑀̅ + 𝑀𝐴
̅̅̅̅ = 0 (1.3.40)
Se asume no deslizamiento entre el “PIE” de apoyo y el SUELO.
𝑅 𝑦
̅̅̅̅ = 𝐹𝑓𝑟𝑖𝑐𝑐𝑖𝑜𝑛𝑌 (1.3.41)
𝑅 𝑧
̅̅̅ = 𝐹𝑓𝑟𝑖𝑐𝑐𝑖𝑜𝑛𝑍 (1.3.42)
R
MA
X
Y
Z
O
28. UNIVERSIDAD NACIONAL DE TRUJILLO
19
Por lo que se tendría:
𝑅 𝑥
̅̅̅̅ + 𝐹𝐴𝑥
̅̅̅̅ = 0 (1.3.43)
Los Momentos se ha reducido al plano horizontal (ejes “Z” y “Y”). Se debe cumplir:
𝑀𝑧
̅̅̅̅ + 𝑀 𝑦
̅̅̅̅ = 0 (1.3.44)
Cambiando el Punto de referencia de O por A figura 6.1 despreciando la masa del pie se
Obtiene las siguientes ecuaciones:
𝑀𝐴
̅̅̅̅ = 0
𝐴𝑃̅̅̅̅ 𝑥𝑅̅ + 𝑀𝐴
̅̅̅̅ = 0
𝐴𝑃̅̅̅̅ 𝑥𝑅̅ = −𝑀𝐴
̅̅̅̅ (1.3.45)
El ZMP existe únicamente dentro del polígono de soporte. Cuando existe el
ZMP el robot únicamente se encuentra dinámicamente estable.
Cuando el momento generado por el cuerpo (𝑀𝐴) es demasiado grande, el punto “P”
puede encentrarse fuera del polígono de soporte figura 1.3.3.5, en este caso, la fuerza de
reacción del piso se encuentra en el borde del pie (no puede seguir al punto “P” y no puede
compensar el momento generado por el cuerpo en movimiento), si el ZMP no existe,
evidentemente el robot tiende a perder su estabilidad.
1.3.5.- Arquitectura Pierna Humana:
La arquitectura de las extremidades inferiores de un ser humano es muy complejo
(ver Figura 1.3.5.1). Su estructura ósea reagrupa 44 huesos, de los cuales el fémur, la tibia
y el peroné son la principal huesos. Aparte de la rótula, los huesos restantes conforman el
pie, que puede ser considerado como un corpus compuesto deformable.
Por tanto, cada miembro tiene un conjunto de tres cuerpos principales (muslo, pierna
y pie) unidos entre sí por articulaciones que tienen muchos grados de movilidad. De esta
manera, la articulación de la cadera tiene 3 grados de libertad (GDL). Las articulaciones de
los tobillos tienen cada uno de 2 GDL y la rodilla 1 GDL.
La musculatura de las extremidades inferiores de un ser humano se compone de 46
músculos esqueléticos. Los músculos que intervienen en el movimiento de propulsión son
los músculos más largos. En esto Así, nos damos cuenta de que muchos músculos
intervienen simultáneamente para ayudar en el movimiento de una articulación. Los
músculos hacen un esfuerzo de tracción y que siempre trabajan en pares conjuntivas con
otro músculo opuesto.
Podemos también ver músculos distintos en los que sólo actúan en una sola
articulación (por ejemplo, el músculo ilíaco en la cadera) y otros, que actúan sobre el cuerpo
enlaces separados, que están separados por dos articulaciones (por ejemplo, el músculo
recto femoral).
29. UNIVERSIDAD NACIONAL DE TRUJILLO
20
Figura 1.3.5.1. Estructura Ósea de Pierna humana
(Fuente: [8])
1.4.- Formulación del Problema:
¿Cuáles son las características del diseño de un prototipo de robot bípedo, con
planificación mediante generación de trayectorias, teniendo como criterio de estabilidad el
ZMP (Punto de Momento Cero), para la caminata en superficie plana?
1.5.- Justificación del Estudio:
Debido a la necesidad de tener una base de conocimiento científico, tecnológico de
robots bípedo la cual se pueda analizar el diseño, las trayectorias y estabilidad de la
caminata y pueda levantarse proyectos de mayor envergadura.
Aplicaciones que integren la mecatrónica y la robótica en brindar soluciones a
problemas en la medicina: como el desarrollo de prótesis, exoesqueletos en la rehabilitación
de lesiones medulares, etc.; en la Industria: en el desarrollo de este robot bípedo –
humanoides que permitan, entrar en espacios de alta peligrosidad la cual sería muy
perjudicial para los seres humanos; en el área militar, industria espacial, etc.
Pero fundamentalmente tendría un impacto investigativo-científico-tecnológico en la
Universidad Nacional de Trujillo, y en el país que permita abrir nuevos campos de estudio
en base a este proyecto de Diseño de robot Bípedo, como es:
30. UNIVERSIDAD NACIONAL DE TRUJILLO
21
- Permitir estudios de planificaciones de caminata en pisos no inclinados, caminata
en superficies irregulares.
- Desarrollos estudios de control, Control PID Neuronal, Fuzzy, algoritmos genéticos,
control adaptativo, control robusto, etc.
- Incluir a robots Bípedos – Humanoides en Sistemas de Visión, que permitan
conducir a través de ambientes con la presencia de obstáculos, muros, etc.
Y esto a la vez nos permita generar industrialización tecnológica, que puedan ayudar
a la sociedad, a un menor costo y rapidez.
1.6.- Hipótesis:
Aplicando la cinemática directa, mediante el método de Denavit Hartenberg, la
cinemática inversa utilizando un método numérico, el análisis dinámico usando el criterio
de Newton-Euler, así también desarrollando la planificación de trayectorias y evaluando la
estabilidad de la caminata mediante el ZMP, es posible obtener las características de diseño
del robot bípedo, así como su simulación al caminar sobre superficie plana.
1.7.- Objetivos:
1.7.1 Objetivo General:
Diseñar prototipo de robot Bípedo, con la planificación mediante generación de
trayectorias, con criterio de ZMP, para la caminata en superficie plana.
1.7.2 Objetivo Específico:
- Desarrollar la cinemática directa del robot bípedo.
- Desarrollar la cinemática inversa.
- Determinar los torques que se producen en cada articulación, mediante el desarrollo
dinámico.
- Determinar las dimensiones, forma y estructura del robot bípedo usando el software
Solidwork.
31. UNIVERSIDAD NACIONAL DE TRUJILLO
22
- Desarrollar la planificación de la caminata del robot bípedo a través de la generación
de trayectorias.
- Analizar la caminata del robot bípedo a través del criterio de estabilidad ZMP (Punto
de Momento Cero).
- Desarrollar la simulación de robot bípedo con su sistema de control en Simulink –
Matlab utilizando la librería de SimMechanics.
- Asimilar de tecnología en el tema de robots bípedos – humanoide
32. UNIVERSIDAD NACIONAL DE TRUJILLO
23
CAPITULO II: MÉTODO.
2.1.- Diseño de Investigación:
Investigación aplicada.
2.2.- Variables:
2.2.1 Variables independientes.
Trayectoria de caminata del robot bípedo.
Características del prototipo del robot bípedo.
Velocidad y aceleración de cada articulación.
Tiempo de paso.
22.2 Variables dependientes.
Ángulos generados por cada articulación al moverse.
Torques producidas al caminar en cada Articulación.
Trayectorias generadas real (Cinemática Inversa, directa)
Velocidad, aceleración de COG del robot bípedo.
Posición del ZMP.
2.3.-Población y Muestra.
2.3.1 Población:
Robots caminantes con 2 piernas, bípedos o humanoides basados en caminata
activa.
2.3.2 Muestra:
Robot bípedo, con planificación de caminata, evaluando su estabilidad con criterio
de ZMP, para la caminata en superficie plana en la Universidad Nacional de Trujillo –
Ingeniería Mecatrónica.
2.4 Técnicas e Instrumentos:
- Se hará uso del programa de Matlab2014b para el cálculo cinemático (directa e
inversa), como también para el cálculo dinámico del robot, planificación de
trayectoria y evaluación de estabilidad.
- Se utilizara la plataforma de Simulink en (Matlab), con la librería de SimMechanics,
para la simulación de la caminata.
- Se utilizará el software Solidworks para el diseño mecánico del robot bípedo.
33. UNIVERSIDAD NACIONAL DE TRUJILLO
24
CAPITULO III: DESARROLLO Y RESULTADOS.
En base a lo expuesto en el capítulo 1.3.5 arquitectura de la pierna humana, y a
las articulaciones que presentan; se define diseñar un robot Bípedo de 10GDL, 5 GDL por
cada pierna unidos mediante un eslabón (cadera).
3.1.- Cinemática directa del robot bípedo 10GDL:
Se trabaja con los valores de la cinemática directa para 2 puntos importantes del
robot bípedo 10GDL, estos puntos son la cadera y el pie flotante tomando como referencia
el pie de Apoyo. Se observa en la Figura 3.1 Siendo “PA” el pie de Apoyo, “C” la cadera y
“PF” pie flotante, es obvio que entre el pie de apoyo y el pie de flotante cambiaran de rol
dependiendo los pasos que se den en la marcha.
Primero se coloca los ejes en cada articulación del robot “X”, “Y”, “Z” para poder
determinar los parámetros de Denavit – Hartenberg.
34. UNIVERSIDAD NACIONAL DE TRUJILLO
25
Figura 3.1 Diagrama ubicación de ejes - Criterio Denavit Hartenberg.
(Fuente: Elaboración propia)
Leyenda:
Eje Rojo: “X” Eje Amarillo: “Y”
Eje Verde: “Z”
PA. Pie de
Apoyo
PF. (Pie Flotante)
C
X0i
Z0i
Z1i
Z2i
Z3i
Z4i
Z5i
X1i
X2i
X3i
iX2i
X4i
iX2i
X5i
iX2i
Xc
ZcX0d
X0d
Z0d
X1d
X2d
X3d
X4d
X5d
Z1d
Z2d
Z3d
Z4d
Z5d
35. UNIVERSIDAD NACIONAL DE TRUJILLO
26
En el gráfico 3.1 se observa que se colocó los ejes considerando la pierna derecha
como pie de apoyo, y la pierna izquierda la flotante, la cual se obtendrá la cinemática directa,
de la cadera con respecto al pie de apoyo y después la cinemática directa del pie flotante
con respecto a la cadera, desde esta forma se obtiene los puntos de interés para el cálculo
de la cinemática inversa.
3.1.1 Cinemática directa de la cadera con respecto al pie de apoyo:
Se utiliza el Método de D-H ya que tiene la virtud de posicionar siempre el eje de
rotación de cada articulación al eje “z” de cada uno de los marcos de referencia, se muestra
las medidas en la figura 3.2
Figura 3.2.- Diagrama dimensiones simbólicas de eslabones Robot 10GDL
(Fuente: Elaboración propia)
L5
L6
L7
L7
L6
L5
L4
L3
L2
L1 L1
L2
L3
L4
36. UNIVERSIDAD NACIONAL DE TRUJILLO
27
Se toma las siguientes dimensiones, teniendo en cuenta que las dimensión del
prototipo de robot bípedo10GDL no debe superar los 50cm de altura, las piezas deben
poder soportar las dimensiones de Servomotor DC MG996R (planos se muestran en
anexos C) y deben tener relación en las medidas de una pierna humana (aprox. Cadera –
rodilla: 52%, rodilla – pie: 48 % respecto a cadera -pie) fuente [11].
Se toma las siguientes dimensiones que se muestran la figura 3.2.
L1 = 45.2 mm L5 =47.3 mm
L2 =28.8 mm L6 =46.8 mm
L3 =47.4 mm L7 =17.4 mm
L4 = 64.0 mm
Los parámetros de Denavit – Hartenberg , se muestra en la tabla 3.1
Tabla 3.1. Parámetros de D–H del robot Bípedo 10 GDL (cadera respecto a pie de apoyo)
Articul. Param 𝜽𝒊 𝒅𝒊 𝒂𝒊 𝜶𝒊
1 q5d 0 L6 -pi/2
2 q4d – pi/6 0 L5 0
3 q3d + pi/3 0 L4 0
4 q2d – pi/6 0 L3 Pi/2
5 q1d 0 0 0
Las variables de qid : son las de rotación definidas en cada articulación, pierna
derecha, como se muestra en la figura 3.3
Tomando en cuenta las distancias máximas de los eslabones, las matrices de
transformación homogéneas y su relación directa entre eslabones se determina la matriz
final. Se observa que como es con respecto al pie de apoyo se usa 5GDL para así tomar
en cuenta el movimiento de una sola pierna del bípedo.
Una vez que se establece el sistema de coordenadas D-H para cada elemento se
procede a desarrollar una matriz de transformación homogénea que relaciona el sistema i-
ésimo con el sistema (i-1) ésimo dado la siguiente matriz, ya definida:
𝐴𝑖−1
𝑖 = [
𝐶𝜃𝑖 −𝐶𝑎𝑖 𝑆𝜃𝑖 𝑆𝑎𝑖 𝑆𝜃𝑖
𝑆𝜃𝑖 𝐶𝑎𝑖 𝐶𝜃𝑖 −𝑆𝑎𝑖 𝐶𝜃𝑖
0 𝑆𝑎𝑖 𝐶𝑎𝑖
𝐶𝜃𝑖
𝑆𝜃𝑖
𝑑𝑖
0 0 0 1
] (3.1)
37. UNIVERSIDAD NACIONAL DE TRUJILLO
28
Figura 3.3 Variables articulares de rotación referida a cada articulación.
(Fuente: Elaboración propia)
Reemplazando en la ecuación 3.1 con los datos de tabla 3.1, en el algoritmo realizado en
Matlab (Anexos B) se tiene las siguientes matrices de trasformación homogéneas:
𝑇4𝑑
3𝑑 = (
cos(q5d), 0 −sin(q5d) L6 ∗ cos(q5d)
sin(q5d)
0
0
0 cos(q5d), L6 ∗ sin(q5d)
−1 0 0
0 0 1
) (3.2)
38. UNIVERSIDAD NACIONAL DE TRUJILLO
29
𝑇3𝑑
2𝑑 = (
cos(q4d − pi/6), −sin(q4d − pi/6) 0 L5 ∗ cos(q4d − pi/6)
sin(q4d − pi/6)
0
0
cos(q4d − pi/6) 0 L5 ∗ sin(q4d − pi/6)
0 1 0
0 0 1
) (3.3)
𝑇2𝑑
1𝑑 = (
cos(q3d + pi/3) −sin(q3d + pi/3) 0 L4 ∗ cos(q3d + pi/3)
sin(q3d + pi/3)
0
0
cos(q3d + pi/3) 0 L4 ∗ sin(q3d + pi/3)
0 1 0
0 0 1
) (3.4)
𝑇1𝑑
0𝑑 = (
cos(q2d − pi/6) 0 sin(q2d − pi/6) L3 ∗ cos(q2d − pi/6)
sin(q2d − pi/6)
0
0
0 −cos(q2d − pi/6) L3 ∗ sin(q2d − pi/6)]
1 1 0
0 0 1
) (3.5)
𝑇0𝑑
0´𝑑 = (
cos(q1d) −sin(q1d) 0 0
sin(q1d)
0
0
cos(q1d) 0 0
0 1 0
0 0 1
) (3.6)
Se observa que en el análisis del robot falta todavía multiplicar por la matriz de
transformación homogénea 𝑇5𝑑
4𝑑 y la matriz 𝑇0𝑑′
𝐶 , las matrices se muestran a
continuación:
𝑇5𝑑
4𝑑 = (
1 0 0 𝐿7
0
0
0
1 0 0
0 1 0
0 0 1
) (3.7)
𝑇0𝑑′
𝐶 = (
1 0 0 𝐿2
0
0
0
0 −1 −𝐿1
1 0 0
0 0 1
) (3.8)
Corregido esto, se procede a multiplicar las matrices homogéneas para poder así
referenciar el sistema de coordenadas de la cadera al sistema de coordenadas del pie de
apoyo.
T5d
c = T5d
4d T4d
3d T3d
2d T2d
1d T1d
0d T0d
0′d T0´d
c (3.9)
39. UNIVERSIDAD NACIONAL DE TRUJILLO
30
Resolviendo la ecuación 3.9 se obtiene las siguiente matriz de transformación
homogénea (3.10) los valores de los elementos de esta matriz se muestran en el anexo A.
T5d
C = (
r11 r12 r13 r14
r21
r31
0
r22 r23 r24
r32 r33 r34
0 0 1
) (3.10)
Las pruebas de funcionamiento de la cinemática directa se muestran en la figura
3.4 realizado en Labview2014.
Figura 3.4 Prueba de la cinemática directa de la cadera respecto a pie de apoyo.
(Fuente: Elaboración propia)
40. UNIVERSIDAD NACIONAL DE TRUJILLO
31
3.1.2.- Cinemática directa del pie flotante con respecto a la cadera:
Los parámetros de Denavit – Hartenberg se muestra en la tabla 3.2. Las variables
q1i, q2i, q3i, q4i, q5i se muestran en el gráfico 3.3 y las longitudes en la figura 3.2
Tabla 3.2. Parámetros de D –H del Bípedo10 GDL del pie flotante respecto a la cadera.
Articul.
Param
𝜽𝒊 𝒅𝒊 𝒂𝒊 𝜶𝒊
1 q1i 0 L3 pi/2
2 q2i + pi/6 0 L4 0
3 q3i - pi/3 0 L5 0
4 q4i + pi/6 0 L6 -Pi/2
5 q5i 0 L7 0
Se observa que como es con respecto a la cadera se usan 5GDL para así tomar en
cuenta el movimiento de una sola pierna del bípedo. Es importante aclara que la articulación
1 empieza en el sistema de referencia 0, y termina en el punto PF.
Siguiendo la nomenclatura las matrices de transformación homogéneas
encontradas, tomando en cuenta la matriz 3.1, son las siguientes:
𝑇0𝑖
1𝑖 = (
cos(q1i) 0 sin(q1i) L3 ∗ cos(q1i)
sin(q1i)
0
0
0 −cos(q1i) L3 ∗ sin(q1i)
1 0 0
0 0 1
) (3.11)
𝑇1𝑖
2𝑖 = (
cos(q2i + pi/6) −sin(q2i + pi/6) 0 L4 ∗ cos(q2i + pi/6)
sin(q2i + pi/6)
0
0
cos(q2i + pi/6) 0 L4 ∗ sin(q2i + pi/6)
0 1 0
0 0 1
) (3.12)
𝑇2𝑖
3𝑖=(
cos(q3i − pi/3) −sin(q3i − pi/3) 0 L5 ∗ cos(q3i − pi/3)
sin(q3i − pi/3)
0
0
cos(q3i − pi/3) 0 L5 ∗ sin(q3i − pi/3)
0 1 0
0 0 1
) (3.13)
𝑇3𝑖
4𝑖=(
cos(q4i + pi/6) 0 −sin(q4i + pi/6) L6 ∗ cos(q4i + pi/6)
sin(q4i + pi/6)
0
0
0 cos(q4i + pi/6) L6 ∗ sin(q4i + pi/6)
1 1 0
0 0 1
) (3.14)
41. UNIVERSIDAD NACIONAL DE TRUJILLO
32
𝑇4𝑖
5𝑖=(
cos(q5i) −sin(q5i) 0 L7i ∗ cos(q5i)
sin(q5i)
0
0
cos(q5i) 0 L7i ∗ cos(q5i)
0 1 0
0 0 1
) (3.15)
Como se desea obtener la posición del pie flotante respecto a la cadera, falta
multiplicar por la matriz de la cadera al punto del eje 0. Denominado 𝑇𝑐
0𝑖
𝑇𝑐
0𝑖 = (
−1 0 0 −𝐿2𝑖
0
0
0
0 1 0
1 0 𝑙1𝑖
0 0 1
) (3.16)
Se procede a multiplicar las matrices de trasformación homogéneas, para así
referenciar el sistema de coordenadas del pie flotante al sistema de coordenadas de la
cadera:
TC
5i = TC
0i T0i
1i T1i
2i T2i
3i T3i
4i T4i
5i (3.17)
Una vez que se resuelve la ecuación 3.17 se obtiene la siguiente matriz 3.18,
cuyos elementos se muestran en Anexo A.2
𝑇𝐶
5𝑖 = (
𝑅11 𝑅12 𝑅13 𝑅14
𝑅21
𝑅31
0
𝑅22 𝑅23 𝑅24
𝑅32 𝑅33 𝑅34
0 0 1
) (3.18)
Teniendo en cuenta la matriz resultante que esta referenciado el sistema de
coordenadas del pie flotante al sistema de coordenadas de la cadera, es posible referenciar
el pie flotante con el sistema de coordenadas del pie de apoyo esto con solo multiplicar
las matrices de las ecuaciones 3.10 y 3.18.
𝑇5𝑑
5𝑖 = 𝑇5𝑑
𝐶 ∗ 𝑇𝐶
5𝑖 (3.19)
Los resultados de la ecuación 3.19 se tienen en la matriz siguiente, y los elementos de la
matriz se muestran en Anexos A.3,
𝑇5𝑑
5𝑖 = (
𝐴11 𝐴12 𝐴13 𝐴14
𝐴21
𝐴31
0
𝐴22 𝐴23 𝐴24
𝐴32 𝐴33 𝐴34
0 0 1
) (3.20)
Se muestra la figura 3.5 prueba la cinemática directa del pie flotante respecto a pie
de apoyo se realizó en LabView 2014
42. UNIVERSIDAD NACIONAL DE TRUJILLO
33
Figura 3.5 Prueba de la cinemática directa del pie flotante respecto al pie de apoyo.
(Fuente: Elaboración propia)
.
La ecuación 3.20 es fundamental para resolver la cinemática inversa para una
trayectoria dada en el pie flotante ya que este punto debe de estar referenciado al pie de
apoyo. Por lo que resultados obtenidos son los siguientes:
43. UNIVERSIDAD NACIONAL DE TRUJILLO
34
1.- La cinemática directa del robot bípedo.
2.- Se referencia el sistema de coordenadas de la cadera al sistema de coordenadas
del pie de apoyo.
3.- Se referencia el sistema de coordenadas del pie flotante al sistema de coordenadas
de la cadera.
4.- Se referencia el sistema de coordenadas del pie flotante al sistema de coordenadas
del pie de apoyo.
Tener en cuenta cuando se realiza el cambio de pie (Izquierda: Pie de Apoyo;
derecha: pie flotante) se modifican las siguiente matrices, por la variación de los sistemas
de referencia.
𝑇0𝑑′
𝐶 = (
1 0 0 𝐿2
0
0
0
0 −1 −𝐿1
1 0 0
0 0 1
) = 𝑇0𝑖′
𝐶 = (
1 0 0 𝐿2
0
0
0
0 −1 𝐿1
1 0 0
0 0 1
) (3.21)
𝑇𝑐
0𝑖 = (
−1 0 0 −𝐿2𝑖
0
0
0
0 1 0
1 0 𝑙1𝑖
0 0 1
) = 𝑇𝑐
0𝑑 = (
−1 0 0 −𝐿2𝑖
0
0
0
0 1 0
1 0 −𝑙1𝑖
0 0 1
) (3.22)
3.2.- Cinemática inversa del robot bípedo de 10GDL:
Según lo expresado en el capítulo 1.3.1.2 de Cinemática inversa se tiene:
3.2.1.- Paso a seguir en el algoritmo implementado:
Consiste en determinar la configuración que debe adoptar un robot para obtener una
posición y orientación determinada del efector final. La cual se puede representar mediante
la siguiente ecuación:
𝑞 = 𝑓−1
(𝑋)
(3.22)
Dónde: q= variables de articulación.
x= son las ecuaciones en coordenadas cartesianas.
44. UNIVERSIDAD NACIONAL DE TRUJILLO
35
(
f1 f4 f7 f10
f2
f3
0
f5 f8 f11
f6 f9 f12
0 0 1
) = (
𝑛 𝑥 𝑜 𝑥 𝑎 𝑥 𝑝 𝑥
𝑛 𝑦
𝑛 𝑧
0
𝑜 𝑦 𝑎 𝑦 𝑝 𝑦
𝑜𝑧 𝑎 𝑧 𝑝𝑧
0 0 1
) (3.23)
De la ecuación (3.23) la 2° matriz de la Igualdad representa la posición y orientación
deseada del efector final del robot. Debido a que las ecuaciones son altamente no lineales
‘f’, se opta por linealizar el sistema de ecuaciones, por lo que se plantea lo siguiente:
𝑓(𝑥̃) =
(
𝑓1
𝑓2
𝑓3
𝑓4
𝑓5
𝑓6
𝑓7
𝑓8
𝑓9
𝑓10
𝑓11
𝑓12)
f(𝑥) =
(
𝑛 𝑥
𝑛 𝑦
𝑛 𝑧
𝑜 𝑥
𝑜 𝑦
𝑜𝑧
𝑎 𝑥
𝑎 𝑦
𝑎 𝑧
𝑃𝑥
𝑃𝑦
𝑃𝑧 )
(3.24)
Dónde:
f(𝑥̂) : Función a linealizar
f(x) : Set point.
Para linealizar utilizamos la Serie de Taylor en cada función “f” para que cada función
dependa de una sola variable a solucionar, se muestra la ecuación 3.25, la linealización de
todas las funciones:
f(x) = f(𝑥̌) + ∑
𝜕f(𝑥̌)
𝜕𝜃𝑖
𝑗
𝑖=1 * (𝜃𝑖 − 𝜃̅𝑖) (3.25)
Se desarrolla las operaciones, la cual se tiene 5 grados de libertad (cadera al pie de
apoyo) o 10 grados de Libertad (pie flotante al pie de apoyo), y el número de funciones a
linealizar son 12, se puede hacer que las ecuaciones resultantes estén expresadas en la
siguiente forma:
Se calcula el error: e= f(x)- f(𝑥̂), mediante el método de IAE, se utiliza para que
cumpla cierta condición y salga del bucle.
45. UNIVERSIDAD NACIONAL DE TRUJILLO
36
(
𝑓(𝑥1) − 𝑓(𝑥1̅̅̅)
𝑓(𝑥2) − 𝑓(𝑥2̅̅̅)
𝑓(𝑥3) − 𝑓(𝑥3̅̅̅)
𝑓(𝑥4) − 𝑓(𝑥4̅̅̅)
𝑓(𝑥5) − 𝑓(𝑥5̅̅̅)
𝑓(𝑥6) − 𝑓(𝑥6̅̅̅)
𝑓(𝑥7) − 𝑓(𝑥7̅̅̅)
𝑓(𝑥8) − 𝑓(𝑥8̅̅̅)
𝑓(𝑥9) − 𝑓(𝑥9̅̅̅)
𝑓(𝑥10) − 𝑓(𝑥10̅̅̅̅̅)
𝑓(𝑥11) − 𝑓(𝑥11̅̅̅̅̅)
𝑓(𝑥12) − 𝑓(𝑥1̅̅̅2))
=
(
𝜕𝑓(𝑥1̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥1̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥1̅̅̅̅)
𝜕3
𝜕𝑓(𝑥1̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥1̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥2̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥3̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥4̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥5̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥6̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥7̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥8̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥9̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥10̅̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥11̅̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥12̅̅̅̅̅)
𝜕𝜃1
𝜕𝑓(𝑥2̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥2̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥2̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥2̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥3̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥3̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥3̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥3̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥4̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥4̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥4̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥4̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥5̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥5̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥5̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥5̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥6̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥6̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥6̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥6̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥7̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥7̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥7̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥7̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥8̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥8̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥8̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥8̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥9̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥9̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥9̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥9̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥10̅̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥10̅̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥1̅̅̅̅0)
𝜕𝜃4
𝜕𝑓(𝑥10̅̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥11̅̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥11̅̅̅̅̅)
𝜕𝜃3
𝜕𝑓(𝑥11̅̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥11̅̅̅̅̅)
𝜕𝜃5
𝜕𝑓(𝑥12̅̅̅̅̅)
𝜕𝜃2
𝜕𝑓(𝑥1̅̅̅̅2)
𝜕𝜃3
𝜕𝑓(𝑥12̅̅̅̅̅)
𝜕𝜃4
𝜕𝑓(𝑥12̅̅̅̅̅)
𝜕𝜃5 )
(
𝜃1 − 𝜃̅1
𝜃2 − 𝜃̅2
𝜃3 − 𝜃̅3
𝜃4 − 𝜃̅4
𝜃5 − 𝜃̅4)
(3.26)
De la ecuación (3.26) queda representado:
∇𝑓 = 𝐽 ∗ ∇θ (3.27)
J : Matriz Jacobiana.
De la ecuación (3.27) se tiene:
∇θ = 𝐽−1
∗ ∇𝑓 (3.28)
Para encontrar la inversa de la matriz jacobina al no ser cuadrático, se tiene aplicar
otros métodos. En este caso se evaluó (según Fuente [3])
El método “Damped Least Squares” o llamado también: Levenberg – Marquardt
Method se utiliza:
∇θ = 𝐽′
(𝐽 ∗ 𝐽′
+ 𝛾2
𝐼)−1
∗ 𝑒 (3.29)
Donde:
e=∇𝑓 (error)
J’: la transpuesta de una matriz Jacobiano.
I: matriz identidad
𝛾: Constante de convergencia.
46. UNIVERSIDAD NACIONAL DE TRUJILLO
37
Si la constante de Convergencia 𝛾 = 0, entonces estaríamos en el caso del método
de Moore Penrose, Sí 𝛾 > 0 mejor comportamiento del algoritmo pero convergencia más
lenta.
Para obtener los ángulos calculados simplemente:
𝜃 = ∇θ + 𝜃̅ (3.30)
Se resume el algoritmo en este diagrama de flujo:
Calculo de la posición y Orientación del
efector final, en el estado actual del robot.
f(x)
Cinemática
Inversa
Calculo del error entre la posición objetivo
y la posición actual. e=f(x) – f(𝑥̅)
Verificación del error y fin de la subrutina
si: ISE < k, donde k: constante
Calculo la matriz Jacobiana numéricamente
Calculo el Incremento de: ∇θ
∇θ = 𝐽′
(𝐽 ∗ 𝐽′
+ 𝛾2
𝐼)−1
∗ 𝑒
𝜃 = 𝜃̅ + ∇θ
Estandarización y restricciones de los
ángulos obtenidos.
47. UNIVERSIDAD NACIONAL DE TRUJILLO
38
3.2.2 – Cinemática inversa de la cadera del robot bípedo 10GDL.
Para calcular la cinemática inversa de la cadera al pie de apoyo utilizamos las
ecuaciones obtenidas por la cinemática directa de la ecuación 3.10.
(
ff1 ff4 ff7 ff10
ff2
ff3
0
ff5 ff8 ff11
ff6 ff9 f1f2
0 0 1
) = (
𝑛 𝑥 𝑜 𝑥 𝑎 𝑥 𝑝 𝑥
𝑛 𝑦
𝑛 𝑧
0
𝑜 𝑦 𝑎 𝑦 𝑝 𝑦
𝑜𝑧 𝑎 𝑧 𝑝𝑧
0 0 1
)
En el algoritmo implementado en Matlab se da valores iniciales a los ángulos
q5d=q4d=q3d=q2d=q1d=0.
Para hacer el cálculo más rápido se discretiza todas las ecuaciones dadas además
los Jacobianos, se trabaja numéricamente, mediante Incrementos finitos.
𝑑𝑓(𝑥)
𝑑𝑥
= lim
Δ𝑥→∞
𝑓(𝑥+Δ𝑥)−𝑓(𝑥)
Δ𝑥
(3.31)
De aquí se tiene, si se toma valores discretos:
Δ𝑓(𝑥)
Δ𝑥
=
𝑓(𝑥+Δ𝑥)−𝑓(𝑥)
Δ𝑥
(3.32)
Se tomara un incremento de dx=0.0001 para cada articulación:
dp5=q5d+dx;
dp4=q4d+dx;
dp3=q3d+dx; (3.33)
dp2=q2d+dx;
dp1=q1d+dx;
Se tomará en cuenta que realiza 1000 interacciones o en todo caso que salga del
bucle si s<0.0001 siendo “s” aplicando el “Criterio Integral del Error Absoluto.”
Por último se realiza una estandarización de Ángulos:
𝜃1𝑑 = 𝜃1𝑑 − 𝑟𝑜𝑢𝑛𝑑 (
𝜃1𝑑
2∗𝜋
) ∗ 2 ∗ 𝜋
𝜃2𝑑 = 𝜃2𝑑 − 𝑟𝑜𝑢𝑛𝑑 (
𝜃2𝑑
2∗𝜋
) ∗ 2 ∗ 𝜋
𝜃3𝑑 = 𝜃3𝑑 − 𝑟𝑜𝑢𝑛𝑑 (
𝜃3𝑑
2∗𝜋
) ∗ 2 ∗ 𝜋 (3.34)
𝜃4𝑑 = 𝜃4𝑑 − 𝑟𝑜𝑢𝑛𝑑 (
𝜃4𝑑
2∗𝜋
) ∗ 2 ∗ 𝜋
𝜃1𝑑 = 𝜃1𝑑 − 𝑟𝑜𝑢𝑛𝑑 (
𝜃1𝑑
2∗𝜋
) ∗ 2 ∗ 𝜋
El la figura 3.6 se muestra la prueba de la cinemática inversa de cadera respecto
al pie de apoyo realizado en Labview2014.
48. UNIVERSIDAD NACIONAL DE TRUJILLO
39
Figura 3.6 Prueba de la Cinemática Inversa de la cadera respecto al pie de apoyo.
(Fuente: Elaboración propia)
El algoritmo completo se implementó en Matlab, (se encuentra en anexos B), se
prueba con las trayectorias de movimiento propuestas más adelante, los resultados se
presentan a continuación, las variables articulares hacen referencia a la figura 3.3:
49. UNIVERSIDAD NACIONAL DE TRUJILLO
40
Figura 3.7.- Posición angular de q5d-plena marcha.
(Fuente: Elaboración propia)
Figura 3.8.- Posición angular de q4d-plena marcha.
(Fuente: Elaboración propia)
0 10 20 30 40 50 60 70 80 90 100
-0.005
0
0.005
0.01
0.015
0.02
0.025
0.03
0.035
0.04
0.045
Pierna apoyo q5d - Plena Marcha.
Número de divisiones tiempo (s)
q5d(radianes)
0 10 20 30 40 50 60 70 80 90 100
-0.14
-0.12
-0.1
-0.08
-0.06
-0.04
-0.02
0
Pierna de apoyo q4d- Plena Marcha.
Número de divisiones tiempo(s)
q4d(radianes)
50. UNIVERSIDAD NACIONAL DE TRUJILLO
41
Figura 3.9.- Posición angular de q3d-plena marcha.
(Fuente: Elaboración propia)
Figura 3.10.- Posición angular de q2d-plena marcha.
(Fuente: Elaboración propia)
0 10 20 30 40 50 60 70 80 90 100
-0.06
-0.05
-0.04
-0.03
-0.02
-0.01
0
0.01
0.02
0.03
Pierna de apoyo q3d - Plena Marcha.
Número de divisiones tiempo(s)
q3d(radianes)
0 10 20 30 40 50 60 70 80 90 100
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
Pierna de apoyo q2d- Plena Marcha.
Número de divisiones tiempo (s)
q2d(radianes)
51. UNIVERSIDAD NACIONAL DE TRUJILLO
42
Figura 3.11.- Posición angular de q1d-plena marcha.
(Fuente: Elaboración propia)
3.2.3.- Cinemática inversa del pie flotante del robot bípedo 10GDL.
Para encontrar la cinemática inversa del pie flotante se realizó un procedimiento
similar al que se describió anteriormente, se tomas las ecuaciones de la cinemática directa
del pie flotante respecto al pie de apoyo (ecuación que se muestra anexo en al capítulo
A3.).
(
ff1 ff4 ff7 ff10
ff2
ff3
0
ff5 ff8 ff11
ff6 ff9 f1f2
0 0 1
) = (
𝑛 𝑥 𝑜 𝑥 𝑎 𝑥 𝑝 𝑥
𝑛 𝑦
𝑛 𝑧
0
𝑜 𝑦 𝑎 𝑦 𝑝 𝑦
𝑜𝑧 𝑎 𝑧 𝑝𝑧
0 0 1
)
Después de esto se resuelve el mismo procedimiento presentado desde la ecuación
3.22 hasta la 3.34. El algoritmo se desarrolló en Matlab (se muestra en anexos B) los
resultados se muestran a continuación referidos a q1i, q2i, q3i, q4i, q5i, estas variables
articulares se presentan en la figura 3.3
0 10 20 30 40 50 60 70 80 90 100
-0.01
-0.005
0
0.005
0.01
0.015
0.02
0.025
0.03
0.035
0.04
Pierna de apoyo q1d- Plena Marcha.
Número de divisiones tiempo (s)
q1d(radianes)
52. UNIVERSIDAD NACIONAL DE TRUJILLO
43
Figura 3.12.- Posición angular de q1i-plena marcha.
(Fuente: Elaboración propia)
Figura 3.13.- Posición angular de q2i-plena marcha.
(Fuente: Elaboración propia)
0 10 20 30 40 50 60 70 80 90 100
-0.06
-0.05
-0.04
-0.03
-0.02
-0.01
0
0.01
Pierna flotante q1i- Plena Marcha.
Número de divisiones tiempo (s)
q1i(radianes)
0 10 20 30 40 50 60 70 80 90 100
-0.25
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
Pierna flotante q2i - Plena Marcha.
Número de divisiones tiempo (s)
q2i(radianes)
53. UNIVERSIDAD NACIONAL DE TRUJILLO
44
Figura 3.14.- Posición angular de q3i-plena marcha.
(Fuente: Elaboración propia)
Figura 3.15.- Posición angular de q4i-plena marcha.
(Fuente: Elaboración propia)
0 10 20 30 40 50 60 70 80 90 100
-20
-15
-10
-5
0
5
10
Pierna flotante q3i- Plena Marcha.
Número de divisiones tiempo (s)
q3i(radianes)
0 10 20 30 40 50 60 70 80 90 100
-20
-15
-10
-5
0
5
10
15
Pierna flotante q4i- Plena Marcha.
Número de divisiones tiempo (s)
q4i(radianes)
54. UNIVERSIDAD NACIONAL DE TRUJILLO
45
Figura 3.16.- Posición angular de q5i-plena marcha.
(Fuente: Elaboración propia)
3.3.- Análisis Dinámico:
Con el expuesto en el capítulo 1.3.2. Y las ecuaciones deducidas aplicando el criterio
de Newton Euler, para encontrar los torques producidos en cada articulación, se tiene lo
siguiente:
3.3.1.- Algoritmo Computacional:
En resumen del anterior expuesto las ecuaciones de Newton – Euler la cual se tiene
un conjunto de ecuaciones recursivas hacia adelante y hacia atrás para obtener un
algoritmo computacional.
Se tiene ( 𝑅𝑖
𝑖−1
)−1
= 𝑅𝑖−1 = ( 𝑅𝑖
𝑖−1
) 𝑇𝑖
En lugar de calcular 𝑤𝑖, 𝑤̇ 𝑖, 𝑣̇ 𝑖, 𝑎̅𝑖, 𝑃∗
𝑖, 𝑠̅𝑖, 𝐹𝑖, 𝑁𝑖, 𝑓𝑖, 𝑛𝑖 𝑦 𝜏𝑖 que se refieren al sistema
coordenadas de la base.
Calculamos 𝑅 𝑜
𝑖
𝑤𝑖, 𝑅 𝑜
𝑖
𝑤̇ 𝑖, 𝑅 𝑜
𝑖
𝑣̇ 𝑖, 𝑅 𝑜
𝑖
𝑎̅ 𝑖, 𝑅 𝑜
𝑖
𝑃∗
𝑖, 𝑅 𝑜
𝑖
𝑠̅𝑖,
𝑅 𝑜
𝑖
𝐹𝑖, 𝑅 𝑜
𝑖
𝑁𝑖, 𝑅 𝑜
𝑖
𝑓𝑖, 𝑅 𝑜
𝑖
𝑛𝑖 𝑦 𝑅 𝑜
𝑖
𝜏𝑖 que se refieren a su propio sistema de
coordenadas del elemento (xi, yi, zi). Por lo tanto las ecuaciones recursivas de N-E
quedan expresadas en la siguiente tabla 3.1.
0 10 20 30 40 50 60 70 80 90 100
0
0.5
1
1.5
2
2.5
3
3.5
Pierna flotante 5i- Plena Marcha.
Número de divisiones tiempo (s)
q5i(radianes)
55. UNIVERSIDAD NACIONAL DE TRUJILLO
46
Ecuaciones hacia adelante: i=1,2,3…..,n
𝑅0 𝑤𝑖 = {
𝑅𝑖−1( 𝑅0 𝑤𝑖−1 + 𝑧0 𝑞𝑖̇ ) 𝑆𝑖 𝑒𝑙 𝑒𝑙𝑒𝑚𝑒𝑛𝑡𝑜 𝑖 𝑒𝑠 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛𝑎𝑙𝑖−1𝑖
𝑅𝑖−1( 𝑅0 𝑤𝑖−1)𝑖−1𝑖
𝑆𝑖 𝑒𝑙 𝑒𝑙𝑒𝑚𝑒𝑛𝑡𝑜 𝑖 𝑒𝑠 𝑡𝑟𝑎𝑙𝑎𝑐𝑖𝑜𝑛𝑎𝑙
𝑖
(3.1.1)
𝑅0 𝑤̇ 𝑖 = {
𝑅𝑖−1( 𝑅0 𝑤̇ 𝑖−1 + 𝑧0 𝑞𝑖̈ + ( 𝑅0 𝑤𝑖−1)𝑥 (𝑧0 𝑞𝑖̇ )𝑖−1
) 𝑆𝑖 𝑒𝑙 𝑒𝑙𝑒𝑚𝑒𝑛𝑡𝑜 𝑖 𝑒𝑠 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛𝑎𝑙𝑖−1𝑖
𝑅𝑖−1( 𝑅0 𝑤𝑖−1̇ )𝑖𝑖
𝑆𝑖 𝑒𝑙 𝑒𝑙𝑒𝑚𝑒𝑛𝑡𝑜 𝑖 𝑒𝑠 𝑡𝑟𝑎𝑙𝑎𝑐𝑖𝑜𝑛𝑎𝑙
𝑖
(3.28)
𝑅0 𝑣̇ 𝑖 = {( 𝑅0 𝑤𝑖̇𝑖
)𝑥( 𝑅0 𝑝𝑖
∗𝑖
) + ( 𝑅0 𝑤𝑖
𝑖
)𝑥[( 𝑅0 𝑤𝑖
𝑖
)𝑥( 𝑅0 𝑝𝑖
∗𝑖
)] + 𝑅𝑖−1( 𝑅0 𝑣𝑖−1)̇𝑖−1𝑖𝑖
(3.1.2)
Si el Elemento i es rotacional.
𝑅0 𝑎𝑖̅ = ( 𝑅0 𝑤̇ 𝑖)𝑥( 𝑅0 𝑠̅𝑖
𝑖
) + ( 𝑅0 𝑤𝑖
𝑖
)𝑥[( 𝑅0 𝑤𝑖
𝑖
)𝑥( 𝑅0 𝑠̅𝑖
𝑖
)]𝑖
+𝑖
𝑅𝑖
0 𝑣𝑖̇ (3.1.3)
Ecuaciones hacia atrás i = n, n-1,n-2,……,1
𝑅𝑖
0 𝑓𝑖 = 𝑅𝑖
𝑖+1( 𝑅𝑖+1
0 𝑓𝑖+1) + 𝑚𝑖 𝑅𝑖
0 𝑎̅𝑖 (3.1.4)
𝑅𝑖
0 𝑛𝑖 = 𝑅𝑖
𝑖+1[ 𝑅𝑖+1
0 𝑛𝑖+1 + ( 𝑅𝑖+1
0 𝑝𝑖
∗
)𝑥( 𝑅𝑖+1
0 𝑓𝑖+1)] + ( 𝑅𝑖
0 𝑝𝑖
∗
+ 𝑅𝑖
0 𝑠̅𝑖)𝑥( 𝑅𝑖
0 𝐹𝑖) +
( 𝑅𝑖
0 𝐼𝑖 𝑅0
𝑖)( 𝑅𝑖
0 𝑤̇ 𝑖) + ( 𝑅𝑖
0 𝑤𝑖)𝑥[( 𝑅𝑖
0 𝐼𝑖 𝑅0
𝑖)( 𝑅𝑖
0 𝑤𝑖)] (3.32)
𝜏𝑖 = ( 𝑅𝑖
0 𝑛𝑖)
𝑇
( 𝑅𝑖
𝑖−1 𝑧0) + 𝑏𝑖 𝑞̇ 𝑖 (3.1.5)
Tabla 3.1 ecuaciones recursivas de Newton – Euler.
(Fuente: [2] Pág. 112)
Se aplicó este método para el cálculo de la dinámica del robot bípedo 10 GDL,
para un mejor estudio, se decidió dividir el análisis en 2 partes, cadera respecto al pie de
apoyo, y pie flotante respecto a Cadera.
Dónde z0 = (0,0,1) 𝑇
𝑦 "𝑏𝑖" es el coeficiente de amortiguamiento viscoso para la
articulación i. Las condiciones iniciales usuales son:
𝑤0 = 𝑤̇ 0 = 𝑣0 = 0
𝑣̇0 = (𝑔 𝑥, 𝑔 𝑦, 𝑔 𝑧)′ Para incluir la aceleración de la gravedad.
Donde g=9.81 m/s^2.
56. UNIVERSIDAD NACIONAL DE TRUJILLO
47
3.3.2 Dinámica Newton Euler del robot bípedo 10 GDL:
3.3.2.1 Dinámica del pie flotante respecto a la cadera:
Las ecuaciones de la tabla 3.1 se programó en Matlab para el cálculo de los torques
de los 5 GDL del robot bípedo (pie flotante a la cadera). Los ángulos calculados de la
cinemática inversa sirven para entrada a la función para el cálculo de las torques.
Las velocidades y aceleraciones angulares, se estima en función al tiempo estimado
a realizar la trayectoria, la cual se dividió en “n=100” puntos; es decir la velocidad se toma
como el cambió posición de un punto a otro, e un tiempo determinado y la aceleración el
cambio de velocidad en un tiempo determinado, como se muestra en las ecuaciones 3.1.6
y 3.1.7.
𝑤𝑖𝑛 =
(𝜃 𝑖𝑛−𝜗 𝑖𝑛−1)
𝑇
(3.1.6)
𝜃̇𝑖𝑛 =
(𝜃̇ 𝑖𝑛−𝜃̇ 𝑖𝑛−1)
𝑇
(3.1.7)
Dónde:
n : Parte que se divide la trayectoria.
𝜃𝑖𝑛 : Posición de la articulación en función de “n”.
𝑤𝑖𝑛 : Velocidad la articulación en función de “n”.
𝜃̇ 𝑖𝑛 : Aceleración la articulación en función de “n”.
El algoritmo empieza con definir los parámetros de Denavit-Hartenberg del robot
(pierna flotante) en función a la figura 3.1, luego se definen las masas de los elementos
(eslabones + masa del servomotor) los datos se presentan en anexos C.
m1=0.0876; elemento L3 (Pierna Flotante)
m2=0.1045; elemento L4
m3=0.0876; elemento L5
m4=0.0876; elemento L6
m5=0.1039; elemento L7
Luego se Incluyen las matrices de Inercia, referidos a su CG de cada eslabón, se
define las matrices de rotación y se sigue el algoritmo con las ecuaciones descritas en la
tabla 3.1, la cual para el cálculo se define las siguientes variables:
Siendo:
q1i, q2i, q3i, q4i, q5i : Posiciones articulares.
qd1i, qd2i, qd3i, qd4i, qd5i : Velocidades articulares.
qdd1i, qdd2i, qdd3i, qdd4i, qdd5i : Aceleraciones articulares.
L3, L4, L 5, L6, L7 : Longitudes de los eslabones.
m1, m2, m3, m4, m5 : Masa de cada eslabón
g= : aceleración de la gravedad.
Mext : Mexterna aplicado el pie flotante
: Momento de inercia aplicado en el pie
flotante externo
57. UNIVERSIDAD NACIONAL DE TRUJILLO
48
I1i , I2i , I3i, I4i, I5i : Momentos de inercia de cada eslabón referido a su mismo sistema
de referencia.
b1, b2, b3, b4, b5: Coeficiente de rozamiento Viscoso de los eslabones del robot.
Las posiciones angulares, se definen en la figura 3.3.1, los resultados de las
ecuaciones encontradas para los torques se muestran en anexos A.4.
Figura 3.3.1 Muestran las variables articulares y los torques
producidos en la pierna flotante.
Al remplazar todos las variables descritas anteriormente con los valores numéricos
respectivos (dimensiones de los eslabones, masas (incluyen masa del eslabón y masa del
Servomotor), posiciones articulares, velocidades articulares, aceleraciones articulares,
58. UNIVERSIDAD NACIONAL DE TRUJILLO
49
momentos de Inercia) se obtienen las los torques en N*m de cada articulación del robot
bípedo del pie Flotante respecto a la cadera.
Tener en cuenta: Tau1 está referida a la articulación qi1, Tau2 referida al q2i, Tau3
referida a la articulación q3i, Tau4 está referida a la articulación q4i, Tau5 está referida a la
articulación q5i.
Parametro de Denavit-Hartenberg del robot:
L3=0.0474; Longitudes en Metros.
L4=0.064;
L5=0.0473;
L6=0.0468;
L7=0.0174;
theta=[qi(1); qi(2)+pi/6; qi(3)-pi/3; qi(4)+pi/6; qi(5) ];
d=[0; 0; 0; 0; 0];
a=[L3; L4; L5; L6; L7 ];
alpha=[pi/2; 0; 0;-pi/2; 0];
DH=[theta d a alpha];
Se asumen el coeficiente de rozamiento viscoso de cada articulación:
b1=0; Coeficiente viscoso elemento1
b2=0; Coeficiente viscoso elemento2
b3=0; Coeficiente viscoso elemento3
b4=0; Coeficiente viscoso elemento4
b5=0; Coeficiente viscoso elemento5
El algoritmo se implementó en Matlab2013 se muestra en el anexo B, las
ecuaciones de cada torques se muestran en anexos A, y se tiene los siguientes
resultados:
59. UNIVERSIDAD NACIONAL DE TRUJILLO
50
Figura 3.3.2 Torques calculados para los 5GDL del pie flotante a la cadera.
(Fuente: Elaboración propia)
Figura 3.3.3 Velocidades Articulares del Pie flotante a la Cadera.
(Fuente: Elaboración propia)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
-0.15
-0.1
-0.05
0
0.05
0.1
ROBOT BIPEDO: GRÁFICO DE TORQUES PIE FLOTANTE - CADERA.
N*m
Tiempo
Torque 1i
Torque 2i
Torque 3i
Torque 4i
Torque 5i
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
tiempo
rad/s
ROBOT BÍPEDO: GRÁFICO DE VELOCIDADES PIE FLOTANTE-CADERA
Velocidad 1i
Velocidad 2i
Velocidad 3i
Velocidad 4i
Velocidad 5i
60. UNIVERSIDAD NACIONAL DE TRUJILLO
51
Figura 3.3.4 Aceleraciones Articulares del Pie flotante a la Cadera.
(Fuente: Elaboración propia)
Figura 3.3.5 Gráfico de Potencia en cada articulación en la
Pierna Flotante (Fuente: Elaboración propia)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
-2.5
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
Tiempo
rad/s2
ROBOT BÍPEDO: GRÁFICO DE ACELERACIONES PE FLOTANTE-CADERA
Aceleración 1i
Aceleración 2i
Aceleración 3i
Aceleración 4i
Aceleración 5i
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
-0.15
-0.1
-0.05
0
0.05
0.1
tiempo
Watts
ROBOT BÍPEDO: GRÁFICO DE POTENCIAS DEL PIE FLOTANTE
Potencia 1i
Potencia 2i
Potencia 3i
Potencia 4i
Potencia 5i
61. UNIVERSIDAD NACIONAL DE TRUJILLO
52
3.3.2.2 Dinámica de la cadera respecto al pie de apoyo:
Las ecuaciones de la tabla 3.1 se programó en Matlab para el cálculo de los torques
de las 5 GDL del robot bípedo (de la cadera respecto al pie de apoyo). Los ángulos
calculados de la cinemática inversa sirven para entrada a la función para el cálculo de las
torques, como las velocidades articulares, aceleraciones articulares.
Siendo:
q5d, q4d, q3d, q2d, q1d : Posiciones articulares.
qd1d, qd2d, qd3d, qd4d, qd5d : Velocidades articulares.
qdd1d, qdd2d, qdd3d, qdd4d, qdd5d : Aceleraciones articulares.
L3, L4, L5, L6, L7 : Longitudes de los eslabones.
m1, m2, m3, m4, m5 : Masa de cada eslabón
g : Aceleración de la gravedad.
Mext : Mexterna aplicado el pie flotante
Iext : Momento de inercia aplicado en el pie
flotante externo.
I1d, I2d, I3d, I4d, I5d : Momentos de inercia de cada eslabón
referido a su mismo sistema de referencia.
b1, b2, b3, b4, b5 : Coeficiente de rozamiento Viscoso de los
eslabones del robot
Los parámetros de los torques de las Pierna de apoyo se presentan en la figura 3.3.6 con
las variables articulares respectivas.
62. UNIVERSIDAD NACIONAL DE TRUJILLO
53
Figura 3.3.6 Muestran las variables articulares y los torques
Producidos en la pierna flotante.
Las ecuaciones que se obtiene son las siguientes que relacionan las torques con
cada articulación. Siendo:
tau1 = está referida a la articulación “q5d”
tau2 = está referida a la articulación “q4d”
tau3 = está referida a la articulación “q3d”
tau4 = está referida a la articulación “q2d”
tau5 = está referida a la articulación “q1d”
63. UNIVERSIDAD NACIONAL DE TRUJILLO
54
Las ecuaciones de los torques de la pierna de apoyo se presentan en anexos A.5
Al remplazar todos las variables descritas anteriormente con los valores numéricos
respectivos (dimensiones de los eslabones, masas (incluyen masa del eslabón y masa del
Servomotor), posiciones articulares, velocidades articulares (figura 3.3.8), aceleraciones
articulares (figura 3.3.9), los momentos de inercia se obtienen las los torques en N*m (figura
3.3.7) de cada articulación del robot bípedo del pie Flotante respecto a la cadera.
Parámetro de Denavit-Hartenberg del robot
L6=0.0468; Longitudes en Metros.
L5=0.0473;
L4=0.064;
L3=0.0474;
theta=[qd(1); qd(2)-pi/6; qd(3)+pi/3; qd(4)-pi/6; qd(5) ];
d=[0; 0; 0; 0; 0];
a=[L6; L5; L4; L3; 0 ];
alpha=[-pi/2; 0; 0; pi/2; 0];
DH=[theta d a alpha];
Masa de cada elemento en Kg.
m1=(0.1039+0.0876); elemento L6 (izquierda)
m2=0.0876; elemento L5 (izquierda)
m3=0.1045; elemento L4 (izquierda)
m4=0.0876; elemento L3 (izquierda)
m5 elemento L2 (izquierda)(Se lo va Incluir en la Masa Externa)
Se asumen los coeficientes de rozamiento viscoso de cada articulación.
b1=0; Coeficiente viscoso elemento1
b2=0; Coeficiente viscoso elemento2
b3=0; Coeficiente viscoso elemento3
b4=0; Coeficiente viscoso elemento4
b5=0; Coeficiente viscoso elemento5
El algoritmo se implementó en Matlab y se muestra en anexos B, se obtiene los
Siguientes resultados:
64. UNIVERSIDAD NACIONAL DE TRUJILLO
55
Figura 3.3.7 Torques calculados para los 5GDL de la cadera respecto al pie de apoyo.
(Fuente: Elaboración propia)
Figura 3.3.8 Velocidades calculados para los 5GDL de la cadera respecto al pie de apoyo.
(Fuente: Elaboración propia)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0.25
ROBOT BIPEDO: GRÁFICO DE TORQUES CADERA A PIE DE APOYO
N*m
Tiempo
Torque 5d
Torque 4d
Torque 3d
Torque 2d
Torque 1d
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
-0.25
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
tiempo
rad/s
ROBOT BÍPEDO: GRÁFICO DE VELOCIDADES CADERA A PIE DE APOYO
Velocidad 5d
Velocidad 4d
Velocidad 3d
Velocidad 2d
Velocidad 1d
65. UNIVERSIDAD NACIONAL DE TRUJILLO
56
Figura 3.3.9 Velocidades calculados para los 5GDL de la cadera respecto al pie de apoyo.
(Fuente: Elaboración propia)
Figura 3.3.10 Gráfico de potencia en cada articulación en la
Pierna de Apoyo (Fuente: Elaboración propia)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
tiempo
rad/s2
ROBOT BÍPEDO: GRÁFICO DE ACELERACIONES CADERA A PIE DE APOYO
Aceleración 5d
Aceleración 4d
Aceleración 3d
Aceleración 2d
Aceleración 1d
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
-0.05
-0.04
-0.03
-0.02
-0.01
0
0.01
0.02
0.03
tiempo
Watts
ROBOT BÍPEDO: GRÁFICO DE POTENCIAS CADERA A PIE DE APOYO
Potencia 5d
Potencia 4d
Potencia 3d
Potencia 2d
Potencia 1d
66. UNIVERSIDAD NACIONAL DE TRUJILLO
57
3.4.- Arquitectura de Robot Bípedo 10GDL.
3.4.1 Modelo CAD 3D del Robot Bípedo 10GDL.
El Robot Bípedo 10GDL, está constituido por 2 piernas, 5 eslabones unidas entre sí
mediante un eslabón central al que, en este tesis se llamara cadera, sus 10 eslabones se
conectan entre sí a través de juntas rotaciones actuadas por servomotores. En la figura
3.4.1 se muestra el modelo CAD 3D del bípedo, en la configuración espacial que adoptan
sus servomotores en posición neutral.
Figura 3.4.1 Modelo CAD 3D del bípedo 10GDL.
(Fuente: Elaboración propia)
La cuál de la figura 3.2. Nos muestran las dimensiones dadas, de cada eslabón.
Los planos de robot Bípedo, así como las dimensiones, masa de cada pieza, masa y
dimensiones de los servomotores se presentara en Anexos C.
67. UNIVERSIDAD NACIONAL DE TRUJILLO
58
3.4.2 Modelo CAD 3D Simplificado Robot Bípedo 10GDL.
También se muestra un modelo simplificado, que se mostró en el análisis
cinemático, en que se apreciará que las juntas son representadas por medio de cilindros
cuyos ejes coinciden con el de los servomotores y se unen a través de barras. En la figura
3.4.2
Figura 3.4.2 Modelo CAD 3d simplificado del robot bípedo 10GDL.
(Fuente: Elaboración propia.)
68. UNIVERSIDAD NACIONAL DE TRUJILLO
59
3.5.- Planificación de la caminata de robot bípedo 10GDL.
3.5.1 Control cinemático:
Recibe como entrada los datos procedentes del programa del robot escrito por el
usuario (punto de destino, precisión, tipo de trayectoria, etc.) apoyándose en el modelo
cinemático del robot, establece las trayectorias para cada articulación como función del
tiempo.
3.5.2.- Trayectorias robot bípedo 10GDL:
La caminata bípeda es un desafío para la robótica, por lo que “planificación de
caminata bípeda” tiene como objetivo generar las secuencias y movimientos para lograr la
caminata bípedo es sus distintas fases. Para luego analizar la estabilidad del robot durante
su caminata.
En esta tesis se analiza la planificación de la caminata bípeda tanto en los planos
sagitales (lateral) y frontal (como se describe en el capítulo 1.3.3.2), además se desarrollará
un programa en Matlab.
3.5.3.- Interpolación de trayectorias:
Para asegurar que la trayectoria presente continuidad en velocidad y aceleración,
puede recurrir a utilizar polinomios de grado 3, Este conjunto de polinomios concatenados,
se denomina “splines cúbico”. [Fuente [2], pag. 178]
Figura 3.5.1 Semiciclo de caminata en el plano sagital.
Para el caso el robot bípedo, en la figura 3.5.1 se esquematiza el estado inicial y
final de un paso de caminata, dónde PA es pierna de apoyo, PF1 es pierna flotante en
estado inicial; PF2 pierna flotante en estado final, C1 es cadera en estado inicial, C2
cadera en el estado final.
La Ecuación spline cúbica se muestra a continuación:
𝑥(𝑡) = 𝑎 + 𝑏𝑡 + 𝑐𝑡2
+ 𝑑𝑡3
(3.5.1)
Donde a,b: constantes de la ecuación: t𝜖 [0 𝑇]