Menü
Aktuelles
Von:
Filter

Perimeter Tracking One Bounce Algorithmus

R

roland

Active member
Hallo zusammen,

ich möchte hier mal meine Fortschritte im Bereich Perimetertracking dokumentieren und ggf. hat ja noch der Eine oder Andere eine Idee, diese zu optimieren.

Am Anfang hatte ich vor innerhalb der Schleife (ca. 1 m Abstand) zur Ladestation zu fahren und dann nahe der Ladestation auf die Schleife zu wechseln und dann in die Ladestation zu fahren. Der Vorteil liegt darin, dass man innerhalb der Schleife wesentlich schneller fahren kann als über der Schleife, da die Toleranzbereiche größer sind.

Leider hat mir meine Umgebung leider einen Strich durch die Rechnung gemacht, da ich zwei Passagen habe wo der Draht teilweise nur 1m bzw. nur 50cm voneinander liegt.

Als erstes habe ich dann meinen Code so geschrieben, dass ich den PID für das Tracking mit der Perimeteramplitude als Eingang versorgt habe. Das hat auch relative gut funktioniert. Hierzu muss ich noch erwähnen, dass ich hinten eine dritte Spule an den Mower angebracht habe, so dass ich nun Rückwärts den Perimeter abfahren kann.
https://youtu.be/st3JQdVQ0fY
Doch es gab zwei Nachteile:
Zum einen muss das Perimetersignal nach Möglichkeit immer die gleiche Stärke haben, so dass die PID Parameter von Schleife zu Schleife extra getuned werden müssen.
Zum anderen darf der Robbi auf keinen Fall weiter als 20 cm mit dem Hinterrad (was nun vorne ist) über den Perimeter fahren. Dies kann mit der Amplitude und einer Spule nicht wirklich festgestellt werden, da das Maximum der Amplitude ca. 8cm vom Drath entfernt ist.
Anbei mal ein Bild der Gemessenen Amplituden Werte. Links von 0 ist Außerhalb des Perimeters:

20170212122456.png


Hier habe ich es nun gemacht wie im Ardumower Code und und stoppe nach zwei Sekunden und drehe den Mower dann.
Hätte ich bestimmt noch besser hinbekommen, doch dann bin ich auf folgendes Video gestoßen Zeit: 1:22
https://www.youtube.com/watch?v=SGJ9yxEgr70&index=34&list=PL4GsVqyuXHkeDUMbMyggVdhITIbzJ1SZK
Weitere Recherchen brachten mich zu folgendem Video Zeit: 5:09
https://www.youtube.com/watch?v=btD0FLkGNDQ&index=33&list=PL4GsVqyuXHkeDUMbMyggVdhITIbzJ1SZK
Man sieht sehr schön, wie der Roboter gegen den Perimeter bounced.
Das es diesen Algorithmus gibt, was mir schon lange bewusst, doch wenn ich Videos davon über Line Follower angeschaut habe, sah das nicht wirklich toll aus.

Der Algorithmus löst auf jeden Fall das Problem, dass ich zu weit über den Perimeter fahre, da der Roboter wenn er über den Perimeter fährt sofort nach innen gelenkt wird. Dann fährt er in einem kleinen Bogen am Perimeter innen entlang, bis er wieder bounced. Weiterhin ist man von der Amplitude wie im originalen Ardumower Code unabhängig.

Hier mein Ergebnis. Wenn ich die Ladestation an einem geraden Abschnitt stelle, wird er vermutlich gerade einfahren.
https://youtu.be/khzK0Fq8tkA

Realisiert habe ich dies, in dem ich das Bouncen fest programmiert habe. Das funktioniert, da die Geschwindigkeit immer gleich ist. Das Fahren innerhalb des Perimeters wird durch das Intergral des PID bestimmt. Weiterhin treffe ich noch einige Fallentscheidungen in Abhängigkeit, wie viel Zeit von der letzten Peirmetertransition vergangen ist. Letztendlich mache ich es wie im Ardumower Code aber etwas abgeändert. Die PID Werte KP und KD habe ich mal drinnen gelassen obwohl aktuell nicht benötigt.


Code: 
class TFLfollowLine: public Node
{
private:

