Controlador PID me da valores muy altos. (MPU6050)

International Español Software
1

Buenas tardes, tengo una problema con un controlador PID que estoy armando para un dron. Este procesa los datos del acelerómetro y giroscopio del MPU6050 para alimentar con el ancho de pulso correspondiente a cada brushless. El tema es que apenas inicia el código, estando el giroscopio en una superficie plana, ya arranca con valores exorbitantes, que si no fuera por la constrain serian aun peores. Poco a poco desciende, pero para bajar de 100 a +- 1 tarda casi 30 segundos lo cual es claramente inaceptable para un controlador de vuelo. Los kp, ki, y kd son =1, fui modificando cada constante varias veces, pero después de darle muchas vueltas no encontré solución en ese ámbito, por ende creo que el problema esta por otro lado.

El código de inicialización y obtención de datos del MPU lo arme por mi parte, pero lo que seria el procesamiento de datos en crudo (Process_MPU) lo obtuve de un código posteado en linea.

Muchas gracias.

Aca los links de lo que me imprime el Serial:

  1. https://ibb.co/JCXhyHW

  2. https://ibb.co/FsK8VNW

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


int vueltas;


bool set_gyro_angles, accCalibOK = false;





///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


// Subrutina inicializacion


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


void init_MPU6050() {


  Wire.setClock(400000);


  Wire.begin();                      // Initialize comunication


  Wire.beginTransmission(MPU);


  Wire.write(0x1A);


  Wire.write(0x04);     // Filtro pasa bajos -> 20Hz(8.5ms):0x04 


  Wire.endTransmission();


  Wire.beginTransmission(MPU);       // Empezar Comunicacion con MPU6050 // 


  Wire.write(0x6B);                  // Llamo al registro 6B


  Wire.write(0);                     // Hacemos reset (con 0)


  Wire.endTransmission(true);        // Final de transmision


 


 Serial.println("Inciando MPU6050");


}





///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


// Calibracion MPU6050


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


void Calib_MPU6050() {


Serial.println("Calibrando Giro ");


  


  for (cal_int = 0; cal_int < 3000 ; cal_int ++) {  // Calibrar giroscopio tomando 3000 muestras


    MPU_6050();


    gyro_X_cal += gx;


    gyro_Y_cal += gy;


    gyro_Z_cal += gz;


  }


  gyro_X_cal = gyro_X_cal / 3000; // Calculamos el valor medio de las 3000 muestras


  gyro_Y_cal = gyro_Y_cal / 3000;


  gyro_Z_cal = gyro_Z_cal / 3000;


  


  Serial.println("Calibrando acc ");


  if (mEstable == 1) {


    for (cal_int = 0; cal_int < 3000 ; cal_int ++) { // Calibrar acelerometro tomando 3000 muestras


      MPU_6050();


      angle_pitch_acc_cal += ax;


      angle_roll_acc_cal += ay;


      angle_yaw_acc_cal += az;


    }


    angle_pitch_acc_cal = angle_pitch_acc_cal / 3000;  // Calculamos el valor medio de las 3000 muestras


    angle_roll_acc_cal = angle_roll_acc_cal / 3000;


    angle_yaw_acc_cal = angle_yaw_acc_cal / 3000;


    accCalibOK = true;


  }


}





///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


// Lectura MPU6050


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


void MPU_6050() {





  Wire.beginTransmission(MPU);     //Empezar comunicacion con MPU6050


  Wire.write(0x3B);


  Wire.endTransmission();


  Wire.requestFrom(MPU, 14);


  tiempo_ejecucion = (micros() - loop_timer1) / 3000;


  loop_timer1 = micros();


  ax = Wire.read() << 8 | Wire.read();


  ay = Wire.read() << 8 | Wire.read();


  az = Wire.read() << 8 | Wire.read();


  temperature = Wire.read() << 8 | Wire.read();


  gx = Wire.read() << 8 | Wire.read();


  gy = Wire.read() << 8 | Wire.read();


  gz = Wire.read() << 8 | Wire.read();





  if (accCalibOK == true) { // Restar valores de calibracion


    ax -= angle_pitch_acc_cal;


    ay -= angle_roll_acc_cal;


    az -= angle_yaw_acc_cal;


    az = az + 4096;








}


}





