Quadrocopter

[Bluesmash]

Hallo Zusammen

Seit den Weihnachtsferien baue ich nun an einem Quadrocopter. Programmiert habe ich alles selber in Bascom.
Heute bin ich das erste mal richtig damit geflogen und möchte euch nun mein Projekt vorstellen.

Eckdaten:
- Atmel Atmega328
- Programmiert in Bascom
- Beschleunigungssensor/Gyro: MPU6050
- Motoren: Multistar MT2213
- Propeller: 10x4.5
- ESC: Afro 20A, angesteuert über I2C
- Akku: Turnigy nano-tech 3s 3000mAh
- Steuerung: Spektrum DX6i
- Empfänger: OrangeRx R410X

Nun habe ich aber noch folgendes Problem:
Auf der Wippe Funktioniert der ACC-Mode relativ gut, die Horizontale Lage wird relativ schnell ausgeregelt.
Im Flug dauert es aber eine gefühlte Ewigkeit bis er nach dem loslassen des Steuerknüppels wieder Horizontal steht (im ersten Video ist es ein wenig ersichtlich).
Mir ist klar, dass die Querbeschleunigungen im Flug die ACC-Winkel beeinflussen. Ich verwende einen einfachen Komplementärfilter. Habt ihr da eine Idee wie man das noch optimieren kann? Die Beschleunigungssensoren habe ich auf +/-2g Eingestellt und in der Software auf +/-1g begrenzt.


Bilder und Videos sagen mehr als Worte und fragt einfach was ihr noch wissen möchtet.



Test auf der Wippe:
(In der ersten Hälfte ist der innere Regelkreis nur mit den Gyros zu sehen, danach die ACC Regelung)




Hier 2 Videos vom ersten Flug:

[Che Guevara]

Hi,

gefällt mir sehr gut, dein Quadrocopter! Hast du die Haube selbst gemacht oder gekauft?
Zu deinem Problem:
Mit welcher Frequenz läuft den der Komp.Filter und die Sensor-Auswertung? Welche Koeffizienten hast du dafür bestimmt?
Wie ist der Regler aufgebaut? P, PI, PD, PID? Kaskadiert oder "Normal"?
Evtl willst du auch dein Programm herzeigen, da werden viele Sachen ersichtlich.
Das langsame nachziehen kann man leider (zumindest ich) eher schlecht erkennen, evtl kannst du nochmal ein Video machen, wo man es deutlicher sieht?

Gruß
Chris

[Bluesmash]

Hallo Chris

Die Haube habe ich aus der Bastelecke, da gibt es solche Plexiglaskugeln die aus 2 Hälften bestehen.
Der Rest ist selbst gemacht. Der Grundrahmen besteht aus Karbon Rohren und Aluminium Teilen die mir ein Freund gedreht hat. Die Rohre sind mit 2K-Epoxid eingegklebt.

Übrigens, auf dem Quadrocopter habe ich noch ein Raspberry Pi A+ mit Kamera verbaut. Mit dem PI möchte ich einen Access-Point aufbauen und darauf soll ein Webserver für die Bedienung der Kamera laufen... Das ist meine nächste Baustelle

Alle 5ms wird der Sensor ausgelesen, die Winkel berechnet und die PID-Regler aufgerufen.
Der Komplementär Filter läuft mit 0.98*Gyro und 0.02*ACC.
Den Regler habe ich als Kaskade aufgebaut. Der innere Regelkreis (PID) Regelt nur auf die Gyro-werte und der äussere (PI) Regelt dann den Winkel. Im Video auf der Wippe ist es gut ersichtlich (In der ersten Hälfte läuft nur der Gyro-Regler und in der zweiten wird der äussere Regler hinzugeschaltet).

Auf dem Video ist es relativ schwierig zu erkennen, da man nicht sieht wann ich den Knüpel los lasse...
Am besten sieht man es im ersten Video ab 0:12 2mal nacheinander. Da gebe ich ihm nur einen kleinen "Schupf", in dem Moment wo er kurz zurückregelt lasse ich den Knüppel los und er fliegt schräg weiter.

