Doppelpost! Positionsbestimmung durch Reifenbewegung

Neue Frage »

BlackDodo Auf diesen Beitrag antworten »
Positionsbestimmung durch Reifenbewegung
Hallo Forum! :-)

Ich sitze hier gerade an einem Problem, was mich noch zur Verzweiflung bringt. Ich erzähle euch einfach mal ein wenig davon und hoffe, dass mir, wie so oft, der Fehler alleine schon klar wird, weil ich es anderen noch mal erkläre und hoffe, dass so viele Rückfragen kommen, dass ich irgendwann selbst auf meinen Denkfehler stoße. Sachdienliche Hinweise, die mich in die richtige Richtung stoßen sind natürlich aber auch willkommen. Big Laugh

Kurzer Prolog:
Stellen wir uns ein Fahrzeug vor, dessen Position auf der Erde wir ermitteln möchten. Jedoch ohne GPS (angenommen, es sei ausgefallen). Eine der vielen (autonomen) Positionsbestimmungsvarianten ist mittels der Bewegung der Reifen.

Problem: Wie kann ich die Position des Fahrzeuges bestimmen, wenn ich nur folgenden Input habe:
(bezogen auf eine Achse. Welche ich betrachte, ist an sich egal, meiner Meinung nach)
- Länge des Weges, den der linke Reifen zurück gelegt hat
- Länge des Weges, den der rechte Reifen zurück gelegt hat
- Spurweite des Wagens
- Umfang des Reifens
- letzte, bekannte Position (von der dann ausgegangen wird)
- letzte, bekannte Richtung, in die das Fahrzeug gefahren ist (beispielsweise durch GPS ermittelt, bis zu dessen Ausfall)

Spurweite ist einfach gegeben. Die Strecke, die die beiden Reifen zurück gelegt haben, sei folgendermaßen gegeben:
Während der Fahrt bekomme ich ausgegeben, wie viel Weg die beiden Reifen jeweils einem Zeitabschnitt t abgefahren sind.

Meine Lösungsidee sieht folgendermaßen aus:
Ich nehme mir die einzelnen Zeitabschnitte (die alle sehr klein sind) und errechne jeweils die Positionsveränderung zur alten Position. Das wird dann wiederum in das globale Koordinatensystem (WGS84) konvertiert.

Im Detail sieht das dann folgendermaßen aus (die Bilder sind im Übrigen mit GeoGebra erstellt, falls jemand anhand der Dateien was zeigen möchte, kann ich die gerne zusätzlich hochladen):
[attach]12788[/attach]
Hier sind die vier Reifen (alt: VL, VR, HL, HR; neu: VL', VR', HL', HR') zu sehen, der Punkt P, der von mir einfach mal auf die Mitte der Vorderachse gelegt wurde und der Verschiebevektor v, der uns von der alten zur neuen Position bringt.

Jetzt schnappe ich mir hiervon nur die Vorderachse (ist ja egal, welche ich nutze) und stelle mir vor, ich fahre genau einen Kreisbogen ab (anders kann ich's irgendwie gar nicht berechnen):
[attach]12789[/attach]
Aus und und der Spurweite kann ich , VL' und VR' berechnen und damit den Vektor (Details gibt's auf Wunsch auch, erst mal erspare ich die euch *g*). Soweit ist das alles kein Problem. Jetzt weiß ich also, ausgehend von der alten Position der Vorderachse, wo sich die Vorderachse nach diesem Abschnitt t befindet, bzw. zunächst mal nur der Punkt P', die Drehung muss ich noch berechnen. Ich habe hierzu testweise einen kleinen Prototypen entwickelt, der mir das alles berechnet. Wenn ich diese einzelnen Vektoren aneinander setze, erhalte ich ziemlich genau die Gesamtdistanz, die der Wagen, von dem ich Messdaten vorliegen habe, tatsächlich gefahren ist. Die Länge der Strecke stimmt also schon mal (scheinbar).

Im zweiten Schritt ist natürlich aber noch wichtig, wie verläuft die Strecke, um damit zu jeder Zeit die Position zu wissen. Ich habe dazu folgende Überlegung angestellt. Zunächst mal ein Bild. An diesem möchte ich dann meine Idee erklären.
[attach]12790[/attach]
Das soll jetzt mal ein Fahrzeug irgendwo auf der Erde sein (meinetwegen auch einfach erst mal in einem normalen, kartesischen Koordinatensystem), dessen Position P ich kenne und den zugehörigen Winkel .
Jetzt verschiebe ich den Punkt P um den Vektor , den ich aus dem ersten Zeitabschnitt t berechnet habe. Dabei muss ich beachten, dass bei der Berechnung, wie sie im zweiten Bild zu sehen ist, die Vorderachse parallel zur x-Achse liegt, was wiederum bei Bild 3 ja nicht der Fall ist. Ich muss also den Vektor noch drehen, damit es wieder passt. Dazu zunächst die Skizze, in der nur der Vektor und dessen Drehung zu sehen ist:
[attach]12791[/attach]
Erklärung: Rechts sehen wir die zur x-Achse parallel Vorderachse, an der ein Vektor abgetragen ist, wie in Bild 2 schon gezeigt. Da die Achse im globalen Koordiantensystem (links) allerdings derzeit um den Winkel verdreht ist, muss der Vektor ebenfalls um diesen Winkel verdreht werden. Ergebnis ist der Vektor v'. Dieser wird dann an die Vorderachse (linker Teil des Bildes) angehängt, um die korrekte Richtung wider zu spiegeln. Die neue Richtung, in die sich das Fahrzeug bewegt, ist dann durch den Winkel gegeben.
Ich habe das Ganze auch noch mal in einem Bild mit zwei Kurven dargestellt. Das sieht dann so aus:
[attach]12792[/attach]
Ich hoffe, dass ich bei dem Bild alle schön übersichtlich gestalten konnte.

Soweit, so gut. Wo ist nun eigentlich das Problem? Ich habe ein Programm geschrieben, welches mir all diese Vektoren aufaddiert, also den direkten Weg darstellt. Die Länge der einzelnen Vektoren entspricht genau dem, was aufgezeichnet wurde. Allerdings stimmt der direkt Vektor nicht. Soll heißen: Das Fahrzeug ist eine Strecke von ca. 10km gefahren. Erst 5km mehr oder weniger gerade aus, dann an einer Kreuzung links abgebogen und wieder 5km gerade aus. In etwa so:
[attach]12798[/attach]
http://img339.imageshack.us/img339/1691/einfachestrecke.png
Hier ist der direkte Vektor etwa 7km. Mein Programm errechnet allerdings nur 500m. Augenzwinkern Ich nehme an, hier stimmt etwas an der Drehung der Vektoren nicht, da ja die Gesamtlänge der Einzelvektoren stimmt. Wahrscheinlich habe ich bei der Implementierung was falsch gemacht und fahre immer nur im Kreis oder so. Big Laugh

Falls überhaupt jemand bis hierhin gelesen hat Mit Zunge , wäre meine Frage jetzt:
Seht ihr einen Fehler in meinen Gedanken und Rechnungen?
Wo könnte da am ehesten ein Fehler liegen?
Wie könnte ich das noch zusätzlich verifizieren?

Durch diese GeoGebra-Zeichnung, die ich eben angefertigt habe, damit ich euch daran die Vektordrehung erklären kann, wie ich sie durchgeführt habe, habe ich noch sicher stellen können, dass diese Methode (von der ich am ehesten dachte, dass sie falsch ist) vom Gedanken her schon mal nicht falsch sein kann. Evtl. eben bei der Implementierung.

Ich freue mich auf eure Kommentare. Und wenn's nur ein "Ich habe alles gelesen und finde nichts!" ist. smile

Viele Grüße,
Dodo
BlackDodo Auf diesen Beitrag antworten »

Da fällt mir gerade noch ein:
Wie könnte ich denn am einfachsten mal diese Vektoren (von denen ich ja die Koordinaten weiß) zeichnen lassen, damit ich mir ein Bild davon mache kann? Ich habe Anwendungen wie Mathematica, Mapla, etc. noch nicht benutzt. Was wäre denn die einfachste Möglichkeit, nur diese simple Zeichnung anzufertigen?
 
 
mirko26 Auf diesen Beitrag antworten »

Hallo, habe bis ans Ende gelesen :-) Ist aber etwas spät zum Denken - nur soviel: funktionieren deine Formeln denn auch, wenn man gerade fährt? Teilst du da bei deinen Radien irgendwo durch Null?
BlackDodo Auf diesen Beitrag antworten »

Zitat:
Original von mirko26
Hallo, habe bis ans Ende gelesen :-) Ist aber etwas spät zum Denken - nur soviel: funktionieren deine Formeln denn auch, wenn man gerade fährt? Teilst du da bei deinen Radien irgendwo durch Null?


