Benemérita Universidad Autónoma de Puebla

Facultad de Ciencias de la Electrónica

Ingeniería en Mecatrónica

Asignatura: Robótica I
Profesor: Dr. Fernando Reyes Cortés

Reporte III

Martes y jueves 11a.m. – 1p.m.

Juárez Campos Ernesto
201745409

Fecha de entrega: 2 de diciembre de 2021

Otoño 2021
RESUMEN

El presente reporte tiene como finalidad el análisis de la estabilidad según Lyapunov, la

obtención de las posiciones, los errores de posición, las velocidades y los pares aplicados de
un esquema de control propuesto aplicado a un robot prototipo de 2 grados de libertad. Se
empleará el segundo método de Lyapunov para determinar la estabilidad del punto de
equilibrio de dicho esquema. También, con ayuda del software MATLAB, se emplearán
métodos numéricos, como el método de Runge—Kutta para la resolución de la ecuación
diferencial que representa al sistema, además del uso de funciones definidas por el usuario.
Con ayuda de la norma L [ ~ 2 q ] se determinará el desempeño del esquema de control
propuesto y se comparará con el desempeño de un esquema de control PD. De igual modo,
se presentarán sus gráficas y se analizarán los comportamientos de ambos algoritmos de
control aplicados al robot de 2gdl.
PROPÓSITOS

Propósito general
 Presentar el estudio dinámico y de control de un robot manipulador de 2gdl y de las
herramientas matemáticas que permiten su modelado y análisis.

Propósitos particulares
 Aplicar los conocimientos adquiridos acerca de la teoría de estabilidad de Lyapunov
 Profundizar en los temas de control de posición y el moldeo de energía
 Emplear las técnicas adquiridas para la resolución de problemas en el entorno de
MATLAB
INTRODUCCIÓN

La robótica un área de investigación en constante desarrollo, por lo que los robots

adquieren aplicaciones cada vez más avanzadas, generando la necesidad de que sus
movimientos sean cada vez más precisos y rápidos. [A. Rivera, 2019]]

En los inicios, los esfuerzos de los científicos se consagraron a la solución del problema de
controlar el movimiento y la posición de robots en ambientes estructurados, con soluciones
de control en el espacio de las articulaciones. Actualmente, los avances tanto científicos
como tecnológicos han generado que estos esquemas de control sean más eficientes y
precisos.

Algunas especificaciones de control que se deben de considerar son: Estabilidad,

Regulación, Seguimiento de trayectoria y Optimización. Para llevar a cabo la primera
propiedad en la cual nuestro sistema debe de tener estabilidad, se empelará la teoría de
estabilidad de Lyapunov, para analizar los controladores que se nos presentan. [Kelly y
Santibañez, 2003]

El problema de regulación o control de posición de robots manipuladores consiste en

colocar en el extremo final del robot en una posición deseada q d (constante en el tiempo)
para cualquier condición inicial [ q ( 0 ) q̇ (0) ] ∈ R2 n . Desde el punto de vista matemático, el
problema de control de posición se describe como diseñar un regulador τ ∈ R ntal que la
velocidad de movimiento q (t ) ∈ Rn y el error de posición ~
q ∈ R : ~q ( t )=q −q ( t ) convergen
n
d

asintóticamente a cero ∀ t ≥ 0. Siendo q d ∈ Rn ; sin importar las condiciones iniciales ~

q ( 0) y
q̇ (0). [F. Reyes, 2011]

La propuesta para el moldeo de energía es la siguiente

τ =∇ U a ( K p , ~
q )−f v ( K v , q̇ ) + g ( q )

Donde

 U a ( K p ,~
q ) > 0es la ganancia potencial artificial.
n× n
 Kp∈R es la ganancia proporcional, matriz diagonal definida positiva.
 n×n
 Kv ∈ R es la ganancia derivativa, matriz definida positiva.

 f v ( K v , q̇ ) es una función disipativa: q̇ T f v ( K v , q̇ ) >0.