    double last_error, integral;

public:
    double Kp, Ki, Kd;
    unsigned long lastRun;

    TFLfollowLine() {
        Kp = 0; //
        Ki = 0.5;
        Kd = 0;

        lastRun = 0;
        last_error=0;
        integral = 0;
    }

    virtual void onInitialize(Blackboard& bb) {

        lastRun = 0;
        last_error=0;
        integral = 0;

    }

    virtual NodeStatus onUpdate(Blackboard& bb) {
        if ( millis() - lastRun  < 100) return BH_RUNNING;
        lastRun = millis();

        double error =  bb.perimeterSenoren.magnetudeB;
        if(error <= 0) {
            error =-1;
        } else {
            error = 1;
        }

        double derivate = error-last_error;
        integral = integral + (Ki*error);

         //Set integral to 0 if crossing the line
        if (sign0minus(error) != sign0minus(integral)) { //sign0minus => 0 belongs to minus
            integral = 0;
        }

         double Output = Kp  * error + integral + Kd * derivate ;

        if(Output > bb.LINEFOLLOW_SPEED_LOW) Output = bb.LINEFOLLOW_SPEED_LOW;
        if(Output < -1*bb.LINEFOLLOW_SPEED_LOW) Output = -1*bb.LINEFOLLOW_SPEED_LOW;

        last_error = error;

        bb.cruiseSpeed = bb.LINEFOLLOW_SPEED_LOW;
        bb.driveDirection = DD_REVERSE_LINE_FOLLOW;

        if(error>0.0f) { //Inside Perimeter
            if ( (millis()-bb.perimeterSenoren.perimeterLastTransitionTimeB) > 3500 ) { // If more than 3.5sec inside rotate full
                bb.cruiseSpeed = 15;
                bb.driveDirection = DD_ROTATECW;
                bb.motor.L->setSpeed(15);
                bb.motor.R->setSpeed(-15);
            } else if ( (millis()-bb.perimeterSenoren.perimeterLastTransitionTimeB) > 2800 ) { // If more than 2.8sec inside rotate more aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed+10));                
            } else if ( (millis()-bb.perimeterSenoren.perimeterLastTransitionTimeB) > 2000 ) { // If more than 2sec inside rotate aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed+5));
            } else {
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed));
            }

        } else { //Outside Perimeter

            if ( (millis()-bb.perimeterSenoren.perimeterLastTransitionTimeB) > 2000 ) { // // If more than 2sec outside rotate full
                bb.cruiseSpeed = 15;
                bb.driveDirection = DD_ROTATECC;
                bb.motor.L->setSpeed(-15);
                bb.motor.R->setSpeed(15);
            } else if ( (millis()-bb.perimeterSenoren.perimeterLastTransitionTimeB) > 1500 ) { // If more than 1.5sec outside rotate more aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+7));
                bb.motor.R->setSpeed(-(-15));
            } else if ( (millis()-bb.perimeterSenoren.perimeterLastTransitionTimeB) > 1000 ) { // If more than 1sec outside rotate aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+7));
                bb.motor.R->setSpeed(-(-8));
            } else {
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+10)); //5
                bb.motor.R->setSpeed(-(bb.cruiseSpeed -25)); //30
            }
        }


        return BH_RUNNING;
    }
};


Wenn man sich die Bedienungsanleitung von Ambrogio anschaut, kann man auch innerhalb des Perimeters fahren und dann bei engen Passagen auf das Perimeterkabel wechseln. Weiterhin kann man ein Dreieckmuster in das Perimeterkabel verlegen und dem Robbi sagen, er soll auf die andere Seite des Rasens wechseln um den Weg zur Ladestation abzukürzen. Ggf. werde ich das Später noch umsetzen.
 
Hallo Roland,

das sieht echt gut aus - ich hatte ja auch schon versucht, eine schöne Geradeausfahrt mit 2 Spulen hinzubekommen (so wie der Indego es macht: Video ) - leider hat das bei mir nicht ohne eine Zick-Zack Fahrt funktioniert.