Vorweg:
Danke für's Lesen! Mit Zunge <-- geiler Smiley! Augenzwinkern

Hast du soweit es die Uhrzeit zu lies halbwegs was verstehen können? Ich finde, ich bin nicht so der Held im Beschreiben. Big Laugh

Zu deiner Frage:
In der Implementierung gibt's eine if-else-Verzweigung:

1. ==
--> Fahrt gerade aus
Dann wird einfach ein Vektor mit den Koordinaten (0/) und dem Abbiegewinkel 0° erstellt, der dann angehängt wird.
2. !=
--> Kurve
Dann wird die oben beschrieben Methode genutzt.

Aber ich müsste bzgl. möglichen Nulldivisionen noch mal durchgehen. Anderseits habe ich diverse Plausibilitätsprüfungen eingebaut, die mir direkt in's Gesicht springen, wenn ein Wert in einem Bereich liegt, in dem er nicht liegen sollte.

edit: Ich sehe gerade, dass dies dein ERSTER Eintrag war. Du hast dich aber nicht angemeldet, nur um MIR antworten zu können? Big Laugh Den Gedanken nehme ich jetzt mit in's Bett, dann kann ich besser schlafen, weil ich weiß, dass das Problem doch nicht so uninteressant ist. :p
Rmn Auf diesen Beitrag antworten »

Naja wäre sinnvoller, wenn du einfach mal Formeln aufschreibst, womit du Umrechnungen machst.
BlackDodo Auf diesen Beitrag antworten »

Zitat:
Original von Rmn
Naja wäre sinnvoller, wenn du einfach mal Formeln aufschreibst, womit du Umrechnungen machst.


Welche "Umrechnungen" meinst du?

Ich werde in einem weiteren Beitrag mal ein paar Formeln aufschreiben, die ich oben, zwecks Übersichtlichkeit, zunächst zurück hielt.
BlackDodo Auf diesen Beitrag antworten »
Formeln: 1. Winkel alpha
So, also mal der Reihe nach. Die Formatierungen sehen irgendwie komisch aus, aber man kann's lesen. Ich habe das aus meiner dazugehörigen Arbeit kopiert, die ich schreibe und geschaut, dass keine Verweise auftauchen auf Textstellen, die ich hier nicht rein stellen / reinstellen darf. Ich teile die Beiträge mal in die einzelnen Schritte auf.

1. Schritt [attach]12794[/attach]:
Berechnung des Winkels

Der Vektor lässt sich beschreiben als



wobei P und P' jeweils in der Mitte der Achse gewählt wurden. Da der Punkt P so gewählt wurde, dass er im Ursprung liegt, ist



Der Vektor kann in Abhängigkeit der Vektoren und beschrieben werden. Es gilt



was sich mit zwei weiteren Zwischenschritten zu



vereinfachen lässt. Die hierfür notwendigen Vektoren und bzw. die Koordinaten der entsprechenden Punkte VL' und VR' lassen sich mittels der trigonometrischen Funktionen Sinus und Kosinus beschreiben. Zunächst ist hierfür der Winkel notwendig. Durch das Bogenmaß gilt

(Gleichung 5)

und

(Gleichung 6)

Mit



lassen sich die Gleichungen 5 und 6 zu



zusammen fassen. Hierin sind , , sowie bekannt, sodass als einzige Variable verbleibt, nach der aufgelöst wird. Zur besseren Lesbarkeit seien hierbei die Spurbreite und Hilfsstrecke . Dann ist



was mit einer weiteren Umformung zu

(Gleichung 10)

vereinfacht werden kann. Das Ergebnis aus Gleichung 10 kann nun wiederum in Gleichung 5 eingesetzt werden, womit man



erhält.

Da später mit Gradmaß und nicht mit Bogenmaß gerechnet werden soll, muss hier noch in umgerechnet werden:

,

also

.

Der nächste Schritt kommt im nächsten Beitrag, damit Ordnung bleibt. smile
BlackDodo Auf diesen Beitrag antworten »
Formeln: 2. Vektoren VL', VR' und P'
2. Schritt:
[attach]12778[/attach]
Bestimmung der Vektoren und .

Da in Schritt 1 bereits und die Strecke welche identisch mit der Strecke ist, berechnet wurden, will ich hier jetzt mit Sinus und Kosinus rangehen.

Die y-Koordianten sind recht schnell berechnet:

y(VL') =
y(VR') =

Dann gilt weiterhin:

x(VL') =
x(VR') =

wobei





sind.

Zuletzt setze ich noch zusammen:
[attach]12779[/attach]


So viel zu Schritt 2.

P.S.: Mir fällt gerade noch ein, dass ich erst beim Aufschreiben meiner Ergebnisse den Punkt P in den Ursprung gelegt habe. Vorher (und auch bei der Implementierung) lag immer ein Reifen im Ursprung. Aber das sollte an den Formalen ja nichts ändern, außer dass der Vektor dann nicht direkt ist, sondern eben, wie oben schon geschrieben .


Edit: Bilder angehängt. Gruß, Reksilat.
Reksilat Auf diesen Beitrag antworten »

Hi BlackDodo,

Nimm doch Deine Bilder bitte als Anhang in den jeweiligen Beitrag auf.
(Beim Erstellen auf Dateianhänge klicken, Bild auswählen, Speichern und dann den angezeigten [attach]-Code an der passenden Stelle einfügen.)
So stellst Du sicher, dass die Bilder auch in Zukunft noch zur Verfügung stehen und außerdem sieht man sie direkt im Thread.

Gruß,
Reksilat.
BlackDodo Auf diesen Beitrag antworten »

Zitat:
Original von Reksilat
Hi BlackDodo,

Nimm doch Deine Bilder bitte als Anhang in den jeweiligen Beitrag auf.
(Beim Erstellen auf Dateianhänge klicken, Bild auswählen, Speichern und dann den angezeigten [attach]-Code an der passenden Stelle einfügen.)
So stellst Du sicher, dass die Bilder auch in Zukunft noch zur Verfügung stehen und außerdem sieht man sie direkt im Thread.

Gruß,
Reksilat.


Ah, so geht das. smile

Danke für's Ändern im Beitrag vorher. Meine anderen Beiträge kann ich leider nicht mehr ändern, aber für zukünftige Beträge weiß ich jetzt Bescheid.
BlackDodo1 Auf diesen Beitrag antworten »
Formeln: 3. Drehen des Vektors v und Berechnen des neuen Headings
3. Schritt:
Drehen des Vektors bzw. (ist ja der gleiche Vektor).

[attach]12781[/attach]

Im Bild sehen wir rechts das, was ich eben berechnet habe: Unten die Achse, darauf der Vektor bei einer Rechtskurve mit einem Abbiegewinkel von.

Auf der linken Seite des Bildes sehen wir das Fahrzeug im globalen Koordinatensystem. Es fährt derzeit um verdreht zur x-Achse. Um diesen Winkel muss ich also zunächst den Vektor verdrehen, damit er korrekt an den Punkt P angesetzt werden kann.

Dazu nutze ich die Drehmatrix im zweidimensionalen Raum:

[attach]12782[/attach]

Diesen gedrehten Vektor setze ich nun an die alte Vorderachse an und erhalte damit den neuen Punkt P'. Die neue Richtung (Heading) ist dann in diesem Fall gerade die Summe des alten Headings und dem Abbiegewinkel :



Danach beginnt das ganze Spiel von vorne. An meinen Punkt P' setze ich den nächsten Vektor an, den ich vorher um das Heading gedreht habe. Die Summe ergibt wieder das neue Heading.

Soweit der mathematische Teil. Sofern hier kein Fehler liegt, muss dieser in der Implementierung stecken. Dazu werde ich Ausschnitte im nächsten Beitrag besprechen. Dieser Beitrag soll zunächst den Abschluss des theoretischen Teils darstellen.
BlackDodo1 Auf diesen Beitrag antworten »

Ich fliege dauernd aus dem Forum. Nicht wundern, wenn ich zusätzlich noch als BlackDodo1 poste. Ich werde schneller wieder automatisch abgemeldet, als ich mich einloggen und posten kann. Augenzwinkern (wie auch bei diesem kurven Beitrag)
BlackDodo Auf diesen Beitrag antworten »

Ich bin gerade noch mal über meinen Code gegangen. Das ist total chaotisch. Ich baue jetzt erst mal den Prototypen an einigen Stellen neu, damit er zu dem passt, was ich hier bereits verändert beschrieben habe. Durch diese Vereinfachung lösen sich vielleicht auch schon manche Probleme.

Danach werde ich dann die Implementierung hier besprechen, falls sich im theoretischen Teil kein Fehler befunden hat.

Freue mich auf weitere Antworten von euch! smile
BlackDodo Auf diesen Beitrag antworten »

So, ich habe eben schnell was zusammen getippt, damit man da mal durch blickt. Augenzwinkern

Verwendet werden folgende beiden bereits vorhandenen Arrays:

code:
1:
2:
3:
4:
5:
double distanceArray[LINES_TO_READ_FROM_FILE][5]; // time slot + 4 wheels (FL=1,FR=2,RL=3,RR=4)
// The array vectorTableUnnormalized contains the vectors with local coordinate systems.
double vectorTableUnnormalized[LINES_TO_READ_FROM_FILE][6]; // 0/1/2: x/y/angle of front vector - 3/4/5: x/y/angle of rear vector


sowie

code:
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
#define TIME_STAMP_INDEX 0
#define FRONT_LEFT_WHEEL_INDEX 1
#define FRONT_RIGHT_WHEEL_INDEX 2
#define REAR_LEFT_WHEEL_INDEX 3
#define REAR_RIGHT_WHEEL_INDEX 4
#define	FRONT_VECTOR_X_INDEX 0
#define	FRONT_VECTOR_Y_INDEX 1
#define	FRONT_VECTOR_ANGLE_INDEX 2
#define	REAR_VECTOR_X_INDEX 3
#define	REAR_VECTOR_Y_INDEX 4
#define	REAR_VECTOR_ANGLE_INDEX 5


Wie ihr seht sind da auch schon die hinteren Reifen mit drin. Die werden aber im folgenden Code-Abschnitt nicht mit genutzt. Nur der Vollständigkeit halber ist alles erklärt.

Der Code folgt jetzt exakt dem, was ich in den vorangegangen Beiträgen erklärt habe. Zusätzlich werden natürlich noch Abfragen gemacht, um INFINITY und NAN zu vermeiden. Das tritt auf, wenn eine Spur (oder beide) 0 sind oder wenn beide Spuren gleich sind, also eine Gerade entsteht. Dann werden die entsprechenden Werte direkt gesetzt.

Außerdem habe ich die Variablen so benannt, wie wir sie jetzt hier hatten. Das sieht im Code grauenhaft aus, aber ich dachte, es macht das Umdenken von den Formeln hier in den Code einfacher. Augenzwinkern

Außerdem ist im Code reichlich kommentiert, sollte also ohne größere Probleme lesbar sein, hoffe ich. smile
Hier gibt es zunächst nur den ersten Schritt: Berechnung von alpha und dem Vektor v.

Den zweiten Schritt programmiere ich auch eben neu, den gibt's dann in ein paar Stunden. Big Laugh

code:
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
// Berechnet den Vektor v und dessen Winkel alpha, also vectorTableUnnormalized[i][0..2]
// Generell wird immer eine Linkskurve berechnet und wenn es eine Rechtskurve war, der Vektor am Ende gespiegelt (x -> -x)
void CDataProcessor::generateUnnormalizedVectors() {
	// Unterscheidung: Kurve oder Gerade (nur Vorderarchse)
	// Sonderfälle: Min. eine Spur ist 0.
	for (int i=0; i<LINES_TO_READ_FROM_FILE; i++) { // Iteration durch das gesamte Array mit allen einzelnen Teilabschnitten
		if (0 == distanceArray[i][FRONT_LEFT_WHEEL_INDEX] && 0 == distanceArray[i][FRONT_RIGHT_WHEEL_INDEX]) {
			// special case: BEIDE Spuren sind null
			// Vektor und Winkel sind null, mehr ist hier nicht zu tun.
			vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = 0;
			vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] = 0;
			vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] = 0;
		} else if (0 == distanceArray[i][FRONT_LEFT_WHEEL_INDEX] || 0 == distanceArray[i][FRONT_RIGHT_WHEEL_INDEX]) {
			// special case: genau EINE Spur ist 0
			// Berechnung muss etwas anders erfolgen, da sonst Null-Division
			double nonZeroWheel = 0;
			bool leftTurn;
			// Welche Spur ist nicht 0?
			if (0 < distanceArray[i][FRONT_LEFT_WHEEL_INDEX]) {
				nonZeroWheel = distanceArray[i][FRONT_LEFT_WHEEL_INDEX];
				leftTurn = false;
			} else {
				nonZeroWheel = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
				leftTurn = true;
			}
			double alpha = nonZeroWheel / WHEEL_GAUGE; // rad!
			if (leftTurn) {
				vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = -WHEEL_GAUGE/2+WHEEL_GAUGE/2*cos(alpha);
			} else {
				vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = - (-WHEEL_GAUGE/2+WHEEL_GAUGE/2*cos(alpha));
			}
			vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] = WHEEL_GAUGE/2 * sin(alpha);
			vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] = alpha / M_PI * 180; // grad!
		} else {
			// standard case: KEINE Spur 0
			// zwei Fälle: Gerade oder Kurve
			if (distanceArray[i][FRONT_LEFT_WHEEL_INDEX] == distanceArray[i][FRONT_RIGHT_WHEEL_INDEX]) {
				// Spezialfall: Gerade (beide Spuren sind gleich)
				vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
				vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
				vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] = 0;
			} else {
				// Normalfall: beide Spuren > 0 und linkeSpur != rechteSpur
				bool leftTurn;
				double longTrack; // längere Spur
				double shortTrack; // kürzere Spur
				// Zunächst mal Abfrage, welche die längere Spur war
				if (shortTrack = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX] < distanceArray[i][FRONT_LEFT_WHEEL_INDEX]) {
					// wenn links weiter gefahren wurde, haben wir eine RECHTS-Kurve
					longTrack = distanceArray[i][FRONT_LEFT_WHEEL_INDEX];
					shortTrack = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
					leftTurn = false;
				} else {
					// wenn rechts weiter gefahren wurde, haben wir eine LINKS-Kurve
					longTrack = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
					shortTrack = distanceArray[i][FRONT_LEFT_WHEEL_INDEX];
					leftTurn = true;
				}
				// Zunächst der Winkel. Rad wird später in cos und sin benötigt, grad für die Berechnung der Richtung im Koordinatensystem
				double alphaRad = (longTrack - shortTrack) / WHEEL_GAUGE;
				double alphaGrad = alphaRad / M_PI * 180;
				// alpha wird schon mal abgelegt
				vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] = alphaGrad;
				// AVL und AVR sind die Strecken aus der Zeichnung
				double AVL = WHEEL_GAUGE * shortTrack / ( longTrack - shortTrack );
				double AVR = AVL + WHEEL_GAUGE;
				// newVL = VL' und newVR = VR'
				double newVL[2];
				newVL[0] = WHEEL_GAUGE/2 + AVL - AVL*cos(alphaRad);
				newVL[1] = AVL * sin(alphaGrad);
				double newVR[2];
				newVR[0] = WHEEL_GAUGE/2 + AVL - AVR*cos(alphaRad);
				newVR[1] = AVR * sin(alphaGrad);
				// Unterscheidung: Wenn Rechtskurve, dann x-Koordiante spiegeln.
				if (leftTurn) {
					vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = 0.5 * ( newVL[0] + newVR[0] );
				} else {
					vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = - (0.5 * ( newVL[0] + newVR[0] ));
				}
				// y bleibt unverändert
				vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] = 0.5 * ( newVL[1] + newVR[1] );
			}
		}
		
		// DEBUG:
		if (isnan(vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]) || isinf(vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]) ||
			isnan(vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]) || isinf(vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]) ||
			isnan(vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX]) || isinf(vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX])) {
			cout << "PLAUSIBILITÄTSPRÜFUNG FEHLGESCHLAGEN!" << endl;
			cout << "i = " << i << " -> vector: (" << vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] 
