F2

F1

M1 m2

q1

q2

Se determina la energa cintica

1 1 1
= 1 12 + 2 12 + 2 22
2 2 2
Se determina la energa cintica potencial

= 1 1 + 2 1
Se obtiene el Lagrangiano mediante la energa cinemtica y potencial
1 1 1
= 1 12 + 2 12 + 2 22 (1 + 2 )1 + 1 1 + 2 2
2 2 2
Usando

( ) =0

Se necesita determinar:

= (1 + 2 ) + 1 = 2
1 2

= (1 + 2 )1 = 2 1
1 1

( ) = (1 + 2 )1 ( ) = 2 2
1 2
 Se obtienen las siguientes ecuaciones.
(1 + 2 )1 + (1 + 2 ) = 1
2 2 = 2
Con condiciones iniciales 1 (0), 1 (0), 2 (0), 2 (0).

Colocando en forma matricial se obtiene

1 + 2 0 1 ( + 2 )
[ ][ ]+ [ 1 ] = [ 1]
0 2 2 0 2
Diseo del controlador para la siguiente propuesta de trayectoria

La trayectoria deseada est definida por:

q1 1 = 1 + cos() 1 = sin() 1 = 2 cos()

2 = 2 + sin() 2 = cos() 2 = 2 sin()

Se proponen

1 = 2 =
2

<
2
Con L, el espacio de trabajo del robot

q2
QQ
Usando el siguiente diagrama de control

1
Trayectoria deseada

1
1
Control

2 -__ 2

2 2
Se definen los errores para las posiciones y las velocidades mediante las siguientes ecuaciones:

1 = 1 1 2 = 2 2

1 = 1 1 2 = 2 2

Usando 1 y 2 como las posiciones deseadas y las velocidades deseadas estn dadas por 1 y 2 .

Derivando las ecuaciones del error de velocidad y sustituyendo las ecuaciones obtenidas para el sistema
se generan las siguientes ecuaciones

1 = 1 1 2 = 2 2

1 2
1 = + 1 2 = 2
1 + 2 2

Donde
1 2
1 = + 2 =
1 + 2 2

Se obtiene

1 1 1 2 2 1
1 = = 2 = =
1 + 2 1 + 2 1 2 2 2

Estas ecuaciones definen el de error de aceleracin en trminos de los errores

1 = 1 1 Y 2 = 2 2

Una propuesta de solucin para las ecuaciones diferenciales del error es definida mediante la siguiente
ecuacin de segundo grado. De la teora de control podemos determinar que estas ecuaciones son estable
y anlogas a nuestro sistema.

1 + 21 1 1 + 1 2 1 = 0 2 + 22 2 2 + 2 2 2 = 0

Estas ecuaciones son estables para valores de que cumplan con el criterio de Routh - Hurwitz.

Sustituyendo las ecuaciones propuestas para la solucin y despejando para 1 y 2 se obtiene:

1 = 1 (1 + 2 )[21 1 + 2 1 ]

2 = 2 2 [22 2 2 + 2 2 2 ]

Estas ecuaciones las definimos como el control nominal del sistema.

Simulacin

Ecuaciones de la trayectoria
q1des=Q1+R*cos(lamb*t);
q2des=Q2+R*sin(lamb*t);

q1dotdes=-R*sin(lamb*t)*lamb;
q2dotdes=R*cos(lamb*t)*lamb;

q1ddotdes=-R*cos(lamb*t)*lamb*lamb;
q2ddotdes=-R*sin(lamb*t)*lamb*lamb;
Ecuaciones del error
e1=q1-q1des;
e2=q2-q2des;

e1dot=q1dot-q1dotdes;
e2dot=q2dot-q2dotdes;
Ecuaciones del control
F1=(m2+m1)*q1ddotdes+(m1+m2)*g-(m1+m2)*[2*damp1*wn1*e1dot+wn1*wn1*e1];
F2=m2*q2ddotdes-m2*[2*damp2*wn2*e2dot+wn2*wn2*e2];

Ecuaciones del sistema

q1ddot=F1/(m1+m2)-g;
q2ddot=F2/m2;

Diagrama esquemtico del subsistema usado

 Diagrama esquemtico del sistema usado en la simulacin

Los valores propuestos para la simulacin son los siguientes.

1 = 2 = 2; 1 = 2 = 1; 1 = 2 = 1; = 10; = 9.81; = .5; 1 = 2 = .5 ;

1 (0) = 4, 1 (0) = 0, 2 (0) = 3, 2 (0) = 0.

Conclusiones

Mediante el desarrollo del presente trabajo se puede concluir que un problema de seguimiento de
trayectoria se puede solucionar definiendo un error el cual se trata de llevar a un valor de cero y este debe
de ser mediante una solucin que sea estable lo cual se define mediante los criterios usados para
estabilidad en cursos anteriores, tambin el desempeo del control depende de los parmetros de diseo
1 , 2 , 1 2, se puede apreciar que al aumentar la velocidad rotacin de la trayectoria deseada,
el sistema tarda ms tiempo en llegar a la trayectoria deseada, si se requiere disminuir el tiempo esto se
se traduce en un aumento de la fuerza requerida por los motores los cuales proporcionan un par limitado
por sus caractersticas fsicas.