Quadcopter: stabilisation le long de l'axe z (pour maintenir l'altitude)

8

J'ai récemment passé un peu de travail sur mon firmware quadcopter. Le modèle stabilise relativement bien son attitude maintenant. Cependant, j'ai remarqué qu'il change parfois d'altitude (peut-être des changements de pression, du vent ou des turbulences). Maintenant, je veux me débarrasser de ces chutes d'altitude et je n'ai pas trouvé beaucoup de littérature. Mon approche utilise l'accéléromètre:

  • Calcule la force g actuelle de l'axe z
  • si la force g est> 0,25 g et supérieure à 25 ms, alors j'introduis le terme accéléromètre (cm par s²) dans le pid
  • la sortie est envoyée aux moteurs

Le modèle réagit maintenant lorsqu'il tombe avec une régulation à la hausse des moteurs. Cependant, je ne sais pas s'il est judicieux d'introduire l'accélération actuelle dans le régulateur et je me demande actuellement s'il existe une méthode plus intelligente pour faire face aux changements soudains et moins importants d'altitude.

Code actuel:

# 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
la source
1
Que diriez-vous d'utiliser un sonar pointant vers le bas pour mesurer l'altitude?
dm76
2
Pourquoi utilisez-vous un seuil de 0,4 g? Pourquoi ne pas, comme vous le dites, toujours envoyer l'accélération Z dans le PID?
Rocketmagnet
Le sonar ne fonctionne que sur 6m et je voulais utiliser l'accéléromètre uniquement pour des changements brusques et petits. J'ai pensé que je devrais utiliser une coupure, car l'accéléromètre est sujet au bruit du capteur. De plus, le PID ne devrait fonctionner que lorsqu'il y a un changement plus important, pas pour chaque petit mouvement.
dgrat
Exécuter les données de l'accéléromètre via un filtre passe-bas numérique?
Simon Richter
4
Avant de pouvoir maintenir une altitude avec précision, vous devez être capable de mesurer votre altitude avec précision. Avant de pouvoir réagir rapidement, vous devez mesurer rapidement. En supposant que vous puissiez déterminer instantanément votre altitude (et modifier également votre poussée souhaitée), combien de temps cela prendra-t-il à vos moteurs pour accélérer et combien de temps cela prendra-t-il pour que cela ait un effet sur la masse et la vitesse du quadcopter? Quelle est la différence entre cette latence optimale et la latence que vous voyez maintenant?
Ian

Réponses:

2

Deux approches sont possibles:

  1. Combinez l'altitude (GPS ou pression) et les données des capteurs d'accélération verticale pour calculer une meilleure altitude géométrique, et ajustez votre contrôleur vertical en utilisant cette rétroaction dans votre boucle.

  2. Utilisez une boucle d'augmentation de stabilité pour l'accélération z (dans le cadre du corps). Dans ce cas, si votre véhicule oscille, comme indiqué dans la réponse de Jon, votre véhicule détectera une accélération z et essaiera de corriger cela. Ce n'est peut-être pas la meilleure pratique pour travailler sur l'accélération z dans la carrosserie , car cela couplera le roulis avec l'altitude pendant que l'avion roule et se déplace. Une conversion trigonométrique peut donc être effectuée pour convertir les données a_z (dans le cadre corporel) en a_z_inertial (dans le cadre inertiel, par exemple en gravité). Il est préférable de travailler cela sur du papier (vous avez à la fois un roulis et un tangage, ce qui affecte le résultat).

À propos de l'algorithme actuel:

  1. Filtrez vos accélérations. Essayez une moyenne courante (filtre passe-bas) de vos accélérations, pour vous débarrasser du bruit. Par exemple, il sera probablement correct d'avoir une moyenne mobile des 0,2 dernières secondes.

  2. N'utilisez pas du tout de coupure. Cela rend la vie non linéaire et ce n'est pas bon. Laissez le contrôleur gérer tous les événements et laissez-le réagir aux petites erreurs avant qu'elles ne deviennent élevées.

Gürkan Çetin
la source
1

Je ne peux pas encore commenter.

Je voudrais ajouter un gyroscope et utiliser un filtre complémentaire ou Kalman. Les accéléromètres ont raison, en moyenne , mais ils ont tort en ce moment . Les gyroscopes ont raison, en ce moment , mais ils ont tort, en moyenne . Le filtre pondère les deux entrées en fonction de leur erreur et génère une valeur quelque part entre le moment et le moment .

entrez la description de l'image ici

Jon
la source
Je ne vois pas comment le gyroscope peut aider dans ce cas, car il mesure la vitesse angulaire et non la variation d'altitude. L'accéléromètre ne donne pas une mesure directe de l'altitude mais peut fournir un retour sur les mouvements linéaires grâce à l'intégration, enfin à peine ...
Ugo Pattacini
Si vous mettez à zéro un gyroscope et un accéléromètre sur votre hélicoptère, puis tournez-le à 45 degrés sur un axe, le gyroscope lira 45 en ce moment, puis commencera à se tromper. L'accéléromètre sera erroné, mais montrera bientôt "en baisse" à 45. Il est probable que votre accéléromètre réagisse à des vibrations non filtrées. Le gyroscope et le filtre sauront qu'il n'y a aucune raison de lire l'accéléromètre et le corrigeront.
Jon
De même, lorsque le gyroscope dit que l'hélicoptère est à l'envers, l'accéléromètre sait que le bas est toujours en bas.
Jon
Ok, vous avez mentionné l'ajustement classique du filtre complémentaire pour corriger les lectures d'accélération, mais je pense que c'est plus le point d'utiliser l'accélération pour obtenir une estimation d'altitude. Je veux dire, "imaginez que nous avons de bonnes valeurs d'accélération alors ce dont nous avons besoin ici est ..."
Ugo Pattacini
Lorsque l'hélicoptère s'incline, l'accéléromètre-z change et l'hélicoptère voit une "chute" qui ne se produit pas. Il l'a déjà contraint à travailler avec la zone nulle mais veut qu'il cesse de se confondre.
Jon