Hier mal der relevante Teil des Codes vielleicht verstehts du etwas ich habe leider nur wenig dokumentiert...

   'Sensordaten einlesen
   Gosub MPU6050_read

   'Gyrowinkel berechnen
   'Offset abziehen
   Gyr_X = Gyr_X - Gyr_X_Offset
   Gyr_Y = Gyr_Y - Gyr_Y_Offset
   Gyr_Z = Gyr_Z - Gyr_Z_Offset
   'Winkel X-Achse
   Ist_Gyr_X = Gyr_X / 32.8
   Temp_s = Ist_Gyr_X * 0.005
   Winkl_Gyr_X = Temp_s
   'Winkel Y-Achse
   Ist_Gyr_Y = Gyr_Y / 32.8
   Temp_s = Ist_Gyr_Y * 0.005
   Winkl_Gyr_Y = Temp_s
   'Winkel Z-Achse
   Ist_Gyr_Z = Gyr_Z / 32.8
   Temp_s = Ist_Gyr_Z * 0.005
   Winkl_Gyr_Z = Temp_s

   'Beschleunigungen auf +/- 1g begrenzen    
   If ACC_X > 17000 Then ACC_X = 17000
   If ACC_X < -17000 Then ACC_X = -17000
   If ACC_Y > 17000 Then ACC_Y = 17000
   If ACC_Y < -17000 Then ACC_Y = -17000
   If ACC_Z > 17000 Then ACC_Z = 17000
   If ACC_Z < -17000 Then ACC_Z = -17000

   'ACC Winkel berechnen X-Achse
   Temp2_s = ACC_Y
   Temp_s = ACC_X * ACC_X
   Temp3_s = ACC_Z * ACC_Z
   Temp_s = Temp_s + Temp3_s
   Temp3_s = sqr(Temp_s)
   Temp_s = atn2(Temp2_s , Temp3_s)
   Winkl_ACC_X = rad2deg(Temp_s)
   'ACC Winkel berechnen Y-Achse
   Temp2_s = ACC_X
   Temp_s = ACC_Y * ACC_Y
   Temp3_s = ACC_Z * ACC_Z
   Temp_s = Temp_s + Temp3_s
   Temp3_s = sqr(Temp_s)
   Temp_s = atn2(Temp2_s , Temp3_s)
   Temp_s = Temp_s * -1
   Winkl_ACC_Y = rad2deg(Temp_s)

   'Komplementärfilter 0,98*Gyro + 0.02*ACC
   'X-Achse
   Temp_s = Winkel_X + Winkl_Gyr_X
   Temp2_s = Temp_s * Kompl_Faktor
   Temp_s = 1 - Kompl_Faktor
   Temp3_s = Winkl_ACC_X * Temp_s
   Winkel_X = Temp2_s + Temp3_s
   'Y-Achse
   Temp_s = Winkel_Y + Winkl_Gyr_Y
   Temp2_s = Temp_s * Kompl_Faktor
   Temp_s = 1 - Kompl_Faktor
   Temp3_s = Winkl_ACC_Y * Temp_s
   Winkel_Y = Temp2_s + Temp3_s

   'Trimm Wert verrechnen
   Winkel_X_T = Winkel_X + Winkel_X_Trimm
   Winkel_Y_T = Winkel_Y + Winkel_Y_Trimm

[Che Guevara]

Hi,

also 5ms funktionieren zwar, aber ich würde versuchen, es auf 2ms oder gar 1ms runterzuschrauben. Die integration der Sensoren läuft dadurch besser und das Flugbild wird insgesamt noch etwas smoother, wobei er jetzt auch schon sehr gut fliegt!
Ich hab in meinen Koptern nen MPU6000 verbaut, der hat SPI und ist dementsprechend wesentlich schneller in der Auswertung, für 7 Register brauche ich gerade mal ein paar µs (hab den Wert nicht im Kopf, denke so ca 30).
Zum einen fällt auf, dass du keine Rotationsmatrix programmiert hast (oder sie nicht gezeigt hast). Zum anderen kommen mir die Komp.Filter Faktoren etwas falsch vor und errinnern mich an ein Paper, das immer mal wieder aufgegriffen wird
Ich hab bei meinem Filter (1kHz) einen Wert von 0.00028 für den Acc.
Die Werte auf 1g begrenzen würde ich nicht, da können u.U. sehr blöde Sachen rauskommen! Außerdem würde ich den Sensor auf +-4g stellen.
Wenn, dann würde ich den Komplementärfilter überspringen, wenn z.b. die Gesamtbeschleunigung mehr als 0.5g von 1g abweicht.

Gruß
Chris

