Modelado del Quadrotor

La derivación de la dinámica no lineal es realizada en coordenadas inerciales NED y de cuerpo fijo. Denotemos con {

$$$ e_N$$

$$$e_E$$

$$$e_D$$

} los ejes inerciales, y con {

$$$x_B$$

$$$y_B$$

$$$z_B$$

} a los ejes del cuerpo. Los ángulos de Euler de los ejes del cuerpo son {

$$\phi$

$$\theta$

$$\psi$

} con respecto a los ejes

$$$e_N$$

$$$e_E$$

$$$e_D$$

respectivamente, y son referidos como roll, pitch and yaw. Definamos a r como el vector de posición desde el origen inercial hacía el centro de gravedad del vehículo(CG), y dejemos que

$$\omega_B$

sea definido como la velocidad angular en con respecto al eje de referencia del cuerpo.

La dirección actual de la velocidad es referida como

$$$e_v$$

en coordenadas inerciales. Los rotores, numerados 1-4, están montados sobre los ejes

$$$x_B$$

$$$y_B$$

$$$-x_B$$

$$$-y_B$$

, respectivamente, con vectores de posición

$$$r_i$$

con respecto a CG. Cada rotor produce un

Fuerza y Momento

$$ \mathbf{F} = -D_B\cdot\vec{e_V} + mg\cdot\vec{e_D} + \sum\limits^{4}_{i=1}(-T_i\cdot\vec{z_B}-D_i\cdot\vec{e_V})$

$$ \mathbf{M} = \sum\limits_{i=1}^4[Q_i\cdot\vec{z_B} - R_i\cdot\vec{e_V} - D_i(\vec{r_i}\times\vec{e_V}) + T_i(\vec{r_i}\times\vec{z_B})]$

$$ \mathbf{F} = m\ddot{r}$

$$ \mathbf{M} = I \dot{\omega}_B + \omega_B \times I \omega_B$

Descripción

Fuerza de Arrastre del Cuerpo

$$ D_B = q_{\infty}SC_D$

Empuje

$$ T_i \cong \mu_i\frac{K_\tau}{1+0.1s}$

Fuerza de arrastre sobre los rotores debido a la velocidad horizontal

$$$ D_i $$

Momento de arrastre sobre el eje de rotación de los rotores

$$ Q_i = K_{\tau}T_i$

Momento de Roll generado en los rotores por la velocidad

$$$ R_i $$

Fuerza de arrastre en los rotores debido a la velocidad

$$$ D_i $$

Empuje Total

$$ T = \sum\limits_{i=1}^4{T_i}$

Aproximación de la Fuerza y el Momento

Suponiendo que el Cuadricóptero está en vuelo estacionario podemos despreciar

$$$ D_B $$

$$$ D_i $$

. Con esto nos queda:

Fuerza

$$ m\ddot{r} = \mathbf{F} = -R_{\psi}R_{\theta}R_{\phi}T\cdot\vec{z_B} + mg\cdot\vec{e_D}$

Aproximando las matrices de rotación de los ángulos para ángulos pequeños, tenemos:

$$ -R_{\psi}R_{\theta}R_{\phi} = \left[\matrix{ 1 & \psi & \theta \cr \psi & 1 & \phi \cr \theta & -\phi & 1 }\right]$

$$ m\ddot{r} = \left[\matrix{ 1 & \psi & \theta \cr \psi & 1 & \phi \cr \theta & -\phi & 1 }\right]\left[\matrix{ 0 \cr 0 \cr -T }\right] + \left[\matrix{ 0 \cr 0 \cr mg }\right]$

$$ m\ddot{r} = \left[\matrix{ 0 & -\bar{T} & 0 \cr \bar{T} & 0 & 0 \cr 0 & 0 & 1 }\right]\left[\matrix{ \phi \cr \theta \cr T }\right] + \left[\matrix{ 0 \cr 0 \cr mg }\right]$

Momento

También podemos despreciar los momemtos de Rolling

$$$ R_i $$

$$ \omega_B\times I\omega_B$