///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


// Calculo de angulo


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


void ProcesMPU() {


  // ================================================================================================== Calculo w


  pitchGyro = (gx - gyro_X_cal) / 65.5; // 65.5: si leo 65.5 en crudo, significa que gira a 1º/s segun el datasheet del MPU6050.


  rollGyro = (gy - gyro_Y_cal) / 65.5;


  yawGyro = (gz - gyro_Z_cal) / 65.5;





  gyro_roll_input = rollGyro;


  gyro_pitch_input = pitchGyro;


  gyro_yaw_input = yawGyro;


  //  gyro_roll_input = (gyro_roll_input * 0.5) + (rollGyro * 0.5);


  //  gyro_pitch_input = (gyro_pitch_input * 0.5) + (pitchGyro * 0.5);


  //  gyro_yaw_input = (gyro_yaw_input * 0.5) + (yawGyro * 0.5);





  // ================================================================================================== Calculo ang


  angulo_pitch += pitchGyro * tiempo_ejecucion / 3000 ;


  angulo_roll += rollGyro * tiempo_ejecucion / 3000 ;


  angulo_pitch += angulo_roll * sin((gz - gyro_Z_cal) * tiempo_ejecucion  * 0.000000266);      //tiempo_ejecucion/1000 /65.5 * PI/180   


  angulo_roll -= angulo_pitch * sin((gz - gyro_Z_cal) * tiempo_ejecucion  * 0.000000266);





  acc_total_vector = sqrt(pow(ay, 2) + pow(ax, 2) + pow(az, 2));


  angle_pitch_acc = asin((float)ay / acc_total_vector) * 57.2958;     // 57.2958 = Conversion de radianes a grados 180/PI


  angle_roll_acc = asin((float)ax / acc_total_vector) * -57.2958;





  if (set_gyro_angles) {


    angulo_pitch = angulo_pitch * 0.995 + angle_pitch_acc * 0.005;  // Angulo Pitch de inclinacion


    angulo_roll = angulo_roll * 0.995 + angle_roll_acc * 0.005;     // Angulo Roll de inclinacion


  }


  else {


    angulo_pitch = angle_pitch_acc;


    angulo_roll = angle_roll_acc;


    set_gyro_angles = true;


  }


}





///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


// PID angulo (inclinacion) -----> 1er calculo


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


void PID_ang() {


//Las entradas del Acelerometro son: angulo_pitch, angulo_roll, angulo_yaw.
//y las entradas del Gyroscopo son: gyro_pitch, gyro_roll, gyro_yaw


//Primer lazo PID.


dT2= micros()-last_t2;


last_t2=micros();


/////////////////////////////////////////////////////////////////////////////////////////


//****ROLL PID****


//Proporcional


roll_prop_error= angulo_roll - wRollConsigna;


//Integral


roll_int_error+= (roll_prop_error)/(dT2/1000); //


roll_int_error= constrain(roll_int_error,-150,150);


//Derivativo


roll_der_error= (roll_prop_error - roll_prop_error_ant)/(dT2/1000);


roll_prop_error_ant=roll_prop_error;


/////////////////////////////////////////////////////////////////////////////////////////


//****PITCH PID****


//Proporcional


pitch_prop_error= angulo_pitch - wPitchConsigna;


//Integral


pitch_int_error+= (pitch_prop_error)*(dT2/1000); //. 


pitch_int_error= constrain(pitch_int_error,-150,150);


//Derivativo
pitch_der_error= (pitch_prop_error - pitch_prop_error_ant)/(dT2/1000);


pitch_prop_error_ant=pitch_prop_error;
/////////////////////////////////////////////////////////////////////////////////////////

//****YAW PID****
//Proporcional


yaw_prop_error= angulo_yaw - (wYawConsigna/10);


//Integral


yaw_int_error+= (yaw_prop_error)*(dT2/1000); //


yaw_int_error= constrain(yaw_int_error,-150,150);


//Derivativo


yaw_der_error= (yaw_prop_error - yaw_prop_error_ant)/(dT2/1000);



yaw_prop_error_ant=yaw_prop_error;


}
2

