Lineare Bewegung

[raze92]

So wie ich es verstanden bestimmt man bei PTP nur die Gelenktstellungen des Ziel uns alle Gelenkte fahren dann auf dem kürzesten Weg(wenn keine Kollision entsteht) zu ihren Sollstellungen.

Seite 20, Kapitel 3.2
https://homepages.thm.de/~hg6458/Robotik/Robotik.pdf

[HaWe]

die Abb. 3.2 und 3.3 zu PTP
(asynchron und synchron, mit dem nicht zu vernachlässigenden Hinweis "Damit ist die PTP-Steuerung für alle Bewegungen ungeeignet, bei denen es auf eine genaue Bahnführung ankommt. Beispiele dafür sind Schweißen, Montage, Kleben, Falzen, Entgraten usw.")
und die Abb 3.4 zu CP
mit den entsprechenden Erklärungen machen doch eigentlich die Sache sehr klar, oder habe ich deine Frage vlt nicht recht verstanden?

- - - Aktualisiert - - -
(edit, OP-post wurde zwischenzeitlich gelöscht)

zunächst ist das ganze in deinem pdf nur ein Modell zur Erklärung, nicht zur praktischen Implementierung.

Praktisch wird man Geschwindigkeiten und Streckenpunkte idealerweise durch unabhängige Setpoints per PID-Steuerung implementieren, mit Rampup- und Rampdown-Funktionen für Anfang und Ende und konstanten Geschwindigkeits-Setpoints für die Strecke dazwischen.

Wie man das richtig programmiert, ist eine ziemliche Herausforderung, schon alleine die korrekte Zielansteuerung ohne Geschwindigkeitssteuerung "in der Mitte" ist in C einigermaßen unübersichtlich, wenn man Oszillationen und Überschwingen und Verrecken unter Last kurz vor dem Ziel vermeiden will:

//*************************************************************//*************************************************************
//                      motor API                              //                      motor API
//*************************************************************//*************************************************************

#define  MAXMOTORS          10    // maximum number of motors
#define  MAXMOTORSLOCAL      2    // maximum number of local motors
#define  MAXPWMRANGE       255    // maximum software-pwm range (0-255)

// motor control structure

typedef struct  {   
                // electrical motor pins
      uint8_t   pind1, pind2, pinpwm;    // dir + pwm L293 H-Bridge type
      uint8_t   pinQa, pinQb;            // rotary enc pins Qa,Qb
     
                // pwm and encoder values
      int16_t   dirpwm;     
      int32_t   motenc, oldenc;          // rotary encoder values
     
                // PID   
      pthread_t tid;
   
                // PID custom target values
      int32_t   target;                  // set target
      int16_t   tarpwm;                  // motor target speed
                // PID custom regulation parameters
      double    P;                       // P: basic propotional to error
      double    I;                       // I: integral: avoid perish
      double    D;                       // D: derivative: avoid oscillating
      double    precis;                  // error precision to target
      int32_t   regtime;                 // PID loop time
      double    damp;                    // damp the integral memory
      int8_t    cont;                    // target: continue or hit once
                // internal control variables
      int16_t   runstate;                // monitors runstate
      int16_t   outp;                    // PID control output value
      int16_t   maxout;                  // max output (max motor pwr)
      int32_t   read;                    // current sensor reading
      double    err;                     // current error
      double    integr;                  // integral of errors
      double    speed;                   // current speed
      int8_t    stopPIDcontrol;          // flag for external termination
     
} tEncMotor;


tEncMotor motor[MAXMOTORS];


#define motorLeft  motor[0]
#define motorRight motor[1]



// motor runstates:

#define OUT_REGSTATE_NULL           0
#define OUT_REGSTATE_COAST          2
#define OUT_REGSTATE_BRAKE          3
#define OUT_REGSTATE_EMERG_STOP     5

#define OUT_REGSTATE_ON             8
#define OUT_REGSTATE_RAMPUP         9
#define OUT_REGSTATE_RAMPDOWN      10

#define OUT_REGSTATE_PIDIDLE       15

