Projekt 70c: Labyrinth SLAM mit EV3

Autoren: Sievers, Scharfenberg
Betreuer: Prof. Schneider

→ zurück zur Übersicht: WS 18/19: Angewandte Elektrotechnik (BSE)

Aufgabe

Ein EV3 Roboter soll den Ausgang aus einem Labyrinth finden

Erwartungen an die Projektlösung

• Aufbau des EV3 Roboters in Abstimmung mit Prof. Schneider
• Recherche SLAM
• Ortung und Navigation via US oder IR Sensor und Odometrie (SLAM)
• Inbetriebnahme mit Matlab/Simulink
• Realisierung der Flucht aus dem Labyrinth
• Stellen Sie die digitale Karte des Labyrinths dar.
• Machen Sie spektakuläre Videos, welche die Funktion visualisieren.
• Test und wiss. Dokumentation
• Live Vorführung während der Abschlusspräsentation

Projekt

Projektplan

Um einen Überblick über das Thema zu erhalten, wurde die Zeitphase des Praktikums genutzt. Dabei wurden sowohl die bestehenden Unterlagen zum Thema SLAM mit Lego EV3 von vorherigen Projektteams gesichtet, als auch eigene Unterlagen. Geplant war mit dem Beginn der eigentlichen Arbeitspakete nach Ende der Praktikumsversuche. Dabei geht es sowohl um den mechanischen Aufbau, als auch der Programmierung des Roboters. Anschließend ist eine Testphase geplant gewesen, bei die Anforderungen schrittweise abgeprüft, Fehler dokumentiert und anschließend beseitigt werden sollten. Anschließend war die eigentliche Dokumentation, die Ausstattung des Messestands sowie die Videodokumentation geplant. Im Anschluss an alle Arbeitspakete steht der Meilenstein der HSHL Hausmesse am 18.01.2018.

Momentaner Stand:

Praktikum 100%

Unterlagen sichten 100%

Lego Roboter und Labyrinth aufbauen 100%

Sensoren verbauen 100%

Software Strategie entwickeln 100%

Software Module schreiben 100%

Testphase 90%

Änderungen an Software 100%

Wiki Dokumentation schreiben 80%

Plakat entwerfen und Messestand 60%

Video drehen und hochladen 100%

Messe 0%

Konstruktion

Grundaufbau Robotor

Lego EV3 Education Roboter Grundaufbau (*1)

Der Grundaufbau des Roboters stütz sich auf die Bauanleitung des Robot Educator des Robot Educator von Lego Mindstorms Education. Das Grundfahrgestell besteht aus dem EV3-Brick, zwei EV3 Motoren, die jeweils mit zwei Rädern verbunden sind und einer Metallkugel auf, die der hintere Teil des Roboters gestützt ist. Der Vorteil dieses Fahrgestells ist es, dass sich der Roboter durch die zwei einzelnen angetriebenen Räder und die unterstützende Metallkugel eine Drehung auf der Stelle realisieren kann und somit sein Bewegungsradius sehr gering ist.

.

Umbau auf 3 Ultraschallsensoren

Optimierte Position Ultraschall links und rechts

Aufbau mit 3 Ultraschallsensoren

Damit der Roboter sein Umfeld wahrnimmt wurden drei EV3 Ultraschallsensoren verwendet und in den Aufbau des Grundfahrgestells integriert. An den beiden Seiten sowie an der Front des Roboters wurde jeweils ein Ultraschallsensor angebracht. Unser Hauptprogramm ist aufgeteilt in 3 Unterprogramme die gleichzeitig ausgeführt werden. Diese Unterprogramme sind die Längsregelung, die Querregelung und die Ausführung der Kamera.

Optimierung Sensorpositionen

Wie in Abb. X zu sehen ist, vergrößerte die Nachrüstung der seitlichen Ultraschallsensoren die Breite des Roboters. Nach ersten Versuchen der Bewegung des Roboters im Labyrinth fiel auf, dass die Ultraschallsensoren die Drehung des Roboters im Labyrinth erschweren. Für eine Drehung ohne dass der Roboter mit den Ultraschallsensoren die Wände streift, musste der Roboter perfekt in der Mitte platziert sein. Um dieses Problem zu lösen wurde an der Positionierung der seitlichen Ultraschallsensoren eine Optimierung durchgeführt.

Aufbau variables Labyrinth

Umbau Labyrinth

-

System Design

Toolbox Schnittstelle EV3

Toolbox Matlab EV3

Begonnen wurde das Projekt mit der integrierten Toolbox von Matlab. Diese Toolbox enthält einfache Befehle zur Ansteuerung von Sensoren und Motoren. Nach einiger Zeit hat sich aber herausgestellt, dass die Updaterate, mit der der EV3 Messwerte an Matlab liefert, so geringt ist, dass keine vernünftige Regelung erstellt werden kann. Durch die Messung im Sekundentakt werden sehr häufig Werte überfahren. Dadurch kann nicht sichergestellt werden, dass der Roboter passend anhält, nicht zu weit dreht und nicht gegen die Wand fährt. Um dieses Problem zu beheben, wurde auf die RWTH EV3 Toolbox gewechselt.

Toolbox RWTH EV3

Die RWTH EV3 Toolbox wird nicht mehr über die offizielle RWTH Mindstorm Internetseite verteilt, sondern ist auf Github zu finden. Die Installation wird in der beiliegenden Dokumentation beschrieben und ist schnell gemacht. Die Toolbox erlaubt eine Vielzahl von Sensoren auszulesen, sowohl EV3 als auch NXT. Für die Motorsteuerung gibt es einige zusätzliche Befehle wie die synchrone Zusammenarbeit oder die integrierte Regelung auf Zeit oder Schritte.

EV3

Connect

Mit dem Befehl connect wird eine Verbindung zum EV3 hergestellt. Die RWTH Toolbox unterstützt USB und Bluetooth Verbindung, wobei die Bluetooth Verbindung nicht hergestellt wurde. Dies kann laut Dokumentation am geänderten Bluetooth Treiber für Win10 liegen.

