Modelado y control de un robot bípedo basado en …posgrado.lapaz.tecnm.mx/uploads/archivos/...En...

TECNOLOGICO NACIONAL DE MEXICOInstituto Tecnologico de La Paz

INSTITUTO TECNOLOGICO DE LA PAZDIVISION DE ESTUDIOS DE POSGRADO E INVESTIGACION

MAESTRIA EN SISTEMAS COMPUTACIONALES

MODELADO Y CONTROL DE UN ROBOT BIPEDO

BASADO EN CONCEPTOS DE ENERGIA

QUE PARA OBTENER EL GRADO DE

MAESTRO EN SISTEMAS COMPUTACIONALES

PRESENTA:

DANIEL ARMANDO HIRALES VALLES

DIRECTOR DE TESIS:

DR. JESUS ALBERTO SANDOVAL GALARZA

LA PAZ, BAJA CALIFORNIA SUR, MEXICO, DICIEMBRE 2017.

Blvd. Forjadores de B. C. S. #4720, Col. 8 de Oct. 1era. Seccion C. P. 23080La Paz, B. C. S. Conmutador (612) 121-04-24, Fax: (612) 121-12-95

www.itlp.edu.mx

Daniel A. Hirales Valles: Modelado y control de un robot bípedo basado en conceptos deenergía, Tecnológico Nacional de México-Instituto Tecnológico de La Paz, ©La Paz, BCS; México, Diciembre 2017

Scanned by CamScanner

Si yo he sido capaz de ver más lejos,es porque me encontraba parado sobre los hombros de Gigantes.

– Bernardo de Chartres

Las cosas más triviales, se vuelven fundamentales.

– Mario Benedetti

D E D I C AT O R I A

Este trabajo deseo dedicarlo en un modo general, a todas aquellas personas quecreen en mí, (tal es el caso de mi Apa, mi tía Paty, tío Geras, Vic, Willy, mis herma-nos Javier y Gerardo, a pesar de ya no estar con nosotros, a mi abuela María, una mujertan creativa y divertida, a Juan Torres Durán, Ing. Othoniel Amador), y mis alumnos deElectrómecanica.

De forma particular, proyecto un haz de luz especial, hacia mi Madre, que siempresupo engañarme para que yo dedicara mi tiempo a los estudios, porque decía (ysigue diciendo) ver en mi, la capacidad para realizar todo lo que me propusiera, apesar de que yo era ciego, ante la belleza del aprendizaje.

Con merito más que desarrollado, también se lo dedico a mi Director de tesisDr. Jesús Sandoval Galarza, que nunca me soltó y siempre me guió por el senderoadecuado.

De igual forma, planto estas lineas para Solange, mi esposa, que a pesar de haberletocado la mitad de este camino, supo darme mi espacio, comprensión y apoyo paraculminar este objetivo, caminando conmigo hombro a hombro.

cuando bebas agua,siempre recuerda la fuente

proverbio chino

A G R A D E C I M I E N T O S

Mi primer agradecimiento va dirigido al mecanismo universal que rige el funciona-miento del todo, o en su caso al matemático supremo que tiene todo calculado, hastalas perturbaciones al sistema, ya sea para comodidad o incomodidad de los que losentimos.

Agradezco al Tecnológico Nacional de México a través del Instituto Tecnológico deLa Paz por brindarme las facilidades para desarrollar este posgrado, otorgándomeuna beca comisión.

En un plano más terrenal, agradezco a mi director de tesis Dr.Jesús Alberto SandovalGalarza; no sólo por permearme de su vasto conocimiento, sino por la pacienciaque me tuvo y mantuvo durante este proceso. Me enseñó la pulcritud, eleganciay elocuencia con la que se deben manejar los detalles de redacción, los conceptosmatemáticos y la objetividad del contenido, sobre todo, la pasión con la que se debehacer un trabajo de investigación.

A los integrantes del comité tutorial, Dr. Israel Santillan y Dr. Marcos Castro, por susatenciones, aportaciones y consejos.A la planta docente de esta maestría, por darme sus enseñanzas.A mis hermanos esclavos; MSC. Cesar Higuera Verdugo, Ing. Abigail Chargoy y MSC.Diego García Molleda, por el apoyo para llegar a comprender estos conceptos y tra-bajar tan plácidamente en equipo. A mis compañeros de generación: Raúl(que nosdesvelamos juntos haciendo tareas), Marizcal(que me ayudo con Matlab® y Solid-works ®), Palafox, Hugo, Cecilia y Ángel, así como al Ing. Jerónimo Moyron Durán, MSC.José Luis Gómez Torres, MSC. Artemio Morales que en algún momento me dedicaronde su tiempo para explicarme algún concepto que no lograba comprender.

Por supuesto, un agradecimiento especial a la MSC. Iliana Castro, por su calidad ycalidez humana; su siempre pronta y positiva ayuda en cualquier detalle que sabiaque ella podía resolver (es la onda!!!).

Fuera de las instalaciones del Tecnológico, claro que también hay personas a lasque les debo mi gratitud. Primero, a mi Madre, quien no sólo me apoyo moralmente,

sino que en repetidas ocasiones lo hizo de manera económica.A mi esposa Solange, que desde que estamos juntos, me ha dado comprensión, por suapoyo a los problemas domésticos, por darme tiempo para que yo trabaje cómodo,porque me preparaba el desayuno, y por las veces en que fue necesario que mepreparara comida extra para llevar y jamás hacerme una escena de celos por hablarmás de Hamilton y ese mentado Robot bípedo tipo compás, que de cualquier otrotema; en resumen, por ser asintóticamente estable de manera global, en el equilibriode su paciencia.El resto de mi familia también ha jugado un papel primordial, ya que sus "¿comovas?"me hacían sentir que les importaba lo que hacia, a pesar de que después deexplicarles, cambiábamos de tema a asuntos más triviales.También me parece justo agradecer a quien me apoyo en el inicio, que zarpó conmigo,pero el viento empujo sus velas en otra dirección, KPMA.

Por ultimo, es mi deseo mostrar mi gratitud a todos los que forman este frondosoárbol de conocimiento, y que sin ellos yo no seria la pequeña rama que soy. Dr. RafaelKelly, Dr. Víctor Santibañez, Dr. Jesús Sandoval, Dr. Romeo Ortega, Dr. Víctor S. deLeón, MSC. César Higuera, Dr. Leornard Euler, Dr. Joseph Louis de Lagrange, DrWilliam Hamilton, Dr. Aleksandr Lyapunov, y a su vez, a quienes inspiraron a losmencionados. A todos MUCHAS GRACIAS.

R E S U M E N

En este trabajo se presenta un análisis de los efectos de la fricción dinámica en unmodelo de Robot Bípedo Tipo Compás. Específicamente, se diseñó un esquema decontrol con compensación de fricción dinámica para mantener la marcha del robotcon fricción dinámica en la articulación actuada. El controlador fue diseñado conuna extensión del método IDA PBC (Control Basado en Pasividad con Interconexióne Inyección de Amortiguamiento) el cual maneja conceptos de energía. Para el diseñodel compensador de fricción se incorporó uno de los modelos más simples de friccióndinámica: el modelo de Dahl. Se presentan resultados de simulación sobre un modelode robot bípedo tipo compás para validar los resultados.

A B S T R A C T

In this thesis an analysis on the dynamic friction effects at the actuated joint ofa Compass Gait Biped Robot is presented. Specifically, a control scheme with dy-namic friction compensation was designed in order to maintain the locomotion ofthe Compass Gait Biped Robot. An extension of the IDA-PBC methodology (Inter-connection and Damping Assignment - Passivity Based Control) was employed todesign the control law, wich handles energy concepts. In order to design the dy-namic friction compensator was incorporated the Dahl friction model. Simulationresults on the Compass Gait Biped Robot model are shown to validate the proposedcontrol scheme.

Í N D I C E G E N E R A L

i perfil del proyecto

1 introducción 2

1.1 Antecedentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2 Motivación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.3 Descripción del problema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.4 Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.4.1 Objetivo general . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.4.2 Objetivos específicos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.5 Hipótesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.6 Alcances y limitaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.6.1 Alcances . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.6.2 Limitaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.7 Organización de la tesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

ii el robot bípedo y su modelo matemático

2 el robot bípedo tipo compás 10

2.1 Modelado matemático . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.1.1 El robot bípedo como robot manipulador . . . . . . . . . . . . . . . . . . . 11

2.1.2 Modelo dinámico con coordenadas en ambos pies . . . . . . . . . . . . . . 16

2.2 Relación entre dos modelos dinámicos del robot bípedo tipo compás . . . . 21

iii el método ida-pbc con fricción dinámica

3 control con compensación de fricción dinámica para el ro-bot bípedo tipo compás , mediante la metodologia ida-pbc 25

3.1 Diseño de la ley de control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.2 Extensión del IDA-PBC para el control de un Robot Bípedo Tipo Compás . 33

3.3 Dinámica deseada en lazo cerrado y ley de control . . . . . . . . . . . . . . 34

3.4 Observador de fricción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

iv resultados

4 simulaciones 37

4.1 IDA-PBC sin fricción viscosa . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.2 IDA-PBC con fricción dinámica . . . . . . . . . . . . . . . . . . . . . . . . . . 38

4.3 IDA-PBC con compensación de fricción dinámica . . . . . . . . . . . . . . . 40

índice general x

v comentarios finales

5 conclusiones 43

5.1 Trabajo futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

vi apéndice

a código matlab® 45

a.1 Código del modelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

a.2 Código del control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

a.3 Codigo de animacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

a.4 Codigo trap-101 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

Í N D I C E D E F I G U R A S

Figura 1.1 Fotografía de un fragmento de la patente de G. Fallis . . . . . . 2

Figura 1.2 Diagrama que muestra el proceso evolutivo de investigacióndel Robot Asimo desarrollado por Honda . . . . . . . . . . . . . 4

Figura 1.3 Robot Atlas, diseñado y desarrollado por Boston Dynamics . . 4

Figura 1.4 Robots bípedos desarrollados por centros de investigación oinstituciones académicas . . . . . . . . . . . . . . . . . . . . . . . 6

Figura 2.1 Diagrama del robot bípedo tipo compás . . . . . . . . . . . . . . 10

Figura 2.2 Diagrama de cuerpo libre, del robot bípedo con coordenadasen pie de soporte y cadera, q1 y q2 respectivamente . . . . . . . 12

Figura 2.3 Robot bípedo en su configuración clásica: θs y θns . . . . . . . . 17

Figura 2.4 Relación geométrica entre ambos modelos . . . . . . . . . . . . . 22

Figura 4.1 Ciclos límite del robot bípedo sin fricción viscosa. . . . . . . . . 38

Figura 4.2 Imagen de la animación del robot bípedo sin fricción despla-zandose sobre una pendiente φ = 3° . . . . . . . . . . . . . . . . 38

Figura 4.3 Ciclos límite de la marcha del robot bípedo, con fricción diná-mica sólo en la coordenada actuada. . . . . . . . . . . . . . . . . 39

Figura 4.4 Imagen del instante en la animación en que el robot bípedopierde la marcha por los efectos de la fricción . . . . . . . . . . . 40

Figura 4.5 Ciclo límite de la marcha del robot bípedo, con compensaciónde fricción dinámica sólo en la coordenada actuada. . . . . . . . 41

Figura 4.6 Instante de la animación de la marcha del robot bípedo concompensación de fricción dinámica en la articulación actuada . 41

Í N D I C E D E TA B L A S

Tabla 2.1 Párametros del robot bípedo tipo compás . . . . . . . . . . . . . 11

Tabla 4.1 Parámetros del modelo de fricción de Dahl del robot bípedotipo compás y la ganancia del controlador (3.51) . . . . . . . . . 39

Parte I

P E R F I L D E L P R O Y E C T O

1I N T R O D U C C I Ó N

El hombre, en la búsqueda por satisfacer distintas necesidades, ha abordado elestudio de movimientos que se asemejen a los de los seres vivos, en especial a loshumanos; desembocando en el desarrollo de la robótica como ciencia, siendo éstaun compendio de disciplinas de la ingeniería, se puede deducir que su campo de in-vestigación es extenso. Dentro de este campo, se encuentran los robots bípedos, quevaliéndose de diversas estrategias y métodos de control, coinciden en el objetivo depropiciar la marcha de este sistema. Al estudiar la locomoción bípeda (actividad enapariencia trivial para una gran mayoría de personas), se entra en un campo extensoque permite entender mejor la marcha humana y las estrategias de control, con la fi-nalidad de vincular estos conocimientos en aplicaciones médicas, como lo puede serpara el diseño de prótesis, exoesqueletos para rehabilitación muscular o de uso coti-diano; también en un ambiente lúdico (juguetes educativos, o animaciones virtualespara videojuegos o cine); de desarrollo científico o civil, para propiciar el acceso enáreas de riesgo para el humano; o en su defecto, en el área militar.

(a) Juguete bipedo diseñado por GeorgeFallis en 1888

(b) Configuración interna del juguetebípedo

Figura 1.1: Fotografía de un diagrama de la patente de G. Fallis

1.1 antecedentes 3

1.1 antecedentes

A finales del siglo XIX, en 1888, George Fallis, patentó un muñeco de juguete quese desplazaba caminando hacia abajo por una rampa, el cual es considerado como unbípedo pasivo, por valerse de dos extremidades y no necesitar de una fuente externade energía mas que la gravedad, para generar la marcha. En la Figura 1.1a y 1.1b semuestran los esquemas descriptivos de la patente. En 1920 se dió origen al vocabloRobot, acuñado por el escritor Karel Capet, y dos décadas más tarde, Issac Assimovintrodujo al uso popular el término Robótica empleado como el desarrollo de unatécnica enfocada a la creación de robots.

Durante las décadas posteriores existieron diversos trabajos, sobre todo, enfocadosen la creación de brazos manipuladores u otros de tipo caminantes, como el de Ge-neral Electric en 1967, que construyó un robot tetrápodo llamado Walking Truck. En1984, T.A. McMahon [25], observó que la marcha del juguete de Fallis tenia en prin-cipio, similitud con la humana, y no fue hasta 1990 que Ted McGeer [24] comprobóque la marcha de un robot bípedo pasivo, ganaba energía en cada paso debido alpotencial de la pendiente, mientras que el impacto de un pie con la superficie, disi-paba el excedente de energía. Con las condiciones iniciales y el ángulo de la rampa La energía

utilizadapor Asimo,se estimaque es 32veces másgrande quela usadapor un serhumanodurante elcaminado.[5]

correctos, cada paso sucesivo igualará exactamente las condiciones iniciales del pasoanterior y surgirá una marcha estable. A este tipo de locomoción se le conoce comomarcha pasiva. Inspirados en estos últimos trabajos, se generó una serie de investi-gaciones que hasta la fecha sigue vigente, por diversos grupos de investigadores quebuscan un control más robusto y adaptable a las condiciones del entorno; tales como[14, 15, 17, 32] por citar algunos, así como laboratorios y empresas que desarrollaronprototipos complejos y de avanzada capacidad de respuesta a pesar de perturbacio-nes, obstáculos o superficies irregulares, para preservar el objetivo esencial: mantenerla marcha. Uno de estos casos, es el Robot Asimo1, que la compañía japonesa Hondacomenzó a desarrollar a mediados de los 80’s y que su evolución ha tenido un granimpacto, siendo este una referencia obligada sobre el tema, tal y como puede verseen la Figura 1.2. Actualmente, de los avances significativos que este robot puede pre-sumir, radican en su capacidad para desplazarse de forma lateral y hacia atrás, asícomo por medio de pequeños saltos en un solo pie.

1 El sitio web donde se proporciona información detallada tanto en Japonés como en Inglés es: http://lars.mec.ua.pt/public/LAR%20Projects/RobotActuation/2002_MarcoMelo_VascoQuinteiro/

Projecto/Artigos_net/catalogo/walking_machines_katalog/walking_machines_katalog.html

http://lars.mec.ua.pt/public/LAR%20Projects/RobotActuation/2002_MarcoMelo_VascoQuinteiro/Projecto/Artigos_net/catalogo/walking_machines_katalog/walking_machines_katalog.html



1.1 antecedentes 4

Figura 1.2: Diagrama que muestra el proceso evolutivo de investigación del Robot Asimo,desarrollado por Honda

Otra compañía que se ha ganado un renombre por sus interesantes avances enel campo de la robótica es la Estadounidense Boston Dynamics que entre sus desa-rrollos tiene el robot bípedo Atlas2, capaz de levantarse por sí mismo, caminar porsuperficies inestables, correr, saltar obstáculos, y saltar girando hacia atrás, cayendoperfectamente de pie. En las Figuras 1.3a y 1.3b se muestran fotografías de dichorobot.

(a) Robot Atlas, caminando en un exteriorde superficie nevada

(b) Robots Atlas, dando una pirueta hacia atrásdesde una plataforma

Figura 1.3: Robot Atlas, diseñado y desarrollado por Boston Dynamics

En cuanto a instituciones educativas y laboratorios de investigación, también sepresentan sólo algunos ejemplos, en caso de querer profundizar en este tipo de ro-

2 el sitio web es: https://www.bostondynamics.com/atlas

https://www.bostondynamics.com/atlas

1.1 antecedentes 5

bots, se puede consultar el Walking Machine Catalogue3 donde se tienen cientos detrabajos de investigación y desarrollo sobre el tema.El Robot Spring Flamingo desarrollado por el Instituto Tecnológico de Massachusetts, El

hardwareAtlasaprovechalaimpresión3D paraahorrarpeso yespacio, loque resultaen unnotablerobotcompactocon unaaltarelaciónresistencia/ peso y unespacio detrabajomásamplio.