PLANTEAMIENTO Y DESCRIPCIÓN DEL PROBLEMA

Sea el siguiente algoritmo de control:

τ =K p [ I −α e−α q ] ~
~ 2

q−K v atan ( q̇ ) +g ( q )

Donde K p , K v ∈ R 2× 2 son matrices diagonales y definidas positivas; I ∈ R 2 ×2 es la matriz

identidad; α ∈ R+¿¿ es una constante positiva, tal que: 0< α <1; ~ 2
q ∈ R es el error de posición
~ ~
q=qd −q ( t ); siendo q d ∈ R2 el vector de referencias deseadas; mientras que e−α q ∈ R2 ×2 es
2

una matriz de la forma:

[ ] [ ]
~2
−α ~
q e−α q 1
0 atan ( q̇1 )
e = −α ~
q2
2 ; atan ( q̇ )=
0 e atan ( q̇2 )

 Llevar a cabo la demostración de estabilidad global del punto de equilibrio de la

ecuación en lazo cerrado del modelo dinámico del robot manipulador y el esquema de
control.
 Realice en MATLAB la simulación comparativa del algoritmo de control propuesto con
el control proporcional—derivativo (PD): τ =K p ~ q−K v q̇+ g ( q ) usando el modelo
dinámico numérico del robot prototipo de 2gdl, con periodo de muestreo h=2.5 ms, tal
que el extremo final del brazo robot se pueda posicionar en el siguiente set—point:
T
q d =[ 45 ° , 90 ° ] [ grados ] .
 Comparativa usando la norma L2 [ ~q]

√∫‖
20
L2 [ ~ ~ 2
q ]= q ( σ )‖ dσ
0

 Presente las gráficas de los errores de posición y pares aplicados. Documentar

correctamente el código fuente del esquema de control.
 Sintonice las ganancias de los algoritmos de control de tal forma que los torques de
max max
control no saturen a los servomotores τ 1 =150 Nm y τ 2 =15 Nm.
SOLUCIÓN DEL PROBLEMA

a) Demostración de estabilidad global del punto de equilibrio

Consideramos el esquema de control con el modelo dinámico de un robot de n grados de
libertad

K p [ I−α e ] ~q−K v atan ( q̇ ) + g ( q ) =M ( q ) q̈ +C ( q , q̇ ) q̇+ B q̇+ g ( q )

−α ~
2
q
(1)

Eliminamos la compensación de gravedad

K p [ I −α e−α q ] ~
~ 2

q−K v atan ( q̇ )=M ( q ) q̈+ C ( q , q̇ ) q̇ +B q̇ (2)

Despejamos q̈

M ( q ) q̈=K p [ I−α e−α q ] ~q−K v atan ( q̇ )−B q̇−C ( q , q̇ ) q̇

~ 2

( 3)

[
q̈=M ( q )−1 K p [ I −α e−α q ] ~
~
]
2

q−K v atan ( q̇ )−B q̇−C ( q , q̇ ) q̇ (4 )

Tenemos que ~ q=qd −q . Al derivar obtenemos ~ ˙

q=− q̇. Así, tenemos que el sistema
representado en variables de estado es igual a.

d q~
[]
dt q̇
=
[ −1
[
− q̇
M ( q ) K p [ I −α e ] q−K v atan ( q̇ ) −B q̇−C ( q , q̇ ) q̇
−α ~q ~
2

] ] ( 5)

Determinamos el punto de equilibrio del sistema

2× 2
i) −q̇=−I q̇=0 ⟺ q̇=0 ; I >0 ∈ R ; q=cte.

[ K [ I −α e ] ~q−K −α ~
]
2
−1 q
ii) M ( q) p v atan ( q̇ )−B q̇−C ( q , q̇ ) q̇ =0
−1
M ( q ) >0 ⟺ M ( q ) >0

K p [ I−α e−α q ] ~
~2
q−K v atan ( q̇ )−B q̇−C ( q , q̇ ) q̇=0

Considerando que K p , K p y B son matrices diagonales definidas positivas y α es una

constante positiva comprendida entre 0< α <1. Así
C ( q , q̇ ) q̇=0 ⟺ q̇=0
B q̇=0 ⟺ q̇=0
K v atan ( q̇ )=0 ⟺ atan ( q̇ ) =0 ⟺ q̇=0

K p [ I −α e ] ~q=0 ⟺ ~q=0 ⟹ q=qd

−α ~
2
q
Por lo tanto, existe el punto de equilibrio y es único ( q̇ , ~
q )=( 0,0 )
Proponemos una función candidata de Lyapunov

V ( q̇ , ~
1 T 1 T
q )= q̇ M ( q ) q̇+ ~
[
1 −α ~q 1
q Kp I+ 2 e − 2 ~
]
2

q (6 )
2 2 ~
q ~
q
Donde

[ ]
1 −α ~q 1
2

1+ e − 2
1
0
[ ]
1 ~ 2
1 ~
q 21 ~
q1
I + 2 e−α q − 2 = ( 7)
~
q ~
q 1 −α ~q 1 2

0 1+ ~2 e − ~2 2

q2 q2
Derivamos la función candidata de Lyapunov, para esto derivamos término por término
para evitar confusiones y errores.
1 T
 Para q̇ M ( q ) q̇
2

[
d 1 T
dt 2
T
] 1 T
q̇ M ( q ) q̇ = q̇ M ( q ) q̈+ q̇ Ṁ ( q ) q̇
2
( 7)

Sustituimos el valor de q̈

d 1 T
dt 2 [ T
] −1
q̇ M ( q ) q̇ = q̇ M ( q ) M ( q ) ¿
(8)
1 T
−B q̇−C ( q , q̇ ) q̇ ¿+ q̇ Ṁ ( q ) q̇
2
d 1 T
dt 2 [ T
]
q̇ M ( q ) q̇ = q̇ K p [ I −α e ] ~
−α ~
q T 2

q−q̇ K v atan ( q̇ )
(9)
T T 1 T
−q̇ B q̇− q̇ C ( q , q̇ ) q̇+ q̇ Ṁ ( q ) q̇
2
d 1 T
dt 2 [ ]
q̇ M ( q ) q̇ = q̇T K p [ I −α e−α q ] ~
~ 2

q−q̇ T K v atan ( q̇ )
( 10 )
1 T
−q̇ B q̇ + q̇ [ Ṁ ( q ) −2C ( q , q̇ ) ] q̇
T
2

Como Ṁ ( q )−2C ( q , q̇ )=0, entonces

[
d 1 T
dt 2
T
]
q̇ M ( q ) q̇ = q̇ K p [ I −α e ] q−q̇ T K v atan ( q̇ )−q̇ T B q̇
q ~
−α ~ 2

( 11 )
 1 ~T
[
1 −α ~q 1
q Kp I+ 2 e − 2 ~
]
2

 Para q
2 ~
q ~
q

d 1 ~T
dt 2 [ 1 −α ~q 1
q K p I + ~2 e − ~2 ~
q q [1 T 1 −α ~q 1
q = ~q˙ K p I + ~2 e − ~2 ~
2 q q
q
2

]] [
2

] ( 12 )
+~
1 T
q+ ~
d 1 −α ~q 1
I+ 2 e − 2 ~
[
1 T
q+ ~
1 −α ~q 1 ˙
q K p I+ 2 e − 2 ~
] [ ]
2 2

q Kp q
2 dt ~
q ~
q 2 ~
q ~
q

