Rotation eines Roboters mit Berücksichtigung der vorherigen Blickrichtung

**Holomino** · 07.04.2017, 13:39

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

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:

Code:

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

Code:

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

Code:

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

Code:

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** · 09.04.2017, 12:13

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** · 10.04.2017, 09:30

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:

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** · 10.04.2017, 10:40

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** · 10.04.2017, 19:08

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: