- LiTime Speicher und Akkus         
Ergebnis 1 bis 8 von 8

Thema: Rotation eines Roboters mit Berücksichtigung der vorherigen Blickrichtung

  1. #1

    Rotation eines Roboters mit Berücksichtigung der vorherigen Blickrichtung

    Anzeige

    Praxistest und DIY Projekte
    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...

    Code:
    	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!

  2. #2
    Erfahrener Benutzer Robotik Einstein Avatar von i_make_it
    Registriert seit
    29.07.2008
    Ort
    Raum DA
    Alter
    55
    Beiträge
    2.814
    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.
    Geändert von i_make_it (07.04.2017 um 12:03 Uhr)

  3. #3
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    07.04.2015
    Beiträge
    865
    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).

  4. #4
    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 ?

  5. #5
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    07.04.2015
    Beiträge
    865
    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:
    Klicke auf die Grafik für eine größere Ansicht

Name:	Movement.png
Hits:	4
Größe:	9,5 KB
ID:	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.

  6. #6
    Erfahrener Benutzer Robotik Einstein Avatar von i_make_it
    Registriert seit
    29.07.2008
    Ort
    Raum DA
    Alter
    55
    Beiträge
    2.814
    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.
    Geändert von i_make_it (10.04.2017 um 11:46 Uhr)

  7. #7
    Erfahrener Benutzer Begeisterter Techniker
    Registriert seit
    09.04.2008
    Beiträge
    384
    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:

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


    Wünsche noch einen schönen Abend!!

Ähnliche Themen

  1. Max. Gewicht eines Roboters
    Von Brainbug24 im Forum Allgemeines zum Thema Roboter / Modellbau
    Antworten: 10
    Letzter Beitrag: 16.08.2009, 20:06
  2. Facharbeit: Bau eines Roboters
    Von catchme im Forum Vorstellungen+Bilder von fertigen Projekten/Bots
    Antworten: 20
    Letzter Beitrag: 07.05.2009, 11:16
  3. Bestandteile eines Roboters
    Von Hectic im Forum Allgemeines zum Thema Roboter / Modellbau
    Antworten: 5
    Letzter Beitrag: 23.12.2008, 12:18
  4. Schwimmfähigkeit eines Roboters?
    Von Maruu im Forum Mechanik
    Antworten: 124
    Letzter Beitrag: 08.12.2007, 12:16
  5. Sensorsteuerung eines Roboters
    Von bkr-Gr.7 im Forum Elektronik
    Antworten: 1
    Letzter Beitrag: 05.04.2005, 11:36

Stichworte

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •  

LiTime Speicher und Akkus