#define OUT_REGSTATE_ACTIVE        16 
#define OUT_REGSTATE_PIDSTART      17
#define OUT_REGSTATE_PIDEND        18
#define OUT_REGSTATE_PIDHOLD       19
#define OUT_REGSTATE_PIDHOLDCLOSE  20





//*************************************************************

#define motorCoast(nr) motorOn(nr, 0)  // alias for motor coast

//*************************************************************



inline void motorBrake(uint nr, int dirpwm) {      // brake by pwm power
   int pwm;
   
   pwm = abs(dirpwm);
   
   digitalWrite(motor[nr].pind1, HIGH);
   digitalWrite(motor[nr].pind2, HIGH);     
   
   motor[nr].dirpwm = pwm;
   softPwmWrite(motor[nr].pinpwm, pwm);    // brake power always > 0   
   
}

//*************************************************************

inline void motorOn(uint nr, int dirpwm) { // motor On (nr, dir_pwm)
   int dir, pwm;                             // signed pwm:
   
   if(dirpwm > 0) dir = +1;                   // pwm > 0: forward
   else if(dirpwm < 0) dir = -1;              // pwm < 0: reverse
   else dir = 0;                              // pwm = 0: coast
   
   if(! _REMOTE_OK_) dirpwm=0;
   pwm = abs(dirpwm);
   
     
   if(dir> 0) {
      digitalWrite( motor[nr].pind1, HIGH);
      digitalWrite( motor[nr].pind2, LOW);     
   }
   else
   if(dir==0) {
      digitalWrite( motor[nr].pind1, LOW);
      digitalWrite( motor[nr].pind2, LOW);
   }
   else {
      digitalWrite( motor[nr].pind1, LOW);
      digitalWrite( motor[nr].pind2, HIGH);
   }
   motor[nr].dirpwm = dirpwm;
   softPwmWrite( motor[nr].pinpwm, pwm);
   
   
}

                       
                       
//*************************************************************//*************************************************************
//                       PID control                           //                       PID control
//*************************************************************//*************************************************************   

// forward: motor API functions
inline void RotatePIDtoTarget (uint nr, int32_t Target, double RotatSpeed); // approach absolute target once
inline void RotatePIDdegrees  (uint nr, int32_t Target, double RotatSpeed); // turn relative degrees
inline void RotatePIDcontinue (uint nr, int32_t Target, double RotatSpeed); // approach target continuously
inline void StopPIDcontrol    (uint nr);               
inline void PIDinit(); // P=0.4, I=0.4, D=10.0
// simple customized PID setting:
inline void SetPIDparam(uint nr, double P,double I,double D);
// extended customized parameter setting:
inline void SetPIDparamEx(uint nr, double P, double I, double D, double prec, int16_t regtime, double damp);   


//*************************************************************                   
                       
inline void PIDcleanup(uint nr) {   
        motorCoast(nr);
        motor[nr].runstate = OUT_REGSTATE_NULL;
        motor[nr].speed    = 0;
        motor[nr].outp     = 0;
        motor[nr].cont     = 0;
}                       
 

//*************************************************************                   

