Quadricottero: stabilizzazione lungo l'asse z (per mantenere l'altitudine)

Di recente ho trascorso alcuni lavori sul mio firmware quadricottero. Il modello sta stabilizzando il suo atteggiamento relativamente bene ora. Tuttavia ho notato che a volte sta cambiando quota (forse variazioni di pressione, vento o turbolenza). Ora voglio sbarazzarmi di questi dislivelli e non ho trovato molta letteratura. Il mio approccio sta usando l'accelerometro:

• Calcola la forza g attuale dell'asse z
• se la forza g è> 0,25 ge più lunga di 25 ms, inserisco il termine dell'accelerometro (cm per s²) nel pid
• l'uscita viene inviata ai motori

Il modello ora reagisce quando cade con una regolazione in alto dei motori. Tuttavia, non sono sicuro se sia intelligente alimentare l'attuale accelerazione nel regolatore e al momento mi chiedo se esiste un metodo più intelligente per gestire variazioni improvvise e minori dell'altitudine.

Codice attuale:

# 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();
}

Sono possibili due approcci:

1. Combina i dati dei sensori di altitudine (GPS o pressione) e di accelerazione verticale per calcolare una migliore altitudine geometrica e sintonizza il tuo controller verticale usando questo feedback nel tuo loop.

2. Utilizzare un ciclo di aumento della stabilità per l'accelerazione z (nel telaio del corpo). In questo caso, se il tuo veicolo oscilla, come mostrato nella risposta di Jon, il tuo veicolo percepirà un'accelerazione z e proverà a correggerlo. Questa potrebbe non essere la migliore pratica per lavorare sull'accelerazione z nel telaio del corpo , poiché accoppierà il rollio con l'altitudine mentre il velivolo rotola e si muove. Quindi una conversione trigonometrica può essere eseguita per convertire i dati a_z (nel frame del corpo) in a_z_inertial (nel frame inerziale, ad es. In gravità). È meglio lavorare su carta (hai sia il rollio che il pitch, influenzando il risultato).

Informazioni sull'algoritmo corrente:

1. Filtra le tue accelerazioni. Prova una media corrente (filtro passa basso) delle tue accelerazioni, per eliminare il rumore. Ad esempio, probabilmente sarà OK avere una media corrente degli ultimi 0,2 secondi.

2. Non usare assolutamente il cut-off. Rende la vita non lineare e non è buona. Lascia che il controller gestisca tutti gli eventi e lascia che reagisca ai piccoli errori, prima che diventino alti.

Non posso ancora commentare.

Vorrei aggiungere un giroscopio e utilizzare un filtro complementare o Kalman. Gli accelerometri hanno ragione, in media , ma in questo momento sono sbagliati . I giroscopi hanno ragione, proprio ora , ma hanno torto, in media . Il filtro pondera i due ingressi in base a quanto sono sbagliati e genera un valore tra destra e ora .