Odometrie / dead reconing - Kalibrierung
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