<< "/" << vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]
			<< "), angle: " << vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] << endl;
		}
		
	}
	
	// DEBUG:
	if (DEBUG) {
		for (int i=DEBUG_START; i<DEBUG_END+1; i++) {
			cout << "i = " << i << " -> vector: (" << vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] 
<< "/" << vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]
				<< "), angle: " << vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] << endl;
		}
	}
	
}


Vielen Dank, dass ihr bis hier hin gelesen habt! Big Laugh Mit Zunge
BlackDodo Auf diesen Beitrag antworten »

Args, ich drehe noch durch. Da war noch ein Fehler drin in dem Code, den ich heute geschrieben und da oben gepostet habe. Den mal bitte nicht lesen, bis ich ordentlich(!) getestet habe. Big Laugh

Wichtiger wären ohnehin erst mal die Gleichungen und Vektoren und so. Die Implementierung ist ja eine andere Sache.

DANKE!
BlackDodo Auf diesen Beitrag antworten »
Code: 1. Teil
So, jetzt aber! Big Laugh

Dieser erste Teil, der jetzt auch fehlerfrei ist Big Laugh spuckt am Ende den Vektor und den Abbiegewinkel aus. Von diesen beiden Werten ausgehend bastel ich jetzt den zweiten Teil auch noch neu, wobei ich schon eine Idee habe, wo etwas falsch sein könnte.

Alles ab Zeile 81 ist übrigens nur noch zu Debugging-Zwecken. Der eigentliche Teil kommt vorher.

code:
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
42:
43:
44:
45:
46:
47:
48:
49:
50:
51:
52:
53:
54:
55:
56:
57:
58:
59:
60:
61:
62:
63:
64:
65:
66:
67:
68:
69:
70:
71:
72:
73:
74:
75:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:
125:
126:
127:
128:
129:
130:
131:
132:
133:
134:
135:
136:
// Berechnet den Vektor v und dessen Winkel alpha, also vectorTableUnnormalized[i][0..2]
// Generell wird immer eine Linkskurve berechnet und wenn es eine Rechtskurve war, der Vektor am Ende gespiegelt (x -> -x)
void CDataProcessor::generateUnnormalizedVectors() {
	// Unterscheidung: Kurve oder Gerade (nur Vorderarchse)
	// Sonderfälle: Min. eine Spur ist 0.
	for (int i=0; i<LINES_TO_READ_FROM_FILE; i++) { // Iteration durch das gesamte Array mit allen einzelnen Teilabschnitten
		if (0 == distanceArray[i][FRONT_LEFT_WHEEL_INDEX] && 0 == distanceArray[i][FRONT_RIGHT_WHEEL_INDEX]) {
			// special case: BEIDE Spuren sind null
			// Vektor und Winkel sind null, mehr ist hier nicht zu tun.
			vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = 0;
			vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] = 0;
			vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] = 0;
		} else if (0 == distanceArray[i][FRONT_LEFT_WHEEL_INDEX] || 0 == distanceArray[i][FRONT_RIGHT_WHEEL_INDEX]) {
			// special case: genau EINE Spur ist 0
			// Berechnung muss etwas anders erfolgen, da sonst Null-Division
			double nonZeroWheel;
			bool leftTurn;
			// Welche Spur ist nicht 0?
			if (0 < distanceArray[i][FRONT_LEFT_WHEEL_INDEX]) {
				nonZeroWheel = distanceArray[i][FRONT_LEFT_WHEEL_INDEX];
				leftTurn = false;
			} else {
				nonZeroWheel = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
				leftTurn = true;
			}
			double alpha = nonZeroWheel / WHEEL_GAUGE; // rad!
			if (leftTurn) {
				vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = -WHEEL_GAUGE/2 + WHEEL_GAUGE/2 * cos(alpha);
			} else {
				vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = - ( -WHEEL_GAUGE/2 + WHEEL_GAUGE/2 * cos(alpha) );
			}
			vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] = WHEEL_GAUGE/2 * sin(alpha);
			vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] = alpha / M_PI * 180; // grad!
		} else {
			// standard case: KEINE Spur 0
			// zwei Fälle: Gerade oder Kurve
			if (distanceArray[i][FRONT_LEFT_WHEEL_INDEX] == distanceArray[i][FRONT_RIGHT_WHEEL_INDEX]) {
				// Spezialfall: Gerade (beide Spuren sind gleich)
				vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = 0;
				vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
				vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] = 0;
			} else {
				// Normalfall: beide Spuren > 0 und linkeSpur != rechteSpur
				bool leftTurn;
				double longTrack; // längere Spur
				double shortTrack; // kürzere Spur
				// Zunächst mal Abfrage, welche die längere Spur war
				if (distanceArray[i][FRONT_RIGHT_WHEEL_INDEX] < distanceArray[i][FRONT_LEFT_WHEEL_INDEX]) {
					// wenn links mehr gefahren wurde, haben wir eine RECHTS-Kurve
					longTrack = distanceArray[i][FRONT_LEFT_WHEEL_INDEX];
					shortTrack = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
					leftTurn = false;
				} else {
					// wenn rechts mehr gefahren wurde, haben wir eine LINKS-Kurve
					longTrack = distanceArray[i][FRONT_RIGHT_WHEEL_INDEX];
					shortTrack = distanceArray[i][FRONT_LEFT_WHEEL_INDEX];
					leftTurn = true;
				}
				// AVL und AVR sind die Strecken aus der Zeichnung
				double AVL = WHEEL_GAUGE * shortTrack / ( longTrack - shortTrack ); // Zähler long oder short?! 0o
				double AVR = AVL + WHEEL_GAUGE;
				// Zunächst der Winkel. Rad wird später in cos und sin benötigt, grad für die Berechnung der Richtung im Koordinatensystem
				double alphaRad = shortTrack / AVL; // (longTrack - shortTrack) / WHEEL_GAUGE;
				double alphaGrad = alphaRad / M_PI * 180;
				// alpha wird schon mal abgelegt
				vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX] = alphaGrad;
				// newVL = VL' und newVR = VR'
				double newVL[2];
				double newVR[2];
				newVL[0] = - WHEEL_GAUGE/2 - AVL + AVL*cos(alphaRad); // Erst zu Punkt A, dann wieder zurück zum Endpunkt
				newVR[0] = - WHEEL_GAUGE/2 - AVL + AVR*cos(alphaRad); // Erst zu Punkt A, dann wieder zurück zum Endpunkt
				newVL[1] = AVL * sin(alphaRad);
				newVR[1] = AVR * sin(alphaRad);
				// Unterscheidung: Wenn Rechtskurve, dann x-Koordinate spiegeln.
				if (leftTurn) {
					vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = 0.5 * ( newVL[0] + newVR[0] );
				} else {
					vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] = - (0.5 * ( newVL[0] + newVR[0] ));
				}
				// y ist in beiden Fällen mit einer Gleichung zu berechnen.
				vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] = 0.5 * ( newVL[1] + newVR[1] );
				
				// DEBUG:
				if (false) {
					if (DEBUG_START-1 < i && i < DEBUG_END+1) {
						cout << "longTrack = " << longTrack << ", shortTrack = " << shortTrack << endl;
						cout << "i = " << i << ", AVL = " << AVL << ", AVR = " << AVR << endl;
						cout << "alphaRad = " << alphaRad << ", alphaGrad = " << alphaGrad << endl;
						cout << "newVL = (" << newVL[0] << "/" << newVL[1] << "), newVR = (" << newVR[0] << "/" << newVR[1] << ")" << endl;
					}
				}
				
			}
		}
		
		// DEBUG:
		double laengeDesVektors = sqrt(vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]*vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]
									   +vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]*vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]);
		if (isnan(vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]) || isinf(vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]) ||
			isnan(vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]) || isinf(vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]) ||
			isnan(vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX]) || isinf(vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX]) ||
			(laengeDesVektors/100000)/(pulseArray[i][0]/3600) > 200) { // Tempo über 200
			cout << "PLAUSIBILITÄTSPRÜFUNG FEHLGESCHLAGEN!" << endl;
			cout << "i = " << i << " -> vector: (" << vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] << "/" << vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]
			<< "), angle: " << vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX]
			<< ", length: " << laengeDesVektors << ", speed: " << (laengeDesVektors/100000)/(pulseArray[i][0]/3600) << endl;

		}
		
	}
	
	// DEBUG:
	if (DEBUG) {
		// Erst mal nur allg. Ausgabe
		cout << "Ungedrehte Vektoren:" << endl;
		for (int i=DEBUG_START; i<DEBUG_END+1; i++) {
			double laengeDesVektors = sqrt(vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]*vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]
										   +vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]*vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]);
			cout << "(t=" << pulseArray[i][0] << ") i = " << i << " -> vector: (" << vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] << "/" << vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX]
				<< "), angle: " << vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX]
			<< ", length: " << laengeDesVektors << ", speed: " << (laengeDesVektors/100000)/(pulseArray[i][0]/3600) << endl;
		}
		// Dann noch Berechnung der Gesamtlänge aller Vektoren und des "direkten" Vektors:
		double gesamtlaenge = 0;
		
		for (int i=0; i<LINES_TO_READ_FROM_FILE; i++) {
			gesamtlaenge += sqrt( vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] * vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]
								 + vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] * vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] );
		}
		cout << endl;
		cout << "Gesamtlänge: " << (int)(gesamtlaenge) << endl;
	}
	
}
BlackDodo Auf diesen Beitrag antworten »
Code: 2. Teil
Und da wäre auch schon der Rest. An sich nur noch ein paar Zeilen.

