Automatischer Lageausgleich Quadrocopter

Hi Leute,
arbeite gerade an meinem Projekt und zwar will ich einen Quadrocopter bauen, der in der Luft selbstständig seine Lage halten kann.
Das ganze soll hardwaretechnisch mit einem Gyro-Sensor realisiert werden.
Allerdings weiß ich noch nicht wie ich das softwaretechnisch realisieren soll.
Der Brushlessmotor wird ja wie ein Servo angesteuert. und hat daher verschiedene Geschwindigkeitsstufen.
Meine Idee wäre jetzt gewesen, dass wenn der Gyrosensor eine Veränderung bemerkt die ensprchenden Motoren eine Stufe hochgestellt werden.
Was meint ihr wird das funktionieren oder wird der dann sehr wild schwanken?

Danke für eure Hilfe schonmal im Vorraus

Ps: das ganze soll mit einem Arduino Mega realisiert werden.

..wird nicht funktionieren *mein(weis....)
Wird runterfallen.
Hoffentlich keinem auf den Kopf.

Sorry aber: mit so völlig überhaupt keiner Ahnung kanns einfach nichts werden.

Die brauchst wie gesagt einen vernünftigen Regler (Stichwort: PID). Ein einfacher P-Regler wie du da beschreibst schwingt immer.

Rabenauge:
..wird nicht funktionieren *mein(weis....)
Wird runterfallen.
Hoffentlich keinem auf den Kopf.

Wenn die Regelung eines Quadrocopter nicht funktioniert bekommt man ihn gar nicht in die Luft. Die Motorsteuerung ist das ein und alles und eine manuelle Steuerung, wie bei anderen Fluggerät, gibt es beim Quadrocopter nicht. Die Steuerung wird immer vom Kontroller gemacht und die Fernsteuerung und der Pilot gibt nur die Richtungen an. Eine falsch eingestellte Regelung äußert sich darin daß der Quadrocopter 20 cm aufsteigt und sich auf den Kopf landet. Eine Lageregelung ist kein netter Zusatz sonder der Kern der Steuerung des Quadrocopter.

Sorry aber: mit so völlig überhaupt keiner Ahnung kanns einfach nichts werden.

Das kann ich leider nur voll zustimmen.

Grüße Uwe

Es gibt ausgereifte Module in allen Preisklassen für so etwas. Wieso immer das Rad zwei-, drei-, .... mal erfinden?

Da ich mich ja auch mit dem Thema befasse kann ich dir diese Seite mal empfehlen Improving the Beginner’s PID – Introduction « Project Blog du kannst auch die PID lib vom Arduino nehmen.

Hier ein Beispiel:

#include <PIDCont.h>
#include <Wire.h>
#include "Kalman.h"
#include <Servo.h>

int arm = 15;
String incomingString;

PIDCont PIDroll, PIDpitch;
int Throttle = 30;

#define ROLL_PID_KP  0.08      //0.09
#define ROLL_PID_KI  0      //.2  //0.09
#define ROLL_PID_KD  0.002
#define ROLL_PID_MIN -15.0
#define ROLL_PID_MAX  15.0

Servo motors[4];
#define FRONTL 0
#define FRONTR 1
#define BACKL  2
#define BACKR  3

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

void setup(){
  Serial.begin(9600);
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;

  timer = micros();
  
    //                          Kp,        Ki,         Kd           Lval         Hval
  PIDroll.ChangeParameters(ROLL_PID_KP,ROLL_PID_KI,ROLL_PID_KD,ROLL_PID_MIN,ROLL_PID_MAX);
  PIDpitch.ChangeParameters(ROLL_PID_KP,ROLL_PID_KI,ROLL_PID_KD,ROLL_PID_MIN,ROLL_PID_MAX);
  
  
  motors[FRONTL].attach(2);
  motors[FRONTR].attach(3);
  motors[BACKL].attach(4);
  motors[BACKR].attach(5);
  setSpeed(FRONTL, arm);
  setSpeed(FRONTR, arm);
  setSpeed(BACKL, arm);
  setSpeed(BACKR, arm);
  delay(3000);
}