EDIT:
Achja, hast du die ACC Sensoren auch kalibriert? Dein

   'Trimm Wert verrechnen
   Winkel_X_T = Winkel_X + Winkel_X_Trimm
   Winkel_Y_T = Winkel_Y + Winkel_Y_Trimm

sieht nicht danach aus
Du musst den Sensor 6x drehen und jeweils die max Werte bestimmen, daraus dann Offset & Range berechnen und bei jedem Auslesen dann mit diesen Werten verrechnen.

[Bluesmash]

Hallo Chris

Vielen Dank für deine Antworten.

Momentan bin ich bei ca. 3,8ms für einen Durchlauf. Eventuell kann ich dies noch ein wenig optimieren... Potential wäre auf jedenfall noch da
Das kommt dann aber erst wenn ich mit allem anderen zufrieden bin...

Ich weiss ja nicht einmal was eine Rotationsmatrix ist...
Dann glaube ich nicht, dass ich diese programmiert habe Kannst du mir da weiterhelfen?

Ja die Kompl. Faktoren habe ich irgendwo so übernommen, und da es auf der Wippe gut funktionierte sah ich keinen Grund diese zu ändern... aber 0.00028 ist ja extrem klein... wie hast du diesen ermittelt?
Hmmm... ein kleinerer Faktor hätte auch einen kleineren Einfluss auf die kurze Querbeschleunigung, ich denke das hilft mir weiter.

Also du meinst, wenn ACC_X+ACC_Y+ACC_Z > 1,5g oder < 0,5g nur noch den Gyro-Winkel integrieren und den ACC-Winkel ignorieren?
Das hört sich gut an!

Der Code Teil ist nur für die Trimmung...
Beim Start bestimme ich den Offset des Gyros (Mittelwert aus 500 Messungen) sollte man den ACC auch kalibrieren?

gruss Bluesmash

[Che Guevara]

Hi,

ich bastel seit ein paar Jahren (glaub 2011 oder 2012) an meinem Kopter, hab angefangen mit Mega328P, dann AtXMega, jetzt STM32F4.
Solange du "nur" normal fliegen willst, also ohne Kompass, GPS & Höhe, solltest du LOCKER auf 1ms kommen, weniger ist mehr
Wichtig ist, dass du Divisionen vermeidest und auch floats, solange du keine FPU hast (was dein Prozessor nicht hat).
Divisionen lassen sich am besten mittels Shift ersetzen, da muss man dann ggf einwenig die Variablen skalieren, dann läufts wesentlich schneller.
Eine Rotationsmatrix (oder auch Drehmatrix) ist eine Matrix, mit der man einen Vektor drehen kann. In deinem Fall musst du die Komplementärfilter Winkel um einen bestimmten Winkel (aus GyroZ bestimmt) drehen. Schau mal bei Wikipedia, da ist das gut erklärt. Da deine Resourcen aber beschränkt sind und der Kopter sowieso nicht allzu schnell sich drehen wird, reicht da auch die Kleinwinkelnäherung.
Gedankenexperiment:
Du nickst 20° nach vorne (und bleibst da), yaw'st dann um 90° nach links (GUZ). Wie stehst du dann?

Ermittelt hab ich den Wert durch ausprobieren, ist über die Jahre so gewachsen Hab aber auch einen Kalmanfilter programmiert, der gibt sehr sehr ähnliche Ergebnisse aus, also denke ich mal, dass es ein sehr guter Wert ist, im Flug isses auch super!
Du musst die Länge des Vektor bestimmen, also

TotalForce = sqrt(AccX*AccX + AccY*AccY + AccZ*AccZ)
Diff1G = 1 - TotalForce
Diff1G = abs(Diff1G)

'Hier Winkel integrieren

If Diff1G < 0.5 Then 'nur anwenden, wenn Gesamtbeschleunigung nahe 1g
   'Komplementärfilter
End If

Den Acc musst du nur einmalig kalibrieren, den Gyro bei jedem Start, das machst du schon gold-richtig.

Gruß
Chris

[Bluesmash]

Hallo Chris

Zu deinem Gedankenexperiment:
Dann schaut der Copter nach links mit 20° Nick...?? Aber das möchte ich ja?
Wird die Drehmatrix verwendet, dass die Front des Copters immer in die selbe Richtung zeigt? Egal wie ich ihn um Yaw drehe?
Wenn ich z.B. den Knüppel nach vorne drücke, dass er immer von mir weg Fliegt? Egal wie er in der Yaw-Achse steht?

