Genetischer Algorithmus für QuadroCopter

[Che Guevara]

Hallo,

da mich die PID-Einstellerei meines Quadros ziemlich nervt (auch, wenns jetzt schon getan ist ) möchte ich versuchen, einen genetischen Algorithmus zu programmieren (ATMEGA328P @ 16MHz in BASCOM), welcher die Parameter selbstständig einstellt. Habe mich auch schon ein bisschen in die Materie eingelesen, jedoch scheint es nicht gerade sehr einfach zu sein. Ich wollte mal Fragen, ob hier schonmal jemand einen genet. Algo. programmiert hat und mir evtl. den SourceCode mal zeigen könnte / möchte?!
Der Grundsätzliche Ablauf ist mir mittlerweile mehr oder weniger geläufig:
- N zufällige Individuen erzeugen
- diese in ihrer "Überlebenschance" bewerten mit sog. Fitness-Punkten
- 2 versch. Individuen auswählen und Rekombinieren
- diese durch Mutation verändern (Bits an best. Stellen negieren)
- diese Vorgänge wiederhole, bis eine neue Population mit N Individuen entstanden ist

Allerdings bin ich mir nicht darüber im klaren, wie genau ich das nun im Programm umsetzen kann.
1. Problem:
Wie kann ich einem Individuum eine Fitnesszahl zuortnen? Im konkreten Fall des QuadroCopters müsste ich dazu ja das Flugverhalten (sprich Aufschaukeln & Reaktionszeit) irgendwie herausfinden (nur wie!?!?)
2. Problem:
Der Prozess der Mutation ist mir eig. nicht sehr gefäufig, könnte mir das jemand mal bitte am Beispiel des QuadroCopters erklären?

Wäre nett, wenn jemand mal ein paar Worte dazu schreiben könnte, ob ich alles richtig verstanden / wiedergegeben habe.
Vielen Dank & Gruß
Chris

[rossir]

Hallo,

ASURO:
So richtig genetisch hab ich das letztlich nicht gemacht. Meine Methode hat eher Ähnlichkeit mit simulated anealing. Immerhin habe ich die Kp, Ki, und Kd Werte für den PID Controller meines ASURO Roboters automatisch bestimmt.
Folgendes sollte der PID Controller beim Versuchsaufbau leisten:
Aus den Stillstand soll sich ASURO im nächsten Moment um seine eigene Achse drehen. Dies dann mit möglichst konstanter Drehgeschwindigkeit (vSoll) für 4000 ms. Dann soll er wieder still stehen.
Die Encoderscheiben liefern dazu das nötige Feedback. (v wird hierbei einfach als Encoderticks pro Zeiteinheit bestimmt.)

Der Kurvenverlauf von vSoll ist also rechteckförmig: Zum Zeitpunkt t0 beginnt er mit einer Flanke von 0 nach vSoll. Bleibt dann 4000 ms auf dem Niveau von vSoll und endet zum Zeitpunkt t4000 mit einer Flanke von vSoll nach 0.
vIst hat natürlich eine ganz andere kurvenform und wird hautsächlich von Kp, Ki, Kd, den zu überwindenden Trägheiten und dem Ladezustand der Batterie bestimmt. vIst wird 8000 ms lang gemessen. Als Fitnessfunktion f nehme ich die Summe der Fehlerquadrate zwischen vIst und vSoll.
f: summe(i in [0, 8000]| sqr(vIst(i)-vSoll(i)))
Je kleiner die Summe der Fehlerquadrate (f) ist, desto besser sind Kp, Ki und Kd gewählt.

Der Suchprozess beginnt mit einem zunächst "nur" plausiblen Wertvektor w(0):=(Kp(0), Ki(0), Kd(0)) und einer Temperatur von T:=1.0. Die Fitness f(0) von w(0) wird bestimmt.

Loop:
Es werden z.B. 5 (oder mehr) neue zufällige Vektoren erzeugt die sich alle in der "Nähe" von w(0) befinden. Z.B. so:
w(1)=(Kp(0)+T*fp*rp, Ki(0)+T*fi*ri, Kd(0)+T*fd*rd)
Wobei rp, ri und rd Zufallszahlen sind die in [-1.0, 1.0] liegen. Und fp, fi und fd sind konstante Faktoren mit denen ich die Dynamikunterschiede zwischen Kp, Ki und Kd anpasse. (Kp darf sich im Unterschied zu Ki stark ändern. Ki liegt meiner Erfahrung nach eher in der Nähe von 0.0)
Für w(1) bis w(5) wird jeweils der oben beschriebene Versuch gemacht und dadurch f(1) bis f(5) bestimmt.
Wenn f(i) (z.B. i=4) die beste Fitness hat wird w(0):=w(i) gesetzt, die Temperatur halbiert (T:=0.5*T) und der Vorgang bei Loop: fortgesetzt.


Idee zu QuadroCopter:
Versuchsaufbau:
Der Quadrocopter ist an einem Ende einer Wippe befestigt. Die Wippe hat ein Gegengewicht so, dass die Wippe im Ruhezustand in waage ist.
Aus den Ruhezustand soll der Quadrocopter im nächsten Moment genau 1m Höhe gewinnen. Diese dann möglichst konstant (hSoll) für 4000 ms halten. Dann soll er wieder in den Ruhezustand gehen.
Ein Distanzsensor (Distanz zwischen Boden und einer Wippenseite) liefert dazu das nötige Feedback.

[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