Bienvenido: Ingresar

Por favor indica el password de tu cuenta en el wiki remoto abajo.
/!\ Deberías confiar en ambos wikis porque los administradoresrespectivos podrían ver tu contraseña.

Quitar mensaje
location: LabElectronica / ProyectoQuadricoptero / InformeFinalCap3

Modelado del Quadrotor

En este capítulo se realiza la derivación del modelo matemético de la planta del QA3, también se realizan las simplificaciones necesarias para poder obtener un controlador simple, pero con una buena performance, luego se explican las mediciones realizadas para obtener las constantes reales de la planta y aplicarlas al modelo. Por último se diseña el controlador.

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

y

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

$$$-y_B$$

, respectivamente, con vectores de posición

$$$r_i$$

con respecto a CG. Cada rotor produce un torque aerodinámico

$$$Q_i$$

y un empuje

$$$T_i$$

, ambos paralelos al eje de rotación del rotor, y ambos usados para el control del vehículo.

Aquí,

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

, donde

$$$u_i$$

es el voltage aplicado a los motores. En vuelo,

$$$T_i$$

puede variar mucho con respecto a esta aproximación. Los torques,

$$$Q_i$$

, son proporcionales al empuje del rotor y están dados por

$$$Q_i=k_rT_i$$

.

Los rotores 1 y 3 giran en dirección opuesta a los rotores 2 y 4, por esto los torques aerodinámicos se contrarrestran y pueden ser usados independientemente para el control de yaw. La velocidad horizontal resulta en un momento en los rotores,

$$$R_i$$

, sobre

$$$-e_v$$

, y una fuerza de arrastre,

$$$D_i$$

, en la dirección

$$$-e_v$$

. La fuerza de arrastre de cuerpo está definida como

$$$D_B$$

, la masa del vehículo es

$$$m$$

, la aceleración debido a la gravedad es

$$$g$$

, y la matriz de inercia es

$$$I \in {R}^{3X3}$$

. Un diagrama de cuerpo libre se muestra en la figura siguiente.

diagrama_cuerpo_libre.png

La fuerza total,

$$$F$$

, y el momento,

$$$M$$

, puede ser sumado como

$$$ \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})]$$

La dinámica no lineal completa puede ser expresada como:

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

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

donde se asume que el momento angular total de los rotores está cerca de cero, porque están en contra rotación.

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

y

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

y

$$$ \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. El montaje se muestra en la siguiente figura.

med_empuje_motor.png

En las siguientes tablas se muestran los resultados obtenidos, RCTimer y Mystery son los 2 controladores usados, Normal y Pusher son los 2 tipos de hélices. También se muestran los datos en forma gráfica.

Como se puede ver, podemos aproximar la función de transferencia entre el ciclo de trabajo y el empuje generado por una recta. Esta es una aproximación no tan real debido a que se desprecian todas las constantes de tiempo del motor y de la hélice. Obtener una función de transferencia real para el conjunto motor hélice representa un trabajo que sobrepasa nuestros conocimientos y objetivos, por lo tanto en adelante se supondrá que la transferencia entre ciclo de trabajo y empuje es una constante.

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

Revisión del Cálculo de las constantes

Esta revisión se hace para adaptarla al nuevo modelo del QA3, que tiene 1 metro de largo en las barras y usa la imu 3DM-GX1.

Partes: las partes que vamos a usar para calcular el momento de inercia son el motor, las baterías, las barras, la IMU y la placa del ARM.

Inercias con respecto al eje X o Y

Motor

Para calcularlo suponemos que el motor es un cilindro

$$$ m_{motor} = 0.052kg $$

$$$ r_{motor} = 0.013850m $$

$$$ I_{cilindro} = \frac{mr^{2}}{2} = \frac{0.052 kg (0.013850 m)^2}{2} = 4.9874e^{-6} kg m^{2} $$

donde m es la masa total del motor y r es el radio del motor.

Usando el teorema de Steiner nos queda:

$$$ I_{motor} = I_{cilindro} + md^{2} = 4.9874e^{-6} kg.m^{2} + 0.052 kg(0.5 m)^{2} = 0.013005 kg.m^{2} $$

donde d es la distancia del motor al centro de gravedad, en este caso la mitad del largo de la barra.

$$$ d = 0.5m $$

Batería

Para calcularlo suponemos que la batería es un paralelepípedo rectangular.

$$$ I_{par} = \frac{1}{3}m(b^2+c^2) = \frac{0.167kg \cdot ((0.03m)^2 + (0.02m)^2)}{3} = 2.1710^e{-4} kg.m^{2} $$

En donde b y c son los lados del paralelepípedo perpendiculares al eje con respecto al que calculamos el momento de inercia.

En nuestro caso

$$$ b = 0.03m, c = 0.02m $$

y

$$$ m = 0.167kg $$

Otra vez por el teorema de Steiner nos queda que:

$$$ I_{bateria} = I_{par} + md^{2} = 2.1710^e{-4} kg.m^{2} + 0.167 kg(0.075 m)^{2} = 0.0011565 kg.m^{2} $$

en donde d representa la distancia del CG a la batería,

$$$ d = 0.075m $$

Barra

Supondremos que la barra es una línea perpendicular al eje sobre el que calculamos el momento de inercia. La masa de las barra es

$$$ m = 0.298kg $$

y

$$$ L = 1m$$

es el largo total de la barra.

$$$ I_{barra} = \frac{mL^2}{12} = \frac{0.298kg(1m)^2}{2} = 0.14900 kg.m^{2}$$

El peso de la barra sin fresar es de

$$$ m = 0.515kg $$

. El peso de las 2 barras fresadas es de

$$$ 581g $$

.

IMU, ARM

Supondremos que la placa central es un cubo de lado

$$$ l = 0.06m $$

y masa

$$$ m = 0.246kg $$

.

$$$ I_{cubo} = \frac{ml^2}{6} = \frac{0.246kg(0.06m)^2}{6} = 1.4760e^{-4} kg.m^{2} $$

Momento total de inercia para el eje x o y

$$$ I_x = 2I_{motor} + 2I_{bateria} + I_{barra} + I_{cubo}$$

$$$ I_x = 2 \cdot 0.013005 kg.m^{2} + 2 \cdot 0.0011565 kg.m^{2} + 0.14900 kg.m^{2} + 1.4760e^{-4} kg.m^{2} = 0.17747 kg.m^{2} $$

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.

También despreciamos la dinámica del motor y suponemos que es lineal.

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.

PID.png

Ecuacion Diferencial

La ecuación diferencial de un controlador PID tiene la siguiente forma:

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

Transformada de Laplace

Aplicando la transformada de Laplace a la ecuanción diferencial anterior podemos encontrar la relación entre entrada y salida en el plano de Laplace:

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

Transformada Z

Para realizar el análisis en tiempo discreto necesitamos pasar del plano de Laplace al plano Z. Esto se hace aplicando la transformada Z a la función obtenida anteriormente. Una vez obtenida la función en el plano Z, podemos realizar análisis de estabilidad, de respuesta y realizar el controlador para obtener la respuesta deseada.

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

a

Podemos definir 3 constantes,

$$$K_p$$

,

$$$K_i$$

,

$$$K_d$$

, y expresar la ecuación anterior como:

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

Está última forma tiene una similitud con el controlador PID en tiempo continuo, pero aclaramos que las constantes no son las mismas.

Tiempo Discreto

Para poder codificar el algoritmo en un microcontrolador, debemos pasar la función de transferencia en el plano Z, a una ecuación en diferencias en el tiempo. En la ecuación siguiente se muestra el algoritmo del controlador PID en el tiempo.

$$$ 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)] $$

A continuación se realiza el análisis de los controladores de los ángulos de roll y pitch.

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

planta_lc_continuo.png

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

planta_lc_discreto.png

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

$$$ k_t = 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