Um eine USB Verbindung aufzubauen benötigt man den folgenden Befehl:

b.connect('bt', 'serPort', '/dev/rfcomm0'); 

serPort muss der Serielle Port der Bluetoothverbindung angegeben werden. Diesen findet man in den Systemeinstellungen für den Bluetooth Adapter, der im Einsatz ist.

Für eine USB Verbindung:

b.connect('usb', 'beep', 'on', ) 

beep on sorgt für einen kurzen Ton wenn der EV3 verbunden ist.

Playtone

Mit playTone kann ein Ton über den Soundchip des EV3 abgespielt werden

myev3.playTone(10,1000,150)

Der erste Parameter ist die Lautstärke von 0 bis 100%

Der zweite Parameter ist die Freuqenz in Hz

Der dritte ist die Dauer in ms

Ultraschallsensoren

Der Primärsensor und seine Funktionen (Auszug aus Wiki Artikel Objekterkennung mit rotierenden Ultraschall mit Matlab/Simulink und EV3[1])

Abbildung 1: Der Ultraschallsensor

Der Sensor für dieses Projekt ist der LEGO Mindstorms EV3 Ultrasonic Sensor (95652) . In Abbildung 1 wird der Sensor dargestellt. Er misst Entfernungen zwischen 3 und 250 cm. Die Messgenauigkeit liegt bei ± 1 cm. Der Sensor leuchtet dauerhaft beim Senden und blinkt beim Empfangen. Die EV3-Software erkennt den angeschlossenen Sensor automatisch und durch das passende Matlab/Simulink Supportpackage kann so auf den Sensor zugegriffen werden. Kurzgefasst besteht der Ultrasonic Sensor aus einem Sender und einem Empfänger. Der Sender emittiert die Ultraschallwellen, während der Empfänger die Ultraschallwellen empfängt. So können Objekte und deren Entfernung in cm gemessen werden. Der Ultrasonic misst die Strecke, indem er die Zeit auswertet die er benötigt, um die Ultraschallwellen auszusenden und wieder zu empfangen.

Der LEGO Mindstorms EV3 Ultrasonic Sensor (95652) besteht unter anderem aus:

• einem STM8S103F3 Mikrocontroller
• einem AW8T40-10OA00 Sender
• einem AW8R40-10OA00 Empfänger

Der Sender und der Empfänger besitzen folgende technische Daten:

Eigenschaft	AW8T40-10OA00 Sender	AW8R40-10OA00 Empfänger
Mittelfrequenz	40.0±1.0KHz
Schalldruckpegel	≥110dB	-
Empfindlichkeit	-	≥-67dB
Strahlungswinkel	100°
Kapazität	1700±20%pF
Gehäuse	Aluminum
Max. Ansteuerungsspannung	30Vrms
Arbeitstemperatur	-20 bis +70℃
Lagertemperatur	-30 bis +80℃

Messfehler

Die Werte der Ultraschallsensoren sind abhängig von diversen Störfaktoren:

- Da die Zeit gemessen wird, zwischen Senden und Empfangen können Störungen in der Elektronik zu verfälschten Werten führen

- Da das Ausbreitungsmedium Luft ist, hat die Umgebungstemperatur Einfluss auf das Ergebnis. Der Sensor ist kalibriert auf 20° Umgebungstemperatur laut Datenblatt.

- Die reflektierende Oberfläche hat starken Einfluss auf das Messergebnis. Da wir bei unserem Labyrinth immer die gleiche Oberfläche haben, kann man davon ausgehen, das dort keine großen Unregelmäßigkeiten auftreten.

- Der Analog- zu Digitalwandler bewirkt eine Quantisierung des Signals. Unser benötigte Messgenauigkeit ist aber um das 10-fache niedriger als die mögliche Ausgabe.

Durch Versuche hat sich herausgestellt, das man mit einer Genauigkeit von +- 1mm rechnen kann. Unser Algorithmus benötigt eine Genauigkeit von +-3mm, wodurch die Störfaktoren erträglich für das System sind.

setProperties

Um den Ultraschallsensor auslesen zu können muss er zunächst über setProperties intiialisiert werden.

Myev3.sensor1.setProperties('mode', DeviceMode.UltraSonic.DistCM);

Dieser Befehl setzt den Sensor an Port 1 in den Modus Ultraschall. Dieser Sensor gibt dann den Wert in cm aus. Der Sensor kann auch wenn gewünscht in Zoll ausgeben (DeviceMode.UltraSonic.DistIn)

Es können eine Vielzahl von Sensoren mit dieser Toolbox genutzt werden, auch NXT Sensoren.

value

Der Befehl value übergibt den aktuellen Wert an Matlab.

lensor.sensor1.setProperties('mode', DeviceMode.UltraSonic.DistCM);  

x = lsensor.value

Kalibrierung

Um zu ermitteln, wie groß der gemessene Wert vom wirklichen abweicht, wurde ein kleines Tool geschrieben, das den Roboter schrittweise fahren lässt und den Messwert mit der gefahrenen Distanz gegenüberstellt. Dies wurde für alle drei Sensoren durchgeführt:

• 

  Linker Sensor

• 

  Mittlerer Sensor

• 

  Rechter Sensor

Die Messungen wurden jede 3x widerholt. Dabei wurde festgestellt, das die Abweichungen im Bereich +- 0,3mm liegen. Dieser Wert ist deutlich genauer als benötigt. Deswegen wurden keine Kennlinien zum Ausgleich im Tool hinterlegt. Sollte eine hohe Genauigkeit der Messwerte erforderlich sein, können die aufgenommenen Werte als Kalibrierung verwendet werden.

Motoren

Der verwendete Roboter besitzt zwei Schrittmotoren, die einzeln oder synchron angesteuert werden können.

Inkrementalgeber

