UNIVERSIDAD CATÓLICA SANTA MARÍA

FACULTAD DE CIENCIAS E INGENIERÍAS FÍSICAS Y FORMALES

PROGRAMA PROFESIONAL DE INGENIERIA ELECTRONICA

PRÁCTICAS DE CONTROL NO LINEAL

CINEMATICA INVERSA

AUTOR:

DOCENTE:

- M.Sc.Ing. Juan Carlos Copa Pineda

GRUPO Nº 2

HORARIO: MARTES DE 3 A 5 PM
Cinemática Inversa

1. Marco Teórico
Cinemática Inversa
- El conocimiento de la cinemática inversa del brazo mecánico nos permite obtener,
mediante conocimiento de la posición y orientación deseadas para el efecto final, el
valor que deben de tener los ángulos entre los eslabones de mecanismo.
- A diferencia de la cinemática directa, en este caso no existe una manera sistemática
de obtener una solución, siendo el procedimiento de obtención fuertemente
dependiente de la configuración del robot.
- Para resolver sistemas de pocos grados de libertad se recomiendo el uso de:
a) Resolución por métodos geométricos.
Este es el método mas sencillo y consiste en la resolución de la cinematica usando
solamente cálculos geométricos simples.
Al igual que la cinematica directa el calculo se simplificara al analizar solamente el
movimiento en un plano en vez de analizarlo en el espacio. Tomando como base
las variables de longitud de eslabones y punto final, ahora lo que se buscara
encontrar los ángulos para esa posición.
2. Desarrollo
- Determinar θ1 y θ2, siendo L1=20∗√ ( 2 ) cm . Y L2=20∗√ ( 2 ) cm., si el brazo robótico
debe posicionarse en X p =28.28 cm y Y P=28.28 cm emplee el software Matlab para
animar el desplazamiento.

Programa MatLab

xx1 =
 -0.0087 90.0173

iter =

Xp =

28.2800

Yp =

28.2800

L1=20*sqrt(2);
L2=20*sqrt(2);
xp=28.28;
yp=28.28;
error1=1.e8;
a1=30;
a2=30;
xx(1)=a1*pi/180;
xx(2)=a2*pi/180;
iter=0;
itermax=50;

while error1>1.e-6
iter=iter+1;
x=xx(1);
y=xx(2);
f(1)=L1*cos(x)+L2*cos(y+x)-xp;
f(2)=L1*sin(x)+L2*sin(y+x)-yp;
J(1,1)=-L1*sin(x)-L2*sin(y+x);
J(1,2)=-L2*sin(y+x);
J(2,1)=L1*cos(x)+L2*cos(y+x);
J(2,2)=L2*cos(y+x);
y=-J\f';

xx=xx+y';

error1=sqrt(y(1)*y(1)+y(2)*y(2));
error(iter)=sqrt(f(1)*f(1)+f(2)*f(2));
ii(iter)=iter;
if(iter>itermax)
error1=0;
s=sprintf('no convergio en las iteraciones',itermax);
disp(s)
end
end
x=xx(1);
y=xx(2);
f(1)=L1*cos(x)+L2*cos(y+x)-xp;
f(2)=L1*sin(x)+L2*sin(y+x)-yp;
 xx1=xx.*(180/pi);
while(xx1(1,1)<-360)
xx1(1,1)=xx1(1,1)+360;
end
while(xx1(1,2)>180)
xx1(1,2)=xx1(1,2)-360;
end
xx1
iter
Xp=L1*cos(xx1(1,1)*pi/180)+L2*cos(xx1(1,1)*pi/180+xx1(1,2)*pi/18
0)
Yp=L1*sin(xx1(1,1)*pi/180)+L2*sin(xx1(1,1)*pi/180+xx1(1,2)*pi/18
0)
x1=[0 L1*cos(xx1(1,1)*pi/180)]
y1=[0 L1*sin(xx1(1,1)*pi/180)]
x2=[L1*cos(xx1(1,1)*pi/180)
L1*cos(xx1(1,1)*pi/180)+L2*cos(xx1(1,1)*pi/180+xx1(1,2)*pi/180)]
y2=[L1*sin(xx1(1,1)*pi/180)
L1*sin(xx1(1,1)*pi/180)+L2*sin(xx1(1,1)*pi/180+xx1(1,2)*pi/180)]
line(x1,y1,'LineWidth',4)
line(x2,y2,'LineWidth',4)
hold on
xlabel('eje X')
ylabel('eje Y')

30

25

20

15
eje Y

10

-5
0 5 10 15 20 25 30
eje X

- Determinar θ1 y θ2, siendo L1=0.5 m Y L2=0.5 m debe posicionarse en X p =0.70 m y

Y P=0.0 m emplee el software Matlab para animar el desplazamiento.
xx1 =
 45.5730 -91.1460
iter =
9
Xp =
0.7000
Yp =
0.0000

Programa MatLab

L1=50;
L2=50;
xp=70;
yp=0;
error1=1.e8;
a1=100;
a2=100;
xx(1)=a1*pi/180;
xx(2)=a2*pi/180;
iter=0;
itermax=50;