void * PID_calc(void *arg) {
  double  aspeed, damp, PWMpwr, readold, errorold, tprop;
  int32_t    readstart, cmax, cmin;                    // for monitoring
  int32_t    starttime, runtime, clock, dtime;         // timer
  int     regloop;

 
  // arg nach index casten
  unsigned nr = (unsigned)arg;
 
 
  motor[nr].runstate = OUT_REGSTATE_PIDSTART   ;           // reg state: RAMPUP
  motor[nr].read     = motor[nr].motenc;             // get current encoder reading
  motor[nr].err      = motor[nr].target - motor[nr].read; // error to target

  readstart      = motor[nr].read;
  regloop        = 1;


  damp=0;                                   // damp the integral memory

  starttime= millis();


  // appoach target
  _Astart:
  motor[nr].runstate = OUT_REGSTATE_ACTIVE;  // run state: RUNNING

  do {
     
    pthread_testcancel();
    if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
    } 

    dtime    = millis() - clock;
    clock    = millis();
    runtime  = clock - starttime;
    tprop    = dtime/20.0;
   
   
    if ((motor[nr].err==errorold)&& (abs(motor[nr].err)>motor[nr].precis)) damp=1;    // stalling
    else
    damp=motor[nr].damp;

    motor[nr].integr = (damp * motor[nr].integr) + motor[nr].err;

    if((motor[nr].integr) > 3*motor[nr].maxout) motor[nr].integr = 3*motor[nr].maxout; // cut away
    else
    if((motor[nr].integr) <-3*motor[nr].maxout) motor[nr].integr =-3*motor[nr].maxout;

    PWMpwr= (motor[nr].P*motor[nr].err) + (motor[nr].I*motor[nr].integr)*tprop + (motor[nr].D*(motor[nr].err-errorold))/tprop;


    if(PWMpwr >  motor[nr].maxout) PWMpwr=  motor[nr].maxout;   // forward maxout
    else
    if(PWMpwr < -motor[nr].maxout) PWMpwr= -motor[nr].maxout;   // reverse maxout


    motor[nr].speed= (motor[nr].read-readold)*100/dtime;  // rotat speed [degrees/100ms]
    aspeed = abs(motor[nr].speed) ;
       
    if (abs(PWMpwr) > motor[nr].tarpwm )  {
        PWMpwr = sign(PWMpwr) * motor[nr].tarpwm ;
    }

    motor[nr].outp = round(PWMpwr);


    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                      // PID regulation !
    motorOn(nr, motor[nr].outp);                  // action !
    delay(motor[nr].regtime);                       // wait regulation time
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    readold     = motor[nr].read;                           // save old sensor
    errorold    = motor[nr].err;                            // save old error

    motor[nr].read  = motor[nr].motenc;                   // get new encoder value
    motor[nr].err   = motor[nr].target-motor[nr].read;  // new error to target

    if (motor[nr].read>cmax) cmax=motor[nr].read;         // monitor overshooting
    else
    if (motor[nr].read<cmin) cmin=motor[nr].read;         // monitor overshooting

    if ((motor[nr].cont)&& (abs(motor[nr].err)<=motor[nr].precis))
         motor[nr].runstate = OUT_REGSTATE_PIDSTART   ;
    else motor[nr].runstate = OUT_REGSTATE_ACTIVE;

    if (motor[nr].cont) continue;
    if (abs(motor[nr].err)<=motor[nr].precis) {
        regloop +=1 ;
        motor[nr].runstate = OUT_REGSTATE_PIDEND  ;
    }
   
    if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
    }


  } while ((abs(motor[nr].err)>=motor[nr].precis) && (regloop<=5));  // target reached

  motorCoast(nr);                                 // finished - stop motor
  motor[nr].runstate = OUT_REGSTATE_PIDEND;       // run state: RAMPDOWN
  motor[nr].outp=0;

  delay(50);
  motor[nr].read = motor[nr].motenc;
  regloop=1;

  if (motor[nr].read>cmax) cmax=motor[nr].read;            // detect overshooting
  if (motor[nr].read<cmin) cmin=motor[nr].read;            // detect overshooting
  motor[nr].err = motor[nr].target-motor[nr].read;


  if (motor[nr].stopPIDcontrol) {
        PIDcleanup(nr);
        return NULL;
  }

  if ((abs(motor[nr].err)>motor[nr].precis))  {goto _Astart;}

  motor[nr].runstate=0;
  delay(1);                                //runstate = IDLE

  return (NULL);
}



//*************************************************************

