Rotation eines Roboters mit Berücksichtigung der vorherigen Blickrichtung

[PeterHgn]

Guten Abend,

ich befürchte ich habe einen kleinen Denkfehler! Ich möchte einen Roboter entsprechend des neuen Zieles rotieren lassen. Die Funktion erhält die Blickrichtung in welcher er sich momentan befindet, sowie die Ausgangs und Zielkoordinaten des Roboters. Dann Soll der neue Winkel zum neuen Punkt(Ziel) berechnet werden...


public int rotate(int x, int y, int t_x, int t_y, double dir){

double Dir_rad=0;
Dir_rad=Math.toRadians(dir);
double angle=(Math.atan2(t_y-y,t_x-x));
double new_Angle=(angle-Dir_rad)%(2*Math.PI);
double Robot_angle=0;
Robot_angle= Math.toDegrees(new_Angle);
return (int) Robot_angle;
}

int x und int y sind die Ausgangskoordinaten und t_x sowie t_y die Zielkoordinaten.

Jetzt soll sich der Roboter in Richtung des neuen Zieles Drehen. Dabei wird die vorherige übergebene Blickrichtung berücksichtigt.


Hoffe wirklich sehr, mir kann jemand bei meiner Funktion helfen!


Vielen Dank im vorraus!!
Schönen Abend noch!

[i_make_it]

Ich sehe keine Fallunterscheidung für die vier Quadranten (bezogen auf die Ausgangspostion nicht auf das Weltkoordinatensystem, da gibt es dann nochmal 4 Quadranten).

einfach mal vier Zahlenbeispiele nehmen und durchrechnen

Bsp.1 Q1

t_y-y = 5-2=3
t_x-x = 6-1=5
tan=3/5=0,6
atan=tan exp-1= 0,540 rad (30°)

Bsp.2 Q2

t_y-y = 3-1=2
t_x-x = 2-4=-2
tan=2/-2=0
atan=tan exp-1= 0 (0°)

Bsp.3 Q3

t_y-y = 1-5=-4
t_x-x = 3-6=-3
tan=-4/-3=-7
atan=tan exp-1= -1,429 (-81°)

Bsp.4 Q4

t_y-y = 2-6=-4
t_x-x = 7-1=6
tan=-4/6=-0,6Periode
atan=tan exp-1= -0,588 rad (-33°)


Spätestens bei den Beispielen für Quadrant 2 und 3 fällt auf, das da was nicht stimmen kann.
Denn wenn man es maßstäblich zeichnet sieht man was anderes.

Mann kann auch für alle vier Quadranten mal mit 45° Winkeln und Differenzen von 2 arbeiten, dann wird es noch deutlicher.

Q1
t_y-y = 3-1=2
t_x-x = 4-2=2
tan=2/2=1
atan=tan exp-1= 0,785 (45°)

Q2
t_y-y = 3-1=2
t_x-x = 2-4=-2
tan=2/-2=0
atan=tan exp-1= 0 (0°)

Q3
t_y-y = 2-4=-2
t_x-x = 1-3=-2
tan=-2/-2=-4
atan=tan exp-1= -1.326 (-75°)

Q4
t_y-y = 1-3=-2
t_x-x = 4-2=2
tan=-2/2= -1
atan=tan exp-1= -0,785 (-45°)

Also Fallunterscheidung:
Ist t_y>y und t_x>x gilt Q1
Ist t_y>y und t_x<x gilt Q2
Ist t_y<y und t_x<x gilt Q3
Ist t_y<y und t_x>x gilt Q4

Dann für jeden Quadranten so umformen, daß das Dreieck für Q1 gilt und den Winkeloffset mitgeben.
Ausrechnen und Ergebniss mit dem Winkeloffset verrechnen.
Dann Differenzwinkel zur Startausrichtung ermitteln und über Vorzeichen entscheiden ob links oder rechts gedreht werden muß.

Und nicht vergessen, daß das Weltkoordinatensystem auch 4 Quadranten hat. Sprich sich da dann die Vorzeichen noch mehrfach drehen können, abhängig davon ob Start und Zielpunkt beide im selben Quadranten liegen oder in verschiedenen.

als Maschinenbauer kenne ich das.
Genau die selbe Situation hat man auch in der Statik wenn man eine Resultierned Kraft und deren Kraftwinkel ermitteln will.
Da zerlegt man alle Kräfte in ihren X und Y Anteil bildet die Summen und ermittelt dann den Quadranten in dem die resultiernende Kraft liegt.
Rechnet dann den Winkel für ein Dreieck in Q1 aus und addiert dann den Offset (90, 180, 270°) für den tatsächlichen Quadranten.
Aus den X und Y Summen dann wieder die eigentliche resultiernde Kraft bilden und man hat Kraft und Kraftwinkel.

Nur das Berücksichtigen von Quadranten des Weltkoordinatnesystems hat man nicht, da man erst mit der resultierenden Kraft deren Ansatzpunkt und Kaftwinkel in die Weiterführende Berechnung von Dreh- Biegemomenten oder Spannungen geht.

[Holomino]

Jepp, die vier Quadranten fehlen:
Vielleicht hilft Dir der folgende Code



double AngleOf(double x, double y)
{
if ((x == 0) && (y == 0))
return 0;

double rval = 0;
uint8_t iPre;
if (y >= 0) //Quadrant determination
iPre = (x > 0 ? 0 : 1);
else
iPre = (x > 0 ? 3 : 2);

if (x != 0)
{
switch (iPre)
{
case 0: //1st Quadrant
rval = atan(y / x);
break;
case 1: //2nd Quadrant
rval = M_PI + atan(y / x);
break;
case 2: //3rd Quadrant
rval = atan(y / x) - M_PI;
break;
case 3: //4th Quadrant
rval = atan(y / x);
break;
}
}

else //special case: vertical lines
{
if (y > 0)
rval = M_PI / 2;
else
rval = -M_PI / 2;

}
return rval;
}
Auch noch ganz wichtig, damit der Winkel statt -270° dann später + 90° wird:

double AngleNorm(double angle)
{
while (angle > M_PI)
angle -= (M_PI * 2);
while (angle <= -M_PI)
angle += (M_PI * 2);

return angle;
}



Wenn man sich jetzt noch die Mühe macht, die ganze Posiergeschichte in Datentypen zu fassen...


typedef struct
{
double X;
double Y;
} Point_t;


typedef struct
{
Point_t Point;
double Orientation;
} Pose_t;

typedef struct
{
double Distance;
double Angle;
} PoseChangeVector_t;



.... gibt die Rechnerei dann auch gleich die Distanz und den Winkel aus....


void GetChangeVector(Pose_t* actual, Point_t* target, PoseChangeVector_t* result)
{
double dx = target->X - actual->Point.X;
double dy = target->Y - actual->Point.Y;

result->Distance = sqrt((dx*dx) + (dy*dy));
result->Angle = AngleNorm ( AngleOf(dx, dy) - actual->Orientation);
}



...und Du kannst schon im Simulator (falls vorhanden)...


double GetChangeVectorTest()
{
Pose_t actual;
actual.Orientation = M_PI/4;
actual.Point.X = 100;
actual.Point.Y = 100;

Point_t target;
target.X = -100;
target.Y = -100;

PoseChangeVector_t change;

GetChangeVector(&actual, &target, &change);
return change.Angle;
}




... durch die Variation von actual- und target-Parametern Dein Ergebnis testen.

(Rennt bei mir seit drei Jahren, hab ich mir damals aber auch erst 'ne Stunde das Hirn verrenkt).

[PeterHgn]

Guten Tag,
ersteinmal ein großes Dankeschön für die schnelle ausführliche Hilfe!!

Ich habe die Funktion jetzt soweit umgeschrieben, dass die Winkel richtig berechnet werden! Allerdings verstehe ich nicht so ganz, wie die Richtung entsprechend aktualisiert wird!
Wo und wie speicherst bzw. überschreibst du actucal->Orientation ?

[Holomino]

Gern geschehen!

Ich hoffe, ich habe die Frage nach dem Überschreiben von actual->Orientation richtig verstanden:
Nun, es kommt auf die Bewegung des Roboters an:
32541
Wenn Du zuerst drehst und anschließend fährst, dann ist der neue Winkel Deiner aktuellen Pose:
actual.Orientation = actual.Orientation + Change.Angle;

wenn Du allerdings eine Bahn fährst, die anhand von zwei PID-Reglern die Regelungsdifferenz von Distanz und Winkel auszugleichen versuchen, dann wirst Du
a) während der Fahrt zyklisch die aktuelle Pose Deines Gefährtes berechnen (aus den Änderungen Deiner Radencoderwerte),
b) daraus dann den ChangeVector zum Zielpunkt ermitteln und
c) zuletzt aus den resultierenden Regeldifferenzen "Winkel" und "Distanz" dann die PWM-Werte für die Motoren ermitteln.

Ich weiß, das wirft jetzt weitere Fragen auf, allerdings hab ich so den Verdacht, ich habe Deine Frage missverstanden.

[i_make_it]

Da Roboterkoordinaten und Ausrichtung die auf ein Weltkoordinatensystem bezogen sind, eigentlich nur Sinn machen wenn man kartieren will. denke ich das es bei der Eingangsfrage um SLAM geht.
Keine Ahnung ob um Rasterkarte (wegen der karthesischen Koordinaten) oder um Vektorkarte (wegen des Drehwinkels), während Holominos Implementierung, bei kurzem Drübersehen, scheinbar die Regleung für das Abfahren einer Bahn darstellt.
Wenn man beides mit einer Funktion erledigen will, ist noch etwas mehr Arbeit zu tun und man muß darauf achten, das man kein Timing Problem bei der Regelung bekommt.
Üblicherweise trennt man auch Bahnplanung und Bahnregelung, da man beim abfahren eines Bahnsegmentes bereits das nächste oder übernächste plant und in der Karte hinterlegt. Real auftretende Abweichungen durch nicht vorhersehbare Hindernisse, zwingen einen eh dazu das ganze rekursiv neu zu berechnen. Sieht man auch bei einem Navi, wenn man mal nicht den Anweisungen folgt und die Routen(Bahn)planung dann, von der jeweils aktuellen Position, mehrfach neu durchgeführt wird bis es wieder passt.

[RP6conrad]

Auch sehr einfache Algoritmen können eine gute Ergebnis liefern ! Beispiel ist meinen GPS-rover. Aus Ist-pos (GPS) und Soll-pos (Waypoint) wird mit atan2(soll-ist) die soll-Himmelsrichting bestimmt. Den GPS-rover steuert dan in diese Richtung. Naturlich kann er nie genau das Waypoint treffen, wegen GPS-Abweichungen und steuergenauigkeit. Darum wird ab eine bestimme Abstand von Waypoint weiter geschaltet nach das naechtste Waypoint. Das functioniert erstaunlich gut, nur muss men in Augen halten das ein GPS doch ziemlich fiel "Verzogerung" hat (bei meinen GPS schon 400 ms).
Hier einige Videos:
https://www.youtube.com/watch?v=yMbmp7c2mrw

https://www.youtube.com/watch?v=L_Jshh8BfnU&amp;t=1s

[PeterHgn]

Ein großes Dankeschön an die Mitglieder hier im Forum!!
Ihr habt mir super weitergeholfen!


Wünsche noch einen schönen Abend!!:)