Centro de Investigación y de Estudios Avanzados del Instituto

Politécnico Nacional Unidad Saltillo

Maestría en Ciencias en Robótica y Manufactura Avanzada

Mayo de 2018

Control en modo par de un robot planar de 3 grados de libertad

García López Melchor Caleb
Muñoz Pepi Iram Alberto
Aguilar Beltrán José Iván

Resumen. El desarrollo de controles para motores es muy importante en el área de la robótica, ya que
permite el diseño de mecanismos servo actuados para realizar distintas tareas. El control en modo par
para un robot brinda la posibilidad de generar trayectorias y movimientos más suaves, ya que es posible
controlar la velocidad y la fuerza del movimiento, además de optimizar la energía consumida por los
motores. En este documento se presenta el control en modo par de un robot manipulador serial de 3
grados de libertad (simulación), utilizando un control PD en los motores para controlar la posición
mediante la corriente que es suministrada.

1 Introducción El movimiento del robot depende directamente de la

Un robot manipulador serial consiste en una serie
de barras rígidas unidas entre sí a través de
articulaciones de un grado de libertad de tipo
rotacional o prismático, en los cuales se instalan
actuadores para generar el movimiento. De acuerdo
con el Instituto Americano del Robot (RIA), un robot
manipulador se define como: “Un manipulador
reprogramable, multifuncional ideado para el
transporte de materiales, partes, herramientas o
sistemas especializados, con movimientos variados y
programados para la realización de tareas diversas”.
Las arquitecturas de los robots manipuladores seriales
clásicos están caracterizadas por una gran rigidez
estructural, repetibilidad y por un elevado peso propio.
Existen distintas maneras de representar a un robot,
entre ellas la más común es por medio de un esquema
planar (dibujado sobre un plano en dos dimensiones),
en el cual se describen el tipo de movimiento de cada
una de sus uniones, sea rotacional o de traslación, así
como las longitudes de los eslabones que componen al
robot, la posición de los centros de masa de cada uno
de los cuerpos rígidos, las masas de cada eslabón, etc.
transmisión de movimiento por parte de los actuadores
hacia las articulaciones entre los cuerpos rígidos que
componen al robot (en el caso de los robots que
funcionan con actuadores) , por lo tanto el control del
robot también depende del control de los actuadores.
Entre los distintos tipos de actuadores que componen
un robot, se encuentran los neumáticos, hidráulicos y
eléctricos, y cada tipo tiene sus particularidades en
cuestión de potencia, velocidad de respuesta y
controlabilidad.
Controlar en modo par un robot, es controlar sus
actuadores considerando la dinámica interna, tanto del
robot como de sus actuadores. En el caso de los
actuadores eléctricos, es necesario conocer las
características de los motores, así como los parámetros
máximos de operación, para asegurar un óptimo
funcionamiento y control adecuado.
En las figuras 2 y 3 se muestran el diagrama de las
partes que componen un motor eléctrico, y el diagrama
eléctrico que lo representa:

Fig. 2 Partes de un motor eléctrico.

Fig. 1 Robot planar de 3 grados de libertad.

1
 respecto al marco de referencia del robot), todo con
