Taller 3 - Cinemática Inversa

1. Dado el manipulador de la Figura 1, determine el modelo cinemático inverso. usando el método

geométrico. ¿Cuántas posibles soluciones existen considerando límites articulares de [ ] para cada
articulación?

1
Cinematica Inversa Doble Pendulo
Se observa que la articulación 2 y 3 representan un doble pendulo, cuya solución fue hallada en clase.

Caso codo arriba

Caso codo abajo

2
Cinematica Inversa Articulación 1

De la ultima ecuación, se tiene que:

Realizando la sustitución

Despejando u, se obtiene que . Con ello, se puede obtener y y por tanto los demas ángulos.

3
Comprobación con cinematica directa
Se halla el modelo de cinematica directa para comprobar los resultados con una configuración arbitraria.

L1 = 2;
L2 = 1;
L3 = 1;
D = 1;
%DH parameters
a = [0;L2;L3];
alpha = [90;0;0] * pi/180;
d = [L1;0;-D];
theta = [0;0;0];
sigma = zeros(3,1);
offset = [-90;90;0]* pi/180;

%Serial Link creation

DH_params = [theta d a alpha sigma offset];
Robot = SerialLink(DH_params)

Robot =

noname (3 axis, RRR, stdDH, fastRNE)

+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 2| 0| 1.571| -1.571|
| 2| q2| 0| 1| 0| 1.571|
| 3| q3| -1| 1| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+

grav = 0 base = 1 0 0 0 tool = 1 0 0 0

0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

Robot.name = "Robot";
PlotRobot(Robot,[20 30 -45])
view(15,30)

4
 T_0T = Robot.fkine(deg2rad([20 30 -45]))

T_0T = 4×4
0.0885 -0.3304 -0.9397 0.8572
-0.2432 0.9077 -0.3420 0.5687
0.9659 0.2588 0.0000 3.8320
0 0 0 1.0000

Se eligen los puntos obtenidos con la anterior configuración

xp = T_0T(1,4);
yp = T_0T(2,4);
zp = T_0T(3,4);

ra = sqrt(D^2 + L1^2)

ra = 2.2361

alpha = atan(L1/D)

alpha = 1.1071

u = roots([ra*cos(alpha)+xp -2*yp ra*cos(alpha)-xp])

u = 2×1
0.4361
0.1763

sin_theta1 = (2.*u)./(1+u.^2)

5
 sin_theta1 = 2×1
0.7328
0.3420

theta1 = asin(sin_theta1)

theta1 = 2×1
0.8224
0.3491

theta1 = theta1(2);
rad2deg(theta1)

ans = 20.0000

p1 = [(xp-ra*cos(alpha)*cos(theta1))*sin(theta1) - (yp-ra*cos(alpha)*sin(theta1))*cos(theta1);
zp-ra*sin(alpha)]

p1 = 2×1
-0.2412
1.8320

cos_theta3 = (norm(p1)^2 - L2^2 - L3^2)/(2*L2*L3);

sin_theta3 = sqrt(1-cos_theta3^2);

theta3 = [atan2(sin_theta3,cos_theta3);atan2(-sin_theta3,cos_theta3)]

theta3 = 2×1
0.7854
-0.7854

Se elige la configuarción codo abajo.

theta3 = theta3(2)

theta3 = -0.7854

rad2deg(theta3)

ans = -45.0000

k1 = L2+L3*cos(theta3);
k2 = L3*sin(theta3);
theta2 = atan2(p1(2),p1(1))-atan2(k2,k1)

theta2 = 2.0944

rad2deg(theta2)

ans = 120.0000

6
Se resta para corresponder con el ángulo de articulación.

theta2 =theta2-pi/2

theta2 = 0.5236

rad2deg(theta2)

ans = 30.0000

theta = rad2deg([theta1 theta2 theta3])

theta = 1×3
20.0000 30.0000 -45.0000

Este mecanismo tiene 4 posibles soluciones, ya que hay 2 valores diferentes para y

7
Cinematica Directa
%DH parameters
a = [0;0;0];
alpha = [-30;-60;0] * pi/180;
d = [0;0;0];
theta = [0;0;-90]*pi/180;
sigma = [0;0;1];
offset = [0;0;1];

DH_params = [theta d a alpha sigma offset];

Robot = SerialLink(DH_params)

Robot =

noname (3 axis, RRP, stdDH, fastRNE)

+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+

8
 | 1| q1| 0| 0| -0.5236| 0|
| 2| q2| 0| 0| -1.047| 0|
| 3| -1.571| q3| 0| 0| 1|
+---+-----------+-----------+-----------+-----------+-----------+

grav = 0 base = 1 0 0 0 tool = 1 0 0 0

0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

Se halla la MTH de la base a la herramienta y cada una de las MTH's intermedias.

syms q1 q2 d3
A1 = trotz(0)*transl(0,0,d(1))*transl(a(1),0,0)*trotx(alpha(1))

A1 = 4×4
1.0000 0 0 0
0 0.8660 0.5000 0
0 -0.5000 0.8660 0
0 0 0 1.0000

A2 = trotz(q2)*transl(d(2),0,0)*transl(a(2),0,0)*trotx(alpha(2))

A2 =

A3 = (trotz(theta(3))*transl(0,0,1+d3)*transl(a(3),0,0)*trotx(alpha(3)))

A3 =

H_BT = A1*A2*A3;
H_BT(1:3,4) = [-6;5;sqrt(3)]

H_BT =

9
Se invierte la primera MTH para obtener la primera ecuación.

H13_1 = vpa(simplify(A1\H_BT),5)

H13_1 =

H13_2 = vpa(A2*A3,5)

H13_2 =

De estas dos matrices se obtiene la ecuación:

D3 = 7

D3 = 7

10
Podemos obtener otra ecuación al reemplazar el valor anterior en la misma MTH.

H13_1 = (simplify(A1\H_BT))

H13_1 =

H13_2 = simplify(subs(A2*A3,d3,D3))

H13_2 =

Se obtiene la ecuación

eq = -4*sqrt(3)*cos(q2) == -2*sqrt(3)

eq =

sol=solve(eq,q2)

sol =

Se escoge el valor de ya que cumple la ecuación

Q2 = pi/3

Q2 = 1.0472

11
Se repite el procedimiento teniendo en cuenta

A1 = trotz(q1)*transl(0,0,d(1))*transl(a(1),0,0)*trotx(alpha(1))

A1 =

H_BT = A1*A2*A3;
H_BT(1:3,4) = [-6;5;sqrt(3)]

H_BT =

H13_1 = simplify(A1\H_BT)

H13_1 =

12
H13_2 = simplify(A2*A3)

H13_2 =

eq = H13_1(1:3,4) == H13_2(1:3,4)

eq =

sol = solve(eq,[q1 q2 d3]);

sol_q1 = sol.q1

sol_q1 =

sol_q2=sol.q2

sol_q2 =

sol_d3=sol.d3

13
 sol_d3 =

En este caso, las soluciones validas son las que no son imaginarias, es decir, y

Cinematica Directa
syms theta1 theta2 theta3 theta4 theta5 theta6 a2 a3 d3 d4
T01=trotz(theta1)*transl([0 0 0])*transl([0 0 0])*trotx(pi/2)

T01 =

T12=trotz(theta2)*transl([0 0 0])*transl([a2 0 0])*trotx(0)

T12 =

T23=trotz(theta3)*transl([0 0 d3])*transl([a3 0 0])*trotx(-pi/2)

T23 =

14
T34=trotz(theta4)*transl([0 0 d4])*transl([0 0 0])*trotx(pi/2)

T34 =

T45=trotz(theta5)*transl([0 0 0])*transl([0 0 0])*trotx(-pi/2)

T45 =

T56=trotz(theta6)*transl([0 0 0])*transl([0 0 0])*trotx(0)

T56 =

T06=T01*T12*T23*T34*T45*T56

T06 =

15
16
Cinematica Inversa
syms O A T

rotO=rotz(O)

rotO =

rotA=roty(A)

rotA =

rotT=rotz(T)

rotT =

raux=[0 1 0;
0 0 -1;
-1 0 0]

raux = 3×3
0 1 0
0 0 -1
-1 0 0

syms nx ny nz sx sy sz ax ay az

R=[nx sx ax; ny sy ay; nz sz az]

R =

izq=simplify(R*inv(rotT))

izq =

17
der=rotO*raux*rotA

der =

Tn=solve(izq(3,2)==der(3,2),T, "Real",true)

Tn =

SA=-izq(3,3)

SA =

CA=-izq(3,1)

CA =

A=(atan2(SA,CA))

A =

A=subs(A,T,Tn(1))

A =

CO=izq(1,2)

CO =

SO=izq(2,2)

SO =

O=atan2(SO,CO)

18
 O =

O=subs(O,T,Tn(1))

O =

Tiene 2 posibles soluciones: codo arriba y codo abajo.

19
Cinematica Directa
L1 = 2;
L2 = 1;
L3 = 0;
L4 = 0;
D1 = 3;
%DH parameters
a = [L1;L2;0;0];
alpha = [0;-180;0;0] * pi/180;
d = [D1;0;0;L4];
theta = [0;0;0;0];
sigma = [0;0;1;0];
offset = [0;0;L3;0];

%Serial Link creation

DH_params = [theta d a alpha sigma offset];
Robot = SerialLink(DH_params)

Robot =

noname (4 axis, RRPR, stdDH, fastRNE)

+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 3| 2| 0| 0|
| 2| q2| 0| 1| -3.142| 0|
| 3| 0| q3| 0| 0| 0|
| 4| q4| 0| 0| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+

grav = 0 base = 1 0 0 0 tool = 1 0 0 0

20
 0 0 1 0 0 0 1 0 0
9.81 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

Robot.name = "Robot1";
figure()
Robot.plot([pi/4 0 0 0],'workspace',[-3 3 -3 3 0 4],'jointdiam',0.5,'nobase')
view(15,30)

T_0T = Robot.fkine(deg2rad([20 30 -45 10]))

T_0T = 4×4
0.7660 0.6428 -0.0000 2.5222
0.6428 -0.7660 0.0000 1.4501
-0.0000 -0.0000 -1.0000 3.7854
0 0 0 1.0000

Cinematica Inversa
La cinematica de las 2 primeras articulaciones corresponde nuevamente a un pendulo doble, por lo que se
usan las mismas ecuaciones anteriores.

El parametro se puede hallar facilmente:

Para establecer una rotación de grados del marco de herramienta con respecto al de base, se tiene que:

21
Existen dos soluciones para este robot correspondientes a codo arriba y codo abajo.

Grados de libertad
Se aplica la ecuación de mobilidad:

Esto significa que el robot SCADA tiene 4 grados de libertad. Estos corresponden a 3 grados de libertad para
moverse en los 3 ejes y un grado de libertad para rotar respecto al eje z de la base.

function PlotRobot(Robot,q)
figure()
Robot.plot(q,'nobase','notiles')
zlim([0 5])
end

22