d 1 ~T
dt 2 [ 1 −α ~q 1
q Kp I+ 2 e − 2 ~
~
q ~q 2[
1 T 1 −α ~q 1
q = ~q˙ K p I + 2 e − 2 ~
~
q ~
q
q
2

]] [
2

] ( 13 )
+~
[
−1 −α ~q α −α ~q 1 ~˙ ~ 1 ~T 1 −α ~q 1
+ 3 q q+ q K p I + 2 e − 2 ~q˙
] [ ]
2 2 2
T
q Kp 3 e −~ e
~
q q ~
q 2 ~
q ~q

Sustituimos el valor de ~
q˙

dt 2 [
d 1 ~T 1
~q [ 1
q K p I + 2 e−α q − 2 ~
~
~
q
q=
−1 T
2
1
~
q
2~ 1
q̇ K p I+ 2 e−α q − 2 ~
~
q
q
]] [
2

] ( 14 )
−~
−1
[ α 1
q T K p 3 e−α q − ~ e−α q + 3 q̇ ~
~ ~ 1 1 1
q− ~qT K p I + 2 e−α q − 2 q̇
~

] [ ]
2 2 2

~q q ~
q 2 ~
q ~
q

[ 1 −α ~q 1
]
2

I+ e − 2 es una matriz diagonal definida positiva. Ahora, empleando la propiedad

~
q
2 ~q
T T
x y= y x , obtenemos lo siguiente

[ ] [ ]
T
1 ~T 1 1 1 1 1
q K p I + 2 e−α q − 2 q̇= K p ~
~ 2~ 2

q T I + 2 e−α q − 2 q̇
2 ~
q ~
q 2 ~
q ~
q
( 15 )
[[ ]] [ ]
T
1 1 −α ~q 1 1 T 1 −α ~q 1
¿ K p I+ 2 e − 2 ~q q̇= q̇ K p I + 2 e − 2 ~
2 2

q
2 ~
q ~
q 2 ~
q ~
q

Sustituimos la expresión (15) en (14).

dt 2 [
d 1 ~T 1
~q
1
~
q [
q K p I + 2 e−α q − 2 ~
~
q=
−1 T
2
1
~
q
~ 1
q̇ K p I+ 2 e−α q − 2 ~
2

~
q
q
]] [
2

] ( 16 )
−~
[
−1 −α ~q α −α ~q 1
q K p 3 e − ~ e + 3 q̇ ~
1 T 1 −α ~q 1
q− q̇ K p I + 2 e − 2 ~
] [ ]
2 2 2
T
q
~
q q ~
q 2 ~
q ~
q

d 1 ~T
dt 2 [ 1 −α ~q 1
q K p I + ~2 e − ~2 ~
q q [ T 1 −α ~q 1 ~
q =−q̇ K p I + ~2 e
q
− ~2 q
q
2

]] [
2

]
( 17 )
 −~
[ −1 −α ~q α −α ~q 1 ~
]
2 2
T
q Kp e − ~ e + 3 q̇ q
~
q
3
q ~
q

[ −1 −α ~q α −α ~q 1
]
Como ~3 e − ~ e + ~3 es una matriz diagonal ∈ R 2× 2 y ~
2 2

q es un vector columna ∈ R 2
q q q
, entones

[ −1 −α ~q α −α ~q 1 ~ −1 −α ~q −α ~ 1
] [ ]
2 2 2 2
q
e − ~ e + 3 q= 2 e −α e + 2 ( 18 )
~
q
3
q ~
q ~
q ~
q

Tenemos que (18) también es una matriz diagonal. Al sustituir (18) en (17), obtenemos

[
d 1 ~T
dt 2
1
~q [ ~ 1
q K p I + 2 e−α q − 2 ~
~q
1
~
q
2 ~ 1
q =−q̇T K p I + 2 e−α q − 2 ~
~
q
q
]] [
2

] ( 19 )
−~
−1 ~ ~ 1
[ ]
2 2

q T K p 2 e−α q −α e−α q + 2 q̇
~q ~
q

Aplicamos de nuevo la propiedad x T y= y T x