Mein Prinzip bei 2 Spulen: innerhalb eines Fensters (linke Spule innerhalb/rechte Spule außerhalb) fährt der Roboter geradeaus, links oder rechts vom Fenster dreht er wieder ins Fenster. Das sieht dann ungefähr so wie beim Indego aus, leider wie gesagt mit ständigen Drehern bei Geradeausfahrt verbunden. Beim Indego scheint es etwas anders zu funktionieren so dass er schön Geradeausfahren kann, denn benutzt scheinbar die linke Spule für's Tracking und diese Seite wird auch in die Ladestation geführt ( erkennbar am Loch unten an der Ladestation ).


Gruss,
Alexander
 
Mit 2 Spulen und PID habe ich es auch mal gemacht. Allerdings kein Video. Das Problem war, dass die innere Spule am Perimeterkabel klebte. Somit bin ich zu weit mit dem rechten Rad außerhalb des Perimeters gefahren.

Ich habe test weise mal die "Schnelle Rückkehr" (Ambrogio Begriff) getestet. Die Idee ist wirklich gut. Zum einen kann man den Weg zur Ladestation verkürzen, zum Anderen hat man nun eine Möglichkeit von Inseln wegzukommen wenn diese auf dem Weg zur Ladestation als erstes angefahren werden.
https://youtu.be/UVDPixXWZv4
Aktuell ist der Algorithmus den ich verwende nicht wirklich gut. Hat jemand eine Idee zum erkennen des Dreiecks?
Normalerweise muss ich erkennen, dass ich einen ca. 45 Grad Linksschwenk mache und kurz danach um ca. 90 Grad rotieren muss.
Wichtig ist die eindeutige Erkennung des 45 Grad Schwenks. Denn ein 90 Grad Aussenwinkel hat diesen nicht. Somit kann ich dann 90 Grad Perimeter Aussenecken von dem Dreieck unterscheiden.
Mit stehen nur Encoderwerte und Zeitmessung zur Verfügung sowie 3 Spulen.
 
Dreieck erkennen: Vielleicht ungefähr so?

2 Zähler: L und R
3 Zeitstempel ZN, ZL und ZR

Ständig ausführen:
1. Beide Zähler L und R um aktuelle Encoder-Änderung (+/-) erhöhen.
2. Zeitstempel ZN setzen
3. Falls R größer als L (Linkskurve) Zeitstempel ZL setzen
4. Falls L größer als R (Rechtskurve) Zeitstempel ZR setzen
5. Falls ZR > ZL + 1 Sek. => Dreieck
6. Falls 1 Sek. abgelaufen, L und R auf Null setzen.
 
So ähnlich hatte ich es gemacht. Der Fehler war, dass ich als Startsignal den Übergang von innen nach außen festgelegt hatte. Nun speichere ich ununterbrochen die Encoderwerte alle 100ms in ein Array ab und Berechne dann die Differenz von den Encoderwerten links und rechts. Funktioniert schon besser. Bin aber noch am testen.
 
So, das Dreieckmuster im Perimeter zu erkennen funktioniert gut. Um eine Kurve zu erkennen sample ich die Encoderveränderungen alle 100ms für insgesamt 3Sekunden. So habe ich einen Ausschnitt der gefahrenen Strecke von 3 Sek. und kann herausfinden, ob ich eine Kurve gefahren habe.



Code: 
/*
  Für das finden eines Dreiecks in der Schleife wird ständig nach einer Linkskurve "Ausschau" gehalten.
  Wenn eine Linkskurve erkannt wurde wird die Zeit festgehaten und timeLeftTurnFound gesetzt. 
  Wenn ein Dreieck vohanden ist, wird der Robbi nicht mehr so einfach von innen nach außen wechseln können,
  da er nach der scharfen Linkskurfe bereits über das Ende des Dreieckes gefahren ist. Daher wird er auf jeden Fall in die if Abfrage:
  "if ( (millis()-lastTransitionTime) > 3500 )" gehen wo er rotiert. Hier wird geguckt, ob die Linkskurve innerhalb
  der letzten 5 Sek. gefunden wurde. Wenn ja, wird flagTriangleFound = true gesetzt und rotiert, bis die andere Seite des Dreiecks erreicht
  wurde. Wenn diese erreicht wurde, geht die Spule von innen nach außen. Bei diesem Übergang wird geprüft ob ein Dreieck gefunden wurde mit
  flagTriangleFound. Weiterhin muss mindesten noch eine Kurve von 80 nach rechts gefahren worden sein und die  Zeit für die Rechtskurve muss mindestens
  4Sek. gedauert haben. Dann werden die Zweige im BehaviourTree mit  bb.flagFollowLine = false;  bb.flagGoHome = true; umgeschaltet, so dass der Mower wieder normal fährt 
  bis die andere Seite des Perimeters erreicht wurde.

*/