en Boston; USA, en el año 2000, cuenta con un actuador lineal en la cadera, rodilla,y tobillo por cada pierna; consta de un soporte lateral que reduce la caminata a unsólo plano; tiene un alto margen de estabilidad y es robusto a pequeñas perturba-ciones, pesa aproximadamente 13.5 kg y mide 90cm de altura. En la Figura 1.4a semuestra este robot. El Bart Robot es un bípedo planar desarrollado por la universi-dad Alemana de Hannover y presentado en el año 2003; capaz de caminar de formalenta y estable con la ayuda de un soporte en la parte superior del robot; posee seisgrados de libretad, (tres en cada pierna) utiliza actuadores brushless DC de 60 W a150 W de potencia, pues es necesario contar con un alto torque en cada una de lasarticulaciones, tal y como se muestra en la imagen 1.4b [2]. El robot bípedo Rabbit dela imagen en la figura 1.4c, fue desarrollado en conjunto por el Institut National deRecherche en Informatique et en Automatique (INRIA), Centre national de la recher-che scientifique (CNRS), y el Grenoble Images Parole Signal Automatique (GIPSA);en Francia, en el año 2004 se demostró su capacidad de correr, es capaz de soportarimpactos asociados a las transiciones de fase, así como mantener la marcha a pesarde la colocación de pequeños obstáculos. Cuenta también con un soporte horizontalen la parte superior del cuerpo, que permite al robot lograr una caminata planar, ade-más de ruedas transversales en los pies para compensar a la fuerza de rozamientoque se genera en cada paso.

En lo que respecta a trabajos actuales, a los que se han tenido acceso, se pue-den mencionar [21] y [22] en donde se desglosan detalladamente las estrategias decontrol para robots bípedos subactuados, así como la propuesta de una matriz detransformación que relaciona dos modelos dinámicos de un mismo sistema. Hastaeste punto, se han destacado algunos trabajos de los cientos que existen, consideran-do que entre los aquí mencionados, la diversificación estriba en el tipo de control quese usa, pudiendo ser secuencial, o de trayectoria controlable, como en su carácter deactuados, subactuados o pasivos, los materiales, las funciones u objetivos para losque fueron diseñados, así como sus características físicas y energéticas.

Cabe destacar, que en el Instituto Tecnológico de La Paz, se han hecho trabajossobre bipedestación como el reportado en [34] así como un exoesqueleto [23], deigual forma, en el ámbito de control de vehículos submarinos se tienen los trabajos[11] y [18].

3 http://lars.mec.ua.pt/public/LAR%20Projects/RobotActuation/2002_MarcoMelo_

VascoQuinteiro/Projecto/Artigos_net/catalogo/walking_machines_katalog/walking_machines_

katalog.html




1.2 motivación 6

(a) Robot Spring Flamingo (b) Robot Bart (c) Robot Rabbit

Figura 1.4: Robots bípedos desarrollados por centro de investigación o instituciones académi-cas

1.2 motivación

Al estudiar diferentes trabajos sobre el tema, por ejemplo [3, 15, 33], se observóque en el análisis del Robot Bípedo Tipo Compás, la fricción no era contemplada, locual inspiró a complementar el trabajo de [21], con el fin de proponer una ley decontrol que atenúe los efectos indeseables de la fricción.

1.3 descripción del problema

En el Robot Bípedo sujeto a análisis, se considera fricción dinámica sólo en lacoordenada actuada. En trabajos reportados en la literatura sobre el control de estaclase de robot bípedos, la fricción está ausente. Resultados de simulación mostraronque aún cuando el controlador lograba un desempeño aceptable en la marcha delrobot sin fricción, fallaba en conservar dicha marcha al considerar fricción sólo en laarticulación actuada.

1.4 objetivos 7

1.4 objetivos

1.4.1 Objetivo general

Proponer una ley de control que compense los efectos de la fricción, con la finali-dad de que el Robot Bípedo Tipo Compás con fricción dinámica en su articulaciónactuada mantenga una marcha estable.

1.4.2 Objetivos específicos

Obtener el modelo dinámico del robot bípedo tipo compás.

Proponer una ley de control por moldeo de energía, que contemple el modelode fricción dinámica de Dahl.

Realizar simulaciones y animaciones en Matlab, para validar la ley de controlpropuesta.

1.5 hipótesis

La presencia de la fricción en las articulaciones afecta la marcha del robot bípedo.Una solución, es mitigar los efectos adversos de la fricción por medio de un controlcon compensación dinámica para mantener la locomoción.

1.6 alcances y limitaciones

1.6.1 Alcances

Una mayor comprensión de la marcha de un robot bípedo tipo compás cuandola fricción en su articulación actuada es tomada en cuenta en el diseño delcontrolador basado en moldeo de energía.

Animaciones que validan el esquema de control propuesto.

1.7 organización de la tesis 8

1.6.2 Limitaciones

Los resultados no pueden ser extrapolados directamente a otros métodos decontrol y en particular, a aquellos que no están basados en moldeo de energía.

Falta validar los resultados experimentalmente en un robot bípedo real.

1.7 organización de la tesis

Este trabajo de tesis se divide en cinco partes, complementadas con un apéndice,organizada de la siguiente manera.En la primera parte se presenta un panorama acerca de los robots bípedos, donde sefija una perspectiva sobre la evolución que este tema ha tenido en cuanto a investiga-ciones se refiere, así como también se planten los razonamientos que fundamentaneste trabajo.En la segunda parte, se presenta el Robot Bípedo Tipo Compás, y dos formas distintasde modelarlo en función de la selección de las coordenadas generalizadas, poste-riormente se efectúa un análisis con la finalidad de obtener una relación entre losmodelos obtenidos.La tercera parte está dirigida al control del Robot Bípedo Tipo Compás, cuando és-te tiene actuación en sólo una de sus articulaciones, valiéndose de la metodologiaIDA-PBC. Como aportación central de este trabajo, se consideraron los efectos dela fricción, por ello se incluye en la ley de control propuesta el modelo de friccióndinámica de Dahl, lo que lleva a una ley de control con compensación de friccióndinámica.Referente a la cuarta parte, se presentan resultados de simulaciones de la marchaen forma de animación, bajo tres esquemas de control diferentes: realizando sólo elmoldeo de energía, implicando valores de fricción dinámica y por último, aplicandoel control con compensación de fricción dinámica. Esto se llevó a cabo, mediante elsoftware Matlab, al haber hecho adaptaciones del programa desarrollado por [21],con la finalidad de validar la ley de control propuesta.Acerca de la quinta parte, se dan algunas conclusiones y el trabajo a futuro.En el Apéndice A se muestran los códigos en Matlab desarrollados por [21], imple-mentados en la simulación, comprendidos por el código del modelo, el código delcontrol y el código de la animación, así como también el código trap 101 que realizalas integraciones por medio del método trapezoidal, y fue desarrollado en Matlabpor J.H. Taylor en Mayo de 1995.

Parte II

E L R O B O T B Í P E D O Y S U M O D E L O M AT E M ÁT I C O

2E L R O B O T B Í P E D O T I P O C O M PÁ S

El robot bípedo más simple está dotado de un par de piernas rígidas así como unaarticulación en la cadera y otra en el pie de soporte, como se muestra en la Figura2.1. Este tipo de robot puede analizarse en su condición pasiva, es decir, en ausenciade pares que propicien la locomoción, colocándose sobre una pendiente de φ grados,debido a la ganancia de energía en cada paso, a razón de cambio en el potencial dela pendiente, mientras que cada impacto con la superficie disipará la energía extra,haciendo que cada paso iguale al anterior [24].

Figura 2.1: Diagrama del robot bípedo tipo compás

El movimiento del bípedo, está considerado únicamente en el plano sagital, por loque al desplazarse, emulará un compás, tomando de ahí su nombre, Robot BípedoTipo Compás o por sus siglas RBTC. El RBTC, tiene un lugar importante en el catá-logo de este tipo de mecanismos pasivos, a pesar de su simplicidad motriz. Es unexcelente punto de partida para iniciar con el estudio de sistemas dinámicos ya queaun existen problemas abiertos, como la marcha con fricción.

2.1 modelado matemático 11

2.1 modelado matemático

El movimiento del bípedo se clasifica en dos etapas: Etapa de balanceo que sedefine como el periodo de transición previo a un paso, cuando un pie no tiene con-tacto con la superficie; y etapa de impacto, presente cuando se tiene una o ambaspiernas en contacto con la superficie. El comportamiento descrito, se plantea a travésde un modelo dinámico. Para llegar al modelo dinámico general, se siguen cuatroetapas que constan de calcular lo siguiente: la función energía cinética, la función deenergía potencial, el lagrangiano (función de energía cinética menos función de ener-gía potencial) y por último las ecuaciones de movimiento, basado en la formulaciónEuler Lagrange, mismas que pueden ser escritas en notación matricial. En [6, 9, 12]se explica una metodología para obtener el modelo de un sistema dinámico en unanálisis más profundo. El Robot Bípedo Tipo Compás, es uno de los robots más sim-ples que permite un estudio sobre su dinámica que determine su marcha. Con esefin, a continuación se presenta el modelo dinámico del robot antes mencionado. Losparámetros que se usaron para el análisis [21], pueden observarse en la Tabla 2.1.

Tabla 2.1: Párametros del robot bípedo tipo compás

Párametro Valor [Unidad] Descripción

m 5 [Kg] Masa de la piernamH 10 [Kg] Masa de la cadera

a 0.5 [m] Distancia entre pie y centro de masab 0.5 [m] Distancia entre centro de masa y cadera

l = a + b 1 [m] Distancia total de la piernag 9.81 [m/s2] Aceleración debido a la gravedad

2.1.1 El robot bípedo como robot manipulador

Una manera de analizar el robot bípedo tipo compás, es haciéndolo desde la pers-pectiva de un robot manipulador, considerando las coordenadas en el pie de soporte,q1 como si se tratara de la base del robot manipulador, donde la coordenada se midedesde el eje horizontal hasta el eslabón y en la cadera q2 referenciado a la segundaarticulación del robot manipulador, tomando la extensión del primer eslabón hastael segundo. En la Figura 2.2 se muestra el diagrama de cuerpo libre, en el cual secentra el análisis geométrico.Existen tres centros de masa que se contemplan para el cálculo de las funciones de


Figura 2.2: Diagrama de cuerpo libre, del robot bípedo con coordenadas en pie de soporte ycadera, q1 y q2 respectivamente

energía cinética y potencial: m1 = m2 = m que corresponden a la masa en cada esla-bón y mH que pertenece a la cadera. Para el cálculo de la función de energía cinéticatotal, es necesario conocer cada una de las contribuciones:

K(q, q) = K1(q, q) + K2(q, q) + KH(q, q) (2.1)

donde K1 es la función de energía cinética del primer eslabón, K2 pertenece a lafunción del segundo y KH se refiere a la cadera.Para el primer eslabón:

K1(q, q) = 12 m‖v1‖2 (2.2)

La norma euclidiana al cuadrado de la velocidad, esta determinada por:

‖v1‖2 = x21 + y2

1 (2.3)

para obtener el cálculo de ella, es necesario conocer la ubicación del centro de masadel primer eslabón, y posteriormente obtener su derivada con respecto al tiempo:

x1 = acos(q1)

y1 = asen(q1)(2.4)

x1 = −asen(q1)q1

y1 = −acos(q1)q1(2.5)

Aplicando (2.5) en (2.3), se tiene:

‖v1‖2 = a2q21 (2.6)


y sustituyendo en la ecuación (2.2) la función de energía cinética del primer eslabónqueda expresada como:

K1(q, q) =12

ma2q21 (2.7)

Para el segundo eslabón, obtenemos la ubicación del centro de masa:

x2 = xH + bcos(β)

y2 = yH − bsen(β)(2.8)

Se puede observar en la Figura 2.2 que 2π = β + q1 + q2, y despejando β tenemosque β = 2π − q1 − q2, asimismo tomando en cuenta las identidades trigonométricasdadas en (2.37), donde ahora α = −(q1 + q2), se obtiene:

x2 = xH + bcos(q1 + q2)

y2 = yH + bsen(q1 + q2)

por lo que las incógnitas a calcular ahora son xH y yH. Con este fin se analizan lasdistancias del centro de masa de la cadera, de donde se obtiene:

xH = lcos(q1)

yH = lsen(q1)(2.9)

Sustituyendo (2.9) en(2.8), habiendo considerado (2.37), se llega a:

x2 = lcos(q1) + bcos(q1 + q2)

y2 = lsen(q1) + bsen(q1 + q2)(2.10)

Para calcular la velocidad del centro de masa, obtenemos primero las derivadas tem-porales de estas posiciones:

x2 = −lsen(q1)q1 − bsen(q1 + q2)(q1 + q2)

y2 = lcos(q1)q1 + bcos(q1 + q2)(q1 + q2)(2.11)

Tal y como se hizo en (2.3), se calcula la velocidad para el eslabón dos:

‖v2‖2 = x22 + y2

2

‖v2‖2 = l2q12(sen2q1 + cos2q1) + 2lb(q2

1 + q1q2)[senq1sen(q1 + q2)

+cosq1cos(q1 + q2)] + b2(q1 + q2)2[sen2(q1 + q2) + cos2(q1 + q2)]

(2.12)

Aplicando las identidades trigonométricas respectivas, la velocidad queda simplifi-cada a:

‖v2‖2 = l2q21 + 2lb(q2

1 + q1q2)cosq2 + b2q21 + 2b2q1q2 + b2q2

2 (2.13)


Una vez obtenida la velocidad, se puede concluir que la energía cinética para elsegundo eslabón es:

K2(q, q) = m2 l2q2

1 +m2 b2q2

1 + mb2q1q2 + mlb(q21 + q1q2)cosq2 +

m2 b2q2

2 (2.14)

Con la finalidad de determinar la función de energía cinética de la cadera, se toma laubicación propuesta en (2.9), y se realizan los cálculos correspondientes, para llegara:

KH(q, q) =12

mH l2q21 (2.15)

Una vez calculadas las funciones de energía cinética de cada masa, se suman (2.1)para obtener la función de energía cinética total del sistema:

K(q, q) = 12 ma2q2

1 +12 ml2q2

1 +12 mb2q2

1 + mb2q1q2 + mlb(q21 + q1q2)cosq2

+12 mb2q2

2 +12 mH l2q2

1

(2.16)

Enseguida, se procede al cálculo de la función de energía potencial, donde se con-sideran las contribuciones de cada eslabón más el de la cadera; expresándose de lasiguiente forma:

V(q) = V1(q) + V2(q) + VH(q) (2.17)

Las funciones de energía potencial en cada eslabón y en la cadera, corresponden a:

V1(q) = mgy1

V2(q) = mgy2

VH(q) = mgyH

(2.18)

Las cuales pueden ser reescritas como:

V1(q) = mg(asenq1)

V2(q) = mg(lsenq1 + bsen(q1 + q2))

VH(q) = mHg(lsenq1)

(2.19)

Por tanto, la función de energía potencial total es:

V(q) = mg(asenq1) + mg(lsenq1 + bsen(q1 + q2)) + mHg(lsenq1) (2.20)

La función lagrangiana está determinada por la diferencia entre la función de energíacinética menos la función de energía potencial:

L(q, q) = K(q, q)−V(q) (2.21)


Finalmente, sustituyendo (2.1) y (2.20) en (2.21), se obtiene:

L(q, q) = 12 ma2q2

1 +12 ml2q2

1 +12 mb2q2

1 + mb2q1q2 + mlb(q21 + q1q2)cosq2

+12 mb2q2

2 +12 mH l2q2

1 −mgasenq1 −mg(lsenq1 + bsen(q1 + q2))

−mHg(lsen(q1))

(2.22)

Con base en el Lagrangiano, y tomando la ecuación de movimiento Euler Lagrangedada por:

ddt

(∂L(q, q)

∂q

)− ∂L(q, q)

∂q= τ (2.23)

donde τ son los pares aplicados en las articulaciones del RBTC cuando se consideranen q1 y q2;En notación matricial la ecuación (2.23) es:[

ddt(∇q1L(q, q)

)ddt(∇q2L(q, q)

)]− [∇q1L(q, q)∇q2L(q, q)

]=

[τ1

τ2

](2.24)

Se resuelve para cada uno de los términos de (2.24). Por lo que para:

∂L(q, q)∂q

=

∂L(q,q)∂q1

∂L(q,q)∂q2

= ∇qL(q, q) =

[∇q1L(q, q)∇q2L(q, q)

](2.25)

∇q1L(q, q) = (ma2 + ml2 + mb2 + mH l2 + 2mlbcos(q2))q1 (2.26)

+(mb2 + mblcos(q2))q2

∇q2L(q, q) = (mb2 + mblcos(q2))q1 + mb2q2 (2.27)ddt(∇q1L(q, q)

)= ma2 + ml2 + mb2 + mH l2 + 2mlbcos(q2))q1 (2.28)

+(mb2 + mlbcos(q2))q2 −mlbsen(q2)q22

−2mlbsen(q2)q1q2

ddt(∇q2L(q, q)

)= (mb2 + mblcos(q2))q1 + mb2q2 −mblsen(q2)q1q2 (2.29)

Ahora para:

∇qL(q, q) =

[∇q1L(q, q)∇q2L(q, q)

]

∇q1L(q, q) = −mgacos(q1)−mglcos(q1)−mgbcos(q1 + q2) (2.30)−mHglcos(q1)

∇q2L(q, q) = −mblsen(q2)q22 −mblsen(q2)q1q2 −mgbcos(q1 + q2) (2.31)


Las ecuaciones de movimiento, se obtienen sustituyendo (2.28) - (2.31) en (2.24), estoes:

τ1 = (ma2 + ml2 + mb2 + mH l2 + 2mlbcos(q2))q1 + (mb2 + mlbcos(q2))q2

−2mlbsen(q2)q2q1 −mlbsen(q2)q2q2 + mgacos(q1) + mglcos(q1)

+mgbcos(q1 + q2) + mHglcos(q1) (2.32)τ2 = (mb2 + mblcos(q2))q1 + (mb2)q2 + mblsen(q2)q1q1

+mgbcos(q1 + q2) (2.33)

En forma compacta, las ecuaciones de movimiento quedan de la siguiente manera:

M(q)q + C(q, q)q + g(q) = τ (2.34)

donde q =[q1 q2

]Tes el vector de posición, mientras que las matrices se definen

como:

M(q) =

[ma2 + ml2 + mb2 + mH l2 + 2mlbcos(q2) mb2 + mlbcos(q2)

mb2 + mlbcos(q2) mb2

](2.35)

C(q, q) =

[−2mlbsen(q2)q2 −mlbsen(q2)q2

mlbsen(q2)q1 0

]

g(q) =

[mgacos(q1) + mglcos(q1) + mgbcos(q1 + q2) + mHglcos(q1)

mgbcos(q1 + q2)

]

De esta manera se concluye la obtención del modelo dinámico del Robot Bípedo TipoCompás, con coordenadas generalizadas en pie y cadera.

2.1.2 Modelo dinámico con coordenadas en ambos pies

La configuración clásica de análisis de este bípedo, consiste en definir las coorde-nadas generalizadas entre una vertical y el pie de soporte así como una vertical y elpie de no soporte, es decir θs y θns respectivamente, con ambas mediciones tomadasen sentido opuesto a las manecillas del reloj, como se muestra en la Figura 2.3a. Bajoestas condiciones, se puede obtener otro modelo del mismo sistema. En esta secciónse muestra el procedimiento que llega a esta forma alternativa de modelo; basadoen la Figura 2.3b. Siguiendo el procedimiento anterior, la energía cinética total delsistema se conforma por las contribuciones de cada eslabón:

K(θ, θ) = K1(θ, θ) + K2(θ, θ) + Kc(θ, θ) (2.36)


donde K1(θ, θ) es la función de energía cinética del primer eslabón, K2(θ, θ) pertenecea la función del segundo y Kc(θ, θ) se refiere a la cadera.

(a) Robot bípedo con coordenadas en θs y θns. (b) Diagrama de cuerpo libre

Figura 2.3: Diagrama del RBTC con coordenadas generalizadas en pie de soporte: θs y pie deno soporte: θns

Haciendo un análisis previo para comprender el manejo de la forma en que lascoordenadas generalizadas son usadas, se parte de la observación de que:

θs + ξ = 2π

y haciendo uso de las identidades trigonométricas siguientes:

sen(−α) = −senα

cos(−α) = cosα

sen(α + 2π) = senα

cos(α + 2π) = cosα

(2.37)

Se deduce que:

sen(ξ) = sen(−θs + 2π) = sen(−θs) = −sen(θs)

cos(ξ) = cos(−θs + 2π) = cos(−θs) = cos(θs)

Ahora, aplicando la ecuación para la función energía cinética, en el primer eslabón:

K1(θ, θ) =12

m‖v1‖2 (2.38)


Para encontrar la velocidad, es necesario calcular la ubicación del centro de masa ysu derivada temporal.

‖v1‖2 = x21 + y2

1 (2.39)

Por lo tanto, la ubicación del centro de masa del primer eslabón queda determinadapor:

x1 = −asen(θs)

y1 = acos(θs)

Su derivada temporal es:

x1 = −acos(θs)θs

y1 = −asen(θs)θs(2.40)

De esta forma, trabajando la ecuación (2.39), aplicando (2.40), se sustituye en (2.38),para obtener:

K1(θ, θ) =ma2θ2

s2

(2.41)

Para el segundo eslabón se tiene que la posición está dada en:

x2 = xc + bsen(θns)

y2 = yc − bcos(θns)(2.42)

Para conocer el valor de x2 y y2 es necesario definir el valor de xc y yc:

xc = −lsen(θs)

yc = lcos(θs)(2.43)

Sustituyendo (2.43) en (2.42), la norma euclidiana al cuadrado de la velocidad, sedefine por medio de:

‖v2‖2 = x22 + y2

2

‖v2‖2 = l2θs2 − 2lbcos(θs − θns)θs ˙θns + b2 ˙θns

2 (2.44)

Con esto, se calcula la función de energía cinética del segundo eslabón:

k2(θ, θ) =ml2θs

2

2−mlbcos(θs − θns)θsθns +

mb2 ˙θns2

2(2.45)


Por último, y haciendo uso de (2.43), se calcula la norma euclidiana al cuadrado dela velocidad:

‖vc‖2 = x2c + y2

c

tal que la función Kc(θ, θ) queda como sigue:

kc(θ, θ) =mcl2θs

2

2(2.46)

Tomando (2.36), con las contribuciones encontradas, se obtiene la energía potencialtotal:

K(θ, θ) =ma2θ2

s2

+ml2θs

2


mb2 ˙θns2

2+

mcl2θs2

2(2.47)

La función de energía potencial total del sistema se define por:

V(θ) = V1(θ) + V2(θ) + Vc(θ) (2.48)

Para la función de energía potencial, el procedimiento es más simple que el calculadoen la anterior subsección, ya que nos valemos de la altura h de cada centro de masaen el sistema, que en este caso se determina por medio de cada y que ya ha sidocalculada en la ubicaciones de cada centro de masa, por lo que la función de energíapotencial de cada masa es:

V1(θ) = mgacos(θs)

V2(θ) = mg(lcos(θs)− bcos(θns))

Vc(θ) = mglcos(θs)

(2.49)

V(θ) = mgacos(θs) + mg(lcos(θs)− bcos(θns)) + mglcos(θs) (2.50)

Con el cálculo de la función de energía cinética (2.47) y energía potencial(2.50), sedefine el lagrangiano:

L(θ, θ) = K(θ, θ)−V(θ)

L(θ, θ) =ma2θ2

s2

+ml2θs

2


mb2 ˙θns2

2+

mcl2θs2

2−mgacos(θs)−mg(lcos(θs)− bcos(θns))−mglcos(θs)

(2.51)

Enseguida, se procede a obtener las ecuaciones de movimiento, usando las ecuacio-nes Euler Lagrange:

ddt

(∂L(θ, θ)

∂θ

)−(

∂L(θ, θ)

∂θ

)= τ (2.52)


o bien:

ddt(∇θL(θ, θ)

)−∇θL(θ, θ) = τ (2.53)

Resolviendo la ecuación (2.53), al tomar (2.51), se determina:

ddt(∇θL(θ, θ)

)=

[ddt

(∇θsL(θ, θ)

) ddt

(∇ ˙θnsL(θ, θ)

)]T

∇θsL(θ, θ) = (ma2 + ml2 + mcl2)θs −mlbcos(θs − ˙θns) ˙θns (2.54)

∇ ˙θnsL(θ, θ) = −mlbcos(θs − ˙θns)θs + mb2 ˙θns (2.55)

ddt

(∇θsL(θ, θ)

)= (ma2 + ml2 + mcl2)θs −mlbcos(θs − θns) ¨θns (2.56)

+mlbsen(θs − θns)(θs − ˙θns)

ddt

(∇ ˙θsnL(θ, θ)

)= −mlbcos(θs − θns)θs + mb2 ¨θns (2.57)

+mlbsen((θs − θns))(θs − ˙θns)q1θs

Finalmente se calculan los siguientes términos:

∇θL(θ, θ) =[∇θsL(θ, θ) ∇θnsL(θ, θ)

]T

∇θsL(θ, θ) = mlbsen(θs − θns)θs ˙θns + mgasen(θs) + mglsen(θs) (2.58)+mcglsen(θs)

∇θnsL(θ, θ) = −mlbsen(θs − θns)θs ˙θns −mgbsen(θns) (2.59)

Con los resultados obtenidos de (2.56) a (2.59), se sustituyen en (2.53) para los pares:

τ1 = (ma2 + ml2 + mcl2)θs −mlbcos(θs − θns) ¨θns + mlbsen(θs − θns)(θs − ˙θns)

+mlbsen(θs − θns)θs ˙θns −mgasen(θs)−mglsen(θs)−mcglsen(θs)

τ2 = −mlbcos(θs − θns)θs + mb2 ¨θns + mlbsen((θs − θns))θs2+ mgbsen(θns)

Con los pares obtenidos τ1 y τ2, se escribe en forma compacta el modelo dinámico:

M(θ)θ + C(θ, θ)θ + g(θ) = τ (2.60)

2.2 relación entre dos modelos dinámicos del robot bípedo tipo compás 21

donde θ =[θs θns

]Tes el vector de posición, mientras que las matrices M(θ), C(θ, θ)

y el vector g(θ) se definen como:

M(θ) =

[ma2 + ml2 + mcl2 mlbcos(θs − θns)

−mlbcos(θs − θns) mb2

](2.61)

C(θ, θ) =

[0 −mlbsen(θs − θns) ˙θns

mlbsen(θs − θns)θs 0

]

g(θ) =

[−mgasen(θs)−mglsen(θs)−mcglsen(θs)

mgbsen(θns)

]De esta manera se concluye la obtención del modelo dinámico del Robot Bípedo TipoCompás, con coordenadas generalizadas en ambos pies (θs y θns).

2.2 relación entre dos modelos dinámicos del robot bípedo tipo

compás

A pesar de tratarse del mismo robot bípedo, dos modelos dinámicos pueden re-presentar las ecuaciones de movimiento del robot y la diferencia entre ellas es laselección de las coordenadas generalizadas (2.34) y (2.60). Se puede observar que elmodelo obtenido con las coordenadas en ambos pies (θs y θns), es más simple encomparación con el modelo obtenido con las coordenadas en pie de soporte y cadera(q1 y q2); sin embargo se dificulta asignar en el modelo (2.60) , los pares en el piede soporte y cadera. Por tal motivo en esta sección, se presenta la obtención de unamatriz de transformación [21], que realiza un mapeo entre las dos distintas coorde-nadas θ y q, así como también se explica el origen de la matriz encargada de asignarlos pares u en pie de soporte y cadera, que se denominará como B, haciendo uso delmodelo con coordenadas en θ. Con este fin, se reescribe el modelo (2.60) como:

M(θ)θ + C(θ, θ) + g(θ) = Bu (2.62)

donde u representa los pares aplicados en el pie de soporte y en la cadera.Sin embargo, al obtener el modelo dinámico del RBTC desde las coordenadas θs yθns, no es posible obtener directamente el par aplicado en pie de soporte y la cadera;sino en el pie de soporte y en el pie de balanceo, a diferencia de cuando se usan lascoordenadas q1 y q2; donde sí se obtienen directamente los pares deseados, tanto enpie de soporte como en cadera.

Un análisis geométrico, muestra la relación existente entre θ y q, donde θ =

[θs θns]T y q = [q1 q2]

T como se muestra en la Figura 2.4.


Figura 2.4: Diagrama de cuerpo libre que relaciona las coordenadas θ y las coordenadas q.

θs + ε = 2π ⇒ ε = 2π − θs

q1 + ε = π2 ⇒ ε = π

2 − q1(2.63)

Igualando ε de (2.63):

2π − θs =π2 − q1 (2.64)

se obtiene:

q1 = θs −3π

2(2.65)

De igual forma, se observa que:

q1 + q2 + β = 2π ⇒ β = 2π − q1 − q2

θns + β = π2 ⇒ β = π

2 − θns(2.66)

igualando β de (2.66) y despejando q2, se obtiene:

q2 = θns − θs + 3π (2.67)

Al obtener las derivadas temporales de (2.65) y (2.67), se podrá expresar este conjuntode ecuaciones en notación matricial. Por lo cual:[

q1

q2

]=

[1 0−1 1

] [θs

θns

](2.68)

donde se define la matriz A =

[1 0−1 1

], que realiza una transformación entre las

coordenadas θ y q.La derivada temporal de (2.68) es:[

q1

q2

]=

[1 0−1 1

] [θs

θns

](2.69)


por tanto, se tiene que q = Aθ y q = Aθ.Se reescribe (2.34) de manera extensa:

M(q1, q2) [q] + C(q1, q2, q1, q2) [q] + g(q1, q2) = τ (2.70)

y tomando los resultados obtenidos en (2.65), (2.67), (2.68) y (2.69), se reescribe laecuación (2.70), en función de θ:

M(θs − 3π2 , θns − θs + 3π)Aθ + C(θs − 3π

2 , θns − θs + 3π, θs, θns − θs)Aθ

+g(θs − 3π2 , θns − θs + 3π) = τ

(2.71)

Se puede observar que MA no es una matriz simétrica , por lo que para cumplircon las propiedades de simetría y positividad de la matriz M, se premultiplicará laecuación por AT, con la finalidad de obtener la simetría buscada:

AT M(θs − 3π2 , θns − θs + 3π)Aθ + ATC(θs − 3π

2 , θns − θs + 3π, θs, θns − θs)Aθ

+AT g(θs − 3π2 , θns − θs + 3π) = AT τ

(2.72)

La cual resulta ser equivalente al modelo planteado en la ecuación (2.60); lo queconduce a concluir que de esta forma el modelo planteado en (2.34) es equivalenteal presentado en la forma tradicional; haciendo posible identificar que:

M(θ) = AT M(θs − 3π2 , θns − θs + 3π)Aθ

C(θ, θ) = ATC(θs − 3π2 , θns − θs + 3π, θs, θns − θs)Aθ

g(θ) = AT g(θs − 3π2 , θns − θs + 3π)

Así como también se presenta:

B = AT

u = τ

donde B se define como:

B =

[1 −10 1

]

Siendo posible, a través de esta transformación, usar el modelo planteado en (2.60),y poder aplicar los pares propuestos en el modelo de la ecuación (2.34).

Parte III

E L M É T O D O I D A - P B C C O N F R I C C I Ó N D I N Á M I C A

3C O N T R O L C O N C O M P E N S A C I Ó N D E F R I C C I Ó N D I N Á M I C APA R A E L R O B O T B Í P E D O T I P O C O M PÁ S , M E D I A N T E L AM E T O D O L O G I A I D A - P B C

En el presente capítulo se describe el desarrollo del diseño de un IDA-PBC parael control de un robot bípedo tipo compás con fricción, el cual está basado en unaextensión del método IDA-PBC que incorpora uno de los modelos más simples defricción dinámica: El modelo de Dahl. Dahl propuso una transición suave entre lafricción estática y la dinámica además de dedicar gran parte de su estudio a modelarel efecto de histéresis presente en el fenómeno de fricción.

Para el diseño, se explota el hecho de que la matriz de inercia M, debe depender dela coordenada subactuada en la metodología IDA-PBC. Por tanto, con la finalidad deaprovechar el modelo dinámico del robot bípedo analizado como robot manipulador(2.34), donde la matriz de inercia está en función de q2, se considerará que el actuadordel bípedo estará unicamente en el pie de soporte, es decir q1. El material que semuestra en el diseño detallado del control, está basado en los trabajos reportados en[21, 22, 31].

Se resalta que a la coordenada subactuada no se le consideró la fricción, con lafinalidad de centrar el análisis en la articulación actuada.

3.1 diseño de la ley de control 26

3.1 diseño de la ley de control

A continuación se muestra en detalle el diseño del controlador IDA-PBC, con énfa-sis en la obtención de Md y J2. El modelo dinámico de la parte mecánica (2.34) puedeser escrito con las siguientes matrices M(q), G y la función de energía potencial V(q):

M(q) =

[C1 + C2 + 2C3cos(q2) C2 + C3cos(q2)

C2 + C3cos(q2) C2

](3.1)

G =

[10

](3.2)

V(q) = C4gsen(q1) + C5gsen(q1 + q2) (3.3)

donde:

C1 = ma2 + ml2 + mH l2

C2 = mb2

C3 = mlbC4 = mH l + ma + mlC5 = mb

y G ∈ Rn×m es la matriz de distribución de pares.

Con la intención de reducir la notación, M(q) se simplifica como:

M(q) = M =

[a1 a2

a2 a3

]

y considerando únicamente el moldeo de la función de energía cinética, se tiene que:

G⊥{∇q(pT M−1p)−MdM−1∇q(pT Md−1p) + 2J2Md

−1} = 0 (3.4)

y dado que la matriz M(q) depende sólo de la coordenada subactuada q2 se puederecurrir a la siguiente simplificación:

∇q(pT M(qk)−1p) = pT d(M(qk)

−1)

dqkp (3.5)

Al sustituir (3.5) en (3.4), se tiene:

G⊥{pT dM−1

dqp−M−1

d M(pT dM−1

dqp) + 2J2Md

−1p} = 0 (3.6)


Enseguida, para definir convenientemente d(M(qk)−1)

dqn, se puede auxiliar de la siguien-

te igualdad dado que M es no singular

I = MM−1

al derivar, queda como:

d(I)dqk

=d(MM−1)

dqk= 0

desarrollando por la regla de la cadena:

M(dM−1

dqk) + (

dMdqk

)M−1 = 0

M(dM−1

dqk) = −(dM

dqk)M−1

al premultiplicar M−1 por ambos lados de la ecuación, se llega a:

dM−1

dqk= −M−1(

dMdqk

)M−1 (3.7)

Al tomar la ecuación (3.7), sustituyéndola en (3.6), y realizando el desarrollo perti-nente, se tiene:

−pT M−1 dMdq2

M−1p + G⊥MdM−1e2pT M−1d

dMddq2

M−1d p + G⊥2J2M−1

d p = 0 (3.8)

donde e2 =[0 1

]Tes un vector base. También se define:

Λ = MdM−1 =

[λ1 λ2

λ3 λ4

](3.9)

Al resolver únicamente la parte G⊥Λe2 se concluye que el resultado es λ4, lo cual alsustituirse en (3.8), la expresión puede reescribirse como:

−pT M−1 dMdq2

M−1p + λ4pT M−1d

dMddq2

M−1d p + G⊥2J2M−1

d p = 0 (3.10)

De la ecuación (3.10), se toma la asignación particular de la matriz J2, definiéndolacomo:

J2 =

[0 pTα(q2)

− pTα(q2) 0

]= pTαW (3.11)


donde p = M−1p, α(q2) = [α1(q2) α2(q2)]T libres y W ∈ so(2)1. De tal forma que la

ecuación (3.10) se expresa como:

−pT M−1 dMdq2

M−1p + λ4pT M−1d

dMddq2

M−1d p + G⊥2(pT M−1

d αW)M−1d p = 0 (3.12)

al factorizar pT por la izquierda y p por la derecha:

pT(−M−1 dMdq2

M−1 + λ4M−1d

dMddq2

M−1d + G⊥2(M−1

d αW)M−1d )p = 0 (3.13)

de la parte G⊥2(M−1d αW) se factoriza M−1

d , de donde se obtiene una matriz A:

A = 2αG⊥W = 2

[α1

α2

][0 1]

[0 −1−1 0

]= −2

[α1 0α2 0

](3.14)

La asignación de la matriz A es crucial en el diseño. Un análisis detallado de la incor-poración de esta matriz puede ser consultado en [1]. Este resultado se descomponeen una matriz simétrica y una antisimétrica:

A = As −Aas =

[−2α1 0−2α2 0

]=

[−2α1 −α2

−α2 0

]−[

0 −α2

α2 0

](3.15)

se asigna la parte simétrica As en la ecuación (3.13), por tanto la igualdad es expre-sada de la forma:

−M−1 dMdq2

M−1 + λ4M−1d

dMddq2

M−1d −M−1

d AsM−1d = 0 (3.16)

al multiplicar Md tanto por la izquierda como por la derecha, la expresión queda:

Md(−M−1 dMdq2

M−1 + λ4M−1d

dMddq2

M−1d −M−1

d AsM−1d )Md = 0

−MdM−1 dMdq2

M−1Md + λ4dMddq2−As = 0 (3.17)

considerando la definición (3.9), se observa que M−1Md = ΛT, lo cual reduce laecuación (3.17) a:

−ΛdMdq2

ΛT + λ4dMddq2−As = 0 (3.18)

1 so(2) denota el espacio ortogonal de dimensión 2, esto es, so(2) =

[0 −1

−1 0

]


desarrollando dMdq2

para luego sustituir en (3.18), tomando los valores de (3.1) :

dMdq2

=

[−2C3sen(q2) −C3sen(q2)

−C3sen(q2) 0

]= −C3sen(q2)

[2 11 0

](3.19)

al sustituir se tiene:

C3sen(q2)Λ

[2 11 0

]ΛT + λ4

dMdq2−As = 0 (3.20)

Es importante destacar que la estrategia del diseño de control que se ha seguido,busca obtener una solución para Md en (3.4). En este caso el conjunto de EDPs mos-tradas en (3.4) se han transformado en el conjunto de ecuaciones algebraicas dadasen (3.20), cual no presenta problema para la solución de Md, dado que α1 y α2 sonfunciones libres.Continuando con el diseño, se observa que de la definición (3.9), se cumple por tanto:

Md = ΛM =

[d1 d2

d3 d4

](3.21)

se define:

d1 = λ1a1 + λ2a2

d2 = λ1a2 + λ2a3

d3 = λ3a1 + λ4a2

d4 = λ3a2 + λ4a3

(3.22)

Al reescribir la ecuación (3.20) se tiene:

C3sen(q2)

([λ1 λ2

λ3 λ4

] [2 11 0

] [λ1 λ3

λ2 λ4

])+ λ4

[d′1 d

′2

d′3 d

′4

]−[

2α1 α2

α2 0

]= 0 (3.23)

donde d′i =

ddidq2

, siendo i = 1, 2, 3, 4.Al resolver (3.23), se genera un sistema de cuatro ecuaciones:

2(λ21 + λ1λ2)C3sen(q2) + λ4d

′1 − 2α1 = 0

(2λ1λ3 + λ2λ3 + λ1λ4)C3sen(q2) + λ4d′2 − α2 = 0

(2λ1λ3 + λ2λ3 + λ1λ4)C3sen(q2) + λ4d′3 − α2 = 0

2(λ23 + λ3λ4)C3sen(q2) + λ4d

′4 − 2α1 = 0

(3.24)

debido a que Md = MTd > 0, es condición que d2 = d3, por lo que el conjunto de

ecuaciones (3.24), se reduce a tres, esto es:

2(λ21 + λ1λ2)C3sen(q2) + λ4d

′1 − 2α1 = 0 (3.25)

(2λ1λ3 + λ2λ3 + λ1λ4)C3sen(q2) + λ4d′2 − α2 = 0 (3.26)

2(λ23 + λ3λ4)C3sen(q2) + λ4d

′4 − 2α1 = 0 (3.27)


Por otro lado, dado que V(q) = Vd(q):

G⊥[∇qV −Λ∇qV

]= 0

G⊥[(I −Λ)∇qV

]= 0[

1 0] [[1 0

0 1

]−[

λ1 λ2

λ3 λ4

] [C4gcos(q1) + C5gcos(q1 + q2)

C5gcos(q1 + q2)

]]= 0

[1 0

] [[1 00 1

]−[

λ1 λ2

λ3 λ4

] [g11(q)g12(q)

]]= 0

y luego de manipulaciones algebraicas se obtiene:

−λ3g11(q) + (1− λ4)g12(q) = 0. (3.28)

Luego de sustituir g11(q) y g12(q) en (3.28), la solución es:

λ3 = 0λ4 = 1.

(3.29)

Con las soluciones de (3.29), se procede a calcular los elementos restantes de Λ deacuerdo a la definición (3.9):

Λ = MdM−1 =

[λ1 λ2

λ3 λ4

]=

[d1 d2

d2 d3

]1∆

[a3 −a2

−a2 a1

](3.30)

tal que:

λ1 =1∆(d1a3 − d2a2) (3.31)

λ2 =1∆(d2a1 − d1a2) (3.32)

λ3 =1∆(d2a3 − d3a2) = 0 (3.33)

λ4 =1∆(d3a1 − d2a2) = 1 (3.34)

donde ∆ = a1a3− a22. De estas cuatro ecuaciones, se toma (3.33) y (3.34) y se propone

que d2 = a2, d3 = a3 con la finalidad de hacer que cumplan estas igualdades. Por lotanto, hasta el momento se puede considerar que la matriz Md es:

Md =

[d1 d2

d2 d3

]=

[d1 a2

a2 a3

]


Es necesario definir d1, para ello se propone λ1 = K; donde el valor de K es unaganancia de ajuste constante y estrictamente positiva, lo que propicia:

Md = ΛM =

[λ1 λ2

λ3 λ4

] [a1 a2

a2 a3

]=

[K λ2

0 1

] [a1 a2

a2 a3

]

Md =

[(Ka1 + λ2a2) (Ka2 + λ2a3)

a2 a3

](3.35)

Para cumplir con la condición Md = MTd , los elementos d12 y d21 de la matriz deben

ser iguales, por consiguiente se propone que Ka2 + λ2a3 = a2, y al resolver estaecuación para λ2 se tiene que Λ es:

Λ =

[K (1−K)a2

a3

0 1

](3.36)

al tomar el resultado obtenido en (3.36), se sustituye éste en (3.35):

Md =

[K (1−K)a2

a3

0 1

] [a1 a2

a2 a3

]

lo que resulta en la obtención de la matriz Md:

Md =

[K∆+a2

2a3

a2

a2 a3

](3.37)

Se observa que se cumple la condición de simétria, sin embargo, se evalúa su con-dición de positividad. Para que Md sea definida positiva; d1 > 0 y det |Md| > 0, esdecir:

d1 =K∆+ a2

2a3

> 0

det |Md| = ∆d = d1d3 − d22 = K∆ > 0

con base en lo anterior, se concluye que Md > 0, ya que K se definió como estricta-mente positiva.Una vez definida la matriz Md, se calcula la matriz J2 (3.25) y (3.26), con λ3 = 0 yλ4 = 1, de donde se tiene que:

α1 = C3sen(q2)[λ2

1 + λ1λ2

]+

12

ddq2

(λ1 [C1 + C2 + 2C3cos(q2)] + λ2 [C2 + C3cos(q2)])

α2 = C3sen(q2)λ1 +d

dq2(λ1 [C2 + C3cos(q2)] + λ2C2)


al sustituir los valores ya conocidos de λ1 y λ2, se obtiene:

α1 = (K− 1)C3 (C2 − (K− 1)C3cos(q2)) sen(q2)C2

α1 = C3sen(q2) (K− 1)(3.38)

de este resultado, se obtiene la matriz J2, la cual se define en (3.11), quedando como:

J2 =

[0 pTα(q2)

− pTα(q2) 0

]=

[0 Ja

−Ja 0

](3.39)

donde:

p =1∆

[(a3p1 − a2p2)

(a1p2 − a2p1)

](3.40)

por lo tanto, con (3.38) y (3.40), se deduce que:

Ja = Ja1 + Ja2

Ja1 = −(

(C2+C3cos(q2))p2−C2 p1KC2(−C1C2+C2

3cos2(q2))

)(K− 1)C3sen(q2) (−C2 + C3cos(q2)(K− 1))

Ja2 =((C2+C3cos(q2))p1+[K(C2

3cos2(q2)−C1C2)−(C2+C3cos(q2))2]p2

KC2(−C1C2+C23cos2(q2))

)C3sen(q2)(K− 1)

Ya definida la matriz J2, Md queda finalmente como:

Md =

[K(C1C2−C2

3cos2(q2))+(C2+C3cos(q2))2

C2C2 + C3cos(q2)

C2 + C3cos(q2) C2

]

Con esto se concluye la búsqueda de Md y J2.

3.2 extensión del ida-pbc para el control de un robot bípedo tipo compás 33

3.2 extensión del ida-pbc para el control de un robot bípedo tipo

compás

El desarrollo de está sección, está basado en [31]. La extensión del método IDA-PBC con fricción dinámica, inicia con la función de energía total la cual está dadapor:

H(q, p) =12

pT M(q)−1p + V(q) (3.41)

Enseguida, el modelo de Dahl para un sistema subactuado es:

z = −GΨ(q)GTΣoz +[

GGT]T

q (3.42)

f (z, q) = GGTΣoz + GFvGT q (3.43)

donde z ∈ Rn es un vector de estados no medibles, G =[

Im 0]T∈ Rn×m ,

Ψ = diag{ |q1|fc1

, . . . , |qm|fm}, es una matriz diagonal semidefinida positiva, donde fci, (i =

1, 2, ..., m) denota los parámetros de Coulomb para cada una de las m coordenadasactuadas, Σo = diag{σ01 , ..., σ0n} es una matriz semidefinida positiva, la cual contienelos parámetros de rigidez de las n coordenadas, f ∈ Rn es el vector de fuerzas debi-das a la fricción, Fv = diag{ fv1 , ..., fvm}, es una matriz diagonal, definida positiva, lacual contiene los coeficientes de fricción viscosa de las m coordenadas actuadas. Aun-que el estado z no es medible, se asume que todos los parámetros del modelo sonconocidos. Para el análisis se ha de considerar que la fricción está presente sólo en lacoordenada actuada, lo cual es equivalente a expresar que z = [z1, ..., zm, zm+1, ..., zn]

T

con zk = 0, para k = m + 1, ..., n. Ahora bien, tomando el modelo de fricción de Dahl(3.42)-(3.43), las ecuaciones de movimiento del sistema completo, puede escribirsecomo: q

pz

=

0 In 0−In −GFvGT −GGT

0[

GGT]T−GΨGT

∇qH∇pH∇zW

+

0Gu0

(3.44)

donde u ∈ Rm es el vector de entradas de control; y W(z) = 12 zTΣoz.

3.3 dinámica deseada en lazo cerrado y ley de control 34

3.3 dinámica deseada en lazo cerrado y ley de control

Siguiendo con el procedimiento del método IDA-PBC, una de sus principales ca-racterísticas consiste en asignar una estructura deseada en lazo cerrado, preservandola estructura presentada en (3.44) con una función deseada de energía total dada por:

Hd(q, p) =12

pT Md(q)−1p + Vd(q) (3.45)

donde Md = MdT > 0 y Vd son las matrices deseadas, de inercia y energía potencial,

respectivamente; por lo que el sistema en lazo cerrado queda expresado como: qp˙z

=

0 M−1Md 0

−MdM−1 J2 − GKvGT −GGT

0[

GGT]T

−GΨGT

∇qHd

∇pHd

∇zWd

(3.46)

donde J2 = −J2T y Kv = Kv

T > 0, son matrices libres, z = z− z, siendo z la estima-ción del estado z, así como la introducción de una función semidefinida positiva

Wd(z) =12

zTΣo z (3.47)

Uno de los principales desafios del método IDA-PBC, es la solución del siguientesistemas de Ecuaciones Diferenciales Parciales (EDP), llamadas Matching Equations:

G⊥{∇q(pT M−1p)−MdM−1∇q(pT Md−1p) + 2J2Md

−1p} = 0 (3.48)G⊥{∇qV −MdM−1∇qVd} = 0 (3.49)

donde G⊥ ∈ R(n−m)xm es una matriz aniquiladora por la izquierda de G, ya queG⊥G = 0. Es importante resaltar que la compensación de fricción se omite en laformulación del análisis original. Se puede añadir que en está formulación, (3.48) -(3.49) no se ven modificadas con respecto a la formulación original [26]. Esto se debea que la compensación de fricción incluye una nueva inyección de amortiguamientodada por u = ues + udi + u f ric, donde ues es el control por moldeo de energía, udi esla inyección de amortiguamiento y u f ric es la compensación de fricción [31].

Finalmente si existen soluciones Md y Vd de (3.48) - (3.49), la ley de control que alsustituirla en (3.44) conduce a (3.46), es la siguiente:

u =[

GTG]−1

GT[∇qH −MdM−1∇qHd + J2Md

−1p]

︸︷︷︸ues

−KvGT Md−1p︸︷︷︸

udi

+ FvGT M−1p + GTΣo z︸︷︷︸u f ric

(3.50)

3.4 observador de fricción 35

Debido a que la función de energía potencial no será moldeada para este sistemaesto es, Vd = V, así como tampoco se considera inyección de amortiguamiento, esdecir, la matriz Kv es considerada nula, y por tanto udi = 0, la estructura final de laley de control (3.50) es:

u =[

GTG]−1

GT[∇qH −MdM−1∇qHd + J2Md

−1p]

︸︷︷︸ues

+ FvGT M−1p + GTΣo z︸︷︷︸u f ric

(3.51)

3.4 observador de fricción

En vista de que el modelo de Dahl es dinámico y tiene un estado no medible z,apoyado en el trabajo de [31], es necesario diseñar un tipo de observador. Se sabe queexisten metodologías bien establecidas para llevar a cabo el diseño del observador, ytambién hay varios observadores no lineales que se proponen de manera particular.Sin embargo, inspirado por [20], en este trabajo presentamos el siguiente observadorde fricción:

˙z =[

GGT]T [

M−1p−Md−1p

]− GΨGTΣo z (3.52)

que proporciona una estimación del estado no medible z. Una ventaja de la inclusióndel modelo Dahl (3.42) y (3.43) en (3.44) es la simplicidad para diseñar el observador.Para aclarar este procedimiento, considere que, a partir de la definición z = z− z, laderivada temporal puede escribirse como:

˙z = z− ˙z (3.53)

por tanto, el observador propuesto (3.52) se logra sustituyendo z y ˙z respectivamenteen (3.53). Finalmente, el observador de fricción del robot bípedo de tipo compáspropuesto queda:

˙z =1∆

[[a3p1 − a2p2]−

1K[a2p1 − a2p2]

]− q1

fc1σ01 z1

Parte IV

R E S U LTA D O S

4S I M U L A C I O N E S

El objetivo de control está dirigido en mantener la marcha del robot bípedo, a pesarde los efectos indeseables que la fricción puede causar. Con la finalidad de validar laley de control propuesta, se llevaron a cabo simulaciones usando Matlab presentandoresultados de simulación sobre el comportamiento de la marcha de un modelo de unrobot bípedo tipo compás bajo diferentes condiciones de los parámetros implicados.Para ello, primero se evalúa el comportamiento del bípedo únicamente moldeandola función de energía cinética con diferentes valores de la ganancia de ajuste K delcontrolador. Luego se contemplan condiciones de fricción y variaciones del valor deK, sin incorporar el esquema de compensación de fricción dinámica. Por último seconsideran los mismos parámetros del robot y el controlador donde la marcha se veafectada debido a la fricción en la articulación actuada del robot, pero ahora aplican-do el esquema de control con compensación de fricción dinámica con la finalidadde mantener una marcha estable, tal y como se propone en [31] para una clase desistemas mecánicos subactuados. Los parámetros a modificar son: el coeficiente defricción viscosa, fc1 el parámetro de fricción de Coulomb, el cual es estrictamentepositivo de acuerdo a (3.2) y σ01 es el parámetro de rigidez.

