Control de robots manipuladores en espacio de tarea: aplicación en el control de posición y orientación

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

) (x

xf

∇ Gradiente de la función f(x)

AT 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

) (q

M Matriz de inercias del robot manipulador )

, (q q

C & Matriz de Coriolis y fuerzas centrípetas )

(q

g Vector de pares gravitacionales )

ˆ q(

g 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 amortiguamiento¹ (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.

1Energy 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.

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.

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.

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.

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

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 R³), 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 R , 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.

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

(

^x,y,z

)

^{, 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.

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.

Figura 2.3 Sistema Cilíndrico

Figura 2.4 Robot de geometría cilíndrica

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 q , ₁ q , ₂ q , 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.

Figura 2.6 Robot SCARA

Figura 2.7 Robot antropomórfico

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).

















=

















=

z z z y z x

y z y y y x

x z x y x x a

a a R

T T T

T T T

T T T

' '

'

' '

'

' '

'

3 2

1 (2.1)

donde x denota la transposición de x. ^T

Note que los vectores columna que conforman R (a , ₁ a , ₂ a₃) 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:

R^TR =I3 (2.2)

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

Es necesario hacer notar que la matriz de rotación presenta el problema de proporcionar una descrip ció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.

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

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 ^k=

[

^{kx ky kz}

]

^Tel 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 R(θ,k), la cual se puede calcular de la siguiente forma:

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

^















+

− +

− +

−

+

− +

− +

−

+

− +

− +

−

=

θ θ θ

θ θ

θ

θ θ

θ θ θ

θ

θ θ

θ θ

θ θ

θ

c c k s k c k k s k c k k

s k c k k c c k s k c k k

s k c k k s k c k k c c k k

R

z x

z y y z

x

x z

y y

z y

x

y z

x z y

x x

1 1

1

1 1

1

1 1

1 )

, (

2 2

2

(2.3)

donde c_θ =cosθ y s_θ =senθ

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





 



 + + −

= ⁻

2 cos ¹ r¹¹ r²² r³³ 1 θ

(2.4)

















−

−

−

=

12 21

31 13

23 32

2 1

r r

r r

r r k sen

θ

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

















=

33 32 31

23 22 21

13 12 11

r r r

r r r

r r r R

def

Note que R(θ,k)=R(−θ,−k), 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:

^{QT =}

[

^{η εT}

]

⁼

[

^{η εx εy εz}

]

(2.5)

Considérese dos sistemas coordenados F₀ y F , 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:

k



 



= 



 



= 

sin 2 2 ;

cos θ ε θ

η (2.6)

nótese que sí

[

^{η εT}

]

es el cuaternión que representa la rotación del sistema coordenado F ₁ con respecto a un sistema coordenado F₀, entonces

[

^{n −εT}

]

representa la orientación de

F0 con relación a F ₁

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:

η² +ε²x +ε²y+ε²z =1 (2.7)

b) Unicidad. Si se considera el ángulo θ acotado al rango −180°≤θ ≤180°, 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:

^{R =}

(

^2η2−1

)

^{I3+2εεT +2ηS(ε)} (2.8)

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

















−

−

−

=

0 0 0 ) (

x y

x z

y z

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):

( )

( )

( )

^















+

−

−

−

+

−

−

−

+

−

−

−

=

+ + +

=

1 sgn

1 sgn

1 sgn

2 1

2 1 1

22 11 33 12 21

33 11 22 31 13

33 22 11 23 32

33 22 11

r r r r r

r r r r r

r r r r r

r r r

ε η

(2.10)

donde r son elementos de la matriz de rotación R. _ij

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

















−

−

− +

+

= +

+ + +

=

12 21

31 13

23 32

33 22 11

33 22 11

1 2

1 2 1 1

r r

r r

r r r r r

r r r

ε η

