Escuela

Politécnica

Superior

Planificación yseguimiento detrayectorias aplicado almovimiento de robotsbípedos

Grado en Ingeniería Robótica

Trabajo Fin de Grado

Autor:Ramón Calvo GonzálezTutor:Jorge Pomares Baeza

Junio 2021

Planificación y seguimiento detrayectorias aplicado al movimiento de

robots bípedos

Generación y reproducción de movimientos para el robot Talos dePAL Robotics

AutorRamón Calvo González

TutorJorge Pomares Baeza

Departamento de Física, Ingeniería de Sistemas y Teoría de la Señal

Grado en Ingeniería Robótica

Escuela

Politécnica

Superior

ALICANTE, Junio 2021

ResumenEl objetivo de este proyecto es la realización de movimientos en el robot humanoide Talos

de PAL Robotics. Este robot dispone de seis grados de libertad en cada pierna y sus actua-dores soportan control por torque.

Se ha realizado un estudio del estado del arte tanto en la planificación de movimientosde robots con patas como de los controladores utilizados para seguir con fidelidad dichastrayectorias.

Seguidamente, se ha incluido al robot Talos en la aplicación towr, un framework de genera-ción de trayectorias para robots con patas con soporte para ROS. Para obtener las trayectoriasarticulares se ha usado IKFast, una utilidad de OpenRAVE, que obtiene la cinemática inversaanalítica de cada pierna. Además, para el cálculo de la trayectoria del centro de masas se usala biblioteca Pinocchio.

Una vez obtenidas las trayectorias articulares y del centro de masas, junto con la secuenciade contactos de los pies con el suelo, se crea un mensaje de ROS con toda esa información yse manda a un controlador de cuerpo completo implementado desde cero. Dicho controladorse ejecuta a una frecuencia de 1KHz y teniendo como entrada las consignas de cada variableen el instante de tiempo correspondiente, intenta llevar al robot al estado deseado creando yresolviendo un problema de programación cuadrática que busca minimizar los errores en elespacio de la tarea.

Del controlador se obtienen las señales de control en forma de torques a aplicar a cadaactuador. Se realizan todas las pruebas en simulación ya que no se ha tenido acceso al robotreal. Aunque los resultados finales no son tan prometedores como se esperaba en un inicio, síque se consigue un controlador capaz de realizar ciertas tareas.

Agradecimientos

En primer lugar, quiero dedicarle estos agradecimientos a mi tutor, Jorge, por su atencióndesde el primer momento. Desde que en los inicios este proyecto iba a estar orientado alseguimiento de movimientos para un exoesqueleto usando aprendizaje por refuerzo, pasandopor un cambio radical de temática y teniendo como resultado esta tesis, siempre ha estadodisponible para guiarme cuando lo he necesitado, no sin dejarme una amplia libertad pa-ra investigar por mi cuenta. Sin su confianza en mí, este trabajo no hubiera sido posible.Además, agradezco enormemente su involucración, que ha tenido como resultado entre otrascosas una posible publicación futura junto a mi compañero de promoción Adrián. Espero quela relación profesional y de amistad forjada a lo largo de estos meses perdure en el tiempo.

Me gustaría agradecer a mi familia y seres queridos por haber estado junto a mí tanto enlos buenos como los malos momentos. A mi padre, hermana y en especial a mi madre, poracompañarme, aguantarme y darme ánimos durante mis momentos más difíciles y por ayu-darme y preocuparse en la realización de mis metas; y a mi pareja, Elena, por estar presentetodos los días sin falta durante estos cuatro años, ser una fuente constante de inspiración ymotivación para seguir adelante y hacerme crecer como persona. Sin vosotras no sería quienque soy ahora mismo y no habría sido capaz de llegar tan lejos.

No puedo olvidarme de mis compañeros de clase, que no sólo han hecho más llevadera larutina de la carrera sino que se han convertido en verdaderos amigos con los que compartomuy buenos recuerdos. Gracias de corazón por vuestra amistad. Aunque ya nos bajemos denuestro ’tándem’ y nuestros caminos se separen, siempre podréis contar conmigo y haré loque tenga en mano para que no perdamos el contacto, ya que sois lo mejor que me ha podidodar la carrera.

También, quiero agradecer a aquellos gigantes a cuyos hombros me he subido para poderaprender todo lo necesario para la realización de este trabajo. Sin la enorme cantidad deprofesionales, tanto de la industria como de la academia, que hacen públicos sus investigacio-nes y seminarios, este trabajo sencillamente no habría sido posible. En especial, me gustaríaagradecer a Stéphane Caron su ayuda con la gestión de las orientaciones para controladoresbasados en optimización.

Por último, agradecer al Ministerio de Educación y Formación Profesional la subvenciónde este proyecto de investigación a través del programa de Becas de colaboración 2020-2021.

Índice general

1 Introducción 11.1 Motivación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Objetivo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Estado del arte 52.1 Optimización . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.1.1 Optimización convexa . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.1.2 Optimización no lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2 Modelos dinámicos para robots bípedos . . . . . . . . . . . . . . . . . . . . . 112.2.1 Dinámica del sólido rígido . . . . . . . . . . . . . . . . . . . . . . . . . 112.2.2 Dinámica del sólido rígido único . . . . . . . . . . . . . . . . . . . . . 122.2.3 Dinámica del péndulo invertido lineal . . . . . . . . . . . . . . . . . . 13

2.3 Optimización de trayectorias . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.4 Controladores de cuerpo completo . . . . . . . . . . . . . . . . . . . . . . . . 18

2.4.1 Clasificación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.4.2 Controladores de cuerpo completo aplicados a robots bípedos . . . . . 23

3 Metodología 273.1 Planificación de trayectorias . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.1.1 Parametrización . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 273.1.2 Restricciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.1.3 Costes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.1.4 Formulación matemática . . . . . . . . . . . . . . . . . . . . . . . . . . 31

3.2 Controlador de cuerpo completo . . . . . . . . . . . . . . . . . . . . . . . . . 323.2.1 Tareas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 333.2.2 Restricciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.2.3 Problema resultante . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

3.3 Simulación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4 Desarrollo 414.1 Dependencias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 414.2 Planificación de trayectorias . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434.3 Implementación del controlador . . . . . . . . . . . . . . . . . . . . . . . . . . 46

4.3.1 Implementación y uso . . . . . . . . . . . . . . . . . . . . . . . . . . . 464.3.2 Integración con ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

5 Resultados 555.1 Resultados de la planificación . . . . . . . . . . . . . . . . . . . . . . . . . . . 555.2 Comportamiento del controlador . . . . . . . . . . . . . . . . . . . . . . . . . 60

ix

x Índice general

6 Conclusiones 65

Bibliografía 67

Índice de figuras

2.1 Se puede considerar la programación lineal como la búsqueda en la frontera delas restricciones de la variable x que minimiza la función objetivo. En el casode que x ∈ R3, las restricciones pueden formar un poliedro como se muestraen la figura y entonces el problema consiste en encontrar el punto sobre lasuperficie de dicho poliedro que minimice el objetivo. . . . . . . . . . . . . . . 7

2.2 Si a-b-c es el camino óptimo de a a c, entonces b-c es el camino óptimo de b a c. 152.3 A la izquierda, se visualiza el coste en cada punto del espacio de estados. A la

derecha, se muestra la acción de control (torque a aplicar) en cada estado. . . 16

3.1 Los prismas rectangulares correspondientes a cada pie se pueden ver abajo.Sus posiciones relativas con la base del robot (visualizada como el prismarectangular negro en el abdomen del robot) son constantes. Las posicionescartesianas de los pies, que están marcadas por las esferas azules, siempretienen que estar dentro de su prisma en todo momento durante la totalidad dela trayectoria. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.2 Visualización de un terreno con una rampa. . . . . . . . . . . . . . . . . . . . 30

4.1 Visualización de los vértices que conforman las superficies de contacto de lospies del robot Talos. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