Der Motor besitzt einen Inkrementalgeber, der die Winkelveränderung vom Motor messen kann. Damit kann abgefragt werden, wie weit der Motor gefahren ist. Ein weiterer Vorteil ist dabei, dass auch bei erzwungener Bewegung der Weg genau erfasst werden kann. Dies kann der Fall sein, wenn der Roboter sich auf einer Schrägen Ebene bewegt und die Gewichtskraft Einfluss nimmt.

Der Inkrementalgeber kann zu jedem Zeitpunkt zurückgesetzt oder ausgelesen werden.

X = l.tachoCount

Hiermit wird der aktuelle Wert übergeben.

resetTachoCount(l);

Der Wert wird auf 0 zurückgesetzt.

Motor Initialisierung

l = myev3.motorB;

Hiermit wird der Variable l der Motor an Port B übergeben. Der EV3 hat vier Ports für Motorsteuerung.

Motor Geschwindigkeit

l.power = 20;

Hiermit wird die Motorgeschwindigkeit festgelegt. Erwartet wird ein Wert von 0 bis 100.

SyncedStart

l.syncedStart(r);

Hiermit werden beide Motoren gleichzeitig gestartet. In der Klammer muss definiert werden, welche Motor mitlaufen soll.

Stop

l.stop();

Der Stop befehl stoppt den Motor. Falls er vorher synchron lief, wird er auch synchron gestoppt.

Bluetooth, Wifi und USB Schnittstellen

Der EV3 besitzt eine USB Schnittstelle, eine Bluetooth Schnittstelle und mit einem zusätzlichen Adapter auch eine W-Lan Schnittstelle. Mit der alten Matlab Toolbox kam der folgende Adapter erfolgreich zum Einsatz:

EDIMAX EW-7811UN Wireless USB Adapter, 150 Mbit/s, IEEE802.11b/g/n

Mit der RWTH Aachen Toolbox ist leider nur USB und Bluetooth möglich. Die Bluetooth Schnittstelle hat bei unseren beiden Notebooks leider keine Verbindung aufgebaut. Wir vermuten das nur bestimmte Bluetooth USB Adapter kompatibel mit der Toolbox sind, da die Bluetooth Schnittstelle mit der LEGO Software funktioniert hat. Für unseren Einsatz haben wir die USB Schnittstelle verwendet und das Kabel per Hand mitgeführt. Das dem EV3 Education Kit beigelegte Kabel hat eine ausreichende Länge für die Größe des Labyrinths.

myev3 = EV3(); 

myev3.connect('usb', 'beep', 'on'); % 

Die beiden Befehle erstellen das EV3 Objekt und verbinden es mit Matlab. Der Parameter beep = on lässt den EV3 bei Verbindung piepen.

Strategie

Strategie Labyrinth

Rechte Hand Regel

Die Rechte Hand Regel ist eine Strategie, bei der sich der Roboter immer an der rechts von ihm liegenden Wand orientiert und diese entlangfährt. Der Algorithmus geht davon aus, das die innenliegende Wand immer zum Ausgang führt. Dies ist auch gegeben, wenn der Anfang nicht an einer „Insel“ passiert. Dies ist ein eingeschlossener Bereich innerhalb des Labyrinths. Diesen würde man mit der rechten Hand Regel endlos umfahren, ohne den Ausgang zu erreichen.

Optimierung mit Inselerkennung - Pledge Algorithmus

Mit dem sogenannten Pledge-Algorithmus kann man das Problem der Inseln beheben. Dabei werden zwei Parameter betrachtet. Zum einen, wie viele Drehungen habe ich gemacht und zum anderen in welche Orientierung schaue ich gerade. Man definiert eine Richtung als sein Ziel (z.B. nach Norden ausgerichtet). Immer wenn wir nach Norden schauen, bewegen wir uns so lange bis wir wieder auf ein Hindernis treffen und nutzen dort erneut die rechte Hand Regel.

Logischer Aufbau mit 4 Ultraschallsensoren

Logischer Aufbau mit 4 Ultraschallsensoren

-

Logischer Aufbau mit 3 Ultraschallsensoren

Da man immer davon ausgehen kann, das nach jeder Bewegung hinter dem Roboter keine Wand ist, wird der Algorithmus vereinfacht mit 8 möglichen Labyrinth Varianten.

Logischer Aufbau mit 3 Ultraschallsensoren

-

Programmierung

Befehlsablauf

Abb.13:PAP Befehlsablauf

%****************************************************************
%        Hochschule Hamm-Lippstadt                              *
%****************************************************************
% Modul	          : BSE Angewandte Elektrotechnik               *
%                                                               *
% Datum           : 15.12.2018                                  *
%                                                               *
% Funktion        : EV3 durch HSHL Labyrinth schicken           *
%                                                               *
% Implementation  : MATLAB 2018b                                *
%                                                               *
% Toolbox         : RWTH Aachen EV3 Toolbox                     *
%                                                               *
% Author          : Florian Scharfenberg und Christian Sievers  *
%                                                               *
% Bemerkung       :                                             * 
% Letzte Änderung : 15.01.2019                                  *
%                                                               *
%***************************************************************/

