Team 2

Teamname: Helium Heroes
Teammitglieder: Thomas Wildgruber
  Klaus Wackerbarth
  Julian Kullick
  Stefan Hinterleuthner
  Johannes Tonn


Projektplan und Präsentationen

Das Ziel unseres Teams ist es, eine Gondel für den Mini Blimp zu konstruieren und programmieren, die den Rahmenbedingungen des Projektes genügt. Der Blimp soll dadurch bis zur Abschlusspräsentation in die Lage versetzt werden, einen vorgegeben Parkour vom Start- zum Zielpunkt autonom abzufliegen, ohne dabei außerhalb des vorgegebenen Weges zu liegen oder mit den Begrenzungen der Strecke zu kollidieren. Außerdem darf das Zeppelin durch keine physikalische Verbindung zwischen Luftschiff und Boden beeinflusst werden. Innerhalb von drei Versuchen muss dabei mindestens eine erfolgreiche Durchquerung stattfinden.
Weitere Informationen gibts in unserem Projektplan: Projektplan_Team2.pdf

Einführungspräsentation.ppt (1.21 Mb)
Zwischenpräsentation.pdf (1.22 Mb)
Abschlusspräsentation.pdf (1.40 Mb)

Organisation

Dateiverwaltung

Alle Projektdateien werden in einem Dropbox Ordner gespeichert.

Versionsverwaltung

Bei Weiterarbeit am Projekt ist wie folgt vorzugehen:

1. Lokale Kopie der gewünschten Version erstellen (kompletter Ordner)
2. Ordner umbenennen in MM-DD-_Controller_Vorname1-Vorname2-...
3. Am Ende, in dem erstellten Versionsordner eine Readme-Datei erstellen, in der die Änderungen beschrieben werden, und den gesamten Ordner in die Dropbox hochladen

Aufgabenverteilung

Die Aufgabenverteilung wird mit dem Programm ToDo-List gemacht. Dieses Software-Tool erlaubt es unter anderem den zu erledigenden Aufgaben Personen, Prioritäten und Terminfristen zu zuordnen.

Hier ein Beispiel:
ToDo-List

ToDoList unseres Projekts nach Abschluss aller Aufgaben

 

Konstruktion der Gondel

Materialliste

Gondelabmessungen

Länge 15,5cm
Breite 7,6cm
Höhe 13,2cm
Gewicht(mit Akku) 79,5g
Anzahl der Motoren 3
Motorabstand 50cm

 

Gondelabmessungen

 

Version 1


Hier der vorläufige Aufbau unserer Gondel, die Länge der Sensor- und Motorschienen wird nach den ersten Testflügen dann angepasst.
Da es bei dieser Konstruktion jedoch zu einigen Sotwareausfällen kam, die durch einen Kabelbruch zwischen Arduino und einem der Motortreiber verursacht wurden und außerdem das Gesamtgewicht das zulässige Wettbewerbsgewicht um 5g überstieg, entschieden wir uns für eine neue Gondelkonstruktion.

Version 2

In der zweiten Version wurde die Carbonstange durch eine aus Balsaholz ersetzt und die Grundplatte aus Styropor stark verkleinert. Dadurch konnte wieder ein zulässiges Gesamtgewicht (<80g) erreicht werden. Beim ersten Test unserer Höhenregelung stellte sich jedoch heraus, dass durch die Motoren starke Vibrationen erzeugt werden. Aufgrund dieser Vibrationen liefen die Motoren nicht rund und der Ultraschallsensor hatte immer wieder kleinere Aussetzter. Die Gondelkostruktion musste ein weiteres mal überdacht werden.

Version 3

Version 3a


Diese Version besitzt anstatt einer Balsaholzstange wieder eine Carbonstange. Zusätzlich wurde die Styroporhalterung des dritten Motors - der für die Höhenregelung zuständige - mit einer Carbonstange verstärkt. Die Styroporbasis der Gondel wurde nochmals leicht verkleinert und mit Balsaholz verstärkt. Außerdem befinden sich Motortreiber, Mikrocontroller und Spannungsschienen auf einer Depronplatte, welche mit einem Gummiband an der Gondel befestigt wird.

Version 3b


In der aktuellen Version wurde das Sensorboard ausgelagert und wird nun an einem Schaumstoffwürfel - welcher direkt am Zeppelin platziert wird - befestigt. Dadurch konnte das Messrauschen am Beschleunigungssensor, welches durch die starken Vibrationen an der Gondel entsteht, um den Faktor 10 verringert werden.

Beschleunigung in mG über einen Zeitraum von 10 Sekunden:

ungedämpft.png (8.14 Kb)

 

Sensorboard auf Gondel befestigt

 

 

Sensorboard gedämpft aufgehängt

 

Schaltplan

Image

