TALLER CINÉMATICA DIRECTA

Nombre: Oscar D. Cárdenas P Código: 285723

Antes de continuar con el desarrollo del taller es necesario precisar la siguiente convención de
colores según el eje:

Rojo: Eje x

Verde: Eje y

Azul: Eje z

Punto 1

DH Modificada:
 i 𝛼𝑖−1 𝑎𝑖−1 𝑑𝑖 𝜃𝑖
1 0 0 0 𝑞1 +90
2 90 𝐿1 𝐿2 + 𝑞2 0
`

Las transformaciones homogéneas para cada uno de los vínculos son:

𝑐𝜃2 −𝑠𝜃2 0 𝑎1 𝑐0 −𝑠0 0 𝐿1

𝑠𝜃 𝑐𝛼 𝑐𝜃2 𝑐𝛼1 −𝑠𝛼1 −𝑠𝛼1 𝑑2 𝑠0𝑐90 𝑐0𝑐90 −𝑠90 −𝑠90(𝐿2 + 𝑞2 )
1𝑇2 = ( 2 1 )=( )
𝑠𝜃2 𝑠𝛼1 𝑐𝜃2 𝑠𝛼1 𝑐𝛼1 𝑐𝛼1 𝑑2 𝑠0𝑠90 𝑐0𝑠90 𝑐90 𝑐90(𝐿2 + 𝑞2 )
0 0 0 1 0 0 0 1

1 0 0 𝐿1
0 0 −1 −(𝐿2 + 𝑞2 )
1𝑇2 = ( )
0 1 0 0
0 0 0 1

𝑐𝜃1 −𝑠𝜃1 0 𝑎0
𝑠𝜃1 𝑐𝛼0 𝑐𝜃1 𝑐𝛼0 −𝑠𝛼0 −𝑠𝛼0 𝑑1
0𝑇1 = ( )
𝑠𝜃1 𝑠𝛼0 𝑐𝜃1 𝑠𝛼0 𝑐𝛼0 𝑐𝛼0 𝑑1
0 0 0 1

𝑐(𝑞1 + 90) −𝑠(𝑞1 + 90) 0 0

𝑠(𝑞1 + 90)𝑐0 𝑐(𝑞1 + 90)𝑐0 −𝑠0 −𝑠0(0)
=( )
𝑠(𝑞1 + 90)𝑠0 𝑐(𝑞1 + 90)𝑠0 𝑐0 𝑐0(0)
0 0 0 1

−𝑠𝑞1 −𝑐𝑞1 0 0
𝑐𝑞 −𝑠𝑞1 0 0
0𝑇1 = ( 1 )
0 0 1 0
0 0 0 0

Finalmente la transformación homogenea que relaciona el vincula n con la base es:

 −𝑠𝑞1 −𝑐𝑞1 0 0 1 0 0 𝐿1
𝑐𝑞 −𝑠𝑞1 0 0 0 0 −1 −(𝐿2 + 𝑞2 )
0𝑇2 = 0𝑇1 ∙ 1𝑇2 = ( 1 )( )
0 0 1 0 0 1 0 0
0 0 0 0 0 0 0 1

−𝑠𝑞1 0 𝑐𝑞1 𝑐𝑞1 ∗ (𝐿2 + 𝑞2 ) − 𝐿1 ∗ 𝑠𝑞1

𝑐𝑞 0 𝑠𝑞1 𝐿1 ∗ 𝑐𝑞1 + 𝑠𝑞1 ∗ (𝐿2 + 𝑞2 )
0𝑇2 = ( 1 )
0 1 0 0
0 0 0 0

Las coordenadas generalizadas para el extreme del robot según las bases ortogonales definidas en
la base son:

𝑋𝑟𝑜𝑏𝑜𝑡 = 𝑐𝑞1 ∗ (𝐿2 + 𝑞2 ) − 𝐿1 ∗ 𝑠𝑞1

𝑌𝑟𝑜𝑏𝑜𝑡 = 𝐿1 ∗ 𝑐𝑞1 + 𝑠𝑞1 ∗ (𝐿2 + 𝑞2 )`

Los ángulos de giro, ya sea desde la visión de ángulos fijos o móviles son:

𝛽 = 𝑎𝑡𝑎𝑛2(0, √(𝑠 2 𝑞1 + 𝑐 2 𝑞1 ) = 𝑎𝑡𝑎𝑛2(0,1) = 0

𝑐𝑞1 𝑠𝑞1
𝛼 = Atan2 ( ,− ) = 𝐴𝑡𝑎𝑛2(𝑐𝑞1, −𝑠𝑞1)
𝑐0 𝑐0
1 0
𝛾 = Atan2 ( , ) = 𝐴𝑡𝑎𝑛2(1,0) = 90
𝑐0 𝑐0

Se ha desarrollado un código en matlab para comprobar la correcta realización del ejercicio, este
se presenta a continuación:

%Programa para cinemática directa e inversa utilizando el toolbox de

%robotica

%Cabecera del programa

clear all
clc

W=[-15 15 -15 15 -2 2];

%L1=3;
%L2=5;
q1_a=90;
q2_a=4;

%Se crea el sistema serial compuesto por L1, L2 y L3

Link1=Link('d', 0, 'a', 0, 'alpha', 0 ,'revolute','modified')
Link2=Link('theta', 0, 'a', 3, 'alpha',
degtorad(90),'prismatic','modified')

%Se unen en el robot llamado "bot" con el tag "robot de prueba".

robot = SerialLink([Link1 Link2], 'name', 'robot de prueba')

%Cinematica directa, se introduce un vector q y se extrae la matriz de

%transformacion homogenea T:
q_a = [degtorad(q1_a+90) (q2_a)+5]'
T = robot.fkine(q_a)

robot.plot(q_a','workspace',W)

syms cq1 sq1 q2 L1 L2

Ta=[-sq1 -cq1 0 0; cq1 -sq1 0 0; 0 0 1 0; 0 0 0 0];

aTb=[1 0 0 L1; 0 0 -1 -(L2+q2); 0 1 0 0; 0 0 0 1];
Tb=Ta*aTb

cq1=cosd(q1_a)
sq1=sind(q1_a)
L2=5;
L1=3;
q2=q2_a;
subs(Tb)

En el anterior código se obtiene la matriz de transformación para el efector final en función del
sistema ortogonal de la base, mediante la utilización de la herramienta peter Corke, en la parte
inferior se introducen las matrices obtenidas mediante el cálculo a mano y se llega a lo siguiente:

Suponiendo:

𝐿1 = 3 𝑐𝑚 𝐿2 = 5 𝑐𝑚

𝑞1 = 90 𝑞2 = 4 𝑐𝑚
Las matrices obtenidas son idénticas y sus valores son:

−1 0 0 −3
0 0 1 9
0𝑇2 = ( )
0 1 0 0
0 0 0 1

Lo cual traducido a coordenadas generalizadas es:

𝑋 = −3 𝑌 = 9 γ = 90 β = 0 𝛼 = 180

A continuación se presenta la gráfica obtenida mediante Peter Corke:

Suponiendo:

𝐿1 = 3 𝑐𝑚 𝐿2 = 5 𝑐𝑚

𝑞1 = 30 𝑞2 = 3 𝑐𝑚
Las matrices obtenidas son idénticas y sus valores son:

−0.5 0 0.866 5.4282

0.866 0 0.5 6.5981
0𝑇2 = ( )
0 1 0 0
0 0 0 1

Lo cual traducido a coordenadas generalizadas es:

𝑋 = 5.4282 𝑌 = 6.5981 γ = 90 β = 0 𝛼 = 180

 PUNTO 2

i 𝛼𝑖−1 𝑎𝑖−1 𝑑𝑖 𝜃𝑖
1 0 0 q1 0
2 90 0 L3+q2 90
3 90 0 L4+q3 0

𝑐0 −𝑠0 0 0 1 0 0 0
𝑠0𝑐90 𝑐0𝑐90 −𝑠90 −𝑠90(𝐿4 + 𝑞3 ) 0 0 −1 −(𝐿4 + 𝑞3 )
2𝑇3 = ( )=( )
𝑠0𝑠90 𝑐0𝑠90 𝑐90 𝑐90(𝐿4 + 𝑞3 ) 0 1 0 0
0 0 0 1 0 0 0 1

𝑐90 −𝑠90 0 0 0 −1 0 0
𝑠90𝑐90 𝑐90𝑐90 −𝑠90 −𝑠90(𝐿3 + 𝑞2 ) 0 0 −1 −(𝐿3 + 𝑞2 )
1𝑇2 = ( )=( )
𝑠90𝑠90 𝑐90𝑠90 𝑐90 𝑐90(𝐿3 + 𝑞2 ) 1 0 0 0
0 0 0 1 0 0 0 1
 𝑐0 −𝑠0 0 0 1 0 0 0
𝑠0𝑐0 𝑐0𝑐0 −𝑠0 −𝑠0𝑞1 0 1 0 0
0𝑇1 = ( )=( )
𝑠0𝑠0 𝑐0𝑠0 𝑐0 𝑐0𝑞1 0 0 1 𝑞1
0 0 0 1 0 0 0 1

0 0 1 𝐿4 + 𝑞3
0 −1 0 −𝐿3 − 𝑞2
0𝑇3 = 0𝑇1 ∙ 1𝑇2 ∙ 2𝑇3 = ( )
1 0 0 𝑞1
0 0 0 1

Para la posición de home se tiene (se asumen todos los q’s como 0):

𝐿1 = 150𝑚𝑚, 𝐿2 = 250𝑚𝑚, 𝐿3 = 350𝑚𝑚, 𝐿4 = 0 𝑚𝑚

Trabajando en mm se llega a:

0 0 1 0
0 −1 0 −350
0𝑇3 = ( )
1 0 0 0
0 0 0 1

𝑥 = 0 𝑚𝑚, 𝑦 = −350 𝑚𝑚, 𝑧 = 0 𝑚𝑚

La configuración del robot para la posición de home sería como se muestra en la figura contigua:
Para otra posición aleatoria definida así:

𝐿1 = 150𝑚𝑚, 𝐿2 = 250𝑚𝑚, 𝐿3 = 350𝑚𝑚, 𝐿4 = 50 𝑚𝑚

𝑞1 = 50𝑚𝑚, 𝑞2 = −10𝑚𝑚, 𝑞3 = 10 𝑚𝑚

La matriz de transformación es:

0 0 1 60
0 −1 0 −340
0𝑇3 = ( )
1 0 0 50
0 0 0 1

𝑥 = 60 𝑚𝑚, 𝑦 = −340 𝑚𝑚, 𝑍 = 50 𝑚𝑚

Para estos nuevos valores de q, la configuración del robot es la presentada a continuación:

Es importante tener en cuenta que se ha tomado como sistema base el ubicado en la intersección
de L2 y L3, ya que requiere de menos elementos para definir la posición del efector.

En el caso en el cual se quiera tomar la articulación cero, como el punto donde se ubica el sistema
de referencia base, basta solo con agregar los términos correspondientes a L1 y L2 en el vector de
posiciones. Como L1 va en sentido contrario a X, se tomaría de forma negativa, en el caso de L2
tiene el mismo sentido de Z, por lo cual la matriz quedaría así:

0 0 1 𝐿4 − 𝐿1 + 𝑞3
0 −1 0 −𝐿3 − 𝑞2
0𝑇3 = ( )
1 0 0 𝐿2 + 𝑞1
0 0 0 1

Si se quiere además de ubicar el sistema base en la articulación cero, podría ser útil también
definir unos ejes coordenados donde el positivo de x apunte hacia arriba y el positivo hacia la
derecha, esto se podría hacer modificando la fila i de la tabla de DHM.
La tabla DHM quedaría así:

i 𝛼𝑖−1 𝑎𝑖−1 𝑑𝑖 𝜃𝑖
1 0 L1 L2+q1 180
2 90 0 L3+q2 90
3 90 0 L4+q3 0

La matriz que cambiaría respecto al trabajo anterior es:

𝑐180 −𝑠180 0 −𝐿1 −1 0 0 𝐿1
𝑠180𝑐0 𝑐180𝑐0 −𝑠0 −𝑠0(𝐿2 + 𝑞1 ) 0 −1 0 0
0𝑇1 = ( )=( )
𝑠180𝑠0 𝑐180𝑠0 𝑐0 𝑐0(𝐿2 + 𝑞1 ) 0 0 1 𝐿2 + 𝑞1
0 0 0 1 0 0 0 1
Finalmente la matriz de transformación total es:

0 0 −1 𝐿1 − 𝐿4 − 𝑞3
0 1 0 𝐿3 + 𝑞2
0𝑇3 = ( )
1 0 0 𝐿2 + 𝑞1
0 0 0 1

En este caso para la posición de home anteriormente comentada los valores en coordenadas
absolutas del efector son:

𝑥 = 150 𝑚𝑚, 𝑦 = 350 𝑚𝑚, 𝑧 = 250 𝑚𝑚

Para la posición aleatoria los valores serian:

𝑥 = 90 𝑚𝑚, 𝑦 = 340 𝑚𝑚, 𝑧 = 300 𝑚𝑚

Las ecuaciones de cinemática directa son:

𝑋 = 𝐿1 − 𝐿4 − 𝑞3
𝑌 = 𝐿3 + 𝑞2
𝑍 = 𝐿2 + 𝑞1
Para la obtención del espacio de trabajo de este robot se realizó el siguiente código en matlab:

L1=150;
L2=250;
L3=350;
L4=0;
tamano=10;
num_datos=15;

q1=linspace(-170,170,num_datos);
q2=linspace(-130,130,num_datos);
q3=linspace(-175,175,num_datos);
%limit=length(q1);
puntos=num_datos^3;
x=zeros(1,puntos);
y=zeros(1,puntos);
z=zeros(1,puntos);

esc_a=puntos/num_datos;
esc_b=esc_a/num_datos;
j=1;
k=1;

for i=1:num_datos
x(esc_a*(i-1)+1:esc_a*i)=L1-L4-q3(i);
j=1;
for j=1:num_datos
y(esc_a*(i-1)+esc_b*(j-1)+1:esc_a*(i-1)+esc_b*j)=L3+q2(j);
k=1;

for k=1:num_datos
z(esc_a*(i-1)+esc_b*(j-1)+k)=L2+q1(k);
end
end
end

scatter3(x,y,z,tamano,'fill','r')
xlabel('eje x')
ylabel('eje y')
zlabel('eje z')

El resultado obtenido es:

 3 PUNTO

Dibuje el espacio de trabajo del robot dado en la Figura 3.

Para hallar este espacio de trabajo, lo primero que se debe hacer es conseguir las expresiones del
punto final del robot en coordenadas absolutas y de forma simbólica, o en función de parámetros,
lo cual se hace con la tabla de DHM y usando transformaciones homogéneas.
 i 𝛼𝑖−1 𝑎𝑖−1 𝑑𝑖 𝜃𝑖
1 0 0 0 q1
2 0 L1 0 q2
3 0 L2 0 q3
4 0 L3 0 0

𝑐0 −𝑠0 0 𝐿3 1 0 0 𝐿3
𝑠0𝑐0 𝑐0𝑐0 −𝑠0 −𝑠0(0) 0 1 0 0
3𝑇4 = ( )=( )
𝑠0𝑠0 𝑐0𝑠0 𝑐0 𝑐0(0) 0 0 1 0
0 0 0 1 0 0 0 1

𝑐𝑞3 −𝑠𝑞3 0 𝐿2 𝑐𝜃3 −𝑠𝜃3 0 𝐿2

𝑠𝑞3 𝑐0 𝑐𝑞3 𝑐0 −𝑠0 −𝑠0(0) 𝑠𝜃 𝑐𝜃3 0 0
2𝑇3 = ( )=( 3 )
𝑠𝑞3 𝑠0 𝑐𝑞3 𝑠0 𝑐0 𝑐0(0) 0 0 1 0
0 0 0 1 0 0 0 1

𝑐𝑞2 −𝑠𝑞2 0 𝐿1 𝑐𝜃2 −𝑠𝜃2 0 𝐿1

𝑠𝑞2 𝑐0 𝑐𝑞2 𝑐0 −𝑠0 −𝑠0(0) 𝑠𝜃 𝑐𝜃2 0 0
1𝑇2 = ( )=( 2 )
𝑠𝑞2 𝑠0 𝑐𝑞2 𝑠0 𝑐0 𝑐0(0) 0 0 1 0
0 0 0 1 0 0 0 1
 𝑐𝑞1 −𝑠𝑞1 0 0 𝑐𝜃1 −𝑠𝜃1 0 0
𝑠𝑞 𝑐0 𝑐𝑞1 𝑐0 −𝑠0 −𝑠0(0) 𝑠𝜃 𝑐𝜃1 0 0
0𝑇1 = ( 1 )=( 1 )
𝑠𝑞1 𝑠0 𝑐𝑞1 𝑠0 𝑐0 𝑐0(0) 0 0 1 0
0 0 0 1 0 0 0 1

0𝑇4 = 0𝑇1 ∙ 1𝑇2 ∙ 2𝑇3 ∙ 3𝑇4

Solo importan las coordenadas absolutas para la posición final del robot por lo tanto se toman de
la matriz 0𝑇3 , estas son:

𝑋 = 𝑐θ1 𝐿1 + 𝐿3 (𝑐θ3 (𝑐θ1 𝑐θ2 − 𝑠θ1 𝑠θ2 ) − 𝑠θ3 (𝑐θ1 𝑠θ2 + 𝑐θ2 𝑠θ1 )) + 𝐿2 (𝑐θ1 𝑐θ2 − 𝑠θ1 𝑠θ2 )

𝑌 = 𝐿1 𝑠𝜃1 + 𝐿3 (𝑐𝜃3 (𝑐𝜃1 𝑠𝜃2 + 𝑐𝜃2 𝑠𝜃1 ) + 𝑠𝜃3 (𝑐𝜃1 𝑐𝜃2 − 𝑠𝜃1 𝑠𝜃2 )) + 𝐿2 (𝑐𝜃1 𝑠𝜃2 + 𝑐𝜃2 𝑠𝜃1 )

El código desarrollado para el barrido de todas las posibles posiciones del robot es el siguiente:

L1=0.5;
L2=0.3;
L3=0.2;
num_dat=15;
amaño=10;

q1=linspace(-60,60,num_dat);
q2=linspace(-120,120,num_dat);
q3=linspace(-90,90,num_dat);

puntos=num_dat^3;
x=zeros(1,puntos);
y=zeros(1,puntos);

esc_a=puntos/num_dat;
esc_b=esc_a/num_dat;
j=1;
k=1;

for i=1:num_dat
for j=1:num_dat
for k=1:num_dat
x(esc_a*(i-1)+esc_b*(j-
1)+k)=cosd(q1(k))*L1+L3*(cosd(q3(i))*(cosd(q1(k))*cosd(q2(j))-
sind(q1(k))*sind(q2(j)))-
sind(q3(i))*(cosd(q1(k))*sind(q2(j))+cosd(q2(j))*sind(q1(k))))+L2*(cosd(q
1(k))*cosd(q2(j))-sind(q1(k))*sind(q2(j)));
y(esc_a*(i-1)+esc_b*(j-
1)+k)=L1*sind(q1(k))+L3*(cosd(q3(i))*(cosd(q1(k))*sind(q2(j))+cosd(q2(j))
*sind(q1(k)))+sind(q3(i))*(cosd(q1(k))*cosd(q2(j))-
sind(q1(k))*sind(q2(j))))+L2*(cosd(q1(k))*sind(q2(j))+cosd(q2(j))*sind(q1
(k)));
end
end
end

scatter(x,y,tamano,’fill’,’r’)
xlabel(‘eje x’)
ylabel(‘eje y’)
grid on

La posición de home del robot corresponde a la gráfica mostrada enseguida.

4 PUNTO
 i 𝛼𝑖−1 𝑎𝑖−1 𝑑𝑖 𝜃𝑖
1 0 0 0 q1
2 -90 L2 0 q2
3 0 L3 0 90+q3
4 90 0 L4 q4-90
5 90 0 0 180+q5
6 90 0 L5+L6 q6

𝑐𝑞1 −𝑠𝑞1 0 0 𝑐𝜃1 −𝑠𝜃1 0 0

𝑠𝑞1 𝑐0 𝑐𝑞1 𝑐0 −𝑠0 −𝑠0(0) 𝑠𝜃 𝑐𝜃1 0 0
0𝑇1 = ( )=( 1 )
𝑠𝑞1 𝑠0 𝑐𝑞1 𝑠0 𝑐0 𝑐0(0) 0 0 1 0
0 0 0 1 0 0 0 1

𝑐𝜃2 −𝑠𝜃2 0 𝐿2 𝑐𝜃2 −𝑠𝜃2 0 𝐿2

𝑠𝜃 𝑐(−90) 𝑐𝜃2 𝑐(−90) −𝑠(−90) 0 0 0 1 0
1𝑇2 = ( 2 )=( )
𝑠𝜃2 𝑠(−90) 𝑐𝜃2 𝑠(−90) 𝑐(−90) 0 −𝑠𝜃2 −𝑐𝜃2 0 0
0 0 0 1 0 0 0 1

𝑐(𝜃3 + 90) −𝑠(𝜃3 + 90) 0 𝐿3 −𝑠𝜃3 −𝑐𝜃3 0 𝐿3

𝑠(𝜃3 + 90)𝑐0 𝑐(𝜃3 + 90)𝑐0 −𝑠0 0 𝑐𝜃 −𝑠𝜃3 0 0
2𝑇3 = ( )=( 3 )
𝑠(𝜃3 + 90)𝑠0 𝑐(𝜃3 + 90)𝑠0 𝑐0 0 0 0 1 0
0 0 0 1 0 0 0 1

𝑐(𝜃4 − 90) −𝑠(𝜃4 − 90) 0 0 𝑠𝜃4 𝑐𝜃4 0 0

𝑠(𝜃 − 90)𝑐90 𝑐(𝜃4 − 90)𝑐90 −𝑠90 −𝑠90(𝐿4 ) 0 0 −1 −𝐿4
3𝑇4 = ( 4 )=( )
𝑠(𝜃4 − 90)𝑠90 𝑐(𝜃4 − 90)𝑠90 𝑐90 𝑐90(𝐿4 ) −𝑐𝜃4 𝑠𝜃4 0 0
0 0 0 1 0 0 0 1

𝑐(𝜃5 + 180) −𝑠(𝜃5 + 180) 0 0 −𝑐𝜃5 𝑠𝜃5 0 0

𝑠(𝜃5 + 180)𝑐90 𝑐(𝜃5 + 180)𝑐90 −𝑠90 0 0 0 −1 0
4𝑇5 = ( )=( )
𝑠(𝜃5 + 180)𝑠90 𝑐(𝜃5 + 180)𝑠90 𝑐90 0 −𝑠𝜃5 −𝑐𝜃5 0 0
0 0 0 1 0 0 0 1
 𝑐𝜃6 −𝑠𝜃6 0 0 𝑐𝜃6 −𝑠𝜃6 0 0
𝑠𝜃 𝑐90 𝑐𝜃6 𝑐90 −𝑠90 −𝑠90(𝐿5 + 𝐿6 ) 0 0 −1 −(𝐿5 + 𝐿6 )
5𝑇6 = ( 6 )=( )
𝑠𝜃6 𝑠90 𝑐𝜃6 𝑠90 𝑐90 𝑐90(𝐿5 + 𝐿6 ) 𝑠𝜃6 𝑐𝜃6 0 0
0 0 0 1 0 0 0 1

0𝑇6 = 0𝑇1 ∙ 1𝑇2 ∙ 2𝑇3 ∙ 3𝑇4 ∙ 4𝑇5 ∙ 5𝑇6

En el Anexo A se encuentra de forma ordenada y clara el vector 0𝑇6 (: 4) para hallar los valores en
coordenadas generalizadas de X , Y y Z, adicionalmente se presenta la matriz total en el formato
de datos de salida de Matlab, las ecuaciones para cada coordenada son:

𝑋 = L2 cθ1 + L4 (cθ1 cθ2 cθ3 − cθ1 sθ2 sθ3 ) + (cθ5 (cθ1 cθ2 cθ3 − cθ1 sθ2 sθ3 ) − sθ5 (cθ4 sθ1
− sθ4 (cθ1 cθ2 sθ3 + cθ1 cθ3 sθ2 )))(L5 + L6 ) + L3 cθ1 cθ2

𝑌 = (cθ5 (cθ2 cθ3 sθ1 − sθ1 sθ2 sθ3 ) + sθ5 (cθ1 cθ4 + sθ4 (cθ2 sθ1 sθ3 + cθ3 sθ1 sθ2 )))(L5 + L6 )
+ L2 sθ1 + L4 (cθ2 cθ3 sθ1 − sθ1 sθ2 sθ3 ) + L3 cθ2 sθ1

𝑍 = L3 sθ2 − (cθ5 (cθ2 sθ3 + cθ3 sθ2 ) − sθ4 sθ5 (cθ2 cθ3 − sθ2 sθ3 ))(L5 + L6 ) − L4 (cθ2 sθ3
+ cθ3 sθ2 )

Si se quieren obtener las coordenadas del punto final respecto a la base fija o articulación 0,
simplemente se suma la cantidad 𝐿1 a la coordenada en Y.
En la figura anterior se observa la configuración del robot de 6GDL para la posición de home, la
cual se obtiene al hacer:
𝑞1_𝑎 = 0;
𝑞2_𝑎 = 0;
𝑞3_𝑎 = 0;
𝑞4_𝑎 = 0;
𝑞5_𝑎 = 0;
𝑞6_𝑎 = 0;

La matriz de transformación total obtenida para esta configuración específica es:

T=

0.0000 -0.0000 1.0000 50.0000

1.0000 -0.0000 -0.0000 -0.0000

0.0000 1.0000 0.0000 10.0000

0 0 0 1.0000

Lo cual arroja que la posición del extremo de este corresponde a las coordenadas:

𝑋 = 50, 𝑌 = 0, 𝑍 = 10
Se ha reconfigurado el robot con los siguientes valores para el vector de variables independientes:
𝑞1_𝑎 = 90;
𝑞2_𝑎 = 45;
𝑞3_𝑎 = 60;
𝑞4_𝑎 = 120;
𝑞5_𝑎 = −45;
𝑞6_𝑎 = 180;

El resultado obtenido tanto grafica como matricialmente se presenta enseguida.

T=

-0.3536 -0.8660 -0.3536 -7.0711

-0.4085 0.4830 -0.7745 -1.0075

0.8415 -0.1294 -0.5245 -17.2207

0 0 0 1.0000
PUNTO 5
 i 𝛼𝑖−1 𝑎𝑖−1 𝑑𝑖 𝜃𝑖
1 0 0 0 q1
2 -30 3 -5 170+q2
3 90 0 11+q3 -90
4 90 0 7 q4-60
5 0 5 0 q5

En la anterior tabla de DHM se han asumido varios valores teniendo en cuenta las siguientes
suposiciones o consideraciones:

 El eje 2 pasa por detrás del eje 1, lo que ocasiona que el Angulo entre 𝑋1 y 𝑋2 sea menor a
180, además de esto el Angulo entre 𝑍1 𝑦 𝑍2 será negativo.
 El Angulo entre 𝑍2 𝑦 𝑍3 es recto, igual que el angulo formado por 𝑍3 𝑦 𝑍4 .
 El Angulo entre 𝑍3 𝑦 𝑋3 es de aproximadamente 30.
 Se manejaron proporciones de distancias, siendo la más larga 𝑑3 = 11.

A continuación se expresan las transformaciones homogéneas utilizando los valores asumidos en

la tabla de DHM, ya que si se deja en términos simbólicos, el usuario podría no seguir las
restricciones del modelo inscrito en el dibujo y el robot final sería diferente al esperado.

𝑐𝑞1 −𝑠𝑞1 0 0 𝑐𝜃1 −𝑠𝜃1 0 0

𝑠𝑞1 𝑐0 𝑐𝑞1 𝑐0 −𝑠0 −𝑠0(0) 𝑠𝜃 𝑐𝜃1 0 0
0𝑇1 = ( )=( 1 )
𝑠𝑞1 𝑠0 𝑐𝑞1 𝑠0 𝑐0 𝑐0(0) 0 0 1 0
0 0 0 1 0 0 0 1

𝑐(170 + 𝜃2 ) −𝑠(170 + 𝜃2 ) 0 3
𝑠(170 + 𝜃2 )𝑐(−30) 𝑐(170 + 𝜃2 )𝑐(−30) −𝑠(−30) −𝑠(−30)(−5)
1𝑇2 = ( )
𝑠(170 + 𝜃2 )𝑠(−30) 𝑐(170 + 𝜃2 )𝑠(−30) 𝑐(−30) 𝑐(−30)(−5)
0 0 0 1
 𝑐(170 + 𝜃2 ) −𝑠(170 + 𝜃2 ) 0 3
−√3 −√3 1 5
𝑠(170 + 𝜃2 ) 𝑐(170 + 𝜃2 ) −
1𝑇2 = 2 2 2 2
1 1 √3 5√3
− 𝑠(170 + 𝜃2 ) − 𝑐(170 + 𝜃2 ) −
2 2 2 2
( 0 0 0 1 )

𝑐(−90) −𝑠(−90) 0 0 0 1 0 0
𝑠(−90)𝑐90 𝑐(−90)𝑐90 −𝑠90 −𝑠(90)(11 + 𝑞3 ) 0 0 −1 −(11 + 𝑞3 )
2𝑇3 = ( )=( )
𝑠(−90)𝑠90 𝑐(−90)𝑠90 𝑐90 𝑐90(11 + 𝑞3 ) −1 0 0 0
0 0 0 1 0 0 0 1

𝑐(𝜃4 − 60) −𝑠(𝜃4 − 60) 0 0

𝑠(𝜃 − 60)𝑐90 𝑐(𝜃4 − 60)𝑐90 −𝑠90 −𝑠90(7)
3𝑇4 = ( 4 )
𝑠(𝜃4 − 60)𝑠90 𝑐(𝜃4 − 60)𝑠90 𝑐90 𝑐90(7)
0 0 0 1

𝑐(𝜃4 − 60) −𝑠(𝜃4 − 60) 0 0

0 0 −1 −7
=( )
𝑠(𝜃4 − 60) 𝑐(𝜃4 − 60) 0 0
0 0 0 1

𝑐𝜃5 −𝑠𝜃5 0 5 𝑐𝜃5 −𝑠𝜃5 0 5

𝑠𝜃5 𝑐0 𝑐𝜃5 𝑐0 −𝑠0 0 𝑠𝜃 𝑐𝜃5 0 0
4𝑇5 = ( )=( 5 )
𝑠𝜃5 𝑠0 𝑐𝜃5 𝑠0 𝑐0 0 0 0 1 0
0 0 0 1 0 0 0 1

La trasformada del extremo del robot respecto a la base se obtiene así:

0𝑇5 = 0𝑇1 ∙ 1𝑇2 ∙ 2𝑇3 ∙ 3𝑇4 ∙ 4𝑇5

La matriz en términos del vector de variables independientes se presenta en el anexo B, así como
el vector de relevancia, el cual corresponde a las coordenadas generalizadas para el punto
extremo del robot.

A continuación de presentan las ecuaciones para determinar la posición en X, Y y Z del extremo del
robot.

1
𝑋 = 3c(q1 ) + (5s(q1 ))/2 − 7c(q1 )c(170 + q2 ) + (5c(q4 -60)s(q1 ))/2 + (c(q1 )s(170 + q2 ) − (32 c(170
1
+ q2 )s(q1 ))/2)(q3 + 11) + 5s(q4 -60)(c(q1 )s(170 + q2 ) − (32 ∗ c(170 + q2 )s(q1 ))/2)

𝑌 = 3s(q1 ) − (5c(q1 ))/2 − (5c(q1 )c(q4 -60))/2 − 7c(170 + q2 )s(q1 ) + (s(q1 )s(170 + q2 )
1 1
+ (32 c(q1 )c(170 + q2 ))/2)(q3 + 11) + 5s(q4 -60)(s(q1 )s(170 + q2 ) + (32
∗ c(q1 )c(170 + q2 ))/2)

1 1
𝑍 = (7 ∗ (170 + q2 ))/2 − (5 ∗ 32 c(q4 -60))/2 + (5c(170 + q2 )s(q4 -60))/2 − (5 ∗ 32 )/2 + (c(170 + q2 ) ∗ (q3
+ 11))/2

La siguiente grafica corresponde al robot en la posición de home.

Para una nueva configuración con los parámetros mostrados a continuación se consigue una
nueva posición final del extremo del robot expresada en la imagen de la parte inferior.

𝑞1_𝑎 = 45;
𝑞2_𝑎 = 65;
𝑞3_𝑎 = 10;
𝑞4_𝑎 = 100;
𝑞5_𝑎 = 30;
 ANEXO A

L2 cθ1 + L4 (cθ1 cθ2 cθ3 − cθ1 sθ2 sθ3 ) + (cθ5 (cθ1 cθ2 cθ3 − cθ1 sθ2 sθ3 ) − sθ5 (cθ4 sθ1 − sθ4 (cθ1 cθ2 sθ3 + cθ1 cθ3 sθ2 )))(L5 + L6 ) + L3 cθ1 cθ2
(cθ (cθ cθ sθ − sθ1 sθ2 sθ3 ) + sθ5 (cθ1 cθ4 + sθ4 (cθ2 sθ1 sθ3 + cθ3 sθ1 sθ2 )))(L5 + L6 ) + L2 sθ1 + L4 (cθ2 cθ3 sθ1 − sθ1 sθ2 sθ3 ) + L3 cθ2 sθ1
( 5 2 3 1 )
L3 sθ2 − (cθ5 (cθ2 sθ3 + cθ3 sθ2 ) − sθ4 sθ5 (cθ2 cθ3 − sθ2 sθ3 ))(L5 + L6 ) − L4 (cθ2 sθ3 + cθ3 sθ2 )
1

[ s\theta_6*(s\theta_1*s\theta_4 + c\theta_4*(c\theta_1*c\theta_2*s\theta_3 + c\theta_1*c\theta_3*s\theta_2)) -

c\theta_6*(s\theta_5*(c\theta_1*c\theta_2*c\theta_3 - c\theta_1*s\theta_2*s\theta_3) + c\theta_5*(c\theta_4*s\theta_1 -
s\theta_4*(c\theta_1*c\theta_2*s\theta_3 + c\theta_1*c\theta_3*s\theta_2))), s\theta_6*(s\theta_5*(c\theta_1*c\theta_2*c\theta_3 -
c\theta_1*s\theta_2*s\theta_3) + c\theta_5*(c\theta_4*s\theta_1 - s\theta_4*(c\theta_1*c\theta_2*s\theta_3 +
c\theta_1*c\theta_3*s\theta_2))) + c\theta_6*(s\theta_1*s\theta_4 + c\theta_4*(c\theta_1*c\theta_2*s\theta_3 +
c\theta_1*c\theta_3*s\theta_2)), c\theta_5*(c\theta_1*c\theta_2*c\theta_3 - c\theta_1*s\theta_2*s\theta_3) -
s\theta_5*(c\theta_4*s\theta_1 - s\theta_4*(c\theta_1*c\theta_2*s\theta_3 + c\theta_1*c\theta_3*s\theta_2)), L_2*c\theta_1 +
L_4*(c\theta_1*c\theta_2*c\theta_3 - c\theta_1*s\theta_2*s\theta_3) + (c\theta_5*(c\theta_1*c\theta_2*c\theta_3 -
c\theta_1*s\theta_2*s\theta_3) - s\theta_5*(c\theta_4*s\theta_1 - s\theta_4*(c\theta_1*c\theta_2*s\theta_3 +
c\theta_1*c\theta_3*s\theta_2)))*(L_5 + L_6) + L_3*c\theta_1*c\theta_2]

[ - c\theta_6*(s\theta_5*(c\theta_2*c\theta_3*s\theta_1 - s\theta_1*s\theta_2*s\theta_3) - c\theta_5*(c\theta_1*c\theta_4 +

s\theta_4*(c\theta_2*s\theta_1*s\theta_3 + c\theta_3*s\theta_1*s\theta_2))) - s\theta_6*(c\theta_1*s\theta_4 -
c\theta_4*(c\theta_2*s\theta_1*s\theta_3 + c\theta_3*s\theta_1*s\theta_2)), s\theta_6*(s\theta_5*(c\theta_2*c\theta_3*s\theta_1 -
s\theta_1*s\theta_2*s\theta_3) - c\theta_5*(c\theta_1*c\theta_4 + s\theta_4*(c\theta_2*s\theta_1*s\theta_3 +
c\theta_3*s\theta_1*s\theta_2))) - c\theta_6*(c\theta_1*s\theta_4 - c\theta_4*(c\theta_2*s\theta_1*s\theta_3 +
c\theta_3*s\theta_1*s\theta_2)), c\theta_5*(c\theta_2*c\theta_3*s\theta_1 - s\theta_1*s\theta_2*s\theta_3) +
s\theta_5*(c\theta_1*c\theta_4 + s\theta_4*(c\theta_2*s\theta_1*s\theta_3 + c\theta_3*s\theta_1*s\theta_2)),
(c\theta_5*(c\theta_2*c\theta_3*s\theta_1 - s\theta_1*s\theta_2*s\theta_3) + s\theta_5*(c\theta_1*c\theta_4 +
s\theta_4*(c\theta_2*s\theta_1*s\theta_3 + c\theta_3*s\theta_1*s\theta_2)))*(L_5 + L_6) + L_2*s\theta_1 +
L_4*(c\theta_2*c\theta_3*s\theta_1 - s\theta_1*s\theta_2*s\theta_3) + L_3*c\theta_2*s\theta_1]
[ c\theta_6*(s\theta_5*(c\theta_2*s\theta_3 + c\theta_3*s\theta_2) + c\theta_5*s\theta_4*(c\theta_2*c\theta_3 -
s\theta_2*s\theta_3)) + c\theta_4*s\theta_6*(c\theta_2*c\theta_3 - s\theta_2*s\theta_3),
c\theta_4*c\theta_6*(c\theta_2*c\theta_3 - s\theta_2*s\theta_3) - s\theta_6*(s\theta_5*(c\theta_2*s\theta_3 + c\theta_3*s\theta_2) +
c\theta_5*s\theta_4*(c\theta_2*c\theta_3 - s\theta_2*s\theta_3)), s\theta_4*s\theta_5*(c\theta_2*c\theta_3 -
s\theta_2*s\theta_3) - c\theta_5*(c\theta_2*s\theta_3 + c\theta_3*s\theta_2), - L_3*s\theta_2 -
(c\theta_5*(c\theta_2*s\theta_3 + c\theta_3*s\theta_2) - s\theta_4*s\theta_5*(c\theta_2*c\theta_3 - s\theta_2*s\theta_3))*(L_5 + L_6) -
L_4*(c\theta_2*s\theta_3 + c\theta_3*s\theta_2)]

[ 0,
0, 0, 1]

ANEXO B

1 1
3c(q1 ) + (5s(q1 ))/2 − 7c(q1 )c(170 + q2 ) + (5c(q4 -60)s(q1 ))/2 + (c(q1 )s(170 + q2 ) − (32 c(170 + q2 )s(q1 ))/2)(q3 + 11) + 5s(q4 -60)(c(q1 )s(170 + q2 ) − (32 ∗ c(170 + q2 )s(q1 ))/2)
1 1
3s(q1 ) − (5c(q1 ))/2 − (5c(q1 )c(q4 -60))/2 − 7c(170 + q2 )s(q1 ) + (s(q1 )s(170 + q2 ) + (32 c(q1 )c(170 + q2 ))/2)(q3 + 11) + 5s(q4 -60)(s(q1 )s(170 + q2 ) + (32 ∗ c(q1 )c(170 + q2 ))/2)
1 1
( (7 ∗ (170 + q2 ))/2 − (5 ∗ 32 c(q4 -60))/2 + (5c(170 + q2 )s(q4 -60))/2 − (5 ∗ 32 )/2 + (c(170 + q2 ) ∗ (q3 + 11))/2

Te =

[ cm5*((cm4*sm1)/2 + sm4*(cm1*sm2 - (3^(1/2)*cm2*sm1)/2)) - sm5*((sm1*sm4)/2 - cm4*(cm1*sm2 - (3^(1/2)*cm2*sm1)/2)), -

cm5*((sm1*sm4)/2 - cm4*(cm1*sm2 - (3^(1/2)*cm2*sm1)/2)) - sm5*((cm4*sm1)/2 + sm4*(cm1*sm2 - (3^(1/2)*cm2*sm1)/2)), - cm1*cm2 -
(3^(1/2)*sm1*sm2)/2, 3*cm1 + (5*sm1)/2 - 7*cm1*cm2 + (5*cm4*sm1)/2 + (cm1*sm2 - (3^(1/2)*cm2*sm1)/2)*(m3 + 11) + 5*sm4*(cm1*sm2 -
(3^(1/2)*cm2*sm1)/2) - (7*3^(1/2)*sm1*sm2)/2]

[ sm5*((cm1*sm4)/2 + cm4*(sm1*sm2 + (3^(1/2)*cm1*cm2)/2)) - cm5*((cm1*cm4)/2 - sm4*(sm1*sm2 + (3^(1/2)*cm1*cm2)/2)),

cm5*((cm1*sm4)/2 + cm4*(sm1*sm2 + (3^(1/2)*cm1*cm2)/2)) + sm5*((cm1*cm4)/2 - sm4*(sm1*sm2 + (3^(1/2)*cm1*cm2)/2)),
(3^(1/2)*cm1*sm2)/2 - cm2*sm1, 3*sm1 - (5*cm1)/2 - (5*cm1*cm4)/2 - 7*cm2*sm1 + (sm1*sm2 + (3^(1/2)*cm1*cm2)/2)*(m3 + 11) +
5*sm4*(sm1*sm2 + (3^(1/2)*cm1*cm2)/2) + (7*3^(1/2)*cm1*sm2)/2]

[ sm5*((cm2*cm4)/2 + (3^(1/2)*sm4)/2) - cm5*((3^(1/2)*cm4)/2 - (cm2*sm4)/2),

cm5*((cm2*cm4)/2 + (3^(1/2)*sm4)/2) + sm5*((3^(1/2)*cm4)/2 - (cm2*sm4)/2), sm2/2,
(7*sm2)/2 - (5*3^(1/2)*cm4)/2 + (5*cm2*sm4)/2 - (5*3^(1/2))/2 + (cm2*(m3 + 11))/2]

[ 0, 0,
0, 1]