% @Misc{rwthmindstormsev3toolbox,
%   author       = {Atorf, L. and Sondermann, B. and Stadtmann, T. and Rossmann, J.},
%   title        = {RWTH - Mindstorms EV3 Toolbox},
%   year         = {2018},
%   version      = {1.0},
%   organization = {Institute for Man-Machine Interaction, RWTH Aachen University},
%   url          = {https://git.rwth-aachen.de/mindstorms/ev3-toolbox-matlab}

%% Vorherige Daten löschen und Verbindungen aufheben. 
clear all;
clc;

%% Zugehörige Programmabschnitte:
% bewege.m Motorsteuerung
% Messung.m Labyrinth scannen mit Medianfilter und Messwerte analysiseren
% SchreibeMesswerte.m Werte speichern
% Umgebungston.m Messwerte als Ton ausgeben zur Analyse
% zeichne.m Übernimmt analysierte Wände in Karte


%% Verbinden des EV3
% Varianten der Kommunikation. Für RWTH wird die letzte Zeile eingesetzt.
% Momentan hat nur USB funktioniert mit RWTH. 

% myev3 = legoev3('wifi','192.168.0.101','0016534341af');
% myev3 = legoev3('USB');
% Setup bluetooth connection via com-port 0 
myev3 = EV3(); 
% Setup usb connection, beep when connection has been established b = EV3(); % 
myev3.connect('usb', 'beep', 'on'); % 

%% Globale Variablen definiert
global posx % Momentanposition in x
global posy % Momentanposition in y
global linex; %
global liney;
global richtung; % Gibt die Momentane Himmelsrichtung an, möglich ist N/E/S/W. Die Anfangsrichtung kann im Code verändert werden. Prinzipiell aber egal
global Anzahl; %Ein Zähler für die gemessenen Werte
global Messwertelinks; %Die Messwerte vom linken Ultraschallsensor
global Messwerterechts; %Die Messwerte vom rechten Ultraschallsensor
global wegstrecke; %Ein Arrey mit allen gefahrenen Positionen
global Messanzahl; %Ein kurzfristiger Zähler für die gemessenen Werte
global FahrDifferenz; %Die Distanz zum letzten als Richtig definierten Wegpunkt
global schrittzahl; %Ein Zähler für die gefahrenen Positionen

%% Initialisierungen
Anzahl = 1; %Anfang bei Wert 1
Messanzahl = 0;
posx = 0 %Startposition bei 0, kann frei gewählt werden
posy = 0 %Startposition bei 0, kann frei gewählt werden
plot(posx,posy,'p') 
hold on
%Skalierung passend für das aktuelle Labyrinth, dies kann je nach Situation frei gewählt werden
xlim([-1900 900]) 
ylim([-400 1400])

richtung = 'E' %Die Startrichtung kann frei gewählt werden, hier passend zur Kameraperspektive. Muss nicht der echten Himmelsrichtung entsprechen
i=0; % Abbruchbedingung für while Schleife. Momentan nicht genutzt, könnte für Ausgang erkennung eingesetzt werden. 

%% Anfangsposition prüfen ob vorne Wand
%Einfacher Start bei dem der Roboter schaut, ob vor ihm eine Wand ist.
%Falls eine Wand vorhanden ist, wird er nach rechts gedreht. So ist sicher
%gestellt, das der Programmablauf funktioniert und nur drei Ultraschall
%notwendig sind. 
pause(0.2);    
x = Messung(myev3); % Messung der aktuellen Position
    zeichne(x);
if x(4)==1
   bewege(3,myev3); % Drehung rechts 90°
end
           
%% Laufalgorithmus
% In der while Schleife wird zunächst geschaut, welche Wände vorhanden sind
% (links rechts vorne). Der Median Filter in dieser Messung filtert
% eventuelle Falschwerte heraus. Dann werden die Wände eingezeichnet und
% die Strategie gewählt. Je nach Fall wird gefahren, gedreht oder eine
% Kombination aus beidem. 

while i == 0
    
    pause(0.5);
    x = Messung(myev3); % Messung der aktuellen Position
    zeichne(x); % aktuelle Postion in Karte einzeichnen 

    % Abfrage des Zustandes für rechts Drehung und geradeaus fahren
    if x(1)==0&&x(2)==0&&x(3)==0&&x(4)==1 ||x(1)==0&&x(2)==1&&x(3)==0&&x(4)==0 || x(1)==0&&x(2)==0&&x(3)==0&&x(4)==0 || x(1)==0&&x(2)==1&&x(3)==0&&x(4)==1  
        
        bewege(2,myev3); % Drehung rechts 90°
        pause(1.5);
        bewege(1,myev3); % Vorwärts 1 Abschnitt
    end
    
    % Abfrage des Zustands für eine links Drehung und geradeaus fahren
    if x(1)==0&&x(2)==0&&x(3)==1&&x(4)==1
        
        bewege(3,myev3); % Drehung links 90°
        pause(1.5);
        bewege(1,myev3); % Vorwärts 1 Abschnitt
    end
    
    % Abfrage des Zustands für zweifache links Drehung und geradeaus fahren
    if x(1)==0&&x(2)==1&&x(3)==1&&x(4)==1
        
        bewege(3,myev3); % Drehung links 90°
        pause(1.5);
        bewege(3,myev3); % Drehung links 90°
        pause(1.5);
        bewege(1,myev3); % Vorwärts 1 Abschnitt
        
    end
    % Abfrage des Zustands für geradeaus fahren
    if x(1)==0&&x(2)==0&&x(3)==1&&x(4)==0 || x(1)==0&&x(2)==1&&x(3)==1&&x(4)==0
        
        bewege(1,myev3); % Vorwärts 1 Abschnitt
        
    end
end

Messung

Die Funktion Messung ermittelt die Abstände der Wände links, rechts und vorne mit Hilfe eines Median-Filters. Wir haben im Testablauf gemerkt, das der Sensorwert sehr häufig richtig misst, nur in sehr seltenen Momenten starke Ausreißer bildet. Diese Ausreißer werden mit dem Median Filter herausgefiltert. An dieser Stelle wäre auch ein erweiterter Filter wie Partikelfilter und Kalman Filter möglich, um die Wände möglichst genau zu setzen. Für unseren Fall (gerade Wände und gleiche Längen) war dies allerdings nicht erforderlich. Die Ausgabe der Funktion ist eine Matrix, in der 0 für den Fall keine Wand und 1 für den Fall Wand geschrieben wird.

Die Reihenfolge ist: hinten - links - rechts - front

Hinten wird immer 0 geschrieben, da es im Programmablauf nur beim Start vorkommen kann, das eine Wand hinter uns steht. Dieser Fehler wird ausgeglichen indem beim Start geprüft wird ob vor uns eine Wand steht und wir in dem Fall uns um 90° drehen.

function x = Messung(myev3)

%% Messungen der Sensordaten
lsensor = myev3.sensor2;
rsensor = myev3.sensor3;
fsensor = myev3.sensor4;

lsensor.mode = DeviceMode.UltraSonic.DistCM;
rsensor.mode = DeviceMode.UltraSonic.DistCM;
fsensor.mode = DeviceMode.UltraSonic.DistCM;
for i=1:10
    pause(0.2);
%     db(i) = readDistance(bsensor)*100;
    dl(i) = lsensor.value;
    dr(i) = rsensor.value;
    df(i) = fsensor.value;
end

%% Messungen mitteln
mdl = median(dl);
mdr = median(dr);
mdf = median(df);

%% Ausgabe des Umgebeungsvektor
x=zeros(4,1);
x(1)=0;
if mdl<20
    x(2) = 1;
end
if mdr<20
    x(3) = 1;
end
if mdf<20
    x(4) = 1;
end

Umgebungston(x,myev3);
% clear myev3;
end

Umgebungston

Das Skript Umgebungston nimmt die vorab gemessenen Positionen der Wände links, rechts und vorne und lässt den Roboter schrittweise einen Ton für die einzelnen Position ausgeben. Bei einer Wand wird ein Ton mit 1000 Hz wiedergegeben, ohne Wand mit 500 Hz. Dies ist zwar nicht für den Programmablauf erforderlich, hilft aber der sofortigen Analyse im Testablauf. Dadurch können Fehler im Messen direkt erkannt werden und das Fehlverhalten vom Roboter genauer interpretiert werden.

Der Roboter piept in dieser Reihenfolge: hinten - links - rechts - vorne.

Hinten wird nie eine Wand angegeben, da das Skript so durchläuft, das hinter dem Roboter nie eine Wand ist beim messen.

%% Beepen des Roboters anhand des Umgebungsvektors
% Beep1: Hinten, Beep2: Links, Beep3: Rechts, Beep4: Vorne
function Umgebungston(x,myev3)

for i=1:4
  pause(0.4);
  if x(i)==0
    myev3.playTone(10,1000,150)
  else
    myev3.playTone(10,500,150)

  end
end
clear myev3;
end

Karte zeichnen

Die Funktion Karte zeichnen verwendet die in Messen ermittelte Matrix und schreibt damit die Wände in die Karte. Dabei wird von Fester Größe von 320mm ausgegangen. Dieser Wert kann für ein anderes Labyrinth angepasst werden oder mit Hilfe von anderen Strategien auch dynamisch geschrieben werden.

function zeichne(x)
% Die Funktion zeichnet die Wände in die Karte ein. Durch das vorhandene
% Labyrinth wird von geraden Wänden mit immer gleichen Längen ausgegangen. 

global richtung;
global posx
global posy
global linex
global liney
global Anzahl

% Schreibe Oben
if (richtung=='N'&&x(4)==1)||(richtung=='E'&&x(2)==1)||(richtung=='W'&&x(3)==1)
linex(Anzahl,1) = posx-160;
linex(Anzahl,2) = posx+160;
liney(Anzahl,1) = posy+160;
liney(Anzahl,2) = posy+160;
plot([linex(Anzahl,1),linex(Anzahl,2)],[liney(Anzahl,1),liney(Anzahl,2)])
Anzahl = Anzahl +1;
end

% Schreibe Unten
if (richtung=='E'&&x(3)==1)||(richtung=='S'&&x(4)==1)||(richtung=='W'&&x(2)==1)
linex(Anzahl,1) = posx-160;
linex(Anzahl,2) = posx+160;
liney(Anzahl,1) = posy-160;
liney(Anzahl,2) = posy-160;
plot([linex(Anzahl,1),linex(Anzahl,2)],[liney(Anzahl,1),liney(Anzahl,2)])
Anzahl = Anzahl +1;
end

% Schreibe Links
if (richtung=='N'&&x(2)==1)||(richtung=='S'&&x(3)==1)||(richtung=='W'&&x(4)==1)
linex(Anzahl,1) = posx-160;
linex(Anzahl,2) = posx-160;
liney(Anzahl,1) = posy-160;
liney(Anzahl,2) = posy+160;
plot([linex(Anzahl,1),linex(Anzahl,2)],[liney(Anzahl,1),liney(Anzahl,2)])
Anzahl = Anzahl +1;
end

% Schreibe rechts
if (richtung=='N'&&x(3)==1)||(richtung=='E'&&x(4)==1)||(richtung=='S'&&x(2)==1)
linex(Anzahl,1) = posx+160;
linex(Anzahl,2) = posx+160;
liney(Anzahl,1) = posy-160;
liney(Anzahl,2) = posy+160;
plot([linex(Anzahl,1),linex(Anzahl,2)],[liney(Anzahl,1),liney(Anzahl,2)])
Anzahl = Anzahl +1;
end

end

Bewegung

function bewege(typ,myev3)

% Auswahl der Bewegung über Variable typ: 
% 1 = geradeaus, 
% 2 = Linksdrehung,
% 3 = Rechtsdrehung


%% Parameter für die Motoren
motordifferenz = 1.070; %Differenz für links und rechts, bei synchronem fahren nicht verwendet.
speed = 50; %Motorgeschwindigkeit
Daempfung = 5;
Reglung = 0.003; % Regelparameter für linkskurve und rechtskurve

%% Übernahme globale Variablen
global richtung
global posx
global posy
global linex;
global liney;
global Anzahl;
global Messwertelinks;
global Messwerterechts;
global wegstrecke;
global Messanzahl;
global FahrDifferenz;
global schrittzahl;
global Regelung;

%% Motor initialisierung
l = myev3.motorB; % Motor links
r = myev3.motorC; % Motor rechts
l.power = 20; % neu definieren der Geschwindigkeit für Motor links

%% Messungen der Sensordaten
lsensor = myev3.sensor2; %Ultraschall links an Port 2
rsensor = myev3.sensor3; %Ultraschall rechts an Port 3
fsensor = myev3.sensor4; % Ultraschall mitte an Port 4

% Einstellen der drei Sensoren auf Ultraschall Modus in cm 
%(inch auch möglich falls gewünscht)
lsensor.mode = DeviceMode.UltraSonic.DistCM; 
rsensor.mode = DeviceMode.UltraSonic.DistCM;
fsensor.mode = DeviceMode.UltraSonic.DistCM;

Bewegung nach vorne

Für die Bewegung nach vorne des Roboters wird die Information vom Inkrementalgeber verwendet. Dabei gibt es zwei Abbruchkriterien. Zum einen wird vorab ein fester Wert definiert, die der Roboter fahren darf bis zum nächsten Wegpunkt. Wenn der Wert vom Inkrementalgeber zurückgegeben wird, stoppt die Bewegung. Zudem wird permanent der Abstand nach vorne bestimmt. Wenn zwei mal der Sensorwert einen vorher bestimmten Fixwert unterschreitet, wird die Bewegung beendet. Parallel zur Bewegung durch die beiden Schrittmotoren, werden die Ultraschallsensoren links und rechts verwendet, um die Mauern um den Roboter in die Karte einzuzeichnen. Dabei wird nach jeder Messung eine kurze Pause von 0,2s gewartet, damit die Anzahl der Messwerte überschaubar bleibt. Dies ist aber frei änderbar. Die Messwerte werden in Matlab gespeichert und können nachträglich ausgewertet und weiterverarbeitet werden.

%% Vorwärts bewegen

if typ == 1
    
Fahrweg = 340; %war 320, etwas überfahren, da der Abstand nach Vorne überprüft wird
Winkel = Fahrweg/0.377; % Umrechnung von Strecke in ° vom Motor

l.limitMode='Tacho'; % Der Motor fährt bis zu einem definierten Wert
resetTachoCount(l); %Inkrementalgeber auf 0 gesetzt
l.limitValue = Winkel; % Limit wird übergeben
l.syncedStart(r);  % Motorstart
schrittzahl = 0; % Initialisierung der Schrittzahl auf 0
Abstandvorne = fsensor.value; % Erste Messung zur Front

while schrittzahl < (Winkel-1) 
    % So lange der gewünschte Wert noch nicht erreicht wird, schreiben die Ultraschallsensoren
    % Je nach Himmelsrichtung müssen die Werte abhängig von der momentanen
    % x-Position anders eingetragen werden. Die vier Fälle sind über Richtung
    % abgefragt. 

    if richtung == 'E' % Nach Osten schauend
        Messanzahl = Messanzahl + 1;

        % Messen links
        Messwertelinks(Messanzahl,1) = Messanzahl;
        Messwertelinks(Messanzahl,2) = posx + l.tachoCount*0.322 - 40; % Aktuelle Position + Schrittweite Motor links(Umrechnung in mm) - Offset Mitte Roboter zum Sensor
        Messwertelinks(Messanzahl,3) = posy + lsensor.value *10 +100; %Aktuelle Position + Sensormessung (in mm umgewandelt) + Offset zur Roboter Mitte
        plot(Messwertelinks(Messanzahl,2),Messwertelinks(Messanzahl,3),'.');

        % Messen rechts
        Messwerterechts(Messanzahl,1) = Messanzahl;
        Messwerterechts(Messanzahl,2) = posx + l.tachoCount*0.322 - 40; % Aktuelle Position + Schrittweite Motor links(Umrechnung in mm) - Offset Mitte Roboter zum Sensor
        Messwerterechts(Messanzahl,3) = posy - rsensor.value *10 -100; %Aktuelle Position + Sensormessung (in mm umgewandelt) + Offset zur Roboter Mitte
        plot(Messwerterechts(Messanzahl,2),Messwerterechts(Messanzahl,3),'.');

        %Einzeichnen der Strecke
        wegstrecke(Messanzahl,1) = Messanzahl;
        wegstrecke(Messanzahl,2) = posx + l.tachoCount*0.322;
        wegstrecke(Messanzahl,3) = posy;
        plot(wegstrecke(Messanzahl,2),wegstrecke(Messanzahl,3),'b--o');% Weg einzeichnen
        
        elseif richtung == 'W'  % Nach Westen schauend

        % Messen links
        Messanzahl = Messanzahl + 1;
        Messwertelinks(Messanzahl,1) = Messanzahl;
        Messwertelinks(Messanzahl,2) = posx - l.tachoCount*0.322 + 40; % Aktuelle Position + Schrittweite Motor links(Umrechnung in mm) - Offset Mitte Roboter zum Sensor
        Messwertelinks(Messanzahl,3) = posy - lsensor.value *10 -100; %Aktuelle Position + Sensormessung (in mm umgewandelt) + Offset zur Roboter Mitte
        plot(Messwertelinks(Messanzahl,2),Messwertelinks(Messanzahl,3),'.');

        % Messen rechts
        Messwerterechts(Messanzahl,1) = Messanzahl;
        Messwerterechts(Messanzahl,2) = posx - l.tachoCount*0.322 + 40; % Aktuelle Position + Schrittweite Motor links(Umrechnung in mm) - Offset Mitte Roboter zum Sensor
        Messwerterechts(Messanzahl,3) = posy + rsensor.value *10 +100; %Aktuelle Position + Sensormessung (in mm umgewandelt) + Offset zur Roboter Mitte
        plot(Messwerterechts(Messanzahl,2),Messwerterechts(Messanzahl,3),'.');

        %Einzeichnen der Strecke
        wegstrecke(Messanzahl,1) = Messanzahl;
        wegstrecke(Messanzahl,2) = posx - l.tachoCount*0.322;
        wegstrecke(Messanzahl,3) = posy;
        plot(wegstrecke(Messanzahl,2),wegstrecke(Messanzahl,3),'b--o');% Weg einzeichnen
                  
        elseif richtung == 'N'  % Nach Norden schauend
          Messanzahl = Messanzahl + 1;

        % Messen links
        Messwertelinks(Messanzahl,1) = Messanzahl;
        Messwertelinks(Messanzahl,3) = posy + l.tachoCount*0.322 - 40; % Aktuelle Position + Schrittweite Motor links(Umrechnung in mm) - Offset Mitte Roboter zum Sensor
        Messwertelinks(Messanzahl,2) = posx - lsensor.value *10 -100; %Aktuelle Position + Sensormessung (in mm umgewandelt) + Offset zur Roboter Mitte
        plot(Messwertelinks(Messanzahl,2),Messwertelinks(Messanzahl,3),'.');

        % Messen rechts
        Messwerterechts(Messanzahl,1) = Messanzahl;
        Messwerterechts(Messanzahl,3) = posy + l.tachoCount*0.322 - 40; % Aktuelle Position + Schrittweite Motor links(Umrechnung in mm) - Offset Mitte Roboter zum Sensor
        Messwerterechts(Messanzahl,2) = posx + rsensor.value *10 +100; %Aktuelle Position + Sensormessung (in mm umgewandelt) + Offset zur Roboter Mitte
        plot(Messwerterechts(Messanzahl,2),Messwerterechts(Messanzahl,3),'.');

        %Einzeichnen der Strecke
        wegstrecke(Messanzahl,1) = Messanzahl;
        wegstrecke(Messanzahl,2) = posx;
        wegstrecke(Messanzahl,3) = posy + l.tachoCount*0.322;
        plot(wegstrecke(Messanzahl,2),wegstrecke(Messanzahl,3),'b--o');% Weg einzeichnen
            
        elseif richtung == 'S'  % Nach Süden schauend
        Messanzahl = Messanzahl + 1;

        % Messen links
        Messwertelinks(Messanzahl,1) = Messanzahl;
        Messwertelinks(Messanzahl,3) = posy - l.tachoCount*0.322 + 40; % Aktuelle Position + Schrittweite Motor links(Umrechnung in mm) - Offset Mitte Roboter zum Sensor
        Messwertelinks(Messanzahl,2) = posx + rsensor.value *10 +100; %Aktuelle Position + Sensormessung (in mm umgewandelt) + Offset zur Roboter Mitte
        plot(Messwertelinks(Messanzahl,2),Messwertelinks(Messanzahl,3),'.');

        % Messen rechts
        Messwerterechts(Messanzahl,1) = Messanzahl;
        Messwerterechts(Messanzahl,3) = posy - l.tachoCount*0.322 + 40; % Aktuelle Position + Schrittweite Motor links(Umrechnung in mm) - Offset Mitte Roboter zum Sensor
        Messwerterechts(Messanzahl,2) = posx - rsensor.value *10 -100; %Aktuelle Position + Sensormessung (in mm umgewandelt) + Offset zur Roboter Mitte
        plot(Messwerterechts(Messanzahl,2),Messwerterechts(Messanzahl,3),'.'); 

        %Einzeichnen der Strecke
        wegstrecke(Messanzahl,1) = Messanzahl;
        wegstrecke(Messanzahl,2) = posx;
        wegstrecke(Messanzahl,3) = posy - l.tachoCount*0.322;
        plot(wegstrecke(Messanzahl,2),wegstrecke(Messanzahl,3),'b--o'); % Weg einzeichnen
end
    pause(0.2); % Variable Pause zwischen zwei Messwerten, kann beliebig gewählt werden
    schrittzahl = l.tachoCount;
    Abstandvorne = fsensor.value
     if Abstandvorne < 8 % Parameter Abstand nach vorne, kann angepasst werden
        l.stop(); % Motor frühzeitig anhalten bei zu geringem Abstand
        break; % while Schleife verlassen!
    end   

end
% Einzeichnen der neuen Position im Labyrinth. Neuer Referenzpunkt für Messung und Strategie  
      if richtung == 'E'
            posx = posx+320;
        elseif richtung == 'W'
            posx = posx -320;
        elseif richtung == 'N'
            posy = posy +320;
        elseif richtung == 'S'
            posy = posy -320;
      end
    plot(posx,posy,'p') % Position als Stern
end

Drehung links und rechts

Die Drehung nach links und rechts wird analog zur Vorwärtsbewegung durchgeführt, allerdings wird der synchronen Bewegung der Sensoren ein Parameter gegeben, damit die Sensoren sich gegenläufig bewegen. Bei einem Parameterwert von 200 bzw. -200 drehen sich die beiden Sensoren bei gleicher Geschwindigkeit in unterschiedliche Richtungen. Das Abbruchkriterium für die Bewegung ist ein vorher festgelegter Wert, der über den Abstand der Räder, die Größe der Räder und dem Abstand der Kugel zu den beiden Rädern berechnet werden kann. Wir haben den Wert durch ausprobieren ermittelt. Wenn man die Genauigkeit der Drehung erhöhen möchte, kann zusätzlich ein Gyro Sensor verwendet werden. Dieser ist deutlich genauer, war aber leider im Magazin der Hochschule nicht vorhanden. Für das Labyrinth das wir verwendet haben, war die Genauigkeit aber ausreichend.

%% Rechtsdrehung

if typ == 2

Fahrweg = 90; % Kann angepasst werden, falls Drehung zu schwach oder zu stark. Besser mit Gyrosensor, momentan aber nicht vorhanden im Bestand. 
Winkel = Fahrweg/0.377;
l.limitMode='Tacho';
resetTachoCount(l);
l.limitValue = Winkel;
l.syncedStart(r,'turnRatio',200); 
     
% Ausrichtung anpassen auf neue Himmelsrichtung
     if richtung == 'N'
        richtung = 'E'
    elseif richtung =='E'
        richtung = 'S'
    elseif richtung == 'S'
        richtung ='W'
    elseif richtung == 'W'
        richtung = 'N'
    end
end

%% Linksdrehung

if typ == 3
    
   Fahrweg = 90; % Kann angepasst werden, falls Drehung zu schwach oder zu stark. Besser mit Gyrosensor, momentan aber nicht vorhanden im Bestand. 
Winkel = Fahrweg/0.377;
l.limitMode='Tacho';
resetTachoCount(l);
l.limitValue = Winkel;
l.syncedStart(r,'turnRatio',-200); 
    
    % Ausrichtung anpassen auf neue Himmelsrichtung
     if richtung == 'N'
        richtung = 'W'
    elseif richtung =='E'
        richtung = 'N'
    elseif richtung == 'S'
        richtung ='E'
    elseif richtung == 'W'
        richtung = 'S'
    end
end

Kalibrierbewegung

Um den gewünschten Abstand bei der Vorwärtsbewegung und den genauen Wert für die 90° Drehung herauszufinden, haben wir uns ein kleines Programm geschrieben, mit dem wir die Parameter schnell ändern und die Änderung prüfen können.

resetRotation(motorrechts)
    resetRotation(motorlinks)
    
    motorlinks.Speed = speed*motordifferenz;
    motorrechts.Speed = speed;

    Fahrweite = 300;
    Winkelzahl = Fahrweite/0.377
i = 0;
j = 0;
 while i == 0
j = j+1;
    start(motorlinks);
    start(motorrechts); 
    rotationlinks(j) = readRotation(motorlinks);
    rotationrechts(j) = readRotation(motorrechts);
    motorlinks.Speed = speed*motordifferenz;
    motorrechts.Speed = speed;
    if rotationrechts(j) > rotationlinks(j)
        motordifferenz = motordifferenz + 0.005
    end
    if rotationrechts(j) < rotationlinks(j)
        motordifferenz = motordifferenz - 0.005
    end
    motorwert(j) = motordifferenz;
    zielwert = rotationlinks(j)*100/Winkelzahl
    motorlinks.Speed = speed*motordifferenz-(zielwert/Daempfung);
    motorrechts.Speed = speed-(zielwert/Daempfung);

    if rotationlinks(j) > Winkelzahl 
        i = 1
        stop(motorlinks);
        stop(motorrechts); 
        rotationlinks(j) = readRotation(motorlinks)
        rotationrechts(j) = readRotation(motorrechts)
    end
 end

Sensortest

Für die Kalibrierwerte der Ultraschallsensoren haben wir ein Script geschrieben, das den Roboter immer Schrittweise (5mm) fahren lässt und anschließend nach einer Pause von 1s den Wert in eine Tabelle schreibt. Dadurch kann die gemessene zur wirklichen Entfernung verglichen werden. Man stellt den Roboter einfach vor eine weiße Wand und startet das Skript. Man sollte nach den 40 Messwerten, die das Programm aufnimmt einmalig überprüfen ob der maximale Wert der X-Achse auch zum Abstand vom Roboter zur Wand passt. Sollte die Position abweichen, kann der Fahrparameter im Skript leicht geändert werden.

myev3 = legoev3('USB');

sensor = sonicSensor(myev3,2)

% Bsensor = sonicSensor(myev3,4)

figure
h = animatedline;
ax = gca;
ax.YGrid = 'on';
ax.YLim = [0 30];
i=0;
stop = false;
startTime = datetime('now');
while ~stop
    i=i+1;
    pause(0.01);
    % Read current voltage value
    d = readDistance(sensor)*100;

   
    
    dd = double(d);
 
    % Get current time
    t =  datetime('now') - startTime;
    % Add points to animation
  
    addpoints(h,datenum(t),dd)
    % Update axes
    ax.XLim = datenum([t-seconds(15) t]);
    datetick('x','keeplimits')
    drawnow
    % Check stop condition
    if i>= 10000
        stop = true;
    end
end

Zusammenfassung

Lessons Learned

Fehler bei der Drehung

Fehler in Vorwärtsbewegung

YouTube Video

Youtube Video https://www.youtube.com/watch?v=mFR9on12qtw&t=7s[[2]]

Weblinks

Aufbauanleitung LEGO EV3 Education https://education.lego.com/de-de/support/mindstorms-ev3/building-instructions[3]

LEGO MINDSTORMS EV3 Support from MATLAB https://de.mathworks.com/hardware-support/lego-mindstorms-ev3-matlab.html[4]

Connect the Host Computer to an EV3 Brick Over a Wireless Network https://de.mathworks.com/help/supportpkg/legomindstormsev3io/ug/connect-to-an-ev3-brick-over-wifi.html[5]

• RWTH Mindstorms EV3 Matlab Toolbox

Quellenangaben Bilder

• 1: https://le-www-live-s.legocdn.com/sc/media/lessons/mindstorms-ev3/building-instructions/ev3-rem-driving-base-79bebfc16bd491186ea9c9069842155e.pdf

Zugehörige HSHL Wiki Artikel

Self Localization and Mapping (SLAM) mit Lidar- oder Kamera: http://193.175.248.52/wiki/index.php/Self_Localization_and_Mapping_(SLAM)_mit_Lidar-_oder_Kamera[6]

Einführung in SLAM – Simultaneous Localization and Mapping http://193.175.248.52/wiki/index.php/Einf%C3%BChrung_in_SLAM_%E2%80%93_Simultaneous_Localization_and_Mapping[7]

Lego Mindstorms EV3 http://193.175.248.52/wiki/index.php/Lego_Mindstorms_EV3[8]

Ultraschall mit Matlab/Simulink http://193.175.248.52/wiki/index.php/Ultraschall_mit_Matlab/Simulink[9]

Objekterkennung mit rotierenden Ultraschall mit Matlab/Simulink und EV3 http://193.175.248.52/wiki/index.php/Objekterkennung_mit_rotierenden_Ultraschall_mit_Matlab/Simulink_und_EV3[10]

--- → zurück zur Übersicht: WS 18/19: Angewandte Elektrotechnik (BSE)