Hierin verwendet wird:

code:
1:
2:
3:
double globalPosition[2];


Hier der Code:

code:
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
30:
31:
32:
33:
34:
35:
36:
37:
38:
39:
40:
41:
void CDataProcessor::generateNormalizedVectors() {
	
	for (int i=0; i<LINES_TO_READ_FROM_FILE; i++) {
		
		// 1. Vektor um aktuelles Heading drehen	
		vectorTableNormalized[i][FRONT_VECTOR_X_INDEX] = vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] * cos(currentHeading)
			- vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] * sin(currentHeading);
		vectorTableNormalized[i][FRONT_VECTOR_Y_INDEX] = vectorTableUnnormalized[i][FRONT_VECTOR_Y_INDEX] * cos(currentHeading)
			+ vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX] * sin(currentHeading);
		vectorTableNormalized[i][FRONT_VECTOR_ANGLE_INDEX] = vectorTableUnnormalized[i][FRONT_VECTOR_ANGLE_INDEX];
		
		// 2. Gedrehten Vektor an letzte Position ansetzen
		globalPosition[0] += vectorTableNormalized[i][FRONT_VECTOR_X_INDEX];
		globalPosition[1] += vectorTableNormalized[i][FRONT_VECTOR_Y_INDEX];
		
		// 3. Neues Heading berechnen
		// Hier muss zwischen Links- und Rechtskurve unterschieden werden.
		if (0 < vectorTableUnnormalized[i][FRONT_VECTOR_X_INDEX]) {
			// x-Koordiante größer 0, also Rechtskurve
			currentHeading -= vectorTableNormalized[i][FRONT_VECTOR_ANGLE_INDEX];
		} else {
			// sonst Linkskurve
			currentHeading += vectorTableNormalized[i][FRONT_VECTOR_ANGLE_INDEX];
		}
		
	}
	
	// DEBUG:
	double laengeDerGedrehtenVektoren = 0; // sollte ja gleich sein
	for (int i=0; i<LINES_TO_READ_FROM_FILE; i++) {
		laengeDerGedrehtenVektoren += sqrt(vectorTableNormalized[i][FRONT_VECTOR_X_INDEX]*vectorTableNormalized[i][FRONT_VECTOR_X_INDEX]+
										   vectorTableNormalized[i][FRONT_VECTOR_Y_INDEX]*vectorTableNormalized[i][FRONT_VECTOR_Y_INDEX]);
	}
	cout << "Gesamtlänge der gedrehten Vektoren: " << (int)laengeDerGedrehtenVektoren << endl;
	cout << "Direkter Vektor: (" << (int)(globalPosition[0]) << "/" << (int)(globalPosition[1]) << ")" << endl;
	double laengeDirekterVektor = sqrt( globalPosition[0] * globalPosition[0] + globalPosition[1] * globalPosition[1] );
	cout << "Länge des direkten Vektors: " << (int)(laengeDirekterVektor) << endl;
	
}


Eine Beispielausgabe, auf einer Datei mit echten Daten beruhend, wäre übrigens:

code:
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
Ungedrehte Vektoren:
(t=0.01998) i = 10000 -> vector: (0/45.85), angle: 0, length: 45.85, speed: 82.6126
(t=0.02002) i = 10001 -> vector: (0/45.85), angle: 0, length: 45.85, speed: 82.4476
(t=0.02) i = 10002 -> vector: (0.44179/43.9366), angle: 0.0201096, length: 43.9388, speed: 79.0899
(t=0.01993) i = 10003 -> vector: (0/45.85), angle: 0, length: 45.85, speed: 82.8199
(t=0.02008) i = 10004 -> vector: (-0.44179/43.9366), angle: 0.0201096, length: 43.9388, speed: 78.7748
(t=0.02) i = 10005 -> vector: (0/45.85), angle: 0, length: 45.85, speed: 82.53

Gesamtlänge der ungedrehten Vektoren: 1229943
Gesamtlänge der gedrehten Vektoren: 1229943
Direkter Vektor: (-689238/-101015)
Länge des direkten Vektors: 696601


Und jetzt kommt's. Der direkt Vektor ist zwar von den Koordinaten her (denke ich) falsch, aber seine Länge stimmt fast exakt mit meinen Messdaten überein! Big Laugh

Ich habe ja aber all die einzelnen, gedrehten Vektoren. Was wäre denn die einfachste Variante, all diese Vektoren (ca. 60000 Stück, da 1200s Messung, alle 0.02s ein Messpunkt) irgendwie anzeigen zu lassen? Kann Mathematica, GeoGebra, Maple, etc. sowas? Ausgeben ist ja kein Problem. Aber vom Zeichnen in C++ habe ich keine Ahnung. Augenzwinkern
Reksilat Auf diesen Beitrag antworten »
RE: Code: 1. Teil
Ich habe bis jetzt keinen Fehler gefunden. Ist aber bei der Menge von Material kein Wunder. Da wirst Du Dir wohl ein paar übersichtlichere Testdaten basteln müssen.
Was war denn mit mit Deinem Test 5km geradeaus -> Linkskurve -> 5km geradeaus? Funktioniert das noch immer nicht?

In Bezug auf die graphische Darstellung kann ich auch nicht wirklich helfen. MATLAB könnte das eventuell gut hinbekommen.

Gruß,
Reksilat.
BlackDodo Auf diesen Beitrag antworten »
RE: Code: 1. Teil
Zitat:
Original von Reksilat
Ich habe bis jetzt keinen Fehler gefunden. Ist aber bei der Menge von Material kein Wunder. Da wirst Du Dir wohl ein paar übersichtlichere Testdaten basteln müssen.


Was genau stellst du dir vor? Dann bastel ich es gerne. smile

Zitat:
Original von Reksilat
Was war denn mit mit Deinem Test 5km geradeaus -> Linkskurve -> 5km geradeaus? Funktioniert das noch immer nicht?