4.1 ida-pbc sin fricción viscosa

El primer análisis que se realiza de la marcha, consiste en reproducir los resultadosreportados en la literatura, por ejemplo [15, 17, 32], que tal como se ha mencionado,no contemplan fricción, sólo se realiza el moldeo de la función de energía cinética.En las gráficas 4.1a, 4.1b y 4.1c se muestra el comportamiento de la marcha delrobot bípedo tipo compás, donde los efectos producidos por la variación de K sonlos que aportan modificaciones en el comportamiento. Las gráficas se presentan encoordenadas hamiltonianas q y p. Conforme se incrementa el valor de K, los pasosse vuelven más lentos, mas no por ello, difieren en su longitud. A pesar de las

4.2 ida-pbc con fricción dinámica 38

variaciones en K, la marcha se mantiene así como la periodicidad de cada paso. Enla Figura 4.2 se aprecia un instante en el cual, el robot bípedo tipo compás mantienesu locomoción.

(a) Ciclo límite de la marcha sinfricción con K = 0.5

(b) Ciclo límite de la marchasin fricción, con K = 0.9

(c) Ciclo límite de la marcha sinfricción con K = 1.5

Figura 4.1: Ciclos límite del bobot bípedo sin fricción viscosa. q1 (eje horizontal) vs p1 (ejevertical). El círculo en verde indica la condición inicial.

Figura 4.2: Imagen de la animación del robot bípedo sin fricción desplazándose sobre unapendiente φ = 3°

4.2 ida-pbc con fricción dinámica

En este epígrafe se considera el mismo modelo del robot bípedo, pero ahora seincorpora el modelo de fricción dinámica sólo en la articulación actuada del robot,usando el modelo de fricción de Dahl. Cabe mencionar que si sólo se considerara fric-ción viscosa en la articulación actuada, el controlador lo pudiera cancelar de maneradirecta y recuperar el mismo comportamiento obtenido cuando la fricción viscosa es-tá ausente en dicha articulación. Sin embargo, es conocido que en la mayoría de los

4.2 ida-pbc con fricción dinámica 39

sistemas mecánicos, la fricción viscosa no aproxima el comportamiento real de esaclase de sistemas. Por tal motivo, en este trabajo se considera aproximar la fricciónen la articulación actuada lo mejor posible con el modelo de fricción de Dahl. En laTabla 4.1, se aprecia un conjunto de valores de los parámetros del modelo de Dahl dela articulación actuada y la ganancia correspondiente al controlador. Los casos (a) y

Tabla 4.1: Parámetros del modelo de fricción de Dahl del robot bípedo tipo compás y laganancia del controlador (3.51)

Caso f v1 f c1 σ01 K

( a ) 0.01 0.5 5 0.5

( b ) 0.1 0.9 10 0.9

( c ) 0.33 1 15.4 1.5

(b) mostrados en la Tabla 4.1, la marcha se mantiene estable, mas no la periodicidaddel paso del robot. Esto se traduce como una marcha con pasos diferentes conformeavanza (ver Figura 4.3). Al comparar las gráficas de la Figura4.3a con la Figura 4.3b,se puede apreciar que la longitud de los pasos se reduce en función del incrementode los valores de los parámetros del modelo de Dhal y en especial de la ganancia Kdel controlador. Las gráficas se presentan en coordenadas hamiltonianas q y p.

(a) Ciclo límite de la marchacon fricción dinámica sóloen la articulación actuada,con K = 0.5

(b) Ciclo límite de la marchacon fricción dinámica sóloen la articulación actuada,con K = 0.9

(c) Ciclo límite de la marchacon fricción dinámica sóloen la articulación actuada,con K = 1.5

Figura 4.3: Ciclo límite de la marcha del robot bípedo, con fricción dinámica sólo en la coor-denada actuada. q1 (eje horizontal) vs p1 (eje vertical). El círculo en verde indicala condición inicial.

En la gráfica de la Figura 4.3c, se muestra un movimiento inicial mínimo por partede las piernas, indicando que los pasos no sólo son cortos, sino también lentos, loque ocasiona que el robot pierda su marcha y finalmente caiga al suelo.

En la Figura 4.4, se aprecia un instante en que el robot bípedo no lograr mantenerla marcha, cayendo de la pendiente de desplazamiento.

4.3 ida-pbc con compensación de fricción dinámica 40

Figura 4.4: Imagen del instante en la animación en que el robot bípedo pierde la marcha porlos efectos de la fricción

4.3 ida-pbc con compensación de fricción dinámica

En el epígrafe anterior se mostraron resultados de simulación, cuando la fricciónen la coordenada actuada se aproximaba con el modelo de fricción de Dahl y elcontrolador IDA-PBC no compensa dicha fricción. Por tanto, en este epígrafe semuestran los resultados cuando la fricción dinámica en la articulación actuada escompensada con el controlador (3.51). Es importante destacar que este controlador(3.51) regula la velocidad y longitud de los pasos, haciendo que se logre una similitudcon la marcha que no contempla fricción, reproduciendo un conjunto de condicionesiniciales muy cercanos entre sí. Esto se puede ver en las Figuras 4.5a, 4.5b y 4.5c. Seobserva que la ley de control (3.51) logra mantener la marcha del robot con friccióndinámica en la articulación actuada. En la Figura 4.6, se muestra una imagen de uninstante en la animación en la que el robot avanza por una pendiente.

4.3 ida-pbc con compensación de fricción dinámica 41

(a) Ciclo límite de la marchacon compensación de fric-ción dinámica, donde K =0.5

(b) Ciclo límite de la marchacon compensación de fric-ción dinámica, donde K =0.9

(c) Ciclo límite de la marchacon compensación de fric-ción dinámica, donde K =1.5

Figura 4.5: Ciclo límite de la marcha del robot bípedo, con compensación de fricción dinámi-ca sólo en la coordenada actuada. q1 (eje horizontal) vs p1 (eje vertical). El círculoen verde indica la condición inicial.

Figura 4.6: Instante de la animación de la marcha del robot bípedo con control con compen-sación de fricción dinámica en la articulación actuada.

Parte V

C O M E N TA R I O S F I N A L E S

5C O N C L U S I O N E S

Se ha presentado un esquema de control IDA-PBC con compensación de friccióndinámica de un modelo de robot bípedo tipo compás para mantener la marcha, cuan-do se considera fricción dinámica en la articulación actuada del robot. La fricción di-námica es representada con el modelo de fricción de Dahl. Resultados de simulaciónilustran el desempeño del esquema de control propuesto. La evidencia mostrada enlas simulaciones resalta cómo la fricción capturada por el modelo de Dahl ocasionaque el robot pueda caer cuando el controlador no compensa la fricción, mientrasque al ser considerada la compensación, se logra mantener la marcha con una ligeravariación en la longitud y velocidad del paso del robot.

5.1 trabajo futuro

Con la finalidad de darle seguimiento y desarrollar mejoras sobre este trabajo, sepropone lo siguiente:

Analizar la estabilidad del ciclo límite del robot cuando la fricción dinámica esconsiderada en el robot.

Construir un robot bípedo tipo compás para implementar la ley de controlpropuesta.

Parte VI

A P É N D I C E

AC Ó D I G O M AT L A B ®

a.1 código del modelo

% ====================================================

% Elaborado por: Dr. Victor Samuel de Leon Gomez 2014

% Adaptado por: Daniel Hirales Valles 2017

% Modelo dinamico hibrido del robot bipedo compas

% ====================================================

%

% Modelo dinamico del robot bipedo compas

% ========================================

% LOS ANGULOS son medidos NEGATIVOS en sentido de las manecillas del reloj

function [xp Fase reset] = Modelo1(t, x, mode)

% ’mode’ es una variable que nos indica si se debe ejecutar la dinamica

% Lagrangiana o la dinamica de impactos. Adopta valores de 1,-1, 1+i y -1+i

Cuando tiene imaginario

% significa que realizo un impacto

% ’Fase’ indica en que parte del caminado se encuentra nuestro modelo. Cuando

es positiva

% esta apoyando con un pie y cuando es negativa esta apoyando con el OTRO % pie

,

% en cero (o lo mas cercano a el) es cuando los 2 pies estan apoyados

% ’reset’ es usado para establecer los nuevos valores de los estados

% despues de la fase de impacto (y ’xp’ se define vacio)

% Este solo es usado la primera vez que corre el programa -----

if isempty(mode) % para inicializar la ’Fase’ (usada en ’trap_101’ y que ahi es

llamada ’phi’)

A.1 código del modelo 46

Fase = 1;

reset = [];

xp =[];

return % Sale rapidamente de la funcion

end

% -----------------------------------------------------------

global phi beta

global contA contB

global t_imp x_imp y_imp

global tau_1 tau_2 E_Cinetica E_Potencial E_Total Long_Paso Periodo_Paso Vprom_

Paso

global t_Sin_imp

%Parametros numericos del modelo

m=5; %masa de las piernas (kg)

m_H=10; %masa de la cadera (kg)

a=0.5; %distancia del tobillo a la masa de la pierna (m)

b=0.5; %distancia de la masa de la pierna a la cadera (m)

l=a+b; %longitud de la pierna (m)

g=9.81; %gravitacion (m/s^2)

%Variables de estado

q = x(1:2);

p = x(3:4);

ZD = x(5:6);

q1 = q(1);

q2 = q(2);

th_s = q1; % Solo para usar el archivo "Controladores.m"

th_ns = q2;

p1 = p(1);

p2 = p(2);

%% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

% Parametros de friccion

%% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

fv1 = 0; %.33

fv2 = 0;

Fv =[fv1 0; 0 fv2];

sigma1= 0; % 15.42


sigma2 = 0;

fc1 = 0.1;

%% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

Z1 = ZD(1);

ZG1 = ZD(2);

% Matriz de inercia

M(1,1)= (m_H+m)*l^2+m*a^2;

M(1,2)=-m*l*b*cos(q1-q2);

M(2,1)= M(1,2);

M(2,2)= m*b^2;

% El momentum se define como p=M*qp, entonces:

qp = inv(M)*p; %Se calcula la velocidad para determinar el momento del impacto

qp1 = qp(1);

qp2 = qp(2);

thp_s = qp1; % Solo para usar el archivo "Controladores.m"

thp_ns = qp2;

% Coordenadas del pie libre TOMANDO EN CUENTA al eje ’x’ del marco de

referencia en

% DIRECCION de la pendiente del terreno, y al eje ’y’ perpendicular al terreno

%--------------------------------

% Altura del pie libre

y_pie = l*cos(q1+phi)-l*cos(q2+phi);

yp_pie = -l*sin(q1+phi)*qp1+l*sin(q2+phi)*qp2;

% Avance del paso

x_pie = -l*sin(q1+phi)+l*sin(q2+phi);

% DETECCION DE DISCONTINUIDAD (En nuestro caso deteccion de un IMPACTO)

% ==============================================

% Se detecta si ha ocurrido un impacto o no ---------------

% Un impacto ocurre cuando se cumple que ’y=0’, ’yp<0’ y ’x>-Numero-’ es decir

% el pie libre toca el suelo, se dirige hacia abajo y esta adelante (al menos

un poco) de la pierna de apoyo

% Cuando ’fase’ es aprox 0 es cuando ocurre un impacto. ’Fase’ es positivo o

negativo dependiendo de con cual pierna esta aopyando

if y_pie<=0 && yp_pie<0 && x_pie>0.2

if real(mode) == 1

Fase = y_pie;

else

Fase = -y_pie;


end

else

if real(mode) == 1

Fase = 1;

else

Fase = -1;

end

end

% --------------------------------

% ELECCION DEL MODELO

% =============================================================

% A continuacion (de acuerdo a lo anterior) se elige si se seleccionara el

modelo con las ecuaciones

% de movimiento de lagrange o con las ecuaciones de impacto. De acuerdo al

% valor de mode (dado por trap_101). Si mode tiene parte imaginaria ocurrio un

impacto, si no, no.

if imag(mode) == 0

%% ======================================

% Modelo conitnuo (Ecs. de movimiento de lagrange)

% -------------------------------------------

%% % Matriz de inercia

M_11= (m_H+m)*l^2+m*a^2;

M_12=-m*l*b*cos(q1-q2);

M_21= M_12;

M_22= m*b^2;

%% % Matriz de fuerzas centrifugas y de coriolis

C_11= 0;

C_12=-m*l*b*sin(q1-q2)*qp2;

C_21= m*l*b*sin(q1-q2)*qp1;

C_22= 0;

%% % Vector de fuerzas gravitacionales

g_1=-(m_H*l + m*a + m*l)*g*sin(q1);

g_2= m*g*b*sin(q2);

% El modelo % M(q)qpp + C(q,qp)qp + g(q) = Bu

% El vector ’q’ esta acomodado: q = [q1 q2]^T

% ------------------------------------------------

M=[M_11 M_12; M_21 M_22];

C=[C_11 C_12; C_21 C_22];

g_vector=[g_1; g_2];


%% % Energias

K = 1/2*qp.’*M*qp; %Energia cinetica

U = m*g*a*cos(q1-beta)+m_H*g*l*cos(q1-beta)+m*g*(l*cos(q1-beta)-b*cos(q2-beta))

; %Energia potencial

E = K + U; %Energia total

%% ==============================

% Controladores

% --------------------------------

% Se utiliza algun controlador en este archivo

Controladores;

% ---------------------------------

% Ecuacion de estado

% --------------------------------

% qpp = inv(M)*( S*Tau- C*qp - g_vector); % Es lo mismo que: M\( S*Tau- C*qp - g_vector);

% ------------------------------------------------------------

A = m_H*l^2 + m*l^2 + m*a^2; %equivale a c1

B = m*l*b; %equivale a c3

CC= m*b^2; % equivale a c2

grad_pMinvp(1,1)= -2*B*sin(q1-q2)*(p1*p2*A*CC+p1*p2*B^2*cos(q1-q2)^2+B*cos(q1-q

2)*p1^2*CC+B*cos(q1-q2)*p2^2*A)/(A^2*CC^2-2*A*CC*B^2*cos(q1-q2)^2+B^4*cos(q

1-q2)^4);

grad_pMinvp(2,1)= 2*B*sin(q1-q2)*(p1*p2*A*CC+p1*p2*B^2*cos(q1-q2)^2+B*cos(q1-q

2)*p1^2*CC+B*cos(q1-q2)*p2^2*A)/(A^2*CC^2-2*A*CC*B^2*cos(q1-q2)^2+B^4*cos(q

1-q2)^4);

grad_H = (1/2)*grad_pMinvp + g_vector;

pp = - grad_H + Tau - fv1*qp -[sigma1*Z1; sigma2];

Z1p = qp1 - sigma1*(abs(qp1)/fc1)*Z1;

ZG1p = qp1 - (CC*p1 + B*cos(q1-q2)*p2)/(k*(A*CC - B^2*(cos(q1-q2))^2)) - abs(qp

1)*sigma1*ZG1/fc1;

% Derivada temporal de la matriz de inercia

% Mp_11= 0;

% Mp_12= m*l*b*sin(q1-q2)*(qp1-qp2);

% Mp_21= Mp_12;


% Mp_22= 0;

% Mp = [Mp_11 Mp_12; Mp_21 Mp_22];

%

% qp = inv(M)*p;

% qpp = inv(M)*(pp - Mp*qp) + extra;

%qpp = inv(M)*(pp - Mp*qp);

xp=[qp; pp; Z1p; ZG1p];

reset = [];

% ===================================================================

% Esta rutina es para Saber el tiempo SIN impactos y almacenar los valores

% de los PARES Y ENERGIAS para luego GRAFICARLOS

if t == 0

Guardar = true;

elseif t > t_Sin_imp(contB)

Guardar = true;

contB = contB+1;

t_Sin_imp(contB) = t; % Tiempo sin tomar en cuenta el tiempo en que ocurren los

impactos

else

Guardar = false;

end

if Guardar

tau_1(contB) = Tau(1);

tau_2(contB) = Tau(2);

E_Cinetica(contB) = K;

E_Potencial(contB) = U;

E_Total(contB) = E;

end

else

%% ===============================

% Modelo discreto (Impactos)

% ----------------------------

% Discontinuidades en las velocidades por conservacion del momento angular

alpha=(q1-q2);

% Matriz de inercia referido al pie de impacto (no soporte) y a la cadera (<-

checar bien)

h_menos11=-m*a*b+(m_H*l^2+2*m*a*l)*cos(alpha);

h_menos12=-m*a*b;


h_menos21=-m*a*b;

h_menos22=0;

h_menos=[h_menos11 h_menos12; h_menos21 h_menos22];

% Los momentums calculados son referidos al pie de soporte y a la cadera,pues

se asumio que al momento

% del impacto la cantidad de movimiento angular en el pie de impacto y en la

cadera se conserva

% por eso es necesario reacomodar los momentums para referirlos al pie de

soporte y no soporte

B = [1 -1;0 1]; % Reacomodo de los momentums al pie de soporte y el

pie de no soporte (es la misma matriz que ayuda a reacomodar los pares)

p_anterior = h_menos*qp; % momentum anterior antes del impacto, en el pie de

impacto (pie de no soporte) y en la cadera

p_New = B*p_anterior; % momentum despues del impacto (es el mismo que el

momentum antes del impacto, pero ahora es referido al pie de soport y no

soporte)

% Se asignan nuevas variables de salida, para que el programa ’trap_101’ sepa

donde va a

% continuar integrando despues de la discontinuidad

xp=[];

reset=[q(2) q(1) p_New(1) p_New(2) qp(2) qp(1)]’;