Den Schaltplan gibt es hier als PDF:
Helium_Heroes_Schaltplan.pdf

Software

Programmübersicht

Das Hauptprogramm befindet sich in der Datei Helium_Heroes.ino.

Dort werden zunächst alle wichtigen Bibliotheken hinzugefügt. Diese sind:

  • Wire.h: Ermöglicht die Kommunikation mit I2C Geräten
  • I2Cdev.h: Einfaches und intuitives Interface für I2C Geräte
  • HMC5883L.h: Stellt Funktionen zum Auslesen der Kompassdaten bereit
  • MPU6050.h: Stellt Funktionen zum Auslesen des Beschleunigungssensors und des Gyroskops bereit
  • math.h: Standardklasse für mathematische Funktionen
  • controller.h: Header-File, enthält alle globalen Variablen, Pinbelegungen und Reglerparameter


Jedes Arduino Programm besteht mindestens aus den Zeilen

void setup() {

}

void loop() {

}



Die setup() Funktion wird genau einmal zu Beginn ausgeführt. Anschließend wird die Funktion loop() in Endlosschleife aufgerufen.

In unserem Fall dient die setup() Funktion der Initialisierung von Buskommunikation, Motortreiber und Sensoren. Nach einer Pause von 3000ms, in welcher anfängliche Schwankungen der Sensorwerte abklingen sollten, werden die Sensoren mit Startwerten belegt, z.B. der Sollwert für die Höhenregelung.

void setup() {
  
  // initialize serial communication
  init_system();
  
  // initialize motors
  init_motors();
  
  // initialize sensors
  init_sensors();
  
  delay(3000);
  
  // initialize legend
  init_legend();
  
  // initialize values
  init_sensorvalues();
  
}



Anschließend beginnt der eigentliche Teil des Programms. In der loop() Funktion werden zu Beginn die Sensordaten neu ausgelesen, anschließend die Regelung ausgeführt und zum Schluss wichtige Werte zur Analyse über den Bus ausgegeben.

void loop() {
  
  // function call for setpath(circle) for the PID-Controller
  kreisbahn();
  
  // writes accelaration and gyroskope values into global variables
  getaccgyro();
  // writes magnetic values into global variables
  getmag();
  // writes hight into global variables
  getus();
  
  // saves time from the previous cycle
  t_tn = t_tm;
  
  // function call for height-control
  hoehenregelung();

  // function call for position-control
  heading_controller();
  
  // output on the serial monitor
  serial_monitor();
  
}

 

Sensorauswertung

Die Funktionen zur Sensorauswertung befinden sich in der Datei Sensoren.ino.

Initialisierung

Die Funktion init_sensors() initialisiert alle benötigten Sensoren.

void init_sensors(){
  Serial.println("Initialize Sensors...");
  // initialize US
  pinMode(us, INPUT);
  pinMode(A3, INPUT);
  // initialize MPU
  acc.initialize();
  Serial.println(acc.testConnection() ? "MPU6050 connected successfully!" : "MPU6050 connection failed!");
  // enable bypass
  acc.setI2CBypassEnabled(true);
  // initialize compass
  mag.initialize();
  Serial.println(mag.testConnection() ? "HMC5883 connected successfully!" : "HMC5883 connection failed!");
  mag.setSampleAveraging(3);
  mag.setGain(1);
  
}



Zum belegen der Sensoren mit Initialwerten steht die Funktion init_sensorvalues() zur Verfügung:

void init_sensorvalues(){
  // getting height
  getus();
  // obtain height
  height_init = height;
  
  // Initialize time and height values
  h_tm = height;
  t_tm = millis();
  
   // missing first values out to avoid errors
   // getting acceleration gyroscope an magnetometer
  for(int i=0; i<10; i++) 
  {
    getaccgyro();
    getmag();
  }

  // Initialize direction phi__xy
  phi_xy_init=phi_xy_tilt; 
  phi_rel_mem=0;
  umdrehung = 0;
  
  // initialization end
  digitalWrite(led_r,LOW);
}

 

Filterung der Sensorwerte

Zur Verminderung des Messrauschens wurde ein Tiefpass 1. Ordnung in Form eines exponentiellen Glättungsfilters programmiert.

float lowpassFilter(float x_new, float x_old, float alpha)
{
   return (1-alpha) * x_old + alpha * x_new; 
}

 

Sensoren

Das verwendete Sensorboard IMU-10DOF  der Firma Drotek, beinhaltet ein Gyroskop, einen Beschleunigungssensor, einen Kompass und einen Luftdruck-Höhensensor. Letzterer ist jedoch für unsere Zwecke zu ungenau, weshalb ein externer Ultraschallsensor direkt an einen Analogeingang des Arduino-Boards angeschlossen wird.

MPU6050: Accelerometer & Gyroskop