respecto al eslabón 𝑖. La fórmula que defina la energía
potencial tiene la siguiente forma:
𝑣
6) 𝑉(𝑞) = 𝑚𝑖 𝑔0 𝑝𝑐𝑚,𝑖
𝑣
donde 𝑔0 es la gravedad, y 𝑝𝑐𝑚,𝑖 es la componente
vertical de la posición del centro de masa del eslabón
Fig. 3. Diagrama eléctrico de un motor
𝑖 con respecto al maro de referencia del robot.
Posteriormente se calcula la función de Lagrange,
dada por la diferencia entre la energía cinética total y
1.1 Ecuaciones dinámicas de un motor la energía potencial total:
eléctrico
7) ℒ(𝑞, 𝑞̇ ) = 𝐾(𝑞, 𝑞̇ ) − 𝑉(𝑞)
Del diagrama de la figura 3, es posible derivar las
Posteriormente se calcula la ecuación de Lagrange,
ecuaciones que definen la dinámica eléctrica y
donde se deducen las ecuaciones de movimiento:
mecánica del motor. Del diagrama eléctrico, y por
medio de la Ley de Voltajes de Kirchoff, se puede 𝑑 𝜕ℒ 𝜕ℒ
8) − =𝛤
calcular la fuerza contraelectromotriz y las caídas de 𝑑𝑡 𝜕𝑞̇ 𝜕𝑞
tensión en los componentes eléctricos del motor, donde 𝛤 es el vector de pares y fuerzas generalizadas.
mediante la siguiente ecuación: Además, si la energía cinética es cuadrática, es decir
𝑉 − 𝑉𝑅 − 𝑉𝐿 = 𝐸𝑔 1
9) 𝐾(𝑞, 𝑞̇ ) = 𝑚𝑖 𝑞̇ ′𝐷(𝑞)𝑞̇
2
Sustituyendo los valores de VR y VL, la ecuación
anterior toma la siguiente forma: donde 𝐷(𝑞) es una matriz simétrica definida positiva,
conocida como matriz de inercia, entonces la ecuación
𝑑𝑖𝑎
1) 𝑉 − 𝑅𝑖𝑎 − 𝐿 = 𝐸𝑔 8 resulta en una ecuación diferencial de segundo
𝑑𝑡
orden, tal que:
Además, el voltaje que se opone al flujo de corriente
en el conductor, Eg, es proporcional a la velocidad 10) 𝐷(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝐺(𝑞) = 𝛤
angular del rotor, de tal forma que: 𝜕𝑉(𝑞)
donde 𝐺(𝑞) = es el vector de gravedad, y
𝜕𝑞
2) 𝐸𝑔 = 𝐾𝑏 𝜔
𝜕
donde Kb es la constante de fuerza contraelectromotriz 11) 𝐶(𝑞, 𝑞̇ )𝑞̇ = ( (𝐷(𝑞)𝑞̇ )) 𝑞̇ −
𝜕𝑞
del rotor. El par (𝜏𝑒 ) generado por el eje del rotor es ′
proporcional al flujo magnético (φ) y a la corriente de 1 𝜕
(𝐷(𝑞)𝑞̇ )) 𝑞̇
(
armadura (ia), por lo tanto: 2 𝜕𝑞

3) 𝜏𝑒 = 𝐾𝑒 𝜑𝑖𝑎 = 𝐾𝑚 𝑖𝑎 es la matriz de Coriolis (multiplicada por el vector de

velocidad de las coordenadas generalizadas). La
donde Km es la constante del par del motor.
matriz función 𝐶 no es única, y normalmente se
Finalmente, la suma de todos los pares da lugar a la calcula cada uno de los elementos de la matriz:
aceleración del eje del motor, multiplicado por la
1 𝜕𝑑𝑘𝑗 𝜕𝑑𝑘𝑖 𝜕𝑑𝑖𝑗
inercia del rotor, de tal manera que: 12) 𝐶𝑘𝑗 = ∑Ñ𝑖=1 ( + − ) 𝑞̇ 𝑖
2 𝜕𝑞𝑖 𝜕𝑞𝑗 𝜕𝑞𝑘
4) ∑ 𝜏 = 𝐽𝜔̇ = 𝜏𝑒 + 𝜏𝑙 + 𝐵𝑚𝑚
donde 1≤ 𝑘 representa el renglón de la posición del
donde Bmm es la fricción viscosa del motor. elemento en la matriz, 𝑗 ≤ Ñ representa el número de
columna, Ñ el número de grados de libertad del
sistema, y 𝑑𝑘𝑗 es el elemento 𝑘𝑗 de la matriz de inercia
1.2 Ecuaciones dinámicas de un robot 𝐷(𝑞). Al cálculo de los elementos de la matriz de
Coriolis, de la ecuación 12, se les conoce como los
Para conocer las ecuaciones que definen la dinámica
símbolos de Christoffel.
de un robot (robot serial), primero se calculan la
energía cinética y la energía potencial total de todo el La ecuación 10 es una forma de representar un robot
sistema: matemáticamente y es muy utilizada en la literatura de
1 2 2 la robótica.
ℎ 𝑣
5) 𝐾(𝑞, 𝑞̇ ) = 𝑚𝑖 ((𝑝̇𝑐𝑚,𝑖 ) + (𝑝̇𝑐𝑚,𝑖 ) )+
2
1
𝐽 (𝜃̇ 𝑎𝑏𝑠 )
2 𝑐𝑚,𝑖 𝑖
1.3 Tipos de controles
donde 𝐾 representa la energía cinética, 𝑚𝑖 es la masa,
ℎ
𝑝̇𝑐𝑚,𝑖 𝑣
y 𝑝̇𝑐𝑚,𝑖 son las componentes vertical y horizontal, Obteniendo las ecuaciones que definen la dinámica del
motor, es posible diseñar un control basado en las
respectivamente, de la velocidad lineal en el centro de
características dinámicas del mismo.
masa, 𝐽𝑐𝑚,𝑖 es el momento de inercia sobre el centro de
masa y 𝜃̇𝑖𝑎𝑏𝑠 es la velocidad angular absoluta (con Existen diferentes tipos de controles, entre ellos los
más usados son:

2
 Control Proporcional (P)
 Control Derivativo (D)
 Control Integral (I)
Un sistema de control proporcional es un tipo de
sistema de control de realimentación lineal, en el cual
la salida del controlador es proporcional a la señal de
error, que es la diferencia entre una señal objetivo o
deseada y la variable de salida de la planta o sistema
en cuestión. Se dice que el control proporcional
depende del error actual.
La acción derivativa se manifiesta cuando existe un
cambio en el valor absoluto de la señal de error, y su
función es la de mantener el error al mínimo
corrigiéndolo proporcionalmente con la misma
velocidad con la que se produce, evitando así que el
error se incremente. Se dice que la acción derivativa es
una predicción de los errores futuros.
La acción integral busca atenuar y eliminar el error en
estado estacionario, provocado por perturbaciones
exteriores. El control integral actúa cuando existe una
diferencia entre la señal objetivo y la variable de
salida, integrando esta desviación en el tiempo y
sumándola a la acción proporcional. El control integral
depende de los errores pasados. Fig. 4 Representación de las ecuaciones con diagramas a bloques

Normalmente, se utilizan combinaciones de los

controles antes mencionados, PD, PI y PID ya que
brindan mayor estabilidad y mejora la respuesta de los 2 Desarrollo
sistemas en los que son empleados.
2.1 Selección del motor
Para la realización del control de corriente es
1.3 Diagramas a bloques necesario definir las características físicas del motor a
controlar, ya que se consideran los parámetros de
Para facilitar la implementación de los controles operación para la sintonización del control. Para ello,
mencionados en la sección anterior, usualmente se se consideró el motor de imán permanente que se
utilizan diagramas a bloques para representar las ilustra en la figura 4:
ecuaciones que definen a los sistemas, en conjunto con
las representaciones de los sistemas de control. En la
figura 4, se ilustran los diagramas a bloques para cada
una de las ecuaciones anteriormente obtenidas
(ecuaciones 1-4), así como la integración de todos
ellas que, en conjunto, definen la dinámica del motor.
Cabe mencionar que, para la representación en
diagrama a bloques, las ecuaciones se transforman al Fig. 5 Servo motor AC, Schneider Electric.
dominio de la frecuencia mediante la Transformada de
Laplace.

Tabla. 1 Características físicas del servo motor.

3
2.2 Control del motor Ahora se conocen las ganancias del control PI,
sustituyendo en el diagrama de la figura 5, se obtiene:
El control del motor se diseñó a partir del diagrama de
la figura 3, donde la entrada principal es el voltaje que
se le suministra al sistema. Se seleccionó un control de
corriente, ya que el par generado por el rotor es
directamente proporcional a la corriente, y ésta a su
vez es proporcional al voltaje. Para ello, se
implementó un control PI, ya que la fuerza
contraelectromotriz (dinámica mecánica) genera una
perturbación al voltaje de entrada (dinámica eléctrica),
Fig. 8 Control de corriente PI con valores de ganancias Kp y Ki
la cual es deseable atenuar y eliminar, con el fin de
controlar de manera eficiente y precisa la posición del El nodo de entrada (2) del diagrama de control de
rotor. El siguiente diagrama a bloques representa el corriente, corresponde al par deseado para controlar la
control de corriente implementado previo a la entrada posición, el cual es calculado a través la estimación de
de voltaje del diagrama de la figura 3: error entre la posición actual del rotor y la posición
deseada. Para ello se aplicó un control PD con la
finalidad de atenuar el error entre las señales de
posición, y que la señal de posición del rotor converja
en el menor tiempo posible a la posición deseada.

Fig. 6 Control de corriente, con acción proporcional e integral

El nodo de salida (1) lleva la señal de voltaje a la

entrada del diagrama completo de la figura 3, mientras
que el nodo de entrada (1) es la corriente ia que se
retroalimenta, multiplicado por una ganancia, de valor
1 correspondiente a la ganancia del sensor, hacia el
sistema de control de corriente. El nodo de entrada (2),
τd, es el valor del par deseado para controlar la posición
deseada del motor. Debido a que el sistema de control
de corriente se vuelve de segundo orden al introducir
la acción integral, se pueden deducir los valores de las
ganancias Kp y Ki por medio de la ecuación general de
segundo orden, al analizar la función de transferencia
del control PI:
Fig. 7 Sistema de control PI.
La función de transferencia que describe al sistema de
la figura 6 es la siguiente:
(𝐾𝑝 𝑠+𝐾𝑖 )/𝐿
1) 𝐺𝑐 (𝑠) = 𝐾𝑝 +𝑅 𝐾
𝑠 2 +( )𝑠+ 𝑖
𝐿 𝐿
Considerando el denominador de la función de
transferencia, se igualan los coeficientes de cada
término a la ecuación general de segundo orden:
𝑓(𝑠) = 𝑠 2 + 2𝜉𝜔𝑛 𝑠 + 𝜔𝑛 2
𝐾𝑝 = 2𝜉𝜔𝑛 𝐿 − 𝑅
𝐾𝑖 = 𝜔𝑛 2 𝐿
Donde ωn es la frecuencia natural del sistema, que está
definida por la relación entre la resistencia del motor y
la inductancia, ξ es el factor de amortiguamiento.
Asumiendo ξ=1, se deduce:
𝐾𝑝 = 𝑅
𝐾𝑖 = 𝑅2 /𝐿
Fig. 9 Diagrama a bloques completo de control PD de posición y PI de
corriente del motor.
El control aplicado tiene la forma Kp+Kd·s, por lo que
la ganancia Kd multiplica la derivada de la posición, ω.
El bloque “control de corriente” corresponde al
diagrama a bloques de la figura 7, y el bloque “motor”
corresponde al diagrama a bloques de la figura 3
(diagrama equivalente completo). El valor de las
ganancias Kp y Kd se estimó a manera de prueba y
error, midiendo la respuesta del sistema, procurando
que la posición del motor efectivamente llegara a la
posición deseada, y en un tiempo menor a medio
segundo, además de medir la velocidad con la que
arrancaba el motor y el par de arranque (que no
estuviera por encima de los valores máximos
permitidos del motor).
2.3 Integración del sistema
Una vez obtenido el control para cada motor, se
procedió a integrarlos para la simulación del
manipulador planar de 3 grados de libertad.
La metodología utilizada se muestra en la Fig. 10 y
consta de 3 bloques de motores, donde cada uno
contiene las dinámicas de cada motor, las entradas de
los bloques son las posiciones deseadas
proporcionadas por la diferencia del error de posición
actual y deseada, la salida de los bloques son los
parámetros de posición, velocidad y aceleración de
cada motor los cuáles alimentan un función de
MATLAB.

