ArduMower: Kartierung

Matrix-basierte Karte der zu mähenden Fläche in Matlab/Simulink

Dieser Wiki-Beitrag ist Teil eines Projektes, welches im Rahmen vom Fachpraktikum Elektrotechnik im 6. Semester und 7. Mechatronik absolviert wurde. Ziel des Beitrags ist es, eine nachhaltige Dokumentation zu schaffen, welche die Ergebnisse festhält und das weitere Arbeiten am Projekt ermöglicht.

Autoren: Marcel Kreuer, Prof. Dr.-Ing. Schneider,

Betreuer: Prof. Dr.-Ing. Schneider, Prof. Dr.-Ing. Mirek Göbel

Projekt: Ardumower

Einleitung

Dieser Artikel beschreibt den Aufbau einer Matrix-basierten Karte zur Darstellung des aktuellen Mähstandes eines Rasenmähroboters. Neben der visuellen Darstellung, dient die Karte als Informationsgrundlage für beispielsweise die Planung der Mähstrategie.

Anforderungen

Die Aufgabe ist die Erstellung einer selbstlernenden Karte, welche im Lastenheft wie folgt gefordert wird:

REQ10.2290	Selbstlernende Karte
Requirement	Der Roboter kartographiert ausgehend von der Ladestation sein Arbeitsgebiet
Notes	In der selbstlernenden Karte sind wichtige Landmarken, wie der Leitdraht, die Ladestation, die zuletzt gemähte Fläche und statische Objekte einzuzeichnen. Die Karte dient als Basis für die Mähstrategie. Beispielsweise sollen statische Objekte umgangen werden und nach dem Laden am letzten Endpunkt der Mähvorgang wieder aufgenommen werden.

In dem, durch das Team Ardumower erstellten Pflichtenheft, werden die Anforderungen weiter ausgeführt:

Spezifikations-ID	Anforderungs-ID	Anforderungs-Titel	Beschreibung	Teilaufgaben
0170	REQ10.2290	2.2 Selbstlende Karte	Der Roboter kartographiert ausgehend von der Ladestation sein Arbeitsgebiet	Karte Händisch vermessen: Selbstlernende Karte Lokalisierung: Dead-Reckoning - Odometrie + INS mit Kompass Rekalibrierungsstrategie Halterung für das Prisma Modultest: Kommunikation der Sensoren mit der Hauptplatine

Um eine Implementierung zu ermöglichen, wurden folgende Anforderungen an die Karte spezifiziert:

ID	Inhalt	Ersteller	Datum	Geprüft von	Datum
1	Die Karte muss als Matrix mit der Rastergröße 0,3 m umgesetzt werden (Grid-Map).	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
2	Die Perimeterschleife muss in der Karte mit dem Wert „1“ hinterlegt sein und in der Darstellung rot markiert werden.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
3	Die aktuelle Position des Mähers muss in der Karte mit dem Wert „4“ hinterlegt sein und in der Darstellung blau markiert werden.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
4	Hindernisse müssen in der Karte mit dem Wert „5“ hinterlegt sein und in der Darstellung magenta markiert werden.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
5	Unbekannte Bereiche müssen in der Karte mit dem Wert „0“ hinterlegt sein und in der Darstellung weiß bleiben.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
6	Ungemähter Rasen muss in der Karte mit dem Wert „2“ hinterlegt sein und in der Darstellung dunkelgrün makiert werden.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
7	Gemähter Rasen muss in der Karte mit dem Wert „3“ hinterlegt sein und in der Darstellung hellgrün makiert werden.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
8	Die Karte muss zyklisch aktualisiert werden.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
9	Die Umsetzung muss als Matlab-Skript erfolgen, so dass eine Einbindung in Simulink als Matlab-Funktion möglich ist.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
10	Eingangsgrößen der Funktion Kartenfunktion.m sind Aktuelle Positionen in m: xNeu,yNeu Alter Positionsvektor in m: PosAlt Ausrichtung der Ardumower in deg: Ausrichtung Aktuelle Karte: Karte Signale zur Objekterkennung: Bumper,Ultraschall Signalstärke der Perimeterschleife: Perimeterschleife	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
11	Ausgangsgrößen der Funktion Kartenfunktion.m sind Aktueller Positionsvektor in m: PosNeu Aktuelle Karte: Karte	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
12	Die Initialisierung muss 1s und die zyklische Darstellung muss 1ms unterschreiten.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017
13	Bei Kommentierung und Dokumentation muss sich an die Projektrichtlinien gehalten werden.	Marcel Kreuer	05.10.2017	Simon Kohfeld	05.10.2017

