Control par calculado para un manipulador robotico RR

Introduccin
Se pretende realizar el control para un robot tipo RR en cual ser implemen-
tado en Matlab donde se generar el algoritmo para generar la trayectoria as
como el algoritmo para el control que deber contener el modelo dinmico del
manipulador y el sistema a controlar estar en Working Model donde se podr
apreciar el comportamiento del robot, y para lograr establecer una comuni-
cacin entre Matlab y Working Model se utilizar el programa MATLABProxy.
Desarrollo
Para este caso se pretende utilizar un robot en conguracin RR con es-
labones de forma triangular con las siguientes especicaciones:
base: / = 0.5 m
altura: a = 0.2 m
1 =
p
/
2
+a
2
1 = 0.54 m
Donde la longitud al centro de masa 1
c
,esta dado por:
1
c
=
_
_
2b
3
_
2
+
_
a
3
_
2
1
c
= 0.3399 m
Espesor del eslabon o = 0.02 m
Donde la densidad del material es
j = 2700 kg, m
2
La masa del eslabon esta dada por:
: = j(/a
s
2
)
: = 2.7 kg
1
z
=
m(b
2
+a
2
)
18
1
z
= 0.0435 kg, m
2
La tarea que se pretende que realice el manipulador es el seguimiento de una
trayectoria predenida donde se establen una serie de puntos que recorrera el
manipulador que sern:
1
1
= (0, 1.08)
1
2
= (0.4, 0.7)
1
3
= (0.4, 0.5)
1
4
= (0.6, 0.5)
1
5
= (0.6, 0.7)
1
6
= (0.4, 0.7)
1
1
= (0, 1.08)
No obstante, el punto inicial y el punto nal de la trayectoria es el punto
donde empezar y terminar el recorrido de la trayectoria se le conoce como
punto "home".
Cabe recalcar que para el calculo de trayectorias existen varios metodos pero
para este caso se utilizarn polinomios de Bezier ya que genera curvas suaves y el
calculo computacional ya estaba implementado con anterioridad con un tiempo
1
de recorrido t = 10 s, para este caso en particular el tiempo de recorrido ser de
t = 1 s.
Debido a que el manipulador trabaja en el espacio cartesiano y es necesario
conocer la variables articulares del manipulador, se utiliza la cinemtica inversa
para obtener estas variables articulares para cada punto de la trayectoria que
sern de utilidad como variables de control (
d
) o valor deseado, mismo que
se necesita conocer la velocidad angular ( _
d
) aplicando la primer derivada y
la aceleracin angular (
d
) aplicando la segunda derivada mismos que sern de
utilidad para utilizar el metodo de par calculado.
Cinematica Directa
Para determinar la posicion del efector nal del manipulador mediante sus
repectivos angulos es necesario obtener los parametros D-H, una vez realizado
esto se realizan las transformaciones desde la base hasta el efector nal para
conocer asi las coordenadas cartesianas del efector nal.
Se denen los parametros de D-H
i c
i1
a
i1
d
i
0
i
1 0 0 0 0
1

2
2 0 1
1
0 0
2
3 0 1
2
0 0
Donde
0
T
1
=
_

_
cos (0
1
) sin(0
1
) 0 0
sin(0
1
) cos (0
1
) 0 0
0 0 1 0
0 0 0 1
_

_
1
T
2
=
_

_
cos (0
2
) sin(0
2
) 0 1
1
sin(0
2
) cos (0
2
) 0 0
0 0 1 0
0 0 0 1
_

_
2
T
3
=
_

_
1 0 0 1
2
0 1 0 0
0 0 1 0
0 0 0 1
_

_
0
T
3
= (
0
T
1
)(
1
T
2
)(
2
T
3
) =
_

_
cos (0
1
) sin(0
1
) 0 0
sin(0
1
) cos (0
1
) 0 0
0 0 1 0
0 0 0 1
_

_
_

_
cos (0
2
) sin(0
2
) 0 1
1
sin(0
2
) cos (0
2
) 0 0
0 0 1 0
0 0 0 1
_

_
_

_
1 0 0 1
2
0 1 0 0
0 0 1 0
0 0 0 1
_

_
=
0
T
3
=
_

_
cos (0
1
+0
2
) sin(0
1
+0
2
) 0 1
2
cos (0
1
+0
2
) +1
1
cos 0
1
sin(0
1
+0
2
) cos (0
1
+0
2
) 0 1
2
sin(0
1
+0
2
) +1
1
sin0
1
0 0 1 0
0 0 0 1
_

