Genetischer Algorithmus für QuadroCopter

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

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.

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

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:

Code:

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.

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? :D
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

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

Hallo Willa,

das freut mich, dass so ein Profi wie du sich für das Thema interressiert :D
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? :D

Vielen Dank & Gruß
Chris

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

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.

@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" ;-) )

@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

@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

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

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...!

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

Zitat:

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.

Hallo,

mit dem Schwebeflug habt ihr natürlich vollkommen recht, daran hatte ich nicht gedacht... war auch schon etwas spät :)
Eure Ideen gefallen mir sehr gut! Was haltet ihr den davon, wenn man diese kombiniert?! Ich stelle mir vor, den Copter auf eine bestimmte Höhe zu fliegen und dann einen Taster zu drücken - dieser startet dann den Algorithmus: Es werden neue Parameter eingestellt und nach einer kurzen Zeit wird auf einen (oder evtl. mehreren) Motor ein sprunghafter Fehler gegeben. Somit hätte man (theoretisch) immer das gleiche Szenario und könnte die einzelnen Parameter immer sehr objektiv bewerten lassen.

Gruß
Chris

hallo...

ich finde die idee mit dem schalter und dem prüfstand super!

klar muss man anch dem prüfstand noch die parameter weiter optimieren aber das könnte ja ggf. in sehr kleinen schritten direkt beim fliegen passieren.

den ein copter an einem prüfstand gebunden verhält sich ja anders als frei schwebend.

prüfstand würde ich mir als eine art wippe über eine oder ggf auch zwei achsen vorstellen. für die gier regelung könnte man dann noch einen prüfstand bauen in dem der copter über die hochachse drehend gelagert wird.

ich finde das ganze klingt nach einem sehr schönes winterprojekt.

was mir aber seit anfang an im kopf rumschwirt warum das nicht schon andere, namenhafte, kopter bauer verwirklicht haben.

gruß, bammel

Zitat:

was mir aber seit anfang an im kopf rumschwirt warum das nicht schon andere, namenhafte, kopter bauer verwirklicht haben.

Ich glaube der TT-Copter optimiert seine Parameter selbstständig. Aber ich weiss nicht wie.

Ja, vom TT-Copter habe ich das auch schon gelesen. Wie weiß ich auch nicht, aber evtl. bekomme ich ja ein paar Infos, werde denen mal ne Mail schreiben :)

Leider hatte ich gestern eine Kollision, deswegen muss ich jetzt erstmal einen Ausleger austauschen....

Gruß
Chris

Hallo,

gibt es schon neuigkeiten?

Gruß, Bammel

Hallo,

nein, leider nicht! Ich hatte eine E-Mail an TT-Copter geschrieben und gefragt, ob sie mir freundlicherweise sagen könnten, mit welchem Verfahren ihr Copter seine Parameter optimiert. Aber anscheinend ist das ein Geheimnis .... Habe nämlich keine E-Mail erhalten.
Mit dem Prüfstandaufbau kann ich auch noch nicht beginnen, da ich derzeit den Feinschliff meiner FA mache und nach den Ferien auch noch ein paar Klausuren schreibe. Ich denke, vor Weihnachten wird sich da nicht allzuviel ergeben, Schule geht vor.
Aber sobald ich weitergemacht habe, poste ich hier natürlich meine Erfahrungen.

Gruß
Chris

Hallo,

nun, da wieder 2 Klausuren vorbei sind, habe ich wieder Zeit, an dem Projekt weiterzumachen.
Zuerst habe ich allerdings eine Frage; folgender Code:

Code:

if (fcandidate<f0) T*=a; w0=wcandidate; f0=fcandidate;

Wird das so:

Code:

if fcandidate < f0 then temperatur = temperatur * t_a w0_kp = wcandidate_kp w0_ki = wcandidate_ki w0_kd = wcandidate_kd f0 = fcandidate endif

oder so:

Code:

if fcandidate < f0 then temperatur = temperatur * t_a endif w0_kp = wcandidate_kp w0_ki = wcandidate_ki w0_kd = wcandidate_kd f0 = fcandidate

