Genetischer Algorithmus für QuadroCopter

[Che Guevara]

Hallo,

deine Umsetzung der Problemstellung finde ich sehr gut, aber:
Was hat das ganze mit der Temperatur zu tun und warum wird diese halbiert? Benutzt du diesen Wert als zufälligen Faktor? Könntest du mir den Code zeigen?

Anscheinend habe ich mich falsch ausgedrückt: Es geht mir nicht um eine Höhenregelung des Copters, sondern um die Lagestabilisierung der 3 Achsen (Gemessen durch die Gyros und evtl. demnächt durch einen ACC). Ich möchte sozusagen das Aufschaukeln irgendwie feststellen und anhand diesen Wertes meine PID-Parameter einstellen. Nur habe ich eben momentan keine Idee, wie ich 1. zwischen der normalen Steuerung (also KEIN Aufschaukeln) und 2. dem Aufschaukeln (aufgrund zu großer Werte) bzw. der zu geringen Regelleistung unterscheiden soll. Wie erkennt also mein Programm, dass die Werte nicht optimal sind und deswegen das Flugverhalten gestört ist?!

Gru0
Chris

[rossir]

simulated annealing = simulierte Abkühlung
- Hat natürlich was mit Temperatur zu tun und ist ein heuristisches Optimierungsverfahren.
- Nein, T ist gerade kein zufälliger Faktor sondern ein systematischer. Denn durch das kleiner werdende T wird das Suchintervall/Umgebung um w(0) immer enger.
- Zufällig sind allein rp, ri und rd. Mit denen wird die Umgebung von w(0) "erkundet".
- Halbieren von T muß nicht sein man kann auch allgemein mit a abkühlen d.h. T:=a*T mit a in ]0.0, 1.0[. (a:=0.5 ist nur die bequeme Mitte.)
- Nein, einen vernünftigen ASURO Quellcode habe ich hier nicht an zu bieten. Denn der ist heute "verseucht" mit "tausend" kleinen Abkürzungen die das zu Grunde liegende simulated annealing Prinzip leider nur verschleiern.

Aber mein C-code sieht ca. wie folgt aus:

typedef struct {double Kp, Ki, Kd;} Pid;

int simulated_annealing(int N) {
int test;
PID w0, wcandidate={1000, 500, 30};
double f0, fcandidate;
double T=1.0;
double fp=50, fi=10, fd=6;
double a=0.99;
 fcandidate=fitness(wcandidate);
 w0=wcandidate;
 f0=fcandidate;
 for(test=0; fcandidate>10; test++) {
  int i;
  for(i=0; i<N; i++) {
   PID wi={w0.Kp + T*fp*r(), w0.Ki + T*fi*r(), w0.Kd + T*fd*r()};
   double f=fitness(wi);
   if (f<fcandidate) {
     fcandidate=f;
     wcandidate=wi;
    }
   }
   if (fcandidate<f0) T*=a;
   w0=wcandidate;
   f0=fcandidate;
   printf("%d T=%.4f, fitness=%4.1f: Kp=%6.1f, Ki=%6.1f, Kd=%6.1f\n", test, T, f0, w0.Kp, w0.Ki, w0.Kd);
 }
}

Und ausgehend von Kp= 1000, Ki= 500 und Kd= 30 auf der Suche nach den (angenommenen) optimalen Parametern z.B. Kp= 845.3, Ki= 555.2 und Kd= 4 liefert ein Beispiellauf folgende Ausgabe:


0 T=0.9900, fitness=14650.0: Kp= 950.7, Ki= 501.5, Kd= 29.4
1 T=0.9801, fitness=8098.1: Kp= 920.4, Ki= 510.6, Kd= 25.8
2 T=0.9703, fitness=3977.0: Kp= 881.8, Ki= 507.4, Kd= 22.8
3 T=0.9606, fitness=2280.7: Kp= 840.4, Ki= 512.2, Kd= 24.1
4 T=0.9510, fitness=2254.4: Kp= 863.2, Ki= 515.8, Kd= 23.5
5 T=0.9415, fitness=2073.1: Kp= 824.1, Ki= 520.2, Kd= 23.9
6 T=0.9321, fitness=1795.5: Kp= 855.7, Ki= 521.1, Kd= 27.0
7 T=0.9321, fitness=1795.5: Kp= 855.7, Ki= 521.1, Kd= 27.0
8 T=0.9227, fitness=1782.5: Kp= 843.6, Ki= 523.9, Kd= 32.3
9 T=0.9135, fitness=1075.5: Kp= 842.3, Ki= 532.9, Kd= 27.9
10 T=0.9044, fitness=807.9: Kp= 857.9, Ki= 539.5, Kd= 24.0
11 T=0.8953, fitness=418.5: Kp= 842.2, Ki= 544.8, Kd= 21.4
12 T=0.8953, fitness=418.5: Kp= 842.2, Ki= 544.8, Kd= 21.4
13 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
14 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
15 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
16 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
17 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
18 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
19 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
20 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
21 T=0.8775, fitness=332.9: Kp= 845.7, Ki= 545.4, Kd= 19.4
22 T=0.8775, fitness=332.9: Kp= 845.7, Ki= 545.4, Kd= 19.4
23 T=0.8687, fitness=213.1: Kp= 846.1, Ki= 554.1, Kd= 18.5
24 T=0.8687, fitness=213.1: Kp= 846.1, Ki= 554.1, Kd= 18.5
25 T=0.8687, fitness=213.1: Kp= 846.1, Ki= 554.1, Kd= 18.5
26 T=0.8601, fitness=205.8: Kp= 838.1, Ki= 558.9, Kd= 15.8
27 T=0.8515, fitness=134.7: Kp= 838.6, Ki= 557.0, Kd= 13.3
28 T=0.8515, fitness=134.7: Kp= 838.6, Ki= 557.0, Kd= 13.3
29 T=0.8515, fitness=134.7: Kp= 838.6, Ki= 557.0, Kd= 13.3
30 T=0.8515, fitness=134.7: Kp= 838.6, Ki= 557.0, Kd= 13.3
31 T=0.8429, fitness=82.4: Kp= 851.1, Ki= 557.6, Kd= 10.6
32 T=0.8345, fitness=68.6: Kp= 840.1, Ki= 560.9, Kd= 6.8
33 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
34 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
35 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
36 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
37 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
38 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
39 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
40 T=0.8179, fitness=65.1: Kp= 838.7, Ki= 556.1, Kd= 8.5
41 T=0.8179, fitness=65.1: Kp= 838.7, Ki= 556.1, Kd= 8.5
42 T=0.8179, fitness=65.1: Kp= 838.7, Ki= 556.1, Kd= 8.5
43 T=0.8097, fitness=60.7: Kp= 838.8, Ki= 551.1, Kd= 5.5
44 T=0.8016, fitness=42.1: Kp= 847.0, Ki= 549.2, Kd= 2.3
45 T=0.8016, fitness=42.1: Kp= 847.0, Ki= 549.2, Kd= 2.3
46 T=0.8016, fitness=42.1: Kp= 847.0, Ki= 549.2, Kd= 2.3
47 T=0.7936, fitness=23.1: Kp= 847.1, Ki= 552.0, Kd= 7.1
48 T=0.7936, fitness=23.1: Kp= 847.1, Ki= 552.0, Kd= 7.1
49 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
50 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
51 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
52 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
53 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
54 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
55 T=0.7778, fitness= 2.6: Kp= 844.9, Ki= 555.5, Kd= 5.5



Lagestabilisierung der 3 Achsen hört sich einfacher an als Höhenregelung. Hier mein Vorschlag für eine Fitnessfunktion:
f: summe(i in [0, tmax| sqr(ax(i-1)-ax(i))+sqr(ay(i-1)-ay(i))+sqr(az(i-1)-az(i)))

Und ax(i), ay(i) und az(i) sind die Werte der 3 Achsen Gyros zum Zeitpunkt i.

[Che Guevara]

Hallo,

sry dass ich mich erst jetzt melde, hatte in der Zwischenzeit einiges zu tun. Deine Funktion verstehe ich nicht ganz. Was meinst du mit summe? f ist der Fitness-wert? Soll ich die Werte in der Summe-Klammer summieren?
Ist es sinnvoll alle 3 Parameter gleichzeitig zu probieren oder sollte ich lieber erst KP, dann KI und dann KD bestimmen?

Vielen Dank & Gruß
Chris

[Willa]

Hi,
das Bewerten der Fitness ist wohl der schwierigste Teil. Ich würde vorschlagen mal zu loggen: Einmal mit guten Parametern und einmal mit schlechten. Dann kannst du mit den aufgezeichneten Daten erstmal herumprobieren ob du irgendwelche Parameter finden kannst, die die Fitness beschreiben. Das wird nicht ganz leicht, denn der Copter kann nicht zwischen Windböen und schlechten Parametern unterscheiden (bzw. vielleicht schon, aber das wird nicht trivial sein). Ein Schwingen wird vielleicht nur per Fourieranalyse von Windböen zu unterscheiden sein, keine Ahnung...
Ich bin mir nicht sicher, ob dein Hauptcontroller diese Aufgabe zusätzlich übernehmen kann. Nach meiner Einschätzung, braucht so eine Analyse eine Menge Daten die über einen mehr oder wenigen langen Zeitraum aufgezeichnet und verrechnet werden. Das ist für so einen armen mega328p anstrengend...
ki, kp und kd würde ich gleichzeitig optimieren, denn die beeinflussen sich alle gegenseitig. Wenn ein kp Wert gut ist und man dann z.B. ki erhöht, ist der Wert für kp plötzlich sehr suboptimal. Da bin ich mal gespannt auf die weitere Entwicklung :-D

[Che Guevara]

Hallo Willa,

das freut mich, dass so ein Profi wie du sich für das Thema interressiert
Hm die Idee mit dem Datenloggen gefällt mir, evtl. werde ich mal mit einer SD-Karte experimentieren, welche dann aber wohl von einem zweiten µC angesteuert werden muss, damit das Regelverhalten sich nicht ändert. Die Windböen sollten kein Problem darstellen, ich werde einfach versuchen, die Daten nur bei ruhigen Ausseneinflüssen zu loggen oder ich machs irgendwo im Haus...
Allerdings habe ich doch starke Bedenken, nicht dass das Ganze nach hinten losgeht und ich demnächst wieder mal alles neu machen muss.... Evtl. sollte ich den Copter irgendwie mit ein oder zwei Schnüren etwas absichern. Ansich finde ich aber das Thema sehr interressant, da es doch etwas anspruchsvoller ist und auch einen praktischen Wert hat
Werde mal sehen, was sich als Datenlogger eignet und melde mich dann wieder.

@rossir:
Mir ist gerade erst der Sinn deiner vorgeschlagenen Fitnessfunktion aufgefallen: Je mehr hochfrequente Drehwinkelgeschwindigkeitsveränderungen *gg* es gibt, desto größer ist der Wert der Fitnessfunktion, d.h. desto schlechter sind die Werte. Ist das so richtig?
Aber wofür ist dann die Wurzel? Diese hat für mich (bis jetzt) keinen tieferen Sinn, könntest du mir das bitte erklären?

Vielen Dank & Gruß
Chris

[Bammel]

Hi,

das ist echt interesant.. wie wäre es damit diesen algo auf einem extra µC plus IMU zu machen? diese übermittelt nur die Kp, Ki und Kp werte an den haupt-µC. so kann man gut mitloggen oder der einstell-µC gibt irgendwann eine art okay zeichen... nun bruacht man nurnoch die werte auslesen und kann diese fest im copter verankern. dies müsste man natürlich wiederholen wnen man den copter irgendwie verändert...

gruß, bammel

[rossir]

Ja, f ist der Fitnesswert:

double f=fitness(wi);

Diese Fitness wird für wi bestimmt:

PID wi={w0.Kp + T*fp*r(), w0.Ki + T*fi*r(), w0.Kd + T*fd*r()};

Der Vektor wi besteht aus den neuen Testkandidaten für Kp, Ki und Kd.

Ich stelle mir vor, dass innerhalb der Funktion fitness(wi) Folgendes passiert:
Der Quadrokopter fliegt schon und wird durch die alten Kp, Ki und Kd Werte (w(i-1)) des PID Controller leidlich Lage stabilisiert. Jetzt werden für den PID Controller die neuen Testkandidaten w(i)= Kp, Ki und Kd eingestellt. Der Test läuft z.B. 30 sec (tmax=30), d.h die Sekunden laufen von t=0 bis t=30.

Jede Sekunde werden die Werte ax(t), ay(t) und az(t) des 3 Achsen Gyros ausgelesen. Wenn zwischen zwei aufeinander folgenden Messungen eine möglichst kleine Differenz ist dann ist die Lagestabilität gut. Das ist die Grundidee hinter der Fitnessfunktion! Diese habe ich wie folgt in einer Formel ausgedrückt:

f: summe(i in [1, tmax| sqr(ax(t-1)-ax(t))+sqr(ay(t-1)-ay(t))+sqr(az(t-1)-az(t)))

Genau wie Du gesagt hast: Die Werte in der "Summe-Klammer" werden summiert. Nach 30 sek habe ich dadurch eine Fitness f für meine Testkandidaten Kp, Ki und Kd.

Dann probiere ich neue Testkandidaten usw.

Ja, es ist sinnvoll alle 3 Parameter gleichzeitig zu probieren, "denn die beeinflussen sich alle gegenseitig".

Anders als Willa meint, ist das keine Aufgabe die den mega328p überanstrengt. Eher ein Ansatz der die armen Akkus quält.

Nachtrag:
Wurzel wäre sqrt(x). Ich schreibe aber sqr(x) und das meint x*x. Damit schlägt man zwei Fliegen mit einer Klappe:
a) x*x ist immer positiv
b) große x werden überproportional "böse" bewertet.

[rossir]

@Bammel
Der simulated annealing Algorithmus benötigt wirklich keine besondere Rechenzeit. Ich würde ihn sogar, auch später im Einsatz, permanent (bei kleiner Temperatur T) mitlaufen lassen.
- Kleine Temperatur bedeutet, dass neue Testkandidaten für Kp, Ki und Kd nur in der Nähe der bisher besten Kp, Ki und Kd Werte gesucht werden.
- Permanent bedeutet, das ich ca. alle 60 sek einen neuen Testkandidaten probiere d.h. live einsetze. (Das ist aber, wie oben gesagt, nur eine kleine und damit "ungefährliche" Variante der bisher besten Werte.)

Dadurch wird das ganze System robust, flexibel und letztendlich selbstadaptiv. (Genau das Richtige also für den Thread "Software, Algorithmen und Ki" )

[Che Guevara]

@Bammel:
das hört sich gut an, ich werd mir die Tage mal was überlegen und ein bisschen testen Evtl. hole ich mir noch einen zweiten Gyro zum experimentieren, damit ich nicht immer hin-und herlöten muss.
Noch besser wäre es aber, wenn man diese Funktion auf dem HauptµC unterbringen könnte, so könnte man ohne irgendwelche Veränderungen jederzeit eine optimale PID-Parameter Einstellung vornehmen.

@rossir:
So langsam verstehe ich, wie du das alles meinst. Danke für die ausführliche Erklärung
Tja, da hätte ich mal wieder genauer lesen müssen, dann wäre mir dieser Fehler nicht passiert. Und ich muss sagen, diese Variante gefällt mir sehr gut, da damit dann präziser zwischen gut und schlecht unterschieden werden kann
Aber denkst du, 30s sind nötig? Ich hätte jetzt eher bei 2-3s angesetzt...

Vielen Dank & Gruß
Chris

[Bammel]

@che guevara: naja umlöten geht ja nicht.. du bruchst ja ein gyro zur flugstabilisierung und einen für die auswertung ob die werte gut sind.

das ganze muss man aber wie rossir gesagt hat über längere zeiten messen da ja noch vibrationen der motoren hinzukomme und auch steuerbefehle des piloten da ja alleine nur mit gyros keine eigenstabilität erreicht werden kann.

ich find die idee klasse und ich glaub ich werd mich da auch mal an eine lösung probieren. aber erstmal habe ich eine andere baustelle die gelöst werden will.

was mir da aber grade einfällt man muss ja die ganzen ergebnisse speichern. oder? das kostst viel speicher. bei mir, also in der firmware der shrediquette von willa, ist zwar noch speicher frei aber irgendwann ist da auch mal ende... man müsste sich warscheinlich am ende des fluges den besten wert extra ins eeprom speichern und dann beim nächsten flug weiter machen. später wenn kaum noch verbesserungen getroffen werden, sollte der algo sich auf minimalste änderungen der werte einstellen.

gruß, bammel