inline void RotatePID(uint nr, int32_t Target, double RotatSpeed, int8_t cont) {
   
    if( !motor[nr].tid && motor[nr].runstate) {
        motor[nr].runstate=0; // repair
        delay(1);
    }
    if( ( motor[nr].runstate ) || ( motor[nr].tid ) ) {
       motor[nr].stopPIDcontrol = 1;
       delay(1);
       while( motor[nr].runstate || motor[nr].tid );  // wait for PID task to terminate
       delay(1);
       PIDcleanup(nr);
    }
    // init new PID structure 
    motor[nr].runstate = 1;               // set runstate: PID active
    // custom init PID [nr]   
    motor[nr].target = Target;                   // assign target
    motor[nr].tarpwm = abs(RotatSpeed);          // assign max rotation speed
    motor[nr].cont=cont;                         // cont vs. hit once
    // Reset PID control defaults
    motor[nr].outp    = 0;                // PID control output value
    motor[nr].maxout  = MAXPWMRANGE;      // absolute max possible output (max pwr)
    motor[nr].read    = 0;                // current reading
    motor[nr].err     = 0;                // current error
    motor[nr].integr  = 0;                // integral of errors
    motor[nr].speed   = 0;                // current speed
    motor[nr].stopPIDcontrol = 0;         // flag for external termination
   
    // start PID control task
    pthread_create( & motor[nr].tid,      // id speichern in dem dazugehörigen Array-Element
                    0,                    // Default Attribute (threads cancelbar)
                    PID_calc,             // Name der PID-Kalkulationsroutinge
                    (void *) nr);         // der Index des Array-Elements für eine PID Struktur,
                                          // die mit dem Motorindex gleich ist.   
   
    // run pthread task in detached mode, auto-cleanup                                   
    pthread_detach(motor[nr].tid);

}

//*************************************************************                   

inline void StopPIDcontrol (uint nr) {

  if (motor[nr].tid) { // stop PID task if already running
     motor[nr].stopPIDcontrol = 1;   
  }

}
               
//*************************************************************                         
                     
// custom PID parameters Extended
inline void SetPIDparamEx(uint nr, double P, double I, double D, double prec, int16_t regtime, double damp) {
    motor[nr].P       = P;             // P: propotional to error
    motor[nr].I       = I;             // I: avoid perish
    motor[nr].D       = D;             // D: derivative: avoid oscillating
    motor[nr].precis  = prec;          // error precision to target
    motor[nr].regtime = regtime;       // PID regulation time
    motor[nr].damp    = damp;          // damp error integral
}


//************************************************************* 
                 
// custom PID parameters
inline void SetPIDparam(uint nr, double P, double I, double D) { 
    motor[nr].P     = P;             // P: propotional to error
    motor[nr].I     = I;             // I: integral: avoid perish
    motor[nr].D     = D;             // D: derivative: avoid oscillating
}                       
   
                   
//*************************************************************                                       
                     
inline void PIDinit() {
  for (uint nr=0; nr < MAXMOTORS; ++nr) {
    SetPIDparamEx(nr, 0.40, 0.40, 10.0, 1.0, 5, 0.75); // p,i,d, precis, reg_time, damp
    delay(1);
  }
}

//*************************************************************                                       


inline void RotatePIDtoTarget(uint nr, int32_t Target, double RotatSpeed) {
   RotatePID(nr, Target, RotatSpeed, false);
   delay(1);
}

//*************************************************************                                       

inline void RotatePIDcont(uint nr, int32_t Target, double RotatSpeed) {
   RotatePID(nr, Target, RotatSpeed, true);
   delay(1);
}

//*************************************************************                                       

inline void RotatePIDdegrees(uint nr, int32_t Target, double RotatSpeed)  {
   RotatePID(nr, Target+motor[nr].motenc, RotatSpeed, false);
   delay(1);
}


//*************************************************************
//  ^^^^^^^^^^^^^^ PID end ^^^^^^^^^^^^^
//*************************************************************

[raze92]

Ich habe PTP und LIN seperat betrachtet. Für beide Bewegungen erzeuge ich eine Tabelle die die Gelenkstellungen beinhaltet für die zu fahrende Bahn.

Ich übergebe der Gelenksteuerung die Tabelle mit den Gelenkstellungen mit zusätzlich den Geschwindigkeiten jedes Gelenks an jedem Zwischenpunkt. Die Gelenke sollen dann mit der Geschwindigkeit fahren, bis die Soll-Gelenkstellung erreicht wurde. Dann wird die nächste Geschwindigkeit an die Gelenke gesendet usw.
Da kam ich aber jetzt auf den Schluss, dass mit diesem vorgehen eine Bahn schnell abweichen kann und nicht mehr in die Bahn zurück kommt.