[ ] [[ ]]
T
~ −1 1 −1 −α ~q 1
e −α e−α q + 2 ~
~ ~2 2 ~ 2 2

q T K p 2 e−α q −α e−α q + 2 q̇=K p q q̇

~
q ~
q ~
q 2 ~
q
( 20 )
−1 −α ~q
[ 1
¿ q̇ K p 2 e −α e + 2 ~
−α ~
]
2 2
T q
q
~
q ~
q

Sustituimos (20) en (19)

[
d 1 ~T
dt 2 ~q [
1 −α ~q 1
q Kp I+ 2 e − 2 ~
~
q
T 1 −α ~q 1 ~
q =−q̇ K p I + 2 e
~
q
2

− 2 q
~
q ]] [
2

] ( 21 )
−1 −α ~q
−q̇ K p 2 e −α e + 2 ~
−α ~ 1
[ ]
2 2
T q
q
~
q ~
q
d 1 ~T
dt 2 [
1
q K p I + 2 e−α q − 2 ~
~
q
~ 1
~
q
q
[
2

]] ( 22 )
1
[ ~ 1 1 ~ ~ 1
¿−q̇T K p I + ~2 e−α q − ~2 − ~2 e−α q −α e−α q + ~2 ~
]
2 2 2

q
q q q q
d 1 ~T
dt 2 [ 1
~
q [
1
~
q
q =−q̇T K p [ I −α e−α q ] ~
q K p I + 2 e−α q − 2 ~
~ ~ 2

q
]]
2

( 23 )

Así, la derivada de la función candidata de Lyapunov queda de la siguiente manera

 q )=q̇ T K p [ I−α e−α q ] ~q− q̇T K v atan ( q̇ )− q̇T B q̇
V̇ ( q̇ , ~
~ 2

( 24 )
−q̇ K [ I −α e ] ~
−α ~
2
T q
p q

V̇ ( q̇ , ~
T T
q )=−q̇ K v atan ( q̇ )−q̇ B q̇ ≤ 0 ( 25 )

Por lo tanto, comprobamos la estabilidad del punto de equilibrio del sistema.

b) Simulación comparativa
 Simulación con Algoritmo de control PD
Para comparar la respuesta del esquema de control PD con el propuesto en este reporte,
debemos obtener las posiciones, velocidades y errores de posición del modelo dinámico del
robot de 2gdl con cada uno de estos algoritmos de control. Para esto, emplearemos el
programa del anexo 1, el cual será el programa principal; en el anexo 2 se presenta el
algoritmo de control PD; en el anexo 3 se encuentra el programa del modelo dinámico con
el esquema de control.
Siguiendo la regla de sintonía para las ganancias proporcional y derivativa, tenemos que
τi
K pi ≤ 0.8 max
K vi=0.5 K pi para i=1 , 2
qdi

Tenemos que los límites físicos de los servomotores del robot de 2gdl son τ ma x =150 Nm y 1

τ ma x =15 Nm. Así, al sustituir los valores tenemos que

2

150 15
K p 1 ≤0.8 ⟹ K p 1 ≤ 2.666 K p 2 ≤0.8 ⟹ K p 2 ≤0.133
45 90

Teniendo en cuenta estos valores, se proponen las siguientes ganancias proporcionales y, en

consecuencia, las ganancias derivativas

K p= [ 1.30 0
0.09 ] [
K v=
0.5 K p 1
0
0
0.5 K p2
=
0.65
0
0
0.045][ ]
Simulando estos tres programas, obtenemos las siguientes respuestas
 Figura 1. Respuesta del robot de 2gdl con el algoritmo de control PD.

Podemos apreciar en la figura 1 que ambas articulaciones llegan a la posición deseada,

teniendo un error prácticamente igual a cero. De igual modo, vemos que las velocidades se
hacen cero cuando las articulaciones del robot se colocan en dicha posición deseada.
Observamos una respuesta críticamente amortiguada, por lo que no tenemos sobretiros, lo
cual se traduce en un movimiento suave.