. El Torque nos queda:

$$ \mathbf{M} = I\dot{\omega}_B = \sum\limits_{i=1}^4[Q_i\cdot\vec{z_B} + T_i(\vec{r_i}\times\vec{z_B})]$

$$ \left[\matrix{ I_x\ddot{\phi} \cr I_y\ddot{\theta} \cr I_z\ddot{\psi} }\right] = \left[\matrix{ 0 & l & 0 & -l \cr l & 0 & -l & 0 \cr K_r & -K_r & K_r & -K_r }\right]\left[\matrix{ T_1 \cr T_2 \cr T_3 \cr T_4 }\right]$

Referencias

[[ http://www.eecs.berkeley.edu/~tomlin/papers/conferences/whjt05_iros.pdf | Multi-Agent Quadrotor Testbed Control Design: Integral Sliding Mode vs. Reinforcement Learning ]]

Path Tracking Control for Quadrotor Helicopters

Medición de los parámetros de la Planta

Para el diseño del controlador del quadricóptero resulta necesario realizar la medición de los distintos parámetros del modelo real. Los parámetros fundamentales son el peso, el momento de inercia, los empujes de los motores.

Empuje de los motores

Al momento de realizar esta medición poseíamos 2 motores rctimer 2830, 1 ESC rctimer de 30A, 1 ESC mystery de 30A, 1 hélice con paso normal y una hélice con paso pusher.

Se realizó la medición del empuje de cada motor, con las distintas hélices y los dos controladores. Esta medición se hizo montando el motor sobre un peso de plomo, el cual se ponía sobre la balanza y luego se realizaron las curvas de transferencia entre PWM y fuerza de empuje.

Motor 1

RCTimer Normal

Duty	2949	5898	8847	11796	14745	17694	20643	23592	26541	29491	32440	35389	38338	41287
Fuerza	0	55	100	145	185	230	270	320	360	400	440	490	540	600

$$$F = 0.0154 * D - 35.83$$

Mystery Pusher

Duty	2949	5898	8847	11796	14745	17694	20643	23592	26541	29491	32440	35389	38338	41287
Fuerza	20	20	95	165	245	325	410	480	575	670	770	855	855	845

$$$F = 0.0283 * D - 155$$

RCTimer Pusher

Duty	2949	5898	8847	11796	14745	17694	20643	23592	26541	29491	32440	35389	38338	41287
Fuerza	5	60	105	150	195	240	290	335	380	425	475	540	605	665

$$$F = 0.017 * D - 45.4$$

Mystery Normal

Duty	2949	5898	8847	11796	14745	17694	20643	23592	26541	29491	32440	35389	38338	41287
Fuerza	5	5	80	150	225	300	380	460	545	640	730	790	760	760

$$$F = 0.0267 * D - 156$$

Motor1

Motor 2

Mystery Normal

Duty	2949	5898	8847	11796	14745	17694	20643	23592	26541	29491	32440	35389	38338
Fuerza	15	15	80	160	235	320	395	475	570	665	765	840	840

$$$F = 0.028 * D - 167.7$$

RCTimer Pusher

Duty	2949	5898	8847	11796	14745	17694	20643	23592	26541	29491	32440	35389	38338	41287
Fuerza	10	65	110	155	200	245	295	345	395	435	490	560	605	660

$$$F = 0.0168 * D - 34.1$$

Mystery Pusher

Duty	2949	5898	8847	11796	14745	17694	20643	23592	26541	29491	32440	35389	38338
Fuerza	5	5	80	155	245	335	425	535	655	795	905	1015	1015

$$$F = 0.03523 * D - 231.7$$

RCTimer Normal

Duty	2949	5898	8847	11796	14745	17694	20643	23592	26541	29491	32440	35389	38338	41287
Fuerza	25	65	110	155	195	235	275	315	360	400	435	485	540	595

$$$F = 0.015 * D - 23.47$$

Motor2

Motor1, RCTimer, Pusher y Motor2, Mystery y Normal

Motor1vsMotor2

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}$

Diseño del controlador

El problema del diseño de un controlador de actitud para el QA3 no es un problema trivial debido a varias razones como son el acoplamiento entre las fuerzas y momentos generados por los motores y hélices, el rozamiento con el aire, la fuerza de empuje del viento, etc; por esto se ha optado por el uso de un controlador PID, el cual se puede ajustar para las mejores condiciones de funcionamiento y de la realización de varias simplificaciones.

La primera simplificación importante es suponer que el control de los distintos ángulos esta desacoplado. Con esta suposición obtenemos 3 plantas independientes a las cuales les agregamos un controlador PID distinto.

Otra simplificación es suponer que las fuerzas de rozamiento con el aire y la fuerza generada por corrientes de aire son despreciables. Estas corrientes de aire no solo se pueden deber al viento, sino también a rebotes del flujo de aire generado por la hélice con el entorno.

En esta sección se explica el controlador PID y los 3 controladores de los ángulos.

Controlador PID

Un controlador PID (Proporcional Integral Derivativo) es un mecanismo de control por realimentación que calcula la desviación o error entre un valor medido y el valor que se quiere obtener, para aplicar una acción correctora que ajuste el proceso.

El algoritmo de cálculo del control PID tiene tres parámetros distintos: el proporcional, el integral, y el derivativo. El valor Proporcional determina la reacción del error actual. El Integral genera una corrección proporcional a la integral del error, esto nos asegura que aplicando un esfuerzo de control suficiente, el error de seguimiento se reduce a cero. El Derivativo determina la reacción del tiempo en el que el error se produce. La suma de estas tres acciones es usada para ajustar al proceso vía un elemento de control, en este caso el PWM aplicado a los motores. Ajustando estas tres variables en el algoritmo de control del PID, el controlador puede proveer un control diseñado para lo que requiera el proceso a realizar.

La respuesta del controlador puede ser descrita en términos de respuesta del control ante un error, el grado el cual el controlador llega al "set point", y el grado de oscilación del sistema. Nótese que el uso del PID para control no garantiza control óptimo del sistema o la estabilidad del mismo. Algunas aplicaciones pueden solo requerir de uno o dos modos de los que provee este sistema de control.

Ecuacion Diferencial

$$ c(t) = k[e(t) + \frac{1}{T_i}\int_0^t \! e(t) dt \ + T_d \frac{de(t)}{dt} ]$

Transformada de Laplace

$$ C(s) = k( 1 + \frac{1}{T_i s} + T_d s ) E(s)$

Transformada Z

$$ C(z) = k[1-\frac{T}{2T_i} + \frac{T}{T_i}\frac{1}{1-z^{-1}} + \frac{T_d}{T}(1-z^{-1}) ]E(z)$

$$ C(z) = [K_p + \frac{K_i}{1-z^{-1}} + K_d(1-z^{-1}) ]E(z)$

Tiempo Discreto

$$$ c(k) = c(k-1) + K_p[e(k)-e(k-1)] + K_i e(k) + K_d[e(k)-2e(k-1)+e(k-2)] $$

Controlador del ángulo de roll

El ángulo de roll depende de la diferencia del empuje entre los motores laterales. A continuación se realiza el cálculo de la función de transferencia a lazo cerrado de la planta más el controlador PID. Suponemos primero un modelo continuo y luego un modelo discreto con un retenedor de orden cero, para tener en cuenta el retardo de fase introducido por el muestreo.

Modelo Continuo

$$ \sum{\tau_x} = \tau_2 - \tau_1 = J\frac{d^2\theta}{dt^2}$

$$ s^2\theta_{(s)}= \frac{\tau_{(s)}}{J}$

$$ G_{bal(s)} = \frac{1}{Js^2}$

$$ G_{torque(s)} = k_\tau$

$$ G_{PID(s)} = k_p\cdot(1 + \frac{1}{T_is} + T_ds})$

