Quadcopter: estabilización a lo largo del eje z (para mantener la altitud)

Recientemente pasé algo de trabajo en mi firmware de quadcopter. El modelo está estabilizando su actitud relativamente bien ahora. Sin embargo, noté que a veces está cambiando su altitud (tal vez cambios de presión, viento o turbulencia). Ahora quiero deshacerme de estas caídas de altitud y no encontré mucha literatura. Mi enfoque es usar el acelerómetro:

• Calcula la fuerza g actual del eje z
• si la fuerza g es> 0.25 gy más de 25 ms, entonces introduzco el término del acelerómetro (cm por s²) en el pid
• la salida se envía a los motores

El modelo ahora reacciona cuando se cae con una regulación ascendente de los motores. Sin embargo, no estoy seguro de si es inteligente alimentar la aceleración actual en el regulador y actualmente me pregunto si existe un método más inteligente para hacer frente a cambios repentinos y menores de altitud.

Código actual:

# define HLD_ALTITUDE_ZGBIAS 0.25f
# define HLD_ALTITUDE_ZTBIAS 25

const float fScaleF_g2cmss = 100.f * INERT_G_CONST;
int_fast16_t iAccZOutput = 0; // Accelerometer

// Calc current g-force
bool bOK_G;
float fAccel_g = Device::get_accel_z_g(m_pHalBoard, bOK_G); // Get the acceleration in g

// Small & fast stabilization using the accelerometer
static short iLAccSign = 0; 
if(fabs(fAccel_g) >= HLD_ALTITUDE_ZGBIAS) {
  if(iLAccSign == 0) {
    iLAccSign = sign_f(fAccel_g);
  }

  // The g-force must act for a minimum time interval before the PID can be used
  uint_fast32_t iAccZTime = m_pHalBoard->m_pHAL->scheduler->millis() - m_iAccZTimer;
  if(iAccZTime < HLD_ALTITUDE_ZTBIAS) {
     return; 
  }

  // Check whether the direction of acceleration changed suddenly
  // If so: reset the timer
  short iCAccSign = sign_f(fAccel_g);
  if(iCAccSign != iLAccSign) {
    // Reset the switch if acceleration becomes normal again
    m_iAccZTimer = m_pHalBoard->m_pHAL->scheduler->millis();
    // Reset the PID integrator
    m_pHalBoard->get_pid(PID_ACC_RATE).reset_I();
    // Save last sign
    iLAccSign = iCAccSign;
    return;
  }

  // Feed the current acceleration into the PID regulator
  float fAccZ_cmss = sign_f(fAccel_g) * (fabs(fAccel_g) - HLD_ALTITUDE_ZGBIAS) * fScaleF_g2cmss;
  iAccZOutput = static_cast<int_fast16_t>(constrain_float(m_pHalBoard->get_pid(PID_ACC_RATE).get_pid(-fAccZ_cmss, 1), -250, 250) );
} else {
  // Reset the switch if acceleration becomes normal again
  m_iAccZTimer = m_pHalBoard->m_pHAL->scheduler->millis();
  // Reset the PID integrator
  m_pHalBoard->get_pid(PID_ACC_RATE).reset_I();
}

Son posibles dos enfoques:

1. Combine los datos de los sensores de altitud (GPS o presión) y de aceleración vertical para calcular una mejor altitud geométrica, y sintonice su controlador vertical utilizando esta retroalimentación en su bucle.

2. Emplee un bucle de aumento de estabilidad para la aceleración z (en el marco del cuerpo). En este caso, si su vehículo se balancea, como se muestra en la respuesta de Jon, su vehículo detectará una aceleración z e intentará corregirlo. Puede que esta no sea la mejor práctica para trabajar en la aceleración z en el marco del cuerpo , ya que acoplará el giro con la altitud a medida que el avión rueda y se mueve. Por lo tanto, se puede realizar una conversión trigonométrica para convertir los datos a_z (en el marco del cuerpo) en a_z_inertial (en el marco inercial, por ejemplo, en gravedad). Es mejor trabajar esto en papel (tiene tanto rollo como cabeceo, lo que afecta el resultado).

Sobre el algoritmo actual:

1. Filtra tus aceleraciones. Pruebe con un promedio de funcionamiento (filtro de paso bajo) de sus aceleraciones, para deshacerse del ruido. Probablemente estará bien tener un promedio de los últimos 0.2 segundos, por ejemplo.

2. No utilice el corte, en absoluto. Hace la vida no lineal, y no es buena. Deje que el controlador maneje todos los eventos, y deje que reaccione a los pequeños errores, antes de que crezcan.

No puedo comentar todavía.

Agregaría un giroscopio y usaría un filtro complementario o Kalman. Los acelerómetros tienen razón, en promedio , pero están equivocados, en este momento . Los giroscopios tienen razón, en este momento , pero están equivocados, en promedio . Los pesos del filtro de las dos entradas sobre la base de lo equivocados que están en algún lugar y generan una valor entre la derecha y en este momento .