Genetischer Algorithmus für QuadroCopter

**rossir** · 19.10.2011, 21:38

Ja, f ist der Fitnesswert:

Code:

double f=fitness(wi);

Diese Fitness wird für wi bestimmt:

Code:

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:

Code:

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** · 19.10.2011, 21:59

@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** · 19.10.2011, 22:02

@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** · 19.10.2011, 23:03

@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

**Che Guevara** · 19.10.2011, 23:35

Ich wollte mir ja ein Testboard bauen, damit ich erstmal ohne Risiko, den Copter zu schrotten, das Programm ausprobieren kann

Den auch, wenn man im Programm Code keinen Fehler erkennen kann, heißt das noch lange nicht, dass da auch kein Fehler ist...
Hm über die Zeit muss ich mir nochmal Gedanken machen, am besten wird es wohl sein, ein paar Tests zu machen. Trotzdem denke ich, eine etwas kürzere Zeit sollte besser sein, den: Innerhalb von ein paar Sekunden muss der Pilot nicht zwingend nachregeln, was die ganze Methode doch um einiges genauert macht. Folgendes Szenario: Der Pilot steuert beim 1. Test etwas stärker, da sich der Copter zu stark bewegt. Somit beinflusst der Pilot die Fitness sehr negativ. Beim 2. Test muss der Pilot nicht / nur wenig steuern, die Fitness wird somit nicht vom Piloten beeinflusst. Dann kann es passieren, dass die Werte des 2. Tests besser abschneiden als die des 1., obwohl die ersten Werte eigentlich die besseren wären. Stimmt ihr mir zu, oder habe ich hier einen Denkfehler?

Also Speichermäßig sollte auf dem Mega328P (der ist ja auf der shrediquette auch drauf, oder?) doch einiges vorhanden sein. Hab mich gerade mal an einer Lösung probiert, ist zwar noch nicht fertig, aber ich denke, ich habe bis auf ein paar Kleinigkeiten alles wichtige drin (7% Flash Used):

Code:

$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 80
$hwstack = 80
$swstack = 80
$baud = 19200



Declare Sub Get_fitness()
Declare Sub Get_new_individuen()




Const Max_time = 50                                         'maximale Zeit zum Testen


Dim I As Byte                                               'Zeitpunkt i
Dim Gyro_yaw(max_time) As Integer                           'Gyro Wert zum Zeitpunkt i
Dim Gyro_roll(max_time) As Integer                          'Gyro Wert zum Zeitpunkt i
Dim Gyro_pitch(max_time) As Integer                         'Gyro Wert zum Zeitpunkt i
Dim Gyro_yaw_sqr As Integer                                 'Gyro Differenz Wert von (i-1) bis i
Dim Gyro_roll_sqr As Integer                                'Gyro Differenz Wert von (i-1) bis i
Dim Gyro_pitch_sqr As Integer                               'Gyro Differenz Wert von (i-1) bis i
Dim Rnd_p As Single                                         'Zufallszahl zu Erstellung neuer Individuen
Dim Rnd_i As Single                                         'Zufallszahl zu Erstellung neuer Individuen
Dim Rnd_d As Single                                         'Zufallszahl zu Erstellung neuer Individuen
Dim Temperatur As Single                                    'simulated annealing
Dim Temperatur_int As Integer
Dim T_a As Single
Dim Kp_startup As Single                                    'Startup P Wert
Dim Ki_startup As Single                                    'Startup I Wert
Dim Kd_startup As Single                                    'Startup D Wert
Dim Kp_mom As Single                                        'momentaner P Wert
Dim Ki_mom As Single                                        'momentaner I Wert
Dim Kd_mom As Single                                        'momentaner D Wert
Dim Kp_best As Single                                       'bester P Wert
Dim Ki_best As Single                                       'bester I Wert
Dim Kd_best As Single                                       'bester D Wert


Dim Tmp As Byte
Dim Tmp_int As Integer
Dim Tmp_single As Single


Dim Fitness As Single
Dim Fitness_int As Integer
Dim Best_fitness As Single
Dim Best_fitness_int As Integer


Dim Fp As Single                                            'Dynamikunterschied
Dim Fi As Single                                            'Dynamikunterschied
Dim Fd As Single                                            'Dynamikunterschied


Dim _yawnow As Integer                                      'aktueller Gyro Wert
Dim _rollnow As Integer                                     'aktueller Gyro Wert
Dim _pitchnow As Integer                                    'aktueller Gyro Wert


Temperatur = 1

T_a = 0.95                                                  'Abweichung


Kp_startup = 0.3
Ki_startup = 0.0001
Kd_startup = 0.000007


Fp = 0.2                                                    'Dynamikunterschied
Fi = 0.05                                                   'Dynamikunterschied
Fd = 0.05                                                   'Dynamikunterschied


Tmp_int = Rnd(20) - 10                                      '-10 < tmp_int < 10
Rnd_p = Tmp_int * 0.1                                       '-1 < rnd_p < 1

Tmp_int = Rnd(20) - 10                                      '-10 < tmp_int < 10
Rnd_i = Tmp_int * 0.1                                       '-1 < rnd_i < 1

