CONTROL PID DE UN BRAZO DE 2GL CON MOTOR DC
DOCENTE
: Ing. Machuca Mines
ALUMNOS
: Espinoza Dueñas, Balmes
CICLO
: XI
2013 - III
1. Objeti Objetivo vo Implementar un controlador PDI para un brazo robótico de grados de libertad con un motor D!.
2. Mo!e"o Di#$%i&o !e" 'obot "a ecuación din#mica $ue representa los eslabones puede ser representada por:
Donde M representa la matriz de inercia, ! representa la matriz de %uerzas centri%ugas & coriolis, g representa las %uerzas gra'itacionales & τ es el 'ector de tor$ue. (e obtienen las ecuaciones din#micas $ue modelan el robot manipulador aplicando las ecuaciones de mo'imiento de "agrange. !abe resaltar $ue este m)todo es e%ecti'o cuando se traba*a con robots de + & grados de libertad luego el m)todo se 'uel'e pesado para mas grados de libertad se utiliza el m)todo de Euler.
Donde "-$,$ es el lagrangiano el cual se de/ne como:
Donde 0 es la energ1a cin)tica del robot & 2 es la energ1a potencial, pudiendo descomponer el manipulador en la suma de partes debido al modelo del robot.
(e considera el modelado particular de un robot manipulador planar de g.d.l. con articulaciones rotatorias, como se muestra en la /gura siguiente.
3ig. +: Manipulador de dos eslabones Para el c#lculo de la energ1a cin)tica, se utilizar#n los *acobianos correspondientes a los centros de masa de cada uno de los eslabones, tomando las 'ariables de las articulaciones - q i , i4+, como coordenadas generalizadas, donde mi denota la masa del eslabón i5th, l i denota la longitud del eslabón i5th, l c i denota la distancia al centro de masa del eslabón i5th desde la articulación i5th, I i denota el momento de inercia del eslabón i5th con re%erencia a un e*e $ue apunta hacia %uera de esta p#gina, pasando a tra')s del centro de masa del eslabón i5th. "a energ1a cin)tica est# dada por:
K=
1 2
. T
q
n
∑[ m
T
i
J v ci (q) J v ci (q) + J
T
T
wi
]
.
(q) R i (q) Ii Ri (q) J w i (q) q
-+
i= 1
K =
1 2
. T
.
q H(q) q .
Para el c#lculo de los *acobianos es necesario tener la cinem#tica directa de los centros de masa de cada uno de los eslabones, los cuales est#n dados por:
X v c 1 Y = vc1 Z v c 1
l c 1 cos q 1 l sen q 1 c1 0
X v c 2 Y = vc2 Z v c 2
l 1 cos q 1 + l c 2 cos(q 1 + q 2 ) l sen q + l sen(q + q ) . 1 c2 1 2 1 0
"uego, para el c#lculo de los *acobianos se procede de la siguiente manera:
J v ci
∂ ∂ ∂ = ∂ ∂ ∂
f !i
∂ f !i
q1 f i
∂ q 2
q1 f zi
∂ q 2
q1
∂ q 2
∂ f i ∂ f zi
,
con lo $ue cada uno de los *acobianos $ueda e6presado por:
J v c1
Jvc2
" l c 1 sen q1 = l c 1 cos q1 0
" l 1 sen q1 " l c 2 sen(q1 + q 2 ) = l 1 cos q 1 + l c 2 cos(q 1 + q 2 ) 0
0
0 0
" l c 2 sen(q 1 + q 2 ) l c 2 cos(q 1 + q 2 ) 0
.
,
.
donde se tiene $ue v c 1 = J v c 1 q & v c 2 = J v c 2 q . 7hora incluiremos el t)rmino de 'elocidad angular para este manipulador. (e puede apreciar $ue:
.
ω 1 = q 1 # ,
&
.
i ω 2
.
= (q 1 + q 2 ) # , los
cuales son e6presados en el mismo marco de base inercial. En este e*emplo el e*e z de w 1 w 2 est#n en la misma dirección, por lo cual el e%ecto de multiplicar los *acobianos angulares por las matrices de rotación de cada eslabón no a%ecta al *acobiano angular.
"o anterior puede ser 'isto en detalle de la manera siguiente. (i R i representa la matriz de rotación b#sica con respecto al e*e z tenemos: −%
$ R (q) = % 0
0
. 1
$
0
0
"a 'elocidad angular del e%ector /nal relati'a al marco base, puede ser e6presada por la 'elocidad de cada eslabón respecto al marco base 8-e*e z 0 J w 1 ( q) = 0 1 0 J w 2 ( q) = 0 1
0
0 0
,tal $ue
.
ω 1 = q 1 #
0 0
,tal $ue
1
.
i ω 2
.
= (q 1 + q 2 ) # .
9ealizando la operación indicada para el primer eslabón + tenemos:
$1 " %1 0 I11 0 0 1 &= %1 $1 0 I 2 1 0 0 0 0 0 1 I '1 9ealizando
el
I1 2 I2 2 I' 2
mismo
I1 ' $1
%1
0 0
0
" %1 $1 0 0 0 = I ' ' 0 I ' ' 0 0 1 1 0
I2'
procedimiento
para
el
0
1 0 = I 1 0 0 0
eslabón
obtenemos:
$2 " %2 0 I11 0 0 1 = %2 $2 0 I 2 1 0 0 1 0 0 1 I '1
I1 2 I2 2 I' 2
I1 ' $2
%2
0 0
0
" %2 $2 0 0 0 = I ' ' I ' ' I ' ' 0 0 1 1 1
I2'
I''
1 1 I = 2 1 1 I''
Donde I1 e I 2 son los momentos de inercia de los eslabones + & con respecto a sus centros de masa a tra')s del e*e z. De esta manera la energ1a cin)tica rotacional de todo el sistema 'iene representada por:
1 q I 1 2 0
1 1 . I + 2 q 0 1 1 0
. T
1
Desarrollando la ecuación -+ tanto para la 'elocidad lineal & angular se tiene: T
T
H(q)= m1 J v c 1 ( q) J v c 1 (q) + m2 J v c 2 ( q) J v c 2 (q)
T
T
+ J T w 1 (q) R1 (q) I 1 R1 (q) J w1 (q) + J T w2 (q) R2 (q) I2 R2 (q) Jw 2 (q) "l c 1%1
H(q)= m1
0
"l c 1%1 0 l c 1%1 0 0 0 0
0
l c 1%1 0
"l 1%1 " l c 2%12 −l c 2%12 "l 1%1" l c 2%12 l 1$1" l c 2$12 0 + I 1 0 + I 2 l $1" l $12 l $12 + m2 1 c2 c2 0 0 I 2 l c 2 $12 0 −l c 2%12 0 0
I2 I 2
m1l 2 c 1 0 m2 (l 2 c 2 + l 21 + 2l 1l c 2$2) m2 (l 2 c 2 + l 1l c 2$2) I1 0 I 2 I 2 + H(q) = + + 2 2 0 0 I I 0 0 m ( l l l $2) m l + c 2 c 2 2 2 2 1 c2 2 m1l c 12 + m2 (l12 + l c 2 2 + 2l1lc 2 $2) + I1 + I2 m2 (lc 2 2 + l1 lc 2 $2) + I2 11 12 H(q)= = 2 2 21 22 m (l l l $2) + I m l I + + 2 c2 1 c2 2 2 c2 2 (e 'eri/ca $ue -$ es sim)trica, &a $ue 12 = 21 . Para obtener la matriz de !oriolis tenemos, necesitamos calcular los s1mbolos de !hristo;el, los cuales est#n dados por: c i * # =
1 ∂ # * ∂ # i ∂ i * + − ∂ q * ∂ q # 2 ∂ q i
Desarrollando todos los t)rminos, apro'echando la propiedad c i * # 4 c * i # , se tiene: c111 =
1 ∂ 11
2
∂ q 1
+
∂ 11 ∂ q 1
−
=0 q1
∂ 11 ∂
c11 2 =
1 ∂ 2 1
2
∂ q1
c1 2 1 = c 2 11 =
2
c2 2 2 =
2
∂ q 1
+
∂ q 2
∂ 2 1 ∂ q 2
1 ∂ 11 = " =− q2 2 ∂ q 2
∂ 11 ∂
∂ 11
−
∂ q 2
+
∂ q 2
+
−
∂ q 1
1 ∂ 2 1
1 ∂ 1 2
2
∂ 2 1
1 ∂ 1 2
c 212 = c 12 2 =
c 2 21 =
+
∂ 2 2 ∂ q1
−
1 = (−2m 2 l1l c 2 %2) = "m 2 l1l c 2 %2 = q1 2
∂ 1 2
−
∂
1 = (−m 2 l1l c 2 %2 + m 2 l1l c 2 %2) = 0 q2 2
∂ 2 1 ∂
1 = (−m 2 l1l c 2%2 " m 2 l1l c 2%2) = "m 2 l1l c 2%2 = q1 2
∂ 2 2 ∂
∂ 2 2 ∂ 2 2 1 ∂ 2 2 + − =0 ∂ q 2 ∂ q 2 2 ∂ q 2
Para calcular la matriz de !oriolis con los simbolos de !hristo;el, se tiene la siguiente ecuación: $(# *) =
2
∑
.
c i * # (q) q i ,
i
por lo $ue /nalmente la matriz de !oriolis $ueda de la siguiente %orma: . . . $ $ q q q + 12 2 1 2 = 11 $(q q) = . $ $ − q 2 1 2 2 0 1 .
"a energ1a potencial del manipulador es la suma de las energ1as potenciales de los dos eslabones. Para cada eslabón la energ1a potencial es la de su masa multiplicada por la aceleración de la gra'edad & altura de su centro de masa, entonces: -1 = m1 , l c 1 sen(q 1 )
-2 = m 2 ,( l1 sen(q 1 ) + l c 2 sen(q 1 + q 2 )
Dado $ue <4 -1 + -2 , entonces la energ1a potencial total del manipulador es: - = m1 , l c 1 sen(q1 ) + m 2 ,( l1 sen(q1 ) + l c 2 sen(q1 + q 2 ))
= ,sen(q 1 )( m 1 l c 1 + m 2 l 1 ) + m 2 l c 2 ,sen(q 1 + q 2 ) .
"uego, desarrollando las deri'adas del potencial:
, 1 (q ) =
∂ ∂ q 1
= ,cos(q 1 )( m 1l c 1 + m 2 l1 ) + g m 2 l c 2 cos(q1 + q 2 )
, 2 (q ) =
∂ ∂ q 2
= g m 2 l c 2 cos(q1 + q 2 ) ,
por lo tanto el 'ector de gra'edad g-$ $ueda e6presado por:
,1 (q) ,(q) = . , ( q ) 2 3inalmente obtenemos las ecuaciones de la din#mica del manipulador de la siguiente manera: .. . 11 12 q1 $11 $12 q1 g 1 (q) Γ 1 21 22 .. + $21 $22 . + g (q) = Γ q 2 q 2 2 2
..
.
.
H(q) q + $(qq ) q + ,(q) = Γ
3. DISE(O DEL CONTROLADOR PID
I#t'o!)&&i*#
Este tutorial le mostrar# las caracter1sticas de los controladores proporcional -P, integral -I, & deri'ati'o -D, & cómo usarlos para obtener una respuesta deseada. En esta =u1a, consideremos el siguiente sistema de realimentación unitaria:
Planta: sistema a controlar !ontrolador: Pro'ee la e6citación de la planta (e diseña para controlar el comportamiento de todo el sistema
E" &o#t'o"+!o' !e t'e, t'%i#o, "a %unción de trans%erencia del controlador PID es:
•
0p 4 =anancia Proporcional 0I 4 =anancia Integral
•
0d 4 =anancia Deri'ati'a
•
Primero, echemos un 'istazo a cómo traba*a el controlador PID en un sistema a lazo cerrado usando el es$uema de aba*o. "a 'ariable -e representa el error de seguimiento, $ue es la di%erencia entre el 'alor deseado de entrada -9 & la salida real ->. Esta señal de error -e ser# en'iada al controlador PID , & )ste calcular# tanto la deri'ada cuanto la integral de esta señal de error. "a señal -u reci)n salida del controlador es ahora igual a la ganancia proporcional -0p 'eces la magnitud del error m#s la ganancia integral -0i 'eces la integral del error, m#s la ganancia deri'ati'a -0d 'eces la deri'ada del error.
"a señal -u se en'iar# a la planta, & se obtendr# la nue'a salida ->. Esta nue'a salida -> se re5en'iar# al sensor para hallar la nue'a señal de error -e. El controlador toma esta nue'a señal de error & computar# su deri'ada & su integral otra 'ez. Este proceso sigue sin parar.
L+, &+'+&te'i,ti&+, !e "o, &o#t'o"+!o'e, P I / D 2n controlador proporcional -0p tendr# el e%ecto de reducir el tiempo de ele'ación & reducir#, sin *am#s eliminar, el error de estado
estacionario. 2n control integral -0i tendr# el e%ecto de eliminar el error de estado estacionario, pero puede empeorar la respuesta transitoria. 2n control deri'ati'o -0d tendr# el e%ecto de incrementar la estabilidad del sistema, reduciendo el sobrepico, & me*orando la respuesta transitoria. "os e%ectos de cada uno de los controladores 0p, 0d, & 0i en un sistema a lazo cerrado se resumen en la tabla de aba*o.
Rt+ + L Ce''+!o
T T.TREPAD SOBREPI E,t+b"e&i A CO %.
Ba*a
(ube
i
Ba*a Poco !ambio
(ube
Poco !ambio (ube
Ba*a
Ba*a
!
ERROR SS Ba*a Elimina Poco !ambio
?ote $ue estas correlaciones podr1an no ser e6actamente seguras, por$ue 0p, 0i, & 0d son dependientes entre s1. De hecho, cambiando una de estas 'ariables se puede 'ariar el e%ecto de las otras dos. Por esta razón, la tabla deber# usarse @nicamente como re%erencia cuando se determina los 'alores de 0i, 0p & 0d.
4. MODELO DE MOTOR DC 4.1. E&)+&io#e, 56,i&+, !e" ,i,te%+ 2n actuador mec#nico mu& di%undido es el motor de !!. Pro'ee directamente mo'imiento rotacional &, adecuadamente acondicionado, mo'imiento traslacional. El circuito el)ctrico de armadura & el diagrama mec#nico rotacional, se muestran en la /gura:
Para el e*emplo se consideraron los siguientes par#metros: A momento de inercia del sistema - 4 C.C+ 8g.ms A coe/ciente de roce -b 4 C.+ ?ms A constante de %uerza electromotriz -040e40t 4 C.C+ ?m7mp A resistencia de armadura -9 4 + ohm
A inductancia de armadura -" 4 C.F A entrada -<: 3uente de Gensión A posición del e*e: H A (e supone rotor & e*e r1gidos. "a cupla -G est# relacionada con la corriente de armadura & la %em -e con la 'elocidad de rotación, seg@n las ecuaciones:
siendo ambas constantes iguales -0t40e40 En base a la le& de ?eton & la le& de 0ircho;, resultan las siguientes ecuaciones di%erenciales $ue describen la din#mica del sistema:
4.2.
7)#&i*# !e T'+#,5e'e#&i+
7plicando la Grans%ormada de "aplace & haciendo cero las condiciones iniciales, las ecuaciones del sistema $uedan e6presadas en el dominio de s:
Eliminando I-s se obtiene la trans%erencia entre la entrada de tensión de armadura < & la 'elocidad de rotación JH como salida:
8. A'&9ivo M-:"e &o# to!o, "o, !+to, )ti"i;+!o,
8.1. Si%)"+&i*# e# ,i%)"i#<
3ig. + !ontrol PID K Motor D! K Blo$ue del robot
3ig. !ontroladores PDI para cada articulación.
3ig. L Diagrama de blo$ues del robot de gl.
3ig. Diagrama de blo$ues del Motor D!
3ig. F Diagrama de blo$ues de la matriz de inercia para t+
3ig. N Diagrama de blo$ues de la matriz de inercia par t+. & t.+
3ig. O Diagrama de blo$ues de la matriz de %ricción para G+ & G
3ig. Diagrama de blo$ues de la matriz de gra'edad para G+
3ig. Q Diagrama de blo$ues de la matriz de gra'edad para G
3ig. +C Diagrama de blo$ues de las tra&ectorias de entrada
=. RESULTADOS RESPUESTA DEL ROBOT A LAS TRA>ECTORIA DE ENTRADA Estabilización para H+ con entrada Gra++ 4 5C.LA-uLK.FA-u5C
Estabilización +.LFA-u
para
H
con
entrada
Gra+
4
5C.CQA-uLK
Para H+ con entrada Gra+4 5sin-u
Para H con entrada Gra 4 -u5L
Para H+ con entrada GraL+ 4 C.+FA-piAcos--piAu
Para H con entrada GraL 4 C.FA-u5
