Está en la página 1de 210

UNIVERSIDAD NACIONAL DE TRUJILLO

UNIVERSIDAD NACIONAL DE TRUJILLO


Facultad de Ingeniera
Escuela Profesional de Ingeniera Mecatrnica

DISEO DE UN PROTOTIPO DE ROBOT BPEDO CON PLANIFICACIN DE


TRAYECTORIAS CON CRITERIO ZMP PARA SU ESTABILIDAD AL CAMINAR
SOBRE SUPERFICIE PLANA

TESIS

Para optar el Ttulo Profesional de


INGENIERO MECATRNICO

AUTOR: Br. Josmell Henry Alva Alcntara.

ASESOR: Mg. Ing. Edward Javier Len Lescano.

Trujillo, Per
2017
UNIVERSIDAD NACIONAL DE TRUJILLO

No hay azar, destino, ni suerte que pueda con la firme resolucin de un alma decidida.
Ella Wheeler Wilcox.

I
UNIVERSIDAD NACIONAL DE TRUJILLO

DEDICATORIA

Dedico el presente trabajo a mis padres Francisco, Mirian y a mis hermanos,


por su compresin y ayuda en cada momento difcil. A mis familiares, amigos, y docentes
que tuvieron palabras de apoyo en todo momento desde el inicio de esta tesis.

II
UNIVERSIDAD NACIONAL DE TRUJILLO

AGRADECIMIENTO.

Quiero agradecer a aquellas personas que compartieron sus conocimientos


conmigo para hacer posible la conclusin de esta tesis. Especialmente agradezco a mi
asesor el Ing. Javier Len Lescano, que mostr su apoyo como docente y asesor en el
desarrollo de este trabajo. Agradecer a todos mis compaeros las cuales compartimos
experiencias e investigacin en esta etapa importante de mi vida.

III
UNIVERSIDAD NACIONAL DE TRUJILLO
INDICE
I.- CAPITULO 1: INTRODUCCIN
1.1 Realidad Problemtica.1
1.2 Trabajos Previos2
1.2.1 Robot Asimov.2
1.2.2 Robot Hubo2
1.2.3 Robot Sdr-4x: Qrio ...3
1.2.4 Robot Nao..3
1.2.5 Tesis: Diseo y Control de Kokone, un pequeo robot humanoide.4
1.2.6 Tesis Construccin de un robot Bpedo, Basado en Caminado
Dinmico4
1.2.7 Tesis Mecatrnica Bio Inspirada de Robots humanoides de
Tamao Natural.5
1.28. Robots humanoides en Per..5
1.3. Marco Terico..6
1.3.1 Cinemtica del Robot Bpedo. ..6
1.3.1.1 Cinemtica Directa del Robot Bpedo..6
1.3.1.2 Cinemtica inversa del Robot bpedo..8
1.3.2 Anlisis Dinmico9
1.3.2.1 Dinmica Newton-Euler 10
1.3.2.2 Sistemas de Coordenadas rotantes10
1.3.2.3 Cinemtica de los elementos del robot...11
1.3.2.4 Ecuaciones de movimiento recursivas para
Manipuladores.13
1.3.3 Planificacin de trayectorias.14
1.3.3.1 Locomocin Humana.15
1.3.3.2 Ciclo de Marcha..15
1.3.2.3 Fases de la Marcha16
1.3.2.4. Locomocin Pasiva...17
1.3.2.5 Locomocin Activa.17
1.3.4 Punto de Momento Cero, ZMP.18
1.3.5.- Arquitectura de la Pierna Humana19
1.4. Formulacin del Problema...20
1.5 Justificacin del Estudio20
1.6 Hiptesis..21
1.7 Objetivos..21
1.7.1 Objetivo General.21
1.7.2 Objetivos especficos.21

II CAPITULO: MTODO
2.1 Diseo de investigacin23
2.2. Variables.
2.2.1 Variables Independiente......23
2.2.2 Variables dependientes....23
2.3 Poblacin y muestra..23
2.4 Tcnicas e Instrumentos..23.

IV
UNIVERSIDAD NACIONAL DE TRUJILLO
III CAPITULO: RESULTADOS.
3.1 Cinemtica directa del robot bpedo 10GDL 24
3.1.1 Cinemtica directa de la cadera con respecto al pie de apoyo..26
3.1.2 Cinemtica directa del pie flotante con respecto a la cadera. 31
3.2 Cinemtica inversa del robot bpedo 10GDL... 34
3.2.1 Paso a seguir en el algoritmo implementado.34
3.2.2 Cinemtica inversa de la cadera del robot bpedo 10GDL..38
3.2.3 Cinemtica inversa del pie flotante del robot bpedo 10GL.42
3.3 Anlisis Dinmico...45
3.3.1 Algoritmo computacional.. 45
3.3.2 Dinmica Newton Euler del robot bpedo 10 GDL.. 47
3.3.3 Dinmica del pie flotante respecto a la cadera 47
3.3.3 Dinmica de la cadera respecto al pie de apoyo..52
3.4 Arquitectura de robot bpedo 10GDL. 57
3.4.1. Modelo CAD 3D del robot bpedo 10GDL... 57
3.4.2. Modelo 3D Simplificado del robot bpedo de 10GDL 58
3.5 Planificacin de la caminata de robot bpedo 10GDL..59
3.5.1 Control cinemtico.59
3.5.2 Trayectorias robot bpedo 10GDL.. 59
3.5.3 Interpolacin de trayectorias59
3.5.4 Planificacin de la cadera respecto al pie de apoyo... 61
3.5.4.1 Plano sagital... 61
3.5.4.2 Plano frontal... 64
3.5.5 Planificacin del pierna flotante.. 66
3.5.6 Inicio y parada del robot bpedo 10GDL. 69
3.5.6.1 Paso de inicio ..70
3.5.6.2 Paso de fin.. 71
3.6.- Criterio de estabilidad ZMP 76
3.6.1 ZMP, robot bpedo de 10GDL .78
3.7 Simulacin de la caminata del robot Bpedo 10GDL ...84
3.7.1 Simulacin del Robot Bpedo 10GDL..85
3.7.2 Movimiento simulado de la pierna de apoyo..91
3.7.3 Movimiento simulado de la pierna flotante.92
IV CAPITULO: DISCUSIN DE RESULTADOS.
4.1 Cinemtica directa. 97
4.2 Cinemtica inversa.97
4.3 Dinmica del robot bpedo... 97
4.4 Diseo del Robot bpedo...97
4.5 Planificacin de trayectorias.97
4.6 Criterio de estabilidad de caminata ZMP97

V CAPITULO: CONCLUSIONES Y RECOMENDACIONES.


5.1 Conclusiones.98
5.2 Recomendaciones99

VI CAPITULO: REFERENCIAS BIBLIOGRFICAS


6.1 Bibliografa.100

V
UNIVERSIDAD NACIONAL DE TRUJILLO

ANEXOS:
ANEXOS A. RESULTADOS DE ECUACIONES OBTENIDAS..102
A.1 Resultado de Cinemtica Directa de la cadera respecto al pie de apoyo102
A.2 Resultado de Cinemtica Directa de la pie flotante respecto a la cadera103
A.3. Resultados de Cinemtica Directa de la pierna flotante respecto al pie
de apoyo.104
A.4 Resultados de la dinmica de la Pierna Flotante109
A.5 Resultados de la dinmica 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 Bpedo. 133
A.6.2 Aceleraciones del COG del Robot Bpedo..138

ANEXO B: ALGORITMOS DE MALTAB IMPLEMENTADOS:..144


B.1 Cinemtica Directa...144
B.1.1. Cinemtica Directa: archivo Script144
B.1.2.Funcin para el clculo de la cinemtica directa- Cadera respecto al
pie de apoyo145
B.1.3 Funcin para el clculo de la cinemtica directa- Pie flotante respecto
a la cadera..146
B.1.4 Cinemtica Directa de la Pie flotante respecto a pie de apoyo....147
B.2. Cinemtica Inversa.148
B.2.1 Cinemtica inversa de la cadera respecto al pie de apoyo...148
B.2.2 Cinemtica inversa del pie flotante respecto al pie de apoyo......152
B.2.3 Algoritmo para generar los ngulos de la Cinemtica inversa en
funcin a las trayectorias propuestas162
B.3 Clculo de la Dinmica163
B.3.1 Se presenta el archivo Script de clculo de la dinmica163
B.3.2 La dinmica del robot bpedo 10GDL del pie flotante respecto
a la cadera.166
B.3.3 La Dinmica para el robot de la cadera respecto al pie de Apoyo...170
B.3.4 Script para el clculo de los toques, velocidades y aceleraciones
angulares...174
B.3.5 5 Script para las grficas de torques de pie flotante, velocidad
y aceleraciones articulares del robot bpedo175
B.4 Algoritmos para la Planificacin de Trayectorias178
B.4.1 Algoritmo que describe la generacin de trayectorias del robot bpedo..178
B.4.2 Algoritmo para la generacin de ngulos para aplicar la
cinemtica inversa.184
B-4.3 Script que permite graficar los ngulos dados para la cinemtica inversa.185
B.5 Algoritmos para el Clculo del ZMP..190
B.5.1 Funcin que permite calcular la velocidad y aceleracin del COG..190
B.5.2 Funcin que permite calcular la velocidad y aceleracin del COG. 191

ANEXO C PLANOS DEL ROBOT BPEDO..193


C.1 Masa de los eslabones del Robot.201

VI
UNIVERSIDAD NACIONAL DE TRUJILLO

RESUMEN

La presente tesis busca disear un robot bpedo de 10GDL en total 5GDL por cada
pierna, con planificacin de trayectorias con criterio de ZMP para su estabilidad al caminar
sobre superficie plana. Se inicia presentando el desarrollo del modelo cinemtico directo,
mediante el mtodo de Denavit-Hartenberg, de matrices de transformacin homognea;
luego se desarroll la cinemtica inversa, para esto se decidi utilizar un mtodo numrico
para su solucin, aplicando el criterio de Damped Least Square, esto nos permitir
encontrar las posiciones, velocidades y aceleraciones de cada uno de los GDL de robot.

El model dinmico de este proyecto de investigacin se realiz mediante la


formulacin de Newton - Euler, para calcular los torques que se producen en cada
articulacin, debido a que es ms rpido computablemente respecto al de Lagrange -Euler,
esto se debe a su naturaleza recursiva y vectorial; adems asumiendo que no existe
deslizamiento entre el suelo y el piso.

Una vez tenido el modelo cinemtico, y la dinmica se procede a hacer el modelo


CAD del robot bpedo en Solidworks.

Se planific las trayectorias de movimiento del robot bpedo 10GDL en los planos
sagital y frontal, para esto se utiliz las Spline Cbic asegurando la existencia de sus
derivadas (velocidad y aceleracin) permitiendo el movimiento del robot se asemejara a la
caminata humana.

Se evalu la estabilidad de la caminata del robot bpedo mediante el criterio de ZMP,


para esto se model el robot bpedo mediante un pndulo invertido en 3D, basado en las
trayectorias generadas, adems para esto fue necesario calcular la velocidad y aceleracin
del COG.

Se logr generar la simulacin del robot bpedo en la plataforma de Simulink


utilizando la librera de SimMechanics, siguiendo las trayectorias propuestas para la cadera
y el pie flotante, esperando fundamentalmente que tenga un impacto investigativo,
cientfico - tecnolgico en la Universidad Nacional de Trujillo, y en el pas que permita abrir
nuevos campos de estudio en base a este proyecto de diseo de robot Bpedo.

VII
UNIVERSIDAD NACIONAL DE TRUJILLO

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 Cbic" 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
UNIVERSIDAD NACIONAL DE TRUJILLO
CAPITULO I: INTRODUCCIN.
1.1 Realidad Problemtica:

La robtica es una rama de la tecnologa integrada por un conjunto de conocimientos


tericos y prcticos que permiten el diseo, construccin, operacin y aplicacin de los
robots; la robtica adems combina diversas disciplinas como la fsica, mecnica,
electrnica, informtica, ingeniera de control e inteligencia artificial.

La robtica est tomando un papel muy importante para el futuro de la sociedad, ya


que cada vez hay ms robots que desempean tareas del ser humano. Por ejemplo los
robots manipuladores que son utilizados en procesos industriales sirven para trabajar con
materiales peligrosos, tambin sirven para desempear procesos en serie.

Por otro lado, existen tambin la robtica mvil con patas, los cuales tienen dos
extremidades inferiores para realizar su desplazamiento, conocidos como bpedos, 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
locomocin bpeda,.

La caminata bpeda al momento de imitar es una de las ms complicadas en


comparacin con cuadrpedos o hexpodos debido a la complejidad de mantener el
equilibrio, por eso, es est la limitacin de los proyectos de investigacin cientfica en esta
rea de la robtica, muchas empresas ya consolidadas tienen grandes resultados pero esto
debido a aos de investigacin, muchos ingenieros atrs del proyecto y presupuestos muy
altos como es el caso de Honda, Sony,etc. tambin se dan grandes competencias
internaciones de robot humanoide como es el caso de Darpa Robotics Challenger,etc.

Dada la importancia de desarrollo de tecnologas de esta tesis que se encuentran en


la lnea de investigacin de robots androides, rama que est en evolucin en nuestro pas,
como lo muestra los ltimos papers presentados en los ltimos (CONEIMERA), por lo que,
mediante la realizacin de un robot humanoide se desea contribuir con un escaln ms
sobre el cual se levante proyectos de una mayor complejidad y aplicaciones en este campo,
como tambin aportar al desarrollo en proyectos que tengan impacto en la industria minera,
metalrgica, metal mecnica, mdica, para rehabilitacin, con desarrollo de exoesqueletos
o robots bpedos humanoide independientes que puedan solucin problemas que es
inaccesible o peligroso para las personas.

Por esas y otras razones la robtica es imprescindible para el desarrollo y competitividad


de un pas. Debe quedar claro que no se est hablando de importacin de tecnologa de
otros pases sino ms bien de la comprensin de la ciencia que hay detrs y el desarrollo
de propia tecnologa.

1
UNIVERSIDAD NACIONAL DE TRUJILLO
1.2 .-Trabajos Previos:

Desde la dcada de 1980's se han construido maquinas bpedas de tamao similar a


las personas. Despus de aos de investigacin y desarrollo, los primeros robots
humanoides hicieron su aparicin en los aos 1990's. La construccin de robots
humanoides de tamao real es un gran reto tecnolgico.
Para el desarrollo de un solo prototipo se necesitan fuertes recursos econmicos y
sobre todo, un respaldo en tecnologa. De tal manera, nicamente grandes empresas e
instituciones acadmicas de prestigio son los principales fabricantes de este tipo de robots.

1.2.1.- Robot Asimo:

El robot ASIMO De la compaa japonesa Honda es uno de los ejemplos ms


completos en robots humanoides bpedos hoy en da. ASIMO (Advanced Step in
Innovative Mobility, Paso Avanzado en Movilidad Innovadora) fue concluido el 20 de
noviembre del ao 2000. La versin ms reciente mide 130 cm y pesa 48 kg. Est
provisto de 57 GDL distribuidos de la siguiente manera: 3 en la cabeza, 7 en cada
brazo, 2 en cada mano, 13 en la cadera y 6 en cada pierna. ASIMO implementa la
tecnologa llamada i-WALK, (Intelligent Real-Time Flexible Walking, Caminado Flexible
e Inteligente en Tiempo Real), que le permite girar cuando camina sin pausas en su
movimiento.
ASIMO posee tambin una gran capacidad de interaccin con las personas gracias
a su tecnologa de reconocimiento visual. Es capaz de interpretar los movimientos de
una persona e interactuar con esta Corre a una velocidad de 9 km/h en lnea recta y a
5 km/h de manera circular.

Figura 1.2.1 Robot Asimo


(Fuente: http://science.howstuffworks.com/asimo.htm)

1.2.2.- Robot Hubo:

La construccin del robot HUBO fue llevada a cabo por el KAIST (Korea
Advanced Instituto of Science and Technology, Instituto Avanzado de Ciencia y
Tecnologa de Korea). Este robot es la continuacin de los trabajos realizados en
robtica humanoide en este instituto. HUBO tiene como predecesores al robot KHR-
1 y KHR-2, adems de su anlogo Albert HUBO (en honor a Albert Einstein, un robot

2
UNIVERSIDAD NACIONAL DE TRUJILLO
humanoide con caractersticas similares a HUBO, pero con la peculiaridad de que su
rostro es una recreacin del rostro del famoso cientfico).

Un HUBO Introducido por TEAM KAIST fue el ganador de las DARPA Robotics reto
final de 6 de junio del 2015. La mquina llamada RDC Hubo fue una adaptacin del
dispositivo multifuncional.

Figura 1.2.2. Robot Hubo


(Fuente: http://www.koreaittimes.com/image/hubo-robot)

Los robots de tamao mediano cuentan con caractersticas parecidas a los robots
humanoides de tamao real y a los de tamao pequeo. Son construidos por instituciones
educativas y por empresas, con un mercado de consumo creciente para el entretenimiento.

1.2.3.- Robot Sdr-4x: Qrio:

El robot SDR-4X, tambin conocido como Qrio, es la culminacin de la investigacin


en robots humanoides por parte de SONY. Tiene como predecesor al robot SDR-3X. Las
siglas SDR derivan de SONY Dream Robot o en espaol Robot Sonado por Sony, y sus
orgenes se remontan al ao de 1997 cuando la compaa empez a desarrollar sus
primeros prototipos. Qrio (como tambin es conocido) mide 58 cm y tiene un peso
aproximado de 7 kg.

1.2.4.- Robot Nao:

Es un robot humanoide programable y autnomo, desarrollado por Aldebaran


Robotics, una compaa de robtica francesa con sede en Pars. El desarrollo del robot
comenz con el lanzamiento del Proyecto Nao en 2004.
En el 15 de agosto de 2007, Nao sustituye al perro robot Aibo de Sony como la
plataforma estndar para la Robocup ("Robot Soccer World Cup"), un concurso
internacional de robtica. y el NaoV3R fue elegido como la plataforma para el SPL en
RoboCup 2010, actualmente es uno de los robot ms modernos.

3
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 1.2.3. Robot Nao


(Fuente: https://www.aldebaran.com/en/humanoid-robot/nao-robot)

1.2.5.- Tesis Diseo y Control de Kokone, un pequeo robot humanoide, - Ing.


Vctor Enrique Gonzlez Hernndez, Centro de Investigacin y de Estudios
Avanzados del Instituto Politcnico Nacional. Mxico 2009.

En este proyecto de tesis, proponemos el diseo y control de Kokone (nios en


nhuatl), un pequeo robot humanoide. Kokone tiene como objetivos asistir en el desarrollo
de estrategias de control y fomentar el uso de mtodos computacionales en la solucin de
problemas de robtica humanoide. El esquema de la plataforma robtica facilita las pruebas
de laboratorio, pues se aprovecha la flexibilidad del uso de una PC como unidad de control.
La distribucin de sus 22 grados de libertad, permiten realizar la mayora de los movimientos
del cuerpo humano, y su diseo mecnico, realizado en Solid Edge, requiere un mnimo de
piezas y un fcil ensamblaje. Con base en el diseo final, se resolvi la cinemtica directa
mediante el mtodo de Denavit-Hartenberg, y se propuso un algoritmo gentico para
resolver la cinemtica inversa. Como primera aplicacin, se propuso una estrategia de
caminado basado en los conceptos de la caminata bpeda esttica. Los desplazamientos
obtenidos se verificaron en Matlab, para despus ser implementados satisfactoriamente en
el robot Kokone.

1.2.6.- Tesis Construccin de un robot Bpedo, Basado en Caminado Dinmico.


Ing. Cesar Humberto Guzmn Valdivia, Centro Nacional de Investigacin y Desarrollo
Tecnolgico, Mxico 2010.

Se llev a cabo un anlisis de la etapa de avance de la pierna, el cual cuenta con


ecuaciones cinemticas y dinmicas. El resultado de las ecuaciones cinemticas es para
conocer la posicin y orientacin del extremo de la pierna. Se utiliz el mtodo del
lagrangiano para describir el comportamiento dinmico de la pierna cuando se encuentra
en la fase de avance. Tambin se analiz la cadera del robot para conocer el peso mximo
que puede soportar la estructura. Se simul el robot virtualmente para comprobar que la
estructura mecnica no presentara problemas y ver la estabilidad que tiene al dar un paso.
El robot se construy bajo un enfoque mecatrnica; cuenta con parte mecnica, electrnica

4
UNIVERSIDAD NACIONAL DE TRUJILLO
y programacin. El prototipo que se realiz cuenta con cinco grados de libertad, uno en la
cadera, dos en las rodillas y dos en los tobillos, los brazos estn mecnicamente ligados a
los muslos por medio de bandas y poleas, las rodillas cuentan con un sistema interno de
enclavamiento, la cadera mantiene el robot erguido mediante un sistema de engranes y
cadenas, los msculos neumticos proporcionan el par necesario para mover el robot. El
prototipo camin de forma anloga a la locomocin, Humana.

1.2.7.- Tesis Mecatrnica BioInspirada de Robots humanoides de Tamao Natura-


Ing. Luis Mara Cabs Ormaechea, Universidad Carlos III de Madrid, Espaa 2009.

Esta tesis se enmarca dentro del estudio de robots humanoides y se centra


especficamente en el estudio de mecatrnica de los mismos. El problema de resolver es
como disear de forma eficiente esta clase de robots para que puedan desplazarse de
forma automticamente y realizar una gran variedad de tareas. Por tal motivo, en esta tesis
se desarrolla y aplica unas metodologas de diseo para u robot humanoide de tamao
natural tomando como modelo algunas caractersticas del ser humano. Esta metodologa
pretende resolver de forma progresiva el problema del diseo de un robot humanoide
utilizando aquellas analogas fsicas y mecnicas que tienen este tipo de ingenieros
mecnicos con el cuerpo humanos y a partir de eso, utilizar como piedra angular para
obtener los modelos que nos ayudaran a logran nuestro propsito, de disear un humanoide
poli funcional.

1.2.8.- Robots humanoides en Per:

En nuestro pas se han desarrollado trabajos muy interesantes en el campo de la


robtica bpeda humanoide. Estudiantes de ingeniera Mecatrnica de la Universidad
Nacional de Ingeniera UNI, presentaron un robot Bpedo en el CONEIMERA- 2010
realizado en la Ciudad de Trujillo, la cual se llev el 1 lugar en la competencia de ms de
200 proyectos a nivel nacional, este robot era una mquina que tena especficamente 2
piernas, y se caracterizaba por su movimiento, el cual se asemejaba a la caminata humana.

Otros robots bpedos construidos en nuestro Pas se desarroll en la Universidad


Peruana de Ciencias Aplicadas, la cual se Titul Diseo y Construccin de un Robot
Bpedo caminante, El proyecto consista en implementar un bpedo capaz de caminar y de
captar imgenes a travs de una cmara inalmbricamente en la parte superior del cuerpo,
actuando como si fuera la cabeza del robot. Contaba con 5 GDL piernas y 2 GDL en la
cabeza la cual suma un total de 12 GDL.

En el INTERCON 2011, estudiantes de la Universidad Nacional de Ingeniera


presentan un proyecto Walking Simulation and Controllers for a 18DOF small humanoid
robot, quedando en el 1 puesto.

Con la literatura revisada, se recabo la informacin necesaria para definir puntos


importantes del proyecto, de los cuales podemos mencionar el esquema general del

5
UNIVERSIDAD NACIONAL DE TRUJILLO
sistema, diseo del robot, software de diseo, eleccin de los materiales para la
construccin y programas de control del robot.

1.3.- Marco Terico.

En este captulo se presentan los conceptos necesarios para el desarrollo de la


tesis.

1.3.1.- Cinemtica del robot bpedo:


El anlisis Cinemtico de un robot, comprende el estudio de su movimiento con respecto
a un sistema de referencia.
- Cinemtica Directa: Determinar cul es la posicin y orientacin del extremo final
del robot, con respecto a un sistema de coordenadas que se toma como referencia,
conocido los valores de las articulaciones y los parmetros geomtricos de los
elementos del robot.

- Cinemtica inversa: Resuelve la configuracin (valores de las articulaciones) que


debe adoptar el robot para una posicin y orientacin del extremo conocidas.

Cinemtica Directa
Valor de las Posicin y orientacin
coordenadas articulares Cinemtica inversa del Extremo del Robot.
(q1, q2,, qn) (X, y, z, , , )

Figura 1.3.1 Diagrama de Relacin entre Cinemtica Directa e Inversa


(Fuente: [1])

1.3.1.1 Cinemtica directa:


El problema cinemtico directo se plantea en trminos de encontrar una matriz de
transformacin que relaciona el sistema de coordenadas ligado al cuerpo en movimiento
respecto a un sistema de coordenadas que se toma como referencia. Para lograr esta
representacin se usan las matrices de transformacin homognea 4x4, la cual incluye
las operaciones de traslacin y la orientacin.

La matriz de transformacin homognea es una matriz 4x4 que transforma un vector


de posicin expresado en coordenadas homogneas desde un sistema de coordenadas
hasta otro sistema de coordenadas. Una matriz de transformacin homognea se puede
considerar que consiste en sub matrices:

31
T= [ 33 ]=[ ]
13 11 .

La Submatriz 3x3 superior izquierda representa la matriz de rotacin; la Submatriz


superior derecho 3x1 representa el vector de posicin del origen del Sistema de
coordenadas rotado con respecto al Sistema de referencia; la Submatriz inferior izquierda
6
UNIVERSIDAD NACIONAL DE TRUJILLO
1x3 representa la transformacin de perspectiva. Y el cuarto elemento diagonal es el factor
de escala global.

La matriz de transformacin homognea se puede utilizar para explicar la relacin


geomtrica entre el sistema ligado al cuerpo, es decir transformar un vector expresado en
coordenadas homogneas OUVW en el sistema de coordenadas OXYZ.



T= [ ]

0 0 0 1

= (1.3.1)

Normalmente la matriz de transformacin homognea que representa la posicin y


orientacin relativa entre los sistemas asociados a dos eslabones consecutivos del robot
se suele denominar matriz : 1 . As pues 1 Describe la posicion y orientacin del
sistema de referencia solidario al 1 eslabn con respecto al sistema de referencia solidario
a la base.

Cuando se consideran todos los grados de libertad, la matriz An se le suele


denominar T. As dado un robot de n grados de libertad, Se tiene que la posicin y
orientacin del eslabn final vendr dada por la matriz T.

T= 0 = 01 12 2
3 34 4 (1.3.2)

a) Denavit hartenberg (D - H) , propusieron en 1955 un mtodo matricial que permita


establecer de manera sistemtica un Sistema de coordenadas {Si} ligado a cada eslabn i,
de una cadena articulada, pudindose determinar a continuacin la ecuaciones cinemticas
de la cadena completa [1].
Segn la representacin de D-H, escogido adecuadamente los sistemas de
coordenadas asociados a cada eslabn, ser posible pasar de uno al siguiente mediante 4
trasformaciones bsicas que dependen exclusivamente de las caractersticas geomtricas
del eslabn.

Estas transformaciones bsicas consisten en una sucesin de rotaciones y


traslaciones que permitan relacionar el sistema de referencia del elemento i con el sistema
del elemento i-1. Las trasformaciones en cuestin son las siguientes:

1.- Rotacin alrededor del eje 1 un Angulo .


2.- Traslacin a lo largo de 1 una distancia .
3.-Traslacion a lo largo de una distancia
4.- Rotacin alrededor del eje un angulo

Dado que el producto de matrices no es conmutativo, las transformaciones se han


de realizar en el orden indicado. De este modo se tiene que:

1
= (, )(0,0, )( , 0,0) (, ) (1.3.3)

7
UNIVERSIDAD NACIONAL DE TRUJILLO

1
= [ ] (1.3.4)
0
0 0 0 1

Donde , , , Son los parmetros D-H de eslabn i . De esta modo basta con
identificar los parmetros para obtener las matrices de transformacin homognea A y
relacionar as todos y cada uno de los eslabones del robot.

Hay que tener encuentra los parmetros de trabajo de DH.

Figura 1.3.2. Parmetros de Denavit Hartenberg para un eslabn giratorio


(Fuente: [1])

Es el ngulo de la articulacin 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
interseccin del eje zi-1 con el eje xi.
ai: Es la distancia de separacin entre los orgenes de los sistemas de referencia i-1 e i,
medida a lo largo del eje xi hasta la interseccin con el eje zi-1. (o la distancia ms corta
entre los ejes zi-1 y zi, cuando estos no se interceptan)
i: Es el ngulo que separa los ejes zi y zi-1, medido respecto del eje xi

1.3.1.2 Cinemtica inversa del robot bpedo:

El objetivo del problema cinemtico 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 segn una determinada localizacin espacial [2]

Con la Cinemtica Directa se obtiene la matriz de transformacin homognea que


relacin el efector final con la base, la misma que es aplicable para robots simples, es decir
a los que poseen pocos grados de Libertad. Dentro de este campo se Puede resolver la
cinemtica inversa por algunos mtodos: mtodo Geomtrico, Matrices Transformacin -
Algebraico, desacoplo cinemtico. La principal ventaja de estos mtodos es que se pueden
obtener una solucin cerrada (exacta), sin embargo, para un robot mayor a 6 GDL ya no
se puede aplicar estos mtodos.

Al Investigar en los mtodos o tcnicas para la resolucin de cinemtica inversa para


este tipo de Robot (fuente: [3]), se logr identificar las siguientes: resolucin de la cinemtica

8
UNIVERSIDAD NACIONAL DE TRUJILLO
inversa mediante redes neuronales, mediante algoritmos genticos, y mediante desarrollo
de mtodo numrico (linealizacin de funciones) .

Para esta tesis se trabaj mediante el desarrollo de mtodo numrico (linealizacin


de funciones) en este robot bpedo 10GDL se tendr que resolver la cinemtica inversa de
la cadera al pie de apoyo y la del pie flotante respecto al pie de apoyo.

Como el sistema de ecuaciones no es lineal, se utiliza el mtodo de Newton


Rhapson para linealizar las siguientes funciones:
= (1 , 2 , 3 , , ); = 1,2,3, , (1.3.5)

Se utiliza Taylor para linealizar las funciones:

() = () + () (1.3.6)

Dnde f(x) es la estimacin inicial, () es la correccin no conocida. Si expandimos la


ecuacin (1.3.6) se tiene un polinomio de Taylor truncado de primer orden alrededor de
:
f()
f(x) = f() + =0 (1.3.7)

El Segundo trmino de las ecuacin (1.3.2.3) se le denomina Jacobiano, J.



J=[] (1.3.8)

1(1 , 2 , 3 , , )
1(1 , 2 , 3 , , )
=[ ] (1.3.9)

1(1 , 2 , 3 , , )

De estas ecuaciones se obtiene el Jacobiano, y esto nos sirve para encontrar los
ngulos deseados.
= (1.3.10)
Se aplica el Criterio de Damped Least Squares [Fuente: [3]], para resolver la
transpuesta de la matriz Jacobiana.

1.3.2.- Anlisis Dinmico:


Las ecuaciones de movimiento de un manipulador son un conjunto de ecuaciones
matemticas que describen su conducta dinmica.

9
UNIVERSIDAD NACIONAL DE TRUJILLO
El modelo dinmico establece relaciones entre las coordenadas articulares (o las
coordenadas del extremo del robot), sus derivadas (velocidad y aceleracin), las fuerzas y
pares aplicados en las articulaciones (o en el extremo) y los parmetros del robot (masas
de los eslabones, inercias, etc).

Para ello se realizara el estudio de la formulacin de Newton Euler, con el fin de


conocer las torques necesarias de los motores para poder mover el Bpedo de 5GDL y a la
vez te sirve para definir el tipo de estructura.

1.3.2.1.- Dinmica Newton Euler.

Esta formulacin cuando se aplica a un robot resulta en un conjunto de ecuaciones


recursivas hacia adelante y hacia atrs con trminos del tipo de producto vectorial, que
relaciones las velocidades de cada elemento articular desde la base se propagan hacia el
extremo, (recursivas hacia adelante) y los torques, momentos de inercia, fuerzas externas,
hacia atrs desde el efector final hasta la base [Fuente: [2]]

1.3.2.2.- Sistemas de Coordenadas rotantes:

Figura 1.3.2.1 Sistemas de coordenadas en movimiento


(Fuente: [2] pg. 111)

Con respecto a la figura 1.3.2.1 se tiene que el sistema de coordenadas 0* est


girando y trasladndose con respecto al sistema de coordenadas sin asterisco 0, que es un
Sistema inercial. Una partcula P con masa m se localiza mediante vectores r* y r con
respecto al sistema coordenadas O* y O respectivamente. El origen O* se localiza mediante
un vector h con respecto al origen O. La relacin entre los vectores de posicin r y r* viene
dada por [Fuente: [2]]

= + (1.3.11)



=
+ = + (1.3.12)

Donde es la velocidad del punto P relativa al sistema de coordenadas 0* en


movimiento y es la velocidad del origen del sistema 0* respecto de la base.


=
+ = (
+ ) + (1.3.13)

10
UNIVERSIDAD NACIONAL DE TRUJILLO
Donde:


es la velocidad lineal del punto P respecto al origen 0* y ( ) es la velocidad angular
del punto P respecto al Origen 0*

Anlogamente, la aceleracin de la partcula P con respecto al Sistema de coordenadas sin


asterisco.

2 2
= = + = + (1.3.14)
2 2

Con la ecuacin 1.3.13 y 1.3.14 se obtienen:

2 2
= 2
+ 2
+ () +
+ 2 (1.3.15)

Con estas ecuaciones de desarrollo de coordenadas mviles, se podr aplicar este


concepto a los sistemas de coordenadas de los elementos que establecimos para un robot.
[Fuente: 2]

1.3.2.3.- Cinemtica de los elementos del robot:

Se desarrolla un conjunto de ecuaciones matemticas que, describirn las


relaciones cinemticas de los elementos en movimiento de un robot con respecto al sistema
de coordenadas de la base.

A partir de las ecuaciones (1.3.11) a (1.3.15) Se tiene:

Figura 1.3.2.2 Relacin entre los sistemas 0, 0* y O


(Fuente: [2] pg. 113)


=
+ 1 + 1 (1.3.16)
=1 +

La aceleracin lineal y la aceleracin angular del sistema de coordenadas de


la articulacin i son:

11
UNIVERSIDAD NACIONAL DE TRUJILLO
2
= 2
+ 1 + 21
+ 1 (1 ) + 1 (1.3.17)

= 1 + (1.3.18)

La aceleracin angular del sistema de referencia i (xi, yi, zi), respecto al sistema de
referencia (1 , 1 , 1 )


=
+ 1 (1.3.19)

La ecuacin 1.3.19 queda definido:


= 1 +
+ 1 (1.3.20)

Los sistemas de coordenadas (1 , 1 , 1 ) y ( , , ) Estn unidos a los


eslabones i-1 e i. La velocidad del eslabn i con respecto al sistema de coordenadas i-1 es
. Si el eslabn es prismtico, la velocidad ser una velocidad de traslacin relativa
respecto del sistema (1 , 1 , 1 ) y si es rotacional le corresponder una velocidad
rotacional relativa de eslabn i respecto del sistema (1 , 1 , 1 ), (Fuente: [2]) en mi
caso del robot bpedo 10GDL todos sus eslabones son rotacionales por lo que las
ecuaciones quedaran definidas de la siguiente forma:

= 1 (1.3.21)

Donde es la magnitud de la velocidad angular del eslabn i con respecto al


sistema de coordenadas (1 , 1 , 1 ) De manera similar:



= 1 (1.3.22)

Debe notarse que el vector de Zi-1 es igual (0,0,1) .

Las velocidades y aceleraciones de los sistemas de coordenadas ligados a cada eslabn


son absolutas y se calcula como:

= 1 + 1 (1.3.23)

= 1 + 1 + 1 x (1 ) (1.3.24)

Las velocidades lineales de los sistemas de referencia de cada eslabn se calcula como:



= (1.3.25)
2
2 =
+ ( ) (1.3.26)

La velocidad lineal absoluta del sistema de coordenadas ligado a cada eslabn se calcula
como:

= + 1 (1.3.27)

12
UNIVERSIDAD NACIONAL DE TRUJILLO
= + ( ) + 1 (1.3.28)

1.3.2.4.- Ecuaciones de movimiento recursivas para manipuladores:

A partir de las ecuaciones cinemticas del desarrollo anterior y aplicando el principio


de D Alembert del equilibrio esttico para todos los instantes de tiempo, se obtienen las
ecuaciones recursivas de Newton Euler.

Figura 1.3.2.3 Fuerzas y momentos sobre el elemento i


(fuente: [2] Pg 116)

Donde:

: Masa total del eslabn i.


: Posicin del centro de masa del elemento i desde el origen del sistema de
referencia de la base.
: Posicin del centro de masas del elemento i desde el origen del sistema
de coordenadas (xi,yi,zi).
: Posicin del origen de coordenadas i esimo con respecto al sistema de
coordenadas (i -1) esimo.

=
: Velocidad lineal del centro de masa del elemento i.

=
: Aceleracin lineal del centro de masa del elemento i.
: Fuerza total externa ejercida sobre el elemento i en el centro de masa.
: Momento total externo ejercido sobre ele elemento i en el centro de masa.
:Matriz de Inercia del elemento i respecto de su centro de masa con
respecto al sistema de coordenadas (xo,yo,zo)
: Fuerza ejercida sobre el elemento i por el elemento i-1 en el sistema de
Coordenadas (1 , 1 , 1) para soportar al elemento i los elemento por
encima de l.
: Momento ejercido sobre el elemento i por el elemento i 1 en el sistema de
Coordenadas (1 , 1 , 1 ).

13
UNIVERSIDAD NACIONAL DE TRUJILLO

Si se omiten los efectos del rozamiento viscoso en las articulaciones, y se aplica el


principio de D Alembert, se obtienen para cada eslabn:

( )
= = (1.3.29)

( )
=
= + ( ) (1.3.30)

Realizando el balance de pares y fuerzas:

= +1 (1.3.31)

= 1 + (1 ) ( )1 (1.3.32)

Que utilizando la relacin geomtrica:

1 = + (1.3.33)

Se obtienen las ecuaciones recursivas:

= + +1 = + +1 (1.3.34)

= +1 + ( )+1 + ( ) + (1.3.35)

Se observa que estas ecuaciones son recursivas y permiten obtener las fuerzas y
momentos en los elementos i=1,2,3,n para un robot de n elementos
+1 +1 representan las fuerzas y momentos ejercidos por el efector final del robot sobre
un objeto externo. Por lo tanto el par de fuerzas para cada articulacin se expresa como:

= 1 + (1.3.36)

Donde bi _ coeficiente viscoso de la articulacin.

1.3.3.- Planificacin de Trayectorias:


Una vez obtenido los modelos cinemticos y dinmicos del robot se pueden abordar
el problema de control de los mismos. Definir el movimiento de un robot, implica controlar
dicho robot de manera que siga un camino pre planificado. EL objetivo es por tanto
establecer cules son las trayectorias que deben seguir cada articulacin del robot a lo largo
del tiempo para conseguir los objetivos fijados.

14
UNIVERSIDAD NACIONAL DE TRUJILLO
1.3.3.1 .-Locomocin Humana:

El conocimiento de la locomocin humana es la base del tratamiento sistemtico y


del manejo de la marcha patolgica, especialmente cuando se usan prtesis. La locomocin
humana se ha descrito como una serie de movimientos alternantes, rtmicos, de las
extremidades y del tronco que determinan un desplazamiento con respecto al centro de
gravedad. Aunque existen pequeas diferencias en la forma de la marcha de un individuo
a otro, estas diferencias caen dentro de pequeos lmites.

1.3.3.2.- El ciclo de la marcha

El ciclo de la marcha comienza cuando el pie contacta con el suelo y termina con el
siguiente contacto con el suelo del mismo pie. Los dos mayores componentes del ciclo de
la marcha son: la fase de apoyo y la fase de balanceo (ver Figura 1.3.3.1). Una pierna est
en fase de apoyo cuando est en contacto con el suelo y est en fase de balanceo cuando
no contacta con el suelo.

Figura 1.3.3.1. Ciclo de caminado del ser humano


(Fuente: [11].])

La longitud del paso completo es la distancia lineal entre los sucesivos puntos de
contacto del taln del mismo pie. Longitud del paso es la distancia lineal en el plano de
progresin entre los puntos de contacto de un pie y el otro pie (ver Figura 1.3.3.2).

Figura 1.3.3.2. Longitud del Paso.


(Fuente:[11])

15
UNIVERSIDAD NACIONAL DE TRUJILLO
Para que un individuo pueda andar, el sistema locomotor debe cumplir los siguientes
requisitos. Cada pierna debe ser capaz de soportar el peso del cuerpo sin colapsar.

El equilibrio debe ser mantenido tanto de forma esttica como dinmica en apoyo
simple. e
La pierna oscilante debe ser capaz de avanzar hasta la posicin 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 orientacin del cuerpo humano, en la cual se consideran
tres planos de corte, estos planos se denominan planos anatmicos (Cabs, 2009, p. 81).

a) Plano sagital: Pasa desde la parte anterior del cuerpo (o segmento de ste) hasta
la posterior, dividiendo a ste en dos mitades, izquierda y derecha.

b) Plano frontal: Pasa desde un extremo lateral del cuerpo (o segmento de ste) hasta
el otro, dividiendo a este en dos mitades, anterior y posterior.

c) Plano transversal: Pasa horizontalmente el cuerpo (o un segmento de ste),


dividindolo en mitades superior e inferior.

Figura 1.3.3.3 Planos de Corte del cuerpo Humano


(Fuente: Blog Biomecnica: Jos A. Acero Juregui)

1.3.3.3.- Fases de la marcha.

El caminado es un proceso cclico, 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
UNIVERSIDAD NACIONAL DE TRUJILLO
Fase de Apoyo: Cuando una pierna est en contacto con el suelo, tambin se utiliza
el trmino apoyo doble que es cuando las dos piernas estn en contacto con el
suelo.

Fase de Balanceo: Cuando una pierna no contacta con el suelo.


En la Figura 8 se observa que la cantidad de tiempo ocupada por la fase de apoyo es 40%
del ciclo, la fase de doble apoyo es del 20% y la fase de balanceo es de 40% del ciclo
(Hernndez, 2008, p. 39).

Figura 1.3.3.4 Componentes de la Marcha.


Fuente: Francisco Hernndez (2008). Diseo y construccin de prototipo neumtico de
prtesis de pierna humana. Captulo 3. P.38.

La locomocin bpeda en una persona consiste en una concatenacin de


movimientos controlados en respuesta a un sistema retroalimentado en tiempo real, en el
caso de robots bpedos es algo similar.

1.3.3.4.- Locomocin pasiva

Esta caminata hace referencia a sistemas robticos que caminan sin necesidad de
control ni de actuadores, es as que su movimiento se genera nicamente por accin de la
gravedad y por una pendiente en el piso. La principal ventaja de la caminata pasiva es que
no se requiere de energa externa para caminar, pero la desventaja est en la
direccionalidad limitada y la imposibilidad para subir pendientes (Santana, 2013, p. 3).

1.3.3.5.- Locomocin Activa:

La cual se relaciona directamente con robots que poseen un sistema de control


complejo, utilizan actuadores y sensores, as el robot desarrolla tareas complejas como
caminar en diferentes direcciones, subir escalones y permanecer en una pierna (Santana,
2013, p. 3).
Se utilizan criterios para lograr su estabilidad al momento de caminar, uno es el
punto de momento cero (ZMP), que se basa en mantener el centro de presin dentro del
contacto del pie (Guzmn, 2010, p. 9).

17
UNIVERSIDAD NACIONAL DE TRUJILLO
1.3.4.- Punto de Momento Cero ZMP:

El ZMP es el punto sobre la superficie de desplazamiento donde la suma de todos


los momentos, ocasionados por la gravedad y las fuerzas inerciales, es igual a cero.

Cuando el ZMP se ubica dentro del rea convexa formada por los puntos de
contacto entre el pie de soporte y el suelo, el robot puede desarrollar una marcha
dinmicamente estable

i) Se comenzara el anlisis de todas las fuerzas y Momentos que actan sobre el Pie
de Apoyo.

MA Z

O
Y

Figura 1.3.4.1 Fuerzas y Momentos Aplicados sobre el pie de Apoyo


(Fuente: [8] Pg. 31)

Respecto a la figura 6.1 MA y FA Son el Momento y la Fuerza Resultante generado


por el cuerpo en movimiento del Robot Bpedo 10GDL y M y R son el Momento y la
fuerza de Reaccin del Piso. Para que se encuentre en equilibrio Esttico se tiene:

= 0 (1.3.37)

+
= 0 (1.3.38)

= 0 (1.3.39)
+
(
+ + = 0 (1.3.40)
Se asume no deslizamiento entre el PIE de apoyo y el SUELO.


= (1.3.41)

= (1.3.42)

18
UNIVERSIDAD NACIONAL DE TRUJILLO
Por lo que se tendra:
+
= 0 (1.3.43)
Los Momentos se ha reducido al plano horizontal (ejes Z y Y). Se debe cumplir:

+
= 0 (1.3.44)

Cambiando el Punto de referencia de O por A figura 6.1 despreciando la masa del pie se
Obtiene las siguientes ecuaciones:

= 0

+
= 0
=

(1.3.45)
El ZMP existe nicamente dentro del polgono de soporte. Cuando existe el
ZMP el robot nicamente se encuentra dinmicamente estable.
Cuando el momento generado por el cuerpo ( ) es demasiado grande, el punto P
puede encentrarse fuera del polgono de soporte figura 1.3.3.5, en este caso, la fuerza de
reaccin del piso se encuentra en el borde del pie (no puede seguir al punto P y no puede
compensar el momento generado por el cuerpo en movimiento), si el ZMP no existe,
evidentemente el robot tiende a perder su estabilidad.

1.3.5.- Arquitectura Pierna Humana:


La arquitectura de las extremidades inferiores de un ser humano es muy complejo
(ver Figura 1.3.5.1). Su estructura sea reagrupa 44 huesos, de los cuales el fmur, la tibia
y el peron son la principal huesos. Aparte de la rtula, los huesos restantes conforman el
pie, que puede ser considerado como un corpus compuesto deformable.
Por tanto, cada miembro tiene un conjunto de tres cuerpos principales (muslo, pierna
y pie) unidos entre s por articulaciones que tienen muchos grados de movilidad. De esta
manera, la articulacin de la cadera tiene 3 grados de libertad (GDL). Las articulaciones de
los tobillos tienen cada uno de 2 GDL y la rodilla 1 GDL.
La musculatura de las extremidades inferiores de un ser humano se compone de 46
msculos esquelticos. Los msculos que intervienen en el movimiento de propulsin son
los msculos ms largos. En esto As, nos damos cuenta de que muchos msculos
intervienen simultneamente para ayudar en el movimiento de una articulacin. Los
msculos hacen un esfuerzo de traccin y que siempre trabajan en pares conjuntivas con
otro msculo opuesto.
Podemos tambin ver msculos distintos en los que slo actan en una sola
articulacin (por ejemplo, el msculo ilaco en la cadera) y otros, que actan sobre el cuerpo
enlaces separados, que estn separados por dos articulaciones (por ejemplo, el msculo
recto femoral).

19
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 1.3.5.1. Estructura sea de Pierna humana


(Fuente: [8])

1.4.- Formulacin del Problema:


Cules son las caractersticas del diseo de un prototipo de robot bpedo, con
planificacin mediante generacin de trayectorias, teniendo como criterio de estabilidad el
ZMP (Punto de Momento Cero), para la caminata en superficie plana?

1.5.- Justificacin del Estudio:


Debido a la necesidad de tener una base de conocimiento cientfico, tecnolgico de
robots bpedo la cual se pueda analizar el diseo, las trayectorias y estabilidad de la
caminata y pueda levantarse proyectos de mayor envergadura.
Aplicaciones que integren la mecatrnica y la robtica en brindar soluciones a
problemas en la medicina: como el desarrollo de prtesis, exoesqueletos en la rehabilitacin
de lesiones medulares, etc.; en la Industria: en el desarrollo de este robot bpedo
humanoides que permitan, entrar en espacios de alta peligrosidad la cual sera muy
perjudicial para los seres humanos; en el rea militar, industria espacial, etc.
Pero fundamentalmente tendra un impacto investigativo-cientfico-tecnolgico en la
Universidad Nacional de Trujillo, y en el pas que permita abrir nuevos campos de estudio
en base a este proyecto de Diseo de robot Bpedo, como es:

20
UNIVERSIDAD NACIONAL DE TRUJILLO
- Permitir estudios de planificaciones de caminata en pisos no inclinados, caminata
en superficies irregulares.
- Desarrollos estudios de control, Control PID Neuronal, Fuzzy, algoritmos genticos,
control adaptativo, control robusto, etc.
- Incluir a robots Bpedos Humanoides en Sistemas de Visin, que permitan
conducir a travs de ambientes con la presencia de obstculos, muros, etc.
Y esto a la vez nos permita generar industrializacin tecnolgica, que puedan ayudar
a la sociedad, a un menor costo y rapidez.

1.6.- Hiptesis:

Aplicando la cinemtica directa, mediante el mtodo de Denavit Hartenberg, la


cinemtica inversa utilizando un mtodo numrico, el anlisis dinmico usando el criterio
de Newton-Euler, as tambin desarrollando la planificacin de trayectorias y evaluando la
estabilidad de la caminata mediante el ZMP, es posible obtener las caractersticas de diseo
del robot bpedo, as como su simulacin al caminar sobre superficie plana.

1.7.- Objetivos:

1.7.1 Objetivo General:


Disear prototipo de robot Bpedo, con la planificacin mediante generacin de
trayectorias, con criterio de ZMP, para la caminata en superficie plana.

1.7.2 Objetivo Especfico:

- Desarrollar la cinemtica directa del robot bpedo.

- Desarrollar la cinemtica inversa.

- Determinar los torques que se producen en cada articulacin, mediante el desarrollo


dinmico.

- Determinar las dimensiones, forma y estructura del robot bpedo usando el software
Solidwork.

21
UNIVERSIDAD NACIONAL DE TRUJILLO
- Desarrollar la planificacin de la caminata del robot bpedo a travs de la generacin
de trayectorias.

- Analizar la caminata del robot bpedo a travs del criterio de estabilidad ZMP (Punto
de Momento Cero).

- Desarrollar la simulacin de robot bpedo con su sistema de control en Simulink


Matlab utilizando la librera de SimMechanics.

- Asimilar de tecnologa en el tema de robots bpedos humanoide

22
UNIVERSIDAD NACIONAL DE TRUJILLO
CAPITULO II: MTODO.
2.1.- Diseo de Investigacin:
Investigacin aplicada.
2.2.- Variables:
2.2.1 Variables independientes.
Trayectoria de caminata del robot bpedo.
Caractersticas del prototipo del robot bpedo.
Velocidad y aceleracin de cada articulacin.
Tiempo de paso.

22.2 Variables dependientes.

ngulos generados por cada articulacin al moverse.


Torques producidas al caminar en cada Articulacin.
Trayectorias generadas real (Cinemtica Inversa, directa)
Velocidad, aceleracin de COG del robot bpedo.
Posicin del ZMP.

2.3.-Poblacin y Muestra.
2.3.1 Poblacin:
Robots caminantes con 2 piernas, bpedos o humanoides basados en caminata
activa.
2.3.2 Muestra:
Robot bpedo, con planificacin de caminata, evaluando su estabilidad con criterio
de ZMP, para la caminata en superficie plana en la Universidad Nacional de Trujillo
Ingeniera Mecatrnica.

2.4 Tcnicas e Instrumentos:


- Se har uso del programa de Matlab2014b para el clculo cinemtico (directa e
inversa), como tambin para el clculo dinmico del robot, planificacin de
trayectoria y evaluacin de estabilidad.

- Se utilizara la plataforma de Simulink en (Matlab), con la librera de SimMechanics,


para la simulacin de la caminata.

- Se utilizar el software Solidworks para el diseo mecnico del robot bpedo.

23
UNIVERSIDAD NACIONAL DE TRUJILLO

CAPITULO III: DESARROLLO Y RESULTADOS.

En base a lo expuesto en el captulo 1.3.5 arquitectura de la pierna humana, y a


las articulaciones que presentan; se define disear un robot Bpedo de 10GDL, 5 GDL por
cada pierna unidos mediante un eslabn (cadera).
3.1.- Cinemtica directa del robot bpedo 10GDL:

Se trabaja con los valores de la cinemtica directa para 2 puntos importantes del
robot bpedo 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 articulacin del robot X, Y, Z para poder
determinar los parmetros de Denavit Hartenberg.

24
UNIVERSIDAD NACIONAL DE TRUJILLO
C
Xc
X0d Zc
X0d

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

Figura 3.1 Diagrama ubicacin de ejes - Criterio Denavit Hartenberg.


(Fuente: Elaboracin propia)
Leyenda:
Eje Rojo: X Eje Amarillo: Y
Eje Verde: Z

25
UNIVERSIDAD NACIONAL DE TRUJILLO
En el grfico 3.1 se observa que se coloc los ejes considerando la pierna derecha
como pie de apoyo, y la pierna izquierda la flotante, la cual se obtendr la cinemtica directa,
de la cadera con respecto al pie de apoyo y despus la cinemtica directa del pie flotante
con respecto a la cadera, desde esta forma se obtiene los puntos de inters para el clculo
de la cinemtica inversa.

3.1.1 Cinemtica directa de la cadera con respecto al pie de apoyo:

Se utiliza el Mtodo de D-H ya que tiene la virtud de posicionar siempre el eje de


rotacin de cada articulacin al eje z de cada uno de los marcos de referencia, se muestra
las medidas en la figura 3.2

L1 L1

L2 L2

L3
L3

L4 L4

L5

L5

L6 L6

L7
L7

Figura 3.2.- Diagrama dimensiones simblicas de eslabones Robot 10GDL


(Fuente: Elaboracin propia)

26
UNIVERSIDAD NACIONAL DE TRUJILLO
Se toma las siguientes dimensiones, teniendo en cuenta que las dimensin del
prototipo de robot bpedo10GDL no debe superar los 50cm de altura, las piezas deben
poder soportar las dimensiones de Servomotor DC MG996R (planos se muestran en
anexos C) y deben tener relacin en las medidas de una pierna humana (aprox. Cadera
rodilla: 52%, rodilla pie: 48 % respecto a cadera -pie) fuente [11].

Se toma las siguientes dimensiones que se muestran la figura 3.2.

L1 = 45.2 mm L5 =47.3 mm
L2 =28.8 mm L6 =46.8 mm
L3 =47.4 mm L7 =17.4 mm
L4 = 64.0 mm

Los parmetros de Denavit Hartenberg , se muestra en la tabla 3.1

Tabla 3.1. Parmetros de DH del robot Bpedo 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 rotacin definidas en cada articulacin, pierna
derecha, como se muestra en la figura 3.3

Tomando en cuenta las distancias mximas de los eslabones, las matrices de


transformacin homogneas y su relacin directa entre eslabones se determina la matriz
final. Se observa que como es con respecto al pie de apoyo se usa 5GDL para as tomar
en cuenta el movimiento de una sola pierna del bpedo.

Una vez que se establece el sistema de coordenadas D-H para cada elemento se
procede a desarrollar una matriz de transformacin homognea que relaciona el sistema i-
simo con el sistema (i-1) simo dado la siguiente matriz, ya definida:


1
= [ ] (3.1)
0
0 0 0 1

27
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.3 Variables articulares de rotacin referida a cada articulacin.


(Fuente: Elaboracin propia)

Reemplazando en la ecuacin 3.1 con los datos de tabla 3.1, en el algoritmo realizado en
Matlab (Anexos B) se tiene las siguientes matrices de trasformacin homogneas:

cos(q5d), 0 sin(q5d) L6 cos(q5d)


4
3 = ( sin(q5d) 0 cos(q5d), L6 sin(q5d)) (3.2)
0 1 0 0
0 0 0 1

28
UNIVERSIDAD NACIONAL DE TRUJILLO
cos(q4d pi/6), sin(q4d pi/6) 0 L5 cos(q4d pi/6)
3
2 = ( sin(q4d pi/6) cos(q4d pi/6) 0 L5 sin(q4d pi/6)) (3.3)
0 0 1 0
0 0 0 1

cos(q3d + pi/3) sin(q3d + pi/3) 0 L4 cos(q3d + pi/3)


2
1 = ( sin(q3d + pi/3) cos(q3d + pi/3) 0 L4 sin(q3d + pi/3)) (3.4)
0 0 1 0
0 0 0 1

cos(q2d pi/6) 0 sin(q2d pi/6) L3 cos(q2d pi/6)


1
0 = ( sin(q2d pi/6) 0 cos(q2d pi/6) L3 sin(q2d pi/6)]
) (3.5)
0 1 1 0
0 0 0 1

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 anlisis del robot falta todava multiplicar por la matriz de
transformacin homognea 54 y la matriz 0 , las matrices se muestran a
continuacin:

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 homogneas 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 0dT0d 0dTc (3.9)

29
UNIVERSIDAD NACIONAL DE TRUJILLO
Resolviendo la ecuacin 3.9 se obtiene las siguiente matriz de transformacin
homognea (3.10) los valores de los elementos de esta matriz se muestran en el anexo A.

r11 r12 r13 r14


5d r21 r22 r23 r24
TC = ( ) (3.10)
r31 r32 r33 r34
0 0 0 1

Las pruebas de funcionamiento de la cinemtica directa se muestran en la figura


3.4 realizado en Labview2014.

Figura 3.4 Prueba de la cinemtica directa de la cadera respecto a pie de apoyo.


(Fuente: Elaboracin propia)

30
UNIVERSIDAD NACIONAL DE TRUJILLO
3.1.2.- Cinemtica directa del pie flotante con respecto a la cadera:

Los parmetros de Denavit Hartenberg se muestra en la tabla 3.2. Las variables


q1i, q2i, q3i, q4i, q5i se muestran en el grfico 3.3 y las longitudes en la figura 3.2

Tabla 3.2. Parmetros de D H del Bpedo10 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 bpedo. Es importante aclara que la articulacin
1 empieza en el sistema de referencia 0, y termina en el punto PF.

Siguiendo la nomenclatura las matrices de transformacin homogneas


encontradas, tomando en cuenta la matriz 3.1, son las siguientes:

cos(q1i) 0 sin(q1i) L3 cos(q1i)


0
1 = ( sin(q1i) 0 cos(q1i) L3 sin(q1i)
) (3.11)
0 1 0 0
0 0 0 1

cos(q2i + pi/6) sin(q2i + pi/6) 0 L4 cos(q2i + pi/6)


1
2 = ( sin(q2i + pi/6) cos(q2i + pi/6) 0 L4 sin(q2i + pi/6)
) (3.12)
0 0 1 0
0 0 0 1

cos(q3i pi/3) sin(q3i pi/3) 0 L5 cos(q3i pi/3)


2
3 =( sin(q3i pi/3) cos(q3i pi/3) 0 L5 sin(q3i pi/3) ) (3.13)
0 0 1 0
0 0 0 1

cos(q4i + pi/6) 0 sin(q4i + pi/6) L6 cos(q4i + pi/6)


3
4 =( sin(q4i + pi/6) 0 cos(q4i + pi/6) L6 sin(q4i + pi/6) ) (3.14)
0 1 1 0
0 0 0 1

31
UNIVERSIDAD NACIONAL DE TRUJILLO
cos(q5i) sin(q5i) 0 L7i cos(q5i)
4
5 =( sin(q5i) cos(q5i) 0 L7i cos(q5i) ) (3.15)
0 0 1 0
0 0 0 1

Como se desea obtener la posicin 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

Se procede a multiplicar las matrices de trasformacin homogneas, para as


referenciar el sistema de coordenadas del pie flotante al sistema de coordenadas de la
cadera:

C
T5i = CT0i 0iT1i 1iT2i 2iT3i 3iT4i 4iT5i (3.17)

Una vez que se resuelve la ecuacin 3.17 se obtiene la siguiente matriz 3.18,
cuyos elementos se muestran en Anexo A.2

11 12 13 14
21 22 23 24
5 = ( ) (3.18)
31 32 33 34
0 0 0 1

Teniendo en cuenta la matriz resultante que esta referenciado el sistema de


coordenadas del pie flotante al sistema de coordenadas de la cadera, es posible referenciar
el pie flotante con el sistema de coordenadas del pie de apoyo esto con solo multiplicar
las matrices de las ecuaciones 3.10 y 3.18.

5 5
5 = 5 (3.19)

Los resultados de la ecuacin 3.19 se tienen en la matriz siguiente, y los elementos de la


matriz se muestran en Anexos A.3,

11 12 13 14
5 21 22 23 24
5 = ( ) (3.20)
31 32 33 34
0 0 0 1
Se muestra la figura 3.5 prueba la cinemtica directa del pie flotante respecto a pie
de apoyo se realiz en LabView 2014

32
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.5 Prueba de la cinemtica directa del pie flotante respecto al pie de apoyo.
(Fuente: Elaboracin propia)

La ecuacin 3.20 es fundamental para resolver la cinemtica inversa para una


trayectoria dada en el pie flotante ya que este punto debe de estar referenciado al pie de
apoyo. Por lo que resultados obtenidos son los siguientes:

33
UNIVERSIDAD NACIONAL DE TRUJILLO

1.- La cinemtica directa del robot bpedo.

2.- Se referencia el sistema de coordenadas de la cadera al sistema de coordenadas


del pie de apoyo.

3.- Se referencia el sistema de coordenadas del pie flotante al sistema de coordenadas


de la cadera.

4.- Se referencia el sistema de coordenadas del pie flotante al sistema de coordenadas


del pie de apoyo.

Tener en cuenta cuando se realiza el cambio de pie (Izquierda: Pie de Apoyo;


derecha: pie flotante) se modifican las siguiente matrices, por la variacin de los sistemas
de referencia.

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

3.2.- Cinemtica inversa del robot bpedo de 10GDL:


Segn lo expresado en el captulo 1.3.1.2 de Cinemtica inversa se tiene:

3.2.1.- Paso a seguir en el algoritmo implementado:

Consiste en determinar la configuracin que debe adoptar un robot para obtener una
posicin y orientacin determinada del efector final. La cual se puede representar mediante
la siguiente ecuacin:
= 1 ()
(3.22)

Dnde: q= variables de articulacin.


x= son las ecuaciones en coordenadas cartesianas.

34
UNIVERSIDAD NACIONAL DE TRUJILLO

f1 f4 f7 f10
f2 f5 f8 f11
( )= ( ) (3.23)
f3 f6 f9 f12
0 0 0 1 0 0 0 1

De la ecuacin (3.23) la 2 matriz de la Igualdad representa la posicin y orientacin


deseada del efector final del robot. Debido a que las ecuaciones son altamente no lineales
f, se opta por linealizar el sistema de ecuaciones, por lo que se plantea lo siguiente:

1
2
3
4
5
6
() = f() = (3.24)
7
8

9

10

11
( )
(12)

Dnde:

f() : Funcin a linealizar


f(x) : Set point.

Para linealizar utilizamos la Serie de Taylor en cada funcin f para que cada funcin
dependa de una sola variable a solucionar, se muestra la ecuacin 3.25, la linealizacin de
todas las funciones:

f()
f(x) = f() + =1 * ( ) (3.25)

Se desarrolla las operaciones, la cual se tiene 5 grados de libertad (cadera al pie de


apoyo) o 10 grados de Libertad (pie flotante al pie de apoyo), y el nmero de funciones a
linealizar son 12, se puede hacer que las ecuaciones resultantes estn expresadas en la
siguiente forma:

Se calcula el error: e= f(x)- f(), mediante el mtodo de IAE, se utiliza para que
cumpla cierta condicin y salga del bucle.

35
UNIVERSIDAD NACIONAL DE TRUJILLO
)
(1 )
(1 )
(1 )
(1 )
(1
1 2 3 4 5
) )
(2 )
(2 )
(2 )
(2
(2
1 2 3 4 5
) )
(3 )
(3 )
(3 )
(3
)
(1) (1 (3
1 2 3 4 5
)
(2) (2 )
(4 )
(4 )
(4 )
(4 )
(4
)
(3) (3 1 2 3 4 5
) ) ) )
)
(4) (4 )
(5 (5 (5 (5 (5

) 1 2 3 4 5 1 1
(5) (5 ) ) )
)
)
(6 (6 (6 (6 )
(6 2 2
(6) (6 1 2 3 4 5
)
= ) )
(7 )
(7 )
(7 ) 3 3 (3.26)
(7) (7 (7 (7
) 1 2 3 4 5 4 4
(8) (8
)
)
(8 )
(8 )
(8 )
(8 )
(8 (5 4 )
(9) (9 1 2 3 4 5
)
(10) (10 )
(9 )
(9 )
(9 )
(9 )
(9
)
(11) (11 1 2 3 4 5
)
(10
(10) )
(10 0)
(1 )
(10
2))
((12) (1 1 2 3 4 5
)
(11 )
(11 )
(11 )
(11 )
(11
1 2 3 4 5
)
(12 )
(12 2)
(1 )
(12 )
(12
( 1 2 3 4 5 )

De la ecuacin (3.26) queda representado:

= (3.27)

J : Matriz Jacobiana.

De la ecuacin (3.27) se tiene:

= 1 (3.28)

Para encontrar la inversa de la matriz jacobina al no ser cuadrtico, se tiene aplicar


otros mtodos. En este caso se evalu (segn Fuente [3])

El mtodo Damped Least Squares o llamado tambin: Levenberg Marquardt


Method se utiliza:

= ( + 2 )1 (3.29)

Donde:
e= (error)
J: la transpuesta de una matriz Jacobiano.
I: matriz identidad
: Constante de convergencia.

36
UNIVERSIDAD NACIONAL DE TRUJILLO
Si la constante de Convergencia = 0, entonces estaramos en el caso del mtodo
de Moore Penrose, S > 0 mejor comportamiento del algoritmo pero convergencia ms
lenta.

Para obtener los ngulos calculados simplemente:

= + (3.30)

Se resume el algoritmo en este diagrama de flujo:

Cinemtica
Inversa

Calculo de la posicin y Orientacin del


efector final, en el estado actual del robot.
f(x)

Calculo del error entre la posicin objetivo


y la posicin actual. e=f(x) f( )

Verificacin del error y fin de la subrutina


si: ISE < k, donde k: constante

Calculo la matriz Jacobiana numricamente

Calculo el Incremento de:

= ( + 2 )1

= +

Estandarizacin y restricciones de los


ngulos obtenidos.

37
UNIVERSIDAD NACIONAL DE TRUJILLO

3.2.2 Cinemtica inversa de la cadera del robot bpedo 10GDL.

Para calcular la cinemtica inversa de la cadera al pie de apoyo utilizamos las


ecuaciones obtenidas por la cinemtica directa de la ecuacin 3.10.

ff1 ff4 ff7 ff10


ff2 ff5 ff8 ff11
( ) = (
)
ff3 ff6 ff9 f1f2
0 0 0 1 0 0 0 1

En el algoritmo implementado en Matlab se da valores iniciales a los ngulos


q5d=q4d=q3d=q2d=q1d=0.
Para hacer el clculo ms rpido se discretiza todas las ecuaciones dadas adems
los Jacobianos, se trabaja numricamente, mediante Incrementos finitos.

() (+)()

= lim
(3.31)

De aqu se tiene, si se toma valores discretos:

() (+)()

=
(3.32)

Se tomara un incremento de dx=0.0001 para cada articulacin:

dp5=q5d+dx;
dp4=q4d+dx;
dp3=q3d+dx; (3.33)
dp2=q2d+dx;
dp1=q1d+dx;

Se tomar en cuenta que realiza 1000 interacciones o en todo caso que salga del
bucle si s<0.0001 siendo s aplicando el Criterio Integral del Error Absoluto.

Por ltimo se realiza una estandarizacin de ngulos:

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

El la figura 3.6 se muestra la prueba de la cinemtica inversa de cadera respecto


al pie de apoyo realizado en Labview2014.

38
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.6 Prueba de la Cinemtica Inversa de la cadera respecto al pie de apoyo.


(Fuente: Elaboracin propia)

El algoritmo completo se implement en Matlab, (se encuentra en anexos B), se


prueba con las trayectorias de movimiento propuestas ms adelante, los resultados se
presentan a continuacin, las variables articulares hacen referencia a la figura 3.3:

39
UNIVERSIDAD NACIONAL DE TRUJILLO

Pierna apoyo q5d - Plena Marcha.


0.045

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
Nmero de divisiones tiempo (s)

Figura 3.7.- Posicin angular de q5d-plena marcha.


(Fuente: Elaboracin propia)

Pierna de apoyo q4d- Plena Marcha.


0

-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
Nmero de divisiones tiempo(s)

Figura 3.8.- Posicin angular de q4d-plena marcha.


(Fuente: Elaboracin propia)

40
UNIVERSIDAD NACIONAL DE TRUJILLO

Pierna de apoyo q3d - Plena Marcha.


0.03

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
Nmero de divisiones tiempo(s)

Figura 3.9.- Posicin angular de q3d-plena marcha.


(Fuente: Elaboracin propia)

Pierna de apoyo q2d- Plena Marcha.


0.08

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
Nmero de divisiones tiempo (s)

Figura 3.10.- Posicin angular de q2d-plena marcha.


(Fuente: Elaboracin propia)

41
UNIVERSIDAD NACIONAL DE TRUJILLO

Pierna de apoyo q1d- Plena Marcha.


0.04

0.035

0.03

q1d (radianes) 0.025

0.02

0.015

0.01

0.005

-0.005

-0.01
0 10 20 30 40 50 60 70 80 90 100
Nmero de divisiones tiempo (s)

Figura 3.11.- Posicin angular de q1d-plena marcha.


(Fuente: Elaboracin propia)

3.2.3.- Cinemtica inversa del pie flotante del robot bpedo 10GDL.

Para encontrar la cinemtica inversa del pie flotante se realiz un procedimiento


similar al que se describi anteriormente, se tomas las ecuaciones de la cinemtica directa
del pie flotante respecto al pie de apoyo (ecuacin que se muestra anexo en al captulo
A3.).

ff1 ff4 ff7 ff10


ff2 ff5 ff8 ff11
( ) = (
)
ff3 ff6 ff9 f1f2
0 0 0 1 0 0 0 1

Despus de esto se resuelve el mismo procedimiento presentado desde la ecuacin


3.22 hasta la 3.34. El algoritmo se desarroll en Matlab (se muestra en anexos B) los
resultados se muestran a continuacin referidos a q1i, q2i, q3i, q4i, q5i, estas variables
articulares se presentan en la figura 3.3

42
UNIVERSIDAD NACIONAL DE TRUJILLO

Pierna flotante q1i- Plena Marcha.


0.01

-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
Nmero de divisiones tiempo (s)

Figura 3.12.- Posicin angular de q1i-plena marcha.


(Fuente: Elaboracin propia)

Pierna flotante q2i - Plena Marcha.


0.2

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
Nmero de divisiones tiempo (s)

Figura 3.13.- Posicin angular de q2i-plena marcha.


(Fuente: Elaboracin propia)

43
UNIVERSIDAD NACIONAL DE TRUJILLO

Pierna flotante q3i- Plena Marcha.


10

0
q3i (radianes)

-5

-10

-15

-20
0 10 20 30 40 50 60 70 80 90 100
Nmero de divisiones tiempo (s)

Figura 3.14.- Posicin angular de q3i-plena marcha.


(Fuente: Elaboracin propia)

Pierna flotante q4i- Plena Marcha.


15

10

5
q4i (radianes)

-5

-10

-15

-20
0 10 20 30 40 50 60 70 80 90 100
Nmero de divisiones tiempo (s)

Figura 3.15.- Posicin angular de q4i-plena marcha.


(Fuente: Elaboracin propia)

44
UNIVERSIDAD NACIONAL DE TRUJILLO

Pierna flotante 5i- Plena Marcha.


3.5

2.5

q5i (radianes)
2

1.5

0.5

0
0 10 20 30 40 50 60 70 80 90 100
Nmero de divisiones tiempo (s)

Figura 3.16.- Posicin angular de q5i-plena marcha.


(Fuente: Elaboracin propia)

3.3.- Anlisis Dinmico:


Con el expuesto en el captulo 1.3.2. Y las ecuaciones deducidas aplicando el criterio
de Newton Euler, para encontrar los torques producidos en cada articulacin, se tiene lo
siguiente:

3.3.1.- Algoritmo Computacional:

En resumen del anterior expuesto las ecuaciones de Newton Euler la cual se tiene
un conjunto de ecuaciones recursivas hacia adelante y hacia atrs para obtener un
algoritmo computacional.

Se tiene ( 1 )1 = 1 = ( 1 )

En lugar de calcular , , , , , , , , , que se refieren al sistema


coordenadas de la base.
Calculamos , , , , , ,

, , , que se refieren a su propio sistema de
coordenadas del elemento (xi, yi, zi). Por lo tanto las ecuaciones recursivas de N-E
quedan expresadas en la siguiente tabla 3.1.

45
UNIVERSIDAD NACIONAL DE TRUJILLO
Ecuaciones hacia adelante: i=1,2,3..,n


1 ( 10 1 + 0 )
0 = { 1
(3.1.1)
1 ( 0 1 )


1 ( 10 1 + 0 + ( 10 1 ) (0 ))
0 = {
1 ( 0 1
)
(3.28)


0 = {( 0 )( 0 ) + ( 0 )[( 0 )( 0 )] +
1 ( 10 1 ) (3.1.2)
Si el Elemento i es rotacional.


0 = ( 0 )( 0 ) + ( 0 )[( 0 )( 0 )] + 0 (3.1.3)

Ecuaciones hacia atrs i = n, n-1,n-2,,1


0 = +1 ( +10 +1 ) + 0 (3.1.4)


0 = +1 [ +10 +1 + ( +10 )( +10 +1 )] + ( 0 + 0 )( 0 ) +
( 0 0 )( 0 ) + ( 0 )[( 0 0 )( 0 )] (3.32)


= ( 0 ) ( 1 0 ) + (3.1.5)

Tabla 3.1 ecuaciones recursivas de Newton Euler.


(Fuente: [2] Pg. 112)
Se aplic este mtodo para el clculo de la dinmica del robot bpedo 10 GDL,
para un mejor estudio, se decidi dividir el anlisis en 2 partes, cadera respecto al pie de
apoyo, y pie flotante respecto a Cadera.

Dnde z0 = (0,0,1) "" es el coeficiente de amortiguamiento viscoso para la


articulacin i. Las condiciones iniciales usuales son:
0 = 0 = 0 = 0
0 = ( , , ) Para incluir la aceleracin de la gravedad.
Donde g=9.81 m/s^2.

46
UNIVERSIDAD NACIONAL DE TRUJILLO
3.3.2 Dinmica Newton Euler del robot bpedo 10 GDL:

3.3.2.1 Dinmica del pie flotante respecto a la cadera:

Las ecuaciones de la tabla 3.1 se program en Matlab para el clculo de los torques
de los 5 GDL del robot bpedo (pie flotante a la cadera). Los ngulos calculados de la
cinemtica inversa sirven para entrada a la funcin para el clculo de las torques.

Las velocidades y aceleraciones angulares, se estima en funcin al tiempo estimado


a realizar la trayectoria, la cual se dividi en n=100 puntos; es decir la velocidad se toma
como el cambi posicin de un punto a otro, e un tiempo determinado y la aceleracin el
cambio de velocidad en un tiempo determinado, como se muestra en las ecuaciones 3.1.6
y 3.1.7.

( 1 )
= (3.1.6)


( )
= 1 (3.1.7)

Dnde:
n : Parte que se divide la trayectoria.
: Posicin de la articulacin en funcin de n.
: Velocidad la articulacin en funcin de n.
: Aceleracin la articulacin en funcin de n.

El algoritmo empieza con definir los parmetros de Denavit-Hartenberg del robot


(pierna flotante) en funcin a la figura 3.1, luego se definen las masas de los elementos
(eslabones + masa del servomotor) los datos se presentan en anexos C.

m1=0.0876; elemento L3 (Pierna Flotante)


m2=0.1045; elemento L4
m3=0.0876; elemento L5
m4=0.0876; elemento L6
m5=0.1039; elemento L7

Luego se Incluyen las matrices de Inercia, referidos a su CG de cada eslabn, se


define las matrices de rotacin y se sigue el algoritmo con las ecuaciones descritas en la
tabla 3.1, la cual para el clculo se define las siguientes variables:

Siendo:
q1i, q2i, q3i, q4i, q5i : Posiciones articulares.
qd1i, qd2i, qd3i, qd4i, qd5i : Velocidades articulares.
qdd1i, qdd2i, qdd3i, qdd4i, qdd5i : Aceleraciones articulares.
L3, L4, L 5, L6, L7 : Longitudes de los eslabones.
m1, m2, m3, m4, m5 : Masa de cada eslabn
g= : aceleracin de la gravedad.
Mext : Mexterna aplicado el pie flotante
: Momento de inercia aplicado en el pie
flotante externo

47
UNIVERSIDAD NACIONAL DE TRUJILLO
I1i , I2i , I3i, I4i, I5i : Momentos de inercia de cada eslabn referido a su mismo sistema
de referencia.
b1, b2, b3, b4, b5: Coeficiente de rozamiento Viscoso de los eslabones del robot.

Las posiciones angulares, se definen en la figura 3.3.1, los resultados de las


ecuaciones encontradas para los torques se muestran en anexos A.4.

Figura 3.3.1 Muestran las variables articulares y los torques


producidos en la pierna flotante.

Al remplazar todos las variables descritas anteriormente con los valores numricos
respectivos (dimensiones de los eslabones, masas (incluyen masa del eslabn y masa del
Servomotor), posiciones articulares, velocidades articulares, aceleraciones articulares,

48
UNIVERSIDAD NACIONAL DE TRUJILLO
momentos de Inercia) se obtienen las los torques en N*m de cada articulacin del robot
bpedo del pie Flotante respecto a la cadera.

Tener en cuenta: Tau1 est referida a la articulacin qi1, Tau2 referida al q2i, Tau3
referida a la articulacin q3i, Tau4 est referida a la articulacin q4i, Tau5 est referida a la
articulacin q5i.

Parametro de Denavit-Hartenberg del robot:

L3=0.0474; Longitudes en Metros.


L4=0.064;
L5=0.0473;
L6=0.0468;
L7=0.0174;

theta=[qi(1); qi(2)+pi/6; qi(3)-pi/3; qi(4)+pi/6; qi(5) ];


d=[0; 0; 0; 0; 0];
a=[L3; L4; L5; L6; L7 ];
alpha=[pi/2; 0; 0;-pi/2; 0];
DH=[theta d a alpha];

Se asumen el coeficiente de rozamiento viscoso de cada articulacin:

b1=0; Coeficiente viscoso elemento1


b2=0; Coeficiente viscoso elemento2
b3=0; Coeficiente viscoso elemento3
b4=0; Coeficiente viscoso elemento4
b5=0; Coeficiente viscoso elemento5

El algoritmo se implement en Matlab2013 se muestra en el anexo B, las


ecuaciones de cada torques se muestran en anexos A, y se tiene los siguientes
resultados:

49
UNIVERSIDAD NACIONAL DE TRUJILLO

ROBOT BIPEDO: GRFICO DE TORQUES PIE FLOTANTE - CADERA.


0.1
Torque 1i
Torque 2i
Torque 3i
Torque 4i
Torque 5i
0.05

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: Elaboracin propia)

ROBOT BPEDO: GRFICO DE VELOCIDADES PIE FLOTANTE-CADERA


2
Velocidad 1i
1.5 Velocidad 2i
Velocidad 3i
Velocidad 4i
1
Velocidad 5i

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

Figura 3.3.3 Velocidades Articulares del Pie flotante a la Cadera.


(Fuente: Elaboracin propia)

50
UNIVERSIDAD NACIONAL DE TRUJILLO

ROBOT BPEDO: GRFICO DE ACELERACIONES PE FLOTANTE-CADERA


2
Aceleracin 1i
1.5 Aceleracin 2i
Aceleracin 3i
1 Aceleracin 4i
Aceleracin 5i
0.5

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

Figura 3.3.4 Aceleraciones Articulares del Pie flotante a la Cadera.


(Fuente: Elaboracin propia)

ROBOT BPEDO: GRFICO DE POTENCIAS DEL PIE FLOTANTE


0.1
Potencia 1i
Potencia 2i
Potencia 3i
0.05
Potencia 4i
Potencia 5i

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

Figura 3.3.5 Grfico de Potencia en cada articulacin en la


Pierna Flotante (Fuente: Elaboracin propia)

51
UNIVERSIDAD NACIONAL DE TRUJILLO
3.3.2.2 Dinmica de la cadera respecto al pie de apoyo:

Las ecuaciones de la tabla 3.1 se program en Matlab para el clculo de los torques
de las 5 GDL del robot bpedo (de la cadera respecto al pie de apoyo). Los ngulos
calculados de la cinemtica inversa sirven para entrada a la funcin para el clculo de las
torques, como las velocidades articulares, aceleraciones articulares.

Siendo:

q5d, q4d, q3d, q2d, q1d : Posiciones articulares.


qd1d, qd2d, qd3d, qd4d, qd5d : Velocidades articulares.
qdd1d, qdd2d, qdd3d, qdd4d, qdd5d : Aceleraciones articulares.
L3, L4, L5, L6, L7 : Longitudes de los eslabones.
m1, m2, m3, m4, m5 : Masa de cada eslabn
g : Aceleracin de la gravedad.
Mext : Mexterna aplicado el pie flotante
Iext : Momento de inercia aplicado en el pie
flotante externo.
I1d, I2d, I3d, I4d, I5d : Momentos de inercia de cada eslabn
referido a su mismo sistema de referencia.
b1, b2, b3, b4, b5 : Coeficiente de rozamiento Viscoso de los
eslabones del robot

Los parmetros de los torques de las Pierna de apoyo se presentan en la figura 3.3.6 con
las variables articulares respectivas.

52
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.3.6 Muestran las variables articulares y los torques


Producidos en la pierna flotante.

Las ecuaciones que se obtiene son las siguientes que relacionan las torques con
cada articulacin. Siendo:
tau1 = est referida a la articulacin q5d
tau2 = est referida a la articulacin q4d
tau3 = est referida a la articulacin q3d
tau4 = est referida a la articulacin q2d
tau5 = est referida a la articulacin q1d

53
UNIVERSIDAD NACIONAL DE TRUJILLO
Las ecuaciones de los torques de la pierna de apoyo se presentan en anexos A.5
Al remplazar todos las variables descritas anteriormente con los valores numricos
respectivos (dimensiones de los eslabones, masas (incluyen masa del eslabn y masa del
Servomotor), posiciones articulares, velocidades articulares (figura 3.3.8), aceleraciones
articulares (figura 3.3.9), los momentos de inercia se obtienen las los torques en N*m (figura
3.3.7) de cada articulacin del robot bpedo del pie Flotante respecto a la cadera.

Parmetro de Denavit-Hartenberg del robot

L6=0.0468; Longitudes en Metros.


L5=0.0473;
L4=0.064;
L3=0.0474;

theta=[qd(1); qd(2)-pi/6; qd(3)+pi/3; qd(4)-pi/6; qd(5) ];


d=[0; 0; 0; 0; 0];
a=[L6; L5; L4; L3; 0 ];
alpha=[-pi/2; 0; 0; pi/2; 0];
DH=[theta d a alpha];

Masa de cada elemento en Kg.

m1=(0.1039+0.0876); elemento L6 (izquierda)


m2=0.0876; elemento L5 (izquierda)
m3=0.1045; elemento L4 (izquierda)
m4=0.0876; elemento L3 (izquierda)
m5 elemento L2 (izquierda)(Se lo va Incluir en la Masa Externa)

Se asumen los coeficientes de rozamiento viscoso de cada articulacin.

b1=0; Coeficiente viscoso elemento1


b2=0; Coeficiente viscoso elemento2
b3=0; Coeficiente viscoso elemento3
b4=0; Coeficiente viscoso elemento4
b5=0; Coeficiente viscoso elemento5

El algoritmo se implement en Matlab y se muestra en anexos B, se obtiene los


Siguientes resultados:

54
UNIVERSIDAD NACIONAL DE TRUJILLO

ROBOT BIPEDO: GRFICO DE TORQUES CADERA A PIE DE APOYO


0.25
Torque 5d
Torque 4d
0.2
Torque 3d
Torque 2d
0.15 Torque 1d

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: Elaboracin propia)

ROBOT BPEDO: GRFICO DE VELOCIDADES CADERA A PIE DE APOYO


0.15
Velocidad 5d
0.1 Velocidad 4d
Velocidad 3d
Velocidad 2d
0.05
Velocidad 1d

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: Elaboracin propia)

55
UNIVERSIDAD NACIONAL DE TRUJILLO

ROBOT BPEDO: GRFICO DE ACELERACIONES CADERA A PIE DE APOYO


2
Aceleracin 5d
1.5 Aceleracin 4d
Aceleracin 3d
Aceleracin 2d
1
Aceleracin 1d

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: Elaboracin propia)

ROBOT BPEDO: GRFICO DE POTENCIAS CADERA A PIE DE APOYO


0.03
Potencia 5d
0.02 Potencia 4d
Potencia 3d
Potencia 2d
0.01
Potencia 1d

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

Figura 3.3.10 Grfico de potencia en cada articulacin en la


Pierna de Apoyo (Fuente: Elaboracin propia)

56
UNIVERSIDAD NACIONAL DE TRUJILLO
3.4.- Arquitectura de Robot Bpedo 10GDL.

3.4.1 Modelo CAD 3D del Robot Bpedo 10GDL.

El Robot Bpedo 10GDL, est constituido por 2 piernas, 5 eslabones unidas entre s
mediante un eslabn central al que, en este tesis se llamara cadera, sus 10 eslabones se
conectan entre s a travs de juntas rotaciones actuadas por servomotores. En la figura
3.4.1 se muestra el modelo CAD 3D del bpedo, en la configuracin espacial que adoptan
sus servomotores en posicin neutral.

Figura 3.4.1 Modelo CAD 3D del bpedo 10GDL.


(Fuente: Elaboracin propia)

La cul de la figura 3.2. Nos muestran las dimensiones dadas, de cada eslabn.
Los planos de robot Bpedo, as como las dimensiones, masa de cada pieza, masa y
dimensiones de los servomotores se presentara en Anexos C.

57
UNIVERSIDAD NACIONAL DE TRUJILLO
3.4.2 Modelo CAD 3D Simplificado Robot Bpedo 10GDL.

Tambin se muestra un modelo simplificado, que se mostr en el anlisis


cinemtico, en que se apreciar que las juntas son representadas por medio de cilindros
cuyos ejes coinciden con el de los servomotores y se unen a travs de barras. En la figura
3.4.2

Figura 3.4.2 Modelo CAD 3d simplificado del robot bpedo 10GDL.


(Fuente: Elaboracin propia.)

58
UNIVERSIDAD NACIONAL DE TRUJILLO
3.5.- Planificacin de la caminata de robot bpedo 10GDL.
3.5.1 Control cinemtico:
Recibe como entrada los datos procedentes del programa del robot escrito por el
usuario (punto de destino, precisin, tipo de trayectoria, etc.) apoyndose en el modelo
cinemtico del robot, establece las trayectorias para cada articulacin como funcin del
tiempo.
3.5.2.- Trayectorias robot bpedo 10GDL:
La caminata bpeda es un desafo para la robtica, por lo que planificacin de
caminata bpeda tiene como objetivo generar las secuencias y movimientos para lograr la
caminata bpedo es sus distintas fases. Para luego analizar la estabilidad del robot durante
su caminata.
En esta tesis se analiza la planificacin de la caminata bpeda tanto en los planos
sagitales (lateral) y frontal (como se describe en el captulo 1.3.3.2), adems se desarrollar
un programa en Matlab.
3.5.3.- Interpolacin de trayectorias:
Para asegurar que la trayectoria presente continuidad en velocidad y aceleracin,
puede recurrir a utilizar polinomios de grado 3, Este conjunto de polinomios concatenados,
se denomina splines cbico. [Fuente [2], pag. 178]

Figura 3.5.1 Semiciclo de caminata en el plano sagital.

Para el caso el robot bpedo, en la figura 3.5.1 se esquematiza el estado inicial y


final de un paso de caminata, dnde PA es pierna de apoyo, PF1 es pierna flotante en
estado inicial; PF2 pierna flotante en estado final, C1 es cadera en estado inicial, C2
cadera en el estado final.
La Ecuacin spline cbica se muestra a continuacin:
() = + + 2 + 3 (3.5.1)

Donde a,b: constantes de la ecuacin: t [0 ]

59
UNIVERSIDAD NACIONAL DE TRUJILLO
Evaluando la ecuacin en un extremo del intervalo:
X(0)=0 = (3.5.2)

() = = + + 2 + 3 (3.5.3)
Y a su vez, evaluando la primera derivada en los mismos puntos:
X(0) = 0 =

() = 1 = + 2 + 3 2 (3.5.4)
Resolviendo el sistema formado por las 4 ecuaciones: a,b,c,d
a = 0 (3.5.5)
b = 0 (3.5.6)
3(1 0 ) (20 +1 )
c= 2
(3.5.7)

2(01 )+ (0 +1 )
d= (3.5.8)
3

De la ecuacin 3.5.1, y la ecuacin 3.5.5, 3.5.6 , 3.5.7, y 3.5.8 se tiene en la ecuacin:


3(1 ) (20 +1 ) 2 2(0 1 )+ (0 +1 ) 3
() = 0 + + [ 2
] +[ 3
] (3.5.9)

En donde:
Ti = Periodo de Interpolacin.
0 = Posicin Inicial.
1 = Posicin final
0 = Velocidad inicial.

La ecuacin se evala con los siguientes valores:


t=0:0.01:1;
v0=0; v1=0;
x0=1;x1=2;Ti=1; se muestra en la figura 3.5.2

60
UNIVERSIDAD NACIONAL DE TRUJILLO
Interpolador Cubico
2

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)

Figura 3.5.2 Interpolador cbico.


(Fuente: Elaboracin propia)

3.5.4.- Planificacin de la cadera respecto al pie de apoyo.

3.5.4.1.- Plano sagital:


Como se observa en la figura 3.5.3 la planificacin de la cadera en el plano sagital se lo
realizara en el eje Z, y X.

Figura 3.5.3 Planeacin de caminata en Plano Sagital.

61
UNIVERSIDAD NACIONAL DE TRUJILLO
Dnde:
- Hcadera : altura que se eleva la cadera respecto a la altura del bpedo inicial.
- hBped : Altura del Bpedo en su posicin Inicial.
- Hpflotante : es la altura que se eleva el pie flotante respecto al piso.
- SL : longitud de paso.
- HSL : Longitud de Paso Cadera.

La trayectoria de la cadera en el eje Z, tomando como punto de referencia el


Punto PA, se realiza la planeacin teniendo en cuenta la ecuacin: 3.5.9
Se toma como Referencia HSL = SL/2.

i) Cadera:
z0=-SL/2;
z1=SL/2; Donde: 0 < t TP
v0=0;
v1=0;
T1=Tp;
Reemplazando en la ecuacin 3.5.9 se tiene:
2 3
Zc(t) = +3 2 (3.5.10)
2 2 3

Siendo TP: es el tiempo de paso. TP=t*k. k=100.


Para la Simulacin se toma los siguientes valores del Robot Bpedo de 10GDL.
SL=0.020; Lp = Longitud de paso
hBiped=0.23678; h = altura de la cadera
hpflotante=0.010; hp = elevacin de la pierna flotante. (3.5.11)
hcadera=0.005; hc = elevacin de la cadera
Tp= 1 Tp = tiempo empleado para cado paso
Planificacin de la Cadera en el Eje "Z"- PLANO SAGITAL
0.01

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)

Figura 3.5.4 Planificacin de la cadera en eje Z Sagital.

62
UNIVERSIDAD NACIONAL DE TRUJILLO
En el eje X. planificacin de la Cadera. Se divide en 2 partes:
Se toma como Referencia HSL = SL/2.
i) x0 = hBiped;
x1 = hBiped+hcadera; Donde 0< t Tp/2;
v0=0;
v1=0;
Ti =Tp/2;
ii)
x0= hBiped+hcadera;
x1= hBiped;
v0=0; Donde Tp/2 < t Tp
v1=0;
Ti = Tp/2;

Reemplazando en la ecuacin 3.5.9 se tiene:


2 3
Xc(t) = + 12 2
16 3
siendo: 0< t Tp/2 (3.5.12)

2 3
Xc(t) = + 12 + 16 siendo: Tp/2 < t Tp (3.5.13)
2 3

Se muestra en la figura 3.5.5 el resultado, de a ecuaciones 3.5.12 y 3.5.13 los


valores numricos de las variables en 3.5.11

Planificacin de la Cadera en el Eje "X" -PLANO SAGITAL


0.243

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)

Figura 3.5.5 Planificacin de la cadera en eje X Sagital.

63
UNIVERSIDAD NACIONAL DE TRUJILLO
3.5.4.2. Plano frontal:
Se tiene en cuenta algunos parmetros, que se muestra a continuacin en la
siguiente figura 3.5.6

Figura 3.5.6- Planeacin de caminata en el plano frontal eje Y.


Dnde:
LH : Longitud de Centro de la cadera al pie, en posicin inicial.
DF : Distancia entre los pies.
EC : Exceso de la cadera, sobre el eje Y.

Se planifica la cadera en el eje Y


y0 = -LH;
y1 = DF;
v0 = Siendo: 0 < t /2
v1=0
T1=Tp/2
Remplazando en la ecuacin 3.5.9 se tiene:

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
UNIVERSIDAD NACIONAL DE TRUJILLO
y para :
y0= DF;
y1= -LH;
v0=0; Siendo: Tp/2 < t
v1=
T1=Tp/2;

Se reemplaza en la ecuacin 3.5.9 Se tiene:

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)

Para el clculo del robot se toman los siguientes datos:

LH=0.0452; Longitud del centro a unos de los extremos de la cadera.


EC=-0.035; Sobre paso del punto central de la cadera sobre el eje "Y".
DF=0.080; Distancia entre los pies.
vyc=0; Velocidad de la cadera al Inicio y final del Intervalo.

Adems se debe tener en cuenta el cambio de pierna ya que en ese caso se debe
multiplicar el y0, y1, v0, v1 por (-1).

Los resultados se muestran en la grfica 3.5.7

Planificacin de la Cadera en el Eje "Y" - Plano Frontal


-0.034

-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)

Figura 3.5.7 trayectoria de la cadera en el eje Y plano frontal.

65
UNIVERSIDAD NACIONAL DE TRUJILLO
3.5.5.- Planificacin del Pierna Flotante.
Para planificar la caminata en la pierna flotante se debe tener en cuenta la etapa
de doble Soporte, es decir el perodo de caminata (Tp) est formado por:
Tp = + (3.5.16)

El perodo de doble soporte aprox. vara entre el 20% a 30% del tiempo de paso.

Figura 3.5.8. Movimiento del robot en doble soporte.


Planificacin en el Eje Z (Plano Sagital).
Tomando como referencia la figura 3.5.3 se tiene los siguientes datos:
z0 = -SL;
z1= SL; Siendo: 0 < t
v0 = 0;
v1= 0;
T1 =Tss;

Reemplazando en la ecuacin 3.5.9 se tiene la siguiente ecuacin en el eje Z.


Zpf(t) = + 6 2 2 4 3 3 (3.5.17)

Para la simulacin se toman los mismos datos para la cadera, las medidas estn en
metros
SL=0.020; Lp = Longitud de paso.
hBiped=0.23678; h = altura de la cadera
hpflotante=0.010; hp = elevacin de la pierna flotante.
hcadera=0.005; hc = elevacin de la cadera
Tp= 1 Tp = tiempo empleado para cado paso
TDobleSoporte = 30% Tp

66
UNIVERSIDAD NACIONAL DE TRUJILLO
Y se Obtiene la Siguiente figura:
PLANIFICACIN DE LA PIERNA FLOTANTE EN EL EJE "Z" - PLANO SAGITAL
0.02

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)

Figura 3.5.9 Planificacin de la pierna flotante en el eje Z, plano sagital

En el Eje X, Plano Sagital:


x0 = 0;
x1 = hpflotante;
v0 = 0; Siendo: 0< t /2
v1 = 0;
T1 = Tss/2;

x0=hpflotante;
x1=0;
v0=0; Siendo: Tp/2 < t Tp
v1=0;
T1 = Tss/2;

Reemplazando en la ecuacin 3.5.9 se tiene:

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)

Remplazando las variables se tiene la Siguiente Grfica:

67
UNIVERSIDAD NACIONAL DE TRUJILLO
PLANIFICACIN DE LA PIERNA FLOTANTE EN EL EJE "X" - PLANO SAGITAL
0.01

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.


Planificacin 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;

Se reemplaza en la ecuacin 5.6 Se tiene:


ypf1_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.20)

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, adems se debe
tener en cuenta el cambio de pierna ya que en ese caso se debe multiplicar el y0, y1, v0,
v1 por (-1).

Los resultados se Muestran en la figura 3.5.11

68
UNIVERSIDAD NACIONAL DE TRUJILLO
Planificacin de la Pierna Flotante en el "Y" -PLANO FRONTAL
-0.08

-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.

3.5.6 Inicio y parada del robot bpedo 10GDL.


La planificacin de caminata del robot bpedo10 GDL en inicio de caminata, como
fin de caminata se muestra en la figura 3.5.12

Figura 3.5.12 Inicio y parada del robot - Plano sagital

69
UNIVERSIDAD NACIONAL DE TRUJILLO
En la figura 3.5.12 se puede notar 2 estados: El estado inicial, y el estado final
adems se muestra las trayectorias por la cadera y la pierna flotante.
La planificacin de caminata vara solo en el eje Z, pues las trayectorias descritas
para la cadera y pierna flotante en los ejes X, Y son aplicables para los pasos de inicio
y fin.
3.5.6.1 Paso de Inicio
a.1) Cadera:
z0=0
z1=HSL o < t
v0=0
v1=0
T1=Tp
Se toma HSL = SL/2.
Se evala en la ecuacin 5.8 se tiene:
3
Zcstart(t) = 3 2 (3.5.22)
2 2 3

Se evala con los valores del robot, y se tienen el siguiente resultado.


Planificacin Cadera en el EJE "Z" - INICIO DE MARCHA
0.01

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)

Figura 3.5.13 Trayectoria de inicio de caminata de la cadera, eje Z.


a.2) Pierna Flotante:
z0=0;
z1=SL
v0=0 0 < t
v1=0
T1=Tp

Se evala en la ecuacin 3.5.9 se tiene:

70
UNIVERSIDAD NACIONAL DE TRUJILLO

2
Zpfstart(t) = 3 2
2 3 3 (3.5.23)

Los resultados se muestran en la figura 3.5.14

Planificacin Pie flotante en el EJE "Z"-INICIO DE MARCHA


0.02

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)

Figura 3.5.14 Trayectoria de inicio de caminata de pie flotante eje Z.

3.5.6.2 .- Paso de Fin:


b.1) Cadera:
z0 = -HSL
z1=0
v0=0 Siendo: 0 < t
v1=0
T1=Tp
Siendo HSL = SL/2
Reemplazando en la ecuacin 3.5.9 Se tiene:

() = 2
+ (3 22 ) 2 3 3 (3.5.24)

Los resultados Se muestran en la figura 3.5.15

71
UNIVERSIDAD NACIONAL DE TRUJILLO
Planificacin de la Cadera en el EJE "Z" - FIN DE MARCHA
0

-0.001

-0.002

-0.003

Z cadera (metros) -0.004

-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)

Figura 3.5.15 Trayectoria de parada de la cadera en eje Z


b.2) Pierna Flotante:
Z0 = -SL
Z1 = 0
V0 = 0 Siendo: 0 < t
V1 = 0
T1 = Tp.
Reemplazando en la ecuacin 3.5.9 Se tiene:
2
() = + (3 2 ) 2 3 3 (3.5.25)

Se reemplaza las variables con los datos del robot, y se tienen la siguiente grfica 3.5.16
Planificacin 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
UNIVERSIDAD NACIONAL DE TRUJILLO
Se mostrar grficos (figura 3.5.17 al 3.5.22) que compara la Trayectorias
propuestas en el Eje X, Y, Z, con las que resultan de la cinemtica inversa y directa,
reales la cual se movera el Robot.
Cadera en el EJE eje "X"
0.243
Resultados de Cinemtica Inversa e Directa.
Trayectoria Planificada "X"
0.242

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
Nmero de Divisiones de Tiempo (s)

Figura 3.5.17 Comparacin delas trayectorias generadas (propuestas) con las que resulta
de la cinemtica inversa y directa de la cadera en el eje X

Cadera en el eje "Y"


-0.034

-0.036

-0.038
"Y" cadera

-0.04

-0.042

-0.044
Resultados de Cinemtica 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
Nmero de Divisiones de Tiempo (s)

Figura 3.5.18 Comparacin de las trayectorias generadas (propuestas) con las que resulta
de la cinemtica inversa y directa de la cadera en el eje Y

73
UNIVERSIDAD NACIONAL DE TRUJILLO

Cadera en el eje "Z"


0.01

0.005

0
"Z" cadera

-0.005

-0.01 Resultados de Cinemtica Inversa e Directa.


Trayectoria Planificada "Z"

-0.015
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Nmero de Divisiones de Tiempo (s)

Figura 3.5.19 Comparacin de las trayectorias generadas (propuestas) con las que resulta
de la cinemtica 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 Cinemtica 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
Nmero de Divisiones de Tiempo(s)

Figura 3.5.20 Comparacin de las trayectorias generadas (propuestas) con las que resulta
de la cinemtica inversa y directa de la pierna flotante en el eje X

74
UNIVERSIDAD NACIONAL DE TRUJILLO

Pierna Flotante en el eje "Y"


-0.078
Resultados de Cinemtica Inversa e Directa.
Trayectoria Planificada "Y"
-0.08

-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
Nmero de Divisiones de Tiempo (s)

Figura 3.5.21 Comparacin del as trayectorias generadas (propuestas) con las que resulta
de la cinemtica inversa y directa de la pierna flotante en el eje Y

Pierna Flotante en el eje "Z"


0.02

0.015

0.01
"Z" Pierna Flotante

0.005

-0.005

-0.01

-0.015 Resultados de Cinemtica Inversa e Directa.


Trayectoria Planificada "Z"
-0.02
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Nmero de Divisiones de Tiempo (s)

Figura 3.5.22 Comparacin de las trayectorias generadas (propuestas) con las que resulta
de la cinemtica inversa y directa de la pierna flotante en el eje Z

75
UNIVERSIDAD NACIONAL DE TRUJILLO
3.6 Criterio de estabilidad, ZMP:
En base a la teora expuesta en el captulo 1.3.4, en esta tesis, para calcular el
ZMP es a partir del modelo simplificado del robot bpedo, es decir el modelo de Pndulo
Invertido 3D ya que el clculo se realiza partir de las coordenadas, velocidad y
aceleraciones del COG y el momento angular del robot bpedo.

Figura 3.6.1 Modelo del Pndulo Invertido 3D.


(Fuente: [8] Pg. 31)
- Se presenta el Siguiente grfico para el Anlisis de del clculo de ZMP con el
modelo de Pndulo Invertido.

Figura 3.6.2 Modelo Simplificado del Robot Bpedo (PENDULO INVERTIDO 3D)
(Fuente: elaboracin propia)
76
UNIVERSIDAD NACIONAL DE TRUJILLO

: .
:

: .
Se tiene la ecuacin de Equilibrio rotacional.
+ = (3.6.1)

= [ , , ] (3.6.2)

= [0 0 ] (3.6.3)

= (3.6.4)

= [, , ] (3.6.5)
Dnde:
= .
m = masa del robot ubicado en el centro de masa.
g = Aceleracin de la gravedad.
Dnde el Momento angular de una masa puntual (Centro de masa), se puede
expresar:

= ( ) (3.6.6)

Siendo:
=
Desarrollando la ecuacin 3.6.6 se tiene lo siguiente:



= ( ) + (
) (3.6.7)

= (3.6.8)



= (3.6.9)


= (3.6.10)

El primer trmino de la ecuacin 3.6.7 es igual a cero. Por ser vectores paralelos.



= (3.6.11)

= [ , , ] (3.6.12)

77
UNIVERSIDAD NACIONAL DE TRUJILLO
Reemplazando las ecuaciones 3.6.12, 3.6.3, 3.6.5, 3.6.6 en las ecuacin 3.6.1
= ( ) (3.6.13)

+ = ( ) (3.6.14)

El ZMP puede ser usado para representar a suma de torques, resultado de las
fuerzas de reaccin del suelo como sigue.
= [ 0 0] (3.6.15)

= [ 0 0] (3.6.16)

Siendo:
= posicin del ZMP. (3.6.17)

= "x" (3.6.18)

Reemplazando las ecuaciones 3.6.2, 3.6.5, y 3.6.16 en 3.6.15


Se tienen:
= (3.6.19)

= (3.6.20)

Reemplazando 3.6.13 y 3.6.14 en 3.6.19 y 3.6.20 respectivamente se tiene:



= +
(3.6.21)


= +
(3.6.22)

3.6.1.- ZMP, Robot Bpedo de 10GDL:

El COG se ubica en la posicin de la cadera y a la vez permita en el momento de


implementarlo fsicamente acoplar un acelermetro en donde se encuentra la cadera del
Robot BIPEDO 10GDL.

78
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.6.3. Un punto ri que se refiere a la cadera del robot bpedo 10GDL
Respecto al Pie de Apoyo

Tomando la figura 3.6.3, se calcula el Velocidad y Aceleracin del COG, del Robot
Bpedo.

Sea:


= ( ) (3.6.23)

1
Sea 0 el mismo punto con respecto al sistema de coordenadas de la base.
1
La matriz de transformacin homognea de coordenadas homogneas que relacin
el desplazamiento espacial.

Por lo que 0 queda definido como:

79
UNIVERSIDAD NACIONAL DE TRUJILLO
0
= 0 (3.6.24)

Como est en reposo en la cadera y suponiendo el movimiento del cuerpo rgido,


otros puntos asi como el punto fijado en la cadera y se expresa con respecto al
sistema iesimo tendr velocidad nula con respecto a dicha sistema de coordenadas. La
velocidad de se expresa en sistemas de coordenadas de la base como:

0 ( 0 ) ( 0 )
= =
=
(3.6.25)

( 0 ) 0

= =1
(3.6.26)

Se cumple por que =0

La derivada parcial de 0 con respecto se puede calcular fcilmente 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)

05
0
5 = (5=1
) (3.6.29)

Velocidad de la cadera respecto al pie de apoyo.

v5C=(T54d*Qi*T43d*T32d*T21d*T10d*T00d*T0Cd*dq5d +... (3.6.30)


T54d*T43d*Qi*T32d*T21d*T10d*T00d*T0Cd*dq4d +...
T54d*T43d*T32d*Qi*T21d*T10d*T00d*T0Cd*dq3d +...
T54d*T43d*T32d*T21d*Qi*T10d*T00d*T0Cd*dq2d +...
T54d*T43d*T32d*T21d*T10d*Qi*T00d*T0Cd*dq1d)*iri;

Ahora bien para obtener la aceleracin lineal del COG se procede a derivar con respecto
al tiempo la ecuacin 3.6.30
Obteniendo:
( ) 2 0
0
=
= (=1 2
) (3.6.31)

Para aplicar la ecuacin 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 ecuacin de aceleraciones para
el robot bpedo queda de la siguiente manera.

80
UNIVERSIDAD NACIONAL DE TRUJILLO
2 05
0
5 = (5=1 2
) (3.6.32)

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 aceleracin 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 aceleracin 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 Bpedo, adems

Longitudes en metros.
L7=0.0174;
L6=0.0468;
L5=0.0473;
L4=0.064;
L3=0.0473;
L2=0.0288;
L1=0.0452;

Los Parmetros de Denavit Hartenberg:



Theta =[qd(1); qd(2)-pi/6; qd(3)+pi/3; qd(4)-pi/6; qd(5)];
d =[0 ; 0; 0; 0; 0];
a =[L6; L5; L4; L3; 0];
alfa =[-pi/2; 0; 0; pi/2; 0];

Adems se Incluyen las velocidades y aceleraciones articulares ya calculadas, y


se obtienen los siguientes resultados en la figura 3.6.4 y 3.6.5.

81
UNIVERSIDAD NACIONAL DE TRUJILLO

Velocidades del COG - Robot Bpedo


0.04

0.03

Velocidad en m/s 0.02

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

Figura 3.6.4 Velocidad COG del Robot Bpedo.


(Fuente: Elaboracin propia)

Aceleracion del COG - Robot Bpedo


0.05
Aceleracin en el Eje X
0.04 Aceleracin en el Eje Y
Aceleracin en el Eje Z
0.03

0.02
Aceleracin 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

Figura 3.6.5 Aceleracin COG del Robot Bpedo.


(Fuente: Elaboracin propia)

82
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.6.6 Clculo del ZMP en Plena Marcha.


(Fuente: Elaboracin propia)

Del grfico 3.6.6 se observa que est dentro del polgono 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 tericos, ya que los ms conveniente
sera medir las velocidades y aceleraciones y reemplazar en la ecuacin 3.6.21 y 3.6.22
para obtener los puntos de ZMP reales.

83
UNIVERSIDAD NACIONAL DE TRUJILLO
3.7 Simulacin de caminata del robot bpedo 10GDL.
La simulacin se realizar en la plataforma de Simulink (Matlab), utilizando las
libreras de SimMechanics. La Simulacin sustituye en cierta forma el anlisis fsico-
Matemtico, la cual se debe tener muy claros los conceptos y principios fundamentales
fsicos.
SimMechanics, es una librera que permite crear diagramas de bloques que
representan un determinado sistema mecnico.

Figura 3.7.1. Diagrama de bloque simple en SimMechanics.


En SimMechanics se puede encontrar varios bloques, entre los ms principales
tenemos:
a) Bodies: Componente sistemas de cuerpos rgidos., entre ellos esta Ground
(tierra), representa un cuerpo de masa infinita en el espacio.

Figura 3.7.2 Boque de Cuerpo.

b) Joints: Las Articulaciones en SimMechanics representan los grados de libertad


que puede tener con respecto a otro.

Figura 7.3 Joints en SimMechanics.

c) Sensors y actuators: Los Sensores permiten medir el movimiento de cuerpo o


articulacin. Los actuadores permiten suministrar fuerzas, torques o movimiento a
los cuerpos articulados.

84
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.7.4. Joints en SimMechanics.

3.7.1 Simulacin del robot bpedo 10GDL.


Para Simular el Robot Bpedo 10GDL, se tiene se ingresa a la plataforma de
Simulink y a la Librera de SimMechanics, se inicia con la colocacin de la tierra (ground),
como se muestra en la figura 3.7.5

Figura 3.7.5 Tierra, bloque grados de Libertad, cadera.


a.- Se configura el bloque tierra:

Figura 3.7.6 Configuracin del Bloque tierra.

b.- Se configura e bloque grados de libertad del robot, permite desplazarse al robot en
movimiento prismticos, respecto a la tierra fijada, en x,y,z.

85
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.7.7 Configuracin del bloque grados de libertad del robot.


c.- Se configura el bloque de la cadera del robot.

Figura 3.7.8 Configuracin del bloque cadera del robot bpedo.

Siguiendo la representacin propuesta en la figura 3.4.2, sigue por colocar los bloques
que representan a la pierna del robot, se tiene:

Figura 3.7.9 Configuracin del bloque del eslabn L2.

En la figura 3.7.9 se observa el bloque Weld, que realiza la funcin de unir (soldar) 2
cuerpos (eslabones), la configuracin de L2d se muestra en la figura 3.7.10

Figura 3.7.10 Configuracin del bloque del eslabn L2.

86
UNIVERSIDAD NACIONAL DE TRUJILLO

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

Figura 3.7.11 Configuracin del bloque control 1d.


Dentro del bloque Control 1d se encuentran 2 sub bloques denominados servo unidos a
la articulacin de rotacin:

Figura 3.7.12, Sub Bloques de control 1d.


La funcin Servo, en la parte interna se tiene un bloque sensor que lee el
movimiento de la articulacin en radianes, este va a compararse con un SetPoint (que es
la entrada de movimiento generado por la planificacin de trayectorias), luego va a un
controlador PID, y sale a un actuador, la cual esta se une directamente con la articulacin.

Figura 3.7.13, Sub bloque Servo, para controlar la articulacin.

87
UNIVERSIDAD NACIONAL DE TRUJILLO
El bloque control 1d explicado anteriormente se repite el esquema para las 10
articulacin del robot bpedo unidos a cada eslabn respectivamente.
Simulink por defecto no trae opciones para simular el piso, la cual va a caminar el
Robot bpedo, la cual se tuvo que Simular el piso ya que si no se fija ninguna parte del robot,
el robot caer y caer por accin de la gravedad. Eliminar la gravedad no es una opcin,
ya que desea disear los controladores de su robot con compensacin por gravedad, peor
an, su robot flotar si simula un entorno de gravedad cero.
Por eso lo que se trata de hacer es: El suelo es un cuerpo rgido que ejerce suficiente
fuerza sobre los cuerpos adyacentes para evitar que caigan. Ahora, los cuerpos rgidos
perfectos no existen en el mundo real, todos los cuerpos se deforman cuando estn
estresados o comprimidos. Esto significa que nuestro modelo de piso debe ser elstico,
esto puede ser modelado como un resorte y un elemento de disipacin

Esa descripcin se asemeja mucho a un sistema masa-resorte-amortiguador.


Como usted puede saber que los sistemas tienden a vibrar, a menos que estn sobre
amortizados. Esto significa que nuestro modelo de piso debe comportarse como un sistema
amortiguador-amortiguador de masa sobre amortiguado. La condicin final es que el objeto
de cada libre debe ser la masa del sistema, esto significa que nuestro modelo de piso no
tiene realmente un parmetro de masa. Otra manera de pensar es que la masa es
desmontable, y esta masa slo es afectada por el resorte y el amortiguador cuando est en
o por debajo del nivel del suelo. (Fuente [14])

Aplicando los conceptos anteriormente se simulara el nivel del piso, como se


muestra la figura 3.7.14

3.7.14. Bloque que simulara el Nivel del Piso en SimMechanics.


En el bloque se observa que tiene entradas Sensor, actuador y Nivel del piso (que
se coloca -0.24) para tener una margen entre el nivel 0 del piso y pueda efectuar el control
al modelos resorte pistn.

88
UNIVERSIDAD NACIONAL DE TRUJILLO

3.7.15. Sub bloque de nivel de piso.


Las figuras 3.7.14 y 3.7.15 se colocan en los pies, y esto ayudara a representar un
piso en la simulacin del robot bpedo.

En resumen uniendo todo lo anteriormente descrito respecto a la simulacin del


robot bpedo de 10GDL se presente la figura 3.7.16

89
UNIVERSIDAD NACIONAL DE TRUJILLO

3.7.16. Representacin general del esquema de simulacin del robot bpedo de 10 GDL
en SimMechanics

90
UNIVERSIDAD NACIONAL DE TRUJILLO

En la figura 3.7.17 representa el esquema de simulacin caminata del Robot


bpedo10 GDL en la plataforma de SimMechanics Simulink.

Figura 3.7.17 Simulacin del Robot bpedo de 10GDL en la Plataforma de SimMehanics


Simulink.

3.7.2 Resultados de la simulacin, movimiento en cada articulacin:


3.7.2.1.- Movimiento simulado de la pierna de apoyo.

91
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.7.18 Resultados de la simulacin, de la articulacin q5d


(Morado: SetPoint, Amarillo: Movimiento Simulado)

Figura 3.7.19 Resultados de la simulacin, de la articulacin q4d


(Morado: SetPoint, Amarillo: movimiento simulado)

92
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.7.20 Resultados de la simulacin, de la articulacin q3d


(Morado: SetPoint, Amarillo: Movimiento simulado)

Figura 3.7.21 Resultados de la simulacin, de la articulacin q2d


(Morado: SetPoint, Amarillo: Movimiento simulado)

93
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.7.22 Resultados de la simulacin, de la articulacin q1d


(Morado: SetPoint, Amarillo: Movimiento simulado)

3.7.2.2.- Movimiento simulado de la pierna flotante.

Figura 3.7.23 Resultados de la simulacin, de la articulacin q1i


(Morado: SetPoint; Amarillo: Movimiento simulado)

94
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.7.24 Resultados de la simulacin, de la articulacin q2i


(Morado: SetPoint; Amarillo: Movimiento simulado)

Figura 3.7.25 Resultados de la simulacin, de la articulacin q3i


(Morado: SetPoint; Amarillo: Movimiento simulado)

95
UNIVERSIDAD NACIONAL DE TRUJILLO

Figura 3.7.26 Resultados de la simulacin, de la articulacin q4i


(Morado: SetPoint; Amarillo: Movimiento simulado)

Figura 3.7.27 Resultados de la simulacin, de la articulacin q5i


(Morado: SetPoint; Amarillo: Movimiento simulado)

96
UNIVERSIDAD NACIONAL DE TRUJILLO
CAPITULO IV: DISCUSIN DE RESULTADOS.
4.1.- Cinemtica directa: El desarrollo de la cinemtica directa se hizo mediante un mtodo
sistemtico, el de Denavit-Hartenberg, ya que tiene la virtud de posicionar siempre el eje de
rotacin de cada articulacin en el eje Z de cada uno de los marcos de referencias. Los
resultados se pueden observar en la figura 3.4 y 3.5, y estas ecuaciones son fundamentales
para el clculo de la cinemtica inversa.
4.2.- Cinemtica inversa: Para calcular la cinemtica inversa se utiliza un mtodo
numrico, este se basa en un paper publicado por la Universidad de California,
departamento de Matemtica, Introduccin to Inverse Kinematic with Jacobian Transpose,
anda Damped Least Square methods, esto nos brinda el fundamento matemtico para
desarrollar el algoritmo. La comprobacin de los resultados dados por la cinemtica inversa
se hace fcilmente con la cinemtica directa, como se muestra la figura 3.6. Adems se
muestran las grficas, los valores de las posiciones articulares de cada eslabn tanto de la
pierna de apoyo, como la pierna flotante en un paso de caminata bpeda.
4.3 Dinmica del robot bpedo.- Los torques obtenidos al aplicar la dinmica inversa por
Newton Euler se muestra en la figura 3.3.7 se observa que los torques producidos no
superan los (0.25Nm) , como tambin se observa que hay torques negativos, esto es debido
es que tienen direccin contraria a los asumidos inicialmente, adems estos resultados son
lgicos debido a las dimensiones que tienen el robot bpedo y adems tiene las condiciones
para que funcione con servomotores (mg996r=1.01Nm) comerciales.
Los resultados de las velocidades articulares como las de aceleraciones se calcula
en funcin a que tiempo de duracin de paso de marcha igual a 1s. Y se muestran en las
grficas 3.3.4 y 3.3.4 de la pierna flotante y 3.3.8 y 3.3.9 del pie de apoyo, se tiene que el
valor mximo de la velocidad articular no supera los 2 rad/s y variando en funcin a la
marcha (+ y -) caso similar sucede con la aceleracin.
4.4 Diseo del robot bpedo: En base a las cinemtica y dinmica calculada, a las
dimensiones propuestas, y teniendo en cuenta que la forma de las piezas deben soportar
las dimensiones de los servomotores, se dise el robot bpedo con esas caractersticas
como se muestra en la figura 3.4.1.
4.5 Planificacin de trayectorias: Los resultados obtenidos para la planificaciones de las
trayectorias a seguir tanto de la cadera, como del pie flotante respecto al pie de apoyo son
ptimos y se toma con base la spline cbica (debido a la existencia de sus derivadas,
velocidad y aceleracin) y estos a la vez se comprueban con los clculos obtenidos por la
cinemtica inversa y directa como se muestran en las figuras 3.517 al 3.5.22 con errores
de precisin en el orden de las 100 um. Estos resultados se comparan con los obtenidos en
la investigacin (fuente [7]); las cuales presentan trayectorias semejantes, esto es debido a
que tambin utilizan la spline cubic, pero en momento de calcular la cinemtica inversa,
hacen un cambio de referencia vectorial para su planificacin.
4.6. Criterio de estabilidad de caminata ZMP: Se analiza la estabilidad de la caminata
del robot Bpedo en plena marcha, lo cual fue necesario calcular la velocidad y
aceleraciones del COG el resultado muestra que se encuentra dentro del polgono de
soporte, pero tener en cuenta que estos resultados se basan en los parmetros elegidos en
trayectoria, tiempo de paso, etc.

97
UNIVERSIDAD NACIONAL DE TRUJILLO
CAPTULO V: CONCLUSIONES Y RECOMENDACIONES.
5.1 Conclusiones:
5.1.1. Cumpliendo con los objetivos propuestos, se obtuvo un modelo para el clculo
de la cinemtica directa del robot bpedo 10GDL aplicando el criterio de Denavith
Hartenberg. Se logr obtener las ecuaciones que relaciona el desarrollo de la cadera
referencia al pie de apoyo, del pie flotante referenciado a la cadera y del pie flotante
respecto al pie de apoyo, estas ecuaciones nos sirven para el clculo de la cinemtica
inversa (CI).
5.1.2 Para la cinemtica inversa se propone desarrollar mediante un mtodo
numrico, linealizacin de funciones, aplicando el criterio de Damped Least Squares, los
resultados son ptimos en el desarrollo (en cuestin de precisin), pero siempre teniendo
cuidado en la convergencia, adems de la inicializacin de los ngulos como entrada del
algoritmo, para obtener los resultados deseados. El gran potencial que he encontrado en
este mtodo de clculo en la cinemtica inversa, es que puedo aplicarlos para otros tipos
de robot (ya sea bpedos, brazos robots, etc), dando una alternativa de solucin ptima,
que tengan n grados de Libertad solo teniendo que adaptar los parmetros requeridos.
5.1.3 En el clculo de la dinmica se opt por elegir el mtodo de Newton Euler
debido a que su respuesta es mucho ms rpida computacionalmente, esto se debe a su
naturaleza vectorial y recursiva respecto a otros mtodos (lagrange Euler), se obtuvo los
torques generados en cada articulacin, utilizando las entradas de los ngulos generados
por la cinemtica inversa, la velocidad y aceleraciones articulares.
5.1.3 Teniendo la cinemtica directa, la cinemtica inversa, y la dinmica se dise
el modelo del bpedo10GDL utilizando el software de SolidWorks teniendo en cuenta las
dimensiones propuestas de los eslabones, y la forma de las piezas para que se pueda
implementar en lo fsico (teniendo en cuenta las dimensiones de los Servomotores).
5.1.4 Para la generacin de trayectorias, se utiliz la Spline Cubic, como funcin
debido a que existan sus derivadas (velocidad y aceleracin), adems se propuso
trayectorias en 2 planos, el plano frontal y plano sagital del robot bpedo, distribuyendo el
anlisis de movimiento en la cadera respecto al pie de apoyo, y el pie flotante respecto al
pie de apoyo, los resultados que se obtuvieron fueron muy buenos ya que se presentan
unas grficas dnde se compara las trayectorias propuestas (tericas) y las trayectorias
generadas (por la cinemtica inversa y directa) cuyo error est en 100 micras aprox;
adems con estas trayectorias se logra una caminata muy parecida al ser humano en
superficie plana.
5.1.5 Se incluy el clculo del ZMP como indicador de estabilidad del robot bpedo,
se modelo mediante un sistema de pndulo invertido en 3D, se encontr las velocidad y
aceleraciones de COG de robot bpedo, y las coordenadas la cual se mueve el ZMP para
un paso de caminata, la cual fue til para saber si la trayectoria propuesta genera una
caminata estable; en el anlisis realizado se encuentra dentro del polgono del soporte es
decir presenta una marcha estable.
5.1.6 Se realiz la simulacin de la caminata del robot bpedo utilizando la plataforma
de Simulink con las libreras de SimMechanics, tener en cuenta que para eso se cre
bloques de control para cada articulacin llamados Control i, y tambin se gener un
98
UNIVERSIDAD NACIONAL DE TRUJILLO
bloque que me permita modelar la superficie del piso del robot bpedo, como entrada se
tena las trayectorias generadas en X, Y, Z. los resultados obtenidos fueron ptimos para
el objetivo requerido.

5.2.- Recomendaciones:
5.2.1 Con los resultados obtenidos se espera que se lleve a la implementacin fsica
del Robot de 10GDL con las caractersticas propuestas.
5.2.2 Se dise un controlador que pueda modificar la posicin del ZMP, cuando
salga del polgono 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 Dinmica del Robot bpedo mediante otro mtodo, por ejemplo de
Lagrange-Euler y poder comparar los resultados obtenidos.
5.2.4 Realizar el anlisis estructural del robot bpedo presentado, para determinar
las dimensiones, esfuerzos y deformaciones de cada pieza (soportes de servomotor,
cadera), as como el material ptimo para su implementacin, teniendo en cuenta las
ergonoma en la estructura.
5.2.5 Hacer uso de controladores utilizando la Inteligencia Artificial, como
controlares difusos, redes neuronales, algoritmos genticos, etc.
5.2.6 Los alcances que pueda tener este trabajo de investigacin depende de la
aplicacin que pueda tener y lo estudios investigativos que se pueda lograr, inclusin de
sensores, caminata en superficies inclinadas, visin artificial, la realizacin de exosqueletos,
aplicados a la medicina, la industria, el campo militar, espacial, etc.

99
UNIVERSIDAD NACIONAL DE TRUJILLO

CAPITULO VI: REFERENCIAS BIBLIOGRAFICAS.


6.1-Bibliografa:
[1] BARRIENTOS Antonio, Fundamentos de Robtica, Mcgraw W hill/interamericana
de Espaa 1997, ISBN: 84-481-0815-9

[2] FU, GONZALEZ, LEE; ROBOTICA Control, deteccin, Vision e Inteligencia.


Sebastian Dormido Bencomo (tradu), Mcgraw W hill/interamericana de Espaa 1988,
ISBN: 0-07-022625-3

[3] SAMUEL R. BUSS, Introduction to Inverse Kinematics with Jacobian Transpose,


Pseudoinverse and Damped Least Squares methods, Departament of Mathematics,
University of California, San DiegoLa Jolla, CA 92093-0112, October 7, 2009.

[4] GUSMAN VALDIVIA, Csar Humberto, Construccin de un Robot Bpedo Basado


en el caminado Dinmico Director, Dr. Enrique Quintero Mrmol Mrquez, Tesis de
maestra en Ciencias, en el Centro Nacional de Investigacin y Desarrollo Tecnolgico,
Departamento de Ingeniera Mecatrnica, 2010

[5] BAEZ Gustavo, OLGUIN Ernesto, Control Cinemtico en el espacio operacional de


un Robot Bpedo en fase de Doble Soporte, Grupo de Robotica y Manufactura Avanzada,
CINVESTAV Saltillo, Coahuila 25903, Mxico.

[6] ShuujiKajita, FumioKanehiro, KenjiKaneko, KiyoshiFujiwara, KensukeHarada,


KazuhitoYokoi and HirohisaHirukawa. Biped Walking Generation by using Preview Control
of Zero-Moment Point, International Conference on Robotics & Automation, Taipei, Taiwan,
2003.

[7] Jos Larriva, Edynson Guillermo, Jos Trelles, Oscar Vele, Diseo y Construccin
de un robot bpedo experimental. Universidad Politcnica Salesiana, escuela de ingeniera
electrnica.

[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, Mtodos Nmericos para Ingenieros, McGraw Hill, Tercera
Edicin, 2001.

[10] JOHN J. CRAIG, Rbotica, Tercera Edicin. Pearson Eduacin, Mxico, 2006.
ISBN: 970-26-00772-8.

100
UNIVERSIDAD NACIONAL DE TRUJILLO
[11]R. Avila Chaurand, L.Prado Len, E.Gonzales Muos, Dimensiones
antropomtricas de poblacin latinoamericana. Universidad de Guadalajara, Mxico,
Segunda Edicin 2007.

[12] Ortsica. [Online] Junio 18, 2015.


http://www.oandplibrary.org/reference/ortesica/.

[13] Online. Enero 06, 2017.


http://embeddedprogrammer.blogspot.com/2013/01/simmechanics-simulating-
floor.html?utm_source=blog&utm_medium=gadget&utm_campaign=bp_featured

[14] Online, Enero 10, 2017.


http://embeddedprogrammer.blogspot.pe/2012/08/simulation-of-humanoid-robot.html

[15] Online, Enero 01, 2017


http://embeddedprogrammer.blogspot.com/2013/01/simmechanics-simulating-
multibody.html?utm_source=blog&utm_medium=gadget&utm_campaign=bp_featured

101
UNIVERSIDAD NACIONAL DE TRUJILLO

ANEXOS
A.- RESULTADOS DE ECUACIONES OBTENIDAS:
A.1 Resultado de cinemtica 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)

r21 = 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)
(a.2)

r31 = sin(q1d - q2d - q3d - q4d)/2 - sin(q1d + q2d + q3d + q4d)/2


(a.3)

r12 = sin(q2d + q3d + q4d + q5d)/2 + sin(q2d + q3d + q4d - q5d)/2


(a.4)

r22 = cos(q2d + q3d + q4d - q5d)/2 - cos(q2d + q3d + q4d + q5d)/2


(a.5)

r32= cos(q2d + q3d + q4d) (a.6)

r13 = 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) (a.7)

r23= 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) (a.8)

r33= os(q1d + q2d + q3d + q4d)/2 - cos(q1d - q2d - q3d - q4d)/2 (a.9)

r14 = 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)) (a.10)

r24=(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) +

102
UNIVERSIDAD NACIONAL DE TRUJILLO
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))
(a.11)

r34=(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
( a.12)

A.2 Resultado de cinemtica directa del pie flotante respecto a la cadera.


R11 = 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)
(a.13)

R21 = sin(q2i + q3i + q4i + q5i)/2 + sin(q2i + q3i + q4i - q5i)/2


(a.14)

R31= cos(q1i)*sin(q5i) + cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i) -


cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) -
cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i) (a.15)

R12= cos(q5i)*sin(q1i) + cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*sin(q5i) -


cos(q1i)*cos(q2i)*sin(q3i)*sin(q4i)*sin(q5i) -
cos(q1i)*cos(q3i)*sin(q2i)*sin(q4i)*sin(q5i) -
cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i)*sin(q5i) (a.16)

R22 = cos(q2i + q3i + q4i + q5i)/2 - cos(q2i + q3i + q4i - q5i)/2


(a.17)

R32 = 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.18)

R13 = sin(q1i + q2i + q3i + q4i)/2 - sin(q1i - q2i - q3i - q4i)/2


(a.19)

R23 = cos(q2i + q3i + q4i) (a.20)

R33 = cos(q1i + q2i + q3i + q4i)/2 - cos(q1i - q2i - q3i - q4i)/2


(a.21)

R14 = (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) +

103
UNIVERSIDAD NACIONAL DE TRUJILLO
L6*cos(q1i)*cos(q4i)*sin(q2i)*sin(q3i) -
(3^(1/2)*L5*cos(q1i)*cos(q2i)*cos(q3i))/2 +
(3^(1/2)*L5*cos(q1i)*sin(q2i)*sin(q3i))/2 -
L6*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i) -
L7*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) +
L7*cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) +
L7*cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) +
L7*cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin(q3i) (a.22)

R24= (L7*sin(q2i + q3i + q4i + q5i))/2 - (L5*cos(q2i + q3i))/2 +


(L4*cos(q2i))/2 + (L7*sin(q2i + q3i + q4i - q5i))/2 + L6*sin(q2i + q3i +
q4i) + (3^(1/2)*L5*sin(q2i + q3i))/2 + (3^(1/2)*L4*sin(q2i))/2 (a.23)

R34 = L1 + L3*sin(q1i) + L7*cos(q1i)*sin(q5i) - (L4*sin(q1i)*sin(q2i))/2


+ (L5*cos(q2i)*sin(q1i)*sin(q3i))/2 + (L5*cos(q3i)*sin(q1i)*sin(q2i))/2 +
(3^(1/2)*L4*cos(q2i)*sin(q1i))/2 + L6*cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)
- L6*cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i) -
L6*cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i) -
L6*cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i) +
(3^(1/2)*L5*cos(q2i)*cos(q3i)*sin(q1i))/2 -
(3^(1/2)*L5*sin(q1i)*sin(q2i)*sin(q3i))/2 -
L7*cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
L7*cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) -
L7*cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i) +
L7*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i) (a.24)

A.3 Resultados de la Cinemtica Directa del pie flotante respecto al pie de apoyo:

A11 = (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)
*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)) (a.25)

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
UNIVERSIDAD NACIONAL DE TRUJILLO
q5d)/2 - cos(q2d + q3d + q4d - q5d)/2)*(sin(q2i + q3i + q4i + q5i)/2 +
sin(q2i + q3i + q4i - q5i)/2) -
(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))*(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)) (a.26)

A31= cos(q2d + q3d + q4d)*(sin(q2i + q3i + q4i + q5i)/2 + sin(q2i + q3i +


q4i - q5i)/2) - (sin(q1d + q2d + q3d + q4d)/2 - sin(q1d - q2d - q3d -
q4d)/2)*(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(q1d + q2d + q3d +
q4d)/2 - cos(q1d - q2d - q3d -
q4d)/2)*(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)) (a.27)

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
UNIVERSIDAD NACIONAL DE TRUJILLO
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.29)

A32 = cos(q2d + q3d + q4d)*(cos(q2i + q3i + q4i + q5i)/2 - cos(q2i + q3i


+ q4i - q5i)/2) + (cos(q1d + q2d + q3d + q4d)/2 - cos(q1d - q2d - q3d -
q4d)/2)*(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)) + (sin(q1d + q2d + q3d +
q4d)/2 - sin(q1d - q2d - q3d -
q4d)/2)*(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)) (a.30)

A13 = cos(q2i + q3i + q4i)*(sin(q2d + q3d + q4d + q5d)/2 + sin(q2d + q3d


+ q4d - q5d)/2) - (sin(q1i + q2i + q3i + q4i)/2 - sin(q1i - q2i - q3i -
q4i)/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)) - (cos(q1i + q2i + q3i +
q4i)/2 - cos(q1i - q2i - q3i -
q4i)/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)) (a.31)

A23= - cos(q2i + q3i + q4i)*(cos(q2d + q3d + q4d + q5d)/2 - cos(q2d + q3d


+ q4d - q5d)/2) - (cos(q1i + q2i + q3i + q4i)/2 - cos(q1i - q2i - q3i -
q4i)/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)) - (sin(q1i + q2i + q3i +
q4i)/2 - sin(q1i - q2i - q3i -
q4i)/2)*(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)) (a.32)

A33= cos(q2d + q3d + q4d)*cos(q2i + q3i + q4i) - (sin(q1d + q2d + q3d +


q4d)/2 - sin(q1d - q2d - q3d - q4d)/2)*(sin(q1i + q2i + q3i + q4i)/2 -
sin(q1i - q2i - q3i - q4i)/2) + (cos(q1d + q2d + q3d + q4d)/2 - cos(q1d -
q2d - q3d - q4d)/2)*(cos(q1i + q2i + q3i + q4i)/2 - cos(q1i - q2i - q3i -
q4i)/2) (a.33)

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
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L5*cos(q1i)*cos(q2i)*cos(q3i))/2 +
(3^(1/2)*L5*cos(q1i)*sin(q2i)*sin(q3i))/2 -
L6*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i) -
L7*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) +
L7*cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) +
L7*cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) +
L7*cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin(q3i)) + (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 + (sin(q2d + q3d +
q4d + q5d)/2 + sin(q2d + q3d + q4d - q5d)/2)*((L7*sin(q2i + q3i + q4i +
q5i))/2 - (L5*cos(q2i + q3i))/2 + (L4*cos(q2i))/2 + (L7*sin(q2i + q3i +
q4i - q5i))/2 + L6*sin(q2i + q3i + q4i) + (3^(1/2)*L5*sin(q2i + q3i))/2 +
(3^(1/2)*L4*sin(q2i))/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)) -
(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))*(L1 + L3*sin(q1i) +
L7*cos(q1i)*sin(q5i) - (L4*sin(q1i)*sin(q2i))/2 +
(L5*cos(q2i)*sin(q1i)*sin(q3i))/2 + (L5*cos(q3i)*sin(q1i)*sin(q2i))/2 +
(3^(1/2)*L4*cos(q2i)*sin(q1i))/2 + L6*cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)
- L6*cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i) -
L6*cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i) -
L6*cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i) +
(3^(1/2)*L5*cos(q2i)*cos(q3i)*sin(q1i))/2 -
(3^(1/2)*L5*sin(q1i)*sin(q2i)*sin(q3i))/2 -
L7*cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
L7*cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) -
L7*cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i) +
L7*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i)) -
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)) (a.34)

A24 = (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 -
(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))*((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) -
(3^(1/2)*L5*cos(q1i)*cos(q2i)*cos(q3i))/2 +
(3^(1/2)*L5*cos(q1i)*sin(q2i)*sin(q3i))/2 -
L6*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i) -
L7*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) +
L7*cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) +
L7*cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) +
L7*cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin(q3i)) - (cos(q2d + q3d + q4d +
q5d)/2 - cos(q2d + q3d + q4d - q5d)/2)*((L7*sin(q2i + q3i + q4i + q5i))/2

107
UNIVERSIDAD NACIONAL DE TRUJILLO
- (L5*cos(q2i + q3i))/2 + (L4*cos(q2i))/2 + (L7*sin(q2i + q3i + q4i -
q5i))/2 + L6*sin(q2i + q3i + q4i) + (3^(1/2)*L5*sin(q2i + q3i))/2 +
(3^(1/2)*L4*sin(q2i))/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)) - (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))*(L1 + L3*sin(q1i) +
L7*cos(q1i)*sin(q5i) - (L4*sin(q1i)*sin(q2i))/2 +
(L5*cos(q2i)*sin(q1i)*sin(q3i))/2 + (L5*cos(q3i)*sin(q1i)*sin(q2i))/2 +
(3^(1/2)*L4*cos(q2i)*sin(q1i))/2 + L6*cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)
- L6*cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i) -
L6*cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i) -
L6*cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i) +
(3^(1/2)*L5*cos(q2i)*cos(q3i)*sin(q1i))/2 -
(3^(1/2)*L5*sin(q1i)*sin(q2i)*sin(q3i))/2 -
L7*cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
L7*cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) -
L7*cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i) +
L7*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i)) (a.35)

A34= cos(q2d + q3d + q4d)*((L7*sin(q2i + q3i + q4i + q5i))/2 -


(L5*cos(q2i + q3i))/2 + (L4*cos(q2i))/2 + (L7*sin(q2i + q3i + q4i -
q5i))/2 + L6*sin(q2i + q3i + q4i) + (3^(1/2)*L5*sin(q2i + q3i))/2 +
(3^(1/2)*L4*sin(q2i))/2) - (L4*cos(q3d + q4d))/2 - (sin(q1d + q2d + q3d +
q4d)/2 - sin(q1d - q2d - q3d - q4d)/2)*((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) -
(3^(1/2)*L5*cos(q1i)*cos(q2i)*cos(q3i))/2 +
(3^(1/2)*L5*cos(q1i)*sin(q2i)*sin(q3i))/2 -
L6*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i) -
L7*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) +
L7*cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) +
L7*cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) +
L7*cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin(q3i)) + (L5*cos(q4d))/2 -
(L2*(sin(q1d + q2d + q3d + q4d) - sin(q1d - q2d - q3d - q4d)))/2 -
L3*sin(q2d + q3d + q4d) + (cos(q1d + q2d + q3d + q4d)/2 - cos(q1d - q2d -
q3d - q4d)/2)*(L1 + L3*sin(q1i) + L7*cos(q1i)*sin(q5i) -
(L4*sin(q1i)*sin(q2i))/2 + (L5*cos(q2i)*sin(q1i)*sin(q3i))/2 +
(L5*cos(q3i)*sin(q1i)*sin(q2i))/2 + (3^(1/2)*L4*cos(q2i)*sin(q1i))/2 +
L6*cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i) -
L6*cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i) -
L6*cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i) -
L6*cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i) +
(3^(1/2)*L5*cos(q2i)*cos(q3i)*sin(q1i))/2 -
(3^(1/2)*L5*sin(q1i)*sin(q2i)*sin(q3i))/2 -

108
UNIVERSIDAD NACIONAL DE TRUJILLO
L7*cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
L7*cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) -
L7*cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i) +
L7*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i)) + 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
(a.36)

A.4 Resultados de la dinmica de la pierna Flotante.


Se muestran las ecuaciones obtenidas de los torques para cada articulacin.

1 = I1i*qdd1i + I2i*qdd1i + I3i*qdd1i + I4i*qdd1i + I5i*qdd1i +


Iext*qdd1i + b1*qd1i + I5i*qdd5i*cos(q2i + q3i + q4i) +
Iext*qdd5i*cos(q2i + q3i + q4i) + (L3^2*m1*qdd1i)/4 + L3^2*m2*qdd1i +
L3^2*m3*qdd1i + (L4^2*m2*qdd1i)/8 + L3^2*m4*qdd1i + (L4^2*m3*qdd1i)/2 +
L3^2*m5*qdd1i + (L4^2*m4*qdd1i)/2 + (L5^2*m3*qdd1i)/8 + (L4^2*m5*qdd1i)/2
+ (L5^2*m4*qdd1i)/2 + (L5^2*m5*qdd1i)/2 + (L6^2*m4*qdd1i)/8 +
(L6^2*m5*qdd1i)/2 + (3*L7^2*m5*qdd1i)/16 - (L7*g*m5*sin(q1i - q2i - q3i -
q4i - q5i))/8 - (L6*g*m4*sin(q1i + q2i + q3i + q4i))/4 - (L6*g*m5*sin(q1i
+ q2i + q3i + q4i))/2 + (L4^2*m2*qdd1i*cos(2*q2i))/16 +
(L4^2*m3*qdd1i*cos(2*q2i))/4 + (L4^2*m4*qdd1i*cos(2*q2i))/4 +
(L4^2*m5*qdd1i*cos(2*q2i))/4 - (L7^2*m5*qdd1i*cos(2*q5i))/16 +
(L6^2*m4*qdd1i*cos(2*q2i + 2*q3i + 2*q4i))/8 + (L6^2*m5*qdd1i*cos(2*q2i +
2*q3i + 2*q4i))/2 + (L7^2*m5*qdd1i*cos(2*q2i + 2*q3i + 2*q4i))/16 -
(L4*g*m2*cos(q1i + q2i))/8 - (L4*g*m3*cos(q1i + q2i))/4 -
(L4*g*m4*cos(q1i + q2i))/4 - (L4*g*m5*cos(q1i + q2i))/4 -
(L7*g*m5*sin(q1i + q5i))/4 - (L6*g*m4*sin(q1i - q2i - q3i - q4i))/4 -
(L6*g*m5*sin(q1i - q2i - q3i - q4i))/2 - (L7^2*m5*qd2i^2*sin(q2i + q3i +
q4i - 2*q5i))/16 + (L7^2*m5*qd2i^2*sin(q2i + q3i + q4i + 2*q5i))/16 -
(L7^2*m5*qd3i^2*sin(q2i + q3i + q4i - 2*q5i))/16 +
(L7^2*m5*qd3i^2*sin(q2i + q3i + q4i + 2*q5i))/16 -
(L7^2*m5*qd4i^2*sin(q2i + q3i + q4i - 2*q5i))/16 +
(L7^2*m5*qd4i^2*sin(q2i + q3i + q4i + 2*q5i))/16 +
(L5^2*m3*qdd1i*cos(2*q2i + 2*q3i))/16 + (L5^2*m4*qdd1i*cos(2*q2i +
2*q3i))/4 + (L5^2*m5*qdd1i*cos(2*q2i + 2*q3i))/4 - (L5*g*m3*cos(q1i - q2i
- q3i))/8 - (L5*g*m4*cos(q1i - q2i - q3i))/4 - (L5*g*m5*cos(q1i - q2i -
q3i))/4 - (L3*g*m1*sin(q1i))/2 - L3*g*m2*sin(q1i) - L3*g*m3*sin(q1i) -
L3*g*m4*sin(q1i) - L3*g*m5*sin(q1i) + (L4*g*m2*cos(q1i - q2i))/8 +
(L4*g*m3*cos(q1i - q2i))/4 + (L4*g*m4*cos(q1i - q2i))/4 +
(L4*g*m5*cos(q1i - q2i))/4 - (L7*g*m5*sin(q1i + q2i + q3i + q4i + q5i))/8
+ (L7*g*m5*sin(q1i - q5i))/4 + (L5*g*m3*cos(q1i + q2i + q3i))/8 +
(L5*g*m4*cos(q1i + q2i + q3i))/4 + (L5*g*m5*cos(q1i + q2i + q3i))/4 -
(L7*g*m5*sin(q1i - q2i - q3i - q4i + q5i))/8 - I5i*qd2i*qd5i*sin(q2i +
q3i + q4i) - I5i*qd3i*qd5i*sin(q2i + q3i + q4i) - I5i*qd4i*qd5i*sin(q2i +
q3i + q4i) - Iext*qd2i*qd5i*sin(q2i + q3i + q4i) - Iext*qd3i*qd5i*sin(q2i
+ q3i + q4i) - Iext*qd4i*qd5i*sin(q2i + q3i + q4i) +
(L7^2*m5*qdd2i*cos(q2i + q3i + q4i - 2*q5i))/16 - (L7^2*m5*qdd2i*cos(q2i
+ q3i + q4i + 2*q5i))/16 + (L7^2*m5*qdd3i*cos(q2i + q3i + q4i -
2*q5i))/16 - (L7^2*m5*qdd3i*cos(q2i + q3i + q4i + 2*q5i))/16 +
(L7^2*m5*qdd4i*cos(q2i + q3i + q4i - 2*q5i))/16 - (L7^2*m5*qdd4i*cos(q2i
+ q3i + q4i + 2*q5i))/16 - (L7*g*m5*sin(q1i + q2i + q3i + q4i - q5i))/8 +
(L7^2*m5*qdd1i*cos(2*q2i + 2*q3i + 2*q4i - 2*q5i))/32 +
(L7^2*m5*qdd1i*cos(2*q2i + 2*q3i + 2*q4i + 2*q5i))/32 +
(L7^2*m5*qdd5i*cos(q2i + q3i + q4i))/4 - (3^(1/2)*L5*g*m3*sin(q1i + q2i +
q3i))/8 - (3^(1/2)*L5*g*m4*sin(q1i + q2i + q3i))/4 -
(3^(1/2)*L5*g*m5*sin(q1i + q2i + q3i))/4 - (L3*L7*m5*qd5i^2*sin(q5i))/2 -

109
UNIVERSIDAD NACIONAL DE TRUJILLO
(L4*L7*m5*qdd1i*sin(2*q2i + q3i + q4i - q5i))/8 +
(L5*L7*m5*qdd1i*sin(2*q2i + 2*q3i + q4i + q5i))/8 -
(L6*L7*m5*qd2i^2*sin(q2i + q3i + q4i - q5i))/4 - (L6*L7*m5*qd3i^2*sin(q2i
+ q3i + q4i - q5i))/4 - (L6*L7*m5*qd4i^2*sin(q2i + q3i + q4i - q5i))/4 +
(L6*L7*m5*qd5i^2*sin(q2i + q3i + q4i - q5i))/4 - (L4*L7*m5*qd2i^2*cos(q2i
- q5i))/8 + (L4*L7*m5*qd5i^2*cos(q2i - q5i))/8 - (L5*L7*m5*qd2i^2*cos(q2i
+ q3i + q5i))/8 - (L5*L7*m5*qd3i^2*cos(q2i + q3i + q5i))/8 +
(L5*L7*m5*qd5i^2*cos(q2i + q3i + q5i))/8 - (L7^2*m5*qd2i*qd3i*sin(q2i +
q3i + q4i - 2*q5i))/8 + (L7^2*m5*qd2i*qd3i*sin(q2i + q3i + q4i +
2*q5i))/8 - (L7^2*m5*qd2i*qd4i*sin(q2i + q3i + q4i - 2*q5i))/8 +
(L7^2*m5*qd2i*qd4i*sin(q2i + q3i + q4i + 2*q5i))/8 +
(L7^2*m5*qd2i*qd5i*sin(q2i + q3i + q4i - 2*q5i))/8 +
(L7^2*m5*qd2i*qd5i*sin(q2i + q3i + q4i + 2*q5i))/8 -
(L7^2*m5*qd3i*qd4i*sin(q2i + q3i + q4i - 2*q5i))/8 +
(L7^2*m5*qd3i*qd4i*sin(q2i + q3i + q4i + 2*q5i))/8 +
(L7^2*m5*qd3i*qd5i*sin(q2i + q3i + q4i - 2*q5i))/8 +
(L7^2*m5*qd3i*qd5i*sin(q2i + q3i + q4i + 2*q5i))/8 +
(L7^2*m5*qd4i*qd5i*sin(q2i + q3i + q4i - 2*q5i))/8 +
(L7^2*m5*qd4i*qd5i*sin(q2i + q3i + q4i + 2*q5i))/8 -
(L4*L6*m4*qdd1i*sin(2*q2i + q3i + q4i))/4 - (L4*L6*m5*qdd1i*sin(2*q2i +
q3i + q4i))/2 + (L4*L7*m5*qdd1i*sin(q3i + q4i - q5i))/8 +
(L5*L7*m5*qdd2i*sin(q2i + q3i - q5i))/8 + (L5*L7*m5*qdd3i*sin(q2i + q3i -
q5i))/8 + (L5*L7*m5*qdd5i*sin(q2i + q3i - q5i))/8 +
(L3*L7*m5*qdd1i*cos(q2i + q3i + q4i + q5i))/2 - (L6*L7*m5*qdd2i*cos(q2i +
q3i + q4i + q5i))/4 - (L6*L7*m5*qdd3i*cos(q2i + q3i + q4i + q5i))/4 -
(L6*L7*m5*qdd4i*cos(q2i + q3i + q4i + q5i))/4 + (L6*L7*m5*qdd5i*cos(q2i +
q3i + q4i + q5i))/4 - (L7^2*m5*qd1i*qd2i*sin(2*q2i + 2*q3i + 2*q4i -
2*q5i))/16 - (L7^2*m5*qd1i*qd2i*sin(2*q2i + 2*q3i + 2*q4i + 2*q5i))/16 -
(L7^2*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + 2*q4i - 2*q5i))/16 -
(L7^2*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + 2*q4i + 2*q5i))/16 -
(L7^2*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + 2*q4i - 2*q5i))/16 -
(L7^2*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + 2*q4i + 2*q5i))/16 +
(L7^2*m5*qd1i*qd5i*sin(2*q2i + 2*q3i + 2*q4i - 2*q5i))/16 -
(L7^2*m5*qd1i*qd5i*sin(2*q2i + 2*q3i + 2*q4i + 2*q5i))/16 +
(L6*L7*m5*qdd1i*cos(2*q2i + 2*q3i + 2*q4i + q5i))/4 -
(L7^2*m5*qd2i*qd5i*sin(q2i + q3i + q4i))/4 - (L7^2*m5*qd3i*qd5i*sin(q2i +
q3i + q4i))/4 - (L7^2*m5*qd4i*qd5i*sin(q2i + q3i + q4i))/4 +
(L3*L5*m3*qdd1i*sin(q2i + q3i))/2 + L3*L5*m4*qdd1i*sin(q2i + q3i) +
L3*L5*m5*qdd1i*sin(q2i + q3i) + (L4*L6*m4*qdd1i*sin(q3i + q4i))/4 +
(L4*L6*m5*qdd1i*sin(q3i + q4i))/2 + (L4*L7*m5*qdd2i*sin(q2i + q5i))/8 -
(L5*L7*m5*qdd1i*sin(q4i + q5i))/8 - (L4*L7*m5*qdd5i*sin(q2i + q5i))/8 +
(L5*L7*m5*qdd1i*sin(2*q2i + 2*q3i + q4i - q5i))/8 -
(3^(1/2)*L4^2*m2*qdd1i*sin(2*q2i))/16 -
(3^(1/2)*L4^2*m3*qdd1i*sin(2*q2i))/4 -
(3^(1/2)*L4^2*m4*qdd1i*sin(2*q2i))/4 -
(3^(1/2)*L4^2*m5*qdd1i*sin(2*q2i))/4 - (3^(1/2)*L4*g*m2*sin(q1i + q2i))/8
- (3^(1/2)*L4*g*m3*sin(q1i + q2i))/4 - (3^(1/2)*L4*g*m4*sin(q1i + q2i))/4
- (3^(1/2)*L4*g*m5*sin(q1i + q2i))/4 - (L4^2*m2*qd1i*qd2i*sin(2*q2i))/8 -
(L4^2*m3*qd1i*qd2i*sin(2*q2i))/2 - (L4^2*m4*qd1i*qd2i*sin(2*q2i))/2 -
(L4^2*m5*qd1i*qd2i*sin(2*q2i))/2 + (L7^2*m5*qd1i*qd5i*sin(2*q5i))/8 -
(L6^2*m4*qd1i*qd2i*sin(2*q2i + 2*q3i + 2*q4i))/4 -
(L6^2*m4*qd1i*qd3i*sin(2*q2i + 2*q3i + 2*q4i))/4 -
L6^2*m5*qd1i*qd2i*sin(2*q2i + 2*q3i + 2*q4i) -
(L6^2*m4*qd1i*qd4i*sin(2*q2i + 2*q3i + 2*q4i))/4 -
L6^2*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + 2*q4i) -
(L7^2*m5*qd1i*qd2i*sin(2*q2i + 2*q3i + 2*q4i))/8 -
L6^2*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + 2*q4i) -

110
UNIVERSIDAD NACIONAL DE TRUJILLO
(L7^2*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + 2*q4i))/8 -
(L7^2*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + 2*q4i))/8 +
(L4*L5*m3*qdd1i*cos(q3i))/4 + (L4*L5*m4*qdd1i*cos(q3i))/2 +
(L4*L5*m5*qdd1i*cos(q3i))/2 + (L6*L7*m5*qdd1i*cos(q5i))/2 +
(L3*L7*m5*qdd5i*cos(q5i))/2 + (L5*L7*m5*qd2i^2*cos(q2i + q3i - q5i))/8 +
(L5*L7*m5*qd3i^2*cos(q2i + q3i - q5i))/8 - (L5*L7*m5*qd5i^2*cos(q2i + q3i
- q5i))/8 - (L3*L4*m2*qdd1i*sin(q2i))/2 - L3*L4*m3*qdd1i*sin(q2i) -
L3*L4*m4*qdd1i*sin(q2i) - L3*L4*m5*qdd1i*sin(q2i) -
(L5*L6*m4*qdd1i*sin(q4i))/4 - (L5*L6*m5*qdd1i*sin(q4i))/2 +
(L5*L6*m4*qdd1i*sin(2*q2i + 2*q3i + q4i))/4 + (L5*L6*m5*qdd1i*sin(2*q2i +
2*q3i + q4i))/2 + (L3*L7*m5*qdd1i*cos(q2i + q3i + q4i - q5i))/2 +
(L6*L7*m5*qdd2i*cos(q2i + q3i + q4i - q5i))/4 + (L6*L7*m5*qdd3i*cos(q2i +
q3i + q4i - q5i))/4 + (L6*L7*m5*qdd4i*cos(q2i + q3i + q4i - q5i))/4 +
(L6*L7*m5*qdd5i*cos(q2i + q3i + q4i - q5i))/4 +
(3^(1/2)*L5^2*m3*qdd1i*sin(2*q2i + 2*q3i))/16 +
(3^(1/2)*L5^2*m4*qdd1i*sin(2*q2i + 2*q3i))/4 +
(3^(1/2)*L5^2*m5*qdd1i*sin(2*q2i + 2*q3i))/4 - (3^(1/2)*L5*g*m3*sin(q1i -
q2i - q3i))/8 - (3^(1/2)*L5*g*m4*sin(q1i - q2i - q3i))/4 -
(3^(1/2)*L5*g*m5*sin(q1i - q2i - q3i))/4 - (L4*L7*m5*qdd1i*sin(2*q2i +
q3i + q4i + q5i))/8 + (L6*L7*m5*qd2i^2*sin(q2i + q3i + q4i + q5i))/4 +
(L6*L7*m5*qd3i^2*sin(q2i + q3i + q4i + q5i))/4 + (L6*L7*m5*qd4i^2*sin(q2i
+ q3i + q4i + q5i))/4 - (L6*L7*m5*qd5i^2*sin(q2i + q3i + q4i + q5i))/4 -
(L5^2*m3*qd1i*qd2i*sin(2*q2i + 2*q3i))/8 - (L5^2*m3*qd1i*qd3i*sin(2*q2i +
2*q3i))/8 - (L5^2*m4*qd1i*qd2i*sin(2*q2i + 2*q3i))/2 -
(L5^2*m4*qd1i*qd3i*sin(2*q2i + 2*q3i))/2 - (L5^2*m5*qd1i*qd2i*sin(2*q2i +
2*q3i))/2 - (L5^2*m5*qd1i*qd3i*sin(2*q2i + 2*q3i))/2 +
(L4*L5*m3*qdd1i*cos(2*q2i + q3i))/2 + L4*L5*m4*qdd1i*cos(2*q2i + q3i) +
L4*L5*m5*qdd1i*cos(2*q2i + q3i) + (L4*L7*m5*qd2i^2*cos(q2i + q5i))/8 -
(L4*L7*m5*qd5i^2*cos(q2i + q5i))/8 + (L6*L7*m5*qdd1i*cos(2*q2i + 2*q3i +
2*q4i - q5i))/4 - (L4*L7*m5*qdd2i*sin(q2i - q5i))/8 -
(L5*L7*m5*qdd1i*sin(q4i - q5i))/8 - (L4*L7*m5*qdd5i*sin(q2i - q5i))/8 +
L3*L6*m4*qdd1i*cos(q2i + q3i + q4i) + 2*L3*L6*m5*qdd1i*cos(q2i + q3i +
q4i) - (3^(1/2)*L4*g*m2*sin(q1i - q2i))/8 - (3^(1/2)*L4*g*m3*sin(q1i -
q2i))/4 - (3^(1/2)*L4*g*m4*sin(q1i - q2i))/4 - (3^(1/2)*L4*g*m5*sin(q1i -
q2i))/4 + (L4*L7*m5*qdd1i*sin(q3i + q4i + q5i))/8 -
(L5*L7*m5*qdd2i*sin(q2i + q3i + q5i))/8 - (L5*L7*m5*qdd3i*sin(q2i + q3i +
q5i))/8 + (L5*L7*m5*qdd5i*sin(q2i + q3i + q5i))/8 +
(3^(1/2)*L3*L4*m2*qdd1i*cos(q2i))/2 + 3^(1/2)*L3*L4*m3*qdd1i*cos(q2i) +
3^(1/2)*L3*L4*m4*qdd1i*cos(q2i) + 3^(1/2)*L3*L4*m5*qdd1i*cos(q2i) +
(3^(1/2)*L5*L6*m4*qdd1i*cos(q4i))/4 + (3^(1/2)*L5*L6*m5*qdd1i*cos(q4i))/2
+ (3^(1/2)*L5*L6*m4*qdd1i*cos(2*q2i + 2*q3i + q4i))/4 +
(3^(1/2)*L5*L6*m5*qdd1i*cos(2*q2i + 2*q3i + q4i))/2 +
(3^(1/2)*L4*L5*m3*qdd1i*sin(q3i))/4 + (3^(1/2)*L4*L5*m4*qdd1i*sin(q3i))/2
+ (3^(1/2)*L4*L5*m5*qdd1i*sin(q3i))/2 - (3^(1/2)*L5*L7*m5*qd2i^2*sin(q2i
+ q3i - q5i))/8 - (3^(1/2)*L5*L7*m5*qd3i^2*sin(q2i + q3i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qd5i^2*sin(q2i + q3i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd1i*cos(2*q2i + q3i + q4i + q5i))/8 -
(L3*L4*m2*qd1i*qd2i*cos(q2i))/2 - L3*L4*m3*qd1i*qd2i*cos(q2i) -
L3*L4*m4*qd1i*qd2i*cos(q2i) - L3*L4*m5*qd1i*qd2i*cos(q2i) -
(L5*L6*m4*qd1i*qd4i*cos(q4i))/4 - (L5*L6*m5*qd1i*qd4i*cos(q4i))/2 +
(L5*L6*m4*qd1i*qd2i*cos(2*q2i + 2*q3i + q4i))/2 +
(L5*L6*m4*qd1i*qd3i*cos(2*q2i + 2*q3i + q4i))/2 +
L5*L6*m5*qd1i*qd2i*cos(2*q2i + 2*q3i + q4i) +
(L5*L6*m4*qd1i*qd4i*cos(2*q2i + 2*q3i + q4i))/4 +
L5*L6*m5*qd1i*qd3i*cos(2*q2i + 2*q3i + q4i) +
(L5*L6*m5*qd1i*qd4i*cos(2*q2i + 2*q3i + q4i))/2 +
(3^(1/2)*L5^2*m3*qd1i*qd2i*cos(2*q2i + 2*q3i))/8 +

111
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L5^2*m3*qd1i*qd3i*cos(2*q2i + 2*q3i))/8 +
(3^(1/2)*L5^2*m4*qd1i*qd2i*cos(2*q2i + 2*q3i))/2 +
(3^(1/2)*L5^2*m4*qd1i*qd3i*cos(2*q2i + 2*q3i))/2 +
(3^(1/2)*L5^2*m5*qd1i*qd2i*cos(2*q2i + 2*q3i))/2 +
(3^(1/2)*L5^2*m5*qd1i*qd3i*cos(2*q2i + 2*q3i))/2 -
(L4*L5*m3*qd1i*qd3i*sin(q3i))/4 - (L4*L5*m4*qd1i*qd3i*sin(q3i))/2 -
(L4*L5*m5*qd1i*qd3i*sin(q3i))/2 - (L6*L7*m5*qd1i*qd5i*sin(q5i))/2 -
(L4*L7*m5*qd1i*qd2i*cos(2*q2i + q3i + q4i + q5i))/4 -
(L4*L7*m5*qd1i*qd3i*cos(2*q2i + q3i + q4i + q5i))/8 -
(L4*L7*m5*qd1i*qd4i*cos(2*q2i + q3i + q4i + q5i))/8 -
(L4*L7*m5*qd1i*qd5i*cos(2*q2i + q3i + q4i + q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd2i*cos(q2i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd1i*cos(q4i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd5i*cos(q2i - q5i))/8 - (L3*L7*m5*qd1i*qd2i*sin(q2i +
q3i + q4i - q5i))/2 - (L3*L7*m5*qd1i*qd3i*sin(q2i + q3i + q4i - q5i))/2 -
(L3*L7*m5*qd1i*qd4i*sin(q2i + q3i + q4i - q5i))/2 +
(L3*L7*m5*qd1i*qd5i*sin(q2i + q3i + q4i - q5i))/2 -
(L6*L7*m5*qd2i*qd3i*sin(q2i + q3i + q4i - q5i))/2 -
(L6*L7*m5*qd2i*qd4i*sin(q2i + q3i + q4i - q5i))/2 -
(L6*L7*m5*qd3i*qd4i*sin(q2i + q3i + q4i - q5i))/2 +
(3^(1/2)*L4*L7*m5*qd2i^2*sin(q2i + q5i))/8 -
(3^(1/2)*L4*L7*m5*qd5i^2*sin(q2i + q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd1i*cos(q3i + q4i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qdd2i*cos(q2i + q3i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qdd3i*cos(q2i + q3i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd5i*cos(q2i + q3i + q5i))/8 -
(L5*L7*m5*qd1i*qd4i*cos(q4i - q5i))/8 + (L5*L7*m5*qd1i*qd5i*cos(q4i -
q5i))/8 - L4*L5*m3*qd1i*qd2i*sin(2*q2i + q3i) -
(L4*L5*m3*qd1i*qd3i*sin(2*q2i + q3i))/2 - 2*L4*L5*m4*qd1i*qd2i*sin(2*q2i
+ q3i) - L4*L5*m4*qd1i*qd3i*sin(2*q2i + q3i) -
2*L4*L5*m5*qd1i*qd2i*sin(2*q2i + q3i) - L4*L5*m5*qd1i*qd3i*sin(2*q2i +
q3i) - (L6*L7*m5*qd1i*qd2i*sin(2*q2i + 2*q3i + 2*q4i - q5i))/2 -
(L6*L7*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + 2*q4i - q5i))/2 -
(L6*L7*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + 2*q4i - q5i))/2 +
(L6*L7*m5*qd1i*qd5i*sin(2*q2i + 2*q3i + 2*q4i - q5i))/4 +
(L4*L7*m5*qd1i*qd3i*cos(q3i + q4i + q5i))/8 - (L5*L7*m5*qd2i*qd3i*cos(q2i
+ q3i + q5i))/4 + (L4*L7*m5*qd1i*qd4i*cos(q3i + q4i + q5i))/8 +
(L4*L7*m5*qd1i*qd5i*cos(q3i + q4i + q5i))/8 - L3*L6*m4*qd1i*qd2i*sin(q2i
+ q3i + q4i) - L3*L6*m4*qd1i*qd3i*sin(q2i + q3i + q4i) -
2*L3*L6*m5*qd1i*qd2i*sin(q2i + q3i + q4i) - L3*L6*m4*qd1i*qd4i*sin(q2i +
q3i + q4i) - 2*L3*L6*m5*qd1i*qd3i*sin(q2i + q3i + q4i) -
2*L3*L6*m5*qd1i*qd4i*sin(q2i + q3i + q4i) +
(3^(1/2)*L4*L7*m5*qdd1i*cos(2*q2i + q3i + q4i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd1i*cos(2*q2i + 2*q3i + q4i + q5i))/8 -
(L4*L7*m5*qd1i*qd2i*cos(2*q2i + q3i + q4i - q5i))/4 -
(L4*L7*m5*qd1i*qd3i*cos(2*q2i + q3i + q4i - q5i))/8 -
(L4*L7*m5*qd1i*qd4i*cos(2*q2i + q3i + q4i - q5i))/8 +
(L5*L7*m5*qd1i*qd2i*cos(2*q2i + 2*q3i + q4i + q5i))/4 +
(L4*L7*m5*qd1i*qd5i*cos(2*q2i + q3i + q4i - q5i))/8 +
(L5*L7*m5*qd1i*qd3i*cos(2*q2i + 2*q3i + q4i + q5i))/4 +
(L5*L7*m5*qd1i*qd4i*cos(2*q2i + 2*q3i + q4i + q5i))/8 +
(L5*L7*m5*qd1i*qd5i*cos(2*q2i + 2*q3i + q4i + q5i))/8 -
(3^(1/2)*L4*L7*m5*qd2i^2*sin(q2i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qd5i^2*sin(q2i - q5i))/8 +
(3^(1/2)*L4*L6*m4*qdd1i*cos(2*q2i + q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qdd1i*cos(2*q2i + q3i + q4i))/2 +
(3^(1/2)*L4*L7*m5*qdd1i*cos(q3i + q4i - q5i))/8 +

112
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L5*L7*m5*qdd2i*cos(q2i + q3i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd3i*cos(q2i + q3i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd5i*cos(q2i + q3i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qd2i^2*sin(q2i + q3i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qd3i^2*sin(q2i + q3i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qd5i^2*sin(q2i + q3i + q5i))/8 -
(L4*L6*m4*qd1i*qd2i*cos(2*q2i + q3i + q4i))/2 -
(L4*L6*m4*qd1i*qd3i*cos(2*q2i + q3i + q4i))/4 -
L4*L6*m5*qd1i*qd2i*cos(2*q2i + q3i + q4i) - (L4*L6*m4*qd1i*qd4i*cos(2*q2i
+ q3i + q4i))/4 - (L4*L6*m5*qd1i*qd3i*cos(2*q2i + q3i + q4i))/2 -
(L4*L6*m5*qd1i*qd4i*cos(2*q2i + q3i + q4i))/2 +
(L4*L7*m5*qd1i*qd3i*cos(q3i + q4i - q5i))/8 + (L5*L7*m5*qd2i*qd3i*cos(q2i
+ q3i - q5i))/4 + (L4*L7*m5*qd1i*qd4i*cos(q3i + q4i - q5i))/8 -
(L4*L7*m5*qd1i*qd5i*cos(q3i + q4i - q5i))/8 +
(3^(1/2)*L3*L5*m3*qdd1i*cos(q2i + q3i))/2 +
3^(1/2)*L3*L5*m4*qdd1i*cos(q2i + q3i) + 3^(1/2)*L3*L5*m5*qdd1i*cos(q2i +
q3i) + (3^(1/2)*L4*L6*m4*qdd1i*cos(q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qdd1i*cos(q3i + q4i))/2 -
(3^(1/2)*L4*L7*m5*qdd2i*cos(q2i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd1i*cos(q4i + q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd5i*cos(q2i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd1i*cos(2*q2i + 2*q3i + q4i - q5i))/8 -
(L3*L7*m5*qd1i*qd2i*sin(q2i + q3i + q4i + q5i))/2 -
(L3*L7*m5*qd1i*qd3i*sin(q2i + q3i + q4i + q5i))/2 -
(L3*L7*m5*qd1i*qd4i*sin(q2i + q3i + q4i + q5i))/2 -
(L3*L7*m5*qd1i*qd5i*sin(q2i + q3i + q4i + q5i))/2 +
(L6*L7*m5*qd2i*qd3i*sin(q2i + q3i + q4i + q5i))/2 +
(L6*L7*m5*qd2i*qd4i*sin(q2i + q3i + q4i + q5i))/2 +
(L6*L7*m5*qd3i*qd4i*sin(q2i + q3i + q4i + q5i))/2 +
(L3*L5*m3*qd1i*qd2i*cos(q2i + q3i))/2 + (L3*L5*m3*qd1i*qd3i*cos(q2i +
q3i))/2 + L3*L5*m4*qd1i*qd2i*cos(q2i + q3i) + L3*L5*m4*qd1i*qd3i*cos(q2i
+ q3i) + L3*L5*m5*qd1i*qd2i*cos(q2i + q3i) + L3*L5*m5*qd1i*qd3i*cos(q2i +
q3i) + (L4*L6*m4*qd1i*qd3i*cos(q3i + q4i))/4 +
(L4*L6*m4*qd1i*qd4i*cos(q3i + q4i))/4 + (L4*L6*m5*qd1i*qd3i*cos(q3i +
q4i))/2 + (L4*L6*m5*qd1i*qd4i*cos(q3i + q4i))/2 -
(L5*L7*m5*qd1i*qd4i*cos(q4i + q5i))/8 - (L5*L7*m5*qd1i*qd5i*cos(q4i +
q5i))/8 + (L5*L7*m5*qd1i*qd2i*cos(2*q2i + 2*q3i + q4i - q5i))/4 +
(L5*L7*m5*qd1i*qd3i*cos(2*q2i + 2*q3i + q4i - q5i))/4 +
(L5*L7*m5*qd1i*qd4i*cos(2*q2i + 2*q3i + q4i - q5i))/8 -
(L5*L7*m5*qd1i*qd5i*cos(2*q2i + 2*q3i + q4i - q5i))/8 -
(3^(1/2)*L4^2*m2*qd1i*qd2i*cos(2*q2i))/8 -
(3^(1/2)*L4^2*m3*qd1i*qd2i*cos(2*q2i))/2 -
(3^(1/2)*L4^2*m4*qd1i*qd2i*cos(2*q2i))/2 -
(3^(1/2)*L4^2*m5*qd1i*qd2i*cos(2*q2i))/2 - (L6*L7*m5*qd1i*qd2i*sin(2*q2i
+ 2*q3i + 2*q4i + q5i))/2 - (L6*L7*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + 2*q4i
+ q5i))/2 - (L6*L7*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + 2*q4i + q5i))/2 -
(L6*L7*m5*qd1i*qd5i*sin(2*q2i + 2*q3i + 2*q4i + q5i))/4 -
(3^(1/2)*L4*L6*m4*qd1i*qd2i*sin(2*q2i + q3i + q4i))/2 -
(3^(1/2)*L4*L6*m4*qd1i*qd3i*sin(2*q2i + q3i + q4i))/4 -
3^(1/2)*L4*L6*m5*qd1i*qd2i*sin(2*q2i + q3i + q4i) -
(3^(1/2)*L4*L6*m4*qd1i*qd4i*sin(2*q2i + q3i + q4i))/4 -
(3^(1/2)*L4*L6*m5*qd1i*qd3i*sin(2*q2i + q3i + q4i))/2 -
(3^(1/2)*L4*L6*m5*qd1i*qd4i*sin(2*q2i + q3i + q4i))/2 -
(3^(1/2)*L4*L7*m5*qd1i*qd3i*sin(q3i + q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qd2i*qd3i*sin(q2i + q3i - q5i))/4 -
(3^(1/2)*L4*L7*m5*qd1i*qd4i*sin(q3i + q4i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qd1i*qd5i*sin(q3i + q4i - q5i))/8 -

113
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L3*L5*m3*qd1i*qd2i*sin(q2i + q3i))/2 -
(3^(1/2)*L3*L5*m3*qd1i*qd3i*sin(q2i + q3i))/2 -
3^(1/2)*L3*L5*m4*qd1i*qd2i*sin(q2i + q3i) -
3^(1/2)*L3*L5*m4*qd1i*qd3i*sin(q2i + q3i) -
3^(1/2)*L3*L5*m5*qd1i*qd2i*sin(q2i + q3i) -
3^(1/2)*L3*L5*m5*qd1i*qd3i*sin(q2i + q3i) -
(3^(1/2)*L4*L6*m4*qd1i*qd3i*sin(q3i + q4i))/4 -
(3^(1/2)*L4*L6*m4*qd1i*qd4i*sin(q3i + q4i))/4 -
(3^(1/2)*L4*L6*m5*qd1i*qd3i*sin(q3i + q4i))/2 -
(3^(1/2)*L4*L6*m5*qd1i*qd4i*sin(q3i + q4i))/2 -
(3^(1/2)*L5*L7*m5*qd1i*qd4i*sin(q4i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qd1i*qd5i*sin(q4i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qd1i*qd2i*sin(2*q2i + 2*q3i + q4i - q5i))/4 -
(3^(1/2)*L5*L7*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + q4i - q5i))/4 -
(3^(1/2)*L5*L7*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + q4i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qd1i*qd5i*sin(2*q2i + 2*q3i + q4i - q5i))/8 +
(3^(1/2)*L4*L5*m3*qd1i*qd3i*cos(q3i))/4 +
(3^(1/2)*L4*L5*m4*qd1i*qd3i*cos(q3i))/2 +
(3^(1/2)*L4*L5*m5*qd1i*qd3i*cos(q3i))/2 -
(3^(1/2)*L3*L4*m2*qd1i*qd2i*sin(q2i))/2 -
3^(1/2)*L3*L4*m3*qd1i*qd2i*sin(q2i) - 3^(1/2)*L3*L4*m4*qd1i*qd2i*sin(q2i)
- 3^(1/2)*L3*L4*m5*qd1i*qd2i*sin(q2i) -
(3^(1/2)*L5*L6*m4*qd1i*qd4i*sin(q4i))/4 -
(3^(1/2)*L5*L6*m5*qd1i*qd4i*sin(q4i))/2 -
(3^(1/2)*L5*L6*m4*qd1i*qd2i*sin(2*q2i + 2*q3i + q4i))/2 -
(3^(1/2)*L5*L6*m4*qd1i*qd3i*sin(2*q2i + 2*q3i + q4i))/2 -
3^(1/2)*L5*L6*m5*qd1i*qd2i*sin(2*q2i + 2*q3i + q4i) -
(3^(1/2)*L5*L6*m4*qd1i*qd4i*sin(2*q2i + 2*q3i + q4i))/4 -
3^(1/2)*L5*L6*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + q4i) -
(3^(1/2)*L5*L6*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + q4i))/2 -
(3^(1/2)*L4*L7*m5*qd1i*qd2i*sin(2*q2i + q3i + q4i + q5i))/4 -
(3^(1/2)*L4*L7*m5*qd1i*qd3i*sin(2*q2i + q3i + q4i + q5i))/8 -
(3^(1/2)*L4*L7*m5*qd1i*qd4i*sin(2*q2i + q3i + q4i + q5i))/8 -
(3^(1/2)*L4*L7*m5*qd1i*qd5i*sin(2*q2i + q3i + q4i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qd1i*qd4i*sin(q4i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qd1i*qd5i*sin(q4i - q5i))/8 -
(3^(1/2)*L4*L7*m5*qd1i*qd3i*sin(q3i + q4i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qd2i*qd3i*sin(q2i + q3i + q5i))/4 -
(3^(1/2)*L4*L7*m5*qd1i*qd4i*sin(q3i + q4i + q5i))/8 -
(3^(1/2)*L4*L7*m5*qd1i*qd5i*sin(q3i + q4i + q5i))/8 -
(3^(1/2)*L4*L7*m5*qd1i*qd2i*sin(2*q2i + q3i + q4i - q5i))/4 -
(3^(1/2)*L4*L7*m5*qd1i*qd3i*sin(2*q2i + q3i + q4i - q5i))/8 -
(3^(1/2)*L4*L7*m5*qd1i*qd4i*sin(2*q2i + q3i + q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qd1i*qd2i*sin(2*q2i + 2*q3i + q4i + q5i))/4 +
(3^(1/2)*L4*L7*m5*qd1i*qd5i*sin(2*q2i + q3i + q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qd1i*qd3i*sin(2*q2i + 2*q3i + q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd1i*qd4i*sin(2*q2i + 2*q3i + q4i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qd1i*qd5i*sin(2*q2i + 2*q3i + q4i + q5i))/8
(a.37)

2 = I2i*qdd2i + I3i*qdd2i + I3i*qdd3i + I4i*qdd2i + I4i*qdd3i +


I5i*qdd2i + I4i*qdd4i + I5i*qdd3i + I5i*qdd4i + Iext*qdd2i + Iext*qdd3i +
Iext*qdd4i + b2*qd2i + (L4^2*m2*qdd2i)/4 + L4^2*m3*qdd2i + L4^2*m4*qdd2i
+ (L5^2*m3*qdd2i)/4 + L4^2*m5*qdd2i + (L5^2*m3*qdd3i)/4 + L5^2*m4*qdd2i +
L5^2*m4*qdd3i + L5^2*m5*qdd2i + (L6^2*m4*qdd2i)/4 + L5^2*m5*qdd3i +
(L6^2*m4*qdd3i)/4 + L6^2*m5*qdd2i + (L6^2*m4*qdd4i)/4 + L6^2*m5*qdd3i +

114
UNIVERSIDAD NACIONAL DE TRUJILLO
(L7^2*m5*qdd2i)/8 + L6^2*m5*qdd4i + (L7^2*m5*qdd3i)/8 + (L7^2*m5*qdd4i)/8
+ (L7*g*m5*sin(q1i - q2i - q3i - q4i - q5i))/8 - (L6*g*m4*sin(q1i + q2i +
q3i + q4i))/4 - (L6*g*m5*sin(q1i + q2i + q3i + q4i))/2 +
(L7^2*m5*qdd2i*cos(2*q5i))/8 + (L7^2*m5*qdd3i*cos(2*q5i))/8 +
(L7^2*m5*qdd4i*cos(2*q5i))/8 - (L4*g*m2*cos(q1i + q2i))/8 -
(L4*g*m3*cos(q1i + q2i))/4 - (L4*g*m4*cos(q1i + q2i))/4 -
(L4*g*m5*cos(q1i + q2i))/4 + (L6*g*m4*sin(q1i - q2i - q3i - q4i))/4 +
(L6*g*m5*sin(q1i - q2i - q3i - q4i))/2 + (L5*g*m3*cos(q1i - q2i - q3i))/8
+ (L5*g*m4*cos(q1i - q2i - q3i))/4 + (L5*g*m5*cos(q1i - q2i - q3i))/4 +
(L7^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i - 2*q5i))/32 +
(L7^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i + 2*q5i))/32 - (L4*g*m2*cos(q1i
- q2i))/8 - (L4*g*m3*cos(q1i - q2i))/4 - (L4*g*m4*cos(q1i - q2i))/4 -
(L4*g*m5*cos(q1i - q2i))/4 - (L7*g*m5*sin(q1i + q2i + q3i + q4i + q5i))/8
+ (L4^2*m2*qd1i^2*sin(2*q2i))/16 + (L4^2*m3*qd1i^2*sin(2*q2i))/4 +
(L4^2*m4*qd1i^2*sin(2*q2i))/4 + (L4^2*m5*qd1i^2*sin(2*q2i))/4 +
(L6^2*m4*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i))/8 + (L6^2*m5*qd1i^2*sin(2*q2i
+ 2*q3i + 2*q4i))/2 + (L7^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i))/16 +
(L5*g*m3*cos(q1i + q2i + q3i))/8 + (L5*g*m4*cos(q1i + q2i + q3i))/4 +
(L5*g*m5*cos(q1i + q2i + q3i))/4 + (L7*g*m5*sin(q1i - q2i - q3i - q4i +
q5i))/8 + I5i*qd1i*qd5i*sin(q2i + q3i + q4i) + Iext*qd1i*qd5i*sin(q2i +
q3i + q4i) + (L5^2*m3*qd1i^2*sin(2*q2i + 2*q3i))/16 +
(L5^2*m4*qd1i^2*sin(2*q2i + 2*q3i))/4 + (L5^2*m5*qd1i^2*sin(2*q2i +
2*q3i))/4 + (L7^2*m5*qdd1i*cos(q2i + q3i + q4i - 2*q5i))/16 -
(L7^2*m5*qdd1i*cos(q2i + q3i + q4i + 2*q5i))/16 - (L7*g*m5*sin(q1i + q2i
+ q3i + q4i - q5i))/8 - (3^(1/2)*L5*g*m3*sin(q1i + q2i + q3i))/8 -
(3^(1/2)*L5*g*m4*sin(q1i + q2i + q3i))/4 - (3^(1/2)*L5*g*m5*sin(q1i + q2i
+ q3i))/4 + (L3*L4*m2*qd1i^2*cos(q2i))/4 + (L3*L4*m3*qd1i^2*cos(q2i))/2 +
(L3*L4*m4*qd1i^2*cos(q2i))/2 + (L3*L4*m5*qd1i^2*cos(q2i))/2 -
(L5*L6*m4*qd4i^2*cos(q4i))/4 - (L5*L6*m5*qd4i^2*cos(q4i))/2 -
(L5*L6*m4*qd1i^2*cos(2*q2i + 2*q3i + q4i))/4 - (L5*L6*m5*qd1i^2*cos(2*q2i
+ 2*q3i + q4i))/2 - (3^(1/2)*L5^2*m3*qd1i^2*cos(2*q2i + 2*q3i))/16 -
(3^(1/2)*L5^2*m4*qd1i^2*cos(2*q2i + 2*q3i))/4 -
(3^(1/2)*L5^2*m5*qd1i^2*cos(2*q2i + 2*q3i))/4 -
(L4*L5*m3*qd3i^2*sin(q3i))/4 - (L4*L5*m4*qd3i^2*sin(q3i))/2 -
(L4*L5*m5*qd3i^2*sin(q3i))/2 + (L4*L7*m5*qd1i^2*cos(2*q2i + q3i + q4i +
q5i))/8 + (L3*L7*m5*qd1i^2*sin(q2i + q3i + q4i - q5i))/4 -
(L5*L7*m5*qd4i^2*cos(q4i - q5i))/8 - (L5*L7*m5*qd5i^2*cos(q4i - q5i))/8 +
(L4*L5*m3*qd1i^2*sin(2*q2i + q3i))/2 + L4*L5*m4*qd1i^2*sin(2*q2i + q3i) +
L4*L5*m5*qd1i^2*sin(2*q2i + q3i) + (L6*L7*m5*qd1i^2*sin(2*q2i + 2*q3i +
2*q4i - q5i))/4 + (L4*L7*m5*qd3i^2*cos(q3i + q4i + q5i))/8 +
(L4*L7*m5*qd4i^2*cos(q3i + q4i + q5i))/8 + (L4*L7*m5*qd5i^2*cos(q3i + q4i
+ q5i))/8 + (L7^2*m5*qd1i*qd5i*sin(q2i + q3i + q4i - 2*q5i))/8 +
(L7^2*m5*qd1i*qd5i*sin(q2i + q3i + q4i + 2*q5i))/8 +
(L5*L7*m5*qdd1i*sin(q2i + q3i - q5i))/8 + (L4*L7*m5*qdd2i*sin(q3i + q4i -
q5i))/4 + (L4*L7*m5*qdd3i*sin(q3i + q4i - q5i))/8 +
(L4*L7*m5*qdd4i*sin(q3i + q4i - q5i))/8 - (L4*L7*m5*qdd5i*sin(q3i + q4i -
q5i))/8 + (L3*L6*m4*qd1i^2*sin(q2i + q3i + q4i))/2 +
L3*L6*m5*qd1i^2*sin(q2i + q3i + q4i) - (L6*L7*m5*qdd1i*cos(q2i + q3i +
q4i + q5i))/4 + (L4*L7*m5*qd1i^2*cos(2*q2i + q3i + q4i - q5i))/8 -
(L5*L7*m5*qd1i^2*cos(2*q2i + 2*q3i + q4i + q5i))/8 +
(L7^2*m5*qd1i*qd5i*sin(q2i + q3i + q4i))/4 + (L4*L6*m4*qdd2i*sin(q3i +
q4i))/2 + (L4*L6*m4*qdd3i*sin(q3i + q4i))/4 + L4*L6*m5*qdd2i*sin(q3i +
q4i) + (L4*L7*m5*qdd1i*sin(q2i + q5i))/8 + (L4*L6*m4*qdd4i*sin(q3i +
q4i))/4 + (L4*L6*m5*qdd3i*sin(q3i + q4i))/2 + (L4*L6*m5*qdd4i*sin(q3i +
q4i))/2 - (L5*L7*m5*qdd2i*sin(q4i + q5i))/4 - (L5*L7*m5*qdd3i*sin(q4i +
q5i))/4 - (L5*L7*m5*qdd4i*sin(q4i + q5i))/8 - (L5*L7*m5*qdd5i*sin(q4i +
q5i))/8 - (3^(1/2)*L4*g*m2*sin(q1i + q2i))/8 - (3^(1/2)*L4*g*m3*sin(q1i +

115
UNIVERSIDAD NACIONAL DE TRUJILLO
q2i))/4 - (3^(1/2)*L4*g*m4*sin(q1i + q2i))/4 - (3^(1/2)*L4*g*m5*sin(q1i +
q2i))/4 - (L7^2*m5*qd2i*qd5i*sin(2*q5i))/4 -
(L7^2*m5*qd3i*qd5i*sin(2*q5i))/4 - (L7^2*m5*qd4i*qd5i*sin(2*q5i))/4 +
(L4*L5*m3*qdd2i*cos(q3i))/2 + (L4*L5*m3*qdd3i*cos(q3i))/4 +
L4*L5*m4*qdd2i*cos(q3i) + (L4*L5*m4*qdd3i*cos(q3i))/2 +
L4*L5*m5*qdd2i*cos(q3i) + (L4*L5*m5*qdd3i*cos(q3i))/2 +
L6*L7*m5*qdd2i*cos(q5i) + L6*L7*m5*qdd3i*cos(q5i) +
L6*L7*m5*qdd4i*cos(q5i) + (L4*L6*m4*qd1i^2*cos(2*q2i + q3i + q4i))/4 +
(L4*L6*m5*qd1i^2*cos(2*q2i + q3i + q4i))/2 + (L4*L7*m5*qd3i^2*cos(q3i +
q4i - q5i))/8 + (L4*L7*m5*qd4i^2*cos(q3i + q4i - q5i))/8 +
(L4*L7*m5*qd5i^2*cos(q3i + q4i - q5i))/8 - (L5*L6*m4*qdd2i*sin(q4i))/2 -
(L5*L6*m4*qdd3i*sin(q4i))/2 - L5*L6*m5*qdd2i*sin(q4i) -
(L5*L6*m4*qdd4i*sin(q4i))/4 - L5*L6*m5*qdd3i*sin(q4i) -
(L5*L6*m5*qdd4i*sin(q4i))/2 + (L6*L7*m5*qdd1i*cos(q2i + q3i + q4i -
q5i))/4 + (3^(1/2)*L5*g*m3*sin(q1i - q2i - q3i))/8 +
(3^(1/2)*L5*g*m4*sin(q1i - q2i - q3i))/4 + (3^(1/2)*L5*g*m5*sin(q1i - q2i
- q3i))/4 + (L3*L7*m5*qd1i^2*sin(q2i + q3i + q4i + q5i))/4 -
(L3*L5*m3*qd1i^2*cos(q2i + q3i))/4 - (L3*L5*m4*qd1i^2*cos(q2i + q3i))/2 -
(L3*L5*m5*qd1i^2*cos(q2i + q3i))/2 + (L4*L6*m4*qd3i^2*cos(q3i + q4i))/4 +
(L4*L6*m4*qd4i^2*cos(q3i + q4i))/4 + (L4*L6*m5*qd3i^2*cos(q3i + q4i))/2 +
(L4*L6*m5*qd4i^2*cos(q3i + q4i))/2 - (L5*L7*m5*qd4i^2*cos(q4i + q5i))/8 -
(L5*L7*m5*qd5i^2*cos(q4i + q5i))/8 - (L5*L7*m5*qd1i^2*cos(2*q2i + 2*q3i +
q4i - q5i))/8 + (3^(1/2)*L4^2*m2*qd1i^2*cos(2*q2i))/16 +
(3^(1/2)*L4^2*m3*qd1i^2*cos(2*q2i))/4 +
(3^(1/2)*L4^2*m4*qd1i^2*cos(2*q2i))/4 +
(3^(1/2)*L4^2*m5*qd1i^2*cos(2*q2i))/4 - (L4*L7*m5*qdd1i*sin(q2i - q5i))/8
- (L5*L7*m5*qdd2i*sin(q4i - q5i))/4 - (L5*L7*m5*qdd3i*sin(q4i - q5i))/4 -
(L5*L7*m5*qdd4i*sin(q4i - q5i))/8 + (L5*L7*m5*qdd5i*sin(q4i - q5i))/8 +
(L6*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i + q5i))/4 +
(3^(1/2)*L4*g*m2*sin(q1i - q2i))/8 + (3^(1/2)*L4*g*m3*sin(q1i - q2i))/4 +
(3^(1/2)*L4*g*m4*sin(q1i - q2i))/4 + (3^(1/2)*L4*g*m5*sin(q1i - q2i))/4 -
(L5*L7*m5*qdd1i*sin(q2i + q3i + q5i))/8 + (L4*L7*m5*qdd2i*sin(q3i + q4i +
q5i))/4 + (L4*L7*m5*qdd3i*sin(q3i + q4i + q5i))/8 +
(L4*L7*m5*qdd4i*sin(q3i + q4i + q5i))/8 + (L4*L7*m5*qdd5i*sin(q3i + q4i +
q5i))/8 + (3^(1/2)*L5*L6*m4*qdd2i*cos(q4i))/2 +
(3^(1/2)*L5*L6*m4*qdd3i*cos(q4i))/2 + 3^(1/2)*L5*L6*m5*qdd2i*cos(q4i) +
(3^(1/2)*L5*L6*m4*qdd4i*cos(q4i))/4 + 3^(1/2)*L5*L6*m5*qdd3i*cos(q4i) +
(3^(1/2)*L5*L6*m5*qdd4i*cos(q4i))/2 + (3^(1/2)*L4*L5*m3*qdd2i*sin(q3i))/2
+ (3^(1/2)*L4*L5*m3*qdd3i*sin(q3i))/4 + 3^(1/2)*L4*L5*m4*qdd2i*sin(q3i) +
(3^(1/2)*L4*L5*m4*qdd3i*sin(q3i))/2 + 3^(1/2)*L4*L5*m5*qdd2i*sin(q3i) +
(3^(1/2)*L4*L5*m5*qdd3i*sin(q3i))/2 + (3^(1/2)*L4*L6*m4*qd1i^2*sin(2*q2i
+ q3i + q4i))/4 + (3^(1/2)*L4*L6*m5*qd1i^2*sin(2*q2i + q3i + q4i))/2 -
(3^(1/2)*L4*L7*m5*qd3i^2*sin(q3i + q4i - q5i))/8 -
(3^(1/2)*L4*L7*m5*qd4i^2*sin(q3i + q4i - q5i))/8 -
(3^(1/2)*L4*L7*m5*qd5i^2*sin(q3i + q4i - q5i))/8 -
(L5*L6*m4*qd2i*qd4i*cos(q4i))/2 - (L5*L6*m4*qd3i*qd4i*cos(q4i))/2 -
L5*L6*m5*qd2i*qd4i*cos(q4i) - L5*L6*m5*qd3i*qd4i*cos(q4i) -
(L4*L5*m3*qd2i*qd3i*sin(q3i))/2 - L4*L5*m4*qd2i*qd3i*sin(q3i) -
L4*L5*m5*qd2i*qd3i*sin(q3i) - L6*L7*m5*qd2i*qd5i*sin(q5i) -
L6*L7*m5*qd3i*qd5i*sin(q5i) - L6*L7*m5*qd4i*qd5i*sin(q5i) +
(3^(1/2)*L4*L7*m5*qdd1i*cos(q2i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd2i*cos(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qdd3i*cos(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qdd4i*cos(q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qdd5i*cos(q4i - q5i))/8 + (L6*L7*m5*qd1i*qd5i*sin(q2i +
q3i + q4i - q5i))/2 + (3^(1/2)*L3*L5*m3*qd1i^2*sin(q2i + q3i))/4 +
(3^(1/2)*L3*L5*m4*qd1i^2*sin(q2i + q3i))/2 +

116
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L3*L5*m5*qd1i^2*sin(q2i + q3i))/2 -
(3^(1/2)*L4*L6*m4*qd3i^2*sin(q3i + q4i))/4 -
(3^(1/2)*L4*L6*m4*qd4i^2*sin(q3i + q4i))/4 -
(3^(1/2)*L4*L6*m5*qd3i^2*sin(q3i + q4i))/2 -
(3^(1/2)*L4*L6*m5*qd4i^2*sin(q3i + q4i))/2 -
(3^(1/2)*L5*L7*m5*qd4i^2*sin(q4i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qd5i^2*sin(q4i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qdd1i*cos(q2i + q3i + q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd2i*cos(q3i + q4i + q5i))/4 +
(3^(1/2)*L4*L7*m5*qdd3i*cos(q3i + q4i + q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd4i*cos(q3i + q4i + q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd5i*cos(q3i + q4i + q5i))/8 +
(L4*L7*m5*qd1i*qd5i*cos(q2i - q5i))/4 - (L5*L7*m5*qd2i*qd4i*cos(q4i -
q5i))/4 + (L5*L7*m5*qd2i*qd5i*cos(q4i - q5i))/4 -
(L5*L7*m5*qd3i*qd4i*cos(q4i - q5i))/4 + (L5*L7*m5*qd3i*qd5i*cos(q4i -
q5i))/4 + (L5*L7*m5*qd4i*qd5i*cos(q4i - q5i))/4 +
(L4*L7*m5*qd2i*qd3i*cos(q3i + q4i + q5i))/4 - (L5*L7*m5*qd1i*qd5i*cos(q2i
+ q3i + q5i))/4 + (L4*L7*m5*qd2i*qd4i*cos(q3i + q4i + q5i))/4 +
(L4*L7*m5*qd2i*qd5i*cos(q3i + q4i + q5i))/4 + (L4*L7*m5*qd3i*qd4i*cos(q3i
+ q4i + q5i))/4 + (L4*L7*m5*qd3i*qd5i*cos(q3i + q4i + q5i))/4 +
(L4*L7*m5*qd4i*qd5i*cos(q3i + q4i + q5i))/4 +
(3^(1/2)*L4*L5*m3*qd3i^2*cos(q3i))/4 +
(3^(1/2)*L4*L5*m4*qd3i^2*cos(q3i))/2 +
(3^(1/2)*L4*L5*m5*qd3i^2*cos(q3i))/2 +
(3^(1/2)*L3*L4*m2*qd1i^2*sin(q2i))/4 +
(3^(1/2)*L3*L4*m3*qd1i^2*sin(q2i))/2 +
(3^(1/2)*L3*L4*m4*qd1i^2*sin(q2i))/2 +
(3^(1/2)*L3*L4*m5*qd1i^2*sin(q2i))/2 -
(3^(1/2)*L5*L6*m4*qd4i^2*sin(q4i))/4 -
(3^(1/2)*L5*L6*m5*qd4i^2*sin(q4i))/2 + (3^(1/2)*L5*L6*m4*qd1i^2*sin(2*q2i
+ 2*q3i + q4i))/4 + (3^(1/2)*L5*L6*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i))/2
+ (3^(1/2)*L4*L7*m5*qd1i^2*sin(2*q2i + q3i + q4i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qd4i^2*sin(q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qd5i^2*sin(q4i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd1i*cos(q2i + q3i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd2i*cos(q3i + q4i - q5i))/4 +
(3^(1/2)*L4*L7*m5*qdd3i*cos(q3i + q4i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd4i*cos(q3i + q4i - q5i))/8 -
(3^(1/2)*L4*L7*m5*qdd5i*cos(q3i + q4i - q5i))/8 -
(3^(1/2)*L4*L7*m5*qd3i^2*sin(q3i + q4i + q5i))/8 -
(3^(1/2)*L4*L7*m5*qd4i^2*sin(q3i + q4i + q5i))/8 -
(3^(1/2)*L4*L7*m5*qd5i^2*sin(q3i + q4i + q5i))/8 +
(L4*L7*m5*qd2i*qd3i*cos(q3i + q4i - q5i))/4 - (L5*L7*m5*qd1i*qd5i*cos(q2i
+ q3i - q5i))/4 + (L4*L7*m5*qd2i*qd4i*cos(q3i + q4i - q5i))/4 -
(L4*L7*m5*qd2i*qd5i*cos(q3i + q4i - q5i))/4 + (L4*L7*m5*qd3i*qd4i*cos(q3i
+ q4i - q5i))/4 - (L4*L7*m5*qd3i*qd5i*cos(q3i + q4i - q5i))/4 -
(L4*L7*m5*qd4i*qd5i*cos(q3i + q4i - q5i))/4 +
(3^(1/2)*L4*L6*m4*qdd2i*cos(q3i + q4i))/2 +
(3^(1/2)*L4*L6*m4*qdd3i*cos(q3i + q4i))/4 +
3^(1/2)*L4*L6*m5*qdd2i*cos(q3i + q4i) - (3^(1/2)*L4*L7*m5*qdd1i*cos(q2i +
q5i))/8 + (3^(1/2)*L4*L6*m4*qdd4i*cos(q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qdd3i*cos(q3i + q4i))/2 +
(3^(1/2)*L4*L6*m5*qdd4i*cos(q3i + q4i))/2 +
(3^(1/2)*L5*L7*m5*qdd2i*cos(q4i + q5i))/4 +
(3^(1/2)*L5*L7*m5*qdd3i*cos(q4i + q5i))/4 +
(3^(1/2)*L5*L7*m5*qdd4i*cos(q4i + q5i))/8 +

117
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L5*L7*m5*qdd5i*cos(q4i + q5i))/8 + (L6*L7*m5*qd1i*qd5i*sin(q2i +
q3i + q4i + q5i))/2 + (3^(1/2)*L4*L7*m5*qd1i^2*sin(2*q2i + q3i + q4i -
q5i))/8 + (3^(1/2)*L5*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i + q5i))/8 +
(L4*L6*m4*qd2i*qd3i*cos(q3i + q4i))/2 + (L4*L6*m4*qd2i*qd4i*cos(q3i +
q4i))/2 + L4*L6*m5*qd2i*qd3i*cos(q3i + q4i) + (L4*L6*m4*qd3i*qd4i*cos(q3i
+ q4i))/2 + L4*L6*m5*qd2i*qd4i*cos(q3i + q4i) +
L4*L6*m5*qd3i*qd4i*cos(q3i + q4i) + (L4*L7*m5*qd1i*qd5i*cos(q2i + q5i))/4
- (L5*L7*m5*qd2i*qd4i*cos(q4i + q5i))/4 - (L5*L7*m5*qd2i*qd5i*cos(q4i +
q5i))/4 - (L5*L7*m5*qd3i*qd4i*cos(q4i + q5i))/4 -
(L5*L7*m5*qd3i*qd5i*cos(q4i + q5i))/4 - (L5*L7*m5*qd4i*qd5i*cos(q4i +
q5i))/4 - (3^(1/2)*L4*L7*m5*qd2i*qd3i*sin(q3i + q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qd1i*qd5i*sin(q2i + q3i - q5i))/4 -
(3^(1/2)*L4*L7*m5*qd2i*qd4i*sin(q3i + q4i - q5i))/4 +
(3^(1/2)*L4*L7*m5*qd2i*qd5i*sin(q3i + q4i - q5i))/4 -
(3^(1/2)*L4*L7*m5*qd3i*qd4i*sin(q3i + q4i - q5i))/4 +
(3^(1/2)*L4*L7*m5*qd3i*qd5i*sin(q3i + q4i - q5i))/4 +
(3^(1/2)*L4*L7*m5*qd4i*qd5i*sin(q3i + q4i - q5i))/4 -
(3^(1/2)*L4*L6*m4*qd2i*qd3i*sin(q3i + q4i))/2 -
(3^(1/2)*L4*L6*m4*qd2i*qd4i*sin(q3i + q4i))/2 -
3^(1/2)*L4*L6*m5*qd2i*qd3i*sin(q3i + q4i) -
(3^(1/2)*L4*L6*m4*qd3i*qd4i*sin(q3i + q4i))/2 -
3^(1/2)*L4*L6*m5*qd2i*qd4i*sin(q3i + q4i) -
3^(1/2)*L4*L6*m5*qd3i*qd4i*sin(q3i + q4i) +
(3^(1/2)*L4*L7*m5*qd1i*qd5i*sin(q2i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd2i*qd4i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd2i*qd5i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd3i*qd4i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd3i*qd5i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd4i*qd5i*sin(q4i + q5i))/4 +
(3^(1/2)*L4*L5*m3*qd2i*qd3i*cos(q3i))/2 +
3^(1/2)*L4*L5*m4*qd2i*qd3i*cos(q3i) + 3^(1/2)*L4*L5*m5*qd2i*qd3i*cos(q3i)
- (3^(1/2)*L5*L6*m4*qd2i*qd4i*sin(q4i))/2 -
(3^(1/2)*L5*L6*m4*qd3i*qd4i*sin(q4i))/2 -
3^(1/2)*L5*L6*m5*qd2i*qd4i*sin(q4i) - 3^(1/2)*L5*L6*m5*qd3i*qd4i*sin(q4i)
+ (3^(1/2)*L4*L7*m5*qd1i*qd5i*sin(q2i - q5i))/4 -
(3^(1/2)*L5*L7*m5*qd2i*qd4i*sin(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qd2i*qd5i*sin(q4i - q5i))/4 -
(3^(1/2)*L5*L7*m5*qd3i*qd4i*sin(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qd3i*qd5i*sin(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qd4i*qd5i*sin(q4i - q5i))/4 -
(3^(1/2)*L4*L7*m5*qd2i*qd3i*sin(q3i + q4i + q5i))/4 +
(3^(1/2)*L5*L7*m5*qd1i*qd5i*sin(q2i + q3i + q5i))/4 -
(3^(1/2)*L4*L7*m5*qd2i*qd4i*sin(q3i + q4i + q5i))/4 -
(3^(1/2)*L4*L7*m5*qd2i*qd5i*sin(q3i + q4i + q5i))/4 -
(3^(1/2)*L4*L7*m5*qd3i*qd4i*sin(q3i + q4i + q5i))/4 -
(3^(1/2)*L4*L7*m5*qd3i*qd5i*sin(q3i + q4i + q5i))/4 -
(3^(1/2)*L4*L7*m5*qd4i*qd5i*sin(q3i + q4i + q5i))/4 (a.38)

3 = I3i*qdd2i + I3i*qdd3i + I4i*qdd2i + I4i*qdd3i + I5i*qdd2i +


I4i*qdd4i + I5i*qdd3i + I5i*qdd4i + Iext*qdd2i + Iext*qdd3i + Iext*qdd4i
+ b3*qd3i + (L5^2*m3*qdd2i)/4 + (L5^2*m3*qdd3i)/4 + L5^2*m4*qdd2i +
L5^2*m4*qdd3i + L5^2*m5*qdd2i + (L6^2*m4*qdd2i)/4 + L5^2*m5*qdd3i +
(L6^2*m4*qdd3i)/4 + L6^2*m5*qdd2i + (L6^2*m4*qdd4i)/4 + L6^2*m5*qdd3i +
(L7^2*m5*qdd2i)/8 + L6^2*m5*qdd4i + (L7^2*m5*qdd3i)/8 + (L7^2*m5*qdd4i)/8
+ (L7*g*m5*sin(q1i - q2i - q3i - q4i - q5i))/8 - (L6*g*m4*sin(q1i + q2i +

118
UNIVERSIDAD NACIONAL DE TRUJILLO
q3i + q4i))/4 - (L6*g*m5*sin(q1i + q2i + q3i + q4i))/2 +
(L7^2*m5*qdd2i*cos(2*q5i))/8 + (L7^2*m5*qdd3i*cos(2*q5i))/8 +
(L7^2*m5*qdd4i*cos(2*q5i))/8 + (L6*g*m4*sin(q1i - q2i - q3i - q4i))/4 +
(L6*g*m5*sin(q1i - q2i - q3i - q4i))/2 + (L5*g*m3*cos(q1i - q2i - q3i))/8
+ (L5*g*m4*cos(q1i - q2i - q3i))/4 + (L5*g*m5*cos(q1i - q2i - q3i))/4 +
(L7^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i - 2*q5i))/32 +
(L7^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i + 2*q5i))/32 - (L7*g*m5*sin(q1i
+ q2i + q3i + q4i + q5i))/8 + (L6^2*m4*qd1i^2*sin(2*q2i + 2*q3i +
2*q4i))/8 + (L6^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i))/2 +
(L7^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i))/16 + (L5*g*m3*cos(q1i + q2i +
q3i))/8 + (L5*g*m4*cos(q1i + q2i + q3i))/4 + (L5*g*m5*cos(q1i + q2i +
q3i))/4 + (L7*g*m5*sin(q1i - q2i - q3i - q4i + q5i))/8 +
I5i*qd1i*qd5i*sin(q2i + q3i + q4i) + Iext*qd1i*qd5i*sin(q2i + q3i + q4i)
+ (L5^2*m3*qd1i^2*sin(2*q2i + 2*q3i))/16 + (L5^2*m4*qd1i^2*sin(2*q2i +
2*q3i))/4 + (L5^2*m5*qd1i^2*sin(2*q2i + 2*q3i))/4 +
(L7^2*m5*qdd1i*cos(q2i + q3i + q4i - 2*q5i))/16 - (L7^2*m5*qdd1i*cos(q2i
+ q3i + q4i + 2*q5i))/16 - (L7*g*m5*sin(q1i + q2i + q3i + q4i - q5i))/8 -
(3^(1/2)*L5*g*m3*sin(q1i + q2i + q3i))/8 - (3^(1/2)*L5*g*m4*sin(q1i + q2i
+ q3i))/4 - (3^(1/2)*L5*g*m5*sin(q1i + q2i + q3i))/4 -
(L5*L6*m4*qd4i^2*cos(q4i))/4 - (L5*L6*m5*qd4i^2*cos(q4i))/2 -
(L5*L6*m4*qd1i^2*cos(2*q2i + 2*q3i + q4i))/4 - (L5*L6*m5*qd1i^2*cos(2*q2i
+ 2*q3i + q4i))/2 - (3^(1/2)*L5^2*m3*qd1i^2*cos(2*q2i + 2*q3i))/16 -
(3^(1/2)*L5^2*m4*qd1i^2*cos(2*q2i + 2*q3i))/4 -
(3^(1/2)*L5^2*m5*qd1i^2*cos(2*q2i + 2*q3i))/4 +
(L4*L5*m3*qd1i^2*sin(q3i))/8 + (L4*L5*m3*qd2i^2*sin(q3i))/4 +
(L4*L5*m4*qd1i^2*sin(q3i))/4 + (L4*L5*m4*qd2i^2*sin(q3i))/2 +
(L4*L5*m5*qd1i^2*sin(q3i))/4 + (L4*L5*m5*qd2i^2*sin(q3i))/2 +
(L4*L7*m5*qd1i^2*cos(2*q2i + q3i + q4i + q5i))/16 +
(L3*L7*m5*qd1i^2*sin(q2i + q3i + q4i - q5i))/4 - (L5*L7*m5*qd4i^2*cos(q4i
- q5i))/8 - (L5*L7*m5*qd5i^2*cos(q4i - q5i))/8 +
(L4*L5*m3*qd1i^2*sin(2*q2i + q3i))/4 + (L4*L5*m4*qd1i^2*sin(2*q2i +
q3i))/2 + (L4*L5*m5*qd1i^2*sin(2*q2i + q3i))/2 +
(L6*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i - q5i))/4 -
(L4*L7*m5*qd1i^2*cos(q3i + q4i + q5i))/16 - (L4*L7*m5*qd2i^2*cos(q3i +
q4i + q5i))/8 + (L7^2*m5*qd1i*qd5i*sin(q2i + q3i + q4i - 2*q5i))/8 +
(L7^2*m5*qd1i*qd5i*sin(q2i + q3i + q4i + 2*q5i))/8 +
(L5*L7*m5*qdd1i*sin(q2i + q3i - q5i))/8 + (L4*L7*m5*qdd2i*sin(q3i + q4i -
q5i))/8 + (L3*L6*m4*qd1i^2*sin(q2i + q3i + q4i))/2 +
L3*L6*m5*qd1i^2*sin(q2i + q3i + q4i) - (L6*L7*m5*qdd1i*cos(q2i + q3i +
q4i + q5i))/4 + (L4*L7*m5*qd1i^2*cos(2*q2i + q3i + q4i - q5i))/16 -
(L5*L7*m5*qd1i^2*cos(2*q2i + 2*q3i + q4i + q5i))/8 +
(L7^2*m5*qd1i*qd5i*sin(q2i + q3i + q4i))/4 + (L4*L6*m4*qdd2i*sin(q3i +
q4i))/4 + (L4*L6*m5*qdd2i*sin(q3i + q4i))/2 - (L5*L7*m5*qdd2i*sin(q4i +
q5i))/4 - (L5*L7*m5*qdd3i*sin(q4i + q5i))/4 - (L5*L7*m5*qdd4i*sin(q4i +
q5i))/8 - (L5*L7*m5*qdd5i*sin(q4i + q5i))/8 -
(L7^2*m5*qd2i*qd5i*sin(2*q5i))/4 - (L7^2*m5*qd3i*qd5i*sin(2*q5i))/4 -
(L7^2*m5*qd4i*qd5i*sin(2*q5i))/4 + (L4*L5*m3*qdd2i*cos(q3i))/4 +
(L4*L5*m4*qdd2i*cos(q3i))/2 + (L4*L5*m5*qdd2i*cos(q3i))/2 +
L6*L7*m5*qdd2i*cos(q5i) + L6*L7*m5*qdd3i*cos(q5i) +
L6*L7*m5*qdd4i*cos(q5i) + (L4*L6*m4*qd1i^2*cos(2*q2i + q3i + q4i))/8 +
(L4*L6*m5*qd1i^2*cos(2*q2i + q3i + q4i))/4 - (L4*L7*m5*qd1i^2*cos(q3i +
q4i - q5i))/16 - (L4*L7*m5*qd2i^2*cos(q3i + q4i - q5i))/8 -
(L5*L6*m4*qdd2i*sin(q4i))/2 - (L5*L6*m4*qdd3i*sin(q4i))/2 -
L5*L6*m5*qdd2i*sin(q4i) - (L5*L6*m4*qdd4i*sin(q4i))/4 -
L5*L6*m5*qdd3i*sin(q4i) - (L5*L6*m5*qdd4i*sin(q4i))/2 +
(L6*L7*m5*qdd1i*cos(q2i + q3i + q4i - q5i))/4 + (3^(1/2)*L5*g*m3*sin(q1i
- q2i - q3i))/8 + (3^(1/2)*L5*g*m4*sin(q1i - q2i - q3i))/4 +

119
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L5*g*m5*sin(q1i - q2i - q3i))/4 + (L3*L7*m5*qd1i^2*sin(q2i + q3i
+ q4i + q5i))/4 - (L3*L5*m3*qd1i^2*cos(q2i + q3i))/4 -
(L3*L5*m4*qd1i^2*cos(q2i + q3i))/2 - (L3*L5*m5*qd1i^2*cos(q2i + q3i))/2 -
(L4*L6*m4*qd1i^2*cos(q3i + q4i))/8 - (L4*L6*m4*qd2i^2*cos(q3i + q4i))/4 -
(L4*L6*m5*qd1i^2*cos(q3i + q4i))/4 - (L4*L6*m5*qd2i^2*cos(q3i + q4i))/2 -
(L5*L7*m5*qd4i^2*cos(q4i + q5i))/8 - (L5*L7*m5*qd5i^2*cos(q4i + q5i))/8 -
(L5*L7*m5*qd1i^2*cos(2*q2i + 2*q3i + q4i - q5i))/8 -
(L5*L7*m5*qdd2i*sin(q4i - q5i))/4 - (L5*L7*m5*qdd3i*sin(q4i - q5i))/4 -
(L5*L7*m5*qdd4i*sin(q4i - q5i))/8 + (L5*L7*m5*qdd5i*sin(q4i - q5i))/8 +
(L6*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i + q5i))/4 -
(L5*L7*m5*qdd1i*sin(q2i + q3i + q5i))/8 + (L4*L7*m5*qdd2i*sin(q3i + q4i +
q5i))/8 + (3^(1/2)*L5*L6*m4*qdd2i*cos(q4i))/2 +
(3^(1/2)*L5*L6*m4*qdd3i*cos(q4i))/2 + 3^(1/2)*L5*L6*m5*qdd2i*cos(q4i) +
(3^(1/2)*L5*L6*m4*qdd4i*cos(q4i))/4 + 3^(1/2)*L5*L6*m5*qdd3i*cos(q4i) +
(3^(1/2)*L5*L6*m5*qdd4i*cos(q4i))/2 + (3^(1/2)*L4*L5*m3*qdd2i*sin(q3i))/4
+ (3^(1/2)*L4*L5*m4*qdd2i*sin(q3i))/2 +
(3^(1/2)*L4*L5*m5*qdd2i*sin(q3i))/2 + (3^(1/2)*L4*L6*m4*qd1i^2*sin(2*q2i
+ q3i + q4i))/8 + (3^(1/2)*L4*L6*m5*qd1i^2*sin(2*q2i + q3i + q4i))/4 +
(3^(1/2)*L4*L7*m5*qd1i^2*sin(q3i + q4i - q5i))/16 +
(3^(1/2)*L4*L7*m5*qd2i^2*sin(q3i + q4i - q5i))/8 -
(L5*L6*m4*qd2i*qd4i*cos(q4i))/2 - (L5*L6*m4*qd3i*qd4i*cos(q4i))/2 -
L5*L6*m5*qd2i*qd4i*cos(q4i) - L5*L6*m5*qd3i*qd4i*cos(q4i) -
L6*L7*m5*qd2i*qd5i*sin(q5i) - L6*L7*m5*qd3i*qd5i*sin(q5i) -
L6*L7*m5*qd4i*qd5i*sin(q5i) + (3^(1/2)*L5*L7*m5*qdd2i*cos(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qdd3i*cos(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qdd4i*cos(q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qdd5i*cos(q4i - q5i))/8 + (L6*L7*m5*qd1i*qd5i*sin(q2i +
q3i + q4i - q5i))/2 + (3^(1/2)*L3*L5*m3*qd1i^2*sin(q2i + q3i))/4 +
(3^(1/2)*L3*L5*m4*qd1i^2*sin(q2i + q3i))/2 +
(3^(1/2)*L3*L5*m5*qd1i^2*sin(q2i + q3i))/2 +
(3^(1/2)*L4*L6*m4*qd1i^2*sin(q3i + q4i))/8 +
(3^(1/2)*L4*L6*m4*qd2i^2*sin(q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qd1i^2*sin(q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qd2i^2*sin(q3i + q4i))/2 -
(3^(1/2)*L5*L7*m5*qd4i^2*sin(q4i + q5i))/8 -
(3^(1/2)*L5*L7*m5*qd5i^2*sin(q4i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qdd1i*cos(q2i + q3i + q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd2i*cos(q3i + q4i + q5i))/8 -
(L5*L7*m5*qd2i*qd4i*cos(q4i - q5i))/4 + (L5*L7*m5*qd2i*qd5i*cos(q4i -
q5i))/4 - (L5*L7*m5*qd3i*qd4i*cos(q4i - q5i))/4 +
(L5*L7*m5*qd3i*qd5i*cos(q4i - q5i))/4 + (L5*L7*m5*qd4i*qd5i*cos(q4i -
q5i))/4 - (L5*L7*m5*qd1i*qd5i*cos(q2i + q3i + q5i))/4 -
(3^(1/2)*L4*L5*m3*qd1i^2*cos(q3i))/8 -
(3^(1/2)*L4*L5*m3*qd2i^2*cos(q3i))/4 -
(3^(1/2)*L4*L5*m4*qd1i^2*cos(q3i))/4 -
(3^(1/2)*L4*L5*m4*qd2i^2*cos(q3i))/2 -
(3^(1/2)*L4*L5*m5*qd1i^2*cos(q3i))/4 -
(3^(1/2)*L4*L5*m5*qd2i^2*cos(q3i))/2 -
(3^(1/2)*L5*L6*m4*qd4i^2*sin(q4i))/4 -
(3^(1/2)*L5*L6*m5*qd4i^2*sin(q4i))/2 + (3^(1/2)*L5*L6*m4*qd1i^2*sin(2*q2i
+ 2*q3i + q4i))/4 + (3^(1/2)*L5*L6*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i))/2
+ (3^(1/2)*L4*L7*m5*qd1i^2*sin(2*q2i + q3i + q4i + q5i))/16 -
(3^(1/2)*L5*L7*m5*qd4i^2*sin(q4i - q5i))/8 -
(3^(1/2)*L5*L7*m5*qd5i^2*sin(q4i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd1i*cos(q2i + q3i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd2i*cos(q3i + q4i - q5i))/8 +

120
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L4*L7*m5*qd1i^2*sin(q3i + q4i + q5i))/16 +
(3^(1/2)*L4*L7*m5*qd2i^2*sin(q3i + q4i + q5i))/8 -
(L5*L7*m5*qd1i*qd5i*cos(q2i + q3i - q5i))/4 +
(3^(1/2)*L4*L6*m4*qdd2i*cos(q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qdd2i*cos(q3i + q4i))/2 +
(3^(1/2)*L5*L7*m5*qdd2i*cos(q4i + q5i))/4 +
(3^(1/2)*L5*L7*m5*qdd3i*cos(q4i + q5i))/4 +
(3^(1/2)*L5*L7*m5*qdd4i*cos(q4i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd5i*cos(q4i + q5i))/8 + (L6*L7*m5*qd1i*qd5i*sin(q2i +
q3i + q4i + q5i))/2 + (3^(1/2)*L4*L7*m5*qd1i^2*sin(2*q2i + q3i + q4i -
q5i))/16 + (3^(1/2)*L5*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i + q5i))/8 -
(L5*L7*m5*qd2i*qd4i*cos(q4i + q5i))/4 - (L5*L7*m5*qd2i*qd5i*cos(q4i +
q5i))/4 - (L5*L7*m5*qd3i*qd4i*cos(q4i + q5i))/4 -
(L5*L7*m5*qd3i*qd5i*cos(q4i + q5i))/4 - (L5*L7*m5*qd4i*qd5i*cos(q4i +
q5i))/4 + (3^(1/2)*L5*L7*m5*qd1i*qd5i*sin(q2i + q3i - q5i))/4 -
(3^(1/2)*L5*L7*m5*qd2i*qd4i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd2i*qd5i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd3i*qd4i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd3i*qd5i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L7*m5*qd4i*qd5i*sin(q4i + q5i))/4 -
(3^(1/2)*L5*L6*m4*qd2i*qd4i*sin(q4i))/2 -
(3^(1/2)*L5*L6*m4*qd3i*qd4i*sin(q4i))/2 -
3^(1/2)*L5*L6*m5*qd2i*qd4i*sin(q4i) - 3^(1/2)*L5*L6*m5*qd3i*qd4i*sin(q4i)
- (3^(1/2)*L5*L7*m5*qd2i*qd4i*sin(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qd2i*qd5i*sin(q4i - q5i))/4 -
(3^(1/2)*L5*L7*m5*qd3i*qd4i*sin(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qd3i*qd5i*sin(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qd4i*qd5i*sin(q4i - q5i))/4 +
(3^(1/2)*L5*L7*m5*qd1i*qd5i*sin(q2i + q3i + q5i))/4 (a.39)

4 = I4i*qdd2i + I4i*qdd3i + I5i*qdd2i + I4i*qdd4i + I5i*qdd3i +


I5i*qdd4i + Iext*qdd2i + Iext*qdd3i + Iext*qdd4i + b4*qd4i +
(L6^2*m4*qdd2i)/4 + (L6^2*m4*qdd3i)/4 + L6^2*m5*qdd2i + (L6^2*m4*qdd4i)/4
+ L6^2*m5*qdd3i + (L7^2*m5*qdd2i)/8 + L6^2*m5*qdd4i + (L7^2*m5*qdd3i)/8 +
(L7^2*m5*qdd4i)/8 + (L7*g*m5*sin(q1i - q2i - q3i - q4i - q5i))/8 -
(L6*g*m4*sin(q1i + q2i + q3i + q4i))/4 - (L6*g*m5*sin(q1i + q2i + q3i +
q4i))/2 + (L7^2*m5*qdd2i*cos(2*q5i))/8 + (L7^2*m5*qdd3i*cos(2*q5i))/8 +
(L7^2*m5*qdd4i*cos(2*q5i))/8 + (L6*g*m4*sin(q1i - q2i - q3i - q4i))/4 +
(L6*g*m5*sin(q1i - q2i - q3i - q4i))/2 + (L7^2*m5*qd1i^2*sin(2*q2i +
2*q3i + 2*q4i - 2*q5i))/32 + (L7^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i +
2*q5i))/32 - (L7*g*m5*sin(q1i + q2i + q3i + q4i + q5i))/8 +
(L6^2*m4*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i))/8 + (L6^2*m5*qd1i^2*sin(2*q2i
+ 2*q3i + 2*q4i))/2 + (L7^2*m5*qd1i^2*sin(2*q2i + 2*q3i + 2*q4i))/16 +
(L7*g*m5*sin(q1i - q2i - q3i - q4i + q5i))/8 + I5i*qd1i*qd5i*sin(q2i +
q3i + q4i) + Iext*qd1i*qd5i*sin(q2i + q3i + q4i) + (L7^2*m5*qdd1i*cos(q2i
+ q3i + q4i - 2*q5i))/16 - (L7^2*m5*qdd1i*cos(q2i + q3i + q4i +
2*q5i))/16 - (L7*g*m5*sin(q1i + q2i + q3i + q4i - q5i))/8 +
(L5*L6*m4*qd1i^2*cos(q4i))/8 + (L5*L6*m4*qd2i^2*cos(q4i))/4 +
(L5*L6*m5*qd1i^2*cos(q4i))/4 + (L5*L6*m4*qd3i^2*cos(q4i))/4 +
(L5*L6*m5*qd2i^2*cos(q4i))/2 + (L5*L6*m5*qd3i^2*cos(q4i))/2 -
(L5*L6*m4*qd1i^2*cos(2*q2i + 2*q3i + q4i))/8 - (L5*L6*m5*qd1i^2*cos(2*q2i
+ 2*q3i + q4i))/4 + (L4*L7*m5*qd1i^2*cos(2*q2i + q3i + q4i + q5i))/16 +
(L3*L7*m5*qd1i^2*sin(q2i + q3i + q4i - q5i))/4 + (L5*L7*m5*qd1i^2*cos(q4i
- q5i))/16 + (L5*L7*m5*qd2i^2*cos(q4i - q5i))/8 +
(L5*L7*m5*qd3i^2*cos(q4i - q5i))/8 + (L6*L7*m5*qd1i^2*sin(2*q2i + 2*q3i +

121
UNIVERSIDAD NACIONAL DE TRUJILLO
2*q4i - q5i))/4 - (L4*L7*m5*qd1i^2*cos(q3i + q4i + q5i))/16 -
(L4*L7*m5*qd2i^2*cos(q3i + q4i + q5i))/8 + (L7^2*m5*qd1i*qd5i*sin(q2i +
q3i + q4i - 2*q5i))/8 + (L7^2*m5*qd1i*qd5i*sin(q2i + q3i + q4i +
2*q5i))/8 + (L4*L7*m5*qdd2i*sin(q3i + q4i - q5i))/8 +
(L3*L6*m4*qd1i^2*sin(q2i + q3i + q4i))/2 + L3*L6*m5*qd1i^2*sin(q2i + q3i
+ q4i) - (L6*L7*m5*qdd1i*cos(q2i + q3i + q4i + q5i))/4 +
(L4*L7*m5*qd1i^2*cos(2*q2i + q3i + q4i - q5i))/16 -
(L5*L7*m5*qd1i^2*cos(2*q2i + 2*q3i + q4i + q5i))/16 +
(L7^2*m5*qd1i*qd5i*sin(q2i + q3i + q4i))/4 + (L4*L6*m4*qdd2i*sin(q3i +
q4i))/4 + (L4*L6*m5*qdd2i*sin(q3i + q4i))/2 - (L5*L7*m5*qdd2i*sin(q4i +
q5i))/8 - (L5*L7*m5*qdd3i*sin(q4i + q5i))/8 -
(L7^2*m5*qd2i*qd5i*sin(2*q5i))/4 - (L7^2*m5*qd3i*qd5i*sin(2*q5i))/4 -
(L7^2*m5*qd4i*qd5i*sin(2*q5i))/4 + L6*L7*m5*qdd2i*cos(q5i) +
L6*L7*m5*qdd3i*cos(q5i) + L6*L7*m5*qdd4i*cos(q5i) +
(L4*L6*m4*qd1i^2*cos(2*q2i + q3i + q4i))/8 + (L4*L6*m5*qd1i^2*cos(2*q2i +
q3i + q4i))/4 - (L4*L7*m5*qd1i^2*cos(q3i + q4i - q5i))/16 -
(L4*L7*m5*qd2i^2*cos(q3i + q4i - q5i))/8 - (L5*L6*m4*qdd2i*sin(q4i))/4 -
(L5*L6*m4*qdd3i*sin(q4i))/4 - (L5*L6*m5*qdd2i*sin(q4i))/2 -
(L5*L6*m5*qdd3i*sin(q4i))/2 + (L6*L7*m5*qdd1i*cos(q2i + q3i + q4i -
q5i))/4 + (L3*L7*m5*qd1i^2*sin(q2i + q3i + q4i + q5i))/4 -
(L4*L6*m4*qd1i^2*cos(q3i + q4i))/8 - (L4*L6*m4*qd2i^2*cos(q3i + q4i))/4 -
(L4*L6*m5*qd1i^2*cos(q3i + q4i))/4 - (L4*L6*m5*qd2i^2*cos(q3i + q4i))/2 +
(L5*L7*m5*qd1i^2*cos(q4i + q5i))/16 + (L5*L7*m5*qd2i^2*cos(q4i + q5i))/8
+ (L5*L7*m5*qd3i^2*cos(q4i + q5i))/8 - (L5*L7*m5*qd1i^2*cos(2*q2i + 2*q3i
+ q4i - q5i))/16 - (L5*L7*m5*qdd2i*sin(q4i - q5i))/8 -
(L5*L7*m5*qdd3i*sin(q4i - q5i))/8 + (L6*L7*m5*qd1i^2*sin(2*q2i + 2*q3i +
2*q4i + q5i))/4 + (L4*L7*m5*qdd2i*sin(q3i + q4i + q5i))/8 +
(3^(1/2)*L5*L6*m4*qdd2i*cos(q4i))/4 + (3^(1/2)*L5*L6*m4*qdd3i*cos(q4i))/4
+ (3^(1/2)*L5*L6*m5*qdd2i*cos(q4i))/2 +
(3^(1/2)*L5*L6*m5*qdd3i*cos(q4i))/2 + (3^(1/2)*L4*L6*m4*qd1i^2*sin(2*q2i
+ q3i + q4i))/8 + (3^(1/2)*L4*L6*m5*qd1i^2*sin(2*q2i + q3i + q4i))/4 +
(3^(1/2)*L4*L7*m5*qd1i^2*sin(q3i + q4i - q5i))/16 +
(3^(1/2)*L4*L7*m5*qd2i^2*sin(q3i + q4i - q5i))/8 +
(L5*L6*m4*qd2i*qd3i*cos(q4i))/2 + L5*L6*m5*qd2i*qd3i*cos(q4i) -
L6*L7*m5*qd2i*qd5i*sin(q5i) - L6*L7*m5*qd3i*qd5i*sin(q5i) -
L6*L7*m5*qd4i*qd5i*sin(q5i) + (3^(1/2)*L5*L7*m5*qdd2i*cos(q4i - q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd3i*cos(q4i - q5i))/8 + (L6*L7*m5*qd1i*qd5i*sin(q2i +
q3i + q4i - q5i))/2 + (3^(1/2)*L4*L6*m4*qd1i^2*sin(q3i + q4i))/8 +
(3^(1/2)*L4*L6*m4*qd2i^2*sin(q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qd1i^2*sin(q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qd2i^2*sin(q3i + q4i))/2 +
(3^(1/2)*L5*L7*m5*qd1i^2*sin(q4i + q5i))/16 +
(3^(1/2)*L5*L7*m5*qd2i^2*sin(q4i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qd3i^2*sin(q4i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i - q5i))/16 +
(3^(1/2)*L4*L7*m5*qdd2i*cos(q3i + q4i + q5i))/8 +
(L5*L7*m5*qd2i*qd3i*cos(q4i - q5i))/4 +
(3^(1/2)*L5*L6*m4*qd1i^2*sin(q4i))/8 +
(3^(1/2)*L5*L6*m4*qd2i^2*sin(q4i))/4 +
(3^(1/2)*L5*L6*m5*qd1i^2*sin(q4i))/4 +
(3^(1/2)*L5*L6*m4*qd3i^2*sin(q4i))/4 +
(3^(1/2)*L5*L6*m5*qd2i^2*sin(q4i))/2 +
(3^(1/2)*L5*L6*m5*qd3i^2*sin(q4i))/2 + (3^(1/2)*L5*L6*m4*qd1i^2*sin(2*q2i
+ 2*q3i + q4i))/8 + (3^(1/2)*L5*L6*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i))/4
+ (3^(1/2)*L4*L7*m5*qd1i^2*sin(2*q2i + q3i + q4i + q5i))/16 +
(3^(1/2)*L5*L7*m5*qd1i^2*sin(q4i - q5i))/16 +
(3^(1/2)*L5*L7*m5*qd2i^2*sin(q4i - q5i))/8 +

122
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L5*L7*m5*qd3i^2*sin(q4i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qdd2i*cos(q3i + q4i - q5i))/8 +
(3^(1/2)*L4*L7*m5*qd1i^2*sin(q3i + q4i + q5i))/16 +
(3^(1/2)*L4*L7*m5*qd2i^2*sin(q3i + q4i + q5i))/8 +
(3^(1/2)*L4*L6*m4*qdd2i*cos(q3i + q4i))/4 +
(3^(1/2)*L4*L6*m5*qdd2i*cos(q3i + q4i))/2 +
(3^(1/2)*L5*L7*m5*qdd2i*cos(q4i + q5i))/8 +
(3^(1/2)*L5*L7*m5*qdd3i*cos(q4i + q5i))/8 + (L6*L7*m5*qd1i*qd5i*sin(q2i +
q3i + q4i + q5i))/2 + (3^(1/2)*L4*L7*m5*qd1i^2*sin(2*q2i + q3i + q4i -
q5i))/16 + (3^(1/2)*L5*L7*m5*qd1i^2*sin(2*q2i + 2*q3i + q4i + q5i))/16 +
(L5*L7*m5*qd2i*qd3i*cos(q4i + q5i))/4 +
(3^(1/2)*L5*L7*m5*qd2i*qd3i*sin(q4i + q5i))/4 +
(3^(1/2)*L5*L6*m4*qd2i*qd3i*sin(q4i))/2 +
3^(1/2)*L5*L6*m5*qd2i*qd3i*sin(q4i) + (3^(1/2)*L5*L7*m5*qd2i*qd3i*sin(q4i
- q5i))/4 (a.40)

5 = b5*qd5i - I5i*(qd1i*qd2i*sin(q2i + q3i + q4i) - qdd1i*cos(q2i +


q3i + q4i) - qdd5i + qd1i*qd3i*sin(q2i + q3i + q4i) + qd1i*qd4i*sin(q2i +
q3i + q4i)) - Iext*(qd1i*qd2i*sin(q2i + q3i + q4i) - qdd1i*cos(q2i + q3i
+ q4i) - qdd5i + qd1i*qd3i*sin(q2i + q3i + q4i) + qd1i*qd4i*sin(q2i + q3i
+ q4i)) - (L7*m5*(cos(q5i)*(g*sin(q1i) - L3*qdd1i + (L4*qdd1i*sin(q2i))/2
- L6*qdd1i*cos(q2i + q3i + q4i) - (L5*qdd1i*sin(q2i + q3i))/2 -
(3^(1/2)*L5*qdd1i*cos(q2i + q3i))/2 - L5*qd1i*qd2i*cos(q2i + q3i) -
L5*qd1i*qd3i*cos(q2i + q3i) - (3^(1/2)*L4*qdd1i*cos(q2i))/2 +
L4*qd1i*qd2i*cos(q2i) + 2*L6*qd1i*qd2i*sin(q2i + q3i + q4i) +
2*L6*qd1i*qd3i*sin(q2i + q3i + q4i) + 2*L6*qd1i*qd4i*sin(q2i + q3i + q4i)
+ 3^(1/2)*L5*qd1i*qd2i*sin(q2i + q3i) + 3^(1/2)*L5*qd1i*qd3i*sin(q2i +
q3i) + 3^(1/2)*L4*qd1i*qd2i*sin(q2i)) + (L7*(qd1i*qd2i*sin(q2i + q3i +
q4i) - qdd1i*cos(q2i + q3i + q4i) - qdd5i + qd1i*qd3i*sin(q2i + q3i +
q4i) + qd1i*qd4i*sin(q2i + q3i + q4i)))/2 - sin(q5i)*((L6*qd1i^2)/2 -
(g*cos(q1i - q2i - q3i - q4i))/2 - (g*cos(q1i + q2i + q3i + q4i))/2 +
L6*qd2i^2 + L6*qd3i^2 + L6*qd4i^2 - (L4*qd1i^2*sin(2*q2i + q3i + q4i))/4
+ (L4*qd1i^2*sin(q3i + q4i))/4 + (L4*qd2i^2*sin(q3i + q4i))/2 -
(L5*qd1i^2*sin(q4i))/4 - (L5*qd2i^2*sin(q4i))/2 - (L5*qd3i^2*sin(q4i))/2
+ (L5*qd1i^2*sin(2*q2i + 2*q3i + q4i))/4 + 2*L6*qd2i*qd3i +
2*L6*qd2i*qd4i + 2*L6*qd3i*qd4i + L3*qd1i^2*cos(q2i + q3i + q4i) +
(L6*qd1i^2*cos(2*q2i + 2*q3i + 2*q4i))/2 + (L4*qdd2i*cos(q3i + q4i))/2 -
(L5*qdd2i*cos(q4i))/2 - (L5*qdd3i*cos(q4i))/2 - (3^(1/2)*L4*qdd2i*sin(q3i
+ q4i))/2 + (3^(1/2)*L4*qd1i^2*cos(2*q2i + q3i + q4i))/4 -
(3^(1/2)*L5*qdd2i*sin(q4i))/2 - (3^(1/2)*L5*qdd3i*sin(q4i))/2 -
L5*qd2i*qd3i*sin(q4i) + (3^(1/2)*L4*qd1i^2*cos(q3i + q4i))/4 +
(3^(1/2)*L4*qd2i^2*cos(q3i + q4i))/2 + (3^(1/2)*L5*qd1i^2*cos(q4i))/4 +
(3^(1/2)*L5*qd2i^2*cos(q4i))/2 + (3^(1/2)*L5*qd3i^2*cos(q4i))/2 +
(3^(1/2)*L5*qd1i^2*cos(2*q2i + 2*q3i + q4i))/4 +
3^(1/2)*L5*qd2i*qd3i*cos(q4i)) - (L7*(qd2i*cos(q5i) - (qd1i*cos(q2i + q3i
+ q4i + q5i))/2 + qd3i*cos(q5i) + qd4i*cos(q5i) + (qd1i*cos(q2i + q3i +
q4i - q5i))/2)*(qd2i*sin(q5i) - (qd1i*sin(q2i + q3i + q4i + q5i))/2 +
qd3i*sin(q5i) + qd4i*sin(q5i) - (qd1i*sin(q2i + q3i + q4i -
q5i))/2))/2))/2
(a.41)

123
UNIVERSIDAD NACIONAL DE TRUJILLO
A.5 Resultados de la dinmica de la pierna de apoyo.
Se muestran las ecuaciones obtenidas de las torques para cada articulacin.

1 = I1d*qdd5d + I2d*qdd5d + I3d*qdd5d + I4d*qdd5d + I5d*qdd5d +


Iext*qdd5d + b1*qd5d + I5d*qdd1d*cos(q2d + q3d + q4d) +
Iext*qdd1d*cos(q2d + q3d + q4d) + (L3^2*Mext*qdd5d)/2 +
(L4^2*Mext*qdd5d)/2 + (L5^2*Mext*qdd5d)/2 + L6^2*Mext*qdd5d +
(L3^2*m4*qdd5d)/8 + (L4^2*m3*qdd5d)/8 + (L5^2*m2*qdd5d)/8 +
(L6^2*m1*qdd5d)/4 + (L3^2*m5*qdd5d)/2 + (L4^2*m4*qdd5d)/2 +
(L5^2*m3*qdd5d)/2 + L6^2*m2*qdd5d + (L4^2*m5*qdd5d)/2 + (L5^2*m4*qdd5d)/2
+ L6^2*m3*qdd5d + (L5^2*m5*qdd5d)/2 + L6^2*m4*qdd5d + L6^2*m5*qdd5d -
(L5*Mext*g*cos(q4d + q5d))/4 + (L3*g*m4*sin(q2d + q3d + q4d + q5d))/4 +
(L3*g*m5*sin(q2d + q3d + q4d + q5d))/2 + (L5^2*m2*qdd5d*cos(2*q4d))/16 +
(L5^2*m3*qdd5d*cos(2*q4d))/4 + (L5^2*m4*qdd5d*cos(2*q4d))/4 +
(L5^2*m5*qdd5d*cos(2*q4d))/4 + (L3^2*m4*qdd5d*cos(2*q2d + 2*q3d +
2*q4d))/8 + (L3^2*m5*qdd5d*cos(2*q2d + 2*q3d + 2*q4d))/2 -
(L5*g*m2*cos(q4d + q5d))/8 - (L5*g*m3*cos(q4d + q5d))/4 -
(L5*g*m4*cos(q4d + q5d))/4 - (L5*g*m5*cos(q4d + q5d))/4 +
(L4^2*Mext*qdd5d*cos(2*q3d + 2*q4d))/4 + L6*Mext*g*sin(q5d) +
(L4^2*m3*qdd5d*cos(2*q3d + 2*q4d))/16 + (L4^2*m4*qdd5d*cos(2*q3d +
2*q4d))/4 + (L4^2*m5*qdd5d*cos(2*q3d + 2*q4d))/4 - (L3*Mext*g*sin(q2d +
q3d + q4d - q5d))/2 + (L6*g*m1*sin(q5d))/2 + L6*g*m2*sin(q5d) +
L6*g*m3*sin(q5d) + L6*g*m4*sin(q5d) + L6*g*m5*sin(q5d) +
(L5*Mext*g*cos(q4d - q5d))/4 + (L4*Mext*g*cos(q3d + q4d + q5d))/4 -
(L3*g*m4*sin(q2d + q3d + q4d - q5d))/4 - (L3*g*m5*sin(q2d + q3d + q4d -
q5d))/2 + (L5*g*m2*cos(q4d - q5d))/8 + (L5*g*m3*cos(q4d - q5d))/4 +
(L5*g*m4*cos(q4d - q5d))/4 + (L5*g*m5*cos(q4d - q5d))/4 +
(L4*g*m3*cos(q3d + q4d + q5d))/8 + (L4*g*m4*cos(q3d + q4d + q5d))/4 +
(L4*g*m5*cos(q3d + q4d + q5d))/4 - I5d*qd1d*qd2d*sin(q2d + q3d + q4d) -
I5d*qd1d*qd3d*sin(q2d + q3d + q4d) - I5d*qd1d*qd4d*sin(q2d + q3d + q4d) -
Iext*qd1d*qd2d*sin(q2d + q3d + q4d) - Iext*qd1d*qd3d*sin(q2d + q3d + q4d)
- Iext*qd1d*qd4d*sin(q2d + q3d + q4d) - (L4*Mext*g*cos(q3d + q4d -
q5d))/4 - (L4*g*m3*cos(q3d + q4d - q5d))/8 - (L4*g*m4*cos(q3d + q4d -
q5d))/4 - (L4*g*m5*cos(q3d + q4d - q5d))/4 + (L3*Mext*g*sin(q2d + q3d +
q4d + q5d))/2 + (L5^2*Mext*qdd5d*cos(2*q4d))/4 +
(L3^2*Mext*qdd5d*cos(2*q2d + 2*q3d + 2*q4d))/2 + (3^(1/2)*L4*g*m3*sin(q3d
+ q4d + q5d))/8 + (3^(1/2)*L4*g*m4*sin(q3d + q4d + q5d))/4 +
(3^(1/2)*L4*g*m5*sin(q3d + q4d + q5d))/4 + (L3*L5*Mext*qdd5d*sin(q2d +
q3d + 2*q4d))/2 - (3^(1/2)*L4*Mext*g*sin(q3d + q4d - q5d))/4 +
(L3*L5*m4*qdd5d*sin(q2d + q3d + 2*q4d))/4 + (L3*L5*m5*qdd5d*sin(q2d + q3d
+ 2*q4d))/2 - (3^(1/2)*L4*g*m3*sin(q3d + q4d - q5d))/8 -
(3^(1/2)*L4*g*m4*sin(q3d + q4d - q5d))/4 - (3^(1/2)*L4*g*m5*sin(q3d + q4d
- q5d))/4 - (L3*L5*Mext*qdd5d*sin(q2d + q3d))/2 -
L4*L6*Mext*qdd5d*sin(q3d + q4d) + (3^(1/2)*L5^2*Mext*qdd5d*sin(2*q4d))/4
+ (3^(1/2)*L5*Mext*g*sin(q4d + q5d))/4 -
(L5^2*Mext*qd4d*qd5d*sin(2*q4d))/2 - L3^2*Mext*qd2d*qd5d*sin(2*q2d +
2*q3d + 2*q4d) - L3^2*Mext*qd3d*qd5d*sin(2*q2d + 2*q3d + 2*q4d) -
L3^2*Mext*qd4d*qd5d*sin(2*q2d + 2*q3d + 2*q4d) +
(L4*L5*Mext*qdd5d*cos(q3d))/2 - (L3*L5*m4*qdd5d*sin(q2d + q3d))/4 -
(L3*L5*m5*qdd5d*sin(q2d + q3d))/2 - (L4*L6*m3*qdd5d*sin(q3d + q4d))/2 -
L4*L6*m4*qdd5d*sin(q3d + q4d) - L4*L6*m5*qdd5d*sin(q3d + q4d) +
(3^(1/2)*L5^2*m2*qdd5d*sin(2*q4d))/16 +
(3^(1/2)*L5^2*m3*qdd5d*sin(2*q4d))/4 +
(3^(1/2)*L5^2*m4*qdd5d*sin(2*q4d))/4 +

124
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L5^2*m5*qdd5d*sin(2*q4d))/4 + (3^(1/2)*L5*g*m2*sin(q4d + q5d))/8
+ (3^(1/2)*L5*g*m3*sin(q4d + q5d))/4 + (3^(1/2)*L5*g*m4*sin(q4d + q5d))/4
+ (3^(1/2)*L5*g*m5*sin(q4d + q5d))/4 + (L3*L4*Mext*qdd5d*sin(q2d))/2 +
L5*L6*Mext*qdd5d*sin(q4d) - (L3*L4*Mext*qdd5d*sin(q2d + 2*q3d + 2*q4d))/2
- (3^(1/2)*L4^2*Mext*qdd5d*sin(2*q3d + 2*q4d))/4 -
(L5^2*m2*qd4d*qd5d*sin(2*q4d))/8 - (L5^2*m3*qd4d*qd5d*sin(2*q4d))/2 -
(L5^2*m4*qd4d*qd5d*sin(2*q4d))/2 - (L5^2*m5*qd4d*qd5d*sin(2*q4d))/2 -
(L3^2*m4*qd2d*qd5d*sin(2*q2d + 2*q3d + 2*q4d))/4 -
(L3^2*m4*qd3d*qd5d*sin(2*q2d + 2*q3d + 2*q4d))/4 -
L3^2*m5*qd2d*qd5d*sin(2*q2d + 2*q3d + 2*q4d) -
(L3^2*m4*qd4d*qd5d*sin(2*q2d + 2*q3d + 2*q4d))/4 -
L3^2*m5*qd3d*qd5d*sin(2*q2d + 2*q3d + 2*q4d) -
L3^2*m5*qd4d*qd5d*sin(2*q2d + 2*q3d + 2*q4d) +
(L4*L5*m3*qdd5d*cos(q3d))/4 + (L4*L5*m4*qdd5d*cos(q3d))/2 +
(L4*L5*m5*qdd5d*cos(q3d))/2 - (L4^2*Mext*qd3d*qd5d*sin(2*q3d + 2*q4d))/2
- (L4^2*Mext*qd4d*qd5d*sin(2*q3d + 2*q4d))/2 +
(L3*L4*m4*qdd5d*sin(q2d))/4 + (L3*L4*m5*qdd5d*sin(q2d))/2 +
(L5*L6*m2*qdd5d*sin(q4d))/2 + L5*L6*m3*qdd5d*sin(q4d) +
L5*L6*m4*qdd5d*sin(q4d) + L5*L6*m5*qdd5d*sin(q4d) +
L4*L5*Mext*qdd5d*cos(q3d + 2*q4d) - (L3*L4*m4*qdd5d*sin(q2d + 2*q3d +
2*q4d))/4 - (L3*L4*m5*qdd5d*sin(q2d + 2*q3d + 2*q4d))/2 -
(3^(1/2)*L4^2*m3*qdd5d*sin(2*q3d + 2*q4d))/16 -
(3^(1/2)*L4^2*m4*qdd5d*sin(2*q3d + 2*q4d))/4 -
(3^(1/2)*L4^2*m5*qdd5d*sin(2*q3d + 2*q4d))/4 + 2*L3*L6*Mext*qdd5d*cos(q2d
+ q3d + q4d) - (L4^2*m3*qd3d*qd5d*sin(2*q3d + 2*q4d))/8 -
(L4^2*m3*qd4d*qd5d*sin(2*q3d + 2*q4d))/8 - (L4^2*m4*qd3d*qd5d*sin(2*q3d +
2*q4d))/2 - (L4^2*m4*qd4d*qd5d*sin(2*q3d + 2*q4d))/2 -
(L4^2*m5*qd3d*qd5d*sin(2*q3d + 2*q4d))/2 - (L4^2*m5*qd4d*qd5d*sin(2*q3d +
2*q4d))/2 - (3^(1/2)*L5*Mext*g*sin(q4d - q5d))/4 +
(L4*L5*m3*qdd5d*cos(q3d + 2*q4d))/2 + L4*L5*m4*qdd5d*cos(q3d + 2*q4d) +
L4*L5*m5*qdd5d*cos(q3d + 2*q4d) + (3^(1/2)*L4*Mext*g*sin(q3d + q4d +
q5d))/4 + L3*L6*m4*qdd5d*cos(q2d + q3d + q4d) + 2*L3*L6*m5*qdd5d*cos(q2d
+ q3d + q4d) - (3^(1/2)*L5*g*m2*sin(q4d - q5d))/8 -
(3^(1/2)*L5*g*m3*sin(q4d - q5d))/4 - (3^(1/2)*L5*g*m4*sin(q4d - q5d))/4 -
(3^(1/2)*L5*g*m5*sin(q4d - q5d))/4 + (3^(1/2)*L3*L4*m4*qdd5d*cos(q2d))/4
+ (3^(1/2)*L3*L4*m5*qdd5d*cos(q2d))/2 +
(3^(1/2)*L5*L6*m2*qdd5d*cos(q4d))/2 + 3^(1/2)*L5*L6*m3*qdd5d*cos(q4d) +
3^(1/2)*L5*L6*m4*qdd5d*cos(q4d) + 3^(1/2)*L5*L6*m5*qdd5d*cos(q4d) +
(3^(1/2)*L3*L4*m4*qdd5d*cos(q2d + 2*q3d + 2*q4d))/4 +
(3^(1/2)*L3*L4*m5*qdd5d*cos(q2d + 2*q3d + 2*q4d))/2 -
(L4*L5*Mext*qd3d*qd5d*sin(q3d))/2 - (3^(1/2)*L4*L5*m3*qdd5d*sin(q3d))/4 -
(3^(1/2)*L4*L5*m4*qdd5d*sin(q3d))/2 - (3^(1/2)*L4*L5*m5*qdd5d*sin(q3d))/2
+ (L3*L4*m4*qd2d*qd5d*cos(q2d))/4 + (L3*L4*m5*qd2d*qd5d*cos(q2d))/2 +
(L5*L6*m2*qd4d*qd5d*cos(q4d))/2 + L5*L6*m3*qd4d*qd5d*cos(q4d) +
L5*L6*m4*qd4d*qd5d*cos(q4d) + L5*L6*m5*qd4d*qd5d*cos(q4d) -
(L3*L4*m4*qd2d*qd5d*cos(q2d + 2*q3d + 2*q4d))/4 -
(L3*L4*m4*qd3d*qd5d*cos(q2d + 2*q3d + 2*q4d))/2 -
(L3*L4*m5*qd2d*qd5d*cos(q2d + 2*q3d + 2*q4d))/2 -
(L3*L4*m4*qd4d*qd5d*cos(q2d + 2*q3d + 2*q4d))/2 -
L3*L4*m5*qd3d*qd5d*cos(q2d + 2*q3d + 2*q4d) - L3*L4*m5*qd4d*qd5d*cos(q2d
+ 2*q3d + 2*q4d) - (3^(1/2)*L4^2*m3*qd3d*qd5d*cos(2*q3d + 2*q4d))/8 -
(3^(1/2)*L4^2*m3*qd4d*qd5d*cos(2*q3d + 2*q4d))/8 -
(3^(1/2)*L4^2*m4*qd3d*qd5d*cos(2*q3d + 2*q4d))/2 -
(3^(1/2)*L4^2*m4*qd4d*qd5d*cos(2*q3d + 2*q4d))/2 -
(3^(1/2)*L4^2*m5*qd3d*qd5d*cos(2*q3d + 2*q4d))/2 -
(3^(1/2)*L4^2*m5*qd4d*qd5d*cos(2*q3d + 2*q4d))/2 -
(L4*L5*m3*qd3d*qd5d*sin(q3d))/4 - (L4*L5*m4*qd3d*qd5d*sin(q3d))/2 -

125
UNIVERSIDAD NACIONAL DE TRUJILLO
(L4*L5*m5*qd3d*qd5d*sin(q3d))/2 - L4*L5*Mext*qd3d*qd5d*sin(q3d + 2*q4d) -
2*L4*L5*Mext*qd4d*qd5d*sin(q3d + 2*q4d) - 2*L3*L6*Mext*qd2d*qd5d*sin(q2d
+ q3d + q4d) - 2*L3*L6*Mext*qd3d*qd5d*sin(q2d + q3d + q4d) -
2*L3*L6*Mext*qd4d*qd5d*sin(q2d + q3d + q4d) - (L4*L5*m3*qd3d*qd5d*sin(q3d
+ 2*q4d))/2 - L4*L5*m3*qd4d*qd5d*sin(q3d + 2*q4d) -
L4*L5*m4*qd3d*qd5d*sin(q3d + 2*q4d) - 2*L4*L5*m4*qd4d*qd5d*sin(q3d +
2*q4d) - L4*L5*m5*qd3d*qd5d*sin(q3d + 2*q4d) -
2*L4*L5*m5*qd4d*qd5d*sin(q3d + 2*q4d) - L3*L6*m4*qd2d*qd5d*sin(q2d + q3d
+ q4d) - L3*L6*m4*qd3d*qd5d*sin(q2d + q3d + q4d) -
2*L3*L6*m5*qd2d*qd5d*sin(q2d + q3d + q4d) - L3*L6*m4*qd4d*qd5d*sin(q2d +
q3d + q4d) - 2*L3*L6*m5*qd3d*qd5d*sin(q2d + q3d + q4d) -
2*L3*L6*m5*qd4d*qd5d*sin(q2d + q3d + q4d) +
(3^(1/2)*L3*L5*Mext*qdd5d*cos(q2d + q3d + 2*q4d))/2 +
(L3*L5*Mext*qd2d*qd5d*cos(q2d + q3d + 2*q4d))/2 +
(L3*L5*Mext*qd3d*qd5d*cos(q2d + q3d + 2*q4d))/2 +
L3*L5*Mext*qd4d*qd5d*cos(q2d + q3d + 2*q4d) +
(3^(1/2)*L3*L5*m4*qdd5d*cos(q2d + q3d + 2*q4d))/4 +
(3^(1/2)*L3*L5*m5*qdd5d*cos(q2d + q3d + 2*q4d))/2 +
(3^(1/2)*L3*L5*Mext*qdd5d*cos(q2d + q3d))/2 +
3^(1/2)*L4*L6*Mext*qdd5d*cos(q3d + q4d) + (L3*L5*m4*qd2d*qd5d*cos(q2d +
q3d + 2*q4d))/4 + (L3*L5*m4*qd3d*qd5d*cos(q2d + q3d + 2*q4d))/4 +
(L3*L5*m5*qd2d*qd5d*cos(q2d + q3d + 2*q4d))/2 +
(L3*L5*m4*qd4d*qd5d*cos(q2d + q3d + 2*q4d))/2 +
(L3*L5*m5*qd3d*qd5d*cos(q2d + q3d + 2*q4d))/2 +
L3*L5*m5*qd4d*qd5d*cos(q2d + q3d + 2*q4d) - (L3*L5*Mext*qd2d*qd5d*cos(q2d
+ q3d))/2 - (L3*L5*Mext*qd3d*qd5d*cos(q2d + q3d))/2 -
L4*L6*Mext*qd3d*qd5d*cos(q3d + q4d) - L4*L6*Mext*qd4d*qd5d*cos(q3d + q4d)
+ (3^(1/2)*L5^2*Mext*qd4d*qd5d*cos(2*q4d))/2 +
(3^(1/2)*L3*L5*m4*qdd5d*cos(q2d + q3d))/4 +
(3^(1/2)*L3*L5*m5*qdd5d*cos(q2d + q3d))/2 +
(3^(1/2)*L4*L6*m3*qdd5d*cos(q3d + q4d))/2 +
3^(1/2)*L4*L6*m4*qdd5d*cos(q3d + q4d) + 3^(1/2)*L4*L6*m5*qdd5d*cos(q3d +
q4d) + (3^(1/2)*L3*L4*Mext*qdd5d*cos(q2d))/2 +
3^(1/2)*L5*L6*Mext*qdd5d*cos(q4d) + (3^(1/2)*L3*L4*Mext*qdd5d*cos(q2d +
2*q3d + 2*q4d))/2 - (L3*L5*m4*qd2d*qd5d*cos(q2d + q3d))/4 -
(L3*L5*m4*qd3d*qd5d*cos(q2d + q3d))/4 - (L3*L5*m5*qd2d*qd5d*cos(q2d +
q3d))/2 - (L3*L5*m5*qd3d*qd5d*cos(q2d + q3d))/2 -
(L4*L6*m3*qd3d*qd5d*cos(q3d + q4d))/2 - (L4*L6*m3*qd4d*qd5d*cos(q3d +
q4d))/2 - L4*L6*m4*qd3d*qd5d*cos(q3d + q4d) - L4*L6*m4*qd4d*qd5d*cos(q3d
+ q4d) - L4*L6*m5*qd3d*qd5d*cos(q3d + q4d) - L4*L6*m5*qd4d*qd5d*cos(q3d +
q4d) + (3^(1/2)*L5^2*m2*qd4d*qd5d*cos(2*q4d))/8 +
(3^(1/2)*L5^2*m3*qd4d*qd5d*cos(2*q4d))/2 +
(3^(1/2)*L5^2*m4*qd4d*qd5d*cos(2*q4d))/2 +
(3^(1/2)*L5^2*m5*qd4d*qd5d*cos(2*q4d))/2 -
(3^(1/2)*L4*L5*Mext*qdd5d*sin(q3d))/2 + (L3*L4*Mext*qd2d*qd5d*cos(q2d))/2
+ L5*L6*Mext*qd4d*qd5d*cos(q4d) - (L3*L4*Mext*qd2d*qd5d*cos(q2d + 2*q3d +
2*q4d))/2 - L3*L4*Mext*qd3d*qd5d*cos(q2d + 2*q3d + 2*q4d) -
L3*L4*Mext*qd4d*qd5d*cos(q2d + 2*q3d + 2*q4d) -
(3^(1/2)*L4^2*Mext*qd3d*qd5d*cos(2*q3d + 2*q4d))/2 -
(3^(1/2)*L4^2*Mext*qd4d*qd5d*cos(2*q3d + 2*q4d))/2 -
(3^(1/2)*L3*L5*Mext*qd2d*qd5d*sin(q2d + q3d + 2*q4d))/2 -
(3^(1/2)*L3*L5*Mext*qd3d*qd5d*sin(q2d + q3d + 2*q4d))/2 -
3^(1/2)*L3*L5*Mext*qd4d*qd5d*sin(q2d + q3d + 2*q4d) -
(3^(1/2)*L3*L5*m4*qd2d*qd5d*sin(q2d + q3d + 2*q4d))/4 -
(3^(1/2)*L3*L5*m4*qd3d*qd5d*sin(q2d + q3d + 2*q4d))/4 -
(3^(1/2)*L3*L5*m5*qd2d*qd5d*sin(q2d + q3d + 2*q4d))/2 -
(3^(1/2)*L3*L5*m4*qd4d*qd5d*sin(q2d + q3d + 2*q4d))/2 -

126
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L3*L5*m5*qd3d*qd5d*sin(q2d + q3d + 2*q4d))/2 -
3^(1/2)*L3*L5*m5*qd4d*qd5d*sin(q2d + q3d + 2*q4d) -
(3^(1/2)*L3*L5*Mext*qd2d*qd5d*sin(q2d + q3d))/2 -
(3^(1/2)*L3*L5*Mext*qd3d*qd5d*sin(q2d + q3d))/2 -
3^(1/2)*L4*L6*Mext*qd3d*qd5d*sin(q3d + q4d) -
3^(1/2)*L4*L6*Mext*qd4d*qd5d*sin(q3d + q4d) -
(3^(1/2)*L4*L5*Mext*qd3d*qd5d*cos(q3d))/2 -
(3^(1/2)*L3*L5*m4*qd2d*qd5d*sin(q2d + q3d))/4 -
(3^(1/2)*L3*L5*m4*qd3d*qd5d*sin(q2d + q3d))/4 -
(3^(1/2)*L3*L5*m5*qd2d*qd5d*sin(q2d + q3d))/2 -
(3^(1/2)*L3*L5*m5*qd3d*qd5d*sin(q2d + q3d))/2 -
(3^(1/2)*L4*L6*m3*qd3d*qd5d*sin(q3d + q4d))/2 -
(3^(1/2)*L4*L6*m3*qd4d*qd5d*sin(q3d + q4d))/2 -
3^(1/2)*L4*L6*m4*qd3d*qd5d*sin(q3d + q4d) -
3^(1/2)*L4*L6*m4*qd4d*qd5d*sin(q3d + q4d) -
3^(1/2)*L4*L6*m5*qd3d*qd5d*sin(q3d + q4d) -
3^(1/2)*L4*L6*m5*qd4d*qd5d*sin(q3d + q4d) -
(3^(1/2)*L3*L4*Mext*qd2d*qd5d*sin(q2d))/2 -
3^(1/2)*L5*L6*Mext*qd4d*qd5d*sin(q4d) -
(3^(1/2)*L3*L4*Mext*qd2d*qd5d*sin(q2d + 2*q3d + 2*q4d))/2 -
3^(1/2)*L3*L4*Mext*qd3d*qd5d*sin(q2d + 2*q3d + 2*q4d) -
3^(1/2)*L3*L4*Mext*qd4d*qd5d*sin(q2d + 2*q3d + 2*q4d) -
(3^(1/2)*L4*L5*m3*qd3d*qd5d*cos(q3d))/4 -
(3^(1/2)*L4*L5*m4*qd3d*qd5d*cos(q3d))/2 -
(3^(1/2)*L4*L5*m5*qd3d*qd5d*cos(q3d))/2 -
(3^(1/2)*L3*L4*m4*qd2d*qd5d*sin(q2d))/4 -
(3^(1/2)*L3*L4*m5*qd2d*qd5d*sin(q2d))/2 -
(3^(1/2)*L5*L6*m2*qd4d*qd5d*sin(q4d))/2 -
3^(1/2)*L5*L6*m3*qd4d*qd5d*sin(q4d) - 3^(1/2)*L5*L6*m4*qd4d*qd5d*sin(q4d)
- 3^(1/2)*L5*L6*m5*qd4d*qd5d*sin(q4d) -
(3^(1/2)*L3*L4*m4*qd2d*qd5d*sin(q2d + 2*q3d + 2*q4d))/4 -
(3^(1/2)*L3*L4*m4*qd3d*qd5d*sin(q2d + 2*q3d + 2*q4d))/2 -
(3^(1/2)*L3*L4*m5*qd2d*qd5d*sin(q2d + 2*q3d + 2*q4d))/2 -
(3^(1/2)*L3*L4*m4*qd4d*qd5d*sin(q2d + 2*q3d + 2*q4d))/2 -
3^(1/2)*L3*L4*m5*qd3d*qd5d*sin(q2d + 2*q3d + 2*q4d) -
3^(1/2)*L3*L4*m5*qd4d*qd5d*sin(q2d + 2*q3d + 2*q4d) (a.42)

2 = I2d*qdd4d + I3d*qdd3d + I4d*qdd2d + I3d*qdd4d + I4d*qdd3d +


I5d*qdd2d + I4d*qdd4d + I5d*qdd3d + I5d*qdd4d + Iext*qdd2d + Iext*qdd3d +
Iext*qdd4d + b2*qd4d + L3^2*Mext*qdd2d + L3^2*Mext*qdd3d +
L3^2*Mext*qdd4d + L4^2*Mext*qdd3d + L4^2*Mext*qdd4d + L5^2*Mext*qdd4d +
(L3^2*m4*qdd2d)/4 + (L3^2*m4*qdd3d)/4 + L3^2*m5*qdd2d + (L4^2*m3*qdd3d)/4
+ (L3^2*m4*qdd4d)/4 + L3^2*m5*qdd3d + (L4^2*m3*qdd4d)/4 + L4^2*m4*qdd3d +
(L5^2*m2*qdd4d)/4 + L3^2*m5*qdd4d + L4^2*m4*qdd4d + L4^2*m5*qdd3d +
L5^2*m3*qdd4d + L4^2*m5*qdd4d + L5^2*m4*qdd4d + L5^2*m5*qdd4d -
(L5*Mext*g*cos(q4d + q5d))/4 + (L3*g*m4*sin(q2d + q3d + q4d + q5d))/4 +
(L3*g*m5*sin(q2d + q3d + q4d + q5d))/2 - (L5*g*m2*cos(q4d + q5d))/8 -
(L5*g*m3*cos(q4d + q5d))/4 - (L5*g*m4*cos(q4d + q5d))/4 -
(L5*g*m5*cos(q4d + q5d))/4 + (L3*Mext*g*sin(q2d + q3d + q4d - q5d))/2 -
(L5*Mext*g*cos(q4d - q5d))/4 + (L5^2*Mext*qd5d^2*sin(2*q4d))/4 +
(L3^2*Mext*qd5d^2*sin(2*q2d + 2*q3d + 2*q4d))/2 + (L4*Mext*g*cos(q3d +
q4d + q5d))/4 + (L3*g*m4*sin(q2d + q3d + q4d - q5d))/4 + (L3*g*m5*sin(q2d
+ q3d + q4d - q5d))/2 - (L5*g*m2*cos(q4d - q5d))/8 - (L5*g*m3*cos(q4d -
q5d))/4 - (L5*g*m4*cos(q4d - q5d))/4 - (L5*g*m5*cos(q4d - q5d))/4 +
(L5^2*m2*qd5d^2*sin(2*q4d))/16 + (L5^2*m3*qd5d^2*sin(2*q4d))/4 +

127
UNIVERSIDAD NACIONAL DE TRUJILLO
(L5^2*m4*qd5d^2*sin(2*q4d))/4 + (L5^2*m5*qd5d^2*sin(2*q4d))/4 +
(L3^2*m4*qd5d^2*sin(2*q2d + 2*q3d + 2*q4d))/8 + (L3^2*m5*qd5d^2*sin(2*q2d
+ 2*q3d + 2*q4d))/2 + (L4*g*m3*cos(q3d + q4d + q5d))/8 + (L4*g*m4*cos(q3d
+ q4d + q5d))/4 + (L4*g*m5*cos(q3d + q4d + q5d))/4 +
(L4^2*Mext*qd5d^2*sin(2*q3d + 2*q4d))/4 + I5d*qd1d*qd5d*sin(q2d + q3d +
q4d) + Iext*qd1d*qd5d*sin(q2d + q3d + q4d) + (L4^2*m3*qd5d^2*sin(2*q3d +
2*q4d))/16 + (L4^2*m4*qd5d^2*sin(2*q3d + 2*q4d))/4 +
(L4^2*m5*qd5d^2*sin(2*q3d + 2*q4d))/4 + (L4*Mext*g*cos(q3d + q4d -
q5d))/4 + (L4*g*m3*cos(q3d + q4d - q5d))/8 + (L4*g*m4*cos(q3d + q4d -
q5d))/4 + (L4*g*m5*cos(q3d + q4d - q5d))/4 + (L3*Mext*g*sin(q2d + q3d +
q4d + q5d))/2 + (3^(1/2)*L4*g*m3*sin(q3d + q4d + q5d))/8 +
(3^(1/2)*L4*g*m4*sin(q3d + q4d + q5d))/4 + (3^(1/2)*L4*g*m5*sin(q3d + q4d
+ q5d))/4 + (L3*L4*m4*qd2d^2*cos(q2d))/4 + (L3*L4*m5*qd2d^2*cos(q2d))/2 -
(L5*L6*m2*qd5d^2*cos(q4d))/4 - (L5*L6*m3*qd5d^2*cos(q4d))/2 -
(L5*L6*m4*qd5d^2*cos(q4d))/2 - (L5*L6*m5*qd5d^2*cos(q4d))/2 +
(L3*L4*m4*qd5d^2*cos(q2d + 2*q3d + 2*q4d))/4 + (L3*L4*m5*qd5d^2*cos(q2d +
2*q3d + 2*q4d))/2 + (3^(1/2)*L4^2*m3*qd5d^2*cos(2*q3d + 2*q4d))/16 +
(3^(1/2)*L4^2*m4*qd5d^2*cos(2*q3d + 2*q4d))/4 +
(3^(1/2)*L4^2*m5*qd5d^2*cos(2*q3d + 2*q4d))/4 -
(L4*L5*m3*qd3d^2*sin(q3d))/4 - (L4*L5*m4*qd3d^2*sin(q3d))/2 -
(L4*L5*m5*qd3d^2*sin(q3d))/2 + L4*L5*Mext*qd5d^2*sin(q3d + 2*q4d) +
L3*L6*Mext*qd5d^2*sin(q2d + q3d + q4d) + (3^(1/2)*L4*Mext*g*sin(q3d + q4d
- q5d))/4 + (L4*L5*m3*qd5d^2*sin(q3d + 2*q4d))/2 +
L4*L5*m4*qd5d^2*sin(q3d + 2*q4d) + L4*L5*m5*qd5d^2*sin(q3d + 2*q4d) +
(L3*L6*m4*qd5d^2*sin(q2d + q3d + q4d))/2 + L3*L6*m5*qd5d^2*sin(q2d + q3d
+ q4d) + (3^(1/2)*L4*g*m3*sin(q3d + q4d - q5d))/8 +
(3^(1/2)*L4*g*m4*sin(q3d + q4d - q5d))/4 + (3^(1/2)*L4*g*m5*sin(q3d + q4d
- q5d))/4 - (L3*L5*Mext*qdd2d*sin(q2d + q3d))/2 -
(L3*L5*Mext*qdd3d*sin(q2d + q3d))/2 - L3*L5*Mext*qdd4d*sin(q2d + q3d) +
(3^(1/2)*L5*Mext*g*sin(q4d + q5d))/4 + (L4*L5*Mext*qdd3d*cos(q3d))/2 +
L4*L5*Mext*qdd4d*cos(q3d) - (L3*L5*m4*qdd2d*sin(q2d + q3d))/4 -
(L3*L5*m4*qdd3d*sin(q2d + q3d))/4 - (L3*L5*m5*qdd2d*sin(q2d + q3d))/2 -
(L3*L5*m4*qdd4d*sin(q2d + q3d))/2 - (L3*L5*m5*qdd3d*sin(q2d + q3d))/2 -
L3*L5*m5*qdd4d*sin(q2d + q3d) - (L3*L5*Mext*qd5d^2*cos(q2d + q3d +
2*q4d))/2 + (3^(1/2)*L5*g*m2*sin(q4d + q5d))/8 + (3^(1/2)*L5*g*m3*sin(q4d
+ q5d))/4 + (3^(1/2)*L5*g*m4*sin(q4d + q5d))/4 + (3^(1/2)*L5*g*m5*sin(q4d
+ q5d))/4 + (L3*L4*Mext*qdd2d*sin(q2d))/2 + L3*L4*Mext*qdd3d*sin(q2d) +
L3*L4*Mext*qdd4d*sin(q2d) + (L4*L5*m3*qdd3d*cos(q3d))/4 +
(L4*L5*m3*qdd4d*cos(q3d))/2 + (L4*L5*m4*qdd3d*cos(q3d))/2 +
L4*L5*m4*qdd4d*cos(q3d) + (L4*L5*m5*qdd3d*cos(q3d))/2 +
L4*L5*m5*qdd4d*cos(q3d) - (L3*L5*m4*qd5d^2*cos(q2d + q3d + 2*q4d))/4 -
(L3*L5*m5*qd5d^2*cos(q2d + q3d + 2*q4d))/2 + (L3*L4*m4*qdd2d*sin(q2d))/4
+ (L3*L4*m4*qdd3d*sin(q2d))/2 + (L3*L4*m5*qdd2d*sin(q2d))/2 +
(L3*L4*m4*qdd4d*sin(q2d))/2 + L3*L4*m5*qdd3d*sin(q2d) +
L3*L4*m5*qdd4d*sin(q2d) - (L3*L5*Mext*qd2d^2*cos(q2d + q3d))/2 -
(L3*L5*Mext*qd3d^2*cos(q2d + q3d))/2 + (L4*L6*Mext*qd5d^2*cos(q3d +
q4d))/2 - (3^(1/2)*L5^2*Mext*qd5d^2*cos(2*q4d))/4 +
(3^(1/2)*L5*Mext*g*sin(q4d - q5d))/4 - (L3*L5*m4*qd2d^2*cos(q2d + q3d))/4
- (L3*L5*m4*qd3d^2*cos(q2d + q3d))/4 - (L3*L5*m5*qd2d^2*cos(q2d + q3d))/2
- (L3*L5*m5*qd3d^2*cos(q2d + q3d))/2 + (L4*L6*m3*qd5d^2*cos(q3d + q4d))/4
+ (L4*L6*m4*qd5d^2*cos(q3d + q4d))/2 + (L4*L6*m5*qd5d^2*cos(q3d + q4d))/2
- (3^(1/2)*L5^2*m2*qd5d^2*cos(2*q4d))/16 -
(3^(1/2)*L5^2*m3*qd5d^2*cos(2*q4d))/4 -
(3^(1/2)*L5^2*m4*qd5d^2*cos(2*q4d))/4 -
(3^(1/2)*L5^2*m5*qd5d^2*cos(2*q4d))/4 + (3^(1/2)*L4*Mext*g*sin(q3d + q4d
+ q5d))/4 + (L3*L4*Mext*qd2d^2*cos(q2d))/2 -
(L5*L6*Mext*qd5d^2*cos(q4d))/2 + (L3*L4*Mext*qd5d^2*cos(q2d + 2*q3d +

128
UNIVERSIDAD NACIONAL DE TRUJILLO
2*q4d))/2 + (3^(1/2)*L4^2*Mext*qd5d^2*cos(2*q3d + 2*q4d))/4 +
(3^(1/2)*L5*g*m2*sin(q4d - q5d))/8 + (3^(1/2)*L5*g*m3*sin(q4d - q5d))/4 +
(3^(1/2)*L5*g*m4*sin(q4d - q5d))/4 + (3^(1/2)*L5*g*m5*sin(q4d - q5d))/4 -
(L4*L5*Mext*qd3d^2*sin(q3d))/2 + (3^(1/2)*L3*L4*m4*qdd2d*cos(q2d))/4 +
(3^(1/2)*L3*L4*m4*qdd3d*cos(q2d))/2 + (3^(1/2)*L3*L4*m5*qdd2d*cos(q2d))/2
+ (3^(1/2)*L3*L4*m4*qdd4d*cos(q2d))/2 + 3^(1/2)*L3*L4*m5*qdd3d*cos(q2d) +
3^(1/2)*L3*L4*m5*qdd4d*cos(q2d) - L4*L5*Mext*qd3d*qd4d*sin(q3d) -
(3^(1/2)*L4*L5*m3*qdd3d*sin(q3d))/4 - (3^(1/2)*L4*L5*m3*qdd4d*sin(q3d))/2
- (3^(1/2)*L4*L5*m4*qdd3d*sin(q3d))/2 - 3^(1/2)*L4*L5*m4*qdd4d*sin(q3d) -
(3^(1/2)*L4*L5*m5*qdd3d*sin(q3d))/2 - 3^(1/2)*L4*L5*m5*qdd4d*sin(q3d) +
(3^(1/2)*L3*L5*m4*qd5d^2*sin(q2d + q3d + 2*q4d))/4 +
(3^(1/2)*L3*L5*m5*qd5d^2*sin(q2d + q3d + 2*q4d))/2 +
(L3*L4*m4*qd2d*qd3d*cos(q2d))/2 + (L3*L4*m4*qd2d*qd4d*cos(q2d))/2 +
L3*L4*m5*qd2d*qd3d*cos(q2d) + L3*L4*m5*qd2d*qd4d*cos(q2d) -
(3^(1/2)*L3*L5*Mext*qd2d^2*sin(q2d + q3d))/2 -
(3^(1/2)*L3*L5*Mext*qd3d^2*sin(q2d + q3d))/2 +
(3^(1/2)*L4*L6*Mext*qd5d^2*sin(q3d + q4d))/2 -
(L4*L5*m3*qd3d*qd4d*sin(q3d))/2 - L4*L5*m4*qd3d*qd4d*sin(q3d) -
L4*L5*m5*qd3d*qd4d*sin(q3d) - (3^(1/2)*L4*L5*Mext*qd3d^2*cos(q3d))/2 -
(3^(1/2)*L3*L5*m4*qd2d^2*sin(q2d + q3d))/4 -
(3^(1/2)*L3*L5*m4*qd3d^2*sin(q2d + q3d))/4 -
(3^(1/2)*L3*L5*m5*qd2d^2*sin(q2d + q3d))/2 -
(3^(1/2)*L3*L5*m5*qd3d^2*sin(q2d + q3d))/2 +
(3^(1/2)*L4*L6*m3*qd5d^2*sin(q3d + q4d))/4 +
(3^(1/2)*L4*L6*m4*qd5d^2*sin(q3d + q4d))/2 +
(3^(1/2)*L4*L6*m5*qd5d^2*sin(q3d + q4d))/2 -
(3^(1/2)*L3*L4*Mext*qd2d^2*sin(q2d))/2 +
(3^(1/2)*L5*L6*Mext*qd5d^2*sin(q4d))/2 +
(3^(1/2)*L3*L4*Mext*qd5d^2*sin(q2d + 2*q3d + 2*q4d))/2 -
(3^(1/2)*L4*L5*m3*qd3d^2*cos(q3d))/4 -
(3^(1/2)*L4*L5*m4*qd3d^2*cos(q3d))/2 -
(3^(1/2)*L4*L5*m5*qd3d^2*cos(q3d))/2 -
(3^(1/2)*L3*L4*m4*qd2d^2*sin(q2d))/4 -
(3^(1/2)*L3*L4*m5*qd2d^2*sin(q2d))/2 +
(3^(1/2)*L5*L6*m2*qd5d^2*sin(q4d))/4 +
(3^(1/2)*L5*L6*m3*qd5d^2*sin(q4d))/2 +
(3^(1/2)*L5*L6*m4*qd5d^2*sin(q4d))/2 +
(3^(1/2)*L5*L6*m5*qd5d^2*sin(q4d))/2 + (3^(1/2)*L3*L4*m4*qd5d^2*sin(q2d +
2*q3d + 2*q4d))/4 + (3^(1/2)*L3*L4*m5*qd5d^2*sin(q2d + 2*q3d + 2*q4d))/2
+ (3^(1/2)*L3*L5*Mext*qdd2d*cos(q2d + q3d))/2 +
(3^(1/2)*L3*L5*Mext*qdd3d*cos(q2d + q3d))/2 +
3^(1/2)*L3*L5*Mext*qdd4d*cos(q2d + q3d) - L3*L5*Mext*qd2d*qd3d*cos(q2d +
q3d) - L3*L5*Mext*qd2d*qd4d*cos(q2d + q3d) - L3*L5*Mext*qd3d*qd4d*cos(q2d
+ q3d) + (3^(1/2)*L3*L5*m4*qdd2d*cos(q2d + q3d))/4 +
(3^(1/2)*L3*L5*m4*qdd3d*cos(q2d + q3d))/4 +
(3^(1/2)*L3*L5*m5*qdd2d*cos(q2d + q3d))/2 +
(3^(1/2)*L3*L5*m4*qdd4d*cos(q2d + q3d))/2 +
(3^(1/2)*L3*L5*m5*qdd3d*cos(q2d + q3d))/2 +
3^(1/2)*L3*L5*m5*qdd4d*cos(q2d + q3d) +
(3^(1/2)*L3*L4*Mext*qdd2d*cos(q2d))/2 + 3^(1/2)*L3*L4*Mext*qdd3d*cos(q2d)
+ 3^(1/2)*L3*L4*Mext*qdd4d*cos(q2d) - (L3*L5*m4*qd2d*qd3d*cos(q2d +
q3d))/2 - (L3*L5*m4*qd2d*qd4d*cos(q2d + q3d))/2 -
L3*L5*m5*qd2d*qd3d*cos(q2d + q3d) - (L3*L5*m4*qd3d*qd4d*cos(q2d + q3d))/2
- L3*L5*m5*qd2d*qd4d*cos(q2d + q3d) - L3*L5*m5*qd3d*qd4d*cos(q2d + q3d) -
(3^(1/2)*L4*L5*Mext*qdd3d*sin(q3d))/2 - 3^(1/2)*L4*L5*Mext*qdd4d*sin(q3d)
+ (3^(1/2)*L3*L5*Mext*qd5d^2*sin(q2d + q3d + 2*q4d))/2 +
L3*L4*Mext*qd2d*qd3d*cos(q2d) + L3*L4*Mext*qd2d*qd4d*cos(q2d) -

129
UNIVERSIDAD NACIONAL DE TRUJILLO
3^(1/2)*L3*L5*Mext*qd2d*qd3d*sin(q2d + q3d) -
3^(1/2)*L3*L5*Mext*qd2d*qd4d*sin(q2d + q3d) -
3^(1/2)*L3*L5*Mext*qd3d*qd4d*sin(q2d + q3d) -
3^(1/2)*L4*L5*Mext*qd3d*qd4d*cos(q3d) -
(3^(1/2)*L3*L5*m4*qd2d*qd3d*sin(q2d + q3d))/2 -
(3^(1/2)*L3*L5*m4*qd2d*qd4d*sin(q2d + q3d))/2 -
3^(1/2)*L3*L5*m5*qd2d*qd3d*sin(q2d + q3d) -
(3^(1/2)*L3*L5*m4*qd3d*qd4d*sin(q2d + q3d))/2 -
3^(1/2)*L3*L5*m5*qd2d*qd4d*sin(q2d + q3d) -
3^(1/2)*L3*L5*m5*qd3d*qd4d*sin(q2d + q3d) -
3^(1/2)*L3*L4*Mext*qd2d*qd3d*sin(q2d) -
3^(1/2)*L3*L4*Mext*qd2d*qd4d*sin(q2d) -
(3^(1/2)*L4*L5*m3*qd3d*qd4d*cos(q3d))/2 -
3^(1/2)*L4*L5*m4*qd3d*qd4d*cos(q3d) - 3^(1/2)*L4*L5*m5*qd3d*qd4d*cos(q3d)
- (3^(1/2)*L3*L4*m4*qd2d*qd3d*sin(q2d))/2 -
(3^(1/2)*L3*L4*m4*qd2d*qd4d*sin(q2d))/2 -
3^(1/2)*L3*L4*m5*qd2d*qd3d*sin(q2d) - 3^(1/2)*L3*L4*m5*qd2d*qd4d*sin(q2d)
(a.43)

3 = I3d*qdd3d + I4d*qdd2d + I3d*qdd4d + I4d*qdd3d + I5d*qdd2d +


I4d*qdd4d + I5d*qdd3d + I5d*qdd4d + Iext*qdd2d + Iext*qdd3d + Iext*qdd4d
+ b3*qd3d + L3^2*Mext*qdd2d + L3^2*Mext*qdd3d + L3^2*Mext*qdd4d +
L4^2*Mext*qdd3d + L4^2*Mext*qdd4d + (L3^2*m4*qdd2d)/4 + (L3^2*m4*qdd3d)/4
+ L3^2*m5*qdd2d + (L4^2*m3*qdd3d)/4 + (L3^2*m4*qdd4d)/4 + L3^2*m5*qdd3d +
(L4^2*m3*qdd4d)/4 + L4^2*m4*qdd3d + L3^2*m5*qdd4d + L4^2*m4*qdd4d +
L4^2*m5*qdd3d + L4^2*m5*qdd4d + (L3*g*m4*sin(q2d + q3d + q4d + q5d))/4 +
(L3*g*m5*sin(q2d + q3d + q4d + q5d))/2 + (L3*Mext*g*sin(q2d + q3d + q4d -
q5d))/2 + (L3^2*Mext*qd5d^2*sin(2*q2d + 2*q3d + 2*q4d))/2 +
(L4*Mext*g*cos(q3d + q4d + q5d))/4 + (L3*g*m4*sin(q2d + q3d + q4d -
q5d))/4 + (L3*g*m5*sin(q2d + q3d + q4d - q5d))/2 +
(L3^2*m4*qd5d^2*sin(2*q2d + 2*q3d + 2*q4d))/8 + (L3^2*m5*qd5d^2*sin(2*q2d
+ 2*q3d + 2*q4d))/2 + (L4*g*m3*cos(q3d + q4d + q5d))/8 + (L4*g*m4*cos(q3d
+ q4d + q5d))/4 + (L4*g*m5*cos(q3d + q4d + q5d))/4 +
(L4^2*Mext*qd5d^2*sin(2*q3d + 2*q4d))/4 + I5d*qd1d*qd5d*sin(q2d + q3d +
q4d) + Iext*qd1d*qd5d*sin(q2d + q3d + q4d) + (L4^2*m3*qd5d^2*sin(2*q3d +
2*q4d))/16 + (L4^2*m4*qd5d^2*sin(2*q3d + 2*q4d))/4 +
(L4^2*m5*qd5d^2*sin(2*q3d + 2*q4d))/4 + (L4*Mext*g*cos(q3d + q4d -
q5d))/4 + (L4*g*m3*cos(q3d + q4d - q5d))/8 + (L4*g*m4*cos(q3d + q4d -
q5d))/4 + (L4*g*m5*cos(q3d + q4d - q5d))/4 + (L3*Mext*g*sin(q2d + q3d +
q4d + q5d))/2 + (3^(1/2)*L4*g*m3*sin(q3d + q4d + q5d))/8 +
(3^(1/2)*L4*g*m4*sin(q3d + q4d + q5d))/4 + (3^(1/2)*L4*g*m5*sin(q3d + q4d
+ q5d))/4 + (L3*L4*m4*qd2d^2*cos(q2d))/4 + (L3*L4*m5*qd2d^2*cos(q2d))/2 +
(L3*L4*m4*qd5d^2*cos(q2d + 2*q3d + 2*q4d))/4 + (L3*L4*m5*qd5d^2*cos(q2d +
2*q3d + 2*q4d))/2 + (3^(1/2)*L4^2*m3*qd5d^2*cos(2*q3d + 2*q4d))/16 +
(3^(1/2)*L4^2*m4*qd5d^2*cos(2*q3d + 2*q4d))/4 +
(3^(1/2)*L4^2*m5*qd5d^2*cos(2*q3d + 2*q4d))/4 +
(L4*L5*m3*qd4d^2*sin(q3d))/4 + (L4*L5*m3*qd5d^2*sin(q3d))/8 +
(L4*L5*m4*qd4d^2*sin(q3d))/2 + (L4*L5*m4*qd5d^2*sin(q3d))/4 +
(L4*L5*m5*qd4d^2*sin(q3d))/2 + (L4*L5*m5*qd5d^2*sin(q3d))/4 +
(L4*L5*Mext*qd5d^2*sin(q3d + 2*q4d))/2 + L3*L6*Mext*qd5d^2*sin(q2d + q3d
+ q4d) + (3^(1/2)*L4*Mext*g*sin(q3d + q4d - q5d))/4 +
(L4*L5*m3*qd5d^2*sin(q3d + 2*q4d))/4 + (L4*L5*m4*qd5d^2*sin(q3d +
2*q4d))/2 + (L4*L5*m5*qd5d^2*sin(q3d + 2*q4d))/2 +
(L3*L6*m4*qd5d^2*sin(q2d + q3d + q4d))/2 + L3*L6*m5*qd5d^2*sin(q2d + q3d
+ q4d) + (3^(1/2)*L4*g*m3*sin(q3d + q4d - q5d))/8 +

130
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L4*g*m4*sin(q3d + q4d - q5d))/4 + (3^(1/2)*L4*g*m5*sin(q3d + q4d
- q5d))/4 - (L3*L5*Mext*qdd4d*sin(q2d + q3d))/2 +
(L4*L5*Mext*qdd4d*cos(q3d))/2 - (L3*L5*m4*qdd4d*sin(q2d + q3d))/4 -
(L3*L5*m5*qdd4d*sin(q2d + q3d))/2 - (L3*L5*Mext*qd5d^2*cos(q2d + q3d +
2*q4d))/4 + (L3*L4*Mext*qdd2d*sin(q2d))/2 + L3*L4*Mext*qdd3d*sin(q2d) +
L3*L4*Mext*qdd4d*sin(q2d) + (L4*L5*m3*qdd4d*cos(q3d))/4 +
(L4*L5*m4*qdd4d*cos(q3d))/2 + (L4*L5*m5*qdd4d*cos(q3d))/2 -
(L3*L5*m4*qd5d^2*cos(q2d + q3d + 2*q4d))/8 - (L3*L5*m5*qd5d^2*cos(q2d +
q3d + 2*q4d))/4 + (L3*L4*m4*qdd2d*sin(q2d))/4 +
(L3*L4*m4*qdd3d*sin(q2d))/2 + (L3*L4*m5*qdd2d*sin(q2d))/2 +
(L3*L4*m4*qdd4d*sin(q2d))/2 + L3*L4*m5*qdd3d*sin(q2d) +
L3*L4*m5*qdd4d*sin(q2d) + (L3*L5*Mext*qd4d^2*cos(q2d + q3d))/2 +
(L3*L5*Mext*qd5d^2*cos(q2d + q3d))/4 + (L4*L6*Mext*qd5d^2*cos(q3d +
q4d))/2 + (L3*L5*m4*qd4d^2*cos(q2d + q3d))/4 + (L3*L5*m4*qd5d^2*cos(q2d +
q3d))/8 + (L3*L5*m5*qd4d^2*cos(q2d + q3d))/2 + (L3*L5*m5*qd5d^2*cos(q2d +
q3d))/4 + (L4*L6*m3*qd5d^2*cos(q3d + q4d))/4 + (L4*L6*m4*qd5d^2*cos(q3d +
q4d))/2 + (L4*L6*m5*qd5d^2*cos(q3d + q4d))/2 + (3^(1/2)*L4*Mext*g*sin(q3d
+ q4d + q5d))/4 + (L3*L4*Mext*qd2d^2*cos(q2d))/2 +
(L3*L4*Mext*qd5d^2*cos(q2d + 2*q3d + 2*q4d))/2 +
(3^(1/2)*L4^2*Mext*qd5d^2*cos(2*q3d + 2*q4d))/4 +
(L4*L5*Mext*qd4d^2*sin(q3d))/2 + (L4*L5*Mext*qd5d^2*sin(q3d))/4 +
(3^(1/2)*L3*L4*m4*qdd2d*cos(q2d))/4 + (3^(1/2)*L3*L4*m4*qdd3d*cos(q2d))/2
+ (3^(1/2)*L3*L4*m5*qdd2d*cos(q2d))/2 +
(3^(1/2)*L3*L4*m4*qdd4d*cos(q2d))/2 + 3^(1/2)*L3*L4*m5*qdd3d*cos(q2d) +
3^(1/2)*L3*L4*m5*qdd4d*cos(q2d) - (3^(1/2)*L4*L5*m3*qdd4d*sin(q3d))/4 -
(3^(1/2)*L4*L5*m4*qdd4d*sin(q3d))/2 - (3^(1/2)*L4*L5*m5*qdd4d*sin(q3d))/2
+ (3^(1/2)*L3*L5*m4*qd5d^2*sin(q2d + q3d + 2*q4d))/8 +
(3^(1/2)*L3*L5*m5*qd5d^2*sin(q2d + q3d + 2*q4d))/4 +
(L3*L4*m4*qd2d*qd3d*cos(q2d))/2 + (L3*L4*m4*qd2d*qd4d*cos(q2d))/2 +
L3*L4*m5*qd2d*qd3d*cos(q2d) + L3*L4*m5*qd2d*qd4d*cos(q2d) +
(3^(1/2)*L3*L5*Mext*qd4d^2*sin(q2d + q3d))/2 +
(3^(1/2)*L3*L5*Mext*qd5d^2*sin(q2d + q3d))/4 +
(3^(1/2)*L4*L6*Mext*qd5d^2*sin(q3d + q4d))/2 +
(3^(1/2)*L4*L5*Mext*qd4d^2*cos(q3d))/2 +
(3^(1/2)*L4*L5*Mext*qd5d^2*cos(q3d))/4 + (3^(1/2)*L3*L5*m4*qd4d^2*sin(q2d
+ q3d))/4 + (3^(1/2)*L3*L5*m4*qd5d^2*sin(q2d + q3d))/8 +
(3^(1/2)*L3*L5*m5*qd4d^2*sin(q2d + q3d))/2 +
(3^(1/2)*L3*L5*m5*qd5d^2*sin(q2d + q3d))/4 +
(3^(1/2)*L4*L6*m3*qd5d^2*sin(q3d + q4d))/4 +
(3^(1/2)*L4*L6*m4*qd5d^2*sin(q3d + q4d))/2 +
(3^(1/2)*L4*L6*m5*qd5d^2*sin(q3d + q4d))/2 -
(3^(1/2)*L3*L4*Mext*qd2d^2*sin(q2d))/2 +
(3^(1/2)*L3*L4*Mext*qd5d^2*sin(q2d + 2*q3d + 2*q4d))/2 +
(3^(1/2)*L4*L5*m3*qd4d^2*cos(q3d))/4 +
(3^(1/2)*L4*L5*m3*qd5d^2*cos(q3d))/8 +
(3^(1/2)*L4*L5*m4*qd4d^2*cos(q3d))/2 +
(3^(1/2)*L4*L5*m4*qd5d^2*cos(q3d))/4 +
(3^(1/2)*L4*L5*m5*qd4d^2*cos(q3d))/2 +
(3^(1/2)*L4*L5*m5*qd5d^2*cos(q3d))/4 -
(3^(1/2)*L3*L4*m4*qd2d^2*sin(q2d))/4 -
(3^(1/2)*L3*L4*m5*qd2d^2*sin(q2d))/2 + (3^(1/2)*L3*L4*m4*qd5d^2*sin(q2d +
2*q3d + 2*q4d))/4 + (3^(1/2)*L3*L4*m5*qd5d^2*sin(q2d + 2*q3d + 2*q4d))/2
+ (3^(1/2)*L3*L5*Mext*qdd4d*cos(q2d + q3d))/2 +
(3^(1/2)*L3*L5*m4*qdd4d*cos(q2d + q3d))/4 +
(3^(1/2)*L3*L5*m5*qdd4d*cos(q2d + q3d))/2 +
(3^(1/2)*L3*L4*Mext*qdd2d*cos(q2d))/2 + 3^(1/2)*L3*L4*Mext*qdd3d*cos(q2d)
+ 3^(1/2)*L3*L4*Mext*qdd4d*cos(q2d) -

131
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L4*L5*Mext*qdd4d*sin(q3d))/2 +
(3^(1/2)*L3*L5*Mext*qd5d^2*sin(q2d + q3d + 2*q4d))/4 +
L3*L4*Mext*qd2d*qd3d*cos(q2d) + L3*L4*Mext*qd2d*qd4d*cos(q2d) -
3^(1/2)*L3*L4*Mext*qd2d*qd3d*sin(q2d) -
3^(1/2)*L3*L4*Mext*qd2d*qd4d*sin(q2d) -
(3^(1/2)*L3*L4*m4*qd2d*qd3d*sin(q2d))/2 -
(3^(1/2)*L3*L4*m4*qd2d*qd4d*sin(q2d))/2 -
3^(1/2)*L3*L4*m5*qd2d*qd3d*sin(q2d) - 3^(1/2)*L3*L4*m5*qd2d*qd4d*sin(q2d)
(a.44)

4 = I4d*qdd2d + I4d*qdd3d + I5d*qdd2d + I4d*qdd4d + I5d*qdd3d +


I5d*qdd4d + Iext*qdd2d + Iext*qdd3d + Iext*qdd4d + b4*qd2d +
L3^2*Mext*qdd2d + L3^2*Mext*qdd3d + L3^2*Mext*qdd4d + (L3^2*m4*qdd2d)/4 +
(L3^2*m4*qdd3d)/4 + L3^2*m5*qdd2d + (L3^2*m4*qdd4d)/4 + L3^2*m5*qdd3d +
L3^2*m5*qdd4d + (L3*g*m4*sin(q2d + q3d + q4d + q5d))/4 + (L3*g*m5*sin(q2d
+ q3d + q4d + q5d))/2 + (L3*Mext*g*sin(q2d + q3d + q4d - q5d))/2 +
(L3^2*Mext*qd5d^2*sin(2*q2d + 2*q3d + 2*q4d))/2 + (L3*g*m4*sin(q2d + q3d
+ q4d - q5d))/4 + (L3*g*m5*sin(q2d + q3d + q4d - q5d))/2 +
(L3^2*m4*qd5d^2*sin(2*q2d + 2*q3d + 2*q4d))/8 + (L3^2*m5*qd5d^2*sin(2*q2d
+ 2*q3d + 2*q4d))/2 + I5d*qd1d*qd5d*sin(q2d + q3d + q4d) +
Iext*qd1d*qd5d*sin(q2d + q3d + q4d) + (L3*Mext*g*sin(q2d + q3d + q4d +
q5d))/2 - (L3*L4*m4*qd3d^2*cos(q2d))/4 - (L3*L4*m4*qd4d^2*cos(q2d))/4 -
(L3*L4*m5*qd3d^2*cos(q2d))/2 - (L3*L4*m4*qd5d^2*cos(q2d))/8 -
(L3*L4*m5*qd4d^2*cos(q2d))/2 - (L3*L4*m5*qd5d^2*cos(q2d))/4 +
(L3*L4*m4*qd5d^2*cos(q2d + 2*q3d + 2*q4d))/8 + (L3*L4*m5*qd5d^2*cos(q2d +
2*q3d + 2*q4d))/4 + L3*L6*Mext*qd5d^2*sin(q2d + q3d + q4d) +
(L3*L6*m4*qd5d^2*sin(q2d + q3d + q4d))/2 + L3*L6*m5*qd5d^2*sin(q2d + q3d
+ q4d) - (L3*L5*Mext*qdd4d*sin(q2d + q3d))/2 - (L3*L5*m4*qdd4d*sin(q2d +
q3d))/4 - (L3*L5*m5*qdd4d*sin(q2d + q3d))/2 - (L3*L5*Mext*qd5d^2*cos(q2d
+ q3d + 2*q4d))/4 + (L3*L4*Mext*qdd3d*sin(q2d))/2 +
(L3*L4*Mext*qdd4d*sin(q2d))/2 - (L3*L5*m4*qd5d^2*cos(q2d + q3d +
2*q4d))/8 - (L3*L5*m5*qd5d^2*cos(q2d + q3d + 2*q4d))/4 +
(L3*L4*m4*qdd3d*sin(q2d))/4 + (L3*L4*m4*qdd4d*sin(q2d))/4 +
(L3*L4*m5*qdd3d*sin(q2d))/2 + (L3*L4*m5*qdd4d*sin(q2d))/2 +
(L3*L5*Mext*qd4d^2*cos(q2d + q3d))/2 + (L3*L5*Mext*qd5d^2*cos(q2d +
q3d))/4 + (L3*L5*m4*qd4d^2*cos(q2d + q3d))/4 + (L3*L5*m4*qd5d^2*cos(q2d +
q3d))/8 + (L3*L5*m5*qd4d^2*cos(q2d + q3d))/2 + (L3*L5*m5*qd5d^2*cos(q2d +
q3d))/4 - (L3*L4*Mext*qd3d^2*cos(q2d))/2 - (L3*L4*Mext*qd4d^2*cos(q2d))/2
- (L3*L4*Mext*qd5d^2*cos(q2d))/4 + (L3*L4*Mext*qd5d^2*cos(q2d + 2*q3d +
2*q4d))/4 + (3^(1/2)*L3*L4*m4*qdd3d*cos(q2d))/4 +
(3^(1/2)*L3*L4*m4*qdd4d*cos(q2d))/4 + (3^(1/2)*L3*L4*m5*qdd3d*cos(q2d))/2
+ (3^(1/2)*L3*L4*m5*qdd4d*cos(q2d))/2 + (3^(1/2)*L3*L5*m4*qd5d^2*sin(q2d
+ q3d + 2*q4d))/8 + (3^(1/2)*L3*L5*m5*qd5d^2*sin(q2d + q3d + 2*q4d))/4 -
(L3*L4*m4*qd3d*qd4d*cos(q2d))/2 - L3*L4*m5*qd3d*qd4d*cos(q2d) +
(3^(1/2)*L3*L5*Mext*qd4d^2*sin(q2d + q3d))/2 +
(3^(1/2)*L3*L5*Mext*qd5d^2*sin(q2d + q3d))/4 +
(3^(1/2)*L3*L5*m4*qd4d^2*sin(q2d + q3d))/4 +
(3^(1/2)*L3*L5*m4*qd5d^2*sin(q2d + q3d))/8 +
(3^(1/2)*L3*L5*m5*qd4d^2*sin(q2d + q3d))/2 +
(3^(1/2)*L3*L5*m5*qd5d^2*sin(q2d + q3d))/4 +
(3^(1/2)*L3*L4*Mext*qd3d^2*sin(q2d))/2 +
(3^(1/2)*L3*L4*Mext*qd4d^2*sin(q2d))/2 +
(3^(1/2)*L3*L4*Mext*qd5d^2*sin(q2d))/4 +
(3^(1/2)*L3*L4*Mext*qd5d^2*sin(q2d + 2*q3d + 2*q4d))/4 +
(3^(1/2)*L3*L4*m4*qd3d^2*sin(q2d))/4 +
(3^(1/2)*L3*L4*m4*qd4d^2*sin(q2d))/4 +

132
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L3*L4*m5*qd3d^2*sin(q2d))/2 +
(3^(1/2)*L3*L4*m4*qd5d^2*sin(q2d))/8 +
(3^(1/2)*L3*L4*m5*qd4d^2*sin(q2d))/2 +
(3^(1/2)*L3*L4*m5*qd5d^2*sin(q2d))/4 + (3^(1/2)*L3*L4*m4*qd5d^2*sin(q2d +
2*q3d + 2*q4d))/8 + (3^(1/2)*L3*L4*m5*qd5d^2*sin(q2d + 2*q3d + 2*q4d))/4
+ (3^(1/2)*L3*L5*Mext*qdd4d*cos(q2d + q3d))/2 +
(3^(1/2)*L3*L5*m4*qdd4d*cos(q2d + q3d))/4 +
(3^(1/2)*L3*L5*m5*qdd4d*cos(q2d + q3d))/2 +
(3^(1/2)*L3*L4*Mext*qdd3d*cos(q2d))/2 +
(3^(1/2)*L3*L4*Mext*qdd4d*cos(q2d))/2 +
(3^(1/2)*L3*L5*Mext*qd5d^2*sin(q2d + q3d + 2*q4d))/4 -
L3*L4*Mext*qd3d*qd4d*cos(q2d) + 3^(1/2)*L3*L4*Mext*qd3d*qd4d*sin(q2d) +
(3^(1/2)*L3*L4*m4*qd3d*qd4d*sin(q2d))/2 +
3^(1/2)*L3*L4*m5*qd3d*qd4d*sin(q2d) (a.45)

5 = b5*qd1d - (I5d + Iext)*(qd2d*qd5d*sin(q2d + q3d + q4d) -


qdd5d*cos(q2d + q3d + q4d) - qdd1d + qd3d*qd5d*sin(q2d + q3d + q4d) +
qd4d*qd5d*sin(q2d + q3d + q4d)) (a.46)

A.6 Resultados de las ecuaciones ZMP, velocidades y aceleraciones del COG.


A.6.1 Velocidades del COG del robot bpedo:
Vcmx = yi*(dq1d*(cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) +
sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) + sin(q1d)*sin(q5d)) -
dq5d*(cos(q1d)*cos(q5d) - sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))
- sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)))) + dq2d*sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) +
dq3d*sin(q1d)*(cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6)
+ cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) - sin(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6))) + dq4d*sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)))) -
dq2d*(L3*cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) - L3*sin(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + L2*cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) -
sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) + L1*sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)))) -
dq4d*(L3*cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) - L3*sin(q2d -

133
UNIVERSIDAD NACIONAL DE TRUJILLO
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + L2*cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) -
sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) + L1*sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) +
L5*cos(q5d)*sin(q4d - pi/6) + L4*cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6)
+ L4*cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) -
dq5d*(L2*(cos(q5d)*sin(q1d) + cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))
- sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)))) - L1*(cos(q1d)*cos(q5d) -
sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d)))) + L6*sin(q5d) + L3*cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + L5*cos(q4d - pi/6)*sin(q5d) +
L4*cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - L4*sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) - xi*(dq1d*(cos(q1d)*sin(q5d) - sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)))) +
dq5d*(cos(q5d)*sin(q1d) + cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))
- sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)))) + dq2d*cos(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) +
dq3d*cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6)
+ cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) - sin(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6))) + dq4d*cos(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)))) -
dq1d*(L1*(cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) + sin(q1d)*sin(q5d)) + L2*(cos(q1d)*sin(q5d) -
sin(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))))) - dq3d*(L3*cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) -
L3*sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + L2*cos(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +

134
UNIVERSIDAD NACIONAL DE TRUJILLO
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) +
L1*sin(q1d)*(cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) - sin(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6))) + L4*cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
L4*cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) - zi*(dq2d*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) + dq3d*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) + dq4d*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) + dq5d*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6)))) (a.47)

Vcmy = dq1d*(L1*(cos(q5d)*sin(q1d) + cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3


+ q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)))) + L2*(cos(q1d)*cos(q5d) -
sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d))))) - dq4d*(L3*cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d -
pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)) + L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) + L5*sin(q5d)*sin(q4d - pi/6) +
L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6))) + L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6))) + L4*cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) +
L4*cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)) -
xi*(dq5d*(cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) + sin(q1d)*sin(q5d)) - dq1d*(cos(q1d)*cos(q5d) -
sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d)))) + dq2d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))
+ sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))) + dq3d*cos(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -

135
UNIVERSIDAD NACIONAL DE TRUJILLO
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6))) + dq4d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))
+ sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)))) + dq5d*(L1*(cos(q1d)*sin(q5d)
- sin(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)))) - L2*(cos(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) + sin(q1d)*sin(q5d))
+ L6*cos(q5d) - L3*cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) - L3*sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) + L5*cos(q5d)*cos(q4d - pi/6) +
L4*cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6) - L4*cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) + yi*(dq2d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))
+ sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))) - dq1d*(cos(q5d)*sin(q1d) +
cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d)))) - dq5d*(cos(q1d)*sin(q5d) - sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)))) +
dq3d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6)
+ cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6))) + dq4d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))
+ sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)))) + zi*(dq5d*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) +
dq2d*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d))) + dq3d*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d)
- sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d))) + dq4d*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d)
- sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d)))) - dq3d*(L3*cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d -
pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)) + L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) + L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))

136
UNIVERSIDAD NACIONAL DE TRUJILLO
+ sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))) + L1*sin(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6))) + L4*cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + L4*cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) - dq2d*(L3*cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))
+ L3*sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) + L2*cos(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6))) + L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6))))
(a.48)

Vcmz = yi*(dq1d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d -


pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) +
dq2d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) -
sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) +
dq3d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) -
sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) +
dq4d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) -
sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)))) -
dq2d*(L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) -
sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) +
L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3
+ q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d -
pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) + L3*cos(q2d - pi/6)*(cos(pi/3
+ q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6)) - L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 +
q3d))) - dq3d*(L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) +
L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3
+ q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d -
pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) + L3*cos(q2d - pi/6)*(cos(pi/3
+ q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6)) - L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))
+ L4*cos(pi/3 + q3d)*cos(q4d - pi/6) - L4*sin(pi/3 + q3d)*sin(q4d -
pi/6)) - zi*(dq2d*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) + dq3d*(cos(q2d

137
UNIVERSIDAD NACIONAL DE TRUJILLO
- pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 +
q3d)) + sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 +
q3d)*sin(q4d - pi/6))) + dq4d*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d -
pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6)))) -
dq1d*(L1*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) -
L2*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)
- sin(pi/3 + q3d)*sin(q4d - pi/6)))) - dq4d*(L5*cos(q4d - pi/6) +
L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3
+ q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d -
pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) + L1*sin(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))
- sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(pi/3 + q3d))) + L3*cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6)) - L3*sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + L4*cos(pi/3 +
q3d)*cos(q4d - pi/6) - L4*sin(pi/3 + q3d)*sin(q4d - pi/6)) -
xi*(dq2d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) -
sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) +
dq3d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) -
sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) +
dq4d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) -
sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) -
dq1d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d
- pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))))
(a.49)

A.6.2.- Aceleraciones del GOG:


acmx = xi*(ddq1d*dq1d*(cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) +
sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) + sin(q1d)*sin(q5d)) +
ddq5d*dq5d*(cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) + sin(q1d)*sin(q5d)) +
ddq2d*dq2d*cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) + ddq3d*dq3d*cos(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) +
ddq4d*dq4d*cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d -

138
UNIVERSIDAD NACIONAL DE TRUJILLO
pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)))) - zi*(ddq2d*dq2d*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) +
ddq3d*dq3d*(cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) - sin(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6))) + ddq4d*dq4d*(cos(q2d - pi/6)*(cos(q5d)*cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) -
sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6))) + ddq5d*dq5d*(cos(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) - sin(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)))) -
yi*(ddq2d*dq2d*sin(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) +
sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) -
ddq5d*dq5d*(cos(q1d)*sin(q5d) - sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)))) -
ddq1d*dq1d*(cos(q1d)*sin(q5d) - sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)))) +
ddq3d*dq3d*sin(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) + ddq4d*dq4d*sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)))) -
ddq1d*dq1d*(L1*(cos(q1d)*sin(q5d) - sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)))) -
L2*(cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) + sin(q1d)*sin(q5d))) + ddq3d*dq3d*(L3*cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + L3*sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) +
L2*cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) + L1*sin(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3
+ q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) +
sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) - L4*cos(q5d)*cos(pi/3 +

139
UNIVERSIDAD NACIONAL DE TRUJILLO
q3d)*cos(q4d - pi/6) + L4*cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) +
ddq2d*dq2d*(L3*cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)
- cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + L3*sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) + L2*cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3
+ q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) +
sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) + L1*sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)))) +
ddq4d*dq4d*(L3*cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)
- cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + L3*sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d)) + L2*cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3
+ q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) +
sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d))) - L5*cos(q5d)*cos(q4d - pi/6)
+ L1*sin(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)
- cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) - L4*cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6) +
L4*cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) -
ddq5d*dq5d*(L1*(cos(q1d)*sin(q5d) - sin(q1d)*(cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) + sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)))) -
L2*(cos(q1d)*(cos(q2d - pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) -
cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)) + sin(q2d -
pi/6)*(cos(q5d)*cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d -
pi/6)*sin(pi/3 + q3d))) + sin(q1d)*sin(q5d)) + L6*cos(q5d) - L3*cos(q2d -
pi/6)*(cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6) - cos(q5d)*cos(pi/3 +
q3d)*cos(q4d - pi/6)) - L3*sin(q2d - pi/6)*(cos(q5d)*cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q5d)*cos(q4d - pi/6)*sin(pi/3 + q3d)) +
L5*cos(q5d)*cos(q4d - pi/6) + L4*cos(q5d)*cos(pi/3 + q3d)*cos(q4d - pi/6)
- L4*cos(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) (a.50)

acmy = yi*(ddq2d*dq2d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d


- pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d))) - ddq5d*dq5d*(cos(q1d)*cos(q5d) -
sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d)))) - ddq1d*dq1d*(cos(q1d)*cos(q5d) - sin(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)))) +
ddq3d*dq3d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -

140
UNIVERSIDAD NACIONAL DE TRUJILLO
pi/6)*sin(q5d)*sin(pi/3 + q3d))) + ddq4d*dq4d*sin(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)))) -
xi*(ddq1d*dq1d*(cos(q5d)*sin(q1d) + cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))
- sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)))) + ddq5d*dq5d*(cos(q5d)*sin(q1d) +
cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d)))) + ddq2d*dq2d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d))) + ddq3d*dq3d*cos(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))) +
ddq4d*dq4d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)))) - zi*(ddq2d*dq2d*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6))) + ddq3d*dq3d*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d -
pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6))) + ddq4d*dq4d*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))
+ sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))) + ddq5d*dq5d*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6)))) - ddq2d*dq2d*(L3*cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3
+ q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))) + L1*sin(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)))) -
ddq4d*dq4d*(L3*cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d)
- sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + L5*cos(q4d - pi/6)*sin(q5d) +
L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d))) + L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -

141
UNIVERSIDAD NACIONAL DE TRUJILLO
pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d))) + L4*cos(pi/3 + q3d)*cos(q4d -
pi/6)*sin(q5d) - L4*sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) +
ddq1d*dq1d*(L1*(cos(q1d)*cos(q5d) - sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))
- sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)))) - L2*(cos(q5d)*sin(q1d) +
cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) -
sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 +
q3d))))) - ddq5d*dq5d*(L2*(cos(q5d)*sin(q1d) + cos(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d)))) -
L1*(cos(q1d)*cos(q5d) - sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6))
- sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)))) + L6*sin(q5d) + L3*cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) - L3*sin(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))
+ L5*cos(q4d - pi/6)*sin(q5d) + L4*cos(pi/3 + q3d)*cos(q4d -
pi/6)*sin(q5d) - L4*sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) -
ddq3d*dq3d*(L3*cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d)
- sin(q5d)*sin(pi/3 + q3d)*sin(q4d - pi/6)) - L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(q5d)*sin(pi/3 + q3d)) + L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3
+ q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))) + L1*sin(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)*sin(q5d) - sin(q5d)*sin(pi/3 +
q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q5d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(q5d)*sin(pi/3 + q3d))) + L4*cos(pi/3 +
q3d)*cos(q4d - pi/6)*sin(q5d) - L4*sin(q5d)*sin(pi/3 + q3d)*sin(q4d -
pi/6)) (a.51)

acmz = xi*(ddq1d*dq1d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d


- pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) +
ddq2d*dq2d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) +
ddq3d*dq3d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) +
ddq4d*dq4d*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6)))) -
zi*(ddq2d*dq2d*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) -
sin(pi/3 + q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 +

142
UNIVERSIDAD NACIONAL DE TRUJILLO
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))) +
ddq3d*dq3d*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 +
q3d)*sin(q4d - pi/6)) - sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6)
+ cos(q4d - pi/6)*sin(pi/3 + q3d))) + ddq4d*dq4d*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))
- sin(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(pi/3 + q3d)))) - yi*(ddq1d*dq1d*sin(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))
+ sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 +
q3d)*sin(q4d - pi/6))) + ddq2d*dq2d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d -
pi/6))) + ddq3d*dq3d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d -
pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) +
ddq4d*dq4d*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6)))) +
ddq4d*dq4d*(L5*sin(q4d - pi/6) + L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d -
pi/6))) + L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) +
cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) + L3*cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))
+ L3*sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 +
q3d)*sin(q4d - pi/6)) + L4*cos(pi/3 + q3d)*sin(q4d - pi/6) + L4*cos(q4d -
pi/6)*sin(pi/3 + q3d)) + ddq2d*dq2d*(L2*cos(q1d)*(cos(q2d -
pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d))
+ sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 +
q3d)*sin(q4d - pi/6))) + L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d -
pi/6))) + L3*cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(pi/3 + q3d)) + L3*sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d -
pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) +
ddq3d*dq3d*(L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6)
+ cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) +
L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)
- sin(pi/3 + q3d)*sin(q4d - pi/6))) + L3*cos(q2d - pi/6)*(cos(pi/3 +
q3d)*sin(q4d - pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + L3*sin(q2d -
pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))
+ L4*cos(pi/3 + q3d)*sin(q4d - pi/6) + L4*cos(q4d - pi/6)*sin(pi/3 +
q3d)) + ddq1d*dq1d*(L2*cos(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d
- pi/6) + cos(q4d - pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 +
q3d)*cos(q4d - pi/6) - sin(pi/3 + q3d)*sin(q4d - pi/6))) +
L1*sin(q1d)*(cos(q2d - pi/6)*(cos(pi/3 + q3d)*sin(q4d - pi/6) + cos(q4d -
pi/6)*sin(pi/3 + q3d)) + sin(q2d - pi/6)*(cos(pi/3 + q3d)*cos(q4d - pi/6)
- sin(pi/3 + q3d)*sin(q4d - pi/6)))) (a.52)

143
UNIVERSIDAD NACIONAL DE TRUJILLO
B.- ALGORITMOS DE MALTAB IMPLEMENTADOS:
B.1.- Cinemtica directa.
Todos los Algoritmos fueron realizados en Matlab 2013b.
B.1.1. Cinemtica Directa: archivo Script.
% Analisis Cinematica Directa del Robot Bipedo 10GDL.
% Ing. Mecatronica - UNT
% Elaborado by Josmell Alva Alcantara.
% Fecha: 09/10/2015
%------------------------------------------------------------
clc, clear all
syms theta d a alfa L1 L2 L3 L4 L5 L6 L7
syms q1i q2i q3i q4i q5i q1d q2d q3d q4d q5d
syms pi

%ANALISIS CINEMATICO DE LA CADERA RESPECTO AL PIE DE APOYO.


% 1 METODO - LEE,Gonzales FU.
%Parametros de Denavit - Hartenberg
theta1=[q5d; q4d-pi/6; q3d+pi/3; q2d-pi/6; q1d];
d1= [0 ; 0; 0; 0; 0];
a1=[L6; L5; L4; L3; 0];
alfa1=[-pi/2; 0; 0; pi/2; 0];
MDH1=[theta1 d1 a1 alfa1] % muestra la tabla de matriz de transformacion
Homogenea

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))

T40d=simplify(T43d*T32d*T21d*T10d*T00d) % calculo intermedio paramatros


de Denavit- H
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;
0 0 -1 -L1;
0 1 0 0;
0 0 0 1] % matriz de transformacion hacia la cadera.

% Matriz de transformacion de la cadera respecto al pie de apoyo.


T5dC=simplify(T54d*T40d*T0Cd)
%T5dCnum=subs(T5dC,{L1d,L2d,L3d,L4d,L5d,L6d,L7d,q5d,q4d,q3d,q2d,q1d},{64.
5,28,64,83,105,62,16.5,0.0013,-1.0090,1.4693,-0.4386,-0.0134})
%T5dCn=double(T5dCnum)

% VECTOR DE POSICION DE LA CADERA RESPECTO A PIE DE APOYO


%P1=T5dC(1:3,4)
% ANALISIS CINEMATICO DEL PIE FLOTANTE RESPECTO A LA CADERA.
theta2=[q1i; q2i+pi/6; q3i-pi/3; q4i+pi/6; q5i ];

144
UNIVERSIDAD NACIONAL DE TRUJILLO
d2=[0; 0; 0; 0; 0];
a2=[L3; L4; L5; L6; L7];
alfa2=[pi/2; 0; 0; -pi/2; 0];

MDH2=[theta2 d2 a2 alfa2] % matriz de los valores Denavith- H

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)

% VECTOR DE POSICION DE LA pie Flotante respecto a la cadera

P2=TC5i(1:3,4)

%Calculo del Jacobiano.


%Jv2=jacobian(P2,[q1i q2i q3i q4i q5i]) % Jacobiano de velocidades
Lineales.

% RELACION DEL PIE FLOTANTE RESPECTO AL PIE DE APOYO.


disp('PIE FLOTANTE RESPECTO AL PIE DE APOYO')
T5d5i=simplify(T5dC*TC5i)

B.1.2. - Funcin para el clculo de la cinemtica directa- Cadera respecto al pie de


apoyo.
% Function Cinemtica Directa de Robot bpedo.
% Elaborado by Josmell Alva Alcntara
% Fecha: 16-01-2016
% University National of Trujillo - Ing. Mechatronics.
% -----------------------------------------------------------------------

% Cinemtica de la Cadera al pie de Apoyo.

function T5dc = directkinematicBiped(q)

% Longitudes de los Eslabones del Robot.


L7d=17.4;
L6d=46.8;
L5d=47.3;
L4d=64;
L3d=47.4;
L2d=28.8;

145
UNIVERSIDAD NACIONAL DE TRUJILLO
L1d=45.2;

% Parmetros de Denavit - Hartemberg del robot.


teta = [q(1); q(2)-pi/6; q(3)+pi/3; q(4)-pi/6; q(5)];
d = [0 ; 0 ; 0 ; 0 ; 0 ];
a = [ L6d; L5d ; L4d ; L3d; 0 ]; % las longitudes en mm.
alfa = [-pi/2; 0 ; 0 ; pi/2; 0];

% Matrices de Transformacin Homognea entre sisstemas de Coordenadas


% consecutivos.
T43d = denavit(teta(1), d(1), a(1), alfa(1));
T32d = denavit(teta(2), d(2), a(2), alfa(2));
T21d = denavit(teta(3), d(3), a(3), alfa(3));
T10d = denavit(teta(4), d(4), a(4), alfa(4));
T00d = denavit(teta(5), d(5), a(5), alfa(5));

%Matriz Tranformacin de la Base del Pie a la Articulacin1


T54d=[1 0 0 L7d;
0 1 0 0;
0 0 1 0;
0 0 0 1];
% Matriz de Tranformacin de la Cadera.
T0Cd =[1 0 0 L2d;
0 0 -1 -L1d;
0 1 0 0;
0 0 0 1];
% Matriz de tranformacin del primer al ltimo sistema de Coordenadas.

T5dc = T54d*T43d*T32d*T21d*T10d*T00d*T0Cd;
%T5dc =[T5dc(1,4);T5dc(2,4);T5dc(3,4)];
end

B.1.3. - Funcin para el clculo de la cinemtica directa- Pie flotante respecto a la


cadera.
% Function Cinemtica Directa de Robot bpedo.
% Elaborado by Josmell Alva Alcntara
% Fecha: 16-01-2016
% University National of Trujillo - Ing. Mechatronics.
% -----------------------------------------------------------------------
--

% Cinemtica del Pie Flotante a la Cadera.

function Tc5 = directkinematicBiped_2(q)

% Longitudes de los Eslabones del Robot.


L7=17.4;
L6=46.8;
L5=47.3;
L4=64;
L3=47.4;
L2=28.8;
L1=45.2;

146
UNIVERSIDAD NACIONAL DE TRUJILLO

% Parmetros de Denavit - Hartemberg del robot.


teta1 = [q(1); q(2)+pi/6; q(3)-pi/3; q(4)+pi/6; q(5)];
d1 = [0 ; 0 ; 0 ; 0 ; 0 ];
a1 = [ L3; L4 ; L5 ; L6; L7 ]; % las longitudes en mm.
alfa1 = [pi/2; 0 ; 0 ; -pi/2; 0];

% Matrices de Transformacin Homognea entre sisstemas de Coordenadas


% consecutivos.
T01 = denavit(teta1(1), d1(1), a1(1), alfa1(1));
T12 = denavit(teta1(2), d1(2), a1(2), alfa1(2));
T23 = denavit(teta1(3), d1(3), a1(3), alfa1(3));
T34 = denavit(teta1(4), d1(4), a1(4), alfa1(4));
T45 = denavit(teta1(5), d1(5), a1(5), alfa1(5));

% Matriz de Tranformacin de la Cadera.


TC0 =[-1 0 0 -L2;
0 0 1 0;
0 1 0 L1;
0 0 0 1];
% Matriz de tranformacin del primer al ltimo sistema de Coordenadas.
Tc5 =TC0*T01*T12*T23*T34*T45;

end

B.1.4.-Cinemtica Directa de la Pie flotante respecto a pie de apoyo.


% Functin Cinemtica Directa de Robot bpedo 10 GDL
% Elaborado by Josmell Alva Alcntara
% Fecha: 16-01-2016
% University National of Trujillo - Ing. Mechatronics.
% -----------------------------------------------------------------------
--

% Pie Flotante a Pie de Apoyo.


function A3=directkinematicBiped_3(qd,qi)
b1=[qd(1) qd(2) qd(3) qd(4) qd(5) ];
b2=[qi(1) qi(2) qi(3) qi(4) qi(5)];

A1=directkinematicBiped(b1);
A2=directkinematicBiped_2(b2);
A3=A1*A2;

A3=[A3(1,4); A3(2,4); A3(3,4)];


end

b.1.5 funcin que clcula los parmetros de Denavit- hartemberg.

%DENAVIT Matriz de transformacion homogenea.


% DH=DENAVIT(TETA,D,A,ALFA) Devuelve la matriz de transformacion
homogenea
% 4x4 a partir de los paramatros de Denavit Hartemberg.
%University National of Trujillo - Ing. Mechatronics.
%ELaborado:Josmell Henry Alva alcntara.

147
UNIVERSIDAD NACIONAL DE TRUJILLO
%Fecha:18/04/2015
%------------------------------------------------------------------------
--
function dh=denavit(theta,d,a,alfa)
%clc, clear all
%syms theta alfa a d

Tztheta =[cos(theta) -sin(theta) 0 0;


sin(theta) cos(theta) 0 0;
0 0 1 0;
0 0 0 1];

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;

B.2 Cinemtica Inversa:


B.2.1.- Cinemtica inversa de la cadera respecto al pie de apoyo.
% ALGORITMO DE CINEMATICA INVERSA ROBOT BIPEDO 10GDL - METODO NUMERICO
% UNIVERSITY NATIONAL OF TRUJILLO - ING. MECHATRONICS
% ELABORADO JOSMELL ALVA ALCNTARA.
% LA CADERA RESPECTO AL PIE DE APOYO
% FECHA: 18/10/2016
%------------------------------------------------------------------------
--
% P: Vector de posicion (Px;Py;pZ)
function Q=InversekinematicBiped_1(P)

%valores Iniciales de la Longitud (mm):


L1= 45.2;
L2= 28.8;
L3= 47.4;
L4= 64;
L5= 47.3;
L6= 46.8;
L7= 17.4;

% Valores Iniciales de las articulaciones.


q5d=0;
q4d=0;

148
UNIVERSIDAD NACIONAL DE TRUJILLO
q3d=0;
q2d=0;
q1d=0;

dx=0.0001; %variacion para el calculo de la derivada discreta.

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
UNIVERSIDAD NACIONAL DE TRUJILLO
0;
0;
0;
0;
-1;
0;
1;
0;
P(1);
P(2);
P(3)];
% Error:
error=Fsp-F;
s=0;
for i=1:12
s=s+abs(error(i));
end
if s<0.0001
break;
end

dp5=q5d+dx;
dp4=q4d+dx;
dp3=q3d+dx;
dp2=q2d+dx;
dp1=q1d+dx;

% F Se clcula los Jacobianos numricamente.

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
UNIVERSIDAD NACIONAL DE TRUJILLO
J(5,4)=(F5HCPA(q5d,q4d,q3d,dp2,q1d)-F5HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(5,5)=(F5HCPA(q5d,q4d,q3d,q2d,dp1)-F5HCPA(q5d,q4d,q3d,q2d,q1d))/dx;

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
UNIVERSIDAD NACIONAL DE TRUJILLO
J(12,4)=(F12HCPA(q5d,q4d,q3d,dp2,q1d)-
F12HCPA(q5d,q4d,q3d,q2d,q1d))/dx;
J(12,5)=(F12HCPA(q5d,q4d,q3d,q2d,dp1)-
F12HCPA(q5d,q4d,q3d,q2d,q1d))/dx;

% 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)];

% Matriz Pseudoinversa por el Teorema de Moore - Penrose


%dtheta=J'*(J*J')^-1*error;
%dtheta=pinv(J)*error;
%METODO LEVENVERG - MARQUARDT.
landa=1;
dtheta=J'*(J*J'+ eye(12)*landa)^-1*error;

q5d=q5d+dtheta(1);
q4d=q4d+dtheta(2);
q3d=q3d+dtheta(3);
q2d=q2d+dtheta(4);
q1d=q1d+dtheta(5);

end

% Estanderizacion de los angulos.


q11d=q1d-round(q1d/(2*pi))*2*pi;
q11ds=q11d*180/pi;
q22d=q2d-round(q2d/(2*pi))*2*pi ;
q22ds=q22d*180/pi;
q33d=q3d-round(q3d/(2*pi))*2*pi;
q33ds=q33d*180/pi;
q44d=q4d-round(q4d/(2*pi))*2*pi;
q44ds=q44d*180/pi;
q55d=q5d-round(q5d/(2*pi))*2*pi;
q55ds=q55d*180/pi;
Q=[q55d; q44d; q33d; q22d; q11d];
Q1=[q5d; q4d; q3d; q2d; q1d];
Q2=[q55ds; q44ds; q33ds; q22ds; q11ds]; % en Sexagesimal.

end

B.2.2.- Cinemtica inversa del pie flotante respecto al pie de apoyo.


% ALGORITMO DE CINEMTICA INVERSA ROBOT BIPEDO 10GDL - METODO NUMERICO
% UNIVERSITY NATIONAL OF TRUJILLO - ING. MECHATRNICS
% ELABORADO JOSMELL ALVA AlCANTARA.

152
UNIVERSIDAD NACIONAL DE TRUJILLO
% FECHA: 18/10/2016
% PIE FLOTANTE AL PIE DE APOYO
%------------------------------------------------------------------------
--
% P: Vector de posicion (Px;Py;PZ)
function Q=InversekinematicBiped_3(P)

%valores Iniciales de la Longitud (mm):


L1= 45.2;
L2= 28.8;
L3= 47.4;
L4= 64;
L5= 47.3;
L6= 46.8;
L7= 17.4;
% Valores iniciales de angulos de la cadera respecto al pie de apoyo
%calculos por la cinematica inversa.
%angulos (rad)
q1d=0; %-0.0134;
q2d=0; %-0.4386;
q3d=0; %1.4693;
q4d=0; %-1.0090;
q5d=0; %0.0013;
% valores iniciales de angulos de pie Flotante respecto a la cadera.
%longitudes (mm)
%L1i= 45.2;
%L2i= 28.8;
%L3i= 47.4;
%L4i= 64;
%L5i= 47.3;
%L6i= 46.8;
%L7i= 17.4;
% angulos (rad)
q1i=0;
q2i=0;
q3i=0;
q4i=0;
q5i=0;

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
UNIVERSIDAD NACIONAL DE TRUJILLO
*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(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 +
q5d)/2 - cos(q2d + q3d + q4d - q5d)/2)*(sin(q2i + q3i + q4i + q5i)/2 +
sin(q2i + q3i + q4i - q5i)/2) -
(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))*(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 + q3d + q4d)*(sin(q2i + q3i + q4i + q5i)/2 + sin(q2i +
q3i + q4i - q5i)/2) - (sin(q1d + q2d + q3d + q4d)/2 - sin(q1d - q2d - q3d
- q4d)/2)*(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(q1d + q2d + q3d +
q4d)/2 - cos(q1d - q2d - q3d -
q4d)/2)*(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(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));
(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) +

154
UNIVERSIDAD NACIONAL DE TRUJILLO
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) +
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));
cos(q2d + q3d + q4d)*(cos(q2i + q3i + q4i + q5i)/2 - cos(q2i +
q3i + q4i - q5i)/2) + (cos(q1d + q2d + q3d + q4d)/2 - cos(q1d - q2d - q3d
- q4d)/2)*(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)) + (sin(q1d + q2d + q3d +
q4d)/2 - sin(q1d - q2d - q3d -
q4d)/2)*(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(q2i + q3i + q4i)*(sin(q2d + q3d + q4d + q5d)/2 + sin(q2d +
q3d + q4d - q5d)/2) - (sin(q1i + q2i + q3i + q4i)/2 - sin(q1i - q2i - q3i
- q4i)/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)) - (cos(q1i + q2i + q3i +
q4i)/2 - cos(q1i - q2i - q3i -
q4i)/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(q2i + q3i + q4i)*(cos(q2d + q3d + q4d + q5d)/2 - cos(q2d +
q3d + q4d - q5d)/2) - (cos(q1i + q2i + q3i + q4i)/2 - cos(q1i - q2i - q3i
- q4i)/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)) - (sin(q1i + q2i + q3i +
q4i)/2 - sin(q1i - q2i - q3i -
q4i)/2)*(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(q2d + q3d + q4d)*cos(q2i + q3i + q4i) - (sin(q1d + q2d + q3d
+ q4d)/2 - sin(q1d - q2d - q3d - q4d)/2)*(sin(q1i + q2i + q3i + q4i)/2 -
sin(q1i - q2i - q3i - q4i)/2) + (cos(q1d + q2d + q3d + q4d)/2 - cos(q1d -
q2d - q3d - q4d)/2)*(cos(q1i + q2i + q3i + q4i)/2 - cos(q1i - q2i - q3i -
q4i)/2);
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 -

155
UNIVERSIDAD NACIONAL DE TRUJILLO
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) -
(3^(1/2)*L5*cos(q1i)*cos(q2i)*cos(q3i))/2 +
(3^(1/2)*L5*cos(q1i)*sin(q2i)*sin(q3i))/2 -
L6*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i) -
L7*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) +
L7*cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) +
L7*cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) +
L7*cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin(q3i)) + (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 + (sin(q2d + q3d +
q4d + q5d)/2 + sin(q2d + q3d + q4d - q5d)/2)*((L7*sin(q2i + q3i + q4i +
q5i))/2 - (L5*cos(q2i + q3i))/2 + (L4*cos(q2i))/2 + (L7*sin(q2i + q3i +
q4i - q5i))/2 + L6*sin(q2i + q3i + q4i) + (3^(1/2)*L5*sin(q2i + q3i))/2 +
(3^(1/2)*L4*sin(q2i))/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)) -
(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))*(L1 + L3*sin(q1i) +
L7*cos(q1i)*sin(q5i) - (L4*sin(q1i)*sin(q2i))/2 +
(L5*cos(q2i)*sin(q1i)*sin(q3i))/2 + (L5*cos(q3i)*sin(q1i)*sin(q2i))/2 +
(3^(1/2)*L4*cos(q2i)*sin(q1i))/2 + L6*cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)
- L6*cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i) -
L6*cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i) -
L6*cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i) +
(3^(1/2)*L5*cos(q2i)*cos(q3i)*sin(q1i))/2 -
(3^(1/2)*L5*sin(q1i)*sin(q2i)*sin(q3i))/2 -
L7*cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
L7*cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) -
L7*cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i) +
L7*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i)) -
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 -
(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))*((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) -
(3^(1/2)*L5*cos(q1i)*cos(q2i)*cos(q3i))/2 +
(3^(1/2)*L5*cos(q1i)*sin(q2i)*sin(q3i))/2 -
L6*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i) -
L7*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) +

156
UNIVERSIDAD NACIONAL DE TRUJILLO
L7*cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) +
L7*cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) +
L7*cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin(q3i)) - (cos(q2d + q3d + q4d +
q5d)/2 - cos(q2d + q3d + q4d - q5d)/2)*((L7*sin(q2i + q3i + q4i + q5i))/2
- (L5*cos(q2i + q3i))/2 + (L4*cos(q2i))/2 + (L7*sin(q2i + q3i + q4i -
q5i))/2 + L6*sin(q2i + q3i + q4i) + (3^(1/2)*L5*sin(q2i + q3i))/2 +
(3^(1/2)*L4*sin(q2i))/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)) - (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))*(L1 + L3*sin(q1i) +
L7*cos(q1i)*sin(q5i) - (L4*sin(q1i)*sin(q2i))/2 +
(L5*cos(q2i)*sin(q1i)*sin(q3i))/2 + (L5*cos(q3i)*sin(q1i)*sin(q2i))/2 +
(3^(1/2)*L4*cos(q2i)*sin(q1i))/2 + L6*cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i)
- L6*cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i) -
L6*cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i) -
L6*cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i) +
(3^(1/2)*L5*cos(q2i)*cos(q3i)*sin(q1i))/2 -
(3^(1/2)*L5*sin(q1i)*sin(q2i)*sin(q3i))/2 -
L7*cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
L7*cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) -
L7*cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i) +
L7*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i));
cos(q2d + q3d + q4d)*((L7*sin(q2i + q3i + q4i + q5i))/2 -
(L5*cos(q2i + q3i))/2 + (L4*cos(q2i))/2 + (L7*sin(q2i + q3i + q4i -
q5i))/2 + L6*sin(q2i + q3i + q4i) + (3^(1/2)*L5*sin(q2i + q3i))/2 +
(3^(1/2)*L4*sin(q2i))/2) - (L4*cos(q3d + q4d))/2 - (sin(q1d + q2d + q3d +
q4d)/2 - sin(q1d - q2d - q3d - q4d)/2)*((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) -
(3^(1/2)*L5*cos(q1i)*cos(q2i)*cos(q3i))/2 +
(3^(1/2)*L5*cos(q1i)*sin(q2i)*sin(q3i))/2 -
L6*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i) -
L7*cos(q1i)*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i) +
L7*cos(q1i)*cos(q2i)*cos(q5i)*sin(q3i)*sin(q4i) +
L7*cos(q1i)*cos(q3i)*cos(q5i)*sin(q2i)*sin(q4i) +
L7*cos(q1i)*cos(q4i)*cos(q5i)*sin(q2i)*sin(q3i)) + (L5*cos(q4d))/2 -
(L2*(sin(q1d + q2d + q3d + q4d) - sin(q1d - q2d - q3d - q4d)))/2 -
L3*sin(q2d + q3d + q4d) + (cos(q1d + q2d + q3d + q4d)/2 - cos(q1d - q2d -
q3d - q4d)/2)*(L1 + L3*sin(q1i) + L7*cos(q1i)*sin(q5i) -
(L4*sin(q1i)*sin(q2i))/2 + (L5*cos(q2i)*sin(q1i)*sin(q3i))/2 +
(L5*cos(q3i)*sin(q1i)*sin(q2i))/2 + (3^(1/2)*L4*cos(q2i)*sin(q1i))/2 +
L6*cos(q2i)*cos(q3i)*cos(q4i)*sin(q1i) -
L6*cos(q2i)*sin(q1i)*sin(q3i)*sin(q4i) -
L6*cos(q3i)*sin(q1i)*sin(q2i)*sin(q4i) -
L6*cos(q4i)*sin(q1i)*sin(q2i)*sin(q3i) +

157
UNIVERSIDAD NACIONAL DE TRUJILLO
(3^(1/2)*L5*cos(q2i)*cos(q3i)*sin(q1i))/2 -
(3^(1/2)*L5*sin(q1i)*sin(q2i)*sin(q3i))/2 -
L7*cos(q2i)*cos(q5i)*sin(q1i)*sin(q3i)*sin(q4i) -
L7*cos(q3i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q4i) -
L7*cos(q4i)*cos(q5i)*sin(q1i)*sin(q2i)*sin(q3i) +
L7*cos(q2i)*cos(q3i)*cos(q4i)*cos(q5i)*sin(q1i)) + 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 ];

% Posicion a la que deseo moverse.


Fsp=[-1;
0;
0;
0;
-1;
0;
0;
0;
1;
P(1);
P(2);
P(3)];

% 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
UNIVERSIDAD NACIONAL DE TRUJILLO
J(2,4)=(F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(2,5)=(F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F2HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;

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
UNIVERSIDAD NACIONAL DE TRUJILLO
J(7,4)=(F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(7,5)=(F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F7HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;

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
UNIVERSIDAD NACIONAL DE TRUJILLO
J(12,4)=(F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,dp4,q5i)-
F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;
J(12,5)=(F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,dp5)-
F12HPAPF(q5d,q4d,q3d,q2d,q1d,q1i,q2i,q3i,q4i,q5i))/dx;

% la Matriz Jacobiana de F para la LInealizacion aplicando Taylor.

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)];

% Matriz Pseudoinversa por el Teorema de Moore - Penrose


%dtheta=J'*(J*J')^-1*error;
%dtheta=pinv(J)*error;
%METODO LEVENVERG - MARQUARDT.
landa=1;
dtheta=J'*(J*J'+ eye(12)*landa)^-1*error;

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;

Q=[q11i; q22i; q33i; q44i; q55i];


Qd=[q5d;q4d;q3d;q2d;q1d];

End

161
UNIVERSIDAD NACIONAL DE TRUJILLO
B.2.3 Algoritmo para generar los ngulos de la Cinemtica inversa en funcin a las
trayectorias propuestas.

% Generacin de ngulos para Aplicar la Cinemtica Inversa - Generacin


de trayectorias.
% Elaborado By Josmell Alva Alcntara.
% University National Of Trujillo - Ing. Mechatronics.
% Fecha: 26-01-2016
% -----------------------------------------------------------------------
----

% Angulos CADERA Respecto al PIE de Apoyo.


%Inicio de Marcha:
for i=1:101
qc_start2(:,i)=InversekinematicBiped_1(1000*PosCadera_start(:,i));

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

%En plena Caminata

%for i=1:101
% qpf2(:,i)=InversekinematicBiped_3(1000*PosPieF(:,i));
%end

162
UNIVERSIDAD NACIONAL DE TRUJILLO
B.3 Clculo de la Dinmica.
B.3.1 Se presenta el archivo Script de clculo de la dinmica
% DINAMICA DE NEWTON EULER ROBOT BIPEDO
% ING. MECATONICA - UNT
% FECHA 31/10/2016
% ELABORADO BY JOSMELL ALVA ALCANTARA
%------------------------------------------------------------------------
% DE PIERNA FLOTANTE A LA CADERA.
clc, clear all

syms theta d a alpha q1i q2i q3i q4i q5i L3 L4 L5 L6 L7 m1 m2 m3 m4 m5


syms qd1i qd2i qd3i qd4i qd5i qdd1i qdd2i qdd3i qdd4i qdd5i
syms pi g Mext Iext I1i I2i I3i I4i I5i b1 b2 b3 b4 b5

%----------------------------------------------------------------------
%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 Rotacin.
% ------------------------------------------------------------
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
UNIVERSIDAD NACIONAL DE TRUJILLO
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);
r65i=r56i';

% ------------------------------------------------------------
% 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
UNIVERSIDAD NACIONAL DE TRUJILLO
r00vp0 = [g; 0; 0] % aceleracion de la gravedad
r10vp1 = simplify(ri0vpi_r(r10i, r00vp0, r10wp1, r10w1, r10p1))
r20vp2 = simplify(ri0vpi_r(r21i, r10vp1, r20wp2, r20w2, r20p2))
r30vp3 = simplify(ri0vpi_r(r32i, r20vp2, r30wp3, r30w3, r30p3))
r40vp4 = simplify(ri0vpi_r(r43i, r30vp3, r40wp4, r40w4, r40p4))
r50vp5 = simplify(ri0vpi_r(r54i, r40vp4, r50wp5, r50w5, r50p5))

r60vp6 = simplify(ri0vpi_r(r65i, r50vp5, r60wp6, r60w6, r60p6));

% ------------------------------------------------------------
% 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
% ------------------------------------------------------------

r60f6 = ri0fi(r60a6, 0) % fuerza inicial de carga =0


r50f5 = ri0fi(r50a5, m5)
r40f4 = ri0fi(r40a4, m4)
r30f3 = ri0fi(r30a3, m3)
r20f2 = ri0fi(r20a2, m2)
r10f1 = ri0fi(r10a1, m1)

% ------------------------------------------------------------
% Par en el centro de masa de cada elemento
% ------------------------------------------------------------

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 = 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
UNIVERSIDAD NACIONAL DE TRUJILLO
% Pares articulares
% ------------------------------------------------------------

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 dinmica del robot bpedo 10GDL del pie flotante respecto a la cadera.
%Funcion de Dinmica de NEWTON -EULER para Robot bpedo 10 GDL - Pie
%Flotante a Cadera.
% University National Of Trujillo. Ing. Mechatronics.
% Elaborado Josmell H. Alva Alcntara.
% Fecha: 06-02-2017
%------------------------------------------------------------------------
--
%Ingreso:
% qi: Vector de Angulos de Posicin (rad)
% qvi: Vector de Velocidad Articular.
% qai: Vector de Aceleracin Articular.

function tau=DinamicaNE_PFC(qi,qvi,qai)

%----------------------------------------------------------------------
%parametro de Denavit-Hartenberg del robot
%----------------------------------------------------------------------
L3=0.0474; %Longitudes en Metros.

166
UNIVERSIDAD NACIONAL DE TRUJILLO
L4=0.064;
L5=0.0473;
L6=0.0468;
L7=0.0174;
theta=[qi(1); qi(2)+pi/6; qi(3)-pi/3; qi(4)+pi/6; qi(5) ];
d=[0; 0; 0; 0; 0];
a=[L3; L4; L5; L6; L7 ];
alpha=[pi/2; 0; 0;-pi/2; 0];
DH=[theta d a alpha];

%------------------------------------------------------------------------
% 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 Rotacin.
% ------------------------------------------------------------
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
UNIVERSIDAD NACIONAL DE TRUJILLO
r65i=r56i';

% ------------------------------------------------------------
% 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,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
UNIVERSIDAD NACIONAL DE TRUJILLO
% 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
% ------------------------------------------------------------

r60f6 = ri0fi(r60a6, 0); % fuerza inicial de carga =0


r50f5 = ri0fi(r50a5, m5);
r40f4 = ri0fi(r40a4, m4);
r30f3 = ri0fi(r30a3, m3);
r20f2 = ri0fi(r20a2, m2);
r10f1 = ri0fi(r10a1, m1);

% ------------------------------------------------------------
% 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
UNIVERSIDAD NACIONAL DE TRUJILLO
r30n3a =ri0nia(r34i, r40n4a, r40f4a, r30n3, r30f3, r40p3, r30p3,r30s3);
r20n2a =ri0nia(r23i, r30n3a, r30f3a, r20n2, r20f2, r30p2, r20p2,r20s2);
r10n1a =ri0nia(r12i, r20n2a, r20f2a, r10n1, r10f1, r20p1, r10p1,r10s1);

% ------------------------------------------------------------
% 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

B.3.3. La dinmica para el robot de la cadera respecto al pie de Apoyo.


%Funcion de Dinmica de NEWTON -EULER para Robot bpedo 10 GDL
% de la Cadera al Pie de Apoyo.
% University National Of Trujillo. Ing. Mechatronics.
% Elaborado Josmell H. Alva Alcntara.
% Fecha: 06-01-2017
%------------------------------------------------------------------------
--
%Ingreso:
% qd: Vector de Angulos de Posicin (rad)
% qvd: Vector de Velocidad Articular.
% qad: Vector de Aceleracin Articular.

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;

theta=[qd(1); qd(2)-pi/6; qd(3)+pi/3; qd(4)-pi/6; qd(5) ];


d=[0; 0; 0; 0; 0];
a=[L6; L5; L4; L3; 0 ];
alpha=[-pi/2; 0; 0; pi/2; 0];
DH=[theta d a alpha];

%------------------------------------------------------------------------
% Masa de cada elemento en Kg
%------------------------------------------------------------------------
m1=(0.1039+0.0876); %elemento L6 (izquierda)
m2=0.0876; %elemento L5 (izquierda)
m3=0.1045; %elemento L4 (izquierda)
m4=0.0876; %elemento L3 (izquierda)
m5=0; %elemento L2 (izquierda)(Se lo va Incluir en la Masa Externa)

170
UNIVERSIDAD NACIONAL DE TRUJILLO

% ------------------------------------------------------------
% 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 Rotacin.
% ------------------------------------------------------------
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);

%posicion del centro de masa del elemento i desde sistema de referenca i


base
r10s1 = [-L6/2;0;0];
r20s2 = [-L5/2;0;0];
r30s3 = [-L4/2;0;0];
r40s4 = [-L3/2;0;0];

171
UNIVERSIDAD NACIONAL DE TRUJILLO
r50s5 = [ 0; 0;0];
r60s6 = zeros(3,1);

% ------------------------------------------------------------
% 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
UNIVERSIDAD NACIONAL DE TRUJILLO
r30f3 = ri0fi(r30a3, m3);
r20f2 = ri0fi(r20a2, m2);
r10f1 = ri0fi(r10a1, m1);

% ------------------------------------------------------------
% 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
UNIVERSIDAD NACIONAL DE TRUJILLO
tau = [t_5d; t_4d ; t_3d; t_2d; t_1d];

end

B.3.4 Script para el clculo de los toques, velocidades y aceleraciones angulares.

% Torques , clculo de la Velocidad y Aceleraciones angulares.


% Calculos de Torques Generados en Robot 10GDL
% Elaborado by Josmell Alva Alcntara.
% Fecha 06-02-2016
%-----------------------------------------------------------
t=0.01;
%clculo de las Velocidades.
%Pierna Flotante a CADERA.

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

% Clculo 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

% Calculo de los Torques de la Dinmica inversa.


for i=1:101
torquesPF(:,i)=DinamicaNE_PFC(qpf2(:,i),vpf(:,i),apf(:,i));
end

%}
%------------------------------------------------------------------------
--
% DE CADERA A PIE DE APOYO.

%{
for i=1:101;
if i==1;
vcadera(5,i)=0;
end

174
UNIVERSIDAD NACIONAL DE TRUJILLO
if i>=2
vcadera(:,i)=(qcadera2(:,i)- qcadera2(:,i-1))/t;
end

end
% Clculo 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

% Calculo de los Torques de la Dinmica inversa.

for i=1:101

torquesCPA(:,i)=DinamicaNE_CPA(qcadera2(:,i),vcadera(:,i),acadera(:,i));

end

%}

B. 3.5 Script para las grficas de torques de pie flotante, velocidad y aceleraciones
articulares del robot bpedo.
% Graficas de Torques Pie Flotante, Velocidad y Aceleraciones ROBOT
BPEDO.
% University National of Trujillo - Ing. Mechatronics.
% Elaborado by Josmell Alva Alcntara.
% Fecha: 06-02-2017
%------------------------------------------------------------------------
--
t=0:0.01:1;

%PIE FLOTANTE A CADERA.

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
UNIVERSIDAD NACIONAL DE TRUJILLO
end
for i=1:101
torque3(i)=torquesPF(3,i);
vqiz3(i)=vpf(3,i);
aqiz3(i)=apf(3,i)/10;
Pot3i(i)= torquesPF(3,i)*vpf(3,i);
end
for i=1:101
torque4(i)=torquesPF(4,i);
vqiz4(i)=vpf(4,i);
aqiz4(i)=apf(4,i)/10;
Pot4i(i)= torquesPF(4,i)*vpf(4,i);
end
for i=1:101
torque5(i)=torquesPF(5,i);
vqiz5(i)=vpf(5,i);
aqiz5(i)=apf(5,i)/10;
Pot5i(i)= torquesPF(5,i)*vpf(5,i);
end

%Grfica de Torques.
plot(t,torque1,'r',t,torque2,'b',t,torque3,'g',t,torque4,'k',t,torque5,'m
')
title('ROBOT BIPEDO: GRFICO DE TORQUES PIE FLOTANTE - CADERA.')
ylabel('N*m')
xlabel('Tiempo')
legend('Torque 1i','Torque 2i','Torque 3i','Torque 4i','Torque 5i')

% Grfica 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 BPEDO: GRFICO DE VELOCIDADES PIE FLOTANTE-CADERA')

%Grfica de Aceleraciones.
figure
plot(t,aqiz1,'r',t,aqiz2,'b',t,aqiz3,'g',t,aqiz4,'k',t,aqiz5,'m')
legend('Aceleracin 1i','Aceleracin 2i','Aceleracin 3i','Aceleracin
4i','Aceleracin 5i')
ylabel('rad/s^2')
xlabel('Tiempo')
title('ROBOT BPEDO: GRFICO DE ACELERACIONES PE FLOTANTE-CADERA')

%Grfica 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 BPEDO: GRFICO DE POTENCIAS DEL PIE FLOTANTE')
%------------------------------------------------------------------------
--

176
UNIVERSIDAD NACIONAL DE TRUJILLO

%{
% 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

%Grfica de Torques.
plot(t,tq5d,'r',t,tq4d,'b',t,tq3d,'g',t,tq2d,'k',t,tq1d,'m')
title('ROBOT BIPEDO: GRFICO DE TORQUES CADERA A PIE DE APOYO')
ylabel('N*m')
xlabel('Tiempo')
legend('Torque 5d','Torque 4d','Torque 3d','Torque 2d','Torque 1d')

% Grfica 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 BPEDO: GRFICO DE VELOCIDADES CADERA A PIE DE APOYO')

%Grfica de Aceleraciones.
figure
plot(t,aqd5,'r',t,aqd4,'b',t,aqd3,'g',t,aqd2,'k',t,aqd1,'m')
legend('Aceleracin 5d','Aceleracin 4d','Aceleracin 3d','Aceleracin
2d','Aceleracin 1d')

177
UNIVERSIDAD NACIONAL DE TRUJILLO
ylabel('rad/s^2')
xlabel('tiempo')
title('ROBOT BPEDO: GRFICO DE ACELERACIONES CADERA A PIE DE APOYO')

%Grfica 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 BPEDO: GRFICO DE POTENCIAS CADERA A PIE DE APOYO')

%}

B.4.- Algoritmos para la Planificacin de Trayectorias.


B.4.1- Algoritmo que describe la generacin de trayectorias del robot bpedo de 10
GDL.
% Generacin de Trayectorias de Caminata Robot Bpedo 10GDL.
% University National of Trujillo - Ing. Mechatronics.
% Elaborado by Josmell Alva Alcntara.
% Fecha 25-11-2016
%------------------------------------------------------------------------
--
clc,clear all,close all
% Se considera Curva Spline Cbica.
% las Medidas estn metros.
%datos del Robot Bpedo
% hc = elevacion de la cadera

Lp=0.020; %Lp = Longitud de paso


h=0.23678; % h = altura de la cadera
hp=0.010; % hp = elevacion de la pierna flotante.
hc=0.005; % hc = elevacion de la cadera
Tp= 1 % Tp = tiempo empleado para cado paso
%------------------------------------------------------------------------
------------------------
% CADERA.
%------------------------------------------------------------------------
------------------------
% Eje "Z".
z0=-Lp/2;
z1= Lp/2;
v0=0;
v1=0;
Ti=Tp;
t=0:0.01:Tp;
zc_PS=z0+v0*t+[(3*(z1-z0)-Ti*(2*v0+v1))/Ti^2]*t.^2+ [(2*(z0-z1)+
Ti*(v0+v1))/Ti^3]*t.^3;
plot(t,zc_PS)
title('Planificacin de la Cadera en el Eje "Z"- PLANO SAGITAL');
grid on
xlabel('TP(s)')

178
UNIVERSIDAD NACIONAL DE TRUJILLO
ylabel('Z cadera(metros)')
%------------------------------------------------------------------------
---------------------------
%Eje "X":
%i)
x0=h;
x1=h+hc;
v0=0;
v1=0;
Ti=Tp/2;
t1=0:0.01:Tp/2;
xc1_PS=x0+v0*t1+[(3*(x1-x0)-Ti*(2*v0+v1))/Ti^2]*t1.^2+ [(2*(x0-x1)+
Ti*(v0+v1))/Ti^3]*t1.^3;

%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('Planificacin 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
UNIVERSIDAD NACIONAL DE TRUJILLO

%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('Planificacin 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
UNIVERSIDAD NACIONAL DE TRUJILLO
title('PLANIFICACIN DE LA PIERNA FLOTANTE EN EL EJE "Z" - PLANO
SAGITAL')
grid on
xlabel('TP(s)')
ylabel('Z PieFlotante(metros)')
%------------------------------------------------------------------------
---------------------------------
% En el Eje "X"
x0=0;
x1=hp;
v0=0;
v1=0;
Ti=Tss/2;
t1=0:0.01:Tss/2;
xpf_Ps=x0+v0*t1+[(3*(x1-x0)-Ti*(2*v0+v1))/Ti^2]*t1.^2+ [(2*(x0-x1)+
Ti*(v0+v1))/Ti^3]*t1.^3;

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('PLANIFICACIN 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
UNIVERSIDAD NACIONAL DE TRUJILLO
ypf1_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;

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('Planificacin 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('Planificacin Cadera en el EJE "Z" - INICIO DE MARCHA');
grid on
xlabel('TP(s)')
ylabel('Z cadera (metros)')

%PIERNA FLOTANTE
z0=0;

182
UNIVERSIDAD NACIONAL DE TRUJILLO
z1= Lp;
v0=0;
v1=0;
Ti=Tp;
zpf_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,zpf_start)
title('Planificacin Pie flotante en el EJE "Z"-INICIO DE MARCHA');
grid on
xlabel('TP(s)')
ylabel('Z Pierna Flotante (metros)')
%------------------------------------------------------------------------
---
%FIN DE MARCHA.
%CADERA
z0=-Lp/2;
z1= 0;
v0=0;
v1=0;
Ti=Tp;
zc_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,zc_end)
title('Planificacin de la Cadera en el EJE "Z" - FIN DE MARCHA');
grid on
xlabel('TP(s)')
ylabel('Z cadera (metros)')

%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('Planificacin 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
UNIVERSIDAD NACIONAL DE TRUJILLO
%RESUMEN INICIO - FIN DE MARCHA:
PosCadera_start = [xc_ps; yc_pf; zc_start]; %En Metros
PosPieF_star = [xpf_ps; ypf_pf;zpf_start];
PosCadera_end = [xc_ps; yc_pf; zc_end ];
PosPieF_end = [xpf_ps; ypf_pf; zpf_end ];

B.4.2 Algoritmo para la generacin de ngulos para aplicar la cinemtica inversa.

% Generacin de Angulos para Aplicar la Cinemtica Inversa - Generacin


de trayectorias.
% Elaborado By Josmell Alva Alcntara.
% University National Of Trujillo - Ing. Mechatronics.
% Fecha: 26-01-2016
% -----------------------------------------------------------------------
----

% Angulos CADERA Respecto al PIE de Apoyo.


%Inicio de Marcha:
for i=1:101
qc_start2(:,i)=InversekinematicBiped_1(1000*PosCadera_start(:,i));

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

%En plena Caminata

%for i=1:101
% qpf2(:,i)=InversekinematicBiped_3(1000*PosPieF(:,i));
%end

184
UNIVERSIDAD NACIONAL DE TRUJILLO
B.4.3.- Script que permite graficar los ngulos dados para la cinemtica inversa.
%Grfica el movimiento de cada Angulo dada por la Cinemtica Inversa.
%Elaborado by Josmell Alva Alcntara.
%University National of Trujillo- Ing. Mechatronics.
%Fecha 30-12-2016
%------------------------------------------------------------------------

t=0:1:100; % vector para graficar.


% de cadera al pie de apoyo.
%Plena Marcha.
n=101;
for i=1:n
q5d(i)=qcadera2(1,i);
end
%q5d=q5d*180/pi; %de radianes a Sexagesimales.
plot(t,q5d)
title('Pierna apoyo q5d - Plena Marcha.');
xlabel(' Nmero de divisiones tiempo (s)')
ylabel('q5d (radianes)')

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(' Nmero 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(' Nmero 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(' Nmero 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
UNIVERSIDAD NACIONAL DE TRUJILLO
figure
plot(t,q1d)
title('Pierna de apoyo q1d- Plena Marcha.');
xlabel(' Nmero de divisiones tiempo (s) ')
ylabel('q1d (radianes)')

%Pie Flotante al Pie de Apoyo.


for i=1:n
q1i(i)=qpf2(1,i);
end
%q1i=q1i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q1i)
title('Pierna flotante q1i- Plena Marcha.');
xlabel(' Nmero de divisiones tiempo (s) ')
ylabel('q1i (radianes)')

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(' Nmero 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(' Nmero 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(' Nmero 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(' Nmero de divisiones tiempo (s)')
ylabel('q5i (radianes)')

186
UNIVERSIDAD NACIONAL DE TRUJILLO

%{
%----------------------------------------------------------------------
%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');

%Pie Flotante al Pie de Apoyo.


for i=1:n
q1i(i)=qpf_start2(1,i);
end
q1i=q1i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q1i)
title('q1i en radianes - Inicio de Marcha.');

for i=1:n
q2i(i)=qpf_start2(2,i);

187
UNIVERSIDAD NACIONAL DE TRUJILLO
end
q2i=q2i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q2i)
title('q2i en radianes - Inicio de Marcha.');

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
UNIVERSIDAD NACIONAL DE TRUJILLO
title('q3d en radianes - Fin de Marcha.');

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.');

%Pie Flotante al Pie de Apoyo.

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
UNIVERSIDAD NACIONAL DE TRUJILLO
q5i=q5i*180/pi; %de radianes a Sexagesimales.
figure
plot(t,q5i)
title('q5i en radianes - Fin de Marcha.');

%}

B.5.- Algoritmos para el Clculo del ZMP.

B.5.1.- Funcin que permite calcular la velocidad y aceleracin del COG. Del robot
bpedo.

%Funcin para el Clculo de la Velocidad y Aceleracin del COG.


% ELaborado by Josmell Alva Alcntara.
% University National of Trujillo - Ing. Mechatronics
% Fecha 07-02-2016
%------------------------------------------------------------------------
---------

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;

%Parametros de Denavit - Hartenberg


theta=[qd(1); qd(2)-pi/6; qd(3)+pi/3; qd(4)-pi/6; qd(5)];
d=[0 ; 0; 0; 0; 0];
a=[L6; L5; L4; L3; 0];
alfa=[-pi/2; 0; 0; pi/2; 0];

MDH1=[theta d a alfa]; % muestra la tabla de matriz de transformacion


Homogenea

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));

T40d=T43d*T32d*T21d*T10d*T00d; % calculo intermedio paramatros de


Denavit- H

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
UNIVERSIDAD NACIONAL DE TRUJILLO
0 1 1 -L1;
0 0 1 0;
0 0 0 1]; % matriz de transformacion hacia la cadera.

% Matriz de transformacion de la cadera respecto al pie de apoyo.


T5dC=T54d*T40d*T0Cd;
P=[T5dC(1,4);T5dC(2,4);T5dC(3,4)];

% Matriz Qi para una articulacion de revolucion.

Qi=[0 -1 0 0;
1 0 0 0;
0 0 0 0;
0 0 0 0];
% Velocidad de cada Articulacion:

% Velocidad de la cadera respecto al pie de apoyo.


v5C=(T54d*Qi*T43d*T32d*T21d*T10d*T00d*T0Cd*qvd(1) +...
T54d*T43d*Qi*T32d*T21d*T10d*T00d*T0Cd*qvd(2) +...
T54d*T43d*T32d*Qi*T21d*T10d*T00d*T0Cd*qvd(3) +...
T54d*T43d*T32d*T21d*Qi*T10d*T00d*T0Cd*qvd(4) +...
T54d*T43d*T32d*T21d*T10d*Qi*T00d*T0Cd*qvd(5))*iri;

Vcmx= v5C(1,1);
Vcmy= v5C(2,1);
Vcmz= v5C(3,1);

%ACELERACION LINEAL DEL COG :

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 clculo del ZMP y las grficas de velocidades y aceleraciones del
COG.

%Clculo del ZMP y grficas de velocidades y aceleraciones de COG.


% Elaborado by Josmell alva Alcntara
% University National of Trujillo - Ing. Mechatronics.
% Fecha: 01-02-2016
%------------------------------------------------------------------------
-----

191
UNIVERSIDAD NACIONAL DE TRUJILLO

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 Aceleracin 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 Bpedo')
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('Aceleracin en m/s')
title('Aceleracion del COG - Robot Bpedo')
legend('Aceleracin en el Eje X',' Aceleracin en el Eje Y','Aceleracin
en el Eje Z')

% Calculos de los Parmetros de ZMP.


m=0.992; %masa total de Bpedo (incluye servomotores) en Kg.
g=9.81; % Acelaracin de la Gravedad.
Hy=0; %Momento angular COG = Iy*alfay, siendo alfay= aceleracion
angular en el eje y.
Hz=0; %Momento angular COG = Iz*alfaz, siendo alfaz= aceleracion
angular 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('Clculo del ZMP del Robot Bpedo')
xlabel(' Eje Y(m)')
ylabel('Eje Z(m)')
axis([-0.030,0.035,-0.060,0.060])

192
UNIVERSIDAD NACIONAL DE TRUJILLO

C.- PLANOS DEL ROBOT BPEDO.


Todos los planos fueron realizados en el Software de Solidworks2014.

193
UNIVERSIDAD NACIONAL DE TRUJILLO

194
UNIVERSIDAD NACIONAL DE TRUJILLO

195
UNIVERSIDAD NACIONAL DE TRUJILLO

196
UNIVERSIDAD NACIONAL DE TRUJILLO

197
UNIVERSIDAD NACIONAL DE TRUJILLO

198
UNIVERSIDAD NACIONAL DE TRUJILLO

199
UNIVERSIDAD NACIONAL DE TRUJILLO

200
UNIVERSIDAD NACIONAL DE TRUJILLO
C.1 Masa de los eslabones del Robot:

m1 = 0.0876 Kg.
m2 = 0.1045 Kg.
m3 = 0.0876 Kg.
m4 = 0.0876 Kg.
m5 = 0.1039 Kg

Las masa estn 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.

La masa de la cadera = 0.0485Kg

La masa total del Bpedo es = 0.992 Kg.

Las masas dadas del robot bpedo est calculado en base al material Aleacin de Aluminio 1060.

201

También podría gustarte