Die stellt wichtige Funktionen zur Initialisierung und Abfragung des Sensors bereit.

Nachdem der Sensor über

acc.initialize();

initialisiert wurde kann er über die Funktion getaccgyro() ausgelesen werden.

In dieser Funktion werden zunächst die Werte aus dem letzten Durchgang gespeichert und anschließend die neuen Werte abgerufen.

for (int i=sampleCount-1; i>0; i--) {
    accx[i] = accx[i-1]; 
    accy[i] = accy[i-1]; 
    accz[i] = accz[i-1]; 
    gyrox[i] = gyrox[i-1];
    gyroy[i] = gyroy[i-1];     
    gyroz[i] = gyroz[i-1];          
}



Die Funktion getMotion6() wird über die oben erwähnte Klasse bereitgestellt. Sie liefert die Werte in Rohform.

acc.getMotion6(accx, accy, accz, gyrox, gyroy, gyroz);


Diese werden in praktische Einheiten umgerechnet und anschließend Tiefpass gefiltert.

accx[0]=lowpassFilter(accx[1], (accx[0]/16.384)+accOffset[0], 0.5); 
accy[0]=lowpassFilter(accy[1], (accy[0]/16.384)+accOffset[1], 0.5);
accz[0]=lowpassFilter(accz[1], (accz[0]/16.384)+accOffset[2], 0.5);
  
gyrox[0]=lowpassFilter(gyrox[1], (gyrox[0]/131)+gyroOffset[0], 0.5); 
gyroy[0]=lowpassFilter(gyroy[1], (gyroy[0]/131)+gyroOffset[1], 0.5);
gyroz[0]=lowpassFilter(gyroz[1], (gyroz[0]/131)+gyroOffset[2], 0.5);



Zum Schluss werden anhand der Beschleunigungswerte die Roll-Pitch-Yaw Winkel berechnet.

rpy[0] = (-1)*(atan2(accz[0],accy[0])-(M_PI/2))-M_PI;  //Roll
rpy[1] = atan2(accz[0],accx[0])-(M_PI/2)+M_PI;                 //Pitch
rpy[2] = atan2(accy[0],accx[0]);                               //Yaw



Gesamter Code:

void getaccgyro(){
  
  // move previous samples
  for (int i=sampleCount-1; i>0; i--) {
    accx[i] = accx[i-1]; 
    accy[i] = accy[i-1]; 
    accz[i] = accz[i-1]; 
    gyrox[i] = gyrox[i-1];
    gyroy[i] = gyroy[i-1];     
    gyroz[i] = gyroz[i-1];          
  }
  
  // refresh values
  acc.getMotion6(accx, accy, accz, gyrox, gyroy, gyroz); 
  
  // transform accelreation values in mg, offset, filter
  accx[0]=lowpassFilter(accx[1], (accx[0]/16.384)+accOffset[0], 0.5); 
  accy[0]=lowpassFilter(accy[1], (accy[0]/16.384)+accOffset[1], 0.5);
  accz[0]=lowpassFilter(accz[1], (accz[0]/16.384)+accOffset[2], 0.5);
  
  // transform angular velocity values in °/s, offset, filter
  gyrox[0]=lowpassFilter(gyrox[1], (gyrox[0]/131)+gyroOffset[0], 0.5); 
  gyroy[0]=lowpassFilter(gyroy[1], (gyroy[0]/131)+gyroOffset[1], 0.5);
  gyroz[0]=lowpassFilter(gyroz[1], (gyroz[0]/131)+gyroOffset[2], 0.5);
  
  // Calculate RPY-Angles
  rpy[0] = (-1)*(atan2(accz[0],accy[0])-(M_PI/2))-M_PI;  //Roll
  rpy[1] = atan2(accz[0],accx[0])-(M_PI/2)+M_PI;         //Pitch
  rpy[2] = atan2(accy[0],accx[0]);                  //Yaw
 
}



Link zum Hersteller:http://www.drotek.fr/shop/en/62-imu-10dof-mpu6050-hmc5883-ms5611.html

HMC5883L: Magnetometer (Kompass)

Für den HMC5883L gibt es ebenfalls eine nützliche Bibliothek, welche das Abfragen der Werte erleichtert.

Das Magnetometer ist nicht direkt an den I2C Bus angeschlossen, sondern indirekt über die MPU6050, welche über einen "Bypass" verfügt und zusätzlich zu den eigenen Sensordaten noch einen externen, 3-achsigen Sensor auslesen kann. Dies geschieht in der sensor_init() Funktion über

acc.setI2CBypassEnabled(true);


Außerdem muss der Sensor selber noch initialisiert werden. Zusätzlich wird festgelegt, dass jeder neue Wert der Durchschnittswert aus 8 Samples ist, d.h. eine Art von Vorfilterung stattfindet.

mag.initialize();
mag.setSampleAveraging(3);
mag.setGain(1);


Das Abfragen der Sensorwerte geschieht über die Funktion getmag().

Erneut werden zunächst die letzten Werte gespeichert und anschließend die neuen Rohwerte mit Hilfe der Funktion getHeading() abgefragt.

for (int i=sampleCount-1; i>0; i--) {
    magx[i] = magx[i-1]; 
    magy[i] = magy[i-1]; 
    magz[i] = magz[i-1];     
  }
mag.getHeading(magx,magz,magy);


Über die atan2 Funktion erhält man aus der y- und x-Komponente des Magnetfeldvektors den Heading-Winkel.
Leider würde diese Methode alleine keine zufriedenstellenden Ergebnisse liefern, da beispielsweise in Tests eine Drehung des Sensorboards um 90° einen Winkel zwischen 50° und 70° zurückgab. Desweiteren ist das Board sehr anfällig für Schwankungen und muss den Sprung der atan2-Funktion beim Übergang von 0° auf 360° erkennen.
Deshalb wurden drei wichtige Korrekturen eingefügt.

Korrektur durch Offset:
Um die fehlerhafte Drehung zu korrigieren wurde manuell ein Offset ermittelt. Dazu wurde der Inertialwert des Sensors, sowie der Wert des um 180° um die z-Achse gedrehten Boards gemessen.
Diese müssen genau komplementär sein, da das Magnetfeld in die andere Richtung gemessen wird. Durch diese Vorgehensweise wurde ein Offsetvektor von {-23,76,140} ermittelt, welcher zum Messwert hinzuaddiert wird. Der Fehler wurde dadurch auf etwa +-3° reduziert. Anschließend wird erneut der Tiefpassfilter angewandt.

magx[0] = lowpassFilter(magx[1], magx[0]+magOffset[0], 0.5);
magy[0] = lowpassFilter(magy[1], magy[0]+magOffset[1], 0.5);
magz[0] = lowpassFilter(magz[1], magz[0]+magOffset[2], 0.5);



Korrektur durch Tilt-Kompensation:
Schwankungen des Sensorboards haben einen erheblichen Effekt auf das gemessene Magnetfeld des Sensors. Daher wurde eine Tilt-Kompensation implementiert, welche über die inverse Rotationsmatrix das Magnetfeld in die Nulllage zurückrechnet.
Eine Schwenkung um die x- bzw. y-Achse wird durch die beiden Winkel Phi und Theta beschrieben und als Roll bzw. Pitch bezeichnet. Diese Winkel wurden bereits mit Hilfe des Beschleunigungssensors ermittelt.
Die neuen x- & y-Werte des Magnetfelds berechnen sich über die inverse Rotationsmatrix und sind im Code wie folgt implementiert:

magTilt[0] = magx[0]*cos(rpy[1])+magy[0]*sin(rpy[0])*sin(rpy[1])+magz[0]*cos(rpy[0])*sin(rpy[1]);
magTilt[1] = magy[0]*cos(rpy[0])-magz[0]*sin(rpy[0]);



Die Berechnung des Winkels erfolgt anschließend über die bereits erwähnte atan2() Funktion.

phi_xy_tilt = atan2(magTilt[1],magTilt[0])*(180.0/M_PI);



Korrektur des Sprungs:
Um den Wertebereich von (0:360) auf (-inf:inf) zu erweitern wurde ein einfacher Trick angewandt. Dazu wird in einer globalen Variable umdrehung die Anzahl der Übergänge gespeichert, anschließend mit dem Faktor 360 multipliziert und zum gemessenen Wert hinzugefügt.
Wird ein Sprung erkannt wird je nach Richtung des Sprungs die Variable umdrehung erhöht, bzw. vermindert.

if(((phi_rel - phi_rel_mem) < -300) && (phi_rel_mem!=0)){
    umdrehung++;
  }
  if(((phi_rel - phi_rel_mem) > 300) && (phi_rel_mem!=0)){
    umdrehung--;
  }



Problem:
Durch die Tilt-Kompensation werden Schwankungen zwar ausgeglichen, jedoch ist dies nur bei geringen Beschleunigungen der Gondel zu empfehlen, da sonst der Beschleunigungssensor die Winkel verfälscht, was zu großen Fehlern führen kann, die sonst nicht entstanden wären. Unter der Annahme, dass unser Zeppelin keine starken Beschleunigungen aufweist, ist der Fehler vernachlässigbar.


Gesamter Code:

void getmag(){
  
  // move previous samples
  for (int i=sampleCount-1; i>0; i--) {
    magx[i] = magx[i-1]; 
    magy[i] = magy[i-1]; 
    magz[i] = magz[i-1];       
  }
  
  // Get raw values from compass
  mag.getHeading(magx,magz,magy);
  
  // Add offset + filter
  magx[0] = lowpassFilter(magx[1], magx[0]+magOffset[0], 0.5);
  magy[0] = lowpassFilter(magy[1], magy[0]+magOffset[1], 0.5);
  magz[0] = lowpassFilter(magz[1], magz[0]+magOffset[2], 0.5);
  
  // Apply tilt compensation  
  magTilt[0] = magx[0]*cos(rpy[1])+magy[0]*sin(rpy[0])*sin(rpy[1])+magz[0]*cos(rpy[0])*sin(rpy[1]);
  magTilt[1] = magy[0]*cos(rpy[0])-magz[0]*sin(rpy[0]);
  
  // Calculate Azimuth
  phi_xy_tilt = atan2(magTilt[1],magTilt[0])*(180.0/M_PI);
  
  //  difference between current heading and initialized heading
  phi_rel = phi_xy_tilt - phi_xy_init;
  
  // clearing jump between 180° and -180°
  if(((phi_rel - phi_rel_mem) < -300) && (phi_rel_mem!=0)){
    umdrehung++;
  }
  if(((phi_rel - phi_rel_mem) > 300) && (phi_rel_mem!=0)){
    umdrehung--;
  }
  
  phi_xy = phi_rel + (umdrehung * 360);
  phi_rel_mem = phi_rel;
//phi_xy = phi_xy_tilt;
  
}



Link zum Hersteller:http://www.drotek.fr/shop/en/62-imu-10dof-mpu6050-hmc5883-ms5611.html

MB1240: Ultraschallsensor

Für die Höhenregelung des Zeppelins wurde der Ultraschallsensor
MB1240 verwendet, da eine exakte Höhenmessung mittels des auf der Drotek IMU impelmentierten Luftdrucksensors nicht möglich war.

Um Sensordaten zu erhalten muss der MB1240 zuerst initialisiert werden. Dies geschieht mit folgender Funktion:

void init_sensors(){
  Serial.println("Initialize Sensors...");
  // initialize US
  pinMode(us, INPUT);
  pinMode(A3, INPUT);
}



Nach der Initialisierung wird dann mit der Funktion getus() die eigentliche Messung vorgenommen und in eine Höhenangabe in Zentimeter umgewandelt.

void getus(){
  // obtain us-value 
  value_us = pulseIn(us, HIGH);
  
  // transform us-value in cm
  height = value_us*(2.54/147)
}



Link zum Hersteller:http://www.maxbotix.com/Ultrasonic_Sensors/MB1240.htm

Motortreiber

Für die Motoransteuerung wurden 2 Motortreiber verwendet, die jeweils 2 Motoren steuern können.
http://www.watterott.com/de/Motor-Driver-1A-Dual-TB6612FNG
Da wir nur 3 Motoren verwenden, bleibt ein Anschluss ungenutzt.

Ein Motortreiber beansprucht neben der Stromversorgung pro Motor 3 Pins des Arduino Boards.
Zwei digitale Pins für die Richtung und einen analogen Pin für die Geschwindigkeit.

Die Richtung unterscheidet dabei zwischen Standby (beide LOW), rechtslauf & linkslauf (einer LOW, einer HIGH) und bremsen (beide HIGH).

Die Geschwindigkeit wird über ein PWM Signal gesteuert, welches das Arduino Board generieren kann.
Die Funktion zur Generierung eines PWM Signals ist analogWrite(Pin, Value), wobei Pin den PWM Pin angibt und Value ein Wert zwischen 0 und 255 ist. Null entspricht einem duty cycle von 0% und 255 einem duty cycle von 100%.
http://arduino.cc/it/Tutorial/PWM

Die Motoransteuerung ist in der Datei Motoren.ino realisiert.

Die Motoren müssen zu Beginn über die Funktion init_motors() initialisiert werden.

Hier werden zunächst jedem Motor die drei Kontrollpins zugewiesen.
Anschließend werden alle Richtungspaare auf (LOW/LOW) gesetzt und die Geschwindkeit auf 0.

void init_motors(){
  Serial.println("Initializing Motors...");
  //initialize pins
  pinMode(pwm_a, OUTPUT);      //motor left
  pinMode(dir_a1, OUTPUT); 
  pinMode(dir_a2, OUTPUT); 
  pinMode(pwm_b, OUTPUT);      //motor right
  pinMode(dir_b1, OUTPUT); 
  pinMode(dir_b2, OUTPUT); 
  pinMode(pwm_c, OUTPUT);      //motor bottom
  pinMode(dir_c1, OUTPUT); 
  pinMode(dir_c2, OUTPUT); 
  //all motors stopp
  analogWrite(pwm_a, 0);        //motor left
  digitalWrite(dir_a1, LOW);
  digitalWrite(dir_a2, LOW);
  analogWrite(pwm_b, 0);        //motor right
  digitalWrite(dir_b1, LOW);
  digitalWrite(dir_b2, LOW);
  analogWrite(pwm_c, 0);        //motor bottom
  digitalWrite(dir_c1, LOW);
  digitalWrite(dir_c2, LOW);
}