4.2 Esquema de funcionamiento de ros_control (Fuente: http://wiki.ros.org/ros_control). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.3 Interfaz para modificar todos los parámetros del controlador en tiempo real. . 54

5.1 Trayectorias generadas para los casos 1 (a), 2 (b) y 3 (c) de la Tabla 5.1. . . . 565.2 Visualización de varias magnitudes de la trayectoria 1 de la Tabla 5.1. De

izquierda a derecha y de arriba a abajo: trayectoria cartesiana de la base delrobot sobre el plano XY, evolución en el tiempo de las velocidades de la base,trayectoria cartesiana de los pies del robot sobre el plano XY, posición en eleje Z de los efectores finales con respecto al tiempo, posiciones X e Y del pieizquierdo con respecto el tiempo, evolución de las fuerzas de contacto sobreel pie izquierdo, posiciones X e Y del pie derecho con respecto el tiempo yevolución de las fuerzas de contacto sobre el pie derecho. . . . . . . . . . . . . 57

xi

xii Índice de figuras

5.3 Visualización de varias magnitudes de la trayectoria 2 de la Tabla 5.1. Deizquierda a derecha y de arriba a abajo: trayectoria cartesiana de la base delrobot sobre el plano XY, evolución en el tiempo de las velocidades de la base,trayectoria cartesiana de los pies del robot sobre el plano XY, posición en eleje Z de los efectores finales con respecto al tiempo, posiciones X e Y del pieizquierdo con respecto el tiempo, evolución de las fuerzas de contacto sobreel pie izquierdo, posiciones X e Y del pie derecho con respecto el tiempo yevolución de las fuerzas de contacto sobre el pie derecho. . . . . . . . . . . . . 58

5.4 Visualización de varias magnitudes de la trayectoria 3 de la Tabla 5.1. Deizquierda a derecha y de arriba a abajo: trayectoria cartesiana de la base delrobot sobre el plano XY, evolución en el tiempo de las velocidades de la base,trayectoria cartesiana de los pies del robot sobre el plano XY, posición en eleje Z de los efectores finales con respecto al tiempo, posiciones X e Y del pieizquierdo con respecto el tiempo, evolución de las fuerzas de contacto sobreel pie izquierdo, posiciones X e Y del pie derecho con respecto el tiempo yevolución de las fuerzas de contacto sobre el pie derecho. . . . . . . . . . . . . 59

5.5 Trayectoria larga de ocho pasos. . . . . . . . . . . . . . . . . . . . . . . . . . . 605.6 Controlador llevando al robot a la posición inicial de las trayectorias. . . . . . 615.7 Movimiento del componente X del centro de masas tras la aplicación de la

perturbación. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 615.8 Visualización de la consigna del eje Z del centro de masas (rojo) y su valor real

(azul). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 625.9 Torques generados por el controlador de cuerpo completo para seguir una os-

cilación en el eje vertical del centro de masas del robot. . . . . . . . . . . . . 635.10 Respuesta del controlador con respecto a la tarea de orientación. En rojo, la

consigna de orientación de la base sobre el eje X, y en azul, la orientación realde la base del robot. El alto error en posición se debe a que las tareas de lapostura y de centro de masas están activas y tienen mayor prioridad. . . . . . 63

5.11 Torques generados por el controlador de cuerpo completo para seguir una os-cilación de la orientación de la base sobre el eje X. . . . . . . . . . . . . . . . 64

5.12 Controlador intentando seguir una trayectoria con dos pasos hacia el frente. . 64

Índice de tablas5.1 Tiempo y evaluaciones de la función objetivo requeridos por cada instancia del

problema de planificación. Las columna ’Dirección’ indica de manera intuitivala orientación del paso a dar por el robot (N y S significan eje X positivo ynegativo respectivamente, mientras que O-E significa eje Y positivo y negativo). 55

xiii

Índice de Códigos4.1 Cálculo de la cinemática directa con Pinocchio. . . . . . . . . . . . . . . . . . 414.2 Cálculo de la velocidad de un marco de referencia del robot 1. . . . . . . . . . 424.3 Definición del modelo cinemático y dinámico respectivamente en towr. . . . . 434.4 Cálculo de la matriz de inercia para la posición nominal del robot. . . . . . . 444.5 Activación de las tareas del seguimiento de la postura y centro de masas. . . 474.6 Inicialización de los pesos y constantes de la dinámica de las tareas. . . . . . 474.7 Método sobrecargado para la inicialización de la dinámica de la tarea a la vez

quese activa. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 474.8 Se indican las familias de contactos del robot a usar. . . . . . . . . . . . . . . 484.9 Se activan todas las restricciones implementadas. . . . . . . . . . . . . . . . . 494.10 Obtener la solución al problema de control. . . . . . . . . . . . . . . . . . . . 494.11 Lanza la simulación y carga el controlador . . . . . . . . . . . . . . . . . . . . 54

xv

1 Introducción

1.1 Motivación

En la década de los 60 surgen los primeros robots con aplicaciones industriales. Princi-palmente, sus áreas de aplicación fueron el control numérico aplicado a la producción y lateleoperación de brazos robóticos para la manipulación de materiales radioactivos. En el pri-mero de los casos, los movimientos se predefinían y el robot los seguía ciegamente, mientrasque en el caso de la teleoperación, se precisaba de un operario humano para cerrar el bucle decontrol. Éstos dos métodos tienen en común la falta de información sobre el mundo exteriorrecibida por el robot. Esto se debió principalmente a las limitaciones de la tecnología de laépoca.

El desarrollo de la electrónica durante la segunda mitad del siglo XX trajo consigo eldesarrollo de los circuitos integrados, la miniaturización y los ordenadores digitales. Todoesto permitió paulatinamente que los robots adquirieran más capacidades sensoriales y portanto más autonomía. El resultado fue el uso en escala de robots industriales a finales dela década de los 70 en nuevos sectores, como la industria del metal, química, electrónica oalimentaria. Sin embargo, la sensorización y autonomía seguía siendo limitada, por lo quela operación de los robots estaba limitada a entornos estructurados y adaptados para ellosmismos.

A partir de la década de los 90, los esfuerzos invertidos en la investigación de la robóticamóvil crecen enormemente. El objetivo es la creación de robots que sean capaces de trabajarfuera de las fábricas, en entornos no estructurados y/o en presencia de humanos. Posiblesaplicaciones de robots que cumplan con estas características son robots de inspección o rescateen zonas catastróficas o robots de servicio, los cuales conviven en los mismos entornos de loshumanos y los asisten de varias maneras: entretenimiento, salud, asistencia, limpieza…

El siguiente salto de la robótica se espera que se haga hacia la automatización de procesosque requieran más cognición, destreza y generalidad. Se pretende crear robots que sean ca-paces de desenvolverse en el entorno cotidiano de las personas, el cual está diseñado para elconfort de las mismas: las puertas tienen el tamaño ideal para que las personas puedan pasara través de ellas, las mesas son de la altura adecuada para poder operar sobre ellas y percibirel área de trabajo, las escaleras son un método eficiente de desplazarse verticalmente, losarmarios y estanterías están dispuestos de manera que sea fácil interaccionar con los objetosque guardan, los vehículos disponen sus controles según la disposición de las extremidadeshumanas…Además, si tuvieran una dexteridad similar a la humana, los robots podrían hacer uso de lasmismas herramientas que las personas, incrementando enormemente su utilidad.

Por tanto, es lógico pensar que si se quiere crear un automatismo capaz de tener un nivelde utilidad y autonomía similar al de una persona, se requiera diseñarlo usando como inspira-ción la forma humana. Los robots con patas son capaces de avanzar por terrenos que no son

1

2 Introducción

accesibles por los robots tradicionales con ruedas, incrementando su versatilidad. En parti-cular, los robots bípedos son en teoría capaces de realizar las mismas tareas que las personase incluso colaborar con ellas en el mismo entorno. Es por ello que hay que desarrollar unasmedidas de seguridad adecuadas, ya que pueden surgir un gran número de interacciones noprevistas y que tienen que ser gestionadas de forma correcta.

Por otro lado, los robots humanoides son también interesantes en el área de interaccióncognitiva humano-robot. Mediante el uso de los mismos canales de comunicación que loshumanos (voz, gestos y lenguaje corporal) se puede llegar a obtener un mejor intercambio deinformación y mayor empatía por parte de las personas.

Los primeros pasos para que la robótica bípeda sea una realidad es la creación tanto deplataformas hardware inspiradas en el aparato locomotor humano como de controladores quepuedan generar y seguir movimientos de forma robusta y que cumplan ciertas tareas.

Se ha visto un gran auge en los últimos 20 años de plataformas robóticas bípedas. De entrelos robots existentes, destacan los siguientes:

• ASIMO (Sakagami y cols. (2002)): resultado de la investigación iniciada por Hondaen 1986, ASIMO es un robot desarrollado por Honda con dos objetivos a largo plazoen mente: la creación de un robot asistente para ayudar a las personas en su día adía así como avanzar la robótica de tal manera que no sea necesario enviar personas azonas peligrosas o inaccesibles. Es por ello que además de tener controladores avanzadosque le permiten andar, subir escaleras y correr a 9Km/h, ASIMO posee la capacidadde percibir su entorno mediante una gran variedad de sensores y comunicarse con laspersonas mediante lenguaje verbal. Para probar su funcionamiento, Honda realizó unademostración donde ASIMO se encargaba de hacer una visita guiada por un museo.El robot era capaz de reaccionar a los movimientos de las personas y de guardar lasconversaciones con cada visitante de manera que no las repitiese.

• Atlas-Unplugged (Nelson y cols. (2019)): fue el primer robot bípedo de Boston Dynamicsque podía moverse de manera autónoma y sin necesidad de una fuente de alimentaciónexterior. Surge a partir de la investigación realizada con el robot PETMAN. Su finali-dad era participar en la final de la competición organizada por Darpa en 2015 conocidapor sus siglas en inglés como DRC (Darpa Robotics Challenge). Las tareas a realizarconsistían en conducir un vehículo y bajarse de él, abrir una puerta, girar una válvula,cortar un agujero en una pared con una herramienta, caminar sobre un terreno compli-cado, subir unas escaleras y una tarea final sorpresa. Tres de los primeros siete equiposen la final usaron Atlas-Unplugged.

• Talos (Stasse y cols. (2017)): es un robot creado por la empresa española PAL Robotics.Surge a partir de las lecciones aprendidas en la DRC, con un enfoque a la realización detareas industriales en entornos compartidos con humanos. Sus brazos, que tienen sietegrados de libertad, pueden soportar pesos de 6Kg completamente estirados. Ademáshan sido diseñados con una cinemática que favorece la realización de tareas de ator-nillar y perforar. Sus piernas tienen seis grados de libertad cada una, y en todas susarticulaciones se permite el control por torque.

En lo que se refiere al control de robots bípedos, es un área muy investigada en la actuali-dad y en la que todavía se tiene mucho por descubrir. Uno de los métodos más comunes para

1.2. Objetivo 3

generar movimientos en robots bípedos es la planificación a priori de las posiciones de loscontactos de los robots, después, conociendo estos puntos de apoyo, se busca una trayectoriaque cumpla con la dinámica del robot y que pase por todos los puntos de contacto especifica-dos. Finalmente, se ejecuta la trayectoria en el robot real o en simulación, donde se tiene uncontrolador que funciona en tiempo real para compensar las perturbaciones y las diferenciasentre el modelo dinámico usado en la planificación y el real.

1.2 ObjetivoEl objetivo del presente trabajo es la realización de movimientos de andar sencillos para el

robot Talos mediante estrategias de control basadas en optimización. Se va a dividir el trabajoen dos partes: la generación de trayectorias para Talos y la creación de un controlador decuerpo completo que siga dichas trayectorias con éxito.

Para la planificación de las trayectorias, se va a partir del trabajo desarrollado por Winklery cols. (2018) donde se introduce por primera vez una formulación que permite la optimizaciónsimultánea tanto de la trayectoria como de los puntos de contacto para robots con patas. Laaportación consiste en la introducción de Talos dentro del optimizador, haciendo los cambiosnecesarios para la generación de trayectorias físicamente realizables. La metodología a seguirse detalla en el apartado 3.1, mientras que la implementación se explica en el apartado 4.2.

En cuanto al seguimiento de las trayectorias, se va a crear desde cero un controlador decuerpo completo basándose en los trabajos de Bouyarmane y cols. (2019); Salini y cols. (2013);Del Prete y cols. (2014). Se formula el control como un problema de optimización cuadráticodonde como solución se obtienen los torques a aplicar en las piernas del robot. No se tienen encuenta los brazos, que permanecen fijos en su posición nominal. El controlador será probadoen el entorno de simulación Gazebo, ya que se hará uso del entorno de desarrollo para el robotTalos proporcionado por PAL Robotics basado en ROS. El desarrollo e implementación sepuede encontrar en los apartados 3.2 y 4.3, respectivamente.

2 Estado del arte

2.1 OptimizaciónEn el presente trabajo se hace un uso extensivo de la optimización matemática para la

resolución de los problemas de control a afrontar. Es por ello que se dedica esta sección aexplicar los fundamentos de esta poderosa herramienta y su estado en la actualidad.

A grandes rasgos, un problema de optimización matemática tiene la forma (Boyd y Van-denberghe (2004)):

minimiza f0(x)tal que fi(x) ≤ bi, i = 1, . . . ,m

(2.1)

Donde x = (x1, . . . , xn) es la variable de optimización del problema, f0 : Rn → R esla función objetivo, las funciones fi : Rn → R son las restricciones de desigualdad y lasconstantes bi son las fronteras o límites de las restricciones.

Un vector x∗ es llamado óptimo (o una solución) si tiene el menor valor objetivo que todaslas otras variables que cumplen la condición de estar dentro de las restricciones del problema.

El problema expresado en 2.1 es una abstracción de cualquier problema de optimizaciónque se pueda encontrar en la práctica. En estos problemas se busca encontrar la mejor opciónposible, siendo esta un vector en Rn, dadas unas restricciones conocidas.

Por ejemplo, el problema de la optimización de la cartera es un famoso problema de op-timización matemática, como su propio nombre indica. En este problema, una cartera serefiere a la distribución de una serie de activos. Se busca por tanto encontrar la distribuciónde activos que maximice el rendimiento de la cartera, es decir, encontrar cuánto dinero setiene que invertir en cada componente de la cartera para que, conocidas las rentabilidades,los riesgos y el capital total a invertir, se maximice la rentabilidad todo lo posible. En esteproblema, un ejemplo de restricción sería claramente el capital del que se dispone para hacerlas inversiones, y un ejemplo de función objetivo podría ser el riesgo total de la inversión, quese busca minimizar.

En la actualidad, y gracias a la llegada de tanto ordenadores como sistemas empotradosde altas prestaciones, se ha extendido el uso de la optimización matemática a un sin finde campos con innumerables aplicaciones. Desde campos de la ingeniería como pueden sersistemas de control, diseño electrónico, ingeniería química o aeroespacial hasta otros sectorescompletamente distintos como pueden ser las finanzas o la gestión de cadenas de suministros.

Debido a la gran cantidad de funciones objetivo que se pueden tener, es necesario distinguirla clase de los distintos problemas de optimización ya que ninguno se trata de la mismamanera. Cada clase de problema se trata con un método de solución. Por lo que si se consigueformular un problema como un problema de optimización de una clase en concreto, seráposible aplicar un optimizador creado para resolver problemas genéricos de esa clase y asíobtener una solución. Gracias a la existencia de estos optimizadores, es posible centrar los

5

6 Estado del arte

esfuerzos en encontrar una formulación del problema y usar un optimizador genérico yaexistente y verificado extensivamente.

La efectividad de estos optimizadores depende de muchos factores. Se puede considerar queel más importante de estos factores es la clase de problema de optimización que soluciona.La clase viene dada por el tipo de función objetivo f0(x) y de funciones frontera fi(x). Perotambién afectan al rendimiento el número de variables y restricciones o el grado de dispersióndel problema (un problema es disperso si fi(x) sólo depende de un número pequeño devariables de x).

Incluso cuando el problema de optimización 2.1 sólo contiene funciones continuas y suaves(por ejemplo, polinomios), es de una gran dificultad encontrar el vector x que minimizael objetivo. Aproximaciones a resolver el problema general son expuestas en el apartado2.1.2. Normalmente, el precio a pagar por trabajar con funciones genéricas es un alto costecomputacional o la posibilidad de no encontrar la solución, sin poder saber a priori si se vaa poder encontrar o no.

Es por ello que desde la década de los 40 se llevan desarrollando optimizadores para cadaproblema de optimización en específico, ya que no es tarea sencilla analizar las peculiaridadesde cada clase de problema de optimización y desarrollar implementaciones de calidad.

A continuación se exponen las clases más comunes de problemas de optimización convexos.

2.1.1 Optimización convexaEn un problema de optimización convexa, las funciones f0(x), . . . , fm(x) cumplen la si-

guiente propiedad:

fi(αx+ βy) ≤ αfi(x) + βfi(y), ∀x, y ∈ Rn, ∀α, β ∈ R (2.2)

∀x, y ∈ Rn y α, β ∈ R/α + β = 1, α ≥ 0, β ≥ 0. Rápidamente se puede observar que laprogramación lineal es un caso particular del problema de optimización convexa. Teniendoen cuenta la definición 2.2, se pueden definir distintas familias de problemas de optimizaciónconvexa, dependiendo de las formas que adoptan f0, . . . , fm(x).

1. Mínimos cuadradosConsiste en minimizar la siguiente expresión donde no se tiene ningún tipo de restric-ción:

minx

f0(x) = ∥Ax− b∥2 (2.3)

Donde A ∈ Rk×n (siendo k ≥ n) es una matriz de rango k y x ∈ Rn. Se puede encontrarla solución analítica a este problema sabiendo que al ser una función cuadrática (ATAes una matriz definida positiva) existe un único mínimo y es global. Por lo tanto:

∇x(Ax− b)2 = 2AT (Ax− b) = 0 (2.4)ATAx−AT b = 0 (2.5)

x = (ATA)−1AT b (2.6)

2.1. Optimización 7

Cabe destacar la presencia de la inversión de la matriz ATA. Para problemas con cientoso miles de variables, la solución analítica (expresión 2.6) no presenta ningún problemapara los procesadores modernos. Sin embargo, en cuanto el número de variables crece alorden de cientos de miles o superior, las soluciones iterativas como puede ser el descensopor gradiente son más eficientes.

2. Programación lineal.

En los programas lineales, tanto f0(x) como fi(x) son funciones lineales, es decir, quese cumple que:

fi(αx+ βy) = αfi(x) + βfi(y), ∀x, y ∈ Rn,∀α, β ∈ R (2.7)

Como consecuencia, el problema 2.1 se puede reescribir de la siguiente manera en estecaso en particular:

minimiza cTxtal que Ax ≤ b, i = 1, . . . ,m

(2.8)

Donde c ∈ Rn, A = [a1, . . . , am]T y b ∈ Rm. Se puede visualizar un programa linealcomo

Figura 2.1: Se puede considerar la programación lineal como la búsqueda en la frontera de las res-tricciones de la variable x que minimiza la función objetivo. En el caso de que x ∈ R3, lasrestricciones pueden formar un poliedro como se muestra en la figura y entonces el pro-blema consiste en encontrar el punto sobre la superficie de dicho poliedro que minimiceel objetivo.

Aunque no existen soluciones analíticas para este problema, si que hay algoritmos pararesolverlo de manera eficiente. Los optimizadores de esta clase de problemas son robustos

8 Estado del arte

y relativamente rápidos. La complejidad de los algoritmos para esta clase de problemases del orden de O(n2m), independientemente del método a usar.

3. Programación cuadráticaEn los programas cuadráticos, la función objetivo f0(x) es cuadrática al igual que enel caso de mínimos cuadrados, pero por otra parte, se tiene una serie de restriccionesf1(x), . . . , fm(x) que acotan el dominio de las posibles soluciones con una forma convexa.Estas funciones pueden ser lineales o cuadráticas. No se entra en detalle de este últimocaso ya que no se aplica en la totalidad del presente trabajo. En su lugar se va a tratarde dar una introducción al problema acotado por restricciones lineales, que es utilizadoampliamente en los apartados 2.4, 3.2 y 4.3.Considérese el siguiente problema de optimización:

minimiza 12x

TPx+ qTxtal que l ≤ Ax ≤ u

(2.9)

donde x ∈ Rn es la variable de decisión, la función objetivo viene dada por la matrizsemidefinida positiva P ∈ Rn×n y las restricciones por la matriz A ∈ Rm×n y losvectores l = (l0, . . . , lm), li ∈ {−∞} ∪ R y u = (u0, . . . , um), ui ∈ R ∪ {+∞}.Este problema de optimización tiene una gran variedad de aplicaciones prácticas. Porcitar algún ejemplo, en inteligencia artificial, es usado entre otras cosas para resolvermáquinas de vectores de soporte (SVM por sus siglas en inglés) mientras que en elcampo de ingeniería de control, es usado en control predictivo por modelo (Carlo y cols.(2018)). Además, los programas cuadráticos se pueden utilizar como subproblemas enalgoritmos de optimización más complejos como por ejemplo la programación cuadráticasecuencial (SQP por sus siglas en inglés), usada para resolver problemas de optimizaciónno lineales.Al contrario que en mínimos cuadrados, no existe una solución analítica para los pro-gramas cuadráticos. Existen varios métodos que se llevan desarrollando desde la décadade los 50 partiendo del trabajo realizado para resolver programas lineales. La mayoríase pueden categorizar en tres clases, dependiendo de la forma de plantear el problema:

• Métodos del conjunto activo: fueron obtenidos a partir de un método de reso-lución de programas lineales y los primeros algoritmos en ser ampliamente usadospara resolver programas cuadráticos. Su funcionamiento consiste en seleccionar unconjunto activo (es decir, un conjunto de fronteras de las restricciones) y de mane-ra iterativa ir añadiendo o quitando fronteras al conjunto. Las decisiones de añadiro quitar se basan en la información del gradiente del objetivo. Estos algoritmossoportan lo que se llama arranque en caliente, que consiste en utilizar la soluciónanterior como conocimiento a priori con la esperanza de que el nuevo problema nohaya cambiado mucho con respecto al problema anterior y así encontrar la solu-ción más rápido. Como desventaja, estos métodos tienen un coste computacionalexponencial con respecto al número de restricciones en el peor de los casos, yaque se tienen que probar todas las combinaciones. Se pueden encontrar implemen-taciones modernas basadas en este método en varios optimizadores comerciales

2.1. Optimización 9

como GUROBI (Inc (2016)) y de código abierto como qpOASES (Ferreau y cols.(2014)).

• Métodos de los puntos interiores: los algoritmos de puntos interiores gozaronde una amplia popularidad para resolver programas lineales en la década de los 80.Futuras investigaciones lograron generalizar este método para resolver programascuadráticos. La principal idea de estos métodos es modelar las restricciones delproblema mediante funciones parametrizadas de penalización. Durante la ejecucióndel algoritmo, estas funciones de penalización, también conocidas como funcionesbarrera, hacen que si se evalúa un punto p ∈ Rn que no pertenece al conjunto sepenalice y el optimizador sea recompensado por hacer que las futuras iteracionesvayan en la dirección del conjunto convexo de restricciones. Para conseguir esto,se resuelve un problema de optimización sin restricciones, donde las restriccionesquedan codificadas en el objetivo a través de las funciones barrera, las cuales se vanhaciendo más restrictivas. Una de las funciones barrera más usada es el logaritmonegativo: g(x, b) = − log(b−x), donde b es el parámetro que se va variando en cadaiteración del algoritmo para penalizar la exploración fuera del conjunto convexode restricciones. En la actualidad un optimizador comercial muy conocido queutiliza este método es MOSEK (ApS (2019)). También existen implementacionesde código abierto como OOQP (Gertz y Wright (2003)).

• Métodos de primer orden: este método intenta encontrar de manera iterativala solución al problema usando solo la información de primer orden del objetivo.Existen varias formas de afrontar el problema de esta manera, pero recientementeestá teniendo tracción un método cuyas ideas vienen de investigaciones de los años50. En la actualidad, se usa mucho el método de los multiplicadores en sentidoalterno (ADMM por sus siglas en inglés). ADMM tiene como ventaja que es unmétodo muy rápido en la resolución de problemas, a costa de encontrar solucio-nes que tienen una precisión modesta. Esto lo hace un gran candidato a usar ensistemas empotrados que trabajan con datos ruidosos. Además, es un algoritmo al-tamente paralelizable, por lo que es posible usarlo en arquitecturas de optimizacióndistribuidas para resolver problemas de gran escala.Entre las desventajas de ADMM, están la imposibilidad de predecir la inviabilidadde un problema y que el número de iteraciones es muy dependiente de los datosdel problema y de la elección del usuario de los parámetros del algoritmo.Uno de los optimizadores más avanzados de esta clase resulta ser el que se utilizacon posterioridad en el presente trabajo (apartado 4.3). OSQP (Stellato y cols.(2020)) es un optimizador de código abierto y creado por la colaboración de inves-tigadores de numerosas universidades. Este optimizador está al frente de los de suclase ya que no requiere convexidad estricta en su objetivo ni independencia linealentre las restricciones. Además, es capaz de resolver el problema tras una única re-factorización que reformula el problema para que no hagan falta hacer divisiones.Por otra parte, para obtener soluciones con gran precisión, se realiza un pulidode la misma. Para ello, se trabaja sobre el conjunto de restricciones activas y seconstruye un programa cuadrático cuyas restricciones únicamente son de igualdady cuya solución coincide con la solución del problema original. En caso de que se

10 Estado del arte

identifiquen correctamente las restricciones activas, se pueden obtener solucionesde precisión similar a las obtenidas con métodos de puntos interiores. Por último,el optimizador soporta la opción de arranque en caliente, que como se ha explicadoanteriormente, favorece enormemente a la eficiencia en caso de que los problemassecuenciales sean parecidos. En algunos casos este optimizador es hasta un ordende magnitud más rápido que otros optimizadores de su clase, sin hacer uso delarranque en caliente.

4. Programación cónica de segundo ordenLa programación cónica de segundo orden tiene la siguiente forma:

minimiza fTxtal que ∥Aix+ bi∥2 ≤ cTi x+ di, i = 1, . . . ,m

Fx = g(2.10)

donde x ∈ Rn, Ai ∈ Rni×n y F ∈ Rp×n. La restricción ∥Ax+ b∥2 ≤ cTx+d se le conocecomo restricción cónica de segundo orden.Estos problemas se pueden resolver con el método de los puntos interiores. Una imple-mentación moderna y de código abierto se puede encontrar en la ECOS (Domahidi ycols. (2013)), un optimizador creado para usar principalmente en procesadores empo-trados.Este tipo de optimización es usado para el diseño de un controlador del balance de unrobot humanoide (Park y cols. (2007)). Además, se demuestra que otros controladoresbasados en optimización creados previamente son instancias de casos especiales delcontrolador propuesto, por lo que es una formulación más general.

2.1.2 Optimización no linealEn la Programación No Lineal (PNL) se trabaja con funciones objetivo o restricciones que

no son lineales y convexas. Debido a la complejidad que esto conlleva, ya que se puede haceruso de cualquier tipo de función, no existen métodos efectivos para resolver estos problemas.Es muy probable que instancias de problemas con muy pocas variables y que parezcan sencillospuedan tardarse mucho tiempo en resolver o incluso sean irresolubles. A la hora de tratarcon estos problemas, en general hay dos formas de afrontarlos: de manera global y de maneralocal.

En la optimización global se busca encontrar la solución real del problema a costa de unalto coste computacional. La complejidad del problema crece exponencialmente tanto conrespecto al número de variables (n) como con el número de restricciones (m). Debido a laslimitaciones evidentes de este método, sólo se usa en casos en los que se pretenda encontrarla solución de un problema con un número muy pequeño de variables (del orden de 10) y sepueda esperar horas o incluso días en encontrar la solución global. Un ejemplo puede ser laverificación de un sistema crítico en un proyecto de ingeniería. Como variables se pueden tenerciertos parámetros de diseño o características del entorno de operación. Se busca entoncesencontrar el peor de los casos, si el peor de los casos resulta en una situación aceptable, sepuede certificar que el sistema es seguro.

2.2. Modelos dinámicos para robots bípedos 11

Por otro lado, los métodos basados en optimización local abandonan por completo la ideade encontrar la solución global. En su lugar, se busca encontrar una solución que minimicelocalmente el objetivo. Como resultado, se puede dar el caso de que el optimizador nuncallegue a explorar zonas donde el objetivo sea menor que la solución local encontrada. Parafuncionar, estos métodos requieren que las funciones objetivo y de restricciones sean dife-renciables, además de un punto inicial donde comenzar la búsqueda de una solución. Estainicialización es de gran importancia para los resultados a obtener, ya que determina de ma-nera directa la zona a explorar por el optimizador. Para resolver un problema práctico conun optimizador local, es necesario seleccionar un algoritmo que de buenos resultados en elproblema a tratar ya que no todos se comportan igual, los parámetros del algoritmo escogidoy el punto inicial o método de elección de puntos iniciales. No hay un consenso de cómo setienen que seleccionar todas estas variables para obtener buenos resultados, aunque se puedellegar a una buena aproximación mediante el empirismo y la experiencia. Razón por la cualse considera a la optimización local más un arte que una ciencia.

Algunos ejemplos de optimizadores locales ampliamente utilizados en la industria sonSNOPT (Gill y cols. (2002)), de licencia comercial, e IPOPT (Wächter y Biegler (2006)),un optimizador de código libre. SNOPT se basa en el concepto de la programación cuadrá-tica secuencial, en el cual se formula un programa cuadrático en cada iteración, el cual seresuelve para avanzar al siguiente paso. Se puede pensar que es un algoritmo con muchassimilitudes al método de Newton. Por otro lado, IPOPT hace uso del método de puntosinteriores, ya comentado en el apartado 2.1.1.

2.2 Modelos dinámicos para robots bípedos

Es posible representar los movimientos de los robots con patas con varios modelos dinámicosdependiendo del rango de movimientos que estos vayan a hacer. Se dice que una planificaciónde un movimiento es físicamente correcta con respecto a un modelo dinámico si en todomomento se cumplen las ecuaciones de dicho modelo. En general, los modelos dinámicosrelacionan las entradas de control u y el estado actual del robot x, con la variación de suestado x mediante una Ecuación Diferencial Ordinaria (EDO):

x = F (x(t), u(t)) (2.11)

Las distintas representaciones dinámicas varían las dimensiones de x, la cantidad que repre-senta el control u y cómo afecta tanto x y u a la evolución del sistema. Sólo existe un modeloque represente de manera fiel las físicas del mundo real y es aquél en el que no se realizaninguna suposición. Trabajar con tanto nivel de detalle es casi imposible y no es necesario yaque no se necesita tanta precisión para la locomoción de robots, por lo que se suele trabajarcon modelos con distintos grados de simplificación dependiendo de la tarea a realizar.

2.2.1 Dinámica del sólido rígido

En este modelo se realiza la siguiente suposición:

• Los cuerpos del sistema no se deforman al aplicarles una fuerza externa.

12 Estado del arte

Al pensar que el sistema dinámico consta de varios eslabones conectados entre sí porarticulaciones, se puede derivar la relación entre el torque de dichas articulaciones u y elestado del sistema.

M(q)q +N(q, q) = STu+ JT f (2.12)

donde:

• M(q) es la matriz de inercia en el espacio articular.

• N(q, q) es la matriz que recoge los términos no lineales. En este caso, engloba los efectosde Coriolis, centrífugos y gravitatorios.

• ST = [0n×6 In×n] es la matriz de selección que filtra los grados de libertad que no sonactuables (en el caso de un robot humanoide, estos grados de libertad no actuados secorresponden con la posición y orientación de la base del robot con respecto al sistemade coordenadas global). n es el número de articulaciones del robot donde se aplican lostorques τ .

• J es la matriz que contiene la concatenación de todos los jacobianos de los contactos.Es decir, si (C1, . . . , CK) define la secuencia de eslabones que están en contacto con elentorno en el mismo orden en el que se define la concatenación de las fuerzas de contactofT (fT1 , . . . , fTK), entonces JT =

[JC1(q)

T . . . JCK(q)T

], donde JCk

(q) es la jacobiana delpunto del contacto. La función de J es convertir las fuerzas externas f a sus respectivasfuerzas y torques para cada articulación del robot. En el caso de que no haya ningúncontacto, este término desaparece.

Se puede separar la dinámica entre los seis grados de libertad no actuados y los actuados:

M(q)q +N(q, q) = JT f (2.13)M(q)q +N(q, q) = u+ JT f (2.14)

La expresión 2.13 se corresponde por tanto con el movimiento de la base del robot y 2.14describe el movimiento de las articulaciones. Nótese que no es posible afectar al movimientode la base con las señales de control de las articulaciones. Es decir, no hay ningún motorque pueda producir un movimiento directo sobre la base. En su lugar, para moverse, el robotnecesita interaccionar con el entorno y mover la base mediante las fuerzas externas f.

Por otro lado, se puede observar que se puede modificar la fuerza externa f directamentecon las aceleraciones articulares q. En caso de que se tenga un control de torque a bajo nivel,es posible seleccionar la q correspondiente para obtener la f necesaria para poder realizarel movimiento deseado de la base, siempre que haya un contacto y los límites físicos de lasarticulaciones lo permitan.

2.2.2 Dinámica del sólido rígido únicoSi los límites en los torques de los actuadores de las piernas son lo suficientemente grandes,

es posible eliminarlos completamente del modelo dinámico. Al hacerlo, desaparecen las n

2.2. Modelos dinámicos para robots bípedos 13

ecuaciones correspondientes a las articulaciones y la necesidad de realizar una planificacióncon respecto a un modelo específico de robot. A la vez, se tiene que mantener este modelo lomás aproximadamente posible con el real. Además de la suposición de la dinámica del sólidorígido (2.2.1), se añaden las siguientes:

• El momento producido por las velocidades articulares es despreciable.

• La inercia del robot completo permanece constante y equivalente a la obtenida dejandoal robot en su estado articular de reposo.

Este modelo por tanto parece razonable con robots donde la mayor parte de su masa seencuentre en la base y las extremidades no supongan un peso relativo alto. Es por tanto muyusado en robots cuadrúpedos como el ANYmal (Hutter y cols. (2016)) o el HyQ (Semini y cols.(2011)), pero también en bípedos que cumplan estas características como Cassie (Robotics(2017)). Un peso pequeño en las extremidades se traduce tanto en pequeños momentos aúncon velocidades grandes y en cambios no muy importantes en la inercia del cuerpo completodel robot. Por eso es de gran interés el uso de este modelo dinámico por las ventajas quese obtienen a la hora de la planificación de trayectorias, ya que únicamente se tiene quetrabajar con las posiciones cartesianas tanto de la base como de los efectores finales de lasextremidades.

Al considerarse al robot como un único sólido rígido al cual se le aplican fuerzas externas,es posible describir su movimiento usando las ecuaciones de Newton-Euler:

mr = mg +K∑i=1

fi (2.15)

I(θ)ω + ω × I(θ)ω =

K∑i=1

fi × (r − pi) (2.16)

donde:

• r se corresponde con la posición del centro de masas.

• θ, ω es la orientación de la base y su velocidad angular, respectivamente.

• m es la masa total del sistema (base y extremidades).

• I(θ) ∈ R3×3 es la inercia del cuerpo completo del robot con respecto al centro de masasteniendo todas sus articulaciones en posición de reposo.

2.2.3 Dinámica del péndulo invertido linealAunque parezca que la simplificación realizada en el apartado 2.2.2 imponga suposiciones

demasiado fuertes, se ha podido ver que se usa en robots reales. Sin embargo, el productovectorial que se introduce en las ecuaciones de Newton-Euler hace que el modelo siga unaexpresión no-lineal. Se puede conseguir un modelo dinámico más restringido pero que puedaser representado mediante una ecuación lineal añadiendo las siguientes restricciones a ladinámica del sólido rígido único:

14 Estado del arte

• La altura del centro de masas (rz) es constante.

• La velocidad y aceleración angular de la base son nulas.

• Los contactos realizados por las extremidades del robot están todos a la misma alturapz.

Aplicando estas suposiciones a las expresiones 2.15 y 2.16, se obtiene el modelo dinámicodel péndulo invertido lineal:

mrx =mg

h(rx − pc,x) (2.17)

mry =mg

h(ry − pc,y) (2.18)

Donde pc,x es la posición del centro de presión de los contactos con el suelo. Queda entoncestotalmente descrita la evolución del centro de masas, que está sobre el plano horizontal alsuelo y a altura h, por el centro de presión. Nótese que las fuerzas de contacto no están enla expresión, lo único que importa es la distancia entre la proyección de las posiciones delcentro de masas y del centro de presión.

Sin embargo, para cambiar la posición del centro de presión se consigue mediante la va-riación en las fuerzas de contacto y por ende en el control aplicado a cada articulación. Elpéndulo invertido lineal provee de una abstracción para poder calcular los torques necesariosa aplicar para conseguir la aceleración deseada del centro de masas. Como desventaja, al in-troducir tantas suposiciones los movimientos realizables por el robot quedan muy limitados,pudiendo caminar únicamente por superficies estructuradas, lo que deshace el propósito dela robótica con patas ya que un robot móvil con ruedas es más eficiente en este caso.

Un célebre ejemplo de robot que usa este modelo es ASIMO (Sakagami y cols. (2002)).Es fácil identificar que este modelo está siendo usado ya que se puede apreciar en cualquierade sus apariciones que la base permanece constantemente a una altura fija, incluso cuandocamina o corre.

2.3 Optimización de trayectoriasEl control óptimo se centra en la formulación de problemas de control que se resuelven

mediante una optimización. Para ello se codifican los comportamientos deseados como fun-ciones de coste que devuelven un escalar, el cual se busca minimizar. De esta manera se puedeafrontar de una manera similar problemas de control de distintos tipos: lineales, no lineales,sistemas subactuados, deterministas o estocásticos y continuos o discretos. Para resolver estosproblemas, se utilizan métodos numéricos, los cual dan muy buenos resultados.

Una posible estrategia para obtener una política óptima es discretizar tanto el espaciode estados como el de acciones, y optimizar con un algoritmo de recursión de manera muysimilar a como se hace en los algoritmos de encontrar el camino mínimo en grafos. Se definela dinámica de un sistema como:

s[n+ 1] = f(s[n], u[n]) (2.19)

2.3. Optimización de trayectorias 15

Donde s[i] es el punto correspondiente en el espacio de estados en el instante i, u[i] es laseñal de control aplicada en el instante i y f es el modelo discretizado de la dinámica, quedevuelve el siguiente estado a partir del estado y acción actuales.

Figura 2.2: Si a-b-c es el camino óptimo de a a c, entonces b-c es el camino óptimo de b a c.

Haciendo uso del principio de optimalidad de Bellman (Figura 2.2), es posible diseñar unalgoritmo de programación dinámica que calcule el coste de ir desde cualquier punto en elespacio de estados con respecto a la consigna deseada. De esta manera, se puede diseñarun controlador que lleve al sistema a su estabilidad a partir de unas condiciones inicialescualquiera. Este coste se define como:

J∗(si)← mina∈A

[l(s, a) + J∗(f(si, a))

](2.20)

El algoritmo que explota esta propiedad para calcular los costes de cada estado se denominaalgoritmo de iteración de valor. Una vez que el algoritmo haya convergido, se tendrá un valorpor cada estado que representa el coste a largo plazo de caer en ese estado. El controlador,por tanto, se limitará a coger la acción que minimice el coste:

π∗(si) = arg mina

[l(si, a) + J∗(f(si, a))] (2.21)

Haciendo uso de esta técnica, es posible encontrar un controlador óptimo para el control deun péndulo invertido (Figura 2.3). El espacio de estados en este caso es de dos dimensiones (qy q) por lo que el algoritmo converge rápidamente. Sin embargo, para sistemas más complejoscon espacios de estados de más dimensiones, el problema puede llegar a ser intratable. Estose debe a la conocida como la maldición de la dimensión. La memoria e iteraciones requeridaspara que el algoritmo de iteración de valor converja crecen exponencialmente con el número dedimensiones del tanto del espacio de estados como el de acciones, por lo que la programacióndinámica no se puede aplicar en problemas más complejos como puede ser el control de unrobot bípedo.

Se podría pensar que una forma de eludir este problema podría ser la exploración delespacio de estados que solamente sea relevante. A fin y al cabo, si considerásemos a laspersonas como un sistema con miles de grados de libertad, en el día a día los movimientosrealizados por el cuerpo se podrían localizar sobre una variedad en el espacio de estados. Unade las aproximaciones a tomar para evitar explorar la totalidad del espacio de estados es laoptimización de trayectorias. Esta técnica consiste en encontrar una secuencia de acciones decontrol para llevar al sistema desde un estado inicial conocido a un objetivo final tambiénconocido.

Es mejor describir la optimización de trayectorias usando de ejemplo el campo para el que

16 Estado del arte

Figura 2.3: A la izquierda, se visualiza el coste en cada punto del espacio de estados. A la derecha,se muestra la acción de control (torque a aplicar) en cada estado.

fue desarrollada inicialmente (Kelly (2017)): las maniobras orbitales de satélites y naves espa-ciales. Partiendo del estado inicial del satélite (posición, velocidad, combustible, …), se quiereencontrar la trayectoria óptima de estados y acciones (control en giroscopios y/o propulsores)con respecto a una función de coste. Los métodos existentes para encontrar dicha trayectoriason conocidos en general con el término ’optimización de trayectorias’.

Dos posibles aproximaciones para solucionar estos problemas son la programación dinámicao los métodos directos:

• Programación dinámica diferencial: propuesta por primera vez por MAYNE (1966),esta familia de métodos consiste en usar aproximaciones locales cuadráticas tanto enlas dinámicas como en las funciones de coste, por lo que está muy relacionado con elmétodo de Newton. Sufre del mismo problema de la maldición de la dimensión, perotrabajos recientes (Mastalli y cols. (2020)) exponen nuevos algoritmos en los que secentra la exploración en los estados relevantes para la optimización y se consigue laconvergencia con muchos menos recursos computacionales, incluso en casos de sistemasaltamente redundantes como los del humanoide Talos.

• Métodos directos: la optimización de una trayectoria continua en el tiempo queda ex-presada como un problema de PNL, descrito por un número finito de variables dedecisión:

minw

a(w) tal que b(w) = 0, c(w) ≥ 0 (2.22)

Donde w son las variables que se quieren encontrar y a es la función objetivo. w tienenque cumplir las condiciones de igualdad impuestas por b(w) = 0 y las de desigual-dad impuestas por c(w) ≥ 0. Tras reformular la optimización de trayectorias como unproblema de este tipo, es posible encontrar una solución (aproximada) usando optimi-zadores no lineales como SNOPT (Gill y cols. (2002)) o IPOPT (Wächter y Biegler(2006)).

2.3. Optimización de trayectorias 17

Existen dos métodos de hacer esta reformulación, que pasa la optimización de trayec-torias discretizada a un problema de optimización no lineal:

– Métodos secuenciales: la señal de control u(t) es parametrizada a partir del con-junto de variables w, mediante un polinomio o un spline, por ejemplo. Esta señales después aplicada a la representación de la dinámica del sistema (ver apartado2.2) y los estados x(t) son obtenidos mediante métodos numéricos para EDOs.El optimizador PNL verifica si los x(t) resultantes cumplen con las restriccionesespecificadas por b(w) y c(w). En caso de no cumplirse, los parámetros que des-criben la trayectoria de las acciones, w, son adaptados a partir de la informaciónde los gradientes. Una vez hecha la actualización, el sistema se vuelve a integrarpara obtener los nuevos estados x(t). Debido a su funcionamiento de ’disparar’ lasdinámicas varias veces hacia adelante, este método se denomina método de tiroteosimple.Una de las ventajas principales de estos métodos es que al hacer uso de un in-tegrador de EDOs, el tamaño del paso entre señales discretas de control puedeser variable, reduciendo así los cálculos innecesarios y por tanto incrementandola eficiencia del algoritmo. Otra ventaja es que el número de variables permanecerelativamente pequeño, ya que sólo se tiene que parametrizar la trayectoria de lasacciones de control u(t).Como desventaja se tiene que la trayectoria sobre x(t) no se puede inicializardirectamente. Además, los sistemas dinámicos inestables pueden ser difíciles demanejar, ya que pequeñas modificaciones en las acciones de control iniciales puedendesencadenar grandes efectos en estados futuros.Si se tienen que encontrar trayectorias prolongadas en el tiempo, es posible queeste método se vuelve inestable y no encuentre una solución. Una forma de solu-cionar este problema es dividir la trayectoria en varios trozos y optimizar cada unode ellos de manera independiente. De esta manera, se tienen varios problemas detiroteo simple que juntados forman la trayectoria solución. Para que esto funcio-ne, es necesario especificar en forma de restricción que los segmentos comiencenen el estado final del segmento anterior, para que la trayectoria global sea cohe-rente. Esta clase de algoritmos se denominan de tiroteo múltiple, ya que se estánresolviendo varios problemas de tiroteo simple de forma conjunta.

– Métodos simultáneos: en este caso, las variables a optimizar parametrizan tantola señal de control u(t) como la evolución del estado x(t). La dinámica del sistemano se impone a través de la integración como en el caso del tiroteo, sino a través delas restricciones de igualdad mediante la función b(w). Las ventajas y desventajasse invierten con respecto a los métodos secuenciales: por un lado se tiene queespecificar con antelación los instantes en el tiempo en los que se haga cumplir ladinámica del sistema, lo que hacer difícil añadir más precisión en los instantes quesea necesaria; por otro lado, es sencillo realizar una inicialización del movimiento, loque puede llevar al sistema a converger más fácilmente. Además, como se desacoplala dinámica de las acciones de control, al estar optimizándose ambas a la vez,desaparece el problema en el que pequeños cambios al inicio de la trayectoria decontrol pueden llevar a estados completamente diferentes, lo que facilita aún más

18 Estado del arte

la convergencia en sistemas inestables.En el trabajo realizado por Winkler y cols. (2017), se utilizan métodos simultáneospara generar trayectorias para robots con patas en general, desde robots con unapata hasta cuadrúpedos, formulando el problema de tal manera que se consigueencontrar una trayectoria óptima en menos de un segundo en la mayoría de loscasos. En el apartado 3.1 se explica en detalle el funcionamiento, ya que a partirde este trabajo se generan las trayectorias para el robot Talos.

2.4 Controladores de cuerpo completoSe espera de los robots humanoides que sean capaces de ejecutar un gran número de diversas

tareas independientes entre ellas con la misma fidelidad de un robot industrial tradicional.Pero, a diferencia de los robots industriales tradicionales, los robots humanoides han detrabajar en entornos no estructurados y dinámicos. Por lo que sus controladores han de serlo suficientemente versátiles, robustos, reconfigurables y diestros. Es aquí donde entran enescena los controladores de cuerpo completo (Whole Body Controller (WBC), por sus siglasen inglés).

Bajo el nombre de WBC se agrupan una gran variedad de controladores basados en laretroalimentación sensorial y que hacen uso de todos los grados de libertad del robot demanera simultánea para llevar a cabo una o varias tareas que define el programador y quese proyectan sobre el espacio de los actuadores del robot. Los WBC son usados con robotsredundantes, ya que gracias a esta redundancia se puede obtener un comportamiento queactúe con respecto a varias tareas simultáneamente.

Las tareas pueden ser simples, como por ejemplo un error cartesiano de un efector final,o pueden referenciar errores en entidades físicas complejas como puede ser el Zero MomentPoint (ZMP).

Para definir un WBC, es necesario tener en cuenta los siguientes aspectos Moro y Sentis(2019):

• Representación dinámica y cinemática del robot.

• Modelos de las tareas que se desea que el robot lleve a cabo.

• Asignación de prioridades de las diferentes tareas.

• Incorporación de las restricciones provenientes del entorno y del propio robot.

• Método para ajustar los distintos parámetros del controlador.

• Integración del WBC con esquemas de control superiores, como puede ser un planifica-dor de trayectorias.

2.4.1 ClasificaciónUna vez definidos todos los aspectos de un controlador de cuerpo completo, es posible

encontrar una política de control de distintas formas. En general, se diferencia entre las

2.4. Controladores de cuerpo completo 19

políticas de control basadas en velocidad o en fuerza por un lado, y en los controladoresbasados en formas cerradas o en optimización, por otro.

La primera de estas distinciones hace referencia a las salidas producidas por el controlador:los controladores de velocidad producen consignas de velocidad mientras que los controlado-res de fuerza producen consignas de fuerzas y torques (para articulaciones prismáticas y derevolución, respectivamente). En última instancia, estas consignas son llevadas a los controla-dores de bajo nivel de los motores, donde son transformadas en señales de corriente eléctrica.Lo que realmente diferencia a estas dos aproximaciones del control es la política de retroali-mentación de las tareas y el lugar donde las leyes de control son calculadas. En la actualidad,todavía es un tema de debate la preferencia de usar un tipo de controlador u otro.

La segunda distinción hace referencia al método de encontrar la salida del controlador.Como se ha comentado, se puede calcular mediante formas cerradas o mediante optimización.En el primer caso, las salidas son obtenidas tras realizar una serie de cálculos algebraicospredefinidos. En el caso de la optimización, se reformula el problema como un problema deoptimización, para ser posteriormente enviado a un optimizador para encontrar la salida delcontrolador. En general, todos los WBC que se pueden representar en forma cerrada tambiénse pueden reformular como problemas de optimización. Pero conviene no hacerlo ya que lacomplejidad computacional es menor para los controladores de forma cerrada, haciéndolosconsiderablemente más rápidos en la mayoría de los casos.

1. Controladores de velocidad de forma cerradaDado un conjunto de tareas expresadas con respecto a sus velocidades [x1, x2, . . . , xk](donde xi = ki(x

desi − xi) es la tarea i), la definición más general de un WBC es:

q = J#1 x1 + J#

2 x2 + . . .+ J#k xk (2.23)

donde Ji es una transformación diferencial, como por ejemplo podría ser una jacobianacinemática, que mapea las velocidades articulares con las velocidades en el espacio de latarea i. De la misma manera, J#

i es la pseudoinversa de Ji. Existen numerosas formasde calcular posibles pseudoinversas, cada una de ellas dando a lugar a controladorescon comportamientos ligeramente distintos. Se enumeran las principales:

• La pseudoinversa de Moore-Penrose:

J#i = JT

i

(JiJ

Ti

)−1 (2.24)

• La pseudoinversa ponderada:

J#i = W−1JT

i

(JiW

−1JTi

)−1 (2.25)

donde W es una matriz diagonal. Se usa para dar más importancia a unas articu-laciones con respecto del resto.

• Proyección óptima mediante mínimos cuadrados:

J#i = (JiNi)

T(JiNi (JiNi)

T)−1

(2.26)

20 Estado del arte

Esta última pseudoinversa únicamente se puede usar cuando se está haciendo uso de uncontrolador jerárquico. En los WBC jerárquicos se ordenan por prioridades cada tareaque se quiere realizar. La idea está en que la primera tarea tiene prioridad total y noes interferida en absoluto por el resto de tareas. Gracias a la redundancia del robot,es posible que este pueda llevar a cabo la siguiente tarea con los grados de libertadredundantes que sobran, o lo que es lo mismo, en el espacio nulo de la tarea anterior.Esta jerarquía se puede escribir como:

q = J#1 x1 + J#

2|1x2 + . . .+ J#k|p(k)xk (2.27)

donde

Ji|p(i) = JiNp(i) (2.28)

es la transformación diferencial asociada a la tarea i definida en el espacio nulo Np(i)

de las tareas con más prioridad p(i). Np(i) se calcula de la siguiente forma:

Np(i) =i−1∏j=1

Nj (2.29)

donde

Nj =(I − J#

j Jj

)(2.30)

Cabe mencionar que al trabajar con humanoides (o cualquier otro robot de base flotante)las expresiones 2.23 y 2.27 se tienen que premultiplicar por una matriz de proyecciónP que mapea las $n$-dimensiones de las velocidades a las $na$-dimensiones de lasvelocidades de los grados de libertad actuados.

2. Controladores de fuerza de forma cerradaDe manera similar a los controladores de velocidad de forma cerrada, si se define Fi =kpi(x

desi − xi) + kdi(−xi) la definición genérica de la ley de control queda como:

τ = JT1 F1 + JT

2 F2 + . . .+ JTk Fk (2.31)

De forma similar al controlador anterior, las prioridades a las tareas se pueden asignarde manera ponderada o jerárquica. En el primero de los casos la ley de control quedacomo:

τ = w1JT1 F1 + w2J

T2 F2 + . . .+ wkJ

Tk Fk (2.32)

mientras que en el caso de usar una jerarquía:

τ = JT1 F1 + JT

2|1F2 + . . .+ JTk|p(k)Fk (2.33)

2.4. Controladores de cuerpo completo 21

Estas dos representaciones tienen sus inconvenientes: en los controladores jerárquicoses necesario definir un orden estricto de las prioridades de las tareas, mientras queen los controladores ponderados es necesario ajustar los parámetros para encontrar elcomportamiento óptimo, tarea que en la actualidad carece de un proceso automático origuroso. La elección de un tipo u otro de controlador depende en gran medida de latarea que se tiene que realizar.Como alternativa, se puede usar un controlador híbrido, el cual mezcla ambas ideas.El objetivo es tener una serie de tareas (generalmente pocas) que se consideran dealta prioridad y que no pueden ser interferidas por tareas con menos importancia. Aestas tareas de alta prioridad se les aplica el mismo tratamiento que en un controladorjerárquico. Por otro lado, las demás tareas con menos importancia se les aplica el mismotratamiento que en un controlador ponderado, pero la diferencia es que dichas tareassólo pueden actuar en el espacio nulo de las tareas de alta prioridad.

3. Controladores basados en optimizaciónComo ya se ha comentado, siempre que se pueda resolver un problema de control con unWBC de forma cerrada no conviene convertirlo a un problema de optimización ya queen general estos suelen requerir de más recursos computacionales que sus contrapartesalgebraicas. Sin embargo, los controladores de forma cerrada son incapaces de expresardesigualdades, cosa que los controladores basados en optimización sí que pueden ma-nejar. Por lo que en el caso que se requiera trabajar con desigualdades es estrictamentenecesario realizar un controlador basado en optimización.Al igual que en los tipos anteriores de WBC, se pueden tratar las tareas de manerajerárquica o ponderada. Con los mismos efectos y desventajas, y siendo un método uotro más conveniente según la aplicación.Un ejemplo de WBC jerárquico basado en optimización se puede encontrar en el trabajorealizado por Kanoun y cols. (2011). Con anterioridad a este trabajo, las desigualdadesse gestionaban de una manera muy ineficiente. De manera similar a la robótica móvil,se imponían dinámicas no lineales mediante un campo de fuerza, usando fuerzas deatracción hacia las tareas que se quieren realizar y de repulsión para alejar al robot delos estados que se quiere evitar. De esta manera se podía reformular el problema denuevo solamente mediante igualdades, formulación que resulta ser más restrictiva quelas desigualdades originales. Kanoun y cols. (2011) soluciona esta limitación mediante elcálculo de la ley de control mediante una cascada secuencial de programas cuadráticos.Se considera una estructura cinemática de n grados de libertad con un vector de confi-guración q ∈ Rn, y una secuencia de k tareas descritas por las funciones fk(q). Normal-mente estas funciones son no-lineales y sin inversas sencillas, por lo que se suele recurrira métodos numéricos para operar con ellas. Considérese la siguiente EDO:

∂f1(q)

∂qq = −λf1(q) (2.34)

Donde λ ∈ R+. Siguiendo la trayectoria de esta EDO, q converge a q∗ de manera quef1(q

∗) = 0. Al trabajar con sistemas sobreactuados, es común encontrarse con que esta

22 Estado del arte

EDO está infradeterminado, dando lugar a infinitas posibles soluciones para q∗. Es deespecial interés la solución con la mínima norma, por lo que se puede reformular laEDO de la siguiente manera:

q1 = arg minx∈S1

1

2∥x∥2 (2.35)

donde:

S1 = {arg minx

1

2∥A1x− b1∥2} (2.36)

Ai =∂fi(q)

∂q(2.37)

bi = −λfi(q) (2.38)

Se puede ver fácilmente que la solución es:

q1 = A#1 b1 (2.39)

El conjunto S1 es un subespacio afín tal que:

S1 = {q1 + P1z1, z1 ∈ Rn} (2.40)

donde P1 es el operador de proyección ortogonal sobre el espacio nulo de la matriz A1:

P1 = I −A#1 A1 (2.41)

Como se puede observar, para cualquier valor que tome z1 se cumple la restricciónexpuesta en 2.34. Por lo tanto, es posible optimizar con respecto a otros objetivos conmenor prioridad que f1(q) dentro del conjunto de soluciones S1 que ya satisface larestricción 2.34.

Considérese una segunda EDO:

A2q = b2 (2.42)

Procediendo de la misma manera que en el primer caso, pero teniendo en cuenta quelas soluciones que se buscan tienen que estar dentro de S1, se calcula el conjunto desoluciones S2 como:

S2 = {arg minx∈S1

1

2∥A2x− b2∥2} (2.43)

2.4. Controladores de cuerpo completo 23

Al igual que con la primera tarea, la solución de interés es la que minimiza la norma:

q2 = arg minx∈S2

1

2∥x∥2 (2.44)

El resultado puede obtenerse de nuevo con la matriz pseudoinversa de A2. Pero primero,es necesario proyectarla sobre el espacio nulo de la matriz A1.

q2 = q1 + (A2P1)#(b2 −A2q1) (2.45)

Si se quisieran añadir más tareas, el procedimiento sería el mismo. Se puede observar unpatrón entre las expresiones 2.39 y 2.45, el cual se repite recursivamente para todas lastareas. Los subespacios S1, S2, . . . se calculan resolviendo los programas cuadráticosdefinidos por las expresiones 2.36, 2.43, ….

2.4.2 Controladores de cuerpo completo aplicados a robots bípedosEn la década de los 80, el campo de investigación del control de robots redundantes llega

a un cierto grado de madurez. Es en una publicación realizada por Khatib (1987) donde seintroducen las bases de los que serán los futuros controladores WBC aplicados a los robotsmóviles con patas y por extensión a los robots bípedos.

Kajita y cols. (2003) fueron los primeros investigadores en aplicar un controlador de cuerpocompleto a un robot humanoide. Para ello, hicieron uso del momento de inercia global delrobot en cada instante de tiempo, simplificando así los cálculos necesarios. Su control es deforma cerrada y basado en la velocidad. Consiguieron que el robot HRP-2 Kaneko y cols.(2004) realizase movimientos de andar y de pataleo.

Por otro lado, en el trabajo realizado por Sentis y Khatib (2005) se propone por primeravez un WBC basado en control por torque capaz de interaccionar con su entorno.

En la actualidad, este campo de investigación no es tan activo como hace unos pocos añosya que se considera que el problema está resuelto. Del Prete y cols. (2014) define un marco detrabajo llamado TSID (Task Space Inverse Dynamics), en el que se propone un WBC basadoen optimización y con una formulación moderna. Se puede encontrar su implementación oficialcomo programa de código abierto en GitHub 1. Mientras que en Bidaud y cols. (2013) se lellama a esta formulación LQP, pero se puede ver que presenta la misma formulación.

De manera separada, y siendo un trabajo más reciente se encuentra el realizado por Bouyar-mane y cols. (2019), en el cual se describe un método de generación de trayectorias, un WBCpara seguir las trayectorias generadas y un controlador Model Predictive Control (MPC) quejunta los dos componentes anteriores para poder generar movimientos altamente dinámicoscapaz de responder a grandes perturbaciones externas.

El WBC que se introduce en dicha publicación consta de una máquina de estados finitacon dos estados. El robot cambia al estado opuesto cuando crea o deshace un contacto conel entorno. Estos estados son:

• ’Mover el centro de masas’: cuando el robot deshace un contacto con el entorno se definendos tareas. La primera de ellas consiste en el seguimiento de la posición tridimensional

1https://github.com/stack-of-tasks/tsid

24 Estado del arte

del centro de masas, mientras que la segunda tarea consiste en seguir las consignas dela configuración articular en el instante actual.

• ’Mover el eslabón del contacto’: cuando el robot crea un contacto, se define entonces trestareas para el controlador. Las dos primeras son las especificadas en el punto anterior,a las que se le añade la tarea de seguimiento en posición y orientación (6D) del eslabónel cual va a hacer el contacto.

La tarea del seguimiento del centro de masas hace que el robot mantenga el equilibrio.La tarea de seguimiento del eslabón que va a hacer el contacto permite que el robot gire yposicione el eslabón pertinente para realizar el contacto contrarrestando los posibles erroresintroducidos por la trayectoria. Finalmente, la tarea de seguimiento de las configuracionesarticulares sirve para rellenar la redundancia del resto de movimientos que no se puedenobtener con las dos tareas anteriores.

En estos dos últimos trabajos comentados, se formula de manera muy similar el controlcomo un problema de optimización cuadrático (la programación cuadrática es explicada enel apartado 2.1.1). En cada instante de tiempo t, el controlador recibe como entradas unconjunto de tareas (τj)j , sus jacobianos Jτj y sus pesos wj . Una tarea es una función de costeque se busca minimizar, y normalmente es un residual: τj(q, q) = pobjetivo − probot(q, q) sobreun comportamiento que se quiere que tenga el robot. Al controlador se le realimenta con elestado actual del robot (sus posiciones y velocidades articulares: q, q). Con esta información,el controlador obtiene como solución las aceleraciones articulares q, el vector de todas lasfuerzas de contacto concatenadas f y los torques a aplicar en cada articulación u, medianteel siguiente programa cuadrático:

min(q, f, u)

∑j

wj∥τj + kv τj + kpτj∥2 (2.46)

Cumpliendo las siguientes restricciones:

• Las ecuaciones del movimiento para un sistema multiarticular:

M(q)q +N(q, q) = STu+ JT f (2.47)

• Los puntos de contacto no experimentan movimiento, por lo que sus velocidades yaceleraciones tienen que ser nulas. Estas restricciones se pueden modelar de la siguienteforma:

p = Jq = 0 (2.48)p = Jq + Jq = 0 (2.49)

Sin embargo, no se está optimizando con respecto a q, por lo que no se puede optimizarcon respecto a la restricción expresada en 2.48. Para gestionar este problema, se va aasumir que todos los contactos que realice el robot serán no deslizantes. De este modo,tendremos la garantía de que mientras se esté siguiendo la trayectoria planificada conanterioridad, cuando el robot haga un contacto con el entorno, la velocidad del puntodel contacto será instantáneamente nula.

2.4. Controladores de cuerpo completo 25

• Los límites de actuación de los motores:

−umáx ≤ u ≤ umáx (2.50)

• La estabilidad del contacto. En este caso se trabaja sólo con contactos unilaterales, porlo que para que dichos contactos sean estables, es decir, que no se produzca deslizamien-to, es necesario que la fuerza de reacción esté dentro del cono de rozamiento. Debidoa las limitaciones numéricas de los optimizadores basados en programación cuadrática,es imposible representar de manera fiel este cono de rozamiento, aún siendo esta formaconvexa. Esto es así porque las desigualdades en el optimizador se pueden represen-tar sólo con hiperplanos, y haría falta un número infinito de hiperplanos para poderrepresentar fielmente el cono de rozamiento.Sin embargo, es posible usar una restricción más limitante que la del cono real derozamiento y que el optimizador pueda trabajar con ella. El precio a pagar es que seestán descartando las posibles soluciones que caen dentro del cono real de rozamientopero fuera de la nueva restricción. Por ejemplo, se puede definir una pirámide con basede polígono regular inscrita dentro del cono de rozamiento. Cuantos más lados tenga labase de la pirámide, más se aproximará su forma a la del cono de rozamiento real. En lapráctica, para el movimiento de un robot humanoide suele bastar aproximar el cono derozamiento inscribiendo una pirámide de cuatro lados. La desigualdad de la pirámidese puede representar entonces como la desigualdad de una transformación afín aplicadaal vector de fuerzas concatenadas f:

Ff ≤ 0 (2.51)

Como resultado se obtienen q y u. Si se puede realizar un control de fuerzas, basta conusar las consignas u. En cambio, si se tiene que usar un controlador de posición, basta conintegrar doblemente q para obtener las posiciones deseadas.

3 Metodología

3.1 Planificación de trayectorias

Se va a usar towr (Winkler y cols. (2018)) para la generación de trayectorias en las queel robot Talos camine. towr es un proyecto de código abierto creado inicialmente para lageneración de distintos andares para robots cuadrúpedos como el HyQ (Semini y cols. (2011))o el ANYmal (Hutter y cols. (2016)). Utiliza un método directo y simultáneo (ver apartado2.3) para la formulación de un problema de programación no lineal que es mandado a unoptimizador para su resolución. Tanto SNOPT (Gill y cols. (2002)) como IPOPT (Wächtery Biegler (2006)) son usados en el trabajo original. Sin embargo, en el presente proyecto sólose ha podido trabajar con IPOPT gracias a su permisiva licencia. towr es una librería sindependencias externas, pero los desarrolladores convenientemente han publicado una interfazpara que la operación con ROS sea sencilla.

La flexibilidad de towr ha permitido usarlo en otros proyectos robóticos, como es el casode los trabajos realizados por Bratta y cols. (2020); Calvo y cols. (2021).

A continuación se describe en detalle cada componente usado para poder reformular elproblema de optimización de trayectorias como uno de programación no lineal.

3.1.1 Parametrización

El objetivo es obtener las posiciones y orientaciones del centro de masas del cuerpo delrobot, así como la posición de sus patas. Como conocimiento previo, se conocen el numero depasos que el robot tiene que dar y el tiempo total en el que tiene que darlos, T . Sin embargo,no hace falta especificar la duración de cada paso, ya que se parametrizan las duraciones comovalores continuos ∆Ti,j que el optimizador tiene que encontrar, dotándolo de mas libertadpara buscar un rango mas amplio de posibles movimientos (y por tanto, ralentizándolo).

Para parametrizar las trayectorias en el espacio del cuerpo y piernas del robot, se podríaoptar por guardar los valores de cada variable para cada instante de tiempo. Pero esto incre-mentaría de manera lineal con el tiempo el número de variables con las que el optimizadortendría que trabajar. Una forma de reducir este crecimiento innecesario en el numero de va-riables es asumir que para intervalos pequeños de tiempo las trayectorias van a ser continuas.

Cada intervalo de tiempo estará descrito entonces por un spline, y las variables a optimizarserán las que describan la secuencia de splines. Para los movimientos de la base del robotse usan splines de cuarto grado, mientras que para los movimientos de las piernas cuandoéstas están en el aire se usan tres splines de tercer grado, que son suficientes para describirel movimiento que hace cada efector final a lo largo de un paso.

27

28 Metodología

3.1.2 Restricciones

Se hace uso del modelo de la dinámica del sólido rígido único (ver apartado 2.2.2) comouna simplificación de las dinámicas del robot real. Esto hace que la optimización sea mássencilla y rápida. Pero como ya se ha comentado con anterioridad, se está asumiendo que lamasa de las piernas es despreciable y que la posición de cada efector final no depende de laestructura cinemática de su extremidad correspondiente.

Mediante las restricciones de igualdad y desigualdad que se pueden introducir en el pro-blema de optimización no lineal (2.22) se describen las propiedades que deben cumplir lastrayectorias a generar. A continuación se enumeran las restricciones utilizadas.

1. Modelo cinemático

No se usa el modelo cinemático del robot ya que lo que se busca es encontrar lastrayectorias cartesianas tanto de la base como de los efectores finales. Sin embargo,se tiene que garantizar que en cada momento la posición de los pies sea alcanzablepor el robot. Esto se consigue definiendo un volumen con forma de prisma rectangularque está siempre en la misma posición relativa con respecto a la base del robot. Lasposiciones cartesianas del efector final tienen que estar en todo momento dentro de suprisma rectangular correspondiente (Figura 3.1).

Figura 3.1: Los prismas rectangulares correspondientes a cada pie se pueden ver abajo. Sus posi-ciones relativas con la base del robot (visualizada como el prisma rectangular negro enel abdomen del robot) son constantes. Las posiciones cartesianas de los pies, que estánmarcadas por las esferas azules, siempre tienen que estar dentro de su prisma en todomomento durante la totalidad de la trayectoria.

3.1. Planificación de trayectorias 29

Debido a que los movimientos generados están descritos por splines, es posible ejerceresta restricción en intervalos de tiempo en vez de en cada iteración. Esta aproximacióntiene sentido siempre que los movimientos generados no sean muy bruscos. Lo que seconsigue es una reducción en el número de restricciones y por tanto mejores condicionespara que el optimizador encuentre la solución más rápido.

2. Modelo dinámicoSe elige trabajar sobre el modelo del sólido rígido único (apartado 2.2.2) ya que permitela generación de movimientos algo más complejos que usando el modelo del pénduloinvertido lineal, siendo aún una simplificación que permite la generación de trayectoriasde manera rápida.No se trabaja con la dinámica completa del robot por la sencillez conceptual que ofrecetrabajar con el modelo cinemático ya expuesto en conjunto con el modelo dinámicoelegido. Al pensar en el sistema como un sólido rígido al que se le aplican fuerzas desdelos puntos de contacto, es posible desacoplar cualquier sistema robótico de la tarea deoptimización de trayectorias. Esto es así porque en ningún momento se tienen en cuentalos parámetros físicos del robot que no sean su masa o inercia. Además, en otros trabajosdonde se planifica teniendo en cuenta todos los parámetros cinemáticos y dinámicos delrobot la generación de trayectorias puede tardar del orden de 10 minutos (Posa y cols.(2016)). Esto ralentiza el desarrollo y dificulta la realización de pruebas.En concreto, la dinámica del robot se expresa como:

mr(t) =

ni∑i=1

fi(t)−mg (3.1)

Iω(t) + ω(t)× Iω(t) =

ni∑i=1

fi(t)× (r(t)− pi(t)) (3.2)

Donde m es la masa del robot, ni es el número de patas, g es la aceleración de lagravedad y ω(t) es la velocidad angular que puede ser calculada a partir de los ángulosde Euler resultantes de la optimización θ(t) y θ(t).La inercia I se calcula con el robot en su posición nominal, y es constante aunque laspiernas cambien de posición, ya que se asume que tienen un peso despreciable.La restricción consiste en hacer que 3.1 se cumpla cada ciertos intervalos de tiempo alo largo de toda la trayectoria. Además, se pone una restricción más para asegurarsede que las aceleraciones en la unión de todos los splines contiguos sean iguales, deesta forma se evitan discontinuidades en la aceleración (ya que estas discontinuidadesimplicarían cambios bruscos en las fuerzas externas o en las posiciones de los pies).El modelo dinámico descrito hasta ahora perfectamente puede servir para describirlos movimientos de un quadrotor. Lo que lo hace especifico para el caso de robotsmóviles con patas son las restricciones de las fuerzas y posiciones de las patas, quecambian abruptamente cuando cada pata entra en contacto con el suelo o despega.

30 Metodología

Estas discontinuidades no trabajan de forma natural con la forma de operar de losoptimizadores no lineales.

Para simplificar un poco este problema, se puede tratar cada pata del robot de maneraindividual, teniendo en cuenta las fuerzas de las patas que están en el suelo. Esto haceque se puedan modelar los contactos de cada pata con una variable booleana, y que lamisma formulación funcione para robots con distinto numero de patas.

3. Modelo de los contactos

El modelo dinámico propuesto hasta ahora sirve para describir el movimiento de cual-quier sólido rígido, como podría ser un dron. Es necesario añadir restricciones querepresenten los cambios abruptos de las fuerzas externas cuando se crean y eliminanlos contactos de las patas del robot con el exterior. Debido a que los optimizadores nolineales no trabajan bien con cambios discretos, se modelan estos cambios mediante susduraciones de cada fase.

4. Altura del terreno

Cuando los pies del robot no están en el aire, tienen que estar sobre el terreno. En casocontrario, los pies tienen que estar por encima del mismo y no debajo del suelo.

La formulación permite usar suelos representados mediante mapas de altura, por loque se pueden generar trayectorias para una gran variedad de terrenos complejos comopuede ser el de la Figura 3.2.

Figura 3.2: Visualización de un terreno con una rampa.

3.1. Planificación de trayectorias 31

Por otro lado, no está en los objetivos del presente trabajo el movimiento del robot enterrenos complicados, por lo que siempre se va a trabajar con el terreno plano.

5. Fuerzas en reposo

Los contactos realizados con el suelo son unilaterales, por lo que las fuerzas solamentepueden ser de repulsión con el suelo. Además, se quiere que todos los contactos seanestables, por lo que las fuerzas tienen que estar dentro de sus conos de rozamientosen todo momento. Se hace una descripción más detallada de la derivación de estarestricción en el apartado 3.2.2. Por lo que no se va a repetir aquí.

3.1.3 Costes

En el trabajo original de Winkler y cols. (2018), el problema planteado para encontrartrayectorias no hace uso de ninguna función objetivo. Cualquier conjunto de variables quesatisfaga todas las restricciones resulta en una trayectoria viable para ser replicada. Estoacelera enormemente la velocidad de la optimización.

Sin embargo, esa formulación se hizo teniendo en mente su aplicación para robots cua-drúpedos. En el caso de usar un robot bípedo, las trayectorias generadas pueden resultar enoscilaciones algo bruscas en la base del robot. Esto es debido a que la duración de los pasos seincrementa en este trabajo para generar trayectorias más estables requeridas por los robotshumanoides. Al incrementar la duración, se le está permitiendo al optimizador encontrartrayectorias que cumplan con las restricciones pero que no sean del todo suaves.

Para solucionar este problema, se introduce un coste en la velocidad de la base del robot conrespecto al eje vertical. Gracias a esto se consigue reducir considerablemente las oscilaciones,a costa de incrementar ligeramente el tiempo requerido para encontrar una solución.

3.1.4 Formulación matemática

Una vez explicados todos las restricciones y costes, a continuación se muestra la formulaciónde la optimización de trayectorias aplicada al problema de la locomoción:

32 Metodología

encuentra

que minimice

tal que:

r(t) ∈ R3 (Posicion del Centro de Masas)θ(t) ∈ R3 (Orientacion de la base)para cada pie i:∆Ti,1, . . . ,∆Ti,2ns,i ∈ R (Duracion de las fases)pi(t,∆Ti,1, . . .) ∈ R3 (Posiciones de las patas)fi(t,∆Ti,1, . . .) ∈ R3 (Fuerza de los pies)

wrz(t) (Velocidad de la base en el eje vertical)

[r,θ](t = 0) = [r0,θ0] (Estado inicial)r(t = T ) = rg (Estado final)[r, ω]T = F (r,p1, . . . ,f1, . . .) (Modelo dinamico)para cada pie i:pi(t) ∈ Ri(r,θ) (Modelo cinematico)si el pie i esta en contacto:

pi(t ∈ Ci) = 0 (Sin deslices)pzi (t ∈ Ci) = hterreno(p

xyi ) Altura del terreno

fi(t ∈ Ci) · n(pxyi ) ≥ 0 (Fuerza de empuje)

fi(t ∈ Ci) ∈ F(µ,n,pxyi ) (Fuerza dentro del cono de friccion)

si el pie i esta en el aire:fi(t ∈ Ci) = 0 (no hay fuerza estando en el aire)

2ns,i∑j=1

∆Ti,j = T (Duracion total)

3.2 Controlador de cuerpo completo

Como ya se ha comentado, las trayectorias generadas se basan en un modelo dinámicosimplificado para que el planificador pueda encontrar una solución en un tiempo razonable.Por lo tanto, estas trayectorias no pueden ser reproducidas en bucle abierto por el robotya que en la trayectoria generada no se tienen en cuenta aspectos como los pesos de loseslabones del robot y sus inercias. Sobre todo, en este caso en particular de especial interésson las inercias generadas por las piernas debido a que un gran parte del peso del robot seencuentra localizado ahí.

Por lo tanto, para poder seguir la trayectoria con éxito es necesario añadir algún controladorque cierre el bucle y permita al robot ir corrigiendo las pequeñas discrepancias en tiempo realmientras se está ejecutando la trayectoria. Una de las posibles soluciones es usar un WBC,

3.2. Controlador de cuerpo completo 33

explicados en el apartado 2.4.En particular, se ha decidido implementar desde cero un WBC para la realización de esta

tarea. Es en esta parte del proyecto en donde se ha centrado la mayoría del trabajo prácticorealizado. Concretamente, el WBC creado se basa en los trabajos de Bouyarmane y cols.(2019); Bidaud y cols. (2013); Eljaik y cols. (2018). Se plantea el control del robot medianteun WBC ponderado formulado como un problema de optimización convexa en forma deprogramación cuadrática. Es decir, se parte de lo explicado en el apartado 2.4.2.

3.2.1 TareasSe definen las tareas a realizar como residuales entre la consigna a seguir y la magnitud

correspondiente actual extraída a partir del estado del robot:

τi = ξ∗i − ci(q) (3.3)

donde τi es la tarea en cuestión, ξi es la consigna de la tarea a seguir (puede ser la referenciaarticular, del centro de masas deseado, la posición y orientación de un eslabón en particular,…) y ci es la función que dada el estado articular del robot calcula la misma magnitud.

Se procede a calcular las derivadas de la tarea:

τi = ξi −∂ci(q)

∂q

dq

dt= ξi − Jτ q (3.4)

τi = ξi − Jτ q − Jτ q (3.5)

Al igual que lo explicado en el apartado 2.4.2, se busca encontrar las variables q, f y u (ace-leración de todos los grados de libertad, fuerzas en los puntos de contacto y fuerzas/torques aaplicar en las articulaciones actuables, respectivamente). Se puede observar que no se puedemodificar directamente ni q ni q, ya que se está actuando directamente el robot mediantecontrol por torque, y las dinámicas de todo sistema mecánico son de segundo orden. La con-secuencia es que no se puede controlar de forma directa el error en posición y en velocidad delas tareas. Para solventar este problema, basta con imponer una dinámica a todas las tareasτi, de manera que se pueda introducir un error en posición y/o velocidad. Cualquier tipo dedinámica es válida, incluso dinámicas no lineales. Pero una dinámica lineal de segundo ordensuele dar buenos resultados en la práctica (Del Prete y cols. (2014)):

τdi = τi + kv τi + kpτi (3.6)

Donde τdi indica la tarea deseada a seguir después de haber aplicado la dinámica a laconsigna original. Por lo general (y es el caso de la implementación realizada), se suele escogerkv = 2

√kp de modo que el sistema queda críticamente amortiguado.

Desde el punto de vista de la optimización, se busca encontrar al siguiente expresión:

minq,f,u

∑i

1

2wi∥τdi ∥2 (3.7)

Es necesario desarrollar 3.7 hasta obtener una expresión que pueda ser usada por el op-timizador de programas cuadráticos. Debido a las propiedades de los sumatorios, es posible

34 Metodología

desarrollar la minimización de cada tarea de manera independiente y posteriormente sumarlos resultados:

minq,f,u∥τdi ∥2 = min

q,f,u∥ξi − Jτi q − Jτi q + kv τi + kpτi∥2 (3.8)

= minq,f,u

1

2(ξi − Jτi q − Jτi q)

2 + (��ξi − Jτi q −���Jτi q )

T (kv τi + kpτi) (3.9)

= minq,f,u

1

2(��ξ

2i − 2ξi(Jτi q +�

��Jτi q ) + (Jτi q + Jτi q)2)− (kv τi + kpτi)

TJτi q (3.10)

= minq,f,u

1

2((Jτi q)

2 + 2 [Jτi q]T Jτi q +����(Jτi q)

2 )− (ξi + kv τi + kpτi)TJτi q (3.11)

= minq,f,u

1

2[Jτi q]

T Jτi q − (ξi − Jτi q + kv τi + kpτi)TJτi q (3.12)

= minq,f,u

1

2qT

[JTτiJτi

]q −

[ξi − Jτi q + kv τi + kpτi

]TJτi q (3.13)

Después de multiplicar 3.13 por wi, se obtiene tanto la matriz a sumar a la matriz Hessianade la formulación cuadrática ,correspondiente al término JT

τiJτi , como la parte correspondiente

del vector gradiente[ξi − Jτi q + kv τi + kpτi

]TJτi .

Por tanto, para modelar cualquier tarea es necesario encontrar su jacobiana y la derivadade su jacobiana. En algunos casos, es posible ahorrarse el cálculo de la derivada del jacobianode la tarea. En su lugar, se calcula directamente Jτi q haciendo uso de la siguiente propiedad:

ci(q, q, q) = Jτ q + Jτ q (3.14)ci(q, q,0) = Jτ q (3.15)

A continuación se exponen las tareas implementadas en este trabajo.

1. Seguimiento de la posturaEsta tarea consiste en el seguimiento de las consignas en el espacio articular por todaslas articulaciones actuables del robot. Es decir, se pretende seguir q∗, q∗ y q∗. Comoretroalimentación, se tienen las posiciones y velocidades articulares actuales del robot(q y q).Hay que calcular el jacobiano de la tarea y su derivada. Con una rápida observación, sepuede deducir que el jacobiano es una matriz diagonal semidefinida positiva, siendo elvalor de Jjj = 1 cuando la articulación j sea actuable y 0 cuando la articulación j nosea actuable. Esta deducción se ve claramente partiendo de 3.4, ya que en este caso enparticular:

ci(q) = q (3.16)∂ci(q)

∂q= 1 (3.17)

3.2. Controlador de cuerpo completo 35

Para la derivada de la jacobiana, sabiendo que todos los elementos de la misma sonconstantes (o 1 o 0), se deduce que es una matriz nula:

∂2ci(q)

∂q2= 0 (3.18)

Por lo que la expresión de la tarea queda como:

minq,f,u

1

2qT Iq −

[ξi + kv τi + kpτi

]Tq (3.19)

2. Seguimiento del centro de masas

En este caso, Jτ se corresponde con la jacobiana del centro de masas. Es posible evitarel cálculo del término Jτ con el método explicado anteriormente. Basta con calcularla cinemática directa del centro de masas con las posiciones y velocidades articularesactuales del robot, pero usando un vector nulo para las aceleraciones articulares. Laexpresión de la tarea por tanto queda igual que en 3.13.

3. Seguimiento de la orientación

Para esta tarea, se pretende conseguir que el robot siga una referencia de orientaciónde un eslabón suyo. Esta tarea se utilizará para intentar mantener la base del robot enla orientación deseada, pero se puede usar para cualquier parte del cuerpo calculandolos jacobianos necesarios. Se parte de la expresión (3.13) para definir la tarea, dondeJτ es la jacobiana de tamaño 3 × n que relaciona la variación de todos los grados delibertad del robot con la variación en su orientación dada por los ángulos de Euler.

El error en posición de la orientación, τ , se calcula de manera similar al trabajo realizadoen Yamane y Nakamura (2003). Siendo R3×3

D la orientación deseada del eslabón y R3×3a

su orientación actual, se calcula su error en posición como:

τ =1

2

∆R(1, 2)−∆R(2, 3)∆R(1, 3)−∆R(3, 1)∆R(2, 1)−∆R(3, 2)

(3.20)

donde ∆R = RDRTa y ∆R(m,n) se corresponde con el elemento de la matriz ∆R en la

fila m y columna n.

3.2.2 Restricciones

1. Ecuaciones del movimiento

Se tiene que cumplir en todo momento las ecuaciones del movimiento de Euler-Lagrange,expuestas en el apartado 2.2.1:

M(q)q +N(q, q) = STu+ JT f (3.21)

36 Metodología

Es posible trabajar con la dinámica completa del robot en tiempo real porque es unalinearización de la misma ya que desde el punto de vista del controlador, en cada instantede tiempo q y q son constantes y por consecuencia las matrices también.

A la hora de pasar esta restricción a una expresión matricial para que el optimizadorpueda encontrar las soluciones a las variables [ �q f u]T, se tiene:

[M −JT − ST

] qfu

= −N(q, q) (3.22)

2. Contactos estáticos

La cinemática de los contactos del robot con el entorno exterior tiene que ser estática,es decir, que los contactos no se muevan con respecto al marco de referencia global.Esto es necesario ya que en este trabajo no se modelan ni se tienen en consideración losdeslizamientos.

Para asegurar a nivel cinemático que se cumple esta propiedad, hay que igualar lavelocidad y la aceleración del contacto a 0. Por lo que:

p = J(q)q = 0 (3.23)p = ˙J(q)q + J(q)q = 0 (3.24)

Como se puede ver en la ecuación 3.23, ningún término depende de alguna de lasvariables a optimizar, por lo que no se puede ejercer dicha restricción. Sin embargo, larestricción 3.24 sí que depende directamente de q. De la siguiente manera se le puedepasar la restricción al optimizador:

J(q)q = − ˙J(q)q (3.25)

[J3n×nv 0 0

] qfu

= − ˙J(q)q (3.26)

Calcular de manera directa ˙J(q) para posteriormente multiplicarlo por q no es unaaproximación computacionalmente eficiente. Teniendo en cuenta que:

p = ˙J(q)q + J(q)q (3.27)

Si se considera q = 0, se tiene que:

p = ˙J(q)q si q = 0 (3.28)

3.2. Controlador de cuerpo completo 37

Por lo que calculando la cinemática directa del contacto siendo q y q las posiciones yvelocidades articulares actuales del robot, y q siendo un vector nulo, se obtiene direc-tamente el término ˙J(q)q.

3. Límites de los actuadores

Los actuadores tienen límites físicos en el torque total que pueden aplicar sobre suarticulación correspondiente. Modelar estos límites en la optimización es tan sencillocomo expresar que cada elemento del vector de torques no debe sobrepasar los límitesde las especificaciones del robot:

−umáx ≤ u ≤ umáx (3.29)

4. Estabilidad de los contactos

Para que los contactos sean estables, las fuerzas resultantes en los contactos unilateralestienen que estar dentro de sus respectivos conos de rozamiento. De lo contrario, estoscontactos deslizarían, provocando en este caso una muy posible caída del robot.

Las dos restricciones que aseguran que la fuerza f caiga dentro del cono de rozamientoes:

fi · ni > 0 fuerza en la dirección normal (3.30)∥fi∥2 ≤ µ(fi · ni) fuerza dentro de cono de rozamiento (3.31)

La restricción 3.31 no es una expresión muy amigable con los programas de optimizaciónnumérica, ya que se necesitarían tener en cuenta los infinitos hiperplanos de soporte quegenera el cono de rozamiento, al trabajar el optimizador únicamente con restriccionesdefinidas con hiperplanos. Una solución es trabajar con una aproximación del cono derozamiento que consiste en inscribir un polígono regular dentro de la circunferenciade rozamiento. En este caso, se va a inscribir un cuadrado, que aproxima de manerasuficiente al cono para este caso.

Se define un sistema de coordenadas centrado en el punto del contacto, el cual estáformado por tres vectores normales entre sí: ti, bi y ni. ni es un vector unidad que esnormal al plano del contacto, y ti y bi son dos vectores pertenecientes al plano de lasuperficie del contacto, da igual su orientación. Al estar el cuadrado inscrito, se define unnuevo coeficiente de rozamiento de tal manera que las aristas de la pirámide coincidancon el cono de rozamiento real:

µ =1√2µ (3.32)

Entonces, la pirámide inscrita dentro del cono queda definida por:

38 Metodología

|ti · fi| ≤ µ(fi · ni) (3.33)|bi · fi| ≤ µ(fi · ni) (3.34)

Expandiendo únicamente la expresión 3.33, quedan las desigualdades:

ti · fi ≤ µ(fi · ni) (3.35)−ti · fi ≤ µ(fi · ni) (3.36)

Que moviendo todos los términos al lado izquierdo de la desigualdad quedan como:

fi(ti − µ · ni) ≤ 0 (3.37)−fi(ti + µ · ni) ≤ 0 (3.38)

Por extensión, la expresión 3.34 queda como:

bi(ti − µ · ni) ≤ 0 (3.39)−bi(ti + µ · ni) ≤ 0 (3.40)

Estas cuatro expresiones, junto con 3.30 conforman todas las desigualdades a tener encuenta para optimizar teniendo en cuenta esta restricción.La expresión matricial queda como:

−∞ ≤

−f0 · n0

f0(t0 − µ · n0)−f0(t0 + µ · n0)b0(t0 − µ · n0)−b0(t0 + µ · n0)−f1 · n1

f1(t1 − µ · n1)−f1(t1 + µ · n1)b1(t1 − µ · n1)−b1(t1 + µ · n1)

...−fi · ni

fi(ti − µ · ni)−fi(ti + µ · ni)bi(ti − µ · ni)−bi(ti + µ · ni)

≤ 0 (3.41)

3.2. Controlador de cuerpo completo 39

Nótese que a fi · ni se le cambia el signo para que las desigualdades sean homogéneasa todos los elementos de la restricción.

Por tanto, la matriz a pasar al optimizador será la siguiente:

−∞ ≤

0

−n0

(t0 − µ · n0)−(t0 + µ · n0)b0(t0 − µ · n0)−b0(t0 + µ · n0)

−n1

(t1 − µ · n1)−(t1 + µ · n1)b1(t1 − µ · n1)−b1(t1 + µ · n1)

...−ni

(ti − µ · ni)−(ti + µ · ni)bi(ti − µ · ni)−bi(ti + µ · ni)

0

qfu

≤ 0 (3.42)

3.2.3 Problema resultante

A continuación se muestra la vista general del problema de programación cuadrática que seresuelve en cada instante de tiempo. En la matriz de restricciones se indican las dimensionesde cada submatriz en sus subíndices para ayudar a la comprensión.

minq,f,u

1

2

[q f u

] wqI + wcJTτcJτc 0 0

0 0 00 0 0

qfu

−

wq(qr + kvq τq + kvqτq) + wc(pc − Jcq + kvc τc + kpcτc)00

T qfu

(3.43)

tal que: −N(q, q)

−J q−umax

−∞

≤Mnv×nv −JT

nv×3n −STnv×na

J3n×nv 0 00 Ina×nv

0 F5n×3n 0

qfu

≤−N(q, q)

−J qumax

0

(3.44)

Donde q, f y u son las aceleraciones articulares, fuerzas de contacto externas y torques delas articulaciones actuadas, respectivamente; wq y wc son los pesos de las tareas de posturay centro de masas, I es la matriz identidad, τq hace referencia a la tarea de la postura y τc a

40 Metodología

la de la posición del centro de masas y Jτc a su jacobiana, kpq y kvq son las constantes de ladinámica de la tarea de la postura y kpc y kvc las de la tarea del centro de masas.

3.3 SimulaciónAl no tener acceso al robot real la validación del controlador se hace de manera virtual

mediante un simulador de físicas. Ya que se hace uso de ROS para la programación de lasdistintas partes del proyecto, se elige al proyecto Gazebo (Koenig y Howard (2004)) comosimulador, ya que la integración con ROS es excelente. Gazebo es un simulador que permitetrabajar con varios motores de físicas. Por defecto, sólo funciona con ODE (Smith y cols.(2005)) y Bullet (Coumans y cols. (2013)). Es posible compilar Gazebo a mano para incluirel soporte a otros dos motores de físicas: Dart (Lee y cols. (2018)) y Simbody (Sherman ycols. (2011)).

• ODE: es una biblioteca de código abierto para simular dinámicas de sólidos rígidos yque incluye un detector de colisiones. Representa las articulaciones entre eslabones enforma de restricciones. Es uno de los motores de físicas más usados y antiguos.

• Bullet: también una biblioteca de código abierto concebida para el campo de gráficospor ordenador y animación. Sin embargo, en versiones recientes se ha incluido soportede problemas lineales complementarios y la implementación del algoritmo de los cuerposarticulados de Featherstone (Featherstone (2014)). Esto ha convertido a Bullet en unsimulador apto para aplicaciones robóticas.

• Dart: es una biblioteca de código abierto creada por el Instituto Tecnológico de Georgia.A diferencia de ODE y Bullet, la dinámica se formula utilizando las ecuaciones deLagrange con motivo de obtener una mayor precisión.

• Simbody: como todas las demás, también es una biblioteca de físicas de código abierto.Implementa los algoritmos del libro de Featherstone (Featherstone (2014)) y ofrece comonovedad con respecto a los otros motores de físicas la posibilidad de utilizar distintosmodelos de contacto como el modelo de Hertz o el rozamiento de Stribeck.

Para probar la estabilidad del controlador desarrollado se decide realizar pruebas en almenos dos motores de físicas distintos. ODE y Bullet son probados inicialmente, al ser losque vienen incluidos por defecto. Por desgracia, no se consiguen resultados satisfactorios conBullet ya que al arrancar la simulación con el robot Talos, el motor de físicas no consigueconverger por motivos desconocidos y el robot sigue movimientos erráticos. Se ha compiladopor tanto Gazebo para que incluya soporte para Dart, y se han realizado las pruebas tantoen ODE como en Dart, obteniendo resultados similares.

4 Desarrollo

4.1 DependenciasEl controlador implementado hace uso de tres bibliotecas para poder funcionar. Las pri-

meras dos se corresponden con el optimizador de programas cuadráticos, OSQP, y la última,llamada Pinocchio, es usada para calcular todas las magnitudes dinámicas y cinemáticasrequeridas por el controlador.

Ya se ha dado una introducción al funcionamiento del optimizador usado al final del apar-tado 2.1.1.3. OSQP1 es la implementación oficial del trabajo publicado por Stellato y cols.(2020). La biblioteca está escrita en el lenguaje C. Mientras que es compatible con C++,lenguaje usado en la implementación del controlador, la interoperabilidad no es óptima, yaque se requiere trabajar con la memoria manualmente, transformando los tipos de C++ enpunteros para que OSQP pueda trabajar con ellos. Como solución, se hace uso de otra bi-blioteca, llamada osqp-eigen2, que sirve de interfaz entre OSQP y C++ haciendo uso deEigen (Guennebaud y cols. (2010)), una biblioteca de álgebra lineal para C++ ampliamenteusada. Esto hace que osqp-eigen sea muy conveniente porque tanto ROS como Pinocchiohacen un uso extensivo de Eigen.

Por otro lado, Pinocchio (Carpentier y cols. (2019)) es una biblioteca que implementa losalgoritmos descritos por el libro Rigid Body Dynamics Algorithms (Featherstone (2014)). Enél, Featherstone presenta varios algoritmos cinemáticos y dinámicos pensados desde el inicioen su implementación en software. Una de sus aportaciones es el uso de la notación espacialde seis dimensiones, lo que agiliza los cálculos considerablemente.

Además, Pinocchio provee de forma automática las derivadas analíticas de todos sus algo-ritmos. Esto es de gran utilidad a la hora de desarrollar controladores avanzados, y ha sido larazón por la que se ha escogido a Pinocchio frente a otras bibliotecas similares, como RBDL.

Pinocchio trabaja separando de forma total los conceptos de modelo y datos (pinocchio::Modely pinocchio::Data, respectivamente). Los modelos encapsulan la descripción física del robot,es decir, los parámetros físicos de los eslabones y su relación entre ellos mediante distintostipos de articulaciones. Por el otro lado, los datos guardan los parámetros que son resultadode alguna operación: las posiciones y sus derivadas tanto de las articulaciones como de la basedel robot, la matriz de inercia en el espacio articular (M(q)), los efectos no lineales (N(q, q)),…La intención de esta separación es clara: teniendo únicamente una instancia del modelo, esposible trabajar sobre distintos robots al mismo tiempo, lo que aporta una gran eficiencia derecursos. Poniendo un ejemplo práctico, si se tuviera que calcular la cinemática directa dedos robots de la misma clase, se haría de la siguiente manera:

1https://osqp.org2https://github.com/robotology/osqp-eigen

41

42 Desarrollo

Código 4.1: Cálculo de la cinemática directa con Pinocchio.

1 // Se instancia un modelo y carga un robot descrito por su URDF2 pinocchio::Model model;3 // Carga un robot a partir de su urdf4 pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer, model);5 // Crea los datos para los dos robots6 pinocchio::Data data1(model), data2(model);7 // Define la configuración articular8 Eigen::VectorXd q = pinocchio::neutral(model); // Posiciones articulares9 Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); // Velocidades articulares

10 Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); // Aceleraciones articulares11 // Calcula la cinemática directa...12 pinocchio::forwardKinematics(model, data1, q, v, a); // ... del robot 113 pinocchio::forwardKinematics(model, data2, q, v, a); // ... del robot 2

Los comentarios del código indican lo que hace cada función, sin embargo cabe aclarar losiguiente:

• Cuando se crea el modelo, se indica que la base del robot no está fija al marco de referen-cia global y se puede mover libremente. Se hace indicando el parámetro pinocchio::JointModelFreeFlyer.Este es el caso de cualquier robot móvil, desde humanoides a cuadrúpedos o drones.Pinocchio modela la libertad de movimiento del robot mediante una articulación de seisgrados de libertad: tres de posición y tres de orientación. Esta articulación tiene comoeslabón padre al marco de referencia global y la base del robot como eslabón hijo.

• q, v y a son vectores que guardan las posiciones, velocidades y aceleraciones articu-lares. Como Pinocchio modela a los robots móviles con la articulación de seis gradosde libertad comentada en el punto anterior, los primeros valores de estos vectores secorresponden con los grados de libertad de la base del robot. Primero se incluyen las po-siciones cartesianas y seguidamente la orientación. Un caso especial es el de q, en el quela orientación está representada por un cuaternión y por tanto se usan cuatro números.La velocidad y aceleración angular en v y a queda representadas por las derivadas delos ángulos de Euler, y por tanto se representan con tres números. Esto hace que eltamaño de q sea nv + 1 donde nv es el número de grados de libertad, mientras que eltamaño de v y a coincide con el número de grados de libertad. Este es el motivo por elque se usa pinocchio::neutral, ya que inicializa el cuaternión para que la orientaciónsea la de reposo (todos los ángulos de Euler nulos).