4
 Fig. 12 Diagrama general con control PD general de posición para los 3
motores

El segundo método utilizado consiste en separar el

control PD de posición para cada motor, esto para
sintonizar cada control en función del desempeño de
cada motor en la cadena cinemática, dado que no es la
misma energía requerida por cada motor para alcanzar
Fig. 10 Sección del diagrama de bloques, mostrando los 3 bloques del
motor que alimentan una función de MATLAB la misma posición. En la Fig. 13 se muestran los
controles por separado.
Dicha función (ver Anexo 1) contiene las ecuaciones
de movimiento para el caso específico de un
manipulador planar de 3 grados de libertad, los
parámetros como masa, longitud a los centros de masa,
longitud del eslabón y gravedad son especificados en
la función para fines de la simulación.
Con todos estos parámetros la función se encarga de
calcular todos los elementos correspondientes a las
ecuaciones de movimiento, matriz de inercia, vector
de Coriolis y vector de gravedad, regresando como
resultado los torques necesarios para el movimiento de Fig. 13 Diagrama general con control PD particular de posición para
cada motor
cada articulación del manipulador.
Tales torques provenientes del manipulador
realimentan la dinámica de los motores 3 Conclusiones
correspondientes, los torques se convierten en torques
de carga para los motores, en la Fig. 11 se muestra que A continuación, se muestran las gráficas con los
dichos son comparados para estabilizar el torque resultados obtenidos simulando el sistema con el
entregado por el motor en una consigna deseada. primer método, en donde se aplica un control PD
general de posición. Se muestran en función del
tiempo: la posición angular (theta), el error de
posición, la velocidad angular (omega), los torques
(tau) y la corriente (Ia). Las consignas fueron 1.6, 2 y
3.5 rads respectivamente para cada motor, las
ganancias del control PD, fueron Kp =100 y Kd = 70.