Das ist die Testausgabe von oben. Waren etwas mehr, als 5km. Die Länge des direkten Vektors stimmt mit 6km (per Google Maps abgemessen) nahezu exakt. Jetzt muss ich nur noch irgendwo die ganzen Vektoren plotten und mir anschauen, ob die Kurve auch so aussieht, wie sie soll. Es würde ja sogar langen, einfach die einzelnen, globalen Positionen zu zeichnen. Es sind ja 60000. Die Kurve sehe ich dann schon. smile

Zitat:
Original von Reksilat
In Bezug auf die graphische Darstellung kann ich auch nicht wirklich helfen. MATLAB könnte das eventuell gut hinbekommen.


Ich sehe auf der Download-Seite nur die Möglichkeit, einen MathWorks-Mitarbeiter zu kontaktieren, um mit dem eine Trial-Version zu bereden?! Lustige Firma. Big Laugh
Reksilat Auf diesen Beitrag antworten »
RE: Code: 1. Teil
Testdaten wären zum Beispiel:
L: 10 R: 10 (10 Meter geradeaus)
L: R: (90° Linkskurve) (Spurbreite 2m)
L: 5 R: 5 (5 Meter geradeaus)
L: R: (90° Rechtskurve)
L: 10 R: 10 (10 Meter geradeaus)

Wäre dann zusammen ein Vektor (23;8). Wenn es da Probleme gibt, findet man die auch leichter. Wenn es gut läuft, sollte das Programm auch in Ordnung sein. Augenzwinkern

bzgl. MATLAB: Für den Privatanwender nicht unbedingt erschwinglich. Dachte Du könntest vielleicht über eine Uni darauf zugreifen.

EDIT: Für die Physiker sollte es aber genügend Lösungen in der Richtung geben. Schau mal nach Gnuplot o.ä.
BlackDodo Auf diesen Beitrag antworten »
RE: Code: 1. Teil
Zitat:
Original von Reksilat
Testdaten wären zum Beispiel:
L: 10 R: 10 (10 Meter geradeaus)
L: R: (90° Linkskurve) (Spurbreite 2m)
L: 5 R: 5 (5 Meter geradeaus)
L: R: (90° Rechtskurve)
L: 10 R: 10 (10 Meter geradeaus)

Wäre dann zusammen ein Vektor (23;8). Wenn es da Probleme gibt, findet man die auch leichter. Wenn es gut läuft, sollte das Programm auch in Ordnung sein. Augenzwinkern


Hm, ich habe die Daten mal eingetippt. Vorher habe ich mir aber schon überlegt, dass ich nicht ganz weiß, wie du auf (23/8) kommst. smile Wenn ich erst gerade aus fahre, also auf der y-Achse entlang, dann links abbiege in den dritten Quadranten rein, und dann wieder rechts abbiege, um erneut parallel zur y-Achse zu fahren, wie kann dann die x-Koordinate positiv sein?

Gehen wir beide davon aus, dass 0° bedeutet, ich fahre nach Norden, also auf der y-Achse in's positiv unendliche? So habe ich's fest gelegt. Meine Ausgabe sieht jetzt auch ganz brauchbar aus.

Ergebnis ist:

code:
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
i = 0 -> vector: (0/10), angle: 0, length: 10
i = 1 -> vector: (-0.0246735/3.14146), angle: 0.015708, length: 3.14156
i = 2 -> vector: (0/5), angle: 0, length: 5
i = 3 -> vector: (0.0246735/3.14146), angle: 0.015708, length: 3.14156
i = 4 -> vector: (0/10), angle: 0, length: 10

globalPosition(i=1): (0/10)
globalPosition(i=2): (-0.0246735/13.1415)
globalPosition(i=3): (-0.10321/18.1408)
globalPosition(i=4): (-0.127884/21.2823)
globalPosition(i=5): (-0.127884/31.2823)


Nicht wundern, dass die Indizes verschoben sind. Im ersten Teil soll das heißen "Ich stehe bei i=0 und fahre von da den Vektor (0/10) ab, damit lande ich bei i=1 an der globalen Position (0/10)". smile

Zitat:
Original von Reksilat
bzgl. MATLAB: Für den Privatanwender nicht unbedingt erschwinglich. Dachte Du könntest vielleicht über eine Uni darauf zugreifen.


War mir jetzt nicht bekannt. Über's MSDNAA kommen wir an so ziemlich alles, was M$ verzapft, aber was Matlab angeht, müsste ich mich mal informieren. Big Laugh

Zitat:
Original von Reksilat
EDIT: Für die Physiker sollte es aber genügend Lösungen in der Richtung geben. Schau mal nach Gnuplot o.ä.