• Los resultados de la cinemática directa quedan guardados en las estructuras de datosdata1 y data2. Si se necesitase calcular por ejemplo la velocidad de un marco dereferencia del robot, se puede hacer de la siguiente manera:

Código 4.2: Cálculo de la velocidad de un marco de referencia del robot 1.

1 // Consigue el id correspondiente del marco de referencia2 const int id = model.getFrameId(”left_sole_link”);3 // Calcula la velocidad del frame ’left_sole_link’4 const auto v = pinocchio::getFrameVelocity(model, data1, id,5 pinocchio::ReferenceFrame::WORLD);

El marco de referencia left_sole_link se corresponde con el que está definido en el cen-tro de la suela del pie izquierdo del robot Talos. La velocidad resultante queda expresada

4.2. Planificación de trayectorias 43

con respecto al marco de referencia global. Si se quisiera obtenerla con respecto al mar-co de referencia local, se puede indicar poniendo pinocchio::ReferenceFrame::LOCALen el último parámetro de pinocchio::getFrameVelocity. Pinocchio también ofrecela posibilidad de representar las cantidades calculadas con respecto a un marco de re-ferencia local, pero cuyos ejes están alineados con el marco de referencia global. Esto seindica con pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED.

Por último, Pinocchio ofrece una interfaz de Python que hace que sea sencillo el prototipadode nuevos algoritmos.