Fig. 11 Dinámica mecánica del motor

Como se aprecia también en la Fig. 11, las constantes

de reducción (n) de los actuadores son necesarios para
comparar los torques eléctrico (Tau_e) y de carga
(Tau_l).
Dos métodos fueron implementados para el control de
los motores del manipulador, ambos esclavizando la
posición angular, en el primero se diseñó un control
general para los tres actuadores, las mismas ganancias
proporcionales y derivativas para el control de
posición como se muestra en la Fig. 12.

Fig. 14 Posición angular de los tres motores con control general de

posición.
5
 Fig. 15 Error de posición con control general de posición. Fig. 18 Corriente de armadura de un motor para control general de
posición.

Para el caso particular donde se aplica un control

general para los tres motores se puede observar, que a
pesar de que convergen a la posición deseada, cada
motor se ve perturbado por la contribución del motor
anterior en la cadena cinemática, por lo que se
procedió a implementar el segundo método, esto con
el fin de sintonizar cada motor de en función de su
desempeño en la cadena cinemática.
Ahora se muestran los resultados obtenidos después de
aplicar el control PD particular para cada motor.

Fig. 16 Velocidad angular de cada motor para control general de

posición.

Fig. 19 Posición angular de los tres motores con control particular de

posición.

Fig. 17 Torque generado por un motor para control general de posición.