$$ G_{LA(s)} = G_{bal(s)}G_{torque(s)}G_{PID(s)} = \frac{k_pk_{\tau}}{T_iJ}\frac{T_iT_ds^2 + T_is + 1}{s^3}$

$$ G_{LC(s)} = \frac{G_{LA(s)}}{1+G_{LA(s)}} = k_p k_{\tau} \frac{T_i T_d s^2 + T_i s + 1}{T_i J s^3 + k_p k_{\tau} T_i T_d s^2 + k_p k_{\tau} T_i s + k_p k_{\tau}}$

Modelo Discreto

$$ G_{bal(s)} = \frac{1}{Js^2}$

$$ G_{ROC(s)}=\frac{1-e^{-TS}}{S}$

$$ G_{torque(s)} = k_\tau$

$$ G_{PID(s)} = k_p\cdot(1 + \frac{1}{T_is} + T_ds})$

$$ G_{planta(Z)} = Z[G_{ROC(s)}G_{bal(s)}G_{torque(s)}] = Z[\frac{1 - e^{-TS}}{S}\frac{k_\tau}{JS^2}] = (1-z^-1)Z[\frac{2k_\tau}{2JS^3}] = \frac{T^2k_\tau}{2J}\frac{z+1}{z^2-2z+1}$

$$ G_{PID(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 }$

$$ G_{LA(Z)} = G_{PID(Z)}G_{planta(Z)} = \frac{T^2k_\tau}{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}$

$$ G_{LC(Z)} = \frac{G_{LA(Z)}}{1+G_{LA(Z)}} = \frac{k_{\tau}T^2((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)k_{\tau}T^2 -6J )z^3 + ((K_I - K_D)k_{\tau}T^2 +6J )z^2 + ( ( -K_P - K_D ) k_{\tau} T^2 -2J )z + K_D k_{\tau} T^2 }$

Controlador del ángulo de pitch

El ángulo de pitch depende de la diferencia del empuje entre los motores delantero y trasero. El controlador de este ángulo es igual al del controlador del roll, por esto no nos extendemos en este controlador.

Simulaciones

Para las simulaciones se van a utilizar las ecuaciones obtenidas anteriormente de la planta más el controlador. Estas fueron realizadas con el software octave.

Los valores de los parámetros usados fueron extraídos del modelo real y son los siguientes.

$$$ kt = 90.63e-6; $$

$$$ J = 8.6556e-3; $$

Simulación a lazo abierto

Se muestran los resultados de la simulación de la planta más el controlador diseñado en lazo abierto. Con estos datos podemos saber como será la respuesta del sistema según el valor de k elegido. Como se puede observar para valores de k muy grandes el sistema se vuelve inestable.

pid_la_discreto

Simulación a lazo cerrado

En esta simulación se muestran los resultados de la simulación de la salida en función del tiempo para una entrada escalón.

pid_la_discreto

Buscar

Herramientas

Modelado del Quadrotor

Fuerza y Momento

Descripción

Fuerza de Arrastre del Cuerpo

Empuje

Fuerza de arrastre sobre los rotores debido a la velocidad horizontal

Momento de arrastre sobre el eje de rotación de los rotores

Momento de Roll generado en los rotores por la velocidad

Fuerza de arrastre en los rotores debido a la velocidad

Empuje Total

Aproximación de la Fuerza y el Momento

Fuerza

Momento

Referencias

Medición de los parámetros de la Planta

Empuje de los motores

Motor 1

RCTimer Normal

Mystery Pusher

RCTimer Pusher

Mystery Normal

Motor 2

Mystery Normal

RCTimer Pusher

Mystery Pusher

RCTimer Normal

Motor1, RCTimer, Pusher y Motor2, Mystery y Normal

Cálculo Aproximado de las constantes

Cálculo de la constante de Inercia

Cálculo de la constante del Motor

Diseño del controlador

Controlador PID

Ecuacion Diferencial

Transformada de Laplace

Transformada Z

Tiempo Discreto

Controlador del ángulo de roll

Modelo Continuo

Modelo Discreto

Controlador del ángulo de pitch

Simulaciones

Simulación a lazo abierto

Simulación a lazo cerrado