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
Einleitung
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 40%
Plakat entwerfen und Messestand 20%
Video drehen und hochladen 100%
Messe 0%
Projektdurchführung
Konstruktion
Grundaufbau Robotor
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
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.
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.
Aufbau variables Labyrinth
Umbau Labyrinth
-
System Design
Toolbox Schnittstelle EV3
Software Matlab/Simulink
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
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.
Filter (Median, Kalman, Partikel)
Programmierung
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
ALT
Start-Algorithmus
Logischer Aufbau mit 4 Ultraschallsensoren
-
Logischer Aufbau mit 3 Ultraschallsensoren
-
Unterteilung der möglichen Fahrmanöver des EV3
1 Schritt nach Vorne
Rechtsdrehung oder Linksdrehung
Ergebnis
Zusammenfassung
Lessons Learned
Fehler bei der Drehung
Fehler in Vorwärtsbewegung
Projektunterlagen
YouTube Video
Youtube Video https://www.youtube.com/watch?v=mFR9on12qtw&t=7s[[1]]
Weblinks
Aufbauanleitung LEGO EV3 Education https://education.lego.com/de-de/support/mindstorms-ev3/building-instructions[2]
LEGO MINDSTORMS EV3 Support from MATLAB https://de.mathworks.com/hardware-support/lego-mindstorms-ev3-matlab.html[3]
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[4]
Literatur
Quellenangaben Bilder
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[5]
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[6]
Lego Mindstorms EV3 http://193.175.248.52/wiki/index.php/Lego_Mindstorms_EV3[7]
Ultraschall mit Matlab/Simulink http://193.175.248.52/wiki/index.php/Ultraschall_mit_Matlab/Simulink[8]
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[9]
--- → zurück zur Übersicht: WS 18/19: Angewandte Elektrotechnik (BSE)