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

8

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();
}
dgrat
quelle
1
Wie wäre es mit einem nach unten zeigenden Sonar, um die Höhe zu messen?
dm76
2
Warum verwenden Sie einen Schwellenwert von 0,4 g? Warum nicht, wie Sie sagen, immer die Z-Beschleunigung in die PID senden?
Raketenmagnet
Das Sonar arbeitet nur für 6 m und ich wollte den Beschleunigungsmesser nur für plötzliche und kleine Änderungen verwenden. Ich dachte, ich sollte einen Cutoff verwenden, da der Beschleunigungsmesser anfällig für Sensorgeräusche ist. Darüber hinaus sollte die PID nur bei einer größeren Änderung funktionieren, nicht bei jeder kleinen Bewegung.
Grat
Beschleunigungsmesserdaten durch ein digitales Tiefpassfilter laufen lassen?
Simon Richter
4
Bevor Sie eine Höhe genau halten können, müssen Sie in der Lage sein, Ihre Höhe genau zu messen . Bevor Sie schnell reagieren können, müssen Sie schnell messen . Angenommen, Sie können Ihre Höhe sofort bestimmen (und Ihren gewünschten Schub gleichermaßen ändern), wie lange brauchen Ihre Motoren, um zu beschleunigen, und wie lange dauert es, bis sich dies auf die Masse und Geschwindigkeit des Quadcopters auswirkt? Was ist der Unterschied zwischen dieser optimalen Latenz und der Latenz, die Sie jetzt sehen?
Ian

Antworten:

2

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.

Gürkan Çetin
quelle
1

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 .

Geben Sie hier die Bildbeschreibung ein

Jon
quelle
Ich sehe nicht, wie der Kreisel in diesem Fall helfen kann, da er die Winkelgeschwindigkeit und nicht die Höhenschwankung misst. Der Beschleunigungsmesser gibt keine direkte Messung der Höhe, kann aber Rückmeldung über lineare Bewegungen durch Integration geben,
na ja
Wenn Sie einen Kreisel und einen Beschleunigungsmesser an Ihrem Hubschrauber auf Null stellen und ihn dann um 45 Grad um eine Achse drehen, zeigt der Kreisel jetzt 45 an und beginnt dann, falsch zu liegen. Der Beschleunigungsmesser ist falsch, zeigt aber bald "unten" bei 45. Es ist wahrscheinlich, dass Ihr Beschleunigungsmesser auf ungefilterte Vibrationen reagiert. Der Kreisel und der Filter wissen, dass es keinen Grund für die Anzeige des Beschleunigungsmessers gibt, und korrigieren ihn.
Jon
Wenn der Kreisel sagt, der Hubschrauber sei verkehrt herum, weiß der Beschleunigungsmesser, dass er immer noch verkehrt ist.
Jon
Ok, Sie haben die klassische komplementäre Filteranpassung zur Korrektur von Beschleunigungswerten erwähnt, aber ich denke, hier ist es eher der Punkt, die Beschleunigung zu verwenden, um eine Höhenschätzung zu erhalten. Ich meine, "stell dir vor, wir haben gute Beschleunigungswerte, dann brauchen wir hier ..."
Ugo Pattacini
Wenn der Copter kippt, ändert sich der Beschleunigungsmesser-z und der Copter sieht "fallen", was nicht geschieht. Er hat es bereits gezwungen, mit der Nullzone zu arbeiten, möchte aber, dass es nicht mehr verwirrt wird.
Jon