Para obtener las gráficas de los pares aplicados, empleamos el código adjunto en el anexo 4
en conjunto con el del anexo 2 y 3. Así
 Figura 2. Pares aplicados con el esquema de control PD.

 Simulación con algoritmo de control propuesto

Ahora, para comparar el esquema de control PD con el propuesto, reemplazaremos el
código del anexo 2 por el que se presenta a continuación.

control_nuevo.m MATLAB 2019a

function [qt2, tau] = control_nuevo(t,x,xp)
% Posiciones articulares.
q = [x(1);
x(2)];
% Velocidades articulares.
qp = [xp(1);
 xp(2)];
% Ganancia proporcional.
kp1=1.3; kp2=0.09;
Kp=[kp1, 0;
0, kp2];
% Ganancia derivativa.
kv1=0.5*kp1; kv2=0.5*kp2;
Kv=[kv1, 0;
0, kv2];
% Referencias.
qd1=45; qd2=90;
% vector de referencias.
qd=[qd1;
qd2];
% Vector de pares de gravitacionales.
par_grav = [38.46*sin(q(1))+1.82*sin(q(1)+q(2));
1.82*sin(q(1)+q(2))];
% Error de posición en radianes.
qt2=[pi*qd(1)/180-q(1);
pi*qd(2)/180-q(2)];
% Error de posición en grados.
qtgrados=(180/pi)*[qt2(1);
qt2(2)];
% Velocidad en grados/segundo.
qpgrados=180*qp/pi;
% Constante alfa y matriz identidad
alfa=0.01;
I=[1, 0;
0, 1];
% Matriz exp(-alfa*qtgrados)
E=[exp(-alfa*(qtgrados(1))^2), 0;
0, exp(-alfa*(qtgrados(2))^2)];
% Matriz atan(qp)
arco_tangente=[atan(qpgrados(1));
atan(qpgrados(2))];
tau=(Kp*(I-alfa*E)*qtgrados)-(Kv*arco_tangente)+par_grav;
end
Al emplear este algoritmo de control, obtenemos los siguientes resultados.
 Figura 3. Respuesta del modelo dinámico con el esquema de control propuesto.

Cabe recalcar que se propuso a la constante α =0.01 . Las ganancias proporcional y

derivativa son las mismas que se emplearon con el algoritmo de control PD.

En la figura 3 podemos observar que, de igual modo, ambas articulaciones llegan a la

posición deseada, el error es mínimo y las velocidades tienden a cero. Sin embargo,
notamos que la respuesta es subamortiguada, es decir, tenemos sobretiros, lo cual nos
indica que el movimiento tiene “brincoteos”, además que, al inicio, la velocidad fluctúa
bastante.

Ahora, empleamos el código del anexo 5 en conjunto con el del anexo 3 y el código del
algoritmo de control propuesto para obtener la gráfica de los pares aplicados.
Figura 4. Pares aplicados con el esquema de control propuesto.
CONCLUSIONES

En este reporte se ha desarrollado un procedimiento para el estudio de la estabilidad, así

como la obtención de las respuestas de posición, velocidad, error de posición y pares
aplicados de los distintos elemento de un modelo dinámico de un robot prototipo de 2gdl en
conjunto con dos esquemas de control diferentes. También se comprobó su eficiencia con la
ayuda de la norma L [ ~
2q ] , así como con la del método de integración de Euler. Una vez más
pudimos comprobar que el software MATLAB es una herramienta muy poderosa, capaz de
realizar cálculos muy complejos en un periodo de tiempo muy pequeño, facilitándonos el
análisis de estos. En cuanto a los esquemas de control, pudimos observar que existen una
gran variedad de estos, los cuales tienen respuestas diferentes, así como distintas
eficiencias. No obstante, es necesario comprobar su estabilidad para poder implementar
tales controladores en los robots manipuladores, no solo prototipos, sino también aquellos
que se usan en la industria, medicina, entretenimiento, etc.

