Professional Documents
Culture Documents
TESIS
Trujillo, Perú
2017
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
«No hay azar, destino, ni suerte que pueda con la firme resolución de un alma decidida.»
— Ella Wheeler Wilcox.
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
DEDICATORIA
II
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
AGRADECIMIENTO.
III
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
IV
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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
VI
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
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.
VII
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
VIII
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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,.
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
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.
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Cinemática Directa
Valor de las Posición y orientación
coordenadas articulares Cinemática inversa del Extremo del Robot.
(q1, q2,…, qn) (X, y, z, 𝛼, 𝛽, 𝛾)
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
𝑛𝑥 𝑠𝑥 𝑎𝑥 𝑝𝑥
𝑛𝑦 𝑠𝑦 𝑎𝑦 𝑝𝑦
T= [ 𝑛 𝑠𝑧 𝑎𝑧 𝑝𝑧 ]
𝑧
0 0 0 1
𝑖−1
𝐴𝑖 = 𝑇(𝑧, 𝜃𝑖 )𝑇(0,0, 𝑑𝑖 )(𝑇𝑎𝑖 , 0,0) 𝑇(𝑥, 𝛼𝑖 ) (1.3.3)
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
𝜃𝑖 ∶ 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
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]
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
𝑟 = 𝑟∗ + ℎ (1.3.11)
𝑑𝑟 𝑑𝑟 ∗ 𝑑ℎ
𝑑𝑡
= 𝑑𝑡
+ 𝑑𝑡 = 𝑣 ∗ + 𝑣ℎ (1.3.12)
𝑑𝑟 ∗ 𝑑ℎ 𝑑∗ 𝑟∗ 𝑑ℎ
𝑣= 𝑑𝑡
+ 𝑑𝑡 = ( 𝑑𝑡
+ 𝑤 𝑥 𝑟 ∗ ) + 𝑑𝑡 (1.3.13)
10
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
𝑑𝑣 𝑑2 𝑟∗ 𝑑2 ℎ
𝑎= 𝑑𝑡
= 𝑑𝑡 2
+ 𝑑𝑡 2 = 𝑎∗ + 𝑎ℎ (1.3.14)
𝑑2 𝑟∗ 𝑑∗ 𝑟∗ 𝑑𝑤 𝑑2 ℎ
𝑎= 𝑑𝑡 2
+ 2𝑤𝑥 𝑑𝑡
+ 𝑤𝑥(𝑤𝑥𝑟) + 𝑑𝑡
𝑥𝑟 ∗ + 𝑑𝑡 2 (1.3.15)
𝑑 ∗ 𝑃𝑖 ∗
𝑣𝑖 = 𝑑𝑡
+ 𝑤𝑖−1 𝑥𝑃𝑖 ∗ + 𝑣𝑖−1 (1.3.16)
𝑤𝑖 =𝑤𝑖−1 + 𝑤𝑖 ∗
11
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
La aceleración angular del sistema de referencia i (xi, yi, zi), respecto al sistema de
referencia (𝑥𝑖−1 , 𝑦𝑖−1 , 𝑧𝑖−1 )
𝑑𝑤𝑖 ∗
𝑤̇ ∗ 𝑖 = 𝑑𝑡
+ 𝑤𝑖−1 𝑥𝑤 ∗ 𝑖 (1.3.19)
𝑑𝑤𝑖 ∗
𝑤̇𝑖 = 𝑤̇𝑖−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)
𝑑∗ 𝑤 ∗
𝑑𝑡
= 𝑍𝑖−1 𝑞̈ 𝑖 (1.3.22)
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:
12
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Donde:
13
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
𝑑(𝑚𝑖 𝑣̅𝑖 )
𝐹𝑖 = 𝑑𝑡
= 𝑚𝑖 𝑎̅𝑖 (1.3.29)
𝑑(𝐼𝑖 𝑤𝑖 )
𝑁𝑖 = 𝑑𝑡
= 𝐼𝑖𝑤̇𝑖 + 𝑤̇𝑖 𝑥(𝐼𝑖 𝑤𝑖 ) (1.3.30)
𝐹𝑖 = 𝑓𝑖 − 𝑓𝑖+1 (1.3.31)
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:
14
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
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).
15
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
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:
16
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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).
17
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
MA Z
O
Y
∑ 𝐹̅ = 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)
18
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
19
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
20
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
1.6.- Hipótesis:
1.7.- Objetivos:
- Determinar las dimensiones, forma y estructura del robot bípedo usando el software
Solidwork.
21
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
- Analizar la caminata del robot bípedo a través del criterio de estabilidad ZMP (Punto
de Momento Cero).
22
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
23
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
24
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Z0d
Z1d X1d
Z0i
X0i
Z1i
X2d
X1i
Z2d
X3d
Z3d Z2i
X2i
Z4d
Z3i
PA. Pie de X4d
Apoyo X3i
X5d
X2i
i
Z5d
Z4i
X4i
X2i
i
Z5i
X5i PF. (Pie Flotante)
X2i
i
25
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
L1 L1
L2 L2
L3
L3
L4 L4
L5
L5
L6 L6
L7
L7
26
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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
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
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:
27
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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:
28
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
cos(q1d) −sin(q1d) 0 0
0𝑑
𝑇0´𝑑 = ( sin(q1d) cos(q1d) 0 0) (3.6)
0 0 1 0
0 0 0 1
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:
1 0 0 𝐿7
5𝑑 0 1 0 0
𝑇4𝑑 =( ) (3.7)
0 0 1 0
0 0 0 1
1 0 0 𝐿2
0𝑑′ 0 0 −1 −𝐿1
𝑇𝐶 = ( ) (3.8)
0 1 0 0
0 0 0 1
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.
5d 5d
Tc = T4d 4dT3d 3dT2d 2dT1d 1dT0d 0dT0′d 0´dTc (3.9)
29
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
30
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
31
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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𝑖
−1 0 0 −𝐿2𝑖
𝑐 0 0 1 0
𝑇0𝑖 = ( ) (3.16)
0 1 0 𝑙1𝑖
0 0 0 1
C
T5i = CT0i 0iT1i 1iT2i 2iT3i 3iT4i 4iT5i (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𝑑 5𝑑
𝑇5𝑖 = 𝑇𝐶 ∗ 𝐶𝑇5𝑖 (3.19)
32
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Figura 3.5 Prueba de la cinemática directa del pie flotante respecto al pie de apoyo.
(Fuente: Elaboración propia)
33
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
1 0 0 𝐿2 1 0 0 𝐿2
0𝑑′ 0 0 −1 −𝐿1 0𝑖′ 0 0 −1 𝐿1
𝑇𝐶 = ( )= 𝑇𝐶 = ( ) (3.21)
0 1 0 0 0 1 0 0
0 0 0 1 0 0 0 1
−1 0 0 −𝐿2𝑖 −1 0 0 −𝐿2𝑖
𝑐 0 0 1 0 𝑐 0 0 1 0
𝑇0𝑖 = ( ) = 𝑇0𝑑 =( ) (3.22)
0 1 0 𝑙1𝑖 0 1 0 −𝑙1𝑖
0 0 0 1 0 0 0 1
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)
34
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
f1 f4 f7 f10 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥
f2 f5 f8 f11 𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑝𝑦
( ) = (𝑛 𝑜𝑧 𝑎𝑧 𝑝𝑧
) (3.23)
f3 f6 f9 f12 𝑧
0 0 0 1 0 0 0 1
𝑓1 𝑛𝑥
𝑓2 𝑛𝑦
𝑓3 𝑛𝑧
𝑓4 𝑜𝑥
𝑓5 𝑜𝑦
𝑓6 𝑜𝑧
𝑓(𝑥̃) = f(𝑥) = 𝑎𝑥 (3.24)
𝑓7
𝑓8 𝑎𝑦
𝑎𝑧
𝑓9
𝑃𝑥
𝑓10
𝑃𝑦
𝑓11
( 𝑃𝑧 )
(𝑓12)
Dónde:
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(𝑥̌)
f(x) = f(𝑥̌) + ∑𝑖=1 𝜕𝜃𝑖
* (𝜃𝑖 − 𝜃̅𝑖 ) (3.25)
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.
35
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
∇𝑓 = 𝐽 ∗ ∇θ (3.27)
J : Matriz Jacobiana.
∇θ = 𝐽−1 ∗ ∇𝑓 (3.28)
∇θ = 𝐽′ (𝐽 ∗ 𝐽′ + 𝛾 2 𝐼)−1 ∗ 𝑒 (3.29)
Donde:
e=∇𝑓 (error)
J’: la transpuesta de una matriz Jacobiano.
I: matriz identidad
𝛾: Constante de convergencia.
36
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
𝜃 = ∇θ + 𝜃̅ (3.30)
Cinemática
Inversa
∇θ = 𝐽′ (𝐽 ∗ 𝐽′ + 𝛾 2 𝐼)−1 ∗ 𝑒
𝜃 = 𝜃̅ + ∇θ
37
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
𝑑𝑓(𝑥) 𝑓(𝑥+Δ𝑥)−𝑓(𝑥)
𝑑𝑥
= lim (3.31)
Δ𝑥→∞ Δ𝑥
Δ𝑓(𝑥) 𝑓(𝑥+Δ𝑥)−𝑓(𝑥)
Δ𝑥
= Δ𝑥
(3.32)
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.”
1𝑑 𝜃
𝜃1𝑑 = 𝜃1𝑑 − 𝑟𝑜𝑢𝑛𝑑 (2∗𝜋 )∗2∗𝜋
𝜃2𝑑
𝜃2𝑑 = 𝜃2𝑑 − 𝑟𝑜𝑢𝑛𝑑 ( )∗2∗𝜋
2∗𝜋
𝜃3𝑑
𝜃3𝑑 = 𝜃3𝑑 − 𝑟𝑜𝑢𝑛𝑑 ( ) ∗ 2 ∗ 𝜋 (3.34)
2∗𝜋
𝜃4𝑑
𝜃4𝑑 = 𝜃4𝑑 − 𝑟𝑜𝑢𝑛𝑑 ( ) ∗ 2 ∗ 𝜋
2∗𝜋
𝜃1𝑑
𝜃1𝑑 = 𝜃1𝑑 − 𝑟𝑜𝑢𝑛𝑑 (2∗𝜋 )∗2∗𝜋
38
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
39
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.04
0.035
0.03
q5d (radianes)
0.025
0.02
0.015
0.01
0.005
-0.005
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo (s)
-0.02
-0.04
q4d (radianes)
-0.06
-0.08
-0.1
-0.12
-0.14
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo(s)
40
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.02
0.01
0
q3d (radianes)
-0.01
-0.02
-0.03
-0.04
-0.05
-0.06
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo(s)
0.07
0.06
q2d (radianes)
0.05
0.04
0.03
0.02
0.01
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo (s)
41
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.035
0.03
0.02
0.015
0.01
0.005
-0.005
-0.01
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo (s)
3.2.3.- Cinemática inversa del pie flotante del robot bípedo 10GDL.
42
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
-0.01
q1i (radianes)
-0.02
-0.03
-0.04
-0.05
-0.06
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo (s)
0.15
0.1
0.05
q2i (radianes)
-0.05
-0.1
-0.15
-0.2
-0.25
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo (s)
43
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0
q3i (radianes)
-5
-10
-15
-20
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo (s)
10
5
q4i (radianes)
-5
-10
-15
-20
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo (s)
44
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
2.5
q5i (radianes)
2
1.5
0.5
0
0 10 20 30 40 50 60 70 80 90 100
Número de divisiones tiempo (s)
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.
45
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
𝑖
𝑖 𝑅𝑖−1 ( 𝑖−1𝑅0 𝑤𝑖−1 + 𝑧0 𝑞𝑖̇ ) 𝑆𝑖 𝑒𝑙 𝑒𝑙𝑒𝑚𝑒𝑛𝑡𝑜 𝑖 𝑒𝑠 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛𝑎𝑙
𝑅0 𝑤𝑖 = { 𝑖 𝑖−1
(3.1.1)
𝑅𝑖−1 ( 𝑅0 𝑤𝑖−1 ) 𝑆𝑖 𝑒𝑙 𝑒𝑙𝑒𝑚𝑒𝑛𝑡𝑜 𝑖 𝑒𝑠 𝑡𝑟𝑎𝑙𝑎𝑐𝑖𝑜𝑛𝑎𝑙
𝑖
𝑖 𝑅𝑖−1 ( 𝑖−1𝑅0 𝑤̇𝑖−1 + 𝑧0 𝑞𝑖̈ + ( 𝑖−1𝑅0 𝑤𝑖−1 )𝑥 (𝑧0 𝑞𝑖̇ )) 𝑆𝑖 𝑒𝑙 𝑒𝑙𝑒𝑚𝑒𝑛𝑡𝑜 𝑖 𝑒𝑠 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛𝑎𝑙
𝑅0 𝑤̇𝑖 = { 𝑖
𝑅𝑖−1 ( 𝑖𝑅0 𝑤𝑖−1
̇ ) 𝑆𝑖 𝑒𝑙 𝑒𝑙𝑒𝑚𝑒𝑛𝑡𝑜 𝑖 𝑒𝑠 𝑡𝑟𝑎𝑙𝑎𝑐𝑖𝑜𝑛𝑎𝑙
(3.28)
𝑖
𝑅0 𝑣̇ 𝑖 = {( 𝑖𝑅0 𝑤̇ 𝑖 )𝑥( 𝑖𝑅0 𝑝𝑖∗ ) + ( 𝑖𝑅0 𝑤𝑖 )𝑥[( 𝑖𝑅0 𝑤𝑖 )𝑥( 𝑖𝑅0 𝑝𝑖∗ )] + 𝑖
𝑅𝑖−1 ( 𝑖−1𝑅0 𝑣𝑖−1̇ ) (3.1.2)
Si el Elemento i es rotacional.
𝑖
𝑅0 𝑎̅𝑖 = ( 𝑖𝑅0 𝑤̇𝑖 )𝑥( 𝑖𝑅0 𝑠̅𝑖 ) + ( 𝑖𝑅0 𝑤𝑖 )𝑥[( 𝑖𝑅0 𝑤𝑖 )𝑥( 𝑖𝑅0 𝑠̅𝑖 )] + 𝑖𝑅0 𝑣𝑖̇ (3.1.3)
𝑖
𝑅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)
46
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
(𝜃𝑖𝑛 −𝜗𝑖𝑛−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”.
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
47
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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,
48
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
49
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0
N*m
-0.05
-0.1
-0.15
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Tiempo
Figura 3.3.2 Torques calculados para los 5GDL del pie flotante a la cadera.
(Fuente: Elaboración propia)
0.5
rad/s
-0.5
-1
-1.5
-2
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
tiempo
50
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0
rad/s 2
-0.5
-1
-1.5
-2
-2.5
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Tiempo
0
Watts
-0.05
-0.1
-0.15
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
tiempo
51
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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:
Los parámetros de los torques de las Pierna de apoyo se presentan en la figura 3.3.6 con
las variables articulares respectivas.
52
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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”
53
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
54
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.1
N*m
0.05
-0.05
-0.1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Tiempo
Figura 3.3.7 Torques calculados para los 5GDL de la cadera respecto al pie de apoyo.
(Fuente: Elaboración propia)
0
rad/s
-0.05
-0.1
-0.15
-0.2
-0.25
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
tiempo
Figura 3.3.8 Velocidades calculados para los 5GDL de la cadera respecto al pie de apoyo.
(Fuente: Elaboración propia)
55
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.5
rad/s 2
-0.5
-1
-1.5
-2
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
tiempo
Figura 3.3.9 Velocidades calculados para los 5GDL de la cadera respecto al pie de apoyo.
(Fuente: Elaboración propia)
0
Watts
-0.01
-0.02
-0.03
-0.04
-0.05
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
tiempo
56
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
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.
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.
57
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
58
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
59
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
En donde:
Ti = Periodo de Interpolación.
𝑥0 = Posición Inicial.
𝑥1 = Posición final
𝑣0 = Velocidad inicial.
60
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
1.9
1.8
1.7
1.6
X(t) (m)
1.5
1.4
1.3
1.2
1.1
1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Ti (s)
61
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
i) Cadera:
z0=-SL/2;
z1=SL/2; Donde: 0 < t ≤ TP
v0=0;
v1=0;
T1=Tp;
Reemplazando en la ecuación 3.5.9 se tiene:
𝑆𝐿 𝑆𝐿 𝑆𝐿
Zc(t) = − + 3 𝑇𝑝2 𝑡 2 − 2 𝑇𝑝3 𝑡 3 (3.5.10)
2
0.008
0.006
0.004
Zcadera(metros)
0.002
-0.002
-0.004
-0.006
-0.008
-0.01
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
62
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
ℎ𝑐𝑎𝑑𝑒𝑟𝑎 2 ℎ𝑐𝑎𝑑𝑒𝑟𝑎 3
Xc(t) = ℎ𝐵𝑖𝑝𝑒𝑑 + ℎ𝑐𝑎𝑑𝑒𝑟𝑎 − 12 𝑇𝑝2
𝑡 + 16 𝑇𝑝3
𝑡 siendo: Tp/2 < t≤ Tp (3.5.13)
0.242
0.241
X cadera(metros)
0.24
0.239
0.238
0.237
0.236
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
63
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
yc1_PF=y0+v0*t1+[(3*(y1-y0)-Ti*(2*v0+v1))/Ti^2]*t1.^2+ [(2*(y0-y1)+
Ti*(v0+v1))/Ti^3]*t1.^3; (3.5.14)
64
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
yc2_PF=y0+v0*t1+[(3*(y1-y0)-Ti*(2*v0+v1))/Ti^2]*t1.^2+ [(2*(y0-y1)+
Ti*(v0+v1))/Ti^3]*t1.^3; (3.5.15)
Además se debe tener en cuenta el cambio de pierna ya que en ese caso se debe
multiplicar el y0, y1, v0, v1 por (-1).
-0.036
-0.038
Y cadera(metros)
-0.04
-0.042
-0.044
-0.046
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
65
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
El período de doble soporte aprox. varía entre el 20% a 30% del tiempo de paso.
𝑆𝐿 𝑆𝐿 3
Zpf(t) = −𝑆𝐿 + 6 𝑇𝑠𝑠2 𝑡 2 − 4 𝑡 (3.5.17)
𝑇𝑠𝑠3
Para la simulación se toman los mismos datos para la cadera, las medidas están en
metros
SL=0.020; Lp = Longitud de paso.
hBiped=0.23678; h = altura de la cadera
hpflotante=0.010; hp = elevación de la pierna flotante.
hcadera=0.005; hc = elevación de la cadera
Tp= 1 Tp = tiempo empleado para cado paso
TDobleSoporte = 30% ∗ Tp
66
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.015
0.01
Z PieFlotante(metros)
0.005
-0.005
-0.01
-0.015
-0.02
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
x0=hpflotante;
x1=0;
v0=0; Siendo: Tp/2 < t≤ Tp
v1=0;
T1 = Tss/2;
ℎ𝑝𝑓𝑙𝑜𝑡𝑎𝑛𝑡𝑒 2 ℎ𝑝𝑓𝑙𝑜𝑡𝑎𝑛𝑡𝑒 3
Xpf(t) = 12 𝑇𝑠𝑠
𝑡 − 16 𝑇𝑠𝑠3
𝑡 siendo: 0< t ≤ Tp/2 (3.5.18)
ℎ𝑝𝑓𝑙𝑜𝑡𝑎𝑛𝑡𝑒 2 ℎ𝑝𝑓𝑙𝑜𝑡𝑎𝑛𝑡𝑒 3
Xpf(t) =ℎ𝑝𝑓𝑙𝑜𝑡𝑎𝑛𝑡𝑒 − 12 𝑇𝑠𝑠2
𝑡 + 16 𝑇𝑠𝑠3
𝑡 siendo: Tp/2 < t≤ Tp (3.5.19)
67
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.009
0.008
0.007
X PieFlotante(metros)
0.006
0.005
0.004
0.003
0.002
0.001
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
Figura 3.5.10. Trayectoria de caminata de pierna flotante en Eje “X” plano sagital.
Planificación en el eje “Y”, plano frontal, Teniendo como referencia la figura 3.5.6
i) y0=-2LH;
y1=-DF; Siendo: 0 < t ≤ 𝑇𝑝/2
v0=0;
v1=0;
T1= Tss/2
y0=-DF;
z1= -2*LH;
v0=0; Siendo: Tp/2 < t ≤ 𝑇𝑝
v1=0;
T1=Tss/2;
ypf2_PF=y0+v0*t1+[(3*(y1-y0)-Ti*(2*v0+v1))/Ti^2]*t1.^2+ [(2*(y0-y1)+
Ti*(v0+v1))/Ti^3]*t1.^3; (3.5.21)
Se remplaza las variables con los datos dados anteriormente, además se debe
tener en cuenta el cambio de pierna ya que en ese caso se debe multiplicar el y0, y1, v0,
v1 por (-1).
68
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
-0.082
Y PieFlotante(metros)
-0.084
-0.086
-0.088
-0.09
-0.092
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
Figura 3.5.11. Trayectoria del Pie flotante, eje “Y” plano frontal.
69
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.009
0.008
0.007
Z cadera (metros)
0.006
0.005
0.004
0.003
0.002
0.001
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
70
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
𝑆𝐿 2 𝑆𝐿
Zpfstart(t) = 3 𝑇𝑝2
𝑡 − 2 𝑇𝑝3 𝑡 3 (3.5.23)
0.018
0.016
Z Pierna Flotante (metros)
0.014
0.012
0.01
0.008
0.006
0.004
0.002
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
71
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
-0.001
-0.002
-0.003
-0.005
-0.006
-0.007
-0.008
-0.009
-0.01
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
Se reemplaza las variables con los datos del robot, y se tienen la siguiente gráfica 3.5.16
Planificación Pie flotante en el EJE "Z"- FIN DE MARCHA
0
-0.002
-0.004
Z Pierna Flotante (metros)
-0.006
-0.008
-0.01
-0.012
-0.014
-0.016
-0.018
-0.02
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
TP(s)
.
Figura 3.5.16 Trayectoria de paro de robot del pie flotante en el eje “Z”.
72
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.241
"X" cadera
0.24
0.239
0.238
0.237
0.236
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Número de Divisiones de Tiempo (s)
Figura 3.5.17 Comparación delas trayectorias generadas (propuestas) con las que resulta
de la cinemática inversa y directa de la cadera en el eje “X”
-0.036
-0.038
"Y" cadera
-0.04
-0.042
-0.044
Resultados de Cinemática Inversa e Directa.
Trayectoria Planificada "Y"
-0.046
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Número de Divisiones de Tiempo (s)
Figura 3.5.18 Comparación de las trayectorias generadas (propuestas) con las que resulta
de la cinemática inversa y directa de la cadera en el eje “Y”
73
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.005
0
"Z" cadera
-0.005
-0.015
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Número de Divisiones de Tiempo (s)
Figura 3.5.19 Comparación de las trayectorias generadas (propuestas) con las que resulta
de la cinemática inversa y directa de la cadera en el eje “Z”
-3
x 10 Pierna Flotante en el EJE eje "X"
10
6
"X" Pierna Flotante
0
Resultados de Cinemática Inversa e Directa.
Trayectoria Planificada "X"
-2
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Número de Divisiones de Tiempo(s)
Figura 3.5.20 Comparación de las trayectorias generadas (propuestas) con las que resulta
de la cinemática inversa y directa de la pierna flotante en el eje “X”
74
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
-0.082
"Y" Pierna Flotante
-0.084
-0.086
-0.088
-0.09
-0.092
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Número de Divisiones de Tiempo (s)
Figura 3.5.21 Comparación del as trayectorias generadas (propuestas) con las que resulta
de la cinemática inversa y directa de la pierna flotante en el eje “Y”
0.015
0.01
"Z" Pierna Flotante
0.005
-0.005
-0.01
Figura 3.5.22 Comparación de las trayectorias generadas (propuestas) con las que resulta
de la cinemática inversa y directa de la pierna flotante en el eje “Z”
75
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Figura 3.6.2 Modelo Simplificado del Robot Bípedo (PENDULO INVERTIDO 3D)
(Fuente: elaboración propia)
76
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Siendo:
𝑝𝑐𝑚 = 𝑀𝑜𝑚𝑒𝑛𝑡𝑢𝑚 𝑑𝑒𝑙 𝑐𝑒𝑛𝑡𝑟𝑜 𝑑𝑒 𝑚𝑎𝑠𝑎
Desarrollando la ecuación 3.6.6 se tiene lo siguiente:
𝑑 𝑑 𝑑
𝐻
𝑑𝑡 𝑐𝑚
= (𝑑𝑡 𝑟𝑐𝑚 𝑥 𝑝𝑐𝑚 ) + ( 𝑟𝑐𝑚 𝑥 𝑑𝑡
𝑝𝑐𝑚 ) (3.6.7)
El primer término de la ecuación 3.6.7 es igual a cero. Por ser vectores paralelos.
𝑑 𝑑
𝐻
𝑑𝑡 𝑐𝑚
= 𝑟𝑐𝑚 𝑥 𝑚 𝑑𝑡 𝑣𝑐𝑚 (3.6.11)
77
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
El ZMP puede ser usado para representar a suma de torques, resultado de las
fuerzas de reacción del suelo como sigue.
∑ 𝑀𝑥𝑦𝑧 − 𝑟𝑧𝑚𝑝 𝑥 𝐹𝑥𝑦𝑧 = [𝑀𝑥 0 0]′ (3.6.15)
Siendo:
𝑟𝑧𝑚𝑝 = posición del ZMP. (3.6.17)
𝑀𝑦 = −𝑚𝑔𝑧𝑧𝑚𝑝 (3.6.20)
𝑧𝑥̈ −𝑥𝑧̈
𝑍𝑍𝑀𝑃 = 𝑧 + 𝑔
(3.6.22)
78
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Figura 3.6.3. Un punto “ri” que se refiere a la cadera del robot bípedo 10GDL
Respecto al Pie de Apoyo
Tomando la figura 3.6.3, se calcula el Velocidad y Aceleración del COG, del Robot
Bípedo.
Sea:
𝑋𝑖
𝑖 𝑌
𝑟𝑖 = ( 𝑖 ) (3.6.23)
𝑍𝑖
1
Sea 0𝑟𝑖 el mismo punto 𝑖𝑟𝑖 con respecto al sistema de coordenadas de la base.
𝑖−1
𝑇𝑖 La matriz de transformación homogénea de coordenadas homogéneas que relación
el desplazamiento espacial.
79
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
La derivada parcial de 0𝑇𝑖 con respecto 𝜃𝑗 se puede calcular fácilmente con ayuda de la
matriz 0 −1 0 0 (3.6.27)
𝑄𝑖 = (1 0 0 0)
0 0 0 0
0 0 0 0
Entonces se define lo siguiente:
𝜕 𝑖−1𝑇𝑖
= 𝑄𝑖 𝑖−1𝑇𝑖 (3.6.28)
𝜕𝜃𝑗
𝜕 0𝑇5
0
𝑣5 = (∑5𝑗=1 𝜕𝜃𝑗 𝑗
𝜃̇ ) 𝑖𝑟̇ 𝑖 (3.6.29)
Ahora bien para obtener la aceleración lineal del COG se procede a derivar con respecto
al tiempo la ecuación 3.6.30
Obteniendo:
𝑑(𝑣𝑖 ) 𝜕2 0𝑇𝑖
0
𝛼𝑖 = 𝑑𝑡
= (∑𝑖𝑗=1 𝜕𝜃2 𝑗
𝜃𝑗̇ 𝜃𝑗̈ ) 𝑖𝑟𝑖 (3.6.31)
Para aplicar la ecuación 3.6.31 se toma en cuenta que los grados de libertad de la
base del pie de apoyo hasta la cadera son 5 es por ello la ecuación de aceleraciones para
el robot bípedo queda de la siguiente manera.
80
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
a5C=(T54d*Qi*Qi*T43d*T32d*T21d*T10d*T00d*T0Cd*dq5d*ddq5d +...
T54d*T43d*Qi*Qi*T32d*T21d*T10d*T00d*T0Cd*dq4d*ddq4d +...
T54d*T43d*T32d*Qi*Qi*T21d*T10d*T00d*T0Cd*dq3d*ddq3d +... (3.6.33)
T54d*T43d*T32d*T21d*Qi*Qi*T10d*T00d*T0Cd*dq2d*ddq2d +...
T54d*T43d*T32d*T21d*T10d*Qi*Qi*T00d*T0Cd*dq1d*ddq1d)*iri;
Siendo dq5d, dq4d, dq3d, dq2d, dq1d representa la velocidad articular desde el pie
hasta la cadera, respectivamente.
Siendo ddq5d, ddq4d, ddq3d, ddq2d, ddq1d representa la aceleración articular
desde el pie hasta la cadera, respectivamente.
Se desarrolló un algoritmo en matlab2013b con las ecuaciones 3.6.30 y 3.6.33 se
obtiene. Las ecuaciones de velocidad y aceleración del Centro de Masa en “X”, “Y”, “Z” se
muestran en anexos A.6
Se reemplazan las ecuaciones a.47 hasta a.52 con los valores del robot Bípedo, además
Longitudes en metros.
L7=0.0174;
L6=0.0468;
L5=0.0473;
L4=0.064;
L3=0.0473;
L2=0.0288;
L1=0.0452;
81
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
0.03
0.01
-0.01
-0.02
Velocidad en el Eje X
-0.03 Velocidad en el Eje Y
Velocidad en el Eje Z
-0.04
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
tiempo
0.02
Aceleración en m/s
0.01
-0.01
-0.02
-0.03
-0.04
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
tiempo
82
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Del gráfico 3.6.6 se observa que está dentro del polígono de soporte la cual las
dimensiones del pie son eje “Z” = 120mm y eje “Y”= 64mm, la cual la caminata del Robot
es estable.
Tener en cuenta que estos resultados son teóricos, ya que los más conveniente
sería medir las velocidades y aceleraciones y reemplazar en la ecuación 3.6.21 y 3.6.22
para obtener los puntos de ZMP reales.
83
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
84
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
b.- Se configura e bloque grados de libertad del robot, permite desplazarse al robot en
movimiento prismáticos, respecto a la tierra fijada, en x,y,z.
85
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Siguiendo la representación propuesta en la figura 3.4.2, sigue por colocar los bloques
que representan a la pierna del robot, se tiene:
En la figura 3.7.9 se observa el bloque Weld, que realiza la función de unir (soldar) 2
cuerpos (eslabones), la configuración de L2d se muestra en la figura 3.7.10
86
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Luego se conecta el bloque de control 1d, este representara el bloque que permita
controlar el movimiento del robot se muestra en la figura 3.7.11
87
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
88
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
89
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
3.7.16. Representación general del esquema de simulación del robot bípedo de 10 GDL
en SimMechanics
90
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
91
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
92
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
93
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
94
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
95
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
96
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
97
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
5.2.- Recomendaciones:
5.2.1 Con los resultados obtenidos se espera que se lleve a la implementación física
del Robot de 10GDL con las características propuestas.
5.2.2 Se diseñé un controlador que pueda modificar la posición del ZMP, cuando
salga del polígono de soporte, y que incluya todos los ítems trabajados anteriormente, por
ejemplo un controlador multiarticular PID, PID pre alimentado, o controles Adaptativos.
5.2.3 Calcular la Dinámica del Robot bípedo mediante otro método, por ejemplo de
Lagrange-Euler y poder comparar los resultados obtenidos.
5.2.4 Realizar el análisis estructural del robot bípedo presentado, para determinar
las dimensiones, esfuerzos y deformaciones de cada pieza (soportes de servomotor,
cadera), así como el material óptimo para su implementación, teniendo en cuenta las
ergonomía en la estructura.
5.2.5 Hacer uso de controladores utilizando la Inteligencia Artificial, como
controlares difusos, redes neuronales, algoritmos genéticos, etc.
5.2.6 Los alcances que pueda tener este trabajo de investigación depende de la
aplicación que pueda tener y lo estudios investigativos que se pueda lograr, inclusión de
sensores, caminata en superficies inclinadas, visión artificial, la realización de exosqueletos,
aplicados a la medicina, la industria, el campo militar, espacial, etc.
99
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
[7] José Larriva, Edynson Guillermo, José Trelles, Oscar Vele, “Diseño y Construcción
de un robot bípedo experimental”. Universidad Politécnica Salesiana, escuela de ingeniería
electrónica.
[8]. Dennis W. Hong Chair, Andrew J. Kurdila, Robert L. West, Thurmmon E. Lockhart.
“Bipedal Walking for a Full – sized Humanoid Robot Utilizing Sinusoidal Feet Trayectories
and Its Energy Consumption”, Doctor of Philosophy in Mechanical Engineering. April 24,
2012, Blacksburg, Virginia.
[9] CHAPRA – CANALE, “Métodos Númericos para Ingenieros”, McGraw Hill, Tercera
Edición, 2001.
[10] JOHN J. CRAIG, “Róbotica”, Tercera Edición. Pearson Eduación, México, 2006.
ISBN: 970-26-00772-8.
100
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
101
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
ANEXOS
A.- RESULTADOS DE ECUACIONES OBTENIDAS:
A.1 Resultado de cinemática directa de la “cadera respecto al pie de apoyo”.
r11= cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) - sin(q1d)*sin(q5d) -
cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) -
cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) -
cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d)
(a.1)
r33= os(q1d + q2d + q3d + q4d)/2 - cos(q1d - q2d - q3d - q4d)/2 (a.9)
102
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
103
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
A.3 Resultados de la Cinemática Directa del pie flotante respecto al pie de apoyo:
A21 = (cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i) - cos(q1i)*sin(q5i) +
cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) +
cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i))*(cos(q1d)*cos(q5d) -
cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin(q5d) +
cos(q2d)*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) +
cos(q3d)*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) +
cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d)) - (cos(q2d + q3d + q4d +
104
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
A12= (cos(q1i)*cos(q2i)*sin(q3i)*sin(q4i)*sin(q5i) -
cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*sin(q5i) - cos(q5i)*sin(q1i) +
cos(q1i)*cos(q3i)*sin(q2i)*sin(q4i)*sin(q5i) +
cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i)*sin(q5i))*(sin(q1d)*sin(q5d) -
cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) +
cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) +
cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) +
cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d)) + (cos(q2i + q3i + q4i +
q5i)/2 - cos(q2i + q3i + q4i - q5i)/2)*(sin(q2d + q3d + q4d + q5d)/2 +
sin(q2d + q3d + q4d - q5d)/2) -
(cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) -
cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - cos(q1d)*sin(q5d) +
cos(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) +
cos(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q3d))*(cos(q1i)*cos(q5i) -
cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)*sin(q5i) +
cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i)*sin(q5i) +
cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i)*sin(q5i) +
cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i)*sin(q5i)) (a.28)
A22=(cos(q1d)*cos(q2d)*sin(q3d)*sin(q4d)*sin(q5d) -
cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) - cos(q5d)*sin(q1d) +
cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) +
cos(q1d)*cos(q4d)*sin(q2d)*sin(q3d)*sin(q5d))*(cos(q1i)*cos(q2i)*sin(q3i)
*sin(q4i)*sin(q5i) - cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*sin(q5i) -
cos(q5i)*sin(q1i) + cos(q1i)*cos(q3i)*sin(q2i)*sin(q4i)*sin(q5i) +
cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i)*sin(q5i)) - (cos(q2d + q3d + q4d +
q5d)/2 - cos(q2d + q3d + q4d - q5d)/2)*(cos(q2i + q3i + q4i + q5i)/2 -
cos(q2i + q3i + q4i - q5i)/2) - (cos(q1d)*cos(q5d) -
cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin(q5d) +
cos(q2d)*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) +
cos(q3d)*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) +
cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d))*(cos(q1i)*cos(q5i) -
cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)*sin(q5i) +
105
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
A14= L7 - (sin(q1d)*sin(q5d) -
cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) +
cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) +
cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) +
cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d))*((L4*cos(q1i)*sin(q2i))/2 -
L3*cos(q1i) - L2 + L7*sin(q1i)*sin(q5i) -
(L5*cos(q1i)*cos(q2i)*sin(q3i))/2 - (L5*cos(q1i)*cos(q3i)*sin(q2i))/2 -
(3^(1/2)*L4*cos(q1i)*cos(q2i))/2 + L6*cos(q1i)*cos(q2i)*sin(q3i)*sin(q4i)
+ L6*cos(q1i)*cos(q3i)*sin(q2i)*sin(q4i) +
L6*cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i) -
106
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
107
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
108
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
109
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
110
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
111
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
112
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
113
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
114
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
115
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
116
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
117
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
118
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
119
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
120
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
121
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
122
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
123
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
124
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
125
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
126
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
127
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
128
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
129
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
130
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
131
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
132
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
133
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
134
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
135
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
136
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
137
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
138
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
139
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
140
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
141
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
142
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
143
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
T43d=denavit(theta1(1),d1(1),a1(1),alfa1(1))
T32d=denavit(theta1(2),d1(2),a1(2),alfa1(2))
T21d=denavit(theta1(3),d1(3),a1(3),alfa1(3))
T10d=denavit(theta1(4),d1(4),a1(4),alfa1(4))
T00d=denavit(theta1(5),d1(5),a1(5),alfa1(5))
T0Cd=[1 0 0 L2;
0 0 -1 -L1;
0 1 0 0;
0 0 0 1] % matriz de transformacion hacia la cadera.
144
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
T01=denavit(theta2(1),d2(1),a2(1),alfa2(1))
T12=denavit(theta2(2),d2(2),a2(2),alfa2(2))
T23=denavit(theta2(3),d2(3),a2(3),alfa2(3))
T34=denavit(theta2(4),d2(4),a2(4),alfa2(4))
T45=denavit(theta2(5),d2(5),a2(5),alfa2(5))
T05=simplify(T01*T12*T23*T34*T45)
TC0=[-1 0 0 -L2;
0 0 1 0;
0 1 0 L1;
0 0 0 1]
% Matriz de transformacion que la relaciona la e pie flotante respecto
a
% la cadera.
TC5i=simplify(TC0*T01*T12*T23*T34*T45)
P2=TC5i(1:3,4)
145
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
T5dc = T54d*T43d*T32d*T21d*T10d*T00d*T0Cd;
%T5dc =[T5dc(1,4);T5dc(2,4);T5dc(3,4)];
end
146
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
end
A1=directkinematicBiped(b1);
A2=directkinematicBiped_2(b2);
A3=A1*A2;
147
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Tzd =[ 1 0 0 0;
0 1 0 0;
0 0 1 d;
0 0 0 1];
Txa =[ 1 0 0 a;
0 1 0 0;
0 0 1 0;
0 0 0 1];
Txalfa =[1 0 0 0;
0 cos(alfa) -sin(alfa) 0;
0 sin(alfa) cos(alfa) 0;
0 0 0 1];
dh =Tztheta*Tzd*Txa*Txalfa;
148
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
for n=1:1000
F =[cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) - sin(q1d)*sin(q5d) -
cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) -
cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) -
cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d);
cos(q5d)*sin(q1d) + cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) -
cos(q1d)*cos(q2d)*sin(q3d)*sin(q4d)*sin(q5d) -
cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) -
cos(q1d)*cos(q4d)*sin(q2d)*sin(q3d)*sin(q5d);
sin(q1d - q2d - q3d - q4d)/2 - sin(q1d + q2d + q3d + q4d)/2;
sin(q2d + q3d + q4d + q5d)/2 + sin(q2d + q3d + q4d - q5d)/2;
cos(q2d + q3d + q4d - q5d)/2 - cos(q2d + q3d + q4d + q5d)/2;
cos(q2d + q3d + q4d);
cos(q1d)*sin(q5d) + cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) -
cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) -
cos(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) -
cos(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q3d);
cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin(q5d) - cos(q1d)*cos(q5d) -
cos(q2d)*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) -
cos(q3d)*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) -
cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d);
cos(q1d + q2d + q3d + q4d)/2 - cos(q1d - q2d - q3d - q4d)/2;
L7 + (cos(q5d)*(2*L6 - L4*sin(q3d + q4d) + L5*sin(q4d) +
2*L3*cos(q2d + q3d + q4d) + 3^(1/2)*L4*cos(q3d + q4d) +
3^(1/2)*L5*cos(q4d)))/2 - L2*(sin(q1d)*sin(q5d) -
cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) +
cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) +
cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) +
cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d)) -
L1*(cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) -
cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - cos(q1d)*sin(q5d) +
cos(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) +
cos(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q3d));
(sin(q5d)*(2*L6 - L4*sin(q3d + q4d) + L5*sin(q4d) + 2*L3*cos(q2d +
q3d + q4d) + 3^(1/2)*L4*cos(q3d + q4d) + 3^(1/2)*L5*cos(q4d)))/2 -
L1*(cos(q1d)*cos(q5d) - cos(q2d)*cos(q3d)*cos(q4d)*sin(q1d)*sin(q5d) +
cos(q2d)*sin(q1d)*sin(q3d)*sin(q4d)*sin(q5d) +
cos(q3d)*sin(q1d)*sin(q2d)*sin(q4d)*sin(q5d) +
cos(q4d)*sin(q1d)*sin(q2d)*sin(q3d)*sin(q5d)) -
L2*(cos(q1d)*cos(q2d)*sin(q3d)*sin(q4d)*sin(q5d) -
cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*sin(q5d) - cos(q5d)*sin(q1d) +
cos(q1d)*cos(q3d)*sin(q2d)*sin(q4d)*sin(q5d) +
cos(q1d)*cos(q4d)*sin(q2d)*sin(q3d)*sin(q5d));
(L5*cos(q4d))/2 - (L4*cos(q3d + q4d))/2 - L2*(sin(q1d + q2d + q3d
+ q4d)/2 - sin(q1d - q2d - q3d - q4d)/2) - L3*sin(q2d + q3d + q4d) +
L1*(cos(q1d + q2d + q3d + q4d)/2 - cos(q1d - q2d - q3d - q4d)/2) -
(3^(1/2)*L4*sin(q3d + q4d))/2 - (3^(1/2)*L5*sin(q4d))/2 ];
% F set Point. Posicionamiento de la cadera a la que se movera.
Fsp=[1;
149
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
dp5=q5d+dx;
dp4=q4d+dx;
dp3=q3d+dx;
dp2=q2d+dx;
dp1=q1d+dx;
J(1,1)=(F1HCPA(dp5,q4d,q3d,q2d,q1d)-F1HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(1,2)=(F1HCPA(q5d,dp4,q3d,q2d,q1d)-F1HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(1,3)=(F1HCPA(q5d,q4d,dp3,q2d,q1d)-F1HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(1,4)=(F1HCPA(q5d,q4d,q3d,dp2,q1d)-F1HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(1,5)=(F1HCPA(q5d,q4d,q3d,q2d,dp1)-F1HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(2,1)=(F2HCPA(dp5,q4d,q3d,q2d,q1d)-F2HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(2,2)=(F2HCPA(q5d,dp4,q3d,q2d,q1d)-F2HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(2,3)=(F2HCPA(q5d,q4d,dp3,q2d,q1d)-F2HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(2,4)=(F2HCPA(q5d,q4d,q3d,dp2,q1d)-F2HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(2,5)=(F2HCPA(q5d,q4d,q3d,q2d,dp1)-F2HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(3,1)=(F3HCPA(dp5,q4d,q3d,q2d,q1d)-F3HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(3,2)=(F3HCPA(q5d,dp4,q3d,q2d,q1d)-F3HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(3,3)=(F3HCPA(q5d,q4d,dp3,q2d,q1d)-F3HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(3,4)=(F3HCPA(q5d,q4d,q3d,dp2,q1d)-F3HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(3,5)=(F3HCPA(q5d,q4d,q3d,q2d,dp1)-F3HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(4,1)=(F4HCPA(dp5,q4d,q3d,q2d,q1d)-F4HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(4,2)=(F4HCPA(q5d,dp4,q3d,q2d,q1d)-F4HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(4,3)=(F4HCPA(q5d,q4d,dp3,q2d,q1d)-F4HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(4,4)=(F4HCPA(q5d,q4d,q3d,dp2,q1d)-F4HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(4,5)=(F4HCPA(q5d,q4d,q3d,q2d,dp1)-F4HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(5,1)=(F5HCPA(dp5,q4d,q3d,q2d,q1d)-F5HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(5,2)=(F5HCPA(q5d,dp4,q3d,q2d,q1d)-F5HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(5,3)=(F5HCPA(q5d,q4d,dp3,q2d,q1d)-F5HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
150
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
J(6,1)=(F6HCPA(dp5,q4d,q3d,q2d,q1d)-F6HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(6,2)=(F6HCPA(q5d,dp4,q3d,q2d,q1d)-F6HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(6,3)=(F6HCPA(q5d,q4d,dp3,q2d,q1d)-F6HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(6,4)=(F6HCPA(q5d,q4d,q3d,dp2,q1d)-F6HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(6,5)=(F6HCPA(q5d,q4d,q3d,q2d,dp1)-F6HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(7,1)=(F7HCPA(dp5,q4d,q3d,q2d,q1d)-F7HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(7,2)=(F7HCPA(q5d,dp4,q3d,q2d,q1d)-F7HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(7,3)=(F7HCPA(q5d,q4d,dp3,q2d,q1d)-F7HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(7,4)=(F7HCPA(q5d,q4d,q3d,dp2,q1d)-F7HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(7,5)=(F7HCPA(q5d,q4d,q3d,q2d,dp1)-F7HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(8,1)=(F8HCPA(dp5,q4d,q3d,q2d,q1d)-F8HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(8,2)=(F8HCPA(q5d,dp4,q3d,q2d,q1d)-F8HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(8,3)=(F8HCPA(q5d,q4d,dp3,q2d,q1d)-F8HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(8,4)=(F8HCPA(q5d,q4d,q3d,dp2,q1d)-F8HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(8,5)=(F8HCPA(q5d,q4d,q3d,q2d,dp1)-F8HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(9,1)=(F9HCPA(dp5,q4d,q3d,q2d,q1d)-F9HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(9,2)=(F9HCPA(q5d,dp4,q3d,q2d,q1d)-F9HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(9,3)=(F9HCPA(q5d,q4d,dp3,q2d,q1d)-F9HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(9,4)=(F9HCPA(q5d,q4d,q3d,dp2,q1d)-F9HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(9,5)=(F9HCPA(q5d,q4d,q3d,q2d,dp1)-F9HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(10,1)=(F10HCPA(dp5,q4d,q3d,q2d,q1d)-
F10HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(10,2)=(F10HCPA(q5d,dp4,q3d,q2d,q1d)-
F10HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(10,3)=(F10HCPA(q5d,q4d,dp3,q2d,q1d)-
F10HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(10,4)=(F10HCPA(q5d,q4d,q3d,dp2,q1d)-
F10HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(10,5)=(F10HCPA(q5d,q4d,q3d,q2d,dp1)-
F10HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(11,1)=(F11HCPA(dp5,q4d,q3d,q2d,q1d)-
F11HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(11,2)=(F11HCPA(q5d,dp4,q3d,q2d,q1d)-
F11HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(11,3)=(F11HCPA(q5d,q4d,dp3,q2d,q1d)-
F11HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(11,4)=(F11HCPA(q5d,q4d,q3d,dp2,q1d)-
F11HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(11,5)=(F11HCPA(q5d,q4d,q3d,q2d,dp1)-
F11HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(12,1)=(F12HCPA(dp5,q4d,q3d,q2d,q1d)-
F12HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(12,2)=(F12HCPA(q5d,dp4,q3d,q2d,q1d)-
F12HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(12,3)=(F12HCPA(q5d,q4d,dp3,q2d,q1d)-
F12HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
151
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% Matriz Jacobiana.
J=[J(1,1) J(1,2) J(1,3) J(1,4) J(1,5);
J(2,1) J(2,2) J(2,3) J(2,4) J(2,5);
J(3,1) J(3,2) J(3,3) J(3,4) J(3,5);
J(4,1) J(4,2) J(4,3) J(4,4) J(4,5);
J(5,1) J(5,2) J(5,3) J(5,4) J(5,5);
J(6,1) J(6,2) J(6,3) J(6,4) J(6,5);
J(7,1) J(7,2) J(7,3) J(7,4) J(7,5);
J(8,1) J(8,2) J(8,3) J(8,4) J(8,5);
J(9,1) J(9,2) J(9,3) J(9,4) J(9,5);
J(10,1) J(10,2) J(10,3) J(10,4) J(10,5);
J(11,1) J(11,2) J(11,3) J(11,4) J(11,5);
J(12,1) J(12,2) J(12,3) J(12,4) J(12,5)];
q5d=q5d+dtheta(1);
q4d=q4d+dtheta(2);
q3d=q3d+dtheta(3);
q2d=q2d+dtheta(4);
q1d=q1d+dtheta(5);
end
end
152
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
dx=0.0001;
for n=1:1000
F=[ (sin(q2d + q3d + q4d + q5d)/2 + sin(q2d + q3d + q4d -
q5d)/2)*(sin(q2i + q3i + q4i + q5i)/2 + sin(q2i + q3i + q4i - q5i)/2) -
(sin(q1d)*sin(q5d) - cos(q1d)*cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d) +
cos(q1d)*cos(q2d)*cos(q5d)*sin(q3d)*sin(q4d) +
cos(q1d)*cos(q3d)*cos(q5d)*sin(q2d)*sin(q4d) +
cos(q1d)*cos(q4d)*cos(q5d)*sin(q2d)*sin(q3d))*(sin(q1i)*sin(q5i) -
cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) +
cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) +
cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) +
cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin(q3i)) +
(cos(q2d)*cos(q5d)*sin(q1d)*sin(q3d)*sin(q4d) -
cos(q2d)*cos(q3d)*cos(q4d)*cos(q5d)*sin(q1d) - cos(q1d)*sin(q5d) +
cos(q3d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q4d) +
cos(q4d)*cos(q5d)*sin(q1d)*sin(q2d)*sin(q3d))*(cos(q2i)*cos(q5i)*sin(q1i)
153
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
154
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
155
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
156
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
157
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% Error:
error=Fsp-F;
s=0;
for i=1:12
s=s+abs(error(i));
end
if s<0.0001
break;
end
dp1=q1i+dx;
dp2=q2i+dx;
dp3=q3i+dx;
dp4=q4i+dx;
dp5=q5i+dx;
J(1,1)=(F1HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(1,2)=(F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(1,3)=(F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(1,4)=(F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(1,5)=(F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F1HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(2,1)=(F2HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(2,2)=(F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(2,3)=(F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
158
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
J(3,1)=(F3HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(3,2)=(F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(3,3)=(F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(3,4)=(F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(3,5)=(F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F3HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(4,1)=(F4HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(4,2)=(F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(4,3)=(F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(4,4)=(F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(4,5)=(F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F4HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(5,1)=(F5HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(5,2)=(F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(5,3)=(F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(5,4)=(F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(5,5)=(F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F5HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(6,1)=(F6HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(6,2)=(F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(6,3)=(F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(6,4)=(F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(6,5)=(F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F6HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(7,1)=(F7HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(7,2)=(F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(7,3)=(F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
159
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
J(8,1)=(F8HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(8,2)=(F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(8,3)=(F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(8,4)=(F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(8,5)=(F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F8HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(9,1)=(F9HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(9,2)=(F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(9,3)=(F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(9,4)=(F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(9,5)=(F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F9HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(10,1)=(F10HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(10,2)=(F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(10,3)=(F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(10,4)=(F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(10,5)=(F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F10HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(11,1)=(F11HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(11,2)=(F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(11,3)=(F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(11,4)=(F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(11,5)=(F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F11HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(12,1)=(F12HPAPF(q5d,q4d,q3d,q2d,q1d,dp1,q2i,q3i,q4i,q5i)-
F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(12,2)=(F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,dp2,q3i,q4i,q5i)-
F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(12,3)=(F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,dp3,q4i,q5i)-
F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
160
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
q1i=q1i+dtheta(1);
q2i=q2i+dtheta(2);
q3i=q3i+dtheta(3);
q4i=q4i+dtheta(4);
q5i=q5i+dtheta(5);
end
q11i=q1i-round(q1i/(2*pi))*2*pi;
q11is=q11i*180/pi;
q22i=q2i-round(q2i/(2*pi))*2*pi;
q22is=q22i*180/pi;
q33i=q3i-round(q3i/(2*pi))*2*pi;
q33is=q33i*180/pi;
q44i=q4i-round(q4i/(2*pi))*2*pi;
q44is=q44i*180/pi;
q55i=q5i-round(q5i/(2*pi))*2*pi;
q55is=q55i*180/pi;
End
161
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
end
%Fin de Marcha.
for i=1:101
qc_end2(:,i)=InversekinematicBiped_1(1000*PosCadera_end(:,i));
end
% En Plena caminata.
%for i=1:101
% qcadera2(:,i)=InversekinematicBiped_1(1000*PosCadera(:,i));
%end
%........................................................................
..
%Angulos de PIE FLOTANTE RESPECTO AL PIE DE APOYO.
%Inicio de Marcha:
for i=1:101
qpf_start2(:,i)=InversekinematicBiped_3(1000*PosPieF_star(:,i));
end
%Fin de Marcha
for i=1:101
qpf_end2(:,i)=InversekinematicBiped_3(1000*PosPieF_end(:,i));
end
%for i=1:101
% qpf2(:,i)=InversekinematicBiped_3(1000*PosPieF(:,i));
%end
162
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%----------------------------------------------------------------------
%parametro de Denavit-Hartenberg del robot
%----------------------------------------------------------------------
theta=[q1i; q2i+pi/6; q3i-pi/3; q4i+pi/6; q5i ];
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]
%------------------------------------------------------------------------
% Masa de cada elemento en Kg
%------------------------------------------------------------------------
% m1=elemento1
% m2=elemento2
% ------------------------------------------------------------
% Coeficiente de rozamiento viscoso de cada articulacion
% ------------------------------------------------------------
% b1=Coeficiente viscoso elemento1
% b2=Coeficiente viscoso elemento2
%------------------------------------------------------------
% Matrices de Inercia (Kg-m^2)
% ------------------------------------------------------------
r10I_r01=I1i;
r20I_r02=I2i;
r30I_r03=I3i;
r40I_r04=I4i;
r50I_r05=I5i;
% ------------------------------------------------------------
% Matrices de Rotación.
% ------------------------------------------------------------
r01i=denavit(theta(1),d(1),a(1),alpha(1));
r01i=r01i(1:3,1:3)
r10i=r01i.'
r12i=denavit(theta(2),d(2),a(2),alpha(2));
r12i=r12i(1:3,1:3)
r21i=r12i.'
163
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% ------------------------------------------------------------
% Vectores ri0pi, ri0si.
% ------------------------------------------------------------
r10p1 = ri0pi(a(1),d(1),alpha(1))
r20p2 = ri0pi(a(2),d(2),alpha(2))
r30p3 = ri0pi(a(3),d(3),alpha(3))
r40p4 = ri0pi(a(4),d(4),alpha(4))
r50p5 = ri0pi(a(5),d(5),alpha(5))
r60p6=zeros(3,1);
%posicion del centro de masa del elemento i desde sistema de referenca i
base
r10s1 = [-L3/2;0;0]
r20s2 = [-L4/2;0;0]
r30s3 = [-L5/2;0;0]
r40s4 = [-L6/2;0;0]
r50s5 = [-L7/2;0;0]
r60s6 = zeros(3,1)
% ------------------------------------------------------------
% Velocidad angular de las articulaciones
% ------------------------------------------------------------
r00w0=zeros(3,1); % velocidad inicial =0
r10w1=ri0wi(r10i,r00w0,qd1i)
r20w2=ri0wi(r21i,r10w1,qd2i)
r30w3=ri0wi(r32i,r20w2,qd3i)
r40w4=ri0wi(r43i,r30w3,qd4i)
r50w5=ri0wi(r54i,r40w4,qd5i)
r60w6=ri0wi(r65i,r50w5,0)
% ------------------------------------------------------------
% Aceleracion angular de las articulaciones
% ------------------------------------------------------------
r00wp0 = zeros(3,1); % ACELERACION INICIAAL.
r10wp1 = ri0wpi(r10i, r00wp0,r00w0,qd1i,qdd1i)
r20wp2 = ri0wpi(r21i, r10wp1,r10w1,qd2i,qdd2i)
r30wp3 = ri0wpi(r32i, r20wp2,r20w2,qd3i,qdd3i)
r40wp4 = ri0wpi(r43i, r30wp3,r30w3,qd4i,qdd4i)
r50wp5 = ri0wpi(r54i, r40wp4,r40w4,qd5i,qdd5i)
r60wp6 = ri0wpi(r65i, r50wp5,r50w5,0,0)
% ------------------------------------------------------------
% Aceleracion lineal articular
% ------------------------------------------------------------
164
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% ------------------------------------------------------------
% Aceleracion del centro de masa de cada elemento
% ------------------------------------------------------------
r10a1 = simplify(ri0ai(r10vp1, r10wp1, r10w1, r10s1))
r20a2 = simplify(ri0ai(r20vp2, r20wp2, r20w2, r20s2))
r30a3 = simplify(ri0ai(r30vp3, r30wp3, r30w3, r30s3))
r40a4 = simplify(ri0ai(r40vp4, r40wp4, r40w4, r40s4))
r50a5 = simplify(ri0ai(r50vp5, r50wp5, r50w5, r50s5))
r60a6 = simplify(ri0ai(r60vp6, r60wp6, r60w6, r60s6));
% ------------------------------------------------------------
% Fuerza en el centro de masa de cada elemento
% ------------------------------------------------------------
% ------------------------------------------------------------
% Par en el centro de masa de cada elemento
% ------------------------------------------------------------
% ------------------------------------------------------------
% Fuerzas articulares
% ------------------------------------------------------------
r60f6a = r60f6
r50f5a = simplify(ri0fia(r56i, r60f6a, r50f5))
r40f4a = simplify(ri0fia(r45i, r50f5a, r40f4))
r30f3a = simplify(ri0fia(r34i, r40f4a, r30f3))
r20f2a = simplify(ri0fia(r23i, r30f3a, r20f2))
r10f1a = simplify(ri0fia(r12i, r20f2a, r10f1))
% ------------------------------------------------------------
165
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
r20p1 = r21i*(r10p1);
r30p2 = r32i*(r20p2);
r40p3 = r43i*(r30p3);
r50p4 = r54i*(r40p4);
r60p5 = r65i*(r50p5);
r60n6a = r60n6
r50n5a =simplify(ri0nia(r56i, r60n6a, r60f6a, r50n5, r50f5, r60p5,
r50p5,r50s5))
r40n4a =simplify(ri0nia(r45i, r50n5a, r50f5a, r40n4, r40f4, r50p4,
r40p4,r40s4))
r30n3a =simplify(ri0nia(r34i, r40n4a, r40f4a, r30n3, r30f3, r40p3,
r30p3,r30s3))
r20n2a =simplify(ri0nia(r23i, r30n3a, r30f3a, r20n2, r20f2, r30p2,
r20p2,r20s2))
r10n1a =simplify(ri0nia(r12i, r20n2a, r20f2a, r10n1, r10f1, r20p1,
r10p1,r10s1))
% ------------------------------------------------------------
% Fuerzas y pares de accionamientos
% ------------------------------------------------------------
t_1 = simplify(t_r(r10i, r10n1a, qd1i, b1))
t_2 = simplify(t_r(r21i, r20n2a, qd2i, b2))
t_3 = simplify(t_r(r32i, r30n3a, qd3i, b3))
t_4 = simplify(t_r(r43i, r40n4a, qd4i, b4))
t_5 = simplify(t_r(r54i, r50n5a, qd5i, b5))
tau = [t_1; t_2 ; t_3; t_4; t_5]
B.3.2.- La dinámica del robot bípedo 10GDL del pie flotante respecto a la cadera.
%Funcion de Dinámica de NEWTON -EULER para Robot bípedo 10 GDL - Pie
%Flotante a Cadera.
% University National Of Trujillo. Ing. Mechatronics.
% Elaborado Josmell H. Alva Alcántara.
% Fecha: 06-02-2017
%------------------------------------------------------------------------
--
%Ingreso:
% qi: Vector de Angulos de Posición (rad)
% qvi: Vector de Velocidad Articular.
% qai: Vector de Aceleración Articular.
function tau=DinamicaNE_PFC(qi,qvi,qai)
%----------------------------------------------------------------------
%parametro de Denavit-Hartenberg del robot
%----------------------------------------------------------------------
L3=0.0474; %Longitudes en Metros.
166
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%------------------------------------------------------------------------
% Masa de cada elemento en Kg
%------------------------------------------------------------------------
m1=0.0876; %elemento L3 (izquierda)
m2=0.1045; %elemento L4 (izquierda)
m3=0.0876; %elemento L5 (izquierda)
m4=0.0876; %elemento L6 (izquierda)
m5=0.1039; %elemento L7 (izquierda)
% ------------------------------------------------------------
% Coeficiente de rozamiento viscoso de cada articulacion
% ------------------------------------------------------------
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
%------------------------------------------------------------
% Matrices de Inercia (Kg-m^2)
% ------------------------------------------------------------
r10I_r01=[ 0 0 0; 0 m1*(L3*L3)/12 0; 0 0 m1*(L3*L3)/12];
r20I_r02=[ 0 0 0; 0 m2*(L4*L4)/12 0; 0 0 m2*(L4*L4)/12];
r30I_r03=[ 0 0 0; 0 m3*(L5*L5)/12 0; 0 0 m3*(L5*L5)/12];
r40I_r04=[ 0 0 0; 0 m4*(L6*L6)/12 0; 0 0 m4*(L6*L6)/12];
r50I_r05=[ 0 0 0; 0 m4*(L6*L6)/12 0; 0 0 m4*(L6*L6)/12];
% ------------------------------------------------------------
% Matrices de Rotación.
% ------------------------------------------------------------
r01i=denavit(theta(1),d(1),a(1),alpha(1));
r01i=r01i(1:3,1:3);
r10i=r01i';
r12i=denavit(theta(2),d(2),a(2),alpha(2));
r12i=r12i(1:3,1:3);
r21i=r12i';
r23i=denavit(theta(3),d(3),a(3),alpha(3));
r23i=r23i(1:3,1:3);
r32i=r23i';
r34i=denavit(theta(4),d(4),a(4),alpha(4));
r34i=r34i(1:3,1:3);
r43i=r34i';
r45i=denavit(theta(5),d(5),a(5),alpha(5));
r45i=r45i(1:3,1:3);
r54i=r45i';
r56i=eye(3);
167
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% ------------------------------------------------------------
% Vectores ri0pi, ri0si.
% ------------------------------------------------------------
r10p1 = ri0pi(a(1),d(1),alpha(1));
r20p2 = ri0pi(a(2),d(2),alpha(2));
r30p3 = ri0pi(a(3),d(3),alpha(3));
r40p4 = ri0pi(a(4),d(4),alpha(4));
r50p5 = ri0pi(a(5),d(5),alpha(5));
r60p6=zeros(3,1);
% ------------------------------------------------------------
% Velocidad angular de las articulaciones
% ------------------------------------------------------------
r00w0=zeros(3,1); % velocidad inicial =0
r10w1=ri0wi(r10i,r00w0,qvi(1));
r20w2=ri0wi(r21i,r10w1,qvi(2));
r30w3=ri0wi(r32i,r20w2,qvi(3));
r40w4=ri0wi(r43i,r30w3,qvi(4));
r50w5=ri0wi(r54i,r40w4,qvi(5));
r60w6=ri0wi(r65i,r50w5,0);
%------------------------------------------------------------
% Aceleracion angular de las articulaciones
% ------------------------------------------------------------
r00wp0 = zeros(3,1); % ACELERACION INICIAAL = 0
r10wp1 = ri0wpi(r10i, r00wp0,r00w0,qvi(1),qai(1));
r20wp2 = ri0wpi(r21i, r10wp1,r10w1,qvi(2),qai(2));
r30wp3 = ri0wpi(r32i, r20wp2,r20w2,qvi(3),qai(3));
r40wp4 = ri0wpi(r43i, r30wp3,r30w3,qvi(4),qai(4));
r50wp5 = ri0wpi(r54i, r40wp4,r40w4,qvi(5),qai(5));
r60wp6 = ri0wpi(r65i, r50wp5,r50w5,0,0);
% ------------------------------------------------------------
% Aceleracion lineal articular
% ------------------------------------------------------------
g=9.81;
r00vp0 = [g; 0; 0]; % aceleracion de la gravedad
r10vp1 = ri0vpi_r(r10i, r00vp0, r10wp1, r10w1, r10p1);
r20vp2 = ri0vpi_r(r21i, r10vp1, r20wp2, r20w2, r20p2);
r30vp3 = ri0vpi_r(r32i, r20vp2, r30wp3, r30w3, r30p3);
r40vp4 = ri0vpi_r(r43i, r30vp3, r40wp4, r40w4, r40p4);
r50vp5 = ri0vpi_r(r54i, r40vp4, r50wp5, r50w5, r50p5);
r60vp6 = ri0vpi_r(r65i, r50vp5, r60wp6, r60w6, r60p6);
% ------------------------------------------------------------
168
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% ------------------------------------------------------------
% Fuerza en el centro de masa de cada elemento
% ------------------------------------------------------------
% ------------------------------------------------------------
% Par en el centro de masa de cada elemento
% ------------------------------------------------------------
Iext=0; % Momento de Inercia Externo kg.m^2.
r60n6 = ri0ni(r60wp6, r60w6, Iext);
r50n5 = ri0ni(r50wp5, r50w5, r50I_r05);
r40n4 = ri0ni(r40wp4, r40w4, r40I_r04);
r30n3 = ri0ni(r30wp3, r30w3, r30I_r03);
r20n2 = ri0ni(r20wp2, r20w2, r20I_r02);
r10n1 = ri0ni(r10wp1, r10w1, r10I_r01);
% ------------------------------------------------------------
% Fuerzas articulares
% ------------------------------------------------------------
r60f6a = r60f6;
r50f5a = ri0fia(r56i, r60f6a, r50f5);
r40f4a = ri0fia(r45i, r50f5a, r40f4);
r30f3a = ri0fia(r34i, r40f4a, r30f3);
r20f2a = ri0fia(r23i, r30f3a, r20f2);
r10f1a = ri0fia(r12i, r20f2a, r10f1);
% ------------------------------------------------------------
% Pares articulares
% ------------------------------------------------------------
r20p1 = r21i*(r10p1);
r30p2 = r32i*(r20p2);
r40p3 = r43i*(r30p3);
r50p4 = r54i*(r40p4);
r60p5 = r65i*(r50p5);
r60n6a = r60n6;
r50n5a =ri0nia(r56i, r60n6a, r60f6a, r50n5, r50f5, r60p5, r50p5,r50s5);
r40n4a =ri0nia(r45i, r50n5a, r50f5a, r40n4, r40f4, r50p4, r40p4,r40s4);
169
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% ------------------------------------------------------------
% Fuerzas y pares de accionamientos
% ------------------------------------------------------------
t_1 = t_r(r10i, r10n1a, qi(1), b1);
t_2 = t_r(r21i, r20n2a, qi(2), b2);
t_3 = t_r(r32i, r30n3a, qi(3), b3);
t_4 = t_r(r43i, r40n4a, qi(4), b4);
t_5 = t_r(r54i, r50n5a, qi(5), b5);
tau = [t_1; t_2 ; t_3; t_4; t_5];
end
function tau=DinamicaNE_CPA(qd,qvd,qad)
%----------------------------------------------------------------------
%parametro de Denavit-Hartenberg del robot
%----------------------------------------------------------------------
L6=0.0468; %Longitudes en Metros.
L5=0.0473;
L4=0.064;
L3=0.0474;
%------------------------------------------------------------------------
% 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=0; %elemento L2 (izquierda)(Se lo va Incluir en la Masa Externa)
170
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% ------------------------------------------------------------
% Coeficiente de rozamiento viscoso de cada articulacion
% ------------------------------------------------------------
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
%------------------------------------------------------------
% Matrices de Inercia (Kg-m^2)
% ------------------------------------------------------------
r10I_r01=[ 0 0 0; 0 m1*(L6*L6)/12 0; 0 0 m1*(L6*L6)/12];
r20I_r02=[ 0 0 0; 0 m2*(L5*L5)/12 0; 0 0 m2*(L5*L5)/12];
r30I_r03=[ 0 0 0; 0 m3*(L4*L4)/12 0; 0 0 m3*(L4*L4)/12];
r40I_r04=[ 0 0 0; 0 m4*(L3*L3)/12 0; 0 0 m4*(L3*L3)/12];
r50I_r05=[ 0 0 0; 0 0 0; 0 0 0];
% ------------------------------------------------------------
% Matrices de Rotación.
% ------------------------------------------------------------
r01d=denavit(theta(1),d(1),a(1),alpha(1));
r01d=r01d(1:3,1:3);
r10d=r01d';
r12d=denavit(theta(2),d(2),a(2),alpha(2));
r12d=r12d(1:3,1:3);
r21d=r12d';
r23d=denavit(theta(3),d(3),a(3),alpha(3));
r23d=r23d(1:3,1:3);
r32d=r23d';
r34d=denavit(theta(4),d(4),a(4),alpha(4));
r34d=r34d(1:3,1:3);
r43d=r34d';
r45d=denavit(theta(5),d(5),a(5),alpha(5));
r45d=r45d(1:3,1:3);
r54d=r45d';
r56d=eye(3);
r65d=r56d';
% ------------------------------------------------------------
% Vectores ri0pi, ri0si.
% ------------------------------------------------------------
r10p1 = ri0pi(a(1),d(1),alpha(1));
r20p2 = ri0pi(a(2),d(2),alpha(2));
r30p3 = ri0pi(a(3),d(3),alpha(3));
r40p4 = ri0pi(a(4),d(4),alpha(4));
r50p5 = ri0pi(a(5),d(5),alpha(5));
r60p6=zeros(3,1);
171
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% ------------------------------------------------------------
% Velocidad angular de las articulaciones
% ------------------------------------------------------------
r00w0=zeros(3,1); % velocidad inicial =0
r10w1=ri0wi(r10d,r00w0,qvd(1));
r20w2=ri0wi(r21d,r10w1,qvd(2));
r30w3=ri0wi(r32d,r20w2,qvd(3));
r40w4=ri0wi(r43d,r30w3,qvd(4));
r50w5=ri0wi(r54d,r40w4,qvd(5));
r60w6=ri0wi(r65d,r50w5,0);
% ------------------------------------------------------------
% Aceleracion angular de las articulaciones
% ------------------------------------------------------------
r00wp0 = zeros(3,1); % ACELERACION INICIAL = 0
r10wp1 = ri0wpi(r10d, r00wp0,r00w0,qvd(1),qad(1));
r20wp2 = ri0wpi(r21d, r10wp1,r10w1,qvd(2),qad(2));
r30wp3 = ri0wpi(r32d, r20wp2,r20w2,qvd(3),qad(3));
r40wp4 = ri0wpi(r43d, r30wp3,r30w3,qvd(4),qad(4));
r50wp5 = ri0wpi(r54d, r40wp4,r40w4,qvd(5),qad(5));
r60wp6 = ri0wpi(r65d, r50wp5,r50w5,0,0);
% ------------------------------------------------------------
% Aceleracion lineal articular
% ------------------------------------------------------------
g=9.81;
r00vp0 = [-g; 0; 0]; % aceleracion de la gravedad
r10vp1 = ri0vpi_r(r10d, r00vp0, r10wp1, r10w1, r10p1);
r20vp2 = ri0vpi_r(r21d, r10vp1, r20wp2, r20w2, r20p2);
r30vp3 = ri0vpi_r(r32d, r20vp2, r30wp3, r30w3, r30p3);
r40vp4 = ri0vpi_r(r43d, r30vp3, r40wp4, r40w4, r40p4);
r50vp5 = ri0vpi_r(r54d, r40vp4, r50wp5, r50w5, r50p5);
r60vp6 = ri0vpi_r(r65d, r50vp5, r60wp6, r60w6, r60p6);
% ------------------------------------------------------------
% Aceleracion del centro de masa de cada elemento
% ------------------------------------------------------------
r10a1 = ri0ai(r10vp1, r10wp1, r10w1, r10s1);
r20a2 = ri0ai(r20vp2, r20wp2, r20w2, r20s2);
r30a3 = ri0ai(r30vp3, r30wp3, r30w3, r30s3);
r40a4 = ri0ai(r40vp4, r40wp4, r40w4, r40s4);
r50a5 = ri0ai(r50vp5, r50wp5, r50w5, r50s5);
r60a6 = ri0ai(r60vp6, r60wp6, r60w6, r60s6);
% ------------------------------------------------------------
% Fuerza en el centro de masa de cada elemento
% ------------------------------------------------------------
Mext=0.5197; % Masa de la Pierna Flotante,incluye la cadera.
r60f6 = ri0fi(r60a6, Mext); % fuerza inicial de carga
r50f5 = ri0fi(r50a5, m5);
r40f4 = ri0fi(r40a4, m4);
172
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
% ------------------------------------------------------------
% Par en el centro de masa de cada elemento
% ------------------------------------------------------------
Lext=(0.342+0.0288);
Iext=[Mext*(Lext*Lext)/12 0 0; 0 Mext*(Lext*Lext)/12 0;0 0
Mext*(Lext*Lext)/12]; %Momentos de Inercia Generado por la Pierna
Flotante.
r60n6 = ri0ni(r60wp6, r60w6, Iext);
r50n5 = ri0ni(r50wp5, r50w5, r50I_r05);
r40n4 = ri0ni(r40wp4, r40w4, r40I_r04);
r30n3 = ri0ni(r30wp3, r30w3, r30I_r03);
r20n2 = ri0ni(r20wp2, r20w2, r20I_r02);
r10n1 = ri0ni(r10wp1, r10w1, r10I_r01);
% ------------------------------------------------------------
% Fuerzas articulares
% ------------------------------------------------------------
r60f6a = r60f6;
r50f5a = ri0fia(r56d, r60f6a, r50f5);
r40f4a = ri0fia(r45d, r50f5a, r40f4);
r30f3a = ri0fia(r34d, r40f4a, r30f3);
r20f2a = ri0fia(r23d, r30f3a, r20f2);
r10f1a = ri0fia(r12d, r20f2a, r10f1);
% ------------------------------------------------------------
% Pares articulares
% ------------------------------------------------------------
r20p1 = r21d*(r10p1);
r30p2 = r32d*(r20p2);
r40p3 = r43d*(r30p3);
r50p4 = r54d*(r40p4);
r60p5 = r65d*(r50p5);
r60n6a = r60n6;
r50n5a =ri0nia(r56d, r60n6a, r60f6a, r50n5, r50f5, r60p5, r50p5,r50s5);
r40n4a =ri0nia(r45d, r50n5a, r50f5a, r40n4, r40f4, r50p4, r40p4,r40s4);
r30n3a =ri0nia(r34d, r40n4a, r40f4a, r30n3, r30f3, r40p3, r30p3,r30s3);
r20n2a =ri0nia(r23d, r30n3a, r30f3a, r20n2, r20f2, r30p2, r20p2,r20s2);
r10n1a =ri0nia(r12d, r20n2a, r20f2a, r10n1, r10f1, r20p1, r10p1,r10s1);
% ------------------------------------------------------------
% Fuerzas y pares de accionamientos
% ------------------------------------------------------------
t_5d = t_r(r10d, r10n1a, qd(1), b1);
t_4d = t_r(r21d, r20n2a, qd(2), b2);
t_3d = t_r(r32d, r30n3a, qd(3), b3);
t_2d = t_r(r43d, r40n4a, qd(4), b4);
t_1d = t_r(r54d, r50n5a, qd(5), b5);
173
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
end
for i=1:101;
if i==1;
vpf(5,i)=0;
end
if i>=2
vpf(:,i)=(qpf2(:,i)- qpf2(:,i-1))/t;
end
end
% Cálculo de Aceleraciones.
for i=1:101;
if i==1;
apf(:,i)=vpf(:,i)/t;
end
if i>=2
apf(:,i)=(vpf(:,i)-vpf(:,i-1))/t;
end
end
%}
%------------------------------------------------------------------------
--
% DE CADERA A PIE DE APOYO.
%{
for i=1:101;
if i==1;
vcadera(5,i)=0;
end
174
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
end
% Cálculo de Aceleraciones.
for i=1:101;
if i==1;
acadera(:,i)=vcadera(:,i)/t;
end
if i>=2
acadera(:,i)=(vcadera(:,i)-vcadera(:,i-1))/t;
end
end
for i=1:101
torquesCPA(:,i)=DinamicaNE_CPA(qcadera2(:,i),vcadera(:,i),acadera(:,i));
end
%}
B. 3.5 Script para las gráficas de torques de pie flotante, velocidad y aceleraciones
articulares del robot bípedo.
% Graficas de Torques Pie Flotante, Velocidad y Aceleraciones ROBOT
BÍPEDO.
% University National of Trujillo - Ing. Mechatronics.
% Elaborado by Josmell Alva Alcántara.
% Fecha: 06-02-2017
%------------------------------------------------------------------------
--
t=0:0.01:1;
for i=1:101
torque1(i)=torquesPF(1,i);
vqiz1(i)=vpf(1,i);
aqiz1(i)=apf(1,i)/10;
Pot1i(i)= torquesPF(1,i)*vpf(1,i);
end
for i=1:101
torque2(i)=torquesPF(2,i);
vqiz2(i)=vpf(2,i);
aqiz2(i)=apf(2,i)/10;
Pot2i(i)= torquesPF(2,i)*vpf(2,i);
175
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%Gráfica de Torques.
plot(t,torque1,'r',t,torque2,'b',t,torque3,'g',t,torque4,'k',t,torque5,'m
')
title('ROBOT BIPEDO: GRÁFICO DE TORQUES PIE FLOTANTE - CADERA.')
ylabel('N*m')
xlabel('Tiempo')
legend('Torque 1i','Torque 2i','Torque 3i','Torque 4i','Torque 5i')
% Gráfica de Velocidades.
figure
plot(t,vqiz1,'r',t,vqiz2,'b',t,vqiz3,'g',t,vqiz4,'k',t,vqiz5,'m')
legend('Velocidad 1i','Velocidad 2i','Velocidad 3i','Velocidad
4i','Velocidad 5i')
ylabel('rad/s')
xlabel('tiempo')
title('ROBOT BÍPEDO: GRÁFICO DE VELOCIDADES PIE FLOTANTE-CADERA')
%Gráfica de Aceleraciones.
figure
plot(t,aqiz1,'r',t,aqiz2,'b',t,aqiz3,'g',t,aqiz4,'k',t,aqiz5,'m')
legend('Aceleración 1i','Aceleración 2i','Aceleración 3i','Aceleración
4i','Aceleración 5i')
ylabel('rad/s^2')
xlabel('Tiempo')
title('ROBOT BÍPEDO: GRÁFICO DE ACELERACIONES PE FLOTANTE-CADERA')
%Gráfica de Potencia.
figure
plot(t,Pot1i,'r',t,Pot2i,'b',t,Pot3i,'g',t,Pot4i,'k',t,Pot5i,'m')
legend('Potencia 1i','Potencia 2i','Potencia 3i','Potencia 4i','Potencia
5i')
ylabel('Watts')
xlabel('tiempo')
title('ROBOT BÍPEDO: GRÁFICO DE POTENCIAS DEL PIE FLOTANTE')
%------------------------------------------------------------------------
--
176
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%{
% DE CADERA A PIE DE APOYO
for i=1:101
tq5d(i)=torquesCPA(1,i);
vqd5(i)=vcadera(1,i);
aqd5(i)=acadera(1,i);
Pot5d(i)=torquesCPA(1,i)*vcadera(1,i);
end
for i=1:101
tq4d(i)=torquesCPA(2,i);
vqd4(i)=vcadera(2,i);
aqd4(i)=acadera(2,i);
Pot4d(i)=torquesCPA(2,i)*vcadera(2,i);
end
for i=1:101
tq3d(i)=torquesCPA(3,i);
vqd3(i)=vcadera(3,i);
aqd3(i)=acadera(3,i);
Pot3d(i)=torquesCPA(3,i)*vcadera(3,i);
end
for i=1:101
tq2d(i)=torquesCPA(4,i);
vqd2(i)=vcadera(4,i);
aqd2(i)=acadera(4,i);
Pot2d(i)=torquesCPA(4,i)*vcadera(4,i);
end
for i=1:101
tq1d(i)=torquesCPA(5,i);
vqd1(i)=vcadera(5,i);
aqd1(i)=acadera(5,i);
Pot1d(i)=torquesCPA(5,i)*vcadera(5,i);
end
%Gráfica de Torques.
plot(t,tq5d,'r',t,tq4d,'b',t,tq3d,'g',t,tq2d,'k',t,tq1d,'m')
title('ROBOT BIPEDO: GRÁFICO DE TORQUES CADERA A PIE DE APOYO')
ylabel('N*m')
xlabel('Tiempo')
legend('Torque 5d','Torque 4d','Torque 3d','Torque 2d','Torque 1d')
% Gráfica de Velocidades.
figure
plot(t,vqd5,'r',t,vqd4,'b',t,vqd3,'g',t,vqd2,'k',t,vqd1,'m')
legend('Velocidad 5d','Velocidad 4d','Velocidad 3d','Velocidad
2d','Velocidad 1d')
ylabel('rad/s')
xlabel('tiempo')
title('ROBOT BÍPEDO: GRÁFICO DE VELOCIDADES CADERA A PIE DE APOYO')
%Gráfica de Aceleraciones.
figure
plot(t,aqd5,'r',t,aqd4,'b',t,aqd3,'g',t,aqd2,'k',t,aqd1,'m')
legend('Aceleración 5d','Aceleración 4d','Aceleración 3d','Aceleración
2d','Aceleración 1d')
177
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%Gráfica de Potencia.
figure
plot(t,Pot5d,'r',t,Pot4d,'b',t,Pot3d,'g',t,Pot2d,'k',t,Pot1d,'m')
legend('Potencia 5d','Potencia 4d','Potencia 3d','Potencia 2d','Potencia
1d')
ylabel('Watts')
xlabel('tiempo')
title('ROBOT BÍPEDO: GRÁFICO DE POTENCIAS CADERA A PIE DE APOYO')
%}
178
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%ii)
x0=h+hc;
x1=h;
v0=0;
v1=0;
Ti=Tp/2;
t2=0:0.01:Tp/2;
xc2_PS=x0+v0*t2+[(3*(x1-x0)-Ti*(2*v0+v1))/Ti^2]*t2.^2+ [(2*(x0-x1)+
Ti*(v0+v1))/Ti^3]*t2.^3;
for(i=1:101)
if(i<=50)
xc_ps(i)=xc1_PS(i);
end
if(i>50)
xc_ps(i)=xc2_PS(i-50);
end
end
figure
plot(t,xc_ps)
title('Planificación de la Cadera en el Eje "X" -PLANO SAGITAL')
grid on
xlabel('TP(s)')
ylabel('X cadera(metros)')
%------------------------------------------------------------------------
-----------------------
%Eje "Y":
%PLANO FRONTAL
lc=0.0452; % longitud del centro a unos de los extremos de la cadera.
dc=-0.035; % Sobre paso del punto central de la cadera sobre el eje "Y".
dp=0.080; % distancia entre los pies.
vyc=0; % Velocidad de la cadera al Inicio y final del Intervalo.
%i)
y0=-lc;
y1=dc;
v0=vyc;
v1=0;
Ti=Tp/2;
t1=0:0.01:Tp/2;
yc1_PF=y0+v0*t1+[(3*(y1-y0)-Ti*(2*v0+v1))/Ti^2]*t1.^2+ [(2*(y0-y1)+
Ti*(v0+v1))/Ti^3]*t1.^3;
179
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%ii)
y0=dc;
y1=-lc;
v0=0;
v1=-vyc;
Ti=Tp/2;
yc2_PF=y0+v0*t2+[(3*(y1-y0)-Ti*(2*v0+v1))/Ti^2]*t2.^2+ [(2*(y0-y1)+
Ti*(v0+v1))/Ti^3]*t2.^3;
for (i=1:101)
if(i<=50)
yc_pf(i)=yc1_PF(i);
end
if(i>50)
yc_pf(i)=yc2_PF(i-50);
end
end
figure
plot(t,yc_pf)
title('Planificación de la Cadera en el Eje "Y" - Plano Frontal')
grid on
xlabel('TP(s)')
ylabel('Y cadera(metros)')
%------------------------------------------------------------------------
---------------------
%PIERNA FLOTANTE.
%------------------------------------------------------------------------
--------------------
%doble soporte.
%En el EJE "Z"
Tds=0.3*Tp;
Tss=0.7*Tp;
z0=-Lp;
z1=Lp;
v0=0;
v1=0;
Ti=Tss;
t5=0:0.01:Tss;
zpf_Ps=z0+v0*t5+[(3*(z1-z0)-Ti*(2*v0+v1))/Ti^2]*t5.^2+ [(2*(z0-z1)+
Ti*(v0+v1))/Ti^3]*t5.^3;
for i=1:101
if(i<=15)
zpf_ps(i)=-Lp;
end
if(i>15 && i<=86)
zpf_ps(i)=zpf_Ps(i-15);
end
if(i>86 && i<=101)
zpf_ps(i)=Lp;
end
end
figure
plot(t,zpf_ps)
180
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
x0=hp;
x1=0;
v0=0;
v1=0;
Ti=Tss/2;
xpf_Ps1=x0+v0*t1+[(3*(x1-x0)-Ti*(2*v0+v1))/Ti^2]*t1.^2+ [(2*(x0-x1)+
Ti*(v0+v1))/Ti^3]*t1.^3;
for i=1:101
if(i<=15)
xpf_ps(i)=0;
end
if(i>15 && i<=50)
xpf_ps(i)=xpf_Ps(i-15);
end
if(i>50 && i<=85)
xpf_ps(i)=xpf_Ps1(i-50);
end
if(i>85)
xpf_ps(i)=0;
end
end
figure
plot(t,xpf_ps)
title('PLANIFICACIÓN DE LA PIERNA FLOTANTE EN EL EJE "X" - PLANO
SAGITAL');
grid on
xlabel('TP(s)')
ylabel('X PieFlotante(metros)')
%------------------------------------------------------------------------
----------------------
%En el EJE "Y"
y0=-2*lc;
y1=-dp;
v0=0;
v1=0;
Ti=Tss/2;
t1=0:0.01:Tss/2;
181
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
y0=-dp;
y1=-2*lc;
v0=0;
v1=0;
Ti=Tss/2;
t1=0:0.01:Tss/2;
ypf2_PF=y0+v0*t1+[(3*(y1-y0)-Ti*(2*v0+v1))/Ti^2]*t1.^2+ [(2*(y0-y1)+
Ti*(v0+v1))/Ti^3]*t1.^3;
for i=1:101
if(i<=15)
ypf_pf(i)=-2*lc;
end
if(i>15 && i<=50)
ypf_pf(i)=ypf1_PF(i-15);
end
if(i>50 && i<=85)
ypf_pf(i)=ypf2_PF(i-50);
end
if(i>85)
ypf_pf(i)=-2*lc;
end
end
figure
plot(t,ypf_pf)
title('Planificación de la Pierna Flotante en el "Y" -PLANO FRONTAL');
grid on
xlabel('TP(s)')
ylabel('Y PieFlotante(metros)')
%------------------------------------------------------------------------
---------------------
%INICIO DE MARCHA.
% SOLO SE PLANIFICA EN EL EJE "Z"
%CADERA
z0=0;
z1= Lp/2;
v0=0;
v1=0;
Ti=Tp;
t=0:0.01:Tp;
zc_start=z0+v0*t+[(3*(z1-z0)-Ti*(2*v0+v1))/Ti^2]*t.^2+ [(2*(z0-z1)+
Ti*(v0+v1))/Ti^3]*t.^3;
figure
plot(t,zc_start)
title('Planificación Cadera en el EJE "Z" - INICIO DE MARCHA');
grid on
xlabel('TP(s)')
ylabel('Z cadera (metros)')
%PIERNA FLOTANTE
z0=0;
182
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%PIERNA FLOTANTE
z0=-Lp;
z1= 0;
v0=0;
v1=0;
Ti=Tp;
zpf_end=z0+v0*t+[(3*(z1-z0)-Ti*(2*v0+v1))/Ti^2]*t.^2+ [(2*(z0-z1)+
Ti*(v0+v1))/Ti^3]*t.^3;
figure
plot(t,zpf_end)
title('Planificación Pie flotante en el EJE "Z"- FIN DE MARCHA');
grid on
xlabel('TP(s)')
ylabel('Z Pierna Flotante (metros)')
for i=1:101
yc_aux(i)=-0.0452;
end
%------------------------------------------------------------------------
--------
%RESUMEN PLANIFCACION CAMINATA:
PosCadera=[xc_ps; yc_pf; zc_PS];
PosPieF= [ xpf_ps; ypf_pf; zpf_ps;];
%------------------------------------------------------------------------
-----
183
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
end
%Fin de Marcha.
for i=1:101
qc_end2(:,i)=InversekinematicBiped_1(1000*PosCadera_end(:,i));
end
% En Plena caminata.
%for i=1:101
% qcadera2(:,i)=InversekinematicBiped_1(1000*PosCadera(:,i));
%end
%........................................................................
..
%Angulos de PIE FLOTANTE RESPECTO AL PIE DE APOYO.
%Inicio de Marcha:
for i=1:101
qpf_start2(:,i)=InversekinematicBiped_3(1000*PosPieF_star(:,i));
end
%Fin de Marcha
for i=1:101
qpf_end2(:,i)=InversekinematicBiped_3(1000*PosPieF_end(:,i));
end
%for i=1:101
% qpf2(:,i)=InversekinematicBiped_3(1000*PosPieF(:,i));
%end
184
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
for i=1:n
q4d(i)=qcadera2(2,i);
end
%q4d=q4d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q4d)
title('Pierna de apoyo q4d- Plena Marcha.');
xlabel(' Número de divisiones tiempo(s) ')
ylabel('q4d (radianes)')
for i=1:n
q3d(i)=qcadera2(3,i);
end
%q3d=q3d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q3d)
title('Pierna de apoyo q3d - Plena Marcha.');
xlabel(' Número de divisiones tiempo(s) ')
ylabel('q3d (radianes)')
for i=1:n
q2d(i)=qcadera2(4,i);
end
%q2d=q2d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q2d)
title('Pierna de apoyo q2d- Plena Marcha.');
xlabel(' Número de divisiones tiempo (s) ')
ylabel('q2d (radianes)')
for i=1:n
q1d(i)=qcadera2(5,i);
end
%q1d=q1d*180/pi; %de radianes a Sexagesimales.
185
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
for i=1:n
q2i(i)=qpf2(2,i);
end
%q2i=q2i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q2i)
title('Pierna flotante q2i - Plena Marcha.');
xlabel(' Número de divisiones tiempo (s) ')
ylabel('q2i (radianes)')
for i=1:n
q3i(i)=qpf2(3,i);
end
q3i=q3i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q3i)
title('Pierna flotante q3i- Plena Marcha.');
xlabel(' Número de divisiones tiempo (s)')
ylabel('q3i (radianes)')
for i=1:n
q4i(i)=qpf2(4,i);
end
q4i=q4i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q4i)
title('Pierna flotante q4i- Plena Marcha.');
xlabel(' Número de divisiones tiempo (s) ')
ylabel('q4i (radianes)')
for i=1:n
q5i(i)=qpf2(5,i);
end
q5i=q5i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q5i)
title('Pierna flotante 5i- Plena Marcha.');
xlabel(' Número de divisiones tiempo (s)')
ylabel('q5i (radianes)')
186
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%{
%----------------------------------------------------------------------
%Inicio de Marcha.
%Cadera a pie de apoyo.
for i=1:n
q5d(i)=qc_start2(1,i);
end
q5d=q5d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q5d)
title('q5d en radianes - Inicio de Marcha');
for i=1:n
q4d(i)=qc_start2(2,i);
end
q4d=q4d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q4d)
title('q4d en radianes - Inicio de Marcha');
for i=1:n
q3d(i)=qc_start2(3,i);
end
q3d=q3d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q3d)
title('q3d en radianes - Inicio de Marcha');
for i=1:n
q2d(i)=qc_start2(4,i);
end
q2d=q2d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q2d)
title('q2d en radianes - Inicio de Marcha');
for i=1:n
q1d(i)=qc_start2(5,i);
end
q1d=q1d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q1d)
title('q1d en radianes -Inicio de Marcha');
for i=1:n
q2i(i)=qpf_start2(2,i);
187
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
for i=1:n
q3i(i)=qpf_start2(3,i);
end
q3i=q3i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q3i)
title('q3i en radianes - Inicio de Marcha.');
for i=1:n
q4i(i)=qpf_start2(4,i);
end
q4i=q4i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q4i)
title('q4i en radianes - Inicio de Marcha.');
for i=1:n
q5i(i)=qpf_start2(5,i);
end
q5i=q5i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q5i)
title('q5i en radianes - Inicio de Marcha.');
%.-----------------------------------------------------------------------
---------------------
% Fin de Marcha.
% Cadera al Pie de apoyo.
for i=1:n
q5d(i)=qc_end2(1,i);
end
q5d=q5d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q5d)
title('q5d en radianes - Fin de Marcha.');
for i=1:n
q4d(i)=qc_end2(2,i);
end
q4d=q4d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q4d)
title('q4d en radianes - Fin de Marcha.');
for i=1:n
q3d(i)=qc_end2(3,i);
end
q3d=q3d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q3d)
188
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
for i=1:n
q2d(i)=qc_end2(4,i);
end
q2d=q2d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q2d)
title('q2d en radianes - Fin de Marcha.');
for i=1:n
q1d(i)=qc_end2(5,i);
end
q1d=q1d*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q1d)
title('q1d en radianes - Fin de Marcha.');
for i=1:n
q1i(i)=qpf_end2(1,i);
end
q1i=q1i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q1i)
title('q1i en radianes - Fin de Marcha.');
for i=1:n
q2i(i)=qpf_end2(2,i);
end
q2i=q2i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q2i)
title('q2i en radianes - Fin de Marcha.');
for i=1:n
q3i(i)=qpf_end2(3,i);
end
q3i=q3i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q3i)
title('q3i en radianes - Fin de Marcha.');
for i=1:n
q4i(i)=qpf_end2(4,i);
end
q4i=q4i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q4i)
title('q4i en radianes - Fin de Marcha.');
for i=1:n
q5i(i)=qpf_end2(5,i);
end
189
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
%}
B.5.1.- Función que permite calcular la velocidad y aceleración del COG. Del robot
bípedo.
function [Vcmx,Vcmy,Vcmz,acmx,acmy,acmz,P]=VelAcelCOG(qd,qvd,qad,iri)
%Longitudes en Metros.
L7=0.0174;
L6=0.0468;
L5=0.0473;
L4=0.064;
L3=0.0473;
L2=0.0288;
L1=0.0452;
T43d=denavit(theta(1),d(1),a(1),alfa(1));
T32d=denavit(theta(2),d(2),a(2),alfa(2));
T21d=denavit(theta(3),d(3),a(3),alfa(3));
T10d=denavit(theta(4),d(4),a(4),alfa(4));
T00d=denavit(theta(5),d(5),a(5),alfa(5));
T54d=[1 0 0 L7;
0 1 0 0;
0 0 1 0;
0 0 0 1]; % Matriz de tralacion del pie(base) al 1 eje.
T0Cd=[1 0 0 L2;
190
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
Qi=[0 -1 0 0;
1 0 0 0;
0 0 0 0;
0 0 0 0];
% Velocidad de cada Articulacion:
Vcmx= v5C(1,1);
Vcmy= v5C(2,1);
Vcmz= v5C(3,1);
a5C=(T54d*Qi*Qi*T43d*T32d*T21d*T10d*T00d*T0Cd*qvd(1)*qad(1) +...
T54d*T43d*Qi*Qi*T32d*T21d*T10d*T00d*T0Cd*qvd(2)*qad(2) +...
T54d*T43d*T32d*Qi*Qi*T21d*T10d*T00d*T0Cd*qvd(3)*qad(3) +...
T54d*T43d*T32d*T21d*Qi*Qi*T10d*T00d*T0Cd*qvd(4)*qad(4) +...
T54d*T43d*T32d*T21d*T10d*Qi*Qi*T00d*T0Cd*qvd(5)*qad(5))*iri;
acmx= a5C(1,1);
acmy= a5C(2,1);
acmz= a5C(3,1);
end
B.5.2.- Script para el cálculo del ZMP y las gráficas de velocidades y aceleraciones del
COG.
191
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
iri=[0;0;0;1];
for i=1:101
[Vcmx1(:,i),Vcmy1(:,i),Vcmz1(:,i),acmx1(:,i),acmy1(:,i),acmz1(:,i),P1(:,i
)]=VelAcelCOG(qcadera2(:,i),vcadera(:,i),acadera(:,i),iri);
end
%Graficas de Velocidad del Centro de Masa y Aceleración de Centro de
Masa.
t=0:0.01:1;
plot(t,Vcmx1,'r',t,Vcmy1,'k',t,Vcmz1,'g')
xlabel('tiempo')
ylabel('Velocidad en m/s')
title('Velocidades del COG - Robot Bípedo')
legend('Velocidad en el Eje X',' Velocidad en el Eje Y',' Velocidad en el
Eje Z')
figure
plot(t,acmx1,'r',t,acmy1,'k',t,acmz1,'g')
xlabel('tiempo')
ylabel('Aceleración en m/s')
title('Aceleracion del COG - Robot Bípedo')
legend('Aceleración en el Eje X',' Aceleración en el Eje Y','Aceleración
en el Eje Z')
for i=1:101
Zzmp(i)= P1(3,i)+(P1(3,i)*acmx1(i)-P1(1,i)*acmz1(i))/g;
Yzmp(i)= P1(2,i)+(P1(1,i)*acmy1(i)-P1(2,i)*acmx1(i))/g+0.0452;
end
figure
plot(Yzmp,Zzmp,'r')
title('Cálculo del ZMP del Robot Bípedo')
xlabel(' Eje Y(m)')
ylabel('Eje Z(m)')
axis([-0.030,0.035,-0.060,0.060])
192
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
193
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
194
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
195
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
196
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
197
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
198
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
199
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
200
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/
BIBLIOTECA DIGITAL - DIRECCIÓN DE SISTEMAS DE INFORMÁTICA Y COMUNICACIÓN
m1 = 0.0876 Kg.
m2 = 0.1045 Kg.
m3 = 0.0876 Kg.
m4 = 0.0876 Kg.
m5 = 0.1039 Kg
Las masa están referidas al peso de las piezas que forman los eslabones (incluyendo el peso de
servomotor Mg966R = 55gr), m1 representa a las longitud L3, L4, L5, L6, L7 respectivamente.
Las masas dadas del robot bípedo está calculado en base al material Aleación de Aluminio 1060.
201
Esta obra ha sido publicada bajo la licencia Creative Commons Reconocimiento-No Comercial-Compartir bajo la misma licencia 2.5 Perú.
Para ver una copia de dicha licencia, visite http://creativecommons.org/licences/by-nc-sa/2.5/pe/