Historie

In erster Instanz wurde die zu mähende Rasenfläche entlang der Perimeterschleife mit einem GroundTruth System vermessen. Die daraus resultierenden Messdaten wurden in Matlab eingelesen und zu einer Karte verarbeitet.

Die Karte selbst wurde als eine Matrix angelegt, wobei jedes Matrixelement eine Fläche von 0,3m x 0,3 m repräsentiert.

Die Auswertung der Messdaten ergab folgende mit Matlab generierte Karte:

Karte aus Messpunkten

Die Matrix selbst wurde exportiert und mithilfe eines einmaligen Algorythmus in die Darstellungsform wie am Anfang des Artikels dargestellt konvertiert.

Eine besondere Herausforderung bei der Umsetzung der Karten-Funktion, stellte die Visualisierung zu Testzwecken dar. Die Laufzeit der Kartenfunktion war so langsam, dass eine Simulation der Mähstrategie quasi nicht möglich war. Ein besonderer Dank geht daher an Prof. Dr.-Ing. Schneider, der durch seine Unterstützung eine laufzeitgerechte Darstellung der Karte ermöglichte.

Funktionaler Systementwurf / Technischer Systementwurf

Bei der Größe des zu erstellenden Modells werden die Schritte Funktionaler Systementwurf/Technischer Systementwurf des V-Modells zusammen gelegt und bereits Schnittstellen zwischen den einzelnen Blöcken definiert.

Schnittstellen für die Modellierung einer selbstlernenden Karte

Komponentenspezifikation

Das Modell wird insgesamt als Komponente aufgefasst, d. h. die einzelnen Blöcke aus dem Systementwurf werden als Bestandteil der Komponente "Selbstlernende Karte" definiert. Die Funktion der Komponente "Selbstlernende Karte" wird in Form eines PAP spezifiziert.

Spezifikation der selbstlernenden Karte

Programmierung

Im Folgenden wird die Matlab-Funktion hinter dem Simulink-Block vorgestellt. Dabei lässt sich die Funktion unterteilen in:

1. den Funktionsaufruf
2. die Initialisierung und
3. die zyklische Aktualisierung

der selbstlernenden Karte.

Funktionsaufruf

Im Funktionsaufruf ist der Aufruf der Funktion und die Deklarierung der persistenten Variabeln beschrieben. Hinzu kommen die Befehle coder.extrinsic() , welche die Matlab-Befehle imshow() und num2str() in Simulink nutzbar machen.

function [PosNeu, Karte] = Kartenfunktion(xNeu,yNeu,PosAlt,Ausrichtung,Karte,Bumper,Ultraschall,Perimeterschleife)
coder.extrinsic('imshow'); 
coder.extrinsic('num2str'); 
%aColorMap
persistent bInitKarte nKartenPosAlt mKarte hKarte hAxes  xt  yt  xtLabel ytLabel hImg
persistent nUnbekannt nPerimeter nNichtGemaeht nGemaeht nArduMower nHindernis

Initialisierung

In der einmaligen Initialisierung werden die Farben der Karteninhalte entsprechend der Anforderungen festgelegt. Die aColorMap wird anderen Programmteilen als globale Variable zur Verfügung gestellt.

% Farben festlegen
cUnbekannt = [1 1 1];        nUnbekannt    = 0;
cPerimeter = [1 0 0];        nPerimeter    = 1;
cNichtGemaeht = [0.5 0.8 0]; nNichtGemaeht = 2;
cGemaeht = [0 1 0];          nGemaeht      = 3;
cArduMower = [0 0 1];        nArduMower    = 4;
cHindernis = [1 0.5 1];      nHindernis    = 5;
    
 aColorMap = [ cUnbekannt ; cPerimeter ; cNichtGemaeht ; cGemaeht  ; cArduMower ; cHindernis ];

Die Karte wird in Form und Größe und aus der wie zuvor beschriebenen ’’’Karte.mat’’’-Datei initialisiert. Hierzu gehört auch die Beschriftung der Achsen und die Darstellung des Gitters in Metern. Die Handles hierzu werden persistent gespeichert.

Karte = zeros(81,71);
mKarte = zeros(81,71);
PosNeu= [0,0]';
xtLabel = struct;
ytLabel = struct;

S=load('Karte.mat')
Karte = S.mKarte;
    
