Está en la página 1de 10

Control por modelo inverso de un brazo articulado

Matlab: Ejecutar invkine

Control inverso de brazo articulado

Sección de código de la función invsurf.m:

Condiciones iniciales:

for i = 1:length(r),

for j = 1:length(theta), xx = r(i)*cos(theta(j));

Cinemática directa

l1 = 10;

yy = r(i)*sin(theta(j));

l2 = 7;

c2 = (xx^2 + yy^2 - l1^2 - l2^2)/(2*l1*l2);

point = 21;

c2 = min(max(c2, -1), 1);

r = linspace(l1-l2, l1+l2, point); theta = linspace(0, 2*pi, 2*point);

s2 = sqrt(1 - c2^2); th2(i, j) = atan2(s2, c2);

k1 = l1 + l2*c2; k2 = l2*s2; th1(i, j) = atan2(yy, xx) - atan2(k2, k1); end

end

Control inverso de brazo articulado (II)

Resultado de ejecutar la función invsurf.m:

Control inverso de brazo articulado (II)  Resultado de ejecutar la función invsurf.m :

Control inverso de brazo articulado (III)

Control inverso de brazo articulado (III)  2 y  1 x x  1 x
 2 y  1 x x
2
y
1
x x

1

x

de brazo articulado (III)  2 y  1 x x  1 x Modelo del

Modelo del brazo articulado

de dos grados de libertad

de brazo articulado (III)  2 y  1 x x  1 x Modelo del

2

y

de brazo articulado (III)  2 y  1 x x  1 x Modelo del
de brazo articulado (III)  2 y  1 x x  1 x Modelo del

Control inverso de brazo articulado (IV)

 x 1  Modelo del brazo articulado 2 de dos grados de libertad y
x
1
Modelo del brazo articulado
2
de dos grados de libertad
y
x
d
Modelo inverso del brazo
articulado
y
d

Control inverso de brazo articulado (V)

Matrices inversas

invkine1

x

x

.

.

x

1

2

n

y

y

1

2

y

n

;

1 1

;

1 2

;

1

n

invkine2

x

.

.

x

x

1

2

n

y

y

1

2

y

n

;

2 1

;

2 2

;

2

n

for i = 1:length(x),

for j = 1:length(y),

xx = x(i);

yy = y(j);

c2 = (xx^2 + yy^2 - l1^2 - l2^2)/(2*l1*l2); s2 = sqrt(1 - c2^2);

th2(i, j) = atan2(s2, c2);

k1 = l1 + l2*c2;

k2 = l2*s2;

th1(i, j) = atan2(yy, xx) - atan2(k2, k1); if abs(c2) < 1;

data1(data_n, :) = [xx yy th1(i, j)];

data2(data_n, :) = [xx yy th2(i, j)];

data_n = data_n + 1; end end

end

invkine1 = data1(1:data_n, :);

invkine2 = data2(1:data_n, :);

Control inverso de brazo articulado (VI)

La función ANFIS

[FIS,ERROR,STEPSIZE]=ANFIS(TRNDATA,INITFIS,TRNOPT,DISPOPT,[],OPTMETHOD) TRNDATA: Datos de entrada-salida

INITFIS: Condiciones iníciales de los parámetros (Funciones de pertenencia, etc…)

TRNOPT(1): Número de épocas

TRNOPT(2): Error medio cuadrático objetivo

TRNOPT(3): Factor de aprendizaje inicial

TRNOPT(4): Razón de decremento del Factor de aprendizaje

TRNOPT(5): Razón de incremento del Factor de aprendizaje

DISPOPT: Visualizar la evolución del aprendizaje

OPTMETHOD: Aprendizaje híbrido o aprendizaje por gradiente descendente

Control inverso de brazo articulado (VII)

Obtención del modelo inverso

Sistema borroso 1: fismat1

[fismat1, error1] = anfis(invkine1, 3, [50, 0, 0.2]);

writefis(fismat1, 'invkine1.fis');

% WRITEFIS (FISMAT,'filename')

1

f

1

(

x y

,

)

Sistema borroso 2: fismat2

[fismat2, error2] = anfis(invkine2, 3, [50, 0, 0.2]);

writefis(fismat2, 'invkine2.fis');

2

f

1

(

x y

,

)

Control inverso de brazo articulado (VIII)

En el programa invkine.m:

fismat1 = readfis('invkine1'); fismat2 = readfis('invkine2');

 x 1  Modelo del brazo articulado 2 de dos grados de libertad y
x
1
Modelo del brazo articulado
2
de dos grados de libertad
y
x
d
theta1 = evalfis([x, y], fismat1);
y
d
theta2 = evalfis([x, y], fismat2);

Control inverso de brazo articulado (IX)

Resultado de ejecutar invkine.m:

Control inverso de brazo articulado (IX)  Resultado de ejecutar invkine.m :