6
Fig. 20 Error de posición angular con control particular de posición. Fig. 23 Corriente de armadura de un motor para control particular de
posición.

Al comparar ambos experimentos, se observa que

aunque teóricamente cada motor debe estar
sintonizado particularmente en función de su posición
en la cadena cinemática, las diferencias son mínimas
comparadas cuando se aplica un control general para
todos los motores. Posiblemente las diferencias sean
más notables cuando las longitudes de los eslabones y
las masas sean mayores.

4 Referencias
[1] Olguín, E. 3D Motion of Rigid Bodies. Vol. 7.9,
(Agosto, 2016).
[2] Spong, M. W., Vidyasagar, M. and Hutchinson,
S. Robot Modeling and Control. Vol 1,
Fig. 21 Velocidad angular de cada motor para control particular de
(Noviembre, 2005), pp. 231-242.
posición.
[3] Schneider Electric, BSH0552P32F2A, AC-
Servo-motor specs. Disponible en línea:
https://www.schneider-
electric.us/en/product/BSH0552P32F2A/ac-
servo-motor-bsh---0.9-n.m---4000-rpm---keyed-
shaft---with-brake---ip65/
[4] Westervelt, E., Grizzle, J., Chevallereau, C.
Feedback Control of Dynamic Bipedal Robot
Locomotion. 1st edition, (Abril, 2007), pp. 418-
420.

Fig. 22 Torque generado por un motor para control particular de

posición.

7
5 Anexos
5.1 Función de MATLAB para calcular ecuaciones de movimiento de un manipulador
planar de 3 grados de libertad
function [T]=robot_3dof(q1,q2,q3,qp1,qp2,qp3,qdp1,qdp2,qdp3)
L1=0.08;
L2=L1;
L3=L2;
lcm1=0.06;
lcm2=0.06;
lcm3=0.06;
m1=0.3;
m2=0.3;
m3=0.3;
Jcm1=0.8;
Jcm2=0.8;
Jcm3=0.8;
g0=9.81;