4.2 Planificación de trayectoriasSe usa towr para la generación de trayectorias. El primer paso es crear el modelo cine-

mático y dinámico de Talos para que towr pueda crear las trayectorias cartesianas. Se crea,dentro de la carpeta towr/include/towr/models/examples, un archivo de cabecera llamadotalos_model.h con el siguiente contenido:

Código 4.3: Definición del modelo cinemático y dinámico respectivamente en towr.

1 class TalosKinematicModel : public KinematicModel {2 public:3 TalosKinematicModel () : KinematicModel(2)4 {5 const double z_nominal_b = −0.9;6 const double y_nominal_b = 0.10;78 nominal_stance_.at(L) << 0.0, y_nominal_b, z_nominal_b;9 nominal_stance_.at(R) << 0.0, −y_nominal_b, z_nominal_b;

1011 max_dev_from_nominal_ << 0.18, 0.10, 0.060;12 }13 };1415 class TalosDynamicModel : public SingleRigidBodyDynamics {16 public:17 TalosDynamicModel()18 : SingleRigidBodyDynamics(93.3357,19 15.4261, 12.6152, 4.54076,20 −0.00456485, 1.27926, 0.0611941,21 2) {}22 };

El modelo cinemático creado es sencillo, simplemente se indica la posición nominal dela base del robot (0.9 metros sobre el suelo en el eje vertical) y la de los pies. Como sepuede observar, el modelo cinemático no tiene en cuenta los parámetros físicos del robot,simplemente se preocupa de las posiciones cartesianas de la base y de los efectores finales, taly como se expuso en el apartado 3.1.

