Control de estabilidad y PID para drones

Índice:
- Conceptos generales sobre drones.
- Material necesario y montaje de los componentes hardware.
- Mando RC y receptor. Programación en Arduino (código).
- MPU6050 y su programación en Arduino (código).
- Batería LiPo (código).
- → Control de estabilidad y PID.
- Motores, ESC y su programación en Arduino (código).
- Calibración de hélices y motores (código).
- Software completo y esquema detallado (código).
- Probando el Software completo antes de volar.
- Como leer variables de Arduino en Matlab (código).
- Los mejores drones de 2018 | Comparativa y guía de compra.
Estrategias de control de vuelo para drones
Las estrategias de control más comunes utilizadas en el mundo de los drones son el modo acrobático y el modo estable. En esta entrada vamos a entender el funcionamiento de las dos estrategias.
Modo de control ‘Acrobático’
En este primer modo de control solo utilizaremos las lecturas de velocidad angular calculadas a partir de los datos obtenidos del sensor MPU6050. La velocidad la vamos a medir en ‘º/s’, es decir, cuantos grados rota cada eje por segundo. Si por ejemplo uno de nuestros ejes da una vuelta completa en un segundo, la velocidad será de 360º/s.
Vamos a empezar repasando unos conceptos básicos de funcionamiento. Si el eje pitch de nuestro drone rotara por cualquier razón, porque uno de los motores tiene más potencia, porque hay viento… el control tendrá que contrarrestar esta desviación actuando sobre los motores correspondientes. En este caso, habría que actuar sobre los motores de la izquierda acelerándolos, y sobre los motores de la derecha decelerándolos. De esta forma conseguiríamos contrarrestar el efecto de la perturbación que ha hecho que nuestro drone rotara en su eje pitch. Pero cuidado, en modo acrobático en drone no volverá a su posición inicial, simplemente compensaremos la rotación hasta detener el drone. Esto es debido a que únicamente estamos utilizando como referencia la velocidad de rotación de los ejes:

Ahora bien, ¿cuánta potencia y durante cuánto tiempo hay que acelerar cada motor para contrarrestar estas perturbaciones? En otras palabras, ¿Cómo hacemos que nuestro control sea capaz de mantener el drone estático en el aire de forma autónoma? Para esto tenemos los PID.
Empecemos por lo básico. Si representáramos el algoritmo de control de estabilidad mediante una conversación entre los diferentes componentes del drone, obtendríamos algo como esto:
- Humano (mediante el mando RC): Ey drone, quiero que te mantengas estático en el aire, las velocidades angulares de tus tres ejes a 0º/s (sin ninguna rotación en ningún sentido).
- Drone (Arduino): recibido humano. MPU6050, necesito que me digas a qué velocidad nos movemos en los tres ángulos.
- Drone (MPU6050): Según mis mediciones rotamos a 0º/s de pitch, 0º/s de yaw y -5º/s de roll.
- Drone (Arduino): Recibido MPU6050, parece que tenemos una desviación de 5º/s en el roll respecto a lo que nuestro humano nos ha pedido. Atentos motores, tenemos que corregir la desviación en la velocidad del eje roll. Acelera el motor 3 y frena el 1 → PIDS
- Drone (Motores): recibido Arduino. Aplicando cambios en la consigna a los motores.
- Y vuelta al punto 1)
Dicho de una manera más ‘técnica’ la secuencia quedaría de la siguiente manera. Como hemos visto en la entrada dedicada a la lectura del mando RC, vamos a utilizar interrupciones hardware para la función de lectura RC (si no has leído esa entrada te recomiendo que antes te la leas). El loop se ejecutará periódicamente y cada vez que haya una interrupción hardware se leerán los ángulos que transmite el mando RC.
Secuencia: Esta secuencia se ejecuta cada 6ms
- Arduino pide lecturas al sensor MPU6050 para calcular las velocidades de rotación en los ejes roll, pitch y yaw.
- El sensor MPU6050 responde con sus lecturas y se realizan los cálculos de velocidad angular en º/s.
- Con estas lecturas y las consignas que recibimos desde el mando, los PID calculan el error y cuanto acelerar o decelerar cada motor para compensarlo.
- Vuelta al paso 1)
**Si hay interrupción hardware: Leer mando RC. Cada 20ms aprox. recibimos una lectura.
A este proceso se le llama ‘Lazo de control’. Es muy importante tener en mente que este lazo de control se ejecutará una y otra vez cada 6ms.