void loop(){ 
      // If there is incoming value
    if(Serial.available() > 0){
        // read the value
   char ch = Serial.read();
   
   if (ch != 10){
     incomingString += ch;
    }
    else{
      // Convert the string to an integer
      int valin = incomingString.toInt();
    
   
   if(valin >=100){
     valin = 100;}
     
   if(valin <=30){
     valin = 30;}
     Serial.println(valin);
     // Reset the value of the incomingString
     Throttle = valin;
      incomingString = "";
    }
    }
    
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
  
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  
  double gyroXrate = (double)gyroX / 131.0;
  double gyroYrate = -((double)gyroY / 131.0);  
  
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000);
  timer = micros();
  
  kalAngleY = (179.2 - kalAngleY);
  kalAngleX = (179.2 - kalAngleX);
  
  int PIDroll_val=  (int)PIDroll.Compute( (float)kalAngleY);
  int PIDpitch_val= (int)PIDpitch.Compute((float)kalAngleX);
  
  //                           yyy          xxx
  int m0_val=(Throttle+(  PIDroll_val -PIDpitch_val));
  int m1_val=(7+Throttle+( -PIDroll_val +PIDpitch_val));
  int m2_val=(2+Throttle+( -PIDroll_val -PIDpitch_val));
  int m3_val=(2+Throttle+(  PIDroll_val +PIDpitch_val));
  
  setSpeed(FRONTL, m0_val);
  setSpeed(BACKR,  m1_val);
  setSpeed(FRONTR, m2_val);
  setSpeed(BACKL,  m3_val);
}

void setSpeed(int motor, int speed)
{
  int fakeAngle = map(speed, 0, 100, 0, 180);
  motors[motor].write(fakeAngle);
}

Mit dem Kalman-Filter werden die Rohdaten vom Accelerometer und Gyro zusammengefasst und ausgebessert, da ja auch das Acceleromter abdriftet je länger es arbeitet und der Gyro nicht Schockresistent ist.

phthalo:
Es gibt ausgereifte Module in allen Preisklassen für so etwas. Wieso immer das Rad zwei-, drei-, .... mal erfinden?

Muss ja nicht neu erfunden werden. Aber ein Modul muss es nicht zwingend sein. Plane mit dem Mega auch soweit eine Platine zum Aufstecken. Bei den meisten Boards fehlten mir zu viele Pins, oder lagen total am falschen Ende. Ich will bei meinem QC keinerlei Loopings fliegen. Des weiteren werde ich den nur im X-Mode fliegen. 6 Rotoren werde ich auch nicht anschließen. Aber ich möchte ein paar Anschlüsse für Led-Strips freihalten.

Danke für die Antworten. Könnt ihr mir noch einen Gyrosensor empfehlen?

Also ich würde dir ein 10DOF Board empfehlen, da es alle wichtigen Sensoren enthält, die du wahrscheinlich brauchen wirst. Hier ein Link http://cgi.ebay.de/ws/eBayISAPI.dll?ViewItem&item=201039555354&var=500235697912

Hallo,
Hatte vor 1,5 Jahren sehr ähnliche Gedanken. Nach mehreren verschiedenen Boards (7) bin ich bei dem Crius AIO PRO gelandet. Als Software die Multiwii auf Arduino Basis. Will Dich aber nicht bremsen, vielleicht programmierst Du eine Alternative zur Multiwii.
Ansonsten meine Empfehlung: MPU6050 als Gyro und Beschleunigungssensor. Beschleunigungssensor wirst Du brauchen, sonst wird das mit dem automatischen Lageausgleich nicht funktionieren. Ebenfalls empfehlenswert ist ein 3 Achsen Magnetometer (entsprechende Abschirmung vorausgesetzt).
Der MPU6050 ist nM besser als die adxl oder die ausgebauten Teile aus den Nunchucks/Multiwii bzw. der bma020.
Und wenn Du keine Loopings fliegen willst, sondern ruhiger wird ein Baro Sensor nicht schaden. Der MS5611 Baro hat bei mir besser als der BMP085 funktioniert.
GPS kannst Du später nachrüsten, dabei solltest Du aber nicht sparen.
Die LED Strips werden bei mir über die 16,8V direkt versorgt, solange ich fliege. Warum willst Du sie schalten?

Voralpenkreuz:
Die LED Strips werden bei mir über die 16,8V direkt versorgt, solange ich fliege. Warum willst Du sie schalten?

Schalten? Nein, ich will die steuern. Sind WS2812B auf eigener Platine. Zwischen Vcc und den Strips kommt dann entsprechend dimensiorniert StepDown Wandler.

