INSTITUTO POLITÉCNICO NACIONAL

Escuela Superior de Ingeniería Mecánica y Eléctrica Unidad Culhuacán

Sección de Estudios de Postgrado e Investigación

CONTROL DE ROBOTS MANIPULADORES EN ESPACIO DE TAREA: APLICACIÓN EN EL CONTROL DE POSICIÓN Y ORIENTACIÓN

Tesis que presenta el:

Ing. Guillermo Trinidad Sánchez

Para obtener el grado de Maestro en Ciencias en Microelectrónica

Director de Tesis:

Dra. Ilse Cervantes Camacho .México D.F.

2004

Agradecimientos

A la Dra. Ilse Cervantes por su asesoría y apoyo durante la realización de esta tesis.

Al Instituto Politécnico Nacional y A la Sección de Estudios de Posgrado e

Investigación de la ESIME Unidad Culhuacán por la formación que me han brindado.

Al CONACYT por el apoyo que me ha otorgado

A mis sinodales por sus valiosos comentarios sobre esta tesis.

A mis amigos y compañeros por su apoyo durante la estancia en la Sección

de Postgrado de ESIME Culhuacàn.

Nomenclatura ⋅ Norma euclidiana

⋅ Norma inducida

)(xfx∇ Gradiente de la función f(x)

TA Transpuesto de A

)(max Aλ Máximo valor propio de la matriz A

)(min Aλ Mínimo valor propio de la matriz A

KD Ganancia derivativa

Kp Ganancia proporcional

KI Ganancia integral

)(qM Matriz de inercias del robot manipulador

),( qqC & Matriz de Coriolis y fuerzas centrípetas

)(qg Vector de pares gravitacionales

)(ˆ qg Vector de pares gravitacionales estimados

y~ Vector de errores de salida

q Vector de posiciones articulares del robot manipulador

q& Vector de posiciones articulares del robot manipulador

Índice General Resumen y organización de la tesis .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .5 1 Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.1 Motivación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2 Planteamiento del problema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.3 Objetivo Principal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

1.3.1 Objetivos Particulares . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

1.4 Relevancia. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2 Descripción del Espacio de Tarea . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.1 Motivación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2 Descripción de Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .11

2.2.1 Coordenadas Cartesianas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.2.2 Coordenadas Cilíndricas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.2.3 Coordenadas Generalizadas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.3 Descripción de Orientación. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .17

2.3.1 Matriz de Rotación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.3.2 Ángulos de Euler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .18

2.3.3 Representación Eje-Ángulo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .20

2.3.4 Cuaterniones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

2.4 Transformación Homogénea. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .23

2.5 Problemas de Cinemática Directa y Cinemática Inversa. . . . . . . . . . . . . . . . . . . . 25

3 Modelo Dinámico. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.1 Formulación Euler Lagrange . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.2 Propiedades de la ecuación de movimiento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

3.2.1 Propiedades de la Matriz de Inercia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

3.2.2 Propiedades del Vector de Coriolis y de Fuerzas Centrípetas . . . . . . . . . . .32

3.2.3 Propiedades del Vector de Gravedad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4 Resultado Principal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.1 Planteamiento del Problema. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.2 Antecedentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

4.3 Control PD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4.4 Control PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .46

5 Simulaciones Numéricas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .48

6 Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

Apéndice A Visual Servoing . . . . . . . . . . . . . . . . .. . . . . . .. . . . . . . . . . . . . . . . . . . . . . . 59

A.1 Modelo de la Cámara. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

A.2 Proyección Perspectiva. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .58

A.3 Distorsiones Geométricas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

A.4 Cambio de Coordenadas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

A.5 Error de Posicionamiento de Imagen. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .61

Apéndice B Sistemas Singularmente Perturbados. . . . . . . . . . . . . . . . . . . . . . . . . . . . .65

Apéndice C Modelo Dinámico de una Muñeca Esférica. . . . . . . . . . . . . . . . . . . . . . . .68

Apéndice D Modelo del Robot en Simulink. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

Índice de Figuras

2.1 Sistema Cartesiano . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.2 Robot de geometría cartesiana . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.3 Sistema Cilíndrico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.4 Robot de geometría cilíndrica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .13

2.4 Robot de geometría esférica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .13

2.6 Robot SCARA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2.7 Robot Antropomórfico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.8 Sistemas coordenados en robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .22

2.9 Transformación Homogénea. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

2.10 Cinemática Directa y Cinemática Inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .30

5.1 Muñeca esférica robótica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .38

5.2 Diagrama a bloques del controlador PD. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

5.3 Desempeño del controlador PD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .53

5.4 Diagrama a bloques del controlador PD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .54

5.5 Desempeño del controlador PID con KP=diag{1,1,1,1}. . . . . . . . . . . . . . . . . . . . . 55

5.6 Desempeño del controlador PID con KP=diag{2,2,2,2}. . . . . . . . . . . . . . . . . . . . . 56

5.7 Desempeño del controlador PID con KP=diag{10,10,10,10}. . . . . . . . . . . . . . . . . 57

Resumen y organización de la tesis

Esta tesis trata el problema de control de posición y orientación de robots manipuladores en

espacio de tarea. En la industria, el control de robots manipuladores es de tipo Proporcional

Integral Derivativo (PID) en coordenadas generalizadas, debido principalmente a su

sencillez; sin embargo, en lo que se refiere a la aplicación de control PID en espacio de

tarea hasta hace muy poco tiempo no se había llevado aplicación alguna, de hecho fue poco

después que se inició esta tesis, que apareció una aplicación en espacio de tarea,

específicamente en visual servoing, realizada por (Martinez, 2002). El hecho que motiva

esta tesis es el de extender los resultados del control en coordenadas articulares al de las

coordenadas en espacio de tarea, así como explicar las limitaciones existentes en esta

extensión.

Para solucionar el problema de regulación de posición y orientación, en esta tesis se partió

de la metodología de modelado de energía mas inyección de amortiguamiento1 (vea por

ejemplo Miyazaki y Arimoto, 1985; Kelly, 1999) en estos trabajos se presenta un

controlador Proporcional Derivativo (PD) basado en Jacobiano transpuesto, al cual se

añadió una acción integral en esta tesis. Específicamente se ha probado que la aplicación de

acciones integrales lentas elimina el sesgo (Alvarez-Ramírez, et al., 2000) debido a

posibles incertidumbres en el modelo del robot por el conocimiento inexacto de los pares

gravitacionales. Los resultados obtenidos se han ilustrado mediante simulaciones numéricas

en una muñeca mecánica de 3 grados de libertad.

La estructura en la conformación de la tesis es la siguiente:

En la Introducción se explica con detalle la motivación de la tesis, se plantea de forma

general en qué consiste el problema de control, cual es su relevancia y se detalla cada uno

de los objetivos particulares a cumplirse.

1 Energy shaping plus damping injection

En el Capítulo 2 se describe de manera extensiva en qué consiste el espacio de tarea y

cuáles son las opciones de representarlo (coordenadas cartesianas, visuales, etc.). Se explica

a detalle cada una de estas representaciones y se establecen sus respectivas ventajas y

desventajas. Se explica además, cuál de estas representaciones es más conveniente en la

solución del problema y su justificación.

En el Capítulo 3 se describe el modelo dinámico del robot y se describen las propiedades de

los elementos que conforman el modelo dinámico.

En el Capítulo 4 se describe la aportación principal de esta tesis, a saber, condiciones para

la estabilidad del robot bajo una acción PID en espacio de tarea. Se plantea el desarrollo

matemático de la ley de control propuesta, analizando el sistema en lazo cerrado y se

derivan guías de sintonizado.

El Capítulo 5 ilustra los resultados obtenidos mediante simulaciones numéricas. En este

capítulo se observa de manera gráfica el desempeño del controlador propuesto y los efectos

en la estabilidad del sistema de las ganancias del controlador.

En el Capítulo 6 se discuten los resultados obtenidos en esta tesis, es decir, cuáles son las

implicaciones de los resultados obtenidos, el impacto de éstos, además de detallar cuál ha

sido la principal aportación en el campo de control no lineal. En esta parte también se habla

del trabajo a futuro que se puede desarrollar a partir del trabajo de esta tesis.

Abstract

This thesis deals with the control problem of robots manipulators position and orientation

on task space. The application of robots manipulators is of the PID (Proportional Integral

Derivative) kind on generalized coordinates, because of its simplicity; however PID control

hadn’t been applied yet until recent time. In fact, the principal motivation is to extend the

results from the control in generalized coordinates to the control in task space.

Finding a solution to the position and orientation problem, the Energy shaping plus

damping injection is used. It has been probed that the application of slow integral actio ns

helps to reduce the offset caused by uncertainties in the robot model.

8

Capítulo 1

Introducción

1.1 Motivación

Recientemente se han desarrollado numerosas técnicas de control dentro del campo de la

teoría del control moderno. Sin embargo, en el uso práctico el control clásico PID es el más

ampliamente usado en el control de robots manipuladores debido a que su estructura es

sencilla.

Por mucho tiempo ha prevalecido la idea de que el control PID es inadecuado para sistemas

altamente no-lineales debido a que su diseño está basado en argumentos de tipo local y

lineal. Sin embargo, existen trabajos como el reportado por (Arimoto y Miyazaki, 1984)

que pueden extenderse para obtener resultados no locales, aunque las primeras pruebas

explicitas fueron presentadas por (Ortega, et al., 1995) para el caso de retroalimentación de

salida. En lo que se refiere al Control en espacio de tarea existen trabajos como el de (Jiang,

et al., 1994) en donde se aplica un control adaptivo de modos deslizantes, y el de (Kelly,

1999) en donde a partir de un modelado Energy shaping se aplica un controlador tipo PD,

sin embargo en lo que se refiere a la aplicación de un controlador PID fue hasta poco

después del inicio de esta tesis que apareció reportado en (Martínez, 2002) una aplicación

específica en visual servoing, que es un tipo especial de espacio de tarea. La motivación de

esta tesis es entonces demostrar por métodos de análisis de control moderno, que es posible

el empleo de controladores PID en el espacio de tarea en una clase de sistemas robóticos,

en especial en la regulación de posición y orientación, que hasta el momento no se

encuentra reportado en la literatura, y dar un sustento teórico.

9

1.2 Planteamiento del problema

Esta tesis está enfocada principalmente al estudio del control PID en espacio de tarea de un

robot manipulador rígido de n grados de libertad. En este caso se hace énfasis en el estudio

de control de posición y orientación del órgano terminal de dicho robot, analizándose

además la forma más conveniente de representar dicha posición en el espacio de tarea.

1.3 Objetivo Principal

El objetivo de esta tesis es demostrar que un robot manipulador de n grados de libertad bajo

la acción de control PID puede alcanzar cualquier posición y orientación no singular en un

conjunto acotado del espacio de tarea. Esto último es debido a que existen posiciones y

orientaciones en las que se presentan singularidades.

1.3.1 Objetivos particulares

§ Demostrar que es posible lograr estabilidad asintótica en robots manipuladores de n

grados de libertad bajo la acción de controladores PID en espacio de tarea, mediante

el empleo de herramientas matemáticas avanzadas.

§ Implementación de un algoritmo de control en simulaciones, para demostrar los

resultados teóricos.

10

1.4 Relevancia

Aunque actualmente puede encontrarse literatura respecto al análisis no-lineal de

controladores PID, en lo que se refiere a su aplicación en espacio de tarea hasta el momento

sólo se encuentra reportado una aplicación en (Martínez, 2002) en el caso de visual

servoing. A este respecto, el objetivo de esta tesis es tanto el de proponer una clase de

controladores PID en espacio de tarea, como el de discutir algunas de sus limitaciones. Se

prueba que la inserción de acciones integrales lentas en los controladores de tipo “Energy

shaping” son suficientes para regular el manipulador sin desestabilizarlo. La principal

limitación para obtener un resultado de estabilidad semiglobal son las singularidades

inherentes al manipulador.

11

Capítulo 2

Descripción de espacio de tarea

Resumen del capítulo

En este capítulo se describe el espacio de tarea, que es de especial interés para determinar la

ubicación del órgano terminal del robot. Es necesario describir los elementos que conforman el

espacio de tarea que son posición y orientación, los cuales pueden representarse de formas

diferentes. Se detalla cada una de los distintos tipos de representaciones que se pueden hallar en

la literatura, y se explica cuáles son sus ventajas y desventajas. Así mismo se habla de una

forma visual para controlar la posición del órgano terminal, conocido como visual servoing, y la

forma peculiar como puede ligarse con el espacio de tarea mediante el elemento cuaternión.

2.1 Motivación

Un cuerpo rígido puede ser descrito completamente en el espacio tridimensional por medio

de su posición y su orientación con respecto a un marco de referencia. En el caso de los

robots manipuladores, la descripción del movimiento del órgano terminal es la de principal

interés. Comúnmente, el marco de referencia empleado se sitúa en la base del robot.

La posición puede expresarse como la magnitud del vector entre el origen del marco de

referencia (en el cruce de los ejes) y un punto del órgano terminal. La orientación es un

desplazamiento angular de un sistema coordenado con respecto a otro. Es conveniente

12

considerar un marco de referencia ortonormal ya que simplifica la descripción de la

posición. A partir de este punto de la tesis, se le conocerá como sistema coordenado adjunto

al sistema coordenado situado convenientemente en el órgano terminal. La orientación del

órgano terminal es el desplazamiento relativo entre el sistema coordenado de la base y el

adjunto. Al espacio donde se encuentran todas las posiciones y orientaciones del órgano

terminal se le conoce como espacio de tarea.

2.2 Descripción de posición

En esta sección se hace una reseña de distintos tipos de coordenadas (Grossman, 1994) que

pueden ser utilizadas para la representación de la posición del órgano terminal de un robot.

2.2.1 Coordenadas Cartesianas Este tipo de coordenadas fue introducido por primera vez por Rene Descartes (de donde se

derivo el nombre de cartesianas), y lo expresó mediante dos ejes perpendiculares entre sí,

denominados X y Y, el primero horizontal, y el segundo vertical, los cuales se encuentran

divididos en unidades. Un punto sobre el plano se expresa mediante sus respectivas

posiciones sobre los ejes X y Y, mediante proyecciones perpendiculares a dichos ejes,

conformando un par ordenado de números reales. Posteriormente se llevó este tipo de

representación al espacio tridimensional (al cual denominaremos como 3R ), mediante la

adición de un nuevo eje, perpendicular al plano XY, y que se de nomina como eje Z. Para

representar un punto en el espacio se comienza por elegir un punto en 3R , al cual se le

llama punto origen, y al dibujar las tres rectas perpendiculares entre sí, su intersección será

en este punto. El tipo de representación más usado, y que emplearemos a lo largo de esta

tesis es el denominado sistema derecho; en el cual, si viéramos el plano XY de frente,

entonces la parte positiva del eje Z se dirigiría hacia nosotros. En la figura 2.1 se muestra

cómo es representado este tipo de sistema.

13

Figura 2.1 Sistema Cartesiano

Los tres ejes del sistema determinan tres planos coordenados, que se llaman plano XY,

plano XZ, y plano YZ.

Teniendo en cuenta la estructura construida de ejes coordenados y planos, un punto en el

espacio P puede ser representado como una terna ordenada de numero reales ( )zyx ,, , en

donde la primer coordenada x es la distancia dirigida del plano YZ a P, medida en la

dirección positiva del eje X. La segunda coordenada y es la distancia dirigida desde el plano

XZ hasta P, medida en la dirección positiva del eje Y; en tanto la tercer coordenada z es la

distancia dirigida desde el plano XY a P, medida en la dirección positiva del eje Z.

Este tipo de representación es la más sencilla de emplear, si lo que se quiere es determinar

la posición de un punto en el espacio, sin embargo, cuando se ven implicadas vectores en el

espacio existen otro tipo de representaciones que pueden tener algunas ventajas, como en el

caso de obtener las mediciones relativas de movimiento de las articulaciones del robot.

Los robots con una geometría cartesiana consisten en tres juntas prismáticas, en donde cada

grado de movilidad corresponde a un grado de libertad en el espacio cartesiano. Por esta

razón en este tipo de geometría es fácil obtener mediciones de salida para determinar la

posición del órgano terminal.

14

Figura 2.2 Robot de geometría cartesiana

2.2.2 Coordenadas Cilíndricas Las coordenadas cilíndricas son básicamente una proyección tridimensional de las

coordenadas polares de un plano, por lo que primero haremos una breve reseña de las

coordenadas polares. En coordenadas polares se usa también como referencia un par de ejes

perpendiculares entre sí, pero en camb io la representación de la posición se hace por medio

de un radio y un ángulo ( )θ,r , donde r>0 es la distancia desde el origen al punto en el

plano P y θ es el ángulo formado entre el eje X y la línea que representa a r. En la

representación cilíndrica, al igual que las coordenadas cartesianas se emplea un sistema de

tres ejes perpendiculares entre sí como sistema de referencia para determinar la posición de

un punto en el espacio P. Estas coordenadas se expresan como ( )z,,φρ , en donde ρ es una

distancia radial (como r en coordenadas polares), φ es un ángulo y z es igual que en las

coordenadas cartesianas. Para obtener tanto ρ como φ , es necesario realizar una

proyección del punto P en el plano XY, por medio de una recta perpendicular a dicho

plano. Mientras tanto z se obtiene igual que en las coordenadas cartesianas.

15

Figura 2.3 Sistema Cilíndrico

Figura 2.4 Robot de geometría cilíndrica

16

2.2.3 Coordenadas Generalizadas Hasta el momento se ha visto que la posición de una partícula en el espacio se puede

especificar por medio de tres coordenadas que pueden ser cartesianas, cilíndricas, o

cualquier conjunto de tres parámetros apropiadamente escogidos, los cuales pueden ser

representados por 1q , 2q , 3q , y que se les conoce como coordenadas generalizadas.

Cualquiera de las coordenadas puede ser un ángulo o una distancia, por lo tanto cualquier

otro sistema de representación de coordenadas vistos anteriormente es en realidad un caso

particular de las coordenadas generalizadas. El uso de coordenadas generalizadas

proporciona la ventaja de cualquier tipo de robot puede ser expresado de acuerdo al tipo de

juntas que emplee el robot, y las respectivas mediciones de las coordenadas son fáciles de

obtener.

Figura 2.5 Robot de geometría esférica

El emplear las coordenadas generalizadas nos permite describir otras geometrías distintas,

como son la SCARA, y la antropomórfica.

17

Figura 2.6 Robot SCARA

Figura 2.7 Robot antropomórfico

18

X

Y

Z

X'

Y'

Z'

Figura 2.8 Sistemas coordenados en robot

2.3 Descripción de Orientación

2.3.1 Matriz de rotación Una de las formas de expresar la orientación es por medio de una matriz de rotación. La

matriz de rotación consiste en tres vectores unitarios (de dimensión 3) que relacionan los

ángulos de desplazamiento de cada uno de los ejes coordenados.

Por ejemplo considere la Figura 2.8 en donde se muestran dos ejes coordenados, uno se

localiza en el órgano terminal (A’) y otro en la base del robot (A). El primer vector de la

matriz de rotación está conformado por los ángulos que existen entre x’ y x, x’ y y, x’ y z. El

segundo vector está formado por y’ y x, y’ y y, y’ y z., y el tercero está formado por z’ y x,

z’ y y, z’ y z, como se muestra en la Ecuación (2.1).

19

=

=

zzzyzx

yzyyyxxzxyxx

aaaRTTT

TTT

TTT

'''

''''''

321 (2.1)

donde Tx denota la transposición de x.

Note que los vectores columna que conforman R ( 1a , 2a , 3a ) son mutuamente ortogonales,

la demostración puede hacerse utilizando argumentos geométricos. Al notar que las

entradas del sistema son parte de una base ortonormal (Sciavicco y Siciliano, 2000). R es

una matriz ortogonal lo que significa que:

3IRRT = (2.2)

donde 3I denota a la matriz identidad de 33× .

Es necesario hacer notar que la matriz de rotación presenta el problema de proporcionar una

descripción redundante de la orientación del marco adjunto al objeto, esto se debe a que los

nueve elementos que la componen no son independientes entre sí, sino que se encuentran

relacionadas por seis restricciones debidas a las condiciones de ortogonalidad dadas en

(2.2). El hecho de manejar tantos elementos provocan la desventaja de que todas las

operaciones que involucran este tipo de representación se vuelven laboriosas y existe

redundancia en los cálculos.

2.3.2 Ángulos de Euler

Existen otros tipos de representación menos complejas que la matriz de rotación, de hecho,

es posible una representación en el que tan sólo tres parámetros son suficientes para

describir la orientación de un cuerpo rígido. El tipo de representación de la orientación de

tres términos independientes se le denomina una representación mínima.

20

Una representación mínima puede ser obtenida usando un conjunto de tres ángulos, que son

conocidos como Ángulos de Euler.

[ ]Tψϑϕφ =

Si consideramos que la matriz de rotación elemental se expresa como la rotación alrededor

de un eje coordenado como función de un solo ángulo, entonces una matriz de rotación

puede ser obtenida a partir de la composición de una adecuada secuencia de tres rotaciones

elementales mientras se garantice que dos rotaciones sucesivas no se realizaran alrededor

de dos ejes paralelos.

2.3.2.1 Pitch Roll Yaw

Existen varias representaciones por medio de los ángulos de Euler (12 en total), pero una de

las más usadas es la XYZ o conocida como (Pitch, Roll, Yaw). Esta representación se

obtiene como sigue:

a) Primero se rota el marco de referencia un ángulo ψ alrededor del eje X.

b) A continuación se rota un ángulo ϑ alrededor del eje Y.

c) Y por último se rota un ángulo ϕ alrededor del eje Z.

Este tipo de representación, tiene la ventaja de que se trabaja con muy pocos elementos, lo

cual facilita su trabajo en los cálculos requeridos por el algoritmo de control, pero tiene el

problema de presentar múltiples soluciones. Este hecho afecta seriamente el problema de

21

control como veremos posteriormente en esta tesis. Esto es , dos orientaciones diferentes

pueden tener ángulos de Euler iguales.

Más material al respecto de ángulos de Euler se puede encontrar en (Sciavicco y Siciliano,

2000) y en (Caccavale, et al., 1999).

2.3.3 Representación Eje -Ángulo

Existen otros tipos de representación que son no-mínimas, los que pueden ser ventajosos en

el problema de planeo de trayectorias. Una de ellas se obtiene haciendo un reordenamiento

en cuatro parámetros que expresan la rotación de un ángulo dado alrededor de un eje (o

vector) en el espacio.

Sea [ ]Tzyx kkkk = el vector unitario de un eje de rotación con respecto al marco de

referencia. Una rotación de un ángulo θ alrededor del vector r se describe por ),( kR θ , la

cual se puede calcular de la siguiente forma:

( ) ( ) ( )( ) ( ) ( )( ) ( ) ( )

+−+−+−

+−+−+−+−+−+−

=

θθθθθθ

θθθθθθ

θθθθθθ

θ

cckskckkskckk

skckkcckskckkskckkskckkcck

kR

zxzyyzx

xzyyzyx

yzxzyxx

111

111111

),(2

2

2

(2.3)

donde θθ cos=c y θθ sens =

Y en forma inversa, a partir de la matriz de rotación, se puede obtener la representación eje-

ángulo.

−++= −

21

cos 3322111 rrrθ

(2.4)

22

−−−

=

1221

3113

2332

21

rrrrrr

senk

θ

donde ijr son elementos de la matriz de rotación R, es decir

=

333231

232221

131211

rrr

rrrrrr

Rdef

Note que ),(),( kRkR −−= θθ , esto indica que la representación no es única, ya que

corresponde a dos tipos de rotación diferentes y por tanto no existe unicidad.

Por otro lado tenemos que si Ο= 0θ , entonces R es la matriz identidad y el eje de rotación

no está bien definido. Por lo tanto presenta la misma desventaja que los ángulos de Euler.

2.3.4 Cuaterniones

Existe otra forma de representación de cuatro parámetros que es la de Parámetros de Euler,

comúnmente conocido como la unidad Cuaternión Q , conformado de dos elementos: η

que corresponde a un escalar y ε que es un vector de tres elementos, esto es:

[ ] [ ]zyxTTQ εεεηεη == (2.5)

Considérese dos sistemas coordenados 0F y 1F , que están separados por una rotación de θ

alrededor de un vector unitario k (la representación eje-ángulo de la sección anterior),

entonces el cuaternión se obtiene como sigue:

23

k

=

=

2sin;

2cos θεθη (2.6)

nótese que sí [ ]Tεη es el cuaternión que representa la rotación del sistema coordenado 1F

con respecto a un sistema coordenado 0F , entonces [ ]Tn ε− representa la orientación de

0F con relación a 1F

El cuaternión presenta algunas propiedades fundamentales que se discuten a continuación.

a) Normalidad. Los elementos de (2.6) son de norma unitaria, es decir:

12222 =+++ zyx εεεη (2.7)

b) Unicidad. Si se considera el ángulo θ acotado al rango °≤≤°− 180180 θ , entonces el

escalar 0≥η y la representación del cuaternión es única.

Como se ve el cuaternión, a diferencia de la representación eje-ángulo, no presenta el

problema de no-unicidad, es decir, que una rotación θ− alrededor de un eje k− tiene

asociado el mismo cuaternión que una rotación θ alrededor de un eje k. Además el calculo

de cuaternión es mucho más sencillo comparado respecto a los cálculos para una matriz de

rotación.

En vista de (2.3), (2.6) y (2.7), la correspondiente transformación de cuaternión a matriz de

Rotación es la siguiente:

24

( ) )(2212 32 εηεεη SIR T ++−= (2.8)

donde 3I es la matriz identidad y )(εS se define como:

−−

−=

00

0)(

xy

xz

yz

Sεε

εεεε

ε (2.9)

Por contraparte la conversión de matriz de rotación a cuaternión es la siguiente, la cual

encontramos en (Sciavicco y Siciliano, 2000):

( )( )( )

+−−−+−−−+−−−

=

+++=

1sgn1sgn1sgn

21

121

2211331221

3311223113

3322112332

332211

rrrrrrrrrrrrrrr

rrr

ε

η

(2.10)

donde ijr son elementos de la matriz de rotación R.

Una variación a este método lo podemos encontrar en (Klump, 1976) que se describe a

continuación:

25

−−−

+++=

+++=

1221

3113

2332

332211

332211

121

121

rrrrrr

rrr

rrr

ε

η

(2.11)

Otra opción considera que si la traza de la matriz de rotación es igual o menor a cero, se

deberá observar cual elemento de la diagonal de la matriz de rotación es mayor a cero.

Otro tipo de representación del espacio de tarea es visual servoing que se explica en el

Apéndice A.

2.4 Transformación homogénea

En muchos casos, para describir la posición de un cuerpo es necesario realizar una

transformación de un espacio de coordenadas a otro, y dado que un robot se compone de

varios elementos y en ocasiones dependiendo del grado de movilidad del robot, en

ocasiones se hacen necesarias varias operaciones intermedias.

Si 2211 rr > y 3311 rr >

( )

( )

( )

+

+=

+=

+−−=

srr

srr

s

srr

rrrs

2332

2112

2332

332211

5.0

1

ε

η

Si 1122 rr > y 3322 rr >

( )

( )

( )

+

+

=

+=

+−−=

srr

s

srr

srr

rrrs

2332

2112

3113

331122

5.0

1

ε

η

Si 1133 rr > y 2233 rr >

( )

( )

( )

+

+

=

+=

+−−=

s

srr

srr

srr

rrrs

5.0

1

3113

2332

2112

221133

ε

η

26

Considere dos sistemas coordenados (sistema 0 y sistema 1) y un punto en el espacio P,

cuyas coordenadas con respecto a cada uno de los sistemas las expresamos por 0p y 1p

respectivamente. Sea 01d el vector que denota la ubicación del origen del sistema 1 respecto

al sistema 0, y sea 01R la matriz de rotación que representa la orientación del sistema 1 con

respecto al sistema 0. Por lo tanto, basándose en geometría simple, es posible determinar la

posición 0p del punto P, si conocemos 1p , aplicando la siguiente regla de transformación:

101

01

0 pRdp += (2.13)

A esta representación se le conoce como transformación de coordenadas.

La relación anterior puede representarse de una forma más compacta, para ello es necesario

definir la representación homogénea p~ de p, esto es

=

1~ pp (2.14)

Adoptando esta representación para los vectores 0p y 1p , la transformación de

coordenadas puede ser escrita en términos de una matriz de 44 × , como sigue:

=

10

01

010

1 T

dRA (2.15)

La cual se denomina matriz de transformación homogénea. Entonces la transformación de

coordenadas puede ser rescrita de forma compacta como:

101

0 ~~ pAp = (2.16)

27

La transformación homogénea es útil para describir los problemas de cinemática directa y

cinemática inversa primeramente, y posteriormente en esta tesis se empleará en el cambio

de coordenadas.

2.5 Problemas de cinemática directa y cinemática inversa

La configuración de un manipulador está determinada por el número de grados de

movilidad, es decir su estructura mecánica. Cada grado de movilidad está asociado con una

articulación de junta y se dice que constituye una variable de junta. El principal objetivo de

la cinemática directa consiste en determinar la posición y la orientación del órgano terminal

a partir de las variables de junta del robot. Para obtener la posición y la orientación del

órgano terminal, respecto a las variables de junta se emplea una matriz de transformación

homogénea.

=

10T

be

beb

e

dRT

donde de beT la b se refiere al marco de base, y la e se refiere al órgano terminal.

En tanto que el problema de cinemática inversa consiste en determinar las variables de

junta correspondientes a una posición y orientación dadas del órgano terminal. La solución

de este problema es de fundamental importancia; es decir que a partir de la apropiada

determinación de variables de junta, se consiga que el órgano terminal alcance la posición y

la orientación deseada, que es lo que se busca en la solución del problema de control.

Un campo prometedor para el control de posición en robots manipuladorse en espacio de

tarea es mediante el uso de Visual Servoing. Una descripción de Visual Servoing se

encuentra en el Apéndice A.

28

Conclusiones del capítulo

En este capítulo se dieron a conocer los tipos de coordenadas que aplicaremos al describir

la posición del órgano terminal, y los medios para conocer su orientación. También

conocimos distintos tipos de geometrías para los robots. El uso de coordenadas

generalizadas y la descripción del espacio de tarea nos ayudarán a formular el problema de

control y para la descripción dinámica del sistema en el siguiente capítulo.

29

Capítulo 3

Modelo Dinámico

Resumen

Al inicio de este capítulo se describe cómo se obtiene el modelo dinámico para robots

manipuladores por medio de las ecuaciones de Euler-Lagrange y empleando la definición de

coordenadas generalizadas. Posteriormente se mencionan las propiedades del modelo obtenido,

las cuales son útiles en la derivación de los resultados de estabilidad.

3.1 Formulación Euler-Lagrange

El modelado dinámico de un robot manipulador consiste en encontrar el mapa entre los

pares que se aplican sobre las estructuras del robot y las posiciones y velocidades de las

juntas. Existen dos tipos de formulaciones que son empleadas para derivar el modelo

dinámico: la formulación Newton-Euler y la formulación Euler-Lagrange.

La formulación Newton-Euler consiste en la aplicación directa de las leyes de Newton de

movimiento, haciendo uso de alguno de los sistemas coordenados mencionados en el

capítulo anterior; sin embargo, la obtención del modelo dinámico para robots

manipuladores puede ser difícil. En este capítulo se deriva el modelo de un manipulador

basados en la formulación Euler-Lagrange que emplea las coordenadas generalizadas y se

basa en balances de energía para derivar las ecuaciones de movimiento.

30

Las ecuaciones Euler-Lagrange, de forma general (Hauser, 1967) adoptan la siguiente

forma:

k

k

kqK

Qq

Kdtd

∂∂

+=∂

∂•

(3.1)

donde K es la energía cinética del sistema y kQ es la fuerza generalizada, que es la suma de

pares para una junta de revolución o de fuerzas para una junta prismática con los efectos del

campo potencial (Spong y Vidyasagar, 1989), entonces podemos describir las fuerzas

generalizadas como sigue:

kk

K qPQ τ+

∂∂−=

De esta forma (3.1) puede escribirse como:

kkk

kqP

qK

q

Kdtd τ+

∂∂−

∂∂=

∂

∂•

(3.2)

donde P es la energía potencial del sistema. La ecuación (3.2) puede ser escrita de forma

más compacta si introducimos una función L, conocida como la función Lagrangiana, tal

que:

PKL −= (3.3)

De (3.3) se deriva que:

kk q

K

q

L••

∂

∂=∂

∂ y kkk q

PqK

qL

∂∂

−∂∂

=∂∂ (3.4)

31

Lo que conlleva que las ecuaciones de Lagrange, puede ser rescrita de la siguiente forma:

k

kk q

LqL

dtd

∂∂

+=∂∂

τ&

(3.5)

donde kτ son las fuerzas generalizadas no conservativas, que se aplican desde el exterior.

En la descripción de robots manipuladores es necesario manejar más de una coordenada

generalizada, por lo que es necesario hacer el cambio de las derivadas parciales por

gradientes. Es decir, las ecuaciones de Euler-Lagrange en este caso toman la siguiente

forma:

Tq

qUqqLqqL

dtd

=∇−

∇

••

• ),(),( (3.7)

donde nRq ∈ son las coordenadas generalizadas y nRU ∈ los pares debidos a las fuerzas

externas. Mientras )(⋅∇ representa el gradiente, un vector renglón que se define como

kdef

k ∂⋅∂=⋅∇ /)()( , y T)(⋅ denota transposición. Retomando la ecuación (3.3) de la forma:

)(),(),( qPqqKqqL −=••

donde la energía cinética ),( qqK & , se expresa de la forma siguiente:

•••

= qqMqqqKT

)(21),( (3.8)

En donde nnRqM ×∈)( es la matriz de inercia, la cual es simétrica y positiva definida. La

función )(qP es la energía potencial, que es acotada inferiormente.

32

Los pares debidos a las fuerzas externas nRU ∈ consisten en acciones de control nR∈τ ,

fuerzas disipativas (fricción), y fuerzas no disipativas que toman en cuenta la interacción

del sistema con el ambiente.

Dado que las fuerzas disipativas del sistema contribuyen al amortiguamiento requerido para

la estabilización del sistema, entonces en esta tesis se considera el caso crítico en que la

suma de dichas fuerzas es nula. Por otra parte puede considerarse que la interacción con el

medio ambiente es irrelevante y la podemos eliminar.

De acuerdo a lo mencionado arriba el sistema estaría representado de la siguiente manera:

τ=++ )(),()( qgqqqCqqM &&&& (3.9)

donde :

nRqqqC ∈&&),( es el vector de Coriolis y fuerzas centrípetas

nRqg ∈)( es el vector de pares gravitacionales

nR∈τ es el vector de pares de entrada

nRq ∈ es el vector de posiciones angulares de los eslabones

Además )(qg se obtiene como gradiente de la energía potencial )(qU debida a la

gravedad, es decir:

)()( qUqg q∇= (3.10)

Este modelo es el que se emplea en el desarrollo de la solución al problema de control en

los capítulos siguientes.

33

La ecuación de movimiento Euler-Lagrange incluye a sistemas mecánicos y mecatrónicos

(De Luca y Manes, 1994). Esta formulación también ha sido empleada para modelar la

dinámica del cuerpo humano (Van der Kooij, et al, 1999) (Sanner y Cosa, 1999).

3.2 Propiedades de la ecuación de movimiento

En esta sección se describen las propiedades de la ecuación dinámica del robot (3.9) en el

caso de que sólo posea juntas rotacionales. Los elementos que conforman la ecuación

cumplen con algunas propiedades que son útiles para simplificar las pruebas de estabilidad

obtenidas en el desarrollo de esta tesis.

3.2.1 Propiedades de la matriz de inercia

La matriz de inercia nnRqM ×∈)( cumple con las siguientes propiedades

P.1 Es simétrica y positiva definida.

P.2 Es acotada superior e inferiormente. Es decir que existen 1µ y 2µ , que son dos

escalares1 tales que

IqMI 21 )( µµ <<

P.3 Es continuamente diferenciable.

1Para el cálculo de las constantes 1µ y 2µ se requiere del uso de los valores propios máximo y mínimo de la

matriz de inercia. A pesar de que esto puede ser complicado, debido a que se requiere de la solución de

ecuaciones no lineales, existen programas como matlab y maple que pueden ayudar a realizar los cálculos más

rápidamente.

34

3.2.2 Propiedades del vector de Coriolis y fuerzas centrípetas

El vector de fuerzas centrípetas y de Coriolis, junto con la matriz de inercia cumple con las

siguientes propiedades:

P.4 La matriz ),(2)(•

− qqCqM& es antisimétrica

3.2.3 Propiedades del vector de gravedad

El vector de pares gravitacionales g(q) cumple con las siguientes propiedades

P.5 Existe una constante positiva gk tal que

)(qDgk g >

donde qqgqDg

∂∂= )()(

es decir que la derivada del vector de pares gravitacionales con respecto a las coordenadas

generalizadas del robot se encuentra acotada por arriba.

El modelo descrito en este capítulo es el que se emplea en el desarrollo de la solución al

problema de control y para las simulaciones del sistema a lazo cerrado, presentes en los

siguientes capítulos.

Así mismo, las propiedades de la ecuación de movimiento son útiles en la derivación de los

resultados de estabilidad que se muestran en el siguiente capítulo.

35

Capítulo 4

Resultado Principal

Resumen

En este capítulo se presenta primeramente la metodología a seguir para el diseño de un

controlador PID que garantice el control de posición y orientación de un robot manipulador sin

el conocimiento exacto del vector de pares gravitacionales. Empleando un análisis de

estabilidad basado en sistemas singularmente perturbados se demuestra que la estructura de

control PID propuesta garantiza la estabilidad del órgano terminal del robot en una posición y

orientación deseada si ésta no es singular.

4.1 Planteamiento del problema de control

El objetivo de control es el de diseñar una ley de retroalimentación de estados ),(•

= qqττ

tal que el órgano terminal del robot manipulador alcance una posición dp y una orientación

deseada dR de manera asintótica, es decir

dt

dt

RtR

ptp

=

=

∞→

∞→

)(lim

)(lim (4.1)

36

donde: 3)( Rtp ∈ es la posición y 33)( ×∈ RtR la matriz de orientación del órgano terminal

como se vio en el capítulo 2, en tanto 3Rp d ∈ y 33×∈ RRd son la posición y la matriz de

orientación deseada.

Dado que la matriz de rotación R tiene asociada un cuaternión, como se demuestra en el

capítulo 2, entonces es posible reformular el objetivo de regulación de la siguiente manera:

Diseñar una ley de retroalimentación tal que:

=

=

∞→

∞→

d

d

t

dt

tt

ptp

εη

εη

)()(

lim

)(lim

(4.2)

donde [ ] 4RTdd ∈εη es el cuaternión correspondiente a la orientación deseada.

Se ha decidido por el uso del cuaternión en vez de la matriz de rotación debido a su

sencillez, ya que es más fácil determinar un error vectorial que un error matricial, además

del importante hecho de que para algunas orientaciones pueden corresponder dos matrices

de rotación diferentes.

De forma equivalente la ecuación (4.2) se puede expresar como:

0~lim =∞→

yt

(4.3)

donde y~ es el error de salida y se define de la forma:

=−=

ep

yyy d

~~

37

y )(~ qppp d

def

−= es el error de posición y ees el error de orientación y está dado por:

ddd Sqqe εεεηεη )()()( +−= (4.4)

Tal definición del error de orientación ha sido empleada antes en (Caccavale et al., 1999), y

su propósito es el que el error de orientación quede expresado en 3R en vez de 4R . Las

velocidades lineal 3Rp ∈•

y angular 3R∈ω de un punto 0P están dadas por:

qqJqqJqJp

Go

p &&&

)()()(

=

=

ω

(4.5)

donde )(qJ p es la matriz de n×3 relativa a la contribución de las velocidades de junta a la

velocidad lineal del órgano terminal, y )(qJ o es la matriz de n×3 relativa a la

contribución de las velocidades de junta a la velocidad angular del órgano terminal, y n

G RqJ ×∈ 6)( es el Jacobiano geométrico del robot.

A partir de la ecuación (4.2) se puede mostrar que la derivada respecto al tiempo del error

de orientación (4.4) de acuerdo con el objetivo de regulación está dada por:

qqJeQy Ge &)(),(~ η−=•

(4.6)

con:

+= )(21

21

0

0),( eSI

IeQ

ee ηη

donde εεηηη Tdde += y 33×∈ RI es la matriz identidad. Cuando el error de orientación es

cero, ),( eQ eη es una matriz diagonal.

38

4.2 Antecedentes

Para el estudio del problema de control, se considera la metodología de modelado de

energía mas inyección de amortiguamiento (Miyazaki y Arimoto, 1985) (Kelly, 1995). La

motivación que lleva a considerar esta metodología es que nos permite dividir el

controlador en dos partes, una compuesta de la energía potencial que permite la

estabilización en un punto de equilibrio deseado y la otra parte que permite la inyección de

amortiguamiento para hacer el punto de equilibrio atractivo (Ortega, et al., 1995); esto nos

conduce a compensadores simples y robustos. Un hecho importante a considerar, es que

esta metodología garantiza estabilidad asintótica del sistema a lazo cerrado sólo si se

supone una cancelación exacta de los pares gravitacionales )(qg . Sin embargo la función

)(qg es siempre incierta en la práctica, debido de un conocimiento inexacto de los

parámetros del sistema, entonces se debe considerar un vector de pares gravitacionales

estimados )(ˆ qg en la retroalimentación, para que un controlador basado en modelado de

energía pueda ser usado.

De acuerdo con la me todología de modelado de energía más inyección de amortiguamiento

la estructura de control está dada por:

)())(~,,( qFqyyqUq

daq &•∇−−∇=τ (4.7)

donde: RRRRqyyqU mmnda →××:))(~,,( es la energía potencial artificial y es una

función continuamente diferenciable. En tanto RRqF n →:)( & es una función de disipación

continuamente diferenciable.

Se supone que la estructura de control compensa los efectos del par gravitacional (3.10). En

tal caso, sin pérdida de generalidad, la energía potencial artificial puede considerarse como:

)())(~,())(~,,( qUqyyUqyyqU dTda −= (4.8)

39

Donde ))(~,( qyyU dT es la energía potencial total y )(qU es la energía potencial debido a la

gravedad.

El sistema a lazo cerrado obtenido de la dinámica del robot en términos de la energía

potencial total puede ser descrito como sigue:

+∇+∇−=

•

− qqqCqFqyyUqM

q

qq

dtd

qdTq &&&

&

& ),()())(~,()( 1 (4.9)

Como se puede observar el sistema a lazo cerrado obtenido involucra dos funciones

potenciales: la energía potencial debida a la gravedad, y la energía potencial artificial

inducida por el controlador.

Observación 1:

Considérese las siguientes elecciones, encontrada en (Kelly, 1995), para la energía

potencial artificial y la función de disipación:

qKqqF

qUyKyqyyqU

DT

pT

da

&&&21)(

)(~~21

))(~,,(

=

−= (4.10)

donde nnDP RKK ×∈, son matrices simétricas positivas.

Haciendo las siguientes definiciones de la ecuación de salida

)(qhy = (4.11)

40

donde mn RRh →: es una función continuamente diferenciable, y se supone que nm ≤ . Al

mapa de salida )(qh se le denomina función de tarea, la cual fue introducida por primera

vez por (Samson, et al., 1991).

Por medio de una selección adecuada del mapa )(qh se puede caracterizar la posición y la

orientación con respecto a un marco adjunto o base de referencia determinado tal como la

función de cinemática directa. Considérese nd Ry ∈ como una posición u orientación

deseada de la terminal del robot, entonces, el problema de control puede ser formulado

como el diseño de una ley de control que permite al órgano terminal del robot alcanzar dy

en el espacio de tarea.

Suposición 1: La función de tarea )(qhy = es invertible en una vecindad nq RN ∈ de dq

donde:

)(1d

def

d yhq −=

Para obtener la representación en la forma de la ecuación (4.7), realizamos lo siguiente:

)(~~))(~,,( qUyKyqyyqU qpT

qdaq ∇−∇=∇ (4.12)

De la ecuación (4.12) se tiene que:

)(~ qJqh

yy qq =∂∂

=∇=∇

donde nnRqJ ×∈)( es el Jacobiano de la función de tarea )(qh , y para la cual se puede

hacer la siguiente suposición:

41

Suposición 2: La matriz Jacobiana nnRqJ ×∈)( tiene rango completo en qN , es decir, que

0)( ≠qJ y además existe una constante positiva Jα tal que JqJ α≤)( , para toda

qNq ∈

La ecuación (4.12) puede rescribirse con la matriz Jacobiana como sigue:

)(~)())(~,,( qUyKqJqyyqU qpT

daq ∇−=∇

y aplicando (3.10) resulta:

)(~)())(~,,( qgyKqJqyyqU pT

daq −=∇ (4.13)

Por otro lado, a partir de (4.10) se deriva que:

qKqF Dq &&& =∇ )( (4.14)

Al sustituir (4.13) y (4.14) en (4.7) se obtiene el siguiente caso del control PD con

Jacobiano transpuesto y compensación de gravedad exacta.

)(~)( qgqKyKqJ DpT +−−= &τ (4.15)

En la ecuación (4.15) el primer término a la derecha corresponde al modelado de energía

(energy shaping), el segundo es la inyección de amortiguamiento (damping inyection) y

)(qg es la cancelación exacta de los efectos gravitacionales.

42

Combinando la ecuación (3.9) con (4.15), se obtiene la siguiente representación del sistema

a lazo cerrado:

( )[ ]

+−−

=

− qKqqCyKqJqM

q

qq

dtd

DpT &&

&

& ),(~)()( 1 (4.16)

En (Kelly, 1999) se establece la existencia de un punto de equilibrio para el sistema (4.16).

Hasta aquí se ha visto como a partir de la metodología de modelado de energía mas

inyección de amortiguamiento, encontrado en la literatura, es posible encontrar una ley de

control PID, que involucra una compensación de gravedad exacta, la cual se tomará como

base de la solución del problema de control planteado.

4.3 Control PD

Al considerar a (4.15) se requiere que el vector de pares gravitacionales )(qg sea bien

conocido, pero esta función es incierta debido a que depende de las cargas sobre el robot

que generalmente son desconocidas. Por lo tanto debe considerarse un estimado )(ˆ qg en

lugar de )(qg y (4.15) se rescribe de la siguiente forma:

)(ˆ~)( qgqKyKqJ DpT +−−= &τ (4.17)

En este caso puede observarse que la cancelación exacta de efectos gravitacionales, no es

del todo satisfecha, lo cual en la práctica puede conducir a un sesgo (offset ) del estado

estacionario (es decir que el error de salida y~ no sería cero, sino que sería de un valor que

aunque podría ser pequeño, no sería irrelevante) El sesgo podría ser evitado con una

ganancia proporcional pK “suficientemente grande”, pero esto llevaría a posibles

inestabilidades dinámicas.

43

Por otro lado, para eliminar el sesgo, rescribimos (4.17):

[ ] )(ˆ~)( , qgqKyKqJ DdIpT +−+−= &γτ (4.18)

de donde, como se podrá observar, se ha añadido otro elemento, nI R∈γ , a l cual se definirá

como: )()(, dT

d

def

dI qgqJ ∆= −γ , donde )(ˆ)()( qgqgqg −=∆ es el par gravitacional residual.

Combinando (4.18) con (3.9) obtenemos la siguiente representación del sistema a lazo

cerrado:

( ) ( )[ ]

∆−+−+−

=

− )(),(~)()( ,

1 qgqKqqCyKqJqMq

qq

dtd

DdIpT &&

&

& γ (4.19)

Al igualar la ecuación dinámica con cero, obtenemos la representación de la ecuación

estática del sistema que es la siguiente:

IpT qgyKqJ γ+∆= )(~)( (4.20a)

A partir de (4.11) se puede definir dq de la forma siguiente:

)(1dd yhq −=

esto nos conduce a lo siguiente:

)~()( ygqg ∆=∆

44

Por lo que podemos rescribir la ecuación estática de la siguiente forma:

IpT ygyKqJ γ+∆= )~(~)(

donde la solución es la siguiente:

[ ]IT

p ygyJKy γ+∆−= − )~()~(~ 1 (4.20b)

La ecuación (4.20b) es una función implícita de y.

Para probar que existe una única solución de (4.20b) es necesario aplicar la teoría de

contracción de mapas. Esto es, que se necesita probar que la norma del gradiente de la parte

derecha de (4.20b) es acotado. Esto es:

[ ] 1)~()~(1 <+∆∇ −I

TP ygyJK γ (4.20c)

Debido a que suponemos que el Jacobiano no es singular y a la propiedad de acotamiento

de los pares gravitacionales es posible mostrar que entonces existe un valor suficientemente

grande de PK tal que la restricción (4.20c) se satisface. Es decir, existe minPK tal que para

toda minPP KK > la ecuación (4.20c) tiene una única solución y si en particular dII ,γγ = ,

esta solución coincide con la posición y la orientación deseada. Sin embargo en la práctica

no contamos con una variable Iγ para compensar los efectos gravitacionales, pero en su

lugar se agrega una acción integral. La ley de control (4.18) se modifica de la siguiente

forma:

σστ dyKJqgqKyKJ IT

DPT )(~)(ˆ~ ∫−+−−= & (4.21)

Donde nnI RK ×∈ es la matriz de ganancias integrales la cual es diagonal y definida

positiva.

45

Como se puede observar en (4.21) el Jacobiano transpuesto )(qJ T afecta al término

integral, esto es porque la acción integral se utiliza para remover el sesgo, lo cual debe ser

a lo largo de la misma dirección del movimiento del órgano terminal definido por )(qJ T .

A lazo cerrado el sistema se representa de la siguiente forma:

[ ] nDIPT qKyKyKJqgqqqCqqM 0~~)(),()( =+++∆++ ∫ &&&&& ε (4.22)

4.3 Estabilidad del sistema a lazo cerrado Ya que se ha establecido la ley de control empleada, el siguiente paso es demostrar la

existencia de ganancias proporcionales suficientemente grandes que garanticen la

existencia de un único punto de equilibrio y de ganancias integrales que eliminen el sesgo

debido al conocimiento inexacto de los pares gravitacionales, asegurando el objetivo de

regulación. En otras palabras el trabajo consiste en demostrar el siguiente teorema.

Teorema 1 Considérese el sistema (3.9) bajo la acción del controlador (4.21) con

II KK ε= , donde 0>ε y 0>IK es una matriz simétrica y definida positiva. Entonces

para un valor suficientemente grande de )(min pKλ existe una constante positiva tal que

para todo max0 εε << el sistema Euler-Lagrange controlado es internamente estable y

0)(~lim =∞→ tyt .

Prueba:

Para un mejor manejo de la ecuación (4.22), es útil definir [ ] [ ]Tdef

T qqxxx &,, 21 == y

∫= yKz I

def ~ε

46

Lo que nos permite rescribir el sistema a lazo cerrado (4.22) de la siguiente forma:

( ) ( )[ ]

+−∆−−−=

−

yK

zyKJxgxKxxCxMx

z

xx

dtd

I

PT

D

~

~)(),()( 122111

2

2

1

ε (4.23)

Primeramente se localiza los puntos de equilibrio de la ecuación (4.23)

Igualando la ecuación (4.23) con cero obtenemos el siguiente sistema de ecuaciones:

02 =x (4.24a)

0)~)(()( 11 =+−∆− zyKxJxg PT (4.24b)

0~ =yK Iε (4.24c)

Para solucionar el sistema de ecuaciones puede tomarse primero por la ecuación (4.24c), y

considerando que 0≠ε y IK como no singular, obtenemos lo siguiente:

0~ =y es decir dyy =

entonces (4.24b) queda como sigue:

0)()( 11 =−∆− zxJxg T

lo que lleva a:

)()( 11 xgxJz T ∆−= − (4.25)

47

Como lo que se espera es llegar a una posición deseada dq , a la que se puede expresar por

su equivalente d

def

d qx =1 , rescribimos (4.25) como:

dI

def

ddT

d xgxJz ,11 )()( γ=∆−= −

Para poder hallar con facilidad la solución de (4.23) se traslada el punto de equilibrio al

origen. Para este fin se considera el siguiente cambio de variables:

[ ] [ ]TT xqxxxx 2*

121 ,~,~~ −== (4.26) dIzz ,

~ γ−=

donde *q es la posición de equilibrio del controlador PD, que no necesariamente es la

misma que la deseada, y que definimos como función de z

)()(*IPzPq γ==

derivando (4.26) obtenemos:

yKzP

xzzP

xqxx I~~

11*

11 ∂∂

−=∂∂

−=−= ε&&&&&&

22~ xx && = zz && =~

48

De esta forma se puede rescribir (4.23) de la siguiente manera:

( ) ( )[ ]

++−∆−−−∂∂

−

=

−

yK

zyKJxgxKxxCxM

yKzP

x

z

xx

dtd

I

dIPT

D

I

~~~)~(~)~,~()~(

~~

~

~~

,122111

2

2

1

ε

γ

ε

(4.27)

Donde, con IK que es una matriz definida positiva.

Haciendo las siguientes definicio nes:

[ ]Txxx 21~~~ =

[ ] ( )[ ]

++−∆−−−

= −−dIP

TD zyKJxgxKxxCxM

x

,12211

11

2~)~(~)~,~()~(

~

γϕ

T

nIKdzdP

B

= 0

Con estas definiciones obtendremos el modelo de sistema singularmente perturbados para

el sistema (4.27):

−=

yK

yByzx

zx

I~

~)~,~,~(~

~

εεϕ

&&

(4.28)

Si 0=ε

)~,~,~(~ yzxx ϕ=& (4.29)

Que es la ecuación que representa al sistema reducido.

49

Para realizar un análisis de la componente más lenta del sistema (4.27), es necesario

realizar un cambio en escala de tiempo tετ = , por lo que también es necesario definir las

variables )/(~)( εττξ xdef

= y )/(~)( εττζ zdef

= , cuyas derivadas pueden definirse como

τξ

εdd

dtxd def

=~

τζ

εdd

dtzd def

=~

que conlleva a que el sistema (4.28) se rescriba de la forma:

( ) ( )[ ]

∆−++−−−∂∂

+

=

−

yK

gyKJKCM

yKzP

dtd

I

dIpT

D

I

~)(~),()(

~

1,2211

1

2

2

1

γζξξξξ

εξ

ζ

εξεξ

Sí 0=ε se tiene que:

yKdd

I~=

τζ (4.30)

A (4.30) se le llama representación de capa límite. Para establecer las propiedades de

estabilidad de (4.30) considérese la siguiente candidata a función de Lyapunov:

ζζζ 1)( −= IT KV

Que es positiva definida a partir del hecho que 11−K es positiva definida. La derivada en

tiempo de )(ζV es:

yV T ~2)( ζζ =& (4.31)

50

La ecuación (4.31)es una función definida negativa. Esto es:

22)( ςδζ −≤V& para algún 0>δ

De aquí se puede concluir que el sistema (4.30) es exponencialmente estable. De esta forma

la estabilidad asintótica de (4.23) se asegura al cumplirse las condiciones de (Hoppensteadt,

1974). Es decir que se garantiza que existe una constante 0max >ε tal que para todo

max0 εε << el sistema (4.23) es asintóticamente estable.

A partir del análisis de estabilidad se puede concluir que la presencia del término integral

en la ley de control garantiza que el error de regulación 0)(~ →ty cuando ∞→t con lo que

prueba que el sistema retroalimentado es asintóticamente estable. Con esto se concluye la

prueba del Teorema 1.

∆

La técnica normalmente a seguir para la sintonización de un controlador PID es escoger las

ganancias PK y DK para un desempeño óptimo durante el transitorio y colocar después

una ganancia integral lo suficientemente pequeña para regular el error de salida a cero. El

comportamiento del robot bajo la acción de la ley de control PID (4.21) a tiempos cortos es

representada por el sistema de capa límite o sistema rápido, es decir que el robot

manipulador está bajo la acción del controlador PD.

En la Figura 4.1 se puede observar la interpretación geométrica del controlador PID en

donde *x representa la curva de equilibrio del control PD, en tanto que las líneas punteadas

representan el comportamiento de la posición del robot manipulador bajo la acción de

control PID a partir de distintas posiciones iniciales. Se puede apreciar que primeramente

actúa la acción PD, lo que lleva al manipulador a la curva de equilibrio *x y posteriormente

actúa la acción integral, que conduce al manipulador a la posición deseada dx .

51

Figura 4.1 Comportamiento del controlador PID

En este capítulo se establece el principal objetivo de control, que es diseñar una ley de

control que permita alcanzar una posición y una orientación deseada para el órgano

terminal de un robot manipulador tomando en cuenta la existencia de incertidumbres en el

vector de pares gravitacionales. Posteriormente, se presenta el diseño del controlador junto

con un análisis de estabilidad. Las leyes de control presentes en este capítulo presentan

algunas ventajas tales como sólo requerir de un estimado de los pares gravitacionales y de

no necesitar de la cinemática inversa ni del Jacobiano inverso.

52

Capítulo 5

Simulaciones Numéricas

En este capítulo se muestran los resultados obtenidos a partir de simulaciones numéricas en

Simulink, correspondientes a una consigna de orientación al implementar las leyes de control

(4.17) y (4.21) en una muñeca esférica cuyos parámetros se encuentran descritos en el

Apéndice B.

El comportamiento de las leyes de control propuestas en el capítulo anterior y aplicadas a

un sistema robótico, en este caso una muñeca esférica de tres grados de libertad como la

que se muestra en la Figura 5.1 se obtuvo a partir de simulaciones numéricas realizadas con

Simulink.

5.1 Cálculo del Jacobiano transpuesto

Para llevar a cabo la implementación práctica de las leyes de control (4.17) y (4.21) es

necesario conocer la matriz del Jacobiano transpuesto )(qJ T , la cual se obtiene a partir del

modelo de la muñeca esférica descrito en el Apéndice B, y que es la siguiente:

=

34333231

24232221

14131211

)(

tttt

tttt

ttttT

jjjjjjjjjjjj

qJ

Donde:

53

∂∂

−∂∂

=

∂∂

+∂∂

=

1

22

1

1112

1

22

1

1111

24)1sgn(

141

qr

qr

sqs

j

qr

qr

sqj

t

t

∂∂

−∂∂

−=

∂∂

+∂∂

−=

1

22

1

1114

1

22

1

1113

44)4sgn(

34)2sgn(

qr

qr

sqs

j

qr

qr

sqs

j

t

t

∂∂

+∂∂

−∂∂

−=

∂∂

−∂∂

+∂∂

−=

∂∂

−∂∂

−∂∂

=

∂∂

+∂∂

+∂∂

=

2

33

2

22

2

1124

2

33

2

22

2

1123

2

33

2

22

2

1122

2

33

2

22

2

1121

44)4sgn(

34)2sgn(

24)1sgn(

141

qr

qr

qr

sqs

j

qr

qr

qr

sqs

j

qr

qr

qr

sqsj

qr

qr

qr

sqj

t

t

t

t

∂∂

−∂∂

−=

∂∂

+∂∂

=

3

33

3

2232

3

33

3

2231

24)1sgn(

141

qr

qr

sqs

j

qr

qr

sqj

t

t

∂∂

+∂∂

−=

∂∂

−∂∂

=

3

33

3

2234

3

33

3

2233

44)4sgn(

34)2sgn(

qr

qr

sqs

j

qr

qr

sqs

j

t

t

con:

)sin()cos()cos()sin()sin(

)sin()cos()sin(

)cos()sin()sin()sin()cos(

)sin()cos(

)cos()sin(

213213

22

3212

22

213211

22

212

11

211

11

qqqqqqr

qqqqr

qqqqqqr

qqqr

qqqr

−=∂∂

=∂∂

−=∂∂

−=∂∂

−=∂∂

54

)sin()cos(

)cos()sin(

323

33

322

33

qqqr

qqqr

−=∂∂

−=∂∂

Figura 5.1 Muñeca esférica robótica

5.2 Implementación de las leyes de control

Primero se realizaron simulaciones con la ley de control PD (4.17) con un vector de pares

gravitacionales estimado )(ˆ qg . Posteriormente se realizaron las simulaciones

correspondientes al aplicar a ley de control PID (4.21) con distintas ganancias

proporcionales.

55

5.2.1 Controlador PD

Considere la ley de control (4.18), sin dI ,γ :

[ ] )(ˆ~)( qgqKyKqJ DpT +−−= &τ

donde la )(ˆ qg es una variación del 85% de )(qg . El diagrama a bloques correspondiente a

este controlador se muestra en la Figura 5.2.

Figura 5.2 Diagrama a bloques del controlador PD

Se escogieron las siguientes ganancias para este control:

=

2000

020000200002

PK

=

100010001

DK

56

La Figura 5.3 muestra el comportamiento del error de salida de la muñeca esférica bajo la

acción de la ley de control PD.

Nótese que la salida del sistema a lazo cerrado está expresada en cuaterniones, por lo tanto

cada una de las curvas de las gráficas mostradas en esta sección corresponde a uno de los

elementos del cuaternión.

Figura 5.3 Desempeño del controlador PD

De la Figura 5.3 se puede observar que el error de salida de cada uno de los elementos del

cuaternión nunca llega a cero, es decir que el objetivo de regulación no es alcanzado, esto

debido a que no existe una compensació n exacta de los pares gravitacionales. De aquí que

se haga necesario introducir una acción integral en la ley de control.

57

5.2.2 Controlador PID

En esta sección se considera la ley de control PID (4.21) expresada como sigue:

στ dyKJqgqKyKJ IT

DPT ∫−+−−= ~)(ˆ~ &

Para este controlador se realizaron varias pruebas con distintos ganancias proporcionales,

para observar como afectan su desempeño a lo largo del tiempo. En la figura 5.4 se muestra

el diagrama a bloques del siguiente correspondiente a este controlador

Figura 5.4 Diagrama a bloques del controlador PID

58

Como primer caso se aplican las siguientes ganancias al controlador PID:

=

5000

050000500005

PK

=

5.00005.00005.0

DK

=

2.0000

02.000002.000002.0

IK

El comportamiento de esta ley de control con estas ganancias se observa en la Figura 5.4

Figura 5.4 Desempeño del controlador PID con KI=diag{0.2,0.2,0.2,0.2}

En la segunda simulación se aplicaron las siguientes ganancias:

=

5000

050000500005

PK

=

5.000

05.00005.0

DK

=

6.0000

06.000006.000006.0

IK

59

En la figura 5.5 se observa el desempeño de la ley de control PID con las ganancias

inmediatamente mencionadas.

Figura 5.5 Desempeño del controlador PID con KI=diag{0.6,0.6,0.6,0.6}

Por último se aplicaron las siguientes ganancias en las simulaciones:

=

5000

050000500005

PK

=

5.00005.00005.0

DK

=

2000

020000200002

IK

La Figura 5.6 muestra el desempeño en el tiempo de este controlador con las ganancias

arriba mencionadas.

60

Figura 5.6 Desempeño del controlador PID con KI=diag{2,2,2,2}

En la Figura 5.4 puede observarse que para el controlador PID con las ganancias integrales

KI=diag{0.2,0.2,0.2,0.2} el error de salida logra hacerse cero, aunque se observa que lo

hace muy lentamente, pero en cambio observamos que es muy estable. En la Figura 5.5 se

observa que el error de salida también logra hacerse cero, si las ganancias integrales son

KI=diag{0.6,0.6,0.6,0.6} pero lo hace notablemente más rápido que en el caso anterior. En

el caso de la Figura 5.6, donde las ganancias proporcionales son KI=diag{2,2,2,2} el error

de salida también logra hacerse cero mucho más rápido, pero a la vez se observa que el

desempeño del controlador es menos estable que en los casos anteriores.

61

5.3 Control de Posición

También se realizaron algunos simulaciones numéricas para la posición de un brazo de dos

grados de libertad.

5.3.1 Control PD

En la Figura 5.2 se muestra el comportamiento del brazo robótico con controlador PD con

ganancias Kp=diag{2.0, 2.0} y Kd=diag{5.0, 5.0}

Figura 5.7 Error de posicionamiento con control PD

0 20 40-0.2

0.0

0.2

0.4

0.6

0.8

1.0

1.2

1.4

1.6

1.8

Err

or d

e sa

lida

tiempo

B C

62

5.3.1 Control PID

En la figura 5.3 se muestra el comportamiento del robot con la ley de control PID con

compensación de gravedad inexacta con ganancias Kp=diag{2.0, 2.0}, Kd=diag{5.0, 5.0} y

Kp=diag{0.5, 0.5}.

Figura 5.7 Error de posicionamiento con control PID

Para el brazo se observan un comportamiento similar al observado con la muñeca esférica,

con el controlador PD existe un sesgo para el error de salida, y este se hace cero cuando se

aplica el controlador PID.

0 20 40

0.0

0.5

1.0

1.5

2.0

Err

or d

e sa

lida

tiempo

63

De los experimentos realizados se observa que el controlador PID es una buena alternativa

para eliminar los efectos producidos por el conocimiento parcial del vector de pares

gravitacionales.

De los experimentos mostrados se puede observar que si la ganancia integral es muy

pequeña respecto a la ganancia proporcional el controlador es bastante estable, pero la

acción integral actúa muy lentamente. En el caso opuesto se tiene que si la relación entre

ambas ganancias es pequeña, resulta que la acción integral actúa rápidamente, pero se tiene

que el controlador tiende a ser inestable.

64

Capítulo 6

Conclusiones

En la presente tesis se realizó el análisis del problema de regulación de posición y

orientación de un robot manipulador mediante la aplicación de una ley de control de

retroalimentación PID, a partir del punto de vista de que el robot es un sistema Euler-

Lagrange.

Antes del desarrollo de las leyes de control, fue necesario introducir los fundamentos de la

descripción del espacio de tarea del robot en el capítulo 2, en donde se describió las

coordenadas empleadas en la descripción de la posición y la representación de la

orientación. En el capítulo 3, a partir de la metodología de Euler-Lagrange se mostró cómo

es posible obtener el modelo dinámico del robot, e introducir sus propiedades, útiles en la

derivación del resultado principal, cuyo desarrollo se mostró en el capítulo 4.

En el capítulo 4 se planteó el problema de control y para el desarrollo de la solución

principal se indicaron los antecedentes encontrados en la literatura. A partir de éstos se

empleó la metodología “Energy shaping plus damping” como base de la solució n del

problema; esta estrategia de control permite obtener un controlador basado en el Jacobiano

transpuesto del robot manipulador, con lo que se elimina la necesidad de emplear el

Jacobiano inverso o la cinemática inversa del manipulador.

65

Primero se analizó el caso del control PD donde se determinó que es imposible obtener una

cancelación exacta de los efectos gravitacionales, debido a las incertidumbres paramétricas

del robot, lo que provoca que exista un sesgo (offset) del estado estacionario. Para la

eliminación de este sesgo se utilizó el control PID, donde la acción integral actúa como un

estimador de incertidumbres de tipo estructural, debido al desconocimiento de la forma y

los parámetros de los términos gravitacionales, demostrándose que es posible eliminar el

sesgo, de una forma muy eficiente.

En el capítulo 5 se mostraron los resultados obtenidos a partir de simulaciones en Simulink

del modelo de una muñeca robótica esférica (tres grados de libertad) para demostrar los

resultados teóricos del capítulo 4. Con base en estos resultados es posible demostrar que

una ley de retroalimentación PID es capaz de lograr estabilidad semiglobal de la posición

deseada, aún cuando los robots manipuladores poseen un comportamiento altamente no

lineal. En esta parte se hizo uso de los cuaterniones

En general, en este trabajo se demuestra que para regulación de posición y orientación en

robots manipuladores, es posible eliminar los errores en estado estacionario debidos al

conocimiento inexacto de los pares gravitacionales. Desde el punto de vista práctico se

muestra que los controladores PID basados en el Jacobiano transpuesto presentan un buen

desempeño en robots manipuladores de tres grados de libertad, y que el uso de cuaterniones

es una buena opción para determinar la orientación en robots manipuladores y que no tiene

ningún conflicto con el uso de controladores PID.

66

6.1 Trabajo futuro

De la presente tesis se pueden derivar varias líneas de investigación que se pueden realizar

a mediano y corto plazo:

Realizar experimentos en la muñeca esférica de la que se tomo el modelo para las simulaciones numéricas.

Extender los resultados de esta tesis al problema de seguimiento.

Llevar los experimentos dentro del campo de Visual servoing, en donde la orientación

se determinaría de forma visual.

67

Apéndice A

Visual Servoing

Visual servoing es el resultado de fusionar varias áreas como son: procesamiento de

imágenes, cinemática, dinámica teoría del control y cómputo en tiempo real. El objetivo de

visual servoing es el control de robots usando información visual, haciéndolos capaces de

manipular su espacio de trabajo. En visual servoing como su nombre lo indica, se involucra

el uso de una o más cámaras, una interfaz electrónica y una unidad de procesamiento.

Para este tipo de estrategia de control existen dos tipos de configuraciones de la cámara que

pueden ser: cámara en mano o cámara fija. En el modo cámara fija, un sistema de visión se

coloca en un marco coordenado desde donde captura imagenes del robot y su entorno. En

este sistema el objetivo principal es alcanzar un blanco deseado. En cambio en la

configuración cámara en mano, una cámara es montada en el robot, proporcionando

información del entorno. En esta otra configuración el objetivo es mover el efector final de

tal forma que la proyección de un objeto esté siempre en una posición deseada en la imagen

dada por la cámara.

En cualquiera de las configuraciones que se use, debe realizarse primeramente una

calibración de la cámara que consiste en determinar su geometría interna y las

características ópticas de la lente (parámetros intrínsecos), así como su posición y

orientación (parámetros extrínsecos) (Lenz R K, R Y Tsai, 1988).

La secuencia de operaciones de un sistema de visión se inicia con la detección, localización

y representación de partes especiales de una imagen, llamadas características de la imagen.

En visión el término características se refiere a dos posibles entidades: propiedades globales

de una imagen o partes de una imagen con propiedades especiales, como pueden ser por

ejemplo orillas, bordes, curvas o superficies. La extracción de características de una imagen

es un paso intermedio, y es utilizado para realizar el control de un robot y para la

68

calibración de la cámara. En visual servoing se emplean las coordenadas de un punto de

cierta característica de la imagen, que comúnmente es el centroide de una región para

realizar el posicionamiento del efector final.

A.1 Modelo de la cámara

Al emplear información visual para el control del efector final de un robot es necesario

entender algunos aspectos de la geometría del procesamiento de imágenes. La cámara

contiene una lente que realiza una proyección en 2D de la escena sobre el plano imagen,

que es el lugar donde se localiza el sensor de la cámara. Esta proyección provoca la pérdida

de información de profundidad.

En visual servoing se involucra el análisis de imágenes del espacio de trabajo del robot,

debido a esto es necesario establecer un modelo que describa la proyección 3D al plano 2D,

en éste deberá tomarse en cuenta la geometría de proyección que determine donde un punto

del espacio real será proyectado en el plano de la imagen.

A.2 Proyección perspectiva

La proyección perspectiva es un mapeo no lineal del sistema coordenado cartesiano 3D al

sistema coordenado de la imagen 2D. La figura muestra la geometría básica de la

proyección de perspectiva, donde [ ]Tzyxk ,,= son las coordenadas en 3D de un punto en

el espacio, referenciado al sistema coordenado de trabajo, en tanto que [ ]Tcccc zyxk ,,= son

las coordenadas 3D del mismo objeto en el sistema coordenado de la cámara, que está

centrado en el punto Oc que es el centro óptico de la cámara, el cual está normalmente

alineado con el eje C. Las coordenadas [ ]Tiii yxk ,= representan un punto en el sistema

coordenado del plano imagen, centrado en el punto Oi que se forma a partir de la

intersección del eje C y el plano imagen. La distancia entre el centro óptico y el plano

imagen es denominado distancia focal λ .

69

Figura A.1

Empleando la ley:

λ111

=+ihh

(A.1)

donde h es la distancia del objeto a la lente y hi es la distancia del plano de la imagen al

lente y usando la geometría, tenemos que las coordenadas de un punto sobre el plano

imagen [ ]Tiii yxk ,= están relacionadas con las coordenadas [ ]Tzyxk ,,= de la siguiente

forma:

=

yx

syx

i

i (A.2)

A esta ecuación se le conoce como proyección perspectiva, donde s es un factor de

magnificación que se define como:

h

s−

=λ

λ (A.3)

X

Y

Z

λ

hh i

P i

Oi

X i

Yi

Xc

Yc

Zc

Oc

70

Figura A.2

A.3 Distorsiones geométricas

El lente de la cámara introduce distorsiones geométricas en la imagen. A las distorsiones

geométricas se les clasifica como distorsiones radiales y tangenciales. Las distorsiones

radiales provocan que los puntos de la imagen se trasladen a lo largo de unas distancia

radial desde el origen iO en el plano imagen, las distorsiones hacia fuera se consideran

positivas como se muestra en la Figura , y se ve que el punto iP es desplazado al punto dP .

Las distorsiones tangenciales ocurren por un incorrecto valor del radio, pero son

generalmente menos significantes que las distorsiones radiales. Estas últimas son

modeladas por polinomios de la siguiente forma:

273

52

31 ... +++++=∆ n

iniii rkrkrkrkr (A.4)

r i

rd

∆r

oi

xi

Yi

Pi

Pd

71

donde ir es la distancia entre el punto imagen y el origen del plano imagen, y el valor de

los coeficientes jk ( ,...3,2,1=j ,n) pueden ser determinados mediante procedimientos de

calibración.

El valor correcto del radio desde el origen al punto en la imagen debido a la distorsión

geométrica radial está dada por:

rrr id ∆−= (A.5)

A.4 Cambio de coordenadas

Suponiendo que el plano imagen es paralelo al plano donde el robot se mueve

(Considerando la Figura A.2.), la transformación de las coordenadas de un punto 3RP ∈ en

el sistema coordenado de tarea del robot a las coordenadas en pantalla 2RPS ∈ , empleados

normalmente por una computadora, consiste de cuatro pasos que son:

1) Transformación de las coordenadas del marco de coordenadas de tarea al marco de la

cámara.

2) Transformación de perspectiva, que se produce en el plano de la lente de la cámara.

3) Transformación debida a las distorsiones radiales y que conduce a las coordenadas de

imagen en la cámara.

4) Conversión de coordenadas entre las coordenadas de imagen a las coordenadas en

pantalla

72

A.5 Error de posicionamiento de imagen

Figura A.3 Error de posicionamiento

En la Figura A.3 se ilustra el error de posicionamiento de imagen Sx~ , que se define como la

distancia entre la posición del efector final del robot y del objetivo, que se expresa como:

SSS xxx −= *~ (A.6)

donde *Sx es la posición del objetivo y Sx es la posición del efector final, ambos en la

posición de imagen.

Entonces, esto conduce a que el objetivo de control en visual servoing consista en hacer que

el error de posicionamiento de imagen sea cero, es decir que 0~ =Sx

73

Apéndice B

Sistemas Singularmente Perturbados

En el análisis de sistemas dinámicos, a menudo se usan modelos de orden reducido

obtenidos al hacer a un lado parámetros parásitos pequeños, los cuales incrementan el orden

del modelo. Esta reducción del orden puede representarse como un problema de

perturbación singular. En donde el modelo singularmente completo representa al sistema

con los parámetros parásitos.

El modelo de un sistema singularmente perturbado es el siguiente:

),,( εzxfx =& (B.1)

),,( εε zxgz =& (B.2)

donde se supone que las funciones f y g son continuamente diferenciables en sus

argumentos para [ ]1,0),,( tDDzx zx ××∈ε , donde nx RD ⊂ y m

z RD ⊂ . Si hacemos

0=ε en (B.1) y (B.2), se produce un cambio fundamental y abrupto en las propiedades

dinámicas del sistema, lo que provoca que la ecuación diferencial gze =& degenere en la

ecuación algebraica o trascendental

)0,,(0 zxg= (B.3)

Se dice que el modelo (A.1)-(A.2) está en la forma estándar si (B.3) tiene 1≥k raíces

reales aisladas

kixhz i ,,2,1),( K== (B.4)

74

Para obtener el modelo reducido, se sustituye (B.4) dentro de (B.1), en 0=ε , para obtener:

)0),(,( xhxfx =& (B.5)

Al modelo (B.5) se le denomina como el modelo reducido o sistema reducido y representa

la respuesta lenta del modelo completo. En tanto que el modelo de la respuesta rápida se

obtiene de la discrepancia entre la respuesta del modelo reducido (B.5) y la del modelo

completo (B.1)-(B.2). Para ello denotemos la solución de (B.5) por x , en tanto que z se

obtiene a partir de la solución de (B.5), calculando:

)(xhzdef

=

Para un análisis de estabilidad es más conveniente realizar el siguiente cambio de variables:

)(xhzy −= (B.6)

Esto cambia el estado estacionario de z al origen.

La derivada (B.6) puede expresarse de la siguiente forma:

xxh

zy &&&∂∂

−= (B.7)

En vista del cambio de variables el modelo completo (B.1)-(B.2) toma la siguiente forma:

)),(,( εxhyxfx +=& (B.8)

)),(,()),(,( εεεε xhyxfxh

xhyxgy +∂∂

−+=& (B.9)

75

Para analizar (B.9) es necesario que y&ε permanezca finito, aún cuando ε tienda a cero y y&

tienda a infinito. Por lo tanto es conveniente el siguiente cambio de variables:

τ

εddy

dtdy

= ; entonces, ε

τ 1=

dtd

esta nueva variable de tiempo τ nos permite:

)),(,()),(,( εεετ

xhyxfxh

xhyxgddy

+∂∂

−+= (B.10)

Haciendo 0=ε el modelo (B.10) se reduce a:

)0),(,( xhyxgddy

+=τ

(B.11)

A (B.11) se le conoce como sistema de capa límite y representa a la respuesta rápida del

sistema completo.

76

Apéndice C

Modelo Dinámico de una Muñeca Esférica

En esta tesis se presentan simulaciones numéricas y pruebas experimentales realizadas en

una muñeca mecánica de tres grados de libertad, que se mueve en el espacio tridimensional.

El modelo y los parámetros del robot están dados por

=

333231

222221

131211

)(

MMM

MMMMMM

qM

donde:

( )( )( )

006199.0)(0)()(

04988.0)(cos006199.0)()(

cos004262.0)()(cos016620.01095.0)(

33

3113

22

23113

22112

211

===

===

==−=

qMqMqM

qMqqMqM

qqMqMqqM

=

333231

222221

131211

),(

CCC

CCCCCC

qqC &

donde:

( )( ) ( ) ( )23222112

2211

sin00309.0sin004262.02sin01662.0),(2sin01662.0),(

qqqqqqqqCqqqqC

&&&&

&&−−=

=

77

( )( ) ( )

( )( )( )

0),(sin00309.0),(

sin00309.0),(

sin00309.0),(0),(

sin00309.02sin01662.0),(sin00309.0),(

22

2131

2231

2223

22

232121

2213

=−=

−=

==

+−=−=

qqCqqqqC

qqqqC

qqqqCqqC

qqqqqqCqqqqC

&

&&

&&&&

&

&&&&&

=

3

2

1

)(

g

gg

qg

( )

0)()cos()sin(3353.0)(

)sin()cos(3353.0sin1563.0)(

2

212

2111

==

+=

qgqqgqg

qqgqgqg

78

Apéndice D

Modelo del Robot en Simulink

A continuación se presenta el modelo de la muñeca esférica y el controlador realizados en

Simulink para las simulaciones requeridas en esta tesis.

Figura D.1 Modelo General

79

Figura D.2 Submodelo Robot

80

Figura D.3 Submodelo controlador

81

Bibliografía

Alvarez-Ramirez, J., Cervantes I., Kelly R., “PID regulation of robot manipulators: stability

and performance”, System and Control Letters, vol. 41, pp. 73-83 (2000)

Caccavale F., Siciliano B., Villani L., “The Role of Euler Parameters in Robot Control”,

Asian J. of Control, vol. 1, no. 1, pp. 25-34 (1999)

De Luca A., Manes C., “Modeling of robots in contact with a dynamic enviroment”, IEEE

Trans. Rob. Autom, vol. 10, pp. 542-548 (1994)

Grossman S. I., Elementary linear algebra with applications, Saunders College Publishing,

NY (1994)

Hoppensteadt F., “Asymptotic Stability of Singular Perturbation Problems II: Problems

Having Matched Asymptotic Expansion Solutions”, J. Diff. Equations, vol 15, pp. 510-521.

(1974)

Hauser W., Introduction to the Principles of Mechanics, Addison-Wesley Publishing

Company, NY (1967)

Jiang Y. A., Park J. S., Clements D. J., Hesketh T., “Adaptive Control of Robot

Manipulators in Task Space”, pp 147-151, (1994)

Kelly R., “Regulation of manipulators in Generic Task Space: An Energy Shaping plus

Damping Injection Approach”, IEEE Trans. Rob. Autom, vol. 11, No. 5, pp. 766-770.

(1999)

82

Klumpp A.R., “Singularity-free extraction of a quaternion from a directional cosine

matrix”, J Spacecraft, vol. 13, pp. 754-755 (1976).

Lenz R.K., Tsai R.Y., “Techniques for calibration of the scale factor and image center for

high accuracy 3-D machine vision metrology”, IEEE Trans. Pattern Anal. Machine Intell.,

vol 10, no. 5, pp. 713-720, (1988).

Martinez A., Control Visual de robots utilizando controladores PID . (Tesis de maestría).

Departamento de Control Automático, CINVESTAV-IPN, (2002)

Miyazaki F., Arimoto S., “Sensory feedback for robots manipulators”, J. Robot Syst., vol.

2, no. 1, pp. 53-71 (1985)

Ortega R., Kelly R., Loria A, “A Cass of Output Feedback Globally Stabilizing Controllers

for flexible Joints Robots”, IEEE Trans. Rob. Autom, vol. 11, No. 5, pp. 766-770. (1995)

Ortega R., Kelly R., Loria A, “A semiglobally stable output feedback PI2D regulator for

robot manipulators, IEEE Trans Automat. Contr. vol. 40, pp 1432-1436. (1995)

Sciavicco L., Siciliano B., Modeling and Control of Robots Manipulators, Springer-Verlag,

London (2000).

Van Der Kooij H., Jacobs R., Koopman B., Grootenboer H., Biol. Cybern., vol. 80, pp 299,

(1999).