%% Karte in Größe und Form initialisieren
mKarte = Karte;  % Ursprüngliche Karte sichern
hKarte = figure;
set(hKarte,'WindowStyle', 'normal')
hImg = imshow(Karte,aColorMap); 
hAxes = gca;
set(hKarte,'Position',[1     1   500   600],'OuterPosition',[ 550   300   500   600]); % 589
set(hAxes,'YDir','normal');
set(hAxes,'OuterPosition',[-0.1068   0    1.1819    1])
set(hAxes,'Visible','on')
    
%% Achsen beschriften
xt = 0: 10:size(Karte, 2)-1;
yt = 0: 10:size(Karte, 1)-1;

set(hAxes, 'xgrid', 'on', 'ygrid', 'on', 'xtick', xt, 'ytick', 1/2+yt, 'xticklabel', {'0','3','6','9','12','15','18','21','24','27'}, 'yticklabel',{'0','3','6','9','12','15','18','21','24','27'})
    
%xtLAbel und ytLabel noch ausfüllen
    
xlabel('x in m'); ylabel('y in m');
    
nKartenPosAlt = floor((PosAlt/0.30))+1; % Umrechnung von m in Kartenmatrix
    
disp('Karte initialisiert');
%disp(['tInit = ',num2str(toc),' s']) % 75ms
    
bInitKarte = true; % Initialisierung nur einmal durchlaufen

Hinweis:

Das Laden einer Datei im PNG-Format bei Aufruf des Matlab-Skripts in Simulink ist nicht möglich.

Aktualisierung

Bei der zyklischen Aktualisierung muss:

• die Position in m in die Kartenmatrix umgerechnet,
• eine Aktualisierung überprüft,
• die neue Position eingetragen und
• die alte Position aktualisiert

werden.

Die Umrechnung erfolgt über:

nKartenPosAlt = floor((PosAlt/0.30))+1;
nKartenPosNeu = floor((PosNeu/0.30))+1;

Eine Aktualisierung erfolgt nur, wenn der Mähroboter in seiner Positionsdarstellung ein neues Karten-Matrixfeld erreicht.

if ~isequal(nKartenPosNeu,nKartenPosAlt)

Hindernisse werden in der Karte vermerkt oder die neue Mäherposition eingetragen:

if Bumper==1
    Karte(nKartenPosNeu(1),nKartenPosNeu(2))  = nHindernis;
    mKarte(nKartenPosNeu(1),nKartenPosNeu(2)) = nHindernis; % Hindernisse nicht aus Karten löschen
else
    Karte(nKartenPosNeu(1),nKartenPosNeu(2)) = nArduMower; % Neue Position des Mähers markieren
end

Anschließend werden die alten Mähexpositionen gemäß den Anforderungen eingefärbt:

switch mKarte(nKartenPosAlt(1),nKartenPosAlt(2))
    case nNichtGemaeht
        Karte(nKartenPosAlt(1),nKartenPosAlt(2)) = nGemaeht;
    case nGemaeht
        Karte(nKartenPosAlt(1),nKartenPosAlt(2)) = nGemaeht;
    case nPerimeter
        Karte(nKartenPosAlt(1),nKartenPosAlt(2)) = nPerimeter;
    case nHindernis
        Karte(nKartenPosAlt(1),nKartenPosAlt(2)) = nHindernis;
    case nUnbekannt
        Karte(nKartenPosAlt(1),nKartenPosAlt(2)) = nUnbekannt;
end

Abschließend wird der aktualisierte Inhalt der Karte dargestellt. Hierzu wird der Inhalt (Bildmatrix) direkt über das Handle hImg aktualisiert.

set(hImg,'CData',Karte); % Karte aktualisieren

Komponententest

Da es sich bei dieser Entwicklung um die einer einzelnen Komponente handelt, schließt der Komponententest mit dem Testbericht die Entwicklung ab.

Der Test wurde mit folgender Testumgebung durchgeführt: Karte_test.slx