t_imp(contA) = t; % Guarda el tiempo en que ocurre cada impacto

disp(’------------------------------’);

disp(’Impacto’);

alpha = (q2-q1)/2;

L = 2*l*sin(alpha);

s = sprintf(’t= %g L= %g’,t,L);

disp(s);

% Guardar informacion sobre cada paso(Periodo, largo y velocidad)

% Asi como la posicion X-Y del punto del pie de soporte donde IMPACTA para

poder

% obtener los puntos del pie de soporte, para luego realizar la animacion de

caminado

Long_Paso(contA) = L;

if contA == 1

Periodo_Paso(contA) = t;

x_imp(contA) = -l*sin(q1)+l*sin(q2);

y_imp(contA) = l*cos(q1)-l*cos(q2);


else

Periodo_Paso(contA) = t - t_imp(contA-1);

x_imp(contA) = -l*sin(q1)+l*sin(q2) + x_imp(contA-1);

y_imp(contA) = l*cos(q1)-l*cos(q2) + y_imp(contA-1);

end

Vprom_Paso(contA) = L/Periodo_Paso(contA);

contA = contA+1;

end

A.2 código del control 53

a.2 código del control

% ==========================================

% CONTROLADORES PARA EL ROBOT BIPEDO COMPAS

% Usando los archivos:

% Animacion.m - Principal

% Modelo.m - Modelo del robot bipedo compas

% ==========================================

% Con la forma en la que esta acomodado el modelo el par es el siguiente

:

% Tau = [u_s u_H]’;

% u_s es el par aplicado en el tobillo (o punta) de la pierna de soporte

% u_H es el par aplicado en la cadera

%

OpcionControlador = 6; % Variable para elegir el Controlador

% 1.- Ninguno (Caminado Pasivo)

% 2.- Control por moldeo de energia Potencial

% 3.- Control por moldeo de energia total

% 4.- Control por moldeo de energia cinetica (Lagrangiano Controlado)

% 5.- Control por moldeo de energia cinetica (verion IDA-PBC del

Lagrangiano Controlado)

% 6.- Control por moldeo de energia cinetica (nueva version IDA-PBC)

k = 1; % Ganancia k para el caso del Lagrangiano Controlado y el IDA-PBC

basado en el LC.

k1 = .5; % Ganancia k1 para el controlador IDA-PBC nuevo k=1 No moldea,

k<1 ciclo limite grande, k>1 ciclo limite chico

k2 = 6; % Utilizada en el controlador por moldeo de energia total (

Rapidez de convergencia al ciclo limite)

switch OpcionControlador

case 1

%----------------------------------------------

% Caminado pasivo, sin controlador, entrada cero

%----------------------------------------------

S=[1 0;1 -1];

Tau=[0;0];

case 2

% ------------------------------------------------------------


% Control por moldeo de energia potencial

% ------------------------------------------------------------

g2_1=-(m_H*l + m*a + m*l)*g*sin(th_s-beta);

g2_2=m*g*b*sin(th_ns-beta);

g2_vector=[g2_1; g2_2]; % Modelo opcion 1

%S=[1 0;1 -1]; % Dado por Spong en "Pasivity Based Control of bipedal

locomotion": Los pares de la cadera y el tobillo song randes y

similares

S=[1 -1;0 1]; % Dada por Mi =) Pues es mas logico que se ejersa mas par

en el tobillo que en la cadera

Tau=inv(S)*(g_vector-g2_vector);

%

================================================================================================================================

% % --------------------------------------------------------------

% % Control por moldeo de energia potencial, conmutacion de actuacion

% % reducida (tobillo) y actuacion total

% % --------------------------------------------------------------

% g_bet_1 =-(m_H*l + m*a + m*l)*g*sin(th_s-beta);

% g_bet_2 = m*g*b*sin(th_ns-beta);

% g_beta = [g_bet_1; g_bet_2];

% epsilon= 0.00001; % %-0.417; % %-14 y 5 de pendiente

% if thp_s > epsilon % esta rao aqui

% %S=[1 0; 0 0]; % S=[1 0]’;

% S=1;

% u_s = g_1-g_bet_1+(thp_ns/thp_s)*(g_2-g_bet_2);

% Tau = [u_s;0];

% else

% S=[1 0;1 -1];

% Tau = inv(S)*(g_vector-g_beta);

% end

%

% S=[1 0]’;

% Tau=-45.2323*(q(1)-q(1)^2)-8.2015*dq(1)-44.8021*q(2)-5.9264*dq(2)

case 3

%

================================================================================================================================

%---------------------------------------------------------------

% Control basado en pasividad utilizando energia total

% ---------------------------------------------------------------


g2_1 = -(m_H*l + m*a + m*l)*g*sin(th_s-beta);

g2_2 = m*g*b*sin(th_ns-beta);

g2_vector = [g2_1; g2_2];

S = [1 0;1 -1];

S = [1 -1;1 0];

% k2 = 6; (antes se llamaba k)

Eref = 153.23;

ug = inv(S)*(g_vector-g2_vector);

ut = -k2*(E - Eref)*qp;

Tau = ug + inv(S)*ut;

case 4

% %

================================================================================================================================

% % Lagrangiano Controlado

% %-------------------------------------------------------------

% %Ley de Control por moldeo de energia, subactuado (cambiar

% %condiciones iniciales, la pendiente debe ser 3 grados)

% %-------------------------------------------------------------

% k = 6;

Mtil = M+[2*k*sin(th_s) 0; 0 0];

gtil = g_vector;

S=[1 0]’;

Ctilqp = [k*cos(q1)*qp1^2-m*l*b*sin(q1-q2)*qp2^2; m*l*b*sin(q1-q2)*qp

1^2];

% Ctilqp = [-m*l*b*sin(q1-q2)*qp2^2; m*l*b*sin(q1-q2)*qp1^2]; % ESTE NO

ESTA BIEN pues es la misma C, es decir C.qp nada mas

u_s = inv(S’*S)*S’*(C*qp + g_vector - M*inv(Mtil)*(Ctilqp + gtil)); % es

lo mismo que: (S’*S)\S’*(C*qp+g_vector-M*Mtil\(Ctil*qp+gtil));

Tau = [u_s 0]’;

S=1;

%

case 5

%

================================================================================================================================

% -------------------------------------------------------------

% Ley de Control por IDA-PBC basado en el control por moldeo de

% energia anterior(la pendiente debe ser 3 grados)

% -------------------------------------------------------------


% k = 6;

S=[1 0]’;

% el momentum se define como:

p = M*qp;

p1= p(1);

p2= p(2);

% -----------------------------------------------

% -----------------------------------------------

A=m_H*l^2 + m*l^2 + m*a^2;

B=m*l*b;

CC=m*b^2;

EE=g*(m*(a+l)+m_H*l);

F=g*b*m;

Md(1,1) = (A^2*CC-A*B^2*cos(q1-q2)^2+2*k*sin(q1)*B^2*cos(q1-q2)^2)/(A*CC

+2*CC*k*sin(q1)-B^2*cos(q1-q2)^2);

Md(1,2) = -B*cos(q1-q2);

Md(2,1) = -B*cos(q1-q2);

Md(2,2) = CC;

J2(1,1) = 0;

J2(1,2) = -2*B*sin(q1-q2)*k*sin(q1)*(p2*A*CC+B^2*cos(q1-q2)^2*p2+2*p1*B*cos(q1-q2)*CC)/(A^2*CC^2-2*A*CC*B^2*cos(q1-q2)^2+2*CC^2*k*sin(q1)*A

-2*CC*k*sin(q1)*B^2*cos(q1-q2)^2+B^4*cos(q1-q2)^4);

J2(2,1) = 2*B*sin(q1-q2)*k*sin(q1)*(p2*A*CC+B^2*cos(q1-q2)^2*p2+2*p1*B*cos(q1-q2)*CC)/(A^2*CC^2-2*A*CC*B^2*cos(q1-q2)^2+2*CC^2*k*sin(q1)*A

-2*CC*k*sin(q1)*B^2*cos(q1-q2)^2+B^4*cos(q1-q2)^4);

J2(2,2) = 0;

grad_pMinvp(1,1)= -2*B*sin(q1-q2)*(p1*p2*A*CC+p1*p2*B^2*cos(q1-q2)^2+B*cos(q1-q2)*p1^2*CC+B*cos(q1-q2)*p2^2*A)/(A^2*CC^2-2*A*CC*B^2*cos(q1-

q2)^2+B^4*cos(q1-q2)^4);

grad_pMinvp(2,1)= 2*B*sin(q1-q2)*(p1*p2*A*CC+p1*p2*B^2*cos(q1-q2)^2+B*cos(q1-q2)*p1^2*CC+B*cos(q1-q2)*p2^2*A)/(A^2*CC^2-2*A*CC*B^2*cos(q1-

q2)^2+B^4*cos(q1-q2)^4);

grad_pMdInvp(1,1)= -(2*B*sin(q1-q2)*A^2*p1*p2*CC^2+2*B^2*sin(q1-q2)*A^2*p2^2*cos(q1-q2)*CC-2*A*k*cos(q1)*p1^2*CC^3+2*B^2*sin(q1-q2)*A*CC^2*cos(q1-q2)*p1^2-4*A*CC^2*p1*k*cos(q1)*p2*B*cos(q1-q2)+4*B*sin(q1-q2)

*A*CC^2*p1*k*sin(q1)*p2+4*B^2*sin(q1-q2)*A*p2^2*k*sin(q1)*cos(q1-q2)

*CC-2*A*CC*p2^2*k*cos(q1)*B^2*cos(q1-q2)^2-2*B^4*sin(q1-q2)*A*p2^2*cos(q1-q2)^3+2*CC^2*k*cos(q1)*p1^2*B^2*cos(q1-q2)^2+8*B^2*sin(q1-q2)

*cos(q1-q2)*k*sin(q1)*p1^2*CC^2-2*B^4*sin(q1-q2)*CC*cos(q1-q2)^3*p


1^2+12*B^3*sin(q1-q2)*CC*p1*k*sin(q1)*p2*cos(q1-q2)^2+4*CC*p1*k*cos(

q1)*p2*B^3*cos(q1-q2)^3+2*p2^2*k*cos(q1)*B^4*cos(q1-q2)^4-2*B^5*sin(

q1-q2)*p1*p2*cos(q1-q2)^4+4*B^4*sin(q1-q2)*cos(q1-q2)^3*p2^2*k*sin(q

1))/(A^3*CC^3-3*A^2*CC^2*B^2*cos(q1-q2)^2+3*A*CC*B^4*cos(q1-q2)^4-B

^6*cos(q1-q2)^6);

grad_pMdInvp(2,1)= 2*B*sin(q1-q2)*(A^2*p1*p2*CC^2+A^2*p2^2*B*cos(q1-q2)*CC+A*CC^2*B*cos(q1-q2)*p1^2+2*A*CC^2*p1*k*sin(q1)*p2+2*A*p2^2*k*sin(

q1)*B*cos(q1-q2)*CC-A*p2^2*B^3*cos(q1-q2)^3+4*B*cos(q1-q2)*k*sin(q1)

*p1^2*CC^2-CC*B^3*cos(q1-q2)^3*p1^2+6*CC*p1*k*sin(q1)*p2*B^2*cos(q1-

q2)^2+2*B^3*cos(q1-q2)^3*p2^2*k*sin(q1)-p1*p2*B^4*cos(q1-q2)^4)/(A

^3*CC^3-3*A^2*CC^2*B^2*cos(q1-q2)^2+3*A*CC*B^4*cos(q1-q2)^4-B^6*cos(

q1-q2)^6);

% IDA-PBC

% NOTA: debido a que solo se moldea la energia cinetica V=Vd, es decir,

tanto la energia en lazo abierto como la deseada (en lazo cerrado)

% tiene la misma energia potencial, osea el vector de gravedad es el

mismo

grad_H = (1/2)*grad_pMinvp + g_vector; % Gradiente del Hamiltoneano

grad_Hd = (1/2)*grad_pMdInvp + g_vector; % Gradiente del Hamiltoneano

Deseado

u_s = inv(S’*S)*S’*(grad_H - Md*inv(M)*grad_Hd + J2*inv(Md)*p);

Tau = [u_s 0]’;

S=1;

% %

====================================================================================

case 6

%

====================================================================================

% IDA-PBC 2 Para el Bipedo Compas (Obtenido con el metodo

aplicado al Pendubot)

%

------------------------------------------------------------------------------------

p = M*qp;

p1= p(1);

p2= p(2);


c1=m_H*l^2 + m*l^2 + m*a^2;

c2=m*b^2;

c3=m*l*b;

c4=m_H*l + m*a + m*l;

c5=m*b;

Md(1,1) = (k1*(c2*c1-c3^2*cos(q2)^2)+(c2+c3*cos(q2))^2)/c2;

Md(1,2) = c2+c3*cos(q2);

Md(2,1) = c2+c3*cos(q2);

Md(2,2) = c2;

vectG(1,1) = -g*(m*(a+l)+m_H*l)*sin(q1);

vectG(2,1) = g*b*m*sin(q2);

vectGd = vectG;

J2(1,1) = 0;

J2(1,2) = -(-c2*p1/(k1*(-c2*c1+c3^2*cos(q2)^2))+(c2+c3*cos(q2))*p2/(k

1*(-c2*c1+c3^2*cos(q2)^2)))*(k1-1)*c3*(-c2+c3*cos(q2)*k1-c3*cos(q2))

*sin(q2)/c2+((c2+c3*cos(q2))*p1/(k1*(-c2*c1+c3^2*cos(q2)^2))+(-k1*c

2*c1+k1*c3^2*cos(q2)^2-c2^2-2*c2*c3*cos(q2)-c3^2*cos(q2)^2)*p2/(k1*c

2*(-c2*c1+c3^2*cos(q2)^2)))*c3*sin(q2)*(k1-1);

J2(2,1) =-J2(1,2);

J2(2,2) = 0;

grad_pMinvp(1,1)= 0;

grad_pMinvp(2,1)= 2*c3*sin(q2)*(-c2*p1^2*c3*cos(q2)+p1*p2*c2*c1+p1*p2*c

3^2*cos(q2)^2+2*p1*p2*c3*cos(q2)*c2-p2^2*c2*c1-p2^2*c3^2*cos(q2)^2-p

2^2*c3*cos(q2)*c1-p2^2*c3*cos(q2)*c2)/(c2^2*c1^2-2*c2*c1*c3^2*cos(q

2)^2+c3^4*cos(q2)^4);

grad_pMdInvp(1,1)= 0;

grad_pMdInvp(2,1)= 2*c3*sin(q2)*(-c2*p1^2*c3*cos(q2)+p1*p2*c2*c1+p1*p2*c

3^2*cos(q2)^2+2*p1*p2*c3*cos(q2)*c2-p2^2*c2*c1-p2^2*c3^2*cos(q2)^2-p

2^2*c3*cos(q2)*c1-p2^2*c3*cos(q2)*c2)/(k1*(c2^2*c1^2-2*c2*c1*c3^2*cos(q2)^2+c3^4*cos(q2)^4));

% IDA-PBC

% NOTA: debido a que solo se moldea la energia cinetica V=Vd, es decir,

tanto la energia en lazo abierto como la deseada (en lazo cerrado)

% tiene la misma energia potencial, osea el vector de gravedad es el

mismo


grad_H = (1/2)*grad_pMinvp + vectG; % Gradiente del Hamiltoniano

grad_Hd = (1/2)*grad_pMdInvp + vectGd; % Gradiente del Hamiltoniano

Deseado

u_frix = 0; %fv1*qp1 + sigma1*ZG1; % se descomentara cuando se desee

aplicar la compensacion

u_g = (1/(c2*(-c2*c1+cos(q1-q2)^2*c3^2)))*(cos(q1-q2)^3*c3^3*c5*g*sin(q

2)*k1-cos(q1-q2)^3*c3^3*c5*g*sin(q2)-cos(q1-q2)^2*c3^2*c4*g*sin(q1)*c2*k1+cos(q1-q2)^2*c3^2*c4*g*sin(q1)*c2+cos(q1-q2)*c3^2*sin(q1-q2)*p

2^2+cos(q1-q2)*c3^2*k1*sin(q1-q2)*p1*p2-cos(q1-q2)*c3^2*k1*sin(q1-q

2)*p2^2-cos(q1-q2)*c3^2*sin(q1-q2)*p1*p2+cos(q1-q2)*c3*c5*g*sin(q2)*c2*c1-cos(q1-q2)*c3*c5*g*sin(q2)*k1*c2*c1+c3*k1*sin(q1-q2)*p2^2*c1-c

3*sin(q1-q2)*p2^2*c1-c4*g*sin(q1)*c2^2*c1+c4*g*sin(q1)*c2^2*k1*c1);

u_s= u_g + u_frix; %la ley de control total esta determinada por u

general + u con frix compensada

Tau = [u_s 0]’;

S=1;

end

A.3 codigo de animacion 60

a.3 codigo de animacion

% Animacion de caminado del robot bipedo compas

% ==============================================

clear all;

clc

global phi beta

global contA contB % Contadores

global t_imp x_imp y_imp % Tiempo de impacto

global tau_1 tau_2 E_Cinetica E_Potencial E_Total Long_Paso Periodo_Paso Vprom_

Paso % Variables para graficacion

global t_Sin_imp

% Inicializacion de contadores

contA = 1; % cuenta cada vez que hay un impacto y por lo tanto UN PASO.

contB = 1; % cuenta cada vez que REALMENTE se incrementa el tiempo

t_Sin_imp = 0; % tiempo normal de la simulacion SIN tomar en cuenta el tiempo