_
Cinemtica Inversa
Analogamente, la cinemtica inversa es utilizada para obtener los angulos
de articulacin una vez conocida la posicin del efector nal respecto a la base
del manipulador.
2
La mejor forma de obtener los angulos para el manipulador, es representadolo
de la siguiente manera:
Fig. Metodo utilizado para obtener los angulos
Donde:
1
1
:Distancia del eslabon 1
1
2
:Distancia del eslabon 2
j
2
= r
2
+j
2
j =
_
r
2
+j
2
1
2
2
= 1
2
1
+j
2
21
1
j cos ,
despejando ,
21
1
j cos , = 1
2
2
1
2
1
j
2
cos , =
(L
2
1
+
2
)L
2
2
2L1
, = arccos
_
(L
2
1
+
2
)L
2
2
2L1
_
0 = arctan
_
y
x
_
Ahora se tiene que

2
=
1
+, +0

1
=

2
, 0
sustituyendo se tiene

1
=

2
arccos
_
(L
2
1
+
2
)L
2
2
2L1
_
arctan
_
y
x
_
Ahora para el calculo del siguiente angulo se considera:
j
2
= 1
2
2
+1
2
1
21
1
1
1
cos
j
2
1
2
2
1
2
1
= 21
1
1
1
cos
cos =
(L
2
2
+L
2
1
)
2
2L1L1
= arccos
_
(L
2
2
+L
2
1
)
2
2L1L1
_
3
Ahora se tiene que
= +
2

2
= +

2
= arccos
_
(L
2
2
+L
2
1
)
2
2L1L1
_
Modelo Dinmico
El modelo dinmico para este manipulador en particular ya fue obtenido
con anterioridad mediante la metodologia de Lagrange, slo se sintetizar el
procedimiento mediante el cual se obtuvo dicho modelo y consecuentemente
slo se representar el modelo en la conguracin de espacio de estados.
Procedimiento
1. Calcular la posicin del centro de masa a cada uno de los eslabones del
robot.
2. Calcular las velocidades del centro de masa a cada uno de los eslabones.
3. Calcular la energa cintica para cada eslabn.
/
i
=
1
2
:
i
(
0
\
T
ei
0
\
ei
) +
1
2
i
.
ei
i
1
i
i
.
i
4. Calcular la energia potencial para cada eslabn.
\
i
= :
i
q/
i
5. Obtener el Lagrangiano.
1 =
n
i=1
1
i
n
i=1
\
i
6. Obtener las ecuaciones de movimiento de Lagrange.
t
i
=
d
dt
_
@L
@
_
i

@L
@i
_
La ecuacin en el espacio de estados
A menudo es conveniente expresar las ecuaciones dinmicas de un manipu-
lador en una sola ecuacin que oculte algunos de los detalles, pero que muestre
parte de la estructura de las ecuaciones.
t = ' () +C (, _ ) +G()
t 2 1
n
:Vector de fuerzas y pares de entrada.
2 1
n
:Vector de coordenadas generalizadas.
' () 2 1
nxn
:Matriz de inercia, tiene que ser simtrica y denida positiva.
C (, _ ) 2 1
nxn
:Matriz con las fuerzas centrgas y de coriolis.
G() 2 1
n
:Vector de fuerzas de gravedad.
t = ' +C _ +G
_
t
1
t
2
_
= '
_

0
1

