Odometrie / dead reconing - Kalibrierung

**robvoi** · 12.04.2013, 18:02

Hallo,

ich versuche derzeit die Position meines Roboters per Odometrie / dead reoning zu bestimmen. Soweit funktioniert es ganz gut. Ich habe aber eine Abweichung, für die mir der Grund nicht aufgeht.
Was gut funktioniert:

Y-Position
Winkel bei Rechtsdrehung
Winkel bei Linksdrehung

Eine Abweichung habe ich bei der X-Position. (Test fahre zu X=0;Y=100 (cm)).
Bei einer Fahrt von 100cm geradeaus habe ich eine Abweichung von ca. 5 cm nach links - gegenüber der Position, die der Roboter zu haben glaubt.
Beispiel: Start X=0;Y=0. Ziel: X=0;Y=100.
Roboter Position gemessen Odometrie: X=-0.13;Y=102.2.
Roboter Position gemessen Zollstock: X=-5.5;Y=102

Ich komme nicht drauf, wo der Ungenauigkeit in den Ausgangswerten ist. Ich habe versucht sowohl Raddruchmesser links, rechts wie auch den Radabstand anzupassen. Die Abweichung wird höchstens schlimmer. Da sie aber relativ konstant ist, muss es ja einen systematischen Grund haben. Der linke Motor läuft generell schwerer, was aber von der Reglung ausgeglichen wird. Selbst eine Drift ohne Reglung ist kein Problem, so lange die gemessene Position stimmt.

Hat einer von euch eine Idee? Wie/was könnte ich testen?

Der relevante Code:

Code:

float MUL_COUNTl;
float MUL_COUNTr;
float MUL_COUNT_avg;
#define WHEEL_DIAMETERl 6.9
#define WHEEL_DIAMETERr 6.9
#define PULSES_PER_REVOLUTION 140.0
#define AXLE_LENGTH 11.9

float theta; 
float X_pos;               
float Y_pos;

Code:

MUL_COUNTl  = PI * WHEEL_DIAMETERl / PULSES_PER_REVOLUTION;
MUL_COUNTr  = PI * WHEEL_DIAMETERl / PULSES_PER_REVOLUTION;
MUL_COUNT_avg = (MUL_COUNTl + MUL_COUNTr) / 2;

Code:

/*********************/
/* define structures */
/*********************/

struct position
{
	float x;        /* meter */
	float y;        /* meter */
	float theta;    /* radian (counterclockwise from x-axis) */
};


/********************/
/* global variables */
/********************/

struct position current_position;


/********************/
/* define functions */
/********************/

void initialize_odometry()
{
	current_position.x = 0.0;
	current_position.y = 0.0;
	current_position.theta = 1.5707963267948966192313216916398;
}

void odometer_thread()
{
	if (millis() - odometersmillis >= 10) {
		float dist_left;
		float dist_right;
		int left_ticks;
		int right_ticks;
		float expr1;
		float cos_current;
		float sin_current;
		float right_minus_left;


		lsamp = encLeft;
		rsamp = encRight; 


		left_ticks = lsamp - last_left;
		right_ticks = rsamp - last_right;


		last_left = lsamp; 
		last_right = rsamp; 


		dist_left = (float)left_ticks * MUL_COUNTl;
		dist_right = (float)right_ticks * MUL_COUNTr;

		cos_current = cos(current_position.theta);
		sin_current = sin(current_position.theta);

		if (dist_left == dist_right)
		{
			current_position.x += dist_left * cos_current;
			current_position.y += dist_left * sin_current;
		}
		else
		{

			expr1 = AXLE_LENGTH * (dist_right + dist_left)
				/ 2.0 / (dist_right - dist_left);

			right_minus_left = dist_right - dist_left;

			current_position.x += expr1 * (sin(right_minus_left /
				AXLE_LENGTH + current_position.theta) - sin_current);

			current_position.y -= expr1 * (cos(right_minus_left /
				AXLE_LENGTH + current_position.theta) - cos_current);

			current_position.theta += right_minus_left / AXLE_LENGTH;


			if (current_position.theta > PI)
				current_position.theta -= (2.0*PI);
			if (current_position.theta < -PI) 
				current_position.theta += (2.0*PI); 
		} 

		theta = current_position.theta;
		X_pos = current_position.x;
		Y_pos = current_position.y;

		odometersmillis = millis();
	}
}

Danke und Gruß
Robert

**Dayzz** · 16.05.2013, 23:24

Hallo Robert.

Ist es möglich, daß der linke Motor etwas mehr ruckelt / vibriert als der rechte, wenn er angetrieben wird. Könnte ja sein, daß er dadurch einen Hauch mehr Schlupf bei jedem Vortriebs-Impuls hat als der rechte und deshalb mit jedem "Schubser" ein wenig durchdreht. Du könntest ja mal versuchen, wie die Abweichung sich auf verschiedenen Untergründen verändert. Asphalt hat von allen leicht erreichbaren Untergründen vermutlich den höchsten Reibwert.