#define ENCDELTAARRAY 30  // Check curve for 3sec

class TFLfollowLine: public Node
{
private:
    double integral;
    unsigned long lastRun;
    int encDeltaL[ENCDELTAARRAY]; //Array measured encoder ticks/100ms
    int encDeltaR[ENCDELTAARRAY];
    int idxL;
    int idxR;
    unsigned long timeLeftTurnFound;
    unsigned long timeTirangleFound;
    bool flagLeftTurnFound;
    bool flagTriangleFound;
    unsigned long lastTransitionTime;

public:
    double Ki;

    TFLfollowLine() {
        Ki = 0.5;
        lastRun = 0;
        integral = 0;
    }

    virtual void onInitialize(Blackboard& bb) {
        lastRun = 0;
        integral = 0;
        memset(&encDeltaL[0], 0, ENCDELTAARRAY*sizeof(int));
        memset(&encDeltaL[0], 0, ENCDELTAARRAY*sizeof(int));
        idxL = 0;
        idxR = 0;
        bb.encoderL.resetPositionCounter();
        bb.encoderR.resetPositionCounter();
        timeLeftTurnFound = 0;
        flagLeftTurnFound = false;
        flagTriangleFound = false;
        lastTransitionTime = 0;
    }

    virtual NodeStatus onUpdate(Blackboard& bb) {
        long sumL=0;
        long sumR=0;
        
        if ( millis() - lastRun  < 100) return BH_RUNNING;
        lastRun = millis();

        // Check for left turn. 
        // Read encoder values and check for left turn.
        // If left turn found, set flagLeftTurnFound=true.
        //============================================
        encDeltaL[idxL++]=bb.encoderL.getPositionCounter();
        encDeltaR[idxR++]=bb.encoderR.getPositionCounter();
        if(idxL >  ENCDELTAARRAY-1) idxL = 0;
        if(idxR >  ENCDELTAARRAY-1) idxR = 0;
        bb.encoderL.resetPositionCounter();
        bb.encoderR.resetPositionCounter();

 /*         
        for(int i=0; i<ENCDELTAARRAY; i++) {
            debug->printf("%d  %d  %drn",encDeltaL[i], encDeltaR[i], encDeltaL[i] - encDeltaR[i]);
        }
        debug->printf("==============rn");
*/        
      
        for(int i=0; i<ENCDELTAARRAY; i++) {
            sumL += encDeltaL[i];
            sumR += encDeltaR[i];
        }

        long angle = (sumL - sumR)/10; // Not really the correct angle but it works because the -80 and 80 for a curve found out during tests.
       

        if(angle < -80 ) { // -80 found out during test by try and error
            debug->printf("Left turn found angle: %drn",angle);
            timeLeftTurnFound = millis();
            flagLeftTurnFound = true;
        } else {
            // Not found
            debug->printf("Winkel: %drn",angle);
        }


        // Calculate Speed
        //============================================
        double error =  bb.perimeterSenoren.magnetudeB;
        if(error <= 0) {
            error =-1;
        } else {
            error = 1;
        }

        integral = integral + (Ki*error);

        //debug->printf("error: %frn",error);

        //Set integral to 0 if crossing the line
        if (sign0minus(error) != sign0minus(integral)) { //sign0minus => 0 belongs to minus
            integral = 0;
            lastTransitionTime = millis();

            // If changing form inside to outside after rotating CW and a triangle was found => cross lawn
            if(error < 0) {
                if(flagTriangleFound) {
                    if(angle > 80) { // Rotated CW
                        debug->printf("delta timeTirangleFound %lurn" , millis() -  timeTirangleFound);
                        if( millis()-timeTirangleFound > 4000) { // Rotation time needed minimum 4Seconds to reach other side of triangle
                            debug->printf("Cross Lawn Activatedrn");
                            bb.flagFollowLine = false;
                            bb.flagGoHome = true;
                            flagTriangleFound = false;
                        } else {
                            flagTriangleFound = false;
                        }
                    }
                }
            }
        }

        //debug->printf("trans: %lurn",millis()-bb.perimeterSenoren.perimeterLastTransitionTimeB);

        double Output = integral ;

        //debug->printf("p:%f i%f d:%f o:%frn",Kp * error , integral, Kd * derivate, Output);

        if(Output > bb.LINEFOLLOW_SPEED_LOW) Output = bb.LINEFOLLOW_SPEED_LOW;
        if(Output < -1*bb.LINEFOLLOW_SPEED_LOW) Output = -1*bb.LINEFOLLOW_SPEED_LOW;


        bb.cruiseSpeed = bb.LINEFOLLOW_SPEED_LOW;
        bb.driveDirection = DD_REVERSE_LINE_FOLLOW;

        if(error>0.0f) { //Set Speed Inside Perimeter and Check for Triangle if more than 3.5 seconds inside.
            if ( (millis()-lastTransitionTime) > 3500 ) { // If more than 3.5sec inside rotate full
                bb.cruiseSpeed = 15;
                bb.driveDirection = DD_ROTATECW;
                bb.motor.L->setSpeed(15);
                bb.motor.R->setSpeed(-15);

                // Check for triangle
                // If the coil is more than 3.5seconds inside perimeter, then we found a sharp corner
                // If there was a left turn in the last 5 seconds, we found a triangel
                if(flagLeftTurnFound) {
                    if (millis() -  timeLeftTurnFound < 5000) {
                        debug->printf("TRIANGLE FOUND deltaTime%lurn" , millis() -  timeLeftTurnFound);
                        timeTirangleFound = millis();
                        flagTriangleFound = true;
                        flagLeftTurnFound = false;
                    } else {
                        flagTriangleFound = false;
                        flagLeftTurnFound = false;
                    }
                }


            } else if ( (millis()-lastTransitionTime) > 2800 ) { // If more than 2.8sec inside rotate more aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed+10));
            } else if ( (millis()-lastTransitionTime) > 2000 ) { // If more than 2sec inside rotate aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed+5));
            } else {
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed));
            }

        } else { //Set Speed Outside Perimeter

            if ( (millis()-lastTransitionTime) > 2000 ) { // // If more than 2sec outside rotate full
                bb.cruiseSpeed = 15;
                bb.driveDirection = DD_ROTATECC;
                bb.motor.L->setSpeed(-15);
                bb.motor.R->setSpeed(15);
            } else if ( (millis()-lastTransitionTime) > 1500 ) { // If more than 1.5sec outside rotate more aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+7));
                bb.motor.R->setSpeed(-(-15));
            } else if ( (millis()-lastTransitionTime) > 1000 ) { // If more than 1sec outside rotate aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+7));
                bb.motor.R->setSpeed(-(-8));
            } else {
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+10)); //5
                bb.motor.R->setSpeed(-(bb.cruiseSpeed -25)); //30

            }
        }

        return BH_RUNNING;
    }
};


Wenn ich mir den Stiga angucke, macht er dies noch etwas anders. Er scheint auf jeden Fall die zweite Linkskurve nach dem Dreieck auch noch mitzunehmen.

Hier nochmal ein video zur Erklärung der Features.
https://www.youtube.com/watch?v=7SvMgdG70TQ#t=220.322655
 
Habe meinen one bounce Perimetertracking Algorithmus an mein neues Chassis angepasst.
Bei dem Originalchassis bin ich mit der hinteren Spule immer von innen an den Perimeter gebounced.
Mit dem neuen Chassis verwende ich nun die vordere rechte Spule und bounce von außen nach innen.
Anbei ein Video über die Stabilität des Algorithmus.

https://youtu.be/RdufyzkYSPU
 
Du musst dich einloggen oder registrieren, um hier zu antworten.
Oben