Ah, daher wohl dein Gedanke, dass ich Matlab an der Uni bekomme. Ich bin (leider) Informatiker, kein Physiker oder gar (hätte ich's doch nur gewählt) Mathematiker! Augenzwinkern

Zu meinen vorliegenden Daten:
[attach]12801[/attach]
Das ist das, was ich mit Mathematica erzeugen konnte. Zur Erklärung:
Start ist bei (0/0), also ganz rechts. Der erste Teil (orangener Kasten) verläuft auf der Straße auch in etwa so, allerdings nicht mit so heftigen Bögen. Der zweite Teil (grüner Kasten) ist sehr gut getroffen, da es sich hier um einen rießigen Kreisel handelt. Die lange Rechtskurve (roter Pfeil) stimmt auch nicht ganz, die ist gerader. Und der Teil nach der langen Kurve (sobald es scharf rechts geht in dieses seltsame Geschlängel), das stimmt überhaupt nicht. Mit etwas "Kantenglättung" sieht's vielleicht schon besser aus. Big Laugh

Erklärung für die Probleme:
1. Zu starke Kurven könnte dadurch hervorgerufen werden, dass die Messintervalle zu KLEIN sind und es Intervalle gibt, in denen sich (laut Messung) nur EIN Reifen bewegt (ein Messintervall hat 20ms). Daher verändert sich der Winkel recht stark, obwohl sich das Fahrzeug nur ca. 2cm bewegt. Das könnte auf Dauer vielleicht ein Grund sein.
2. Das Gekrackel am Ende, was überhaupt nicht mehr rein passt, könnte auch damit zusammen hängen, dass ich die Messdaten zu spät abgeschnitten habe. Da muss ich noch mal kontrollieren.

Alles in allem kann man aber die Originalstrecke wieder erkennen, was mir schon mal Hoffnung gibt. Jetzt muss ich nur noch am Feinschliff arbeiten, damit die beiden mehr oder weniger gerade verlaufenden Stück auch so aussehen. smile

Ein Abgleich mit der zugehörigen Straßenkarte (im Rahmen meiner Arbeit nicht mehr geplant) sollte dann die restlichen Unstimmigkeiten beseitigen können.

Reksilat: Kannst du mit meinen Testdaten was anfangen? Habe ich bei deinen Ausführungen was falsch verstanden?

Und an alle: Hat jemand eine Idee, woran diese zu stark gekrümmte Gerade noch liegen könnte? smile
Reksilat Auf diesen Beitrag antworten »
RE: Code: 1. Teil
Dass Du in Richtung der y-Achse losfährst, ist mir danach auch aufgefallen. Dementsprechend müsste das Ergebnis dann (-8;23) sein.
Zitat:

Ergebnis ist:

code:
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
i = 0 -> vector: (0/10), angle: 0, length: 10
i = 1 -> vector: (-0.0246735/3.14146), angle: 0.015708, length: 3.14156
i = 2 -> vector: (0/5), angle: 0, length: 5
i = 3 -> vector: (0.0246735/3.14146), angle: 0.015708, length: 3.14156
i = 4 -> vector: (0/10), angle: 0, length: 10

globalPosition(i=1): (0/10)
globalPosition(i=2): (-0.0246735/13.1415)
globalPosition(i=3): (-0.10321/18.1408)
globalPosition(i=4): (-0.127884/21.2823)
globalPosition(i=5): (-0.127884/31.2823)

Stell mal bitte WHEEL_GAUGE auf 2 und nicht auf 200. Meine Angaben waren alle in Metern. Augenzwinkern

PS: Ich dachte mir schon, dass das hier eher in Richtung Informatik geht. Mir fielen halt nur die ganzen Physiker in meinem Bekanntenkreis ein, die ja auch andauernd ihre Messdaten plotten müssen, und die nehmen dann so was wie Gnuplot (freeware).
BlackDodo Auf diesen Beitrag antworten »

Oups, das habe ich natürlich vergessen. Big Laugh

code:
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
Ungedrehte Vektoren:
i = 0 -> vector: (0/10), angle: 0, length: 10
i = 1 -> vector: (-2/2), angle: 1.5708, length: 2.82843
i = 2 -> vector: (0/5), angle: 0, length: 5
i = 3 -> vector: (2/2), angle: 1.5708, length: 2.82843
i = 4 -> vector: (0/10), angle: 0, length: 10

globalPosition(i=0): (0/0)
globalPosition(i=1): (0/10)
globalPosition(i=2): (-2/12)
globalPosition(i=3): (-7/12)
globalPosition(i=4): (-9/14)
globalPosition(i=5): (-9/24)


Da ist jetzt auch die Spurweite korrekt.

Deinen Vektor (-8/23) kann ich nochvollziehen, die (-9/24) sollten also falsch sein. Ich habe die Ausgabe eben erst erstellt. Über den Fehler mache ich mir jetzt mal Gedanken.

GnuPlot habe ich übrigens auch entdeckt, aber irgendwie keine Version für Mac OS bekommen. Vielleicht habe ich aber auch nur nicht ordentlich gesucht. Auf der Webseite gibt's zwar zwei Links ("there MAY be Mac OS versions available" steht dran Big Laugh ), aber die funktionieren nicht. Macht ja auch nichts. Mathematica erweist wunderbare Arbeit. smile
Reksilat Auf diesen Beitrag antworten »

Zitat:
Deinen Vektor (-8/23) kann ich nochvollziehen, die (-9/24) sollten also falsch sein.

Bin jetzt gerade soweit, dass ich denke, dass (-9,24) korrekt ist. Nehmen wir mal die Linkskurve (mit etwas abgewandelter Startposition):
Vorher: VL=(1,0) VR=(3,0) P=(2,0)
Nachher: VL=(0,1) VR=(0,3) P=(0,2)

Wir fahren schließlich einen Viertelkreis und der Radius des inneren Reifens ist dabei und der des äußeren .

Demnach stimmt die Rechung des Programms. Ich hatte zu Beginn des Beispiels eine Spurbreite von 1m genommen, diese dann aber irgendwann auf 2m geändert ohne das Gesamtergebnis nachzubessern. Sorry!

Gruß,
Reksilat.
BlackDodo Auf diesen Beitrag antworten »

Nicht entschuldigen! Du hast 1. mein Programm schon mal verifiziert (zumindest grob, Details muss ich natürlich noch testen) und 2. hast du mir schöne Denkanstöße gegeben.

Ich habe deine Daten noch mal mit GeoGebra aufgezeichnet, damit wir uns beide davon überzeugen können, dass es wohl mit (-9/24) am besten passt. Augenzwinkern

Vektoren, Strecken und Kreisbogenlängen stehen dran. Sieht gut aus. smile Prost

[attach]12807[/attach]

Ich werde jetzt einerseits mal ein komplexeres Beispiel basteln und zweitens habe ich eine Idee, wo ein weiterer Fehler stecken könnte:
Die Daten liegen mir ja nicht fertig als Streckenlängen für jedes einzelne Rad in jedem einzelnen Abschnitt vor. Die Messung funktioniert folgendermaßen:
Die Reifen senden Pulse während ihrer Umdrehung aus. Und nach einer bestimmten Anzahl an Pulsen, die aufgezeichnet werden, hat sich das Rad ein mal gedreht. So, als würde man bei einem Fahrradreifen die Speichen zählen und wüsste, dass er einmal rum ist, wenn man einmal die Anzahl seiner Speichen gezählt hat. Hoffe, ich konnte das verständlich erklären. Big Laugh

Das Problem ist jetzt: Die Reifen laufen nicht unbedingt parallel. Das heißt, wenn ich eine Intervall erwische, in dem ich vom linken Reifen gerade noch so einen Puls messe, der vom rechten aber erst 0.00002ms später kommt und damit im nächsten Intervall liegt, gibt es sehr häufig (um nicht zu sagen dauernd) folgenden Fehler:

L: 10 R: 11
L: 11 R: 10
L: 10 R: 11
L: 11 R: 10
L: 10 R: 11
L: 11 R: 10
L: 10 R: 11
L: 11 R: 10

Sieht jetzt also so aus, als würde das Fahrzeug Schlangenlinien fahren. Es ist aber eigentlich eine Gerade, bei der der oben beschrieben Fehler auf tritt. Ich werde jetzt mal eine zusätzliche Abfrage einbauen, die folgendes macht:
Falls bei zwei aufeinander folgenden Messungen gilt

( L0 - R0 ) = - ( L1 - R1 )
werde ich das ein wenig glätten und als Gerade betrachten. Das führt wiederum bei echten Schlangenlinien zu Fehlern, aber mal sehen, wie die Kurve dann aussieht. smile Und verschiedene Messreichen mit unterschiedlichen Zeitintervallen werde ich auch noch anfertigen, um zu sehen, ob 20ms vielleicht ein zu kurzes Intervall ist.

*macht sich an die Arbeit*
Reksilat Auf diesen Beitrag antworten »

Die Verringerung der Messfrequenz wäre auch meine nächste Idee gewesen. Wie viele Pulse sendest Du denn pro Umdrehung?
Kurze Rechnung:
Reifendurchmesser ~60cm
Umfang ~2m
Geschwindigkeit ~20m/s (72km/h)

Macht 10 Umdrehungen pro Sekunde.

Ich denke, dass man pro Messeinheit fürs erste vielleicht durchschnittlich fünf Pulse sammeln sollte. Der Fall, dass ein Reifen sich gar nicht bewegt, sollte besser nicht auftauchen.

Gruß,
Reksilat.
BlackDodo Auf diesen Beitrag antworten »

Zitat:
Original von Reksilat
Die Verringerung der Messfrequenz wäre auch meine nächste Idee gewesen. Wie viele Pulse sendest Du denn pro Umdrehung?
Kurze Rechnung:
Reifendurchmesser ~60cm
Umfang ~2m
Geschwindigkeit ~20m/s (72km/h)

Macht 10 Umdrehungen pro Sekunde.

Ich denke, dass man pro Messeinheit fürs erste vielleicht durchschnittlich fünf Pulse sammeln sollte. Der Fall, dass ein Reifen sich gar nicht bewegt, sollte besser nicht auftauchen.

Gruß,
Reksilat.


Hm, das muss ich dann erst umprogrammieren. Ich habe die Intervalle nicht an an der Anzahl der Pulse festgemacht, sondern an der Zeit, die verstrichen ist. Alle 20ms gibt es eine Aufzeichnung des aktuellen Standes des Puls-Zählers. Freude

[attach]12810[/attach]

Hier mal als Beispiel die Ergebnisse aus Mathematica, für 100ms-Schritte. Die obere Reihe ist: 100ms, 200ms, 300ms, 400ms. Die mittlere Reihe dann 500ms, 600ms, 700ms und 800ms. Unten noch 900ms und 1s. Kleinere Intervalle ergeben nur Mist, wie etwa:

[attach]12811[/attach]

Das ist das minimale Intervall. Hier habe ich so feinkörnig ausgewertet, wie es möglich war (also alle 20ms).

Die "Kantenglättung", die ich oben beschrieben habe, brachte nicht sehr viel. Aus dem ersten Bild liegt das zweite Ergebnis (200ms) und das fünfte Ergebnis (500ms) am nächsten an der Realität (500ms so sogar noch eher, würde ich sagen). mal abgesehen von den langen Kurven (die viel gerader sein müssten), stimmt da fast alles. Wenn ich meine Kantenglättung anwerfe, passiert aber folgendes:

[attach]12812[/attach]

Das Ergebnis wird also nur verzerrter.

Zu deiner Frage noch:
Ich habe folgende Annahmen gemacht, da ich die exakten Werte nicht habe:
Spurweite: ~190cm (interessanterweise ist der Unterschied zwischen 100cm und 300cm Spurweite auf 12km gerade mal 3cm, also ist es nicht mal wichtig, exakt die Spurweite zu kennen)
Reifenumfang: ~190cm, was so der Durchschnittsreifen sein könnte und auch auf Grund der Gesamtlänge der Strecke gut hin kommt.

Variablen, die ich sicher kenne:
Pulse pro Umdrehung: 48

Um mal noch ein Beispiel zu geben, was mir vorliegt:
Ich habe eine Datei, in der stehen folgende Werte:
- verstrichene Zeit seit Start
- alle 20ms eine Aufzeichnung der aktuellen Pulszähler für alle vier Reifen in hexadezimaler Darstellung (die Differenzen berechne ich selbst)
- alle 20ms eine Aufzeichnung der aktuellen Geschwindigkeit --> darüber habe ich mal den Reifenumfang berechnet: 179.67cm = ~180er Reifen, das müsste ich noch mal an einer anderen Messstelle nachrechnen, dann wäre auch der Wert des Reifenumfangs möglich fehlerfrei.
Und da ja die Spurweite nicht viel ausmacht, sollten dann alle "Unbekannten" soweit ok sein.

Die Rechnung funktioniert ja auch. Alle Formeln habe ich aufgeschrieben, anhand deiner Beispieldaten haben wir das auch verifiziert. Jetzt kann es nur noch an den Messdaten liegen, bzw. an der Aufzeichnung dieser Daten.

Eine Idee war ja meine Kantenglättung, die leider nicht viel Erfolg brachte. Als nächstes werde ich mal deine Idee auffassen und abfragen, ob ich irgendwo Intervalle drin habe, in denen sich nur ein Reifen bewegt und werde dann dort weitere Intervalle zusammen fassen, damit sich jeder Reifen mindestens ein wenig bewegt.

P.S.: Ich habe mal ein wenig gebastelt, damit ich euch mal die Strecke, die raus kommen soll, in etwa zeigen kann.

Original:

[attach]12825[/attach]

Ich habe das Bild mal gedreht, damit es von der Richtung her zum Bild von Mathematica passt. Die Startrichtung ist auch erst mal zweitrangig. Erst mal muss die Strecke stimmen. Big Laugh
Aber ansonsten passen die Bilder mit 200ms und 500ms sehr gut dazu, finde ich.
Reksilat Auf diesen Beitrag antworten »

Zitat:
Hm, das muss ich dann erst umprogrammieren. Ich habe die Intervalle nicht an an der Anzahl der Pulse festgemacht, sondern an der Zeit, die verstrichen ist.

Ich meinte ja nur, dass man die Zeitintervalle bei der Berechnung den Beobachtungen entsprechend verlängert, also Die Messanordnung habe ich jetzt gar nicht gemeint.

Deine Ergebnisse mit unterschiedlichen Intervalllängen sind allerdings auch recht durcheinander. Hätte nicht gedacht, dass das so einen Unterschied macht.

Die Fehler in der Grafik werden aber wohl auch von den Messergebnissen herrühren, schließlich ist bei so einem Fahrzeug das Gewicht ungleichmäßig verteilt, die Reifen haben unterschiedliches Profil, die Straße ist in der Mitte von anderer Qualität als am Rand,... Da wird man den exakten Weg der Reifen schwer nachvollziehen können.

Wenn der linke Reifen weniger Profil hat, rutscht er mehr, er legt mehr Umdrehungen zurück und der Rechner interpretiert das als Rechtskurve....
BlackDodo Auf diesen Beitrag antworten »

Zitat:
Original von Reksilat
Zitat:
Hm, das muss ich dann erst umprogrammieren. Ich habe die Intervalle nicht an an der Anzahl der Pulse festgemacht, sondern an der Zeit, die verstrichen ist.

Ich meinte ja nur, dass man die Zeitintervalle bei der Berechnung den Beobachtungen entsprechend verlängert, also Die Messanordnung habe ich jetzt gar nicht gemeint.


Ja, so meinte ich das. smile Bin jetzt wieder zu Hause und werde das gleich mal machen.

Zitat:
Original von Reksilat
Deine Ergebnisse mit unterschiedlichen Intervalllängen sind allerdings auch recht durcheinander. Hätte nicht gedacht, dass das so einen Unterschied macht.


Ja, das muss ich dir recht geben. Mich verwirrt das auch. Vor allem, warum 200ms und 500ms gut aussehen, 300ms und 400ms wiederum totaler Müll sind. Das Original kennst du ja jetzt und siehst, dass nur die beiden halbwegs passen. Bin etwas verwirrt. Big Laugh

Zitat:
Original von Reksilat
Die Fehler in der Grafik werden aber wohl auch von den Messergebnissen herrühren, schließlich ist bei so einem Fahrzeug das Gewicht ungleichmäßig verteilt, die Reifen haben unterschiedliches Profil, die Straße ist in der Mitte von anderer Qualität als am Rand,... Da wird man den exakten Weg der Reifen schwer nachvollziehen können.

Wenn der linke Reifen weniger Profil hat, rutscht er mehr, er legt mehr Umdrehungen zurück und der Rechner interpretiert das als Rechtskurve....


Über Schlupf habe ich auch schon nachgedacht. Mein Prof. meinte aber, das wäre nur gering (er kennt sich in die Richtung gut aus), von daher hoffe ich mal, dass das stimmt.

Die Frage ist: Wie kann ich die Ergebnisse verbessern mit den vorliegenden Messdaten? Diese "Kantenglättung" war ja z.B. ein Reinfall. Augenzwinkern
BlackDodo Auf diesen Beitrag antworten »

So, wenn ich unterbinde, dass nur ein Rad Weg gefahren ist (also einfach den Fall "1 Puls auf einer und 0 Pulse auf der anderen Seite" ausschließe), bekomme ich folgendes Ergebnis:

[attach]12830[/attach]

Wieder nix. Big Laugh
BlackDodo Auf diesen Beitrag antworten »

Ich habe übrigens auch noch in einem anderen Forum (ich weiß: Chrossposting, aber hier geht's ja nicht um ein einfaches Problem, dass schnell gelöst ist) über dieses Thema diskutiert:
http://www.chemieonline.de/forum/showthread.php?t=149733

Ich habe da jetzt den letzten Post mal angewandt und approximiert.

Aber viel besser wird's dadurch auch nicht. Augenzwinkern

Siehe hier:

[attach]12850[/attach][attach]12851[/attach][attach]12852[/attach][attach]12853[/attach][attach]12854[/attach]

Das ist der Reihe nach:
1. ohne Approximation
2. Approximation mit drei Werten (also Radius = 1)
3. Approximation mit fünf Werten (Radius = 2)
4. Radius = 3
5. Radius = 4
Reksilat Auf diesen Beitrag antworten »

Mit den Daten, die im anderen Thread standen, wirst Du die Strecke sowieso nicht gerade bekommen. Bei Deinen Meßdaten legt VL ungefähr einen Meter mehr zurück als VR und das führt zu einem Winkel von 0,475 bzw. 27° (das ist von der Gesamtstrecke unabhängig). Diesen Winkel wirst Du nie wegbekommen können.

Skizze einer Linkskurve mit Winkel 27°:
[attach]12857[/attach]

Die Interpolation, die Nick im anderen Forum vorgeschlagen hat, bringt mMn auch nicht viel, da wir ja keine plötzlich ausreißenden Messwerte haben, sondern einen Fehler, der sich über die gesamte Strecke beobachten lässt, nämlich einen leichten Rechtsdrift. Das sieht man auch sehr gut an den Bildern.
Am Algorithmus wirst Du demnach nichts mehr machen können. Der Fehler liegt bei den Messdaten und wird Dir keine bessere Kurve bringen können.

Gruß,
Reksilat.

Nachtrag: Da zwei parallele Diskussionen zu diesem Thema nichts bringen, wird hier geschlossen. Weiter geht es dort.
Gruß, Reksilat.
Neue Frage »
Antworten »



Verwandte Themen

Die Beliebtesten »
Die Größten »
Die Neuesten »