Wenn ich jetzt stattdessen eine Interpolation durchführe der Linearen Bahn und mit den 3 Phasen das Geschwindigkeitsprofil der Bahn bestimme und jeden Interpolationspunkt nun über eine synchrone PTP-Steuerung übergebe:
Wie Regel ich die Geschwindigkeit in der PTP-Steuerung des TCP?

[HaWe]

das habe ich doch gerade erklärt: über eine PID-Steuerung.
Ich habe den Eindruck, du hast überhaupt keine praktische Erfahrung, und aus einem rein theoretischen Ansatz heraus wirst du hier nicht weiterkommen, fürchte ich.

Hast du überhaupt jemals praktisch einen Roboterarm programmiert?
Wenn ja, welchen (Hardware) und mit welcher Programmiersprache (Software)?

Wenn du tatsächlich noch keine praktische Erfahrung hast: Kauf dir mal einen Arduino Mega und einen Arduino Braccio Roboterarm, und dann fang einfach mal an.

[raze92]

Deine Antwort hab ich erst danach gelesen

das habe ich doch gerade erklärt: über eine PID-Steuerung.
Ich habe den Eindruck, du hast überhaupt keine praktische Erfahrung, und aus einem rein theoretsichen Ansatz heraus wirst du hier nicht weiterkommen, fürchte ich.

Hast du überhaupt jemals praktisch einen Roboterarm programmiert?
Wenn ja, welchen (Hardware) und mit welcher Programmiersprache (Software)?

Nein hab ich leider noch nicht. Das Projekt ist eine Aufgabe in der Firma wo ich momentan tätig bin, dort soll ich erstmal alles Theoretisch erarbeiten.

[i_make_it]

Wie Regel ich die Geschwindigkeit in der PTP-Steuerung des TCP?

Hatten wir schon mal.
Motorregler für NC-Achsen haben Positionsregler, nachgeschaltete Geschwindigkeitsregler und nachgeschaltete Stromregler.

Dein Positionsregler hat eine Zykluszeit z.B. regelt er alle 1ms.
Dann hast du einen Istzustand (Istposition) und den Sollzustand (Sollposition) der innerhalb der Zykluszeit erreicht werden soll.
Wir haben also eine Zeit (Zykluszeit 1ms) und eine Strecke (sollposition minus Istposition)
Geschwindigkeit ist Weg pro Zeiteinheit.
Sprich dein Positionsregler liefert Dir den Sollwert für Deinen Geschwindigkeitsregler und dein Meßsystem (Tachogenerator, Resolver, Drehgeber) liefert Dir den Istwert.
Der Stromregler sorgt dann dafür das auch das notwendige Drehmoment da ist.

wenn man Oszillationen und Überschwingen und Verrecken unter Last kurz vor dem Ziel vermeiden will:

Um das zu vermeiden.

PID-Steuerung

Ist meiner Meinung Nach eine unglückliche Wortwahl, da es nicht um eine Steuerung (Steuerstrecke ohne Feedback) geht sondern um eine Regelung (Regelkreis mit Soll-Ist vergleich und entsprechnder Nachführung) geht.
Eine Steuerhardware oder eine Steuersoftware die PID Regelung enthält, wird in Marketingsprache allerdings mittlerweile gerne als PID-Steuerung angepriesen.

Interessant wäre jetzt zu erfahren wie Dein Wissensstand bei Steuer- und Regelungstechnik ist.
Ein Fachbuch zum Thema wäre z.B. "Elementare Regelungstechnik".
Ein Buch mit dem man direkt in die Softwareimplementierung von Reglern starten kann, wäre "Praxisbuch Regelungstechnik" (von dem es allerdings anscheinend keine Neuaflage gibt)

[raze92]

Ich habe beschlossen für unsere Anwendung die Gelenkstellungen vorher zu berechnen und nicht zur laufzeit.

Dein Positionsregler hat eine Zykluszeit z.B. regelt er alle 1ms.
Dann hast du einen Istzustand (Istposition) und den Sollzustand (Sollposition) der innerhalb der Zykluszeit erreicht werden soll.
Wir haben also eine Zeit (Zykluszeit 1ms) und eine Strecke (sollposition minus Istposition)
Geschwindigkeit ist Weg pro Zeiteinheit.
Sprich dein Positionsregler liefert Dir den Sollwert für Deinen Geschwindigkeitsregler und dein Meßsystem (Tachogenerator, Resolver, Drehgeber) liefert Dir den Istwert.
Der Stromregler sorgt dann dafür das auch das notwendige Drehmoment da ist.

Wenn ich es dann über die Zykluszeit berechne, dann muss ja gegeben sein, dass die Interpolationspunkte den selben Abstand haben. Gedacht war, dass in der Beschleunigungsphase und Bremsphase die Interpolationsdichte größer ist.
Ich möchte die Regelung Softwaretechnisch lösen. Bestimme ich dann die Zykluszeit bei der Interpolation und übergebe diese zusammen mit den Rücktransformierten Gelenkstellungen? Dann könnte ich die nötige Geschwindigkeit berechnen und Fahren. Und um das Phasenproblem zu lösen übergebe ich 3, bzw 2, Zykluszeiten für die Phasen.

Kenntnisse zu Regelungstechnik sind vorhanden.

[i_make_it]

Wenn ich es dann über die Zykluszeit berechne, dann muss ja gegeben sein, dass die Interpolationspunkte den selben Abstand haben.

Hä?????????????????
Bei gleich großem zeitlichem Delta, (z.B. 1ms) kann der Abstand der Interpolationspunkte gar nicht immer gleich sein.
Bei einer Geschwindigkeit von 0mm/s ist der Abstand z.B. 0mm.
Bei 500mm/s ist der Abstand 0,5mm.
Bei gleicher Zeit ist der Abstand immer Proportional zur Geschwindigkeit.

Sorry wenn ich bei anscheinend so grundlegenden Verständnissproblemen der physikalischen Zusammenhänge, den Kenntniss Stand (oder besser den Verständniss Stand) zur Regelungstechnik als nicht besonders hoch einschätze.

Ich kann Deinen Wissensstand halt nur anhand dessen einschätzen was ich hier von Dir lese. Und das wirkt halt grade nicht so.

Ich denke am besten wäre wenn Du erst einmal anhand des Vorlesungsscripts das Du gepostet hast Deine Eingangsfragestellung betrachtest und Dann noch mal neu formulierts wo du ein Verständnissproblem hast.

[raze92]

Ach jaaa .

Sorry wenn ich bei anscheinend so grundlegenden Verständnissproblemen der physikalischen Zusammenhänge, den Kenntniss Stand (oder besser den Verständniss Stand) zur Regelungstechnik als nicht besonders hoch einschätze.

Ich kann Deinen Wissensstand halt nur anhand dessen einschätzen was ich hier von Dir lese. Und das wirkt halt grade nicht so.

Kein problem. Brauche auch langsam eine Pause von der Theorie.

Dann so:
Ich lege eine Zykluszeit fest. Anhand dieser setze ich die Interpolationspunkte. Ja das hatten wir schon.

Ich bedanke mich vielmals für die Mühe, mir die ganzen Fragen zu beantworten.

[i_make_it]

Wenn Du Dich mit der ganzen Theorie grade anfängst im Kreis zu drehen und vor lauter Bäumen keinen Wald mehr siehst, dann gehe hin und bastel Dir Zur Not aus Pappe ein kleines Model mit Winkelskalen und als Hilfsmittel mal ein Lineal oder ein Stück von einer Pringlesdose (oder was anderes) dann kannst Du ganz praktisch Graden und Kreisinterpolation und deren Auswirkungen auf die Gelenkstellungen durchspielen und ggf. auch mit konkreten Zahlenwerten durcharbeiten.

Es hilft oft simplifizierte, praktische Versuche zu machen um ein Verständniss zu entwickeln oder sich aus einer selbst gegrabenen gedanklichen Grube wieder zu befreien.

Bei so einem Modellarm kann man auch mit etwas Schnur und einem Pappstück das man festklemmt, schnell Anschläge für die Gelenk Start- und Endstellungen herstellen und sich dann einfach mal ansehen, wie sich die PTP-Bewegung von der Bahnbewegung, für die selben gegebenen Start- und Endpositionen unterscheidet.