Nun können die Motoren über die Funktionen

  • motor_links(int pwm)
  • motor_rechts(int pwm)
  • motor_unten(int pwm)

gesteuert werden.

Die drei Funktionen sind bis auf den Namen und die Pinzuordnung identisch, weshalb hier exemplarisch nur die Funktion motor_links(...) erklärt wird.

Eingang der Funktion ist ein Vorzeichenbehafteter Integer Wert pwm.
Die Drehrichtung wird anhand des Vorzeichens von pwm erkannt.

Wenn pwm<0 ist, wird die Richtung als „negativ“ angegeben, ansonsten ist sie "positiv". Unabhängig davon wird danach mit dem Betrag des Parameters weitergearbeitet.

boolean dir = HIGH;
  if (pwm<0){
    dir = LOW;
    pwm = -pwm;
}



Wenn pwm>motor_max ist, wird der Wert auf motor_max gesetzt, damit der zulässige Grenzwert von 255 als Eingang für die PWM Signal Generierung, bzw. ein vordefinierter, kleinerer Wert nicht überschritten wird.

if (pwm>motor_max){
    pwm = motor_max;
}



Zum Schluss werden die Pins je nach Richtung und Geschwindigkeit belegt, sodass der Motortreiber diese in eine Spannung für den Motor umsetzen kann.

if (dir == HIGH){
    digitalWrite(dir_a1, LOW);
    digitalWrite(dir_a2, HIGH);
    analogWrite(pwm_a, pwm);
  } else {
    digitalWrite(dir_a1, HIGH);
    digitalWrite(dir_a2, LOW);
    analogWrite(pwm_a, pwm);
}



Gesamter Code:

void motor_links(int pwm){
  boolean dir = HIGH;
  if (pwm<0){
    dir = LOW;
    pwm = -pwm;
  }
  if (pwm>motor_max){
    pwm = motor_max;
  }
  if (dir == HIGH){
    digitalWrite(dir_a1, LOW);
    digitalWrite(dir_a2, HIGH);
    analogWrite(pwm_a, pwm);
  } else {
    digitalWrite(dir_a1, HIGH);
    digitalWrite(dir_a2, LOW);
    analogWrite(pwm_a, pwm);
  }
}

 

Regelung

Die Regelung ist in der Datei Regelung.ino realisiert.

Sowohl für die Höhen- als auch für die Lagereglung verwenden wir in unserer Software PID-Regler . Um diese Umsetzen zu können benötigen wir die zwei Funktionen derivation(...) und integral(...).

float derivation(float tm,float tn){
  
  float dt = 1000.0*((tm-tn)/(t_tm-t_tn));
  return dt;
}


float integral(float tm,float tn){
  
  // trapezoidal method 
  float dtau   = ((tm+tn)/2) * (t_tm-t_tn) * 0.001;
    
  return dtau;
}



Die erste Funktion berechnet die Ableitung nach der Zeit. Dazu wird die Differenz aus dem aktuellen Messwert tm und dem vorherigen Wert tn gebildet und durch die zwischen den Messungen vergangene Zeit geteilt.

Die zweite Funktion berechnet das Integral mit Hilfe der Trapezrege . Diese beschreibt ein mathematisches Verfahren, bei dem man das Integral einer Funktion in einem Intervall numerisch annähert. Dazu ersetzt man die Fläche unter der Kurve mit einem Trapez ((tm+tn)/2). Wird die Trapezfläche nun mit der zwischen den Messungen vergangenen Zeit multipliziert erhält man eine Annäherung an das Integral der Funktion.

Höhenregelung


Mit Hilfe eines PID-Reglers wird nun die für die Höhenregelung notwendige Motorgeschwindigkeit berechnet.

