Fahrtrichtung mit IMU

Weist man bei einem Roboter, der von zwei Motoren angetrieben wird, beiden Motoren die gleiche Geschwindigkeit oder die gleiche Leistung zu, so wird der Roboter aufgrund von produktionsbedingten Unterschieden in den Motoren bzw. den darin enthaltenen mechanischen und elektronischen Komponenten über längere Strecken nie genau geradeaus fahren.

Soll ein Roboter seine Fahrtrichtung exakt beibehalten, so kann dazu der IMU-Sensor verwendet werden. Mit Hilfe dieses Sensors wird laufend die aktuelle Richtung ermittelt und mit der gewünschten Richtung verglichen. Fall es dabei eine Abweichung gibt, wird die Geschwindigkeit der Motoren angepasst um den Roboter wieder in die gewünschte Richtung zurückzudrehen.

Das closed loop - Verfahren

Das Verfahren, das hier zur Anwendung kommt, wird als closed loop bzw. als Regelkreis mit Rückkopplung bezeichnet.

Die Änderung von gemessenen Werten führt dazu, dass ein Prozess (hier die Ansteuerung von Motoren) geregelt wird. Bei einem closed loop Verfahren fließen die Änderungen, die sich aus diesem Regelprozess ergeben, bei den nächsten Entscheidungen oder Berechnungen für die folgenden Aktionen direkt mit ein.

Im folgenden Codebeispiel wird zu Beginn des Programms die aktuelle Richtung ermittelt in die der Roboter fahren soll und in der Variable ZielWert abgelegt. In der while-Schleife wird zunächst der aktuelle YAW-Wert des IMU-Sensors eingelesen und der Variable AktuellerWert zugewiesen. In der Folge wird in if-Abfragen überprüft, ob die aktuelle Richtung links oder rechts von der gewünschten Richtung abweicht. Ist dies der Fall, wird die Geschwindigkeit des jeweiligen Motors um drei erhöht, damit sich der Roboter wieder zur gewünschten Richtung zurückdreht.

Gleicht die aktuelle Richtung der gewünschten Richtung, so bekommen beide Motoren die gleiche Geschwindigkeit zugewiesen.

Programmbeispiel - Exaktes Geradeausfahren mit dem Gyro-Sensor ausgehend von einer Startrichtung

#include "KeplerOpenBOT.h"
 
uint16_t ZielWert;
uint16_t AktuellerWert;
 
void setup()
{
  KeplerOpenBOT_INIT();
  WRITE_I2C_BNO055_INIT();
  ZielWert = READ_I2C_BNO055_YAW();
}
 
void loop()
{
  AktuellerWert = READ_I2C_BNO055_YAW();
  if(AktuellerWert>ZielWert)
  {
    WRITE_MOTOR(MR, 43);
    WRITE_MOTOR(ML, 40);
  }
  if(AktuellerWert==ZielWert)
  {
    WRITE_MOTOR(MR, 40);
    WRITE_MOTOR(ML, 40);
  }
  if(AktuellerWert<ZielWert)
  {
    WRITE_MOTOR(MR, 40);
    WRITE_MOTOR(ML, 43);
  }
}

Erklärungen zu diesem Programmbeispiel

Zeile 10:

Hier wird der Integer-Variable ZielWert der aktuelle YAW-Wert des IMU-Sensors zu Programmstart zugewiesen.

Zeile 15:

Kontinuierliches Einlesen des aktuellen YAW-Werts zu Beginn der while-Schleife.

Zeile 17-21:

Hier wird überprüft, ob die aktuelle Richtung größer als die Zielrichtung ist, der Roboter also von der gewünschten Richtung aus nach rechts gedreht hat. Ist dies der Fall, wird die Geschwindigkeit des rechten Motors um 3 erhöht, damit der Roboter wieder in Richtung des gewünschten Werts zurückdreht.

Zeile 23-27:

Stimmt die aktuelle Richtung genau mit der gewünschten Richtung überein, so werden die Geschwindigkeiten bei beiden Motoren auf den gleichen Wert gesetzt.

Zeile 29-33:

Wenn der aktuelle Richtungswert kleiner als der Zielwert ist, so hat sich der Roboter zu weit nach links verdreht und muss durch Erhöhen der Geschwindigkeit des linken Motors wieder nach rechts zurückdrehen.

Exakte Beibehaltung der Fahrrichtung mit dem IMU-Sensor