cuando ocurren los impactos

% ---------------------------

%Parametros numericos del modelo

% ---------------------------

m=5; %masa de las piernas (kg)

m_H=10; %masa de la cadera (kg)

a=0.5; %distancia del tobillo a la masa de la pierna (m)

b=0.5; %distancia de la masa de la pierna a la cadera (m)

l=a+b; %longitud de la pierna (m)

g=9.81; %gravitacion (m/s^2)

% ============= OPCIONES ===================

t_ini = 0; %Tiempo Inicial

t_fin = 10; %14.8; %Tiempo Final

% Inclinacion de la Pendiente

phi= deg2rad(3); % 5

beta = deg2rad(3) - phi; % Diferencia entre pendiente de marcha pasiva y

pendiente actual

% Mostar animacion del bipedo y diagrama de fase

Animacion_Caminado = true;

Animacion_PlanoFase = true; % Solo sirve si Animacion_Caminado = true;

Animacion_PlanoFaseIndividual = true; % Solo sirve si Animacion_Caminado = true

;

% Condiciones iniciales

% -------------------------------------------------------------------------


% para el controlador por moldeo de energia potencia, total y sin controlador

% fac = 1.0; %1.07 Factor de modificacion de las condiciones iniciales

% x_Ini = fac*[0.2187+beta -0.3234+beta -1.0918 -0.3772]’;

% Caminado pasivo

% q1 = 0.2187+beta;

% q2 = -0.3234+beta;

% qp1 = -1.0918;

% qp2 = -0.3772;

% x_Ini = [0.2187+beta -0.3234+beta -1.0918 -0.3772]’; % Condiciones

Iniciales ciclo Pasivo

% Para el IDA-PBC

% Con k1 = 1 Md=M por lo tanto no se moldea nada, y es como si no trajera

controlador

% Ciclos limites mas grandes

% x_Ini = [ .2216 -.3263 -1.1444 -.3777 0 0]’; % k1 = .9 %agregar CI de Z y

ZG

% x_Ini = [ .2243 -.3291 -1.1941 -.3756 0 0]’; % k1 = .8

% x_Ini = [ .2269 -.3316 -1.2417 -.3712 0 0]’; % k1 = .7

% x_Ini = [ .2294 -.3341 -1.2878 -.3646 0 0]’; % k1 = .6

x_Ini = [ .2318 -.3366 -1.3323 -.3556 0 0]’; % k1 = .5

% x_Ini = [ .2306 -.3421 -1.3714 -.3418 0 0]’; % k1 = .4 % Pasos NO simetricos

% x_Ini = [ .2258 -.3495 -1.4077 -.3370 0 0]’; % k1 = .3 % Pasos NO simetricos

% Ciclos limites mas chicos

% x_Ini = [ .1942 -0.2989 -.7787 -0.345 0 0]’; % k1 = 1.5

% x_Ini = [ .1441 -0.2488 -.4829 -0.2795 0 0]’; % k1 = 1.8

% x_Ini = [ .0871 -0.1918 -.2976 -0.1987 0 0]’; % k1 = 1.9

% x_Ini = [ .0484 -0.1531 -.1990 -0.1342 0 0]’; % k1 = 1.93

% Caminado pasivo

q1 = x_Ini(1);

q2 = x_Ini(2);

qp1 = x_Ini(3);

qp2 = x_Ini(4);

Z1 = x_Ini(5);

ZG1 = x_Ini(6);

q =[q1 q2]’;

qp = [qp1 qp2]’;

ZD = [Z1 ZG1]’;

% Matriz de inercia


M(1,1)= (m_H+m)*l^2+m*a^2;

M(1,2)=-m*l*b*cos(q1-q2);

M(2,1)= M(1,2);

M(2,2)= m*b^2;

% El momentum se define como:

p = M*qp;

% ===== x_Ini = [q1 q2 p1 p2]’; ========

% para el controlador por moldeo de energia cinetica

% -----------------------------------------------------

% x_Ini = [q; p]; %

x_Ini = [q; p; ZD];

% OBTENCION DE LA SOLUCION del modelo dinamico hibirod del robot bipedoCompas

% ======================================================================

% Integrador que detecta los puntos donde la solucion es dicontinua

addpath hybrid

[t, x] = trap_101a(’Modelo1’,t_ini,t_fin,x_Ini); % Modelo dinamico Hamiltoniano

del bipedo compas

rmpath hybrid

% Se almacenan los estados

q = x(:,[1,2]);

p = x(:,[3,4]);

ZD = x(:,[5,6]);

Z1 = x(:,5); % el vector ZD hace referencia a z y z gorro.

ZG1 = x(:,6);

th_s = x(:,1);

th_ns = x(:,2);

p_s = x(:,3);

p_ns = x(:,4);

thp_s = x(:,5);

thp_ns = x(:,6);

% OBTENCION DEL MOMENTO EN QUE OCURREN LOS IMPACTOS

% ======================================================================

% Indice de los impactos (Posicion del vector donde ocurre el impacto)

--------------------------------

Impacto = zeros(length(t_imp),1);

for i=1:length(t_imp)

v = find(t==t_imp(i)); % Se detecta la posicion del vector donde ocurrio el

impacto


Impacto(i) = v(2); % Existen 2 posiciones la primera antes del impacto y la

segunda despues del impacto (se tomo el segundo elemento)

end

Impacto(i+1) = 0; % este valor solo es para evitar errores

if isempty(Impacto), Impacto=inf; end % SI NO SE RALIZO NINGUN IMPACTO, SE LE

ASIGNARA ESE VALOR PARA PODER IDENTIFICARLO

% OBTENCIoN DEL vector de posicion y velocidad PARA CADA PIERNA (durante todo

el caminado) ---------------------------

% ======================================================================

cambio = 1;

j = 1;

th_1 = zeros(length(t),1);

p_1 = zeros(length(t),1);

th_2 = zeros(length(t),1);

p_2 = zeros(length(t),1);

thp_1 = zeros(length(t),1);

thp_2 = zeros(length(t),1);

for i=1:length(t)

if i == Impacto(j)

cambio = -cambio;

j = j+1;

end

if cambio == 1 % Al momento del impacto se intercambian los valores de la

pierna a de apoyo y la de soporte

th_1(i) = th_s(i);

p_1(i) = p_s(i);

thp_1(i) = thp_s(i);

th_2(i) = th_ns(i);

p_2(i) = p_ns(i);

thp_2(i) = thp_ns(i);

else

th_1(i) = th_ns(i);

p_1(i) = p_ns(i);

thp_1(i) = thp_s(i);

th_2(i) = th_s(i);

p_2(i) = p_s(i);

thp_2(i) = thp_ns(i);

end

end

% OBTENCIoN DE LOS PUNTOS DEL PIE DE SOPORTE, PIE LIBRE Y CADERA DURANTE TODO

EL CAMINADO


% Para poder realizar la animacion del camiando del robot bipedo compas

%----------------------------------------------------------

x_s = zeros(1,length(t));

y_s = zeros(1,length(t));

if Impacto~=inf % Si no existio algun impacto, no se realiza esta rutina

for i=1:length(Impacto)-1

x_s(Impacto(i):Impacto(i+1)) = x_imp(i); % Posicion del Pie de soporte

y_s(Impacto(i):Impacto(i+1)) = y_imp(i);

end

x_s(Impacto(i):length(t)) = x_imp(i); % Posicion del Pie de soporte

y_s(Impacto(i):length(t)) = y_imp(i);

end

% * * CINEMATICA DIRECTA DE POSICIoN * * *% Puntos del para graficar el bipedo con rodillas

% ------------------------------------------------

% Cadera

x_mH=-l*sin(th_s)+x_s’;

y_mH=l*cos(th_s)+y_s’;

% Pie libre

x_ns=-l*sin(th_s)+l*sin(th_ns)+x_s’;

y_ns=l*cos(th_s)-l*cos(th_ns)+y_s’;

% =============================================================

figure(’Name’,’Diagramas de fase’);

aux1 = get(gcf,’Position’); % Almacena el tamano y posicion default de la

figura

set(gcf,’Position’,[590 aux1(2)-100 aux1(3)+200 aux1(4)+100]); % Cambia la

posicion y tamano de la figura

% %--------------------------------

% % Plano de Fase: Ciclo completo de una sola pierna (th_1 vs p_1)

subplot(’Position’,[0.07, 0.51, 0.42, 0.47]);

FaseTheta_1pie = plot3(t,th_1,p_1,’b’);

view([90 0 0]);

axis tight

Ejes1 = axis;

Ejes1 = [Ejes1(1)-.1 Ejes1(2)+.1 Ejes1(3)-.1 Ejes1(4)+.1 Ejes1(5)-.1 Ejes1(6)

+.1];

if Animacion_PlanoFase && Animacion_Caminado

FaseTheta_1pie = plot3(t(1),th_1(1),p_1(1),’b’);

hold on

FaseTheta_1pie_circ = plot3(t(1),th_1(1),p_1(1),’o’,’MarkerEdgeColor’,’k’,’

MarkerFaceColor’,’g’,’MarkerSize’,5);

end

ylabel(’\theta_{1pie}(rad)’);


zlabel(’p_{1pie}(rad/s)’);

xlabel(’t (s)’);

set(gca,’FontSize’,7)

view([90 0 0]);

grid on

axis(Ejes1)

if Impacto==inf, axis auto; end % Si no hay impacto los ejes se ajustan

automaticamente

%--------------------------------

% Plano de Fase: Ciclo completo de una sola pierna (th_2 vs p_2)


FaseTheta_2pie = plot3(t,th_2,p_2,’black’);

view([90 0 0]);

axis tight

Ejes2 = axis;

Ejes2 = [Ejes2(1)-.1 Ejes2(2)+.1 Ejes2(3)-.1 Ejes2(4)+.1 Ejes2(5)-.1 Ejes2(6)

+.1];

if Animacion_PlanoFase && Animacion_Caminado

FaseTheta_2pie = plot3(t(1),th_2(1),p_2(1),’k’);

hold on