Como vemos, los encargados de calcular el error de cada ángulo y actuar en consecuencia son los PID. Los PID son una parte fundamental de nuestro drone por lo que es necesario saber algo de teoría sobre ellos. Hay infinidad de información en todos los idiomas sobre sistemas de control basados en PIDs y sus aplicaciones en drones, por lo que no me voy a extender mucho en este tema. Recomiendo acudir a Internet y dedicar algo de tiempo a entender bien su funcionamiento. Arduino cuenta con una librería para PID, pero recomiendo no utilizarla e intentar entender el funcionamiento de estos controladores programándolos vosotros mismo (son muy sencillos). El objetivo de un PID es conseguir un error entre la consigna de velocidad y la velocidad real de 0º/s (metros, grados… según la aplicación), es decir, que la velocidad de rotación real sea igual a la consigna que llega desde el mando en todo momento. Pongamos como ejemplo uno de los ejes de nuestro drone, por ejemplo el eje Pitch:
Lo primero que hace esta estructura de control es comparar la referencia de velocidad angular que nos llega desde el mando, en la imagen ‘W Pitch* (mando)’, con la lectura que recibimos del sensor MPU6050, en la imagen ‘W Pitch (IMU)’. Haciendo la resta de estas dos señales conseguimos en valor de desviación o error en nuestro eje pitch, en la imagen, señal Err.1:
Por ejemplo, si desde el mando nos llega la consigna de 0º/s de pitch y el sensor MPU6050 está leyendo que la velocidad real es de +10º/s, la variable ‘Err.1’ tomará el valor -10º/s, es decir, tenemos un error en la velocidad del eje pitch de -10º/s. Este podría ser un ejemplo de que debido al viento, nuestro drone se está inclinando en una dirección, cuando lo que queremos es que se mantenga estable. El objetivo de nuestro PID será hacer que este error sea siempre de 0, para lo que habrá que actuar sobre los motores, contrarrestando el viento y haciendo que la lectura del sensor MPU6050 sea de 0º/s (que no gire). El error antes de contrarrestar la perturbación:
Err.1 = w Pitch* (mando) – w Pitch (IMU) = 0 rad/s – 10rad/s = -10rad/s
El error es enviado al PID y este genera la consigna en micro-segundos acelerando/decelerando los correspondientes motores hasta contrarrestar la perturbación. Cuando el drone comience a girar y se vaya corrigiendo la desviación, la variable Err.1 irá disminuyendo hasta convertirse en 0, momento en el que drone habrá detenido la rotación y no haya desviación alguna entre la consigna que enviamos desde el mando y la rotación real.Una vez corregida la desviación y que el drone a dejado de rotar, el error baja a 0º/s, CONSEGUIDO, el drone es estable:
Err.1 = w Pitch* (mando) – w Pitch (IMU) = 0 rad/s – 0 rad/s = 0 rad/s
Si por el contrario desde el mando nos llega una referencia de 10º/s, es decir, queremos que nuestro drone se incline y desplace en su eje pitch, el funcionamiento sería similar. El error pasaría a ser de 10º/s y el PID aceleraría los correspondientes motores hasta aumentar la velocidad a 10º/s, es decir, hasta hacer el error cero.
Err.1 = w Pitch* (mando) – w Pitch (IMU) = 10 rad/s – 0 rad/s = + 10rad/s
Tras acelerar los motores, el drone empezará a rotar en el sentido que hayamos indicado hasta alcanzar la velocidad deseada, momento en el que el error bajará a 0º/s y habrá finalizado la operación:
Err.1 = w Pitch* (mando) – w Pitch (IMU) = 10 rad/s – 10 rad/s = 0 rad/s
Este error ‘Err.1’ lo recibe el PID y genera una salida ‘Pulso (us)’ en función de los parámetros Kp, Ki y Kd que hayamos establecido. Simplemente cogemos el error y lo multiplicamos por estos valores, haciendo más o menos agresivo el control de estabilidad de nuestro drone.
La parte Kp (PID) es proporcional al error, simplemente multiplicamos ambos términos. Si por ejemplo tenemos un valor de Kp de 10 y tenemos un error de 10º:
10*10º = 100us
Si el motor estaba girando con un PWM de 1.5ms, aceleraría hasta 1.5ms+0.1ms = 1.6ms.
La parte Ki (PID) es proporcional al error que vamos acumulando en cada ciclo. Cogemos el error actual y lo multiplicamos por el término Ki, pero en cada nuevo ciclo de control sumamos el valor obtenido en el ciclo anterior. De esta forma conseguimos que el error en régimen permanente sea de 0.
La parte Kd (PID) es proporcional a la diferencia de error entre ciclos. Sirve para suavizar la respuesta del control.Mas adelante en esta entrada veremos como programar los PID en Arduino.
La salida de los PID se da en milisegundos. Si recordáis esta entrada donde hablé sobre los motores (si no, id a releedla), hablábamos de cómo variando el tiempo en el que el PWM está en estado HIGH podíamos aumentar o reducir la velocidad de los motores. Por esta razón, la salida de los PID se da en milisegundos, porque para corregir las desviaciones o el error, necesitamos variar el tiempo en el que el PWM está en estado HIGH. Como ya habréis imaginado, necesitamos un PID para cada eje del drone que queramos controlar, en nuestro caso 3: Pitch, Roll y Yaw.
Finalmente todas las señales se combinan para generar una señal para cada motor. Al cálculo de estas cuatro señales he decidido llamarlo MODULADOR. ¡Recordad que estas cuatro señales se miden en milisegundos!
esc1 = throttle – salida PID pitch + salida PID roll + salida PID yaw
esc2 = throttle – salida PID pitch – salida PID roll – salida PID yaw
esc3 = throttle + salida PID pitch – salida PID roll + salida PID yaw
esc4 = throttle + salida PID pitch + salida PID roll – salida PID yaw
Importante, las señales esc1, esc2, esc3 y esc 4 nunca deben superar los 2ms, que es la consigna de máxima potencia para los motores. Si alguna de las señales supera esta cifra significa que algo hemos hecho mal, ¡no podemos enviar al motor una consigna mayor a 2ms! Como veremos más adelante, vamos a capar por software estas señales para que nunca sean mayores de 2ms, aunque esta protección nunca debería entrar en funcionamiento si hemos programado bien el software. Para ello recordad limitar la señal máxima de throttle a un máximo de 1.7ms aproximadamente, para dejar margen para los PID. Los PID también tendrán que ser limitados a un valor aproximado de 300us.
El signo de cada término de la ecuación puede variar en función de cómo tengáis orientada la IMU en vuestros drone o de la disposición de los motores. Es importante asegurar que los signos están bien puestos para evitar accidentes catastróficos el primer día de vuelo. Para ello, es imprescindible montar nuestro MPU6050 en el sentido que indico en esta entrada. Una vez hecho esto, recomiendo situar el drone en el suelo e ir inclinándolo manualmente en sus tres ejes mientras visualizamos las señales esc1, esc2, esc3 y esc4 para comprobar que todo esté en orden.
La estrategia de control completa representada en bloques quedaría de la siguiente forma. Únicamente programando esto nuestro drone podría volar sin problemas.
Pasemos a entender cómo programamos todo esto en Arduino. Partiremos asumiendo que ya hemos leído y procesado los datos del mando RC y del sensor MPU6050 en Arduino. Primero vamos a analizar todas las partes por separado, y finalmente pondré el código completo.
En primer lugar hay que hacer que el lazo de control se ejecute de forma constante exactamente cada 6ms (en mi caso). Hacer esto en Arduino es extremadamente simple:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 | float tiempo_ejecucion, loop_timer; void setup() { Serial.begin(115200); } void loop() { // // PROGRAMA PRINCIPAL AQUÍ // while (micros() - loop_timer < 6000); tiempo_ejecucion = (micros() - loop_timer) / 1000; loop_timer = micros(); // // O AQUÍ // Serial.println(tiempo_ejecucion); } |
Como veis, es muy fácil de entender, simplemente hacemos los cálculos pertinentes y esperamos sin hacer nada hasta que pasen 6ms. Más adelante veremos cómo aprovechar estos tiempos muertos donde Arduino no hace nada (solo espera) para ejecutar pequeñas acciones que no necesitan de mucha carga de procesamiento, como puede ser medir la tensión de la batería o gestionar algún led. De esta forma nos aseguramos de que aunque el tiempo de ejecución del lazo de control varíe por lo que sea (mas adelante en este apartado veremos porque), siempre se ejecutará una vez exactamente cada 6ms. El resto del tiempo estará esperando a llegar al tiempo fijado.
** El tiempo de ejecución no tiene por qué ser de 6ms, pueden ser de 4ms, 10ms o el valor que sea, vosotros elegís el valor que queráis utilizar, a mi con 6ms me va bien. Evidentemente cuanto más rápido sea el control, mayor estabilidad tendrá nuestro drone.
El siguiente paso es programar los PID. Como ya he dicho, sobre este tema se han escrito ríos de tinta, por lo que no voy a entrar a explicarlo una vez más. Como veis, es bastante simple de entender, simplemente calculamos en error como hemos hecho más arriba, y lo multiplicamos por los valores de Kp, Ki y Kd. Partiendo de que ya dominamos el tema del sensor MPU6050 y la lectura de las variables recibidas desde el mando, vamos a ver como se programan los famosos PID utilizando Arduino. Este código no es ejecutable como tal, es simplemente para que veías como se programa y entendíais el código. Intentad entender en código y la función de los tres parámetros del PID:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 | //===================================================== PID PITCH w pitch_w_error_prop = PIDwInPitch - gyro_pitch_input; // Calculamos el error ITerm_pitch_w += (Ki_pitch_w * pitch_w_error_prop); ITerm_pitch_w = constrain(ITerm_pitch_w, salidaPI_pitch_w_min1, salidaPI_pitch_w_max1); DPitch_w = Kd_pitch_w * (gyro_pitch_input - pitch_w_giroscopio_anterior); PID_pitch_w = Kp_pitch_w * pitch_w_error_prop + ITerm_pitch_w - DPitch_w; // salida PID PID_pitch_w = constrain(PID_pitch_w, salidaPI_pitch_w_min2, salidaPI_pitch_w_max2); pitch_w_giroscopio_anterior = gyro_pitch_input; //=============================================== PID ROLL w roll_w_error_prop = PIDwInRoll - gyro_roll_input; ITerm_roll_w += (Ki_roll_w * roll_w_error_prop); ITerm_roll_w = constrain(ITerm_roll_w, salidaPI_roll_w_min1, salidaPI_roll_w_max1); DRoll_w = Kd_roll_w * (gyro_roll_input - roll_w_giroscopio_anterior); PID_roll_w = Kp_roll_w * roll_w_error_prop + ITerm_roll_w - DRoll_w; // salida PID PID_roll_w = constrain(PID_roll_w, salidaPI_roll_w_min2, salidaPI_roll_w_max2); roll_w_giroscopio_anterior = gyro_roll_input; //================================================= PID YAW w yaw_w_error_prop = wYawConsigna - gyro_yaw_input; ITerm_yaw_w += (Ki_yaw_w * yaw_w_error_prop); ITerm_yaw_w = constrain(ITerm_yaw_w, salidaPI_yaw_w_min1, salidaPI_yaw_w_max1); DYaw_w = Kd_yaw_w * (gyro_yaw_input - yaw_w_giroscopio_anterior); PID_yaw_w = Kp_yaw_w * yaw_w_error_prop + ITerm_yaw_w - DYaw_w; // salida PID PID_yaw_w = constrain(PID_yaw_w, salidaPI_yaw_w_min2, salidaPI_yaw_w_max2); yaw_w_giroscopio_anterior = gyro_yaw_input; |
El siguiente paso es generar las señales PWM para los motores en función las salidas de los controladores PID (recordad que los motores se gobiernan con salidas PWM). ¿Por qué no utilizar la función milliseconds() que ya incorpora Arduino? Esto generaría una señal PWM del ancho de pulso que indiquemos entre paréntesis. Pues la respuesta es bien sencilla: porque estas salidas PWM se ejecutan a 50Hz (cada 20ms), y ¿de qué serviría ejecutar el lazo de control cada 6ms, si luego esa información no puede transmitirse a los motores hasta pasados 20ms? Para ello, tenemos que hacer que la frecuencia de las salidas PWM de nuestra placa Arduino sea la misma que la de nuestro lazo de control, 6ms, de esta forma logramos que salida de los PID se aplique sin ningún retraso.
Vamos a analizar la siguiente imagen donde resumo el funcionamiento del lazo de control y la generacion PWM. Todos los ciclos comienzan con las cuatro señales PWM en estado HIGH, siempre. La duración del pulso es calculada por los controladores PID en el ciclo anterior. Cuando la señal paso a estado LOW, se hace la lectura del sensor MPU6050, la lectura del mando, y el cálculo de los PID del periodo siguiente. En el ejemplo de la imagen inferior, el primer cálculo arroja como resultado 1.6ms de PWM, que se aplica al pulso del siguiente ciclo.:
El tiempo que el pulso está en estado HIGH puede variar entre 1ms (parado) y 2ms (máxima velocidad). Esto hace que para que cada periodo dure exactamente 6ms, el tiempo de espera varíe en la misma proporción, de ahí la importancia de controlar el tiempo de ejecución.
Para generar las salidas PWM y que estas estén sincronizadas con la frecuencia de ejecución del lazo de control vamos a utilizar el siguiente código. Al principio puede que resulte un tanto complicado de entender, pero en cuento lo tengáis, comprenderéis que es una solución muy simple. Lo primero que hacemos al comienzo de cada nuevo periodo de 6ms es poner las 4 salidas PWM en estado HIGH:
1 2 3 4 5 6 7 8 9 10 11 | // ============================================== Modulador esc1 = PotenciaFilt + PID_pitch_w - PID_roll_w - PID_yaw_w; // Motor 1 esc2 = PotenciaFilt + PID_pitch_w + PID_roll_w + PID_yaw_w; // Motor 2 esc3 = PotenciaFilt - PID_pitch_w + PID_roll_w - PID_yaw_w; // Motor 3 esc4 = PotenciaFilt - PID_pitch_w - PID_roll_w + PID_yaw_w; // Motor 4 // ============================================== PWMs a HIGH digitalWrite(3, HIGH); //Motor 1 HIGH digitalWrite(4, HIGH); //Motor 2 HIGH digitalWrite(5, HIGH); //Motor 3 HIGH digitalWrite(6, HIGH); //Motor 4 HIGH |
Después, sumamos al tiempo calculado por el modulador (señales esc1, esc2, esc3 y esc4) el tiempo total transcurrido desde el inicio del programa (loop_timer). Por ejemplo, si el modulador calcula un tiempo de PWM de 1.6ms para el motor número 1, y desde que hemos inicial el programa han transcurrido 1s (1000ms):
accion_m1 = 1000ms + 1.6ms = 1001.6ms
1 2 3 4 | accion_m1 = esc1 + loop_timer; accion_m2 = esc2 + loop_timer; accion_m3 = esc3 + loop_timer; accion_m4 = esc4 + loop_timer; |
Finalmente, utilizando el siguiente bucle while, pasamos a estado LOW las cuatro señales PWM, cada una cuando corresponda. Cuando la variable esc_loop_timer (el tiempo real transcurrido) sea igual a la variable , significará que el pulso ha estado en estado HIGH exactamente 1.6ms, por lo que es hora de pasarlo a estado LOW:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 | while (aM1 || aM2 || aM3 || aM4 == true) { esc_loop_timer = micros(); if (accion_m1 <= esc_loop_timer) { // Motor LOW aM1 = false; digitalWrite(3, LOW); if (accion_m2 <= esc_loop_timer) { // Motor 2 LOW aM2 = false; digitalWrite(4, LOW); } if (accion_m3 <= esc_loop_timer) { // Motor 3 LOW aM3 = false; digitalWrite(5, LOW); } if (accion_m4 <= esc_loop_timer) { // Motor 4 LOW aM4 = false; digitalWrite(6, LOW); } } |
Arduino no saldrá de este bucle while hasta que las cuatro señales PWM estén en estado LOW. ¡Ya tenemos señales PWM de frecuencia personalizada!
Una vez que las cuatro señales PWM están en estado HIGH, sabemos que tenemos un margen de tiempo de 1ms (que es el ancho de pulso mínimo para las señales PWM) donde Arduino no va a hacer nada, solo esperar. Podemos aprovechar este mili-segundo para hacer alguna pequeña tarea que para Arduino suponga un tiempo de procesado menor a 1ms. Yo, por ejemplo, utilizo este tiempo para leer la tensión de la batería y activar la correspondiente alarma en caso de que la tensión sea demasiado baja:
Realmente el tiempo de espera de Arduino varía entre 1ms (parado) y 2ms (máxima potencia) dependiendo del estado de los motores. Aun así siempre hay que asegurarse de que la tarea que realizamos en este tiempo muerto sea inferior a 1ms, en caso contrario no será posible controlar los motores a bajas velocidades ya que el pulso no pasará a estado LOW hasta haber acabado esta tarea. Imaginemos que en el tiempo muerto realizamos una tarea que requiere 1.5ms de tiempo de ejecución, ¡el ancho de pulso mínimo de las señales PWM que conseguiríamos sería de 1.5ms! los motores nunca llegarían a detenerse por completo, cosa muy peligrosa.
Como hemos visto, el modo acrobático se basa únicamente en las lecturas de velocidad angular obtenidas del sensor MPU6050. Cuando giramos alguna de las palancas de nuestro mando, estamos ordenando al drone que gire en una determinada dirección a una determinada velocidad, por lo que al soltar la palanca el drone se mantendrá inclinando en el punto donde lo hayamos dejado, no volverá a su posición inicial a 0º de inclinación, solo se habrá detenido la rotación (0º/s). Imaginemos que giramos la palanca de pitch y el drone empieza a gira sobre su eje a una determinada velocidad. Cuando soltemos la palanca y esta vuelva a su posición inicial, el drone recibirá la nueva consigan de velocidad 0, es decir, que se quede quieto en su posición, por lo que quedará inclinado. Para que el drone vuelva a su posición inicial de 0º de inclinación, habrá que girar la palanca en sentido contrario y mandar una consigna de velocidad negativa hasta que quede nivelado y podamos soltar la palanca. El modo acrobático es muy difícil de controlar, únicamente los pilotos experimentados puede hacerlo forma segura. Para nosotros es un paso intermedio antes de conseguir el drone estable que veremos en el siguiente apartado.
¿Veis? ¡No es tan complicado! Acudid al apartado donde os dejo el software completo y dedicadle algo de tiempo a entender bien todos estos conceptos. Si habéis llegado hasta aquí entendiéndolo todo, ya tenéis en 80% hecho. Vayamos a por el modo estable.
Modo de control ‘Estable’
Este modo necesita de dos PID en cascada por cada eje a controlar, además de lecturas de velocidad de rotación y aceleración del sensor MPU6050 a partir de los cuales calcular el ángulo de inclinación de cada eje en grados (º). La ventaja de este modo de vuelo es que el drone es completamente estable y por lo tanto mucho más fácil de manejar. Al contrario que en el modo acrobático, cuando soltemos alguna de las palancas del mando RC el drone volverá automáticamente a su posición de 0º de inclinación. La consigna que mandamos desde el mando es de grados de inclinación (º), no de velocidad (º/s) como en el caso acrobático.

El funcionamiento de la estrategia de control es muy simple. Si detectamos una inclinación, ordenamos al drone que gire en dirección contraria a una velocidad determinada hasta contrarrestar esta inclinación:
Secuencia: Esta secuencia se ejecuta cada 5ms (200Hz)
- Arduino pide lecturas de velocidad angular y aceleración en los tres ejes al sensor MPU6050.
- El sensor responde con sus lecturas y se realizan los cálculos de velocidad angular (º/s) e inclinación (º).
- Con las lecturas de inclinación y las consignas que recibimos desde el mando, el primer PID calcula el error de inclinación y genera la consigna de velocidad para contrarrestarla.
- El segundo PID toma esta consigna del lazo exterior y con la lectura de velocidad de la del sensor MPU6050, genera la salida en microsegundos para enviar a los motores. Los motores aceleran y contrarrestan la desviación de velocidad, que a la vez está contrarrestando la desviación en la inclinación.
- Vuelta al paso 1)
**Si hay interrupción hardware: Leer mando RC → Cada 20ms aprox. recibimos una lectura.
Lo primero que hace esta estructura de control es comparar la consigna de inclinación (º) que nos llega desde el mando, en la imagen ‘Pitch* (mando)’, con la lectura que recibimos del sensor MPU6050, en la imagen ‘Pitch (IMU)’. Fijaos en que en este modo de vuelo el mando fija la consigna de inclinación (º) y no de velocidad de rotación (º/s) como en el modo acrobático. Haciendo la resta de estas dos señales conseguimos en valor de desviación o error en nuestro eje pitch, en la imagen, señal Err.1. Esta variable se da en grados de inclinación (º), 5º, 10º… la que sea:
Esta variable Err.1 pasa por el primer PID (PID_angulo o PID estable) y genera una salida que se utilizará como referencia para el siguiente lazo o lazo de velocidad (el utilizado en el modo acrobático). Lo que conseguimos con esto es indicar al drone que si está inclinado, tiene que aumentar/reducir la velocidad en los motores y rotar a una determinada velocidad y en dirección contaría a la inclinación para contrarrestarla y volver a la posición inicial de 0º:
Como hemos dicho, se compara la consigna que nos llega desde el lazo estable, en la imagen ‘w* pitch’, con la lectura de velocidad de rotación que recibimos del sensor MPU6050, en la imagen ‘w Pitch (IMU)’, para generar el error ‘Err.2’. Este error representa la desviación entre la velocidad que necesitamos para contrarrestar la inclinación y la velocidad real de rotación del drone. Finalmente la variable Err.2 pasa por el PID de velocidad (PID_w) y generamos la salida para el modulador que actuará sobre los motores.
Cuando el drone comience a girar y se vaya corrigiendo la desviación, tanto las variables Err.1 y Err 2 irán disminuyendo hasta convertirse en 0, momento en el que drone habrá vuelto a su posición inicial y no haya desviación alguna entre la consigna que enviamos desde el mando y la inclinación real.
La siguiente figura muestra la estrategia de control total utilizada en el modo estable representada con bloques. Es parecida a la figura mostrada para el modo acrobático, solo que utilizando un PID más en los ejes pitch y roll (el eje yaw no requiere de otro PID). El primer PID toma la lectura de inclinación (º) calculada a partir de las lecturas del sensor MPU6050 y la compara con la consigna del mando. Si hay una desviación de inclinacion, este primer PID genera una referencia de velocidad para el siguiente lazo, acelerando los correspondientes motores y contrarrestando la inclinación. El segundo PID controla la velocidad a que que rota el drone mientras contrarresta la inclinación. Es una estrategia bastante intuitiva:
Utilizando este método conseguiremos que al soltar la palanca del mando, el drone vuelva automáticamente a su posición inicial de 0º sin tener que mandar una consigna de velocidad negativa para contrarrestar la inclinación. El funcionamiento de los dos métodos es evidente, el acrobático simplemente compensa la rotación (º/s), mientras que el estable compensa la inclinación (º):


En el apartado ‘código completo’ podéis descargaros el software completo que utilizo yo. Utilizad esta entrada para entender el código.
Continuar con la siguiente entrada:
- Conceptos generales sobre drones.
- Material necesario y montaje de los componentes hardware.
- Mando RC y receptor. Programación en Arduino (código).
- MPU6050 y su programación en Arduino (código).
- Batería LiPo (código).
- Control de estabilidad y PID.
- → Motores, ESC y su programación en Arduino (código).
- Calibración de hélices y motores (código).
- Software completo y esquema detallado (código).
- Como leer variables de Arduino en Matlab (código).
- Los mejores drones de 2018 | Comparativa y guía de compra.
Que control viene por defecto en el software principal??
Buenas Asier,
En el control principal vienen activado el modo estable por defecto.
Un saludo
Muy buen trabajo todo tu proyecto, te felicito. Solo tengo una duda y no la he encontrado en este material… ¿De dónde sacas el valor de los PID? Es decir, los Kp, Ki y Kd. ¿Has usado MATLAB y a través de ahí sacado función de transferencia?
Buenas Guerrillero! Los parámetros de los PID están ajustando a prueba y error. Como bien dices, se podría hacer con matlab, pero sería necesario disponer del modelo matemático completo del drone, que es extremadamente complejo.
Puedes partir de los valores que yo utilizo y ajustarlos a tu drone.
Un saludo 😉
Que tal, tengo una duda, al momento de calcular la salida PID la parte derivativa se esta restando en lugar de sumarse, por que esta escrito asi.
Saludos
Buenas Gabriel,
La parte Derivativa es normal que reste, ya que su función es reducir el efecto de la parte integral y proporcional cuando hay transitorios grandes en la entrada.
Un saludo!
Hola buenas. Excelente trabajo. Tengo una duda con respecto los PIDs.
En mi caso, en el momento en que pasan 1300ms (el valor en el que se activa el control de estabilidad), los valores de los motores empiezan a desvariar, y creo que es a causa de los PID. ¿Cuál es la forma correcta de hacer ensayo y error para conseguir adecuar los valores y que funcionen bien?. Gracias.
Buenas Francisco, a que te refieres con que empiezan a desvariar? al volar? o moviendo el drone con la mano??
Puede que sea por la parte integral… si lo mueves con la mano, el la parte integral del PID se saturará rápidamente al no poder hacer el error 0.
Un saludo
Buenas tardes,
el drone ya vuela pero no es muy estable. He ajustado todos los parámetros PID a prueba y error, pero aun así el vuelo se tambalea. He visto en tu video de YouTube que tu drone vuela y se mantiene totalmente quieto en el aire, es decir que no se mueve de un lado a otro a no ser que muevas la palanca del pitch,roll o yaw. Pero el mio se mueve de un lado para otro en un rango de unos dos metros tocando unicamente la del throttle.
El sensor MPU6050 está fijo y las conexiones son estables, ¿tienes alguna idea de cual puede ser el error? No sé si hay algo más que pueda cambiar para mejorar la estabilidad, puede que subir algun parametro o bajarlo, agradezco mucho este blog y tu ayuda.
Muchas gracias.
Buenas Gillermo…
Si has ajustado los parámetros del los PID, intenta ajustar el filtrado de la estimación de inclinación, los parámetros 0.995 y 0.005. Prueba a bajar el 0.995, a ver si va mejor. Importante, amos número tiene que sumar siempre 1, ahora mismo, 0.995+0.005=1):
angulo_pitch = angulo_pitch * 0.995 + angle_pitch_acc * 0.005;
angulo_roll = angulo_roll * 0.995 + angle_roll_acc * 0.005;
Un saludo
Hola arduproject,
muchas gracias por tu respuesta, he reducido el angle_pitch_acc y el angle_roll_acc a 0 y el drone parece ser más estable. Pero el problema principal continua, el drone se mueve en un rango de unos dos metros al mover unicamente el throttle. Al principio puede parecer que se eleva con normalidad, pero finalmente se decanta hacia un lado y no se mantiene recto en el aire. No creo que tenga que ver con una mala distribución dell peso porque cada vez se inclina hacia un lado diferente.
¿Tienes alguna idea de que puede estar provocando esta irregularidad?
Por favor, estoy muy ilusionado con el drone, pero esto de que no se mantenga firme en un mismo lugar cuando únicamente pulso la palanca del throttle hace que no se pueda volar bien.
hola Arduproject, excelente trabajo! Estamos intentando seguir las instrucciones para realizar nuestro dron. Tengo alguna duda, y las que tendré…
La parte derivativa, por qué resta el ángulo del MPU? La teoría dice que debería sumar el error entre la consigna y el ángulo MPU, no?
Hola,
sinceramente no sé como tu drone ha conseguido despegar ya que el codigo relativo al PID es erróneo. La parte diferencial corresponde con la derivada de la variable a lo largo del tiempo, osea que te falta dividir entre el tiempo del ciclo (6ms en tu caso). Lo mismo con la parte integral, falta meter el tiempo multiplicando. He revisado tu codigo final y no encuentro esto que te digo por ningún lado, de verdad has volado con ese codigo? Por que lo he probado y el PID no hace mas que acumularse sin fin
Un saludo
Buenas,
Este código vuela a la perfección. Lo que me dices es verdad, pero solo a medias. Puedo dividir la parte integral entre el tiempo de ejecución, pero seguiré obteniendo una constante como resultado. Si dices que la salida se acumula sin fin, por mucho que dividas ese ‘sin fin’ entre 6ms, seguirás teniendo una salida acumulada ‘sin fin’. Puedes dividir la parte integral entre 6ms, pero tendrás que multiplicar la Ki x6 para conseguir el mismo funcionamiento. Que viene a ser lo mismo.
Me imagino que verás que los valores se acumulan sin fin manteniendo el drone en el suelo quieto y conectado al PC… esto es normal, ya que el error (que es la entrada del PID) nunca lo corriges, y vas acumulando a la salida cada vez un valor más alto. Esto se debe al propio offset del MPU6050… con que marque 0.001 en vez de 0, si lo dejas quieto acumularás sin fin como dices ya que nunca corrige ese error. Esto ya lo he comentado en alguna entrada.
En vuelo en cambio este error tenderá a hacerse cero por el propio control de estabilidad, por lo que a la salida tendrás un valor no mayor la necesario para compensar la desviación. Nunca un valor que se incrementa sin control.
Un saludo
Buenas,
despues de trastear con el código te confirmo que es cierto lo que comentas, el drone puede volar de igual manera ya que al final el tiempo es simplemente una constante, solo habría que ajustar los parámetros kp ki y kd de distinta manera. Sin embargo, el error real que tienes en tu código es restar la componente derivativa al total de la señal de control. Con ello estas haciendo que a mayor sea la derivada del error (aceleración en el bucle de velocidad y velocidad en el bucle de estabilidad), menor es la corrección que haces al respecto. Es decir, si el drone esta girando muy rapido hacia la derecha la correccion será menor que si estuviera rotando más lentamente. Probablemente te funcione ya que la componente derivativa no tiene una función importante sobre la estabilidad general, simplemente hace que la corrección no tenga un offset y se ajuste a la consigna. Por ello probablemente te funcione pero yo lo revisaría ya que estoy bastante seguro de que es erróneo.
Un saludo