## page was renamed from LabElectronica/PVTOL_ControlDelAngulo #format inline_latex #acl BecariosGrupo:read,write,admin,delete All:read <> == Modelo Balancín == || Correcciones: la referencia a lazo abierto no es ángulo, debería ser torque. || {{attachment:FdTLASinRea.png || width="600"}} $$$G_p = \frac{K_J}{s^2}$$ $$$G_c = K_p$$ $$$K_M = \frac{\Delta_F}{\Delta_D}$$ $$$K_J = \frac{r}{J_T}$$ En donde $$G_c$$ es el compensador, $$k_M$$ es la función de transferencia entre fuerza y duty, y $$k_J$$ es la función de transferencia entre fuerza y ángulo. $$r$$ representa la distancia desde la aplicación de la fuerza hasta el centro de giro y $$J_T$$ es la inercia total. La función de transferencia a Lazo abierto queda: $$$G_{LA}(s) = \frac{K_pK_MK_J}{s^2}$$ == Modelo Balancín con Compensación Proporcional y Realimentación == Suponemos que la función de transferencia del sensor es lineal, por esto podemos decir que la $$H(s)=1$$. De esto obtenemos: $$$\frac{\Phi_{out}(s)}{\Phi_{ref}(s)}=\frac{G_{LA}(s)}{1+G_{LA}(s)H(s)}=\frac{K_pK_MK_J}{s^2 + K_pK_MK_J}$$ == Modelo Balancín con Compensación Proporcional-Derivativa y Realimentación == {{attachment:FdTLA_PD.png || width="600"}} Suponemos que la función de transferencia del sensor es lineal, por esto podemos decir que la $$H(s)=1$$. De esto obtenemos: $$$G_c(s) = K_p + K_ds$$ $$$G_{LA}(s) = \frac{K_MK_J}{s^2}$$ $$$\frac{\Phi_{out}(s)}{\Phi_{ref}(s)}=\frac{G_{LA}(s)G_c(s)}{1+G_{LA}(s)H(s)G_c(s)}=\frac{K_pK_MK_J + K_dK_MK_Js}{s^2 + K_dK_MK_Js + K_pK_MK_J}$$ == Modelo Balancín con Compensador PID == $$$ G_p(s) = \frac{1}{Js^2} $$ ; $$$ G_c(s) = k_p\cdot(1 + \frac{1}{T_is} + T_ds}) $$. $$$ G_{LA}(s) = \frac{k_p}{T_iJ}\frac{T_iT_ds^2 + T_is + 1}{s^3}$$ $$$ G_{LC}(s) = k_p\frac{T_iT_ds^2 + T_is + 1}{T_iJs^3 + k_pT_iT_ds^2 + k_pT_is + k_p}$$ == Cálculo Aproximado de las constantes == === Cálculo de la constante de Inercia === $$$K_J = \frac{r}{J_T}$$ $$$J = \sum{m.r^2}$$ $$$J_T = 2(J_{motor} + J_{ESC} + J_{helice}) + J_{barra}$$ $$$J_{motor} = 52.{20,5}^2 [g.{cm}^2] = 21853 [g.{cm}^2]$$ $$$J_{ESC} = 32.{10}^2 [g.{cm}^2] = 3200 [g.{cm}^2]$$ $$$J_{helice} = 15.{20,5}^2 [g.{cm}^2] = 6303,75 [g.{cm}^2]$$ $$$J_{barra} = \frac{m.L^2}{12} = \frac{110.{51}^2}{12} [g.{cm}^2] = 23842,5 [g.{cm}^2]$$ $$$J_T = 86556 [g.{cm}^2] = 0.0086556 [Kg.{m}^2]$$ $$$\boxed{K_J = \frac{21}{86556}[\frac{1}{g.cm}]= 24.26174962[\frac{1}{Kg.m}]}$$ === Cálculo de la constante del Motor === Según las mediciones realizadas podemos aproximar estas constantes. $$$K_{MPusher} = \frac{\Delta_F}{\Delta_D} = \frac{40}{3000}\frac{g}{cuentas}$$ $$$K_{MNormal} = \frac{\Delta_F}{\Delta_D} = \frac{70}{3000}\frac{g}{cuentas}$$ ==== Estabilización en modo común ==== En esta medición se trato de llevar el ángulo del PVTOL a cero seteando el pwm de un motor y ajustando el otro para que el ángulo sea 0. || Motor Normal || Motor Pusher || || 7700 || 5750 || || 8350 || 7600 || || 10100 || 11150 || || 11750 || 14200 || == Análisis Discreto == || Correcciones: El bloque del retentor de orden cero va después del bloque del compensador || {{attachment:Planta_PVTOL.png}} Planta: $$$G_p = \frac{1}{J*S^2}$$ Retentor de Orden Cero:$$$ G_r=\frac{1-e^{-TS}}{S}$$ $$$ Z[\frac{1 - e^{-TS}}{S} * \frac{1}{J*S^2}] = (1-z^-1) * Z[\frac{2}{2*J*S^3}] = \frac{1-z^{-1}}{2*J}*\frac{T^2*z^{-1}*(1+z^{-1})}{(1-z^{-1})^3}$$ $$$ Z[G_p, G_r] = \frac{T^2}{2*J} * \frac{z^{-1}*(1+z^{-1})}{(1-z^{-1})^{2}} $$ $$$ F(z) = G_p(z)*G_r(z)*G_c(z) = \frac{T^2}{2*J} * \frac{z^{-1}*(1+z^{-1})}{(1-z^{-1})^{2}} * Kp * [1 + \frac{1}{T_i*(1-z^{-1})} + T_d*(1-z^{-1})] $$ $$$ F(z) = \frac{T^2 * K_p}{2*J} * [\frac{z^{-2} + z^{-1}}{(1-z^{-1})^2} + \frac{z^{-2} + z^{-1}}{T_i*(1-z^{-1})^3} + \frac{T_d*z^ {-2} + T_d*z^{-1}}{(1-z^{-1})}] $$ $$$ F(z) = \frac{T^2 * K_p}{2*J} * \frac{T_i*T_d+(-T_i-T_i*T_d)*z+(1-T_i*T_d)*z^2+(1+T_i+T_i*T_d)*z^3}{z^4-3*z^3+3*z^2-z} $$ == Análisis en tiempo continuo, con retentor de orden cero aproximado == $$$ G_{h0}(S) = \frac{1-e^{-TS}}{S} $$ Una aproximación de esto es $$$ G_{h0} = \frac{2-TS}{S*(2+TS)} $$ $$$ G_p(S) = \frac{1}{J*S^2} $$ $$$ G_c(S) = K_p*(1 + \frac{1}{T_iS} + T_dS) $$ $$$ G_p(S)G_{h0}(S)G_c(S) = \frac{K_p*T_d}{J}*\frac{(2-TS)*(S^2+\frac{S}{T_i}+\frac{1}{T_iT_d})}{(2S+TS^2)*S^2*S} $$ $$$ G_p(S)G_{h0}(S)G_c(S) = \frac{K_p*T_d}{J}*\frac{-S^3+S^2*(\frac{2}{T}-\frac{1}{T_d})+S*(\frac{2}{T_dT}-\frac{1}{T_dT_i})+\frac{2}{T_dT_iT}}{S^5+S^4*\frac{2}{T}} $$ = Diseño del Controlador PID Discreto para el Balancín = == Funciones de Transferencia == === Función de Transferencia de la Planta más el Retentor de Orden Cero === ==== Tiempo Continuo ==== $$$ G_{h0}(s) = \frac{1-e^{-Ts}}{s} $$ Una aproximación de esto es $$$ G_{h0} = \frac{2-Ts}{s(2+Ts)} $$, pero no la usaremos. $$$ G_p(s) = \frac{1}{Js^2} $$ $$$ G(s) = G_{h0}(s)G_p(s) = \frac{1-e^{-Ts}}{Js^3}$$ ==== Tiempo Discreto ==== $$$ G(z) = \frac{T^2}{2J}\frac{z + 1}{(z-1)^2} = \frac{T^2}{2J}\frac{z + 1}{z^2 -2z + 1 }$$ === Función de Transferencia del Controlador PID === ==== Tiempo Discreto ==== $$$ G_c(z) = K_P + \frac{K_I}{1-z^{-1}} + K_D(1-z^{-1}) = \frac{(K_P + K_I + K_D)z^2 + ( -2K_D - K_P )z + K_D }{ z^2 - z }$$ === Función de Transferencia a Lazo Abierto === ==== Tiempo Discreto ==== $$$ G_{LA}(z) = G(z)G_c(z) = \frac{T^2}{2J}\frac{(K_P + K_I + K_D)z^3 + (K_I - K_D)z^2 + (-K_P - K_D)z + K_D}{z^4-3z^3+3z^2-z}$$ Si definimos $$$ K_{In}=\frac{K_I}{K_P} $$ y $$$ K_{Dn}=\frac{K_D}{K_P} $$. $$$ G_{LA}(z) = G(z)G_c(z) = \frac{T^2K_P}{2J}\frac{(1 + K_{In} + K_{Dn})z^3 + (K_{In} - K_{Dn})z^2 + (-1 - K_{Dn})z + K_{Dn}}{z^4-3z^3+3z^2-z}$$ === Función de Transferencia a Lazo Cerrado === ==== Tiempo Discreto ==== $$$ G_{LC}(z) = \frac{G(z)G_c(z)}{1+G(z)G_c(z)} = {T^2}\frac{(K_P + K_I + K_D)z^3 + (K_I - K_D)z^2 + (-K_P - K_D)z + K_D}{ 2Jz^4 + [(K_P+K_I+K_D)T^2 -6J]z^3 + [(K_I-K_D)T^2 + 6J]z^2 + [(-K_P-K_D)T^2 -2J]z + K_DT^2}$$ Definiendo $$$ K_{In} $$ y $$$ K_{Dn} $$ como en el punto anterior y definiendo $$$ J_n = \frac{J}{K_P} $$ y $$$ K_1 = \frac{2J_n}{T^2} $$. $$$ G_{LC}(z) = \frac{G(z)G_c(z)}{1+G(z)G_c(z)} = \frac{(1 + K_{In} + K_{Dn})z^3 + (K_{In} - K_{Dn})z^2 + (-1 - K_{Dn})z + K_{Dn}}{ K_1z^4 + ( 1+K_{In}+K_{Dn} - 3K_1)z^3 + (K_{In}-K_{Dn} + 3K_1)z^2 + (-1-K_{Dn} -K_1)z + K_{Dn}}$$ = Diseño del Controlador PI PD para el Balancín = == Planta == {{attachment:planta_rate_feedback.png}} == Función de Transferencia a Lazo Abierto == $$$ G_{PI}=k_p+\frac{K_i}{s}=k(1+\frac{t_i}{s})$$ $$$ G_D = k_ds $$ $$$ G_P = \frac{k_t}{Js^2} $$ $$$ G_{PD} = \frac{G_P}{1+G_PG_D} = \frac{k_t}{Js^2+k_tk_ds} $$ $$$ G_{LA} = \frac{kk_ts+kk_tt_i}{s^3J+k_dk_ts^2} $$ == Función de Transferencia a Lazo Cerrado == $$$ G_{LC} = \frac{G_{LA}}{1+G_{LA}} = \frac{ k k_t t_i + k k_t s}{s^3 J + k_d k_t s^2 + k k_t s + k k_t t_i }$$ === Análisis suponiendo ti = 0 === Polos de la función de transferencia a lazo cerrado. $$$ \[[s=-\frac{\sqrt{{kd}^{2}\,{kt}^{2}-4\,j\,k\,kt}+kd\,kt}{2\,j},s=\frac{\sqrt{{kd}^{2}\,{kt}^{2}-4\,j\,k\,kt}-kd\,kt}{2\,j}]\] $$ Vemos que los polos arrancan sobre el eje sigma negativo y a medida que aumentamos k se acercan entre ellos hasta que se juntan en un punto de ruptura en el eje sigma negativo y empiezan a ser complejos conjugados. El punto de ruptura depende del valor de kd, a medida que este aumenta este punto de ruptura se mueve hacia la izquierda.