Continnuo el código aca por cantidad de caracteres:

[code]void CALCULO_0(){

// CALCULO DE ROLL


kp_roll_error=(Kp * roll_prop_error);


ki_roll_error=(Ki * roll_int_error);


kd_roll_error=(Kd* roll_der_error);


kp_roll_error+= ki_roll_error;


kp_roll_error+= kd_roll_error;


kp_roll_error=constrain(kp_roll_error,-100,100);


// CALCULO DE PITCH


kp_pitch_error=(Kp * pitch_prop_error);


ki_pitch_error=(Ki * pitch_int_error);


kd_pitch_error=(Kd* pitch_der_error);

kp_pitch_error+= ki_pitch_error;


kp_pitch_error+= kd_pitch_error;


kp_pitch_error= constrain(kp_pitch_error,-100,+100);


// CALCULO DE YAW

kp_yaw_error=(Kp * yaw_prop_error);


ki_yaw_error=(Ki * yaw_int_error);


kd_yaw_error=(Kd* yaw_der_error);

kp_yaw_error+=ki_yaw_error;


kp_yaw_error+=kd_yaw_error;

kp_yaw_error= constrain(kp_yaw_error,-100,100);


   


  }

[/code]

3

Moderador:
@giulianoc75 vienes haciendo varias consultas sobre el dron y problemas derivados de éste.
Porque no las unimos asi tenemos una idea mas general de todo, porque se pierde el hilo si en una preguntas por los motores ESC y en otra por el PID. Estoy de acuerdo que son varios temas pero el hilo conductor sigue siendo el mismo.
Si estas de acuerdo, los uno.
Otra cosa, en donde publicas la imagen PWM del osciloscopio te he pedido que es mejor que adjuntes las imágenes para no perderas. Los repositorios en 1 o 2 meses las borran.
Corrige las dos que has puesto en el post anterior por favor.

4

Como dices esto

Poco a poco desciende, pero para bajar de 100 a +- 1 tarda casi 30 segundos lo cual es claramente inaceptable para un controlador de vuelo. Los kp, ki, y kd son =1, fui modificando cada constante varias veces, pero después de darle muchas vueltas no encontré solución en ese ámbito, por ende creo que el problema esta por otro lado.

La respuesta de tu dron depende justamente de kp, ki y kd.
Mira este video y espero puedas resolverlo

Diras, pero yo no tengo un dron de ese tipo, no puedo armar el log. De todos modos verás que el primer enfoque con el banco de pruebas muestra justamente lo que quieres hacer y como debe ser la respuesta de tu PID.
Todos = 1 nunca ha sido mas que la versión inicial de un PID, jamás puedes decir que eso es suficiente porque no es el caso.
Hay muchos otros videos indicándote como hacer para ajustarlo.

5

Hola surbyte, gracias por tu respuesta. Mirando ese video y dandole vueltas lo pude sacar, mi problema era que no tenia correctamente medido el tiempo de ejecución y al dividir la integral y derivada me daba cualquier cosa, llevándome a valores absurdos. Por eso aunque le modificaba los valores Kp, Ki o Kd se me iba de las manos igual. Pido disculpas si arme varios hilos, no era mi intención abusar del foro, pero no sabia si era lo indicado consultar en un mismo hilo diferentes temas.
Por mi no hay problema si queres unir los hilos. La próxima vez que haga una consulta subo la imagen directamente como me recomendaste. Muchas gracias por tu aporte.

6

Uniré los hilos para que se vea mejor el objetivo que persigues.