Projekt ( Spurverfolger mit PID - Regler)

Der Roboter folgt einer schwarzen Spur. Dafür hat er einen Lichtsensor zur Verfügung. Der Roboter soll dabei so schnell als möglich fahren.

Die Steuerung geschieht über eine PID - Regelung. 

Zur Theorie des PID-Reglers  

Im folgendem sieht man die Java-Quelltexte für den PID - Regler. Die hierfür eingesetzten Variablen sind für den Roboter und seinen Sensor spezifisch. Diese muss man an die Situation angleichen.

Quelltext (P - Regler)

Spurverfolger P-Regler

import lejos.hardware.Button;
import lejos.hardware.motor.UnregulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.robotics.EncoderMotor;
 
 
public class SpurVerfolger1 { 
    EV3ColorSensor lichtSensor;
    float[] farbe;
    EncoderMotor MotorL;
    EncoderMotor MotorR;
    //Konstruktor
    SpurVerfolger1() {
        lichtSensor = new EV3ColorSensor(SensorPort.S3);
        lichtSensor.setCurrentMode("Red");
        farbe = new float[lichtSensor.sampleSize()];
        MotorL = new UnregulatedMotor(MotorPort.B);
        MotorR = new UnregulatedMotor(MotorPort.C);
    }
   
 
    public static void main(String[] args) {
         
        //Konstruktor aufrufen
    	SpurVerfolger1 m = new SpurVerfolger1();
         
        // Variablen für die Helligkeit definieren
        float wert; // Messwert
        float grau = (float) 0.3;  // Grauwert
    
        // Variablen für die Geschwindigkeiten der Räder
        int v_schnitt = 80;  // Durchschnittsgeschwindigkeit  (möglichst hoch)
        
        int v_links =v_schnitt;   
        int v_rechts =v_schnitt;
        
        
        // Variablen für den Regler:
        float fehler = 0; //// Differenz zwischen ist und soll-Wert für P-Regler
        double regler = 1;
        double k_p = 17;   // anpassen!
         
 
        while (Button.ESCAPE.isUp()) {
       
            // Lichtwert messen und bereitstellen
            m.lichtSensor.fetchSample(m.farbe, 0);
            wert = m.farbe[0]; // Messwert
             
            //Motoren nach vorne fahren lassen 
            m.MotorL.forward();
            m.MotorR.forward();
             
            //Abweichung bzw. Fehler berechnen
            fehler = wert - grau; 
            
            // Einfluss auf die Geschwindigkeit ist dem Fehler proportional
            regler = k_p * fehler;
    
            v_links = (int) (regler + v_schnitt);   // links
            v_rechts = (int) ((-1)*regler + v_schnitt); // rechts
             
            //Werte begrenzen  ? zwischen 20 und 100 ?
            if (v_links < 20) {
            	v_links = 20;
            }
            if (v_links > 100) {
            	v_links = 100;
            }
     
            if (v_rechts < 20) {
            	v_rechts = 20;
            }
            if (v_rechts > 100) {
            	v_rechts = 100;
            }
     
         
    
            //die Geschwindigkeit der Motoren berechnen und auf die Motoren anwenden 
            m.MotorL.setPower(v_links);
            m.MotorR.setPower(v_rechts);
             
                 
        } // ende while
        
    } // ende main
} // ende class
Quelltext (PID-Regler)
import lejos.hardware.Button;
import lejos.hardware.motor.UnregulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.robotics.EncoderMotor;


public class SpurPID1 {

	EV3ColorSensor lichtSensor;
	float[] farbe;
	EncoderMotor MotorL;
	EncoderMotor MotorR;
	//Konstruktor
	SpurPID1() {
		lichtSensor = new EV3ColorSensor(SensorPort.S3);
		lichtSensor.setCurrentMode("Red");
		farbe = new float[lichtSensor.sampleSize()];
		MotorL = new UnregulatedMotor(MotorPort.B);
		MotorR = new UnregulatedMotor(MotorPort.C);
	}
	
	
	public static void main(String[] args) {
		
		//Konstruktor aufrufen
		SpurPID1 m = new SpurPID1();
		
		// Datenliste für die Fehler
		float abweichung[] = new float[4];
		 for (int i = 0; i < abweichung.length; i++) {  
			 abweichung[i]=0;
		 }
		
		// Variablen definieren
		float wert; // Messwert
		float grau = (float) 0.31;  // Grauwert
		int count = 0; // Zähler
		
		float summe = 0; //Summe der Fehler für I-Regler
		float differenz = 0; // Differenz zwischen zwei Fehler für D-Regler
		float fehler = 0; //// Differenz zwischen ist und soll-Wert für P-Regler
		
		//konstanten definieren 
		double k_p = 180;  //180
		double k_i = 2  ;   // 3
		double k_d = 9000;
		
		int v_schnitt = 90;  // Grundgeschwindigkeit
		
		
		
		// ersten Wert einlesen
		m.lichtSensor.fetchSample(m.farbe, 0);
		wert = m.farbe[0];
		float alterFehler = wert-grau;
		

		while (Button.ESCAPE.isUp()) {
			count = count + 1;
			// Lichtwert messen und bereitstellen
			m.lichtSensor.fetchSample(m.farbe, 0);
			wert = m.farbe[0]; // Messwert
			
			
			//Motoren nach vorne fahren lassen 
			m.MotorL.forward();
			m.MotorR.forward();
			
			//PID - Teile berechnen
			// P-Teil
			fehler = wert - grau; 	
			abweichung[count%abweichung.length]= fehler;
		
			// I-Teil Summe berechnen
			summe =0;
			for(int i=0 ; i<abweichung.length ; i++){
		        summe = summe + abweichung[i];
			}
			// D - Teil Änderung des Fehlers
			differenz = fehler - alterFehler; 
			

			//die Geschwindigkeit der Motoren berechnen und auf die Motoren anwenden
			double regler = k_p * fehler + k_i * summe + k_d * differenz;
			int links = (int) ((regler) + v_schnitt); // links
			int rechts = (int) (-1* (regler) + v_schnitt); // rechts
			
			//Werte begrenzen
			if (links < 20) {
				links = 20;
			}
			if (links > 100) {
				links = 100;
			}

			if (rechts < 20) {
				rechts = 20;
			}
			if (rechts > 100) {
				rechts = 100;
			}
			
			m.MotorL.setPower(links);
			m.MotorR.setPower(rechts);
			
			//alte Werte merken
			alterFehler = fehler;
			
		} // ende while
	} // ende main
} // ende class

Theorie: der PID - Regler

Damit ein Roboter einer Spur folgen kann, muss man in Abhängigkeit von der erfassten Helligkeit des Untergrundes Befehle an die beiden Motoren liefern.

Bei der Regelung werden im Gegensatz zur Steuerung die Stellgrößen immer wieder neu justiert.