Por otro lado, el modelo dinámico creado hereda de la clase SingleRigidBodyDynamics,que como su nombre indica implementa la dinámica del sólido rígido único (2.15 y 2.16).Como parámetros, se pasa la masa total del robot, que son 93.3357Kg, la matriz de inerciadel robot cuando todas las articulaciones están en la posición nominal y el número de efectoresfinales. En este caso, la posición nominal no coincide con el reposo de las articulaciones, yaque al estar la base del robot a 0.9 metros de altura, las rodillas del robot están flexionadasy se considera esta posición como la posición nominal.

44 Desarrollo

Para calcular la inercia del robot en esa configuración articular, se ha creado el siguientescript:

Código 4.4: Cálculo de la matriz de inercia para la posición nominal del robot.

1 import pinocchio2 import os3 import numpy45 # Recupera el archivo urdf del robot6 script_dir = os.path.dirname(os.path.realpath(__file__))7 pinocchio_model_dir = os.path.join(script_dir, ”../urdf”)8 urdf_filename = pinocchio_model_dir + ”/talos_full_legs_v2.urdf”9

10 # Carga el modelo del robot11 model = pinocchio.buildModelFromUrdf(urdf_filename, pinocchio.JointModelFreeFlyer())12 data = model.createData()13 print(’model name: ’ + model.name)1415 # Se inicializa la configuración inicial del robot16 q = pinocchio.neutral(model)17 q[7:] = numpy.ndarray((12,), \18 buffer=numpy.array([0.0, 0.028730, −0.706060, 1.477414, \19 −0.771353, −0.028730, 0.0, −0.028730, −0.706060, \20 1.477414, −0.771353, 0.028730]))21 v = pinocchio.utils.zero(model.nv)2223 # Ejecuta el algoritmo usado para calcular la matriz de inercia24 pinocchio.ccrba(model, data, q, v)2526 # Muestra la matriz de inercia27 print(’Inertia’)28 print(data.Ig)

Como se puede ver, se hace uso de Pinocchio para calcular la inercia. El algoritmo usadose llama CCRBA, cuyas siglas vienen del inglés: Centroidal Composite Rigid Body Inertia. Olo que es lo mismo, la matriz de inercia con respecto al centro de masas y que incluye a todoslos eslabones del sistema dinámico. Como resultado, se obtiene:

Ig =

15.4261 −0.00456485 1.27926−0.00456485 12.6152 0.0611941

1.27926 0.0611941 4.54076

(4.1)

que son los parámetros de inercia que se ponen en el modelo dinámico del robot en elcódigo 4.3.

Pero para poder calcular esta magnitud, ha sido necesario conocer de antemano las posicio-nes articulares de las piernas del robot. No sólo es vital poder calcular la cinemática inversapara la obtención de Ig, sino que es también necesaria para poder generar las trayectoriasarticulares a seguir por el controlador a partir de las trayectorias cartesianas obtenidas contowr.

Se ha creado el paquete xpp_talos, el cual contiene la implementación de la cinemáticainversa de las piernas de Talos, la visualización de las trayectorias generadas con el modelocompleto del robot y la generación de mensajes para ser enviados al controlador de cuerpocompleto.

Se hace uso de una utilidad llamada IKFast, que es parte del proyecto OpenRAVE (Diankovy Kuffner (2008)), para hacer el cálculo de la cinemática inversa. OpenRAVE es un conjunto de

4.2. Planificación de trayectorias 45

herramientas que permite trabajar en el ámbito de planificación de trayectorias para generarmovimientos complejos en sistemas robóticos. Pero en este trabajo no se hace uso más que deIKFast, que es un script de Python capaz de generar código C++ para calcular la cinemáticainversa analítica. Esto es posible gracias a que los tres primeros ejes de las piernas de Taloscoinciden en un punto. A continuación se describen los pasos seguidos para la obtención dela expresión de la cinemática inversa analítica:

1. Se instala OpenRAVE siguiendo los pasos de la página oficial.

2. OpenRAVE trabaja con el formato COLLADA para representar a los robots, por lo quees necesario convertir el URDF del robot a formato .dae. Esto se puede conseguir conel paquete de ROS collada-urdf-tools, ejecutando el siguiente comando:

$ roscore && rosun collada_urdf urdf_to_collada robot.urdf robot.dae

Se puede comprobar que el .dae creado es válido cargándolo en cualquier visualizador3D, como puede ser el incluido con OpenRAVE u otro software de gráficos por ordenadorcomo por ejemplo Blender.

3. Se tiene que obtener el id de los marcos de referencia entre los que se quiere calcular lacinemática inversa. En este caso, se quiere calcular la cinemática inversa de dos cade-nas cinemáticas que van desde base_link hasta left_sole_link y right_sole_linkrespectivamente. Para mirar los id de cada marco de referencia se usa el comando:

$ openrave−robot.py robot.dae −−info links

4. Finalmente, para obtener los archivos de código fuente C++ que implementa la cine-mática inversa analítica, es necesario ejecutar:

python $(openrave−config −−python−dir)/openravepy/_openravepy_/ikfast.py \−−robot=robot.dae −−iktype=transform6d −−baselink=$(id_base) −−eelink=$(id_efector) \−−savefile=ik.cc

Esto guardará el código creado en el archivo ik.cc.

5. Como cada pierna tiene seis grados de libertad, existen múltiples soluciones a la ci-nemática inversa. De entre todas, se escoge la que tiene más sentido para la tarea decaminar, esto es, con la cadera sin girar y con las rodillas doblándose hacia delante.

En el último paso se puede ver una de las grandes ventajas de trabajar con las expresionesanalíticas de la cinemática inversa. Al contrario que en los métodos iterativos para calcularla,aquí se tiene certeza de cuál va a ser la solución obtenida. Otra ventaja es que al tener laexpresión matemática, la cinemática inversa es computada muy eficientemente. Tanto, quesupera a los métodos iterativos por varios órdenes de magnitud, llegando a velocidades de 4microsegundos3.

Habiendo obtenido la cinemática inversa, ya se puede hacer el código necesario para incluiral robot Talos en la visualización de las trayectorias generadas.

3Tal y como se expresa en http://openrave.org/docs/latest_stable/openravepy/ikfast/.

46 Desarrollo

xpp4 es un paquete creado por el mismo autor de towr que sirve para visualizar las tra-yectorias generadas. xpp hace uso de RViZ para mostrar todos los datos y se incluyen losmismos robots de ejemplo que en towr, de manera que se pueden visualizar sus trayectorias.Desafortunadamente, todos los robots de ejemplo tienen las patas del robot cuadrúpedo HyQ,que son de tres grados de libertad. Las piernas de Talos tiene seis grados de libertad, porlo que el método de calcular la cinemática inversa no es igual: para las piernas de Talos esnecesario saber la posición y orientación del efector final, mientras que para las piernas delHyQ sólamente hace falta especificar la posición.

Se ha creado la clase InverseKinematicsTalos, que encapsula el cálculo de la cinemáticainversa para las dos piernas del robot, ofreciendo una interfaz fácil de usar para el programa-dor. Además, esta clase es capaz de calcular la velocidad y aceleración articulares del robot.Se obtienen mediante el siguiente cálculo:

q = J−1x (4.2)q = J−1(x− J q) (4.3)

Finalmente, para la generación de mensajes para el controlador se usa el nodo talos_contact_traj.Este nodo hace uso de Pincochio y InverseKinematicsTalos para generar las trayectoriasarticulares, del centro de masas y de los contactos. Se usa de la siguiente manera:

$ rosrun xpp_talos talos_contact_traj

talos_contact_traj crea pro defecto un mensaje que contiene la última trayectoria gene-rada por towr, que se guarda en la ruta ~/.ros/towr_trajectory.bag. Si se quisiera crearel mensaje a partir de otra trayectoria diferente, es posible hacerlo indicando la ruta comoparámetro.

4.3 Implementación del controlador4.3.1 Implementación y uso

Desde el comienzo se han tenido en mente los siguientes principios a la hora de diseñar elcontrolador de cuerpo completo:

• El controlador tiene que ser una biblioteca, que se pueda incluir en cualquier proyectofácilmente.

• Debe tener el mínimo necesario de dependencias. Solamente se usan las bibliotecasexpuestas en el apartado 4.1, por lo que el controlador no depende de ROS y se puedeusar en cualquier entorno.

• El controlador es agnóstico al robot que se desea controlar. Aunque los esfuerzos delpresente trabajo se centran en seguir las trayectorias generadas para Talos, ningunaparte del controlador se ha diseñado en específico para este robot y debería de poderser usado en cualquier robot móvil con patas.

4https://github.com/leggedrobotics/xpp.

4.3. Implementación del controlador 47

El objetivo del controlador no es el de seguir la trayectoria, sino que dado el estado actualdel robot y el estado deseado, éste calcula las señales de control óptimas (torques) a enviar asus actuadores en ese instante de tiempo. Es decir, el controlador crea la formulación descritaen las expresiones 3.43 y 3.44 y la resuelve. La abstracción de la formulación se hace medianteuna clase llamada QpFormulation y que está definida e implementada en los archivos llamadosqp_formulation.hpp y qp_formulation.cc respectivamente.

El constructor de la clase recibe un único argumento que indica la ruta del modelo URDFdel robot con el que se quiere trabajar. A partir del URDF, se crean los contenedores del mo-delo y los datos de Pinocchio, se inicializa el optimizador y las variables necesarias (la matrizde selección, los torques máximos permitidos umax, las posiciones y velocidades articulares,…).

El siguiente paso es indicarle al controlador qué conjunto de tareas van a ser activadasy por tanto integradas en la función objetivo. Por ejemplo, si se quisiera usar la tarea deseguimiento de la postura y del seguimiento del centro de masas, se hace de la siguientemanera:

Código 4.5: Activación de las tareas del seguimiento de la postura y centro de masas.

1 // Instancia la clase del optimizador2 const std::string ruta_urdf = ”/ruta/a/archivo.urdf”;3 std::shared_ptr<QpFormulation> solver;4 solver.reset(new QpFormulation(ruta_urdf));5 // Se indican las tareas a activar6 solver−>PushTask(QpFormulation::TaskName::FOLLOW_JOINT);7 solver−>PushTask(QpFormulation::TaskName::FOLLOW_COM);

Se puede ver que el método PushTask sirve para añadir una tarea al conjunto de tareas acti-vas. Todas las tareas implementadas están enumeradas en QpFormulation::TaskName. Es po-sible eliminar todas las tareas activadas con el método QpFormulation::ClearTasks(void).

El siguiente paso es definir los parámetros correspondientes a todas las tareas activadas.Estos parámetros son el peso de la tarea wi, donde la suma de todos los pesos tiene que serigual a 1:

∑iwi = 1, y las constantes que definen la dinámica de segundo orden de la tarea:

Kpi y Kvi .

Código 4.6: Inicialización de los pesos y constantes de la dinámica de las tareas.

1 // Se asigna el valor a los pesos de las tareas2 solver−>SetTaskWeight(QpFormulation::TaskName::FOLLOW_JOINT, 0.3);3 solver−>SetTaskWeight(QpFormulation::TaskName::FOLLOW_COM, 0.7);4 // Se asignan los valores de las constantes de la dinámica de la tarea5 solver−>SetTaskDynamics(QpFormulation::TaskName::FOLLOW_JOINT, 20000.0, 200.0);6 solver−>SetTaskDynamics(QpFormulation::TaskName::FOLLOW_COM, 20000.0, 200.0);

También es posible inicializar la dinámica de la tarea a la vez que se activa, mediante eluso del método sobrecargado:

Código 4.7: Método sobrecargado para la inicialización de la dinámica de la tarea a la vez quese activa.

1 void QpFormulation::PushTask(const QpFormulation::TaskName task, const double kp, const double kv);

Por defecto, todos los contactos son modelados como si fueran puntos de contacto. Peroen el caso de Talos, el contacto de cada pie es mediante de la superficie de la suela. Paramodelar de manera cómoda contactos a través de superficies, se introduce el concepto defamilia de contactos en QpFormulation. Una familia de contactos es una manera de agrupar

48 Desarrollo

una serie de puntos de contacto bajo el mismo nombre. Todos los puntos de la familia decontactos comparten la misma orientación. La idea está en que el contacto de una superficiepuede ser aproximada mediante los contacto de los puntos de los vértices de la superficie,si su superficie es convexa. Aunque da al optimizador el permiso para encontrar solucionesfísicamente no correctas, como por ejemplo que las fuerzas de repulsión de cada punto vayanen sentidos opuestos, es una aproximación razonable y usada en la práctica que da buenosresultados (Bouyarmane y cols. (2019); Ott y cols. (2011)).

La clase QpFormulation recupera los puntos de contacto a través de los marcos de referenciadefinidos en el URDF del robot. En el URDF del robot Talos únicamente se define el marcode referencia que está en el centro de la superficie de contacto de cada pie. Por lo que seha tenido que añadir, modificando el URDF, marcos de referencia en las esquinas de lasuperficie de la suela de los pies. Para validar que los marcos de referencia están en susposiciones correspondientes, se hace uso del paquete de ROS collada-urdf para convertir elarchivo URDF a formato Collada (.dae) y poder ver visualmente el robot con Blender, unprograma de modelado y animación 3D. En la Figura 4.1 se pueden ver las posiciones de losmarcos de referencia que definen la superficie de cada contacto.

Figura 4.1: Visualización de los vértices que conforman las superficies de contacto de los pies delrobot Talos.

Para indicar la existencia de una familia de contactos a la clase QpFormulation, se tieneque hacer uso del método QpFormulation::SetContactFamily, tal y como se muestra en elcódigo 4.8.

Código 4.8: Se indican las familias de contactos del robot a usar.

1 // El primer parámetro es el nombre de la familia de contactos2 // El segundo parámetro es una lista con todos los marcos de referencia que pertenecen a la familia.

4.3. Implementación del controlador 49

3 // Los marcos de referencia tienen que estar definidos en el URDF.4 solver−>SetContactFamily(”left_sole_link”,5 {”left_sole_front_left_link”, ”left_sole_front_right_link”,6 ”left_sole_back_left_link”, ”left_sole_back_right_link”});7 solver−>SetContactFamily(”right_sole_link”,8 {”right_sole_front_left_link”, ”right_sole_front_right_link”,9 ”right_sole_back_left_link”, ”right_sole_back_right_link”});