(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 r₁₁>r₂₂ y r₁₁>r₃₃

( )

( )

( )





















+

= +

= +

+

−

−

=

s r r

s r r s

s r r

r r r s

23 32

21 12

23 32

33 22 11

5 . 0

1

ε η

Si r₂₂ >r₁₁ y r₂₂>r₃₃

( )

( )

( )





















+ +

=

= +

+

−

−

=

s r r

s s r r

s r r

r r r s

23 32

21 12

31 13

33 11 22

5 . 0

1

ε η

Si r₃₃ >r₁₁ y r₃₃>r₂₂

( )

( )

( )



















 + +

=

= +

+

−

−

=

s s r r

s r r

s r r

r r r s

5 . 0

1

31 13

23 32

21 12

22 11 33

ε η

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 p y ⁰ p ¹ respectivamente. Sea d el vector que denota la ubicación del origen del sistema 1 respecto ₁⁰ al sistema 0, y sea R 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 p del punto P, si conocemos ⁰ p , aplicando la siguiente regla de transformación: ¹

p⁰ =d₁⁰+R₁⁰p¹ (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

~ p

p (2.14)

Adoptando esta representación para los vectores p⁰ y p¹, la transformación de coordenadas puede ser escrita en términos de una matriz de 4×4, como sigue:





 



=

1 0

0 1 0 1 0

1 T

d

A R (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:

~p⁰ = A₁⁰~p¹ (2.16)

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.





 



=

1 0^T

b e b e b e

d T R

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

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.

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.

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.

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

k k k

q Q K q K dt

d

∂ + ∂

=

∂

∂

• (3.1)

donde K es la energía cinética del sistema y Q_k 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:

_k

k

K q

Q P +τ

∂

− ∂

=

De esta forma (3.1) puede escribirse como:

_k

k k k

q P q

K q

K dt

d +τ

∂

− ∂

∂

= ∂

∂

∂

• (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:

L=K−P (3.3)

De (3.3) se deriva que:

k

k q

K q

L

•

• ∂

= ∂

∂

∂ y

k k

k q

P q K q

L

∂

− ∂

∂

= ∂

∂

∂ (3.4)

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

k k

k q

L q

L dt d

∂ + ∂

∂ =

∂ τ

& (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 ecuacio nes de Euler-Lagrange en este caso toman la siguiente forma:

_q ^T

qL q q L q q U

dt

d −∇ =

∇• ( ,^•) ( ,^•) (3.7)

donde q∈Rⁿ son las coordenadas generalizadas y U∈Rⁿ los pares debidos a las fuerzas externas. Mientras ∇(⋅) representa el gradiente, un vector renglón que se define como

k

def

k ⋅ =∂ ⋅ ∂

∇ () ()/ , y (⋅)^Tdenota transposición. Retomando la ecuación (3.3) de la forma:

L(q,q^•)=K(q,q^•)−P(q)

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

•

•

• = q M q q

q q K

T

) 2 (

) 1 ,

( (3.8)

En donde M(q)∈R^n×n es la matriz de inercia, la cual es simétrica y positiva definida. La función P(q) es la energía potencial, que es acotada inferiormente.

Los pares debidos a las fuerzas externas U∈Rⁿ consisten en acciones de control τ ∈Rⁿ, 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:

M(q)q&&+C(q,q&)q&+g(q)=τ (3.9)

donde :

Rn

q q q

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

q

g( )∈ es el vector de pares gravitacionales Rn

∈

τ es el vector de pares de entrada Rn

q∈ es el vector de posiciones angulares de los eslabones

Además g(q) se obtiene como gradiente de la energía potencial U(q) debida a la gravedad, es decir:

g(q)=∇_qU(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.

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 M(q)∈R^n×n cumple con las siguientes propiedades

P.1 Es simétrica y positiva definida.

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

escalares¹ tales que

µ₁I<M(q)<µ₂I

P.3 Es continuamente diferenciable.

1Para el cálculo de las constantes µ₁ ^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álcu los más rápidamente.

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 ( , )

− C q q•

q

M& 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 k_g tal que

k_g > Dg(q)

donde

q q q g

Dg ∂

= ∂ ( ) )

(

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.

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 p_d y una orientación deseada R_d de manera asintótica, es decir

t d t d

R t R

p t p

=

=

∞

→

∞

→

) ( lim

) ( lim

(4.1)

donde: p(t)∈R³ es la posición y R(t)∈R^3×3 la matriz de orientación del órgano terminal como se vio en el capítulo 2, en tanto p_d ∈R³ y R_d∈R^3×3 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

t d

t t

p t p

ε η ε

η ) (

) lim (

) ( lim

(4.2)

donde

[

ηd εd

]

^T∈^R4 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:

lim ~=0

∞

→ y

t (4.3)

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



 



=

−

= e

y p y

y _d

~ ~

y ~p p_d p(q)

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

e=η(q)ε_d −η_dε(q)+S(ε)ε_d (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 R³ en vez de R⁴. Las velocidades lineal p^• ∈R³ y angular ω∈R³ de un punto P₀ están dadas por:

q J q q q

J q J p

G o

p & &

&

) ) (

( ) (  =

 



=



 





ω (4.5)

donde J_p(q) es la matriz de 3×n relativa a la contribución de las velocidades de junta a la velocidad lineal del órgano terminal, y J_o(q) es la matriz de 3×n relativa a la contribución de las velocidades de junta a la velocidad angular del órgano terminal, y

n

G q R

J ( )∈ ^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:

~y^• =−Q(η_e,e)J_G(q)q& (4.6) con:













= +

) 2 ( 1 2

0 1

0 )

,

( I S e

I e Q

e

e η

η

donde ηe =ηηd +εd^Tε y I∈R^3×3 es la matriz identidad. Cuando el error de orientación es cero, Q(η ,e) es una matriz diagonal.

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 g(q). Sin embargo la función

) (q

g 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 gˆ q( ) 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:

U (q,y ,~y(q)) F(q)

d q a

q −∇• &

−∇

=

τ (4.7)

donde:U_a(q,y_d,y~(q)):Rⁿ×R^m×R^m →R es la energía potencial artificial y es una función continuamente diferenciable. En tanto F( &q):Rⁿ → R 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:

U_a(q,y_d,~y(q))=U_T(y_d,~y(q))−U(q) (4.8)

Donde U_T(y_d,~y(q)) es la energía potencial total y U(q) 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:















∇ +∇ +

= −





•

− U y y q F q C q q q

q M

q q

q dt

d

d q T

q & & &

&

& ( ) ¹ ( ,~( )) ( ) ( , ) (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:

q K q q F

q U y K y q

y y q U

D T

p T d

a

&

&

&

2 ) 1 (

)

~ (

~ 2 )) 1

~( , , (

=

−

=

(4.10)

donde K_P,K_D ∈R^n×n son matrices simétricas positivas.

Haciendo las siguientes definiciones de la ecuación de salida

y =h(q) (4.11)

donde h:Rⁿ→R^m es una función continuamente diferenciable, y se supone que m≤ n. Al mapa de salida h(q) 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 h(q)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 yd∈Rⁿ 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 y_d en el espacio de tarea.

Suposición 1: La función de tarea y=h(q) es invertible en una vecindad N_q∈Rⁿ de q_d donde:

¹( _d)

def

d h y

q = ⁻

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

∇_qU_a(q,y_d,~y(q))=∇_q~y^TK_p~y−∇_qU(q) (4.12)

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

~ J(q) q

y h

y _q

q =

∂

= ∂

∇

=

∇

donde J(q)∈R^n×n es el Jacobiano de la función de tarea h(q), y para la cual se puede hacer la siguiente suposición:

Suposición 2: La matriz Jacobiana J(q)∈R^n×n tiene rango completo en N , es decir, que _q 0

) (q ≠

J y además existe una constante positiva α tal que J J(q) ≤α_J, para toda Nq

q∈

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

∇_qU_a(q,y_d,~y(q))=J^T(q)K_p~y−∇_qU(q)

y aplicando (3.10) resulta:

∇_qU_a(q,y_d,~y(q))=J^T(q)K_p~y−g(q) (4.13)

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

∇_q&F(q&)= K_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.

τ =−J(q)^TK_p~y−K_Dq&+g(q) (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

) (q

g es la cancelación exacta de los efectos gravitacionales.