0
2
_
+C
_
_
0
_
0
_
+G
4
' =
_
'
11
'
12
'
21
'
22
_
'
11
= 1
z1
+1
z2
+1
2
c1
:
1
+:
2
_
1
2
1
+1
2
c2
+1
2
2y
:
2
+ 21
1
1
c2
cos (0
2

2
)
_
'
12
= 1
z2
+:
2
(1
2
c2
+1
1
1
c2
cos (0
2
+
2
))
'
21
= 1
z2
+:
2
(1
2
c2
+1
1
1
c2
cos (0
2
+
2
))
'
22
= 1
z2
+:
2
1
2
c2
C =
_
C
11
C
12
C
21
C
22
_
C
11
= 2
_
0
2
1
1
1
c2
:
2
sin(0
2
+
2
)
C
12
=
_
0
2
1
1
1
c2
:
2
sin(0
2
+
2
)
C
21
=
_
0
1
1
1
1
c2
:
2
sin(0
2
+
2
)
C
22
= 0
G =
_
G
11
G
12
_
G
11
= q:
1
1
c1
sin(0
1
+
1
) +q:
2
1
1
sin(0
1
) +q:
2
1
c1
sin(0
1
+0
2
+
1
)
G
12
= q:
2
1
c2
sin(0
1
+0
2
+
1
)
Generacin de trayectoria
Para la generacion de trayectoria de este ejemplo se utiliz un polinomio de
Bezir de quinto orden el cual se representa como
,(t, t
1
, t
2
) =
5
(
1

2
+
3

3
+
5

4
)
Donde :
=
tt1
t2t1
Donde la trayectoria que debe seguir el robot del punto 1
1
= [r
1
, j
1
] al punto
1
2
= [r
2
, j
2
] est dada por las siguientes expresiones
r(t
1
, t
2
) = (r
2
r
1
),(t, t
1
, t
2
) +r
1
j(t
1
, t
2
) = (r
2
r
1
),(t, t
1
, t
2
) +j
1
Control par calculado
A travs de los aos han sido propuestos muchos tipos de sistemas de control
para robots. Como suele pasar, muchos de ellos pueden ser considerados como
casos especiales de la clase de controladores de par calculado. Par calculado, al
mismo tiempo, es una aplicacin especial de la linealizacin de retroalimentacin
para sistemas no lineales, lo cual ha ganado popularidad en la teora de sistemas
modernos de control.
Control par calculado aparece en control robusto, control adaptable, control
de aprendizaje y otros.
Ley de control par calculado
El modelo matematico de un manipulador esta dado por la expresin:
t = ' () +C (, _ ) +G() +t
d
con la variable de articulacin (t) 2 1
n
,el control de par como t(t), y t
d
(t)
como una perturbacin. Si la ecuacin incluye la dinmica de un actuador (para
este caso un motor), en este caso t(t / ) es una entrada de voltaje.
Suponiendo que una trayectoria deseada
d
ha sido seleccionada para el
movimiento del robot, para asegurar el seguimiento de la trayectoria por la
variable de articulacin, se dene una salida o un error de seguimento como:
5
c(t) =
d
(t) (t)
Para demostrar la inuencia de la entrada t(t) en el error de seguimiento,
se deriva dos veces para obtener:
_ c = _
d
_
c =
d

Resolviendo ahora para y sustituyendo en la ecuacin del modelo dinamico
c =
d
+'
1
(C (, _ ) +G() +t
d
t)
Se dene la funcion de control de entrada como
n =
d
+'
1
(C (, _ ) +G() t)
y la funcin de perturbacin como
n = '
1
t
d
Se dene un estado r(t) 2 1
2n
por
r =
_
c
_ c
_
y se expresa la dinmica del error del seguimento como
d
dt
_
c
_ c
_
=
_
0 1
0 0
_ _
c
_ c
_
+
_
0
1
_
n +
_
0
1
_
n
Este es un sistema linear de error en la forma cannica de Brunovsky que
consiste en
n pares de dobles integradores, uno por articulacin. Es manejado por el
control de entrada n(t) y la perturbacin n(t).
La transformacin de la linealizacin de la retroalimentacin puede ser in-
vertida a
t = ' (
d
n) +C (, _ ) +G()
A esto se le conoce como la ley de control par calculado.
Para este caso en particular utilizaremos el control par calculado PD y se
dene de la siguiente forma:
t = ' +C _ +G
=
d
+1
p
c +1
v
_ c
Donde el control propuesto se representa en la siguiente gura
Se implement un programa en Matlab donde se establecen los puntos para
construir la trayectoria, con la posibilidad de denir un gran numero de puntos
siempre y cuando se encuentren en el rango de alcance del robot, un ejemplo
de esto es la denicin de una serie de puntos para denir una trayectoria que
describa la palabra UPIITA.
La funcin trayectoria.m calcula las
d
y _
d
necesarias para el algoritmo de
control par calculado .
Tambin se implementaron la funciones bezier.m y derivx.m que sirven para
calcular las derivadas para el polinomio de Bezir una vez que se dene el orden
del polinomio y a su vez calcular las trayectorias de Bezir a partir de los puntos
dados y el tiempo de recorrido.
6
Figure 1: g. Programa principal donde se establecen los puntos de la trayec-
toria a seguir.
7
Figure 2: g. Funcin que calcula la trayectoria de Bezir.
8
Figure 3: g. funcin bezier.m
Figure 4: g. Funcin derivx.m
9
Para obtener las posiciones articulares a partir de la posicin del efector nal
del robot se utilizo la funcin ciRR.m para realizar la cinemtica inversa.
10