Objekte Finden und den Roboter darauf ausrichten

Auf ein Hindernis ausrichten ohne zu suchen

Mit Hilfe der Näherungssensoren der Motoren kann sich der Zumo 32U4 auf ein nahes Objekt hin ausrichten. Dafür muss überprüft werden, ob sich das Objekt eher rechts oder eher links vom Roboter befindet. Befindet sich das Objekt eher links, muss sich der Roboter nach rechts drehen, befindet es sich eher rechts, muss er sich andersherum drehen. Geben beide Sensoren die selben Werte zurück, so befindet sich das Objekt eher in der Mitte und der Roboter bleibt stehen.

Daraus ergibt sich ein einfacher Algorithmus zum Ausrichten.

// Richtet den Roboter zu einem Hindernis aus
wenn (sensorLinks > sensorRechts)
    drehenLinks()
sonst wenn (sensorLinks < sensorRechts)
    drehenRechts()
sonst
    motorenStopp()

Das vollständiges Programm für den Zumo 32U4 könnte dann folgendermaßen aussehen:

/*  Zu einem Hindernis ausrichten. Wird kein Hindernis erkannt, bleibt der Roboter stehen. */

#include <Zumo32U4.h>

Zumo32U4LCD lcd;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;

const  int turnSpeed = 100;

void setup()
{
  // Initialisierung des vorderen IR Sensors.
  proxSensors.initFrontSensor();

  // Helligkeitswerte: Je höher der Wert, desto eher wird ein Objekt erkannt.
  // Die verwendeten Helligkeitswerte eigenen sich gut für eine weiße Box
  int levels[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9 };            
  proxSensors.setBrightnessLevels(levels, sizeof(levels)/sizeof(levels[0]));
}

void loop()
{
  proxSensors.read();  // Wertet den vorderen IR Sensor mit den IR-LEDs links und rechts aus.
  int anzahlLinks = proxSensors.countsFrontWithLeftLeds();
  int anzahlRechts = proxSensors.countsFrontWithRightLeds();

  if (anzahlLinks < anzahlRechts) {
    motors.setSpeeds (turnSpeed, -turnSpeed);
  }
  else if (anzahlLinks > anzahlRechts) {
    motors.setSpeeds (-turnSpeed, turnSpeed);
  }
  else {
    motors.setSpeeds (0, 0);
  }

  lcd.gotoXY(0, 0);
  lcd.print("L"); lcd.print(anzahlLinks); lcd.print(" R"); lcd.print(anzahlRechts);
}

Ein Objekt suchen und dann ausrichten

Im letztem Absatz hat sich der Roboter zu einem Hindernis ausgerichtet, welches mit den Näherungssensoren bereits erfasst wurde. Nun soll der Roboter das Hinderniss suchen und sich dann darauf ausrichten. Dafür wird der logische Wert seheHindernis benötigt, der angibt, ob der Roboter ein Hinderniss sieht, also einer der beiden Sensoren einen gewissen Schwellwert überschreitet.

bool seheHindernis = (sensorLinks > schwellWert) ODER (sensorRechts > schwellWert)

Das Beispiel dreht den Roboter mit konstanter Geschwindigkeit so lange in eine Richtung, bis der Roboter ein Hindernis erkennt.

Leider funktioniert der Algorithmus nicht besonders gut. Variieren Sie die Drehgeschwindigkeit des Roboter, wenn er kein Hindernis sieht und analysieren Sie das Verhalten.

Aufgaben

Schreiben Sie ein einfaches Programm, welches den Roboter mit konstanter Drehgeschwindigkeit auf der Stelle nach einem Objekt suchen lässt. Hat er ein Objekt gesehen, soll er sich langsam darauf ausrichten. Nutzen Sie die Variable `bool seheHindernis` .

Schnelles Suchen und Ausrichten

Ein Möglichkeit des Suchen und Ausrichten zu beschleunigen, ist die Drehgeschwindigkeit nicht konstant zu lassen, sondern sie, je nachdem ob ein Objekt erkannt wurde oder nicht, schrittweise zu erhöhen oder zu verringern.

Wenn kein Hindernis bool seheHindernis = (sensorLinks > schwellWert) || (sensorRechts > schwellWert) gesehen wird, wird die Drehengeschwindigkeit des Roboter also erhöht

turnSpeed += 20; // Geschwindigkeit erhöhen

Wenn ein Hindernis erkannt wird, soll die Geschwindigkeit langsam verringert werden:

turnSpeed -= 20; // Geschwindigkeit verringern

Damit der Geschwindigkeitswert aber nicht zu groß oder klein wird, sollte dieser anschließend noch begrenzt werden:

turnSpeed = constrain(turnSpeed, minSpeed, maxSpeed);

Schnelles Suchen und Ausrichten mit einem P(ID)-Regler

Ein weiterer Ansatz fürs Suchen und Ausrichten wäre eine PID-Regler. Der Fehler für den Regler ergibt sich dabei aus dem Unterschied zwischen dem linken und dem rechten Näherungssensor:

anzahlLinks  = proxSensors.countsFrontWithLeftLeds();
anzahlRechts = proxSensors.countsFrontWithRightLeds();
error        = anzahlRechts - anzahlLinks;

Wurde der Fehler bestimmt, kann dann mit dem P-Regler

turnSpeed = kp * error;

oder einem vollständigen PID-Regler

turnSpeed = kp * error + ki * integral + kd * (lastError - error);

und den geeigneten Faktoren $k_p$, und $k_i$, $k_d$, die Geschwindigkeit für die Räder bestimmt werden.

Aufgaben

Überlegen Sie sich ein Programm, welches den Roboter so schnell wie möglich zu einen Objekt ausrichtet. Sieht der Roboter kein Objekt, soll er sich in die Richtung drehen, in welcher das Objekt das letzte mal gesehen wurde.

Einem Objekt folgen

Nutzen Sie die obigen Ansätze, um den Zumo 32u4 einem Objekt (z.B. weiße Box) folgen zu lassen. Dafür sollte sich der Roboter auf das Objekt ausrichten können und zusätzlich den Abstand zum Objekt konstant halten.