Tmp_int = Rnd(20) - 10                                      '-10 < tmp_int < 10
Rnd_d = Tmp_int * 0.1                                       '-1 < rnd_d < 1



Do


   Call Get_new_individuen()

   'teste neue Individuen
   For I = 0 To 30
      Gyro_yaw(i) = _yawnow
      Gyro_roll(i) = _rollnow
      Gyro_pitch(i) = _pitchnow
      Waitms 1
   Next I

   Call Get_fitness()

   If Fitness < Best_fitness Then
      Best_fitness = Fitness
      Kp_best = Kp_mom
      Ki_best = Ki_mom
      Kd_best = Kd_mom
   End If

   Temperatur = Temperatur * T_a

   Tmp_int = Rnd(20) - 10                                   '-10 < tmp_int < 10
   Rnd_p = Tmp_int * 0.1                                    '-1 < rnd_p < 1

   Tmp_int = Rnd(20) - 10                                   '-10 < tmp_int < 10
   Rnd_i = Tmp_int * 0.1                                    '-1 < rnd_i < 1

   Tmp_int = Rnd(20) - 10                                   '-10 < tmp_int < 10
   Rnd_d = Tmp_int * 0.1                                    '-1 < rnd_d < 1


Loop


Sub Get_new_individuen()

   Kp_mom = Temperatur * Fp
   Kp_mom = Kp_mom * Rnd_p
   Kp_mom = Kp_mom + Kp_startup

   Ki_mom = Temperatur * Fi
   Ki_mom = Ki_mom * Rnd_i
   Ki_mom = Ki_mom + Ki_startup

   Kd_mom = Temperatur * Fd
   Kd_mom = Kd_mom * Rnd_d
   Kd_mom = Kd_mom + Kd_startup

End Sub


Sub Get_fitness()

   For I = 1 To 30

      Tmp = I - 1

      Tmp_int = Gyro_yaw(i) - Gyro_yaw(tmp)
      Tmp_int = Tmp_int ^ 2
      Gyro_yaw_sqr = Gyro_yaw_sqr + Tmp_int

      Tmp_int = Gyro_roll(i) - Gyro_roll(tmp)
      Tmp_int = Tmp_int ^ 2
      Gyro_roll_sqr = Gyro_roll_sqr + Tmp_int

      Tmp_int = Gyro_pitch(i) - Gyro_pitch(tmp)
      Tmp_int = Tmp_int ^ 2
      Gyro_pitch_sqr = Gyro_pitch_sqr + Tmp_int

   Next I

   Fitness_int = Gyro_yaw_sqr + Gyro_roll_sqr
   Fitness_int = Fitness_int + Gyro_pitch_sqr
   Fitness = Fitness_int

End Sub

End

Es ist jetzt aber schon relativ spät, weshalb der Code noch seeeehr im Anfangsstadium ist. Werds mir morgen nochmal genauer ansehen.

Gruß
Chris

**Willa** · 20.10.2011, 07:46

Ich bin sehr gespannt ob das im Prinzip funktioniert... Ich stelle mir das Bewerten wie gesagt sehr schwierig vor. Wenn ich als Mensch manchmal schon Probleme habe zu bewerten wie gut die eingestellten Parameter sind, was soll dann ein armer Computer das alles machen? Richtig spannend und sinnvoll wird so eine Optimierung aber nicht im Schwebeflug. Meine Copter fliegen im Schwebeflug (und bei Windstille) auch mit total verdrehten Regelparametern super. Nur wenn man schneller fliegt und enge Kurven etc. bei Wind, dann merkt man ob die Parameter wirklich was taugen.
Aber klar, man sollte erstmal im Schwebeflug bei optimalen Randbedingungen anfangen. Ich möchte aber auch nicht derjenige sein der immer rumnörgelt und sagt wie schwierig alles ist... Vielleicht hast du ja schon übermorgen fix und fertig evolvierte Parameter...!

**Bammel** · 20.10.2011, 10:37

moin,

ich hätte noch folgende idee...
man stellt den copter in schwebeflug ein... denn geht der algo los... es leuchtet eine LED nun muss man den copter an einem ausleger nach oben stupsen... fitness wird bewertet... dann werden neue PID werte übermittelt udn das ganze szenario geht neu los.

das ganze wäre so eine "extrem" situation. den william hat recht. mein copter fliegt im schwebeflug auch astrein nur bei wind und beim umherfliegen zuckt dieser.

evtl kann man das starten der auswertung der fitness auch über einen schalter am sender machen!?

gruß, bammel

**Willa** · 20.10.2011, 13:05

evtl kann man das starten der auswertung der fitness auch über einen schalter am sender machen!?

Das ist eine gute Idee... Man könnte einfach verschiedene Arten von Störungen auf den Copter legen und danach bewerten wie gut diese ausgeregelt werden. Z.B. ein Szenario ist, dass man ein Rauschen auf die Motor-Stellwerte gibt und danach beurteilt wie gut diese Störung kompensiert wird. Oder man gibt auf einen Motor einen sprunghaften Fehler und beurteilt das Ausregeln. Das kingt dann für mich schon deutlich einfacher und "kontrollierter"... Vor allem könnte man diese Versuche wirklich auf einem Teststand machen, d.h. man riskiert nicht mehr so viele Abstürze.