void hoehenregelung(){
  // Save values from previous cycle
  h_tn = h_tm;
  // Get current values
  h_tm = height;
  
  // Refresh time value
  t_tm = millis();
  
  // calculate slope
  float h_slope = derivation(h_tm, h_tn);
  float h_int   = integral(h_tm, h_tn);
  
  // calculates difference
  int diff = height-height_init;
  
  // calculates integral
  int sum_h = sum_h + h_int;  
  float f = f + diff;

  // PID-controller
  mspeed_h = H_p * diff + H_i * sum_h + H_d * h_slope;



Die Regelparameter wurden mithilfe eines MATLAB Models, das in Kooperation mit Team 3 entstanden ist, ermittelt. Näheres dazu gibts unter folgendem Link.
MATLAB-Model Höhenregelung
Anschließend wird die vom PID-Regler berechnete Geschwindigkeit an die Motorregelung übergeben.

// transfer to motor-control
  motor_unten(mspeed_h);



Um die Flugbahn möglichst flach zu halten und am Hindernis keine unnötig hohe Abweichung vom Kurs zu haben wird durch folgende einfache if-Verzweigung die anfangs initialisierte Flughöhe angepasst. Dies geschieht einmal wenn das Hindernis vom Ultraschallsensor erkannt wird und ein weiteres mal wenn das Hindernis überflogen ist.

// height control on the obstacle
  // arrival
  if(height <= (height_init-30)){
    height_init = height;
  }
  // exit
  if(height >= (height_init+20)){
    height_init = height;
  }



Da die Reichweite des Ultraschallsensors begrenzt ist, können nicht beliebige Entfernungen gemessen werden. Wird die Reichweite überschritten liefert der MB1240 negative Werte. Dadurch wird von der Regelung ein Höhenverlust detektiert und um ein „vermeintliches“ Abstürzten zu verhindern die Motorgeschwindigkeit des unteren Motors auf die maximal mögliche Geschwindigkeit gesetzt. Das hat zur Folge, dass der Zeppelin solange steigen würde bis der Akku leer ist. Um dieses Szenario zu verhindern haben wir folgenden Fail-Save eingebaut. Dabei gehen wir davon aus, dass es nicht nötig ist höher als 2,5m zu fliegen. Wird diese Grenze überschritten greift der Fail-Save ein und steuert den Blimp auf eine niedrigere Höhe. Signalisiert wird das durch ein Aufleuchten einer roten Kontrollleuchte.

// fail save
  if(height >= 250){
    mspeed_h = 200;
    digitalWrite(led_r,HIGH);
  }

 

Bahnregelung

Die Bahnregelung wurde wie die Höhenregelung mit einem PID-Regler realisiert. Die Regelgröße war in diesem Fall der Heading-Winkel (Azimut) mit einer Stufenfunktion als Sollvorgabe.

Blockschaltbild der Lageregelung
Blockschaltbild der Lageregelung



Die Stufenfunktion ermöglicht es den Zeppelin in einem n-Eck durch den Parkour fliegen zu lassen. Hierfür wurden die Parameter der Stufenfunktion durch Ausprobieren ermittelt.
Image

Anhand der folgenden Abbildung kann man das PT1-Verhalten der Regelstrecke erkennen und wie sich der Zeppelin innerhalb des gegebenen Zeitfensters optimal auf Kurs regelt.

Vergleich von Soll und Ist Verhalten anhand eines Testflugs
Vergleich von Soll und Ist Verhalten anhand eines Testflugs



Die gewählte Art der Regelung über den Heading-Winkel funktioniert zwar, hat jedoch noch reichlich Verbesserungspotential und ist als Regelungskonzept nur für statische Parkoure zu empfehlen. Es gibt keine Möglichkeit zur Reaktion auf Störungen oder dynamisches Verhalten im Parkour.

Die Bahnregelung ist in der Funktion heading_controller(...) realisiert.

Zunächst wird das letzte Wertepaar (Zeit/Winkel) gespeichert und anschließend mit den zuvor erwähnten Funktionen derivation(...) und integral(...) die Ableitung und das Integral ermittelt.

a_tn = a_tm;
a_tm = phi_xy;

t_tm = millis();
  
float adt = derivation(a_tm, a_tn);

float adtau = integral(a_tm, a_tn);
float sum_l = sum_l+adtau;

float diff = phi_xy;



Nun hat man alle für die Regelung notwendigen Werte und kann die neue Motorgeschwindigkeit ermitteln.

float motorspeed = L_p * diff + L_i * sum_l + L_d * gyroz[0];



Diese wird nun zu einer Grundgeschwindigkeit der beiden Motoren dazuaddiert, beziehungsweise abgezogen, sodass die beiden Motoren gegenläufig arbeiten und der Zeppelin trotzdem vorwärts fliegt statt sich nur im Kreis zu drehen.

mspeed_l = geschw - motorspeed/2;  // variable for motor-speed left
mspeed_r = geschw + motorspeed/2;  // variable für motor-speed righ

motor_links(mspeed_l);
motor_rechts(mspeed_r);



Gesamter Code:

void heading_controller()
{ 
  // Save values from previous cycle
  a_tn = a_tm;
  // Get current values
  a_tm = phi_xy;

  // Refresh time value
  t_tm = millis();
  
  // calculate slope
  float adt = derivation(a_tm, a_tn);
  float adtau = integral(a_tm, a_tn);
  
  float diff = phi_xy;
  float sum_l = sum_l+adtau;
  
  // PID-controller  
  float motorspeed = L_p * diff + L_i * sum_l + L_d * gyroz[0];
    
  // transfer to motor-control
  mspeed_l = geschw - motorspeed/2;  // variable for motor-speed left
  mspeed_r = geschw + motorspeed/2;  // variable für motor-speed righ
  
  // calls motorfunctions
  motor_links(mspeed_l);
  motor_rechts(mspeed_r); 
  
}

 

Matlab/Simulink

Höhenregelung

Image
Für die Parametrierung der Höhenregelung wurde ein Zeppelinmodell mittels Simulink erstellt. Anhand dieses Modells wurden die Parameter für den PID-Regler berechnet. Da es sich hierbei allerdings um ein vereinfachendes Modell handelt - so wird beispielsweise von einer Punktmasse ausgegangen - können die berechneten Reglerparameter nicht eins zu eins übernommen werden, sondern dienen lediglich als Startwerte für die Feinjustierung.

Sensorauswertung

Zum Auswerten der Sensordaten wurde mit Matlab eine Schnittstelle zur Gondel und daraus eine GUI entwickelt, die es sowohl erlaubt, 4 verschiedene Sensordaten live zu plotten, als auch mit dem Datenlogger aufgezeichnete Messwerte graphisch darzustellen, auch hier bis zu 4 Sensordaten gleichzeitig.
Um aufgezeichnete Daten plotten zu können, müssen die einzelnen Sensordaten durch Kommata getrennt, in einer csv-Datei gespeichert und in folgender Reihenfolge angeordnet sein:
time,Acc_X,Acc_Y,Acc_Z,Gyro_X,Gyro_Y,Gyro_Z,Mag_X,Mag_Y,Mag_Z,compass,altitude

Image

Um die Daten plotten zu können, muss man zuerst unter Verwendung des SELECT-Buttons die gewünschte csv-Datei auswählen. Anschließend können bis zu 4 verschiedene Sensordaten, Farbe, Linienart durch die drei hierfür vorgesehenen Popup-Menüs gewählt werden.

Für den Liveplot muss zuerst der Arduino über den Programmieradapter an den PC angeschlossen werden. Anschließend muss dann der Auswahl-Button Liveplot gewählt und über das Popup-Menü darunter der richtige COM-Port eingestellt werden. Im Folgenden können wieder unten die gewünschten Sensoren, Farben und Linienstil eingestellt werden. Durch einen Klick auf START wird der Plot gestartet. Mit STOP kann der Liveplot angehalten werden um einen anderen Sensor zu wählen.

Um das Programm an verschiedene Reihenfolgen oder unterschiedliche Sensorwerte anzupassen, oder Farben und Linienstile zu ändern, können im Code folgende Arrays modifiziert werden:

sensors_gui = {'Time', 'AccX', 'AccY', 'AccZ', ... };
sensors_plot = {'Time/ms', 'AccX/mG', 'AccY/mG', 'AccZ/mG',  ... };

linestyles_gui = {'.','o','x','+','*', ... };
linestyles_plot = {'.','o','x','+','*', ... };

colors_gui = {'black','blue','cyan','green','magenta','red','yellow'};
colors_plot = {'black','blue','cyan','green','magenta','red','yellow'};



Die Arrays ..._gui enthalten grundsätzlich die Bezeichnungen, die in der GUI verwendet werden, die Arrays ..._plot die für MATLAB verständlichen Bezeichnungen.
Das Array sensors_gui und sensors__plot muss dabei die Bezeichnungen für die Sensoren in der richtigen Reihenfolge enthalten. Die Werte des Arrays sensors_plot werden beim Plotten in der Legende angezeigt, das bietet z.B. die Möglichkeit, zusätzlich die Einheit anzugeben.
Die Arrays stehen im Code ab Zeile 77.

Weitere Informationen zu dem Programm finden sich auch in der HeliumHeroesGUI Doku.

Inbetriebnahme

  • Akku vollständig laden
  • Überprüfen ob sich der richtige Programmcode auf dem Arduino befindet, falls nicht: Code hochladen
  • Gondel und Sensorboard - an den dafür vorgesehenen Klettstreifen - am Zeppelin befestigen
  • einschalten

Downloads & Links

Helium Heroes Programmcode

Der komplette Programmcode kann unter nachfolgendem Link heruntergeladen werden:
Helium_Heroes.zip

Matlab

GUI zum Auswerten der Sensordaten

Die GUI zum Auswerten der Sensordaten gibts hier:

Modell Höhenregelung

Das Modell kann unter nachfolgendem Wert runtergeladen werden:
Höhenregelung
Informationen unter:
MATLAB-Model Höhenregelung

Softwaretools

doxygen

ToDoList