nach BASIC übersetzt?
Mich verwirrt diese IF-Abfrage. C habe ich nie gelernt, nur in der Schule etwas JAVA, aber ich denke, das sollte in etwa eine ähnliche Syntax haben?!
Warum ist bei der IF-Abfrage keine {-Klammer vor T*=a?

Mittlerweile habe ich auch schon versucht, den kompletten Code anzupassen, hier mal mein aktuelles Geschreibsel:

Code:

$regfile = "m328pdef.dat" $crystal = 16000000 $framesize = 250 $hwstack = 250 $swstack = 250 $baud = 38400 Config Serialin = Buffered , Size = 100 Config Serialout = Buffered , Size = 100 Declare Sub Simulated_annealing(byval N As Byte) Declare Function Getfitness() As Single Declare Sub Log_data() Declare Sub Init_system() Declare Sub Filter_gyro_data() Declare Sub Filter_rx_data() Declare Sub Mixer() Declare Sub Pid_regulator() Declare Sub Wmp_init() Declare Sub Send_zero() Declare Sub Read_wmp_data() Declare Sub Set_wmp_offset() Declare Sub Failsave() Declare Sub Set_pwm() Config Portc.2 = Output Config Portc.3 = Output Portc.2 = 0 Portc.3 = 0 Config Portb.1 = Output Config Portb.2 = Output Config Portd.5 = Output Config Portd.6 = Output Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64 Pwm1a = 113 Pwm1b = 113 Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64 Pwm2a = 113 Pwm2b = 113 Config Pind.2 = Input Portd.2 = 0 Config Int0 = Falling On Int0 Measure Enable Int0 Config Timer0 = Timer , Prescale = 256 On Timer0 Pausedetect Enable Timer0 $lib "I2C_TWI.LBX" Config Scl = Portc.5 Config Sda = Portc.4 Config Twi = 100000 I2cinit Const Minthrottle = 30 Const Maxchannel = 8 Const _throttlechannel = 1 Const _rollchannel = 2 Const _pitchchannel = 3 Const _yawchannel = 4 Const _statechannel = 5 Const _datachannel = 6 Dim I As Byte Dim J As Byte Dim Newvalsreceived As Bit Dim _blink As Byte Dim _yaw_kp As Single Dim _roll_kp As Single Dim _pitch_kp As Single Dim _yaw_ki As Single Dim _roll_ki As Single Dim _pitch_ki As Single Dim _yaw_kd As Single Dim _roll_kd As Single Dim _pitch_kd As Single _yaw_kp = 0.28 '0.32 _roll_kp = 0.28 '0.32 _pitch_kp = 0.25 '0.32 _yaw_ki = 0.0001 '0.000135 _roll_ki = 0.0001 '0.000135 _pitch_ki = 0.0001 '0.000135 _yaw_kd = 0.000007 '0.0000096 _roll_kd = 0.000007 '0.0000096 _pitch_kd = 0.000007 '0.0000096 '################################### '#########RC-EMPFÄNGER############## '################################### Dim Bufferbyte As Byte Dim Kanal(maxchannel) As Word Dim Fkanal(maxchannel) As Word Dim Channel As Byte Dim _bl(4) As Word Dim _sbl(maxchannel) As Integer Dim State As Byte Dim Oldstate As Byte '################################### '################################### '################################### '################################### '###########I2C-Inputs############## '################################### Dim Wmplus_buffer(6) As Byte Dim Ar(6) As Byte '################################### '################################### '################################### '################################### '#########GYRO###################### '################################### Dim Yaw As Word Dim Yaw0 As Byte At Yaw + 1 Overlay Dim Yaw1 As Byte At Yaw Overlay Dim Roll As Word Dim Roll0 As Byte At Roll + 1 Overlay Dim Roll1 As Byte At Roll Overlay Dim Pitch As Word Dim Pitch0 As Byte At Pitch + 1 Overlay Dim Pitch1 As Byte At Pitch Overlay Dim _yawoffset As Long Dim _rolloffset As Long Dim _pitchoffset As Long Dim _yawoffset_int As Integer Dim _rolloffset_int As Integer Dim _pitchoffset_int As Integer Dim _yawnow As Integer Dim _rollnow As Integer Dim _pitchnow As Integer '################################### '################################### '################################### '################################## '#########PID-REGLER############### '################################## Dim _yaw_kp_err As Single Dim _roll_kp_err As Single Dim _pitch_kp_err As Single Dim _yaw_ki_err As Single Dim _roll_ki_err As Single Dim _pitch_ki_err As Single Dim _yaw_ki_sum As Single Dim _roll_ki_sum As Single Dim _pitch_ki_sum As Single Dim _yaw_kd_err As Single Dim _roll_kd_err As Single Dim _pitch_kd_err As Single Dim _yaw_kd_old As Single Dim _roll_kd_old As Single Dim _pitch_kd_old As Single Dim _yaw_pid_int As Integer Dim _roll_pid_int As Integer Dim _pitch_pid_int As Integer Dim _yaw_pid As Single Dim _roll_pid As Single Dim _pitch_pid As Single Dim _yaw_err_int As Integer Dim _roll_err_int As Integer Dim _pitch_err_int As Integer Dim _yaw_err As Single Dim _roll_err As Single Dim _pitch_err As Single '################################# '################################# '################################# Dim Setpoint_yaw As Single Dim Setpoint_roll As Single Dim Setpoint_pitch As Single Dim Yawstickvel As Integer Dim Rollstickvel As Integer Dim Pitchstickvel As Integer Dim Yawstickold As Integer Dim Rollstickold As Integer Dim Pitchstickold As Integer Dim Hempf(maxchannel) As Word Dim Lempf(maxchannel) As Word Dim _x1 As Single Dim _x2 As Single Dim Rc_on As Word Dim Failure As Byte Dim Extmp As Single 'Integer Dim Eytmp As Single 'Integer Dim Eztmp As Single 'Integer Dim Ax(50) As Single Dim Ay(50) As Single Dim Az(50) As Single Dim Ex As Single 'Integer Dim Ey As Single 'Integer Dim Ez As Single 'Integer Dim Atmp As Byte Dim A As Byte Dim F As Single Dim Fcandidate As Single Dim Kp_mom As Single Dim Ki_mom As Single Dim Kd_mom As Single Dim Temperatur As Single Dim T_a As Single Dim Fp As Single Dim Fi As Single Dim Fd As Single Dim Rp As Single Dim Ri As Single Dim Rd As Single Dim Rp_i As Integer Dim Ri_i As Integer Dim Rd_i As Integer Dim Wi_kp As Single Dim Wi_ki As Single Dim Wi_kd As Single Dim Wcandidate_kp As Single Dim Wcandidate_ki As Single Dim Wcandidate_kd As Single Dim W0_kp As Single Dim W0_ki As Single Dim W0_kd As Single Dim F0 As Single Dim Test As Word 'Byte Dim Log_cnt As Byte Dim Send_logged_data As Byte Dim Rx_in As String * 30 'Dim Var(50) As Byte Dim Kanal_6_old As Byte Dim B As String * 1 Dim Lenght As Byte Dim Itmp As Byte Dim Itmp_old As Byte Dim Xtmp As Byte Call Init_system() Waitms 500 Enable Interrupts Waitms 500 Rc_test: If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then If Rc_on < 65000 Then Incr Rc_on Else If Rc_on > 0 Then Decr Rc_on End If If Rc_on < 500 Then Goto Rc_test End If '_bl(1) = rechts hinten (US) '_bl(2) = links hinten (GUS) '_bl(3) = rechts vorne (GUS) '_bl(4) = links vorne (US) 'PWM = 113 --> 0.900us 'PWM = 250 --> 2.000ms Wcandidate_kp = 0.3 Wcandidate_ki = 0.0001 Wcandidate_kd = 0.000007 Temperatur = 1.0 T_a = 0.8 '0.95 Fp = 0.1 '0.1 Fi = 0.0001 Fd = 0.000005 Fcandidate = 500 '150 'Fcandidate = Getfitness() W0_kp = Wcandidate_kp W0_ki = Wcandidate_ki W0_kd = Wcandidate_kd F0 = Fcandidate Do Call Filter_rx_data() Call Filter_gyro_data() Call Pid_regulator() Call Mixer() Call Failsave() _bl(1) = 113 _bl(4) = 113 Call Set_pwm() If State > 0 Then Incr Test If Itmp < 32 Then If Itmp > Itmp_old Then Rp_i = Rnd(20) - 10 Rp = Rp_i / 10 Wi_kp = Temperatur * Fp Wi_kp = Wi_kp * Rp Wi_kp = Wi_kp + W0_kp Ri_i = Rnd(20) - 10 Ri = Ri_i / 10 Wi_ki = Temperatur * Fi Wi_ki = Wi_ki * Ri Wi_ki = Wi_ki + W0_ki Rd_i = Rnd(20) - 10 Rd = Rd_i / 10 Wi_kd = Temperatur * Fd Wi_kd = Wi_kd * Rd Wi_kd = Wi_kd + W0_kd If Wi_kp < 0 Then Wi_kp = 0 If Wi_ki < 0 Then Wi_ki = 0 If Wi_kd < 0 Then Wi_kd = 0 '####### _roll_kp = Wi_kp _pitch_kp = Wi_kp _roll_ki = Wi_ki _pitch_ki = Wi_ki _roll_kd = Wi_kd _pitch_kd = Wi_kd End If Itmp_old = Itmp Incr Xtmp If Xtmp = 31 Then Incr Itmp Xtmp = 0 F = Getfitness() If F < Fcandidate Then Fcandidate = F Wcandidate_kp = Wi_kp Wcandidate_ki = Wi_ki Wcandidate_kd = Wi_kd End If Elseif Xtmp < 31 Then Ax(xtmp) = _roll_err End If End If If Itmp = 32 Then Itmp = 0 If Fcandidate < F0 Then Temperatur = Temperatur * T_a W0_kp = Wcandidate_kp W0_ki = Wcandidate_ki W0_kd = Wcandidate_kd F0 = Fcandidate End If End If Elseif State = 0 Then If Ischarwaiting() > 0 Then Input Rx_in Noecho B = Left(rx_in , 1) If Asc(b) = 10 Or Asc(b) = 13 Then Lenght = Len(rx_in) Lenght = Lenght - 1 Rx_in = Right(rx_in , Lenght) Clear Serialin '###### End If Send_logged_data = Instr(rx_in , "send!") If Send_logged_data = 1 Then Print Test ; " " ; Temperatur ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd Test = 0 Send_logged_data = 0 Clear Serialin '###### End If End If End If Loop Sub Simulated_annealing(byval N As Byte) Wcandidate_kp = 0.3 Wcandidate_ki = 0.0001 Wcandidate_kd = 0.000007 Temperatur = 1.0 T_a = 0.90 Fp = 0.1 Fi = 0.0002 Fd = 0.000005 Fcandidate = Getfitness() W0_kp = Wcandidate_kp W0_ki = Wcandidate_ki W0_kd = Wcandidate_kd F0 = Fcandidate Test = 0 While Fcandidate > 50 Incr Test For I = 1 To N Rp_i = Rnd(20) - 10 Rp = Rp_i / 10 '################ Wi_kp = Temperatur * Fp Wi_kp = Wi_kp * Rp Wi_kp = Wi_kp + W0_kp Ri_i = Rnd(20) - 10 Ri = Ri_i / 10 Wi_ki = Temperatur * Fi Wi_ki = Wi_ki * Ri Wi_ki = Wi_ki + W0_ki Rd_i = Rnd(20) - 10 Rd = Rd_i / 10 Wi_kd = Temperatur * Fd Wi_kd = Wi_kd * Rd Wi_kd = Wi_kd + W0_kd F = Getfitness() If F < Fcandidate Then Fcandidate = F Wcandidate_kp = Wi_kp Wcandidate_ki = Wi_ki Wcandidate_kd = Wi_kd End If Next I If Fcandidate < F0 Then Temperatur = Temperatur * T_a 'End If W0_kp = Wcandidate_kp '######### W0_ki = Wcandidate_ki W0_kd = Wcandidate_kd F0 = Fcandidate End If 'Print Test ; " " ; Temperatur ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd Wend End Sub Sub Log_data() If State > 0 Then Incr Log_cnt If Log_cnt < 30 Then Ax(log_cnt) = _rollnow 'Ay(log_cnt) = _pitchnow 'Az(log_cnt) = _yawnow Elseif Log_cnt = 250 Then Log_cnt = 200 End If Elseif State = 0 Then If Ischarwaiting() > 0 Then Input Rx_in Noecho B = Left(rx_in , 1) If Asc(b) = 10 Or Asc(b) = 13 Then Lenght = Len(rx_in) Lenght = Lenght - 1 Rx_in = Right(rx_in , Lenght) End If Send_logged_data = Instr(rx_in , "send!") If Send_logged_data = 1 Then F = Getfitness() Print F Send_logged_data = 0 End If End If End If End Sub Function Getfitness() As Single 'f = (ax(i) - ax(i-1))^2 + (ay(i) - ay(i-1))^2 + (az(i) - az(i-1))^2 Ex = 0 Ey = 0 Ez = 0 For A = 1 To 30 If A > 1 Then Atmp = A - 1 Else Atmp = 30 End If Extmp = Ax(a) - Ax(atmp) Extmp = Extmp * Extmp Eytmp = Ay(a) - Ay(atmp) Eytmp = Eytmp * Eytmp Eztmp = Az(a) - Az(atmp) Eztmp = Eztmp * Eztmp Ex = Ex + Extmp Ey = Ey + Eytmp Ez = Ez + Eztmp Next A Getfitness = Ex + Ey Getfitness = Getfitness + Ez End Function Sub Set_pwm() If _bl(1) > 250 Then _bl(1) = 250 If _bl(1) < 113 Then _bl(1) = 113 If _bl(2) > 250 Then _bl(2) = 250 If _bl(2) < 113 Then _bl(2) = 113 If _bl(3) > 250 Then _bl(3) = 250 If _bl(3) < 113 Then _bl(3) = 113 If _bl(4) > 250 Then _bl(4) = 250 If _bl(4) < 113 Then _bl(4) = 113 Pwm2a = _bl(4) 'links vorne (US) Pwm2b = _bl(2) 'links hinten (GUS) Pwm1a = _bl(3) 'rechts vorne (GUS) Pwm1b = _bl(1) 'rechts hinten (US) End Sub Sub Failsave() If Channel >= 11 Then If Failure < 255 Then Incr Failure End If End If If Channel < 11 Then If Failure > 0 Then Decr Failure End If End If End Sub Measure: If Channel > 0 And Channel < 9 Then Kanal(channel) = Timer0 End If Timer0 = 6 Incr Channel Return Pausedetect: If Channel <> 0 Then Newvalsreceived = 1 End If Channel = 0 Return Sub Filter_gyro_data() Call Read_wmp_data() _yawnow = Yaw - _yawoffset_int _rollnow = Roll - _rolloffset_int _pitchnow = Pitch - _pitchoffset_int End Sub Sub Filter_rx_data() If Newvalsreceived = 1 Then Newvalsreceived = 0 For I = 1 To Maxchannel Fkanal(i) = Fkanal(i) * 3 Fkanal(i) = Fkanal(i) + Kanal(i) Shift Fkanal(i) , Right , 2 Hempf(i) = Fkanal(i) + 17 Lempf(i) = Fkanal(i) - 17 If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then Kanal(i) = Fkanal(i) End If Next I If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100 _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2 If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60 If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68 End If For I = 2 To Maxchannel If Kanal(i) >= 62 And Kanal(i) < 139 Then _sbl(i) = Kanal(i) - 100 End If Next I If Kanal(5) < 80 Then State = 0 If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1 If Kanal(5) >= 115 Then State = 2 Setpoint_yaw = _sbl(_yawchannel) * 1.5 If State < 2 Then Setpoint_roll = _sbl(_rollchannel) * 0.6 Setpoint_pitch = _sbl(_pitchchannel) * 0.6 Else Setpoint_roll = _sbl(_rollchannel) * 1.5 Setpoint_pitch = _sbl(_pitchchannel) * 1.5 End If If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then Call Set_wmp_offset() End If Oldstate = State End If End Sub Sub Wmp_init() I2cstart I2cwbyte &HA6 I2cwbyte &HFE I2cwbyte &H04 I2cstop End Sub Sub Send_zero() I2cstart I2cwbyte &HA4 I2cwbyte &H00 I2cstop End Sub Sub Read_wmp_data() Gosub Send_zero I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6 Yaw1 = Wmplus_buffer(1) Roll1 = Wmplus_buffer(2) Pitch1 = Wmplus_buffer(3) Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4) Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5) Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6) Shift Yaw , Right , 4 Shift Roll , Right , 4 Shift Pitch , Right , 4 End Sub Sub Set_wmp_offset() _yawoffset = 0 _rolloffset = 0 _pitchoffset = 0 For I = 1 To 100 Call Read_wmp_data() _yawoffset = _yawoffset + Yaw _rolloffset = _rolloffset + Roll _pitchoffset = _pitchoffset + Pitch Next I _yawoffset = _yawoffset / 100 _rolloffset = _rolloffset / 100 _pitchoffset = _pitchoffset / 100 _yawoffset_int = _yawoffset _rolloffset_int = _rolloffset _pitchoffset_int = _pitchoffset End Sub Sub Pid_regulator() '-------------YAW--------------- _yaw_err = Setpoint_yaw - _yawnow _yaw_kp_err = _yaw_err * _yaw_kp _yaw_ki_sum = _yaw_ki_sum + _yaw_err If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000 If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000 _yaw_ki_err = _yaw_ki_sum * _yaw_ki _yaw_kd_err = _yaw_err - _yaw_kd_old _yaw_kd_old = _yaw_err _yaw_kd_err = _yaw_kd_err * _yaw_kd '-------------ROLL-------------- _roll_err = Setpoint_roll - _rollnow _roll_kp_err = _roll_err * _roll_kp _roll_ki_sum = _roll_ki_sum + _roll_err If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000 If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000 _roll_ki_err = _roll_ki_sum * _roll_ki _roll_kd_err = _roll_err - _roll_kd_old _roll_kd_old = _roll_err _roll_kd_err = _roll_kd_err * _roll_kd '-------------PITCH------------- _pitch_err = Setpoint_pitch - _pitchnow _pitch_kp_err = _pitch_err * _pitch_kp _pitch_ki_sum = _pitch_ki_sum + _pitch_err If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000 If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000 _pitch_ki_err = _pitch_ki_sum * _pitch_ki _pitch_kd_err = _pitch_err - _pitch_kd_old _pitch_kd_old = _pitch_err _pitch_kd_err = _pitch_kd_err * _pitch_kd _yaw_pid = _yaw_kp_err + _yaw_ki_err _yaw_pid = _yaw_pid + _yaw_kd_err _yaw_pid_int = _yaw_pid _roll_pid = _roll_kp_err + _roll_ki_err _roll_pid = _roll_pid + _roll_kd_err _roll_pid_int = _roll_pid _pitch_pid = _pitch_kp_err + _pitch_ki_err _pitch_pid = _pitch_pid + _pitch_kd_err _pitch_pid_int = _pitch_pid If _yaw_pid_int < -68 Then _yaw_pid_int = -68 If _yaw_pid_int > 68 Then _yaw_pid_int = 68 If _roll_pid_int < -68 Then _roll_pid_int = -68 If _roll_pid_int > 68 Then _roll_pid_int = 68 If _pitch_pid_int < -68 Then _pitch_pid_int = -68 If _pitch_pid_int > 68 Then _pitch_pid_int = 68 End Sub Sub Mixer() '_bl(1) = rechts hinten (US) '_bl(2) = links hinten (GUS) '_bl(3) = rechts vorne (GUS) '_bl(4) = links vorne (US) _bl(1) = 182 + _sbl(_throttlechannel) _bl(2) = _bl(1) _bl(3) = _bl(1) _bl(4) = _bl(1) If State <> 0 And Failure < 20 Then Portc.2 = 0 Portc.3 = 1 _bl(1) = _bl(1) + Minthrottle _bl(2) = _bl(2) + Minthrottle _bl(3) = _bl(3) + Minthrottle _bl(4) = _bl(4) + Minthrottle _bl(1) = _bl(1) - _pitch_pid_int _bl(4) = _bl(4) + _pitch_pid_int _bl(2) = _bl(2) - _roll_pid_int _bl(3) = _bl(3) + _roll_pid_int _bl(1) = _bl(1) - _yaw_pid_int _bl(2) = _bl(2) + _yaw_pid_int _bl(3) = _bl(3) + _yaw_pid_int _bl(4) = _bl(4) - _yaw_pid_int Else Portc.2 = 0 _bl(1) = 113 _bl(2) = 113 _bl(3) = 113 _bl(4) = 113 Incr _blink If _blink = 100 Then Portc.3 = 1 _blink = 101 End If If _blink = 200 Then Portc.3 = 0 _blink = 0 End If _yaw_kp_err = 0 _yaw_ki_sum = 0 _yaw_kd_err = 0 _yaw_ki_err = 0 _roll_kp_err = 0 _roll_ki_sum = 0 _roll_kd_err = 0 _roll_ki_err = 0 _pitch_kp_err = 0 _pitch_ki_sum = 0 _pitch_kd_err = 0 _pitch_ki_err = 0 _yaw_pid_int = 0 _roll_pid_int = 0 _pitch_pid_int = 0 _yaw_pid = 0 _roll_pid = 0 _pitch_pid = 0 Yawstickvel = 0 Rollstickvel = 0 Pitchstickvel = 0 Yawstickold = 0 Rollstickold = 0 Pitchstickold = 0 End If End Sub Sub Init_system() _bl(1) = 113 _bl(2) = 113 _bl(3) = 113 _bl(4) = 113 _sbl(1) = -68 _sbl(2) = 0 _sbl(3) = 0 _sbl(4) = 0 _sbl(5) = -600 _sbl(6) = -600 _sbl(7) = 0 _sbl(8) = 0 Fkanal(1) = 63 Fkanal(2) = 100 Fkanal(3) = 100 Fkanal(4) = 100 Fkanal(5) = 63 Fkanal(6) = 63 Fkanal(7) = 100 Fkanal(8) = 100 Reset Newvalsreceived State = 0 Oldstate = 0 _yawnow = 0 _rollnow = 0 _pitchnow = 0 Call Wmp_init() Waitms 500 For I = 1 To 10 Call Read_wmp_data() Next I Waitms 500 Call Set_wmp_offset() Rc_on = 0 End Sub End

Ich weiß, manche Variablen haben komische Namen, das wird noch ausgebessert. Zur Info: XTMP und ITMP sind reine Zählvariablen. Vielleicht hat jemand von euch ja die Geduld, sich das ganze mal kurz anzusehen (also nur / hauptsächlich den Simulated-Annealing-Teil im Main-loop)?!

Vielen Dank & Gruß
Chris

EDIT:
Kennt jemand ein gutes Buch, in dem ausführlich auf sowohl Theorie, als auch Praxis des Simulated Annealing eingegangen wird? Am besten mit ein paar Programmcodes als Beispiel ... Wäre nett :)