while error1>1.e-6
iter=iter+1;
x=xx(1);
y=xx(2);
f(1)=L1*cos(x)+L2*cos(y+x)-xp;
f(2)=L1*sin(x)+L2*sin(y+x)-yp;
J(1,1)=-L1*sin(x)-L2*sin(y+x);
J(1,2)=-L2*sin(y+x);
J(2,1)=L1*cos(x)+L2*cos(y+x);
J(2,2)=L2*cos(y+x);
y=-J\f';

xx=xx+y';

error1=sqrt(y(1)*y(1)+y(2)*y(2));
error(iter)=sqrt(f(1)*f(1)+f(2)*f(2));
ii(iter)=iter;
if(iter>itermax)
error1=0;
s=sprintf('no convergio en las iteraciones',itermax);
disp(s)
end
end
x=xx(1);
y=xx(2);
f(1)=L1*cos(x)+L2*cos(y+x)-xp;
f(2)=L1*sin(x)+L2*sin(y+x)-yp;

xx1=xx.*(180/pi);
while(xx1(1,1)<-360)
xx1(1,1)=xx1(1,1)+360;
 end
while(xx1(1,2)>180)
xx1(1,2)=xx1(1,2)-360;
end
xx1
iter
Xp=L1*cos(xx1(1,1)*pi/180)+L2*cos(xx1(1,1)*pi/180+xx1(1,2)*pi/18
0)
Yp=L1*sin(xx1(1,1)*pi/180)+L2*sin(xx1(1,1)*pi/180+xx1(1,2)*pi/18
0)
x1=[0 L1*cos(xx1(1,1)*pi/180)]
y1=[0 L1*sin(xx1(1,1)*pi/180)]
x2=[L1*cos(xx1(1,1)*pi/180)
L1*cos(xx1(1,1)*pi/180)+L2*cos(xx1(1,1)*pi/180+xx1(1,2)*pi/180)]
y2=[L1*sin(xx1(1,1)*pi/180)
L1*sin(xx1(1,1)*pi/180)+L2*sin(xx1(1,1)*pi/180+xx1(1,2)*pi/180)]
line(x1,y1,'LineWidth',4)
line(x2,y2,'LineWidth',4)
hold on
xlabel('eje X')
ylabel('eje Y')

40

35

30

25
eje Y

20

15

10

0
0 10 20 30 40 50 60 70
eje X

- Se desea ubicar el efector final del mecanismo (figura(2.2)) en xp=0.20m. y ,yp=0m; con los
siguientes datos: l1 = 500mm, l2 = 500mm, l3 = 100mm., el último eslabón siempre es
perpendicular al plano de trabajo (ángulo noventa grados), emplee el software matlab.
Para determinar los ángulos (Newton Raphson) y animar el desplazamiento. Detalle el
procedimiento de solución.
L1=500;
L2=500;
L3=100;
z=300;
xp=0;
yp=100;
error1=1.e8;
a1=30;
a2=30;
xx(1)=a1*pi/180;
xx(2)=a2*pi/180;
iter=0;
itermax=50;

while error1>1.e-6
iter=iter+1;
x=xx(1);
y=xx(2);
f(1)=L1*cos(x)+L2*cos(y+x)-L3;
f(2)=L1*sin(x)+L2*sin(y+x)-yp;
J(1,1)=-L1*sin(x)-L2*sin(y+x);
J(1,2)=-L2*sin(y+x);
J(2,1)=L1*cos(x)+L2*cos(y+x);
J(2,2)=L2*cos(y+x);
y=-J\f';

xx=xx+y';

error1=sqrt(y(1)*y(1)+y(2)*y(2));
error(iter)=sqrt(f(1)*f(1)+f(2)*f(2));
ii(iter)=iter;
if(iter>itermax)
error1=0;
s=sprintf('no convergio en las iteraciones',itermax);
disp(s)
end
end
x=xx(1);
y=xx(2);
f(1)=L1*cos(x)+L2*cos(y+x)-L3;
f(2)=L1*sin(x)+L2*sin(y+x)-yp;

xx1=xx.*(180/pi);
while(xx1(1,1)<-360)
xx1(1,1)=xx1(1,1)+360;
end
while(xx1(1,2)>360)
xx1(1,2)=xx1(1,2)-360;
end
xx1
iter
[x,y,z]=robotico(z,L1,L2,L3,xx1(1,1),xx1(1,2),180-
(xx1(1,1)+xx1(1,2)))
xlabel('eje X')
ylabel('eje Y')
 xx1 =

-36.8699 163.7398
iter =
6
z=
300
x=
-5.1159e-13
y=
100.0000
x1 =
400.0000
x2 =
-300.0000
x3 =
-100
x=
-5.1159e-13
y=
100.0000
z=
300

450

400

350

300

250
eje X

200

150

100

50

-50
-300 -250 -200 -150 -100 -50 0 50 100
eje Y

3. Conclusiones
- Se pueden comprobar las respuestas con la práctica anterior, ya que en esta nos
resulta los ángulos.
- La resolución por métodos geométricos realizados en teoría resulta más sencilla.
- Como se observa en la práctica, la cinemática inversa se encarga de encontrar los
ángulos perfectos para llegar al punto final dado.
- Se usó el método de Newton Raphson para las ecuaciones no lineales.