Problem mit PID-Regler

[mattsarz]

Hi,

Ich habe für ein Linienfolger Programm einen PID Regler verwendet (von http://www.henkessoft.de/Roboter/ASURO.htm), dieser Funktionierte auch wunderbar und war viel schneller und Präziser als ein einfacher P-Regler.


/************************************************** *****************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Autor: Waste 26.8.05, Verändert durch: ehenkes 26.05.2007
*
************************************************** ***************************/

#include "asuro.h"
#include <stdlib.h>

#define SPEED 80
#define SPEEDMAX 200
#define SPEEDMIN 30
#define IMAX 16000
#define IMAXALARM 15000

unsigned char speed, j;
int speedLeft,speedRight;
unsigned int lineData[2];
int x, xalt, kp, kd, ki, yp, yd, yi, drest=0, y, y2, isum=0, ADOffset;

void FollowLine (void)
{
unsigned char leftDir = FWD, rightDir = FWD;

LineData(lineData);
x = (lineData[LEFT] - lineData[RIGHT]) - ADOffset;

yp = x*kp; // P-Anteil

isum += x;
if (isum > IMAX) isum = IMAX;
if (isum < -IMAX) isum = -IMAX;
yi = isum / 625 * ki; //I-Anteil

yd = (x - xalt) * kd; // D-Anteil
yd += drest;
if (yd > 255)
{
drest = yd - 255;
}
else if (yd < -255)
{
drest = yd + 255;
}
else
{
drest = 0;
}

if (isum > IMAXALARM) BackLED(OFF,ON);
else if (isum < -IMAXALARM) BackLED(ON,OFF);
else BackLED(OFF,OFF);

y = yp + yi + yd; // PID
y2 = y / 2;
xalt = x;

speedLeft = speedRight = speed;
MotorDir(FWD,FWD);

if ( y > 0)
{
StatusLED(GREEN);
speedLeft = speed + y2;
if (speedLeft > SPEEDMAX)
{
speedLeft = SPEEDMAX;
y2 = speedLeft - speed;
}
y = y - y2;
speedRight = speed - y;
if (speedRight < SPEEDMIN)
{
speedRight = SPEEDMIN;
}
}

if ( y < 0)
{
StatusLED(RED);
speedRight = speed - y2;
if (speedRight > SPEEDMAX)
{
speedRight = SPEEDMAX;
y2 = speed - speedRight;
}
y = y - y2;
speedLeft = speed + y;
if (speedLeft < SPEEDMIN)
{
speedLeft = SPEEDMIN;
}
}
leftDir = rightDir = FWD;
if (speedLeft < SPEEDMIN + 5) leftDir = BREAK;
if (speedRight < SPEEDMIN + 5) rightDir = BREAK;
MotorDir(leftDir,rightDir);
MotorSpeed(abs(speedLeft),abs(speedRight));
}

int main(void)
{
Init();
FrontLED(ON);
for (j = 0; j < 255; j++) LineData(lineData);
LineData(lineData);
ADOffset = lineData[LEFT] - lineData[RIGHT]; // Helligkeitsunterschied links und rechts

MotorDir(FWD,FWD);
StatusLED(GREEN);

speed = SPEED;
speedLeft = speed;
speedRight = speed;

kp = 10; ki = 4; kd = 70; // Regler Parameter kd enthält bereits Division durch dt
/* 10(!) 4(!) 70(!) */ /*<=== gute Ausgangswerte*/

while(1)
{
FollowLine();
}
return 0;
}



Um aber flexibler zu sein habe ich meinen Asuro ein wenig umgebaut:


----------------------------------------------

|_|''''''''''''''''''''''''''''''|_|'''''''''''''' '''''''''''''''|_|

(schematische Ansicht von vorne; linker Fototransistor, FrontLED, rechter Fototransistor)

Den Code habe ich nun folgendermassen angepasst:


#include "asuro.h"
#include <stdlib.h>

#define SPEED 240
#define SPEEDMAX 255
#define SPEEDMIN 80
#define IMAX 16000
#define IMAXALARM 15000

unsigned char speed, j;
int speedLeft,speedRight;
unsigned int lineData[2];
int x, xalt, kp, kd, ki, yp, yd, yi, drest=0, y, y2, isum=0, ADOffset;

void FollowLine (void)
{
unsigned char leftDir = FWD, rightDir = FWD;

LineData(lineData);
x = (lineData[LEFT] - lineData[RIGHT]) - ADOffset;

yp = x*kp; // P-Anteil

isum += x;
if (isum > IMAX) isum = IMAX;
if (isum < -IMAX) isum = -IMAX;
yi = isum / 625 * ki; //I-Anteil

yd = (x - xalt) * kd; // D-Anteil
yd += drest;
if (yd > 255)
{
drest = yd - 255;
}
else if (yd < -255)
{
drest = yd + 255;
}
else
{
drest = 0;
}

if (isum > IMAXALARM) BackLED(OFF,ON);
else if (isum < -IMAXALARM) BackLED(ON,OFF);
else BackLED(OFF,OFF);

y = yp + yi + yd; // PID
y2 = y / 2;
xalt = x;

speedLeft = speedRight = speed;
MotorDir(RWD,RWD);

if ( y < 0)
{
StatusLED(GREEN);
speedLeft = speed + y2;
if (speedLeft > SPEEDMAX)
{
speedLeft = SPEEDMAX;
y2 = speedLeft - speed;
}
y = y - y2;
if (speedRight < SPEEDMIN)
{
speedRight = SPEEDMIN;
}
}

if ( y > 0)
{
StatusLED(RED);
speedRight = speed - y2;
if (speedRight > SPEEDMAX)
{
speedRight = SPEEDMAX;
y2 = speed - speedRight;
}
y = y - y2;
speedLeft = speed + y;
if (speedLeft < SPEEDMIN)
{
speedLeft = SPEEDMIN;
}
}
leftDir = rightDir = RWD;
if (speedLeft < SPEEDMIN + 5) leftDir = BREAK;
if (speedRight < SPEEDMIN + 5) rightDir = BREAK;
MotorDir(leftDir,rightDir);
MotorSpeed(abs(speedLeft),abs(speedRight));
}

int main(void)
{
Init();
FrontLED(ON);
for (j = 0; j < 255; j++)
{
LineData(lineData);
}
LineData(lineData);
ADOffset = lineData[LEFT] - lineData[RIGHT]; // Helligkeitsunterschied links und rechts

MotorDir(RWD,RWD);
StatusLED(GREEN);

speed = SPEED;
speedLeft = speed;
speedRight = speed;

kp = 10; ki = 4; kd = 70; // Regler Parameter kd enthält bereits Division durch dt


while(1)
{
FollowLine();
}
return 0;
}

Als wichtigste Änderung habe ich die Umkehrung der > und < Operatoren betrachtet.

Nun fährt der Asuro aber nur noch sehr langsam der Linie nach...
Kann mir jemand einen Tipp geben, wie ich die Geschwindigkeit wieder steigern kann?

Gruss,
Matt

Edit:
Bilder hinzugefügt

[Sternthaler]

Hallo mattsarz
willkommen im Forum.

Sag mal wie breit ist denn deine Linie, auf der du fährst?
Bzw: Was hat dich bewogen die Sensoren so weit auseinander zu legen?

Zu deinem Umbau im Programm (< > getauscht):
Interessant. Ich vermute, dass du auf einer weißen Linie auf sonst schwarzem Grund fährst. Aber warum der Asuro deshalb nun langsamer sein soll, ist mir dann auch ein Rätzel.
Sind sonst noch Änderungen im Programm? Ich finde nur die 2 Stellen:
"if ( y > 0)" in FollowLine().

[mattsarz]

Hi Sternthaler,

danke für deine Antwort!
Ich benütze schwarzes Tesaband für die Linie (3,5 cm breit). Das Tesaband passt ziemlich genau zwischen die Sensoren. Somit fährt der Asuro geradeaus, wenn er weiss, weiss "sieht".
Diesen Umbau habe ich vollzogen um die linienfolge allgemein gültiger zu machen... Denn bei einer 90° kurve merkt der Asuro nun selbst, in welche Richtung er fahren soll.

Die grösser als zeichen sind bis jetzt wirklich die einzigen Veränderungen im Programm... ich hoffe du kannst mir evt. sagen, was ich sonst noch ändern müsste.

MfG,
Matt

[recycle]

Nun fährt der Asuro aber nur noch sehr langsam der Linie nach...
Kann mir jemand einen Tipp geben, wie ich die Geschwindigkeit wieder steigern kann?

Ich kenne das Programm nicht und verstehe auch kein C.


Bei einem PID Regler würde ich aber mal vermuten, dass du bei einem Umbau auch die Regelung oder mindestens die Faktoren kp, kd und ki auf deinen Umbau anpassen musst.

Durch deine sehr breite Linie und die anders angeordneten Sensoren dürfte deine Regelung es mit anderen Abweichungen vom Sollwert zu tun bekommen als im Originalaufbau.
Da die Geschwindigkeit mit der auf eine Abweichung reagiert wird und die Geschwindigkeit mit der dein Roboter vorwärts kommt voneinander abhängen dürften sich falsche Parameter der Regelung auch auf die Fahrtgeschwindigkeit auswirken.

[damaltor]

.... warum sollte der asuro bei schmalen linien nicht wissen in welche richtung er fahren muss? wenn unter einem sensor schwarz wird, muss er in die richtung drehen. das ist bei schmalen linien genauso wie bei hellen...

[Sternthaler]

Hallo mattsarz
mmmhhh, im Moment kann ich mit der Farbe der Linie nicht so richtig folgen. (Ist nun mein Regler kaputt?)
Das letzte Bild von dir hat bei meinem Monitor fast genau ein 1:1-Verhältnis mit dem Asuro in meiner Hand. Da sind dann aber die Liniensensoren auf dem Bild ca. 7 cm entfernt. Dein Tesaband hat 3,5 cm.
Mach doch mal bitte ein Bild mit dem Asuro auf der Linie.

Um aber mal eine Gegenprobe, so auf die schnelle, zu machen.
Dreh doch mal die < > wieder im Programm um. Fährt der Asuro dann wieder schneller? Und vor allem: Bleibt er auf der Linie?
Sonst bin ich erst einmal sprachlos.

Oder ist die Geschwindigkeitsänderung erst mit dem Hardwareumbau gekommen? Dann hat recycle ja schon einen Hinweis auf die PID-Reglerparameter gegeben.

Erst einmal Gute Nacht.

[mattsarz]

Das mit den Reglerwerten habe ich schon zur genüge ausgetestet.. Ich frage mich nur, ob ich nicht etwas vergessen habe. Ich habe schon wie ein verrückter gesucht, bin aber noch nicht fündig geworden.
Anbei liefere ich euch noch ein paar bilder mit, mit denen ihr hoffentlich meine situation nachvollziehen könnt.

Durch meinen Umbau ist der Spielraum der breite des Tesafilms grösser, da sich die Fototransistoren nun nicht mehr auf, sondern neben dem Klebeband befinden..
Die von mir beschriebene Situation (die 90 grad kurve), seht ihr auf meinen Bildern; dieses Problem ist durch den Umbau gelöst worden.

[recycle]

Ich würde als erstes mal ausprobieren, ob es überhaupt an der Regelung liegt. Erreicht der Asuro denn noch die volle Geschwindigkeit, wenn du ihn ohne Regeung einfach geradaus fahren lässt?

Wenn ja, würde ich das Programm mal so umschreiben, dass die wichtigen Werte der Regelung aufs Terminal ausgegeben werden, während du den Asuro ein bischen über der Linie bewegst.
Dann müsstest du an den Ausgabewerten wenigstens ungefähr sehen können, wo es klemmt.

[damaltor]

hmm aber ich verstehe das problem immer noch nicht... was ist der unterschied zur schmalen linie? bei der sieht der asuro doch genauso ob er links oder rechts muss...

[mattsarz]

Ohne Regelung erreicht er volle Geschwindigkeit...

@damaltor: Ich bin Praktikant an einer Hochschule, hier bin ich im OK der RobOlympics (www.robolympics.ch). Da die Regeln für die Wettbewerbe genau bestimmt sind, ist auch die breite des Klebbands beim Linienfolger vorgegeben.
Ich teste den ASURO nun als Preis.. Daher sollte er auch Teilnahmefähig sein, da die Gewinner nächstes Jahr mit ihm antreten könnten.

So viel erst mal dazu ;)

MfG,
Matt

[mattsarz]

Hi!

Ich hab hier noch einen weiteren Code, der nicht richtig läuft und der für euch wahrscheinlich einfacher nachzuvollziehen ist, da er nichts mit dem Umbau zu tun hat.
Und zwar handelt es sich um einen PID Regler, der mittels Odometrie die Geradeaus-fahrt regeln soll.
Das Programm beruht auf dem zuvor geposteten LinienPID-Regler.
Ich bin gespannt auf eure Vorschläge!

Gruss,
Matt



#include "asuro.h"
#include <stdlib.h>

#define SPEED 240
#define SPEEDMAX 255
#define SPEEDMIN 80
#define IMAX 16000
#define IMAXALARM 15000

unsigned char speed, j;
int speedLeft,speedRight;
unsigned int DrehDataLeft, DrehDataRight;
int x, xalt, kp, kd, ki, yp, yd, yi, drest=0, y, y2, isum=0, ADOffset;





void FollowLine (void)
{
unsigned char leftDir = FWD, rightDir = FWD;

OdoLeft();
OdoRight();
x = (DrehDataLeft - DrehDataRight) - ADOffset;

SerWrite("\n\rBeginn FollowLine(void)... Wert von x: ", 41);
PrintInt(x);

yp = x*kp; // P-Anteil

isum += x;
if (isum > IMAX) isum = IMAX;
if (isum < -IMAX) isum = -IMAX;
yi = isum / 625 * ki; //I-Anteil

yd = (x - xalt) * kd; // D-Anteil
yd += drest;
if (yd > 255)
{
drest = yd - 255;
}
else if (yd < -255)
{
drest = yd + 255;
}
else
{
drest = 0;
}

if (isum > IMAXALARM) BackLED(OFF,ON);
else if (isum < -IMAXALARM) BackLED(ON,OFF);
else BackLED(OFF,OFF);

y = yp + yi + yd; // PID
y2 = y / 2;
xalt = x;

speedLeft = speedRight = speed;
MotorDir(RWD,RWD);

if ( y > 0)
{
StatusLED(GREEN);
speedLeft = speed + y2;
if (speedLeft > SPEEDMAX)
{
speedLeft = SPEEDMAX;
y2 = speedLeft - speed;
}
y = y - y2;
if (speedRight < SPEEDMIN)
{
speedRight = SPEEDMIN;
}
}

if ( y < 0)
{
StatusLED(RED);
speedRight = speed - y2;
if (speedRight > SPEEDMAX)
{
speedRight = SPEEDMAX;
y2 = speed - speedRight;
}
y = y - y2;
speedLeft = speed + y;
if (speedLeft < SPEEDMIN)
{
speedLeft = SPEEDMIN;
}
}
leftDir = rightDir = RWD;
if (speedLeft < SPEEDMIN + 5) leftDir = BREAK;
if (speedRight < SPEEDMIN + 5) rightDir = BREAK;
MotorDir(leftDir,rightDir);
MotorSpeed(abs(speedLeft),abs(speedRight));
}



void OdoLeft(void)
{
unsigned long zeitbeginn, zeitende, zeitdiff;
unsigned int OdoData[2];
int count, l_old;

Init();

SerWrite("\n\rBeginn OdoLeft()", 20);
zeitbeginn = Gettime();
for(count = 0; count < 6;)
{
OdometrieData(OdoData);
if (OdoData[0] > 800 && l_old < 800)
{
count++;
}
l_old = OdoData[0];
}
count = 0;
zeitende = Gettime();
zeitdiff = zeitende - zeitbeginn;
DrehDataLeft = 60000 / zeitdiff;
SerWrite("\n\rEnde OdoLeft(), wert von DrehDataLeft ", 43);
PrintInt(DrehDataLeft);
}



void OdoRight(void)
{
unsigned long zeitbeginn, zeitende, zeitdiff;
unsigned int OdoData[2], drehzahl_right;
int count, r_old;

Init();

SerWrite("\n\rBeginn OdoRight()", 21);
zeitbeginn = Gettime();
for(count = 0; count < 6;)
{
OdometrieData(OdoData);
if (OdoData[1] > 600 && r_old < 600)
{
count++;
}
r_old = OdoData[1];
}
count = 0;
zeitende = Gettime();
zeitdiff = zeitende - zeitbeginn;
DrehDataRight = 60000 / zeitdiff;
SerWrite("\n\rEnde OdoRight(), wert von DrehDataRight ", 43);
PrintInt(DrehDataRight);
}



int main(void)
{
Init();
FrontLED(ON);
MotorDir(RWD,RWD);
MotorSpeed(SPEED,SPEED);
SerWrite("Beginn main(void)", 17);
OdoLeft();
OdoRight();
SerWrite("\n\rDrehDataLeft, DrehDataRight ", 30);
PrintInt(DrehDataLeft);
SerWrite(" ", 1);
PrintInt(DrehDataRight);
OdoLeft();
OdoRight();
ADOffset = DrehDataLeft - DrehDataRight; // Helligkeitsunterschied links und rechts

MotorDir(RWD,RWD);
StatusLED(GREEN);

speed = SPEED;
speedLeft = speed;
speedRight = speed;

kp = 10; ki = 4; kd = 70; // Regler Parameter kd enthält bereits Division durch dt


while(1)
{
FollowLine();
}
return 0;
}

[damaltor]

achso... ich dachte es geht bloss darum dass er eine sschmale linie nicht mehr erkennen würde wenn diese einen scharfen knick macht...

[mattsarz]

Soo... Überraschung ;)
Für alle die's interessiert... Ich habe den Geradeaus PID Regler fertig. Es lag bloss an den überzähligen Init(); aufrufen.

Hier der fertige code:

include "asuro.h"
#include <stdlib.h>

#define SPEED 180
#define SPEEDMAX 255
#define SPEEDMIN 80
#define IMAX 16000
#define IMAXALARM 15000

unsigned char speed, j;
int speedLeft,speedRight;
unsigned int DrehDataLeft, DrehDataRight;
int x, xalt, kp, kd, ki, yp, yd, yi, drest=0, y, y2, isum=0, ADOffset;

void FollowLine (void)
{
unsigned char leftDir = FWD, rightDir = FWD;

OdoLeft();
OdoRight();
x = (DrehDataLeft - DrehDataRight) - ADOffset;

SerWrite("\n\rBeginn FollowLine(void)... Wert von x: ", 41);
PrintInt(x);

yp = x*kp; // P-Anteil

isum += x;
if (isum > IMAX) isum = IMAX;
if (isum < -IMAX) isum = -IMAX;
yi = isum / 625 * ki; //I-Anteil

yd = (x - xalt) * kd; // D-Anteil
yd += drest;
if (yd > 255)
{
drest = yd - 255;
}
else if (yd < -255)
{
drest = yd + 255;
}
else
{
drest = 0;
}

if (isum > IMAXALARM) BackLED(OFF,ON);
else if (isum < -IMAXALARM) BackLED(ON,OFF);
else BackLED(OFF,OFF);

y = yp + yi + yd; // PID
y2 = y / 2;
xalt = x;

speedLeft = speedRight = speed;
MotorDir(RWD,RWD);

if ( y > 0)
{
StatusLED(GREEN);
speedLeft = speed + y2;
if (speedLeft > SPEEDMAX)
{
speedLeft = SPEEDMAX;
y2 = speedLeft - speed;
}
y = y - y2;
if (speedRight < SPEEDMIN)
{
speedRight = SPEEDMIN;
}
}

if ( y < 0)
{
StatusLED(RED);
speedRight = speed - y2;
if (speedRight > SPEEDMAX)
{
speedRight = SPEEDMAX;
y2 = speed - speedRight;
}
y = y - y2;
speedLeft = speed + y;
if (speedLeft < SPEEDMIN)
{
speedLeft = SPEEDMIN;
}
}
leftDir = rightDir = RWD;
if (speedLeft < SPEEDMIN + 5) leftDir = BREAK;
if (speedRight < SPEEDMIN + 5) rightDir = BREAK;
MotorDir(leftDir,rightDir);
MotorSpeed(abs(speedLeft),abs(speedRight));
}



void OdoLeft(void)
{
unsigned long zeitbeginn, zeitende, zeitdiff;
unsigned int OdoData[2];
int count, l_old;


SerWrite("\n\rBeginn OdoLeft()", 20);
zeitbeginn = Gettime();
for(count = 0; count < 6;)
{
OdometrieData(OdoData);
if (OdoData[0] > 800 && l_old < 800)
{
count++;
}
l_old = OdoData[0];
}
count = 0;
zeitende = Gettime();
zeitdiff = zeitende - zeitbeginn;
DrehDataLeft = 60000 / zeitdiff;
SerWrite("\n\rEnde OdoLeft(), wert von DrehDataLeft ", 43);
PrintInt(DrehDataLeft);
}



void OdoRight(void)
{
unsigned long zeitbeginn, zeitende, zeitdiff;
unsigned int OdoData[2], drehzahl_right;
int count, r_old;


SerWrite("\n\rBeginn OdoRight()", 21);
zeitbeginn = Gettime();
for(count = 0; count < 6;)
{
OdometrieData(OdoData);
if (OdoData[1] > 600 && r_old < 600)
{
count++;
}
r_old = OdoData[1];
}
count = 0;
zeitende = Gettime();
zeitdiff = zeitende - zeitbeginn;
DrehDataRight = 60000 / zeitdiff;
SerWrite("\n\rEnde OdoRight(), wert von DrehDataRight ", 43);
PrintInt(DrehDataRight);
}



int main(void)
{
Init();
FrontLED(ON);
MotorDir(RWD,RWD);
MotorSpeed(SPEED,SPEED);
SerWrite("Beginn main(void)", 17);
OdoLeft();
OdoRight();
SerWrite("\n\rDrehDataLeft, DrehDataRight ", 30);
PrintInt(DrehDataLeft);
SerWrite(" ", 1);
PrintInt(DrehDataRight);
OdoLeft();
OdoRight();
ADOffset = DrehDataLeft - DrehDataRight; // Helligkeitsunterschied links und rechts

MotorDir(RWD,RWD);
StatusLED(GREEN);

speed = SPEED;
speedLeft = speed;
speedRight = speed;

kp = 10; ki = 4; kd = 70; // Regler Parameter kd enthält bereits Division durch dt


while(1)
{
FollowLine();
}
return 0;
}



Über optiemierungsvorschläge würde ich mich sehr freuen!

Gruss,
Matt