DISE ˜
NO E IMPLEMENTACI ´
ON DE UN SISTEMA DE
CONTROL DE BALANCE PARA UN ROBOT
HUMANOIDE
JUAN DIEGO GIRALDO RODR´
IGUEZ
UNIVERSIDAD SANTO TOM ´
AS
FACULTAD DE INGENIER´
IA ELECTR ´
ONICA
BOGOT ´
A D.C.
DISE ˜
NO E IMPLEMENTACI ´
ON DE UN SISTEMA DE
CONTROL DE BALANCE PARA UN ROBOT
HUMANOIDE
JUAN DIEGO GIRALDO RODR´
IGUEZ
C´
odigo: 2100385
Trabajo de Grado
Director: Ingeniero JUAN MANUEL CALDER ´
ON CH ´
AVEZ
UNIVERSIDAD SANTO TOM ´
AS
FACULTAD DE INGENIER´
IA ELECTR ´
ONICA
BOGOT ´
A D.C.
CONTENIDO
P´
ag.
1. INTRODUCCI ´ON 6
2. JUSTIFICACI ´ON 8
3. PLANTEAMIENTO Y DESCRIPCI ´ON DEL PROBLEMA 10
4. ANTECEDENTES 12
5. OBJETIVOS 14
5.1. Objetivo General . . . 14
5.2. Objetivos Espec´ıficos . . . 14
6. MARCO TE ´ORICO 15 6.1. Equilibrio . . . 15
6.2. Centro de Masa, Punto de Momento Cero y Punto de Captura Ins-tant´anea (CoM, ZMP e ICP) . . . 15
6.3. Estrategias de balance y estabilidad en robots b´ıpedos: . . . 16
6.4. Modelo cinem´atico y din´amico del robot humanoide: . . . 18
6.4.1. Modelo Cinem´atico: . . . 18
6.4.2. Modelo Din´amico: . . . 19
7. DISE ˜NO METODOL ´OGICO 21
7.1. Enfoque metodol´ogico: . . . 21
7.2. Fuentes de Informaci´on: . . . 21
7.3. T´ecnicas de investigaci´on: . . . 21
7.4. Herramientas e instrumentos: . . . 22
7.5. Sistematizaci´on de la informaci´on: . . . 23
8. DESARROLLO DEL PROYECTO 24 8.1. LIPM como modelo para robot DARwIn-OP: . . . 24
8.2. C´alculo del CoM y ICP: . . . 24
8.3. Uso de V-REP como simulador del Robot DARwIn-OP: . . . 27
8.4. Modelo matem´atico del servomotor MX-28: . . . 29
8.5. Dise˜no del modelo de control: . . . 31
8.5.1. Lectura de la IMU, c´alculo de la posici´on y la velocidad del CoM del Robot: . . . 32
8.5.2. C´alculo del ZMP, ICP y estimaci´on de la posici´on deseada del CoM: 35 8.5.3. Control PID ´optimo: . . . 36
9. IMPLEMENTACI ´ON Y RESULTADOS EXPERIMENTALES 40 9.1. Simulaci´on del sistema de control de balance: . . . 40
9.2. Implementaci´on en la plataforma DARwIn-OP: . . . 45
11.ANEXOS 52
11.1. Anexo 1: . . . 52
11.2. Anexo 2: . . . 52
12.BIBLIOGRAF´IA 55
1.
INTRODUCCI ´
ON
En el presente trabajo se encuentra el desarrollo del dise˜no e implementaci´on de un sistema de control de balance para un robot humanoide, puntualmente en la plataforma DARwIn-OP, dise˜nado bajo la teor´ıa de control ´optimo y utilizando la estrategia del tobillo para mantener al robot erguido frente a inclinaciones en la superficie de apoyo del robot. Inicialmente se relacionan los aspectos te´oricos que preceden esta tem´atica, tales como: estrategias de control de balance en seres b´ıpedos, centro de masa, punto de captura instant´anea, regi´on de estabilidad, entre otros.
Por otra parte, el problema a abordar es el de dise˜nar un sistema de control bajo los conceptos de la teor´ıa de control ´optimo, con el prop´osito de otorgarle al robot la ca-pacidad de mantener su posici´on de erguido frente a perturbaciones en la inclinaci´on de su superficie. Para ello es necesario obtener un modelo matem´atico que describa el movimiento del robot humanoide, siendo as´ı el modelo lineal del p´endulo invertido (LIPM), el modelo de trabajo para la elaboraci´on del sistema de control. Posteriormen-te se utiliza la herramienta de simulaci´on V-REP para recrear el entorno de prueba necesario y as´ı permitir el an´alisis, validaci´on e implementaci´on del sistema de control, creando un ambiente que contiene al robot y la superficie de prueba.
Una vez construido el espacio de simulaci´on, se procede al dise˜no del sistema de con-trol, cuyo objetivo principal es el de mantener la proyecci´on del centro de masa (CoM) del robot dentro del ´area de soporte (regi´on de estabilidad). Para ello se trabaja en la elaboraci´on de un controlador que modifique las ganancias PID de los servomotores del robot en relaci´on al error de posici´on del CoM y la posici´on de los actuadores. Esto, bajo el concepto de la rigidez variable que se obtiene mediante el uso de los reguladores cuadr´aticos Lineales (LQR). Adicionalmente se muestra la elaboraci´on del modelo ma-tem´atico de los servomotores MX-28 de la compa˜n´ıa Dynamixel, cuya elaboraci´on es crucial a la hora de dise˜nar el sistema de control. El controlador fue dise˜nado dentro de la herramienta de MATLAB y posteriormente se migra al lenguaje C, con el prop´osito de poder ser implementarlo en la plataforma real.
permitiendo de esa manera las conclusiones correspondientes y los trabajos futuros que deja la presente investigaci´on.
La elaboraci´on del presente documento se encuentra distribuido en 10 cap´ıtulos, que siguen la misma secuencia de desarrollo expresada anteriormente.
2.
JUSTIFICACI ´
ON
En el mundo moderno existen diferentes iniciativas que trabajan en el desarrollo de pro-puestas que mediante la investigaci´on, logren converger en soluciones que var´ıan frente a las diferentes ´areas de la sociedad, como la milicia, la salud, la educaci´on, entreteni-miento y desarrollos cient´ıficos. Desde el a˜no 2002 DARPA (Agencia de Proyectos de Investigaci´on Avanzados de Defensa) ha estado financiando proyectos enfocados en el desarrollo y evoluci´on de la rob´otica humanoide, con el prop´osito de buscar plataformas que realicen tareas de asistencia, b´usqueda y rescate de v´ıctimas en situaciones afecta-das por cat´astrofes naturales o desastres en diferentes contextos [4]. Tales plataformas deben caracterizarse por presentar patrones de suavidad, fluidez, estabilidad y balance en sus movimientos frente al entorno que lo rodea.
Otra iniciativa que estimula el desarrollo de robots humanoides, es RoboCup, con el enfoque de reunir investigadores a nivel mundial que trabajen en innovaci´on tecnol´ogica; se incluye el desarrollo de plataformas rob´oticas humanoides que est´en en la capacidad de responder ante las necesidades que cada liga propone. Dos de las ligas m´as grandes que hay en ella son Soccer y @Home, donde en la primera se pretende para el a˜no 2050, consolidar un equipo rob´otico humanoide de futbol, que pueda enfrentarse en un encuentro amistoso al campe´on de la FIFA de ese momento. Es claro que para llegar a eso, se debe contar con robots que est´en en capacidad de conservar el equilibrio y estabilidad, al realizar movimientos como caminar, saltar y correr. Por otra parte, en la liga @Home, se busca dise˜nar plataformas rob´oticas humanoides para asistencia dom´estica, con el perfil de ayudar en tareas del hogar y a su vez a los seres humanos. De este modo, la necesidad de involucrar conceptos de estabilidad frente a la locomoci´on en ambientes cotidianos, resalta la importancia de trabajar en algoritmos de control de estabilidad para plataformas rob´oticas humanoides, tal y como se plantea en este trabajo.
presentados en estas conferencias relacionados con este tema. Siendo as´ı este, el even-to de primer reconocimieneven-to internacional de la comunidad de rob´otica humanoide. (IEEE-RAS Technical Committee on Humanoid Robotics, 2015).
En un sentido m´as amplio, este proyecto permite la articulaci´on de la academia, la inves-tigaci´on y la proyecci´on social, factores que hacen parte de las funciones sustantivas de la Universidad. Desde el punto de vista acad´emico e investigativo, este proyecto permite de manera personal aplicar los conocimientos adquiridos en ingenier´ıa, mostr´andome como un profesional ´util para la sociedad y basado en los principios humanistas de la Universidad, contribuir ´eticamente para el mejoramiento de la vida humana. A su vez, este trabajo se encuentra relacionado con los proyectos que se ejecutan en el grupo de investigaci´on y desarrollo en rob´otica (G.E.D), de la Facultad de Ingenier´ıa Electr´onica de la Universidad. Teniendo en cuenta ´esto, se espera que el proyecto sirva de base para futuros trabajos, enfocados hacia la investigaci´on y el desarrollo de plataformas rob´oticas humanoides.
Finalmente, este proyecto tiene un enfoque de proyecci´on social que es coherente con la misi´on de la Universidad, dado que la implementaci´on de un control de balance genera una base para la creaci´on de aplicaciones que impacten positivamente en la sociedad, esto en sectores como la salud, la educaci´on, entre otros. A su vez, la rob´otica humanoide busca ayudar en tareas que presentan un riesgo al bienestar y seguridad de los seres humanos, tales como: b´usqueda y rescate de v´ıctimas en zonas de desastre, asistencia dom´estica y cooperaci´on en ´areas de trabajo.
3.
PLANTEAMIENTO Y DESCRIPCI ´
ON DEL
PROBLEMA
A lo largo de las ´ultimas d´ecadas, la rob´otica ha tomado importancia en la generaci´on de nuevas propuestas de investigaci´on gracias a los desarrollos y a la versatilidad en muchos de los sectores de la sociedad. Los sectores que se pueden mencionar son: el industrial, la salud, la milicia, la educaci´on, entre otros. Estos sectores, han generado temas de investigaci´on que acaparan gran parte de las necesidades humanas con el prop´osito de solucionarlas. Algunas necesidades humanas incluidas en estas investigaciones son: los procesos autom´aticos industriales, pr´otesis y ´ortesis, b´usqueda y rescate de personas, asistentes educativos, etc.
De este modo, uno de los temas que m´as ha exigido a la ingenier´ıa es la rob´otica humanoide, en ella, se encuentra la necesidad de elaborar plataformas que cumplan con el mismo o mejor desempe˜no en la ejecuci´on de tareas que el ser humano. Es conocido que dichas tareas requieren de un sistema de locomoci´on que involucra tanto la fluidez de los movimientos y una ´optima marcha b´ıpeda, como la estabilidad y la versatilidad de dichas plataformas. Aspectos tales como movilidad, inteligencia y adaptabilidad de estos robots, son requisitos fundamentales para poder llegar a la construcci´on de un robot parecido a los seres humanos, aun cuando en la actualidad se evidencia la falta de similitud que hay frente a los mismos.
Para el a˜no 2050, se planea contar con la existencia de robots humanoides en funci´on de asistentes dom´esticos que atiendan las necesidades de la poblaci´on, ya que se estima que el ´ındice de poblaci´on mayor a 60 a˜nos, ser´a el triple en relaci´on a la poblaci´on actual [5]. Por lo tanto, es necesario el desarrollo de habilidades esenciales como la estabilidad y equilibrio de los robots, as´ı como tambi´en la suavidad de los movimientos del robot con su entorno.
Bajo esta idea, surge la pregunta que abarca el desarrollo de este proyecto: ¿C´omo establecer un sistema de control de balance para un robot humanoide que se encuentra en posici´on de erguido, ante perturbaciones relacionadas al cambio en la inclinaci´on de la superficie de contacto?.
4.
ANTECEDENTES
Uno de los aspectos fundamentales que se explora en los procesos de desarrollo investi-gativo en rob´otica, es el control de movimiento de robots humanoides, actualmente se encuentran numerosos trabajos que relacionan tanto t´ecnicas tradicionales, como nue-vas t´ecnicas propuestas por los investigadores, con el fin de establecer movimientos m´as suaves y controlados en los robots. Uno de los modelos matem´aticos que han generado m´as impacto en esta ´area, es el planteado por [12], en donde se propone un modelo tridimensional del p´endulo invertido para un robot humanoide y donde se fundamen-ta gran parte de la teor´ıa utilizada para la generaci´on de estrategias de control. Este modelo es utilizado tambi´en en sistemas de control de estabilidad en postura erguida, marcha, trote y carrera, presentados en [21].
Bajo el marco de estrategias de control de estabilidad, existe un sinn´umero de aplica-ciones desarrolladas con el fin de mantener el balance del robot en presencia de pertur-baciones de diferente tipo y naturaleza, algunos de esos trabajos son los que se citan a continuaci´on. En el art´ıculo llamado “Balancing a Humanoid Robot Using Backdrive Concerned Torque Control and Direct Angular Momentum Feedback”los autores Ka-jita, Yokoi y Kazuo hacen uso del control de torque para mantener el equilibrio de un robot humanoide, partiendo de la velocidad angular de los links del tren inferior del robot [12]. En el art´ıculo titulado Lateral Disturbance Rejection for the Nao Robot [3], usan un modelo lineal de p´endulo invertido (LIPM) para dise˜nar un control predictivo, el cual es mejorado adicionando dos controladores m´as, uno para el tiempo de paso de un robot y el otro para generar trayectorias del CoM. Con ello se puede corregir el balance de un robot frente a perturbaciones fuertes, las pruebas y los resultados se hicieron en un robot NAO que a su vez se basan en medidas reales de experimentos desarrollados con el robot. Adicionalmente, un nuevo modelo lineal de control predictivo es propuesto en [22] para mantener el Zero Moment Point (ZMP) estable y de la misma manera que los art´ıculos anteriores se utiliza un sistema de generaci´on de trayectorias para mantener el balance de un robot humanoide.
En estos ´ultimos trabajos se implementan procesos de optimizaci´on y dise˜nos de control ´
En [10] se propone un algoritmo de reducci´on de la fuerza de impacto en robots huma-noides que saltan. El algoritmo propuesto se basa en la reducci´on del impacto mediante la variaci´on de la rigidez en los motores del tobillo y la rodilla. La reducci´on de la rigidez se alcanza mediante el uso de un sistema difuso. Dicho sistema se encarga de estimar la ganancia de los sistemas de control en cada uno de los actuadores para gene-rar diferentes grados de rigidez, tal como se explica en [11]. Estos dos trabajos reducen la fuerza de impacto en el momento que el robot aterriza de nuevo. A diferencia de la propuesta del presente proyecto, [11] utiliza un generador de trayectorias para lograr el salto y un sistema de control basado en equilibrio din´amico para reducir el impacto. En el presente trabajo, se propone la conservaci´on del equilibrio en un robot que se encuentra en una posici´on est´atica (erguido) y el modelo de control se decidir´a seg´un el estudio que se realice dentro del desarrollo de este proyecto.
5.
OBJETIVOS
Conforme al problema mencionado anteriormente, este trabajo de grado se llevar´a acabo bajo los siguientes objetivos.
5.1.
Objetivo General
Dise˜nar e implementar un control de balance para un robot humanoide DARwIn-OP en posici´on de erguido frente a perturbaciones relacionadas al cambio en la inclinaci´on de la superficie de contacto, usando t´ecnicas de control moderno, permitiendo as´ı, que el robot mantenga su postura y logre un equilibrio est´atico.
5.2.
Objetivos Espec´ıficos
Validar un modelo matem´atico acorde a la din´amica de movimiento del robot humanoide DARwIn-OP.
Dise˜nar un sistema de control basado en control ´optimo, de tal forma que permita posicionar el centro de masa del robot, proyect´andolo dentro de la superficie de contacto.
Implementar el controlador en un ambiente de simulaci´on y validar su funcio-namiento mediante la evaluaci´on del desplazamiento del centro de masa, ante la presencia de perturbaciones.
6.
MARCO TE ´
ORICO
Cuando se habla de balance, hay que remitirse inicialmente a conceptos esenciales como equilibrio, centro de masa (CoM), punto de momento cero (ZMP) y punto de captura instant´anea (ICP por sus siglas en ingl´es); sin embargo, las estrategias de control y estabilidad en robots b´ıpedos como: estrategia del tobillo, estrategia de la cadera y paso de recuperaci´on son la clave para lograr el objetivo principal de este trabajo. Adicional a eso, cuando se habla de control de balance en robots humanoides, relucen aspectos como modelo din´amico de un robot humanoide, c´alculo del CoM y c´alculo del ´
area efectiva de apoyo.
6.1.
Equilibrio
Dentro de los criterios de equilibrio para robots humanoides, es indispensable compren-der el concepto de equilibrio en sus dos definiciones m´as relevantes: equilibrio est´atico y equilibrio din´amico.
Equilibrio est´atico: Basa su estabilidad en el centro de masa (CoM) del robot, en donde se dice que se encuentra en equilibrio, si el CoM se encuentra siempre proyectado verticalmente sobre el ´area efectiva de apoyo y no presenta un despla-zamiento significativo del robot para lograrlo (ejemplo: un paso en direcci´on a la perturbaci´on).
Equilibrio din´amico: En este punto, el robot necesita de un movimiento pro-longado que modifique su ubicaci´on actual, permitiendo que el CoM abandone el ´
area efectiva de soporte por periodos cortos, apareciendo un desplazamiento prin-cipalmente horizontal, de forma que no haya inestabilidad (ejemplo: apresurar la marcha frente a perturbaciones externas).
6.2.
Centro de Masa, Punto de Momento Cero y Punto de
Captura Instant´
anea (CoM, ZMP e ICP)
Dentro de la rob´otica humanoide, el centro de masa y el centroide, coinciden con el cen-tro de gravedad, pero corresponden a conceptos diferentes y difieren num´ericamente.
En ese sentido el centro de gravedad se define como el punto de aplicaci´on de las resul-tantes de todas las fuerzas que la gravedad ejerce sobre los diferentes puntos materiales que constituyen el cuerpo. [13].
Centro de masa: El centro de masa (CoM) de un objeto se define como el punto geom´etrico que din´amicamente se comporta como si estuviera sometido a la resultante de las fuerzas externas del sistema.
Punto de momento cero:Desde su parte m´as te´orica, el punto de momento cero (ZMP), se define como el punto con respecto al cual la resultante de las fuerzas de reacci´on que act´uan entre la superficie y la planta de un pie, no genera momento en el eje horizontal. Por otra parte, es el concepto mayormente utilizado como criterio de estabilidad en la rob´otica humanoide, ya que para lograr un equilibrio din´amico en un robot humanoide, es necesario que el ZMP debe estar siempre proyectado dentro del ´area efectiva de apoyo, para evitar fuerzas de volcado en las diferentes partes del robot.
Punto de captura instant´anea: El ICP es el punto mediante el cual se deter-mina si el CoM de un robot est´a desplaz´andose hacia los l´ımites de la regi´on de estabilidad. En otras palabras, permite saber si el robot est´a a punto de caer o no, mediante el c´alculo que asocia la velocidad y el desplazamiento del centro de masa del robot, en relaci´on al eje de movimiento ( x,y oz).
6.3.
Estrategias de balance y estabilidad en robots b´ıpedos:
Con relaci´on a los conceptos anteriores, se desprenden una serie de estrategias, capaces de establecer un control de balance en robots b´ıpedos, teniendo como referencia la biomec´anica del balance en los seres humanos [1]. Entre las principales estrategias de estabilidad y balance, se pueden distinguir dos en particular: la estrategia del tobillo y la estrategia de la cadera [19].
funciona para perturbaciones relativamente peque˜nas y en ocasiones el resto de las extremidades, como la cadera, se encuentran r´ıgidas (ver figura 1-a)[19].
Estrategia de la cadera: Cuando la perturbaci´on es m´as grande y la estrate-gia del tobillo no logra mantener el CoM dentro de la regi´on de estabilidad, la estrategia de la cadera se emplea para lograr reposicionar el centro de masa en una regi´on favorable, de modo que la estrategia del tobillo reubique el CoM en el lugar deseado [20].
Dicha estrategia consiste en realizar una inflexi´on en el torso, generalmente en el mismo sentido de la perturbaci´on, con un torque grande en la articulaci´on de la cadera, generando as´ı una aceleraci´on del CoM y de ese modo un movimiento de restituci´on que lo reubica en la regi´on de estabilidad como lo explica [19]. Usual-mente se emplea simult´aneamente con la del tobillo, toda vez que la perturbaci´on haya sido lo suficientemente grande (ver figura 1-b)1
Figura 1: Estrategia del tobillo y cadera- uso del tobillo (a). uso de tobillo y cadera (b).
1Imagen tomada de: Balance maintenance of a humanoid robot using the hip-ankle strategy by S.
Kiemel,2012.
6.4.
Modelo cinem´
atico y din´
amico del robot humanoide:
El modelo a trabajar se sit´ua en un espacio bidimensional, espec´ıficamente en el plano sagital del robot, donde los movimientos realizados en un pie ser´an los mismos para el otro pie de la plataforma DARwIn-OP -ver figura 22-. De esta manera, la estructura del robot est´a determinada por 4 puntos de uni´on r´ıgidos que corresponden al torso (link 3), f´emur (link 2), tibia (link 1) y pie (link 0), cada uni´on est´a conectada por 3 puntos rotacionales que corresponden a la cadera, rodilla y tobillo -ver figura 3-. Por otra parte, las perturbaciones ser´an generadas mediante el cambio en la inclinaci´on de la superficie de contacto del robot. Tal superficie, es plana y de friccion suficiente para la ausencia de desplazamientos horizontales.
Figura 2: Robot DARwIn-OP.
6.4.1. Modelo Cinem´atico:
La posici´on final de los links y la posici´on de sus centros de masa, definen el modelo cinem´atico del sistema, en donde xic denota la direcci´on en el eje x y yic en el eje y. (Ver anexo 2 ecuaci´on 1). Posteriormente se calcula la ubicaci´on absoluta del centro de masa del robot bajo la expresi´on descrita en (1):
xcom = Pn
i=1mixi Pn
i=1mi
, ycom = Pn
i=1miyi Pn
i=1mi
(1)
Figura 3: Modelo bidimensional del pie del robot DARwIn-OP.
Donde mi y ci es la masa y la posici´on absoluta respectivamente, de cada uno de los eslabones.
6.4.2. Modelo Din´amico:
Figura 4: Modelo lineal del p´endulo invertido - LIPM DARwIn-OP.
La din´amica de cualquier sistema de cuerpos r´ıgidos, puede ser determinada mediante el an´alisis del movimiento de sus centros de masa. De esta manera, la aproximaci´on com´unmente utilizada en la literatura, es la del modelo lineal del p´endulo invertido (LIPM), la cual se caracteriza por concentrar toda la masa del robot en un punto, con
un solo link o eslab´on sin masa y de altura fija [12], tal como se muestra en la figura 4, donde m es la masa total del robot, zcom la altura del CoM, xcom el desplazamiento horizontal del CoM, θ el ´angulo de inclinaci´on, g la aceleraci´on gravitacional y τa el torque en el tobillo. A su vez, la ecuaci´on que modela el sistema mencionado, se deduce a partir de la formulaci´on matem´atica de Euler-Lagrange, que se puede visualizar en la expresion (2) y (3).
d dx
∂L
∂θ˙
− ∂L
∂θ =τ (2)
L=KE−PE (3)
Siendo L la ecuaci´on de Lagrange y τ el torque o par ejercido. A su vez, la ecuaci´on incluye la energ´ıa cin´etica y potencial del sistema (KE y PE respectivamente), dando como resultado una expresi´on similar a la ecuaci´on (4).
D(θ)¨θ+H(θ,θ˙) ˙θ+G(θ) =Tθ (4)
Donde D(θ) es la representaci´on matricial de las fuerzas inerciales, H(θ,θ˙) la matriz que agrupa tanto la fuerza centr´ıfuga como las fuerzas de coriolis, G(θ) hace referencia a la matriz de fuerzas gravitacionales y Tθ el vector que agrupa fuerzas externas y los pares aplicados en cada uni´on. Por otra parte, los vectores θ, ˙θ y ¨θ, corresponden a la posici´on, velocidad y aceleraci´on rotacional respectivamente. Para el caso puntual de un sistema de un DoF como el del LIPM, se determina las ecuaciones de energ´ıa cin´etica
KE y energ´ıa potencial PE, para posteriormente establecer la ecuaci´on de torque de la uni´on rotacional (ver anexo 2 procedimiento 1). Una vez determinada la ecuaci´on de torque, se reagrupan en una expresi´on que da forma a la ecuaci´on generalizada del modelo lineal del p´endulo invertido descrito en (5).
7.
DISE ˜
NO METODOL ´
OGICO
7.1.
Enfoque metodol´
ogico:
Como enfoque metodol´ogico para la resoluci´on de este proyecto, se utilizar´an m´etodos cient´ıficos de orden cuantitativo, que permitir´an evaluar los resultados obtenidos a partir de caracter´ısticas de rendimiento, como margen de error del sistema y estabilidad frente a perturbaciones.
7.2.
Fuentes de Informaci´
on:
Las fuentes de informaci´on para este proyecto ser´an extra´ıdas mediante herramientas como IEEE explore, junto con trabajos relacionados a rob´otica humanoide desarrollados por los investigadores del grupo de estudio y desarrollo en rob´otica GED. Adicional-mente, se consultar´an art´ıculos extra´ıdos de las memorias de eventos como RoboCup Symposium”, donde se muestran importantes desarrollos hechos en humanoides y so-bre t´ecnicas de control modernas. Para ello es necesario recurrir al estudio riguroso del estado del arte, con el fundamento de establecer puntos de referencias suficientes para lograr el objetivo de este proyecto.
7.3.
T´
ecnicas de investigaci´
on:
Inicialmente se debe realizar un primer estudio de los antecedentes sobre el ´area de investigaci´on de rob´otica humanoide; seguidamente es necesario identificar los conceptos te´oricos que m´as tienen relevancia dentro del ´area a trabajar y posteriormente poder establecer un enfoque metodol´ogico para la ejecuci´on del problema. Conociendo los conceptos y variables que est´an involucrados, se puede realizar una selecci´on de las herramientas tanto matem´aticas como computacionales, con las cuales se va a trabajar cada ´ıtem de los objetivos de este proyecto, y determinar la arquitectura que constituir´a el buen cumplimiento del objetivo principal.
En ese sentido, los algoritmos a realizar en este trabajo se fundamentan en la revisi´on te´orica, puesto que los conceptos que all´ı se identifican, establecen las estrategias de
simulaci´on y las t´ecnicas de control m´as convenientes que ser´an implementadas. Es claro que los procesos computacionales en un sistema de control, requieren de una carga computacional baja, para alcanzar respuestas suficientemente r´apidas, que permitan un correcto desempe˜no; por tal motivo, se busca usar lenguajes de programaci´on de medio nivel, como por ejemplo, lenguaje C.
Una vez realizado el algoritmo, se proceder´a a validar los resultados en una plataforma de simulaci´on, que permitir´a depurar aspectos indeseables del comportamiento del ro-bot, antes de ser aplicados en una plataforma f´ısica, y as´ı validar el cumplimiento de los objetivos propuestos. Finalmente, se realizar´a un an´alisis de los resultados obtenidos relacionado con las caracter´ısticas de rendimiento esperados en el sistema, y ´esto me-diante el uso cuantitativo de la informaci´on adquirida a partir de las diferentes pruebas. Las conclusiones obtenidas se expondr´an en una presentaci´on que re´una la comunidad profesional.
7.4.
Herramientas e instrumentos:
Como herramientas de trabajo se acude al uso tanto de software libre, como al licen-ciado existente en las instalaciones de la Universidad; adem´as se utilizar´a la plataforma rob´otica DARwIn-OP. Algunas de las herramientas a utilizar son:
V-REP:Este es un software de simulaci´on de plataformas rob´oticas, que median-te librer´ıas inmedian-ternas, permimedian-te la ejecuci´on de procesos complejos, que involucran aspectos de forma y espacio de una gran variedad de robots; como herramienta opcional se incluye el ambiente de simulaci´on Webots 7 y ROS, para el mismo prop´osito.
MATLAB:Esta herramienta principalmente se usa para analizar y dise˜nar siste-mas mediante el uso del lenguaje t´ecnico, permitiendo realizar procesos de control, automatizaci´on y visi´on artificial, entre otros; a su vez, facilita el an´alisis gr´afico y matricial de operaciones matem´aticas.
7.5.
Sistematizaci´
on de la informaci´
on:
Para poder validar la informaci´on obtenida experimentalmente y mediante simulaci´on, es necesario agrupar la informaci´on de manera estructurada y precisa, conforme a los par´ametros de rendimiento tales como margen de error y estabilidad. Estas son las variables de medida que ser´an consignadas cuantitativamente para ser analizadas y comparadas con los par´ametros esperados, determinando as´ı los resultados y las con-clusiones del trabajo.
8.
DESARROLLO DEL PROYECTO
Conforme a los objetivos planteados previamente para el desarrollo del proyecto, ini-cialmente se presenta el an´alisis del LIPM como modelo din´amico para el robot b´ıpedo DARwIn-OP. Seguidamente se dise˜na un ambiente de simulaci´on en la plataforma V-REP para trabajar y validar tanto el control como el movimiento del CoM del robot. Adicional a ello, se muestra el dise˜no del controlador y su implementaci´on usando la herramienta MATLAB. Finalmente se muestra el an´alisis del comportamiento del con-trolador en el ambiente de simulaci´on V-REP, para posteriormente ser implementado en el robot real y as´ı obtener los resultados experimentales del proceso.
8.1.
LIPM como modelo para robot DARwIn-OP:
En la figura 4 mostrada anteriormente se present´o el modelo lineal del p´endulo invertido; sin embargo, dicho diagrama requiere de una restricci´on importante frente alxcom, dado que la base de apoyo (planta del pie), tiene una longitud que comprende la distancia entre el tal´on y los dedos del pie; dicha longitud se denota porδ− yδ+respectivamente
[20]; para el caso puntual, δ− = −0,062 m y δ+ = 0,042 m debido a que el robot en
la posici´on de marcha desplaza su CoM 1cm hacia adelante. De esta manera el modelo modificado se aprecia en la figura 5.
De igual manera, el modelo matem´atico descrito en la ecuaci´on (5) cambia, en el sentido de expresar el sistema en t´erminos del desplazamiento horizontal (xcom) del centro de masa; entonces, reorganizando la ecuaci´on (5) y sustituyendo w=qzg
com, se obtiene la
ecuaci´on (6), donde m= 2,9kg, g = 9,8 sm2 y zcom = 0,25 m.
¨
xcom =w2(xcom−
Ta
mg) (6)
8.2.
C´
alculo del CoM y ICP:
Figura 5: Diagrama de cuerpo Libre - DARwIn-OP como LIPM.
en posici´on de erguido [1]. En primer lugar, es necesario aclarar que en [21] el t´ermino
−Ta
mg de la ecuaci´on (6), representa el ZMP; tambi´en en [8] lo definen como el centro de presi´on (CoP), que es equivalente al ZMP. De esta manera, el modelo LIPM permite determinar los valores de saturaci´on de torque (ecuaci´on (7)) a partir de las variables de estados (xcom, ˙xcom), ubicando el ZMP en los l´ımites del pie de apoyo (xzmp =δ− y
xzmp =δ+) como lo demuestra [1].
τamax=δ±mg (7)
Reemplazando (7) en (6) se obtiene (8).
¨
xcom=w2(xcom−δ±) (8)
Solucionando la ecuaci´on diferencial (8) [1], da como resultado la ecuaci´on (9) que determina la regi´on de estabilidad del CoM.
δ− < xcom+ ˙
xcom
w < δ
+ (9)
Figura 6: C´alculo del punto de captura instant´anea (xICP).
Estableciendo los l´ımites de la regi´on de estabilidad (figura 7), se conoce tambi´en la posici´on del ICP (xicp)(ver Figura 6), con este punto se determina la cantidad de torque necesario para reposicionar el CoM; adicional a ello, si alxicp (ecuaci´on (10)) se le asig-nan dos gaasig-nancias para la posici´on y la velocidad respectivamente, se puede establecer un valor de prioridad entre la variaci´on de la posici´on del CoM y/o su velocidad. En [20], emplean un PD para ajustar el valor del ICP, como se muestra en la ecuaci´on (11). No obstante en este trabajo se considera que: Kp +Kd = 1, donde Kp es la ganancia de peso para la posici´on y Kd de la velocidad; con eso se le otorga mayor prioridad a la posici´on del CoM o a la velocidad con la que se desplaza.
xICP =xcom+ ˙
xcom
w (10)
τa =Kpxcom+Kd ˙
xcom
Figura 7: Regi´on de estabilidad plano sagital robot DARwIn-OP.
Adicionalmente, el torque puede ser aproximado a una posici´on de salida en el actuador, seg´un se demuestra en [9]; de modo que la ecuaci´on (11) quedar´ıa como se muestra en (12).
∆x=Kpxcom+Kd ˙
xcom
w (12)
Donde ∆xrepresenta un valor excedente o restante de posici´on lineal sobre el actuador,
Kp y Kd las ganancias proporcional y derivativa respectivamente.
8.3.
Uso de V-REP como simulador del Robot DARwIn-OP:
Para el correcto funcionamiento de los sistemas en el mundo real, es necesario acudir a herramientas de simulaci´on, que permitan recrear los fen´omenos f´ısicos dentro de un entorno controlado, con el prop´osito de desarrollar aplicaciones y procesos de manera segura. En ese sentido, V-REP es un software de acceso libre para la simulaci´on de robots, que permite elaborar y trabajar en sistemas rob´oticos como tambi´en en procesos industriales automatizados. Permite a su vez la creaci´on de prototipos rob´oticos, como la posibilidad de utilizar algunas plataformas con las que vienen predefinidas en su paquete
de instalaci´on (ejemplo: robots NAO, KUKA, entre otros). Su modo de operaci´on se basa en una arquitectura de control distribuido, donde cada objeto dentro del entorno de desarrollo integrado (IDE), puede ser controlado mediante el uso de script’s embebidos, un cliente API remoto o una soluci´on personalizada. Los controladores pueden ser programados en lenguajes como: C/C++, Phyton, Java, Lua, MATLAB, etc.3
Figura 8: Modelo robot DARwIn-OP en V-REP- (a) Estructura din´amica. (b) Visuali-zaci´on final.
En primer lugar, el robot DARwIn-OP es una plataforma rob´otica de 20 grados de libertad dise˜nada por la empresa Robotis, cuya descripci´on detallada se encuentra en [17]. El proceso de implementaci´on en V-REP, comienza con la importaci´on de los archivos CAD y STL de cada uno de los componentes que constituyen al Robot (ejem-plo: cabeza, torso, motores, etc), los cuales est´an disponibles para uso libre en la red. Adicionalmente se incluye el sensor de medidas inerciales (IMU) compuesto por ace-ler´ometro y giroscopio, junto con los 20 actuadores (joint) configur´andolos conforme a las caracter´ısticas din´amicas de los motores Dynamixel MX-28 (velocidad nominal, resoluci´on, torque m´aximo, etc. Finalmente, se ensambla el robot con las caracter´ısticas cinem´aticas y din´amicas de la plataforma, brindadas por Robert L. Williams en [17], dando como resultado un modelo aproximado del robot real, como se aprecia en la figura 8. A su vez, se recrea el ambiente al cual va a ser sometido el robot, incluyendo una mesa con un actuador, que permitir´a inclinar la superficie donde estar´a ubicado el robot; superficie que tiene como caracter´ıstica un coeficiente de fricci´on alto (ver figura 9). Por ´ultimo, se dispone de 3 ventanas en el ambiente gr´afico (como se muestra en la figura 10), para visualizar: en A), la imagen completa del entorno; en B), la imagen de los tobillos trabajando, y en C), un gr´afico que muestra el desplazamiento del CoM del
robot (en verde) y la inclinaci´on de la tabla (en gris).
Figura 9: Modelo de estudio en V-REP- robot sobre una mesa
Figura 10: Interfaz gr´afica del ambiente de simulaci´on.
8.4.
Modelo matem´
atico del servomotor MX-28:
Tal como se expres´o anteriormente, la acci´on de control resultar´a en una posici´on en el actuador como se observ´o en la ecuaci´on (12), es necesario extraer el modelo ma-tem´atico de los actuadores del robot, con el prop´osito de controlar la rigidez con la que los actuadores realicen tales movimientos. A continuaci´on se presenta la elaboraci´on del modelo matem´atico del servomotor Dynamixel MX-28 de la empresa Robotis, ya que dicho motor conforma los 20 actuadores que posee la plataforma DARwIn-OP (ver Anexo 1, tabla 2). Los MX-28 poseen en su interior un motor DC sin escobillas (BLDC,
Figura 11: Circuito el´ectrico de un motor DC t´ıpico- sistema de un solo polo sin esco-billas.
por sus siglas en ingl´es) de la industria MAXON con referencia REmax17∅17 mm y un controlador PID digital configurado de f´abrica con un P = 32, I = 0 yD= 0. De esta manera, es posible determinar un modelo matem´atico que defina el torque del motor en t´erminos de la velocidad angular y el voltaje de entrada conforme al diagrama que se muestra en la Figura 11. Las variables θ(t), ˙θ(t) y ¨θ(t), corresponden a la posici´on,
velo-cidad y aceleraci´on angular respectivamente.v(t), i(t) y τ(t), corresponden al voltaje de
entrada, la corriente de inducci´on y el torque de salida respectivamente. Finalmente R,
L, e, Kr, B y J, corresponden respectivamente a la resistencia de inducci´on, la induc-tancia, la fuerza electromotriz (FEM), constante de relaci´on de reducci´on , el coeficiente de fricci´on viscoso y el momento de inercia. En primer lugar, se establecen las ecuacio-nes el´ectricas y mec´anicas del sistema descrito en la figura 11, bajo la ley de Kirchhoff de voltaje (LKV) y la primera ley de newton (ecuaci´on (13) y (15) respectivamente).
v(t) =vR+vL+vf em (13)
vf em=e=KeKrθ˙(t) (14)
τ =τf +τl (15)
τ =KtKri(t) (16)
troque respectivamente. En ese sentido, el sistema del motor se puede expresar en una funci´on de transferencia que relacione una salida (θ, ˙θ o ¨θ) con la entradav(t), de modo
que la funci´on de transferencia en el dominio de la frecuencia es como se muestra en (17). El procedimiento matem´atico completo se puede encontrar en el Anexo 2-procedimiento 2 y las constantes del motor se encuentran consignadas en el Anexo 1 tabla (3).
˙
θ(s)
v(s)
= KtKr/J L
s2+ (R L +
B J)s+ (
RB J L +
KeKtKr2
J L )
(17)
El modelo en el espacio de estados de la ecuaci´on (17), se obtiene a˜nadiendo un inte-grador, quedando como se muestra en la ecuaci´on (18).
d dt θa ˙ θa ¨ θa =
0 1 0
0 0 1
0 −RB J L +
KeKtKr2
J L − R L + B J θa ˙ θa ¨ θa + 0 0 KtKr
J L
V (18)
Y = [1 0 0] θa ˙ θa ¨ θa (19)
Finalmente se le a˜nade el controlador PID, que viene dentro de las caracter´ısticas no-minales del motor cuya funci´on de transferencia, es como se muestra en la ecuaci´on (20).
HP ID(s) =kp+
ki
s +kds (20)
8.5.
Dise˜
no del modelo de control:
Para alcanzar el objetivo principal de este trabajo, el modelo de control que se va a implementar, est´a enfocado en el uso de un controlador PID dise˜nado mediante t´ecnicas de control ´optimo. El prop´osito es mantener la proyecci´on del CoM dentro de la regi´on de soporte, mediante el control de posici´on en los actuadores del tobillo. Para ello se utiliza la teor´ıa de control ´optimo basado en el dise˜no de reguladores cuadr´aticos
lineales (LQR) para la obtenci´on de los valores de las ganancias en el controlador PID. Dicho controlador, es aplicado en los motores MX-28 que se encuentran localizados en los tobillos del robot. La idea propuesta se encarga de la generaci´on de rigidez variable mediante la optimizaci´on del balance entre la energ´ıa y el error de posici´on en el actuador. La modificaci´on de la rigidez se hace con el prop´osito de regular la aceleraci´on del movimiento de los actuadores, y a su vez mantener el CoM dentro de una regi´on que permita el equilibrio del robot.
En [11], se explica que la variaci´on de la rigidez en los actuadores se alcanza mediante la multiplicaci´on de la matriz de penalizaci´on Q (error en las variable de estado), por un factor (ρ) que depender´a del error en las trayectorias del sistema, en este caso de xcom. Una vez obtenida la nueva matriz Q, se recalcula el sistema LQR, que dar´a como resultado un cambio en las ganancias PID de los motores, alcanzando la posici´on deseada con un movimiento en los tobillos en presencia de una mayor o menor rigidez. En las secciones posteriores se explicar´a en detalle cada proceso. Habiendo mencionado lo anterior, el sistema de control para este trabajo, est´a estructurado de la siguiente manera (ver tambi´en figura 12):
1. Lectura de los datos de la unidad de medidas inerciales (IMU) del robot, c´alculo de la posici´on y de la velocidad del CoM.
2. C´alculo del ZMP a trav´es del ICP para la estrategia del tobillo, estimaci´on de la nueva posici´on de los actuadores del tobillo y valor para el factor ρ del control PID ´optimo.
3. Implementaci´on del LQR y c´alculo del PID ´optimo para los servomotores MX-28.
8.5.1. Lectura de la IMU, c´alculo de la posici´on y la velocidad del CoM del Robot:
Figura 12: Sistema de control de balance - diagrama procedimental.
Es por eso que para determinar el ´angulo de inclinaci´on por medio de un dispositivo IMU, se suele utilizar el filtro complementario y/o el filtro de Kalman. En ese sentido, se utiliza conjuntamente un filtro complementario (FCMP) y seguidamente un filtro de Kalman (FK), para determinar los valores de xcom y ˙xcom de manera apropiada.
Para la implementaci´on del filtro complementario es necesario obtener la inclinaci´on dada por el aceler´ometro y por el giroscopio, donde se considera lo siguiente:
θ= (0,98)(θprevio+θGyro) + (0,02)(θAccel) (21)
θGyro = (Gyro[y])(dt) (22)
θAccel =atan2
Accel[y]
Accel[z]
(23)
Donde Gyro y Accel contienen los valores de medida para cada eje, en rad s y en
m s2
respectivamente. θGyro y θAccel el ´angulo de inclinaci´on dado por el giroscopio y el aceler´ometro, y θ el ´angulo de inclinaci´on del robot (Figura 5). Una vez obtenido el ´
angulo de inclinaci´on del robot, se conoce la posici´on lineal del centro de masa con filtro complementario (xcomCF), mediante la expresi´on (24):
xcomCF =sin(θ)·zcom (24)
Con el valor delxcomCF se procede a realizar una estimaci´on m´as precisa de las variables de estado (xcom,x˙com) mediante el uso del LIPM y el filtro de Kalman, a partir de la ecuaci´on de estados que se desprende de la expresi´on (8) previamente mencionada y se muestra en la ecuaci´on (25-29).
˙
x(t) =Ax(t)+Bu(t) (25)
y(t) =Cx(t) (26)
x(t) = [xcom x˙com]T (27)
" ˙ xcom ¨ xcom # = " 0 1 0 w2
# " xcom ˙ xcom # + " 0
−zg com
#
δ± (28)
y= [1 0] " xcom ˙ xcom # (29)
Finalmente se establece la matriz de covarianzaP, que contiene los errores de estimaci´on del filtro de Kalman, la ganancia de Kalman KG y el vector de estimaci´onxb, de modo que el filtro de Kalman queda como se muestra en las ecuaciones (30-35)
b
x=Axprevia+Bu (30)
P =APpreviaAT +Q (31)
KG=
P
P +R (32)
x=xb+KG(Y −bx) (33)
xprevia =x (34)
Pprevia = (I −KG)P (35)
experimental. Por otra parte, I es la matriz identidad, y contiene el valor medido del CoM (xcomCF) yxel vector de estados que contienexcom y ˙xcomdefinitivos. En la figura 13, se muestra el gr´afico comparativo del desplazamiento del CoM (xcom) al inclinar el robot 7 grados hacia adelante y 7 hacia atr´as (±0,03 m). La gr´afica muestra el xcom obtenido por el filtro complementario en rojo y por el filtro complementario m´as el de Kalman, en azul.
Figura 13: Gr´afica del desplazamiento del CoM en el eje x.
8.5.2. C´alculo del ZMP, ICP y estimaci´on de la posici´on deseada del CoM:
En la secci´on 8.2 se explic´o de manera generalizada los procedimientos necesarios para el c´alculo del ZMP a partir del ICP. No obstante, a´un se desconoce el valor de la posici´on deseada del actuador que reubicar´a el CoM en una zona de estabilidad. En ese sentido, a la ecuaci´on (12) se le agrega el CoM deseado (xcomD), donde el ´angulo de inclinaci´on es 0, es decir, cuando el robot se encuentra totalmente erguido -Ver Figura 5-; de este modo xcomD = 0. Por otra parte, Kp y Kd se establecen con valores de 0.7 y 0.3 respectivamente, para penalizar m´as la velocidad que la posici´on. Finalmente, la posici´on final de los actuadores estar´a determinada por un error de posici´on entre el valor del xcomD, el valor del xICP y el valor presente del actuador (θactual) tal y como se muestra en la ecuaci´on (36), donde ∆x es el error de posici´on delxcom.
θa=θactual−asin
∆x zcom
(36)
∆x=xcomD−xICP (37)
Gr´aficamente se puede observar lo mencionado anteriormente en la figura 14.
Figura 14: Gr´afica de las trayectorias del xICP, xcom, xcomD y ∆x.
Finalmente, el valor del factor ρ est´a determinado por la relaci´on entre el error de la posici´on del xcom -ecuaci´on (37)- y el limite m´aximo en la regi´on de soporte δ± como se muestra en la ecuaci´on (38).
ρ= |∆x|
δ± ; (38)
De este modo se garantiza que ρ tendr´a valores entre 0 y 1 con relaci´on al error de posici´on del CoM.
8.5.3. Control PID ´optimo:
controla-PID de los actuadores, y mediante la variaci´on de la rigidez de ellos, mantener el CoM dentro de la regi´on de estabilidad. En ese sentido, se requiere implementar el sistema LQR con la planta del motor (ecuaci´on (18)). El sistema LQR procura minimizar el error en las trayectorias de las variables de estado de un sistema, haciendo uso de una m´ınima cantidad de energ´ıa en la planta a controlar; ´esto mediante la minimizaci´on del ´ındice de rendimiento J que se muestra en la expresi´on (39).
J = Z ∞
0
[xT(t) Q x(t)+uT(t) R u(t)]dt (39)
Donde Q es la matriz de penalizaci´on para el error en las variables de estado, y R
la matriz de penalizaci´on para la se˜nal de control. La relaci´on entre las matrices de penalizaci´onQy R permite determinar la proporci´on m´as significativa entre la energ´ıa del controlador y el error de estados. De esta manera la variaci´on de la rigidez se consigue modificando la matrizQ, donde para valores deQgrandes, el error de posici´on admisible se minimiza, traduci´endose en una m´axima rigidez; por otra parte si Q tiene valores peque˜nos, el error de posici´on permitido aumenta y la rigidez de los actuadores se reduce; es aqu´ı donde el factor ρmencionado previamente, permite modificar la matriz
Q. La ecuaci´on que define la ley de control por realimentaci´on en un sistema LQR, se define por la ecuaci´on (40).
u(t) =−Kx(t) =−R−1BTP x(t) (40)
DondeK es la matriz de ganancias de realimentaci´on definida porK =R−1BP, siendo
P una matriz sim´etrica definida positiva obtenida mediante la soluci´on algebraica de la ecuaci´on de Riccati, como se muestra en la ecuaci´on (41).
ATP +P A−P BR−1P +Q= 0 (41)
DondeAyBson las matrices de estados de la planta, en este caso la del motor (ecuaci´on (18)) que en t´erminos pr´acticos, A y B se pueden ver de la forma descrita en (42). De este modo, lo primordial es determinar una matriz Q usual o nominal, donde el motor presente una rigidez alta (P = 32, I = 0, D= 0), para posteriormente utilizar el factor
nuevaQrecalcular la ecuaci´on de Riccati para la matrizP, obteniendo as´ı las ganancias de realimentaci´onK y con ello el desarrollo del sistema de control.
A =
0 1 0 0 0 1 0 −b −a
B =
0 0 −c (42)
De tal manera, la matriz Q nominal se calcula a partir de los polos deseados de la ecuaci´on polinomial del motor, tal y como se explica en [11], donde la ecuaci´on que describe el comportamiento deseado, es una ecuaci´on de tercer orden que contiene 3 ra´ıces llamadas −α1, −α2 y −α3. Seg´un el m´etodo propuesto en [11], la matriz P y Q
est´an definidas de la forma descrita en la expresi´on (43).
P =
p11 p12 p13
p21 p22 p23
p31 p32 p33
Q=
q1 0 0
0 q2 0
0 0 q3
(43)
Donde los valores de Q pueden ser obtenidos a partir de los coeficientes de la funci´on de transferencia de la planta, y de los polos de la ecuaci´on caracter´ıstica polinomial deseada, como se describe en las ecuaciones (44-46).
q1 =
Rα2 1α22α23
c2 (44)
q2 =
R(α21α22+α21α32+α32α22−b2)
c2 (45)
q3 =
R(α2
1+α22+α23−a2−b2)
c2 (46)
Figura 15: Sistema PID ´optimo para el control de balance del robot DARwIn-op
9.
IMPLEMENTACI ´
ON Y RESULTADOS
EXPERIMENTALES
En esta secci´on se muestra la implementaci´on tanto en el simulador como en la platafor-ma DARwIn-OP real del modelo de control realizado previamente. Posteriormente se eval´ua el rendimiento del controlador mediante el an´alisis del desplazamiento del CoM frente a distintas inclinaciones en la superficie de contacto, con el prop´osito de validar el comportamiento del sistema de control y as´ı realizar las conclusiones pertinentes del presente trabajo.
9.1.
Simulaci´
on del sistema de control de balance:
Para que el robot no caiga y mantenga su equilibrio, es necesario proyectar el CoM del robot dentro de la superficie de contacto; una vez la posici´on del CoM deje dicha regi´on, el robot pierde su equilibrio y cae. De esta manera, el prop´osito del sistema de control es mantener esa proyecci´on del CoM dentro de la regi´on de soporte; por esta raz´on, el objetivo de la simulaci´on es monitorear las trayectorias del CoM, para evaluar la estabilidad que presenta el robot, manteniendo su equilibrio frente a distintas inclinaciones en su superficie de contacto, y a su vez, validar el modelo LIPM, como modelo que describe el comportamiento del robot b´ıpedo. Esto, a trav´es de verificar lo que ocurre si el CoM supera los l´ımites de soporte del robot y sale de la regi´on de estabilidad (figura 7) o qu´e ocurre si esa proyecci´on permanece dentro de la regi´on de estabilidad.
controlador. Posteriormente en la figura 19, se encuentran las gr´aficas obtenidas al implementar el controlador frente a las perturbaciones±5,±9 y±15 grados a velocidad constante, mostrando la permanencia del CoM dentro de la regi´on de estabilidad y con valores cercanos al xcomD. Todas las gr´aficas anteriores muestran los l´ımites de la regi´on de soporte, el xICP, el xcom, el xcomD, el punto de no retorno y la inclinaci´on de la tabla. Todas bajo las siguientes concesiones: l´ınea discontinua horizontal roja, l´ınea verde, l´ınea azul, l´ınea discontinua negra, l´ınea vertical discontinua roja y l´ınea magenta respectivamente.
Figura 16: Ca´ıda del Robot- a), b): +15 grados, y c), d): -15 grados
Figura 17: Gr´aficas CoM sin control - a) ±5 grados, b) ±9 grados.
A partir de la informaci´on obtenida durante el proceso de implementaci´on en el simula-dor, se extraen los valores m´aximos y m´ınimos delxcom presentes en cada perturbaci´on (±5, ±9 y ±15 grados), esto tanto en presencia del controlador, como en ausencia del mismo. La extracci´on de esos valores permite determinar el porcentaje de correcci´on del controlador evaluando los desplazamientos del CoM, como se muestra en la tabla 1.
´
Angulos
5 -5 9 -9 15 -15
xcomSinControl 0,02318 m -0,0235 m 0,0414 m -0,0417 m 0,0420 m -0,0629 m
xcomControl 0,0026 m -0,0041 m 0,0026 m -0,0041 m 0,0025 m -0,0041 m % reducido 88,4 82,1 93,7 90 93,9 93,4
Tabla 1: Valores xcom m´aximos y minimos durante la presencia y la ausencia del con-trolador - Porcentajes de reducci´on.
Posteriormente a la validaci´on de la implementaci´on del modelo de control, se grafi-can los valores de las ganancia PID entregadas por el regulador LQR, para apreciar el comportamiento din´amico de dichas ganancias en presencia de las perturbaciones, donde se observa por ejemplo, que la ganancia proporcional (Kp) aumenta cuando la perturbaci´on es grande y disminuye cuando la perturbaci´on es peque˜na - ver figura 20-. Por otro lado la ganancia integral (Ki) usualmente obtiene valores menores a 1.
9.2.
Implementaci´
on en la plataforma DARwIn-OP:
Para la implementaci´on del sistema de control en el robot DARwIN-op, se construye una base m´ovil donde ser´a puesto el robot a prueba, permitiendo inclinar su base en el plano sagital. En la figura 21, se muestra la plataforma construida, y en la figura 22 se aprecia el robot dentro de la base experimental. Por otra parte, se realiza la
Figura 21: Plataforma de pruebas para la inclinaci´on del robot real.
migraci´on del controlador dise˜nado en MATLAB a lenguaje C , con el prop´osito de poder implementar el sistema en el robot. Una vez obtenido el algoritmo dise˜nado en el robot, lo primero es realizar pruebas iniciales para validar la regi´on de estabilidad calculada previamente en simulaci´on. En la figura 23 se aprecia la secuencia del robot cuando cae al mover la tabla a valores cercanos a 10 grados, y en la figura 24 se aprecia la gr´afica de CoM correspondiente a dicha ca´ıda, evidenciando que el punto de no retorno se encuentra justo cuando el CoM coincide con el l´ımite de la regi´on de estabilidad calculada previamente. A su vez, en la figura 17 se muestra el comportamiento del CoM, en ausencia del controlador cuando su base se inclina a±5 grados.
Figura 22: Plataforma de pruebas experimental con DARwIn-OP.
Finalmente se implementa el controlador y se grafican las trayectorias del CoM regis-tradas por el robot (figura 27), permitiendo apreciar la permanencia del CoM dentro de
su regi´on de estabilidad frente a inclinaciones de (±5,±9 y ±15 grados), iguales a las realizadas en simulaci´on; cabe resaltar que las gr´aficas resultantes de este experimento, est´an propensas a las condiciones inestables de la base construida para las pruebas, co-mo por ejemplo la velocidad de co-movimiento de la tabla y la imprecisi´on del movimiento giratorio de la base. A´un as´ı se aprecia el correcto funcionamiento del sistema de control frente a esas limitaciones.
Figura 23: Secuencia de ca´ıda frontal del Robot DARwIn-OP Real.
Figura 25: Gr´afica del CoM sin control con ±5 grados en robot DARwIn-OP Real.
Figura 26: Secuencia del robot DARwIn-OP Real en presencia del control.
10.
CONCLUSIONES Y TRABAJOS FUTUROS
En este cap´ıtulo, se presentan las conclusiones pertinentes frente a los resultados obteni-dos durante el dise˜no e implementaci´on del sistema de control de balance para el robot DARwIn-OP, resaltando tambi´en mejoras y trabajos futuros que podr´ıan realizarse a partir de lo desarrollado en este trabajo.
10.1.
Conclusiones:
El uso de un modelo matem´atico como el del LIPM para describir la din´amica de movimiento en robots b´ıpedos, es una buena alternativa para interpretar y dise˜nar sistemas de control. En la secci´on 8.1 se valid´o el modelo LIPM, al corroborar que si las proyecciones del CoM provenientes del modelo LIPM superan los l´ımites de la regi´on de estabilidad, el robot cae y en el caso contrario, ´este permanecer´a erguido. Lo que significa que los conceptos te´oricos del LIPM como modelo para robots b´ıpedos, fueron validados mediante resultados experimentales en simulaci´on y en la pr´actica, al analizar las trayectorias del CoM. (seccion 9.1).
Por otra parte, se dise˜n´o satisfactoriamente un modelo de control, basado en los concep-tos de la teor´ıa de control ´optimo (seccion 8.5), al utilizar el regulador cuadr´atico lineal (LQR) para determinar las ganancias del controlador PID de los actuadores, teniendo como referencia el error en la posici´on del xcom, xcomD y la posici´on de los actuadores, para llevar el CoM dentro de la regi´on de soporte.
Adicionalmente, se implement´o el controlador dentro del ambiente de simulaci´on di-se˜nado en V-REP, permitiendo observar el correcto funcionamiento del controlador, para mantener las trayectorias del CoM frente a distintas perturbaciones en la base de soporte del robot, y de esa manera validar el comportamiento del modelo de control, observando que la posici´on del CoM permanece dentro de la regi´on de estabilidad frente a distintas inclinaciones de la superficie de contacto, inclusive ante las perturbaciones m´as cr´ıticas, como cuando la base alcanza los 15 grados de inclinaci´on.
Posteriormente se implement´o el sistema de control en la plataforma rob´otica DARwIn-OP y se evalu´o el comportamiento de la proyecci´on del CoM frente a inclinaciones en su superficie de contacto, pudi´endose apreciar el comportamiento esperado, al mantener elxcom constantemente dentro de la regi´on de estabilidad, corroborando as´ı el correcto
funcionamiento del controlador en la plataforma real y permitiendo extraer las siguientes apreciaciones puntuales: el sistema de control funciona correctamente para inclinaciones comprendidas entre los +15 grados y -15 grados, ya que m´as all´a de esos valores, el robot comienza a deslizarse sobre su superficie de contacto; adem´as, seg´un los valores obtenidos en simulaci´on mostrados en la tabla 1, se concluye que el controlador reduce en un 90 % promedio las perturbaciones del sistema. Por otra parte, el sistema no solo permite perturbaciones con velocidad constante, sino que experimentalmente se pudo observar que frente a cambios en la velocidad de movimiento de la base, el robot controla aceptablemente su posici´on (los resultados de ese experimento no fueron considerados dentro de los objetivos del presente documento).
Finalmente el objetivo principal de este trabajo se cumple satisfactoriamente, ya que se dise˜n´o e implemento un sistema de control de balance en la plataforma DARwIn-OP y se expuso el robot frente a perturbaciones de inclinaci´on en su superficie de contacto, observando el comportamiento del controlador al mantener las proyecciones del CoM dentro de su regi´on de estabilidad, conservando as´ı su posici´on de erguido y a trav´es de un equilibrio est´atico.
10.2.
Trabajos futuros:
Antes de mencionar los trabajos a futuro, es necesario se˜nalar las siguientes conclusiones adicionales que enmarcan los factores a mejorar:
El modelo del LIPM puede ser notablemente m´as robusto si se le a˜nade otro grado de libertad para incluir otras estrategias de balance, como por ejemplo el modelo lineal del p´endulo invertido m´as el volante de inercia (LIPM+FlyWheel por sus siglas en ingl´es) descrito en [20].
El sistema de control puede ser complementado mediante la adici´on de la estrate-gia de la cadera, como la mencionada en la secci´on 6.3, permitiendo soportar un amplio rango de perturbaciones, inclusive distintas a la inclinaci´on de la superficie de contacto (ejemplo: golpes o colisiones).
velo-cidad con la que se presentan las perturbaciones, con el prop´osito de obtener las limitaciones del controlador de manera m´as amplia y precisa.
Como trabajos futuros se establecen los siguientes:
Incluir la estrategia de la cadera para aumentar la estabilidad del robot frente a otro tipo de perturbaciones. Adicionalmente incluir no solo el plano sagital, sino tambi´en el plano coronal, para poder establecer un sistema de control de balance m´as completo en el robot.
Comparar el sistema de control propuesto en este trabajo frente a otros algoritmos existentes, como por ejemplo sistemas de control difuso, sistemas de aprendizaje de m´aquinas, etc. con el prop´osito de adquirir factores adicionales en pro de mejorar el sistema de control de balance en robots humanoides.
11.
ANEXOS
En esta secci´on se ubican los diferentes procedimientos matem´aticos, ecuaciones y dia-gramas auxiliares mencionados previamente durante el desarrollo del trabajo.
11.1.
Anexo 1:
Descripci´on Valor 1 Numero de actuadores 20 2 Altura del robot 0.455 m
3 Peso 2.89 Kg
4 Altura del Centro de masa 0.25 m
5 Dimensiones planta del pie largo 0.104 m, ancho 0.066m
Tabla 2: Tabla de datos - DARwIn-OP.
Descripci´on S´ımbolo Unidad Valor 1 Voltaje nominal v(t) V 12
2 Caja de reducci´on Kr – 193
3 Constante de Torque Kt Nm 9.4348e-04 4 Constante FEM Ke Vs/rad 0.0099 5 Resistencia de inducci´on R Ohm 8.3
6 Inductanica L H 2.06e-4
7 Coeficiente de fricci´on biscosa B Nms/rad 0.0051 8 Momento de inercia J Kgm2 0.0034
Tabla 3: Tabla de datos - motor MX-28.
11.2.
Anexo 2:
En ´este apartado se consignan los diferentes procedimientos matem´aticos utilizados durante el desarrollo del presente trabajo.
Procedimiento 1:
PE =−m∗g∗h
PE =−m∗g∗zcom∗cos(θ)
KE = 12 ∗m∗v2
Donde v es la velocidad y para el sistema espec´ıfico es la velocidad angular:
v =zcom∗θ˙
KE = 12 ∗m∗z2com∗θ˙2
L= 12m∗z2
comθ˙2+mgzcomcos(θ)
τa= dxd
∂L
∂θ˙
− ∂L
∂θ ∂L
∂θ˙ =m∗z
2
com∗θ˙ d dx ∂L ∂θ˙
=m∗z2
com∗θ¨ ∂L
∂θ =−m∗g∗zcomsen(θ)
τa=mzcom2 θ¨+mgzcomsen(θ)
Sabiendo que para valores de θ muy peque˜nos sen(θ)≈ θ entonces decimos que:
τa=mzcom2 θ¨+mgzcomθ
Procedimiento 2: Sabiendo que:
v(t) =vR+vL+vf em
vf em =e=KeKrθ˙(t)
Y que:
τ =τf +τl
τ =KtKri(t)
Entonces:
v(t) =R∗i(t)+L
di(t)
dt +KeKrθ˙(t)
KtKri(t)=Bθ(˙t)+Jθ¨(t)
De esa manera decimos que:
i(t) = Kt1Kr(Bθ˙(t)+Jθ¨(t))
Ahora calculando Ldi(t)
dt se obtiene que:
Ldi(t)
dt = L KtKr(B
¨
θ(t)+J
...
θ(t))
reemplazando la expresion anterior y agrupando terminos se obtiene lo siguiente:
v(t)KtKr = ˙θ(t)(RB+KeKtKr2) + ¨θ(t)(RJ +LB) +
...
θ(t)(LJ)
De este modo, se aplica trnasformada de la plaz y se expresa todo en t´erminos de
˙
θ(s)
˙
θ(s)
v(s) =
KtKr/J L
s2+(R L+
B J)s+(
RB J L+
KeKtKr2
12.
BIBLIOGRAF´
IA
Referencias
[1] Ahmed I Elhasairi.Humanoid Robot Full-Body Control, Balance Restoration Junio 2015.
[2] Alcaraz-Jimenez, J., Herrero-P´erez, D. Mart´ınez-Barber´a, H. (2013).Robust feed-back control of ZMP-based gait for the humanoid robot Nao. The International Journal of Robotics Research.
[3] Alcaraz-Jimenez, 2012, J., Marcell, M., Behnke, S. (2012). Lateral Disturbance Rejection for the Nao Robot. Lecture Notes in Artificial Intelligence, RoboCup 2012.
[4] DARPA 2015. DARPA Robotics Challenge. Obtenido de DARPA Robotics Cha-llenge: http://www.theroboticschallenge.org/
[5] Europe, S. R. (Febrero de 2015). Robotics 2020 Multi-Annual Roadmap For Robotics in Europe. SPARC. Obtenido de (http://sparc-robotics.eu/wp-content/uploads/2015/02/Multi-Annual-Roadmap2020-ICT-24-Rev-B-full.pdf.
[6] Fogel, Karl.Producing Open Source Software, How to Run a Successsful Free Soft-ware Project Karl Fogel. 2005.
[7] Georgia , I., Massachusetts, I. (2013). A Roadmap for U.S. Robotics From Internet to Robotics. Robotics in The United State of America.
[8] Goswami,Postural Stability of Biped Robots and the Foot-Rotation Indicator (FRI) Point,1999.
[9] Inyong Ha, Yusuke Tamura, and Hajime Asama, Gait Pattern Generation and Stabilization for Humanoid Robot Based on Coupled Oscillators, 2011.
[10] Juan M. Llofriu, Martin, Moreno Wilfrido, and Weitzenfeld Alfredo,Soft landing in jumping robot using compliant motor capability, Worhshop Get in touch! Tactile Force Sensing for Autonomous, Compliant, Intelligent Robots, ICRA 2015, May 26th-30th, 2015 Seattle, Washington (U.S.A.)
[11] Juan M., Moreno Wilfrido, and Weitzenfeld Alfredo.Impact Force Reduction Using uzzy Variable Stiffness in a Jumping Robot, Journal of Network and Innovative Computing 4, no. 2016: 152-163.
[12] Kajita, 2001, S., Yokoi, K., Kazuo , T. (2001).Balancing a Humanoid Robot Using Backdrive Concerned Torque Control and Direct Angular Momentum Feedback, International Conference on Robotics Automation. Seoul, Korea: IEEE.
[13] Luis Maria Cab´as Ormaechea, Mecatr´onica bioinspirada de robots humanoides de tama˜no natural, Junio 2009.
[14] Meira, 2005. Model-based control of a one-dimensional pendulum actuated with Pleated Pneumatic Artificial Muscles with adaptable stiffness. Vrije Universiteit Brussel.
[15] Missura, Behnke, S. (2013). Self-stable Omnidirectional Walking with Compliant Joints. International Conference on Humanoid Robots. Atlanta, GA.
[16] Morisawa, 2009, M., Harada, K., Kajita, S., Kaneko, K., Yoshida, E., Yokoi, K., Laumond, J.-P. (2009). Reactive Stepping to Prevent Falling for Humanoids. In-ternational Conference on Humanoid Robots. Paris, France: IEEE.
[17] Robert L. Williams II, DARWIN-OP Humanoid Robot Kinematics, 2012.
[18] RoboCup, 2015. RoboCup. Obtenido de http://www.robocup.org/
[19] Sebastian KiemelBalance maintenance of a humanoid robot using the hip-ankle strategy 2012.
[20] Seung-Joon Yi , Byoung-Tak Zhang, Dennis Hong, Daniel D. LeeOnline Learning of Low Dimensional Strategies for High-Level Push Recovery in Bipedal Humanoid Robots, 2013.
[21] Takashi, Kajita, 2004, S., Kaneko, K. (2004). A Running Experiment of Humanoid Biped. International Conference on Intelligent Robots and Systems. Sendai, Japan: IEEE.