El último paso para preparar el controlador es activar las restricciones. Esto se realiza de for-ma similar a la activación de las tareas: la clase guarda un listado con todas las restricciones ac-tivas, a la cual se le pueden añadir elementos con el método QpFormulation::PushConstrainty se puede limpiar con QpFormulation::ClearConstraints. Además, todas las restriccionesestán enumeradas en QpFormulation::ConstraintName.

Código 4.9: Se activan todas las restricciones implementadas.

1 // Primero se borran todas las restricciones previamente activas2 solver−>ClearConstraints();3 solver−>PushConstraint(QpFormulation::ConstraintName::EQUATION_OF_MOTION);4 solver−>PushConstraint(QpFormulation::ConstraintName::FIXED_CONTACT_CONDITION);5 solver−>PushConstraint(QpFormulation::ConstraintName::ACTUATION_LIMITS);6 solver−>PushConstraint(QpFormulation::ConstraintName::CONTACT_STABILITY);

Todo el código explicado hasta el momento sólo es necesario ejecutarlo una sola vez yantes de comenzar el control, ya que la clase QpFormulation se encarga de guardar todosestos parámetros. Sin embargo, es posible cambiar cualquiera de los parámetros explicadosen tiempo real, tal y como se hará en el apartado 4.3.2.

Una vez se ha obtenido el estado actual del robot, así como las consignas de los estadosdeseados de cada tarea, se puede obtener señal de control óptima con respecto a los parámetrosdel controlador de la siguiente manera:

Código 4.10: Obtener la solución al problema de control.

1 // Se obtiene el estado actual del robot2 solver−>SetRobotState(base_pos, base_vel, q, qd,3 contact_names, contact_orientations);4 // Se guardan los errores y consignas de las tareas5 solver−>SetPositionErrors(joint_pos_errors); // Error en posición articular6 solver−>SetVelocityErrors(joint_vel_errors); // Error en velocidad articular7 solver−>SetReferenceAccelerations(joint_des_accel); // Consigna de aceleración articular8 solver−>SetDesiredCoM(com_pos, com_vel); // Posición y velocidad deseadas del centro de masas9 // Construye el problema y lo soluciona

10 solver−>BuildProblem();11 solver−>SolveProblem();12 // Recupera el vector solución13 Eigen::VectorXd sol = solver−>GetSolution();

A continuación se explica el código 4.10 paso a paso:

• Lo primero que se tiene que hacer es especificar el estado actual del robot. Esto seconsigue con el método QpFormulation::SetRobotState, el cual tiene los siguientesparámetros (en orden):

– base_pos: es la posición y orientación de la base del robot con respecto al marcode referencia global. La base se refiere al eslabón que no tiene padre, o cuyo padrees el marco de referencia global. La orientación se representa por un cuaternión,por lo que base_pos es un vector de siete números reales (tres para la posición ycuatro para la orientación).

50 Desarrollo

– base_vel: es la velocidad cartesiana y angular de la base del robot, expresados enel marco de referencia de la base del robot.

– q y qd: son las posiciones y velocidades articulares respectivamente.– contact_names: es un listado con todos los contactos que están activos en ese

instante. Si uno de los nombres del listado se corresponde con una familia decontactos, todos los contactos de la familia se activan. De lo contrario, se busca elnombre del marco de referencia en el modelo del URDF y si está se añade a loscontactos activos como un contacto puntual.

– contact_orientations: es un vector que guarda una matriz de rotación paracada elemento de contact_names. Este argumento se puede no especificar, lo queresulta en que todos los contactos por defecto son modelados con respecto a unasuperficie horizontal en el marco de referencia global.

A partir de las cantidades anteriores, se calculan todas las magnitudes necesarias parapoder formular el problema. Esto incluye: la matriz de inercia en el espacio articularM(q), los efectos no lineales N(q, q), la jacobiana de todos los contactos J y la posición,velocidad, jacobiana y el término Jτ q de todas las tareas implementadas (como porejemplo la del centro de masas). El término Jτ q se obtiene usando el método explicadoen el apartado 3.2.1, ya que es una aproximación considerablemente más eficiente acalcular la derivada del jacobiano explícitamente.

• Se indican las consignas de cada tarea. En este caso se tienen activadas las tareas de pos-tura y de seguimiento del centro de masas, por lo que se indican sus estados deseados.Para la tarea de la postura, esto se consigue con QpFormulation::SetPositionErrors,QpFormulation::SetVelocityErrors y QpFormulation::SetReferenceAccelerations,cuyos nombres son autodescriptivos. Para la tarea del centro de masas, esto se consi-gue con QpFormulation::SetDesiredCoM, que recibe como parámetros dos vectorestridimensionales con la posición y velocidad deseada del centro de masas.

• Una vez ya se tienen todos los datos para poder construir el problema según las expre-siones 3.43 y 3.44, se llama al método QpFormulation::BuildProblem. Éste se encargade construir todas las matrices del programa cuadrático a partir del estado del robot ylas consignas de las tareas. Las matrices quedan guardadas dentro de la propia clase.

• Lo único que queda es ejecutar el optimizador sobre el problema creado y obtener lasolución. Para esto sirve el método QpFormulation::SolveProblem. Por defecto, eloptimizador usa un arranque en caliente para acelerar considerablemente la búsquedade la solución óptima. Esta aceleración se debe a que entre dos instantes contiguos elproblema resulta ser muy similar, ya que las trayectorias a seguir son continuas y en unintervalo tan pequeño de tiempo el estado del robot no puede variar mucho. Sin embargo,no es posible usar un arranque en caliente cuando el número de variables cambia. Estosucede cuando el número de eslabones que están en contacto con el exterior cambia, loque hace que el número de fuerzas externas varíe acordemente y por tanto lo haga eltamaño del vector [q f u]. QpFormulation::SolveProblem reinicializa el optimizadorautomáticamente en caso de que esto pase y resuelve el problema desde cero. Cómo sepuede ver en el código 4.10, el programador es ajeno a todo esto y simplemente tieneque centrarse en activar las tareas y restricciones y ajustar los parámetros.

4.3. Implementación del controlador 51

• El vector que se tiene como solución es [q f u]. Si las articulaciones del robot tienenla opción de ser controladas por torque, se puede aplicar u directamente sin tener quehacer ningún procesado adicional. Sin embargo, en caso de tener un control por posiciónes necesario coger q y pasarlo por un integrador. En caso del robot Talos, todas susarticulaciones tienen control por torque por lo que se usa la primera opción.

4.3.2 Integración con ROS

PAL Robotics5 ofrece un entorno de desarrollo gratuito y de código abierto basado en ROS6

para el robot Talos. Por tanto, para poder hacer funcionar el controlador de cuerpo completoes necesario integrar la biblioteca creada en el apartado 4.3.1 en ROS.

Para el control de los actuadores de un robot, ROS provee una serie de bibliotecas cu-yo objetivo es unificar la programación de los controladores para que se pueda reciclar elcódigo lo máximo posible entre distintos robots. Estas bibliotecas están bajo el proyecto deros_control (Chitta y cols. (2017)), que surge como una refactorización de los controladoresescritos originalmente para el robot PR2.

En la Figura 4.2 se puede ver como se relacionan las distintas partes de ros_control entresí. A la izquierda del todo, se reciben los mensajes con las consignas. Estas consignas puedenser de posición, velocidad, torque o incluso trayectorias enteras. Cada uno de estos mensajesva dirigido a un controlador en particular. Todos los controladores son gestionados por elController Manager, el cual se encarga de crearlos, inicializarlos, ejecutarlos en tiempo realy pararlos. Además, todos los controladores se actualizan a la vez, por lo que comparten lamisma frecuencia y no es posible tener varios controladores con distintas frecuencias.

Cada controlador tiene acceso al estado de las articulaciones que controla mediante elJoint State Interface, y puede leer tanto la posición como la velocidad articular. Con estainformación y la consigna recibida a través de los mensajes, se calcula un error y se aplica la leyde control (por ejemplo, un simple PID). El control realizado aquí es de alto nivel, ya que trasenviar el comando mediante el Joint Command Interface, éste es alimentado en un controladorde bajo nivel que produce los torques que finalmente se enviarán a las articulaciones. Estecontrol de bajo nivel se haya en el HadrwareInterface. El HardwareInterface es una clasecon varias especializaciones de plantilla, dependiendo de los actuadores del robot:

• PositionJointInterface: para controladores de bajo nivel en posición.

• VelocityJointInterface: para controladores de bajo nivel en velocidad.

• EffortJointInterface: para controladores de bajo nivel en fuerza.

• PosVelJointInterface: para controladores de bajo nivel que reciban la posición, ve-locidad y aceleración.

• PosVelAccJointInterface: para controladores de bajo nivel que reciban la posición,velocidad y aceleración.

5https://pal-robotics.com/6http://wiki.ros.org/Robots/TALOS/Tutorials/Installation/Simulation

52 Desarrollo

