Quadcopter: Stabilisierung entlang der z-Achse (zum Halten der Höhe)

Ich habe kürzlich einige Arbeiten an meiner Quadcopter-Firmware durchgeführt. Das Modell stabilisiert seine Haltung jetzt relativ gut. Ich bemerkte jedoch, dass es manchmal seine Höhe ändert (möglicherweise Druckänderungen, Wind oder Turbulenzen). Jetzt möchte ich diese Höhenunterschiede loswerden und habe nicht viel Literatur gefunden. Mein Ansatz ist die Verwendung des Beschleunigungsmessers:

• Berechnet die aktuelle g-Kraft der z-Achse
• Wenn die g-Kraft> 0,25 g und länger als 25 ms ist, führe ich den Beschleunigungsmesserterm (cm pro s²) in die PID ein
• Der Ausgang wird an die Motoren gesendet

Das Modell reagiert jetzt beim Herunterfallen mit einer Hochregulierung der Motoren. Ich bin mir jedoch nicht sicher, ob es sinnvoll ist, die aktuelle Beschleunigung in den Regler einzuspeisen, und ich frage mich derzeit, ob es eine intelligentere Methode gibt, um mit plötzlichen und kleineren Höhenänderungen umzugehen.

Aktueller Code:

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

Zwei Ansätze sind möglich:

1. Kombinieren Sie die Daten der Höhen- (GPS- oder Druck-) und Vertikalbeschleunigungssensoren, um eine bessere geometrische Höhe zu berechnen, und stellen Sie Ihren Vertikalregler mithilfe dieser Rückmeldung in Ihrer Schleife ein.

2. Verwenden Sie eine Stabilitätsvergrößerungsschleife für die Z-Beschleunigung (im Körperrahmen). In diesem Fall erkennt Ihr Fahrzeug eine Z-Beschleunigung und versucht, diese zu korrigieren, wenn Ihr Fahrzeug schwingt, wie in Jons Antwort gezeigt. Dies ist möglicherweise keine bewährte Methode, um an der Z-Beschleunigung im Körperrahmen zu arbeiten , da dadurch die Rolle mit der Höhe gekoppelt wird, wenn das Flugzeug rollt und sich bewegt. So kann eine trigonometrische Konvertierung durchgeführt werden, um die a_z-Daten (im Körperrahmen) in a_z_inertial (im Trägheitsrahmen, z. B. in der Schwerkraft) zu konvertieren. Es ist am besten, dies auf Papier zu arbeiten (Sie haben sowohl Roll als auch Pitch, was sich auf das Ergebnis auswirkt).

Über den aktuellen Algorithmus:

1. Filtern Sie Ihre Beschleunigungen. Versuchen Sie einen laufenden Durchschnitt (Tiefpassfilter) Ihrer Beschleunigungen, um das Rauschen zu beseitigen. Es wird wahrscheinlich in Ordnung sein, zum Beispiel einen laufenden Durchschnitt der letzten 0,2 Sekunden zu haben.

2. Verwenden Sie überhaupt keinen Cut-Off. Es macht das Leben nicht linear und es ist nicht gut. Lassen Sie den Controller alle Ereignisse behandeln und auf kleine Fehler reagieren, bevor sie hoch werden.

Kann noch nicht kommentieren.

Ich würde einen Kreisel hinzufügen und einen Komplementär- oder Kalman-Filter verwenden. Beschleunigungsmesser sind im Durchschnitt richtig, aber im Moment falsch . Gyros haben im Moment Recht, aber im Durchschnitt liegen sie falsch . Der Filter gewichtet die beiden Eingaben basierend darauf, wie falsch sie sind, und gibt einen Wert irgendwo zwischen richtig und gerade aus .