ID	Testfallbeschreibung	Eingänge x,y,PosAlt,Ausrichtung,Karte,Bumper,Ultraschall,Perimeterschleife	Erwartetes Ergebnis	Testergebnis	Testperson	Datum
1	Der Mäher fährt kein Feld weiter.	0.1, 0.0, [0;0], 0, Karte, 0, 0, 1000	Kein neues Feld wird blau markiert.	OK	Marcel Kreuer	24.01.2018
2	Der Mäher fährt auf der Perimeterschleife.	0.6, 0.0, [0.3;0.0], 0, Karte, 0, 0, 1000	Neues Feld wird blau markiert, altes Feld wird rot markiert.	OK	Marcel Kreuer	24.01.2018
3	Der Mäher fährt ein Feld weiter.	0.3, 0.0, [0;0], 0, Karte, 0, 0, 1000	Neues Feld wird blau markiert.	OK	Marcel Kreuer	24.01.2018
4	Der Mäher trifft auf ein Hindernis.	34.0, 4.0, [33.7;4.0], 0, Karte, 1, 0, 1000	Neues Feld wird magenta markiert, altes Feld wird hellgrün markiert.	OK	Marcel Kreuer	24.01.2018
5	Das überschreitet die Perimeterschleife in unbekanntes Gebiet.	0.0, 3.0, [0.3;3.0], 0, Karte, 0, 0, 1000	Neues Feld wird weiß markiert, altes Feld wird rot markiert.	OK	Marcel Kreuer	24.01.2018
6	Der Mäher fährt auf ungemähtem Rasen.	0.6, 0.6, [0.3;0.3], 0, Karte, 0, 0, 1000	Neues Feld wird blau markiert, altes Feld wird hellgrün markiert.	OK	Marcel Kreuer	24.01.2018
6b	Der Mäher fährt auf gemähtem Rasen.	0.6, 0.6, [0.3;0.3], 0, Karte, 0, 0, 1000	Neues Feld wird blau markiert, altes Feld wird hellgrün markiert.	OK	Marcel Kreuer	24.01.2018
7	Der Mäher fährt mit fester Zykluszeit eine simulierte Mäanderfahrt.	Mäander simuliert, *,*,*, 0, Karte, 0, 0, 1000	Die Karte wird zyklisch aktualisiert.	OK	Marcel Kreuer	24.01.2018
8	Einbindung des Matlab-Skriptes in Simulink.	Mäander simuliert in Simulink	Die Karte wird zyklisch aktualisiert.	OK	Marcel Kreuer	24.01.2018
9	Test der Eingangsparameter	Variation der Eingangsparameter	Die Karte wird zyklisch gemäß Anforderungen aktualisiert.	OK	Marcel Kreuer	24.01.2018
10	Test der Ausgangsparameter	Variation der Eingangsparameter	Die Karte wird zyklisch gemäß Anforderungen aktualisiert.	OK	Marcel Kreuer	24.01.2018
11	Laufzeitmessung	0.3, 0.0, [0;0], 0, Karte, 0, 0, 1000	Die Initialisierung muss 1s und die zyklische Darstellung muss 1ms unterschreiten.	OK	Marcel Kreuer	24.01.2018
12	Kommentierung und Dokumentation entsprechen den Projektrichtlinien	-	Code Review, Artikel-Review	OK	Prof. Dr.-Ing. Schneider,	24.01.2018

Der Karten-Funktion hat alle Testfälle bestanden und wird daher im Gesamtergebnis als i.O. eingestuft. Sie ist zur Verwendung im Hauptprogramm freigegeben.

Zusammenfassung

Eine selbstlernende Karte wurde gemäß der Anforderungen programmiert, in Matlab und Simulink getestet, und dokumentiert. Als Ansteuerung wurde durch Prof. Dr.-Ing. Schneider eine einfache Mäanderfahrt implementiert. Die Kartenfunktion wurde anschließend als Librarie in das Hauptprojekt integriert und steht nun allen Teilteams zur Verfügung, so dass auch die Mähstrategie anhand dieser Karte in Echtzeit visualisiert werden kann.

Die nachfolgende Abbildung zeigt die simulierte Funktionalität der Karte.

Ergebnisdarstellung der Modellierung einer selbstlernenden Karte

Ansatzpunkte für die Fortführung des Ardumower Projektes

Seitens der Karte gibt es noch einige Funktionalitäten, die noch nicht implementiert werden konnten. Dazu zählen:

1. Eine Rekalibrierung, falls der Tower bemerkt, dass seine errechnete Position nicht mit der tatsächlichen Position übereinstimmt.
2. Die Erstellung der Karte auf unbekanntem Terrain. Einige Ansatzpunkte für die Erstellung einer Karte in unbekanntem Gebiet, auf Basis der vorherigen, langsamen Kartendarstellung, sind im SVN hier zu finden:

SVN: ArduMower Karte_selbstlernend.slx

Link zum Quelltext in SVN

SVN: ArduMower Karte.slx

---

→ zurück zum Hauptartikel: Projekt ArduMower