Ich habe mal den Komplementärfilter mit 0.002 versucht. Das funktioniert aber gar nicht...
Solange ich den Copter nur um eine Achse kippe und dann wieder Horzontal auf den Tisch stelle stimmen die Winkel.
Wenn ich jetzt aber den Copter "rühre" z.B. 30° Nick danach 30° Roll, -30° Nick, -30° Roll und Ihn wieder auf den Tisch stelle, steht der Nick Winkel z.B. auf 10° und Kompensiert sich ganz langsam wieder auf 0°
Das kommt von der Gyro integration wenn gleichzeitig um alle Achsen gekippt wird stimmen die Winkel überhaupt nicht mehr, ist das eigentlich normal? oder habe ich hier noch einen Fehler?


Gruss Bluesmash

[Che Guevara]

Hi,

nein, da denkst du falsch
Der Kopter steht dann mit Nick bei 0° und Roll bei 20° nach rechts.
Überleg mal:
Wenn du den Kopter, wenn dieser nicht waagerecht ist, um die Yaw Achse drehst, verändert sich dessen Winkel (wie beim Gedankenexperiment). Der Acc bekommt diese Veränderung mit, die Gyros aber nicht, weil sie nur die Drehung um die Yaw Achse registrieren. Folge: Es dauert eine ganze Weile, bis sich die Komplementärfilter Winkel wieder an die Acc Winkel angleichen.
Du musst also die Gyro-Drehungen (welche im Körpereigenen Koordinatensystem "stattfinden") in das gleiche Bezugssystem wie die Acc Winkel bringen, welche im Erdkoordinatensystem arbeiten.
Hier hab ich mal einen Code für dich, kann sein, dass du manche Vorzeichen wechseln musst:

GyroZSin = Sin(GyroZ*0.01745) 'Drehung um Yaw Achse von Deg zu Rad konvertieren
GyroZCos = Cos(GyroZ*0.01745)

'Drehmatrix
KompFilterXTmp = (GyroZCos * KompFilterX) - (GyroZSin * KompFilterY)
KompFilterYTmp = (GyroZSin * KompFilterX) + (GyroZCos * KompFilterY)

'Werte zurückschreiben
KompFilterX = KompFilterXTmp
KompFilterY = KompFilterYTmp

Die Variablen KompFilterXYTmp brauchst du, damit nach der ersten Zeile der Drehmatrix die Variable KompFilterX noch gleich bleibt, weil der Wert noch in der zweiten Zeile gebraucht wird.
Die grundsätzliche Formel stimmt, es kann aber sein, je nachdem, in welche Richtung deine Winkel ausgelegt sind (also ob Nick positiv vorne oder hinten, Roll positiv vorne oder hinten, Yaw positiv UZ oder GUZ).
Dein oben beschriebenes Verhalten, der Kopter steuere immer von dir weg (wie zb. MK Carefree), wird damit nicht erreicht!
Um das zu realisieren braucht man zwar auch eine Rotationsmatrix, aber auch einen Kompasssensor, welchen du glaube ich nicht verbaut hast?

Ich denke, dein beschriebenes Verhalten (rühren) hängt mit der fehlenden Rotationsmatrix zusammen. Probier das mal aus und berichte dann.

Gruß
Chris

EDIT:
Achja, deine Homepage funktioniert irgendwie nicht?

[Bluesmash]

Vielen Dank für deine Hilfe!

Irgendwie macht es bei mir nicht "klick" ;(
Wenn ich doch um die Yaw-Achse drehe, ist der einzige Sensor der eine Änderung feststellt, der Gyro von der Yaw-Achse?
Der Copter behält ja seinen Winkel (einfach um die Yaw-Achse gedreht) und somit bleiben doch die Werte der ACC gleich, oder?

Ah ja danke, ich war schon seit jahren nicht mehr aktiv hier, ich muss mir noch nen neuen Platz für meine Homepage suchen...

Gruss Bluesmash

[Che Guevara]

Hi,

ja, wenn die Platine / der Kopter / Sensor waagrecht ist, ändert sich nur die Yaw-Achse.
Wenn er aber schief steht und du dann um Yaw drehst, verändern sich die Acc-Werte, weil er dann anders im Raum steht!
Is jetzt schwierig zu erklären, evtl probierst dus nochmal mit dem Gedankenexperiment, eigentlich sollte es da klar werden.

Gruß
Chris