Grüße, Dayzz

WL · 17.05.2013, 08:34

Hallo Robert,

Zitat von robvoi

Der linke Motor läuft generell schwerer, was aber von der Reglung ausgeglichen wird. Selbst eine Drift ohne Reglung ist kein Problem, so lange die gemessene Position stimmt.

Was wird geregelt ? Die Drehzahl ?

Versuch mal die Differenz der Motorimpulse (Links/Rechts) in die Regelung als Zusatzsollwert einzubeziehen.
Dann sollte die absolute Abweichung nur noch von Schlupf und Getriebefaktor (Raddurchmesser) abhängen.

**robvoi** · 17.05.2013, 10:56

Hallo,

danke für die Antworten.

@Dayzz
Ich lasse den Roboter sehr langsam beschleunigen, bremsen und fahren - um Schlupf zu vermeiden. Der Robot hat Gummieräder und fährt auf Laminat. Eigentlich sollte es kaum Schlupf geben. Für Asphalt ist er nicht geeignet. Es ist ein Turtle Bot mit einem kleinen caster wheel (als Ball in einem Gehäuse).
Schlupf könnte es aber wirklich sein, da er immer leicht nach links zieht, was vom Regler dann ausgeglichen wird. Dieses Manöver könnte ihn dann schon Seitwärts ziehen, was beim Differentialantrieb ja eigentlich nicht möglich ist.

@WL
Ich nutze zwei Fahrtenregler (RPM li und RPM re) mit für das Getriebe angepassten Werten. Zusätzlich noch einen Regler, der Soll und Zielwinkel (bei der Geradeausfahrt also genau die Differenz der Encoderpulse) anpasst.

Ich hab schon viel rumprobiert. Leider bekomme ich die Abweichungen nicht kleiner. Mein Ansatz ist jetzt, dass er sich nach ein paar Metern Fahrtstrecke zum Ausgangspunkt zurückbegibt und an Landmarken ausrichtet. Mal sehen ...

Gruß
Robert

**Klebwax** · 17.05.2013, 11:07

Du bist ja mächtig großzügig mit floats. Auf was für einem Prozessor läuft denn der Code?

MfG Klebwax

**robvoi** · 17.05.2013, 11:32

Der läuft auf einem ATmega2560.
Ohne Floats ist die Genauigkeit aber auch direkt dahin. Oder übersehe ich da was?

Gruß
Robert

**damfino** · 17.05.2013, 12:16

Wie ist eigentlich die Anzahl der Gesamtimpulse Links/Rechts nach 1m? Und wie sind diese auf den ersten 10cm? Es genügt ein kleiner Unterschied beim Anfahren, und schon macht er eine kleine Kurve und fährt dann den Rest der Strecke gerade weiter.

Die Berechnungen könnte man etwas aufräumen, zB einmal am Anfang right_minus_left / AXLE_LENGTH berechnen, und nicht in jedem Schritt einzeln.
Ich hab auf meiner Homepage eine ähnliche Berechnung für Odometrie angeben, vielleicht kannst diese mal vergleichen http://members.aon.at/wstocke6/Positionsbestimmung2.pdf

LG!

**robvoi** · 17.05.2013, 12:26

Der fährt unter Garantie nicht komplett gerade an. Das macht aber auch nichts, solange er das weiß/berechnet. Mein Problem ist ja, dass es eine Abweichung gibt, die nicht in die Positionsschätzung eingeht.

Das Aufräumen habe ich direkt auf meine ToDo Liste gesetzt. Danke für den Hinweis!

Gruß
Robert

**damfino** · 17.05.2013, 15:50

Es wäre halt interessant zu wissen ob er eine Abweichung erkennt und später ausgleicht oder nicht.

Andere mögliche Ursachen für die Drift:
Haben die Antriebsräder die gleiche Radlast? Mal mit der Küchenwaage überprüfen.
Ist das Caster Wheel genau mittig? Ist der Ball wirklich leicht beweglich? Vielleicht durch ein Rad ersetzen?

LG!

**RP6conrad** · 17.05.2013, 17:28

Ich soll mal die raw data (puls L/R rad) ausgeben und speichern wahrend der Robby fahrt. Danach konnen sie diese Werte in excell einlesen und dort die Berechnungen machen (Radbasis + Auflosung kennen sie). Dan sieht man gleich wo ess klemmt. Wen du die gleiche Ergebnissen hat in excell, dan ist das problem in die hardware (werden alle Pulsen L/R richtig gezaehlt ?). Wen ihre Robby in excel gerade fahrt, dan muss das ein SW problem sein.