EDIT2:
Hab mich jetzt selbst mal an einem SA versucht, vielleicht wäre jemand ja mal so nett, um kurz drüberzuschauen, ob grobe Fehler drin sind.

Code:

$regfile = "m32def.dat" $crystal = 16000000 $framesize = 80 $hwstack = 80 $swstack = 80 $baud = 38400 Declare Function Getfitness() As Single Dim T As Single Dim T_a As Single Dim Wi_kp As Single Dim Wi_ki As Single Dim Wi_kd As Single Dim W0_kp As Single Dim W0_ki As Single Dim W0_kd As Single Dim Rp As Single Dim Ri As Single Dim Rd As Single Dim Rp_i As Integer Dim Ri_i As Integer Dim Rd_i As Integer Dim Fp As Single Dim Fi As Single Dim Fd As Single Dim F As Single Dim F0 As Single '------evtl. als long!!!------------------------------ Dim Extmp As Single 'Integer Dim Eytmp As Single 'Integer Dim Eztmp As Single 'Integer Dim Ax(50) As Single 'Integer Dim Ay(50) As Single 'Integer Dim Az(50) As Single 'Integer Dim Ex As Single 'Integer Dim Ey As Single 'Integer Dim Ez As Single 'Integer Dim Atmp As Word Dim A As Word Dim Log_cnt As Word Dim Max_log_cnt As Word Dim Tries As Word Dim Max_tries As Word Dim _roll_kp As Single Dim _roll_ki As Single Dim _roll_kd As Single Dim _rollnow As Integer Dim X As Integer '-------initialize the values T = 1.0 T_a = 0.98 F0 = 500.0 F = F0 W0_kp = 5 W0_ki = 0.1 W0_kd = 0.043 Fp = 2 Fi = 0.08 Fd = 0.005 Max_log_cnt = 15 Max_tries = 250 Enable Interrupts Do '-------randomize according to the current temperature-------- Rp_i = Rnd(200) - 100 Rp = Rp_i * 0.01 Rp = Rp * T Ri_i = Rnd(200) - 100 Ri = Ri_i * 0.01 Ri = Ri * T Rd_i = Rnd(200) - 100 Rd = Rd_i * 0.01 Rd = Rd * T '-------generate new parameters------------------------------ Wi_kp = Rp * Fp Wi_kp = Wi_kp + W0_kp Wi_ki = Ri * Fi Wi_ki = Wi_ki + W0_ki Wi_kd = Rd * Fd Wi_kd = Wi_kd + W0_kd '-------test current parameters------------------------------ _roll_kp = Wi_kp _roll_ki = Wi_ki _roll_kd = Wi_kd '-------log data for fitness function------------------------ Incr Log_cnt If Log_cnt < Max_log_cnt Then Ax(log_cnt) = _rollnow '################################################################################ Elseif Log_cnt = Max_log_cnt Then F = Getfitness() Log_cnt = 0 End If '-------better then current solution?------------------------ If F < F0 Then F0 = F W0_kp = Wi_kp W0_ki = Wi_ki W0_kd = Wi_kd T = T * T_a Print Tries ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd End If '-------counter for max tries--------------------------------- Incr Tries Loop Function Getfitness() As Single 'f = (ax(i) - ax(i-1))^2 + (ay(i) - ay(i-1))^2 + (az(i) - az(i-1))^2 Ex = 0 Ey = 0 Ez = 0 For A = 1 To Max_log_cnt If A > 1 Then Atmp = A - 1 Else Atmp = Max_log_cnt End If Extmp = Ax(a) - Ax(atmp) Extmp = Extmp * Extmp Eytmp = Ay(a) - Ay(atmp) Eytmp = Eytmp * Eytmp Eztmp = Az(a) - Az(atmp) Eztmp = Eztmp * Eztmp Ex = Ex + Extmp Ey = Ey + Eytmp Ez = Ez + Eztmp Next A Getfitness = Ex + Ey Getfitness = Getfitness + Ez End Function End