BIBLIOGRAFÍA

[1] F. Reyes. “Robótica, control de robots manipuladores”. Ed. Alfaomega. 2011.

[2] R. Relly y V. Santibañez. "Control de movimiento de robots manipuladores". Ed.
Prentice-Hall. 2003.
[3] A. Rivera. “Control de posición de un robot manipulador de tres grados de libertad”,
(Tesis de maestría). BUAP. Facultad de Ciencias de la Electrónica. Puebla, Pue. Diciembre
2019.
ANEXOS

Anexo 1

robot2gdl_ode.m MATLAB 2019a

clc;
clear all;
close all;
format short g
ti = 0; h = 0.0025; tf = 20;
% Vector tiempo
t=ti:h:tf;
% Condiciones iniciales
ci=[0;
0;
0;
0];
%Solución numérica del sistema dinámico del robot de 2gdl
opciones=odeset('RelTol',1e-6,'AbsTol',1e-
6,'InitialStep',h,'MaxStep',h);
[t,x]=ode45('robot2gdl',t,ci,opciones);
q1=x(:,1);
q2=x(:,2);
qp1=x(:,3);
qp2=x(:,4);

subplot(2,2,1); plot(t, 180*q1/pi, t,180*q2/pi);

title('Posición q');
xlabel('Tiempo [s]'); ylabel('Posición [°]'); grid on;
legend('q_{1}','q_{2}')
subplot(2,2,3); plot(t, 45-180*q1/pi, t, 90-180*q2/pi)
title('Error de posición q_{t}');
xlabel('Tiempo [s]'); ylabel('Error [°]'); grid on;
legend('q_{t1}','q_{t2}')
subplot(1,2,2); plot(t, 180*qp1/pi, t,180*qp2/pi);
title('Velocidad q_{p}'); xlabel('Tiempo [s]'); ylabel('Velocidad
[°/s]'); grid on;
legend('q_{p1}','q_{p2}')
Anexo 2

control_pd.m MATLAB 2019a

function [qt1, tau] = control_pd(t,x,xp)
% Posiciones articulares.
q = [x(1);
x(2)];
% Velocidades articulares.
qp = [xp(1);
xp(2)];
% Ganancia proporcional.
kp1=1.3; kp2=0.09;
Kp=[kp1, 0;
0, kp2];
% Ganancia derivativa.
kv1=0.5*kp1; kv2=0.5*kp2;
Kv=[kv1, 0;
0, kv2];
% Referencias.
qd1=45; qd2=90;
% vector de referencias.
qd=[qd1;
qd2];
% Vector de pares de gravitacionales.
par_grav = [38.46*sin(q(1))+1.82*sin(q(1)+q(2));
1.82*sin(q(1)+q(2))];
% Error de posición en radianes.
qt1=[pi*qd(1)/180-q(1);
pi*qd(2)/180-q(2)];
% Error de posición en grados.
qtgrados=(180/pi)*[qt1(1);
qt1(2)];
% Velocidad en grados/segundo.
qpgrados=180*qp/pi;
% Esquema de control
tau=Kp*qtgrados-Kv*qpgrados+par_grav;
end
Anexo 3

robot2gdl.m MATLAB 2019a

function xp=robot2gdl(t,x)
% Vector de posición articular
q1=x(1);
q2=x(2);
q = [q1;
q2];
% Vector de velocidad articular
qp1=x(3);
qp2=x(4);
qp = [qp1;
qp2];
% Matriz de inercia
M=[2.351+0.1676*cos(q2), 0.102+0.0838*cos(q2);
0.102+0.0838*cos(q2), 0.102];
% Matriz de fuerzas centrípetas y de coriolis
C=[-0.1676*sin(q2)*qp2, -0.0838*sin(q2)*qp2;
0.084*sin(q2)*qp1, 0.0];
% Par gravitacional
gq=9.81*[3.9211*sin(q1)+0.1862*sin(q1+q2);
0.1862*sin(q1+q2)];
% Fricción viscosa
fr=[2.288*qp1;
0.175*qp2];
% Controlador
[qt1,tau]=control_pd(t,q,qp);
qpp = M^(-1)*(tau-C*qp-gq-fr);
% Vector de salida
xp = [qp1; qp2; qpp(1); qpp(2)];
end
Anexo 4

robot2gdl_pd_torques.m MATLAB 2019a

clc;
clear all;
close all;
format short g
ti = 0; h = 0.0025; tf = 20;
% Vector tiempo
t=ti:h:tf;
% Condiciones iniciales
ci=[0;
0;
0;
0];
%Solución numérica del sistema dinámico del robot de 2gdl
opciones=odeset('RelTol',1e-6,'AbsTol',1e-
6,'InitialStep',h,'MaxStep',h);
[t,x]=ode45('robot2gdl',t,ci,opciones);
q1=x(:,1);
q2=x(:,2);
qp1=x(:,3);
qp2=x(:,4);
% Posiciones y velocidades en grados
q1grad=(180/pi)*q1;
q2grad=(180/pi)*q2;
qp1grad=(180/pi)*qp1;
qp2grad=(180/pi)*qp2;
% Ganancia proporcional.
kp1=1.3; kp2=0.09;
% Ganancia derivativa.
kv1=0.5*kp1; kv2=0.5*kp2;
% Vector de pares de gravitacionales.
par_grav1 = 38.46*sin(q1)+1.82*sin(q1+q2);
par_grav2 = 1.82*sin(q1+q2);
% Pares aplicados algoritmo de control PD
taupd1=kp1*(45-q1grad)-kv1*qp1grad+par_grav1;
taupd2=kp2*(90-q2grad)-kv2*qp2grad+par_grav2;
plot(t, taupd1, t, taupd2);
title('Pares aplicados con algoritmo de control PD');
xlabel('Tiempo [s]'); ylabel('Torque [Nm]'); grid on;
legend('Torque hombro','Torque codo')
Anexo 5

robot2gdl_propuesto_torques.m MATLAB 2019a

clc;
clear all;
close all;
format short g
ti = 0; h = 0.0025; tf = 20;
% Vector tiempo
t=ti:h:tf;
% Condiciones iniciales
ci=[0;0;0;0];
%Solución numérica del sistema dinámico del robot de 2gdl
opciones=odeset('RelTol',1e-6,'AbsTol',1e-
6,'InitialStep',h,'MaxStep',h);
[t,x]=ode45('robot2gdl',t,ci,opciones);
q1=x(:,1);
q2=x(:,2);
qp1=x(:,3);
qp2=x(:,4);
% Posiciones y velocidades en grados
q1grad=(180/pi)*q1;
q2grad=(180/pi)*q2;
qp1grad=(180/pi)*qp1;
qp2grad=(180/pi)*qp2;
% Ganancia proporcional.
kp1=1.3; kp2=0.09;
% Ganancia derivativa.
kv1=0.5*kp1; kv2=0.5*kp2;
% Vector de pares de gravitacionales.
par_grav1 = 38.46*sin(q1)+1.82*sin(q1+q2);
par_grav2 = 1.82*sin(q1+q2);
alfa=0.01;
% Pares aplicados algoritmo de control propuesto
taupr1=kp1*(1-alfa.*exp(-alfa.*(45-q1grad).^2)).*(45-q1grad)-
kv1*atan(qp1)+par_grav1;
taupr2=kp2*(1-alfa.*exp(-alfa.*(90-q2grad).^2)).*(90-q2grad)-
kv1*atan(qp2)+par_grav2;
plot(t, taupr1, t, taupr2);
title('Pares aplicados con algoritmo de control propuesto');
xlabel('Tiempo [s]'); ylabel('Torque [Nm]'); grid on;
legend('Torque hombro','Torque codo')