Bei mir sind es normale 5050 LED-Strips die von den Vorwiderständen her eigentlich auf 12V ausgelegt sind. Zweck: Lageerkennung
Was hast Du vor?

Da ich relativ viele “Kanäle” auf meiner Fernsteuerung habe, möchte ich enstprechend gerne ausschalten. Einige Sachen kann ich aus anderen Projekten von mir gut übernehmen.

Diese Modis hab ich mir bislang vorgestellt:

  • Lageerkennung
  • Motorendrehzahl
  • Beschleunigung vorwärts/rückwärts (Das soll so ein bisschen dann aussehen wie bei einem Raumschiff, wenn der Warpantrieb gezündet wird. Sprich leichtes flimmern, was je nach Beschleunigung heller wird.)

Beschleunigungssensor wirst Du brauchen, sonst wird das mit dem automatischen Lageausgleich nicht funktionieren.

Was misst der Beschleunigungssensor dann genau?

Ebenfalls empfehlenswert ist ein 3 Achsen Magnetometer (entsprechende Abschirmung vorausgesetzt).

Ein Magnetometer misst doch die Magnetische Flussdichte. Was hat das damit zu tun?

Und wenn Du keine Loopings fliegen willst, sondern ruhiger wird ein Baro Sensor nicht schaden.

Mit einem Barometer kann ich ja die Höhe anhand des Luftdrucks feststellen. Inwiefern soll das bei der Stabilisierung des Copters helfen?

Was misst der Beschleunigungssensor dann genau?

Die Erdbeschleunigung, dh er mißt wo unten ist.

Ein Magnetometer misst doch die Magnetische Flussdichte. Was hat das damit zu tun?

Du meinst Kompaß?

Mit einem Barometer kann ich ja die Höhe anhand des Luftdrucks feststellen. Inwiefern soll das bei der Stabilisierung des Copters helfen?

Willst Du ihn nicht in der Luft schweben lassen? Dazu gehört auch die Höhe zu halten.

Grüße Uwe

Ok danke für die Antworten

Ich nehme den MPU9150. Gleiche Lib wie MPU6050 aber mit 9 Achsen.
Ich bin auch schon seit vielen Monaten am Werkeln. Ich möchte das Rad zwar nicht neu erfinden, aber auch nichts einfach kopieren. Es kann sogar sein, wenn das Ding fertig ist und endlich fliegt, für mich nicht mehr interessant ist. Aber der Weg dahin war mehr als interessant. Wenn man hier im Forum richtig fragt, und bereit ist selbst was dazu zu lernen, bist Du hier richtig. Musste ich auch lernen.
Die Lagekontrolle ist das A und O beim Quadcopter. Ich versuche seit Wochen meinen PID v1.0.1 zu justieren. Leider noch kein Erfolg. Habe mir dazu nun eine Wippe gebaut, um den PID zu verstehen. Ist für Seiteneinsteiger ein hartes Brot. Ich finde leider kein Quadrocopter-Projekt, das diesen Regler nutzt. Frage: Kann man diese PID-Lib zur Lagebestimmung eines Quadcopter nehmen?
Gruß Kucky
(immer noch Newbie)

Denke schon dass das klappt.
Ich benutz sie in nem kleinen Segway und die tut ihren Job recht gut.
Zum justieren (der Tipp ist von Volvodani) lohnt es sich, mal einige Potis anzuschliessen, und das Programm so zu modifizieren, dass du wenigstens Kp und Ki mal direkt am Gerät verstellen kannst. So findet man zumindest sehr fix brauchbare Werte.
Zudem kann man die PID-Regelung noch weiter aufbohren, indem man bei geringen Abweichungen sanftere Parameter nutzt.

Super Idee, das mit den Potis. Werde ich wohl umsetzen. Ich mache das derzeit mit einem kleinen Sketch, der Tasteneingaben in 0.1er Schritten erledigt.
Gruß Willi

Hallo Rabenauge,
Kannst du mir vielleicht den Link, wo volvodani dies beschreibt, zusenden? Ich muss ja einen Bereich von 0 bis 1023 in einen von 0 bis z.B. 20 umrechnen. Dies ist aber ein Float. Mit Map geht das nicht, und 20/1023*x ist wohl auch nicht richtig.
Gruß Willi