FaseTheta_2pie_circ = plot3(t(1),th_2(1),p_2(1),’o’,’MarkerEdgeColor’,’k’,’


end

ylabel(’\theta_{2pie}(rad)’);

zlabel(’p_{2pie}(rad/s)’);



view([90 0 0]);

grid on

axis(Ejes2)


automaticamente

%--------------------------------

% Plano de Fase Pierna: de No Soporte (th_ns vs p_ns)


FaseTheta_ns = plot3(t,th_ns,p_ns,’.b’);

if Animacion_PlanoFaseIndividual && Animacion_Caminado

FaseTheta_ns = plot3(t(1),th_ns(1),p_ns(1),’b.’);

hold on

FaseTheta_ns_circ = plot3(t(1),th_ns(1),p_ns(1),’o’,’MarkerEdgeColor’,’k’,’


end

ylabel(’\theta_{ns}(rad)’);


zlabel(’p_{ns}(rad/s)’);



grid on

view([90 0 0]);

axis(max(Ejes1,Ejes2));


automaticamente

%--------------------------------

% Plano de Fase: Pierna de Soporte (th_s vs p_s)


FaseTheta_s = plot3(t,th_s,p_s,’.r’);

if Animacion_PlanoFaseIndividual && Animacion_Caminado

FaseTheta_s = plot3(t(1),th_s(1),p_s(1),’r.’);

hold on

FaseTheta_s_circ = plot3(t(1),th_s(1),p_s(1),’o’,’MarkerEdgeColor’,’k’,’


end

ylabel(’\theta_{s}(rad)’);

zlabel(’p_{s}(rad/s)’);



grid on

view([90 0 0]);

axis(max(Ejes1,Ejes2));


automaticamente

if Animacion_Caminado

%

====================================================================================

% -------------------------

% Animacion caminado

% -------------------------

figure(’Name’,’Animacion del caminado’)

aux1 = get(gcf,’Position’); % Almacena el tamano y posicion default de la

figura

set(gcf,’Position’,[10 aux1(2) aux1(3) aux1(4)]); % Cambia la posicion de la

figura

%--------------------------------

plot([-1 t_fin],[tan(phi) -t_fin*tan(phi)],’-k’,’LineWidth’,2); % grafica la

Pendiente

hold on


Biped_Apoyo = plot([0 x_mH(1)],[0 y_mH(1)],’-or’,’LineWidth’,2,’MarkerEdgeColor

’,’k’,’MarkerFaceColor’,’w’,’MarkerSize’,10);

Biped_NoApoyo = plot([x_mH(1) x_ns(1)],[y_mH(1) y_ns(1)],’-ob’,’LineWidth’,2,’

MarkerEdgeColor’,’k’,’MarkerFaceColor’,’w’,’MarkerSize’,5);

grid on

Ejes = [min(x_ns)-0.2 max(x_ns)+0.2 min(y_ns)-0.2 max(y_mH)+0.2];

axis(Ejes);

xlabel(’x (m)’);

ylabel(’y (m)’);

text((Ejes(1)+Ejes(2))/2-0.2,Ejes(4)+0.1,’t = [s]’);

text_t=text((Ejes(1)+Ejes(2))/2+0.2,Ejes(4)+0.1,’00.00’);

% =================== ANIMACION ===============================

for i=1:length(t)

set(Biped_Apoyo,’XData’,[x_s(i) x_mH(i)],’YData’,[y_s(i) y_mH(i)]);

set(Biped_NoApoyo,’XData’,[x_mH(i) x_ns(i)],’YData’,[y_mH(i) y_ns(i)]);

pause(0.01)

drawnow

s=sprintf(’ %2.2f’,t(i));

set(text_t,’String’,s);

if Animacion_PlanoFase

set(FaseTheta_1pie,’XData’,t(1:i),’YData’,th_1(1:i),’ZData’,p_1(1:i));

set(FaseTheta_1pie_circ,’XData’,t(i),’YData’,th_1(i),’ZData’,p_1(i));

set(FaseTheta_2pie,’XData’,t(1:i),’YData’,th_2(1:i),’ZData’,p_2(1:i));

set(FaseTheta_2pie_circ,’XData’,t(i),’YData’,th_2(i),’ZData’,p_2(i));

end

if Animacion_PlanoFaseIndividual

set(FaseTheta_ns,’XData’,t(1:i),’YData’,th_ns(1:i),’ZData’,p_ns(1:i));

set(FaseTheta_ns_circ,’XData’,t(i),’YData’,th_ns(i),’ZData’,p_ns(i));

set(FaseTheta_s,’XData’,t(1:i),’YData’,th_s(1:i),’ZData’,p_s(1:i));

set(FaseTheta_s_circ,’XData’,t(i),’YData’,th_s(i),’ZData’,p_s(i));

end

end

% ===============================================================

end

% ===========================================================

% Grafica para analizar de cerca los puntos de impacto

% -----------------------------------------------------------

Plano de Fase: Ciclo completo de la pierna 1 (th_1 vs p_1)

figure(’Name’,’Fase de Caminado’);

figure (10)

hold on

plot(th_1,p_1,’b’);


xlabel(’\theta_{1pie}(rad)’);

ylabel(’p_{1pie}(rad/s)’);


grid on

% Plano de Fase: Ciclo completo de la pierna 2(th_2 vs p_2)

figure(’Name’,’Fase de Caminado’);

figure (11)

hold on

plot(th_2,p_2,’k’);

xlabel(’\theta_{2pie}(rad)’);

ylabel(’p_{2pie}(rad/s)’);


grid on

% ========================================================================

if Impacto==inf % Se identifica que no hubo impactos y ya no se grafica lo

demas

disp(’------------------------------’);

disp(’No se alcanzo a realizar ningun paso’);

return;

end

A.4 codigo trap-101 69

a.4 codigo trap-101

unction [tout, yout] = trap_101(dyfun, t0, tf, y0, step, tol, trace)

% TRAP_101 Solve differential equations, "trapezoidal" method.

% mod 6 -- 4 March 1996

% TRAP_101 integrates a system of ordinary differential equations

% using the "trapezoidal" algorithm, with a hybrid interpolation

% scheme to catch state events (points where ydot is discontinuous).

%

% CALL: [tout,yout] = trap_101(dyfun,t0,tf,y0,step,tol,trace)

% INPUT:

% dyfun - String containing name of user-supplied problem description.

% Call: [ydot,phi,reset] = fun(t,y,mode) where dyfun = ’fun’.

% t - Time (scalar).

% y - Solution column-vector.

% mode - Governs "mode" of the dynamic model

% ydot - Returned derivative column-vector; ydot(i) = dy(i)/dt.

% phi - Returned switching function signalling mode change

% reset - Returned reset value of the state y

% t0 - Initial value of t.

% tf - Final value of t.

% y0 - Initial value column-vector.

% step - The specified integration step. (Default: step = 1.e-2).

% tol - Zero-finding tolerance. (Default: tol = eps).

% trace - If nonzero, each step is printed, if ==2 zero-finding debug

% output is provided, if == 3 state-event handling debug out-

% put is provided, if == 4 you get both. (Default: trace = 0).

%

% OUTPUT:

% T - Returned integration time points (column-vector).

% Y - Returned solution, one solution column-vector per state.

%

%% JH Taylor, University of New Brunswick, 13 May 1995 and subsequent mods.

%% April 1999 - revised for new matlab syntax requirements; isempty(X) used

%% to replace "if X == []", and reserved work "catch" replaced with "gotcha".

% This routine contains part of MATLAB’s FZERO algorithm, with permission.

%% Initialization

if nargin < 5, step = 1.e-2; end

if nargin < 6, tol = eps; end

if nargin < 7, trace = 0; end

disp(’This is routine TRAP_101, version 6; 09 April 1999’);

disp(’Simulating Compass Gait Biped Robot’);


t = t0;

h = step;

y = y0(:);

%% Call model to get phi for initial mode evaluation:

mode = [];

[junk,phi,trash] = feval(dyfun,t,y,mode);

mode = sign(phi);

mdim = length(mode);

%% allocate arrays, store initial condition:

chunk = 256;

tout = zeros(chunk,1);

yout = zeros(chunk,length(y));

k = 1;

tout(k) = t;

yout(k,:) = y.’;

% initialize arrays for handling state events

ym = zeros(length(y),mdim + 1);

phim = zeros(mdim,mdim + 1);

tv = zeros(1,mdim + 1);

cv = zeros(1,mdim + 1);

% set the elapsed time stopwatch:

timer = clock;

% also, set the total time stopwatch:

max_timer = clock;

if trace, dt = h, time_0 = t, y_0 = y’, phi_0 = phi’, m_0 = mode’, end

%

%% Main integration loop

while (t < tf) & (t + h > t)

if t + h > tf, h = tf - t; end

% report progress in simulation:

if (trace & etime(clock,timer) > 30),

timer = clock;

tot_elapsed = etime(clock,max_timer);

disp([’T_real = ’,num2str(tot_elapsed),’ , T_sim = ’,num2str(t,6)]);

end

% save data from last "accepted" point:

yold = y(:); told = t; phiold = phi;

% Also: compute the slope at the accepted point:

[ydotold,junk,trash]=feval(dyfun,t,y,mode); ydotold=ydotold(:);

% Update the solution (TRIAL point!)

tt = t + h;

yp = yold + h*ydotold; % use old ydot to predict; get new ydot ...

[ydot,junk,trash] = feval(dyfun,tt,yp,mode); ydot = ydot(:);

yt = yold + h*(ydotold + ydot)/2; % ... and correct (trapezoidal rule)


% Update the (trial point) phi:

[junk,phi,trash] = feval(dyfun,tt,yt,mode); phi=phi(:);

% load into arrays for use in SEH:

for im = 1:mdim+1

ym(:,im) = yt;

phim(:,im) = phi;

tv(im) = tt;

cv(im) = 0;

end

gotcha = 0;

for im = 1:mdim

%% if phi(im)*phiold(im) < 0, THIS TEST IS BAD FOR MODE == 0 CASES

if sign(phi(im)) ~= sign(phiold(im)) % NEW TEST

%% possible state event detected:

gotcha = 1;

cv(im) = 1;

niter = 0;

% set up fzero-like zero-finding scheme:

fini = 1;

% NB: bias phi so that the boundary is surely crossed:

bias = -tol*sign(phi(im));

phia = phiold(im)+bias; phib = phi(im)+bias; phic = phia;

ha=0; hb=h; hc=0; hd=h; he=h; % init hd & he to elim complaints...

while fini > 0

% hybrid zero-finding algorithm based on fzero

% hb = best h so far, ha is previous hb, and hc is an h

% such that phi has the opposite sign from that at hb.

niter = niter + 1;

if (phib > 0) == (phic > 0)

hc = ha; phic = phia;

hd = hb - ha; he = hd;

end

if abs(phic) < abs(phib)

ha = hb; hb = hc; hc = ha;

phia = phib; phib = phic; phic = phia;

end

if (trace == 2) | (trace == 4),

msg1 = [’ha = ’,num2str(ha,8),’ hb = ’,num2str(hb,8),...

’ hc = ’,num2str(hc,8),’ hd = ’,num2str(hd,8),...

’ he = ’,num2str(he,8)];

msg2 = [’phia = ’,num2str(phia,8),’ phib = ’,num2str(phib,8),...

’ phic = ’,num2str(phic,8)];

disp(msg1);

disp(msg2);


end

hm = 0.5*(hc - hb);

% Convergence test and possible side-door exit:

toler = 2.0*tol*max(abs(hb),1.0);

if (abs(hm) <= toler | phib == 0.0)

% NB: abs(hm) <= toler => as close as you’re gonna get!

% see which "h" to use to make sure SE has happened:

if sign(phib) ~= sign(phiold(im) + bias), hb = hb;

else hb = hc; end % hb or hc _has_ to be past the SE

%% evaluate the current y and phi:

tb = told + hb;

yp = yold + hb*ydotold;

[ydot,junk,trash] = feval(dyfun,tb,yp,mode); ydot = ydot(:);

yb = yold + hb*(ydotold + ydot)/2; % trapezoidal rule again

[junk,phit,trash] = feval(dyfun,tb,yb,mode); phit=phit(:);

tv(im) = tb;

ym(:,im) = yb;

phim(:,im) = phit;

fini = 0;

end

% Choose bisection or interpolation

if (abs(he) < tol) + (abs(phia) <= abs(phib))

% Bisection

hd = hm; he = hm;

else

% Interpolation

zs = phib/phia;

if (ha == hc)

% Linear interpolation

zp = 2.0*hm*zs;

zq = 1.0 - zs;

else

% Inverse quadratic interpolation

zq = phia/phic;

zr = phib/phic;

zp = zs*(2.0*hm*zq*(zq - zr) - (hb - ha)*(zr - 1.0));

zq = (zq - 1.0)*(zr - 1.0)*(zs - 1.0);

end;

if zp > 0, zq = -zq; else zp = -zp; end;

% Is interpolated point acceptable

if (2.0*zp < 3.0*hm*zq - abs(tol*zq)) * (zp < abs(0.5*he*zq))

he = hd; hd = zp/zq;

else

hd = hm; he = hm;


end;

end % bisection/interpolation

% evaluate next point

ha = hb;

phia = phib;

if abs(hd) > tol, hb = hb + hd;

else if hb > hc, hb = hb - tol;

else hb = hb + tol;

end

end

tb = told + hb;

yp = yold + hb*ydotold;

[ydot,junk,trash] = feval(dyfun,tb,yp,mode); ydot = ydot(:);

yb = yold + hb*(ydotold + ydot)/2; % trapezoidal rule again

[junk,phit,trash] = feval(dyfun,tb,yb,mode); phit = phit(:);

phib=phit(im) + bias;

% normal exit (converged to solution):

if abs(phib) < tol

tv(im) = tb;

ym(:,im) = yb;

phim(:,im) = phit;

fini = 0;

end

end % of WHILE fini > 0 loop

if (trace == 2) | (trace == 4),

msg = [’N_iter = ’,num2str(niter),’ im = ’,num2str(im),’ cv(im) = ’,...

num2str(cv(im)),’ tv(im) = ’,num2str(tv(im)) ];

disp(msg)

time_seh = tb, y_seh = yb’

end

end % of IF sign(phi(im)) ~= sign(phiold(im)) statement

end % of FOR im = 1:mdim loop

if gotcha == 0 % we didn’t catch a SE

imin = mdim + 1; % the trial point is accepted

t = tv(imin);

y = ym(:,imin);

phi = phim(:,imin);

else

% check for earliest change:

tmin = tt; %% values from trapezoidal integration

imin = mdim + 1;

for im = 1:mdim

if (tv(im) < tmin),

tmin = tv(im);


imin = im;

end

end

t = tmin; % this is an accepted point (with SE(s))

y = ym(:,imin);

phi = phim(:,imin);

if (trace == 3) | (trace == 4),

msg=[’Point C (SEH): t = ’,num2str(t), ...

’ , imin = ’,num2str(imin)];

disp(msg)

y_SEH = y’, phi_SEH = phi’, cv_SEH = cv, mode_SEH = mode

end

% one or more SEs have happened; rigorous test for simultaneity:

num_ses = 0;

for im = 1:mdim

if (sign(phim(im,imin)) ~= sign(phiold(im)) & cv(im) == 1),

num_ses = num_ses + 1;

%% cv(im) = 1; % be sure to retain it

else

cv(im) = 0; % discard it

end

end

if (trace & num_ses == 0),

disp([’Oops: SE glitch at t = ’,num2str(tv(im))]); end

if (trace & num_ses > 1), disp([’Note: simultaneous SEs at t = ’,...

num2str(tv(im))]); end

% get the reset value of the state and/or of phi

% (NB: make mode(im s.t. cv(im) = 1) complex):

rmode = mode;

for im = 1:mdim

if cv(im) == 1, rmode(im) = rmode(im) + j; end

end

[junk,phi_reset,y_reset] = feval(dyfun, t, y, rmode);

% see if state reset is called for:

if ~isempty(y_reset),

% put the captured point in the output (make room for state reset)

k = k+1;

if k > length(tout)

tout = [tout; zeros(chunk,1)];

yout = [yout; zeros(chunk,length(y))];

end

tout(k) = t;

yout(k,:) = y.’;

% set up state reset value to be put on next:


y = y_reset(:); %% y_reset; time doesn’t change

if (trace == 3) | (trace == 4),

msg = [’Point D (y_reset): t = ’,num2str(t)];

disp(msg)

y_reset = y’, phi_reset = phi’, cv_reset = cv, mode_reset = mode

end

end % of y_reset

% now, at last, update the mode:

for im = 1:mdim

if cv(im) == 0, % nothing to be done

% see if we are entering "on-boundary" situation:

elseif phi_reset(im) == 0, phi(im) = 0; mode(im) = 0;

% otherwise implement crossing event:

else

if sign(phi(im)) == sign(mode(im)),

disp([’Inconsistent mode at t = ’,num2str(tv(im))]); end

mode(im) = sign(phi(im));

end

end % of mode updating

if (trace == 3 | trace == 4), mode_update = mode, end

end % of IF catch == 0 statement

% we have a good point to add - y_trap, y_se, or y_reset:

k = k+1;

if k > length(tout)

tout = [tout; zeros(chunk,1)];

yout = [yout; zeros(chunk,length(y))];

end

tout(k) = t;

yout(k,:) = y.’;

if trace, time_k = t, y_k = y’, end

end % of main WHILE loop

if (t < tf)

disp([’Premature end of simulation, t = ’,num2str(t)]);

end

tout = tout(1:k);

yout = yout(1:k,:);

R E F E R E N C I A S

[1] Acosta J., Ortega R., Astolfi A. and Mahindrakar A., Interconnection and dampingassignment pasivity-based control of mechanical systems with underactuation degreeone, IEEE Transactions on Automatic Control, Vol. 50, No. 12, pp. 1936-1955,2005.

[2] Alfaro P., Modelamiento y estrategia de control virtual de la caminata de un robotbípedo, Tesis de Licenciatura, Universidad Nacional de Ingeniería, Lima, Perú,2012.

[3] Asano F. and Luo Z., Asymptotically stable biped gait generation based on stabilityprinciple of rimless wheel, Cambridge University Press, Vol. 27, pp. 949-958, 2009.

[4] Bloch A., Leonard N., and Marsden J., Controlled lagrangians and the stabilizationof mechanical systems I: The first matching theorem, IEEE Transactions on AutomaticControl, Vol. 45, pp. 2253-2270, 2000.

[5] Collins S., Ruina A., Tedrake R. and Wisse M., Efficient bipedal robots based onpassive dynamic walker, Science, Vol. 307, pp. 1082-1085, 2005.

[6] Contreras L. y Vargas L., Generación de modelos de caminata bípeda a través dediversas técnicas de modelamiento, Revista Científica y Tecnológica de la Facultadde Ingeniería, Universidad Distrital Francisco José de Caldas, Bogotá, Colombia,Vol. 11, No. 2, pp. 4-13, 2006

[7] Corke P., Robotics, vision and control: fundamental algorithms in MATLAB, Ed.Springer, 2011.

[8] Dahl P., Solid friction damping of mechanical vabrations AIAA Journal, pp. 1675-1682, 1976.

[9] Duarte J., Montero B., Ospina-Henao P. y González E., Mecánica lagrangianapara el modelado dinámico y simulación mediante Simulink-Matlab de un péndulo deFuruta, Congreso Internacional de Ciencias Básicas e Ingeniería, Villavicencio,Colombia, 2016.

referencias 77

[10] Duindam V. and Stramigioli S., Port-based control of a compass-gait bipedal robot,IFAC, Prague, Czech Republic, pp. 471-476, 2005.

[11] García D., Control de un vehículo subacuático, Tesis de Maestría, TecnológicoNacional de México-Instituto Tecnológico de La Paz, La Paz, B.C. Sur, México,2016.

[12] Gómez-Estern F., Control de sistemas no lineales basado en la estructura hamiltoniana,Tesis Doctoral, Universidad de Sevilla, 2002.

[13] Gordillo F., Estabilidad de sistemas no lineales basada en la teoría de Liapunov, RevistaIberoamericana de Automática e Informática Industrial, ISSN: 1697-7912, Vol. 6,No. 2, pp. 5-16, 2009.

[14] Goswami A., Thuilot B. and Espiau B., A study of the passive gait of a compass likebiped robot: Symmetry and chaos, International Journal of Robotics Research, Vol.17, No. 12, pp. 1282-1301, 1998.

[15] Goswami A., Thuilot B. and Espiau B., Compass-like biped robot part I: Stabilityand bifurcation of passive gaits, Institut National de Recherche en Informatique eten Automatique (INRIA), 1996.

[16] Goswami A., Espiau B. and Keramane A., Limit cycles in a passive compass gaitbiped and passivity-mimicking control laws, Journal of Autonomous Robots, Vol. 4,No. 3, pp. 273-286, 1997.

[17] Grizzle J., Hurst J., Morris B., Park H.W. and Sreenath K., Mabel, a new roboticbipedal walker and runner, American Control Conference, St. Louis, USA, pp.2030-2036, 2009.

[18] Higuera C., Implementación del control de un vehículo subacuático, Tesis de Maestría,Tecnológico Nacional de México-Instituto Tecnológico de La Paz, La Paz, B.C.Sur, México 2016.

[19] Kelly R. y Santibáñez V., Control de movimiento de robots manipuladores, Ed. PEAR-SON EDUCATION, S.A., Madrid, 2003.

[20] Kelly R., de la Torre D. and Campa R., Energy control of the furuta pendulum withfriction: speed gradient method in Hamiltonian formulation, VI Congreso Mexicanode Robótica, San Luis Potosí, México, 2004.

[21] De León Gómez V., Control de robots bípedos, Tesis Doctoral, Tecnológico Nacionalde México-Instituto Tecnológico de la Laguna, Torreón, Coahuila, México, 2014.

referencias 78

[22] De León V., Santibáñez V. and Sandoval J., Interconnection and damping assign-ment passivity based control for a compass-like biped robot, International Journal ofAdvanced Robotics Systems, pp. 1-18, July-August 2017.

[23] Marizcal A., Diseño de un exoesqueleto para terapias de rehabilitación, Tesis deMaestría, Tecnológico Nacional de México-Instituto Tecnológico de La Paz, LaPaz, B.C. Sur, México, 2017.

[24] McGeer T., Passive dynamic walking, International Journal of Robotics Research,Vol. 9, No. 2, pp. 62-82, 1990.

[25] McMahon T.A., Mechanics of locomotion, The International Journal of RoboticsResearch, Vol. 3, No. 2, pp. 4-28, 1984.

[26] Ortega R., Spong M., Gómez-Estern F. and Blankenstein G., Stabilization of aclass of underactuated mechanical system via interconnection and damping assignment,IEEE, Transactions on Automatic Control, Vol. 47, No. 8, pp. 1218-1233, 2002.

[27] Ortega R., Van der Shaft A., Maschke B. and Escobar G., Interconnection anddamping assignment pasivity-based control of port-controlled hamiltinian systems, Au-tomatica, Vol. 38, No. 4, pp. 585-596, 2002.

[28] Rodríguez C. Modelado y simulación de sistemas dinámicos utilizando el formalis-mo Euler-Lagrange, Reporte Técnico, Centro de Desarrollo Industrial San Pablo,Querétaro, México.

[29] Sandoval J., Kelly R. and Ortega R., Interconnection and damping assignment passi-vity based control of the pendubot, 17th IFAC, Seoul, Korea, pp. 7700-7704, 2008.

[30] Sandoval J., Kelly R. and Santibáñez V., Regulation of mechanisms with friction dri-ven by brushed DC motors via IDA-PBC method, 49th IEEE Conference on Decisionand Control, Atlanta, USA, pp. 6225-6229, 2010.

[31] Sandoval J., Kelly R. and Santibáñez V., Interconnection and damping assignmentpassivity based control of a class of underactuated mechanical systems with dynamicfriction, International Journal of Robust and Nonlinear Control, pp. 738-751,2011.

[32] Spong M.W., Passivity based control of the compass gait biped, IFAC World Congress,Bejing, China, pp. 12-16, 1999.

[33] Spong M. and Bullo F., Controlled symetries and passive walking, IEEE Transactionon Automatic Control, Vol. 50, No. 7, pp. 1025-1032, 2005.

referencias 79

[34] Torres J. L., Diseño e implementación en FPGA de un sistema de control para bi-pedestador móvil, Tesis de Maestría, Tecnológico Nacional de México-InstitutoTecnológico de La Paz, La Paz, B.C. Sur, México, 2015.

colofón

Este documento fue realizado usando la tipografía look-and-feel classicthesisdesarrollado por André Miede e Ivo Pletikosic. El estilo está inspirado en el libro deRobert Bringhurst’s "The Elements of Typographic Style". El archivo classicthesis estádisponible para LATEX y LYXen:

https://bitbucket.org/amiede/classicthesis/

https://bitbucket.org/amiede/classicthesis/

Modelado y control de un robot bípedo basado en …posgrado.lapaz.tecnm.mx/uploads/archivos/...En...

Documents

Transcript of Modelado y control de un robot bípedo basado en …posgrado.lapaz.tecnm.mx/uploads/archivos/...En...