View
0
Download
0
Category
Preview:
Citation preview
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
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
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
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
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).
Recommended