Zitat:

Zitat von Che Guevara

Warum ist bei der IF-Abfrage keine {-Klammer vor T*=a?

Hinter der IF Abfrage ist keine {-Klammer weil nur ein Kommando ("T*=a") steht.
Man könnte stattdessen auch "if (fcandidate<f0) { T*=a;}" schreiben.
Ist aber das gleiche. Wenn man mehrere Befehle/Kommandos hat muss man eine {-Klammer schreiben.

MfG Hannes

Tip: Ich kenne mich zwar mit Optimierungsmethoden wenig aus, weiß aber, dass MATLAB eine Menge Algorithmen hierfür bereithält.

Oh... ok :)
Ist zwar jetzt sowieso nicht mehr relevant, da ich meinen eigenen Algorithmus geschrieben habe, aber trotzdem danke, Wissen ist nie verkehrt!

Hm also eigentlich soll der Algorithmus später dann die ganze Zeit auf dem Quadrocopter mitlaufen, damit die Parameter ständig angepasst werden können, falls nötig. Deswegen muss ich auf einen PC usw.. verzichten. Trotzdem danke für den Tipp.

Heute werde ich mal, sofern ich Zeit finde, meinen Algorithmus testen und sehen, was dabei rauskommt. Sobald ich Ergebnisse habe, poste ich sie hier.

Gruß
Chris

EDIT:
Hier ist jetzt mal mein aktueller Code:

Code:

$regfile = "m328pdef.dat" $crystal = 16000000 $framesize = 100 $hwstack = 100 $swstack = 100 $baud = 38400 Declare Sub Serial0charmatch() Config Serialin = Buffered , Size = 30 , Bytematch = 13 Declare Sub Simulated_annealing() Dim T As Single Dim T_a As Single Dim Wi_kp As Single Dim Wi_ki As Single Dim Wi_kd As Single Dim W0_kp As Single Dim W0_ki As Single Dim W0_kd As Single Dim Rp As Single Dim Ri As Single Dim Rd As Single Dim Rp_i As Integer Dim Ri_i As Integer Dim Rd_i As Integer Dim Fp As Single Dim Fi As Single Dim Fd As Single Dim F As Double Dim F0 As Double '------evtl. als long!!!------------------------------ Dim Extmp As Double Dim Eytmp As Double Dim Eztmp As Double Dim Ax(50) As Double Dim Ay(50) As Double Dim Az(50) As Double Dim Ex As Double Dim Ey As Double Dim Ez As Double Dim Atmp As Word Dim A As Word Dim Log_step As Byte Dim Log_cnt As Word Dim Max_log_cnt As Word Dim Tries As Word Dim Max_tries As Word Dim Test_anz As Long Dim X As Integer '################################################################################################################################################################ '################################################################################################################################################################ '################################################################################################################################################################ Declare Sub Init_system() Declare Sub Filter_gyro_data() Declare Sub Filter_rx_data() Declare Sub Mixer() Declare Sub Pid_regulator() Declare Sub Wmp_init() Declare Sub Send_zero() Declare Sub Read_wmp_data() Declare Sub Set_wmp_offset() Declare Sub Failsave() Declare Sub Set_pwm() Config Portc.2 = Output Config Portc.3 = Output Portc.2 = 0 Portc.3 = 0 Config Portb.1 = Output Config Portb.2 = Output Config Portd.5 = Output Config Portd.6 = Output Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64 Pwm1a = 113 Pwm1b = 113 Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64 Pwm2a = 113 Pwm2b = 113 Config Pind.2 = Input Portd.2 = 0 Config Int0 = Falling On Int0 Measure Enable Int0 Config Timer0 = Timer , Prescale = 256 On Timer0 Pausedetect Enable Timer0 $lib "I2C_TWI.LBX" Config Scl = Portc.5 Config Sda = Portc.4 Config Twi = 100000 I2cinit Const Minthrottle = 30 Const Maxchannel = 8 Const _throttlechannel = 1 Const _rollchannel = 2 Const _pitchchannel = 3 Const _yawchannel = 4 Const _statechannel = 5 Const _datachannel = 6 Dim I As Byte Dim J As Byte Dim Newvalsreceived As Bit Dim _blink As Byte Dim _yaw_kp As Single Dim _roll_kp As Single Dim _pitch_kp As Single Dim _yaw_ki As Single Dim _roll_ki As Single Dim _pitch_ki As Single Dim _yaw_kd As Single Dim _roll_kd As Single Dim _pitch_kd As Single _yaw_kp = 0.4 '0.32 _roll_kp = 0.4 '0.32 _pitch_kp = 0.4 '0.32 _yaw_ki = 0.0008 '0.0008 _roll_ki = 0.0015 '0.0018 _pitch_ki = 0.0015 '0.0018 _yaw_kd = 0 _roll_kd = 0 _pitch_kd = 0 '_yaw_ki = 0.0001 '0.000135 '_roll_ki = 0.0001 '0.000135 '_pitch_ki = 0.0001 '0.000135 '_yaw_kd = 0.000007 '0.0000096 '_roll_kd = 0.000007 '0.0000096 '_pitch_kd = 0.000007 '0.0000096 '################################### '#########RC-EMPFÄNGER############## '################################### Dim Bufferbyte As Byte Dim Kanal(maxchannel) As Word Dim Fkanal(maxchannel) As Word Dim Channel As Byte Dim _bl(4) As Word Dim _sbl(maxchannel) As Integer Dim State As Byte Dim Oldstate As Byte '################################### '################################### '################################### '################################### '###########I2C-Inputs############## '################################### Dim Wmplus_buffer(6) As Byte Dim Ar(6) As Byte '################################### '################################### '################################### '################################### '#########GYRO###################### '################################### Dim Yaw As Word Dim Yaw0 As Byte At Yaw + 1 Overlay Dim Yaw1 As Byte At Yaw Overlay Dim Roll As Word Dim Roll0 As Byte At Roll + 1 Overlay Dim Roll1 As Byte At Roll Overlay Dim Pitch As Word Dim Pitch0 As Byte At Pitch + 1 Overlay Dim Pitch1 As Byte At Pitch Overlay Dim _yawoffset As Long Dim _rolloffset As Long Dim _pitchoffset As Long Dim _yawoffset_int As Integer Dim _rolloffset_int As Integer Dim _pitchoffset_int As Integer Dim _yawnow As Integer Dim _rollnow As Integer Dim _pitchnow As Integer '################################### '################################### '################################### '################################## '#########PID-REGLER############### '################################## Dim _yaw_kp_err As Single Dim _roll_kp_err As Single Dim _pitch_kp_err As Single Dim _yaw_ki_err As Single Dim _roll_ki_err As Single Dim _pitch_ki_err As Single Dim _yaw_ki_sum As Single Dim _roll_ki_sum As Single Dim _pitch_ki_sum As Single Dim _yaw_kd_err As Single Dim _roll_kd_err As Single Dim _pitch_kd_err As Single Dim _yaw_kd_old As Single Dim _roll_kd_old As Single Dim _pitch_kd_old As Single Dim _yaw_pid_int As Integer Dim _roll_pid_int As Integer Dim _pitch_pid_int As Integer Dim _yaw_pid As Single Dim _roll_pid As Single Dim _pitch_pid As Single Dim _yaw_err_int As Integer Dim _roll_err_int As Integer Dim _pitch_err_int As Integer Dim _yaw_err As Single Dim _roll_err As Single Dim _pitch_err As Single '################################# '################################# '################################# Dim Setpoint_yaw As Single Dim Setpoint_roll As Single Dim Setpoint_pitch As Single Dim Yawstickvel As Integer Dim Rollstickvel As Integer Dim Pitchstickvel As Integer Dim Yawstickold As Integer Dim Rollstickold As Integer Dim Pitchstickold As Integer Dim Hempf(maxchannel) As Word Dim Lempf(maxchannel) As Word Dim _x1 As Single Dim _x2 As Single Dim Rc_on As Word Dim Failure As Byte '_bl(1) = rechts hinten (US) '_bl(2) = links hinten (GUS) '_bl(3) = rechts vorne (GUS) '_bl(4) = links vorne (US) 'PWM = 113 --> 0.900us 'PWM = 250 --> 2.000ms '################################################################################################################################################################ '################################################################################################################################################################ '################################################################################################################################################################ '-------initialize the values '_roll_kp = 0.4 '_roll_ki = 0.0015 T = 1.0 T_a = 0.75 F0 = 500 F = F0 W0_kp = 0.3 W0_ki = 0.001 W0_kd = 0.000005 Fp = 0.2 Fi = 0.0008 Fd = 0.00001 Log_step = 3 Max_log_cnt = 90 Max_tries = 30 Call Init_system() Waitms 500 Enable Interrupts Waitms 500 Rc_test: If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then If Rc_on < 65000 Then Incr Rc_on Else If Rc_on > 0 Then Decr Rc_on End If If Rc_on < 500 Then Goto Rc_test End If Do Call Filter_rx_data() Call Filter_gyro_data() Call Pid_regulator() Call Mixer() Call Failsave() '##### _bl(1) = 113 _bl(4) = 113 '##### Call Set_pwm() Call Simulated_annealing() Loop Sub Simulated_annealing() If State > 0 Then Incr Test_anz '-------randomize according to the current temperature-------- Rp_i = Rnd(200) - 100 Rp = Rp_i * 0.01 Rp = Rp * T Ri_i = Rnd(200) - 100 Ri = Ri_i * 0.01 Ri = Ri * T Rd_i = Rnd(200) - 100 Rd = Rd_i * 0.01 Rd = Rd * T '-------generate new parameters------------------------------ Wi_kp = Rp * Fp Wi_kp = Wi_kp + W0_kp Wi_ki = Ri * Fi Wi_ki = Wi_ki + W0_ki Wi_kd = Rd * Fd Wi_kd = Wi_kd + W0_kd '-------cut off parameters----------------------------------- If Wi_kp < 0 Then Wi_kp = 0 If Wi_ki < 0 Then Wi_ki = 0 '-------test current parameters------------------------------ _roll_kp = Wi_kp _roll_ki = Wi_ki _roll_kd = Wi_kd '-------log data and get fitness----------------------------- Incr Log_cnt If Log_cnt < Max_log_cnt Then Ax(log_cnt) = _roll_err '#### If Log_cnt > Log_step Then Atmp = Log_cnt - Log_step Else Atmp = 1 End If Extmp = Ax(log_cnt) - Ax(atmp) Extmp = Extmp ^ 2 Eytmp = Ay(log_cnt) - Ay(atmp) Eytmp = Eytmp ^ 2 Eztmp = Az(log_cnt) - Az(atmp) Eztmp = Eztmp ^ 2 Ex = Ex + Extmp Ey = Ey + Eytmp Ez = Ez + Eztmp '#### Elseif Log_cnt = Max_log_cnt Then F = Ex + Ey F = F + Ez Ex = 0 Ey = 0 Ez = 0 Log_cnt = 0 Incr Tries End If '-------better then current solution?------------------------ If F < F0 Then F0 = F W0_kp = Wi_kp W0_ki = Wi_ki W0_kd = Wi_kd End If '-------counter for max tries--------------------------------- If Tries = Max_tries Then Toggle Portc.2 T = T * T_a Tries = 0 End If End If End Sub Sub Serial0charmatch() Clear Serialin If State = 0 Then 'Print F Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd End If End Sub Sub Set_pwm() If _bl(1) > 250 Then _bl(1) = 250 If _bl(1) < 113 Then _bl(1) = 113 If _bl(2) > 250 Then _bl(2) = 250 If _bl(2) < 113 Then _bl(2) = 113 If _bl(3) > 250 Then _bl(3) = 250 If _bl(3) < 113 Then _bl(3) = 113 If _bl(4) > 250 Then _bl(4) = 250 If _bl(4) < 113 Then _bl(4) = 113 Pwm2a = _bl(4) 'links vorne (US) Pwm2b = _bl(2) 'links hinten (GUS) Pwm1a = _bl(3) 'rechts vorne (GUS) Pwm1b = _bl(1) 'rechts hinten (US) End Sub Sub Failsave() If Channel >= 11 Then If Failure < 255 Then Incr Failure End If End If If Channel < 11 Then If Failure > 0 Then Decr Failure End If End If End Sub Measure: If Channel > 0 And Channel < 9 Then Kanal(channel) = Timer0 End If Timer0 = 6 Incr Channel Return Pausedetect: If Channel <> 0 Then Newvalsreceived = 1 End If Channel = 0 Return Sub Filter_gyro_data() Call Read_wmp_data() _yawnow = Yaw - _yawoffset_int _rollnow = Roll - _rolloffset_int _pitchnow = Pitch - _pitchoffset_int End Sub Sub Filter_rx_data() If Newvalsreceived = 1 Then Newvalsreceived = 0 For I = 1 To Maxchannel Fkanal(i) = Fkanal(i) * 3 Fkanal(i) = Fkanal(i) + Kanal(i) Shift Fkanal(i) , Right , 2 Hempf(i) = Fkanal(i) + 17 Lempf(i) = Fkanal(i) - 17 If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then Kanal(i) = Fkanal(i) End If Next I If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100 _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2 If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60 If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68 End If For I = 2 To Maxchannel If Kanal(i) >= 62 And Kanal(i) < 139 Then _sbl(i) = Kanal(i) - 100 End If Next I If Kanal(5) < 80 Then State = 0 If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1 If Kanal(5) >= 115 Then State = 2 Setpoint_yaw = _sbl(_yawchannel) * 1.5 If State < 2 Then Setpoint_roll = _sbl(_rollchannel) * 0.6 Setpoint_pitch = _sbl(_pitchchannel) * 0.6 Else Setpoint_roll = _sbl(_rollchannel) * 1.5 Setpoint_pitch = _sbl(_pitchchannel) * 1.5 End If If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then Call Set_wmp_offset() End If Oldstate = State End If End Sub Sub Wmp_init() I2cstart I2cwbyte &HA6 I2cwbyte &HFE I2cwbyte &H04 I2cstop End Sub Sub Send_zero() I2cstart I2cwbyte &HA4 I2cwbyte &H00 I2cstop End Sub Sub Read_wmp_data() Gosub Send_zero I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6 Yaw1 = Wmplus_buffer(1) Roll1 = Wmplus_buffer(2) Pitch1 = Wmplus_buffer(3) Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4) Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5) Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6) Shift Yaw , Right , 4 Shift Roll , Right , 4 Shift Pitch , Right , 4 End Sub Sub Set_wmp_offset() _yawoffset = 0 _rolloffset = 0 _pitchoffset = 0 For I = 1 To 100 Call Read_wmp_data() _yawoffset = _yawoffset + Yaw _rolloffset = _rolloffset + Roll _pitchoffset = _pitchoffset + Pitch Next I _yawoffset = _yawoffset / 100 _rolloffset = _rolloffset / 100 _pitchoffset = _pitchoffset / 100 _yawoffset_int = _yawoffset _rolloffset_int = _rolloffset _pitchoffset_int = _pitchoffset End Sub Sub Pid_regulator() '-------------YAW--------------- _yaw_err = Setpoint_yaw - _yawnow _yaw_kp_err = _yaw_err * _yaw_kp _yaw_ki_sum = _yaw_ki_sum + _yaw_err If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000 If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000 _yaw_ki_err = _yaw_ki_sum * _yaw_ki _yaw_kd_err = _yaw_err - _yaw_kd_old _yaw_kd_old = _yaw_err _yaw_kd_err = _yaw_kd_err * _yaw_kd '-------------ROLL-------------- _roll_err = Setpoint_roll - _rollnow _roll_kp_err = _roll_err * _roll_kp _roll_ki_sum = _roll_ki_sum + _roll_err If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000 If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000 _roll_ki_err = _roll_ki_sum * _roll_ki _roll_kd_err = _roll_err - _roll_kd_old _roll_kd_old = _roll_err _roll_kd_err = _roll_kd_err * _roll_kd '-------------PITCH------------- _pitch_err = Setpoint_pitch - _pitchnow _pitch_kp_err = _pitch_err * _pitch_kp _pitch_ki_sum = _pitch_ki_sum + _pitch_err If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000 If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000 _pitch_ki_err = _pitch_ki_sum * _pitch_ki _pitch_kd_err = _pitch_err - _pitch_kd_old _pitch_kd_old = _pitch_err _pitch_kd_err = _pitch_kd_err * _pitch_kd _yaw_pid = _yaw_kp_err + _yaw_ki_err _yaw_pid = _yaw_pid + _yaw_kd_err _yaw_pid_int = _yaw_pid _roll_pid = _roll_kp_err + _roll_ki_err _roll_pid = _roll_pid + _roll_kd_err _roll_pid_int = _roll_pid _pitch_pid = _pitch_kp_err + _pitch_ki_err _pitch_pid = _pitch_pid + _pitch_kd_err _pitch_pid_int = _pitch_pid If _yaw_pid_int < -68 Then _yaw_pid_int = -68 If _yaw_pid_int > 68 Then _yaw_pid_int = 68 If _roll_pid_int < -68 Then _roll_pid_int = -68 If _roll_pid_int > 68 Then _roll_pid_int = 68 If _pitch_pid_int < -68 Then _pitch_pid_int = -68 If _pitch_pid_int > 68 Then _pitch_pid_int = 68 End Sub Sub Mixer() '_bl(1) = rechts hinten (US) '_bl(2) = links hinten (GUS) '_bl(3) = rechts vorne (GUS) '_bl(4) = links vorne (US) _bl(1) = 182 + _sbl(_throttlechannel) _bl(2) = _bl(1) _bl(3) = _bl(1) _bl(4) = _bl(1) If State <> 0 And Failure < 20 Then _bl(1) = _bl(1) + Minthrottle _bl(2) = _bl(2) + Minthrottle _bl(3) = _bl(3) + Minthrottle _bl(4) = _bl(4) + Minthrottle _bl(1) = _bl(1) - _pitch_pid_int _bl(4) = _bl(4) + _pitch_pid_int _bl(2) = _bl(2) - _roll_pid_int _bl(3) = _bl(3) + _roll_pid_int _bl(1) = _bl(1) - _yaw_pid_int _bl(2) = _bl(2) + _yaw_pid_int _bl(3) = _bl(3) + _yaw_pid_int _bl(4) = _bl(4) - _yaw_pid_int Else _bl(1) = 113 _bl(2) = 113 _bl(3) = 113 _bl(4) = 113 _yaw_kp_err = 0 _yaw_ki_sum = 0 _yaw_kd_err = 0 _yaw_ki_err = 0 _roll_kp_err = 0 _roll_ki_sum = 0 _roll_kd_err = 0 _roll_ki_err = 0 _pitch_kp_err = 0 _pitch_ki_sum = 0 _pitch_kd_err = 0 _pitch_ki_err = 0 _yaw_pid_int = 0 _roll_pid_int = 0 _pitch_pid_int = 0 _yaw_pid = 0 _roll_pid = 0 _pitch_pid = 0 Yawstickvel = 0 Rollstickvel = 0 Pitchstickvel = 0 Yawstickold = 0 Rollstickold = 0 Pitchstickold = 0 End If End Sub Sub Init_system() _bl(1) = 113 _bl(2) = 113 _bl(3) = 113 _bl(4) = 113 _sbl(1) = -68 _sbl(2) = 0 _sbl(3) = 0 _sbl(4) = 0 _sbl(5) = -600 _sbl(6) = -600 _sbl(7) = 0 _sbl(8) = 0 Fkanal(1) = 63 Fkanal(2) = 100 Fkanal(3) = 100 Fkanal(4) = 100 Fkanal(5) = 63 Fkanal(6) = 63 Fkanal(7) = 100 Fkanal(8) = 100 Reset Newvalsreceived State = 0 Oldstate = 0 _yawnow = 0 _rollnow = 0 _pitchnow = 0 Call Wmp_init() Waitms 500 For I = 1 To 10 Call Read_wmp_data() Next I Waitms 500 Call Set_wmp_offset() Rc_on = 0 End Sub End

Im Unterschied zu meiner vorherigen Version ist hier die Fitness-Funtkion anders aufgebaut. Es wird nicht, wie vorher, die Fitness der geloggten Daten auf einmal bestimmt, sondern es wird nach jedem Datenloggen wieder ein Teil der Fitness bestimmt. Das hat den Vorteil, dass die Rechenzeit für die Fitness-Funktion gleichmäßiger verteilt ist und nicht mehr auf einen kurzen Zeitpunkt fixiert. Somit kann die Regelung besser funktionieren.
Das einzige, was jetzt noch fehlt, ist die "kontrollierte unkontrollierte" Abweichung im System, z.b. eine Windböe in der Software nachgebildet ^^
Das möchte ich über die Variable Log_cnt realisieren: Wenn diese in dem Bereich von x1 bis x2 ist, wird eine RC-Eingabe simuliert (z.b. Vollausschlag links)... Hier kann ich dann versch. beliebige Muster einprogrammieren, evtl. sogar über eine GUI (wird sicherlich noch folgen).
Habe mir jetzt auch zwei Bücher bestellt, welche das Thema naturanaloge Algorithmen behandeln (also auch Simulated Annealing, genet. Algos, usw...). Vielleicht finde ich ja darin dann noch ein paar Tipps zur besseren Umsetzung... Es gäbe da noch einiges, z.b. die Möglichkeit, einen Kandidaten mit schlechterer Fitness trotzdem zu übernehmen, um lokale Minima zu vermeiden, usw... Das kommt aber erst, wenn der Grundalgo mal läuft. Aber so wies aussieht, läuft er schon :)
Wenn Interresse besteht, kann ich ja mal ein Video von meinem 5 Minuten Teststand reinstellen...

Hallo,

nun bin ich wieder etwas weiter, nachdem ich die Fitness-Funktion debuggt habe. Jetzt kommen richtige Werte raus, vorher nicht! Außerdem habe ich jetzt noch eine Art Startup Prozedur eingebaut, d.h. erst schwingt sich der Quadro ein und dann wird die Fitness von den Startparametern bestimmt.
Hier der Code:

Code:

$regfile = "m328pdef.dat" $crystal = 16000000 $framesize = 200 $hwstack = 200 $swstack = 200 $baud = 38400 Declare Sub Serial0charmatch() Config Serialin = Buffered , Size = 30 , Bytematch = 13 Declare Sub Simulated_annealing() Dim T As Single Dim T_a As Single Dim Wi_kp As Single Dim Wi_ki As Single Dim Wi_kd As Single Dim W0_kp As Single Dim W0_ki As Single Dim W0_kd As Single Dim Rp As Single Dim Ri As Single Dim Rd As Single Dim Rp_i As Integer Dim Ri_i As Integer Dim Rd_i As Integer Dim Fp As Single Dim Fi As Single Dim Fd As Single Dim F As Long Dim F0 As Long '------evtl. als long!!!------------------------------ Dim Extmp As Long Dim Eytmp As Long Dim Eztmp As Long Dim Ax(50) As Integer 'Integer Dim Ay(50) As Integer 'Integer Dim Az(50) As Integer 'Integer Dim Ex As Long Dim Ey As Long Dim Ez As Long Dim Atmp As Word Dim A As Word Dim Log_step As Byte Dim Log_cnt As Word Dim Max_log_cnt As Word Dim Tries As Word Dim Max_tries As Word Dim Test_anz As Long Dim X As Integer '################################################################################################################################################################ '################################################################################################################################################################ '################################################################################################################################################################ Declare Sub Init_system() Declare Sub Filter_gyro_data() Declare Sub Filter_rx_data() Declare Sub Mixer() Declare Sub Pid_regulator() Declare Sub Wmp_init() Declare Sub Send_zero() Declare Sub Read_wmp_data() Declare Sub Set_wmp_offset() Declare Sub Failsave() Declare Sub Set_pwm() Config Portc.2 = Output Config Portc.3 = Output Portc.2 = 0 Portc.3 = 0 Config Portb.1 = Output Config Portb.2 = Output Config Portd.5 = Output Config Portd.6 = Output Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64 Pwm1a = 113 Pwm1b = 113 Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64 Pwm2a = 113 Pwm2b = 113 Config Pind.2 = Input Portd.2 = 0 Config Int0 = Falling On Int0 Measure Enable Int0 Config Timer0 = Timer , Prescale = 256 On Timer0 Pausedetect Enable Timer0 $lib "I2C_TWI.LBX" Config Scl = Portc.5 Config Sda = Portc.4 Config Twi = 100000 I2cinit Const Minthrottle = 50 '30 Const Maxchannel = 8 Const _throttlechannel = 1 Const _rollchannel = 2 Const _pitchchannel = 3 Const _yawchannel = 4 Const _statechannel = 5 Const _datachannel = 6 Dim I As Byte Dim J As Byte Dim Newvalsreceived As Bit Dim _blink As Byte Dim _yaw_kp As Single Dim _roll_kp As Single Dim _pitch_kp As Single Dim _yaw_ki As Single Dim _roll_ki As Single Dim _pitch_ki As Single Dim _yaw_kd As Single Dim _roll_kd As Single Dim _pitch_kd As Single _yaw_kp = 0.4 '0.32 _roll_kp = 0.4 '0.32 _pitch_kp = 0.4 '0.32 _yaw_ki = 0.0008 '0.0008 _roll_ki = 0.0015 '0.0018 _pitch_ki = 0.0015 '0.0018 _yaw_kd = 0 _roll_kd = 0 _pitch_kd = 0 '_yaw_ki = 0.0001 '0.000135 '_roll_ki = 0.0001 '0.000135 '_pitch_ki = 0.0001 '0.000135 '_yaw_kd = 0.000007 '0.0000096 '_roll_kd = 0.000007 '0.0000096 '_pitch_kd = 0.000007 '0.0000096 '################################### '#########RC-EMPFÄNGER############## '################################### Dim Bufferbyte As Byte Dim Kanal(maxchannel) As Word Dim Fkanal(maxchannel) As Word Dim Channel As Byte Dim _bl(4) As Word Dim _sbl(maxchannel) As Integer Dim State As Byte Dim Oldstate As Byte '################################### '################################### '################################### '################################### '###########I2C-Inputs############## '################################### Dim Wmplus_buffer(6) As Byte Dim Ar(6) As Byte '################################### '################################### '################################### '################################### '#########GYRO###################### '################################### Dim Yaw As Word Dim Yaw0 As Byte At Yaw + 1 Overlay Dim Yaw1 As Byte At Yaw Overlay Dim Roll As Word Dim Roll0 As Byte At Roll + 1 Overlay Dim Roll1 As Byte At Roll Overlay Dim Pitch As Word Dim Pitch0 As Byte At Pitch + 1 Overlay Dim Pitch1 As Byte At Pitch Overlay Dim _yawoffset As Long Dim _rolloffset As Long Dim _pitchoffset As Long Dim _yawoffset_int As Integer Dim _rolloffset_int As Integer Dim _pitchoffset_int As Integer Dim _yawnow As Integer Dim _rollnow As Integer Dim _pitchnow As Integer '################################### '################################### '################################### '################################## '#########PID-REGLER############### '################################## Dim _yaw_kp_err As Single Dim _roll_kp_err As Single Dim _pitch_kp_err As Single Dim _yaw_ki_err As Single Dim _roll_ki_err As Single Dim _pitch_ki_err As Single Dim _yaw_ki_sum As Single Dim _roll_ki_sum As Single Dim _pitch_ki_sum As Single Dim _yaw_kd_err As Single Dim _roll_kd_err As Single Dim _pitch_kd_err As Single Dim _yaw_kd_old As Single Dim _roll_kd_old As Single Dim _pitch_kd_old As Single Dim _yaw_pid_int As Integer Dim _roll_pid_int As Integer Dim _pitch_pid_int As Integer Dim _yaw_pid As Single Dim _roll_pid As Single Dim _pitch_pid As Single Dim _yaw_err_int As Integer Dim _roll_err_int As Integer Dim _pitch_err_int As Integer Dim _yaw_err As Single Dim _roll_err As Single Dim _pitch_err As Single '################################# '################################# '################################# Dim Setpoint_yaw As Single Dim Setpoint_roll As Single Dim Setpoint_pitch As Single Dim Yawstickvel As Integer Dim Rollstickvel As Integer Dim Pitchstickvel As Integer Dim Yawstickold As Integer Dim Rollstickold As Integer Dim Pitchstickold As Integer Dim Hempf(maxchannel) As Word Dim Lempf(maxchannel) As Word Dim _x1 As Single Dim _x2 As Single Dim Rc_on As Word Dim Failure As Byte Dim New_parameters As Bit Dim Sim_an_oldstate As Byte '_bl(1) = rechts hinten (US) '_bl(2) = links hinten (GUS) '_bl(3) = rechts vorne (GUS) '_bl(4) = links vorne (US) 'PWM = 113 --> 0.900us 'PWM = 250 --> 2.000ms '################################################################################################################################################################ '################################################################################################################################################################ '################################################################################################################################################################ '-------initialize the values '_roll_kp = 0.4 '_roll_ki = 0.0015 T = 1.0 T_a = 0.98 '0.8 F0 = 99999999 F = F0 W0_kp = 0.4 W0_ki = 0.0015 '0.001 W0_kd = 0.0 '0.000005 Fp = 0.2 Fi = 0.001 '0.0008 Fd = 0.00001 '0.00001 Log_step = 5 Max_log_cnt = 80 '50 Max_tries = 50 '25 New_parameters = 1 Dim Einschwingen As Word Einschwingen = 0 Call Init_system() Waitms 500 Enable Interrupts Waitms 500 Rc_test: If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then If Rc_on < 65000 Then Incr Rc_on Else If Rc_on > 0 Then Decr Rc_on End If If Rc_on < 500 Then Goto Rc_test End If Do Call Filter_rx_data() Call Filter_gyro_data() Call Pid_regulator() Call Mixer() Call Failsave() '##### _bl(1) = 113 _bl(4) = 113 '##### Call Set_pwm() If State > 0 Then Incr Einschwingen End If Loop Until Einschwingen = 3000 Portc.2 = 1 Do Call Filter_rx_data() Call Filter_gyro_data() If State > 0 Then _roll_kp = W0_kp _roll_ki = W0_ki _roll_kd = W0_kd '-------log data and get fitness----------------------------- Incr Log_cnt If Log_cnt < Max_log_cnt Then Ax(log_cnt) = _roll_err '_rollnow If Log_cnt > Log_step Then Atmp = Log_cnt - Log_step Extmp = Ax(log_cnt) - Ax(atmp) Extmp = Extmp * Extmp Ex = Ex + Extmp If Log_cnt >= 10 And Log_cnt < 20 Then Setpoint_roll = 20 Elseif Log_cnt >= 40 And Log_cnt < 50 Then Setpoint_roll = -20 End If End If Elseif Log_cnt = Max_log_cnt Then F = Ex + Ey F = F + Ez F0 = F Print "Fitness: " ; F Ex = 0 Ey = 0 Ez = 0 Log_cnt = 0 Incr Tries New_parameters = 1 Incr Test_anz End If End If Call Pid_regulator() Call Mixer() Call Failsave() '##### _bl(1) = 113 _bl(4) = 113 '##### Call Set_pwm() Loop Until F <> 99999999 Do Call Filter_rx_data() Call Filter_gyro_data() Call Simulated_annealing() Call Pid_regulator() Call Mixer() Call Failsave() '##### _bl(1) = 113 _bl(4) = 113 '##### Call Set_pwm() Loop Sub Simulated_annealing() If State > 0 Then '-------new parameters needed?-------------------------------- If New_parameters = 1 Then New_parameters = 0 '-------randomize according to the current temperature-------- Rp_i = Rnd(200) - 100 Rp = Rp_i * 0.01 Rp = Rp * T Ri_i = Rnd(200) - 100 Ri = Ri_i * 0.01 Ri = Ri * T Rd_i = Rnd(200) - 100 Rd = Rd_i * 0.01 Rd = Rd * T '-------generate new parameters------------------------------ Wi_kp = Rp * Fp Wi_kp = Wi_kp + W0_kp Wi_ki = Ri * Fi Wi_ki = Wi_ki + W0_ki Wi_kd = Rd * Fd Wi_kd = Wi_kd + W0_kd '-------cut off parameters----------------------------------- If Wi_kp < 0.25 Then Wi_kp = 0.25 If Wi_kp > 0.5 Then Wi_kp = 0.5 If Wi_ki < 0.0005 Then Wi_ki = 0.0005 If Wi_ki > 0.003 Then Wi_ki = 0.003 '-------test current parameters------------------------------ _roll_kp = Wi_kp _roll_ki = Wi_ki _roll_kd = Wi_kd End If '-------log data and get fitness----------------------------- Incr Log_cnt If Log_cnt < Max_log_cnt Then Ax(log_cnt) = _roll_err '_rollnow If Log_cnt > Log_step Then Atmp = Log_cnt - Log_step Extmp = Ax(log_cnt) - Ax(atmp) Extmp = Extmp * Extmp Ex = Ex + Extmp If Log_cnt >= 10 And Log_cnt < 20 Then Setpoint_roll = 20 Elseif Log_cnt >= 40 And Log_cnt < 50 Then Setpoint_roll = -20 End If End If Elseif Log_cnt = Max_log_cnt Then F = Ex + Ey F = F + Ez 'Print "Fitness: " ; F Ex = 0 Ey = 0 Ez = 0 Log_cnt = 0 Incr Tries New_parameters = 1 Incr Test_anz End If '-------better then current solution?------------------------ If F < F0 Then F0 = F W0_kp = Wi_kp W0_ki = Wi_ki W0_kd = Wi_kd Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd End If '-------counter for max tries--------------------------------- If Tries = Max_tries Then Toggle Portc.2 T = T * T_a Tries = 0 End If End If End Sub Sub Serial0charmatch() Clear Serialin If State = 0 Then Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd End If End Sub Sub Set_pwm() If _bl(1) > 250 Then _bl(1) = 250 If _bl(1) < 113 Then _bl(1) = 113 If _bl(2) > 250 Then _bl(2) = 250 If _bl(2) < 113 Then _bl(2) = 113 If _bl(3) > 250 Then _bl(3) = 250 If _bl(3) < 113 Then _bl(3) = 113 If _bl(4) > 250 Then _bl(4) = 250 If _bl(4) < 113 Then _bl(4) = 113 Pwm2a = _bl(4) 'links vorne (US) Pwm2b = _bl(2) 'links hinten (GUS) Pwm1a = _bl(3) 'rechts vorne (GUS) Pwm1b = _bl(1) 'rechts hinten (US) End Sub Sub Failsave() If Channel >= 11 Then If Failure < 255 Then Incr Failure End If End If If Channel < 11 Then If Failure > 0 Then Decr Failure End If End If End Sub Measure: If Channel > 0 And Channel < 9 Then Kanal(channel) = Timer0 End If Timer0 = 6 Incr Channel Return Pausedetect: If Channel <> 0 Then Newvalsreceived = 1 End If Channel = 0 Return Sub Filter_gyro_data() Call Read_wmp_data() _yawnow = Yaw - _yawoffset_int _rollnow = Roll - _rolloffset_int _pitchnow = Pitch - _pitchoffset_int End Sub Sub Filter_rx_data() If Newvalsreceived = 1 Then Newvalsreceived = 0 For I = 1 To Maxchannel Fkanal(i) = Fkanal(i) * 3 Fkanal(i) = Fkanal(i) + Kanal(i) Shift Fkanal(i) , Right , 2 Hempf(i) = Fkanal(i) + 17 Lempf(i) = Fkanal(i) - 17 If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then Kanal(i) = Fkanal(i) End If Next I If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100 _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2 If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60 If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68 End If For I = 2 To Maxchannel If Kanal(i) >= 62 And Kanal(i) < 139 Then _sbl(i) = Kanal(i) - 100 End If Next I If Kanal(5) < 80 Then State = 0 If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1 If Kanal(5) >= 115 Then State = 2 Setpoint_yaw = _sbl(_yawchannel) * 1.5 If State < 2 Then Setpoint_roll = _sbl(_rollchannel) * 0.6 Setpoint_pitch = _sbl(_pitchchannel) * 0.6 Else Setpoint_roll = _sbl(_rollchannel) * 1.5 Setpoint_pitch = _sbl(_pitchchannel) * 1.5 End If If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then Call Set_wmp_offset() End If Oldstate = State End If End Sub Sub Wmp_init() I2cstart I2cwbyte &HA6 I2cwbyte &HFE I2cwbyte &H04 I2cstop End Sub Sub Send_zero() I2cstart I2cwbyte &HA4 I2cwbyte &H00 I2cstop End Sub Sub Read_wmp_data() Gosub Send_zero I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6 Yaw1 = Wmplus_buffer(1) Roll1 = Wmplus_buffer(2) Pitch1 = Wmplus_buffer(3) Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4) Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5) Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6) Shift Yaw , Right , 4 Shift Roll , Right , 4 Shift Pitch , Right , 4 End Sub Sub Set_wmp_offset() _yawoffset = 0 _rolloffset = 0 _pitchoffset = 0 For I = 1 To 100 Call Read_wmp_data() _yawoffset = _yawoffset + Yaw _rolloffset = _rolloffset + Roll _pitchoffset = _pitchoffset + Pitch Next I _yawoffset = _yawoffset / 100 _rolloffset = _rolloffset / 100 _pitchoffset = _pitchoffset / 100 _yawoffset_int = _yawoffset _rolloffset_int = _rolloffset _pitchoffset_int = _pitchoffset End Sub Sub Pid_regulator() '-------------YAW--------------- _yaw_err = Setpoint_yaw - _yawnow _yaw_kp_err = _yaw_err * _yaw_kp _yaw_ki_sum = _yaw_ki_sum + _yaw_err If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000 If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000 _yaw_ki_err = _yaw_ki_sum * _yaw_ki _yaw_kd_err = _yaw_err - _yaw_kd_old _yaw_kd_old = _yaw_err _yaw_kd_err = _yaw_kd_err * _yaw_kd '-------------ROLL-------------- _roll_err = Setpoint_roll - _rollnow _roll_kp_err = _roll_err * _roll_kp _roll_ki_sum = _roll_ki_sum + _roll_err If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000 If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000 _roll_ki_err = _roll_ki_sum * _roll_ki _roll_kd_err = _roll_err - _roll_kd_old _roll_kd_old = _roll_err _roll_kd_err = _roll_kd_err * _roll_kd '-------------PITCH------------- _pitch_err = Setpoint_pitch - _pitchnow _pitch_kp_err = _pitch_err * _pitch_kp _pitch_ki_sum = _pitch_ki_sum + _pitch_err If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000 If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000 _pitch_ki_err = _pitch_ki_sum * _pitch_ki _pitch_kd_err = _pitch_err - _pitch_kd_old _pitch_kd_old = _pitch_err _pitch_kd_err = _pitch_kd_err * _pitch_kd _yaw_pid = _yaw_kp_err + _yaw_ki_err _yaw_pid = _yaw_pid + _yaw_kd_err _yaw_pid_int = _yaw_pid _roll_pid = _roll_kp_err + _roll_ki_err _roll_pid = _roll_pid + _roll_kd_err _roll_pid_int = _roll_pid _pitch_pid = _pitch_kp_err + _pitch_ki_err _pitch_pid = _pitch_pid + _pitch_kd_err _pitch_pid_int = _pitch_pid If _yaw_pid_int < -68 Then _yaw_pid_int = -68 If _yaw_pid_int > 68 Then _yaw_pid_int = 68 If _roll_pid_int < -68 Then _roll_pid_int = -68 If _roll_pid_int > 68 Then _roll_pid_int = 68 If _pitch_pid_int < -68 Then _pitch_pid_int = -68 If _pitch_pid_int > 68 Then _pitch_pid_int = 68 End Sub Sub Mixer() '_bl(1) = rechts hinten (US) '_bl(2) = links hinten (GUS) '_bl(3) = rechts vorne (GUS) '_bl(4) = links vorne (US) _bl(1) = 182 + _sbl(_throttlechannel) _bl(2) = _bl(1) _bl(3) = _bl(1) _bl(4) = _bl(1) If State <> 0 And Failure < 20 Then _bl(1) = _bl(1) + Minthrottle _bl(2) = _bl(2) + Minthrottle _bl(3) = _bl(3) + Minthrottle _bl(4) = _bl(4) + Minthrottle _bl(1) = _bl(1) - _pitch_pid_int _bl(4) = _bl(4) + _pitch_pid_int _bl(2) = _bl(2) - _roll_pid_int _bl(3) = _bl(3) + _roll_pid_int _bl(1) = _bl(1) - _yaw_pid_int _bl(2) = _bl(2) + _yaw_pid_int _bl(3) = _bl(3) + _yaw_pid_int _bl(4) = _bl(4) - _yaw_pid_int Else _bl(1) = 113 _bl(2) = 113 _bl(3) = 113 _bl(4) = 113 _yaw_kp_err = 0 _yaw_ki_sum = 0 _yaw_kd_err = 0 _yaw_ki_err = 0 _roll_kp_err = 0 _roll_ki_sum = 0 _roll_kd_err = 0 _roll_ki_err = 0 _pitch_kp_err = 0 _pitch_ki_sum = 0 _pitch_kd_err = 0 _pitch_ki_err = 0 _yaw_pid_int = 0 _roll_pid_int = 0 _pitch_pid_int = 0 _yaw_pid = 0 _roll_pid = 0 _pitch_pid = 0 Yawstickvel = 0 Rollstickvel = 0 Pitchstickvel = 0 Yawstickold = 0 Rollstickold = 0 Pitchstickold = 0 End If End Sub Sub Init_system() _bl(1) = 113 _bl(2) = 113 _bl(3) = 113 _bl(4) = 113 _sbl(1) = -68 _sbl(2) = 0 _sbl(3) = 0 _sbl(4) = 0 _sbl(5) = -600 _sbl(6) = -600 _sbl(7) = 0 _sbl(8) = 0 Fkanal(1) = 63 Fkanal(2) = 100 Fkanal(3) = 100 Fkanal(4) = 100 Fkanal(5) = 63 Fkanal(6) = 63 Fkanal(7) = 100 Fkanal(8) = 100 Reset Newvalsreceived State = 0 Oldstate = 0 _yawnow = 0 _rollnow = 0 _pitchnow = 0 Call Wmp_init() Waitms 500 For I = 1 To 10 Call Read_wmp_data() Next I Waitms 500 Call Set_wmp_offset() Rc_on = 0 End Sub End

Jetzt muss ich nur noch die Simulated-Annealing Parameter richtig einstellen xD Aber vom Prinzip her funktionierts.
Evtl. werd ich das ganze morgen mal, mit kleiner Temperatur, im richtigen Flug (also ohne Teststand) probieren... Mal sehen.

Gruß
Chris

EDIT:
Um das ganze besser testen zu können, verwende ich nicht die Gyro-werte für die Fitness-Berechnung, sondern den Fehler aus Gyro-wert minus RC-wert. Somit kann auch mit einbezogen werden, wie gut die Parameter dem Piloten folgen. Was haltet ihr davon?

Hallo,

das sieht sehr interessant aus. hab mal nen genalg am standartbeispiel handlungsreisende in java gemacht. das ganze für einen pid regler ist ne feine sache. gibts schon ein video oder ist es angedacht?

Hallo,

ja, das beispiel kenne ich auch, aber ehrlich gesagt finde ich das komplizierter als für die PID-Regelung...
Ein Video wird folgen, sobald die Ergebnisse einigermassen brauchbar sind. Momentan kämpfe ich mit den richtigen Parametern fürs Simulated Annealing. Da ich jetzt aber auch 2 Bücher zum Thema habe, werde ich auch mal versuchen, einen genet. Algo dafür zu schreiben, weil ich glaube, dass sich dieser besser eignet.... mal sehen.

Gruß
Chris

Liste der Anhänge anzeigen (Anzahl: 1)

Hallo,

nachdem ich nun längere Zeit nichts mehr gemacht habe, melde ich mich mal wieder. Gestern Abend habe ich auf die schnelle mal eine Quick&Dirty Umsetzung eines SA in vb.net geschrieben. Das Programm funktioniert m.E.n. sehr gut, man kann gut erkennen, wie sich die Parameter aufs Ergebnis auswirken. Wers mal testen möchte, das Prog ist im Anhang. Einfach Ta und Y eingeben und auf Start drücken (bitte für Fließkommaeingaben nur den . verwenden). Wie gesagt, ist Quick&Dirty, aber es stellt für mich einen großen Ansporn dar, weiter zu machen. Heute werde ich mich mal an einem GA in vb.net versuchen.

Gruß
Chris

Liste der Anhänge anzeigen (Anzahl: 1)

Hallo,

gestern habe ich die ersten erfolgreichen Tests mit meinem GA durchgeführt. Das Programm ist wieder Quick&Dirty, funktioniert aber. Hier zeigen sich auch schon die großen Unterschiede zw. SA und GA. Beim GA passiert es des öfteren, dass er nie eine optimale Lösung findet und in irgendeinem lokalen Minimum hängen bleibt. Außerdem benötigt man eine relativ große Population, um mehr oder minder sicher und schnell das optimale Ergebnis zu erreichen. Als guten Wert hat sich jetzt bei mir der Wert 500 (für m) herauskristallisiert. Allerdings muss ich dazu sagen, dass bis jetzt keine Mutation integriert ist (kommt aber noch) und die Rekombination ist eine einfache Mittelwertbildung von den 2 Werten. Auch das Heiratsschema ist nicht optimal, allerdings habe ich bis jetzt noch keinerlei Quellen gefunden, wie man ein einigermaßen gutes Heiratsschema implementiert... Wenn jemand da einen Tipp hat, immer her damit :D

Das Programm ist wieder im Anhang, setzt jedoch, genauso wie das SA das .net Framework vorraus.

Gruß
Chris

EDIT:
Gerade eben habe ich das Heiratsschema verändert. Jetzt werden zufällig einzelne Individuen miteinander rekombiniert, das brachte eine viel kürzere Laufzeit bei nur m = 50. Das neue Programm ist im Anhang.

Hallo,

der Thread ist zwar schon etwas älter aber hat die Parameterbestimmung so gut funktioniert, dass der Quadrokopter auch in der Luft stabil fliegt? Hast du an den Algorithmen noch weitergearbeitet oder ruht das Projekt?

Gruß Manu

Hi,

ja es hat sehr gut funktioniert! Wenn man den richtigen Parameter zur "Abkühlung" gefunden hat, ist der Algo unglaublich schnell und kommt zu guten Ergebnissen! Irgendwann hab ich den Algo dann aber wieder aus dem Quadro geschmissen, weil ich die Regelalgorithmen verbessert habe und ich ihn somit nicht mehr brauche.

Gruß
Chris

Hast du dann zur Parameterbestimmung eine bestimmte Auslenkung programmiert wie du beschrieben hast? Also eine Auslenkung aus der Gleichgewichtslage, die eine Windböe simuliert und dann vom Quadrokopter ausgesteuert werden muss? Und dann wurde dieser Vorgang so lange wiederholt, bis das zurückkehren in die gleichgewichtslage schnell ging und die Parameter gut waren?
Danke ! Sehr interessantes Thema.

Gruß Manu

Das hab ich mal ausprobiert, war allerdings nicht der richtige Weg! Letzendlich hab ichs so gelöst, dass der gewollte Fehler durch die Funke (also der Soll-Wert) als sozusagen Auslöser zur Bestimmung des Fitnesswerts genutzt wurde. Also nur, solange aktiv die Lage verändert wird, ist der Algo aktiv.

Gruß
Chris