Figura 4.2: Esquema de funcionamiento de ros_control (Fuente: http://wiki.ros.org/ros_control).

4.3. Implementación del controlador 53

Para el presente trabajo, es necesario seguir las trayectorias generadas por towr en la etapade planificación de los movimientos, es por esto que parece razonable escoger el controla-dor de tipo JointTrajectoryController para hacer esta tarea. Este controlador no recibeuna única consigna para cada articulación, sino que recibe un mensaje con una trayectoria(secuencia de puntos) en posición, velocidad y aceleración para todas las articulaciones. Sinembargo, no es suficiente con la funcionalidad provista por este controlador, ya que el con-trolador de cuerpo completo requiere más información: la trayectoria del centro de masas yla secuencia de contactos.

Es por esto que se ha decidido crear una nueva clase de controlador que soporte estasfuncionalidades. Se ha partido del código original de JointTrajectoryController, a los quese le han hecho los siguientes cambios:

• Se ha creado un nuevo tipo de mensaje en el que se puede especificar las trayectoriasarticulares y del centro de masas, así como la secuencia de los contactos. Este nuevomensaje se llama JointContactTrajectory.msg.

• Para poder procesar la secuencia de contactos se han creado las siguientes cabeceras:– contact_segment.hpp: define la clase que representa un segmento de la trayectoria

de los contactos. Sólamente guarda si un contacto está activo o no, y el tiempoque le corresponde al segmento.

– contact_joint_trajectory_segment.hpp: es una copia de joint_trajectory_segment.hppperteneciente al controlador de trayectorias original de ROS a la que se le han he-cho los cambios necesarios para que funcione con el nuevo mensaje del controlador.

– init_contact_joint_trajectory.hpp: en esta cabecera se define la funcionali-dad necesaria para convertir el mensaje de trayectoria recibido a través del topiccorrespondiente del controlador a la representación interna de las trayectorias porel controlador. Esta representación interna consiste en una secuencia de splinesquínticos para todos los componentes de todas las trayectorias (menos la trayec-toria de los contactos que es una secuencia de valores booleanos). Gracias a esto,el controlador de trayectorias puede coger una muestra en cualquier instante detiempo, ya que se hará una interpolación.

– Se crea la acción de ROS FollowContactJointTrajectory.action a partir dela acción FollowJointTrajectory.action del controlador original. En el nuevoarchivo creado se realizan los cambios necesarios para que la acción funcione conel nuevo mensaje de trayectorias.

• Se ha creado la clase JointTrajectoryWholeBodyController a partir del código deJointTrajectoryController. Esta clase soporta el uso de trayectorias articulares, delcentro de masas y de contactos. En su interior, se instancia la clase QpFormulation. Encada instante de tiempo, se recupera el estado de las articulaciones del robot (posicionesy velocidad)y se obtienen las consignas de todas las trayectorias. Con esta información,se puede crear el programa cuadrático dentro del objeto QpFormulation, resolverlo yobtener las señales de control óptimas.Además, esta clase hace uso de ddynamic_reconfigure, un paquete de ROS que per-mite exponer ciertos parámetros para que sean cambiados en tiempo real mediante

54 Desarrollo

la interfaz gráfica proporcionada por rqt_reconfigure. Se puede modificar tanto convalores numéricos como de manera visual mediante deslizadores los valores de los pa-rámetros. Se exponen tanto los pesos como la dinámica de cada tarea. Además, esposible activar y desactivar en tiempo real todas las restricciones de la formulación.Esta interfaz se puede ver en la Figura 4.3.

• La interfaz de hardware de ROS que se usa para el control de torque tiene en su interiorun PID para calcular el torque a partir de los errores de posición, velocidad y aceleración.Sin embargo, ya se obtienen los torques directamente al resolver el programa cuadrático.Por lo que se ha tenido que crear una nueva interfaz de hardware que permita indicar lostorques a aplicar directamente sin que se haga ningún tipo de procesado de bajo nivel.Esta interfaz está definida en el archivo hardware_interface_direct_effort.hpp.

Figura 4.3: Interfaz para modificar todos los parámetros del controlador en tiempo real.

Se ha integrado todo lo expuesto anteriormente en un paquete de ROS llamado talos_wbc_controller7.De esta manera es posible instalar y usar el controlador simplemente instalando el paqueteen el entorno de trabajo de ROS. Además, se incluye OSQP y osqp-eigen como proyec-tos CMake aparte, para que no se tenga que realizar la instalación de manera manual yel sistema de compilación y enlazado de ROS se encargue de resolver las dependencias detalos_wbc_controller.

Si se quiere lanzar el controlador para que este tome el control de las piernas del robot,basta con ejecutar los siguientes comandos:

Código 4.11: Lanza la simulación y carga el controlador

1 $ roslaunch talos_gazebo talos_gazebo.launch # Lanza la simulación en Gazebo2 $ roslaunch talos_wbc_controller talos_trajectory_wbc_controller.launch # Lanza el controlador3 $ rosrun rqt_reconfigure rqt_reconfigure # Interfaz para ajustar los parámetros

7https://github.com/noctrog/talos_wbc_controller

5 Resultados

5.1 Resultados de la planificaciónSe realizan varios experimentos para la evaluación de la planificación de trayectorias. Pri-

mero, se generarán trayectorias para dar dos pasos, manteniendo constante la orientación dela base, en las ocho direcciones (adelante, atrás, hacia los lados y en diagonal). Todas lastrayectorias tienen una duración de 2.4s. En la Tabla 5.1 se muestra por cada instancia elcoste temporal y el número de evaluaciones de la función objetivo, que está relacionado conla dificultad del problema de optimización. En la Figura 5.1 se muestran varios fotogramasde la visualización de la trayectoria de los casos 1, 2 y 3 de la Tabla 5.1. En las Figuras 5.2,5.2 y 5.3 se muestra mediante gráficas varias de las magnitudes de estas mismas trayectorias.

N Dirección Destino (x,y) m Tiempo (s) Evaluaciones de la función objetivo1 N (0.10, 0.0) 3.17 4022 NO (0.10, 0.10) 4.232 5943 O (0.0, 0.10) 2.428 2914 SO (-0.10, 0.10) 0.880 695 S (-0.10, 0.0) 2.116 2006 SE (-0.10, -0.10) 7.032 11267 E (0.0, -0.10) 4.856 6418 NE (0.10, -0.10) 0.164 9

Tabla 5.1: Tiempo y evaluaciones de la función objetivo requeridos por cada instancia del problemade planificación. Las columna ’Dirección’ indica de manera intuitiva la orientación del pasoa dar por el robot (N y S significan eje X positivo y negativo respectivamente, mientrasque O-E significa eje Y positivo y negativo).

Se puede observar que entre las distintas instancias de los problemas de planificación lasdiscrepancias en el coste computacional son muy altas. En las dos métricas utilizadas parala evaluación del coste del algoritmo se observa una diferencia de dos órdenes de magnituden el peor de los casos. Esto es llamativo teniendo en cuenta que todos los problemas sonsimilares. De hecho, los problemas 4 y 6 de la Tabla 5.1 son simétricos: en ambos se da un pasohacia atrás en diagonal en direcciones opuestas. Sin embargo, la complejidad del problema deoptimización cambia radicalmente. Esto se explica con lo expuesto en el apartado 2.1.2, yaque IPOPT es un optimizador local y por tanto carece de garantías de convergencia ademásde depender fuertemente de los parámetros del problema.

Una posible solución sería probar de manera empírica con otros optimizadores no linealescomo SNOPT, o reformular la planificación como un problema convexo usando un modelodinámico más simplificado aún.

Se concluye también que no es posible usar el planificador dentro de un bucle de control

55

56 Resultados

(a) Paso hacia el frente.

(b) Paso en diagonal.

(c) Paso hacia el lado izquierdo.

Figura 5.1: Trayectorias generadas para los casos 1 (a), 2 (b) y 3 (c) de la Tabla 5.1.

5.1. Resultados de la planificación 57

Figura 5.2: Visualización de varias magnitudes de la trayectoria 1 de la Tabla 5.1. De izquierda aderecha y de arriba a abajo: trayectoria cartesiana de la base del robot sobre el planoXY, evolución en el tiempo de las velocidades de la base, trayectoria cartesiana de lospies del robot sobre el plano XY, posición en el eje Z de los efectores finales con respectoal tiempo, posiciones X e Y del pie izquierdo con respecto el tiempo, evolución de lasfuerzas de contacto sobre el pie izquierdo, posiciones X e Y del pie derecho con respectoel tiempo y evolución de las fuerzas de contacto sobre el pie derecho.

58 Resultados

Figura 5.3: Visualización de varias magnitudes de la trayectoria 2 de la Tabla 5.1. De izquierda aderecha y de arriba a abajo: trayectoria cartesiana de la base del robot sobre el planoXY, evolución en el tiempo de las velocidades de la base, trayectoria cartesiana de lospies del robot sobre el plano XY, posición en el eje Z de los efectores finales con respectoal tiempo, posiciones X e Y del pie izquierdo con respecto el tiempo, evolución de lasfuerzas de contacto sobre el pie izquierdo, posiciones X e Y del pie derecho con respectoel tiempo y evolución de las fuerzas de contacto sobre el pie derecho.

5.1. Resultados de la planificación 59

Figura 5.4: Visualización de varias magnitudes de la trayectoria 3 de la Tabla 5.1. De izquierda aderecha y de arriba a abajo: trayectoria cartesiana de la base del robot sobre el planoXY, evolución en el tiempo de las velocidades de la base, trayectoria cartesiana de lospies del robot sobre el plano XY, posición en el eje Z de los efectores finales con respectoal tiempo, posiciones X e Y del pie izquierdo con respecto el tiempo, evolución de lasfuerzas de contacto sobre el pie izquierdo, posiciones X e Y del pie derecho con respectoel tiempo y evolución de las fuerzas de contacto sobre el pie derecho.

60 Resultados

predictivo por modelo, ya que los imprevisibles altos tiempos para encontrar una solución,así como la incertidumbre de encontrar una solución no lo hacen factible para tal aplicación.

Pero con towr se pueden generar movimientos más complejos y prolongados en el tiempo,aunque resulte en un mayor coste computacional. Esto incrementa considerablemente la di-ficultad del problema de optimización no lineal generado, lo que lleva a que no se llegue aresolver dentro de un tiempo razonable si no se realizan algunas simplificaciones.

Para ejemplificar una trayectoria más larga, se incrementa el número de pasos a ocho, eltiempo total de la trayectoria a 2.80s y la distancia del robot al objetivo 0.90m en línea recta.Se sustituye el coste de la velocidad en el eje Z de la base del robot por una restricción ensu posición vertical. Esto hace que el problema no contenga ninguna función objetivo, porlo que no hay que realizar ninguna minimización y en cuanto se encuentre una solución quecumpla con todas las restricciones se dará por buena. El tiempo requerido para encontraruna solución es de 0.588s y se evalúa un total de 22 veces la función objetivo. El resultado sepuede ver en la Figura 5.5.

Figura 5.5: Trayectoria larga de ocho pasos.

Al incrementar el tiempo y número de pasos que se dan, se está dotando de más libertad aloptimizador para encontrar posibles trayectorias. Al carecer de funciones objetivo, es posibleque las trayectorias generadas no sean todo lo suaves que se desease, lo que no las hace aptaspara su reproducción en un robot real.

Además, la rápida velocidad de la trayectoria generada no sería factible o sería muy difícilde seguir en el robot real, ya que cabe recordar que en la planificación se está asumiendo unpeso nulo de las extremidades inferiores.

5.2 Comportamiento del controladorLa evaluación del controlador se lleva a cabo mediante distintas pruebas que evalúan su

comportamiento con respecto a diferentes métricas.

• Cambio de postura: al iniciar el controlador como se indica en el Código 4.11, éstelleva al robot desde su postura por defecto de la simulación a la postura inicial delas planificaciones generadas por towr. Para ello, crea un spline multidimensional paracada articulación y para el centro de masas cuyos extremos son ambas posiciones.En la Figura 5.6 se puede observar una secuencia con el movimiento realizado por elcontrolador siguiendo esta trayectoria. Se puede ver que la completa con éxito y ademásmantiene el equilibrio al acabar de ejecutarla.

5.2. Comportamiento del controlador 61

Figura 5.6: Controlador llevando al robot a la posición inicial de las trayectorias.

• Equilibrio: se deja al robot en la postura de reposo (la postura final de la Figura 5.6)durante un largo período de tiempo. Sin el controlador activado, el robot en esta posturase acaba cayendo en la simulación cuando se usa el motor de físicas Dart debido a loserrores propios del simulador. Sin embargo cuando se activa el controlador, el robotpuede estar manteniendo la postura indefinidamente, por lo que se llega a la conclusiónde que el controlador está compensando por los errores del simulador. En las pruebasrealizadas se ha dejado al robot en la postura de reposo durante un período superior aseis horas.Por otra parte, el robot es capaz de recuperarse de pequeñas perturbaciones sin perder elequilibrio. Para demostrarlo se ha realizado un experimento donde se aplica un impulsode 12.000 N de fuerza durante un milisegundo sobre el eslabón base del robot y ensentido positivo del eje X. El robot, tras unas oscilaciones, es capaz de volver a laposición inicial. En la Figura 5.7 se puede visualizar la evolución del centro de masasen el eje X después del impulso.

Figura 5.7: Movimiento del componente X del centro de masas tras la aplicación de la perturbación.

62 Resultados

• Seguimiento del centro de masas: para corroborar el correcto funcionamiento de la tareadel seguimiento del centro de masas, se aplica una oscilación de manera arbitraria en eleje Z de la posición deseada del centro de masas sin hacer ningún cambio en las consignasarticulares. Si el controlador funcionase correctamente, se podrá observar una pequeñaoscilación en el eje Z del centro de masas real. En la Figura 5.8 se puede observar queesto es exactamente lo que sucede. En la Figura 5.9 se muestran las señales de controlgeneradas por el controlador.

Figura 5.8: Visualización de la consigna del eje Z del centro de masas (rojo) y su valor real (azul).

• Seguimiento de la orientación deseada: de manera similar al experimento con el centrode masas, se envía una consigna al controlador para seguir en el eslabón de la base delrobot una orientación oscilatoria en el eje X. En la Figura 5.10 se muestra el seguimientode la orientación deseada, y en la Figura 5.11 se muestran las señales generadas por elcontrolador.

• Seguimiento de trayectorias: se pretende seguir una trayectoria generada por towr en laque se dan un par de pasos. En la Figura 5.12 se puede ver una secuencia del resultado.Se aprecia que el robot no es capaz de seguir la trayectoria de manera adecuada yacaba cayendo. Varias son las posibles causas de este resultado, siendo la principal laincapacidad del controlador de mantener la orientación del cuerpo del robot a pesar dela existencia de una tarea que se encargue específicamente de esto. El pie izquierdo portanto no queda bien apoyado cuando se termina el primer paso. El controlador siguecon la trayectoria, activando la restricción de movimiento del pie izquierdo aunque esteno esté bien colocado en el suelo, y al realizar el siguiente paso se acaba cayendo.

5.2. Comportamiento del controlador 63

Figura 5.9: Torques generados por el controlador de cuerpo completo para seguir una oscilación enel eje vertical del centro de masas del robot.

Figura 5.10: Respuesta del controlador con respecto a la tarea de orientación. En rojo, la consignade orientación de la base sobre el eje X, y en azul, la orientación real de la base delrobot. El alto error en posición se debe a que las tareas de la postura y de centro demasas están activas y tienen mayor prioridad.

64 Resultados

Figura 5.11: Torques generados por el controlador de cuerpo completo para seguir una oscilación dela orientación de la base sobre el eje X.

Figura 5.12: Controlador intentando seguir una trayectoria con dos pasos hacia el frente.

6 Conclusiones

En este apartado se discuten los resultados presentados en este trabajo y posteriormentese exponen varias líneas en las que podrían ir encaminados trabajos futuros.

En el presente trabajo se ha añadido al robot Talos dentro del framework de optimizaciónde trayectorias conocido como towr. Para poder generar trayectorias con un reducido nivel deoscilaciones y mayor realismo, se han introducido ciertas restricciones y costes que limitan elmovimiento de la base del robot. Además, se ha obtenido la cinemática inversa analítica delas piernas del robot para que los resultados sean coherentes, no dependan de la inicializacióny su cálculo sea más eficiente. Como resultado se pueden obtener trayectorias de andarescortas en tiempos razonables. Si se desactivan los costes es posible obtener trayectorias máslargas a costa la mayor posibilidad de no obtener movimientos suaves en la base del robot.

Para poder seguir las trayectorias generadas, se ha implementado desde cero un controla-dor de cuerpo completo. Este controlador trabaja en el espacio de tareas. Estas tareas sondefinidas por el programador y tienen que ser diferenciables. En éste trabajo se implementanlas tareas de la postura, que minimiza el error de los estados articulares, y del centro demasas, que minimiza el error en posición, velocidad y aceleración de esa misma magnitud.El controlador implementado consigue mantener el equilibrio del robot, seguir el centro demasas deseado y hacer cambios en la postura que no requieran deshacer los contactos de lospies. Sin embargo, no alcanza a seguir las trayectorias de caminar generadas por towr.

Para arreglar esto último es posible realizar mejoras tanto en la fase de optimización detrayectorias como en el controlador de cuerpo completo. La planificación de los movimientosse hace con respecto a un modelo dinámico que supone que las piernas del robot no tienenmasa. Esto puede funcionar bien con robots cuadrúpedos ya que son estables y la masa desus piernas representa un porcentaje pequeño de su masa total. En el caso del robot Talosesta suposición no es válida, ya que sus piernas son bastante pesadas. Por tanto, si se hiciesela planificación con respecto al modelo dinámico completo del robot, sería posible generartrayectorias más realistas y por tanto que el controlador de cuerpo completo pudiera seguir.

Por otro lado, se propone en el controlador de cuerpo completo la creación de una tareaque siga las posiciones y orientaciones cartesianas de los efectores finales de cada pierna delrobot tal y como se explica en el trabajo realizado por Bouyarmane y cols. (2019). De estamanera se podría conseguir que la posición de los pies no dependa sólamente de la orientaciónde la base y por tanto conseguir ’cerrar el bucle’ de las posiciones de los efectores finales,evitando la desalineación del pie al volver al suelo como se ve en la Figura 5.12.

Por último, es posible incrementar la eficiencia del problema de optimización cuadrática(Herzog y cols. (2014)). Teniendo en cuenta que la dinámica del robot (2.12) puede serdescompuesta en dos ecuaciones:

65

66 Conclusiones

Mu(q)q +N(q, q) = JTu f (6.1)

Ma(q)q +N(q, q) = u+ JTa f (6.2)

donde (6.1) se corresponde con las seis primeras ecuaciones del movimiento correspondien-tes con la base del robot, y (6.2) con los grados de libertad actuados, o lo que es lo mismolas articulaciones del robot. A partir de (6.2) puede despejar u:

u = Ma(q)q +N(q, q)− JTa f (6.3)

Por tanto, u es linealmente dependiente de q y f. Es posible realizar la sustitución (6.3) enel problema de optimización dado por (3.43) y (3.44). Esto reduce el número de variables aoptimizar de (2n+ 6 + 3c) a (n+ 6 + 3c), donde n es el número de grados de libertad y c esla cantidad de contactos.

Los valores de u siguen siendo necesarios para poder aplicar la fuerza deseada en todos losactuadores. Conociendo q y f es posible obtener u mediante la dinámica inversa, la cual sepuede obtener por ejemplo con el algoritmo RNEA. Según Carpentier y cols. (2019), el costetemporal de computar la dinámica con este algoritmo, implementado en Pinocchio, está pordebajo del microsegundo.

Bibliografía

ApS, M. (2019). Mosek optimization suite.

Bidaud, P., Salini, J., Barthélemy, S., y Padois, V. (2013, 01). Whole-body motion synthesiswith lqp-based controller – application to icub. En (p. 199-210). doi: 10.1007/978-3-642-36368-9_16

Bouyarmane, K., Caron, S., Escande, A., y Kheddar, A. (2019). Multi-contact motionplanning and control. En A. Goswami y P. Vadakkepat (Eds.), Humanoid robotics: Areference (pp. 1763–1804). Dordrecht: Springer Netherlands. Descargado de https://doi.org/10.1007/978-94-007-6046-2_32 doi: 10.1007/978-94-007-6046-2_32

Boyd, S., y Vandenberghe, L. (2004). Convex optimization.

Bratta, A., Orsolino, R., Focchi, M., Barasuol, V., Muscolo, G. G., y Semini, C. (2020). Onthe hardware feasibility of nonlinear trajectory optimization for legged locomotion basedon a simplified dynamics. En 2020 ieee international conference on robotics and automation(icra) (p. 1417-1423). doi: 10.1109/ICRA40945.2020.9196903

Calvo, R., Trujillo, A., Felicetti, L., y Pomares, J. (2021). Trajectory optimization and controlof a free floating two arm humanoid robot, journal of guidance, control and dynamics.Journal of guidance, control and dynamics (enviado, pendiente de aceptación).

Carlo, J., Wensing, P., Katz, B., Bledt, G., y Kim, S. (2018, 10). Dynamic locomotion inthe mit cheetah 3 through convex model-predictive control. En (p. 1-9). doi: 10.1109/IROS.2018.8594448

Carpentier, J., Saurel, G., Buondonno, G., Mirabel, J., Lamiraux, F., Stasse, O., y Mansard,N. (2019, 01). The pinocchio c++ library — a fast and flexible implementation of rigidbody dynamics algorithms and their analytical derivatives.. doi: 10.1109/SII.2019.8700380

Chitta, S., Marder-Eppstein, E., Meeussen, W., Pradeep, V., Rodríguez Tsouroukdissian,A., Bohren, J., … Fernández Perdomo, E. (2017). ros_control: A generic and simplecontrol framework for ros. The Journal of Open Source Software. Descargado de http://www.theoj.org/joss-papers/joss.00456/10.21105.joss.00456.pdf doi: 10.21105/joss.00456

Coumans, E., y cols. (2013). Bullet physics library. Open source: bulletphysics. org, 15(49),5.

Del Prete, A., Nori, F., Metta, G., y Natale, L. (2014, 10). Prioritized motion-force control ofconstrained fully-actuated robots:“task space inverse dynamics”. Robotics and AutonomousSystems. doi: 10.1016/j.robot.2014.08.016

67

68 Bibliografía

Diankov, R., y Kuffner, J. (2008). Openrave: A planning architecture for autonomous robo-tics. Robotics Institute, Pittsburgh, PA, Tech. Rep. CMU-RI-TR-08-34, 79.

Domahidi, A., Chu, E., y Boyd, S. (2013). Ecos: An socp solver for embedded systems. En2013 european control conference (ecc) (p. 3071-3076). doi: 10.23919/ECC.2013.6669541

Eljaik, J. G., Lober, R., Hoarau, A., y Padois, V. (2018). Optimization-based controllers forrobotics applications (ocra): The case of icub’s whole-body control. Frontiers in Roboticsand AI , 5, 24. Descargado de https://www.frontiersin.org/article/10.3389/frobt.2018.00024 doi: 10.3389/frobt.2018.00024

Featherstone, R. (2014). Rigid body dynamics algorithms. Springer.

Ferreau, H., Kirches, C., Potschka, A., Bock, H., y Diehl, M. (2014). qpOASES: A parametricactive-set algorithm for quadratic programming. Mathematical Programming Computation,6(4), 327–363.

Gertz, E. M., y Wright, S. J. (2003, marzo). Object-oriented software for quadratic pro-gramming. ACM Trans. Math. Softw., 29(1), 58–81. Descargado de https://doi.org/10.1145/641876.641880 doi: 10.1145/641876.641880

Gill, P., Murray, W., y Saunders, M. (2002, 04). Snopt: An sqp algorithm for large-scaleconstrained optimization. SIAM Journal on Optimization, 12, 979-1006. doi: 10.2307/20453604

Guennebaud, G., Jacob, B., y cols. (2010). Eigen. URl: http://eigen. tuxfamily. org.

Herzog, A., Righetti, L., Grimminger, F., Pastor, P., y Schaal, S. (2014, Sep). Balancingexperiments on a torque-controlled humanoid with hierarchical inverse dynamics. 2014IEEE/RSJ International Conference on Intelligent Robots and Systems. Descargado dehttp://dx.doi.org/10.1109/IROS.2014.6942678 doi: 10.1109/iros.2014.6942678

Hutter, M., Gehring, C., Jud, D., Lauber, A., Bellicoso, D., Tsounis, V., … Höpflinger, M.(2016). Anymal - a highly mobile and dynamic quadrupedal robot. 2016 IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS), 38-44.

Inc, G. O. (2016). Gurobi optimizer reference manual. Descargado de http://www.gurobi.com

Kajita, S., Kanehiro, F., Kaneko, K., Fujiwara, K., Harada, K., Yokoi, K., y Hirukawa, H.(2003). Resolved momentum control: humanoid motion planning based on the linear andangular momentum. En Proceedings 2003 ieee/rsj international conference on intelligentrobots and systems (iros 2003) (cat. no.03ch37453) (Vol. 2, p. 1644-1650 vol.2). doi: 10.1109/IROS.2003.1248880

Kaneko, K., Kanehiro, F., Kajita, S., Hirukawa, H., Kawasaki, T., Hirata, M., … Isozumi,T. (2004). Humanoid robot hrp-2. En Ieee international conference on robotics andautomation, 2004. proceedings. icra ’04. 2004 (Vol. 2, p. 1083-1090 Vol.2). doi: 10.1109/ROBOT.2004.1307969

Bibliografía 69

Kanoun, O., Lamiraux, F., y Wieber, P.-B. (2011, 09). Kinematic control of redundantmanipulators: Generalizing the task-priority framework to inequality task. Robotics, IEEETransactions on, 27 , 785 - 792. doi: 10.1109/TRO.2011.2142450

Kelly, M. (2017). An introduction to trajectory optimization: How to do your own directcollocation. SIAM Review, 59(4), 849-904. Descargado de https://doi.org/10.1137/16M1062569 doi: 10.1137/16M1062569

Khatib, O. (1987). A unified approach for motion and force control of robot manipulators:The operational space formulation. IEEE Journal on Robotics and Automation, 3(1), 43-53.doi: 10.1109/JRA.1987.1087068

Koenig, N., y Howard, A. (2004). Design and use paradigms for gazebo, an open-sourcemulti-robot simulator. En 2004 ieee/rsj international conference on intelligent robots andsystems (iros) (ieee cat. no.04ch37566) (Vol. 3, p. 2149-2154 vol.3). doi: 10.1109/IROS.2004.1389727

Lee, J., Grey, M. X., Ha, S., Kunz, T., Jain, S., Ye, Y., … Liu, C. K. (2018). Dart: Dynamicanimation and robotics toolkit. Journal of Open Source Software, 3(22), 500.

Mastalli, C., Budhiraja, R., Merkt, W., Saurel, G., Hammoud, B., Naveau, M., … Mansard,N. (2020, 03). Crocoddyl: An efficient and versatile framework for multi-contact optimalcontrol.. doi: 10.1109/ICRA40945.2020.9196673

MAYNE, B. D. (1966). A second-order gradient method for determining optimal trajectoriesof non-linear discrete-time systems. International Journal of Control, 3(1), 85-95. Descarga-do de https://doi.org/10.1080/00207176608921369 doi: 10.1080/00207176608921369

Moro, F. L., y Sentis, L. (2019). Whole-body control of humanoid robots. En A. Goswamiy P. Vadakkepat (Eds.), Humanoid robotics: A reference (pp. 1161–1183). Dordrecht:Springer Netherlands. Descargado de https://doi.org/10.1007/978-94-007-6046-2_51doi: 10.1007/978-94-007-6046-2_51

Nelson, G., Saunders, A., y Playter, R. (2019, 01). The petman and atlas robots at bostondynamics. En (p. 169-186). doi: 10.1007/978-94-007-6046-2_15

Ott, C., Roa, M. A., y Hirzinger, G. (2011). Posture and balance control for biped robotsbased on contact force optimization. En 2011 11th ieee-ras international conference onhumanoid robots (p. 26-33). doi: 10.1109/Humanoids.2011.6100882

Park, J., Haan, J., y Park, F. C. (2007). Convex optimization algorithms for active balancingof humanoid robots. IEEE Transactions on Robotics, 23(4), 817-822. doi: 10.1109/TRO.2007.900639

Posa, M., Kuindersma, S., y Tedrake, R. (2016). Optimization and stabilization of trajectoriesfor constrained dynamical systems. En 2016 ieee international conference on robotics andautomation (icra) (p. 1366-1373). doi: 10.1109/ICRA.2016.7487270

Robotics, A. (2017). Cassie. Youtube: https://youtu. be/Is4JZqhAy-M.

70 Bibliografía

Sakagami, Y., Watanabe, R., Aoyama, C., Matsunaga, S., Higaki, N., y Fujimura, K. (2002).The intelligent asimo: system overview and integration. En Ieee/rsj international conferenceon intelligent robots and systems (Vol. 3, p. 2478-2483 vol.3). doi: 10.1109/IRDS.2002.1041641

Salini, J., Barthélemy, S., Bidaud, P., y Padois, V. (2013). Whole-body motion synthesis withlqp-based controller – application to icub. En K. Mombaur y K. Berns (Eds.), Modeling,simulation and optimization of bipedal walking (pp. 199–210). Berlin, Heidelberg: Sprin-ger Berlin Heidelberg. Descargado de https://doi.org/10.1007/978-3-642-36368-9_16doi: 10.1007/978-3-642-36368-9_16

Semini, C., Tsagarakis, N., Guglielmino, E., Focchi, M., Cannella, F., y Caldwell, D. (2011,08). Design of hyq -a hydraulically and electrically actuated quadruped robot. Procee-dings of the Institution of Mechanical Engineers. Part I: Journal of Systems and ControlEngineering, 225, 831-849. doi: 10.1177/0959651811402275

Sentis, L., y Khatib, O. (2005, 12). Synthesis of whole-body behaviors through hie-rarchical control of behavioral primitives. I. J. Humanoid Robotics, 2, 505-518. doi:10.1142/S0219843605000594

Sherman, M. A., Seth, A., y Delp, S. L. (2011). Simbody: multibody dynamics for biomedicalresearch. Procedia Iutam, 2, 241–261.

Smith, R., y cols. (2005). Open dynamics engine.

Stasse, O., Flayols, T., Budhiraja, R., Giraud-Esclasse, K., Carpentier, J., Mirabel, J., …Ferro, F. (2017, noviembre). TALOS: A new humanoid research platform targetedfor industrial applications. En International Conference on Humanoid Robotics, ICHR,Birmingham 2017. Birmingham, United Kingdom: IEEE. Descargado de https://hal.archives-ouvertes.fr/hal-01485519 doi: 10.1109/HUMANOIDS.2017.8246947

Stellato, B., Banjac, G., Goulart, P., Bemporad, A., y Boyd, S. (2020). OSQP: an operatorsplitting solver for quadratic programs. Mathematical Programming Computation, 12(4),637–672. Descargado de https://doi.org/10.1007/s12532-020-00179-2 doi: 10.1007/s12532-020-00179-2

Winkler, A. W., Bellicoso, C. D., Hutter, M., y Buchli, J. (2018). Gait and trajectoryoptimization for legged systems through phase-based end-effector parameterization. IEEERobotics and Automation Letters, 3(3), 1560-1567. doi: 10.1109/LRA.2018.2798285

Winkler, A. W., Farshidian, F., Pardo, D., Neunert, M., y Buchli, J. (2017). Fast trajectoryoptimization for legged robots using vertex-based zmp constraints. IEEE Robotics andAutomation Letters, 2(4), 2201-2208. doi: 10.1109/LRA.2017.2723931

Wächter, A., y Biegler, L. (2006, 03). On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical programming, 106,25-57. doi: 10.1007/s10107-004-0559-y

Bibliografía 71

Yamane, K., y Nakamura, Y. (2003). Natural motion animation through constraining anddeconstraining at will. IEEE Transactions on Visualization and Computer Graphics, 9(3),352-360. doi: 10.1109/TVCG.2003.1207443