%Generalized coordinates position and velocity vectors

q=[q1;q2;q3];
qp=[qp1;qp2;qp3];
qdp=[qdp1;qdp2;qdp3];

%Absolute angles, function of the generalized coordinates

th_abs=[q1;q1+q2;q1+q2+q3];

%Cartesian positions of the center of mass of each link

pcm1=Rot_z(th_abs(1))*[lcm1;0];
pcm2=Rot_z(th_abs(1))*[L1;0]+Rot_z(th_abs(2))*[lcm2;0];
pcm3=Rot_z(th_abs(1))*[L1;0]+Rot_z(th_abs(2))*[L2;0]+Rot_z(th_abs(3))*[lcm3;0];

%Linear Velocity (Linear Jacobians)

pcmp1 =[-(3*sin(q1))/50, 0, 0;(3*cos(q1))/50, 0, 0];
pcmp2 =[- (3*sin(q1 + q2))/50 - (2*sin(q1))/25, -(3*sin(q1 + q2))/50, 0;(3*cos(q1 + q2))/50 +
(2*cos(q1))/25, (3*cos(q1 + q2))/50, 0];
pcmp3 =[- (3*sin(q1 + q2 + q3))/50 - (2*sin(q1 + q2))/25 - (2*sin(q1))/25, - (3*sin(q1 + q2 + q3))/50 -
(2*sin(q1 + q2))/25, -(3*sin(q1 + q2 + q3))/50;(3*cos(q1 + q2 + q3))/50 + (2*cos(q1 + q2))/25 +
(2*cos(q1))/25, (3*cos(q1 + q2 + q3))/50 + (2*cos(q1 + q2))/25, (3*cos(q1 + q2 + q3))/50];

%Angular Velocity (Angular Jacobians)

th_absp1 =[1, 0, 0;0, 0, 0;0, 0, 0];
th_absp2 =[0, 0, 0;1, 1, 0;0, 0, 0];
th_absp3 =[0, 0, 0;0, 0, 0;1, 1, 1];

%Total center of mass of the whole kinematic chain

pcm=(m1/(m1+m2+m3))*pcm1 + (m2/(m1+m2+m3))*pcm2 + (m3/(m1+m2+m3))*pcm3;

%Potential energy (locally, on each link)

Vt=(m1+m2+m3)*g0*pcm(2);

%Mass-Inertia Matrix
D=(m1*(pcmp1.')*pcmp1)+(m2*(pcmp2.')*pcmp2)+(m3*(pcmp3.')*pcmp3)...
+(Jcm1*(th_absp1.')*th_absp1)+(Jcm2*(th_absp2.')*th_absp2)+(Jcm3*(th_absp3.')*th_absp3);

%Coriollis Matrix
C =[- (qp3*((9*sin(q2 + q3))/3125 + (9*sin(q3))/3125))/2 - (qp2*((9*sin(q2 + q3))/3125 +
(21*sin(q2))/3125))/2, - (qp3*((9*sin(q2 + q3))/3125 + (9*sin(q3))/3125))/2 - (qp1*((9*sin(q2 + q3))/3125 +
(21*sin(q2))/3125))/2 - (qp2*((9*sin(q2 + q3))/3125 + (21*sin(q2))/3125))/2, -(9*(sin(q2 + q3) +
sin(q3))*(qp1 + qp2 + qp3))/6250;
(qp1*((9*sin(q2 + q3))/3125 + (21*sin(q2))/3125))/2 -
(9*qp3*sin(q3))/6250,
-(9*qp3*sin(q3))/6250, -(9*sin(q3)*(qp1 + qp2 + qp3))/6250;
(qp1*((9*sin(q2 + q3))/3125 + (9*sin(q3))/3125))/2 +
(9*qp2*sin(q3))/6250,
(9*sin(q3)*(qp1 + qp2))/6250, 0];

%Gravity vector
G =[(8829*cos(q1 + q2 + q3))/50000 + (20601*cos(q1 + q2))/50000 + (32373*cos(q1))/50000;
(8829*cos(q1 + q2 + q3))/50000 + (20601*cos(q1 + q2))/50000;
(8829*cos(q1 + q2 + q3))/50000];

%Dynamic equations
T=D*qdp+C*qp+G;
end